Skip to content

Commit

Permalink
when libccd reports the nearest feature is an edge, validate the resu…
Browse files Browse the repository at this point in the history 
…lt. Also fixes #319.
  • Loading branch information
@hongkai-dai
hongkai-dai committed Apr 6, 2019
1 parent 5e93762 commit 295c07a
Show file tree
Hide file tree
Showing 2 changed files with 238 additions and 12 deletions.
140 changes: 128 additions & 12 deletions include/fcl/narrowphase/detail/convexity_based_algorithm/gjk_libccd-inl.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -369,6 +369,11 @@ static int doSimplex2(ccd_simplex_t *simplex, ccd_vec3_t *dir) {
return 0;
}

static bool isAbsValueLessThanEpsSquared(ccd_real_t val) {
return std::abs(val) < std::numeric_limits<ccd_real_t>::epsilon() *
std::numeric_limits<ccd_real_t>::epsilon();
}

// TODO(SeanCurtis-TRI): Define the return value:
// 1: (like doSimplex2) --> origin is "in" the simplex.
// 0:
Expand All @@ -386,10 +391,8 @@ static int doSimplex3(ccd_simplex_t *simplex, ccd_vec3_t *dir)
C = ccdSimplexPoint(simplex, 0);

// check touching contact
// TODO(SeanCurtis-TRI): This is dist2 (i.e., dist_squared) and should be
// tested to be zero within a tolerance of ε² (and not just ε).
dist = ccdVec3PointTriDist2(ccd_vec3_origin, &A->v, &B->v, &C->v, nullptr);
if (ccdIsZero(dist)){
if (isAbsValueLessThanEpsSquared(dist)){
return 1;
}

Expand Down Expand Up @@ -460,11 +463,6 @@ static int doSimplex3(ccd_simplex_t *simplex, ccd_vec3_t *dir)
return 0;
}

static bool isAbsValueLessThanEpsSquared(ccd_real_t val) {
return std::abs(val) < std::numeric_limits<ccd_real_t>::epsilon() *
std::numeric_limits<ccd_real_t>::epsilon();
}

static int doSimplex4(ccd_simplex_t *simplex, ccd_vec3_t *dir)
{
const ccd_support_t *A, *B, *C, *D;
Expand Down Expand Up @@ -940,6 +938,28 @@ static bool triangle_area_is_zero(const ccd_vec3_t& a, const ccd_vec3_t& b,
return false;
}

/**
* Determines if the point P is on the line segment AB.
* If A, B, P are coincident, report true.
*/
static bool is_point_on_line_segment(const ccd_vec3_t& p, const ccd_vec3_t& a, const ccd_vec3_t& b) {
if (are_coincident(a, b)) {
return are_coincident(a, p);
}
// A and B are not coincident, if the triangle ABP has non-zero area, then P
// is not on the line adjoining AB, and hence not on the line segment AB.
if (!triangle_area_is_zero(a, b, p)) {
return false;
}
// P is on the line adjoinging AB. If P is on the line segment AB, then
// PA.dot(PB) <= 0.
ccd_vec3_t PA, PB;
ccdVec3Sub2(&PA, &p, &a);
ccdVec3Sub2(&PB, &p, &b);
return ccdVec3Dot(&PA, &PB) <= 0;
}


/**
* Computes the normal vector of a triangular face on a polytope, and the normal
* vector points outward from the polytope. Notice we assume that the origin
Expand Down Expand Up @@ -1286,7 +1306,6 @@ static int expandPolytope(ccd_pt_t *polytope, ccd_pt_el_t *el,
"The visible feature is a vertex. This should already have been "
"identified as a touching contact");
}

// Start with the face on which the closest point lives
if (el->type == CCD_PT_FACE) {
start_face = reinterpret_cast<ccd_pt_face_t*>(el);
Expand Down Expand Up @@ -1381,7 +1400,7 @@ static ccd_vec3_t supportEPADirection(const ccd_pt_t* polytope,
pointing outward from the polytope.
*/
ccd_vec3_t dir;
if (ccdIsZero(nearest_feature->dist)) {
if (isAbsValueLessThanEpsSquared(nearest_feature->dist)) {
// nearest point is the origin.
switch (nearest_feature->type) {
case CCD_PT_VERTEX: {
Expand Down Expand Up @@ -1533,6 +1552,95 @@ static int __ccdGJK(const void *obj1, const void *obj2,
return -1;
}

/**
* When the nearest feature of a polytope to the origin is an edge, and the
* origin is inside the polytope, it implies one of the following conditions
* 1. The origin lies exactly on that edge
* 2. The two neighbouring faces of that edge are coplanar, and the projection
* of the origin onto that plane is on the edge. Inside this function, we will
* verify if one of these two conditions are true. If not, we will modify the
* nearest feature stored inside @p polytope, such that it stores the right
* nearest feature and distance.
* @param polytope The polytope whose nearest feature is being verified (and
* corrected if the edge should not be nearest feature).
* @note Only call this function in the EPA functions, where the origin should
* be inside the polytope.
*/
static void validateNearestFeatureOfPolytopeBeingEdge(ccd_pt_t* polytope) {
if (polytope->nearest_type == CCD_PT_EDGE) {
// Only verify the feature if the nearest feature is an edge.

ccd_pt_edge_t* nearest_edge =
reinterpret_cast<ccd_pt_edge_t*>(polytope->nearest);
// Find the outward normals on the two neighbouring faces of the edge, if
// the origin is on the "inner" side of these two faces, then we regard the
// origin to be inside the polytope. Note that these @p face_normals are
// normalized.
std::array<ccd_vec3_t, 2> face_normals;
std::array<double, 2> origin_to_face_distance;
for (int i = 0; i < 2; ++i) {
face_normals[i] =
faceNormalPointingOutward(polytope, nearest_edge->faces[i]);
ccdVec3Normalize(&(face_normals[i]));
// If the origin is on the "inner" side of the face, then
// face_normals[i].dot(origin - edge.vertex) <= 0.
origin_to_face_distance[i] =
-ccdVec3Dot(&(face_normals[i]), &(nearest_edge->vertex[0]->v.v));
if (origin_to_face_distance[i] > 0) {
FCL_THROW_FAILED_AT_THIS_CONFIGURATION(
"The origin is outside of the polytope. This should already have "
"been identified as separating.");
}
}
// If the two neighbouring faces are co-planar, then their face normals are
// the same.
bool are_neighbouring_faces_coplanar = true;
for (int i = 0; i < 3; ++i) {
if (!ccdIsZero(face_normals[0].v[i] - face_normals[1].v[i])) {
are_neighbouring_faces_coplanar = false;
break;
}
}
bool is_edge_closest_feature = true;
if (!are_neighbouring_faces_coplanar) {
// If the closest feature is an edge, and the two neighouring faces are
// not co-planar, then the origin has to be on that edge, and hence the
// distance from the origin to the face has to be 0.
if (!ccdIsZero(origin_to_face_distance[0]) ||
!ccdIsZero(origin_to_face_distance[1])) {
// The distance from the origin to the face is not 0. This violates the
// assumption that the closest feature is an edge. We will recompute the
// closest feature.
is_edge_closest_feature = false;
}
} else {
// The neighbouring faces are coplanar.
// We compute the projection of the origin onto the plane as
// -face_normals[0] * origin_to_face_distance[0]
ccd_vec3_t origin_projection_on_edge = face_normals[0];
ccdVec3Scale(&origin_projection_on_edge, -origin_to_face_distance[0]);
if (!is_point_on_line_segment(origin_projection_on_edge,
nearest_edge->vertex[0]->v.v,
nearest_edge->vertex[1]->v.v)) {
is_edge_closest_feature = false;
}
}
if (!is_edge_closest_feature) {
// We assume that libccd is not crazily wrong. Although the closest
// feature should not be the edge, it is near that edge. Hence we pick one
// of the neighbouring faces as the closest feature.
polytope->nearest_type = CCD_PT_FACE;
// Note origin_to_face_distance is the signed distance.
const int closest_face =
origin_to_face_distance[0] < origin_to_face_distance[1] ? 1 : 0;
polytope->nearest =
reinterpret_cast<ccd_pt_el_t*>(nearest_edge->faces[closest_face]);
// polytope->nearest_dist stores the SQUARED distance.
polytope->nearest_dist = origin_to_face_distance[closest_face] *
origin_to_face_distance[closest_face];
}
}
}

static int __ccdEPA(const void *obj1, const void *obj2,
const ccd_t *ccd,
Expand All @@ -1557,6 +1665,7 @@ static int __ccdEPA(const void *obj1, const void *obj2,
ret = simplexToPolytope2(obj1, obj2, ccd, simplex, polytope, nearest);
}


if (ret == -1){
// touching contact
return 0;
Expand All @@ -1566,12 +1675,19 @@ static int __ccdEPA(const void *obj1, const void *obj2,
}

while (1){
// get triangle nearest to origin
// get triangle nearest to origin
*nearest = ccdPtNearest(polytope);
if (polytope->nearest_type == CCD_PT_EDGE) {
// When libccd thinks the nearest feature is an edge, that is often
// wrong, hence we validate the nearest feature by ourselves.
validateNearestFeatureOfPolytopeBeingEdge(polytope);
*nearest = ccdPtNearest(polytope);
}

// get next support point
if (nextSupport(polytope, obj1, obj2, ccd, *nearest, &supp) != 0)
if (nextSupport(polytope, obj1, obj2, ccd, *nearest, &supp) != 0) {
break;
}

// expand nearest triangle using new point - supp
if (expandPolytope(polytope, *nearest, &supp) != 0)
Expand Down
110 changes: 110 additions & 0 deletions test/test_fcl_signed_distance.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -164,6 +164,106 @@ void test_distance_spherecapsule(GJKSolverType solver_type)
}
}

template <typename S>
void test_distance_cylinder_sphere1() {
// This is a specific case that has cropped up in the wild that reaches the
// unexpected `expandPolytope()` error, where the nearest point and the new
// vertiex botih lie on an edge. It turns out that libccd incorrectly thinks
// that the edge is the nearest feature, while actually one of the
// neighbouring face is. This test confirms that the bug is fixed, by the
// function validateNearestFeatureOfPolytopeBeingEdge.
using CollisionGeometryPtr_t = std::shared_ptr<fcl::CollisionGeometry<S>>;
const S cylinder_radius = 0.03;
const S cylinder_length = 0.65;
CollisionGeometryPtr_t cylinder_geo(
new fcl::Cylinder<S>(cylinder_radius, cylinder_length));
Transform3<S> X_WC = Transform3<S>::Identity();
X_WC.translation() <<0.6, 0,0.325;
fcl::CollisionObject<S> cylinder(cylinder_geo, X_WC);

const S sphere_radius = 0.055;
CollisionGeometryPtr_t sphere_geo(new fcl::Sphere<S>(sphere_radius));
Transform3<S> X_WS = Transform3<S>::Identity();
// clang-format off
X_WS.matrix() << -0.9954758066974283004, -0.029586630161622884394, 0.090291470227168560414, 0.54197940181714598928,
-0.085103478665251586222, -0.14494505659711237611, -0.98577295990868651909, -0.062117502536077888464,
0.042253002250460247602, -0.98899725069574340175, 0.14177137199407846557, 0.60162365763662117857,
0, 0, 0, 1;
// clang-format on
fcl::CollisionObject<S> sphere(sphere_geo, X_WS);

fcl::DistanceRequest<S> request;
request.gjk_solver_type = GJKSolverType::GST_LIBCCD;
request.distance_tolerance = 9.9999999999999995475e-07;
request.enable_signed_distance = true;
fcl::DistanceResult<S> result;

EXPECT_NO_THROW(fcl::distance(&cylinder, &sphere, request, result));
// The two objects are penetrating.
EXPECT_NEAR(-(result.nearest_points[0] - result.nearest_points[1]).norm(),
result.min_distance, request.distance_tolerance);
// p_CPc is the position of the witness point Pc on the cylinder, measured
// and expressed in the cylinder frame C.
const Vector3<S> p_CPc = X_WC.inverse() * result.nearest_points[0];
EXPECT_LE(std::abs(p_CPc(2)), cylinder_length / 2);
EXPECT_LE(p_CPc.template head<2>().norm(), cylinder_radius);
// p_SPs is the position of the witness point Ps on the sphere, measured and
// expressed in the sphere frame S.
const Vector3<S> p_SPs = X_WS.inverse() * result.nearest_points[1];
EXPECT_LE(p_SPs.norm(), sphere_radius);
}

// This is a *specific* case that has cropped up in the wild that reaches the
// unexpected `expandPolytope()` error. This error was reported in
// https://github.com/flexible-collision-library/fcl/issues/319
template <typename S>
void test_distance_cylinder_box1() {
using CollisionGeometryPtr_t = std::shared_ptr<fcl::CollisionGeometryd>;
const S cylinder_radius = 0.05;
const S cylinder_length = 0.06;
CollisionGeometryPtr_t cylinder_geo(
new fcl::Cylinder<S>(cylinder_radius, cylinder_length));
Transform3<S> X_WC = Transform3<S>::Identity();
X_WC.matrix() << -0.99999999997999022838, 6.2572835802045040178e-10,
6.3260669852976095481e-06, 0.57500009756757608503,
6.3260669851683709551e-06, -6.3943303429958554955e-10,
0.99999999997999056145, -0.42711963046787942977,
6.2573180158128459924e-10, 1, 6.3942912945996747041e-10,
1.1867093358746836351, 0, 0, 0, 1;
fcl::CollisionObject<S> cylinder(cylinder_geo, X_WC);

const Vector3<S> box_size(0.025, 0.35, 1.845);
CollisionGeometryPtr_t box_geo(
new fcl::Box<S>(box_size(0), box_size(1), box_size(2)));
Transform3<S> X_WB = Transform3<S>::Identity();
X_WB.matrix() << 6.1232339957367660359e-17, -1, 0, 0.80000000000000004441, 1,
6.1232339957367660359e-17, 0, -0.45750000000000001776, 0, 0, 1,
1.0224999999999999645, 0, 0, 0, 1;
fcl::CollisionObject<S> box(box_geo, X_WB);

fcl::DistanceRequest<S> request;
request.gjk_solver_type = GJKSolverType::GST_LIBCCD;
request.distance_tolerance = 9.9999999999999995475e-07;
request.enable_signed_distance = true;
fcl::DistanceResult<S> result;

EXPECT_NO_THROW(fcl::distance(&cylinder, &box, request, result));
EXPECT_NEAR(result.min_distance,
(result.nearest_points[0] - result.nearest_points[1]).norm(),
request.distance_tolerance);
// p_CPc is the position of the witness point Pc on the cylinder, measured
// and expressed in the cylinder frame C.
const Vector3<S> p_CPc = X_WC.inverse() * result.nearest_points[0];
EXPECT_LE(std::abs(p_CPc(2)), cylinder_length / 2);
EXPECT_LE(p_CPc.template head<2>().norm(), cylinder_radius);
// p_BPb is the position of the witness point Pb on the box, measured and
// expressed in the box frame B.
const Vector3<S> p_BPb = X_WB.inverse() * result.nearest_points[1];
EXPECT_TRUE((p_BPb.array().abs() <=
box_size.array() / 2 + 10 * std::numeric_limits<S>::epsilon())
.all());
}

//==============================================================================

GTEST_TEST(FCL_NEGATIVE_DISTANCE, sphere_sphere_ccd)
Expand All @@ -186,6 +286,16 @@ GTEST_TEST(FCL_NEGATIVE_DISTANCE, sphere_capsule_indep)
test_distance_spherecapsule<double>(GST_INDEP);
}

GTEST_TEST(FCL_SIGNED_DISTANCE, cylinder_sphere1_ccd)
{
test_distance_cylinder_sphere1<double>();
}

GTEST_TEST(FCL_SIGNED_DISTANCE, cylinder_box1_ccd)
{
test_distance_cylinder_box1<double>();
}

//==============================================================================
int main(int argc, char* argv[])
{
Expand Down

0 comments on commit 295c07a

Please sign in to comment.