diff --git a/test/test_fcl_geometric_shapes.cpp b/test/test_fcl_geometric_shapes.cpp index 0abfc32d4df15fc780b4a313b6f21d8d89556010..ef349770d92caece58a3d6faf67cff0baf4c6516 100644 --- a/test/test_fcl_geometric_shapes.cpp +++ b/test/test_fcl_geometric_shapes.cpp @@ -688,8 +688,8 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_spheretriangle) Transform3f transform; generateRandomTransform(extents, transform); - Transform3f identity; + Vec3f normal; bool res; res = solver1.shapeTriangleIntersect(s, Transform3f(), t[0], t[1], t[2], NULL, NULL, NULL); @@ -707,6 +707,14 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_spheretriangle) res = solver1.shapeTriangleIntersect(s, transform, t[0], t[1], t[2], transform, NULL, NULL, NULL); BOOST_CHECK(res); + + res = solver1.shapeTriangleIntersect(s, Transform3f(), t[0], t[1], t[2], NULL, NULL, &normal); + BOOST_CHECK(res); + BOOST_CHECK(normal.equal(Vec3f(1, 0, 0), 1e-9)); + + res = solver1.shapeTriangleIntersect(s, transform, t[0], t[1], t[2], transform, NULL, NULL, &normal); + BOOST_CHECK(res); + BOOST_CHECK(normal.equal(transform.getRotation() * Vec3f(1, 0, 0), 1e-9)); } BOOST_AUTO_TEST_CASE(shapeIntersection_halfspacesphere)