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