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