diff --git a/test/test_fcl_geometric_shapes.cpp b/test/test_fcl_geometric_shapes.cpp index f6b34d17644fea6f601df8e6f3808632b0aed6cb..7fbbd4e9fd94b49fa252cc84aba61dd32598ae9e 100644 --- a/test/test_fcl_geometric_shapes.cpp +++ b/test/test_fcl_geometric_shapes.cpp @@ -3053,6 +3053,9 @@ BOOST_AUTO_TEST_CASE(shapeIntersectionGJK_spheretriangle) Transform3f transform; generateRandomTransform(extents, transform); + // Vec3f point; + // FCL_REAL depth; + Vec3f normal; bool res; res = solver2.shapeTriangleIntersect(s, Transform3f(), t[0], t[1], t[2], NULL, NULL, NULL); @@ -3069,6 +3072,14 @@ BOOST_AUTO_TEST_CASE(shapeIntersectionGJK_spheretriangle) res = solver2.shapeTriangleIntersect(s, transform, t[0], t[1], t[2], transform, NULL, NULL, NULL); BOOST_CHECK(res); + + res = solver2.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 = solver2.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)); }