wip

include/box2d/dynamic_tree.h

@@ -95,7 +95,7 @@ extern "C"
 
 	/// Enlarge a proxy and enlarge ancestors as necessary.
 	/// @return true if the internal bounds grew. The node move flag is set true if the tree is non-static.
-	void b2DynamicTree_EnlargeProxy(b2DynamicTree* tree, int32_t proxyId, b2AABB aabb, b2AABB* outFatAABB);
+	void b2DynamicTree_EnlargeProxy(b2DynamicTree* tree, int32_t proxyId, b2AABB aabb);
 
 	/// This function receives proxies found in the AABB query.
 	/// @return true if the query should continue

samples/collection/sample_dynamic_tree.cpp

@@ -295,6 +295,8 @@ class DynamicTree : public Sample
 			}
 		}
 
+		const b2Vec2 aabbExtension = {b2_aabbExtension, b2_aabbExtension};
+
 		switch (m_updateType)
 		{
 		case Update_Incremental:
@@ -321,7 +323,9 @@ class DynamicTree : public Sample
 				Proxy* p = m_proxies + i;
 				if (p->moved)
 				{
-					b2DynamicTree_EnlargeProxy(&m_tree, p->proxyId, p->box, &p->fatBox);
+					p->fatBox.lowerBound = b2Sub(p->box.lowerBound, aabbExtension);
+					p->fatBox.upperBound = b2Add(p->box.upperBound, aabbExtension);
+					b2DynamicTree_EnlargeProxy(&m_tree, p->proxyId, p->fatBox);
 				}
 			}
 
@@ -340,7 +344,9 @@ class DynamicTree : public Sample
 				Proxy* p = m_proxies + i;
 				if (p->moved)
 				{
-					b2DynamicTree_EnlargeProxy(&m_tree, p->proxyId, p->box, &p->fatBox);
+					p->fatBox.lowerBound = b2Sub(p->box.lowerBound, aabbExtension);
+					p->fatBox.upperBound = b2Add(p->box.upperBound, aabbExtension);
+					b2DynamicTree_EnlargeProxy(&m_tree, p->proxyId, p->fatBox);
 				}
 			}
 

src/body.c

@@ -79,7 +79,7 @@ b2BodyId b2World_CreateBody(b2WorldId worldId, const b2BodyDef* def)
 		// Every new body gets a new island. Islands get merged during simulation.
 		b2Island* island = (b2Island*)b2AllocObject(&world->islandPool);
 		world->islands = (b2Island*)world->islandPool.memory;
-		b2ClearIsland(island);
+		b2CreateIsland(island);
 		island->world = world;
 
 		b->islandIndex = island->object.index;
@@ -160,9 +160,15 @@ void b2World_DestroyBody(b2BodyId bodyId)
 		}
 
 		// Remove from awake contact array
-		if (contact->awakeIndex != B2_NULL_INDEX)
+		// TODO_ERIN perf problem?
+		int32_t awakeContactCount = b2Array(world->awakeContactArray).count;
+		for (int32_t i = 0; i < awakeContactCount; ++i)
 		{
-			world->awakeContactArray[contact->awakeIndex] = B2_NULL_INDEX;
+			if (world->awakeContactArray[i] == contactIndex)
+			{
+				b2Array_RemoveSwap(world->awakeContactArray, i);
+				break;
+			}
 		}
 
 		// Remove pair from set
@@ -239,6 +245,7 @@ void b2World_DestroyBody(b2BodyId bodyId)
 				}
 
 				// Free the island
+				b2DestroyIsland(island);
 				b2FreeObject(&world->islandPool, &island->object);
 				islandDestroyed = true;
 			}

src/broad_phase.c

@@ -25,7 +25,7 @@
 
 //static FILE* s_file = NULL;
 
-void b2BroadPhase_Create(b2BroadPhase* bp)
+void b2CreateBroadPhase(b2BroadPhase* bp)
 {
 	//if (s_file == NULL)
 	//{
@@ -52,9 +52,13 @@ void b2BroadPhase_Create(b2BroadPhase* bp)
 		bool isStatic = i == b2_staticBody;
 		bp->trees[i] = b2DynamicTree_Create(isStatic);
 	}
+
+	bp->enlargedProxies = NULL;
+	bp->enlargedProxyCapacity = 0;
+	bp->enlargedProxyCount = 0;
 }
 
-void b2BroadPhase_Destroy(b2BroadPhase* bp)
+void b2DestroyBroadPhase(b2BroadPhase* bp)
 {
 	for (int32_t i = 0; i < b2_bodyTypeCount; ++i)
 	{
@@ -65,6 +69,8 @@ void b2BroadPhase_Destroy(b2BroadPhase* bp)
 	b2DestroyArray(bp->moveArray, sizeof(int32_t));
 	b2DestroySet(&bp->pairSet);
 
+	b2Free(bp->enlargedProxies, bp->enlargedProxyCapacity * sizeof(b2EnlargedProxy));
+
 	memset(bp, 0, sizeof(b2BroadPhase));
 
 	//if (s_file != NULL)
@@ -83,6 +89,25 @@ static inline void b2BufferMove(b2BroadPhase* bp, int32_t proxyKey)
 	}
 }
 
+static inline void b2UnBufferMove(b2BroadPhase* bp, int32_t proxyKey)
+{
+	bool found = b2RemoveKey(&bp->moveSet, proxyKey);
+
+	if (found)
+	{
+		// Purge from move buffer. Linear search.
+		int32_t count = b2Array(bp->moveArray).count;
+		for (int32_t i = 0; i < count; ++i)
+		{
+			if (bp->moveArray[i] == proxyKey)
+			{
+				b2Array_RemoveSwap(bp->moveArray, i);
+				break;
+			}
+		}
+	}
+}
+
 int32_t b2BroadPhase_CreateProxy(b2BroadPhase* bp, b2BodyType bodyType, b2AABB aabb, uint32_t categoryBits, int32_t shapeIndex,
 								 b2AABB* outFatAABB)
 {
@@ -98,21 +123,8 @@ int32_t b2BroadPhase_CreateProxy(b2BroadPhase* bp, b2BodyType bodyType, b2AABB a
 
 void b2BroadPhase_DestroyProxy(b2BroadPhase* bp, int32_t proxyKey)
 {
-	// Purge from move buffer. Linear search.
-	bool found = b2RemoveKey(&bp->moveSet, proxyKey);
-
-	if (found)
-	{
-		int32_t count = b2Array(bp->moveArray).count;
-		for (int32_t i = 0; i < count; ++i)
-		{
-			if (bp->moveArray[i] == proxyKey)
-			{
-				bp->moveArray[i] = B2_NULL_INDEX;
-				break;
-			}
-		}
-	}
+	B2_ASSERT(b2Array(bp->moveArray).count == (int32_t)bp->moveSet.count);
+	b2UnBufferMove(bp, proxyKey);
 
 	--bp->proxyCount;
 
@@ -137,14 +149,14 @@ void b2BroadPhase_MoveProxy(b2BroadPhase* bp, int32_t proxyKey, b2AABB aabb, b2A
 	}
 }
 
-void b2BroadPhase_EnlargeProxy(b2BroadPhase* bp, int32_t proxyKey, b2AABB aabb, b2AABB* outFatAABB)
+void b2BroadPhase_EnlargeProxy(b2BroadPhase* bp, int32_t proxyKey, b2AABB aabb)
 {
 	int32_t typeIndex = B2_PROXY_TYPE(proxyKey);
 	int32_t proxyId = B2_PROXY_ID(proxyKey);
 
 	B2_ASSERT(typeIndex == b2_dynamicBody || typeIndex == b2_kinematicBody);
 
-	b2DynamicTree_EnlargeProxy(bp->trees + typeIndex, proxyId, aabb, outFatAABB);
+	b2DynamicTree_EnlargeProxy(bp->trees + typeIndex, proxyId, aabb);
 	b2BufferMove(bp, proxyKey);
 }
 
@@ -321,7 +333,7 @@ void b2FindPairsTask(int32_t startIndex, int32_t endIndex, uint32_t threadIndex,
 
 extern bool b2_parallel;
 
-void b2BroadPhase_UpdatePairs(b2World* world)
+void b2UpdateBroadPhasePairs(b2World* world)
 {
 	b2BroadPhase* bp = &world->broadPhase;
 
@@ -422,8 +434,6 @@ void b2BroadPhase_UpdatePairs(b2World* world)
 	b2FreeStackItem(alloc, bp->moveResults);
 	bp->moveResults = NULL;
 
-	//b2ValidateNoMoved(&world->broadPhase);
-
 	b2TracyCZoneEnd(create_contacts);
 
 	b2TracyCZoneEnd(update_pairs);
@@ -455,10 +465,25 @@ int32_t b2BroadPhase_GetShapeIndex(b2BroadPhase* bp, int32_t proxyKey)
 	return b2DynamicTree_GetUserData(bp->trees + typeIndex, proxyId);
 }
 
+void b2PrepareBroadPhase(b2BroadPhase* bp)
+{
+	int32_t proxyCapacity = bp->trees[b2_dynamicBody].proxyCount + bp->trees[b2_kinematicBody].proxyCount;
+	if (proxyCapacity > bp->enlargedProxyCapacity)
+	{
+		b2Free(bp->enlargedProxies, bp->enlargedProxyCapacity * sizeof(b2EnlargedProxy));
+		bp->enlargedProxyCapacity = proxyCapacity + proxyCapacity / 2;
+		bp->enlargedProxies = b2Alloc(bp->enlargedProxyCapacity * sizeof(b2EnlargedProxy));
+	}
+
+	bp->enlargedProxyCount = 0;
+}
+
 void b2ValidateBroadphase(const b2BroadPhase* bp)
 {
 	b2DynamicTree_Validate(bp->trees + b2_dynamicBody);
 	b2DynamicTree_Validate(bp->trees + b2_kinematicBody);
+
+	// TODO_ERIN validate every shape AABB is contained in tree AABB
 }
 
 void b2ValidateNoEnlarged(const b2BroadPhase* bp)

src/broad_phase.h

@@ -17,6 +17,12 @@ typedef struct b2World b2World;
 #define B2_PROXY_ID(KEY) ((KEY) >> 4)
 #define B2_PROXY_KEY(ID, TYPE) (((ID) << 4) | (TYPE))
 
+typedef struct b2EnlargedProxy
+{
+	b2AABB aabb;
+	int32_t proxyKey;
+} b2EnlargedProxy;
+
 /// The broad-phase is used for computing pairs and performing volume queries and ray casts.
 /// This broad-phase does not persist pairs. Instead, this reports potentially new pairs.
 /// It is up to the client to consume the new pairs and to track subsequent overlap.
@@ -25,6 +31,12 @@ typedef struct b2BroadPhase
 	b2DynamicTree trees[b2_bodyTypeCount];
 	int32_t proxyCount;
 
+	// Buffer that holds all the proxies enlarged from the island solver stage.
+	// The order doesn't matter. Rebuilt every time step.
+	b2EnlargedProxy* enlargedProxies;
+	int32_t enlargedProxyCapacity;
+	_Atomic int enlargedProxyCount;
+
 	b2Set moveSet;
 
 	// TODO_ERIN perhaps just a move set?
@@ -39,22 +51,24 @@ typedef struct b2BroadPhase
 
 } b2BroadPhase;
 
-void b2BroadPhase_Create(b2BroadPhase* bp);
-void b2BroadPhase_Destroy(b2BroadPhase* bp);
+void b2CreateBroadPhase(b2BroadPhase* bp);
+void b2DestroyBroadPhase(b2BroadPhase* bp);
 int32_t b2BroadPhase_CreateProxy(b2BroadPhase* bp, b2BodyType bodyType, b2AABB aabb, uint32_t categoryBits, int32_t shapeIndex,
 								 b2AABB* outFatAABB);
 void b2BroadPhase_DestroyProxy(b2BroadPhase* bp, int32_t proxyKey);
 
 void b2BroadPhase_MoveProxy(b2BroadPhase* bp, int32_t proxyKey, b2AABB aabb, b2AABB* outFatAABB);
-void b2BroadPhase_EnlargeProxy(b2BroadPhase* bp, int32_t proxyKey, b2AABB aabb, b2AABB* outFatAABB);
+void b2BroadPhase_EnlargeProxy(b2BroadPhase* bp, int32_t proxyKey, b2AABB aabb);
 
 void b2BroadPhase_RebuildTrees(b2BroadPhase* bp);
 
 int32_t b2BroadPhase_GetShapeIndex(b2BroadPhase* bp, int32_t proxyKey);
 
-void b2BroadPhase_UpdatePairs(b2World* world);
+void b2UpdateBroadPhasePairs(b2World* world);
 bool b2BroadPhase_TestOverlap(const b2BroadPhase* bp, int32_t proxyKeyA, int32_t proxyKeyB);
 
+void b2PrepareBroadPhase(b2BroadPhase* bp);
+
 void b2ValidateBroadphase(const b2BroadPhase* bp);
 void b2ValidateNoEnlarged(const b2BroadPhase* bp);
 

src/contact.c

@@ -18,6 +18,24 @@
 #include <float.h>
 #include <math.h>
 
+// Contacts and determinism
+// A deterministic simulation requires contacts to exist in the same order in b2Island no matter the thread count.
+// The order must reproduce from run to run. This is necessary because Gauss-Seidel is order dependent.
+// 
+// Creation:
+// - Contacts are created using results from b2UpdateBroadPhasePairs
+// - These results are ordered according to the order of the broad-phase move array
+// - The move array is ordered according to the island/shape/body order. (TODO_ERIN WRONG!!!! Use bit set)
+// - The island/shape/body order is determined by creation order
+// - Logically contacts are only created for awake bodies, so they are immediately added to the awake contact array (serially)
+// 
+// Island linking:
+// - The awake contact array is built from the body-contact graph for all awake bodies in awake islands (TODO_ERIN use bit set)
+// - Awake contacts are solved in parallel and they generate contact state changes.
+// - These state changes may link islands together using union find.
+// - The state changes are ordered using a bit array that encompasses all contacts
+// - As long as contacts are created in deterministic order, island link order is deterministic
+
 // Friction mixing law. The idea is to allow either fixture to drive the friction to zero.
 // For example, anything slides on ice.
 static inline float b2MixFriction(float friction1, float friction2)
@@ -124,7 +142,6 @@ void b2CreateContact(b2World* world, b2Shape* shapeA, b2Shape* shapeB)
 	contact->shapeIndexB = shapeB->object.index;
 	contact->cache = b2_emptyDistanceCache;
 	contact->manifold = b2_emptyManifold;
-	contact->awakeIndex = B2_NULL_INDEX;
 	contact->friction = b2MixFriction(shapeA->friction, shapeB->friction);
 	contact->restitution = b2MixRestitution(shapeA->restitution, shapeB->restitution);
 	contact->tangentSpeed = 0.0f;
@@ -172,7 +189,6 @@ void b2CreateContact(b2World* world, b2Shape* shapeA, b2Shape* shapeB)
 	if (b2IsBodyAwake(world, bodyA) || b2IsBodyAwake(world, bodyB))
 	{
 		// A contact does not need to be in an island to be awake.
-		contact->awakeIndex = b2Array(world->awakeContactArray).count;
 		b2Array_Push(world->awakeContactArray, contact->object.index);
 	}
 
@@ -249,9 +265,17 @@ void b2DestroyContact(b2World* world, b2Contact* contact)
 		b2UnlinkContact(world, contact);
 	}
 
-	if (contact->awakeIndex != B2_NULL_INDEX)
+	// Remove from awake contact array
+	// TODO_ERIN perf problem?
+	int32_t contactIndex = contact->object.index;
+	int32_t awakeContactCount = b2Array(world->awakeContactArray).count;
+	for (int32_t i = 0; i < awakeContactCount; ++i)
 	{
-		world->awakeContactArray[contact->awakeIndex] = B2_NULL_INDEX;
+		if (world->awakeContactArray[i] == contactIndex)
+		{
+			b2Array_RemoveSwap(world->awakeContactArray, i);
+			break;
+		}
 	}
 
 	b2FreeObject(&world->contactPool, &contact->object);

src/contact.h

@@ -75,7 +75,7 @@ typedef struct b2Contact
 
 	// Awake contacts are associated with bodies in awake islands.
 	// The contact does not need to be touching or be in an island to be awake.
-	int32_t awakeIndex;
+	//int32_t awakeIndex;
 
 	// Mixed friction and restitution
 	float friction;

src/dynamic_tree.c

@@ -843,7 +843,7 @@ bool b2DynamicTree_MoveProxy(b2DynamicTree* tree, int32_t proxyId, b2AABB aabb,
 	return true;
 }
 
-void b2DynamicTree_EnlargeProxy(b2DynamicTree* tree, int32_t proxyId, b2AABB aabb, b2AABB* outFatAABB)
+void b2DynamicTree_EnlargeProxy(b2DynamicTree* tree, int32_t proxyId, b2AABB aabb)
 {
 	b2TreeNode* nodes = tree->nodes;
 
@@ -858,16 +858,12 @@ void b2DynamicTree_EnlargeProxy(b2DynamicTree* tree, int32_t proxyId, b2AABB aab
 	// Caller must ensure this
 	B2_ASSERT(b2AABB_Contains(nodes[proxyId].aabb, aabb) == false);
 
-	b2AABB fatAABB;
-	b2Vec2 r = {b2_aabbExtension, b2_aabbExtension};
-	fatAABB.lowerBound = b2Sub(aabb.lowerBound, r);
-	fatAABB.upperBound = b2Add(aabb.upperBound, r);
-	nodes[proxyId].aabb = fatAABB;
+	nodes[proxyId].aabb = aabb;
 
 	int32_t parentIndex = nodes[proxyId].parent;
 	while (parentIndex != B2_NULL_INDEX)
 	{
-		bool changed = b2AABB_Enlarge(&nodes[parentIndex].aabb, fatAABB);
+		bool changed = b2AABB_Enlarge(&nodes[parentIndex].aabb, aabb);
 		nodes[parentIndex].enlarged = true;
 		parentIndex = nodes[parentIndex].parent;
 
@@ -888,22 +884,6 @@ void b2DynamicTree_EnlargeProxy(b2DynamicTree* tree, int32_t proxyId, b2AABB aab
 		nodes[parentIndex].enlarged = true;
 		parentIndex = nodes[parentIndex].parent;
 	}
-
-	*outFatAABB = fatAABB;
-
-	//if (tree->isStatic)
-	//{
-	//	return true;
-	//}
-
-	//bool alreadyMoved = nodes[proxyId].moved;
-	//if (alreadyMoved)
-	//{
-	//	return false;
-	//}
-
-	//nodes[proxyId].moved = true;
-	//return true;
 }
 
 int32_t b2DynamicTree_GetHeight(const b2DynamicTree* tree)