diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt index c07c10af53fa57978aa7fe13f4aa6ff6ca64db57..9f0efffd4e85c416dd9fcaf9b57e9908e52e7b3d 100644 --- a/test/CMakeLists.txt +++ b/test/CMakeLists.txt @@ -21,7 +21,7 @@ include_directories(${CMAKE_CURRENT_BINARY_DIR}) include_directories(${Boost_INCLUDE_DIRS}) -add_fcl_template_test(test_fcl_math test_fcl_math.cpp) +add_fcl_test(test_fcl_math test_fcl_math.cpp) add_fcl_test(test_fcl_collision test_fcl_collision.cpp test_fcl_utility.cpp) add_fcl_test(test_fcl_distance test_fcl_distance.cpp test_fcl_utility.cpp) diff --git a/test/test_fcl_math.cpp b/test/test_fcl_math.cpp index b38070e2f997baaf06686ba0071a4629608c22bd..1a1bda0a2f6b40506b191e7cd9b5a4c03950eab9 100644 --- a/test/test_fcl_math.cpp +++ b/test/test_fcl_math.cpp @@ -44,6 +44,8 @@ #include <hpp/fcl/math/transform.h> #include <hpp/fcl/broadphase/morton.h> +#include <hpp/fcl/intersect.h> + using namespace fcl; template<typename Derived> @@ -162,3 +164,71 @@ BOOST_AUTO_TEST_CASE(transform) // std::cout << output.lhs() << std::endl; BOOST_CHECK(isEqual(rv + T, tf.transform(v))); } + +BOOST_AUTO_TEST_CASE(intersect_triangle) +{ + std::vector< Vec3f > points(3 * 6); + points[0] = Vec3f(0,0,0); + points[1] = Vec3f(1,0,0); + points[2] = Vec3f(0,1,0); + + // FCL_REAL eps = +1e-16; + FCL_REAL eps = 0; + points[3] = Vec3f(0.5,0,eps); + points[4] = Vec3f(0.5,0,1); + points[5] = Vec3f(0.5,1,eps); + + eps = -1e-3; + points[6] = Vec3f(0.5,0,eps); + points[7] = Vec3f(0.5,0,1); + points[8] = Vec3f(0.5,1,eps); + + eps = -1e-9; + points[9] = Vec3f(0.5,0,eps); + points[10] = Vec3f(0.5,0,1); + points[11] = Vec3f(0.5,1,eps); + + points[12] = Vec3f(0.43977451324462891,0.047868609428405762,-0.074923992156982422); + points[13] = Vec3f(0.409393310546875,0.048755228519439697,-0.083331555128097534); + points[14] = Vec3f(0.41051089763641357,0.059760168194770813,-0.071275442838668823); + + points[15] = Vec3f(0.43746706770940053,0.04866138334047334,-0.075818714863365125); + points[16] = Vec3f(0.44251195980451652,0.043831023891018804,-0.074980982849817135); + points[17] = Vec3f(0.4213840328819074,0.076059133343436849,-0.07361578194185768); + + std::vector < int > pairs(2 * 4); + pairs[0] = 0; + pairs[1] = 3; + pairs[2] = 0; + pairs[3] = 6; + pairs[4] = 0; + pairs[5] = 9; + pairs[6] = 12; + pairs[7] = 15; + + for (std::size_t ip = 6; ip < pairs.size(); ip += 2) { + Vec3f& p1 = points[pairs[ip + 0] ]; + Vec3f& p2 = points[pairs[ip + 0] + 1]; + Vec3f& p3 = points[pairs[ip + 0] + 2]; + + Vec3f& q1 = points[pairs[ip + 1] ]; + Vec3f& q2 = points[pairs[ip + 1] + 1]; + Vec3f& q3 = points[pairs[ip + 1] + 2]; + + FCL_REAL penetration; + Vec3f normal; + unsigned int n_contacts; + Vec3f contacts[2]; + + bool intersect = + Intersect::intersect_Triangle(p1, p2, p3, q1, q2, q3, + contacts, &n_contacts, &penetration, &normal); + + if (intersect) { + std::cout << ip << " intersect" << std::endl; + BOOST_CHECK_MESSAGE (n_contacts > 0, "There shoud be at least 1 contact: " << n_contacts); + } else { + BOOST_CHECK_MESSAGE (n_contacts == 0, "There shoud be 0 contact: " << n_contacts); + } + } +}