38 #ifndef HPP_FCL_TRAVERSAL_NODE_SETUP_H 39 #define HPP_FCL_TRAVERSAL_NODE_SETUP_H 48 #ifdef HPP_FCL_HAS_OCTOMAP 60 #ifdef HPP_FCL_HAS_OCTOMAP 61 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)
68 node.result = &result;
70 node.model1 = &model1;
71 node.model2 = &model2;
73 node.otsolver = otsolver;
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)
90 node.request = request;
91 node.result = &result;
93 node.model1 = &model1;
94 node.model2 = &model2;
96 node.otsolver = otsolver;
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)
112 node.result = &result;
114 node.model1 = &model1;
115 node.model2 = &model2;
117 node.otsolver = otsolver;
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)
133 node.result = &result;
135 node.model1 = &model1;
136 node.model2 = &model2;
138 node.otsolver = otsolver;
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)
155 node.request = request;
156 node.result = &result;
158 node.model1 = &model1;
159 node.model2 = &model2;
161 node.otsolver = otsolver;
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)
178 node.request = request;
179 node.result = &result;
181 node.model1 = &model1;
182 node.model2 = &model2;
184 node.otsolver = otsolver;
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)
200 node.result = &result;
202 node.model1 = &model1;
203 node.model2 = &model2;
205 node.otsolver = otsolver;
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)
221 node.result = &result;
223 node.model1 = &model1;
224 node.model2 = &model2;
226 node.otsolver = otsolver;
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)
243 node.request = request;
244 node.result = &result;
246 node.model1 = &model1;
247 node.model2 = &model2;
249 node.otsolver = otsolver;
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)
266 node.request = request;
267 node.result = &result;
269 node.model1 = &model1;
270 node.model2 = &model2;
272 node.otsolver = otsolver;
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)
291 node.model1 = &shape1;
293 node.model2 = &shape2;
295 node.nsolver = nsolver;
297 node.result = &result;
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)
312 HPP_FCL_THROW_PRETTY(
"model1 should be of type BVHModelType::BVH_MODEL_TRIANGLES.",std::invalid_argument)
314 if(!tf1.isIdentity())
316 std::vector<Vec3f> vertices_transformed((
size_t)model1.num_vertices);
317 for(
int i = 0; i < model1.num_vertices; ++i)
319 const Vec3f & p = model1.vertices[i];
320 Vec3f new_v = tf1.transform(p);
321 vertices_transformed[(size_t)i] = new_v;
324 model1.beginReplaceModel();
325 model1.replaceSubModel(vertices_transformed);
326 model1.endReplaceModel(use_refit, refit_bottomup);
331 node.model1 = &model1;
333 node.model2 = &model2;
335 node.nsolver = nsolver;
339 node.vertices = model1.vertices;
340 node.tri_indices = model1.tri_indices;
342 node.result = &result;
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)
356 HPP_FCL_THROW_PRETTY(
"model1 should be of type BVHModelType::BVH_MODEL_TRIANGLES.",std::invalid_argument)
358 node.model1 = &model1;
360 node.model2 = &model2;
362 node.nsolver = nsolver;
366 node.vertices = model1.vertices;
367 node.tri_indices = model1.tri_indices;
369 node.result = &result;
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)
385 HPP_FCL_THROW_PRETTY(
"model2 should be of type BVHModelType::BVH_MODEL_TRIANGLES.",std::invalid_argument)
387 node.model1 = &model1;
389 node.model2 = &model2;
391 node.nsolver = nsolver;
395 node.vertices = model2.vertices;
396 node.tri_indices = model2.tri_indices;
398 node.result = &result;
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)
415 HPP_FCL_THROW_PRETTY(
"model1 should be of type BVHModelType::BVH_MODEL_TRIANGLES.",std::invalid_argument)
417 HPP_FCL_THROW_PRETTY(
"model2 should be of type BVHModelType::BVH_MODEL_TRIANGLES.",std::invalid_argument)
419 if(!tf1.isIdentity())
421 std::vector<Vec3f> vertices_transformed1(model1.num_vertices);
422 for(
int i = 0; i < model1.num_vertices; ++i)
424 Vec3f& p = model1.vertices[i];
425 Vec3f new_v = tf1.transform(p);
426 vertices_transformed1[(size_t)i] = new_v;
429 model1.beginReplaceModel();
430 model1.replaceSubModel(vertices_transformed1);
431 model1.endReplaceModel(use_refit, refit_bottomup);
436 if(!tf2.isIdentity())
438 std::vector<Vec3f> vertices_transformed2(model2.num_vertices);
439 for(
int i = 0; i < model2.num_vertices; ++i)
441 Vec3f& p = model2.vertices[i];
442 Vec3f new_v = tf2.transform(p);
443 vertices_transformed2[(size_t)i] = new_v;
446 model2.beginReplaceModel();
447 model2.replaceSubModel(vertices_transformed2);
448 model2.endReplaceModel(use_refit, refit_bottomup);
453 node.model1 = &model1;
455 node.model2 = &model2;
458 node.vertices1 = model1.vertices;
459 node.vertices2 = model2.vertices;
461 node.tri_indices1 = model1.tri_indices;
462 node.tri_indices2 = model2.tri_indices;
464 node.result = &result;
469 template<
typename BV>
471 const BVHModel<BV>& model1,
const Transform3f& tf1,
472 const BVHModel<BV>& model2,
const Transform3f& tf2,
473 CollisionResult& result)
476 HPP_FCL_THROW_PRETTY(
"model1 should be of type BVHModelType::BVH_MODEL_TRIANGLES.",std::invalid_argument)
478 HPP_FCL_THROW_PRETTY(
"model2 should be of type BVHModelType::BVH_MODEL_TRIANGLES.",std::invalid_argument)
480 node.vertices1 = model1.vertices;
481 node.vertices2 = model2.vertices;
483 node.tri_indices1 = model1.tri_indices;
484 node.tri_indices2 = model2.tri_indices;
486 node.model1 = &model1;
488 node.model2 = &model2;
491 node.result = &result;
493 node.RT.R.noalias() = tf1.getRotation().transpose() * tf2.getRotation();
494 node.RT.T.noalias() = tf1.getRotation().transpose() * (tf2.getTranslation() - tf1.getTranslation());
500 template<
typename S1,
typename S2>
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)
508 node.request = request;
509 node.result = &result;
511 node.model1 = &shape1;
513 node.model2 = &shape2;
515 node.nsolver = nsolver;
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)
530 HPP_FCL_THROW_PRETTY(
"model1 should be of type BVHModelType::BVH_MODEL_TRIANGLES.",std::invalid_argument)
532 HPP_FCL_THROW_PRETTY(
"model2 should be of type BVHModelType::BVH_MODEL_TRIANGLES.",std::invalid_argument)
534 if(!tf1.isIdentity())
536 std::vector<Vec3f> vertices_transformed1(model1.num_vertices);
537 for(
int i = 0; i < model1.num_vertices; ++i)
539 const Vec3f & p = model1.vertices[i];
540 Vec3f new_v = tf1.transform(p);
541 vertices_transformed1[(size_t)i] = new_v;
544 model1.beginReplaceModel();
545 model1.replaceSubModel(vertices_transformed1);
546 model1.endReplaceModel(use_refit, refit_bottomup);
551 if(!tf2.isIdentity())
553 std::vector<Vec3f> vertices_transformed2(model2.num_vertices);
554 for(
int i = 0; i < model2.num_vertices; ++i)
556 const Vec3f & p = model2.vertices[i];
557 Vec3f new_v = tf2.transform(p);
558 vertices_transformed2[(size_t)i] = new_v;
561 model2.beginReplaceModel();
562 model2.replaceSubModel(vertices_transformed2);
563 model2.endReplaceModel(use_refit, refit_bottomup);
568 node.request = request;
569 node.result = &result;
571 node.model1 = &model1;
573 node.model2 = &model2;
576 node.vertices1 = model1.vertices;
577 node.vertices2 = model2.vertices;
579 node.tri_indices1 = model1.tri_indices;
580 node.tri_indices2 = model2.tri_indices;
586 template<
typename BV>
588 const BVHModel<BV>& model1,
const Transform3f& tf1,
589 const BVHModel<BV>& model2,
const Transform3f& tf2,
590 const DistanceRequest& request,
591 DistanceResult& result)
594 HPP_FCL_THROW_PRETTY(
"model1 should be of type BVHModelType::BVH_MODEL_TRIANGLES.",std::invalid_argument)
596 HPP_FCL_THROW_PRETTY(
"model2 should be of type BVHModelType::BVH_MODEL_TRIANGLES.",std::invalid_argument)
598 node.request = request;
599 node.result = &result;
601 node.model1 = &model1;
603 node.model2 = &model2;
606 node.vertices1 = model1.vertices;
607 node.vertices2 = model2.vertices;
609 node.tri_indices1 = model1.tri_indices;
610 node.tri_indices2 = model2.tri_indices;
613 tf2.getRotation(), tf2.getTranslation(),
614 node.RT.R, node.RT.T);
620 template<
typename BV,
typename S>
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)
630 HPP_FCL_THROW_PRETTY(
"model1 should be of type BVHModelType::BVH_MODEL_TRIANGLES.",std::invalid_argument)
632 if(!tf1.isIdentity())
634 std::vector<Vec3f> vertices_transformed1(model1.num_vertices);
635 for(
int i = 0; i < model1.num_vertices; ++i)
637 const Vec3f & p = model1.vertices[i];
638 Vec3f new_v = tf1.transform(p);
639 vertices_transformed1[(size_t)i] = new_v;
642 model1.beginReplaceModel();
643 model1.replaceSubModel(vertices_transformed1);
644 model1.endReplaceModel(use_refit, refit_bottomup);
649 node.request = request;
650 node.result = &result;
652 node.model1 = &model1;
654 node.model2 = &model2;
656 node.nsolver = nsolver;
658 node.vertices = model1.vertices;
659 node.tri_indices = model1.tri_indices;
667 template<
typename S,
typename BV>
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)
677 HPP_FCL_THROW_PRETTY(
"model2 should be of type BVHModelType::BVH_MODEL_TRIANGLES.",std::invalid_argument)
679 if(!tf2.isIdentity())
681 std::vector<Vec3f> vertices_transformed(model2.num_vertices);
682 for(
int i = 0; i < model2.num_vertices; ++i)
684 const Vec3f & p = model2.vertices[i];
685 Vec3f new_v = tf2.transform(p);
686 vertices_transformed[(size_t)i] = new_v;
689 model2.beginReplaceModel();
690 model2.replaceSubModel(vertices_transformed);
691 model2.endReplaceModel(use_refit, refit_bottomup);
696 node.request = request;
697 node.result = &result;
699 node.model1 = &model1;
701 node.model2 = &model2;
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)
726 HPP_FCL_THROW_PRETTY(
"model1 should be of type BVHModelType::BVH_MODEL_TRIANGLES.",std::invalid_argument)
728 node.request = request;
729 node.result = &result;
731 node.model1 = &model1;
733 node.model2 = &model2;
735 node.nsolver = nsolver;
739 node.vertices = model1.vertices;
740 node.tri_indices = model1.tri_indices;
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)
756 return details::setupMeshShapeDistanceOrientedNode(node, model1, tf1, model2, tf2, nsolver, request, result);
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)
768 return details::setupMeshShapeDistanceOrientedNode(node, model1, tf1, model2, tf2, nsolver, request, result);
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)
780 return details::setupMeshShapeDistanceOrientedNode(node, model1, tf1, model2, tf2, nsolver, request, result);
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)
795 HPP_FCL_THROW_PRETTY(
"model2 should be of type BVHModelType::BVH_MODEL_TRIANGLES.",std::invalid_argument)
797 node.request = request;
798 node.result = &result;
800 node.model1 = &model1;
802 node.model2 = &model2;
804 node.nsolver = nsolver;
808 node.vertices = model2.vertices;
809 node.tri_indices = model2.tri_indices;
810 node.R = tf2.getRotation();
811 node.T = tf2.getTranslation();
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)
827 return details::setupShapeMeshDistanceOrientedNode(node, model1, tf1, model2, tf2, nsolver, request, result);
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)
839 return details::setupShapeMeshDistanceOrientedNode(node, model1, tf1, model2, tf2, nsolver, request, result);
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)
851 return details::setupShapeMeshDistanceOrientedNode(node, model1, tf1, model2, tf2, nsolver, request, result);
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
Main namespace.
Definition: AABB.h:43
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
unknown model type
Definition: BVH_internal.h:80
Definition: traversal_node_setup.h:784
Triangle * tri_indices
Definition: traversal_node_bvh_shape.h:745
Definition: traversal_node_bvh_shape.h:787
Traversal node for distance between shape and mesh.
Definition: traversal_node_bvh_shape.h:696
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
#define HPP_FCL_THROW_PRETTY(message, exception)
Definition: fwd.hh:51
Eigen::Matrix< FCL_REAL, 3, 1 > Vec3f
Definition: data_types.h:67
Definition: traversal_node_bvh_shape.h:630
Vec3f * vertices
Definition: traversal_node_bvh_shape.h:744
const GJKSolver * nsolver
Definition: traversal_node_bvh_shape.h:750