38#ifndef HPP_FCL_TRAVERSAL_NODE_SETUP_H
39#define HPP_FCL_TRAVERSAL_NODE_SETUP_H
52#ifdef HPP_FCL_HAS_OCTOMAP
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)
72 node.result = &result;
74 node.model1 = &model1;
75 node.model2 = &model2;
77 node.otsolver = otsolver;
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)
94 node.request = request;
95 node.result = &result;
97 node.model1 = &model1;
98 node.model2 = &model2;
100 node.otsolver = otsolver;
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)
116 node.result = &result;
118 node.model1 = &model1;
119 node.model2 = &model2;
121 node.otsolver = otsolver;
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)
137 node.result = &result;
139 node.model1 = &model1;
140 node.model2 = &model2;
142 node.otsolver = otsolver;
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)
159 node.request = request;
160 node.result = &result;
162 node.model1 = &model1;
163 node.model2 = &model2;
165 node.otsolver = otsolver;
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)
182 node.request = request;
183 node.result = &result;
185 node.model1 = &model1;
186 node.model2 = &model2;
188 node.otsolver = otsolver;
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)
204 node.result = &result;
206 node.model1 = &model1;
207 node.model2 = &model2;
209 node.otsolver = otsolver;
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)
225 node.result = &result;
227 node.model1 = &model1;
228 node.model2 = &model2;
230 node.otsolver = otsolver;
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)
247 node.request = request;
248 node.result = &result;
250 node.model1 = &model1;
251 node.model2 = &model2;
253 node.otsolver = otsolver;
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)
270 node.request = request;
271 node.result = &result;
273 node.model1 = &model1;
274 node.model2 = &model2;
276 node.otsolver = otsolver;
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)
295 node.model1 = &shape1;
297 node.model2 = &shape2;
299 node.nsolver = nsolver;
301 node.result = &result;
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)
316 HPP_FCL_THROW_PRETTY(
"model1 should be of type BVHModelType::BVH_MODEL_TRIANGLES.",std::invalid_argument)
318 if(!tf1.isIdentity())
320 std::vector<Vec3f> vertices_transformed(model1.num_vertices);
321 for(
unsigned int i = 0; i < model1.num_vertices; ++i)
323 const Vec3f & p = model1.vertices[i];
324 Vec3f new_v = tf1.transform(p);
325 vertices_transformed[i] = new_v;
328 model1.beginReplaceModel();
329 model1.replaceSubModel(vertices_transformed);
330 model1.endReplaceModel(use_refit, refit_bottomup);
335 node.model1 = &model1;
337 node.model2 = &model2;
339 node.nsolver = nsolver;
343 node.vertices = model1.vertices;
344 node.tri_indices = model1.tri_indices;
346 node.result = &result;
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)
360 HPP_FCL_THROW_PRETTY(
"model1 should be of type BVHModelType::BVH_MODEL_TRIANGLES.",std::invalid_argument)
362 node.model1 = &model1;
364 node.model2 = &model2;
366 node.nsolver = nsolver;
370 node.vertices = model1.vertices;
371 node.tri_indices = model1.tri_indices;
373 node.result = &result;
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)
387 if(!tf1.isIdentity())
389 std::vector<Vec3f> vertices_transformed(model1.num_vertices);
390 for(
unsigned int i = 0; i < model1.num_vertices; ++i)
392 const Vec3f & p = model1.vertices[i];
393 Vec3f new_v = tf1.transform(p);
394 vertices_transformed[i] = new_v;
397 model1.beginReplaceModel();
398 model1.replaceSubModel(vertices_transformed);
399 model1.endReplaceModel(use_refit, refit_bottomup);
404 node.model1 = &model1;
406 node.model2 = &model2;
408 node.nsolver = nsolver;
412 node.vertices = model1.vertices;
413 node.tri_indices = model1.tri_indices;
415 node.result = &result;
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)
428 node.model1 = &model1;
430 node.model2 = &model2;
432 node.nsolver = nsolver;
436 node.result = &result;
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)
452 HPP_FCL_THROW_PRETTY(
"model2 should be of type BVHModelType::BVH_MODEL_TRIANGLES.",std::invalid_argument)
454 node.model1 = &model1;
456 node.model2 = &model2;
458 node.nsolver = nsolver;
462 node.vertices = model2.vertices;
463 node.tri_indices = model2.tri_indices;
465 node.result = &result;
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)
482 HPP_FCL_THROW_PRETTY(
"model1 should be of type BVHModelType::BVH_MODEL_TRIANGLES.",std::invalid_argument)
484 HPP_FCL_THROW_PRETTY(
"model2 should be of type BVHModelType::BVH_MODEL_TRIANGLES.",std::invalid_argument)
486 if(!tf1.isIdentity())
488 std::vector<Vec3f> vertices_transformed1(model1.num_vertices);
489 for(
unsigned int i = 0; i < model1.num_vertices; ++i)
491 Vec3f& p = model1.vertices[i];
492 Vec3f new_v = tf1.transform(p);
493 vertices_transformed1[i] = new_v;
496 model1.beginReplaceModel();
497 model1.replaceSubModel(vertices_transformed1);
498 model1.endReplaceModel(use_refit, refit_bottomup);
503 if(!tf2.isIdentity())
505 std::vector<Vec3f> vertices_transformed2(model2.num_vertices);
506 for(
unsigned int i = 0; i < model2.num_vertices; ++i)
508 Vec3f& p = model2.vertices[i];
509 Vec3f new_v = tf2.transform(p);
510 vertices_transformed2[i] = new_v;
513 model2.beginReplaceModel();
514 model2.replaceSubModel(vertices_transformed2);
515 model2.endReplaceModel(use_refit, refit_bottomup);
520 node.model1 = &model1;
522 node.model2 = &model2;
525 node.vertices1 = model1.vertices;
526 node.vertices2 = model2.vertices;
528 node.tri_indices1 = model1.tri_indices;
529 node.tri_indices2 = model2.tri_indices;
531 node.result = &result;
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)
543 HPP_FCL_THROW_PRETTY(
"model1 should be of type BVHModelType::BVH_MODEL_TRIANGLES.",std::invalid_argument)
545 HPP_FCL_THROW_PRETTY(
"model2 should be of type BVHModelType::BVH_MODEL_TRIANGLES.",std::invalid_argument)
547 node.vertices1 = model1.vertices;
548 node.vertices2 = model2.vertices;
550 node.tri_indices1 = model1.tri_indices;
551 node.tri_indices2 = model2.tri_indices;
553 node.model1 = &model1;
555 node.model2 = &model2;
558 node.result = &result;
560 node.RT.R.noalias() = tf1.getRotation().transpose() * tf2.getRotation();
561 node.RT.T.noalias() = tf1.getRotation().transpose() * (tf2.getTranslation() - tf1.getTranslation());
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)
575 node.request = request;
576 node.result = &result;
578 node.model1 = &shape1;
580 node.model2 = &shape2;
582 node.nsolver = nsolver;
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)
597 HPP_FCL_THROW_PRETTY(
"model1 should be of type BVHModelType::BVH_MODEL_TRIANGLES.",std::invalid_argument)
599 HPP_FCL_THROW_PRETTY(
"model2 should be of type BVHModelType::BVH_MODEL_TRIANGLES.",std::invalid_argument)
601 if(!tf1.isIdentity())
603 std::vector<Vec3f> vertices_transformed1(model1.num_vertices);
604 for(
unsigned int i = 0; i < model1.num_vertices; ++i)
606 const Vec3f & p = model1.vertices[i];
607 Vec3f new_v = tf1.transform(p);
608 vertices_transformed1[i] = new_v;
611 model1.beginReplaceModel();
612 model1.replaceSubModel(vertices_transformed1);
613 model1.endReplaceModel(use_refit, refit_bottomup);
618 if(!tf2.isIdentity())
620 std::vector<Vec3f> vertices_transformed2(model2.num_vertices);
621 for(
unsigned int i = 0; i < model2.num_vertices; ++i)
623 const Vec3f & p = model2.vertices[i];
624 Vec3f new_v = tf2.transform(p);
625 vertices_transformed2[i] = new_v;
628 model2.beginReplaceModel();
629 model2.replaceSubModel(vertices_transformed2);
630 model2.endReplaceModel(use_refit, refit_bottomup);
635 node.request = request;
636 node.result = &result;
638 node.model1 = &model1;
640 node.model2 = &model2;
643 node.vertices1 = model1.vertices;
644 node.vertices2 = model2.vertices;
646 node.tri_indices1 = model1.tri_indices;
647 node.tri_indices2 = model2.tri_indices;
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)
661 HPP_FCL_THROW_PRETTY(
"model1 should be of type BVHModelType::BVH_MODEL_TRIANGLES.",std::invalid_argument)
663 HPP_FCL_THROW_PRETTY(
"model2 should be of type BVHModelType::BVH_MODEL_TRIANGLES.",std::invalid_argument)
665 node.request = request;
666 node.result = &result;
668 node.model1 = &model1;
670 node.model2 = &model2;
673 node.vertices1 = model1.vertices;
674 node.vertices2 = model2.vertices;
676 node.tri_indices1 = model1.tri_indices;
677 node.tri_indices2 = model2.tri_indices;
680 tf2.getRotation(), tf2.getTranslation(),
681 node.RT.R, node.RT.T);
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)
697 HPP_FCL_THROW_PRETTY(
"model1 should be of type BVHModelType::BVH_MODEL_TRIANGLES.",std::invalid_argument)
699 if(!tf1.isIdentity())
701 std::vector<Vec3f> vertices_transformed1(model1.num_vertices);
702 for(
unsigned int i = 0; i < model1.num_vertices; ++i)
704 const Vec3f & p = model1.vertices[i];
705 Vec3f new_v = tf1.transform(p);
706 vertices_transformed1[i] = new_v;
709 model1.beginReplaceModel();
710 model1.replaceSubModel(vertices_transformed1);
711 model1.endReplaceModel(use_refit, refit_bottomup);
716 node.request = request;
717 node.result = &result;
719 node.model1 = &model1;
721 node.model2 = &model2;
723 node.nsolver = nsolver;
725 node.vertices = model1.vertices;
726 node.tri_indices = model1.tri_indices;
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)
744 HPP_FCL_THROW_PRETTY(
"model2 should be of type BVHModelType::BVH_MODEL_TRIANGLES.",std::invalid_argument)
746 if(!tf2.isIdentity())
748 std::vector<Vec3f> vertices_transformed(model2.num_vertices);
749 for(
unsigned int i = 0; i < model2.num_vertices; ++i)
751 const Vec3f & p = model2.vertices[i];
752 Vec3f new_v = tf2.transform(p);
753 vertices_transformed[i] = new_v;
756 model2.beginReplaceModel();
757 model2.replaceSubModel(vertices_transformed);
758 model2.endReplaceModel(use_refit, refit_bottomup);
763 node.request = request;
764 node.result = &result;
766 node.model1 = &model1;
768 node.model2 = &model2;
770 node.nsolver = nsolver;
772 node.vertices = model2.vertices;
773 node.tri_indices = model2.tri_indices;
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)
793 HPP_FCL_THROW_PRETTY(
"model1 should be of type BVHModelType::BVH_MODEL_TRIANGLES.",std::invalid_argument)
795 node.request = request;
796 node.result = &result;
798 node.model1 = &model1;
800 node.model2 = &model2;
802 node.nsolver = nsolver;
806 node.vertices = model1.vertices;
807 node.tri_indices = model1.tri_indices;
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)
823 return details::setupMeshShapeDistanceOrientedNode(node, model1, tf1, model2, tf2, nsolver, request, result);
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)
835 return details::setupMeshShapeDistanceOrientedNode(node, model1, tf1, model2, tf2, nsolver, request, result);
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)
847 return details::setupMeshShapeDistanceOrientedNode(node, model1, tf1, model2, tf2, nsolver, request, result);
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)
862 HPP_FCL_THROW_PRETTY(
"model2 should be of type BVHModelType::BVH_MODEL_TRIANGLES.",std::invalid_argument)
864 node.request = request;
865 node.result = &result;
867 node.model1 = &model1;
869 node.model2 = &model2;
871 node.nsolver = nsolver;
875 node.vertices = model2.vertices;
876 node.tri_indices = model2.tri_indices;
877 node.R = tf2.getRotation();
878 node.T = tf2.getTranslation();
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)
894 return details::setupShapeMeshDistanceOrientedNode(node, model1, tf1, model2, tf2, nsolver, request, result);
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)
906 return details::setupShapeMeshDistanceOrientedNode(node, model1, tf1, model2, tf2, nsolver, request, result);
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)
918 return details::setupShapeMeshDistanceOrientedNode(node, model1, tf1, model2, tf2, nsolver, request, result);
#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