Improved handling of hulls with very acute angles (#179)

When points were nearly co-planar (but not co-planar enough for the algorithm to detect a 2D hull) it was possible that a point that was within tolerance to the plane but outside of the polygon was discarded. Now we calculate the distance to the edges and add the point to the hull at the end.

Jolt/Geometry/ConvexHullBuilder.cpp

@@ -141,46 +141,98 @@ void ConvexHullBuilder::FreeFaces()
 	mFaces.clear();
 }
 
-void ConvexHullBuilder::AssignPointToFace(int inPositionIdx, const Faces &inFaces) const
+void ConvexHullBuilder::GetFaceForPoint(Vec3Arg inPoint, const Faces &inFaces, Face *&outFace, float &outDistSq) const
 {
-	Vec3 point = mPositions[inPositionIdx];
-
-	Face *best_face = nullptr;
-	float best_dist_sq = 0.0f;
+	outFace = nullptr;
+	outDistSq = 0.0f;
 
-	// Test against all faces
 	for (Face *f : inFaces)
 		if (!f->mRemoved)
 		{
 			// Determine distance to face
-			float dot = f->mNormal.Dot(point - f->mCentroid);
+			float dot = f->mNormal.Dot(inPoint - f->mCentroid);
 			if (dot > 0.0f)
 			{
 				float dist_sq = dot * dot / f->mNormal.LengthSq();
-				if (dist_sq > best_dist_sq)
+				if (dist_sq > outDistSq)
 				{
-					best_face = f;
-					best_dist_sq = dist_sq;
+					outFace = f;
+					outDistSq = dist_sq;
 				}
 			}
 		}
+}
+
+float ConvexHullBuilder::GetDistanceToEdgeSq(Vec3Arg inPoint, const Face *inFace) const
+{
+	bool all_inside = true;
+	float edge_dist_sq = FLT_MAX;
+
+	// Test if it is inside the edges of the polygon
+	Edge *edge = inFace->mFirstEdge;
+	Vec3 p1 = mPositions[edge->GetPreviousEdge()->mStartIdx];
+	do
+	{
+		Vec3 p2 = mPositions[edge->mStartIdx];
+		if ((p2 - p1).Cross(inPoint - p1).Dot(inFace->mNormal) < 0.0f)
+		{
+			// It is outside
+			all_inside = false;
+
+			// Measure distance to this edge
+			uint32 s;
+			edge_dist_sq = min(edge_dist_sq, ClosestPoint::GetClosestPointOnLine(p1 - inPoint, p2 - inPoint, s).LengthSq());
+		}
+		p1 = p2;
+		edge = edge->mNextEdge;
+	} while (edge != inFace->mFirstEdge);
+
+	return all_inside? 0.0f : edge_dist_sq;
+}
+
+bool ConvexHullBuilder::AssignPointToFace(int inPositionIdx, const Faces &inFaces, float inToleranceSq)
+{
+	Vec3 point = mPositions[inPositionIdx];
+
+	// Find the face for which the point is furthest away
+	Face *best_face;
+	float best_dist_sq;
+	GetFaceForPoint(point, inFaces, best_face, best_dist_sq);
 
-	// If this point is in front of the face, add it to the conflict list
 	if (best_face != nullptr)
 	{
-		if (best_dist_sq > best_face->mFurthestPointDistanceSq)
+		// Check if this point is within the tolerance margin to the plane
+		if (best_dist_sq <= inToleranceSq)
 		{
-			// This point is futher away than any others, update the distance and add point as last point
-			best_face->mFurthestPointDistanceSq = best_dist_sq;
-			best_face->mConflictList.push_back(inPositionIdx);
+			// Check distance to edges
+			float dist_to_edge_sq = GetDistanceToEdgeSq(point, best_face);
+			if (dist_to_edge_sq > inToleranceSq)
+			{
+				// Point is outside of the face and too far away to discard
+				mCoplanarList.push_back({ inPositionIdx, dist_to_edge_sq });
+			}
 		}
 		else
 		{
-			// Not the furthest point, add it as the before last point
-			best_face->mConflictList.push_back(best_face->mConflictList.back());
-			best_face->mConflictList[best_face->mConflictList.size() - 2] = inPositionIdx;
+			// This point is in front of the face, add it to the conflict list
+			if (best_dist_sq > best_face->mFurthestPointDistanceSq)
+			{
+				// This point is futher away than any others, update the distance and add point as last point
+				best_face->mFurthestPointDistanceSq = best_dist_sq;
+				best_face->mConflictList.push_back(inPositionIdx);
+			}
+			else
+			{
+				// Not the furthest point, add it as the before last point
+				best_face->mConflictList.push_back(best_face->mConflictList.back());
+				best_face->mConflictList[best_face->mConflictList.size() - 2] = inPositionIdx;
+			}
+			
+			return true;
 		}
 	}
+
+	return false;
 }
 
 float ConvexHullBuilder::DetermineCoplanarDistance() const
@@ -433,7 +485,7 @@ ConvexHullBuilder::EResult ConvexHullBuilder::Initialize(int inMaxVertices, floa
 	Faces faces { t1, t2, t3, t4 };
 	for (int idx = 0; idx < (int)mPositions.size(); ++idx)
 		if (idx != idx1 && idx != idx2 && idx != idx3 && idx != idx4)
-			AssignPointToFace(idx, faces);
+			AssignPointToFace(idx, faces, tolerance_sq);
 
 #ifdef JPH_CONVEX_BUILDER_DEBUG
 	// Draw current state including conflict list
@@ -459,9 +511,64 @@ ConvexHullBuilder::EResult ConvexHullBuilder::Initialize(int inMaxVertices, floa
 				face_with_furthest_point = f;
 			}
 
-		// If there is none closer than our tolerance value, we're done
-		if (face_with_furthest_point == nullptr || furthest_dist_sq < tolerance_sq)
+		int furthest_point_idx;
+		if (face_with_furthest_point != nullptr)
+		{
+			// Take the furthest point
+			furthest_point_idx = face_with_furthest_point->mConflictList.back();
+			face_with_furthest_point->mConflictList.pop_back();
+		}
+		else if (!mCoplanarList.empty())
+		{
+			// Try to assign points to faces (this also recalculates the distance to the hull for the coplanar vertices)
+			CoplanarList coplanar;
+			mCoplanarList.swap(coplanar);
+			bool added = false;
+			for (const Coplanar &c : coplanar)
+				added |= AssignPointToFace(c.mPositionIdx, mFaces, tolerance_sq);
+
+			// If we were able to assign a point, loop again to pick it up
+			if (added)
+				continue;
+
+			// If the coplanar list is empty, there are no points left and we're done
+			if (mCoplanarList.empty())
+				break;
+
+			do
+			{
+				// Find the vertex that is furthest from the hull
+				CoplanarList::size_type best_idx = 0;
+				float best_dist_sq = mCoplanarList.front().mDistanceSq;
+				for (CoplanarList::size_type idx = 1; idx < mCoplanarList.size(); ++idx)
+				{
+					const Coplanar &c = mCoplanarList[idx];
+					if (c.mDistanceSq > best_dist_sq)
+					{
+						best_idx = idx;
+						best_dist_sq = c.mDistanceSq;
+					}
+				}
+
+				// Swap it to the end
+				swap(mCoplanarList[best_idx], mCoplanarList.back());
+
+				// Remove it
+				furthest_point_idx = mCoplanarList.back().mPositionIdx;
+				mCoplanarList.pop_back();
+
+				// Find the face for which the point is furthest away
+				GetFaceForPoint(mPositions[furthest_point_idx], mFaces, face_with_furthest_point, best_dist_sq);
+			} while (!mCoplanarList.empty() && face_with_furthest_point == nullptr);
+
+			if (face_with_furthest_point == nullptr)
+				break;
+		}
+		else
+		{
+			// If there are no more vertices, we're done
 			break;
+		}
 
 		// Check if we have a limit on the max vertices that we should produce
 		if (num_vertices_used >= inMaxVertices)
@@ -477,10 +584,6 @@ ConvexHullBuilder::EResult ConvexHullBuilder::Initialize(int inMaxVertices, floa
 		// We're about to add another vertex
 		++num_vertices_used;
 
-		// Take the furthest point
-		int furthest_point_idx = face_with_furthest_point->mConflictList.back();
-		face_with_furthest_point->mConflictList.pop_back();
-
 		// Add the point to the hull
 		Faces new_faces;
 		AddPoint(face_with_furthest_point, furthest_point_idx, coplanar_tolerance_sq, new_faces);
@@ -489,7 +592,7 @@ ConvexHullBuilder::EResult ConvexHullBuilder::Initialize(int inMaxVertices, floa
 		for (const Face *face : mFaces)
 			if (face->mRemoved)
 				for (int idx : face->mConflictList)
-					AssignPointToFace(idx, new_faces);
+					AssignPointToFace(idx, new_faces, tolerance_sq);
 
 		// Permanently delete faces that we removed in AddPoint()
 		GarbageCollectFaces();
@@ -1055,7 +1158,7 @@ bool ConvexHullBuilder::RemoveTwoEdgeFace(Face *inFace, Faces &ioAffectedFaces)
 
 	return false;
 }
-	
+
 #ifdef JPH_ENABLE_ASSERTS
 
 void ConvexHullBuilder::DumpFace(const Face *inFace) const
@@ -1208,7 +1311,7 @@ void ConvexHullBuilder::DetermineMaxError(Face *&outFaceWithMaxError, float &out
 		// Note that we take the min of all faces since there may be multiple near coplanar faces so if we were to test this per face
 		// we may find that a point is outside of a polygon and mark it as an error, while it is actually inside a nearly coplanar
 		// polygon.
-		float min_edge_dist = FLT_MAX;
+		float min_edge_dist_sq = FLT_MAX;
 		Face *min_edge_dist_face = nullptr;
 
 		for (Face *f : mFaces)
@@ -1219,50 +1322,26 @@ void ConvexHullBuilder::DetermineMaxError(Face *&outFaceWithMaxError, float &out
 			float plane_dist = f->mNormal.Dot(v - f->mCentroid) / normal_len;
 			if (plane_dist > -outCoplanarDistance)
 			{
-				bool all_inside = true;
-
-				// Test if it is inside the edges of the polygon
-				Edge *edge = f->mFirstEdge;
-				Vec3 p1 = mPositions[edge->GetPreviousEdge()->mStartIdx];
-				do
+				// Check distance to the edges of this face
+				float edge_dist_sq = GetDistanceToEdgeSq(v, f);
+				if (edge_dist_sq < min_edge_dist_sq)
 				{
-					Vec3 p2 = mPositions[edge->mStartIdx];
-					if ((p2 - p1).Cross(v - p1).Dot(f->mNormal) < 0.0f)
-					{
-						// It is outside
-						all_inside = false;
-
-						// Measure distance to this edge
-						uint32 s;
-						float edge_dist = ClosestPoint::GetClosestPointOnLine(p1 - v, p2 - v, s).Length();
-						if (edge_dist < min_edge_dist)
-						{
-							min_edge_dist = edge_dist;
-							min_edge_dist_face = f;
-						}
-					}
-					p1 = p2;
-					edge = edge->mNextEdge;
-				} while (edge != f->mFirstEdge);
-
-				if (all_inside)
-				{
-					// The point is inside the polygon, reset distance to edge
-					min_edge_dist = 0.0f;
+					min_edge_dist_sq = edge_dist_sq;
 					min_edge_dist_face = f;
+				}
 
-					// If the point is in front of the plane, measure the distance
-					if (plane_dist > max_error)
-					{
-						max_error = plane_dist;
-						max_error_face = f;
-						max_error_point = i;
-					}
+				// If the point is inside the polygon and the point is in front of the plane, measure the distance
+				if (edge_dist_sq == 0.0f && plane_dist > max_error)
+				{
+					max_error = plane_dist;
+					max_error_face = f;
+					max_error_point = i;
 				}
 			}
 		}
 
 		// If the minimum distance to an edge is further than our current max error, we use that as max error
+		float min_edge_dist = sqrt(min_edge_dist_sq);
 		if (min_edge_dist_face != nullptr && min_edge_dist > max_error)
 		{
 			max_error = min_edge_dist;
@@ -1319,7 +1398,7 @@ void ConvexHullBuilder::DrawState(bool inDrawConflictList) const
 
 			// Draw normal
 			Vec3 centroid = cDrawScale * (f->mCentroid + mOffset);
-			DebugRenderer::sInstance->DrawArrow(centroid, centroid + f->mNormal.Normalized(), face_color, 0.01f);
+			DebugRenderer::sInstance->DrawArrow(centroid, centroid + f->mNormal.NormalizedOr(Vec3::sZero()), face_color, 0.01f);
 
 			// Draw conflict list
 			if (inDrawConflictList)

Jolt/Geometry/ConvexHullBuilder.h

@@ -147,10 +147,25 @@ private:
 	// Determine a suitable tolerance for detecting that points are coplanar
 	float				DetermineCoplanarDistance() const;
 
+	/// Find the face for which inPoint is furthest to the front
+	/// @param inPoint Point to test
+	/// @param inFaces List of faces to test
+	/// @param outFace Returns the best face
+	/// @param outDistSq Returns the squared distance how much inPoint is in front of the plane of the face
+	void				GetFaceForPoint(Vec3Arg inPoint, const Faces &inFaces, Face *&outFace, float &outDistSq) const;
+
+	/// @brief Calculates the distance between inPoint and inFace
+	/// @param inFace Face to test
+	/// @param inPoint Point to test
+	/// @return If the projection of the point on the plane is interior to the face 0, otherwise the squared distance to the closest edge
+	float				GetDistanceToEdgeSq(Vec3Arg inPoint, const Face *inFace) const;
+
 	/// Assigns a position to one of the supplied faces based on which face is closest.
 	/// @param inPositionIdx Index of the position to add
 	/// @param inFaces List of faces to consider
-	void				AssignPointToFace(int inPositionIdx, const Faces &inFaces) const;
+	/// @param inToleranceSq Tolerance of the hull, if the point is closer to the face than this, we ignore it
+	/// @return True if point was assigned, false if it was discarded or added to the coplanar list
+	bool				AssignPointToFace(int inPositionIdx, const Faces &inFaces, float inToleranceSq);
 
 	/// Add a new point to the convex hull
 	void				AddPoint(Face *inFacingFace, int inIdx, float inToleranceSq, Faces &outNewFaces);
@@ -236,6 +251,15 @@ private:
 	const Positions &	mPositions;							///< List of positions (some of them are part of the hull)
 	Faces 				mFaces;								///< List of faces that are part of the hull (if !mRemoved)
 
+	struct Coplanar
+	{
+		int				mPositionIdx;						///< Index in mPositions
+		float			mDistanceSq;						///< Distance to the edge of closest face (should be > 0)
+	};
+	using CoplanarList = vector<Coplanar>;
+
+	CoplanarList		mCoplanarList;						///< List of positions that are coplanar to a face but outside of the face, these are added to the hull at the end
+
 #ifdef JPH_CONVEX_BUILDER_DEBUG
 	int					mIteration;							///< Number of iterations we've had so far (for debug purposes)	
 	mutable Vec3		mOffset;							///< Offset to use for state drawing

Samples/Tests/ConvexCollision/ConvexHullTest.cpp

@@ -376,6 +376,36 @@ void ConvexHullTest::Initialize()
 			Vec3(-0.00565947127f, 0.00477081537f, -0.0243848339f),
 			Vec3(0.0118075963f, 0.00124305487f, -0.0258472487f),
 			Vec3(0.00860248506f, -0.00697988272f, -0.0276725553f),
+		},
+		{
+			// Nearly co-planar hull (but not enough to go through the 2d hull builder)
+			Vec3(0.129325435f, -0.213319957f, 0.00901593268f),
+			Vec3(0.129251331f, -0.213436425f, 0.00932094082f),
+			Vec3(0.160741553f, -0.171540618f, 0.0494558439f),
+			Vec3(0.160671368f, -0.17165187f, 0.049765937f),
+			Vec3(0.14228563f, 0.432965666f, 0.282429159f),
+			Vec3(0.142746598f, 0.433226734f, 0.283286631f),
+			Vec3(0.296031296f, 0.226935148f, 0.312804461f),
+			Vec3(0.296214104f, 0.227568939f, 0.313606918f),
+			Vec3(-0.00354258716f, -0.180767179f, -0.0762089267f),
+			Vec3(-0.00372517109f, -0.1805875f, -0.0766792595f),
+			Vec3(-0.0157070309f, -0.176182508f, -0.0833940506f),
+			Vec3(-0.0161666721f, -0.175898403f, -0.0840280354f),
+			Vec3(-0.342764735f, 0.0259497911f, -0.244388372f),
+			Vec3(-0.342298329f, 0.0256615728f, -0.24456653f),
+			Vec3(-0.366584063f, 0.0554589033f, -0.250078142f),
+			Vec3(-0.366478682f, 0.0556178838f, -0.250342518f),
+		},
+		{
+			// A hull with a very acute angle that won't properly build when using distance to plane only
+			Vec3(-0.0451235026f, -0.103826642f, -0.0346511155f),
+			Vec3(-0.0194563419f, -0.123563275f, -0.032212317f),
+			Vec3(0.0323024541f, -0.0468643308f, -0.0307639092f),
+			Vec3(0.0412166864f, -0.0884782523f, -0.0288816988f),
+			Vec3(-0.0564572513f, 0.0207469314f, 0.0169318169f),
+			Vec3(0.00537410378f, 0.105688639f, 0.0355164111f),
+			Vec3(0.0209896415f, 0.117749952f, 0.0365252197f),
+			Vec3(0.0211542398f, 0.118546993f, 0.0375355929f),
 		}
 	};
 
@@ -400,6 +430,18 @@ void ConvexHullTest::Initialize()
 		mPoints.push_back(move(p));
 	}
 
+	// Add wedge shaped disc that is just above the hull tolerance on its widest side and zero on the other side
+	{
+		Points p;
+		for (float phi = 0.0f; phi <= 2.0f * JPH_PI; phi += 2.0f * JPH_PI / 40.0f)
+		{
+			Vec3 pos(2.0f * cos(phi), 0, 2.0f * sin(phi));
+			p.push_back(pos);
+			p.push_back(pos + Vec3(0, 2.0e-3f * (2.0f + pos.GetX()) / 4.0f, 0));
+		}
+		mPoints.push_back(move(p));
+	}
+
 	// Add a sphere of many points
 	{
 		Points p;