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