wip

include/box2d/callbacks.h

@@ -48,7 +48,7 @@ typedef void b2EndContactFcn(b2ShapeId shapeIdA, b2ShapeId shapeIdB, void* conte
 /// get an EndContact callback. However, you may get a BeginContact callback
 /// the next step.
 /// - the supplied manifold has impulse values from the previous frame
-typedef bool b2PreSolveFcn(b2ShapeId shapeIdA, b2ShapeId shapeIdB, b2Manifold* manifold, void* context);
+typedef bool b2PreSolveFcn(b2ShapeId shapeIdA, b2ShapeId shapeIdB, b2Manifold* manifold, int32_t color, void* context);
 BOX2D_API void b2World_SetPreSolveCallback(b2WorldId worldId, b2PreSolveFcn* fcn, void* context);
 
 /// This lets you inspect a contact after the solver is finished. This is useful

include/box2d/manifold.h

@@ -41,7 +41,7 @@ typedef struct b2ManifoldPoint
 	bool persisted;
 } b2ManifoldPoint;
 
-/// Conact manifold convex shapes.
+/// Contact manifold convex shapes.
 typedef struct b2Manifold
 {
 	b2ManifoldPoint points[b2_maxManifoldPoints];

samples/collection/benchmark_pyramid.cpp

@@ -23,7 +23,7 @@ class BenchmarkPyramid : public Sample
 		m_round = 0.0f;
 		m_baseCount = 10;
 		m_rowCount = g_sampleDebug ? 1 : 16;
-		m_columnCount = g_sampleDebug ? 4 : 16;
+		m_columnCount = g_sampleDebug ? 1 : 16;
 		m_groundId = b2_nullBodyId;
 		m_bodyIds = nullptr;
 		m_bodyCount = 0;
@@ -63,7 +63,7 @@ class BenchmarkPyramid : public Sample
 
 			for (int32_t j = i; j < m_baseCount; ++j)
 			{
-				float x = (i + 1.0f) * m_extent + 2.0f * (j - i) * m_extent + centerX;
+				float x = (i + 1.0f) * m_extent + 2.25f * (j - i) * m_extent + centerX - 0.5f;
 				bodyDef.position = {x, y};
 
 				assert(m_bodyIndex < m_bodyCount);

samples/sample.cpp

@@ -16,10 +16,10 @@
 #include <stdio.h>
 #include <string.h>
 
-bool PreSolveFcn(b2ShapeId shapeIdA, b2ShapeId shapeIdB, b2Manifold* manifold, void* context)
+bool PreSolveFcn(b2ShapeId shapeIdA, b2ShapeId shapeIdB, b2Manifold* manifold, int32_t color, void* context)
 {
 	Sample* sample = static_cast<Sample*>(context);
-	return sample->PreSolve(shapeIdA, shapeIdB, manifold);
+	return sample->PreSolve(shapeIdA, shapeIdB, manifold, color);
 }
 
 static void* EnqueueTask(b2TaskCallback* task, int32_t itemCount, int32_t minRange, void* taskContext, void* userContext)
@@ -336,11 +336,20 @@ void Sample::Step(Settings& settings)
 		b2Color addColor = {0.3f, 0.95f, 0.3f, 1.0f};
 		b2Color persistColor = {0.3f, 0.3f, 0.95f, 1.0f};
 
+		b2HexColor colors[8] = {b2_colorAquamarine, b2_colorBisque,	   b2_colorBlue,	   b2_colorBrown,
+								b2_colorBurlywood,	b2_colorCadetBlue, b2_colorChartreuse, b2_colorChocolate};
+
 		for (int32_t i = 0; i < m_pointCount; ++i)
 		{
 			ContactPoint* point = m_points + i;
 
-			if (point->separation > b2_linearSlop)
+			if (0 <= point->color && point->color < 8)
+			{
+				// graph color
+				g_draw.DrawPoint(point->position, 5.0f, b2MakeColor(colors[point->color], 1.0f));
+				g_draw.DrawString(point->position, "%d", point->color);
+			}
+			else if (point->separation > b2_linearSlop)
 			{
 				// Speculative
 				g_draw.DrawPoint(point->position, 5.0f, speculativeColor);
@@ -388,7 +397,7 @@ void Sample::ShiftOrigin(b2Vec2 newOrigin)
 }
 
 // Thread-safe callback
-bool Sample::PreSolve(b2ShapeId shapeIdA, b2ShapeId shapeIdB, b2Manifold* manifold)
+bool Sample::PreSolve(b2ShapeId shapeIdA, b2ShapeId shapeIdB, b2Manifold* manifold, int32_t color)
 {
 	long startCount = m_pointCount.fetch_add(manifold->pointCount);
 	if (startCount >= k_maxContactPoints)
@@ -411,6 +420,7 @@ bool Sample::PreSolve(b2ShapeId shapeIdA, b2ShapeId shapeIdB, b2Manifold* manifo
 		cp->normalImpulse = manifold->points[j].normalImpulse;
 		cp->tangentImpulse = manifold->points[j].tangentImpulse;
 		cp->persisted = manifold->points[j].persisted;
+		cp->color = color;
 		++j;
 	}
 

samples/sample.h

@@ -70,6 +70,7 @@ struct ContactPoint
 	float normalImpulse;
 	float tangentImpulse;
 	float separation;
+	int32_t color;
 };
 
 class SampleTask : public enki::ITaskSet
@@ -112,7 +113,7 @@ class Sample
 	void ResetProfile();
 	void ShiftOrigin(b2Vec2 newOrigin);
 
-	bool PreSolve(b2ShapeId shapeIdA, b2ShapeId shapeIdB, b2Manifold* manifold);
+	bool PreSolve(b2ShapeId shapeIdA, b2ShapeId shapeIdB, b2Manifold* manifold, int32_t color);
 
 	friend class DestructionListener;
 	friend class BoundaryListener;

src/CMakeLists.txt

@@ -22,6 +22,8 @@ set(BOX2D_SOURCE_FILES
 	distance.c
 	dynamic_tree.c
 	geometry.c
+	graph.c
+	graph.h
 	hull.c
 	island.c
 	island.h

src/bitset.h

@@ -24,10 +24,31 @@ void b2InPlaceUnion(b2BitSet* setA, const b2BitSet* setB);
 static inline void b2SetBit(b2BitSet* bitSet, uint32_t bitIndex)
 {
 	uint32_t wordIndex = bitIndex / 64;
+	// TODO_ERIN support growing
 	B2_ASSERT(wordIndex < bitSet->wordCount);
 	bitSet->bits[wordIndex] |= ((uint64_t)1) << (bitIndex % 64);
 }
 
+static inline void b2ClearBit(b2BitSet* bitSet, uint32_t bitIndex)
+{
+	uint32_t wordIndex = bitIndex / 64;
+	if (wordIndex >= bitSet->wordCount)
+	{
+		return;
+	}
+	bitSet->bits[wordIndex] &= ~(((uint64_t)1) << (bitIndex % 64));
+}
+
+static inline bool b2GetBit(const b2BitSet* bitSet, uint32_t bitIndex)
+{
+	uint32_t wordIndex = bitIndex / 64;
+	if (wordIndex >= bitSet->wordCount)
+	{
+		return false;
+	}
+	return (bitSet->bits[wordIndex] & ((uint64_t)1) << (bitIndex % 64)) != 0;
+}
+
 #if defined(_MSC_VER) && !defined(__clang__)
 #include <intrin.h>
 

src/body.c

@@ -9,6 +9,7 @@
 #include "body.h"
 #include "contact.h"
 #include "core.h"
+#include "graph.h"
 #include "island.h"
 #include "joint.h"
 #include "world.h"
@@ -127,6 +128,10 @@ void b2World_DestroyBody(b2BodyId bodyId)
 		int32_t twinIndex = twinKey & 1;
 
 		b2Contact* contact = world->contacts + contactIndex;
+
+		// TODO_ERIN could pass bodies
+		b2RemoveContactFromGraph(world, &world->graph, contact);
+
 		b2ContactEdge* twin = contact->edges + twinIndex;
 
 		// Remove contact from other body's doubly linked list

src/contact.c

@@ -197,6 +197,8 @@ void b2CreateContact(b2World* world, b2Shape* shapeA, b2Shape* shapeB)
 	contact->islandIndex = B2_NULL_INDEX;
 	contact->islandPrev = B2_NULL_INDEX;
 	contact->islandNext = B2_NULL_INDEX;
+	contact->colorContactIndex = B2_NULL_INDEX;
+	contact->colorIndex = B2_NULL_INDEX;
 
 	b2Body* bodyA = world->bodies + shapeA->bodyIndex;
 	b2Body* bodyB = world->bodies + shapeB->bodyIndex;
@@ -254,6 +256,9 @@ void b2CreateContact(b2World* world, b2Shape* shapeA, b2Shape* shapeB)
 	// Add to pair set for fast lookup
 	uint64_t pairKey = B2_SHAPE_PAIR_KEY(contact->shapeIndexA, contact->shapeIndexB);
 	b2AddKey(&world->broadPhase.pairSet, pairKey);
+
+	// TODO_ERIN could pass bodies
+	b2AddContactToGraph(world, &world->graph, contact);
 }
 
 void b2DestroyContact(b2World* world, b2Contact* contact)
@@ -268,6 +273,9 @@ void b2DestroyContact(b2World* world, b2Contact* contact)
 	b2Body* bodyA = world->bodies + edgeA->bodyIndex;
 	b2Body* bodyB = world->bodies + edgeB->bodyIndex;
 
+	// TODO_ERIN pass bodies
+	b2RemoveContactFromGraph(world, &world->graph, contact);
+
 	// if (contactListener && contact->IsTouching())
 	//{
 	//	contactListener->EndContact(contact);
@@ -439,7 +447,7 @@ void b2UpdateContact(b2World* world, b2Contact* contact, b2Shape* shapeA, b2Body
 		if (touching && world->preSolveFcn)
 		{
 			// TODO_ERIN this call assumes thread safety
-			bool collide = world->preSolveFcn(shapeIdA, shapeIdB, &contact->manifold, world->preSolveContext);
+			bool collide = world->preSolveFcn(shapeIdA, shapeIdB, &contact->manifold, contact->colorIndex, world->preSolveContext);
 			if (collide == false)
 			{
 				// disable contact

src/contact.h

@@ -66,6 +66,9 @@ typedef struct b2Contact
 	// This is too hot and has been moved to a separate array
 	//int32_t awakeIndex;
 
+	int32_t colorIndex;
+	int32_t colorContactIndex;
+
 	b2ContactEdge edges[2];
 
 	int32_t shapeIndexA;

src/graph.c

@@ -0,0 +1,160 @@
+// SPDX-FileCopyrightText: 2023 Erin Catto
+// SPDX-License-Identifier: MIT
+
+#include "graph.h"
+
+#include "allocate.h"
+#include "array.h"
+#include "body.h"
+#include "contact.h"
+#include "core.h"
+#include "shape.h"
+#include "stack_allocator.h"
+#include "world.h"
+
+#include <stdbool.h>
+
+void b2CreateGraph(b2Graph* graph, int32_t bodyCapacity, int32_t contactCapacity)
+{
+	bodyCapacity = B2_MAX(bodyCapacity, 8);
+	contactCapacity = B2_MAX(contactCapacity, 8);
+
+	for (int32_t i = 0; i < b2_graphColorCount; ++i)
+	{
+		b2GraphColor* color = graph->colors + i;
+		color->bodySet = b2CreateBitSet(bodyCapacity);
+		b2SetBitCountAndClear(&color->bodySet, bodyCapacity);
+
+		color->contactArray = b2CreateArray(sizeof(int32_t), contactCapacity);
+	}
+}
+
+void b2DestroyGraph(b2Graph* graph)
+{
+	for (int32_t i = 0; i < b2_graphColorCount; ++i)
+	{
+		b2GraphColor* color = graph->colors + i;
+		b2DestroyBitSet(&color->bodySet);
+		b2DestroyArray(color->contactArray, sizeof(int32_t));
+	}
+}
+
+void b2AddContactToGraph(b2World* world, b2Graph* graph, b2Contact* contact)
+{
+	B2_ASSERT(contact->colorContactIndex == B2_NULL_INDEX);
+	B2_ASSERT(contact->colorIndex == B2_NULL_INDEX);
+
+	int32_t bodyIndexA = contact->edges[0].bodyIndex;
+	int32_t bodyIndexB = contact->edges[1].bodyIndex;
+
+	b2BodyType typeA = world->bodies[bodyIndexA].type;
+	b2BodyType typeB = world->bodies[bodyIndexB].type;
+
+	if (typeA == b2_dynamicBody && typeB == b2_dynamicBody)
+	{
+		for (int32_t i = 0; i < b2_graphColorCount; ++i)
+		{
+			b2GraphColor* color = graph->colors + i;
+			if (b2GetBit(&color->bodySet, bodyIndexA) || b2GetBit(&color->bodySet, bodyIndexB))
+			{
+				continue;
+			}
+
+			b2SetBit(&color->bodySet, bodyIndexA);
+			b2SetBit(&color->bodySet, bodyIndexB);
+
+			contact->colorContactIndex = b2Array(color->contactArray).count;
+			b2Array_Push(color->contactArray, contact->object.index);
+			contact->colorIndex = i;
+			break;
+		}
+	}
+	else if (typeA == b2_dynamicBody)
+	{
+		for (int32_t i = 0; i < b2_graphColorCount; ++i)
+		{
+			b2GraphColor* color = graph->colors + i;
+			if (b2GetBit(&color->bodySet, bodyIndexA))
+			{
+				continue;
+			}
+
+			b2SetBit(&color->bodySet, bodyIndexA);
+
+			contact->colorContactIndex = b2Array(color->contactArray).count;
+			b2Array_Push(color->contactArray, contact->object.index);
+			contact->colorIndex = i;
+			break;
+		}
+	}
+	else if (typeB == b2_dynamicBody)
+	{
+		for (int32_t i = 0; i < b2_graphColorCount; ++i)
+		{
+			b2GraphColor* color = graph->colors + i;
+			if (b2GetBit(&color->bodySet, bodyIndexB))
+			{
+				continue;
+			}
+
+			b2SetBit(&color->bodySet, bodyIndexB);
+
+			contact->colorContactIndex = b2Array(color->contactArray).count;
+			b2Array_Push(color->contactArray, contact->object.index);
+			contact->colorIndex = i;
+			break;
+		}
+	}
+}
+
+void b2RemoveContactFromGraph(b2World* world, b2Graph* graph, b2Contact* contact)
+{
+	if (contact->colorIndex == B2_NULL_INDEX)
+	{
+		return;
+	}
+
+	B2_ASSERT(0 <= contact->colorIndex && contact->colorIndex < b2_graphColorCount);
+	int32_t bodyIndexA = contact->edges[0].bodyIndex;
+	int32_t bodyIndexB = contact->edges[1].bodyIndex;
+
+	b2BodyType typeA = world->bodies[bodyIndexA].type;
+	b2BodyType typeB = world->bodies[bodyIndexB].type;
+
+	b2GraphColor* color = graph->colors + contact->colorIndex;
+
+	int32_t colorContactIndex = contact->colorContactIndex;
+	b2Array_RemoveSwap(color->contactArray, colorContactIndex);
+	if (colorContactIndex < b2Array(color->contactArray).count)
+	{
+		// Fix index on swapped contact
+		int32_t swappedContactIndex = color->contactArray[colorContactIndex];
+		world->contacts[swappedContactIndex].colorContactIndex = colorContactIndex;
+	}
+
+	if (typeA == b2_dynamicBody && typeB == b2_dynamicBody)
+	{
+		B2_ASSERT(b2GetBit(&color->bodySet, bodyIndexA) && b2GetBit(&color->bodySet, bodyIndexB));
+
+		b2ClearBit(&color->bodySet, bodyIndexA);
+		b2ClearBit(&color->bodySet, bodyIndexB);
+	}
+	else if (typeA == b2_dynamicBody)
+	{
+		B2_ASSERT(b2GetBit(&color->bodySet, bodyIndexA));
+
+		b2ClearBit(&color->bodySet, bodyIndexA);
+	}
+	else if (typeB == b2_dynamicBody)
+	{
+		B2_ASSERT(b2GetBit(&color->bodySet, bodyIndexB));
+
+		b2ClearBit(&color->bodySet, bodyIndexB);
+	}
+}
+
+void b2SolveGraph(b2World* world, b2Graph* graph)
+{
+	B2_MAYBE_UNUSED(world);
+	B2_MAYBE_UNUSED(graph);
+}