diff --git a/src/narrowphase/narrowphase.cpp b/src/narrowphase/narrowphase.cpp
index bcd28b638cba5b733d9292c8bc6e7926fb6b758d..0fb235b9988492064a466c95bbedd7ae3d2b07cd 100644
--- a/src/narrowphase/narrowphase.cpp
+++ b/src/narrowphase/narrowphase.cpp
@@ -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;
diff --git a/test/test_fcl_geometric_shapes.cpp b/test/test_fcl_geometric_shapes.cpp
index 48f6545c692701747f5c9ca7e52e1337c4e94c5e..f3f0d68971c648def6ed41d69772aa84780c4bf2 100644
--- a/test/test_fcl_geometric_shapes.cpp
+++ b/test/test_fcl_geometric_shapes.cpp
@@ -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);