Unbuffer move if proxy gets destroyed

include/bounce/collision/broad_phase.h

@@ -22,6 +22,8 @@
 #include <bounce/collision/trees/dynamic_tree.h>
 #include <algorithm>
 
+#define B3_NULL_PROXY (0xFFFFFFFF)
+
 // A pair of broad-phase proxies.
 struct b3Pair
 {
@@ -49,9 +51,8 @@ public:
 	// Return true if the proxy has moved.
 	bool MoveProxy(u32 proxyId, const b3AABB3& aabb, const b3Vec3& displacement);
 
-	// Add a proxy to the list of moved proxies.
-	// Only moved proxies will be used internally as an AABB query reference object.
-	void BufferMove(u32 proxyId);
+	// Force move the proxy
+	void TouchProxy(u32 proxyId);
 
 	// Get the AABB of a given proxy.
 	const b3AABB3& GetAABB(u32 proxyId) const;
@@ -59,6 +60,9 @@ public:
 	// Get the user data attached to a proxy.
 	void* GetUserData(u32 proxyId) const;
 
+	// Get the number of proxies.
+	u32 GetProxyCount() const;
+
 	// Test if two proxy AABBs are overlapping.
 	bool TestOverlap(u32 proxy1, u32 proxy2) const;
 	
@@ -81,6 +85,9 @@ public:
 	void Draw() const;
 private :
 	friend class b3DynamicTree;
+
+	void BufferMove(u32 proxyId);
+	void UnbufferMove(u32 proxyId);
 	
 	// The client callback used to add an overlapping pair
 	// to the overlapping pair buffer.
@@ -89,6 +96,9 @@ private :
 	// The dynamic tree.
 	b3DynamicTree m_tree;
 
+	// Number of proxies
+	u32 m_proxyCount;
+
 	// The current proxy being queried for overlap with another proxies. 
 	// It is used to avoid a proxy overlap with itself.
 	u32 m_queryProxyId;
@@ -114,6 +124,11 @@ inline void* b3BroadPhase::GetUserData(u32 proxyId) const
 	return m_tree.GetUserData(proxyId);
 }
 
+inline u32 b3BroadPhase::GetProxyCount() const
+{
+	return m_proxyCount;
+}
+
 template<class T>
 inline void b3BroadPhase::QueryAABB(T* callback, const b3AABB3& aabb) const 
 {
@@ -152,8 +167,10 @@ inline void b3BroadPhase::FindPairs(T* callback)
 	{
 		// Keep the current queried proxy ID to avoid self overlapping.
 		m_queryProxyId = m_moveBuffer[i];
-		if (m_queryProxyId == B3_NULL_NODE_D) 
+
+		if (m_queryProxyId == B3_NULL_PROXY)
 		{
+			// Proxy was unbuffered
 			continue;
 		}
 

src/bounce/collision/broad_phase.cpp

@@ -20,6 +20,8 @@
 
 b3BroadPhase::b3BroadPhase() 
 {
+	m_proxyCount = 0;
+
 	m_moveBufferCapacity = 16;
 	m_moveBuffer = (u32*)b3Alloc(m_moveBufferCapacity * sizeof(u32));
 	memset(m_moveBuffer, 0, m_moveBufferCapacity * sizeof(u32));
@@ -57,6 +59,17 @@ void b3BroadPhase::BufferMove(u32 proxyId)
 	++m_moveBufferCount;
 }
 
+void b3BroadPhase::UnbufferMove(u32 proxyId)
+{
+	for (u32 i = 0; i < m_moveBufferCount; ++i)
+	{
+		if (m_moveBuffer[i] == proxyId)
+		{
+			m_moveBuffer[i] = B3_NULL_PROXY;
+		}
+	}
+}
+
 bool b3BroadPhase::TestOverlap(u32 proxy1, u32 proxy2) const 
 {
 	return m_tree.TestOverlap(proxy1, proxy2);
@@ -64,20 +77,23 @@ bool b3BroadPhase::TestOverlap(u32 proxy1, u32 proxy2) const
 
 u32 b3BroadPhase::CreateProxy(const b3AABB3& aabb, void* userData) 
 {
-	// Later, if the node aabb has changed then it should be reinserted into the tree.
-	// However, this can be expansive due to the hierarchy reconstruction.
-	// Therefore, the original AABB is extended and inserted into the tree,
-	// so we can check later if the new (original) AABB is inside the old (fat) AABB.
 	b3AABB3 fatAABB = aabb;
 	fatAABB.Extend(B3_AABB_EXTENSION);	
+	
 	u32 proxyId = m_tree.InsertNode(fatAABB, userData);
+	
+	++m_proxyCount;
+	
 	BufferMove(proxyId);
+
 	return proxyId;
 }
 
 void b3BroadPhase::DestroyProxy(u32 proxyId) 
 {
-	return m_tree.RemoveNode(proxyId);
+	UnbufferMove(proxyId);
+	--m_proxyCount;
+	m_tree.RemoveNode(proxyId);
 }
 
 bool b3BroadPhase::MoveProxy(u32 proxyId, const b3AABB3& aabb, const b3Vec3& displacement)
@@ -88,39 +104,38 @@ bool b3BroadPhase::MoveProxy(u32 proxyId, const b3AABB3& aabb, const b3Vec3& dis
 		return false;
 	}
 
-	// Update the tree with a fat and motion predicted AABB.
-	const b3Vec3 kExtension(B3_AABB_EXTENSION, B3_AABB_EXTENSION, B3_AABB_EXTENSION);
+	// Extend the AABB.
+	b3AABB3 fatAABB = aabb;
+	fatAABB.Extend(B3_AABB_EXTENSION);
 
-	// Extend the new (original) AABB.
-	b3AABB3 fatAABB;
-	fatAABB.m_lower = aabb.m_lower - kExtension;
-	fatAABB.m_upper = aabb.m_upper + kExtension;
+	// Predict AABB displacement.
+	b3Vec3 d = B3_AABB_MULTIPLIER * displacement;
 
-	if (displacement.x < 0.0f)
+	if (d.x < 0.0f)
 	{
-		fatAABB.m_lower.x += B3_AABB_MULTIPLIER * displacement.x;
+		fatAABB.m_lower.x += d.x;
 	}
 	else
 	{
-		fatAABB.m_upper.x += B3_AABB_MULTIPLIER * displacement.x;
+		fatAABB.m_upper.x += d.x;
 	}
 
-	if (displacement.y < 0.0f)
+	if (d.y < 0.0f)
 	{
-		fatAABB.m_lower.y += B3_AABB_MULTIPLIER * displacement.y;
+		fatAABB.m_lower.y += d.y;
 	}
 	else
 	{
-		fatAABB.m_upper.y += B3_AABB_MULTIPLIER * displacement.y;
+		fatAABB.m_upper.y += d.y;
 	}
 
-	if (displacement.z < 0.0f)
+	if (d.z < 0.0f)
 	{
-		fatAABB.m_lower.z += B3_AABB_MULTIPLIER * displacement.z;
+		fatAABB.m_lower.z += d.z;
 	}
 	else
 	{
-		fatAABB.m_upper.z += B3_AABB_MULTIPLIER * displacement.z;
+		fatAABB.m_upper.z += d.z;
 	}
 
 	// Update proxy with the extented AABB.
@@ -133,6 +148,11 @@ bool b3BroadPhase::MoveProxy(u32 proxyId, const b3AABB3& aabb, const b3Vec3& dis
 	return true;
 }
 
+void b3BroadPhase::TouchProxy(u32 proxyId)
+{
+	BufferMove(proxyId);
+}
+
 bool b3BroadPhase::Report(u32 proxyId) 
 {
 	if (proxyId == m_queryProxyId) 

src/bounce/collision/trees/dynamic_tree.cpp

@@ -123,6 +123,7 @@ void b3DynamicTree::RemoveNode(u32 proxyId)
 {
 	// Remove from the tree.
 	RemoveLeaf(proxyId);
+	
 	// Remove from the node array and make it available.
 	FreeNode(proxyId);
 }
@@ -131,8 +132,10 @@ void b3DynamicTree::UpdateNode(u32 proxyId, const b3AABB3& aabb)
 {
 	B3_ASSERT(m_root != B3_NULL_NODE_D);
 	B3_ASSERT(m_nodes[proxyId].IsLeaf());
+	
 	// Remove old AABB from the tree.
 	RemoveLeaf(proxyId);
+	
 	// Insert the new AABB to the tree.
 	m_nodes[proxyId].aabb = aabb;
 	InsertLeaf(proxyId);

src/bounce/dynamics/body.cpp

@@ -457,7 +457,7 @@ void b3Body::SetType(b3BodyType type)
 	b3BroadPhase* phase = &m_world->m_contactMan.m_broadPhase;
 	for (b3Shape* s = m_shapeList.m_head; s; s = s->m_next)
 	{
-		phase->BufferMove(s->m_broadPhaseID);
+		phase->TouchProxy(s->m_broadPhaseID);
 	}
 }