diff --git a/include/fcl/narrowphase/detail/convexity_based_algorithm/gjk_libccd-inl.h b/include/fcl/narrowphase/detail/convexity_based_algorithm/gjk_libccd-inl.h index 7008b8863..572455826 100644 --- a/include/fcl/narrowphase/detail/convexity_based_algorithm/gjk_libccd-inl.h +++ b/include/fcl/narrowphase/detail/convexity_based_algorithm/gjk_libccd-inl.h @@ -40,6 +40,9 @@ #include "fcl/narrowphase/detail/convexity_based_algorithm/gjk_libccd.h" +#include +#include + #include "fcl/common/unused.h" #include "fcl/common/warning.h" @@ -555,90 +558,134 @@ static int simplexToPolytope2(const void *obj1, const void *obj2, return 0; } - -/** Transforms simplex to polytope, three vertices required */ -static int simplexToPolytope3(const void *obj1, const void *obj2, - const ccd_t *ccd, - const ccd_simplex_t *simplex, - ccd_pt_t *pt, ccd_pt_el_t **nearest) -{ - const ccd_support_t *a, *b, *c; - ccd_support_t d, d2; - ccd_vec3_t ab, ac, dir; - ccd_pt_vertex_t *v[5]; - ccd_pt_edge_t *e[9]; - ccd_real_t dist, dist2; - - *nearest = NULL; - - a = ccdSimplexPoint(simplex, 0); - b = ccdSimplexPoint(simplex, 1); - c = ccdSimplexPoint(simplex, 2); - - // If only one triangle left from previous GJK run origin lies on this - // triangle. So it is necessary to expand triangle into two - // tetrahedrons connected with base (which is exactly abc triangle). - - // get next support point in direction of normal of triangle - ccdVec3Sub2(&ab, &b->v, &a->v); - ccdVec3Sub2(&ac, &c->v, &a->v); - ccdVec3Cross(&dir, &ab, &ac); - __ccdSupport(obj1, obj2, &dir, ccd, &d); - dist = ccdVec3PointTriDist2(&d.v, &a->v, &b->v, &c->v, NULL); - - // and second one take in opposite direction - ccdVec3Scale(&dir, -CCD_ONE); - __ccdSupport(obj1, obj2, &dir, ccd, &d2); - dist2 = ccdVec3PointTriDist2(&d2.v, &a->v, &b->v, &c->v, NULL); - - // check if face isn't already on edge of minkowski sum and thus we - // have touching contact - if (ccdIsZero(dist) || ccdIsZero(dist2)){ - v[0] = ccdPtAddVertex(pt, a); - v[1] = ccdPtAddVertex(pt, b); - v[2] = ccdPtAddVertex(pt, c); - e[0] = ccdPtAddEdge(pt, v[0], v[1]); - e[1] = ccdPtAddEdge(pt, v[1], v[2]); - e[2] = ccdPtAddEdge(pt, v[2], v[0]); - *nearest = (ccd_pt_el_t *)ccdPtAddFace(pt, e[0], e[1], e[2]); - if (*nearest == NULL) - return -2; - - return -1; +#ifndef NDEBUG +static bool isPolytopeEmpty(const ccd_pt_t& polytope) { + ccd_pt_vertex_t* v = nullptr; + ccdListForEachEntry(&polytope.vertices, v, ccd_pt_vertex_t, list) { + if (v) { + return false; } + } + ccd_pt_edge_t* e = nullptr; + ccdListForEachEntry(&polytope.edges, e, ccd_pt_edge_t, list) { + if (e) { + return false; + } + } + ccd_pt_face_t* f = nullptr; + ccdListForEachEntry(&polytope.faces, f, ccd_pt_face_t, list) { + if (f) { + return false; + } + } + return true; +} +#endif - // form polyhedron - v[0] = ccdPtAddVertex(pt, a); - v[1] = ccdPtAddVertex(pt, b); - v[2] = ccdPtAddVertex(pt, c); - v[3] = ccdPtAddVertex(pt, &d); - v[4] = ccdPtAddVertex(pt, &d2); - - e[0] = ccdPtAddEdge(pt, v[0], v[1]); - e[1] = ccdPtAddEdge(pt, v[1], v[2]); - e[2] = ccdPtAddEdge(pt, v[2], v[0]); - - e[3] = ccdPtAddEdge(pt, v[3], v[0]); - e[4] = ccdPtAddEdge(pt, v[3], v[1]); - e[5] = ccdPtAddEdge(pt, v[3], v[2]); - - e[6] = ccdPtAddEdge(pt, v[4], v[0]); - e[7] = ccdPtAddEdge(pt, v[4], v[1]); - e[8] = ccdPtAddEdge(pt, v[4], v[2]); +/** Transforms a 2-simplex (triangle) to a polytope (tetrahedron), three + * vertices required. + * Both the simplex and the transformed polytope contain the origin. The simplex + * vertices lie on the surface of the Minkowski difference obj1 ⊖ obj2. + * @param[in] obj1 object 1 on which the distance is queried. + * @param[in] obj2 object 2 on which the distance is queried. + * @param[in] ccd The ccd solver. + * @param[in] simplex The simplex (with three vertices) that contains the + * origin. + * @param[out] polytope The polytope (tetrahedron) that also contains the origin + * on one of its faces. At input, the polytope should be empty. + * @param[out] nearest If the function detects that obj1 and obj2 are touching, + * then set *nearest to be the nearest points on obj1 and obj2 respectively; + * otherwise set *nearest to NULL. @note nearest cannot be NULL. + * @retval status return 0 on success, -1 if touching contact is detected, and + * -2 on non-recoverable failure (mostly due to memory allocation bug). + */ +static int convert2SimplexToTetrahedron(const void* obj1, const void* obj2, + const ccd_t* ccd, const ccd_simplex_t* simplex, + ccd_pt_t* polytope, ccd_pt_el_t** nearest) { + assert(nearest); + assert(isPolytopeEmpty(*polytope)); + assert(simplex->last == 2); // a 2-simplex. + const ccd_support_t *a, *b, *c; + ccd_support_t d, d2; + ccd_vec3_t ab, ac, dir; + ccd_pt_vertex_t* v[4]; + ccd_pt_edge_t* e[6]; + ccd_real_t dist, dist2; + + *nearest = NULL; + + a = ccdSimplexPoint(simplex, 0); + b = ccdSimplexPoint(simplex, 1); + c = ccdSimplexPoint(simplex, 2); + + // The 2-simplex is just a triangle containing the origin. We will expand this + // triangle to a tetrahedron, by adding the support point along the normal + // direction of the triangle. + ccdVec3Sub2(&ab, &b->v, &a->v); + ccdVec3Sub2(&ac, &c->v, &a->v); + ccdVec3Cross(&dir, &ab, &ac); + __ccdSupport(obj1, obj2, &dir, ccd, &d); + dist = ccdVec3PointTriDist2(&d.v, &a->v, &b->v, &c->v, NULL); + + // and second one take in opposite direction + ccdVec3Scale(&dir, -CCD_ONE); + __ccdSupport(obj1, obj2, &dir, ccd, &d2); + dist2 = ccdVec3PointTriDist2(&d2.v, &a->v, &b->v, &c->v, NULL); + + // check if face isn't already on edge of minkowski sum and thus we + // have touching contact + if (ccdIsZero(dist) || ccdIsZero(dist2)) { + v[0] = ccdPtAddVertex(polytope, a); + v[1] = ccdPtAddVertex(polytope, b); + v[2] = ccdPtAddVertex(polytope, c); + e[0] = ccdPtAddEdge(polytope, v[0], v[1]); + e[1] = ccdPtAddEdge(polytope, v[1], v[2]); + e[2] = ccdPtAddEdge(polytope, v[2], v[0]); + *nearest = (ccd_pt_el_t*)ccdPtAddFace(polytope, e[0], e[1], e[2]); + if (*nearest == NULL) return -2; - if (ccdPtAddFace(pt, e[3], e[4], e[0]) == NULL - || ccdPtAddFace(pt, e[4], e[5], e[1]) == NULL - || ccdPtAddFace(pt, e[5], e[3], e[2]) == NULL + return -1; + } + // Form a tetrahedron with abc as one face, pick either d or d2, based + // on which one has larger distance to the face abc. We pick the larger + // distance because it gives a tetrahedron with larger volume, so potentially + // more "expanded" than the one with the smaller volume. + auto FormTetrahedron = [polytope, a, b, c, &v, + &e](const ccd_support_t& new_support) -> int { + v[0] = ccdPtAddVertex(polytope, a); + v[1] = ccdPtAddVertex(polytope, b); + v[2] = ccdPtAddVertex(polytope, c); + v[3] = ccdPtAddVertex(polytope, &new_support); + + e[0] = ccdPtAddEdge(polytope, v[0], v[1]); + e[1] = ccdPtAddEdge(polytope, v[1], v[2]); + e[2] = ccdPtAddEdge(polytope, v[2], v[0]); + e[3] = ccdPtAddEdge(polytope, v[0], v[3]); + e[4] = ccdPtAddEdge(polytope, v[1], v[3]); + e[5] = ccdPtAddEdge(polytope, v[2], v[3]); - || ccdPtAddFace(pt, e[6], e[7], e[0]) == NULL - || ccdPtAddFace(pt, e[7], e[8], e[1]) == NULL - || ccdPtAddFace(pt, e[8], e[6], e[2]) == NULL){ - return -2; + // ccdPtAdd*() functions return NULL either if the memory allocation + // failed of if any of the input pointers are NULL, so the bad + // allocation can be checked by the last calls of ccdPtAddFace() + // because the rest of the bad allocations eventually "bubble up" here + // Note, there is no requirement on the winding of the face, namely we do + // not guarantee if all f.e(0).cross(f.e(1)) points outward (or inward) for + // all the faces added below. + if (ccdPtAddFace(polytope, e[0], e[1], e[2]) == NULL || + ccdPtAddFace(polytope, e[3], e[4], e[0]) == NULL || + ccdPtAddFace(polytope, e[4], e[5], e[1]) == NULL || + ccdPtAddFace(polytope, e[5], e[3], e[2]) == NULL) { + return -2; } - return 0; -} + }; + if (std::abs(dist) > std::abs(dist2)) { + return FormTetrahedron(d); + } else { + return FormTetrahedron(d2); + } +} /** Transforms simplex to polytope. It is assumed that simplex has 4 * vertices! */ @@ -660,13 +707,15 @@ static int simplexToPolytope4(const void *obj1, const void *obj2, d = ccdSimplexPoint(simplex, 3); // check if origin lies on some of tetrahedron's face - if so use - // simplexToPolytope3() + // convert2SimplexToTetrahedron() use_polytope3 = 0; dist = ccdVec3PointTriDist2(ccd_vec3_origin, &a->v, &b->v, &c->v, NULL); if (ccdIsZero(dist)){ use_polytope3 = 1; } dist = ccdVec3PointTriDist2(ccd_vec3_origin, &a->v, &c->v, &d->v, NULL); + // TODO(hongkai.dai@tri.global) Optimize this part, check whether these + // "if" branches are mutually exclusive. if (ccdIsZero(dist)){ use_polytope3 = 1; ccdSimplexSet(simplex, 1, c); @@ -687,7 +736,7 @@ static int simplexToPolytope4(const void *obj1, const void *obj2, if (use_polytope3){ ccdSimplexSetSize(simplex, 3); - return simplexToPolytope3(obj1, obj2, ccd, simplex, pt, nearest); + return convert2SimplexToTetrahedron(obj1, obj2, ccd, simplex, pt, nearest); } // no touching contact - simply create tetrahedron @@ -716,190 +765,498 @@ static int simplexToPolytope4(const void *obj1, const void *obj2, return 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 + * lives within the polytope, and the normal vector may not have unit length. + * @param[in] polytope The polytope on which the face lives. We assume that the + * origin also lives inside the polytope. + * @param[in] face The face for which the normal vector is computed. + * @retval dir The vector normal to the face, and points outward from the + * polytope. `dir` is unnormalized, that it does not necessarily have a unit + * length. + */ +static ccd_vec3_t faceNormalPointingOutward(const ccd_pt_t* polytope, + const ccd_pt_face_t* face) { + // We find two edges of the triangle as e1 and e2, and the normal vector + // of the face is e1.cross(e2). + ccd_vec3_t e1, e2; + ccdVec3Sub2(&e1, &(face->edge[0]->vertex[1]->v.v), + &(face->edge[0]->vertex[0]->v.v)); + ccdVec3Sub2(&e2, &(face->edge[1]->vertex[1]->v.v), + &(face->edge[1]->vertex[0]->v.v)); + ccd_vec3_t dir; + // TODO(hongkai.dai): we ignore the degeneracy here, namely we assume e1 and + // e2 are not colinear. We should check if e1 and e2 are colinear, and handle + // this corner case. + ccdVec3Cross(&dir, &e1, &e2); + const ccd_real_t dir_norm = std::sqrt(ccdVec3Len2(&dir)); + // The winding of the triangle is *not* guaranteed. The normal `n = e₁ × e₂` + // may point inside or outside. We rely on the fact that the origin lies + // within the polytope to resolve this ambiguity. A vector from the origin to + // any point on the triangle must point in the "same" direction as the normal + // (positive dot product). + + // However, the distance to the origin may be too small for the origin to + // serve as a reliable witness of inside-ness. In that case, we examine the + // polytope's *other* vertices; they should all lie on the "inside" of the + // current triangle. If at least one is a reliable distance, then that is + // considered to be the inside. If all vertices are "too close" (like the + // origin), then "inside" is defined as the side of the triangle that had the + // most distant vertex. + + // For these tests, we use the arbitrary distance of 0.01 unit length as a + // "reliable" distance for both the origin and other vertices. Even if it + // seems large, the fall through case of comparing the maximum distance will + // always guarantee correctness. + const ccd_real_t dist_tol = 0.01; + ccd_real_t tol = dist_tol * dir_norm; + ccd_real_t projection = ccdVec3Dot(&dir, &(face->edge[0]->vertex[0]->v.v)); + if (projection < -tol) { + // Origin is more than `dist_tol` away from the plane, but the negative + // value implies that the normal vector is pointing in the wrong direction; + // flip it. + ccdVec3Scale(&dir, ccd_real_t(-1)); + } else if (projection >= -tol && projection <= tol) { + // The origin is close to the plane of the face. Pick another vertex to test + // the normal direction. + ccd_real_t max_projection = -CCD_REAL_MAX; + ccd_real_t min_projection = CCD_REAL_MAX; + ccd_pt_vertex_t* v; + // If the magnitude of the projection is larger than tolerance, then it + // means one of the vertices is at least 1cm away from the plane coinciding + // with the face. + ccdListForEachEntry(&polytope->vertices, v, ccd_pt_vertex_t, list) { + projection = ccdVec3Dot(&dir, &(v->v.v)); + if (projection > tol) { + // The vertex is at least dist_tol away from the face plane, on the same + // direction of `dir`. So we flip dir to point it outward from the + // polytope. + ccdVec3Scale(&dir, ccd_real_t(-1)); + break; + } else if (projection < -tol) { + // The vertex is at least 1cm away from the face plane, on the opposite + // direction of `dir`. So `dir` points outward already. + break; + } else { + max_projection = std::max(max_projection, projection); + min_projection = std::min(min_projection, projection); + } + } + // If max_projection > |min_projection|, it means that the vertices that are + // on the positive side of the plane, has a larger maximal distance than the + // vertices on the negative side of the plane. Thus we regard that `dir` + // points into the polytope. Hence we flip `dir`. + if (max_projection > std::abs(min_projection)) { + ccdVec3Scale(&dir, ccd_real_t(-1)); + } + } + return dir; +} + +// Return true if the point `pt` is on the outward side of the half-plane, on +// which the triangle `f1 lives. Notice if the point `pt` is exactly on the +// half-plane, the return is false. +// @param f A triangle on a polytope. +// @param pt A point. +static bool isOutsidePolytopeFace(const ccd_pt_t* polytope, + const ccd_pt_face_t* f, const ccd_vec3_t* pt) { + ccd_vec3_t n = faceNormalPointingOutward(polytope, f); + // r_VP is the vector from a vertex V on the face `f`, to the point P `pt`. + ccd_vec3_t r_VP; + ccdVec3Sub2(&r_VP, pt, &(f->edge[0]->vertex[0]->v.v)); + return ccdVec3Dot(&n, &r_VP) > 0; +} + +#ifndef NDEBUG +// The function ComputeVisiblePatchRecursiveSanityCheck() is only called in the +// debug mode. In the release mode, this function is declared/defined but not +// used. Without this NDEBUG macro, the function will cause a -Wunused-function +// error on CI's release builds. +/** + * Reports true if the visible patch is valid. + * The invariant for computing the visible patch is that for each edge in the + * polytope, if both neighbouring faces are visible, then the edge is an + * internal edge; if only one neighbouring face is visible, then the edge + * is a border edge. + * For each face, if one of its edges is an internal edge, then the face is + * visible. + */ +static bool ComputeVisiblePatchRecursiveSanityCheck( + const ccd_pt_t& polytope, + const std::unordered_set& border_edges, + const std::unordered_set& visible_faces, + const std::unordered_set& internal_edges) { + ccd_pt_face_t* f; + ccdListForEachEntry(&polytope.faces, f, ccd_pt_face_t, list) { + bool has_edge_internal = false; + for (int i = 0; i < 3; ++i) { + // Since internal_edges is a set, internal_edges.count(e) means that e + // is contained in the set internal_edges. + if (internal_edges.count(f->edge[i]) > 0) { + has_edge_internal = true; + break; + } + } + if (has_edge_internal) { + if (visible_faces.count(f) == 0) { + return false; + } + } + } + ccd_pt_edge_t* e; + ccdListForEachEntry(&polytope.edges, e, ccd_pt_edge_t, list) { + if (visible_faces.count(e->faces[0]) > 0 && + visible_faces.count(e->faces[1]) > 0) { + if (internal_edges.count(e) == 0) { + return false; + } + } else if (visible_faces.count(e->faces[0]) + + visible_faces.count(e->faces[1]) == + 1) { + if (border_edges.count(e) == 0) { + return false; + } + } + } + return true; +} +#endif +/** + * This function contains the implementation detail of ComputeVisiblePatch() + * function. It should not be called by any function other than + * ComputeVisiblePatch(). + */ +static void ComputeVisiblePatchRecursive( + const ccd_pt_t& polytope, ccd_pt_face_t& f, int edge_index, + const ccd_vec3_t& query_point, + std::unordered_set* border_edges, + std::unordered_set* visible_faces, + std::unordered_set* internal_edges) { + /* + This function will be called recursively. It first checks if the face `g` + neighouring face `f` along the common edge `f->edge[edge_index]` can be seen + from the point `query_point`. If this face g cannot be seen, then stop. + Otherwise, we continue to check the neighbouring faces of g, by calling this + function recursively. + */ + ccd_pt_face_t* g = f.edge[edge_index]->faces[0] == &f + ? f.edge[edge_index]->faces[1] + : f.edge[edge_index]->faces[0]; + if (visible_faces->count(g) == 0) { + // g is not a visible face + if (!isOutsidePolytopeFace(&polytope, g, &query_point)) { + // Cannot see the neighbouring face from the new vertex. + border_edges->insert(f.edge[edge_index]); + return; + } + visible_faces->insert(g); + internal_edges->insert(f.edge[edge_index]); + for (int i = 0; i < 3; ++i) { + if (g->edge[i] != f.edge[edge_index]) { + // One of the neighbouring face is `f`, so do not need to visit again. + ComputeVisiblePatchRecursive(polytope, *g, i, query_point, border_edges, + visible_faces, internal_edges); + } + } + } else { + // Face f is visible (prerequisite), and g has been previously + // marked visible (via a different path); their shared edge should be marked + // internal. + internal_edges->insert(f.edge[edge_index]); + } +} - -/** Expands polytope's tri by new vertex v. Triangle tri is replaced by - * three triangles each with one vertex in v. */ -static int expandPolytope(ccd_pt_t *pt, ccd_pt_el_t *el, +/** Defines the "visible" patch on the given convex `polytope` (with respect to + * the given `query_vertex` which is a point outside the polytope.) + * + * A patch is characterized by: + * - a contiguous set of visible faces + * - internal edges (the edges for which *both* adjacent faces are in the + * patch) + * - border edges (edges for which only *one* adjacent face is in the patch) + * + * A face `f` (with vertices `(v₀, v₁, v₂)` and outward pointing normal `n̂`) is + * "visible" and included in the patch if, for query vertex `q`: + * + * `n̂ ⋅ (q - v₀) > 0` + * + * Namely the query vertex `q` is on the "outer" side of the face `f`. + * + * @param polytope The polytope to evaluate. + * @param f A face known to be visible to the query point. + * @param query_point The point from which visibility is evaluated. + * @param[out] border_edges The collection of patch border edges. + * @param[out] visible_faces The collection of patch faces. + * @param[out] internal_edges The collection of internal edges. + * + * @pre The `polytope` is convex. + * @pre The face `f` is visible from `query_point`. + * @pre Output parameters are non-null. + * TODO(hongkai.dai@tri.global) Replace patch computation with patch deletion + * and return border edges as an optimization. + * TODO(hongkai.dai@tri.global) Consider caching results of per-face visiblity + * status to prevent redundant recalculation -- or by associating the face + * normal with the face. + */ +static void ComputeVisiblePatch( + const ccd_pt_t& polytope, ccd_pt_face_t& f, + const ccd_vec3_t& query_point, + std::unordered_set* border_edges, + std::unordered_set* visible_faces, + std::unordered_set* internal_edges) { + assert(border_edges); + assert(visible_faces); + assert(internal_edges); + assert(border_edges->empty()); + assert(visible_faces->empty()); + assert(internal_edges->empty()); + assert(isOutsidePolytopeFace(&polytope, &f, &query_point)); + visible_faces->insert(&f); + for (int edge_index = 0; edge_index < 3; ++edge_index) { + ComputeVisiblePatchRecursive(polytope, f, edge_index, query_point, + border_edges, visible_faces, internal_edges); + } + assert(ComputeVisiblePatchRecursiveSanityCheck( + polytope, *border_edges, *visible_faces, *internal_edges)); +} + +/** Expands the polytope by adding a new vertex `newv` to the polytope. The + * new polytope is the convex hull of the new vertex together with the old + * polytope. This new polytope includes new edges (by connecting the new vertex + * with existing vertices) and new faces (by connecting the new vertex with + * existing edges). We only keep the edges and faces that are on the boundary + * of the new polytope. The edges/faces on the original polytope that would be + * interior to the new convex hull are discarded. + * @param[in/out] polytope The polytope. + * @param[in] el A feature that is visible from the point `newv` and contains + * the polytope boundary point that is closest to the origin. This feature + * should be either a face or an edge. A face is visible from a point ouside the + * original polytope, if the point is on the "outer" side of the face. An edge + * is visible from that point, if one of its neighbouring faces is visible. This + * feature contains the point that is closest to the origin on the boundary of + * the polytope. If the feature is an edge, and the two neighbouring faces of + * that edge are not co-planar, then the origin must lie on that edge. The + * feature should not be a vertex, as that would imply the two objects are in + * touching contact, causing the algorithm to exit before calling + * expandPolytope() function. + * @param[in] newv The new vertex add to the polytope. + * @retval status Returns 0 on success. Returns -2 otherwise. + */ +static int expandPolytope(ccd_pt_t *polytope, ccd_pt_el_t *el, const ccd_support_t *newv) { - ccd_pt_vertex_t *v[5]; - ccd_pt_edge_t *e[8]; - ccd_pt_face_t *f[2]; - - - // element can be either segment or triangle - if (el->type == CCD_PT_EDGE){ - // In this case, segment should be replaced by new point. - // Simpliest case is when segment stands alone and in this case - // this segment is replaced by two other segments both connected to - // newv. - // Segment can be also connected to max two faces and in that case - // each face must be replaced by two other faces. To do this - // correctly it is necessary to have correctly ordered edges and - // vertices which is exactly what is done in following code. - // - - ccdPtEdgeVertices((const ccd_pt_edge_t *)el, &v[0], &v[2]); - - ccdPtEdgeFaces((ccd_pt_edge_t *)el, &f[0], &f[1]); - - if (f[0]){ - ccdPtFaceEdges(f[0], &e[0], &e[1], &e[2]); - if (e[0] == (ccd_pt_edge_t *)el){ - e[0] = e[2]; - }else if (e[1] == (ccd_pt_edge_t *)el){ - e[1] = e[2]; - } - ccdPtEdgeVertices(e[0], &v[1], &v[3]); - if (v[1] != v[0] && v[3] != v[0]){ - e[2] = e[0]; - e[0] = e[1]; - e[1] = e[2]; - if (v[1] == v[2]) - v[1] = v[3]; - }else{ - if (v[1] == v[0]) - v[1] = v[3]; - } - - if (f[1]){ - ccdPtFaceEdges(f[1], &e[2], &e[3], &e[4]); - if (e[2] == (ccd_pt_edge_t *)el){ - e[2] = e[4]; - }else if (e[3] == (ccd_pt_edge_t *)el){ - e[3] = e[4]; - } - ccdPtEdgeVertices(e[2], &v[3], &v[4]); - if (v[3] != v[2] && v[4] != v[2]){ - e[4] = e[2]; - e[2] = e[3]; - e[3] = e[4]; - if (v[3] == v[0]) - v[3] = v[4]; - }else{ - if (v[3] == v[2]) - v[3] = v[4]; - } - } - - - v[4] = ccdPtAddVertex(pt, newv); - - ccdPtDelFace(pt, f[0]); - if (f[1]){ - ccdPtDelFace(pt, f[1]); - ccdPtDelEdge(pt, (ccd_pt_edge_t *)el); - } - - e[4] = ccdPtAddEdge(pt, v[4], v[2]); - e[5] = ccdPtAddEdge(pt, v[4], v[0]); - e[6] = ccdPtAddEdge(pt, v[4], v[1]); - if (f[1]) - e[7] = ccdPtAddEdge(pt, v[4], v[3]); - - - if (ccdPtAddFace(pt, e[1], e[4], e[6]) == NULL - || ccdPtAddFace(pt, e[0], e[6], e[5]) == NULL){ - return -2; - } - - if (f[1]){ - FCL_SUPPRESS_MAYBE_UNINITIALIZED_BEGIN - if (ccdPtAddFace(pt, e[3], e[5], e[7]) == NULL - || ccdPtAddFace(pt, e[4], e[7], e[2]) == NULL){ - return -2; - } - FCL_SUPPRESS_MAYBE_UNINITIALIZED_END - }else{ - if (ccdPtAddFace(pt, e[4], e[5], (ccd_pt_edge_t *)el) == NULL) - return -2; - } - } - }else{ // el->type == CCD_PT_FACE - // replace triangle by tetrahedron without base (base would be the - // triangle that will be removed) - - // get triplet of surrounding edges and vertices of triangle face - ccdPtFaceEdges((const ccd_pt_face_t *)el, &e[0], &e[1], &e[2]); - ccdPtEdgeVertices(e[0], &v[0], &v[1]); - ccdPtEdgeVertices(e[1], &v[2], &v[3]); - - // following code sorts edges to have e[0] between vertices 0-1, - // e[1] between 1-2 and e[2] between 2-0 - if (v[2] != v[1] && v[3] != v[1]){ - // swap e[1] and e[2] - e[3] = e[1]; - e[1] = e[2]; - e[2] = e[3]; - } - if (v[3] != v[0] && v[3] != v[1]) - v[2] = v[3]; + // The outline of the algorithm is as follows: + // 1. Compute the visible patch relative to the new vertex (See + // ComputeVisiblePatch() for details). + // 2. Delete the faces and internal edges. + // 3. Build a new face from each border edge and the new vertex. + + // To remove all faces that can be seen from the new vertex, we start with the + // face on which the closest point lives, and then do a depth-first search on + // its neighbouring triangles, until the triangle cannot be seen from the new + // vertex. + // TODO(hongkai.dai@tri.global): it is inefficient to store visible + // faces/edges. A better implementation should remove visible faces and + // internal edges inside ComputeVisiblePatch() function, when traversing the + // faces on the polytope. We focus on the correctness in the first place. + // Later when we make sure that the whole EPA implementation is bug free, we + // will improve the performance. + + ccd_pt_face_t* start_face = NULL; + // If the feature is a point, in the EPA algorithm, this means that the two + // objects are in touching contact. The EPA should terminate before calling + // this expandPolytope function, when it detects touching contact. + assert(el->type != CCD_PT_VERTEX); + // Start with the face on which the closest point lives + if (el->type == CCD_PT_FACE) { + start_face = reinterpret_cast(el); + } else if (el->type == CCD_PT_EDGE) { + // Check the two neighbouring faces of the edge. + ccd_pt_face_t* f[2]; + ccdPtEdgeFaces(reinterpret_cast(el), &f[0], &f[1]); + if (isOutsidePolytopeFace(polytope, f[0], &newv->v)) { + start_face = f[0]; + } else if (isOutsidePolytopeFace(polytope, f[1], &newv->v)) { + start_face = f[1]; + } else { + throw std::logic_error( + "FCL expandPolytope(): This should not happen. Both the nearest " + "point and the new vertex are on an edge, thus the nearest distance " + "should be 0. This is touching contact, and we should not expand the " + "polytope for touching contact."); + } + } - // remove triangle face - ccdPtDelFace(pt, (ccd_pt_face_t *)el); + std::unordered_set visible_faces; + std::unordered_set internal_edges; + std::unordered_set border_edges; + ComputeVisiblePatch(*polytope, *start_face, newv->v, &border_edges, + &visible_faces, &internal_edges); + + // Now remove all the obsolete faces. + // TODO(hongkai.dai@tri.global): currently we need to loop through each face + // in visible_faces, and then do a linear search in the list pt->faces to + // delete `face`. It would be better if we only loop through the list + // polytope->faces for once. Same for the edges. + for (const auto& f : visible_faces) { + ccdPtDelFace(polytope, f); + } - // expand triangle to tetrahedron - v[3] = ccdPtAddVertex(pt, newv); - e[3] = ccdPtAddEdge(pt, v[3], v[0]); - e[4] = ccdPtAddEdge(pt, v[3], v[1]); - e[5] = ccdPtAddEdge(pt, v[3], v[2]); + // Now remove all the obsolete edges. + for (const auto& e : internal_edges) { + ccdPtDelEdge(polytope, e); + } - if (ccdPtAddFace(pt, e[3], e[4], e[0]) == NULL - || ccdPtAddFace(pt, e[4], e[5], e[1]) == NULL - || ccdPtAddFace(pt, e[5], e[3], e[2]) == NULL){ - return -2; - } + // A vertex cannot be obsolete, since a vertex is always on the boundary of + // the Minkowski difference A ⊖ B. + // TODO(hongkai.dai@tri.global): as a sanity check, we should make sure that + // all vertices has at least one face/edge invisible from the new vertex + // `newv`. + + // Now add the new vertex. + ccd_pt_vertex_t* new_vertex = ccdPtAddVertex(polytope, newv); + + // Now add the new edges and faces, by connecting the new vertex with vertices + // on border_edges. map_vertex_to_new_edge maps a vertex on the silhouette + // edges to a new edge, with one end being the new vertex, and the other end + // being that vertex on the silhouette edges. + std::unordered_map map_vertex_to_new_edge; + for (const auto& border_edge : border_edges) { + ccd_pt_edge_t* e[2]; // The two new edges added by connecting new_vertex + // to the two vertices on border_edge. + for (int i = 0; i < 2; ++i) { + auto it = map_vertex_to_new_edge.find(border_edge->vertex[i]); + if (it == map_vertex_to_new_edge.end()) { + // This edge has not been added yet. + e[i] = ccdPtAddEdge(polytope, new_vertex, border_edge->vertex[i]); + map_vertex_to_new_edge.emplace_hint(it, border_edge->vertex[i], + e[i]); + } else { + e[i] = it->second; + } } + // Now add the face. + ccdPtAddFace(polytope, border_edge, e[0], e[1]); + } - return 0; + return 0; +} + +/** In each iteration of EPA algorithm, given the nearest point on the polytope + * boundary to the origin, a support direction will be computed, to find the + * support of the Minkowski difference A ⊖ B along that direction, so as to + * expand the polytope. + * @param polytope The polytope contained in A ⊖ B. + * @param nearest_feature The feature containing the nearest point on the + * boundary of the polytope to the origin. + * @retval dir The support direction along which to expand the polytope. Notice + * that dir is a normalized vector. + */ +static ccd_vec3_t supportEPADirection(const ccd_pt_t* polytope, + const ccd_pt_el_t* nearest_feature) { + /* + If we denote the nearest point as v, when v is not the origin, then the + support direction is v. If v is the origin, then v should be an interior + point on a face, and the support direction is the normal of that face, + pointing outward from the polytope. + */ + ccd_vec3_t dir; + if (ccdIsZero(nearest_feature->dist)) { + // nearest point is the origin. + switch (nearest_feature->type) { + case CCD_PT_VERTEX: { + throw std::logic_error( + "FCL supportEPADirection(): The nearest point to the origin is a " + "vertex of the polytope. This should be identified as a touching " + "contact, before calling function supportEPADirection()"); + } + case CCD_PT_EDGE: { + // When the nearest point is on an edge, the origin must be on that + // edge. The support direction could be in the range between + // edge.faces[0].normal and edge.faces[1].normal, where the face normals + // point outward from the polytope. In this implementation, we + // arbitrarily choose faces[0] normal. + const ccd_pt_edge_t* edge = + reinterpret_cast(nearest_feature); + dir = faceNormalPointingOutward(polytope, edge->faces[0]); + break; + } + case CCD_PT_FACE: { + // If origin is an interior point of a face, then choose the normal of + // that face as the sample direction. + const ccd_pt_face_t* face = + reinterpret_cast(nearest_feature); + dir = faceNormalPointingOutward(polytope, face); + break; + } + } + } else { + ccdVec3Copy(&dir, &(nearest_feature->witness)); + } + ccdVec3Normalize(&dir); + return dir; } /** Finds next support point (and stores it in out argument). - * Returns 0 on success, -1 otherwise */ -static int nextSupport(const void *obj1, const void *obj2, const ccd_t *ccd, - const ccd_pt_el_t *el, - ccd_support_t *out) -{ - ccd_vec3_t *a, *b, *c; - ccd_real_t dist; + * @param[in] polytope The current polytope contained inside the Minkowski + * difference A ⊖ B. + * @param[in] obj1 Geometric object A. + * @param[in] obj2 Geometric object B. + * @param[in] ccd The libccd solver. + * @param[in] el The polytope boundary feature that contains the point nearest + * to the origin. + * @param[out] out The next support point. + * @retval status If the next support point is found, then returns 0; otherwise + * returns -1. There are several reasons why the next support point is not + * found: + * 1. If the nearest point on the boundary of the polytope to the origin is a + * vertex of the polytope. Then we know the two objects A and B are in touching + * contact. + * 2. If the difference between the upper bound and lower bound of the distance + * is below sqrt(ccd->epa_tolerance), then we converge to a distance whose + * difference from the real distance is less than sqrt(ccd->epa_tolerance). + */ +static int nextSupport(const ccd_pt_t* polytope, const void* obj1, + const void* obj2, const ccd_t* ccd, + const ccd_pt_el_t* el, ccd_support_t* out) { + ccd_vec3_t *a, *b, *c; + ccd_real_t dist; - if (el->type == CCD_PT_VERTEX) - return -1; + if (el->type == CCD_PT_VERTEX) return -1; - // touch contact - if (ccdIsZero(el->dist)) - return -1; + const ccd_vec3_t dir = supportEPADirection(polytope, el); - __ccdSupport(obj1, obj2, &el->witness, ccd, out); + __ccdSupport(obj1, obj2, &dir, ccd, out); - // Compute dist of support point along element witness point direction - // so we can determine whether we expanded a polytope surrounding the - // origin a bit. - dist = ccdVec3Dot(&out->v, &el->witness); + // Compute distance of support point in the support direction, so we can + // determine whether we expanded a polytope surrounding the origin a bit. + dist = ccdVec3Dot(&out->v, &dir); - if (dist - el->dist < ccd->epa_tolerance) - return -1; + // el->dist is the squared distance from the feature "el" to the origin.. + // dist is an upper bound on the distance from the boundary of the Minkowski + // difference to the origin, and sqrt(el->dist) is a lower bound of that + // distance. + if (dist - std::sqrt(el->dist) < ccd->epa_tolerance) return -1; - if (el->type == CCD_PT_EDGE){ - // fetch end points of edge - ccdPtEdgeVec3((ccd_pt_edge_t *)el, &a, &b); + if (el->type == CCD_PT_EDGE) { + // fetch end points of edge + ccdPtEdgeVec3(reinterpret_cast(el), &a, &b); - // get distance from segment - dist = ccdVec3PointSegmentDist2(&out->v, a, b, NULL); - }else{ // el->type == CCD_PT_FACE - // fetch vertices of triangle face - ccdPtFaceVec3((ccd_pt_face_t *)el, &a, &b, &c); + // get distance from segment + dist = ccdVec3PointSegmentDist2(&out->v, a, b, NULL); + } else { // el->type == CCD_PT_FACE + // fetch vertices of triangle face + ccdPtFaceVec3(reinterpret_cast(el), &a, &b, &c); - // check if new point can significantly expand polytope - dist = ccdVec3PointTriDist2(&out->v, a, b, c, NULL); - } + // check if new point can significantly expand polytope + dist = ccdVec3PointTriDist2(&out->v, a, b, c, NULL); + } - if (dist < ccd->epa_tolerance) - return -1; + if (std::sqrt(dist) < ccd->epa_tolerance) return -1; - return 0; + return 0; } @@ -975,8 +1332,9 @@ static int __ccdEPA(const void *obj1, const void *obj2, size = ccdSimplexSize(simplex); if (size == 4){ ret = simplexToPolytope4(obj1, obj2, ccd, simplex, polytope, nearest); - }else if (size == 3){ - ret = simplexToPolytope3(obj1, obj2, ccd, simplex, polytope, nearest); + } else if (size == 3) { + ret = convert2SimplexToTetrahedron(obj1, obj2, ccd, simplex, polytope, + nearest); }else{ // size == 2 ret = simplexToPolytope2(obj1, obj2, ccd, simplex, polytope, nearest); } @@ -994,7 +1352,7 @@ static int __ccdEPA(const void *obj1, const void *obj2, *nearest = ccdPtNearest(polytope); // get next support point - if (nextSupport(obj1, obj2, ccd, *nearest, &supp) != 0) + if (nextSupport(polytope, obj1, obj2, ccd, *nearest, &supp) != 0) break; // expand nearest triangle using new point - supp @@ -1379,52 +1737,72 @@ static inline ccd_real_t _ccdDist(const void *obj1, const void *obj2, return -CCD_REAL(1.); } -static int penEPAPosCmp(const void *a, const void *b) -{ - ccd_pt_vertex_t *v1, *v2; - v1 = *(ccd_pt_vertex_t **)a; - v2 = *(ccd_pt_vertex_t **)b; - - if (ccdEq(v1->dist, v2->dist)){ - return 0; - }else if (v1->dist < v2->dist){ - return -1; - }else{ - return 1; - } -} - -static int penEPAPosClosest(const ccd_pt_t *pt, const ccd_pt_el_t *nearest, - ccd_vec3_t *p1, ccd_vec3_t* p2) -{ - FCL_UNUSED(nearest); - - ccd_pt_vertex_t *v; - ccd_pt_vertex_t **vs; - size_t i, len; - // compute median - len = 0; - ccdListForEachEntry(&pt->vertices, v, ccd_pt_vertex_t, list){ - len++; - } - - vs = CCD_ALLOC_ARR(ccd_pt_vertex_t*, len); - if (vs == NULL) - return -1; - - i = 0; - ccdListForEachEntry(&pt->vertices, v, ccd_pt_vertex_t, list){ - vs[i++] = v; +/** + * Given the nearest point on the polytope inside the Minkowski difference + * A ⊖ B, returns the point p1 on geometric object A and p2 on geometric object + * B, such that p1 is the deepest penetration point on A, and p2 is the deepest + * penetration point on B. + * @param[in] nearest The feature with the point that is nearest to the origin + * on the boundary of the polytope. + * @param[out] p1 the deepest penetration point on A, measured and expressed in + * the world frame. + * @param[out] p2 the deepest penetration point on B, measured and expressed in + * the world frame. + * @retval status Return 0 on success, and -1 on failure. + */ +static int penEPAPosClosest(const ccd_pt_el_t* nearest, ccd_vec3_t* p1, + ccd_vec3_t* p2) { + // We reconstruct the simplex on which the nearest point lives, and then + // compute the deepest penetration point on each geometric objects. Note that + // the reconstructed simplex has size up to 3 (at most 3 vertices). + if (nearest->type == CCD_PT_VERTEX) { + ccd_pt_vertex_t* v = (ccd_pt_vertex_t*)nearest; + ccdVec3Copy(p1, &v->v.v1); + ccdVec3Copy(p2, &v->v.v2); + return 0; + } else { + ccd_simplex_t s; + ccdSimplexInit(&s); + if (nearest->type == CCD_PT_EDGE) { + const ccd_pt_edge_t* e = reinterpret_cast(nearest); + ccdSimplexAdd(&s, &(e->vertex[0]->v)); + ccdSimplexAdd(&s, &(e->vertex[1]->v)); + } else if (nearest->type == CCD_PT_FACE) { + const ccd_pt_face_t* f = reinterpret_cast(nearest); + // The face triangle has three edges, each edge consists of two end + // points, so there are 6 end points in total, each vertex of the triangle + // appears twice among the 6 end points. We need to choose the three + // distinctive vertices out of these 6 end points. + // First we pick edge0, the two end points of edge0 are distinct. + ccdSimplexAdd(&s, &(f->edge[0]->vertex[0]->v)); + ccdSimplexAdd(&s, &(f->edge[0]->vertex[1]->v)); + // Next we pick edge1, one of the two end points on edge1 is distinct from + // the end points in edge0, we will add this distinct vertex to the + // simplex. + for (int i = 0; i < 2; ++i) { + ccd_pt_vertex_t* third_vertex = f->edge[1]->vertex[i]; + if (third_vertex != f->edge[0]->vertex[0] && + third_vertex != f->edge[0]->vertex[1]) { + ccdSimplexAdd(&s, &(third_vertex->v)); + break; + } + } + } else { + throw std::logic_error( + "FCL penEPAPosClosest(): Unsupported feature type. The closest point " + "should be either a vertex, on an edge, or on a face."); } - - qsort(vs, len, sizeof(ccd_pt_vertex_t*), penEPAPosCmp); - - ccdVec3Copy(p1, &vs[0]->v.v1); - ccdVec3Copy(p2, &vs[0]->v.v2); - - free(vs); - + // Now compute the closest point in the simplex. + // TODO(hongkai.dai@tri.global): we do not need to compute the closest point + // on the simplex, as that is already given in `nearest`. We only need to + // extract the deepest penetration points on each geometric object. + // Sean.Curtis@tri.global and I will refactor this code in the future, to + // avoid calling extractClosestPoints. + ccd_vec3_t p; + ccdVec3Copy(&p, &(nearest->witness)); + extractClosestPoints(&s, p1, p2, &p); return 0; + } } static inline ccd_real_t ccdGJKSignedDist(const void* obj1, const void* obj2, const ccd_t* ccd, ccd_vec3_t* p1, ccd_vec3_t* p2) @@ -1444,7 +1822,7 @@ static inline ccd_real_t ccdGJKSignedDist(const void* obj1, const void* obj2, co depth = -CCD_SQRT(nearest->dist); ccd_vec3_t pos1, pos2; - penEPAPosClosest(&polytope, nearest, &pos1, &pos2); + penEPAPosClosest(nearest, &pos1, &pos2); if (p1) *p1 = pos1; if (p2) *p2 = pos2; diff --git a/test/narrowphase/detail/convexity_based_algorithm/CMakeLists.txt b/test/narrowphase/detail/convexity_based_algorithm/CMakeLists.txt index 82f645866..f3a9465c3 100644 --- a/test/narrowphase/detail/convexity_based_algorithm/CMakeLists.txt +++ b/test/narrowphase/detail/convexity_based_algorithm/CMakeLists.txt @@ -1,6 +1,7 @@ set(tests - test_gjk_libccd-inl.cpp + test_gjk_libccd-inl_epa.cpp test_gjk_libccd-inl_extractClosestPoints.cpp + test_gjk_libccd-inl_signed_distance.cpp ) # Build all the tests diff --git a/test/narrowphase/detail/convexity_based_algorithm/test_gjk_libccd-inl.cpp b/test/narrowphase/detail/convexity_based_algorithm/test_gjk_libccd-inl.cpp deleted file mode 100644 index 6bc7bbdb3..000000000 --- a/test/narrowphase/detail/convexity_based_algorithm/test_gjk_libccd-inl.cpp +++ /dev/null @@ -1,163 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2018. 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 Hongkai Dai*/ -#include "fcl/narrowphase/detail/convexity_based_algorithm/gjk_libccd-inl.h" - -#include -#include - -#include "fcl/narrowphase/detail/gjk_solver_libccd.h" - -namespace fcl { -namespace detail { - -// Given two spheres, sphere 1 has radius1, and centered at point A, whose -// position is p_FA measured and expressed in frame F; sphere 2 has radius2, -// and centered at point B, whose position is p_FB measured and expressed in -// frame F. Computes the signed distance between the two spheres, together with -// the two closest points Na on sphere 1 and Nb on sphere 2, returns the -// position of Na and Nb expressed in frame F. -// We use the monogram notation on spatial vectors. The monogram notation is -// explained in -// http://drake.mit.edu/doxygen_cxx/group__multibody__spatial__pose.html -template -S ComputeSphereSphereDistance(S radius1, S radius2, const Vector3& p_FA, - const Vector3& p_FB, Vector3* p_FNa, - Vector3* p_FNb) { - S min_distance = (p_FA - p_FB).norm() - radius1 - radius2; - const Vector3 p_AB_F = - p_FB - p_FA; // The vector AB measured and expressed - // in frame F. - *p_FNa = p_FA + p_AB_F.normalized() * radius1; - *p_FNb = p_FB - p_AB_F.normalized() * radius2; - return min_distance; -} - -template -void TestSphereToSphereGJKSignedDistance(S radius1, S radius2, - const Vector3& p_FA, - const Vector3& p_FB, S tol, - S min_distance_expected, - const Vector3& p_FNa_expected, - const Vector3& p_FNb_expected) { - // Test if GJKSignedDistance computes the right distance. Here we used sphere - // to sphere as the geometries. The distance between sphere and sphere should - // be computed using distance between primitives, instead of the GJK - // algorithm. But here we choose spheres for simplicity. - - fcl::Sphere s1(radius1); - fcl::Sphere s2(radius2); - fcl::Transform3 tf1, tf2; - tf1.setIdentity(); - tf2.setIdentity(); - tf1.translation() = p_FA; - tf2.translation() = p_FB; - void* o1 = GJKInitializer>::createGJKObject(s1, tf1); - void* o2 = GJKInitializer>::createGJKObject(s2, tf2); - - S dist; - Vector3 p1, p2; - GJKSolver_libccd gjkSolver; - bool res = GJKSignedDistance( - o1, detail::GJKInitializer>::getSupportFunction(), o2, - detail::GJKInitializer>::getSupportFunction(), - gjkSolver.max_distance_iterations, gjkSolver.distance_tolerance, &dist, - &p1, &p2); - - EXPECT_EQ(res, min_distance_expected >= 0); - - EXPECT_NEAR(dist, min_distance_expected, tol); - EXPECT_TRUE(p1.isApprox(p_FNa_expected, tol)); - EXPECT_TRUE(p2.isApprox(p_FNb_expected, tol)); - - GJKInitializer>::deleteGJKObject(o1); - GJKInitializer>::deleteGJKObject(o2); -} - -template -struct SphereSpecification { - SphereSpecification(S radius_, const Vector3& center_) - : radius{radius_}, center{center_} {} - S radius; - Vector3 center; -}; - -template -void TestNonCollidingSphereGJKSignedDistance(S tol) { - std::vector> spheres; - spheres.emplace_back(0.5, Vector3(0, 0, -1.2)); - spheres.emplace_back(0.5, Vector3(1.25, 0, 0)); - spheres.emplace_back(0.3, Vector3(-0.2, 0, 0)); - spheres.emplace_back(0.4, Vector3(-0.2, 0, 1.1)); - for (int i = 0; i < static_cast(spheres.size()); ++i) { - for (int j = i + 1; j < static_cast(spheres.size()); ++j) { - if ((spheres[i].center - spheres[j].center).norm() > - spheres[i].radius + spheres[j].radius) { - // Not in collision. - Vector3 p_FNa, p_FNb; - const S min_distance_expected = ComputeSphereSphereDistance( - spheres[i].radius, spheres[j].radius, spheres[i].center, - spheres[j].center, &p_FNa, &p_FNb); - TestSphereToSphereGJKSignedDistance( - spheres[i].radius, spheres[j].radius, spheres[i].center, - spheres[j].center, tol, min_distance_expected, p_FNa, p_FNb); - } else { - GTEST_FAIL() << "The two spheres collide." - << "\nSpheres[" << i << "] with radius " - << spheres[i].radius << ", centered at " - << spheres[i].center.transpose() << "\nSpheres[" << j - << "] with radius " << spheres[j].radius - << ", centered at " << spheres[j].center.transpose() - << "\n"; - } - } - } -} - -GTEST_TEST(FCL_GJKSignedDistance, sphere_sphere) { - // TODO(hongkai.dai@tri.global): By setting gjkSolver.distance_tolerance to - // the default value (1E-6), the tolerance we get on the closest points are - // only up to 1E-3. Should investigate why there is such a big difference. - TestNonCollidingSphereGJKSignedDistance(1E-3); - TestNonCollidingSphereGJKSignedDistance(1E-3); -} -} // namespace detail -} // namespace fcl - -//============================================================================== -int main(int argc, char* argv[]) { - ::testing::InitGoogleTest(&argc, argv); - return RUN_ALL_TESTS(); -} diff --git a/test/narrowphase/detail/convexity_based_algorithm/test_gjk_libccd-inl_epa.cpp b/test/narrowphase/detail/convexity_based_algorithm/test_gjk_libccd-inl_epa.cpp new file mode 100644 index 000000000..6a9afcba0 --- /dev/null +++ b/test/narrowphase/detail/convexity_based_algorithm/test_gjk_libccd-inl_epa.cpp @@ -0,0 +1,1279 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018. 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 Hongkai Dai (hongkai.dai@tri.global) */ + +/** Tests the Expanded Polytope Algorithm (EPA) implementation inside FCL. EPA + * computes the penetration depth and the points with the deepest penetration + * between two convex objects. + */ + +#include "fcl/narrowphase/detail/convexity_based_algorithm/gjk_libccd-inl.h" + +#include + +#include + +#include "fcl/narrowphase/detail/convexity_based_algorithm/polytope.h" + +namespace fcl { +namespace detail { + +class Polytope { + public: + Polytope() { + polytope_ = new ccd_pt_t; + ccdPtInit(polytope_); + } + + ~Polytope() { + // ccdPtDestroy() destroys the vertices, edges, and faces, contained in the + // polytope, allowing the polytope itself to be subsequently deleted. + ccdPtDestroy(polytope_); + delete polytope_; + } + + ccd_pt_vertex_t& v(int i) { return *v_[i]; } + + ccd_pt_edge_t& e(int i) { return *e_[i]; } + + ccd_pt_face_t& f(int i) { return *f_[i]; } + + ccd_pt_t& polytope() { return *polytope_; } + + const ccd_pt_vertex_t& v(int i) const { return *v_[i]; } + + const ccd_pt_edge_t& e(int i) const { return *e_[i]; } + + const ccd_pt_face_t& f(int i) const { return *f_[i]; } + + const ccd_pt_t& polytope() const { return *polytope_; } + + protected: + std::vector& v() { return v_; } + std::vector& e() { return e_; } + std::vector& f() { return f_; } + + private: + std::vector v_; + std::vector e_; + std::vector f_; + ccd_pt_t* polytope_; +}; + +/** + Simple equilateral tetrahedron. + +Geometrically, its edge lengths are the given length (default to unit length). +Its "bottom" face is parallel with the z = 0 plane. It's default configuration +places the bottom face *on* the z = 0 plane with the origin contained in the +bottom face. + +In representation, the edge ordering is arbitrary (i.e., an edge can be defined +as (vᵢ, vⱼ) or (vⱼ, vᵢ). However, given an arbitrary definition of edges, the +*faces* have been defined to have a specific winding which causes e₀ × e₁ to +point inwards or outwards for that face. This allows us to explicitly fully +exercise the functionality for computing an outward normal. + - face 0: points outward + - face 1: points inward (requires flipping) + - face 2: points inward (requires flipping) + - face 3: points outward + +All property accessors are *mutable*. +*/ +class EquilateralTetrahedron : public Polytope { + public: + EquilateralTetrahedron(ccd_real_t bottom_center_x = 0, + ccd_real_t bottom_center_y = 0, + ccd_real_t bottom_center_z = 0, + ccd_real_t edge_length = 1) + : Polytope() { + v().resize(4); + e().resize(6); + f().resize(4); + auto AddTetrahedronVertex = [bottom_center_x, bottom_center_y, + bottom_center_z, edge_length, this]( + ccd_real_t x, ccd_real_t y, ccd_real_t z) { + return ccdPtAddVertexCoords( + &this->polytope(), x * edge_length + bottom_center_x, + y * edge_length + bottom_center_y, z * edge_length + bottom_center_z); + }; + v()[0] = AddTetrahedronVertex(0.5, -0.5 / std::sqrt(3), 0); + v()[1] = AddTetrahedronVertex(-0.5, -0.5 / std::sqrt(3), 0); + v()[2] = AddTetrahedronVertex(0, 1 / std::sqrt(3), 0); + v()[3] = AddTetrahedronVertex(0, 0, std::sqrt(2.0 / 3.0)); + e()[0] = ccdPtAddEdge(&polytope(), &v(0), &v(1)); + e()[1] = ccdPtAddEdge(&polytope(), &v(1), &v(2)); + e()[2] = ccdPtAddEdge(&polytope(), &v(2), &v(0)); + e()[3] = ccdPtAddEdge(&polytope(), &v(0), &v(3)); + e()[4] = ccdPtAddEdge(&polytope(), &v(1), &v(3)); + e()[5] = ccdPtAddEdge(&polytope(), &v(2), &v(3)); + f()[0] = ccdPtAddFace(&polytope(), &e(0), &e(1), &e(2)); + f()[1] = ccdPtAddFace(&polytope(), &e(0), &e(3), &e(4)); + f()[2] = ccdPtAddFace(&polytope(), &e(1), &e(4), &e(5)); + f()[3] = ccdPtAddFace(&polytope(), &e(3), &e(5), &e(2)); + } +}; + +GTEST_TEST(FCL_GJK_EPA, faceNormalPointingOutward) { + // Construct a equilateral tetrahedron, compute the normal on each face. + auto CheckTetrahedronFaceNormal = [](const EquilateralTetrahedron& p) { + for (int i = 0; i < 4; ++i) { + const ccd_vec3_t n = + libccd_extension::faceNormalPointingOutward(&p.polytope(), &p.f(i)); + for (int j = 0; j < 4; ++j) { + EXPECT_LE(ccdVec3Dot(&n, &p.v(j).v.v), + ccdVec3Dot(&n, &p.f(i).edge[0]->vertex[0]->v.v) + + constants::eps_34()); + } + } + }; + /* + p1-p4: The tetrahedron is positioned so that the origin is placed on each + face (some requiring flipping, some not) + p5: Origin is well within + p6: Origin on the bottom face, but the tetrahedron is too small; it must + evaluate all vertices and do a min/max comparison. + p7: Small tetrahedron with origin properly inside. + p8: Origin on the side face. + We do not test the case that the origin is on a vertex of the polytope. When + the origin coincides with a vertex, the two objects are touching, and we do + not need to call faceNormalPointOutward function to compute the direction + along which the polytope is expanded. + */ + EquilateralTetrahedron p1; + CheckTetrahedronFaceNormal(p1); + // Origin on the plane, and requires flipping the direction. + EquilateralTetrahedron p2(1.0 / 6, -1.0 / (6 * std::sqrt(3)), + -std::sqrt(6) / 9); + CheckTetrahedronFaceNormal(p2); + EquilateralTetrahedron p3(0, 1.0 / (3 * std::sqrt(3)), -std::sqrt(6) / 9); + CheckTetrahedronFaceNormal(p3); + EquilateralTetrahedron p4(-1.0 / 6, -1.0 / (6 * std::sqrt(3)), + -std::sqrt(6) / 9); + CheckTetrahedronFaceNormal(p4); + + // Check when the origin is within the polytope. + EquilateralTetrahedron p5(0, 0, -0.1); + CheckTetrahedronFaceNormal(p5); + + // Small tetrahedrons. + EquilateralTetrahedron p6(0, 0, 0, 0.01); + CheckTetrahedronFaceNormal(p6); + EquilateralTetrahedron p7(0, 0, -0.002, 0.01); + CheckTetrahedronFaceNormal(p7); + EquilateralTetrahedron p8(0, 0.01 / (3 * std::sqrt(3)), + -0.01 * std::sqrt(6) / 9, 0.01); + CheckTetrahedronFaceNormal(p8); +} + +GTEST_TEST(FCL_GJK_EPA, supportEPADirection) { + auto CheckSupportEPADirection = []( + const ccd_pt_t* polytope, const ccd_pt_el_t* nearest_pt, + const Eigen::Ref>& dir_expected, + ccd_real_t tol) { + const ccd_vec3_t dir = + libccd_extension::supportEPADirection(polytope, nearest_pt); + for (int i = 0; i < 3; ++i) { + EXPECT_NEAR(dir.v[i], dir_expected(i), tol); + } + }; + + // Nearest point is on the bottom triangle. + // The sampled direction should be -z unit vector. + EquilateralTetrahedron p1(0, 0, -0.1); + // The computation on Mac is very imprecise, thus the tolerance is big. + // TODO(hongkai.dai@tri.global): this tolerance should be cranked up once + // #291 is resolved. + const ccd_real_t tol = 3E-5; + CheckSupportEPADirection(&p1.polytope(), + reinterpret_cast(&p1.f(0)), + Vector3(0, 0, -1), tol); + // Nearest point is on an edge, as the origin is on an edge. + EquilateralTetrahedron p2(0, 0.5 / std::sqrt(3), 0); + // e(0) has two neighbouring faces, f(0) and f(1). The support direction could + // be the normal direction of either face. + if (p2.e(0).faces[0] == &p2.f(0)) { + // Check the support direction, should be the normal direction of f(0). + CheckSupportEPADirection(&p2.polytope(), + reinterpret_cast(&p2.e(0)), + Vector3(0, 0, -1), tol); + } else { + // The support direction should be the normal direction of f(1) + CheckSupportEPADirection( + &p2.polytope(), reinterpret_cast(&p2.e(0)), + Vector3(0, -2 * std::sqrt(2) / 3, 1.0 / 3), tol); + } + // Nearest point is on a vertex, should throw an error. + EquilateralTetrahedron p3(-0.5, 0.5 / std::sqrt(3), 0); + EXPECT_THROW( + libccd_extension::supportEPADirection( + &p3.polytope(), reinterpret_cast(&p3.v(0))), + std::logic_error); + + // Origin is an internal point of the bottom triangle + EquilateralTetrahedron p4(0, 0, 0); + CheckSupportEPADirection(&p4.polytope(), + reinterpret_cast(&p4.f(0)), + Vector3(0, 0, -1), tol); + + // Nearest point is on face(1) + EquilateralTetrahedron p5(0, 1 / (3 * std::sqrt(3)), + -std::sqrt(6) / 9 + 0.01); + CheckSupportEPADirection( + &p5.polytope(), reinterpret_cast(&p5.f(1)), + Vector3(0, -2 * std::sqrt(2) / 3, 1.0 / 3), tol); +} + +GTEST_TEST(FCL_GJK_EPA, isOutsidePolytopeFace) { + EquilateralTetrahedron p; + + auto CheckPointOutsidePolytopeFace = [&p](ccd_real_t x, ccd_real_t y, + ccd_real_t z, int face_index, + bool is_outside_expected) { + ccd_vec3_t pt; + pt.v[0] = x; + pt.v[1] = y; + pt.v[2] = z; + EXPECT_EQ(libccd_extension::isOutsidePolytopeFace(&p.polytope(), + &p.f(face_index), &pt), + is_outside_expected); + }; + + const bool expect_inside = false; + const bool expect_outside = true; + // point (0, 0, 0.1) is inside the tetrahedron. + CheckPointOutsidePolytopeFace(0, 0, 0.1, 0, expect_inside); + CheckPointOutsidePolytopeFace(0, 0, 0.1, 1, expect_inside); + CheckPointOutsidePolytopeFace(0, 0, 0.1, 2, expect_inside); + CheckPointOutsidePolytopeFace(0, 0, 0.1, 3, expect_inside); + + // point(0, 0, 2) is outside the tetrahedron. But it is on the "inner" side + // of the bottom face. + CheckPointOutsidePolytopeFace(0, 0, 2, 0, expect_inside); + CheckPointOutsidePolytopeFace(0, 0, 2, 1, expect_outside); + CheckPointOutsidePolytopeFace(0, 0, 2, 2, expect_outside); + CheckPointOutsidePolytopeFace(0, 0, 2, 3, expect_outside); + + // point (0, 0, 0) is right on the bottom face. + CheckPointOutsidePolytopeFace(0, 0, 0, 0, expect_inside); + CheckPointOutsidePolytopeFace(0, 0, 0, 1, expect_inside); + CheckPointOutsidePolytopeFace(0, 0, 0, 2, expect_inside); + CheckPointOutsidePolytopeFace(0, 0, 0, 3, expect_inside); +} + +// Construct a polytope with the following shape, namely an equilateral triangle +// on the top, and an equilateral triangle of the same size, but rotate by 60 +// degrees on the bottom. We will then connect the vertices of the equilateral +// triangles to form a convex polytope. +// v₄ +// v₃__╱╲__v₅ +// ╲╱ ╲╱ +// ╱____╲ +// v₂ ╲╱ v₀ +// v₁ +// The edges are in this order connected in a counter-clockwise order. +// e(0) connects v(0) and v(2). +// e(1) connects v(2) and v(4). +// e(2) connects v(4) and v(0). +// e(3) connects v(1) and v(3). +// e(4) connects v(3) and v(5). +// e(5) connects v(5) and v(1). +// e(6 + i) connects v(i) and v(i + 1). +// f(0) is the upper triangle. +// f(1) is the lower triangle. +// f(2 + i) is the triangle that connects v(i), v(i + 1) and v(i + 2), namely +// the triangles on the side. +// For each face, the edge e(0).cross(e(1)) has the following direction +// f(0) points inward. +// f(1) points outward. +// f(2) points inward. +// f(3) points outward. +// f(4) points inward. +// f(5) points outward. +// f(6) points inward +// f(7) points outward. +class Hexagram : public Polytope { + public: + Hexagram(ccd_real_t bottom_center_x = 0, ccd_real_t bottom_center_y = 0, + ccd_real_t bottom_center_z = 0) + : Polytope() { + v().resize(6); + e().resize(12); + f().resize(8); + auto AddHexagramVertex = [bottom_center_x, bottom_center_y, bottom_center_z, + this](ccd_real_t x, ccd_real_t y, ccd_real_t z) { + return ccdPtAddVertexCoords(&this->polytope(), x + bottom_center_x, + y + bottom_center_y, z + bottom_center_z); + }; + // right corner of upper triangle + v()[0] = AddHexagramVertex(0.5, -1 / std::sqrt(3), 1); + // bottom corner of lower triangle + v()[1] = AddHexagramVertex(0, -2 / std::sqrt(3), 0); + // left corner of upper triangle + v()[2] = AddHexagramVertex(-0.5, -1 / std::sqrt(3), 1); + // left corner of lower triangle + v()[3] = AddHexagramVertex(-0.5, 1 / std::sqrt(3), 0); + // top corner of upper triangle + v()[4] = AddHexagramVertex(0, 2 / std::sqrt(3), 1); + // right corner of lower triangle + v()[5] = AddHexagramVertex(0.5, 1 / std::sqrt(3), 0); + + // edges on the upper triangle + e()[0] = ccdPtAddEdge(&polytope(), &v(0), &v(2)); + e()[1] = ccdPtAddEdge(&polytope(), &v(2), &v(4)); + e()[2] = ccdPtAddEdge(&polytope(), &v(4), &v(0)); + // edges on the lower triangle + e()[3] = ccdPtAddEdge(&polytope(), &v(1), &v(3)); + e()[4] = ccdPtAddEdge(&polytope(), &v(3), &v(5)); + e()[5] = ccdPtAddEdge(&polytope(), &v(5), &v(1)); + // edges connecting the upper triangle to the lower triangle + for (int i = 0; i < 6; ++i) { + e()[6 + i] = ccdPtAddEdge(&polytope(), &v(i), &v((i + 1) % 6)); + } + + // upper triangle + f()[0] = ccdPtAddFace(&polytope(), &e(0), &e(1), &e(2)); + // lower triangle + f()[1] = ccdPtAddFace(&polytope(), &e(3), &e(4), &e(5)); + // triangles on the side + f()[2] = ccdPtAddFace(&polytope(), &e(0), &e(7), &e(6)); + f()[3] = ccdPtAddFace(&polytope(), &e(7), &e(8), &e(3)); + f()[4] = ccdPtAddFace(&polytope(), &e(8), &e(9), &e(1)); + f()[5] = ccdPtAddFace(&polytope(), &e(9), &e(10), &e(4)); + f()[6] = ccdPtAddFace(&polytope(), &e(10), &e(11), &e(2)); + f()[7] = ccdPtAddFace(&polytope(), &e(11), &e(6), &e(5)); + } +}; + +template +bool IsElementInSet(const std::unordered_set& S, const T* element) { + return S.count(const_cast(element)) > 0; +} + +// @param border_edge_indices_expected +// polytope.e(border_edge_indices_expected(i)) is a border edge. Similarly for +// visible_face_indices_expected and internal_edges_indices_expected. +void CheckComputeVisiblePatchCommon( + const Polytope& polytope, + const std::unordered_set& border_edges, + const std::unordered_set& visible_faces, + const std::unordered_set internal_edges, + const std::unordered_set& border_edge_indices_expected, + const std::unordered_set& visible_face_indices_expected, + const std::unordered_set internal_edges_indices_expected) { + // Check border_edges + EXPECT_EQ(border_edges.size(), border_edge_indices_expected.size()); + for (const int edge_index : border_edge_indices_expected) { + EXPECT_TRUE(IsElementInSet(border_edges, &polytope.e(edge_index))); + } + // Check visible_faces + EXPECT_EQ(visible_faces.size(), visible_face_indices_expected.size()); + for (const int face_index : visible_face_indices_expected) { + EXPECT_TRUE(IsElementInSet(visible_faces, &polytope.f(face_index))); + } + // Check internal_edges + EXPECT_EQ(internal_edges.size(), internal_edges_indices_expected.size()); + for (const auto edge_index : internal_edges_indices_expected) { + EXPECT_TRUE(IsElementInSet(internal_edges, &polytope.e(edge_index))); + } +} + +// @param edge_indices we will call ComputeVisiblePatchRecursive(polytope, face, +// edge_index, new_vertex, ...) for each edge_index in edge_indices. Namely we +// will compute the visible patches, starting from face.e(edge_index). +void CheckComputeVisiblePatchRecursive( + const Polytope& polytope, ccd_pt_face_t& face, + const std::vector& edge_indices, const ccd_vec3_t& new_vertex, + const std::unordered_set& border_edge_indices_expected, + const std::unordered_set& visible_face_indices_expected, + const std::unordered_set& internal_edges_indices_expected) { + std::unordered_set border_edges; + std::unordered_set visible_faces; + visible_faces.insert(&face); + std::unordered_set internal_edges; + for (const int edge_index : edge_indices) { + libccd_extension::ComputeVisiblePatchRecursive( + polytope.polytope(), face, edge_index, new_vertex, &border_edges, + &visible_faces, &internal_edges); + } + CheckComputeVisiblePatchCommon(polytope, border_edges, visible_faces, + internal_edges, border_edge_indices_expected, + visible_face_indices_expected, + internal_edges_indices_expected); +} + +void CheckComputeVisiblePatch( + const Polytope& polytope, ccd_pt_face_t& face, const ccd_vec3_t& new_vertex, + const std::unordered_set& border_edge_indices_expected, + const std::unordered_set& visible_face_indices_expected, + const std::unordered_set& internal_edges_indices_expected) { + std::unordered_set border_edges; + std::unordered_set visible_faces; + std::unordered_set internal_edges; + libccd_extension::ComputeVisiblePatch(polytope.polytope(), face, new_vertex, + &border_edges, &visible_faces, + &internal_edges); + + CheckComputeVisiblePatchCommon(polytope, border_edges, visible_faces, + internal_edges, border_edge_indices_expected, + visible_face_indices_expected, + internal_edges_indices_expected); +} + +GTEST_TEST(FCL_GJK_EPA, ComputeVisiblePatch_TopFaceVisible) { + // 1 visible face. + Hexagram hex; + // Point P is just slightly above the top triangle. Only the top triangle can + // be seen from point P. + ccd_vec3_t p; + p.v[0] = 0; + p.v[1] = 0; + p.v[2] = 1.1; + const std::unordered_set empty_set; + // Test recursive implementation. + CheckComputeVisiblePatchRecursive(hex, hex.f(0), {0}, p, {0}, {0}, empty_set); + + // Test ComputeVisiblePatch. + CheckComputeVisiblePatch(hex, hex.f(0), p, {0, 1, 2}, {0}, empty_set); +} + +GTEST_TEST(FCL_GJK_EPA, ComputeVisiblePatch_4FacesVisible) { + // 4 visible faces. + Hexagram hex; + // Point P is just above the top triangle by a certain height, such that it + // can see the triangles on the side, which connects two vertices on the upper + // triangle, and one vertex on the lower triangle. + ccd_vec3_t p; + p.v[0] = 0; + p.v[1] = 0; + p.v[2] = 2.1; + // Test recursive implementation. + CheckComputeVisiblePatchRecursive(hex, hex.f(0), {0}, p, {6, 7}, {0, 2}, {0}); + + // Test ComputeVisiblePatch. + CheckComputeVisiblePatch(hex, hex.f(0), p, {6, 7, 8, 9, 10, 11}, {0, 2, 4, 6}, + {0, 1, 2}); +} + +GTEST_TEST(FCL_GJK_EPA, ComputeVisiblePatch_TopAndSideFacesVisible) { + // 2 visible faces. + Hexagram hex; + // Point P is just outside the upper triangle (face0) and the triangle face2, + // it can see both face0 and face2, but not the other triangles. + ccd_vec3_t p; + p.v[0] = 0; + p.v[1] = -1 / std::sqrt(3) - 0.1; + p.v[2] = 1.1; + CheckComputeVisiblePatchRecursive(hex, hex.f(0), {0}, p, {6, 7}, {0, 2}, {0}); + + CheckComputeVisiblePatch(hex, hex.f(0), p, {1, 2, 6, 7}, {0, 2}, {0}); +} + +GTEST_TEST(FCL_GJK_EPA, ComputeVisiblePatch_2FacesVisible) { + // Test with the equilateral tetrahedron. + // Point P is outside of an edge on the bottom triangle. It can see both faces + // neighbouring that edge. + + EquilateralTetrahedron tetrahedron(0, 0, -0.1); + ccd_vec3_t p; + p.v[0] = 0; + p.v[1] = -1 / std::sqrt(3) - 0.1; + p.v[2] = -0.2; + + // Start with from face 0. + CheckComputeVisiblePatch(tetrahedron, tetrahedron.f(0), p, {1, 2, 3, 4}, + {0, 1}, {0}); + + // Start with from face 1. + CheckComputeVisiblePatch(tetrahedron, tetrahedron.f(1), p, {1, 2, 3, 4}, + {0, 1}, {0}); +} + +// Returns true if the the distance difference between the two vertices are +// below tol. +bool VertexPositionCoincide(const ccd_pt_vertex_t* v1, + const ccd_pt_vertex_t* v2, ccd_real_t tol) { + return ccdVec3Dist2(&v1->v.v, &v2->v.v) < tol * tol; +} + +// Return true, if the vertices in e1 are all mapped to the vertices in e2, +// according to the mapping @p map_v1_to_v2. +bool EdgeMatch(const ccd_pt_edge_t* e1, const ccd_pt_edge_t* e2, + const std::unordered_map& + map_v1_to_v2) { + ccd_pt_vertex_t* v2_expected[2]; + for (int i = 0; i < 2; ++i) { + auto it = map_v1_to_v2.find(e1->vertex[i]); + if (it == map_v1_to_v2.end()) { + throw std::logic_error("vertex[" + std::to_string(i) + + "] in e1 is not found in map_v1_to_v2"); + } + v2_expected[i] = it->second; + } + return (v2_expected[0] == e2->vertex[0] && v2_expected[1] == e2->vertex[1]) || + (v2_expected[0] == e2->vertex[1] && v2_expected[1] == e2->vertex[0]); +} + +// Return true, if the edges in f1 are all mapped to the edges in f2, according +// to the mapping @p map_e1_to_e2. +bool TriangleMatch( + const ccd_pt_face_t* f1, const ccd_pt_face_t* f2, + const std::unordered_map& map_e1_to_e2) { + std::unordered_set e2_expected; + for (int i = 0; i < 3; ++i) { + auto it = map_e1_to_e2.find(f1->edge[i]); + if (it == map_e1_to_e2.end()) { + throw std::logic_error("edge[" + std::to_string(i) + + "] in f1 is not found in map_e1_to_e2"); + } + e2_expected.insert(it->second); + } + // The edges in f1 have to be distinct. + EXPECT_EQ(e2_expected.size(), 3u); + for (int i = 0; i < 3; ++i) { + auto it = e2_expected.find(f2->edge[i]); + if (it == e2_expected.end()) { + return false; + } + } + return true; +} + +// Construct the mapping from feature1_list to feature2_list. There should be a +// one-to-one correspondence between feature1_list and feature2_list. +// @param feature1_list[in] A list of features to be mapped from. +// @param feature2_list[in] A list of features to be mapped to. +// @param cmp_feature[in] Returns true if two features are identical, otherwise +// returns false. +// @param feature1[out] The set of features in feature1_list. +// @param feature2[out] The set of features in feature2_list. +// @param map_feature1_to_feature2[out] Maps a feature in feature1_list to +// a feature in feature2_list. +// @note The features in feature1_list should be unique, so are in +// feature2_list. +template +void MapFeature1ToFeature2( + const ccd_list_t* feature1_list, const ccd_list_t* feature2_list, + std::function cmp_feature, + std::unordered_set* feature1, std::unordered_set* feature2, + std::unordered_map* map_feature1_to_feature2) { + feature1->clear(); + feature2->clear(); + map_feature1_to_feature2->clear(); + T* f; + ccdListForEachEntry(feature1_list, f, T, list) { + auto it = feature1->find(f); + assert(it == feature1->end()); + feature1->emplace_hint(it, f); + } + ccdListForEachEntry(feature2_list, f, T, list) { + auto it = feature2->find(f); + assert(it == feature2->end()); + feature2->emplace_hint(it, f); + } + EXPECT_EQ(feature1->size(), feature2->size()); + for (const auto& f1 : *feature1) { + bool found_match = false; + for (const auto& f2 : *feature2) { + if (cmp_feature(f1, f2)) { + if (!found_match) { + map_feature1_to_feature2->emplace_hint( + map_feature1_to_feature2->end(), f1, f2); + found_match = true; + } else { + GTEST_FAIL() << "There should be only one element in feature2_list " + "that matches with an element in feature1_list."; + } + } + } + EXPECT_TRUE(found_match); + } + // Every feature in feature1_list should be matched to a feature in + // feature2_list. + EXPECT_EQ(map_feature1_to_feature2->size(), feature1->size()); +} + +void ComparePolytope(const ccd_pt_t* polytope1, const ccd_pt_t* polytope2, + ccd_real_t tol) { + // Build the mapping between the vertices in polytope1 to the vertices in + // polytope2. + std::unordered_set v1_set, v2_set; + std::unordered_map map_v1_to_v2; + MapFeature1ToFeature2( + &polytope1->vertices, &polytope2->vertices, + [tol](const ccd_pt_vertex_t* v1, const ccd_pt_vertex_t* v2) { + return VertexPositionCoincide(v1, v2, tol); + }, + &v1_set, &v2_set, &map_v1_to_v2); + + // Build the mapping between the edges in polytope1 to the edges in polytope2. + std::unordered_set e1_set, e2_set; + std::unordered_map map_e1_to_e2; + MapFeature1ToFeature2( + &polytope1->edges, &polytope2->edges, + [map_v1_to_v2](const ccd_pt_edge_t* e1, const ccd_pt_edge_t* e2) { + return EdgeMatch(e1, e2, map_v1_to_v2); + }, + &e1_set, &e2_set, &map_e1_to_e2); + + // Build the mapping between the faces in polytope1 to the faces in polytope2. + std::unordered_set f1_set, f2_set; + std::unordered_map map_f1_to_f2; + MapFeature1ToFeature2( + &polytope1->faces, &polytope2->faces, + [map_e1_to_e2](const ccd_pt_face_t* f1, const ccd_pt_face_t* f2) { + return TriangleMatch(f1, f2, map_e1_to_e2); + }, + &f1_set, &f2_set, &map_f1_to_f2); + + /* TODO(hongkai.dai@tri.global): enable the following check, when issue + https://github.com/danfis/libccd/issues/46 has been fixed. Currently + ccd_pt_vertex_t.edges are garbage. + // Now make sure that the edges connected to a vertex in polytope 1, are the + // same edges connected to the corresponding vertex in polytope 2. + for (const auto& v1 : v1_set) { + auto v2 = map_v1_to_v2[v1]; + std::unordered_set v1_edges, v2_edges; + ccd_pt_edge_t* e; + ccdListForEachEntry(&v1->edges, e, ccd_pt_edge_t, list) { + v1_edges.insert(e); + } + ccdListForEachEntry(&v2->edges, e, ccd_pt_edge_t, list) { + v2_edges.insert(e); + } + EXPECT_EQ(v1_edges.size(), v2_edges.size()); + // Now check for each edge connecting to v1, the corresponding edge is + // connected to v2. + for (const auto& v1_e : v1_edges) { + auto it = map_e1_to_e2.find(v1_e); + EXPECT_NE(it, map_e1_to_e2.end()) { + auto v2_e = it->second; + EXPECT_NE(v2_edges.find(v2_e), v2_edges.end()); + } + }*/ + + // Make sure that the faces connected to each edge in polytope 1, are the same + // face connected to the corresponding face in polytope 2. + for (const auto& e1 : e1_set) { + auto e2 = map_e1_to_e2[e1]; + ccd_pt_face_t* f2_expected[2]; + for (int i = 0; i < 2; ++i) { + f2_expected[i] = map_f1_to_f2[e1->faces[i]]; + } + EXPECT_TRUE( + (f2_expected[0] == e2->faces[0] && f2_expected[1] == e2->faces[1]) || + (f2_expected[0] == e2->faces[1] && f2_expected[1] == e2->faces[0])); + } +} + +GTEST_TEST(FCL_GJK_EPA, expandPolytope_tetrahedron1) { + // Expand the equilateral tetrahedron by adding a point just outside one of + // the triangle face. That nearest triangle face will be deleted, and the + // three new faces will be added, by connecting the new vertex with the three + // vertices on the removed face. + EquilateralTetrahedron polytope(0, 0, -0.1); + // nearest point is on the bottom triangle + ccd_support_t newv; + newv.v.v[0] = 0; + newv.v.v[1] = 0; + newv.v.v[2] = -0.2; + + const int result = libccd_extension::expandPolytope( + &polytope.polytope(), reinterpret_cast(&polytope.f(0)), + &newv); + EXPECT_EQ(result, 0); + + // Construct the expanded polytope manually. + EquilateralTetrahedron tetrahedron(0, 0, -0.1); + ccd_pt_t& polytope_expected = tetrahedron.polytope(); + // The bottom face is removed. + ccdPtDelFace(&polytope_expected, &tetrahedron.f(0)); + // Insert the vertex. + ccd_pt_vertex_t* new_vertex = + ccdPtAddVertexCoords(&polytope_expected, 0, 0, -0.2); + // Add new edges. + ccd_pt_edge_t* new_edges[3]; + for (int i = 0; i < 3; ++i) { + new_edges[i] = + ccdPtAddEdge(&polytope_expected, new_vertex, &tetrahedron.v(i)); + } + // Add new faces. + ccdPtAddFace(&polytope_expected, &tetrahedron.e(0), new_edges[0], + new_edges[1]); + ccdPtAddFace(&polytope_expected, &tetrahedron.e(1), new_edges[1], + new_edges[2]); + ccdPtAddFace(&polytope_expected, &tetrahedron.e(2), new_edges[2], + new_edges[0]); + + ComparePolytope(&polytope.polytope(), &polytope_expected, + constants::eps_34()); +} + +GTEST_TEST(FCL_GJK_EPA, expandPolytope_tetrahedron_2visible_faces) { + // Expand the equilateral tetrahedron by adding a point just outside one edge. + // The two neighbouring faces of that edge will be deleted. Four new faces + // will be added, by connecting the new vertex with the remaining vertex on + // the two removed faces, that is opposite to the removed edge. + EquilateralTetrahedron polytope(0, 0, -0.1); + // nearest point is on the bottom triangle + ccd_support_t newv; + newv.v.v[0] = 0; + newv.v.v[1] = -0.5 / std::sqrt(3) - 0.1; + newv.v.v[2] = -0.2; + + const int result = libccd_extension::expandPolytope( + &polytope.polytope(), reinterpret_cast(&polytope.e(0)), + &newv); + EXPECT_EQ(result, 0); + + // Construct the expanded polytope manually. + EquilateralTetrahedron tetrahedron(0, 0, -0.1); + ccd_pt_t& polytope_expected = tetrahedron.polytope(); + // The bottom face is removed. + ccdPtDelFace(&polytope_expected, &tetrahedron.f(0)); + // The other face that neighbours with f(0) is removed. + ccdPtDelFace(&polytope_expected, &tetrahedron.f(1)); + // The nearest edge is removed. + ccdPtDelEdge(&polytope_expected, &tetrahedron.e(0)); + // Insert the vertex. + ccd_pt_vertex_t* new_vertex = ccdPtAddVertexCoords( + &polytope_expected, newv.v.v[0], newv.v.v[1], newv.v.v[2]); + // Add new edges. + ccd_pt_edge_t* new_edges[4]; + new_edges[0] = + ccdPtAddEdge(&polytope_expected, new_vertex, &tetrahedron.v(0)); + new_edges[1] = + ccdPtAddEdge(&polytope_expected, new_vertex, &tetrahedron.v(1)); + new_edges[2] = + ccdPtAddEdge(&polytope_expected, new_vertex, &tetrahedron.v(2)); + new_edges[3] = + ccdPtAddEdge(&polytope_expected, new_vertex, &tetrahedron.v(3)); + // Add new faces. + ccdPtAddFace(&polytope_expected, &tetrahedron.e(3), new_edges[0], + new_edges[3]); + ccdPtAddFace(&polytope_expected, &tetrahedron.e(2), new_edges[0], + new_edges[2]); + ccdPtAddFace(&polytope_expected, &tetrahedron.e(4), new_edges[1], + new_edges[3]); + ccdPtAddFace(&polytope_expected, &tetrahedron.e(1), new_edges[1], + new_edges[2]); + + ComparePolytope(&polytope.polytope(), &polytope_expected, + constants::eps_34()); +} + +GTEST_TEST(FCL_GJK_EPA, expandPolytope_hexagram_1visible_face) { + // Expand the Hexagram by adding a point just above the upper triangle. + // The upper triangle will be deleted. Three new faces will be added, by + // connecting the new vertex with the three vertices of the removed triangle. + Hexagram hex(0, 0, -0.9); + // nearest point is on the top triangle + ccd_support_t newv; + newv.v.v[0] = 0; + newv.v.v[1] = 0; + newv.v.v[2] = 0.2; + + const int result = libccd_extension::expandPolytope( + &hex.polytope(), reinterpret_cast(&hex.f(0)), &newv); + EXPECT_EQ(result, 0); + + // Construct the expanded polytope manually. + Hexagram hex_duplicate(0, 0, -0.9); + ccd_pt_t& polytope_expected = hex_duplicate.polytope(); + // Remove the upper triangle. + ccdPtDelFace(&polytope_expected, &hex_duplicate.f(0)); + + // Add the new vertex. + ccd_pt_vertex_t* new_vertex = ccdPtAddVertexCoords( + &polytope_expected, newv.v.v[0], newv.v.v[1], newv.v.v[2]); + // Add the new edges. + ccd_pt_edge_t* new_edges[3]; + new_edges[0] = + ccdPtAddEdge(&polytope_expected, new_vertex, &hex_duplicate.v(0)); + new_edges[1] = + ccdPtAddEdge(&polytope_expected, new_vertex, &hex_duplicate.v(2)); + new_edges[2] = + ccdPtAddEdge(&polytope_expected, new_vertex, &hex_duplicate.v(4)); + // Add the new faces. + ccdPtAddFace(&polytope_expected, new_edges[0], new_edges[1], + &hex_duplicate.e(0)); + ccdPtAddFace(&polytope_expected, new_edges[1], new_edges[2], + &hex_duplicate.e(1)); + ccdPtAddFace(&polytope_expected, new_edges[2], new_edges[0], + &hex_duplicate.e(2)); + + ComparePolytope(&hex.polytope(), &polytope_expected, + constants::eps_34()); +} + +GTEST_TEST(FCL_GJK_EPA, expandPolytope_hexagram_4_visible_faces) { + // Expand the Hexagram by adding a point above the upper triangle by a certain + // height, such that the new vertex can see the upper triangle, together with + // the three triangles on the side of the hexagram. All these four triangles + // will be removed. 6 new faces will be added by connecting the new vertex, + // with eah vertex in the old hexagram. + Hexagram hex(0, 0, -0.9); + // nearest point is on the top triangle + ccd_support_t newv; + newv.v.v[0] = 0; + newv.v.v[1] = 0; + newv.v.v[2] = 1.2; + + const int result = libccd_extension::expandPolytope( + &hex.polytope(), reinterpret_cast(&hex.f(0)), &newv); + EXPECT_EQ(result, 0); + + // Construct the expanded polytope manually. + Hexagram hex_duplicate(0, 0, -0.9); + ccd_pt_t& polytope_expected = hex_duplicate.polytope(); + // Remove the upper triangle. + ccdPtDelFace(&polytope_expected, &hex_duplicate.f(0)); + // Remove the triangles on the side, which consists of two vertices on the + // upper triangle, and one vertex on the lower triangle. + ccdPtDelFace(&polytope_expected, &hex_duplicate.f(2)); + ccdPtDelFace(&polytope_expected, &hex_duplicate.f(4)); + ccdPtDelFace(&polytope_expected, &hex_duplicate.f(6)); + // Remove the edges of the upper triangle. + ccdPtDelEdge(&polytope_expected, &hex_duplicate.e(0)); + ccdPtDelEdge(&polytope_expected, &hex_duplicate.e(1)); + ccdPtDelEdge(&polytope_expected, &hex_duplicate.e(2)); + + // Add the new vertex. + ccd_pt_vertex_t* new_vertex = ccdPtAddVertexCoords( + &polytope_expected, newv.v.v[0], newv.v.v[1], newv.v.v[2]); + // Add the new edges. + ccd_pt_edge_t* new_edges[6]; + for (int i = 0; i < 6; ++i) { + new_edges[i] = + ccdPtAddEdge(&polytope_expected, new_vertex, &hex_duplicate.v(i)); + } + // Add the new faces. + for (int i = 0; i < 6; ++i) { + ccdPtAddFace(&polytope_expected, new_edges[i % 6], new_edges[(i + 1) % 6], + &hex_duplicate.e(i + 6)); + } + ComparePolytope(&hex.polytope(), &polytope_expected, + constants::eps_34()); +} + +void CompareCcdVec3(const ccd_vec3_t& v, const ccd_vec3_t& v_expected, + ccd_real_t tol) { + for (int i = 0; i < 3; ++i) { + EXPECT_NEAR(v.v[i], v_expected.v[i], tol); + } +} + +GTEST_TEST(FCL_GJK_EPA, penEPAPosClosest_vertex) { + // The nearest point is a vertex on the polytope. + // tetrahedron.v(0) is the origin. + EquilateralTetrahedron tetrahedron(-0.5, 0.5 / std::sqrt(3), 0); + // Make sure that v1 - v2 = v. + tetrahedron.v(0).v.v1.v[0] = 1; + tetrahedron.v(0).v.v1.v[1] = 2; + tetrahedron.v(0).v.v1.v[2] = 3; + for (int i = 0; i < 3; ++i) { + tetrahedron.v(0).v.v2.v[i] = tetrahedron.v(0).v.v1.v[i]; + } + ccd_vec3_t p1, p2; + EXPECT_EQ( + libccd_extension::penEPAPosClosest( + reinterpret_cast(&tetrahedron.v(0)), &p1, &p2), + 0); + CompareCcdVec3(p1, tetrahedron.v(0).v.v1, constants::eps_78()); + CompareCcdVec3(p2, tetrahedron.v(0).v.v2, constants::eps_78()); +} + +GTEST_TEST(FCL_GJK_EPA, penEPAPosClosest_edge) { + // The nearest point is on an edge of the polytope. + // tetrahedron.e(1) contains the origin. + EquilateralTetrahedron tetrahedron(0.25, -0.25 / std::sqrt(3), 0); + // e(1) connects two vertices v(1) and v(2), make sure that v(1).v1 - v(1).v2 + // = v(1).v, also v(2).v1 - v(2).v2 = v(2).v + ccdVec3Set(&tetrahedron.v(1).v.v1, 1, 2, 3); + ccdVec3Set(&tetrahedron.v(2).v.v1, 4, 5, 6); + for (int i = 1; i <= 2; ++i) { + for (int j = 0; j < 3; ++j) { + tetrahedron.v(i).v.v2.v[j] = + tetrahedron.v(i).v.v1.v[j] - tetrahedron.v(i).v.v.v[j]; + } + } + // Notice that origin = 0.5*v(1).v + 0.5*v(2).v + // So p1 = 0.5*v(1).v1 + 0.5*v(2).v1 + // p2 = 0.5*v(1).v2 + 0.5*v(2).v2 + ccd_vec3_t p1, p2; + EXPECT_EQ( + libccd_extension::penEPAPosClosest( + reinterpret_cast(&tetrahedron.e(1)), &p1, &p2), + 0); + ccd_vec3_t p1_expected, p2_expected; + ccdVec3Copy(&p1_expected, &tetrahedron.v(1).v.v1); + ccdVec3Add(&p1_expected, &tetrahedron.v(2).v.v1); + ccdVec3Scale(&p1_expected, ccd_real_t(0.5)); + ccdVec3Copy(&p2_expected, &tetrahedron.v(1).v.v2); + ccdVec3Add(&p2_expected, &tetrahedron.v(2).v.v2); + ccdVec3Scale(&p2_expected, ccd_real_t(0.5)); + + CompareCcdVec3(p1, p1_expected, constants::eps_78()); + CompareCcdVec3(p2, p2_expected, constants::eps_78()); +} + +GTEST_TEST(FCL_GJK_EPA, penEPAPosClosest_face) { + // The nearest point is on a face of the polytope, It is the center of + // tetrahedron.f(1). + const Vector3 bottom_center_pos = + Vector3(0, 1.0 / (3 * std::sqrt(3)), -std::sqrt(6) / 9) + + 0.01 * Vector3(0, -2 * std::sqrt(2) / 3, 1.0 / 3); + EquilateralTetrahedron tetrahedron(bottom_center_pos(0), bottom_center_pos(1), + bottom_center_pos(2)); + // Assign v(i).v1 and v(i).v2 for i = 0, 1, 3, such that + // v(i).v = v(i).v1 - v(i).v2 + tetrahedron.v(0).v.v1.v[0] = 1; + tetrahedron.v(0).v.v1.v[1] = 2; + tetrahedron.v(0).v.v1.v[2] = 3; + tetrahedron.v(1).v.v1.v[0] = 4; + tetrahedron.v(1).v.v1.v[1] = 5; + tetrahedron.v(1).v.v1.v[2] = 6; + tetrahedron.v(3).v.v1.v[0] = 7; + tetrahedron.v(3).v.v1.v[1] = 8; + tetrahedron.v(3).v.v1.v[2] = 9; + for (int i : {0, 1, 3}) { + for (int j = 0; j < 3; ++j) { + tetrahedron.v(i).v.v2.v[j] = + tetrahedron.v(i).v.v1.v[j] - tetrahedron.v(i).v.v.v[j]; + } + } + + ccd_vec3_t p1, p2; + EXPECT_EQ( + libccd_extension::penEPAPosClosest( + reinterpret_cast(&tetrahedron.f(1)), &p1, &p2), + 0); + + // Notice that the nearest point = 1/3 * v(0).v + 1/3 * v(1).v + 1/3 * v(3).v + // So p1 = 1/3 * (v(0).v1 + v(1).v1 + v(3).v1) + // p2 = 1/3 * (v(0).v2 + v(1).v2 + v(3).v2) + ccd_vec3_t p1_expected, p2_expected; + ccdVec3Copy(&p1_expected, &tetrahedron.v(0).v.v1); + ccdVec3Add(&p1_expected, &tetrahedron.v(1).v.v1); + ccdVec3Add(&p1_expected, &tetrahedron.v(3).v.v1); + ccdVec3Scale(&p1_expected, ccd_real_t(1.0 / 3)); + ccdVec3Copy(&p2_expected, &tetrahedron.v(0).v.v2); + ccdVec3Add(&p2_expected, &tetrahedron.v(1).v.v2); + ccdVec3Add(&p2_expected, &tetrahedron.v(3).v.v2); + ccdVec3Scale(&p2_expected, ccd_real_t(1.0 / 3)); + + CompareCcdVec3(p1, p1_expected, constants::eps_78()); + CompareCcdVec3(p2, p2_expected, constants::eps_78()); +} + +// Test convert2SimplexToTetrahedron function. +// We construct a test scenario that two boxes are on the xy plane of frame F. +// The two boxes penetrate to each other, as shown in the bottom plot. +// y +// ┏━━━│━━━┓Box1 +// ┏┃━━┓ ┃ +// ───┃┃──┃O───┃─────x +// box2┗┃━━┛│ ┃ +// ┗━━━│━━━┛ +// +// @param[in] X_WF The pose of the frame F measured and expressed in the world +// frame W. +// @param[out] o1 Box1 +// @param[out] o2 Box2 +// @param[out] ccd The ccd solver info. +// @param[out] X_FB1 The pose of box 1 frame measured and expressed in frame F. +// @param[out] X_FB2 The pose of box 2 frame measured and expressed in frame F. +template +void SetUpBoxToBox(const Transform3& X_WF, void** o1, void** o2, ccd_t* ccd, + fcl::Transform3* X_FB1, fcl::Transform3* X_FB2) { + const fcl::Vector3 box1_size(2, 2, 2); + const fcl::Vector3 box2_size(1, 1, 2); + // Box 1 is fixed. + X_FB1->setIdentity(); + X_FB1->translation() << 0, 0, 1; + X_FB2->setIdentity(); + X_FB2->translation() << -0.6, 0, 1; + + const fcl::Transform3 X_WB1 = X_WF * (*X_FB1); + const fcl::Transform3 X_WB2 = X_WF * (*X_FB2); + fcl::Box box1(box1_size); + fcl::Box box2(box2_size); + *o1 = GJKInitializer>::createGJKObject(box1, X_WB1); + *o2 = GJKInitializer>::createGJKObject(box1, X_WB2); + + // Set up ccd solver. + CCD_INIT(ccd); + ccd->support1 = detail::GJKInitializer>::getSupportFunction(); + ccd->support2 = detail::GJKInitializer>::getSupportFunction(); + ccd->max_iterations = 1000; + ccd->dist_tolerance = 1E-6; +} + +template +Vector3 ToEigenVector(const ccd_vec3_t& v) { + return (Vector3() << v.v[0], v.v[1], v.v[2]).finished(); +} + +template +ccd_vec3_t ToCcdVec3(const Eigen::Ref>& v) { + ccd_vec3_t u; + u.v[0] = v(0); + u.v[1] = v(1); + u.v[2] = v(2); + return u; +} + +template +void TestSimplexToPolytope3InGivenFrame(const Transform3& X_WF) { + void* o1 = NULL; + void* o2 = NULL; + ccd_t ccd; + fcl::Transform3 X_FB1, X_FB2; + SetUpBoxToBox(X_WF, &o1, &o2, &ccd, &X_FB1, &X_FB2); + + // Construct a 2-simplex that contains the origin. The vertices of this + // 2-simplex are on the boundary of the Minkowski difference. + ccd_simplex_t simplex; + ccdSimplexInit(&simplex); + ccd_support_t pts[3]; + // 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 p_FPa1(-1, -1, -1); + const Vector3 p_FPa2(-0.1, 0.5, -1); + pts[0].v = ToCcdVec3(p_FPa1 - p_FPa2); + pts[0].v1 = ToCcdVec3(p_FPa1); + pts[0].v2 = ToCcdVec3(p_FPa2); + + const Vector3 p_FPb1(-1, 1, -1); + const Vector3 p_FPb2(-0.1, 0.5, -1); + pts[1].v = ToCcdVec3(p_FPb1 - p_FPb2); + pts[1].v1 = ToCcdVec3(p_FPb1); + pts[1].v2 = ToCcdVec3(p_FPb2); + + const Vector3 p_FPc1(1, 1, -1); + const Vector3 p_FPc2(-0.1, 0.5, -1); + pts[2].v = ToCcdVec3(p_FPc1 - p_FPc2); + pts[2].v1 = ToCcdVec3(p_FPc1); + pts[2].v2 = ToCcdVec3(p_FPc2); + for (int i = 0; i < 3; ++i) { + ccdSimplexAdd(&simplex, &pts[i]); + } + // Make sure that the origin is on the triangle. + const Vector3 a = ToEigenVector(pts[0].v); + const Vector3 b = ToEigenVector(pts[1].v); + const Vector3 c = ToEigenVector(pts[2].v); + // We first check if the origin is co-planar with vertices a, b, and c. + // If a, b, c and origin are co-planar, then aᵀ · (b × c)) = 0 + EXPECT_NEAR(a.dot(b.cross(c)), 0, 1E-10); + // Now check if origin is within the triangle, by checking the condition + // (a × b)ᵀ · (b × c) ≥ 0 + // (b × c)ᵀ · (c × a) ≥ 0 + // (c × a)ᵀ · (a × b) ≥ 0 + // Namely the cross product a × b, b × c, c × a all point to the same + // direction. + // Note the check above is valid when either a, b, c is a zero vector, or + // they are co-linear. + EXPECT_GE(a.cross(b).dot(b.cross(c)), 0); + EXPECT_GE(b.cross(c).dot(c.cross(a)), 0); + EXPECT_GE(c.cross(a).dot(a.cross(b)), 0); + + ccd_pt_t polytope; + ccdPtInit(&polytope); + ccd_pt_el_t* nearest{}; + libccd_extension::convert2SimplexToTetrahedron(o1, o2, &ccd, &simplex, + &polytope, &nearest); + // Box1 and Box2 are not touching, so nearest is set to null. + EXPECT_EQ(nearest, nullptr); + + // Check the polytope + // The polytope should have 4 vertices, with three of them being the vertices + // of the 2-simplex, and another vertex that has the maximal support along + // the normal directions of the 2-simplex. + // We first construct the set containing the polytope vertices. + std::unordered_set polytope_vertices; + { + ccd_pt_vertex_t* v; + ccdListForEachEntry(&polytope.vertices, v, ccd_pt_vertex_t, list) { + const auto it = polytope_vertices.find(v); + EXPECT_EQ(it, polytope_vertices.end()); + polytope_vertices.emplace_hint(it, v); + } + } + EXPECT_EQ(polytope_vertices.size(), 4u); + // We need to find out the vertex on the polytope, that is not the vertex + // of the simplex. + ccd_pt_vertex_t* non_simplex_vertex{nullptr}; + // A simplex vertex matches with a polytope vertex if they coincide. + int num_matched_vertices = 0; + for (const auto& v : polytope_vertices) { + bool found_match = false; + for (int i = 0; i < 3; ++i) { + if (ccdVec3Dist2(&v->v.v, &pts[i].v) < constants::eps_78()) { + num_matched_vertices++; + found_match = true; + break; + } + } + if (!found_match) { + non_simplex_vertex = v; + } + } + EXPECT_EQ(num_matched_vertices, 3); + EXPECT_NE(non_simplex_vertex, nullptr); + // Make sure that the non-simplex vertex has the maximal support along the + // simplex normal direction. + // Find the two normal directions of the 2-simplex. + Vector3 dir1, dir2; + Vector3 ab, ac; + ab = ToEigenVector(pts[1].v) - ToEigenVector(pts[0].v); + ac = ToEigenVector(pts[2].v) - ToEigenVector(pts[0].v); + dir1 = ab.cross(ac); + dir2 = -dir1; + // Now make sure non_simplex_vertex has the largest support + // p_B1V1 are the position of the box 1 vertices in box1 frame B1. + // p_B2V1 are the position of the box 2 vertices in box2 frame B2. + const Eigen::Vector3d box1_size{2, 2, 2}; + const Eigen::Vector3d box2_size{1, 1, 2}; + Eigen::Matrix p_B1V1, p_B2V2; + p_B1V1 << -1, -1, -1, -1, 1, 1, 1, 1, -1, -1, 1, 1, -1, -1, 1, 1, -1, 1, -1, + 1, -1, 1, -1, 1; + p_B2V2 << -1, -1, -1, -1, 1, 1, 1, 1, -1, -1, 1, 1, -1, -1, 1, 1, -1, 1, -1, + 1, -1, 1, -1, 1; + for (int i = 0; i < 3; ++i) { + p_B1V1.row(i) *= box1_size[i] / 2; + p_B2V2.row(i) *= box2_size[i] / 2; + } + + const Eigen::Matrix p_FV1 = X_FB1 * p_B1V1; + const Eigen::Matrix p_FV2 = X_FB2 * p_B2V2; + // The support of the Minkowski difference along direction dir1. + const S max_support1 = (dir1.transpose() * p_FV1).maxCoeff() - + (dir1.transpose() * p_FV2).minCoeff(); + // The support of the Minkowski difference along direction dir2. + const S max_support2 = (dir2.transpose() * p_FV1).maxCoeff() - + (dir2.transpose() * p_FV2).minCoeff(); + + const double expected_max_support = std::max(max_support1, max_support2); + const double non_simplex_vertex_support1 = + ToEigenVector(non_simplex_vertex->v.v).dot(dir1); + const double non_simplex_vertex_support2 = + ToEigenVector(non_simplex_vertex->v.v).dot(dir2); + EXPECT_NEAR( + std::max(non_simplex_vertex_support1, non_simplex_vertex_support2), + expected_max_support, constants::eps_78()); + + // Also make sure the non_simplex_vertex actually is inside the Minkowski + // difference. + // Call the non-simplex vertex as D. This vertex equals to the difference + // between a point Dv1 in box1, and a point Dv2 in box2. + const Vector3 p_B1Dv1 = + (X_WF * X_FB1).inverse() * ToEigenVector(non_simplex_vertex->v.v1); + const Vector3 p_B2Dv2 = + (X_WF * X_FB2).inverse() * ToEigenVector(non_simplex_vertex->v.v2); + // Now check if p_B1Dv1 is in box1, and p_B2Dv2 is in box2. + for (int i = 0; i < 3; ++i) { + EXPECT_LE(p_B1Dv1(i), box1_size(i) / 2 + constants::eps_78()); + EXPECT_LE(p_B2Dv2(i), box2_size(i) / 2 + constants::eps_78()); + EXPECT_GE(p_B1Dv1(i), -box1_size(i) / 2 - constants::eps_78()); + EXPECT_GE(p_B2Dv2(i), -box2_size(i) / 2 - constants::eps_78()); + } + // Now that we make sure the vertices of the polytope is correct, we will + // check the edges and faces of the polytope. We do so by constructing an + // expected polytope, and compare it with the polytope obtained from + // convert2SimplexToTetrahedron(). + ccd_pt_t polytope_expected; + ccdPtInit(&polytope_expected); + + ccd_pt_vertex_t* vertices_expected[4]; + int v_count = 0; + for (const auto& v : polytope_vertices) { + vertices_expected[v_count++] = ccdPtAddVertex(&polytope_expected, &v->v); + } + ccd_pt_edge_t* edges_expected[6]; + edges_expected[0] = ccdPtAddEdge(&polytope_expected, vertices_expected[0], + vertices_expected[1]); + edges_expected[1] = ccdPtAddEdge(&polytope_expected, vertices_expected[1], + vertices_expected[2]); + edges_expected[2] = ccdPtAddEdge(&polytope_expected, vertices_expected[2], + vertices_expected[0]); + edges_expected[3] = ccdPtAddEdge(&polytope_expected, vertices_expected[3], + vertices_expected[0]); + edges_expected[4] = ccdPtAddEdge(&polytope_expected, vertices_expected[3], + vertices_expected[1]); + edges_expected[5] = ccdPtAddEdge(&polytope_expected, vertices_expected[3], + vertices_expected[2]); + + ccdPtAddFace(&polytope_expected, edges_expected[0], edges_expected[3], + edges_expected[4]); + ccdPtAddFace(&polytope_expected, edges_expected[1], edges_expected[4], + edges_expected[5]); + ccdPtAddFace(&polytope_expected, edges_expected[2], edges_expected[3], + edges_expected[5]); + ccdPtAddFace(&polytope_expected, edges_expected[0], edges_expected[1], + edges_expected[2]); + + ComparePolytope(&polytope, &polytope_expected, constants::eps_34()); + + ccdPtDestroy(&polytope_expected); + ccdPtDestroy(&polytope); +} + +template +void TestSimplexToPolytope3() { + Transform3 X_WF; + X_WF.setIdentity(); + TestSimplexToPolytope3InGivenFrame(X_WF); + + X_WF.translation() << 0, 0, 1; + TestSimplexToPolytope3InGivenFrame(X_WF); + + X_WF.translation() << -0.2, 0.4, 0.1; + TestSimplexToPolytope3InGivenFrame(X_WF); +} + +GTEST_TEST(FCL_GJK_EPA, convert2SimplexToTetrahedron) { + TestSimplexToPolytope3(); + TestSimplexToPolytope3(); +} + +} // namespace detail +} // namespace fcl + +//============================================================================== +int main(int argc, char* argv[]) { + ::testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/test/narrowphase/detail/convexity_based_algorithm/test_gjk_libccd-inl_signed_distance.cpp b/test/narrowphase/detail/convexity_based_algorithm/test_gjk_libccd-inl_signed_distance.cpp new file mode 100644 index 000000000..6add8eb41 --- /dev/null +++ b/test/narrowphase/detail/convexity_based_algorithm/test_gjk_libccd-inl_signed_distance.cpp @@ -0,0 +1,342 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018. 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 Hongkai Dai */ +/** + * Test the signed distance query between two convex objects, when calling with + * solver = GST_LIBCCD. + */ +#include "fcl/narrowphase/detail/convexity_based_algorithm/gjk_libccd-inl.h" + +#include +#include + +#include "fcl/narrowphase/detail/gjk_solver_libccd.h" + +namespace fcl { +namespace detail { +// Given two spheres, sphere 1 has radius1, and centered at point A, whose +// position is p_FA measured and expressed in frame F; sphere 2 has radius2, +// and centered at point B, whose position is p_FB measured and expressed in +// frame F. Computes the signed distance between the two spheres, together with +// the two closest points Na on sphere 1 and Nb on sphere 2, returns the +// position of Na and Nb expressed in frame F. +// We use the monogram notation on spatial vectors. The monogram notation is +// explained in +// http://drake.mit.edu/doxygen_cxx/group__multibody__spatial__pose.html +template +S ComputeSphereSphereDistance(S radius1, S radius2, const Vector3& p_FA, + const Vector3& p_FB, Vector3* p_FNa, + Vector3* p_FNb) { + S min_distance = (p_FA - p_FB).norm() - radius1 - radius2; + const Vector3 p_AB_F = + p_FB - p_FA; // The vector AB measured and expressed + // in frame F. + *p_FNa = p_FA + p_AB_F.normalized() * radius1; + *p_FNb = p_FB - p_AB_F.normalized() * radius2; + return min_distance; +} + +template +void TestSphereToSphereGJKSignedDistance(S radius1, S radius2, + const Vector3& p_FA, + const Vector3& p_FB, S tol, + S min_distance_expected, + const Vector3& p_FNa_expected, + const Vector3& p_FNb_expected) { + // Test if GJKSignedDistance computes the right distance. Here we used sphere + // to sphere as the geometries. The distance between sphere and sphere should + // be computed using distance between primitives, instead of the GJK + // algorithm. But here we choose spheres for simplicity. + + fcl::Sphere s1(radius1); + fcl::Sphere s2(radius2); + fcl::Transform3 tf1, tf2; + tf1.setIdentity(); + tf2.setIdentity(); + tf1.translation() = p_FA; + tf2.translation() = p_FB; + void* o1 = GJKInitializer>::createGJKObject(s1, tf1); + void* o2 = GJKInitializer>::createGJKObject(s2, tf2); + + S dist; + Vector3 p1, p2; + GJKSolver_libccd gjkSolver; + bool res = GJKSignedDistance( + o1, detail::GJKInitializer>::getSupportFunction(), o2, + detail::GJKInitializer>::getSupportFunction(), + gjkSolver.max_distance_iterations, gjkSolver.distance_tolerance, &dist, + &p1, &p2); + + EXPECT_EQ(res, min_distance_expected >= 0); + + EXPECT_NEAR(dist, min_distance_expected, tol); + EXPECT_TRUE(p1.isApprox(p_FNa_expected, tol)); + EXPECT_TRUE(p2.isApprox(p_FNb_expected, tol)); + + GJKInitializer>::deleteGJKObject(o1); + GJKInitializer>::deleteGJKObject(o2); +} + +template +struct SphereSpecification { + SphereSpecification(S radius_, const Vector3& center_) + : radius{radius_}, center{center_} {} + S radius; + Vector3 center; +}; + +template +void TestNonCollidingSphereGJKSignedDistance(S tol) { + std::vector> spheres; + spheres.emplace_back(0.5, Vector3(0, 0, -1.2)); + spheres.emplace_back(0.5, Vector3(1.25, 0, 0)); + spheres.emplace_back(0.3, Vector3(-0.2, 0, 0)); + spheres.emplace_back(0.4, Vector3(-0.2, 0, 1.1)); + for (int i = 0; i < static_cast(spheres.size()); ++i) { + for (int j = i + 1; j < static_cast(spheres.size()); ++j) { + if ((spheres[i].center - spheres[j].center).norm() > + spheres[i].radius + spheres[j].radius) { + // Not in collision. + Vector3 p_FNa, p_FNb; + const S min_distance_expected = ComputeSphereSphereDistance( + spheres[i].radius, spheres[j].radius, spheres[i].center, + spheres[j].center, &p_FNa, &p_FNb); + TestSphereToSphereGJKSignedDistance( + spheres[i].radius, spheres[j].radius, spheres[i].center, + spheres[j].center, tol, min_distance_expected, p_FNa, p_FNb); + } else { + GTEST_FAIL() << "The two spheres collide." + << "\nSpheres[" << i << "] with radius " + << spheres[i].radius << ", centered at " + << spheres[i].center.transpose() << "\nSpheres[" << j + << "] with radius " << spheres[j].radius + << ", centered at " << spheres[j].center.transpose() + << "\n"; + } + } + } +} + +GTEST_TEST(FCL_GJKSignedDistance, sphere_sphere) { + // By setting gjkSolver.distance_tolerance to the default value (1E-6), the + // tolerance we get on the closest points are only up to the square root of + // 1E-6, namely 1E-3. + TestNonCollidingSphereGJKSignedDistance(1E-3); + TestNonCollidingSphereGJKSignedDistance(1E-3); +} + +//---------------------------------------------------------------------------- +// Box test +// Given two boxes, we can perturb the pose of one box so the boxes are +// penetrating, touching or separated. +template +struct BoxSpecification { + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + BoxSpecification(const fcl::Vector3& m_size) : size(m_size) { + X_FB.setIdentity(); + } + fcl::Vector3 size; + fcl::Transform3 X_FB; +}; + +template +void TestBoxesInFrameF(S tol, const Transform3& X_WF) { + const fcl::Vector3 box1_size(1, 1, 1); + const fcl::Vector3 box2_size(0.6, 0.8, 1); + // Put the two boxes on the xy plane of frame F. + // B1 is the frame rigidly attached to box 1, B2 is the frame rigidly attached + // to box 2. W is the world frame. F is a frame fixed to the world. X_FB1 is + // the pose of box 1 expressed and measured in frame F, X_FB2 is the pose of + // box 2 expressed and measured in frame F. + fcl::Transform3 X_FB1, X_FB2; + // Box 1 is fixed. + X_FB1.setIdentity(); + X_FB1.translation() << 0, 0, 0.5; + + // First fix the orientation of box 2, such that one of its diagonal (the one + // connecting the vertex (0.3, -0.4, 1) and (-0.3, 0.4, 1) is parallel to the + // x axis in frame F. If we move the position of box 2, we get different + // signed distance. + X_FB2.setIdentity(); + X_FB2.linear() << 0.6, -0.8, 0, 0.8, 0.6, 0, 0, 0, 1; + + // p_xy_FNa is the xy position of point Na (the deepest penetration point on + // box 1) measured and expressed in the frame F. + // p_xy_FNb is the xy position of point Nb (the deepest penetration point on + // box 2) measured and expressed in the frame F. + auto CheckDistance = [&box1_size, &box2_size, &X_FB1, &X_WF]( + const Transform3& X_FB2, S distance_expected, + const Vector2& p_xy_FNa_expected, const Vector2& p_xy_FNb_expected, + S tol) { + const fcl::Transform3 X_WB1 = X_WF * X_FB1; + const fcl::Transform3 X_WB2 = X_WF * X_FB2; + fcl::Box box1(box1_size); + fcl::Box box2(box2_size); + void* o1 = GJKInitializer>::createGJKObject(box1, X_WB1); + void* o2 = GJKInitializer>::createGJKObject(box2, X_WB2); + S dist; + Vector3 p_WNa, p_WNb; + GJKSolver_libccd gjkSolver; + bool res = GJKSignedDistance( + o1, detail::GJKInitializer>::getSupportFunction(), o2, + detail::GJKInitializer>::getSupportFunction(), + gjkSolver.max_distance_iterations, gjkSolver.distance_tolerance, &dist, + &p_WNa, &p_WNb); + + // It is unclear how FCL handles touching contact. It could return either + // true or false for touching contact. So, we ignore the condition where + // expected distance is zero. + if (distance_expected < 0) { + EXPECT_FALSE(res); + } else if (distance_expected > 0) { + EXPECT_TRUE(res); + } + + EXPECT_NEAR(dist, distance_expected, tol); + const Vector3 p_FNa = + X_WF.linear().transpose() * (p_WNa - X_WF.translation()); + const Vector3 p_FNb = + X_WF.linear().transpose() * (p_WNb - X_WF.translation()); + + EXPECT_TRUE(p_FNa.template head<2>().isApprox(p_xy_FNa_expected, tol)); + EXPECT_TRUE(p_FNb.template head<2>().isApprox(p_xy_FNb_expected, tol)); + // The z height of the closest points should be the same. + EXPECT_NEAR(p_FNa(2), p_FNb(2), tol); + // The closest point is within object A/B, so the z height should be within + // [0, 1] + EXPECT_GE(p_FNa(2), 0); + EXPECT_GE(p_FNb(2), 0); + EXPECT_LE(p_FNa(2), 1); + EXPECT_LE(p_FNb(2), 1); + + GJKInitializer>::deleteGJKObject(o1); + GJKInitializer>::deleteGJKObject(o2); + }; + + auto CheckBoxEdgeBoxFaceDistance = [&CheckDistance]( + const Transform3& X_FB2, S tol) { + const double expected_distance = -X_FB2.translation()(0) - 1; + CheckDistance( + X_FB2, expected_distance, Vector2(-0.5, X_FB2.translation()(1)), + Vector2(X_FB2.translation()(0) + 0.5, X_FB2.translation()(1)), tol); + }; + //--------------------------------------------------------------- + // Touching contact + // An edge of box 2 is touching a face of box 1 + X_FB2.translation() << -1, 0, 0.5; + CheckBoxEdgeBoxFaceDistance(X_FB2, tol); + + // The touching face on box 1 is parallel to the y axis, so shifting box 2 on + // y axis still gives touching contact. Shift box 2 on y axis by 0.1m. + X_FB2.translation() << -1, 0.1, 0.5; + CheckBoxEdgeBoxFaceDistance(X_FB2, tol); + + // Shift box 2 on y axis by -0.1m. + X_FB2.translation() << -1, -0.1, 0.5; + CheckBoxEdgeBoxFaceDistance(X_FB2, tol); + // TODO(hongkai.dai@tri.global): Add other touching contact cases, including + // face-face, face-vertex, edge-edge, edge-vertex and vertex-vertex. + + //-------------------------------------------------------------- + // Penetrating contact + // An edge of box 2 penetrates into a face of box 1 + X_FB2.translation() << -0.9, 0, 0.5; + CheckBoxEdgeBoxFaceDistance(X_FB2, tol); + + // Shift box 2 on y axis by 0.1m. + X_FB2.translation() << -0.9, 0.1, 0.5; + CheckBoxEdgeBoxFaceDistance(X_FB2, tol); + + // Shift box 2 on y axis by -0.05m. + X_FB2.translation() << -0.9, -0.05, 0.5; + CheckBoxEdgeBoxFaceDistance(X_FB2, tol); + + // Shift box 2 on y axis by -0.1m. + X_FB2.translation() << -0.9, -0.1, 0.5; + CheckBoxEdgeBoxFaceDistance(X_FB2, tol); +} + +template +void TestBoxes(S tol) { + // Frame F coincides with the world frame W. + Transform3 X_WF; + X_WF.setIdentity(); + TestBoxesInFrameF(tol, X_WF); + + // Frame F is shifted from the world frame W. + X_WF.translation() << 0, 0, 1; + TestBoxesInFrameF(tol, X_WF); + + X_WF.translation() << 0, 1, 0; + TestBoxesInFrameF(tol, X_WF); + + X_WF.translation() << 1, 0, 0; + TestBoxesInFrameF(tol, X_WF); + + // Frame F is rotated from the world frame W. + X_WF.setIdentity(); + const S kPi = fcl::constants::pi(); + X_WF.linear() = + Eigen::AngleAxis(0.1 * kPi, Vector3::UnitZ()).toRotationMatrix(); + TestBoxesInFrameF(tol, X_WF); + + // TODO(hongkai.dai): This test exposes an error in simplexToPolytope4, that + // the initial simplex can be degenerate. Should add the special case on + // degenerate simplex in simplexToPolytope4. + /*X_WF.translation() << 0, 0, 0; + X_WF.linear() = + Eigen::AngleAxis(0.1 * kPi, Vector3(1.0 / 3, 2.0 / 3, -2.0 / 3)) + .toRotationMatrix(); + TestBoxesInFrameF(tol, X_WF);*/ + + // Frame F is rotated and shifted from the world frame W. + X_WF.translation() << 0.1, 0.2, 0.3; + TestBoxesInFrameF(tol, X_WF); +} + +GTEST_TEST(FCL_GJKSignedDistance, box_box) { + // By setting gjkSolver.distance_tolerance to the default value (1E-6), the + // tolerance we get on the closest points are only up to 1E-3 + TestBoxes(1E-3); + TestBoxes(1E-3); +} +} // namespace detail +} // namespace fcl + +//============================================================================== +int main(int argc, char* argv[]) { + ::testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/test/test_fcl_signed_distance.cpp b/test/test_fcl_signed_distance.cpp index 1d6296352..c67eb9f89 100644 --- a/test/test_fcl_signed_distance.cpp +++ b/test/test_fcl_signed_distance.cpp @@ -78,16 +78,21 @@ void test_distance_spheresphere(GJKSolverType solver_type) res = distance(&s1, tf1, &s2, tf2, request, result); EXPECT_TRUE(res); - EXPECT_TRUE(std::abs(result.min_distance - (-5)) < 1.5e-1); - // TODO(JS): The negative distance computation using libccd requires - // unnecessarily high error tolerance. + // request.distance_tolerance is actually the square of the distance + // tolerance, namely the difference between distance returned from FCL's EPA + // implementation and the actual distance, is less than + // sqrt(request.distance_tolerance). + const S distance_tolerance = std::sqrt(request.distance_tolerance); + EXPECT_NEAR(result.min_distance, -5, distance_tolerance); // TODO(JS): Only GST_LIBCCD can compute the pair of nearest points on the // surface of the spheres. if (solver_type == GST_LIBCCD) { - EXPECT_TRUE(result.nearest_points[0].isApprox(Vector3(20, 0, 0))); - EXPECT_TRUE(result.nearest_points[1].isApprox(Vector3(-10, 0, 0))); + EXPECT_TRUE(result.nearest_points[0].isApprox(Vector3(20, 0, 0), + distance_tolerance)); + EXPECT_TRUE(result.nearest_points[1].isApprox(Vector3(-10, 0, 0), + distance_tolerance)); } }