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