diff --git a/test/test_fcl_geometric_shapes.cpp b/test/test_fcl_geometric_shapes.cpp index a3f66c23c..ed437c73e 100755 --- a/test/test_fcl_geometric_shapes.cpp +++ b/test/test_fcl_geometric_shapes.cpp @@ -955,6 +955,57 @@ GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeIntersection_cylindercylinder) test_shapeIntersection_cylindercylinder(); } +template +void test_shapeIntersection_cylindercylinder_fingers() +{ + Cylinder s1(0.007, 0.040); + Cylinder s2(0.007, 0.025); + Transform3 tf1; + Transform3 tf2; + std::vector> contacts; + + Transform3 tfj1pos(Translation3(Vector3(0.011, 0, 0.004))); + Transform3 tfs1center(Translation3(Vector3(0.0, 0, 0.025))); + Transform3 tfs2center(Translation3(Vector3(0.0, 0, 0.0575))); + Transform3 tfj2pos(Translation3(Vector3(-0.011, 0, 0.0))); + + Quaternion q(AngleAxis((S)-0.13, Vector3(0, -1, 0))); + Transform3 tfj1rot = Transform3(q); + q = AngleAxis((S)0.12, Vector3(0, 1, 0)); + Transform3 tfj2rot = Transform3(q); + std::cout << "test: no proximal / middle offset (both at same height)\n"; + // without offset + tf1 = tfj1pos; + tf2 = tfj2pos; + testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, false, contacts, false, false, false, false); + std::cout << "test: proximal and middle as in the hand\n"; + // without rotation + tf1 = tfj1pos * tfs1center; + tf2 = tfj2pos * tfs2center; + testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, false, contacts, false, false, false, false); + std::cout << "test: proximal and middle as in the hand with 0.12 rad angle\n"; + // with rotation ok + tf1 = tfj1pos * tfj1rot * tfs1center; + tf2 = tfj2pos * tfj2rot * tfs2center; + testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, false, contacts, false, false, false, false); + + std::cout << "test: proximal and middle as in the hand with 0.13 rad angle\n"; + // with rotation 0.01 RAD more + q = AngleAxis((S)0.13, Vector3(0, 1, 0)); + tfj2rot = Transform3(q); + + tf1 = tfj1pos * tfj1rot * tfs1center; + tf2 = tfj2pos * tfj2rot * tfs2center; + testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, false, contacts, false, false, false, false); + +} + +GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeIntersection_cylindercylinder_fingers) +{ +// test_shapeIntersection_cylindercylinder_fingers(); + test_shapeIntersection_cylindercylinder_fingers(); +} + template void test_shapeIntersection_conecone() {