diff --git a/include/box2d/box2d.h b/include/box2d/box2d.h
index a85e761f..8d11d60f 100644
--- a/include/box2d/box2d.h
+++ b/include/box2d/box2d.h
@@ -4,6 +4,7 @@
 #pragma once
 
 #include "box2d/api.h"
+#include "box2d/geometry.h"
 #include "box2d/id.h"
 #include "box2d/joint_types.h"
 #include "box2d/timer.h"
@@ -41,16 +42,51 @@ BOX2D_API void b2World_DestroyBody(b2BodyId bodyId);
 
 BOX2D_API b2Vec2 b2Body_GetPosition(b2BodyId bodyId);
 BOX2D_API float b2Body_GetAngle(b2BodyId bodyId);
+BOX2D_API void b2Body_SetTransform(b2BodyId bodyId, b2Vec2 position, float angle);
+
 BOX2D_API b2Vec2 b2Body_GetLocalPoint(b2BodyId bodyId, b2Vec2 globalPoint);
+BOX2D_API b2Vec2 b2Body_GetWorldPoint(b2BodyId bodyId, b2Vec2 localPoint);
 
-BOX2D_API void b2Body_SetTransform(b2BodyId bodyId, b2Vec2 position, float angle);
+BOX2D_API b2Vec2 b2Body_GetLinearVelocity(b2BodyId bodyId);
+BOX2D_API float  b2Body_GetAngularVelocity(b2BodyId bodyId);
 BOX2D_API void b2Body_SetLinearVelocity(b2BodyId bodyId, b2Vec2 linearVelocity);
 BOX2D_API void b2Body_SetAngularVelocity(b2BodyId bodyId, float angularVelocity);
 
 BOX2D_API b2BodyType b2Body_GetType(b2BodyId bodyId);
+BOX2D_API void b2Body_SetType(b2BodyId bodyId, b2BodyType type);
+
+/// Get the mass of the body (kilograms)
 BOX2D_API float b2Body_GetMass(b2BodyId bodyId);
+
+/// Get the inertia tensor of the body. In 2D this is a single number. (kilograms * meters^2)
+BOX2D_API float b2Body_GetInertiaTensor(b2BodyId bodyId);
+
+/// Get the center of mass position of the body in local space.
+BOX2D_API b2Vec2 b2Body_GetLocalCenterOfMass(b2BodyId bodyId);
+
+/// Get the center of mass position of the body in world space.
+BOX2D_API b2Vec2 b2Body_GetWorldCenterOfMass(b2BodyId bodyId);
+
+/// Override the body's mass properties. Normally this is computed automatically using the
+///	shape geometry and density. This information is lost if a shape is added or removed or if the
+///	body type changes.
+BOX2D_API void b2Body_SetMassData(b2MassData massData);
+
+/// Is this body awake?
+BOX2D_API void b2Body_IsAwake(b2BodyId bodyId);
+
+/// Wake a body from sleep. This wakes the entire island the body is touching.
 BOX2D_API void b2Body_Wake(b2BodyId bodyId);
 
+/// Is this body enabled?
+BOX2D_API bool b2Body_IsEnabled(b2BodyId bodyId);
+
+/// Disable a body by removing it completely from the simulation
+BOX2D_API void b2Body_Disable(b2BodyId bodyId);
+
+/// Enable a body by adding it to the simulation
+BOX2D_API void b2Body_Enable(b2BodyId bodyId);
+
 /// Create a shape and attach it to a body. Contacts are not created until the next time step.
 /// @warning This function is locked during callbacks.
 BOX2D_API b2ShapeId b2Body_CreateCircle(b2BodyId bodyId, const b2ShapeDef* def, const b2Circle* circle);
@@ -62,6 +98,7 @@ BOX2D_API b2BodyId b2Shape_GetBody(b2ShapeId shapeId);
 BOX2D_API bool b2Shape_TestPoint(b2ShapeId shapeId, b2Vec2 point);
 
 BOX2D_API b2JointId b2World_CreateMouseJoint(b2WorldId worldId, const b2MouseJointDef* def);
+BOX2D_API b2JointId b2World_CreatePrismaticJoint(b2WorldId worldId, const b2PrismaticJointDef* def);
 BOX2D_API b2JointId b2World_CreateRevoluteJoint(b2WorldId worldId, const b2RevoluteJointDef* def);
 BOX2D_API b2JointId b2World_CreateWeldJoint(b2WorldId worldId, const b2WeldJointDef* def);
 BOX2D_API void b2World_DestroyJoint(b2JointId jointId);
@@ -78,7 +115,7 @@ BOX2D_API float b2RevoluteJoint_GetMotorTorque(b2JointId jointId, float inverseT
 BOX2D_API void b2RevoluteJoint_SetMaxMotorTorque(b2JointId jointId, float torque);
 BOX2D_API b2Vec2 b2RevoluteJoint_GetConstraintForce(b2JointId jointId);
 
-	/// This function receives shapes found in the AABB query.
+/// This function receives shapes found in the AABB query.
 /// @return true if the query should continue
 typedef bool b2QueryCallbackFcn(b2ShapeId shapeId, void* context);
 
@@ -87,7 +124,6 @@ typedef bool b2QueryCallbackFcn(b2ShapeId shapeId, void* context);
 /// @param aabb the query box.
 BOX2D_API void b2World_QueryAABB(b2WorldId worldId, b2AABB aabb, b2QueryCallbackFcn* fcn, void* context);
 
-
 /// Advanced API for testing and special cases
 
 /// Enable/disable sleep.
diff --git a/include/box2d/geometry.h b/include/box2d/geometry.h
index d6a231da..e9024c8d 100644
--- a/include/box2d/geometry.h
+++ b/include/box2d/geometry.h
@@ -23,6 +23,8 @@ typedef struct b2MassData
 	/// The rotational inertia of the shape about the local origin.
 	float I;
 
+	/// TODO_ERIN remove geometry info from this
+
 	/// Distance from shape centroid to closest point on perimeter.
 	float minExtent;
 
diff --git a/include/box2d/joint_types.h b/include/box2d/joint_types.h
index 4f8bc87c..7a5927bf 100644
--- a/include/box2d/joint_types.h
+++ b/include/box2d/joint_types.h
@@ -35,7 +35,7 @@ typedef struct b2MouseJointDef
 	float damping;
 } b2MouseJointDef;
 
-static inline struct b2MouseJointDef b2DefaultMouseJointDef(void)
+static inline b2MouseJointDef b2DefaultMouseJointDef(void)
 {
 	b2MouseJointDef def = {0};
 	def.bodyIdA = b2_nullBodyId;
@@ -47,6 +47,73 @@ static inline struct b2MouseJointDef b2DefaultMouseJointDef(void)
 	return def;
 }
 
+/// Prismatic joint definition. This requires defining a line of
+/// motion using an axis and an anchor point. The definition uses local
+/// anchor points and a local axis so that the initial configuration
+/// can violate the constraint slightly. The joint translation is zero
+/// when the local anchor points coincide in world space.
+typedef struct b2PrismaticJointDef
+{
+	/// The first attached body.
+	b2BodyId bodyIdA;
+
+	/// The second attached body.
+	b2BodyId bodyIdB;
+
+	/// The local anchor point relative to bodyA's origin.
+	b2Vec2 localAnchorA;
+
+	/// The local anchor point relative to bodyB's origin.
+	b2Vec2 localAnchorB;
+
+	/// The local translation unit axis in bodyA.
+	b2Vec2 localAxisA;
+
+	/// The constrained angle between the bodies: bodyB_angle - bodyA_angle.
+	float referenceAngle;
+
+	/// Enable/disable the joint limit.
+	bool enableLimit;
+
+	/// The lower translation limit, usually in meters.
+	float lowerTranslation;
+
+	/// The upper translation limit, usually in meters.
+	float upperTranslation;
+
+	/// Enable/disable the joint motor.
+	bool enableMotor;
+
+	/// The maximum motor torque, usually in N-m.
+	float maxMotorForce;
+
+	/// The desired motor speed in radians per second.
+	float motorSpeed;
+
+	/// Set this flag to true if the attached bodies should collide.
+	bool collideConnected;
+} b2PrismaticJointDef;
+
+/// Use this to initialize your joint definition
+static inline b2PrismaticJointDef b2DefaultPrismaticJointDef(void)
+{
+	b2PrismaticJointDef def = {0};
+	def.bodyIdA = b2_nullBodyId;
+	def.bodyIdB = b2_nullBodyId;
+	def.localAnchorA = B2_LITERAL(b2Vec2){0.0f, 0.0f};
+	def.localAnchorB = B2_LITERAL(b2Vec2){0.0f, 0.0f};
+	def.localAxisA = B2_LITERAL(b2Vec2){1.0f, 0.0f};
+	def.referenceAngle = 0.0f;
+	def.enableLimit = false;
+	def.lowerTranslation = 0.0f;
+	def.upperTranslation = 0.0f;
+	def.enableMotor = false;
+	def.maxMotorForce = 0.0f;
+	def.motorSpeed = 0.0f;
+	def.collideConnected = false;
+	return def;
+}
+
 /// Revolute joint definition. This requires defining an anchor point where the
 /// bodies are joined. The definition uses local anchor points so that the
 /// initial configuration can violate the constraint slightly. You also need to
@@ -98,7 +165,8 @@ typedef struct b2RevoluteJointDef
 	bool collideConnected;
 } b2RevoluteJointDef;
 
-static inline struct b2RevoluteJointDef b2DefaultRevoluteJointDef(void)
+/// Use this to initialize your joint definition
+static inline b2RevoluteJointDef b2DefaultRevoluteJointDef(void)
 {
 	b2RevoluteJointDef def = {0};
 	def.bodyIdA = b2_nullBodyId;
@@ -146,7 +214,8 @@ typedef struct b2WeldJointDef
 	bool collideConnected;
 } b2WeldJointDef;
 
-static inline struct b2WeldJointDef b2DefaultWeldJointDef(void)
+/// Use this to initialize your joint definition
+static inline b2WeldJointDef b2DefaultWeldJointDef(void)
 {
 	b2WeldJointDef def = {0};
 	def.bodyIdA = b2_nullBodyId;
diff --git a/include/box2d/math.h b/include/box2d/math.h
index a287765e..51eb3ec8 100644
--- a/include/box2d/math.h
+++ b/include/box2d/math.h
@@ -72,7 +72,13 @@ static inline b2Vec2 b2CrossSV(float s, b2Vec2 v)
 	return B2_LITERAL(b2Vec2){-s * v.y, s * v.x};
 }
 
-/// Get a right pointing perpendicular vector. Equivalent to b2CrossVS(v, 1.0f).
+/// Get a left pointing perpendicular vector. Equivalent to b2CrossSV(1.0f, v)
+static inline b2Vec2 b2LeftPerp(b2Vec2 v)
+{
+	return B2_LITERAL(b2Vec2){-v.y, v.x};
+}
+
+/// Get a right pointing perpendicular vector. Equivalent to b2CrossVS(v, 1.0f)
 static inline b2Vec2 b2RightPerp(b2Vec2 v)
 {
 	return B2_LITERAL(b2Vec2){v.y, -v.x};
diff --git a/samples/CMakeLists.txt b/samples/CMakeLists.txt
index 324c1f2c..791a690d 100644
--- a/samples/CMakeLists.txt
+++ b/samples/CMakeLists.txt
@@ -69,8 +69,9 @@ set(BOX2D_SAMPLES
 	collection/benchmark_many_tumblers.cpp
 	collection/benchmark_pyramid.cpp
 	collection/benchmark_tumbler.cpp
-	collection/behavior.cpp
-	collection/sample_continuous1.cpp
+	collection/sample_robustness.cpp
+	collection/sample_bodies.cpp
+	collection/sample_continuous.cpp
 	collection/sample_distance.cpp
 	collection/sample_dynamic_tree.cpp
 	collection/sample_hull.cpp
diff --git a/samples/collection/sample_bodies.cpp b/samples/collection/sample_bodies.cpp
new file mode 100644
index 00000000..90684118
--- /dev/null
+++ b/samples/collection/sample_bodies.cpp
@@ -0,0 +1,183 @@
+// SPDX-FileCopyrightText: 2022 Erin Catto
+// SPDX-License-Identifier: MIT
+
+#include "sample.h"
+
+#include "box2d/box2d.h"
+#include "box2d/geometry.h"
+#include "box2d/hull.h"
+
+#include <imgui.h>
+
+class BodyType : public Sample
+{
+  public:
+	BodyType(const Settings& settings)
+		: Sample(settings)
+	{
+		b2BodyId groundId = b2_nullBodyId;
+		{
+			b2BodyDef bodyDef = b2DefaultBodyDef();
+			groundId = b2World_CreateBody(m_worldId, &bodyDef);
+
+			b2Segment segment = {{-20.0f, 0.0f}, {20.0f, 0.0f}};
+			b2ShapeDef shapeDef = b2DefaultShapeDef();
+			b2Body_CreateSegment(groundId, &shapeDef, &segment);
+		}
+
+		// Define attachment
+		{
+			b2BodyDef bodyDef = b2DefaultBodyDef();
+			bodyDef.type = b2_dynamicBody;
+			bodyDef.position = {0.0f, 3.0f};
+			m_attachmentId = b2World_CreateBody(m_worldId, &bodyDef);
+
+			b2Polygon box = b2MakeBox(0.5f, 2.0f);
+			b2ShapeDef shapeDef = b2DefaultShapeDef();
+			shapeDef.density = 1.0f;
+			b2Body_CreatePolygon(m_attachmentId, &shapeDef, &box);
+		}
+
+		// Define platform
+		{
+			b2BodyDef bodyDef = b2DefaultBodyDef();
+			bodyDef.type = b2_dynamicBody;
+			bodyDef.position = {-4.0f, 5.0f};
+			m_platformId = b2World_CreateBody(m_worldId, &bodyDef);
+
+			b2Polygon box = b2MakeOffsetBox(0.5f, 4.0f, {4.0f, 0.0f}, 0.5f * b2_pi);
+
+			b2ShapeDef shapeDef = b2DefaultShapeDef();
+			shapeDef.friction = 0.6f;
+			shapeDef.density = 2.0f;
+			b2Body_CreatePolygon(m_platformId, &shapeDef, &box);
+
+			b2RevoluteJointDef revoluteDef = b2DefaultRevoluteJointDef();
+			b2Vec2 pivot = {0.0f, 5.0f};
+			revoluteDef.bodyIdA = m_attachmentId;
+			revoluteDef.bodyIdB = m_platformId;
+			revoluteDef.localAnchorA = b2Body_GetLocalPoint(m_attachmentId, pivot);
+			revoluteDef.localAnchorB = b2Body_GetLocalPoint(m_platformId, pivot);
+			revoluteDef.maxMotorTorque = 50.0f;
+			revoluteDef.enableMotor = true;
+			b2World_CreateRevoluteJoint(m_worldId, &revoluteDef);
+
+			b2PrismaticJointDef prismaticDef = b2DefaultPrismaticJointDef();
+			b2Vec2 anchor = {0.0f, 5.0f};
+			prismaticDef.bodyIdA = groundId;
+			prismaticDef.bodyIdB = m_platformId;
+			prismaticDef.localAnchorA = b2Body_GetLocalPoint(groundId, anchor);
+			prismaticDef.localAnchorB = b2Body_GetLocalPoint(m_platformId, anchor);
+			prismaticDef.localAxisA = {1.0f, 0.0f};
+			prismaticDef.maxMotorForce = 1000.0f;
+			prismaticDef.motorSpeed = 0.0f;
+			prismaticDef.enableMotor = true;
+			prismaticDef.lowerTranslation = -10.0f;
+			prismaticDef.upperTranslation = 10.0f;
+			prismaticDef.enableLimit = true;
+
+			b2World_CreatePrismaticJoint(m_worldId, &prismaticDef);
+
+			m_speed = 3.0f;
+		}
+
+		// Create a payload
+		{
+			b2BodyDef bodyDef = b2DefaultBodyDef();
+			bodyDef.type = b2_dynamicBody;
+			bodyDef.position = {0.0f, 8.0f};
+			b2BodyId bodyId = b2World_CreateBody(m_worldId, &bodyDef);
+
+			b2Polygon box = b2MakeBox(0.75f, 0.75f);
+
+			b2ShapeDef shapeDef = b2DefaultShapeDef();
+			shapeDef.friction = 0.6f;
+			shapeDef.density = 2.0f;
+
+			b2Body_CreatePolygon(bodyId, &shapeDef, &box);
+		}
+	}
+
+	void UpdateUI() override
+	{
+		ImGui::SetNextWindowPos(ImVec2(10.0f, 400.0f));
+		ImGui::SetNextWindowSize(ImVec2(200.0f, 150.0f));
+		ImGui::Begin("Controls", nullptr, ImGuiWindowFlags_NoMove | ImGuiWindowFlags_NoResize);
+
+		b2BodyType bodyType = b2Body_GetType(m_platformId);
+
+		if (ImGui::RadioButton("Static", bodyType == b2_staticBody))
+		{
+			b2Body_SetType(m_platformId, b2_staticBody);
+		}
+
+		if (ImGui::RadioButton("Kinematic", bodyType == b2_kinematicBody))
+		{
+			b2Body_SetType(m_platformId, b2_kinematicBody);
+			b2Body_SetLinearVelocity(m_platformId, {-m_speed, 0.0f});
+			b2Body_SetAngularVelocity(m_platformId, 0.0f);
+		}
+		
+		if (ImGui::RadioButton("Dynamic", bodyType == b2_dynamicBody))
+		{
+			b2Body_SetType(m_platformId, b2_dynamicBody);
+		}
+
+		bool isEnabled = b2Body_IsEnabled(m_platformId);
+		if (ImGui::Checkbox("Enable", &isEnabled))
+		{
+			if (isEnabled)
+			{
+				b2Body_Enable(m_platformId);
+			}
+			else
+			{
+				b2Body_Disable(m_platformId);
+			}
+		}
+
+		ImGui::End();
+	}
+
+	void Step(Settings& settings) override
+	{
+		// Drive the kinematic body.
+		if (b2Body_GetType(m_platformId) == b2_kinematicBody)
+		{
+			b2Vec2 p = b2Body_GetPosition(m_platformId);
+			b2Vec2 v = b2Body_GetLinearVelocity(m_platformId);
+
+			if ((p.x < -14.0f && v.x < 0.0f) || (p.x > 6.0f && v.x > 0.0f))
+			{
+				v.x = -v.x;
+				b2Body_SetLinearVelocity(m_platformId, v);
+			}
+		}
+
+		Sample::Step(settings);
+	}
+
+	static Sample* Create(const Settings& settings)
+	{
+		return new BodyType(settings);
+	}
+
+	b2BodyId m_attachmentId;
+	b2BodyId m_platformId;
+	float m_speed;
+};
+
+static int sampleBodyType = RegisterSample("Bodies", "Body Type", BodyType::Create);
+
+
+// Test all these APIs:
+#if 0
+void b2Body_SetTransform(b2BodyId bodyId, b2Vec2 position, float angle);
+float b2Body_GetMass(b2BodyId bodyId);
+float b2Body_GetInertiaTensor(b2BodyId bodyId);
+float b2Body_GetCenterOfMass(b2BodyId bodyId);
+void b2Body_SetMassData(b2MassData massData);
+void b2Body_Wake(b2BodyId bodyId);
+void b2Body_Disable(b2BodyId bodyId);
+void b2Body_Enable(b2BodyId bodyId);
+#endif
\ No newline at end of file
diff --git a/samples/collection/sample_continuous1.cpp b/samples/collection/sample_continuous.cpp
similarity index 100%
rename from samples/collection/sample_continuous1.cpp
rename to samples/collection/sample_continuous.cpp
diff --git a/samples/collection/behavior.cpp b/samples/collection/sample_robustness.cpp
similarity index 100%
rename from samples/collection/behavior.cpp
rename to samples/collection/sample_robustness.cpp
diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
index c4eb1009..9d6c7924 100644
--- a/src/CMakeLists.txt
+++ b/src/CMakeLists.txt
@@ -35,6 +35,7 @@ set(BOX2D_SOURCE_FILES
 	polygon_shape.h
 	pool.c
 	pool.h
+	prismatic_joint.c
 	revolute_joint.c
 	shape.c
 	shape.h
diff --git a/src/body.c b/src/body.c
index 681d50d3..ec7a8720 100644
--- a/src/body.c
+++ b/src/body.c
@@ -1,123 +1,111 @@
 // SPDX-FileCopyrightText: 2023 Erin Catto
 // SPDX-License-Identifier: MIT
 
-#include "box2d/aabb.h"
-#include "box2d/id.h"
+#include "body.h"
 
 #include "array.h"
 #include "block_allocator.h"
-#include "body.h"
 #include "contact.h"
 #include "core.h"
 #include "graph.h"
 #include "island.h"
 #include "joint.h"
-#include "world.h"
 #include "shape.h"
+#include "world.h"
 
-b2BodyId b2World_CreateBody(b2WorldId worldId, const b2BodyDef* def)
+#include "box2d/aabb.h"
+#include "box2d/id.h"
+
+static void b2CreateIslandForBody(b2World* world, b2Body* body, bool isAwake)
 {
-	b2World* world = b2GetWorldFromId(worldId);
-	B2_ASSERT(world->locked == false);
+	B2_ASSERT(body->islandIndex == B2_NULL_INDEX);
+	B2_ASSERT(body->islandPrev == B2_NULL_INDEX);
+	B2_ASSERT(body->islandNext == B2_NULL_INDEX);
 
-	if (world->locked)
+	if (body->type == b2_staticBody)
 	{
-		return b2_nullBodyId;
+		return;
 	}
 
-	b2Body* b = (b2Body*)b2AllocObject(&world->bodyPool);
-	world->bodies = (b2Body*)world->bodyPool.memory;
+	// Every new body gets a new island. Islands get merged during simulation.
+	b2Island* island = (b2Island*)b2AllocObject(&world->islandPool);
+	world->islands = (b2Island*)world->islandPool.memory;
+	b2CreateIsland(island);
+	island->world = world;
 
-	B2_ASSERT(0 <= def->type && def->type < b2_bodyTypeCount);
-	B2_ASSERT(b2IsValidVec2(def->position));
-	B2_ASSERT(b2IsValid(def->angle));
-	B2_ASSERT(b2IsValidVec2(def->linearVelocity));
-	B2_ASSERT(b2IsValid(def->angularVelocity));
-	B2_ASSERT(b2IsValid(def->linearDamping) && def->linearDamping >= 0.0f);
-	B2_ASSERT(b2IsValid(def->angularDamping) && def->angularDamping >= 0.0f);
-	B2_ASSERT(b2IsValid(def->gravityScale) && def->gravityScale >= 0.0f);
+	body->islandIndex = island->object.index;
+	island->headBody = body->object.index;
+	island->tailBody = body->object.index;
+	island->bodyCount = 1;
 
-	b->type = def->type;
-	b->transform.p = def->position;
-	b->transform.q = b2MakeRot(def->angle);
-	b->position0 = def->position;
-	b->position = def->position;
-	b->angle0 = def->angle;
-	b->angle = def->angle;
-	b->localCenter = b2Vec2_zero;
-	b->linearVelocity = def->linearVelocity;
-	b->angularVelocity = def->angularVelocity;
-	b->deltaPosition = b2Vec2_zero;
-	b->deltaAngle = 0.0f;
-	b->force = b2Vec2_zero;
-	b->torque = 0.0f;
-	b->shapeList = B2_NULL_INDEX;
-	b->jointList = B2_NULL_INDEX;
-	b->jointCount = 0;
-	b->contactList = B2_NULL_INDEX;
-	b->contactCount = 0;
-	b->mass = 0.0f;
-	b->invMass = 0.0f;
-	b->I = 0.0f;
-	b->invI = 0.0f;
-	b->minExtent = b2_huge;
-	b->linearDamping = def->linearDamping;
-	b->angularDamping = def->angularDamping;
-	b->gravityScale = def->gravityScale;
-	b->sleepTime = 0.0f;
-	b->userData = def->userData;
-	b->world = worldId.index;
-	b->enableSleep = def->enableSleep;
-	b->fixedRotation = def->fixedRotation;
-	b->isEnabled = def->isEnabled;
-	b->isMarked = false;
-	b->enlargeAABB = false;
-	b->isFast = false;
-	b->islandIndex = B2_NULL_INDEX;
-	b->islandPrev = B2_NULL_INDEX;
-	b->islandNext = B2_NULL_INDEX;
-
-	if (b->type != b2_staticBody)
-	{
-		// Every new body gets a new island. Islands get merged during simulation.
-		b2Island* island = (b2Island*)b2AllocObject(&world->islandPool);
-		world->islands = (b2Island*)world->islandPool.memory;
-		b2CreateIsland(island);
-		island->world = world;
-
-		b->islandIndex = island->object.index;
-		island->headBody = b->object.index;
-		island->tailBody = b->object.index;
-		island->bodyCount = 1;
-
-		if (def->isAwake)
-		{
-			island->awakeIndex = b2Array(world->awakeIslandArray).count;
-			b2Array_Push(world->awakeIslandArray, island->object.index);
-		}
+	if (isAwake)
+	{
+		island->awakeIndex = b2Array(world->awakeIslandArray).count;
+		b2Array_Push(world->awakeIslandArray, island->object.index);
 	}
-
-	b2BodyId id = {b->object.index, worldId.index, b->object.revision};
-	return id;
 }
 
-void b2World_DestroyBody(b2BodyId bodyId)
+static void b2RemoveBodyFromIsland(b2World* world, b2Body* body)
 {
-	b2World* world = b2GetWorldFromIndex(bodyId.world);
-	B2_ASSERT(world->locked == false);
-
-	if (world->locked)
+	if (body->islandIndex == B2_NULL_INDEX)
 	{
+		B2_ASSERT(body->islandPrev == B2_NULL_INDEX);
+		B2_ASSERT(body->islandNext == B2_NULL_INDEX);
 		return;
 	}
 
-	B2_ASSERT(0 <= bodyId.index && bodyId.index < world->bodyPool.capacity);
+	b2Island* island = world->islands + body->islandIndex;
 
-	b2Body* body = world->bodies + bodyId.index;
+	// Fix the island's linked list of bodies
+	if (body->islandPrev != B2_NULL_INDEX)
+	{
+		world->bodies[body->islandPrev].islandNext = body->islandNext;
+	}
 
-	// User must destroy joints before destroying bodies
-	B2_ASSERT(body->jointList == B2_NULL_INDEX && body->jointCount == 0);
+	if (body->islandNext != B2_NULL_INDEX)
+	{
+		world->bodies[body->islandNext].islandPrev = body->islandPrev;
+	}
+
+	B2_ASSERT(island->bodyCount > 0);
+	island->bodyCount -= 1;
+	bool islandDestroyed = false;
 
+	if (island->headBody == body->object.index)
+	{
+		island->headBody = body->islandNext;
+
+		if (island->headBody == B2_NULL_INDEX)
+		{
+			// Destroy empty island
+			B2_ASSERT(island->tailBody == body->object.index);
+			B2_ASSERT(island->bodyCount == 0);
+			B2_ASSERT(island->contactCount == 0);
+			B2_ASSERT(island->jointCount == 0);
+
+			// Free the island
+			b2DestroyIsland(island);
+			islandDestroyed = true;
+		}
+	}
+	else if (island->tailBody == body->object.index)
+	{
+		island->tailBody = body->islandPrev;
+	}
+
+	if (islandDestroyed == false)
+	{
+		b2WakeIsland(island);
+		b2ValidateIsland(island, true);
+	}
+
+	body->islandIndex = B2_NULL_INDEX;
+	body->islandPrev = B2_NULL_INDEX;
+	body->islandNext = B2_NULL_INDEX;
+}
+
+static void b2DestroyBodyContacts(b2World* world, b2Body* body)
+{
 	// Destroy the attached contacts
 	int32_t edgeKey = body->contactList;
 	while (edgeKey != B2_NULL_INDEX)
@@ -188,101 +176,202 @@ void b2World_DestroyBody(b2BodyId bodyId)
 		b2FreeObject(&world->contactPool, &contact->object);
 	}
 
-	// Delete the attached shapes. This destroys broad-phase proxies.
+	body->contactList = B2_NULL_INDEX;
+	body->contactCount = 0;
+}
+
+static void b2EnableBody(b2World* world, b2Body* body)
+{
+	// Add shapes to broad-phase
 	int32_t shapeIndex = body->shapeList;
 	while (shapeIndex != B2_NULL_INDEX)
 	{
 		b2Shape* shape = world->shapes + shapeIndex;
 		shapeIndex = shape->nextShapeIndex;
 
-		// The broad-phase proxies only exist if the body is enabled
-		if (body->isEnabled)
+		b2Shape_CreateProxy(shape, &world->broadPhase, body->type, body->transform);
+	}
+
+	b2CreateIslandForBody(world, body, true);
+
+	int32_t jointKey = body->jointList;
+	while (jointKey != B2_NULL_INDEX)
+	{
+		int32_t jointIndex = jointKey >> 1;
+		int32_t edgeIndex = jointKey & 1;
+		b2Joint* joint = world->joints + jointIndex;
+		B2_ASSERT(joint->islandIndex == B2_NULL_INDEX);
+		b2Body* bodyA = world->bodies + joint->edges[0].bodyIndex;
+		b2Body* bodyB = world->bodies + joint->edges[1].bodyIndex;
+		if (bodyA->type == b2_dynamicBody || bodyB->type == b2_dynamicBody)
 		{
-			b2Shape_DestroyProxy(shape, &world->broadPhase);
+			b2LinkJoint(world, joint);
 		}
-
-		b2FreeObject(&world->shapePool, &shape->object);
+		jointKey = joint->edges[edgeIndex].nextKey;
 	}
+}
 
-	// Remove from island
-	if (body->islandIndex != B2_NULL_INDEX)
+static void b2DisableBody(b2World* world, b2Body* body)
+{
+	b2DestroyBodyContacts(world, body);
+	b2RemoveBodyFromIsland(world, body);
+
+	// Remove shapes from broad-phase
+	int32_t shapeIndex = body->shapeList;
+	while (shapeIndex != B2_NULL_INDEX)
 	{
-		b2Island* island = world->islands + body->islandIndex;
+		b2Shape* shape = world->shapes + shapeIndex;
+		shapeIndex = shape->nextShapeIndex;
 
-		// Fix the island's linked list of bodies
-		if (body->islandPrev != B2_NULL_INDEX)
-		{
-			world->bodies[body->islandPrev].islandNext = body->islandNext;
-		}
+		b2Shape_DestroyProxy(shape, &world->broadPhase);
+	}
 
-		if (body->islandNext != B2_NULL_INDEX)
+	int32_t jointKey = body->jointList;
+	while (jointKey != B2_NULL_INDEX)
+	{
+		int32_t jointIndex = jointKey >> 1;
+		int32_t edgeIndex = jointKey & 1;
+		b2Joint* joint = world->joints + jointIndex;
+		if (joint->islandIndex != B2_NULL_INDEX)
 		{
-			world->bodies[body->islandNext].islandPrev = body->islandPrev;
+			b2UnlinkJoint(world, joint);
 		}
+		jointKey = joint->edges[edgeIndex].nextKey;
+	}
+}
 
-		B2_ASSERT(island->bodyCount > 0);
-		island->bodyCount -= 1;
-		bool islandDestroyed = false;
+b2BodyId b2World_CreateBody(b2WorldId worldId, const b2BodyDef* def)
+{
+	b2World* world = b2GetWorldFromId(worldId);
+	B2_ASSERT(world->locked == false);
 
-		if (island->headBody == body->object.index)
-		{
-			island->headBody = body->islandNext;
-
-			if (island->headBody == B2_NULL_INDEX)
-			{
-				// Destroy empty island
-				B2_ASSERT(island->tailBody == body->object.index);
-				B2_ASSERT(island->bodyCount == 0);
-				B2_ASSERT(island->contactCount == 0);
-				B2_ASSERT(island->jointCount == 0);
-
-				// Free the island
-				b2DestroyIsland(island);
-				islandDestroyed = true;
-			}
-		}
-		else if (island->tailBody == body->object.index)
-		{
-			island->tailBody = body->islandPrev;
-		}
+	if (world->locked)
+	{
+		return b2_nullBodyId;
+	}
 
-		if (islandDestroyed == false)
-		{
-			b2WakeIsland(island);
-			b2ValidateIsland(island, true);
-		}
+	b2Body* body = (b2Body*)b2AllocObject(&world->bodyPool);
+	world->bodies = (b2Body*)world->bodyPool.memory;
+
+	B2_ASSERT(0 <= def->type && def->type < b2_bodyTypeCount);
+	B2_ASSERT(b2IsValidVec2(def->position));
+	B2_ASSERT(b2IsValid(def->angle));
+	B2_ASSERT(b2IsValidVec2(def->linearVelocity));
+	B2_ASSERT(b2IsValid(def->angularVelocity));
+	B2_ASSERT(b2IsValid(def->linearDamping) && def->linearDamping >= 0.0f);
+	B2_ASSERT(b2IsValid(def->angularDamping) && def->angularDamping >= 0.0f);
+	B2_ASSERT(b2IsValid(def->gravityScale) && def->gravityScale >= 0.0f);
+
+	body->type = def->type;
+	body->transform.p = def->position;
+	body->transform.q = b2MakeRot(def->angle);
+	body->position0 = def->position;
+	body->position = def->position;
+	body->angle0 = def->angle;
+	body->angle = def->angle;
+	body->localCenter = b2Vec2_zero;
+	body->linearVelocity = def->linearVelocity;
+	body->angularVelocity = def->angularVelocity;
+	body->deltaPosition = b2Vec2_zero;
+	body->deltaAngle = 0.0f;
+	body->force = b2Vec2_zero;
+	body->torque = 0.0f;
+	body->shapeList = B2_NULL_INDEX;
+	body->jointList = B2_NULL_INDEX;
+	body->jointCount = 0;
+	body->contactList = B2_NULL_INDEX;
+	body->contactCount = 0;
+	body->mass = 0.0f;
+	body->invMass = 0.0f;
+	body->I = 0.0f;
+	body->invI = 0.0f;
+	body->minExtent = b2_huge;
+	body->linearDamping = def->linearDamping;
+	body->angularDamping = def->angularDamping;
+	body->gravityScale = def->gravityScale;
+	body->sleepTime = 0.0f;
+	body->userData = def->userData;
+	body->world = worldId.index;
+	body->enableSleep = def->enableSleep;
+	body->fixedRotation = def->fixedRotation;
+	body->isEnabled = def->isEnabled;
+	body->isMarked = false;
+	body->enlargeAABB = false;
+	body->isFast = false;
+	body->islandIndex = B2_NULL_INDEX;
+	body->islandPrev = B2_NULL_INDEX;
+	body->islandNext = B2_NULL_INDEX;
+
+	if (body->isEnabled)
+	{
+		b2CreateIslandForBody(world, body, def->isAwake);
+	}
+
+	b2BodyId id = {body->object.index, worldId.index, body->object.revision};
+	return id;
+}
+
+void b2World_DestroyBody(b2BodyId bodyId)
+{
+	b2World* world = b2GetWorldFromIndex(bodyId.world);
+	B2_ASSERT(world->locked == false);
+
+	if (world->locked)
+	{
+		return;
 	}
 
-	// Free body
+	B2_ASSERT(0 <= bodyId.index && bodyId.index < world->bodyPool.capacity);
+
+	b2Body* body = world->bodies + bodyId.index;
+
+	// User must destroy joints before destroying bodies
+	B2_ASSERT(body->jointList == B2_NULL_INDEX && body->jointCount == 0);
+
+	b2DestroyBodyContacts(world, body);
+
+	// Delete the attached shapes and their broad-phase proxies.
+	int32_t shapeIndex = body->shapeList;
+	while (shapeIndex != B2_NULL_INDEX)
+	{
+		b2Shape* shape = world->shapes + shapeIndex;
+		shapeIndex = shape->nextShapeIndex;
+
+		b2Shape_DestroyProxy(shape, &world->broadPhase);
+		b2FreeObject(&world->shapePool, &shape->object);
+	}
+
+	b2RemoveBodyFromIsland(world, body);
+
 	b2FreeObject(&world->bodyPool, &body->object);
 }
 
-static void b2ComputeMass(b2World* w, b2Body* b)
+static void b2ComputeMass(b2World* world, b2Body* body)
 {
 	// Compute mass data from shapes. Each shape has its own density.
-	b->mass = 0.0f;
-	b->invMass = 0.0f;
-	b->I = 0.0f;
-	b->invI = 0.0f;
-	b->localCenter = b2Vec2_zero;
-	b->minExtent = b2_huge;
-	b->maxExtent = 0.0f;
+	body->mass = 0.0f;
+	body->invMass = 0.0f;
+	body->I = 0.0f;
+	body->invI = 0.0f;
+	body->localCenter = b2Vec2_zero;
+	body->minExtent = b2_huge;
+	body->maxExtent = 0.0f;
 
 	// Static and kinematic bodies have zero mass.
-	if (b->type == b2_staticBody || b->type == b2_kinematicBody)
+	if (body->type == b2_staticBody || body->type == b2_kinematicBody)
 	{
-		b->position = b->transform.p;
+		body->position = body->transform.p;
 		return;
 	}
 
-	B2_ASSERT(b->type == b2_dynamicBody);
+	B2_ASSERT(body->type == b2_dynamicBody);
 
 	// Accumulate mass over all shapes.
 	b2Vec2 localCenter = b2Vec2_zero;
-	int32_t shapeIndex = b->shapeList;
+	int32_t shapeIndex = body->shapeList;
 	while (shapeIndex != B2_NULL_INDEX)
 	{
-		const b2Shape* s = w->shapes + shapeIndex;
+		const b2Shape* s = world->shapes + shapeIndex;
 		shapeIndex = s->nextShapeIndex;
 
 		if (s->density == 0.0f)
@@ -292,42 +381,42 @@ static void b2ComputeMass(b2World* w, b2Body* b)
 
 		b2MassData massData = b2Shape_ComputeMass(s);
 
-		b->mass += massData.mass;
+		body->mass += massData.mass;
 		localCenter = b2MulAdd(localCenter, massData.mass, massData.center);
-		b->I += massData.I;
+		body->I += massData.I;
 
-		b->minExtent = B2_MIN(b->minExtent, massData.minExtent);
-		b->maxExtent = B2_MAX(b->maxExtent, massData.maxExtent);
+		body->minExtent = B2_MIN(body->minExtent, massData.minExtent);
+		body->maxExtent = B2_MAX(body->maxExtent, massData.maxExtent);
 	}
 
 	// Compute center of mass.
-	if (b->mass > 0.0f)
+	if (body->mass > 0.0f)
 	{
-		b->invMass = 1.0f / b->mass;
-		localCenter = b2MulSV(b->invMass, localCenter);
+		body->invMass = 1.0f / body->mass;
+		localCenter = b2MulSV(body->invMass, localCenter);
 	}
 
-	if (b->I > 0.0f && b->fixedRotation == false)
+	if (body->I > 0.0f && body->fixedRotation == false)
 	{
 		// Center the inertia about the center of mass.
-		b->I -= b->mass * b2Dot(localCenter, localCenter);
-		B2_ASSERT(b->I > 0.0f);
-		b->invI = 1.0f / b->I;
+		body->I -= body->mass * b2Dot(localCenter, localCenter);
+		B2_ASSERT(body->I > 0.0f);
+		body->invI = 1.0f / body->I;
 	}
 	else
 	{
-		b->I = 0.0f;
-		b->invI = 0.0f;
+		body->I = 0.0f;
+		body->invI = 0.0f;
 	}
 
 	// Move center of mass.
-	b2Vec2 oldCenter = b->position;
-	b->localCenter = localCenter;
-	b->position = b2TransformPoint(b->transform, b->localCenter);
+	b2Vec2 oldCenter = body->position;
+	body->localCenter = localCenter;
+	body->position = b2TransformPoint(body->transform, body->localCenter);
 
 	// Update center of mass velocity.
-	b2Vec2 deltaLinear = b2CrossSV(b->angularVelocity, b2Sub(b->position, oldCenter));
-	b->linearVelocity = b2Add(b->linearVelocity, deltaLinear);
+	b2Vec2 deltaLinear = b2CrossSV(body->angularVelocity, b2Sub(body->position, oldCenter));
+	body->linearVelocity = b2Add(body->linearVelocity, deltaLinear);
 }
 
 static b2ShapeId b2CreateShape(b2BodyId bodyId, const b2ShapeDef* def, const void* geometry, b2ShapeType shapeType)
@@ -352,25 +441,25 @@ static b2ShapeId b2CreateShape(b2BodyId bodyId, const b2ShapeDef* def, const voi
 
 	switch (shapeType)
 	{
-	case b2_capsuleShape:
+		case b2_capsuleShape:
 			shape->capsule = *(const b2Capsule*)geometry;
-		break;
+			break;
 
-	case b2_circleShape:
+		case b2_circleShape:
 			shape->circle = *(const b2Circle*)geometry;
-		break;
+			break;
 
-	case b2_polygonShape:
+		case b2_polygonShape:
 			shape->polygon = *(const b2Polygon*)geometry;
-		break;
+			break;
 
-	case b2_segmentShape:
+		case b2_segmentShape:
 			shape->segment = *(const b2Segment*)geometry;
-		break;
+			break;
 
-	default:
-		B2_ASSERT(false);
-		break;
+		default:
+			B2_ASSERT(false);
+			break;
 	}
 
 	shape->bodyIndex = body->object.index;
@@ -384,6 +473,7 @@ static b2ShapeId b2CreateShape(b2BodyId bodyId, const b2ShapeDef* def, const voi
 	shape->enlargedAABB = false;
 	shape->reportContacts = false;
 	shape->isFast = false;
+	shape->proxyKey = B2_NULL_INDEX;
 
 	if (body->isEnabled)
 	{
@@ -394,7 +484,7 @@ static b2ShapeId b2CreateShape(b2BodyId bodyId, const b2ShapeDef* def, const voi
 	shape->nextShapeIndex = body->shapeList;
 	body->shapeList = shape->object.index;
 
-	if (shape->density)
+	if (shape->density > 0.0f)
 	{
 		b2ComputeMass(w, body);
 	}
@@ -422,17 +512,6 @@ b2ShapeId b2Body_CreateSegment(b2BodyId bodyId, const b2ShapeDef* def, const b2S
 		return b2_nullShapeId;
 	}
 
-	//b2Vec2 axis = b2Normalize(b2Sub(segment->point2, segment->point1));
-
-	//b2Polygon polygon;
-	//polygon.vertices[0] = segment->point1;
-	//polygon.vertices[1] = segment->point2;
-	//polygon.count = 2;
-	//polygon.radius = 0.0f;
-	//
-	//polygon.normals[0] = b2RightPerp(axis);
-	//polygon.normals[1] = b2Neg(polygon.normals[0]);
-
 	return b2CreateShape(bodyId, def, segment, b2_segmentShape);
 }
 
@@ -445,17 +524,6 @@ b2ShapeId b2Body_CreateCapsule(b2BodyId bodyId, const b2ShapeDef* def, const b2C
 		return b2_nullShapeId;
 	}
 
-	//b2Vec2 axis = b2Normalize(b2Sub(segment->point2, segment->point1));
-
-	//b2Polygon polygon;
-	//polygon.vertices[0] = segment->point1;
-	//polygon.vertices[1] = segment->point2;
-	//polygon.count = 2;
-	//polygon.radius = 0.0f;
-	//
-	//polygon.normals[0] = b2RightPerp(axis);
-	//polygon.normals[1] = b2Neg(polygon.normals[0]);
-
 	return b2CreateShape(bodyId, def, capsule, b2_capsuleShape);
 }
 
@@ -504,8 +572,8 @@ void b2Body_DestroyShape(b2ShapeId shapeId)
 	// TODO_ERIN
 	B2_ASSERT(false);
 	// Destroy any contacts associated with the shape.
-	//b2ContactEdge* edge = m_contactList;
-	//while (edge)
+	// b2ContactEdge* edge = m_contactList;
+	// while (edge)
 	//{
 	//	b2Contact* c = edge->contact;
 	//	edge = edge->next;
@@ -568,6 +636,13 @@ b2Vec2 b2Body_GetLocalPoint(b2BodyId bodyId, b2Vec2 globalPoint)
 	return b2InvTransformPoint(world->bodies[bodyId.index].transform, globalPoint);
 }
 
+b2Vec2 b2Body_GetWorldPoint(b2BodyId bodyId, b2Vec2 localPoint)
+{
+	b2World* world = b2GetWorldFromIndex(bodyId.world);
+	B2_ASSERT(0 <= bodyId.index && bodyId.index < world->bodyPool.capacity);
+	return b2TransformPoint(world->bodies[bodyId.index].transform, localPoint);
+}
+
 void b2Body_SetTransform(b2BodyId bodyId, b2Vec2 position, float angle)
 {
 	b2World* world = b2GetWorldFromIndex(bodyId.world);
@@ -605,6 +680,20 @@ void b2Body_SetTransform(b2BodyId bodyId, b2Vec2 position, float angle)
 	}
 }
 
+b2Vec2 b2Body_GetLinearVelocity(b2BodyId bodyId)
+{
+	b2World* world = b2GetWorldFromIndex(bodyId.world);
+	B2_ASSERT(0 <= bodyId.index && bodyId.index < world->bodyPool.capacity);
+	return world->bodies[bodyId.index].linearVelocity;
+}
+
+float b2Body_GetAngularVelocity(b2BodyId bodyId)
+{
+	b2World* world = b2GetWorldFromIndex(bodyId.world);
+	B2_ASSERT(0 <= bodyId.index && bodyId.index < world->bodyPool.capacity);
+	return world->bodies[bodyId.index].angularVelocity;
+}
+
 void b2Body_SetLinearVelocity(b2BodyId bodyId, b2Vec2 linearVelocity)
 {
 	b2World* world = b2GetWorldFromIndex(bodyId.world);
@@ -626,6 +715,33 @@ b2BodyType b2Body_GetType(b2BodyId bodyId)
 	return world->bodies[bodyId.index].type;
 }
 
+void b2Body_SetType(b2BodyId bodyId, b2BodyType type)
+{
+	b2World* world = b2GetWorldFromIndex(bodyId.world);
+	B2_ASSERT(0 <= bodyId.index && bodyId.index < world->bodyPool.capacity);
+	b2Body* body = world->bodies + bodyId.index;
+	if (body->type == type)
+	{
+		return;
+	}
+
+	if (body->isEnabled == true)
+	{
+		b2DisableBody(world, body);
+
+		body->type = type;
+
+		b2EnableBody(world, body);
+	}
+	else
+	{
+		body->type = type;
+	}
+
+	// Body type affects the mass
+	b2ComputeMass(world, body);
+}
+
 float b2Body_GetMass(b2BodyId bodyId)
 {
 	b2World* world = b2GetWorldFromIndex(bodyId.world);
@@ -633,6 +749,27 @@ float b2Body_GetMass(b2BodyId bodyId)
 	return world->bodies[bodyId.index].mass;
 }
 
+float b2Body_GetInertiaTensor(b2BodyId bodyId)
+{
+	b2World* world = b2GetWorldFromIndex(bodyId.world);
+	B2_ASSERT(0 <= bodyId.index && bodyId.index < world->bodyPool.capacity);
+	return world->bodies[bodyId.index].I;
+}
+
+b2Vec2 b2Body_GetLocalCenterOfMass(b2BodyId bodyId)
+{
+	b2World* world = b2GetWorldFromIndex(bodyId.world);
+	B2_ASSERT(0 <= bodyId.index && bodyId.index < world->bodyPool.capacity);
+	return world->bodies[bodyId.index].localCenter;
+}
+
+b2Vec2 b2Body_GetWorldCenterOfMass(b2BodyId bodyId)
+{
+	b2World* world = b2GetWorldFromIndex(bodyId.world);
+	B2_ASSERT(0 <= bodyId.index && bodyId.index < world->bodyPool.capacity);
+	return world->bodies[bodyId.index].position;
+}
+
 void b2Body_Wake(b2BodyId bodyId)
 {
 	b2World* world = b2GetWorldFromIndex(bodyId.world);
@@ -649,6 +786,37 @@ void b2Body_Wake(b2BodyId bodyId)
 	b2WakeIsland(world->islands + islandIndex);
 }
 
+bool b2Body_IsEnabled(b2BodyId bodyId)
+{
+	b2World* world = b2GetWorldFromIndex(bodyId.world);
+	B2_ASSERT(0 <= bodyId.index && bodyId.index < world->bodyPool.capacity);
+	return world->bodies[bodyId.index].isEnabled;
+}
+
+void b2Body_Disable(b2BodyId bodyId)
+{
+	b2World* world = b2GetWorldFromIndex(bodyId.world);
+	B2_ASSERT(0 <= bodyId.index && bodyId.index < world->bodyPool.capacity);
+	b2Body* body = world->bodies + bodyId.index;
+	if (body->isEnabled == true)
+	{
+		b2DisableBody(world, body);
+		body->isEnabled = false;
+	}
+}
+
+void b2Body_Enable(b2BodyId bodyId)
+{
+	b2World* world = b2GetWorldFromIndex(bodyId.world);
+	B2_ASSERT(0 <= bodyId.index && bodyId.index < world->bodyPool.capacity);
+	b2Body* body = world->bodies + bodyId.index;
+	if (body->isEnabled == false)
+	{
+		b2EnableBody(world, body);
+		body->isEnabled = true;
+	}
+}
+
 bool b2ShouldBodiesCollide(b2World* world, b2Body* bodyA, b2Body* bodyB)
 {
 	int32_t jointKey;
@@ -807,14 +975,14 @@ void b2Body::SetFixedRotation(bool flag)
 
 void b2Body_Dump(b2Body* b)
 {
-	int32_t bodyIndex = b->islandIndex;
+	int32_t bodyIndex = body->islandIndex;
 
 	// %.9g is sufficient to save and load the same value using text
 	// FLT_DECIMAL_DIG == 9
 
 	b2Dump("{\n");
 	b2Dump("  b2BodyDef bd;\n");
-	b2Dump("  bd.type = b2BodyType(%d);\n", b->type);
+	b2Dump("  bd.type = b2BodyType(%d);\n", body->type);
 	b2Dump("  bd.position.Set(%.9g, %.9g);\n", m_xf.p.x, m_xf.p.y);
 	b2Dump("  bd.angle = %.9g;\n", m_sweep.a);
 	b2Dump("  bd.linearVelocity.Set(%.9g, %.9g);\n", m_linearVelocity.x, m_linearVelocity.y);
diff --git a/src/broad_phase.c b/src/broad_phase.c
index 3065bd7c..5ef1c58b 100644
--- a/src/broad_phase.c
+++ b/src/broad_phase.c
@@ -132,6 +132,7 @@ void b2BroadPhase_MoveProxy(b2BroadPhase* bp, int32_t proxyKey, b2AABB aabb)
 
 void b2BroadPhase_EnlargeProxy(b2BroadPhase* bp, int32_t proxyKey, b2AABB aabb)
 {
+	B2_ASSERT(proxyKey != B2_NULL_INDEX);
 	int32_t typeIndex = B2_PROXY_TYPE(proxyKey);
 	int32_t proxyId = B2_PROXY_ID(proxyKey);
 
diff --git a/src/graph.c b/src/graph.c
index a5c493bc..16145c5c 100644
--- a/src/graph.c
+++ b/src/graph.c
@@ -88,8 +88,9 @@ void b2AddContactToGraph(b2World* world, b2Contact* contact)
 
 	b2BodyType typeA = world->bodies[bodyIndexA].type;
 	b2BodyType typeB = world->bodies[bodyIndexB].type;
+	B2_ASSERT(typeA != b2_staticBody || typeB != b2_staticBody);
 
-	if (typeA == b2_dynamicBody && typeB == b2_dynamicBody)
+	if (typeA != b2_staticBody && typeB != b2_staticBody)
 	{
 		for (int32_t i = 0; i < b2_graphColorCount; ++i)
 		{
@@ -108,7 +109,7 @@ void b2AddContactToGraph(b2World* world, b2Contact* contact)
 			break;
 		}
 	}
-	else if (typeA == b2_dynamicBody)
+	else if (typeA != b2_staticBody)
 	{
 		// Static contacts never in color 0
 		for (int32_t i = 1; i < b2_graphColorCount; ++i)
@@ -127,7 +128,7 @@ void b2AddContactToGraph(b2World* world, b2Contact* contact)
 			break;
 		}
 	}
-	else if (typeB == b2_dynamicBody)
+	else if (typeB != b2_staticBody)
 	{
 		// Static contacts never in color 0
 		for (int32_t i = 1; i < b2_graphColorCount; ++i)
@@ -188,8 +189,9 @@ void b2RemoveContactFromGraph(b2World* world, b2Contact* contact)
 
 	b2BodyType typeA = world->bodies[bodyIndexA].type;
 	b2BodyType typeB = world->bodies[bodyIndexB].type;
+	B2_ASSERT(typeA != b2_staticBody || typeB != b2_staticBody);
 
-	if (typeA == b2_dynamicBody && typeB == b2_dynamicBody)
+	if (typeA != b2_staticBody && typeB != b2_staticBody)
 	{
 		b2GraphColor* color = graph->colors + contact->colorIndex;
 		B2_ASSERT(b2GetBit(&color->bodySet, bodyIndexA) && b2GetBit(&color->bodySet, bodyIndexB));
@@ -206,7 +208,7 @@ void b2RemoveContactFromGraph(b2World* world, b2Contact* contact)
 		b2ClearBit(&color->bodySet, bodyIndexA);
 		b2ClearBit(&color->bodySet, bodyIndexB);
 	}
-	else if (typeA == b2_dynamicBody)
+	else if (typeA != b2_staticBody)
 	{
 		b2GraphColor* color = graph->colors + contact->colorIndex;
 		B2_ASSERT(b2GetBit(&color->bodySet, bodyIndexA));
@@ -222,7 +224,7 @@ void b2RemoveContactFromGraph(b2World* world, b2Contact* contact)
 
 		b2ClearBit(&color->bodySet, bodyIndexA);
 	}
-	else if (typeB == b2_dynamicBody)
+	else if (typeB != b2_staticBody)
 	{
 		b2GraphColor* color = graph->colors + contact->colorIndex;
 		B2_ASSERT(b2GetBit(&color->bodySet, bodyIndexB));
diff --git a/src/joint.c b/src/joint.c
index 7509d8b8..853f9588 100644
--- a/src/joint.c
+++ b/src/joint.c
@@ -7,6 +7,7 @@
 #include "contact.h"
 #include "core.h"
 #include "shape.h"
+#include "solver_data.h"
 #include "world.h"
 
 #include "box2d/debug_draw.h"
@@ -123,6 +124,9 @@ static b2Joint* b2CreateJoint(b2World* world, b2Body* bodyA, b2Body* bodyB)
 
 	if (bodyA->type == b2_dynamicBody || bodyB->type == b2_dynamicBody)
 	{
+		// TODO_ERIN
+		B2_ASSERT(bodyA->isEnabled == true && bodyB->isEnabled == true);
+		
 		// Add edge to island graph
 		b2LinkJoint(world, joint);
 
@@ -244,7 +248,58 @@ b2JointId b2World_CreateRevoluteJoint(b2WorldId worldId, const b2RevoluteJointDe
 	joint->revoluteJoint.motorSpeed = def->motorSpeed;
 	joint->revoluteJoint.enableLimit = def->enableLimit;
 	joint->revoluteJoint.enableMotor = def->enableMotor;
-	joint->revoluteJoint.angle = 0.0f;
+
+	// If the joint prevents collisions, then destroy all contacts between attached bodies
+	if (def->collideConnected == false)
+	{
+		b2DestroyContactsBetweenBodies(world, bodyA, bodyB);
+	}
+
+	b2JointId jointId = {joint->object.index, world->index, joint->object.revision};
+
+	return jointId;
+}
+
+b2JointId b2World_CreatePrismaticJoint(b2WorldId worldId, const b2PrismaticJointDef* def)
+{
+	b2World* world = b2GetWorldFromId(worldId);
+
+	B2_ASSERT(world->locked == false);
+
+	if (world->locked)
+	{
+		return b2_nullJointId;
+	}
+
+	B2_ASSERT(b2IsBodyIdValid(world, def->bodyIdA));
+	B2_ASSERT(b2IsBodyIdValid(world, def->bodyIdB));
+
+	b2Body* bodyA = world->bodies + def->bodyIdA.index;
+	b2Body* bodyB = world->bodies + def->bodyIdB.index;
+
+	b2Joint* joint = b2CreateJoint(world, bodyA, bodyB);
+
+	joint->type = b2_prismaticJoint;
+	joint->localAnchorA = def->localAnchorA;
+	joint->localAnchorB = def->localAnchorB;
+	joint->collideConnected = def->collideConnected;
+
+	b2PrismaticJoint empty = {0};
+	joint->prismaticJoint = empty;
+
+	joint->prismaticJoint.localAxisA = b2Normalize(def->localAxisA);
+	joint->prismaticJoint.referenceAngle = def->referenceAngle;
+	joint->prismaticJoint.impulse = b2Vec2_zero;
+	joint->prismaticJoint.axialMass = 0.0f;
+	joint->prismaticJoint.motorImpulse = 0.0f;
+	joint->prismaticJoint.lowerImpulse = 0.0f;
+	joint->prismaticJoint.upperImpulse = 0.0f;
+	joint->prismaticJoint.lowerTranslation = def->lowerTranslation;
+	joint->prismaticJoint.upperTranslation = def->upperTranslation;
+	joint->prismaticJoint.maxMotorForce = def->maxMotorForce;
+	joint->prismaticJoint.motorSpeed = def->motorSpeed;
+	joint->prismaticJoint.enableLimit = def->enableLimit;
+	joint->prismaticJoint.enableMotor = def->enableMotor;
 
 	// If the joint prevents collisions, then destroy all contacts between attached bodies
 	if (def->collideConnected == false)
@@ -421,17 +476,30 @@ b2BodyId b2Joint_GetBodyB(b2JointId jointId)
 }
 
 extern void b2PrepareMouse(b2Joint* base, b2StepContext* context);
+extern void b2PreparePrismatic(b2Joint* base, b2StepContext* context);
 extern void b2PrepareRevolute(b2Joint* base, b2StepContext* context);
 extern void b2PrepareWeld(b2Joint* base, b2StepContext* context);
 
 void b2PrepareJoint(b2Joint* joint, b2StepContext* context)
 {
+	// TODO_ERIN temp until joints are in graph
+	b2Body* bodyA = context->bodies + joint->edges[0].bodyIndex;
+	b2Body* bodyB = context->bodies + joint->edges[1].bodyIndex;
+	if (bodyA->isEnabled == false || bodyB->isEnabled == false)
+	{
+		return;
+	}
+
 	switch (joint->type)
 	{
 		case b2_mouseJoint:
 			b2PrepareMouse(joint, context);
 			break;
 
+		case b2_prismaticJoint:
+			b2PreparePrismatic(joint, context);
+			break;
+
 		case b2_revoluteJoint:
 			b2PrepareRevolute(joint, context);
 			break;
@@ -446,26 +514,39 @@ void b2PrepareJoint(b2Joint* joint, b2StepContext* context)
 }
 
 extern void b2SolveMouseVelocity(b2Joint* base, b2StepContext* context);
-extern void b2SolveRevoluteVelocity(b2Joint* base, b2StepContext* context, bool removeOverlap);
-extern void b2SolveWeldVelocity(b2Joint* base, b2StepContext* context, bool removeOverlap);
+extern void b2SolvePrismaticVelocity(b2Joint* base, b2StepContext* context, bool useBias);
+extern void b2SolveRevoluteVelocity(b2Joint* base, b2StepContext* context, bool useBias);
+extern void b2SolveWeldVelocity(b2Joint* base, b2StepContext* context, bool useBias);
 
-void b2SolveJointVelocity(b2Joint* joint, b2StepContext* context, bool removeOverlap)
+void b2SolveJointVelocity(b2Joint* joint, b2StepContext* context, bool useBias)
 {
+	// TODO_ERIN temp until joints are in graph
+	b2Body* bodyA = context->bodies + joint->edges[0].bodyIndex;
+	b2Body* bodyB = context->bodies + joint->edges[1].bodyIndex;
+	if (bodyA->isEnabled == false || bodyB->isEnabled == false)
+	{
+		return;
+	}
+
 	switch (joint->type)
 	{
 		case b2_mouseJoint:
-			if (removeOverlap)
+			if (useBias)
 			{
 				b2SolveMouseVelocity(joint, context);
 			}
 			break;
 
+		case b2_prismaticJoint:
+			b2SolvePrismaticVelocity(joint, context, useBias);
+			break;
+
 		case b2_revoluteJoint:
-			b2SolveRevoluteVelocity(joint, context, removeOverlap);
+			b2SolveRevoluteVelocity(joint, context, useBias);
 			break;
 
 		case b2_weldJoint:
-			b2SolveWeldVelocity(joint, context, removeOverlap);
+			b2SolveWeldVelocity(joint, context, useBias);
 			break;
 
 		default:
@@ -473,12 +554,17 @@ void b2SolveJointVelocity(b2Joint* joint, b2StepContext* context, bool removeOve
 	}
 }
 
+extern void b2DrawPrismatic(b2DebugDraw* draw, b2Joint* base, b2Body* bodyA, b2Body* bodyB);
 extern void b2DrawRevolute(b2DebugDraw* draw, b2Joint* base, b2Body* bodyA, b2Body* bodyB);
 
 void b2DrawJoint(b2DebugDraw* draw, b2World* world, b2Joint* joint)
 {
 	b2Body* bodyA = world->bodies + joint->edges[0].bodyIndex;
 	b2Body* bodyB = world->bodies + joint->edges[1].bodyIndex;
+	if (bodyA->isEnabled == false || bodyB->isEnabled == false)
+	{
+		return;
+	}
 
 	b2Transform xfA = bodyA->transform;
 	b2Transform xfB = bodyB->transform;
@@ -517,6 +603,10 @@ void b2DrawJoint(b2DebugDraw* draw, b2World* world, b2Joint* joint)
 		}
 		break;
 
+		case b2_prismaticJoint:
+			b2DrawPrismatic(draw, joint, bodyA, bodyB);
+			break;
+
 		case b2_revoluteJoint:
 			b2DrawRevolute(draw, joint, bodyA, bodyB);
 			break;
diff --git a/src/joint.h b/src/joint.h
index d21c5514..121ceba1 100644
--- a/src/joint.h
+++ b/src/joint.h
@@ -13,17 +13,16 @@ typedef struct b2World b2World;
 
 typedef enum b2JointType
 {
-	b2_unknownJoint,
-	b2_revoluteJoint,
-	b2_prismaticJoint,
 	b2_distanceJoint,
-	b2_pulleyJoint,
-	b2_mouseJoint,
+	b2_frictionJoint,
 	b2_gearJoint,
-	b2_wheelJoint,
+	b2_motorJoint,
+	b2_mouseJoint,
+	b2_prismaticJoint,
+	b2_pulleyJoint,
+	b2_revoluteJoint,
 	b2_weldJoint,
-	b2_frictionJoint,
-	b2_motorJoint
+	b2_wheelJoint,
 } b2JointType;
 
 /// A joint edge is used to connect bodies and joints together
@@ -86,10 +85,40 @@ typedef struct b2RevoluteJoint
 	float biasCoefficient;
 	float massCoefficient;
 	float impulseCoefficient;
-	float angle;
 	float axialMass;
 } b2RevoluteJoint;
 
+typedef struct b2PrismaticJoint
+{
+	// Solver shared
+	b2Vec2 localAxisA;
+	b2Vec2 impulse;
+	float motorImpulse;
+	float lowerImpulse;
+	float upperImpulse;
+	bool enableMotor;
+	float maxMotorForce;
+	float motorSpeed;
+	bool enableLimit;
+	float referenceAngle;
+	float lowerTranslation;
+	float upperTranslation;
+
+	// Solver temp
+	int32_t indexA;
+	int32_t indexB;
+	b2Vec2 positionA;
+	b2Vec2 positionB;
+	float angleA;
+	float angleB;
+	b2Vec2 localCenterA;
+	b2Vec2 localCenterB;
+	float biasCoefficient;
+	float massCoefficient;
+	float impulseCoefficient;
+	float axialMass;
+} b2PrismaticJoint;
+
 typedef struct b2WeldJoint
 {
 	// Solver shared
@@ -142,6 +171,7 @@ typedef struct b2Joint
 	{
 		b2MouseJoint mouseJoint;
 		b2RevoluteJoint revoluteJoint;
+		b2PrismaticJoint prismaticJoint;
 		b2WeldJoint weldJoint;
 	};
 
diff --git a/src/prismatic_joint.c b/src/prismatic_joint.c
new file mode 100644
index 00000000..fb211818
--- /dev/null
+++ b/src/prismatic_joint.c
@@ -0,0 +1,526 @@
+// SPDX-FileCopyrightText: 2023 Erin Catto
+// SPDX-License-Identifier: MIT
+
+#define _CRT_SECURE_NO_WARNINGS
+
+#include "body.h"
+#include "core.h"
+#include "joint.h"
+#include "solver_data.h"
+#include "world.h"
+
+#include "box2d/debug_draw.h"
+
+#include <stdio.h>
+
+// Linear constraint (point-to-line)
+// d = p2 - p1 = x2 + r2 - x1 - r1
+// C = dot(perp, d)
+// Cdot = dot(d, cross(w1, perp)) + dot(perp, v2 + cross(w2, r2) - v1 - cross(w1, r1))
+//      = -dot(perp, v1) - dot(cross(d + r1, perp), w1) + dot(perp, v2) + dot(cross(r2, perp), v2)
+// J = [-perp, -cross(d + r1, perp), perp, cross(r2,perp)]
+//
+// Angular constraint
+// C = a2 - a1 + a_initial
+// Cdot = w2 - w1
+// J = [0 0 -1 0 0 1]
+//
+// K = J * invM * JT
+//
+// J = [-a -s1 a s2]
+//     [0  -1  0  1]
+// a = perp
+// s1 = cross(d + r1, a) = cross(p2 - x1, a)
+// s2 = cross(r2, a) = cross(p2 - x2, a)
+
+// Motor/Limit linear constraint
+// C = dot(ax1, d)
+// Cdot = -dot(ax1, v1) - dot(cross(d + r1, ax1), w1) + dot(ax1, v2) + dot(cross(r2, ax1), v2)
+// J = [-ax1 -cross(d+r1,ax1) ax1 cross(r2,ax1)]
+
+// Predictive limit is applied even when the limit is not active.
+// Prevents a constraint speed that can lead to a constraint error in one time step.
+// Want C2 = C1 + h * Cdot >= 0
+// Or:
+// Cdot + C1/h >= 0
+// I do not apply a negative constraint error because that is handled in position correction.
+// So:
+// Cdot + max(C1, 0)/h >= 0
+
+// Block Solver
+// We develop a block solver that includes the angular and linear constraints. This makes the limit stiffer.
+//
+// The Jacobian has 2 rows:
+// J = [-uT -s1 uT s2] // linear
+//     [0   -1   0  1] // angular
+//
+// u = perp
+// s1 = cross(d + r1, u), s2 = cross(r2, u)
+// a1 = cross(d + r1, v), a2 = cross(r2, v)
+
+void b2PreparePrismatic(b2Joint* base, b2StepContext* context)
+{
+	B2_ASSERT(base->type == b2_prismaticJoint);
+
+	int32_t indexA = base->edges[0].bodyIndex;
+	int32_t indexB = base->edges[1].bodyIndex;
+	b2Body* bodyA = context->bodies + indexA;
+	b2Body* bodyB = context->bodies + indexB;
+
+	B2_ASSERT(b2ObjectValid(&bodyA->object));
+	B2_ASSERT(b2ObjectValid(&bodyB->object));
+
+	b2PrismaticJoint* joint = &base->prismaticJoint;
+
+	joint->indexA = context->bodyToSolverMap[indexA];
+	joint->indexB = context->bodyToSolverMap[indexB];
+	joint->localCenterA = bodyA->localCenter;
+	joint->localCenterB = bodyB->localCenter;
+	joint->positionA = bodyA->position;
+	joint->positionB = bodyB->position;
+	joint->angleA = bodyA->angle;
+	joint->angleB = bodyB->angle;
+
+	// This is a dummy body to represent a static body since static bodies don't have a solver body.
+	b2SolverBody dummyBody = {0};
+
+	// Note: must warm start solver bodies
+	b2SolverBody* solverBodyA = joint->indexA == B2_NULL_INDEX ? &dummyBody : context->solverBodies + joint->indexA;
+	float mA = solverBodyA->invMass;
+	float iA = solverBodyA->invI;
+
+	b2SolverBody* solverBodyB = joint->indexB == B2_NULL_INDEX ? &dummyBody : context->solverBodies + joint->indexB;
+	float mB = solverBodyB->invMass;
+	float iB = solverBodyB->invI;
+
+	b2Rot qA = bodyA->transform.q;
+	b2Rot qB = bodyB->transform.q;
+
+	// Compute the effective masses.
+	b2Vec2 rA = b2RotateVector(qA, b2Sub(base->localAnchorA, joint->localCenterA));
+	b2Vec2 rB = b2RotateVector(qB, b2Sub(base->localAnchorB, joint->localCenterB));
+	b2Vec2 d = b2Add(b2Sub(bodyB->position, bodyA->position),  b2Sub(rB, rA));
+
+	b2Vec2 axis = b2RotateVector(qA, joint->localAxisA);
+	float a1 = b2Cross(b2Add(d, rA), axis);
+	float a2 = b2Cross(rB, axis);
+
+	float k = mA + mB + iA * a1 * a1 + iB * a2 * a2;
+	joint->axialMass = k > 0.0f ? 1.0f / k : 0.0f;
+
+	// hertz = 1/4 * substep Hz
+	const float hertz = 0.25f * context->velocityIterations * context->inv_dt;
+	const float zeta = 1.0f;
+	float omega = 2.0f * b2_pi * hertz;
+	float h = context->dt;
+
+	joint->biasCoefficient = omega / (2.0f * zeta + h * omega);
+	float c = h * omega * (2.0f * zeta + h * omega);
+	joint->impulseCoefficient = 1.0f / (1.0f + c);
+	joint->massCoefficient = c * joint->impulseCoefficient;
+
+	if (joint->enableLimit == false)
+	{
+		joint->lowerImpulse = 0.0f;
+		joint->upperImpulse = 0.0f;
+	}
+
+	if (joint->enableMotor == false)
+	{
+		joint->motorImpulse = 0.0f;
+	}
+
+	if (context->enableWarmStarting)
+	{
+		float dtRatio = context->dtRatio;
+
+		// Soft step works best when bilateral constraints have no warm starting.
+		joint->impulse = b2Vec2_zero;
+		joint->motorImpulse *= dtRatio;
+		joint->lowerImpulse *= dtRatio;
+		joint->upperImpulse *= dtRatio;
+
+		float axialImpulse = joint->motorImpulse + joint->lowerImpulse - joint->upperImpulse;
+\
+		b2Vec2 P = b2MulSV(axialImpulse, axis);
+		float LA = axialImpulse * a1;
+		float LB = axialImpulse * a2;
+
+		solverBodyA->linearVelocity = b2MulSub(solverBodyA->linearVelocity, mA, P);
+		solverBodyA->angularVelocity -= iA * LA;
+
+		solverBodyB->linearVelocity = b2MulAdd(solverBodyB->linearVelocity, mB, P);
+		solverBodyB->angularVelocity += iB * LB;
+	}
+	else
+	{
+		joint->impulse = b2Vec2_zero;
+		joint->motorImpulse = 0.0f;
+		joint->lowerImpulse = 0.0f;
+		joint->upperImpulse = 0.0f;
+	}
+}
+
+void b2SolvePrismaticVelocity(b2Joint* base, b2StepContext* context, bool useBias)
+{
+	B2_ASSERT(base->type == b2_prismaticJoint);
+
+	b2PrismaticJoint* joint = &base->prismaticJoint;
+
+	// This is a dummy body to represent a static body since static bodies don't have a solver body.
+	b2SolverBody dummyBody = {0};
+
+	b2SolverBody* bodyA = joint->indexA == B2_NULL_INDEX ? &dummyBody : context->solverBodies + joint->indexA;
+	b2Vec2 vA = bodyA->linearVelocity;
+	float wA = bodyA->angularVelocity;
+	float mA = bodyA->invMass;
+	float iA = bodyA->invI;
+
+	b2SolverBody* bodyB = joint->indexB == B2_NULL_INDEX ? &dummyBody : context->solverBodies + joint->indexB;
+	b2Vec2 vB = bodyB->linearVelocity;
+	float wB = bodyB->angularVelocity;
+	float mB = bodyB->invMass;
+	float iB = bodyB->invI;
+
+	const b2Vec2 cA = b2Add(joint->positionA, bodyA->deltaPosition);
+	const float aA = joint->angleA + bodyA->deltaAngle;
+	const b2Vec2 cB = b2Add(joint->positionB, bodyB->deltaPosition);
+	const float aB = joint->angleB + bodyB->deltaAngle;
+
+	b2Rot qA = b2MakeRot(aA);
+	b2Rot qB = b2MakeRot(aB);
+
+	b2Vec2 rA = b2RotateVector(qA, b2Sub(base->localAnchorA, joint->localCenterA));
+	b2Vec2 rB = b2RotateVector(qB, b2Sub(base->localAnchorB, joint->localCenterB));
+	b2Vec2 d = b2Add(b2Sub(cB, cA), b2Sub(rB, rA));
+
+	b2Vec2 axis = b2RotateVector(qA, joint->localAxisA);
+	float a1 = b2Cross(b2Add(d, rA), axis);
+	float a2 = b2Cross(rB, axis);
+
+	// Solve motor constraint
+	if (joint->enableMotor)
+	{
+		float Cdot = b2Dot(axis, b2Sub(vB, vA)) + a2 * wB - a1 * wA;
+		float impulse = joint->axialMass * (joint->motorSpeed - Cdot);
+		float oldImpulse = joint->motorImpulse;
+		float maxImpulse = context->dt * joint->maxMotorForce;
+		joint->motorImpulse = B2_CLAMP(joint->motorImpulse + impulse, -maxImpulse, maxImpulse);
+		impulse = joint->motorImpulse - oldImpulse;
+
+		b2Vec2 P = b2MulSV(impulse, axis);
+		float LA = impulse * a1;
+		float LB = impulse * a2;
+
+		vA = b2MulSub(vA, mA, P);
+		wA -= iA * LA;
+		vB = b2MulAdd(vB, mB, P);
+		wB += iB * LB;
+	}
+
+	if (joint->enableLimit)
+	{
+		float translation = b2Dot(axis, d);
+
+		// Lower limit
+		{
+			float C = translation - joint->lowerTranslation;
+			float bias = 0.0f;
+			float massScale = 1.0f;
+			float impulseScale = 0.0f;
+
+			if (C > 0.0f)
+			{
+				// speculation
+				bias = C * context->inv_dt;
+			}
+			else if (useBias)
+			{
+				bias = joint->biasCoefficient * C;
+				massScale = joint->massCoefficient;
+				impulseScale = joint->impulseCoefficient;
+			}
+
+			float oldImpulse = joint->lowerImpulse;
+			float Cdot = b2Dot(axis, b2Sub(vB, vA)) + a2 * wB - a1 * wA;
+			float impulse = -joint->axialMass * massScale * (Cdot + bias) - impulseScale * oldImpulse;
+			joint->lowerImpulse = B2_MAX(oldImpulse + impulse, 0.0f);
+			impulse = joint->lowerImpulse - oldImpulse;
+
+			b2Vec2 P = b2MulSV(impulse, axis);
+			float LA = impulse * a1;
+			float LB = impulse * a2;
+
+			vA = b2MulSub(vA, mA, P);
+			wA -= iA * LA;
+			vB = b2MulAdd(vB, mB, P);
+			wB += iB * LB;
+		}
+
+		// Upper limit
+		// Note: signs are flipped to keep C positive when the constraint is satisfied.
+		// This also keeps the impulse positive when the limit is active.
+		{
+			// sign flipped
+			float C = joint->upperTranslation - translation;
+			float bias = 0.0f;
+			float massScale = 1.0f;
+			float impulseScale = 0.0f;
+
+			if (C > 0.0f)
+			{
+				// speculation
+				bias = C * context->inv_dt;
+			}
+			else if (useBias)
+			{
+				bias = joint->biasCoefficient * C;
+				massScale = joint->massCoefficient;
+				impulseScale = joint->impulseCoefficient;
+			}
+
+			float oldImpulse = joint->upperImpulse;
+			// sign flipped
+			float Cdot = b2Dot(axis, b2Sub(vA, vB)) + a1 * wA - a2 * wB;
+			float impulse = -joint->axialMass * massScale * (Cdot + bias) - impulseScale * oldImpulse;
+			joint->upperImpulse = B2_MAX(oldImpulse + impulse, 0.0f);
+			impulse = joint->upperImpulse - oldImpulse;
+
+			b2Vec2 P = b2MulSV(impulse, axis);
+			float LA = impulse * a1;
+			float LB = impulse * a2;
+
+			// sign flipped
+			vA = b2MulAdd(vA, mA, P);
+			wA += iA * LA;
+			vB = b2MulSub(vB, mB, P);
+			wB -= iB * LB;
+		}
+	}
+
+	// Solve the prismatic constraint in block form
+	{
+		b2Vec2 perp = b2LeftPerp(axis);
+
+		float s1 = b2Cross(b2Add(d, rA), perp);
+		float s2 = b2Cross(rB, perp);
+
+		float k11 = mA + mB + iA * s1 * s1 + iB * s2 * s2;
+		float k12 = iA * s1 + iB * s2;
+		float k22 = iA + iB;
+		if (k22 == 0.0f)
+		{
+			// For bodies with fixed rotation.
+			k22 = 1.0f;
+		}
+
+		b2Mat22 K = {{k11, k12}, {k12, k22}};
+
+		b2Vec2 Cdot;
+		Cdot.x = b2Dot(perp, b2Sub(vB, vA)) + s2 * wB - s1 * wA;
+		Cdot.y = wB - wA;
+
+		b2Vec2 bias = b2Vec2_zero;
+		float massScale = 1.0f;
+		float impulseScale = 0.0f;
+		if (useBias)
+		{
+			b2Vec2 C;
+			C.x = b2Dot(perp, d);
+			C.y = aB - aA - joint->referenceAngle;
+
+			bias = b2MulSV(joint->biasCoefficient, C);
+			massScale = joint->massCoefficient;
+			impulseScale = joint->impulseCoefficient;
+		}
+
+		b2Vec2 b = b2Solve22(K, b2Add(Cdot, bias));
+		b2Vec2 impulse;
+		impulse.x = -massScale * b.x - impulseScale * joint->impulse.x;
+		impulse.y = -massScale * b.y - impulseScale * joint->impulse.y;
+
+		b2Vec2 P = b2MulSV(impulse.x, perp);
+		float LA = impulse.x * s1 + impulse.y;
+		float LB = impulse.x * s2 + impulse.y;
+
+		vA = b2MulSub(vA, mA, P);
+		wA -= iA * LA;
+		vB = b2MulAdd(vB, mB, P);
+		wB += iB * LB;
+	}
+
+	bodyA->linearVelocity = vA;
+	bodyA->angularVelocity = wA;
+	bodyB->linearVelocity = vB;
+	bodyB->angularVelocity = wB;
+}
+
+void b2PrismaticJoint_EnableLimit(b2JointId jointId, bool enableLimit)
+{
+	b2World* world = b2GetWorldFromIndex(jointId.world);
+	B2_ASSERT(world->locked == false);
+	if (world->locked)
+	{
+		return;
+	}
+
+	B2_ASSERT(0 <= jointId.index && jointId.index < world->jointPool.capacity);
+
+	b2Joint* joint = world->joints + jointId.index;
+	B2_ASSERT(joint->object.index == joint->object.next);
+	B2_ASSERT(joint->object.revision == jointId.revision);
+	B2_ASSERT(joint->type == b2_prismaticJoint);
+	joint->prismaticJoint.enableLimit = enableLimit;
+}
+
+void b2PrismaticJoint_EnableMotor(b2JointId jointId, bool enableMotor)
+{
+	b2World* world = b2GetWorldFromIndex(jointId.world);
+	B2_ASSERT(world->locked == false);
+	if (world->locked)
+	{
+		return;
+	}
+
+	B2_ASSERT(0 <= jointId.index && jointId.index < world->jointPool.capacity);
+
+	b2Joint* joint = world->joints + jointId.index;
+	B2_ASSERT(joint->object.index == joint->object.next);
+	B2_ASSERT(joint->object.revision == jointId.revision);
+	B2_ASSERT(joint->type == b2_prismaticJoint);
+	joint->prismaticJoint.enableMotor = enableMotor;
+}
+
+void b2PrismaticJoint_SetMotorSpeed(b2JointId jointId, float motorSpeed)
+{
+	b2World* world = b2GetWorldFromIndex(jointId.world);
+	B2_ASSERT(world->locked == false);
+	if (world->locked)
+	{
+		return;
+	}
+
+	B2_ASSERT(0 <= jointId.index && jointId.index < world->jointPool.capacity);
+
+	b2Joint* joint = world->joints + jointId.index;
+	B2_ASSERT(joint->object.index == joint->object.next);
+	B2_ASSERT(joint->object.revision == jointId.revision);
+	B2_ASSERT(joint->type == b2_prismaticJoint);
+	joint->prismaticJoint.motorSpeed = motorSpeed;
+}
+
+float b2PrismaticJoint_GetMotorForce(b2JointId jointId, float inverseTimeStep)
+{
+	b2World* world = b2GetWorldFromIndex(jointId.world);
+	B2_ASSERT(world->locked == false);
+	if (world->locked)
+	{
+		return 0.0f;
+	}
+
+	B2_ASSERT(0 <= jointId.index && jointId.index < world->jointPool.capacity);
+
+	b2Joint* joint = world->joints + jointId.index;
+	B2_ASSERT(joint->object.index == joint->object.next);
+	B2_ASSERT(joint->object.revision == jointId.revision);
+	B2_ASSERT(joint->type == b2_prismaticJoint);
+	return inverseTimeStep * joint->prismaticJoint.motorImpulse;
+}
+
+void b2PrismaticJoint_SetMaxMotorForce(b2JointId jointId, float force)
+{
+	b2World* world = b2GetWorldFromIndex(jointId.world);
+	B2_ASSERT(world->locked == false);
+	if (world->locked)
+	{
+		return;
+	}
+
+	B2_ASSERT(0 <= jointId.index && jointId.index < world->jointPool.capacity);
+
+	b2Joint* joint = world->joints + jointId.index;
+	B2_ASSERT(joint->object.index == joint->object.next);
+	B2_ASSERT(joint->object.revision == jointId.revision);
+	B2_ASSERT(joint->type == b2_prismaticJoint);
+	joint->prismaticJoint.maxMotorForce = force;
+}
+
+b2Vec2 b2PrismaticJoint_GetConstraintForce(b2JointId jointId)
+{
+	b2World* world = b2GetWorldFromIndex(jointId.world);
+	B2_ASSERT(world->locked == false);
+	if (world->locked)
+	{
+		return b2Vec2_zero;
+	}
+
+	B2_ASSERT(0 <= jointId.index && jointId.index < world->jointPool.capacity);
+
+	b2Joint* joint = world->joints + jointId.index;
+	B2_ASSERT(joint->object.index == joint->object.next);
+	B2_ASSERT(joint->object.revision == jointId.revision);
+	B2_ASSERT(joint->type == b2_prismaticJoint);
+	return joint->prismaticJoint.impulse;
+}
+
+#if 0
+void b2PrismaticJoint::Dump()
+{
+	int32 indexA = joint->bodyA->joint->islandIndex;
+	int32 indexB = joint->bodyB->joint->islandIndex;
+
+	b2Dump("  b2PrismaticJointDef jd;\n");
+	b2Dump("  jd.bodyA = bodies[%d];\n", indexA);
+	b2Dump("  jd.bodyB = bodies[%d];\n", indexB);
+	b2Dump("  jd.collideConnected = bool(%d);\n", joint->collideConnected);
+	b2Dump("  jd.localAnchorA.Set(%.9g, %.9g);\n", joint->localAnchorA.x, joint->localAnchorA.y);
+	b2Dump("  jd.localAnchorB.Set(%.9g, %.9g);\n", joint->localAnchorB.x, joint->localAnchorB.y);
+	b2Dump("  jd.referenceAngle = %.9g;\n", joint->referenceAngle);
+	b2Dump("  jd.enableLimit = bool(%d);\n", joint->enableLimit);
+	b2Dump("  jd.lowerAngle = %.9g;\n", joint->lowerAngle);
+	b2Dump("  jd.upperAngle = %.9g;\n", joint->upperAngle);
+	b2Dump("  jd.enableMotor = bool(%d);\n", joint->enableMotor);
+	b2Dump("  jd.motorSpeed = %.9g;\n", joint->motorSpeed);
+	b2Dump("  jd.maxMotorTorque = %.9g;\n", joint->maxMotorTorque);
+	b2Dump("  joints[%d] = joint->world->CreateJoint(&jd);\n", joint->index);
+}
+#endif
+
+void b2DrawPrismatic(b2DebugDraw* draw, b2Joint* base, b2Body* bodyA, b2Body* bodyB)
+{
+	B2_ASSERT(base->type == b2_prismaticJoint);
+
+	b2PrismaticJoint* joint = &base->prismaticJoint;
+
+	b2Transform xfA = bodyA->transform;
+	b2Transform xfB = bodyB->transform;
+	b2Vec2 pA = b2TransformPoint(xfA, base->localAnchorA);
+	b2Vec2 pB = b2TransformPoint(xfB, base->localAnchorB);
+
+	b2Vec2 axis = b2RotateVector(xfA.q, joint->localAxisA);
+
+	b2Color c1 = {0.7f, 0.7f, 0.7f, 1.0f};
+	b2Color c2 = {0.3f, 0.9f, 0.3f, 1.0f};
+	b2Color c3 = {0.9f, 0.3f, 0.3f, 1.0f};
+	b2Color c4 = {0.3f, 0.3f, 0.9f, 1.0f};
+	b2Color c5 = {0.4f, 0.4f, 0.4f, 1.0f};
+
+	draw->DrawSegment(pA, pB, c5, draw->context);
+
+	if (joint->enableLimit)
+	{
+		b2Vec2 lower = b2MulAdd(pA, joint->lowerTranslation, axis);
+		b2Vec2 upper = b2MulAdd(pA, joint->upperTranslation, axis);
+		b2Vec2 perp = b2LeftPerp(axis);
+		draw->DrawSegment(lower, upper, c1, draw->context);
+		draw->DrawSegment(b2MulSub(lower, 0.5f, perp), b2MulAdd(lower, 0.5f, perp), c2, draw->context);
+		draw->DrawSegment(b2MulSub(upper, 0.5f, perp), b2MulAdd(upper, 0.5f, perp), c3, draw->context);
+	}
+	else
+	{
+		draw->DrawSegment(b2MulSub(pA, 1.0f, axis), b2MulAdd(pA, 1.0f, axis), c1, draw->context);
+	}
+
+	draw->DrawPoint(pA, 5.0f, c1, draw->context);
+	draw->DrawPoint(pB, 5.0f, c4, draw->context);
+}
diff --git a/src/revolute_joint.c b/src/revolute_joint.c
index 002f0b1f..606b72ca 100644
--- a/src/revolute_joint.c
+++ b/src/revolute_joint.c
@@ -81,7 +81,6 @@ void b2PrepareRevolute(b2Joint* base, b2StepContext* context)
 	joint->impulseCoefficient = 1.0f / (1.0f + c);
 	joint->massCoefficient = c * joint->impulseCoefficient;
 
-	joint->angle = bodyB->angle - bodyA->angle - joint->referenceAngle;
 	if (joint->enableLimit == false || fixedRotation)
 	{
 		joint->lowerImpulse = 0.0f;
diff --git a/src/shape.c b/src/shape.c
index f6d5e13a..f4a5e3ae 100644
--- a/src/shape.c
+++ b/src/shape.c
@@ -47,6 +47,8 @@ b2MassData b2Shape_ComputeMass(const b2Shape* shape)
 
 void b2Shape_CreateProxy(b2Shape* shape, b2BroadPhase* bp, b2BodyType type, b2Transform xf)
 {
+	B2_ASSERT(shape->proxyKey == B2_NULL_INDEX);
+
 	// Create proxies in the broad-phase.
 	shape->aabb = b2Shape_ComputeAABB(shape, xf);
 
@@ -63,8 +65,11 @@ void b2Shape_CreateProxy(b2Shape* shape, b2BroadPhase* bp, b2BodyType type, b2Tr
 
 void b2Shape_DestroyProxy(b2Shape* shape, b2BroadPhase* bp)
 {
-	b2BroadPhase_DestroyProxy(bp, shape->proxyKey);
-	shape->proxyKey = B2_NULL_INDEX;
+	if (shape->proxyKey != B2_NULL_INDEX)
+	{
+		b2BroadPhase_DestroyProxy(bp, shape->proxyKey);
+		shape->proxyKey = B2_NULL_INDEX;
+	}
 }
 
 b2DistanceProxy b2Shape_MakeDistanceProxy(const b2Shape* shape)