38 #ifndef HPP_FCL_TRAVERSAL_NODE_SETUP_H 39 #define HPP_FCL_TRAVERSAL_NODE_SETUP_H 48 #ifdef HPP_FCL_HAVE_OCTOMAP 59 #ifdef HPP_FCL_HAVE_OCTOMAP 60 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)
67 node.result = &result;
69 node.model1 = &model1;
70 node.model2 = &model2;
72 node.otsolver = otsolver;
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)
89 node.request = request;
90 node.result = &result;
92 node.model1 = &model1;
93 node.model2 = &model2;
95 node.otsolver = otsolver;
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)
111 node.result = &result;
113 node.model1 = &model1;
114 node.model2 = &model2;
116 node.otsolver = otsolver;
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)
132 node.result = &result;
134 node.model1 = &model1;
135 node.model2 = &model2;
137 node.otsolver = otsolver;
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)
154 node.request = request;
155 node.result = &result;
157 node.model1 = &model1;
158 node.model2 = &model2;
160 node.otsolver = otsolver;
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)
177 node.request = request;
178 node.result = &result;
180 node.model1 = &model1;
181 node.model2 = &model2;
183 node.otsolver = otsolver;
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)
199 node.result = &result;
201 node.model1 = &model1;
202 node.model2 = &model2;
204 node.otsolver = otsolver;
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)
220 node.result = &result;
222 node.model1 = &model1;
223 node.model2 = &model2;
225 node.otsolver = otsolver;
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)
242 node.request = request;
243 node.result = &result;
245 node.model1 = &model1;
246 node.model2 = &model2;
248 node.otsolver = otsolver;
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)
265 node.request = request;
266 node.result = &result;
268 node.model1 = &model1;
269 node.model2 = &model2;
271 node.otsolver = otsolver;
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)
290 node.model1 = &shape1;
292 node.model2 = &shape2;
294 node.nsolver = nsolver;
296 node.result = &result;
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)
313 if(!tf1.isIdentity())
315 std::vector<Vec3f> vertices_transformed(model1.num_vertices);
316 for(
int i = 0; i < model1.num_vertices; ++i)
318 Vec3f& p = model1.vertices[i];
319 Vec3f new_v = tf1.transform(p);
320 vertices_transformed[i] = new_v;
323 model1.beginReplaceModel();
324 model1.replaceSubModel(vertices_transformed);
325 model1.endReplaceModel(use_refit, refit_bottomup);
330 node.model1 = &model1;
332 node.model2 = &model2;
334 node.nsolver = nsolver;
338 node.vertices = model1.vertices;
339 node.tri_indices = model1.tri_indices;
341 node.result = &result;
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)
357 node.model1 = &model1;
359 node.model2 = &model2;
361 node.nsolver = nsolver;
365 node.vertices = model1.vertices;
366 node.tri_indices = model1.tri_indices;
368 node.result = &result;
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)
386 node.model1 = &model1;
388 node.model2 = &model2;
390 node.nsolver = nsolver;
394 node.vertices = model2.vertices;
395 node.tri_indices = model2.tri_indices;
397 node.result = &result;
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)
416 if(!tf1.isIdentity())
418 std::vector<Vec3f> vertices_transformed1(model1.num_vertices);
419 for(
int i = 0; i < model1.num_vertices; ++i)
421 Vec3f& p = model1.vertices[i];
422 Vec3f new_v = tf1.transform(p);
423 vertices_transformed1[i] = new_v;
426 model1.beginReplaceModel();
427 model1.replaceSubModel(vertices_transformed1);
428 model1.endReplaceModel(use_refit, refit_bottomup);
433 if(!tf2.isIdentity())
435 std::vector<Vec3f> vertices_transformed2(model2.num_vertices);
436 for(
int i = 0; i < model2.num_vertices; ++i)
438 Vec3f& p = model2.vertices[i];
439 Vec3f new_v = tf2.transform(p);
440 vertices_transformed2[i] = new_v;
443 model2.beginReplaceModel();
444 model2.replaceSubModel(vertices_transformed2);
445 model2.endReplaceModel(use_refit, refit_bottomup);
450 node.model1 = &model1;
452 node.model2 = &model2;
455 node.vertices1 = model1.vertices;
456 node.vertices2 = model2.vertices;
458 node.tri_indices1 = model1.tri_indices;
459 node.tri_indices2 = model2.tri_indices;
461 node.result = &result;
466 template<
typename BV>
468 const BVHModel<BV>& model1,
const Transform3f& tf1,
469 const BVHModel<BV>& model2,
const Transform3f& tf2,
470 CollisionResult& result)
475 node.vertices1 = model1.vertices;
476 node.vertices2 = model2.vertices;
478 node.tri_indices1 = model1.tri_indices;
479 node.tri_indices2 = model2.tri_indices;
481 node.model1 = &model1;
483 node.model2 = &model2;
486 node.result = &result;
488 node.RT.R = tf1.getRotation().transpose() * tf2.getRotation();
489 node.RT.T = tf1.getRotation().transpose() * (tf2.getTranslation() - tf1.getTranslation());
495 template<
typename S1,
typename S2>
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)
503 node.request = request;
504 node.result = &result;
506 node.model1 = &shape1;
508 node.model2 = &shape2;
510 node.nsolver = nsolver;
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)
527 if(!tf1.isIdentity())
529 std::vector<Vec3f> vertices_transformed1(model1.num_vertices);
530 for(
int i = 0; i < model1.num_vertices; ++i)
532 Vec3f& p = model1.vertices[i];
533 Vec3f new_v = tf1.transform(p);
534 vertices_transformed1[i] = new_v;
537 model1.beginReplaceModel();
538 model1.replaceSubModel(vertices_transformed1);
539 model1.endReplaceModel(use_refit, refit_bottomup);
544 if(!tf2.isIdentity())
546 std::vector<Vec3f> vertices_transformed2(model2.num_vertices);
547 for(
int i = 0; i < model2.num_vertices; ++i)
549 Vec3f& p = model2.vertices[i];
550 Vec3f new_v = tf2.transform(p);
551 vertices_transformed2[i] = new_v;
554 model2.beginReplaceModel();
555 model2.replaceSubModel(vertices_transformed2);
556 model2.endReplaceModel(use_refit, refit_bottomup);
561 node.request = request;
562 node.result = &result;
564 node.model1 = &model1;
566 node.model2 = &model2;
569 node.vertices1 = model1.vertices;
570 node.vertices2 = model2.vertices;
572 node.tri_indices1 = model1.tri_indices;
573 node.tri_indices2 = model2.tri_indices;
579 template<
typename BV>
581 const BVHModel<BV>& model1,
const Transform3f& tf1,
582 const BVHModel<BV>& model2,
const Transform3f& tf2,
583 const DistanceRequest& request,
584 DistanceResult& result)
589 node.request = request;
590 node.result = &result;
592 node.model1 = &model1;
594 node.model2 = &model2;
597 node.vertices1 = model1.vertices;
598 node.vertices2 = model2.vertices;
600 node.tri_indices1 = model1.tri_indices;
601 node.tri_indices2 = model2.tri_indices;
604 tf2.getRotation(), tf2.getTranslation(),
605 node.RT.R, node.RT.T);
611 template<
typename BV,
typename S>
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)
623 if(!tf1.isIdentity())
625 std::vector<Vec3f> vertices_transformed1(model1.num_vertices);
626 for(
int i = 0; i < model1.num_vertices; ++i)
628 Vec3f& p = model1.vertices[i];
629 Vec3f new_v = tf1.transform(p);
630 vertices_transformed1[i] = new_v;
633 model1.beginReplaceModel();
634 model1.replaceSubModel(vertices_transformed1);
635 model1.endReplaceModel(use_refit, refit_bottomup);
640 node.request = request;
641 node.result = &result;
643 node.model1 = &model1;
645 node.model2 = &model2;
647 node.nsolver = nsolver;
649 node.vertices = model1.vertices;
650 node.tri_indices = model1.tri_indices;
658 template<
typename S,
typename BV>
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)
670 if(!tf2.isIdentity())
672 std::vector<Vec3f> vertices_transformed(model2.num_vertices);
673 for(
int i = 0; i < model2.num_vertices; ++i)
675 Vec3f& p = model2.vertices[i];
676 Vec3f new_v = tf2.transform(p);
677 vertices_transformed[i] = new_v;
680 model2.beginReplaceModel();
681 model2.replaceSubModel(vertices_transformed);
682 model2.endReplaceModel(use_refit, refit_bottomup);
687 node.request = request;
688 node.result = &result;
690 node.model1 = &model1;
692 node.model2 = &model2;
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)
719 node.request = request;
720 node.result = &result;
722 node.model1 = &model1;
724 node.model2 = &model2;
726 node.nsolver = nsolver;
730 node.vertices = model1.vertices;
731 node.tri_indices = model1.tri_indices;
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)
747 return details::setupMeshShapeDistanceOrientedNode(node, model1, tf1, model2, tf2, nsolver, request, result);
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)
759 return details::setupMeshShapeDistanceOrientedNode(node, model1, tf1, model2, tf2, nsolver, request, result);
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)
771 return details::setupMeshShapeDistanceOrientedNode(node, model1, tf1, model2, tf2, nsolver, request, result);
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)
788 node.request = request;
789 node.result = &result;
791 node.model1 = &model1;
793 node.model2 = &model2;
795 node.nsolver = nsolver;
799 node.vertices = model2.vertices;
800 node.tri_indices = model2.tri_indices;
801 node.R = tf2.getRotation();
802 node.T = tf2.getTranslation();
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)
818 return details::setupShapeMeshDistanceOrientedNode(node, model1, tf1, model2, tf2, nsolver, request, result);
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)
830 return details::setupShapeMeshDistanceOrientedNode(node, model1, tf1, model2, tf2, nsolver, request, result);
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)
842 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:407
unknown model type
Definition: BVH_internal.h:80
Definition: traversal_node_setup.h:775
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
Eigen::Matrix< FCL_REAL, 3, 1 > Vec3f
Definition: data_types.h:73
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