38 #ifndef HPP_FCL_TRAVERSAL_NODE_SETUP_H
39 #define HPP_FCL_TRAVERSAL_NODE_SETUP_H
52 #ifdef HPP_FCL_HAS_OCTOMAP
61 #ifdef HPP_FCL_HAS_OCTOMAP
62 inline bool initialize(OcTreeCollisionTraversalNode& node,
const OcTree& model1,
65 const Transform3f&
tf1,
const OcTree& model2,
66 const Transform3f&
tf2,
const OcTreeSolver* otsolver,
67 CollisionResult& result) {
68 node.result = &result;
70 node.model1 = &model1;
71 node.model2 = &model2;
73 node.otsolver = otsolver;
83 inline bool initialize(OcTreeDistanceTraversalNode& node,
const OcTree& model1,
84 const Transform3f&
tf1,
const OcTree& model2,
85 const Transform3f&
tf2,
const OcTreeSolver* otsolver,
86 const DistanceRequest& request, DistanceResult& result)
89 node.request = request;
90 node.result = &result;
92 node.model1 = &model1;
93 node.model2 = &model2;
95 node.otsolver = otsolver;
105 template <
typename S>
106 bool initialize(ShapeOcTreeCollisionTraversalNode<S>& node,
const S& model1,
107 const Transform3f&
tf1,
const OcTree& model2,
108 const Transform3f&
tf2,
const OcTreeSolver* otsolver,
109 CollisionResult& result) {
110 node.result = &result;
112 node.model1 = &model1;
113 node.model2 = &model2;
115 node.otsolver = otsolver;
125 template <
typename S>
126 bool initialize(OcTreeShapeCollisionTraversalNode<S>& node,
127 const OcTree& model1,
const Transform3f&
tf1,
const S& model2,
128 const Transform3f&
tf2,
const OcTreeSolver* otsolver,
129 CollisionResult& result) {
130 node.result = &result;
132 node.model1 = &model1;
133 node.model2 = &model2;
135 node.otsolver = otsolver;
145 template <
typename S>
146 bool initialize(ShapeOcTreeDistanceTraversalNode<S>& node,
const S& model1,
147 const Transform3f&
tf1,
const OcTree& model2,
148 const Transform3f&
tf2,
const OcTreeSolver* otsolver,
149 const DistanceRequest& request, DistanceResult& result) {
150 node.request = request;
151 node.result = &result;
153 node.model1 = &model1;
154 node.model2 = &model2;
156 node.otsolver = otsolver;
166 template <
typename S>
167 bool initialize(OcTreeShapeDistanceTraversalNode<S>& node,
const OcTree& model1,
168 const Transform3f&
tf1,
const S& model2,
const Transform3f&
tf2,
169 const OcTreeSolver* otsolver,
const DistanceRequest& request,
170 DistanceResult& result) {
171 node.request = request;
172 node.result = &result;
174 node.model1 = &model1;
175 node.model2 = &model2;
177 node.otsolver = otsolver;
187 template <
typename BV>
188 bool initialize(MeshOcTreeCollisionTraversalNode<BV>& node,
189 const BVHModel<BV>& model1,
const Transform3f&
tf1,
190 const OcTree& model2,
const Transform3f&
tf2,
191 const OcTreeSolver* otsolver, CollisionResult& result) {
192 node.result = &result;
194 node.model1 = &model1;
195 node.model2 = &model2;
197 node.otsolver = otsolver;
207 template <
typename BV>
208 bool initialize(OcTreeMeshCollisionTraversalNode<BV>& node,
209 const OcTree& model1,
const Transform3f&
tf1,
210 const BVHModel<BV>& model2,
const Transform3f&
tf2,
211 const OcTreeSolver* otsolver, CollisionResult& result) {
212 node.result = &result;
214 node.model1 = &model1;
215 node.model2 = &model2;
217 node.otsolver = otsolver;
227 template <
typename BV>
228 bool initialize(MeshOcTreeDistanceTraversalNode<BV>& node,
229 const BVHModel<BV>& model1,
const Transform3f&
tf1,
230 const OcTree& model2,
const Transform3f&
tf2,
231 const OcTreeSolver* otsolver,
const DistanceRequest& request,
232 DistanceResult& result) {
233 node.request = request;
234 node.result = &result;
236 node.model1 = &model1;
237 node.model2 = &model2;
239 node.otsolver = otsolver;
249 template <
typename BV>
250 bool initialize(OcTreeMeshDistanceTraversalNode<BV>& node,
const OcTree& model1,
251 const Transform3f&
tf1,
const BVHModel<BV>& model2,
252 const Transform3f&
tf2,
const OcTreeSolver* otsolver,
253 const DistanceRequest& request, DistanceResult& result) {
254 node.request = request;
255 node.result = &result;
257 node.model1 = &model1;
258 node.model2 = &model2;
260 node.otsolver = otsolver;
272 template <
typename S1,
typename S2>
273 bool initialize(ShapeCollisionTraversalNode<S1, S2>& node,
const S1& shape1,
274 const Transform3f&
tf1,
const S2& shape2,
275 const Transform3f&
tf2,
const GJKSolver* nsolver,
276 CollisionResult& result) {
277 node.model1 = &shape1;
279 node.model2 = &shape2;
281 node.nsolver = nsolver;
283 node.result = &result;
290 template <
typename BV,
typename S>
291 bool initialize(MeshShapeCollisionTraversalNode<BV, S>& node,
292 BVHModel<BV>& model1, Transform3f&
tf1,
const S& model2,
293 const Transform3f&
tf2,
const GJKSolver* nsolver,
294 CollisionResult& result,
bool use_refit =
false,
295 bool refit_bottomup =
false) {
298 "model1 should be of type BVHModelType::BVH_MODEL_TRIANGLES.",
299 std::invalid_argument)
301 if (!
tf1.isIdentity())
303 std::vector<Vec3f> vertices_transformed(model1.num_vertices);
304 for (
unsigned int i = 0; i < model1.num_vertices; ++i) {
305 const Vec3f& p = model1.vertices[i];
307 vertices_transformed[i] = new_v;
310 model1.beginReplaceModel();
311 model1.replaceSubModel(vertices_transformed);
312 model1.endReplaceModel(use_refit, refit_bottomup);
317 node.model1 = &model1;
319 node.model2 = &model2;
321 node.nsolver = nsolver;
325 node.vertices = model1.vertices;
326 node.tri_indices = model1.tri_indices;
328 node.result = &result;
335 template <
typename BV,
typename S>
336 bool initialize(MeshShapeCollisionTraversalNode<BV, S, 0>& node,
337 const BVHModel<BV>& model1,
const Transform3f&
tf1,
338 const S& model2,
const Transform3f&
tf2,
339 const GJKSolver* nsolver, CollisionResult& result) {
342 "model1 should be of type BVHModelType::BVH_MODEL_TRIANGLES.",
343 std::invalid_argument)
345 node.model1 = &model1;
347 node.model2 = &model2;
349 node.nsolver = nsolver;
353 node.vertices = model1.vertices;
354 node.tri_indices = model1.tri_indices;
356 node.result = &result;
363 template <
typename BV,
typename S>
364 bool initialize(HeightFieldShapeCollisionTraversalNode<BV, S>& node,
365 HeightField<BV>& model1, Transform3f&
tf1,
const S& model2,
366 const Transform3f&
tf2,
const GJKSolver* nsolver,
367 CollisionResult& result,
bool use_refit =
false,
368 bool refit_bottomup =
false);
372 template <
typename BV,
typename S>
373 bool initialize(HeightFieldShapeCollisionTraversalNode<BV, S, 0>& node,
374 const HeightField<BV>& model1,
const Transform3f&
tf1,
375 const S& model2,
const Transform3f&
tf2,
376 const GJKSolver* nsolver, CollisionResult& result) {
377 node.model1 = &model1;
379 node.model2 = &model2;
381 node.nsolver = nsolver;
385 node.result = &result;
392 template <
typename S,
typename BV,
template <
typename>
class OrientedNode>
393 static inline bool setupShapeMeshCollisionOrientedNode(
394 OrientedNode<S>& node,
const S& model1,
const Transform3f&
tf1,
395 const BVHModel<BV>& model2,
const Transform3f&
tf2,
396 const GJKSolver* nsolver, CollisionResult& result) {
399 "model2 should be of type BVHModelType::BVH_MODEL_TRIANGLES.",
400 std::invalid_argument)
402 node.model1 = &model1;
404 node.model2 = &model2;
406 node.nsolver = nsolver;
410 node.vertices = model2.vertices;
411 node.tri_indices = model2.tri_indices;
413 node.result = &result;
422 template <
typename BV>
424 MeshCollisionTraversalNode<BV, RelativeTransformationIsIdentity>& node,
425 BVHModel<BV>& model1, Transform3f&
tf1, BVHModel<BV>& model2,
426 Transform3f&
tf2, CollisionResult& result,
bool use_refit =
false,
427 bool refit_bottomup =
false) {
430 "model1 should be of type BVHModelType::BVH_MODEL_TRIANGLES.",
431 std::invalid_argument)
434 "model2 should be of type BVHModelType::BVH_MODEL_TRIANGLES.",
435 std::invalid_argument)
437 if (!
tf1.isIdentity()) {
438 std::vector<Vec3f> vertices_transformed1(model1.num_vertices);
439 for (
unsigned int i = 0; i < model1.num_vertices; ++i) {
440 Vec3f& p = model1.vertices[i];
442 vertices_transformed1[i] = new_v;
445 model1.beginReplaceModel();
446 model1.replaceSubModel(vertices_transformed1);
447 model1.endReplaceModel(use_refit, refit_bottomup);
452 if (!
tf2.isIdentity()) {
453 std::vector<Vec3f> vertices_transformed2(model2.num_vertices);
454 for (
unsigned int i = 0; i < model2.num_vertices; ++i) {
455 Vec3f& p = model2.vertices[i];
457 vertices_transformed2[i] = new_v;
460 model2.beginReplaceModel();
461 model2.replaceSubModel(vertices_transformed2);
462 model2.endReplaceModel(use_refit, refit_bottomup);
467 node.model1 = &model1;
469 node.model2 = &model2;
472 node.vertices1 = model1.vertices;
473 node.vertices2 = model2.vertices;
475 node.tri_indices1 = model1.tri_indices;
476 node.tri_indices2 = model2.tri_indices;
478 node.result = &result;
483 template <
typename BV>
485 const BVHModel<BV>& model1,
const Transform3f&
tf1,
486 const BVHModel<BV>& model2,
const Transform3f&
tf2,
487 CollisionResult& result) {
490 "model1 should be of type BVHModelType::BVH_MODEL_TRIANGLES.",
491 std::invalid_argument)
494 "model2 should be of type BVHModelType::BVH_MODEL_TRIANGLES.",
495 std::invalid_argument)
497 node.vertices1 = model1.vertices;
498 node.vertices2 = model2.vertices;
500 node.tri_indices1 = model1.tri_indices;
501 node.tri_indices2 = model2.tri_indices;
503 node.model1 = &model1;
505 node.model2 = &model2;
508 node.result = &result;
510 node.RT.R.noalias() =
tf1.getRotation().transpose() *
tf2.getRotation();
511 node.RT.T.noalias() =
tf1.getRotation().transpose() *
512 (
tf2.getTranslation() -
tf1.getTranslation());
518 template <
typename S1,
typename S2>
519 bool initialize(ShapeDistanceTraversalNode<S1, S2>& node,
const S1& shape1,
520 const Transform3f&
tf1,
const S2& shape2,
521 const Transform3f&
tf2,
const GJKSolver* nsolver,
522 const DistanceRequest& request, DistanceResult& result) {
523 node.request = request;
524 node.result = &result;
526 node.model1 = &shape1;
528 node.model2 = &shape2;
530 node.nsolver = nsolver;
537 template <
typename BV>
539 MeshDistanceTraversalNode<BV, RelativeTransformationIsIdentity>& node,
540 BVHModel<BV>& model1, Transform3f&
tf1, BVHModel<BV>& model2,
541 Transform3f&
tf2,
const DistanceRequest& request, DistanceResult& result,
542 bool use_refit =
false,
bool refit_bottomup =
false) {
545 "model1 should be of type BVHModelType::BVH_MODEL_TRIANGLES.",
546 std::invalid_argument)
549 "model2 should be of type BVHModelType::BVH_MODEL_TRIANGLES.",
550 std::invalid_argument)
552 if (!
tf1.isIdentity()) {
553 std::vector<Vec3f> vertices_transformed1(model1.num_vertices);
554 for (
unsigned int i = 0; i < model1.num_vertices; ++i) {
555 const Vec3f& p = model1.vertices[i];
557 vertices_transformed1[i] = new_v;
560 model1.beginReplaceModel();
561 model1.replaceSubModel(vertices_transformed1);
562 model1.endReplaceModel(use_refit, refit_bottomup);
567 if (!
tf2.isIdentity()) {
568 std::vector<Vec3f> vertices_transformed2(model2.num_vertices);
569 for (
unsigned int i = 0; i < model2.num_vertices; ++i) {
570 const Vec3f& p = model2.vertices[i];
572 vertices_transformed2[i] = new_v;
575 model2.beginReplaceModel();
576 model2.replaceSubModel(vertices_transformed2);
577 model2.endReplaceModel(use_refit, refit_bottomup);
582 node.request = request;
583 node.result = &result;
585 node.model1 = &model1;
587 node.model2 = &model2;
590 node.vertices1 = model1.vertices;
591 node.vertices2 = model2.vertices;
593 node.tri_indices1 = model1.tri_indices;
594 node.tri_indices2 = model2.tri_indices;
600 template <
typename BV>
602 const BVHModel<BV>& model1,
const Transform3f&
tf1,
603 const BVHModel<BV>& model2,
const Transform3f&
tf2,
604 const DistanceRequest& request, DistanceResult& result) {
607 "model1 should be of type BVHModelType::BVH_MODEL_TRIANGLES.",
608 std::invalid_argument)
611 "model2 should be of type BVHModelType::BVH_MODEL_TRIANGLES.",
612 std::invalid_argument)
614 node.request = request;
615 node.result = &result;
617 node.model1 = &model1;
619 node.model2 = &model2;
622 node.vertices1 = model1.vertices;
623 node.vertices2 = model2.vertices;
625 node.tri_indices1 = model1.tri_indices;
626 node.tri_indices2 = model2.tri_indices;
629 tf2.getTranslation(), node.RT.R, node.RT.T);
636 template <
typename BV,
typename S>
638 BVHModel<BV>& model1, Transform3f&
tf1,
const S& model2,
639 const Transform3f&
tf2,
const GJKSolver* nsolver,
640 const DistanceRequest& request, DistanceResult& result,
641 bool use_refit =
false,
bool refit_bottomup =
false) {
644 "model1 should be of type BVHModelType::BVH_MODEL_TRIANGLES.",
645 std::invalid_argument)
647 if (!
tf1.isIdentity()) {
648 std::vector<Vec3f> vertices_transformed1(model1.num_vertices);
649 for (
unsigned int i = 0; i < model1.num_vertices; ++i) {
650 const Vec3f& p = model1.vertices[i];
652 vertices_transformed1[i] = new_v;
655 model1.beginReplaceModel();
656 model1.replaceSubModel(vertices_transformed1);
657 model1.endReplaceModel(use_refit, refit_bottomup);
662 node.request = request;
663 node.result = &result;
665 node.model1 = &model1;
667 node.model2 = &model2;
669 node.nsolver = nsolver;
671 node.vertices = model1.vertices;
672 node.tri_indices = model1.tri_indices;
681 template <
typename S,
typename BV>
683 const Transform3f&
tf1, BVHModel<BV>& model2, Transform3f&
tf2,
684 const GJKSolver* nsolver,
const DistanceRequest& request,
685 DistanceResult& result,
bool use_refit =
false,
686 bool refit_bottomup =
false) {
689 "model2 should be of type BVHModelType::BVH_MODEL_TRIANGLES.",
690 std::invalid_argument)
692 if (!
tf2.isIdentity()) {
693 std::vector<Vec3f> vertices_transformed(model2.num_vertices);
694 for (
unsigned int i = 0; i < model2.num_vertices; ++i) {
695 const Vec3f& p = model2.vertices[i];
697 vertices_transformed[i] = new_v;
700 model2.beginReplaceModel();
701 model2.replaceSubModel(vertices_transformed);
702 model2.endReplaceModel(use_refit, refit_bottomup);
707 node.request = request;
708 node.result = &result;
710 node.model1 = &model1;
712 node.model2 = &model2;
727 template <
typename BV,
typename S,
template <
typename>
class OrientedNode>
728 static inline bool setupMeshShapeDistanceOrientedNode(
729 OrientedNode<S>& node,
const BVHModel<BV>& model1,
const Transform3f&
tf1,
730 const S& model2,
const Transform3f&
tf2,
const GJKSolver* nsolver,
731 const DistanceRequest& request, DistanceResult& result) {
734 "model1 should be of type BVHModelType::BVH_MODEL_TRIANGLES.",
735 std::invalid_argument)
737 node.request = request;
738 node.result = &result;
740 node.model1 = &model1;
742 node.model2 = &model2;
744 node.nsolver = nsolver;
748 node.vertices = model1.vertices;
749 node.tri_indices = model1.tri_indices;
758 template <
typename S>
760 const BVHModel<RSS>& model1,
const Transform3f&
tf1,
761 const S& model2,
const Transform3f&
tf2,
762 const GJKSolver* nsolver,
const DistanceRequest& request,
763 DistanceResult& result) {
764 return details::setupMeshShapeDistanceOrientedNode(
765 node, model1,
tf1, model2,
tf2, nsolver, request, result);
770 template <
typename S>
772 const BVHModel<kIOS>& model1,
const Transform3f&
tf1,
773 const S& model2,
const Transform3f&
tf2,
774 const GJKSolver* nsolver,
const DistanceRequest& request,
775 DistanceResult& result) {
776 return details::setupMeshShapeDistanceOrientedNode(
777 node, model1,
tf1, model2,
tf2, nsolver, request, result);
782 template <
typename S>
784 const BVHModel<OBBRSS>& model1,
const Transform3f&
tf1,
785 const S& model2,
const Transform3f&
tf2,
786 const GJKSolver* nsolver,
const DistanceRequest& request,
787 DistanceResult& result) {
788 return details::setupMeshShapeDistanceOrientedNode(
789 node, model1,
tf1, model2,
tf2, nsolver, request, result);
793 template <
typename S,
typename BV,
template <
typename>
class OrientedNode>
795 OrientedNode<S>& node,
const S& model1,
const Transform3f&
tf1,
796 const BVHModel<BV>& model2,
const Transform3f&
tf2,
797 const GJKSolver* nsolver,
const DistanceRequest& request,
798 DistanceResult& result) {
801 "model2 should be of type BVHModelType::BVH_MODEL_TRIANGLES.",
802 std::invalid_argument)
804 node.request = request;
805 node.result = &result;
807 node.model1 = &model1;
809 node.model2 = &model2;
811 node.nsolver = nsolver;
815 node.vertices = model2.vertices;
816 node.tri_indices = model2.tri_indices;
817 node.R =
tf2.getRotation();
818 node.T =
tf2.getTranslation();
826 template <
typename S>
828 const Transform3f&
tf1,
const BVHModel<RSS>& model2,
829 const Transform3f&
tf2,
const GJKSolver* nsolver,
830 const DistanceRequest& request, DistanceResult& result) {
832 node, model1,
tf1, model2,
tf2, nsolver, request, result);
837 template <
typename S>
839 const Transform3f&
tf1,
const BVHModel<kIOS>& model2,
840 const Transform3f&
tf2,
const GJKSolver* nsolver,
841 const DistanceRequest& request, DistanceResult& result) {
843 node, model1,
tf1, model2,
tf2, nsolver, request, result);
848 template <
typename S>
850 const Transform3f&
tf1,
const BVHModel<OBBRSS>& model2,
851 const Transform3f&
tf2,
const GJKSolver* nsolver,
852 const DistanceRequest& request, DistanceResult& result) {
854 node, model1,
tf1, model2,
tf2, nsolver, request, result);