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