implemented kinematic tree and custom filter callback

todo sample with custom filter

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
 )

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.

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())
 	{

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,23 +1115,25 @@ 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
 	{
 		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)

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,34 +34,29 @@ 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();
 	}
 }
 
 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,15 +457,16 @@ 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
 }
 
 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;