-
Notifications
You must be signed in to change notification settings - Fork 417
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
- Loading branch information
1 parent
bb939ee
commit e91f23b
Showing
2 changed files
with
150 additions
and
75 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
|
|
@@ -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,48 +800,36 @@ 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<ccd_pt_edge_t*>* silhouette_edges, | ||
| std::unordered_set<ccd_pt_face_t*>* obsolete_faces, | ||
| std::unordered_set<ccd_pt_edge_t*>* obsolete_edges) { | ||
| 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,24 +839,69 @@ 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); | ||
| } | ||
| } | ||
| } | ||
| } | ||
| } | ||
|
|
||
| /** | ||
| * 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<ccd_pt_edge_t*>* silhouette_edges, | ||
| std::unordered_set<ccd_pt_face_t*>* obsolete_faces, | ||
| std::unordered_set<ccd_pt_edge_t*>* 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,21 +934,22 @@ 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<ccd_pt_face_t*> obsolete_faces; | ||
| std::unordered_set<ccd_pt_edge_t*> obsolete_edges; | ||
| std::unordered_set<ccd_pt_edge_t*> 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; | ||
| } else if (el->type == CCD_PT_EDGE) { | ||
| // 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<ccd_pt_face_t*> obsolete_faces; | ||
| std::unordered_set<ccd_pt_edge_t*> obsolete_edges; | ||
| std::unordered_set<ccd_pt_edge_t*> silhouette_edges; | ||
| floodFillSilhouette(pt, start_face, &newv->v, &silhouette_edges, | ||
| &obsolete_faces, &obsolete_edges); | ||
|
|
||
| // Now remove all the obsolete faces. | ||
| // TODO([email protected]): 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); | ||
| } | ||
|
|
||
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters