validation and bug fixing

include/box2d/dynamic_tree.h

@@ -63,6 +63,9 @@ typedef struct b2DynamicTree
 	b2Vec2* leafCenters;
 	int32_t* binIndices;
 	int32_t rebuildCapacity;
+
+	/// A static tree never marks a node as moved
+	bool isStatic;
 } b2DynamicTree;
 
 #ifdef __cplusplus
@@ -71,7 +74,7 @@ extern "C"
 #endif
 
 	/// Constructing the tree initializes the node pool.
-	b2DynamicTree b2DynamicTree_Create();
+	b2DynamicTree b2DynamicTree_Create(bool isStatic);
 
 	/// Destroy the tree, freeing the node pool.
 	void b2DynamicTree_Destroy(b2DynamicTree* tree);
@@ -89,10 +92,11 @@ extern "C"
 	/// fattened AABB, then the proxy is removed from the tree and re-inserted.
 	/// Otherwise the function returns immediately.
 	/// @return true if the proxy was re-inserted and the moved flag was previously false
+	/// for a static tree this is true if the proxy was re-inserted, the move flag is not set.
 	bool b2DynamicTree_MoveProxy(b2DynamicTree* tree, int32_t proxyId, b2AABB aabb, b2AABB* outFatAABB);
 
 	/// Enlarge a proxy and enlarge ancestors as necessary.
-	/// @return true if the internal bounds grew
+	/// @return true if the internal bounds grew. The node move flag is set true if the tree is non-static.
 	bool b2DynamicTree_EnlargeProxy(b2DynamicTree* tree, int32_t proxyId, b2AABB aabb, b2AABB* outFatAABB);
 
 	/// This function receives proxies found in the AABB query.

samples/collection/sample_dynamic_tree.cpp

@@ -86,7 +86,8 @@ class DynamicTree : public Sample
 
 		float y = -4.0f;
 
-		m_tree = b2DynamicTree_Create();
+		bool isStatic = false;
+		m_tree = b2DynamicTree_Create(isStatic);
 
 		for (int i = 0; i < m_rowCount; ++i)
 		{

src/broad_phase.c

@@ -16,6 +16,7 @@
 #include "box2d/timer.h"
 
 #include <stdatomic.h>
+#include <stdbool.h>
 #include <string.h>
 
 void b2BroadPhase_Create(b2BroadPhase* bp)
@@ -37,7 +38,8 @@ void b2BroadPhase_Create(b2BroadPhase* bp)
 
 	for (int32_t i = 0; i < b2_bodyTypeCount; ++i)
 	{
-		bp->trees[i] = b2DynamicTree_Create();
+		bool isStatic = i == b2_staticBody;
+		bp->trees[i] = b2DynamicTree_Create(isStatic);
 	}
 }
 
@@ -75,6 +77,13 @@ int32_t b2BroadPhase_CreateProxy(b2BroadPhase* bp, b2BodyType bodyType, b2AABB a
 								 b2AABB* outFatAABB)
 {
 	B2_ASSERT(0 <= bodyType && bodyType < b2_bodyTypeCount);
+	if (bodyType == b2_staticBody)
+	{
+		int32_t proxyId = b2DynamicTree_CreateProxy(bp->trees + bodyType, aabb, categoryBits, shapeIndex, outFatAABB);
+		int32_t proxyKey = B2_PROXY_KEY(proxyId, bodyType);
+		return proxyKey;
+	}
+
 	int32_t proxyId = b2DynamicTree_CreateProxy(bp->trees + bodyType, aabb, categoryBits, shapeIndex, outFatAABB);
 	int32_t proxyKey = B2_PROXY_KEY(proxyId, bodyType);
 	b2BufferMove(bp, proxyKey);
@@ -393,8 +402,10 @@ void b2BroadPhase_UpdatePairs(b2World* world)
 	b2FreeStackItem(alloc, bp->moveResults);
 	bp->moveResults = NULL;
 
+	b2ValidateNoMoved(&world->broadPhase);
+
 	b2TracyCZoneEnd(create_contacts);
-	
+
 	b2TracyCZoneEnd(update_pairs);
 }
 
@@ -429,3 +440,56 @@ void b2ValidateBroadphase(const b2BroadPhase* bp)
 	b2DynamicTree_Validate(bp->trees + b2_dynamicBody);
 	b2DynamicTree_Validate(bp->trees + b2_kinematicBody);
 }
+
+void b2ValidateNoMoved(const b2BroadPhase* bp)
+{
+#if B2_VALIDATE == 1
+	for (int32_t j = 0; j < b2_bodyTypeCount; ++j)
+	{
+		const b2DynamicTree* tree = bp->trees + j;
+		int32_t capacity = tree->nodeCapacity;
+		const b2TreeNode* nodes = tree->nodes;
+		for (int32_t i = 0; i < capacity; ++i)
+		{
+			const b2TreeNode* node = nodes + i;
+			if (node->height < 0)
+			{
+				continue;
+			}
+
+			B2_ASSERT(node->moved == false);
+		}
+	}
+#else
+	B2_MAYBE_UNUSED(bp);
+#endif
+}
+
+void b2ValidateNoEnlarged(const b2BroadPhase* bp)
+{
+#if B2_VALIDATE == 1
+	for (int32_t j = 0; j < b2_bodyTypeCount; ++j)
+	{
+		const b2DynamicTree* tree = bp->trees + j;
+		int32_t capacity = tree->nodeCapacity;
+		const b2TreeNode* nodes = tree->nodes;
+		for (int32_t i = 0; i < capacity; ++i)
+		{
+			const b2TreeNode* node = nodes + i;
+			if (node->height < 0)
+			{
+				continue;
+			}
+
+			if (node->enlarged == true)
+			{
+				capacity += 0;
+			}
+
+			B2_ASSERT(node->enlarged == false);
+		}
+	}
+#else
+	B2_MAYBE_UNUSED(bp);
+#endif
+}

src/broad_phase.h

@@ -57,6 +57,8 @@ void b2BroadPhase_UpdatePairs(b2World* world);
 bool b2BroadPhase_TestOverlap(const b2BroadPhase* bp, int32_t proxyKeyA, int32_t proxyKeyB);
 
 void b2ValidateBroadphase(const b2BroadPhase* bp);
+void b2ValidateNoMoved(const b2BroadPhase* bp);
+void b2ValidateNoEnlarged(const b2BroadPhase* bp);
 
 static inline b2AABB b2BroadPhase_GetFatAABB(b2BroadPhase* bp, int32_t proxyKey)
 {

src/dynamic_tree.c

@@ -25,7 +25,7 @@ static inline bool b2IsLeaf(const b2TreeNode* node)
 	return node->child1 == B2_NULL_INDEX;
 }
 
-b2DynamicTree b2DynamicTree_Create()
+b2DynamicTree b2DynamicTree_Create(bool isStatic)
 {
 	b2DynamicTree tree;
 	tree.root = B2_NULL_INDEX;
@@ -53,6 +53,8 @@ b2DynamicTree b2DynamicTree_Create()
 	tree.binIndices = NULL;
 	tree.rebuildCapacity = 0;
 
+	tree.isStatic = isStatic;
+
 	return tree;
 }
 
@@ -519,7 +521,7 @@ int32_t b2DynamicTree_CreateProxy(b2DynamicTree* tree, b2AABB aabb, uint32_t cat
 	node->userData = userData;
 	node->categoryBits = categoryBits;
 	node->height = 0;
-	node->moved = true;
+	node->moved = !tree->isStatic;
 	*outFatAABB = node->aabb;
 
 	b2InsertLeaf(tree, proxyId);
@@ -589,6 +591,11 @@ bool b2DynamicTree_MoveProxy(b2DynamicTree* tree, int32_t proxyId, b2AABB aabb,
 
 	b2InsertLeaf(tree, proxyId);
 
+	if (tree->isStatic)
+	{
+		return true;
+	}
+
 	bool alreadyMoved = tree->nodes[proxyId].moved;
 
 	if (alreadyMoved)
@@ -622,7 +629,6 @@ bool b2DynamicTree_EnlargeProxy(b2DynamicTree* tree, int32_t proxyId, b2AABB aab
 	fatAABB.lowerBound = b2Sub(aabb.lowerBound, r);
 	fatAABB.upperBound = b2Add(aabb.upperBound, r);
 	nodes[proxyId].aabb = fatAABB;
-	nodes[proxyId].enlarged = true;
 
 	int32_t parentIndex = nodes[proxyId].parent;
 	while (parentIndex != B2_NULL_INDEX)
@@ -639,12 +645,23 @@ bool b2DynamicTree_EnlargeProxy(b2DynamicTree* tree, int32_t proxyId, b2AABB aab
 
 	while (parentIndex != B2_NULL_INDEX)
 	{
+		//if (nodes[parentIndex].enlarged == true)
+		//{
+		//	// early out because this ancestor was previously ascended and marked as enlarged
+		//	break;
+		//}
+
 		nodes[parentIndex].enlarged = true;
 		parentIndex = nodes[parentIndex].parent;
 	}
 
 	*outFatAABB = fatAABB;
 
+	if (tree->isStatic)
+	{
+		return true;
+	}
+
 	bool alreadyMoved = nodes[proxyId].moved;
 	if (alreadyMoved)
 	{
@@ -745,6 +762,11 @@ static void b2ValidateStructure(const b2DynamicTree* tree, int32_t index)
 	B2_ASSERT(tree->nodes[child1].parent == index);
 	B2_ASSERT(tree->nodes[child2].parent == index);
 
+	if (tree->nodes[child1].enlarged || tree->nodes[child2].enlarged)
+	{
+		B2_ASSERT(node->enlarged == true);
+	}
+
 	b2ValidateStructure(tree, child1);
 	b2ValidateStructure(tree, child2);
 }
@@ -778,15 +800,15 @@ static void b2ValidateMetrics(const b2DynamicTree* tree, int32_t index)
 	height = 1 + B2_MAX(height1, height2);
 	B2_ASSERT(node->height == height);
 
-	//b2AABB aabb = b2AABB_Union(tree->nodes[child1].aabb, tree->nodes[child2].aabb);
+	// b2AABB aabb = b2AABB_Union(tree->nodes[child1].aabb, tree->nodes[child2].aabb);
 
 	B2_ASSERT(b2AABB_Contains(node->aabb, tree->nodes[child1].aabb));
 	B2_ASSERT(b2AABB_Contains(node->aabb, tree->nodes[child2].aabb));
 
-	//B2_ASSERT(aabb.lowerBound.x == node->aabb.lowerBound.x);
-	//B2_ASSERT(aabb.lowerBound.y == node->aabb.lowerBound.y);
-	//B2_ASSERT(aabb.upperBound.x == node->aabb.upperBound.x);
-	//B2_ASSERT(aabb.upperBound.y == node->aabb.upperBound.y);
+	// B2_ASSERT(aabb.lowerBound.x == node->aabb.lowerBound.x);
+	// B2_ASSERT(aabb.lowerBound.y == node->aabb.lowerBound.y);
+	// B2_ASSERT(aabb.upperBound.x == node->aabb.upperBound.x);
+	// B2_ASSERT(aabb.upperBound.y == node->aabb.upperBound.y);
 
 	uint32_t categoryBits = tree->nodes[child1].categoryBits | tree->nodes[child2].categoryBits;
 	B2_ASSERT(node->categoryBits == categoryBits);
@@ -1396,7 +1418,7 @@ static int32_t b2PartitionSAH(int32_t* indices, int32_t* binIndices, b2AABB* box
 		}
 	}
 	B2_ASSERT(i1 == i2);
-	
+
 	if (i1 > 0 && i1 < count)
 	{
 		return i1;
@@ -1639,7 +1661,6 @@ int32_t b2DynamicTree_Rebuild(b2DynamicTree* tree, bool fullBuild)
 
 			// Detach
 			node->parent = B2_NULL_INDEX;
-			node->enlarged = false;
 		}
 		else
 		{
@@ -1671,6 +1692,17 @@ int32_t b2DynamicTree_Rebuild(b2DynamicTree* tree, bool fullBuild)
 		node = nodes + nodeIndex;
 	}
 
+	#if B2_VALIDATE == 1
+	int32_t capacity = tree->nodeCapacity;
+	for (int32_t i = 0; i < capacity; ++i)
+	{
+		if (nodes[i].height >= 0)
+		{
+			B2_ASSERT(nodes[i].enlarged == false);
+		}
+	}
+	#endif
+
 	B2_ASSERT(leafCount <= proxyCount);
 
 	tree->root = b2BuildTree(tree, leafCount);

src/world.c

@@ -353,6 +353,8 @@ static void b2Collide(b2World* world)
 		b2CollideTask(0, awakeContactCount, 0, world);
 	}
 
+	b2ValidateNoEnlarged(&world->broadPhase);
+
 	// Serially update contact state
 	b2TracyCZoneNC(contact_state, "Contact State", b2_colorCoral, true);