diff --git a/test/test_fcl_geometric_shapes.cpp b/test/test_fcl_geometric_shapes.cpp index 931e06d4d8193373f2bad11124b7122a127b789a..5e05a114f55bed4e86f207ab903e2feb8115ecb0 100644 --- a/test/test_fcl_geometric_shapes.cpp +++ b/test/test_fcl_geometric_shapes.cpp @@ -3563,92 +3563,6 @@ BOOST_AUTO_TEST_CASE(conecone) -template<typename S1, typename S2> -void testReversibleShapeIntersection(const S1& s1, const S2& s2, FCL_REAL distance) -{ - Transform3f tf1(Vec3f(-0.5 * distance, 0.0, 0.0)); - Transform3f tf2(Vec3f(+0.5 * distance, 0.0, 0.0)); - - Vec3f contactA; - Vec3f contactB; - FCL_REAL depthA; - FCL_REAL depthB; - Vec3f normalA; - Vec3f normalB; - - bool resA; - bool resB; - - const double tol = 1e-6; - - resA = solver1.shapeIntersect(s1, tf1, s2, tf2, &contactA, &depthA, &normalA); - resB = solver1.shapeIntersect(s2, tf2, s1, tf1, &contactB, &depthB, &normalB); - - BOOST_CHECK(resA); - BOOST_CHECK(resB); - BOOST_CHECK(isEqual(normalA, -normalB, tol)); // normal should be opposite - BOOST_CHECK_CLOSE(depthA, depthB, tol); // penetration depth should be same - - resA = solver2.shapeIntersect(s1, tf1, s2, tf2, &contactA, &depthA, &normalA); - resB = solver2.shapeIntersect(s2, tf2, s1, tf1, &contactB, &depthB, &normalB); - - BOOST_CHECK(resA); - BOOST_CHECK(resB); - BOOST_CHECK(isEqual(normalA, -normalB, tol)); - BOOST_CHECK_CLOSE(depthA, depthB, tol); -} - -BOOST_AUTO_TEST_CASE(reversibleShapeIntersection_allshapes) -{ - // This test check whether a shape intersection algorithm is called for the - // reverse case as well. For example, if FCL has sphere-capsule intersection - // algorithm, then this algorithm should be called for capsule-sphere case. - - // Prepare all kinds of primitive shapes (7) -- box, sphere, capsule, cone, cylinder, plane, halfspace - Box box(10, 10, 10); - Sphere sphere(5); - Capsule capsule(5, 10); - Cone cone(5, 10); - Cylinder cylinder(5, 10); - Plane plane(Vec3f(), 0.0); - Halfspace halfspace(Vec3f(), 0.0); - - // Use sufficiently short distance so that all the primitive shapes can intersect - FCL_REAL distance = 5.0; - - // If new shape intersection algorithm is added for two distinct primitive - // shapes, uncomment associated lines. For example, box-sphere intersection - // algorithm is added, then uncomment box-sphere. - -// testReversibleShapeIntersection(box, sphere, distance); -// testReversibleShapeIntersection(box, capsule, distance); -// testReversibleShapeIntersection(box, cone, distance); -// testReversibleShapeIntersection(box, cylinder, distance); - testReversibleShapeIntersection(box, plane, distance); - testReversibleShapeIntersection(box, halfspace, distance); - - testReversibleShapeIntersection(sphere, capsule, distance); -// testReversibleShapeIntersection(sphere, cone, distance); -// testReversibleShapeIntersection(sphere, cylinder, distance); - testReversibleShapeIntersection(sphere, plane, distance); - testReversibleShapeIntersection(sphere, halfspace, distance); - -// testReversibleShapeIntersection(capsule, cone, distance); -// testReversibleShapeIntersection(capsule, cylinder, distance); - testReversibleShapeIntersection(capsule, plane, distance); - testReversibleShapeIntersection(capsule, halfspace, distance); - -// testReversibleShapeIntersection(cone, cylinder, distance); - testReversibleShapeIntersection(cone, plane, distance); - testReversibleShapeIntersection(cone, halfspace, distance); - - testReversibleShapeIntersection(cylinder, plane, distance); - testReversibleShapeIntersection(cylinder, halfspace, distance); - - // TODO no contact, penetration depth and normal return for this pair - // testReversibleShapeIntersection(plane, halfspace, distance); -} - template<typename S1, typename S2> void testReversibleShapeDistance(const S1& s1, const S2& s2, FCL_REAL distance) {