From 69b9f55ecea7354ea591a1b3b81213016ac98edb Mon Sep 17 00:00:00 2001 From: Erin Catto Date: Tue, 28 May 2024 17:22:07 -0700 Subject: [PATCH] implemented kinematic tree and custom filter callback todo sample with custom filter --- CMakeLists.txt | 2 +- include/box2d/types.h | 3 ++ samples/main.cpp | 2 +- src/body.c | 19 +++++---- src/broad_phase.c | 91 ++++++++++++++++++++++++------------------- src/broad_phase.h | 48 ++++++----------------- src/joint.c | 2 +- src/shape.c | 20 +++++----- src/shape.h | 2 +- src/solver.c | 38 ++++++++++++------ src/world.c | 49 ++++++++++++++--------- src/world.h | 3 ++ 12 files changed, 150 insertions(+), 129 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 31c21b68..b5f944ee 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -3,7 +3,7 @@ include(FetchContent) project(box2d VERSION 3.0.0 - DESCRIPTION "A 2D physics engine" + DESCRIPTION "A 2D physics engine for games" HOMEPAGE_URL "https://box2d.org" LANGUAGES C CXX ) diff --git a/include/box2d/types.h b/include/box2d/types.h index 532d84d8..accf85ee 100644 --- a/include/box2d/types.h +++ b/include/box2d/types.h @@ -129,6 +129,9 @@ typedef enum b2BodyType /// positive mass, velocity determined by forces, moved by solver b2_dynamicBody = 2, + + /// number of body types + b2_bodyTypeCount, } b2BodyType; /// A body definition holds all the data needed to construct a rigid body. diff --git a/samples/main.cpp b/samples/main.cpp index eb0cecbc..09bbc11d 100644 --- a/samples/main.cpp +++ b/samples/main.cpp @@ -566,7 +566,7 @@ int main(int, char**) glfwWindowHint(GLFW_SAMPLES, 4); b2Version version = b2GetVersion(); - snprintf(buffer, 128, "Box2D Version %d.%d.%d (alpha)", version.major, version.minor, version.revision); + snprintf(buffer, 128, "Box2D Version %d.%d.%d (beta)", version.major, version.minor, version.revision); if (GLFWmonitor* primaryMonitor = glfwGetPrimaryMonitor()) { diff --git a/src/body.c b/src/body.c index bba7b534..56ed35e5 100644 --- a/src/body.c +++ b/src/body.c @@ -725,7 +725,7 @@ void b2Body_SetTransform(b2BodyId bodyId, b2Vec2 position, float angle) // They body could be disabled if (shape->proxyKey != B2_NULL_INDEX) { - b2BroadPhase_MoveProxy(broadPhase, shape->proxyKey, fatAABB, shape->filter.maskBits); + b2BroadPhase_MoveProxy(broadPhase, shape->proxyKey, fatAABB); } } @@ -1040,7 +1040,8 @@ void b2Body_SetType(b2BodyId bodyId, b2BodyType type) shapeId = shape->nextShapeId; b2DestroyShapeProxy(shape, &world->broadPhase); bool forcePairCreation = true; - b2CreateShapeProxy(shape, &world->broadPhase, b2_movableProxy, transform, forcePairCreation); + b2BodyType proxyType = type; + b2CreateShapeProxy(shape, &world->broadPhase, proxyType, transform, forcePairCreation); } } else if (type == b2_staticBody) @@ -1114,7 +1115,7 @@ void b2Body_SetType(b2BodyId bodyId, b2BodyType type) shapeId = shape->nextShapeId; b2DestroyShapeProxy(shape, &world->broadPhase); bool forcePairCreation = true; - b2CreateShapeProxy(shape, &world->broadPhase, b2_staticProxy, transform, forcePairCreation); + b2CreateShapeProxy(shape, &world->broadPhase, b2_staticBody, transform, forcePairCreation); } } else @@ -1122,15 +1123,17 @@ void b2Body_SetType(b2BodyId bodyId, b2BodyType type) B2_ASSERT(originalType == b2_dynamicBody || originalType == b2_kinematicBody); B2_ASSERT(type == b2_dynamicBody || type == b2_kinematicBody); - // Converting between kinematic and dynamic is much simpler - - // Touch the broad-phase proxies to ensure the correct contacts get created + // Recreate shape proxies in static tree. + b2Transform transform = b2GetBodyTransformQuick(world, body); int shapeId = body->headShapeId; while (shapeId != B2_NULL_INDEX) { b2Shape* shape = world->shapeArray + shapeId; - b2BufferMove(&world->broadPhase, (b2MovedProxy){shape->proxyKey, shape->filter.maskBits}); shapeId = shape->nextShapeId; + b2DestroyShapeProxy(shape, &world->broadPhase); + b2BodyType proxyType = type; + bool forcePairCreation = true; + b2CreateShapeProxy(shape, &world->broadPhase, proxyType, transform, forcePairCreation); } } @@ -1513,7 +1516,7 @@ void b2Body_Enable(b2BodyId bodyId) b2Transform transform = b2GetBodyTransformQuick(world, body); // Add shapes to broad-phase - b2ProxyType proxyType = setId == b2_staticSet ? b2_staticProxy : b2_movableProxy; + b2BodyType proxyType = body->type; bool forcePairCreation = true; int shapeId = body->headShapeId; while (shapeId != B2_NULL_INDEX) diff --git a/src/broad_phase.c b/src/broad_phase.c index ece81472..40597bc2 100644 --- a/src/broad_phase.c +++ b/src/broad_phase.c @@ -25,7 +25,7 @@ void b2CreateBroadPhase(b2BroadPhase* bp) { - _Static_assert(b2_proxyTypeCount == 2, "must be only two proxy types"); + _Static_assert(b2_bodyTypeCount == 3, "must be three body types"); // if (s_file == NULL) //{ @@ -34,20 +34,15 @@ void b2CreateBroadPhase(b2BroadPhase* bp) // } bp->proxyCount = 0; - - // TODO_ERIN initial size in b2WorldDef? bp->moveSet = b2CreateSet(16); - bp->moveArray = b2CreateArray(sizeof(b2MovedProxy), 16); - + bp->moveArray = b2CreateArray(sizeof(int), 16); bp->moveResults = NULL; bp->movePairs = NULL; bp->movePairCapacity = 0; bp->movePairIndex = 0; - - // TODO_ERIN initial size from b2WorldDef bp->pairSet = b2CreateSet(32); - for (int i = 0; i < b2_proxyTypeCount; ++i) + for (int i = 0; i < b2_bodyTypeCount; ++i) { bp->trees[i] = b2DynamicTree_Create(); } @@ -55,13 +50,13 @@ void b2CreateBroadPhase(b2BroadPhase* bp) void b2DestroyBroadPhase(b2BroadPhase* bp) { - for (int i = 0; i < b2_proxyTypeCount; ++i) + for (int i = 0; i < b2_bodyTypeCount; ++i) { b2DynamicTree_Destroy(bp->trees + i); } b2DestroySet(&bp->moveSet); - b2DestroyArray(bp->moveArray, sizeof(b2MovedProxy)); + b2DestroyArray(bp->moveArray, sizeof(int)); b2DestroySet(&bp->pairSet); memset(bp, 0, sizeof(b2BroadPhase)); @@ -84,7 +79,7 @@ static inline void b2UnBufferMove(b2BroadPhase* bp, int proxyKey) int count = b2Array(bp->moveArray).count; for (int i = 0; i < count; ++i) { - if (bp->moveArray[i].proxyKey == proxyKey) + if (bp->moveArray[i] == proxyKey) { b2Array_RemoveSwap(bp->moveArray, i); break; @@ -93,15 +88,15 @@ static inline void b2UnBufferMove(b2BroadPhase* bp, int proxyKey) } } -int b2BroadPhase_CreateProxy(b2BroadPhase* bp, b2ProxyType proxyType, b2AABB aabb, uint32_t categoryBits, uint32_t maskBits, int shapeIndex, +int b2BroadPhase_CreateProxy(b2BroadPhase* bp, b2BodyType proxyType, b2AABB aabb, uint32_t categoryBits, int shapeIndex, bool forcePairCreation) { - B2_ASSERT(0 <= proxyType && proxyType < b2_proxyTypeCount); + B2_ASSERT(0 <= proxyType && proxyType < b2_bodyTypeCount); int proxyId = b2DynamicTree_CreateProxy(bp->trees + proxyType, aabb, categoryBits, shapeIndex); int proxyKey = B2_PROXY_KEY(proxyId, proxyType); - if (proxyType != b2_staticProxy || forcePairCreation) + if (proxyType != b2_staticBody || forcePairCreation) { - b2BufferMove(bp, (b2MovedProxy){proxyKey, maskBits}); + b2BufferMove(bp, proxyKey); } return proxyKey; } @@ -113,32 +108,32 @@ void b2BroadPhase_DestroyProxy(b2BroadPhase* bp, int proxyKey) --bp->proxyCount; - b2ProxyType proxyType = B2_PROXY_TYPE(proxyKey); + b2BodyType proxyType = B2_PROXY_TYPE(proxyKey); int proxyId = B2_PROXY_ID(proxyKey); - B2_ASSERT(0 <= proxyType && proxyType <= b2_proxyTypeCount); + B2_ASSERT(0 <= proxyType && proxyType <= b2_bodyTypeCount); b2DynamicTree_DestroyProxy(bp->trees + proxyType, proxyId); } -void b2BroadPhase_MoveProxy(b2BroadPhase* bp, int proxyKey, b2AABB aabb, uint32_t maskBits) +void b2BroadPhase_MoveProxy(b2BroadPhase* bp, int proxyKey, b2AABB aabb) { - b2ProxyType proxyType = B2_PROXY_TYPE(proxyKey); + b2BodyType proxyType = B2_PROXY_TYPE(proxyKey); int proxyId = B2_PROXY_ID(proxyKey); b2DynamicTree_MoveProxy(bp->trees + proxyType, proxyId, aabb); - b2BufferMove(bp, (b2MovedProxy){proxyKey, maskBits}); + b2BufferMove(bp, proxyKey); } -void b2BroadPhase_EnlargeProxy(b2BroadPhase* bp, int proxyKey, b2AABB aabb, uint32_t maskBits) +void b2BroadPhase_EnlargeProxy(b2BroadPhase* bp, int proxyKey, b2AABB aabb) { B2_ASSERT(proxyKey != B2_NULL_INDEX); int typeIndex = B2_PROXY_TYPE(proxyKey); int proxyId = B2_PROXY_ID(proxyKey); - B2_ASSERT(typeIndex == b2_movableProxy); + B2_ASSERT(typeIndex != b2_staticBody); b2DynamicTree_EnlargeProxy(bp->trees + typeIndex, proxyId, aabb); - b2BufferMove(bp, (b2MovedProxy){proxyKey, maskBits}); + b2BufferMove(bp, proxyKey); } typedef struct b2MovePair @@ -158,7 +153,7 @@ typedef struct b2QueryPairContext { b2World* world; b2MoveResult* moveResult; - b2ProxyType queryTreeType; + b2BodyType queryTreeType; int queryProxyKey; int queryShapeIndex; } b2QueryPairContext; @@ -178,7 +173,7 @@ static bool b2PairQueryCallback(int proxyId, int shapeId, void* context) } // Is this proxy also moving? - if (queryContext->queryTreeType == b2_movableProxy) + if (queryContext->queryTreeType != b2_staticBody) { bool moved = b2ContainsKey(&bp->moveSet, proxyKey + 1); if (moved && proxyKey < queryContext->queryProxyKey) @@ -237,6 +232,19 @@ static bool b2PairQueryCallback(int proxyId, int shapeId, void* context) return true; } + // Custom user filter + b2CustomFilterFcn* customFilterFcn = queryContext->world->customFilterFcn; + if (customFilterFcn != NULL) + { + b2ShapeId idA = {shapeIdA + 1, world->worldId, shapeA->revision}; + b2ShapeId idB = {shapeIdB + 1, world->worldId, shapeB->revision}; + bool shouldCollide = customFilterFcn(idA, idB, queryContext->world->customFilterContext); + if (shouldCollide == false) + { + return true; + } + } + // #todo per thread to eliminate atomic? int pairIndex = atomic_fetch_add(&bp->movePairIndex, 1); @@ -279,18 +287,14 @@ void b2FindPairsTask(int startIndex, int endIndex, uint32_t threadIndex, void* c queryContext.moveResult = bp->moveResults + i; queryContext.moveResult->pairList = NULL; - int proxyKey = bp->moveArray[i].proxyKey; + int proxyKey = bp->moveArray[i]; if (proxyKey == B2_NULL_INDEX) { // proxy was destroyed after it moved continue; } - b2ProxyType proxyType = B2_PROXY_TYPE(proxyKey); - uint32_t maskBits = bp->moveArray[i].maskBits; - - // todo moving this choice to a higher level - //B2_ASSERT(proxyType == b2_movableProxy); + b2BodyType proxyType = B2_PROXY_TYPE(proxyKey); int proxyId = B2_PROXY_ID(proxyKey); queryContext.queryProxyKey = proxyKey; @@ -302,14 +306,19 @@ void b2FindPairsTask(int startIndex, int endIndex, uint32_t threadIndex, void* c b2AABB fatAABB = b2DynamicTree_GetAABB(baseTree, proxyId); queryContext.queryShapeIndex = b2DynamicTree_GetUserData(baseTree, proxyId); - // Query trees - if (proxyType == b2_movableProxy) + // Query trees. Only dynamic proxies collide with kinematic and static proxies + if (proxyType == b2_dynamicBody) { - queryContext.queryTreeType = b2_staticProxy; - b2DynamicTree_Query(bp->trees + b2_staticProxy, fatAABB, maskBits, b2PairQueryCallback, &queryContext); + queryContext.queryTreeType = b2_kinematicBody; + b2DynamicTree_Query(bp->trees + b2_kinematicBody, fatAABB, b2_defaultMaskBits, b2PairQueryCallback, &queryContext); + + queryContext.queryTreeType = b2_staticBody; + b2DynamicTree_Query(bp->trees + b2_staticBody, fatAABB, b2_defaultMaskBits, b2PairQueryCallback, &queryContext); } - queryContext.queryTreeType = b2_movableProxy; - b2DynamicTree_Query(bp->trees + b2_movableProxy, fatAABB, maskBits, b2PairQueryCallback, &queryContext); + + // All proxies collide with dynamic proxies + queryContext.queryTreeType = b2_dynamicBody; + b2DynamicTree_Query(bp->trees + b2_dynamicBody, fatAABB, b2_defaultMaskBits, b2PairQueryCallback, &queryContext); } b2TracyCZoneEnd(pair_task); @@ -434,7 +443,8 @@ bool b2BroadPhase_TestOverlap(const b2BroadPhase* bp, int proxyKeyA, int proxyKe void b2BroadPhase_RebuildTrees(b2BroadPhase* bp) { - b2DynamicTree_Rebuild(bp->trees + b2_movableProxy, false); + b2DynamicTree_Rebuild(bp->trees + b2_dynamicBody, false); + b2DynamicTree_Rebuild(bp->trees + b2_kinematicBody, false); } int b2BroadPhase_GetShapeIndex(b2BroadPhase* bp, int proxyKey) @@ -447,7 +457,8 @@ int b2BroadPhase_GetShapeIndex(b2BroadPhase* bp, int proxyKey) void b2ValidateBroadphase(const b2BroadPhase* bp) { - b2DynamicTree_Validate(bp->trees + b2_movableProxy); + b2DynamicTree_Validate(bp->trees + b2_dynamicBody); + b2DynamicTree_Validate(bp->trees + b2_kinematicBody); // TODO_ERIN validate every shape AABB is contained in tree AABB } @@ -455,7 +466,7 @@ void b2ValidateBroadphase(const b2BroadPhase* bp) void b2ValidateNoEnlarged(const b2BroadPhase* bp) { #if B2_VALIDATE == 1 - for (int j = 0; j < b2_proxyTypeCount; ++j) + for (int j = 0; j < b2_bodyTypeCount; ++j) { const b2DynamicTree* tree = bp->trees + j; int capacity = tree->nodeCapacity; diff --git a/src/broad_phase.h b/src/broad_phase.h index 065ce93a..4444458b 100644 --- a/src/broad_phase.h +++ b/src/broad_phase.h @@ -15,31 +15,17 @@ typedef struct b2MoveResult b2MoveResult; typedef struct b2StackAllocator b2StackAllocator; typedef struct b2World b2World; -// todo kinematic tree to support large kinematic compounds -typedef enum b2ProxyType -{ - b2_staticProxy = 0, - b2_movableProxy = 1, - b2_proxyTypeCount = 2 -} b2ProxyType; - -// Store the proxy type in the lower 1 bit of the proxy key. This leaves 31 bits for the id. -#define B2_PROXY_TYPE(KEY) ((b2ProxyType)((KEY) & 1)) -#define B2_PROXY_ID(KEY) ((KEY) >> 1) -#define B2_PROXY_KEY(ID, TYPE) (((ID) << 1) | (TYPE)) - -typedef struct b2MovedProxy -{ - int proxyKey; - uint32_t maskBits; -} b2MovedProxy; +// Store the proxy type in the lower 2 bits of the proxy key. This leaves 30 bits for the id. +#define B2_PROXY_TYPE(KEY) ((b2BodyType)((KEY) & 3)) +#define B2_PROXY_ID(KEY) ((KEY) >> 2) +#define B2_PROXY_KEY(ID, TYPE) (((ID) << 2) | (TYPE)) /// 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. typedef struct b2BroadPhase { - b2DynamicTree trees[b2_proxyTypeCount]; + b2DynamicTree trees[b2_bodyTypeCount]; int proxyCount; // The move set and array are used to track shapes that have moved significantly @@ -48,7 +34,7 @@ typedef struct b2BroadPhase // todo implement a 32bit hash set for faster lookup // todo moveSet can grow quite large on the first time step and remain large b2HashSet moveSet; - b2MovedProxy* moveArray; + int* moveArray; // These are the results from the pair query and are used to create new contacts // in deterministic order. @@ -67,12 +53,11 @@ typedef struct b2BroadPhase void b2CreateBroadPhase(b2BroadPhase* bp); void b2DestroyBroadPhase(b2BroadPhase* bp); -// todo get rid of mask bits for proxies because this breaks groups -int b2BroadPhase_CreateProxy(b2BroadPhase* bp, b2ProxyType proxyType, b2AABB aabb, uint32_t categoryBits, uint32_t maskBits, int shapeIndex, bool forcePairCreation); +int b2BroadPhase_CreateProxy(b2BroadPhase* bp, b2BodyType proxyType, b2AABB aabb, uint32_t categoryBits, int shapeIndex, bool forcePairCreation); void b2BroadPhase_DestroyProxy(b2BroadPhase* bp, int proxyKey); -void b2BroadPhase_MoveProxy(b2BroadPhase* bp, int proxyKey, b2AABB aabb, uint32_t maskBits); -void b2BroadPhase_EnlargeProxy(b2BroadPhase* bp, int proxyKey, b2AABB aabb, uint32_t maskBits); +void b2BroadPhase_MoveProxy(b2BroadPhase* bp, int proxyKey, b2AABB aabb); +void b2BroadPhase_EnlargeProxy(b2BroadPhase* bp, int proxyKey, b2AABB aabb); void b2BroadPhase_RebuildTrees(b2BroadPhase* bp); @@ -86,21 +71,10 @@ void b2ValidateNoEnlarged(const b2BroadPhase* bp); // This is what triggers new contact pairs to be created // Warning: this must be called in deterministic order -static inline void b2BufferMove(b2BroadPhase* bp, b2MovedProxy queryProxy) +static inline void b2BufferMove(b2BroadPhase* bp, int queryProxy) { - // todo moving this choice to a higher level - // Why only mobile proxies? Because we need to be able insert a large number of static shapes - // without triggering a large number of pair updates that do nothing. - // If you need to spawn non-moving shapes close to dynamic bodies then use kinematic bodies. - //b2ProxyType proxyType = B2_PROXY_TYPE(proxyKey); - //if (proxyType != b2_movableProxy) - //{ - // B2_ASSERT(false); - // return; - //} - // Adding 1 because 0 is the sentinel - bool alreadyAdded = b2AddKey(&bp->moveSet, queryProxy.proxyKey + 1); + bool alreadyAdded = b2AddKey(&bp->moveSet, queryProxy + 1); if (alreadyAdded == false) { b2Array_Push(bp->moveArray, queryProxy); diff --git a/src/joint.c b/src/joint.c index 675a8dea..00cae634 100644 --- a/src/joint.c +++ b/src/joint.c @@ -837,7 +837,7 @@ void b2Joint_SetCollideConnected(b2JointId jointId, bool shouldCollide) if (shape->proxyKey != B2_NULL_INDEX) { - b2BufferMove(&world->broadPhase, (b2MovedProxy){shape->proxyKey, shape->filter.maskBits}); + b2BufferMove(&world->broadPhase, shape->proxyKey); } shapeId = shape->nextShapeId; diff --git a/src/shape.c b/src/shape.c index c9df7688..a2a54856 100644 --- a/src/shape.c +++ b/src/shape.c @@ -35,7 +35,7 @@ static b2ChainShape* b2GetChainShape(b2World* world, b2ChainId chainId) return chain; } -static void b2UpdateShapeAABBs(b2Shape* shape, b2Transform transform, b2ProxyType proxyType) +static void b2UpdateShapeAABBs(b2Shape* shape, b2Transform transform, b2BodyType proxyType) { // Compute a bounding box with a speculative margin const float speculativeDistance = b2_speculativeDistance; @@ -49,7 +49,7 @@ static void b2UpdateShapeAABBs(b2Shape* shape, b2Transform transform, b2ProxyTyp shape->aabb = aabb; // Smaller margin for static bodies. Cannot be zero due to TOI tolerance. - float margin = proxyType == b2_staticProxy ? speculativeDistance : aabbMargin; + float margin = proxyType == b2_staticBody ? speculativeDistance : aabbMargin; b2AABB fatAABB; fatAABB.lowerBound.x = aabb.lowerBound.x - margin; fatAABB.lowerBound.y = aabb.lowerBound.y - margin; @@ -129,7 +129,7 @@ static b2Shape* b2CreateShapeInternal(b2World* world, b2Body* body, b2Transform if (body->setIndex != b2_disabledSet) { - b2ProxyType proxyType = body->setIndex == b2_staticSet ? b2_staticProxy : b2_movableProxy; + b2BodyType proxyType = body->type; b2CreateShapeProxy(shape, &world->broadPhase, proxyType, transform, def->forceContactCreation); } @@ -687,15 +687,15 @@ b2CastOutput b2ShapeCastShape(const b2ShapeCastInput* input, const b2Shape* shap return output; } -void b2CreateShapeProxy(b2Shape* shape, b2BroadPhase* bp, b2ProxyType type, b2Transform transform, bool forcePairCreation) +void b2CreateShapeProxy(b2Shape* shape, b2BroadPhase* bp, b2BodyType type, b2Transform transform, bool forcePairCreation) { B2_ASSERT(shape->proxyKey == B2_NULL_INDEX); b2UpdateShapeAABBs(shape, transform, type); // Create proxies in the broad-phase. - shape->proxyKey = b2BroadPhase_CreateProxy(bp, type, shape->fatAABB, shape->filter.categoryBits, shape->filter.maskBits, shape->id, forcePairCreation); - B2_ASSERT(B2_PROXY_TYPE(shape->proxyKey) < b2_proxyTypeCount); + shape->proxyKey = b2BroadPhase_CreateProxy(bp, type, shape->fatAABB, shape->filter.categoryBits, shape->id, forcePairCreation); + B2_ASSERT(B2_PROXY_TYPE(shape->proxyKey) < b2_bodyTypeCount); } void b2DestroyShapeProxy(b2Shape* shape, b2BroadPhase* bp) @@ -936,7 +936,7 @@ static void b2ResetProxy(b2World* world, b2Shape* shape, bool wakeBodies, bool d b2Transform transform = b2GetBodyTransformQuick(world, body); if (shape->proxyKey != B2_NULL_INDEX) { - b2ProxyType proxyType = B2_PROXY_TYPE(shape->proxyKey); + b2BodyType proxyType = B2_PROXY_TYPE(shape->proxyKey); b2UpdateShapeAABBs(shape, transform, proxyType); if (destroyProxy) @@ -945,16 +945,16 @@ static void b2ResetProxy(b2World* world, b2Shape* shape, bool wakeBodies, bool d bool forcePairCreation = true; shape->proxyKey = b2BroadPhase_CreateProxy(&world->broadPhase, proxyType, shape->fatAABB, shape->filter.categoryBits, - shape->filter.maskBits, shapeId, forcePairCreation); + shapeId, forcePairCreation); } else { - b2BroadPhase_MoveProxy(&world->broadPhase, shape->proxyKey, shape->fatAABB, shape->filter.maskBits); + b2BroadPhase_MoveProxy(&world->broadPhase, shape->proxyKey, shape->fatAABB); } } else { - b2ProxyType proxyType = body->type == b2_staticBody ? b2_staticProxy : b2_movableProxy; + b2BodyType proxyType = body->type; b2UpdateShapeAABBs(shape, transform, proxyType); } diff --git a/src/shape.h b/src/shape.h index 6eedb41d..92eeaccb 100644 --- a/src/shape.h +++ b/src/shape.h @@ -64,7 +64,7 @@ typedef struct b2ShapeExtent float maxExtent; } b2ShapeExtent; -void b2CreateShapeProxy(b2Shape* shape, b2BroadPhase* bp, b2ProxyType type, b2Transform transform, bool forcePairCreation); +void b2CreateShapeProxy(b2Shape* shape, b2BroadPhase* bp, b2BodyType type, b2Transform transform, bool forcePairCreation); void b2DestroyShapeProxy(b2Shape* shape, b2BroadPhase* bp); b2MassData b2ComputeShapeMass(const b2Shape* shape); diff --git a/src/solver.c b/src/solver.c index 99fecc6b..3a1444a3 100644 --- a/src/solver.c +++ b/src/solver.c @@ -839,7 +839,6 @@ static bool b2ContinuousQueryCallback(int proxyId, int shapeId, void* context) } // Skip filtered bodies - b2CheckIndex(world->bodyArray, fastBodySim->bodyId); b2Body* fastBody = world->bodyArray + fastBodySim->bodyId; canCollide = b2ShouldBodiesCollide(world, fastBody, body); @@ -848,6 +847,19 @@ static bool b2ContinuousQueryCallback(int proxyId, int shapeId, void* context) return true; } + // Custom user filtering + b2CustomFilterFcn* customFilterFcn = world->customFilterFcn; + if (customFilterFcn != NULL) + { + b2ShapeId idA = {shape->id + 1, world->worldId, shape->revision}; + b2ShapeId idB = {fastShape->id + 1, world->worldId, fastShape->revision}; + canCollide = customFilterFcn(idA, idB, world->customFilterContext); + if (canCollide == false) + { + return true; + } + } + // Prevent pausing on smooth segment junctions if (shape->type == b2_smoothSegmentShape) { @@ -914,8 +926,9 @@ static void b2SolveContinuous(b2World* world, int bodySimIndex) xf2.q = sweep.q2; xf2.p = b2Sub(sweep.c2, b2RotateVector(sweep.q2, sweep.localCenter)); - b2DynamicTree* staticTree = world->broadPhase.trees + b2_staticProxy; - b2DynamicTree* movableTree = world->broadPhase.trees + b2_movableProxy; + b2DynamicTree* staticTree = world->broadPhase.trees + b2_staticBody; + b2DynamicTree* kinematicTree = world->broadPhase.trees + b2_kinematicBody; + b2DynamicTree* dynamicTree = world->broadPhase.trees + b2_dynamicBody; struct b2ContinuousContext context; context.world = world; @@ -949,11 +962,12 @@ static void b2SolveContinuous(b2World* world, int bodySimIndex) // Store this for later fastShape->aabb = box2; - b2DynamicTree_Query(staticTree, box, fastShape->filter.maskBits, b2ContinuousQueryCallback, &context); + b2DynamicTree_Query(staticTree, box, b2_defaultMaskBits, b2ContinuousQueryCallback, &context); if (isBullet) { - b2DynamicTree_Query(movableTree, box, fastShape->filter.maskBits, b2ContinuousQueryCallback, &context); + b2DynamicTree_Query(kinematicTree, box, b2_defaultMaskBits, b2ContinuousQueryCallback, &context); + b2DynamicTree_Query(dynamicTree, box, b2_defaultMaskBits, b2ContinuousQueryCallback, &context); } shapeId = fastShape->nextShapeId; @@ -1752,13 +1766,13 @@ void b2Solve(b2World* world, b2StepContext* stepContext) { B2_ASSERT(shape->isFast == false); - b2BroadPhase_EnlargeProxy(broadPhase, shape->proxyKey, shape->fatAABB, shape->filter.maskBits); + b2BroadPhase_EnlargeProxy(broadPhase, shape->proxyKey, shape->fatAABB); shape->enlargedAABB = false; } else if (shape->isFast) { // Shape is fast. It's aabb will be enlarged in continuous collision. - b2BufferMove(broadPhase, (b2MovedProxy){shape->proxyKey, shape->filter.maskBits}); + b2BufferMove(broadPhase, shape->proxyKey); } shapeId = shape->nextShapeId; @@ -1808,7 +1822,7 @@ void b2Solve(b2World* world, b2StepContext* stepContext) // Serially enlarge broad-phase proxies for fast shapes { b2BroadPhase* broadPhase = &world->broadPhase; - b2DynamicTree* movableTree = broadPhase->trees + b2_movableProxy; + b2DynamicTree* dynamicTree = broadPhase->trees + b2_dynamicBody; b2Body* bodies = world->bodyArray; b2Shape* shapes = world->shapeArray; @@ -1846,12 +1860,12 @@ void b2Solve(b2World* world, b2StepContext* stepContext) int proxyKey = shape->proxyKey; int proxyId = B2_PROXY_ID(proxyKey); - B2_ASSERT(B2_PROXY_TYPE(proxyKey) == b2_movableProxy); + B2_ASSERT(B2_PROXY_TYPE(proxyKey) == b2_dynamicBody); // all fast shapes should already be in the move buffer B2_ASSERT(b2ContainsKey(&broadPhase->moveSet, proxyKey + 1)); - b2DynamicTree_EnlargeProxy(movableTree, proxyId, shape->fatAABB); + b2DynamicTree_EnlargeProxy(dynamicTree, proxyId, shape->fatAABB); shapeId = shape->nextShapeId; } @@ -1892,12 +1906,12 @@ void b2Solve(b2World* world, b2StepContext* stepContext) int proxyKey = shape->proxyKey; int proxyId = B2_PROXY_ID(proxyKey); - B2_ASSERT(B2_PROXY_TYPE(proxyKey) == b2_movableProxy); + B2_ASSERT(B2_PROXY_TYPE(proxyKey) == b2_dynamicBody); // all fast shapes should already be in the move buffer B2_ASSERT(b2ContainsKey(&broadPhase->moveSet, proxyKey + 1)); - b2DynamicTree_EnlargeProxy(movableTree, proxyId, shape->fatAABB); + b2DynamicTree_EnlargeProxy(dynamicTree, proxyId, shape->fatAABB); shapeId = shape->nextShapeId; } diff --git a/src/world.c b/src/world.c index c657610e..69342e80 100644 --- a/src/world.c +++ b/src/world.c @@ -100,9 +100,12 @@ b2WorldId b2CreateWorld(const b2WorldDef* def) b2InitializeContactRegisters(); b2World* world = b2_worlds + worldId; + uint16_t revision = world->revision; + *world = (b2World){0}; world->worldId = (uint16_t)worldId; + world->revision = revision; world->inUse = true; world->stackAllocator = b2CreateStackAllocator(2048); @@ -936,7 +939,7 @@ static void b2DrawWithBounds(b2World* world, b2DebugDraw* draw) struct DrawContext drawContext = {world, draw}; - for (int i = 0; i < b2_proxyTypeCount; ++i) + for (int i = 0; i < b2_bodyTypeCount; ++i) { b2DynamicTree_Query(world->broadPhase.trees + i, draw->drawingBounds, b2_defaultMaskBits, DrawQueryCallback, &drawContext); } @@ -1655,11 +1658,12 @@ b2Counters b2World_GetCounters(b2WorldId worldId) s.jointCount = b2GetIdCount(&world->jointIdPool); s.islandCount = b2GetIdCount(&world->islandIdPool); - b2DynamicTree* staticTree = world->broadPhase.trees + b2_staticProxy; + b2DynamicTree* staticTree = world->broadPhase.trees + b2_staticBody; s.staticTreeHeight = b2DynamicTree_GetHeight(staticTree); - b2DynamicTree* tree = world->broadPhase.trees + b2_movableProxy; - s.treeHeight = b2DynamicTree_GetHeight(tree); + b2DynamicTree* dynamicTree = world->broadPhase.trees + b2_dynamicBody; + b2DynamicTree* kinematicTree = world->broadPhase.trees + b2_kinematicBody; + s.treeHeight = b2MaxInt(b2DynamicTree_GetHeight(dynamicTree), b2DynamicTree_GetHeight(kinematicTree)); s.stackUsed = b2GetMaxStackAllocation(&world->stackAllocator); s.byteCount = b2GetByteCount(); @@ -1706,8 +1710,9 @@ void b2World_DumpMemoryStats(b2WorldId worldId) // broad-phase fprintf(file, "broad-phase\n"); - fprintf(file, "static tree: %d\n", b2DynamicTree_GetByteCount(world->broadPhase.trees + b2_staticProxy)); - fprintf(file, "movable tree: %d\n", b2DynamicTree_GetByteCount(world->broadPhase.trees + b2_movableProxy)); + fprintf(file, "static tree: %d\n", b2DynamicTree_GetByteCount(world->broadPhase.trees + b2_staticBody)); + fprintf(file, "kinematic tree: %d\n", b2DynamicTree_GetByteCount(world->broadPhase.trees + b2_kinematicBody)); + fprintf(file, "dynamic tree: %d\n", b2DynamicTree_GetByteCount(world->broadPhase.trees + b2_dynamicBody)); b2HashSet* moveSet = &world->broadPhase.moveSet; fprintf(file, "moveSet: %d (%d, %d)\n", b2GetHashSetBytes(moveSet), moveSet->count, moveSet->capacity); fprintf(file, "moveArray: %d\n", b2GetArrayBytes(world->broadPhase.moveArray, sizeof(int))); @@ -1816,7 +1821,7 @@ void b2World_OverlapAABB(b2WorldId worldId, b2AABB aabb, b2QueryFilter filter, b WorldQueryContext worldContext = {world, fcn, filter, context}; - for (int i = 0; i < b2_proxyTypeCount; ++i) + for (int i = 0; i < b2_bodyTypeCount; ++i) { b2DynamicTree_Query(world->broadPhase.trees + i, aabb, filter.maskBits, TreeQueryCallback, &worldContext); } @@ -1891,7 +1896,7 @@ void b2World_OverlapCircle(b2WorldId worldId, const b2Circle* circle, b2Transfor world, fcn, filter, b2MakeProxy(&circle->center, 1, circle->radius), transform, context, }; - for (int i = 0; i < b2_proxyTypeCount; ++i) + for (int i = 0; i < b2_bodyTypeCount; ++i) { b2DynamicTree_Query(world->broadPhase.trees + i, aabb, filter.maskBits, TreeOverlapCallback, &worldContext); } @@ -1915,7 +1920,7 @@ void b2World_OverlapCapsule(b2WorldId worldId, const b2Capsule* capsule, b2Trans world, fcn, filter, b2MakeProxy(&capsule->center1, 2, capsule->radius), transform, context, }; - for (int i = 0; i < b2_proxyTypeCount; ++i) + for (int i = 0; i < b2_bodyTypeCount; ++i) { b2DynamicTree_Query(world->broadPhase.trees + i, aabb, filter.maskBits, TreeOverlapCallback, &worldContext); } @@ -1939,7 +1944,7 @@ void b2World_OverlapPolygon(b2WorldId worldId, const b2Polygon* polygon, b2Trans world, fcn, filter, b2MakeProxy(polygon->vertices, polygon->count, polygon->radius), transform, context, }; - for (int i = 0; i < b2_proxyTypeCount; ++i) + for (int i = 0; i < b2_bodyTypeCount; ++i) { b2DynamicTree_Query(world->broadPhase.trees + i, aabb, filter.maskBits, TreeOverlapCallback, &worldContext); } @@ -2003,7 +2008,7 @@ void b2World_CastRay(b2WorldId worldId, b2Vec2 origin, b2Vec2 translation, b2Que WorldRayCastContext worldContext = {world, fcn, filter, 1.0f, context}; - for (int i = 0; i < b2_proxyTypeCount; ++i) + for (int i = 0; i < b2_bodyTypeCount; ++i) { b2DynamicTree_RayCast(world->broadPhase.trees + i, &input, filter.maskBits, RayCastCallback, &worldContext); @@ -2045,7 +2050,7 @@ b2RayResult b2World_CastRayClosest(b2WorldId worldId, b2Vec2 origin, b2Vec2 tran b2RayCastInput input = {origin, translation, 1.0f}; WorldRayCastContext worldContext = {world, b2RayCastClosestFcn, filter, 1.0f, &result}; - for (int i = 0; i < b2_proxyTypeCount; ++i) + for (int i = 0; i < b2_bodyTypeCount; ++i) { b2DynamicTree_RayCast(world->broadPhase.trees + i, &input, filter.maskBits, RayCastCallback, &worldContext); @@ -2115,7 +2120,7 @@ void b2World_CastCircle(b2WorldId worldId, const b2Circle* circle, b2Transform o WorldRayCastContext worldContext = {world, fcn, filter, 1.0f, context}; - for (int i = 0; i < b2_proxyTypeCount; ++i) + for (int i = 0; i < b2_bodyTypeCount; ++i) { b2DynamicTree_ShapeCast(world->broadPhase.trees + i, &input, filter.maskBits, ShapeCastCallback, &worldContext); @@ -2152,7 +2157,7 @@ void b2World_CastCapsule(b2WorldId worldId, const b2Capsule* capsule, b2Transfor WorldRayCastContext worldContext = {world, fcn, filter, 1.0f, context}; - for (int i = 0; i < b2_proxyTypeCount; ++i) + for (int i = 0; i < b2_bodyTypeCount; ++i) { b2DynamicTree_ShapeCast(world->broadPhase.trees + i, &input, filter.maskBits, ShapeCastCallback, &worldContext); @@ -2191,7 +2196,7 @@ void b2World_CastPolygon(b2WorldId worldId, const b2Polygon* polygon, b2Transfor WorldRayCastContext worldContext = {world, fcn, filter, 1.0f, context}; - for (int i = 0; i < b2_proxyTypeCount; ++i) + for (int i = 0; i < b2_bodyTypeCount; ++i) { b2DynamicTree_ShapeCast(world->broadPhase.trees + i, &input, filter.maskBits, ShapeCastCallback, &worldContext); @@ -2294,6 +2299,13 @@ void b2World_Dump() } #endif +void b2World_SetCustomFilterCallback(b2WorldId worldId, b2CustomFilterFcn* fcn, void* context) +{ + b2World* world = b2GetWorldFromId(worldId); + world->customFilterFcn = fcn; + world->customFilterContext = context; +} + void b2World_SetPreSolveCallback(b2WorldId worldId, b2PreSolveFcn* fcn, void* context) { b2World* world = b2GetWorldFromId(worldId); @@ -2409,7 +2421,7 @@ void b2World_Explode(b2WorldId worldId, b2Vec2 position, float radius, float mag aabb.upperBound.x = position.x + radius; aabb.upperBound.y = position.y + radius; - b2DynamicTree_Query(world->broadPhase.trees + b2_movableProxy, aabb, b2_defaultMaskBits, ExplosionCallback, &explosionContext); + b2DynamicTree_Query(world->broadPhase.trees + b2_dynamicBody, aabb, b2_defaultMaskBits, ExplosionCallback, &explosionContext); } #if B2_VALIDATE @@ -2603,11 +2615,12 @@ void b2ValidateSolverSets(b2World* world) } else if (setIndex == b2_staticSet) { - B2_ASSERT(B2_PROXY_TYPE(shape->proxyKey) == b2_staticProxy); + B2_ASSERT(B2_PROXY_TYPE(shape->proxyKey) == b2_staticBody); } else { - B2_ASSERT(B2_PROXY_TYPE(shape->proxyKey) == b2_movableProxy); + b2BodyType proxyType = B2_PROXY_TYPE(shape->proxyKey); + B2_ASSERT(proxyType == b2_kinematicBody || proxyType == b2_dynamicBody); } prevShapeId = shapeId; diff --git a/src/world.h b/src/world.h index b62e7f18..1a39a7b2 100644 --- a/src/world.h +++ b/src/world.h @@ -138,6 +138,9 @@ typedef struct b2World b2PreSolveFcn* preSolveFcn; void* preSolveContext; + b2CustomFilterFcn* customFilterFcn; + void* customFilterContext; + int workerCount; b2EnqueueTaskCallback* enqueueTaskFcn; b2FinishTaskCallback* finishTaskFcn;