Added Bex2d, Deep Profilerng, and better lua profileing

.gitmodules

@@ -1,4 +1,5 @@
 [submodule "Remake"]
 	path = Remake
 	url = https://dock-it.dev/GigabiteStudios/Remake.git
+	branch = master
 

.vscode/settings.json

@@ -76,6 +76,7 @@
         "variant": "cpp",
         "fstream": "cpp",
         "codecvt": "cpp",
-        "*.inc": "cpp"
+        "*.inc": "cpp",
+        "future": "cpp"
     }
 }

remake.yaml

@@ -1,11 +1,10 @@
-# Remake Build Configuration
 
 # Source folders to recursively find .c/.cpp files
 src_dirs:
   - src/src
-  - src/vendor
   - src/include/lua
-  - src/vendor/box2d/src  # <- actual Box2D sources if you're compiling them
+  - src/vendor/imgui
+  - src/vendor/box2d
 
 # Include directories (-I)
 include_dirs:
@@ -13,7 +12,8 @@ include_dirs:
   - src/include/lua
   - src/vendor
   - src/vendor/imgui
-  - src/vendor/box2d/include  # ✅ correct Box2D C++ API include path
+  - src/vendor/box2d
+
 
   - C:/msys64/mingw64/include
 
@@ -42,6 +42,7 @@ cxxflags:
   - -std=c++20
   - -Wall
   - -g
+# - -DDISABLE_DEEP_PROFILING
 
 # Auto-detect libraries and headers
 auto_libs:

remake/build.log

@@ -1,8 +1,2 @@
-[COMPILE] g++ -std=c++20 -Wall -g -Isrc/include -Isrc/include/lua -Isrc/vendor -Isrc/vendor/imgui -Isrc/vendor/box2d/include -IC:/msys64/mingw64/include -IC:\msys64\mingw64\lib\libyaml-cpp.a -Isrc\vendor\imgui -MMD -MP -c src\src\Components\PhysicsComponent.cpp -o src\build\Components\PhysicsComponent.o
-[ERROR] gcc -std=c99 -Wall -Isrc/include -Isrc/include/lua -Isrc/vendor -Isrc/vendor/imgui -Isrc/vendor/box2d/include -IC:/msys64/mingw64/include -IC:\msys64\mingw64\lib\libyaml-cpp.a -Isrc\vendor\imgui -MMD -MP -c src\vendor\box2d\benchmark\main.c -o src\build\box2d\benchmark\main.o
-cc1.exe: warning: C:\msys64\mingw64\lib\libyaml-cpp.a: not a directory
-src\vendor\box2d\benchmark\main.c:8:10: fatal error: TaskScheduler_c.h: No such file or directory
-    8 | #include "TaskScheduler_c.h"
-      |          ^~~~~~~~~~~~~~~~~~~
-compilation terminated.
-
+[LINK] g++ src\build\Engine.o src\build\main.o src\build\Renderer.o src\build\Components\CameraComponent.o src\build\Components\LightComponent.o src\build\Components\PhysicsComponent.o src\build\Components\ScriptComponent.o src\build\Components\SpriteComponent.o src\build\Components\TextComonent.o src\build\Components\TilemapComponent.o src\build\Entitys\Object.o src\build\utils\EngineConfig.o src\build\utils\ExceptionHandler.o src\build\utils\FileDialog.o src\build\utils\GameObjectsList.o src\build\utils\Logging.o src\build\utils\Profiler.o src\build\utils\Shader.o src\build\utils\UID.o src\build\utils\utils.o src\build\lapi.o src\build\lauxlib.o src\build\lbaselib.o src\build\lcode.o src\build\lcorolib.o src\build\lctype.o src\build\ldblib.o src\build\ldebug.o src\build\ldo.o src\build\ldump.o src\build\lfunc.o src\build\lgc.o src\build\linit.o src\build\liolib.o src\build\llex.o src\build\lmathlib.o src\build\lmem.o src\build\loadlib.o src\build\lobject.o src\build\lopcodes.o src\build\loslib.o src\build\lparser.o src\build\lstate.o src\build\lstring.o src\build\lstrlib.o src\build\ltable.o src\build\ltablib.o src\build\ltm.o src\build\lua.o src\build\luac.o src\build\lundump.o src\build\lutf8lib.o src\build\lvm.o src\build\lzio.o src\build\imgui.o src\build\imgui_demo.o src\build\imgui_draw.o src\build\imgui_impl_glfw.o src\build\imgui_impl_opengl3.o src\build\imgui_tables.o src\build\imgui_widgets.o src\build\aabb.o src\build\arena_allocator.o src\build\array.o src\build\bitset.o src\build\body.o src\build\broad_phase.o src\build\constraint_graph.o src\build\contact.o src\build\contact_solver.o src\build\core.o src\build\distance.o src\build\distance_joint.o src\build\dynamic_tree.o src\build\geometry.o src\build\hull.o src\build\id_pool.o src\build\island.o src\build\joint.o src\build\manifold.o src\build\math_functions.o src\build\motor_joint.o src\build\mouse_joint.o src\build\mover.o src\build\prismatic_joint.o src\build\revolute_joint.o src\build\sensor.o src\build\shape.o src\build\solver.o src\build\solver_set.o src\build\table.o src\build\timer.o src\build\types.o src\build\weld_joint.o src\build\wheel_joint.o src\build\world.o -o src\build\app.exe -LC:\msys64\mingw64\lib -lglfw3 -lglew32 -lopengl32 -lgdi32 -lyaml-cpp -lcomdlg32 -lssl -lcrypto
+[RUN] Executed app.exe successfully.

src/src/Components/Component.h

@@ -2,6 +2,7 @@
 
 #include <string>
 #include <memory>
+#include <vector>
 #include <yaml-cpp/yaml.h>
 #include "../utils/ExceptionHandler.h"
 

src/src/Components/PhysicsComponent.cpp

@@ -0,0 +1,102 @@
+#include "PhysicsComponent.h"
+#include "../Entitys/Object.h"
+#include "imgui.h"
+
+extern "C" {
+    #include "base.h"
+    #include "math_functions.h"
+    #include "types.h"
+    #include "body.h"
+    #include "shape.h"
+    #include "world.h"
+}
+
+PhysicsComponent::PhysicsComponent(Object* owner, BodyType type, float width, float height)
+    : Component(owner), type(type), width(width), height(height) {}
+
+PhysicsComponent::~PhysicsComponent() {
+    DestroyBody();
+}
+
+void PhysicsComponent::CreateBody(b2WorldId world, const glm::vec2& position) {
+    this->world = world;
+
+    b2BodyDef def = b2DefaultBodyDef();
+    def.type = (type == BodyType::Static)   ? b2_staticBody :
+               (type == BodyType::Dynamic)  ? b2_dynamicBody :
+                                              b2_kinematicBody;
+    def.position = { position.x, position.y };
+    def.fixedRotation = fixedRotation;
+
+    body = b2CreateBody(world, &def);
+
+    b2Polygon box = b2MakeBox(width * 0.5f, height * 0.5f);
+    b2ShapeDef shapeDef = b2DefaultShapeDef();
+    shapeDef.density = density;
+    
+
+    b2CreatePolygonShape(body, &shapeDef, &box);
+}
+
+void PhysicsComponent::DestroyBody() {
+    if (!B2_IS_NULL(body)) {
+        b2DestroyBody(body);
+        body = b2_nullBodyId;
+    }
+}
+
+void PhysicsComponent::SetVelocity(const glm::vec2& vel) {
+    if (!B2_IS_NULL(body))
+        b2Body_SetLinearVelocity(body, { vel.x, vel.y });
+}
+
+glm::vec2 PhysicsComponent::GetVelocity() const {
+    if (!B2_IS_NULL(body)) {
+        b2Vec2 v = b2Body_GetLinearVelocity(body);
+        return { v.x, v.y };
+    }
+    return {};
+}
+
+void PhysicsComponent::SetFriction(float f) {
+    friction = f;
+    // Not dynamically supported in Box2D C API. Requires fixture recreation.
+}
+
+void PhysicsComponent::SetDensity(float d) {
+    density = d;
+    // Same here, not dynamically applied.
+}
+
+void PhysicsComponent::SetFixedRotation(bool fixed) {
+    fixedRotation = fixed;
+    if (!B2_IS_NULL(body))
+        b2Body_SetFixedRotation(body, fixed);
+}
+
+void PhysicsComponent::SyncFromPhysics() {
+    if (!B2_IS_NULL(body)) {
+        b2Transform xf = b2Body_GetTransform(body);
+        owner->SetLocalPosition({ xf.p.x, xf.p.y });
+    }
+}
+
+void PhysicsComponent::SyncToPhysics() {
+    if (!B2_IS_NULL(body)) {
+        glm::vec2 pos = owner->GetLocalPosition();
+        b2Body_SetTransform(body, { pos.x, pos.y }, b2Body_GetRotation(body));
+    }
+}
+
+float PhysicsComponent::GetFriction() const { return friction; }
+float PhysicsComponent::GetDensity() const { return density; }
+bool PhysicsComponent::IsFixedRotation() const { return fixedRotation; }
+
+void PhysicsComponent::SetBodyType(BodyType t) { type = t; }
+PhysicsComponent::BodyType PhysicsComponent::GetBodyType() const { return type; }
+
+void PhysicsComponent::SetSize(float w, float h) { width = w; height = h; }
+float PhysicsComponent::GetWidth() const { return width; }
+float PhysicsComponent::GetHeight() const { return height; }
+
+bool PhysicsComponent::HasBody() const { return (!B2_IS_NULL(body)); }

src/src/Components/PhysicsComponent.h

@@ -0,0 +1,51 @@
+#pragma once
+#include "Component.h"
+#include <glm/glm.hpp>
+#include "box2d/box2d.h"
+
+class PhysicsComponent : public Component {
+public:
+    enum class BodyType { Static, Dynamic, Kinematic };
+
+    PhysicsComponent(Object* owner, BodyType type = BodyType::Dynamic, float width = 1.0f, float height = 1.0f);
+    ~PhysicsComponent();
+
+    void CreateBody(b2WorldId world, const glm::vec2& position);
+    void DestroyBody();
+
+    void SetVelocity(const glm::vec2& vel);
+    glm::vec2 GetVelocity() const;
+
+    void SetFriction(float friction);
+    float GetFriction() const;
+
+    void SetDensity(float density);
+    float GetDensity() const;
+
+    void SetFixedRotation(bool fixed);
+    bool IsFixedRotation() const;
+
+    void SetBodyType(BodyType type);
+    BodyType GetBodyType() const;
+
+    void SetSize(float width, float height);
+    float GetWidth() const;
+    float GetHeight() const;
+
+    bool HasBody() const;
+
+    void SyncFromPhysics();
+    void SyncToPhysics();
+
+    b2BodyId GetBodyId() const { return body; }
+    b2WorldId GetWorldId() const { return world; }
+
+private:
+    b2WorldId world = { 0 };
+    b2BodyId body = { 0 };
+    BodyType type;
+    float width, height;
+    float friction = 0.3f;
+    float density = 1.0f;
+    bool fixedRotation = false;
+};

src/src/Components/ScriptComponent.cpp

@@ -45,39 +45,61 @@ void ScriptComponent::SetScriptPath(const std::string &path)
 }
 const std::string &ScriptComponent::GetScriptPath() const { return scriptPath; }
 
-// Logging bindings
+void ScriptComponent::Hook(lua_State* L, lua_Debug* ar) {
+    if (!g_engineConfig.settings.profile_deep) return;
+
+    lua_getinfo(L, "nS", ar);
+
+    ScriptComponent* self = *reinterpret_cast<ScriptComponent**>(lua_getextraspace(L));
+    if (!self) return;
+
+    std::string name = ar->name ? ar->name : "unknown";
+    std::string label = name + "()";
+
+    if (ar->event == LUA_HOOKCALL) {
+        self->luaCallStack.push_back(label);
+        profiler.BeginDeepSection(label);
+    } else if (ar->event == LUA_HOOKRET) {
+        if (!self->luaCallStack.empty()) {
+            profiler.EndDeepSection();
+            self->luaCallStack.pop_back();
+        }
+    }
+}
+
+
+
 static int Lua_LogInfo(lua_State *L)
 {
+    PROFILE_DEEP_SCOPE("Lua_LogInfo");
     Logger::LogInfo("[Lua] %s", lua_tostring(L, 1));
     return 0;
 }
 static int Lua_LogError(lua_State *L)
 {
+    PROFILE_DEEP_SCOPE("Lua_LogError");
     Logger::LogError("[Lua] %s", lua_tostring(L, 1));
     return 0;
 }
 static int Lua_LogDebug(lua_State *L)
 {
+    PROFILE_DEEP_SCOPE("Lua_LogDebug");
     Logger::LogDebug("[Lua] %s", lua_tostring(L, 1));
     return 0;
 }
 static int Lua_DebugLua(lua_State *L)
 {
+    PROFILE_DEEP_SCOPE("Lua_DebugLua");
     luaDebugEnabled = lua_toboolean(L, 1);
     if (old_state != luaDebugEnabled)
-    {
         Logger::LogInfo("[Lua] DebugLua(%s)", luaDebugEnabled ? "true" : "false");
-    }
     old_state = luaDebugEnabled;
-
     return 0;
 }
 
-// Component resolver
 static Component *GetComponentByName(Object *obj, const std::string &type)
 {
-    PROFILE_SCOPE("LUA_GetComponentByName");
-
+    PROFILE_DEEP_SCOPE("GetComponentByName");
     if (type == "SpriteComponent")
         return obj->GetComponent<SpriteComponent>().get();
     if (type == "CameraComponent")
@@ -93,30 +115,21 @@ static Component *GetComponentByName(Object *obj, const std::string &type)
     return nullptr;
 }
 
-// Object:GetComponent("Type")
 static int Lua_Object_GetComponent(lua_State *L)
 {
-    PROFILE_SCOPE("Lua_Object_GetComponent");
-
+    PROFILE_DEEP_SCOPE("Object::GetComponent");
     auto *wrapper = (LuaObjectWrapper *)luaL_checkudata(L, 1, LUA_OBJECT_MT);
     const char *type = luaL_checkstring(L, 2);
-
     Component *comp = GetComponentByName(wrapper->obj, type);
-    if (comp)
-        lua_pushlightuserdata(L, comp);
-    else
-        lua_pushnil(L);
+    lua_pushlightuserdata(L, comp ? comp : nullptr);
     return 1;
 }
 
-// Object:GetPosition()
 static int Lua_Object_GetPosition(lua_State *L)
 {
-    PROFILE_SCOPE("Lua_Object_GetPosition");
-
+    PROFILE_DEEP_SCOPE("Object::GetPosition");
     auto *wrapper = (LuaObjectWrapper *)luaL_checkudata(L, 1, LUA_OBJECT_MT);
     glm::vec2 pos = wrapper->obj->GetLocalPosition();
-
     LuaVector2 *vec = (LuaVector2 *)lua_newuserdata(L, sizeof(LuaVector2));
     vec->x = pos.x;
     vec->y = pos.y;
@@ -125,22 +138,18 @@ static int Lua_Object_GetPosition(lua_State *L)
     return 1;
 }
 
-// Object:SetPosition(Vector2)
 static int Lua_Object_SetPosition(lua_State *L)
 {
-    PROFILE_SCOPE("Lua_Object_SetPosition");
-
+    PROFILE_DEEP_SCOPE("Object::SetPosition");
     auto *wrapper = (LuaObjectWrapper *)luaL_checkudata(L, 1, LUA_OBJECT_MT);
     auto *vec = (LuaVector2 *)luaL_checkudata(L, 2, LUA_VECTOR2_MT);
     wrapper->obj->SetLocalPosition({vec->x, vec->y});
     return 0;
 }
 
-// __index for Object
 static int Lua_Object_Index(lua_State *L)
 {
-    PROFILE_SCOPE("Lua_Object_Index");
-
+    PROFILE_DEEP_SCOPE("Object::__index");
     const char *key = luaL_checkstring(L, 2);
     lua_getfield(L, lua_upvalueindex(1), key);
     return 1;
@@ -148,35 +157,33 @@ static int Lua_Object_Index(lua_State *L)
 
 void RegisterObjectType(lua_State *L)
 {
-    luaL_newmetatable(L, LUA_OBJECT_MT);
+    PROFILE_DEEP_SCOPE("RegisterObjectType");
 
-    lua_newtable(L); // method table
+    luaL_newmetatable(L, LUA_OBJECT_MT);
+    lua_newtable(L);
     lua_pushcfunction(L, Lua_Object_GetComponent);
     lua_setfield(L, -2, "GetComponent");
     lua_pushcfunction(L, Lua_Object_GetPosition);
     lua_setfield(L, -2, "GetPosition");
     lua_pushcfunction(L, Lua_Object_SetPosition);
     lua_setfield(L, -2, "SetPosition");
-
     lua_pushcclosure(L, Lua_Object_Index, 1);
     lua_setfield(L, -2, "__index");
-
     lua_pop(L, 1);
 }
 
 static void PushObject(lua_State *L, Object *obj)
 {
+    PROFILE_DEEP_SCOPE("PushObject");
     auto *wrapper = (LuaObjectWrapper *)lua_newuserdata(L, sizeof(LuaObjectWrapper));
     wrapper->obj = obj;
     luaL_getmetatable(L, LUA_OBJECT_MT);
     lua_setmetatable(L, -2);
 }
 
-// Engine.GetObjectByTag(name)
 static int Lua_GetObjectByTag(lua_State *L)
 {
-    PROFILE_SCOPE("Lua_GetObjectByTag");
-
+    PROFILE_DEEP_SCOPE("GetObjectByTag");
     const char *name = luaL_checkstring(L, 1);
     for (const auto &obj : objects)
     {
@@ -191,25 +198,20 @@ static int Lua_GetObjectByTag(lua_State *L)
     return 1;
 }
 
-// Vector2(x, y)
 static int Lua_Vector2_New(lua_State *L)
 {
-    PROFILE_SCOPE("Lua_Vector2_New");
-
+    PROFILE_DEEP_SCOPE("Vector2()");
     LuaVector2 *vec = static_cast<LuaVector2 *>(lua_newuserdata(L, sizeof(LuaVector2)));
-
     int nargs = lua_gettop(L);
     vec->x = nargs >= 1 ? (float)lua_tonumber(L, 1) : 0.0f;
     vec->y = nargs >= 2 ? (float)lua_tonumber(L, 2) : 0.0f;
-
     luaL_setmetatable(L, LUA_VECTOR2_MT);
     return 1;
 }
 
 static int Lua_Vector2_Index(lua_State *L)
 {
-    PROFILE_SCOPE("Lua_Vector2_Index");
-
+    PROFILE_DEEP_SCOPE("Vector2::index");
     auto *vec = (LuaVector2 *)luaL_checkudata(L, 1, LUA_VECTOR2_MT);
     const char *key = luaL_checkstring(L, 2);
     if (strcmp(key, "x") == 0)
@@ -220,10 +222,10 @@ static int Lua_Vector2_Index(lua_State *L)
         lua_pushnil(L);
     return 1;
 }
+
 static int Lua_Vector2_NewIndex(lua_State *L)
 {
-    PROFILE_SCOPE("Lua_Vector2_NewIndex");
-
+    PROFILE_DEEP_SCOPE("Vector2::newindex");
     auto *vec = (LuaVector2 *)luaL_checkudata(L, 1, LUA_VECTOR2_MT);
     const char *key = luaL_checkstring(L, 2);
     float value = (float)luaL_checknumber(L, 3);
@@ -236,7 +238,7 @@ static int Lua_Vector2_NewIndex(lua_State *L)
 
 void RegisterVector2Type(lua_State *L)
 {
-
+    PROFILE_DEEP_SCOPE("RegisterVector2Type");
     luaL_newmetatable(L, LUA_VECTOR2_MT);
     lua_pushcfunction(L, Lua_Vector2_Index);
     lua_setfield(L, -2, "__index");
@@ -249,8 +251,9 @@ void RegisterVector2Type(lua_State *L)
 
 void ScriptComponent::RegisterEngineBindings()
 {
-    lua_newtable(L);
+    PROFILE_DEEP_SCOPE("RegisterEngineBindings");
 
+    lua_newtable(L);
     lua_pushcfunction(L, Lua_LogInfo);
     lua_setfield(L, -2, "LogInfo");
     lua_pushcfunction(L, Lua_LogError);
@@ -261,26 +264,28 @@ void ScriptComponent::RegisterEngineBindings()
     lua_setfield(L, -2, "GetObjectByTag");
     lua_pushcfunction(L, Lua_DebugLua);
     lua_setfield(L, -2, "DebugLua");
-
     lua_setglobal(L, "Engine");
 }
 
 void ScriptComponent::ReloadScript()
 {
+    PROFILE_DEEP_SCOPE("ScriptComponent::ReloadScript");
     if (scriptPath.empty())
         return;
-
     if (L)
         lua_close(L);
+
     L = luaL_newstate();
+    *(ScriptComponent**)lua_getextraspace(L) = this;
     luaL_openlibs(L);
+    lua_sethook(L, Hook, LUA_MASKCALL | LUA_MASKRET, 0);
+
 
     RegisterObjectType(L);
     RegisterVector2Type(L);
     RegisterEngineBindings();
 
     Logger::LogVerbose("[Lua] Loading Script from file.");
-
     if (luaL_dofile(L, scriptPath.c_str()))
     {
         Logger::LogError("[Lua] %s", lua_tostring(L, -1));
@@ -289,9 +294,7 @@ void ScriptComponent::ReloadScript()
     }
 
     if (luaDebugEnabled)
-    {
         Logger::LogVerbose("[Lua][call] OnInit()");
-    }
     lua_getglobal(L, "OnInit");
     if (lua_isfunction(L, -1))
     {
@@ -302,14 +305,12 @@ void ScriptComponent::ReloadScript()
         }
     }
     else
-    {
         lua_pop(L, 1);
 }
-}
 
 void ScriptComponent::OnUpdate(float dt)
 {
-    PROFILE_SCOPE("ScriptComponent::OnUpdate");
+    PROFILE_DEEP_SCOPE("ScriptComponent::OnUpdate");
 
     if (!L)
         return;
@@ -324,10 +325,8 @@ void ScriptComponent::OnUpdate(float dt)
         }
     }
     else
-    {
         lua_pop(L, 1);
 }
-}
 
 void ScriptComponent::Save(YAML::Emitter &out) const
 {

src/src/Components/ScriptComponent.h

@@ -1,6 +1,8 @@
 #include <string>
 #include "Component.h"
 #include <yaml-cpp/yaml.h>
+#include <chrono>
+
 
 extern "C" {
 #include <lua.h>
@@ -8,6 +10,12 @@ extern "C" {
 #include <lauxlib.h>
 }
 
+struct LuaCallInfo {
+    std::string name;
+    std::chrono::high_resolution_clock::time_point start;
+};
+
+
 class ScriptComponent : public Component {
 public:
     ScriptComponent(Object* owner);
@@ -27,5 +35,10 @@ private:
     std::string scriptPath;
     lua_State* L;
 
+    std::vector<std::string> luaCallStack;
+    static void Hook(lua_State* L, lua_Debug* ar);
+
+
+
     void RegisterEngineBindings();
 };

src/src/Engine.cpp

@@ -6,6 +6,7 @@
 #include "components/LightComponent.h"
 #include "components/TilemapComponent.h"
 #include "components/ScriptComponent.h"
+#include "components/PhysicsComponent.h"
 
 #include "utils/FileDialog.h"
 #include "utils/Logging.h"
@@ -15,7 +16,6 @@
 #include "utils/Profiler.h"
 #include "utils/utils.h"
 
-
 #include <GL/glew.h>
 #include <GLFW/glfw3.h>
 #include <imgui.h>
@@ -317,9 +317,12 @@ void ShowColorCorrectionWindow()
 void Engine::Init()
 {
 
-    if (DeleteLatestLogFile()) {
+    if (DeleteLatestLogFile())
+    {
         Logger::LogVerbose("Log file deleted");
-    } else {
+    }
+    else
+    {
         Logger::LogVerbose("Failed to delete log file");
     }
 
@@ -355,6 +358,8 @@ void Engine::Init()
     m_toDraw.reserve(1024);
     m_scriptUpdates.reserve(256);
     m_collectStack.reserve(1024);
+    m_physicsUpdates.reserve(1024);
+
     Logger::LogInfo("Initialized Engine");
 }
 void DrawInspectorUI(std::shared_ptr<Object> selected)
@@ -564,6 +569,25 @@ void DrawInspectorUI(std::shared_ptr<Object> selected)
         if (ImGui::Button("Remove ScriptComponent"))
             selected->RemoveComponent<ScriptComponent>();
     }
+    if (auto phys = selected->GetComponent<PhysicsComponent>())
+    {
+        ImGui::SeparatorText("Physics Component");
+
+        glm::vec2 vel = phys->GetVelocity();
+        if (ImGui::DragFloat2("Velocity", &vel.x))
+            phys->SetVelocity(vel);
+
+        float friction = phys->GetFriction();
+        if (ImGui::DragFloat("Friction", &friction, 0.01f, 0.0f, 1.0f))
+            phys->SetFriction(friction);
+
+        bool fixed = phys->IsFixedRotation();
+        if (ImGui::Checkbox("Fixed Rotation", &fixed))
+            phys->SetFixedRotation(fixed);
+
+        if (ImGui::Button("Remove PhysicsComponent"))
+            selected->RemoveComponent<PhysicsComponent>();
+    }
 
     if (auto tilemap = selected->GetComponent<TilemapComponent>())
     {
@@ -589,6 +613,7 @@ void Engine::collectObjects(bool playing, const glm::vec2 &camPos, float camZoom
     m_scriptUpdates.clear();
     m_collectStack.clear();
     m_activeCamera = nullptr;
+    m_physicsUpdates.clear();
 
     for (auto &root : objects)
         if (!root->GetParent())
@@ -604,7 +629,6 @@ void Engine::collectObjects(bool playing, const glm::vec2 &camPos, float camZoom
 
         m_toDraw.push_back(obj.get());
 
-        // Handle light components
         if (auto light = obj->GetComponent<LightComponent>())
         {
             glm::vec2 world = obj->GetWorldPosition();
@@ -612,7 +636,6 @@ void Engine::collectObjects(bool playing, const glm::vec2 &camPos, float camZoom
             Renderer::AddLight(screen, light->GetColor(), light->GetIntensity(), light->GetRadius() * camZoom);
         }
 
-        // Handle primary camera
         if (!m_activeCamera)
         {
             if (auto camera = obj->GetComponent<CameraComponent>())
@@ -622,18 +645,22 @@ void Engine::collectObjects(bool playing, const glm::vec2 &camPos, float camZoom
             }
         }
         
+
         if (playing)
         {
             if (auto script = obj->GetComponent<ScriptComponent>())
                 m_scriptUpdates.push_back(script.get());
+
+            if (auto physics = obj->GetComponent<PhysicsComponent>())
+                m_physicsUpdates.push_back(physics.get());
         }
 
-        // Add children to stack
         for (auto &child : obj->GetChildren())
             m_collectStack.push_back(child);
     }
 }
 
+
 void Engine::Run()
 {
     while (!glfwWindowShouldClose(window))
@@ -746,6 +773,8 @@ void Engine::Run()
                     if (g_engineConfig.settings.profile_enabled)
                     {
                         ImGui::Checkbox("Profile Engine", &g_engineConfig.settings.profile_editor);
+                        ImGui::Checkbox("Deep Profileing", &g_engineConfig.settings.profile_deep);
+
                     }
                     ImGui::EndMenu();
                 }
@@ -911,10 +940,6 @@ void Engine::Run()
         m_scriptUpdates.clear();
         profiler.EndEngineSection();
 
-
-        
-
-
         profiler.BeginEngineSection("Draw Editor Grid");
         Renderer::DrawEditorGrid(cameraPos, cameraZoom);
         profiler.EndEngineSection();
@@ -1123,7 +1148,6 @@ void Engine::DrawObjectNode(const std::shared_ptr<Object> &obj)
             }
         }
         ImGui::EndDragDropTarget();
-
     }
 
     // === Children ===

src/src/Engine.h

@@ -8,6 +8,7 @@
 
 class Object;
 class ScriptComponent;
+class PhysicsComponent;
 
 class Engine
 {
@@ -37,4 +38,6 @@ private:
     int m_OnUpdateCalls;
 
     std::vector<std::shared_ptr<Object>>   m_collectStack;
+    std::vector<PhysicsComponent*> m_physicsUpdates;
+
 };

src/src/Renderer.cpp

@@ -29,7 +29,6 @@ int Renderer::s_DrawCalls = 0;
 int Renderer::s_LightsCount = 0;
 std::unique_ptr<ColorCorrection> Renderer::s_ColorCorrection = nullptr;
 
-
 std::vector<Light> Renderer::s_Lights;
 
 static Shader tilemapShader;
@@ -78,7 +77,6 @@ void Renderer::Init()
     if (glCheckFramebufferStatus(GL_FRAMEBUFFER) != GL_FRAMEBUFFER_COMPLETE)
         Logger::LogError("[Renderer::Init] FBO incomplete");
 
-
     glBindFramebuffer(GL_FRAMEBUFFER, 0);
 
     InitQuad();
@@ -91,7 +89,6 @@ void Renderer::Init()
 
     SetColorCorrection(std::make_unique<ColorCorrection>());
 
-
     // Create a 1x1 flat normal map (RGB: 128,128,255)
     unsigned char flatNormal[3] = {128, 128, 255};
     glGenTextures(1, &defaultNormalMap);
@@ -124,7 +121,6 @@ void Renderer::Begin()
     {
         PROFILE_ENGINE_SCOPE("glBindFramebuffer");
         glBindFramebuffer(GL_FRAMEBUFFER, fbo);
-
     }
 
     glViewport(0, 0, width, height);
@@ -138,8 +134,6 @@ void Renderer::Begin()
     ClearLights();
 }
 
-
-
 void Renderer::End()
 {
     glBindFramebuffer(GL_FRAMEBUFFER, 0);
@@ -219,8 +213,14 @@ void Renderer::DrawTilemap(TilemapComponent *tilemap, const glm::vec2 &worldPos,
     glBindVertexArray(0);
 }
 
+
+
+
+
 void Renderer::DrawSprite(SpriteComponent *sprite, const glm::vec2 &pos, float zoom, glm::vec2 &CameraPos)
 {
+    PROFILE_DEEP_SCOPE("DrawSprite");
+
     if (!sprite->HasTexture())
     {
         static bool warned = false;
@@ -234,50 +234,94 @@ void Renderer::DrawSprite(SpriteComponent *sprite, const glm::vec2 &pos, float z
 
     Shader *shader = &unlitShader;
     bool useLighting = false;
+
+    {
+        PROFILE_DEEP_SCOPE("ShaderSelect");
         if (g_engineConfig.settings.lighting_enabled && sprite->GetRenderType() == SpriteComponent::RenderType::Lit)
         {
             shader = &spriteShader;
             useLighting = true;
         }
+    }
 
+    {
+        PROFILE_DEEP_SCOPE("ShaderUse");
         shader->Use();
+    }
 
     glm::vec2 size = sprite->GetSize();
     glm::vec2 screenPos = (pos - CameraPos) * zoom + glm::vec2(width, height) * 0.5f - (size * zoom * 0.5f);
     float rotationDeg = sprite->GetOwner()->GetWorldRotation();
 
+    {
+        PROFILE_DEEP_SCOPE("SetUniforms");
+
+        {
+            PROFILE_DEEP_SCOPE("Transform");
             shader->SetVec2("uPos", screenPos);
             shader->SetVec2("uSize", size * zoom);
             shader->SetVec2("uScreen", glm::vec2(width, height));
             shader->SetFloat("uRotation", glm::radians(rotationDeg));
+        }
+
+        {
+            PROFILE_DEEP_SCOPE("TexSlot");
             shader->SetInt("uTex", 0);
+        }
+    }
 
-
+    {
+        PROFILE_DEEP_SCOPE("BindTex");
         glActiveTexture(GL_TEXTURE0);
         glBindTexture(GL_TEXTURE_2D, sprite->GetTextureID());
+    }
 
     if (useLighting)
     {
+        PROFILE_DEEP_SCOPE("Lighting");
+
+        {
+            PROFILE_DEEP_SCOPE("ColorCorrection");
             s_ColorCorrection->Upload(*shader);
+        }
 
         shader->SetInt("uLightCount", static_cast<int>(s_Lights.size()));
+
         for (size_t i = 0; i < s_Lights.size(); ++i)
         {
+            PROFILE_DEEP_SCOPE(("L" + std::to_string(i)).c_str());
+
             shader->SetVec2(("uLightPos[" + std::to_string(i) + "]").c_str(), s_Lights[i].screenPos);
             shader->SetVec3(("uLightColor[" + std::to_string(i) + "]").c_str(), s_Lights[i].color);
             shader->SetFloat(("uLightIntensity[" + std::to_string(i) + "]").c_str(), s_Lights[i].intensity);
             shader->SetFloat(("uLightRadius[" + std::to_string(i) + "]").c_str(), s_Lights[i].radius);
         }
 
+        {
+            PROFILE_DEEP_SCOPE("BindNorm");
             shader->SetInt("uNormalMap", 1);
             glActiveTexture(GL_TEXTURE1);
             glBindTexture(GL_TEXTURE_2D, sprite->GetNormalMapID() ? sprite->GetNormalMapID() : defaultNormalMap);
         }
+    }
 
+    {
+        PROFILE_DEEP_SCOPE("Draw");
+
+        {
+            PROFILE_DEEP_SCOPE("VAO Bind");
             glBindVertexArray(quadVAO);
+        }
+
+        {
+            PROFILE_DEEP_SCOPE("GL Draw");
             glDrawArrays(GL_TRIANGLE_FAN, 0, 4);
+        }
+
         glBindVertexArray(0);
-    s_DrawCalls++;
+    }
+
+    ++s_DrawCalls;
 }
 
 int Renderer::GetDrawCallCount()

src/src/utils/EngineConfig.cpp

@@ -16,13 +16,15 @@ EngineConfig g_engineConfig{
         .profile_editor = false,
         .profile_enabled = true,
         .show_color_correction_window = false,
-        .lighting_enabled = true
-    }
-};
+        .lighting_enabled = true,
+        .profile_deep = false,
+    }};
 
-static std::filesystem::path GetUserSettingsPath() {
+static std::filesystem::path GetUserSettingsPath()
+{
     char userPath[MAX_PATH];
-    if (SUCCEEDED(SHGetFolderPathA(NULL, CSIDL_PROFILE, NULL, 0, userPath))) {
+    if (SUCCEEDED(SHGetFolderPathA(NULL, CSIDL_PROFILE, NULL, 0, userPath)))
+    {
         std::filesystem::path path = std::filesystem::path(userPath) / ".CreateEngine" / ".user_settings.yaml";
         std::filesystem::create_directories(path.parent_path());
         Logger::LogVerbose("Settings Path: %s", path.string().c_str());
@@ -32,7 +34,8 @@ static std::filesystem::path GetUserSettingsPath() {
     return {};
 }
 
-void EngineConfig::SaveToFile() {
+void EngineConfig::SaveToFile()
+{
     Logger::LogVerbose("Saving User Settings");
 
     YAML::Emitter out;
@@ -43,6 +46,8 @@ void EngineConfig::SaveToFile() {
     out << YAML::Key << "profile_enabled" << YAML::Value << settings.profile_enabled;
     out << YAML::Key << "show_color_correction_window" << YAML::Value << settings.show_color_correction_window;
     out << YAML::Key << "lighting_enabled" << YAML::Value << settings.lighting_enabled;
+    out << YAML::Key << "profile_deep" << YAML::Value << settings.profile_deep;
+
 
     out << YAML::EndMap;
 
@@ -50,11 +55,13 @@ void EngineConfig::SaveToFile() {
     fout << out.c_str();
 }
 
-void EngineConfig::LoadFromFile() {
+void EngineConfig::LoadFromFile()
+{
     Logger::LogVerbose("Loading User Settings");
 
     auto path = GetUserSettingsPath();
-    if (!std::filesystem::exists(path)) return;
+    if (!std::filesystem::exists(path))
+        return;
 
     YAML::Node root = YAML::LoadFile(path.string());
 
@@ -68,4 +75,6 @@ void EngineConfig::LoadFromFile() {
         settings.show_color_correction_window = root["show_color_correction_window"].as<bool>();
     if (root["lighting_enabled"])
         settings.lighting_enabled = root["lighting_enabled"].as<bool>();
+    if (root["profile_deep"])
+        settings.profile_deep = root["profile_deep"].as<bool>();
 }

src/src/utils/EngineConfig.h

@@ -7,6 +7,7 @@ struct UserSettings {
     bool profile_enabled;
     bool show_color_correction_window;
     bool lighting_enabled;
+    bool profile_deep;
 };
 
 struct EngineConfig {

src/src/utils/PhysicsSystem.h

@@ -0,0 +1,18 @@
+// PhysicsSystem.h
+#pragma once
+
+extern "C" {
+    #include "box2d.h"
+}
+
+class PhysicsSystem {
+public:
+    void Initialize();
+    void Shutdown();
+    void Step(float deltaTime);
+
+    b2WorldId GetWorld() const { return world; }
+
+private:
+    b2WorldId world = b2_nullWorldId;
+};

src/src/utils/Profiler.cpp

@@ -79,6 +79,17 @@ void HierarchicalProfiler::EndEngineSection() {
         EndSection();
 }
 
+void HierarchicalProfiler::BeginDeepSection(const std::string& name) {
+    if (g_engineConfig.settings.profile_deep)
+        BeginSection(name);
+}
+
+void HierarchicalProfiler::EndDeepSection() {
+    if (g_engineConfig.settings.profile_deep)
+        EndSection();
+}
+
+
 void HierarchicalProfiler::EndFrame() {
     if (!g_engineConfig.settings.profile_enabled)
         return;

src/src/utils/Profiler.h

@@ -4,8 +4,7 @@
 #include <chrono>
 #include "EngineConfig.h"
 
-struct ProfileNode
-{
+struct ProfileNode {
     std::string name;
     double startMs = 0.0;
     double durationMs = 0.0;
@@ -13,20 +12,13 @@ struct ProfileNode
     double visualDurationMs = 0.0;
     std::vector<ProfileNode> children;
 
-
-    
     ProfileNode() = default;
-    
-
-    ProfileNode(const std::string &n, double start)
-        : name(n), startMs(start)
-    {
+    ProfileNode(const std::string& n, double start) : name(n), startMs(start) {
         children.reserve(8);
     }
 };
 
-class HierarchicalProfiler
-{
+class HierarchicalProfiler {
 public:
     void BeginFrame();
     void EndFrame();
@@ -37,6 +29,9 @@ public:
     void BeginEngineSection(const std::string& name);
     void EndEngineSection();
 
+    void BeginDeepSection(const std::string& name);
+    void EndDeepSection();
+
     const std::vector<ProfileNode>& GetFrames() const;
     const ProfileNode* GetLatestFrame() const;
 
@@ -55,40 +50,49 @@ private:
 
 extern HierarchicalProfiler profiler;
 
-// RAII Scoped Profiling (Zero-overhead when disabled)
-struct ScopedProfile
-{
-    ScopedProfile(const std::string &name)
-    {
+struct ScopedProfile {
+    ScopedProfile(const std::string& name) {
         if (g_engineConfig.settings.profile_enabled)
             profiler.BeginSection(name);
     }
-    ~ScopedProfile()
-    {
+    ~ScopedProfile() {
         if (g_engineConfig.settings.profile_enabled)
             profiler.EndSection();
     }
 };
 
-struct ScopedEngineProfile
-{
-    ScopedEngineProfile(const std::string &name)
-    {
+struct ScopedEngineProfile {
+    ScopedEngineProfile(const std::string& name) {
         if (g_engineConfig.settings.profile_enabled && g_engineConfig.settings.profile_editor)
             profiler.BeginSection(name);
     }
-    ~ScopedEngineProfile()
-    {
+    ~ScopedEngineProfile() {
         if (g_engineConfig.settings.profile_enabled && g_engineConfig.settings.profile_editor)
             profiler.EndSection();
     }
 };
 
+#ifndef DISABLE_DEEP_PROFILING
+struct ScopedDeepProfile {
+    ScopedDeepProfile(const std::string& name) {
+        if (g_engineConfig.settings.profile_deep)
+            profiler.BeginDeepSection(name);
+    }
+    ~ScopedDeepProfile() {
+        if (g_engineConfig.settings.profile_deep)
+            profiler.EndDeepSection();
+    }
+};
+#else
+struct ScopedDeepProfile {
+    ScopedDeepProfile(const std::string&) {}
+    ~ScopedDeepProfile() {}
+};
+#endif
+
 #define CONCAT_IMPL(a, b) a##b
 #define CONCAT(a, b) CONCAT_IMPL(a, b)
 
-#define PROFILE_SCOPE(label) \
-    ScopedProfile CONCAT(_scopedProfile_, __LINE__)(label)
-
-#define PROFILE_ENGINE_SCOPE(label) \
-    ScopedEngineProfile CONCAT(_scopedEngineProfile_, __LINE__)(label)
+#define PROFILE_SCOPE(name) ScopedProfile CONCAT(_prof_, __LINE__)(name)
+#define PROFILE_ENGINE_SCOPE(name) ScopedEngineProfile CONCAT(_eprof_, __LINE__)(name)
+#define PROFILE_DEEP_SCOPE(name) ScopedDeepProfile CONCAT(_dprof_, __LINE__)(name)

src/src/utils/Shader.cpp

@@ -29,8 +29,6 @@ std::string Shader::ReadFile(const std::string& path) {
     return buffer.str();
 }
 
-
-
 GLuint Shader::Compile(GLenum type, const std::string& source) {
     Logger::LogVerbose("[Shader] Compiling %s shader...",
                        type == GL_VERTEX_SHADER ? "Vertex" : "Fragment");
@@ -55,7 +53,6 @@ GLuint Shader::Compile(GLenum type, const std::string& source) {
     return shader;
 }
 
-
 bool Shader::LoadFromFile(const std::string& vertexPath, const std::string& fragmentPath) {
     Logger::LogVerbose("[Shader] Loading from file: %s + %s", vertexPath.c_str(), fragmentPath.c_str());
     return LoadFromSource(ReadFile(vertexPath), ReadFile(fragmentPath));
@@ -97,40 +94,14 @@ GLuint Shader::GetID() const {
 }
 
 GLint Shader::GetUniformLocation(const std::string& name) {
-    if (uniformCache.contains(name)) return uniformCache[name];
+    auto it = uniformCache.find(name);
+    if (it != uniformCache.end())
+        return it->second;
+
     GLint loc = glGetUniformLocation(id, name.c_str());
     if (loc == -1)
         Logger::LogWarning("[Shader] Uniform not found: %s", name.c_str());
+
     uniformCache[name] = loc;
     return loc;
 }
-
-// === Uniform Setters ===
-
-void Shader::SetBool(const std::string& name, bool value) {
-    glUniform1i(GetUniformLocation(name), (int)value);
-}
-
-void Shader::SetInt(const std::string& name, int value) {
-    glUniform1i(GetUniformLocation(name), value);
-}
-
-void Shader::SetFloat(const std::string& name, float value) {
-    glUniform1f(GetUniformLocation(name), value);
-}
-
-void Shader::SetVec2(const std::string& name, const glm::vec2& value) {
-    glUniform2fv(GetUniformLocation(name), 1, &value[0]);
-}
-
-void Shader::SetVec3(const std::string& name, const glm::vec3& value) {
-    glUniform3fv(GetUniformLocation(name), 1, &value[0]);
-}
-
-void Shader::SetVec4(const std::string& name, const glm::vec4& value) {
-    glUniform4fv(GetUniformLocation(name), 1, &value[0]);
-}
-
-void Shader::SetMat4(const std::string& name, const glm::mat4& mat) {
-    glUniformMatrix4fv(GetUniformLocation(name), 1, GL_FALSE, &mat[0][0]);
-}

src/src/utils/Shader.h

@@ -1,9 +1,8 @@
 #pragma once
-
 #include <string>
 #include <unordered_map>
 #include <glm/glm.hpp>
-#include <gl/glew.h>
+#include <GL/glew.h>
 
 class Shader {
 public:
@@ -12,25 +11,38 @@ public:
 
     bool LoadFromFile(const std::string& vertexPath, const std::string& fragmentPath);
     bool LoadFromSource(const std::string& vertexSrc, const std::string& fragmentSrc);
-
     void Use() const;
     GLuint GetID() const;
+    void Clear();
 
-    // Setters
-    void SetBool(const std::string& name, bool value);
-    void SetInt(const std::string& name, int value);
-    void SetFloat(const std::string& name, float value);
-    void SetVec2(const std::string& name, const glm::vec2& value);
-    void SetVec3(const std::string& name, const glm::vec3& value);
-    void SetVec4(const std::string& name, const glm::vec4& value);
-    void SetMat4(const std::string& name, const glm::mat4& mat);
+    // === Uniform Setters ===
+    inline void SetBool(const std::string& name, bool value) {
+        glUniform1i(GetUniformLocation(name), static_cast<int>(value));
+    }
+    inline void SetInt(const std::string& name, int value) {
+        glUniform1i(GetUniformLocation(name), value);
+    }
+    inline void SetFloat(const std::string& name, float value) {
+        glUniform1f(GetUniformLocation(name), value);
+    }
+    inline void SetVec2(const std::string& name, const glm::vec2& value) {
+        glUniform2fv(GetUniformLocation(name), 1, &value[0]);
+    }
+    inline void SetVec3(const std::string& name, const glm::vec3& value) {
+        glUniform3fv(GetUniformLocation(name), 1, &value[0]);
+    }
+    inline void SetVec4(const std::string& name, const glm::vec4& value) {
+        glUniform4fv(GetUniformLocation(name), 1, &value[0]);
+    }
+    inline void SetMat4(const std::string& name, const glm::mat4& mat) {
+        glUniformMatrix4fv(GetUniformLocation(name), 1, GL_FALSE, &mat[0][0]);
+    }
 
 private:
     GLuint id = 0;
     std::unordered_map<std::string, GLint> uniformCache;
 
+    GLint GetUniformLocation(const std::string& name);
     std::string ReadFile(const std::string& path);
     GLuint Compile(GLenum type, const std::string& source);
-    GLint GetUniformLocation(const std::string& name);
-    void Clear();
 };

src/vendor/box2d/LICENSE

@@ -0,0 +1,21 @@
+MIT License
+
+Copyright (c) 2022 Erin Catto
+
+Permission is hereby granted, free of charge, to any person obtaining a copy
+of this software and associated documentation files (the "Software"), to deal
+in the Software without restriction, including without limitation the rights
+to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+copies of the Software, and to permit persons to whom the Software is
+furnished to do so, subject to the following conditions:
+
+The above copyright notice and this permission notice shall be included in all
+copies or substantial portions of the Software.
+
+THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+SOFTWARE.

src/vendor/box2d/aabb.c

@@ -0,0 +1,132 @@
+// SPDX-FileCopyrightText: 2023 Erin Catto
+// SPDX-License-Identifier: MIT
+
+#include "aabb.h"
+
+#include "box2d/math_functions.h"
+
+#include <float.h>
+
+bool b2IsValidAABB( b2AABB a )
+{
+	b2Vec2 d = b2Sub( a.upperBound, a.lowerBound );
+	bool valid = d.x >= 0.0f && d.y >= 0.0f;
+	valid = valid && b2IsValidVec2( a.lowerBound ) && b2IsValidVec2( a.upperBound );
+	return valid;
+}
+
+// From Real-time Collision Detection, p179.
+b2CastOutput b2AABB_RayCast( b2AABB a, b2Vec2 p1, b2Vec2 p2 )
+{
+	// Radius not handled
+	b2CastOutput output = { 0 };
+
+	float tmin = -FLT_MAX;
+	float tmax = FLT_MAX;
+
+	b2Vec2 p = p1;
+	b2Vec2 d = b2Sub( p2, p1 );
+	b2Vec2 absD = b2Abs( d );
+
+	b2Vec2 normal = b2Vec2_zero;
+
+	// x-coordinate
+	if ( absD.x < FLT_EPSILON )
+	{
+		// parallel
+		if ( p.x < a.lowerBound.x || a.upperBound.x < p.x )
+		{
+			return output;
+		}
+	}
+	else
+	{
+		float inv_d = 1.0f / d.x;
+		float t1 = ( a.lowerBound.x - p.x ) * inv_d;
+		float t2 = ( a.upperBound.x - p.x ) * inv_d;
+
+		// Sign of the normal vector.
+		float s = -1.0f;
+
+		if ( t1 > t2 )
+		{
+			float tmp = t1;
+			t1 = t2;
+			t2 = tmp;
+			s = 1.0f;
+		}
+
+		// Push the min up
+		if ( t1 > tmin )
+		{
+			normal.y = 0.0f;
+			normal.x = s;
+			tmin = t1;
+		}
+
+		// Pull the max down
+		tmax = b2MinFloat( tmax, t2 );
+
+		if ( tmin > tmax )
+		{
+			return output;
+		}
+	}
+
+	// y-coordinate
+	if ( absD.y < FLT_EPSILON )
+	{
+		// parallel
+		if ( p.y < a.lowerBound.y || a.upperBound.y < p.y )
+		{
+			return output;
+		}
+	}
+	else
+	{
+		float inv_d = 1.0f / d.y;
+		float t1 = ( a.lowerBound.y - p.y ) * inv_d;
+		float t2 = ( a.upperBound.y - p.y ) * inv_d;
+
+		// Sign of the normal vector.
+		float s = -1.0f;
+
+		if ( t1 > t2 )
+		{
+			float tmp = t1;
+			t1 = t2;
+			t2 = tmp;
+			s = 1.0f;
+		}
+
+		// Push the min up
+		if ( t1 > tmin )
+		{
+			normal.x = 0.0f;
+			normal.y = s;
+			tmin = t1;
+		}
+
+		// Pull the max down
+		tmax = b2MinFloat( tmax, t2 );
+
+		if ( tmin > tmax )
+		{
+			return output;
+		}
+	}
+
+	// Does the ray start inside the box?
+	// Does the ray intersect beyond the max fraction?
+	if ( tmin < 0.0f || 1.0f < tmin )
+	{
+		return output;
+	}
+
+	// Intersection.
+	output.fraction = tmin;
+	output.normal = normal;
+	output.point = b2Lerp( p1, p2, tmin );
+	output.hit = true;
+	return output;
+}

src/vendor/box2d/aabb.h

@@ -0,0 +1,56 @@
+// SPDX-FileCopyrightText: 2023 Erin Catto
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include "box2d/types.h"
+
+// Ray cast an AABB
+b2CastOutput b2AABB_RayCast( b2AABB a, b2Vec2 p1, b2Vec2 p2 );
+
+// Get surface area of an AABB (the perimeter length)
+static inline float b2Perimeter( b2AABB a )
+{
+	float wx = a.upperBound.x - a.lowerBound.x;
+	float wy = a.upperBound.y - a.lowerBound.y;
+	return 2.0f * ( wx + wy );
+}
+
+/// Enlarge a to contain b
+/// @return true if the AABB grew
+static inline bool b2EnlargeAABB( b2AABB* a, b2AABB b )
+{
+	bool changed = false;
+	if ( b.lowerBound.x < a->lowerBound.x )
+	{
+		a->lowerBound.x = b.lowerBound.x;
+		changed = true;
+	}
+
+	if ( b.lowerBound.y < a->lowerBound.y )
+	{
+		a->lowerBound.y = b.lowerBound.y;
+		changed = true;
+	}
+
+	if ( a->upperBound.x < b.upperBound.x )
+	{
+		a->upperBound.x = b.upperBound.x;
+		changed = true;
+	}
+
+	if ( a->upperBound.y < b.upperBound.y )
+	{
+		a->upperBound.y = b.upperBound.y;
+		changed = true;
+	}
+
+	return changed;
+}
+
+/// Do a and b overlap
+static inline bool b2AABB_Overlaps( b2AABB a, b2AABB b )
+{
+	return !( b.lowerBound.x > a.upperBound.x || b.lowerBound.y > a.upperBound.y || a.lowerBound.x > b.upperBound.x ||
+			  a.lowerBound.y > b.upperBound.y );
+}

src/vendor/box2d/arena_allocator.c

@@ -0,0 +1,112 @@
+// SPDX-FileCopyrightText: 2023 Erin Catto
+// SPDX-License-Identifier: MIT
+
+#include "arena_allocator.h"
+
+#include "array.h"
+#include "core.h"
+
+#include <stdbool.h>
+#include <stddef.h>
+
+B2_ARRAY_SOURCE( b2ArenaEntry, b2ArenaEntry )
+
+b2ArenaAllocator b2CreateArenaAllocator( int capacity )
+{
+	B2_ASSERT( capacity >= 0 );
+	b2ArenaAllocator allocator = { 0 };
+	allocator.capacity = capacity;
+	allocator.data = b2Alloc( capacity );
+	allocator.allocation = 0;
+	allocator.maxAllocation = 0;
+	allocator.index = 0;
+	allocator.entries = b2ArenaEntryArray_Create( 32 );
+	return allocator;
+}
+
+void b2DestroyArenaAllocator( b2ArenaAllocator* allocator )
+{
+	b2ArenaEntryArray_Destroy( &allocator->entries );
+	b2Free( allocator->data, allocator->capacity );
+}
+
+void* b2AllocateArenaItem( b2ArenaAllocator* alloc, int size, const char* name )
+{
+	// ensure allocation is 32 byte aligned to support 256-bit SIMD
+	int size32 = ( ( size - 1 ) | 0x1F ) + 1;
+
+	b2ArenaEntry entry;
+	entry.size = size32;
+	entry.name = name;
+	if ( alloc->index + size32 > alloc->capacity )
+	{
+		// fall back to the heap (undesirable)
+		entry.data = b2Alloc( size32 );
+		entry.usedMalloc = true;
+
+		B2_ASSERT( ( (uintptr_t)entry.data & 0x1F ) == 0 );
+	}
+	else
+	{
+		entry.data = alloc->data + alloc->index;
+		entry.usedMalloc = false;
+		alloc->index += size32;
+
+		B2_ASSERT( ( (uintptr_t)entry.data & 0x1F ) == 0 );
+	}
+
+	alloc->allocation += size32;
+	if ( alloc->allocation > alloc->maxAllocation )
+	{
+		alloc->maxAllocation = alloc->allocation;
+	}
+
+	b2ArenaEntryArray_Push( &alloc->entries, entry );
+	return entry.data;
+}
+
+void b2FreeArenaItem( b2ArenaAllocator* alloc, void* mem )
+{
+	int entryCount = alloc->entries.count;
+	B2_ASSERT( entryCount > 0 );
+	b2ArenaEntry* entry = alloc->entries.data + ( entryCount - 1 );
+	B2_ASSERT( mem == entry->data );
+	if ( entry->usedMalloc )
+	{
+		b2Free( mem, entry->size );
+	}
+	else
+	{
+		alloc->index -= entry->size;
+	}
+	alloc->allocation -= entry->size;
+	b2ArenaEntryArray_Pop( &alloc->entries );
+}
+
+void b2GrowArena( b2ArenaAllocator* alloc )
+{
+	// Stack must not be in use
+	B2_ASSERT( alloc->allocation == 0 );
+
+	if ( alloc->maxAllocation > alloc->capacity )
+	{
+		b2Free( alloc->data, alloc->capacity );
+		alloc->capacity = alloc->maxAllocation + alloc->maxAllocation / 2;
+		alloc->data = b2Alloc( alloc->capacity );
+	}
+}
+
+int b2GetArenaCapacity( b2ArenaAllocator* alloc )
+{
+	return alloc->capacity;
+}
+
+int b2GetArenaAllocation( b2ArenaAllocator* alloc )
+{
+	return alloc->allocation;
+}
+
+int b2GetMaxArenaAllocation( b2ArenaAllocator* alloc )
+{
+	return alloc->maxAllocation;
+}

src/vendor/box2d/arena_allocator.h

@@ -0,0 +1,48 @@
+// SPDX-FileCopyrightText: 2023 Erin Catto
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include "array.h"
+
+B2_ARRAY_DECLARE( b2ArenaEntry, b2ArenaEntry );
+
+typedef struct b2ArenaEntry
+{
+	char* data;
+	const char* name;
+	int size;
+	bool usedMalloc;
+} b2ArenaEntry;
+
+// This is a stack-like arena allocator used for fast per step allocations.
+// You must nest allocate/free pairs. The code will B2_ASSERT
+// if you try to interleave multiple allocate/free pairs.
+// This allocator uses the heap if space is insufficient.
+// I could remove the need to free entries individually.
+typedef struct b2ArenaAllocator
+{
+	char* data;
+	int capacity;
+	int index;
+
+	int allocation;
+	int maxAllocation;
+
+	b2ArenaEntryArray entries;
+} b2ArenaAllocator;
+
+b2ArenaAllocator b2CreateArenaAllocator( int capacity );
+void b2DestroyArenaAllocator( b2ArenaAllocator* allocator );
+
+void* b2AllocateArenaItem( b2ArenaAllocator* alloc, int size, const char* name );
+void b2FreeArenaItem( b2ArenaAllocator* alloc, void* mem );
+
+// Grow the arena based on usage
+void b2GrowArena( b2ArenaAllocator* alloc );
+
+int b2GetArenaCapacity( b2ArenaAllocator* alloc );
+int b2GetArenaAllocation( b2ArenaAllocator* alloc );
+int b2GetMaxArenaAllocation( b2ArenaAllocator* alloc );
+
+B2_ARRAY_INLINE( b2ArenaEntry, b2ArenaEntry )

src/vendor/box2d/array.c

@@ -0,0 +1,8 @@
+// SPDX-FileCopyrightText: 2023 Erin Catto
+// SPDX-License-Identifier: MIT
+
+#include "array.h"
+
+#include <stddef.h>
+
+B2_ARRAY_SOURCE( int, b2Int )

src/vendor/box2d/array.h

@@ -0,0 +1,179 @@
+// SPDX-FileCopyrightText: 2023 Erin Catto
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include "core.h"
+
+// Macro generated functions for dynamic arrays
+// Pros
+// - type safe
+// - array data debuggable (visible count and capacity)
+// - bounds checking
+// - forward declaration
+// - simple implementation
+// - generates functions (like C++ templates)
+// - functions have https://en.wikipedia.org/wiki/Sequence_point
+// - avoids stretchy buffer dropped pointer update bugs
+// Cons
+// - cannot debug
+// - breaks code navigation
+
+// todo_erin consider code-gen: https://github.com/IbrahimHindawi/haikal
+
+// Array declaration that doesn't need the type T to be defined
+#define B2_ARRAY_DECLARE( T, PREFIX )                                                                                            \
+	typedef struct                                                                                                               \
+	{                                                                                                                            \
+		struct T* data;                                                                                                          \
+		int count;                                                                                                               \
+		int capacity;                                                                                                            \
+	} PREFIX##Array;                                                                                                             \
+	PREFIX##Array PREFIX##Array_Create( int capacity );                                                                          \
+	void PREFIX##Array_Reserve( PREFIX##Array* a, int newCapacity );                                                             \
+	void PREFIX##Array_Destroy( PREFIX##Array* a )
+
+#define B2_DECLARE_ARRAY_NATIVE( T, PREFIX )                                                                                     \
+	typedef struct                                                                                                               \
+	{                                                                                                                            \
+		T* data;                                                                                                                 \
+		int count;                                                                                                               \
+		int capacity;                                                                                                            \
+	} PREFIX##Array;                                                                                                             \
+	/* Create array with initial capacity. Zero initialization is also supported */                                              \
+	PREFIX##Array PREFIX##Array_Create( int capacity );                                                                          \
+	void PREFIX##Array_Reserve( PREFIX##Array* a, int newCapacity );                                                             \
+	void PREFIX##Array_Destroy( PREFIX##Array* a )
+
+// Inline array functions that need the type T to be defined
+#define B2_ARRAY_INLINE( T, PREFIX )                                                                                             \
+	/* Resize */                                                                                                                 \
+	static inline void PREFIX##Array_Resize( PREFIX##Array* a, int count )                                                       \
+	{                                                                                                                            \
+		PREFIX##Array_Reserve( a, count );                                                                                       \
+		a->count = count;                                                                                                        \
+	}                                                                                                                            \
+	/* Get */                                                                                                                    \
+	static inline T* PREFIX##Array_Get( PREFIX##Array* a, int index )                                                            \
+	{                                                                                                                            \
+		B2_ASSERT( 0 <= index && index < a->count );                                                                             \
+		return a->data + index;                                                                                                  \
+	}                                                                                                                            \
+	/* Add */                                                                                                                    \
+	static inline T* PREFIX##Array_Add( PREFIX##Array* a )                                                                       \
+	{                                                                                                                            \
+		if ( a->count == a->capacity )                                                                                           \
+		{                                                                                                                        \
+			int newCapacity = a->capacity < 2 ? 2 : a->capacity + ( a->capacity >> 1 );                                          \
+			PREFIX##Array_Reserve( a, newCapacity );                                                                             \
+		}                                                                                                                        \
+		a->count += 1;                                                                                                           \
+		return a->data + ( a->count - 1 );                                                                                       \
+	}                                                                                                                            \
+	/* Push */                                                                                                                   \
+	static inline void PREFIX##Array_Push( PREFIX##Array* a, T value )                                                           \
+	{                                                                                                                            \
+		if ( a->count == a->capacity )                                                                                           \
+		{                                                                                                                        \
+			int newCapacity = a->capacity < 2 ? 2 : a->capacity + ( a->capacity >> 1 );                                          \
+			PREFIX##Array_Reserve( a, newCapacity );                                                                             \
+		}                                                                                                                        \
+		a->data[a->count] = value;                                                                                               \
+		a->count += 1;                                                                                                           \
+	}                                                                                                                            \
+	/* Set */                                                                                                                    \
+	static inline void PREFIX##Array_Set( PREFIX##Array* a, int index, T value )                                                 \
+	{                                                                                                                            \
+		B2_ASSERT( 0 <= index && index < a->count );                                                                             \
+		a->data[index] = value;                                                                                                  \
+	}                                                                                                                            \
+	/* RemoveSwap */                                                                                                             \
+	static inline int PREFIX##Array_RemoveSwap( PREFIX##Array* a, int index )                                                    \
+	{                                                                                                                            \
+		B2_ASSERT( 0 <= index && index < a->count );                                                                             \
+		int movedIndex = B2_NULL_INDEX;                                                                                          \
+		if ( index != a->count - 1 )                                                                                             \
+		{                                                                                                                        \
+			movedIndex = a->count - 1;                                                                                           \
+			a->data[index] = a->data[movedIndex];                                                                                \
+		}                                                                                                                        \
+		a->count -= 1;                                                                                                           \
+		return movedIndex;                                                                                                       \
+	}                                                                                                                            \
+	/* Pop */                                                                                                                    \
+	static inline T PREFIX##Array_Pop( PREFIX##Array* a )                                                                        \
+	{                                                                                                                            \
+		B2_ASSERT( a->count > 0 );                                                                                               \
+		T value = a->data[a->count - 1];                                                                                         \
+		a->count -= 1;                                                                                                           \
+		return value;                                                                                                            \
+	}                                                                                                                            \
+	/* Clear */                                                                                                                  \
+	static inline void PREFIX##Array_Clear( PREFIX##Array* a )                                                                   \
+	{                                                                                                                            \
+		a->count = 0;                                                                                                            \
+	}                                                                                                                            \
+	/* ByteCount */                                                                                                              \
+	static inline int PREFIX##Array_ByteCount( PREFIX##Array* a )                                                                \
+	{                                                                                                                            \
+		return (int)( a->capacity * sizeof( T ) );                                                                               \
+	}
+
+// Array implementations to be instantiated in a source file where the type T is known
+#define B2_ARRAY_SOURCE( T, PREFIX )                                                                                             \
+	/* Create */                                                                                                                 \
+	PREFIX##Array PREFIX##Array_Create( int capacity )                                                                           \
+	{                                                                                                                            \
+		PREFIX##Array a = { 0 };                                                                                                 \
+		if ( capacity > 0 )                                                                                                      \
+		{                                                                                                                        \
+			a.data = b2Alloc( capacity * sizeof( T ) );                                                                          \
+			a.capacity = capacity;                                                                                               \
+		}                                                                                                                        \
+		return a;                                                                                                                \
+	}                                                                                                                            \
+	/* Reserve */                                                                                                                \
+	void PREFIX##Array_Reserve( PREFIX##Array* a, int newCapacity )                                                              \
+	{                                                                                                                            \
+		if ( newCapacity <= a->capacity )                                                                                        \
+		{                                                                                                                        \
+			return;                                                                                                              \
+		}                                                                                                                        \
+		a->data = b2GrowAlloc( a->data, a->capacity * sizeof( T ), newCapacity * sizeof( T ) );                                  \
+		a->capacity = newCapacity;                                                                                               \
+	}                                                                                                                            \
+	/* Destroy */                                                                                                                \
+	void PREFIX##Array_Destroy( PREFIX##Array* a )                                                                               \
+	{                                                                                                                            \
+		b2Free( a->data, a->capacity * sizeof( T ) );                                                                            \
+		a->data = NULL;                                                                                                          \
+		a->count = 0;                                                                                                            \
+		a->capacity = 0;                                                                                                         \
+	}
+
+B2_DECLARE_ARRAY_NATIVE( int, b2Int );
+B2_ARRAY_INLINE( int, b2Int )
+
+// Declare all the arrays
+B2_ARRAY_DECLARE( b2Body, b2Body );
+B2_ARRAY_DECLARE( b2BodyMoveEvent, b2BodyMoveEvent );
+B2_ARRAY_DECLARE( b2BodySim, b2BodySim );
+B2_ARRAY_DECLARE( b2BodyState, b2BodyState );
+B2_ARRAY_DECLARE( b2ChainShape, b2ChainShape );
+B2_ARRAY_DECLARE( b2Contact, b2Contact );
+B2_ARRAY_DECLARE( b2ContactBeginTouchEvent, b2ContactBeginTouchEvent );
+B2_ARRAY_DECLARE( b2ContactEndTouchEvent, b2ContactEndTouchEvent );
+B2_ARRAY_DECLARE( b2ContactHitEvent, b2ContactHitEvent );
+B2_ARRAY_DECLARE( b2ContactSim, b2ContactSim );
+B2_ARRAY_DECLARE( b2Island, b2Island );
+B2_ARRAY_DECLARE( b2IslandSim, b2IslandSim );
+B2_ARRAY_DECLARE( b2Joint, b2Joint );
+B2_ARRAY_DECLARE( b2JointSim, b2JointSim );
+B2_ARRAY_DECLARE( b2Sensor, b2Sensor );
+B2_ARRAY_DECLARE( b2SensorBeginTouchEvent, b2SensorBeginTouchEvent );
+B2_ARRAY_DECLARE( b2SensorEndTouchEvent, b2SensorEndTouchEvent );
+B2_ARRAY_DECLARE( b2SensorTaskContext, b2SensorTaskContext );
+B2_ARRAY_DECLARE( b2Shape, b2Shape );
+B2_ARRAY_DECLARE( b2ShapeRef, b2ShapeRef );
+B2_ARRAY_DECLARE( b2SolverSet, b2SolverSet );
+B2_ARRAY_DECLARE( b2TaskContext, b2TaskContext );

src/vendor/box2d/atomic.h

@@ -0,0 +1,79 @@
+// SPDX-FileCopyrightText: 2023 Erin Catto
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include "core.h"
+
+#include <stdint.h>
+
+#if defined( _MSC_VER )
+#include <intrin.h>
+#endif
+
+static inline void b2AtomicStoreInt( b2AtomicInt* a, int value )
+{
+#if defined( _MSC_VER )
+	(void)_InterlockedExchange( (long*)&a->value, value );
+#elif defined( __GNUC__ ) || defined( __clang__ )
+	__atomic_store_n( &a->value, value, __ATOMIC_SEQ_CST );
+#else
+#error "Unsupported platform"
+#endif
+}
+
+static inline int b2AtomicLoadInt( b2AtomicInt* a )
+{
+#if defined( _MSC_VER )
+	return _InterlockedOr( (long*)&a->value, 0 );
+#elif defined( __GNUC__ ) || defined( __clang__ )
+	return __atomic_load_n( &a->value, __ATOMIC_SEQ_CST );
+#else
+#error "Unsupported platform"
+#endif
+}
+
+static inline int b2AtomicFetchAddInt( b2AtomicInt* a, int increment )
+{
+#if defined( _MSC_VER )
+	return _InterlockedExchangeAdd( (long*)&a->value, (long)increment );
+#elif defined( __GNUC__ ) || defined( __clang__ )
+	return __atomic_fetch_add( &a->value, increment, __ATOMIC_SEQ_CST );
+#else
+#error "Unsupported platform"
+#endif
+}
+
+static inline bool b2AtomicCompareExchangeInt( b2AtomicInt* a, int expected, int desired )
+{
+#if defined( _MSC_VER )
+	return _InterlockedCompareExchange( (long*)&a->value, (long)desired, (long)expected ) == expected;
+#elif defined( __GNUC__ ) || defined( __clang__ )
+	// The value written to expected is ignored
+	return __atomic_compare_exchange_n( &a->value, &expected, desired, false, __ATOMIC_SEQ_CST, __ATOMIC_SEQ_CST );
+#else
+#error "Unsupported platform"
+#endif
+}
+
+static inline void b2AtomicStoreU32( b2AtomicU32* a, uint32_t value )
+{
+#if defined( _MSC_VER )
+	(void)_InterlockedExchange( (long*)&a->value, value );
+#elif defined( __GNUC__ ) || defined( __clang__ )
+	__atomic_store_n( &a->value, value, __ATOMIC_SEQ_CST );
+#else
+#error "Unsupported platform"
+#endif
+}
+
+static inline uint32_t b2AtomicLoadU32( b2AtomicU32* a )
+{
+#if defined( _MSC_VER )
+	return (uint32_t)_InterlockedOr( (long*)&a->value, 0 );
+#elif defined( __GNUC__ ) || defined( __clang__ )
+	return __atomic_load_n( &a->value, __ATOMIC_SEQ_CST );
+#else
+#error "Unsupported platform"
+#endif
+}

src/vendor/box2d/base.h

@@ -0,0 +1,131 @@
+// SPDX-FileCopyrightText: 2023 Erin Catto
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <stdint.h>
+
+// clang-format off
+// 
+// Shared library macros
+#if defined( _MSC_VER ) && defined( box2d_EXPORTS )
+	// build the Windows DLL
+	#define BOX2D_EXPORT __declspec( dllexport )
+#elif defined( _MSC_VER ) && defined( BOX2D_DLL )
+	// using the Windows DLL
+	#define BOX2D_EXPORT __declspec( dllimport )
+#elif defined( box2d_EXPORTS )
+	// building or using the shared library
+	#define BOX2D_EXPORT __attribute__( ( visibility( "default" ) ) )
+#else
+	// static library
+	#define BOX2D_EXPORT
+#endif
+
+// C++ macros
+#ifdef __cplusplus
+	#define B2_API extern "C" BOX2D_EXPORT
+	#define B2_INLINE inline
+	#define B2_LITERAL(T) T
+	#define B2_ZERO_INIT {}
+#else
+	#define B2_API BOX2D_EXPORT
+	#define B2_INLINE static inline
+	/// Used for C literals like (b2Vec2){1.0f, 2.0f} where C++ requires b2Vec2{1.0f, 2.0f}
+	#define B2_LITERAL(T) (T)
+	#define B2_ZERO_INIT {0}
+#endif
+// clang-format on
+
+/**
+ * @defgroup base Base
+ * Base functionality
+ * @{
+ */
+
+/// Prototype for user allocation function
+/// @param size the allocation size in bytes
+/// @param alignment the required alignment, guaranteed to be a power of 2
+typedef void* b2AllocFcn( unsigned int size, int alignment );
+
+/// Prototype for user free function
+/// @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 );
+
+/// @return the total bytes allocated by Box2D
+B2_API int b2GetByteCount( void );
+
+/// Override the default assert callback
+/// @param assertFcn a non-null assert callback
+B2_API void b2SetAssertFcn( b2AssertFcn* assertFcn );
+
+/// Version numbering scheme.
+/// See https://semver.org/
+typedef struct b2Version
+{
+	/// Significant changes
+	int major;
+
+	/// Incremental changes
+	int minor;
+
+	/// Bug fixes
+	int revision;
+} b2Version;
+
+/// Get the current version of Box2D
+B2_API b2Version b2GetVersion( void );
+
+/**@}*/
+
+//! @cond
+
+// see https://github.com/scottt/debugbreak
+#if defined( _MSC_VER )
+#define B2_BREAKPOINT __debugbreak()
+#elif defined( __GNUC__ ) || defined( __clang__ )
+#define B2_BREAKPOINT __builtin_trap()
+#else
+// Unknown compiler
+#include <assert.h>
+#define B2_BREAKPOINT assert( 0 )
+#endif
+
+#if !defined( NDEBUG ) || defined( B2_ENABLE_ASSERT )
+B2_API int b2InternalAssertFcn( const char* condition, const char* fileName, int lineNumber );
+#define B2_ASSERT( condition )                                                                                                   \
+	do                                                                                                                           \
+	{                                                                                                                            \
+		if ( !( condition ) && b2InternalAssertFcn( #condition, __FILE__, (int)__LINE__ ) )                                          \
+			B2_BREAKPOINT;                                                                                                       \
+	}                                                                                                                            \
+	while ( 0 )
+#else
+#define B2_ASSERT( ... ) ( (void)0 )
+#endif
+
+/// Get the absolute number of system ticks. The value is platform specific.
+B2_API uint64_t b2GetTicks( void );
+
+/// Get the milliseconds passed from an initial tick value.
+B2_API float b2GetMilliseconds( uint64_t ticks );
+
+/// Get the milliseconds passed from an initial tick value. Resets the passed in
+/// value to the current tick value.
+B2_API float b2GetMillisecondsAndReset( uint64_t* ticks );
+
+/// Yield to be used in a busy loop.
+B2_API void b2Yield( void );
+
+/// Simple djb2 hash function for determinism testing
+#define B2_HASH_INIT 5381
+B2_API uint32_t b2Hash( uint32_t hash, const uint8_t* data, int count );
+
+//! @endcond

src/vendor/box2d/bitset.c

@@ -0,0 +1,67 @@
+// SPDX-FileCopyrightText: 2023 Erin Catto
+// SPDX-License-Identifier: MIT
+
+#include "bitset.h"
+
+#include <string.h>
+
+b2BitSet b2CreateBitSet( uint32_t bitCapacity )
+{
+	b2BitSet bitSet = { 0 };
+
+	bitSet.blockCapacity = ( bitCapacity + sizeof( uint64_t ) * 8 - 1 ) / ( sizeof( uint64_t ) * 8 );
+	bitSet.blockCount = 0;
+	bitSet.bits = b2Alloc( bitSet.blockCapacity * sizeof( uint64_t ) );
+	memset( bitSet.bits, 0, bitSet.blockCapacity * sizeof( uint64_t ) );
+	return bitSet;
+}
+
+void b2DestroyBitSet( b2BitSet* bitSet )
+{
+	b2Free( bitSet->bits, bitSet->blockCapacity * sizeof( uint64_t ) );
+	bitSet->blockCapacity = 0;
+	bitSet->blockCount = 0;
+	bitSet->bits = NULL;
+}
+
+void b2SetBitCountAndClear( b2BitSet* bitSet, uint32_t bitCount )
+{
+	uint32_t blockCount = ( bitCount + sizeof( uint64_t ) * 8 - 1 ) / ( sizeof( uint64_t ) * 8 );
+	if ( bitSet->blockCapacity < blockCount )
+	{
+		b2DestroyBitSet( bitSet );
+		uint32_t newBitCapacity = bitCount + ( bitCount >> 1 );
+		*bitSet = b2CreateBitSet( newBitCapacity );
+	}
+
+	bitSet->blockCount = blockCount;
+	memset( bitSet->bits, 0, bitSet->blockCount * sizeof( uint64_t ) );
+}
+
+void b2GrowBitSet( b2BitSet* bitSet, uint32_t blockCount )
+{
+	B2_ASSERT( blockCount > bitSet->blockCount );
+	if ( blockCount > bitSet->blockCapacity )
+	{
+		uint32_t oldCapacity = bitSet->blockCapacity;
+		bitSet->blockCapacity = blockCount + blockCount / 2;
+		uint64_t* newBits = b2Alloc( bitSet->blockCapacity * sizeof( uint64_t ) );
+		memset( newBits, 0, bitSet->blockCapacity * sizeof( uint64_t ) );
+		B2_ASSERT( bitSet->bits != NULL );
+		memcpy( newBits, bitSet->bits, oldCapacity * sizeof( uint64_t ) );
+		b2Free( bitSet->bits, oldCapacity * sizeof( uint64_t ) );
+		bitSet->bits = newBits;
+	}
+
+	bitSet->blockCount = blockCount;
+}
+
+void b2InPlaceUnion( b2BitSet* B2_RESTRICT setA, const b2BitSet* B2_RESTRICT setB )
+{
+	B2_ASSERT( setA->blockCount == setB->blockCount );
+	uint32_t blockCount = setA->blockCount;
+	for ( uint32_t i = 0; i < blockCount; ++i )
+	{
+		setA->bits[i] |= setB->bits[i];
+	}
+}

src/vendor/box2d/bitset.h

@@ -0,0 +1,65 @@
+// SPDX-FileCopyrightText: 2023 Erin Catto
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include "core.h"
+
+#include <stdbool.h>
+#include <stdint.h>
+
+// Bit set provides fast operations on large arrays of bits.
+typedef struct b2BitSet
+{
+	uint64_t* bits;
+	uint32_t blockCapacity;
+	uint32_t blockCount;
+} b2BitSet;
+
+b2BitSet b2CreateBitSet( uint32_t bitCapacity );
+void b2DestroyBitSet( b2BitSet* bitSet );
+void b2SetBitCountAndClear( b2BitSet* bitSet, uint32_t bitCount );
+void b2InPlaceUnion( b2BitSet* setA, const b2BitSet* setB );
+void b2GrowBitSet( b2BitSet* bitSet, uint32_t blockCount );
+
+static inline void b2SetBit( b2BitSet* bitSet, uint32_t bitIndex )
+{
+	uint32_t blockIndex = bitIndex / 64;
+	B2_ASSERT( blockIndex < bitSet->blockCount );
+	bitSet->bits[blockIndex] |= ( (uint64_t)1 << bitIndex % 64 );
+}
+
+static inline void b2SetBitGrow( b2BitSet* bitSet, uint32_t bitIndex )
+{
+	uint32_t blockIndex = bitIndex / 64;
+	if ( blockIndex >= bitSet->blockCount )
+	{
+		b2GrowBitSet( bitSet, blockIndex + 1 );
+	}
+	bitSet->bits[blockIndex] |= ( (uint64_t)1 << bitIndex % 64 );
+}
+
+static inline void b2ClearBit( b2BitSet* bitSet, uint32_t bitIndex )
+{
+	uint32_t blockIndex = bitIndex / 64;
+	if ( blockIndex >= bitSet->blockCount )
+	{
+		return;
+	}
+	bitSet->bits[blockIndex] &= ~( (uint64_t)1 << bitIndex % 64 );
+}
+
+static inline bool b2GetBit( const b2BitSet* bitSet, uint32_t bitIndex )
+{
+	uint32_t blockIndex = bitIndex / 64;
+	if ( blockIndex >= bitSet->blockCount )
+	{
+		return false;
+	}
+	return ( bitSet->bits[blockIndex] & ( (uint64_t)1 << bitIndex % 64 ) ) != 0;
+}
+
+static inline int b2GetBitSetBytes( b2BitSet* bitSet )
+{
+	return bitSet->blockCapacity * sizeof( uint64_t );
+}

src/vendor/box2d/body.h

@@ -0,0 +1,194 @@
+// SPDX-FileCopyrightText: 2023 Erin Catto
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include "array.h"
+
+#include "box2d/math_functions.h"
+#include "box2d/types.h"
+
+typedef struct b2World b2World;
+
+// Body organizational details that are not used in the solver.
+typedef struct b2Body
+{
+	char name[32];
+
+	void* userData;
+
+	// index of solver set stored in b2World
+	// may be B2_NULL_INDEX
+	int setIndex;
+
+	// body sim and state index within set
+	// may be B2_NULL_INDEX
+	int localIndex;
+
+	// [31 : contactId | 1 : edgeIndex]
+	int headContactKey;
+	int contactCount;
+
+	// todo maybe move this to the body sim
+	int headShapeId;
+	int shapeCount;
+
+	int headChainId;
+
+	// [31 : jointId | 1 : edgeIndex]
+	int headJointKey;
+	int jointCount;
+
+	// All enabled dynamic and kinematic bodies are in an island.
+	int islandId;
+
+	// doubly-linked island list
+	int islandPrev;
+	int islandNext;
+
+	float mass;
+
+	// Rotational inertia about the center of mass.
+	float inertia;
+
+	float sleepThreshold;
+	float sleepTime;
+
+	// this is used to adjust the fellAsleep flag in the body move array
+	int bodyMoveIndex;
+
+	int id;
+
+	b2BodyType type;
+
+	// This is monotonically advanced when a body is allocated in this slot
+	// Used to check for invalid b2BodyId
+	uint16_t generation;
+
+	bool enableSleep;
+	bool fixedRotation;
+	bool isSpeedCapped;
+	bool isMarked;
+} b2Body;
+
+// Body State
+// The body state is designed for fast conversion to and from SIMD via scatter-gather.
+// Only awake dynamic and kinematic bodies have a body state.
+// This is used in the performance critical constraint solver
+//
+// The solver operates on the body state. The body state array does not hold static bodies. Static bodies are shared
+// across worker threads. It would be okay to read their states, but writing to them would cause cache thrashing across
+// workers, even if the values don't change.
+// This causes some trouble when computing anchors. I rotate joint anchors using the body rotation every sub-step. For static
+// bodies the anchor doesn't rotate. Body A or B could be static and this can lead to lots of branching. This branching
+// should be minimized.
+//
+// Solution 1:
+// Use delta rotations. This means anchors need to be prepared in world space. The delta rotation for static bodies will be
+// identity using a dummy state. Base separation and angles need to be computed. Manifolds will be behind a frame, but that
+// is probably best if bodies move fast.
+//
+// Solution 2:
+// Use full rotation. The anchors for static bodies will be in world space while the anchors for dynamic bodies will be in local
+// space. Potentially confusing and bug prone.
+//
+// Note:
+// I rotate joint anchors each sub-step but not contact anchors. Joint stability improves a lot by rotating joint anchors
+// according to substep progress. Contacts have reduced stability when anchors are rotated during substeps, especially for
+// round shapes.
+
+// 32 bytes
+typedef struct b2BodyState
+{
+	b2Vec2 linearVelocity; // 8
+	float angularVelocity; // 4
+	int flags;			   // 4
+
+	// Using delta position reduces round-off error far from the origin
+	b2Vec2 deltaPosition; // 8
+
+	// Using delta rotation because I cannot access the full rotation on static bodies in
+	// the solver and must use zero delta rotation for static bodies (c,s) = (1,0)
+	b2Rot deltaRotation; // 8
+} b2BodyState;
+
+// Identity body state, notice the deltaRotation is {1, 0}
+static const b2BodyState b2_identityBodyState = { { 0.0f, 0.0f }, 0.0f, 0, { 0.0f, 0.0f }, { 1.0f, 0.0f } };
+
+// Body simulation data used for integration of position and velocity
+// Transform data used for collision and solver preparation.
+typedef struct b2BodySim
+{
+	// todo better to have transform in sim or in base body? Try both!
+	// transform for body origin
+	b2Transform transform;
+
+	// center of mass position in world space
+	b2Vec2 center;
+
+	// previous rotation and COM for TOI
+	b2Rot rotation0;
+	b2Vec2 center0;
+
+	// location of center of mass relative to the body origin
+	b2Vec2 localCenter;
+
+	b2Vec2 force;
+	float torque;
+
+	// inverse inertia
+	float invMass;
+	float invInertia;
+
+	float minExtent;
+	float maxExtent;
+	float linearDamping;
+	float angularDamping;
+	float gravityScale;
+
+	// body data can be moved around, the id is stable (used in b2BodyId)
+	int bodyId;
+
+	// This flag is used for debug draw
+	bool isFast;
+
+	bool isBullet;
+	bool isSpeedCapped;
+	bool allowFastRotation;
+	bool enlargeAABB;
+} b2BodySim;
+
+// Get a validated body from a world using an id.
+b2Body* b2GetBodyFullId( b2World* world, b2BodyId bodyId );
+
+b2Transform b2GetBodyTransformQuick( b2World* world, b2Body* body );
+b2Transform b2GetBodyTransform( b2World* world, int bodyId );
+
+// Create a b2BodyId from a raw id.
+b2BodyId b2MakeBodyId( b2World* world, int bodyId );
+
+bool b2ShouldBodiesCollide( b2World* world, b2Body* bodyA, b2Body* bodyB );
+
+b2BodySim* b2GetBodySim( b2World* world, b2Body* body );
+b2BodyState* b2GetBodyState( b2World* world, b2Body* body );
+
+// careful calling this because it can invalidate body, state, joint, and contact pointers
+bool b2WakeBody( b2World* world, b2Body* body );
+
+void b2UpdateBodyMassData( b2World* world, b2Body* body );
+
+static inline b2Sweep b2MakeSweep( const b2BodySim* bodySim )
+{
+	b2Sweep s;
+	s.c1 = bodySim->center0;
+	s.c2 = bodySim->center;
+	s.q1 = bodySim->rotation0;
+	s.q2 = bodySim->transform.q;
+	s.localCenter = bodySim->localCenter;
+	return s;
+}
+
+// Define inline functions for arrays
+B2_ARRAY_INLINE( b2Body, b2Body )
+B2_ARRAY_INLINE( b2BodySim, b2BodySim )
+B2_ARRAY_INLINE( b2BodyState, b2BodyState )

src/vendor/box2d/broad_phase.c

@@ -0,0 +1,524 @@
+// SPDX-FileCopyrightText: 2023 Erin Catto
+// SPDX-License-Identifier: MIT
+
+#if defined( _MSC_VER ) && !defined( _CRT_SECURE_NO_WARNINGS )
+#define _CRT_SECURE_NO_WARNINGS
+#endif
+
+#include "broad_phase.h"
+
+#include "aabb.h"
+#include "array.h"
+#include "atomic.h"
+#include "body.h"
+#include "contact.h"
+#include "core.h"
+#include "shape.h"
+#include "arena_allocator.h"
+#include "world.h"
+
+#include <stdbool.h>
+#include <string.h>
+
+// #include <stdio.h>
+
+// static FILE* s_file = NULL;
+
+void b2CreateBroadPhase( b2BroadPhase* bp )
+{
+	_Static_assert( b2_bodyTypeCount == 3, "must be three body types" );
+
+	// if (s_file == NULL)
+	//{
+	//	s_file = fopen("pairs01.txt", "a");
+	//	fprintf(s_file, "============\n\n");
+	// }
+
+	bp->proxyCount = 0;
+	bp->moveSet = b2CreateSet( 16 );
+	bp->moveArray = b2IntArray_Create( 16 );
+	bp->moveResults = NULL;
+	bp->movePairs = NULL;
+	bp->movePairCapacity = 0;
+	b2AtomicStoreInt(&bp->movePairIndex, 0);
+	bp->pairSet = b2CreateSet( 32 );
+
+	for ( int i = 0; i < b2_bodyTypeCount; ++i )
+	{
+		bp->trees[i] = b2DynamicTree_Create();
+	}
+}
+
+void b2DestroyBroadPhase( b2BroadPhase* bp )
+{
+	for ( int i = 0; i < b2_bodyTypeCount; ++i )
+	{
+		b2DynamicTree_Destroy( bp->trees + i );
+	}
+
+	b2DestroySet( &bp->moveSet );
+	b2IntArray_Destroy( &bp->moveArray );
+	b2DestroySet( &bp->pairSet );
+
+	memset( bp, 0, sizeof( b2BroadPhase ) );
+
+	// if (s_file != NULL)
+	//{
+	//	fclose(s_file);
+	//	s_file = NULL;
+	// }
+}
+
+static inline void b2UnBufferMove( b2BroadPhase* bp, int proxyKey )
+{
+	bool found = b2RemoveKey( &bp->moveSet, proxyKey + 1 );
+
+	if ( found )
+	{
+		// Purge from move buffer. Linear search.
+		// todo if I can iterate the move set then I don't need the moveArray
+		int count = bp->moveArray.count;
+		for ( int i = 0; i < count; ++i )
+		{
+			if ( bp->moveArray.data[i] == proxyKey )
+			{
+				b2IntArray_RemoveSwap( &bp->moveArray, i );
+				break;
+			}
+		}
+	}
+}
+
+int b2BroadPhase_CreateProxy( b2BroadPhase* bp, b2BodyType proxyType, b2AABB aabb, uint64_t categoryBits, int shapeIndex,
+							  bool forcePairCreation )
+{
+	B2_ASSERT( 0 <= proxyType && proxyType < b2_bodyTypeCount );
+	int proxyId = b2DynamicTree_CreateProxy( bp->trees + proxyType, aabb, categoryBits, shapeIndex );
+	int proxyKey = B2_PROXY_KEY( proxyId, proxyType );
+	if ( proxyType != b2_staticBody || forcePairCreation )
+	{
+		b2BufferMove( bp, proxyKey );
+	}
+	return proxyKey;
+}
+
+void b2BroadPhase_DestroyProxy( b2BroadPhase* bp, int proxyKey )
+{
+	B2_ASSERT( bp->moveArray.count == (int)bp->moveSet.count );
+	b2UnBufferMove( bp, proxyKey );
+
+	--bp->proxyCount;
+
+	b2BodyType proxyType = B2_PROXY_TYPE( proxyKey );
+	int proxyId = B2_PROXY_ID( proxyKey );
+
+	B2_ASSERT( 0 <= proxyType && proxyType <= b2_bodyTypeCount );
+	b2DynamicTree_DestroyProxy( bp->trees + proxyType, proxyId );
+}
+
+void b2BroadPhase_MoveProxy( b2BroadPhase* bp, int proxyKey, b2AABB aabb )
+{
+	b2BodyType proxyType = B2_PROXY_TYPE( proxyKey );
+	int proxyId = B2_PROXY_ID( proxyKey );
+
+	b2DynamicTree_MoveProxy( bp->trees + proxyType, proxyId, aabb );
+	b2BufferMove( bp, proxyKey );
+}
+
+void b2BroadPhase_EnlargeProxy( b2BroadPhase* bp, int proxyKey, b2AABB aabb )
+{
+	B2_ASSERT( proxyKey != B2_NULL_INDEX );
+	int typeIndex = B2_PROXY_TYPE( proxyKey );
+	int proxyId = B2_PROXY_ID( proxyKey );
+
+	B2_ASSERT( typeIndex != b2_staticBody );
+
+	b2DynamicTree_EnlargeProxy( bp->trees + typeIndex, proxyId, aabb );
+	b2BufferMove( bp, proxyKey );
+}
+
+typedef struct b2MovePair
+{
+	int shapeIndexA;
+	int shapeIndexB;
+	b2MovePair* next;
+	bool heap;
+} b2MovePair;
+
+typedef struct b2MoveResult
+{
+	b2MovePair* pairList;
+} b2MoveResult;
+
+typedef struct b2QueryPairContext
+{
+	b2World* world;
+	b2MoveResult* moveResult;
+	b2BodyType queryTreeType;
+	int queryProxyKey;
+	int queryShapeIndex;
+} b2QueryPairContext;
+
+// This is called from b2DynamicTree::Query when we are gathering pairs.
+static bool b2PairQueryCallback( int proxyId, uint64_t userData, void* context )
+{
+	int shapeId = (int)userData;
+
+	b2QueryPairContext* queryContext = context;
+	b2BroadPhase* broadPhase = &queryContext->world->broadPhase;
+
+	int proxyKey = B2_PROXY_KEY( proxyId, queryContext->queryTreeType );
+	int queryProxyKey = queryContext->queryProxyKey;
+
+	// A proxy cannot form a pair with itself.
+	if ( proxyKey == queryContext->queryProxyKey )
+	{
+		return true;
+	}
+
+	b2BodyType treeType = queryContext->queryTreeType;
+	b2BodyType queryProxyType = B2_PROXY_TYPE( queryProxyKey );
+
+	// De-duplication
+	// It is important to prevent duplicate contacts from being created. Ideally I can prevent duplicates
+	// early and in the worker. Most of the time the moveSet contains dynamic and kinematic proxies, but
+	// sometimes it has static proxies.
+
+	// I had an optimization here to skip checking the move set if this is a query into
+	// the static tree. The assumption is that the static proxies are never in the move set
+	// so there is no risk of duplication. However, this is not true with
+	// b2ShapeDef::forceContactCreation, b2ShapeDef::isSensor, or when a static shape is modified.
+	// There can easily be scenarios where the static proxy is in the moveSet but the dynamic proxy is not.
+	// I could have some flag to indicate that there are any static bodies in the moveSet.
+	
+	// Is this proxy also moving?
+	if ( queryProxyType == b2_dynamicBody)
+	{
+		if ( treeType == b2_dynamicBody && proxyKey < queryProxyKey)
+		{
+			bool moved = b2ContainsKey( &broadPhase->moveSet, proxyKey + 1 );
+			if ( moved )
+			{
+				// Both proxies are moving. Avoid duplicate pairs.
+				return true;
+			}
+		}
+	}
+	else
+	{
+		B2_ASSERT( treeType == b2_dynamicBody );
+		bool moved = b2ContainsKey( &broadPhase->moveSet, proxyKey + 1 );
+		if ( moved )
+		{
+			// Both proxies are moving. Avoid duplicate pairs.
+			return true;
+		}
+	}
+
+	uint64_t pairKey = B2_SHAPE_PAIR_KEY( shapeId, queryContext->queryShapeIndex );
+	if ( b2ContainsKey( &broadPhase->pairSet, pairKey ) )
+	{
+		// contact exists
+		return true;
+	}
+
+	int shapeIdA, shapeIdB;
+	if ( proxyKey < queryProxyKey )
+	{
+		shapeIdA = shapeId;
+		shapeIdB = queryContext->queryShapeIndex;
+	}
+	else
+	{
+		shapeIdA = queryContext->queryShapeIndex;
+		shapeIdB = shapeId;
+	}
+
+	b2World* world = queryContext->world;
+
+	b2Shape* shapeA = b2ShapeArray_Get( &world->shapes, shapeIdA );
+	b2Shape* shapeB = b2ShapeArray_Get( &world->shapes, shapeIdB );
+
+	int bodyIdA = shapeA->bodyId;
+	int bodyIdB = shapeB->bodyId;
+
+	// Are the shapes on the same body?
+	if ( bodyIdA == bodyIdB )
+	{
+		return true;
+	}
+
+	// Sensors are handled elsewhere
+	if ( shapeA->sensorIndex != B2_NULL_INDEX || shapeB->sensorIndex != B2_NULL_INDEX )
+	{
+		return true;
+	}
+
+	if ( b2ShouldShapesCollide( shapeA->filter, shapeB->filter ) == false )
+	{
+		return true;
+	}
+
+	// Does a joint override collision?
+	b2Body* bodyA = b2BodyArray_Get( &world->bodies, bodyIdA );
+	b2Body* bodyB = b2BodyArray_Get( &world->bodies, bodyIdB );
+	if ( b2ShouldBodiesCollide( world, bodyA, bodyB ) == false )
+	{
+		return true;
+	}
+
+	// Custom user filter
+	b2CustomFilterFcn* customFilterFcn = queryContext->world->customFilterFcn;
+	if ( customFilterFcn != NULL )
+	{
+		b2ShapeId idA = { shapeIdA + 1, world->worldId, shapeA->generation };
+		b2ShapeId idB = { shapeIdB + 1, world->worldId, shapeB->generation };
+		bool shouldCollide = customFilterFcn( idA, idB, queryContext->world->customFilterContext );
+		if ( shouldCollide == false )
+		{
+			return true;
+		}
+	}
+
+	// todo per thread to eliminate atomic?
+	int pairIndex = b2AtomicFetchAddInt( &broadPhase->movePairIndex, 1 );
+
+	b2MovePair* pair;
+	if ( pairIndex < broadPhase->movePairCapacity )
+	{
+		pair = broadPhase->movePairs + pairIndex;
+		pair->heap = false;
+	}
+	else
+	{
+		pair = b2Alloc( sizeof( b2MovePair ) );
+		pair->heap = true;
+	}
+
+	pair->shapeIndexA = shapeIdA;
+	pair->shapeIndexB = shapeIdB;
+	pair->next = queryContext->moveResult->pairList;
+	queryContext->moveResult->pairList = pair;
+
+	// continue the query
+	return true;
+}
+
+// Warning: writing to these globals significantly slows multithreading performance
+#if B2_SNOOP_PAIR_COUNTERS
+b2TreeStats b2_dynamicStats;
+b2TreeStats b2_kinematicStats;
+b2TreeStats b2_staticStats;
+#endif
+
+static void b2FindPairsTask( int startIndex, int endIndex, uint32_t threadIndex, void* context )
+{
+	b2TracyCZoneNC( pair_task, "Pair", b2_colorMediumSlateBlue, true );
+
+	B2_UNUSED( threadIndex );
+
+	b2World* world = context;
+	b2BroadPhase* bp = &world->broadPhase;
+
+	b2QueryPairContext queryContext;
+	queryContext.world = world;
+
+	for ( int i = startIndex; i < endIndex; ++i )
+	{
+		// Initialize move result for this moved proxy
+		queryContext.moveResult = bp->moveResults + i;
+		queryContext.moveResult->pairList = NULL;
+
+		int proxyKey = bp->moveArray.data[i];
+		if ( proxyKey == B2_NULL_INDEX )
+		{
+			// proxy was destroyed after it moved
+			continue;
+		}
+
+		b2BodyType proxyType = B2_PROXY_TYPE( proxyKey );
+
+		int proxyId = B2_PROXY_ID( proxyKey );
+		queryContext.queryProxyKey = proxyKey;
+
+		const b2DynamicTree* baseTree = bp->trees + proxyType;
+
+		// We have to query the tree with the fat AABB so that
+		// we don't fail to create a contact that may touch later.
+		b2AABB fatAABB = b2DynamicTree_GetAABB( baseTree, proxyId );
+		queryContext.queryShapeIndex = (int)b2DynamicTree_GetUserData( baseTree, proxyId );
+
+		// Query trees. Only dynamic proxies collide with kinematic and static proxies.
+		// Using B2_DEFAULT_MASK_BITS so that b2Filter::groupIndex works.
+		b2TreeStats stats = { 0 };
+		if ( proxyType == b2_dynamicBody )
+		{
+			// consider using bits = groupIndex > 0 ? B2_DEFAULT_MASK_BITS : maskBits
+			queryContext.queryTreeType = b2_kinematicBody;
+			b2TreeStats statsKinematic = b2DynamicTree_Query( bp->trees + b2_kinematicBody, fatAABB, B2_DEFAULT_MASK_BITS, b2PairQueryCallback, &queryContext );
+			stats.nodeVisits += statsKinematic.nodeVisits;
+			stats.leafVisits += statsKinematic.leafVisits;
+
+			queryContext.queryTreeType = b2_staticBody;
+			b2TreeStats statsStatic = b2DynamicTree_Query( bp->trees + b2_staticBody, fatAABB, B2_DEFAULT_MASK_BITS, b2PairQueryCallback, &queryContext );
+			stats.nodeVisits += statsStatic.nodeVisits;
+			stats.leafVisits += statsStatic.leafVisits;
+		}
+
+		// All proxies collide with dynamic proxies
+		// Using B2_DEFAULT_MASK_BITS so that b2Filter::groupIndex works.
+		queryContext.queryTreeType = b2_dynamicBody;
+		b2TreeStats statsDynamic = b2DynamicTree_Query( bp->trees + b2_dynamicBody, fatAABB, B2_DEFAULT_MASK_BITS, b2PairQueryCallback, &queryContext );
+		stats.nodeVisits += statsDynamic.nodeVisits;
+		stats.leafVisits += statsDynamic.leafVisits;
+	}
+
+	b2TracyCZoneEnd( pair_task );
+}
+
+void b2UpdateBroadPhasePairs( b2World* world )
+{
+	b2BroadPhase* bp = &world->broadPhase;
+
+	int moveCount = bp->moveArray.count;
+	B2_ASSERT( moveCount == (int)bp->moveSet.count );
+
+	if ( moveCount == 0 )
+	{
+		return;
+	}
+
+	b2TracyCZoneNC( update_pairs, "Find Pairs", b2_colorMediumSlateBlue, true );
+
+	b2ArenaAllocator* alloc = &world->arena;
+
+	// todo these could be in the step context
+	bp->moveResults = b2AllocateArenaItem( alloc, moveCount * sizeof( b2MoveResult ), "move results" );
+	bp->movePairCapacity = 16 * moveCount;
+	bp->movePairs = b2AllocateArenaItem( alloc, bp->movePairCapacity * sizeof( b2MovePair ), "move pairs" );
+	b2AtomicStoreInt(&bp->movePairIndex, 0);
+
+#if B2_SNOOP_TABLE_COUNTERS
+	extern b2AtomicInt b2_probeCount;
+	b2AtomicStoreInt(&b2_probeCount, 0);
+#endif
+
+	int minRange = 64;
+	void* userPairTask = world->enqueueTaskFcn( &b2FindPairsTask, moveCount, minRange, world, world->userTaskContext );
+	if (userPairTask != NULL)
+	{
+		world->finishTaskFcn( userPairTask, world->userTaskContext );
+		world->taskCount += 1;
+	}
+
+	// todo_erin could start tree rebuild here
+
+	b2TracyCZoneNC( create_contacts, "Create Contacts", b2_colorCoral, true );
+
+	// Single-threaded work
+	// - Clear move flags
+	// - Create contacts in deterministic order
+	for ( int i = 0; i < moveCount; ++i )
+	{
+		b2MoveResult* result = bp->moveResults + i;
+		b2MovePair* pair = result->pairList;
+		while ( pair != NULL )
+		{
+			int shapeIdA = pair->shapeIndexA;
+			int shapeIdB = pair->shapeIndexB;
+
+			// if (s_file != NULL)
+			//{
+			//	fprintf(s_file, "%d %d\n", shapeIdA, shapeIdB);
+			// }
+
+			b2Shape* shapeA = b2ShapeArray_Get( &world->shapes, shapeIdA );
+			b2Shape* shapeB = b2ShapeArray_Get( &world->shapes, shapeIdB );
+
+			b2CreateContact( world, shapeA, shapeB );
+
+			if ( pair->heap )
+			{
+				b2MovePair* temp = pair;
+				pair = pair->next;
+				b2Free( temp, sizeof( b2MovePair ) );
+			}
+			else
+			{
+				pair = pair->next;
+			}
+		}
+
+		// if (s_file != NULL)
+		//{
+		//	fprintf(s_file, "\n");
+		// }
+	}
+
+	// if (s_file != NULL)
+	//{
+	//	fprintf(s_file, "count = %d\n\n", pairCount);
+	// }
+
+	// Reset move buffer
+	b2IntArray_Clear( &bp->moveArray );
+	b2ClearSet( &bp->moveSet );
+
+	b2FreeArenaItem( alloc, bp->movePairs );
+	bp->movePairs = NULL;
+	b2FreeArenaItem( alloc, bp->moveResults );
+	bp->moveResults = NULL;
+
+	b2ValidateSolverSets( world );
+
+	b2TracyCZoneEnd( create_contacts );
+
+	b2TracyCZoneEnd( update_pairs );
+}
+
+bool b2BroadPhase_TestOverlap( const b2BroadPhase* bp, int proxyKeyA, int proxyKeyB )
+{
+	int typeIndexA = B2_PROXY_TYPE( proxyKeyA );
+	int proxyIdA = B2_PROXY_ID( proxyKeyA );
+	int typeIndexB = B2_PROXY_TYPE( proxyKeyB );
+	int proxyIdB = B2_PROXY_ID( proxyKeyB );
+
+	b2AABB aabbA = b2DynamicTree_GetAABB( bp->trees + typeIndexA, proxyIdA );
+	b2AABB aabbB = b2DynamicTree_GetAABB( bp->trees + typeIndexB, proxyIdB );
+	return b2AABB_Overlaps( aabbA, aabbB );
+}
+
+void b2BroadPhase_RebuildTrees( b2BroadPhase* bp )
+{
+	b2DynamicTree_Rebuild( bp->trees + b2_dynamicBody, false );
+	b2DynamicTree_Rebuild( bp->trees + b2_kinematicBody, false );
+}
+
+int b2BroadPhase_GetShapeIndex( b2BroadPhase* bp, int proxyKey )
+{
+	int typeIndex = B2_PROXY_TYPE( proxyKey );
+	int proxyId = B2_PROXY_ID( proxyKey );
+
+	return (int)b2DynamicTree_GetUserData( bp->trees + typeIndex, proxyId );
+}
+
+void b2ValidateBroadphase( const b2BroadPhase* bp )
+{
+	b2DynamicTree_Validate( bp->trees + b2_dynamicBody );
+	b2DynamicTree_Validate( bp->trees + b2_kinematicBody );
+
+	// TODO_ERIN validate every shape AABB is contained in tree AABB
+}
+
+void b2ValidateNoEnlarged( const b2BroadPhase* bp )
+{
+#if B2_VALIDATE == 1
+	for ( int j = 0; j < b2_bodyTypeCount; ++j )
+	{
+		const b2DynamicTree* tree = bp->trees + j;
+		b2DynamicTree_ValidateNoEnlarged( tree );
+	}
+#else
+	B2_UNUSED( bp );
+#endif
+}

src/vendor/box2d/broad_phase.h

@@ -0,0 +1,83 @@
+// SPDX-FileCopyrightText: 2023 Erin Catto
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include "array.h"
+#include "table.h"
+
+#include "box2d/collision.h"
+#include "box2d/types.h"
+
+typedef struct b2Shape b2Shape;
+typedef struct b2MovePair b2MovePair;
+typedef struct b2MoveResult b2MoveResult;
+typedef struct b2ArenaAllocator b2ArenaAllocator;
+typedef struct b2World b2World;
+
+// Store the proxy type in the lower 2 bits of the proxy key. This leaves 30 bits for the id.
+#define B2_PROXY_TYPE( KEY ) ( (b2BodyType)( ( KEY ) & 3 ) )
+#define B2_PROXY_ID( KEY ) ( ( KEY ) >> 2 )
+#define B2_PROXY_KEY( ID, TYPE ) ( ( ( ID ) << 2 ) | ( TYPE ) )
+
+/// The broad-phase is used for computing pairs and performing volume queries and ray casts.
+/// This broad-phase does not persist pairs. Instead, this reports potentially new pairs.
+/// It is up to the client to consume the new pairs and to track subsequent overlap.
+typedef struct b2BroadPhase
+{
+	b2DynamicTree trees[b2_bodyTypeCount];
+	int proxyCount;
+
+	// The move set and array are used to track shapes that have moved significantly
+	// and need a pair query for new contacts. The array has a deterministic order.
+	// todo perhaps just a move set?
+	// todo implement a 32bit hash set for faster lookup
+	// todo moveSet can grow quite large on the first time step and remain large
+	b2HashSet moveSet;
+	b2IntArray moveArray;
+
+	// These are the results from the pair query and are used to create new contacts
+	// in deterministic order.
+	// todo these could be in the step context
+	b2MoveResult* moveResults;
+	b2MovePair* movePairs;
+	int movePairCapacity;
+	b2AtomicInt movePairIndex;
+
+	// Tracks shape pairs that have a b2Contact
+	// todo pairSet can grow quite large on the first time step and remain large
+	b2HashSet pairSet;
+
+} b2BroadPhase;
+
+void b2CreateBroadPhase( b2BroadPhase* bp );
+void b2DestroyBroadPhase( b2BroadPhase* bp );
+
+int b2BroadPhase_CreateProxy( b2BroadPhase* bp, b2BodyType proxyType, b2AABB aabb, uint64_t categoryBits, int shapeIndex,
+							  bool forcePairCreation );
+void b2BroadPhase_DestroyProxy( b2BroadPhase* bp, int proxyKey );
+
+void b2BroadPhase_MoveProxy( b2BroadPhase* bp, int proxyKey, b2AABB aabb );
+void b2BroadPhase_EnlargeProxy( b2BroadPhase* bp, int proxyKey, b2AABB aabb );
+
+void b2BroadPhase_RebuildTrees( b2BroadPhase* bp );
+
+int b2BroadPhase_GetShapeIndex( b2BroadPhase* bp, int proxyKey );
+
+void b2UpdateBroadPhasePairs( b2World* world );
+bool b2BroadPhase_TestOverlap( const b2BroadPhase* bp, int proxyKeyA, int proxyKeyB );
+
+void b2ValidateBroadphase( const b2BroadPhase* bp );
+void b2ValidateNoEnlarged( const b2BroadPhase* bp );
+
+// This is what triggers new contact pairs to be created
+// Warning: this must be called in deterministic order
+static inline void b2BufferMove( b2BroadPhase* bp, int queryProxy )
+{
+	// Adding 1 because 0 is the sentinel
+	bool alreadyAdded = b2AddKey( &bp->moveSet, queryProxy + 1 );
+	if ( alreadyAdded == false )
+	{
+		b2IntArray_Push( &bp->moveArray, queryProxy );
+	}
+}

src/vendor/box2d/collision.h

@@ -0,0 +1,830 @@
+// SPDX-FileCopyrightText: 2023 Erin Catto
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include "base.h"
+#include "math_functions.h"
+
+#include <stdbool.h>
+
+typedef struct b2SimplexCache b2SimplexCache;
+typedef struct b2Hull b2Hull;
+
+/**
+ * @defgroup geometry Geometry
+ * @brief Geometry types and algorithms
+ *
+ * Definitions of circles, capsules, segments, and polygons. Various algorithms to compute hulls, mass properties, and so on.
+ * @{
+ */
+
+/// The maximum number of vertices on a convex polygon. Changing this affects performance even if you
+/// don't use more vertices.
+#define B2_MAX_POLYGON_VERTICES 8
+
+/// Low level ray cast input data
+typedef struct b2RayCastInput
+{
+	/// Start point of the ray cast
+	b2Vec2 origin;
+
+	/// Translation of the ray cast
+	b2Vec2 translation;
+
+	/// The maximum fraction of the translation to consider, typically 1
+	float maxFraction;
+} b2RayCastInput;
+
+/// A distance proxy is used by the GJK algorithm. It encapsulates any shape.
+/// You can provide between 1 and B2_MAX_POLYGON_VERTICES and a radius.
+typedef struct b2ShapeProxy
+{
+	/// The point cloud
+	b2Vec2 points[B2_MAX_POLYGON_VERTICES];
+
+	/// The number of points. Must be greater than 0.
+	int count;
+
+	/// The external radius of the point cloud. May be zero.
+	float radius;
+} b2ShapeProxy;
+
+/// Low level shape cast input in generic form. This allows casting an arbitrary point
+/// cloud wrap with a radius. For example, a circle is a single point with a non-zero radius.
+/// A capsule is two points with a non-zero radius. A box is four points with a zero radius.
+typedef struct b2ShapeCastInput
+{
+	/// A generic shape
+	b2ShapeProxy proxy;
+
+	/// The translation of the shape cast
+	b2Vec2 translation;
+
+	/// The maximum fraction of the translation to consider, typically 1
+	float maxFraction;
+
+	/// Allow shape cast to encroach when initially touching. This only works if the radius is greater than zero.
+	bool canEncroach;
+} b2ShapeCastInput;
+
+/// Low level ray cast or shape-cast output data
+typedef struct b2CastOutput
+{
+	/// The surface normal at the hit point
+	b2Vec2 normal;
+
+	/// The surface hit point
+	b2Vec2 point;
+
+	/// The fraction of the input translation at collision
+	float fraction;
+
+	/// The number of iterations used
+	int iterations;
+
+	/// Did the cast hit?
+	bool hit;
+} b2CastOutput;
+
+/// This holds the mass data computed for a shape.
+typedef struct b2MassData
+{
+	/// The mass of the shape, usually in kilograms.
+	float mass;
+
+	/// The position of the shape's centroid relative to the shape's origin.
+	b2Vec2 center;
+
+	/// The rotational inertia of the shape about the local origin.
+	float rotationalInertia;
+} b2MassData;
+
+/// A solid circle
+typedef struct b2Circle
+{
+	/// The local center
+	b2Vec2 center;
+
+	/// The radius
+	float radius;
+} b2Circle;
+
+/// A solid capsule can be viewed as two semicircles connected
+/// by a rectangle.
+typedef struct b2Capsule
+{
+	/// Local center of the first semicircle
+	b2Vec2 center1;
+
+	/// Local center of the second semicircle
+	b2Vec2 center2;
+
+	/// The radius of the semicircles
+	float radius;
+} b2Capsule;
+
+/// A solid convex polygon. It is assumed that the interior of the polygon is to
+/// the left of each edge.
+/// Polygons have a maximum number of vertices equal to B2_MAX_POLYGON_VERTICES.
+/// In most cases you should not need many vertices for a convex polygon.
+/// @warning DO NOT fill this out manually, instead use a helper function like
+/// b2MakePolygon or b2MakeBox.
+typedef struct b2Polygon
+{
+	/// The polygon vertices
+	b2Vec2 vertices[B2_MAX_POLYGON_VERTICES];
+
+	/// The outward normal vectors of the polygon sides
+	b2Vec2 normals[B2_MAX_POLYGON_VERTICES];
+
+	/// The centroid of the polygon
+	b2Vec2 centroid;
+
+	/// The external radius for rounded polygons
+	float radius;
+
+	/// The number of polygon vertices
+	int count;
+} b2Polygon;
+
+/// A line segment with two-sided collision.
+typedef struct b2Segment
+{
+	/// The first point
+	b2Vec2 point1;
+
+	/// The second point
+	b2Vec2 point2;
+} b2Segment;
+
+/// A line segment with one-sided collision. Only collides on the right side.
+/// Several of these are generated for a chain shape.
+/// ghost1 -> point1 -> point2 -> ghost2
+typedef struct b2ChainSegment
+{
+	/// The tail ghost vertex
+	b2Vec2 ghost1;
+
+	/// The line segment
+	b2Segment segment;
+
+	/// The head ghost vertex
+	b2Vec2 ghost2;
+
+	/// The owning chain shape index (internal usage only)
+	int chainId;
+} b2ChainSegment;
+
+/// Validate ray cast input data (NaN, etc)
+B2_API bool b2IsValidRay( const b2RayCastInput* input );
+
+/// Make a convex polygon from a convex hull. This will assert if the hull is not valid.
+/// @warning Do not manually fill in the hull data, it must come directly from b2ComputeHull
+B2_API b2Polygon b2MakePolygon( const b2Hull* hull, float radius );
+
+/// Make an offset convex polygon from a convex hull. This will assert if the hull is not valid.
+/// @warning Do not manually fill in the hull data, it must come directly from b2ComputeHull
+B2_API b2Polygon b2MakeOffsetPolygon( const b2Hull* hull, b2Vec2 position, b2Rot rotation );
+
+/// Make an offset convex polygon from a convex hull. This will assert if the hull is not valid.
+/// @warning Do not manually fill in the hull data, it must come directly from b2ComputeHull
+B2_API b2Polygon b2MakeOffsetRoundedPolygon( const b2Hull* hull, b2Vec2 position, b2Rot rotation, float radius );
+
+/// Make a square polygon, bypassing the need for a convex hull.
+/// @param halfWidth the half-width
+B2_API b2Polygon b2MakeSquare( float halfWidth );
+
+/// Make a box (rectangle) polygon, bypassing the need for a convex hull.
+/// @param halfWidth the half-width (x-axis)
+/// @param halfHeight the half-height (y-axis)
+B2_API b2Polygon b2MakeBox( float halfWidth, float halfHeight );
+
+/// Make a rounded box, bypassing the need for a convex hull.
+/// @param halfWidth the half-width (x-axis)
+/// @param halfHeight the half-height (y-axis)
+/// @param radius the radius of the rounded extension
+B2_API b2Polygon b2MakeRoundedBox( float halfWidth, float halfHeight, float radius );
+
+/// Make an offset box, bypassing the need for a convex hull.
+/// @param halfWidth the half-width (x-axis)
+/// @param halfHeight the half-height (y-axis)
+/// @param center the local center of the box
+/// @param rotation the local rotation of the box
+B2_API b2Polygon b2MakeOffsetBox( float halfWidth, float halfHeight, b2Vec2 center, b2Rot rotation );
+
+/// Make an offset rounded box, bypassing the need for a convex hull.
+/// @param halfWidth the half-width (x-axis)
+/// @param halfHeight the half-height (y-axis)
+/// @param center the local center of the box
+/// @param rotation the local rotation of the box
+/// @param radius the radius of the rounded extension
+B2_API b2Polygon b2MakeOffsetRoundedBox( float halfWidth, float halfHeight, b2Vec2 center, b2Rot rotation, float radius );
+
+/// 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
+B2_API b2MassData b2ComputeCircleMass( const b2Circle* shape, float density );
+
+/// Compute mass properties of a capsule
+B2_API b2MassData b2ComputeCapsuleMass( const b2Capsule* shape, float density );
+
+/// Compute mass properties of a polygon
+B2_API b2MassData b2ComputePolygonMass( const b2Polygon* shape, float density );
+
+/// Compute the bounding box of a transformed circle
+B2_API b2AABB b2ComputeCircleAABB( const b2Circle* shape, b2Transform transform );
+
+/// Compute the bounding box of a transformed capsule
+B2_API b2AABB b2ComputeCapsuleAABB( const b2Capsule* shape, b2Transform transform );
+
+/// Compute the bounding box of a transformed polygon
+B2_API b2AABB b2ComputePolygonAABB( const b2Polygon* shape, b2Transform transform );
+
+/// Compute the bounding box of a transformed line segment
+B2_API b2AABB b2ComputeSegmentAABB( const b2Segment* shape, b2Transform transform );
+
+/// Test a point for overlap with a circle in local space
+B2_API bool b2PointInCircle( b2Vec2 point, const b2Circle* shape );
+
+/// Test a point for overlap with a capsule in local space
+B2_API bool b2PointInCapsule( b2Vec2 point, const b2Capsule* shape );
+
+/// Test a point for overlap with a convex polygon in local space
+B2_API bool b2PointInPolygon( b2Vec2 point, const b2Polygon* shape );
+
+/// Ray cast versus circle shape in local space. Initial overlap is treated as a miss.
+B2_API b2CastOutput b2RayCastCircle( const b2RayCastInput* input, const b2Circle* shape );
+
+/// Ray cast versus capsule shape in local space. Initial overlap is treated as a miss.
+B2_API b2CastOutput b2RayCastCapsule( const b2RayCastInput* input, const b2Capsule* shape );
+
+/// Ray cast versus segment shape in local space. Optionally treat the segment as one-sided with hits from
+/// the left side being treated as a miss.
+B2_API b2CastOutput b2RayCastSegment( const b2RayCastInput* input, const b2Segment* shape, bool oneSided );
+
+/// Ray cast versus polygon shape in local space. Initial overlap is treated as a miss.
+B2_API b2CastOutput b2RayCastPolygon( const b2RayCastInput* input, const b2Polygon* shape );
+
+/// Shape cast versus a circle. Initial overlap is treated as a miss.
+B2_API b2CastOutput b2ShapeCastCircle( const b2ShapeCastInput* input, const b2Circle* shape );
+
+/// Shape cast versus a capsule. Initial overlap is treated as a miss.
+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 b2CastOutput b2ShapeCastSegment( const b2ShapeCastInput* input, const b2Segment* shape );
+
+/// Shape cast versus a convex polygon. Initial overlap is treated as a miss.
+B2_API b2CastOutput b2ShapeCastPolygon( const b2ShapeCastInput* input, const b2Polygon* shape );
+
+/// A convex hull. Used to create convex polygons.
+/// @warning Do not modify these values directly, instead use b2ComputeHull()
+typedef struct b2Hull
+{
+	/// The final points of the hull
+	b2Vec2 points[B2_MAX_POLYGON_VERTICES];
+
+	/// The number of points
+	int count;
+} b2Hull;
+
+/// Compute the convex hull of a set of points. Returns an empty hull if it fails.
+/// Some failure cases:
+/// - all points very close together
+/// - all points on a line
+/// - less than 3 points
+/// - more than B2_MAX_POLYGON_VERTICES points
+/// This welds close points and removes collinear points.
+/// @warning Do not modify a hull once it has been computed
+B2_API b2Hull b2ComputeHull( const b2Vec2* points, int count );
+
+/// This determines if a hull is valid. Checks for:
+/// - convexity
+/// - collinear points
+/// This is expensive and should not be called at runtime.
+B2_API bool b2ValidateHull( const b2Hull* hull );
+
+/**@}*/
+
+/**
+ * @defgroup distance Distance
+ * Functions for computing the distance between shapes.
+ *
+ * These are advanced functions you can use to perform distance calculations. There
+ * are functions for computing the closest points between shapes, doing linear shape casts,
+ * and doing rotational shape casts. The latter is called time of impact (TOI).
+ * @{
+ */
+
+/// Result of computing the distance between two line segments
+typedef struct b2SegmentDistanceResult
+{
+	/// The closest point on the first segment
+	b2Vec2 closest1;
+
+	/// The closest point on the second segment
+	b2Vec2 closest2;
+
+	/// The barycentric coordinate on the first segment
+	float fraction1;
+
+	/// The barycentric coordinate on the second segment
+	float fraction2;
+
+	/// The squared distance between the closest points
+	float distanceSquared;
+} b2SegmentDistanceResult;
+
+/// Compute the distance between two line segments, clamping at the end points if needed.
+B2_API b2SegmentDistanceResult b2SegmentDistance( b2Vec2 p1, b2Vec2 q1, b2Vec2 p2, b2Vec2 q2 );
+
+/// Used to warm start the GJK simplex. If you call this function multiple times with nearby
+/// transforms this might improve performance. Otherwise you can zero initialize this.
+/// The distance cache must be initialized to zero on the first call.
+/// Users should generally just zero initialize this structure for each call.
+typedef struct b2SimplexCache
+{
+	/// The number of stored simplex points
+	uint16_t count;
+
+	/// The cached simplex indices on shape A
+	uint8_t indexA[3];
+
+	/// The cached simplex indices on shape B
+	uint8_t indexB[3];
+} b2SimplexCache;
+
+static const b2SimplexCache b2_emptySimplexCache = B2_ZERO_INIT;
+
+/// Input for b2ShapeDistance
+typedef struct b2DistanceInput
+{
+	/// The proxy for shape A
+	b2ShapeProxy proxyA;
+
+	/// The proxy for shape B
+	b2ShapeProxy proxyB;
+
+	/// The world transform for shape A
+	b2Transform transformA;
+
+	/// The world transform for shape B
+	b2Transform transformB;
+
+	/// Should the proxy radius be considered?
+	bool useRadii;
+} b2DistanceInput;
+
+/// Output for b2ShapeDistance
+typedef struct b2DistanceOutput
+{
+	b2Vec2 pointA;	  ///< Closest point on shapeA
+	b2Vec2 pointB;	  ///< Closest point on shapeB
+	b2Vec2 normal;	  ///< Normal vector that points from A to B
+	float distance;	  ///< The final distance, zero if overlapped
+	int iterations;	  ///< Number of GJK iterations used
+	int simplexCount; ///< The number of simplexes stored in the simplex array
+} b2DistanceOutput;
+
+/// Simplex vertex for debugging the GJK algorithm
+typedef struct b2SimplexVertex
+{
+	b2Vec2 wA;	///< support point in proxyA
+	b2Vec2 wB;	///< support point in proxyB
+	b2Vec2 w;	///< wB - wA
+	float a;	///< barycentric coordinate for closest point
+	int indexA; ///< wA index
+	int indexB; ///< wB index
+} b2SimplexVertex;
+
+/// Simplex from the GJK algorithm
+typedef struct b2Simplex
+{
+	b2SimplexVertex v1, v2, v3; ///< vertices
+	int count;					///< number of valid vertices
+} b2Simplex;
+
+/// Compute the closest points between two shapes represented as point clouds.
+/// b2SimplexCache cache is input/output. On the first call set b2SimplexCache.count to zero.
+/// The underlying GJK algorithm may be debugged by passing in debug simplexes and capacity. You may pass in NULL and 0 for these.
+B2_API b2DistanceOutput b2ShapeDistance( const b2DistanceInput* input, b2SimplexCache* cache, b2Simplex* simplexes,
+										 int simplexCapacity );
+
+/// Input parameters for b2ShapeCast
+typedef struct b2ShapeCastPairInput
+{
+	b2ShapeProxy proxyA;	///< The proxy for shape A
+	b2ShapeProxy proxyB;	///< The proxy for shape B
+	b2Transform transformA; ///< The world transform for shape A
+	b2Transform transformB; ///< The world transform for shape B
+	b2Vec2 translationB;	///< The translation of shape B
+	float maxFraction;		///< The fraction of the translation to consider, typically 1
+	bool canEncroach;		///< Allows shapes with a radius to move slightly closer if already touching
+} b2ShapeCastPairInput;
+
+/// Perform a linear shape cast of shape B moving and shape A fixed. Determines the hit point, normal, and translation fraction.
+/// You may optionally supply an array to hold debug data.
+B2_API b2CastOutput b2ShapeCast( const b2ShapeCastPairInput* input);
+
+/// Make a proxy for use in overlap, shape cast, and related functions. This is a deep copy of the points.
+B2_API b2ShapeProxy b2MakeProxy( const b2Vec2* points, int count, float radius );
+
+/// Make a proxy with a transform. This is a deep copy of the points.
+B2_API b2ShapeProxy b2MakeOffsetProxy( const b2Vec2* points, int count, float radius, b2Vec2 position, b2Rot rotation );
+
+/// This describes the motion of a body/shape for TOI computation. Shapes are defined with respect to the body origin,
+/// which may not coincide with the center of mass. However, to support dynamics we must interpolate the center of mass
+/// position.
+typedef struct b2Sweep
+{
+	b2Vec2 localCenter; ///< Local center of mass position
+	b2Vec2 c1;			///< Starting center of mass world position
+	b2Vec2 c2;			///< Ending center of mass world position
+	b2Rot q1;			///< Starting world rotation
+	b2Rot q2;			///< Ending world rotation
+} b2Sweep;
+
+/// Evaluate the transform sweep at a specific time.
+B2_API b2Transform b2GetSweepTransform( const b2Sweep* sweep, float time );
+
+/// Input parameters for b2TimeOfImpact
+typedef struct b2TOIInput
+{
+	b2ShapeProxy proxyA; ///< The proxy for shape A
+	b2ShapeProxy proxyB; ///< The proxy for shape B
+	b2Sweep sweepA;		 ///< The movement of shape A
+	b2Sweep sweepB;		 ///< The movement of shape B
+	float maxFraction;	 ///< Defines the sweep interval [0, maxFraction]
+} b2TOIInput;
+
+/// Describes the TOI output
+typedef enum b2TOIState
+{
+	b2_toiStateUnknown,
+	b2_toiStateFailed,
+	b2_toiStateOverlapped,
+	b2_toiStateHit,
+	b2_toiStateSeparated
+} b2TOIState;
+
+/// Output parameters for b2TimeOfImpact.
+typedef struct b2TOIOutput
+{
+	b2TOIState state; ///< The type of result
+	float fraction;	  ///< The sweep time of the collision
+} b2TOIOutput;
+
+/// Compute the upper bound on time before two shapes penetrate. Time is represented as
+/// a fraction between [0,tMax]. This uses a swept separating axis and may miss some intermediate,
+/// non-tunneling collisions. If you change the time interval, you should call this function
+/// again.
+B2_API b2TOIOutput b2TimeOfImpact( const b2TOIInput* input );
+
+/**@}*/
+
+/**
+ * @defgroup collision Collision
+ * @brief Functions for colliding pairs of shapes
+ * @{
+ */
+
+/// A manifold point is a contact point belonging to a contact manifold.
+/// It holds details related to the geometry and dynamics of the contact points.
+/// Box2D uses speculative collision so some contact points may be separated.
+/// You may use the totalNormalImpulse to determine if there was an interaction during
+/// the time step.
+typedef struct b2ManifoldPoint
+{
+	/// Location of the contact point in world space. Subject to precision loss at large coordinates.
+	/// @note Should only be used for debugging.
+	b2Vec2 point;
+
+	/// Location of the contact point relative to shapeA's origin in world space
+	/// @note When used internally to the Box2D solver, this is relative to the body center of mass.
+	b2Vec2 anchorA;
+
+	/// Location of the contact point relative to shapeB's origin in world space
+	/// @note When used internally to the Box2D solver, this is relative to the body center of mass.
+	b2Vec2 anchorB;
+
+	/// The separation of the contact point, negative if penetrating
+	float separation;
+
+	/// The impulse along the manifold normal vector.
+	float normalImpulse;
+
+	/// The friction impulse
+	float tangentImpulse;
+
+	/// The total normal impulse applied across sub-stepping and restitution. This is important
+	/// to identify speculative contact points that had an interaction in the time step.
+	float totalNormalImpulse;
+
+	/// Relative normal velocity pre-solve. Used for hit events. If the normal impulse is
+	/// zero then there was no hit. Negative means shapes are approaching.
+	float normalVelocity;
+
+	/// Uniquely identifies a contact point between two shapes
+	uint16_t id;
+
+	/// Did this contact point exist the previous step?
+	bool persisted;
+} b2ManifoldPoint;
+
+/// A contact manifold describes the contact points between colliding shapes.
+/// @note Box2D uses speculative collision so some contact points may be separated.
+typedef struct b2Manifold
+{
+	/// The unit normal vector in world space, points from shape A to bodyB
+	b2Vec2 normal;
+
+	/// Angular impulse applied for rolling resistance. N * m * s = kg * m^2 / s
+	float rollingImpulse;
+
+	/// The manifold points, up to two are possible in 2D
+	b2ManifoldPoint points[2];
+
+	/// The number of contacts points, will be 0, 1, or 2
+	int pointCount;
+
+} b2Manifold;
+
+/// Compute the contact manifold between two circles
+B2_API b2Manifold b2CollideCircles( const b2Circle* circleA, b2Transform xfA, const b2Circle* circleB, b2Transform xfB );
+
+/// Compute the contact manifold between a capsule and circle
+B2_API b2Manifold b2CollideCapsuleAndCircle( const b2Capsule* capsuleA, b2Transform xfA, const b2Circle* circleB,
+											 b2Transform xfB );
+
+/// Compute the contact manifold between an segment and a circle
+B2_API b2Manifold b2CollideSegmentAndCircle( const b2Segment* segmentA, b2Transform xfA, const b2Circle* circleB,
+											 b2Transform xfB );
+
+/// Compute the contact manifold between a polygon and a circle
+B2_API b2Manifold b2CollidePolygonAndCircle( const b2Polygon* polygonA, b2Transform xfA, const b2Circle* circleB,
+											 b2Transform xfB );
+
+/// Compute the contact manifold between a capsule and circle
+B2_API b2Manifold b2CollideCapsules( const b2Capsule* capsuleA, b2Transform xfA, const b2Capsule* capsuleB, b2Transform xfB );
+
+/// Compute the contact manifold between an segment and a capsule
+B2_API b2Manifold b2CollideSegmentAndCapsule( const b2Segment* segmentA, b2Transform xfA, const b2Capsule* capsuleB,
+											  b2Transform xfB );
+
+/// Compute the contact manifold between a polygon and capsule
+B2_API b2Manifold b2CollidePolygonAndCapsule( const b2Polygon* polygonA, b2Transform xfA, const b2Capsule* capsuleB,
+											  b2Transform xfB );
+
+/// Compute the contact manifold between two polygons
+B2_API b2Manifold b2CollidePolygons( const b2Polygon* polygonA, b2Transform xfA, const b2Polygon* polygonB, b2Transform xfB );
+
+/// Compute the contact manifold between an segment and a polygon
+B2_API b2Manifold b2CollideSegmentAndPolygon( const b2Segment* segmentA, b2Transform xfA, const b2Polygon* polygonB,
+											  b2Transform xfB );
+
+/// Compute the contact manifold between a chain segment and a circle
+B2_API b2Manifold b2CollideChainSegmentAndCircle( const b2ChainSegment* segmentA, b2Transform xfA, const b2Circle* circleB,
+												  b2Transform xfB );
+
+/// Compute the contact manifold between a chain segment and a capsule
+B2_API b2Manifold b2CollideChainSegmentAndCapsule( const b2ChainSegment* segmentA, b2Transform xfA, const b2Capsule* capsuleB,
+												   b2Transform xfB, b2SimplexCache* cache );
+
+/// Compute the contact manifold between a chain segment and a rounded polygon
+B2_API b2Manifold b2CollideChainSegmentAndPolygon( const b2ChainSegment* segmentA, b2Transform xfA, const b2Polygon* polygonB,
+												   b2Transform xfB, b2SimplexCache* cache );
+
+/**@}*/
+
+/**
+ * @defgroup tree Dynamic Tree
+ * The dynamic tree is a binary AABB tree to organize and query large numbers of geometric objects
+ *
+ * Box2D uses the dynamic tree internally to sort collision shapes into a binary bounding volume hierarchy.
+ * This data structure may have uses in games for organizing other geometry data and may be used independently
+ * of Box2D rigid body simulation.
+ *
+ * A dynamic AABB tree broad-phase, inspired by Nathanael Presson's btDbvt.
+ * A dynamic tree arranges data in a binary tree to accelerate
+ * queries such as AABB queries and ray casts. Leaf nodes are proxies
+ * with an AABB. These are used to hold a user collision object.
+ * Nodes are pooled and relocatable, so I use node indices rather than pointers.
+ * The dynamic tree is made available for advanced users that would like to use it to organize
+ * spatial game data besides rigid bodies.
+ * @{
+ */
+
+/// The dynamic tree structure. This should be considered private data.
+/// It is placed here for performance reasons.
+typedef struct b2DynamicTree
+{
+	/// The tree nodes
+	struct b2TreeNode* nodes;
+
+	/// The root index
+	int root;
+
+	/// The number of nodes
+	int nodeCount;
+
+	/// The allocated node space
+	int nodeCapacity;
+
+	/// Node free list
+	int freeList;
+
+	/// Number of proxies created
+	int proxyCount;
+
+	/// Leaf indices for rebuild
+	int* leafIndices;
+
+	/// Leaf bounding boxes for rebuild
+	b2AABB* leafBoxes;
+
+	/// Leaf bounding box centers for rebuild
+	b2Vec2* leafCenters;
+
+	/// Bins for sorting during rebuild
+	int* binIndices;
+
+	/// Allocated space for rebuilding
+	int rebuildCapacity;
+} b2DynamicTree;
+
+/// These are performance results returned by dynamic tree queries.
+typedef struct b2TreeStats
+{
+	/// Number of internal nodes visited during the query
+	int nodeVisits;
+
+	/// Number of leaf nodes visited during the query
+	int leafVisits;
+} b2TreeStats;
+
+/// Constructing the tree initializes the node pool.
+B2_API b2DynamicTree b2DynamicTree_Create( void );
+
+/// Destroy the tree, freeing the node pool.
+B2_API void b2DynamicTree_Destroy( b2DynamicTree* tree );
+
+/// Create a proxy. Provide an AABB and a userData value.
+B2_API int b2DynamicTree_CreateProxy( b2DynamicTree* tree, b2AABB aabb, uint64_t categoryBits, uint64_t userData );
+
+/// Destroy a proxy. This asserts if the id is invalid.
+B2_API void b2DynamicTree_DestroyProxy( b2DynamicTree* tree, int proxyId );
+
+/// Move a proxy to a new AABB by removing and reinserting into the tree.
+B2_API void b2DynamicTree_MoveProxy( b2DynamicTree* tree, int proxyId, b2AABB aabb );
+
+/// Enlarge a proxy and enlarge ancestors as necessary.
+B2_API void b2DynamicTree_EnlargeProxy( b2DynamicTree* tree, int proxyId, b2AABB aabb );
+
+/// Modify the category bits on a proxy. This is an expensive operation.
+B2_API void b2DynamicTree_SetCategoryBits( b2DynamicTree* tree, int proxyId, uint64_t categoryBits );
+
+/// Get the category bits on a proxy.
+B2_API uint64_t b2DynamicTree_GetCategoryBits( b2DynamicTree* tree, int proxyId );
+
+/// This function receives proxies found in the AABB query.
+/// @return true if the query should continue
+typedef bool b2TreeQueryCallbackFcn( int proxyId, uint64_t userData, void* context );
+
+/// Query an AABB for overlapping proxies. The callback class is called for each proxy that overlaps the supplied AABB.
+///	@return performance data
+B2_API b2TreeStats b2DynamicTree_Query( const b2DynamicTree* tree, b2AABB aabb, uint64_t maskBits,
+										b2TreeQueryCallbackFcn* callback, void* context );
+
+/// This function receives clipped ray cast input for a proxy. The function
+/// returns the new ray fraction.
+/// - return a value of 0 to terminate the ray cast
+/// - return a value less than input->maxFraction to clip the ray
+/// - return a value of input->maxFraction to continue the ray cast without clipping
+typedef float b2TreeRayCastCallbackFcn( const b2RayCastInput* input, int proxyId, uint64_t userData, void* context );
+
+/// Ray cast against the proxies in the tree. This relies on the callback
+/// to perform a exact ray cast in the case were the proxy contains a shape.
+/// The callback also performs the any collision filtering. This has performance
+/// roughly equal to k * log(n), where k is the number of collisions and n is the
+/// number of proxies in the tree.
+/// Bit-wise filtering using mask bits can greatly improve performance in some scenarios.
+///	However, this filtering may be approximate, so the user should still apply filtering to results.
+/// @param tree the dynamic tree to ray cast
+/// @param input the ray cast input data. The ray extends from p1 to p1 + maxFraction * (p2 - p1)
+/// @param maskBits mask bit hint: `bool accept = (maskBits & node->categoryBits) != 0;`
+/// @param callback a callback class that is called for each proxy that is hit by the ray
+/// @param context user context that is passed to the callback
+///	@return performance data
+B2_API b2TreeStats b2DynamicTree_RayCast( const b2DynamicTree* tree, const b2RayCastInput* input, uint64_t maskBits,
+										  b2TreeRayCastCallbackFcn* callback, void* context );
+
+/// This function receives clipped ray cast input for a proxy. The function
+/// returns the new ray fraction.
+/// - return a value of 0 to terminate the ray cast
+/// - return a value less than input->maxFraction to clip the ray
+/// - return a value of input->maxFraction to continue the ray cast without clipping
+typedef float b2TreeShapeCastCallbackFcn( const b2ShapeCastInput* input, int proxyId, uint64_t userData, void* context );
+
+/// Ray cast against the proxies in the tree. This relies on the callback
+/// to perform a exact ray cast in the case were the proxy contains a shape.
+/// The callback also performs the any collision filtering. This has performance
+/// roughly equal to k * log(n), where k is the number of collisions and n is the
+/// number of proxies in the tree.
+/// @param tree the dynamic tree to ray cast
+/// @param input the ray cast input data. The ray extends from p1 to p1 + maxFraction * (p2 - p1).
+/// @param maskBits filter bits: `bool accept = (maskBits & node->categoryBits) != 0;`
+/// @param callback a callback class that is called for each proxy that is hit by the shape
+/// @param context user context that is passed to the callback
+///	@return performance data
+B2_API b2TreeStats b2DynamicTree_ShapeCast( const b2DynamicTree* tree, const b2ShapeCastInput* input, uint64_t maskBits,
+											b2TreeShapeCastCallbackFcn* callback, void* context );
+
+/// Get the height of the binary tree.
+B2_API int b2DynamicTree_GetHeight( const b2DynamicTree* tree );
+
+/// Get the ratio of the sum of the node areas to the root area.
+B2_API float b2DynamicTree_GetAreaRatio( const b2DynamicTree* tree );
+
+/// Get the bounding box that contains the entire tree
+B2_API b2AABB b2DynamicTree_GetRootBounds( const b2DynamicTree* tree );
+
+/// Get the number of proxies created
+B2_API int b2DynamicTree_GetProxyCount( const b2DynamicTree* tree );
+
+/// Rebuild the tree while retaining subtrees that haven't changed. Returns the number of boxes sorted.
+B2_API int b2DynamicTree_Rebuild( b2DynamicTree* tree, bool fullBuild );
+
+/// Get the number of bytes used by this tree
+B2_API int b2DynamicTree_GetByteCount( const b2DynamicTree* tree );
+
+/// Get proxy user data
+B2_API uint64_t b2DynamicTree_GetUserData( const b2DynamicTree* tree, int proxyId );
+
+/// Get the AABB of a proxy
+B2_API b2AABB b2DynamicTree_GetAABB( const b2DynamicTree* tree, int proxyId );
+
+/// Validate this tree. For testing.
+B2_API void b2DynamicTree_Validate( const b2DynamicTree* tree );
+
+/// Validate this tree has no enlarged AABBs. For testing.
+B2_API void b2DynamicTree_ValidateNoEnlarged( const b2DynamicTree* tree );
+
+/**@}*/
+
+/**
+ * @defgroup character
+ * Character movement solver
+ * @{
+ */
+
+/// These are the collision planes returned from b2World_CollideMover
+typedef struct b2PlaneResult
+{
+	/// The collision plane between the mover and a convex shape
+	b2Plane plane;
+
+	/// Did the collision register a hit? If not this plane should be ignored.
+	bool hit;
+} b2PlaneResult;
+
+/// These are collision planes that can be fed to b2SolvePlanes. Normally
+/// this is assembled by the user from plane results in b2PlaneResult
+typedef struct b2CollisionPlane
+{
+	/// The collision plane between the mover and some shape
+	b2Plane plane;
+
+	/// Setting this to FLT_MAX makes the plane as rigid as possible. Lower values can
+	/// make the plane collision soft. Usually in meters.
+	float pushLimit;
+
+	/// The push on the mover determined by b2SolvePlanes. Usually in meters.
+	float push;
+
+	/// Indicates if b2ClipVector should clip against this plane. Should be false for soft collision.
+	bool clipVelocity;
+} b2CollisionPlane;
+
+/// Result returned by b2SolvePlanes
+typedef struct b2PlaneSolverResult
+{
+	/// The final position of the mover
+	b2Vec2 position;
+
+	/// The number of iterations used by the plane solver. For diagnostics.
+	int iterationCount;
+} b2PlaneSolverResult;
+
+/// Solves the position of a mover that satisfies the given collision planes.
+/// @param position this must be the position used to generate the collision planes
+/// @param planes the collision planes
+/// @param count the number of collision planes
+B2_API b2PlaneSolverResult b2SolvePlanes( b2Vec2 position, b2CollisionPlane* planes, int count );
+
+/// Clips the velocity against the given collision planes. Planes with clipVelocity set to
+/// true are skipped.
+B2_API b2Vec2 b2ClipVector( b2Vec2 vector, const b2CollisionPlane* planes, int count );
+
+/**@}*/

src/vendor/box2d/constants.h

@@ -0,0 +1,54 @@
+// SPDX-FileCopyrightText: 2023 Erin Catto
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+extern float b2_lengthUnitsPerMeter;
+
+// Used to detect bad values. Positions greater than about 16km will have precision
+// problems, so 100km as a limit should be fine in all cases.
+#define B2_HUGE ( 100000.0f * b2_lengthUnitsPerMeter )
+
+// Maximum parallel workers. Used to size some static arrays.
+#define B2_MAX_WORKERS 64
+
+// Maximum number of colors in the constraint graph. Constraints that cannot
+// find a color are added to the overflow set which are solved single-threaded.
+#define B2_GRAPH_COLOR_COUNT 12
+
+// A small length used as a collision and constraint tolerance. Usually it is
+// chosen to be numerically significant, but visually insignificant. In meters.
+// Normally this is 0.5cm.
+// @warning modifying this can have a significant impact on stability
+#define B2_LINEAR_SLOP ( 0.005f * b2_lengthUnitsPerMeter )
+
+// Maximum number of simultaneous worlds that can be allocated
+#ifndef B2_MAX_WORLDS
+#define B2_MAX_WORLDS 128
+#endif
+
+// The maximum rotation of a body per time step. This limit is very large and is used
+// to prevent numerical problems. You shouldn't need to adjust this.
+// @warning increasing this to 0.5f * b2_pi or greater will break continuous collision.
+#define B2_MAX_ROTATION ( 0.25f * B2_PI )
+
+// Box2D uses limited speculative collision. This reduces jitter.
+// Normally this is 2cm.
+// @warning modifying this can have a significant impact on performance and stability
+#define B2_SPECULATIVE_DISTANCE ( 4.0f * B2_LINEAR_SLOP )
+
+// This is used to fatten AABBs in the dynamic tree. This allows proxies
+// to move by a small amount without triggering a tree adjustment. This is in meters.
+// Normally this is 5cm.
+// @warning modifying this can have a significant impact on performance
+#define B2_AABB_MARGIN ( 0.05f * b2_lengthUnitsPerMeter )
+
+// The time that a body must be still before it will go to sleep. In seconds.
+#define B2_TIME_TO_SLEEP 0.5f
+
+enum b2TreeNodeFlags
+{
+	b2_allocatedNode = 0x0001,
+	b2_enlargedNode = 0x0002,
+	b2_leafNode = 0x0004,
+};

src/vendor/box2d/constraint_graph.c

@@ -0,0 +1,322 @@
+// SPDX-FileCopyrightText: 2023 Erin Catto
+// SPDX-License-Identifier: MIT
+
+#include "constraint_graph.h"
+
+#include "array.h"
+#include "bitset.h"
+#include "body.h"
+#include "contact.h"
+#include "joint.h"
+#include "solver_set.h"
+#include "world.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
+
+_Static_assert( B2_GRAPH_COLOR_COUNT == 12, "graph color count assumed to be 12" );
+
+void b2CreateGraph( b2ConstraintGraph* graph, int bodyCapacity )
+{
+	_Static_assert( B2_GRAPH_COLOR_COUNT >= 2, "must have at least two constraint graph colors" );
+	_Static_assert( B2_OVERFLOW_INDEX == B2_GRAPH_COLOR_COUNT - 1, "bad over flow index" );
+
+	*graph = ( b2ConstraintGraph ){ 0 };
+
+	bodyCapacity = b2MaxInt( bodyCapacity, 8 );
+
+	// Initialize graph color bit set.
+	// No bitset for overflow color.
+	for ( int i = 0; i < B2_OVERFLOW_INDEX; ++i )
+	{
+		b2GraphColor* color = graph->colors + i;
+		color->bodySet = b2CreateBitSet( bodyCapacity );
+		b2SetBitCountAndClear( &color->bodySet, bodyCapacity );
+	}
+}
+
+void b2DestroyGraph( b2ConstraintGraph* graph )
+{
+	for ( int i = 0; i < B2_GRAPH_COLOR_COUNT; ++i )
+	{
+		b2GraphColor* color = graph->colors + i;
+
+		// The bit set should never be used on the overflow color
+		B2_ASSERT( i != B2_OVERFLOW_INDEX || color->bodySet.bits == NULL );
+
+		b2DestroyBitSet( &color->bodySet );
+
+		b2ContactSimArray_Destroy( &color->contactSims );
+		b2JointSimArray_Destroy( &color->jointSims );
+	}
+}
+
+// Contacts are always created as non-touching. They get cloned into the constraint
+// graph once they are found to be touching.
+// todo maybe kinematic bodies should not go into graph
+void b2AddContactToGraph( b2World* world, b2ContactSim* contactSim, b2Contact* contact )
+{
+	B2_ASSERT( contactSim->manifold.pointCount > 0 );
+	B2_ASSERT( contactSim->simFlags & b2_simTouchingFlag );
+	B2_ASSERT( contact->flags & b2_contactTouchingFlag );
+
+	b2ConstraintGraph* graph = &world->constraintGraph;
+	int colorIndex = B2_OVERFLOW_INDEX;
+
+	int bodyIdA = contact->edges[0].bodyId;
+	int bodyIdB = contact->edges[1].bodyId;
+	b2Body* bodyA = b2BodyArray_Get( &world->bodies, bodyIdA );
+	b2Body* bodyB = b2BodyArray_Get( &world->bodies, bodyIdB );
+	bool staticA = bodyA->setIndex == b2_staticSet;
+	bool staticB = bodyB->setIndex == b2_staticSet;
+	B2_ASSERT( staticA == false || staticB == false );
+
+#if B2_FORCE_OVERFLOW == 0
+	if ( staticA == false && staticB == false )
+	{
+		for ( int i = 0; i < B2_OVERFLOW_INDEX; ++i )
+		{
+			b2GraphColor* color = graph->colors + i;
+			if ( b2GetBit( &color->bodySet, bodyIdA ) || b2GetBit( &color->bodySet, bodyIdB ) )
+			{
+				continue;
+			}
+
+			b2SetBitGrow( &color->bodySet, bodyIdA );
+			b2SetBitGrow( &color->bodySet, bodyIdB );
+			colorIndex = i;
+			break;
+		}
+	}
+	else if ( staticA == false )
+	{
+		// No static contacts in color 0
+		for ( int i = 1; i < B2_OVERFLOW_INDEX; ++i )
+		{
+			b2GraphColor* color = graph->colors + i;
+			if ( b2GetBit( &color->bodySet, bodyIdA ) )
+			{
+				continue;
+			}
+
+			b2SetBitGrow( &color->bodySet, bodyIdA );
+			colorIndex = i;
+			break;
+		}
+	}
+	else if ( staticB == false )
+	{
+		// No static contacts in color 0
+		for ( int i = 1; i < B2_OVERFLOW_INDEX; ++i )
+		{
+			b2GraphColor* color = graph->colors + i;
+			if ( b2GetBit( &color->bodySet, bodyIdB ) )
+			{
+				continue;
+			}
+
+			b2SetBitGrow( &color->bodySet, bodyIdB );
+			colorIndex = i;
+			break;
+		}
+	}
+#endif
+
+	b2GraphColor* color = graph->colors + colorIndex;
+	contact->colorIndex = colorIndex;
+	contact->localIndex = color->contactSims.count;
+
+	b2ContactSim* newContact = b2ContactSimArray_Add( &color->contactSims );
+	memcpy( newContact, contactSim, sizeof( b2ContactSim ) );
+
+	// todo perhaps skip this if the contact is already awake
+
+	if ( staticA )
+	{
+		newContact->bodySimIndexA = B2_NULL_INDEX;
+		newContact->invMassA = 0.0f;
+		newContact->invIA = 0.0f;
+	}
+	else
+	{
+		B2_ASSERT( bodyA->setIndex == b2_awakeSet );
+		b2SolverSet* awakeSet = b2SolverSetArray_Get( &world->solverSets, b2_awakeSet );
+
+		int localIndex = bodyA->localIndex;
+		newContact->bodySimIndexA = localIndex;
+
+		b2BodySim* bodySimA = b2BodySimArray_Get( &awakeSet->bodySims, localIndex );
+		newContact->invMassA = bodySimA->invMass;
+		newContact->invIA = bodySimA->invInertia;
+	}
+
+	if ( staticB )
+	{
+		newContact->bodySimIndexB = B2_NULL_INDEX;
+		newContact->invMassB = 0.0f;
+		newContact->invIB = 0.0f;
+	}
+	else
+	{
+		B2_ASSERT( bodyB->setIndex == b2_awakeSet );
+		b2SolverSet* awakeSet = b2SolverSetArray_Get( &world->solverSets, b2_awakeSet );
+
+		int localIndex = bodyB->localIndex;
+		newContact->bodySimIndexB = localIndex;
+
+		b2BodySim* bodySimB = b2BodySimArray_Get( &awakeSet->bodySims, localIndex );
+		newContact->invMassB = bodySimB->invMass;
+		newContact->invIB = bodySimB->invInertia;
+	}
+}
+
+void b2RemoveContactFromGraph( b2World* world, int bodyIdA, int bodyIdB, int colorIndex, int localIndex )
+{
+	b2ConstraintGraph* graph = &world->constraintGraph;
+
+	B2_ASSERT( 0 <= colorIndex && colorIndex < B2_GRAPH_COLOR_COUNT );
+	b2GraphColor* color = graph->colors + colorIndex;
+
+	if ( colorIndex != B2_OVERFLOW_INDEX )
+	{
+		// might clear a bit for a static body, but this has no effect
+		b2ClearBit( &color->bodySet, bodyIdA );
+		b2ClearBit( &color->bodySet, bodyIdB );
+	}
+
+	int movedIndex = b2ContactSimArray_RemoveSwap( &color->contactSims, localIndex );
+	if ( movedIndex != B2_NULL_INDEX )
+	{
+		// Fix index on swapped contact
+		b2ContactSim* movedContactSim = color->contactSims.data + localIndex;
+
+		// Fix moved contact
+		int movedId = movedContactSim->contactId;
+		b2Contact* movedContact = b2ContactArray_Get( &world->contacts, movedId );
+		B2_ASSERT( movedContact->setIndex == b2_awakeSet );
+		B2_ASSERT( movedContact->colorIndex == colorIndex );
+		B2_ASSERT( movedContact->localIndex == movedIndex );
+		movedContact->localIndex = localIndex;
+	}
+}
+
+static int b2AssignJointColor( b2ConstraintGraph* graph, int bodyIdA, int bodyIdB, bool staticA, bool staticB )
+{
+	B2_ASSERT( staticA == false || staticB == false );
+
+#if B2_FORCE_OVERFLOW == 0
+	if ( staticA == false && staticB == false )
+	{
+		for ( int i = 0; i < B2_OVERFLOW_INDEX; ++i )
+		{
+			b2GraphColor* color = graph->colors + i;
+			if ( b2GetBit( &color->bodySet, bodyIdA ) || b2GetBit( &color->bodySet, bodyIdB ) )
+			{
+				continue;
+			}
+
+			b2SetBitGrow( &color->bodySet, bodyIdA );
+			b2SetBitGrow( &color->bodySet, bodyIdB );
+			return i;
+		}
+	}
+	else if ( staticA == false )
+	{
+		for ( int i = 0; i < B2_OVERFLOW_INDEX; ++i )
+		{
+			b2GraphColor* color = graph->colors + i;
+			if ( b2GetBit( &color->bodySet, bodyIdA ) )
+			{
+				continue;
+			}
+
+			b2SetBitGrow( &color->bodySet, bodyIdA );
+			return i;
+		}
+	}
+	else if ( staticB == false )
+	{
+		for ( int i = 0; i < B2_OVERFLOW_INDEX; ++i )
+		{
+			b2GraphColor* color = graph->colors + i;
+			if ( b2GetBit( &color->bodySet, bodyIdB ) )
+			{
+				continue;
+			}
+
+			b2SetBitGrow( &color->bodySet, bodyIdB );
+			return i;
+		}
+	}
+#else
+	B2_UNUSED( graph, bodyIdA, bodyIdB, staticA, staticB );
+#endif
+
+	return B2_OVERFLOW_INDEX;
+}
+
+b2JointSim* b2CreateJointInGraph( b2World* world, b2Joint* joint )
+{
+	b2ConstraintGraph* graph = &world->constraintGraph;
+
+	int bodyIdA = joint->edges[0].bodyId;
+	int bodyIdB = joint->edges[1].bodyId;
+	b2Body* bodyA = b2BodyArray_Get( &world->bodies, bodyIdA );
+	b2Body* bodyB = b2BodyArray_Get( &world->bodies, bodyIdB );
+	bool staticA = bodyA->setIndex == b2_staticSet;
+	bool staticB = bodyB->setIndex == b2_staticSet;
+
+	int colorIndex = b2AssignJointColor( graph, bodyIdA, bodyIdB, staticA, staticB );
+
+	b2JointSim* jointSim = b2JointSimArray_Add( &graph->colors[colorIndex].jointSims );
+	memset( jointSim, 0, sizeof( b2JointSim ) );
+
+	joint->colorIndex = colorIndex;
+	joint->localIndex = graph->colors[colorIndex].jointSims.count - 1;
+	return jointSim;
+}
+
+void b2AddJointToGraph( b2World* world, b2JointSim* jointSim, b2Joint* joint )
+{
+	b2JointSim* jointDst = b2CreateJointInGraph( world, joint );
+	memcpy( jointDst, jointSim, sizeof( b2JointSim ) );
+}
+
+void b2RemoveJointFromGraph( b2World* world, int bodyIdA, int bodyIdB, int colorIndex, int localIndex )
+{
+	b2ConstraintGraph* graph = &world->constraintGraph;
+
+	B2_ASSERT( 0 <= colorIndex && colorIndex < B2_GRAPH_COLOR_COUNT );
+	b2GraphColor* color = graph->colors + colorIndex;
+
+	if ( colorIndex != B2_OVERFLOW_INDEX )
+	{
+		// May clear static bodies, no effect
+		b2ClearBit( &color->bodySet, bodyIdA );
+		b2ClearBit( &color->bodySet, bodyIdB );
+	}
+
+	int movedIndex = b2JointSimArray_RemoveSwap( &color->jointSims, localIndex );
+	if ( movedIndex != B2_NULL_INDEX )
+	{
+		// Fix moved joint
+		b2JointSim* movedJointSim = color->jointSims.data + localIndex;
+		int movedId = movedJointSim->jointId;
+		b2Joint* movedJoint = b2JointArray_Get( &world->joints, movedId );
+		B2_ASSERT( movedJoint->setIndex == b2_awakeSet );
+		B2_ASSERT( movedJoint->colorIndex == colorIndex );
+		B2_ASSERT( movedJoint->localIndex == movedIndex );
+		movedJoint->localIndex = localIndex;
+	}
+}

src/vendor/box2d/constraint_graph.h

@@ -0,0 +1,58 @@
+// SPDX-FileCopyrightText: 2023 Erin Catto
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include "array.h"
+#include "bitset.h"
+#include "constants.h"
+
+typedef struct b2Body b2Body;
+typedef struct b2ContactSim b2ContactSim;
+typedef struct b2Contact b2Contact;
+typedef struct b2ContactConstraint b2ContactConstraint;
+typedef struct b2ContactConstraintSIMD b2ContactConstraintSIMD;
+typedef struct b2JointSim b2JointSim;
+typedef struct b2Joint b2Joint;
+typedef struct b2StepContext b2StepContext;
+typedef struct b2World b2World;
+
+// This holds constraints that cannot fit the graph color limit. This happens when a single dynamic body
+// is touching many other bodies.
+#define B2_OVERFLOW_INDEX (B2_GRAPH_COLOR_COUNT - 1)
+
+typedef struct b2GraphColor
+{
+	// This bitset is indexed by bodyId so this is over-sized to encompass static bodies
+	// however I never traverse these bits or use the bit count for anything
+	// This bitset is unused on the overflow color.
+	// todo consider having a uint_16 per body that tracks the graph color membership
+	b2BitSet bodySet;
+
+	// cache friendly arrays
+	b2ContactSimArray contactSims;
+	b2JointSimArray jointSims;
+
+	// transient
+	union
+	{
+		b2ContactConstraintSIMD* simdConstraints;
+		b2ContactConstraint* overflowConstraints;
+	};
+} b2GraphColor;
+
+typedef struct b2ConstraintGraph
+{
+	// including overflow at the end
+	b2GraphColor colors[B2_GRAPH_COLOR_COUNT];
+} b2ConstraintGraph;
+
+void b2CreateGraph( b2ConstraintGraph* graph, int bodyCapacity );
+void b2DestroyGraph( b2ConstraintGraph* graph );
+
+void b2AddContactToGraph( b2World* world, b2ContactSim* contactSim, b2Contact* contact );
+void b2RemoveContactFromGraph( b2World* world, int bodyIdA, int bodyIdB, int colorIndex, int localIndex );
+
+b2JointSim* b2CreateJointInGraph( b2World* world, b2Joint* joint );
+void b2AddJointToGraph( b2World* world, b2JointSim* jointSim, b2Joint* joint );
+void b2RemoveJointFromGraph( b2World* world, int bodyIdA, int bodyIdB, int colorIndex, int localIndex );

src/vendor/box2d/contact.c

@@ -0,0 +1,650 @@
+// SPDX-FileCopyrightText: 2023 Erin Catto
+// SPDX-License-Identifier: MIT
+
+#include "contact.h"
+
+#include "array.h"
+#include "body.h"
+#include "core.h"
+#include "island.h"
+#include "shape.h"
+#include "solver_set.h"
+#include "table.h"
+#include "world.h"
+
+#include "box2d/collision.h"
+
+#include <float.h>
+#include <math.h>
+#include <stddef.h>
+
+B2_ARRAY_SOURCE( b2Contact, b2Contact )
+B2_ARRAY_SOURCE( b2ContactSim, b2ContactSim )
+
+// Contacts and determinism
+// A deterministic simulation requires contacts to exist in the same order in b2Island no matter the thread count.
+// The order must reproduce from run to run. This is necessary because the Gauss-Seidel constraint solver is order dependent.
+//
+// Creation:
+// - Contacts are created using results from b2UpdateBroadPhasePairs
+// - These results are ordered according to the order of the broad-phase move array
+// - The move array is ordered according to the shape creation order using a bitset.
+// - The island/shape/body order is determined by creation order
+// - Logically contacts are only created for awake bodies, so they are immediately added to the awake contact array (serially)
+//
+// Island linking:
+// - The awake contact array is built from the body-contact graph for all awake bodies in awake islands.
+// - Awake contacts are solved in parallel and they generate contact state changes.
+// - These state changes may link islands together using union find.
+// - The state changes are ordered using a bit array that encompasses all contacts
+// - As long as contacts are created in deterministic order, island link order is deterministic.
+// - This keeps the order of contacts in islands deterministic
+
+// Manifold functions should compute important results in local space to improve precision. However, this
+// interface function takes two world transforms instead of a relative transform for these reasons:
+//
+// First:
+// The anchors need to be computed relative to the shape origin in world space. This is necessary so the
+// solver does not need to access static body transforms. Not even in constraint preparation. This approach
+// has world space vectors yet retains precision.
+//
+// Second:
+// b2ManifoldPoint::point is very useful for debugging and it is in world space.
+//
+// Third:
+// The user may call the manifold functions directly and they should be easy to use and have easy to use
+// results.
+typedef b2Manifold b2ManifoldFcn( const b2Shape* shapeA, b2Transform xfA, const b2Shape* shapeB, b2Transform xfB,
+								  b2SimplexCache* cache );
+
+struct b2ContactRegister
+{
+	b2ManifoldFcn* fcn;
+	bool primary;
+};
+
+static struct b2ContactRegister s_registers[b2_shapeTypeCount][b2_shapeTypeCount];
+static bool s_initialized = false;
+
+static b2Manifold b2CircleManifold( const b2Shape* shapeA, b2Transform xfA, const b2Shape* shapeB, b2Transform xfB,
+									b2SimplexCache* cache )
+{
+	B2_UNUSED( cache );
+	return b2CollideCircles( &shapeA->circle, xfA, &shapeB->circle, xfB );
+}
+
+static b2Manifold b2CapsuleAndCircleManifold( const b2Shape* shapeA, b2Transform xfA, const b2Shape* shapeB, b2Transform xfB,
+											  b2SimplexCache* cache )
+{
+	B2_UNUSED( cache );
+	return b2CollideCapsuleAndCircle( &shapeA->capsule, xfA, &shapeB->circle, xfB );
+}
+
+static b2Manifold b2CapsuleManifold( const b2Shape* shapeA, b2Transform xfA, const b2Shape* shapeB, b2Transform xfB,
+									 b2SimplexCache* cache )
+{
+	B2_UNUSED( cache );
+	return b2CollideCapsules( &shapeA->capsule, xfA, &shapeB->capsule, xfB );
+}
+
+static b2Manifold b2PolygonAndCircleManifold( const b2Shape* shapeA, b2Transform xfA, const b2Shape* shapeB, b2Transform xfB,
+											  b2SimplexCache* cache )
+{
+	B2_UNUSED( cache );
+	return b2CollidePolygonAndCircle( &shapeA->polygon, xfA, &shapeB->circle, xfB );
+}
+
+static b2Manifold b2PolygonAndCapsuleManifold( const b2Shape* shapeA, b2Transform xfA, const b2Shape* shapeB, b2Transform xfB,
+											   b2SimplexCache* cache )
+{
+	B2_UNUSED( cache );
+	return b2CollidePolygonAndCapsule( &shapeA->polygon, xfA, &shapeB->capsule, xfB );
+}
+
+static b2Manifold b2PolygonManifold( const b2Shape* shapeA, b2Transform xfA, const b2Shape* shapeB, b2Transform xfB,
+									 b2SimplexCache* cache )
+{
+	B2_UNUSED( cache );
+	return b2CollidePolygons( &shapeA->polygon, xfA, &shapeB->polygon, xfB );
+}
+
+static b2Manifold b2SegmentAndCircleManifold( const b2Shape* shapeA, b2Transform xfA, const b2Shape* shapeB, b2Transform xfB,
+											  b2SimplexCache* cache )
+{
+	B2_UNUSED( cache );
+	return b2CollideSegmentAndCircle( &shapeA->segment, xfA, &shapeB->circle, xfB );
+}
+
+static b2Manifold b2SegmentAndCapsuleManifold( const b2Shape* shapeA, b2Transform xfA, const b2Shape* shapeB, b2Transform xfB,
+											   b2SimplexCache* cache )
+{
+	B2_UNUSED( cache );
+	return b2CollideSegmentAndCapsule( &shapeA->segment, xfA, &shapeB->capsule, xfB );
+}
+
+static b2Manifold b2SegmentAndPolygonManifold( const b2Shape* shapeA, b2Transform xfA, const b2Shape* shapeB, b2Transform xfB,
+											   b2SimplexCache* cache )
+{
+	B2_UNUSED( cache );
+	return b2CollideSegmentAndPolygon( &shapeA->segment, xfA, &shapeB->polygon, xfB );
+}
+
+static b2Manifold b2ChainSegmentAndCircleManifold( const b2Shape* shapeA, b2Transform xfA, const b2Shape* shapeB, b2Transform xfB,
+												   b2SimplexCache* cache )
+{
+	B2_UNUSED( cache );
+	return b2CollideChainSegmentAndCircle( &shapeA->chainSegment, xfA, &shapeB->circle, xfB );
+}
+
+static b2Manifold b2ChainSegmentAndCapsuleManifold( const b2Shape* shapeA, b2Transform xfA, const b2Shape* shapeB,
+													b2Transform xfB, b2SimplexCache* cache )
+{
+	return b2CollideChainSegmentAndCapsule( &shapeA->chainSegment, xfA, &shapeB->capsule, xfB, cache );
+}
+
+static b2Manifold b2ChainSegmentAndPolygonManifold( const b2Shape* shapeA, b2Transform xfA, const b2Shape* shapeB,
+													b2Transform xfB, b2SimplexCache* cache )
+{
+	return b2CollideChainSegmentAndPolygon( &shapeA->chainSegment, xfA, &shapeB->polygon, xfB, cache );
+}
+
+static void b2AddType( b2ManifoldFcn* fcn, b2ShapeType type1, b2ShapeType type2 )
+{
+	B2_ASSERT( 0 <= type1 && type1 < b2_shapeTypeCount );
+	B2_ASSERT( 0 <= type2 && type2 < b2_shapeTypeCount );
+
+	s_registers[type1][type2].fcn = fcn;
+	s_registers[type1][type2].primary = true;
+
+	if ( type1 != type2 )
+	{
+		s_registers[type2][type1].fcn = fcn;
+		s_registers[type2][type1].primary = false;
+	}
+}
+
+void b2InitializeContactRegisters( void )
+{
+	if ( s_initialized == false )
+	{
+		b2AddType( b2CircleManifold, b2_circleShape, b2_circleShape );
+		b2AddType( b2CapsuleAndCircleManifold, b2_capsuleShape, b2_circleShape );
+		b2AddType( b2CapsuleManifold, b2_capsuleShape, b2_capsuleShape );
+		b2AddType( b2PolygonAndCircleManifold, b2_polygonShape, b2_circleShape );
+		b2AddType( b2PolygonAndCapsuleManifold, b2_polygonShape, b2_capsuleShape );
+		b2AddType( b2PolygonManifold, b2_polygonShape, b2_polygonShape );
+		b2AddType( b2SegmentAndCircleManifold, b2_segmentShape, b2_circleShape );
+		b2AddType( b2SegmentAndCapsuleManifold, b2_segmentShape, b2_capsuleShape );
+		b2AddType( b2SegmentAndPolygonManifold, b2_segmentShape, b2_polygonShape );
+		b2AddType( b2ChainSegmentAndCircleManifold, b2_chainSegmentShape, b2_circleShape );
+		b2AddType( b2ChainSegmentAndCapsuleManifold, b2_chainSegmentShape, b2_capsuleShape );
+		b2AddType( b2ChainSegmentAndPolygonManifold, b2_chainSegmentShape, b2_polygonShape );
+		s_initialized = true;
+	}
+}
+
+void b2CreateContact( b2World* world, b2Shape* shapeA, b2Shape* shapeB )
+{
+	b2ShapeType type1 = shapeA->type;
+	b2ShapeType type2 = shapeB->type;
+
+	B2_ASSERT( 0 <= type1 && type1 < b2_shapeTypeCount );
+	B2_ASSERT( 0 <= type2 && type2 < b2_shapeTypeCount );
+
+	if ( s_registers[type1][type2].fcn == NULL )
+	{
+		// For example, no segment vs segment collision
+		return;
+	}
+
+	if ( s_registers[type1][type2].primary == false )
+	{
+		// flip order
+		b2CreateContact( world, shapeB, shapeA );
+		return;
+	}
+
+	b2Body* bodyA = b2BodyArray_Get( &world->bodies, shapeA->bodyId );
+	b2Body* bodyB = b2BodyArray_Get( &world->bodies, shapeB->bodyId );
+
+	B2_ASSERT( bodyA->setIndex != b2_disabledSet && bodyB->setIndex != b2_disabledSet );
+	B2_ASSERT( bodyA->setIndex != b2_staticSet || bodyB->setIndex != b2_staticSet );
+
+	int setIndex;
+	if ( bodyA->setIndex == b2_awakeSet || bodyB->setIndex == b2_awakeSet )
+	{
+		setIndex = b2_awakeSet;
+	}
+	else
+	{
+		// sleeping and non-touching contacts live in the disabled set
+		// later if this set is found to be touching then the sleeping
+		// islands will be linked and the contact moved to the merged island
+		setIndex = b2_disabledSet;
+	}
+
+	b2SolverSet* set = b2SolverSetArray_Get( &world->solverSets, setIndex );
+
+	// Create contact key and contact
+	int contactId = b2AllocId( &world->contactIdPool );
+	if ( contactId == world->contacts.count )
+	{
+		b2ContactArray_Push( &world->contacts, ( b2Contact ){ 0 } );
+	}
+
+	int shapeIdA = shapeA->id;
+	int shapeIdB = shapeB->id;
+
+	b2Contact* contact = b2ContactArray_Get( &world->contacts, contactId );
+	contact->contactId = contactId;
+	contact->setIndex = setIndex;
+	contact->colorIndex = B2_NULL_INDEX;
+	contact->localIndex = set->contactSims.count;
+	contact->islandId = B2_NULL_INDEX;
+	contact->islandPrev = B2_NULL_INDEX;
+	contact->islandNext = B2_NULL_INDEX;
+	contact->shapeIdA = shapeIdA;
+	contact->shapeIdB = shapeIdB;
+	contact->isMarked = false;
+	contact->flags = 0;
+
+	B2_ASSERT( shapeA->sensorIndex == B2_NULL_INDEX && shapeB->sensorIndex == B2_NULL_INDEX );
+
+	if ( shapeA->enableContactEvents || shapeB->enableContactEvents )
+	{
+		contact->flags |= b2_contactEnableContactEvents;
+	}
+
+	// Connect to body A
+	{
+		contact->edges[0].bodyId = shapeA->bodyId;
+		contact->edges[0].prevKey = B2_NULL_INDEX;
+		contact->edges[0].nextKey = bodyA->headContactKey;
+
+		int keyA = ( contactId << 1 ) | 0;
+		int headContactKey = bodyA->headContactKey;
+		if ( headContactKey != B2_NULL_INDEX )
+		{
+			b2Contact* headContact = b2ContactArray_Get( &world->contacts, headContactKey >> 1 );
+			headContact->edges[headContactKey & 1].prevKey = keyA;
+		}
+		bodyA->headContactKey = keyA;
+		bodyA->contactCount += 1;
+	}
+
+	// Connect to body B
+	{
+		contact->edges[1].bodyId = shapeB->bodyId;
+		contact->edges[1].prevKey = B2_NULL_INDEX;
+		contact->edges[1].nextKey = bodyB->headContactKey;
+
+		int keyB = ( contactId << 1 ) | 1;
+		int headContactKey = bodyB->headContactKey;
+		if ( bodyB->headContactKey != B2_NULL_INDEX )
+		{
+			b2Contact* headContact = b2ContactArray_Get( &world->contacts, headContactKey >> 1 );
+			headContact->edges[headContactKey & 1].prevKey = keyB;
+		}
+		bodyB->headContactKey = keyB;
+		bodyB->contactCount += 1;
+	}
+
+	// Add to pair set for fast lookup
+	uint64_t pairKey = B2_SHAPE_PAIR_KEY( shapeIdA, shapeIdB );
+	b2AddKey( &world->broadPhase.pairSet, pairKey );
+
+	// Contacts are created as non-touching. Later if they are found to be touching
+	// they will link islands and be moved into the constraint graph.
+	b2ContactSim* contactSim = b2ContactSimArray_Add( &set->contactSims );
+	contactSim->contactId = contactId;
+
+#if B2_VALIDATE
+	contactSim->bodyIdA = shapeA->bodyId;
+	contactSim->bodyIdB = shapeB->bodyId;
+#endif
+
+	contactSim->bodySimIndexA = B2_NULL_INDEX;
+	contactSim->bodySimIndexB = B2_NULL_INDEX;
+	contactSim->invMassA = 0.0f;
+	contactSim->invIA = 0.0f;
+	contactSim->invMassB = 0.0f;
+	contactSim->invIB = 0.0f;
+	contactSim->shapeIdA = shapeIdA;
+	contactSim->shapeIdB = shapeIdB;
+	contactSim->cache = b2_emptySimplexCache;
+	contactSim->manifold = ( b2Manifold ){ 0 };
+
+	// These also get updated in the narrow phase
+	contactSim->friction = world->frictionCallback(shapeA->friction, shapeA->userMaterialId, shapeB->friction, shapeB->userMaterialId);
+	contactSim->restitution = world->restitutionCallback(shapeA->restitution, shapeA->userMaterialId, shapeB->restitution, shapeB->userMaterialId);
+
+	contactSim->tangentSpeed = 0.0f;
+	contactSim->simFlags = 0;
+
+	if ( shapeA->enablePreSolveEvents || shapeB->enablePreSolveEvents )
+	{
+		contactSim->simFlags |= b2_simEnablePreSolveEvents;
+	}
+}
+
+// A contact is destroyed when:
+// - broad-phase proxies stop overlapping
+// - a body is destroyed
+// - a body is disabled
+// - a body changes type from dynamic to kinematic or static
+// - a shape is destroyed
+// - contact filtering is modified
+void b2DestroyContact( b2World* world, b2Contact* contact, bool wakeBodies )
+{
+	// Remove pair from set
+	uint64_t pairKey = B2_SHAPE_PAIR_KEY( contact->shapeIdA, contact->shapeIdB );
+	b2RemoveKey( &world->broadPhase.pairSet, pairKey );
+
+	b2ContactEdge* edgeA = contact->edges + 0;
+	b2ContactEdge* edgeB = contact->edges + 1;
+
+	int bodyIdA = edgeA->bodyId;
+	int bodyIdB = edgeB->bodyId;
+	b2Body* bodyA = b2BodyArray_Get( &world->bodies, bodyIdA );
+	b2Body* bodyB = b2BodyArray_Get( &world->bodies, bodyIdB );
+
+	uint32_t flags = contact->flags;
+	bool touching = ( flags & b2_contactTouchingFlag ) != 0;
+
+	// End touch event
+	if ( touching && ( flags & b2_contactEnableContactEvents ) != 0 )
+	{
+		uint16_t worldId = world->worldId;
+		const b2Shape* shapeA = b2ShapeArray_Get( &world->shapes, contact->shapeIdA );
+		const b2Shape* shapeB = b2ShapeArray_Get( &world->shapes, contact->shapeIdB );
+		b2ShapeId shapeIdA = { shapeA->id + 1, worldId, shapeA->generation };
+		b2ShapeId shapeIdB = { shapeB->id + 1, worldId, shapeB->generation };
+
+		b2ContactEndTouchEvent event = { shapeIdA, shapeIdB };
+		b2ContactEndTouchEventArray_Push( world->contactEndEvents + world->endEventArrayIndex, event );
+	}
+
+	// Remove from body A
+	if ( edgeA->prevKey != B2_NULL_INDEX )
+	{
+		b2Contact* prevContact = b2ContactArray_Get( &world->contacts, edgeA->prevKey >> 1 );
+		b2ContactEdge* prevEdge = prevContact->edges + ( edgeA->prevKey & 1 );
+		prevEdge->nextKey = edgeA->nextKey;
+	}
+
+	if ( edgeA->nextKey != B2_NULL_INDEX )
+	{
+		b2Contact* nextContact = b2ContactArray_Get( &world->contacts, edgeA->nextKey >> 1 );
+		b2ContactEdge* nextEdge = nextContact->edges + ( edgeA->nextKey & 1 );
+		nextEdge->prevKey = edgeA->prevKey;
+	}
+
+	int contactId = contact->contactId;
+
+	int edgeKeyA = ( contactId << 1 ) | 0;
+	if ( bodyA->headContactKey == edgeKeyA )
+	{
+		bodyA->headContactKey = edgeA->nextKey;
+	}
+
+	bodyA->contactCount -= 1;
+
+	// Remove from body B
+	if ( edgeB->prevKey != B2_NULL_INDEX )
+	{
+		b2Contact* prevContact = b2ContactArray_Get( &world->contacts, edgeB->prevKey >> 1 );
+		b2ContactEdge* prevEdge = prevContact->edges + ( edgeB->prevKey & 1 );
+		prevEdge->nextKey = edgeB->nextKey;
+	}
+
+	if ( edgeB->nextKey != B2_NULL_INDEX )
+	{
+		b2Contact* nextContact = b2ContactArray_Get( &world->contacts, edgeB->nextKey >> 1 );
+		b2ContactEdge* nextEdge = nextContact->edges + ( edgeB->nextKey & 1 );
+		nextEdge->prevKey = edgeB->prevKey;
+	}
+
+	int edgeKeyB = ( contactId << 1 ) | 1;
+	if ( bodyB->headContactKey == edgeKeyB )
+	{
+		bodyB->headContactKey = edgeB->nextKey;
+	}
+
+	bodyB->contactCount -= 1;
+
+	// Remove contact from the array that owns it
+	if ( contact->islandId != B2_NULL_INDEX )
+	{
+		b2UnlinkContact( world, contact );
+	}
+
+	if ( contact->colorIndex != B2_NULL_INDEX )
+	{
+		// contact is an active constraint
+		B2_ASSERT( contact->setIndex == b2_awakeSet );
+		b2RemoveContactFromGraph( world, bodyIdA, bodyIdB, contact->colorIndex, contact->localIndex );
+	}
+	else
+	{
+		// contact is non-touching or is sleeping or is a sensor
+		B2_ASSERT( contact->setIndex != b2_awakeSet || ( contact->flags & b2_contactTouchingFlag ) == 0 );
+		b2SolverSet* set = b2SolverSetArray_Get( &world->solverSets, contact->setIndex );
+		int movedIndex = b2ContactSimArray_RemoveSwap( &set->contactSims, contact->localIndex );
+		if ( movedIndex != B2_NULL_INDEX )
+		{
+			b2ContactSim* movedContactSim = set->contactSims.data + contact->localIndex;
+			b2Contact* movedContact = b2ContactArray_Get( &world->contacts, movedContactSim->contactId );
+			movedContact->localIndex = contact->localIndex;
+		}
+	}
+
+	contact->contactId = B2_NULL_INDEX;
+	contact->setIndex = B2_NULL_INDEX;
+	contact->colorIndex = B2_NULL_INDEX;
+	contact->localIndex = B2_NULL_INDEX;
+
+	b2FreeId( &world->contactIdPool, contactId );
+
+	if ( wakeBodies && touching )
+	{
+		b2WakeBody( world, bodyA );
+		b2WakeBody( world, bodyB );
+	}
+}
+
+b2ContactSim* b2GetContactSim( b2World* world, b2Contact* contact )
+{
+	if ( contact->setIndex == b2_awakeSet && contact->colorIndex != B2_NULL_INDEX )
+	{
+		// contact lives in constraint graph
+		B2_ASSERT( 0 <= contact->colorIndex && contact->colorIndex < B2_GRAPH_COLOR_COUNT );
+		b2GraphColor* color = world->constraintGraph.colors + contact->colorIndex;
+		return b2ContactSimArray_Get( &color->contactSims, contact->localIndex );
+	}
+
+	b2SolverSet* set = b2SolverSetArray_Get( &world->solverSets, contact->setIndex );
+	return b2ContactSimArray_Get( &set->contactSims, contact->localIndex );
+}
+
+bool b2ShouldShapesCollide( b2Filter filterA, b2Filter filterB )
+{
+	if ( filterA.groupIndex == filterB.groupIndex && filterA.groupIndex != 0 )
+	{
+		return filterA.groupIndex > 0;
+	}
+
+	bool collide = ( filterA.maskBits & filterB.categoryBits ) != 0 && ( filterA.categoryBits & filterB.maskBits ) != 0;
+	return collide;
+}
+
+// Update the contact manifold and touching status. Also updates sensor overlap.
+// Note: do not assume the shape AABBs are overlapping or are valid.
+bool b2UpdateContact( b2World* world, b2ContactSim* contactSim, b2Shape* shapeA, b2Transform transformA, b2Vec2 centerOffsetA,
+					  b2Shape* shapeB, b2Transform transformB, b2Vec2 centerOffsetB )
+{
+	// Save old manifold
+	b2Manifold oldManifold = contactSim->manifold;
+
+	// Compute new manifold
+	b2ManifoldFcn* fcn = s_registers[shapeA->type][shapeB->type].fcn;
+	contactSim->manifold = fcn( shapeA, transformA, shapeB, transformB, &contactSim->cache );
+
+	// Keep these updated in case the values on the shapes are modified
+	contactSim->friction = world->frictionCallback( shapeA->friction, shapeA->userMaterialId, shapeB->friction, shapeB->userMaterialId );
+	contactSim->restitution = world->restitutionCallback( shapeA->restitution, shapeA->userMaterialId, shapeB->restitution, shapeB->userMaterialId );
+
+	// todo branch improves perf?
+	if (shapeA->rollingResistance > 0.0f || shapeB->rollingResistance > 0.0f)
+	{
+		float radiusA = b2GetShapeRadius( shapeA );
+		float radiusB = b2GetShapeRadius( shapeB );
+		float maxRadius = b2MaxFloat( radiusA, radiusB );
+		contactSim->rollingResistance = b2MaxFloat( shapeA->rollingResistance, shapeB->rollingResistance ) * maxRadius;
+	}
+	else
+	{
+		contactSim->rollingResistance = 0.0f;
+	}
+
+	contactSim->tangentSpeed = shapeA->tangentSpeed + shapeB->tangentSpeed;
+
+	int pointCount = contactSim->manifold.pointCount;
+	bool touching = pointCount > 0;
+
+	if ( touching && world->preSolveFcn && ( contactSim->simFlags & b2_simEnablePreSolveEvents ) != 0 )
+	{
+		b2ShapeId shapeIdA = { shapeA->id + 1, world->worldId, shapeA->generation };
+		b2ShapeId shapeIdB = { shapeB->id + 1, world->worldId, shapeB->generation };
+
+		// this call assumes thread safety
+		touching = world->preSolveFcn( shapeIdA, shapeIdB, &contactSim->manifold, world->preSolveContext );
+		if ( touching == false )
+		{
+			// disable contact
+			pointCount = 0;
+			contactSim->manifold.pointCount = 0;
+		}
+	}
+
+	// This flag is for testing
+	if ( world->enableSpeculative == false && pointCount == 2 )
+	{
+		if ( contactSim->manifold.points[0].separation > 1.5f * B2_LINEAR_SLOP )
+		{
+			contactSim->manifold.points[0] = contactSim->manifold.points[1];
+			contactSim->manifold.pointCount = 1;
+		}
+		else if ( contactSim->manifold.points[0].separation > 1.5f * B2_LINEAR_SLOP )
+		{
+			contactSim->manifold.pointCount = 1;
+		}
+
+		pointCount = contactSim->manifold.pointCount;
+	}
+
+	if ( touching && ( shapeA->enableHitEvents || shapeB->enableHitEvents ) )
+	{
+		contactSim->simFlags |= b2_simEnableHitEvent;
+	}
+	else
+	{
+		contactSim->simFlags &= ~b2_simEnableHitEvent;
+	}
+
+	if (pointCount > 0)
+	{
+		contactSim->manifold.rollingImpulse = oldManifold.rollingImpulse;
+	}
+
+	// Match old contact ids to new contact ids and copy the
+	// stored impulses to warm start the solver.
+	int unmatchedCount = 0;
+	for ( int i = 0; i < pointCount; ++i )
+	{
+		b2ManifoldPoint* mp2 = contactSim->manifold.points + i;
+
+		// shift anchors to be center of mass relative
+		mp2->anchorA = b2Sub( mp2->anchorA, centerOffsetA );
+		mp2->anchorB = b2Sub( mp2->anchorB, centerOffsetB );
+
+		mp2->normalImpulse = 0.0f;
+		mp2->tangentImpulse = 0.0f;
+		mp2->totalNormalImpulse = 0.0f;
+		mp2->normalVelocity = 0.0f;
+		mp2->persisted = false;
+
+		uint16_t id2 = mp2->id;
+
+		for ( int j = 0; j < oldManifold.pointCount; ++j )
+		{
+			b2ManifoldPoint* mp1 = oldManifold.points + j;
+
+			if ( mp1->id == id2 )
+			{
+				mp2->normalImpulse = mp1->normalImpulse;
+				mp2->tangentImpulse = mp1->tangentImpulse;
+				mp2->persisted = true;
+
+				// clear old impulse
+				mp1->normalImpulse = 0.0f;
+				mp1->tangentImpulse = 0.0f;
+				break;
+			}
+		}
+
+		unmatchedCount += mp2->persisted ? 0 : 1;
+	}
+
+	B2_UNUSED( unmatchedCount );
+
+#if 0
+		// todo I haven't found an improvement from this yet
+		// If there are unmatched new contact points, apply any left over old impulse.
+		if (unmatchedCount > 0)
+		{
+			float unmatchedNormalImpulse = 0.0f;
+			float unmatchedTangentImpulse = 0.0f;
+			for (int i = 0; i < oldManifold.pointCount; ++i)
+			{
+				b2ManifoldPoint* mp = oldManifold.points + i;
+				unmatchedNormalImpulse += mp->normalImpulse;
+				unmatchedTangentImpulse += mp->tangentImpulse;
+			}
+
+			float inverse = 1.0f / unmatchedCount;
+			unmatchedNormalImpulse *= inverse;
+			unmatchedTangentImpulse *= inverse;
+
+			for ( int i = 0; i < pointCount; ++i )
+			{
+				b2ManifoldPoint* mp2 = contactSim->manifold.points + i;
+
+				if (mp2->persisted)
+				{
+					continue;
+				}
+
+				mp2->normalImpulse = unmatchedNormalImpulse;
+				mp2->tangentImpulse = unmatchedTangentImpulse;
+			}
+		}
+#endif
+
+	if ( touching )
+	{
+		contactSim->simFlags |= b2_simTouchingFlag;
+	}
+	else
+	{
+		contactSim->simFlags &= ~b2_simTouchingFlag;
+	}
+
+	return touching;
+}
+
+b2Manifold b2ComputeManifold( b2Shape* shapeA, b2Transform transformA, b2Shape* shapeB, b2Transform transformB )
+{
+	b2ManifoldFcn* fcn = s_registers[shapeA->type][shapeB->type].fcn;
+	b2SimplexCache cache = { 0 };
+	return fcn( shapeA, transformA, shapeB, transformB, &cache );
+}

src/vendor/box2d/contact.h

@@ -0,0 +1,148 @@
+// SPDX-FileCopyrightText: 2023 Erin Catto
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include "array.h"
+#include "core.h"
+
+#include "box2d/collision.h"
+#include "box2d/types.h"
+
+typedef struct b2Shape b2Shape;
+typedef struct b2World b2World;
+
+enum b2ContactFlags
+{
+	// Set when the solid shapes are touching.
+	b2_contactTouchingFlag = 0x00000001,
+
+	// Contact has a hit event
+	b2_contactHitEventFlag = 0x00000002,
+
+	// This contact wants contact events
+	b2_contactEnableContactEvents = 0x00000004,
+};
+
+// A contact edge is used to connect bodies and contacts together
+// in a contact graph where each body is a node and each contact
+// is an edge. A contact edge belongs to a doubly linked list
+// maintained in each attached body. Each contact has two contact
+// edges, one for each attached body.
+typedef struct b2ContactEdge
+{
+	int bodyId;
+	int prevKey;
+	int nextKey;
+} b2ContactEdge;
+
+// Cold contact data. Used as a persistent handle and for persistent island
+// connectivity.
+typedef struct b2Contact
+{
+	// index of simulation set stored in b2World
+	// B2_NULL_INDEX when slot is free
+	int setIndex;
+
+	// index into the constraint graph color array
+	// B2_NULL_INDEX for non-touching or sleeping contacts
+	// B2_NULL_INDEX when slot is free
+	int colorIndex;
+
+	// contact index within set or graph color
+	// B2_NULL_INDEX when slot is free
+	int localIndex;
+
+	b2ContactEdge edges[2];
+	int shapeIdA;
+	int shapeIdB;
+
+	// A contact only belongs to an island if touching, otherwise B2_NULL_INDEX.
+	int islandPrev;
+	int islandNext;
+	int islandId;
+
+	int contactId;
+
+	// b2ContactFlags
+	uint32_t flags;
+
+	bool isMarked;
+} b2Contact;
+
+// Shifted to be distinct from b2ContactFlags
+enum b2ContactSimFlags
+{
+	// Set when the shapes are touching
+	b2_simTouchingFlag = 0x00010000,
+
+	// This contact no longer has overlapping AABBs
+	b2_simDisjoint = 0x00020000,
+
+	// This contact started touching
+	b2_simStartedTouching = 0x00040000,
+
+	// This contact stopped touching
+	b2_simStoppedTouching = 0x00080000,
+
+	// This contact has a hit event
+	b2_simEnableHitEvent = 0x00100000,
+
+	// This contact wants pre-solve events
+	b2_simEnablePreSolveEvents = 0x00200000,
+};
+
+/// The class manages contact between two shapes. A contact exists for each overlapping
+/// AABB in the broad-phase (except if filtered). Therefore a contact object may exist
+/// that has no contact points.
+typedef struct b2ContactSim
+{
+	int contactId;
+
+#if B2_VALIDATE
+	int bodyIdA;
+	int bodyIdB;
+#endif
+
+	int bodySimIndexA;
+	int bodySimIndexB;
+
+	int shapeIdA;
+	int shapeIdB;
+
+	float invMassA;
+	float invIA;
+
+	float invMassB;
+	float invIB;
+
+	b2Manifold manifold;
+
+	// Mixed friction and restitution
+	float friction;
+	float restitution;
+	float rollingResistance;
+	float tangentSpeed;
+
+	// b2ContactSimFlags
+	uint32_t simFlags;
+
+	b2SimplexCache cache;
+} b2ContactSim;
+
+void b2InitializeContactRegisters( void );
+
+void b2CreateContact( b2World* world, b2Shape* shapeA, b2Shape* shapeB );
+void b2DestroyContact( b2World* world, b2Contact* contact, bool wakeBodies );
+
+b2ContactSim* b2GetContactSim( b2World* world, b2Contact* contact );
+
+bool b2ShouldShapesCollide( b2Filter filterA, b2Filter filterB );
+
+bool b2UpdateContact( b2World* world, b2ContactSim* contactSim, b2Shape* shapeA, b2Transform transformA, b2Vec2 centerOffsetA,
+					  b2Shape* shapeB, b2Transform transformB, b2Vec2 centerOffsetB );
+
+b2Manifold b2ComputeManifold( b2Shape* shapeA, b2Transform transformA, b2Shape* shapeB, b2Transform transformB );
+
+B2_ARRAY_INLINE( b2Contact, b2Contact )
+B2_ARRAY_INLINE( b2ContactSim, b2ContactSim )

src/vendor/box2d/contact_solver.h

@@ -0,0 +1,54 @@
+// SPDX-FileCopyrightText: 2023 Erin Catto
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include "solver.h"
+
+typedef struct b2ContactSim b2ContactSim;
+
+typedef struct b2ContactConstraintPoint
+{
+	b2Vec2 anchorA, anchorB;
+	float baseSeparation;
+	float relativeVelocity;
+	float normalImpulse;
+	float tangentImpulse;
+	float totalNormalImpulse;
+	float normalMass;
+	float tangentMass;
+} b2ContactConstraintPoint;
+
+typedef struct b2ContactConstraint
+{
+	int indexA;
+	int indexB;
+	b2ContactConstraintPoint points[2];
+	b2Vec2 normal;
+	float invMassA, invMassB;
+	float invIA, invIB;
+	float friction;
+	float restitution;
+	float tangentSpeed;
+	float rollingResistance;
+	float rollingMass;
+	float rollingImpulse;
+	b2Softness softness;
+	int pointCount;
+} b2ContactConstraint;
+
+int b2GetContactConstraintSIMDByteCount( void );
+
+// Overflow contacts don't fit into the constraint graph coloring
+void b2PrepareOverflowContacts( b2StepContext* context );
+void b2WarmStartOverflowContacts( b2StepContext* context );
+void b2SolveOverflowContacts( b2StepContext* context, bool useBias );
+void b2ApplyOverflowRestitution( b2StepContext* context );
+void b2StoreOverflowImpulses( b2StepContext* context );
+
+// Contacts that live within the constraint graph coloring
+void b2PrepareContactsTask( int startIndex, int endIndex, b2StepContext* context );
+void b2WarmStartContactsTask( int startIndex, int endIndex, b2StepContext* context, int colorIndex );
+void b2SolveContactsTask( int startIndex, int endIndex, b2StepContext* context, int colorIndex, bool useBias );
+void b2ApplyRestitutionTask( int startIndex, int endIndex, b2StepContext* context, int colorIndex );
+void b2StoreImpulsesTask( int startIndex, int endIndex, b2StepContext* context );

src/vendor/box2d/core.c

@@ -0,0 +1,178 @@
+// SPDX-FileCopyrightText: 2023 Erin Catto
+// SPDX-License-Identifier: MIT
+
+#include "core.h"
+
+#if defined( B2_COMPILER_MSVC )
+#define _CRTDBG_MAP_ALLOC
+#include <crtdbg.h>
+#include <stdlib.h>
+#else
+#include <stdlib.h>
+#endif
+
+#include <stdio.h>
+#include <string.h>
+
+#ifdef BOX2D_PROFILE
+
+#include <tracy/TracyC.h>
+#define b2TracyCAlloc( ptr, size ) TracyCAlloc( ptr, size )
+#define b2TracyCFree( ptr ) TracyCFree( ptr )
+
+#else
+
+#define b2TracyCAlloc( ptr, size )
+#define b2TracyCFree( ptr )
+
+#endif
+
+#include "atomic.h"
+
+// This allows the user to change the length units at runtime
+float b2_lengthUnitsPerMeter = 1.0f;
+
+void b2SetLengthUnitsPerMeter( float lengthUnits )
+{
+	B2_ASSERT( b2IsValidFloat( lengthUnits ) && lengthUnits > 0.0f );
+	b2_lengthUnitsPerMeter = lengthUnits;
+}
+
+float b2GetLengthUnitsPerMeter( void )
+{
+	return b2_lengthUnitsPerMeter;
+}
+
+static int b2DefaultAssertFcn( const char* condition, const char* fileName, int lineNumber )
+{
+	printf( "BOX2D ASSERTION: %s, %s, line %d\n", condition, fileName, lineNumber );
+
+	// return non-zero to break to debugger
+	return 1;
+}
+
+b2AssertFcn* b2AssertHandler = b2DefaultAssertFcn;
+
+void b2SetAssertFcn( b2AssertFcn* assertFcn )
+{
+	B2_ASSERT( assertFcn != NULL );
+	b2AssertHandler = assertFcn;
+}
+
+#if !defined( NDEBUG ) || defined( B2_ENABLE_ASSERT )
+int b2InternalAssertFcn( const char* condition, const char* fileName, int lineNumber )
+{
+	return b2AssertHandler( condition, fileName, lineNumber );
+}
+#endif
+
+b2Version b2GetVersion( void )
+{
+	return (b2Version){
+		.major = 3,
+		.minor = 1,
+		.revision = 0,
+	};
+}
+
+static b2AllocFcn* b2_allocFcn = NULL;
+static b2FreeFcn* b2_freeFcn = NULL;
+
+b2AtomicInt b2_byteCount;
+
+void b2SetAllocator( b2AllocFcn* allocFcn, b2FreeFcn* freeFcn )
+{
+	b2_allocFcn = allocFcn;
+	b2_freeFcn = freeFcn;
+}
+
+// Use 32 byte alignment for everything. Works with 256bit SIMD.
+#define B2_ALIGNMENT 32
+
+void* b2Alloc( int size )
+{
+	if ( size == 0 )
+	{
+		return NULL;
+	}
+
+	// This could cause some sharing issues, however Box2D rarely calls b2Alloc.
+	b2AtomicFetchAddInt( &b2_byteCount, size );
+
+	// Allocation must be a multiple of 32 or risk a seg fault
+	// https://en.cppreference.com/w/c/memory/aligned_alloc
+	int size32 = ( ( size - 1 ) | 0x1F ) + 1;
+
+	if ( b2_allocFcn != NULL )
+	{
+		void* ptr = b2_allocFcn( size32, B2_ALIGNMENT );
+		b2TracyCAlloc( ptr, size );
+
+		B2_ASSERT( ptr != NULL );
+		B2_ASSERT( ( (uintptr_t)ptr & 0x1F ) == 0 );
+
+		return ptr;
+	}
+
+#ifdef B2_PLATFORM_WINDOWS
+	void* ptr = _aligned_malloc( size32, B2_ALIGNMENT );
+#elif defined( B2_PLATFORM_ANDROID )
+	void* ptr = NULL;
+	if ( posix_memalign( &ptr, B2_ALIGNMENT, size32 ) != 0 )
+	{
+		// allocation failed, exit the application
+		exit( EXIT_FAILURE );
+	}
+#else
+	void* ptr = aligned_alloc( B2_ALIGNMENT, size32 );
+#endif
+
+	b2TracyCAlloc( ptr, size );
+
+	B2_ASSERT( ptr != NULL );
+	B2_ASSERT( ( (uintptr_t)ptr & 0x1F ) == 0 );
+
+	return ptr;
+}
+
+void b2Free( void* mem, int size )
+{
+	if ( mem == NULL )
+	{
+		return;
+	}
+
+	b2TracyCFree( mem );
+
+	if ( b2_freeFcn != NULL )
+	{
+		b2_freeFcn( mem );
+	}
+	else
+	{
+#ifdef B2_PLATFORM_WINDOWS
+		_aligned_free( mem );
+#else
+		free( mem );
+#endif
+	}
+
+	b2AtomicFetchAddInt( &b2_byteCount, -size );
+}
+
+void* b2GrowAlloc( void* oldMem, int oldSize, int newSize )
+{
+	B2_ASSERT( newSize > oldSize );
+	void* newMem = b2Alloc( newSize );
+	if ( oldSize > 0 )
+	{
+		memcpy( newMem, oldMem, oldSize );
+		b2Free( oldMem, oldSize );
+	}
+	return newMem;
+}
+
+int b2GetByteCount( void )
+{
+	return b2AtomicLoadInt( &b2_byteCount );
+}

src/vendor/box2d/core.h

@@ -0,0 +1,143 @@
+// SPDX-FileCopyrightText: 2023 Erin Catto
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include "box2d/math_functions.h"
+
+// clang-format off
+
+#define B2_NULL_INDEX ( -1 )
+
+// for performance comparisons
+#define B2_RESTRICT restrict
+
+#ifdef NDEBUG
+	#define B2_DEBUG 0
+#else
+	#define B2_DEBUG 1
+#endif
+
+#if defined( BOX2D_VALIDATE ) && !defined( NDEBUG )
+	#define B2_VALIDATE 1
+#else
+	#define B2_VALIDATE 0
+#endif
+
+// Define platform
+#if defined(_WIN32) || defined(_WIN64)
+	#define B2_PLATFORM_WINDOWS
+#elif defined( __ANDROID__ )
+	#define B2_PLATFORM_ANDROID
+#elif defined( __linux__ )
+	#define B2_PLATFORM_LINUX
+#elif defined( __APPLE__ )
+	#include <TargetConditionals.h>
+	#if defined( TARGET_OS_IPHONE ) && !TARGET_OS_IPHONE
+		#define B2_PLATFORM_MACOS
+	#else
+		#define B2_PLATFORM_IOS
+	#endif
+#elif defined( __EMSCRIPTEN__ )
+	#define B2_PLATFORM_WASM
+#else
+	#define B2_PLATFORM_UNKNOWN
+#endif
+
+// Define CPU
+#if defined( __x86_64__ ) || defined( _M_X64 ) || defined( __i386__ ) || defined( _M_IX86 )
+	#define B2_CPU_X86_X64
+#elif defined( __aarch64__ ) || defined( _M_ARM64 ) || defined( __arm__ ) || defined( _M_ARM )
+	#define B2_CPU_ARM
+#elif defined( __EMSCRIPTEN__ )
+	#define B2_CPU_WASM
+#else
+	#define B2_CPU_UNKNOWN
+#endif
+
+// Define SIMD
+#if defined( BOX2D_ENABLE_SIMD )
+	#if defined( B2_CPU_X86_X64 )
+		#if defined( BOX2D_AVX2 )
+			#define B2_SIMD_AVX2
+			#define B2_SIMD_WIDTH 8
+		#else
+			#define B2_SIMD_SSE2
+			#define B2_SIMD_WIDTH 4
+		#endif
+	#elif defined( B2_CPU_ARM )
+		#define B2_SIMD_NEON
+		#define B2_SIMD_WIDTH 4
+	#elif defined( B2_CPU_WASM )
+		#define B2_CPU_WASM
+		#define B2_SIMD_SSE2
+		#define B2_SIMD_WIDTH 4
+	#else
+		#define B2_SIMD_NONE
+		#define B2_SIMD_WIDTH 4
+	#endif
+#else
+	#define B2_SIMD_NONE
+	// note: I tried width of 1 and got no performance change
+	#define B2_SIMD_WIDTH 4
+#endif
+
+// Define compiler
+#if defined( __clang__ )
+	#define B2_COMPILER_CLANG
+#elif defined( __GNUC__ )
+	#define B2_COMPILER_GCC
+#elif defined( _MSC_VER )
+	#define B2_COMPILER_MSVC
+#endif
+
+/// Tracy profiler instrumentation
+/// https://github.com/wolfpld/tracy
+#ifdef BOX2D_PROFILE
+	#include <tracy/TracyC.h>
+	#define b2TracyCZoneC( ctx, color, active ) TracyCZoneC( ctx, color, active )
+	#define b2TracyCZoneNC( ctx, name, color, active ) TracyCZoneNC( ctx, name, color, active )
+	#define b2TracyCZoneEnd( ctx ) TracyCZoneEnd( ctx )
+#else
+	#define b2TracyCZoneC( ctx, color, active )
+	#define b2TracyCZoneNC( ctx, name, color, active )
+	#define b2TracyCZoneEnd( ctx )
+#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_UNUSED( ... ) (void)sizeof( ( __VA_ARGS__, 0 ) )
+
+// Use to validate definitions. Do not take my cookie.
+#define B2_SECRET_COOKIE 1152023
+
+// Snoop counters. These should be disabled in optimized builds because they are expensive.
+#define B2_SNOOP_TABLE_COUNTERS B2_DEBUG
+#define B2_SNOOP_PAIR_COUNTERS B2_DEBUG
+#define B2_SNOOP_TOI_COUNTERS B2_DEBUG
+
+#define B2_CHECK_DEF( DEF ) B2_ASSERT( DEF->internalValue == B2_SECRET_COOKIE )
+
+typedef struct b2AtomicInt
+{
+	int value;
+} b2AtomicInt;
+
+typedef struct b2AtomicU32
+{
+	uint32_t value;
+} b2AtomicU32;
+
+void* b2Alloc( int size );
+#define B2_ALLOC_STRUCT( type ) b2Alloc(sizeof(type))
+#define B2_ALLOC_ARRAY( count, type ) b2Alloc(count * sizeof(type))
+
+void b2Free( void* mem, int size );
+#define B2_FREE_STRUCT( mem, type ) b2Free( mem, sizeof(type));
+#define B2_FREE_ARRAY( mem, count, type ) b2Free(mem, count * sizeof(type))
+
+void* b2GrowAlloc( void* oldMem, int oldSize, int newSize );

src/vendor/box2d/ctz.h

@@ -0,0 +1,112 @@
+// SPDX-FileCopyrightText: 2023 Erin Catto
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <stdbool.h>
+#include <stdint.h>
+
+#if defined( _MSC_VER ) && !defined( __clang__ )
+	#include <intrin.h>
+
+// https://en.wikipedia.org/wiki/Find_first_set
+
+static inline uint32_t b2CTZ32( uint32_t block )
+{
+	unsigned long index;
+	_BitScanForward( &index, block );
+	return index;
+}
+
+// This function doesn't need to be fast, so using the Ivy Bridge fallback.
+static inline uint32_t b2CLZ32( uint32_t value )
+{
+	#if 1
+
+	// Use BSR (Bit Scan Reverse) which is available on Ivy Bridge
+	unsigned long index;
+	if ( _BitScanReverse( &index, value ) )
+	{
+		// BSR gives the index of the most significant 1-bit
+		// We need to invert this to get the number of leading zeros
+		return 31 - index;
+	}
+	else
+	{
+		// If x is 0, BSR sets the zero flag and doesn't modify index
+		// LZCNT should return 32 for an input of 0
+		return 32;
+	}
+
+	#else
+
+	return __lzcnt( value );
+
+	#endif
+}
+
+static inline uint32_t b2CTZ64( uint64_t block )
+{
+	unsigned long index;
+
+	#ifdef _WIN64
+	_BitScanForward64( &index, block );
+	#else
+	// 32-bit fall back
+	if ( (uint32_t)block != 0 )
+	{
+		_BitScanForward( &index, (uint32_t)block );
+	}
+	else
+	{
+		_BitScanForward( &index, (uint32_t)( block >> 32 ) );
+		index += 32;
+	}
+	#endif
+
+	return index;
+}
+
+#else
+
+static inline uint32_t b2CTZ32( uint32_t block )
+{
+	return __builtin_ctz( block );
+}
+
+static inline uint32_t b2CLZ32( uint32_t value )
+{
+	return __builtin_clz( value );
+}
+
+static inline uint32_t b2CTZ64( uint64_t block )
+{
+	return __builtin_ctzll( block );
+}
+
+#endif
+
+static inline bool b2IsPowerOf2( int x )
+{
+	return ( x & ( x - 1 ) ) == 0;
+}
+
+static inline int b2BoundingPowerOf2( int x )
+{
+	if ( x <= 1 )
+	{
+		return 1;
+	}
+
+	return 32 - (int)b2CLZ32( (uint32_t)x - 1 );
+}
+
+static inline int b2RoundUpPowerOf2( int x )
+{
+	if ( x <= 1 )
+	{
+		return 1;
+	}
+
+	return 1 << ( 32 - (int)b2CLZ32( (uint32_t)x - 1 ) );
+}

src/vendor/box2d/distance_joint.c

@@ -0,0 +1,556 @@
+// SPDX-FileCopyrightText: 2023 Erin Catto
+// SPDX-License-Identifier: MIT
+
+#if defined( _MSC_VER ) && !defined( _CRT_SECURE_NO_WARNINGS )
+#define _CRT_SECURE_NO_WARNINGS
+#endif
+
+#include "body.h"
+#include "core.h"
+#include "joint.h"
+#include "solver.h"
+#include "solver_set.h"
+#include "world.h"
+
+// needed for dll export
+#include "box2d/box2d.h"
+
+#include <stdio.h>
+
+void b2DistanceJoint_SetLength( b2JointId jointId, float length )
+{
+	b2JointSim* base = b2GetJointSimCheckType( jointId, b2_distanceJoint );
+	b2DistanceJoint* joint = &base->distanceJoint;
+
+	joint->length = b2ClampFloat( length, B2_LINEAR_SLOP, B2_HUGE );
+	joint->impulse = 0.0f;
+	joint->lowerImpulse = 0.0f;
+	joint->upperImpulse = 0.0f;
+}
+
+float b2DistanceJoint_GetLength( b2JointId jointId )
+{
+	b2JointSim* base = b2GetJointSimCheckType( jointId, b2_distanceJoint );
+	b2DistanceJoint* joint = &base->distanceJoint;
+	return joint->length;
+}
+
+void b2DistanceJoint_EnableLimit( b2JointId jointId, bool enableLimit )
+{
+	b2JointSim* base = b2GetJointSimCheckType( jointId, b2_distanceJoint );
+	b2DistanceJoint* joint = &base->distanceJoint;
+	joint->enableLimit = enableLimit;
+}
+
+bool b2DistanceJoint_IsLimitEnabled( b2JointId jointId )
+{
+	b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_distanceJoint );
+	return joint->distanceJoint.enableLimit;
+}
+
+void b2DistanceJoint_SetLengthRange( b2JointId jointId, float minLength, float maxLength )
+{
+	b2JointSim* base = b2GetJointSimCheckType( jointId, b2_distanceJoint );
+	b2DistanceJoint* joint = &base->distanceJoint;
+
+	minLength = b2ClampFloat( minLength, B2_LINEAR_SLOP, B2_HUGE );
+	maxLength = b2ClampFloat( maxLength, B2_LINEAR_SLOP, B2_HUGE );
+	joint->minLength = b2MinFloat( minLength, maxLength );
+	joint->maxLength = b2MaxFloat( minLength, maxLength );
+	joint->impulse = 0.0f;
+	joint->lowerImpulse = 0.0f;
+	joint->upperImpulse = 0.0f;
+}
+
+float b2DistanceJoint_GetMinLength( b2JointId jointId )
+{
+	b2JointSim* base = b2GetJointSimCheckType( jointId, b2_distanceJoint );
+	b2DistanceJoint* joint = &base->distanceJoint;
+	return joint->minLength;
+}
+
+float b2DistanceJoint_GetMaxLength( b2JointId jointId )
+{
+	b2JointSim* base = b2GetJointSimCheckType( jointId, b2_distanceJoint );
+	b2DistanceJoint* joint = &base->distanceJoint;
+	return joint->maxLength;
+}
+
+float b2DistanceJoint_GetCurrentLength( b2JointId jointId )
+{
+	b2JointSim* base = b2GetJointSimCheckType( jointId, b2_distanceJoint );
+
+	b2World* world = b2GetWorld( jointId.world0 );
+	B2_ASSERT( world->locked == false );
+	if ( world->locked )
+	{
+		return 0.0f;
+	}
+
+	b2Transform transformA = b2GetBodyTransform( world, base->bodyIdA );
+	b2Transform transformB = b2GetBodyTransform( world, base->bodyIdB );
+
+	b2Vec2 pA = b2TransformPoint( transformA, base->localOriginAnchorA );
+	b2Vec2 pB = b2TransformPoint( transformB, base->localOriginAnchorB );
+	b2Vec2 d = b2Sub( pB, pA );
+	float length = b2Length( d );
+	return length;
+}
+
+void b2DistanceJoint_EnableSpring( b2JointId jointId, bool enableSpring )
+{
+	b2JointSim* base = b2GetJointSimCheckType( jointId, b2_distanceJoint );
+	base->distanceJoint.enableSpring = enableSpring;
+}
+
+bool b2DistanceJoint_IsSpringEnabled( b2JointId jointId )
+{
+	b2JointSim* base = b2GetJointSimCheckType( jointId, b2_distanceJoint );
+	return base->distanceJoint.enableSpring;
+}
+
+void b2DistanceJoint_SetSpringHertz( b2JointId jointId, float hertz )
+{
+	b2JointSim* base = b2GetJointSimCheckType( jointId, b2_distanceJoint );
+	base->distanceJoint.hertz = hertz;
+}
+
+void b2DistanceJoint_SetSpringDampingRatio( b2JointId jointId, float dampingRatio )
+{
+	b2JointSim* base = b2GetJointSimCheckType( jointId, b2_distanceJoint );
+	base->distanceJoint.dampingRatio = dampingRatio;
+}
+
+float b2DistanceJoint_GetSpringHertz( b2JointId jointId )
+{
+	b2JointSim* base = b2GetJointSimCheckType( jointId, b2_distanceJoint );
+	b2DistanceJoint* joint = &base->distanceJoint;
+	return joint->hertz;
+}
+
+float b2DistanceJoint_GetSpringDampingRatio( b2JointId jointId )
+{
+	b2JointSim* base = b2GetJointSimCheckType( jointId, b2_distanceJoint );
+	b2DistanceJoint* joint = &base->distanceJoint;
+	return joint->dampingRatio;
+}
+
+void b2DistanceJoint_EnableMotor( b2JointId jointId, bool enableMotor )
+{
+	b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_distanceJoint );
+	if ( enableMotor != joint->distanceJoint.enableMotor )
+	{
+		joint->distanceJoint.enableMotor = enableMotor;
+		joint->distanceJoint.motorImpulse = 0.0f;
+	}
+}
+
+bool b2DistanceJoint_IsMotorEnabled( b2JointId jointId )
+{
+	b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_distanceJoint );
+	return joint->distanceJoint.enableMotor;
+}
+
+void b2DistanceJoint_SetMotorSpeed( b2JointId jointId, float motorSpeed )
+{
+	b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_distanceJoint );
+	joint->distanceJoint.motorSpeed = motorSpeed;
+}
+
+float b2DistanceJoint_GetMotorSpeed( b2JointId jointId )
+{
+	b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_distanceJoint );
+	return joint->distanceJoint.motorSpeed;
+}
+
+float b2DistanceJoint_GetMotorForce( b2JointId jointId )
+{
+	b2World* world = b2GetWorld( jointId.world0 );
+	b2JointSim* base = b2GetJointSimCheckType( jointId, b2_distanceJoint );
+	return world->inv_h * base->distanceJoint.motorImpulse;
+}
+
+void b2DistanceJoint_SetMaxMotorForce( b2JointId jointId, float force )
+{
+	b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_distanceJoint );
+	joint->distanceJoint.maxMotorForce = force;
+}
+
+float b2DistanceJoint_GetMaxMotorForce( b2JointId jointId )
+{
+	b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_distanceJoint );
+	return joint->distanceJoint.maxMotorForce;
+}
+
+b2Vec2 b2GetDistanceJointForce( b2World* world, b2JointSim* base )
+{
+	b2DistanceJoint* joint = &base->distanceJoint;
+
+	b2Transform transformA = b2GetBodyTransform( world, base->bodyIdA );
+	b2Transform transformB = b2GetBodyTransform( world, base->bodyIdB );
+
+	b2Vec2 pA = b2TransformPoint( transformA, base->localOriginAnchorA );
+	b2Vec2 pB = b2TransformPoint( transformB, base->localOriginAnchorB );
+	b2Vec2 d = b2Sub( pB, pA );
+	b2Vec2 axis = b2Normalize( d );
+	float force = ( joint->impulse + joint->lowerImpulse - joint->upperImpulse + joint->motorImpulse ) * world->inv_h;
+	return b2MulSV( force, axis );
+}
+
+// 1-D constrained system
+// m (v2 - v1) = lambda
+// v2 + (beta/h) * x1 + gamma * lambda = 0, gamma has units of inverse mass.
+// x2 = x1 + h * v2
+
+// 1-D mass-damper-spring system
+// m (v2 - v1) + h * d * v2 + h * k *
+
+// C = norm(p2 - p1) - L
+// u = (p2 - p1) / norm(p2 - p1)
+// Cdot = dot(u, v2 + cross(w2, r2) - v1 - cross(w1, r1))
+// J = [-u -cross(r1, u) u cross(r2, u)]
+// K = J * invM * JT
+//   = invMass1 + invI1 * cross(r1, u)^2 + invMass2 + invI2 * cross(r2, u)^2
+
+void b2PrepareDistanceJoint( b2JointSim* base, b2StepContext* context )
+{
+	B2_ASSERT( base->type == b2_distanceJoint );
+
+	// chase body id to the solver set where the body lives
+	int idA = base->bodyIdA;
+	int idB = base->bodyIdB;
+
+	b2World* world = context->world;
+	b2Body* bodyA = b2BodyArray_Get( &world->bodies, idA );
+	b2Body* bodyB = b2BodyArray_Get( &world->bodies, idB );
+
+	B2_ASSERT( bodyA->setIndex == b2_awakeSet || bodyB->setIndex == b2_awakeSet );
+
+	b2SolverSet* setA = b2SolverSetArray_Get( &world->solverSets, bodyA->setIndex );
+	b2SolverSet* setB = b2SolverSetArray_Get( &world->solverSets, bodyB->setIndex );
+
+	int localIndexA = bodyA->localIndex;
+	int localIndexB = bodyB->localIndex;
+
+	b2BodySim* bodySimA = b2BodySimArray_Get( &setA->bodySims, localIndexA );
+	b2BodySim* bodySimB = b2BodySimArray_Get( &setB->bodySims, localIndexB );
+
+	float mA = bodySimA->invMass;
+	float iA = bodySimA->invInertia;
+	float mB = bodySimB->invMass;
+	float iB = bodySimB->invInertia;
+
+	base->invMassA = mA;
+	base->invMassB = mB;
+	base->invIA = iA;
+	base->invIB = iB;
+
+	b2DistanceJoint* joint = &base->distanceJoint;
+
+	joint->indexA = bodyA->setIndex == b2_awakeSet ? localIndexA : B2_NULL_INDEX;
+	joint->indexB = bodyB->setIndex == b2_awakeSet ? localIndexB : B2_NULL_INDEX;
+
+	// initial anchors in world space
+	joint->anchorA = b2RotateVector( bodySimA->transform.q, b2Sub( base->localOriginAnchorA, bodySimA->localCenter ) );
+	joint->anchorB = b2RotateVector( bodySimB->transform.q, b2Sub( base->localOriginAnchorB, bodySimB->localCenter ) );
+	joint->deltaCenter = b2Sub( bodySimB->center, bodySimA->center );
+
+	b2Vec2 rA = joint->anchorA;
+	b2Vec2 rB = joint->anchorB;
+	b2Vec2 separation = b2Add( b2Sub( rB, rA ), joint->deltaCenter );
+	b2Vec2 axis = b2Normalize( separation );
+
+	// compute effective mass
+	float crA = b2Cross( rA, axis );
+	float crB = b2Cross( rB, axis );
+	float k = mA + mB + iA * crA * crA + iB * crB * crB;
+	joint->axialMass = k > 0.0f ? 1.0f / k : 0.0f;
+
+	joint->distanceSoftness = b2MakeSoft( joint->hertz, joint->dampingRatio, context->h );
+
+	if ( context->enableWarmStarting == false )
+	{
+		joint->impulse = 0.0f;
+		joint->lowerImpulse = 0.0f;
+		joint->upperImpulse = 0.0f;
+		joint->motorImpulse = 0.0f;
+	}
+}
+
+void b2WarmStartDistanceJoint( b2JointSim* base, b2StepContext* context )
+{
+	B2_ASSERT( base->type == b2_distanceJoint );
+
+	float mA = base->invMassA;
+	float mB = base->invMassB;
+	float iA = base->invIA;
+	float iB = base->invIB;
+
+	// dummy state for static bodies
+	b2BodyState dummyState = b2_identityBodyState;
+
+	b2DistanceJoint* joint = &base->distanceJoint;
+	b2BodyState* stateA = joint->indexA == B2_NULL_INDEX ? &dummyState : context->states + joint->indexA;
+	b2BodyState* stateB = joint->indexB == B2_NULL_INDEX ? &dummyState : context->states + joint->indexB;
+
+	b2Vec2 rA = b2RotateVector( stateA->deltaRotation, joint->anchorA );
+	b2Vec2 rB = b2RotateVector( stateB->deltaRotation, joint->anchorB );
+
+	b2Vec2 ds = b2Add( b2Sub( stateB->deltaPosition, stateA->deltaPosition ), b2Sub( rB, rA ) );
+	b2Vec2 separation = b2Add( joint->deltaCenter, ds );
+	b2Vec2 axis = b2Normalize( separation );
+
+	float axialImpulse = joint->impulse + joint->lowerImpulse - joint->upperImpulse + joint->motorImpulse;
+	b2Vec2 P = b2MulSV( axialImpulse, axis );
+
+	stateA->linearVelocity = b2MulSub( stateA->linearVelocity, mA, P );
+	stateA->angularVelocity -= iA * b2Cross( rA, P );
+	stateB->linearVelocity = b2MulAdd( stateB->linearVelocity, mB, P );
+	stateB->angularVelocity += iB * b2Cross( rB, P );
+}
+
+void b2SolveDistanceJoint( b2JointSim* base, b2StepContext* context, bool useBias )
+{
+	B2_ASSERT( base->type == b2_distanceJoint );
+
+	float mA = base->invMassA;
+	float mB = base->invMassB;
+	float iA = base->invIA;
+	float iB = base->invIB;
+
+	// dummy state for static bodies
+	b2BodyState dummyState = b2_identityBodyState;
+
+	b2DistanceJoint* joint = &base->distanceJoint;
+	b2BodyState* stateA = joint->indexA == B2_NULL_INDEX ? &dummyState : context->states + joint->indexA;
+	b2BodyState* stateB = joint->indexB == B2_NULL_INDEX ? &dummyState : context->states + joint->indexB;
+
+	b2Vec2 vA = stateA->linearVelocity;
+	float wA = stateA->angularVelocity;
+	b2Vec2 vB = stateB->linearVelocity;
+	float wB = stateB->angularVelocity;
+
+	// current anchors
+	b2Vec2 rA = b2RotateVector( stateA->deltaRotation, joint->anchorA );
+	b2Vec2 rB = b2RotateVector( stateB->deltaRotation, joint->anchorB );
+
+	// current separation
+	b2Vec2 ds = b2Add( b2Sub( stateB->deltaPosition, stateA->deltaPosition ), b2Sub( rB, rA ) );
+	b2Vec2 separation = b2Add( joint->deltaCenter, ds );
+
+	float length = b2Length( separation );
+	b2Vec2 axis = b2Normalize( separation );
+
+	// joint is soft if
+	// - spring is enabled
+	// - and (joint limit is disabled or limits are not equal)
+	if ( joint->enableSpring && ( joint->minLength < joint->maxLength || joint->enableLimit == false ) )
+	{
+		// spring
+		if ( joint->hertz > 0.0f )
+		{
+			// Cdot = dot(u, v + cross(w, r))
+			b2Vec2 vr = b2Add( b2Sub( vB, vA ), b2Sub( b2CrossSV( wB, rB ), b2CrossSV( wA, rA ) ) );
+			float Cdot = b2Dot( axis, vr );
+			float C = length - joint->length;
+			float bias = joint->distanceSoftness.biasRate * C;
+
+			float m = joint->distanceSoftness.massScale * joint->axialMass;
+			float impulse = -m * ( Cdot + bias ) - joint->distanceSoftness.impulseScale * joint->impulse;
+			joint->impulse += impulse;
+
+			b2Vec2 P = b2MulSV( impulse, axis );
+			vA = b2MulSub( vA, mA, P );
+			wA -= iA * b2Cross( rA, P );
+			vB = b2MulAdd( vB, mB, P );
+			wB += iB * b2Cross( rB, P );
+		}
+
+		if ( joint->enableLimit )
+		{
+			// lower limit
+			{
+				b2Vec2 vr = b2Add( b2Sub( vB, vA ), b2Sub( b2CrossSV( wB, rB ), b2CrossSV( wA, rA ) ) );
+				float Cdot = b2Dot( axis, vr );
+
+				float C = length - joint->minLength;
+
+				float bias = 0.0f;
+				float massCoeff = 1.0f;
+				float impulseCoeff = 0.0f;
+				if ( C > 0.0f )
+				{
+					// speculative
+					bias = C * context->inv_h;
+				}
+				else if ( useBias )
+				{
+					bias = context->jointSoftness.biasRate * C;
+					massCoeff = context->jointSoftness.massScale;
+					impulseCoeff = context->jointSoftness.impulseScale;
+				}
+
+				float impulse = -massCoeff * joint->axialMass * ( Cdot + bias ) - impulseCoeff * joint->lowerImpulse;
+				float newImpulse = b2MaxFloat( 0.0f, joint->lowerImpulse + impulse );
+				impulse = newImpulse - joint->lowerImpulse;
+				joint->lowerImpulse = newImpulse;
+
+				b2Vec2 P = b2MulSV( impulse, axis );
+				vA = b2MulSub( vA, mA, P );
+				wA -= iA * b2Cross( rA, P );
+				vB = b2MulAdd( vB, mB, P );
+				wB += iB * b2Cross( rB, P );
+			}
+
+			// upper
+			{
+				b2Vec2 vr = b2Add( b2Sub( vA, vB ), b2Sub( b2CrossSV( wA, rA ), b2CrossSV( wB, rB ) ) );
+				float Cdot = b2Dot( axis, vr );
+
+				float C = joint->maxLength - length;
+
+				float bias = 0.0f;
+				float massScale = 1.0f;
+				float impulseScale = 0.0f;
+				if ( C > 0.0f )
+				{
+					// speculative
+					bias = C * context->inv_h;
+				}
+				else if ( useBias )
+				{
+					bias = context->jointSoftness.biasRate * C;
+					massScale = context->jointSoftness.massScale;
+					impulseScale = context->jointSoftness.impulseScale;
+				}
+
+				float impulse = -massScale * joint->axialMass * ( Cdot + bias ) - impulseScale * joint->upperImpulse;
+				float newImpulse = b2MaxFloat( 0.0f, joint->upperImpulse + impulse );
+				impulse = newImpulse - joint->upperImpulse;
+				joint->upperImpulse = newImpulse;
+
+				b2Vec2 P = b2MulSV( -impulse, axis );
+				vA = b2MulSub( vA, mA, P );
+				wA -= iA * b2Cross( rA, P );
+				vB = b2MulAdd( vB, mB, P );
+				wB += iB * b2Cross( rB, P );
+			}
+		}
+
+		if ( joint->enableMotor )
+		{
+			b2Vec2 vr = b2Add( b2Sub( vB, vA ), b2Sub( b2CrossSV( wB, rB ), b2CrossSV( wA, rA ) ) );
+			float Cdot = b2Dot( axis, vr );
+			float impulse = joint->axialMass * ( joint->motorSpeed - Cdot );
+			float oldImpulse = joint->motorImpulse;
+			float maxImpulse = context->h * joint->maxMotorForce;
+			joint->motorImpulse = b2ClampFloat( joint->motorImpulse + impulse, -maxImpulse, maxImpulse );
+			impulse = joint->motorImpulse - oldImpulse;
+
+			b2Vec2 P = b2MulSV( impulse, axis );
+			vA = b2MulSub( vA, mA, P );
+			wA -= iA * b2Cross( rA, P );
+			vB = b2MulAdd( vB, mB, P );
+			wB += iB * b2Cross( rB, P );
+		}
+	}
+	else
+	{
+		// rigid constraint
+		b2Vec2 vr = b2Add( b2Sub( vB, vA ), b2Sub( b2CrossSV( wB, rB ), b2CrossSV( wA, rA ) ) );
+		float Cdot = b2Dot( axis, vr );
+
+		float C = length - joint->length;
+
+		float bias = 0.0f;
+		float massScale = 1.0f;
+		float impulseScale = 0.0f;
+		if ( useBias )
+		{
+			bias = context->jointSoftness.biasRate * C;
+			massScale = context->jointSoftness.massScale;
+			impulseScale = context->jointSoftness.impulseScale;
+		}
+
+		float impulse = -massScale * joint->axialMass * ( Cdot + bias ) - impulseScale * joint->impulse;
+		joint->impulse += impulse;
+
+		b2Vec2 P = b2MulSV( impulse, axis );
+		vA = b2MulSub( vA, mA, P );
+		wA -= iA * b2Cross( rA, P );
+		vB = b2MulAdd( vB, mB, P );
+		wB += iB * b2Cross( rB, P );
+	}
+
+	stateA->linearVelocity = vA;
+	stateA->angularVelocity = wA;
+	stateB->linearVelocity = vB;
+	stateB->angularVelocity = wB;
+}
+
+#if 0
+void b2DistanceJoint::Dump()
+{
+	int32 indexA = m_bodyA->m_islandIndex;
+	int32 indexB = m_bodyB->m_islandIndex;
+
+	b2Dump("  b2DistanceJointDef jd;\n");
+	b2Dump("  jd.bodyA = sims[%d];\n", indexA);
+	b2Dump("  jd.bodyB = sims[%d];\n", indexB);
+	b2Dump("  jd.collideConnected = bool(%d);\n", m_collideConnected);
+	b2Dump("  jd.localAnchorA.Set(%.9g, %.9g);\n", m_localAnchorA.x, m_localAnchorA.y);
+	b2Dump("  jd.localAnchorB.Set(%.9g, %.9g);\n", m_localAnchorB.x, m_localAnchorB.y);
+	b2Dump("  jd.length = %.9g;\n", m_length);
+	b2Dump("  jd.minLength = %.9g;\n", m_minLength);
+	b2Dump("  jd.maxLength = %.9g;\n", m_maxLength);
+	b2Dump("  jd.stiffness = %.9g;\n", m_stiffness);
+	b2Dump("  jd.damping = %.9g;\n", m_damping);
+	b2Dump("  joints[%d] = m_world->CreateJoint(&jd);\n", m_index);
+}
+#endif
+
+void b2DrawDistanceJoint( b2DebugDraw* draw, b2JointSim* base, b2Transform transformA, b2Transform transformB )
+{
+	B2_ASSERT( base->type == b2_distanceJoint );
+
+	b2DistanceJoint* joint = &base->distanceJoint;
+
+	b2Vec2 pA = b2TransformPoint( transformA, base->localOriginAnchorA );
+	b2Vec2 pB = b2TransformPoint( transformB, base->localOriginAnchorB );
+
+	b2Vec2 axis = b2Normalize( b2Sub( pB, pA ) );
+
+	if ( joint->minLength < joint->maxLength && joint->enableLimit )
+	{
+		b2Vec2 pMin = b2MulAdd( pA, joint->minLength, axis );
+		b2Vec2 pMax = b2MulAdd( pA, joint->maxLength, axis );
+		b2Vec2 offset = b2MulSV( 0.05f * b2_lengthUnitsPerMeter, b2RightPerp( axis ) );
+
+		if ( joint->minLength > B2_LINEAR_SLOP )
+		{
+			// draw->DrawPoint(pMin, 4.0f, c2, draw->context);
+			draw->DrawSegmentFcn( b2Sub( pMin, offset ), b2Add( pMin, offset ), b2_colorLightGreen, draw->context );
+		}
+
+		if ( joint->maxLength < B2_HUGE )
+		{
+			// draw->DrawPoint(pMax, 4.0f, c3, draw->context);
+			draw->DrawSegmentFcn( b2Sub( pMax, offset ), b2Add( pMax, offset ), b2_colorRed, draw->context );
+		}
+
+		if ( joint->minLength > B2_LINEAR_SLOP && joint->maxLength < B2_HUGE )
+		{
+			draw->DrawSegmentFcn( pMin, pMax, b2_colorGray, draw->context );
+		}
+	}
+
+	draw->DrawSegmentFcn( pA, pB, b2_colorWhite, draw->context );
+	draw->DrawPointFcn( pA, 4.0f, b2_colorWhite, draw->context );
+	draw->DrawPointFcn( pB, 4.0f, b2_colorWhite, draw->context );
+
+	if ( joint->hertz > 0.0f && joint->enableSpring )
+	{
+		b2Vec2 pRest = b2MulAdd( pA, joint->length, axis );
+		draw->DrawPointFcn( pRest, 4.0f, b2_colorBlue, draw->context );
+	}
+}

src/vendor/box2d/hull.c

@@ -0,0 +1,328 @@
+// SPDX-FileCopyrightText: 2023 Erin Catto
+// SPDX-License-Identifier: MIT
+
+#include "constants.h"
+#include "core.h"
+
+#include "box2d/collision.h"
+#include "box2d/math_functions.h"
+
+#include <float.h>
+
+// quickhull recursion
+static b2Hull b2RecurseHull( b2Vec2 p1, b2Vec2 p2, b2Vec2* ps, int count )
+{
+	b2Hull hull;
+	hull.count = 0;
+
+	if ( count == 0 )
+	{
+		return hull;
+	}
+
+	// create an edge vector pointing from p1 to p2
+	b2Vec2 e = b2Normalize( b2Sub( p2, p1 ) );
+
+	// discard points left of e and find point furthest to the right of e
+	b2Vec2 rightPoints[B2_MAX_POLYGON_VERTICES];
+	int rightCount = 0;
+
+	int bestIndex = 0;
+	float bestDistance = b2Cross( b2Sub( ps[bestIndex], p1 ), e );
+	if ( bestDistance > 0.0f )
+	{
+		rightPoints[rightCount++] = ps[bestIndex];
+	}
+
+	for ( int i = 1; i < count; ++i )
+	{
+		float distance = b2Cross( b2Sub( ps[i], p1 ), e );
+		if ( distance > bestDistance )
+		{
+			bestIndex = i;
+			bestDistance = distance;
+		}
+
+		if ( distance > 0.0f )
+		{
+			rightPoints[rightCount++] = ps[i];
+		}
+	}
+
+	if ( bestDistance < 2.0f * B2_LINEAR_SLOP )
+	{
+		return hull;
+	}
+
+	b2Vec2 bestPoint = ps[bestIndex];
+
+	// compute hull to the right of p1-bestPoint
+	b2Hull hull1 = b2RecurseHull( p1, bestPoint, rightPoints, rightCount );
+
+	// compute hull to the right of bestPoint-p2
+	b2Hull hull2 = b2RecurseHull( bestPoint, p2, rightPoints, rightCount );
+
+	// stitch together hulls
+	for ( int i = 0; i < hull1.count; ++i )
+	{
+		hull.points[hull.count++] = hull1.points[i];
+	}
+
+	hull.points[hull.count++] = bestPoint;
+
+	for ( int i = 0; i < hull2.count; ++i )
+	{
+		hull.points[hull.count++] = hull2.points[i];
+	}
+
+	B2_ASSERT( hull.count < B2_MAX_POLYGON_VERTICES );
+
+	return hull;
+}
+
+// quickhull algorithm
+// - merges vertices based on B2_LINEAR_SLOP
+// - removes collinear points using B2_LINEAR_SLOP
+// - returns an empty hull if it fails
+b2Hull b2ComputeHull( const b2Vec2* points, int count )
+{
+	b2Hull hull;
+	hull.count = 0;
+
+	if ( count < 3 || count > B2_MAX_POLYGON_VERTICES )
+	{
+		// check your data
+		return hull;
+	}
+
+	count = b2MinInt( count, B2_MAX_POLYGON_VERTICES );
+
+	b2AABB aabb = { { FLT_MAX, FLT_MAX }, { -FLT_MAX, -FLT_MAX } };
+
+	// Perform aggressive point welding. First point always remains.
+	// Also compute the bounding box for later.
+	b2Vec2 ps[B2_MAX_POLYGON_VERTICES];
+	int n = 0;
+	const float linearSlop = B2_LINEAR_SLOP;
+	const float tolSqr = 16.0f * linearSlop * linearSlop;
+	for ( int i = 0; i < count; ++i )
+	{
+		aabb.lowerBound = b2Min( aabb.lowerBound, points[i] );
+		aabb.upperBound = b2Max( aabb.upperBound, points[i] );
+
+		b2Vec2 vi = points[i];
+
+		bool unique = true;
+		for ( int j = 0; j < i; ++j )
+		{
+			b2Vec2 vj = points[j];
+
+			float distSqr = b2DistanceSquared( vi, vj );
+			if ( distSqr < tolSqr )
+			{
+				unique = false;
+				break;
+			}
+		}
+
+		if ( unique )
+		{
+			ps[n++] = vi;
+		}
+	}
+
+	if ( n < 3 )
+	{
+		// all points very close together, check your data and check your scale
+		return hull;
+	}
+
+	// Find an extreme point as the first point on the hull
+	b2Vec2 c = b2AABB_Center( aabb );
+	int f1 = 0;
+	float dsq1 = b2DistanceSquared( c, ps[f1] );
+	for ( int i = 1; i < n; ++i )
+	{
+		float dsq = b2DistanceSquared( c, ps[i] );
+		if ( dsq > dsq1 )
+		{
+			f1 = i;
+			dsq1 = dsq;
+		}
+	}
+
+	// remove p1 from working set
+	b2Vec2 p1 = ps[f1];
+	ps[f1] = ps[n - 1];
+	n = n - 1;
+
+	int f2 = 0;
+	float dsq2 = b2DistanceSquared( p1, ps[f2] );
+	for ( int i = 1; i < n; ++i )
+	{
+		float dsq = b2DistanceSquared( p1, ps[i] );
+		if ( dsq > dsq2 )
+		{
+			f2 = i;
+			dsq2 = dsq;
+		}
+	}
+
+	// remove p2 from working set
+	b2Vec2 p2 = ps[f2];
+	ps[f2] = ps[n - 1];
+	n = n - 1;
+
+	// split the points into points that are left and right of the line p1-p2.
+	b2Vec2 rightPoints[B2_MAX_POLYGON_VERTICES - 2];
+	int rightCount = 0;
+
+	b2Vec2 leftPoints[B2_MAX_POLYGON_VERTICES - 2];
+	int leftCount = 0;
+
+	b2Vec2 e = b2Normalize( b2Sub( p2, p1 ) );
+
+	for ( int i = 0; i < n; ++i )
+	{
+		float d = b2Cross( b2Sub( ps[i], p1 ), e );
+
+		// slop used here to skip points that are very close to the line p1-p2
+		if ( d >= 2.0f * linearSlop )
+		{
+			rightPoints[rightCount++] = ps[i];
+		}
+		else if ( d <= -2.0f * linearSlop )
+		{
+			leftPoints[leftCount++] = ps[i];
+		}
+	}
+
+	// compute hulls on right and left
+	b2Hull hull1 = b2RecurseHull( p1, p2, rightPoints, rightCount );
+	b2Hull hull2 = b2RecurseHull( p2, p1, leftPoints, leftCount );
+
+	if ( hull1.count == 0 && hull2.count == 0 )
+	{
+		// all points collinear
+		return hull;
+	}
+
+	// stitch hulls together, preserving CCW winding order
+	hull.points[hull.count++] = p1;
+
+	for ( int i = 0; i < hull1.count; ++i )
+	{
+		hull.points[hull.count++] = hull1.points[i];
+	}
+
+	hull.points[hull.count++] = p2;
+
+	for ( int i = 0; i < hull2.count; ++i )
+	{
+		hull.points[hull.count++] = hull2.points[i];
+	}
+
+	B2_ASSERT( hull.count <= B2_MAX_POLYGON_VERTICES );
+
+	// merge collinear
+	bool searching = true;
+	while ( searching && hull.count > 2 )
+	{
+		searching = false;
+
+		for ( int i = 0; i < hull.count; ++i )
+		{
+			int i1 = i;
+			int i2 = ( i + 1 ) % hull.count;
+			int i3 = ( i + 2 ) % hull.count;
+
+			b2Vec2 s1 = hull.points[i1];
+			b2Vec2 s2 = hull.points[i2];
+			b2Vec2 s3 = hull.points[i3];
+
+			// unit edge vector for s1-s3
+			b2Vec2 r = b2Normalize( b2Sub( s3, s1 ) );
+
+			float distance = b2Cross( b2Sub( s2, s1 ), r );
+			if ( distance <= 2.0f * linearSlop )
+			{
+				// remove midpoint from hull
+				for ( int j = i2; j < hull.count - 1; ++j )
+				{
+					hull.points[j] = hull.points[j + 1];
+				}
+				hull.count -= 1;
+
+				// continue searching for collinear points
+				searching = true;
+
+				break;
+			}
+		}
+	}
+
+	if ( hull.count < 3 )
+	{
+		// all points collinear, shouldn't be reached since this was validated above
+		hull.count = 0;
+	}
+
+	return hull;
+}
+
+bool b2ValidateHull( const b2Hull* hull )
+{
+	if ( hull->count < 3 || B2_MAX_POLYGON_VERTICES < hull->count )
+	{
+		return false;
+	}
+
+	// test that every point is behind every edge
+	for ( int i = 0; i < hull->count; ++i )
+	{
+		// create an edge vector
+		int i1 = i;
+		int i2 = i < hull->count - 1 ? i1 + 1 : 0;
+		b2Vec2 p = hull->points[i1];
+		b2Vec2 e = b2Normalize( b2Sub( hull->points[i2], p ) );
+
+		for ( int j = 0; j < hull->count; ++j )
+		{
+			// skip points that subtend the current edge
+			if ( j == i1 || j == i2 )
+			{
+				continue;
+			}
+
+			float distance = b2Cross( b2Sub( hull->points[j], p ), e );
+			if ( distance >= 0.0f )
+			{
+				return false;
+			}
+		}
+	}
+
+	// test for collinear points
+	const float linearSlop = B2_LINEAR_SLOP;
+	for ( int i = 0; i < hull->count; ++i )
+	{
+		int i1 = i;
+		int i2 = ( i + 1 ) % hull->count;
+		int i3 = ( i + 2 ) % hull->count;
+
+		b2Vec2 p1 = hull->points[i1];
+		b2Vec2 p2 = hull->points[i2];
+		b2Vec2 p3 = hull->points[i3];
+
+		b2Vec2 e = b2Normalize( b2Sub( p3, p1 ) );
+
+		float distance = b2Cross( b2Sub( p2, p1 ), e );
+		if ( distance <= linearSlop )
+		{
+			// p1-p2-p3 are collinear
+			return false;
+		}
+	}
+
+	return true;
+}

src/vendor/box2d/id.h

@@ -0,0 +1,144 @@
+// SPDX-FileCopyrightText: 2023 Erin Catto
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include "base.h"
+
+#include <stdint.h>
+
+/**
+ * @defgroup id Ids
+ * These ids serve as handles to internal Box2D objects.
+ * These should be considered opaque data and passed by value.
+ * Include this header if you need the id types and not the whole Box2D API.
+ * All ids are considered null if initialized to zero.
+ *
+ * For example in C++:
+ *
+ * @code{.cxx}
+ * b2WorldId worldId = {};
+ * @endcode
+ *
+ * Or in C:
+ *
+ * @code{.c}
+ * b2WorldId worldId = {0};
+ * @endcode
+ *
+ * These are both considered null.
+ *
+ * @warning Do not use the internals of these ids. They are subject to change. Ids should be treated as opaque objects.
+ * @warning You should use ids to access objects in Box2D. Do not access files within the src folder. Such usage is unsupported.
+ * @{
+ */
+
+/// World id references a world instance. This should be treated as an opaque handle.
+typedef struct b2WorldId
+{
+	uint16_t index1;
+	uint16_t generation;
+} b2WorldId;
+
+/// Body id references a body instance. This should be treated as an opaque handle.
+typedef struct b2BodyId
+{
+	int32_t index1;
+	uint16_t world0;
+	uint16_t generation;
+} b2BodyId;
+
+/// Shape id references a shape instance. This should be treated as an opaque handle.
+typedef struct b2ShapeId
+{
+	int32_t index1;
+	uint16_t world0;
+	uint16_t generation;
+} b2ShapeId;
+
+/// Chain id references a chain instances. This should be treated as an opaque handle.
+typedef struct b2ChainId
+{
+	int32_t index1;
+	uint16_t world0;
+	uint16_t generation;
+} b2ChainId;
+
+/// Joint id references a joint instance. This should be treated as an opaque handle.
+typedef struct b2JointId
+{
+	int32_t index1;
+	uint16_t world0;
+	uint16_t generation;
+} b2JointId;
+
+/// Use these to make your identifiers null.
+/// You may also use zero initialization to get null.
+static const b2WorldId b2_nullWorldId = B2_ZERO_INIT;
+static const b2BodyId b2_nullBodyId = B2_ZERO_INIT;
+static const b2ShapeId b2_nullShapeId = B2_ZERO_INIT;
+static const b2ChainId b2_nullChainId = B2_ZERO_INIT;
+static const b2JointId b2_nullJointId = B2_ZERO_INIT;
+
+/// Macro to determine if any id is null.
+#define B2_IS_NULL( id ) ( id.index1 == 0 )
+
+/// Macro to determine if any id is non-null.
+#define B2_IS_NON_NULL( id ) ( id.index1 != 0 )
+
+/// Compare two ids for equality. Doesn't work for b2WorldId.
+#define B2_ID_EQUALS( id1, id2 ) ( id1.index1 == id2.index1 && id1.world0 == id2.world0 && id1.generation == id2.generation )
+
+/// Store a body id into a uint64_t.
+B2_INLINE uint64_t b2StoreBodyId( b2BodyId id )
+{
+	return ( (uint64_t)id.index1 << 32 ) | ( (uint64_t)id.world0 ) << 16 | (uint64_t)id.generation;
+}
+
+/// Load a uint64_t into a body id.
+B2_INLINE b2BodyId b2LoadBodyId( uint64_t x )
+{
+	b2BodyId id = { (int32_t)( x >> 32 ), (uint16_t)( x >> 16 ), (uint16_t)( x ) };
+	return id;
+}
+
+/// Store a shape id into a uint64_t.
+B2_INLINE uint64_t b2StoreShapeId( b2ShapeId id )
+{
+	return ( (uint64_t)id.index1 << 32 ) | ( (uint64_t)id.world0 ) << 16 | (uint64_t)id.generation;
+}
+
+/// Load a uint64_t into a shape id.
+B2_INLINE b2ShapeId b2LoadShapeId( uint64_t x )
+{
+	b2ShapeId id = { (int32_t)( x >> 32 ), (uint16_t)( x >> 16 ), (uint16_t)( x ) };
+	return id;
+}
+
+/// Store a chain id into a uint64_t.
+B2_INLINE uint64_t b2StoreChainId( b2ChainId id )
+{
+	return ( (uint64_t)id.index1 << 32 ) | ( (uint64_t)id.world0 ) << 16 | (uint64_t)id.generation;
+}
+
+/// Load a uint64_t into a chain id.
+B2_INLINE b2ChainId b2LoadChainId( uint64_t x )
+{
+	b2ChainId id = { (int32_t)( x >> 32 ), (uint16_t)( x >> 16 ), (uint16_t)( x ) };
+	return id;
+}
+
+/// Store a joint id into a uint64_t.
+B2_INLINE uint64_t b2StoreJointId( b2JointId id )
+{
+	return ( (uint64_t)id.index1 << 32 ) | ( (uint64_t)id.world0 ) << 16 | (uint64_t)id.generation;
+}
+
+/// Load a uint64_t into a joint id.
+B2_INLINE b2JointId b2LoadJointId( uint64_t x )
+{
+	b2JointId id = { (int32_t)( x >> 32 ), (uint16_t)( x >> 16 ), (uint16_t)( x ) };
+	return id;
+}
+
+/**@}*/

src/vendor/box2d/id_pool.c

@@ -0,0 +1,79 @@
+// SPDX-FileCopyrightText: 2023 Erin Catto
+// SPDX-License-Identifier: MIT
+
+#include "id_pool.h"
+
+b2IdPool b2CreateIdPool( void )
+{
+	b2IdPool pool = { 0 };
+	pool.freeArray = b2IntArray_Create( 32 );
+	return pool;
+}
+
+void b2DestroyIdPool( b2IdPool* pool )
+{
+	b2IntArray_Destroy( &pool->freeArray );
+	*pool = ( b2IdPool ){ 0 };
+}
+
+int b2AllocId( b2IdPool* pool )
+{
+	int count = pool->freeArray.count;
+	if ( count > 0 )
+	{
+		int id = b2IntArray_Pop( &pool->freeArray );
+		return id;
+	}
+
+	int id = pool->nextIndex;
+	pool->nextIndex += 1;
+	return id;
+}
+
+void b2FreeId( b2IdPool* pool, int id )
+{
+	B2_ASSERT( pool->nextIndex > 0 );
+	B2_ASSERT( 0 <= id && id < pool->nextIndex );
+	b2IntArray_Push( &pool->freeArray, id );
+}
+
+#if B2_VALIDATE
+
+void b2ValidateFreeId( b2IdPool* pool, int id )
+{
+	int freeCount = pool->freeArray.count;
+	for ( int i = 0; i < freeCount; ++i )
+	{
+		if ( pool->freeArray.data[i] == id )
+		{
+			return;
+		}
+	}
+
+	B2_ASSERT( 0 );
+}
+
+void b2ValidateUsedId( b2IdPool* pool, int id )
+{
+	int freeCount = pool->freeArray.count;
+	for ( int i = 0; i < freeCount; ++i )
+	{
+		if ( pool->freeArray.data[i] == id )
+		{
+			B2_ASSERT( 0 );
+		}
+	}
+}
+
+#else
+
+void b2ValidateFreeId( b2IdPool* pool, int id )
+{
+	B2_UNUSED( pool, id );
+}
+
+void b2ValidateUsedId( b2IdPool* pool, int id )
+{
+	B2_UNUSED( pool, id );
+}
+#endif

src/vendor/box2d/id_pool.h

@@ -0,0 +1,35 @@
+// SPDX-FileCopyrightText: 2023 Erin Catto
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include "array.h"
+
+typedef struct b2IdPool
+{
+	b2IntArray freeArray;
+	int nextIndex;
+} b2IdPool;
+
+b2IdPool b2CreateIdPool( void );
+void b2DestroyIdPool( b2IdPool* pool );
+
+int b2AllocId( b2IdPool* pool );
+void b2FreeId( b2IdPool* pool, int id );
+void b2ValidateFreeId( b2IdPool* pool, int id );
+void b2ValidateUsedId( b2IdPool* pool, int id );
+
+static inline int b2GetIdCount( b2IdPool* pool )
+{
+	return pool->nextIndex - pool->freeArray.count;
+}
+
+static inline int b2GetIdCapacity( b2IdPool* pool )
+{
+	return pool->nextIndex;
+}
+
+static inline int b2GetIdBytes( b2IdPool* pool )
+{
+	return b2IntArray_ByteCount(&pool->freeArray);
+}

src/vendor/box2d/island.c

@@ -0,0 +1,977 @@
+// SPDX-FileCopyrightText: 2023 Erin Catto
+// SPDX-License-Identifier: MIT
+
+#include "island.h"
+
+#include "body.h"
+#include "contact.h"
+#include "core.h"
+#include "joint.h"
+#include "solver_set.h"
+#include "world.h"
+
+#include <stddef.h>
+
+B2_ARRAY_SOURCE( b2Island, b2Island )
+B2_ARRAY_SOURCE( b2IslandSim, b2IslandSim )
+
+b2Island* b2CreateIsland( b2World* world, int setIndex )
+{
+	B2_ASSERT( setIndex == b2_awakeSet || setIndex >= b2_firstSleepingSet );
+
+	int islandId = b2AllocId( &world->islandIdPool );
+
+	if ( islandId == world->islands.count )
+	{
+		b2Island emptyIsland = { 0 };
+		b2IslandArray_Push( &world->islands, emptyIsland );
+	}
+	else
+	{
+		B2_ASSERT( world->islands.data[islandId].setIndex == B2_NULL_INDEX );
+	}
+
+	b2SolverSet* set = b2SolverSetArray_Get( &world->solverSets, setIndex );
+
+	b2Island* island = b2IslandArray_Get( &world->islands, islandId );
+	island->setIndex = setIndex;
+	island->localIndex = set->islandSims.count;
+	island->islandId = islandId;
+	island->headBody = B2_NULL_INDEX;
+	island->tailBody = B2_NULL_INDEX;
+	island->bodyCount = 0;
+	island->headContact = B2_NULL_INDEX;
+	island->tailContact = B2_NULL_INDEX;
+	island->contactCount = 0;
+	island->headJoint = B2_NULL_INDEX;
+	island->tailJoint = B2_NULL_INDEX;
+	island->jointCount = 0;
+	island->parentIsland = B2_NULL_INDEX;
+	island->constraintRemoveCount = 0;
+
+	b2IslandSim* islandSim = b2IslandSimArray_Add( &set->islandSims );
+	islandSim->islandId = islandId;
+
+	return island;
+}
+
+void b2DestroyIsland( b2World* world, int islandId )
+{
+	if (world->splitIslandId == islandId)
+	{
+		world->splitIslandId = B2_NULL_INDEX;
+	}
+
+	// assume island is empty
+	b2Island* island = b2IslandArray_Get( &world->islands, islandId );
+	b2SolverSet* set = b2SolverSetArray_Get( &world->solverSets, island->setIndex );
+	int movedIndex = b2IslandSimArray_RemoveSwap( &set->islandSims, island->localIndex );
+	if ( movedIndex != B2_NULL_INDEX )
+	{
+		// Fix index on moved element
+		b2IslandSim* movedElement = set->islandSims.data + island->localIndex;
+		int movedId = movedElement->islandId;
+		b2Island* movedIsland = b2IslandArray_Get( &world->islands, movedId );
+		B2_ASSERT( movedIsland->localIndex == movedIndex );
+		movedIsland->localIndex = island->localIndex;
+	}
+
+	// Free island and id (preserve island revision)
+	island->islandId = B2_NULL_INDEX;
+	island->setIndex = B2_NULL_INDEX;
+	island->localIndex = B2_NULL_INDEX;
+	b2FreeId( &world->islandIdPool, islandId );
+}
+
+static void b2AddContactToIsland( b2World* world, int islandId, b2Contact* contact )
+{
+	B2_ASSERT( contact->islandId == B2_NULL_INDEX );
+	B2_ASSERT( contact->islandPrev == B2_NULL_INDEX );
+	B2_ASSERT( contact->islandNext == B2_NULL_INDEX );
+
+	b2Island* island = b2IslandArray_Get( &world->islands, islandId );
+
+	if ( island->headContact != B2_NULL_INDEX )
+	{
+		contact->islandNext = island->headContact;
+		b2Contact* headContact = b2ContactArray_Get( &world->contacts, island->headContact);
+		headContact->islandPrev = contact->contactId;
+	}
+
+	island->headContact = contact->contactId;
+	if ( island->tailContact == B2_NULL_INDEX )
+	{
+		island->tailContact = island->headContact;
+	}
+
+	island->contactCount += 1;
+	contact->islandId = islandId;
+
+	b2ValidateIsland( world, islandId );
+}
+
+// Link a contact into an island.
+// This performs union-find and path compression to join islands.
+// https://en.wikipedia.org/wiki/Disjoint-set_data_structure
+void b2LinkContact( b2World* world, b2Contact* contact )
+{
+	B2_ASSERT( ( contact->flags & b2_contactTouchingFlag ) != 0 );
+
+	int bodyIdA = contact->edges[0].bodyId;
+	int bodyIdB = contact->edges[1].bodyId;
+
+	b2Body* bodyA = b2BodyArray_Get( &world->bodies, bodyIdA );
+	b2Body* bodyB = b2BodyArray_Get( &world->bodies, bodyIdB );
+
+	B2_ASSERT( bodyA->setIndex != b2_disabledSet && bodyB->setIndex != b2_disabledSet );
+	B2_ASSERT( bodyA->setIndex != b2_staticSet || bodyB->setIndex != b2_staticSet );
+
+	// Wake bodyB if bodyA is awake and bodyB is sleeping
+	if ( bodyA->setIndex == b2_awakeSet && bodyB->setIndex >= b2_firstSleepingSet )
+	{
+		b2WakeSolverSet( world, bodyB->setIndex );
+	}
+
+	// Wake bodyA if bodyB is awake and bodyA is sleeping
+	if ( bodyB->setIndex == b2_awakeSet && bodyA->setIndex >= b2_firstSleepingSet )
+	{
+		b2WakeSolverSet( world, bodyA->setIndex );
+	}
+
+	int islandIdA = bodyA->islandId;
+	int islandIdB = bodyB->islandId;
+
+	// Static bodies have null island indices.
+	B2_ASSERT( bodyA->setIndex != b2_staticSet || islandIdA == B2_NULL_INDEX );
+	B2_ASSERT( bodyB->setIndex != b2_staticSet || islandIdB == B2_NULL_INDEX );
+	B2_ASSERT( islandIdA != B2_NULL_INDEX || islandIdB != B2_NULL_INDEX );
+
+	if ( islandIdA == islandIdB )
+	{
+		// Contact in same island
+		b2AddContactToIsland( world, islandIdA, contact );
+		return;
+	}
+
+	// Union-find root of islandA
+	b2Island* islandA = NULL;
+	if ( islandIdA != B2_NULL_INDEX )
+	{
+		islandA = b2IslandArray_Get( &world->islands, islandIdA );
+		int parentId = islandA->parentIsland;
+		while ( parentId != B2_NULL_INDEX )
+		{
+			b2Island* parent = b2IslandArray_Get( &world->islands, parentId );
+			if ( parent->parentIsland != B2_NULL_INDEX )
+			{
+				// path compression
+				islandA->parentIsland = parent->parentIsland;
+			}
+
+			islandA = parent;
+			islandIdA = parentId;
+			parentId = islandA->parentIsland;
+		}
+	}
+
+	// Union-find root of islandB
+	b2Island* islandB = NULL;
+	if ( islandIdB != B2_NULL_INDEX )
+	{
+		islandB = b2IslandArray_Get( &world->islands, islandIdB );
+		int parentId = islandB->parentIsland;
+		while ( islandB->parentIsland != B2_NULL_INDEX )
+		{
+			b2Island* parent = b2IslandArray_Get( &world->islands, parentId );
+			if ( parent->parentIsland != B2_NULL_INDEX )
+			{
+				// path compression
+				islandB->parentIsland = parent->parentIsland;
+			}
+
+			islandB = parent;
+			islandIdB = parentId;
+			parentId = islandB->parentIsland;
+		}
+	}
+
+	B2_ASSERT( islandA != NULL || islandB != NULL );
+
+	// Union-Find link island roots
+	if ( islandA != islandB && islandA != NULL && islandB != NULL )
+	{
+		B2_ASSERT( islandA != islandB );
+		B2_ASSERT( islandB->parentIsland == B2_NULL_INDEX );
+		islandB->parentIsland = islandIdA;
+	}
+
+	if ( islandA != NULL )
+	{
+		b2AddContactToIsland( world, islandIdA, contact );
+	}
+	else
+	{
+		b2AddContactToIsland( world, islandIdB, contact );
+	}
+
+	// todo why not merge the islands right here?
+}
+
+// This is called when a contact no longer has contact points or when a contact is destroyed.
+void b2UnlinkContact( b2World* world, b2Contact* contact )
+{
+	B2_ASSERT( contact->islandId != B2_NULL_INDEX );
+
+	// remove from island
+	int islandId = contact->islandId;
+	b2Island* island = b2IslandArray_Get( &world->islands, islandId );
+
+	if ( contact->islandPrev != B2_NULL_INDEX )
+	{
+		b2Contact* prevContact = b2ContactArray_Get( &world->contacts, contact->islandPrev);
+		B2_ASSERT( prevContact->islandNext == contact->contactId );
+		prevContact->islandNext = contact->islandNext;
+	}
+
+	if ( contact->islandNext != B2_NULL_INDEX )
+	{
+		b2Contact* nextContact = b2ContactArray_Get( &world->contacts, contact->islandNext );
+		B2_ASSERT( nextContact->islandPrev == contact->contactId );
+		nextContact->islandPrev = contact->islandPrev;
+	}
+
+	if ( island->headContact == contact->contactId )
+	{
+		island->headContact = contact->islandNext;
+	}
+
+	if ( island->tailContact == contact->contactId )
+	{
+		island->tailContact = contact->islandPrev;
+	}
+
+	B2_ASSERT( island->contactCount > 0 );
+	island->contactCount -= 1;
+	island->constraintRemoveCount += 1;
+
+	contact->islandId = B2_NULL_INDEX;
+	contact->islandPrev = B2_NULL_INDEX;
+	contact->islandNext = B2_NULL_INDEX;
+
+	b2ValidateIsland( world, islandId );
+}
+
+static void b2AddJointToIsland( b2World* world, int islandId, b2Joint* joint )
+{
+	B2_ASSERT( joint->islandId == B2_NULL_INDEX );
+	B2_ASSERT( joint->islandPrev == B2_NULL_INDEX );
+	B2_ASSERT( joint->islandNext == B2_NULL_INDEX );
+
+	b2Island* island = b2IslandArray_Get( &world->islands, islandId );
+
+	if ( island->headJoint != B2_NULL_INDEX )
+	{
+		joint->islandNext = island->headJoint;
+		b2Joint* headJoint = b2JointArray_Get( &world->joints, island->headJoint );
+		headJoint->islandPrev = joint->jointId;
+	}
+
+	island->headJoint = joint->jointId;
+	if ( island->tailJoint == B2_NULL_INDEX )
+	{
+		island->tailJoint = island->headJoint;
+	}
+
+	island->jointCount += 1;
+	joint->islandId = islandId;
+
+	b2ValidateIsland( world, islandId );
+}
+
+void b2LinkJoint( b2World* world, b2Joint* joint, bool mergeIslands )
+{
+	b2Body* bodyA = b2BodyArray_Get( &world->bodies, joint->edges[0].bodyId );
+	b2Body* bodyB = b2BodyArray_Get( &world->bodies, joint->edges[1].bodyId );
+
+	if ( bodyA->setIndex == b2_awakeSet && bodyB->setIndex >= b2_firstSleepingSet )
+	{
+		b2WakeSolverSet( world, bodyB->setIndex );
+	}
+	else if ( bodyB->setIndex == b2_awakeSet && bodyA->setIndex >= b2_firstSleepingSet )
+	{
+		b2WakeSolverSet( world, bodyA->setIndex );
+	}
+
+	int islandIdA = bodyA->islandId;
+	int islandIdB = bodyB->islandId;
+
+	B2_ASSERT( islandIdA != B2_NULL_INDEX || islandIdB != B2_NULL_INDEX );
+
+	if ( islandIdA == islandIdB )
+	{
+		// Joint in same island
+		b2AddJointToIsland( world, islandIdA, joint );
+		return;
+	}
+
+	// Union-find root of islandA
+	b2Island* islandA = NULL;
+	if ( islandIdA != B2_NULL_INDEX )
+	{
+		islandA = b2IslandArray_Get( &world->islands, islandIdA );
+		while ( islandA->parentIsland != B2_NULL_INDEX )
+		{
+			b2Island* parent = b2IslandArray_Get( &world->islands, islandA->parentIsland );
+			if ( parent->parentIsland != B2_NULL_INDEX )
+			{
+				// path compression
+				islandA->parentIsland = parent->parentIsland;
+			}
+
+			islandIdA = islandA->parentIsland;
+			islandA = parent;
+		}
+	}
+
+	// Union-find root of islandB
+	b2Island* islandB = NULL;
+	if ( islandIdB != B2_NULL_INDEX )
+	{
+		islandB = b2IslandArray_Get( &world->islands, islandIdB );
+		while ( islandB->parentIsland != B2_NULL_INDEX )
+		{
+			b2Island* parent = b2IslandArray_Get( &world->islands, islandB->parentIsland );
+			if ( parent->parentIsland != B2_NULL_INDEX )
+			{
+				// path compression
+				islandB->parentIsland = parent->parentIsland;
+			}
+
+			islandIdB = islandB->parentIsland;
+			islandB = parent;
+		}
+	}
+
+	B2_ASSERT( islandA != NULL || islandB != NULL );
+
+	// Union-Find link island roots
+	if ( islandA != islandB && islandA != NULL && islandB != NULL )
+	{
+		B2_ASSERT( islandA != islandB );
+		B2_ASSERT( islandB->parentIsland == B2_NULL_INDEX );
+		islandB->parentIsland = islandIdA;
+	}
+
+	if ( islandA != NULL )
+	{
+		b2AddJointToIsland( world, islandIdA, joint );
+	}
+	else
+	{
+		b2AddJointToIsland( world, islandIdB, joint );
+	}
+
+	// Joints need to have islands merged immediately when they are created
+	// to keep the island graph valid.
+	// However, when a body type is being changed the merge can be deferred until
+	// all joints are linked.
+	if (mergeIslands)
+	{
+		b2MergeAwakeIslands( world );
+	}
+}
+
+void b2UnlinkJoint( b2World* world, b2Joint* joint )
+{
+	B2_ASSERT( joint->islandId != B2_NULL_INDEX );
+
+	// remove from island
+	int islandId = joint->islandId;
+	b2Island* island = b2IslandArray_Get( &world->islands, islandId );
+
+	if ( joint->islandPrev != B2_NULL_INDEX )
+	{
+		b2Joint* prevJoint = b2JointArray_Get( &world->joints, joint->islandPrev );
+		B2_ASSERT( prevJoint->islandNext == joint->jointId );
+		prevJoint->islandNext = joint->islandNext;
+	}
+
+	if ( joint->islandNext != B2_NULL_INDEX )
+	{
+		b2Joint* nextJoint = b2JointArray_Get( &world->joints, joint->islandNext );
+		B2_ASSERT( nextJoint->islandPrev == joint->jointId );
+		nextJoint->islandPrev = joint->islandPrev;
+	}
+
+	if ( island->headJoint == joint->jointId )
+	{
+		island->headJoint = joint->islandNext;
+	}
+
+	if ( island->tailJoint == joint->jointId )
+	{
+		island->tailJoint = joint->islandPrev;
+	}
+
+	B2_ASSERT( island->jointCount > 0 );
+	island->jointCount -= 1;
+	island->constraintRemoveCount += 1;
+
+	joint->islandId = B2_NULL_INDEX;
+	joint->islandPrev = B2_NULL_INDEX;
+	joint->islandNext = B2_NULL_INDEX;
+
+	b2ValidateIsland( world, islandId );
+}
+
+// Merge an island into its root island.
+// todo we can assume all islands are awake here
+static void b2MergeIsland( b2World* world, b2Island* island )
+{
+	B2_ASSERT( island->parentIsland != B2_NULL_INDEX );
+
+	int rootId = island->parentIsland;
+	b2Island* rootIsland = b2IslandArray_Get( &world->islands, rootId );
+	B2_ASSERT( rootIsland->parentIsland == B2_NULL_INDEX );
+
+	// remap island indices
+	int bodyId = island->headBody;
+	while ( bodyId != B2_NULL_INDEX )
+	{
+		b2Body* body = b2BodyArray_Get( &world->bodies, bodyId );
+		body->islandId = rootId;
+		bodyId = body->islandNext;
+	}
+
+	int contactId = island->headContact;
+	while ( contactId != B2_NULL_INDEX )
+	{
+		b2Contact* contact = b2ContactArray_Get( &world->contacts, contactId );
+		contact->islandId = rootId;
+		contactId = contact->islandNext;
+	}
+
+	int jointId = island->headJoint;
+	while ( jointId != B2_NULL_INDEX )
+	{
+		b2Joint* joint = b2JointArray_Get( &world->joints, jointId );
+		joint->islandId = rootId;
+		jointId = joint->islandNext;
+	}
+
+	// connect body lists
+	B2_ASSERT( rootIsland->tailBody != B2_NULL_INDEX );
+	b2Body* tailBody = b2BodyArray_Get( &world->bodies, rootIsland->tailBody );
+	B2_ASSERT( tailBody->islandNext == B2_NULL_INDEX );
+	tailBody->islandNext = island->headBody;
+
+	B2_ASSERT( island->headBody != B2_NULL_INDEX );
+	b2Body* headBody = b2BodyArray_Get( &world->bodies, island->headBody );
+	B2_ASSERT( headBody->islandPrev == B2_NULL_INDEX );
+	headBody->islandPrev = rootIsland->tailBody;
+
+	rootIsland->tailBody = island->tailBody;
+	rootIsland->bodyCount += island->bodyCount;
+
+	// connect contact lists
+	if ( rootIsland->headContact == B2_NULL_INDEX )
+	{
+		// Root island has no contacts
+		B2_ASSERT( rootIsland->tailContact == B2_NULL_INDEX && rootIsland->contactCount == 0 );
+		rootIsland->headContact = island->headContact;
+		rootIsland->tailContact = island->tailContact;
+		rootIsland->contactCount = island->contactCount;
+	}
+	else if ( island->headContact != B2_NULL_INDEX )
+	{
+		// Both islands have contacts
+		B2_ASSERT( island->tailContact != B2_NULL_INDEX && island->contactCount > 0 );
+		B2_ASSERT( rootIsland->tailContact != B2_NULL_INDEX && rootIsland->contactCount > 0 );
+
+		b2Contact* tailContact = b2ContactArray_Get( &world->contacts, rootIsland->tailContact );
+		B2_ASSERT( tailContact->islandNext == B2_NULL_INDEX );
+		tailContact->islandNext = island->headContact;
+
+		b2Contact* headContact = b2ContactArray_Get( &world->contacts, island->headContact );
+		B2_ASSERT( headContact->islandPrev == B2_NULL_INDEX );
+		headContact->islandPrev = rootIsland->tailContact;
+
+		rootIsland->tailContact = island->tailContact;
+		rootIsland->contactCount += island->contactCount;
+	}
+
+	if ( rootIsland->headJoint == B2_NULL_INDEX )
+	{
+		// Root island has no joints
+		B2_ASSERT( rootIsland->tailJoint == B2_NULL_INDEX && rootIsland->jointCount == 0 );
+		rootIsland->headJoint = island->headJoint;
+		rootIsland->tailJoint = island->tailJoint;
+		rootIsland->jointCount = island->jointCount;
+	}
+	else if ( island->headJoint != B2_NULL_INDEX )
+	{
+		// Both islands have joints
+		B2_ASSERT( island->tailJoint != B2_NULL_INDEX && island->jointCount > 0 );
+		B2_ASSERT( rootIsland->tailJoint != B2_NULL_INDEX && rootIsland->jointCount > 0 );
+
+		b2Joint* tailJoint = b2JointArray_Get( &world->joints, rootIsland->tailJoint );
+		B2_ASSERT( tailJoint->islandNext == B2_NULL_INDEX );
+		tailJoint->islandNext = island->headJoint;
+
+		b2Joint* headJoint = b2JointArray_Get( &world->joints, island->headJoint );
+		B2_ASSERT( headJoint->islandPrev == B2_NULL_INDEX );
+		headJoint->islandPrev = rootIsland->tailJoint;
+
+		rootIsland->tailJoint = island->tailJoint;
+		rootIsland->jointCount += island->jointCount;
+	}
+
+	// Track removed constraints
+	rootIsland->constraintRemoveCount += island->constraintRemoveCount;
+
+	b2ValidateIsland( world, rootId );
+}
+
+// Iterate over all awake islands and merge any that need merging
+// Islands that get merged into a root island will be removed from the awake island array
+// and returned to the pool.
+// todo this might be faster if b2IslandSim held the connectivity data
+void b2MergeAwakeIslands( b2World* world )
+{
+	b2TracyCZoneNC( merge_islands, "Merge Islands", b2_colorMediumTurquoise, true );
+
+	b2SolverSet* awakeSet = b2SolverSetArray_Get( &world->solverSets, b2_awakeSet );
+	b2IslandSim* islandSims = awakeSet->islandSims.data;
+	int awakeIslandCount = awakeSet->islandSims.count;
+
+	// Step 1: Ensure every child island points to its root island. This avoids merging a child island with
+	// a parent island that has already been merged with a grand-parent island.
+	for ( int i = 0; i < awakeIslandCount; ++i )
+	{
+		int islandId = islandSims[i].islandId;
+
+		b2Island* island = b2IslandArray_Get( &world->islands, islandId );
+
+		// find the root island
+		int rootId = islandId;
+		b2Island* rootIsland = island;
+		while ( rootIsland->parentIsland != B2_NULL_INDEX )
+		{
+			b2Island* parent = b2IslandArray_Get( &world->islands, rootIsland->parentIsland );
+			if ( parent->parentIsland != B2_NULL_INDEX )
+			{
+				// path compression
+				rootIsland->parentIsland = parent->parentIsland;
+			}
+
+			rootId = rootIsland->parentIsland;
+			rootIsland = parent;
+		}
+
+		if ( rootIsland != island )
+		{
+			island->parentIsland = rootId;
+		}
+	}
+
+	// Step 2: merge every awake island into its parent (which must be a root island)
+	// Reverse to support removal from awake array.
+	for ( int i = awakeIslandCount - 1; i >= 0; --i )
+	{
+		int islandId = islandSims[i].islandId;
+		b2Island* island = b2IslandArray_Get( &world->islands, islandId );
+
+		if ( island->parentIsland == B2_NULL_INDEX )
+		{
+			continue;
+		}
+
+		b2MergeIsland( world, island );
+
+		// this call does a remove swap from the end of the island sim array
+		b2DestroyIsland( world, islandId );
+	}
+
+	b2ValidateConnectivity( world );
+
+	b2TracyCZoneEnd( merge_islands );
+}
+
+#define B2_CONTACT_REMOVE_THRESHOLD 1
+
+void b2SplitIsland( b2World* world, int baseId )
+{
+	b2Island* baseIsland = b2IslandArray_Get( &world->islands, baseId );
+	int setIndex = baseIsland->setIndex;
+
+	if ( setIndex != b2_awakeSet )
+	{
+		// can only split awake island
+		return;
+	}
+
+	if ( baseIsland->constraintRemoveCount == 0 )
+	{
+		// this island doesn't need to be split
+		return;
+	}
+
+	b2ValidateIsland( world, baseId );
+
+	int bodyCount = baseIsland->bodyCount;
+
+	b2Body* bodies = world->bodies.data;
+	b2ArenaAllocator* alloc = &world->arena;
+
+	// No lock is needed because I ensure the allocator is not used while this task is active.
+	int* stack = b2AllocateArenaItem( alloc, bodyCount * sizeof( int ), "island stack" );
+	int* bodyIds = b2AllocateArenaItem( alloc, bodyCount * sizeof( int ), "body ids" );
+
+	// Build array containing all body indices from base island. These
+	// serve as seed bodies for the depth first search (DFS).
+	int index = 0;
+	int nextBody = baseIsland->headBody;
+	while ( nextBody != B2_NULL_INDEX )
+	{
+		bodyIds[index++] = nextBody;
+		b2Body* body = bodies + nextBody;
+
+		// Clear visitation mark
+		body->isMarked = false;
+
+		nextBody = body->islandNext;
+	}
+	B2_ASSERT( index == bodyCount );
+
+	// Clear contact island flags. Only need to consider contacts
+	// already in the base island.
+	int nextContactId = baseIsland->headContact;
+	while ( nextContactId != B2_NULL_INDEX )
+	{
+		b2Contact* contact = b2ContactArray_Get( &world->contacts, nextContactId );
+		contact->isMarked = false;
+		nextContactId = contact->islandNext;
+	}
+
+	// Clear joint island flags.
+	int nextJoint = baseIsland->headJoint;
+	while ( nextJoint != B2_NULL_INDEX )
+	{
+		b2Joint* joint = b2JointArray_Get( &world->joints, nextJoint );
+		joint->isMarked = false;
+		nextJoint = joint->islandNext;
+	}
+
+	// Done with the base split island.
+	b2DestroyIsland( world, baseId );
+
+	// Each island is found as a depth first search starting from a seed body
+	for ( int i = 0; i < bodyCount; ++i )
+	{
+		int seedIndex = bodyIds[i];
+		b2Body* seed = bodies + seedIndex;
+		B2_ASSERT( seed->setIndex == setIndex );
+
+		if ( seed->isMarked == true )
+		{
+			// The body has already been visited
+			continue;
+		}
+
+		int stackCount = 0;
+		stack[stackCount++] = seedIndex;
+		seed->isMarked = true;
+
+		// Create new island
+		// No lock needed because only a single island can split per time step. No islands are being used during the constraint
+		// solve. However, islands are touched during body finalization.
+		b2Island* island = b2CreateIsland( world, setIndex );
+
+		int islandId = island->islandId;
+
+		// Perform a depth first search (DFS) on the constraint graph.
+		while ( stackCount > 0 )
+		{
+			// Grab the next body off the stack and add it to the island.
+			int bodyId = stack[--stackCount];
+			b2Body* body = bodies + bodyId;
+			B2_ASSERT( body->setIndex == b2_awakeSet );
+			B2_ASSERT( body->isMarked == true );
+
+			// Add body to island
+			body->islandId = islandId;
+			if ( island->tailBody != B2_NULL_INDEX )
+			{
+				bodies[island->tailBody].islandNext = bodyId;
+			}
+			body->islandPrev = island->tailBody;
+			body->islandNext = B2_NULL_INDEX;
+			island->tailBody = bodyId;
+
+			if ( island->headBody == B2_NULL_INDEX )
+			{
+				island->headBody = bodyId;
+			}
+
+			island->bodyCount += 1;
+
+			// Search all contacts connected to this body.
+			int contactKey = body->headContactKey;
+			while ( contactKey != B2_NULL_INDEX )
+			{
+				int contactId = contactKey >> 1;
+				int edgeIndex = contactKey & 1;
+
+				b2Contact* contact = b2ContactArray_Get( &world->contacts, contactId );
+				B2_ASSERT( contact->contactId == contactId );
+
+				// Next key
+				contactKey = contact->edges[edgeIndex].nextKey;
+
+				// Has this contact already been added to this island?
+				if ( contact->isMarked )
+				{
+					continue;
+				}
+
+				// Is this contact enabled and touching?
+				if ( ( contact->flags & b2_contactTouchingFlag ) == 0 )
+				{
+					continue;
+				}
+
+				contact->isMarked = true;
+
+				int otherEdgeIndex = edgeIndex ^ 1;
+				int otherBodyId = contact->edges[otherEdgeIndex].bodyId;
+				b2Body* otherBody = bodies + otherBodyId;
+
+				// Maybe add other body to stack
+				if ( otherBody->isMarked == false && otherBody->setIndex != b2_staticSet )
+				{
+					B2_ASSERT( stackCount < bodyCount );
+					stack[stackCount++] = otherBodyId;
+					otherBody->isMarked = true;
+				}
+
+				// Add contact to island
+				contact->islandId = islandId;
+				if ( island->tailContact != B2_NULL_INDEX )
+				{
+					b2Contact* tailContact = b2ContactArray_Get( &world->contacts, island->tailContact );
+					tailContact->islandNext = contactId;
+				}
+				contact->islandPrev = island->tailContact;
+				contact->islandNext = B2_NULL_INDEX;
+				island->tailContact = contactId;
+
+				if ( island->headContact == B2_NULL_INDEX )
+				{
+					island->headContact = contactId;
+				}
+
+				island->contactCount += 1;
+			}
+
+			// Search all joints connect to this body.
+			int jointKey = body->headJointKey;
+			while ( jointKey != B2_NULL_INDEX )
+			{
+				int jointId = jointKey >> 1;
+				int edgeIndex = jointKey & 1;
+
+				b2Joint* joint = b2JointArray_Get( &world->joints, jointId );
+				B2_ASSERT( joint->jointId == jointId );
+
+				// Next key
+				jointKey = joint->edges[edgeIndex].nextKey;
+
+				// Has this joint already been added to this island?
+				if ( joint->isMarked )
+				{
+					continue;
+				}
+
+				joint->isMarked = true;
+
+				int otherEdgeIndex = edgeIndex ^ 1;
+				int otherBodyId = joint->edges[otherEdgeIndex].bodyId;
+				b2Body* otherBody = bodies + otherBodyId;
+
+				// Don't simulate joints connected to disabled bodies.
+				if ( otherBody->setIndex == b2_disabledSet )
+				{
+					continue;
+				}
+
+				// Maybe add other body to stack
+				if ( otherBody->isMarked == false && otherBody->setIndex == b2_awakeSet )
+				{
+					B2_ASSERT( stackCount < bodyCount );
+					stack[stackCount++] = otherBodyId;
+					otherBody->isMarked = true;
+				}
+
+				// Add joint to island
+				joint->islandId = islandId;
+				if ( island->tailJoint != B2_NULL_INDEX )
+				{
+					b2Joint* tailJoint = b2JointArray_Get( &world->joints, island->tailJoint );
+					tailJoint->islandNext = jointId;
+				}
+				joint->islandPrev = island->tailJoint;
+				joint->islandNext = B2_NULL_INDEX;
+				island->tailJoint = jointId;
+
+				if ( island->headJoint == B2_NULL_INDEX )
+				{
+					island->headJoint = jointId;
+				}
+
+				island->jointCount += 1;
+			}
+		}
+
+		b2ValidateIsland( world, islandId );
+	}
+
+	b2FreeArenaItem( alloc, bodyIds );
+	b2FreeArenaItem( alloc, stack );
+}
+
+// Split an island because some contacts and/or joints have been removed.
+// This is called during the constraint solve while islands are not being touched. This uses DFS and touches a lot of memory,
+// so it can be quite slow.
+// Note: contacts/joints connected to static bodies must belong to an island but don't affect island connectivity
+// Note: static bodies are never in an island
+// Note: this task interacts with some allocators without locks under the assumption that no other tasks
+// are interacting with these data structures.
+void b2SplitIslandTask( int startIndex, int endIndex, uint32_t threadIndex, void* context )
+{
+	b2TracyCZoneNC( split, "Split Island", b2_colorOlive, true );
+
+	B2_UNUSED( startIndex, endIndex, threadIndex );
+
+	uint64_t ticks = b2GetTicks();
+	b2World* world = context;
+
+	B2_ASSERT( world->splitIslandId != B2_NULL_INDEX );
+
+	b2SplitIsland( world, world->splitIslandId );
+
+	world->profile.splitIslands += b2GetMilliseconds( ticks );
+	b2TracyCZoneEnd( split );
+}
+
+#if B2_VALIDATE
+void b2ValidateIsland( b2World* world, int islandId )
+{
+	b2Island* island = b2IslandArray_Get( &world->islands, islandId );
+	B2_ASSERT( island->islandId == islandId );
+	B2_ASSERT( island->setIndex != B2_NULL_INDEX );
+	B2_ASSERT( island->headBody != B2_NULL_INDEX );
+
+	{
+		B2_ASSERT( island->tailBody != B2_NULL_INDEX );
+		B2_ASSERT( island->bodyCount > 0 );
+		if ( island->bodyCount > 1 )
+		{
+			B2_ASSERT( island->tailBody != island->headBody );
+		}
+		B2_ASSERT( island->bodyCount <= b2GetIdCount( &world->bodyIdPool ) );
+
+		int count = 0;
+		int bodyId = island->headBody;
+		while ( bodyId != B2_NULL_INDEX )
+		{
+			b2Body* body = b2BodyArray_Get(&world->bodies, bodyId);
+			B2_ASSERT( body->islandId == islandId );
+			B2_ASSERT( body->setIndex == island->setIndex );
+			count += 1;
+
+			if ( count == island->bodyCount )
+			{
+				B2_ASSERT( bodyId == island->tailBody );
+			}
+
+			bodyId = body->islandNext;
+		}
+		B2_ASSERT( count == island->bodyCount );
+	}
+
+	if ( island->headContact != B2_NULL_INDEX )
+	{
+		B2_ASSERT( island->tailContact != B2_NULL_INDEX );
+		B2_ASSERT( island->contactCount > 0 );
+		if ( island->contactCount > 1 )
+		{
+			B2_ASSERT( island->tailContact != island->headContact );
+		}
+		B2_ASSERT( island->contactCount <= b2GetIdCount( &world->contactIdPool ) );
+
+		int count = 0;
+		int contactId = island->headContact;
+		while ( contactId != B2_NULL_INDEX )
+		{
+			b2Contact* contact = b2ContactArray_Get( &world->contacts, contactId );
+			B2_ASSERT( contact->setIndex == island->setIndex );
+			B2_ASSERT( contact->islandId == islandId );
+			count += 1;
+
+			if ( count == island->contactCount )
+			{
+				B2_ASSERT( contactId == island->tailContact );
+			}
+
+			contactId = contact->islandNext;
+		}
+		B2_ASSERT( count == island->contactCount );
+	}
+	else
+	{
+		B2_ASSERT( island->tailContact == B2_NULL_INDEX );
+		B2_ASSERT( island->contactCount == 0 );
+	}
+
+	if ( island->headJoint != B2_NULL_INDEX )
+	{
+		B2_ASSERT( island->tailJoint != B2_NULL_INDEX );
+		B2_ASSERT( island->jointCount > 0 );
+		if ( island->jointCount > 1 )
+		{
+			B2_ASSERT( island->tailJoint != island->headJoint );
+		}
+		B2_ASSERT( island->jointCount <= b2GetIdCount( &world->jointIdPool ) );
+
+		int count = 0;
+		int jointId = island->headJoint;
+		while ( jointId != B2_NULL_INDEX )
+		{
+			b2Joint* joint = b2JointArray_Get( &world->joints, jointId );
+			B2_ASSERT( joint->setIndex == island->setIndex );
+			count += 1;
+
+			if ( count == island->jointCount )
+			{
+				B2_ASSERT( jointId == island->tailJoint );
+			}
+
+			jointId = joint->islandNext;
+		}
+		B2_ASSERT( count == island->jointCount );
+	}
+	else
+	{
+		B2_ASSERT( island->tailJoint == B2_NULL_INDEX );
+		B2_ASSERT( island->jointCount == 0 );
+	}
+}
+
+#else
+
+void b2ValidateIsland( b2World* world, int islandId )
+{
+	B2_UNUSED( world );
+	B2_UNUSED( islandId );
+}
+#endif

src/vendor/box2d/island.h

@@ -0,0 +1,89 @@
+// SPDX-FileCopyrightText: 2023 Erin Catto
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include "array.h"
+
+#include <stdbool.h>
+#include <stdint.h>
+
+typedef struct b2Contact b2Contact;
+typedef struct b2Joint b2Joint;
+typedef struct b2World b2World;
+
+// Deterministic solver
+//
+// Collide all awake contacts
+// Use bit array to emit start/stop touching events in defined order, per thread. Try using contact index, assuming contacts are
+// created in a deterministic order. bit-wise OR together bit arrays and issue changes:
+// - start touching: merge islands - temporary linked list - mark root island dirty - wake all - largest island is root
+// - stop touching: increment constraintRemoveCount
+
+// Persistent island for awake bodies, joints, and contacts
+// https://en.wikipedia.org/wiki/Component_(graph_theory)
+// https://en.wikipedia.org/wiki/Dynamic_connectivity
+// map from int to solver set and index
+typedef struct b2Island
+{
+	// index of solver set stored in b2World
+	// may be B2_NULL_INDEX
+	int setIndex;
+
+	// island index within set
+	// may be B2_NULL_INDEX
+	int localIndex;
+
+	int islandId;
+
+	int headBody;
+	int tailBody;
+	int bodyCount;
+
+	int headContact;
+	int tailContact;
+	int contactCount;
+
+	int headJoint;
+	int tailJoint;
+	int jointCount;
+
+	// Union find
+	// todo this could go away if islands are merged immediately with b2LinkJoint and b2LinkContact
+	int parentIsland;
+
+	// Keeps track of how many contacts have been removed from this island.
+	// This is used to determine if an island is a candidate for splitting.
+	int constraintRemoveCount;
+} b2Island;
+
+// This is used to move islands across solver sets
+typedef struct b2IslandSim
+{
+	int islandId;
+} b2IslandSim;
+
+b2Island* b2CreateIsland( b2World* world, int setIndex );
+void b2DestroyIsland( b2World* world, int islandId );
+
+// Link contacts into the island graph when it starts having contact points
+void b2LinkContact( b2World* world, b2Contact* contact );
+
+// Unlink contact from the island graph when it stops having contact points
+void b2UnlinkContact( b2World* world, b2Contact* contact );
+
+// Link a joint into the island graph when it is created
+void b2LinkJoint( b2World* world, b2Joint* joint, bool mergeIslands );
+
+// Unlink a joint from the island graph when it is destroyed
+void b2UnlinkJoint( b2World* world, b2Joint* joint );
+
+void b2MergeAwakeIslands( b2World* world );
+
+void b2SplitIsland( b2World* world, int baseId );
+void b2SplitIslandTask( int startIndex, int endIndex, uint32_t threadIndex, void* context );
+
+void b2ValidateIsland( b2World* world, int islandId );
+
+B2_ARRAY_INLINE( b2Island, b2Island )
+B2_ARRAY_INLINE( b2IslandSim, b2IslandSim )

src/vendor/box2d/joint.h

@@ -0,0 +1,335 @@
+// SPDX-FileCopyrightText: 2023 Erin Catto
+// SPDX-License-Identifier: MIT
+#pragma once
+
+#include "array.h"
+#include "solver.h"
+
+#include "box2d/types.h"
+
+typedef struct b2DebugDraw b2DebugDraw;
+typedef struct b2StepContext b2StepContext;
+typedef struct b2World b2World;
+
+/// A joint edge is used to connect bodies and joints together
+/// in a joint graph where each body is a node and each joint
+/// is an edge. A joint edge belongs to a doubly linked list
+/// maintained in each attached body. Each joint has two joint
+/// nodes, one for each attached body.
+typedef struct b2JointEdge
+{
+	int bodyId;
+	int prevKey;
+	int nextKey;
+} b2JointEdge;
+
+// Map from b2JointId to b2Joint in the solver sets
+typedef struct b2Joint
+{
+	void* userData;
+
+	// index of simulation set stored in b2World
+	// B2_NULL_INDEX when slot is free
+	int setIndex;
+
+	// index into the constraint graph color array, may be B2_NULL_INDEX for sleeping/disabled joints
+	// B2_NULL_INDEX when slot is free
+	int colorIndex;
+
+	// joint index within set or graph color
+	// B2_NULL_INDEX when slot is free
+	int localIndex;
+
+	b2JointEdge edges[2];
+
+	int jointId;
+	int islandId;
+	int islandPrev;
+	int islandNext;
+
+	float drawSize;
+
+	b2JointType type;
+
+	// This is monotonically advanced when a body is allocated in this slot
+	// Used to check for invalid b2JointId
+	uint16_t generation;
+
+	bool isMarked;
+	bool collideConnected;
+
+} b2Joint;
+
+typedef struct b2DistanceJoint
+{
+	float length;
+	float hertz;
+	float dampingRatio;
+	float minLength;
+	float maxLength;
+
+	float maxMotorForce;
+	float motorSpeed;
+
+	float impulse;
+	float lowerImpulse;
+	float upperImpulse;
+	float motorImpulse;
+
+	int indexA;
+	int indexB;
+	b2Vec2 anchorA;
+	b2Vec2 anchorB;
+	b2Vec2 deltaCenter;
+	b2Softness distanceSoftness;
+	float axialMass;
+
+	bool enableSpring;
+	bool enableLimit;
+	bool enableMotor;
+} b2DistanceJoint;
+
+typedef struct b2MotorJoint
+{
+	b2Vec2 linearOffset;
+	float angularOffset;
+	b2Vec2 linearImpulse;
+	float angularImpulse;
+	float maxForce;
+	float maxTorque;
+	float correctionFactor;
+
+	int indexA;
+	int indexB;
+	b2Vec2 anchorA;
+	b2Vec2 anchorB;
+	b2Vec2 deltaCenter;
+	float deltaAngle;
+	b2Mat22 linearMass;
+	float angularMass;
+} b2MotorJoint;
+
+typedef struct b2MouseJoint
+{
+	b2Vec2 targetA;
+	float hertz;
+	float dampingRatio;
+	float maxForce;
+
+	b2Vec2 linearImpulse;
+	float angularImpulse;
+
+	b2Softness linearSoftness;
+	b2Softness angularSoftness;
+	int indexB;
+	b2Vec2 anchorB;
+	b2Vec2 deltaCenter;
+	b2Mat22 linearMass;
+} b2MouseJoint;
+
+typedef struct b2PrismaticJoint
+{
+	b2Vec2 localAxisA;
+	b2Vec2 impulse;
+	float springImpulse;
+	float motorImpulse;
+	float lowerImpulse;
+	float upperImpulse;
+	float hertz;
+	float dampingRatio;
+	float maxMotorForce;
+	float motorSpeed;
+	float referenceAngle;
+	float lowerTranslation;
+	float upperTranslation;
+
+	int indexA;
+	int indexB;
+	b2Vec2 anchorA;
+	b2Vec2 anchorB;
+	b2Vec2 axisA;
+	b2Vec2 deltaCenter;
+	float deltaAngle;
+	float axialMass;
+	b2Softness springSoftness;
+
+	bool enableSpring;
+	bool enableLimit;
+	bool enableMotor;
+} b2PrismaticJoint;
+
+typedef struct b2RevoluteJoint
+{
+	b2Vec2 linearImpulse;
+	float springImpulse;
+	float motorImpulse;
+	float lowerImpulse;
+	float upperImpulse;
+	float hertz;
+	float dampingRatio;
+	float maxMotorTorque;
+	float motorSpeed;
+	float referenceAngle;
+	float lowerAngle;
+	float upperAngle;
+
+	int indexA;
+	int indexB;
+	b2Vec2 anchorA;
+	b2Vec2 anchorB;
+	b2Vec2 deltaCenter;
+	float deltaAngle;
+	float axialMass;
+	b2Softness springSoftness;
+
+	bool enableSpring;
+	bool enableMotor;
+	bool enableLimit;
+} b2RevoluteJoint;
+
+typedef struct b2WeldJoint
+{
+	float referenceAngle;
+	float linearHertz;
+	float linearDampingRatio;
+	float angularHertz;
+	float angularDampingRatio;
+
+	b2Softness linearSoftness;
+	b2Softness angularSoftness;
+	b2Vec2 linearImpulse;
+	float angularImpulse;
+
+	int indexA;
+	int indexB;
+	b2Vec2 anchorA;
+	b2Vec2 anchorB;
+	b2Vec2 deltaCenter;
+	float deltaAngle;
+	float axialMass;
+} b2WeldJoint;
+
+typedef struct b2WheelJoint
+{
+	b2Vec2 localAxisA;
+	float perpImpulse;
+	float motorImpulse;
+	float springImpulse;
+	float lowerImpulse;
+	float upperImpulse;
+	float maxMotorTorque;
+	float motorSpeed;
+	float lowerTranslation;
+	float upperTranslation;
+	float hertz;
+	float dampingRatio;
+
+	int indexA;
+	int indexB;
+	b2Vec2 anchorA;
+	b2Vec2 anchorB;
+	b2Vec2 axisA;
+	b2Vec2 deltaCenter;
+	float perpMass;
+	float motorMass;
+	float axialMass;
+	b2Softness springSoftness;
+
+	bool enableSpring;
+	bool enableMotor;
+	bool enableLimit;
+} b2WheelJoint;
+
+/// The base joint class. Joints are used to constraint two bodies together in
+/// various fashions. Some joints also feature limits and motors.
+typedef struct b2JointSim
+{
+	int jointId;
+
+	int bodyIdA;
+	int bodyIdB;
+
+	b2JointType type;
+
+	// Anchors relative to body origin
+	b2Vec2 localOriginAnchorA;
+	b2Vec2 localOriginAnchorB;
+
+	float invMassA, invMassB;
+	float invIA, invIB;
+
+	union
+	{
+		b2DistanceJoint distanceJoint;
+		b2MotorJoint motorJoint;
+		b2MouseJoint mouseJoint;
+		b2RevoluteJoint revoluteJoint;
+		b2PrismaticJoint prismaticJoint;
+		b2WeldJoint weldJoint;
+		b2WheelJoint wheelJoint;
+	};
+} b2JointSim;
+
+void b2DestroyJointInternal( b2World* world, b2Joint* joint, bool wakeBodies );
+
+b2Joint* b2GetJointFullId( b2World* world, b2JointId jointId );
+b2JointSim* b2GetJointSim( b2World* world, b2Joint* joint );
+b2JointSim* b2GetJointSimCheckType( b2JointId jointId, b2JointType type );
+
+void b2PrepareJoint( b2JointSim* joint, b2StepContext* context );
+void b2WarmStartJoint( b2JointSim* joint, b2StepContext* context );
+void b2SolveJoint( b2JointSim* joint, b2StepContext* context, bool useBias );
+
+void b2PrepareOverflowJoints( b2StepContext* context );
+void b2WarmStartOverflowJoints( b2StepContext* context );
+void b2SolveOverflowJoints( b2StepContext* context, bool useBias );
+
+void b2DrawJoint( b2DebugDraw* draw, b2World* world, b2Joint* joint );
+
+b2Vec2 b2GetDistanceJointForce( b2World* world, b2JointSim* base );
+b2Vec2 b2GetMotorJointForce( b2World* world, b2JointSim* base );
+b2Vec2 b2GetMouseJointForce( b2World* world, b2JointSim* base );
+b2Vec2 b2GetPrismaticJointForce( b2World* world, b2JointSim* base );
+b2Vec2 b2GetRevoluteJointForce( b2World* world, b2JointSim* base );
+b2Vec2 b2GetWeldJointForce( b2World* world, b2JointSim* base );
+b2Vec2 b2GetWheelJointForce( b2World* world, b2JointSim* base );
+
+float b2GetMotorJointTorque( b2World* world, b2JointSim* base );
+float b2GetMouseJointTorque( b2World* world, b2JointSim* base );
+float b2GetPrismaticJointTorque( b2World* world, b2JointSim* base );
+float b2GetRevoluteJointTorque( b2World* world, b2JointSim* base );
+float b2GetWeldJointTorque( b2World* world, b2JointSim* base );
+float b2GetWheelJointTorque( b2World* world, b2JointSim* base );
+
+void b2PrepareDistanceJoint( b2JointSim* base, b2StepContext* context );
+void b2PrepareMotorJoint( b2JointSim* base, b2StepContext* context );
+void b2PrepareMouseJoint( b2JointSim* base, b2StepContext* context );
+void b2PreparePrismaticJoint( b2JointSim* base, b2StepContext* context );
+void b2PrepareRevoluteJoint( b2JointSim* base, b2StepContext* context );
+void b2PrepareWeldJoint( b2JointSim* base, b2StepContext* context );
+void b2PrepareWheelJoint( b2JointSim* base, b2StepContext* context );
+
+void b2WarmStartDistanceJoint( b2JointSim* base, b2StepContext* context );
+void b2WarmStartMotorJoint( b2JointSim* base, b2StepContext* context );
+void b2WarmStartMouseJoint( b2JointSim* base, b2StepContext* context );
+void b2WarmStartPrismaticJoint( b2JointSim* base, b2StepContext* context );
+void b2WarmStartRevoluteJoint( b2JointSim* base, b2StepContext* context );
+void b2WarmStartWeldJoint( b2JointSim* base, b2StepContext* context );
+void b2WarmStartWheelJoint( b2JointSim* base, b2StepContext* context );
+
+void b2SolveDistanceJoint( b2JointSim* base, b2StepContext* context, bool useBias );
+void b2SolveMotorJoint( b2JointSim* base, b2StepContext* context, bool useBias );
+void b2SolveMouseJoint( b2JointSim* base, b2StepContext* context );
+void b2SolvePrismaticJoint( b2JointSim* base, b2StepContext* context, bool useBias );
+void b2SolveRevoluteJoint( b2JointSim* base, b2StepContext* context, bool useBias );
+void b2SolveWeldJoint( b2JointSim* base, b2StepContext* context, bool useBias );
+void b2SolveWheelJoint( b2JointSim* base, b2StepContext* context, bool useBias );
+
+void b2DrawDistanceJoint( b2DebugDraw* draw, b2JointSim* base, b2Transform transformA, b2Transform transformB );
+void b2DrawPrismaticJoint( b2DebugDraw* draw, b2JointSim* base, b2Transform transformA, b2Transform transformB );
+void b2DrawRevoluteJoint( b2DebugDraw* draw, b2JointSim* base, b2Transform transformA, b2Transform transformB, float drawSize );
+void b2DrawWheelJoint( b2DebugDraw* draw, b2JointSim* base, b2Transform transformA, b2Transform transformB );
+
+// Define inline functions for arrays
+B2_ARRAY_INLINE( b2Joint, b2Joint )
+B2_ARRAY_INLINE( b2JointSim, b2JointSim )

src/vendor/box2d/math_functions.c

@@ -0,0 +1,159 @@
+// SPDX-FileCopyrightText: 2023 Erin Catto
+// SPDX-License-Identifier: MIT
+
+#include "box2d/math_functions.h"
+
+#include <float.h>
+
+_Static_assert( sizeof( int32_t ) == sizeof( int ), "Box2D expects int32_t and int to be the same" );
+
+bool b2IsValidFloat( float a )
+{
+	if ( isnan( a ) )
+	{
+		return false;
+	}
+
+	if ( isinf( a ) )
+	{
+		return false;
+	}
+
+	return true;
+}
+
+bool b2IsValidVec2( b2Vec2 v )
+{
+	if ( isnan( v.x ) || isnan( v.y ) )
+	{
+		return false;
+	}
+
+	if ( isinf( v.x ) || isinf( v.y ) )
+	{
+		return false;
+	}
+
+	return true;
+}
+
+bool b2IsValidRotation( b2Rot q )
+{
+	if ( isnan( q.s ) || isnan( q.c ) )
+	{
+		return false;
+	}
+
+	if ( isinf( q.s ) || isinf( q.c ) )
+	{
+		return false;
+	}
+
+	return b2IsNormalizedRot( q );
+}
+
+bool b2IsValidPlane( b2Plane a )
+{
+	return b2IsValidVec2( a.normal ) && b2IsNormalized( a.normal ) && b2IsValidFloat( a.offset );
+}
+
+// https://stackoverflow.com/questions/46210708/atan2-approximation-with-11bits-in-mantissa-on-x86with-sse2-and-armwith-vfpv4
+float b2Atan2( float y, float x )
+{
+	// Added check for (0,0) to match atan2f and avoid NaN
+	if (x == 0.0f && y == 0.0f)
+	{
+		return 0.0f;
+	}
+
+	float ax = b2AbsFloat( x );
+	float ay = b2AbsFloat( y );
+	float mx = b2MaxFloat( ay, ax );
+	float mn = b2MinFloat( ay, ax );
+	float a = mn / mx;
+
+	// Minimax polynomial approximation to atan(a) on [0,1]
+	float s = a * a;
+	float c = s * a;
+	float q = s * s;
+	float r = 0.024840285f * q + 0.18681418f;
+	float t = -0.094097948f * q - 0.33213072f;
+	r = r * s + t;
+	r = r * c + a;
+
+	// Map to full circle
+	if ( ay > ax )
+	{
+		r = 1.57079637f - r;
+	}
+
+	if ( x < 0 )
+	{
+		r = 3.14159274f - r;
+	}
+
+	if ( y < 0 )
+	{
+		r = -r;
+	}
+
+	return r;
+}
+
+// Approximate cosine and sine for determinism. In my testing cosf and sinf produced
+// the same results on x64 and ARM using MSVC, GCC, and Clang. However, I don't trust
+// this result.
+// https://en.wikipedia.org/wiki/Bh%C4%81skara_I%27s_sine_approximation_formula
+b2CosSin b2ComputeCosSin( float radians )
+{
+	float x = b2UnwindLargeAngle( radians );
+	float pi2 = B2_PI * B2_PI;
+
+	// cosine needs angle in [-pi/2, pi/2]
+	float c;
+	if ( x < -0.5f * B2_PI )
+	{
+		float y = x + B2_PI;
+		float y2 = y * y;
+		c = -( pi2 - 4.0f * y2 ) / ( pi2 + y2 );
+	}
+	else if ( x > 0.5f * B2_PI )
+	{
+		float y = x - B2_PI;
+		float y2 = y * y;
+		c = -( pi2 - 4.0f * y2 ) / ( pi2 + y2 );
+	}
+	else
+	{
+		float y2 = x * x;
+		c = ( pi2 - 4.0f * y2 ) / ( pi2 + y2 );
+	}
+
+	// sine needs angle in [0, pi]
+	float s;
+	if ( x < 0.0f )
+	{
+		float y = x + B2_PI;
+		s = -16.0f * y * ( B2_PI - y ) / ( 5.0f * pi2 - 4.0f * y * ( B2_PI - y ) );
+	}
+	else
+	{
+		s = 16.0f * x * ( B2_PI - x ) / ( 5.0f * pi2 - 4.0f * x * ( B2_PI - x ) );
+	}
+
+	float mag = sqrtf( s * s + c * c );
+	float invMag = mag > 0.0 ? 1.0f / mag : 0.0f;
+	b2CosSin cs = { c * invMag, s * invMag };
+	return cs;
+}
+
+b2Rot b2ComputeRotationBetweenUnitVectors(b2Vec2 v1, b2Vec2 v2)
+{
+	B2_ASSERT( b2AbsFloat( 1.0f - b2Length( v1 ) ) < 100.0f * FLT_EPSILON );
+	B2_ASSERT( b2AbsFloat( 1.0f - b2Length( v2 ) ) < 100.0f * FLT_EPSILON );
+
+	b2Rot rot;
+	rot.c = b2Dot( v1, v2 );
+	rot.s = b2Cross( v1, v2 );
+	return b2NormalizeRot( rot );
+}

src/vendor/box2d/math_functions.h

@@ -0,0 +1,761 @@
+// SPDX-FileCopyrightText: 2023 Erin Catto
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include "base.h"
+
+#include <float.h>
+#include <math.h>
+#include <stdbool.h>
+
+/**
+ * @defgroup math Math
+ * @brief Vector math types and functions
+ * @{
+ */
+
+/// 2D vector
+/// This can be used to represent a point or free vector
+typedef struct b2Vec2
+{
+	/// coordinates
+	float x, y;
+} b2Vec2;
+
+/// Cosine and sine pair
+/// This uses a custom implementation designed for cross-platform determinism
+typedef struct b2CosSin
+{
+	/// cosine and sine
+	float cosine;
+	float sine;
+} b2CosSin;
+
+/// 2D rotation
+/// This is similar to using a complex number for rotation
+typedef struct b2Rot
+{
+	/// cosine and sine
+	float c, s;
+} 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;
+
+/// separation = dot(normal, point) - offset
+typedef struct b2Plane
+{
+	b2Vec2 normal;
+	float offset;
+} b2Plane;
+
+/**@}*/
+
+/**
+ * @addtogroup math
+ * @{
+ */
+
+/// https://en.wikipedia.org/wiki/Pi
+#define B2_PI 3.14159265359f
+
+static const b2Vec2 b2Vec2_zero = { 0.0f, 0.0f };
+static const b2Rot b2Rot_identity = { 1.0f, 0.0f };
+static const b2Transform b2Transform_identity = { { 0.0f, 0.0f }, { 1.0f, 0.0f } };
+static const b2Mat22 b2Mat22_zero = { { 0.0f, 0.0f }, { 0.0f, 0.0f } };
+
+/// @return the minimum of two integers
+B2_INLINE int b2MinInt( int a, int b )
+{
+	return a < b ? a : b;
+}
+
+/// @return the maximum of two integers
+B2_INLINE int b2MaxInt( int a, int b )
+{
+	return a > b ? a : b;
+}
+
+/// @return the absolute value of an integer
+B2_INLINE int b2AbsInt( int a )
+{
+	return a < 0 ? -a : a;
+}
+
+/// @return an integer clamped between a lower and upper bound
+B2_INLINE int b2ClampInt( int a, int lower, int upper )
+{
+	return a < lower ? lower : ( a > upper ? upper : a );
+}
+
+/// @return the minimum of two floats
+B2_INLINE float b2MinFloat( float a, float b )
+{
+	return a < b ? a : b;
+}
+
+/// @return the maximum of two floats
+B2_INLINE float b2MaxFloat( float a, float b )
+{
+	return a > b ? a : b;
+}
+
+/// @return the absolute value of a float
+B2_INLINE float b2AbsFloat( float a )
+{
+	return a < 0 ? -a : a;
+}
+
+/// @return a float clamped between a lower and upper bound
+B2_INLINE float b2ClampFloat( float a, float lower, float upper )
+{
+	return a < lower ? lower : ( a > upper ? upper : a );
+}
+
+/// Compute an approximate arctangent in the range [-pi, pi]
+/// This is hand coded for cross-platform determinism. The atan2f
+/// function in the standard library is not cross-platform deterministic.
+///	Accurate to around 0.0023 degrees
+B2_API float b2Atan2( float y, float x );
+
+/// Compute the cosine and sine of an angle in radians. Implemented
+/// for cross-platform determinism.
+B2_API b2CosSin b2ComputeCosSin( float radians );
+
+/// Vector dot product
+B2_INLINE float b2Dot( b2Vec2 a, b2Vec2 b )
+{
+	return a.x * b.x + a.y * b.y;
+}
+
+/// Vector cross product. In 2D this yields a scalar.
+B2_INLINE float b2Cross( b2Vec2 a, b2Vec2 b )
+{
+	return a.x * b.y - a.y * b.x;
+}
+
+/// Perform the cross product on a vector and a scalar. In 2D this produces a vector.
+B2_INLINE b2Vec2 b2CrossVS( b2Vec2 v, float s )
+{
+	return B2_LITERAL( b2Vec2 ){ s * v.y, -s * v.x };
+}
+
+/// Perform the cross product on a scalar and a vector. In 2D this produces a vector.
+B2_INLINE b2Vec2 b2CrossSV( float s, b2Vec2 v )
+{
+	return B2_LITERAL( b2Vec2 ){ -s * v.y, s * v.x };
+}
+
+/// Get a left pointing perpendicular vector. Equivalent to b2CrossSV(1.0f, v)
+B2_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)
+B2_INLINE b2Vec2 b2RightPerp( b2Vec2 v )
+{
+	return B2_LITERAL( b2Vec2 ){ v.y, -v.x };
+}
+
+/// Vector addition
+B2_INLINE b2Vec2 b2Add( b2Vec2 a, b2Vec2 b )
+{
+	return B2_LITERAL( b2Vec2 ){ a.x + b.x, a.y + b.y };
+}
+
+/// Vector subtraction
+B2_INLINE b2Vec2 b2Sub( b2Vec2 a, b2Vec2 b )
+{
+	return B2_LITERAL( b2Vec2 ){ a.x - b.x, a.y - b.y };
+}
+
+/// Vector negation
+B2_INLINE b2Vec2 b2Neg( b2Vec2 a )
+{
+	return B2_LITERAL( b2Vec2 ){ -a.x, -a.y };
+}
+
+/// Vector linear interpolation
+/// https://fgiesen.wordpress.com/2012/08/15/linear-interpolation-past-present-and-future/
+B2_INLINE b2Vec2 b2Lerp( b2Vec2 a, b2Vec2 b, float t )
+{
+	return B2_LITERAL( b2Vec2 ){ ( 1.0f - t ) * a.x + t * b.x, ( 1.0f - t ) * a.y + t * b.y };
+}
+
+/// Component-wise multiplication
+B2_INLINE b2Vec2 b2Mul( b2Vec2 a, b2Vec2 b )
+{
+	return B2_LITERAL( b2Vec2 ){ a.x * b.x, a.y * b.y };
+}
+
+/// Multiply a scalar and vector
+B2_INLINE b2Vec2 b2MulSV( float s, b2Vec2 v )
+{
+	return B2_LITERAL( b2Vec2 ){ s * v.x, s * v.y };
+}
+
+/// a + s * b
+B2_INLINE b2Vec2 b2MulAdd( b2Vec2 a, float s, b2Vec2 b )
+{
+	return B2_LITERAL( b2Vec2 ){ a.x + s * b.x, a.y + s * b.y };
+}
+
+/// a - s * b
+B2_INLINE b2Vec2 b2MulSub( b2Vec2 a, float s, b2Vec2 b )
+{
+	return B2_LITERAL( b2Vec2 ){ a.x - s * b.x, a.y - s * b.y };
+}
+
+/// Component-wise absolute vector
+B2_INLINE b2Vec2 b2Abs( b2Vec2 a )
+{
+	b2Vec2 b;
+	b.x = b2AbsFloat( a.x );
+	b.y = b2AbsFloat( a.y );
+	return b;
+}
+
+/// Component-wise minimum vector
+B2_INLINE b2Vec2 b2Min( b2Vec2 a, b2Vec2 b )
+{
+	b2Vec2 c;
+	c.x = b2MinFloat( a.x, b.x );
+	c.y = b2MinFloat( a.y, b.y );
+	return c;
+}
+
+/// Component-wise maximum vector
+B2_INLINE b2Vec2 b2Max( b2Vec2 a, b2Vec2 b )
+{
+	b2Vec2 c;
+	c.x = b2MaxFloat( a.x, b.x );
+	c.y = b2MaxFloat( a.y, b.y );
+	return c;
+}
+
+/// Component-wise clamp vector v into the range [a, b]
+B2_INLINE b2Vec2 b2Clamp( b2Vec2 v, b2Vec2 a, b2Vec2 b )
+{
+	b2Vec2 c;
+	c.x = b2ClampFloat( v.x, a.x, b.x );
+	c.y = b2ClampFloat( v.y, a.y, b.y );
+	return c;
+}
+
+/// Get the length of this vector (the norm)
+B2_INLINE float b2Length( b2Vec2 v )
+{
+	return sqrtf( v.x * v.x + v.y * v.y );
+}
+
+/// Get the distance between two points
+B2_INLINE float b2Distance( b2Vec2 a, b2Vec2 b )
+{
+	float dx = b.x - a.x;
+	float dy = b.y - a.y;
+	return sqrtf( dx * dx + dy * dy );
+}
+
+/// Convert a vector into a unit vector if possible, otherwise returns the zero vector.
+/// todo MSVC is not inlining this function in several places per warning 4710
+B2_INLINE b2Vec2 b2Normalize( b2Vec2 v )
+{
+	float length = sqrtf( v.x * v.x + v.y * v.y );
+	if ( length < FLT_EPSILON )
+	{
+		return B2_LITERAL( b2Vec2 ){ 0.0f, 0.0f };
+	}
+
+	float invLength = 1.0f / length;
+	b2Vec2 n = { invLength * v.x, invLength * v.y };
+	return n;
+}
+
+/// Determines if the provided vector is normalized (norm(a) == 1).
+B2_INLINE bool b2IsNormalized( b2Vec2 a )
+{
+	float aa = b2Dot( a, a );
+	return b2AbsFloat( 1.0f - aa ) < 10.0f * FLT_EPSILON;
+}
+
+/// Convert a vector into a unit vector if possible, otherwise returns the zero vector. Also
+/// outputs the length.
+B2_INLINE b2Vec2 b2GetLengthAndNormalize( float* length, b2Vec2 v )
+{
+	*length = sqrtf( v.x * v.x + v.y * v.y );
+	if ( *length < FLT_EPSILON )
+	{
+		return B2_LITERAL( b2Vec2 ){ 0.0f, 0.0f };
+	}
+
+	float invLength = 1.0f / *length;
+	b2Vec2 n = { invLength * v.x, invLength * v.y };
+	return n;
+}
+
+/// Normalize rotation
+B2_INLINE b2Rot b2NormalizeRot( b2Rot q )
+{
+	float mag = sqrtf( q.s * q.s + q.c * q.c );
+	float invMag = mag > 0.0 ? 1.0f / mag : 0.0f;
+	b2Rot qn = { q.c * invMag, q.s * invMag };
+	return qn;
+}
+
+/// Integrate rotation from angular velocity
+/// @param q1 initial rotation
+/// @param deltaAngle the angular displacement in radians
+B2_INLINE b2Rot b2IntegrateRotation( b2Rot q1, float deltaAngle )
+{
+	// dc/dt = -omega * sin(t)
+	// ds/dt = omega * cos(t)
+	// c2 = c1 - omega * h * s1
+	// s2 = s1 + omega * h * c1
+	b2Rot q2 = { q1.c - deltaAngle * q1.s, q1.s + deltaAngle * q1.c };
+	float mag = sqrtf( q2.s * q2.s + q2.c * q2.c );
+	float invMag = mag > 0.0 ? 1.0f / mag : 0.0f;
+	b2Rot qn = { q2.c * invMag, q2.s * invMag };
+	return qn;
+}
+
+/// Get the length squared of this vector
+B2_INLINE float b2LengthSquared( b2Vec2 v )
+{
+	return v.x * v.x + v.y * v.y;
+}
+
+/// Get the distance squared between points
+B2_INLINE float b2DistanceSquared( b2Vec2 a, b2Vec2 b )
+{
+	b2Vec2 c = { b.x - a.x, b.y - a.y };
+	return c.x * c.x + c.y * c.y;
+}
+
+/// Make a rotation using an angle in radians
+B2_INLINE b2Rot b2MakeRot( float radians )
+{
+	b2CosSin cs = b2ComputeCosSin( radians );
+	return B2_LITERAL( b2Rot ){ cs.cosine, cs.sine };
+}
+
+/// Compute the rotation between two unit vectors
+B2_API b2Rot b2ComputeRotationBetweenUnitVectors( b2Vec2 v1, b2Vec2 v2 );
+
+/// Is this rotation normalized?
+B2_INLINE bool b2IsNormalizedRot( b2Rot q )
+{
+	// larger tolerance due to failure on mingw 32-bit
+	float qq = q.s * q.s + q.c * q.c;
+	return 1.0f - 0.0006f < qq && qq < 1.0f + 0.0006f;
+}
+
+/// Normalized linear interpolation
+/// https://fgiesen.wordpress.com/2012/08/15/linear-interpolation-past-present-and-future/
+///	https://web.archive.org/web/20170825184056/http://number-none.com/product/Understanding%20Slerp,%20Then%20Not%20Using%20It/
+B2_INLINE b2Rot b2NLerp( b2Rot q1, b2Rot q2, float t )
+{
+	float omt = 1.0f - t;
+	b2Rot q = {
+		omt * q1.c + t * q2.c,
+		omt * q1.s + t * q2.s,
+	};
+
+	float mag = sqrtf( q.s * q.s + q.c * q.c );
+	float invMag = mag > 0.0 ? 1.0f / mag : 0.0f;
+	b2Rot qn = { q.c * invMag, q.s * invMag };
+	return qn;
+}
+
+/// Compute the angular velocity necessary to rotate between two rotations over a give time
+/// @param q1 initial rotation
+/// @param q2 final rotation
+/// @param inv_h inverse time step
+B2_INLINE float b2ComputeAngularVelocity( b2Rot q1, b2Rot q2, float inv_h )
+{
+	// ds/dt = omega * cos(t)
+	// dc/dt = -omega * sin(t)
+	// s2 = s1 + omega * h * c1
+	// c2 = c1 - omega * h * s1
+
+	// omega * h * s1 = c1 - c2
+	// omega * h * c1 = s2 - s1
+	// omega * h = (c1 - c2) * s1 + (s2 - s1) * c1;
+	// omega * h = s1 * c1 - c2 * s1 + s2 * c1 - s1 * c1
+	// omega * h = s2 * c1 - c2 * s1 = sin(a2 - a1) ~= a2 - a1 for small delta
+	float omega = inv_h * ( q2.s * q1.c - q2.c * q1.s );
+	return omega;
+}
+
+/// Get the angle in radians in the range [-pi, pi]
+B2_INLINE float b2Rot_GetAngle( b2Rot q )
+{
+	return b2Atan2( q.s, q.c );
+}
+
+/// Get the x-axis
+B2_INLINE b2Vec2 b2Rot_GetXAxis( b2Rot q )
+{
+	b2Vec2 v = { q.c, q.s };
+	return v;
+}
+
+/// Get the y-axis
+B2_INLINE b2Vec2 b2Rot_GetYAxis( b2Rot q )
+{
+	b2Vec2 v = { -q.s, q.c };
+	return v;
+}
+
+/// Multiply two rotations: q * r
+B2_INLINE b2Rot b2MulRot( b2Rot q, b2Rot r )
+{
+	// [qc -qs] * [rc -rs] = [qc*rc-qs*rs -qc*rs-qs*rc]
+	// [qs  qc]   [rs  rc]   [qs*rc+qc*rs -qs*rs+qc*rc]
+	// s(q + r) = qs * rc + qc * rs
+	// c(q + r) = qc * rc - qs * rs
+	b2Rot qr;
+	qr.s = q.s * r.c + q.c * r.s;
+	qr.c = q.c * r.c - q.s * r.s;
+	return qr;
+}
+
+/// Transpose multiply two rotations: qT * r
+B2_INLINE b2Rot b2InvMulRot( b2Rot q, b2Rot r )
+{
+	// [ qc qs] * [rc -rs] = [qc*rc+qs*rs -qc*rs+qs*rc]
+	// [-qs qc]   [rs  rc]   [-qs*rc+qc*rs qs*rs+qc*rc]
+	// s(q - r) = qc * rs - qs * rc
+	// c(q - r) = qc * rc + qs * rs
+	b2Rot qr;
+	qr.s = q.c * r.s - q.s * r.c;
+	qr.c = q.c * r.c + q.s * r.s;
+	return qr;
+}
+
+/// relative angle between b and a (rot_b * inv(rot_a))
+B2_INLINE float b2RelativeAngle( b2Rot b, b2Rot a )
+{
+	// sin(b - a) = bs * ac - bc * as
+	// cos(b - a) = bc * ac + bs * as
+	float s = b.s * a.c - b.c * a.s;
+	float c = b.c * a.c + b.s * a.s;
+	return b2Atan2( s, c );
+}
+
+/// Convert an angle in the range [-2*pi, 2*pi] into the range [-pi, pi]
+B2_INLINE float b2UnwindAngle( float radians )
+{
+	if ( radians < -B2_PI )
+	{
+		return radians + 2.0f * B2_PI;
+	}
+	else if ( radians > B2_PI )
+	{
+		return radians - 2.0f * B2_PI;
+	}
+
+	return radians;
+}
+
+/// Convert any into the range [-pi, pi] (slow)
+B2_INLINE float b2UnwindLargeAngle( float radians )
+{
+	while ( radians > B2_PI )
+	{
+		radians -= 2.0f * B2_PI;
+	}
+
+	while ( radians < -B2_PI )
+	{
+		radians += 2.0f * B2_PI;
+	}
+
+	return radians;
+}
+
+/// Rotate a vector
+B2_INLINE b2Vec2 b2RotateVector( b2Rot q, b2Vec2 v )
+{
+	return B2_LITERAL( b2Vec2 ){ q.c * v.x - q.s * v.y, q.s * v.x + q.c * v.y };
+}
+
+/// Inverse rotate a vector
+B2_INLINE b2Vec2 b2InvRotateVector( b2Rot q, b2Vec2 v )
+{
+	return B2_LITERAL( b2Vec2 ){ q.c * v.x + q.s * v.y, -q.s * v.x + q.c * v.y };
+}
+
+/// Transform a point (e.g. local space to world space)
+B2_INLINE b2Vec2 b2TransformPoint( b2Transform t, const b2Vec2 p )
+{
+	float x = ( t.q.c * p.x - t.q.s * p.y ) + t.p.x;
+	float y = ( t.q.s * p.x + t.q.c * p.y ) + t.p.y;
+
+	return B2_LITERAL( b2Vec2 ){ x, y };
+}
+
+/// Inverse transform a point (e.g. world space to local space)
+B2_INLINE b2Vec2 b2InvTransformPoint( b2Transform t, const b2Vec2 p )
+{
+	float vx = p.x - t.p.x;
+	float vy = p.y - t.p.y;
+	return B2_LITERAL( b2Vec2 ){ t.q.c * vx + t.q.s * vy, -t.q.s * vx + t.q.c * vy };
+}
+
+/// Multiply two transforms. If the result is applied to a point p local to frame B,
+/// the transform would first convert p to a point local to frame A, then into a point
+/// in the world frame.
+/// v2 = A.q.Rot(B.q.Rot(v1) + B.p) + A.p
+///    = (A.q * B.q).Rot(v1) + A.q.Rot(B.p) + A.p
+B2_INLINE b2Transform b2MulTransforms( b2Transform A, b2Transform B )
+{
+	b2Transform C;
+	C.q = b2MulRot( A.q, B.q );
+	C.p = b2Add( b2RotateVector( A.q, B.p ), A.p );
+	return C;
+}
+
+/// Creates a transform that converts a local point in frame B to a local point in frame A.
+/// v2 = A.q' * (B.q * v1 + B.p - A.p)
+///    = A.q' * B.q * v1 + A.q' * (B.p - A.p)
+B2_INLINE b2Transform b2InvMulTransforms( b2Transform A, b2Transform B )
+{
+	b2Transform C;
+	C.q = b2InvMulRot( A.q, B.q );
+	C.p = b2InvRotateVector( A.q, b2Sub( B.p, A.p ) );
+	return C;
+}
+
+/// Multiply a 2-by-2 matrix times a 2D vector
+B2_INLINE b2Vec2 b2MulMV( b2Mat22 A, b2Vec2 v )
+{
+	b2Vec2 u = {
+		A.cx.x * v.x + A.cy.x * v.y,
+		A.cx.y * v.x + A.cy.y * v.y,
+	};
+	return u;
+}
+
+/// Get the inverse of a 2-by-2 matrix
+B2_INLINE b2Mat22 b2GetInverse22( b2Mat22 A )
+{
+	float a = A.cx.x, b = A.cy.x, c = A.cx.y, d = A.cy.y;
+	float det = a * d - b * c;
+	if ( det != 0.0f )
+	{
+		det = 1.0f / det;
+	}
+
+	b2Mat22 B = {
+		{ det * d, -det * c },
+		{ -det * b, det * a },
+	};
+	return B;
+}
+
+/// Solve A * x = b, where b is a column vector. This is more efficient
+/// than computing the inverse in one-shot cases.
+B2_INLINE b2Vec2 b2Solve22( b2Mat22 A, b2Vec2 b )
+{
+	float a11 = A.cx.x, a12 = A.cy.x, a21 = A.cx.y, a22 = A.cy.y;
+	float det = a11 * a22 - a12 * a21;
+	if ( det != 0.0f )
+	{
+		det = 1.0f / det;
+	}
+	b2Vec2 x = { det * ( a22 * b.x - a12 * b.y ), det * ( a11 * b.y - a21 * b.x ) };
+	return x;
+}
+
+/// Does a fully contain b
+B2_INLINE bool b2AABB_Contains( b2AABB a, b2AABB b )
+{
+	bool s = true;
+	s = s && a.lowerBound.x <= b.lowerBound.x;
+	s = s && a.lowerBound.y <= b.lowerBound.y;
+	s = s && b.upperBound.x <= a.upperBound.x;
+	s = s && b.upperBound.y <= a.upperBound.y;
+	return s;
+}
+
+/// Get the center of the AABB.
+B2_INLINE b2Vec2 b2AABB_Center( b2AABB a )
+{
+	b2Vec2 b = { 0.5f * ( a.lowerBound.x + a.upperBound.x ), 0.5f * ( a.lowerBound.y + a.upperBound.y ) };
+	return b;
+}
+
+/// Get the extents of the AABB (half-widths).
+B2_INLINE b2Vec2 b2AABB_Extents( b2AABB a )
+{
+	b2Vec2 b = { 0.5f * ( a.upperBound.x - a.lowerBound.x ), 0.5f * ( a.upperBound.y - a.lowerBound.y ) };
+	return b;
+}
+
+/// Union of two AABBs
+B2_INLINE b2AABB b2AABB_Union( b2AABB a, b2AABB b )
+{
+	b2AABB c;
+	c.lowerBound.x = b2MinFloat( a.lowerBound.x, b.lowerBound.x );
+	c.lowerBound.y = b2MinFloat( a.lowerBound.y, b.lowerBound.y );
+	c.upperBound.x = b2MaxFloat( a.upperBound.x, b.upperBound.x );
+	c.upperBound.y = b2MaxFloat( a.upperBound.y, b.upperBound.y );
+	return c;
+}
+
+/// Compute the bounding box of an array of circles
+B2_INLINE b2AABB b2MakeAABB( const b2Vec2* points, int count, float radius )
+{
+	B2_ASSERT( count > 0 );
+	b2AABB a = { points[0], points[0] };
+	for ( int i = 1; i < count; ++i )
+	{
+		a.lowerBound = b2Min( a.lowerBound, points[i] );
+		a.upperBound = b2Max( a.upperBound, points[i] );
+	}
+
+	b2Vec2 r = { radius, radius };
+	a.lowerBound = b2Sub( a.lowerBound, r );
+	a.upperBound = b2Add( a.upperBound, r );
+
+	return a;
+}
+
+/// Signed separation of a point from a plane
+B2_INLINE float b2PlaneSeparation( b2Plane plane, b2Vec2 point )
+{
+	return b2Dot( plane.normal, point ) - plane.offset;
+}
+
+/// Is this a valid number? Not NaN or infinity.
+B2_API bool b2IsValidFloat( float a );
+
+/// Is this a valid vector? Not NaN or infinity.
+B2_API bool b2IsValidVec2( b2Vec2 v );
+
+/// Is this a valid rotation? Not NaN or infinity. Is normalized.
+B2_API bool b2IsValidRotation( b2Rot q );
+
+/// Is this a valid bounding box? Not Nan or infinity. Upper bound greater than or equal to lower bound.
+B2_API bool b2IsValidAABB( b2AABB aabb );
+
+/// Is this a valid plane? Normal is a unit vector. Not Nan or infinity.
+B2_API bool b2IsValidPlane( b2Plane a );
+
+/// Box2D bases all length units on meters, but you may need different units for your game.
+/// You can set this value to use different units. This should be done at application startup
+/// and only modified once. Default value is 1.
+/// For example, if your game uses pixels for units you can use pixels for all length values
+/// sent to Box2D. There should be no extra cost. However, Box2D has some internal tolerances
+/// and thresholds that have been tuned for meters. By calling this function, Box2D is able
+/// to adjust those tolerances and thresholds to improve accuracy.
+/// A good rule of thumb is to pass the height of your player character to this function. So
+/// if your player character is 32 pixels high, then pass 32 to this function. Then you may
+/// confidently use pixels for all the length values sent to Box2D. All length values returned
+/// from Box2D will also be pixels because Box2D does not do any scaling internally.
+/// However, you are now on the hook for coming up with good values for gravity, density, and
+/// forces.
+/// @warning This must be modified before any calls to Box2D
+B2_API void b2SetLengthUnitsPerMeter( float lengthUnits );
+
+/// Get the current length units per meter.
+B2_API float b2GetLengthUnitsPerMeter( void );
+
+/**@}*/
+
+/**
+ * @defgroup math_cpp C++ Math
+ * @brief Math operator overloads for C++
+ *
+ * See math_functions.h for details.
+ * @{
+ */
+
+#ifdef __cplusplus
+
+/// Unary add one vector to another
+inline void operator+=( b2Vec2& a, b2Vec2 b )
+{
+	a.x += b.x;
+	a.y += b.y;
+}
+
+/// Unary subtract one vector from another
+inline void operator-=( b2Vec2& a, b2Vec2 b )
+{
+	a.x -= b.x;
+	a.y -= b.y;
+}
+
+/// Unary multiply a vector by a scalar
+inline void operator*=( b2Vec2& a, float b )
+{
+	a.x *= b;
+	a.y *= b;
+}
+
+/// Unary negate a vector
+inline b2Vec2 operator-( b2Vec2 a )
+{
+	return { -a.x, -a.y };
+}
+
+/// Binary vector addition
+inline b2Vec2 operator+( b2Vec2 a, b2Vec2 b )
+{
+	return { a.x + b.x, a.y + b.y };
+}
+
+/// Binary vector subtraction
+inline b2Vec2 operator-( b2Vec2 a, b2Vec2 b )
+{
+	return { a.x - b.x, a.y - b.y };
+}
+
+/// Binary scalar and vector multiplication
+inline b2Vec2 operator*( float a, b2Vec2 b )
+{
+	return { a * b.x, a * b.y };
+}
+
+/// Binary scalar and vector multiplication
+inline b2Vec2 operator*( b2Vec2 a, float b )
+{
+	return { a.x * b, a.y * b };
+}
+
+/// Binary vector equality
+inline bool operator==( b2Vec2 a, b2Vec2 b )
+{
+	return a.x == b.x && a.y == b.y;
+}
+
+/// Binary vector inequality
+inline bool operator!=( b2Vec2 a, b2Vec2 b )
+{
+	return a.x != b.x || a.y != b.y;
+}
+
+#endif
+
+/**@}*/

src/vendor/box2d/motor_joint.c

@@ -0,0 +1,283 @@
+// SPDX-FileCopyrightText: 2023 Erin Catto
+// SPDX-License-Identifier: MIT
+
+#include "body.h"
+#include "core.h"
+#include "joint.h"
+#include "solver.h"
+#include "solver_set.h"
+#include "world.h"
+
+// needed for dll export
+#include "box2d/box2d.h"
+
+void b2MotorJoint_SetLinearOffset( b2JointId jointId, b2Vec2 linearOffset )
+{
+	b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_motorJoint );
+	joint->motorJoint.linearOffset = linearOffset;
+}
+
+b2Vec2 b2MotorJoint_GetLinearOffset( b2JointId jointId )
+{
+	b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_motorJoint );
+	return joint->motorJoint.linearOffset;
+}
+
+void b2MotorJoint_SetAngularOffset( b2JointId jointId, float angularOffset )
+{
+	b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_motorJoint );
+	joint->motorJoint.angularOffset = b2ClampFloat( angularOffset, -B2_PI, B2_PI );
+}
+
+float b2MotorJoint_GetAngularOffset( b2JointId jointId )
+{
+	b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_motorJoint );
+	return joint->motorJoint.angularOffset;
+}
+
+void b2MotorJoint_SetMaxForce( b2JointId jointId, float maxForce )
+{
+	b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_motorJoint );
+	joint->motorJoint.maxForce = b2MaxFloat( 0.0f, maxForce );
+}
+
+float b2MotorJoint_GetMaxForce( b2JointId jointId )
+{
+	b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_motorJoint );
+	return joint->motorJoint.maxForce;
+}
+
+void b2MotorJoint_SetMaxTorque( b2JointId jointId, float maxTorque )
+{
+	b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_motorJoint );
+	joint->motorJoint.maxTorque = b2MaxFloat( 0.0f, maxTorque );
+}
+
+float b2MotorJoint_GetMaxTorque( b2JointId jointId )
+{
+	b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_motorJoint );
+	return joint->motorJoint.maxTorque;
+}
+
+void b2MotorJoint_SetCorrectionFactor( b2JointId jointId, float correctionFactor )
+{
+	b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_motorJoint );
+	joint->motorJoint.correctionFactor = b2ClampFloat( correctionFactor, 0.0f, 1.0f );
+}
+
+float b2MotorJoint_GetCorrectionFactor( b2JointId jointId )
+{
+	b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_motorJoint );
+	return joint->motorJoint.correctionFactor;
+}
+
+b2Vec2 b2GetMotorJointForce( b2World* world, b2JointSim* base )
+{
+	b2Vec2 force = b2MulSV( world->inv_h, base->motorJoint.linearImpulse );
+	return force;
+}
+
+float b2GetMotorJointTorque( b2World* world, b2JointSim* base )
+{
+	return world->inv_h * base->motorJoint.angularImpulse;
+}
+
+// Point-to-point constraint
+// C = p2 - p1
+// Cdot = v2 - v1
+//      = v2 + cross(w2, r2) - v1 - cross(w1, r1)
+// J = [-I -r1_skew I r2_skew ]
+// Identity used:
+// w k % (rx i + ry j) = w * (-ry i + rx j)
+
+// Angle constraint
+// C = angle2 - angle1 - referenceAngle
+// Cdot = w2 - w1
+// J = [0 0 -1 0 0 1]
+// K = invI1 + invI2
+
+void b2PrepareMotorJoint( b2JointSim* base, b2StepContext* context )
+{
+	B2_ASSERT( base->type == b2_motorJoint );
+
+	// chase body id to the solver set where the body lives
+	int idA = base->bodyIdA;
+	int idB = base->bodyIdB;
+
+	b2World* world = context->world;
+
+	b2Body* bodyA = b2BodyArray_Get( &world->bodies, idA );
+	b2Body* bodyB = b2BodyArray_Get( &world->bodies, idB );
+
+	B2_ASSERT( bodyA->setIndex == b2_awakeSet || bodyB->setIndex == b2_awakeSet );
+
+	b2SolverSet* setA = b2SolverSetArray_Get( &world->solverSets, bodyA->setIndex );
+	b2SolverSet* setB = b2SolverSetArray_Get( &world->solverSets, bodyB->setIndex );
+
+	int localIndexA = bodyA->localIndex;
+	int localIndexB = bodyB->localIndex;
+
+	b2BodySim* bodySimA = b2BodySimArray_Get( &setA->bodySims, localIndexA );
+	b2BodySim* bodySimB = b2BodySimArray_Get( &setB->bodySims, localIndexB );
+
+	float mA = bodySimA->invMass;
+	float iA = bodySimA->invInertia;
+	float mB = bodySimB->invMass;
+	float iB = bodySimB->invInertia;
+
+	base->invMassA = mA;
+	base->invMassB = mB;
+	base->invIA = iA;
+	base->invIB = iB;
+
+	b2MotorJoint* joint = &base->motorJoint;
+	joint->indexA = bodyA->setIndex == b2_awakeSet ? localIndexA : B2_NULL_INDEX;
+	joint->indexB = bodyB->setIndex == b2_awakeSet ? localIndexB : B2_NULL_INDEX;
+
+	joint->anchorA = b2RotateVector( bodySimA->transform.q, b2Sub( base->localOriginAnchorA, bodySimA->localCenter ) );
+	joint->anchorB = b2RotateVector( bodySimB->transform.q, b2Sub( base->localOriginAnchorB, bodySimB->localCenter ) );
+	joint->deltaCenter = b2Sub( b2Sub( bodySimB->center, bodySimA->center ), joint->linearOffset );
+	joint->deltaAngle = b2RelativeAngle( bodySimB->transform.q, bodySimA->transform.q ) - joint->angularOffset;
+	joint->deltaAngle = b2UnwindAngle( joint->deltaAngle );
+
+	b2Vec2 rA = joint->anchorA;
+	b2Vec2 rB = joint->anchorB;
+
+	b2Mat22 K;
+	K.cx.x = mA + mB + rA.y * rA.y * iA + rB.y * rB.y * iB;
+	K.cx.y = -rA.y * rA.x * iA - rB.y * rB.x * iB;
+	K.cy.x = K.cx.y;
+	K.cy.y = mA + mB + rA.x * rA.x * iA + rB.x * rB.x * iB;
+	joint->linearMass = b2GetInverse22( K );
+
+	float ka = iA + iB;
+	joint->angularMass = ka > 0.0f ? 1.0f / ka : 0.0f;
+
+	if ( context->enableWarmStarting == false )
+	{
+		joint->linearImpulse = b2Vec2_zero;
+		joint->angularImpulse = 0.0f;
+	}
+}
+
+void b2WarmStartMotorJoint( b2JointSim* base, b2StepContext* context )
+{
+	float mA = base->invMassA;
+	float mB = base->invMassB;
+	float iA = base->invIA;
+	float iB = base->invIB;
+
+	b2MotorJoint* joint = &base->motorJoint;
+
+	// dummy state for static bodies
+	b2BodyState dummyState = b2_identityBodyState;
+
+	b2BodyState* bodyA = joint->indexA == B2_NULL_INDEX ? &dummyState : context->states + joint->indexA;
+	b2BodyState* bodyB = joint->indexB == B2_NULL_INDEX ? &dummyState : context->states + joint->indexB;
+
+	b2Vec2 rA = b2RotateVector( bodyA->deltaRotation, joint->anchorA );
+	b2Vec2 rB = b2RotateVector( bodyB->deltaRotation, joint->anchorB );
+
+	bodyA->linearVelocity = b2MulSub( bodyA->linearVelocity, mA, joint->linearImpulse );
+	bodyA->angularVelocity -= iA * ( b2Cross( rA, joint->linearImpulse ) + joint->angularImpulse );
+	bodyB->linearVelocity = b2MulAdd( bodyB->linearVelocity, mB, joint->linearImpulse );
+	bodyB->angularVelocity += iB * ( b2Cross( rB, joint->linearImpulse ) + joint->angularImpulse );
+}
+
+void b2SolveMotorJoint( b2JointSim* base, b2StepContext* context, bool useBias )
+{
+	B2_UNUSED( useBias );
+	B2_ASSERT( base->type == b2_motorJoint );
+
+	float mA = base->invMassA;
+	float mB = base->invMassB;
+	float iA = base->invIA;
+	float iB = base->invIB;
+
+	// dummy state for static bodies
+	b2BodyState dummyState = b2_identityBodyState;
+
+	b2MotorJoint* joint = &base->motorJoint;
+	b2BodyState* bodyA = joint->indexA == B2_NULL_INDEX ? &dummyState : context->states + joint->indexA;
+	b2BodyState* bodyB = joint->indexB == B2_NULL_INDEX ? &dummyState : context->states + joint->indexB;
+
+	b2Vec2 vA = bodyA->linearVelocity;
+	float wA = bodyA->angularVelocity;
+	b2Vec2 vB = bodyB->linearVelocity;
+	float wB = bodyB->angularVelocity;
+
+	// angular constraint
+	{
+		float angularSeperation = b2RelativeAngle( bodyB->deltaRotation, bodyA->deltaRotation ) + joint->deltaAngle;
+		angularSeperation = b2UnwindAngle( angularSeperation );
+
+		float angularBias = context->inv_h * joint->correctionFactor * angularSeperation;
+
+		float Cdot = wB - wA;
+		float impulse = -joint->angularMass * ( Cdot + angularBias );
+
+		float oldImpulse = joint->angularImpulse;
+		float maxImpulse = context->h * joint->maxTorque;
+		joint->angularImpulse = b2ClampFloat( joint->angularImpulse + impulse, -maxImpulse, maxImpulse );
+		impulse = joint->angularImpulse - oldImpulse;
+
+		wA -= iA * impulse;
+		wB += iB * impulse;
+	}
+
+	// linear constraint
+	{
+		b2Vec2 rA = b2RotateVector( bodyA->deltaRotation, joint->anchorA );
+		b2Vec2 rB = b2RotateVector( bodyB->deltaRotation, joint->anchorB );
+
+		b2Vec2 ds = b2Add( b2Sub( bodyB->deltaPosition, bodyA->deltaPosition ), b2Sub( rB, rA ) );
+		b2Vec2 linearSeparation = b2Add( joint->deltaCenter, ds );
+		b2Vec2 linearBias = b2MulSV( context->inv_h * joint->correctionFactor, linearSeparation );
+
+		b2Vec2 Cdot = b2Sub( b2Add( vB, b2CrossSV( wB, rB ) ), b2Add( vA, b2CrossSV( wA, rA ) ) );
+		b2Vec2 b = b2MulMV( joint->linearMass, b2Add( Cdot, linearBias ) );
+		b2Vec2 impulse = { -b.x, -b.y };
+
+		b2Vec2 oldImpulse = joint->linearImpulse;
+		float maxImpulse = context->h * joint->maxForce;
+		joint->linearImpulse = b2Add( joint->linearImpulse, impulse );
+
+		if ( b2LengthSquared( joint->linearImpulse ) > maxImpulse * maxImpulse )
+		{
+			joint->linearImpulse = b2Normalize( joint->linearImpulse );
+			joint->linearImpulse.x *= maxImpulse;
+			joint->linearImpulse.y *= maxImpulse;
+		}
+
+		impulse = b2Sub( joint->linearImpulse, oldImpulse );
+
+		vA = b2MulSub( vA, mA, impulse );
+		wA -= iA * b2Cross( rA, impulse );
+		vB = b2MulAdd( vB, mB, impulse );
+		wB += iB * b2Cross( rB, impulse );
+	}
+
+	bodyA->linearVelocity = vA;
+	bodyA->angularVelocity = wA;
+	bodyB->linearVelocity = vB;
+	bodyB->angularVelocity = wB;
+}
+
+#if 0
+void b2DumpMotorJoint()
+{
+	int32 indexA = m_bodyA->m_islandIndex;
+	int32 indexB = m_bodyB->m_islandIndex;
+
+	b2Dump("  b2MotorJointDef jd;\n");
+	b2Dump("  jd.bodyA = sims[%d];\n", indexA);
+	b2Dump("  jd.bodyB = sims[%d];\n", indexB);
+	b2Dump("  jd.collideConnected = bool(%d);\n", m_collideConnected);
+	b2Dump("  jd.localAnchorA.Set(%.9g, %.9g);\n", m_localAnchorA.x, m_localAnchorA.y);
+	b2Dump("  jd.localAnchorB.Set(%.9g, %.9g);\n", m_localAnchorB.x, m_localAnchorB.y);
+	b2Dump("  jd.referenceAngle = %.9g;\n", m_referenceAngle);
+	b2Dump("  jd.stiffness = %.9g;\n", m_stiffness);
+	b2Dump("  jd.damping = %.9g;\n", m_damping);
+	b2Dump("  joints[%d] = m_world->CreateJoint(&jd);\n", m_index);
+}
+#endif

src/vendor/box2d/mouse_joint.c

@@ -0,0 +1,214 @@
+// SPDX-FileCopyrightText: 2023 Erin Catto
+// SPDX-License-Identifier: MIT
+
+#include "body.h"
+#include "core.h"
+#include "joint.h"
+#include "solver.h"
+#include "solver_set.h"
+#include "world.h"
+
+// needed for dll export
+#include "box2d/box2d.h"
+
+void b2MouseJoint_SetTarget( b2JointId jointId, b2Vec2 target )
+{
+	B2_ASSERT( b2IsValidVec2( target ) );
+	b2JointSim* base = b2GetJointSimCheckType( jointId, b2_mouseJoint );
+	base->mouseJoint.targetA = target;
+}
+
+b2Vec2 b2MouseJoint_GetTarget( b2JointId jointId )
+{
+	b2JointSim* base = b2GetJointSimCheckType( jointId, b2_mouseJoint );
+	return base->mouseJoint.targetA;
+}
+
+void b2MouseJoint_SetSpringHertz( b2JointId jointId, float hertz )
+{
+	B2_ASSERT( b2IsValidFloat( hertz ) && hertz >= 0.0f );
+	b2JointSim* base = b2GetJointSimCheckType( jointId, b2_mouseJoint );
+	base->mouseJoint.hertz = hertz;
+}
+
+float b2MouseJoint_GetSpringHertz( b2JointId jointId )
+{
+	b2JointSim* base = b2GetJointSimCheckType( jointId, b2_mouseJoint );
+	return base->mouseJoint.hertz;
+}
+
+void b2MouseJoint_SetSpringDampingRatio( b2JointId jointId, float dampingRatio )
+{
+	B2_ASSERT( b2IsValidFloat( dampingRatio ) && dampingRatio >= 0.0f );
+	b2JointSim* base = b2GetJointSimCheckType( jointId, b2_mouseJoint );
+	base->mouseJoint.dampingRatio = dampingRatio;
+}
+
+float b2MouseJoint_GetSpringDampingRatio( b2JointId jointId )
+{
+	b2JointSim* base = b2GetJointSimCheckType( jointId, b2_mouseJoint );
+	return base->mouseJoint.dampingRatio;
+}
+
+void b2MouseJoint_SetMaxForce( b2JointId jointId, float maxForce )
+{
+	B2_ASSERT( b2IsValidFloat( maxForce ) && maxForce >= 0.0f );
+	b2JointSim* base = b2GetJointSimCheckType( jointId, b2_mouseJoint );
+	base->mouseJoint.maxForce = maxForce;
+}
+
+float b2MouseJoint_GetMaxForce( b2JointId jointId )
+{
+	b2JointSim* base = b2GetJointSimCheckType( jointId, b2_mouseJoint );
+	return base->mouseJoint.maxForce;
+}
+
+b2Vec2 b2GetMouseJointForce( b2World* world, b2JointSim* base )
+{
+	b2Vec2 force = b2MulSV( world->inv_h, base->mouseJoint.linearImpulse );
+	return force;
+}
+
+float b2GetMouseJointTorque( b2World* world, b2JointSim* base )
+{
+	return world->inv_h * base->mouseJoint.angularImpulse;
+}
+
+void b2PrepareMouseJoint( b2JointSim* base, b2StepContext* context )
+{
+	B2_ASSERT( base->type == b2_mouseJoint );
+
+	// chase body id to the solver set where the body lives
+	int idB = base->bodyIdB;
+
+	b2World* world = context->world;
+
+	b2Body* bodyB = b2BodyArray_Get( &world->bodies, idB );
+
+	B2_ASSERT( bodyB->setIndex == b2_awakeSet );
+	b2SolverSet* setB = b2SolverSetArray_Get( &world->solverSets, bodyB->setIndex );
+
+	int localIndexB = bodyB->localIndex;
+	b2BodySim* bodySimB = b2BodySimArray_Get( &setB->bodySims, localIndexB );
+
+	base->invMassB = bodySimB->invMass;
+	base->invIB = bodySimB->invInertia;
+
+	b2MouseJoint* joint = &base->mouseJoint;
+	joint->indexB = bodyB->setIndex == b2_awakeSet ? localIndexB : B2_NULL_INDEX;
+	joint->anchorB = b2RotateVector( bodySimB->transform.q, b2Sub( base->localOriginAnchorB, bodySimB->localCenter ) );
+
+	joint->linearSoftness = b2MakeSoft( joint->hertz, joint->dampingRatio, context->h );
+
+	float angularHertz = 0.5f;
+	float angularDampingRatio = 0.1f;
+	joint->angularSoftness = b2MakeSoft( angularHertz, angularDampingRatio, context->h );
+
+	b2Vec2 rB = joint->anchorB;
+	float mB = bodySimB->invMass;
+	float iB = bodySimB->invInertia;
+
+	// K = [(1/m1 + 1/m2) * eye(2) - skew(r1) * invI1 * skew(r1) - skew(r2) * invI2 * skew(r2)]
+	//   = [1/m1+1/m2     0    ] + invI1 * [r1.y*r1.y -r1.x*r1.y] + invI2 * [r1.y*r1.y -r1.x*r1.y]
+	//     [    0     1/m1+1/m2]           [-r1.x*r1.y r1.x*r1.x]           [-r1.x*r1.y r1.x*r1.x]
+	b2Mat22 K;
+	K.cx.x = mB + iB * rB.y * rB.y;
+	K.cx.y = -iB * rB.x * rB.y;
+	K.cy.x = K.cx.y;
+	K.cy.y = mB + iB * rB.x * rB.x;
+
+	joint->linearMass = b2GetInverse22( K );
+	joint->deltaCenter = b2Sub( bodySimB->center, joint->targetA );
+
+	if ( context->enableWarmStarting == false )
+	{
+		joint->linearImpulse = b2Vec2_zero;
+		joint->angularImpulse = 0.0f;
+	}
+}
+
+void b2WarmStartMouseJoint( b2JointSim* base, b2StepContext* context )
+{
+	B2_ASSERT( base->type == b2_mouseJoint );
+
+	float mB = base->invMassB;
+	float iB = base->invIB;
+
+	b2MouseJoint* joint = &base->mouseJoint;
+
+	b2BodyState* stateB = context->states + joint->indexB;
+	b2Vec2 vB = stateB->linearVelocity;
+	float wB = stateB->angularVelocity;
+
+	b2Rot dqB = stateB->deltaRotation;
+	b2Vec2 rB = b2RotateVector( dqB, joint->anchorB );
+
+	vB = b2MulAdd( vB, mB, joint->linearImpulse );
+	wB += iB * ( b2Cross( rB, joint->linearImpulse ) + joint->angularImpulse );
+
+	stateB->linearVelocity = vB;
+	stateB->angularVelocity = wB;
+}
+
+void b2SolveMouseJoint( b2JointSim* base, b2StepContext* context )
+{
+	float mB = base->invMassB;
+	float iB = base->invIB;
+
+	b2MouseJoint* joint = &base->mouseJoint;
+	b2BodyState* stateB = context->states + joint->indexB;
+
+	b2Vec2 vB = stateB->linearVelocity;
+	float wB = stateB->angularVelocity;
+
+	// Softness with no bias to reduce rotation speed
+	{
+		float massScale = joint->angularSoftness.massScale;
+		float impulseScale = joint->angularSoftness.impulseScale;
+
+		float impulse = iB > 0.0f ? -wB / iB : 0.0f;
+		impulse = massScale * impulse - impulseScale * joint->angularImpulse;
+		joint->angularImpulse += impulse;
+
+		wB += iB * impulse;
+	}
+
+	float maxImpulse = joint->maxForce * context->h;
+
+	{
+		b2Rot dqB = stateB->deltaRotation;
+		b2Vec2 rB = b2RotateVector( dqB, joint->anchorB );
+		b2Vec2 Cdot = b2Add( vB, b2CrossSV( wB, rB ) );
+
+		b2Vec2 separation = b2Add( b2Add( stateB->deltaPosition, rB ), joint->deltaCenter );
+		b2Vec2 bias = b2MulSV( joint->linearSoftness.biasRate, separation );
+
+		float massScale = joint->linearSoftness.massScale;
+		float impulseScale = joint->linearSoftness.impulseScale;
+
+		b2Vec2 b = b2MulMV( joint->linearMass, b2Add( Cdot, bias ) );
+
+		b2Vec2 impulse;
+		impulse.x = -massScale * b.x - impulseScale * joint->linearImpulse.x;
+		impulse.y = -massScale * b.y - impulseScale * joint->linearImpulse.y;
+
+		b2Vec2 oldImpulse = joint->linearImpulse;
+		joint->linearImpulse.x += impulse.x;
+		joint->linearImpulse.y += impulse.y;
+
+		float mag = b2Length( joint->linearImpulse );
+		if ( mag > maxImpulse )
+		{
+			joint->linearImpulse = b2MulSV( maxImpulse, b2Normalize( joint->linearImpulse ) );
+		}
+
+		impulse.x = joint->linearImpulse.x - oldImpulse.x;
+		impulse.y = joint->linearImpulse.y - oldImpulse.y;
+
+		vB = b2MulAdd( vB, mB, impulse );
+		wB += iB * b2Cross( rB, impulse );
+	}
+
+	stateB->linearVelocity = vB;
+	stateB->angularVelocity = wB;
+}

src/vendor/box2d/mover.c

@@ -0,0 +1,73 @@
+// SPDX-FileCopyrightText: 2025 Erin Catto
+// SPDX-License-Identifier: MIT
+
+#include "constants.h"
+
+#include "box2d/collision.h"
+
+b2PlaneSolverResult b2SolvePlanes( b2Vec2 position, b2CollisionPlane* planes, int count )
+{
+	for ( int i = 0; i < count; ++i )
+	{
+		planes[i].push = 0.0f;
+	}
+
+	b2Vec2 delta = b2Vec2_zero;
+	float tolerance = B2_LINEAR_SLOP;
+
+	int iteration;
+	for ( iteration = 0; iteration < 20; ++iteration )
+	{
+		float totalPush = 0.0f;
+		for ( int planeIndex = 0; planeIndex < count; ++planeIndex )
+		{
+			b2CollisionPlane* plane = planes + planeIndex;
+
+			// Add slop to prevent jitter
+			float separation = b2PlaneSeparation( plane->plane, delta ) + B2_LINEAR_SLOP;
+			// if (separation > 0.0f)
+			//{
+			//	continue;
+			// }
+
+			float push = -separation;
+
+			// Clamp accumulated push
+			float accumulatedPush = plane->push;
+			plane->push = b2ClampFloat( plane->push + push, 0.0f, plane->pushLimit );
+			push = plane->push - accumulatedPush;
+			delta = b2MulAdd( delta, push, plane->plane.normal );
+
+			// Track maximum push for convergence
+			totalPush += b2AbsFloat( push );
+		}
+
+		if ( totalPush < tolerance )
+		{
+			break;
+		}
+	}
+
+	return (b2PlaneSolverResult){
+		.position = b2Add( delta, position ),
+		.iterationCount = iteration,
+	};
+}
+
+b2Vec2 b2ClipVector( b2Vec2 vector, const b2CollisionPlane* planes, int count )
+{
+	b2Vec2 v = vector;
+
+	for ( int planeIndex = 0; planeIndex < count; ++planeIndex )
+	{
+		const b2CollisionPlane* plane = planes + planeIndex;
+		if ( plane->push == 0.0f || plane->clipVelocity == false )
+		{
+			continue;
+		}
+
+		v = b2MulSub( v, b2MinFloat( 0.0f, b2Dot( v, plane->plane.normal ) ), plane->plane.normal );
+	}
+
+	return v;
+}

src/vendor/box2d/prismatic_joint.c

@@ -0,0 +1,654 @@
+// SPDX-FileCopyrightText: 2023 Erin Catto
+// SPDX-License-Identifier: MIT
+
+#include "body.h"
+#include "core.h"
+#include "joint.h"
+#include "solver.h"
+#include "solver_set.h"
+#include "world.h"
+
+// needed for dll export
+#include "box2d/box2d.h"
+
+#include <stdio.h>
+
+void b2PrismaticJoint_EnableSpring( b2JointId jointId, bool enableSpring )
+{
+	b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_prismaticJoint );
+	if ( enableSpring != joint->prismaticJoint.enableSpring )
+	{
+		joint->prismaticJoint.enableSpring = enableSpring;
+		joint->prismaticJoint.springImpulse = 0.0f;
+	}
+}
+
+bool b2PrismaticJoint_IsSpringEnabled( b2JointId jointId )
+{
+	b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_prismaticJoint );
+	return joint->prismaticJoint.enableSpring;
+}
+
+void b2PrismaticJoint_SetSpringHertz( b2JointId jointId, float hertz )
+{
+	b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_prismaticJoint );
+	joint->prismaticJoint.hertz = hertz;
+}
+
+float b2PrismaticJoint_GetSpringHertz( b2JointId jointId )
+{
+	b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_prismaticJoint );
+	return joint->prismaticJoint.hertz;
+}
+
+void b2PrismaticJoint_SetSpringDampingRatio( b2JointId jointId, float dampingRatio )
+{
+	b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_prismaticJoint );
+	joint->prismaticJoint.dampingRatio = dampingRatio;
+}
+
+float b2PrismaticJoint_GetSpringDampingRatio( b2JointId jointId )
+{
+	b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_prismaticJoint );
+	return joint->prismaticJoint.dampingRatio;
+}
+
+void b2PrismaticJoint_EnableLimit( b2JointId jointId, bool enableLimit )
+{
+	b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_prismaticJoint );
+	if ( enableLimit != joint->prismaticJoint.enableLimit )
+	{
+		joint->prismaticJoint.enableLimit = enableLimit;
+		joint->prismaticJoint.lowerImpulse = 0.0f;
+		joint->prismaticJoint.upperImpulse = 0.0f;
+	}
+}
+
+bool b2PrismaticJoint_IsLimitEnabled( b2JointId jointId )
+{
+	b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_prismaticJoint );
+	return joint->prismaticJoint.enableLimit;
+}
+
+float b2PrismaticJoint_GetLowerLimit( b2JointId jointId )
+{
+	b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_prismaticJoint );
+	return joint->prismaticJoint.lowerTranslation;
+}
+
+float b2PrismaticJoint_GetUpperLimit( b2JointId jointId )
+{
+	b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_prismaticJoint );
+	return joint->prismaticJoint.upperTranslation;
+}
+
+void b2PrismaticJoint_SetLimits( b2JointId jointId, float lower, float upper )
+{
+	b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_prismaticJoint );
+	if ( lower != joint->prismaticJoint.lowerTranslation || upper != joint->prismaticJoint.upperTranslation )
+	{
+		joint->prismaticJoint.lowerTranslation = b2MinFloat( lower, upper );
+		joint->prismaticJoint.upperTranslation = b2MaxFloat( lower, upper );
+		joint->prismaticJoint.lowerImpulse = 0.0f;
+		joint->prismaticJoint.upperImpulse = 0.0f;
+	}
+}
+
+void b2PrismaticJoint_EnableMotor( b2JointId jointId, bool enableMotor )
+{
+	b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_prismaticJoint );
+	if ( enableMotor != joint->prismaticJoint.enableMotor )
+	{
+		joint->prismaticJoint.enableMotor = enableMotor;
+		joint->prismaticJoint.motorImpulse = 0.0f;
+	}
+}
+
+bool b2PrismaticJoint_IsMotorEnabled( b2JointId jointId )
+{
+	b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_prismaticJoint );
+	return joint->prismaticJoint.enableMotor;
+}
+
+void b2PrismaticJoint_SetMotorSpeed( b2JointId jointId, float motorSpeed )
+{
+	b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_prismaticJoint );
+	joint->prismaticJoint.motorSpeed = motorSpeed;
+}
+
+float b2PrismaticJoint_GetMotorSpeed( b2JointId jointId )
+{
+	b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_prismaticJoint );
+	return joint->prismaticJoint.motorSpeed;
+}
+
+float b2PrismaticJoint_GetMotorForce( b2JointId jointId )
+{
+	b2World* world = b2GetWorld( jointId.world0 );
+	b2JointSim* base = b2GetJointSimCheckType( jointId, b2_prismaticJoint );
+	return world->inv_h * base->prismaticJoint.motorImpulse;
+}
+
+void b2PrismaticJoint_SetMaxMotorForce( b2JointId jointId, float force )
+{
+	b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_prismaticJoint );
+	joint->prismaticJoint.maxMotorForce = force;
+}
+
+float b2PrismaticJoint_GetMaxMotorForce( b2JointId jointId )
+{
+	b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_prismaticJoint );
+	return joint->prismaticJoint.maxMotorForce;
+}
+
+float b2PrismaticJoint_GetTranslation(b2JointId jointId)
+{
+	b2World* world = b2GetWorld( jointId.world0 );
+	b2JointSim* jointSim = b2GetJointSimCheckType( jointId, b2_prismaticJoint );
+	b2Transform transformA = b2GetBodyTransform( world, jointSim->bodyIdA );
+	b2Transform transformB = b2GetBodyTransform( world, jointSim->bodyIdB );
+
+	b2PrismaticJoint* joint = &jointSim->prismaticJoint;
+	b2Vec2 axisA = b2RotateVector( transformA.q, joint->localAxisA );
+	b2Vec2 pA = b2TransformPoint( transformA, jointSim->localOriginAnchorA );
+	b2Vec2 pB = b2TransformPoint( transformB, jointSim->localOriginAnchorB );
+	b2Vec2 d = b2Sub( pB, pA );
+	float translation = b2Dot( d, axisA );
+	return translation;
+}
+
+float b2PrismaticJoint_GetSpeed(b2JointId jointId)
+{
+	b2World* world = b2GetWorld( jointId.world0 );
+	b2Joint* joint = b2GetJointFullId( world, jointId );
+	B2_ASSERT( joint->type == b2_prismaticJoint );
+	b2JointSim* jointSim = b2GetJointSim( world, joint );
+	B2_ASSERT( jointSim->type == b2_prismaticJoint );
+
+	b2Body* bodyA = b2BodyArray_Get( &world->bodies, jointSim->bodyIdA );
+	b2Body* bodyB = b2BodyArray_Get( &world->bodies, jointSim->bodyIdB );
+	b2BodySim* bodySimA = b2GetBodySim( world, bodyA );
+	b2BodySim* bodySimB = b2GetBodySim( world, bodyB );
+	b2BodyState* bodyStateA = b2GetBodyState( world, bodyA );
+	b2BodyState* bodyStateB = b2GetBodyState( world, bodyB );
+
+	b2Transform transformA = bodySimA->transform;
+	b2Transform transformB = bodySimB->transform;
+
+	b2PrismaticJoint* prismatic = &jointSim->prismaticJoint;
+	b2Vec2 axisA = b2RotateVector( transformA.q, prismatic->localAxisA );
+	b2Vec2 cA = bodySimA->center;
+	b2Vec2 cB = bodySimB->center;
+	b2Vec2 rA = b2RotateVector( transformA.q, b2Sub( jointSim->localOriginAnchorA, bodySimA->localCenter ) );
+	b2Vec2 rB = b2RotateVector( transformB.q, b2Sub( jointSim->localOriginAnchorB, bodySimB->localCenter ) );
+
+	b2Vec2 d = b2Add(b2Sub(cB, cA), b2Sub( rB, rA ));
+
+	b2Vec2 vA = bodyStateA ? bodyStateA->linearVelocity : b2Vec2_zero;
+	b2Vec2 vB = bodyStateB ? bodyStateB->linearVelocity : b2Vec2_zero;
+	float wA = bodyStateA ? bodyStateA->angularVelocity : 0.0f;
+	float wB = bodyStateB ? bodyStateB->angularVelocity : 0.0f;
+
+	b2Vec2 vRel = b2Sub( b2Add( vB, b2CrossSV( wB, rB ) ), b2Add( vA, b2CrossSV( wA, rA ) ) );
+	float speed = b2Dot( d, b2CrossSV( wA, axisA ) ) + b2Dot( axisA, vRel );
+	return speed;
+}
+
+b2Vec2 b2GetPrismaticJointForce( b2World* world, b2JointSim* base )
+{
+	int idA = base->bodyIdA;
+	b2Transform transformA = b2GetBodyTransform( world, idA );
+
+	b2PrismaticJoint* joint = &base->prismaticJoint;
+
+	b2Vec2 axisA = b2RotateVector( transformA.q, joint->localAxisA );
+	b2Vec2 perpA = b2LeftPerp( axisA );
+
+	float inv_h = world->inv_h;
+	float perpForce = inv_h * joint->impulse.x;
+	float axialForce = inv_h * ( joint->motorImpulse + joint->lowerImpulse - joint->upperImpulse );
+
+	b2Vec2 force = b2Add( b2MulSV( perpForce, perpA ), b2MulSV( axialForce, axisA ) );
+	return force;
+}
+
+float b2GetPrismaticJointTorque( b2World* world, b2JointSim* base )
+{
+	return world->inv_h * base->prismaticJoint.impulse.y;
+}
+
+// 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 b2PreparePrismaticJoint( b2JointSim* base, b2StepContext* context )
+{
+	B2_ASSERT( base->type == b2_prismaticJoint );
+
+	// chase body id to the solver set where the body lives
+	int idA = base->bodyIdA;
+	int idB = base->bodyIdB;
+
+	b2World* world = context->world;
+
+	b2Body* bodyA = b2BodyArray_Get( &world->bodies, idA );
+	b2Body* bodyB = b2BodyArray_Get( &world->bodies, idB );
+
+	B2_ASSERT( bodyA->setIndex == b2_awakeSet || bodyB->setIndex == b2_awakeSet );
+	b2SolverSet* setA = b2SolverSetArray_Get( &world->solverSets, bodyA->setIndex );
+	b2SolverSet* setB = b2SolverSetArray_Get( &world->solverSets, bodyB->setIndex );
+
+	int localIndexA = bodyA->localIndex;
+	int localIndexB = bodyB->localIndex;
+
+	b2BodySim* bodySimA = b2BodySimArray_Get( &setA->bodySims, localIndexA );
+	b2BodySim* bodySimB = b2BodySimArray_Get( &setB->bodySims, localIndexB );
+
+	float mA = bodySimA->invMass;
+	float iA = bodySimA->invInertia;
+	float mB = bodySimB->invMass;
+	float iB = bodySimB->invInertia;
+
+	base->invMassA = mA;
+	base->invMassB = mB;
+	base->invIA = iA;
+	base->invIB = iB;
+
+	b2PrismaticJoint* joint = &base->prismaticJoint;
+	joint->indexA = bodyA->setIndex == b2_awakeSet ? localIndexA : B2_NULL_INDEX;
+	joint->indexB = bodyB->setIndex == b2_awakeSet ? localIndexB : B2_NULL_INDEX;
+
+	b2Rot qA = bodySimA->transform.q;
+	b2Rot qB = bodySimB->transform.q;
+
+	joint->anchorA = b2RotateVector( qA, b2Sub( base->localOriginAnchorA, bodySimA->localCenter ) );
+	joint->anchorB = b2RotateVector( qB, b2Sub( base->localOriginAnchorB, bodySimB->localCenter ) );
+	joint->axisA = b2RotateVector( qA, joint->localAxisA );
+	joint->deltaCenter = b2Sub( bodySimB->center, bodySimA->center );
+	joint->deltaAngle = b2RelativeAngle( qB, qA ) - joint->referenceAngle;
+	joint->deltaAngle = b2UnwindAngle( joint->deltaAngle );
+
+	b2Vec2 rA = joint->anchorA;
+	b2Vec2 rB = joint->anchorB;
+
+	b2Vec2 d = b2Add( joint->deltaCenter, b2Sub( rB, rA ) );
+	float a1 = b2Cross( b2Add( d, rA ), joint->axisA );
+	float a2 = b2Cross( rB, joint->axisA );
+
+	// effective masses
+	float k = mA + mB + iA * a1 * a1 + iB * a2 * a2;
+	joint->axialMass = k > 0.0f ? 1.0f / k : 0.0f;
+
+	joint->springSoftness = b2MakeSoft( joint->hertz, joint->dampingRatio, context->h );
+
+	if ( context->enableWarmStarting == false )
+	{
+		joint->impulse = b2Vec2_zero;
+		joint->springImpulse = 0.0f;
+		joint->motorImpulse = 0.0f;
+		joint->lowerImpulse = 0.0f;
+		joint->upperImpulse = 0.0f;
+	}
+}
+
+void b2WarmStartPrismaticJoint( b2JointSim* base, b2StepContext* context )
+{
+	B2_ASSERT( base->type == b2_prismaticJoint );
+
+	float mA = base->invMassA;
+	float mB = base->invMassB;
+	float iA = base->invIA;
+	float iB = base->invIB;
+
+	// dummy state for static bodies
+	b2BodyState dummyState = b2_identityBodyState;
+
+	b2PrismaticJoint* joint = &base->prismaticJoint;
+
+	b2BodyState* stateA = joint->indexA == B2_NULL_INDEX ? &dummyState : context->states + joint->indexA;
+	b2BodyState* stateB = joint->indexB == B2_NULL_INDEX ? &dummyState : context->states + joint->indexB;
+
+	b2Vec2 rA = b2RotateVector( stateA->deltaRotation, joint->anchorA );
+	b2Vec2 rB = b2RotateVector( stateB->deltaRotation, joint->anchorB );
+
+	b2Vec2 d = b2Add( b2Add( b2Sub( stateB->deltaPosition, stateA->deltaPosition ), joint->deltaCenter ), b2Sub( rB, rA ) );
+	b2Vec2 axisA = b2RotateVector( stateA->deltaRotation, joint->axisA );
+
+	// impulse is applied at anchor point on body B
+	float a1 = b2Cross( b2Add( d, rA ), axisA );
+	float a2 = b2Cross( rB, axisA );
+	float axialImpulse = joint->springImpulse + joint->motorImpulse + joint->lowerImpulse - joint->upperImpulse;
+
+	// perpendicular constraint
+	b2Vec2 perpA = b2LeftPerp( axisA );
+	float s1 = b2Cross( b2Add( d, rA ), perpA );
+	float s2 = b2Cross( rB, perpA );
+	float perpImpulse = joint->impulse.x;
+	float angleImpulse = joint->impulse.y;
+
+	b2Vec2 P = b2Add( b2MulSV( axialImpulse, axisA ), b2MulSV( perpImpulse, perpA ) );
+	float LA = axialImpulse * a1 + perpImpulse * s1 + angleImpulse;
+	float LB = axialImpulse * a2 + perpImpulse * s2 + angleImpulse;
+
+	stateA->linearVelocity = b2MulSub( stateA->linearVelocity, mA, P );
+	stateA->angularVelocity -= iA * LA;
+	stateB->linearVelocity = b2MulAdd( stateB->linearVelocity, mB, P );
+	stateB->angularVelocity += iB * LB;
+}
+
+void b2SolvePrismaticJoint( b2JointSim* base, b2StepContext* context, bool useBias )
+{
+	B2_ASSERT( base->type == b2_prismaticJoint );
+
+	float mA = base->invMassA;
+	float mB = base->invMassB;
+	float iA = base->invIA;
+	float iB = base->invIB;
+
+	// dummy state for static bodies
+	b2BodyState dummyState = b2_identityBodyState;
+
+	b2PrismaticJoint* joint = &base->prismaticJoint;
+
+	b2BodyState* stateA = joint->indexA == B2_NULL_INDEX ? &dummyState : context->states + joint->indexA;
+	b2BodyState* stateB = joint->indexB == B2_NULL_INDEX ? &dummyState : context->states + joint->indexB;
+
+	b2Vec2 vA = stateA->linearVelocity;
+	float wA = stateA->angularVelocity;
+	b2Vec2 vB = stateB->linearVelocity;
+	float wB = stateB->angularVelocity;
+
+	// current anchors
+	b2Vec2 rA = b2RotateVector( stateA->deltaRotation, joint->anchorA );
+	b2Vec2 rB = b2RotateVector( stateB->deltaRotation, joint->anchorB );
+
+	b2Vec2 d = b2Add( b2Add( b2Sub( stateB->deltaPosition, stateA->deltaPosition ), joint->deltaCenter ), b2Sub( rB, rA ) );
+	b2Vec2 axisA = b2RotateVector( stateA->deltaRotation, joint->axisA );
+	float translation = b2Dot( axisA, d );
+
+	// These scalars are for torques generated by axial forces
+	float a1 = b2Cross( b2Add( d, rA ), axisA );
+	float a2 = b2Cross( rB, axisA );
+
+	// spring constraint
+	if ( joint->enableSpring )
+	{
+		// This is a real spring and should be applied even during relax
+		float C = translation;
+		float bias = joint->springSoftness.biasRate * C;
+		float massScale = joint->springSoftness.massScale;
+		float impulseScale = joint->springSoftness.impulseScale;
+
+		float Cdot = b2Dot( axisA, b2Sub( vB, vA ) ) + a2 * wB - a1 * wA;
+		float deltaImpulse = -massScale * joint->axialMass * ( Cdot + bias ) - impulseScale * joint->springImpulse;
+		joint->springImpulse += deltaImpulse;
+
+		b2Vec2 P = b2MulSV( deltaImpulse, axisA );
+		float LA = deltaImpulse * a1;
+		float LB = deltaImpulse * a2;
+
+		vA = b2MulSub( vA, mA, P );
+		wA -= iA * LA;
+		vB = b2MulAdd( vB, mB, P );
+		wB += iB * LB;
+	}
+
+	// Solve motor constraint
+	if ( joint->enableMotor )
+	{
+		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->h * joint->maxMotorForce;
+		joint->motorImpulse = b2ClampFloat( joint->motorImpulse + impulse, -maxImpulse, maxImpulse );
+		impulse = joint->motorImpulse - oldImpulse;
+
+		b2Vec2 P = b2MulSV( impulse, axisA );
+		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 )
+	{
+		// 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_h;
+			}
+			else if ( useBias )
+			{
+				bias = context->jointSoftness.biasRate * C;
+				massScale = context->jointSoftness.massScale;
+				impulseScale = context->jointSoftness.impulseScale;
+			}
+
+			float oldImpulse = joint->lowerImpulse;
+			float Cdot = b2Dot( axisA, b2Sub( vB, vA ) ) + a2 * wB - a1 * wA;
+			float impulse = -joint->axialMass * massScale * ( Cdot + bias ) - impulseScale * oldImpulse;
+			joint->lowerImpulse = b2MaxFloat( oldImpulse + impulse, 0.0f );
+			impulse = joint->lowerImpulse - oldImpulse;
+
+			b2Vec2 P = b2MulSV( impulse, axisA );
+			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_h;
+			}
+			else if ( useBias )
+			{
+				bias = context->jointSoftness.biasRate * C;
+				massScale = context->jointSoftness.massScale;
+				impulseScale = context->jointSoftness.impulseScale;
+			}
+
+			float oldImpulse = joint->upperImpulse;
+			// sign flipped
+			float Cdot = b2Dot( axisA, b2Sub( vA, vB ) ) + a1 * wA - a2 * wB;
+			float impulse = -joint->axialMass * massScale * ( Cdot + bias ) - impulseScale * oldImpulse;
+			joint->upperImpulse = b2MaxFloat( oldImpulse + impulse, 0.0f );
+			impulse = joint->upperImpulse - oldImpulse;
+
+			b2Vec2 P = b2MulSV( impulse, axisA );
+			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 perpA = b2LeftPerp( axisA );
+
+		// These scalars are for torques generated by the perpendicular constraint force
+		float s1 = b2Cross( b2Add( d, rA ), perpA );
+		float s2 = b2Cross( rB, perpA );
+
+		b2Vec2 Cdot;
+		Cdot.x = b2Dot( perpA, 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( perpA, d );
+			C.y = b2RelativeAngle( stateB->deltaRotation, stateA->deltaRotation ) + joint->deltaAngle;
+
+			bias = b2MulSV( context->jointSoftness.biasRate, C );
+			massScale = context->jointSoftness.massScale;
+			impulseScale = context->jointSoftness.impulseScale;
+		}
+
+		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 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;
+
+		joint->impulse.x += impulse.x;
+		joint->impulse.y += impulse.y;
+
+		b2Vec2 P = b2MulSV( impulse.x, perpA );
+		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;
+	}
+
+	stateA->linearVelocity = vA;
+	stateA->angularVelocity = wA;
+	stateB->linearVelocity = vB;
+	stateB->angularVelocity = wB;
+}
+
+#if 0
+void b2PrismaticJoint::Dump()
+{
+	int32 indexA = joint->bodyA->joint->islandIndex;
+	int32 indexB = joint->bodyB->joint->islandIndex;
+
+	b2Dump("  b2PrismaticJointDef jd;\n");
+	b2Dump("  jd.bodyA = sims[%d];\n", indexA);
+	b2Dump("  jd.bodyB = sims[%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 b2DrawPrismaticJoint( b2DebugDraw* draw, b2JointSim* base, b2Transform transformA, b2Transform transformB )
+{
+	B2_ASSERT( base->type == b2_prismaticJoint );
+
+	b2PrismaticJoint* joint = &base->prismaticJoint;
+
+	b2Vec2 pA = b2TransformPoint( transformA, base->localOriginAnchorA );
+	b2Vec2 pB = b2TransformPoint( transformB, base->localOriginAnchorB );
+
+	b2Vec2 axis = b2RotateVector( transformA.q, joint->localAxisA );
+
+	b2HexColor c1 = b2_colorGray;
+	b2HexColor c2 = b2_colorGreen;
+	b2HexColor c3 = b2_colorRed;
+	b2HexColor c4 = b2_colorBlue;
+	b2HexColor c5 = b2_colorDimGray;
+
+	draw->DrawSegmentFcn( 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->DrawSegmentFcn( lower, upper, c1, draw->context );
+		draw->DrawSegmentFcn( b2MulSub( lower, 0.1f, perp ), b2MulAdd( lower, 0.1f, perp ), c2, draw->context );
+		draw->DrawSegmentFcn( b2MulSub( upper, 0.1f, perp ), b2MulAdd( upper, 0.1f, perp ), c3, draw->context );
+	}
+	else
+	{
+		draw->DrawSegmentFcn( b2MulSub( pA, 1.0f, axis ), b2MulAdd( pA, 1.0f, axis ), c1, draw->context );
+	}
+
+	draw->DrawPointFcn( pA, 5.0f, c1, draw->context );
+	draw->DrawPointFcn( pB, 5.0f, c4, draw->context );
+}

src/vendor/box2d/revolute_joint.c

@@ -0,0 +1,530 @@
+// SPDX-FileCopyrightText: 2023 Erin Catto
+// SPDX-License-Identifier: MIT
+
+#if defined( _MSC_VER ) && !defined( _CRT_SECURE_NO_WARNINGS )
+#define _CRT_SECURE_NO_WARNINGS
+#endif
+
+#include "body.h"
+#include "core.h"
+#include "joint.h"
+#include "solver.h"
+#include "solver_set.h"
+#include "world.h"
+
+// needed for dll export
+#include "box2d/box2d.h"
+
+#include <stdio.h>
+
+void b2RevoluteJoint_EnableSpring( b2JointId jointId, bool enableSpring )
+{
+	b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_revoluteJoint );
+	if ( enableSpring != joint->revoluteJoint.enableSpring )
+	{
+		joint->revoluteJoint.enableSpring = enableSpring;
+		joint->revoluteJoint.springImpulse = 0.0f;
+	}
+}
+
+bool b2RevoluteJoint_IsSpringEnabled( b2JointId jointId )
+{
+	b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_revoluteJoint );
+	return joint->revoluteJoint.enableSpring;
+}
+
+void b2RevoluteJoint_SetSpringHertz( b2JointId jointId, float hertz )
+{
+	b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_revoluteJoint );
+	joint->revoluteJoint.hertz = hertz;
+}
+
+float b2RevoluteJoint_GetSpringHertz( b2JointId jointId )
+{
+	b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_revoluteJoint );
+	return joint->revoluteJoint.hertz;
+}
+
+void b2RevoluteJoint_SetSpringDampingRatio( b2JointId jointId, float dampingRatio )
+{
+	b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_revoluteJoint );
+	joint->revoluteJoint.dampingRatio = dampingRatio;
+}
+
+float b2RevoluteJoint_GetSpringDampingRatio( b2JointId jointId )
+{
+	b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_revoluteJoint );
+	return joint->revoluteJoint.dampingRatio;
+}
+
+float b2RevoluteJoint_GetAngle( b2JointId jointId )
+{
+	b2World* world = b2GetWorld( jointId.world0 );
+	b2JointSim* jointSim = b2GetJointSimCheckType( jointId, b2_revoluteJoint );
+	b2Transform transformA = b2GetBodyTransform( world, jointSim->bodyIdA );
+	b2Transform transformB = b2GetBodyTransform( world, jointSim->bodyIdB );
+
+	float angle = b2RelativeAngle( transformB.q, transformA.q ) - jointSim->revoluteJoint.referenceAngle;
+	angle = b2UnwindAngle( angle );
+	return angle;
+}
+
+void b2RevoluteJoint_EnableLimit( b2JointId jointId, bool enableLimit )
+{
+	b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_revoluteJoint );
+	if ( enableLimit != joint->revoluteJoint.enableLimit )
+	{
+		joint->revoluteJoint.enableLimit = enableLimit;
+		joint->revoluteJoint.lowerImpulse = 0.0f;
+		joint->revoluteJoint.upperImpulse = 0.0f;
+	}
+}
+
+bool b2RevoluteJoint_IsLimitEnabled( b2JointId jointId )
+{
+	b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_revoluteJoint );
+	return joint->revoluteJoint.enableLimit;
+}
+
+float b2RevoluteJoint_GetLowerLimit( b2JointId jointId )
+{
+	b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_revoluteJoint );
+	return joint->revoluteJoint.lowerAngle;
+}
+
+float b2RevoluteJoint_GetUpperLimit( b2JointId jointId )
+{
+	b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_revoluteJoint );
+	return joint->revoluteJoint.upperAngle;
+}
+
+void b2RevoluteJoint_SetLimits( b2JointId jointId, float lower, float upper )
+{
+	b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_revoluteJoint );
+	if ( lower != joint->revoluteJoint.lowerAngle || upper != joint->revoluteJoint.upperAngle )
+	{
+		joint->revoluteJoint.lowerAngle = b2MinFloat( lower, upper );
+		joint->revoluteJoint.upperAngle = b2MaxFloat( lower, upper );
+		joint->revoluteJoint.lowerImpulse = 0.0f;
+		joint->revoluteJoint.upperImpulse = 0.0f;
+	}
+}
+
+void b2RevoluteJoint_EnableMotor( b2JointId jointId, bool enableMotor )
+{
+	b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_revoluteJoint );
+	if ( enableMotor != joint->revoluteJoint.enableMotor )
+	{
+		joint->revoluteJoint.enableMotor = enableMotor;
+		joint->revoluteJoint.motorImpulse = 0.0f;
+	}
+}
+
+bool b2RevoluteJoint_IsMotorEnabled( b2JointId jointId )
+{
+	b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_revoluteJoint );
+	return joint->revoluteJoint.enableMotor;
+}
+
+void b2RevoluteJoint_SetMotorSpeed( b2JointId jointId, float motorSpeed )
+{
+	b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_revoluteJoint );
+	joint->revoluteJoint.motorSpeed = motorSpeed;
+}
+
+float b2RevoluteJoint_GetMotorSpeed( b2JointId jointId )
+{
+	b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_revoluteJoint );
+	return joint->revoluteJoint.motorSpeed;
+}
+
+float b2RevoluteJoint_GetMotorTorque( b2JointId jointId )
+{
+	b2World* world = b2GetWorld( jointId.world0 );
+	b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_revoluteJoint );
+	return world->inv_h * joint->revoluteJoint.motorImpulse;
+}
+
+void b2RevoluteJoint_SetMaxMotorTorque( b2JointId jointId, float torque )
+{
+	b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_revoluteJoint );
+	joint->revoluteJoint.maxMotorTorque = torque;
+}
+
+float b2RevoluteJoint_GetMaxMotorTorque( b2JointId jointId )
+{
+	b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_revoluteJoint );
+	return joint->revoluteJoint.maxMotorTorque;
+}
+
+b2Vec2 b2GetRevoluteJointForce( b2World* world, b2JointSim* base )
+{
+	b2Vec2 force = b2MulSV( world->inv_h, base->revoluteJoint.linearImpulse );
+	return force;
+}
+
+float b2GetRevoluteJointTorque( b2World* world, b2JointSim* base )
+{
+	const b2RevoluteJoint* revolute = &base->revoluteJoint;
+	float torque = world->inv_h * ( revolute->motorImpulse + revolute->lowerImpulse - revolute->upperImpulse );
+	return torque;
+}
+
+// Point-to-point constraint
+// C = p2 - p1
+// Cdot = v2 - v1
+//      = v2 + cross(w2, r2) - v1 - cross(w1, r1)
+// J = [-I -r1_skew I r2_skew ]
+// Identity used:
+// w k % (rx i + ry j) = w * (-ry i + rx j)
+
+// Motor constraint
+// Cdot = w2 - w1
+// J = [0 0 -1 0 0 1]
+// K = invI1 + invI2
+
+void b2PrepareRevoluteJoint( b2JointSim* base, b2StepContext* context )
+{
+	B2_ASSERT( base->type == b2_revoluteJoint );
+
+	// chase body id to the solver set where the body lives
+	int idA = base->bodyIdA;
+	int idB = base->bodyIdB;
+
+	b2World* world = context->world;
+
+	b2Body* bodyA = b2BodyArray_Get(&world->bodies, idA);
+	b2Body* bodyB = b2BodyArray_Get(&world->bodies, idB);
+
+	B2_ASSERT( bodyA->setIndex == b2_awakeSet || bodyB->setIndex == b2_awakeSet );
+	b2SolverSet* setA = b2SolverSetArray_Get( &world->solverSets, bodyA->setIndex );
+	b2SolverSet* setB = b2SolverSetArray_Get( &world->solverSets, bodyB->setIndex );
+
+	int localIndexA = bodyA->localIndex;
+	int localIndexB = bodyB->localIndex;
+
+	b2BodySim* bodySimA = b2BodySimArray_Get( &setA->bodySims, localIndexA );
+	b2BodySim* bodySimB = b2BodySimArray_Get( &setB->bodySims, localIndexB );
+
+	float mA = bodySimA->invMass;
+	float iA = bodySimA->invInertia;
+	float mB = bodySimB->invMass;
+	float iB = bodySimB->invInertia;
+
+	base->invMassA = mA;
+	base->invMassB = mB;
+	base->invIA = iA;
+	base->invIB = iB;
+
+	b2RevoluteJoint* joint = &base->revoluteJoint;
+
+	joint->indexA = bodyA->setIndex == b2_awakeSet ? localIndexA : B2_NULL_INDEX;
+	joint->indexB = bodyB->setIndex == b2_awakeSet ? localIndexB : B2_NULL_INDEX;
+
+	// initial anchors in world space
+	joint->anchorA = b2RotateVector( bodySimA->transform.q, b2Sub( base->localOriginAnchorA, bodySimA->localCenter ) );
+	joint->anchorB = b2RotateVector( bodySimB->transform.q, b2Sub( base->localOriginAnchorB, bodySimB->localCenter ) );
+	joint->deltaCenter = b2Sub( bodySimB->center, bodySimA->center );
+	joint->deltaAngle = b2RelativeAngle( bodySimB->transform.q, bodySimA->transform.q ) - joint->referenceAngle;
+	joint->deltaAngle = b2UnwindAngle( joint->deltaAngle );
+
+	float k = iA + iB;
+	joint->axialMass = k > 0.0f ? 1.0f / k : 0.0f;
+
+	joint->springSoftness = b2MakeSoft( joint->hertz, joint->dampingRatio, context->h );
+
+	if ( context->enableWarmStarting == false )
+	{
+		joint->linearImpulse = b2Vec2_zero;
+		joint->springImpulse = 0.0f;
+		joint->motorImpulse = 0.0f;
+		joint->lowerImpulse = 0.0f;
+		joint->upperImpulse = 0.0f;
+	}
+}
+
+void b2WarmStartRevoluteJoint( b2JointSim* base, b2StepContext* context )
+{
+	B2_ASSERT( base->type == b2_revoluteJoint );
+
+	float mA = base->invMassA;
+	float mB = base->invMassB;
+	float iA = base->invIA;
+	float iB = base->invIB;
+
+	// dummy state for static bodies
+	b2BodyState dummyState = b2_identityBodyState;
+
+	b2RevoluteJoint* joint = &base->revoluteJoint;
+	b2BodyState* stateA = joint->indexA == B2_NULL_INDEX ? &dummyState : context->states + joint->indexA;
+	b2BodyState* stateB = joint->indexB == B2_NULL_INDEX ? &dummyState : context->states + joint->indexB;
+
+	b2Vec2 rA = b2RotateVector( stateA->deltaRotation, joint->anchorA );
+	b2Vec2 rB = b2RotateVector( stateB->deltaRotation, joint->anchorB );
+
+	float axialImpulse = joint->springImpulse + joint->motorImpulse + joint->lowerImpulse - joint->upperImpulse;
+
+	stateA->linearVelocity = b2MulSub( stateA->linearVelocity, mA, joint->linearImpulse );
+	stateA->angularVelocity -= iA * ( b2Cross( rA, joint->linearImpulse ) + axialImpulse );
+
+	stateB->linearVelocity = b2MulAdd( stateB->linearVelocity, mB, joint->linearImpulse );
+	stateB->angularVelocity += iB * ( b2Cross( rB, joint->linearImpulse ) + axialImpulse );
+}
+
+void b2SolveRevoluteJoint( b2JointSim* base, b2StepContext* context, bool useBias )
+{
+	B2_ASSERT( base->type == b2_revoluteJoint );
+
+	float mA = base->invMassA;
+	float mB = base->invMassB;
+	float iA = base->invIA;
+	float iB = base->invIB;
+
+	// dummy state for static bodies
+	b2BodyState dummyState = b2_identityBodyState;
+
+	b2RevoluteJoint* joint = &base->revoluteJoint;
+
+	b2BodyState* stateA = joint->indexA == B2_NULL_INDEX ? &dummyState : context->states + joint->indexA;
+	b2BodyState* stateB = joint->indexB == B2_NULL_INDEX ? &dummyState : context->states + joint->indexB;
+
+	b2Vec2 vA = stateA->linearVelocity;
+	float wA = stateA->angularVelocity;
+	b2Vec2 vB = stateB->linearVelocity;
+	float wB = stateB->angularVelocity;
+
+	bool fixedRotation = ( iA + iB == 0.0f );
+	// const float maxBias = context->maxBiasVelocity;
+
+	// Solve spring.
+	if ( joint->enableSpring && fixedRotation == false )
+	{
+		float C = b2RelativeAngle( stateB->deltaRotation, stateA->deltaRotation ) + joint->deltaAngle;
+		float bias = joint->springSoftness.biasRate * C;
+		float massScale = joint->springSoftness.massScale;
+		float impulseScale = joint->springSoftness.impulseScale;
+
+		float Cdot = wB - wA;
+		float impulse = -massScale * joint->axialMass * ( Cdot + bias ) - impulseScale * joint->springImpulse;
+		joint->springImpulse += impulse;
+
+		wA -= iA * impulse;
+		wB += iB * impulse;
+	}
+
+	// Solve motor constraint.
+	if ( joint->enableMotor && fixedRotation == false )
+	{
+		float Cdot = wB - wA - joint->motorSpeed;
+		float impulse = -joint->axialMass * Cdot;
+		float oldImpulse = joint->motorImpulse;
+		float maxImpulse = context->h * joint->maxMotorTorque;
+		joint->motorImpulse = b2ClampFloat( joint->motorImpulse + impulse, -maxImpulse, maxImpulse );
+		impulse = joint->motorImpulse - oldImpulse;
+
+		wA -= iA * impulse;
+		wB += iB * impulse;
+	}
+
+	if ( joint->enableLimit && fixedRotation == false )
+	{
+		float jointAngle = b2RelativeAngle( stateB->deltaRotation, stateA->deltaRotation ) + joint->deltaAngle;
+		jointAngle = b2UnwindAngle( jointAngle );
+
+		// Lower limit
+		{
+			float C = jointAngle - joint->lowerAngle;
+			float bias = 0.0f;
+			float massScale = 1.0f;
+			float impulseScale = 0.0f;
+			if ( C > 0.0f )
+			{
+				// speculation
+				bias = C * context->inv_h;
+			}
+			else if ( useBias )
+			{
+				bias = context->jointSoftness.biasRate * C;
+				massScale = context->jointSoftness.massScale;
+				impulseScale = context->jointSoftness.impulseScale;
+			}
+
+			float Cdot = wB - wA;
+			float oldImpulse = joint->lowerImpulse;
+			float impulse = -massScale * joint->axialMass * ( Cdot + bias ) - impulseScale * oldImpulse;
+			joint->lowerImpulse = b2MaxFloat( oldImpulse + impulse, 0.0f );
+			impulse = joint->lowerImpulse - oldImpulse;
+
+			wA -= iA * impulse;
+			wB += iB * impulse;
+		}
+
+		// 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.
+		{
+			float C = joint->upperAngle - jointAngle;
+			float bias = 0.0f;
+			float massScale = 1.0f;
+			float impulseScale = 0.0f;
+			if ( C > 0.0f )
+			{
+				// speculation
+				bias = C * context->inv_h;
+			}
+			else if ( useBias )
+			{
+				bias = context->jointSoftness.biasRate * C;
+				massScale = context->jointSoftness.massScale;
+				impulseScale = context->jointSoftness.impulseScale;
+			}
+
+			// sign flipped on Cdot
+			float Cdot = wA - wB;
+			float oldImpulse = joint->upperImpulse;
+			float impulse = -massScale * joint->axialMass * ( Cdot + bias ) - impulseScale * oldImpulse;
+			joint->upperImpulse = b2MaxFloat( oldImpulse + impulse, 0.0f );
+			impulse = joint->upperImpulse - oldImpulse;
+
+			// sign flipped on applied impulse
+			wA += iA * impulse;
+			wB -= iB * impulse;
+		}
+	}
+
+	// Solve point-to-point constraint
+	{
+		// J = [-I -r1_skew I r2_skew]
+		// r_skew = [-ry; rx]
+		// K = [ mA+r1y^2*iA+mB+r2y^2*iB,  -r1y*iA*r1x-r2y*iB*r2x]
+		//     [  -r1y*iA*r1x-r2y*iB*r2x, mA+r1x^2*iA+mB+r2x^2*iB]
+
+		// current anchors
+		b2Vec2 rA = b2RotateVector( stateA->deltaRotation, joint->anchorA );
+		b2Vec2 rB = b2RotateVector( stateB->deltaRotation, joint->anchorB );
+
+		b2Vec2 Cdot = b2Sub( b2Add( vB, b2CrossSV( wB, rB ) ), b2Add( vA, b2CrossSV( wA, rA ) ) );
+
+		b2Vec2 bias = b2Vec2_zero;
+		float massScale = 1.0f;
+		float impulseScale = 0.0f;
+		if ( useBias )
+		{
+			b2Vec2 dcA = stateA->deltaPosition;
+			b2Vec2 dcB = stateB->deltaPosition;
+
+			b2Vec2 separation = b2Add( b2Add( b2Sub( dcB, dcA ), b2Sub( rB, rA ) ), joint->deltaCenter );
+			bias = b2MulSV( context->jointSoftness.biasRate, separation );
+			massScale = context->jointSoftness.massScale;
+			impulseScale = context->jointSoftness.impulseScale;
+		}
+
+		b2Mat22 K;
+		K.cx.x = mA + mB + rA.y * rA.y * iA + rB.y * rB.y * iB;
+		K.cy.x = -rA.y * rA.x * iA - rB.y * rB.x * iB;
+		K.cx.y = K.cy.x;
+		K.cy.y = mA + mB + rA.x * rA.x * iA + rB.x * rB.x * iB;
+		b2Vec2 b = b2Solve22( K, b2Add( Cdot, bias ) );
+
+		b2Vec2 impulse;
+		impulse.x = -massScale * b.x - impulseScale * joint->linearImpulse.x;
+		impulse.y = -massScale * b.y - impulseScale * joint->linearImpulse.y;
+		joint->linearImpulse.x += impulse.x;
+		joint->linearImpulse.y += impulse.y;
+
+		vA = b2MulSub( vA, mA, impulse );
+		wA -= iA * b2Cross( rA, impulse );
+		vB = b2MulAdd( vB, mB, impulse );
+		wB += iB * b2Cross( rB, impulse );
+	}
+
+	stateA->linearVelocity = vA;
+	stateA->angularVelocity = wA;
+	stateB->linearVelocity = vB;
+	stateB->angularVelocity = wB;
+}
+
+#if 0
+void b2RevoluteJoint::Dump()
+{
+	int32 indexA = joint->bodyA->joint->islandIndex;
+	int32 indexB = joint->bodyB->joint->islandIndex;
+
+	b2Dump("  b2RevoluteJointDef 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 b2DrawRevoluteJoint( b2DebugDraw* draw, b2JointSim* base, b2Transform transformA, b2Transform transformB, float drawSize )
+{
+	B2_ASSERT( base->type == b2_revoluteJoint );
+
+	b2RevoluteJoint* joint = &base->revoluteJoint;
+
+	b2Vec2 pA = b2TransformPoint( transformA, base->localOriginAnchorA );
+	b2Vec2 pB = b2TransformPoint( transformB, base->localOriginAnchorB );
+
+	b2HexColor c1 = b2_colorGray;
+	b2HexColor c2 = b2_colorGreen;
+	b2HexColor c3 = b2_colorRed;
+
+	const float L = drawSize;
+	// draw->drawPoint(pA, 3.0f, b2_colorGray40, draw->context);
+	// draw->drawPoint(pB, 3.0f, b2_colorLightBlue, draw->context);
+	draw->DrawCircleFcn( pB, L, c1, draw->context );
+
+	float angle = b2RelativeAngle( transformB.q, transformA.q );
+
+	b2Rot rot = b2MakeRot( angle );
+	b2Vec2 r = { L * rot.c, L * rot.s };
+	b2Vec2 pC = b2Add( pB, r );
+	draw->DrawSegmentFcn( pB, pC, c1, draw->context );
+
+	if ( draw->drawJointExtras )
+	{
+		float jointAngle = b2UnwindAngle( angle - joint->referenceAngle );
+		char buffer[32];
+		snprintf( buffer, 32, " %.1f deg", 180.0f * jointAngle / B2_PI );
+		draw->DrawStringFcn( pC, buffer, b2_colorWhite, draw->context );
+	}
+
+	float lowerAngle = joint->lowerAngle + joint->referenceAngle;
+	float upperAngle = joint->upperAngle + joint->referenceAngle;
+
+	if ( joint->enableLimit )
+	{
+		b2Rot rotLo = b2MakeRot( lowerAngle );
+		b2Vec2 rlo = { L * rotLo.c, L * rotLo.s };
+
+		b2Rot rotHi = b2MakeRot( upperAngle );
+		b2Vec2 rhi = { L * rotHi.c, L * rotHi.s };
+
+		draw->DrawSegmentFcn( pB, b2Add( pB, rlo ), c2, draw->context );
+		draw->DrawSegmentFcn( pB, b2Add( pB, rhi ), c3, draw->context );
+
+		b2Rot rotRef = b2MakeRot( joint->referenceAngle );
+		b2Vec2 ref = ( b2Vec2 ){ L * rotRef.c, L * rotRef.s };
+		draw->DrawSegmentFcn( pB, b2Add( pB, ref ), b2_colorBlue, draw->context );
+	}
+
+	b2HexColor color = b2_colorGold;
+	draw->DrawSegmentFcn( transformA.p, pA, color, draw->context );
+	draw->DrawSegmentFcn( pA, pB, color, draw->context );
+	draw->DrawSegmentFcn( transformB.p, pB, color, draw->context );
+
+	// char buffer[32];
+	// sprintf(buffer, "%.1f", b2Length(joint->impulse));
+	// draw->DrawString(pA, buffer, draw->context);
+}

src/vendor/box2d/sensor.c

@@ -0,0 +1,389 @@
+// SPDX-FileCopyrightText: 2023 Erin Catto
+// SPDX-License-Identifier: MIT
+
+#include "sensor.h"
+
+#include "array.h"
+#include "body.h"
+#include "contact.h"
+#include "ctz.h"
+#include "shape.h"
+#include "world.h"
+
+#include "box2d/collision.h"
+
+#include <stddef.h>
+#include <stdlib.h>
+
+B2_ARRAY_SOURCE( b2ShapeRef, b2ShapeRef )
+B2_ARRAY_SOURCE( b2Sensor, b2Sensor )
+B2_ARRAY_SOURCE( b2SensorTaskContext, b2SensorTaskContext )
+
+struct b2SensorQueryContext
+{
+	b2World* world;
+	b2SensorTaskContext* taskContext;
+	b2Sensor* sensor;
+	b2Shape* sensorShape;
+	b2Transform transform;
+};
+
+// Sensor shapes need to
+// - detect begin and end overlap events
+// - events must be reported in deterministic order
+// - maintain an active list of overlaps for query
+
+// Assumption
+// - sensors don't detect shapes on the same body
+
+// Algorithm
+// Query all sensors for overlaps
+// Check against previous overlaps
+
+// Data structures
+// Each sensor has an double buffered array of overlaps
+// These overlaps use a shape reference with index and generation
+
+static bool b2SensorQueryCallback( int proxyId, uint64_t userData, void* context )
+{
+	B2_UNUSED( proxyId );
+
+	int shapeId = (int)userData;
+
+	struct b2SensorQueryContext* queryContext = context;
+	b2Shape* sensorShape = queryContext->sensorShape;
+	int sensorShapeId = sensorShape->id;
+
+	if ( shapeId == sensorShapeId )
+	{
+		return true;
+	}
+
+	b2World* world = queryContext->world;
+	b2Shape* otherShape = b2ShapeArray_Get( &world->shapes, shapeId );
+
+	// Are sensor events enabled on the other shape?
+	if ( otherShape->enableSensorEvents == false )
+	{
+		return true;
+	}
+
+	// Skip shapes on the same body
+	if ( otherShape->bodyId == sensorShape->bodyId )
+	{
+		return true;
+	}
+
+	// Check filter
+	if ( b2ShouldShapesCollide( sensorShape->filter, otherShape->filter ) == false )
+	{
+		return true;
+	}
+
+	b2Transform otherTransform = b2GetBodyTransform( world, otherShape->bodyId );
+
+	b2DistanceInput input;
+	input.proxyA = b2MakeShapeDistanceProxy( sensorShape );
+	input.proxyB = b2MakeShapeDistanceProxy( otherShape );
+	input.transformA = queryContext->transform;
+	input.transformB = otherTransform;
+	input.useRadii = true;
+	b2SimplexCache cache = { 0 };
+	b2DistanceOutput output = b2ShapeDistance(&input, &cache, NULL, 0 );
+
+	bool overlaps = output.distance < 10.0f * FLT_EPSILON;
+	if ( overlaps == false )
+	{
+		return true;
+	}
+
+	// Record the overlap
+	b2Sensor* sensor = queryContext->sensor;
+	b2ShapeRef* shapeRef = b2ShapeRefArray_Add( &sensor->overlaps2 );
+	shapeRef->shapeId = shapeId;
+	shapeRef->generation = otherShape->generation;
+
+	return true;
+}
+
+static int b2CompareShapeRefs( const void* a, const void* b )
+{
+	const b2ShapeRef* sa = a;
+	const b2ShapeRef* sb = b;
+
+	if ( sa->shapeId < sb->shapeId )
+	{
+		return -1;
+	}
+
+	if ( sa->shapeId == sb->shapeId )
+	{
+		if ( sa->generation < sb->generation )
+		{
+			return -1;
+		}
+
+		if ( sa->generation == sb->generation )
+		{
+			return 0;
+		}
+	}
+
+	return 1;
+}
+
+static void b2SensorTask( int startIndex, int endIndex, uint32_t threadIndex, void* context )
+{
+	b2TracyCZoneNC( sensor_task, "Overlap", b2_colorBrown, true );
+
+	b2World* world = context;
+	B2_ASSERT( (int)threadIndex < world->workerCount );
+	b2SensorTaskContext* taskContext = world->sensorTaskContexts.data + threadIndex;
+
+	B2_ASSERT( startIndex < endIndex );
+
+	b2DynamicTree* trees = world->broadPhase.trees;
+	for ( int sensorIndex = startIndex; sensorIndex < endIndex; ++sensorIndex )
+	{
+		b2Sensor* sensor = b2SensorArray_Get( &world->sensors, sensorIndex );
+		b2Shape* sensorShape = b2ShapeArray_Get( &world->shapes, sensor->shapeId );
+
+		// swap overlap arrays
+		b2ShapeRefArray temp = sensor->overlaps1;
+		sensor->overlaps1 = sensor->overlaps2;
+		sensor->overlaps2 = temp;
+		b2ShapeRefArray_Clear( &sensor->overlaps2 );
+
+		b2Body* body = b2BodyArray_Get( &world->bodies, sensorShape->bodyId );
+		if ( body->setIndex == b2_disabledSet || sensorShape->enableSensorEvents == false )
+		{
+			if ( sensor->overlaps1.count != 0 )
+			{
+				b2SetBit( &taskContext->eventBits, sensorIndex );
+			}
+			continue;
+		}
+
+		b2Transform transform = b2GetBodyTransformQuick( world, body );
+
+		struct b2SensorQueryContext queryContext = {
+			.world = world,
+			.taskContext = taskContext,
+			.sensorShape = sensorShape,
+			.sensor = sensor,
+			.transform = transform,
+		};
+
+		B2_ASSERT( sensorShape->sensorIndex == sensorIndex );
+		b2AABB queryBounds = sensorShape->aabb;
+
+		// Query all trees
+		b2DynamicTree_Query( trees + 0, queryBounds, sensorShape->filter.maskBits, b2SensorQueryCallback, &queryContext );
+		b2DynamicTree_Query( trees + 1, queryBounds, sensorShape->filter.maskBits, b2SensorQueryCallback, &queryContext );
+		b2DynamicTree_Query( trees + 2, queryBounds, sensorShape->filter.maskBits, b2SensorQueryCallback, &queryContext );
+
+		// Sort the overlaps to enable finding begin and end events.
+		qsort( sensor->overlaps2.data, sensor->overlaps2.count, sizeof( b2ShapeRef ), b2CompareShapeRefs );
+
+		int count1 = sensor->overlaps1.count;
+		int count2 = sensor->overlaps2.count;
+		if ( count1 != count2 )
+		{
+			// something changed
+			b2SetBit( &taskContext->eventBits, sensorIndex );
+		}
+		else
+		{
+			for ( int i = 0; i < count1; ++i )
+			{
+				b2ShapeRef* s1 = sensor->overlaps1.data + i;
+				b2ShapeRef* s2 = sensor->overlaps2.data + i;
+
+				if ( s1->shapeId != s2->shapeId || s1->generation != s2->generation )
+				{
+					// something changed
+					b2SetBit( &taskContext->eventBits, sensorIndex );
+					break;
+				}
+			}
+		}
+	}
+
+	b2TracyCZoneEnd( sensor_task );
+}
+
+void b2OverlapSensors( b2World* world )
+{
+	int sensorCount = world->sensors.count;
+	if ( sensorCount == 0 )
+	{
+		return;
+	}
+
+	B2_ASSERT( world->workerCount > 0 );
+
+	b2TracyCZoneNC( overlap_sensors, "Sensors", b2_colorMediumPurple, true );
+
+	for ( int i = 0; i < world->workerCount; ++i )
+	{
+		b2SetBitCountAndClear( &world->sensorTaskContexts.data[i].eventBits, sensorCount );
+	}
+
+	// Parallel-for sensors overlaps
+	int minRange = 16;
+	void* userSensorTask = world->enqueueTaskFcn( &b2SensorTask, sensorCount, minRange, world, world->userTaskContext );
+	world->taskCount += 1;
+	if ( userSensorTask != NULL )
+	{
+		world->finishTaskFcn( userSensorTask, world->userTaskContext );
+	}
+
+	b2TracyCZoneNC( sensor_state, "Events", b2_colorLightSlateGray, true );
+
+	b2BitSet* bitSet = &world->sensorTaskContexts.data[0].eventBits;
+	for ( int i = 1; i < world->workerCount; ++i )
+	{
+		b2InPlaceUnion( bitSet, &world->sensorTaskContexts.data[i].eventBits );
+	}
+
+	// Iterate sensors bits and publish events
+	// Process contact state changes. Iterate over set bits
+	uint64_t* bits = bitSet->bits;
+	uint32_t blockCount = bitSet->blockCount;
+
+	for ( uint32_t k = 0; k < blockCount; ++k )
+	{
+		uint64_t word = bits[k];
+		while ( word != 0 )
+		{
+			uint32_t ctz = b2CTZ64( word );
+			int sensorIndex = (int)( 64 * k + ctz );
+
+			b2Sensor* sensor = b2SensorArray_Get( &world->sensors, sensorIndex );
+			b2Shape* sensorShape = b2ShapeArray_Get( &world->shapes, sensor->shapeId );
+			b2ShapeId sensorId = { sensor->shapeId + 1, world->worldId, sensorShape->generation };
+
+			int count1 = sensor->overlaps1.count;
+			int count2 = sensor->overlaps2.count;
+			const b2ShapeRef* refs1 = sensor->overlaps1.data;
+			const b2ShapeRef* refs2 = sensor->overlaps2.data;
+
+			// overlaps1 can have overlaps that end
+			// overlaps2 can have overlaps that begin
+			int index1 = 0, index2 = 0;
+			while ( index1 < count1 && index2 < count2 )
+			{
+				const b2ShapeRef* r1 = refs1 + index1;
+				const b2ShapeRef* r2 = refs2 + index2;
+				if ( r1->shapeId == r2->shapeId )
+				{
+					if ( r1->generation < r2->generation )
+					{
+						// end
+						b2ShapeId visitorId = { r1->shapeId + 1, world->worldId, r1->generation };
+						b2SensorEndTouchEvent event = {
+							.sensorShapeId = sensorId,
+							.visitorShapeId = visitorId,
+						};
+						b2SensorEndTouchEventArray_Push( &world->sensorEndEvents[world->endEventArrayIndex], event );
+						index1 += 1;
+					}
+					else if ( r1->generation > r2->generation )
+					{
+						// begin
+						b2ShapeId visitorId = { r2->shapeId + 1, world->worldId, r2->generation };
+						b2SensorBeginTouchEvent event = { sensorId, visitorId };
+						b2SensorBeginTouchEventArray_Push( &world->sensorBeginEvents, event );
+						index2 += 1;
+					}
+					else
+					{
+						// persisted
+						index1 += 1;
+						index2 += 1;
+					}
+				}
+				else if ( r1->shapeId < r2->shapeId )
+				{
+					// end
+					b2ShapeId visitorId = { r1->shapeId + 1, world->worldId, r1->generation };
+					b2SensorEndTouchEvent event = { sensorId, visitorId };
+					b2SensorEndTouchEventArray_Push( &world->sensorEndEvents[world->endEventArrayIndex], event );
+					index1 += 1;
+				}
+				else
+				{
+					// begin
+					b2ShapeId visitorId = { r2->shapeId + 1, world->worldId, r2->generation };
+					b2SensorBeginTouchEvent event = { sensorId, visitorId };
+					b2SensorBeginTouchEventArray_Push( &world->sensorBeginEvents, event );
+					index2 += 1;
+				}
+			}
+
+			while ( index1 < count1 )
+			{
+				// end
+				const b2ShapeRef* r1 = refs1 + index1;
+				b2ShapeId visitorId = { r1->shapeId + 1, world->worldId, r1->generation };
+				b2SensorEndTouchEvent event = { sensorId, visitorId };
+				b2SensorEndTouchEventArray_Push( &world->sensorEndEvents[world->endEventArrayIndex], event );
+				index1 += 1;
+			}
+
+			while ( index2 < count2 )
+			{
+				// begin
+				const b2ShapeRef* r2 = refs2 + index2;
+				b2ShapeId visitorId = { r2->shapeId + 1, world->worldId, r2->generation };
+				b2SensorBeginTouchEvent event = { sensorId, visitorId };
+				b2SensorBeginTouchEventArray_Push( &world->sensorBeginEvents, event );
+				index2 += 1;
+			}
+
+			// Clear the smallest set bit
+			word = word & ( word - 1 );
+		}
+	}
+
+	b2TracyCZoneEnd( sensor_state );
+	b2TracyCZoneEnd( overlap_sensors );
+}
+
+void b2DestroySensor( b2World* world, b2Shape* sensorShape )
+{
+	b2Sensor* sensor = b2SensorArray_Get( &world->sensors, sensorShape->sensorIndex );
+	for ( int i = 0; i < sensor->overlaps2.count; ++i )
+	{
+		b2ShapeRef* ref = sensor->overlaps2.data + i;
+		b2SensorEndTouchEvent event = {
+			.sensorShapeId =
+				{
+					.index1 = sensorShape->id + 1,
+					.generation = sensorShape->generation,
+					.world0 = world->worldId,
+				},
+			.visitorShapeId =
+				{
+					.index1 = ref->shapeId + 1,
+					.generation = ref->generation,
+					.world0 = world->worldId,
+				},
+		};
+
+		b2SensorEndTouchEventArray_Push( world->sensorEndEvents + world->endEventArrayIndex, event );
+	}
+
+	// Destroy sensor
+	b2ShapeRefArray_Destroy( &sensor->overlaps1 );
+	b2ShapeRefArray_Destroy( &sensor->overlaps2 );
+
+	int movedIndex = b2SensorArray_RemoveSwap( &world->sensors, sensorShape->sensorIndex );
+	if ( movedIndex != B2_NULL_INDEX )
+	{
+		// Fixup moved sensor
+		b2Sensor* movedSensor = b2SensorArray_Get( &world->sensors, sensorShape->sensorIndex );
+		b2Shape* otherSensorShape = b2ShapeArray_Get( &world->shapes, movedSensor->shapeId );
+		otherSensorShape->sensorIndex = sensorShape->sensorIndex;
+	}
+}

src/vendor/box2d/sensor.h

@@ -0,0 +1,36 @@
+// SPDX-FileCopyrightText: 2023 Erin Catto
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include "array.h"
+#include "bitset.h"
+
+typedef struct b2Shape b2Shape;
+typedef struct b2World b2World;
+
+typedef struct b2ShapeRef
+{
+	int shapeId;
+	uint16_t generation;
+} b2ShapeRef;
+
+typedef struct b2Sensor
+{
+	b2ShapeRefArray overlaps1;
+	b2ShapeRefArray overlaps2;
+	int shapeId;
+} b2Sensor;
+
+typedef struct b2SensorTaskContext
+{
+	b2BitSet eventBits;
+} b2SensorTaskContext;
+
+void b2OverlapSensors( b2World* world );
+
+void b2DestroySensor( b2World* world, b2Shape* sensorShape );
+
+B2_ARRAY_INLINE( b2ShapeRef, b2ShapeRef )
+B2_ARRAY_INLINE( b2Sensor, b2Sensor )
+B2_ARRAY_INLINE( b2SensorTaskContext, b2SensorTaskContext )

src/vendor/box2d/shape.h

@@ -0,0 +1,123 @@
+// SPDX-FileCopyrightText: 2023 Erin Catto
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include "array.h"
+
+#include "box2d/types.h"
+
+typedef struct b2BroadPhase b2BroadPhase;
+typedef struct b2World b2World;
+
+typedef struct b2Shape
+{
+	int id;
+	int bodyId;
+	int prevShapeId;
+	int nextShapeId;
+	int sensorIndex;
+	b2ShapeType type;
+	float density;
+	float friction;
+	float restitution;
+	float rollingResistance;
+	float tangentSpeed;
+	int userMaterialId;
+
+	b2AABB aabb;
+	b2AABB fatAABB;
+	b2Vec2 localCentroid;
+	int proxyKey;
+
+	b2Filter filter;
+	void* userData;
+	uint32_t customColor;
+
+	union
+	{
+		b2Capsule capsule;
+		b2Circle circle;
+		b2Polygon polygon;
+		b2Segment segment;
+		b2ChainSegment chainSegment;
+	};
+
+	uint16_t generation;
+	bool enableSensorEvents;
+	bool enableContactEvents;
+	bool enableHitEvents;
+	bool enablePreSolveEvents;
+	bool enlargedAABB;
+} b2Shape;
+
+typedef struct b2ChainShape
+{
+	int id;
+	int bodyId;
+	int nextChainId;
+	int count;
+	int materialCount;
+	int* shapeIndices;
+	b2SurfaceMaterial* materials;
+	uint16_t generation;
+} b2ChainShape;
+
+typedef struct b2ShapeExtent
+{
+	float minExtent;
+	float maxExtent;
+} b2ShapeExtent;
+
+// Sensors are shapes that live in the broad-phase but never have contacts.
+// At the end of the time step all sensors are queried for overlap with any other shapes.
+// Sensors ignore body type and sleeping.
+// Sensors generate events when there is a new overlap or and overlap disappears.
+// The sensor overlaps don't get cleared until the next time step regardless of the overlapped
+// shapes being destroyed.
+// When a sensor is destroyed.
+typedef struct
+{
+	b2IntArray overlaps;
+} b2SensorOverlaps;
+
+void b2CreateShapeProxy( b2Shape* shape, b2BroadPhase* bp, b2BodyType type, b2Transform transform, bool forcePairCreation );
+void b2DestroyShapeProxy( b2Shape* shape, b2BroadPhase* bp );
+
+void b2FreeChainData( b2ChainShape* chain );
+
+b2MassData b2ComputeShapeMass( const b2Shape* shape );
+b2ShapeExtent b2ComputeShapeExtent( const b2Shape* shape, b2Vec2 localCenter );
+b2AABB b2ComputeShapeAABB( const b2Shape* shape, b2Transform transform );
+b2Vec2 b2GetShapeCentroid( const b2Shape* shape );
+float b2GetShapePerimeter( const b2Shape* shape );
+float b2GetShapeProjectedPerimeter( const b2Shape* shape, b2Vec2 line );
+
+b2ShapeProxy b2MakeShapeDistanceProxy( const b2Shape* shape );
+
+b2CastOutput b2RayCastShape( const b2RayCastInput* input, const b2Shape* shape, b2Transform transform );
+b2CastOutput b2ShapeCastShape( const b2ShapeCastInput* input, const b2Shape* shape, b2Transform transform );
+
+b2PlaneResult b2CollideMoverAndCircle( const b2Circle* shape, const b2Capsule* mover );
+b2PlaneResult b2CollideMoverAndCapsule( const b2Capsule* shape, const b2Capsule* mover );
+b2PlaneResult b2CollideMoverAndPolygon( const b2Polygon* shape, const b2Capsule* mover );
+b2PlaneResult b2CollideMoverAndSegment( const b2Segment* shape, const b2Capsule* mover );
+b2PlaneResult b2CollideMover( const b2Shape* shape, b2Transform transform, const b2Capsule* mover );
+
+static inline float b2GetShapeRadius( const b2Shape* shape )
+{
+	switch ( shape->type )
+	{
+		case b2_capsuleShape:
+			return shape->capsule.radius;
+		case b2_circleShape:
+			return shape->circle.radius;
+		case b2_polygonShape:
+			return shape->polygon.radius;
+		default:
+			return 0.0f;
+	}
+}
+
+B2_ARRAY_INLINE( b2ChainShape, b2ChainShape )
+B2_ARRAY_INLINE( b2Shape, b2Shape )

src/vendor/box2d/solver.h

@@ -0,0 +1,155 @@
+// SPDX-FileCopyrightText: 2023 Erin Catto
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include "box2d/math_functions.h"
+
+#include "core.h"
+
+#include <stdbool.h>
+#include <stdint.h>
+
+typedef struct b2BodySim b2BodySim;
+typedef struct b2BodyState b2BodyState;
+typedef struct b2ContactSim b2ContactSim;
+typedef struct b2JointSim b2JointSim;
+typedef struct b2World b2World;
+
+typedef struct b2Softness
+{
+	float biasRate;
+	float massScale;
+	float impulseScale;
+} b2Softness;
+
+typedef enum b2SolverStageType
+{
+	b2_stagePrepareJoints,
+	b2_stagePrepareContacts,
+	b2_stageIntegrateVelocities,
+	b2_stageWarmStart,
+	b2_stageSolve,
+	b2_stageIntegratePositions,
+	b2_stageRelax,
+	b2_stageRestitution,
+	b2_stageStoreImpulses
+} b2SolverStageType;
+
+typedef enum b2SolverBlockType
+{
+	b2_bodyBlock,
+	b2_jointBlock,
+	b2_contactBlock,
+	b2_graphJointBlock,
+	b2_graphContactBlock
+} b2SolverBlockType;
+
+// Each block of work has a sync index that gets incremented when a worker claims the block. This ensures only a single worker
+// claims a block, yet lets work be distributed dynamically across multiple workers (work stealing). This also reduces contention
+// on a single block index atomic. For non-iterative stages the sync index is simply set to one. For iterative stages (solver
+// iteration) the same block of work is executed once per iteration and the atomic sync index is shared across iterations, so it
+// increases monotonically.
+typedef struct b2SolverBlock
+{
+	int startIndex;
+	int16_t count;
+	int16_t blockType; // b2SolverBlockType
+	// todo consider false sharing of this atomic
+	b2AtomicInt syncIndex;
+} b2SolverBlock;
+
+// Each stage must be completed before going to the next stage.
+// Non-iterative stages use a stage instance once while iterative stages re-use the same instance each iteration.
+typedef struct b2SolverStage
+{
+	b2SolverStageType type;
+	b2SolverBlock* blocks;
+	int blockCount;
+	int colorIndex;
+	// todo consider false sharing of this atomic
+	b2AtomicInt completionCount;
+} b2SolverStage;
+
+// Context for a time step. Recreated each time step.
+typedef struct b2StepContext
+{
+	// time step
+	float dt;
+
+	// inverse time step (0 if dt == 0).
+	float inv_dt;
+
+	// sub-step
+	float h;
+	float inv_h;
+
+	int subStepCount;
+
+	b2Softness jointSoftness;
+	b2Softness contactSoftness;
+	b2Softness staticSoftness;
+
+	float restitutionThreshold;
+	float maxLinearVelocity;
+
+	struct b2World* world;
+	struct b2ConstraintGraph* graph;
+
+	// shortcut to body states from awake set
+	b2BodyState* states;
+
+	// shortcut to body sims from awake set
+	b2BodySim* sims;
+
+	// array of all shape ids for shapes that have enlarged AABBs
+	int* enlargedShapes;
+	int enlargedShapeCount;
+
+	// Array of bullet bodies that need continuous collision handling
+	int* bulletBodies;
+	b2AtomicInt bulletBodyCount;
+
+	// joint pointers for simplified parallel-for access.
+	b2JointSim** joints;
+
+	// contact pointers for simplified parallel-for access.
+	// - parallel-for collide with no gaps
+	// - parallel-for prepare and store contacts with NULL gaps for SIMD remainders
+	// despite being an array of pointers, these are contiguous sub-arrays corresponding
+	// to constraint graph colors
+	b2ContactSim** contacts;
+
+	struct b2ContactConstraintSIMD* simdContactConstraints;
+	int activeColorCount;
+	int workerCount;
+
+	b2SolverStage* stages;
+	int stageCount;
+	bool enableWarmStarting;
+
+	// todo padding to prevent false sharing
+	char dummy1[64];
+
+	// sync index (16-bits) | stage type (16-bits)
+	b2AtomicU32 atomicSyncBits;
+
+	char dummy2[64];
+
+} b2StepContext;
+
+static inline b2Softness b2MakeSoft( float hertz, float zeta, float h )
+{
+	if ( hertz == 0.0f )
+	{
+		return ( b2Softness ){ 0.0f, 1.0f, 0.0f };
+	}
+
+	float omega = 2.0f * B2_PI * hertz;
+	float a1 = 2.0f * zeta + h * omega;
+	float a2 = h * omega * a1;
+	float a3 = 1.0f / ( 1.0f + a2 );
+	return ( b2Softness ){ omega / a1, a2 * a3, a3 };
+}
+
+void b2Solve( b2World* world, b2StepContext* stepContext );

src/vendor/box2d/solver_set.c

@@ -0,0 +1,613 @@
+// SPDX-FileCopyrightText: 2023 Erin Catto
+// SPDX-License-Identifier: MIT
+
+#include "solver_set.h"
+
+#include "body.h"
+#include "constraint_graph.h"
+#include "contact.h"
+#include "core.h"
+#include "island.h"
+#include "joint.h"
+#include "world.h"
+
+#include <string.h>
+
+B2_ARRAY_SOURCE( b2SolverSet, b2SolverSet )
+
+void b2DestroySolverSet( b2World* world, int setIndex )
+{
+	b2SolverSet* set = b2SolverSetArray_Get( &world->solverSets, setIndex );
+	b2BodySimArray_Destroy( &set->bodySims );
+	b2BodyStateArray_Destroy( &set->bodyStates );
+	b2ContactSimArray_Destroy( &set->contactSims );
+	b2JointSimArray_Destroy( &set->jointSims );
+	b2IslandSimArray_Destroy( &set->islandSims );
+	b2FreeId( &world->solverSetIdPool, setIndex );
+	*set = ( b2SolverSet ){ 0 };
+	set->setIndex = B2_NULL_INDEX;
+}
+
+// Wake a solver set. Does not merge islands.
+// Contacts can be in several places:
+// 1. non-touching contacts in the disabled set
+// 2. non-touching contacts already in the awake set
+// 3. touching contacts in the sleeping set
+// This handles contact types 1 and 3. Type 2 doesn't need any action.
+void b2WakeSolverSet( b2World* world, int setIndex )
+{
+	B2_ASSERT( setIndex >= b2_firstSleepingSet );
+	b2SolverSet* set = b2SolverSetArray_Get( &world->solverSets, setIndex );
+	b2SolverSet* awakeSet = b2SolverSetArray_Get( &world->solverSets, b2_awakeSet );
+	b2SolverSet* disabledSet = b2SolverSetArray_Get( &world->solverSets, b2_disabledSet );
+
+	b2Body* bodies = world->bodies.data;
+
+	int bodyCount = set->bodySims.count;
+	for ( int i = 0; i < bodyCount; ++i )
+	{
+		b2BodySim* simSrc = set->bodySims.data + i;
+
+		b2Body* body = bodies + simSrc->bodyId;
+		B2_ASSERT( body->setIndex == setIndex );
+		body->setIndex = b2_awakeSet;
+		body->localIndex = awakeSet->bodySims.count;
+
+		// Reset sleep timer
+		body->sleepTime = 0.0f;
+
+		b2BodySim* simDst = b2BodySimArray_Add( &awakeSet->bodySims );
+		memcpy( simDst, simSrc, sizeof( b2BodySim ) );
+
+		b2BodyState* state = b2BodyStateArray_Add( &awakeSet->bodyStates );
+		*state = b2_identityBodyState;
+
+		// move non-touching contacts from disabled set to awake set
+		int contactKey = body->headContactKey;
+		while ( contactKey != B2_NULL_INDEX )
+		{
+			int edgeIndex = contactKey & 1;
+			int contactId = contactKey >> 1;
+
+			b2Contact* contact = b2ContactArray_Get( &world->contacts, contactId );
+
+			contactKey = contact->edges[edgeIndex].nextKey;
+
+			if ( contact->setIndex != b2_disabledSet )
+			{
+				B2_ASSERT( contact->setIndex == b2_awakeSet || contact->setIndex == setIndex );
+				continue;
+			}
+
+			int localIndex = contact->localIndex;
+			b2ContactSim* contactSim = b2ContactSimArray_Get( &disabledSet->contactSims, localIndex );
+
+			B2_ASSERT( ( contact->flags & b2_contactTouchingFlag ) == 0 && contactSim->manifold.pointCount == 0 );
+
+			contact->setIndex = b2_awakeSet;
+			contact->localIndex = awakeSet->contactSims.count;
+			b2ContactSim* awakeContactSim = b2ContactSimArray_Add( &awakeSet->contactSims );
+			memcpy( awakeContactSim, contactSim, sizeof( b2ContactSim ) );
+
+			int movedLocalIndex = b2ContactSimArray_RemoveSwap( &disabledSet->contactSims, localIndex );
+			if ( movedLocalIndex != B2_NULL_INDEX )
+			{
+				// fix moved element
+				b2ContactSim* movedContactSim = disabledSet->contactSims.data + localIndex;
+				b2Contact* movedContact = b2ContactArray_Get( &world->contacts, movedContactSim->contactId );
+				B2_ASSERT( movedContact->localIndex == movedLocalIndex );
+				movedContact->localIndex = localIndex;
+			}
+		}
+	}
+
+	// transfer touching contacts from sleeping set to contact graph
+	{
+		int contactCount = set->contactSims.count;
+		for ( int i = 0; i < contactCount; ++i )
+		{
+			b2ContactSim* contactSim = set->contactSims.data + i;
+			b2Contact* contact = b2ContactArray_Get( &world->contacts, contactSim->contactId );
+			B2_ASSERT( contact->flags & b2_contactTouchingFlag );
+			B2_ASSERT( contactSim->simFlags & b2_simTouchingFlag );
+			B2_ASSERT( contactSim->manifold.pointCount > 0 );
+			B2_ASSERT( contact->setIndex == setIndex );
+			b2AddContactToGraph( world, contactSim, contact );
+			contact->setIndex = b2_awakeSet;
+		}
+	}
+
+	// transfer joints from sleeping set to awake set
+	{
+		int jointCount = set->jointSims.count;
+		for ( int i = 0; i < jointCount; ++i )
+		{
+			b2JointSim* jointSim = set->jointSims.data + i;
+			b2Joint* joint = b2JointArray_Get( &world->joints, jointSim->jointId );
+			B2_ASSERT( joint->setIndex == setIndex );
+			b2AddJointToGraph( world, jointSim, joint );
+			joint->setIndex = b2_awakeSet;
+		}
+	}
+
+	// transfer island from sleeping set to awake set
+	// Usually a sleeping set has only one island, but it is possible
+	// that joints are created between sleeping islands and they
+	// are moved to the same sleeping set.
+	{
+		int islandCount = set->islandSims.count;
+		for ( int i = 0; i < islandCount; ++i )
+		{
+			b2IslandSim* islandSrc = set->islandSims.data + i;
+			b2Island* island = b2IslandArray_Get( &world->islands, islandSrc->islandId );
+			island->setIndex = b2_awakeSet;
+			island->localIndex = awakeSet->islandSims.count;
+			b2IslandSim* islandDst = b2IslandSimArray_Add( &awakeSet->islandSims );
+			memcpy( islandDst, islandSrc, sizeof( b2IslandSim ) );
+		}
+	}
+
+	// destroy the sleeping set
+	b2DestroySolverSet( world, setIndex );
+
+	b2ValidateSolverSets( world );
+}
+
+void b2TrySleepIsland( b2World* world, int islandId )
+{
+	b2Island* island = b2IslandArray_Get( &world->islands, islandId );
+	B2_ASSERT( island->setIndex == b2_awakeSet );
+
+	// cannot put an island to sleep while it has a pending split
+	if ( island->constraintRemoveCount > 0 )
+	{
+		return;
+	}
+
+	// island is sleeping
+	// - create new sleeping solver set
+	// - move island to sleeping solver set
+	// - identify non-touching contacts that should move to sleeping solver set or disabled set
+	// - remove old island
+	// - fix island
+	int sleepSetId = b2AllocId( &world->solverSetIdPool );
+	if ( sleepSetId == world->solverSets.count )
+	{
+		b2SolverSet set = { 0 };
+		set.setIndex = B2_NULL_INDEX;
+		b2SolverSetArray_Push( &world->solverSets, set );
+	}
+
+	b2SolverSet* sleepSet = b2SolverSetArray_Get( &world->solverSets, sleepSetId );
+	*sleepSet = ( b2SolverSet ){ 0 };
+
+	// grab awake set after creating the sleep set because the solver set array may have been resized
+	b2SolverSet* awakeSet = b2SolverSetArray_Get( &world->solverSets, b2_awakeSet );
+	B2_ASSERT( 0 <= island->localIndex && island->localIndex < awakeSet->islandSims.count );
+
+	sleepSet->setIndex = sleepSetId;
+	sleepSet->bodySims = b2BodySimArray_Create( island->bodyCount );
+	sleepSet->contactSims = b2ContactSimArray_Create( island->contactCount );
+	sleepSet->jointSims = b2JointSimArray_Create( island->jointCount );
+
+	// move awake bodies to sleeping set
+	// this shuffles around bodies in the awake set
+	{
+		b2SolverSet* disabledSet = b2SolverSetArray_Get( &world->solverSets, b2_disabledSet );
+		int bodyId = island->headBody;
+		while ( bodyId != B2_NULL_INDEX )
+		{
+			b2Body* body = b2BodyArray_Get( &world->bodies, bodyId );
+			B2_ASSERT( body->setIndex == b2_awakeSet );
+			B2_ASSERT( body->islandId == islandId );
+
+			// Update the body move event to indicate this body fell asleep
+			// It could happen the body is forced asleep before it ever moves.
+			if ( body->bodyMoveIndex != B2_NULL_INDEX )
+			{
+				b2BodyMoveEvent* moveEvent = b2BodyMoveEventArray_Get( &world->bodyMoveEvents, body->bodyMoveIndex );
+				B2_ASSERT( moveEvent->bodyId.index1 - 1 == bodyId );
+				B2_ASSERT( moveEvent->bodyId.generation == body->generation );
+				moveEvent->fellAsleep = true;
+				body->bodyMoveIndex = B2_NULL_INDEX;
+			}
+
+			int awakeBodyIndex = body->localIndex;
+			b2BodySim* awakeSim = b2BodySimArray_Get( &awakeSet->bodySims, awakeBodyIndex );
+
+			// move body sim to sleep set
+			int sleepBodyIndex = sleepSet->bodySims.count;
+			b2BodySim* sleepBodySim = b2BodySimArray_Add( &sleepSet->bodySims );
+			memcpy( sleepBodySim, awakeSim, sizeof( b2BodySim ) );
+
+			int movedIndex = b2BodySimArray_RemoveSwap( &awakeSet->bodySims, awakeBodyIndex );
+			if ( movedIndex != B2_NULL_INDEX )
+			{
+				// fix local index on moved element
+				b2BodySim* movedSim = awakeSet->bodySims.data + awakeBodyIndex;
+				int movedId = movedSim->bodyId;
+				b2Body* movedBody = b2BodyArray_Get( &world->bodies, movedId );
+				B2_ASSERT( movedBody->localIndex == movedIndex );
+				movedBody->localIndex = awakeBodyIndex;
+			}
+
+			// destroy state, no need to clone
+			b2BodyStateArray_RemoveSwap( &awakeSet->bodyStates, awakeBodyIndex );
+
+			body->setIndex = sleepSetId;
+			body->localIndex = sleepBodyIndex;
+
+			// Move non-touching contacts to the disabled set.
+			// Non-touching contacts may exist between sleeping islands and there is no clear ownership.
+			int contactKey = body->headContactKey;
+			while ( contactKey != B2_NULL_INDEX )
+			{
+				int contactId = contactKey >> 1;
+				int edgeIndex = contactKey & 1;
+
+				b2Contact* contact = b2ContactArray_Get( &world->contacts, contactId );
+
+				B2_ASSERT( contact->setIndex == b2_awakeSet || contact->setIndex == b2_disabledSet );
+				contactKey = contact->edges[edgeIndex].nextKey;
+
+				if ( contact->setIndex == b2_disabledSet )
+				{
+					// already moved to disabled set by another body in the island
+					continue;
+				}
+
+				if ( contact->colorIndex != B2_NULL_INDEX )
+				{
+					// contact is touching and will be moved separately
+					B2_ASSERT( ( contact->flags & b2_contactTouchingFlag ) != 0 );
+					continue;
+				}
+
+				// the other body may still be awake, it still may go to sleep and then it will be responsible
+				// for moving this contact to the disabled set.
+				int otherEdgeIndex = edgeIndex ^ 1;
+				int otherBodyId = contact->edges[otherEdgeIndex].bodyId;
+				b2Body* otherBody = b2BodyArray_Get( &world->bodies, otherBodyId );
+				if ( otherBody->setIndex == b2_awakeSet )
+				{
+					continue;
+				}
+
+				int localIndex = contact->localIndex;
+				b2ContactSim* contactSim = b2ContactSimArray_Get( &awakeSet->contactSims, localIndex );
+
+				B2_ASSERT( contactSim->manifold.pointCount == 0 );
+				B2_ASSERT( ( contact->flags & b2_contactTouchingFlag ) == 0 );
+
+				// move the non-touching contact to the disabled set
+				contact->setIndex = b2_disabledSet;
+				contact->localIndex = disabledSet->contactSims.count;
+				b2ContactSim* disabledContactSim = b2ContactSimArray_Add( &disabledSet->contactSims );
+				memcpy( disabledContactSim, contactSim, sizeof( b2ContactSim ) );
+
+				int movedLocalIndex = b2ContactSimArray_RemoveSwap( &awakeSet->contactSims, localIndex );
+				if ( movedLocalIndex != B2_NULL_INDEX )
+				{
+					// fix moved element
+					b2ContactSim* movedContactSim = awakeSet->contactSims.data + localIndex;
+					b2Contact* movedContact = b2ContactArray_Get( &world->contacts, movedContactSim->contactId );
+					B2_ASSERT( movedContact->localIndex == movedLocalIndex );
+					movedContact->localIndex = localIndex;
+				}
+			}
+
+			bodyId = body->islandNext;
+		}
+	}
+
+	// move touching contacts
+	// this shuffles contacts in the awake set
+	{
+		int contactId = island->headContact;
+		while ( contactId != B2_NULL_INDEX )
+		{
+			b2Contact* contact = b2ContactArray_Get( &world->contacts, contactId );
+			B2_ASSERT( contact->setIndex == b2_awakeSet );
+			B2_ASSERT( contact->islandId == islandId );
+			int colorIndex = contact->colorIndex;
+			B2_ASSERT( 0 <= colorIndex && colorIndex < B2_GRAPH_COLOR_COUNT );
+
+			b2GraphColor* color = world->constraintGraph.colors + colorIndex;
+
+			// Remove bodies from graph coloring associated with this constraint
+			if ( colorIndex != B2_OVERFLOW_INDEX )
+			{
+				// might clear a bit for a static body, but this has no effect
+				b2ClearBit( &color->bodySet, contact->edges[0].bodyId );
+				b2ClearBit( &color->bodySet, contact->edges[1].bodyId );
+			}
+
+			int localIndex = contact->localIndex;
+			b2ContactSim* awakeContactSim = b2ContactSimArray_Get( &color->contactSims, localIndex );
+
+			int sleepContactIndex = sleepSet->contactSims.count;
+			b2ContactSim* sleepContactSim = b2ContactSimArray_Add( &sleepSet->contactSims );
+			memcpy( sleepContactSim, awakeContactSim, sizeof( b2ContactSim ) );
+
+			int movedLocalIndex = b2ContactSimArray_RemoveSwap( &color->contactSims, localIndex );
+			if ( movedLocalIndex != B2_NULL_INDEX )
+			{
+				// fix moved element
+				b2ContactSim* movedContactSim = color->contactSims.data + localIndex;
+				b2Contact* movedContact = b2ContactArray_Get( &world->contacts, movedContactSim->contactId );
+				B2_ASSERT( movedContact->localIndex == movedLocalIndex );
+				movedContact->localIndex = localIndex;
+			}
+
+			contact->setIndex = sleepSetId;
+			contact->colorIndex = B2_NULL_INDEX;
+			contact->localIndex = sleepContactIndex;
+
+			contactId = contact->islandNext;
+		}
+	}
+
+	// move joints
+	// this shuffles joints in the awake set
+	{
+		int jointId = island->headJoint;
+		while ( jointId != B2_NULL_INDEX )
+		{
+			b2Joint* joint = b2JointArray_Get( &world->joints, jointId );
+			B2_ASSERT( joint->setIndex == b2_awakeSet );
+			B2_ASSERT( joint->islandId == islandId );
+			int colorIndex = joint->colorIndex;
+			int localIndex = joint->localIndex;
+
+			B2_ASSERT( 0 <= colorIndex && colorIndex < B2_GRAPH_COLOR_COUNT );
+
+			b2GraphColor* color = world->constraintGraph.colors + colorIndex;
+
+			b2JointSim* awakeJointSim = b2JointSimArray_Get( &color->jointSims, localIndex );
+
+			if ( colorIndex != B2_OVERFLOW_INDEX )
+			{
+				// might clear a bit for a static body, but this has no effect
+				b2ClearBit( &color->bodySet, joint->edges[0].bodyId );
+				b2ClearBit( &color->bodySet, joint->edges[1].bodyId );
+			}
+
+			int sleepJointIndex = sleepSet->jointSims.count;
+			b2JointSim* sleepJointSim = b2JointSimArray_Add( &sleepSet->jointSims );
+			memcpy( sleepJointSim, awakeJointSim, sizeof( b2JointSim ) );
+
+			int movedIndex = b2JointSimArray_RemoveSwap( &color->jointSims, localIndex );
+			if ( movedIndex != B2_NULL_INDEX )
+			{
+				// fix moved element
+				b2JointSim* movedJointSim = color->jointSims.data + localIndex;
+				int movedId = movedJointSim->jointId;
+				b2Joint* movedJoint = b2JointArray_Get( &world->joints, movedId );
+				B2_ASSERT( movedJoint->localIndex == movedIndex );
+				movedJoint->localIndex = localIndex;
+			}
+
+			joint->setIndex = sleepSetId;
+			joint->colorIndex = B2_NULL_INDEX;
+			joint->localIndex = sleepJointIndex;
+
+			jointId = joint->islandNext;
+		}
+	}
+
+	// move island struct
+	{
+		B2_ASSERT( island->setIndex == b2_awakeSet );
+
+		int islandIndex = island->localIndex;
+		b2IslandSim* sleepIsland = b2IslandSimArray_Add( &sleepSet->islandSims );
+		sleepIsland->islandId = islandId;
+
+		int movedIslandIndex = b2IslandSimArray_RemoveSwap( &awakeSet->islandSims, islandIndex );
+		if ( movedIslandIndex != B2_NULL_INDEX )
+		{
+			// fix index on moved element
+			b2IslandSim* movedIslandSim = awakeSet->islandSims.data + islandIndex;
+			int movedIslandId = movedIslandSim->islandId;
+			b2Island* movedIsland = b2IslandArray_Get( &world->islands, movedIslandId );
+			B2_ASSERT( movedIsland->localIndex == movedIslandIndex );
+			movedIsland->localIndex = islandIndex;
+		}
+
+		island->setIndex = sleepSetId;
+		island->localIndex = 0;
+	}
+
+	b2ValidateSolverSets( world );
+}
+
+// This is called when joints are created between sets. I want to allow the sets
+// to continue sleeping if both are asleep. Otherwise one set is waked.
+// Islands will get merge when the set is waked.
+void b2MergeSolverSets( b2World* world, int setId1, int setId2 )
+{
+	B2_ASSERT( setId1 >= b2_firstSleepingSet );
+	B2_ASSERT( setId2 >= b2_firstSleepingSet );
+	b2SolverSet* set1 = b2SolverSetArray_Get( &world->solverSets, setId1 );
+	b2SolverSet* set2 = b2SolverSetArray_Get( &world->solverSets, setId2 );
+
+	// Move the fewest number of bodies
+	if ( set1->bodySims.count < set2->bodySims.count )
+	{
+		b2SolverSet* tempSet = set1;
+		set1 = set2;
+		set2 = tempSet;
+
+		int tempId = setId1;
+		setId1 = setId2;
+		setId2 = tempId;
+	}
+
+	// transfer bodies
+	{
+		b2Body* bodies = world->bodies.data;
+		int bodyCount = set2->bodySims.count;
+		for ( int i = 0; i < bodyCount; ++i )
+		{
+			b2BodySim* simSrc = set2->bodySims.data + i;
+
+			b2Body* body = bodies + simSrc->bodyId;
+			B2_ASSERT( body->setIndex == setId2 );
+			body->setIndex = setId1;
+			body->localIndex = set1->bodySims.count;
+
+			b2BodySim* simDst = b2BodySimArray_Add( &set1->bodySims );
+			memcpy( simDst, simSrc, sizeof( b2BodySim ) );
+		}
+	}
+
+	// transfer contacts
+	{
+		int contactCount = set2->contactSims.count;
+		for ( int i = 0; i < contactCount; ++i )
+		{
+			b2ContactSim* contactSrc = set2->contactSims.data + i;
+
+			b2Contact* contact = b2ContactArray_Get( &world->contacts, contactSrc->contactId );
+			B2_ASSERT( contact->setIndex == setId2 );
+			contact->setIndex = setId1;
+			contact->localIndex = set1->contactSims.count;
+
+			b2ContactSim* contactDst = b2ContactSimArray_Add( &set1->contactSims );
+			memcpy( contactDst, contactSrc, sizeof( b2ContactSim ) );
+		}
+	}
+
+	// transfer joints
+	{
+		int jointCount = set2->jointSims.count;
+		for ( int i = 0; i < jointCount; ++i )
+		{
+			b2JointSim* jointSrc = set2->jointSims.data + i;
+
+			b2Joint* joint = b2JointArray_Get( &world->joints, jointSrc->jointId );
+			B2_ASSERT( joint->setIndex == setId2 );
+			joint->setIndex = setId1;
+			joint->localIndex = set1->jointSims.count;
+
+			b2JointSim* jointDst = b2JointSimArray_Add( &set1->jointSims );
+			memcpy( jointDst, jointSrc, sizeof( b2JointSim ) );
+		}
+	}
+
+	// transfer islands
+	{
+		int islandCount = set2->islandSims.count;
+		for ( int i = 0; i < islandCount; ++i )
+		{
+			b2IslandSim* islandSrc = set2->islandSims.data + i;
+			int islandId = islandSrc->islandId;
+
+			b2Island* island = b2IslandArray_Get( &world->islands, islandId );
+			island->setIndex = setId1;
+			island->localIndex = set1->islandSims.count;
+
+			b2IslandSim* islandDst = b2IslandSimArray_Add( &set1->islandSims );
+			memcpy( islandDst, islandSrc, sizeof( b2IslandSim ) );
+		}
+	}
+
+	// destroy the merged set
+	b2DestroySolverSet( world, setId2 );
+
+	b2ValidateSolverSets( world );
+}
+
+void b2TransferBody( b2World* world, b2SolverSet* targetSet, b2SolverSet* sourceSet, b2Body* body )
+{
+	B2_ASSERT( targetSet != sourceSet );
+
+	int sourceIndex = body->localIndex;
+	b2BodySim* sourceSim = b2BodySimArray_Get( &sourceSet->bodySims, sourceIndex );
+
+	int targetIndex = targetSet->bodySims.count;
+	b2BodySim* targetSim = b2BodySimArray_Add( &targetSet->bodySims );
+	memcpy( targetSim, sourceSim, sizeof( b2BodySim ) );
+
+	// Remove body sim from solver set that owns it
+	int movedIndex = b2BodySimArray_RemoveSwap( &sourceSet->bodySims, sourceIndex );
+	if ( movedIndex != B2_NULL_INDEX )
+	{
+		// Fix moved body index
+		b2BodySim* movedSim = sourceSet->bodySims.data + sourceIndex;
+		int movedId = movedSim->bodyId;
+		b2Body* movedBody = b2BodyArray_Get( &world->bodies, movedId );
+		B2_ASSERT( movedBody->localIndex == movedIndex );
+		movedBody->localIndex = sourceIndex;
+	}
+
+	if ( sourceSet->setIndex == b2_awakeSet )
+	{
+		b2BodyStateArray_RemoveSwap( &sourceSet->bodyStates, sourceIndex );
+	}
+	else if ( targetSet->setIndex == b2_awakeSet )
+	{
+		b2BodyState* state = b2BodyStateArray_Add( &targetSet->bodyStates );
+		*state = b2_identityBodyState;
+	}
+
+	body->setIndex = targetSet->setIndex;
+	body->localIndex = targetIndex;
+}
+
+void b2TransferJoint( b2World* world, b2SolverSet* targetSet, b2SolverSet* sourceSet, b2Joint* joint )
+{
+	B2_ASSERT( targetSet != sourceSet );
+
+	int localIndex = joint->localIndex;
+	int colorIndex = joint->colorIndex;
+
+	// Retrieve source.
+	b2JointSim* sourceSim;
+	if ( sourceSet->setIndex == b2_awakeSet )
+	{
+		B2_ASSERT( 0 <= colorIndex && colorIndex < B2_GRAPH_COLOR_COUNT );
+		b2GraphColor* color = world->constraintGraph.colors + colorIndex;
+
+		sourceSim = b2JointSimArray_Get( &color->jointSims, localIndex );
+	}
+	else
+	{
+		B2_ASSERT( colorIndex == B2_NULL_INDEX );
+		sourceSim = b2JointSimArray_Get( &sourceSet->jointSims, localIndex );
+	}
+
+	// Create target and copy. Fix joint.
+	if ( targetSet->setIndex == b2_awakeSet )
+	{
+		b2AddJointToGraph( world, sourceSim, joint );
+		joint->setIndex = b2_awakeSet;
+	}
+	else
+	{
+		joint->setIndex = targetSet->setIndex;
+		joint->localIndex = targetSet->jointSims.count;
+		joint->colorIndex = B2_NULL_INDEX;
+
+		b2JointSim* targetSim = b2JointSimArray_Add( &targetSet->jointSims );
+		memcpy( targetSim, sourceSim, sizeof( b2JointSim ) );
+	}
+
+	// Destroy source.
+	if ( sourceSet->setIndex == b2_awakeSet )
+	{
+		b2RemoveJointFromGraph( world, joint->edges[0].bodyId, joint->edges[1].bodyId, colorIndex, localIndex );
+	}
+	else
+	{
+		int movedIndex = b2JointSimArray_RemoveSwap( &sourceSet->jointSims, localIndex );
+		if ( movedIndex != B2_NULL_INDEX )
+		{
+			// fix swapped element
+			b2JointSim* movedJointSim = sourceSet->jointSims.data + localIndex;
+			int movedId = movedJointSim->jointId;
+			b2Joint* movedJoint = b2JointArray_Get( &world->joints, movedId );
+			movedJoint->localIndex = localIndex;
+		}
+	}
+}

src/vendor/box2d/solver_set.h

@@ -0,0 +1,57 @@
+// SPDX-FileCopyrightText: 2023 Erin Catto
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include "array.h"
+
+typedef struct b2Body b2Body;
+typedef struct b2Joint b2Joint;
+typedef struct b2World b2World;
+
+// This holds solver set data. The following sets are used:
+// - static set for all static bodies (no contacts or joints)
+// - active set for all active bodies with body states (no contacts or joints)
+// - disabled set for disabled bodies and their joints
+// - all further sets are sleeping island sets along with their contacts and joints
+// The purpose of solver sets is to achieve high memory locality.
+// https://www.youtube.com/watch?v=nZNd5FjSquk
+typedef struct b2SolverSet
+{
+	// Body array. Empty for unused set.
+	b2BodySimArray bodySims;
+
+	// Body state only exists for active set
+	b2BodyStateArray bodyStates;
+
+	// This holds sleeping/disabled joints. Empty for static/active set.
+	b2JointSimArray jointSims;
+
+	// This holds all contacts for sleeping sets.
+	// This holds non-touching contacts for the awake set.
+	b2ContactSimArray contactSims;
+
+	// The awake set has an array of islands. Sleeping sets normally have a single islands. However, joints
+	// created between sleeping sets causes the sets to merge, leaving them with multiple islands. These sleeping
+	// islands will be naturally merged with the set is woken.
+	// The static and disabled sets have no islands.
+	// Islands live in the solver sets to limit the number of islands that need to be considered for sleeping.
+	b2IslandSimArray islandSims;
+
+	// Aligns with b2World::solverSetIdPool. Used to create a stable id for body/contact/joint/islands.
+	int setIndex;
+} b2SolverSet;
+
+void b2DestroySolverSet( b2World* world, int setIndex );
+
+void b2WakeSolverSet( b2World* world, int setIndex );
+void b2TrySleepIsland( b2World* world, int islandId );
+
+// Merge set 2 into set 1 then destroy set 2.
+// Warning: any pointers into these sets will be orphaned.
+void b2MergeSolverSets( b2World* world, int setIndex1, int setIndex2 );
+
+void b2TransferBody( b2World* world, b2SolverSet* targetSet, b2SolverSet* sourceSet, b2Body* body );
+void b2TransferJoint( b2World* world, b2SolverSet* targetSet, b2SolverSet* sourceSet, b2Joint* joint );
+
+B2_ARRAY_INLINE( b2SolverSet, b2SolverSet )

src/vendor/box2d/table.c

@@ -0,0 +1,238 @@
+// SPDX-FileCopyrightText: 2023 Erin Catto
+// SPDX-License-Identifier: MIT
+
+#include "table.h"
+
+#include "atomic.h"
+#include "core.h"
+#include "ctz.h"
+
+#include <stdbool.h>
+#include <string.h>
+
+#if B2_SNOOP_TABLE_COUNTERS
+b2AtomicInt b2_findCount;
+b2AtomicInt b2_probeCount;
+#endif
+
+// todo compare with https://github.com/skeeto/scratch/blob/master/set32/set32.h
+
+b2HashSet b2CreateSet( int capacity )
+{
+	b2HashSet set = { 0 };
+
+	// Capacity must be a power of 2
+	if ( capacity > 16 )
+	{
+		set.capacity = b2RoundUpPowerOf2( capacity );
+	}
+	else
+	{
+		set.capacity = 16;
+	}
+
+	set.count = 0;
+	set.items = b2Alloc( capacity * sizeof( b2SetItem ) );
+	memset( set.items, 0, capacity * sizeof( b2SetItem ) );
+
+	return set;
+}
+
+void b2DestroySet( b2HashSet* set )
+{
+	b2Free( set->items, set->capacity * sizeof( b2SetItem ) );
+	set->items = NULL;
+	set->count = 0;
+	set->capacity = 0;
+}
+
+void b2ClearSet( b2HashSet* set )
+{
+	set->count = 0;
+	memset( set->items, 0, set->capacity * sizeof( b2SetItem ) );
+}
+
+// I need a good hash because the keys are built from pairs of increasing integers.
+// A simple hash like hash = (integer1 XOR integer2) has many collisions.
+// https://lemire.me/blog/2018/08/15/fast-strongly-universal-64-bit-hashing-everywhere/
+// https://preshing.com/20130107/this-hash-set-is-faster-than-a-judy-array/
+// todo try: https://www.jandrewrogers.com/2019/02/12/fast-perfect-hashing/
+// todo try: https://probablydance.com/2018/06/16/fibonacci-hashing-the-optimization-that-the-world-forgot-or-a-better-alternative-to-integer-modulo/
+static uint32_t b2KeyHash( uint64_t key )
+{
+	// Murmur hash
+	uint64_t h = key;
+	h ^= h >> 33;
+	h *= 0xff51afd7ed558ccduLL;
+	h ^= h >> 33;
+	h *= 0xc4ceb9fe1a85ec53uLL;
+	h ^= h >> 33;
+
+	return (uint32_t)h;
+}
+
+static int b2FindSlot( const b2HashSet* set, uint64_t key, uint32_t hash )
+{
+#if B2_SNOOP_TABLE_COUNTERS 
+		b2AtomicFetchAddInt( &b2_findCount, 1 );
+#endif
+
+	uint32_t capacity = set->capacity;
+	int index = hash & ( capacity - 1 );
+	const b2SetItem* items = set->items;
+	while ( items[index].hash != 0 && items[index].key != key )
+	{
+#if B2_SNOOP_TABLE_COUNTERS
+		b2AtomicFetchAddInt( &b2_probeCount, 1 );
+#endif
+		index = ( index + 1 ) & ( capacity - 1 );
+	}
+
+	return index;
+}
+
+static void b2AddKeyHaveCapacity( b2HashSet* set, uint64_t key, uint32_t hash )
+{
+	int index = b2FindSlot( set, key, hash );
+	b2SetItem* items = set->items;
+	B2_ASSERT( items[index].hash == 0 );
+
+	items[index].key = key;
+	items[index].hash = hash;
+	set->count += 1;
+}
+
+static void b2GrowTable( b2HashSet* set )
+{
+	uint32_t oldCount = set->count;
+	B2_UNUSED( oldCount );
+
+	uint32_t oldCapacity = set->capacity;
+	b2SetItem* oldItems = set->items;
+
+	set->count = 0;
+	// Capacity must be a power of 2
+	set->capacity = 2 * oldCapacity;
+	set->items = b2Alloc( set->capacity * sizeof( b2SetItem ) );
+	memset( set->items, 0, set->capacity * sizeof( b2SetItem ) );
+
+	// Transfer items into new array
+	for ( uint32_t i = 0; i < oldCapacity; ++i )
+	{
+		b2SetItem* item = oldItems + i;
+		if ( item->hash == 0 )
+		{
+			// this item was empty
+			continue;
+		}
+
+		b2AddKeyHaveCapacity( set, item->key, item->hash );
+	}
+
+	B2_ASSERT( set->count == oldCount );
+
+	b2Free( oldItems, oldCapacity * sizeof( b2SetItem ) );
+}
+
+bool b2ContainsKey( const b2HashSet* set, uint64_t key )
+{
+	// key of zero is a sentinel
+	B2_ASSERT( key != 0 );
+	uint32_t hash = b2KeyHash( key );
+	int index = b2FindSlot( set, key, hash );
+	return set->items[index].key == key;
+}
+
+int b2GetHashSetBytes( b2HashSet* set )
+{
+	return set->capacity * (int)sizeof( b2SetItem );
+}
+
+bool b2AddKey( b2HashSet* set, uint64_t key )
+{
+	// key of zero is a sentinel
+	B2_ASSERT( key != 0 );
+
+	uint32_t hash = b2KeyHash( key );
+	B2_ASSERT( hash != 0 );
+
+	int index = b2FindSlot( set, key, hash );
+	if ( set->items[index].hash != 0 )
+	{
+		// Already in set
+		B2_ASSERT( set->items[index].hash == hash && set->items[index].key == key );
+		return true;
+	}
+
+	if ( 2 * set->count >= set->capacity )
+	{
+		b2GrowTable( set );
+	}
+
+	b2AddKeyHaveCapacity( set, key, hash );
+	return false;
+}
+
+// See https://en.wikipedia.org/wiki/Open_addressing
+bool b2RemoveKey( b2HashSet* set, uint64_t key )
+{
+	uint32_t hash = b2KeyHash( key );
+	int i = b2FindSlot( set, key, hash );
+	b2SetItem* items = set->items;
+	if ( items[i].hash == 0 )
+	{
+		// Not in set
+		return false;
+	}
+
+	// Mark item i as unoccupied
+	items[i].key = 0;
+	items[i].hash = 0;
+
+	B2_ASSERT( set->count > 0 );
+	set->count -= 1;
+
+	// Attempt to fill item i
+	int j = i;
+	uint32_t capacity = set->capacity;
+	for ( ;; )
+	{
+		j = ( j + 1 ) & ( capacity - 1 );
+		if ( items[j].hash == 0 )
+		{
+			break;
+		}
+
+		// k is the first item for the hash of j
+		int k = items[j].hash & ( capacity - 1 );
+
+		// determine if k lies cyclically in (i,j]
+		// i <= j: | i..k..j |
+		// i > j: |.k..j  i....| or |....j     i..k.|
+		if ( i <= j )
+		{
+			if ( i < k && k <= j )
+			{
+				continue;
+			}
+		}
+		else
+		{
+			if ( i < k || k <= j )
+			{
+				continue;
+			}
+		}
+
+		// Move j into i
+		items[i] = items[j];
+
+		// Mark item j as unoccupied
+		items[j].key = 0;
+		items[j].hash = 0;
+
+		i = j;
+	}
+
+	return true;
+}

src/vendor/box2d/table.h

@@ -0,0 +1,37 @@
+// SPDX-FileCopyrightText: 2023 Erin Catto
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <stdbool.h>
+#include <stdint.h>
+
+#define B2_SHAPE_PAIR_KEY( K1, K2 ) K1 < K2 ? (uint64_t)K1 << 32 | (uint64_t)K2 : (uint64_t)K2 << 32 | (uint64_t)K1
+
+typedef struct b2SetItem
+{
+	uint64_t key;
+	uint32_t hash;
+} b2SetItem;
+
+typedef struct b2HashSet
+{
+	b2SetItem* items;
+	uint32_t capacity;
+	uint32_t count;
+} b2HashSet;
+
+b2HashSet b2CreateSet( int capacity );
+void b2DestroySet( b2HashSet* set );
+
+void b2ClearSet( b2HashSet* set );
+
+// Returns true if key was already in set
+bool b2AddKey( b2HashSet* set, uint64_t key );
+
+// Returns true if the key was found
+bool b2RemoveKey( b2HashSet* set, uint64_t key );
+
+bool b2ContainsKey( const b2HashSet* set, uint64_t key );
+
+int b2GetHashSetBytes( b2HashSet* set );

src/vendor/box2d/timer.c

@@ -0,0 +1,185 @@
+// SPDX-FileCopyrightText: 2023 Erin Catto
+// SPDX-License-Identifier: MIT
+
+#include "box2d/base.h"
+
+#include <stddef.h>
+
+#if defined( _MSC_VER )
+
+#ifndef WIN32_LEAN_AND_MEAN
+#define WIN32_LEAN_AND_MEAN 1
+#endif
+
+#include <windows.h>
+
+static double s_invFrequency = 0.0;
+
+uint64_t b2GetTicks( void )
+{
+	LARGE_INTEGER counter;
+	QueryPerformanceCounter( &counter );
+	return (uint64_t)counter.QuadPart;
+}
+
+float b2GetMilliseconds( uint64_t ticks )
+{
+	if ( s_invFrequency == 0.0 )
+	{
+		LARGE_INTEGER frequency;
+		QueryPerformanceFrequency( &frequency );
+
+		s_invFrequency = (double)frequency.QuadPart;
+		if ( s_invFrequency > 0.0 )
+		{
+			s_invFrequency = 1000.0 / s_invFrequency;
+		}
+	}
+
+	uint64_t ticksNow = b2GetTicks();
+	return (float)( s_invFrequency * ( ticksNow - ticks ) );
+}
+
+float b2GetMillisecondsAndReset( uint64_t* ticks )
+{
+	if ( s_invFrequency == 0.0 )
+	{
+		LARGE_INTEGER frequency;
+		QueryPerformanceFrequency( &frequency );
+
+		s_invFrequency = (double)frequency.QuadPart;
+		if ( s_invFrequency > 0.0 )
+		{
+			s_invFrequency = 1000.0 / s_invFrequency;
+		}
+	}
+
+	uint64_t ticksNow = b2GetTicks();
+	float ms = (float)( s_invFrequency * ( ticksNow - *ticks ) );
+	*ticks = ticksNow;
+	return ms;
+}
+
+void b2Yield( void )
+{
+	SwitchToThread();
+}
+
+#elif defined( __linux__ ) || defined( __EMSCRIPTEN__ )
+
+#include <sched.h>
+#include <time.h>
+
+uint64_t b2GetTicks( void )
+{
+	struct timespec ts;
+	clock_gettime( CLOCK_MONOTONIC, &ts );
+	return ts.tv_sec * 1000000000LL + ts.tv_nsec;
+}
+
+float b2GetMilliseconds( uint64_t ticks )
+{
+	uint64_t ticksNow = b2GetTicks();
+	return (float)( (ticksNow - ticks) / 1000000.0 );
+}
+
+float b2GetMillisecondsAndReset( uint64_t* ticks )
+{
+	uint64_t ticksNow = b2GetTicks();
+	float ms = (float)( (ticksNow - *ticks) / 1000000.0 );
+	*ticks = ticksNow;
+	return ms;
+}
+
+void b2Yield( void )
+{
+	sched_yield();
+}
+
+#elif defined( __APPLE__ )
+
+#include <mach/mach_time.h>
+#include <sched.h>
+#include <sys/time.h>
+
+static double s_invFrequency = 0.0;
+
+uint64_t b2GetTicks( void )
+{
+	return mach_absolute_time();
+}
+
+float b2GetMilliseconds( uint64_t ticks )
+{
+	if ( s_invFrequency == 0 )
+	{
+		mach_timebase_info_data_t timebase;
+		mach_timebase_info( &timebase );
+
+		// convert to ns then to ms
+		s_invFrequency = 1e-6 * (double)timebase.numer / (double)timebase.denom;
+	}
+
+	uint64_t ticksNow = b2GetTicks();
+	return (float)( s_invFrequency * (ticksNow - ticks) );
+}
+
+float b2GetMillisecondsAndReset( uint64_t* ticks )
+{
+	if ( s_invFrequency == 0 )
+	{
+		mach_timebase_info_data_t timebase;
+		mach_timebase_info( &timebase );
+
+		// convert to ns then to ms
+		s_invFrequency = 1e-6 * (double)timebase.numer / (double)timebase.denom;
+	}
+
+	uint64_t ticksNow = b2GetTicks();
+	float ms = (float)( s_invFrequency * ( ticksNow - *ticks ) );
+	*ticks = ticksNow;
+	return ms;
+}
+
+void b2Yield( void )
+{
+	sched_yield();
+}
+
+#else
+
+uint64_t b2GetTicks( void )
+{
+	return 0;
+}
+
+float b2GetMilliseconds( uint64_t ticks )
+{
+	( (void)( ticks ) );
+	return 0.0f;
+}
+
+float b2GetMillisecondsAndReset( uint64_t* ticks )
+{
+	( (void)( ticks ) );
+	return 0.0f;
+}
+
+void b2Yield( void )
+{
+}
+
+#endif
+
+// djb2 hash
+// https://en.wikipedia.org/wiki/List_of_hash_functions
+uint32_t b2Hash( uint32_t hash, const uint8_t* data, int count )
+{
+	uint32_t result = hash;
+	for ( int i = 0; i < count; i++ )
+	{
+		result = ( result << 5 ) + result + data[i];
+	}
+
+	return result;
+}

src/vendor/box2d/types.c

@@ -0,0 +1,151 @@
+// SPDX-FileCopyrightText: 2023 Erin Catto
+// SPDX-License-Identifier: MIT
+
+#include "box2d/types.h"
+
+#include "constants.h"
+#include "core.h"
+
+b2WorldDef b2DefaultWorldDef( void )
+{
+	b2WorldDef def = { 0 };
+	def.gravity.x = 0.0f;
+	def.gravity.y = -10.0f;
+	def.hitEventThreshold = 1.0f * b2_lengthUnitsPerMeter;
+	def.restitutionThreshold = 1.0f * b2_lengthUnitsPerMeter;
+	def.maxContactPushSpeed = 3.0f * b2_lengthUnitsPerMeter;
+	def.contactHertz = 30.0;
+	def.contactDampingRatio = 10.0f;
+	def.jointHertz = 60.0;
+	def.jointDampingRatio = 2.0f;
+	// 400 meters per second, faster than the speed of sound
+	def.maximumLinearSpeed = 400.0f * b2_lengthUnitsPerMeter;
+	def.enableSleep = true;
+	def.enableContinuous = true;
+	def.internalValue = B2_SECRET_COOKIE;
+	return def;
+}
+
+b2BodyDef b2DefaultBodyDef( void )
+{
+	b2BodyDef def = { 0 };
+	def.type = b2_staticBody;
+	def.rotation = b2Rot_identity;
+	def.sleepThreshold = 0.05f * b2_lengthUnitsPerMeter;
+	def.gravityScale = 1.0f;
+	def.enableSleep = true;
+	def.isAwake = true;
+	def.isEnabled = true;
+	def.internalValue = B2_SECRET_COOKIE;
+	return def;
+}
+
+b2Filter b2DefaultFilter( void )
+{
+	b2Filter filter = { B2_DEFAULT_CATEGORY_BITS, B2_DEFAULT_MASK_BITS, 0 };
+	return filter;
+}
+
+b2QueryFilter b2DefaultQueryFilter( void )
+{
+	b2QueryFilter filter = { B2_DEFAULT_CATEGORY_BITS, B2_DEFAULT_MASK_BITS };
+	return filter;
+}
+
+b2ShapeDef b2DefaultShapeDef( void )
+{
+	b2ShapeDef def = { 0 };
+	def.material.friction = 0.6f;
+	def.density = 1.0f;
+	def.filter = b2DefaultFilter();
+	def.updateBodyMass = true;
+	def.invokeContactCreation = true;
+	def.internalValue = B2_SECRET_COOKIE;
+	return def;
+}
+
+b2SurfaceMaterial b2DefaultSurfaceMaterial( void )
+{
+	b2SurfaceMaterial material = {
+		.friction = 0.6f,
+	};
+
+	return material;
+}
+
+b2ChainDef b2DefaultChainDef( void )
+{
+	static b2SurfaceMaterial defaultMaterial = {
+		.friction = 0.6f,
+	};
+
+	b2ChainDef def = { 0 };
+	def.materials = &defaultMaterial;
+	def.materialCount = 1;
+	def.filter = b2DefaultFilter();
+	def.internalValue = B2_SECRET_COOKIE;
+	return def;
+}
+
+static void b2EmptyDrawPolygon( const b2Vec2* vertices, int vertexCount, b2HexColor color, void* context )
+{
+	B2_UNUSED( vertices, vertexCount, color, context );
+}
+
+static void b2EmptyDrawSolidPolygon( b2Transform transform, const b2Vec2* vertices, int vertexCount, float radius,
+									 b2HexColor color, void* context )
+{
+	B2_UNUSED( transform, vertices, vertexCount, radius, color, context );
+}
+
+static void b2EmptyDrawCircle( b2Vec2 center, float radius, b2HexColor color, void* context )
+{
+	B2_UNUSED( center, radius, color, context );
+}
+
+static void b2EmptyDrawSolidCircle( b2Transform transform, float radius, b2HexColor color, void* context )
+{
+	B2_UNUSED( transform, radius, color, context );
+}
+
+static void b2EmptyDrawSolidCapsule( b2Vec2 p1, b2Vec2 p2, float radius, b2HexColor color, void* context )
+{
+	B2_UNUSED( p1, p2, radius, color, context );
+}
+
+static void b2EmptyDrawSegment( b2Vec2 p1, b2Vec2 p2, b2HexColor color, void* context )
+{
+	B2_UNUSED( p1, p2, color, context );
+}
+
+static void b2EmptyDrawTransform( b2Transform transform, void* context )
+{
+	B2_UNUSED( transform, context );
+}
+
+static void b2EmptyDrawPoint( b2Vec2 p, float size, b2HexColor color, void* context )
+{
+	B2_UNUSED( p, size, color, context );
+}
+
+static void b2EmptyDrawString( b2Vec2 p, const char* s, b2HexColor color, void* context )
+{
+	B2_UNUSED( p, s, color, context );
+}
+
+b2DebugDraw b2DefaultDebugDraw( void )
+{
+	b2DebugDraw draw = { 0 };
+
+	// These allow the user to skip some implementations and not hit null exceptions.
+	draw.DrawPolygonFcn = b2EmptyDrawPolygon;
+	draw.DrawSolidPolygonFcn = b2EmptyDrawSolidPolygon;
+	draw.DrawCircleFcn = b2EmptyDrawCircle;
+	draw.DrawSolidCircleFcn = b2EmptyDrawSolidCircle;
+	draw.DrawSolidCapsuleFcn = b2EmptyDrawSolidCapsule;
+	draw.DrawSegmentFcn = b2EmptyDrawSegment;
+	draw.DrawTransformFcn = b2EmptyDrawTransform;
+	draw.DrawPointFcn = b2EmptyDrawPoint;
+	draw.DrawStringFcn = b2EmptyDrawString;
+	return draw;
+}

src/vendor/box2d/weld_joint.c

@@ -0,0 +1,310 @@
+// SPDX-FileCopyrightText: 2023 Erin Catto
+// SPDX-License-Identifier: MIT
+
+#include "body.h"
+#include "core.h"
+#include "joint.h"
+#include "solver.h"
+#include "solver_set.h"
+#include "world.h"
+
+// needed for dll export
+#include "box2d/box2d.h"
+
+float b2WeldJoint_GetReferenceAngle( b2JointId jointId )
+{
+	b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_weldJoint );
+	return joint->weldJoint.referenceAngle;
+}
+
+void b2WeldJoint_SetReferenceAngle( b2JointId jointId, float angleInRadians )
+{
+	B2_ASSERT( b2IsValidFloat( angleInRadians ) );
+	b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_weldJoint );
+	joint->weldJoint.referenceAngle = b2ClampFloat(angleInRadians, -B2_PI, B2_PI);
+}
+
+void b2WeldJoint_SetLinearHertz( b2JointId jointId, float hertz )
+{
+	B2_ASSERT( b2IsValidFloat( hertz ) && hertz >= 0.0f );
+	b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_weldJoint );
+	joint->weldJoint.linearHertz = hertz;
+}
+
+float b2WeldJoint_GetLinearHertz( b2JointId jointId )
+{
+	b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_weldJoint );
+	return joint->weldJoint.linearHertz;
+}
+
+void b2WeldJoint_SetLinearDampingRatio( b2JointId jointId, float dampingRatio )
+{
+	B2_ASSERT( b2IsValidFloat( dampingRatio ) && dampingRatio >= 0.0f );
+	b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_weldJoint );
+	joint->weldJoint.linearDampingRatio = dampingRatio;
+}
+
+float b2WeldJoint_GetLinearDampingRatio( b2JointId jointId )
+{
+	b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_weldJoint );
+	return joint->weldJoint.linearDampingRatio;
+}
+
+void b2WeldJoint_SetAngularHertz( b2JointId jointId, float hertz )
+{
+	B2_ASSERT( b2IsValidFloat( hertz ) && hertz >= 0.0f );
+	b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_weldJoint );
+	joint->weldJoint.angularHertz = hertz;
+}
+
+float b2WeldJoint_GetAngularHertz( b2JointId jointId )
+{
+	b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_weldJoint );
+	return joint->weldJoint.angularHertz;
+}
+
+void b2WeldJoint_SetAngularDampingRatio( b2JointId jointId, float dampingRatio )
+{
+	B2_ASSERT( b2IsValidFloat( dampingRatio ) && dampingRatio >= 0.0f );
+	b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_weldJoint );
+	joint->weldJoint.angularDampingRatio = dampingRatio;
+}
+
+float b2WeldJoint_GetAngularDampingRatio( b2JointId jointId )
+{
+	b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_weldJoint );
+	return joint->weldJoint.angularDampingRatio;
+}
+
+b2Vec2 b2GetWeldJointForce( b2World* world, b2JointSim* base )
+{
+	b2Vec2 force = b2MulSV( world->inv_h, base->weldJoint.linearImpulse );
+	return force;
+}
+
+float b2GetWeldJointTorque( b2World* world, b2JointSim* base )
+{
+	return world->inv_h * base->weldJoint.angularImpulse;
+}
+
+// Point-to-point constraint
+// C = p2 - p1
+// Cdot = v2 - v1
+//      = v2 + cross(w2, r2) - v1 - cross(w1, r1)
+// J = [-I -r1_skew I r2_skew ]
+// Identity used:
+// w k % (rx i + ry j) = w * (-ry i + rx j)
+
+// Angle constraint
+// C = angle2 - angle1 - referenceAngle
+// Cdot = w2 - w1
+// J = [0 0 -1 0 0 1]
+// K = invI1 + invI2
+
+void b2PrepareWeldJoint( b2JointSim* base, b2StepContext* context )
+{
+	B2_ASSERT( base->type == b2_weldJoint );
+
+	// chase body id to the solver set where the body lives
+	int idA = base->bodyIdA;
+	int idB = base->bodyIdB;
+
+	b2World* world = context->world;
+
+	b2Body* bodyA = b2BodyArray_Get( &world->bodies, idA );
+	b2Body* bodyB = b2BodyArray_Get( &world->bodies, idB );
+
+	B2_ASSERT( bodyA->setIndex == b2_awakeSet || bodyB->setIndex == b2_awakeSet );
+	b2SolverSet* setA = b2SolverSetArray_Get( &world->solverSets, bodyA->setIndex );
+	b2SolverSet* setB = b2SolverSetArray_Get( &world->solverSets, bodyB->setIndex );
+
+	int localIndexA = bodyA->localIndex;
+	int localIndexB = bodyB->localIndex;
+
+	b2BodySim* bodySimA = b2BodySimArray_Get( &setA->bodySims, localIndexA );
+	b2BodySim* bodySimB = b2BodySimArray_Get( &setB->bodySims, localIndexB );
+
+	float mA = bodySimA->invMass;
+	float iA = bodySimA->invInertia;
+	float mB = bodySimB->invMass;
+	float iB = bodySimB->invInertia;
+
+	base->invMassA = mA;
+	base->invMassB = mB;
+	base->invIA = iA;
+	base->invIB = iB;
+
+	b2WeldJoint* joint = &base->weldJoint;
+	joint->indexA = bodyA->setIndex == b2_awakeSet ? localIndexA : B2_NULL_INDEX;
+	joint->indexB = bodyB->setIndex == b2_awakeSet ? localIndexB : B2_NULL_INDEX;
+
+	b2Rot qA = bodySimA->transform.q;
+	b2Rot qB = bodySimB->transform.q;
+
+	joint->anchorA = b2RotateVector( qA, b2Sub( base->localOriginAnchorA, bodySimA->localCenter ) );
+	joint->anchorB = b2RotateVector( qB, b2Sub( base->localOriginAnchorB, bodySimB->localCenter ) );
+	joint->deltaCenter = b2Sub( bodySimB->center, bodySimA->center );
+	joint->deltaAngle = b2RelativeAngle( qB, qA ) - joint->referenceAngle;
+	joint->deltaAngle = b2UnwindAngle( joint->deltaAngle );
+
+	float ka = iA + iB;
+	joint->axialMass = ka > 0.0f ? 1.0f / ka : 0.0f;
+
+	if ( joint->linearHertz == 0.0f )
+	{
+		joint->linearSoftness = context->jointSoftness;
+	}
+	else
+	{
+		joint->linearSoftness = b2MakeSoft( joint->linearHertz, joint->linearDampingRatio, context->h );
+	}
+
+	if ( joint->angularHertz == 0.0f )
+	{
+		joint->angularSoftness = context->jointSoftness;
+	}
+	else
+	{
+		joint->angularSoftness = b2MakeSoft( joint->angularHertz, joint->angularDampingRatio, context->h );
+	}
+
+	if ( context->enableWarmStarting == false )
+	{
+		joint->linearImpulse = b2Vec2_zero;
+		joint->angularImpulse = 0.0f;
+	}
+}
+
+void b2WarmStartWeldJoint( b2JointSim* base, b2StepContext* context )
+{
+	float mA = base->invMassA;
+	float mB = base->invMassB;
+	float iA = base->invIA;
+	float iB = base->invIB;
+
+	// dummy state for static bodies
+	b2BodyState dummyState = b2_identityBodyState;
+
+	b2WeldJoint* joint = &base->weldJoint;
+
+	b2BodyState* stateA = joint->indexA == B2_NULL_INDEX ? &dummyState : context->states + joint->indexA;
+	b2BodyState* stateB = joint->indexB == B2_NULL_INDEX ? &dummyState : context->states + joint->indexB;
+
+	b2Vec2 rA = b2RotateVector( stateA->deltaRotation, joint->anchorA );
+	b2Vec2 rB = b2RotateVector( stateB->deltaRotation, joint->anchorB );
+
+	stateA->linearVelocity = b2MulSub( stateA->linearVelocity, mA, joint->linearImpulse );
+	stateA->angularVelocity -= iA * ( b2Cross( rA, joint->linearImpulse ) + joint->angularImpulse );
+
+	stateB->linearVelocity = b2MulAdd( stateB->linearVelocity, mB, joint->linearImpulse );
+	stateB->angularVelocity += iB * ( b2Cross( rB, joint->linearImpulse ) + joint->angularImpulse );
+}
+
+void b2SolveWeldJoint( b2JointSim* base, b2StepContext* context, bool useBias )
+{
+	B2_ASSERT( base->type == b2_weldJoint );
+
+	float mA = base->invMassA;
+	float mB = base->invMassB;
+	float iA = base->invIA;
+	float iB = base->invIB;
+
+	// dummy state for static bodies
+	b2BodyState dummyState = b2_identityBodyState;
+
+	b2WeldJoint* joint = &base->weldJoint;
+
+	b2BodyState* stateA = joint->indexA == B2_NULL_INDEX ? &dummyState : context->states + joint->indexA;
+	b2BodyState* stateB = joint->indexB == B2_NULL_INDEX ? &dummyState : context->states + joint->indexB;
+
+	b2Vec2 vA = stateA->linearVelocity;
+	float wA = stateA->angularVelocity;
+	b2Vec2 vB = stateB->linearVelocity;
+	float wB = stateB->angularVelocity;
+
+	// angular constraint
+	{
+		float bias = 0.0f;
+		float massScale = 1.0f;
+		float impulseScale = 0.0f;
+		if ( useBias || joint->angularHertz > 0.0f )
+		{
+			float C = b2RelativeAngle( stateB->deltaRotation, stateA->deltaRotation ) + joint->deltaAngle;
+			bias = joint->angularSoftness.biasRate * C;
+			massScale = joint->angularSoftness.massScale;
+			impulseScale = joint->angularSoftness.impulseScale;
+		}
+
+		float Cdot = wB - wA;
+		float impulse = -massScale * joint->axialMass * ( Cdot + bias ) - impulseScale * joint->angularImpulse;
+		joint->angularImpulse += impulse;
+
+		wA -= iA * impulse;
+		wB += iB * impulse;
+	}
+
+	// linear constraint
+	{
+		b2Vec2 rA = b2RotateVector( stateA->deltaRotation, joint->anchorA );
+		b2Vec2 rB = b2RotateVector( stateB->deltaRotation, joint->anchorB );
+
+		b2Vec2 bias = b2Vec2_zero;
+		float massScale = 1.0f;
+		float impulseScale = 0.0f;
+		if ( useBias || joint->linearHertz > 0.0f )
+		{
+			b2Vec2 dcA = stateA->deltaPosition;
+			b2Vec2 dcB = stateB->deltaPosition;
+			b2Vec2 C = b2Add( b2Add( b2Sub( dcB, dcA ), b2Sub( rB, rA ) ), joint->deltaCenter );
+
+			bias = b2MulSV( joint->linearSoftness.biasRate, C );
+			massScale = joint->linearSoftness.massScale;
+			impulseScale = joint->linearSoftness.impulseScale;
+		}
+
+		b2Vec2 Cdot = b2Sub( b2Add( vB, b2CrossSV( wB, rB ) ), b2Add( vA, b2CrossSV( wA, rA ) ) );
+
+		b2Mat22 K;
+		K.cx.x = mA + mB + rA.y * rA.y * iA + rB.y * rB.y * iB;
+		K.cy.x = -rA.y * rA.x * iA - rB.y * rB.x * iB;
+		K.cx.y = K.cy.x;
+		K.cy.y = mA + mB + rA.x * rA.x * iA + rB.x * rB.x * iB;
+		b2Vec2 b = b2Solve22( K, b2Add( Cdot, bias ) );
+
+		b2Vec2 impulse = {
+			-massScale * b.x - impulseScale * joint->linearImpulse.x,
+			-massScale * b.y - impulseScale * joint->linearImpulse.y,
+		};
+
+		joint->linearImpulse = b2Add( joint->linearImpulse, impulse );
+
+		vA = b2MulSub( vA, mA, impulse );
+		wA -= iA * b2Cross( rA, impulse );
+		vB = b2MulAdd( vB, mB, impulse );
+		wB += iB * b2Cross( rB, impulse );
+	}
+
+	stateA->linearVelocity = vA;
+	stateA->angularVelocity = wA;
+	stateB->linearVelocity = vB;
+	stateB->angularVelocity = wB;
+}
+
+#if 0
+void b2DumpWeldJoint()
+{
+	int32 indexA = m_bodyA->m_islandIndex;
+	int32 indexB = m_bodyB->m_islandIndex;
+
+	b2Dump("  b2WeldJointDef jd;\n");
+	b2Dump("  jd.bodyA = sims[%d];\n", indexA);
+	b2Dump("  jd.bodyB = sims[%d];\n", indexB);
+	b2Dump("  jd.collideConnected = bool(%d);\n", m_collideConnected);
+	b2Dump("  jd.localAnchorA.Set(%.9g, %.9g);\n", m_localAnchorA.x, m_localAnchorA.y);
+	b2Dump("  jd.localAnchorB.Set(%.9g, %.9g);\n", m_localAnchorB.x, m_localAnchorB.y);
+	b2Dump("  jd.referenceAngle = %.9g;\n", m_referenceAngle);
+	b2Dump("  jd.stiffness = %.9g;\n", m_stiffness);
+	b2Dump("  jd.damping = %.9g;\n", m_damping);
+	b2Dump("  joints[%d] = m_world->CreateJoint(&jd);\n", m_index);
+}
+#endif

src/vendor/box2d/wheel_joint.c

@@ -0,0 +1,549 @@
+// SPDX-FileCopyrightText: 2023 Erin Catto
+// SPDX-License-Identifier: MIT
+
+#include "body.h"
+#include "core.h"
+#include "joint.h"
+#include "solver.h"
+#include "solver_set.h"
+#include "world.h"
+
+// needed for dll export
+#include "box2d/box2d.h"
+
+#include <stdio.h>
+
+void b2WheelJoint_EnableSpring( b2JointId jointId, bool enableSpring )
+{
+	b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_wheelJoint );
+
+	if ( enableSpring != joint->wheelJoint.enableSpring )
+	{
+		joint->wheelJoint.enableSpring = enableSpring;
+		joint->wheelJoint.springImpulse = 0.0f;
+	}
+}
+
+bool b2WheelJoint_IsSpringEnabled( b2JointId jointId )
+{
+	b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_wheelJoint );
+	return joint->wheelJoint.enableSpring;
+}
+
+void b2WheelJoint_SetSpringHertz( b2JointId jointId, float hertz )
+{
+	b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_wheelJoint );
+	joint->wheelJoint.hertz = hertz;
+}
+
+float b2WheelJoint_GetSpringHertz( b2JointId jointId )
+{
+	b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_wheelJoint );
+	return joint->wheelJoint.hertz;
+}
+
+void b2WheelJoint_SetSpringDampingRatio( b2JointId jointId, float dampingRatio )
+{
+	b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_wheelJoint );
+	joint->wheelJoint.dampingRatio = dampingRatio;
+}
+
+float b2WheelJoint_GetSpringDampingRatio( b2JointId jointId )
+{
+	b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_wheelJoint );
+	return joint->wheelJoint.dampingRatio;
+}
+
+void b2WheelJoint_EnableLimit( b2JointId jointId, bool enableLimit )
+{
+	b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_wheelJoint );
+	if ( joint->wheelJoint.enableLimit != enableLimit )
+	{
+		joint->wheelJoint.lowerImpulse = 0.0f;
+		joint->wheelJoint.upperImpulse = 0.0f;
+		joint->wheelJoint.enableLimit = enableLimit;
+	}
+}
+
+bool b2WheelJoint_IsLimitEnabled( b2JointId jointId )
+{
+	b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_wheelJoint );
+	return joint->wheelJoint.enableLimit;
+}
+
+float b2WheelJoint_GetLowerLimit( b2JointId jointId )
+{
+	b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_wheelJoint );
+	return joint->wheelJoint.lowerTranslation;
+}
+
+float b2WheelJoint_GetUpperLimit( b2JointId jointId )
+{
+	b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_wheelJoint );
+	return joint->wheelJoint.upperTranslation;
+}
+
+void b2WheelJoint_SetLimits( b2JointId jointId, float lower, float upper )
+{
+	b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_wheelJoint );
+	if ( lower != joint->wheelJoint.lowerTranslation || upper != joint->wheelJoint.upperTranslation )
+	{
+		joint->wheelJoint.lowerTranslation = b2MinFloat( lower, upper );
+		joint->wheelJoint.upperTranslation = b2MaxFloat( lower, upper );
+		joint->wheelJoint.lowerImpulse = 0.0f;
+		joint->wheelJoint.upperImpulse = 0.0f;
+	}
+}
+
+void b2WheelJoint_EnableMotor( b2JointId jointId, bool enableMotor )
+{
+	b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_wheelJoint );
+	if ( joint->wheelJoint.enableMotor != enableMotor )
+	{
+		joint->wheelJoint.motorImpulse = 0.0f;
+		joint->wheelJoint.enableMotor = enableMotor;
+	}
+}
+
+bool b2WheelJoint_IsMotorEnabled( b2JointId jointId )
+{
+	b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_wheelJoint );
+	return joint->wheelJoint.enableMotor;
+}
+
+void b2WheelJoint_SetMotorSpeed( b2JointId jointId, float motorSpeed )
+{
+	b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_wheelJoint );
+	joint->wheelJoint.motorSpeed = motorSpeed;
+}
+
+float b2WheelJoint_GetMotorSpeed( b2JointId jointId )
+{
+	b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_wheelJoint );
+	return joint->wheelJoint.motorSpeed;
+}
+
+float b2WheelJoint_GetMotorTorque( b2JointId jointId )
+{
+	b2World* world = b2GetWorld( jointId.world0 );
+	b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_wheelJoint );
+	return world->inv_h * joint->wheelJoint.motorImpulse;
+}
+
+void b2WheelJoint_SetMaxMotorTorque( b2JointId jointId, float torque )
+{
+	b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_wheelJoint );
+	joint->wheelJoint.maxMotorTorque = torque;
+}
+
+float b2WheelJoint_GetMaxMotorTorque( b2JointId jointId )
+{
+	b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_wheelJoint );
+	return joint->wheelJoint.maxMotorTorque;
+}
+
+b2Vec2 b2GetWheelJointForce( b2World* world, b2JointSim* base )
+{
+	b2WheelJoint* joint = &base->wheelJoint;
+
+	// This is a frame behind
+	b2Vec2 axisA = joint->axisA;
+	b2Vec2 perpA = b2LeftPerp( axisA );
+
+	float perpForce = world->inv_h * joint->perpImpulse;
+	float axialForce = world->inv_h * ( joint->springImpulse + joint->lowerImpulse - joint->upperImpulse );
+
+	b2Vec2 force = b2Add( b2MulSV( perpForce, perpA ), b2MulSV( axialForce, axisA ) );
+	return force;
+}
+
+float b2GetWheelJointTorque( b2World* world, b2JointSim* base )
+{
+	return world->inv_h * base->wheelJoint.motorImpulse;
+}
+
+// Linear constraint (point-to-line)
+// d = pB - pA = xB + rB - xA - rA
+// C = dot(ay, d)
+// Cdot = dot(d, cross(wA, ay)) + dot(ay, vB + cross(wB, rB) - vA - cross(wA, rA))
+//      = -dot(ay, vA) - dot(cross(d + rA, ay), wA) + dot(ay, vB) + dot(cross(rB, ay), vB)
+// J = [-ay, -cross(d + rA, ay), ay, cross(rB, ay)]
+
+// Spring linear constraint
+// C = dot(ax, d)
+// Cdot = = -dot(ax, vA) - dot(cross(d + rA, ax), wA) + dot(ax, vB) + dot(cross(rB, ax), vB)
+// J = [-ax -cross(d+rA, ax) ax cross(rB, ax)]
+
+// Motor rotational constraint
+// Cdot = wB - wA
+// J = [0 0 -1 0 0 1]
+
+void b2PrepareWheelJoint( b2JointSim* base, b2StepContext* context )
+{
+	B2_ASSERT( base->type == b2_wheelJoint );
+
+	// chase body id to the solver set where the body lives
+	int idA = base->bodyIdA;
+	int idB = base->bodyIdB;
+
+	b2World* world = context->world;
+
+	b2Body* bodyA = b2BodyArray_Get( &world->bodies, idA );
+	b2Body* bodyB = b2BodyArray_Get( &world->bodies, idB );
+
+	B2_ASSERT( bodyA->setIndex == b2_awakeSet || bodyB->setIndex == b2_awakeSet );
+	b2SolverSet* setA = b2SolverSetArray_Get( &world->solverSets, bodyA->setIndex );
+	b2SolverSet* setB = b2SolverSetArray_Get( &world->solverSets, bodyB->setIndex );
+
+	int localIndexA = bodyA->localIndex;
+	int localIndexB = bodyB->localIndex;
+
+	b2BodySim* bodySimA = b2BodySimArray_Get( &setA->bodySims, localIndexA );
+	b2BodySim* bodySimB = b2BodySimArray_Get( &setB->bodySims, localIndexB );
+
+	float mA = bodySimA->invMass;
+	float iA = bodySimA->invInertia;
+	float mB = bodySimB->invMass;
+	float iB = bodySimB->invInertia;
+
+	base->invMassA = mA;
+	base->invMassB = mB;
+	base->invIA = iA;
+	base->invIB = iB;
+
+	b2WheelJoint* joint = &base->wheelJoint;
+
+	joint->indexA = bodyA->setIndex == b2_awakeSet ? localIndexA : B2_NULL_INDEX;
+	joint->indexB = bodyB->setIndex == b2_awakeSet ? localIndexB : B2_NULL_INDEX;
+
+	b2Rot qA = bodySimA->transform.q;
+	b2Rot qB = bodySimB->transform.q;
+
+	joint->anchorA = b2RotateVector( qA, b2Sub( base->localOriginAnchorA, bodySimA->localCenter ) );
+	joint->anchorB = b2RotateVector( qB, b2Sub( base->localOriginAnchorB, bodySimB->localCenter ) );
+	joint->axisA = b2RotateVector( qA, joint->localAxisA );
+	joint->deltaCenter = b2Sub( bodySimB->center, bodySimA->center );
+
+	b2Vec2 rA = joint->anchorA;
+	b2Vec2 rB = joint->anchorB;
+
+	b2Vec2 d = b2Add( joint->deltaCenter, b2Sub( rB, rA ) );
+	b2Vec2 axisA = joint->axisA;
+	b2Vec2 perpA = b2LeftPerp( axisA );
+
+	// perpendicular constraint (keep wheel on line)
+	float s1 = b2Cross( b2Add( d, rA ), perpA );
+	float s2 = b2Cross( rB, perpA );
+
+	float kp = mA + mB + iA * s1 * s1 + iB * s2 * s2;
+	joint->perpMass = kp > 0.0f ? 1.0f / kp : 0.0f;
+
+	// spring constraint
+	float a1 = b2Cross( b2Add( d, rA ), axisA );
+	float a2 = b2Cross( rB, axisA );
+
+	float ka = mA + mB + iA * a1 * a1 + iB * a2 * a2;
+	joint->axialMass = ka > 0.0f ? 1.0f / ka : 0.0f;
+
+	joint->springSoftness = b2MakeSoft( joint->hertz, joint->dampingRatio, context->h );
+
+	float km = iA + iB;
+	joint->motorMass = km > 0.0f ? 1.0f / km : 0.0f;
+
+	if ( context->enableWarmStarting == false )
+	{
+		joint->perpImpulse = 0.0f;
+		joint->springImpulse = 0.0f;
+		joint->motorImpulse = 0.0f;
+		joint->lowerImpulse = 0.0f;
+		joint->upperImpulse = 0.0f;
+	}
+}
+
+void b2WarmStartWheelJoint( b2JointSim* base, b2StepContext* context )
+{
+	B2_ASSERT( base->type == b2_wheelJoint );
+
+	float mA = base->invMassA;
+	float mB = base->invMassB;
+	float iA = base->invIA;
+	float iB = base->invIB;
+
+	// dummy state for static bodies
+	b2BodyState dummyState = b2_identityBodyState;
+
+	b2WheelJoint* joint = &base->wheelJoint;
+
+	b2BodyState* stateA = joint->indexA == B2_NULL_INDEX ? &dummyState : context->states + joint->indexA;
+	b2BodyState* stateB = joint->indexB == B2_NULL_INDEX ? &dummyState : context->states + joint->indexB;
+
+	b2Vec2 rA = b2RotateVector( stateA->deltaRotation, joint->anchorA );
+	b2Vec2 rB = b2RotateVector( stateB->deltaRotation, joint->anchorB );
+
+	b2Vec2 d = b2Add( b2Add( b2Sub( stateB->deltaPosition, stateA->deltaPosition ), joint->deltaCenter ), b2Sub( rB, rA ) );
+	b2Vec2 axisA = b2RotateVector( stateA->deltaRotation, joint->axisA );
+	b2Vec2 perpA = b2LeftPerp( axisA );
+
+	float a1 = b2Cross( b2Add( d, rA ), axisA );
+	float a2 = b2Cross( rB, axisA );
+	float s1 = b2Cross( b2Add( d, rA ), perpA );
+	float s2 = b2Cross( rB, perpA );
+
+	float axialImpulse = joint->springImpulse + joint->lowerImpulse - joint->upperImpulse;
+
+	b2Vec2 P = b2Add( b2MulSV( axialImpulse, axisA ), b2MulSV( joint->perpImpulse, perpA ) );
+	float LA = axialImpulse * a1 + joint->perpImpulse * s1 + joint->motorImpulse;
+	float LB = axialImpulse * a2 + joint->perpImpulse * s2 + joint->motorImpulse;
+
+	stateA->linearVelocity = b2MulSub( stateA->linearVelocity, mA, P );
+	stateA->angularVelocity -= iA * LA;
+	stateB->linearVelocity = b2MulAdd( stateB->linearVelocity, mB, P );
+	stateB->angularVelocity += iB * LB;
+}
+
+void b2SolveWheelJoint( b2JointSim* base, b2StepContext* context, bool useBias )
+{
+	B2_ASSERT( base->type == b2_wheelJoint );
+
+	float mA = base->invMassA;
+	float mB = base->invMassB;
+	float iA = base->invIA;
+	float iB = base->invIB;
+
+	// dummy state for static bodies
+	b2BodyState dummyState = b2_identityBodyState;
+
+	b2WheelJoint* joint = &base->wheelJoint;
+
+	b2BodyState* stateA = joint->indexA == B2_NULL_INDEX ? &dummyState : context->states + joint->indexA;
+	b2BodyState* stateB = joint->indexB == B2_NULL_INDEX ? &dummyState : context->states + joint->indexB;
+
+	b2Vec2 vA = stateA->linearVelocity;
+	float wA = stateA->angularVelocity;
+	b2Vec2 vB = stateB->linearVelocity;
+	float wB = stateB->angularVelocity;
+
+	bool fixedRotation = ( iA + iB == 0.0f );
+
+	// current anchors
+	b2Vec2 rA = b2RotateVector( stateA->deltaRotation, joint->anchorA );
+	b2Vec2 rB = b2RotateVector( stateB->deltaRotation, joint->anchorB );
+
+	b2Vec2 d = b2Add( b2Add( b2Sub( stateB->deltaPosition, stateA->deltaPosition ), joint->deltaCenter ), b2Sub( rB, rA ) );
+	b2Vec2 axisA = b2RotateVector( stateA->deltaRotation, joint->axisA );
+	float translation = b2Dot( axisA, d );
+
+	float a1 = b2Cross( b2Add( d, rA ), axisA );
+	float a2 = b2Cross( rB, axisA );
+
+	// motor constraint
+	if ( joint->enableMotor && fixedRotation == false )
+	{
+		float Cdot = wB - wA - joint->motorSpeed;
+		float impulse = -joint->motorMass * Cdot;
+		float oldImpulse = joint->motorImpulse;
+		float maxImpulse = context->h * joint->maxMotorTorque;
+		joint->motorImpulse = b2ClampFloat( joint->motorImpulse + impulse, -maxImpulse, maxImpulse );
+		impulse = joint->motorImpulse - oldImpulse;
+
+		wA -= iA * impulse;
+		wB += iB * impulse;
+	}
+
+	// spring constraint
+	if ( joint->enableSpring )
+	{
+		// This is a real spring and should be applied even during relax
+		float C = translation;
+		float bias = joint->springSoftness.biasRate * C;
+		float massScale = joint->springSoftness.massScale;
+		float impulseScale = joint->springSoftness.impulseScale;
+
+		float Cdot = b2Dot( axisA, b2Sub( vB, vA ) ) + a2 * wB - a1 * wA;
+		float impulse = -massScale * joint->axialMass * ( Cdot + bias ) - impulseScale * joint->springImpulse;
+		joint->springImpulse += impulse;
+
+		b2Vec2 P = b2MulSV( impulse, axisA );
+		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 )
+	{
+		// 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_h;
+			}
+			else if ( useBias )
+			{
+				bias = context->jointSoftness.biasRate * C;
+				massScale = context->jointSoftness.massScale;
+				impulseScale = context->jointSoftness.impulseScale;
+			}
+
+			float Cdot = b2Dot( axisA, b2Sub( vB, vA ) ) + a2 * wB - a1 * wA;
+			float impulse = -massScale * joint->axialMass * ( Cdot + bias ) - impulseScale * joint->lowerImpulse;
+			float oldImpulse = joint->lowerImpulse;
+			joint->lowerImpulse = b2MaxFloat( oldImpulse + impulse, 0.0f );
+			impulse = joint->lowerImpulse - oldImpulse;
+
+			b2Vec2 P = b2MulSV( impulse, axisA );
+			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_h;
+			}
+			else if ( useBias )
+			{
+				bias = context->jointSoftness.biasRate * C;
+				massScale = context->jointSoftness.massScale;
+				impulseScale = context->jointSoftness.impulseScale;
+			}
+
+			// sign flipped on Cdot
+			float Cdot = b2Dot( axisA, b2Sub( vA, vB ) ) + a1 * wA - a2 * wB;
+			float impulse = -massScale * joint->axialMass * ( Cdot + bias ) - impulseScale * joint->upperImpulse;
+			float oldImpulse = joint->upperImpulse;
+			joint->upperImpulse = b2MaxFloat( oldImpulse + impulse, 0.0f );
+			impulse = joint->upperImpulse - oldImpulse;
+
+			b2Vec2 P = b2MulSV( impulse, axisA );
+			float LA = impulse * a1;
+			float LB = impulse * a2;
+
+			// sign flipped on applied impulse
+			vA = b2MulAdd( vA, mA, P );
+			wA += iA * LA;
+			vB = b2MulSub( vB, mB, P );
+			wB -= iB * LB;
+		}
+	}
+
+	// point to line constraint
+	{
+		b2Vec2 perpA = b2LeftPerp( axisA );
+
+		float bias = 0.0f;
+		float massScale = 1.0f;
+		float impulseScale = 0.0f;
+		if ( useBias )
+		{
+			float C = b2Dot( perpA, d );
+			bias = context->jointSoftness.biasRate * C;
+			massScale = context->jointSoftness.massScale;
+			impulseScale = context->jointSoftness.impulseScale;
+		}
+
+		float s1 = b2Cross( b2Add( d, rA ), perpA );
+		float s2 = b2Cross( rB, perpA );
+		float Cdot = b2Dot( perpA, b2Sub( vB, vA ) ) + s2 * wB - s1 * wA;
+
+		float impulse = -massScale * joint->perpMass * ( Cdot + bias ) - impulseScale * joint->perpImpulse;
+		joint->perpImpulse += impulse;
+
+		b2Vec2 P = b2MulSV( impulse, perpA );
+		float LA = impulse * s1;
+		float LB = impulse * s2;
+
+		vA = b2MulSub( vA, mA, P );
+		wA -= iA * LA;
+		vB = b2MulAdd( vB, mB, P );
+		wB += iB * LB;
+	}
+
+	stateA->linearVelocity = vA;
+	stateA->angularVelocity = wA;
+	stateB->linearVelocity = vB;
+	stateB->angularVelocity = wB;
+}
+
+#if 0
+void b2WheelJoint_Dump()
+{
+	int32 indexA = joint->bodyA->joint->islandIndex;
+	int32 indexB = joint->bodyB->joint->islandIndex;
+
+	b2Dump("  b2WheelJointDef jd;\n");
+	b2Dump("  jd.bodyA = sims[%d];\n", indexA);
+	b2Dump("  jd.bodyB = sims[%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 b2DrawWheelJoint( b2DebugDraw* draw, b2JointSim* base, b2Transform transformA, b2Transform transformB )
+{
+	B2_ASSERT( base->type == b2_wheelJoint );
+
+	b2WheelJoint* joint = &base->wheelJoint;
+
+	b2Vec2 pA = b2TransformPoint( transformA, base->localOriginAnchorA );
+	b2Vec2 pB = b2TransformPoint( transformB, base->localOriginAnchorB );
+	b2Vec2 axis = b2RotateVector( transformA.q, joint->localAxisA );
+
+	b2HexColor c1 = b2_colorGray;
+	b2HexColor c2 = b2_colorGreen;
+	b2HexColor c3 = b2_colorRed;
+	b2HexColor c4 = b2_colorDimGray;
+	b2HexColor c5 = b2_colorBlue;
+
+	draw->DrawSegmentFcn( 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->DrawSegmentFcn( lower, upper, c1, draw->context );
+		draw->DrawSegmentFcn( b2MulSub( lower, 0.1f, perp ), b2MulAdd( lower, 0.1f, perp ), c2, draw->context );
+		draw->DrawSegmentFcn( b2MulSub( upper, 0.1f, perp ), b2MulAdd( upper, 0.1f, perp ), c3, draw->context );
+	}
+	else
+	{
+		draw->DrawSegmentFcn( b2MulSub( pA, 1.0f, axis ), b2MulAdd( pA, 1.0f, axis ), c1, draw->context );
+	}
+
+	draw->DrawPointFcn( pA, 5.0f, c1, draw->context );
+	draw->DrawPointFcn( pB, 5.0f, c4, draw->context );
+}

src/vendor/box2d/world.h

@@ -0,0 +1,192 @@
+// SPDX-FileCopyrightText: 2023 Erin Catto
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include "array.h"
+#include "bitset.h"
+#include "broad_phase.h"
+#include "constraint_graph.h"
+#include "id_pool.h"
+#include "arena_allocator.h"
+
+#include "box2d/types.h"
+
+enum b2SetType
+{
+	b2_staticSet = 0,
+	b2_disabledSet = 1,
+	b2_awakeSet = 2,
+	b2_firstSleepingSet = 3,
+};
+
+// Per thread task storage
+typedef struct b2TaskContext
+{
+	// These bits align with the b2ConstraintGraph::contactBlocks and signal a change in contact status
+	b2BitSet contactStateBitSet;
+
+	// Used to track bodies with shapes that have enlarged AABBs. This avoids having a bit array
+	// that is very large when there are many static shapes.
+	b2BitSet enlargedSimBitSet;
+
+	// Used to put islands to sleep
+	b2BitSet awakeIslandBitSet;
+
+	// Per worker split island candidate
+	float splitSleepTime;
+	int splitIslandId;
+
+} b2TaskContext;
+
+// The world struct manages all physics entities, dynamic simulation,  and asynchronous queries.
+// The world also contains efficient memory management facilities.
+typedef struct b2World
+{
+	b2ArenaAllocator arena;
+	b2BroadPhase broadPhase;
+	b2ConstraintGraph constraintGraph;
+
+	// The body id pool is used to allocate and recycle body ids. Body ids
+	// provide a stable identifier for users, but incur caches misses when used
+	// to access body data. Aligns with b2Body.
+	b2IdPool bodyIdPool;
+
+	// This is a sparse array that maps body ids to the body data
+	// stored in solver sets. As sims move within a set or across set.
+	// Indices come from id pool.
+	b2BodyArray bodies;
+
+	// Provides free list for solver sets.
+	b2IdPool solverSetIdPool;
+
+	// Solvers sets allow sims to be stored in contiguous arrays. The first
+	// set is all static sims. The second set is active sims. The third set is disabled
+	// sims. The remaining sets are sleeping islands.
+	b2SolverSetArray solverSets;
+
+	// Used to create stable ids for joints
+	b2IdPool jointIdPool;
+
+	// This is a sparse array that maps joint ids to the joint data stored in the constraint graph
+	// or in the solver sets.
+	b2JointArray joints;
+
+	// Used to create stable ids for contacts
+	b2IdPool contactIdPool;
+
+	// This is a sparse array that maps contact ids to the contact data stored in the constraint graph
+	// or in the solver sets.
+	b2ContactArray contacts;
+
+	// Used to create stable ids for islands
+	b2IdPool islandIdPool;
+
+	// This is a sparse array that maps island ids to the island data stored in the solver sets.
+	b2IslandArray islands;
+
+	b2IdPool shapeIdPool;
+	b2IdPool chainIdPool;
+
+	// These are sparse arrays that point into the pools above
+	b2ShapeArray shapes;
+	b2ChainShapeArray chainShapes;
+
+	// This is a dense array of sensor data.
+	b2SensorArray sensors;
+
+	// Per thread storage
+	b2TaskContextArray taskContexts;
+	b2SensorTaskContextArray sensorTaskContexts;
+
+	b2BodyMoveEventArray bodyMoveEvents;
+	b2SensorBeginTouchEventArray sensorBeginEvents;
+	b2ContactBeginTouchEventArray contactBeginEvents;
+
+	// End events are double buffered so that the user doesn't need to flush events
+	b2SensorEndTouchEventArray sensorEndEvents[2];
+	b2ContactEndTouchEventArray contactEndEvents[2];
+	int endEventArrayIndex;
+
+	b2ContactHitEventArray contactHitEvents;
+
+	// Used to track debug draw
+	b2BitSet debugBodySet;
+	b2BitSet debugJointSet;
+	b2BitSet debugContactSet;
+	b2BitSet debugIslandSet;
+
+	// Id that is incremented every time step
+	uint64_t stepIndex;
+
+	// Identify islands for splitting as follows:
+	// - I want to split islands so smaller islands can sleep
+	// - when a body comes to rest and its sleep timer trips, I can look at the island and flag it for splitting
+	//   if it has removed constraints
+	// - islands that have removed constraints must be put split first because I don't want to wake bodies incorrectly
+	// - otherwise I can use the awake islands that have bodies wanting to sleep as the splitting candidates
+	// - if no bodies want to sleep then there is no reason to perform island splitting
+	int splitIslandId;
+
+	b2Vec2 gravity;
+	float hitEventThreshold;
+	float restitutionThreshold;
+	float maxLinearSpeed;
+	float maxContactPushSpeed;
+	float contactHertz;
+	float contactDampingRatio;
+	float jointHertz;
+	float jointDampingRatio;
+
+	b2FrictionCallback* frictionCallback;
+	b2RestitutionCallback* restitutionCallback;
+
+	uint16_t generation;
+
+	b2Profile profile;
+
+	b2PreSolveFcn* preSolveFcn;
+	void* preSolveContext;
+
+	b2CustomFilterFcn* customFilterFcn;
+	void* customFilterContext;
+
+	int workerCount;
+	b2EnqueueTaskCallback* enqueueTaskFcn;
+	b2FinishTaskCallback* finishTaskFcn;
+	void* userTaskContext;
+	void* userTreeTask;
+
+	void* userData;
+
+	// Remember type step used for reporting forces and torques
+	float inv_h;
+
+	int activeTaskCount;
+	int taskCount;
+
+	uint16_t worldId;
+
+	bool enableSleep;
+	bool locked;
+	bool enableWarmStarting;
+	bool enableContinuous;
+	bool enableSpeculative;
+	bool inUse;
+} b2World;
+
+b2World* b2GetWorldFromId( b2WorldId id );
+b2World* b2GetWorld( int index );
+b2World* b2GetWorldLocked( int index );
+
+void b2ValidateConnectivity( b2World* world );
+void b2ValidateSolverSets( b2World* world );
+void b2ValidateContacts( b2World* world );
+
+B2_ARRAY_INLINE( b2BodyMoveEvent, b2BodyMoveEvent )
+B2_ARRAY_INLINE( b2ContactBeginTouchEvent, b2ContactBeginTouchEvent )
+B2_ARRAY_INLINE( b2ContactEndTouchEvent, b2ContactEndTouchEvent )
+B2_ARRAY_INLINE( b2ContactHitEvent, b2ContactHitEvent )
+B2_ARRAY_INLINE( b2SensorBeginTouchEvent, b2SensorBeginTouchEvent )
+B2_ARRAY_INLINE( b2SensorEndTouchEvent, b2SensorEndTouchEvent )
+B2_ARRAY_INLINE( b2TaskContext, b2TaskContext )