Optimize broadphase AABB and pair management

include/box2d/b2_broad_phase.h

@@ -26,7 +26,6 @@
 #include "b2_settings.h"
 #include "b2_collision.h"
 #include "b2_dynamic_tree.h"
-#include <algorithm>
 
 struct b2Pair
 {
@@ -132,22 +131,6 @@ class b2BroadPhase
 	int32 m_queryProxyId;
 };
 
-/// This is used to sort pairs.
-inline bool b2PairLessThan(const b2Pair& pair1, const b2Pair& pair2)
-{
-	if (pair1.proxyIdA < pair2.proxyIdA)
-	{
-		return true;
-	}
-
-	if (pair1.proxyIdA == pair2.proxyIdA)
-	{
-		return pair1.proxyIdB < pair2.proxyIdB;
-	}
-
-	return false;
-}
-
 inline void* b2BroadPhase::GetUserData(int32 proxyId) const
 {
 	return m_tree.GetUserData(proxyId);
@@ -208,37 +191,30 @@ void b2BroadPhase::UpdatePairs(T* callback)
 		m_tree.Query(this, fatAABB);
 	}
 
-	// Reset move buffer
-	m_moveCount = 0;
-
-	// Sort the pair buffer to expose duplicates.
-	std::sort(m_pairBuffer, m_pairBuffer + m_pairCount, b2PairLessThan);
-
-	// Send the pairs back to the client.
-	int32 i = 0;
-	while (i < m_pairCount)
+	// Send pairs to caller
+	for (int32 i = 0; i < m_pairCount; ++i)
 	{
 		b2Pair* primaryPair = m_pairBuffer + i;
 		void* userDataA = m_tree.GetUserData(primaryPair->proxyIdA);
 		void* userDataB = m_tree.GetUserData(primaryPair->proxyIdB);
 
 		callback->AddPair(userDataA, userDataB);
-		++i;
+	}
 
-		// Skip any duplicate pairs.
-		while (i < m_pairCount)
+	// Clear move flags
+	for (int32 i = 0; i < m_moveCount; ++i)
+	{
+		int32 proxyId = m_moveBuffer[i];
+		if (proxyId == e_nullProxy)
 		{
-			b2Pair* pair = m_pairBuffer + i;
-			if (pair->proxyIdA != primaryPair->proxyIdA || pair->proxyIdB != primaryPair->proxyIdB)
-			{
-				break;
-			}
-			++i;
+			continue;
 		}
+
+		m_tree.ClearMoved(proxyId);
 	}
 
-	// Try to keep the tree balanced.
-	//m_tree.Rebalance(4);
+	// Reset move buffer
+	m_moveCount = 0;
 }
 
 template <typename T>

include/box2d/b2_dynamic_tree.h

@@ -52,6 +52,8 @@ struct b2TreeNode
 
 	// leaf = 0, free node = -1
 	int32 height;
+
+	bool moved;
 };
 
 /// A dynamic AABB tree broad-phase, inspired by Nathanael Presson's btDbvt.
@@ -87,6 +89,9 @@ class b2DynamicTree
 	/// @return the proxy user data or 0 if the id is invalid.
 	void* GetUserData(int32 proxyId) const;
 
+	bool WasMoved(int32 proxyId) const;
+	void ClearMoved(int32 proxyId);
+
 	/// Get the fat AABB for a proxy.
 	const b2AABB& GetFatAABB(int32 proxyId) const;
 
@@ -163,6 +168,18 @@ inline void* b2DynamicTree::GetUserData(int32 proxyId) const
 	return m_nodes[proxyId].userData;
 }
 
+inline bool b2DynamicTree::WasMoved(int32 proxyId) const
+{
+	b2Assert(0 <= proxyId && proxyId < m_nodeCapacity);
+	return m_nodes[proxyId].moved;
+}
+
+inline void b2DynamicTree::ClearMoved(int32 proxyId)
+{
+	b2Assert(0 <= proxyId && proxyId < m_nodeCapacity);
+	m_nodes[proxyId].moved = false;
+}
+
 inline const b2AABB& b2DynamicTree::GetFatAABB(int32 proxyId) const
 {
 	b2Assert(0 <= proxyId && proxyId < m_nodeCapacity);

include/box2d/b2_settings.h

@@ -67,7 +67,7 @@ typedef unsigned int uint32;
 /// This is used to fatten AABBs in the dynamic tree. This is used to predict
 /// the future position based on the current displacement.
 /// This is a dimensionless multiplier.
-#define b2_aabbMultiplier		2.0f
+#define b2_aabbMultiplier		4.0f
 
 /// A small length used as a collision and constraint tolerance. Usually it is
 /// chosen to be numerically significant, but visually insignificant.

include/box2d/b2_world.h

@@ -66,7 +66,7 @@ class b2World
 	void SetContactListener(b2ContactListener* listener);
 
 	/// Register a routine for debug drawing. The debug draw functions are called
-	/// inside with b2World::DrawDebugData method. The debug draw object is owned
+	/// inside with b2World::DebugDraw method. The debug draw object is owned
 	/// by you and must remain in scope.
 	void SetDebugDraw(b2Draw* debugDraw);
 
@@ -109,7 +109,7 @@ class b2World
 	void ClearForces();
 
 	/// Call this to draw shapes and other debug draw data. This is intentionally non-const.
-	void DrawDebugData();
+	void DebugDraw();
 
 	/// Query the world for all fixtures that potentially overlap the
 	/// provided AABB.

src/collision/b2_broad_phase.cpp

@@ -21,6 +21,7 @@
 // SOFTWARE.
 
 #include "box2d/b2_broad_phase.h"
+#include <string.h>
 
 b2BroadPhase::b2BroadPhase()
 {
@@ -105,11 +106,18 @@ bool b2BroadPhase::QueryCallback(int32 proxyId)
 		return true;
 	}
 
+	const bool moved = m_tree.WasMoved(proxyId);
+	if (moved && proxyId > m_queryProxyId)
+	{
+		// Both proxies are moving. Avoid duplicate pairs.
+		return true;
+	}
+
 	// Grow the pair buffer as needed.
 	if (m_pairCount == m_pairCapacity)
 	{
 		b2Pair* oldBuffer = m_pairBuffer;
-		m_pairCapacity *= 2;
+		m_pairCapacity = m_pairCapacity + (m_pairCapacity >> 1);
 		m_pairBuffer = (b2Pair*)b2Alloc(m_pairCapacity * sizeof(b2Pair));
 		memcpy(m_pairBuffer, oldBuffer, m_pairCount * sizeof(b2Pair));
 		b2Free(oldBuffer);

src/collision/b2_dynamic_tree.cpp

@@ -87,6 +87,7 @@ int32 b2DynamicTree::AllocateNode()
 	m_nodes[nodeId].child2 = b2_nullNode;
 	m_nodes[nodeId].height = 0;
 	m_nodes[nodeId].userData = nullptr;
+	m_nodes[nodeId].moved = false;
 	++m_nodeCount;
 	return nodeId;
 }
@@ -115,6 +116,7 @@ int32 b2DynamicTree::CreateProxy(const b2AABB& aabb, void* userData)
 	m_nodes[proxyId].aabb.upperBound = aabb.upperBound + r;
 	m_nodes[proxyId].userData = userData;
 	m_nodes[proxyId].height = 0;
+	m_nodes[proxyId].moved = true;
 
 	InsertLeaf(proxyId);
 
@@ -136,43 +138,61 @@ bool b2DynamicTree::MoveProxy(int32 proxyId, const b2AABB& aabb, const b2Vec2& d
 
 	b2Assert(m_nodes[proxyId].IsLeaf());
 
-	if (m_nodes[proxyId].aabb.Contains(aabb))
-	{
-		return false;
-	}
-
-	RemoveLeaf(proxyId);
-
-	// Extend AABB.
-	b2AABB b = aabb;
+	// Extend AABB
+	b2AABB fatAABB;
 	b2Vec2 r(b2_aabbExtension, b2_aabbExtension);
-	b.lowerBound = b.lowerBound - r;
-	b.upperBound = b.upperBound + r;
+	fatAABB.lowerBound = aabb.lowerBound - r;
+	fatAABB.upperBound = aabb.upperBound + r;
 
-	// Predict AABB displacement.
+	// Predict AABB movement
 	b2Vec2 d = b2_aabbMultiplier * displacement;
 
 	if (d.x < 0.0f)
 	{
-		b.lowerBound.x += d.x;
+		fatAABB.lowerBound.x += d.x;
 	}
 	else
 	{
-		b.upperBound.x += d.x;
+		fatAABB.upperBound.x += d.x;
 	}
 
 	if (d.y < 0.0f)
 	{
-		b.lowerBound.y += d.y;
+		fatAABB.lowerBound.y += d.y;
 	}
 	else
 	{
-		b.upperBound.y += d.y;
+		fatAABB.upperBound.y += d.y;
+	}
+
+	const b2AABB& treeAABB = m_nodes[proxyId].aabb;
+	if (treeAABB.Contains(aabb))
+	{
+		// The tree AABB still contains the object, but it might be too large.
+		// Perhaps the object was moving fast but has since gone to sleep.
+		// The huge AABB is larger than the new fat AABB.
+		b2AABB hugeAABB;
+		hugeAABB.lowerBound = fatAABB.lowerBound - 4.0f * r;
+		hugeAABB.upperBound = fatAABB.upperBound + 4.0f * r;
+
+		if (hugeAABB.Contains(treeAABB))
+		{
+			// The tree AABB contains the object AABB and the tree AABB is
+			// not too large. No tree update needed.
+			return false;
+		}
+
+		// Otherwise the tree AABB is huge and needs to be shrunk
 	}
 
-	m_nodes[proxyId].aabb = b;
+	RemoveLeaf(proxyId);
+
+	m_nodes[proxyId].aabb = fatAABB;
 
 	InsertLeaf(proxyId);
+
+	m_nodes[proxyId].moved = true;
+
 	return true;
 }
 

src/dynamics/b2_body.cpp

@@ -26,6 +26,8 @@
 #include "box2d/b2_joint.h"
 #include "box2d/b2_world.h"
 
+#include <new>
+
 b2Body::b2Body(const b2BodyDef* bd, b2World* world)
 {
 	b2Assert(bd->position.IsValid());
@@ -449,14 +451,25 @@ void b2Body::SetTransform(const b2Vec2& position, float angle)
 
 void b2Body::SynchronizeFixtures()
 {
-	b2Transform xf1;
-	xf1.q.Set(m_sweep.a0);
-	xf1.p = m_sweep.c0 - b2Mul(xf1.q, m_sweep.localCenter);
-
 	b2BroadPhase* broadPhase = &m_world->m_contactManager.m_broadPhase;
-	for (b2Fixture* f = m_fixtureList; f; f = f->m_next)
+
+	if (m_flags & b2Body::e_awakeFlag)
+	{
+		b2Transform xf1;
+		xf1.q.Set(m_sweep.a0);
+		xf1.p = m_sweep.c0 - b2Mul(xf1.q, m_sweep.localCenter);
+
+		for (b2Fixture* f = m_fixtureList; f; f = f->m_next)
+		{
+			f->Synchronize(broadPhase, xf1, m_xf);
+		}
+	}
+	else
 	{
-		f->Synchronize(broadPhase, xf1, m_xf);
+		for (b2Fixture* f = m_fixtureList; f; f = f->m_next)
+		{
+			f->Synchronize(broadPhase, m_xf, m_xf);
+		}
 	}
 }
 

src/dynamics/b2_fixture.cpp

@@ -171,7 +171,7 @@ void b2Fixture::Synchronize(b2BroadPhase* broadPhase, const b2Transform& transfo
 
 		proxy->aabb.Combine(aabb1, aabb2);
 
-		b2Vec2 displacement = transform2.p - transform1.p;
+		b2Vec2 displacement = aabb2.GetCenter() - aabb1.GetCenter();
 
 		broadPhase->MoveProxy(proxy->proxyId, proxy->aabb, displacement);
 	}

src/dynamics/b2_world.cpp

@@ -1167,7 +1167,7 @@ void b2World::DrawJoint(b2Joint* joint)
 	}
 }
 
-void b2World::DrawDebugData()
+void b2World::DebugDraw()
 {
 	if (m_debugDraw == nullptr)
 	{
@@ -1220,13 +1220,14 @@ void b2World::DrawDebugData()
 		b2Color color(0.3f, 0.9f, 0.9f);
 		for (b2Contact* c = m_contactManager.m_contactList; c; c = c->GetNext())
 		{
-			//b2Fixture* fixtureA = c->GetFixtureA();
-			//b2Fixture* fixtureB = c->GetFixtureB();
-
-			//b2Vec2 cA = fixtureA->GetAABB().GetCenter();
-			//b2Vec2 cB = fixtureB->GetAABB().GetCenter();
-
-			//g_debugDraw->DrawSegment(cA, cB, color);
+			b2Fixture* fixtureA = c->GetFixtureA();
+			b2Fixture* fixtureB = c->GetFixtureB();
+			int32 indexA = c->GetChildIndexA();
+			int32 indexB = c->GetChildIndexB();
+			b2Vec2 cA = fixtureA->GetAABB(indexA).GetCenter();
+			b2Vec2 cB = fixtureB->GetAABB(indexB).GetCenter();
+
+			m_debugDraw->DrawSegment(cA, cB, color);
 		}
 	}
 

testbed/CMakeLists.txt

@@ -11,7 +11,7 @@ set (TESTBED_SOURCE_FILES
 	test.cpp
 	test.h
 	tests/add_pair.cpp
-	tests/apply_forces.cpp
+	tests/apply_force.cpp
 	tests/body_types.cpp
 	tests/box_stack.cpp
 	tests/breakable.cpp

testbed/draw.cpp

@@ -23,6 +23,7 @@
 #include "draw.h"
 #include <stdio.h>
 #include <stdarg.h>
+#include <stdlib.h>
 
 #include "imgui/imgui.h"