diff --git a/test/test_fcl_geometric_shapes.cpp b/test/test_fcl_geometric_shapes.cpp index 7fbbd4e9fd94b49fa252cc84aba61dd32598ae9e..e12ca957801d0a420d4f5a49e7915b6edb3bf429 100644 --- a/test/test_fcl_geometric_shapes.cpp +++ b/test/test_fcl_geometric_shapes.cpp @@ -3082,6 +3082,88 @@ BOOST_AUTO_TEST_CASE(shapeIntersectionGJK_spheretriangle) BOOST_CHECK(normal.equal(transform.getRotation() * Vec3f(1, 0, 0), 1e-9)); } +BOOST_AUTO_TEST_CASE(shapeIntersectionGJK_halfspacetriangle) +{ + Halfspace hs(Vec3f(1, 0, 0), 0); + Vec3f t[3]; + t[0].setValue(20, 0, 0); + t[1].setValue(-20, 0, 0); + t[2].setValue(0, 20, 0); + + Transform3f transform; + generateRandomTransform(extents, transform); + + // Vec3f point; + // FCL_REAL depth; + Vec3f normal; + bool res; + + res = solver2.shapeTriangleIntersect(hs, Transform3f(), t[0], t[1], t[2], Transform3f(), NULL, NULL, NULL); + BOOST_CHECK(res); + + res = solver2.shapeTriangleIntersect(hs, transform, t[0], t[1], t[2], transform, NULL, NULL, NULL); + BOOST_CHECK(res); + + + t[0].setValue(20, 0, 0); + t[1].setValue(0, -20, 0); + t[2].setValue(0, 20, 0); + res = solver1.shapeTriangleIntersect(hs, Transform3f(), t[0], t[1], t[2], Transform3f(), NULL, NULL, NULL); + BOOST_CHECK(res); + + res = solver2.shapeTriangleIntersect(hs, transform, t[0], t[1], t[2], transform, NULL, NULL, NULL); + BOOST_CHECK(res); + + res = solver2.shapeTriangleIntersect(hs, Transform3f(), t[0], t[1], t[2], Transform3f(), NULL, NULL, &normal); + BOOST_CHECK(res); + BOOST_CHECK(normal.equal(Vec3f(1, 0, 0), 1e-9)); + + res = solver2.shapeTriangleIntersect(hs, 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(shapeIntersectionGJK_planetriangle) +{ + Plane hs(Vec3f(1, 0, 0), 0); + Vec3f t[3]; + t[0].setValue(20, 0, 0); + t[1].setValue(-20, 0, 0); + t[2].setValue(0, 20, 0); + + Transform3f transform; + generateRandomTransform(extents, transform); + + // Vec3f point; + // FCL_REAL depth; + Vec3f normal; + bool res; + + res = solver1.shapeTriangleIntersect(hs, Transform3f(), t[0], t[1], t[2], Transform3f(), NULL, NULL, NULL); + BOOST_CHECK(res); + + res = solver1.shapeTriangleIntersect(hs, transform, t[0], t[1], t[2], transform, NULL, NULL, NULL); + BOOST_CHECK(res); + + + t[0].setValue(20, 0, 0); + t[1].setValue(-0.1, -20, 0); + t[2].setValue(-0.1, 20, 0); + res = solver1.shapeTriangleIntersect(hs, Transform3f(), t[0], t[1], t[2], Transform3f(), NULL, NULL, NULL); + BOOST_CHECK(res); + + res = solver2.shapeTriangleIntersect(hs, transform, t[0], t[1], t[2], transform, NULL, NULL, NULL); + BOOST_CHECK(res); + + res = solver2.shapeTriangleIntersect(hs, Transform3f(), t[0], t[1], t[2], Transform3f(), NULL, NULL, &normal); + BOOST_CHECK(res); + BOOST_CHECK(normal.equal(Vec3f(1, 0, 0), 1e-9)); + + res = solver2.shapeTriangleIntersect(hs, 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)); +} +