diff --git a/include/box2d/debug_draw.h b/include/box2d/debug_draw.h
index 2c54aa95..2a86be0e 100644
--- a/include/box2d/debug_draw.h
+++ b/include/box2d/debug_draw.h
@@ -39,6 +39,9 @@ typedef struct b2DebugDraw
 	/// Draw a point.
 	void (*DrawPoint)(b2Vec2 p, float size, b2Color color, void* context);
 
+	/// Draw a string.
+	void (*DrawString)(b2Vec2 p, const char* s, void* context);
+
 	bool drawShapes;
 	bool drawJoints;
 	bool drawAABBs;
diff --git a/samples/collection/benchmark_pyramid.cpp b/samples/collection/benchmark_pyramid.cpp
index ee7532b1..a48bfa19 100644
--- a/samples/collection/benchmark_pyramid.cpp
+++ b/samples/collection/benchmark_pyramid.cpp
@@ -63,7 +63,9 @@ class BenchmarkPyramid : public Sample
 
 			for (int32_t j = i; j < m_baseCount; ++j)
 			{
+				//float x = (1.5f * i + 1.0f) * m_extent + 3.0f * (j - i) * m_extent + centerX - 0.5f;
 				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);
@@ -105,6 +107,7 @@ class BenchmarkPyramid : public Sample
 		for (int32_t i = 0; i < m_rowCount; ++i)
 		{
 			b2Segment segment = {{-0.5f * groundWidth, groundY}, {0.5f * groundWidth, groundY}};
+			//b2Segment segment = {{-0.5f * 2.0f * groundWidth, groundY}, {0.5f * 2.0f * groundWidth, groundY}};
 			b2Body_CreateSegment(m_groundId, &shapeDef, &segment);
 			groundY += groundDeltaY;
 		}
diff --git a/samples/draw.cpp b/samples/draw.cpp
index 42bce19a..1dfbe0c6 100644
--- a/samples/draw.cpp
+++ b/samples/draw.cpp
@@ -860,7 +860,11 @@ void DrawPointFcn(b2Vec2 p, float size, b2Color color, void* context)
 	static_cast<Draw*>(context)->DrawPoint(p, size, color);
 }
 
-//
+void DrawStringFcn(b2Vec2 p, const char* s, void* context)
+{
+	static_cast<Draw*>(context)->DrawString(p, s);
+}
+
 Draw::Draw()
 {
 	m_showUI = true;
@@ -871,7 +875,6 @@ Draw::Draw()
 	m_debugDraw = {};
 }
 
-//
 Draw::~Draw()
 {
 	assert(m_points == nullptr);
@@ -879,7 +882,6 @@ Draw::~Draw()
 	assert(m_triangles == nullptr);
 }
 
-//
 void Draw::Create()
 {
 	m_points = static_cast<GLRenderPoints*>(malloc(sizeof(GLRenderPoints)));
@@ -901,6 +903,7 @@ void Draw::Create()
 				   DrawSegmentFcn,
 				   DrawTransformFcn,
 				   DrawPointFcn,
+				   DrawStringFcn,
 				   true,
 				   true,
 				   false,
@@ -908,7 +911,6 @@ void Draw::Create()
 				   this};
 }
 
-//
 void Draw::Destroy()
 {
 	m_points->Destroy();
@@ -928,7 +930,6 @@ void Draw::Destroy()
 	m_roundedTriangles = nullptr;
 }
 
-//
 void Draw::DrawPolygon(const b2Vec2* vertices, int32_t vertexCount, b2Color color)
 {
 	b2Vec2 p1 = vertices[vertexCount - 1];
@@ -941,7 +942,6 @@ void Draw::DrawPolygon(const b2Vec2* vertices, int32_t vertexCount, b2Color colo
 	}
 }
 
-//
 void Draw::DrawSolidPolygon(const b2Vec2* vertices, int32_t vertexCount, b2Color color)
 {
 	b2Color fillColor = {0.5f * color.r, 0.5f * color.g, 0.5f * color.b, 0.5f};
@@ -1038,7 +1038,6 @@ void Draw::DrawRoundedPolygon(const b2Vec2* vertices, int32_t count, float radiu
 	}
 }
 
-//
 void Draw::DrawCircle(b2Vec2 center, float radius, b2Color color)
 {
 	const float k_segments = 32.0f;
@@ -1061,7 +1060,6 @@ void Draw::DrawCircle(b2Vec2 center, float radius, b2Color color)
 	}
 }
 
-//
 void Draw::DrawSolidCircle(b2Vec2 center, float radius, b2Vec2 axis, b2Color color)
 {
 	b2Color fillColor = {0.5f * color.r, 0.5f * color.g, 0.5f * color.b, 0.5f};
@@ -1166,7 +1164,6 @@ void Draw::DrawCapsule(b2Vec2 p1, b2Vec2 p2, float radius, b2Color color)
 	m_lines->Vertex(p2, color);
 }
 
-//
 void Draw::DrawSolidCapsule(b2Vec2 p1, b2Vec2 p2, float radius, b2Color color)
 {
 	float length;
@@ -1267,14 +1264,12 @@ void Draw::DrawSolidCapsule(b2Vec2 p1, b2Vec2 p2, float radius, b2Color color)
 	m_lines->Vertex(p2, color);
 }
 
-//
 void Draw::DrawSegment(b2Vec2 p1, b2Vec2 p2, b2Color color)
 {
 	m_lines->Vertex(p1, color);
 	m_lines->Vertex(p2, color);
 }
 
-//
 void Draw::DrawTransform(b2Transform xf)
 {
 	const float k_axisScale = 0.4f;
@@ -1291,13 +1286,11 @@ void Draw::DrawTransform(b2Transform xf)
 	m_lines->Vertex(p2, green);
 }
 
-//
 void Draw::DrawPoint(b2Vec2 p, float size, b2Color color)
 {
 	m_points->Vertex(p, color, size);
 }
 
-//
 void Draw::DrawString(int x, int y, const char* string, ...)
 {
 	// if (m_showUI == false)
@@ -1316,7 +1309,6 @@ void Draw::DrawString(int x, int y, const char* string, ...)
 	va_end(arg);
 }
 
-//
 void Draw::DrawString(b2Vec2 pw, const char* string, ...)
 {
 	b2Vec2 ps = g_camera.ConvertWorldToScreen(pw);
@@ -1332,7 +1324,6 @@ void Draw::DrawString(b2Vec2 pw, const char* string, ...)
 	va_end(arg);
 }
 
-//
 void Draw::DrawAABB(b2AABB aabb, b2Color c)
 {
 	b2Vec2 p1 = aabb.lowerBound;
@@ -1353,7 +1344,6 @@ void Draw::DrawAABB(b2AABB aabb, b2Color c)
 	m_lines->Vertex(p1, c);
 }
 
-//
 void Draw::Flush()
 {
 	m_roundedTriangles->Flush();
diff --git a/src/bitset.h b/src/bitset.h
index 5298e2de..f4fefacf 100644
--- a/src/bitset.h
+++ b/src/bitset.h
@@ -26,7 +26,7 @@ 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);
+	bitSet->bits[wordIndex] |= ((uint64_t)1 << bitIndex % 64);
 }
 
 static inline void b2ClearBit(b2BitSet* bitSet, uint32_t bitIndex)
@@ -36,7 +36,7 @@ static inline void b2ClearBit(b2BitSet* bitSet, uint32_t bitIndex)
 	{
 		return;
 	}
-	bitSet->bits[wordIndex] &= ~(((uint64_t)1) << (bitIndex % 64));
+	bitSet->bits[wordIndex] &= ~((uint64_t)1 << bitIndex % 64);
 }
 
 static inline bool b2GetBit(const b2BitSet* bitSet, uint32_t bitIndex)
@@ -46,7 +46,7 @@ static inline bool b2GetBit(const b2BitSet* bitSet, uint32_t bitIndex)
 	{
 		return false;
 	}
-	return (bitSet->bits[wordIndex] & ((uint64_t)1) << (bitIndex % 64)) != 0;
+	return (bitSet->bits[wordIndex] & ((uint64_t)1 << bitIndex % 64)) != 0;
 }
 
 #if defined(_MSC_VER) && !defined(__clang__)
diff --git a/src/body.c b/src/body.c
index 8bd1e9be..ec36272c 100644
--- a/src/body.c
+++ b/src/body.c
@@ -129,8 +129,10 @@ void b2World_DestroyBody(b2BodyId bodyId)
 
 		b2Contact* contact = world->contacts + contactIndex;
 
-		// TODO_ERIN could pass bodies
-		b2RemoveContactFromGraph(world, &world->graph, contact);
+		if (contact->colorIndex != B2_NULL_INDEX)
+		{
+			b2RemoveContactFromGraph(world, contact);
+		}
 
 		b2ContactEdge* twin = contact->edges + twinIndex;
 
diff --git a/src/contact.c b/src/contact.c
index 4fe2e3fd..9db06d84 100644
--- a/src/contact.c
+++ b/src/contact.c
@@ -256,9 +256,6 @@ 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)
@@ -273,8 +270,10 @@ 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 (contact->colorIndex != B2_NULL_INDEX)
+	{
+		b2RemoveContactFromGraph(world, contact);
+	}
 
 	// if (contactListener && contact->IsTouching())
 	//{
diff --git a/src/graph.c b/src/graph.c
index 97800721..75110a5c 100644
--- a/src/graph.c
+++ b/src/graph.c
@@ -39,11 +39,13 @@ void b2DestroyGraph(b2Graph* graph)
 	}
 }
 
-void b2AddContactToGraph(b2World* world, b2Graph* graph, b2Contact* contact)
+void b2AddContactToGraph(b2World* world, b2Contact* contact)
 {
 	B2_ASSERT(contact->colorContactIndex == B2_NULL_INDEX);
 	B2_ASSERT(contact->colorIndex == B2_NULL_INDEX);
 
+	b2Graph* graph = &world->graph;
+
 	int32_t bodyIndexA = contact->edges[0].bodyIndex;
 	int32_t bodyIndexB = contact->edges[1].bodyIndex;
 
@@ -107,12 +109,12 @@ void b2AddContactToGraph(b2World* world, b2Graph* graph, b2Contact* contact)
 	}
 }
 
-void b2RemoveContactFromGraph(b2World* world, b2Graph* graph, b2Contact* contact)
+void b2RemoveContactFromGraph(b2World* world, b2Contact* contact)
 {
-	if (contact->colorIndex == B2_NULL_INDEX)
-	{
-		return;
-	}
+	B2_ASSERT(contact->colorIndex != B2_NULL_INDEX);
+	B2_ASSERT(contact->colorContactIndex != B2_NULL_INDEX);
+
+	b2Graph* graph = &world->graph;
 
 	B2_ASSERT(0 <= contact->colorIndex && contact->colorIndex < b2_graphColorCount);
 	int32_t bodyIndexA = contact->edges[0].bodyIndex;
@@ -151,10 +153,136 @@ void b2RemoveContactFromGraph(b2World* world, b2Graph* graph, b2Contact* contact
 
 		b2ClearBit(&color->bodySet, bodyIndexB);
 	}
+
+	contact->colorIndex = B2_NULL_INDEX;
+	contact->colorContactIndex = B2_NULL_INDEX;
 }
 
-void b2SolveGraph(b2World* world, b2Graph* graph)
+typedef struct b2ConstraintPoint
+{
+	b2Vec2 rA;
+	b2Vec2 rB;
+	float normalImpulse;
+	float tangentImpulse;
+	float normalMass;
+	float tangentMass;
+	float velocityBias;
+} b2ConstraintPoint;
+
+typedef struct b2Constraint
 {
+	b2Contact* contact;
+	b2ConstraintPoint points[2];
+	b2Vec2 normal;
+	float friction;
+	int32_t pointCount;
+} b2Constraint;
+
+static void b2InitializeConstraints(b2World* world, b2GraphColor* color, const b2StepContext* stepContext)
+{
+	int32_t constraintCount = b2Array(color->contactArray).count;
+	int32_t* contactIndices = color->contactArray;
+	b2Contact* contacts = world->contacts;
+	b2Body* bodies = world->bodies;
+	float inv_dt = stepContext->inv_dt;
+
+	for (int32_t i = 0; i < constraintCount; ++i)
+	{
+		b2Contact* contact = contacts + contactIndices[i];
+
+		const b2Manifold* manifold = &contact->manifold;
+		int32_t pointCount = manifold->pointCount;
+
+		B2_ASSERT(0 < pointCount && pointCount <= 2);
+
+		int32_t indexA = contact->edges[0].bodyIndex;
+		int32_t indexB = contact->edges[1].bodyIndex;
+		b2Body* bodyA = bodies + indexA;
+		b2Body* bodyB = bodies + indexB;
+
+		b2Constraint* constraint = color->contraints + i;
+		constraint->contact = contact;
+		constraint->normal = manifold->normal;
+		constraint->friction = contact->friction;
+		constraint->pointCount = pointCount;
+
+		float mA = bodyA->invMass;
+		float iA = bodyA->invI;
+		float mB = bodyB->invMass;
+		float iB = bodyB->invI;
+
+		b2Rot qA = bodyA->transform.q;
+		b2Vec2 cA = bodyA->position;
+		b2Rot qB = bodyB->transform.q;
+		b2Vec2 cB = bodyB->position;
+
+		b2Vec2 vA = bodyA->linearVelocity;
+		float wA = bodyA->angularVelocity;
+		b2Vec2 vB = bodyB->linearVelocity;
+		float wB = bodyB->angularVelocity;
+
+		b2Vec2 tangent = b2RightPerp(constraint->normal);
+
+		for (int32_t j = 0; j < pointCount; ++j)
+		{
+			const b2ManifoldPoint* cp = manifold->points + j;
+			b2ConstraintPoint* constraintPoint = constraint->points + j;
+
+			constraintPoint->normalImpulse = cp->normalImpulse;
+			constraintPoint->tangentImpulse =  cp->tangentImpulse;
+
+			constraintPoint->rA = b2Sub(cp->point, cA);
+			constraintPoint->rB = b2Sub(cp->point, cB);
+
+			float rnA = b2Cross(constraintPoint->rA, constraint->normal);
+			float rnB = b2Cross(constraintPoint->rB, constraint->normal);
+
+			float kNormal = mA + mB + iA * rnA * rnA + iB * rnB * rnB;
+
+			constraintPoint->normalMass = kNormal > 0.0f ? 1.0f / kNormal : 0.0f;
+
+			float rtA = b2Cross(constraintPoint->rA, tangent);
+			float rtB = b2Cross(constraintPoint->rB, tangent);
+
+			float kTangent = mA + mB + iA * rtA * rtA + iB * rtB * rtB;
+
+			constraintPoint->tangentMass = kTangent > 0.0f ? 1.0f / kTangent : 0.0f;
+
+			// Velocity bias for speculative collision
+			constraintPoint->velocityBias = -B2_MAX(0.0f, cp->separation * inv_dt);
+		}
+
+		constraintCount += 1;
+	}
+}
+
+void b2SolveGraph(b2World* world, const b2StepContext* stepContext)
+{
+	b2Graph* graph = &world->graph;
+	b2GraphColor* colors = graph->colors;
+
+	int32_t constraintCount = 0;
+	for (int32_t i = 0; i < b2_graphColorCount; ++i)
+	{
+		constraintCount += b2Array(colors[i].contactArray).count;
+	}
+
+	b2Constraint* constraints = b2AllocateStackItem(&world->stackAllocator, constraintCount * sizeof(b2Constraint), "constraint");
+	int32_t base = 0;
+
+	for (int32_t i = 0; i < b2_graphColorCount; ++i)
+	{
+		colors[i].contraints = constraints + base;
+		base += b2Array(colors[i].contactArray).count;
+	}
+
+	B2_ASSERT(base == constraintCount);
+
+	for (int32_t i = 0; i < b2_graphColorCount; ++i)
+	{
+		colors[i].contraints = constraints + base;
+		base += b2Array(colors[i].contactArray).count;
+	}
+
 	B2_MAYBE_UNUSED(world);
-	B2_MAYBE_UNUSED(graph);
 }
diff --git a/src/graph.h b/src/graph.h
index 5c9b7386..93cd6dc7 100644
--- a/src/graph.h
+++ b/src/graph.h
@@ -18,6 +18,7 @@ typedef struct b2GraphColor
 {
 	b2BitSet bodySet;
 	int32_t* contactArray;
+	struct b2Contraint* contraints;
 } b2GraphColor;
 
 typedef struct b2Graph
@@ -29,7 +30,7 @@ typedef struct 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 b2AddContactToGraph(b2World* world, b2Contact* contact);
+void b2RemoveContactFromGraph(b2World* world, b2Contact* contact);
 
-void b2SolveGraph(b2World* world, b2Graph* graph);
+void b2SolveGraph(b2World* world);
diff --git a/src/world.c b/src/world.c
index a2f81c8a..33f3f13e 100644
--- a/src/world.c
+++ b/src/world.c
@@ -1,6 +1,8 @@
 // SPDX-FileCopyrightText: 2023 Erin Catto
 // SPDX-License-Identifier: MIT
 
+#define _CRT_SECURE_NO_WARNINGS
+
 #include "world.h"
 
 #include "allocate.h"
@@ -25,6 +27,7 @@
 #include "box2d/distance.h"
 #include "box2d/timer.h"
 
+#include <stdio.h>
 #include <string.h>
 
 b2World b2_worlds[b2_maxWorlds];
@@ -377,6 +380,7 @@ static void b2Collide(b2World* world)
 			{
 				B2_ASSERT(contact->islandIndex == B2_NULL_INDEX);
 				b2LinkContact(world, contact);
+				b2AddContactToGraph(world, contact);
 				contact->flags &= ~b2_contactStartedTouching;
 			}
 			else
@@ -384,6 +388,7 @@ static void b2Collide(b2World* world)
 				B2_ASSERT(contact->flags & b2_contactStoppedTouching);
 
 				b2UnlinkContact(world, contact);
+				b2RemoveContactFromGraph(world, contact);
 				contact->flags &= ~b2_contactStoppedTouching;
 			}
 
@@ -1168,6 +1173,10 @@ void b2World_Draw(b2WorldId worldId, b2DebugDraw* draw)
 				continue;
 			}
 
+			char buffer[32];
+			sprintf(buffer, "%d", b->object.index);
+			draw->DrawString(b->position, buffer, draw->context);
+
 			int32_t shapeIndex = b->shapeList;
 			while (shapeIndex != B2_NULL_INDEX)
 			{
diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt
index 5f073571..948339b1 100644
--- a/test/CMakeLists.txt
+++ b/test/CMakeLists.txt
@@ -2,6 +2,7 @@
 
 set(BOX2D_TESTS
     main.c
+    test_bitset.c
     test_collision.c
     test_determinism.c
     test_distance.c
diff --git a/test/main.c b/test/main.c
index 29b29cd2..48044774 100644
--- a/test/main.c
+++ b/test/main.c
@@ -18,6 +18,7 @@
 //}
 #endif
 
+extern int BitSetTest();
 extern int MathTest();
 extern int CollisionTest();
 extern int DeterminismTest();
@@ -47,6 +48,7 @@ int main(void)
 	RUN_TEST(HelloWorld);
 	RUN_TEST(ShapeTest);
 	RUN_TEST(TableTest);
+	RUN_TEST(BitSetTest);
 
 	printf("======================================\n");
 	printf("All Box2D tests passed!\n");
diff --git a/test/test_bitset.c b/test/test_bitset.c
new file mode 100644
index 00000000..fdec9a0a
--- /dev/null
+++ b/test/test_bitset.c
@@ -0,0 +1,39 @@
+// SPDX-FileCopyrightText: 2023 Erin Catto
+// SPDX-License-Identifier: MIT
+
+#include "test_macros.h"
+#include "bitset.h"
+#include "box2d/timer.h"
+
+#define COUNT 169
+
+int BitSetTest()
+{
+	b2BitSet bitSet = b2CreateBitSet(COUNT);
+	
+	b2SetBitCountAndClear(&bitSet, COUNT);
+	bool values[COUNT] = {false};
+
+	int32_t i1 = 0, i2 = 1;
+	b2SetBit(&bitSet, i1);
+	values[i1] = true; 
+
+	while (i2 < COUNT)
+	{
+		b2SetBit(&bitSet, i2);
+		values[i2] = true;
+		int32_t next = i1 + i2;
+		i1 = i2;
+		i2 = next;
+	}
+
+	for (int32_t i = 0; i < COUNT; ++i)
+	{
+		bool value = b2GetBit(&bitSet, i);
+		ENSURE(value == values[i]);
+	}
+
+	b2DestroyBitSet(&bitSet);
+
+	return 0;
+}
diff --git a/test/test_determinism.c b/test/test_determinism.c
index 80c787e5..2eb26787 100644
--- a/test/test_determinism.c
+++ b/test/test_determinism.c
@@ -116,6 +116,8 @@ void TiltedStacks(int testIndex, int workerCount)
 	worldDef.finishAllTasks = FinishAllTasks;
 	worldDef.workerCount = workerCount;
 	worldDef.enableSleep = false;
+	worldDef.bodyCapacity = 1024;
+	worldDef.contactCapacity = 4 * 1024;
 
 	b2WorldId worldId = b2CreateWorld(&worldDef);