0029702: Foundation Classes - Introduce possibility to control parallel execution...
[occt.git] / src / BVH / BVH_DistanceField.lxx
CommitLineData
df932fdf 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
df932fdf 16#include <BVH_Triangulation.hxx>
da555fc2 17#include <OSD_Parallel.hxx>
df932fdf 18
19// =======================================================================
20// function : BVH_DistanceField
21// purpose :
22// =======================================================================
23template<class T, int N>
24BVH_DistanceField<T, N>::BVH_DistanceField (const Standard_Integer theMaximumSize,
25 const Standard_Boolean theComputeSign)
26: myMaximumSize (theMaximumSize),
3c7a61ea 27 myComputeSign (theComputeSign),
28 myIsParallel (Standard_False)
df932fdf 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// =======================================================================
39template<class T, int N>
40BVH_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
53namespace 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
f5b72419 183 const opencascade::handle<BVH_Tree<T, N> >& aBVH = aTriangulation->BVH();
df932fdf 184 if (aBVH.IsNull())
185 {
186 return Standard_False;
187 }
188
f5b72419 189 std::pair<Standard_Integer, T> aStack[BVH_Constants_MaxTreeDepth];
df932fdf 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
f2474958 303 const BVH_Tree<T, N, BVH_BinaryTree>* aBVH = theGeometry.BVH().get();
df932fdf 304
f2474958 305 if (aBVH == NULL)
df932fdf 306 {
307 return Standard_False;
308 }
309
f5b72419 310 std::pair<Standard_Integer, T> aStack[BVH_Constants_MaxTreeDepth];
df932fdf 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
df932fdf 400//! Tool object for parallel construction of distance field (uses Intel TBB).
401template<class T, int N>
402class BVH_ParallelDistanceFieldBuilder
403{
404private:
405
406 //! Input BVH geometry.
407 BVH_Geometry<T, N>* myGeometry;
408
409 //! Output distance field.
410 BVH_DistanceField<T, N>* myOutField;
411
412public:
413
414 BVH_ParallelDistanceFieldBuilder (BVH_DistanceField<T, N>* theOutField, BVH_Geometry<T, N>* theGeometry)
415 : myGeometry (theGeometry),
416 myOutField (theOutField)
417 {
418 //
419 }
420
da555fc2 421 void operator() (const Standard_Integer theIndex) const
df932fdf 422 {
da555fc2 423 myOutField->BuildSlices (*myGeometry, theIndex, theIndex + 1);
df932fdf 424 }
425};
426
df932fdf 427// =======================================================================
428// function : BuildSlices
429// purpose : Performs building of distance field for the given Z slices
430// =======================================================================
431template<class T, int N>
432void 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// =======================================================================
462template<class T, int N>
463Standard_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
3c7a61ea 499 OSD_Parallel::For (0, myDimensionZ, BVH_ParallelDistanceFieldBuilder<T, N> (this, &theGeometry), !IsParallel());
df932fdf 500
501 return Standard_True;
502}