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) |
27 | : myMaximumSize (theMaximumSize), |
3c7a61ea |
28 | myComputeSign (theComputeSign), |
29 | myIsParallel (Standard_False) |
df932fdf |
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 | //======================================================================= |
7c1a8210 |
167 | //function : SquareDistanceToPoint |
168 | //purpose : Abstract class to compute squared distance from point to BVH tree |
df932fdf |
169 | //======================================================================= |
7c1a8210 |
170 | template<class T, int N, class BVHSetType> |
171 | class SquareDistanceToPoint : public BVH_Distance<T,N,typename VectorType<T, N>::Type, BVHSetType> |
df932fdf |
172 | { |
7c1a8210 |
173 | public: |
174 | typedef typename VectorType<T, N>::Type BVH_VecNt; |
df932fdf |
175 | |
7c1a8210 |
176 | public: |
177 | SquareDistanceToPoint() |
178 | : BVH_Distance<T, N, BVH_VecNt, BVHSetType>(), |
179 | myIsOutside (Standard_True) |
180 | {} |
df932fdf |
181 | |
7c1a8210 |
182 | public: |
df932fdf |
183 | |
7c1a8210 |
184 | //! IsOutside |
185 | Standard_Boolean IsOutside() const { return myIsOutside; } |
df932fdf |
186 | |
7c1a8210 |
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 |
df932fdf |
193 | { |
7c1a8210 |
194 | theMetric = DistanceToBox<T, N> (this->myObject, theCMin, theCMax); |
195 | return theMetric > this->myDistance; |
df932fdf |
196 | } |
197 | |
7c1a8210 |
198 | public: |
df932fdf |
199 | |
7c1a8210 |
200 | //! Redefine the Stop to never stop the selection |
201 | virtual Standard_Boolean Stop() const Standard_OVERRIDE |
df932fdf |
202 | { |
7c1a8210 |
203 | return Standard_False; |
204 | } |
df932fdf |
205 | |
7c1a8210 |
206 | protected: |
df932fdf |
207 | |
7c1a8210 |
208 | Standard_Boolean myIsOutside; |
209 | }; |
df932fdf |
210 | |
7c1a8210 |
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; |
df932fdf |
220 | |
7c1a8210 |
221 | public: |
222 | //! Constructor |
223 | PointTriangulationSquareDistance() |
224 | : SquareDistanceToPoint<T, N, BVH_Triangulation<T, N>>() |
225 | { |
226 | } |
df932fdf |
227 | |
7c1a8210 |
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]; |
df932fdf |
234 | |
7c1a8210 |
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()]; |
df932fdf |
238 | |
7c1a8210 |
239 | const BVH_VecNt aDirection = |
240 | DirectionToNearestPoint<T, N> (this->myObject, aVertex0, aVertex1, aVertex2); |
df932fdf |
241 | |
7c1a8210 |
242 | const T aDistance = BVH_DOT3 (aDirection, aDirection); |
df932fdf |
243 | |
7c1a8210 |
244 | if (aDistance < this->myDistance) |
245 | { |
246 | this->myDistance = aDistance; |
df932fdf |
247 | |
7c1a8210 |
248 | BVH_VecNt aTrgEdges[] = { aVertex1 - aVertex0, aVertex2 - aVertex0 }; |
df932fdf |
249 | |
7c1a8210 |
250 | BVH_VecNt aTrgNormal; |
df932fdf |
251 | |
7c1a8210 |
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 (); |
df932fdf |
255 | |
7c1a8210 |
256 | this->myIsOutside = BVH_DOT3 (aTrgNormal, aDirection) > 0; |
257 | |
258 | return Standard_True; |
df932fdf |
259 | } |
7c1a8210 |
260 | |
261 | return Standard_False; |
df932fdf |
262 | } |
7c1a8210 |
263 | }; |
df932fdf |
264 | |
265 | //======================================================================= |
7c1a8210 |
266 | //function : SquareDistanceToObject |
267 | //purpose : Computes squared distance from point to BVH triangulation |
df932fdf |
268 | //======================================================================= |
269 | template<class T, int N> |
7c1a8210 |
270 | T SquareDistanceToObject (BVH_Object<T, N>* theObject, |
df932fdf |
271 | const typename VectorType<T, N>::Type& thePnt, Standard_Boolean& theIsOutside) |
272 | { |
273 | Standard_STATIC_ASSERT (N == 3 || N == 4); |
274 | |
7c1a8210 |
275 | T aMinDistance = std::numeric_limits<T>::max(); |
df932fdf |
276 | |
7c1a8210 |
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()) |
df932fdf |
285 | { |
286 | return Standard_False; |
287 | } |
288 | |
7c1a8210 |
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 | } |
df932fdf |
296 | |
7c1a8210 |
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; |
df932fdf |
306 | |
7c1a8210 |
307 | public: |
308 | //! Constructor |
309 | PointGeometrySquareDistance() |
310 | : SquareDistanceToPoint<T, N, BVH_Geometry<T, N>>() |
311 | { |
312 | } |
df932fdf |
313 | |
7c1a8210 |
314 | public: |
315 | // Accepting the element |
316 | virtual Standard_Boolean Accept (const Standard_Integer theIndex, |
317 | const T&) Standard_OVERRIDE |
df932fdf |
318 | { |
7c1a8210 |
319 | Standard_Boolean isOutside = Standard_True; |
320 | const T aDistance = SquareDistanceToObject ( |
321 | this->myBVHSet->Objects ()(theIndex).operator->(), this->myObject, isOutside); |
df932fdf |
322 | |
7c1a8210 |
323 | if (aDistance < this->myDistance) |
df932fdf |
324 | { |
7c1a8210 |
325 | this->myDistance = aDistance; |
326 | this->myIsOutside = isOutside; |
df932fdf |
327 | |
7c1a8210 |
328 | return Standard_True; |
329 | } |
330 | return Standard_False; |
331 | } |
332 | }; |
df932fdf |
333 | |
7c1a8210 |
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); |
df932fdf |
343 | |
7c1a8210 |
344 | const BVH_Tree<T, N, BVH_BinaryTree>* aBVH = theGeometry.BVH().get(); |
df932fdf |
345 | |
7c1a8210 |
346 | if (aBVH == NULL) |
347 | { |
348 | return Standard_False; |
df932fdf |
349 | } |
7c1a8210 |
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(); |
df932fdf |
357 | } |
358 | } |
359 | |
360 | #undef BVH_DOT3 |
361 | |
df932fdf |
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 | |
da555fc2 |
383 | void operator() (const Standard_Integer theIndex) const |
df932fdf |
384 | { |
da555fc2 |
385 | myOutField->BuildSlices (*myGeometry, theIndex, theIndex + 1); |
df932fdf |
386 | } |
387 | }; |
388 | |
df932fdf |
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 | |
3c7a61ea |
461 | OSD_Parallel::For (0, myDimensionZ, BVH_ParallelDistanceFieldBuilder<T, N> (this, &theGeometry), !IsParallel()); |
df932fdf |
462 | |
463 | return Standard_True; |
464 | } |