translate the hull; invalid catch

include/bounce/quickhull/qh_hull.h

@@ -64,15 +64,14 @@ struct qhFace
 
 struct qhHalfEdge
 {
+	qhVertex* tail;
+
 	qhHalfEdge* prev;
 	qhHalfEdge* next;
-
 	qhHalfEdge* twin;
 
 	qhFace* face;
 
-	qhVertex* tail;
-
 	//
 	qhHalfEdge* freeNext;
 	bool active;
@@ -111,6 +110,9 @@ public:
 	// Get the list of faces in this convex hull.
 	const qhList<qhFace>& GetFaceList() const;
 
+	// Translate this hull.
+	void Translate(const b3Vec3& translation);
+
 	// Get the number of iterations this algorithm ran.
 	u32 GetIterations() const;
 
@@ -173,6 +175,14 @@ private:
 	
 	void* m_buffer;
 
+	u32 m_vertexCapacity;
+	u32 m_faceCapacity;
+	u32 m_edgeCapacity;
+
+	u32 m_vertexCount;
+	u32 m_faceCount;
+	u32 m_edgeCount;
+	
 	qhVertex* m_freeVertices;
 	qhHalfEdge* m_freeEdges;
 	qhFace* m_freeFaces;

include/bounce/quickhull/qh_hull.inl

@@ -42,39 +42,6 @@ inline T* qhList<T>::Remove(T* link)
 
 // qhHull
 
-// Given a number of points return the required memory size in 
-// bytes for constructing the convex hull of those points. 
-inline u32 qhGetBufferSize(u32 pointCount)
-{
-	u32 size = 0;
-
-	// Hull using Euler's Formula
-	u32 V = pointCount;
-	u32 E = 3 * V - 6;
-	u32 HE = 2 * E;
-	u32 F = 2 * V - 4;
-
-	size += V * sizeof(qhVertex);
-	size += HE * sizeof(qhHalfEdge);
-	size += F * sizeof(qhFace);
-
-	// Horizon 
-	size += HE * sizeof(qhHalfEdge*);
-
-	// Saved horizon vertices
-	// One vertex per horizon edge
-	size += HE * sizeof(qhVertex*);
-
-	// Saved conflict vertices
-	size += V * sizeof(qhVertex*);
-
-	// New Faces
-	// One face per horizon edge
-	size += HE * sizeof(qhFace*);
-
-	return size;
-}
-
 inline const qhList<qhVertex>& qhHull::GetVertexList() const
 {
 	return m_vertexList;
@@ -92,6 +59,9 @@ inline u32 qhHull::GetIterations() const
 
 inline qhVertex* qhHull::AllocateVertex()
 {
+	B3_ASSERT(m_vertexCount < m_vertexCapacity);
+	++m_vertexCount;
+
 	qhVertex* v = m_freeVertices;
 	B3_ASSERT(v->active == false);
 	v->active = true;
@@ -101,7 +71,10 @@ inline qhVertex* qhHull::AllocateVertex()
 
 inline void qhHull::FreeVertex(qhVertex* v)
 {
-	//B3_ASSERT(v->active == true);
+	B3_ASSERT(m_vertexCount > 0);
+	--m_vertexCount;
+
+	B3_ASSERT(v->active == true);
 	v->active = false;
 	v->freeNext = m_freeVertices;
 	m_freeVertices = v;
@@ -109,6 +82,9 @@ inline void qhHull::FreeVertex(qhVertex* v)
 
 inline qhHalfEdge* qhHull::AllocateEdge()
 {
+	B3_ASSERT(m_edgeCount < m_edgeCapacity);
+	++m_edgeCount;
+
 	qhHalfEdge* e = m_freeEdges;
 	B3_ASSERT(e->active == false);
 	e->active = true;
@@ -118,7 +94,10 @@ inline qhHalfEdge* qhHull::AllocateEdge()
 
 inline void qhHull::FreeEdge(qhHalfEdge* e)
 {
-	//B3_ASSERT(e->active == true);
+	B3_ASSERT(m_edgeCount > 0);
+	--m_edgeCount;
+
+	B3_ASSERT(e->active == true);
 	e->active = false;
 	e->freeNext = m_freeEdges;
 	m_freeEdges = e;
@@ -126,6 +105,9 @@ inline void qhHull::FreeEdge(qhHalfEdge* e)
 
 inline qhFace* qhHull::AllocateFace()
 {
+	B3_ASSERT(m_faceCount < m_faceCapacity);
+	++m_faceCount;
+
 	qhFace* f = m_freeFaces;
 	B3_ASSERT(f->active == false);
 	f->active = true;
@@ -135,31 +117,11 @@ inline qhFace* qhHull::AllocateFace()
 
 inline void qhHull::FreeFace(qhFace* f)
 {
-	//B3_ASSERT(f->active == true);
+	B3_ASSERT(m_faceCount > 0);
+	--m_faceCount;
+	
+	B3_ASSERT(f->active == true);
 	f->active = false;
 	f->freeNext = m_freeFaces;
 	m_freeFaces = f;
 }
-
-inline qhHalfEdge* qhHull::FindHalfEdge(const qhVertex* v1, const qhVertex* v2) const
-{
-	for (qhFace* face = m_faceList.head; face != NULL; face = face->next)
-	{
-		qhHalfEdge* e = face->edge;
-		do
-		{
-			if (e->tail == v1 && e->next->tail == v2)
-			{
-				return e;
-			}
-
-			if (e->tail == v2 && e->next->tail == v1)
-			{
-				return e->twin;
-			}
-
-			e = e->next;
-		} while (e != face->edge);
-	}
-	return NULL;
-}

src/bounce/quickhull/qh_hull.cpp

@@ -62,7 +62,7 @@ qhHull::qhHull()
 qhHull::~qhHull()
 {
 	qhVertex* v = m_vertexList.head;
-	while(v) 
+	while (v)
 	{
 		qhVertex* v0 = v;
 		v = v->next;
@@ -70,6 +70,8 @@ qhHull::~qhHull()
 		FreeVertex(v0);
 	}
 
+	B3_ASSERT(m_vertexCount == 0);
+	
 	qhFace* f = m_faceList.head;
 	while (f)
 	{
@@ -88,36 +90,69 @@ qhHull::~qhHull()
 		FreeFace(f0);
 	}
 
+	B3_ASSERT(m_edgeCount == 0);
+	B3_ASSERT(m_faceCount == 0);
+
 	b3Free(m_buffer);
 }
 
 void qhHull::Construct(const b3Vec3* vs, u32 count)
 {
-	// Allocate memory buffer for the worst case.
+	// Compute memory buffer size for the worst case.
-	u32 bufferSize = qhGetBufferSize(count);
+	u32 size = 0;
-	m_buffer = b3Alloc(bufferSize);
 
-	// Euler's formula
+	// Hull using Euler's Formula
-	// V - E + F = 2
 	u32 V = count;
 	u32 E = 3 * V - 6;
 	u32 HE = 2 * E;
 	u32 F = 2 * V - 4;
 
+	size += V * sizeof(qhVertex);
+	size += HE * sizeof(qhHalfEdge);
+	size += F * sizeof(qhFace);
+
+	// Horizon 
+	size += HE * sizeof(qhHalfEdge*);
+
+	// Saved horizon vertices
+	// One vertex per horizon edge
+	size += HE * sizeof(qhVertex*);
+
+	// Saved conflict vertices
+	size += V * sizeof(qhVertex*);
+
+	// New Faces
+	// One face per horizon edge
+	size += HE * sizeof(qhFace*);
+
+	// Allocate memory buffer 
+	m_buffer = b3Alloc(size);
+
+	// Initialize free lists
+	m_vertexCapacity = V;
+	m_vertexCount = m_vertexCapacity;
 	m_freeVertices = NULL;
 	qhVertex* vertices = (qhVertex*)m_buffer;
 	for (u32 i = 0; i < V; ++i)
 	{
+		vertices[i].active = true;
 		FreeVertex(vertices + i);
 	}
+	B3_ASSERT(m_vertexCount == 0);
 
+	m_edgeCapacity = HE;
+	m_edgeCount = m_edgeCapacity;
 	m_freeEdges = NULL;
 	qhHalfEdge* edges = (qhHalfEdge*)((u8*)vertices + V * sizeof(qhVertex));
 	for (u32 i = 0; i < HE; ++i)
 	{
+		edges[i].active = true;
 		FreeEdge(edges + i);
 	}
+	B3_ASSERT(m_edgeCount == 0);
 
+	m_faceCapacity = F;
+	m_faceCount = m_faceCapacity;
 	m_freeFaces = NULL;
 	qhFace* faces = (qhFace*)((u8*)edges + HE * sizeof(qhHalfEdge));
 	for (u32 i = 0; i < F; ++i)
@@ -125,8 +160,10 @@ void qhHull::Construct(const b3Vec3* vs, u32 count)
 		qhFace* f = faces + i;
 		f->conflictList.head = NULL;
 		f->conflictList.count = 0;
+		f->active = true;
 		FreeFace(f);
 	}
+	B3_ASSERT(m_faceCount == 0);
 
 	m_horizon = (qhHalfEdge**)((u8*)faces + F * sizeof(qhFace));
 	m_horizonCount = 0;
@@ -163,6 +200,20 @@ void qhHull::Construct(const b3Vec3* vs, u32 count)
 
 		++m_iterations;
 	}
+
+	Validate();
+
+	// Ensure the hull contains the original vertex set
+	for (u32 i = 0; i < count; ++i)
+	{
+		b3Vec3 v = vs[i];
+
+		for (qhFace* f = m_faceList.head; f; f = f->next)
+		{
+			float32 d = b3Distance(v, f->plane);
+			//B3_ASSERT(d < m_tolerance);
+		}
+	}
 }
 
 bool qhHull::BuildInitialHull(const b3Vec3* vertices, u32 vertexCount)
@@ -578,12 +629,38 @@ qhVertex* qhHull::AddVertex(const b3Vec3& position)
 	return v;
 }
 
-static inline b3Vec3 b3Newell(const b3Vec3& a, const b3Vec3& b)
+inline qhHalfEdge* qhHull::FindHalfEdge(const qhVertex* v1, const qhVertex* v2) const
+{
+	for (qhFace* face = m_faceList.head; face != NULL; face = face->next)
+	{
+		qhHalfEdge* e = face->edge;
+		do
+		{
+			B3_ASSERT(e->active == true);
+			B3_ASSERT(e->twin->active == true);
+
+			if (e->tail == v1 && e->twin->tail == v2)
+			{
+				return e;
+			}
+
+			if (e->tail == v2 && e->twin->tail == v1)
+			{
+				return e->twin;
+			}
+
+			e = e->next;
+		} while (e != face->edge);
+	}
+	return NULL;
+}
+
+static B3_FORCE_INLINE b3Vec3 b3Newell(const b3Vec3& a, const b3Vec3& b)
 {
 	return b3Vec3((a.y - b.y) * (a.z + b.z), (a.z - b.z) * (a.x + b.x), (a.x - b.x) * (a.y + b.y));
 }
 
-static inline void b3ComputeFaceData(const qhFace* face, b3Plane& plane, b3Vec3& center, float32& area)
+static inline void b3ResetFaceData(qhFace* face)
 {
 	b3Vec3 n;
 	n.SetZero();
@@ -608,63 +685,81 @@ static inline void b3ComputeFaceData(const qhFace* face, b3Plane& plane, b3Vec3&
 	B3_ASSERT(count > 0);
 	c /= float32(count);
 
-	center = c;
+	face->center = c;
 
 	float32 len = b3Length(n);
-	area = 0.5f * len;
+	face->area = 0.5f * len;
 
 	if (len > B3_EPSILON)
 	{
 		n /= len;
 	}
 
-	plane.normal = n;
+	face->plane.normal = n;
-	plane.offset = b3Dot(n, c);
+	face->plane.offset = b3Dot(n, c);
 }
 
 qhFace* qhHull::RemoveEdge(qhHalfEdge* e)
 {
-	qhFace* leftFace = e->twin->face;
 	qhFace* rightFace = e->face;
 
-	// Move left vertices into right
+	qhHalfEdge* twin = e->twin;
+	qhFace* leftFace = twin->face;
+
+	// Set right face to reference a non-deleted edge
+	rightFace->edge = e->prev;
+
+	// Absorb face
+	qhHalfEdge* te = twin->next;
+	do
+	{
+		B3_ASSERT(te->face == leftFace);
+		te->face = rightFace;
+		te = te->next;
+	} while (te != twin->next);
+
+	// Link edges
+	e->prev->next = twin->next;
+	e->next->prev = twin->prev;
+	twin->prev->next = e->next;
+	twin->next->prev = e->prev;
+
+	// Remove edge
+	e->twin = NULL;
+	e->tail = NULL;
+	e->face = NULL;
+	e->prev = NULL;
+	e->next = NULL;
+	FreeEdge(e);
+
+	// Remove twin
+	twin->twin = NULL;
+	twin->tail = NULL;
+	twin->face = NULL;
+	twin->prev = NULL;
+	twin->next = NULL;
+	FreeEdge(twin);
+
+	// Move left conflict vertices into right
 	qhVertex* v = leftFace->conflictList.head;
 	while (v)
 	{
 		qhVertex* v0 = v;
 		v = leftFace->conflictList.Remove(v);
+
 		rightFace->conflictList.PushFront(v0);
 		v0->conflictFace = rightFace;
 	}
 
-	// Set right face to reference a non-deleted edge
+	// Remove left face
-	B3_ASSERT(e->face == rightFace);
+	leftFace->edge = NULL;
-	rightFace->edge = e->prev;
 
-	// Absorb face
-	qhHalfEdge* te = e->twin;
-	do
-	{
-		te->face = rightFace;
-		te = te->next;
-	} while (te != e->twin);
-
-	// Link edges
-	e->prev->next = e->twin->next;
-	e->next->prev = e->twin->prev;
-	e->twin->prev->next = e->next;
-	e->twin->next->prev = e->prev;
-
-	FreeEdge(e->twin);
-	FreeEdge(e);
 	m_faceList.Remove(leftFace);
+
 	FreeFace(leftFace);
 
-	// Compute face center and plane
+	// Reset face data
-	b3ComputeFaceData(rightFace, rightFace->plane, rightFace->center, rightFace->area);
+	b3ResetFaceData(rightFace);
-	
-	// Validate
-	//Validate(rightFace);
 
 	return rightFace;
 }
@@ -677,87 +772,91 @@ qhFace* qhHull::AddFace(qhVertex* v1, qhVertex* v2, qhVertex* v3)
 	if (e1 == NULL)
 	{
 		e1 = AllocateEdge();
-		e1->face = NULL;
+		e1->tail = v1;
-		e1->tail = NULL;
+		e1->face = face;
+		e1->prev = NULL;
+		e1->next = NULL;
 
 		e1->twin = AllocateEdge();
+		e1->twin->tail = v2;
 		e1->twin->face = NULL;
-		e1->twin->tail = NULL;
+		e1->twin->prev = NULL;
-
+		e1->twin->next = NULL;
 		e1->twin->twin = e1;
 	}
-
+	else
-	if (e1->tail == NULL)
 	{
-		e1->tail = v1;
+		B3_ASSERT(e1->tail == v1);
-	}
 
-	if (e1->face == NULL)
+		if (e1->face == NULL)
-	{
+		{
-		e1->face = face;
+			e1->face = face;
-	}
+		}
 
-	if (e1->twin->tail == NULL)
+		B3_ASSERT(e1->twin != NULL);
-	{
+		B3_ASSERT(e1->twin->active == true);
-		e1->twin->tail = v2;
+		B3_ASSERT(e1->twin->tail == v2);
 	}
 
 	qhHalfEdge* e2 = FindHalfEdge(v2, v3);
 	if (e2 == NULL)
 	{
 		e2 = AllocateEdge();
-		e2->face = NULL;
+		e2->tail = v2;
-		e2->tail = NULL;
+		e2->face = face;
+		e2->prev = NULL;
+		e2->next = NULL;
 
 		e2->twin = AllocateEdge();
+		e2->twin->tail = v3;
 		e2->twin->face = NULL;
-		e2->twin->tail = NULL;
+		e2->twin->prev = NULL;
-
+		e2->twin->next = NULL;
 		e2->twin->twin = e2;
 	}
-
+	else
-	if (e2->face == NULL)
 	{
-		e2->face = face;
+		B3_ASSERT(e2->tail == v2);
-	}
 
-	if (e2->tail == NULL)
+		if (e2->face == NULL)
-	{
+		{
-		e2->tail = v2;
+			e2->face = face;
-	}
+		}
 
-	if (e2->twin->tail == NULL)
+		B3_ASSERT(e2->twin != NULL);
-	{
+		B3_ASSERT(e2->twin->active == true);
-		e2->twin->tail = v3;
+		B3_ASSERT(e2->twin->tail == v3);
 	}
 
 	qhHalfEdge* e3 = FindHalfEdge(v3, v1);
 	if (e3 == NULL)
 	{
 		e3 = AllocateEdge();
-		e3->face = NULL;
+		e3->tail = v3;
-		e3->tail = NULL;
+		e3->face = face;
+		e3->prev = NULL;
+		e3->next = NULL;
 
 		e3->twin = AllocateEdge();
+		e3->twin->tail = v1;
 		e3->twin->face = NULL;
-		e3->twin->tail = NULL;
+		e3->twin->prev = NULL;
+		e3->twin->next = NULL;
 
 		e3->twin->twin = e3;
 	}
-
+	else
-	if (e3->face == NULL)
 	{
-		e3->face = face;
+		B3_ASSERT(e3->tail == v3);
-	}
 
-	if (e3->tail == NULL)
+		if (e3->face == NULL)
-	{
+		{
-		e3->tail = v3;
+			e3->face = face;
-	}
+		}
 
-	if (e3->twin->tail == NULL)
+		B3_ASSERT(e3->twin != NULL);
-	{
+		B3_ASSERT(e3->twin->active == true);
-		e3->twin->tail = v1;
+		B3_ASSERT(e3->twin->tail == v1);
 	}
 	
 	e1->prev = e3;
@@ -771,18 +870,7 @@ qhFace* qhHull::AddFace(qhVertex* v1, qhVertex* v2, qhVertex* v3)
 
 	face->edge = e1;
 
-	b3Vec3 A = v1->position, B = v2->position, C = v3->position;
+	b3ResetFaceData(face);
-
-	b3Vec3 N = b3Cross(B - A, C - A);
-	float32 L = b3Length(N);
-	if (L > B3_EPSILON)
-	{
-		N /= L;
-	}
-
-	face->center = (A + B + C) / 3.0f;
-	face->plane = b3Plane(N, face->center);
-	face->area = 0.5f * L;
 
 	m_faceList.PushFront(face);
 
@@ -791,6 +879,8 @@ qhFace* qhHull::AddFace(qhVertex* v1, qhVertex* v2, qhVertex* v3)
 
 qhFace* qhHull::RemoveFace(qhFace* face)
 {
+	B3_ASSERT(face->conflictList.count == 0);
+
 	// Remove half-edges 
 	qhHalfEdge* e = face->edge;
 	do
@@ -800,32 +890,40 @@ qhFace* qhHull::RemoveFace(qhFace* face)
 
 		qhHalfEdge* twin = e0->twin;
 
-		// Is the edge a boundary edge?
+		// Is the edge a boundary?
 		if (twin->face == NULL)
 		{
+			// Remove both half-edges if the edge is not shared.
 			e0->twin = NULL;
-
 			e0->tail = NULL;
 			e0->face = NULL;
-			e0->next = NULL;
 			e0->prev = NULL;
+			e0->next = NULL;
+			FreeEdge(e0);
 
 			twin->twin = NULL;
-
+			twin->tail = NULL;
-			// Free both half-edges if edge is a boundary.
+			twin->face = NULL;
-			FreeEdge(e0);
+			twin->prev = NULL;
+			twin->next = NULL;
 			FreeEdge(twin);
 		}
 		else
 		{
-			e0->tail = NULL;
+			// Remove the non-shared half-edge.
+			// However, we preserve the edge, its twin, and their connection.
+			B3_ASSERT(e0->twin != NULL);
+			B3_ASSERT(e0->twin->twin == e0);
+			//e0->tail = NULL;
 			e0->face = NULL;
-			e0->next = NULL;
 			e0->prev = NULL;
+			e0->next = NULL;
 		}
 
 	} while (e != face->edge);
 
+	face->edge = NULL;
+
 	// Remove face 
 	qhFace* nextFace = m_faceList.Remove(face);
 	FreeFace(face);
@@ -843,6 +941,8 @@ bool qhHull::MergeFace(qhFace* rightFace)
 	{
 		qhFace* leftFace = e->twin->face;
 
+		//B3_ASSERT(leftFace != rightFace);
+
 		if (leftFace == rightFace)
 		{
 			e = e->next;
@@ -881,9 +981,15 @@ bool qhHull::MergeLargeFace(qhFace* rightFace)
 
 	qhHalfEdge* e = rightFace->edge;
 
+	B3_ASSERT(e->face == rightFace);
+
 	do
 	{
-		qhFace* leftFace = e->twin->face;
+		qhHalfEdge* twin = e->twin;
+
+		qhFace* leftFace = twin->face;
+
+		//B3_ASSERT(leftFace != rightFace);
 
 		if (leftFace == rightFace)
 		{
@@ -966,6 +1072,21 @@ void qhHull::MergeNewFaces()
 	}
 }
 
+void qhHull::Translate(const b3Vec3& translation)
+{
+	// Shift vertices
+	for (qhVertex* v = m_vertexList.head; v != NULL; v = v->next)
+	{
+		v->position += translation;
+	}
+
+	// Reset face data
+	for (qhFace* f = m_faceList.head; f; f = f->next)
+	{
+		b3ResetFaceData(f);
+	}
+}
+
 void qhHull::Validate(const qhHalfEdge* edge) const
 {
 	B3_ASSERT(edge->active == true);
@@ -1007,19 +1128,25 @@ void qhHull::Validate(const qhFace* face) const
 		B3_ASSERT(edge->active == true);
 		B3_ASSERT(edge->face == face);
 
-		// Validate face convexity
+		B3_ASSERT(edge->twin != NULL);
-		//B3_ASSERT(edge->twin->active == true);
+		B3_ASSERT(edge->twin->active == true);
-		//qhFace* other = edge->twin->face;
+		qhFace* other = edge->twin->face;
-		//B3_ASSERT(other->active == true);
+		if (other != NULL)
+		{
+			// Ensure face convexity
+			B3_ASSERT(other->active == true);
 
-		//float32 d1 = b3Distance(other->center, face->plane);
+			//B3_ASSERT(face != other);
-		//float32 d2 = b3Distance(face->center, other->plane);
 
-		//B3_ASSERT(d1 < -m_tolerance);
+			//float32 d1 = b3Distance(other->center, face->plane);
-		//B3_ASSERT(d2 < -m_tolerance);
+			//float32 d2 = b3Distance(face->center, other->plane);
 			
-		// Verify vertex redundancy 
+			//B3_ASSERT(d1 < -m_tolerance);
-		//B3_ASSERT(edge->next->twin->face != edge->twin->face);
+			//B3_ASSERT(d2 < -m_tolerance);
+
+			// Vertex redundancy 
+			// B3_ASSERT(edge->next->twin->face != edge->twin->face);
+		}
 		
 		edge = edge->next;
 	} while (edge != begin);