hpp-fcl 2.2.0
HPP 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
38#ifndef HPP_FCL_TRAVERSAL_NODE_SETUP_H
39#define HPP_FCL_TRAVERSAL_NODE_SETUP_H
40
42
45
48
49// #include <hpp/fcl/internal/traversal_node_hfields.h>
51
52#ifdef HPP_FCL_HAS_OCTOMAP
54#endif
55
57
58namespace hpp {
59namespace fcl {
60
61#ifdef HPP_FCL_HAS_OCTOMAP
64inline bool initialize(OcTreeCollisionTraversalNode& node, const OcTree& model1,
65 const Transform3f& tf1, const OcTree& model2,
66 const Transform3f& tf2, const OcTreeSolver* otsolver,
67 CollisionResult& result) {
68 node.result = &result;
69
70 node.model1 = &model1;
71 node.model2 = &model2;
72
73 node.otsolver = otsolver;
74
75 node.tf1 = tf1;
76 node.tf2 = tf2;
77
78 return true;
79}
80
83inline bool initialize(OcTreeDistanceTraversalNode& node, const OcTree& model1,
84 const Transform3f& tf1, const OcTree& model2,
85 const Transform3f& tf2, const OcTreeSolver* otsolver,
86 const DistanceRequest& request, DistanceResult& result)
87
88{
89 node.request = request;
90 node.result = &result;
91
92 node.model1 = &model1;
93 node.model2 = &model2;
94
95 node.otsolver = otsolver;
96
97 node.tf1 = tf1;
98 node.tf2 = tf2;
99
100 return true;
101}
102
105template <typename S>
106bool initialize(ShapeOcTreeCollisionTraversalNode<S>& node, const S& model1,
107 const Transform3f& tf1, const OcTree& model2,
108 const Transform3f& tf2, const OcTreeSolver* otsolver,
109 CollisionResult& result) {
110 node.result = &result;
111
112 node.model1 = &model1;
113 node.model2 = &model2;
114
115 node.otsolver = otsolver;
116
117 node.tf1 = tf1;
118 node.tf2 = tf2;
119
120 return true;
121}
122
125template <typename S>
126bool initialize(OcTreeShapeCollisionTraversalNode<S>& node,
127 const OcTree& model1, const Transform3f& tf1, const S& model2,
128 const Transform3f& tf2, const OcTreeSolver* otsolver,
129 CollisionResult& result) {
130 node.result = &result;
131
132 node.model1 = &model1;
133 node.model2 = &model2;
134
135 node.otsolver = otsolver;
136
137 node.tf1 = tf1;
138 node.tf2 = tf2;
139
140 return true;
141}
142
145template <typename S>
146bool initialize(ShapeOcTreeDistanceTraversalNode<S>& node, const S& model1,
147 const Transform3f& tf1, const OcTree& model2,
148 const Transform3f& tf2, const OcTreeSolver* otsolver,
149 const DistanceRequest& request, DistanceResult& result) {
150 node.request = request;
151 node.result = &result;
152
153 node.model1 = &model1;
154 node.model2 = &model2;
155
156 node.otsolver = otsolver;
157
158 node.tf1 = tf1;
159 node.tf2 = tf2;
160
161 return true;
162}
163
166template <typename S>
167bool initialize(OcTreeShapeDistanceTraversalNode<S>& node, const OcTree& model1,
168 const Transform3f& tf1, const S& model2, const Transform3f& tf2,
169 const OcTreeSolver* otsolver, const DistanceRequest& request,
170 DistanceResult& result) {
171 node.request = request;
172 node.result = &result;
173
174 node.model1 = &model1;
175 node.model2 = &model2;
176
177 node.otsolver = otsolver;
178
179 node.tf1 = tf1;
180 node.tf2 = tf2;
181
182 return true;
183}
184
187template <typename BV>
188bool initialize(MeshOcTreeCollisionTraversalNode<BV>& node,
189 const BVHModel<BV>& model1, const Transform3f& tf1,
190 const OcTree& model2, const Transform3f& tf2,
191 const OcTreeSolver* otsolver, CollisionResult& result) {
192 node.result = &result;
193
194 node.model1 = &model1;
195 node.model2 = &model2;
196
197 node.otsolver = otsolver;
198
199 node.tf1 = tf1;
200 node.tf2 = tf2;
201
202 return true;
203}
204
207template <typename BV>
208bool initialize(OcTreeMeshCollisionTraversalNode<BV>& node,
209 const OcTree& model1, const Transform3f& tf1,
210 const BVHModel<BV>& model2, const Transform3f& tf2,
211 const OcTreeSolver* otsolver, CollisionResult& result) {
212 node.result = &result;
213
214 node.model1 = &model1;
215 node.model2 = &model2;
216
217 node.otsolver = otsolver;
218
219 node.tf1 = tf1;
220 node.tf2 = tf2;
221
222 return true;
223}
224
227template <typename BV>
228bool initialize(MeshOcTreeDistanceTraversalNode<BV>& node,
229 const BVHModel<BV>& model1, const Transform3f& tf1,
230 const OcTree& model2, const Transform3f& tf2,
231 const OcTreeSolver* otsolver, const DistanceRequest& request,
232 DistanceResult& result) {
233 node.request = request;
234 node.result = &result;
235
236 node.model1 = &model1;
237 node.model2 = &model2;
238
239 node.otsolver = otsolver;
240
241 node.tf1 = tf1;
242 node.tf2 = tf2;
243
244 return true;
245}
246
249template <typename BV>
250bool initialize(OcTreeMeshDistanceTraversalNode<BV>& node, const OcTree& model1,
251 const Transform3f& tf1, const BVHModel<BV>& model2,
252 const Transform3f& tf2, const OcTreeSolver* otsolver,
253 const DistanceRequest& request, DistanceResult& result) {
254 node.request = request;
255 node.result = &result;
256
257 node.model1 = &model1;
258 node.model2 = &model2;
259
260 node.otsolver = otsolver;
261
262 node.tf1 = tf1;
263 node.tf2 = tf2;
264
265 return true;
266}
267
268#endif
269
272template <typename S1, typename S2>
273bool initialize(ShapeCollisionTraversalNode<S1, S2>& node, const S1& shape1,
274 const Transform3f& tf1, const S2& shape2,
275 const Transform3f& tf2, const GJKSolver* nsolver,
276 CollisionResult& result) {
277 node.model1 = &shape1;
278 node.tf1 = tf1;
279 node.model2 = &shape2;
280 node.tf2 = tf2;
281 node.nsolver = nsolver;
282
283 node.result = &result;
284
285 return true;
286}
287
290template <typename BV, typename S>
291bool initialize(MeshShapeCollisionTraversalNode<BV, S>& node,
292 BVHModel<BV>& model1, Transform3f& tf1, const S& model2,
293 const Transform3f& tf2, const GJKSolver* nsolver,
294 CollisionResult& result, bool use_refit = false,
295 bool refit_bottomup = false) {
296 if (model1.getModelType() != BVH_MODEL_TRIANGLES)
298 "model1 should be of type BVHModelType::BVH_MODEL_TRIANGLES.",
299 std::invalid_argument)
300
301 if (!tf1.isIdentity()) // TODO(jcarpent): vectorized version
302 {
303 std::vector<Vec3f> vertices_transformed(model1.num_vertices);
304 for (unsigned int i = 0; i < model1.num_vertices; ++i) {
305 const Vec3f& p = model1.vertices[i];
306 Vec3f new_v = tf1.transform(p);
307 vertices_transformed[i] = new_v;
308 }
309
310 model1.beginReplaceModel();
311 model1.replaceSubModel(vertices_transformed);
312 model1.endReplaceModel(use_refit, refit_bottomup);
313
314 tf1.setIdentity();
315 }
316
317 node.model1 = &model1;
318 node.tf1 = tf1;
319 node.model2 = &model2;
320 node.tf2 = tf2;
321 node.nsolver = nsolver;
322
323 computeBV(model2, tf2, node.model2_bv);
324
325 node.vertices = model1.vertices;
326 node.tri_indices = model1.tri_indices;
327
328 node.result = &result;
329
330 return true;
331}
332
335template <typename BV, typename S>
336bool initialize(MeshShapeCollisionTraversalNode<BV, S, 0>& node,
337 const BVHModel<BV>& model1, const Transform3f& tf1,
338 const S& model2, const Transform3f& tf2,
339 const GJKSolver* nsolver, CollisionResult& result) {
340 if (model1.getModelType() != BVH_MODEL_TRIANGLES)
342 "model1 should be of type BVHModelType::BVH_MODEL_TRIANGLES.",
343 std::invalid_argument)
344
345 node.model1 = &model1;
346 node.tf1 = tf1;
347 node.model2 = &model2;
348 node.tf2 = tf2;
349 node.nsolver = nsolver;
350
351 computeBV(model2, tf2, node.model2_bv);
352
353 node.vertices = model1.vertices;
354 node.tri_indices = model1.tri_indices;
355
356 node.result = &result;
357
358 return true;
359}
360
363template <typename BV, typename S>
364bool initialize(HeightFieldShapeCollisionTraversalNode<BV, S>& node,
365 HeightField<BV>& model1, Transform3f& tf1, const S& model2,
366 const Transform3f& tf2, const GJKSolver* nsolver,
367 CollisionResult& result, bool use_refit = false,
368 bool refit_bottomup = false);
369
372template <typename BV, typename S>
373bool initialize(HeightFieldShapeCollisionTraversalNode<BV, S, 0>& node,
374 const HeightField<BV>& model1, const Transform3f& tf1,
375 const S& model2, const Transform3f& tf2,
376 const GJKSolver* nsolver, CollisionResult& result) {
377 node.model1 = &model1;
378 node.tf1 = tf1;
379 node.model2 = &model2;
380 node.tf2 = tf2;
381 node.nsolver = nsolver;
382
383 computeBV(model2, tf2, node.model2_bv);
384
385 node.result = &result;
386
387 return true;
388}
389
391namespace details {
392template <typename S, typename BV, template <typename> class OrientedNode>
393static inline bool setupShapeMeshCollisionOrientedNode(
394 OrientedNode<S>& node, const S& model1, const Transform3f& tf1,
395 const BVHModel<BV>& model2, const Transform3f& tf2,
396 const GJKSolver* nsolver, CollisionResult& result) {
397 if (model2.getModelType() != BVH_MODEL_TRIANGLES)
399 "model2 should be of type BVHModelType::BVH_MODEL_TRIANGLES.",
400 std::invalid_argument)
401
402 node.model1 = &model1;
403 node.tf1 = tf1;
404 node.model2 = &model2;
405 node.tf2 = tf2;
406 node.nsolver = nsolver;
407
408 computeBV(model1, tf1, node.model1_bv);
409
410 node.vertices = model2.vertices;
411 node.tri_indices = model2.tri_indices;
412
413 node.result = &result;
414
415 return true;
416}
417} // namespace details
419
422template <typename BV>
423bool initialize(
424 MeshCollisionTraversalNode<BV, RelativeTransformationIsIdentity>& node,
425 BVHModel<BV>& model1, Transform3f& tf1, BVHModel<BV>& model2,
426 Transform3f& tf2, CollisionResult& result, bool use_refit = false,
427 bool refit_bottomup = false) {
428 if (model1.getModelType() != BVH_MODEL_TRIANGLES)
430 "model1 should be of type BVHModelType::BVH_MODEL_TRIANGLES.",
431 std::invalid_argument)
432 if (model2.getModelType() != BVH_MODEL_TRIANGLES)
434 "model2 should be of type BVHModelType::BVH_MODEL_TRIANGLES.",
435 std::invalid_argument)
436
437 if (!tf1.isIdentity()) {
438 std::vector<Vec3f> vertices_transformed1(model1.num_vertices);
439 for (unsigned int i = 0; i < model1.num_vertices; ++i) {
440 Vec3f& p = model1.vertices[i];
441 Vec3f new_v = tf1.transform(p);
442 vertices_transformed1[i] = new_v;
443 }
444
445 model1.beginReplaceModel();
446 model1.replaceSubModel(vertices_transformed1);
447 model1.endReplaceModel(use_refit, refit_bottomup);
448
449 tf1.setIdentity();
450 }
451
452 if (!tf2.isIdentity()) {
453 std::vector<Vec3f> vertices_transformed2(model2.num_vertices);
454 for (unsigned int i = 0; i < model2.num_vertices; ++i) {
455 Vec3f& p = model2.vertices[i];
456 Vec3f new_v = tf2.transform(p);
457 vertices_transformed2[i] = new_v;
458 }
459
460 model2.beginReplaceModel();
461 model2.replaceSubModel(vertices_transformed2);
462 model2.endReplaceModel(use_refit, refit_bottomup);
463
464 tf2.setIdentity();
465 }
466
467 node.model1 = &model1;
468 node.tf1 = tf1;
469 node.model2 = &model2;
470 node.tf2 = tf2;
471
472 node.vertices1 = model1.vertices;
473 node.vertices2 = model2.vertices;
474
475 node.tri_indices1 = model1.tri_indices;
476 node.tri_indices2 = model2.tri_indices;
477
478 node.result = &result;
479
480 return true;
481}
482
483template <typename BV>
484bool initialize(MeshCollisionTraversalNode<BV, 0>& node,
485 const BVHModel<BV>& model1, const Transform3f& tf1,
486 const BVHModel<BV>& model2, const Transform3f& tf2,
487 CollisionResult& result) {
488 if (model1.getModelType() != BVH_MODEL_TRIANGLES)
490 "model1 should be of type BVHModelType::BVH_MODEL_TRIANGLES.",
491 std::invalid_argument)
492 if (model2.getModelType() != BVH_MODEL_TRIANGLES)
494 "model2 should be of type BVHModelType::BVH_MODEL_TRIANGLES.",
495 std::invalid_argument)
496
497 node.vertices1 = model1.vertices;
498 node.vertices2 = model2.vertices;
499
500 node.tri_indices1 = model1.tri_indices;
501 node.tri_indices2 = model2.tri_indices;
502
503 node.model1 = &model1;
504 node.tf1 = tf1;
505 node.model2 = &model2;
506 node.tf2 = tf2;
507
508 node.result = &result;
509
510 node.RT.R.noalias() = tf1.getRotation().transpose() * tf2.getRotation();
511 node.RT.T.noalias() = tf1.getRotation().transpose() *
512 (tf2.getTranslation() - tf1.getTranslation());
513
514 return true;
515}
516
518template <typename S1, typename S2>
519bool initialize(ShapeDistanceTraversalNode<S1, S2>& node, const S1& shape1,
520 const Transform3f& tf1, const S2& shape2,
521 const Transform3f& tf2, const GJKSolver* nsolver,
522 const DistanceRequest& request, DistanceResult& result) {
523 node.request = request;
524 node.result = &result;
525
526 node.model1 = &shape1;
527 node.tf1 = tf1;
528 node.model2 = &shape2;
529 node.tf2 = tf2;
530 node.nsolver = nsolver;
531
532 return true;
533}
534
537template <typename BV>
538bool initialize(
539 MeshDistanceTraversalNode<BV, RelativeTransformationIsIdentity>& node,
540 BVHModel<BV>& model1, Transform3f& tf1, BVHModel<BV>& model2,
541 Transform3f& tf2, const DistanceRequest& request, DistanceResult& result,
542 bool use_refit = false, bool refit_bottomup = false) {
543 if (model1.getModelType() != BVH_MODEL_TRIANGLES)
545 "model1 should be of type BVHModelType::BVH_MODEL_TRIANGLES.",
546 std::invalid_argument)
547 if (model2.getModelType() != BVH_MODEL_TRIANGLES)
549 "model2 should be of type BVHModelType::BVH_MODEL_TRIANGLES.",
550 std::invalid_argument)
551
552 if (!tf1.isIdentity()) {
553 std::vector<Vec3f> vertices_transformed1(model1.num_vertices);
554 for (unsigned int i = 0; i < model1.num_vertices; ++i) {
555 const Vec3f& p = model1.vertices[i];
556 Vec3f new_v = tf1.transform(p);
557 vertices_transformed1[i] = new_v;
558 }
559
560 model1.beginReplaceModel();
561 model1.replaceSubModel(vertices_transformed1);
562 model1.endReplaceModel(use_refit, refit_bottomup);
563
564 tf1.setIdentity();
565 }
566
567 if (!tf2.isIdentity()) {
568 std::vector<Vec3f> vertices_transformed2(model2.num_vertices);
569 for (unsigned int i = 0; i < model2.num_vertices; ++i) {
570 const Vec3f& p = model2.vertices[i];
571 Vec3f new_v = tf2.transform(p);
572 vertices_transformed2[i] = new_v;
573 }
574
575 model2.beginReplaceModel();
576 model2.replaceSubModel(vertices_transformed2);
577 model2.endReplaceModel(use_refit, refit_bottomup);
578
579 tf2.setIdentity();
580 }
581
582 node.request = request;
583 node.result = &result;
584
585 node.model1 = &model1;
586 node.tf1 = tf1;
587 node.model2 = &model2;
588 node.tf2 = tf2;
589
590 node.vertices1 = model1.vertices;
591 node.vertices2 = model2.vertices;
592
593 node.tri_indices1 = model1.tri_indices;
594 node.tri_indices2 = model2.tri_indices;
595
596 return true;
597}
598
600template <typename BV>
601bool initialize(MeshDistanceTraversalNode<BV, 0>& node,
602 const BVHModel<BV>& model1, const Transform3f& tf1,
603 const BVHModel<BV>& model2, const Transform3f& tf2,
604 const DistanceRequest& request, DistanceResult& result) {
605 if (model1.getModelType() != BVH_MODEL_TRIANGLES)
607 "model1 should be of type BVHModelType::BVH_MODEL_TRIANGLES.",
608 std::invalid_argument)
609 if (model2.getModelType() != BVH_MODEL_TRIANGLES)
611 "model2 should be of type BVHModelType::BVH_MODEL_TRIANGLES.",
612 std::invalid_argument)
613
614 node.request = request;
615 node.result = &result;
616
617 node.model1 = &model1;
618 node.tf1 = tf1;
619 node.model2 = &model2;
620 node.tf2 = tf2;
621
622 node.vertices1 = model1.vertices;
623 node.vertices2 = model2.vertices;
624
625 node.tri_indices1 = model1.tri_indices;
626 node.tri_indices2 = model2.tri_indices;
627
628 relativeTransform(tf1.getRotation(), tf1.getTranslation(), tf2.getRotation(),
629 tf2.getTranslation(), node.RT.R, node.RT.T);
630
631 return true;
632}
633
636template <typename BV, typename S>
637bool initialize(MeshShapeDistanceTraversalNode<BV, S>& node,
638 BVHModel<BV>& model1, Transform3f& tf1, const S& model2,
639 const Transform3f& tf2, const GJKSolver* nsolver,
640 const DistanceRequest& request, DistanceResult& result,
641 bool use_refit = false, bool refit_bottomup = false) {
642 if (model1.getModelType() != BVH_MODEL_TRIANGLES)
644 "model1 should be of type BVHModelType::BVH_MODEL_TRIANGLES.",
645 std::invalid_argument)
646
647 if (!tf1.isIdentity()) {
648 std::vector<Vec3f> vertices_transformed1(model1.num_vertices);
649 for (unsigned int i = 0; i < model1.num_vertices; ++i) {
650 const Vec3f& p = model1.vertices[i];
651 Vec3f new_v = tf1.transform(p);
652 vertices_transformed1[i] = new_v;
653 }
654
655 model1.beginReplaceModel();
656 model1.replaceSubModel(vertices_transformed1);
657 model1.endReplaceModel(use_refit, refit_bottomup);
658
659 tf1.setIdentity();
660 }
661
662 node.request = request;
663 node.result = &result;
664
665 node.model1 = &model1;
666 node.tf1 = tf1;
667 node.model2 = &model2;
668 node.tf2 = tf2;
669 node.nsolver = nsolver;
670
671 node.vertices = model1.vertices;
672 node.tri_indices = model1.tri_indices;
673
674 computeBV(model2, tf2, node.model2_bv);
675
676 return true;
677}
678
681template <typename S, typename BV>
682bool initialize(ShapeMeshDistanceTraversalNode<S, BV>& node, const S& model1,
683 const Transform3f& tf1, BVHModel<BV>& model2, Transform3f& tf2,
684 const GJKSolver* nsolver, const DistanceRequest& request,
685 DistanceResult& result, bool use_refit = false,
686 bool refit_bottomup = false) {
687 if (model2.getModelType() != BVH_MODEL_TRIANGLES)
689 "model2 should be of type BVHModelType::BVH_MODEL_TRIANGLES.",
690 std::invalid_argument)
691
692 if (!tf2.isIdentity()) {
693 std::vector<Vec3f> vertices_transformed(model2.num_vertices);
694 for (unsigned int i = 0; i < model2.num_vertices; ++i) {
695 const Vec3f& p = model2.vertices[i];
696 Vec3f new_v = tf2.transform(p);
697 vertices_transformed[i] = new_v;
698 }
699
700 model2.beginReplaceModel();
701 model2.replaceSubModel(vertices_transformed);
702 model2.endReplaceModel(use_refit, refit_bottomup);
703
704 tf2.setIdentity();
705 }
706
707 node.request = request;
708 node.result = &result;
709
710 node.model1 = &model1;
711 node.tf1 = tf1;
712 node.model2 = &model2;
713 node.tf2 = tf2;
714 node.nsolver = nsolver;
715
716 node.vertices = model2.vertices;
717 node.tri_indices = model2.tri_indices;
718
719 computeBV(model1, tf1, node.model1_bv);
720
721 return true;
722}
723
725namespace details {
726
727template <typename BV, typename S, template <typename> class OrientedNode>
728static inline bool setupMeshShapeDistanceOrientedNode(
729 OrientedNode<S>& node, const BVHModel<BV>& model1, const Transform3f& tf1,
730 const S& model2, const Transform3f& tf2, const GJKSolver* nsolver,
731 const DistanceRequest& request, DistanceResult& result) {
732 if (model1.getModelType() != BVH_MODEL_TRIANGLES)
734 "model1 should be of type BVHModelType::BVH_MODEL_TRIANGLES.",
735 std::invalid_argument)
736
737 node.request = request;
738 node.result = &result;
739
740 node.model1 = &model1;
741 node.tf1 = tf1;
742 node.model2 = &model2;
743 node.tf2 = tf2;
744 node.nsolver = nsolver;
745
746 computeBV(model2, tf2, node.model2_bv);
747
748 node.vertices = model1.vertices;
749 node.tri_indices = model1.tri_indices;
750
751 return true;
752}
753} // namespace details
755
758template <typename S>
759bool initialize(MeshShapeDistanceTraversalNodeRSS<S>& node,
760 const BVHModel<RSS>& model1, const Transform3f& tf1,
761 const S& model2, const Transform3f& tf2,
762 const GJKSolver* nsolver, const DistanceRequest& request,
763 DistanceResult& result) {
764 return details::setupMeshShapeDistanceOrientedNode(
765 node, model1, tf1, model2, tf2, nsolver, request, result);
766}
767
770template <typename S>
771bool initialize(MeshShapeDistanceTraversalNodekIOS<S>& node,
772 const BVHModel<kIOS>& model1, const Transform3f& tf1,
773 const S& model2, const Transform3f& tf2,
774 const GJKSolver* nsolver, const DistanceRequest& request,
775 DistanceResult& result) {
776 return details::setupMeshShapeDistanceOrientedNode(
777 node, model1, tf1, model2, tf2, nsolver, request, result);
778}
779
782template <typename S>
783bool initialize(MeshShapeDistanceTraversalNodeOBBRSS<S>& node,
784 const BVHModel<OBBRSS>& model1, const Transform3f& tf1,
785 const S& model2, const Transform3f& tf2,
786 const GJKSolver* nsolver, const DistanceRequest& request,
787 DistanceResult& result) {
788 return details::setupMeshShapeDistanceOrientedNode(
789 node, model1, tf1, model2, tf2, nsolver, request, result);
790}
791
792namespace details {
793template <typename S, typename BV, template <typename> class OrientedNode>
794static inline bool setupShapeMeshDistanceOrientedNode(
795 OrientedNode<S>& node, const S& model1, const Transform3f& tf1,
796 const BVHModel<BV>& model2, const Transform3f& tf2,
797 const GJKSolver* nsolver, const DistanceRequest& request,
798 DistanceResult& result) {
799 if (model2.getModelType() != BVH_MODEL_TRIANGLES)
801 "model2 should be of type BVHModelType::BVH_MODEL_TRIANGLES.",
802 std::invalid_argument)
803
804 node.request = request;
805 node.result = &result;
806
807 node.model1 = &model1;
808 node.tf1 = tf1;
809 node.model2 = &model2;
810 node.tf2 = tf2;
811 node.nsolver = nsolver;
812
813 computeBV(model1, tf1, node.model1_bv);
814
815 node.vertices = model2.vertices;
816 node.tri_indices = model2.tri_indices;
817 node.R = tf2.getRotation();
818 node.T = tf2.getTranslation();
819
820 return true;
821}
822} // namespace details
823
826template <typename S>
827bool initialize(ShapeMeshDistanceTraversalNodeRSS<S>& node, const S& model1,
828 const Transform3f& tf1, const BVHModel<RSS>& model2,
829 const Transform3f& tf2, const GJKSolver* nsolver,
830 const DistanceRequest& request, DistanceResult& result) {
831 return details::setupShapeMeshDistanceOrientedNode(
832 node, model1, tf1, model2, tf2, nsolver, request, result);
833}
834
837template <typename S>
838bool initialize(ShapeMeshDistanceTraversalNodekIOS<S>& node, const S& model1,
839 const Transform3f& tf1, const BVHModel<kIOS>& model2,
840 const Transform3f& tf2, const GJKSolver* nsolver,
841 const DistanceRequest& request, DistanceResult& result) {
842 return details::setupShapeMeshDistanceOrientedNode(
843 node, model1, tf1, model2, tf2, nsolver, request, result);
844}
845
848template <typename S>
849bool initialize(ShapeMeshDistanceTraversalNodeOBBRSS<S>& node, const S& model1,
850 const Transform3f& tf1, const BVHModel<OBBRSS>& model2,
851 const Transform3f& tf2, const GJKSolver* nsolver,
852 const DistanceRequest& request, DistanceResult& result) {
853 return details::setupShapeMeshDistanceOrientedNode(
854 node, model1, tf1, model2, tf2, nsolver, request, result);
855}
856
857} // namespace fcl
858
859} // namespace hpp
860
862
863#endif
#define HPP_FCL_THROW_PRETTY(message, exception)
Definition: fwd.hh:57
void computeBV(const S &s, const Transform3f &tf, BV &bv)
calculate a bounding volume for a shape in a specific configuration
Definition: geometric_shapes_utility.h:74
Eigen::Matrix< FCL_REAL, 3, 1 > Vec3f
Definition: data_types.h:66
@ BVH_MODEL_TRIANGLES
unknown model type
Definition: BVH_internal.h:82
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:92
Main namespace.
Definition: broadphase_bruteforce.h:44