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

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