Commit ab5321bc authored by Jeongseok Lee's avatar Jeongseok Lee
Browse files

Add box-box collision test to check correctness of contact point

parent 33ffb691
......@@ -199,6 +199,56 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_spheresphere)
BOOST_CHECK_FALSE(res);
}
bool compareContactPoints(const Vec3f& c1,const Vec3f& c2)
{
return c1[2] < c2[2];
} // Ascending order
void testBoxBoxContactPoints(const Matrix3f& R)
{
Box s1(100, 100, 100);
Box s2(10, 20, 30);
// Vertices of s2
std::vector<Vec3f> vertices(8);
vertices[0].setValue( 1, 1, 1);
vertices[1].setValue( 1, 1, -1);
vertices[2].setValue( 1, -1, 1);
vertices[3].setValue( 1, -1, -1);
vertices[4].setValue(-1, 1, 1);
vertices[5].setValue(-1, 1, -1);
vertices[6].setValue(-1, -1, 1);
vertices[7].setValue(-1, -1, -1);
for (int i = 0; i < 8; ++i)
{
vertices[i][0] *= 0.5 * s2.side[0];
vertices[i][1] *= 0.5 * s2.side[1];
vertices[i][2] *= 0.5 * s2.side[2];
}
Transform3f tf1 = Transform3f(Vec3f(0, 0, -50));
Transform3f tf2 = Transform3f(R);
Vec3f normal;
Vec3f point;
double penetration;
// Make sure the two boxes are colliding
bool res = solver1.shapeIntersect(s1, tf1, s2, tf2, &point, &penetration, &normal);
BOOST_CHECK(res);
// Compute global vertices
for (int i = 0; i < 8; ++i)
vertices[i] = tf2.transform(vertices[i]);
// Sort the vertices so that the lowest vertex along z-axis comes first
std::sort(vertices.begin(), vertices.end(), compareContactPoints);
// The lowest vertex along z-axis should be the contact point
BOOST_CHECK(vertices[0].equal(point));
}
BOOST_AUTO_TEST_CASE(shapeIntersection_boxbox)
{
Box s1(20, 40, 50);
......@@ -250,6 +300,14 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_boxbox)
result.clear();
res = (collide(&s1, transform, &s2, transform * Transform3f(q), request, result) > 0);
BOOST_CHECK(res);
FCL_UINT32 numTests = 1e+2;
for (FCL_UINT32 i = 0; i < numTests; ++i)
{
Transform3f tf;
generateRandomTransform(extents, tf);
testBoxBoxContactPoints(tf.getRotation());
}
}
BOOST_AUTO_TEST_CASE(shapeIntersection_spherebox)
......
Supports Markdown
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