0029177: Foundation Classes - Adapt BVH package for use of OSD_Parallel
[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),
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// =======================================================================
38template<class T, int N>
39BVH_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
52namespace 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
f5b72419 182 const opencascade::handle<BVH_Tree<T, N> >& aBVH = aTriangulation->BVH();
df932fdf 183 if (aBVH.IsNull())
184 {
185 return Standard_False;
186 }
187
f5b72419 188 std::pair<Standard_Integer, T> aStack[BVH_Constants_MaxTreeDepth];
df932fdf 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
f2474958 302 const BVH_Tree<T, N, BVH_BinaryTree>* aBVH = theGeometry.BVH().get();
df932fdf 303
f2474958 304 if (aBVH == NULL)
df932fdf 305 {
306 return Standard_False;
307 }
308
f5b72419 309 std::pair<Standard_Integer, T> aStack[BVH_Constants_MaxTreeDepth];
df932fdf 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
df932fdf 399//! Tool object for parallel construction of distance field (uses Intel TBB).
400template<class T, int N>
401class BVH_ParallelDistanceFieldBuilder
402{
403private:
404
405 //! Input BVH geometry.
406 BVH_Geometry<T, N>* myGeometry;
407
408 //! Output distance field.
409 BVH_DistanceField<T, N>* myOutField;
410
411public:
412
413 BVH_ParallelDistanceFieldBuilder (BVH_DistanceField<T, N>* theOutField, BVH_Geometry<T, N>* theGeometry)
414 : myGeometry (theGeometry),
415 myOutField (theOutField)
416 {
417 //
418 }
419
da555fc2 420 void operator() (const Standard_Integer theIndex) const
df932fdf 421 {
da555fc2 422 myOutField->BuildSlices (*myGeometry, theIndex, theIndex + 1);
df932fdf 423 }
424};
425
df932fdf 426// =======================================================================
427// function : BuildSlices
428// purpose : Performs building of distance field for the given Z slices
429// =======================================================================
430template<class T, int N>
431void 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// =======================================================================
461template<class T, int N>
462Standard_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
da555fc2 498 OSD_Parallel::For (0, myDimensionZ, BVH_ParallelDistanceFieldBuilder<T, N> (this, &theGeometry));
df932fdf 499
500 return Standard_True;
501}