optimization

src/bounce/collision/shapes/qhull.cpp

@@ -21,45 +21,52 @@
 
 #define B3_NULL_HULL_FEATURE 0xFF
 
-//
-struct b3PointerIndex
-{
-	void* pointer;
-	u8 index;
-};
-
 // 
-template<u32 N>
+template<class T, u32 N>
-struct b3PIMap
+struct b3UniqueArray
 {
-	b3PIMap()
+	b3UniqueArray()
 	{
 		count = 0;
 	}
 
-	void Add(const b3PointerIndex& pi)
+	void PushBack(const T& value)
 	{
+		// B3_ASSERT(Find(value) == count)
 		B3_ASSERT(count < N);
-		pis[count++] = pi;
+		values[count++] = value;
 	}
 
-	b3PointerIndex* Find(void* pointer)
+	u32 Find(const T& value) const
 	{
 		for (u32 i = 0; i < count; ++i)
 		{
-			b3PointerIndex* index = pis + i;
+			if (values[i] == value)
-			if (index->pointer == pointer)
 			{
-				return index;
+				return i;
 			}
 		}
-		return NULL;
+		return count;
 	}
 	
+	T values[N];
 	u32 count;
-	b3PointerIndex pis[N];
 };
 
+//
+template<class T> 
+static inline u32 b3Find(const T* values, u32 count, const T* value)
+{
+	for (u32 i = 0; i < count; ++i)
+	{
+		if (values[i] == value)
+		{
+			return i;
+		}
+	}
+	B3_ASSERT(false);
+}
+
 //
 static b3Vec3 b3ComputeCentroid(b3QHull* hull)
 {
@@ -182,30 +189,37 @@ void b3QHull::Set(const b3Vec3* points, u32 count)
 	qhHull hull;
 	hull.Construct(qhBuffer, ps, psCount);
 
-	const qhList<qhFace>& faceList = hull.GetFaceList();
-	if (faceList.count > B3_MAX_HULL_FACES)
-	{
-		// Face excess
-		return;
-	}
-
 	// Cheaply convert the constructed hull into a run-time hull.
-	vertexCount = 0;
-	edgeCount = 0;
-	faceCount = 0;
 
-	// These structures map vertex/edge pointers to indices in the run-time hull
+	// Unique vertices and edges
-	b3PIMap<B3_MAX_HULL_VERTICES> vs;
+	b3UniqueArray<qhVertex*, B3_MAX_HULL_VERTICES> vs;
-	b3PIMap<B3_MAX_HULL_EDGES> es;
+	b3UniqueArray<qhHalfEdge*, B3_MAX_HULL_EDGES> es;
+	u32 fs_count = 0;
 
 	// Face half-edges
 	u8 fhs[B3_MAX_HULL_EDGES];
 	u32 nfh = 0;
 
+	const qhList<qhFace>& faceList = hull.GetFaceList();
 	qhFace* face = faceList.head;
 	while (face)
 	{
-		qhHalfEdge* edge = face->edge;
+		if (fs_count == B3_MAX_HULL_FACES)
+		{
+			// Face excess
+			return;
+		}
+
+		// Add face
+		B3_ASSERT(fs_count < B3_MAX_HULL_FACES);
+		b3Face* f = faces + fs_count;
+		u32 fi = fs_count;
+		++fs_count;
+
+		planes[fi] = face->plane;
+		
+		qhHalfEdge* begin = face->edge;
+		qhHalfEdge* edge = begin;
 		do
 		{
 			qhHalfEdge* twin = edge->twin;
@@ -213,125 +227,87 @@ void b3QHull::Set(const b3Vec3* points, u32 count)
 			qhVertex* v1 = edge->tail;
 			qhVertex* v2 = twin->tail;
 
-			b3PointerIndex* mv1 = vs.Find(v1);
+			u32 iv1 = vs.Find(v1);
-
+			if (iv1 == vs.count)
-			u8 iv1;
-			if (mv1)
 			{
-				iv1 = mv1->index;
-			}
-			else
-			{
-				// Add a unique vertex
-
 				// Vertex excess
-				if (vertexCount == B3_MAX_HULL_VERTICES)
+				if (vs.count == B3_MAX_HULL_VERTICES)
 				{
-					vertexCount = 0;
-					edgeCount = 0;
-					faceCount = 0;
 					return;
 				}
 
-				vertices[vertexCount] = v1->position;
+				// Add vertex
-				iv1 = vertexCount;
+				vs.PushBack(v1);
-				++vertexCount;
-				
-				// Add to vertex-index pair to the map
-				vs.Add({ v1, iv1 });
 			}
 
-			b3PointerIndex* mv2 = vs.Find(v2);
+			u32 iv2 = vs.Find(v2);
-			
+			if (iv2 == vs.count)
-			u8 iv2;
-			if (mv2)
-			{
-				iv2 = mv2->index;
-			}
-			else
 			{
 				// Vertex excess
-				if (vertexCount == B3_MAX_HULL_VERTICES)
+				if (vs.count == B3_MAX_HULL_VERTICES)
 				{
-					vertexCount = 0;
-					edgeCount = 0;
-					faceCount = 0;
 					return;
 				}
 				
-				vertices[vertexCount] = v2->position;
+				// Add vertex
-				iv2 = vertexCount;
+				vs.PushBack(v2);
-				++vertexCount;
-
-				vs.Add({ v2, iv2 });
 			}
 			
-			b3PointerIndex* mte = es.Find(edge);
+			u32 ie2 = es.Find(edge);
-			
+			if(ie2 == es.count)
-			if (mte)
-			{
-				u8 ie2 = mte->index;
-				b3HalfEdge* e2 = edges + ie2;
-				B3_ASSERT(e2->face == B3_NULL_HULL_FEATURE);
-				e2->face = u8(faceCount);
-				
-				B3_ASSERT(nfh < B3_MAX_HULL_EDGES);
-				fhs[nfh++] = ie2;
-			}
-			else
 			{
 				// Edge excess
-				if (edgeCount + 2 >= B3_MAX_HULL_EDGES)
+				if (es.count + 2 >= B3_MAX_HULL_EDGES)
 				{
-					vertexCount = 0;
-					edgeCount = 0;
-					faceCount = 0;
 					return;
 				}
 
-				b3HalfEdge* e1 = edges + edgeCount;
+				// Add half-edges
-				u8 ie1 = edgeCount;
+				u32 ie1 = es.count;
-				++edgeCount;
+				es.PushBack(edge);
-				
-				b3HalfEdge* e2 = edges + edgeCount;
-				u8 ie2 = edgeCount;
-				++edgeCount;
 
-				e1->face = u8(faceCount);
+				u32 ie2 = es.count;
+				es.PushBack(twin);
+
+				// Link half-edges
+				b3HalfEdge* e1 = edges + ie1;
+				e1->face = u8(fi);
 				e1->origin = iv1;
 				e1->twin = ie2;
 
+				b3HalfEdge* e2 = edges + ie2;
 				e2->face = B3_NULL_HULL_FEATURE;
 				e2->origin = iv2;
 				e2->twin = ie1;
 
-				es.Add({ edge, ie1 });
-				es.Add({ twin, ie2 });
-
 				B3_ASSERT(nfh < B3_MAX_HULL_EDGES);
 				fhs[nfh++] = ie1;
 			}
+			else
+			{
+				b3HalfEdge* e2 = edges + ie2;
+				
+				B3_ASSERT(e2->face == B3_NULL_HULL_FEATURE);
+				e2->face = u8(fi);
+
+				B3_ASSERT(nfh < B3_MAX_HULL_EDGES);
+				fhs[nfh++] = ie2;
+			}
 
 			edge = edge->next;
-		} while (edge != face->edge);
+		} while (edge != begin);
 		
-		B3_ASSERT(faceCount < B3_MAX_HULL_FACES);
+		// Link any face half-edge to face
-		
-		planes[faceCount] = face->plane;
-		
-		b3Face* f = faces + faceCount;
-		++faceCount;
-
 		B3_ASSERT(nfh > 0);
 		f->edge = fhs[0];
 		
-		// Link face half-edges 
+		// Link half-edge list 
 		for (u32 i = 0; i < nfh; ++i)
 		{
 			u8 edge = fhs[i];
+			u8 nextEdge = i < nfh - 1 ? i + 1 : 0;
 			
-			u32 j = i < nfh - 1 ? i + 1 : 0;
+			edges[edge].next = fhs[nextEdge];
-			edges[edge].next = fhs[j];
 		}
 
 		nfh = 0;
@@ -339,6 +315,19 @@ void b3QHull::Set(const b3Vec3* points, u32 count)
 		face = face->next;
 	}
 
+	B3_ASSERT(vs.count <= B3_MAX_HULL_VERTICES);
+	vertexCount = vs.count;
+	for (u32 i = 0; i < vs.count; ++i)
+	{
+		vertices[i] = vs.values[i]->position;
+	}
+
+	B3_ASSERT(es.count <= B3_MAX_HULL_EDGES);
+	edgeCount = es.count;
+	
+	B3_ASSERT(fs_count <= B3_MAX_HULL_FACES);
+	faceCount = fs_count;
+
 	// Validate
 	Validate();
 
@@ -402,7 +391,7 @@ void b3QHull::SetAsCone(float32 radius, float32 height)
 {
 	B3_ASSERT(radius > 0.0f);
 	B3_ASSERT(height > 0.0f);
-	
+
 	const u32 kEdgeCount = 20;
 	const u32 kVertexCount = 2 * kEdgeCount + 1;
 	b3Vec3 vs[kVertexCount];