Add unit-test for triangle intersection consistency

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)

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);
+    }
+  }
+}