Directory: | ./ |
---|---|
File: | include/coal/internal/traversal_node_bvh_shape.h |
Date: | 2025-04-01 09:23:31 |
Exec | Total | Coverage | |
---|---|---|---|
Lines: | 107 | 160 | 66.9% |
Branches: | 42 | 104 | 40.4% |
Line | Branch | Exec | Source |
---|---|---|---|
1 | /* | ||
2 | * Software License Agreement (BSD License) | ||
3 | * | ||
4 | * Copyright (c) 2011-2014, Willow Garage, Inc. | ||
5 | * Copyright (c) 2014-2015, Open Source Robotics Foundation | ||
6 | * All rights reserved. | ||
7 | * | ||
8 | * Redistribution and use in source and binary forms, with or without | ||
9 | * modification, are permitted provided that the following conditions | ||
10 | * are met: | ||
11 | * | ||
12 | * * Redistributions of source code must retain the above copyright | ||
13 | * notice, this list of conditions and the following disclaimer. | ||
14 | * * Redistributions in binary form must reproduce the above | ||
15 | * copyright notice, this list of conditions and the following | ||
16 | * disclaimer in the documentation and/or other materials provided | ||
17 | * with the distribution. | ||
18 | * * Neither the name of Open Source Robotics Foundation nor the names of its | ||
19 | * contributors may be used to endorse or promote products derived | ||
20 | * from this software without specific prior written permission. | ||
21 | * | ||
22 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS | ||
23 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT | ||
24 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS | ||
25 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE | ||
26 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, | ||
27 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, | ||
28 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; | ||
29 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER | ||
30 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT | ||
31 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN | ||
32 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE | ||
33 | * POSSIBILITY OF SUCH DAMAGE. | ||
34 | */ | ||
35 | |||
36 | /** \author Jia Pan */ | ||
37 | |||
38 | #ifndef COAL_TRAVERSAL_NODE_MESH_SHAPE_H | ||
39 | #define COAL_TRAVERSAL_NODE_MESH_SHAPE_H | ||
40 | |||
41 | /// @cond INTERNAL | ||
42 | |||
43 | #include "coal/collision_data.h" | ||
44 | #include "coal/shape/geometric_shapes.h" | ||
45 | #include "coal/narrowphase/narrowphase.h" | ||
46 | #include "coal/shape/geometric_shapes_utility.h" | ||
47 | #include "coal/internal/traversal_node_base.h" | ||
48 | #include "coal/internal/traversal.h" | ||
49 | #include "coal/BVH/BVH_model.h" | ||
50 | #include "coal/internal/shape_shape_func.h" | ||
51 | |||
52 | namespace coal { | ||
53 | |||
54 | /// @addtogroup Traversal_For_Collision | ||
55 | /// @{ | ||
56 | |||
57 | /// @brief Traversal node for collision between BVH and shape | ||
58 | template <typename BV, typename S> | ||
59 | class BVHShapeCollisionTraversalNode : public CollisionTraversalNodeBase { | ||
60 | public: | ||
61 | 252 | BVHShapeCollisionTraversalNode(const CollisionRequest& request) | |
62 |
1/2✓ Branch 2 taken 126 times.
✗ Branch 3 not taken.
|
252 | : CollisionTraversalNodeBase(request) { |
63 | 252 | model1 = NULL; | |
64 | 252 | model2 = NULL; | |
65 | |||
66 | 252 | num_bv_tests = 0; | |
67 | 252 | num_leaf_tests = 0; | |
68 | 252 | query_time_seconds = 0.0; | |
69 | } | ||
70 | |||
71 | /// @brief Whether the BV node in the first BVH tree is leaf | ||
72 | 7688 | bool isFirstNodeLeaf(unsigned int b) const { | |
73 | 7688 | return model1->getBV(b).isLeaf(); | |
74 | } | ||
75 | |||
76 | /// @brief Obtain the left child of BV node in the first BVH | ||
77 | 3840 | int getFirstLeftChild(unsigned int b) const { | |
78 | 3840 | return model1->getBV(b).leftChild(); | |
79 | } | ||
80 | |||
81 | /// @brief Obtain the right child of BV node in the first BVH | ||
82 | 3840 | int getFirstRightChild(unsigned int b) const { | |
83 | 3840 | return model1->getBV(b).rightChild(); | |
84 | } | ||
85 | |||
86 | const BVHModel<BV>* model1; | ||
87 | const S* model2; | ||
88 | BV model2_bv; | ||
89 | |||
90 | mutable int num_bv_tests; | ||
91 | mutable int num_leaf_tests; | ||
92 | mutable Scalar query_time_seconds; | ||
93 | }; | ||
94 | |||
95 | /// @brief Traversal node for collision between mesh and shape | ||
96 | template <typename BV, typename S, | ||
97 | int _Options = RelativeTransformationIsIdentity> | ||
98 | class MeshShapeCollisionTraversalNode | ||
99 | : public BVHShapeCollisionTraversalNode<BV, S> { | ||
100 | public: | ||
101 | enum { | ||
102 | Options = _Options, | ||
103 | RTIsIdentity = _Options & RelativeTransformationIsIdentity | ||
104 | }; | ||
105 | |||
106 | 252 | MeshShapeCollisionTraversalNode(const CollisionRequest& request) | |
107 | 252 | : BVHShapeCollisionTraversalNode<BV, S>(request) { | |
108 | 252 | vertices = NULL; | |
109 | 252 | tri_indices = NULL; | |
110 | |||
111 | 252 | nsolver = NULL; | |
112 | } | ||
113 | |||
114 | /// test between BV b1 and shape | ||
115 | /// @param b1 BV to test, | ||
116 | /// @retval sqrDistLowerBound square of a lower bound of the minimal | ||
117 | /// distance between bounding volumes. | ||
118 | /// @brief BV culling test in one BVTT node | ||
119 | 6870 | bool BVDisjoints(unsigned int b1, unsigned int /*b2*/, | |
120 | Scalar& sqrDistLowerBound) const { | ||
121 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 3435 times.
|
6870 | if (this->enable_statistics) this->num_bv_tests++; |
122 | bool disjoint; | ||
123 | if (RTIsIdentity) | ||
124 | ✗ | disjoint = !this->model1->getBV(b1).bv.overlap( | |
125 | ✗ | this->model2_bv, this->request, sqrDistLowerBound); | |
126 | else | ||
127 | 6870 | disjoint = !overlap(this->tf1.getRotation(), this->tf1.getTranslation(), | |
128 | 6870 | this->model1->getBV(b1).bv, this->model2_bv, | |
129 | this->request, sqrDistLowerBound); | ||
130 |
2/2✓ Branch 0 taken 1515 times.
✓ Branch 1 taken 1920 times.
|
6870 | if (disjoint) |
131 | 3030 | internal::updateDistanceLowerBoundFromBV(this->request, *this->result, | |
132 | sqrDistLowerBound); | ||
133 |
3/4✓ Branch 0 taken 1515 times.
✓ Branch 1 taken 1920 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 1515 times.
|
6870 | assert(!disjoint || sqrDistLowerBound > 0); |
134 | 6870 | return disjoint; | |
135 | } | ||
136 | |||
137 | /// @brief Intersection testing between leaves (one triangle and one shape) | ||
138 | 818 | void leafCollides(unsigned int b1, unsigned int /*b2*/, | |
139 | Scalar& sqrDistLowerBound) const { | ||
140 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 409 times.
|
818 | if (this->enable_statistics) this->num_leaf_tests++; |
141 | 818 | const BVNode<BV>& node = this->model1->getBV(b1); | |
142 | |||
143 | 818 | int primitive_id = node.primitiveId(); | |
144 | |||
145 | 818 | const Triangle& tri_id = this->tri_indices[primitive_id]; | |
146 |
1/2✓ Branch 2 taken 409 times.
✗ Branch 3 not taken.
|
818 | const TriangleP tri(this->vertices[tri_id[0]], this->vertices[tri_id[1]], |
147 | 818 | this->vertices[tri_id[2]]); | |
148 | |||
149 | // When reaching this point, `this->solver` has already been set up | ||
150 | // by the CollisionRequest `this->request`. | ||
151 | // The only thing we need to (and can) pass to `ShapeShapeDistance` is | ||
152 | // whether or not penetration information is should be computed in case of | ||
153 | // collision. | ||
154 | 818 | const bool compute_penetration = | |
155 |
1/4✗ Branch 0 not taken.
✓ Branch 1 taken 409 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
|
818 | this->request.enable_contact || (this->request.security_margin < 0); |
156 |
3/6✓ Branch 1 taken 409 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 409 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 409 times.
✗ Branch 8 not taken.
|
818 | Vec3s c1, c2, normal; |
157 | Scalar distance; | ||
158 | |||
159 | if (RTIsIdentity) { | ||
160 | ✗ | static const Transform3s Id; | |
161 | ✗ | distance = internal::ShapeShapeDistance<TriangleP, S>( | |
162 | ✗ | &tri, Id, this->model2, this->tf2, this->nsolver, compute_penetration, | |
163 | c1, c2, normal); | ||
164 | } else { | ||
165 | 1636 | distance = internal::ShapeShapeDistance<TriangleP, S>( | |
166 |
1/2✓ Branch 1 taken 409 times.
✗ Branch 2 not taken.
|
818 | &tri, this->tf1, this->model2, this->tf2, this->nsolver, |
167 | compute_penetration, c1, c2, normal); | ||
168 | } | ||
169 | 818 | const Scalar distToCollision = distance - this->request.security_margin; | |
170 | |||
171 |
1/2✓ Branch 1 taken 409 times.
✗ Branch 2 not taken.
|
818 | internal::updateDistanceLowerBoundFromLeaf(this->request, *(this->result), |
172 | distToCollision, c1, c2, normal); | ||
173 | |||
174 |
2/2✓ Branch 0 taken 46 times.
✓ Branch 1 taken 363 times.
|
818 | if (distToCollision <= this->request.collision_distance_threshold) { |
175 | 92 | sqrDistLowerBound = 0; | |
176 |
1/2✓ Branch 1 taken 46 times.
✗ Branch 2 not taken.
|
92 | if (this->result->numContacts() < this->request.num_max_contacts) { |
177 |
2/4✓ Branch 1 taken 46 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 46 times.
✗ Branch 5 not taken.
|
92 | this->result->addContact(Contact(this->model1, this->model2, |
178 | primitive_id, Contact::NONE, c1, c2, | ||
179 | normal, distance)); | ||
180 |
1/2✗ Branch 1 not taken.
✓ Branch 2 taken 46 times.
|
92 | assert(this->result->isCollision()); |
181 | } | ||
182 | } else { | ||
183 | 726 | sqrDistLowerBound = distToCollision * distToCollision; | |
184 | } | ||
185 | |||
186 |
3/4✓ Branch 1 taken 361 times.
✓ Branch 2 taken 48 times.
✗ Branch 3 not taken.
✓ Branch 4 taken 361 times.
|
818 | assert(this->result->isCollision() || sqrDistLowerBound > 0); |
187 | 818 | } // leafCollides | |
188 | |||
189 | Vec3s* vertices; | ||
190 | Triangle* tri_indices; | ||
191 | |||
192 | const GJKSolver* nsolver; | ||
193 | }; | ||
194 | |||
195 | /// @} | ||
196 | |||
197 | /// @addtogroup Traversal_For_Distance | ||
198 | /// @{ | ||
199 | |||
200 | /// @brief Traversal node for distance computation between BVH and shape | ||
201 | template <typename BV, typename S> | ||
202 | class BVHShapeDistanceTraversalNode : public DistanceTraversalNodeBase { | ||
203 | public: | ||
204 |
1/2✓ Branch 2 taken 102 times.
✗ Branch 3 not taken.
|
204 | BVHShapeDistanceTraversalNode() : DistanceTraversalNodeBase() { |
205 | 204 | model1 = NULL; | |
206 | 204 | model2 = NULL; | |
207 | |||
208 | 204 | num_bv_tests = 0; | |
209 | 204 | num_leaf_tests = 0; | |
210 | 204 | query_time_seconds = 0.0; | |
211 | } | ||
212 | |||
213 | /// @brief Whether the BV node in the first BVH tree is leaf | ||
214 | 13654 | bool isFirstNodeLeaf(unsigned int b) const { | |
215 | 13654 | return model1->getBV(b).isLeaf(); | |
216 | } | ||
217 | |||
218 | /// @brief Obtain the left child of BV node in the first BVH | ||
219 | 10796 | int getFirstLeftChild(unsigned int b) const { | |
220 | 10796 | return model1->getBV(b).leftChild(); | |
221 | } | ||
222 | |||
223 | /// @brief Obtain the right child of BV node in the first BVH | ||
224 | 10796 | int getFirstRightChild(unsigned int b) const { | |
225 | 10796 | return model1->getBV(b).rightChild(); | |
226 | } | ||
227 | |||
228 | /// @brief BV culling test in one BVTT node | ||
229 | ✗ | Scalar BVDistanceLowerBound(unsigned int b1, unsigned int /*b2*/) const { | |
230 | ✗ | return model1->getBV(b1).bv.distance(model2_bv); | |
231 | } | ||
232 | |||
233 | const BVHModel<BV>* model1; | ||
234 | const S* model2; | ||
235 | BV model2_bv; | ||
236 | |||
237 | mutable int num_bv_tests; | ||
238 | mutable int num_leaf_tests; | ||
239 | mutable Scalar query_time_seconds; | ||
240 | }; | ||
241 | |||
242 | /// @brief Traversal node for distance computation between shape and BVH | ||
243 | template <typename S, typename BV> | ||
244 | class ShapeBVHDistanceTraversalNode : public DistanceTraversalNodeBase { | ||
245 | public: | ||
246 | ShapeBVHDistanceTraversalNode() : DistanceTraversalNodeBase() { | ||
247 | model1 = NULL; | ||
248 | model2 = NULL; | ||
249 | |||
250 | num_bv_tests = 0; | ||
251 | num_leaf_tests = 0; | ||
252 | query_time_seconds = 0.0; | ||
253 | } | ||
254 | |||
255 | /// @brief Whether the BV node in the second BVH tree is leaf | ||
256 | bool isSecondNodeLeaf(unsigned int b) const { | ||
257 | return model2->getBV(b).isLeaf(); | ||
258 | } | ||
259 | |||
260 | /// @brief Obtain the left child of BV node in the second BVH | ||
261 | int getSecondLeftChild(unsigned int b) const { | ||
262 | return model2->getBV(b).leftChild(); | ||
263 | } | ||
264 | |||
265 | /// @brief Obtain the right child of BV node in the second BVH | ||
266 | int getSecondRightChild(unsigned int b) const { | ||
267 | return model2->getBV(b).rightChild(); | ||
268 | } | ||
269 | |||
270 | /// @brief BV culling test in one BVTT node | ||
271 | Scalar BVDistanceLowerBound(unsigned int /*b1*/, unsigned int b2) const { | ||
272 | return model1_bv.distance(model2->getBV(b2).bv); | ||
273 | } | ||
274 | |||
275 | const S* model1; | ||
276 | const BVHModel<BV>* model2; | ||
277 | BV model1_bv; | ||
278 | |||
279 | mutable int num_bv_tests; | ||
280 | mutable int num_leaf_tests; | ||
281 | mutable Scalar query_time_seconds; | ||
282 | }; | ||
283 | |||
284 | /// @brief Traversal node for distance between mesh and shape | ||
285 | template <typename BV, typename S> | ||
286 | class MeshShapeDistanceTraversalNode | ||
287 | : public BVHShapeDistanceTraversalNode<BV, S> { | ||
288 | public: | ||
289 | 204 | MeshShapeDistanceTraversalNode() : BVHShapeDistanceTraversalNode<BV, S>() { | |
290 | 204 | vertices = NULL; | |
291 | 204 | tri_indices = NULL; | |
292 | |||
293 | 204 | rel_err = 0; | |
294 | 204 | abs_err = 0; | |
295 | |||
296 | 204 | nsolver = NULL; | |
297 | } | ||
298 | |||
299 | /// @brief Distance testing between leaves (one triangle and one shape) | ||
300 | ✗ | void leafComputeDistance(unsigned int b1, unsigned int /*b2*/) const { | |
301 | ✗ | if (this->enable_statistics) this->num_leaf_tests++; | |
302 | |||
303 | ✗ | const BVNode<BV>& node = this->model1->getBV(b1); | |
304 | |||
305 | ✗ | int primitive_id = node.primitiveId(); | |
306 | |||
307 | ✗ | const Triangle& tri_id = tri_indices[primitive_id]; | |
308 | ✗ | const TriangleP tri(this->vertices[tri_id[0]], this->vertices[tri_id[1]], | |
309 | ✗ | this->vertices[tri_id[2]]); | |
310 | |||
311 | ✗ | Vec3s p1, p2, normal; | |
312 | ✗ | const Scalar distance = internal::ShapeShapeDistance<TriangleP, S>( | |
313 | ✗ | &tri, this->tf1, this->model2, this->tf2, this->nsolver, | |
314 | ✗ | this->request.enable_signed_distance, p1, p2, normal); | |
315 | |||
316 | ✗ | this->result->update(distance, this->model1, this->model2, primitive_id, | |
317 | DistanceResult::NONE, p1, p2, normal); | ||
318 | } | ||
319 | |||
320 | /// @brief Whether the traversal process can stop early | ||
321 | 21592 | bool canStop(Scalar c) const { | |
322 |
2/2✓ Branch 0 taken 4071 times.
✓ Branch 1 taken 6725 times.
|
21592 | if ((c >= this->result->min_distance - abs_err) && |
323 |
1/2✓ Branch 0 taken 4071 times.
✗ Branch 1 not taken.
|
8142 | (c * (1 + rel_err) >= this->result->min_distance)) |
324 | 8142 | return true; | |
325 | 13450 | return false; | |
326 | } | ||
327 | |||
328 | Vec3s* vertices; | ||
329 | Triangle* tri_indices; | ||
330 | |||
331 | Scalar rel_err; | ||
332 | Scalar abs_err; | ||
333 | |||
334 | const GJKSolver* nsolver; | ||
335 | }; | ||
336 | |||
337 | /// @cond IGNORE | ||
338 | namespace details { | ||
339 | |||
340 | template <typename BV, typename S> | ||
341 | 2858 | void meshShapeDistanceOrientedNodeleafComputeDistance( | |
342 | unsigned int b1, unsigned int /* b2 */, const BVHModel<BV>* model1, | ||
343 | const S& model2, Vec3s* vertices, Triangle* tri_indices, | ||
344 | const Transform3s& tf1, const Transform3s& tf2, const GJKSolver* nsolver, | ||
345 | bool enable_statistics, int& num_leaf_tests, const DistanceRequest& request, | ||
346 | DistanceResult& result) { | ||
347 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 1429 times.
|
2858 | if (enable_statistics) num_leaf_tests++; |
348 | |||
349 | 2858 | const BVNode<BV>& node = model1->getBV(b1); | |
350 | 2858 | int primitive_id = node.primitiveId(); | |
351 | |||
352 | 2858 | const Triangle& tri_id = tri_indices[primitive_id]; | |
353 |
1/2✓ Branch 2 taken 1429 times.
✗ Branch 3 not taken.
|
2858 | const TriangleP tri(vertices[tri_id[0]], vertices[tri_id[1]], |
354 | 2858 | vertices[tri_id[2]]); | |
355 | |||
356 |
3/6✓ Branch 1 taken 1429 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1429 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1429 times.
✗ Branch 8 not taken.
|
2858 | Vec3s p1, p2, normal; |
357 | 5716 | const Scalar distance = internal::ShapeShapeDistance<TriangleP, S>( | |
358 |
1/2✓ Branch 1 taken 1429 times.
✗ Branch 2 not taken.
|
2858 | &tri, tf1, &model2, tf2, nsolver, request.enable_signed_distance, p1, p2, |
359 | normal); | ||
360 | |||
361 |
1/2✓ Branch 1 taken 1429 times.
✗ Branch 2 not taken.
|
2858 | result.update(distance, model1, &model2, primitive_id, DistanceResult::NONE, |
362 | p1, p2, normal); | ||
363 | } | ||
364 | |||
365 | template <typename BV, typename S> | ||
366 | 204 | static inline void distancePreprocessOrientedNode( | |
367 | const BVHModel<BV>* model1, Vec3s* vertices, Triangle* tri_indices, | ||
368 | int init_tri_id, const S& model2, const Transform3s& tf1, | ||
369 | const Transform3s& tf2, const GJKSolver* nsolver, | ||
370 | const DistanceRequest& request, DistanceResult& result) { | ||
371 | 204 | const Triangle& tri_id = tri_indices[init_tri_id]; | |
372 |
1/2✓ Branch 2 taken 102 times.
✗ Branch 3 not taken.
|
204 | const TriangleP tri(vertices[tri_id[0]], vertices[tri_id[1]], |
373 | 204 | vertices[tri_id[2]]); | |
374 | |||
375 |
3/6✓ Branch 1 taken 102 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 102 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 102 times.
✗ Branch 8 not taken.
|
204 | Vec3s p1, p2, normal; |
376 | 408 | const Scalar distance = internal::ShapeShapeDistance<TriangleP, S>( | |
377 |
1/2✓ Branch 1 taken 102 times.
✗ Branch 2 not taken.
|
204 | &tri, tf1, &model2, tf2, nsolver, request.enable_signed_distance, p1, p2, |
378 | normal); | ||
379 |
1/2✓ Branch 1 taken 102 times.
✗ Branch 2 not taken.
|
204 | result.update(distance, model1, &model2, init_tri_id, DistanceResult::NONE, |
380 | p1, p2, normal); | ||
381 | } | ||
382 | |||
383 | } // namespace details | ||
384 | |||
385 | /// @endcond | ||
386 | |||
387 | /// @brief Traversal node for distance between mesh and shape, when mesh BVH is | ||
388 | /// one of the oriented node (RSS, kIOS, OBBRSS) | ||
389 | template <typename S> | ||
390 | class MeshShapeDistanceTraversalNodeRSS | ||
391 | : public MeshShapeDistanceTraversalNode<RSS, S> { | ||
392 | public: | ||
393 | ✗ | MeshShapeDistanceTraversalNodeRSS() | |
394 | ✗ | : MeshShapeDistanceTraversalNode<RSS, S>() {} | |
395 | |||
396 | ✗ | void preprocess() { | |
397 | ✗ | details::distancePreprocessOrientedNode( | |
398 | ✗ | this->model1, this->vertices, this->tri_indices, 0, *(this->model2), | |
399 | ✗ | this->tf1, this->tf2, this->nsolver, this->request, *(this->result)); | |
400 | } | ||
401 | |||
402 | ✗ | void postprocess() {} | |
403 | |||
404 | ✗ | Scalar BVDistanceLowerBound(unsigned int b1, unsigned int /*b2*/) const { | |
405 | ✗ | if (this->enable_statistics) this->num_bv_tests++; | |
406 | ✗ | return distance(this->tf1.getRotation(), this->tf1.getTranslation(), | |
407 | ✗ | this->model2_bv, this->model1->getBV(b1).bv); | |
408 | } | ||
409 | |||
410 | ✗ | void leafComputeDistance(unsigned int b1, unsigned int b2) const { | |
411 | ✗ | details::meshShapeDistanceOrientedNodeleafComputeDistance( | |
412 | ✗ | b1, b2, this->model1, *(this->model2), this->vertices, | |
413 | ✗ | this->tri_indices, this->tf1, this->tf2, this->nsolver, | |
414 | ✗ | this->enable_statistics, this->num_leaf_tests, this->request, | |
415 | ✗ | *(this->result)); | |
416 | } | ||
417 | }; | ||
418 | |||
419 | template <typename S> | ||
420 | class MeshShapeDistanceTraversalNodekIOS | ||
421 | : public MeshShapeDistanceTraversalNode<kIOS, S> { | ||
422 | public: | ||
423 | ✗ | MeshShapeDistanceTraversalNodekIOS() | |
424 | ✗ | : MeshShapeDistanceTraversalNode<kIOS, S>() {} | |
425 | |||
426 | ✗ | void preprocess() { | |
427 | ✗ | details::distancePreprocessOrientedNode( | |
428 | ✗ | this->model1, this->vertices, this->tri_indices, 0, *(this->model2), | |
429 | ✗ | this->tf1, this->tf2, this->nsolver, this->request, *(this->result)); | |
430 | } | ||
431 | |||
432 | ✗ | void postprocess() {} | |
433 | |||
434 | ✗ | Scalar BVDistanceLowerBound(unsigned int b1, unsigned int /*b2*/) const { | |
435 | ✗ | if (this->enable_statistics) this->num_bv_tests++; | |
436 | ✗ | return distance(this->tf1.getRotation(), this->tf1.getTranslation(), | |
437 | ✗ | this->model2_bv, this->model1->getBV(b1).bv); | |
438 | } | ||
439 | |||
440 | ✗ | void leafComputeDistance(unsigned int b1, unsigned int b2) const { | |
441 | ✗ | details::meshShapeDistanceOrientedNodeleafComputeDistance( | |
442 | ✗ | b1, b2, this->model1, *(this->model2), this->vertices, | |
443 | ✗ | this->tri_indices, this->tf1, this->tf2, this->nsolver, | |
444 | ✗ | this->enable_statistics, this->num_leaf_tests, this->request, | |
445 | ✗ | *(this->result)); | |
446 | } | ||
447 | }; | ||
448 | |||
449 | template <typename S> | ||
450 | class MeshShapeDistanceTraversalNodeOBBRSS | ||
451 | : public MeshShapeDistanceTraversalNode<OBBRSS, S> { | ||
452 | public: | ||
453 | 204 | MeshShapeDistanceTraversalNodeOBBRSS() | |
454 | 204 | : MeshShapeDistanceTraversalNode<OBBRSS, S>() {} | |
455 | |||
456 | 204 | void preprocess() { | |
457 | 204 | details::distancePreprocessOrientedNode( | |
458 | 204 | this->model1, this->vertices, this->tri_indices, 0, *(this->model2), | |
459 | 204 | this->tf1, this->tf2, this->nsolver, this->request, *(this->result)); | |
460 | } | ||
461 | |||
462 | 204 | void postprocess() {} | |
463 | |||
464 | 21592 | Scalar BVDistanceLowerBound(unsigned int b1, unsigned int /*b2*/) const { | |
465 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 10796 times.
|
21592 | if (this->enable_statistics) this->num_bv_tests++; |
466 | 21592 | return distance(this->tf1.getRotation(), this->tf1.getTranslation(), | |
467 | 43184 | this->model2_bv, this->model1->getBV(b1).bv); | |
468 | } | ||
469 | |||
470 | 2858 | void leafComputeDistance(unsigned int b1, unsigned int b2) const { | |
471 | 2858 | details::meshShapeDistanceOrientedNodeleafComputeDistance( | |
472 | 2858 | b1, b2, this->model1, *(this->model2), this->vertices, | |
473 | 2858 | this->tri_indices, this->tf1, this->tf2, this->nsolver, | |
474 | 2858 | this->enable_statistics, this->num_leaf_tests, this->request, | |
475 | 2858 | *(this->result)); | |
476 | } | ||
477 | }; | ||
478 | |||
479 | /// @} | ||
480 | |||
481 | } // namespace coal | ||
482 | |||
483 | /// @endcond | ||
484 | |||
485 | #endif | ||
486 |