hpp-fcl: include/hpp/fcl/internal/traversal_node_setup.h Source File

hpp-fcl 1.7.0 HPP fork of FCL -- The Flexible Collision Library

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