From 71da0e2ace6148517590eb74bf82468ba5b45e37 Mon Sep 17 00:00:00 2001 From: Erin Catto Date: Sun, 23 Jul 2023 16:01:18 -0700 Subject: [PATCH] validation and bug fixing --- include/box2d/dynamic_tree.h | 8 ++- samples/collection/sample_dynamic_tree.cpp | 3 +- src/broad_phase.c | 68 +++++++++++++++++++++- src/broad_phase.h | 2 + src/dynamic_tree.c | 52 +++++++++++++---- src/world.c | 2 + 6 files changed, 120 insertions(+), 15 deletions(-) diff --git a/include/box2d/dynamic_tree.h b/include/box2d/dynamic_tree.h index 019d163f..b9f227bf 100644 --- a/include/box2d/dynamic_tree.h +++ b/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. diff --git a/samples/collection/sample_dynamic_tree.cpp b/samples/collection/sample_dynamic_tree.cpp index e4691377..d9bc232a 100644 --- a/samples/collection/sample_dynamic_tree.cpp +++ b/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) { diff --git a/src/broad_phase.c b/src/broad_phase.c index f5543a41..aa159395 100644 --- a/src/broad_phase.c +++ b/src/broad_phase.c @@ -16,6 +16,7 @@ #include "box2d/timer.h" #include +#include #include 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 +} diff --git a/src/broad_phase.h b/src/broad_phase.h index c315e650..bda1bcac 100644 --- a/src/broad_phase.h +++ b/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) { diff --git a/src/dynamic_tree.c b/src/dynamic_tree.c index 575c1c2e..77c079c1 100644 --- a/src/dynamic_tree.c +++ b/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); diff --git a/src/world.c b/src/world.c index 6ca63c36..8647eb77 100644 --- a/src/world.c +++ b/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);