0031186: Foundation Classes - add additional useful methods to BVH_Box.
[occt.git] / src / BVH / BVH_Tools.hxx
1 // Created by: Eugeny MALTCHIKOV
2 // Created on: 2019-04-17
3 // Copyright (c) 2019 OPEN CASCADE SAS
4 //
5 // This file is part of Open CASCADE Technology software library.
6 //
7 // This library is free software; you can redistribute it and/or modify it under
8 // the terms of the GNU Lesser General Public License version 2.1 as published
9 // by the Free Software Foundation, with special exception defined in the file
10 // OCCT_LGPL_EXCEPTION.txt. Consult the file LICENSE_LGPL_21.txt included in OCCT
11 // distribution for complete text of the license and disclaimer of any warranty.
12 //
13 // Alternatively, this file may be used under the terms of Open CASCADE
14 // commercial license or contractual agreement.
15
16 #ifndef _BVH_Tools_Header
17 #define _BVH_Tools_Header
18
19 #include <BVH_Box.hxx>
20 #include <BVH_Ray.hxx>
21 #include <BVH_Types.hxx>
22
23 //! Defines a set of static methods operating with points and bounding boxes.
24 //! \tparam T Numeric data type
25 //! \tparam N Vector dimension
26 template <class T, int N>
27 class BVH_Tools
28 {
29 public: //! @name public types
30
31   typedef typename BVH::VectorType<T, N>::Type BVH_VecNt;
32
33 public: //! @name Box-Box Square distance
34
35   //! Computes Square distance between Axis aligned bounding boxes
36   static T BoxBoxSquareDistance (const BVH_Box<T, N>& theBox1,
37                                  const BVH_Box<T, N>& theBox2)
38   {
39     if (!theBox1.IsValid() || !theBox2.IsValid())
40     {
41       return static_cast<T>(0);
42     }
43     return BoxBoxSquareDistance (theBox1.CornerMin(), theBox1.CornerMax(),
44                                  theBox2.CornerMin(), theBox2.CornerMax());
45   }
46
47   //! Computes Square distance between Axis aligned bounding boxes
48   static T BoxBoxSquareDistance (const BVH_VecNt& theCMin1,
49                                  const BVH_VecNt& theCMax1,
50                                  const BVH_VecNt& theCMin2,
51                                  const BVH_VecNt& theCMax2)
52   {
53     T aDist = 0;
54     for (int i = 0; i < N; ++i)
55     {
56       if      (theCMin1[i] > theCMax2[i]) { T d = theCMin1[i] - theCMax2[i]; d *= d; aDist += d; }
57       else if (theCMax1[i] < theCMin2[i]) { T d = theCMin2[i] - theCMax1[i]; d *= d; aDist += d; }
58     }
59     return aDist;
60   }
61
62 public: //! @name Point-Box Square distance
63
64   //! Computes square distance between point and bounding box
65   static T PointBoxSquareDistance (const BVH_VecNt& thePoint,
66                                    const BVH_Box<T, N>& theBox)
67   {
68     if (!theBox.IsValid())
69     {
70       return static_cast<T>(0);
71     }
72     return PointBoxSquareDistance (thePoint,
73                                    theBox.CornerMin(),
74                                    theBox.CornerMax());
75   }
76
77   //! Computes square distance between point and bounding box
78   static T PointBoxSquareDistance (const BVH_VecNt& thePoint,
79                                    const BVH_VecNt& theCMin,
80                                    const BVH_VecNt& theCMax)
81   {
82     T aDist = 0;
83     for (int i = 0; i < N; ++i)
84     {
85       if      (thePoint[i] < theCMin[i]) { T d = theCMin[i] - thePoint[i]; d *= d; aDist += d; }
86       else if (thePoint[i] > theCMax[i]) { T d = thePoint[i] - theCMax[i]; d *= d; aDist += d; }
87     }
88     return aDist;
89   }
90
91 public: //! @name Point-Box projection
92
93   //! Computes projection of point on bounding box
94   static BVH_VecNt PointBoxProjection (const BVH_VecNt& thePoint,
95                                        const BVH_Box<T, N>& theBox)
96   {
97     if (!theBox.IsValid())
98     {
99       return thePoint;
100     }
101     return PointBoxProjection (thePoint,
102                                theBox.CornerMin(),
103                                theBox.CornerMax());
104   }
105
106   //! Computes projection of point on bounding box
107   static BVH_VecNt PointBoxProjection (const BVH_VecNt& thePoint,
108                                        const BVH_VecNt& theCMin,
109                                        const BVH_VecNt& theCMax)
110   {
111     return thePoint.cwiseMax (theCMin).cwiseMin (theCMax);
112   }
113
114 public: //! @name Point-Triangle Square distance
115
116   //! Computes square distance between point and triangle
117   static T PointTriangleSquareDistance (const BVH_VecNt& thePoint,
118                                         const BVH_VecNt& theNode0,
119                                         const BVH_VecNt& theNode1,
120                                         const BVH_VecNt& theNode2)
121   {
122     const BVH_VecNt aAB = theNode1 - theNode0;
123     const BVH_VecNt aAC = theNode2 - theNode0;
124     const BVH_VecNt aAP = thePoint - theNode0;
125   
126     T aABdotAP = aAB.Dot(aAP);
127     T aACdotAP = aAC.Dot(aAP);
128   
129     if (aABdotAP <= 0. && aACdotAP <= 0.)
130     {
131       return aAP.Dot(aAP);
132     }
133   
134     const BVH_VecNt aBC = theNode2 - theNode1;
135     const BVH_VecNt aBP = thePoint - theNode1;
136   
137     T aBAdotBP = -(aAB.Dot(aBP));
138     T aBCdotBP =  (aBC.Dot(aBP));
139   
140     if (aBAdotBP <= 0. && aBCdotBP <= 0.)
141     {
142       return (aBP.Dot(aBP));
143     }
144   
145     const BVH_VecNt aCP = thePoint - theNode2;
146   
147     T aCBdotCP = -(aBC.Dot(aCP));
148     T aCAdotCP = -(aAC.Dot(aCP));
149   
150     if (aCAdotCP <= 0. && aCBdotCP <= 0.)
151     {
152       return (aCP.Dot(aCP));
153     }
154   
155     T aACdotBP = (aAC.Dot(aBP));
156   
157     T aVC = aABdotAP * aACdotBP + aBAdotBP * aACdotAP;
158   
159     if (aVC <= 0. && aABdotAP > 0. && aBAdotBP > 0.)
160     {
161       const BVH_VecNt aDirect = aAP - aAB * (aABdotAP / (aABdotAP + aBAdotBP));
162   
163       return (aDirect.Dot(aDirect));
164     }
165   
166     T aABdotCP = (aAB.Dot(aCP));
167   
168     T aVA = aBAdotBP * aCAdotCP - aABdotCP * aACdotBP;
169   
170     if (aVA <= 0. && aBCdotBP > 0. && aCBdotCP > 0.)
171     {
172       const BVH_VecNt aDirect = aBP - aBC * (aBCdotBP / (aBCdotBP + aCBdotCP));
173   
174       return (aDirect.Dot(aDirect));
175     }
176   
177     T aVB = aABdotCP * aACdotAP + aABdotAP * aCAdotCP;
178   
179     if (aVB <= 0. && aACdotAP > 0. && aCAdotCP > 0.)
180     {
181       const BVH_VecNt aDirect = aAP - aAC * (aACdotAP / (aACdotAP + aCAdotCP));
182   
183       return (aDirect.Dot(aDirect));
184     }
185   
186     T aNorm = aVA + aVB + aVC;
187   
188     const BVH_VecNt& aDirect = thePoint - (theNode0 * aVA +
189                                            theNode1 * aVB +
190                                            theNode2 * aVC) / aNorm;
191   
192     return (aDirect.Dot(aDirect));
193   }
194
195 public: //! @name Ray-Box Intersection
196
197   //! Computes hit time of ray-box intersection
198   static Standard_Boolean RayBoxIntersection (const BVH_Ray<T, N>& theRay,
199                                               const BVH_Box<T, N>& theBox,
200                                               T& theTimeEnter,
201                                               T& theTimeLeave)
202   {
203     if (!theBox.IsValid())
204     {
205       return Standard_False;
206     }
207     return RayBoxIntersection (theRay, theBox.CornerMin(), theBox.CornerMax(), theTimeEnter, theTimeLeave);
208   }
209
210   //! Computes hit time of ray-box intersection
211   static Standard_Boolean RayBoxIntersection (const BVH_Ray<T, N>& theRay,
212                                               const BVH_VecNt& theBoxCMin,
213                                               const BVH_VecNt& theBoxCMax,
214                                               T& theTimeEnter,
215                                               T& theTimeLeave)
216   {
217     return RayBoxIntersection (theRay.Origin, theRay.Direct,
218                                theBoxCMin, theBoxCMax, theTimeEnter, theTimeLeave);
219   }
220
221   //! Computes hit time of ray-box intersection
222   static Standard_Boolean RayBoxIntersection (const BVH_VecNt& theRayOrigin,
223                                               const BVH_VecNt& theRayDirection,
224                                               const BVH_Box<T, N>& theBox,
225                                               T& theTimeEnter,
226                                               T& theTimeLeave)
227   {
228     if (!theBox.IsValid())
229     {
230       return Standard_False;
231     }
232     return RayBoxIntersection (theRayOrigin, theRayDirection,
233                                theBox.CornerMin(), theBox.CornerMax(),
234                                theTimeEnter, theTimeLeave);
235   }
236
237   //! Computes hit time of ray-box intersection
238   static Standard_Boolean RayBoxIntersection (const BVH_VecNt& theRayOrigin,
239                                               const BVH_VecNt& theRayDirection,
240                                               const BVH_VecNt& theBoxCMin,
241                                               const BVH_VecNt& theBoxCMax,
242                                               T& theTimeEnter,
243                                               T& theTimeLeave)
244   {
245     BVH_VecNt aNodeMin, aNodeMax;
246     for (int i = 0; i < N; ++i)
247     {
248       if (theRayDirection[i] == 0)
249       {
250         aNodeMin[i] = (theBoxCMin[i] - theRayOrigin[i]) < 0 ?
251                        (std::numeric_limits<T>::min)() : (std::numeric_limits<T>::max)();
252         aNodeMax[i] = (theBoxCMax[i] - theRayOrigin[i]) < 0 ?
253                        (std::numeric_limits<T>::min)() : (std::numeric_limits<T>::max)();
254       }
255       else
256       {
257         aNodeMin[i] = (theBoxCMin[i] - theRayOrigin[i]) / theRayDirection[i];
258         aNodeMax[i] = (theBoxCMax[i] - theRayOrigin[i]) / theRayDirection[i];
259       }
260     }
261
262     BVH_VecNt aTimeMin, aTimeMax;
263     for (int i = 0; i < N; ++i)
264     {
265       aTimeMin[i] = Min (aNodeMin[i], aNodeMax[i]);
266       aTimeMax[i] = Max (aNodeMin[i], aNodeMax[i]);
267     }
268
269     T aTimeEnter = Max (aTimeMin[0], Max (aTimeMin[1], aTimeMin[2]));
270     T aTimeLeave = Min (aTimeMax[0], Min (aTimeMax[1], aTimeMax[2]));
271
272     Standard_Boolean hasIntersection = aTimeEnter < aTimeLeave && aTimeLeave > 0;
273     if (hasIntersection)
274     {
275       theTimeEnter = aTimeEnter;
276       theTimeLeave = aTimeLeave;
277     }
278
279     return hasIntersection;
280   }
281 };
282
283 #endif