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 cf464d381..03168c94b 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 @@ -720,9 +720,9 @@ static int simplexToPolytope4(const void *obj1, const void *obj2, } /** - * Computes the normal vector of a face on a polytope, and the normal vector - * points outward from the polytope. Notice we assume that the origin lives - * within the polytope. + * 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. * @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 unit length normal vector is computed. @@ -739,6 +739,8 @@ static ccd_vec3_t faceNormalPointingOutward(const ccd_pt_t* polytope, 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. ccdVec3Cross(&dir, &e1, &e2); ccd_real_t projection = ccdVec3Dot(&dir, &(face->edge[0]->vertex[0]->v.v)); if (projection < 0) { @@ -754,9 +756,17 @@ static ccd_vec3_t faceNormalPointingOutward(const ccd_pt_t* polytope, 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 vertex is at least 1cm away from the plane coinciding + // means one of the vertices is at least 1cm away from the plane coinciding // with the face. - ccd_real_t tol = 1E-2 / std::sqrt(ccdVec3Len2(&dir)); + // The choice of 1cm is arbitrary here. We could choose any threshold here, + // since if we do not find any vertices whose distance to the plane is less + // than 1cm, the function will proceed to record the maximal/minimal + // distance among all vertices, and then choose whether to flip the dir + // vector based on the maximal/minimal distance. The value of the tolerance + // determines whether the function needs to loop through all the vertices + // or not. Thus by choosing a different threshold, the computation time of + // this function would change, but the result is the same. + ccd_real_t tol = 1E-2 * std::sqrt(ccdVec3Len2(&dir)); ccdListForEachEntry(&polytope->vertices, v, ccd_pt_vertex_t, list) { projection = ccdVec3Dot(&dir, &(v->v.v)); if (projection > tol) { @@ -774,7 +784,10 @@ static ccd_vec3_t faceNormalPointingOutward(const ccd_pt_t* polytope, min_projection = std::min(min_projection, projection); } } - // If max_projection > |min_projection|, then flip dir. + // 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)); } @@ -787,37 +800,26 @@ static ccd_vec3_t faceNormalPointingOutward(const ccd_pt_t* polytope, // half-plane, the return if false. // @param f A triangle on a polytope. // @param pt A point. -static bool outsidePolytopeFace(const ccd_pt_t* polytope, +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); - return ccdVec3Dot(&n, pt) > ccdVec3Dot(&n, &(f->edge[0]->vertex[0]->v.v)); + ccd_vec3_t p_minus_v; + ccdVec3Sub2(&p_minus_v, pt, &(f->edge[0]->vertex[0]->v.v)); + return ccdVec3Dot(&n, &p_minus_v) > 0; } /** - * Test if the face neighbouring triangle f, and sharing the common edge - * f->edge[edge_index] can be seen from the new vertex or not. If the face - * cannot be seen, then we add the common edge to silhouette_edges; otherwise - * we add the common edge to obsolete_edges. The face f will be added to - * obsolete_faces. - * We will call this function recursively to traverse all faces that can be seen - * from the new vertex. - * @param[in] f A face that can be seen from the new vertex. This face will be - * deleted at the end of the function. - * @param[in] edge_index We will check if the face neighbouring f with this - * common edge f->edge[edge_index] can be seen from the new vertex. - * @param[in] new_vertex The new vertex to be added to the polytope. - * @param[in/out] visited_faces If the neighbouring face hasn't been visited, - * then add it to visited_faces. - * @param[in/out] silhouette_edges If the neighbouring face cannot be seen from - * the new vertex, then the common edge will be preserved, after adding the new - * vertex, and this common edge will be added to silhouette_edges. - * @param[in/out] obsolete_faces If the neighbouring face can be seen from - * the new vertex, then add it to obsolete_faces. - * @param[in/out] obsolete_edges If the neighbouring face can be seen from - * the new vertex, then add this common edge to obsolete_edges. + * This function contains the implementation detail of floodFillSilhouette + * function. This function will be called recursively. It first checks if + * the face neighouring face `f` along the common edge `f->edge[edge_index]` can + * be seen from the point `new_vertex`. We denote this face as "g", If this face + * "g" cannot be seen, then stop. Otherwise, we continue to check the + * neighbouring faces of "g", by calling this function recursively. + * This function should not be called by any function other than + * floodFillSilhouette. */ -static void floodFillSilhouette( - const ccd_pt_t* polytope, ccd_pt_face_t* f, int edge_index, +static void floodFillSilhouetteRecursive( + const ccd_pt_t* polytope, const ccd_pt_face_t* f, int edge_index, const ccd_vec3_t* new_vertex, std::unordered_set* silhouette_edges, std::unordered_set* obsolete_faces, @@ -825,10 +827,9 @@ static void floodFillSilhouette( ccd_pt_face_t* f_neighbour = f->edge[edge_index]->faces[0] == f ? f->edge[edge_index]->faces[1] : f->edge[edge_index]->faces[0]; - const auto it = obsolete_faces->find(f_neighbour); - if (it == obsolete_faces->end()) { + if (obsolete_faces->count(f_neighbour) == 0) { // f_neighbour is not an obsolete face - if (!outsidePolytopeFace(polytope, f_neighbour, new_vertex)) { + if (!isOutsidePolytopeFace(polytope, f_neighbour, new_vertex)) { // Cannot see the neighbouring face from the new vertex. silhouette_edges->insert(f->edge[edge_index]); return; @@ -838,7 +839,7 @@ static void floodFillSilhouette( for (int i = 0; i < 3; ++i) { if (f_neighbour->edge[i] != f->edge[edge_index]) { // One of the neighbouring face is `f`, so do not need to visit again. - floodFillSilhouette(polytope, f_neighbour, i, new_vertex, + floodFillSilhouetteRecursive(polytope, f_neighbour, i, new_vertex, silhouette_edges, obsolete_faces, obsolete_edges); } } @@ -846,16 +847,61 @@ static void floodFillSilhouette( } } +/** + * Traverse the faces of the polytope, to find out all the faces that can be + * seen from a point `new_vertex`. A face can be seen from a point outside + * of the polytope, if the point lies on the outward side of the plane. + * @param[in] f The starting face for the traversal. f is a face that can be + * seen from the new vertex. + * @param[in] new_vertex A point ouside of the polytope. + * @param[out] silhouette_edges If one of the neighbouring faces cannot be + * seen from the new vertex, and one of the neighbouring faces is visible from + * @p new_vertex, then the edge is a silhouette edge. silhouette_edges contains + * all the silhouette edges on the polytope. + * @param[out] obsolete_faces If the face can be seen from the new vertex, then + * this face is obsolete. obsolete_faces contains all the obsolete faces on the + * polytope. + * @param[out] obsolete_edges If both of the neighbouring faces are obsolete, + * then this edge is obsolete. obsolete_edges contains all the obsolete edges + * on the polytope. + * @pre polytope is convex, new_vertex is outside of the polytope. + * silhouette_edges, obsolete_faces and obsolete_edges cannot be null. + * TODO(hongkai.dai): do not store obsolete_faces/edges in a set, but remove + * them within this function. Also we can record the faces being visited, + * together with if they are visible/occuluded. So that we do not need to + * compute if a face is visible for multiple times. + */ +static void floodFillSilhouette( + const ccd_pt_t* polytope, ccd_pt_face_t* f, + const ccd_vec3_t* new_vertex, + std::unordered_set* silhouette_edges, + std::unordered_set* obsolete_faces, + std::unordered_set* obsolete_edges) { + assert(obsolete_faces->empty()); + assert(silhouette_edges->empty()); + assert(obsolete_edges->empty()); + assert(silhouette_edges); + assert(obsolete_faces); + assert(obsolete_edges); + obsolete_faces->insert(f); + for (int i = 0; i < 3; ++i) { + floodFillSilhouetteRecursive(polytope, f, i, new_vertex, silhouette_edges, + obsolete_faces, obsolete_edges); + } +} + /** Expands the polytope by adding a new vertex @p 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 that are interior to the polytope are - * discarded. + * 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] pt The polytope. - * @param[in] el The point on the boundary of the old polytope that is nearest - * to the origin. + * @param[in] el A feature that is visible from the point `newv`. 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. * @param[in] newv The new vertex add to the polytope. * @retval status Returns 0 on success. Returns -2 otherwise. */ @@ -870,9 +916,7 @@ static int expandPolytope(ccd_pt_t *pt, ccd_pt_el_t *el, // where face.normal points outward from the polytope. // 2. For each edge, if both neighbouring faces of the edge is removed, then // remove that edge. - // 3. For each vertex, if all edges connecting that vertex is removed, then - // remove that vertex. - // 4. If an edge has one of its neighbouring face being removed, and one of + // 3. If an edge has one of its neighbouring face being removed, and one of // its neighbouring face is preserved, we call this edge "silhouette edge". // We connect the new vertex to each boundary edge, to form new faces and // new edges. @@ -890,11 +934,12 @@ static int expandPolytope(ccd_pt_t *pt, ccd_pt_el_t *el, // // Traverse the polytope faces to determine which face/edge shall be obsolete, // together with the silhouette edges. - std::unordered_set obsolete_faces; - std::unordered_set obsolete_edges; - std::unordered_set silhouette_edges; 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 = (ccd_pt_face_t*)el; @@ -902,9 +947,9 @@ static int expandPolytope(ccd_pt_t *pt, ccd_pt_el_t *el, // Check the two neighbouring faces of the edge. ccd_pt_face_t* f[2]; ccdPtEdgeFaces((ccd_pt_edge_t*)el, &f[0], &f[1]); - if (outsidePolytopeFace(pt, f[0], &newv->v)) { + if (isOutsidePolytopeFace(pt, f[0], &newv->v)) { start_face = f[0]; - } else if (outsidePolytopeFace(pt, f[1], &newv->v)) { + } else if (isOutsidePolytopeFace(pt, f[1], &newv->v)) { start_face = f[1]; } else { throw std::logic_error( @@ -914,13 +959,18 @@ static int expandPolytope(ccd_pt_t *pt, ccd_pt_el_t *el, "touching contact."); } } - obsolete_faces.insert(start_face); - for (int i = 0; i < 3; ++i) { - floodFillSilhouette(pt, start_face, i, &newv->v, &silhouette_edges, - &obsolete_faces, &obsolete_edges); - } + + std::unordered_set obsolete_faces; + std::unordered_set obsolete_edges; + std::unordered_set silhouette_edges; + floodFillSilhouette(pt, start_face, &newv->v, &silhouette_edges, + &obsolete_faces, &obsolete_edges); // Now remove all the obsolete faces. + // TODO(hongkai.dai@tri.global): currently we need to loop through each face + // in obsolete_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 + // pt->faces for once. Same for the edges. for (const auto& f : obsolete_faces) { ccdPtDelFace(pt, f); } diff --git a/test/narrowphase/detail/convexity_based_algorithm/test_epa.cpp b/test/narrowphase/detail/convexity_based_algorithm/test_epa.cpp index fee9e454f..b3f50b12a 100644 --- a/test/narrowphase/detail/convexity_based_algorithm/test_epa.cpp +++ b/test/narrowphase/detail/convexity_based_algorithm/test_epa.cpp @@ -143,7 +143,7 @@ GTEST_TEST(FCL_GJK_EPA, sampledEPADirection) { std::runtime_error); } -GTEST_TEST(FCL_GJK_EPA, outsidePolytopeFace) { +GTEST_TEST(FCL_GJK_EPA, isOutsidePolytopeFace) { EquilateralTetrahedron p; auto CheckPointOutsidePolytopeFace = [&p](ccd_real_t x, ccd_real_t y, @@ -153,8 +153,8 @@ GTEST_TEST(FCL_GJK_EPA, outsidePolytopeFace) { pt.v[0] = x; pt.v[1] = y; pt.v[2] = z; - EXPECT_EQ(libccd_extension::outsidePolytopeFace(p.polytope(), - p.f(face_index), &pt), + EXPECT_EQ(libccd_extension::isOutsidePolytopeFace(p.polytope(), + p.f(face_index), &pt), is_outside_expected); }; @@ -262,7 +262,7 @@ bool IsElementInSet(const std::unordered_set& S, const T& element) { } template -void CheckFloodFillSilhouette( +void CheckFloodFillSilhouetteRecursive( const T& polytope, ccd_pt_face_t* face, const std::vector& edge_indices, const ccd_vec3_t& new_vertex, const std::unordered_set& silhouette_edge_indices_expected, @@ -273,10 +273,40 @@ void CheckFloodFillSilhouette( obsolete_faces.insert(face); std::unordered_set obsolete_edges; for (const int edge_index : edge_indices) { - libccd_extension::floodFillSilhouette(polytope.polytope(), face, edge_index, - &new_vertex, &silhouette_edges, - &obsolete_faces, &obsolete_edges); + libccd_extension::floodFillSilhouetteRecursive( + polytope.polytope(), face, edge_index, &new_vertex, &silhouette_edges, + &obsolete_faces, &obsolete_edges); + } + + // Check silhouette_edges + EXPECT_EQ(silhouette_edges.size(), silhouette_edge_indices_expected.size()); + for (const int edge_index : silhouette_edge_indices_expected) { + EXPECT_TRUE(IsElementInSet(silhouette_edges, polytope.e(edge_index))); } + // Check obsolete_faces + EXPECT_EQ(obsolete_faces.size(), obsolete_face_indices_expected.size()); + for (const int face_index : obsolete_face_indices_expected) { + EXPECT_TRUE(IsElementInSet(obsolete_faces, polytope.f(face_index))); + } + // Check obsolete_edges + EXPECT_EQ(obsolete_edges.size(), obsolete_edge_indices_expected.size()); + for (const auto edge_index : obsolete_edge_indices_expected) { + EXPECT_TRUE(IsElementInSet(obsolete_edges, polytope.e(edge_index))); + } +} + +template +void CheckFloodFillSilhouette( + const T& polytope, ccd_pt_face_t* face, const ccd_vec3_t& new_vertex, + const std::unordered_set& silhouette_edge_indices_expected, + const std::unordered_set& obsolete_face_indices_expected, + const std::unordered_set& obsolete_edge_indices_expected) { + std::unordered_set silhouette_edges; + std::unordered_set obsolete_faces; + std::unordered_set obsolete_edges; + libccd_extension::floodFillSilhouette(polytope.polytope(), face, &new_vertex, + &silhouette_edges, &obsolete_faces, + &obsolete_edges); // Check silhouette_edges EXPECT_EQ(silhouette_edges.size(), silhouette_edge_indices_expected.size()); @@ -304,11 +334,9 @@ GTEST_TEST(FCL_GJK_EPA, floodFillSilhouette1) { p.v[1] = 0; p.v[2] = 1.1; const std::unordered_set empty_set; - CheckFloodFillSilhouette(hex, hex.f(0), {0}, p, {0}, {0}, empty_set); + CheckFloodFillSilhouetteRecursive(hex, hex.f(0), {0}, p, {0}, {0}, empty_set); - // Run silhouette algorithm for the other edges - CheckFloodFillSilhouette(hex, hex.f(0), {0, 1, 2}, p, {0, 1, 2}, {0}, - empty_set); + CheckFloodFillSilhouette(hex, hex.f(0), p, {0, 1, 2}, {0}, empty_set); } GTEST_TEST(FCL_GJK_EPA, floodFillSilhouette2) { @@ -320,11 +348,10 @@ GTEST_TEST(FCL_GJK_EPA, floodFillSilhouette2) { p.v[0] = 0; p.v[1] = 0; p.v[2] = 2.1; - CheckFloodFillSilhouette(hex, hex.f(0), {0}, p, {6, 7}, {0, 2}, {0}); + CheckFloodFillSilhouetteRecursive(hex, hex.f(0), {0}, p, {6, 7}, {0, 2}, {0}); - // Run silhouette algorithm for the other edges - CheckFloodFillSilhouette(hex, hex.f(0), {0, 1, 2}, p, {6, 7, 8, 9, 10, 11}, - {0, 2, 4, 6}, {0, 1, 2}); + CheckFloodFillSilhouette(hex, hex.f(0), p, {6, 7, 8, 9, 10, 11}, {0, 2, 4, 6}, + {0, 1, 2}); } GTEST_TEST(FCL_GJK_EPA, floodFillSilhouette3) { @@ -335,11 +362,9 @@ GTEST_TEST(FCL_GJK_EPA, floodFillSilhouette3) { p.v[0] = 0; p.v[1] = -1 / std::sqrt(3) - 0.1; p.v[2] = 1.1; - CheckFloodFillSilhouette(hex, hex.f(0), {0}, p, {6, 7}, {0, 2}, {0}); + CheckFloodFillSilhouetteRecursive(hex, hex.f(0), {0}, p, {6, 7}, {0, 2}, {0}); - // Run silhouette algorithm for the other edges - CheckFloodFillSilhouette(hex, hex.f(0), {0, 1, 2}, p, {1, 2, 6, 7}, {0, 2}, - {0}); + CheckFloodFillSilhouette(hex, hex.f(0), p, {1, 2, 6, 7}, {0, 2}, {0}); } GTEST_TEST(FCL_GJK_EPA, floodFillSilhouette4) { @@ -354,12 +379,12 @@ GTEST_TEST(FCL_GJK_EPA, floodFillSilhouette4) { p.v[2] = -0.2; // Start with from face 0. - CheckFloodFillSilhouette(tetrahedron, tetrahedron.f(0), {0, 1, 2}, p, - {1, 2, 3, 4}, {0, 1}, {0}); + CheckFloodFillSilhouette(tetrahedron, tetrahedron.f(0), p, {1, 2, 3, 4}, + {0, 1}, {0}); // Start with from face 1. - CheckFloodFillSilhouette(tetrahedron, tetrahedron.f(1), {0, 1, 2}, p, - {1, 2, 3, 4}, {0, 1}, {0}); + CheckFloodFillSilhouette(tetrahedron, tetrahedron.f(1), p, {1, 2, 3, 4}, + {0, 1}, {0}); } // Returns true if the the position difference between the two vertices are