0027590: Visualization, Ray Tracing - port to quad BVH trees (QBVH)
[occt.git] / src / BVH / BVH_DistanceField.lxx
1 // Created on: 2014-09-06
2 // Created by: Denis BOGOLEPOV
3 // Copyright (c) 2013-2014 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 #include <BVH_Triangulation.hxx>
17
18 #ifdef HAVE_TBB
19   // On Windows, function TryEnterCriticalSection has appeared in Windows NT
20   // and is surrounded by #ifdef in MS VC++ 7.1 headers.
21   // Thus to use it we need to define appropriate macro saying that we will
22   // run on Windows NT 4.0 at least
23   #if defined(_WIN32) && !defined(_WIN32_WINNT)
24     #define _WIN32_WINNT 0x0501
25   #endif
26
27   #include <tbb/tbb.h>
28 #endif
29
30 // =======================================================================
31 // function : BVH_DistanceField
32 // purpose  :
33 // =======================================================================
34 template<class T, int N>
35 BVH_DistanceField<T, N>::BVH_DistanceField (const Standard_Integer theMaximumSize,
36                                             const Standard_Boolean theComputeSign)
37 : myMaximumSize (theMaximumSize),
38   myComputeSign (theComputeSign)
39 {
40   Standard_STATIC_ASSERT (N == 3 || N == 4);
41
42   myVoxelData = new T[myMaximumSize * myMaximumSize * myMaximumSize];
43 }
44
45 // =======================================================================
46 // function : ~BVH_DistanceField
47 // purpose  :
48 // =======================================================================
49 template<class T, int N>
50 BVH_DistanceField<T, N>::~BVH_DistanceField()
51 {
52   delete [] myVoxelData;
53 }
54
55 #if defined (_WIN32) && defined (max)
56   #undef max
57 #endif
58
59 #include <limits>
60
61 #define BVH_DOT3(A, B) (A.x() * B.x() + A.y() * B.y() + A.z() * B.z())
62
63 namespace BVH
64 {
65   //=======================================================================
66   //function : DistanceToBox
67   //purpose  : Computes squared distance from point to box
68   //=======================================================================
69   template<class T, int N>
70   T DistanceToBox (const typename VectorType<T, N>::Type& thePnt,
71                    const typename VectorType<T, N>::Type& theMin,
72                    const typename VectorType<T, N>::Type& theMax)
73   {
74     Standard_STATIC_ASSERT (N == 3 || N == 4);
75
76     T aNearestX = Min (Max (thePnt.x(), theMin.x()), theMax.x());
77     T aNearestY = Min (Max (thePnt.y(), theMin.y()), theMax.y());
78     T aNearestZ = Min (Max (thePnt.z(), theMin.z()), theMax.z());
79
80     if (aNearestX == thePnt.x()
81      && aNearestY == thePnt.y()
82      && aNearestZ == thePnt.z())
83     {
84       return static_cast<T> (0);
85     }
86
87     aNearestX -= thePnt.x();
88     aNearestY -= thePnt.y();
89     aNearestZ -= thePnt.z();
90
91     return aNearestX * aNearestX +
92            aNearestY * aNearestY +
93            aNearestZ * aNearestZ;
94   }
95
96   //=======================================================================
97   //function : DirectionToNearestPoint
98   //purpose  : Computes squared distance from point to triangle
99   // ======================================================================
100   template<class T, int N>
101   typename VectorType<T, N>::Type DirectionToNearestPoint (
102     const typename VectorType<T, N>::Type& thePoint,
103     const typename VectorType<T, N>::Type& theVertA,
104     const typename VectorType<T, N>::Type& theVertB,
105     const typename VectorType<T, N>::Type& theVertC)
106   {
107     Standard_STATIC_ASSERT (N == 3 || N == 4);
108
109     const typename VectorType<T, N>::Type aAB = theVertB - theVertA;
110     const typename VectorType<T, N>::Type aAC = theVertC - theVertA;
111     const typename VectorType<T, N>::Type aAP = thePoint - theVertA;
112
113     const T aABdotAP = BVH_DOT3 (aAB, aAP);
114     const T aACdotAP = BVH_DOT3 (aAC, aAP);
115
116     if (aABdotAP <= static_cast<T> (0) && aACdotAP <= static_cast<T> (0))
117     {
118       return aAP;
119     }
120
121     const typename VectorType<T, N>::Type aBC = theVertC - theVertB;
122     const typename VectorType<T, N>::Type aBP = thePoint - theVertB;
123
124     const T aBAdotBP = -BVH_DOT3 (aAB, aBP);
125     const T aBCdotBP =  BVH_DOT3 (aBC, aBP);
126
127     if (aBAdotBP <= static_cast<T> (0) && aBCdotBP <= static_cast<T> (0))
128     {
129       return aBP;
130     }
131
132     const typename VectorType<T, N>::Type aCP = thePoint - theVertC;
133
134     const T aCBdotCP = -BVH_DOT3 (aBC, aCP);
135     const T aCAdotCP = -BVH_DOT3 (aAC, aCP);
136
137     if (aCAdotCP <= static_cast<T> (0) && aCBdotCP <= static_cast<T> (0))
138     {
139       return aCP;
140     }
141
142     const T aACdotBP = BVH_DOT3 (aAC, aBP);
143
144     const T aVC = aABdotAP * aACdotBP + aBAdotBP * aACdotAP;
145
146     if (aVC <= static_cast<T> (0) && aABdotAP >= static_cast<T> (0) && aBAdotBP >= static_cast<T> (0))
147     {
148       return aAP - aAB * (aABdotAP / (aABdotAP + aBAdotBP));
149     }
150
151     const T aABdotCP = BVH_DOT3 (aAB, aCP);
152
153     const T aVA = aBAdotBP * aCAdotCP - aABdotCP * aACdotBP;
154
155     if (aVA <= static_cast<T> (0) && aBCdotBP >= static_cast<T> (0) && aCBdotCP >= static_cast<T> (0))
156     {
157       return aBP - aBC * (aBCdotBP / (aBCdotBP + aCBdotCP));
158     }
159
160     const T aVB = aABdotCP * aACdotAP + aABdotAP * aCAdotCP;
161
162     if (aVB <= static_cast<T> (0) && aACdotAP >= static_cast<T> (0) && aCAdotCP >= static_cast<T> (0))
163     {
164       return aAP - aAC * (aACdotAP / (aACdotAP + aCAdotCP));
165     }
166
167     const T aNorm = static_cast<T> (1.0) / (aVA + aVB + aVC);
168
169     const T aU = aVA * aNorm;
170     const T aV = aVB * aNorm;
171
172     return thePoint - (theVertA * aU + theVertB * aV + theVertC * (static_cast<T> (1.0) - aU - aV));
173   }
174
175   //=======================================================================
176   //function : SquareDistanceToObject
177   //purpose  : Computes squared distance from point to BVH triangulation
178   //=======================================================================
179   template<class T, int N>
180   T SquareDistanceToObject (BVH_Object<T, N>* theObject,
181     const typename VectorType<T, N>::Type& thePnt, Standard_Boolean& theIsOutside)
182   {
183     Standard_STATIC_ASSERT (N == 3 || N == 4);
184
185     T aMinDistance = std::numeric_limits<T>::max();
186
187     BVH_Triangulation<T, N>* aTriangulation =
188       dynamic_cast<BVH_Triangulation<T, N>*> (theObject);
189
190     Standard_ASSERT_RETURN (aTriangulation != NULL,
191       "Error: Unsupported BVH object (non triangulation)", aMinDistance);
192
193     const NCollection_Handle<BVH_Tree<T, N> >& aBVH = aTriangulation->BVH();
194
195     if (aBVH.IsNull())
196     {
197       return Standard_False;
198     }
199
200     std::pair<Standard_Integer, T> aStack[32];
201
202     Standard_Integer aHead = -1;
203     Standard_Integer aNode =  0; // root node
204
205     for (;;)
206     {
207       BVH_Vec4i aData = aBVH->NodeInfoBuffer()[aNode];
208
209       if (aData.x() == 0) // if inner node
210       {
211         const T aDistToLft = DistanceToBox<T, N> (thePnt,
212                                                   aBVH->MinPoint (aData.y()),
213                                                   aBVH->MaxPoint (aData.y()));
214
215         const T aDistToRgh = DistanceToBox<T, N> (thePnt,
216                                                   aBVH->MinPoint (aData.z()),
217                                                   aBVH->MaxPoint (aData.z()));
218
219         const Standard_Boolean aHitLft = aDistToLft <= aMinDistance;
220         const Standard_Boolean aHitRgh = aDistToRgh <= aMinDistance;
221
222         if (aHitLft & aHitRgh)
223         {
224           aNode = (aDistToLft < aDistToRgh) ? aData.y() : aData.z();
225
226           aStack[++aHead] = std::pair<Standard_Integer, T> (
227             aDistToLft < aDistToRgh ? aData.z() : aData.y(), Max (aDistToLft, aDistToRgh));
228         }
229         else
230         {
231           if (aHitLft | aHitRgh)
232           {
233             aNode = aHitLft ? aData.y() : aData.z();
234           }
235           else
236           {
237             if (aHead < 0)
238               return aMinDistance;
239
240             std::pair<Standard_Integer, T>& anInfo = aStack[aHead--];
241
242             while (anInfo.second > aMinDistance)
243             {
244               if (aHead < 0)
245                 return aMinDistance;
246
247               anInfo = aStack[aHead--];
248             }
249
250             aNode = anInfo.first;
251           }
252         }
253       }
254       else  // if leaf node
255       {
256         for (Standard_Integer aTrgIdx = aData.y(); aTrgIdx <= aData.z(); ++aTrgIdx)
257         {
258           const BVH_Vec4i aTriangle = aTriangulation->Elements[aTrgIdx];
259
260           const typename VectorType<T, N>::Type aVertex0 = aTriangulation->Vertices[aTriangle.x()];
261           const typename VectorType<T, N>::Type aVertex1 = aTriangulation->Vertices[aTriangle.y()];
262           const typename VectorType<T, N>::Type aVertex2 = aTriangulation->Vertices[aTriangle.z()];
263
264           const typename VectorType<T, N>::Type aDirection =
265             DirectionToNearestPoint<T, N> (thePnt, aVertex0, aVertex1, aVertex2);
266
267           const T aDistance = BVH_DOT3 (aDirection, aDirection);
268
269           if (aDistance < aMinDistance)
270           {
271             aMinDistance = aDistance;
272
273             typename VectorType<T, N>::Type aTrgEdges[] = { aVertex1 - aVertex0,
274                                                             aVertex2 - aVertex0 };
275
276             typename VectorType<T, N>::Type aTrgNormal;
277             
278             aTrgNormal.x() = aTrgEdges[0].y() * aTrgEdges[1].z() - aTrgEdges[0].z() * aTrgEdges[1].y();
279             aTrgNormal.y() = aTrgEdges[0].z() * aTrgEdges[1].x() - aTrgEdges[0].x() * aTrgEdges[1].z();
280             aTrgNormal.z() = aTrgEdges[0].x() * aTrgEdges[1].y() - aTrgEdges[0].y() * aTrgEdges[1].x();
281
282             theIsOutside = BVH_DOT3 (aTrgNormal, aDirection) > 0;
283           }
284         }
285
286         if (aHead < 0)
287           return aMinDistance;
288
289         std::pair<Standard_Integer, T>& anInfo = aStack[aHead--];
290
291         while (anInfo.second > aMinDistance)
292         {
293           if (aHead < 0)
294             return aMinDistance;
295
296           anInfo = aStack[aHead--];
297         }
298
299         aNode = anInfo.first;
300       }
301     }
302   }
303
304   //=======================================================================
305   //function : SquareDistanceToGeomerty
306   //purpose  : Computes squared distance from point to BVH geometry
307   //=======================================================================
308   template<class T, int N>
309   T SquareDistanceToGeomerty (BVH_Geometry<T, N>& theGeometry,
310     const typename VectorType<T, N>::Type& thePnt, Standard_Boolean& theIsOutside)
311   {
312     Standard_STATIC_ASSERT (N == 3 || N == 4);
313
314     const BVH_Tree<T, N, BVH_BinaryTree>* aBVH = theGeometry.BVH().get();
315
316     if (aBVH == NULL)
317     {
318       return Standard_False;
319     }
320
321     std::pair<Standard_Integer, T> aStack[32];
322
323     Standard_Integer aHead = -1;
324     Standard_Integer aNode =  0; // root node
325
326     T aMinDistance = std::numeric_limits<T>::max();
327
328     for (;;)
329     {
330       BVH_Vec4i aData = aBVH->NodeInfoBuffer()[aNode];
331
332       if (aData.x() == 0) // if inner node
333       {
334         const T aDistToLft = DistanceToBox<T, N> (thePnt,
335                                                   aBVH->MinPoint (aData.y()),
336                                                   aBVH->MaxPoint (aData.y()));
337
338         const T aDistToRgh = DistanceToBox<T, N> (thePnt,
339                                                   aBVH->MinPoint (aData.z()),
340                                                   aBVH->MaxPoint (aData.z()));
341
342         const Standard_Boolean aHitLft = aDistToLft <= aMinDistance;
343         const Standard_Boolean aHitRgh = aDistToRgh <= aMinDistance;
344
345         if (aHitLft & aHitRgh)
346         {
347           aNode = (aDistToLft < aDistToRgh) ? aData.y() : aData.z();
348
349           aStack[++aHead] = std::pair<Standard_Integer, T> (
350             aDistToLft < aDistToRgh ? aData.z() : aData.y(), Max (aDistToLft, aDistToRgh));
351         }
352         else
353         {
354           if (aHitLft | aHitRgh)
355           {
356             aNode = aHitLft ? aData.y() : aData.z();
357           }
358           else
359           {
360             if (aHead < 0)
361               return aMinDistance;
362
363             std::pair<Standard_Integer, T>& anInfo = aStack[aHead--];
364
365             while (anInfo.second > aMinDistance)
366             {
367               if (aHead < 0)
368                 return aMinDistance;
369
370               anInfo = aStack[aHead--];
371             }
372
373             aNode = anInfo.first;
374           }
375         }
376       }
377       else  // if leaf node
378       {
379         Standard_Boolean isOutside = Standard_True;
380
381         const T aDistance = SquareDistanceToObject (
382           theGeometry.Objects()(aNode).operator->(), thePnt, isOutside);
383
384         if (aDistance < aMinDistance)
385         {
386           aMinDistance = aDistance;
387           theIsOutside = isOutside;
388         }
389
390         if (aHead < 0)
391           return aMinDistance;
392
393         std::pair<Standard_Integer, T>& anInfo = aStack[aHead--];
394
395         while (anInfo.second > aMinDistance)
396         {
397           if (aHead < 0)
398             return aMinDistance;
399
400           anInfo = aStack[aHead--];
401         }
402
403         aNode = anInfo.first;
404       }
405     }
406   }
407 }
408
409 #undef BVH_DOT3
410
411 #ifdef HAVE_TBB
412
413 //! Tool object for parallel construction of distance field (uses Intel TBB).
414 template<class T, int N>
415 class BVH_ParallelDistanceFieldBuilder
416 {
417 private:
418
419   //! Input BVH geometry.
420   BVH_Geometry<T, N>* myGeometry;
421
422   //! Output distance field.
423   BVH_DistanceField<T, N>* myOutField;
424
425 public:
426
427   BVH_ParallelDistanceFieldBuilder (BVH_DistanceField<T, N>* theOutField, BVH_Geometry<T, N>* theGeometry)
428   : myGeometry (theGeometry),
429     myOutField (theOutField)
430   {
431     //
432   }
433
434   void operator() (const tbb::blocked_range<Standard_Integer>& theRange) const
435   {
436     myOutField->BuildSlices (*myGeometry, theRange.begin(), theRange.end());
437   }
438 };
439
440 #endif
441
442 // =======================================================================
443 // function : BuildSlices
444 // purpose  : Performs building of distance field for the given Z slices
445 // =======================================================================
446 template<class T, int N>
447 void BVH_DistanceField<T, N>::BuildSlices (BVH_Geometry<T, N>& theGeometry,
448   const Standard_Integer theStartSlice, const Standard_Integer theFinalSlice)
449 {
450   for (Standard_Integer aZ = theStartSlice; aZ < theFinalSlice; ++aZ)
451   {
452     for (Standard_Integer aY = 0; aY < myDimensionY; ++aY)
453     {
454       for (Standard_Integer aX = 0; aX < myDimensionX; ++aX)
455       {
456         BVH_VecNt aCenter;
457         
458         aCenter.x() = myCornerMin.x() + myVoxelSize.x() * (aX + static_cast<T> (0.5));
459         aCenter.y() = myCornerMin.y() + myVoxelSize.y() * (aY + static_cast<T> (0.5));
460         aCenter.z() = myCornerMin.z() + myVoxelSize.z() * (aZ + static_cast<T> (0.5));
461
462         Standard_Boolean isOutside = Standard_True;
463
464         const T aDistance = sqrt (
465           BVH::SquareDistanceToGeomerty<T, N> (theGeometry, aCenter, isOutside));
466
467         Voxel (aX, aY, aZ) = (!myComputeSign || isOutside) ? aDistance : -aDistance;
468       }
469     }
470   }
471 }
472
473 // =======================================================================
474 // function : Build
475 // purpose  : Builds 3D distance field from BVH geometry
476 // =======================================================================
477 template<class T, int N>
478 Standard_Boolean BVH_DistanceField<T, N>::Build (BVH_Geometry<T, N>& theGeometry)
479 {
480   if (theGeometry.Size() == 0)
481   {
482     return Standard_False;
483   }
484
485   const BVH_VecNt aGlobalBoxSize = theGeometry.Box().Size();
486
487   const T aMaxBoxSide = Max (Max (aGlobalBoxSize.x(), aGlobalBoxSize.y()), aGlobalBoxSize.z());
488
489   myDimensionX = static_cast<Standard_Integer> (myMaximumSize * aGlobalBoxSize.x() / aMaxBoxSide);
490   myDimensionY = static_cast<Standard_Integer> (myMaximumSize * aGlobalBoxSize.y() / aMaxBoxSide);
491   myDimensionZ = static_cast<Standard_Integer> (myMaximumSize * aGlobalBoxSize.z() / aMaxBoxSide);
492
493   myDimensionX = Min (myMaximumSize, Max (myDimensionX, 16));
494   myDimensionY = Min (myMaximumSize, Max (myDimensionY, 16));
495   myDimensionZ = Min (myMaximumSize, Max (myDimensionZ, 16));
496
497   const BVH_VecNt aGlobalBoxMin = theGeometry.Box().CornerMin();
498   const BVH_VecNt aGlobalBoxMax = theGeometry.Box().CornerMax();
499
500   const Standard_Integer aVoxelOffset = 2;
501
502   myCornerMin.x() = aGlobalBoxMin.x() - aVoxelOffset * aGlobalBoxSize.x() / (myDimensionX - 2 * aVoxelOffset);
503   myCornerMin.y() = aGlobalBoxMin.y() - aVoxelOffset * aGlobalBoxSize.y() / (myDimensionY - 2 * aVoxelOffset);
504   myCornerMin.z() = aGlobalBoxMin.z() - aVoxelOffset * aGlobalBoxSize.z() / (myDimensionZ - 2 * aVoxelOffset);
505
506   myCornerMax.x() = aGlobalBoxMax.x() + aVoxelOffset * aGlobalBoxSize.x() / (myDimensionX - 2 * aVoxelOffset);
507   myCornerMax.y() = aGlobalBoxMax.y() + aVoxelOffset * aGlobalBoxSize.y() / (myDimensionY - 2 * aVoxelOffset);
508   myCornerMax.z() = aGlobalBoxMax.z() + aVoxelOffset * aGlobalBoxSize.z() / (myDimensionZ - 2 * aVoxelOffset);
509   
510   myVoxelSize.x() = (myCornerMax.x() - myCornerMin.x()) / myDimensionX;
511   myVoxelSize.y() = (myCornerMax.y() - myCornerMin.y()) / myDimensionY;
512   myVoxelSize.z() = (myCornerMax.z() - myCornerMin.z()) / myDimensionZ;
513
514 #ifdef HAVE_TBB
515   
516   tbb::parallel_for (tbb::blocked_range<Standard_Integer> (0, myDimensionZ),
517     BVH_ParallelDistanceFieldBuilder<T, N> (this, &theGeometry));
518
519 #else
520
521   BuildSlices (theGeometry, 0, myDimensionZ);
522
523 #endif
524
525   return Standard_True;
526 }