Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Fix epa box support #397

Merged
Merged
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
Original file line number Diff line number Diff line change
@@ -1186,6 +1186,17 @@ static void ComputeVisiblePatchRecursive(
// g is not a visible face
if (!isOutsidePolytopeFace(&polytope, g, &query_point)) {
// Cannot see the neighbouring face from the new vertex.

// TODO([email protected]): when the new vertex is colinear with a
// border edge, we should remove the degenerate triangle. We could remove
// the middle vertex on that line from the polytope, and then reconnect
// the polytope.
if (triangle_area_is_zero(query_point, f.edge[edge_index]->vertex[0]->v.v,
f.edge[edge_index]->vertex[1]->v.v)) {
FCL_THROW_FAILED_AT_THIS_CONFIGURATION(
"The new vertex is colinear with an existing edge. The added "
"triangle would be degenerate.");
}
border_edges->insert(f.edge[edge_index]);
return;
}
@@ -2241,13 +2252,21 @@ static void convexToGJK(const Convex<S>& s, const Transform3<S>& tf,
static inline void supportBox(const void* obj, const ccd_vec3_t* dir_,
ccd_vec3_t* v)
{
// Use a customized sign function, so that the support of the box always
// appears in one of the box vertices.
// Picking support vertices on the interior of faces/edges can lead to
// degenerate triangles in the EPA algorithm and are no more correct than just
// picking box corners.
auto sign = [](ccd_real_t x) -> ccd_real_t {
return x >= 0 ? ccd_real_t(1.0) : ccd_real_t(-1.0);
};
const ccd_box_t* o = static_cast<const ccd_box_t*>(obj);
ccd_vec3_t dir;
ccdVec3Copy(&dir, dir_);
ccdQuatRotVec(&dir, &o->rot_inv);
ccdVec3Set(v, ccdSign(ccdVec3X(&dir)) * o->dim[0],
ccdSign(ccdVec3Y(&dir)) * o->dim[1],
ccdSign(ccdVec3Z(&dir)) * o->dim[2]);
ccdVec3Set(v, sign(ccdVec3X(&dir)) * o->dim[0],
sign(ccdVec3Y(&dir)) * o->dim[1],
sign(ccdVec3Z(&dir)) * o->dim[2]);
ccdQuatRotVec(v, &o->rot);
ccdVec3Add(v, &o->pos);
}
Original file line number Diff line number Diff line change
@@ -1289,7 +1289,7 @@ void SetUpBoxToBox(const Transform3<S>& X_WF, void** o1, void** o2, ccd_t* ccd,
fcl::Box<S> box1(box1_size);
fcl::Box<S> box2(box2_size);
*o1 = GJKInitializer<S, fcl::Box<S>>::createGJKObject(box1, X_WB1);
*o2 = GJKInitializer<S, fcl::Box<S>>::createGJKObject(box1, X_WB2);
*o2 = GJKInitializer<S, fcl::Box<S>>::createGJKObject(box2, X_WB2);

// Set up ccd solver.
CCD_INIT(ccd);
@@ -1329,20 +1329,20 @@ void TestSimplexToPolytope3InGivenFrame(const Transform3<S>& X_WF) {
// We find three points Pa1, Pb1, Pc1 on box 1, and three points Pa2, Pb2, Pc2
// on box 2, such that the 2-simplex with vertices (Pa1 - Pa2, Pb1 - Pb2,
// Pc1 - Pc2) contains the origin.
const Vector3<S> p_FPa1(-1, -1, -1);
const Vector3<S> p_FPa2(-0.1, 0.5, -1);
const Vector3<S> p_FPa1(-1, -1, 0.1);
const Vector3<S> p_FPa2(-0.1, 0.5, 0.1);
pts[0].v = ToCcdVec3<S>(p_FPa1 - p_FPa2);
pts[0].v1 = ToCcdVec3<S>(p_FPa1);
pts[0].v2 = ToCcdVec3<S>(p_FPa2);

const Vector3<S> p_FPb1(-1, 1, -1);
const Vector3<S> p_FPb2(-0.1, 0.5, -1);
const Vector3<S> p_FPb1(-1, 1, 0.1);
const Vector3<S> p_FPb2(-0.1, 0.5, 0.1);
pts[1].v = ToCcdVec3<S>(p_FPb1 - p_FPb2);
pts[1].v1 = ToCcdVec3<S>(p_FPb1);
pts[1].v2 = ToCcdVec3<S>(p_FPb2);

const Vector3<S> p_FPc1(1, 1, -1);
const Vector3<S> p_FPc2(-0.1, 0.5, -1);
const Vector3<S> p_FPc1(1, 1, 0.1);
const Vector3<S> p_FPc2(-0.1, 0.5, 0.1);
pts[2].v = ToCcdVec3<S>(p_FPc1 - p_FPc2);
pts[2].v1 = ToCcdVec3<S>(p_FPc1);
pts[2].v2 = ToCcdVec3<S>(p_FPc2);
2 changes: 1 addition & 1 deletion test/test_fcl_capsule_box_1.cpp
Original file line number Diff line number Diff line change
@@ -117,7 +117,7 @@ void test_distance_capsule_box(fcl::GJKSolverType solver_type, S solver_toleranc

GTEST_TEST(FCL_GEOMETRIC_SHAPES, distance_capsule_box_ccd)
{
test_distance_capsule_box<double>(fcl::GJKSolverType::GST_LIBCCD, 1e-6, 1e-4);
test_distance_capsule_box<double>(fcl::GJKSolverType::GST_LIBCCD, 1e-8, 4e-4);
}

GTEST_TEST(FCL_GEOMETRIC_SHAPES, distance_capsule_box_indep)
73 changes: 53 additions & 20 deletions test/test_fcl_signed_distance.cpp
Original file line number Diff line number Diff line change
@@ -296,34 +296,18 @@ void test_distance_cylinder_box() {
box_size, X_WB);
}

// This is a *specific* case that has cropped up in the wild that reaches the
// unexpected `validateNearestFeatureOfPolytopeBeingEdge` error. This error was
// reported in https://github.com/flexible-collision-library/fcl/issues/388
template <typename S>
void test_distance_box_box1() {
void test_distance_box_box_helper(const Vector3<S>& box1_size,
const Transform3<S>& X_WB1,
const Vector3<S>& box2_size,
const Transform3<S>& X_WB2) {
using CollisionGeometryPtr_t = std::shared_ptr<fcl::CollisionGeometryd>;
const Vector3<S> box1_size(0.03, 0.12, 0.1);
CollisionGeometryPtr_t box1_geo(
new fcl::Box<S>(box1_size(0), box1_size(1), box1_size(2)));
Transform3<S> X_WB1 = Transform3<S>::Identity();
X_WB1.matrix() << -3.0627937852578681533e-08, -0.99999999999999888978,
-2.8893865161583314238e-08, 0.63499979627350811029, 0.9999999999999980016,
-3.0627939739957803544e-08, 6.4729926918527511769e-08,
-0.48500002215636439651, -6.4729927722963847085e-08,
-2.8893863029448751323e-08, 0.99999999999999711342, 1.0778146458339641356,
0, 0, 0, 1;
fcl::CollisionObject<S> box1(box1_geo, X_WB1);

const Vector3<S> box2_size(0.025, 0.35, 1.845);
CollisionGeometryPtr_t box2_geo(
new fcl::Box<S>(box2_size(0), box2_size(1), box2_size(2)));
Transform3<S> X_WB2 = Transform3<S>::Identity();
// clang-format off
X_WB2.matrix() << 0, -1, 0, 0.8,
1, 0, 0, -0.4575,
0, 0, 1, 1.0225,
0, 0, 0, 1;
// clang-format on
fcl::CollisionObject<S> box2(box2_geo, X_WB2);

fcl::DistanceRequest<S> request;
@@ -347,6 +331,54 @@ void test_distance_box_box1() {
EXPECT_TRUE((p_B2P2.array().abs() <= (box2_size / 2).array() + tol).all());
}

// This is a *specific* case that has cropped up in the wild that reaches the
// unexpected `validateNearestFeatureOfPolytopeBeingEdge` error. This error was
// reported in https://github.com/flexible-collision-library/fcl/issues/388
template <typename S>
void test_distance_box_box1() {
const Vector3<S> box1_size(0.03, 0.12, 0.1);
Transform3<S> X_WB1 = Transform3<S>::Identity();
X_WB1.matrix() << -3.0627937852578681533e-08, -0.99999999999999888978,
-2.8893865161583314238e-08, 0.63499979627350811029, 0.9999999999999980016,
-3.0627939739957803544e-08, 6.4729926918527511769e-08,
-0.48500002215636439651, -6.4729927722963847085e-08,
-2.8893863029448751323e-08, 0.99999999999999711342, 1.0778146458339641356,
0, 0, 0, 1;

const Vector3<S> box2_size(0.025, 0.35, 1.845);
Transform3<S> X_WB2 = Transform3<S>::Identity();
// clang-format off
X_WB2.matrix() << 0, -1, 0, 0.8,
1, 0, 0, -0.4575,
0, 0, 1, 1.0225,
0, 0, 0, 1;
// clang-format on
test_distance_box_box_helper(box1_size, X_WB1, box2_size, X_WB2);
}

// This is a *specific* case that has cropped up in the wild that reaches the
// unexpected `triangle_size_is_zero` error. This error was
// reported in https://github.com/flexible-collision-library/fcl/issues/395
template <typename S>
void test_distance_box_box2() {
const Vector3<S> box1_size(0.46, 0.48, 0.01);
Transform3<S> X_WB1 = Transform3<S>::Identity();
X_WB1.matrix() << 1,0,0, -0.72099999999999997424,
0,1,0, -0.77200000000000001954,
0,0,1, 0.81000000000000005329,
0,0,0,1;

const Vector3<S> box2_size(0.049521, 0.146, 0.0725);
Transform3<S> X_WB2 = Transform3<S>::Identity();
// clang-format off
X_WB2.matrix() << 0.10758262492983036718, -0.6624881850015212903, -0.74130653817877356637, -0.42677133002999478872,
0.22682184885125472595, -0.709614040775253474, 0.6670830248314786326, -0.76596851247746788882,
-0.96797615037608542021, -0.23991106241273435495, 0.07392465377049164954, 0.80746731400091054098,
0, 0, 0, 1;
// clang-format on
test_distance_box_box_helper(box1_size, X_WB1, box2_size, X_WB2);
}

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

GTEST_TEST(FCL_NEGATIVE_DISTANCE, sphere_sphere_ccd)
@@ -380,6 +412,7 @@ GTEST_TEST(FCL_SIGNED_DISTANCE, cylinder_box_ccd) {

GTEST_TEST(FCL_SIGNED_DISTANCE, box_box1_ccd) {
test_distance_box_box1<double>();
test_distance_box_box2<double>();
}

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