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