EPA

include/anki/collision/GjkEpa.h

@@ -7,7 +7,9 @@
 #define ANKI_COLLISION_GJK_EPA_H
 
 #include "anki/Math.h"
+#include "anki/util/Allocator.h"
 #include "anki/collision/ContactPoint.h"
+#include "anki/collision/GjkEpaInternal.h"
 
 namespace anki {
 
@@ -21,18 +23,14 @@ class ConvexShape;
 /// checking the intersection between convex shapes.
 class Gjk
 {
+	friend class GjkEpa;
+
 public:
 	/// Return true if the two convex shapes intersect
 	Bool intersect(const ConvexShape& shape0, const ConvexShape& shape1);
 
-public: // XXX
-	class Support
-	{
-	public:
-		Vec4 m_v;
-		Vec4 m_v0;
-		Vec4 m_v1;
-	};
+private:
+	using Support = detail::Support;
 
 	Array<Support, 4> m_simplex;
 	U32 m_count; ///< Simplex count
@@ -47,41 +45,27 @@ public: // XXX
 };
 
 /// The implementation of EPA
-class GjkEpa: public Gjk
+class GjkEpa
 {
 public:
-	Bool intersect(const ConvexShape& shape0, const ConvexShape& shape1,
-		ContactPoint& contact);
-
-public: // XXX
-	static const U MAX_SIMPLEX_COUNT = 50;
-	static const U MAX_FACE_COUNT = 100;
+	detail::Polytope* m_poly; // XXX
 
-	class Face
-	{
-	public:
-		Array<U32, 3> m_idx;
-		Vec4 m_normal;
-		F32 m_dist; ///< Distance from the origin
-		U8 m_originInside;
+	GjkEpa(U32 maxSimplexSize, U32 maxFaceCount)
+	:	m_maxSimplexSize(maxSimplexSize),
+		m_maxFaceCount(maxFaceCount)
+	{}
 
-		void init()
-		{
-			m_normal[0] = -100.0;
-			m_originInside = 2;
-		}
-	};
+	~GjkEpa()
+	{}
 
-	Array<Support, MAX_SIMPLEX_COUNT> m_simplexArr;
-
-	Array<Face, MAX_FACE_COUNT> m_faces;
-	U32 m_faceCount;
-
-	void findClosestFace(U& index);
+	Bool intersect(const ConvexShape& shape0, const ConvexShape& shape1,
+		ContactPoint& contact, StackAllocator<U8>& alloc);
 
-	void expandPolytope(Face& closestFace, const Vec4& point);
+private:
+	using Support = detail::Support;
 
-	void computeFace(Face& face);
+	U32 m_maxSimplexSize;
+	U32 m_maxFaceCount;
 };
 
 /// @}

include/anki/collision/GjkEpaInternal.h

@@ -0,0 +1,201 @@
+// Copyright (C) 2014, Panagiotis Christopoulos Charitos.
+// All rights reserved.
+// Code licensed under the BSD License.
+// http://www.anki3d.org/LICENSE
+
+#ifndef ANKI_COLLISION_GJK_EPA_INTERNAL_H
+#define ANKI_COLLISION_GJK_EPA_INTERNAL_H
+
+#include "anki/Math.h"
+#include "anki/util/Allocator.h"
+#include "anki/util/Vector.h"
+
+namespace anki {
+namespace detail {
+
+// Forward
+class Polytope;
+class Face;
+
+/// @addtogroup collision_internal
+/// @{
+
+/// GJK support
+class Support
+{
+public:
+	Vec4 m_v;
+	Vec4 m_v0;
+	Vec4 m_v1;
+
+	Bool operator==(const Support& b) const
+	{
+		return m_v == b.m_v && m_v0 == b.m_v0 && m_v1 == b.m_v1;
+	}
+};
+
+/// Edge of a polytope
+class Edge
+{
+public:
+	Array<U32, 2> m_idx;
+	Face* m_face;
+
+	Edge(U32 i, U32 j, Face* face)
+	:	m_idx{i, j},
+		m_face(face)
+	{}
+
+	Edge(const Edge&) = default;
+
+	Bool operator==(const Edge& b) const
+	{
+		return (m_idx[0] == b.m_idx[1] && m_idx[1] == b.m_idx[0])
+			|| (m_idx[0] == b.m_idx[0] && m_idx[1] == b.m_idx[1]);
+	}
+};
+
+class EdgeHasher
+{
+public:
+	PtrSize operator()(const Edge& e) const
+	{
+		if(e.m_idx[0] < e.m_idx[1])
+		{
+			return (e.m_idx[1] << 16) | e.m_idx[0];
+		}
+		else
+		{
+			return (e.m_idx[0] << 16) | e.m_idx[1];
+		}
+	}
+};
+
+class EdgeCompare
+{
+public:
+	Bool operator()(const Edge& a, const Edge& b) const
+	{
+		return a == b;
+	}
+};
+
+/// The face of the polytope used for EPA
+class Face
+{
+public:
+	Face()
+	{
+		m_normal[0] = -100.0;
+		m_dist = -1.0;
+		m_originInside = 2;
+		m_dead = false;
+#if ANKI_DEBUG
+		m_idx[0] = m_idx[1] = m_idx[2] = MAX_U32;
+#endif
+	}
+
+	Face(U i, U j, U k)
+	:	Face()
+	{
+		m_idx[0] = i;
+		m_idx[1] = j;
+		m_idx[2] = k;
+	}
+	
+	const Array<U32, 3>& idx() const
+	{
+		return m_idx;
+	}
+
+	const Vec4& normal(Polytope& poly) const;
+
+	F32 distance(Polytope& poly) const
+	{
+		normal(poly);
+		return m_dist;
+	}
+
+	Bool originInside(Polytope& poly) const;
+
+	Edge edge(Polytope& poly, U i)
+	{
+		normal(poly);
+		return Edge(m_idx[i], (i == 2) ? m_idx[0] : m_idx[i + 1], this);
+	}
+
+	Bool dead() const
+	{
+		return m_dead;
+	}
+
+	void kill()
+	{
+		m_dead = true;
+	}
+
+private:
+	mutable Array<U32, 3> m_idx;
+	mutable Vec4 m_normal;
+	mutable F32 m_dist; ///< Distance from the origin
+	mutable U8 m_originInside; ///< 0: Not use it, 1: Use it, 2: Initial
+	Bool8 m_dead;
+};
+
+/// The polytope used for EPA
+class Polytope
+{
+	friend class Face;
+
+public:
+	Polytope(StackAllocator<U8>& alloc, U32 maxSimplexSize, U32 maxFaceCount)
+	:	m_maxSimplexSize(maxSimplexSize),
+		m_maxFaceCount(maxFaceCount),
+		m_simplex(alloc),
+		m_faces(alloc)
+	{}
+
+	void init(const Array<Support, 4>& gjkSupport);
+
+	Bool addSupportAndExpand(const Support& s, Face& cface)
+	{
+		Bool found;
+		U idx;
+		addNewSupport(s, found, idx);
+
+		if(!found)
+		{
+			return expand(cface, idx);
+		}
+		else
+		{
+			return false;
+		}
+	}
+
+	/// Find closest face to the origin
+	Face& findClosestFace();
+
+public: // XXX
+	U32 m_maxSimplexSize;
+	U32 m_maxFaceCount;
+
+	Vector<Support, StackAllocator<Support>> m_simplex;
+	Vector<Face, StackAllocator<Face>> m_faces;
+
+	StackAllocator<U8> getAllocator() const
+	{
+		return m_simplex.get_allocator();
+	}
+
+	void addNewSupport(const Support& s, Bool& found, U& idx);
+
+	Bool expand(Face& cface, U supportIdx);
+};
+
+/// @}
+
+} // end namesapce detail
+} // end namesapce anki
+
+#endif

src/collision/GjkEpa.cpp

@@ -242,54 +242,20 @@ Bool Gjk::intersect(const ConvexShape& shape0, const ConvexShape& shape1)
 
 //==============================================================================
 Bool GjkEpa::intersect(const ConvexShape& shape0, const ConvexShape& shape1,
-	ContactPoint& contact)
+	ContactPoint& contact, StackAllocator<U8>& alloc)
 {
 	// Do the intersection test
-	if(!Gjk::intersect(shape0, shape1))
+	Gjk gjk;
+	if(!gjk.intersect(shape0, shape1))
 	{
 		return false;
 	}
 
-	U faceIndex = 0;
-	Face* face;
-
-	// Set the array simplex
-	ANKI_ASSERT(m_count == 4);
-	m_simplexArr[0] = m_simplex[0];
-	m_simplexArr[1] = m_simplex[1];
-	m_simplexArr[2] = m_simplex[2];
-	m_simplexArr[3] = m_simplex[3];
-
-	// Set the first 4 faces
-	face = &m_faces[0];
-	face->m_idx[0] = 0;
-	face->m_idx[1] = 1;
-	face->m_idx[2] = 2;
-	face->init();
-	computeFace(*face);
-
-	face = &m_faces[1];
-	face->m_idx[0] = 1;
-	face->m_idx[1] = 2;
-	face->m_idx[2] = 3;
-	face->init();
-	computeFace(*face);
-
-	face = &m_faces[2];
-	face->m_idx[0] = 2;
-	face->m_idx[1] = 3;
-	face->m_idx[2] = 0;
-	face->init();
-	computeFace(*face);
-
-	face = &m_faces[3];
-	face->m_idx[0] = 3;
-	face->m_idx[1] = 0;
-	face->m_idx[2] = 1;
-	face->init();
-	computeFace(*face);
-
-	m_faceCount = 4;
+	// Init polytope
+	detail::Polytope* poly = alloc.newInstance<detail::Polytope>(
+		alloc, m_maxSimplexSize, m_maxFaceCount);
+	m_poly = poly;
+	poly->init(gjk.m_simplex);
 
 	std::cout << "-----------------------" << std::endl;
 
@@ -297,33 +263,21 @@ Bool GjkEpa::intersect(const ConvexShape& shape0, const ConvexShape& shape1,
 	while(1) 
 	{
 		// Find the closest to the origin face
-		findClosestFace(faceIndex);
+		detail::Face& cface = poly->findClosestFace();
 
-		face = &m_faces[faceIndex];
-		const Vec4& normal = face->m_normal;
-		F32 distance = face->m_dist;
+		const Vec4& normal = cface.normal(*poly);
+		F32 distance = cface.distance(*poly);
 
 		// Get new support
 		Support p;
-		support(shape0, shape1, normal, p);
+		Gjk::support(shape0, shape1, normal, p);
 		F32 d = p.m_v.dot(normal);
 
-		// Search if the simplex is there
-		U pIdx;
-		for(pIdx = 0; pIdx < m_count; pIdx++)
-		{
-			if(p.m_v == m_simplexArr[pIdx].m_v)
-			{
-				std::cout << "p found " << std::endl;
-				break;
-			}
-		}
-
 		// Check new distance
 		if(d - distance < 0.001 
-			|| m_faceCount == m_faces.size() - 2
-			|| m_count == m_simplexArr.size() - 1
-			|| pIdx != m_count
+			//|| m_faceCount == m_faces.size() - 2
+			//|| m_count == m_simplexArr.size() - 1
+			//|| pIdx != m_count
 			/*|| iterations == 3*/)
 		{
 			/*if(pIdx != m_count)
@@ -340,35 +294,12 @@ Bool GjkEpa::intersect(const ConvexShape& shape0, const ConvexShape& shape1,
 		} 
 		else 
 		{
-			// Create 3 new faces by adding 'p'
-
-			//if(pIdx == m_count)
+			if(!poly->addSupportAndExpand(p, cface))
 			{
-				// Add p
-				m_simplexArr[m_count++] = p;
+				contact.m_normal = normal;
+				contact.m_depth = d;
+				break;
 			}
-
-			Array<U32, 3> idx = face->m_idx;
-
-			// Canibalize existing face
-			face->m_idx[0] = idx[0];
-			face->m_idx[1] = idx[1];
-			face->m_idx[2] = pIdx;
-			face->m_normal[0] = -100.0;
-
-			// Next face
-			face = &m_faces[m_faceCount++];
-			face->m_idx[0] = idx[1];
-			face->m_idx[1] = idx[2];
-			face->m_idx[2] = pIdx;
-			face->m_normal[0] = -100.0;
-
-			// Next face
-			face = &m_faces[m_faceCount++];
-			face->m_idx[0] = idx[2];
-			face->m_idx[1] = idx[0];
-			face->m_idx[2] = pIdx;
-			face->m_normal[0] = -100.0;
 		}
 
 		++iterations;
@@ -377,218 +308,5 @@ Bool GjkEpa::intersect(const ConvexShape& shape0, const ConvexShape& shape1,
 	return true;
 }
 
-//==============================================================================
-void GjkEpa::findClosestFace(U& index)
-{
-	F32 minDistance = MAX_F32;
-
-	// Iterate the faces
-	for(U i = 0; i < m_faceCount; i++) 
-	{
-		Face& face = m_faces[i];
-		
-		// Check if we calculated the normal before
-		if(face.m_normal[0] == -100.0)
-		{
-			computeFace(face);
-		}
-
-		// Check the distance against the other distances
-		if(face.m_dist < minDistance) 
-		{
-			// Check if the origin lies within the face
-			if(face.m_originInside == 2)
-			{
-				const Vec4& a = m_simplexArr[face.m_idx[0]].m_v;
-				const Vec4& b = m_simplexArr[face.m_idx[1]].m_v;
-				const Vec4& c = m_simplexArr[face.m_idx[2]].m_v;
-				const Vec4& n = face.m_normal;
-
-				face.m_originInside = 1;
-
-				// Compute the face edges
-				Vec4 e0 = b - a;
-				Vec4 e1 = c - b;
-				Vec4 e2 = a - c;
-
-				Vec4 adjacentNormal;
-				F32 d;
-
-				// Check the 1st edge
-				adjacentNormal = e0.cross(n);
-				d = adjacentNormal.dot(a);
-				if(d <= 0.0)
-				{
-					face.m_originInside = 0;
-				}
-
-				// Check the 2nd edge
-				if(face.m_originInside == 1)
-				{
-					adjacentNormal = e1.cross(n);
-					d = adjacentNormal.dot(b);
-
-					if(d <= 0.0)
-					{
-						face.m_originInside = 0;
-					}
-				}
-
-				// Check the 3rd edge
-				if(face.m_originInside == 1)
-				{
-					adjacentNormal = e2.cross(n);
-					d = adjacentNormal.dot(c);
-
-					if(d <= 0.0)
-					{
-						face.m_originInside = 0;
-					}
-				}
-			}
-
-			if(face.m_originInside)
-			{
-				// We have a candidate
-				index = i;
-				minDistance = face.m_dist;
-			}
-			else
-			{
-				std::cout << "origin not in face" << std::endl;
-			}
-		}
-	}
-}
-
-//==============================================================================
-void GjkEpa::computeFace(Face& face)
-{
-	ANKI_ASSERT(face.m_normal[0] == -100.0);
-
-	const Vec4& a = m_simplexArr[face.m_idx[0]].m_v;
-	const Vec4& b = m_simplexArr[face.m_idx[1]].m_v;
-	const Vec4& c = m_simplexArr[face.m_idx[2]].m_v;
-
-	// Compute the face edges
-	Vec4 e0 = b - a;
-	Vec4 e1 = c - b;
-
-	// Compute the face normal
-	Vec4 n = e0.cross(e1);
-	n.normalize();
-
-	// Calculate the distance from the origin to the edge
-	F32 d = n.dot(a);
-
-	// Check the winding
-	if(d < 0.0)
-	{
-		// It's not facing the origin so fix some stuff
-
-		d = -d;
-		n = -n;
-
-		// Swap some indices
-		auto idx = face.m_idx[0];
-		face.m_idx[0] = face.m_idx[1];
-		face.m_idx[1] = idx;
-	}
-
-	face.m_normal = n;
-	face.m_dist = d;
-}
-
-//==============================================================================
-#if 0
-static Bool commonEdge(const Face& f0, const Face& f1, U& edge0, U& edge1)
-{
-	// For all edges of f0
-	for(U i0 = 0; i0 < 3; i0++)
-	{
-		Array<U, 2> e0 = {f0.m_idx[i0], f0.m_idx[(i0 == 2) ? 0 : i0 + 1]};
-
-		// For all edges of f1
-		for(U i1 = 0; i1 < 3; i1++)
-		{
-			Array<U, 2> e1 = {f1.m_idx[i1], f1.m_idx[(i1 == 2) ? 0 : i1 + 1]};
-
-			if((e0[0] == e1[0] && e0[1] == e1[1])
-				|| (e0[0] == e1[1] && e0[1] == e1[0]))
-			{
-				edge0 = i0;
-				edge1 = i1;
-				return true;
-			}
-		}	
-	}
-
-	return false;
-}
-#endif
-
-//==============================================================================
-void GjkEpa::expandPolytope(Face& cface, const Vec4& point)
-{
-#if 0
-	static const Array2d<U, 3, 2> edges = {
-		{0,  1},
-		{1,  2},
-		{2,  0}};
-
-	// Find other faces that share the same edge
-	for(U f = 0; f < m_faceCount; f++)
-	{
-		Face& face = m_face[f];
-
-		// Skip the same face
-		if(&face == &cface)
-		{
-			continue;
-		}
-
-		for(U i = 0, i < 3; i++)
-		{
-			for(U j = 0, j < 3; j++)
-			{
-				if(cface.m_idx[edges[i][0]] == face.m_idx[edges[j][1]]
-					&& cface.m_idx[edges[i][1]] == face.m_idx[edges[j][0]])
-				{
-					// Found common edge
-
-					// Check if the point will create concave polytope
-					if(face.m_normal.dot(point) > 0.0)
-					{
-						// Concave
-
-
-					}
-				}
-			}
-		}
-
-		ANKI_ASSER(e0 < 3 && e1 < 3);
-
-		// Check if the point will create concave polytope
-		if(face.m_normal.dot(point) < 0.0)
-		{
-			// No concave
-			continue;
-		}
-
-		// XXX
-		Array<U32, 3> idx = face.m_idx;
-
-		for(U e = 0; e < 3; e++)
-		{
-			if(e != e1)
-			{
-			}
-		}
-
-	}
-#endif
-}
-
 } // end namespace anki
 

src/collision/GjkEpaInternal.cpp

@@ -0,0 +1,369 @@
+#include "anki/collision/GjkEpaInternal.h"
+#include <unordered_map>
+#include <algorithm>
+
+namespace anki {
+namespace detail {
+
+//==============================================================================
+// Edge                                                                        =
+//==============================================================================
+
+//==============================================================================
+static inline std::ostream& operator<<(std::ostream& os, const Edge& e) 
+{
+	os << e.m_idx[0] << " " << e.m_idx[1];
+	return os;
+}
+
+//==============================================================================
+// Face                                                                        =
+//==============================================================================
+
+//==============================================================================
+const Vec4& Face::normal(Polytope& poly) const
+{
+	if(m_normal[0] == -100.0)
+	{
+		const Vec4& a = poly.m_simplex[m_idx[0]].m_v;
+		const Vec4& b = poly.m_simplex[m_idx[1]].m_v;
+		const Vec4& c = poly.m_simplex[m_idx[2]].m_v;
+
+		// Compute the face edges
+		Vec4 e0 = b - a;
+		Vec4 e1 = c - b;
+
+		// Compute the face normal
+		m_normal = e0.cross(e1);
+		m_normal.normalize();
+
+		// Compute the distance as well for the winding
+		
+		// Calculate the distance from the origin to the edge
+		m_dist = m_normal.dot(a);
+
+		// Check the winding
+		if(m_dist < 0.0)
+		{
+			// It's not facing the origin so fix some stuff
+
+			m_dist = -m_dist;
+			m_normal = -m_normal;
+
+			// Swap some indices
+			auto idx = m_idx[0];
+			m_idx[0] = m_idx[1];
+			m_idx[1] = idx;
+		}
+	}
+
+	return m_normal;
+}
+
+//==============================================================================
+Bool Face::originInside(Polytope& poly) const
+{
+	if(m_originInside == 2)
+	{
+		const Vec4& a = poly.m_simplex[m_idx[0]].m_v;
+		const Vec4& b = poly.m_simplex[m_idx[1]].m_v;
+		const Vec4& c = poly.m_simplex[m_idx[2]].m_v;
+		const Vec4& n = normal(poly);
+
+		m_originInside = 1;
+
+		// Compute the face edges
+		Vec4 e0 = b - a;
+		Vec4 e1 = c - b;
+		Vec4 e2 = a - c;
+
+		Vec4 adjacentNormal;
+		F32 d;
+
+		// Check the 1st edge
+		adjacentNormal = e0.cross(n);
+		d = adjacentNormal.dot(a);
+		if(d <= 0.0)
+		{
+			m_originInside = 0;
+		}
+
+		// Check the 2nd edge
+		if(m_originInside == 1)
+		{
+			adjacentNormal = e1.cross(n);
+			d = adjacentNormal.dot(b);
+
+			if(d <= 0.0)
+			{
+				m_originInside = 0;
+			}
+		}
+
+		// Check the 3rd edge
+		if(m_originInside == 1)
+		{
+			adjacentNormal = e2.cross(n);
+			d = adjacentNormal.dot(c);
+
+			if(d <= 0.0)
+			{
+				m_originInside = 0;
+			}
+		}
+	}
+
+	return m_originInside;
+}
+
+//==============================================================================
+// Polytope                                                                    =
+//==============================================================================
+
+//==============================================================================
+void Polytope::init(const Array<Support, 4>& gjkSupport)
+{
+	// Allocate memory up front
+	m_simplex.reserve(m_maxSimplexSize);
+	m_faces.reserve(m_maxFaceCount);
+
+	// Set the simplex
+	m_simplex.push_back(gjkSupport[0]);
+	m_simplex.push_back(gjkSupport[1]);
+	m_simplex.push_back(gjkSupport[2]);
+	m_simplex.push_back(gjkSupport[3]);
+
+	// Set the first 4 faces
+	m_faces.emplace_back(0, 1, 2);
+	m_faces.emplace_back(1, 2, 3);
+	m_faces.emplace_back(2, 3, 0);
+	m_faces.emplace_back(3, 0, 1);
+}
+
+//==============================================================================
+Face& Polytope::findClosestFace()
+{
+	F32 minDistance = MAX_F32;
+	U index = MAX_U32;
+
+	// Iterate the faces
+	for(U i = 0; i < m_faces.size(); i++) 
+	{
+		Face& face = m_faces[i];
+
+		// Check the distance against the other distances
+		if(!face.dead() && face.distance(*this) < minDistance)
+		{
+			if(face.originInside(*this))
+			{
+				// We have a candidate
+				index = i;
+				minDistance = face.distance(*this);
+			}
+			else
+			{
+				std::cout << "origin not in face" << std::endl;
+			}
+		}
+	}
+
+	return m_faces[index];
+}
+
+//==============================================================================
+void Polytope::addNewSupport(const Support& s, Bool& found, U& idx)
+{
+	// Search if support is already there
+	U i;
+	for(i = 0; i < m_simplex.size(); i++)
+	{
+		if(m_simplex[i] == s)
+		{
+			break;
+		}
+	}
+
+	idx = i;
+
+	if(i == m_simplex.size())
+	{
+		// Not found
+		m_simplex.push_back(s);
+		found = false;
+	}
+	else
+	{
+		found = true;
+	}
+}
+
+//==============================================================================
+Bool Polytope::expand(Face& cface, U supportIdx)
+{
+	if(m_faces.size() + 2 > m_maxFaceCount)
+	{
+		// Reached a limit, cannot expand 
+		return false;
+	}
+
+	//
+	// First add the point to the polytope by spliting the cface
+	//
+	Array<U32, 3> idx = cface.idx();
+
+	// Canibalize existing face
+	cface = Face(idx[0], idx[1], supportIdx);
+
+	// Next face
+	m_faces.emplace_back(idx[1], idx[2], supportIdx);
+
+	// Next face
+	m_faces.emplace_back(idx[2], idx[0], supportIdx);
+
+	//
+	// Find other faces that hide the new support
+	//
+	Vector<Face*, StackAllocator<Face*>> badFaces(getAllocator());
+	badFaces.reserve(20);
+
+	const Vec4& support = m_simplex[supportIdx].m_v;
+
+	for(Face& face : m_faces)
+	{
+		if(face.dead())
+		{
+			continue;
+		}
+
+		F32 dot = face.normal(*this).dot(support);
+		if(dot > getEpsilon<F32>())
+		{
+			badFaces.push_back(&face);
+			face.kill();
+		}
+	}
+
+	if(badFaces.size() == 0)
+	{
+		// Early exit
+		return true;
+	}
+
+	//
+	// Check if the new 3 faces share edges with the bad faces
+	//
+
+	Array<Face*, 3> newFaces = {
+		&cface, &m_faces[m_faces.size() - 2], &m_faces[m_faces.size() - 1]};
+
+	Array<Face*, 3> newFacesBad = {nullptr, nullptr, nullptr};
+	U newFacesBadCount = 0;
+
+	for(Face* face : badFaces)
+	{
+		for(Face* nface : newFaces)
+		{
+			for(U i = 0; i < 3; i++)
+			{
+				for(U j = 0; j < 3; j++)
+				{
+					if(nface->edge(*this, i) == face->edge(*this, j))
+					{
+						newFacesBad[newFacesBadCount++] = nface;
+					}
+				}
+			}
+		}
+	}
+
+	for(U i = 0; i < newFacesBadCount; i++)
+	{
+		badFaces.push_back(newFacesBad[i]);
+	}
+
+	//
+	// Get the edges of the deleted faces that belong to the surface of the
+	// deleted faces
+	//
+
+	std::unordered_map<Edge, U, EdgeHasher, EdgeCompare, 
+		StackAllocator<std::pair<Edge, U>>> edgeMap(
+		10, EdgeHasher(), EdgeCompare(), getAllocator());
+
+	for(Face* face : badFaces)
+	{
+		for(U i = 0; i < 3; i ++)
+		{
+			Edge e = face->edge(*this, i);
+			auto it = edgeMap.find(e);
+
+			if(it == edgeMap.end())
+			{
+				// Not found
+				edgeMap[e] = 1;
+			}
+			else
+			{
+				++(it->second);
+			}
+		}
+	}
+
+	//
+	// Get the edges that are in the loop
+	//
+	Vector<Edge, StackAllocator<Edge>> edgeLoop(getAllocator());
+	edgeLoop.reserve(edgeMap.size() + 1);
+
+	for(auto& it : edgeMap)
+	{
+		if(it.second == 1)
+		{
+			edgeLoop.push_back(it.first);
+		}
+	}
+
+	//
+	// Sort those edges to a continues loop
+	// 
+	ANKI_ASSERT(edgeLoop.size() > 2);
+	std::sort(edgeLoop.begin(), edgeLoop.end(), 
+		[](const Edge& a, const Edge& b)
+	{
+		return a.m_idx[1] <= b.m_idx[0];
+	});
+
+	U edgeLoopBegin = 0;
+	if(edgeLoop[0].m_face == edgeLoop[1].m_face)
+	{
+		// The first 2 edges come from the same triangle. That will not work
+		// with the following algorithm
+		edgeLoop.push_back(edgeLoop[0]);
+		edgeLoopBegin = 1;	
+	}
+	
+	ANKI_ASSERT(edgeLoop[0].m_face != edgeLoop[1].m_face);
+
+	//
+	// Spawn faces from the edge loop
+	//
+	Edge edge = edgeLoop[edgeLoopBegin];
+	for(U i = edgeLoopBegin + 1; i < edgeLoop.size(); i++)
+	{
+		ANKI_ASSERT(edge.m_idx[1] == edgeLoop[i].m_idx[0]);
+
+		m_faces.emplace_back(
+			edge.m_idx[0],
+			edge.m_idx[1],
+			edgeLoop[i].m_idx[1]);
+
+		edge = m_faces.back().edge(*this, 2);
+		std::swap(edge.m_idx[0], edge.m_idx[1]);
+	}
+
+	return true;
+}
+
+} // end namesapce detail
+} // end namesapce anki
+

src/renderer/Dbg.cpp

@@ -197,11 +197,12 @@ void Dbg::run(GlJobChainHandle& jobs)
 
 		CollisionDebugDrawer dr(m_drawer.get());
 
-		GjkEpa gjk;
+		GjkEpa gjk(100, 100);
 		ContactPoint cp;
+		StackAllocator<U8> alloc(
+			StackMemoryPool(allocAligned, nullptr, 1024 * 1024));
 
-		Bool intersect = gjk.intersect(s0, s1, cp);
-
+		Bool intersect = gjk.intersect(s0, s1, cp, alloc);
 
 		if(intersect)
 		{
@@ -240,8 +241,8 @@ void Dbg::run(GlJobChainHandle& jobs)
 		{
 			m_drawer->setColor(Vec4(.0, 1.0, 1.0, 1.0));
 			m_drawer->pushBackVertex(Vec3(0, 0, 0));
-			m_drawer->pushBackVertex(gjk.m_simplexArr[0].m_v.xyz());
-			std::cout << gjk.m_simplexArr[0].m_v.xyz().toString() << std::endl;
+			m_drawer->pushBackVertex(gjk.m_poly->m_simplex[0].m_v.xyz());
+			std::cout << gjk.m_poly->m_simplex[0].m_v.xyz().toString() << std::endl;
 		}
 
 		if(0)
@@ -270,12 +271,12 @@ void Dbg::run(GlJobChainHandle& jobs)
 			Array<U, 12> idx = {{0, 1, 2, 1, 2, 3, 2, 3, 0, 3, 0, 1}};
 			for(U i = 0; i < idx.size(); i += 3)
 			{
-				m_drawer->pushBackVertex(gjk.m_simplexArr[idx[i + 0]].m_v.xyz());
-				m_drawer->pushBackVertex(gjk.m_simplexArr[idx[i + 1]].m_v.xyz());
-				m_drawer->pushBackVertex(gjk.m_simplexArr[idx[i + 1]].m_v.xyz());
-				m_drawer->pushBackVertex(gjk.m_simplexArr[idx[i + 2]].m_v.xyz());
-				m_drawer->pushBackVertex(gjk.m_simplexArr[idx[i + 2]].m_v.xyz());
-				m_drawer->pushBackVertex(gjk.m_simplexArr[idx[i + 0]].m_v.xyz());
+				m_drawer->pushBackVertex(gjk.m_poly->m_simplex[idx[i + 0]].m_v.xyz());
+				m_drawer->pushBackVertex(gjk.m_poly->m_simplex[idx[i + 1]].m_v.xyz());
+				m_drawer->pushBackVertex(gjk.m_poly->m_simplex[idx[i + 1]].m_v.xyz());
+				m_drawer->pushBackVertex(gjk.m_poly->m_simplex[idx[i + 2]].m_v.xyz());
+				m_drawer->pushBackVertex(gjk.m_poly->m_simplex[idx[i + 2]].m_v.xyz());
+				m_drawer->pushBackVertex(gjk.m_poly->m_simplex[idx[i + 0]].m_v.xyz());
 			}
 			m_drawer->end();
 		}
@@ -286,10 +287,10 @@ void Dbg::run(GlJobChainHandle& jobs)
 			m_drawer->setModelMatrix(m);
 			m_drawer->setColor(Vec4(1.0, 1.0, 0.0, 1.0));
 			m_drawer->begin();
-			for(U i = 0; i < gjk.m_count - 1; i++)
+			for(U i = 0; i < gjk.m_poly->m_simplex.size() - 1; i++)
 			{
-				m_drawer->pushBackVertex(gjk.m_simplexArr[i].m_v.xyz());
-				m_drawer->pushBackVertex(gjk.m_simplexArr[i + 1].m_v.xyz());
+				m_drawer->pushBackVertex(gjk.m_poly->m_simplex[i].m_v.xyz());
+				m_drawer->pushBackVertex(gjk.m_poly->m_simplex[i + 1].m_v.xyz());
 			}
 			m_drawer->end();
 		}
@@ -304,12 +305,13 @@ void Dbg::run(GlJobChainHandle& jobs)
 			static U64 count = 0;
 
 			++count;
-			U offset = (count / 80) % gjk.m_faceCount;
+			U faceCount = gjk.m_poly->m_faces.size();
+			U offset = (count / 80) % faceCount;
 			//for(U i = 0; i < 1; i++)
-			for(U i = 0; i < gjk.m_faceCount; i++)
+			for(U i = 0; i < faceCount; i++)
 			{
 				//auto idx = gjk.m_faces[offset].m_idx;
-				auto idx = gjk.m_faces[i].m_idx;
+				auto idx = gjk.m_poly->m_faces[i].idx();
 
 				if(i % 2)
 				{
@@ -322,9 +324,9 @@ void Dbg::run(GlJobChainHandle& jobs)
 				m_drawer->setModelMatrix(m);
 
 				m_drawer->setColor(Vec4(1.0, 0.0, 1.0, 1.0) 
-					* Vec4(F32(i + 1) / gjk.m_faceCount));
+					* Vec4(F32(i + 1) / faceCount));
 
-				#define WHAT(i_) gjk.m_simplexArr[idx[i_]].m_v.xyz()
+				#define WHAT(i_) gjk.m_poly->m_simplex[idx[i_]].m_v.xyz()
 
 				m_drawer->pushBackVertex(WHAT(0));
 				m_drawer->pushBackVertex(WHAT(1));

src/scene/SceneGraph.cpp

@@ -79,15 +79,15 @@ struct UpdateSceneNodesJob: ThreadpoolTask
 
 //==============================================================================
 SceneGraph::SceneGraph(AllocAlignedCallback allocCb, void* allocCbUserData)
-	:	m_alloc(StackMemoryPool(allocCb, allocCbUserData, 
-			ANKI_SCENE_ALLOCATOR_SIZE)),
-		m_frameAlloc(StackMemoryPool(allocCb, allocCbUserData, 
-			ANKI_SCENE_FRAME_ALLOCATOR_SIZE)),
-		m_nodes(m_alloc),
-		m_dict(10, DictionaryHasher(), DictionaryEqual(), m_alloc),
-		m_physics(),
-		m_sectorGroup(this),
-		m_events(this)
+:	m_alloc(StackMemoryPool(allocCb, allocCbUserData, 
+		ANKI_SCENE_ALLOCATOR_SIZE)),
+	m_frameAlloc(StackMemoryPool(allocCb, allocCbUserData, 
+		ANKI_SCENE_FRAME_ALLOCATOR_SIZE)),
+	m_nodes(m_alloc),
+	m_dict(10, DictionaryHasher(), DictionaryEqual(), m_alloc),
+	m_physics(),
+	m_sectorGroup(this),
+	m_events(this)
 {
 	m_nodes.reserve(ANKI_SCENE_OPTIMAL_SCENE_NODES_COUNT);