Commit 60a56899 authored by Joseph Mirabel's avatar Joseph Mirabel Committed by Joseph Mirabel
Browse files

Add unit-test for triangle intersection consistency

parent de6955ed
......@@ -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)
......
......@@ -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);
}
}
}
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment