diff --git a/include/box2d/api.h b/include/box2d/api.h
index 832a3960..9ac97c1f 100644
--- a/include/box2d/api.h
+++ b/include/box2d/api.h
@@ -33,16 +33,19 @@
 		#define BOX2D_INLINE extern inline
 	#endif
 #else
-	//#pragma message("BOX2D inline")
+	// #pragma message("BOX2D inline")
 	#define BOX2D_INLINE inline
 #endif
 
 #ifdef __cplusplus
 	#define B2_API extern "C" BOX2D_EXPORT
 	#define B2_INLINE extern "C" BOX2D_INLINE
+	#define B2_LITERAL(T) T
 #else
 	#define B2_API BOX2D_EXPORT
 	#define B2_INLINE BOX2D_INLINE
+	/// Used for C literals like (b2Vec2){1.0f, 2.0f} where C++ requires b2Vec2{1.0f, 2.0f}
+	#define B2_LITERAL(T) (T)
 #endif
 
 /// Prototype for user allocation function.
@@ -54,6 +57,9 @@ typedef void* b2AllocFcn(uint32_t size, int32_t alignment);
 ///	@param mem the memory previously allocated through `b2AllocFcn`
 typedef void b2FreeFcn(void* mem);
 
+/// Prototype for the user assert callback. Return 0 to skip the debugger break.
+typedef int b2AssertFcn(const char* condition, const char* fileName, int lineNumber);
+
 /// This allows the user to override the allocation functions. These should be
 ///	set during application startup.
 B2_API void b2SetAllocator(b2AllocFcn* allocFcn, b2FreeFcn* freeFcn);
@@ -61,9 +67,6 @@ B2_API void b2SetAllocator(b2AllocFcn* allocFcn, b2FreeFcn* freeFcn);
 /// Total bytes allocated by Box2D
 B2_API uint32_t b2GetByteCount(void);
 
-/// Prototype for the user assert callback. Return 0 to skip the debugger break.
-typedef int b2AssertFcn(const char* condition, const char* fileName, int lineNumber);
-
 /// Override the default assert callback.
 ///	@param assertFcn a non-null assert callback
 B2_API void b2SetAssertFcn(b2AssertFcn* assertFcn);
diff --git a/include/box2d/box2d.h b/include/box2d/box2d.h
index 7dc9839e..9709ee55 100644
--- a/include/box2d/box2d.h
+++ b/include/box2d/box2d.h
@@ -104,6 +104,9 @@ B2_API void b2World_EnableContinuous(b2WorldId worldId, bool flag);
 /// Adjust the restitution threshold. Advanced feature for testing.
 B2_API void b2World_SetRestitutionThreshold(b2WorldId worldId, float value);
 
+/// Register the pre-solve callback. This is optional.
+B2_API void b2World_SetPreSolveCallback(b2WorldId worldId, b2PreSolveFcn* fcn, void* context);
+
 /// Adjust contact tuning parameters:
 /// - hertz is the contact stiffness (cycles per second)
 /// - damping ratio is the contact bounciness with 1 being critical damping (non-dimensional)
@@ -212,18 +215,25 @@ B2_API void b2Body_ApplyTorque(b2BodyId bodyId, float torque, bool wake);
 /// Apply an impulse at a point. This immediately modifies the velocity.
 /// It also modifies the angular velocity if the point of application
 /// is not at the center of mass. This wakes up the body.
+/// This should be used for one-shot impulses. If you need a steady force,
+/// use a force instead, which will work better with the sub-stepping solver.
 /// @param impulse the world impulse vector, usually in N-seconds or kg-m/s.
 /// @param point the world position of the point of application.
 /// @param wake also wake up the body
 B2_API void b2Body_ApplyLinearImpulse(b2BodyId bodyId, b2Vec2 impulse, b2Vec2 point, bool wake);
 
 /// Apply an impulse to the center of mass. This immediately modifies the velocity.
+/// This should be used for one-shot impulses. If you need a steady force,
+/// use a force instead, which will work better with the sub-stepping solver.
 /// @param impulse the world impulse vector, usually in N-seconds or kg-m/s.
 /// @param wake also wake up the body
 B2_API void b2Body_ApplyLinearImpulseToCenter(b2BodyId bodyId, b2Vec2 impulse, bool wake);
 
 /// Apply an angular impulse.
-/// @param impulse the angular impulse in units of kg*m*m/s
+/// This should be used for one-shot impulses. If you need a steady force,
+/// use a force instead, which will work better with the sub-stepping solver.
+/// @param impulse the angular impulse in units of
+/// kg*m*m/s
 /// @param wake also wake up the body
 B2_API void b2Body_ApplyAngularImpulse(b2BodyId bodyId, float impulse, bool wake);
 
@@ -247,6 +257,11 @@ B2_API void b2Body_SetMassData(b2BodyId bodyId, b2MassData massData);
 /// Get the mass data for a body.
 B2_API b2MassData b2Body_GetMassData(b2BodyId bodyId);
 
+/// This resets the mass properties to the sum of the mass properties of the fixtures.
+/// This normally does not need to be called unless you called SetMassData to override
+/// the mass and you later want to reset the mass.
+B2_API void b2Body_ResetMassData(b2BodyId bodyId);
+
 /// Adjust the linear damping. Normally this is set in b2BodyDef before creation.
 B2_API void b2Body_SetLinearDamping(b2BodyId bodyId, float linearDamping);
 
@@ -352,7 +367,8 @@ B2_API bool b2Shape_IsSensor(b2ShapeId shapeId);
 B2_API void* b2Shape_GetUserData(b2ShapeId shapeId);
 
 /// Set the density on a shape. Normally this is specified in b2ShapeDef.
-///	This will recompute the mass properties on the parent body.
+///	This will not update the mass properties on the parent body until you
+/// call b2Body_ResetMassData.
 B2_API void b2Shape_SetDensity(b2ShapeId shapeId, float density);
 
 /// Get the density on a shape.
@@ -379,20 +395,37 @@ B2_API void b2Shape_SetFilter(b2ShapeId shapeId, b2Filter filter);
 /// Test a point for overlap with a shape
 B2_API bool b2Shape_TestPoint(b2ShapeId shapeId, b2Vec2 point);
 
-/// Access the circle geometry of a shape.
-B2_API const b2Circle* b2Shape_GetCircle(b2ShapeId shapeId);
+/// Ray cast a shape directly
+B2_API b2RayResult b2Shape_RayCast(b2ShapeId shapeId, b2Vec2 origin, b2Vec2 translation);
 
-/// Access the line segment geometry of a shape.
-B2_API const b2Segment* b2Shape_GetSegment(b2ShapeId shapeId);
+/// Access the circle geometry of a shape. Asserts the type is correct.
+B2_API const b2Circle b2Shape_GetCircle(b2ShapeId shapeId);
+
+/// Access the line segment geometry of a shape. Asserts the type is correct.
+B2_API const b2Segment b2Shape_GetSegment(b2ShapeId shapeId);
 
 /// Access the smooth line segment geometry of a shape. These come from chain shapes.
-B2_API const b2SmoothSegment* b2Shape_GetSmoothSegment(b2ShapeId shapeId);
+/// Asserts the type is correct.
+B2_API const b2SmoothSegment b2Shape_GetSmoothSegment(b2ShapeId shapeId);
+
+/// Access the capsule geometry of a shape. Asserts the type is correct.
+B2_API const b2Capsule b2Shape_GetCapsule(b2ShapeId shapeId);
+
+/// Access the convex polygon geometry of a shape. Asserts the type is correct.
+B2_API const b2Polygon b2Shape_GetPolygon(b2ShapeId shapeId);
+
+/// Allows you to change a shape to be a circle or update the current circle.
+/// This does not modify the mass properties.
+B2_API const void b2Shape_SetCircle(b2ShapeId shapeId, b2Circle circle);
+
+/// Allows you to change a shape to be a capsule or update the current capsule.
+B2_API const void b2Shape_SetCapsule(b2ShapeId shapeId, b2Capsule capsule);
 
-/// Access the capsule geometry of a shape.
-B2_API const b2Capsule* b2Shape_GetCapsule(b2ShapeId shapeId);
+/// Allows you to change a shape to be a segment or update the current segment.
+B2_API const void b2Shape_SetSegment(b2ShapeId shapeId, b2Segment segment);
 
-/// Access the convex polygon geometry of a shape.
-B2_API const b2Polygon* b2Shape_GetPolygon(b2ShapeId shapeId);
+/// Allows you to change a shape to be a segment or update the current segment.
+B2_API const void b2Shape_SetPolygon(b2ShapeId shapeId, b2Polygon polygon);
 
 /// If the type is b2_smoothSegmentShape then you can get the parent chain id.
 /// If the shape is not a smooth segment then this will return b2_nullChainId.
diff --git a/include/box2d/callbacks.h b/include/box2d/callbacks.h
index 5fd334b4..e2ad0dc9 100644
--- a/include/box2d/callbacks.h
+++ b/include/box2d/callbacks.h
@@ -3,12 +3,44 @@
 
 #pragma once
 
-#include "api.h"
 #include "id.h"
-#include "types.h"
+#include "math_types.h"
+
+#include <stdbool.h>
 
 typedef struct b2Manifold b2Manifold;
 
+/// Task interface
+/// This is prototype for a Box2D task. Your task system is expected to invoke the Box2D task with these arguments.
+/// The task spans a range of the parallel-for: [startIndex, endIndex)
+/// The worker index must correctly identify each worker in the user thread pool, expected in [0, workerCount).
+///	A worker must only exist on only one thread at a time and is analogous to the thread index.
+/// The task context is the context pointer sent from Box2D when it is enqueued.
+///	The startIndex and endIndex are expected in the range [0, itemCount) where itemCount is the argument to b2EnqueueTaskCallback
+/// below. Box2D expects startIndex < endIndex and will execute a loop like this:
+///
+/// for (int i = startIndex; i < endIndex; ++i)
+///	{
+///		DoWork();
+///	}
+typedef void b2TaskCallback(int32_t startIndex, int32_t endIndex, uint32_t workerIndex, void* taskContext);
+
+/// These functions can be provided to Box2D to invoke a task system. These are designed to work well with enkiTS.
+/// Returns a pointer to the user's task object. May be nullptr. A nullptr indicates to Box2D that the work was executed
+///	serially within the callback and there is no need to call b2FinishTaskCallback.
+///	The itemCount is the number of Box2D work items that are to be partitioned among workers by the user's task system.
+///	This is essentially a parallel-for. The minRange parameter is a suggestion of the minimum number of items to assign
+///	per worker to reduce overhead. For example, suppose the task is small and that itemCount is 16. A minRange of 8 suggests
+///	that your task system should split the work items among just two workers, even if you have more available.
+///	In general the range [startIndex, endIndex) send to b2TaskCallback should obey:
+///	endIndex - startIndex >= minRange
+///	The exception of course is when itemCount < minRange.
+typedef void* b2EnqueueTaskCallback(b2TaskCallback* task, int32_t itemCount, int32_t minRange, void* taskContext,
+									void* userContext);
+
+/// Finishes a user task object that wraps a Box2D task.
+typedef void b2FinishTaskCallback(void* userTask, void* userContext);
+
 /// Prototype for a pre-solve callback.
 /// This is called after a contact is updated. This allows you to inspect a
 /// contact before it goes to the solver. If you are careful, you can modify the
@@ -22,9 +54,6 @@ typedef struct b2Manifold b2Manifold;
 ///	Return false if you want to disable the contact this step
 typedef bool b2PreSolveFcn(b2ShapeId shapeIdA, b2ShapeId shapeIdB, b2Manifold* manifold, void* context);
 
-/// Register the pre-solve callback. This is optional.
-B2_API void b2World_SetPreSolveCallback(b2WorldId worldId, b2PreSolveFcn* fcn, void* context);
-
 /// Prototype callback for AABB queries.
 /// See b2World_Query
 /// Called for each shape found in the query AABB.
@@ -35,6 +64,7 @@ typedef bool b2QueryResultFcn(b2ShapeId shapeId, void* context);
 /// See b2World::RayCast
 /// Called for each shape found in the query. You control how the ray cast
 /// proceeds by returning a float:
+/// #todo rework this to return penetration
 /// return -1: ignore this shape and continue
 /// return 0: terminate the ray cast
 /// return fraction: clip the ray to this point
@@ -46,13 +76,3 @@ typedef bool b2QueryResultFcn(b2ShapeId shapeId, void* context);
 /// @return -1 to filter, 0 to terminate, fraction to clip the ray for
 /// closest hit, 1 to continue
 typedef float b2CastResultFcn(b2ShapeId shapeId, b2Vec2 point, b2Vec2 normal, float fraction, void* context);
-
-/// Use an instance of this structure and the callback below to get the closest hit.
-typedef struct b2RayResult
-{
-	b2ShapeId shapeId;
-	b2Vec2 point;
-	b2Vec2 normal;
-	float fraction;
-	bool hit;
-} b2RayResult;
diff --git a/include/box2d/color.h b/include/box2d/color.h
index e148462a..34e99fdb 100644
--- a/include/box2d/color.h
+++ b/include/box2d/color.h
@@ -4,12 +4,7 @@
 #pragma once
 
 #include "api.h"
-
-/// Color for debug drawing. Each value has the range [0,1].
-typedef struct b2Color
-{
-	float r, g, b, a;
-} b2Color;
+#include "types.h"
 
 /// All the colors! Credit to wherever I got this from, I forget.
 typedef enum b2HexColor
diff --git a/include/box2d/debug_draw.h b/include/box2d/debug_draw.h
index 8a28efa6..56a10b40 100644
--- a/include/box2d/debug_draw.h
+++ b/include/box2d/debug_draw.h
@@ -3,7 +3,7 @@
 
 #pragma once
 
-#include "types.h"
+#include "math_types.h"
 
 /// This struct holds callbacks you can implement to draw a box2d world.
 typedef struct b2DebugDraw
diff --git a/include/box2d/distance.h b/include/box2d/distance.h
index c2dac4e9..a9b76551 100644
--- a/include/box2d/distance.h
+++ b/include/box2d/distance.h
@@ -5,7 +5,7 @@
 
 #include "api.h"
 #include "constants.h"
-#include "types.h"
+#include "geometry.h"
 
 /// Result of computing the distance between two line segments
 typedef struct b2SegmentDistanceResult
@@ -79,7 +79,7 @@ typedef struct b2ShapeCastPairInput
 
 /// Perform a linear shape cast of shape B moving and shape A fixed. Determines the hit point, normal, and translation fraction.
 /// @returns true if hit, false if there is no hit or an initial overlap
-B2_API b2RayCastOutput b2ShapeCast(const b2ShapeCastPairInput* input);
+B2_API b2CastOutput b2ShapeCast(const b2ShapeCastPairInput* input);
 
 /// Make a proxy for use in GJK and related functions.
 B2_API b2DistanceProxy b2MakeProxy(const b2Vec2* vertices, int32_t count, float radius);
diff --git a/include/box2d/dynamic_tree.h b/include/box2d/dynamic_tree.h
index e5b47be1..ecf1d3a6 100644
--- a/include/box2d/dynamic_tree.h
+++ b/include/box2d/dynamic_tree.h
@@ -5,11 +5,16 @@
 
 #include "api.h"
 #include "constants.h"
-#include "types.h"
+#include "math_types.h"
+
+#include <stdbool.h>
 
 #define b2_defaultCategoryBits (0x00000001)
 #define b2_defaultMaskBits (0xFFFFFFFF)
 
+typedef struct b2RayCastInput b2RayCastInput;
+typedef struct b2ShapeCastInput b2ShapeCastInput;
+
 /// A node in the dynamic tree. The user does not interact with this directly.
 /// 16 + 16 + 8 + pad(8)
 typedef struct b2TreeNode
diff --git a/include/box2d/geometry.h b/include/box2d/geometry.h
index fd1349e3..1b90cb52 100644
--- a/include/box2d/geometry.h
+++ b/include/box2d/geometry.h
@@ -8,8 +8,33 @@
 #include "types.h"
 
 typedef struct b2Hull b2Hull;
-typedef struct b2CastOutput b2RayCastOutput;
-typedef struct b2RayCastInput b2RayCastInput;
+
+/// Low level ray-cast input data
+typedef struct b2RayCastInput
+{
+	b2Vec2 origin, translation;
+	float maxFraction;
+} b2RayCastInput;
+
+/// Low level shape cast input in generic form
+typedef struct b2ShapeCastInput
+{
+	b2Vec2 points[b2_maxPolygonVertices];
+	int32_t count;
+	float radius;
+	b2Vec2 translation;
+	float maxFraction;
+} b2ShapeCastInput;
+
+/// Low level ray-cast or shape-cast output data
+typedef struct b2CastOutput
+{
+	b2Vec2 normal;
+	b2Vec2 point;
+	float fraction;
+	int32_t iterations;
+	bool hit;
+} b2CastOutput;
 
 /// This holds the mass data computed for a shape.
 typedef struct b2MassData
@@ -98,7 +123,7 @@ B2_API b2Polygon b2MakeRoundedBox(float hx, float hy, float radius);
 /// Make an offset box, bypassing the need for a convex hull.
 B2_API b2Polygon b2MakeOffsetBox(float hx, float hy, b2Vec2 center, float angle);
 
-/// Transform a polygon. This is useful for transfering a shape from one body to another.
+/// Transform a polygon. This is useful for transferring a shape from one body to another.
 B2_API b2Polygon b2TransformPolygon(b2Transform transform, const b2Polygon* polygon);
 
 /// Compute mass properties of a circle
@@ -132,26 +157,26 @@ B2_API bool b2PointInCapsule(b2Vec2 point, const b2Capsule* shape);
 B2_API bool b2PointInPolygon(b2Vec2 point, const b2Polygon* shape);
 
 /// Ray cast versus circle in shape local space. Initial overlap is treated as a miss.
-B2_API b2RayCastOutput b2RayCastCircle(const b2RayCastInput* input, const b2Circle* shape);
+B2_API b2CastOutput b2RayCastCircle(const b2RayCastInput* input, const b2Circle* shape);
 
 /// Ray cast versus capsule in shape local space. Initial overlap is treated as a miss.
-B2_API b2RayCastOutput b2RayCastCapsule(const b2RayCastInput* input, const b2Capsule* shape);
+B2_API b2CastOutput b2RayCastCapsule(const b2RayCastInput* input, const b2Capsule* shape);
 
 /// Ray cast versus segment in shape local space. Optionally treat the segment as one-sided with hits from
 /// the left side being treated as a miss.
-B2_API b2RayCastOutput b2RayCastSegment(const b2RayCastInput* input, const b2Segment* shape, bool oneSided);
+B2_API b2CastOutput b2RayCastSegment(const b2RayCastInput* input, const b2Segment* shape, bool oneSided);
 
 /// Ray cast versus polygon in shape local space. Initial overlap is treated as a miss.
-B2_API b2RayCastOutput b2RayCastPolygon(const b2RayCastInput* input, const b2Polygon* shape);
+B2_API b2CastOutput b2RayCastPolygon(const b2RayCastInput* input, const b2Polygon* shape);
 
 /// Shape cast versus a circle. Initial overlap is treated as a miss.
-B2_API b2RayCastOutput b2ShapeCastCircle(const b2ShapeCastInput* input, const b2Circle* shape);
+B2_API b2CastOutput b2ShapeCastCircle(const b2ShapeCastInput* input, const b2Circle* shape);
 
 /// Shape cast versus a capsule. Initial overlap is treated as a miss.
-B2_API b2RayCastOutput b2ShapeCastCapsule(const b2ShapeCastInput* input, const b2Capsule* shape);
+B2_API b2CastOutput b2ShapeCastCapsule(const b2ShapeCastInput* input, const b2Capsule* shape);
 
 /// Shape cast versus a line segment. Initial overlap is treated as a miss.
-B2_API b2RayCastOutput b2ShapeCastSegment(const b2ShapeCastInput* input, const b2Segment* shape);
+B2_API b2CastOutput b2ShapeCastSegment(const b2ShapeCastInput* input, const b2Segment* shape);
 
 /// Shape cast versus a convex polygon. Initial overlap is treated as a miss.
-B2_API b2RayCastOutput b2ShapeCastPolygon(const b2ShapeCastInput* input, const b2Polygon* shape);
+B2_API b2CastOutput b2ShapeCastPolygon(const b2ShapeCastInput* input, const b2Polygon* shape);
diff --git a/include/box2d/joint_util.h b/include/box2d/joint_util.h
deleted file mode 100644
index 0509603d..00000000
--- a/include/box2d/joint_util.h
+++ /dev/null
@@ -1,15 +0,0 @@
-// SPDX-FileCopyrightText: 2023 Erin Catto
-// SPDX-License-Identifier: MIT
-
-#pragma once
-
-#include "api.h"
-#include "id.h"
-
-/// Utility to compute linear stiffness values from frequency and damping ratio
-B2_API void b2LinearStiffness(float* stiffness, float* damping, float frequencyHertz, float dampingRatio, b2BodyId bodyA,
-								 b2BodyId bodyB);
-
-/// Utility to compute angular stiffness values from frequency and damping ratio
-B2_API void b2AngularStiffness(float* stiffness, float* damping, float frequencyHertz, float dampingRatio, b2BodyId bodyA,
-								  b2BodyId bodyB);
diff --git a/include/box2d/math.h b/include/box2d/math.h
index 6df0d298..701432a0 100644
--- a/include/box2d/math.h
+++ b/include/box2d/math.h
@@ -4,10 +4,11 @@
 #pragma once
 
 #include "api.h"
-#include "types.h"
+#include "math_types.h"
 
 #include <float.h>
 #include <math.h>
+#include <stdbool.h>
 
 /// Macro to get the minimum of two values
 #define B2_MIN(A, B) ((A) < (B) ? (A) : (B))
diff --git a/include/box2d/math_types.h b/include/box2d/math_types.h
new file mode 100644
index 00000000..08b5fa5d
--- /dev/null
+++ b/include/box2d/math_types.h
@@ -0,0 +1,47 @@
+// SPDX-FileCopyrightText: 2023 Erin Catto
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+/// 2D vector
+/// This can be used to represent a point or free vector
+typedef struct b2Vec2
+{
+	/// coordinates
+	float x, y;
+} b2Vec2;
+
+/// 2D rotation
+/// This is similar to using a complex number for rotation
+typedef struct b2Rot
+{
+	/// sine and cosine
+	float s, c;
+} b2Rot;
+
+/// A 2D rigid transform
+typedef struct b2Transform
+{
+	b2Vec2 p;
+	b2Rot q;
+} b2Transform;
+
+/// A 2-by-2 Matrix
+typedef struct b2Mat22
+{
+	/// columns
+	b2Vec2 cx, cy;
+} b2Mat22;
+
+/// Axis-aligned bounding box
+typedef struct b2AABB
+{
+	b2Vec2 lowerBound;
+	b2Vec2 upperBound;
+} b2AABB;
+
+/// Color for debug drawing. Each value has the range [0,1].
+typedef struct b2Color
+{
+	float r, g, b, a;
+} b2Color;
diff --git a/include/box2d/types.h b/include/box2d/types.h
index d6f53e54..632b769d 100644
--- a/include/box2d/types.h
+++ b/include/box2d/types.h
@@ -5,128 +5,37 @@
  * @file types.h
  * @brief types used by the Box2D API
  *
- * Mostly definition structs
+ * Mostly definition structures
  * @see http://www.box2d.org
  */
 
 #pragma once
 
-#include "color.h"
+#include "api.h"
+#include "callbacks.h"
 #include "constants.h"
 #include "id.h"
+#include "math_types.h"
 
 #include <stdbool.h>
 #include <stddef.h>
 #include <stdint.h>
 
-// clang-format off
-#ifdef __cplusplus
-	#define B2_LITERAL(T) T
-#else
-	/// Used for C literals like (b2Vec2){1.0f, 2.0f} where C++ requires b2Vec2{1.0f, 2.0f}
-	#define B2_LITERAL(T) (T)
-#endif
-// clang-format on
-
 /// Returns the number of elements of an array
 #define B2_ARRAY_COUNT(A) (int)(sizeof(A) / sizeof(A[0]))
 
 /// Used to prevent the compiler from warning about unused variables
 #define B2_MAYBE_UNUSED(x) ((void)(x))
 
-/// A 2D vector
-/// This can be used to represent a point or free vector.
-typedef struct b2Vec2
-{
-	/// coordinates
-	float x, y;
-} b2Vec2;
-
-/// 2D rotation
-typedef struct b2Rot
-{
-	/// sine and cosine
-	float s, c;
-} b2Rot;
-
-/// A 2D rigid transform
-typedef struct b2Transform
-{
-	b2Vec2 p;
-	b2Rot q;
-} b2Transform;
-
-/// A 2-by-2 Matrix
-typedef struct b2Mat22
-{
-	/// columns
-	b2Vec2 cx, cy;
-} b2Mat22;
-
-/// Axis-aligned bounding box
-typedef struct b2AABB
-{
-	b2Vec2 lowerBound;
-	b2Vec2 upperBound;
-} b2AABB;
-
-/// Low level ray-cast input data
-typedef struct b2RayCastInput
-{
-	b2Vec2 origin, translation;
-	float maxFraction;
-} b2RayCastInput;
-
-/// Low level hape cast input in generic form
-typedef struct b2ShapeCastInput
+/// Result from b2World_RayCastClosest
+typedef struct b2RayResult
 {
-	b2Vec2 points[b2_maxPolygonVertices];
-	int32_t count;
-	float radius;
-	b2Vec2 translation;
-	float maxFraction;
-} b2ShapeCastInput;
-
-/// Low level ray-cast or shape-cast output data
-typedef struct b2CastOutput
-{
-	b2Vec2 normal;
+	b2ShapeId shapeId;
 	b2Vec2 point;
+	b2Vec2 normal;
 	float fraction;
-	int32_t iterations;
 	bool hit;
-} b2RayCastOutput;
-
-/// Task interface
-/// This is prototype for a Box2D task. Your task system is expected to invoke the Box2D task with these arguments.
-/// The task spans a range of the parallel-for: [startIndex, endIndex)
-/// The worker index must correctly identify each worker in the user thread pool, expected in [0, workerCount).
-///	A worker must only exist on only one thread at a time and is analogous to the thread index.
-/// The task context is the context pointer sent from Box2D when it is enqueued.
-///	The startIndex and endIndex are expected in the range [0, itemCount) where itemCount is the argument to b2EnqueueTaskCallback
-/// below. Box2D expects startIndex < endIndex and will execute a loop like this:
-///
-/// for (int i = startIndex; i < endIndex; ++i)
-///	{
-///		DoWork();
-///	}
-typedef void b2TaskCallback(int32_t startIndex, int32_t endIndex, uint32_t workerIndex, void* taskContext);
-
-/// These functions can be provided to Box2D to invoke a task system. These are designed to work well with enkiTS.
-/// Returns a pointer to the user's task object. May be nullptr. A nullptr indicates to Box2D that the work was executed
-///	serially within the callback and there is no need to call b2FinishTaskCallback.
-///	The itemCount is the number of Box2D work items that are to be partitioned among workers by the user's task system.
-///	This is essentially a parallel-for. The minRange parameter is a suggestion of the minimum number of items to assign
-///	per worker to reduce overhead. For example, suppose the task is small and that itemCount is 16. A minRange of 8 suggests
-///	that your task system should split the work items among just two workers, even if you have more available.
-///	In general the range [startIndex, endIndex) send to b2TaskCallback should obey:
-///	endIndex - startIndex >= minRange
-///	The exception of course is when itemCount < minRange.
-typedef void* b2EnqueueTaskCallback(b2TaskCallback* task, int32_t itemCount, int32_t minRange, void* taskContext,
-									void* userContext);
-
-/// Finishes a user task object that wraps a Box2D task.
-typedef void b2FinishTaskCallback(void* userTask, void* userContext);
+} b2RayResult;
 
 /// World definition used to create a simulation world. Must be initialized using b2DefaultWorldDef.
 typedef struct b2WorldDef
@@ -189,9 +98,6 @@ typedef struct b2WorldDef
 	void* userTaskContext;
 } b2WorldDef;
 
-/// Use this to initialize your world definition
-B2_API b2WorldDef b2DefaultWorldDef();
-
 /// The body type.
 /// static: zero mass, zero velocity, may be manually moved
 /// kinematic: zero mass, non-zero velocity set by user, moved by solver
@@ -255,18 +161,6 @@ typedef struct b2BodyDef
 	bool isEnabled;
 } b2BodyDef;
 
-/// Use this to initialize your body definition
-static inline b2BodyDef b2DefaultBodyDef()
-{
-	b2BodyDef def = B2_ZERO_INIT;
-	def.type = b2_staticBody;
-	def.gravityScale = 1.0f;
-	def.enableSleep = true;
-	def.isAwake = true;
-	def.isEnabled = true;
-	return def;
-}
-
 /// This holds contact filtering data.
 typedef struct b2Filter
 {
@@ -283,13 +177,6 @@ typedef struct b2Filter
 	int32_t groupIndex;
 } b2Filter;
 
-/// Use this to initialize your filter
-static inline b2Filter b2DefaultFilter()
-{
-	b2Filter filter = {0x00000001, 0xFFFFFFFF, 0};
-	return filter;
-}
-
 /// This holds contact filtering data.
 typedef struct b2QueryFilter
 {
@@ -301,13 +188,6 @@ typedef struct b2QueryFilter
 	uint32_t maskBits;
 } b2QueryFilter;
 
-/// Use this to initialize your query filter
-static inline b2QueryFilter b2DefaultQueryFilter()
-{
-	b2QueryFilter filter = {0x00000001, 0xFFFFFFFF};
-	return filter;
-}
-
 /// Shape type
 typedef enum b2ShapeType
 {
@@ -352,18 +232,6 @@ typedef struct b2ShapeDef
 
 } b2ShapeDef;
 
-/// Use this to initialize your shape definition
-static inline b2ShapeDef b2DefaultShapeDef()
-{
-	b2ShapeDef def = B2_ZERO_INIT;
-	def.friction = 0.6f;
-	def.density = 1.0f;
-	def.filter = b2DefaultFilter();
-	def.enableSensorEvents = true;
-	def.enableContactEvents = true;
-	return def;
-}
-
 /// Used to create a chain of edges. This is designed to eliminate ghost collisions with some limitations.
 ///	- DO NOT use chain shapes unless you understand the limitations. This is an advanced feature!
 ///	- chains are one-sided
@@ -376,6 +244,7 @@ static inline b2ShapeDef b2DefaultShapeDef()
 ///	- an open chain shape has NO COLLISION on the first and final edge
 ///	- you may overlap two open chains on their first three and/or last three points to get smooth collision
 ///	- a chain shape creates multiple hidden shapes on the body
+/// https://en.wikipedia.org/wiki/Polygonal_chain
 typedef struct b2ChainDef
 {
 	/// An array of at least 4 points. These are cloned and may be temporary.
@@ -400,15 +269,6 @@ typedef struct b2ChainDef
 	b2Filter filter;
 } b2ChainDef;
 
-/// Use this to initialize your chain definition
-static inline b2ChainDef b2DefaultChainDef()
-{
-	b2ChainDef def = B2_ZERO_INIT;
-	def.friction = 0.6f;
-	def.filter = b2DefaultFilter();
-	return def;
-}
-
 /// Profiling data. Times are in milliseconds.
 typedef struct b2Profile
 {
@@ -438,3 +298,21 @@ typedef struct b2Counters
 	int32_t taskCount;
 	int32_t colorCounts[b2_graphColorCount + 1];
 } b2Counters;
+
+/// Use this to initialize your world definition
+B2_API b2WorldDef b2DefaultWorldDef();
+
+/// Use this to initialize your body definition
+B2_API b2BodyDef b2DefaultBodyDef();
+
+/// Use this to initialize your filter
+B2_API b2Filter b2DefaultFilter();
+
+/// Use this to initialize your query filter
+B2_API b2QueryFilter b2DefaultQueryFilter();
+
+/// Use this to initialize your shape definition
+B2_API b2ShapeDef b2DefaultShapeDef();
+
+/// Use this to initialize your chain definition
+B2_API b2ChainDef b2DefaultChainDef();
diff --git a/samples/collection/sample_dynamic_tree.cpp b/samples/collection/sample_dynamic_tree.cpp
index c27a2781..5356cbd1 100644
--- a/samples/collection/sample_dynamic_tree.cpp
+++ b/samples/collection/sample_dynamic_tree.cpp
@@ -1,10 +1,10 @@
 // SPDX-FileCopyrightText: 2023 Erin Catto
 // SPDX-License-Identifier: MIT
 
-//#include "aabb.h"
 #include "sample.h"
 
 #include "box2d/dynamic_tree.h"
+#include "box2d/geometry.h"
 #include "box2d/math.h"
 
 #include <GLFW/glfw3.h>
diff --git a/samples/collection/sample_joints.cpp b/samples/collection/sample_joints.cpp
index 4b3fa37b..29429ea3 100644
--- a/samples/collection/sample_joints.cpp
+++ b/samples/collection/sample_joints.cpp
@@ -7,9 +7,9 @@
 #include "settings.h"
 
 #include "box2d/box2d.h"
+#include "box2d/color.h"
 #include "box2d/geometry.h"
 #include "box2d/hull.h"
-#include "box2d/joint_util.h"
 #include "box2d/math.h"
 
 #include <GLFW/glfw3.h>
diff --git a/samples/collection/sample_manifold.cpp b/samples/collection/sample_manifold.cpp
index 8bf1f914..ec5132c9 100644
--- a/samples/collection/sample_manifold.cpp
+++ b/samples/collection/sample_manifold.cpp
@@ -3,6 +3,7 @@
 
 #include "sample.h"
 
+#include "box2d/color.h"
 #include "box2d/distance.h"
 #include "box2d/geometry.h"
 #include "box2d/hull.h"
diff --git a/samples/collection/sample_ray_cast.cpp b/samples/collection/sample_ray_cast.cpp
index b35c4ad0..aa89e078 100644
--- a/samples/collection/sample_ray_cast.cpp
+++ b/samples/collection/sample_ray_cast.cpp
@@ -4,6 +4,7 @@
 #include "sample.h"
 
 #include "box2d/box2d.h"
+#include "box2d/color.h"
 #include "box2d/geometry.h"
 #include "box2d/hull.h"
 #include "box2d/math.h"
@@ -139,7 +140,7 @@ class RayCast : public Sample
 		}
 	}
 
-	void DrawRay(const b2RayCastOutput* output)
+	void DrawRay(const b2CastOutput* output)
 	{
 		b2Color white = {1.0f, 1.0f, 1.0f, 1.0f};
 		b2Color green = {0.0f, 1.0f, 0.0f, 1.0f};
@@ -194,7 +195,7 @@ class RayCast : public Sample
 		b2Color color1 = {0.3f, 0.8f, 0.6f, 1.0f};
 		b2Color dim1 = {0.5f * color1.r, 0.5f * color1.g, 0.5f * color1.b, 1.0f};
 
-		b2RayCastOutput output = {0};
+		b2CastOutput output = {0};
 		float maxFraction = 1.0f;
 
 		// circle
@@ -208,7 +209,7 @@ class RayCast : public Sample
 			b2Vec2 translation = b2InvRotateVector(xf.q, b2Sub(m_rayEnd, m_rayStart));
 			b2RayCastInput input = {start, translation, maxFraction};
 
-			b2RayCastOutput localOutput = b2RayCastCircle(&input, &m_circle);
+			b2CastOutput localOutput = b2RayCastCircle(&input, &m_circle);
 			if (localOutput.hit)
 			{
 				output = localOutput;
@@ -231,7 +232,7 @@ class RayCast : public Sample
 			b2Vec2 translation = b2InvRotateVector(xf.q, b2Sub(m_rayEnd, m_rayStart));
 			b2RayCastInput input = {start, translation, maxFraction};
 
-			b2RayCastOutput localOutput = b2RayCastCapsule(&input, &m_capsule);
+			b2CastOutput localOutput = b2RayCastCapsule(&input, &m_capsule);
 			if (localOutput.hit)
 			{
 				output = localOutput;
@@ -258,7 +259,7 @@ class RayCast : public Sample
 			b2Vec2 translation = b2InvRotateVector(xf.q, b2Sub(m_rayEnd, m_rayStart));
 			b2RayCastInput input = {start, translation, maxFraction};
 
-			b2RayCastOutput localOutput = b2RayCastPolygon(&input, &m_box);
+			b2CastOutput localOutput = b2RayCastPolygon(&input, &m_box);
 			if (localOutput.hit)
 			{
 				output = localOutput;
@@ -285,7 +286,7 @@ class RayCast : public Sample
 			b2Vec2 translation = b2InvRotateVector(xf.q, b2Sub(m_rayEnd, m_rayStart));
 			b2RayCastInput input = {start, translation, maxFraction};
 
-			b2RayCastOutput localOutput = b2RayCastPolygon(&input, &m_triangle);
+			b2CastOutput localOutput = b2RayCastPolygon(&input, &m_triangle);
 			if (localOutput.hit)
 			{
 				output = localOutput;
@@ -309,7 +310,7 @@ class RayCast : public Sample
 			b2Vec2 translation = b2InvRotateVector(xf.q, b2Sub(m_rayEnd, m_rayStart));
 			b2RayCastInput input = {start, translation, maxFraction};
 
-			b2RayCastOutput localOutput = b2RayCastSegment(&input, &m_segment, false);
+			b2CastOutput localOutput = b2RayCastSegment(&input, &m_segment, false);
 			if (localOutput.hit)
 			{
 				output = localOutput;
diff --git a/samples/collection/sample_robustness.cpp b/samples/collection/sample_robustness.cpp
index a1c331a0..bff5be3f 100644
--- a/samples/collection/sample_robustness.cpp
+++ b/samples/collection/sample_robustness.cpp
@@ -7,7 +7,6 @@
 #include "box2d/box2d.h"
 #include "box2d/geometry.h"
 #include "box2d/hull.h"
-#include "box2d/joint_util.h"
 
 #include <GLFW/glfw3.h>
 #include <imgui.h>
diff --git a/samples/collection/sample_shape_cast.cpp b/samples/collection/sample_shape_cast.cpp
index 46274116..bb8a44ce 100644
--- a/samples/collection/sample_shape_cast.cpp
+++ b/samples/collection/sample_shape_cast.cpp
@@ -85,7 +85,7 @@ class ShapeCast : public Sample
 		input.transformB = m_transformB;
 		input.translationB = m_translationB;
 
-		b2RayCastOutput output = b2ShapeCast(&input);
+		b2CastOutput output = b2ShapeCast(&input);
 
 		b2Transform transformB2;
 		transformB2.q = m_transformB.q;
diff --git a/samples/collection/sample_shapes.cpp b/samples/collection/sample_shapes.cpp
index 558311c4..4f5d84b9 100644
--- a/samples/collection/sample_shapes.cpp
+++ b/samples/collection/sample_shapes.cpp
@@ -5,6 +5,7 @@
 #include "settings.h"
 
 #include "box2d/box2d.h"
+#include "box2d/color.h"
 #include "box2d/geometry.h"
 #include "box2d/hull.h"
 #include "box2d/math.h"
diff --git a/samples/sample.cpp b/samples/sample.cpp
index 516d84ce..be62372c 100644
--- a/samples/sample.cpp
+++ b/samples/sample.cpp
@@ -7,7 +7,6 @@
 
 #include "box2d/box2d.h"
 #include "box2d/callbacks.h"
-#include "box2d/joint_util.h"
 #include "box2d/manifold.h"
 #include "box2d/math.h"
 #include "box2d/math_cpp.h"
diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
index 337aa149..e8d8a3a1 100644
--- a/src/CMakeLists.txt
+++ b/src/CMakeLists.txt
@@ -3,8 +3,6 @@ set(BOX2D_SOURCE_FILES
 	aabb.h
 	allocate.c
 	allocate.h
-	arena_allocator.c
-	arena_allocator.h
 	array.c
 	array.h
 	bitset.c
@@ -16,6 +14,8 @@ set(BOX2D_SOURCE_FILES
 	body.h
 	broad_phase.c
 	broad_phase.h
+	constraint_graph.c
+	constraint_graph.h
 	contact.c
 	contact.h
 	contact_solver.c
@@ -26,9 +26,8 @@ set(BOX2D_SOURCE_FILES
 	distance_joint.c
 	dynamic_tree.c
 	geometry.c
-	graph.c
-	graph.h
 	hull.c
+	implementation.c
 	island.c
 	island.h
 	joint.c
@@ -44,7 +43,10 @@ set(BOX2D_SOURCE_FILES
 	revolute_joint.c
 	shape.c
 	shape.h
-	solver_data.h
+	solver.c
+	solver.h
+	stack_allocator.c
+	stack_allocator.h
 	table.c
 	table.h
 	timer.c
@@ -70,10 +72,10 @@ set(BOX2D_API_FILES
 	../include/box2d/hull.h
 	../include/box2d/id.h
 	../include/box2d/joint_types.h
-	../include/box2d/joint_util.h
 	../include/box2d/manifold.h
 	../include/box2d/math.h
 	../include/box2d/math_cpp.h
+	../include/box2d/math_types.h
 	../include/box2d/timer.h
 	../include/box2d/types.h
 )
diff --git a/src/aabb.c b/src/aabb.c
index 4ae9533e..c53c7aa4 100644
--- a/src/aabb.c
+++ b/src/aabb.c
@@ -4,6 +4,7 @@
 #include "aabb.h"
 
 #include "box2d/constants.h"
+#include "box2d/geometry.h"
 #include "box2d/math.h"
 
 #include <float.h>
@@ -17,10 +18,10 @@ bool b2AABB_IsValid(b2AABB a)
 }
 
 // From Real-time Collision Detection, p179.
-b2RayCastOutput b2AABB_RayCast(b2AABB a, b2Vec2 p1, b2Vec2 p2)
+b2CastOutput b2AABB_RayCast(b2AABB a, b2Vec2 p1, b2Vec2 p2)
 {
 	// Radius not handled
-	b2RayCastOutput output = {0};
+	b2CastOutput output = {0};
 
 	float tmin = -FLT_MAX;
 	float tmax = FLT_MAX;
diff --git a/src/aabb.h b/src/aabb.h
index 859bbcb4..d43e7cd9 100644
--- a/src/aabb.h
+++ b/src/aabb.h
@@ -5,10 +5,10 @@
 
 #include "box2d/constants.h"
 #include "box2d/math.h"
-#include "box2d/types.h"
+#include "box2d/geometry.h"
 
 // Ray cast an AABB
-b2RayCastOutput b2AABB_RayCast(b2AABB a, b2Vec2 p1, b2Vec2 p2);
+b2CastOutput b2AABB_RayCast(b2AABB a, b2Vec2 p1, b2Vec2 p2);
 
 // Get the perimeter length
 static inline float b2Perimeter(b2AABB a)
diff --git a/src/body.c b/src/body.c
index 8b613433..38f69842 100644
--- a/src/body.c
+++ b/src/body.c
@@ -7,9 +7,9 @@
 #include "allocate.h"
 #include "array.h"
 #include "block_allocator.h"
+#include "constraint_graph.h"
 #include "contact.h"
 #include "core.h"
-#include "graph.h"
 #include "island.h"
 #include "joint.h"
 #include "shape.h"
@@ -1223,6 +1223,19 @@ b2MassData b2Body_GetMassData(b2BodyId bodyId)
 	return massData;
 }
 
+void b2Body_ResetMassData(b2BodyId bodyId)
+{
+	b2World* world = b2GetWorldFromIndexLocked(bodyId.world);
+	if (world == NULL)
+	{
+		return;
+	}
+
+	b2Body* body = b2GetBody(world, bodyId);
+
+	b2UpdateBodyMassData(world, body);
+}
+
 void b2Body_SetLinearDamping(b2BodyId bodyId, float linearDamping)
 {
 	B2_ASSERT(b2IsValid(linearDamping) && linearDamping >= 0.0f);
diff --git a/src/broad_phase.c b/src/broad_phase.c
index 93a2b5c6..630b8e5b 100644
--- a/src/broad_phase.c
+++ b/src/broad_phase.c
@@ -7,7 +7,7 @@
 
 #include "aabb.h"
 #include "allocate.h"
-#include "arena_allocator.h"
+#include "stack_allocator.h"
 #include "array.h"
 #include "body.h"
 #include "contact.h"
diff --git a/src/broad_phase.h b/src/broad_phase.h
index b8883bb6..55293903 100644
--- a/src/broad_phase.h
+++ b/src/broad_phase.h
@@ -7,6 +7,7 @@
 #include "table.h"
 
 #include "box2d/dynamic_tree.h"
+#include "box2d/types.h"
 
 typedef struct b2Shape b2Shape;
 typedef struct b2MovePair b2MovePair;
diff --git a/src/constraint_graph.c b/src/constraint_graph.c
new file mode 100644
index 00000000..c36e0407
--- /dev/null
+++ b/src/constraint_graph.c
@@ -0,0 +1,401 @@
+// SPDX-FileCopyrightText: 2023 Erin Catto
+// SPDX-License-Identifier: MIT
+
+#include "constraint_graph.h"
+
+#include "array.h"
+#include "bitset.inl"
+#include "body.h"
+#include "contact.h"
+#include "joint.h"
+#include "world.h"
+
+#include <stdatomic.h>
+
+// Solver using graph coloring. Islands are only used for sleep.
+// High-Performance Physical Simulations on Next-Generation Architecture with Many Cores
+// http://web.eecs.umich.edu/~msmelyan/papers/physsim_onmanycore_itj.pdf
+
+// Kinematic bodies have to be treated like dynamic bodies in graph coloring. Unlike static bodies, we cannot use a dummy solver
+// body for kinematic bodies. We cannot access a kinematic body from multiple threads efficiently because the SIMD solver body
+// scatter would write to the same kinematic body from multiple threads. Even if these writes don't modify the body, they will
+// cause horrible cache stalls. To make this feasible I would need a way to block these writes.
+
+// This is used for debugging by making all constraints be assigned to overflow.
+#define B2_FORCE_OVERFLOW 0
+
+void b2CreateGraph(b2ConstraintGraph* graph, int32_t bodyCapacity, int32_t contactCapacity, int32_t jointCapacity)
+{
+	*graph = (b2ConstraintGraph){0};
+
+	bodyCapacity = B2_MAX(bodyCapacity, 8);
+	contactCapacity = B2_MAX(contactCapacity, 8);
+	jointCapacity = B2_MAX(jointCapacity, 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);
+		color->jointArray = b2CreateArray(sizeof(int32_t), jointCapacity);
+		color->contactConstraints = NULL;
+	}
+
+	graph->overflow.contactArray = b2CreateArray(sizeof(int32_t), contactCapacity);
+	graph->overflow.jointArray = b2CreateArray(sizeof(int32_t), jointCapacity);
+	graph->overflow.contactConstraints = NULL;
+}
+
+void b2DestroyGraph(b2ConstraintGraph* graph)
+{
+	for (int32_t i = 0; i < b2_graphColorCount; ++i)
+	{
+		b2GraphColor* color = graph->colors + i;
+		b2DestroyBitSet(&color->bodySet);
+		b2DestroyArray(color->contactArray, sizeof(int32_t));
+		b2DestroyArray(color->jointArray, sizeof(int32_t));
+	}
+
+	b2DestroyArray(graph->overflow.contactArray, sizeof(int32_t));
+	b2DestroyArray(graph->overflow.jointArray, sizeof(int32_t));
+}
+
+void b2AddContactToGraph(b2World* world, b2Contact* contact)
+{
+	B2_ASSERT(contact->colorIndex == B2_NULL_INDEX);
+	B2_ASSERT(contact->colorSubIndex == B2_NULL_INDEX);
+
+	b2ConstraintGraph* graph = &world->graph;
+
+#if B2_FORCE_OVERFLOW == 0
+	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;
+	B2_ASSERT(typeA != b2_staticBody || typeB != b2_staticBody);
+
+	if (typeA != b2_staticBody && typeB != b2_staticBody)
+	{
+		for (int32_t i = 0; i < b2_graphColorCount; ++i)
+		{
+			b2GraphColor* color = graph->colors + i;
+			if (b2GetBit(&color->bodySet, bodyIndexA) || b2GetBit(&color->bodySet, bodyIndexB))
+			{
+				continue;
+			}
+
+			b2SetBitGrow(&color->bodySet, bodyIndexA);
+			b2SetBitGrow(&color->bodySet, bodyIndexB);
+
+			contact->colorSubIndex = b2Array(color->contactArray).count;
+			b2Array_Push(color->contactArray, contact->object.index);
+			contact->colorIndex = i;
+			break;
+		}
+	}
+	else if (typeA != b2_staticBody)
+	{
+		// Static contacts never in color 0
+		for (int32_t i = 1; i < b2_graphColorCount; ++i)
+		{
+			b2GraphColor* color = graph->colors + i;
+			if (b2GetBit(&color->bodySet, bodyIndexA))
+			{
+				continue;
+			}
+
+			b2SetBitGrow(&color->bodySet, bodyIndexA);
+
+			contact->colorSubIndex = b2Array(color->contactArray).count;
+			b2Array_Push(color->contactArray, contact->object.index);
+			contact->colorIndex = i;
+			break;
+		}
+	}
+	else if (typeB != b2_staticBody)
+	{
+		// Static contacts never in color 0
+		for (int32_t i = 1; i < b2_graphColorCount; ++i)
+		{
+			b2GraphColor* color = graph->colors + i;
+			if (b2GetBit(&color->bodySet, bodyIndexB))
+			{
+				continue;
+			}
+
+			b2SetBitGrow(&color->bodySet, bodyIndexB);
+
+			contact->colorSubIndex = b2Array(color->contactArray).count;
+			b2Array_Push(color->contactArray, contact->object.index);
+			contact->colorIndex = i;
+			break;
+		}
+	}
+#endif
+
+	// Overflow
+	if (contact->colorIndex == B2_NULL_INDEX)
+	{
+		contact->colorSubIndex = b2Array(graph->overflow.contactArray).count;
+		b2Array_Push(graph->overflow.contactArray, contact->object.index);
+		contact->colorIndex = b2_overflowIndex;
+	}
+}
+
+void b2RemoveContactFromGraph(b2World* world, b2Contact* contact)
+{
+	B2_ASSERT(contact->colorIndex != B2_NULL_INDEX);
+	B2_ASSERT(contact->colorSubIndex != B2_NULL_INDEX);
+
+	b2ConstraintGraph* graph = &world->graph;
+
+	// Overflow
+	if (contact->colorIndex == b2_overflowIndex)
+	{
+		int32_t colorSubIndex = contact->colorSubIndex;
+		b2Array_RemoveSwap(graph->overflow.contactArray, colorSubIndex);
+		if (colorSubIndex < b2Array(graph->overflow.contactArray).count)
+		{
+			// Fix index on swapped contact
+			int32_t swappedIndex = graph->overflow.contactArray[colorSubIndex];
+			B2_ASSERT(world->contacts[swappedIndex].colorIndex == b2_overflowIndex);
+			world->contacts[swappedIndex].colorSubIndex = colorSubIndex;
+		}
+
+		contact->colorIndex = B2_NULL_INDEX;
+		contact->colorSubIndex = 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;
+	B2_ASSERT(typeA != b2_staticBody || typeB != b2_staticBody);
+
+	if (typeA != b2_staticBody && typeB != b2_staticBody)
+	{
+		b2GraphColor* color = graph->colors + contact->colorIndex;
+		B2_ASSERT(b2GetBit(&color->bodySet, bodyIndexA) && b2GetBit(&color->bodySet, bodyIndexB));
+
+		int32_t colorSubIndex = contact->colorSubIndex;
+		b2Array_RemoveSwap(color->contactArray, colorSubIndex);
+		if (colorSubIndex < b2Array(color->contactArray).count)
+		{
+			// Fix index on swapped contact
+			int32_t swappedIndex = color->contactArray[colorSubIndex];
+			world->contacts[swappedIndex].colorSubIndex = colorSubIndex;
+		}
+
+		b2ClearBit(&color->bodySet, bodyIndexA);
+		b2ClearBit(&color->bodySet, bodyIndexB);
+	}
+	else if (typeA != b2_staticBody)
+	{
+		b2GraphColor* color = graph->colors + contact->colorIndex;
+		B2_ASSERT(b2GetBit(&color->bodySet, bodyIndexA));
+
+		int32_t colorSubIndex = contact->colorSubIndex;
+		b2Array_RemoveSwap(color->contactArray, colorSubIndex);
+		if (colorSubIndex < b2Array(color->contactArray).count)
+		{
+			// Fix index on swapped contact
+			int32_t swappedIndex = color->contactArray[colorSubIndex];
+			world->contacts[swappedIndex].colorSubIndex = colorSubIndex;
+		}
+
+		b2ClearBit(&color->bodySet, bodyIndexA);
+	}
+	else if (typeB != b2_staticBody)
+	{
+		b2GraphColor* color = graph->colors + contact->colorIndex;
+		B2_ASSERT(b2GetBit(&color->bodySet, bodyIndexB));
+
+		int32_t colorSubIndex = contact->colorSubIndex;
+		b2Array_RemoveSwap(color->contactArray, colorSubIndex);
+		if (colorSubIndex < b2Array(color->contactArray).count)
+		{
+			// Fix index on swapped contact
+			int32_t swappedIndex = color->contactArray[colorSubIndex];
+			world->contacts[swappedIndex].colorSubIndex = colorSubIndex;
+		}
+
+		b2ClearBit(&color->bodySet, bodyIndexB);
+	}
+
+	contact->colorIndex = B2_NULL_INDEX;
+	contact->colorSubIndex = B2_NULL_INDEX;
+}
+
+void b2AddJointToGraph(b2World* world, b2Joint* joint)
+{
+	B2_ASSERT(joint->colorIndex == B2_NULL_INDEX);
+	B2_ASSERT(joint->colorSubIndex == B2_NULL_INDEX);
+
+	b2ConstraintGraph* graph = &world->graph;
+
+#if B2_FORCE_OVERFLOW == 0
+	int32_t bodyIndexA = joint->edges[0].bodyIndex;
+	int32_t bodyIndexB = joint->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;
+			}
+
+			b2SetBitGrow(&color->bodySet, bodyIndexA);
+			b2SetBitGrow(&color->bodySet, bodyIndexB);
+
+			joint->colorSubIndex = b2Array(color->jointArray).count;
+			b2Array_Push(color->jointArray, joint->object.index);
+			joint->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;
+			}
+
+			b2SetBitGrow(&color->bodySet, bodyIndexA);
+
+			joint->colorSubIndex = b2Array(color->jointArray).count;
+			b2Array_Push(color->jointArray, joint->object.index);
+			joint->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;
+			}
+
+			b2SetBitGrow(&color->bodySet, bodyIndexB);
+
+			joint->colorSubIndex = b2Array(color->jointArray).count;
+			b2Array_Push(color->jointArray, joint->object.index);
+			joint->colorIndex = i;
+			break;
+		}
+	}
+#endif
+
+	// Overflow
+	if (joint->colorIndex == B2_NULL_INDEX)
+	{
+		joint->colorSubIndex = b2Array(graph->overflow.jointArray).count;
+		b2Array_Push(graph->overflow.jointArray, joint->object.index);
+		joint->colorIndex = b2_overflowIndex;
+	}
+}
+
+void b2RemoveJointFromGraph(b2World* world, b2Joint* joint)
+{
+	B2_ASSERT(joint->colorIndex != B2_NULL_INDEX);
+	B2_ASSERT(joint->colorSubIndex != B2_NULL_INDEX);
+
+	b2ConstraintGraph* graph = &world->graph;
+
+	// Overflow
+	if (joint->colorIndex == b2_overflowIndex)
+	{
+		int32_t colorSubIndex = joint->colorSubIndex;
+		b2Array_RemoveSwap(graph->overflow.jointArray, colorSubIndex);
+		if (colorSubIndex < b2Array(graph->overflow.jointArray).count)
+		{
+			// Fix index on swapped joint
+			int32_t swappedIndex = graph->overflow.jointArray[colorSubIndex];
+			B2_ASSERT(world->joints[swappedIndex].colorIndex == b2_overflowIndex);
+			world->joints[swappedIndex].colorSubIndex = colorSubIndex;
+		}
+
+		joint->colorIndex = B2_NULL_INDEX;
+		joint->colorSubIndex = B2_NULL_INDEX;
+
+		return;
+	}
+
+	B2_ASSERT(0 <= joint->colorIndex && joint->colorIndex < b2_graphColorCount);
+	int32_t bodyIndexA = joint->edges[0].bodyIndex;
+	int32_t bodyIndexB = joint->edges[1].bodyIndex;
+
+	b2BodyType typeA = world->bodies[bodyIndexA].type;
+	b2BodyType typeB = world->bodies[bodyIndexB].type;
+
+	if (typeA == b2_dynamicBody && typeB == b2_dynamicBody)
+	{
+		b2GraphColor* color = graph->colors + joint->colorIndex;
+		B2_ASSERT(b2GetBit(&color->bodySet, bodyIndexA) && b2GetBit(&color->bodySet, bodyIndexB));
+
+		int32_t colorSubIndex = joint->colorSubIndex;
+		b2Array_RemoveSwap(color->jointArray, colorSubIndex);
+		if (colorSubIndex < b2Array(color->jointArray).count)
+		{
+			// Fix index on swapped joint
+			int32_t swappedIndex = color->jointArray[colorSubIndex];
+			world->joints[swappedIndex].colorSubIndex = colorSubIndex;
+		}
+
+		b2ClearBit(&color->bodySet, bodyIndexA);
+		b2ClearBit(&color->bodySet, bodyIndexB);
+	}
+	else if (typeA == b2_dynamicBody)
+	{
+		b2GraphColor* color = graph->colors + joint->colorIndex;
+		B2_ASSERT(b2GetBit(&color->bodySet, bodyIndexA));
+
+		int32_t colorSubIndex = joint->colorSubIndex;
+		b2Array_RemoveSwap(color->jointArray, colorSubIndex);
+		if (colorSubIndex < b2Array(color->jointArray).count)
+		{
+			// Fix index on swapped joint
+			int32_t swappedIndex = color->jointArray[colorSubIndex];
+			world->joints[swappedIndex].colorSubIndex = colorSubIndex;
+		}
+
+		b2ClearBit(&color->bodySet, bodyIndexA);
+	}
+	else if (typeB == b2_dynamicBody)
+	{
+		b2GraphColor* color = graph->colors + joint->colorIndex;
+		B2_ASSERT(b2GetBit(&color->bodySet, bodyIndexB));
+
+		int32_t colorSubIndex = joint->colorSubIndex;
+		b2Array_RemoveSwap(color->jointArray, colorSubIndex);
+		if (colorSubIndex < b2Array(color->jointArray).count)
+		{
+			// Fix index on swapped joint
+			int32_t swappedIndex = color->jointArray[colorSubIndex];
+			world->joints[swappedIndex].colorSubIndex = colorSubIndex;
+		}
+
+		b2ClearBit(&color->bodySet, bodyIndexB);
+	}
+
+	joint->colorIndex = B2_NULL_INDEX;
+	joint->colorSubIndex = B2_NULL_INDEX;
+}
diff --git a/src/graph.h b/src/constraint_graph.h
similarity index 96%
rename from src/graph.h
rename to src/constraint_graph.h
index 2f65d5c7..f8b3464d 100644
--- a/src/graph.h
+++ b/src/constraint_graph.h
@@ -56,5 +56,3 @@ void b2RemoveContactFromGraph(b2World* world, b2Contact* contact);
 
 void b2AddJointToGraph(b2World* world, b2Joint* joint);
 void b2RemoveJointFromGraph(b2World* world, b2Joint* joint);
-
-void b2Solve(b2World* world, b2StepContext* stepContext);
diff --git a/src/contact_solver.c b/src/contact_solver.c
index 89eb730b..da09250c 100644
--- a/src/contact_solver.c
+++ b/src/contact_solver.c
@@ -5,14 +5,14 @@
 
 #include "array.h"
 #include "body.h"
+#include "constraint_graph.h"
 #include "contact.h"
 #include "core.h"
-#include "graph.h"
 #include "world.h"
 #include "x86/avx2.h"
 #include "x86/fma.h"
 
-// Soft constraints with constraint error sub-stepping. Includes a relax stage to help remove excess energy.
+// Soft contact constraints with sub-stepping support
 // http://mmacklin.com/smallsteps.pdf
 // https://box2d.org/files/ErinCatto_SoftConstraints_GDC2011.pdf
 
diff --git a/src/contact_solver.h b/src/contact_solver.h
index 2b63671b..d93fb811 100644
--- a/src/contact_solver.h
+++ b/src/contact_solver.h
@@ -3,12 +3,11 @@
 
 #pragma once
 
-#include "solver_data.h"
+#include "solver.h"
 
-// todo this could be hidden in contact_solver.c, then graph.c just needs to know the sizeof(b2ContactConstraintSIMD)
-#include "x86/avx.h"
+#include "box2d/math_types.h"
 
-#include "box2d/math.h"
+#include "x86/avx.h"
 
 typedef struct b2Contact b2Contact;
 
diff --git a/src/distance.c b/src/distance.c
index 9cf8e38b..5734cd30 100644
--- a/src/distance.c
+++ b/src/distance.c
@@ -502,7 +502,7 @@ int32_t b2_gjkIters;
 int32_t b2_gjkMaxIters;
 #endif
 
-b2DistanceOutput b2ShapeDistance(b2DistanceCache* B2_RESTRICT cache, const b2DistanceInput* B2_RESTRICT input)
+b2DistanceOutput b2ShapeDistance(b2DistanceCache* cache, const b2DistanceInput* input)
 {
 #if B2_GJK_DEBUG
 	++b2_gjkCalls;
@@ -659,9 +659,9 @@ b2DistanceOutput b2ShapeDistance(b2DistanceCache* B2_RESTRICT cache, const b2Dis
 // Algorithm by Gino van den Bergen.
 // "Smooth Mesh Contacts with GJK" in Game Physics Pearls. 2010
 // TODO_ERIN this is failing when used to raycast a box
-b2RayCastOutput b2ShapeCast(const b2ShapeCastPairInput* input)
+b2CastOutput b2ShapeCast(const b2ShapeCastPairInput* input)
 {
-	b2RayCastOutput output = {0};
+	b2CastOutput output = {0};
 
 	const b2DistanceProxy* proxyA = &input->proxyA;
 	const b2DistanceProxy* proxyB = &input->proxyB;
diff --git a/src/distance_joint.c b/src/distance_joint.c
index 0ac53804..52d7e672 100644
--- a/src/distance_joint.c
+++ b/src/distance_joint.c
@@ -6,7 +6,7 @@
 #include "body.h"
 #include "core.h"
 #include "joint.h"
-#include "solver_data.h"
+#include "solver.h"
 #include "world.h"
 
 // needed for dll export
@@ -16,14 +16,6 @@
 
 #include <stdio.h>
 
-b2DistanceJointDef b2DefaultDistanceJointDef()
-{
-	b2DistanceJointDef def = {0};
-	def.length = 1.0f;
-	def.maxLength = b2_huge;
-	return def;
-}
-
 // 1-D constrained system
 // m (v2 - v1) = lambda
 // v2 + (beta/h) * x1 + gamma * lambda = 0, gamma has units of inverse mass.
diff --git a/src/geometry.c b/src/geometry.c
index 5b07700d..d72d2d30 100644
--- a/src/geometry.c
+++ b/src/geometry.c
@@ -472,13 +472,13 @@ bool b2PointInPolygon(b2Vec2 point, const b2Polygon* shape)
 
 // Precision Improvements for Ray / Sphere Intersection - Ray Tracing Gems 2019
 // http://www.codercorner.com/blog/?p=321
-b2RayCastOutput b2RayCastCircle(const b2RayCastInput* input, const b2Circle* shape)
+b2CastOutput b2RayCastCircle(const b2RayCastInput* input, const b2Circle* shape)
 {
 	B2_ASSERT(b2IsValidRay(input));
 
 	b2Vec2 p = shape->point;
 
-	b2RayCastOutput output = {0};
+	b2CastOutput output = {0};
 
 	// Shift ray so circle center is the origin
 	b2Vec2 s = b2Sub(input->origin, p);
@@ -529,11 +529,11 @@ b2RayCastOutput b2RayCastCircle(const b2RayCastInput* input, const b2Circle* sha
 	return output;
 }
 
-b2RayCastOutput b2RayCastCapsule(const b2RayCastInput* input, const b2Capsule* shape)
+b2CastOutput b2RayCastCapsule(const b2RayCastInput* input, const b2Capsule* shape)
 {
 	B2_ASSERT(b2IsValidRay(input));
 
-	b2RayCastOutput output = {0};
+	b2CastOutput output = {0};
 
 	b2Vec2 v1 = shape->point1;
 	b2Vec2 v2 = shape->point2;
@@ -663,7 +663,7 @@ b2RayCastOutput b2RayCastCapsule(const b2RayCastInput* input, const b2Capsule* s
 }
 
 // Ray vs line segment
-b2RayCastOutput b2RayCastSegment(const b2RayCastInput* input, const b2Segment* shape, bool oneSided)
+b2CastOutput b2RayCastSegment(const b2RayCastInput* input, const b2Segment* shape, bool oneSided)
 {
 	if (oneSided)
 	{
@@ -671,7 +671,7 @@ b2RayCastOutput b2RayCastSegment(const b2RayCastInput* input, const b2Segment* s
 		float offset = b2Cross(b2Sub(input->origin, shape->point1), b2Sub(shape->point2, shape->point1));
 		if (offset < 0.0f)
 		{
-			b2RayCastOutput output = {0};
+			b2CastOutput output = {0};
 			return output;
 		}
 	}
@@ -684,7 +684,7 @@ b2RayCastOutput b2RayCastSegment(const b2RayCastInput* input, const b2Segment* s
 	b2Vec2 v2 = shape->point2;
 	b2Vec2 e = b2Sub(v2, v1);
 
-	b2RayCastOutput output = {0};
+	b2CastOutput output = {0};
 
 	float length;
 	b2Vec2 eUnit = b2GetLengthAndNormalize(&length, e);
@@ -744,7 +744,7 @@ b2RayCastOutput b2RayCastSegment(const b2RayCastInput* input, const b2Segment* s
 	return output;
 }
 
-b2RayCastOutput b2RayCastPolygon(const b2RayCastInput* input, const b2Polygon* shape)
+b2CastOutput b2RayCastPolygon(const b2RayCastInput* input, const b2Polygon* shape)
 {
 	B2_ASSERT(b2IsValidRay(input));
 
@@ -758,7 +758,7 @@ b2RayCastOutput b2RayCastPolygon(const b2RayCastInput* input, const b2Polygon* s
 
 		int32_t index = -1;
 
-		b2RayCastOutput output = {0};
+		b2CastOutput output = {0};
 
 		for (int32_t i = 0; i < shape->count; ++i)
 		{
@@ -830,7 +830,7 @@ b2RayCastOutput b2RayCastPolygon(const b2RayCastInput* input, const b2Polygon* s
 	return b2ShapeCast(&castInput);
 }
 
-b2RayCastOutput b2ShapeCastCircle(const b2ShapeCastInput* input, const b2Circle* shape)
+b2CastOutput b2ShapeCastCircle(const b2ShapeCastInput* input, const b2Circle* shape)
 {
 	b2ShapeCastPairInput pairInput;
 	pairInput.proxyA = b2MakeProxy(&shape->point, 1, shape->radius);
@@ -840,11 +840,11 @@ b2RayCastOutput b2ShapeCastCircle(const b2ShapeCastInput* input, const b2Circle*
 	pairInput.translationB = input->translation;
 	pairInput.maxFraction = input->maxFraction;
 
-	b2RayCastOutput output = b2ShapeCast(&pairInput);
+	b2CastOutput output = b2ShapeCast(&pairInput);
 	return output;
 }
 
-b2RayCastOutput b2ShapeCastCapsule(const b2ShapeCastInput* input, const b2Capsule* shape)
+b2CastOutput b2ShapeCastCapsule(const b2ShapeCastInput* input, const b2Capsule* shape)
 {
 	b2ShapeCastPairInput pairInput;
 	pairInput.proxyA = b2MakeProxy(&shape->point1, 2, shape->radius);
@@ -854,11 +854,11 @@ b2RayCastOutput b2ShapeCastCapsule(const b2ShapeCastInput* input, const b2Capsul
 	pairInput.translationB = input->translation;
 	pairInput.maxFraction = input->maxFraction;
 
-	b2RayCastOutput output = b2ShapeCast(&pairInput);
+	b2CastOutput output = b2ShapeCast(&pairInput);
 	return output;
 }
 
-b2RayCastOutput b2ShapeCastSegment(const b2ShapeCastInput* input, const b2Segment* shape)
+b2CastOutput b2ShapeCastSegment(const b2ShapeCastInput* input, const b2Segment* shape)
 {
 	b2ShapeCastPairInput pairInput;
 	pairInput.proxyA = b2MakeProxy(&shape->point1, 2, 0.0f);
@@ -868,11 +868,11 @@ b2RayCastOutput b2ShapeCastSegment(const b2ShapeCastInput* input, const b2Segmen
 	pairInput.translationB = input->translation;
 	pairInput.maxFraction = input->maxFraction;
 
-	b2RayCastOutput output = b2ShapeCast(&pairInput);
+	b2CastOutput output = b2ShapeCast(&pairInput);
 	return output;
 }
 
-b2RayCastOutput b2ShapeCastPolygon(const b2ShapeCastInput* input, const b2Polygon* shape)
+b2CastOutput b2ShapeCastPolygon(const b2ShapeCastInput* input, const b2Polygon* shape)
 {
 	b2ShapeCastPairInput pairInput;
 	pairInput.proxyA = b2MakeProxy(shape->vertices, shape->count, shape->radius);
@@ -882,6 +882,6 @@ b2RayCastOutput b2ShapeCastPolygon(const b2ShapeCastInput* input, const b2Polygo
 	pairInput.translationB = input->translation;
 	pairInput.maxFraction = input->maxFraction;
 
-	b2RayCastOutput output = b2ShapeCast(&pairInput);
+	b2CastOutput output = b2ShapeCast(&pairInput);
 	return output;
 }
diff --git a/src/implementation.c b/src/implementation.c
new file mode 100644
index 00000000..a7101341
--- /dev/null
+++ b/src/implementation.c
@@ -0,0 +1,8 @@
+// SPDX-FileCopyrightText: 2023 Erin Catto
+// SPDX-License-Identifier: MIT
+
+#define BOX2D_IMPLEMENTATION
+
+#include "box2d/color.h"
+#include "box2d/dynamic_tree.h"
+#include "box2d/math.h"
diff --git a/src/island.c b/src/island.c
index b796fd92..55e1ad57 100644
--- a/src/island.c
+++ b/src/island.c
@@ -4,14 +4,14 @@
 #include "island.h"
 
 #include "aabb.h"
-#include "arena_allocator.h"
+#include "stack_allocator.h"
 #include "array.h"
 #include "body.h"
 #include "contact.h"
 #include "core.h"
 #include "joint.h"
 #include "shape.h"
-#include "solver_data.h"
+#include "solver.h"
 #include "world.h"
 
 #include "box2d/callbacks.h"
diff --git a/src/joint.c b/src/joint.c
index b36a3eef..18339a04 100644
--- a/src/joint.c
+++ b/src/joint.c
@@ -7,7 +7,7 @@
 #include "contact.h"
 #include "core.h"
 #include "shape.h"
-#include "solver_data.h"
+#include "solver.h"
 #include "world.h"
 
 // needed for dll export
@@ -15,7 +15,60 @@
 #include "box2d/color.h"
 #include "box2d/debug_draw.h"
 #include "box2d/joint_types.h"
-#include "box2d/joint_util.h"
+
+b2DistanceJointDef b2DefaultDistanceJointDef()
+{
+	b2DistanceJointDef def = {0};
+	def.length = 1.0f;
+	def.maxLength = b2_huge;
+	return def;
+}
+
+b2MotorJointDef b2DefaultMotorJointDef()
+{
+	b2MotorJointDef def = {0};
+	def.maxForce = 1.0f;
+	def.maxTorque = 1.0f;
+	def.correctionFactor = 0.3f;
+	return def;
+}
+
+b2MouseJointDef b2DefaultMouseJointDef()
+{
+	b2MouseJointDef def = {0};
+	def.hertz = 4.0f;
+	def.dampingRatio = 1.0f;
+	return def;
+}
+
+b2PrismaticJointDef b2DefaultPrismaticJointDef()
+{
+	b2PrismaticJointDef def = {0};
+	def.localAxisA = (b2Vec2){1.0f, 0.0f};
+	return def;
+}
+
+b2RevoluteJointDef b2DefaultRevoluteJointDef()
+{
+	b2RevoluteJointDef def = {0};
+	def.drawSize = 0.25f;
+	return def;
+}
+
+b2WeldJointDef b2DefaultWeldJointDef()
+{
+	b2WeldJointDef def = {0};
+	return def;
+}
+
+b2WheelJointDef b2DefaultWheelJointDef()
+{
+	b2WheelJointDef def = {0};
+	def.localAxisA.y = 1.0f;
+	def.hertz = 1.0f;
+	def.dampingRatio = 0.7f;
+	return def;
+}
 
 // Get joint from id with validation
 b2Joint* b2GetJointCheckType(b2JointId id, b2JointType type)
@@ -47,72 +100,6 @@ b2Joint* b2GetJoint(b2World* world, b2JointId jointId)
 	return joint;
 }
 
-void b2LinearStiffness(float* stiffness, float* damping, float frequencyHertz, float dampingRatio, b2BodyId bodyIdA,
-					   b2BodyId bodyIdB)
-{
-	B2_ASSERT(bodyIdA.world == bodyIdB.world);
-
-	b2World* world = b2GetWorldFromIndex(bodyIdA.world);
-	B2_ASSERT(0 <= bodyIdA.index && bodyIdA.index < world->bodyPool.capacity);
-	B2_ASSERT(0 <= bodyIdB.index && bodyIdB.index < world->bodyPool.capacity);
-
-	b2Body* bodyA = world->bodies + bodyIdA.index;
-	b2Body* bodyB = world->bodies + bodyIdB.index;
-
-	float massA = bodyA->mass;
-	float massB = bodyB->mass;
-	float mass;
-	if (massA > 0.0f && massB > 0.0f)
-	{
-		mass = massA * massB / (massA + massB);
-	}
-	else if (massA > 0.0f)
-	{
-		mass = massA;
-	}
-	else
-	{
-		mass = massB;
-	}
-
-	float omega = 2.0f * b2_pi * frequencyHertz;
-	*stiffness = mass * omega * omega;
-	*damping = 2.0f * mass * dampingRatio * omega;
-}
-
-void b2AngularStiffness(float* stiffness, float* damping, float frequencyHertz, float dampingRatio, b2BodyId bodyIdA,
-						b2BodyId bodyIdB)
-{
-	B2_ASSERT(bodyIdA.world == bodyIdB.world);
-
-	b2World* world = b2GetWorldFromIndex(bodyIdA.world);
-	B2_ASSERT(0 <= bodyIdA.index && bodyIdA.index < world->bodyPool.capacity);
-	B2_ASSERT(0 <= bodyIdB.index && bodyIdB.index < world->bodyPool.capacity);
-
-	b2Body* bodyA = world->bodies + bodyIdA.index;
-	b2Body* bodyB = world->bodies + bodyIdB.index;
-
-	float IA = bodyA->I;
-	float IB = bodyB->I;
-	float I;
-	if (IA > 0.0f && IB > 0.0f)
-	{
-		I = IA * IB / (IA + IB);
-	}
-	else if (IA > 0.0f)
-	{
-		I = IA;
-	}
-	else
-	{
-		I = IB;
-	}
-
-	float omega = 2.0f * b2_pi * frequencyHertz;
-	*stiffness = I * omega * omega;
-	*damping = 2.0f * I * dampingRatio * omega;
-}
-
 static b2Joint* b2CreateJoint(b2World* world, b2Body* bodyA, b2Body* bodyB)
 {
 	b2Joint* joint = (b2Joint*)b2AllocObject(&world->jointPool);
diff --git a/src/joint.h b/src/joint.h
index 3e3ffcfd..0804b6fa 100644
--- a/src/joint.h
+++ b/src/joint.h
@@ -3,7 +3,7 @@
 #pragma once
 
 #include "pool.h"
-#include "solver_data.h"
+#include "solver.h"
 
 #include "box2d/joint_types.h"
 
diff --git a/src/motor_joint.c b/src/motor_joint.c
index 7b72e267..5cd637d7 100644
--- a/src/motor_joint.c
+++ b/src/motor_joint.c
@@ -4,22 +4,12 @@
 #include "body.h"
 #include "core.h"
 #include "joint.h"
-#include "solver_data.h"
+#include "solver.h"
 #include "world.h"
 
 // needed for dll export
 #include "box2d/box2d.h"
 #include "box2d/debug_draw.h"
-#include "box2d/joint_types.h"
-
-b2MotorJointDef b2DefaultMotorJointDef()
-{
-	b2MotorJointDef def = {0};
-	def.maxForce = 1.0f;
-	def.maxTorque = 1.0f;
-	def.correctionFactor = 0.3f;
-	return def;
-}
 
 // Point-to-point constraint
 // C = p2 - p1
diff --git a/src/mouse_joint.c b/src/mouse_joint.c
index cd0cb5ea..78e6a70b 100644
--- a/src/mouse_joint.c
+++ b/src/mouse_joint.c
@@ -4,20 +4,11 @@
 #include "body.h"
 #include "core.h"
 #include "joint.h"
-#include "solver_data.h"
+#include "solver.h"
 #include "world.h"
 
 // needed for dll export
 #include "box2d/box2d.h"
-#include "box2d/joint_types.h"
-
-b2MouseJointDef b2DefaultMouseJointDef()
-{
-	b2MouseJointDef def = {0};
-	def.hertz = 4.0f;
-	def.dampingRatio = 1.0f;
-	return def;
-}
 
 void b2MouseJoint_SetTarget(b2JointId jointId, b2Vec2 target)
 {
diff --git a/src/prismatic_joint.c b/src/prismatic_joint.c
index 29931075..40fa6213 100644
--- a/src/prismatic_joint.c
+++ b/src/prismatic_joint.c
@@ -4,23 +4,15 @@
 #include "body.h"
 #include "core.h"
 #include "joint.h"
-#include "solver_data.h"
+#include "solver.h"
 #include "world.h"
 
 // needed for dll export
 #include "box2d/box2d.h"
 #include "box2d/debug_draw.h"
-#include "box2d/joint_types.h"
 
 #include <stdio.h>
 
-b2PrismaticJointDef b2DefaultPrismaticJointDef()
-{
-	b2PrismaticJointDef def = {0};
-	def.localAxisA = (b2Vec2){1.0f, 0.0f};
-	return def;
-}
-
 // Linear constraint (point-to-line)
 // d = p2 - p1 = x2 + r2 - x1 - r1
 // C = dot(perp, d)
@@ -202,7 +194,7 @@ void b2SolvePrismaticJoint(b2Joint* base, b2StepContext* context, bool useBias)
 		float Cdot = b2Dot(axisA, b2Sub(vB, vA)) + a2 * wB - a1 * wA;
 		float impulse = joint->axialMass * (joint->motorSpeed - Cdot);
 		float oldImpulse = joint->motorImpulse;
-		float maxImpulse = context->dt * joint->maxMotorForce;
+		float maxImpulse = context->h * joint->maxMotorForce;
 		joint->motorImpulse = B2_CLAMP(joint->motorImpulse + impulse, -maxImpulse, maxImpulse);
 		impulse = joint->motorImpulse - oldImpulse;
 
diff --git a/src/revolute_joint.c b/src/revolute_joint.c
index 467637ca..ace7064e 100644
--- a/src/revolute_joint.c
+++ b/src/revolute_joint.c
@@ -6,23 +6,15 @@
 #include "body.h"
 #include "core.h"
 #include "joint.h"
-#include "solver_data.h"
+#include "solver.h"
 #include "world.h"
 
 // needed for dll export
 #include "box2d/box2d.h"
 #include "box2d/debug_draw.h"
-#include "box2d/joint_types.h"
 
 #include <stdio.h>
 
-b2RevoluteJointDef b2DefaultRevoluteJointDef()
-{
-	b2RevoluteJointDef def = {0};
-	def.drawSize = 0.25f;
-	return def;
-}
-
 // Point-to-point constraint
 // C = p2 - p1
 // Cdot = v2 - v1
diff --git a/src/shape.c b/src/shape.c
index cb9ed8d8..d83572fd 100644
--- a/src/shape.c
+++ b/src/shape.c
@@ -138,13 +138,13 @@ b2ShapeExtent b2ComputeShapeExtent(const b2Shape* shape)
 	return extent;
 }
 
-b2RayCastOutput b2RayCastShape(const b2RayCastInput* input, const b2Shape* shape, b2Transform xf)
+b2CastOutput b2RayCastShape(const b2RayCastInput* input, const b2Shape* shape, b2Transform xf)
 {
 	b2RayCastInput localInput = *input;
 	localInput.origin = b2InvTransformPoint(xf, input->origin);
 	localInput.translation = b2InvRotateVector(xf.q, input->translation);
 
-	b2RayCastOutput output = {0};
+	b2CastOutput output = {0};
 	switch (shape->type)
 	{
 		case b2_capsuleShape:
@@ -171,7 +171,7 @@ b2RayCastOutput b2RayCastShape(const b2RayCastInput* input, const b2Shape* shape
 	return output;
 }
 
-b2RayCastOutput b2ShapeCastShape(const b2ShapeCastInput* input, const b2Shape* shape, b2Transform xf)
+b2CastOutput b2ShapeCastShape(const b2ShapeCastInput* input, const b2Shape* shape, b2Transform xf)
 {
 	b2ShapeCastInput localInput = *input;
 
@@ -182,7 +182,7 @@ b2RayCastOutput b2ShapeCastShape(const b2ShapeCastInput* input, const b2Shape* s
 
 	localInput.translation = b2InvRotateVector(xf.q, input->translation);
 
-	b2RayCastOutput output = {0};
+	b2CastOutput output = {0};
 	switch (shape->type)
 	{
 		case b2_capsuleShape:
@@ -329,11 +329,6 @@ void b2Shape_SetDensity(b2ShapeId shapeId, float density)
 	}
 
 	shape->density = density;
-
-	b2Body* body = world->bodies + shape->bodyIndex;
-	B2_ASSERT(b2ObjectValid(&body->object));
-
-	b2UpdateBodyMassData(world, body);
 }
 
 float b2Shape_GetDensity(b2ShapeId shapeId)
diff --git a/src/shape.h b/src/shape.h
index 2dbf3903..9fd2ef00 100644
--- a/src/shape.h
+++ b/src/shape.h
@@ -74,7 +74,7 @@ b2Vec2 b2GetShapeCentroid(const b2Shape* shape);
 
 b2DistanceProxy b2MakeShapeDistanceProxy(const b2Shape* shape);
 
-b2RayCastOutput b2RayCastShape(const b2RayCastInput* input, const b2Shape* shape, b2Transform xf);
-b2RayCastOutput b2ShapeCastShape(const b2ShapeCastInput* input, const b2Shape* shape, b2Transform xf);
+b2CastOutput b2RayCastShape(const b2RayCastInput* input, const b2Shape* shape, b2Transform xf);
+b2CastOutput b2ShapeCastShape(const b2ShapeCastInput* input, const b2Shape* shape, b2Transform xf);
 
 b2Shape* b2GetShape(b2World* world, b2ShapeId shapeId);
diff --git a/src/graph.c b/src/solver.c
similarity index 81%
rename from src/graph.c
rename to src/solver.c
index ea4adeb8..36143bff 100644
--- a/src/graph.c
+++ b/src/solver.c
@@ -1,11 +1,11 @@
 // SPDX-FileCopyrightText: 2023 Erin Catto
 // SPDX-License-Identifier: MIT
 
-#include "graph.h"
+#include "solver.h"
 
 #include "aabb.h"
 #include "allocate.h"
-#include "arena_allocator.h"
+#include "stack_allocator.h"
 #include "array.h"
 #include "bitset.inl"
 #include "body.h"
@@ -14,7 +14,7 @@
 #include "core.h"
 #include "joint.h"
 #include "shape.h"
-#include "solver_data.h"
+#include "solver.h"
 #include "world.h"
 
 // for mm_pause
@@ -25,18 +25,6 @@
 #include <stdbool.h>
 #include <string.h>
 
-// Solver using graph coloring. Islands are only used for sleep.
-// High-Performance Physical Simulations on Next-Generation Architecture with Many Cores
-// http://web.eecs.umich.edu/~msmelyan/papers/physsim_onmanycore_itj.pdf
-
-// Kinematic bodies have to be treated like dynamic bodies in graph coloring. Unlike static bodies, we cannot use a dummy solver
-// body for kinematic bodies. We cannot access a kinematic body from multiple threads efficiently because the SIMD solver body
-// scatter would write to the same kinematic body from multiple threads. Even if these writes don't modify the body, they will
-// cause horrible cache stalls. To make this feasible I would need a way to block these writes.
-
-// This is used for debugging by making all constraints be assigned to overflow.
-#define B2_FORCE_OVERFLOW 0
-
 typedef struct b2WorkerContext
 {
 	b2StepContext* context;
@@ -44,382 +32,6 @@ typedef struct b2WorkerContext
 	void* userTask;
 } b2WorkerContext;
 
-void b2CreateGraph(b2ConstraintGraph* graph, int32_t bodyCapacity, int32_t contactCapacity, int32_t jointCapacity)
-{
-	*graph = (b2ConstraintGraph){0};
-
-	bodyCapacity = B2_MAX(bodyCapacity, 8);
-	contactCapacity = B2_MAX(contactCapacity, 8);
-	jointCapacity = B2_MAX(jointCapacity, 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);
-		color->jointArray = b2CreateArray(sizeof(int32_t), jointCapacity);
-		color->contactConstraints = NULL;
-	}
-
-	graph->overflow.contactArray = b2CreateArray(sizeof(int32_t), contactCapacity);
-	graph->overflow.jointArray = b2CreateArray(sizeof(int32_t), jointCapacity);
-	graph->overflow.contactConstraints = NULL;
-}
-
-void b2DestroyGraph(b2ConstraintGraph* graph)
-{
-	for (int32_t i = 0; i < b2_graphColorCount; ++i)
-	{
-		b2GraphColor* color = graph->colors + i;
-		b2DestroyBitSet(&color->bodySet);
-		b2DestroyArray(color->contactArray, sizeof(int32_t));
-		b2DestroyArray(color->jointArray, sizeof(int32_t));
-	}
-
-	b2DestroyArray(graph->overflow.contactArray, sizeof(int32_t));
-	b2DestroyArray(graph->overflow.jointArray, sizeof(int32_t));
-}
-
-void b2AddContactToGraph(b2World* world, b2Contact* contact)
-{
-	B2_ASSERT(contact->colorIndex == B2_NULL_INDEX);
-	B2_ASSERT(contact->colorSubIndex == B2_NULL_INDEX);
-
-	b2ConstraintGraph* graph = &world->graph;
-
-#if B2_FORCE_OVERFLOW == 0
-	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;
-	B2_ASSERT(typeA != b2_staticBody || typeB != b2_staticBody);
-
-	if (typeA != b2_staticBody && typeB != b2_staticBody)
-	{
-		for (int32_t i = 0; i < b2_graphColorCount; ++i)
-		{
-			b2GraphColor* color = graph->colors + i;
-			if (b2GetBit(&color->bodySet, bodyIndexA) || b2GetBit(&color->bodySet, bodyIndexB))
-			{
-				continue;
-			}
-
-			b2SetBitGrow(&color->bodySet, bodyIndexA);
-			b2SetBitGrow(&color->bodySet, bodyIndexB);
-
-			contact->colorSubIndex = b2Array(color->contactArray).count;
-			b2Array_Push(color->contactArray, contact->object.index);
-			contact->colorIndex = i;
-			break;
-		}
-	}
-	else if (typeA != b2_staticBody)
-	{
-		// Static contacts never in color 0
-		for (int32_t i = 1; i < b2_graphColorCount; ++i)
-		{
-			b2GraphColor* color = graph->colors + i;
-			if (b2GetBit(&color->bodySet, bodyIndexA))
-			{
-				continue;
-			}
-
-			b2SetBitGrow(&color->bodySet, bodyIndexA);
-
-			contact->colorSubIndex = b2Array(color->contactArray).count;
-			b2Array_Push(color->contactArray, contact->object.index);
-			contact->colorIndex = i;
-			break;
-		}
-	}
-	else if (typeB != b2_staticBody)
-	{
-		// Static contacts never in color 0
-		for (int32_t i = 1; i < b2_graphColorCount; ++i)
-		{
-			b2GraphColor* color = graph->colors + i;
-			if (b2GetBit(&color->bodySet, bodyIndexB))
-			{
-				continue;
-			}
-
-			b2SetBitGrow(&color->bodySet, bodyIndexB);
-
-			contact->colorSubIndex = b2Array(color->contactArray).count;
-			b2Array_Push(color->contactArray, contact->object.index);
-			contact->colorIndex = i;
-			break;
-		}
-	}
-#endif
-
-	// Overflow
-	if (contact->colorIndex == B2_NULL_INDEX)
-	{
-		contact->colorSubIndex = b2Array(graph->overflow.contactArray).count;
-		b2Array_Push(graph->overflow.contactArray, contact->object.index);
-		contact->colorIndex = b2_overflowIndex;
-	}
-}
-
-void b2RemoveContactFromGraph(b2World* world, b2Contact* contact)
-{
-	B2_ASSERT(contact->colorIndex != B2_NULL_INDEX);
-	B2_ASSERT(contact->colorSubIndex != B2_NULL_INDEX);
-
-	b2ConstraintGraph* graph = &world->graph;
-
-	// Overflow
-	if (contact->colorIndex == b2_overflowIndex)
-	{
-		int32_t colorSubIndex = contact->colorSubIndex;
-		b2Array_RemoveSwap(graph->overflow.contactArray, colorSubIndex);
-		if (colorSubIndex < b2Array(graph->overflow.contactArray).count)
-		{
-			// Fix index on swapped contact
-			int32_t swappedIndex = graph->overflow.contactArray[colorSubIndex];
-			B2_ASSERT(world->contacts[swappedIndex].colorIndex == b2_overflowIndex);
-			world->contacts[swappedIndex].colorSubIndex = colorSubIndex;
-		}
-
-		contact->colorIndex = B2_NULL_INDEX;
-		contact->colorSubIndex = 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;
-	B2_ASSERT(typeA != b2_staticBody || typeB != b2_staticBody);
-
-	if (typeA != b2_staticBody && typeB != b2_staticBody)
-	{
-		b2GraphColor* color = graph->colors + contact->colorIndex;
-		B2_ASSERT(b2GetBit(&color->bodySet, bodyIndexA) && b2GetBit(&color->bodySet, bodyIndexB));
-
-		int32_t colorSubIndex = contact->colorSubIndex;
-		b2Array_RemoveSwap(color->contactArray, colorSubIndex);
-		if (colorSubIndex < b2Array(color->contactArray).count)
-		{
-			// Fix index on swapped contact
-			int32_t swappedIndex = color->contactArray[colorSubIndex];
-			world->contacts[swappedIndex].colorSubIndex = colorSubIndex;
-		}
-
-		b2ClearBit(&color->bodySet, bodyIndexA);
-		b2ClearBit(&color->bodySet, bodyIndexB);
-	}
-	else if (typeA != b2_staticBody)
-	{
-		b2GraphColor* color = graph->colors + contact->colorIndex;
-		B2_ASSERT(b2GetBit(&color->bodySet, bodyIndexA));
-
-		int32_t colorSubIndex = contact->colorSubIndex;
-		b2Array_RemoveSwap(color->contactArray, colorSubIndex);
-		if (colorSubIndex < b2Array(color->contactArray).count)
-		{
-			// Fix index on swapped contact
-			int32_t swappedIndex = color->contactArray[colorSubIndex];
-			world->contacts[swappedIndex].colorSubIndex = colorSubIndex;
-		}
-
-		b2ClearBit(&color->bodySet, bodyIndexA);
-	}
-	else if (typeB != b2_staticBody)
-	{
-		b2GraphColor* color = graph->colors + contact->colorIndex;
-		B2_ASSERT(b2GetBit(&color->bodySet, bodyIndexB));
-
-		int32_t colorSubIndex = contact->colorSubIndex;
-		b2Array_RemoveSwap(color->contactArray, colorSubIndex);
-		if (colorSubIndex < b2Array(color->contactArray).count)
-		{
-			// Fix index on swapped contact
-			int32_t swappedIndex = color->contactArray[colorSubIndex];
-			world->contacts[swappedIndex].colorSubIndex = colorSubIndex;
-		}
-
-		b2ClearBit(&color->bodySet, bodyIndexB);
-	}
-
-	contact->colorIndex = B2_NULL_INDEX;
-	contact->colorSubIndex = B2_NULL_INDEX;
-}
-
-void b2AddJointToGraph(b2World* world, b2Joint* joint)
-{
-	B2_ASSERT(joint->colorIndex == B2_NULL_INDEX);
-	B2_ASSERT(joint->colorSubIndex == B2_NULL_INDEX);
-
-	b2ConstraintGraph* graph = &world->graph;
-
-#if B2_FORCE_OVERFLOW == 0
-	int32_t bodyIndexA = joint->edges[0].bodyIndex;
-	int32_t bodyIndexB = joint->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;
-			}
-
-			b2SetBitGrow(&color->bodySet, bodyIndexA);
-			b2SetBitGrow(&color->bodySet, bodyIndexB);
-
-			joint->colorSubIndex = b2Array(color->jointArray).count;
-			b2Array_Push(color->jointArray, joint->object.index);
-			joint->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;
-			}
-
-			b2SetBitGrow(&color->bodySet, bodyIndexA);
-
-			joint->colorSubIndex = b2Array(color->jointArray).count;
-			b2Array_Push(color->jointArray, joint->object.index);
-			joint->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;
-			}
-
-			b2SetBitGrow(&color->bodySet, bodyIndexB);
-
-			joint->colorSubIndex = b2Array(color->jointArray).count;
-			b2Array_Push(color->jointArray, joint->object.index);
-			joint->colorIndex = i;
-			break;
-		}
-	}
-#endif
-
-	// Overflow
-	if (joint->colorIndex == B2_NULL_INDEX)
-	{
-		joint->colorSubIndex = b2Array(graph->overflow.jointArray).count;
-		b2Array_Push(graph->overflow.jointArray, joint->object.index);
-		joint->colorIndex = b2_overflowIndex;
-	}
-}
-
-void b2RemoveJointFromGraph(b2World* world, b2Joint* joint)
-{
-	B2_ASSERT(joint->colorIndex != B2_NULL_INDEX);
-	B2_ASSERT(joint->colorSubIndex != B2_NULL_INDEX);
-
-	b2ConstraintGraph* graph = &world->graph;
-
-	// Overflow
-	if (joint->colorIndex == b2_overflowIndex)
-	{
-		int32_t colorSubIndex = joint->colorSubIndex;
-		b2Array_RemoveSwap(graph->overflow.jointArray, colorSubIndex);
-		if (colorSubIndex < b2Array(graph->overflow.jointArray).count)
-		{
-			// Fix index on swapped joint
-			int32_t swappedIndex = graph->overflow.jointArray[colorSubIndex];
-			B2_ASSERT(world->joints[swappedIndex].colorIndex == b2_overflowIndex);
-			world->joints[swappedIndex].colorSubIndex = colorSubIndex;
-		}
-
-		joint->colorIndex = B2_NULL_INDEX;
-		joint->colorSubIndex = B2_NULL_INDEX;
-
-		return;
-	}
-
-	B2_ASSERT(0 <= joint->colorIndex && joint->colorIndex < b2_graphColorCount);
-	int32_t bodyIndexA = joint->edges[0].bodyIndex;
-	int32_t bodyIndexB = joint->edges[1].bodyIndex;
-
-	b2BodyType typeA = world->bodies[bodyIndexA].type;
-	b2BodyType typeB = world->bodies[bodyIndexB].type;
-
-	if (typeA == b2_dynamicBody && typeB == b2_dynamicBody)
-	{
-		b2GraphColor* color = graph->colors + joint->colorIndex;
-		B2_ASSERT(b2GetBit(&color->bodySet, bodyIndexA) && b2GetBit(&color->bodySet, bodyIndexB));
-
-		int32_t colorSubIndex = joint->colorSubIndex;
-		b2Array_RemoveSwap(color->jointArray, colorSubIndex);
-		if (colorSubIndex < b2Array(color->jointArray).count)
-		{
-			// Fix index on swapped joint
-			int32_t swappedIndex = color->jointArray[colorSubIndex];
-			world->joints[swappedIndex].colorSubIndex = colorSubIndex;
-		}
-
-		b2ClearBit(&color->bodySet, bodyIndexA);
-		b2ClearBit(&color->bodySet, bodyIndexB);
-	}
-	else if (typeA == b2_dynamicBody)
-	{
-		b2GraphColor* color = graph->colors + joint->colorIndex;
-		B2_ASSERT(b2GetBit(&color->bodySet, bodyIndexA));
-
-		int32_t colorSubIndex = joint->colorSubIndex;
-		b2Array_RemoveSwap(color->jointArray, colorSubIndex);
-		if (colorSubIndex < b2Array(color->jointArray).count)
-		{
-			// Fix index on swapped joint
-			int32_t swappedIndex = color->jointArray[colorSubIndex];
-			world->joints[swappedIndex].colorSubIndex = colorSubIndex;
-		}
-
-		b2ClearBit(&color->bodySet, bodyIndexA);
-	}
-	else if (typeB == b2_dynamicBody)
-	{
-		b2GraphColor* color = graph->colors + joint->colorIndex;
-		B2_ASSERT(b2GetBit(&color->bodySet, bodyIndexB));
-
-		int32_t colorSubIndex = joint->colorSubIndex;
-		b2Array_RemoveSwap(color->jointArray, colorSubIndex);
-		if (colorSubIndex < b2Array(color->jointArray).count)
-		{
-			// Fix index on swapped joint
-			int32_t swappedIndex = color->jointArray[colorSubIndex];
-			world->joints[swappedIndex].colorSubIndex = colorSubIndex;
-		}
-
-		b2ClearBit(&color->bodySet, bodyIndexB);
-	}
-
-	joint->colorIndex = B2_NULL_INDEX;
-	joint->colorSubIndex = B2_NULL_INDEX;
-}
-
 // Integrate velocities and apply damping.
 static void b2IntegrateVelocitiesTask(int32_t startIndex, int32_t endIndex, b2StepContext* context)
 {
diff --git a/src/solver_data.h b/src/solver.h
similarity index 97%
rename from src/solver_data.h
rename to src/solver.h
index eb91bfa7..975af96e 100644
--- a/src/solver_data.h
+++ b/src/solver.h
@@ -11,6 +11,7 @@
 
 typedef struct b2BodyState b2BodyState;
 typedef struct b2BodyParam b2BodyParam;
+typedef struct b2World b2World;
 
 typedef struct b2Softness
 {
@@ -121,7 +122,6 @@ typedef struct b2StepContext
 	bool enableWarmStarting;
 } b2StepContext;
 
-
 static inline b2Softness b2MakeSoft(float hertz, float zeta, float h)
 {
 	if (hertz == 0.0f)
@@ -135,3 +135,5 @@ static inline b2Softness b2MakeSoft(float hertz, float zeta, float h)
 	float a3 = 1.0f / (1.0f + a2);
 	return (b2Softness){omega / a1, a2 * a3, a3};
 }
+
+void b2Solve(b2World* world, b2StepContext* stepContext);
diff --git a/src/arena_allocator.c b/src/stack_allocator.c
similarity index 99%
rename from src/arena_allocator.c
rename to src/stack_allocator.c
index e449aea9..f41a99e7 100644
--- a/src/arena_allocator.c
+++ b/src/stack_allocator.c
@@ -1,7 +1,7 @@
 // SPDX-FileCopyrightText: 2023 Erin Catto
 // SPDX-License-Identifier: MIT
 
-#include "arena_allocator.h"
+#include "stack_allocator.h"
 
 #include "allocate.h"
 #include "array.h"
diff --git a/src/arena_allocator.h b/src/stack_allocator.h
similarity index 100%
rename from src/arena_allocator.h
rename to src/stack_allocator.h
diff --git a/src/types.c b/src/types.c
index 373b89f4..227e9f14 100644
--- a/src/types.c
+++ b/src/types.c
@@ -4,3 +4,62 @@
 #include "box2d/types.h"
 
 #include <float.h>
+
+b2WorldDef b2DefaultWorldDef()
+{
+	b2WorldDef def = {0};
+	def.gravity.x = 0.0f;
+	def.gravity.y = -10.0f;
+	def.restitutionThreshold = 1.0f * b2_lengthUnitsPerMeter;
+	def.contactPushoutVelocity = 3.0f * b2_lengthUnitsPerMeter;
+	def.contactHertz = 30.0;
+	def.contactDampingRatio = 10.0f;
+	def.jointHertz = 60.0;
+	def.jointDampingRatio = 2.0f;
+	def.enableSleep = true;
+	def.enableContinous = true;
+	def.stackAllocatorCapacity = 1024 * 1024;
+	return def;
+}
+
+b2BodyDef b2DefaultBodyDef()
+{
+	b2BodyDef def = {0};
+	def.type = b2_staticBody;
+	def.gravityScale = 1.0f;
+	def.enableSleep = true;
+	def.isAwake = true;
+	def.isEnabled = true;
+	return def;
+}
+
+b2Filter b2DefaultFilter()
+{
+	b2Filter filter = {0x00000001, 0xFFFFFFFF, 0};
+	return filter;
+}
+
+b2QueryFilter b2DefaultQueryFilter()
+{
+	b2QueryFilter filter = {0x00000001, 0xFFFFFFFF};
+	return filter;
+}
+
+b2ShapeDef b2DefaultShapeDef()
+{
+	b2ShapeDef def = {0};
+	def.friction = 0.6f;
+	def.density = 1.0f;
+	def.filter = b2DefaultFilter();
+	def.enableSensorEvents = true;
+	def.enableContactEvents = true;
+	return def;
+}
+
+b2ChainDef b2DefaultChainDef()
+{
+	b2ChainDef def = {0};
+	def.friction = 0.6f;
+	def.filter = b2DefaultFilter();
+	return def;
+}
diff --git a/src/weld_joint.c b/src/weld_joint.c
index 39f302a5..441b9c97 100644
--- a/src/weld_joint.c
+++ b/src/weld_joint.c
@@ -4,18 +4,11 @@
 #include "body.h"
 #include "core.h"
 #include "joint.h"
-#include "solver_data.h"
+#include "solver.h"
 #include "world.h"
 
 // needed for dll export
 #include "box2d/box2d.h"
-#include "box2d/joint_types.h"
-
-b2WeldJointDef b2DefaultWeldJointDef()
-{
-	b2WeldJointDef def = {0};
-	return def;
-}
 
 // Point-to-point constraint
 // C = p2 - p1
diff --git a/src/wheel_joint.c b/src/wheel_joint.c
index 18f09b29..ed7cf8cb 100644
--- a/src/wheel_joint.c
+++ b/src/wheel_joint.c
@@ -4,25 +4,15 @@
 #include "body.h"
 #include "core.h"
 #include "joint.h"
-#include "solver_data.h"
+#include "solver.h"
 #include "world.h"
 
 // needed for dll export
 #include "box2d/box2d.h"
 #include "box2d/debug_draw.h"
-#include "box2d/joint_types.h"
 
 #include <stdio.h>
 
-b2WheelJointDef b2DefaultWheelJointDef()
-{
-	b2WheelJointDef def = {0};
-	def.localAxisA.y = 1.0f;
-	def.hertz = 1.0f;
-	def.dampingRatio = 0.7f;
-	return def;
-}
-
 // Linear constraint (point-to-line)
 // d = pB - pA = xB + rB - xA - rA
 // C = dot(ay, d)
diff --git a/src/world.c b/src/world.c
index f1a5f881..641852bb 100644
--- a/src/world.c
+++ b/src/world.c
@@ -7,24 +7,25 @@
 
 #include "aabb.h"
 #include "allocate.h"
-#include "arena_allocator.h"
+#include "stack_allocator.h"
 #include "array.h"
 #include "bitset.h"
 #include "bitset.inl"
 #include "block_allocator.h"
 #include "body.h"
 #include "broad_phase.h"
+#include "constraint_graph.h"
 #include "contact.h"
 #include "core.h"
-#include "graph.h"
 #include "island.h"
 #include "joint.h"
 #include "pool.h"
 #include "shape.h"
-#include "solver_data.h"
+#include "solver.h"
 
 // needed for dll export
 #include "box2d/box2d.h"
+#include "box2d/color.h"
 #include "box2d/constants.h"
 #include "box2d/debug_draw.h"
 #include "box2d/distance.h"
@@ -37,23 +38,6 @@
 
 b2World b2_worlds[b2_maxWorlds];
 
-b2WorldDef b2DefaultWorldDef()
-{
-	b2WorldDef def = {0};
-	def.gravity.x = 0.0f;
-	def.gravity.y = -10.0f;
-	def.restitutionThreshold = 1.0f * b2_lengthUnitsPerMeter;
-	def.contactPushoutVelocity = 3.0f * b2_lengthUnitsPerMeter;
-	def.contactHertz = 30.0;
-	def.contactDampingRatio = 10.0f;
-	def.jointHertz = 60.0;
-	def.jointDampingRatio = 2.0f;
-	def.enableSleep = true;
-	def.enableContinous = true;
-	def.stackAllocatorCapacity = 1024 * 1024;
-	return def;
-}
-
 b2World* b2GetWorldFromId(b2WorldId id)
 {
 	B2_ASSERT(1 <= id.index && id.index <= b2_maxWorlds);
@@ -1368,7 +1352,7 @@ static float RayCastCallback(const b2RayCastInput* input, int32_t proxyId, int32
 	B2_ASSERT(b2ObjectValid(&body->object));
 
 	b2Transform transform = b2MakeTransform(body);
-	b2RayCastOutput output = b2RayCastShape(input, shape, transform);
+	b2CastOutput output = b2RayCastShape(input, shape, transform);
 
 	if (output.hit)
 	{
@@ -1480,7 +1464,7 @@ static float ShapeCastCallback(const b2ShapeCastInput* input, int32_t proxyId, i
 	B2_ASSERT(b2ObjectValid(&body->object));
 
 	b2Transform transform = b2MakeTransform(body);
-	b2RayCastOutput output = b2ShapeCastShape(input, shape, transform);
+	b2CastOutput output = b2ShapeCastShape(input, shape, transform);
 
 	if (output.hit)
 	{
diff --git a/src/world.h b/src/world.h
index c3b5f1f9..d4013ac9 100644
--- a/src/world.h
+++ b/src/world.h
@@ -5,8 +5,8 @@
 
 #include "bitset.h"
 #include "broad_phase.h"
+#include "constraint_graph.h"
 #include "island.h"
-#include "graph.h"
 #include "pool.h"
 
 #include "box2d/callbacks.h"
diff --git a/test/test_collision.c b/test/test_collision.c
index 13be7c91..5cf13a02 100644
--- a/test/test_collision.c
+++ b/test/test_collision.c
@@ -24,7 +24,7 @@ static int AABBTest(void)
 	b2Vec2 p1 = (b2Vec2){-2.0f, 0.0f};
 	b2Vec2 p2 = (b2Vec2){2.0f, 0.0f};
 
-	b2RayCastOutput output = b2AABB_RayCast(a, p1, p2);
+	b2CastOutput output = b2AABB_RayCast(a, p1, p2);
 	ENSURE(output.hit == true);
 	ENSURE(0.1f < output.fraction && output.fraction < 0.9f);
 
diff --git a/test/test_distance.c b/test/test_distance.c
index def1200b..261c9df7 100644
--- a/test/test_distance.c
+++ b/test/test_distance.c
@@ -77,7 +77,7 @@ static int ShapeCastTest(void)
 	input.translationB = (b2Vec2){-2.0f, 0.0f};
 	input.maxFraction = 1.0f;
 
-	b2RayCastOutput output = b2ShapeCast(&input);
+	b2CastOutput output = b2ShapeCast(&input);
 
 	ENSURE(output.hit);
 	ENSURE_SMALL(output.fraction - 0.5f, b2_linearSlop);
diff --git a/test/test_shape.c b/test/test_shape.c
index 24d3cc53..f779970f 100644
--- a/test/test_shape.c
+++ b/test/test_shape.c
@@ -130,7 +130,7 @@ static int RayCastShapeTest(void)
 	b2RayCastInput input = {{-4.0f, 0.0f}, {8.0f, 0.0f}, 1.0f};
 	
 	{
-		b2RayCastOutput output = b2RayCastCircle(&input, &circle);
+		b2CastOutput output = b2RayCastCircle(&input, &circle);
 		ENSURE(output.hit);
 		ENSURE_SMALL(output.normal.x + 1.0f, FLT_EPSILON);
 		ENSURE_SMALL(output.normal.y, FLT_EPSILON);
@@ -138,7 +138,7 @@ static int RayCastShapeTest(void)
 	}
 
 	{
-		b2RayCastOutput output = b2RayCastPolygon(&input, &box);
+		b2CastOutput output = b2RayCastPolygon(&input, &box);
 		ENSURE(output.hit);
 		ENSURE_SMALL(output.normal.x + 1.0f, FLT_EPSILON);
 		ENSURE_SMALL(output.normal.y, FLT_EPSILON);
@@ -146,7 +146,7 @@ static int RayCastShapeTest(void)
 	}
 
 	{
-		b2RayCastOutput output = b2RayCastSegment(&input, &segment, true);
+		b2CastOutput output = b2RayCastSegment(&input, &segment, true);
 		ENSURE(output.hit);
 		ENSURE_SMALL(output.normal.x + 1.0f, FLT_EPSILON);
 		ENSURE_SMALL(output.normal.y, FLT_EPSILON);