Making hull builder more robust (#735)

nitially I tried to make gift wrapping handle collinear points better. Conclusion: gift wrapping is a bad algorithm.
So I removed gift wrapping and implemented quickhull. I also added code to explicitly remove collinear points. This turns out to be quite easy once you have a convex hull.

I ran over 200 million randomized tests successfully with test data that is very likely to have collinear points with small numerical deviations. All the tests passed and algorithm correctly rejects very close points and fully collinear points.

I also tested performance at around 150 milliseconds per million hulls.

I split out the convex hull code into a separate function b2ComputeHull. This lets people build hulls offline and check for hull validity before using them in b2PolygonShape.

This video is a good reference for the 2D quickhull algorithm (my implementation is different): https://www.youtube.com/watch?v=2EKIZrimeuk

Fixes: #671 #728

include/box2d/b2_collision.h

@@ -255,6 +255,29 @@ B2_API bool b2TestOverlap(	const b2Shape* shapeA, int32 indexA,
 					const b2Shape* shapeB, int32 indexB,
 					const b2Transform& xfA, const b2Transform& xfB);
 
+/// Convex hull used for polygon collision
+struct b2Hull
+{
+	b2Vec2 points[b2_maxPolygonVertices];
+	int32 count;
+};
+
+/// Compute the convex hull of a set of points. Returns an empty hull if it fails.
+/// Some failure cases:
+/// - all points very close together
+/// - all points on a line
+/// - less than 3 points
+/// - more than b2_maxPolygonVertices points
+/// This welds close points and removes collinear points.
+b2Hull b2ComputeHull(const b2Vec2* points, int32 count);
+
+/// This determines if a hull is valid. Checks for:
+/// - convexity
+/// - collinear points
+/// This is expensive and should not be called at runtime.
+bool b2ValidateHull(const b2Hull& hull);
+
+
 // ---------------- Inline Functions ------------------------------------------
 
 inline bool b2AABB::IsValid() const

include/box2d/b2_polygon_shape.h

@@ -25,6 +25,8 @@
 #include "b2_api.h"
 #include "b2_shape.h"
 
+struct b2Hull;
+
 /// A solid convex polygon. It is assumed that the interior of the polygon is to
 /// the left of each edge.
 /// Polygons have a maximum number of vertices equal to b2_maxPolygonVertices.
@@ -43,9 +45,13 @@ class B2_API b2PolygonShape : public b2Shape
 	/// Create a convex hull from the given array of local points.
 	/// The count must be in the range [3, b2_maxPolygonVertices].
 	/// @warning the points may be re-ordered, even if they form a convex polygon
-	/// @warning collinear points are handled but not removed. Collinear points
-	/// may lead to poor stacking behavior.
-	void Set(const b2Vec2* points, int32 count);
+	/// @warning if this fails then the polygon is invalid
+	/// @returns true if valid
+	bool Set(const b2Vec2* points, int32 count);
+
+	/// Create a polygon from a given convex hull (see b2ComputeHull).
+	/// @warning the hull must be valid or this will crash or have unexpected behavior
+	void Set(const b2Hull& hull);
 
 	/// Build vertices to represent an axis-aligned box centered on the local origin.
 	/// @param hx the half-width.
@@ -84,12 +90,4 @@ class B2_API b2PolygonShape : public b2Shape
 	int32 m_count;
 };
 
-inline b2PolygonShape::b2PolygonShape()
-{
-	m_type = e_polygon;
-	m_radius = b2_polygonRadius;
-	m_count = 0;
-	m_centroid.SetZero();
-}
-
 #endif

src/collision/b2_collision.cpp

@@ -256,3 +256,325 @@ bool b2TestOverlap(	const b2Shape* shapeA, int32 indexA,
 
 	return output.distance < 10.0f * b2_epsilon;
 }
+
+// quickhull recursion
+static b2Hull b2RecurseHull(b2Vec2 p1, b2Vec2 p2, b2Vec2* ps, int32 count)
+{
+	b2Hull hull;
+	hull.count = 0;
+
+	if (count == 0)
+	{
+		return hull;
+	}
+
+	// create an edge vector pointing from p1 to p2
+	b2Vec2 e = p2 - p1;
+	e.Normalize();
+
+	// discard points left of e and find point furthest to the right of e
+	b2Vec2 rightPoints[b2_maxPolygonVertices]{};
+	int32 rightCount = 0;
+
+	int32 bestIndex = 0;
+	float bestDistance = b2Cross(ps[bestIndex] - p1, e);
+	if (bestDistance > 0.0f)
+	{
+		rightPoints[rightCount++] = ps[bestIndex];
+	}
+
+	for (int32 i = 1; i < count; ++i)
+	{
+		float distance = b2Cross(ps[i] - p1, e);
+		if (distance > bestDistance)
+		{
+			bestIndex = i;
+			bestDistance = distance;
+		}
+
+		if (distance > 0.0f)
+		{
+			rightPoints[rightCount++] = ps[i];
+		}
+	}
+
+	if (bestDistance < 2.0f * b2_linearSlop)
+	{
+		return hull;
+	}
+
+	b2Vec2 bestPoint = ps[bestIndex];
+
+	// compute hull to the right of p1-bestPoint
+	b2Hull hull1 = b2RecurseHull(p1, bestPoint, rightPoints, rightCount);
+
+	// compute hull to the right of bestPoint-p2
+	b2Hull hull2 = b2RecurseHull(bestPoint, p2, rightPoints, rightCount);
+
+	// stich together hulls
+	for (int32 i = 0; i < hull1.count; ++i)
+	{
+		hull.points[hull.count++] = hull1.points[i];
+	}
+
+	hull.points[hull.count++] = bestPoint;
+
+	for (int32 i = 0; i < hull2.count; ++i)
+	{
+		hull.points[hull.count++] = hull2.points[i];
+	}
+
+	b2Assert(hull.count < b2_maxPolygonVertices);
+
+	return hull;
+}
+
+// quickhull algorithm
+// - merges vertices based on b2_linearSlop
+// - removes collinear points using b2_linearSlop
+// - returns an empty hull if it fails
+b2Hull b2ComputeHull(const b2Vec2* points, int32 count)
+{
+	b2Hull hull;
+	hull.count = 0;
+
+	if (count < 3 || count > b2_maxPolygonVertices)
+	{
+		// check your data
+		return hull;
+	}
+
+	count = b2Min(count, b2_maxPolygonVertices);
+
+	b2AABB aabb = { {b2_maxFloat, b2_maxFloat}, {-b2_maxFloat, -b2_maxFloat} };
+
+	// Perform aggressive point welding. First point always remains.
+	// Also compute the bounding box for later.
+	b2Vec2 ps[b2_maxPolygonVertices];
+	int32 n = 0;
+	const float tolSqr = 16.0f * b2_linearSlop * b2_linearSlop;
+	for (int32 i = 0; i < count; ++i)
+	{
+		aabb.lowerBound = b2Min(aabb.lowerBound, points[i]);
+		aabb.upperBound = b2Max(aabb.upperBound, points[i]);
+
+		b2Vec2 vi = points[i];
+
+		bool unique = true;
+		for (int32 j = 0; j < i; ++j)
+		{
+			b2Vec2 vj = points[j];
+
+			float distSqr = b2DistanceSquared(vi, vj);
+			if (distSqr < tolSqr)
+			{
+				unique = false;
+				break;
+			}
+		}
+
+		if (unique)
+		{
+			ps[n++] = vi;
+		}
+	}
+
+	if (n < 3)
+	{
+		// all points very close together, check your data and check your scale
+		return hull;
+	}
+
+	// Find an extreme point as the first point on the hull
+	b2Vec2 c = aabb.GetCenter();
+	int32 i1 = 0;
+	float dsq1 = b2DistanceSquared(c, ps[i1]);
+	for (int32 i = 1; i < n; ++i)
+	{
+		float dsq = b2DistanceSquared(c, ps[i]);
+		if (dsq > dsq1)
+		{
+			i1 = i;
+			dsq1 = dsq;
+		}
+	}
+
+	// remove p1 from working set
+	b2Vec2 p1 = ps[i1];
+	ps[i1] = ps[n - 1];
+	n = n - 1;
+
+	int32 i2 = 0;
+	float dsq2 = b2DistanceSquared(p1, ps[i2]);
+	for (int32 i = 1; i < n; ++i)
+	{
+		float dsq = b2DistanceSquared(p1, ps[i]);
+		if (dsq > dsq2)
+		{
+			i2 = i;
+			dsq2 = dsq;
+		}
+	}
+
+	// remove p2 from working set
+	b2Vec2 p2 = ps[i2];
+	ps[i2] = ps[n - 1];
+	n = n - 1;
+
+	// split the points into points that are left and right of the line p1-p2.
+	b2Vec2 rightPoints[b2_maxPolygonVertices - 2];
+	int32 rightCount = 0;
+
+	b2Vec2 leftPoints[b2_maxPolygonVertices - 2];
+	int32 leftCount = 0;
+
+	b2Vec2 e = p2 - p1;
+	e.Normalize();
+
+	for (int32 i = 0; i < n; ++i)
+	{
+		float d = b2Cross(ps[i] - p1, e);
+
+		// slop used here to skip points that are very close to the line p1-p2
+		if (d >= 2.0f * b2_linearSlop)
+		{
+			rightPoints[rightCount++] = ps[i];
+		}
+		else if (d <= -2.0f * b2_linearSlop)
+		{
+			leftPoints[leftCount++] = ps[i];
+		}
+	}
+
+	// compute hulls on right and left
+	b2Hull hull1 = b2RecurseHull(p1, p2, rightPoints, rightCount);
+	b2Hull hull2 = b2RecurseHull(p2, p1, leftPoints, leftCount);
+
+	if (hull1.count == 0 && hull2.count == 0)
+	{
+		// all points collinear
+		return hull;
+	}
+
+	// stitch hulls together, preserving CCW winding order
+	hull.points[hull.count++] = p1;
+
+	for (int32 i = 0; i < hull1.count; ++i)
+	{
+		hull.points[hull.count++] = hull1.points[i];
+	}
+
+	hull.points[hull.count++] = p2;
+
+	for (int32 i = 0; i < hull2.count; ++i)
+	{
+		hull.points[hull.count++] = hull2.points[i];
+	}
+
+	b2Assert(hull.count <= b2_maxPolygonVertices);
+
+	// merge collinear
+	bool searching = true;
+	while (searching && hull.count > 2)
+	{
+		searching = false;
+
+		for (int32 i = 0; i < hull.count; ++i)
+		{
+			int32 i1 = i;
+			int32 i2 = (i + 1) % hull.count;
+			int32 i3 = (i + 2) % hull.count;
+
+			b2Vec2 p1 = hull.points[i1];
+			b2Vec2 p2 = hull.points[i2];
+			b2Vec2 p3 = hull.points[i3];
+
+			b2Vec2 e = p3 - p1;
+			e.Normalize();
+
+			b2Vec2 v = p2 - p1;
+			float distance = b2Cross(p2 - p1, e);
+			if (distance <= 2.0f * b2_linearSlop)
+			{
+				// remove midpoint from hull
+				for (int32 j = i2; j < hull.count - 1; ++j)
+				{
+					hull.points[j] = hull.points[j + 1];
+				}
+				hull.count -= 1;
+
+				// continue searching for collinear points
+				searching = true;
+
+				break;
+			}
+		}
+	}
+
+	if (hull.count < 3)
+	{
+		// all points collinear, shouldn't be reached since this was validated above
+		hull.count = 0;
+	}
+
+	return hull;
+}
+
+bool b2ValidateHull(const b2Hull& hull)
+{
+	if (hull.count < 3 || b2_maxPolygonVertices < hull.count)
+	{
+		return false;
+	}
+
+	// test that every point is behind every edge
+	for (int32 i = 0; i < hull.count; ++i)
+	{
+		// create an edge vector
+		int32 i1 = i;
+		int32 i2 = i < hull.count - 1 ? i1 + 1 : 0;
+		b2Vec2 p = hull.points[i1];
+		b2Vec2 e = hull.points[i2] - p;
+		e.Normalize();
+
+		for (int32 j = 0; j < hull.count; ++j)
+		{
+			// skip points that subtend the current edge
+			if (j == i1 || j == i2)
+			{
+				continue;
+			}
+
+			float distance = b2Cross(hull.points[j] - p, e);
+			if (distance >= 0.0f)
+			{
+				return false;
+			}
+		}
+	}
+
+	// test for collinear points
+	for (int32 i = 0; i < hull.count; ++i)
+	{
+		int32 i1 = i;
+		int32 i2 = (i + 1) % hull.count;
+		int32 i3 = (i + 2) % hull.count;
+
+		b2Vec2 p1 = hull.points[i1];
+		b2Vec2 p2 = hull.points[i2];
+		b2Vec2 p3 = hull.points[i3];
+
+		b2Vec2 e = p3 - p1;
+		e.Normalize();
+
+		b2Vec2 v = p2 - p1;
+		float distance = b2Cross(p2 - p1, e);
+		if (distance <= b2_linearSlop)
+		{
+			// p1-p2-p3 are collinear
+			return false;
+		}
+	}
+
+	return true;
+}