coal 3.0.2
Coal, The Collision Detection Library. Previously known as HPP-FCL, fork of FCL -- The Flexible Collision Library
Loading...
Searching...
No Matches
traversal_node_setup.h
Go to the documentation of this file.
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
37
38#ifndef COAL_TRAVERSAL_NODE_SETUP_H
39#define COAL_TRAVERSAL_NODE_SETUP_H
40
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
58namespace coal {
59
60#ifdef COAL_HAS_OCTOMAP
63inline 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
82inline 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
104template <typename S>
105bool 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
124template <typename S>
125bool 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 node.result = &result;
130
131 node.model1 = &model1;
132 node.model2 = &model2;
133
134 node.otsolver = otsolver;
135
136 node.tf1 = tf1;
137 node.tf2 = tf2;
138
139 return true;
140}
141
144template <typename S>
145bool 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
165template <typename S>
166bool 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
186template <typename BV>
187bool 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 node.result = &result;
192
193 node.model1 = &model1;
194 node.model2 = &model2;
195
196 node.otsolver = otsolver;
197
198 node.tf1 = tf1;
199 node.tf2 = tf2;
200
201 return true;
202}
203
206template <typename BV>
207bool 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
226template <typename BV>
227bool 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 node.result = &result;
232
233 node.model1 = &model1;
234 node.model2 = &model2;
235
236 node.otsolver = otsolver;
237
238 node.tf1 = tf1;
239 node.tf2 = tf2;
240
241 return true;
242}
243
246template <typename BV>
247bool 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 node.result = &result;
252
253 node.model1 = &model1;
254 node.model2 = &model2;
255
256 node.otsolver = otsolver;
257
258 node.tf1 = tf1;
259 node.tf2 = tf2;
260
261 return true;
262}
263
266template <typename BV>
267bool 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
288template <typename BV>
289bool 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
311template <typename S1, typename S2>
312bool 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
329template <typename BV, typename S>
330bool 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
377template <typename BV, typename S>
378bool 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 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 node.model1 = &model1;
388 node.tf1 = tf1;
389 node.model2 = &model2;
390 node.tf2 = tf2;
391 node.nsolver = nsolver;
392
393 computeBV(model2, tf2, node.model2_bv);
394
395 node.vertices = model1.vertices.get() ? model1.vertices->data() : NULL;
396 node.tri_indices =
397 model1.tri_indices.get() ? model1.tri_indices->data() : NULL;
398
399 node.result = &result;
400
401 return true;
402}
403
406template <typename BV, typename S>
407bool 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
415template <typename BV, typename S>
416bool 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 node.model1 = &model1;
421 node.tf1 = tf1;
422 node.model2 = &model2;
423 node.tf2 = tf2;
424 node.nsolver = nsolver;
425
426 computeBV(model2, tf2, node.model2_bv);
427
428 node.result = &result;
429
430 return true;
431}
432
434namespace details {
435template <typename S, typename BV, template <typename> class OrientedNode>
436static 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
463
466template <typename BV>
467bool 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 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 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 if (!tf1.isIdentity() && model1.vertices.get()) {
482 std::vector<Vec3s> vertices_transformed1(model1.num_vertices);
483 const std::vector<Vec3s>& model1_vertices_ = *(model1.vertices);
484 for (unsigned int i = 0; i < model1.num_vertices; ++i) {
485 const Vec3s& p = model1_vertices_[i];
486 Vec3s new_v = tf1.transform(p);
487 vertices_transformed1[i] = new_v;
488 }
489
490 model1.beginReplaceModel();
491 model1.replaceSubModel(vertices_transformed1);
492 model1.endReplaceModel(use_refit, refit_bottomup);
493
494 tf1.setIdentity();
495 }
496
497 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 node.model1 = &model1;
514 node.tf1 = tf1;
515 node.model2 = &model2;
516 node.tf2 = tf2;
517
518 node.vertices1 = model1.vertices.get() ? model1.vertices->data() : NULL;
519 node.vertices2 = model2.vertices.get() ? model2.vertices->data() : NULL;
520
521 node.tri_indices1 =
522 model1.tri_indices.get() ? model1.tri_indices->data() : NULL;
523 node.tri_indices2 =
524 model2.tri_indices.get() ? model2.tri_indices->data() : NULL;
525
526 node.result = &result;
527
528 return true;
529}
530
531template <typename BV>
532bool 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 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 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 node.vertices1 = model1.vertices ? model1.vertices->data() : NULL;
546 node.vertices2 = model2.vertices ? model2.vertices->data() : NULL;
547
548 node.tri_indices1 =
549 model1.tri_indices.get() ? model1.tri_indices->data() : NULL;
550 node.tri_indices2 =
551 model2.tri_indices.get() ? model2.tri_indices->data() : NULL;
552
553 node.model1 = &model1;
554 node.tf1 = tf1;
555 node.model2 = &model2;
556 node.tf2 = tf2;
557
558 node.result = &result;
559
560 node.RT.R.noalias() = tf1.getRotation().transpose() * tf2.getRotation();
561 node.RT.T.noalias() = tf1.getRotation().transpose() *
562 (tf2.getTranslation() - tf1.getTranslation());
563
564 return true;
565}
566
568template <typename S1, typename S2>
569bool 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
587template <typename BV>
588bool 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 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 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 if (!tf1.isIdentity() && model1.vertices.get()) {
603 std::vector<Vec3s> vertices_transformed1(model1.num_vertices);
604 const std::vector<Vec3s>& model1_vertices_ = *(model1.vertices);
605 for (unsigned int i = 0; i < model1.num_vertices; ++i) {
606 const Vec3s& p = model1_vertices_[i];
607 Vec3s new_v = tf1.transform(p);
608 vertices_transformed1[i] = new_v;
609 }
610
611 model1.beginReplaceModel();
612 model1.replaceSubModel(vertices_transformed1);
613 model1.endReplaceModel(use_refit, refit_bottomup);
614
615 tf1.setIdentity();
616 }
617
618 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 node.request = request;
635 node.result = &result;
636
637 node.model1 = &model1;
638 node.tf1 = tf1;
639 node.model2 = &model2;
640 node.tf2 = tf2;
641
642 node.vertices1 = model1.vertices.get() ? model1.vertices->data() : NULL;
643 node.vertices2 = model2.vertices.get() ? model2.vertices->data() : NULL;
644
645 node.tri_indices1 =
646 model1.tri_indices.get() ? model1.tri_indices->data() : NULL;
647 node.tri_indices2 =
648 model2.tri_indices.get() ? model2.tri_indices->data() : NULL;
649
650 return true;
651}
652
654template <typename BV>
655bool 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 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 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 node.request = request;
669 node.result = &result;
670
671 node.model1 = &model1;
672 node.tf1 = tf1;
673 node.model2 = &model2;
674 node.tf2 = tf2;
675
676 node.vertices1 = model1.vertices.get() ? model1.vertices->data() : NULL;
677 node.vertices2 = model2.vertices.get() ? model2.vertices->data() : NULL;
678
679 node.tri_indices1 =
680 model1.tri_indices.get() ? model1.tri_indices->data() : NULL;
681 node.tri_indices2 =
682 model2.tri_indices.get() ? model2.tri_indices->data() : NULL;
683
684 COAL_COMPILER_DIAGNOSTIC_PUSH
685 COAL_COMPILER_DIAGNOSTIC_IGNORED_MAYBE_UNINITIALIZED
686
687 relativeTransform(tf1.getRotation(), tf1.getTranslation(), tf2.getRotation(),
688 tf2.getTranslation(), node.RT.R, node.RT.T);
689
690 COAL_COMPILER_DIAGNOSTIC_POP
691
692 return true;
693}
694
697template <typename BV, typename S>
698bool 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
743namespace details {
744
745template <typename BV, typename S, template <typename> class OrientedNode>
746static 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 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 node.request = request;
756 node.result = &result;
757
758 node.model1 = &model1;
759 node.tf1 = tf1;
760 node.model2 = &model2;
761 node.tf2 = tf2;
762 node.nsolver = nsolver;
763
764 computeBV(model2, tf2, node.model2_bv);
765
766 node.vertices = model1.vertices.get() ? model1.vertices->data() : NULL;
767 node.tri_indices =
768 model1.tri_indices.get() ? model1.tri_indices->data() : NULL;
769
770 return true;
771}
772} // namespace details
774
777template <typename S>
778bool 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
789template <typename S>
790bool 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
801template <typename S>
802bool 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 return details::setupMeshShapeDistanceOrientedNode(
808 node, model1, tf1, model2, tf2, nsolver, request, result);
809}
810
811} // namespace coal
812
814
815#endif
BVH_utility.h
coal::BVHModel
A class describing the bounding hierarchy of a mesh model or a point cloud model (which is viewed as ...
Definition BVH_model.h:314
coal::HeightField
Data structure depicting a height field given by the base grid dimensions and the elevation along the...
Definition hfield.h:202
coal::OcTree
Octree is one type of collision geometry which can encode uncertainty information in the sensor data.
Definition octree.h:53
coal::Transform3s
Simple transform class used locally by InterpMotion.
Definition transform.h:55
COAL_COMPILER_DIAGNOSTIC_PUSH
#define COAL_COMPILER_DIAGNOSTIC_PUSH
Definition fwd.hh:120
COAL_COMPILER_DIAGNOSTIC_IGNORED_MAYBE_UNINITIALIZED
#define COAL_COMPILER_DIAGNOSTIC_IGNORED_MAYBE_UNINITIALIZED
Definition fwd.hh:123
COAL_COMPILER_DIAGNOSTIC_POP
#define COAL_COMPILER_DIAGNOSTIC_POP
Definition fwd.hh:121
COAL_THROW_PRETTY
#define COAL_THROW_PRETTY(message, exception)
Definition fwd.hh:64
coal::details
Definition hfield.h:170
coal
Main namespace.
Definition broadphase_bruteforce.h:44
coal::computeBV
void computeBV(const S &s, const Transform3s &tf, BV &bv)
calculate a bounding volume for a shape in a specific configuration
Definition geometric_shapes_utility.h:73
coal::BVH_MODEL_TRIANGLES
@ BVH_MODEL_TRIANGLES
unknown model type
Definition BVH_internal.h:81
coal::relativeTransform
void relativeTransform(const Eigen::MatrixBase< Derived > &R1, const Eigen::MatrixBase< OtherDerived > &t1, const Eigen::MatrixBase< Derived > &R2, const Eigen::MatrixBase< OtherDerived > &t2, const Eigen::MatrixBase< Derived > &R, const Eigen::MatrixBase< OtherDerived > &t)
Definition tools.h:90
coal::Vec3s
Eigen::Matrix< CoalScalar, 3, 1 > Vec3s
Definition data_types.h:77
coal::CollisionResult
collision result
Definition collision_data.h:390
coal::DistanceRequest
request to the distance computation
Definition collision_data.h:985
coal::DistanceResult
distance result
Definition collision_data.h:1051
coal::GJKSolver
collision and distance solver based on the GJK and EPA algorithms. Originally, GJK and EPA were imple...
Definition narrowphase.h:57
traversal_node_bvh_shape.h
traversal_node_bvhs.h
traversal_node_hfield_shape.h
traversal_node_octree.h
traversal_node_shapes.h