Commit 00694bc9 authored by Jeongseok Lee's avatar Jeongseok Lee
Browse files

Merge pull request #63 from dartsim/fix_boxbox_collision

Fix box-box intersection algorithm with test
parents d166d216 366a9154
......@@ -951,6 +951,7 @@ int boxBox2(const Vec3f& side1, const Matrix3f& R1, const Vec3f& T1,
// the normal should be flipped.
int best_col_id = -1;
const Matrix3f* normalR = 0;
FCL_REAL tmp = 0;
s = - std::numeric_limits<FCL_REAL>::max();
......@@ -965,6 +966,7 @@ int boxBox2(const Vec3f& side1, const Matrix3f& R1, const Vec3f& T1,
{
s = s2;
best_col_id = 0;
normalR = &R1;
invert_normal = (tmp < 0);
code = 1;
}
......@@ -976,6 +978,7 @@ int boxBox2(const Vec3f& side1, const Matrix3f& R1, const Vec3f& T1,
{
s = s2;
best_col_id = 1;
normalR = &R1;
invert_normal = (tmp < 0);
code = 2;
}
......@@ -987,6 +990,7 @@ int boxBox2(const Vec3f& side1, const Matrix3f& R1, const Vec3f& T1,
{
s = s2;
best_col_id = 2;
normalR = &R1;
invert_normal = (tmp < 0);
code = 3;
}
......@@ -999,6 +1003,8 @@ int boxBox2(const Vec3f& side1, const Matrix3f& R1, const Vec3f& T1,
{
s = s2;
best_col_id = 0;
normalR = &R2;
invert_normal = (tmp < 0);
code = 4;
}
......@@ -1009,6 +1015,8 @@ int boxBox2(const Vec3f& side1, const Matrix3f& R1, const Vec3f& T1,
{
s = s2;
best_col_id = 1;
normalR = &R2;
invert_normal = (tmp < 0);
code = 5;
}
......@@ -1019,6 +1027,8 @@ int boxBox2(const Vec3f& side1, const Matrix3f& R1, const Vec3f& T1,
{
s = s2;
best_col_id = 2;
normalR = &R2;
invert_normal = (tmp < 0);
code = 6;
}
......@@ -1032,7 +1042,7 @@ int boxBox2(const Vec3f& side1, const Matrix3f& R1, const Vec3f& T1,
// separating axis = u1 x (v1,v2,v3)
tmp = pp[2] * R(1, 0) - pp[1] * R(2, 0);
s2 = std::abs(tmp) - (A[1] * Q(2, 0) + A[2] * Q(1, 0) + B[1] * Q(0, 2) + B[2] * Q(0, 1));
if(s2 > eps) { *return_code = 0; return 0; }
if(s2 > 0) { *return_code = 0; return 0; }
n = Vec3f(0, -R(2, 0), R(1, 0));
l = n.length();
if(l > eps)
......@@ -1041,7 +1051,7 @@ int boxBox2(const Vec3f& side1, const Matrix3f& R1, const Vec3f& T1,
if(s2 * fudge_factor > s)
{
s = s2;
best_col_id = 0;
best_col_id = -1;
normalC = n / l;
invert_normal = (tmp < 0);
code = 7;
......@@ -1050,7 +1060,7 @@ int boxBox2(const Vec3f& side1, const Matrix3f& R1, const Vec3f& T1,
tmp = pp[2] * R(1, 1) - pp[1] * R(2, 1);
s2 = std::abs(tmp) - (A[1] * Q(2, 1) + A[2] * Q(1, 1) + B[0] * Q(0, 2) + B[2] * Q(0, 0));
if(s2 > eps) { *return_code = 0; return 0; }
if(s2 > 0) { *return_code = 0; return 0; }
n = Vec3f(0, -R(2, 1), R(1, 1));
l = n.length();
if(l > eps)
......@@ -1059,7 +1069,7 @@ int boxBox2(const Vec3f& side1, const Matrix3f& R1, const Vec3f& T1,
if(s2 * fudge_factor > s)
{
s = s2;
best_col_id = 0;
best_col_id = -1;
normalC = n / l;
invert_normal = (tmp < 0);
code = 8;
......@@ -1068,7 +1078,7 @@ int boxBox2(const Vec3f& side1, const Matrix3f& R1, const Vec3f& T1,
tmp = pp[2] * R(1, 2) - pp[1] * R(2, 2);
s2 = std::abs(tmp) - (A[1] * Q(2, 2) + A[2] * Q(1, 2) + B[0] * Q(0, 1) + B[1] * Q(0, 0));
if(s2 > eps) { *return_code = 0; return 0; }
if(s2 > 0) { *return_code = 0; return 0; }
n = Vec3f(0, -R(2, 2), R(1, 2));
l = n.length();
if(l > eps)
......@@ -1077,7 +1087,7 @@ int boxBox2(const Vec3f& side1, const Matrix3f& R1, const Vec3f& T1,
if(s2 * fudge_factor > s)
{
s = s2;
best_col_id = 0;
best_col_id = -1;
normalC = n / l;
invert_normal = (tmp < 0);
code = 9;
......@@ -1087,7 +1097,7 @@ int boxBox2(const Vec3f& side1, const Matrix3f& R1, const Vec3f& T1,
// separating axis = u2 x (v1,v2,v3)
tmp = pp[0] * R(2, 0) - pp[2] * R(0, 0);
s2 = std::abs(tmp) - (A[0] * Q(2, 0) + A[2] * Q(0, 0) + B[1] * Q(1, 2) + B[2] * Q(1, 1));
if(s2 > eps) { *return_code = 0; return 0; }
if(s2 > 0) { *return_code = 0; return 0; }
n = Vec3f(R(2, 0), 0, -R(0, 0));
l = n.length();
if(l > eps)
......@@ -1096,7 +1106,7 @@ int boxBox2(const Vec3f& side1, const Matrix3f& R1, const Vec3f& T1,
if(s2 * fudge_factor > s)
{
s = s2;
best_col_id = 0;
best_col_id = -1;
normalC = n / l;
invert_normal = (tmp < 0);
code = 10;
......@@ -1105,7 +1115,7 @@ int boxBox2(const Vec3f& side1, const Matrix3f& R1, const Vec3f& T1,
tmp = pp[0] * R(2, 1) - pp[2] * R(0, 1);
s2 = std::abs(tmp) - (A[0] * Q(2, 1) + A[2] * Q(0, 1) + B[0] * Q(1, 2) + B[2] * Q(1, 0));
if(s2 > eps) { *return_code = 0; return 0; }
if(s2 > 0) { *return_code = 0; return 0; }
n = Vec3f(R(2, 1), 0, -R(0, 1));
l = n.length();
if(l > eps)
......@@ -1114,7 +1124,7 @@ int boxBox2(const Vec3f& side1, const Matrix3f& R1, const Vec3f& T1,
if(s2 * fudge_factor > s)
{
s = s2;
best_col_id = 0;
best_col_id = -1;
normalC = n / l;
invert_normal = (tmp < 0);
code = 11;
......@@ -1123,7 +1133,7 @@ int boxBox2(const Vec3f& side1, const Matrix3f& R1, const Vec3f& T1,
tmp = pp[0] * R(2, 2) - pp[2] * R(0, 2);
s2 = std::abs(tmp) - (A[0] * Q(2, 2) + A[2] * Q(0, 2) + B[0] * Q(1, 1) + B[1] * Q(1, 0));
if(s2 > eps) { *return_code = 0; return 0; }
if(s2 > 0) { *return_code = 0; return 0; }
n = Vec3f(R(2, 2), 0, -R(0, 2));
l = n.length();
if(l > eps)
......@@ -1132,7 +1142,7 @@ int boxBox2(const Vec3f& side1, const Matrix3f& R1, const Vec3f& T1,
if(s2 * fudge_factor > s)
{
s = s2;
best_col_id = 0;
best_col_id = -1;
normalC = n / l;
invert_normal = (tmp < 0);
code = 12;
......@@ -1142,7 +1152,7 @@ int boxBox2(const Vec3f& side1, const Matrix3f& R1, const Vec3f& T1,
// separating axis = u3 x (v1,v2,v3)
tmp = pp[1] * R(0, 0) - pp[0] * R(1, 0);
s2 = std::abs(tmp) - (A[0] * Q(1, 0) + A[1] * Q(0, 0) + B[1] * Q(2, 2) + B[2] * Q(2, 1));
if(s2 > eps) { *return_code = 0; return 0; }
if(s2 > 0) { *return_code = 0; return 0; }
n = Vec3f(-R(1, 0), R(0, 0), 0);
l = n.length();
if(l > eps)
......@@ -1151,7 +1161,7 @@ int boxBox2(const Vec3f& side1, const Matrix3f& R1, const Vec3f& T1,
if(s2 * fudge_factor > s)
{
s = s2;
best_col_id = 0;
best_col_id = -1;
normalC = n / l;
invert_normal = (tmp < 0);
code = 13;
......@@ -1160,7 +1170,7 @@ int boxBox2(const Vec3f& side1, const Matrix3f& R1, const Vec3f& T1,
tmp = pp[1] * R(0, 1) - pp[0] * R(1, 1);
s2 = std::abs(tmp) - (A[0] * Q(1, 1) + A[1] * Q(0, 1) + B[0] * Q(2, 2) + B[2] * Q(2, 0));
if(s2 > eps) { *return_code = 0; return 0; }
if(s2 > 0) { *return_code = 0; return 0; }
n = Vec3f(-R(1, 1), R(0, 1), 0);
l = n.length();
if(l > eps)
......@@ -1169,7 +1179,7 @@ int boxBox2(const Vec3f& side1, const Matrix3f& R1, const Vec3f& T1,
if(s2 * fudge_factor > s)
{
s = s2;
best_col_id = 0;
best_col_id = -1;
normalC = n / l;
invert_normal = (tmp < 0);
code = 14;
......@@ -1178,7 +1188,7 @@ int boxBox2(const Vec3f& side1, const Matrix3f& R1, const Vec3f& T1,
tmp = pp[1] * R(0, 2) - pp[0] * R(1, 2);
s2 = std::abs(tmp) - (A[0] * Q(1, 2) + A[1] * Q(0, 2) + B[0] * Q(2, 1) + B[1] * Q(2, 0));
if(s2 > eps) { *return_code = 0; return 0; }
if(s2 > 0) { *return_code = 0; return 0; }
n = Vec3f(-R(1, 2), R(0, 2), 0);
l = n.length();
if(l > eps)
......@@ -1187,7 +1197,7 @@ int boxBox2(const Vec3f& side1, const Matrix3f& R1, const Vec3f& T1,
if(s2 * fudge_factor > s)
{
s = s2;
best_col_id = 0;
best_col_id = -1;
normalC = n / l;
invert_normal = (tmp < 0);
code = 15;
......@@ -1201,14 +1211,14 @@ int boxBox2(const Vec3f& side1, const Matrix3f& R1, const Vec3f& T1,
// if we get to this point, the boxes interpenetrate. compute the normal
// in global coordinates.
if(best_col_id != -1)
normal = R.getColumn(best_col_id);
normal = normalR->getColumn(best_col_id);
else
normal = R1 * normalC;
if(invert_normal)
normal.negate();
*depth = -s;
*depth = -s; // s is negative when the boxes are in collision
// compute contact point(s)
......@@ -1226,11 +1236,11 @@ int boxBox2(const Vec3f& side1, const Matrix3f& R1, const Vec3f& T1,
}
// find a point pb on the intersecting edge of box 2
Vec3f pb;
pb = T2;
Vec3f pb(T2);
for(int j = 0; j < 3; ++j)
{
sign = (R2.transposeDot(j, normal) > 0) ? 1 : -1;
sign = (R2.transposeDot(j, normal) > 0) ? -1 : 1;
pb += R2.getColumn(j) * (B[j] * sign);
}
......@@ -1243,8 +1253,9 @@ int boxBox2(const Vec3f& side1, const Matrix3f& R1, const Vec3f& T1,
pb += ub * beta;
Vec3f pointInWorld((pa + pb) * 0.5);
contacts.push_back(ContactPoint(-normal, pointInWorld, -*depth));
// Vec3f pointInWorld((pa + pb) * 0.5);
// contacts.push_back(ContactPoint(-normal, pointInWorld, -*depth));
contacts.push_back(ContactPoint(-normal,pb,-*depth));
*return_code = code;
return 1;
......@@ -1462,7 +1473,7 @@ int boxBox2(const Vec3f& side1, const Matrix3f& R1, const Vec3f& T1,
for(int j = 0; j < maxc; ++j)
{
Vec3f posInWorld = points[iret[j] * 3] + (*pa);
Vec3f posInWorld = points[iret[j]] + (*pa);
if(code < 4)
contacts.push_back(ContactPoint(-normal, posInWorld, -dep[iret[j]]));
else
......@@ -1475,7 +1486,7 @@ int boxBox2(const Vec3f& side1, const Matrix3f& R1, const Vec3f& T1,
return cnum;
}
bool compareContactPoints(const ContactPoint& c1,const ContactPoint& c2) { return c1.depth < c2.depth; } // Accending order, assuming penetration depth is a negative number!
bool boxBoxIntersect(const Box& s1, const Transform3f& tf1,
const Box& s2, const Transform3f& tf2,
......@@ -1495,15 +1506,12 @@ bool boxBoxIntersect(const Box& s1, const Transform3f& tf1,
if(contact_points)
{
Vec3f contact_point;
for(size_t i = 0; i < contacts.size(); ++i)
if(contacts.size() > 0)
{
contact_point += contacts[i].point;
std::sort(contacts.begin(), contacts.end(), compareContactPoints);
*contact_points = contacts[0].point;
if(penetration_depth_) *penetration_depth_ = -contacts[0].depth;
}
contact_point = contact_point / (FCL_REAL)contacts.size();
*contact_points = contact_point;
}
return return_code != 0;
......
......@@ -360,6 +360,56 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_spheresphere)
testShapeInersection(s1, tf1, s2, tf2, GST_LIBCCD, false);
}
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);
......@@ -387,7 +437,7 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_boxbox)
tf1 = transform;
tf2 = transform;
// TODO: Need convention for normal when the centers of two objects are at same position. The current result is (1, 0, 0).
normal.setValue(1, 0, 0);
normal = transform.getRotation() * Vec3f(1, 0, 0);
testShapeInersection(s1, tf1, s2, tf2, GST_LIBCCD, true, NULL, NULL, &normal);
tf1 = Transform3f();
......@@ -401,13 +451,21 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_boxbox)
tf1 = Transform3f();
tf2 = Transform3f(q);
normal = Transform3f(q).getRotation() * Vec3f(1, 0, 0);
normal.setValue(1, 0, 0);
testShapeInersection(s1, tf1, s2, tf2, GST_LIBCCD, true, NULL, NULL, &normal);
tf1 = transform;
tf2 = transform * Transform3f(q);
normal = Transform3f(q).getRotation() * Vec3f(1, 0, 0);
normal = transform.getRotation() * Vec3f(1, 0, 0);
testShapeInersection(s1, tf1, s2, tf2, GST_LIBCCD, true, NULL, NULL, &normal);
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)
......@@ -433,9 +491,8 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_spherebox)
tf1 = transform;
tf2 = transform;
// TODO: Need convention for normal when the centers of two objects are at same position. The current result is (-0.9985590945508502, 0.02998909000838618, -0.04450156368325561).
normal.setValue(-0.9985590945508502, 0.02998909000838618, -0.04450156368325561);
testShapeInersection(s1, tf1, s2, tf2, GST_LIBCCD, true, NULL, NULL, &normal);
// TODO: Need convention for normal when the centers of two objects are at same position.
testShapeInersection(s1, tf1, s2, tf2, GST_LIBCCD, true, NULL, NULL, NULL);
tf1 = Transform3f();
tf2 = Transform3f(Vec3f(22.5, 0, 0));
......@@ -2751,7 +2808,7 @@ BOOST_AUTO_TEST_CASE(shapeIntersectionGJK_boxbox)
tf1 = transform;
tf2 = transform;
// TODO: Need convention for normal when the centers of two objects are at same position. The current result is (1, 0, 0).
normal.setValue(1, 0, 0);
normal = transform.getRotation() * Vec3f(1, 0, 0);
testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, &normal);
tf1 = Transform3f();
......@@ -2765,12 +2822,12 @@ BOOST_AUTO_TEST_CASE(shapeIntersectionGJK_boxbox)
tf1 = Transform3f();
tf2 = Transform3f(q);
normal = Transform3f(q).getRotation() * Vec3f(1, 0, 0);
normal.setValue(1, 0, 0);
testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, &normal);
tf1 = transform;
tf2 = transform * Transform3f(q);
normal = Transform3f(q).getRotation() * Vec3f(1, 0, 0);
normal = transform.getRotation() * Vec3f(1, 0, 0);
testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, &normal);
}
......@@ -3102,7 +3159,7 @@ BOOST_AUTO_TEST_CASE(shapeIntersectionGJK_halfspacetriangle)
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);
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);
......@@ -3143,7 +3200,7 @@ BOOST_AUTO_TEST_CASE(shapeIntersectionGJK_planetriangle)
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);
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);
......
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