diff --git a/include/box2d/callbacks.h b/include/box2d/callbacks.h
index d1e4b653..05ce7038 100644
--- a/include/box2d/callbacks.h
+++ b/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
diff --git a/include/box2d/manifold.h b/include/box2d/manifold.h
index 71c4149a..740ba1c4 100644
--- a/include/box2d/manifold.h
+++ b/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];
diff --git a/samples/collection/benchmark_pyramid.cpp b/samples/collection/benchmark_pyramid.cpp
index b6c4c04a..ee7532b1 100644
--- a/samples/collection/benchmark_pyramid.cpp
+++ b/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);
diff --git a/samples/sample.cpp b/samples/sample.cpp
index 5472f8d4..2d9725be 100644
--- a/samples/sample.cpp
+++ b/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;
 	}
 
diff --git a/samples/sample.h b/samples/sample.h
index 9f90ca8f..59d8542a 100644
--- a/samples/sample.h
+++ b/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;
diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
index 4f553c88..4affce80 100644
--- a/src/CMakeLists.txt
+++ b/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
diff --git a/src/bitset.h b/src/bitset.h
index 3d240eeb..5298e2de 100644
--- a/src/bitset.h
+++ b/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>
 
diff --git a/src/body.c b/src/body.c
index 25b0d10b..8bd1e9be 100644
--- a/src/body.c
+++ b/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
diff --git a/src/contact.c b/src/contact.c
index 1413d179..4fe2e3fd 100644
--- a/src/contact.c
+++ b/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
diff --git a/src/contact.h b/src/contact.h
index 0b2018d4..f59e4987 100644
--- a/src/contact.h
+++ b/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;
diff --git a/src/graph.c b/src/graph.c
new file mode 100644
index 00000000..97800721
--- /dev/null
+++ b/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);
+}
diff --git a/src/graph.h b/src/graph.h
new file mode 100644
index 00000000..5c9b7386
--- /dev/null
+++ b/src/graph.h
@@ -0,0 +1,35 @@
+// SPDX-FileCopyrightText: 2023 Erin Catto
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include "array.h"
+#include "bitset.h"
+#include "table.h"
+
+#include "box2d/dynamic_tree.h"
+
+typedef struct b2Contact b2Contact;
+typedef struct b2World b2World;
+
+#define b2_graphColorCount 8
+
+typedef struct b2GraphColor
+{
+	b2BitSet bodySet;
+	int32_t* contactArray;
+} b2GraphColor;
+
+typedef struct b2Graph
+{
+	b2GraphColor colors[b2_graphColorCount];
+
+} b2Graph;
+
+void b2CreateGraph(b2Graph* graph, int32_t bodyCapacity, int32_t contactCapacity);
+void b2DestroyGraph(b2Graph* graph);
+
+void b2AddContactToGraph(b2World* world, b2Graph* graph, b2Contact* contact);
+void b2RemoveContactFromGraph(b2World* world, b2Graph* graph, b2Contact* contact);
+
+void b2SolveGraph(b2World* world, b2Graph* graph);
diff --git a/src/world.c b/src/world.c
index d21451e4..a2f81c8a 100644
--- a/src/world.c
+++ b/src/world.c
@@ -96,6 +96,7 @@ b2WorldId b2CreateWorld(const b2WorldDef* def)
 	world->stackAllocator = b2CreateStackAllocator(def->stackAllocatorCapacity);
 
 	b2CreateBroadPhase(&world->broadPhase);
+	b2CreateGraph(&world->graph, def->bodyCapacity, def->contactCapacity);
 
 	// pools
 	world->bodyPool = b2CreatePool(sizeof(b2Body), B2_MAX(def->bodyCapacity, 1));
@@ -200,6 +201,7 @@ void b2DestroyWorld(b2WorldId id)
 	b2DestroyPool(&world->shapePool);
 	b2DestroyPool(&world->bodyPool);
 
+	b2DestroyGraph(&world->graph);
 	b2DestroyBroadPhase(&world->broadPhase);
 
 	b2DestroyBlockAllocator(world->blockAllocator);
diff --git a/src/world.h b/src/world.h
index 09e79151..05ddaf35 100644
--- a/src/world.h
+++ b/src/world.h
@@ -6,6 +6,7 @@
 #include "bitset.h"
 #include "broad_phase.h"
 #include "island.h"
+#include "graph.h"
 #include "pool.h"
 
 #include "box2d/callbacks.h"
@@ -39,6 +40,7 @@ typedef struct b2World
 	struct b2StackAllocator* stackAllocator;
 
 	b2BroadPhase broadPhase;
+	b2Graph graph;
 
 	b2Pool bodyPool;
 	b2Pool contactPool;