diff --git a/include/fcl/narrowphase/detail/primitive_shape_algorithm/box_box-inl.h b/include/fcl/narrowphase/detail/primitive_shape_algorithm/box_box-inl.h old mode 100755 new mode 100644 index 7a1057cae..495482726 --- a/include/fcl/narrowphase/detail/primitive_shape_algorithm/box_box-inl.h +++ b/include/fcl/narrowphase/detail/primitive_shape_algorithm/box_box-inl.h @@ -258,6 +258,9 @@ int boxBox2( int maxc, std::vector>& contacts) { + // NOTE: This fudge factor is used to prefer face-feature contact over + // edge-edge. It is unclear why edge-edge contact receives this 5% penalty. + // Someone should document this. const S fudge_factor = S(1.05); Vector3 normalC; S s, s2, l; @@ -274,7 +277,6 @@ int boxBox2( Matrix3 R = R1.transpose() * R2; Matrix3 Q = R.cwiseAbs(); - // for all 15 possible separating axes: // * see if the axis separates the boxes. if so, return 0. // * find the depth of the penetration along the separating axis (s2) @@ -368,11 +370,34 @@ int boxBox2( } - S fudge2(1.0e-6); - Q.array() += fudge2; + // This is used to detect zero-length axes which arise from taking the cross + // product of parallel edges. + S eps = std::numeric_limits::epsilon(); + + // We are performing the mathematical test: 0 > 0 (which should always be + // false). However, zero can sometimes be 1e-16 or 1.5e-16. Thus, + // mathematically we would interpret a scenario as 0 > 0 but end up with + // 1e5e-16 > 1e-16. The former evaluates to false, the latter evaluates to + // true; a clear falsehood. + // (See http://gamma.cs.unc.edu/users/gottschalk/main.pdf, page 83). + // + // Gottschalk simply offers the value 1e-6 without any justification. + // For double-precision values, that is quite a large epsilon; we can do + // better. The idea is that this fictional zero can be scaled by the boxes' + // dimensions and summed up four times. To account for that zero, we need to + // make sure our epsilon is large enough to account for the *maximum* scale + // factor and the sum. We pad Q by that amount so that the error will *not* + // be the largest term. + { + using std::max; + // For small boxes (dimensions all less than 1), limit the scale factor to + // be no smaller than 10 * eps. This assumes all dimensions are strictly + // non-negative. + S scale_factor = max(max(A.maxCoeff(), B.maxCoeff()), 1.0) * 10 * eps; + Q.array() += scale_factor; + } Vector3 n; - S eps = std::numeric_limits::epsilon(); // separating axis = u1 x (v1,v2,v3) tmp = pp[2] * R(1, 0) - pp[1] * R(2, 0); @@ -539,8 +564,6 @@ int boxBox2( } } - - if (!code) { *return_code = code; return 0; } // if we get to this point, the boxes interpenetrate. compute the normal @@ -587,10 +610,8 @@ int boxBox2( pa += ua * alpha; pb += ub * beta; - - // Vector3 pointInWorld((pa + pb) * 0.5); - // contacts.push_back(ContactPoint(-normal, pointInWorld, -*depth)); - contacts.emplace_back(normal, pb, -*depth); + Vector3 pointInWorld((pa + pb) * 0.5); + contacts.push_back(ContactPoint(normal, pointInWorld, *depth)); *return_code = code; return 1; @@ -768,31 +789,17 @@ int boxBox2( if(maxc > cnum) maxc = cnum; if(maxc < 1) maxc = 1; - if(cnum <= maxc) - { - if(code<4) - { - // we have less contacts than we need, so we use them all - for(int j = 0; j < cnum; ++j) - { - Vector3 pointInWorld = points[j] + (*pa); - contacts.emplace_back(normal, pointInWorld, -dep[j]); - } - } - else - { - // we have less contacts than we need, so we use them all - for(int j = 0; j < cnum; ++j) - { - Vector3 pointInWorld = points[j] + (*pa) - normal * dep[j]; - contacts.emplace_back(normal, pointInWorld, -dep[j]); - } - } - } - else - { - // we have more contacts than are wanted, some of them must be culled. - // find the deepest point, it is always the first contact. + // The determination of these contact computations are tested in: + // test_fcl_box_box.cpp. + // The case where cnum <= maxc is tested by the test: + // test_collision_box_box_all_contacts + // The case where cnum > maxc is tested by the test: + // test_collision_box_box_cull_contacts + // + // Each of those tests is exercised twice: once when code < 4 and once when + // 4 <= code < 7. + int iret[] = {0, 1, 2, 3, 4, 5, 6, 7}; + if (cnum > maxc) { int i1 = 0; S maxdepth = dep[0]; for(int i = 1; i < cnum; ++i) @@ -804,18 +811,24 @@ int boxBox2( } } - int iret[8]; cullPoints2(cnum, ret, maxc, i1, iret); + cnum = maxc; + } - for(int j = 0; j < maxc; ++j) + if (code < 4) { + for(int j = 0; j < cnum; ++j) { - Vector3 posInWorld = points[iret[j]] + (*pa); - if(code < 4) - contacts.emplace_back(normal, posInWorld, -dep[iret[j]]); - else - contacts.emplace_back(normal, posInWorld - normal * dep[iret[j]], -dep[iret[j]]); + int i = iret[j]; + Vector3 pointInWorld = points[i] + (*pa) + normal * (dep[i] / 2); + contacts.emplace_back(normal, pointInWorld, dep[i]); + } + } else { + for(int j = 0; j < cnum; ++j) + { + int i = iret[j]; + Vector3 pointInWorld = points[i] + (*pa) - normal * (dep[i] / 2); + contacts.emplace_back(normal, pointInWorld, dep[i]); } - cnum = maxc; } *return_code = code; @@ -835,559 +848,8 @@ int boxBox2( int maxc, std::vector>& contacts) { - const S fudge_factor = S(1.05); - Vector3 normalC; - - const Vector3 p = tf2.translation() - tf1.translation(); // get vector from centers of box 1 to box 2, relative to box 1 - const Vector3 pp = tf1.linear().transpose() * p; // get pp = p relative to body 1 - - // get side lengths / 2 - const Vector3 A = side1 * 0.5; - const Vector3 B = side2 * 0.5; - - // Rij is R1'*R2, i.e. the relative rotation between R1 and R2 - const Matrix3 R = tf1.linear().transpose() * tf2.linear(); - Matrix3 Q = R.cwiseAbs(); - - // for all 15 possible separating axes: - // * see if the axis separates the boxes. if so, return 0. - // * find the depth of the penetration along the separating axis (s2) - // * if this is the largest depth so far, record it. - // the normal vector will be set to the separating axis with the smallest - // depth. note: normalR is set to point to a column of R1 or R2 if that is - // the smallest depth normal so far. otherwise normalR is 0 and normalC is - // set to a vector relative to body 1. invert_normal is 1 if the sign of - // the normal should be flipped. - - int best_col_id = -1; - const Transform3* normalT = nullptr; - - S s = - std::numeric_limits::max(); - int invert_normal = 0; - int code = 0; - - // separating axis = u1, u2, u3 - S tmp = pp[0]; - S s2 = std::abs(tmp) - (Q.row(0).dot(B) + A[0]); - if(s2 > 0) { *return_code = 0; return 0; } - if(s2 > s) - { - s = s2; - best_col_id = 0; - normalT = &tf1; - invert_normal = (tmp < 0); - code = 1; - } - - tmp = pp[1]; - s2 = std::abs(tmp) - (Q.row(1).dot(B) + A[1]); - if(s2 > 0) { *return_code = 0; return 0; } - if(s2 > s) - { - s = s2; - best_col_id = 1; - normalT = &tf1; - invert_normal = (tmp < 0); - code = 2; - } - - tmp = pp[2]; - s2 = std::abs(tmp) - (Q.row(2).dot(B) + A[2]); - if(s2 > 0) { *return_code = 0; return 0; } - if(s2 > s) - { - s = s2; - best_col_id = 2; - normalT = &tf1; - invert_normal = (tmp < 0); - code = 3; - } - - // separating axis = v1, v2, v3 - tmp = tf2.linear().col(0).dot(p); - s2 = std::abs(tmp) - (Q.col(0).dot(A) + B[0]); - if(s2 > 0) { *return_code = 0; return 0; } - if(s2 > s) - { - s = s2; - best_col_id = 0; - normalT = &tf2; - invert_normal = (tmp < 0); - code = 4; - } - - tmp = tf2.linear().col(1).dot(p); - s2 = std::abs(tmp) - (Q.col(1).dot(A) + B[1]); - if(s2 > 0) { *return_code = 0; return 0; } - if(s2 > s) - { - s = s2; - best_col_id = 1; - normalT = &tf2; - invert_normal = (tmp < 0); - code = 5; - } - - tmp = tf2.linear().col(2).dot(p); - s2 = std::abs(tmp) - (Q.col(2).dot(A) + B[2]); - if(s2 > 0) { *return_code = 0; return 0; } - if(s2 > s) - { - s = s2; - best_col_id = 2; - normalT = &tf2; - invert_normal = (tmp < 0); - code = 6; - } - - - S fudge2(1.0e-6); - Q.array() += fudge2; - - Vector3 n; - S eps = std::numeric_limits::epsilon(); - - // 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 > 0) { *return_code = 0; return 0; } - n = Vector3(0, -R(2, 0), R(1, 0)); - S l = n.norm(); - if(l > eps) - { - s2 /= l; - if(s2 * fudge_factor > s) - { - s = s2; - best_col_id = -1; - normalC = n / l; - invert_normal = (tmp < 0); - code = 7; - } - } - - 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 > 0) { *return_code = 0; return 0; } - n = Vector3(0, -R(2, 1), R(1, 1)); - l = n.norm(); - if(l > eps) - { - s2 /= l; - if(s2 * fudge_factor > s) - { - s = s2; - best_col_id = -1; - normalC = n / l; - invert_normal = (tmp < 0); - code = 8; - } - } - - 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 > 0) { *return_code = 0; return 0; } - n = Vector3(0, -R(2, 2), R(1, 2)); - l = n.norm(); - if(l > eps) - { - s2 /= l; - if(s2 * fudge_factor > s) - { - s = s2; - best_col_id = -1; - normalC = n / l; - invert_normal = (tmp < 0); - code = 9; - } - } - - // 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 > 0) { *return_code = 0; return 0; } - n = Vector3(R(2, 0), 0, -R(0, 0)); - l = n.norm(); - if(l > eps) - { - s2 /= l; - if(s2 * fudge_factor > s) - { - s = s2; - best_col_id = -1; - normalC = n / l; - invert_normal = (tmp < 0); - code = 10; - } - } - - 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 > 0) { *return_code = 0; return 0; } - n = Vector3(R(2, 1), 0, -R(0, 1)); - l = n.norm(); - if(l > eps) - { - s2 /= l; - if(s2 * fudge_factor > s) - { - s = s2; - best_col_id = -1; - normalC = n / l; - invert_normal = (tmp < 0); - code = 11; - } - } - - 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 > 0) { *return_code = 0; return 0; } - n = Vector3(R(2, 2), 0, -R(0, 2)); - l = n.norm(); - if(l > eps) - { - s2 /= l; - if(s2 * fudge_factor > s) - { - s = s2; - best_col_id = -1; - normalC = n / l; - invert_normal = (tmp < 0); - code = 12; - } - } - - // 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 > 0) { *return_code = 0; return 0; } - n = Vector3(-R(1, 0), R(0, 0), 0); - l = n.norm(); - if(l > eps) - { - s2 /= l; - if(s2 * fudge_factor > s) - { - s = s2; - best_col_id = -1; - normalC = n / l; - invert_normal = (tmp < 0); - code = 13; - } - } - - 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 > 0) { *return_code = 0; return 0; } - n = Vector3(-R(1, 1), R(0, 1), 0); - l = n.norm(); - if(l > eps) - { - s2 /= l; - if(s2 * fudge_factor > s) - { - s = s2; - best_col_id = -1; - normalC = n / l; - invert_normal = (tmp < 0); - code = 14; - } - } - - 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 > 0) { *return_code = 0; return 0; } - n = Vector3(-R(1, 2), R(0, 2), 0); - l = n.norm(); - if(l > eps) - { - s2 /= l; - if(s2 * fudge_factor > s) - { - s = s2; - best_col_id = -1; - normalC = n / l; - invert_normal = (tmp < 0); - code = 15; - } - } - - - - if (!code) { *return_code = code; return 0; } - - // if we get to this point, the boxes interpenetrate. compute the normal - // in global coordinates. - if(best_col_id != -1) - normal = normalT->linear().col(best_col_id); - else - normal = tf1.linear() * normalC; - - if(invert_normal) - normal = -normal; - - *depth = -s; // s is negative when the boxes are in collision - - // compute contact point(s) - - if(code > 6) - { - // an edge from box 1 touches an edge from box 2. - // find a point pa on the intersecting edge of box 1 - Vector3 pa(tf1.translation()); - S sign; - - for(int j = 0; j < 3; ++j) - { - sign = (tf1.linear().col(j).dot(normal) > 0) ? 1 : -1; - pa += tf1.linear().col(j) * (A[j] * sign); - } - - // find a point pb on the intersecting edge of box 2 - Vector3 pb(tf2.translation()); - - for(int j = 0; j < 3; ++j) - { - sign = (tf2.linear().col(j).dot(normal) > 0) ? -1 : 1; - pb += tf2.linear().col(j) * (B[j] * sign); - } - - S alpha, beta; - Vector3 ua(tf1.linear().col((code-7)/3)); - Vector3 ub(tf2.linear().col((code-7)%3)); - - lineClosestApproach(pa, ua, pb, ub, &alpha, &beta); - pa += ua * alpha; - pb += ub * beta; - - - // Vector3 pointInWorld((pa + pb) * 0.5); - // contacts.push_back(ContactPoint(-normal, pointInWorld, -*depth)); - contacts.emplace_back(normal, pb, -*depth); - *return_code = code; - - return 1; - } - - // okay, we have a face-something intersection (because the separating - // axis is perpendicular to a face). define face 'a' to be the reference - // face (i.e. the normal vector is perpendicular to this) and face 'b' to be - // the incident face (the closest face of the other box). - - const Transform3 *Ta, *Tb; - const Vector3 *Sa, *Sb; - - if(code <= 3) - { - Ta = &tf1; - Tb = &tf2; - Sa = &A; - Sb = &B; - } - else - { - Ta = &tf2; - Tb = &tf1; - Sa = &B; - Sb = &A; - } - - // nr = normal vector of reference face dotted with axes of incident box. - // anr = absolute values of nr. - Vector3 normal2, nr, anr; - if(code <= 3) - normal2 = normal; - else - normal2 = -normal; - - nr = Tb->linear().transpose() * normal2; - anr = nr.cwiseAbs(); - - // find the largest compontent of anr: this corresponds to the normal - // for the indident face. the other axis numbers of the indicent face - // are stored in a1,a2. - int lanr, a1, a2; - if(anr[1] > anr[0]) - { - if(anr[1] > anr[2]) - { - a1 = 0; - lanr = 1; - a2 = 2; - } - else - { - a1 = 0; - a2 = 1; - lanr = 2; - } - } - else - { - if(anr[0] > anr[2]) - { - lanr = 0; - a1 = 1; - a2 = 2; - } - else - { - a1 = 0; - a2 = 1; - lanr = 2; - } - } - - // compute center point of incident face, in reference-face coordinates - Vector3 center; - if(nr[lanr] < 0) - center = Tb->translation() - Ta->translation() + Tb->linear().col(lanr) * ((*Sb)[lanr]); - else - center = Tb->translation() - Ta->translation() - Tb->linear().col(lanr) * ((*Sb)[lanr]); - - // find the normal and non-normal axis numbers of the reference box - int codeN, code1, code2; - if(code <= 3) - codeN = code-1; - else codeN = code-4; - - if(codeN == 0) - { - code1 = 1; - code2 = 2; - } - else if(codeN == 1) - { - code1 = 0; - code2 = 2; - } - else - { - code1 = 0; - code2 = 1; - } - - // find the four corners of the incident face, in reference-face coordinates - S quad[8]; // 2D coordinate of incident face (x,y pairs) - S c1, c2, m11, m12, m21, m22; - c1 = Ta->linear().col(code1).dot(center); - c2 = Ta->linear().col(code2).dot(center); - // optimize this? - we have already computed this data above, but it is not - // stored in an easy-to-index format. for now it's quicker just to recompute - // the four dot products. - Vector3 tempRac = Ta->linear().col(code1); - m11 = Tb->linear().col(a1).dot(tempRac); - m12 = Tb->linear().col(a2).dot(tempRac); - tempRac = Ta->linear().col(code2); - m21 = Tb->linear().col(a1).dot(tempRac); - m22 = Tb->linear().col(a2).dot(tempRac); - - S k1 = m11 * (*Sb)[a1]; - S k2 = m21 * (*Sb)[a1]; - S k3 = m12 * (*Sb)[a2]; - S k4 = m22 * (*Sb)[a2]; - quad[0] = c1 - k1 - k3; - quad[1] = c2 - k2 - k4; - quad[2] = c1 - k1 + k3; - quad[3] = c2 - k2 + k4; - quad[4] = c1 + k1 + k3; - quad[5] = c2 + k2 + k4; - quad[6] = c1 + k1 - k3; - quad[7] = c2 + k2 - k4; - - // find the size of the reference face - S rect[2]; - rect[0] = (*Sa)[code1]; - rect[1] = (*Sa)[code2]; - - // intersect the incident and reference faces - S ret[16]; - int n_intersect = intersectRectQuad2(rect, quad, ret); - if(n_intersect < 1) { *return_code = code; return 0; } // this should never happen - - // convert the intersection points into reference-face coordinates, - // and compute the contact position and depth for each point. only keep - // those points that have a positive (penetrating) depth. delete points in - // the 'ret' array as necessary so that 'point' and 'ret' correspond. - Vector3 points[8]; // penetrating contact points - S dep[8]; // depths for those points - S det1 = 1.f/(m11*m22 - m12*m21); - m11 *= det1; - m12 *= det1; - m21 *= det1; - m22 *= det1; - int cnum = 0; // number of penetrating contact points found - for(int j = 0; j < n_intersect; ++j) - { - S k1 = m22*(ret[j*2]-c1) - m12*(ret[j*2+1]-c2); - S k2 = -m21*(ret[j*2]-c1) + m11*(ret[j*2+1]-c2); - points[cnum] = center + Tb->linear().col(a1) * k1 + Tb->linear().col(a2) * k2; - dep[cnum] = (*Sa)[codeN] - normal2.dot(points[cnum]); - if(dep[cnum] >= 0) - { - ret[cnum*2] = ret[j*2]; - ret[cnum*2+1] = ret[j*2+1]; - cnum++; - } - } - if(cnum < 1) { *return_code = code; return 0; } // this should never happen - - // we can't generate more contacts than we actually have - if(maxc > cnum) maxc = cnum; - if(maxc < 1) maxc = 1; - - if(cnum <= maxc) - { - if(code<4) - { - // we have less contacts than we need, so we use them all - for(int j = 0; j < cnum; ++j) - { - Vector3 pointInWorld = points[j] + Ta->translation(); - contacts.emplace_back(normal, pointInWorld, -dep[j]); - } - } - else - { - // we have less contacts than we need, so we use them all - for(int j = 0; j < cnum; ++j) - { - Vector3 pointInWorld = points[j] + Ta->translation() - normal * dep[j]; - contacts.emplace_back(normal, pointInWorld, -dep[j]); - } - } - } - else - { - // we have more contacts than are wanted, some of them must be culled. - // find the deepest point, it is always the first contact. - int i1 = 0; - S maxdepth = dep[0]; - for(int i = 1; i < cnum; ++i) - { - if(dep[i] > maxdepth) - { - maxdepth = dep[i]; - i1 = i; - } - } - - int iret[8]; - cullPoints2(cnum, ret, maxc, i1, iret); - - for(int j = 0; j < maxc; ++j) - { - Vector3 posInWorld = points[iret[j]] + Ta->translation(); - if(code < 4) - contacts.emplace_back(normal, posInWorld, -dep[iret[j]]); - else - contacts.emplace_back(normal, posInWorld - normal * dep[iret[j]], -dep[iret[j]]); - } - cnum = maxc; - } - - *return_code = code; - return cnum; + return boxBox2(side1, tf1.linear(), tf1.translation(), side2, tf2.linear(), + tf2.translation(), normal, depth, return_code, maxc, contacts); } //============================================================================== diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt index bc0dc988b..3d0ac1cc0 100644 --- a/test/CMakeLists.txt +++ b/test/CMakeLists.txt @@ -38,6 +38,7 @@ target_link_libraries(test_fcl_utility fcl) # test file list set(tests test_fcl_auto_diff.cpp + test_fcl_box_box.cpp test_fcl_broadphase_collision_1.cpp test_fcl_broadphase_collision_2.cpp test_fcl_broadphase_distance.cpp diff --git a/test/test_fcl_box_box.cpp b/test/test_fcl_box_box.cpp new file mode 100644 index 000000000..e61e78af0 --- /dev/null +++ b/test/test_fcl_box_box.cpp @@ -0,0 +1,470 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2017. Toyota Research Institute + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of CNRS-LAAS and AIST nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/** @author Sean Curtis */ + +#include +#include +#include +#include +#include + +#include +#include + +#include "fcl/math/constants.h" +#include "fcl/narrowphase/collision.h" +#include "fcl/narrowphase/collision_object.h" + +using std::map; +using std::pair; +using std::string; +using std::vector; + +// Simple specification for defining a box collision object. Specifies the +// dimensions and pose of the box in some frame F (X_FB). For an explanation +// of the notation X_FB, see: +// http://drake.mit.edu/doxygen_cxx/group__multibody__spatial__pose.html +template struct BoxSpecification { + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + fcl::Vector3 size; + fcl::Transform3 X_FB; +}; + +// Class for executing and evaluating various box-box tests. +// The class is initialized with two box specifications (size and pose). +// The test performs a collision test between the two boxes in 12 permutations: +// One axis is the order (box 1 vs box 2 and box 2 vs box 1). +// The other axis is the orientation box 2. Given that box 2 must be a cube, it +// can be oriented in six different configurations and still produced the same +// answer. +// The 12 permutations are the two orderings crossed with the six orientations. +template +class BoxBoxTest { + public: + // Construct the test scenario with the given box specifications. Box 2 + // must be a *cube* (all sides equal). + BoxBoxTest(const BoxSpecification& box_spec_1, + const BoxSpecification& box_spec_2) + : box_spec_1_(box_spec_1), box_spec_2_(box_spec_2) { + using fcl::AngleAxis; + using fcl::Transform3; + using fcl::Vector3; + + // Confirm box 2 is a cube + EXPECT_EQ(box_spec_2.size(0), box_spec_2.size(1)); + EXPECT_EQ(box_spec_2.size(0), box_spec_2.size(2)); + + const S pi = fcl::constants::pi(); + + // Initialize isomorphic rotations of box 2. + iso_poses_["top"] = Transform3::Identity(); + iso_poses_["bottom"] = + Transform3{AngleAxis(pi, Vector3::UnitX())}; + iso_poses_["back"] = + Transform3{AngleAxis(pi / 2, Vector3::UnitX())}; + iso_poses_["front"] = + Transform3{AngleAxis(3 * pi / 2, Vector3::UnitX())}; + iso_poses_["left"] = + Transform3{AngleAxis(pi / 2, Vector3::UnitY())}; + iso_poses_["right"] = + Transform3{AngleAxis(3 * pi / 2, Vector3::UnitY())}; + } + + // Runs the 12 tests for the two specified boxes. + // + // @param solver_type The solver type to use for computing collision. + // @param test_tolerance The tolerance to which the collision contact + // results will be compared to the results. + // @param expected_normal The expected normal for the (1, 2) query order. + // @param expected_depth The expected penetration depth. + // @param contact_pos_test A function to evaluate the reported contact + // position for validity; this should be written to + // account for the possibility of the contact + // position lying on some manifold (e.g., an edge, or + // face). This function should invoke googletest + // EXPECT_* methods. + // @param origin_name A string which is appended to error message to + // more easily parse failures and which test failed. + void + RunTests(fcl::GJKSolverType solver_type, S test_tolerance, + const fcl::Vector3& expected_normal, S expected_depth, + std::function &, S, const std::string &)> + contact_pos_test, + const std::string& origin_name) { + fcl::Contact expected_contact; + expected_contact.penetration_depth = expected_depth; + + for (const auto& reorient_pair : iso_poses_) { + const std::string& top_face = reorient_pair.first; + const fcl::Transform3& pre_pose = reorient_pair.second; + + BoxSpecification box_2_posed{ + box_spec_2_.size, + box_spec_2_.X_FB * pre_pose + }; + + // Collide (1, 2) + expected_contact.normal = expected_normal; + RunSingleTest(box_spec_1_, + box_2_posed, + solver_type, + test_tolerance, + expected_contact, + contact_pos_test, + origin_name + " (1, 2) - " + top_face); + + // Collide (2, 1) + expected_contact.normal = -expected_normal; + RunSingleTest(box_2_posed, + box_spec_1_, + solver_type, + test_tolerance, + expected_contact, + contact_pos_test, + origin_name + " (2, 1) - " + top_face); + } + } + +private: +// Performs a collision test between two boxes and tests the *single* contact +// result against given expectations. +// +// @param box_spec_A A specification of the first box (treated as object +// 1 in the query). +// @param box_spec_B A specification of the second box (treated as object +// 2 in the query). +// @param solver_type The solver type to use for computing collision. +// @param test_tolerance The tolerance to which the collision contact results +// will be compared to the results. +// @param expected_contact The expected contact details (only penetration depth +// and normal are used). +// @param contact_pos_test A function to evaluate the reported contact position +// for validity; this should be written to account for +// the possibility of the contact position lying on +// some manifold (e.g., an edge, or face). This +// function should invoke googletest EXPECT_* methods. +// @param origin_name A string which is appended to error message to more +// easily parse failures and which test failed. + void RunSingleTest( + const BoxSpecification& box_spec_A, + const BoxSpecification& box_spec_B, fcl::GJKSolverType solver_type, + S test_tolerance, const fcl::Contact& expected_contact, + std::function&, S, const std::string&)> + contact_pos_test, + const std::string& origin_name) { + using CollisionGeometryPtr_t = std::shared_ptr>; + CollisionGeometryPtr_t box_geometry_A(new fcl::Box(box_spec_A.size)); + CollisionGeometryPtr_t box_geometry_B(new fcl::Box(box_spec_B.size)); + + fcl::CollisionObject box_A(box_geometry_A, box_spec_A.X_FB); + fcl::CollisionObject box_B(box_geometry_B, box_spec_B.X_FB); + + // Compute collision - single contact and enable contact. + fcl::CollisionRequest collisionRequest(1, true); + collisionRequest.gjk_solver_type = solver_type; + fcl::CollisionResult collisionResult; + fcl::collide(&box_A, &box_B, collisionRequest, collisionResult); + EXPECT_TRUE(collisionResult.isCollision()) << origin_name; + std::vector> contacts; + collisionResult.getContacts(contacts); + GTEST_ASSERT_EQ(contacts.size(), 1u) << origin_name; + + const fcl::Contact& contact = contacts[0]; + EXPECT_NEAR(expected_contact.penetration_depth, contact.penetration_depth, + test_tolerance) + << origin_name; + EXPECT_TRUE(expected_contact.normal.isApprox(contact.normal, + test_tolerance)) + << origin_name << ":\n\texpected: " + << expected_contact.normal.transpose() + << "\n\tcontact.normal: " << contact.normal.transpose(); + contact_pos_test(contact.pos, test_tolerance, origin_name); + } + + const BoxSpecification box_spec_1_; + const BoxSpecification box_spec_2_; + map, std::less, + Eigen::aligned_allocator>>> + iso_poses_; +}; + +// This test exercises the case of face-something contact. In the language of +// boxBox2() (in box_box-inl.h) it is for codes 1-6. +// +// More particularly it is designed to exercise the case where no contact points +// need be culled. It assumes that boxBox2() is invoked with a maximum contact +// count of 4. The collision produces a manifold that is a four-sided polygon. +// +// The test looks like this: +// +// z +// │ +// │ +// ╱│╲ +// ╱ │ ╲ Box1 +// ──┲━━╱━━O━━╲━━┱─── x +// ┃ ╲ │ ╱ ┃ +// ┃ ╲ │ ╱ ┃ +// ┃ ╲│╱ ┃ +// ┃ │ ┃ Box2 +// ┃ │ ┃ +// ┗━━━━━┿━━━━━┛ +// │ +// +// There are two boxes: +// Box 1: A cube with side length of 1, centered on the world origin (O) and +// rotated 45 degrees around the y-axis. +// Box 2: A cube with side length of 3, moved in the negative z-direction such +// that it's top face lies on the z = 0 plane. +// +// The penetration depth should be sqrt(2) / 2. +// The normal should be parallel with the z-axis. We test both the collision of +// 1 with 2 and 2 with 1. In those cases, the normal would be -z and +z, +// respectively. +// The contact position, should lie on an edge parallel with the y-axis at x = 0 +// and z = sqrt(2) / 4. +template +void test_collision_box_box_all_contacts(fcl::GJKSolverType solver_type, + S test_tolerance) +{ + const S pi = fcl::constants::pi(); + const S size_1 = 1; + BoxSpecification box_1{ + fcl::Vector3{size_1, size_1, size_1}, + fcl::Transform3{fcl::AngleAxis(pi / 4, fcl::Vector3::UnitY())}}; + + const S size_2 = 3; + BoxSpecification box_2{fcl::Vector3{size_2, size_2, size_2}, + fcl::Transform3{fcl::Translation3( + fcl::Vector3(0, 0, -size_2 / 2))}}; + + fcl::Vector3 expected_normal{0, 0, -1}; + S expected_depth = size_1 * sqrt(2) / 2; + + auto contact_pos_test = [size_1](const fcl::Vector3 &pos, S tolerance, + const std::string& origin_name) { + const double expected_pos_z = -size_1 * std::sqrt(2) / 4; + EXPECT_NEAR(expected_pos_z, pos(2), tolerance) << origin_name; + EXPECT_NEAR(0, pos(0), tolerance) << origin_name; + EXPECT_LE(pos(1), 0.5) << origin_name; + EXPECT_GE(pos(1), -0.5) << origin_name; + }; + + BoxBoxTest tests(box_1, box_2); + tests.RunTests(solver_type, test_tolerance, expected_normal, expected_depth, + contact_pos_test, "test_colliion_box_box_all_contacts"); +} + +GTEST_TEST(FCL_BOX_BOX, collision_box_box_all_contacts_ccd) +{ + test_collision_box_box_all_contacts(fcl::GJKSolverType::GST_LIBCCD, + 1e-14); +} + +GTEST_TEST(FCL_BOX_BOX, collision_box_box_all_contacts_indep) +{ + test_collision_box_box_all_contacts(fcl::GJKSolverType::GST_INDEP, + 1e-12); +} + +// This test exercises the case of face-something contact. In the language of +// boxBox2() (in box_box-inl.h) it is for codes 1-6. +// +// In contrast with the previous test (test_collision_box_box_all_contacts), +// the contact manifold is an eight-sided polygon and contacts will need to be +// culled. +// +// The test looks like this: +// +// Top view Side view +// +// +y +z +// │ 2 ┆ +// ╱│╲ __─────┐ Box1 +// ┏━━━╱━━━━━╲━━━┓ 1 ┌─────── ┆ │ +// ┣━╱━━━━━━━━━╲━┫ │ ┆ │ +// ╱ │ ╲ │ ┆ │ +// ___╱_┃ │ ┃_╲__ +x ┄┄┴┬┄┄┄┄┄┄─┼┄─┄─┄┴┬┄┄┄┄ +y +// ╲ ┃ │ ┃ ╱ │ ┆ │ +// ╲ │ ╱ ┏━┥ ┏━━━╈━━━┓ ┝━━┓ +// ┃ ╲ │ ╱ ┃ ┃ │ ┃ ┃ ┖──┘ ┃ +// ┗━━━●━━━━━●━━━┛ ┃ └───┚ ┃ ┃ +// ╲│╱ ┃ ┃ ┃ Box2 +// │ ┃ ┃ ┃ +// │ ┃ ┃ ┃ +// ┗━━━━━━━━━┻━━━━━━━━━┛ +// +// +// There are two boxes: +// Box 1: A cube with side length of 1, centered on the world origin and +// rotated θ = 22.5 degrees around the x-axis +// Box 2: A cube with side length of 1, rotated 45 degrees around the z-axis +// and centered on the point [0, 0, -0.75]. +// +// The penetration depth should be √2/2 * cos(π/4 - θ) - 0.25 ≊ 0.4032814. +// The normal should be parallel with the z-axis. We test both the collision of +// 1 with 2 and 2 with 1. In those cases, the normal would be -z and +z, +// respectively. +// The contact position, should lie alone the line segment illustrated +// in the top view (indicated by the two points '●') at z = -0.25 - depth / 2. +template +void test_collision_box_box_cull_contacts(fcl::GJKSolverType solver_type, + S test_tolerance) +{ + const S pi = fcl::constants::pi(); + const S size = 1; + const S tilt = pi / 8; + BoxSpecification box_1{ + fcl::Vector3{size, size, size}, + fcl::Transform3{fcl::AngleAxis(tilt, fcl::Vector3::UnitX())}}; + + BoxSpecification box_2{fcl::Vector3{size, size, size}, + fcl::Transform3{fcl::AngleAxis( + pi / 4, fcl::Vector3::UnitZ())}}; + box_2.X_FB.translation() << 0, 0, -0.75; + + fcl::Vector3 expected_normal{0, 0, -1}; + const S expected_depth{sqrt(2) / 2 * cos(pi / 4 - tilt) - 0.25}; + auto contact_pos_test = [expected_depth, tilt, pi]( + const fcl::Vector3 &pos, S tolerance, const std::string& origin_name) { + // Edge is parallel to the x-axis at + // z = expected_z = -0.25 - depth / 2 + // y = expected_y = -√2/2 * sin(π/4 - θ) + // x = lines in the range [-x_e, x_e] where + // x_e = √2/2 + expected_y + const S expected_z{-0.25 - expected_depth / 2}; + const S expected_y{-sqrt(2) / 2 * sin(pi / 4 - tilt)}; + const S expected_x{sqrt(2) / 2 + expected_y + + std::numeric_limits::epsilon()}; + EXPECT_NEAR(expected_z, pos(2), tolerance) << origin_name; + EXPECT_NEAR(expected_y, pos(1), tolerance) << origin_name; + EXPECT_GE(pos(0), -expected_x); + EXPECT_LE(pos(0), expected_x); + }; + + BoxBoxTest tests(box_1, box_2); + tests.RunTests(solver_type, test_tolerance, expected_normal, expected_depth, + contact_pos_test, "test_collision_box_box_cull_contacts"); +} + +GTEST_TEST(FCL_BOX_BOX, collision_box_box_cull_contacts_ccd) +{ + test_collision_box_box_cull_contacts(fcl::GJKSolverType::GST_LIBCCD, + 1e-14); +} + +GTEST_TEST(FCL_BOX_BOX, collision_box_box_cull_contacts_indep) +{ + test_collision_box_box_cull_contacts(fcl::GJKSolverType::GST_INDEP, + 1e-14); +} + +// This test exercises the case where the contact is between edges (rather than +// face-something as in previous tests). +// +// The test looks like this. +// +// ╱╲ +// ╱ ╲ Box1 +// +z ╱ 1 ╲ +// ╱╲ ╲ +// ┆ ╱ ╲ ╱ +// ┆╱ ╲ ╱ +// ┏━━━━━╲━━━━┓ ╲ ╱ +// ┃ ┆ ╲ ┃ ╱ +// ┃ ┆ ╲┃ ╱ +// ┄┄┄┄┄┃┄┄┄┄┼┄┄┄┄┄┃╲╱┄┄┄┄┄┄┄┄ +x +// ┃ ┆ ┃ +// ┃ ┆ ┃ Box2 +// ┗━━━━━━━━━━┛ +// ┆ +// +// There are two boxes: +// Box 1: A cube with side length of 1. It is rotated 45° around the world's +// z-axis and then -45° around the world's y-axis. Given a target +// penetration depth of 0.1, it's center is finally moved along the +// vector <√2/2, √2/2> a distance of `1 * √2 - 0.1`. +// Box 2: A cube with side length of 1. It is aligned with and centered on +// the world frame. +// +template +void test_collision_box_box_edge_contact(fcl::GJKSolverType solver_type, + S test_tolerance) { + const S pi = fcl::constants::pi(); + const S size = 1; + BoxSpecification box_1{ + fcl::Vector3{size, size, size}, + fcl::Transform3{fcl::AngleAxis(-pi / 4, fcl::Vector3::UnitY())} * + fcl::Transform3{fcl::AngleAxis(pi / 4, fcl::Vector3::UnitZ())}}; + const fcl::Vector3 dir{sqrt(2) / 2, 0, sqrt(2) / 2}; + const S expected_depth = 0.1; + box_1.X_FB.translation() = dir * (size * sqrt(2) - expected_depth); + + BoxSpecification box_2{fcl::Vector3{size, size, size}, + fcl::Transform3::Identity()}; + + auto contact_pos_test = [expected_depth, size, dir]( + const fcl::Vector3 &pos, S tolerance, const std::string& origin_name) { + // The contact point should unambiguously be a single point. + const S dist = size * sqrt(2) / 2 - expected_depth / 2; + const fcl::Vector3 expected_pos{dir * dist}; + EXPECT_TRUE(expected_pos.isApprox(pos, tolerance)) << origin_name + << "\n\texpected: " << expected_pos.transpose() + << "\n\tpos: " << pos.transpose(); + }; + + BoxBoxTest tests(box_1, box_2); + tests.RunTests(solver_type, test_tolerance, -dir, expected_depth, + contact_pos_test, "test_collision_box_box_edge_contact"); +} + +GTEST_TEST(FCL_BOX_BOX, collision_box_box_edge_contact_ccd) +{ + test_collision_box_box_edge_contact(fcl::GJKSolverType::GST_LIBCCD, + 1e-14); +} + +GTEST_TEST(FCL_BOX_BOX, collision_box_box_edge_contact_indep) +{ + test_collision_box_box_edge_contact(fcl::GJKSolverType::GST_INDEP, + 1e-14); +} + +//============================================================================== +int main(int argc, char* argv[]) +{ + ::testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/test/test_fcl_geometric_shapes.cpp b/test/test_fcl_geometric_shapes.cpp index a8663b7af..dde0a64e3 100755 --- a/test/test_fcl_geometric_shapes.cpp +++ b/test/test_fcl_geometric_shapes.cpp @@ -661,6 +661,30 @@ bool compareContactPointds2(const ContactPoint& cp1,const ContactPoint& cp return cp1.pos[2] < cp2.pos[2]; } // Ascending order +// Simple aligned boxes intersecting each other +// +// s2 ┌───┐ +// │ │ +// ┏━━━━━━━━━┿━━━┿━━━━━━━━━┓┄┄┄┄┄ z = 0 +// ┃ │┄┄┄│┄┄┄┄┄┄┄┄┄┃┄┄┄ Reported contact depth +// ┃ └───┘ ┃ +// ┃ ┃ +// ┃ ┃ +// ┃ s1 ┃ +// ┃ ┃ +// ┃ ┃ +// ┃ ┃ +// ┗━━━━━━━━━━━━━━━━━━━━━━━┛ +// +// s1 is a cube, 100 units to a side. Shifted downward 50 units so that it's +// top face is at z = 0. +// s2 is a box with dimensions 10, 20, 30 in the x-, y-, and z-directions, +// respectively. It is centered on the origin and oriented according to the +// provided rotation matrix `R`. +// The total penetration depth is the |z_max| where z_max is the z-position of +// the most deeply penetrating vertex on s2. The contact position will be +// located at -z_max / 2 (with the assumption that all penetration happens +// *below* the z = 0 axis. template void testBoxBoxContactPointds(const Eigen::MatrixBase& R) { @@ -711,7 +735,13 @@ void testBoxBoxContactPointds(const Eigen::MatrixBase& R) // We just check the deepest one as workaround. for (size_t i = 0; i < numContacts; ++i) { - EXPECT_TRUE(vertices[i].isApprox(contacts[i].pos)); + // The reported contact position will lie mid-way between the deepest + // penetrating vertex on s2 and the z = 0 plane. + Vector3 contact_pos(vertices[i]); + contact_pos(2) /= 2; + EXPECT_TRUE(contact_pos.isApprox(contacts[i].pos)) + << "\n\tExpected: " << contact_pos + << "\n\tFound: " << contacts[i].pos; EXPECT_TRUE(Vector3(0, 0, 1).isApprox(contacts[i].normal)); } }