GCC Code Coverage Report

Directory: ./
File: include/coal/internal/traversal_node_setup.h
Date: 2025-04-01 09:23:31
Exec Total Coverage
Lines: 164 321 51.1%
Branches: 71 590 12.0%
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_SETUP_H
39 #define COAL_TRAVERSAL_NODE_SETUP_H
40
41 /// @cond INTERNAL
42
43 #include "coal/internal/tools.h"
44 #include "coal/internal/traversal_node_shapes.h"
45
46 #include "coal/internal/traversal_node_bvhs.h"
47 #include "coal/internal/traversal_node_bvh_shape.h"
48
49 // #include <hpp/fcl/internal/traversal_node_hfields.h>
50 #include "coal/internal/traversal_node_hfield_shape.h"
51
52 #ifdef COAL_HAS_OCTOMAP
53 #include "coal/internal/traversal_node_octree.h"
54 #endif
55
56 #include "coal/BVH/BVH_utility.h"
57
58 namespace coal {
59
60 #ifdef COAL_HAS_OCTOMAP
61 /// @brief Initialize traversal node for collision between two octrees, given
62 /// current object transform
63 ✗ inline bool initialize(OcTreeCollisionTraversalNode& node, const OcTree& model1,
64 const Transform3s& tf1, const OcTree& model2,
65 const Transform3s& tf2, const OcTreeSolver* otsolver,
66 CollisionResult& result) {
67 ✗ node.result = &result;
68
69 ✗ node.model1 = &model1;
70 ✗ node.model2 = &model2;
71
72 ✗ node.otsolver = otsolver;
73
74 ✗ node.tf1 = tf1;
75 ✗ node.tf2 = tf2;
76
77 ✗ return true;
78 }
79
80 /// @brief Initialize traversal node for distance between two octrees, given
81 /// current object transform
82 ✗ inline bool initialize(OcTreeDistanceTraversalNode& node, const OcTree& model1,
83 const Transform3s& tf1, const OcTree& model2,
84 const Transform3s& tf2, const OcTreeSolver* otsolver,
85 const DistanceRequest& request, DistanceResult& result)
86
87 {
88 ✗ node.request = request;
89 ✗ node.result = &result;
90
91 ✗ node.model1 = &model1;
92 ✗ node.model2 = &model2;
93
94 ✗ node.otsolver = otsolver;
95
96 ✗ node.tf1 = tf1;
97 ✗ node.tf2 = tf2;
98
99 ✗ return true;
100 }
101
102 /// @brief Initialize traversal node for collision between one shape and one
103 /// octree, given current object transform
104 template <typename S>
105 ✗ bool initialize(ShapeOcTreeCollisionTraversalNode<S>& node, const S& model1,
106 const Transform3s& tf1, const OcTree& model2,
107 const Transform3s& tf2, const OcTreeSolver* otsolver,
108 CollisionResult& result) {
109 ✗ node.result = &result;
110
111 ✗ node.model1 = &model1;
112 ✗ node.model2 = &model2;
113
114 ✗ node.otsolver = otsolver;
115
116 ✗ node.tf1 = tf1;
117 ✗ node.tf2 = tf2;
118
119 ✗ return true;
120 }
121
122 /// @brief Initialize traversal node for collision between one octree and one
123 /// shape, given current object transform
124 template <typename S>
125 2000 bool initialize(OcTreeShapeCollisionTraversalNode<S>& node,
126 const OcTree& model1, const Transform3s& tf1, const S& model2,
127 const Transform3s& tf2, const OcTreeSolver* otsolver,
128 CollisionResult& result) {
129 2000 node.result = &result;
130
131 2000 node.model1 = &model1;
132 2000 node.model2 = &model2;
133
134 2000 node.otsolver = otsolver;
135
136 2000 node.tf1 = tf1;
137 2000 node.tf2 = tf2;
138
139 2000 return true;
140 }
141
142 /// @brief Initialize traversal node for distance between one shape and one
143 /// octree, given current object transform
144 template <typename S>
145 ✗ bool initialize(ShapeOcTreeDistanceTraversalNode<S>& node, const S& model1,
146 const Transform3s& tf1, const OcTree& model2,
147 const Transform3s& tf2, const OcTreeSolver* otsolver,
148 const DistanceRequest& request, DistanceResult& result) {
149 ✗ node.request = request;
150 ✗ node.result = &result;
151
152 ✗ node.model1 = &model1;
153 ✗ node.model2 = &model2;
154
155 ✗ node.otsolver = otsolver;
156
157 ✗ node.tf1 = tf1;
158 ✗ node.tf2 = tf2;
159
160 ✗ return true;
161 }
162
163 /// @brief Initialize traversal node for distance between one octree and one
164 /// shape, given current object transform
165 template <typename S>
166 ✗ bool initialize(OcTreeShapeDistanceTraversalNode<S>& node, const OcTree& model1,
167 const Transform3s& tf1, const S& model2, const Transform3s& tf2,
168 const OcTreeSolver* otsolver, const DistanceRequest& request,
169 DistanceResult& result) {
170 ✗ node.request = request;
171 ✗ node.result = &result;
172
173 ✗ node.model1 = &model1;
174 ✗ node.model2 = &model2;
175
176 ✗ node.otsolver = otsolver;
177
178 ✗ node.tf1 = tf1;
179 ✗ node.tf2 = tf2;
180
181 ✗ return true;
182 }
183
184 /// @brief Initialize traversal node for collision between one mesh and one
185 /// octree, given current object transform
186 template <typename BV>
187 200 bool initialize(MeshOcTreeCollisionTraversalNode<BV>& node,
188 const BVHModel<BV>& model1, const Transform3s& tf1,
189 const OcTree& model2, const Transform3s& tf2,
190 const OcTreeSolver* otsolver, CollisionResult& result) {
191 200 node.result = &result;
192
193 200 node.model1 = &model1;
194 200 node.model2 = &model2;
195
196 200 node.otsolver = otsolver;
197
198 200 node.tf1 = tf1;
199 200 node.tf2 = tf2;
200
201 200 return true;
202 }
203
204 /// @brief Initialize traversal node for collision between one octree and one
205 /// mesh, given current object transform
206 template <typename BV>
207 ✗ bool initialize(OcTreeMeshCollisionTraversalNode<BV>& node,
208 const OcTree& model1, const Transform3s& tf1,
209 const BVHModel<BV>& model2, const Transform3s& tf2,
210 const OcTreeSolver* otsolver, CollisionResult& result) {
211 ✗ node.result = &result;
212
213 ✗ node.model1 = &model1;
214 ✗ node.model2 = &model2;
215
216 ✗ node.otsolver = otsolver;
217
218 ✗ node.tf1 = tf1;
219 ✗ node.tf2 = tf2;
220
221 ✗ return true;
222 }
223
224 /// @brief Initialize traversal node for collision between one octree and one
225 /// height field, given current object transform
226 template <typename BV>
227 2000 bool initialize(OcTreeHeightFieldCollisionTraversalNode<BV>& node,
228 const OcTree& model1, const Transform3s& tf1,
229 const HeightField<BV>& model2, const Transform3s& tf2,
230 const OcTreeSolver* otsolver, CollisionResult& result) {
231 2000 node.result = &result;
232
233 2000 node.model1 = &model1;
234 2000 node.model2 = &model2;
235
236 2000 node.otsolver = otsolver;
237
238 2000 node.tf1 = tf1;
239 2000 node.tf2 = tf2;
240
241 2000 return true;
242 }
243
244 /// @brief Initialize traversal node for collision between one height field and
245 /// one octree, given current object transform
246 template <typename BV>
247 2000 bool initialize(HeightFieldOcTreeCollisionTraversalNode<BV>& node,
248 const HeightField<BV>& model1, const Transform3s& tf1,
249 const OcTree& model2, const Transform3s& tf2,
250 const OcTreeSolver* otsolver, CollisionResult& result) {
251 2000 node.result = &result;
252
253 2000 node.model1 = &model1;
254 2000 node.model2 = &model2;
255
256 2000 node.otsolver = otsolver;
257
258 2000 node.tf1 = tf1;
259 2000 node.tf2 = tf2;
260
261 2000 return true;
262 }
263
264 /// @brief Initialize traversal node for distance between one mesh and one
265 /// octree, given current object transform
266 template <typename BV>
267 ✗ bool initialize(MeshOcTreeDistanceTraversalNode<BV>& node,
268 const BVHModel<BV>& model1, const Transform3s& tf1,
269 const OcTree& model2, const Transform3s& tf2,
270 const OcTreeSolver* otsolver, const DistanceRequest& request,
271 DistanceResult& result) {
272 ✗ node.request = request;
273 ✗ node.result = &result;
274
275 ✗ node.model1 = &model1;
276 ✗ node.model2 = &model2;
277
278 ✗ node.otsolver = otsolver;
279
280 ✗ node.tf1 = tf1;
281 ✗ node.tf2 = tf2;
282
283 ✗ return true;
284 }
285
286 /// @brief Initialize traversal node for collision between one octree and one
287 /// mesh, given current object transform
288 template <typename BV>
289 ✗ bool initialize(OcTreeMeshDistanceTraversalNode<BV>& node, const OcTree& model1,
290 const Transform3s& tf1, const BVHModel<BV>& model2,
291 const Transform3s& tf2, const OcTreeSolver* otsolver,
292 const DistanceRequest& request, DistanceResult& result) {
293 ✗ node.request = request;
294 ✗ node.result = &result;
295
296 ✗ node.model1 = &model1;
297 ✗ node.model2 = &model2;
298
299 ✗ node.otsolver = otsolver;
300
301 ✗ node.tf1 = tf1;
302 ✗ node.tf2 = tf2;
303
304 ✗ return true;
305 }
306
307 #endif
308
309 /// @brief Initialize traversal node for collision between two geometric shapes,
310 /// given current object transform
311 template <typename S1, typename S2>
312 bool initialize(ShapeCollisionTraversalNode<S1, S2>& node, const S1& shape1,
313 const Transform3s& tf1, const S2& shape2,
314 const Transform3s& tf2, const GJKSolver* nsolver,
315 CollisionResult& result) {
316 node.model1 = &shape1;
317 node.tf1 = tf1;
318 node.model2 = &shape2;
319 node.tf2 = tf2;
320 node.nsolver = nsolver;
321
322 node.result = &result;
323
324 return true;
325 }
326
327 /// @brief Initialize traversal node for collision between one mesh and one
328 /// shape, given current object transform
329 template <typename BV, typename S>
330 ✗ bool initialize(MeshShapeCollisionTraversalNode<BV, S>& node,
331 BVHModel<BV>& model1, Transform3s& tf1, const S& model2,
332 const Transform3s& tf2, const GJKSolver* nsolver,
333 CollisionResult& result, bool use_refit = false,
334 bool refit_bottomup = false) {
335 ✗ if (model1.getModelType() != BVH_MODEL_TRIANGLES)
336 ✗ COAL_THROW_PRETTY(
337 "model1 should be of type BVHModelType::BVH_MODEL_TRIANGLES.",
338 std::invalid_argument)
339
340 ✗ if (!tf1.isIdentity() &&
341 ✗ model1.vertices.get()) // TODO(jcarpent): vectorized version
342 {
343 ✗ std::vector<Vec3s> vertices_transformed(model1.num_vertices);
344 ✗ const std::vector<Vec3s>& model1_vertices_ = *(model1.vertices);
345 ✗ for (unsigned int i = 0; i < model1.num_vertices; ++i) {
346 ✗ const Vec3s& p = model1_vertices_[i];
347 ✗ Vec3s new_v = tf1.transform(p);
348 ✗ vertices_transformed[i] = new_v;
349 }
350
351 ✗ model1.beginReplaceModel();
352 ✗ model1.replaceSubModel(vertices_transformed);
353 ✗ model1.endReplaceModel(use_refit, refit_bottomup);
354
355 ✗ tf1.setIdentity();
356 }
357
358 ✗ node.model1 = &model1;
359 ✗ node.tf1 = tf1;
360 ✗ node.model2 = &model2;
361 ✗ node.tf2 = tf2;
362 ✗ node.nsolver = nsolver;
363
364 ✗ computeBV(model2, tf2, node.model2_bv);
365
366 ✗ node.vertices = model1.vertices.get() ? model1.vertices->data() : NULL;
367 ✗ node.tri_indices =
368 ✗ model1.tri_indices.get() ? model1.tri_indices->data() : NULL;
369
370 ✗ node.result = &result;
371
372 ✗ return true;
373 }
374
375 /// @brief Initialize traversal node for collision between one mesh and one
376 /// shape
377 template <typename BV, typename S>
378 252 bool initialize(MeshShapeCollisionTraversalNode<BV, S, 0>& node,
379 const BVHModel<BV>& model1, const Transform3s& tf1,
380 const S& model2, const Transform3s& tf2,
381 const GJKSolver* nsolver, CollisionResult& result) {
382
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 126 times.
 252 if (model1.getModelType() != BVH_MODEL_TRIANGLES)
383 ✗ COAL_THROW_PRETTY(
384 "model1 should be of type BVHModelType::BVH_MODEL_TRIANGLES.",
385 std::invalid_argument)
386
387 252 node.model1 = &model1;
388 252 node.tf1 = tf1;
389 252 node.model2 = &model2;
390 252 node.tf2 = tf2;
391 252 node.nsolver = nsolver;
392
393 252 computeBV(model2, tf2, node.model2_bv);
394
395
1/2
✓ Branch 1 taken 126 times.
✗ Branch 2 not taken.
 252 node.vertices = model1.vertices.get() ? model1.vertices->data() : NULL;
396 252 node.tri_indices =
397
1/2
✓ Branch 1 taken 126 times.
✗ Branch 2 not taken.
 252 model1.tri_indices.get() ? model1.tri_indices->data() : NULL;
398
399 252 node.result = &result;
400
401 252 return true;
402 }
403
404 /// @brief Initialize traversal node for collision between one mesh and one
405 /// shape, given current object transform
406 template <typename BV, typename S>
407 bool initialize(HeightFieldShapeCollisionTraversalNode<BV, S>& node,
408 HeightField<BV>& model1, Transform3s& tf1, const S& model2,
409 const Transform3s& tf2, const GJKSolver* nsolver,
410 CollisionResult& result, bool use_refit = false,
411 bool refit_bottomup = false);
412
413 /// @brief Initialize traversal node for collision between one mesh and one
414 /// shape
415 template <typename BV, typename S>
416 110 bool initialize(HeightFieldShapeCollisionTraversalNode<BV, S, 0>& node,
417 const HeightField<BV>& model1, const Transform3s& tf1,
418 const S& model2, const Transform3s& tf2,
419 const GJKSolver* nsolver, CollisionResult& result) {
420 110 node.model1 = &model1;
421 110 node.tf1 = tf1;
422 110 node.model2 = &model2;
423 110 node.tf2 = tf2;
424 110 node.nsolver = nsolver;
425
426 110 computeBV(model2, tf2, node.model2_bv);
427
428 110 node.result = &result;
429
430 110 return true;
431 }
432
433 /// @cond IGNORE
434 namespace details {
435 template <typename S, typename BV, template <typename> class OrientedNode>
436 static inline bool setupShapeMeshCollisionOrientedNode(
437 OrientedNode<S>& node, const S& model1, const Transform3s& tf1,
438 const BVHModel<BV>& model2, const Transform3s& tf2,
439 const GJKSolver* nsolver, CollisionResult& result) {
440 if (model2.getModelType() != BVH_MODEL_TRIANGLES)
441 COAL_THROW_PRETTY(
442 "model2 should be of type BVHModelType::BVH_MODEL_TRIANGLES.",
443 std::invalid_argument)
444
445 node.model1 = &model1;
446 node.tf1 = tf1;
447 node.model2 = &model2;
448 node.tf2 = tf2;
449 node.nsolver = nsolver;
450
451 computeBV(model1, tf1, node.model1_bv);
452
453 node.vertices = model2.vertices.get() ? model2.vertices->data() : NULL;
454 node.tri_indices =
455 model2.tri_indices.get() ? model2.tri_indices->data() : NULL;
456
457 node.result = &result;
458
459 return true;
460 }
461 } // namespace details
462 /// @endcond
463
464 /// @brief Initialize traversal node for collision between two meshes, given the
465 /// current transforms
466 template <typename BV>
467 202 bool initialize(
468 MeshCollisionTraversalNode<BV, RelativeTransformationIsIdentity>& node,
469 BVHModel<BV>& model1, Transform3s& tf1, BVHModel<BV>& model2,
470 Transform3s& tf2, CollisionResult& result, bool use_refit = false,
471 bool refit_bottomup = false) {
472
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 101 times.
 202 if (model1.getModelType() != BVH_MODEL_TRIANGLES)
473 ✗ COAL_THROW_PRETTY(
474 "model1 should be of type BVHModelType::BVH_MODEL_TRIANGLES.",
475 std::invalid_argument)
476
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 101 times.
 202 if (model2.getModelType() != BVH_MODEL_TRIANGLES)
477 ✗ COAL_THROW_PRETTY(
478 "model2 should be of type BVHModelType::BVH_MODEL_TRIANGLES.",
479 std::invalid_argument)
480
481
6/8
✓ Branch 2 taken 101 times.
✗ Branch 3 not taken.
✓ Branch 4 taken 67 times.
✓ Branch 5 taken 34 times.
✓ Branch 7 taken 67 times.
✗ Branch 8 not taken.
✓ Branch 9 taken 67 times.
✓ Branch 10 taken 34 times.
 202 if (!tf1.isIdentity() && model1.vertices.get()) {
482
1/2
✓ Branch 2 taken 67 times.
✗ Branch 3 not taken.
 134 std::vector<Vec3s> vertices_transformed1(model1.num_vertices);
483 134 const std::vector<Vec3s>& model1_vertices_ = *(model1.vertices);
484
2/2
✓ Branch 0 taken 344352 times.
✓ Branch 1 taken 67 times.
 688838 for (unsigned int i = 0; i < model1.num_vertices; ++i) {
485 688704 const Vec3s& p = model1_vertices_[i];
486
1/2
✓ Branch 1 taken 344352 times.
✗ Branch 2 not taken.
 688704 Vec3s new_v = tf1.transform(p);
487
1/2
✓ Branch 2 taken 344352 times.
✗ Branch 3 not taken.
 688704 vertices_transformed1[i] = new_v;
488 }
489
490
1/2
✓ Branch 1 taken 67 times.
✗ Branch 2 not taken.
 134 model1.beginReplaceModel();
491
1/2
✓ Branch 1 taken 67 times.
✗ Branch 2 not taken.
 134 model1.replaceSubModel(vertices_transformed1);
492
1/2
✓ Branch 1 taken 67 times.
✗ Branch 2 not taken.
 134 model1.endReplaceModel(use_refit, refit_bottomup);
493
494
1/2
✓ Branch 1 taken 67 times.
✗ Branch 2 not taken.
 134 tf1.setIdentity();
495 134 }
496
497
3/8
✓ Branch 2 taken 101 times.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✓ Branch 5 taken 101 times.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 9 not taken.
✓ Branch 10 taken 101 times.
 202 if (!tf2.isIdentity() && model2.vertices.get()) {
498 ✗ std::vector<Vec3s> vertices_transformed2(model2.num_vertices);
499 ✗ const std::vector<Vec3s>& model2_vertices_ = *(model2.vertices);
500 ✗ for (unsigned int i = 0; i < model2.num_vertices; ++i) {
501 ✗ const Vec3s& p = model2_vertices_[i];
502 ✗ Vec3s new_v = tf2.transform(p);
503 ✗ vertices_transformed2[i] = new_v;
504 }
505
506 ✗ model2.beginReplaceModel();
507 ✗ model2.replaceSubModel(vertices_transformed2);
508 ✗ model2.endReplaceModel(use_refit, refit_bottomup);
509
510 ✗ tf2.setIdentity();
511 }
512
513 202 node.model1 = &model1;
514 202 node.tf1 = tf1;
515 202 node.model2 = &model2;
516 202 node.tf2 = tf2;
517
518
1/2
✓ Branch 1 taken 101 times.
✗ Branch 2 not taken.
 202 node.vertices1 = model1.vertices.get() ? model1.vertices->data() : NULL;
519
1/2
✓ Branch 1 taken 101 times.
✗ Branch 2 not taken.
 202 node.vertices2 = model2.vertices.get() ? model2.vertices->data() : NULL;
520
521 202 node.tri_indices1 =
522
1/2
✓ Branch 1 taken 101 times.
✗ Branch 2 not taken.
 202 model1.tri_indices.get() ? model1.tri_indices->data() : NULL;
523 202 node.tri_indices2 =
524
1/2
✓ Branch 1 taken 101 times.
✗ Branch 2 not taken.
 202 model2.tri_indices.get() ? model2.tri_indices->data() : NULL;
525
526 202 node.result = &result;
527
528 202 return true;
529 }
530
531 template <typename BV>
532 10709 bool initialize(MeshCollisionTraversalNode<BV, 0>& node,
533 const BVHModel<BV>& model1, const Transform3s& tf1,
534 const BVHModel<BV>& model2, const Transform3s& tf2,
535 CollisionResult& result) {
536
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 1 times.
 10709 if (model1.getModelType() != BVH_MODEL_TRIANGLES)
537 ✗ COAL_THROW_PRETTY(
538 "model1 should be of type BVHModelType::BVH_MODEL_TRIANGLES.",
539 std::invalid_argument)
540
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 1 times.
 10709 if (model2.getModelType() != BVH_MODEL_TRIANGLES)
541 ✗ COAL_THROW_PRETTY(
542 "model2 should be of type BVHModelType::BVH_MODEL_TRIANGLES.",
543 std::invalid_argument)
544
545
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 10709 node.vertices1 = model1.vertices ? model1.vertices->data() : NULL;
546
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 10709 node.vertices2 = model2.vertices ? model2.vertices->data() : NULL;
547
548 10709 node.tri_indices1 =
549
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 10709 model1.tri_indices.get() ? model1.tri_indices->data() : NULL;
550 10709 node.tri_indices2 =
551
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 10709 model2.tri_indices.get() ? model2.tri_indices->data() : NULL;
552
553 10709 node.model1 = &model1;
554 10709 node.tf1 = tf1;
555 10709 node.model2 = &model2;
556 10709 node.tf2 = tf2;
557
558 10709 node.result = &result;
559
560
3/6
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
 10709 node.RT.R.noalias() = tf1.getRotation().transpose() * tf2.getRotation();
561
4/8
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 1 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 1 times.
✗ Branch 13 not taken.
 10709 node.RT.T.noalias() = tf1.getRotation().transpose() *
562 10709 (tf2.getTranslation() - tf1.getTranslation());
563
564 10709 return true;
565 }
566
567 /// @brief Initialize traversal node for distance between two geometric shapes
568 template <typename S1, typename S2>
569 bool initialize(ShapeDistanceTraversalNode<S1, S2>& node, const S1& shape1,
570 const Transform3s& tf1, const S2& shape2,
571 const Transform3s& tf2, const GJKSolver* nsolver,
572 const DistanceRequest& request, DistanceResult& result) {
573 node.request = request;
574 node.result = &result;
575
576 node.model1 = &shape1;
577 node.tf1 = tf1;
578 node.model2 = &shape2;
579 node.tf2 = tf2;
580 node.nsolver = nsolver;
581
582 return true;
583 }
584
585 /// @brief Initialize traversal node for distance computation between two
586 /// meshes, given the current transforms
587 template <typename BV>
588 48 bool initialize(
589 MeshDistanceTraversalNode<BV, RelativeTransformationIsIdentity>& node,
590 BVHModel<BV>& model1, Transform3s& tf1, BVHModel<BV>& model2,
591 Transform3s& tf2, const DistanceRequest& request, DistanceResult& result,
592 bool use_refit = false, bool refit_bottomup = false) {
593
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 24 times.
 48 if (model1.getModelType() != BVH_MODEL_TRIANGLES)
594 ✗ COAL_THROW_PRETTY(
595 "model1 should be of type BVHModelType::BVH_MODEL_TRIANGLES.",
596 std::invalid_argument)
597
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 24 times.
 48 if (model2.getModelType() != BVH_MODEL_TRIANGLES)
598 ✗ COAL_THROW_PRETTY(
599 "model2 should be of type BVHModelType::BVH_MODEL_TRIANGLES.",
600 std::invalid_argument)
601
602
4/8
✓ Branch 2 taken 24 times.
✗ Branch 3 not taken.
✓ Branch 4 taken 24 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 24 times.
✗ Branch 8 not taken.
✓ Branch 9 taken 24 times.
✗ Branch 10 not taken.
 48 if (!tf1.isIdentity() && model1.vertices.get()) {
603
1/2
✓ Branch 2 taken 24 times.
✗ Branch 3 not taken.
 48 std::vector<Vec3s> vertices_transformed1(model1.num_vertices);
604 48 const std::vector<Vec3s>& model1_vertices_ = *(model1.vertices);
605
2/2
✓ Branch 0 taken 156960 times.
✓ Branch 1 taken 24 times.
 313968 for (unsigned int i = 0; i < model1.num_vertices; ++i) {
606 313920 const Vec3s& p = model1_vertices_[i];
607
1/2
✓ Branch 1 taken 156960 times.
✗ Branch 2 not taken.
 313920 Vec3s new_v = tf1.transform(p);
608
1/2
✓ Branch 2 taken 156960 times.
✗ Branch 3 not taken.
 313920 vertices_transformed1[i] = new_v;
609 }
610
611
1/2
✓ Branch 1 taken 24 times.
✗ Branch 2 not taken.
 48 model1.beginReplaceModel();
612
1/2
✓ Branch 1 taken 24 times.
✗ Branch 2 not taken.
 48 model1.replaceSubModel(vertices_transformed1);
613
1/2
✓ Branch 1 taken 24 times.
✗ Branch 2 not taken.
 48 model1.endReplaceModel(use_refit, refit_bottomup);
614
615
1/2
✓ Branch 1 taken 24 times.
✗ Branch 2 not taken.
 48 tf1.setIdentity();
616 48 }
617
618
3/8
✓ Branch 2 taken 24 times.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✓ Branch 5 taken 24 times.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 9 not taken.
✓ Branch 10 taken 24 times.
 48 if (!tf2.isIdentity() && model2.vertices.get()) {
619 ✗ std::vector<Vec3s> vertices_transformed2(model2.num_vertices);
620 ✗ const std::vector<Vec3s>& model2_vertices_ = *(model2.vertices);
621 ✗ for (unsigned int i = 0; i < model2.num_vertices; ++i) {
622 ✗ const Vec3s& p = model2_vertices_[i];
623 ✗ Vec3s new_v = tf2.transform(p);
624 ✗ vertices_transformed2[i] = new_v;
625 }
626
627 ✗ model2.beginReplaceModel();
628 ✗ model2.replaceSubModel(vertices_transformed2);
629 ✗ model2.endReplaceModel(use_refit, refit_bottomup);
630
631 ✗ tf2.setIdentity();
632 }
633
634 48 node.request = request;
635 48 node.result = &result;
636
637 48 node.model1 = &model1;
638 48 node.tf1 = tf1;
639 48 node.model2 = &model2;
640 48 node.tf2 = tf2;
641
642
1/2
✓ Branch 1 taken 24 times.
✗ Branch 2 not taken.
 48 node.vertices1 = model1.vertices.get() ? model1.vertices->data() : NULL;
643
1/2
✓ Branch 1 taken 24 times.
✗ Branch 2 not taken.
 48 node.vertices2 = model2.vertices.get() ? model2.vertices->data() : NULL;
644
645 48 node.tri_indices1 =
646
1/2
✓ Branch 1 taken 24 times.
✗ Branch 2 not taken.
 48 model1.tri_indices.get() ? model1.tri_indices->data() : NULL;
647 48 node.tri_indices2 =
648
1/2
✓ Branch 1 taken 24 times.
✗ Branch 2 not taken.
 48 model2.tri_indices.get() ? model2.tri_indices->data() : NULL;
649
650 48 return true;
651 }
652
653 /// @brief Initialize traversal node for distance computation between two meshes
654 template <typename BV>
655 14308 bool initialize(MeshDistanceTraversalNode<BV, 0>& node,
656 const BVHModel<BV>& model1, const Transform3s& tf1,
657 const BVHModel<BV>& model2, const Transform3s& tf2,
658 const DistanceRequest& request, DistanceResult& result) {
659
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 7145 times.
 14308 if (model1.getModelType() != BVH_MODEL_TRIANGLES)
660 ✗ COAL_THROW_PRETTY(
661 "model1 should be of type BVHModelType::BVH_MODEL_TRIANGLES.",
662 std::invalid_argument)
663
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 7145 times.
 14308 if (model2.getModelType() != BVH_MODEL_TRIANGLES)
664 ✗ COAL_THROW_PRETTY(
665 "model2 should be of type BVHModelType::BVH_MODEL_TRIANGLES.",
666 std::invalid_argument)
667
668 14308 node.request = request;
669 14308 node.result = &result;
670
671 14308 node.model1 = &model1;
672 14308 node.tf1 = tf1;
673 14308 node.model2 = &model2;
674 14308 node.tf2 = tf2;
675
676
1/2
✓ Branch 1 taken 7145 times.
✗ Branch 2 not taken.
 14308 node.vertices1 = model1.vertices.get() ? model1.vertices->data() : NULL;
677
1/2
✓ Branch 1 taken 7145 times.
✗ Branch 2 not taken.
 14308 node.vertices2 = model2.vertices.get() ? model2.vertices->data() : NULL;
678
679 14308 node.tri_indices1 =
680
1/2
✓ Branch 1 taken 7145 times.
✗ Branch 2 not taken.
 14308 model1.tri_indices.get() ? model1.tri_indices->data() : NULL;
681 14308 node.tri_indices2 =
682
1/2
✓ Branch 1 taken 7145 times.
✗ Branch 2 not taken.
 14308 model2.tri_indices.get() ? model2.tri_indices->data() : NULL;
683
684 COAL_COMPILER_DIAGNOSTIC_PUSH
685 COAL_COMPILER_DIAGNOSTIC_IGNORED_MAYBE_UNINITIALIZED
686
687 14308 relativeTransform(tf1.getRotation(), tf1.getTranslation(), tf2.getRotation(),
688 14308 tf2.getTranslation(), node.RT.R, node.RT.T);
689
690 COAL_COMPILER_DIAGNOSTIC_POP
691
692 14308 return true;
693 }
694
695 /// @brief Initialize traversal node for distance computation between one mesh
696 /// and one shape, given the current transforms
697 template <typename BV, typename S>
698 ✗ bool initialize(MeshShapeDistanceTraversalNode<BV, S>& node,
699 BVHModel<BV>& model1, Transform3s& tf1, const S& model2,
700 const Transform3s& tf2, const GJKSolver* nsolver,
701 const DistanceRequest& request, DistanceResult& result,
702 bool use_refit = false, bool refit_bottomup = false) {
703 ✗ if (model1.getModelType() != BVH_MODEL_TRIANGLES)
704 ✗ COAL_THROW_PRETTY(
705 "model1 should be of type BVHModelType::BVH_MODEL_TRIANGLES.",
706 std::invalid_argument)
707
708 ✗ if (!tf1.isIdentity() && model1.vertices.get()) {
709 ✗ const std::vector<Vec3s>& model1_vertices_ = *(model1.vertices);
710 ✗ std::vector<Vec3s> vertices_transformed1(model1.num_vertices);
711 ✗ for (unsigned int i = 0; i < model1.num_vertices; ++i) {
712 ✗ const Vec3s& p = model1_vertices_[i];
713 ✗ Vec3s new_v = tf1.transform(p);
714 ✗ vertices_transformed1[i] = new_v;
715 }
716
717 ✗ model1.beginReplaceModel();
718 ✗ model1.replaceSubModel(vertices_transformed1);
719 ✗ model1.endReplaceModel(use_refit, refit_bottomup);
720
721 ✗ tf1.setIdentity();
722 }
723
724 ✗ node.request = request;
725 ✗ node.result = &result;
726
727 ✗ node.model1 = &model1;
728 ✗ node.tf1 = tf1;
729 ✗ node.model2 = &model2;
730 ✗ node.tf2 = tf2;
731 ✗ node.nsolver = nsolver;
732
733 ✗ node.vertices = model1.vertices.get() ? model1.vertices->data() : NULL;
734 ✗ node.tri_indices =
735 ✗ model1.tri_indices.get() ? model1.tri_indices->data() : NULL;
736
737 ✗ computeBV(model2, tf2, node.model2_bv);
738
739 ✗ return true;
740 }
741
742 /// @cond IGNORE
743 namespace details {
744
745 template <typename BV, typename S, template <typename> class OrientedNode>
746 204 static inline bool setupMeshShapeDistanceOrientedNode(
747 OrientedNode<S>& node, const BVHModel<BV>& model1, const Transform3s& tf1,
748 const S& model2, const Transform3s& tf2, const GJKSolver* nsolver,
749 const DistanceRequest& request, DistanceResult& result) {
750
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 102 times.
 204 if (model1.getModelType() != BVH_MODEL_TRIANGLES)
751 ✗ COAL_THROW_PRETTY(
752 "model1 should be of type BVHModelType::BVH_MODEL_TRIANGLES.",
753 std::invalid_argument)
754
755 204 node.request = request;
756 204 node.result = &result;
757
758 204 node.model1 = &model1;
759 204 node.tf1 = tf1;
760 204 node.model2 = &model2;
761 204 node.tf2 = tf2;
762 204 node.nsolver = nsolver;
763
764 204 computeBV(model2, tf2, node.model2_bv);
765
766
1/2
✓ Branch 1 taken 102 times.
✗ Branch 2 not taken.
 204 node.vertices = model1.vertices.get() ? model1.vertices->data() : NULL;
767 204 node.tri_indices =
768
1/2
✓ Branch 1 taken 102 times.
✗ Branch 2 not taken.
 204 model1.tri_indices.get() ? model1.tri_indices->data() : NULL;
769
770 204 return true;
771 }
772 } // namespace details
773 /// @endcond
774
775 /// @brief Initialize traversal node for distance computation between one mesh
776 /// and one shape, specialized for RSS type
777 template <typename S>
778 ✗ bool initialize(MeshShapeDistanceTraversalNodeRSS<S>& node,
779 const BVHModel<RSS>& model1, const Transform3s& tf1,
780 const S& model2, const Transform3s& tf2,
781 const GJKSolver* nsolver, const DistanceRequest& request,
782 DistanceResult& result) {
783 ✗ return details::setupMeshShapeDistanceOrientedNode(
784 ✗ node, model1, tf1, model2, tf2, nsolver, request, result);
785 }
786
787 /// @brief Initialize traversal node for distance computation between one mesh
788 /// and one shape, specialized for kIOS type
789 template <typename S>
790 ✗ bool initialize(MeshShapeDistanceTraversalNodekIOS<S>& node,
791 const BVHModel<kIOS>& model1, const Transform3s& tf1,
792 const S& model2, const Transform3s& tf2,
793 const GJKSolver* nsolver, const DistanceRequest& request,
794 DistanceResult& result) {
795 ✗ return details::setupMeshShapeDistanceOrientedNode(
796 ✗ node, model1, tf1, model2, tf2, nsolver, request, result);
797 }
798
799 /// @brief Initialize traversal node for distance computation between one mesh
800 /// and one shape, specialized for OBBRSS type
801 template <typename S>
802 102 bool initialize(MeshShapeDistanceTraversalNodeOBBRSS<S>& node,
803 const BVHModel<OBBRSS>& model1, const Transform3s& tf1,
804 const S& model2, const Transform3s& tf2,
805 const GJKSolver* nsolver, const DistanceRequest& request,
806 DistanceResult& result) {
807 102 return details::setupMeshShapeDistanceOrientedNode(
808 102 node, model1, tf1, model2, tf2, nsolver, request, result);
809 }
810
811 } // namespace coal
812
813 /// @endcond
814
815 #endif
816