Add box3d src to project; Add simple build script for Windows

vendor/box3d/src/build.bat

@@ -0,0 +1,8 @@
+@echo off
+
+if not exist "..\lib" mkdir ..\lib
+
+cl -nologo -MT -TC -O2 -c -std:c17 -I "include" src\*.c
+lib -nologo *.obj -out:..\lib\box3d.lib
+
+del *.obj

vendor/box3d/src/src/CMakeLists.txt

@@ -0,0 +1,256 @@
+include(GNUInstallDirs)
+
+set(BOX3D_SOURCE_FILES
+	aabb.c
+	aabb.h
+	algorithm.h
+	arena_allocator.c
+	arena_allocator.h
+	bitset.c
+	bitset.h
+	block_allocator.c
+	block_allocator.h
+	body.c
+	body.h
+	broad_phase.c
+	broad_phase.h
+	capsule.c
+	compound.c
+	compound.h
+	constraint_graph.c
+	constraint_graph.h
+	contact.c
+	contact.h
+	contact_solver.c
+	contact_solver.h
+	container.h
+	convex_manifold.c
+	core.c
+	core.h
+	ctz.h
+	distance.c
+	distance_joint.c
+	dynamic_tree.c
+	height_field.c
+	hull.c
+	id_pool.c
+	id_pool.h
+	island.c
+	island.h
+	joint.c
+	joint.h
+	manifold.c
+	manifold.h
+	math_functions.c
+	math_internal.h
+	mesh.c
+	mesh_contact.c
+	motor_joint.c
+	mover.c
+	name_cache.c
+	name_cache.h
+	parallel_for.c
+	parallel_for.h
+	parallel_joint.c
+	physics_world.c
+	physics_world.h
+	platform.h
+	prismatic_joint.c
+	qsort.h
+	recording.c
+	recording.h
+	recording_ops.inl
+	recording_replay.c
+	recording_replay.h
+	world_snapshot.c
+	world_snapshot.h
+	revolute_joint.c
+	scheduler.c
+	scheduler.h
+	sensor.c
+	sensor.h
+	shape.c
+	shape.h
+	simd.c
+	simd.h
+	solver.c
+	solver.h
+	solver_set.c
+	solver_set.h
+	sphere.c
+	spherical_joint.c
+	table.c
+	table.h
+	timer.c
+	triangle_manifold.c
+	types.c
+	verstable.h
+	weld_joint.c
+	wheel_joint.c
+
+	box3d.natvis
+)
+
+set(BOX3D_INCLUDE_FILES
+	../include/box3d/base.h
+	../include/box3d/box3d.h
+	../include/box3d/collision.h
+	../include/box3d/config.h
+	../include/box3d/constants.h
+	../include/box3d/id.h
+	../include/box3d/math_functions.h
+	../include/box3d/types.h
+)
+
+# Hide internal functions
+# https://gcc.gnu.org/wiki/Visibility
+set(CMAKE_C_VISIBILITY_PRESET hidden)
+set(CMAKE_VISIBILITY_INLINES_HIDDEN 1)
+
+add_library(box3d ${BOX3D_SOURCE_FILES} ${BOX3D_INCLUDE_FILES})
+
+# target_link_libraries(box3d PRIVATE eastl)
+
+target_include_directories(box3d
+  PUBLIC
+    $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/../include>
+    $<BUILD_INTERFACE:${CMAKE_CURRENT_BINARY_DIR}>
+    $<INSTALL_INTERFACE:${CMAKE_INSTALL_INCLUDEDIR}>
+  PRIVATE
+    ${CMAKE_CURRENT_SOURCE_DIR}
+)
+
+set(CMAKE_DEBUG_POSTFIX "d")
+
+# Box3D uses C17 for _Static_assert and anonymous unions
+set_target_properties(box3d PROPERTIES
+	C_STANDARD 17
+    C_STANDARD_REQUIRED YES
+    C_EXTENSIONS YES
+    VERSION ${PROJECT_VERSION}
+    SOVERSION ${PROJECT_VERSION_MAJOR}
+	DEBUG_POSTFIX ${CMAKE_DEBUG_POSTFIX}
+)
+
+if (BOX3D_COMPILE_WARNING_AS_ERROR)
+	set_target_properties(box3d PROPERTIES COMPILE_WARNING_AS_ERROR ON)
+endif()
+
+if (BOX3D_PROFILE)
+	target_compile_definitions(box3d PRIVATE BOX3D_PROFILE)
+   	set(TRACY_DELAYED_INIT ON CACHE BOOL "Enable delayed init for Tracy" FORCE)
+    set(TRACY_MANUAL_LIFETIME ON CACHE BOOL "Enable manual lifetime for Tracy" FORCE)
+
+	FetchContent_Declare(
+		tracy
+		GIT_REPOSITORY https://github.com/wolfpld/tracy.git
+        GIT_TAG v0.13.1
+		GIT_SHALLOW TRUE
+		GIT_PROGRESS TRUE
+	)
+	FetchContent_MakeAvailable(tracy)
+
+	target_link_libraries(box3d PUBLIC TracyClient)
+endif()
+
+if(BOX3D_VALIDATE)
+	message(STATUS "Box3D validation ON")	
+	target_compile_definitions(box3d PRIVATE BOX3D_VALIDATE)
+endif()
+
+if (BOX3D_DISABLE_SIMD)
+	message(STATUS "Box3D SIMD disabled")
+	target_compile_definitions(box3d PRIVATE BOX3D_DISABLE_SIMD)
+endif()
+
+if (BOX3D_DOUBLE_PRECISION)
+	message(STATUS "Box3D double precision ON")
+	# PUBLIC: consumers must see the same precision mode in the headers or they cannot match the ABI
+	target_compile_definitions(box3d PUBLIC BOX3D_DOUBLE_PRECISION)
+endif()
+
+if (MSVC)
+	message(STATUS "Box3D on MSVC")	
+	if (BUILD_SHARED_LIBS)
+		# this is needed by DLL users to import Box3D symbols
+		target_compile_definitions(box3d INTERFACE BOX3D_DLL)
+	endif()
+
+	# Visual Studio won't load the natvis unless it is in the project
+	target_sources(box3d PRIVATE box3d.natvis)
+
+	# Enable asserts in release with debug info
+	target_compile_definitions(box3d PUBLIC "$<$<CONFIG:RELWITHDEBINFO>:B3_ENABLE_ASSERT>")
+
+	# Warnings Wall is problematic in mixed C/C++, so using W4
+	target_compile_options(box3d PRIVATE /W4)
+	# target_compile_options(box3d PRIVATE /wd4820 /wd5045 /wd4061 /wd4711 /wd4514 /wd4365 /wd5219 /wd5039)
+	target_compile_options(box3d PRIVATE /wd4820 /wd5045 /wd4061 /wd4711)
+	# 4710 - warn about inline functions that are not inlined
+	target_compile_options(box3d PRIVATE /wd4710 )
+
+	if (${CMAKE_CXX_COMPILER_ID} STREQUAL "Clang")
+		target_compile_options(box3d PRIVATE -Wmissing-prototypes)
+	endif()
+
+elseif (MINGW)
+	message(STATUS "Box3D on MinGW")
+elseif (APPLE)
+	message(STATUS "Box3D on Apple")
+	target_compile_options(box3d PRIVATE -Wmissing-prototypes -Wall -Wextra -pedantic)
+elseif (EMSCRIPTEN)
+	message(STATUS "Box3D on Emscripten")
+	# SSE2/wasm SIMD flags are set directory-wide in the top-level CMakeLists so the
+	# tests, which include simd.h, get them too.
+elseif (UNIX)
+	message(STATUS "Box3D using Unix")
+	target_compile_options(box3d PRIVATE -Wmissing-prototypes -Wall -Wextra -pedantic -Wno-unused-value)
+	if ("${CMAKE_HOST_SYSTEM_PROCESSOR}" STREQUAL "aarch64")
+		# raspberry pi
+		# -mfpu=neon
+		# target_compile_options(box3d PRIVATE)
+	else()
+	endif()
+endif()
+
+source_group(TREE "${CMAKE_CURRENT_SOURCE_DIR}" PREFIX "src" FILES ${BOX3D_SOURCE_FILES})
+source_group(TREE "${CMAKE_CURRENT_SOURCE_DIR}/../include" PREFIX "include" FILES ${BOX3D_INCLUDE_FILES})
+
+add_library(box3d::box3d ALIAS box3d)
+
+# libm is required on many Unix toolchains for sqrtf, etc.
+if(UNIX AND NOT APPLE AND NOT EMSCRIPTEN)
+  target_link_libraries(box3d PUBLIC m)
+endif()
+
+install(
+  TARGETS box3d
+  EXPORT box3dConfig
+  LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR}
+  ARCHIVE DESTINATION ${CMAKE_INSTALL_LIBDIR}
+  RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR}
+)
+
+install(
+  EXPORT box3dConfig
+  NAMESPACE box3d::
+  DESTINATION "${CMAKE_INSTALL_LIBDIR}/cmake/box3d"
+)
+
+install(
+  FILES ${BOX3D_INCLUDE_FILES}
+  DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/box3d
+)
+
+include(CMakePackageConfigHelpers)
+
+write_basic_package_version_file(
+  "${CMAKE_CURRENT_BINARY_DIR}/box3dConfigVersion.cmake"
+  VERSION ${PROJECT_VERSION}
+  COMPATIBILITY SameMajorVersion
+)
+
+install(
+  FILES "${CMAKE_CURRENT_BINARY_DIR}/box3dConfigVersion.cmake"
+  DESTINATION "${CMAKE_INSTALL_LIBDIR}/cmake/box3d"
+)

vendor/box3d/src/src/aabb.c

@@ -0,0 +1,176 @@
+// SPDX-FileCopyrightText: 2025 Erin Catto
+// SPDX-License-Identifier: MIT
+
+#include "aabb.h"
+
+#include "math_internal.h"
+
+#include "box3d/math_functions.h"
+
+#include <float.h>
+
+// Similar to Real-time Collision Detection, p179.
+// todo try
+// https://www.scratchapixel.com/lessons/3d-basic-rendering/minimal-ray-tracer-rendering-simple-shapes/ray-box-intersection.html
+bool b3RayCastAABB( b3AABB a, b3Vec3 p1, b3Vec3 p2, float* minFraction, float* maxFraction )
+{
+	// Ray direction and length
+	b3Vec3 d = b3Sub( p2, p1 );
+	float rayLength = b3Length( d );
+
+	// Handle degenerate ray
+	if ( rayLength < FLT_EPSILON )
+	{
+		// Check if point is inside AABB
+		if ( p1.x >= a.lowerBound.x && p1.x <= a.upperBound.x && p1.y >= a.lowerBound.y && p1.y <= a.upperBound.y &&
+			 p1.z >= a.lowerBound.z && p1.z <= a.upperBound.z )
+		{
+			*minFraction = 0.0f;
+			*maxFraction = 0.0f;
+			return true;
+		}
+
+		return false;
+	}
+
+	b3Vec3 rayDir = b3MulSV( 1.0f / rayLength, d );
+
+	// Slab method for ray-AABB intersection
+	float tMin = 0.0f;
+	float tMax = rayLength;
+
+	// x-axis
+	{
+		float rayComponent = rayDir.x;
+		float rayStart = p1.x;
+		float boxMin = a.lowerBound.x;
+		float boxMax = a.upperBound.x;
+
+		if ( b3AbsFloat( rayComponent ) < FLT_EPSILON )
+		{
+			// Ray is parallel to slab, check if ray origin is within slab
+			if ( rayStart < boxMin || rayStart > boxMax )
+			{
+				return false;
+			}
+		}
+		else
+		{
+			// Compute intersection distances
+			float t1 = ( boxMin - rayStart ) / rayComponent;
+			float t2 = ( boxMax - rayStart ) / rayComponent;
+
+			// Ensure t1 <= t2
+			if ( t1 > t2 )
+			{
+				float temp = t1;
+				t1 = t2;
+				t2 = temp;
+			}
+
+			// Update intersection interval
+			tMin = b3MaxFloat( tMin, t1 );
+			tMax = b3MinFloat( tMax, t2 );
+
+			// Check for no intersection
+			if ( tMin > tMax )
+			{
+				return false;
+			}
+		}
+	}
+	
+	// y-axis
+	{
+		float rayComponent = rayDir.y;
+		float rayStart = p1.y;
+		float boxMin = a.lowerBound.y;
+		float boxMax = a.upperBound.y;
+
+		if ( b3AbsFloat( rayComponent ) < FLT_EPSILON )
+		{
+			// Ray is parallel to slab, check if ray origin is within slab
+			if ( rayStart < boxMin || rayStart > boxMax )
+			{
+				return false;
+			}
+		}
+		else
+		{
+			// Compute intersection distances
+			float t1 = ( boxMin - rayStart ) / rayComponent;
+			float t2 = ( boxMax - rayStart ) / rayComponent;
+
+			// Ensure t1 <= t2
+			if ( t1 > t2 )
+			{
+				float temp = t1;
+				t1 = t2;
+				t2 = temp;
+			}
+
+			// Update intersection interval
+			tMin = b3MaxFloat( tMin, t1 );
+			tMax = b3MinFloat( tMax, t2 );
+
+			// Check for no intersection
+			if ( tMin > tMax )
+			{
+				return false;
+			}
+		}
+	}
+
+	// z-axis
+	{
+		float rayComponent = rayDir.z;
+		float rayStart = p1.z;
+		float boxMin = a.lowerBound.z;
+		float boxMax = a.upperBound.z;
+
+		if ( b3AbsFloat( rayComponent ) < FLT_EPSILON )
+		{
+			// Ray is parallel to slab, check if ray origin is within slab
+			if ( rayStart < boxMin || rayStart > boxMax )
+			{
+				return false;
+			}
+		}
+		else
+		{
+			// Compute intersection distances
+			float t1 = ( boxMin - rayStart ) / rayComponent;
+			float t2 = ( boxMax - rayStart ) / rayComponent;
+
+			// Ensure t1 <= t2
+			if ( t1 > t2 )
+			{
+				float temp = t1;
+				t1 = t2;
+				t2 = temp;
+			}
+
+			// Update intersection interval
+			tMin = b3MaxFloat( tMin, t1 );
+			tMax = b3MinFloat( tMax, t2 );
+
+			// Check for no intersection
+			if ( tMin > tMax )
+			{
+				return false;
+			}
+		}
+	}
+
+	// Check if intersection is behind ray start
+	if ( tMax < 0.0f )
+	{
+		return false;
+	}
+
+	// Convert distances to fractions
+	*minFraction = b3ClampFloat( tMin / rayLength, 0.0f, 1.0f );
+	*maxFraction = b3ClampFloat( tMax / rayLength, 0.0f, 1.0f );
+
+	return true;
+}

vendor/box3d/src/src/aabb.h

@@ -0,0 +1,80 @@
+// SPDX-FileCopyrightText: 2025 Erin Catto
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include "box3d/types.h"
+
+// Ray cast an AABB. This is a custom function used by height fields.
+bool b3RayCastAABB( b3AABB a, b3Vec3 p1, b3Vec3 p2, float* minFraction, float* maxFraction );
+
+// Get the surface area (perimeter)
+static inline float b3Perimeter( b3AABB a )
+{
+	float wx = a.upperBound.x - a.lowerBound.x;
+	float wy = a.upperBound.y - a.lowerBound.y;
+	float wz = a.upperBound.z - a.lowerBound.z;
+	return 2.0f * ( wx * wz + wy * wx + wz * wy );
+}
+
+/// Enlarge a to contain b
+/// @return true if the AABB grew
+static inline bool b3EnlargeAABB( b3AABB* a, b3AABB 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 ( b.lowerBound.z < a->lowerBound.z )
+	{
+		a->lowerBound.z = b.lowerBound.z;
+		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;
+	}
+
+	if ( a->upperBound.z < b.upperBound.z )
+	{
+		a->upperBound.z = b.upperBound.z;
+		changed = true;
+	}
+
+	return changed;
+}
+
+#if 0
+/// Do a and b overlap
+inline bool b3OverlapAABBs( b3AABB a, b3AABB b )
+{
+	return !( b.lowerBound.x > a.upperBound.x || b.lowerBound.y > a.upperBound.y || b.lowerBound.z > a.upperBound.z ||
+			  a.lowerBound.x > b.upperBound.x || a.lowerBound.y > b.upperBound.y || a.lowerBound.z > b.upperBound.z );
+}
+#endif
+
+static inline b3Vec3 b3FarthestPointOnAABB( b3AABB b, b3Vec3 p )
+{
+	return (b3Vec3){
+		.x = ( p.x - b.lowerBound.x ) > ( b.upperBound.x - p.x ) ? b.lowerBound.x : b.upperBound.x,
+		.y = ( p.y - b.lowerBound.y ) > ( b.upperBound.y - p.y ) ? b.lowerBound.y : b.upperBound.y,
+		.z = ( p.z - b.lowerBound.z ) > ( b.upperBound.z - p.z ) ? b.lowerBound.z : b.upperBound.z,
+	};
+}

vendor/box3d/src/src/algorithm.h

@@ -0,0 +1,19 @@
+// SPDX-FileCopyrightText: 2026 Erin Catto
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <stddef.h>
+#include <string.h>
+
+// Swap two same-size lvalues through a byte buffer. Avoids __typeof__ so it builds on any C compiler.
+#define B3_SWAP( x, y )                                                                                                          \
+	do                                                                                                                           \
+	{                                                                                                                            \
+		_Static_assert( sizeof( x ) == sizeof( y ), "size mismatch" );                                                           \
+		char B3_SWAP_TEMP[sizeof( x )];                                                                                          \
+		memcpy( B3_SWAP_TEMP, &( x ), sizeof( x ) );                                                                             \
+		memcpy( &( x ), &( y ), sizeof( x ) );                                                                                   \
+		memcpy( &( y ), B3_SWAP_TEMP, sizeof( x ) );                                                                             \
+	}                                                                                                                            \
+	while ( 0 )

vendor/box3d/src/src/arena_allocator.c

@@ -0,0 +1,178 @@
+// SPDX-FileCopyrightText: 2025 Erin Catto
+// SPDX-License-Identifier: MIT
+
+#include "arena_allocator.h"
+
+#include "core.h"
+
+#include <stdbool.h>
+#include <stddef.h>
+
+b3Stack b3CreateStack( int capacity )
+{
+	B3_ASSERT( capacity >= 0 );
+	b3Stack stack = { 0 };
+	stack.capacity = capacity;
+	stack.memory = (char*)b3Alloc( capacity );
+	return stack;
+}
+
+void b3DestroyStack( b3Stack* stack )
+{
+	b3Free( stack->memory, stack->capacity );
+}
+
+void* b3StackAlloc( b3Stack* stack, int size, const char* name )
+{
+	if ( stack->entryCount == B3_MAX_STACK_ENTRIES )
+	{
+		B3_ASSERT( false );
+		return NULL;
+	}
+
+	int alignedSize = ( ( size - 1 ) | ( B3_ALIGNMENT - 1 ) ) + 1;
+
+	b3StackEntry entry;
+	entry.size = alignedSize;
+	entry.name = name;
+	if ( stack->index + alignedSize > stack->capacity )
+	{
+		// fall back to the heap (undesirable)
+		entry.data = (char*)b3Alloc( alignedSize );
+		entry.usedMalloc = true;
+
+		B3_ASSERT( ( (uintptr_t)entry.data & ( B3_ALIGNMENT - 1 ) ) == 0 );
+	}
+	else
+	{
+		entry.data = stack->memory + stack->index;
+		entry.usedMalloc = false;
+		stack->index += alignedSize;
+
+		B3_ASSERT( ( (uintptr_t)entry.data & ( B3_ALIGNMENT - 1 ) ) == 0 );
+	}
+
+	stack->allocation += alignedSize;
+	if ( stack->allocation > stack->maxAllocation )
+	{
+		stack->maxAllocation = stack->allocation;
+	}
+
+	stack->entries[stack->entryCount] = entry;
+	stack->entryCount += 1;
+	return entry.data;
+}
+
+void b3StackFree( b3Stack* stack, void* mem )
+{
+	int entryCount = stack->entryCount;
+	B3_ASSERT( entryCount > 0 );
+	b3StackEntry* entry = stack->entries + ( entryCount - 1 );
+	B3_ASSERT( mem == entry->data );
+	if ( entry->usedMalloc )
+	{
+		b3Free( mem, entry->size );
+	}
+	else
+	{
+		stack->index -= entry->size;
+	}
+	stack->allocation -= entry->size;
+	stack->entryCount -= 1;
+}
+
+void b3GrowStack( b3Stack* stack )
+{
+	// Stack must not be in use
+	B3_ASSERT( stack->allocation == 0 );
+
+	if ( stack->maxAllocation > stack->capacity )
+	{
+		b3Free( stack->memory, stack->capacity );
+		stack->capacity = stack->maxAllocation + stack->maxAllocation / 2;
+		stack->memory = (char*)b3Alloc( stack->capacity );
+	}
+}
+
+int b3GetStackCapacity( b3Stack* stack )
+{
+	return stack->capacity;
+}
+
+int b3GetStackAllocation( b3Stack* stack )
+{
+	return stack->allocation;
+}
+
+int b3GetMaxStackAllocation( b3Stack* stack )
+{
+	return stack->maxAllocation;
+}
+
+b3Arena b3CreateArena( int capacity )
+{
+	int c = capacity > 8 ? capacity : 8;
+	b3ArenaSharedState* shared = (b3ArenaSharedState*)b3Alloc( sizeof( b3ArenaSharedState ) );
+	*shared = (b3ArenaSharedState){ 0 };
+
+	return (b3Arena){
+		.memory = (char*)b3Alloc( c ),
+		.capacity = c,
+		.index = 0,
+		.shared = shared,
+	};
+}
+
+void b3DestroyArena( b3Arena* arena )
+{
+	b3ArenaSharedState* shared = arena->shared;
+	if ( shared != NULL )
+	{
+		for ( int i = 0; i < shared->overflows.count; ++i )
+		{
+			b3Free( shared->overflows.data[i].data, shared->overflows.data[i].size );
+		}
+		b3Array_Destroy( shared->overflows );
+		b3Free( shared, sizeof( b3ArenaSharedState ) );
+	}
+	b3Free( arena->memory, arena->capacity );
+	*arena = (b3Arena){ 0 };
+}
+
+void* b3ArenaOverflowAlloc( b3Arena* arena, int size )
+{
+	b3ArenaSharedState* shared = arena->shared;
+	char* data = (char*)b3Alloc( size );
+	b3OverflowBlock block = { data, size };
+	b3Array_Push( shared->overflows, block );
+	shared->overflowBytes += size;
+	return data;
+}
+
+void b3ArenaSync( b3Arena* arena )
+{
+	b3ArenaSharedState* shared = arena->shared;
+
+	for ( int i = 0; i < shared->overflows.count; ++i )
+	{
+		b3Free( shared->overflows.data[i].data, shared->overflows.data[i].size );
+	}
+	b3Array_Clear( shared->overflows );
+
+	int demand = shared->maxIndex + shared->overflowBytes;
+	if ( demand > shared->peakDemand )
+	{
+		shared->peakDemand = demand;
+	}
+	if ( demand > arena->capacity )
+	{
+		b3Free( arena->memory, arena->capacity );
+		int newCapacity = demand + demand / 2;
+		arena->memory = (char*)b3Alloc( newCapacity );
+		arena->capacity = newCapacity;
+	}
+
+	arena->index = 0;
+	shared->maxIndex = 0;
+	shared->overflowBytes = 0;
+}

vendor/box3d/src/src/arena_allocator.h

@@ -0,0 +1,114 @@
+// SPDX-FileCopyrightText: 2025 Erin Catto
+// SPDX-License-Identifier: MIT
+
+#pragma once
+#include "box3d/base.h"
+#include "container.h"
+
+#include <stdbool.h>
+#include <stddef.h>
+
+#define B3_MAX_STACK_ENTRIES 32
+
+typedef struct b3StackEntry
+{
+	char* data;
+	const char* name;
+	int size;
+	bool usedMalloc;
+} b3StackEntry;
+
+// This is a stack-like arena allocator used for fast per step allocations.
+// You must nest allocate/free pairs. The code will B3_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 b3Stack
+{
+	char* memory;
+	int capacity;
+	int index;
+
+	int allocation;
+	int maxAllocation;
+
+	b3StackEntry entries[B3_MAX_STACK_ENTRIES];
+	int entryCount;
+} b3Stack;
+
+// Heap-allocated fallback block tracked when an arena bump overflows.
+typedef struct b3OverflowBlock
+{
+	char* data;
+	int size;
+} b3OverflowBlock;
+
+b3DeclareArray( b3OverflowBlock );
+
+// Shared, heap-allocated state co-owned by every copy of a b3Arena.
+// b3Arena is passed by value so its bump pointer auto-restores on
+// function return, but overflow tracking and watermarks must persist
+// across copies -- hence this pointer-shared block.
+typedef struct b3ArenaSharedState
+{
+	b3Array( b3OverflowBlock ) overflows;
+	int maxIndex;          // high water mark of the bump pointer this step
+	int overflowBytes;     // total bytes in overflow blocks this step
+	int peakDemand;        // all-time peak of (maxIndex + overflowBytes), survives sync
+} b3ArenaSharedState;
+
+typedef struct b3Arena
+{
+	char* memory;
+	int capacity;
+	int index;
+	b3ArenaSharedState* shared;
+} b3Arena;
+
+// 16-byte alignment for SSE2 + typical struct alignment.
+#define B3_ARENA_ALIGNMENT 16
+
+b3Stack b3CreateStack( int capacity );
+void b3DestroyStack( b3Stack* stack );
+
+void* b3StackAlloc( b3Stack* stack, int size, const char* name );
+void b3StackFree( b3Stack* stack, void* mem );
+
+// Grow the stack based on usage
+void b3GrowStack( b3Stack* stack );
+
+int b3GetStackCapacity( b3Stack* stack );
+int b3GetStackAllocation( b3Stack* stack );
+int b3GetMaxStackAllocation( b3Stack* stack );
+
+b3Arena b3CreateArena( int capacity );
+void b3DestroyArena( b3Arena* arena );
+
+// Heap-allocate an overflow block, register it in the shared state, return it.
+void* b3ArenaOverflowAlloc( b3Arena* arena, int size );
+
+// Call between simulation steps. Frees this step's overflow blocks and grows
+// the backing capacity if last step's demand (maxIndex + overflowBytes) exceeded it.
+void b3ArenaSync( b3Arena* arena );
+
+static inline void* b3Bump( b3Arena* arena, int size )
+{
+	if ( size == 0 )
+	{
+		return NULL;
+	}
+
+	int aligned = ( arena->index + ( B3_ARENA_ALIGNMENT - 1 ) ) & ~( B3_ARENA_ALIGNMENT - 1 );
+
+	if ( aligned + size > arena->capacity )
+	{
+		return b3ArenaOverflowAlloc( arena, size );
+	}
+
+	arena->index = aligned + size;
+	if ( arena->index > arena->shared->maxIndex )
+	{
+		arena->shared->maxIndex = arena->index;
+	}
+	return arena->memory + aligned;
+}

vendor/box3d/src/src/bitset.c

@@ -0,0 +1,66 @@
+// SPDX-FileCopyrightText: 2025 Erin Catto
+// SPDX-License-Identifier: MIT
+
+#include "bitset.h"
+
+#include <string.h>
+
+b3BitSet b3CreateBitSet( uint32_t bitCapacity )
+{
+	b3BitSet bitSet = { 0 };
+	bitSet.blockCapacity = ( bitCapacity + sizeof( uint64_t ) * 8 - 1 ) / ( sizeof( uint64_t ) * 8 );
+	bitSet.blockCount = 0;
+	bitSet.bits = (uint64_t*)b3Alloc( bitSet.blockCapacity * sizeof( uint64_t ) );
+	memset( bitSet.bits, 0, bitSet.blockCapacity * sizeof( uint64_t ) );
+	return bitSet;
+}
+
+void b3DestroyBitSet( b3BitSet* bitSet )
+{
+	b3Free( bitSet->bits, bitSet->blockCapacity * sizeof( uint64_t ) );
+	bitSet->blockCapacity = 0;
+	bitSet->blockCount = 0;
+	bitSet->bits = NULL;
+}
+
+void b3SetBitCountAndClear( b3BitSet* bitSet, uint32_t bitCount )
+{
+	uint32_t blockCount = ( bitCount + sizeof( uint64_t ) * 8 - 1 ) / ( sizeof( uint64_t ) * 8 );
+	if ( bitSet->blockCapacity < blockCount )
+	{
+		b3DestroyBitSet( bitSet );
+		uint32_t newBitCapacity = bitCount + ( bitCount >> 1 );
+		*bitSet = b3CreateBitSet( newBitCapacity );
+	}
+
+	bitSet->blockCount = blockCount;
+	memset( bitSet->bits, 0, bitSet->blockCount * sizeof( uint64_t ) );
+}
+
+void b3GrowBitSet( b3BitSet* bitSet, uint32_t blockCount )
+{
+	B3_ASSERT( blockCount > bitSet->blockCount );
+	if ( blockCount > bitSet->blockCapacity )
+	{
+		uint32_t oldCapacity = bitSet->blockCapacity;
+		bitSet->blockCapacity = blockCount + blockCount / 2;
+		uint64_t* newBits = (uint64_t*)b3Alloc( bitSet->blockCapacity * sizeof( uint64_t ) );
+		memset( newBits, 0, bitSet->blockCapacity * sizeof( uint64_t ) );
+		B3_ASSERT( bitSet->bits != NULL );
+		memcpy( newBits, bitSet->bits, oldCapacity * sizeof( uint64_t ) );
+		b3Free( bitSet->bits, oldCapacity * sizeof( uint64_t ) );
+		bitSet->bits = newBits;
+	}
+
+	bitSet->blockCount = blockCount;
+}
+
+void b3InPlaceUnion( b3BitSet* __restrict setA, const b3BitSet* __restrict setB )
+{
+	B3_ASSERT( setA->blockCount == setB->blockCount );
+	uint32_t blockCount = setA->blockCount;
+	for ( uint32_t i = 0; i < blockCount; ++i )
+	{
+		setA->bits[i] |= setB->bits[i];
+	}
+}

vendor/box3d/src/src/bitset.h

@@ -0,0 +1,67 @@
+// SPDX-FileCopyrightText: 2025 Erin Catto
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include "core.h"
+
+#include <stdint.h>
+#include <stdbool.h>
+
+// Bit set provides fast operations on large arrays of bits.
+typedef struct b3BitSet
+{
+	uint64_t* bits;
+	uint32_t blockCapacity;
+	uint32_t blockCount;
+} b3BitSet;
+
+
+b3BitSet b3CreateBitSet( uint32_t bitCapacity );
+void b3DestroyBitSet( b3BitSet* bitSet );
+void b3SetBitCountAndClear( b3BitSet* bitSet, uint32_t bitCount );
+void b3InPlaceUnion( b3BitSet* setA, const b3BitSet* setB );
+void b3GrowBitSet( b3BitSet* bitSet, uint32_t blockCount );
+int b3CountSetBits( b3BitSet* bitSet );
+
+static inline void b3SetBit( b3BitSet* bitSet, uint32_t bitIndex )
+{
+	uint32_t blockIndex = bitIndex / 64;
+	B3_ASSERT( blockIndex < bitSet->blockCount );
+	bitSet->bits[blockIndex] |= ( (uint64_t)1 << bitIndex % 64 );
+}
+
+static inline void b3SetBitGrow( b3BitSet* bitSet, uint32_t bitIndex )
+{
+	uint32_t blockIndex = bitIndex / 64;
+	if ( blockIndex >= bitSet->blockCount )
+	{
+		b3GrowBitSet( bitSet, blockIndex + 1 );
+	}
+	bitSet->bits[blockIndex] |= ( (uint64_t)1 << bitIndex % 64 );
+}
+
+static inline void b3ClearBit( b3BitSet* 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 b3GetBit( const b3BitSet* 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 b3GetBitSetBytes( b3BitSet* bitSet )
+{
+	return bitSet->blockCapacity * sizeof( uint64_t );
+}

vendor/box3d/src/src/block_allocator.c

@@ -0,0 +1,90 @@
+// SPDX-FileCopyrightText: 2026 Erin Catto
+// SPDX-License-Identifier: MIT
+
+#include "block_allocator.h"
+
+#include "core.h"
+
+b3BlockAllocator b3CreateBlockAllocator( int elementSize, int initialCount )
+{
+	B3_ASSERT( elementSize >= (int)sizeof( void* ) );
+
+	b3BlockAllocator allocator = { 0 };
+
+	b3Array_Create( allocator.blocks );
+	allocator.elementSize = elementSize;
+	allocator.freeList = NULL;
+	allocator.nextIndex = 0;
+	allocator.allocationCount = 0;
+
+	if ( initialCount > 0 )
+	{
+		int count = ( initialCount + B3_BLOCK_SIZE - 1 ) >> B3_BLOCK_EXPONENT;
+		b3Array_Resize( allocator.blocks, count );
+
+		for ( int i = 0; i < allocator.blocks.count; ++i )
+		{
+			allocator.blocks.data[i].memory = b3Alloc( B3_BLOCK_SIZE * elementSize );
+		}
+	}
+
+	return allocator;
+}
+
+void b3DestroyBlockAllocator( b3BlockAllocator* allocator )
+{
+	for ( int i = 0; i < allocator->blocks.count; ++i )
+	{
+		b3Free( allocator->blocks.data[i].memory, B3_BLOCK_SIZE * allocator->elementSize );
+	}
+
+	b3Array_Destroy( allocator->blocks );
+}
+
+void* b3AllocateElement( b3BlockAllocator* allocator )
+{
+	B3_ASSERT( allocator != NULL );
+
+	allocator->allocationCount += 1;
+
+	// Pop from free list first
+	if ( allocator->freeList != NULL )
+	{
+		void* element = allocator->freeList;
+		allocator->freeList = *(void**)element;
+		return element;
+	}
+
+	int index = allocator->nextIndex++;
+
+	int requiredBlockCount = ( index >> B3_BLOCK_EXPONENT ) + 1;
+
+	if ( requiredBlockCount > allocator->blocks.count )
+	{
+		int oldCount = allocator->blocks.count;
+		b3Array_Resize( allocator->blocks, requiredBlockCount );
+
+		for ( int i = oldCount; i < requiredBlockCount; ++i )
+		{
+			allocator->blocks.data[i].memory = b3Alloc( B3_BLOCK_SIZE * allocator->elementSize );
+		}
+	}
+
+	int blockIndex = index >> B3_BLOCK_EXPONENT;
+	int blockOffset = index & ( B3_BLOCK_SIZE - 1 );
+
+	return allocator->blocks.data[blockIndex].memory + blockOffset * allocator->elementSize;
+}
+
+void b3FreeElement( b3BlockAllocator* allocator, void* element )
+{
+	B3_ASSERT( allocator != NULL );
+	B3_ASSERT( element != NULL );
+	B3_ASSERT( allocator->allocationCount > 0 );
+
+	allocator->allocationCount -= 1;
+
+	// Push onto free list
+	*(void**)element = allocator->freeList;
+	allocator->freeList = element;
+}

vendor/box3d/src/src/block_allocator.h

@@ -0,0 +1,35 @@
+// SPDX-FileCopyrightText: 2026 Erin Catto
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include "container.h"
+
+#include "box3d/base.h"
+
+#define B3_BLOCK_EXPONENT 8
+#define B3_BLOCK_SIZE ( 1 << B3_BLOCK_EXPONENT )
+
+typedef struct b3Block
+{
+	char* memory;
+} b3Block;
+
+b3DeclareArray( b3Block );
+
+typedef struct b3BlockAllocator
+{
+	b3Array( b3Block ) blocks;
+	void* freeList;
+	int elementSize;
+	int nextIndex;
+	int allocationCount;
+} b3BlockAllocator;
+
+// Element must be large enough to hold a pointer
+b3BlockAllocator b3CreateBlockAllocator( int elementSize, int initialCount );
+void b3DestroyBlockAllocator( b3BlockAllocator* allocator );
+
+// Returns one element of elementSize contiguous bytes. Address is stable until freed.
+void* b3AllocateElement( b3BlockAllocator* allocator );
+void b3FreeElement( b3BlockAllocator* allocator, void* element );

vendor/box3d/src/src/body.h

@@ -0,0 +1,256 @@
+// SPDX-FileCopyrightText: 2025 Erin Catto
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include "box3d/constants.h"
+#include "box3d/math_functions.h"
+#include "box3d/types.h"
+
+typedef struct b3World b3World;
+
+enum b3BodyFlags
+{
+	// This body has fixed translation along the x-axis
+	b3_lockLinearX = 0x00000001,
+
+	// This body has fixed translation along the y-axis
+	b3_lockLinearY = 0x00000002,
+
+	// This body has fixed translation along the z-axis
+	b3_lockLinearZ = 0x00000004,
+
+	// This body has fixed rotation around the x-axis
+	b3_lockAngularX = 0x00000008,
+
+	// This body has fixed rotation around the y-axis
+	b3_lockAngularY = 0x00000010,
+
+	// This body has fixed rotation around the z-axis
+	b3_lockAngularZ = 0x00000020,
+
+	// This flag is used for debug draw
+	b3_isFast = 0x00000040,
+
+	// This dynamic body does a final CCD pass against all body types, but not other bullets
+	b3_isBullet = 0x00000080,
+
+	// This body was speed capped in the current time step
+	b3_isSpeedCapped = 0x00000100,
+
+	// This body had a time of impact event in the current time step
+	b3_hadTimeOfImpact = 0x00000200,
+
+	// This body has no limit on angular velocity
+	b3_allowFastRotation = 0x00000400,
+
+	// This body need's to have its AABB increased
+	b3_enlargeBounds = 0x00000800,
+
+	// This body is dynamic so the solver should write to it.
+	// This prevents writing to kinematic bodies that causes a multithreaded sharing
+	// cache coherence problem even when the values are not changing.
+	// Used for b3BodyState flags.
+	b3_dynamicFlag = 0x00001000,
+
+	b3_enableSleep = 0x00002000,
+
+	b3_bodyEnableContactRecycling = 0x00004000,
+
+	// The user deferred mass computation via the updateBodyMass shape option and mass
+	// data still hasn't been set.
+	b3_dirtyMass = 0x00008000,
+
+	// All lock flags
+	b3_allLocks = b3_lockLinearX | b3_lockLinearY | b3_lockLinearZ | b3_lockAngularX | b3_lockAngularY | b3_lockAngularZ,
+
+	// If all these flags are set then the body has fixed rotation
+	b3_fixedRotation = b3_lockAngularX | b3_lockAngularY | b3_lockAngularZ,
+
+	// These flags are transient per time step. These may be different across b3Body, b3BodySim, and b3BodyState.
+	b3_bodyTransientFlags = b3_isFast | b3_isSpeedCapped | b3_hadTimeOfImpact,
+};
+
+// Body organizational details that are not used in the solver.
+typedef struct b3Body
+{
+	void* userData;
+
+	// index of solver set stored in b3World
+	// may be B3_NULL_INDEX
+	int setIndex;
+
+	// body sim and state index within set
+	// may be B3_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;
+
+	// Index into the island's bodies array for O(1) swap-removal.
+	// B3_NULL_INDEX when not in an island.
+	int islandIndex;
+
+	float sleepThreshold;
+	float sleepTime;
+	float sleepVelocity;
+	float mass;
+
+	// local space inertia
+	b3Matrix3 inertia;
+
+	// this is used to adjust the fellAsleep flag in the body move array
+	int bodyMoveIndex;
+
+	int id;
+
+	// b3BodyFlags
+	uint32_t flags;
+	uint32_t nameId;
+
+	b3BodyType type;
+
+	// This is monotonically advanced when a body is allocated in this slot
+	// Used to check for invalid b3BodyId
+	uint16_t generation;
+} b3Body;
+
+// 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.
+//
+// 56 bytes
+// todo_erin measure perf padding to 64 bytes
+typedef struct b3BodyState
+{
+	b3Vec3 linearVelocity;	// 12
+	b3Vec3 angularVelocity; // 12
+
+	// Using delta position reduces round-off error far from the origin
+	b3Vec3 deltaPosition; // 12
+
+	// 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)
+	b3Quat deltaRotation; // 16
+
+	// b3BodyFlags
+	// Important flags: locking, dynamic
+	uint32_t flags; // 4
+} b3BodyState;
+
+// Identity body state, notice the deltaRotation is identity
+static const b3BodyState b3_identityBodyState = {
+	{ 0.0f, 0.0f, 0.0f }, { 0.0f, 0.0f, 0.0f }, { 0.0f, 0.0f, 0.0f }, { { 0.0f, 0.0f, 0.0f }, 1.0f }, 0,
+};
+
+// Body simulation data used for integration of position and velocity
+// Transform data used for collision and solver preparation.
+typedef struct b3BodySim
+{
+	// transform for body origin, double translation in large world mode
+	b3WorldTransform transform;
+
+	// center of mass position in world space
+	b3Pos center;
+
+	// previous rotation and COM for TOI
+	b3Quat rotation0;
+	b3Pos center0;
+
+	// location of center of mass relative to the body origin
+	b3Vec3 localCenter;
+
+	b3Vec3 force;
+	b3Vec3 torque;
+
+	float invMass;
+
+	// Rotational inertia about the center of mass. The world space inverse inertia tensor
+	// must be updated whenever the body rotation is modified.
+	b3Matrix3 invInertiaLocal;
+	b3Matrix3 invInertiaWorld;
+
+	float minExtent;
+	b3Vec3 maxExtent;
+	float maxAngularVelocity;
+	float linearDamping;
+	float angularDamping;
+	float gravityScale;
+
+	// Index of b3Body
+	int bodyId;
+
+	// b3BodyFlags
+	uint32_t flags;
+} b3BodySim;
+
+// Get a validated body from a world using an id.
+b3Body* b3GetBodyFullId( b3World* world, b3BodyId bodyId );
+
+b3WorldTransform b3GetBodyTransformQuick( b3World* world, b3Body* body );
+b3WorldTransform b3GetBodyTransform( b3World* world, int bodyId );
+
+// Create a b3BodyId from a raw id.
+b3BodyId b3MakeBodyId( b3World* world, int bodyId );
+
+bool b3ShouldBodiesCollide( b3World* world, b3Body* bodyA, b3Body* bodyB );
+bool b3IsBodyAwake( b3World* world, b3Body* body );
+
+b3BodySim* b3GetBodySim( b3World* world, b3Body* body );
+b3BodyState* b3GetBodyState( b3World* world, b3Body* body );
+
+// careful calling this because it can invalidate body, state, joint, and contact pointers
+bool b3WakeBody( b3World* world, b3Body* body );
+bool b3WakeBodyWithLock( b3World* world, b3Body* body );
+
+void b3UpdateBodyMassData( b3World* world, b3Body* body );
+void b3SyncBodyFlags( b3World* world, b3Body* body );
+
+// Make a sweep relative to a base position to keep TOI in float precision far from the origin.
+static inline b3Sweep b3MakeRelativeSweep( const b3BodySim* bodySim, b3Pos base )
+{
+	b3Sweep s;
+	s.c1 = b3SubPos( bodySim->center0, base );
+	s.c2 = b3SubPos( bodySim->center, base );
+	s.q1 = bodySim->rotation0;
+	s.q2 = bodySim->transform.q;
+	s.localCenter = bodySim->localCenter;
+	return s;
+}
+

vendor/box3d/src/src/box3d.natvis

@@ -0,0 +1,147 @@
+<?xml version="1.0" encoding="utf-8"?>
+
+<AutoVisualizer xmlns="http://schemas.microsoft.com/vstudio/debugger/natvis/2010">
+
+	<Type Name="b3Vector2">
+		<DisplayString>[ {x}, {y} ]</DisplayString>
+	</Type>
+
+	<Type Name="b3Vec3">
+		<DisplayString>[ {x}, {y}, {z} ]</DisplayString>
+	</Type>
+
+	<Type Name="b3Quat">
+		<DisplayString>[ {v}, {s} ]</DisplayString>
+	</Type>
+
+	<!-- Distinct types only in BOX3D_DOUBLE_PRECISION builds; otherwise they alias b3Vec3 / b3Transform -->
+	<Type Name="b3Pos">
+		<DisplayString>[ {x}, {y}, {z} ]</DisplayString>
+	</Type>
+
+	<Type Name="b3WorldTransform">
+		<DisplayString>[ {p}, {q} ]</DisplayString>
+	</Type>
+
+	<Type Name="b3WorldId">
+		<DisplayString>index: {index1}</DisplayString>
+		<Expand>
+			<ExpandedItem>b3_worlds[ index1 - 1 ]</ExpandedItem>
+		</Expand>
+	</Type>
+
+	<Type Name="b3BodyId">
+		<DisplayString>
+			{b3_worlds[world0].bodies.data[index1-1].name,na},
+			{b3_worlds[world0].bodies.data[index1-1].type}
+		</DisplayString>
+		<Expand>
+			<ExpandedItem>b3_worlds[ world0 ].bodies.data[index1-1]</ExpandedItem>
+		</Expand>
+	</Type>
+	
+	<Type Name="b3ShapeId">
+		<DisplayString>
+			{b3_worlds[world0].shapes.data[index1-1].type}
+		</DisplayString>
+	</Type>
+
+	<Type Name="b3ManifoldPoint">
+		<DisplayString>
+			{point}, sep = {separation}
+		</DisplayString>
+	</Type>
+	
+	<Type Name="b3FeaturePair">
+		<DisplayString>
+			{int(owner1)}:{int(index1)}, {int(owner2)}:{int(index2)}
+		</DisplayString>
+	</Type>
+
+	<Type Name="b3TempManifold">
+		<DisplayString>count: { pointCount }, triangle: { triangleIndex }, cluster: {clusterIndex}</DisplayString>
+		<Expand>
+			<Item Name="normal">normal</Item>
+			<Item Name="feature">feature</Item>
+			<Item Name="flags">triangleFlags</Item>
+			<ArrayItems>
+				<Size>pointCount</Size>
+				<ValuePointer>points</ValuePointer>
+			</ArrayItems>
+		</Expand>
+	</Type>
+	
+	<Type Name="b3ShapeProxy">
+		<DisplayString>{{{count} : {radius}}}</DisplayString>
+		<Expand>
+			<Item Name="[count]">count</Item>
+			<Item Name="[radius]">radius</Item>
+			<ArrayItems Condition="count > 0">
+				<Size>count</Size>
+				<ValuePointer>points</ValuePointer>
+			</ArrayItems>
+		</Expand>
+	</Type>
+
+	<Type Name="b3SeparationFunction">
+		<DisplayString>type: {type}</DisplayString>
+		<Expand>
+			<Item Name="[witnessA]">witnessA</Item>
+			<Item Name="[witnessB]">witnessB</Item>
+			<Item Name="[sweepA]">sweepA</Item>
+			<Item Name="[sweepB]">sweepB</Item>
+			<Item Name="[proxyA]">proxyA</Item>
+			<Item Name="[proxyB]">proxyB</Item>
+		</Expand>
+	</Type>
+
+	<Type Name="b3DynamicArray_*">
+		<DisplayString>{{{count} / {capacity}}}</DisplayString>
+		<Expand>
+			<Item Name="[count]">count</Item>
+			<Item Name="[capacity]">capacity</Item>
+			<ArrayItems Condition="count > 0">
+				<Size>count</Size>
+				<ValuePointer>data</ValuePointer>
+			</ArrayItems>
+		</Expand>
+	</Type>
+
+	<!-- natvis does not handle wildcards in struct names -->
+	<Type Name="b3DynamicArray_int">
+		<DisplayString>{{{count} / {capacity}}}</DisplayString>
+		<Expand>
+			<Item Name="[count]">count</Item>
+			<Item Name="[capacity]">capacity</Item>
+			<ArrayItems Condition="count > 0">
+				<Size>count</Size>
+				<ValuePointer>data</ValuePointer>
+			</ArrayItems>
+		</Expand>
+	</Type>
+
+	<Type Name="b3DynamicArray_Foo">
+		<DisplayString>{{{count} / {capacity}}}</DisplayString>
+		<Expand>
+			<Item Name="[count]">count</Item>
+			<Item Name="[capacity]">capacity</Item>
+			<ArrayItems Condition="count > 0">
+				<Size>count</Size>
+				<ValuePointer>data</ValuePointer>
+			</ArrayItems>
+		</Expand>
+	</Type>
+
+	<Type Name="b3DynamicArray_b3Contact">
+		<DisplayString>{{{count} / {capacity}}}</DisplayString>
+		<Expand>
+			<Item Name="[count]">count</Item>
+			<Item Name="[capacity]">capacity</Item>
+			<ArrayItems Condition="count > 0">
+				<Size>count</Size>
+				<ValuePointer>data</ValuePointer>
+			</ArrayItems>
+		</Expand>
+	</Type>
+
+</AutoVisualizer>

vendor/box3d/src/src/broad_phase.c

@@ -0,0 +1,535 @@
+// SPDX-FileCopyrightText: 2025 Erin Catto
+// SPDX-License-Identifier: MIT
+
+#include "broad_phase.h"
+
+#include "aabb.h"
+#include "arena_allocator.h"
+#include "body.h"
+#include "contact.h"
+#include "core.h"
+#include "parallel_for.h"
+#include "physics_world.h"
+#include "platform.h"
+#include "shape.h"
+
+#include <string.h>
+
+void b3CreateBroadPhase( b3BroadPhase* bp, const b3Capacity* capacity )
+{
+	_Static_assert( b3_bodyTypeCount == 3, "must be three body types" );
+
+	bp->movedProxies[b3_staticBody] = b3CreateBitSet( b3MaxInt( 16, capacity->staticShapeCount ) );
+	bp->movedProxies[b3_kinematicBody] = b3CreateBitSet( 16 );
+	bp->movedProxies[b3_dynamicBody] = b3CreateBitSet( b3MaxInt( 16, capacity->dynamicShapeCount ) );
+	b3Array_Reserve( bp->moveArray, capacity->dynamicShapeCount );
+	bp->moveResults = NULL;
+	bp->movePairs = NULL;
+	bp->movePairCapacity = 0;
+	b3AtomicStoreInt( &bp->movePairIndex, 0 );
+	bp->pairSet = b3CreateSet( 2 * capacity->contactCount );
+
+	int staticCapacity = b3MaxInt( 16, capacity->staticShapeCount );
+	bp->trees[b3_staticBody] = b3DynamicTree_Create( staticCapacity );
+
+	int kinematicCapacity = 16;
+	bp->trees[b3_kinematicBody] = b3DynamicTree_Create( kinematicCapacity );
+
+	int dynamicCapacity = b3MaxInt( 16, capacity->dynamicShapeCount );
+	bp->trees[b3_dynamicBody] = b3DynamicTree_Create( dynamicCapacity );
+}
+
+void b3DestroyBroadPhase( b3BroadPhase* bp )
+{
+	for ( int i = 0; i < b3_bodyTypeCount; ++i )
+	{
+		b3DynamicTree_Destroy( bp->trees + i );
+	}
+
+	for ( int i = 0; i < b3_bodyTypeCount; ++i )
+	{
+		b3DestroyBitSet( &bp->movedProxies[i] );
+	}
+	b3Array_Destroy( bp->moveArray );
+	b3DestroySet( &bp->pairSet );
+
+	*bp = (b3BroadPhase){ 0 };
+
+	memset( bp, 0, sizeof( b3BroadPhase ) );
+}
+
+static void b3UnBufferMove( b3BroadPhase* bp, int proxyKey )
+{
+	b3BodyType proxyType = B3_PROXY_TYPE( proxyKey );
+	int proxyId = B3_PROXY_ID( proxyKey );
+	b3BitSet* set = &bp->movedProxies[proxyType];
+
+	if ( b3GetBit( set, proxyId ) )
+	{
+		b3ClearBit( set, proxyId );
+
+		// 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 )
+			{
+				b3Array_RemoveSwap( bp->moveArray, i );
+				break;
+			}
+		}
+	}
+}
+
+int b3BroadPhase_CreateProxy( b3BroadPhase* bp, b3BodyType proxyType, b3AABB aabb, uint64_t categoryBits, int shapeIndex,
+							  bool forcePairCreation )
+{
+	B3_ASSERT( 0 <= proxyType && proxyType < b3_bodyTypeCount );
+	int proxyId = b3DynamicTree_CreateProxy( bp->trees + proxyType, aabb, categoryBits, shapeIndex );
+	int proxyKey = B3_PROXY_KEY( proxyId, proxyType );
+	if ( proxyType != b3_staticBody || forcePairCreation )
+	{
+		b3BufferMove( bp, proxyKey );
+	}
+	return proxyKey;
+}
+
+void b3BroadPhase_DestroyProxy( b3BroadPhase* bp, int proxyKey )
+{
+	b3UnBufferMove( bp, proxyKey );
+
+	b3BodyType proxyType = B3_PROXY_TYPE( proxyKey );
+	int proxyId = B3_PROXY_ID( proxyKey );
+
+	B3_ASSERT( 0 <= proxyType && proxyType <= b3_bodyTypeCount );
+	b3DynamicTree_DestroyProxy( bp->trees + proxyType, proxyId );
+}
+
+void b3BroadPhase_MoveProxy( b3BroadPhase* bp, int proxyKey, b3AABB aabb )
+{
+	b3BodyType proxyType = B3_PROXY_TYPE( proxyKey );
+	int proxyId = B3_PROXY_ID( proxyKey );
+
+	b3DynamicTree_MoveProxy( bp->trees + proxyType, proxyId, aabb );
+	b3BufferMove( bp, proxyKey );
+}
+
+void b3BroadPhase_EnlargeProxy( b3BroadPhase* bp, int proxyKey, b3AABB aabb )
+{
+	B3_ASSERT( proxyKey != B3_NULL_INDEX );
+	int typeIndex = B3_PROXY_TYPE( proxyKey );
+	int proxyId = B3_PROXY_ID( proxyKey );
+
+	B3_ASSERT( typeIndex != b3_staticBody );
+
+	b3DynamicTree_EnlargeProxy( bp->trees + typeIndex, proxyId, aabb );
+	b3BufferMove( bp, proxyKey );
+}
+
+typedef struct b3MovePair
+{
+	int shapeIndexA;
+	int shapeIndexB;
+	int childIndex;
+	b3MovePair* next;
+	bool heap;
+} b3MovePair;
+
+typedef struct b3MoveResult
+{
+	b3MovePair* pairList;
+} b3MoveResult;
+
+typedef struct b3QueryPairContext
+{
+	b3World* world;
+	b3MoveResult* moveResult;
+	b3AABB aabb;
+	b3BodyType queryTreeType;
+	int queryProxyKey;
+	int queryShapeIndex;
+
+	int compoundProxyId;
+	int compoundShapeIndex;
+} b3QueryPairContext;
+
+// This is called from b3DynamicTree::Query when we are gathering pairs.
+static bool b3PairQueryCallback( int proxyId, uint64_t userData, void* context )
+{
+	b3QueryPairContext* queryContext = (b3QueryPairContext*)context;
+	b3World* world = queryContext->world;
+	int shapeIndex;
+	int childIndex = 0;
+
+	if ( queryContext->compoundShapeIndex == B3_NULL_INDEX )
+	{
+		// Outer query: userData is a shape index.
+		shapeIndex = (int)userData;
+
+		// A proxy cannot form a pair with itself.
+		if ( shapeIndex == queryContext->queryShapeIndex )
+		{
+			return true;
+		}
+
+		b3Shape* shape = b3Array_Get( world->shapes, shapeIndex );
+		if ( shape->type == b3_compoundShape )
+		{
+			// Query bounds are float world space, so the demoted transform is the matching float frame
+			b3Transform compoundTransform = b3ToRelativeTransform( b3GetBodyTransform( world, shape->bodyId ), b3Pos_zero );
+			b3AABB localAABB = b3AABB_Transform( b3InvertTransform( compoundTransform ), queryContext->aabb );
+
+			// recurse
+			queryContext->compoundShapeIndex = shapeIndex;
+			queryContext->compoundProxyId = proxyId;
+
+			b3DynamicTree_Query( &shape->compound->tree, localAABB, B3_DEFAULT_MASK_BITS, false, b3PairQueryCallback, context );
+			queryContext->compoundShapeIndex = B3_NULL_INDEX;
+			queryContext->compoundProxyId = B3_NULL_INDEX;
+			return true;
+		}
+	}
+	else
+	{
+		// Inner query into a compound shape: userData is the compound child index, not a shape
+		// index, so do not compare it against queryShapeIndex.
+		shapeIndex = queryContext->compoundShapeIndex;
+		proxyId = queryContext->compoundProxyId;
+		childIndex = (int)userData;
+	}
+
+	b3BroadPhase* broadPhase = &queryContext->world->broadPhase;
+
+	int proxyKey = B3_PROXY_KEY( proxyId, queryContext->queryTreeType );
+	int queryProxyKey = queryContext->queryProxyKey;
+
+	// A proxy cannot form a pair with itself.
+	B3_ASSERT( proxyKey != queryContext->queryProxyKey );
+
+	b3BodyType treeType = queryContext->queryTreeType;
+	b3BodyType queryProxyType = B3_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 movedProxies bit sets contain dynamic and kinematic
+	// proxies, but sometimes static proxies are in there too (b3ShapeDef::invokeContactCreation or a
+	// modified static shape), so we always have to check.
+
+	// Is this proxy also moving?
+	if ( queryProxyType == b3_dynamicBody )
+	{
+		if ( treeType == b3_dynamicBody && proxyKey < queryProxyKey )
+		{
+			bool moved = b3GetBit( &broadPhase->movedProxies[treeType], proxyId );
+			if ( moved )
+			{
+				// Both proxies are moving. Avoid duplicate pairs.
+				return true;
+			}
+		}
+	}
+	else
+	{
+		B3_ASSERT( treeType == b3_dynamicBody );
+		bool moved = b3GetBit( &broadPhase->movedProxies[treeType], proxyId );
+		if ( moved )
+		{
+			// Both proxies are moving. Avoid duplicate pairs.
+			return true;
+		}
+	}
+
+	uint64_t pairKey = b3ShapePairKey( shapeIndex, queryContext->queryShapeIndex, childIndex );
+	if ( b3ContainsKey( &broadPhase->pairSet, pairKey ) )
+	{
+		// contact exists
+		return true;
+	}
+
+	// Order shapes so that B3_SHAPE_PAIR_KEY works correctly
+	int shapeIdA = shapeIndex;
+	int shapeIdB = queryContext->queryShapeIndex;
+	b3Shape* shapeA = b3Array_Get( world->shapes, shapeIdA );
+	b3Shape* shapeB = b3Array_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 != B3_NULL_INDEX || shapeB->sensorIndex != B3_NULL_INDEX )
+	{
+		return true;
+	}
+
+	if ( b3ShouldShapesCollide( shapeA->filter, shapeB->filter ) == false )
+	{
+		return true;
+	}
+
+	// Does a joint override collision?
+	b3Body* bodyA = b3Array_Get( world->bodies, bodyIdA );
+	b3Body* bodyB = b3Array_Get( world->bodies, bodyIdB );
+	if ( b3ShouldBodiesCollide( world, bodyA, bodyB ) == false )
+	{
+		return true;
+	}
+
+	// Custom user filter
+	if ( ( shapeA->flags & b3_enableCustomFiltering ) || ( shapeB->flags & b3_enableCustomFiltering ) )
+	{
+		b3CustomFilterFcn* customFilterFcn = queryContext->world->customFilterFcn;
+		if ( customFilterFcn != NULL )
+		{
+			b3ShapeId idA = { shapeIdA + 1, world->worldId, shapeA->generation };
+			b3ShapeId 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 = b3AtomicFetchAddInt( &broadPhase->movePairIndex, 1 );
+
+	b3MovePair* pair;
+	if ( pairIndex < broadPhase->movePairCapacity )
+	{
+		pair = broadPhase->movePairs + pairIndex;
+		pair->heap = false;
+	}
+	else
+	{
+		// todo experimenting with ignoring this pair if we ran out of space
+		return true;
+		// pair = (b3MovePair*)b3Alloc( sizeof( b3MovePair ) );
+		// pair->heap = true;
+	}
+
+	pair->shapeIndexA = shapeIdA;
+	pair->shapeIndexB = shapeIdB;
+	pair->childIndex = childIndex;
+	pair->next = queryContext->moveResult->pairList;
+	queryContext->moveResult->pairList = pair;
+
+	// continue the query
+	return true;
+}
+
+static void b3FindPairsTask( int startIndex, int endIndex, int workerIndex, void* context )
+{
+	b3TracyCZoneNC( pair_task, "Pair Task", b3_colorAquamarine, true );
+
+	B3_UNUSED( workerIndex );
+
+	b3World* world = (b3World*)context;
+	b3BroadPhase* bp = &world->broadPhase;
+
+	b3QueryPairContext queryContext = { 0 };
+	queryContext.world = world;
+	queryContext.compoundShapeIndex = B3_NULL_INDEX;
+
+	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];
+		b3BodyType proxyType = B3_PROXY_TYPE( proxyKey );
+
+		int proxyId = B3_PROXY_ID( proxyKey );
+		queryContext.queryProxyKey = proxyKey;
+
+		const b3DynamicTree* 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.
+		b3AABB fatAABB = b3DynamicTree_GetAABB( baseTree, proxyId );
+		queryContext.queryShapeIndex = (int)b3DynamicTree_GetUserData( baseTree, proxyId );
+		queryContext.aabb = fatAABB;
+
+		// Compound shape collision invocation is not supported
+		B3_VALIDATE( world->shapes.data[queryContext.queryShapeIndex].type != b3_compoundShape );
+
+		// Query trees. Only dynamic proxies collide with kinematic and static proxies.
+		// Using B3_DEFAULT_MASK_BITS so that b3Filter::groupIndex works.
+		// consider using bits = groupIndex > 0 ? B3_DEFAULT_MASK_BITS : maskBits
+		bool requireAllBits = false;
+		if ( proxyType == b3_dynamicBody )
+		{
+			queryContext.queryTreeType = b3_kinematicBody;
+			b3DynamicTree_Query( bp->trees + b3_kinematicBody, fatAABB, B3_DEFAULT_MASK_BITS, requireAllBits, b3PairQueryCallback,
+								 &queryContext );
+
+			queryContext.queryTreeType = b3_staticBody;
+			b3DynamicTree_Query( bp->trees + b3_staticBody, fatAABB, B3_DEFAULT_MASK_BITS, requireAllBits, b3PairQueryCallback,
+								 &queryContext );
+		}
+
+		// All proxies collide with dynamic proxies
+		// Using B3_DEFAULT_MASK_BITS so that b3Filter::groupIndex works.
+		queryContext.queryTreeType = b3_dynamicBody;
+		b3DynamicTree_Query( bp->trees + b3_dynamicBody, fatAABB, B3_DEFAULT_MASK_BITS, requireAllBits, b3PairQueryCallback,
+							 &queryContext );
+	}
+
+	b3TracyCZoneEnd( pair_task );
+}
+
+static void b3UpdateTreesTask( void* context )
+{
+	b3TracyCZoneNC( tree_task, "Rebuild Trees", b3_colorFireBrick, true );
+
+	b3World* world = (b3World*)context;
+	b3DynamicTree_Rebuild( world->broadPhase.trees + b3_dynamicBody, false );
+	b3DynamicTree_Rebuild( world->broadPhase.trees + b3_kinematicBody, false );
+
+	b3TracyCZoneEnd( tree_task );
+}
+
+void b3UpdateBroadPhasePairs( b3World* world )
+{
+	b3BroadPhase* bp = &world->broadPhase;
+
+	int moveCount = bp->moveArray.count;
+
+	if ( moveCount == 0 )
+	{
+		return;
+	}
+
+	b3TracyCZoneNC( update_pairs, "Pairs", b3_colorMediumSlateBlue, true );
+
+	b3Stack* alloc = &world->stack;
+
+	// todo these could be in the step context
+	bp->moveResults = (b3MoveResult*)b3StackAlloc( alloc, moveCount * sizeof( b3MoveResult ), "move results" );
+	bp->movePairCapacity = 16 * moveCount;
+	bp->movePairs = (b3MovePair*)b3StackAlloc( alloc, bp->movePairCapacity * sizeof( b3MovePair ), "move pairs" );
+
+	b3AtomicStoreInt( &bp->movePairIndex, 0 );
+
+#ifndef NDEBUG
+	extern b3AtomicInt b3_probeCount;
+	b3AtomicStoreInt( &b3_probeCount, 0 );
+#endif
+
+	int minRange = 64;
+	b3ParallelFor( world, b3FindPairsTask, moveCount, minRange, world, "pairs" );
+
+	b3TracyCZoneNC( create_contacts, "Create Contacts", b3_colorCoral, true );
+
+	// Task that can be done in parallel with the narrow-phase
+	// - rebuild the collision tree for dynamic and kinematic bodies to keep their query performance good
+	if ( world->taskCount < B3_MAX_TASKS )
+	{
+		world->userTreeTask = world->enqueueTaskFcn( &b3UpdateTreesTask, world, world->userTaskContext, "rebuild tree" );
+		world->taskCount += 1;
+		world->activeTaskCount += world->userTreeTask == NULL ? 0 : 1;
+	}
+	else
+	{
+		world->userTreeTask = NULL;
+		b3UpdateTreesTask( world );
+	}
+
+	// Single-threaded work
+	// - Clear move flags
+	// - Create contacts in deterministic order
+	for ( int i = 0; i < moveCount; ++i )
+	{
+		b3MoveResult* result = bp->moveResults + i;
+		b3MovePair* pair = result->pairList;
+		while ( pair != NULL )
+		{
+			int shapeIdA = pair->shapeIndexA;
+			int shapeIdB = pair->shapeIndexB;
+			int childIndex = pair->childIndex;
+
+			b3Shape* shapeA = b3Array_Get( world->shapes, shapeIdA );
+			b3Shape* shapeB = b3Array_Get( world->shapes, shapeIdB );
+
+			b3CreateContact( world, shapeA, shapeB, childIndex );
+
+			if ( pair->heap )
+			{
+				b3MovePair* temp = pair;
+				pair = pair->next;
+				b3Free( temp, sizeof( b3MovePair ) );
+			}
+			else
+			{
+				pair = pair->next;
+			}
+		}
+	}
+
+	// Reset move buffer: clear only the bits that were set this step.
+	// Invariant: bit set in movedProxies[type] iff proxyKey is present in moveArray.
+	for ( int i = 0; i < bp->moveArray.count; ++i )
+	{
+		int proxyKey = bp->moveArray.data[i];
+		b3ClearBit( &bp->movedProxies[B3_PROXY_TYPE( proxyKey )], B3_PROXY_ID( proxyKey ) );
+	}
+	b3Array_Clear( bp->moveArray );
+
+	b3StackFree( alloc, bp->movePairs );
+	bp->movePairs = NULL;
+	b3StackFree( alloc, bp->moveResults );
+	bp->moveResults = NULL;
+
+	b3ValidateSolverSets( world );
+
+	b3TracyCZoneEnd( create_contacts );
+
+	b3TracyCZoneEnd( update_pairs );
+}
+
+bool b3BroadPhase_TestOverlap( const b3BroadPhase* bp, int proxyKeyA, int proxyKeyB )
+{
+	int typeIndexA = B3_PROXY_TYPE( proxyKeyA );
+	int proxyIdA = B3_PROXY_ID( proxyKeyA );
+	int typeIndexB = B3_PROXY_TYPE( proxyKeyB );
+	int proxyIdB = B3_PROXY_ID( proxyKeyB );
+
+	b3AABB aabbA = b3DynamicTree_GetAABB( bp->trees + typeIndexA, proxyIdA );
+	b3AABB aabbB = b3DynamicTree_GetAABB( bp->trees + typeIndexB, proxyIdB );
+	return b3AABB_Overlaps( aabbA, aabbB );
+}
+
+int b3BroadPhase_GetShapeIndex( b3BroadPhase* bp, int proxyKey )
+{
+	int typeIndex = B3_PROXY_TYPE( proxyKey );
+	int proxyId = B3_PROXY_ID( proxyKey );
+
+	return (int)b3DynamicTree_GetUserData( bp->trees + typeIndex, proxyId );
+}
+
+void b3ValidateBroadPhase( const b3BroadPhase* bp )
+{
+	b3DynamicTree_Validate( bp->trees + b3_dynamicBody );
+	b3DynamicTree_Validate( bp->trees + b3_kinematicBody );
+
+	// todo validate every shape AABB is contained in tree AABB
+}
+
+void b3ValidateNoEnlarged( const b3BroadPhase* bp )
+{
+#if B3_ENABLE_VALIDATION == 1
+	for ( int j = 0; j < b3_bodyTypeCount; ++j )
+	{
+		const b3DynamicTree* tree = bp->trees + j;
+		b3DynamicTree_ValidateNoEnlarged( tree );
+	}
+#else
+	B3_UNUSED( bp );
+#endif
+}

vendor/box3d/src/src/broad_phase.h

@@ -0,0 +1,79 @@
+// SPDX-FileCopyrightText: 2025 Erin Catto
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include "bitset.h"
+#include "container.h"
+#include "table.h"
+
+#include "box3d/collision.h"
+#include "box3d/types.h"
+
+typedef struct b3Shape b3Shape;
+typedef struct b3MovePair b3MovePair;
+typedef struct b3MoveResult b3MoveResult;
+typedef struct b3Stack b3Stack;
+typedef struct b3World b3World;
+
+// Store the proxy type in the lower 2 bits of the proxy key. This leaves 30 bits for the id.
+#define B3_PROXY_TYPE( KEY ) ( (b3BodyType)( ( KEY ) & 3 ) )
+#define B3_PROXY_ID( KEY ) ( ( KEY ) >> 2 )
+#define B3_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 b3BroadPhase
+{
+	b3DynamicTree trees[b3_bodyTypeCount];
+
+	// Per body-type bit sets indexed by proxyId, marking proxies moved this step.
+	// Paired with moveArray which preserves deterministic insertion order for pair queries.
+	b3BitSet movedProxies[b3_bodyTypeCount];
+	b3Array( int ) 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
+	b3MoveResult* moveResults;
+	b3MovePair* movePairs;
+	int movePairCapacity;
+	b3AtomicInt movePairIndex;
+
+	// Tracks shape pairs that have a b3Contact
+	// todo pairSet can grow quite large on the first time step and remain large
+	b3HashSet pairSet;
+} b3BroadPhase;
+
+void b3CreateBroadPhase( b3BroadPhase* bp, const b3Capacity* capacity );
+void b3DestroyBroadPhase( b3BroadPhase* bp );
+
+int b3BroadPhase_CreateProxy( b3BroadPhase* bp, b3BodyType proxyType, b3AABB aabb, uint64_t categoryBits, int shapeIndex,
+							  bool forcePairCreation );
+void b3BroadPhase_DestroyProxy( b3BroadPhase* bp, int proxyKey );
+
+void b3BroadPhase_MoveProxy( b3BroadPhase* bp, int proxyKey, b3AABB aabb );
+void b3BroadPhase_EnlargeProxy( b3BroadPhase* bp, int proxyKey, b3AABB aabb );
+
+int b3BroadPhase_GetShapeIndex( b3BroadPhase* bp, int proxyKey );
+
+void b3UpdateBroadPhasePairs( b3World* world );
+bool b3BroadPhase_TestOverlap( const b3BroadPhase* bp, int proxyKeyA, int proxyKeyB );
+
+void b3ValidateBroadPhase( const b3BroadPhase* bp );
+void b3ValidateNoEnlarged( const b3BroadPhase* bp );
+
+// This is what triggers new contact pairs to be created
+// Warning: this must be called in deterministic order
+static inline void b3BufferMove( b3BroadPhase* bp, int queryProxy )
+{
+	b3BodyType proxyType = B3_PROXY_TYPE( queryProxy );
+	int proxyId = B3_PROXY_ID( queryProxy );
+	b3BitSet* set = &bp->movedProxies[proxyType];
+	if ( b3GetBit( set, proxyId ) == false )
+	{
+		b3SetBitGrow( set, proxyId );
+		b3Array_Push( bp->moveArray, queryProxy );
+	}
+}

vendor/box3d/src/src/capsule.c

@@ -0,0 +1,385 @@
+// SPDX-FileCopyrightText: 2025 Erin Catto
+// SPDX-License-Identifier: MIT
+
+#include "math_internal.h"
+#include "shape.h"
+
+#include "box3d/base.h"
+#include "box3d/collision.h"
+#include "box3d/constants.h"
+
+b3MassData b3ComputeCapsuleMass( const b3Capsule* shape, float density )
+{
+	b3Vec3 c1 = shape->center1;
+	b3Vec3 c2 = shape->center2;
+	float r = shape->radius;
+
+	// Cylinder
+	float cylinderHeight = b3Distance( c1, c2 );
+	float cylinderVolume = B3_PI * r * r * cylinderHeight;
+	float cylinderMass = cylinderVolume * density;
+
+	// Sphere
+	float sphereVolume = ( 4.0f / 3.0f ) * B3_PI * r * r * r;
+	float sphereMass = sphereVolume * density;
+
+	// Local accumulated inertia
+	b3Matrix3 inertia = b3AddMM( b3CylinderInertia( cylinderMass, r, cylinderHeight ), b3SphereInertia( sphereMass, r ) );
+
+	float steiner = 0.125f * sphereMass * ( 3.0f * r + 2.0f * cylinderHeight ) * cylinderHeight;
+	inertia.cx.x += steiner;
+	inertia.cz.z += steiner;
+
+	// Align capsule axis with chosen up-axis
+	b3Matrix3 rotation = b3Mat3_identity;
+	if ( cylinderHeight * cylinderHeight > 1000.0f * FLT_MIN )
+	{
+		b3Vec3 direction = b3Normalize( b3Sub( c2, c1 ) );
+		b3Quat q = b3ComputeQuatBetweenUnitVectors( b3Vec3_axisY, direction );
+		rotation = b3MakeMatrixFromQuat( q );
+	}
+
+	float mass = sphereMass + cylinderMass;
+	b3Vec3 center = b3MulSV( 0.5f, b3Add( c1, c2 ) );
+
+	b3MassData out;
+	out.mass = mass;
+	out.center = center;
+
+	// Rotate the central inertia into the shape frame
+	out.inertia = b3MulMM( rotation, b3MulMM( inertia, b3Transpose( rotation ) ) );
+
+	return out;
+}
+
+b3AABB b3ComputeCapsuleAABB( const b3Capsule* shape, b3Transform transform )
+{
+	float r = shape->radius;
+
+	b3Vec3 center1 = b3TransformPoint( transform, shape->center1 );
+	b3Vec3 center2 = b3TransformPoint( transform, shape->center2 );
+	b3Vec3 extent = { r, r, r };
+
+	b3AABB aabb;
+	aabb.lowerBound = b3Sub( b3Min( center1, center2 ), extent );
+	aabb.upperBound = b3Add( b3Max( center1, center2 ), extent );
+	return aabb;
+}
+
+b3AABB b3ComputeSweptCapsuleAABB( const b3Capsule* shape, b3Transform xf1, b3Transform xf2 )
+{
+	b3Vec3 r = { shape->radius, shape->radius, shape->radius };
+	b3Vec3 a = b3TransformPoint( xf1, shape->center1 );
+	b3Vec3 b = b3TransformPoint( xf1, shape->center2 );
+	b3Vec3 c = b3TransformPoint( xf2, shape->center1 );
+	b3Vec3 d = b3TransformPoint( xf2, shape->center2 );
+
+	b3AABB aabb = {
+		.lowerBound = b3Sub( b3Min( b3Min( a, b ), b3Min( c, d ) ), r ),
+		.upperBound = b3Add( b3Max( b3Max( a, b ), b3Max( c, d ) ), r ),
+	};
+	return aabb;
+}
+
+bool b3OverlapCapsule( const b3Capsule* shape, b3Transform shapeTransform, const b3ShapeProxy* proxy )
+{
+	b3DistanceInput input;
+	input.proxyA = (b3ShapeProxy){ &shape->center1, 2, shape->radius };
+	input.proxyB = *proxy;
+	input.transform = b3InvMulTransforms( shapeTransform, b3Transform_identity );
+	input.useRadii = true;
+
+	b3SimplexCache cache = { 0 };
+	b3DistanceOutput output = b3ShapeDistance( &input, &cache, NULL, 0 );
+	return output.distance < B3_OVERLAP_SLOP;
+}
+
+// Precision Improvements for Ray / Sphere Intersection - Ray Tracing Gems 2019
+// http://www.codercorner.com/blog/?p=321
+b3CastOutput b3RayCastCapsule( const b3Capsule* shape, const b3RayCastInput* input )
+{
+	B3_ASSERT( b3IsValidRay( input ) );
+
+	b3Vec3 c1 = shape->center1;
+	b3Vec3 c2 = shape->center2;
+	float r = shape->radius;
+
+	// Initialize result structure
+	b3CastOutput output = { 0 };
+
+	b3Vec3 d = b3Sub( c2, c1 );
+
+	// Fall back to sphere if the capsule is short
+	float tol = 0.01f * B3_LINEAR_SLOP;
+	float lengthSquared = b3LengthSquared( d );
+	if ( lengthSquared < tol * tol )
+	{
+		b3Vec3 sphereCenter = b3MulSV( 0.5f, b3Add( shape->center1, shape->center2 ) );
+		b3Sphere sphere = { sphereCenter, shape->radius };
+		return b3RayCastSphere( &sphere, input );
+	}
+
+	// Vector from first center to ray origin.
+	b3Vec3 s = b3Sub( input->origin, c1 );
+
+	// Capsule axis
+	float length = sqrtf( lengthSquared );
+	b3Vec3 axis = b3MulSV( 1.0f / length, d );
+
+	// Project ray origin onto capsule axis.
+	float u = b3Dot( s, axis );
+
+	// Closest point on infinite capsule axis, relative to c1.
+	b3Vec3 c = b3MulSV( u, axis );
+
+	// Vector from closest point to ray origin
+	b3Vec3 sc = b3Sub( s, c );
+
+	// Squared distance from ray origin to capsule axis
+	float sc2 = b3LengthSquared( sc );
+
+	// Is the ray origin within the infinite cylinder along the capsule axis?
+	if ( sc2 < r * r )
+	{
+		// Clamped barycentric coordinate of ray origin projected onto capsule axis.
+		float uClamped = b3ClampFloat( u, 0.0f, length );
+
+		// The closest point on the bounded capsule segment, relative to c1.
+		b3Vec3 cp = b3MulSV( uClamped, axis );
+
+		// Vector from ray origin to closest point on segment.
+		b3Vec3 scp = b3Sub( s, cp );
+
+		// Squared distance of ray origin from capsule segment.
+		float scp2 = b3LengthSquared( scp );
+
+		// Is the ray origin within the capsule?
+		if ( scp2 < r * r )
+		{
+			output.hit = true;
+			output.point = input->origin;
+			return output;
+		}
+
+		// The ray can hit an endcap.
+		b3Sphere sphere = {
+			.center = b3Add( c1, cp ),
+			.radius = r,
+		};
+
+		return b3RayCastSphere( &sphere, input );
+	}
+
+	// Ray axis. A zero length ray reaching here starts outside the capsule, so it misses.
+	// Same zero length convention as b3RayCastSphere.
+	b3Vec3 dr = input->translation;
+	float rayLength;
+	b3Vec3 rayAxis = b3GetLengthAndNormalize( &rayLength, dr );
+	if ( rayLength == 0.0f )
+	{
+		return output;
+	}
+
+	// Barycentric coordinate of ray end point.
+	float v = u + input->maxFraction * b3Dot( dr, axis );
+
+	// Early out: does the projected ray fall outside the capsule?
+	if ( ( u < -r && v < -r ) || ( length + r < u && length + r < v ) )
+	{
+		return output;
+	}
+
+	// Compute the closest point between the ray segment and the capsule segment.
+	// See Real-Time Collision Detection, section 5.1.9
+
+	// Closest point on capsule : a1 = segment unit axis, t1 = unknown fraction
+	// p1 = t1 * a1
+
+	// Closet point on ray : a2 = ray unit axis, t2 = unknown fraction
+	// p2 = s + t2 * a2
+
+	// Closest point perpendicularity conditions.
+	// dot(p2 - p1, a1) = 0
+	// dot(p2 - p1, a2) = 0
+
+	// Expand
+	// dot(t1 * a1 - s - t2 * a2, a1) = 0
+	// dot(t1 * a1 - s - t2 * a2, a2) = 0
+
+	// Expand
+	// t1 - dot(s, a1) - t2 * dot(a1, a2) = 0
+	// t1 * dot(a1, a2) - dot(s, a2) - t2 = 0
+
+	// Group : a12 = dot(a1, a2), sa1 = dot(s, a1), sa2 = dot(s, a2)
+	// t1       - a12 * t2 = sa1
+	// a12 * t1 -       t2 = sa2
+
+	// Solve
+	// https://en.wikipedia.org/wiki/Cramer%27s_rule
+	// I've flipped the signs of the numerator and denominator to give a positive determinant.
+	// det = 1 - a12 * a12
+	// t1 = (sa1 - a12 * sa2) / det
+	// t2 = (a12 * sa1 - sa2) / det
+
+	b3Vec3 a1 = axis;
+	b3Vec3 a2 = rayAxis;
+	float a12 = b3Dot( a1, a2 );
+
+	// Ray distance to the near intersection with the infinite cylinder. Length units.
+	float tr;
+
+	float det = 1.0f - a12 * a12;
+	if ( det < FLT_EPSILON )
+	{
+		// Solve the 2D problem of ray versus circle starting at the ray origin, where the circle is
+		// the axial view of the infinite capsule cylinder. This works well when the ray origin is
+		// not too far from the capsule axis.
+
+		// Instead of a cross product, subtract the parallel part to get a perpendicular vector. Non-dimensional.
+		b3Vec3 perp = b3MulSub( a2, a12, a1 );
+		float perp2 = b3LengthSquared( perp );
+
+		// Project to origin to infinite capsule axis vector onto the perpendicular vector. beta has length units.
+		float beta = b3Dot( sc, perp );
+
+		// Setup quadratic root finder.
+		float gamma = sc2 - r * r;
+
+		// Discriminant
+		float disc = beta * beta - perp2 * gamma;
+
+		// Casting away from the axis, or the perpendicular gap never closes to the radius.
+		if ( beta >= 0.0f || disc < 0.0f )
+		{
+			return output;
+		}
+
+		// Quadratic near root. Expressed in an alternate form to avoid the (-beta - sqrt) cancellation as
+		// the ray nears parallel.
+		tr = gamma / ( -beta + sqrtf( disc ) );
+	}
+	else
+	{
+		// Ray and capsules axes are not parallel.
+
+		// Closest points between the infinite ray and the infinite capsule axis.
+		float invDet = 1.0f / det;
+		float sa1 = u;
+		float sa2 = b3Dot( s, a2 );
+
+		float t1 = ( sa1 - a12 * sa2 ) * invDet;
+		float t2 = ( a12 * sa1 - sa2 ) * invDet;
+
+		// Closest points
+		b3Vec3 p1 = b3MulSV( t1, a1 );
+		b3Vec3 p2 = b3MulAdd( s, t2, a2 );
+
+		// Vector from closest point on infinite capsule to infinite ray.
+		b3Vec3 g = b3Sub( p2, p1 );
+
+		float g2 = b3LengthSquared( g );
+		if ( g2 > r * r )
+		{
+			// Early out: closest point on infinite ray is outside infinite cylinder.
+			return output;
+		}
+
+		// Intersect the infinite ray with the infinite cylinder. Like ray versus sphere this is done
+		// relative to the closest point to avoid round-off errors. Length units, not a fraction.
+		// https://en.wikipedia.org/wiki/Line-cylinder_intersection
+		float h = sqrtf( ( r * r - g2 ) * invDet );
+
+		tr = t2 - h;
+	}
+
+	// Outside ray?
+	if ( tr < 0.0f || input->maxFraction * rayLength < tr )
+	{
+		return output;
+	}
+
+	// The corresponding distance on the capsule axis. Length units.
+	float tc = u + tr * a12;
+
+	// Outside c1 end?
+	if ( tc < 0.0f )
+	{
+		// Ray cast sphere 1.
+		b3Sphere sphere = {
+			.center = c1,
+			.radius = r,
+		};
+
+		return b3RayCastSphere( &sphere, input );
+	}
+
+	// Outside c2 end?
+	if (length < tc)
+	{
+		// Ray cast sphere 2.
+		b3Sphere sphere = {
+			.center = c2,
+			.radius = r,
+		};
+
+		return b3RayCastSphere( &sphere, input );
+	}
+
+	// Hit point on capsule side, relative to c1.
+	b3Vec3 p = b3MulAdd( s, tr, rayAxis );
+
+	// Hit normal.
+	b3Vec3 normal = b3MulSub( p, tc, axis );
+	normal = b3Normalize( normal );
+
+	output.point = b3Add( c1, p );
+	output.normal = normal;
+	output.fraction = b3ClampFloat( tr / rayLength, 0.0f, input->maxFraction );
+	output.hit = true;
+	return output;
+}
+
+b3CastOutput b3ShapeCastCapsule( const b3Capsule* capsule, const b3ShapeCastInput* input )
+{
+	b3ShapeCastPairInput pairInput;
+	pairInput.proxyA = (b3ShapeProxy){ &capsule->center1, 2, capsule->radius };
+	pairInput.proxyB = input->proxy;
+	pairInput.transform = b3Transform_identity;
+	pairInput.translationB = input->translation;
+	pairInput.maxFraction = input->maxFraction;
+	pairInput.canEncroach = input->canEncroach;
+
+	b3CastOutput output = b3ShapeCast( &pairInput );
+	return output;
+}
+
+int b3CollideMoverAndCapsule( b3PlaneResult* result, const b3Capsule* shape, const b3Capsule* mover )
+{
+	float totalRadius = mover->radius + shape->radius;
+
+	b3SegmentDistanceResult approach = b3SegmentDistance( shape->center1, shape->center2, mover->center1, mover->center2 );
+
+	// The normal points from the shape toward the mover.
+	float distance;
+	b3Vec3 normal = b3GetLengthAndNormalize( &distance, b3Sub( approach.point2, approach.point1 ) );
+
+	if ( distance > totalRadius )
+	{
+		return 0;
+	}
+
+	float linearSlop = B3_LINEAR_SLOP;
+	if ( distance < linearSlop )
+	{
+		// Deep overlap: the core segments intersect. Pick an arbitrary direction perpendicular
+		// the to capsule axis.
+		float moverLength;
+		b3Vec3 moverAxis = b3GetLengthAndNormalize( &moverLength, b3Sub( mover->center2, mover->center1 ) );
+		normal = moverLength > linearSlop ? b3Perp( moverAxis ) : b3Vec3_axisY;
+		distance = 0.0f;
+	}
+
+	b3Plane plane = { normal, totalRadius - distance };
+	*result = (b3PlaneResult){ plane, approach.point1 };
+	return 1;
+}

vendor/box3d/src/src/compound.h

@@ -0,0 +1,14 @@
+// SPDX-FileCopyrightText: 2025 Erin Catto
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include "box3d/types.h"
+
+b3TOIOutput b3CompoundTimeOfImpact( const b3CompoundData* compound, b3Transform transform, const b3ShapeProxy* proxy,
+									const b3Sweep* sweep, float maxFraction );
+
+// Transforms a sweep for a compound child shape
+b3Sweep b3MakeCompoundChildSweep( b3Transform compoundTransform, b3Transform childTransform );
+
+int b3CollideMoverAndCompound( b3PlaneResult* planes, int capacity, const b3CompoundData* shape, const b3Capsule* mover );

vendor/box3d/src/src/constraint_graph.c

@@ -0,0 +1,328 @@
+// SPDX-FileCopyrightText: 2025 Erin Catto
+// SPDX-License-Identifier: MIT
+
+#include "constraint_graph.h"
+
+#include "bitset.h"
+#include "body.h"
+#include "contact.h"
+#include "joint.h"
+#include "physics_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 B3_FORCE_OVERFLOW 0
+
+static const b3HexColor b3_graphColors[B3_GRAPH_COLOR_COUNT] = {
+	b3_colorRed,	   b3_colorOrange,		b3_colorYellow,			b3_colorLimeGreen,		 b3_colorSpringGreen,
+	b3_colorAqua,	   b3_colorDodgerBlue,	b3_colorBlueViolet,		b3_colorMagenta,		 b3_colorDeepPink,
+	b3_colorCrimson,   b3_colorCoral,		b3_colorGold,			b3_colorGreenYellow,	 b3_colorMediumSeaGreen,
+	b3_colorTurquoise, b3_colorDeepSkyBlue, b3_colorCornflowerBlue, b3_colorMediumSlateBlue, b3_colorMediumOrchid,
+	b3_colorHotPink,   b3_colorTomato,		b3_colorKhaki,			b3_colorSilver,
+};
+
+b3HexColor b3GetGraphColor( int index )
+{
+	B3_ASSERT( 0 <= index && index < B3_GRAPH_COLOR_COUNT );
+	return b3_graphColors[index];
+}
+
+void b3CreateGraph( b3ConstraintGraph* graph, int bodyCapacity )
+{
+	_Static_assert( B3_GRAPH_COLOR_COUNT >= 2, "must have at least two constraint graph colors" );
+	_Static_assert( B3_OVERFLOW_INDEX == B3_GRAPH_COLOR_COUNT - 1, "bad over flow index" );
+
+	*graph = (b3ConstraintGraph){ 0 };
+
+	bodyCapacity = b3MaxInt( bodyCapacity, 8 );
+
+	// Initialize graph color bit set.
+	// No bitset for overflow color.
+	for ( int i = 0; i < B3_OVERFLOW_INDEX; ++i )
+	{
+		b3GraphColor* color = graph->colors + i;
+		color->bodySet = b3CreateBitSet( bodyCapacity );
+		b3SetBitCountAndClear( &color->bodySet, bodyCapacity );
+	}
+}
+
+void b3DestroyGraph( b3ConstraintGraph* graph )
+{
+	for ( int i = 0; i < B3_GRAPH_COLOR_COUNT; ++i )
+	{
+		b3GraphColor* color = graph->colors + i;
+
+		// The bit set should never be used on the overflow color
+		B3_ASSERT( i != B3_OVERFLOW_INDEX || color->bodySet.bits == NULL );
+
+		b3DestroyBitSet( &color->bodySet );
+
+		b3Array_Destroy( color->convexContacts );
+		b3Array_Destroy( color->contacts );
+		b3Array_Destroy( color->jointSims );
+	}
+}
+
+// Contacts are always created as non-touching. They get cloned into the constraint
+// graph once they are found to be touching.
+void b3AddContactToGraph( b3World* world, b3Contact* contact )
+{
+	B3_ASSERT( contact->manifoldCount > 0 );
+	B3_ASSERT( contact->flags & b3_contactTouchingFlag );
+
+	b3ConstraintGraph* graph = &world->constraintGraph;
+	int colorIndex = B3_OVERFLOW_INDEX;
+
+	int bodyIdA = contact->edges[0].bodyId;
+	int bodyIdB = contact->edges[1].bodyId;
+	b3Body* bodyA = b3Array_Get( world->bodies, bodyIdA );
+	b3Body* bodyB = b3Array_Get( world->bodies, bodyIdB );
+
+	b3BodyType typeA = bodyA->type;
+	b3BodyType typeB = bodyB->type;
+	B3_ASSERT( typeA == b3_dynamicBody || typeB == b3_dynamicBody );
+
+#if B3_FORCE_OVERFLOW == 0
+	if ( typeA == b3_dynamicBody && typeB == b3_dynamicBody )
+	{
+		// Dynamic constraint colors cannot encroach on colors reserved for static constraints
+		for ( int i = 0; i < B3_DYNAMIC_COLOR_COUNT; ++i )
+		{
+			b3GraphColor* color = graph->colors + i;
+			if ( b3GetBit( &color->bodySet, bodyIdA ) || b3GetBit( &color->bodySet, bodyIdB ) )
+			{
+				continue;
+			}
+
+			b3SetBitGrow( &color->bodySet, bodyIdA );
+			b3SetBitGrow( &color->bodySet, bodyIdB );
+			colorIndex = i;
+			break;
+		}
+	}
+	else if ( typeA == b3_dynamicBody )
+	{
+		// Static constraint colors build from the end to get higher priority than dyn-dyn constraints
+		for ( int i = B3_OVERFLOW_INDEX - 1; i >= 1; --i )
+		{
+			b3GraphColor* color = graph->colors + i;
+			if ( b3GetBit( &color->bodySet, bodyIdA ) )
+			{
+				continue;
+			}
+
+			b3SetBitGrow( &color->bodySet, bodyIdA );
+			colorIndex = i;
+			break;
+		}
+	}
+	else if ( typeB == b3_dynamicBody )
+	{
+		// Static constraint colors build from the end to get higher priority than dyn-dyn constraints
+		for ( int i = B3_OVERFLOW_INDEX - 1; i >= 1; --i )
+		{
+			b3GraphColor* color = graph->colors + i;
+			if ( b3GetBit( &color->bodySet, bodyIdB ) )
+			{
+				continue;
+			}
+
+			b3SetBitGrow( &color->bodySet, bodyIdB );
+			colorIndex = i;
+			break;
+		}
+	}
+#endif
+
+	bool isScalar = ( contact->flags & b3_simMeshContact ) || colorIndex == B3_OVERFLOW_INDEX;
+
+	b3GraphColor* color = graph->colors + colorIndex;
+	contact->colorIndex = colorIndex;
+	contact->localIndex = isScalar ? color->contacts.count : color->convexContacts.count;
+	contact->bodySimIndexA = bodyA->type == b3_staticBody ? B3_NULL_INDEX : bodyA->localIndex;
+	contact->bodySimIndexB = bodyB->type == b3_staticBody ? B3_NULL_INDEX : bodyB->localIndex;
+
+	if ( isScalar )
+	{
+		B3_ASSERT( contact->manifoldCount < UINT16_MAX );
+		b3ContactSpec spec = {
+			.contactId = contact->contactId,
+			.manifoldStart = 0,
+			.manifoldCount = (uint16_t)contact->manifoldCount,
+		};
+		b3Array_Push( color->contacts, spec );
+	}
+	else
+	{
+		b3Array_Push( color->convexContacts, contact->contactId );
+	}
+}
+
+void b3RemoveContactFromGraph( b3World* world, int bodyIdA, int bodyIdB, int colorIndex, int localIndex, bool meshContact )
+{
+	b3ConstraintGraph* graph = &world->constraintGraph;
+
+	B3_ASSERT( 0 <= colorIndex && colorIndex < B3_GRAPH_COLOR_COUNT );
+	b3GraphColor* color = graph->colors + colorIndex;
+
+	if ( colorIndex != B3_OVERFLOW_INDEX )
+	{
+		// This might clear a bit for a static body, but this has no effect
+		b3ClearBit( &color->bodySet, bodyIdA );
+		b3ClearBit( &color->bodySet, bodyIdB );
+	}
+
+	if ( meshContact || colorIndex == B3_OVERFLOW_INDEX )
+	{
+		int movedIndex = b3Array_RemoveSwap( color->contacts, localIndex );
+		if ( movedIndex != B3_NULL_INDEX )
+		{
+			// Fix index on swapped contact
+			int movedContactId = color->contacts.data[localIndex].contactId;
+			b3Contact* movedContact = b3Array_Get( world->contacts, movedContactId );
+			B3_ASSERT( movedContact->setIndex == b3_awakeSet );
+			B3_ASSERT( movedContact->colorIndex == colorIndex );
+			B3_ASSERT( movedContact->localIndex == movedIndex );
+			movedContact->localIndex = localIndex;
+		}
+	}
+	else
+	{
+		int movedIndex = b3Array_RemoveSwap( color->convexContacts, localIndex );
+		if ( movedIndex != B3_NULL_INDEX )
+		{
+			// Fix index on swapped contact
+			int movedContactId = color->convexContacts.data[localIndex];
+			b3Contact* movedContact = b3Array_Get( world->contacts, movedContactId );
+			B3_ASSERT( movedContact->setIndex == b3_awakeSet );
+			B3_ASSERT( movedContact->colorIndex == colorIndex );
+			B3_ASSERT( movedContact->localIndex == movedIndex );
+			B3_ASSERT( ( movedContact->flags & b3_simMeshContact ) == 0 );
+			movedContact->localIndex = localIndex;
+		}
+	}
+}
+
+static int b3AssignJointColor( b3ConstraintGraph* graph, int bodyIdA, int bodyIdB, b3BodyType typeA, b3BodyType typeB )
+{
+	B3_ASSERT( typeA == b3_dynamicBody || typeB == b3_dynamicBody );
+	B3_UNUSED( graph );
+	B3_UNUSED( bodyIdA );
+	B3_UNUSED( bodyIdB );
+	B3_UNUSED( typeA );
+	B3_UNUSED( typeB );
+
+#if B3_FORCE_OVERFLOW == 0
+	if ( typeA == b3_dynamicBody && typeB == b3_dynamicBody )
+	{
+		// Dynamic constraint colors cannot encroach on colors reserved for static constraints
+		for ( int i = 0; i < B3_DYNAMIC_COLOR_COUNT; ++i )
+		{
+			b3GraphColor* color = graph->colors + i;
+			if ( b3GetBit( &color->bodySet, bodyIdA ) || b3GetBit( &color->bodySet, bodyIdB ) )
+			{
+				continue;
+			}
+
+			b3SetBitGrow( &color->bodySet, bodyIdA );
+			b3SetBitGrow( &color->bodySet, bodyIdB );
+			return i;
+		}
+	}
+	else if ( typeA == b3_dynamicBody )
+	{
+		// Static constraint colors build from the end to get higher priority than dyn-dyn constraints
+		for ( int i = B3_OVERFLOW_INDEX - 1; i >= 1; --i )
+		{
+			b3GraphColor* color = graph->colors + i;
+			if ( b3GetBit( &color->bodySet, bodyIdA ) )
+			{
+				continue;
+			}
+
+			b3SetBitGrow( &color->bodySet, bodyIdA );
+			return i;
+		}
+	}
+	else if ( typeB == b3_dynamicBody )
+	{
+		// Static constraint colors build from the end to get higher priority than dyn-dyn constraints
+		for ( int i = B3_OVERFLOW_INDEX - 1; i >= 1; --i )
+		{
+			b3GraphColor* color = graph->colors + i;
+			if ( b3GetBit( &color->bodySet, bodyIdB ) )
+			{
+				continue;
+			}
+
+			b3SetBitGrow( &color->bodySet, bodyIdB );
+			return i;
+		}
+	}
+#endif
+
+	return B3_OVERFLOW_INDEX;
+}
+
+b3JointSim* b3CreateJointInGraph( b3World* world, b3Joint* joint )
+{
+	b3ConstraintGraph* graph = &world->constraintGraph;
+
+	int bodyIdA = joint->edges[0].bodyId;
+	int bodyIdB = joint->edges[1].bodyId;
+	b3Body* bodyA = b3Array_Get( world->bodies, bodyIdA );
+	b3Body* bodyB = b3Array_Get( world->bodies, bodyIdB );
+
+	int colorIndex = b3AssignJointColor( graph, bodyIdA, bodyIdB, bodyA->type, bodyB->type );
+
+	b3JointSim* jointSim = b3Array_Emplace( graph->colors[colorIndex].jointSims );
+	memset( jointSim, 0, sizeof( b3JointSim ) );
+
+	joint->colorIndex = colorIndex;
+	joint->localIndex = graph->colors[colorIndex].jointSims.count - 1;
+	return jointSim;
+}
+
+void b3AddJointToGraph( b3World* world, b3JointSim* jointSim, b3Joint* joint )
+{
+	b3JointSim* jointDst = b3CreateJointInGraph( world, joint );
+	memcpy( jointDst, jointSim, sizeof( b3JointSim ) );
+}
+
+void b3RemoveJointFromGraph( b3World* world, int bodyIdA, int bodyIdB, int colorIndex, int localIndex )
+{
+	b3ConstraintGraph* graph = &world->constraintGraph;
+
+	B3_ASSERT( 0 <= colorIndex && colorIndex < B3_GRAPH_COLOR_COUNT );
+	b3GraphColor* color = graph->colors + colorIndex;
+
+	if ( colorIndex != B3_OVERFLOW_INDEX )
+	{
+		// May clear static bodies, no effect
+		b3ClearBit( &color->bodySet, bodyIdA );
+		b3ClearBit( &color->bodySet, bodyIdB );
+	}
+
+	int movedIndex = b3Array_RemoveSwap( color->jointSims, localIndex );
+	if ( movedIndex != B3_NULL_INDEX )
+	{
+		// Fix moved joint
+		b3JointSim* movedJointSim = color->jointSims.data + localIndex;
+		int movedId = movedJointSim->jointId;
+		b3Joint* movedJoint = b3Array_Get( world->joints, movedId );
+		B3_ASSERT( movedJoint->setIndex == b3_awakeSet );
+		B3_ASSERT( movedJoint->colorIndex == colorIndex );
+		B3_ASSERT( movedJoint->localIndex == movedIndex );
+		movedJoint->localIndex = localIndex;
+	}
+}

vendor/box3d/src/src/constraint_graph.h

@@ -0,0 +1,77 @@
+// SPDX-FileCopyrightText: 2025 Erin Catto
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include "bitset.h"
+#include "contact.h"
+#include "container.h"
+#include "solver.h"
+#include "solver_set.h"
+#include "box3d/constants.h"
+
+typedef struct b3Body b3Body;
+typedef struct b3Contact b3Contact;
+typedef struct b3JointSim b3JointSim;
+typedef struct b3Joint b3Joint;
+typedef struct b3StepContext b3StepContext;
+typedef struct b3World b3World;
+
+// This holds constraints that cannot fit the graph color limit. This happens when a single dynamic body
+// is touching many other bodies.
+#define B3_OVERFLOW_INDEX ( B3_GRAPH_COLOR_COUNT - 1 )
+
+// This keeps constraints involving two dynamic bodies at a lower solver priority than constraints
+// involving a dynamic and static bodies. This reduces tunneling due to push through.
+#define B3_DYNAMIC_COLOR_COUNT ( B3_GRAPH_COLOR_COUNT - 4 )
+
+// todo optimize mesh contact constraints
+// They can be lumped with convex contacts and I can use the bitset event to re-link if the manifold count increases
+// Could also create a group for two wide manifolds and use bitset event
+// This could create ping-pong jitter so may need to pin to high water mark or introduce hysteresis somehow
+//
+// Dirk has the idea to do graph coloring based on manifolds. This suggests mesh contact will have manifolds
+// in multiple graph colors. So each manifold with have a color and local index.
+// Some concerns about this:
+// - manifolds don't have a strong identity, would this affect stability/jitter?
+// - this creates a lot of static graph colors and can overflow
+
+typedef struct b3GraphColor
+{
+	// 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.
+	b3BitSet bodySet;
+
+	// cache friendly arrays
+	b3Array( b3JointSim ) jointSims;
+
+	b3Array( int ) convexContacts;
+	b3Array( b3ContactSpec ) contacts;
+
+	// These are used for convex contacts
+	struct b3ContactConstraintWide* wideConstraints;
+	int wideConstraintCount;
+
+	// These are used for mesh and overflow contacts
+	struct b3ManifoldConstraint* manifoldConstraints;
+	int manifoldConstraintCount;
+	struct b3ContactConstraint* contactConstraints;
+	int contactConstraintCount;
+} b3GraphColor;
+
+typedef struct b3ConstraintGraph
+{
+	// including overflow at the end
+	b3GraphColor colors[B3_GRAPH_COLOR_COUNT];
+} b3ConstraintGraph;
+
+void b3CreateGraph( b3ConstraintGraph* graph, int bodyCapacity );
+void b3DestroyGraph( b3ConstraintGraph* graph );
+
+void b3AddContactToGraph( b3World* world, b3Contact* contact );
+void b3RemoveContactFromGraph( b3World* world, int bodyIdA, int bodyIdB, int colorIndex, int localIndex, bool meshContact );
+
+b3JointSim* b3CreateJointInGraph( b3World* world, b3Joint* joint );
+void b3AddJointToGraph( b3World* world, b3JointSim* jointSim, b3Joint* joint );
+void b3RemoveJointFromGraph( b3World* world, int bodyIdA, int bodyIdB, int colorIndex, int localIndex );

vendor/box3d/src/src/contact.c

@@ -0,0 +1,873 @@
+// SPDX-FileCopyrightText: 2025 Erin Catto
+// SPDX-License-Identifier: MIT
+
+#include "contact.h"
+
+#include "algorithm.h"
+#include "body.h"
+#include "compound.h"
+#include "island.h"
+#include "manifold.h"
+#include "physics_world.h"
+#include "shape.h"
+#include "solver_set.h"
+#include "table.h"
+
+#include "box3d/box3d.h"
+
+// Contacts and determinism
+// A deterministic simulation requires contacts to exist in the same order in b3Island 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 b3UpdateBroadPhasePairs
+// - 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:
+// b3ManifoldPoint::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 b3Manifold b3ManifoldFcn( const b3Shape* shapeA, b3Transform xfA, const b3Shape* shapeB, b3Transform xfB,
+//								  b3ContactCache* cache );
+
+static b3Contact* b3GetContactFullId( b3World* world, b3ContactId contactId )
+{
+	int id = contactId.index1 - 1;
+	b3Contact* contact = b3Array_Get( world->contacts, id );
+	B3_ASSERT( contact->contactId == id && contact->generation == contactId.generation );
+	return contact;
+}
+
+b3ContactData b3Contact_GetData( b3ContactId contactId )
+{
+	b3World* world = b3GetWorld( contactId.world0 );
+	b3Contact* contact = b3GetContactFullId( world, contactId );
+
+	const b3Shape* shapeA = b3Array_Get( world->shapes, contact->shapeIdA );
+	const b3Shape* shapeB = b3Array_Get( world->shapes, contact->shapeIdB );
+
+	b3ContactData data = { 0 };
+	data.contactId = contactId;
+	data.shapeIdA = (b3ShapeId){
+		.index1 = shapeA->id + 1,
+		.world0 = contactId.world0,
+		.generation = shapeA->generation,
+	};
+	data.shapeIdB = (b3ShapeId){
+		.index1 = shapeB->id + 1,
+		.world0 = contactId.world0,
+		.generation = shapeB->generation,
+	};
+
+	if ( contact->manifoldCount > 0 )
+	{
+		data.manifolds = contact->manifolds;
+		data.manifoldCount = contact->manifoldCount;
+	}
+	else
+	{
+		data.manifolds = NULL;
+		data.manifoldCount = 0;
+	}
+
+	return data;
+}
+
+struct b3ContactRegister
+{
+	// b3ManifoldFcn* fcn;
+	bool supported;
+	bool primary;
+};
+
+static struct b3ContactRegister s_registers[b3_shapeTypeCount][b3_shapeTypeCount];
+static bool s_initialized = false;
+
+static void b3AddType( b3ShapeType type1, b3ShapeType type2 )
+{
+	B3_ASSERT( 0 <= type1 && type1 < b3_shapeTypeCount );
+	B3_ASSERT( 0 <= type2 && type2 < b3_shapeTypeCount );
+
+	s_registers[type1][type2].supported = true;
+	s_registers[type1][type2].primary = true;
+
+	if ( type1 != type2 )
+	{
+		s_registers[type2][type1].supported = true;
+		s_registers[type2][type1].primary = false;
+	}
+}
+
+void b3InitializeContactRegisters( void )
+{
+	if ( s_initialized == false )
+	{
+		b3AddType( b3_sphereShape, b3_sphereShape );
+		b3AddType( b3_capsuleShape, b3_sphereShape );
+		b3AddType( b3_capsuleShape, b3_capsuleShape );
+		b3AddType( b3_compoundShape, b3_sphereShape );
+		b3AddType( b3_compoundShape, b3_capsuleShape );
+		b3AddType( b3_compoundShape, b3_hullShape );
+		b3AddType( b3_hullShape, b3_sphereShape );
+		b3AddType( b3_hullShape, b3_capsuleShape );
+		b3AddType( b3_hullShape, b3_hullShape );
+		b3AddType( b3_meshShape, b3_sphereShape );
+		b3AddType( b3_meshShape, b3_capsuleShape );
+		b3AddType( b3_meshShape, b3_hullShape );
+		b3AddType( b3_heightShape, b3_sphereShape );
+		b3AddType( b3_heightShape, b3_capsuleShape );
+		b3AddType( b3_heightShape, b3_hullShape );
+		s_initialized = true;
+	}
+}
+
+void b3CreateContact( b3World* world, b3Shape* shapeA, b3Shape* shapeB, int childIndex )
+{
+	b3ShapeType typeA = shapeA->type;
+	b3ShapeType typeB = shapeB->type;
+
+	B3_ASSERT( 0 <= typeA && typeA < b3_shapeTypeCount );
+	B3_ASSERT( 0 <= typeB && typeB < b3_shapeTypeCount );
+
+	if ( s_registers[typeA][typeB].supported == false )
+	{
+		// For example, no mesh vs mesh collision
+		return;
+	}
+
+	if ( s_registers[typeA][typeB].primary == false )
+	{
+		// flip order
+		b3CreateContact( world, shapeB, shapeA, childIndex );
+		return;
+	}
+
+	b3Body* bodyA = b3Array_Get( world->bodies, shapeA->bodyId );
+	b3Body* bodyB = b3Array_Get( world->bodies, shapeB->bodyId );
+
+	B3_ASSERT( bodyA->setIndex != b3_disabledSet && bodyB->setIndex != b3_disabledSet );
+	B3_ASSERT( bodyA->setIndex != b3_staticSet || bodyB->setIndex != b3_staticSet );
+
+	int setIndex;
+	if ( bodyA->setIndex == b3_awakeSet || bodyB->setIndex == b3_awakeSet )
+	{
+		setIndex = b3_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
+
+		// This is possible if a shape moves slightly then falls asleep
+		setIndex = b3_disabledSet;
+	}
+
+	b3SolverSet* set = b3Array_Get( world->solverSets, setIndex );
+
+	// Create contact key and contact
+	int contactId = b3AllocId( &world->contactIdPool );
+	if ( contactId == world->contacts.count )
+	{
+		b3Contact emptyContact = { 0 };
+		b3Array_Push( world->contacts, emptyContact );
+	}
+
+	int shapeIdA = shapeA->id;
+	int shapeIdB = shapeB->id;
+
+	b3Contact* contact = b3Array_Get( world->contacts, contactId );
+	int generation = contact->generation;
+	*contact = (b3Contact){ 0 };
+	contact->contactId = contactId;
+	contact->generation = generation + 1;
+	contact->setIndex = setIndex;
+	contact->colorIndex = B3_NULL_INDEX;
+	contact->localIndex = set->contactIndices.count;
+	contact->islandId = B3_NULL_INDEX;
+	contact->islandIndex = B3_NULL_INDEX;
+	contact->shapeIdA = shapeIdA;
+	contact->shapeIdB = shapeIdB;
+	contact->childIndex = childIndex;
+
+	// Both bodies must enable recycling
+	if ( ( bodyA->flags & b3_bodyEnableContactRecycling ) != 0 && ( bodyB->flags & b3_bodyEnableContactRecycling ) != 0 )
+	{
+		contact->flags |= b3_contactRecycleFlag;
+	}
+
+	if ( shapeA->type == b3_meshShape || shapeA->type == b3_heightShape )
+	{
+		contact->flags |= b3_simMeshContact;
+	}
+	else if ( shapeA->type == b3_compoundShape )
+	{
+		b3ChildShape child = b3GetCompoundChild( shapeA->compound, childIndex );
+		if ( child.type == b3_meshShape )
+		{
+			contact->flags |= b3_simMeshContact;
+		}
+	}
+
+	// todo impose these restrictions to make life easier
+	B3_ASSERT( shapeB->type == b3_sphereShape || shapeB->type == b3_capsuleShape || shapeB->type == b3_hullShape );
+	// B3_ASSERT( bodyB->type != b3_staticBody );
+
+	// Is either body static?
+	// Note: it is possible to have a dynamic mesh collide with a static convex shape. Maybe I should disallow this.
+	if ( bodyA->type == b3_staticBody || bodyB->type == b3_staticBody )
+	{
+		contact->flags |= b3_contactStaticFlag;
+	}
+
+	B3_ASSERT( shapeA->sensorIndex == B3_NULL_INDEX && shapeB->sensorIndex == B3_NULL_INDEX );
+
+	if ( ( shapeA->flags & b3_enableContactEvents ) || ( shapeB->flags & b3_enableContactEvents ) )
+	{
+		contact->flags |= b3_contactEnableContactEvents;
+	}
+
+	if ( ( shapeA->flags & b3_enableSpeculative ) && ( shapeB->flags & b3_enableSpeculative ) )
+	{
+		contact->flags |= b3_enableSpeculativePoints;
+	}
+
+	// Connect to body A
+	{
+		contact->edges[0].bodyId = shapeA->bodyId;
+		contact->edges[0].prevKey = B3_NULL_INDEX;
+		contact->edges[0].nextKey = bodyA->headContactKey;
+
+		int keyA = ( contactId << 1 ) | 0;
+		int headContactKey = bodyA->headContactKey;
+		if ( headContactKey != B3_NULL_INDEX )
+		{
+			b3Contact* headContact = b3Array_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 = B3_NULL_INDEX;
+		contact->edges[1].nextKey = bodyB->headContactKey;
+
+		int keyB = ( contactId << 1 ) | 1;
+		int headContactKey = bodyB->headContactKey;
+		if ( bodyB->headContactKey != B3_NULL_INDEX )
+		{
+			b3Contact* headContact = b3Array_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 = b3ShapePairKey( shapeIdA, shapeIdB, childIndex );
+	b3AddKey( &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.
+	b3Array_Push( set->contactIndices, contactId );
+
+	float radiusA = 0.0f;
+	if ( typeA == b3_sphereShape )
+	{
+		radiusA = shapeA->sphere.radius;
+	}
+	else if ( typeA == b3_capsuleShape )
+	{
+		radiusA = shapeA->capsule.radius;
+	}
+
+	float radiusB = 0.0f;
+	if ( typeB == b3_sphereShape )
+	{
+		radiusB = shapeB->sphere.radius;
+	}
+	else if ( typeB == b3_capsuleShape )
+	{
+		radiusB = shapeB->capsule.radius;
+	}
+
+	float maxRadius = b3MaxFloat( radiusA, radiusB );
+
+	// Assuming the rolling resistance doesn't change
+	contact->rollingResistance =
+		b3MaxFloat( b3GetShapeMaterials( shapeA )[0].rollingResistance, b3GetShapeMaterials( shapeB )[0].rollingResistance ) *
+		maxRadius;
+
+	if ( ( shapeA->flags & b3_enablePreSolveEvents ) || ( shapeB->flags & b3_enablePreSolveEvents ) )
+	{
+		contact->flags |= b3_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 b3DestroyContact( b3World* world, b3Contact* contact, bool wakeBodies )
+{
+	// Remove pair from set
+	uint64_t pairKey = b3ShapePairKey( contact->shapeIdA, contact->shapeIdB, contact->childIndex );
+	b3RemoveKey( &world->broadPhase.pairSet, pairKey );
+
+	b3FreeManifolds( world, contact->manifolds, contact->manifoldCount );
+	contact->manifolds = NULL;
+	contact->manifoldCount = 0;
+
+	b3ContactEdge* edgeA = contact->edges + 0;
+	b3ContactEdge* edgeB = contact->edges + 1;
+
+	int bodyIdA = edgeA->bodyId;
+	int bodyIdB = edgeB->bodyId;
+	b3Body* bodyA = b3Array_Get( world->bodies, bodyIdA );
+	b3Body* bodyB = b3Array_Get( world->bodies, bodyIdB );
+
+	uint32_t flags = contact->flags;
+	bool touching = ( flags & b3_contactTouchingFlag ) != 0;
+
+	// End touch event
+	if ( touching && ( flags & b3_contactEnableContactEvents ) != 0 )
+	{
+		uint16_t worldId = world->worldId;
+		const b3Shape* shapeA = b3Array_Get( world->shapes, contact->shapeIdA );
+		const b3Shape* shapeB = b3Array_Get( world->shapes, contact->shapeIdB );
+		b3ShapeId shapeIdA = { shapeA->id + 1, worldId, shapeA->generation };
+		b3ShapeId shapeIdB = { shapeB->id + 1, worldId, shapeB->generation };
+
+		b3ContactId contactId = {
+			.index1 = contact->contactId + 1,
+			.world0 = world->worldId,
+			.padding = 0,
+			.generation = contact->generation,
+		};
+
+		b3ContactEndTouchEvent event = {
+			.shapeIdA = shapeIdA,
+			.shapeIdB = shapeIdB,
+			.contactId = contactId,
+		};
+
+		b3Array_Push( world->contactEndEvents[world->endEventArrayIndex], event );
+	}
+
+	// Remove from body A
+	if ( edgeA->prevKey != B3_NULL_INDEX )
+	{
+		b3Contact* prevContact = b3Array_Get( world->contacts, edgeA->prevKey >> 1 );
+		b3ContactEdge* prevEdge = prevContact->edges + ( edgeA->prevKey & 1 );
+		prevEdge->nextKey = edgeA->nextKey;
+	}
+
+	if ( edgeA->nextKey != B3_NULL_INDEX )
+	{
+		b3Contact* nextContact = b3Array_Get( world->contacts, edgeA->nextKey >> 1 );
+		b3ContactEdge* 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 != B3_NULL_INDEX )
+	{
+		b3Contact* prevContact = b3Array_Get( world->contacts, edgeB->prevKey >> 1 );
+		b3ContactEdge* prevEdge = prevContact->edges + ( edgeB->prevKey & 1 );
+		prevEdge->nextKey = edgeB->nextKey;
+	}
+
+	if ( edgeB->nextKey != B3_NULL_INDEX )
+	{
+		b3Contact* nextContact = b3Array_Get( world->contacts, edgeB->nextKey >> 1 );
+		b3ContactEdge* 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;
+
+	if ( contact->flags & b3_simMeshContact )
+	{
+		b3Array_Destroy( contact->meshContact.triangleCache );
+	}
+
+	// Remove contact from the array that owns it
+	if ( contact->islandId != B3_NULL_INDEX )
+	{
+		b3UnlinkContact( world, contact );
+	}
+
+	if ( contact->colorIndex != B3_NULL_INDEX )
+	{
+		// contact is an active constraint
+		B3_ASSERT( contact->setIndex == b3_awakeSet );
+		bool meshContact = contact->flags & b3_simMeshContact;
+		b3RemoveContactFromGraph( world, bodyIdA, bodyIdB, contact->colorIndex, contact->localIndex, meshContact );
+	}
+	else
+	{
+		// contact is non-touching or is sleeping or is a sensor
+		B3_ASSERT( contact->setIndex != b3_awakeSet || ( contact->flags & b3_contactTouchingFlag ) == 0 );
+		b3SolverSet* set = b3Array_Get( world->solverSets, contact->setIndex );
+
+		int localIndex = contact->localIndex;
+		int movedIndex = b3Array_RemoveSwap( set->contactIndices, localIndex );
+		if ( movedIndex != B3_NULL_INDEX )
+		{
+			int movedContactIndex = set->contactIndices.data[localIndex];
+			b3Contact* movedContact = b3Array_Get( world->contacts, movedContactIndex );
+			movedContact->localIndex = localIndex;
+		}
+	}
+
+	// Free contact and id (preserve generation)
+	contact->contactId = B3_NULL_INDEX;
+	contact->setIndex = B3_NULL_INDEX;
+	contact->colorIndex = B3_NULL_INDEX;
+	contact->localIndex = B3_NULL_INDEX;
+	b3FreeId( &world->contactIdPool, contactId );
+
+	if ( wakeBodies && touching )
+	{
+		b3WakeBody( world, bodyA );
+		b3WakeBody( world, bodyB );
+	}
+}
+
+static bool b3ComputeConvexManifold( b3World* world, int workerIndex, b3Contact* contact, const b3Shape* shapeA,
+									 b3WorldTransform xfA, const b3Shape* shapeB, b3WorldTransform xfB, b3Arena arena )
+{
+	b3ShapeType typeA = shapeA->type;
+	b3ShapeType typeB = shapeB->type;
+
+	b3ContactCache* cache = &contact->convexContact.cache;
+
+	int pointCapacity = 32;
+	b3LocalManifoldPoint* pointBuffer = (b3LocalManifoldPoint*)b3Bump( &arena, pointCapacity * sizeof( b3LocalManifoldPoint ) );
+
+	b3LocalManifold geomManifold = { 0 };
+	geomManifold.points = pointBuffer;
+
+	b3Transform transformBtoA = b3InvMulWorldTransforms( xfA, xfB );
+
+	if ( typeA == b3_sphereShape )
+	{
+		B3_ASSERT( typeB == b3_sphereShape );
+		b3CollideSpheres( &geomManifold, pointCapacity, &shapeA->sphere, &shapeB->sphere, transformBtoA );
+	}
+	else if ( typeA == b3_capsuleShape )
+	{
+		if ( typeB == b3_sphereShape )
+		{
+			b3CollideCapsuleAndSphere( &geomManifold, pointCapacity, &shapeA->capsule, &shapeB->sphere, transformBtoA );
+		}
+		else
+		{
+			B3_ASSERT( typeB == b3_capsuleShape );
+			b3CollideCapsules( &geomManifold, pointCapacity, &shapeA->capsule, &shapeB->capsule, transformBtoA );
+		}
+	}
+	else
+	{
+		B3_ASSERT( typeA == b3_hullShape );
+
+		if ( typeB == b3_sphereShape )
+		{
+			b3CollideHullAndSphere( &geomManifold, pointCapacity, shapeA->hull, &shapeB->sphere, transformBtoA,
+									&cache->simplexCache );
+		}
+		else if ( typeB == b3_capsuleShape )
+		{
+			b3CollideHullAndCapsule( &geomManifold, pointCapacity, shapeA->hull, &shapeB->capsule, transformBtoA,
+									 &cache->simplexCache );
+		}
+		else
+		{
+			B3_ASSERT( typeB == b3_hullShape );
+			b3CollideHulls( &geomManifold, pointCapacity, shapeA->hull, shapeB->hull, transformBtoA, &cache->satCache );
+			world->taskContexts.data[workerIndex].satCallCount += 1;
+			world->taskContexts.data[workerIndex].satCacheHitCount += cache->satCache.hit;
+		}
+	}
+
+	if ( geomManifold.pointCount == 0 )
+	{
+		if ( contact->manifoldCount > 0 )
+		{
+			b3FreeManifolds( world, contact->manifolds, contact->manifoldCount );
+			contact->manifolds = NULL;
+			contact->manifoldCount = 0;
+		}
+
+		return false;
+	}
+
+	b3ManifoldPoint oldPoints[B3_MAX_MANIFOLD_POINTS];
+	int oldCount = 0;
+
+	if ( contact->manifoldCount == 0 )
+	{
+		contact->manifolds = b3AllocateManifolds( world, 1 );
+		contact->manifoldCount = 1;
+	}
+	else
+	{
+		oldCount = contact->manifolds[0].pointCount;
+		memcpy( oldPoints, contact->manifolds[0].points, oldCount * sizeof( b3ManifoldPoint ) );
+	}
+
+	b3Manifold* manifold = contact->manifolds;
+	manifold->pointCount = geomManifold.pointCount;
+
+	b3Matrix3 matrixA = b3MakeMatrixFromQuat( xfA.q );
+	manifold->normal = b3MulMV( matrixA, geomManifold.normal );
+
+	// Store point data in contact
+	for ( int i = 0; i < geomManifold.pointCount; ++i )
+	{
+		const b3LocalManifoldPoint* source = geomManifold.points + i;
+		b3ManifoldPoint* target = manifold->points + i;
+
+		// Contact points are computed in frame A
+		target->anchorA = b3MulMV( matrixA, source->point );
+		target->anchorB = b3Add( target->anchorA, b3SubPos( xfA.p, xfB.p ) );
+		target->separation = source->separation;
+		target->featureId = b3MakeFeatureId( source->pair );
+		target->triangleIndex = B3_NULL_INDEX;
+		target->normalVelocity = 0.0f;
+	}
+
+	// Copy impulses from old points
+	for ( int i = 0; i < geomManifold.pointCount; ++i )
+	{
+		b3ManifoldPoint* pt2 = manifold->points + i;
+		pt2->totalNormalImpulse = 0.0f;
+		pt2->persisted = false;
+
+		for ( int j = 0; j < oldCount; ++j )
+		{
+			b3ManifoldPoint* pt1 = oldPoints + j;
+
+			if ( pt2->featureId == pt1->featureId )
+			{
+				pt2->normalImpulse = pt1->normalImpulse;
+				pt2->persisted = true;
+
+				// claimed
+				pt1->featureId = UINT32_MAX;
+
+				break;
+			}
+		}
+
+		if ( pt2->persisted == false )
+		{
+			pt2->normalImpulse = 0.0f;
+		}
+	}
+
+	return true;
+}
+
+static bool b3UpdateConvexContact( b3World* world, int workerIndex, b3Contact* contact, b3Shape* shapeA, b3WorldTransform xfA,
+								   b3Shape* shapeB, b3WorldTransform xfB, bool flip, b3Arena arena )
+{
+	// Compute new manifold
+	bool touching = b3ComputeConvexManifold( world, workerIndex, contact, shapeA, xfA, shapeB, xfB, arena );
+
+	if ( touching == false )
+	{
+		B3_ASSERT( contact->manifolds == NULL && contact->manifoldCount == 0 );
+		return false;
+	}
+
+	B3_ASSERT( contact->manifoldCount == 1 );
+
+	if ( flip )
+	{
+		// Not flipping the feature ids because they just need to match and flipping is consistent.
+		b3Manifold* manifold = contact->manifolds + 0;
+		manifold->normal = b3Neg( manifold->normal );
+		int pointCount = manifold->pointCount;
+		for ( int i = 0; i < pointCount; ++i )
+		{
+			b3ManifoldPoint* mp = manifold->points + i;
+			B3_SWAP( mp->anchorA, mp->anchorB );
+		}
+	}
+
+	const b3SurfaceMaterial* materialA = b3GetShapeMaterials( shapeA );
+	const b3SurfaceMaterial* materialB = b3GetShapeMaterials( shapeB );
+
+	// Keep these updated in case the values on the shapes are modified
+	contact->friction =
+		world->frictionCallback( materialA->friction, materialA->userMaterialId, materialB->friction, materialB->userMaterialId );
+	contact->restitution = world->restitutionCallback( materialA->restitution, materialA->userMaterialId, materialB->restitution,
+													   materialB->userMaterialId );
+
+	if ( materialA->rollingResistance > 0.0f || materialB->rollingResistance > 0.0f )
+	{
+		b3ShapeType typeA = shapeA->type;
+		b3ShapeType typeB = shapeB->type;
+
+		float radiusA = 0.0f;
+		if ( typeA == b3_sphereShape )
+		{
+			radiusA = shapeA->sphere.radius;
+		}
+		else if ( typeA == b3_capsuleShape )
+		{
+			radiusA = shapeA->capsule.radius;
+		}
+		else if ( typeA == b3_hullShape )
+		{
+			radiusA = 0.25f * shapeA->hull->innerRadius;
+		}
+
+		float radiusB = 0.0f;
+		if ( typeB == b3_sphereShape )
+		{
+			radiusB = shapeB->sphere.radius;
+		}
+		else if ( typeB == b3_capsuleShape )
+		{
+			radiusB = shapeB->capsule.radius;
+		}
+		else if ( typeB == b3_hullShape )
+		{
+			radiusB = 0.25f * shapeB->hull->innerRadius;
+		}
+
+		float maxRadius = b3MaxFloat( radiusA, radiusB );
+		contact->rollingResistance = b3MaxFloat( materialA->rollingResistance, materialB->rollingResistance ) * maxRadius;
+	}
+	else
+	{
+		contact->rollingResistance = 0.0f;
+	}
+
+	b3Vec3 tangentVelocityA = b3RotateVector( xfA.q, materialA->tangentVelocity );
+	b3Vec3 tangentVelocityB = b3RotateVector( xfB.q, materialB->tangentVelocity );
+	contact->tangentVelocity = b3Sub( tangentVelocityA, tangentVelocityB );
+
+	if ( world->preSolveFcn && ( contact->flags & b3_simEnablePreSolveEvents ) != 0 )
+	{
+		b3ShapeId shapeIdA = { shapeA->id + 1, world->worldId, shapeA->generation };
+		b3ShapeId shapeIdB = { shapeB->id + 1, world->worldId, shapeB->generation };
+
+		// this call assumes thread safety
+		b3Pos point = b3OffsetPos( xfA.p, contact->manifolds[0].points[0].anchorA );
+		b3Vec3 normal = contact->manifolds[0].normal;
+		touching = world->preSolveFcn( shapeIdA, shapeIdB, point, normal, world->preSolveContext );
+		if ( touching == false )
+		{
+			// disable contact
+			b3FreeManifolds( world, contact->manifolds, contact->manifoldCount );
+			contact->manifolds = NULL;
+			contact->manifoldCount = 0;
+			return false;
+		}
+	}
+
+	if ( ( shapeA->flags & b3_enableHitEvents ) || ( shapeB->flags & b3_enableHitEvents ) )
+	{
+		contact->flags |= b3_simEnableHitEvent;
+	}
+	else
+	{
+		contact->flags &= ~b3_simEnableHitEvent;
+	}
+
+	return true;
+}
+
+// Update the contact manifold and touching status.
+// Note: do not assume the shape AABBs are overlapping or are valid.
+bool b3UpdateContact( b3World* world, int workerIndex, b3Contact* contact, b3Shape* shapeA, b3Vec3 localCenterA,
+					  b3WorldTransform xfA, b3Shape* shapeB, b3Vec3 localCenterB, b3WorldTransform xfB, bool isFast,
+					  b3Arena arena )
+{
+	bool touching;
+
+	B3_ASSERT( shapeB->type != b3_compoundShape );
+
+	if ( shapeA->type == b3_compoundShape )
+	{
+		int childIndex = contact->childIndex;
+		b3ChildShape child = b3GetCompoundChild( shapeA->compound, childIndex );
+
+		// Temporary child shape to match existing function signatures
+		b3Shape childShapeA;
+		memcpy( &childShapeA, shapeA, sizeof( b3Shape ) );
+
+		childShapeA.type = child.type;
+
+		if ( child.type == b3_capsuleShape )
+		{
+			childShapeA.capsule = child.capsule;
+			if ( shapeB->type == b3_hullShape )
+			{
+				// Flip
+				bool flip = true;
+				touching = b3UpdateConvexContact( world, workerIndex, contact, shapeB, xfB, &childShapeA, xfA, flip, arena );
+			}
+			else
+			{
+				bool flip = false;
+				touching = b3UpdateConvexContact( world, workerIndex, contact, &childShapeA, xfA, shapeB, xfB, flip, arena );
+			}
+		}
+		else if ( child.type == b3_hullShape )
+		{
+			childShapeA.hull = child.hull;
+			b3WorldTransform xfChild = b3MulWorldTransforms( xfA, child.transform );
+			bool flip = false;
+			touching = b3UpdateConvexContact( world, workerIndex, contact, &childShapeA, xfChild, shapeB, xfB, flip, arena );
+		}
+		else if ( child.type == b3_meshShape )
+		{
+			childShapeA.mesh = child.mesh;
+			b3WorldTransform xfChild = b3MulWorldTransforms( xfA, child.transform );
+
+			touching = b3ComputeMeshManifolds( world, workerIndex, contact, &childShapeA, child.materialIndices, xfChild, shapeB,
+											   xfB, isFast, arena );
+
+			if ( touching && ( ( shapeA->flags & b3_enableHitEvents ) || ( shapeB->flags & b3_enableHitEvents ) ) )
+			{
+				contact->flags |= b3_simEnableHitEvent;
+			}
+			else
+			{
+				contact->flags &= ~b3_simEnableHitEvent;
+			}
+
+			B3_ASSERT( ( touching == true && contact->manifoldCount > 0 ) ||
+					   ( touching == false && contact->manifoldCount == 0 ) );
+		}
+		else
+		{
+			B3_ASSERT( child.type == b3_sphereShape );
+
+			childShapeA.sphere = child.sphere;
+			if ( shapeB->type == b3_capsuleShape || shapeB->type == b3_hullShape )
+			{
+				// Flip
+				bool flip = true;
+				touching = b3UpdateConvexContact( world, workerIndex, contact, shapeB, xfB, &childShapeA, xfA, flip, arena );
+			}
+			else
+			{
+				bool flip = false;
+				touching = b3UpdateConvexContact( world, workerIndex, contact, &childShapeA, xfA, shapeB, xfB, flip, arena );
+			}
+		}
+
+		// The anchor is relative to the child origin but oriented in world space.
+		// Offset the anchor to be relative to the compound origin.
+		int manifoldCount = contact->manifoldCount;
+		b3Vec3 offset = b3RotateVector( xfA.q, child.transform.p );
+		for ( int i = 0; i < manifoldCount; ++i )
+		{
+			b3Manifold* manifold = contact->manifolds + i;
+			int pointCount = manifold->pointCount;
+			for ( int j = 0; j < pointCount; ++j )
+			{
+				b3ManifoldPoint* mp = manifold->points + j;
+				mp->anchorA = b3Add( mp->anchorA, offset );
+			}
+		}
+	}
+	else if ( shapeA->type == b3_meshShape || shapeA->type == b3_heightShape )
+	{
+		// Does this contact touch a mesh or height-field?
+
+		// Compute mesh manifolds
+		touching = b3ComputeMeshManifolds( world, workerIndex, contact, shapeA, NULL, xfA, shapeB, xfB, isFast, arena );
+
+		if ( touching && ( ( shapeA->flags & b3_enableHitEvents ) || ( shapeB->flags & b3_enableHitEvents ) ) )
+		{
+			contact->flags |= b3_simEnableHitEvent;
+		}
+		else
+		{
+			contact->flags &= ~b3_simEnableHitEvent;
+		}
+
+		B3_ASSERT( ( touching == true && contact->manifoldCount > 0 ) || ( touching == false && contact->manifoldCount == 0 ) );
+	}
+	else
+	{
+		// Convex-vs-convex
+		bool flip = false;
+		touching = b3UpdateConvexContact( world, workerIndex, contact, shapeA, xfA, shapeB, xfB, flip, arena );
+	}
+
+	if ( touching )
+	{
+		b3Vec3 centerA = b3RotateVector( xfA.q, localCenterA );
+		b3Vec3 centerB = b3RotateVector( xfB.q, localCenterB );
+
+		// Adjust anchors to be relative to center of mass
+		for ( int i = 0; i < contact->manifoldCount; ++i )
+		{
+			b3Manifold* manifold = contact->manifolds + i;
+			for ( int j = 0; j < manifold->pointCount; ++j )
+			{
+				b3ManifoldPoint* mp = manifold->points + j;
+				mp->anchorA = b3Sub( mp->anchorA, centerA );
+				mp->anchorB = b3Sub( mp->anchorB, centerB );
+			}
+		}
+
+		contact->flags |= b3_simTouchingFlag;
+	}
+	else
+	{
+		contact->flags &= ~b3_simTouchingFlag;
+	}
+
+	return touching;
+}

vendor/box3d/src/src/contact.h

@@ -0,0 +1,183 @@
+// SPDX-FileCopyrightText: 2025 Erin Catto
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include "arena_allocator.h"
+#include "container.h"
+
+#include "box3d/collision.h"
+#include "box3d/types.h"
+
+#define B3_FORCE_GHOST_COLLISIONS 0
+
+typedef struct b3Shape b3Shape;
+typedef struct b3World b3World;
+
+typedef union b3ContactCache
+{
+	b3SATCache satCache;
+	b3SimplexCache simplexCache;
+} b3ContactCache;
+
+typedef struct b3TriangleCache
+{
+	int triangleIndex;
+	b3ContactCache cache;
+} b3TriangleCache;
+
+b3DeclareArray( b3TriangleCache );
+
+enum b3ContactFlags
+{
+	// Set when the solid shapes are touching.
+	b3_contactTouchingFlag = 0x00000001,
+
+	// Contact has a hit event
+	b3_contactHitEventFlag = 0x00000002,
+
+	// This contact wants contact events
+	b3_contactEnableContactEvents = 0x00000004,
+
+	// This is contact is between a dynamic and static body
+	b3_contactStaticFlag = 0x00000008,
+
+	b3_contactRecycleFlag = 0x00000010,
+
+	// Set when the shapes are touching
+	b3_simTouchingFlag = 0x00010000,
+
+	// This contact no longer has overlapping AABBs
+	b3_simDisjoint = 0x00020000,
+
+	// This contact started touching
+	b3_simStartedTouching = 0x00040000,
+
+	// This contact stopped touching
+	b3_simStoppedTouching = 0x00080000,
+
+	// This contact has a hit event
+	b3_simEnableHitEvent = 0x00100000,
+
+	// This contact wants pre-solve events
+	b3_simEnablePreSolveEvents = 0x00200000,
+
+	// This is a mesh contact
+	b3_simMeshContact = 0x00400000,
+
+	// Relative transform is cached for contact recycling
+	b3_relativeTransformValid = 0x00800000,
+
+	// Enable speculative contact points
+	b3_enableSpeculativePoints = 0x01000000,
+};
+
+// 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 b3ContactEdge
+{
+	int bodyId;
+	int prevKey;
+	int nextKey;
+} b3ContactEdge;
+
+typedef struct b3MeshContact
+{
+	b3Array( b3TriangleCache ) triangleCache;
+	b3AABB queryBounds;
+} b3MeshContact;
+
+typedef struct b3ConvexContact
+{
+	b3ContactCache cache;
+} b3ConvexContact;
+
+// Represents the persistent interaction between two shapes
+typedef struct b3Contact
+{
+	// index of simulation set stored in b3World
+	// B3_NULL_INDEX when slot is free
+	int setIndex;
+
+	// index into the constraint graph color array
+	// B3_NULL_INDEX for non-touching or sleeping contacts
+	// B3_NULL_INDEX when slot is free
+	int colorIndex;
+
+	// contact index within set or graph color
+	// B3_NULL_INDEX when slot is free
+	int localIndex;
+
+	b3ContactEdge edges[2];
+	int shapeIdA;
+	int shapeIdB;
+	int childIndex;
+
+	// A contact only belongs to an island if touching, otherwise B3_NULL_INDEX.
+	int islandId;
+
+	// Index into the island's contacts array for O(1) swap-removal.
+	// B3_NULL_INDEX when not in an island.
+	int islandIndex;
+
+	// Back index into b3World::contacts
+	int contactId;
+
+	// These are transient and cached for improved performance. B3_NULL_INDEX for static bodies.
+	int bodySimIndexA;
+	int bodySimIndexB;
+
+	// b3ContactFlags
+	uint32_t flags;
+
+	b3Manifold* manifolds;
+	int manifoldCount;
+
+	// Cache for contact recycling.
+	b3Quat cachedRotationA;
+	b3Quat cachedRotationB;
+	b3Transform cachedRelativePose;
+
+	// Mixed friction and restitution
+	float friction;
+
+	// Usage determined by b3_simMeshContact in simFlags
+	union
+	{
+		b3ConvexContact convexContact;
+		b3MeshContact meshContact;
+	};
+
+	float restitution;
+	float rollingResistance;
+	b3Vec3 tangentVelocity;
+
+	// This is monotonically advanced when a contact is allocated in this slot
+	// Used to check for invalid b3ContactId
+	uint32_t generation;
+} b3Contact;
+
+typedef struct b3ContactSpec
+{
+	int contactId;
+
+	// Start of the global manifold constraint array
+	int manifoldStart;
+	uint16_t manifoldCount;
+} b3ContactSpec;
+
+b3DeclareArray( b3ContactSpec );
+
+void b3InitializeContactRegisters( void );
+
+void b3CreateContact( b3World* world, b3Shape* shapeA, b3Shape* shapeB, int childIndex );
+void b3DestroyContact( b3World* world, b3Contact* contact, bool wakeBodies );
+
+bool b3UpdateContact( b3World* world, int workerIndex, b3Contact* contact, b3Shape* shapeA, b3Vec3 localCenterA, b3WorldTransform xfA,
+					  b3Shape* shapeB, b3Vec3 localCenterB, b3WorldTransform xfB, bool isFast, b3Arena arena );
+
+bool b3ComputeMeshManifolds( b3World* world, int workerIndex, b3Contact* contact, const b3Shape* shapeA, const int* materialMap,
+							 b3WorldTransform xfA, const b3Shape* shapeB, b3WorldTransform xfB, bool isFast, b3Arena arena );

vendor/box3d/src/src/contact_solver.h

@@ -0,0 +1,73 @@
+// SPDX-FileCopyrightText: 2025 Erin Catto
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include "math_internal.h"
+#include "solver.h"
+
+typedef struct b3ManifoldConstraintPoint
+{
+	b3Vec3 rA, rB;
+	float baseSeparation;
+	float relativeVelocity;
+	float normalImpulse;
+	float totalNormalImpulse;
+	float normalMass;
+	float leverArm;
+} b3ManifoldConstraintPoint;
+
+typedef struct b3ManifoldConstraint
+{
+	// todo use pointer buffer
+	b3ManifoldConstraintPoint points[4];
+	int pointCount;
+	b3Vec3 normal;
+	b3Vec3 tangent1;
+	b3Vec3 tangent2;
+	b3Vec3 originA, originB;
+	float twistMass;
+	float twistImpulse;
+	b3Matrix2 tangentMass;
+	b3Vec2 frictionImpulse;
+	b3Vec3 rollingImpulse;
+	float tangentVelocity1;
+	float tangentVelocity2;
+} b3ManifoldConstraint;
+
+typedef struct b3ContactConstraint
+{
+	b3ManifoldConstraint* constraints;
+	struct b3Contact* contact;
+	int indexA;
+	int indexB;
+	float invMassA, invMassB;
+	b3Matrix3 invIA, invIB;
+	b3Softness softness;
+	b3Matrix3 rollingMass;
+	float friction;
+	float restitution;
+	float rollingResistance;
+	int manifoldCount;
+} b3ContactConstraint;
+
+int b3GetWideContactConstraintByteCount( void );
+
+// Overflow contacts don't fit into the constraint graph coloring
+void b3PrepareContacts_Overflow( b3StepContext* context );
+void b3WarmStartContacts_Overflow( b3StepContext* context );
+void b3SolveContacts_Overflow( b3StepContext* context, bool useBias );
+void b3ApplyRestitution_Overflow( b3StepContext* context );
+void b3StoreImpulses_Overflow( b3StepContext* context );
+
+void b3PrepareContacts_Mesh( b3SolverBlock block, b3StepContext* context );
+void b3WarmStartContacts_Mesh( b3SolverBlock block, b3StepContext* context );
+void b3SolveContacts_Mesh( b3SolverBlock block, b3StepContext* context, bool useBias );
+void b3ApplyRestitution_Mesh( b3SolverBlock block, b3StepContext* context );
+void b3StoreImpulses_Mesh( b3SolverBlock block, b3StepContext* context, int workerIndex );
+
+void b3PrepareContacts_Convex( b3SolverBlock block, b3StepContext* context );
+void b3WarmStartContacts_Convex( b3SolverBlock block, b3StepContext* context );
+void b3SolveContacts_Convex( b3SolverBlock block, b3StepContext* context, bool useBias );
+void b3ApplyRestitution_Convex( b3SolverBlock block, b3StepContext* context );
+void b3StoreImpulses_Convex( b3SolverBlock block, b3StepContext* context, int workerIndex );

vendor/box3d/src/src/container.h

@@ -0,0 +1,184 @@
+// SPDX-FileCopyrightText: 2025 Erin Catto
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include "algorithm.h"
+#include "core.h"
+
+#include <stddef.h>
+#include <string.h>
+
+#define b3DeclareArray( T )                                                                                                      \
+	typedef struct b3DynamicArray_##T                                                                                            \
+	{                                                                                                                            \
+		struct T* data;                                                                                                          \
+		int count;                                                                                                               \
+		int capacity;                                                                                                            \
+	} b3DynamicArray_##T
+
+#define b3DeclareArrayNative( T )                                                                                                \
+	typedef struct b3DynamicArray_##T                                                                                            \
+	{                                                                                                                            \
+		T* data;                                                                                                                 \
+		int count;                                                                                                               \
+		int capacity;                                                                                                            \
+	} b3DynamicArray_##T
+
+// Define an array.
+// It may be zero initialized:
+// b3Array(int) myArray = { 0 };
+#define b3Array( T ) b3DynamicArray_##T
+
+// Alternative to zero initialization
+#define b3Array_Create( a )                                                                                                      \
+	do                                                                                                                           \
+	{                                                                                                                            \
+		( a ).data = NULL;                                                                                                       \
+		( a ).count = 0;                                                                                                         \
+		( a ).capacity = 0;                                                                                                      \
+	}                                                                                                                            \
+	while ( 0 )
+
+#define b3Array_CreateN( a, n )                                                                                                  \
+	do                                                                                                                           \
+	{                                                                                                                            \
+		( a ).data = ( n ) > 0 ? b3GrowAlloc( NULL, 0, ( n ) * sizeof( *( a ).data ) ) : NULL;         \
+		( a ).count = 0;                                                                                                         \
+		( a ).capacity = ( n );                                                                                                  \
+	}                                                                                                                            \
+	while ( 0 )
+
+#define b3Array_Destroy( a )                                                                                                     \
+	do                                                                                                                           \
+	{                                                                                                                            \
+		b3Free( ( a ).data, ( a ).capacity * sizeof( *( a ).data ) );                                                            \
+		( a ).data = NULL;                                                                                                       \
+		( a ).count = 0;                                                                                                         \
+		( a ).capacity = 0;                                                                                                      \
+	}                                                                                                                            \
+	while ( 0 )
+
+#define b3Array_Reserve( a, n )                                                                                                  \
+	do                                                                                                                           \
+	{                                                                                                                            \
+		if ( ( a ).capacity < n )                                                                                                \
+		{                                                                                                                        \
+			int oldSize = ( a ).capacity * sizeof( *( a ).data );                                                                \
+			int newSize = ( n ) * sizeof( *( a ).data );                                                                         \
+			( a ).data = b3GrowAlloc( ( a ).data, oldSize, newSize );                                  \
+			( a ).capacity = ( n );                                                                                              \
+		}                                                                                                                        \
+	}                                                                                                                            \
+	while ( 0 )
+
+#define b3Array_Resize( a, n )                                                                                                   \
+	do                                                                                                                           \
+	{                                                                                                                            \
+		b3Array_Reserve( a, n );                                                                                                 \
+		( a ).count = ( n );                                                                                                     \
+	}                                                                                                                            \
+	while ( 0 )
+
+// Push a new element by value
+#define b3Array_Push( a, value )                                                                                                 \
+	do                                                                                                                           \
+	{                                                                                                                            \
+		if ( ( a ).count >= ( a ).capacity )                                                                                     \
+		{                                                                                                                        \
+			int oldSize = ( a ).capacity * sizeof( *( a ).data );                                                                \
+			int newCapacity = ( a ).capacity == 0 ? 8 : 2 * ( a ).capacity;                                                      \
+			int newSize = newCapacity * sizeof( *( a ).data );                                                                   \
+			( a ).data = b3GrowAlloc( ( a ).data, oldSize, newSize );                                  \
+			( a ).capacity = newCapacity;                                                                                        \
+		}                                                                                                                        \
+		( a ).data[( a ).count++] = ( value );                                                                                   \
+	}                                                                                                                            \
+	while ( 0 )
+
+// Get a pointer to an element
+#define b3Array_Get( a, index ) ( B3_ASSERT( 0 <= ( index ) && ( index ) < ( a ).count ), ( a ).data + ( index ) )
+
+// Create a new uninitialized element and return a pointer to it
+#define b3Array_Emplace( a )                                                                                                     \
+	( b3EmplaceHelper( (void**)&( a ).data, &( a ).count, &( a ).capacity, sizeof( *( a ).data ) ) )
+
+// Remove the last element and return it by value.
+#define b3Array_Pop( a ) ( B3_ASSERT( 0 < ( a ).count ), ( a ).data[-1 + ( a ).count--] )
+
+// Add an uninitialized element and return its index.
+#define b3Array_AddIndex( a )                                                                                                    \
+	( b3EmplaceHelper( (void**)&( a ).data, &( a ).count, &( a ).capacity, sizeof( *( a ).data ) ), ( a ).count - 1 )
+
+// Append a contiguous run of values. _n is used to cache the input count while avoiding naming conflicts.
+#define b3Array_Append( a, src, n )                                                                                              \
+	do                                                                                                                           \
+	{                                                                                                                            \
+		int _n = ( n );                                                                                                          \
+		if ( ( a ).count + _n > ( a ).capacity )                                                                                 \
+		{                                                                                                                        \
+			int req = ( a ).count + _n;                                                                                          \
+			int newCapacity = req > 2 ? req + ( req >> 1 ) : 8;                                                                  \
+			int oldSize = ( a ).capacity * sizeof( *( a ).data );                                                                \
+			int newSize = newCapacity * sizeof( *( a ).data );                                                                   \
+			( a ).data = b3GrowAlloc( ( a ).data, oldSize, newSize );                                  \
+			( a ).capacity = newCapacity;                                                                                        \
+		}                                                                                                                        \
+		memcpy( ( a ).data + ( a ).count, ( src ), _n * sizeof( *( a ).data ) );                                                 \
+		( a ).count += _n;                                                                                                       \
+	}                                                                                                                            \
+	while ( 0 )
+
+// Zero the entire allocated buffer (capacity, not just count).
+#define b3Array_MemZero( a )                                                                                                     \
+	do                                                                                                                           \
+	{                                                                                                                            \
+		if ( ( a ).capacity > 0 )                                                                                                \
+		{                                                                                                                        \
+			memset( ( a ).data, 0, ( a ).capacity * sizeof( *( a ).data ) );                                                     \
+		}                                                                                                                        \
+	}                                                                                                                            \
+	while ( 0 )
+
+// Remove and element by swapping with the last element. If the index is the last element it returns
+// B3_NULL_INDEX, otherwise it returns the index of the last element (which is now out of bounds).
+#define b3Array_RemoveSwap( a, index ) b3RemoveHelper( ( a ).data, &( a ).count, ( index ), sizeof( *( a ).data ) )
+
+B3_INLINE void* b3EmplaceHelper( void** data, int* count, int* capacity, int elem_size )
+{
+	if ( *count >= *capacity )
+	{
+		int oldCapacity = *capacity;
+		int oldSize = oldCapacity * elem_size;
+		int newCapacity = ( oldCapacity == 0 ? 16 : 2 * oldCapacity );
+		int newSize = newCapacity * elem_size;
+		*data = b3GrowAlloc( *data, oldSize, newSize );
+		*capacity = newCapacity;
+	}
+	return (char*)*data + ( *count )++ * elem_size;
+}
+
+B3_INLINE int b3RemoveHelper( void* data, int* count, int index, int elementSize )
+{
+	B3_ASSERT( 0 <= index && index < *count && "Array index out of bounds" );
+
+	( *count )--;
+	if ( index != *count )
+	{
+		memcpy( (char*)data + index * elementSize, (char*)data + ( *count ) * elementSize, elementSize );
+		return *count;
+	}
+
+	return B3_NULL_INDEX;
+}
+
+#define b3Array_Clear( a )                                                                                                       \
+	do                                                                                                                           \
+	{                                                                                                                            \
+		( a ).count = 0;                                                                                                         \
+	}                                                                                                                            \
+	while ( 0 )
+
+#define b3Array_ByteCount( a ) ( ( a ).capacity * (int)sizeof( *( a ).data ) )
+
+b3DeclareArrayNative( int );

vendor/box3d/src/src/core.c

@@ -0,0 +1,255 @@
+// SPDX-FileCopyrightText: 2025 Erin Catto
+// SPDX-License-Identifier: MIT
+
+#if defined( B3_COMPILER_MSVC )
+// CRTDBG requires these to be included first
+#define _CRTDBG_MAP_ALLOC
+#include <crtdbg.h>
+#include <stdlib.h>
+#else
+#include <stdlib.h>
+#endif
+
+#include "core.h"
+
+#include "box3d/constants.h"
+#include "box3d/math_functions.h"
+
+#include <stdarg.h>
+#include <string.h>
+
+#ifdef BOX2D_PROFILE
+
+#include <tracy/TracyC.h>
+#define b3TracyCAlloc( ptr, size ) TracyCAlloc( ptr, size )
+#define b3TracyCFree( ptr ) TracyCFree( ptr )
+
+#else
+
+#define b3TracyCAlloc( ptr, size )
+#define b3TracyCFree( ptr )
+
+#endif
+
+#include "platform.h"
+
+#include <stdio.h>
+
+// This allows the user to change the length units at runtime
+static float b3_lengthUnitsPerMeter = 1.0f;
+
+void b3SetLengthUnitsPerMeter( float lengthUnits )
+{
+	B3_ASSERT( b3IsValidFloat( lengthUnits ) && lengthUnits > 0.0f );
+	b3_lengthUnitsPerMeter = lengthUnits;
+}
+
+float b3GetLengthUnitsPerMeter( void )
+{
+	return b3_lengthUnitsPerMeter;
+}
+
+static float b3_stallThreshold = FLT_MAX;
+
+void b3SetStallThreshold( float seconds )
+{
+	B3_ASSERT( b3IsValidFloat( seconds ) && seconds > 0.0f );
+	b3_stallThreshold = seconds;
+}
+
+float b3GetStallThreshold( void )
+{
+	return b3_stallThreshold;
+}
+
+static int b3DefaultAssertFcn( const char* condition, const char* fileName, int lineNumber )
+{
+	printf( "BOX3D ASSERTION: %s, %s, line %d\n", condition, fileName, lineNumber );
+
+	// return non-zero to break to debugger
+	return 1;
+}
+
+b3AssertFcn* b3AssertHandler = b3DefaultAssertFcn;
+
+void b3SetAssertFcn( b3AssertFcn* assertFcn )
+{
+	B3_ASSERT( assertFcn != NULL );
+	b3AssertHandler = assertFcn;
+}
+
+#if !defined( NDEBUG ) || defined( B3_ENABLE_ASSERT )
+int b3InternalAssert( const char* condition, const char* fileName, int lineNumber )
+{
+	int result = b3AssertHandler( condition, fileName, lineNumber );
+	if ( result )
+	{
+		B3_BREAKPOINT;
+	}
+	return result;
+}
+#endif
+
+static void b3DefaultLogFcn( const char* message )
+{
+	printf( "Box3D: %s\n", message );
+}
+
+b3LogFcn* b3LogHandler = b3DefaultLogFcn;
+
+void b3SetLogFcn( b3LogFcn* logFcn )
+{
+	B3_ASSERT( logFcn != NULL );
+	b3LogHandler = logFcn;
+}
+
+void b3Log( const char* format, ... )
+{
+	va_list args;
+	va_start( args, format );
+	char buffer[512];
+	vsnprintf( buffer, sizeof( buffer ), format, args );
+	b3LogHandler( buffer );
+	va_end( args );
+}
+
+b3Version b3GetVersion( void )
+{
+	return (b3Version){ 0, 1, 0 };
+}
+
+bool b3IsDoublePrecision( void )
+{
+#if defined( BOX3D_DOUBLE_PRECISION )
+	return true;
+#else
+	return false;
+#endif
+}
+
+static b3AllocFcn* b3_allocFcn = NULL;
+static b3FreeFcn* b3_freeFcn = NULL;
+
+b3AtomicInt b3_byteCount;
+
+void b3SetAllocator( b3AllocFcn* allocFcn, b3FreeFcn* freeFcn )
+{
+	b3_allocFcn = allocFcn;
+	b3_freeFcn = freeFcn;
+}
+
+void* b3Alloc( size_t size )
+{
+	if ( size == 0 )
+	{
+		return NULL;
+	}
+
+	// This could cause some sharing issues, however Box3D rarely calls b3Alloc.
+	// todo this is not true, Box3D allocates a lot.
+	b3AtomicFetchAddInt( &b3_byteCount, (int)size );
+
+	// Allocation must be a multiple of B3_ALIGNMENT (required by spec).
+	// https://en.cppreference.com/w/c/memory/aligned_alloc
+	int alignedSize = ( ( (int)size - 1 ) | ( B3_ALIGNMENT - 1 ) ) + 1;
+
+	if ( b3_allocFcn != NULL )
+	{
+		void* ptr = b3_allocFcn( alignedSize, B3_ALIGNMENT );
+		b3TracyCAlloc( ptr, size );
+
+		B3_ASSERT( ptr != NULL );
+		B3_ASSERT( ( (uintptr_t)ptr & ( B3_ALIGNMENT - 1 ) ) == 0 );
+
+		return ptr;
+	}
+
+#ifdef B3_PLATFORM_WINDOWS
+	void* ptr = _aligned_malloc( alignedSize, B3_ALIGNMENT );
+#elif defined( B3_PLATFORM_ANDROID )
+	void* ptr = NULL;
+	if ( posix_memalign( &ptr, B3_ALIGNMENT, alignedSize ) != 0 )
+	{
+		// allocation failed, exit the application
+		exit( EXIT_FAILURE );
+	}
+#else
+	void* ptr = aligned_alloc( B3_ALIGNMENT, alignedSize );
+#endif
+
+	b3TracyCAlloc( ptr, size );
+
+	B3_ASSERT( ptr != NULL );
+	B3_ASSERT( ( (uintptr_t)ptr & ( B3_ALIGNMENT - 1 ) ) == 0 );
+
+	return ptr;
+}
+
+void b3Free( void* mem, size_t size )
+{
+	if ( mem == NULL )
+	{
+		return;
+	}
+
+	b3TracyCFree( mem );
+
+	if ( b3_freeFcn != NULL )
+	{
+		b3_freeFcn( mem );
+	}
+	else
+	{
+#ifdef B3_PLATFORM_WINDOWS
+		_aligned_free( mem );
+#else
+		free( mem );
+#endif
+	}
+
+	b3AtomicFetchAddInt( &b3_byteCount, -(int)size );
+}
+
+void* b3GrowAlloc( void* oldMem, int oldSize, int newSize )
+{
+	B3_ASSERT( newSize > oldSize );
+	void* newMem = b3Alloc( newSize );
+	if ( oldSize > 0 )
+	{
+		memcpy( newMem, oldMem, oldSize );
+		b3Free( oldMem, oldSize );
+	}
+	return newMem;
+}
+
+int b3GetByteCount( void )
+{
+	return b3AtomicLoadInt( &b3_byteCount );
+}
+
+void* b3AllocZeroed( size_t size )
+{
+	void* mem = b3Alloc( size );
+	memset( mem, 0, size );
+	return mem;
+}
+
+// Not used. Keeping around in case I need this.
+void b3StrCpy( char* dst, int size, const char* src )
+{
+	B3_ASSERT( size > 0 );
+
+	if ( src != NULL )
+	{
+#if defined( _MSC_VER )
+		strncpy_s( dst, size, src, size - 1 );
+#else
+		strncpy( dst, src, size - 1 );
+		dst[size - 1] = 0;
+#endif
+	}
+	else
+	{
+		memset( dst, 0, size );
+	}
+}

vendor/box3d/src/src/core.h

@@ -0,0 +1,161 @@
+// SPDX-FileCopyrightText: 2025 Erin Catto
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include "box3d/base.h"
+
+#include <stddef.h>
+
+// clang-format off
+
+#ifdef NDEBUG
+	#define B3_DEBUG 0
+#else
+	#define B3_DEBUG 1
+#endif
+
+// Define platform
+#if defined(_WIN32) || defined(_WIN64)
+	#define B3_PLATFORM_WINDOWS
+#elif defined( __ANDROID__ )
+	#define B3_PLATFORM_ANDROID
+#elif defined( __linux__ )
+	#define B3_PLATFORM_LINUX
+#elif defined( __APPLE__ )
+	#include <TargetConditionals.h>
+	#if defined( TARGET_OS_IPHONE ) && !TARGET_OS_IPHONE
+		#define B3_PLATFORM_MACOS
+	#else
+		#define B3_PLATFORM_IOS
+	#endif
+#elif defined( __EMSCRIPTEN__ )
+	#define B3_PLATFORM_WASM
+#else
+	#define B3_PLATFORM_UNKNOWN
+#endif
+
+// Define CPU
+#if defined( __x86_64__ ) || defined( _M_X64 ) || defined( __i386__ ) || defined( _M_IX86 )
+	#define B3_CPU_X86_X64
+#elif defined( __aarch64__ ) || defined( _M_ARM64 ) || defined( __arm__ ) || defined( _M_ARM )
+	#define B3_CPU_ARM
+#elif defined( __EMSCRIPTEN__ )
+	#define B3_CPU_WASM
+#else
+	#define B3_CPU_UNKNOWN
+#endif
+
+// Define SIMD
+#if defined( BOX3D_DISABLE_SIMD )
+	#define B3_SIMD_NONE
+	#define B3_SIMD_WIDTH 4
+	//#pragma message("B3_SIMD_NONE")
+#else
+	#if defined( B3_CPU_X86_X64 )
+		#define B3_SIMD_SSE2
+		#define B3_SIMD_WIDTH 4
+		//#pragma message("B3_SIMD_SSE2")
+	#elif defined( B3_CPU_ARM )
+		#define B3_SIMD_NEON
+		#define B3_SIMD_WIDTH 4
+		//#pragma message("B3_SIMD_NEON")
+	#elif defined( B3_CPU_WASM )
+		#define B3_CPU_WASM
+		#define B3_SIMD_SSE2
+		#define B3_SIMD_WIDTH 4
+		//#pragma message("B3_SIMD_SSE2")
+	#else
+		#define B3_SIMD_NONE
+		#define B3_SIMD_WIDTH 4
+		//#pragma message("B3_SIMD_NONE")
+	#endif
+#endif
+
+// Define compiler
+#if defined( __clang__ )
+	#define B3_COMPILER_CLANG
+#elif defined( __GNUC__ )
+	#define B3_COMPILER_GCC
+#elif defined( _MSC_VER )
+	#define B3_COMPILER_MSVC
+#endif
+
+/// Tracy profiler instrumentation
+/// https://github.com/wolfpld/tracy
+#ifdef BOX3D_PROFILE
+	#include <tracy/TracyC.h>
+	#define b3TracyCZoneC( ctx, color, active ) TracyCZoneC( ctx, color, active )
+	#define b3TracyCZoneNC( ctx, name, color, active ) TracyCZoneNC( ctx, name, color, active )
+	#define b3TracyCZoneEnd( ctx ) TracyCZoneEnd( ctx )
+	#define b3TracyCFrame TracyCFrameMark
+#else
+	#define b3TracyCZoneC( ctx, color, active )
+	#define b3TracyCZoneNC( ctx, name, color, active )
+	#define b3TracyCZoneEnd( ctx )
+	#define b3TracyCFrame
+#endif
+
+// clang-format on
+
+typedef struct b3AtomicInt
+{
+	int value;
+} b3AtomicInt;
+
+typedef struct b3AtomicU32
+{
+	uint32_t value;
+} b3AtomicU32;
+
+// Minimum memory alignment used for all allocations
+#define B3_ALIGNMENT 16
+
+// Returns the number of elements of an array
+#define B3_ARRAY_COUNT( A ) (int)( sizeof( A ) / sizeof( A[0] ) )
+
+// Used to prevent the compiler from warning about unused variables
+#define B3_UNUSED( ... ) (void)sizeof( ( __VA_ARGS__, 0 ) )
+
+// Use to validate definitions. Do not take my cookie.
+#define B3_SECRET_COOKIE 1152023
+
+#define B3_CHECK_DEF( DEF ) B3_ASSERT( DEF->internalValue == B3_SECRET_COOKIE )
+#define B3_CHECK_JOINT_DEF( DEF ) B3_ASSERT( DEF->base.internalValue == B3_SECRET_COOKIE )
+
+// These macros help avoid sizeof bugs
+#define B3_ALLOC( T, N ) (T*)b3Alloc( N * sizeof( T ) );
+#define B3_FREE( M, T, N ) b3Free( M, N * sizeof( T ) );
+
+void* b3Alloc( size_t size );
+void* b3AllocZeroed( size_t size );
+void b3Free( void* mem, size_t size );
+void* b3GrowAlloc( void* oldMem, int oldSize, int newSize );
+
+void b3Log( const char* format, ... );
+
+// Geometry content hashes reserve zero to mean unhashed
+static inline uint32_t b3NonZeroHash( uint32_t hash )
+{
+	return hash != 0 ? hash : 1;
+}
+
+typedef struct b3Mutex b3Mutex;
+b3Mutex* b3CreateMutex( void );
+void b3DestroyMutex( b3Mutex* m );
+void b3LockMutex( b3Mutex* m );
+void b3UnlockMutex( b3Mutex* m );
+
+typedef struct b3Semaphore b3Semaphore;
+b3Semaphore* b3CreateSemaphore( int initCount );
+void b3DestroySemaphore( b3Semaphore* s );
+void b3WaitSemaphore( b3Semaphore* s );
+void b3SignalSemaphore( b3Semaphore* s );
+
+typedef void b3ThreadFunction( void* context );
+typedef struct b3Thread b3Thread;
+// Name may be NULL, otherwise it is copied.
+b3Thread* b3CreateThread( b3ThreadFunction* function, void* context, const char* name );
+void b3JoinThread( b3Thread* t );
+
+void b3StrCpy( char* dst, int size, const char* src );

vendor/box3d/src/src/ctz.h

@@ -0,0 +1,134 @@
+// SPDX-FileCopyrightText: 2025 Erin Catto
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include "box3d/base.h"
+
+#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 b3CTZ32( 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 b3CLZ32( 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 b3CTZ64( 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;
+}
+
+static inline int b3PopCount64( uint64_t block )
+{
+	return (int)__popcnt64( block );
+}
+
+#else
+
+static inline uint32_t b3CTZ32( uint32_t block )
+{
+	return __builtin_ctz( block );
+}
+
+static inline uint32_t b3CLZ32( uint32_t value )
+{
+	return __builtin_clz( value );
+}
+
+static inline uint32_t b3CTZ64( uint64_t block )
+{
+	return __builtin_ctzll( block );
+}
+
+static inline int b3PopCount64( uint64_t block )
+{
+	return __builtin_popcountll( block );
+}
+
+#endif
+
+static inline bool b3IsPowerOf2( int x )
+{
+	return ( x & ( x - 1 ) ) == 0;
+}
+
+static inline int b3BoundingPowerOf2( int x )
+{
+	if ( x <= 1 )
+	{
+		return 1;
+	}
+
+	return 32 - (int)b3CLZ32( (uint32_t)x - 1 );
+}
+
+static inline int b3RoundUpPowerOf2( int x )
+{
+	if ( x <= 1 )
+	{
+		return 1;
+	}
+
+	return 1 << ( 32 - (int)b3CLZ32( (uint32_t)x - 1 ) );
+}
+
+static inline int b3LowerPowerOf2Exponent( int x )
+{
+	B3_ASSERT( x > 0 );
+	int clz = (int)b3CLZ32( (uint32_t)x );
+
+	// Position of most significant bit = floor(log2(M))
+	return 31 - clz;
+}

vendor/box3d/src/src/distance_joint.c

@@ -0,0 +1,579 @@
+// SPDX-FileCopyrightText: 2025 Erin Catto
+// SPDX-License-Identifier: MIT
+
+#include "body.h"
+#include "core.h"
+#include "joint.h"
+#include "physics_world.h"
+#include "recording.h"
+#include "solver.h"
+#include "solver_set.h"
+
+// needed for dll export
+#include "box3d/box3d.h"
+
+void b3DistanceJoint_SetLength( b3JointId jointId, float length )
+{
+	b3World* world = b3GetWorld( jointId.world0 );
+	B3_REC( world, DistanceJointSetLength, jointId, length );
+	b3JointSim* base = b3GetJointSimCheckType( jointId, b3_distanceJoint );
+	b3DistanceJoint* joint = &base->distanceJoint;
+
+	joint->length = b3ClampFloat( length, B3_LINEAR_SLOP, B3_HUGE );
+	joint->impulse = 0.0f;
+	joint->lowerImpulse = 0.0f;
+	joint->upperImpulse = 0.0f;
+}
+
+float b3DistanceJoint_GetLength( b3JointId jointId )
+{
+	b3JointSim* base = b3GetJointSimCheckType( jointId, b3_distanceJoint );
+	b3DistanceJoint* joint = &base->distanceJoint;
+	return joint->length;
+}
+
+void b3DistanceJoint_EnableLimit( b3JointId jointId, bool enableLimit )
+{
+	b3World* world = b3GetWorld( jointId.world0 );
+	B3_REC( world, DistanceJointEnableLimit, jointId, enableLimit );
+	b3JointSim* base = b3GetJointSimCheckType( jointId, b3_distanceJoint );
+	b3DistanceJoint* joint = &base->distanceJoint;
+	joint->enableLimit = enableLimit;
+}
+
+bool b3DistanceJoint_IsLimitEnabled( b3JointId jointId )
+{
+	b3JointSim* joint = b3GetJointSimCheckType( jointId, b3_distanceJoint );
+	return joint->distanceJoint.enableLimit;
+}
+
+void b3DistanceJoint_SetLengthRange( b3JointId jointId, float minLength, float maxLength )
+{
+	b3World* world = b3GetWorld( jointId.world0 );
+	B3_REC( world, DistanceJointSetLengthRange, jointId, minLength, maxLength );
+	b3JointSim* base = b3GetJointSimCheckType( jointId, b3_distanceJoint );
+	b3DistanceJoint* joint = &base->distanceJoint;
+
+	minLength = b3ClampFloat( minLength, B3_LINEAR_SLOP, B3_HUGE );
+	maxLength = b3ClampFloat( maxLength, B3_LINEAR_SLOP, B3_HUGE );
+	joint->minLength = b3MinFloat( minLength, maxLength );
+	joint->maxLength = b3MaxFloat( minLength, maxLength );
+	joint->impulse = 0.0f;
+	joint->lowerImpulse = 0.0f;
+	joint->upperImpulse = 0.0f;
+}
+
+float b3DistanceJoint_GetMinLength( b3JointId jointId )
+{
+	b3JointSim* base = b3GetJointSimCheckType( jointId, b3_distanceJoint );
+	b3DistanceJoint* joint = &base->distanceJoint;
+	return joint->minLength;
+}
+
+float b3DistanceJoint_GetMaxLength( b3JointId jointId )
+{
+	b3JointSim* base = b3GetJointSimCheckType( jointId, b3_distanceJoint );
+	b3DistanceJoint* joint = &base->distanceJoint;
+	return joint->maxLength;
+}
+
+float b3DistanceJoint_GetCurrentLength( b3JointId jointId )
+{
+	b3JointSim* base = b3GetJointSimCheckType( jointId, b3_distanceJoint );
+
+	b3World* world = b3GetUnlockedWorld( jointId.world0 );
+	if ( world == NULL )
+	{
+		return 0.0f;
+	}
+
+	b3WorldTransform transformA = b3GetBodyTransform( world, base->bodyIdA );
+	b3WorldTransform transformB = b3GetBodyTransform( world, base->bodyIdB );
+
+	b3Pos pA = b3TransformWorldPoint( transformA, base->localFrameA.p );
+	b3Pos pB = b3TransformWorldPoint( transformB, base->localFrameB.p );
+	b3Vec3 d = b3SubPos( pB, pA );
+	float length = b3Length( d );
+	return length;
+}
+
+void b3DistanceJoint_EnableSpring( b3JointId jointId, bool enableSpring )
+{
+	b3World* world = b3GetWorld( jointId.world0 );
+	B3_REC( world, DistanceJointEnableSpring, jointId, enableSpring );
+	b3JointSim* base = b3GetJointSimCheckType( jointId, b3_distanceJoint );
+	base->distanceJoint.enableSpring = enableSpring;
+}
+
+bool b3DistanceJoint_IsSpringEnabled( b3JointId jointId )
+{
+	b3JointSim* base = b3GetJointSimCheckType( jointId, b3_distanceJoint );
+	return base->distanceJoint.enableSpring;
+}
+
+
+void b3DistanceJoint_SetSpringForceRange( b3JointId jointId, float lowerForce, float upperForce )
+{
+	B3_ASSERT( lowerForce <= upperForce );
+	b3World* world = b3GetWorld( jointId.world0 );
+	B3_REC( world, DistanceJointSetSpringForceRange, jointId, lowerForce, upperForce );
+	b3JointSim* base = b3GetJointSimCheckType( jointId, b3_distanceJoint );
+	base->distanceJoint.lowerSpringForce = lowerForce;
+	base->distanceJoint.upperSpringForce = upperForce;
+}
+
+void b3DistanceJoint_GetSpringForceRange( b3JointId jointId, float* lowerForce, float* upperForce )
+{
+	b3JointSim* base = b3GetJointSimCheckType( jointId, b3_distanceJoint );
+	*lowerForce = base->distanceJoint.lowerSpringForce;
+	*upperForce = base->distanceJoint.upperSpringForce;
+}
+
+void b3DistanceJoint_SetSpringHertz( b3JointId jointId, float hertz )
+{
+	b3World* world = b3GetWorld( jointId.world0 );
+	B3_REC( world, DistanceJointSetSpringHertz, jointId, hertz );
+	b3JointSim* base = b3GetJointSimCheckType( jointId, b3_distanceJoint );
+	base->distanceJoint.hertz = hertz;
+}
+
+void b3DistanceJoint_SetSpringDampingRatio( b3JointId jointId, float dampingRatio )
+{
+	b3World* world = b3GetWorld( jointId.world0 );
+	B3_REC( world, DistanceJointSetSpringDampingRatio, jointId, dampingRatio );
+	b3JointSim* base = b3GetJointSimCheckType( jointId, b3_distanceJoint );
+	base->distanceJoint.dampingRatio = dampingRatio;
+}
+
+float b3DistanceJoint_GetSpringHertz( b3JointId jointId )
+{
+	b3JointSim* base = b3GetJointSimCheckType( jointId, b3_distanceJoint );
+	b3DistanceJoint* joint = &base->distanceJoint;
+	return joint->hertz;
+}
+
+float b3DistanceJoint_GetSpringDampingRatio( b3JointId jointId )
+{
+	b3JointSim* base = b3GetJointSimCheckType( jointId, b3_distanceJoint );
+	b3DistanceJoint* joint = &base->distanceJoint;
+	return joint->dampingRatio;
+}
+
+void b3DistanceJoint_EnableMotor( b3JointId jointId, bool enableMotor )
+{
+	b3World* world = b3GetWorld( jointId.world0 );
+	B3_REC( world, DistanceJointEnableMotor, jointId, enableMotor );
+	b3JointSim* joint = b3GetJointSimCheckType( jointId, b3_distanceJoint );
+	if ( enableMotor != joint->distanceJoint.enableMotor )
+	{
+		joint->distanceJoint.enableMotor = enableMotor;
+		joint->distanceJoint.motorImpulse = 0.0f;
+	}
+}
+
+bool b3DistanceJoint_IsMotorEnabled( b3JointId jointId )
+{
+	b3JointSim* joint = b3GetJointSimCheckType( jointId, b3_distanceJoint );
+	return joint->distanceJoint.enableMotor;
+}
+
+void b3DistanceJoint_SetMotorSpeed( b3JointId jointId, float motorSpeed )
+{
+	b3World* world = b3GetWorld( jointId.world0 );
+	B3_REC( world, DistanceJointSetMotorSpeed, jointId, motorSpeed );
+	b3JointSim* joint = b3GetJointSimCheckType( jointId, b3_distanceJoint );
+	joint->distanceJoint.motorSpeed = motorSpeed;
+}
+
+float b3DistanceJoint_GetMotorSpeed( b3JointId jointId )
+{
+	b3JointSim* joint = b3GetJointSimCheckType( jointId, b3_distanceJoint );
+	return joint->distanceJoint.motorSpeed;
+}
+
+float b3DistanceJoint_GetMotorForce( b3JointId jointId )
+{
+	b3World* world = b3GetWorld( jointId.world0 );
+	b3JointSim* base = b3GetJointSimCheckType( jointId, b3_distanceJoint );
+	return world->inv_h * base->distanceJoint.motorImpulse;
+}
+
+void b3DistanceJoint_SetMaxMotorForce( b3JointId jointId, float force )
+{
+	b3World* world = b3GetWorld( jointId.world0 );
+	B3_REC( world, DistanceJointSetMaxMotorForce, jointId, force );
+	b3JointSim* joint = b3GetJointSimCheckType( jointId, b3_distanceJoint );
+	joint->distanceJoint.maxMotorForce = force;
+}
+
+float b3DistanceJoint_GetMaxMotorForce( b3JointId jointId )
+{
+	b3JointSim* joint = b3GetJointSimCheckType( jointId, b3_distanceJoint );
+	return joint->distanceJoint.maxMotorForce;
+}
+
+b3Vec3 b3GetDistanceJointForce( b3World* world, b3JointSim* base )
+{
+	b3DistanceJoint* joint = &base->distanceJoint;
+
+	b3WorldTransform transformA = b3GetBodyTransform( world, base->bodyIdA );
+	b3WorldTransform transformB = b3GetBodyTransform( world, base->bodyIdB );
+
+	b3Pos pA = b3TransformWorldPoint( transformA, base->localFrameA.p );
+	b3Pos pB = b3TransformWorldPoint( transformB, base->localFrameB.p );
+	b3Vec3 d = b3SubPos( pB, pA );
+	b3Vec3 axis = b3Normalize( d );
+	float force = ( joint->impulse + joint->lowerImpulse - joint->upperImpulse + joint->motorImpulse ) * world->inv_h;
+	return b3MulSV( 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 b3PrepareDistanceJoint( b3JointSim* base, b3StepContext* context )
+{
+	B3_ASSERT( base->type == b3_distanceJoint );
+
+	// chase body id to the solver set where the body lives
+	int idA = base->bodyIdA;
+	int idB = base->bodyIdB;
+
+	b3World* world = context->world;
+	b3Body* bodyA = b3Array_Get( world->bodies, idA  );
+	b3Body* bodyB = b3Array_Get( world->bodies, idB  );
+
+	B3_ASSERT( bodyA->setIndex == b3_awakeSet || bodyB->setIndex == b3_awakeSet );
+
+	b3SolverSet* setA = b3Array_Get( world->solverSets, bodyA->setIndex  );
+	b3SolverSet* setB = b3Array_Get( world->solverSets, bodyB->setIndex  );
+
+	int localIndexA = bodyA->localIndex;
+	int localIndexB = bodyB->localIndex;
+
+	b3BodySim* bodySimA = b3Array_Get( setA->bodySims, localIndexA  );
+	b3BodySim* bodySimB = b3Array_Get( setB->bodySims, localIndexB  );
+
+	float mA = bodySimA->invMass;
+	b3Matrix3 iA = bodySimA->invInertiaWorld;
+	float mB = bodySimB->invMass;
+	b3Matrix3 iB = bodySimB->invInertiaWorld;
+
+	base->invMassA = mA;
+	base->invMassB = mB;
+	base->invIA = iA;
+	base->invIB = iB;
+
+	b3DistanceJoint* joint = &base->distanceJoint;
+
+	joint->indexA = bodyA->setIndex == b3_awakeSet ? localIndexA : B3_NULL_INDEX;
+	joint->indexB = bodyB->setIndex == b3_awakeSet ? localIndexB : B3_NULL_INDEX;
+
+	// initial anchors in world space
+	joint->anchorA = b3RotateVector( bodySimA->transform.q, b3Sub( base->localFrameA.p, bodySimA->localCenter ) );
+	joint->anchorB = b3RotateVector( bodySimB->transform.q, b3Sub( base->localFrameB.p, bodySimB->localCenter ) );
+	joint->deltaCenter = b3SubPos( bodySimB->center, bodySimA->center );
+
+	b3Vec3 rA = joint->anchorA;
+	b3Vec3 rB = joint->anchorB;
+	b3Vec3 separation = b3Add( b3Sub( rB, rA ), joint->deltaCenter );
+	b3Vec3 axis = b3Normalize( separation );
+
+	// compute effective mass
+	b3Vec3 crA = b3Cross( rA, axis );
+	b3Vec3 crB = b3Cross( rB, axis );
+	float k = mA + mB + b3Dot( crA, b3MulMV( iA, crA ) ) + b3Dot( crB, b3MulMV( iB, crB ) );
+	joint->axialMass = k > 0.0f ? 1.0f / k : 0.0f;
+
+	joint->distanceSoftness = b3MakeSoft( 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 b3WarmStartDistanceJoint( b3JointSim* base, b3StepContext* context )
+{
+	B3_ASSERT( base->type == b3_distanceJoint );
+
+	float mA = base->invMassA;
+	float mB = base->invMassB;
+	b3Matrix3 iA = base->invIA;
+	b3Matrix3 iB = base->invIB;
+
+	// dummy state for static bodies
+	b3BodyState dummyState = b3_identityBodyState;
+
+	b3DistanceJoint* joint = &base->distanceJoint;
+	b3BodyState* stateA = joint->indexA == B3_NULL_INDEX ? &dummyState : context->states + joint->indexA;
+	b3BodyState* stateB = joint->indexB == B3_NULL_INDEX ? &dummyState : context->states + joint->indexB;
+
+	b3Vec3 rA = b3RotateVector( stateA->deltaRotation, joint->anchorA );
+	b3Vec3 rB = b3RotateVector( stateB->deltaRotation, joint->anchorB );
+
+	b3Vec3 ds = b3Add( b3Sub( stateB->deltaPosition, stateA->deltaPosition ), b3Sub( rB, rA ) );
+	b3Vec3 separation = b3Add( joint->deltaCenter, ds );
+	b3Vec3 axis = b3Normalize( separation );
+
+	float axialImpulse = joint->impulse + joint->lowerImpulse - joint->upperImpulse + joint->motorImpulse;
+	b3Vec3 P = b3MulSV( axialImpulse, axis );
+
+	if ( stateA->flags & b3_dynamicFlag )
+	{
+		stateA->linearVelocity = b3MulSub( stateA->linearVelocity, mA, P );
+		stateA->angularVelocity = b3Sub( stateA->angularVelocity, b3MulMV( iA, b3Cross( rA, P ) ) );
+	}
+
+	if ( stateB->flags & b3_dynamicFlag )
+	{
+		stateB->linearVelocity = b3MulAdd( stateB->linearVelocity, mB, P );
+		stateB->angularVelocity = b3Add( stateB->angularVelocity, b3MulMV( iB, b3Cross( rB, P ) ) );
+	}
+}
+
+void b3SolveDistanceJoint( b3JointSim* base, b3StepContext* context, bool useBias )
+{
+	B3_ASSERT( base->type == b3_distanceJoint );
+
+	float mA = base->invMassA;
+	float mB = base->invMassB;
+	b3Matrix3 iA = base->invIA;
+	b3Matrix3 iB = base->invIB;
+
+	// dummy state for static bodies
+	b3BodyState dummyState = b3_identityBodyState;
+
+	b3DistanceJoint* joint = &base->distanceJoint;
+	b3BodyState* stateA = joint->indexA == B3_NULL_INDEX ? &dummyState : context->states + joint->indexA;
+	b3BodyState* stateB = joint->indexB == B3_NULL_INDEX ? &dummyState : context->states + joint->indexB;
+
+	b3Vec3 vA = stateA->linearVelocity;
+	b3Vec3 wA = stateA->angularVelocity;
+	b3Vec3 vB = stateB->linearVelocity;
+	b3Vec3 wB = stateB->angularVelocity;
+
+	// current anchors
+	b3Vec3 rA = b3RotateVector( stateA->deltaRotation, joint->anchorA );
+	b3Vec3 rB = b3RotateVector( stateB->deltaRotation, joint->anchorB );
+
+	// current separation
+	b3Vec3 ds = b3Add( b3Sub( stateB->deltaPosition, stateA->deltaPosition ), b3Sub( rB, rA ) );
+	b3Vec3 separation = b3Add( joint->deltaCenter, ds );
+
+	float length = b3Length( separation );
+	b3Vec3 axis = b3Normalize( 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))
+			b3Vec3 vr = b3Add( b3Sub( vB, vA ), b3Sub( b3Cross( wB, rB ), b3Cross( wA, rA ) ) );
+			float Cdot = b3Dot( axis, vr );
+			float C = length - joint->length;
+			float bias = joint->distanceSoftness.biasRate * C;
+
+			float m = joint->distanceSoftness.massScale * joint->axialMass;
+			float oldImpulse = joint->impulse;
+			float impulse = -m * ( Cdot + bias ) - joint->distanceSoftness.impulseScale * oldImpulse;
+			float h = context->h;
+			joint->impulse = b3ClampFloat( joint->impulse + impulse, joint->lowerSpringForce * h, joint->upperSpringForce * h );
+			impulse = joint->impulse - oldImpulse;
+
+			b3Vec3 P = b3MulSV( impulse, axis );
+			vA = b3MulSub( vA, mA, P );
+			wA = b3Sub( wA, b3MulMV( iA, b3Cross( rA, P ) ) );
+			vB = b3MulAdd( vB, mB, P );
+			wB = b3Add( wB, b3MulMV( iB, b3Cross( rB, P ) ) );
+		}
+
+		if ( joint->enableLimit )
+		{
+			// lower limit
+			{
+				b3Vec3 vr = b3Add( b3Sub( vB, vA ), b3Sub( b3Cross( wB, rB ), b3Cross( wA, rA ) ) );
+				float Cdot = b3Dot( 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 = base->constraintSoftness.biasRate * C;
+					massCoeff = base->constraintSoftness.massScale;
+					impulseCoeff = base->constraintSoftness.impulseScale;
+				}
+
+				float impulse = -massCoeff * joint->axialMass * ( Cdot + bias ) - impulseCoeff * joint->lowerImpulse;
+				float newImpulse = b3MaxFloat( 0.0f, joint->lowerImpulse + impulse );
+				impulse = newImpulse - joint->lowerImpulse;
+				joint->lowerImpulse = newImpulse;
+
+				b3Vec3 P = b3MulSV( impulse, axis );
+				vA = b3MulSub( vA, mA, P );
+				wA = b3Sub( wA, b3MulMV( iA, b3Cross( rA, P ) ) );
+				vB = b3MulAdd( vB, mB, P );
+				wB = b3Add( wB, b3MulMV( iB, b3Cross( rB, P ) ) );
+			}
+
+			// upper
+			{
+				b3Vec3 vr = b3Add( b3Sub( vA, vB ), b3Sub( b3Cross( wA, rA ), b3Cross( wB, rB ) ) );
+				float Cdot = b3Dot( 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 = base->constraintSoftness.biasRate * C;
+					massScale = base->constraintSoftness.massScale;
+					impulseScale = base->constraintSoftness.impulseScale;
+				}
+
+				float impulse = -massScale * joint->axialMass * ( Cdot + bias ) - impulseScale * joint->upperImpulse;
+				float newImpulse = b3MaxFloat( 0.0f, joint->upperImpulse + impulse );
+				impulse = newImpulse - joint->upperImpulse;
+				joint->upperImpulse = newImpulse;
+
+				b3Vec3 P = b3MulSV( -impulse, axis );
+				vA = b3MulSub( vA, mA, P );
+				wA = b3Sub( wA, b3MulMV( iA, b3Cross( rA, P ) ) );
+				vB = b3MulAdd( vB, mB, P );
+				wB = b3Add( wB, b3MulMV( iB, b3Cross( rB, P ) ) );
+			}
+		}
+
+		if ( joint->enableMotor )
+		{
+			b3Vec3 vr = b3Add( b3Sub( vB, vA ), b3Sub( b3Cross( wB, rB ), b3Cross( wA, rA ) ) );
+			float Cdot = b3Dot( axis, vr );
+			float impulse = joint->axialMass * ( joint->motorSpeed - Cdot );
+			float oldImpulse = joint->motorImpulse;
+			float maxImpulse = context->h * joint->maxMotorForce;
+			joint->motorImpulse = b3ClampFloat( joint->motorImpulse + impulse, -maxImpulse, maxImpulse );
+			impulse = joint->motorImpulse - oldImpulse;
+
+			b3Vec3 P = b3MulSV( impulse, axis );
+			vA = b3MulSub( vA, mA, P );
+			wA = b3Sub( wA, b3MulMV( iA, b3Cross( rA, P ) ) );
+			vB = b3MulAdd( vB, mB, P );
+			wB = b3Add( wB, b3MulMV( iB, b3Cross( rB, P ) ) );
+		}
+	}
+	else
+	{
+		// rigid constraint
+		b3Vec3 vr = b3Add( b3Sub( vB, vA ), b3Sub( b3Cross( wB, rB ), b3Cross( wA, rA ) ) );
+		float Cdot = b3Dot( axis, vr );
+
+		float C = length - joint->length;
+
+		float bias = 0.0f;
+		float massScale = 1.0f;
+		float impulseScale = 0.0f;
+		if ( useBias )
+		{
+			bias = base->constraintSoftness.biasRate * C;
+			massScale = base->constraintSoftness.massScale;
+			impulseScale = base->constraintSoftness.impulseScale;
+		}
+
+		float impulse = -massScale * joint->axialMass * ( Cdot + bias ) - impulseScale * joint->impulse;
+		joint->impulse += impulse;
+
+		b3Vec3 P = b3MulSV( impulse, axis );
+		vA = b3MulSub( vA, mA, P );
+		wA = b3Sub( wA, b3MulMV( iA, b3Cross( rA, P ) ) );
+		vB = b3MulAdd( vB, mB, P );
+		wB = b3Add( wB, b3MulMV( iB, b3Cross( rB, P ) ) );
+	}
+
+	if ( stateA->flags & b3_dynamicFlag )
+	{
+		stateA->linearVelocity = vA;
+		stateA->angularVelocity = wA;
+	}
+
+	if ( stateB->flags & b3_dynamicFlag )
+	{
+		stateB->linearVelocity = vB;
+		stateB->angularVelocity = wB;
+	}
+}
+
+void b3DrawDistanceJoint( b3DebugDraw* draw, b3JointSim* base, b3WorldTransform transformA, b3WorldTransform transformB )
+{
+	B3_ASSERT( base->type == b3_distanceJoint );
+
+	b3DistanceJoint* joint = &base->distanceJoint;
+
+	b3Pos pA = b3TransformWorldPoint( transformA, base->localFrameA.p );
+	b3Pos pB = b3TransformWorldPoint( transformB, base->localFrameB.p );
+
+	b3Vec3 axis = b3Normalize( b3SubPos( pB, pA ) );
+
+	if ( joint->minLength < joint->maxLength && joint->enableLimit )
+	{
+		b3Pos pMin = b3OffsetPos( pA, b3MulSV( joint->minLength, axis ) );
+		b3Pos pMax = b3OffsetPos( pA, b3MulSV( joint->maxLength, axis ) );
+
+		if ( joint->minLength > B3_LINEAR_SLOP )
+		{
+			draw->DrawPointFcn( pMin, 6.0f, b3_colorLightGreen, draw->context );
+		}
+
+		if ( joint->maxLength < B3_HUGE )
+		{
+			draw->DrawPointFcn(pMax, 6.0f, b3_colorRed, draw->context);
+		}
+
+		if ( joint->minLength > B3_LINEAR_SLOP && joint->maxLength < B3_HUGE )
+		{
+			draw->DrawSegmentFcn( pMin, pMax, b3_colorGray, draw->context );
+		}
+	}
+
+	draw->DrawSegmentFcn( pA, pB, b3_colorWhite, draw->context );
+	draw->DrawPointFcn( pA, 4.0f, b3_colorWhite, draw->context );
+	draw->DrawPointFcn( pB, 4.0f, b3_colorWhite, draw->context );
+
+	if ( joint->hertz > 0.0f && joint->enableSpring )
+	{
+		b3Pos pRest = b3OffsetPos( pA, b3MulSV( joint->length, axis ) );
+		draw->DrawPointFcn( pRest, 4.0f, b3_colorBlue, draw->context );
+	}
+}

vendor/box3d/src/src/hull_map.h

@@ -0,0 +1,24 @@
+// SPDX-FileCopyrightText: 2026 Erin Catto
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include "box3d/types.h"
+
+#include <stddef.h>
+
+// Content hash and equality over the whole baked hull. Shared by every hull
+// de-duplication map so they agree on identity.
+uint64_t b3HashHullData( const b3HullData* hull );
+bool b3CompareHullData( const b3HullData* hull1, const b3HullData* hull2 );
+
+// Map keyed by hull content. The world hull database stores a reference count,
+// compound baking stores a byte offset. Implementation lives in hull.c.
+#define NAME b3HullMap
+#define KEY_TY const b3HullData*
+#define VAL_TY int
+#define HEADER_MODE
+#include "verstable.h"
+
+// Total map allocation in bytes, excluding the stored hull data
+size_t b3HullMapByteCount( b3HullMap* map );

vendor/box3d/src/src/id_pool.c

@@ -0,0 +1,73 @@
+// SPDX-FileCopyrightText: 2025 Erin Catto
+// SPDX-License-Identifier: MIT
+
+#include "id_pool.h"
+
+b3IdPool b3CreateIdPool( void )
+{
+	b3IdPool pool = { 0 };
+	b3Array_Reserve( pool.freeArray, 32 );
+	return pool;
+}
+
+void b3DestroyIdPool( b3IdPool* pool )
+{
+	b3Array_Destroy( pool->freeArray );
+	*pool = (b3IdPool){ 0 };
+}
+
+int b3AllocId( b3IdPool* pool )
+{
+	int count = pool->freeArray.count;
+	if ( count > 0 )
+	{
+		int id = b3Array_Pop( pool->freeArray );
+		return id;
+	}
+
+	int id = pool->nextIndex;
+	pool->nextIndex += 1;
+	return id;
+}
+
+void b3FreeId( b3IdPool* pool, int id )
+{
+	B3_ASSERT( pool->nextIndex > 0 );
+	B3_ASSERT( 0 <= id && id < pool->nextIndex );
+
+	// todo does not work with assertion above
+	// should probably be `id == pool->nextIndex - 1`
+	if ( id == pool->nextIndex )
+	{
+		pool->nextIndex -= 1;
+		return;
+	}
+
+	b3Array_Push( pool->freeArray, id );
+}
+
+#if B3_ENABLE_VALIDATION
+
+void b3ValidateFreeId( const b3IdPool* pool, int id )
+{
+	int freeCount = pool->freeArray.count;
+	for ( int i = 0; i < freeCount; ++i )
+	{
+		if ( pool->freeArray.data[i] == id )
+		{
+			return;
+		}
+	}
+
+	B3_ASSERT( 0 );
+}
+
+#else
+
+void b3ValidateFreeId( const b3IdPool* pool, int id )
+{
+	B3_UNUSED( pool );
+	B3_UNUSED( id );
+}
+
+#endif

vendor/box3d/src/src/id_pool.h

@@ -0,0 +1,34 @@
+// SPDX-FileCopyrightText: 2025 Erin Catto
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include "container.h"
+
+typedef struct b3IdPool
+{
+	b3Array( int ) freeArray;
+	int nextIndex;
+} b3IdPool;
+
+b3IdPool b3CreateIdPool( void );
+void b3DestroyIdPool( b3IdPool* pool );
+
+int b3AllocId( b3IdPool* pool );
+void b3FreeId( b3IdPool* pool, int id );
+void b3ValidateFreeId( const b3IdPool* pool, int id );
+
+static inline int b3GetIdCount( const b3IdPool* pool )
+{
+	return pool->nextIndex - pool->freeArray.count;
+}
+
+static inline int b3GetIdCapacity( const b3IdPool* pool )
+{
+	return pool->nextIndex;
+}
+
+static inline int b3GetIdBytes( const b3IdPool* pool )
+{
+	return b3Array_ByteCount( pool->freeArray );
+}

vendor/box3d/src/src/island.c

@@ -0,0 +1,735 @@
+// SPDX-FileCopyrightText: 2025 Erin Catto
+// SPDX-License-Identifier: MIT
+
+#include "island.h"
+
+#include "body.h"
+#include "contact.h"
+#include "core.h"
+#include "id_pool.h"
+#include "joint.h"
+#include "physics_world.h"
+#include "solver_set.h"
+
+#include <stddef.h>
+
+b3Island* b3CreateIsland( b3World* world, int setIndex )
+{
+	B3_ASSERT( setIndex == b3_awakeSet || setIndex >= b3_firstSleepingSet );
+
+	int islandId = b3AllocId( &world->islandIdPool );
+
+	if ( islandId == world->islands.count )
+	{
+		b3Island emptyIsland = { 0 };
+		b3Array_Push( world->islands, emptyIsland );
+	}
+	else
+	{
+		B3_ASSERT( world->islands.data[islandId].setIndex == B3_NULL_INDEX );
+	}
+
+	b3SolverSet* set = b3Array_Get( world->solverSets, setIndex );
+
+	b3Island* island = b3Array_Get( world->islands, islandId );
+	island->setIndex = setIndex;
+	island->localIndex = set->islandSims.count;
+	island->islandId = islandId;
+	b3Array_Create( island->bodies );
+	b3Array_Create( island->contacts );
+	b3Array_Create( island->joints );
+	island->constraintRemoveCount = 0;
+
+	b3IslandSim* islandSim = b3Array_Emplace( set->islandSims );
+	islandSim->islandId = islandId;
+
+	return island;
+}
+
+void b3DestroyIsland( b3World* world, int islandId )
+{
+	if ( world->splitIslandId == islandId )
+	{
+		world->splitIslandId = B3_NULL_INDEX;
+	}
+
+	// assume island is empty
+	b3Island* island = b3Array_Get( world->islands, islandId );
+	b3SolverSet* set = b3Array_Get( world->solverSets, island->setIndex );
+	{
+		int localIndex = island->localIndex;
+		int lastIndex = set->islandSims.count - 1;
+		B3_ASSERT( 0 <= localIndex && localIndex <= lastIndex );
+		int moveIslandId = set->islandSims.data[lastIndex].islandId;
+		set->islandSims.data[localIndex] = set->islandSims.data[lastIndex];
+		world->islands.data[moveIslandId].localIndex = localIndex;
+		set->islandSims.count -= 1;
+	}
+
+	// Free island and id (preserve island generation)
+	b3Array_Destroy( island->bodies );
+	b3Array_Destroy( island->contacts );
+	b3Array_Destroy( island->joints );
+	island->constraintRemoveCount = 0;
+	island->localIndex = B3_NULL_INDEX;
+	island->islandId = B3_NULL_INDEX;
+	island->setIndex = B3_NULL_INDEX;
+
+	b3FreeId( &world->islandIdPool, islandId );
+}
+
+static int b3MergeIslands( b3World* world, int islandIdA, int islandIdB )
+{
+	if ( islandIdA == islandIdB )
+	{
+		return islandIdA;
+	}
+
+	if ( islandIdA == B3_NULL_INDEX )
+	{
+		B3_ASSERT( islandIdB != B3_NULL_INDEX );
+		return islandIdB;
+	}
+
+	if ( islandIdB == B3_NULL_INDEX )
+	{
+		B3_ASSERT( islandIdA != B3_NULL_INDEX );
+		return islandIdA;
+	}
+
+	b3Island* smallIsland;
+	b3Island* bigIsland;
+	{
+		b3Island* islandA = b3Array_Get( world->islands, islandIdA );
+		b3Island* islandB = b3Array_Get( world->islands, islandIdB );
+
+		// Keep the biggest island to reduce cache misses
+		if ( islandA->bodies.count >= islandB->bodies.count )
+		{
+			bigIsland = islandA;
+			smallIsland = islandB;
+		}
+		else
+		{
+			bigIsland = islandB;
+			smallIsland = islandA;
+		}
+	}
+
+	int bigIslandId = bigIsland->islandId;
+	b3Array_Reserve( bigIsland->bodies, bigIsland->bodies.count + smallIsland->bodies.count );
+
+	// Move bodies from smaller island to larger island
+	for ( int i = 0; i < smallIsland->bodies.count; ++i )
+	{
+		int bodyId = smallIsland->bodies.data[i];
+		b3Body* body = b3Array_Get( world->bodies, bodyId );
+		B3_VALIDATE( body->islandId == smallIsland->islandId );
+		body->islandId = bigIslandId;
+		body->islandIndex = bigIsland->bodies.count;
+		b3Array_Push( bigIsland->bodies, bodyId );
+	}
+
+	// Migrate contacts from smaller island to larger island
+	if ( smallIsland->contacts.count > 0 )
+	{
+		b3Array_Reserve( bigIsland->contacts, bigIsland->contacts.count + smallIsland->contacts.count );
+
+		for ( int i = 0; i < smallIsland->contacts.count; ++i )
+		{
+			b3ContactLink* link = smallIsland->contacts.data + i;
+			b3Contact* contact = b3Array_Get( world->contacts, link->contactId );
+			contact->islandId = bigIslandId;
+			contact->islandIndex = bigIsland->contacts.count;
+			b3Array_Push( bigIsland->contacts, *link );
+		}
+	}
+
+	// Migrate joints from smaller island to larger island
+	if ( smallIsland->joints.count > 0 )
+	{
+		b3Array_Reserve( bigIsland->joints, bigIsland->joints.count + smallIsland->joints.count );
+
+		for ( int i = 0; i < smallIsland->joints.count; ++i )
+		{
+			b3JointLink* link = smallIsland->joints.data + i;
+			b3Joint* joint = b3Array_Get( world->joints, link->jointId );
+			joint->islandId = bigIslandId;
+			joint->islandIndex = bigIsland->joints.count;
+			b3Array_Push( bigIsland->joints, *link );
+		}
+	}
+
+	// Track removed constraints
+	bigIsland->constraintRemoveCount += smallIsland->constraintRemoveCount;
+
+	b3DestroyIsland( world, smallIsland->islandId );
+
+	b3ValidateIsland( world, bigIslandId );
+
+	return bigIslandId;
+}
+
+static void b3AddContactToIsland( b3World* world, int islandId, b3Contact* contact )
+{
+	B3_ASSERT( contact->islandId == B3_NULL_INDEX );
+	B3_ASSERT( contact->islandIndex == B3_NULL_INDEX );
+
+	b3Island* island = b3Array_Get( world->islands, islandId );
+
+	contact->islandId = islandId;
+	contact->islandIndex = island->contacts.count;
+
+	b3ContactLink link;
+	link.contactId = contact->contactId;
+	link.bodyIdA = contact->edges[0].bodyId;
+	link.bodyIdB = contact->edges[1].bodyId;
+	b3Array_Push( island->contacts, link );
+
+	b3ValidateIsland( world, islandId );
+}
+
+// Link a contact into an island.
+void b3LinkContact( b3World* world, b3Contact* contact )
+{
+	B3_ASSERT( ( contact->flags & b3_contactTouchingFlag ) != 0 );
+
+	int bodyIdA = contact->edges[0].bodyId;
+	int bodyIdB = contact->edges[1].bodyId;
+
+	b3Body* bodyA = b3Array_Get( world->bodies, bodyIdA );
+	b3Body* bodyB = b3Array_Get( world->bodies, bodyIdB );
+
+	B3_ASSERT( bodyA->setIndex != b3_disabledSet && bodyB->setIndex != b3_disabledSet );
+	B3_ASSERT( bodyA->setIndex != b3_staticSet || bodyB->setIndex != b3_staticSet );
+
+	// Wake bodyB if bodyA is awake and bodyB is sleeping
+	if ( bodyA->setIndex == b3_awakeSet && bodyB->setIndex >= b3_firstSleepingSet )
+	{
+		b3WakeSolverSet( world, bodyB->setIndex );
+	}
+
+	// Wake bodyA if bodyB is awake and bodyA is sleeping
+	if ( bodyB->setIndex == b3_awakeSet && bodyA->setIndex >= b3_firstSleepingSet )
+	{
+		b3WakeSolverSet( world, bodyA->setIndex );
+	}
+
+	int islandIdA = bodyA->islandId;
+	int islandIdB = bodyB->islandId;
+
+	// Static bodies have null island indices.
+	B3_ASSERT( bodyA->setIndex != b3_staticSet || islandIdA == B3_NULL_INDEX );
+	B3_ASSERT( bodyB->setIndex != b3_staticSet || islandIdB == B3_NULL_INDEX );
+	B3_ASSERT( islandIdA != B3_NULL_INDEX || islandIdB != B3_NULL_INDEX );
+
+	// Merge islands. This will destroy one of the islands.
+	int finalIslandId = b3MergeIslands( world, islandIdA, islandIdB );
+
+	// Add contact to the island that survived
+	b3AddContactToIsland( world, finalIslandId, contact );
+}
+
+// This is called when a contact no longer has contact points or when a contact is destroyed.
+void b3UnlinkContact( b3World* world, b3Contact* contact )
+{
+	B3_ASSERT( contact->islandId != B3_NULL_INDEX );
+
+	// remove from island
+	int islandId = contact->islandId;
+	b3Island* island = b3Array_Get( world->islands, islandId );
+
+	int removeIndex = contact->islandIndex;
+	B3_ASSERT( 0 <= removeIndex && removeIndex < island->contacts.count );
+	B3_ASSERT( island->contacts.data[removeIndex].contactId == contact->contactId );
+
+	int movedIndex = b3Array_RemoveSwap( island->contacts, removeIndex );
+	if ( movedIndex != B3_NULL_INDEX )
+	{
+		// Fix islandIndex on the contact that was swapped into removeIndex
+		b3ContactLink* movedLink = island->contacts.data + removeIndex;
+		b3Contact* movedContact = b3Array_Get( world->contacts, movedLink->contactId );
+		B3_ASSERT( movedContact->islandIndex == movedIndex );
+		movedContact->islandIndex = removeIndex;
+	}
+
+	contact->islandId = B3_NULL_INDEX;
+	contact->islandIndex = B3_NULL_INDEX;
+	island->constraintRemoveCount += 1;
+
+	b3ValidateIsland( world, islandId );
+}
+
+static void b3AddJointToIsland( b3World* world, int islandId, b3Joint* joint )
+{
+	B3_ASSERT( joint->islandId == B3_NULL_INDEX );
+	B3_ASSERT( joint->islandIndex == B3_NULL_INDEX );
+
+	b3Island* island = b3Array_Get( world->islands, islandId );
+
+	joint->islandId = islandId;
+	joint->islandIndex = island->joints.count;
+
+	b3JointLink link;
+	link.jointId = joint->jointId;
+	link.bodyIdA = joint->edges[0].bodyId;
+	link.bodyIdB = joint->edges[1].bodyId;
+	b3Array_Push( island->joints, link );
+
+	b3ValidateIsland( world, islandId );
+}
+
+void b3LinkJoint( b3World* world, b3Joint* joint )
+{
+	b3Body* bodyA = b3Array_Get( world->bodies, joint->edges[0].bodyId );
+	b3Body* bodyB = b3Array_Get( world->bodies, joint->edges[1].bodyId );
+
+	B3_ASSERT( bodyA->type == b3_dynamicBody || bodyB->type == b3_dynamicBody );
+
+	if ( bodyA->setIndex == b3_awakeSet && bodyB->setIndex >= b3_firstSleepingSet )
+	{
+		b3WakeSolverSet( world, bodyB->setIndex );
+	}
+	else if ( bodyB->setIndex == b3_awakeSet && bodyA->setIndex >= b3_firstSleepingSet )
+	{
+		b3WakeSolverSet( world, bodyA->setIndex );
+	}
+
+	int islandIdA = bodyA->islandId;
+	int islandIdB = bodyB->islandId;
+
+	B3_ASSERT( islandIdA != B3_NULL_INDEX || islandIdB != B3_NULL_INDEX );
+
+	// Merge islands. This will destroy one of the islands.
+	int finalIslandId = b3MergeIslands( world, islandIdA, islandIdB );
+
+	// Add joint the island that survived
+	b3AddJointToIsland( world, finalIslandId, joint );
+}
+
+void b3UnlinkJoint( b3World* world, b3Joint* joint )
+{
+	if ( joint->islandId == B3_NULL_INDEX )
+	{
+		return;
+	}
+
+	// remove from island
+	int islandId = joint->islandId;
+	b3Island* island = b3Array_Get( world->islands, islandId );
+
+	int removeIndex = joint->islandIndex;
+	B3_ASSERT( 0 <= removeIndex && removeIndex < island->joints.count );
+	B3_ASSERT( island->joints.data[removeIndex].jointId == joint->jointId );
+
+	int movedIndex = b3Array_RemoveSwap( island->joints, removeIndex );
+	if ( movedIndex != B3_NULL_INDEX )
+	{
+		// Fix islandIndex on the joint that was swapped into removeIndex
+		b3JointLink* movedLink = island->joints.data + removeIndex;
+		b3Joint* movedJoint = b3Array_Get( world->joints, movedLink->jointId );
+		B3_ASSERT( movedJoint->islandIndex == movedIndex );
+		movedJoint->islandIndex = removeIndex;
+	}
+
+	joint->islandId = B3_NULL_INDEX;
+	joint->islandIndex = B3_NULL_INDEX;
+	island->constraintRemoveCount += 1;
+
+	b3ValidateIsland( world, islandId );
+}
+
+// Find parent of a node. Use path halving to speed up further queries.
+static inline int b3IslandFindParent( int* parents, int node )
+{
+	// Walk the chain of parents to find the node that is its own parent (the root)
+	while ( parents[node] != node )
+	{
+		int grandParent = parents[parents[node]];
+		parents[node] = grandParent;
+		node = grandParent;
+	}
+
+	return node;
+}
+
+// Connect the components containing node1 and node2.
+// Uses rank to keep tree balanced. Tracks per-component contact and joint counts.
+static inline void b3IslandUnion( int* parents, int* ranks, int node1, int node2, int* contactCounts, int* jointCounts )
+{
+	int root1 = b3IslandFindParent( parents, node1 );
+	int root2 = b3IslandFindParent( parents, node2 );
+	if ( root1 != root2 )
+	{
+		if ( ranks[root1] < ranks[root2] )
+		{
+			parents[root1] = root2;
+			contactCounts[root2] += contactCounts[root1];
+			jointCounts[root2] += jointCounts[root1];
+		}
+		else if ( ranks[root1] > ranks[root2] )
+		{
+			parents[root2] = root1;
+			contactCounts[root1] += contactCounts[root2];
+			jointCounts[root1] += jointCounts[root2];
+		}
+		else
+		{
+			parents[root2] = root1;
+			ranks[root1] += 1;
+			contactCounts[root1] += contactCounts[root2];
+			jointCounts[root1] += jointCounts[root2];
+		}
+	}
+}
+
+// This uses union-find.
+// https://en.wikipedia.org/wiki/Disjoint-set_data_structure
+void b3SplitIsland( b3World* world, int baseId )
+{
+	b3Island* baseIsland = b3Array_Get( world->islands, baseId );
+	B3_ASSERT( baseIsland->constraintRemoveCount > 0 );
+	B3_ASSERT( baseIsland->setIndex == b3_awakeSet );
+
+	b3ValidateIsland( world, baseId );
+
+	// Cache base island fields before b3CreateIsland, which may reallocate
+	// world->islands and invalidate the baseIsland pointer.
+	int baseBodyCount = baseIsland->bodies.count;
+	int* baseBodyIds = baseIsland->bodies.data;
+	int baseBodyCapacity = baseIsland->bodies.capacity;
+
+	int baseContactCount = baseIsland->contacts.count;
+	b3ContactLink* baseContacts = baseIsland->contacts.data;
+	int baseContactCapacity = baseIsland->contacts.capacity;
+
+	int baseJointCount = baseIsland->joints.count;
+	b3JointLink* baseJoints = baseIsland->joints.data;
+	int baseJointCapacity = baseIsland->joints.capacity;
+
+	b3Stack* alloc = &world->stack;
+
+	// No lock is needed because I ensure the allocator is not used while this task is active.
+	// Allocate contactCounts and jointCounts before ranks so ranks can be freed first (LIFO arena).
+	int* parents = b3StackAlloc( alloc, baseBodyCount * sizeof( int ), "parents" );
+	int* contactCounts = b3StackAlloc( alloc, baseBodyCount * sizeof( int ), "contact counts" );
+	int* jointCounts = b3StackAlloc( alloc, baseBodyCount * sizeof( int ), "joint counts" );
+	int* ranks = b3StackAlloc( alloc, baseBodyCount * sizeof( int ), "ranks" );
+	for ( int i = 0; i < baseBodyCount; ++i )
+	{
+		parents[i] = i;
+		ranks[i] = 0;
+		contactCounts[i] = 0;
+		jointCounts[i] = 0;
+	}
+
+	b3Body* bodies = world->bodies.data;
+
+	// Union over contacts, tracking per-component contact counts
+	for ( int i = 0; i < baseContactCount; ++i )
+	{
+		int bodyIdA = baseContacts[i].bodyIdA;
+		int bodyIdB = baseContacts[i].bodyIdB;
+		B3_VALIDATE( 0 <= bodyIdA && bodyIdA < world->bodies.count );
+		B3_VALIDATE( 0 <= bodyIdB && bodyIdB < world->bodies.count );
+		b3Body* bodyA = bodies + bodyIdA;
+		b3Body* bodyB = bodies + bodyIdB;
+		int islandIndexA = bodyA->islandIndex;
+		int islandIndexB = bodyB->islandIndex;
+
+		// Only connect non-static bodies
+		if ( islandIndexA != B3_NULL_INDEX && islandIndexB != B3_NULL_INDEX )
+		{
+			B3_VALIDATE( 0 <= islandIndexA && islandIndexA < baseBodyCount );
+			B3_VALIDATE( 0 <= islandIndexB && islandIndexB < baseBodyCount );
+			b3IslandUnion( parents, ranks, islandIndexA, islandIndexB, contactCounts, jointCounts );
+			int root = b3IslandFindParent( parents, islandIndexA );
+			contactCounts[root] += 1;
+		}
+		else
+		{
+			int islandIndex = islandIndexA != B3_NULL_INDEX ? islandIndexA : islandIndexB;
+			int root = b3IslandFindParent( parents, islandIndex );
+			contactCounts[root] += 1;
+		}
+	}
+
+	// Union over joints, tracking per-component joint counts
+	for ( int i = 0; i < baseJointCount; ++i )
+	{
+		int bodyIdA = baseJoints[i].bodyIdA;
+		int bodyIdB = baseJoints[i].bodyIdB;
+		B3_VALIDATE( 0 <= bodyIdA && bodyIdA < world->bodies.count );
+		B3_VALIDATE( 0 <= bodyIdB && bodyIdB < world->bodies.count );
+		b3Body* bodyA = bodies + bodyIdA;
+		b3Body* bodyB = bodies + bodyIdB;
+		int islandIndexA = bodyA->islandIndex;
+		int islandIndexB = bodyB->islandIndex;
+
+		// Only connect non-static bodies
+		if ( islandIndexA != B3_NULL_INDEX && islandIndexB != B3_NULL_INDEX )
+		{
+			B3_VALIDATE( 0 <= islandIndexA && islandIndexA < baseBodyCount );
+			B3_VALIDATE( 0 <= islandIndexB && islandIndexB < baseBodyCount );
+			b3IslandUnion( parents, ranks, islandIndexA, islandIndexB, contactCounts, jointCounts );
+			int root = b3IslandFindParent( parents, islandIndexA );
+			jointCounts[root] += 1;
+		}
+		else
+		{
+			int islandIndex = islandIndexA != B3_NULL_INDEX ? islandIndexA : islandIndexB;
+			int root = b3IslandFindParent( parents, islandIndex );
+			jointCounts[root] += 1;
+		}
+	}
+
+	// Done with ranks
+	b3StackFree( alloc, ranks );
+	ranks = NULL;
+
+	// Flatten all parent indices and count connected components.
+	int componentCount = 0;
+	for ( int i = 0; i < baseBodyCount; ++i )
+	{
+		parents[i] = b3IslandFindParent( parents, i );
+		if ( parents[i] == i )
+		{
+			componentCount += 1;
+		}
+	}
+
+	// Early return — island is still fully connected, no split needed.
+	if ( componentCount == 1 )
+	{
+		baseIsland->constraintRemoveCount = 0;
+		b3StackFree( alloc, jointCounts );
+		b3StackFree( alloc, contactCounts );
+		b3StackFree( alloc, parents );
+		return;
+	}
+
+	// Detach body/contact/joint arrays from base island so b3DestroyIsland won't free them
+	baseIsland->bodies.data = NULL;
+	baseIsland->bodies.count = 0;
+	baseIsland->bodies.capacity = 0;
+
+	baseIsland->contacts.data = NULL;
+	baseIsland->contacts.count = 0;
+	baseIsland->contacts.capacity = 0;
+
+	baseIsland->joints.data = NULL;
+	baseIsland->joints.count = 0;
+	baseIsland->joints.capacity = 0;
+
+	// Null so code below doesn't accidentally use this.
+	baseIsland = NULL;
+
+	// Map from body index to new island index. Only set for root bodies.
+	int* rootMap = b3StackAlloc( alloc, baseBodyCount * sizeof( int ), "root map" );
+	for ( int i = 0; i < baseBodyCount; ++i )
+	{
+		rootMap[i] = B3_NULL_INDEX;
+	}
+
+	int* componentBodyCounts = b3StackAlloc( alloc, componentCount * sizeof( int ), "component body counts" );
+	int* componentContactCounts = b3StackAlloc( alloc, componentCount * sizeof( int ), "component contact counts" );
+	int* componentJointCounts = b3StackAlloc( alloc, componentCount * sizeof( int ), "component joint counts" );
+	int islandCount = 0;
+
+	// Find the root body for each body and create islands as needed.
+	// Extract per-component counts from the root nodes' accumulated counts.
+	for ( int i = 0; i < baseBodyCount; ++i )
+	{
+		int rootIndex = parents[i];
+		if ( rootMap[rootIndex] == B3_NULL_INDEX )
+		{
+			rootMap[rootIndex] = islandCount;
+			componentBodyCounts[islandCount] = 0;
+			componentContactCounts[islandCount] = contactCounts[rootIndex];
+			componentJointCounts[islandCount] = jointCounts[rootIndex];
+			islandCount += 1;
+		}
+
+		componentBodyCounts[rootMap[rootIndex]] += 1;
+	}
+
+	B3_ASSERT( islandCount == componentCount );
+
+	// Map from new island index to island id
+	int* islandIds = b3StackAlloc( alloc, islandCount * sizeof( int ), "island ids" );
+
+	// Create new islands and reserve body/contact/joint arrays
+	for ( int i = 0; i < islandCount; ++i )
+	{
+		// WARNING: this invalidates baseIsland pointer
+		b3Island* newIsland = b3CreateIsland( world, b3_awakeSet );
+		islandIds[i] = newIsland->islandId;
+
+		// Reserve arrays to avoid wasteful growth and memcpy.
+		b3Array_Reserve( newIsland->bodies, componentBodyCounts[i] );
+		b3Array_Reserve( newIsland->contacts, componentContactCounts[i] );
+		b3Array_Reserve( newIsland->joints, componentJointCounts[i] );
+	}
+
+	// Assign bodies to new islands
+	for ( int i = 0; i < baseBodyCount; ++i )
+	{
+		int bodyId = baseBodyIds[i];
+		int root = b3IslandFindParent( parents, i );
+		int newIslandId = islandIds[rootMap[root]];
+
+		b3Body* body = b3Array_Get( world->bodies, bodyId );
+		b3Island* newIsland = b3Array_Get( world->islands, newIslandId );
+
+		body->islandId = newIslandId;
+		body->islandIndex = newIsland->bodies.count;
+
+		// Ensure the array has the correct capacity
+		B3_VALIDATE( newIsland->bodies.count < newIsland->bodies.capacity );
+		b3Array_Push( newIsland->bodies, bodyId );
+	}
+
+	// Assign contacts to the island of their bodies
+	for ( int i = 0; i < baseContactCount; ++i )
+	{
+		b3ContactLink* link = baseContacts + i;
+		b3Contact* contact = b3Array_Get( world->contacts, link->contactId );
+
+		// Static bodies don't have an island id.
+		b3Body* bodyA = b3Array_Get( world->bodies, link->bodyIdA );
+		b3Body* bodyB = b3Array_Get( world->bodies, link->bodyIdB );
+		int targetIslandId = bodyA->islandId != B3_NULL_INDEX ? bodyA->islandId : bodyB->islandId;
+
+		b3Island* targetIsland = b3Array_Get( world->islands, targetIslandId );
+		contact->islandId = targetIslandId;
+		contact->islandIndex = targetIsland->contacts.count;
+
+		// Ensure the array has the correct capacity
+		B3_VALIDATE( targetIsland->contacts.count < targetIsland->contacts.capacity );
+		b3Array_Push( targetIsland->contacts, *link );
+	}
+
+	// Assign joints to the island of their bodies
+	for ( int i = 0; i < baseJointCount; ++i )
+	{
+		b3JointLink* link = baseJoints + i;
+		b3Joint* joint = b3Array_Get( world->joints, link->jointId );
+
+		// Static bodies don't have an island id.
+		b3Body* bodyA = b3Array_Get( world->bodies, link->bodyIdA );
+		b3Body* bodyB = b3Array_Get( world->bodies, link->bodyIdB );
+		int targetIslandId = bodyA->islandId != B3_NULL_INDEX ? bodyA->islandId : bodyB->islandId;
+
+		b3Island* targetIsland = b3Array_Get( world->islands, targetIslandId );
+		joint->islandId = targetIslandId;
+		joint->islandIndex = targetIsland->joints.count;
+
+		// Ensure the array has the correct capacity
+		B3_VALIDATE( targetIsland->joints.count < targetIsland->joints.capacity );
+		b3Array_Push( targetIsland->joints, *link );
+	}
+
+	// Destroy the base island
+	b3DestroyIsland( world, baseId );
+
+	// Free the detached arrays manually
+	b3Free( baseBodyIds, baseBodyCapacity * sizeof( int ) );
+	b3Free( baseContacts, baseContactCapacity * sizeof( b3ContactLink ) );
+	b3Free( baseJoints, baseJointCapacity * sizeof( b3JointLink ) );
+
+	// Free arena items in LIFO order
+	b3StackFree( alloc, islandIds );
+	b3StackFree( alloc, componentJointCounts );
+	b3StackFree( alloc, componentContactCounts );
+	b3StackFree( alloc, componentBodyCounts );
+	b3StackFree( alloc, rootMap );
+	b3StackFree( alloc, jointCounts );
+	b3StackFree( alloc, contactCounts );
+	b3StackFree( alloc, parents );
+}
+
+// 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 union find and
+// touches a lot of memory, so it can be 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 b3SplitIslandTask( void* context )
+{
+	b3TracyCZoneNC( split, "Split Island", b3_colorOlive, true );
+
+	uint64_t ticks = b3GetTicks();
+	b3World* world = context;
+
+	B3_ASSERT( world->splitIslandId != B3_NULL_INDEX );
+
+	b3SplitIsland( world, world->splitIslandId );
+
+	world->splitIslandId = B3_NULL_INDEX;
+	world->profile.splitIslands += b3GetMilliseconds( ticks );
+	b3TracyCZoneEnd( split );
+}
+
+#if B3_ENABLE_VALIDATION
+void b3ValidateIsland( b3World* world, int islandId )
+{
+	if ( islandId == B3_NULL_INDEX )
+	{
+		return;
+	}
+
+	b3Island* island = b3Array_Get( world->islands, islandId );
+	B3_ASSERT( island->islandId == islandId );
+	B3_ASSERT( island->setIndex != B3_NULL_INDEX );
+
+	{
+		B3_ASSERT( island->bodies.count > 0 );
+		B3_ASSERT( island->bodies.count <= b3GetIdCount( &world->bodyIdPool ) );
+
+		for ( int i = 0; i < island->bodies.count; ++i )
+		{
+			b3Body* body = b3Array_Get( world->bodies, island->bodies.data[i] );
+			B3_ASSERT( body->islandId == islandId );
+			B3_ASSERT( body->islandIndex == i );
+			B3_ASSERT( body->setIndex == island->setIndex );
+		}
+	}
+
+	if ( island->contacts.count > 0 )
+	{
+		B3_ASSERT( island->contacts.count <= b3GetIdCount( &world->contactIdPool ) );
+
+		for ( int i = 0; i < island->contacts.count; ++i )
+		{
+			b3ContactLink* link = island->contacts.data + i;
+			b3Contact* contact = b3Array_Get( world->contacts, link->contactId );
+			B3_ASSERT( contact->setIndex == island->setIndex );
+			B3_ASSERT( contact->islandId == islandId );
+			B3_ASSERT( contact->islandIndex == i );
+		}
+	}
+
+	if ( island->joints.count > 0 )
+	{
+		B3_ASSERT( island->joints.count <= b3GetIdCount( &world->jointIdPool ) );
+
+		for ( int i = 0; i < island->joints.count; ++i )
+		{
+			b3JointLink* link = island->joints.data + i;
+			b3Joint* joint = b3Array_Get( world->joints, link->jointId );
+			B3_ASSERT( joint->setIndex == island->setIndex );
+			B3_ASSERT( joint->islandId == islandId );
+			B3_ASSERT( joint->islandIndex == i );
+		}
+	}
+}
+
+#else
+
+void b3ValidateIsland( b3World* world, int islandId )
+{
+	B3_UNUSED( world );
+	B3_UNUSED( islandId );
+}
+#endif

vendor/box3d/src/src/island.h

@@ -0,0 +1,101 @@
+// SPDX-FileCopyrightText: 2025 Erin Catto
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include "container.h"
+
+#include <stdint.h>
+
+typedef struct b3Contact b3Contact;
+typedef struct b3Joint b3Joint;
+typedef struct b3World b3World;
+
+// Cached contact data stored in the island for fast contiguous iteration.
+// Avoids touching b3Contact during union-find in b3SplitIsland.
+typedef struct b3ContactLink
+{
+	int contactId;
+	int bodyIdA;
+	int bodyIdB;
+} b3ContactLink;
+
+b3DeclareArray( b3ContactLink );
+
+// Cached joint data stored in the island for fast contiguous iteration.
+typedef struct b3JointLink
+{
+	int jointId;
+	int bodyIdA;
+	int bodyIdB;
+} b3JointLink;
+
+b3DeclareArray( b3JointLink );
+
+// 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.
+// Contacts are touching.
+// Contacts and joints may connect to static bodies, but static bodies are not in the island.
+// https://en.wikipedia.org/wiki/Component_(graph_theory)
+// https://en.wikipedia.org/wiki/Dynamic_connectivity
+typedef struct b3Island
+{
+	// index of solver set stored in b3World
+	// may be B3_NULL_INDEX
+	int setIndex;
+
+	// island index within set
+	// may be B3_NULL_INDEX
+	int localIndex;
+
+	int islandId;
+
+	// 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;
+
+	// I tried using a stack array for this but the data pointer goes out of
+	// sync when the world island array grows.
+	b3Array( int ) bodies;
+
+	// Contacts and joints that belong to this island. May connect to static
+	// bodies not in the island.
+	// Each link has the two body ids so that b3SplitIsland's union-find pass
+	// never needs to touch b3Contact/b3Joint.
+	b3Array( b3ContactLink ) contacts;
+	b3Array( b3JointLink ) joints;
+
+} b3Island;
+
+// This is used to move islands across solver sets
+typedef struct b3IslandSim
+{
+	int islandId;
+} b3IslandSim;
+
+b3Island* b3CreateIsland( b3World* world, int setIndex );
+void b3DestroyIsland( b3World* world, int islandId );
+
+// Link contacts into the island graph when it starts having contact points
+void b3LinkContact( b3World* world, b3Contact* contact );
+
+// Unlink contact from the island graph when it stops having contact points
+void b3UnlinkContact( b3World* world, b3Contact* contact );
+
+// Link a joint into the island graph when it is created
+void b3LinkJoint( b3World* world, b3Joint* joint );
+
+// Unlink a joint from the island graph when it is destroyed
+void b3UnlinkJoint( b3World* world, b3Joint* joint );
+
+void b3SplitIsland( b3World* world, int baseId );
+void b3SplitIslandTask( void* context );
+
+void b3ValidateIsland( b3World* world, int islandId );

vendor/box3d/src/src/joint.h

@@ -0,0 +1,415 @@
+// SPDX-FileCopyrightText: 2025 Erin Catto
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include "math_internal.h"
+#include "solver.h"
+
+#include "box3d/types.h"
+
+typedef struct b3DebugDraw b3DebugDraw;
+typedef struct b3StepContext b3StepContext;
+typedef struct b3World b3World;
+
+/// 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 b3JointEdge
+{
+	int bodyId;
+	int prevKey;
+	int nextKey;
+} b3JointEdge;
+
+// Map from b3JointId to b3Joint in the solver sets
+typedef struct b3Joint
+{
+	void* userData;
+
+	// index of simulation set stored in b3World
+	// B3_NULL_INDEX when slot is free
+	int setIndex;
+
+	// index into the constraint graph color array, may be B3_NULL_INDEX for sleeping/disabled joints
+	// B3_NULL_INDEX when slot is free
+	int colorIndex;
+
+	// joint index within set or graph color
+	// B3_NULL_INDEX when slot is free
+	int localIndex;
+
+	b3JointEdge edges[2];
+
+	int jointId;
+	int islandId;
+
+	// Index into the island's joints array for O(1) swap-removal.
+	// B3_NULL_INDEX when not in an island.
+	int islandIndex;
+
+	float drawScale;
+
+	b3JointType type;
+
+	// This is monotonically advanced when a body is allocated in this slot
+	// Used to check for invalid b3JointId
+	uint16_t generation;
+
+	bool collideConnected;
+} b3Joint;
+
+typedef struct b3DistanceJoint
+{
+	float length;
+	float hertz;
+	float dampingRatio;
+	float lowerSpringForce;
+	float upperSpringForce;
+	float minLength;
+	float maxLength;
+
+	float maxMotorForce;
+	float motorSpeed;
+
+	float impulse;
+	float lowerImpulse;
+	float upperImpulse;
+	float motorImpulse;
+
+	int indexA;
+	int indexB;
+	b3Vec3 anchorA;
+	b3Vec3 anchorB;
+	b3Vec3 deltaCenter;
+	b3Softness distanceSoftness;
+	float axialMass;
+
+	bool enableSpring;
+	bool enableLimit;
+	bool enableMotor;
+} b3DistanceJoint;
+
+typedef struct b3MotorJoint
+{
+	b3Vec3 linearVelocity;
+	b3Vec3 angularVelocity;
+	float maxVelocityForce;
+	float maxVelocityTorque;
+	float linearHertz;
+	float linearDampingRatio;
+	float maxSpringForce;
+	float angularHertz;
+	float angularDampingRatio;
+	float maxSpringTorque;
+
+	b3Vec3 linearVelocityImpulse;
+	b3Vec3 angularVelocityImpulse;
+	b3Vec3 linearSpringImpulse;
+	b3Vec3 angularSpringImpulse;
+
+	b3Softness linearSpring;
+	b3Softness angularSpring;
+
+	int indexA;
+	int indexB;
+	b3Transform frameA;
+	b3Transform frameB;
+	b3Vec3 deltaCenter;
+	b3Matrix3 angularMass;
+} b3MotorJoint;
+
+typedef struct b3ParallelJoint
+{
+	float hertz;
+	float dampingRatio;
+	float maxTorque;
+
+	b3Vec2 perpImpulse;
+	b3Vec3 perpAxisX;
+	b3Vec3 perpAxisY;
+
+	b3Quat quatA;
+	b3Quat quatB;
+	int indexA;
+	int indexB;
+	b3Softness softness;
+} b3ParallelJoint;
+
+typedef struct b3PrismaticJoint
+{
+	b3Vec2 perpImpulse;
+	b3Vec3 angularImpulse;
+	float springImpulse;
+	float motorImpulse;
+	float lowerImpulse;
+	float upperImpulse;
+	float hertz;
+	float dampingRatio;
+	float maxMotorForce;
+	float motorSpeed;
+	float targetTranslation;
+	float lowerTranslation;
+	float upperTranslation;
+
+	int indexA;
+	int indexB;
+	b3Transform frameA;
+	b3Transform frameB;
+	b3Vec3 jointAxis;
+	b3Vec3 perpAxisY;
+	b3Vec3 perpAxisZ;
+	b3Vec3 deltaCenter;
+	float deltaAngle;
+	b3Matrix3 rotationMass;
+	b3Softness springSoftness;
+
+	bool enableSpring;
+	bool enableLimit;
+	bool enableMotor;
+} b3PrismaticJoint;
+
+typedef struct b3RevoluteJoint
+{
+	b3Vec3 linearImpulse;
+	b3Vec2 perpImpulse;
+	float springImpulse;
+	float motorImpulse;
+	float lowerImpulse;
+	float upperImpulse;
+	float hertz;
+	float dampingRatio;
+	float maxMotorTorque;
+	float motorSpeed;
+	float targetAngle;
+	float lowerAngle;
+	float upperAngle;
+
+	int indexA;
+	int indexB;
+	b3Transform frameA;
+	b3Transform frameB;
+	b3Vec3 rotationAxisZ;
+	b3Vec3 perpAxisX;
+	b3Vec3 perpAxisY;
+	b3Vec3 deltaCenter;
+	float deltaAngle;
+	float axialMass;
+	b3Softness springSoftness;
+
+	bool enableSpring;
+	bool enableMotor;
+	bool enableLimit;
+} b3RevoluteJoint;
+
+typedef struct b3SphericalJoint
+{
+	b3Vec3 linearImpulse;
+	b3Vec3 springImpulse;
+	b3Vec3 motorImpulse;
+	float lowerTwistImpulse;
+	float upperTwistImpulse;
+	float swingImpulse;
+	float hertz;
+	float dampingRatio;
+	float maxMotorTorque;
+	b3Vec3 motorVelocity;
+	float lowerTwistAngle;
+	float upperTwistAngle;
+	float coneAngle;
+	b3Quat targetRotation;
+
+	int indexA;
+	int indexB;
+	b3Transform frameA;
+	b3Transform frameB;
+	b3Vec3 deltaCenter;
+	b3Vec3 swingAxis;
+	b3Vec3 twistJacobian;
+
+	b3Matrix3 rotationMass;
+	float swingMass;
+	float twistMass;
+	b3Softness springSoftness;
+
+	bool enableSpring;
+	bool enableMotor;
+	bool enableConeLimit;
+	bool enableTwistLimit;
+} b3SphericalJoint;
+
+typedef struct b3WeldJoint
+{
+	float linearHertz;
+	float linearDampingRatio;
+	float angularHertz;
+	float angularDampingRatio;
+
+	b3Softness linearSpring;
+	b3Softness angularSpring;
+	b3Vec3 linearImpulse;
+	b3Vec3 angularImpulse;
+
+	int indexA;
+	int indexB;
+	b3Transform frameA;
+	b3Transform frameB;
+	b3Vec3 deltaCenter;
+
+	b3Matrix3 angularMass;
+} b3WeldJoint;
+
+typedef struct b3WheelJoint
+{
+	b3Vec2 linearImpulse;
+	b3Vec2 angularImpulse;
+	float spinImpulse;
+	float maxSpinTorque;
+	float spinSpeed;
+	float suspensionSpringImpulse;
+	float lowerSuspensionImpulse;
+	float upperSuspensionImpulse;
+	float lowerSuspensionLimit;
+	float upperSuspensionLimit;
+	float suspensionHertz;
+	float suspensionDampingRatio;
+	float steeringSpringImpulse;
+	float lowerSteeringImpulse;
+	float upperSteeringImpulse;
+	float lowerSteeringLimit;
+	float upperSteeringLimit;
+	float targetSteeringAngle;
+	float maxSteeringTorque;
+	float steeringHertz;
+	float steeringDampingRatio;
+
+	int indexA;
+	int indexB;
+	b3Transform frameA;
+	b3Transform frameB;
+	b3Vec3 deltaCenter;
+	float spinMass;
+	float suspensionMass;
+	float steeringMass;
+	b3Softness suspensionSoftness;
+	b3Softness steeringSoftness;
+
+	bool enableSpinMotor;
+	bool enableSuspensionSpring;
+	bool enableSuspensionLimit;
+	bool enableSteering;
+	bool enableSteeringLimit;
+	bool enableSteeringMotor;
+} b3WheelJoint;
+
+/// The base joint class. Joints are used to constraint two bodies together in
+/// various fashions. Some joints also feature limits and motors.
+typedef struct b3JointSim
+{
+	int jointId;
+
+	int bodyIdA;
+	int bodyIdB;
+
+	b3JointType type;
+
+	// Joint frames local to body origin
+	b3Transform localFrameA;
+	b3Transform localFrameB;
+
+	float invMassA, invMassB;
+	b3Matrix3 invIA, invIB;
+
+	float constraintHertz;
+	float constraintDampingRatio;
+
+	b3Softness constraintSoftness;
+
+	float forceThreshold;
+	float torqueThreshold;
+
+	bool fixedRotation;
+
+	union
+	{
+		b3DistanceJoint distanceJoint;
+		b3MotorJoint motorJoint;
+		b3ParallelJoint parallelJoint;
+		b3RevoluteJoint revoluteJoint;
+		b3SphericalJoint sphericalJoint;
+		b3PrismaticJoint prismaticJoint;
+		b3WeldJoint weldJoint;
+		b3WheelJoint wheelJoint;
+	};
+} b3JointSim;
+
+void b3DestroyJointInternal( b3World* world, b3Joint* joint, bool wakeBodies );
+
+b3Joint* b3GetJointFullId( b3World* world, b3JointId jointId );
+b3JointSim* b3GetJointSim( b3World* world, b3Joint* joint );
+b3JointSim* b3GetJointSimCheckType( b3JointId jointId, b3JointType type );
+
+void b3PrepareJoint( b3JointSim* joint, b3StepContext* context );
+void b3WarmStartJoint( b3JointSim* joint, b3StepContext* context );
+void b3SolveJoint( b3JointSim* joint, b3StepContext* context, bool useBias );
+
+void b3PrepareJoints_Overflow( b3StepContext* context );
+void b3WarmStartJoints_Overflow( b3StepContext* context );
+void b3SolveJoints_Overflow( b3StepContext* context, bool useBias );
+
+void b3GetJointReaction( b3World* world, b3JointSim* sim, float invTimeStep, float* force, float* torque );
+
+void b3DrawJoint( b3DebugDraw* draw, b3World* world, b3Joint* joint );
+
+b3Vec3 b3GetDistanceJointForce( b3World* world, b3JointSim* base );
+b3Vec3 b3GetMotorJointForce( b3World* world, b3JointSim* base );
+b3Vec3 b3GetPrismaticJointForce( b3World* world, b3JointSim* base );
+b3Vec3 b3GetRevoluteJointForce( b3World* world, b3JointSim* base );
+b3Vec3 b3GetSphericalJointForce( b3World* world, b3JointSim* base );
+b3Vec3 b3GetWeldJointForce( b3World* world, b3JointSim* base );
+b3Vec3 b3GetWheelJointForce( b3World* world, b3JointSim* base );
+
+b3Vec3 b3GetMotorJointTorque( b3World* world, b3JointSim* base );
+b3Vec3 b3GetParallelJointTorque( b3World* world, b3JointSim* base );
+b3Vec3 b3GetPrismaticJointTorque( b3World* world, b3JointSim* base );
+b3Vec3 b3GetRevoluteJointTorque( b3World* world, b3JointSim* base );
+b3Vec3 b3GetSphericalJointTorque( b3World* world, b3JointSim* base );
+b3Vec3 b3GetWeldJointTorque( b3World* world, b3JointSim* base );
+b3Vec3 b3GetWheelJointTorque( b3World* world, b3JointSim* base );
+
+void b3PrepareDistanceJoint( b3JointSim* base, b3StepContext* context );
+void b3PrepareMotorJoint( b3JointSim* base, b3StepContext* context );
+void b3PrepareParallelJoint( b3JointSim* base, b3StepContext* context );
+void b3PreparePrismaticJoint( b3JointSim* base, b3StepContext* context );
+void b3PrepareRevoluteJoint( b3JointSim* base, b3StepContext* context );
+void b3PrepareSphericalJoint( b3JointSim* base, b3StepContext* context );
+void b3PrepareWeldJoint( b3JointSim* base, b3StepContext* context );
+void b3PrepareWheelJoint( b3JointSim* base, b3StepContext* context );
+
+void b3WarmStartDistanceJoint( b3JointSim* base, b3StepContext* context );
+void b3WarmStartMotorJoint( b3JointSim* base, b3StepContext* context );
+void b3WarmStartParallelJoint( b3JointSim* base, b3StepContext* context );
+void b3WarmStartPrismaticJoint( b3JointSim* base, b3StepContext* context );
+void b3WarmStartRevoluteJoint( b3JointSim* base, b3StepContext* context );
+void b3WarmStartSphericalJoint( b3JointSim* base, b3StepContext* context );
+void b3WarmStartWeldJoint( b3JointSim* base, b3StepContext* context );
+void b3WarmStartWheelJoint( b3JointSim* base, b3StepContext* context );
+
+void b3SolveDistanceJoint( b3JointSim* base, b3StepContext* context, bool useBias );
+void b3SolveMotorJoint( b3JointSim* base, b3StepContext* context );
+void b3SolveParallelJoint( b3JointSim* base, b3StepContext* context );
+void b3SolvePrismaticJoint( b3JointSim* base, b3StepContext* context, bool useBias );
+void b3SolveRevoluteJoint( b3JointSim* base, b3StepContext* context, bool useBias );
+void b3SolveSphericalJoint( b3JointSim* base, b3StepContext* context, bool useBias );
+void b3SolveWeldJoint( b3JointSim* base, b3StepContext* context, bool useBias );
+void b3SolveWheelJoint( b3JointSim* base, b3StepContext* context, bool useBias );
+
+void b3DrawDistanceJoint( b3DebugDraw* draw, b3JointSim* base, b3WorldTransform transformA, b3WorldTransform transformB );
+void b3DrawParallelJoint( b3DebugDraw* draw, b3JointSim* base, b3WorldTransform transformA, b3WorldTransform transformB, float scale );
+void b3DrawPrismaticJoint( b3DebugDraw* draw, b3JointSim* base, b3WorldTransform transformA, b3WorldTransform transformB, float scale );
+void b3DrawRevoluteJoint( b3DebugDraw* draw, b3JointSim* base, b3WorldTransform transformA, b3WorldTransform transformB, float scale );
+void b3DrawSphericalJoint( b3DebugDraw* draw, b3JointSim* base, b3WorldTransform transformA, b3WorldTransform transformB, float scale );
+void b3DrawWeldJoint( b3DebugDraw* draw, b3JointSim* base, b3WorldTransform transformA, b3WorldTransform transformB, float scale );
+void b3DrawWheelJoint( b3DebugDraw* draw, b3JointSim* base, b3WorldTransform transformA, b3WorldTransform transformB, float scale );

vendor/box3d/src/src/manifold.c

@@ -0,0 +1,246 @@
+// SPDX-FileCopyrightText: 2026 Erin Catto
+// SPDX-License-Identifier: MIT
+
+// Dirk Gregorius contributed portions of this code
+
+#include "manifold.h"
+
+#include "algorithm.h"
+#include "shape.h"
+
+#include "box3d/math_functions.h"
+
+// p1 : origin on edge 1
+// e1 : edge 1
+// c1 : shape 1 centroid
+// p2 : origin on edge 2
+// e2 : edge 2
+// c2 : shape 2 centroid
+float b3EdgeEdgeSeparation( b3Vec3 p1, b3Vec3 e1, b3Vec3 c1, b3Vec3 p2, b3Vec3 e2, b3Vec3 c2 )
+{
+	// Build search direction
+	b3Vec3 u = b3Cross( e1, e2 );
+	float length = b3Length( u );
+
+	// Skip near parallel edges: |e1 x e1| = sin(alpha) * |e1| * |e2|
+	const float kTolerance = 0.005f;
+	if ( length < kTolerance * sqrtf( b3LengthSquared( e1 ) * b3LengthSquared( e2 ) ) )
+	{
+		return -FLT_MAX;
+	}
+
+	if ( length * length < 1000.0f * FLT_MIN )
+	{
+		return -FLT_MAX;
+	}
+
+	b3Vec3 n = b3MulSV( 1.0f / length, u );
+
+	// Make sure normal points away from the first shape
+	// For a triangle, it is possible that N is aligned with the triangle normal and the sign
+	// value can be close to zero and flicker between small negative and positive values, leading to
+	// an incorrect separation value. So we assume the other hull has some volume and pick the most
+	// significant sign value to orient N.
+	float sign1 = b3Dot( n, b3Sub( p1, c1 ) );
+	float sign2 = b3Dot( n, b3Sub( p2, c2 ) );
+	if ( b3AbsFloat( sign1 ) > b3AbsFloat( sign2 ) )
+	{
+		if ( sign1 < 0.0f )
+		{
+			n = b3Neg( n );
+		}
+	}
+	else
+	{
+		if ( sign2 > 0.0f )
+		{
+			n = b3Neg( n );
+		}
+	}
+
+	// s = Dot(n, p2) - d = Dot(n, p2) - Dot(n, p1) = Dot(n, p2 - p1)
+	return b3Dot( n, b3Sub( p2, p1 ) );
+}
+
+// This was extended to make the wedge shape get the correct incident face.
+// Instead of looking directly for the most anti-parallel face, we first find the closest vertex (passed in).
+// Then we look for all edges coming out of that vertex and look for the edge that is
+// most perpendicular to the reference normal.
+// Then from that edge, we select the adjacent face that is most anti-parallel to the reference normal.
+int b3FindIncidentFace( const b3HullData* hull, b3Vec3 refNormal, int vertexIndex )
+{
+	const b3HullVertex* vertices = b3GetHullVertices( hull );
+	const b3HullHalfEdge* edges = b3GetHullEdges( hull );
+	const b3Plane* planes = b3GetHullPlanes( hull );
+	const b3Vec3* points = b3GetHullPoints( hull );
+
+	int minEdgeIndex = -1;
+	float minEdgeProjection = FLT_MAX;
+
+	const b3HullVertex* vertex = vertices + vertexIndex;
+	B3_ASSERT( vertex );
+
+	int edgeIndex = vertex->edge;
+	const b3HullHalfEdge* edge = edges + edgeIndex;
+	b3Vec3 edgeOrigin = points[edge->origin];
+	B3_ASSERT( edge->origin == vertexIndex );
+
+	do
+	{
+		const b3HullHalfEdge* twin = edges + edge->twin;
+		b3Vec3 twinOrigin = points[twin->origin];
+
+		b3Vec3 axis = b3Normalize( b3Sub( twinOrigin, edgeOrigin ) );
+		float edgeProjection = b3AbsFloat( b3Dot( axis, refNormal ) );
+		if ( edgeProjection < minEdgeProjection )
+		{
+			minEdgeIndex = edgeIndex;
+			minEdgeProjection = edgeProjection;
+		}
+
+		edgeIndex = twin->next;
+		edge = edges + edgeIndex;
+		B3_ASSERT( edge->origin == vertexIndex );
+	}
+	while ( edge != edges + vertex->edge );
+	B3_ASSERT( minEdgeIndex >= 0 );
+
+	const b3HullHalfEdge* minEdge = edges + minEdgeIndex;
+	int minFaceIndex1 = minEdge->face;
+	b3Plane minPlane1 = planes[minFaceIndex1];
+
+	const b3HullHalfEdge* minTwin = edges + minEdge->twin;
+	int minFaceIndex2 = minTwin->face;
+	b3Plane minPlane2 = planes[minFaceIndex2];
+
+	return b3Dot( minPlane1.normal, refNormal ) < b3Dot( minPlane2.normal, refNormal ) ? minFaceIndex1 : minFaceIndex2;
+}
+
+b3FeaturePair b3MakeFeaturePair( b3FeatureOwner owner1, int index1, b3FeatureOwner owner2, int index2 )
+{
+	B3_ASSERT( 0 <= index1 && index1 <= UINT8_MAX );
+	B3_ASSERT( 0 <= index2 && index2 <= UINT8_MAX );
+
+	b3FeaturePair pair;
+	pair.index1 = (uint8_t)index1;
+	pair.owner1 = (uint8_t)owner1;
+	pair.index2 = (uint8_t)index2;
+	pair.owner2 = (uint8_t)owner2;
+	return pair;
+}
+
+// This logic seems wrong but it is designed so that choosing
+// face A or B as the reference face does not change the resulting
+// feature pair. This way the contact impulses are persisted even
+// if there is reference face flip-flop. This is verified in the
+// HullAndHull sample using the b3SATCache with a manual feature
+// specified such as b3_manualFaceAxisA.
+b3FeaturePair b3FlipPair( b3FeaturePair pair )
+{
+	B3_ASSERT( pair.owner1 == 0 || pair.owner1 == 1 );
+	B3_ASSERT( pair.owner2 == 0 || pair.owner2 == 1 );
+	B3_SWAP( pair.owner1, pair.owner2 );
+	pair.owner1 = 1 - pair.owner1;
+	pair.owner2 = 1 - pair.owner2;
+	B3_SWAP( pair.index1, pair.index2 );
+	return pair;
+}
+
+#if B3_ENABLE_VALIDATION
+bool b3ValidatePolygon( b3ClipVertex* polygon, int count )
+{
+	// Empty polygons are valid (we can clip away all points when re-constructing manifolds from cache)
+	if ( count == 0 )
+	{
+		return true;
+	}
+
+	// Validate that incoming and outgoing edges match
+	b3ClipVertex vertex1 = polygon[count - 1];
+	for ( int i = 0; i < count; ++i )
+	{
+		b3ClipVertex vertex2 = polygon[i];
+
+		if ( vertex1.pair.owner2 != vertex2.pair.owner1 )
+		{
+			return false;
+		}
+
+		if ( vertex1.pair.index2 != vertex2.pair.index1 )
+		{
+			return false;
+		}
+
+		vertex1 = vertex2;
+	}
+
+	return true;
+}
+#endif
+
+int b3ClipPolygon( b3ClipVertex* out, b3ClipVertex* polygon, int count, b3Plane clipPlane, int edge, b3Plane refPlane )
+{
+	B3_ASSERT( count >= 3 );
+
+	b3ClipVertex vertex1 = polygon[count - 1];
+	float distance1 = b3PlaneSeparation( clipPlane, vertex1.position );
+	int outCount = 0;
+
+	for ( int index = 0; index < count; ++index )
+	{
+		b3ClipVertex vertex2 = polygon[index];
+		float distance2 = b3PlaneSeparation( clipPlane, vertex2.position );
+
+		// Clip edge against plane (Sutherland-Hodgman clipping)
+		if ( distance1 <= 0.0f && distance2 <= 0.0f )
+		{
+			// Both vertices are behind the plane - keep vertex2
+			out[outCount] = vertex2;
+			outCount += 1;
+		}
+		else if ( distance1 <= 0.0f && distance2 > 0.0f )
+		{
+			// Vertex1 is behind of the plane, vertex2 is in front -> intersection point
+			float fraction = distance1 / ( distance1 - distance2 );
+			b3Vec3 position = b3MulAdd( vertex1.position, fraction, b3Sub( vertex2.position, vertex1.position ) );
+
+			// Keep intersection point and adjust outgoing edge
+			b3ClipVertex vertex;
+			vertex.position = position;
+			vertex.separation = b3PlaneSeparation( refPlane, position );
+			vertex.pair = vertex2.pair;
+			vertex.pair.owner2 = b3_featureShapeA;
+			vertex.pair.index2 = (uint8_t)edge;
+			out[outCount] = vertex;
+			outCount += 1;
+		}
+		else if ( distance2 <= 0.0f && distance1 > 0.0f )
+		{
+			// Vertex1 is in front, vertex2 is behind of the plane, -> intersection point
+			float fraction = distance1 / ( distance1 - distance2 );
+			b3Vec3 position = b3MulAdd( vertex1.position, fraction, b3Sub( vertex2.position, vertex1.position ) );
+
+			// Keep intersection point and adjust incoming edge
+			b3ClipVertex vertex;
+			vertex.position = position;
+			vertex.separation = b3PlaneSeparation( refPlane, position );
+			vertex.pair = vertex1.pair;
+			vertex.pair.owner1 = b3_featureShapeA;
+			vertex.pair.index1 = (uint8_t)edge;
+			out[outCount] = vertex;
+			outCount += 1;
+
+			// And also keep vertex2
+			out[outCount] = vertex2;
+			outCount += 1;
+		}
+
+		// Keep vertex2 as starting vertex for next edge
+		vertex1 = vertex2;
+		distance1 = distance2;
+	}
+
+	B3_VALIDATE( b3ValidatePolygon( out, outCount ) );
+
+	return outCount;
+}

vendor/box3d/src/src/manifold.h

@@ -0,0 +1,57 @@
+// SPDX-FileCopyrightText: 2026 Erin Catto
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include "box3d/collision.h"
+#include "box3d/math_functions.h"
+
+#define B3_MAX_CLIP_POINTS 64
+
+typedef struct b3FaceQuery
+{
+	float separation;
+	int faceIndex;
+	int vertexIndex;
+} b3FaceQuery;
+
+typedef struct b3EdgeQuery
+{
+	float separation;
+	int indexA;
+	int indexB;
+} b3EdgeQuery;
+
+typedef struct b3ClipVertex
+{
+	b3Vec3 position;
+	float separation;
+	b3FeaturePair pair;
+} b3ClipVertex;
+
+typedef enum b3FeatureOwner
+{
+	b3_featureShapeA = 0,
+	b3_featureShapeB = 1
+} b3FeatureOwner;
+
+float b3EdgeEdgeSeparation( b3Vec3 p1, b3Vec3 e1, b3Vec3 c1, b3Vec3 p2, b3Vec3 e2, b3Vec3 c2 );
+int b3FindIncidentFace( const b3HullData* hull, b3Vec3 refNormal, int vertexIndex );
+b3FeaturePair b3MakeFeaturePair( b3FeatureOwner owner1, int index1, b3FeatureOwner owner2, int index2 );
+
+b3FeaturePair b3FlipPair( b3FeaturePair pair );
+
+int b3ClipPolygon( b3ClipVertex* out, b3ClipVertex* polygon, int count, b3Plane clipPlane, int edge, b3Plane refPlane );
+
+#if B3_ENABLE_VALIDATION
+bool b3ValidatePolygon( b3ClipVertex* polygon, int count );
+#endif
+
+// For single point contact, such as sphere-sphere, sphere-capsule, sphere-triangle
+static const b3FeaturePair b3FeaturePair_single = { 0 };
+
+static inline uint32_t b3MakeFeatureId( b3FeaturePair pair )
+{
+	return ( (uint32_t)pair.owner1 << 24 ) | ( (uint32_t)pair.index1 << 16 ) | ( (uint32_t)pair.owner2 << 8 ) |
+		   (uint32_t)pair.index2;
+}

vendor/box3d/src/src/math_functions.c

@@ -0,0 +1,616 @@
+// SPDX-FileCopyrightText: 2025 Erin Catto
+// SPDX-License-Identifier: MIT
+
+#include "box3d/math_functions.h"
+
+#include "math_internal.h"
+
+#include "box3d/collision.h"
+#include "box3d/constants.h"
+
+#include <math.h>
+#include <string.h>
+
+bool b3IsValidFloat( float a )
+{
+	if ( isnan( a ) )
+	{
+		return false;
+	}
+
+	if ( isinf( a ) )
+	{
+		return false;
+	}
+
+	return true;
+}
+
+bool b3IsValidVec3( b3Vec3 a )
+{
+	if ( isnan( a.x ) || isnan( a.y ) || isnan( a.z ) )
+	{
+		return false;
+	}
+
+	if ( isinf( a.x ) || isinf( a.y ) || isinf( a.z ) )
+	{
+		return false;
+	}
+
+	return true;
+}
+
+bool b3IsValidQuat( b3Quat a )
+{
+	if ( isnan( a.v.x ) || isnan( a.v.y ) || isnan( a.v.z ) || isnan( a.s ) )
+	{
+		return false;
+	}
+
+	if ( isinf( a.v.x ) || isinf( a.v.y ) || isinf( a.v.z ) || isinf( a.s ) )
+	{
+		return false;
+	}
+
+	return b3IsNormalizedQuat( a );
+}
+
+bool b3IsValidTransform( b3Transform a )
+{
+	return b3IsValidVec3( a.p ) && b3IsValidQuat( a.q );
+}
+
+bool b3IsValidMatrix3( b3Matrix3 a )
+{
+	return b3IsValidVec3( a.cx ) && b3IsValidVec3( a.cy ) && b3IsValidVec3( a.cz );
+}
+
+bool b3IsValidAABB( b3AABB a )
+{
+	if ( b3IsValidVec3( a.lowerBound ) == false )
+	{
+		return false;
+	}
+
+	if ( b3IsValidVec3( a.upperBound ) == false )
+	{
+		return false;
+	}
+
+	if ( a.lowerBound.x > a.upperBound.x )
+	{
+		return false;
+	}
+
+	if ( a.lowerBound.y > a.upperBound.y )
+	{
+		return false;
+	}
+
+	if ( a.lowerBound.z > a.upperBound.z )
+	{
+		return false;
+	}
+
+	return true;
+}
+
+bool b3IsBoundedAABB( b3AABB a )
+{
+	if ( a.lowerBound.x < -B3_HUGE || a.lowerBound.y < -B3_HUGE || a.lowerBound.z < -B3_HUGE )
+	{
+		return false;
+	}
+
+	if ( a.upperBound.x > B3_HUGE || a.upperBound.y > B3_HUGE || a.upperBound.z > B3_HUGE )
+	{
+		return false;
+	}
+
+	return true;
+}
+
+bool b3IsSaneAABB( b3AABB a )
+{
+	if ( b3IsValidAABB( a ) == false )
+	{
+		return false;
+	}
+
+	if ( a.lowerBound.x < -B3_HUGE || a.lowerBound.y < -B3_HUGE || a.lowerBound.z < -B3_HUGE )
+	{
+		return false;
+	}
+
+	if ( a.upperBound.x > B3_HUGE || a.upperBound.y > B3_HUGE || a.upperBound.z > B3_HUGE )
+	{
+		return false;
+	}
+
+	return true;
+}
+
+bool b3IsValidPlane( b3Plane a )
+{
+	if ( b3IsValidVec3( a.normal ) == false )
+	{
+		return false;
+	}
+
+	if ( b3IsNormalized( a.normal ) == false )
+	{
+		return false;
+	}
+
+	return b3IsValidFloat( a.offset );
+}
+
+bool b3IsValidPosition( b3Pos p )
+{
+	if ( isnan( p.x ) || isnan( p.y ) || isnan( p.z ) )
+	{
+		return false;
+	}
+
+	if ( isinf( p.x ) || isinf( p.y ) || isinf( p.z ) )
+	{
+		return false;
+	}
+
+	return true;
+}
+
+bool b3IsValidWorldTransform( b3WorldTransform t )
+{
+	return b3IsValidPosition( t.p ) && b3IsValidQuat( t.q );
+}
+
+// https://stackoverflow.com/questions/46210708/atan2-approximation-with-11bits-in-mantissa-on-x86with-sse2-and-armwith-vfpv4
+float b3Atan2( 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 = b3AbsFloat( x );
+	float ay = b3AbsFloat( y );
+	float mx = b3MaxFloat( ay, ax );
+	float mn = b3MinFloat( 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
+b3CosSin b3ComputeCosSin( float radians )
+{
+#if 0
+	return {
+		cosf( radians ),
+		sinf( radians ),
+	};
+#else
+	float x = b3UnwindAngle( radians );
+	float pi2 = B3_PI * B3_PI;
+
+	// cosine needs angle in [-pi/2, pi/2]
+	float c;
+	if ( x < -0.5f * B3_PI )
+	{
+		float y = x + B3_PI;
+		float y2 = y * y;
+		c = -( pi2 - 4.0f * y2 ) / ( pi2 + y2 );
+	}
+	else if ( x > 0.5f * B3_PI )
+	{
+		float y = x - B3_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 + B3_PI;
+		s = -16.0f * y * ( B3_PI - y ) / ( 5.0f * pi2 - 4.0f * y * ( B3_PI - y ) );
+	}
+	else
+	{
+		s = 16.0f * x * ( B3_PI - x ) / ( 5.0f * pi2 - 4.0f * x * ( B3_PI - x ) );
+	}
+
+	float mag = sqrtf( s * s + c * c );
+	float invMag = mag > 0.0f ? 1.0f / mag : 0.0f;
+	b3CosSin cs = { c * invMag, s * invMag };
+	return cs;
+#endif
+}
+
+b3Quat b3MakeQuatFromMatrix( const b3Matrix3* m )
+{
+	b3Vec3 c1 = m->cx;
+	b3Vec3 c2 = m->cy;
+	b3Vec3 c3 = m->cz;
+
+	b3Quat q;
+
+	float trace = m->cx.x + m->cy.y + m->cz.z;
+	if ( trace >= 0.0f )
+	{
+		q.v.x = c2.z - c3.y;
+		q.v.y = c3.x - c1.z;
+		q.v.z = c1.y - c2.x;
+		q.s = trace + 1.0f;
+	}
+	else
+	{
+		if ( c1.x > c2.y && c1.x > c3.z )
+		{
+			q.v.x = c1.x - c2.y - c3.z + 1.0f;
+			q.v.y = c2.x + c1.y;
+			q.v.z = c3.x + c1.z;
+			q.s = c2.z - c3.y;
+		}
+		else if ( c2.y > c3.z )
+		{
+			q.v.x = c1.y + c2.x;
+			q.v.y = c2.y - c3.z - c1.x + 1.0f;
+			q.v.z = c3.y + c2.z;
+			q.s = c3.x - c1.z;
+		}
+		else
+		{
+			q.v.x = c1.z + c3.x;
+			q.v.y = c2.z + c3.y;
+			q.v.z = c3.z - c1.x - c2.y + 1.0f;
+			q.s = c1.y - c2.x;
+		}
+	}
+
+	// The algorithm is simplified and made more accurate by normalizing at the end
+	return b3NormalizeQuat( q );
+}
+
+b3Quat b3ComputeQuatBetweenUnitVectors( b3Vec3 v1, b3Vec3 v2 )
+{
+	B3_ASSERT( b3IsNormalized( v1 ) );
+	B3_ASSERT( b3IsNormalized( v2 ) );
+
+	b3Quat out;
+
+	b3Vec3 m = b3Lerp( v1, v2, 0.5f );
+	float tolerance = 100.0f * FLT_EPSILON;
+	if ( b3LengthSquared( m ) > tolerance * tolerance )
+	{
+		out.v = b3Cross( v1, m );
+		out.s = b3Dot( v1, m );
+	}
+	else
+	{
+		// Anti-parallel: Use a perpendicular vector
+		if ( b3AbsFloat( v1.x ) > 0.5f )
+		{
+			out.v.x = v1.y;
+			out.v.y = -v1.x;
+			out.v.z = 0.0f;
+		}
+		else
+		{
+			out.v.x = 0.0f;
+			out.v.y = v1.z;
+			out.v.z = -v1.y;
+		}
+
+		out.s = 0.0f;
+	}
+
+	// The algorithm is simplified and made more accurate by normalizing at the end
+	return b3NormalizeQuat( out );
+}
+
+b3SegmentDistanceResult b3LineDistance( b3Vec3 p1, b3Vec3 d1, b3Vec3 p2, b3Vec3 d2 )
+{
+	b3SegmentDistanceResult result;
+
+	// Solve A*x = b
+	float a11 = b3Dot( d1, d1 );
+	float a12 = -b3Dot( d1, d2 );
+	float a21 = b3Dot( d2, d1 );
+	float a22 = -b3Dot( d2, d2 );
+
+	b3Vec3 w = b3Sub( p1, p2 );
+	float b1 = -b3Dot( d1, w );
+	float b2 = -b3Dot( d2, w );
+
+	float det = a11 * a22 - a12 * a21;
+	if ( det * det < 1000.0f * FLT_MIN )
+	{
+		// Lines are parallel - project p2 onto line L1: x1 = p1 + s1 * d1
+		float s1 = b3Dot( b3Sub( p2, p1 ), d1 ) / b3Dot( d1, d1 );
+		float s2 = 0.0f;
+
+		result.point1 = b3MulAdd( p1, s1, d1 );
+		result.fraction1 = s1;
+		result.point2 = b3MulAdd( p2, s2, d2 );
+		result.fraction2 = s2;
+
+		return result;
+	}
+
+	float s1 = ( a22 * b1 - a12 * b2 ) / det;
+	float s2 = ( a11 * b2 - a21 * b1 ) / det;
+
+	result.point1 = b3MulAdd( p1, s1, d1 );
+	result.fraction1 = s1;
+	result.point2 = b3MulAdd( p2, s2, d2 );
+	result.fraction2 = s2;
+	return result;
+}
+
+b3SegmentDistanceResult b3SegmentDistance( b3Vec3 p1, b3Vec3 q1, b3Vec3 p2, b3Vec3 q2 )
+{
+	b3SegmentDistanceResult result;
+
+	b3Vec3 d1 = b3Sub( q1, p1 );
+	b3Vec3 d2 = b3Sub( q2, p2 );
+	b3Vec3 r = b3Sub( p1, p2 );
+
+	float a = b3Dot( d1, d1 );
+	float b = b3Dot( d1, d2 );
+	float c = b3Dot( d1, r );
+	float e = b3Dot( d2, d2 );
+	float f = b3Dot( d2, r );
+
+	// Check if one of the segments degenerates into a point
+	if ( a < 100.0f * FLT_EPSILON && e < 100.0f * FLT_EPSILON )
+	{
+		// Both segments degenerate into points
+		result.point1 = p1;
+		result.fraction1 = 0.0f;
+		result.point2 = p2;
+		result.fraction2 = 0.0f;
+
+		return result;
+	}
+
+	if ( a < 100.0f * FLT_EPSILON )
+	{
+		// First segment degenerates into a point
+		float s2 = b3ClampFloat( f / e, 0.0f, 1.0f );
+
+		result.point1 = p1;
+		result.fraction1 = 0.0f;
+		result.point2 = b3MulAdd( p2, s2, d2 );
+		result.fraction2 = s2;
+
+		return result;
+	}
+
+	if ( e < 100.0f * FLT_EPSILON )
+	{
+		// Second segment degenerates into a point
+		float s1 = b3ClampFloat( -c / a, 0.0f, 1.0f );
+
+		result.point1 = b3MulAdd( p1, s1, d1 );
+		result.fraction1 = s1;
+		result.point2 = p2;
+		result.fraction2 = 0.0f;
+
+		return result;
+	}
+
+	// Non-degenerate case
+	float denom = a * e - b * b;
+	float s1 = denom > 1000.0f * FLT_MIN ? b3ClampFloat( ( b * f - c * e ) / denom, 0.0f, 1.0f ) : 0.0f;
+	float s2 = ( b * s1 + f ) / e;
+
+	// Clamp lambda2 and recompute lambda1 if necessary
+	if ( s2 < 0.0f )
+	{
+		s1 = b3ClampFloat( -c / a, 0.0f, 1.0f );
+		s2 = 0.0f;
+	}
+	else if ( s2 > 1.0f )
+	{
+		s1 = b3ClampFloat( ( b - c ) / a, 0.0f, 1.0f );
+		s2 = 1.0f;
+	}
+
+	result.point1 = b3MulAdd( p1, s1, d1 );
+	result.fraction1 = s1;
+	result.point2 = b3MulAdd( p2, s2, d2 );
+	result.fraction2 = s2;
+
+	return result;
+}
+
+b3Vec3 b3PointToSegmentDistance( b3Vec3 a, b3Vec3 b, b3Vec3 q )
+{
+	b3Vec3 ab = b3Sub( b, a );
+	b3Vec3 aq = b3Sub( q, a );
+
+	float alpha = b3Dot( ab, aq );
+
+	if ( alpha <= 0.0f )
+	{
+		// q projects outside interval [a, b] on the side of a
+		return a;
+	}
+	else
+	{
+		float denominator = b3Dot( ab, ab );
+		if ( alpha > denominator )
+		{
+			// q projects outside interval [a, b] on the side of b
+			return b;
+		}
+		else
+		{
+			// q projects inside interval [a, b]
+			alpha /= denominator;
+			return b3MulAdd( a, alpha, ab );
+		}
+	}
+}
+
+b3TrianglePoint b3ClosestPointOnTriangle( b3Vec3 a, b3Vec3 b, b3Vec3 c, b3Vec3 q )
+{
+	// Check if P lies in vertex region of A
+	b3Vec3 ab = b3Sub( b, a );
+	b3Vec3 ac = b3Sub( c, a );
+	b3Vec3 aq = b3Sub( q, a );
+
+	float d1 = b3Dot( ab, aq );
+	float d2 = b3Dot( ac, aq );
+	if ( d1 <= 0.0f && d2 <= 0.0f )
+	{
+		return (b3TrianglePoint){ a, b3_featureVertex1 };
+	}
+
+	// Check if P lies in vertex region of B
+	b3Vec3 bq = b3Sub( q, b );
+
+	float d3 = b3Dot( ab, bq );
+	float d4 = b3Dot( ac, bq );
+	if ( d3 > 0.0f && d4 <= d3 )
+	{
+		return (b3TrianglePoint){ b, b3_featureVertex2 };
+	}
+
+	// Check if P lies in edge region AB
+	float vc = d1 * d4 - d3 * d2;
+	if ( vc <= 0.0f && d1 >= 0.0f && d3 <= 0.0f )
+	{
+		float t = d1 / ( d1 - d3 );
+		return (b3TrianglePoint){ b3MulAdd( a, t, ab ), b3_featureEdge1 };
+	}
+
+	// Check if P lies in vertex region of C
+	b3Vec3 cq = b3Sub( q, c );
+
+	float d5 = b3Dot( ab, cq );
+	float d6 = b3Dot( ac, cq );
+	if ( d6 >= 0.0f && d5 <= d6 )
+	{
+		return (b3TrianglePoint){ c, b3_featureVertex3 };
+	}
+
+	// Check if P lies in edge region AC
+	float vb = d5 * d2 - d1 * d6;
+	if ( vb <= 0.0f && d2 >= 0.0f && d6 <= 0.0f )
+	{
+		float t = d2 / ( d2 - d6 );
+		return (b3TrianglePoint){ b3MulAdd( a, t, ac ), b3_featureEdge3 };
+	}
+
+	// Check if P lies in edge region of BC
+	float va = d3 * d6 - d5 * d4;
+	if ( va <= 0.0f && d4 >= d3 && d5 >= d6 )
+	{
+		b3Vec3 bc = b3Sub( c, b );
+
+		float t = ( d4 - d3 ) / ( ( d4 - d3 ) + ( d5 - d6 ) );
+		return (b3TrianglePoint){ b3MulAdd( b, t, bc ), b3_featureEdge2 };
+	}
+
+	// P inside face region ABC
+	float t1 = vb / ( va + vb + vc );
+	float t2 = vc / ( va + vb + vc );
+
+	b3Vec3 p = b3MulAdd( a, t1, ab );
+	p = b3MulAdd( p, t2, ac );
+	return (b3TrianglePoint){ p, b3_featureTriangleFace };
+}
+
+b3Matrix3 b3SphereInertia( float mass, float radius )
+{
+	float i = 0.4f * mass * radius * radius;
+	return b3MakeDiagonalMatrix( i, i, i );
+}
+
+b3Matrix3 b3CylinderInertia( float mass, float radius, float height )
+{
+	float ixx = mass * ( 3 * radius * radius + height * height ) / 12.0f;
+	float iyy = 0.5f * mass * radius * radius;
+	return b3MakeDiagonalMatrix( ixx, iyy, ixx );
+}
+
+b3Matrix3 b3BoxInertia( float mass, b3Vec3 min, b3Vec3 max )
+{
+	b3Vec3 delta = b3Sub( max, min );
+	float ixx = mass * ( delta.y * delta.y + delta.z * delta.z ) / 12.0f;
+	float iyy = mass * ( delta.x * delta.x + delta.z * delta.z ) / 12.0f;
+	float izz = mass * ( delta.x * delta.x + delta.y * delta.y ) / 12.0f;
+
+	return b3MakeDiagonalMatrix( ixx, iyy, izz );
+}
+
+// https://en.wikipedia.org/wiki/Parallel_axis_theorem
+b3Matrix3 b3Steiner( float mass, b3Vec3 origin )
+{
+	// Usage: Io = Ic + Is and Ic = Io - Is
+	float ixx = mass * ( origin.y * origin.y + origin.z * origin.z );
+	float iyy = mass * ( origin.x * origin.x + origin.z * origin.z );
+	float izz = mass * ( origin.x * origin.x + origin.y * origin.y );
+	float ixy = -mass * origin.x * origin.y;
+	float ixz = -mass * origin.x * origin.z;
+	float iyz = -mass * origin.y * origin.z;
+
+	// Write
+	b3Matrix3 out;
+	out.cx.x = ixx;
+	out.cy.x = ixy;
+	out.cz.x = ixz;
+	out.cx.y = ixy;
+	out.cy.y = iyy;
+	out.cz.y = iyz;
+	out.cx.z = ixz;
+	out.cy.z = iyz;
+	out.cz.z = izz;
+
+	return out;
+}
+
+bool b3IsValidRay( const b3RayCastInput* input )
+{
+	bool isValid = b3IsValidVec3( input->origin ) && b3IsValidVec3( input->translation ) &&
+				   b3IsValidFloat( input->maxFraction ) && 0.0f <= input->maxFraction && input->maxFraction < B3_HUGE;
+	return isValid;
+}

vendor/box3d/src/src/math_internal.h

@@ -0,0 +1,480 @@
+// SPDX-FileCopyrightText: 2025 Erin Catto
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include "core.h"
+
+#include "box3d/collision.h"
+#include "box3d/math_functions.h"
+
+struct b3Sweep;
+struct b3Plane;
+
+#define B3_TWO_PI 6.283185307f
+#define B3_PI_OVER_TWO 1.570796327f
+#define B3_PI_OVER_FOUR 0.785398163f
+#define B3_SQRT3 1.732050808f
+
+// todo eliminate this
+static const b3AABB B3_BOUNDS3_EMPTY = { { FLT_MAX, FLT_MAX, FLT_MAX }, { -FLT_MAX, -FLT_MAX, -FLT_MAX } };
+
+typedef struct b3Matrix2
+{
+	b3Vec2 cx, cy;
+} b3Matrix2;
+
+typedef struct b3Triangle
+{
+	b3Vec3 vertices[3];
+	int i1, i2, i3;
+	int flags;
+} b3Triangle;
+
+typedef struct b3TrianglePoint
+{
+	b3Vec3 point;
+	b3TriangleFeature feature;
+} b3TrianglePoint;
+
+typedef struct b3ShapeExtent
+{
+	float minExtent;
+	b3Vec3 maxExtent;
+} b3ShapeExtent;
+
+b3TrianglePoint b3ClosestPointOnTriangle( b3Vec3 a, b3Vec3 b, b3Vec3 c, b3Vec3 q );
+
+float b3IntersectSegmentTriangle( b3Vec3 p, b3Vec3 q, b3Vec3 a, b3Vec3 b, b3Vec3 c );
+float b3IntersectSegmentSphere( b3Vec3 p, b3Vec3 q, b3Vec3 c, float r );
+
+b3MassData b3ComputeMassProperties( int triangleCount, const int* triangles, int vertexCount, const b3Vec3* vertices,
+									float density );
+
+bool b3IsValidMassData( const b3MassData* massData );
+
+b3Matrix3 b3SphereInertia( float mass, float radius );
+b3Matrix3 b3CylinderInertia( float mass, float radius, float height );
+b3Matrix3 b3BoxInertia( float mass, b3Vec3 min, b3Vec3 max );
+
+// Inertia helper (Io = Ic + Is and Ic = Io - Is)
+int b3GetProxySupport( const b3ShapeProxy* proxy, b3Vec3 axis );
+int b3GetPointSupport( const b3Vec3* points, int count, b3Vec3 axis );
+
+static inline size_t b3AlignUp8( size_t x )
+{
+	return ( x + 7u ) & ~(size_t)7u;
+}
+
+// https://en.wikipedia.org/wiki/Floor_and_ceiling_functions
+static inline int b3CeilingInt( int numerator, int denominator )
+{
+	B3_VALIDATE( denominator > 0 );
+	return ( numerator + denominator - 1 ) / denominator;
+}
+
+// Assumes denominator == 2^exponent
+static inline int b3CeilingPow2( int numerator, int denominator, int exponent )
+{
+	B3_VALIDATE( exponent > 0 && ( denominator == 1 << exponent ) );
+	return ( numerator + denominator - 1 ) >> exponent;
+}
+
+bool b3IsSweepNormalized( b3Sweep* sweep );
+
+static inline float b3Dot2( b3Vec2 v1, b3Vec2 v2 )
+{
+	return v1.x * v2.x + v1.y * v2.y;
+}
+
+static inline float b3Length2( b3Vec2 v )
+{
+	return sqrtf( b3Dot2( v, v ) );
+}
+
+static inline float b3LengthSquared2( b3Vec2 v )
+{
+	return b3Dot2( v, v );
+}
+
+static inline b3Vec2 b3MinVec2( b3Vec2 v1, b3Vec2 v2 )
+{
+	b3Vec2 v;
+	v.x = b3MinFloat( v1.x, v2.x );
+	v.y = b3MinFloat( v1.y, v2.y );
+	return v;
+}
+
+static inline b3Vec2 b3MaxVec2( b3Vec2 v1, b3Vec2 v2 )
+{
+	b3Vec2 v;
+	v.x = b3MaxFloat( v1.x, v2.x );
+	v.y = b3MaxFloat( v1.y, v2.y );
+	return v;
+}
+
+static inline void b3Store( float* dst, b3Vec3 src )
+{
+	dst[0] = src.x;
+	dst[1] = src.y;
+	dst[2] = src.z;
+}
+
+static inline b3Vec3 b3ClampLength( b3Vec3 v, float maxLength )
+{
+	float lengthSq = b3LengthSquared( v );
+	if ( lengthSq <= maxLength * maxLength )
+	{
+		return v;
+	}
+
+	float length = sqrtf( lengthSq );
+	return b3MulSV( maxLength / length, v );
+}
+
+// Assume v is a unit vector
+static inline b3Vec3 b3ArbitraryPerp( b3Vec3 v )
+{
+	// Suppose vector a has all equal components and is a unit vector: a = (s, s, s)
+	// Then 3*s*s = 1, s = sqrt(1/3) = 0.57735. This means that at least one component
+	// of a unit vector must be greater or equal to 0.57735.
+	b3Vec3 p;
+	if ( v.x < -0.5f || 0.5f < v.x )
+	{
+		// x is non-zero and it should not go into the x component
+		// dot([ay + bz, cx, dx], [x, y, z]) = ayx + bzx + cxy + dzx
+		// for the dot product to be zero need: c = -a, d = -b
+		float a = 0.67f;
+		float b = -0.42f;
+		p = B3_LITERAL( b3Vec3 ){ a * v.y + b * v.z, -a * v.x, -b * v.x };
+	}
+	else if ( v.y < -0.5f || 0.5f < v.y )
+	{
+		// y is non-zero and it should not go into the y component
+		// p = [ay, bx + cz, dy]
+		// axy + bxy + cyz + dyz = 0
+		// b = -a, d = -c
+		float a = 0.67f;
+		float c = -0.42f;
+		p = B3_LITERAL( b3Vec3 ){ a * v.y, -a * v.x + c * v.z, -c * v.y };
+	}
+	else
+	{
+		// This would trip if the input is not a unit vector
+		B3_VALIDATE( v.z < -0.5f || 0.5f < v.z );
+
+		// z is non-zero and it should not go into the z component
+		// p = [az, bz, cx + dy]
+		// axz + byz + cxz + dyz = 0
+		// c = -a, d = -b
+		float a = 0.67f;
+		float b = -0.42f;
+		p = B3_LITERAL( b3Vec3 ){ a * v.z, b * v.z, -a * v.x - b * v.y };
+	}
+
+	B3_VALIDATE( b3LengthSquared( p ) > 0.1f );
+	B3_VALIDATE( b3AbsFloat( b3Dot( p, v ) ) < 100.0f * FLT_EPSILON );
+
+	return b3Normalize( p );
+}
+
+static inline b3Quat b3QuatFromExponentialMap( b3Vec3 v )
+{
+	// Exponential map (Grassia)
+	float threshold = 0.018581361f;
+
+	float angle = b3Length( v );
+	if ( angle < threshold )
+	{
+		// Taylor expansion
+		b3Quat out;
+		out.v = b3MulSV( 0.5f + angle * angle / 48.0f, v );
+		out.s = b3Cos( 0.5f * angle );
+
+		return out;
+	}
+
+	return b3MakeQuatFromAxisAngle( b3MulSV( 1.0f / angle, v ), angle );
+}
+
+/// Integrate rotation from angular velocity
+/// @param q1 initial rotation
+/// @param deltaRotation the angular displacement vector in radians (angular velocity multiplied by the time step)
+/// q2 = q1 + 0.5 * omega * q1
+static inline b3Quat b3IntegrateRotation( b3Quat q1, b3Vec3 deltaRotation )
+{
+#if 1
+	// https://fgiesen.wordpress.com/2012/08/24/quaternion-differentiation/
+	b3Quat qd = { b3MulSV( 0.5f, deltaRotation ), 0.0f };
+	qd = b3MulQuat( qd, q1 );
+	b3Quat q2 = { b3Add( q1.v, qd.v ), qd.s + q1.s };
+	q2 = b3NormalizeQuat( q2 );
+	return q2;
+#else
+	return b3NormalizeQuat( b3MulQuat(b3QuatFromExponentialMap( deltaRotation ), q1) );
+#endif
+}
+
+// Pseudo angular velocity from a quaternion target
+// w = 2 * (target - q) * conj(q)
+static inline b3Vec3 b3DeltaQuatToRotation( b3Quat q, b3Quat target )
+{
+	b3Quat s = q;
+	if ( b3DotQuat( q, target ) < 0.0f )
+	{
+		// Correct polarity
+		s = b3NegateQuat( q );
+	}
+
+	b3Quat diff = { b3Sub( target.v, s.v ), target.s - s.s };
+	b3Quat product = b3MulQuat( diff, b3Conjugate( s ) );
+	return b3MulSV( 2.0f, product.v );
+}
+
+static inline float b3ScalarTripleProduct( b3Vec3 a, b3Vec3 b, b3Vec3 c )
+{
+	b3Vec3 d;
+	d.x = b.y * c.z - b.z * c.y;
+	d.y = b.z * c.x - b.x * c.z;
+	d.z = b.x * c.y - b.y * c.x;
+	return a.x * d.x + a.y * d.y + a.z * d.z;
+}
+
+// Get a value by index. Avoid undefined behavior of code like (&v.x)[2].
+static inline float b3GetByIndex( b3Vec3 v, int index )
+{
+	B3_VALIDATE( 0 <= index && index < 3 );
+	float temp[3] = { v.x, v.y, v.z };
+	return temp[index];
+}
+
+static inline int b3MajorAxis( b3Vec3 v )
+{
+	return v.x < v.y ? ( v.y < v.z ? 2 : 1 ) : ( v.x < v.z ? 2 : 0 );
+}
+
+static inline float b3MinElement( b3Vec3 v )
+{
+	return b3MinFloat( v.x, b3MinFloat( v.y, v.z ) );
+}
+
+static inline float b3MaxElement( b3Vec3 v )
+{
+	return b3MaxFloat( v.x, b3MaxFloat( v.y, v.z ) );
+}
+
+static inline int b3MaxElementIndex( b3Vec3 v )
+{
+	return v.x < v.y ? ( v.y < v.z ? 2 : 1 ) : ( v.x < v.z ? 2 : 0 );
+}
+
+static inline b3Vec2 b3Add2( b3Vec2 a, b3Vec2 b )
+{
+	b3Vec2 c = { a.x + b.x, a.y + b.y };
+	return c;
+}
+
+static inline b3Vec2 b3Sub2( b3Vec2 a, b3Vec2 b )
+{
+	b3Vec2 c = { a.x - b.x, a.y - b.y };
+	return c;
+}
+
+static inline b3Vec2 b3Neg2( b3Vec2 v )
+{
+	b3Vec2 c = { -v.x, -v.y };
+	return c;
+}
+
+static inline b3Vec2 b3MulSV2( float s, b3Vec2 v )
+{
+	b3Vec2 c = { s * v.x, s * v.y };
+	return c;
+}
+
+// a + s * b
+static inline b3Vec2 b3MulAdd2( b3Vec2 a, float s, b3Vec2 b )
+{
+	b3Vec2 c = { a.x + s * b.x, a.y + s * b.y };
+	return c;
+}
+
+// a - s * b
+static inline b3Vec2 b3MulSub2( b3Vec2 a, float s, b3Vec2 b )
+{
+	b3Vec2 c = { a.x - s * b.x, a.y - s * b.y };
+	return c;
+}
+
+static inline float b3Cross2( b3Vec2 a, b3Vec2 b )
+{
+	return a.x * b.y - a.y * b.x;
+}
+
+static inline float b3DistanceSquared2( b3Vec2 a, b3Vec2 b )
+{
+	float dx = b.x - a.x;
+	float dy = b.y - a.y;
+	return dx * dx + dy * dy;
+}
+
+static inline b3Vec2 b3MulMV2( b3Matrix2 m, b3Vec2 a )
+{
+	b3Vec2 b = { m.cx.x * a.x + m.cy.x * a.y, m.cx.y * a.x + m.cy.y * a.y };
+	return b;
+}
+
+static inline b3Matrix2 b3MulMM2( b3Matrix2 m1, b3Matrix2 m2 )
+{
+	b3Matrix2 out;
+	out.cx = b3MulMV2( m1, m2.cx );
+	out.cy = b3MulMV2( m1, m2.cy );
+	return out;
+}
+
+static inline float b3Det2( b3Matrix2 m )
+{
+	return m.cx.x * m.cy.y - m.cx.y * m.cy.x;
+}
+
+static inline b3Matrix2 b3Invert2( b3Matrix2 m )
+{
+	float det = b3Det2( m );
+	if ( b3AbsFloat( det ) > 1000.0f * FLT_MIN )
+	{
+		float invDet = 1.0f / det;
+		return B3_LITERAL( b3Matrix2 ){
+			{ invDet * m.cy.y, -invDet * m.cx.y },
+			{ -invDet * m.cy.x, invDet * m.cx.x },
+		};
+	}
+
+	return B3_LITERAL( b3Matrix2 ){ { 0.0f, 0.0f }, { 0.0f, 0.0f } };
+}
+
+// Assumes positive semi-definite
+static inline b3Vec2 b3Solve2( b3Matrix2 m, b3Vec2 b )
+{
+	float det = b3Det2( m );
+	if ( det > 1000.0f * FLT_MIN )
+	{
+		float invDet = 1.0f / det;
+		return B3_LITERAL( b3Vec2 ){
+			invDet * m.cy.y * b.x - invDet * m.cy.x * b.y,
+			-invDet * m.cx.y * b.x + invDet * m.cx.x * b.y,
+		};
+	}
+
+	return B3_LITERAL( b3Vec2 ){ 0.0f, 0.0f };
+}
+
+// Convenience function: s * a + t * b + u * c
+static inline b3Vec3 b3Blend3( float s, b3Vec3 a, float t, b3Vec3 b, float u, b3Vec3 c )
+{
+	b3Vec3 d = {
+		s * a.x + t * b.x + u * c.x,
+		s * a.y + t * b.y + u * c.y,
+		s * a.z + t * b.z + u * c.z,
+	};
+	return d;
+}
+
+static inline b3Vec3 b3ModifiedCross( b3Vec3 a, b3Vec3 b )
+{
+	b3Vec3 c;
+	c.x = a.y * b.z + a.z * b.y;
+	c.y = a.z * b.x + a.x * b.z;
+	c.z = a.x * b.y + a.y * b.x;
+	return c;
+}
+
+static inline b3Matrix3 b3MakeDiagonalMatrix( float a, float b, float c )
+{
+	return (b3Matrix3){ { a, 0.0f, 0.0f }, { 0.0f, b, 0.0f }, { 0.0f, 0.0f, c } };
+}
+
+static inline b3Matrix3 b3Skew( b3Vec3 v )
+{
+	b3Matrix3 out;
+	out.cx = (b3Vec3){ 0, v.z, -v.y };
+	out.cy = (b3Vec3){ -v.z, 0, v.x };
+	out.cz = (b3Vec3){ v.y, -v.x, 0 };
+
+	return out;
+}
+
+static inline b3Plane b3NormalizePlane( b3Plane plane )
+{
+	float invLength = 1.0f / b3Length( plane.normal );
+	return (b3Plane){ b3MulSV( invLength, plane.normal ), invLength * plane.offset };
+}
+
+static inline b3Plane b3MakePlaneFromNormalAndPoint( b3Vec3 normal, b3Vec3 point )
+{
+	return (b3Plane){ normal, b3Dot( normal, point ) };
+}
+
+static inline b3Plane b3MakePlaneFromPoints( b3Vec3 point1, b3Vec3 point2, b3Vec3 point3 )
+{
+	b3Plane plane;
+	plane.normal = b3Cross( b3Sub( point2, point1 ), b3Sub( point3, point1 ) );
+	plane.normal = b3Normalize( plane.normal );
+	plane.offset = b3Dot( plane.normal, point1 );
+	return plane;
+}
+
+static inline b3Vec3 b3MakeNormalFromPoints( b3Vec3 point1, b3Vec3 point2, b3Vec3 point3 )
+{
+	b3Vec3 normal = b3Cross( b3Sub( point2, point1 ), b3Sub( point3, point1 ) );
+	return b3Normalize( normal );
+}
+
+// normal2 = q * normal1
+// offset2 = dot(normal2, p) + offset1
+static inline b3Plane b3TransformPlane( b3Transform transform, b3Plane plane )
+{
+	b3Vec3 normal = b3RotateVector( transform.q, plane.normal );
+	return B3_LITERAL( b3Plane ){ normal, plane.offset + b3Dot( normal, transform.p ) };
+}
+
+/// Signed separation of a point from a plane
+static inline float b3PlaneSeparation( b3Plane plane, b3Vec3 point )
+{
+	return b3Dot( plane.normal, point ) - plane.offset;
+}
+
+// Negative if p is below the triangle v1-v2-v3
+static inline float b3SignedVolume( b3Vec3 v1, b3Vec3 v2, b3Vec3 v3, b3Vec3 p )
+{
+	b3Vec3 e1 = b3Sub( v2, v1 );
+	b3Vec3 e2 = b3Sub( v3, v1 );
+	b3Vec3 n = b3Cross( e1, e2 );
+	return b3Dot( n, b3Sub( p, v1 ) );
+}
+
+// todo eliminate this
+static inline bool b3IsWithinSegments( const b3SegmentDistanceResult* result )
+{
+	return ( 0.0f <= result->fraction1 && result->fraction1 <= 1.0f ) &&
+		   ( 0.0f <= result->fraction2 && result->fraction2 <= 1.0f );
+}
+
+static inline b3Matrix3 b3RotateInertia( b3Quat q, b3Matrix3 centralInertia )
+{
+	b3Matrix3 rotationMatrix = b3MakeMatrixFromQuat( q );
+	b3Matrix3 inertia = b3MulMM( rotationMatrix, b3MulMM( centralInertia, b3Transpose( rotationMatrix ) ) );
+	return inertia;
+}
+
+static inline b3Matrix3 b3TransformInertia( b3Transform transform, b3Matrix3 centralInertia, float mass )
+{
+	b3Matrix3 inertia = b3RotateInertia( transform.q, centralInertia );
+	inertia = b3AddMM( inertia, b3Steiner( mass, transform.p ) );
+	return inertia;
+}
+
+// Add a point to an AABB.
+static inline b3AABB b3AABB_AddPoint( b3AABB a, b3Vec3 point )
+{
+	return (b3AABB){ b3Min( a.lowerBound, point ), b3Max( a.upperBound, point ) };
+}

vendor/box3d/src/src/motor_joint.c

@@ -0,0 +1,439 @@
+// SPDX-FileCopyrightText: 2025 Erin Catto
+// SPDX-License-Identifier: MIT
+
+#include "body.h"
+#include "core.h"
+#include "joint.h"
+#include "math_internal.h"
+#include "physics_world.h"
+#include "solver.h"
+#include "solver_set.h"
+#include "recording.h"
+
+// needed for dll export
+#include "box3d/box3d.h"
+
+void b3MotorJoint_SetLinearVelocity( b3JointId jointId, b3Vec3 velocity )
+{
+	b3World* world = b3GetWorld( jointId.world0 );
+	B3_REC( world, MotorJointSetLinearVelocity, jointId, velocity );
+	b3JointSim* joint = b3GetJointSimCheckType( jointId, b3_motorJoint );
+	joint->motorJoint.linearVelocity = velocity;
+}
+
+b3Vec3 b3MotorJoint_GetLinearVelocity( b3JointId jointId )
+{
+	b3JointSim* joint = b3GetJointSimCheckType( jointId, b3_motorJoint );
+	return joint->motorJoint.linearVelocity;
+}
+
+void b3MotorJoint_SetAngularVelocity( b3JointId jointId, b3Vec3 velocity )
+{
+	b3World* world = b3GetWorld( jointId.world0 );
+	B3_REC( world, MotorJointSetAngularVelocity, jointId, velocity );
+	b3JointSim* joint = b3GetJointSimCheckType( jointId, b3_motorJoint );
+	joint->motorJoint.angularVelocity = velocity;
+}
+
+b3Vec3 b3MotorJoint_GetAngularVelocity( b3JointId jointId )
+{
+	b3JointSim* joint = b3GetJointSimCheckType( jointId, b3_motorJoint );
+	return joint->motorJoint.angularVelocity;
+}
+
+void b3MotorJoint_SetMaxVelocityTorque( b3JointId jointId, float maxTorque )
+{
+	b3World* world = b3GetWorld( jointId.world0 );
+	B3_REC( world, MotorJointSetMaxVelocityTorque, jointId, maxTorque );
+	b3JointSim* joint = b3GetJointSimCheckType( jointId, b3_motorJoint );
+	joint->motorJoint.maxVelocityTorque = maxTorque;
+}
+
+float b3MotorJoint_GetMaxVelocityTorque( b3JointId jointId )
+{
+	b3JointSim* joint = b3GetJointSimCheckType( jointId, b3_motorJoint );
+	return joint->motorJoint.maxVelocityTorque;
+}
+
+void b3MotorJoint_SetMaxVelocityForce( b3JointId jointId, float maxForce )
+{
+	b3World* world = b3GetWorld( jointId.world0 );
+	B3_REC( world, MotorJointSetMaxVelocityForce, jointId, maxForce );
+	b3JointSim* joint = b3GetJointSimCheckType( jointId, b3_motorJoint );
+	joint->motorJoint.maxVelocityForce = maxForce;
+}
+
+float b3MotorJoint_GetMaxVelocityForce( b3JointId jointId )
+{
+	b3JointSim* joint = b3GetJointSimCheckType( jointId, b3_motorJoint );
+	return joint->motorJoint.maxVelocityForce;
+}
+
+void b3MotorJoint_SetLinearHertz( b3JointId jointId, float hertz )
+{
+	b3World* world = b3GetWorld( jointId.world0 );
+	B3_REC( world, MotorJointSetLinearHertz, jointId, hertz );
+	b3JointSim* joint = b3GetJointSimCheckType( jointId, b3_motorJoint );
+	joint->motorJoint.linearHertz = hertz;
+}
+
+float b3MotorJoint_GetLinearHertz( b3JointId jointId )
+{
+	b3JointSim* joint = b3GetJointSimCheckType( jointId, b3_motorJoint );
+	return joint->motorJoint.linearHertz;
+}
+
+void b3MotorJoint_SetLinearDampingRatio( b3JointId jointId, float damping )
+{
+	b3World* world = b3GetWorld( jointId.world0 );
+	B3_REC( world, MotorJointSetLinearDampingRatio, jointId, damping );
+	b3JointSim* joint = b3GetJointSimCheckType( jointId, b3_motorJoint );
+	joint->motorJoint.linearDampingRatio = damping;
+}
+
+float b3MotorJoint_GetLinearDampingRatio( b3JointId jointId )
+{
+	b3JointSim* joint = b3GetJointSimCheckType( jointId, b3_motorJoint );
+	return joint->motorJoint.linearDampingRatio;
+}
+
+void b3MotorJoint_SetAngularHertz( b3JointId jointId, float hertz )
+{
+	b3World* world = b3GetWorld( jointId.world0 );
+	B3_REC( world, MotorJointSetAngularHertz, jointId, hertz );
+	b3JointSim* joint = b3GetJointSimCheckType( jointId, b3_motorJoint );
+	joint->motorJoint.angularHertz = hertz;
+}
+
+float b3MotorJoint_GetAngularHertz( b3JointId jointId )
+{
+	b3JointSim* joint = b3GetJointSimCheckType( jointId, b3_motorJoint );
+	return joint->motorJoint.angularHertz;
+}
+
+void b3MotorJoint_SetAngularDampingRatio( b3JointId jointId, float damping )
+{
+	b3World* world = b3GetWorld( jointId.world0 );
+	B3_REC( world, MotorJointSetAngularDampingRatio, jointId, damping );
+	b3JointSim* joint = b3GetJointSimCheckType( jointId, b3_motorJoint );
+	joint->motorJoint.angularDampingRatio = damping;
+}
+
+float b3MotorJoint_GetAngularDampingRatio( b3JointId jointId )
+{
+	b3JointSim* joint = b3GetJointSimCheckType( jointId, b3_motorJoint );
+	return joint->motorJoint.angularDampingRatio;
+}
+
+void b3MotorJoint_SetMaxSpringForce( b3JointId jointId, float maxForce )
+{
+	b3World* world = b3GetWorld( jointId.world0 );
+	B3_REC( world, MotorJointSetMaxSpringForce, jointId, maxForce );
+	b3JointSim* joint = b3GetJointSimCheckType( jointId, b3_motorJoint );
+	joint->motorJoint.maxSpringForce = b3MaxFloat( 0.0f, maxForce );
+}
+
+float b3MotorJoint_GetMaxSpringForce( b3JointId jointId )
+{
+	b3JointSim* joint = b3GetJointSimCheckType( jointId, b3_motorJoint );
+	return joint->motorJoint.maxSpringForce;
+}
+
+void b3MotorJoint_SetMaxSpringTorque( b3JointId jointId, float maxTorque )
+{
+	b3World* world = b3GetWorld( jointId.world0 );
+	B3_REC( world, MotorJointSetMaxSpringTorque, jointId, maxTorque );
+	b3JointSim* joint = b3GetJointSimCheckType( jointId, b3_motorJoint );
+	joint->motorJoint.maxSpringTorque = b3MaxFloat( 0.0f, maxTorque );
+}
+
+float b3MotorJoint_GetMaxSpringTorque( b3JointId jointId )
+{
+	b3JointSim* joint = b3GetJointSimCheckType( jointId, b3_motorJoint );
+	return joint->motorJoint.maxSpringTorque;
+}
+
+b3Vec3 b3GetMotorJointForce( b3World* world, b3JointSim* base )
+{
+	b3Vec3 force = b3MulSV( world->inv_h, b3Add( base->motorJoint.linearVelocityImpulse, base->motorJoint.linearSpringImpulse ) );
+	return force;
+}
+
+b3Vec3 b3GetMotorJointTorque( b3World* world, b3JointSim* base )
+{
+	return b3MulSV( world->inv_h, b3Add( base->motorJoint.angularVelocityImpulse, base->motorJoint.angularSpringImpulse ) );
+}
+
+// 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 b3PrepareMotorJoint( b3JointSim* base, b3StepContext* context )
+{
+	B3_ASSERT( base->type == b3_motorJoint );
+
+	// chase body id to the solver set where the body lives
+	int idA = base->bodyIdA;
+	int idB = base->bodyIdB;
+
+	b3World* world = context->world;
+
+	b3Body* bodyA = b3Array_Get( world->bodies, idA  );
+	b3Body* bodyB = b3Array_Get( world->bodies, idB  );
+
+	B3_ASSERT( bodyA->setIndex == b3_awakeSet || bodyB->setIndex == b3_awakeSet );
+
+	b3SolverSet* setA = b3Array_Get( world->solverSets, bodyA->setIndex  );
+	b3SolverSet* setB = b3Array_Get( world->solverSets, bodyB->setIndex  );
+
+	int localIndexA = bodyA->localIndex;
+	int localIndexB = bodyB->localIndex;
+
+	b3BodySim* bodySimA = b3Array_Get( setA->bodySims, localIndexA  );
+	b3BodySim* bodySimB = b3Array_Get( setB->bodySims, localIndexB  );
+
+	base->invMassA = bodySimA->invMass;
+	base->invMassB = bodySimB->invMass;
+	base->invIA = bodySimA->invInertiaWorld;
+	base->invIB = bodySimB->invInertiaWorld;
+
+	b3Matrix3 invInertiaSum = b3AddMM( base->invIA, base->invIB );
+	base->fixedRotation = b3Det( invInertiaSum ) < 1000.0f * FLT_MIN;
+
+	b3MotorJoint* joint = &base->motorJoint;
+	joint->indexA = bodyA->setIndex == b3_awakeSet ? localIndexA : B3_NULL_INDEX;
+	joint->indexB = bodyB->setIndex == b3_awakeSet ? localIndexB : B3_NULL_INDEX;
+
+	// Compute joint anchor frames with world space rotation, relative to center of mass
+	joint->frameA.q = b3MulQuat( bodySimA->transform.q, base->localFrameA.q );
+	joint->frameA.p = b3RotateVector( bodySimA->transform.q, b3Sub( base->localFrameA.p, bodySimA->localCenter ) );
+	joint->frameB.q = b3MulQuat( bodySimB->transform.q, base->localFrameB.q );
+	joint->frameB.p = b3RotateVector( bodySimB->transform.q, b3Sub( base->localFrameB.p, bodySimB->localCenter ) );
+
+	// Compute the initial center delta. Incremental position updates are relative to this.
+	joint->deltaCenter = b3SubPos( bodySimB->center, bodySimA->center );
+
+	joint->linearSpring = b3MakeSoft( joint->linearHertz, joint->linearDampingRatio, context->h );
+	joint->angularSpring = b3MakeSoft( joint->angularHertz, joint->angularDampingRatio, context->h );
+
+	joint->angularMass = b3InvertMatrix( invInertiaSum );
+
+	if ( context->enableWarmStarting == false )
+	{
+		joint->linearVelocityImpulse = b3Vec3_zero;
+		joint->angularVelocityImpulse = b3Vec3_zero;
+		joint->linearSpringImpulse = b3Vec3_zero;
+		joint->angularSpringImpulse = b3Vec3_zero;
+	}
+}
+
+void b3WarmStartMotorJoint( b3JointSim* base, b3StepContext* context )
+{
+	B3_ASSERT( base->type == b3_motorJoint );
+
+	float mA = base->invMassA;
+	float mB = base->invMassB;
+	b3Matrix3 iA = base->invIA;
+	b3Matrix3 iB = base->invIB;
+
+	b3MotorJoint* joint = &base->motorJoint;
+
+	// dummy state for static bodies
+	b3BodyState dummyState = b3_identityBodyState;
+
+	b3BodyState* stateA = joint->indexA == B3_NULL_INDEX ? &dummyState : context->states + joint->indexA;
+	b3BodyState* stateB = joint->indexB == B3_NULL_INDEX ? &dummyState : context->states + joint->indexB;
+
+	b3Vec3 rA = b3RotateVector( stateA->deltaRotation, joint->frameA.p );
+	b3Vec3 rB = b3RotateVector( stateB->deltaRotation, joint->frameB.p );
+
+	b3Vec3 linearImpulse = b3Add( joint->linearVelocityImpulse, joint->linearSpringImpulse );
+	b3Vec3 angularImpulse = b3Add( joint->angularVelocityImpulse, joint->angularSpringImpulse );
+
+	stateA->linearVelocity = b3MulSub( stateA->linearVelocity, mA, linearImpulse );
+	stateA->angularVelocity = b3Sub( stateA->angularVelocity, b3MulMV( iA, b3Add( b3Cross( rA, linearImpulse ), angularImpulse ) ) );
+	stateB->linearVelocity = b3MulAdd( stateB->linearVelocity, mB, linearImpulse );
+	stateB->angularVelocity = b3Add( stateB->angularVelocity, b3MulMV( iB, b3Add( b3Cross( rB, linearImpulse ), angularImpulse ) ) );
+}
+
+void b3SolveMotorJoint( b3JointSim* base, b3StepContext* context )
+{
+	B3_ASSERT( base->type == b3_motorJoint );
+
+	float mA = base->invMassA;
+	float mB = base->invMassB;
+	b3Matrix3 iA = base->invIA;
+	b3Matrix3 iB = base->invIB;
+
+	// dummy state for static bodies
+	b3BodyState dummyState = b3_identityBodyState;
+
+	b3MotorJoint* joint = &base->motorJoint;
+	b3BodyState* stateA = joint->indexA == B3_NULL_INDEX ? &dummyState : context->states + joint->indexA;
+	b3BodyState* stateB = joint->indexB == B3_NULL_INDEX ? &dummyState : context->states + joint->indexB;
+
+	b3Vec3 vA = stateA->linearVelocity;
+	b3Vec3 wA = stateA->angularVelocity;
+	b3Vec3 vB = stateB->linearVelocity;
+	b3Vec3 wB = stateB->angularVelocity;
+
+	b3Quat quatA = b3MulQuat( stateA->deltaRotation, joint->frameA.q );
+	b3Quat quatB = b3MulQuat( stateB->deltaRotation, joint->frameB.q );
+
+	if ( b3DotQuat( quatA, quatB ) < 0.0f )
+	{
+		// this keeps the rotation angle in the range [-pi, pi]
+		quatB = b3NegateQuat( quatB );
+	}
+
+	b3Quat relQ = b3InvMulQuat( quatA, quatB );
+
+	// angular spring
+	if ( joint->maxSpringTorque > 0.0f && joint->angularHertz > 0.0f )
+	{
+		b3Quat targetQuat = b3Quat_identity;
+		b3Vec3 deltaRotation = b3DeltaQuatToRotation( relQ, targetQuat );
+		b3Vec3 c = b3Neg( b3RotateVector( quatA, deltaRotation ) );
+
+		b3Vec3 bias = b3MulSV( joint->angularSpring.biasRate, c );
+		float massScale = joint->angularSpring.massScale;
+		float impulseScale = joint->angularSpring.impulseScale;
+
+		b3Vec3 cdot = b3Sub( wB, wA );
+
+		float maxImpulse = context->h * joint->maxSpringTorque;
+		b3Vec3 oldImpulse = joint->angularSpringImpulse;
+		b3Vec3 impulse = b3MulSub( b3MulSV( -massScale, b3MulMV( joint->angularMass, b3Add( cdot, bias ) ) ), impulseScale, oldImpulse );
+		joint->angularSpringImpulse = b3Add( oldImpulse, impulse );
+		if ( b3LengthSquared( joint->angularSpringImpulse ) > maxImpulse * maxImpulse )
+		{
+			joint->angularSpringImpulse = b3MulSV( maxImpulse, b3Normalize( joint->angularSpringImpulse ) );
+		}
+		impulse = b3Sub( joint->angularSpringImpulse, oldImpulse );
+
+		wA = b3Sub( wA, b3MulMV( iA, impulse ) );
+		wB = b3Add( wB, b3MulMV( iB, impulse ) );
+	}
+
+	// angular velocity
+	if ( joint->maxVelocityTorque > 0.0 )
+	{
+		b3Vec3 cdot = b3Sub( b3Sub( wB, wA ), joint->angularVelocity );
+		b3Vec3 impulse = b3Neg( b3MulMV( joint->angularMass, cdot ) );
+
+		float maxImpulse = context->h * joint->maxVelocityTorque;
+		b3Vec3 oldImpulse = joint->angularVelocityImpulse;
+		joint->angularVelocityImpulse = b3Add( oldImpulse, impulse );
+		if ( b3LengthSquared( joint->angularVelocityImpulse ) > maxImpulse * maxImpulse )
+		{
+			joint->angularVelocityImpulse = b3MulSV( maxImpulse, b3Normalize( joint->angularVelocityImpulse ) );
+		}
+		impulse = b3Sub( joint->angularVelocityImpulse, oldImpulse );
+
+		wA = b3Sub( wA, b3MulMV( iA, impulse ) );
+		wB = b3Add( wB, b3MulMV( iB, impulse ) );
+	}
+
+	b3Vec3 rA = b3RotateVector( stateA->deltaRotation, joint->frameA.p );
+	b3Vec3 rB = b3RotateVector( stateB->deltaRotation, joint->frameB.p );
+
+	// linear spring
+	if ( joint->maxSpringForce > 0.0f && joint->linearHertz > 0.0f )
+	{
+		b3Vec3 dcA = stateA->deltaPosition;
+		b3Vec3 dcB = stateB->deltaPosition;
+		b3Vec3 c = b3Add( b3Add( b3Sub( dcB, dcA ), b3Sub( rB, rA ) ), joint->deltaCenter );
+
+		b3Vec3 bias = b3MulSV( joint->linearSpring.biasRate, c );
+		float massScale = joint->linearSpring.massScale;
+		float impulseScale = joint->linearSpring.impulseScale;
+
+		b3Vec3 cdot = b3Sub( b3Add( vB, b3Cross( wB, rB ) ), b3Add( vA, b3Cross( wA, rA ) ) );
+
+		//// K = [(1/m1 + 1/m2) * eye(2) - skew(r1) * invI1 * skew(r1) - skew(r2) * invI2 * skew(r2)]
+		b3Matrix3 sA = b3Skew( rA );
+		b3Matrix3 sB = b3Skew( rB );
+		b3Matrix3 kA = b3MulMM( sA, b3MulMM( base->invIA, sA ) );
+		b3Matrix3 kB = b3MulMM( sB, b3MulMM( base->invIB, sB ) );
+		b3Matrix3 k = b3NegateMat3( b3AddMM( kA, kB ) );
+		k.cx.x += mA + mB;
+		k.cy.y += mA + mB;
+		k.cz.z += mA + mB;
+
+		b3Vec3 b = b3Solve3( k, b3Add( cdot, bias ) );
+
+		b3Vec3 oldImpulse = joint->linearSpringImpulse;
+		b3Vec3 impulse = b3MulSub( b3MulSV( -massScale, b ), impulseScale, oldImpulse );
+		float maxImpulse = context->h * joint->maxSpringForce;
+		joint->linearSpringImpulse = b3Add( joint->linearSpringImpulse, impulse );
+
+		if ( b3LengthSquared( joint->linearSpringImpulse ) > maxImpulse * maxImpulse )
+		{
+			joint->linearSpringImpulse = b3MulSV( maxImpulse, b3Normalize( joint->linearSpringImpulse ) );
+		}
+
+		impulse = b3Sub( joint->linearSpringImpulse, oldImpulse );
+
+		vA = b3MulSub( vA, mA, impulse );
+		wA = b3Sub( wA, b3MulMV( iA, b3Cross( rA, impulse ) ) );
+		vB = b3MulAdd( vB, mB, impulse );
+		wB = b3Add( wB, b3MulMV( iB, b3Cross( rB, impulse ) ) );
+	}
+
+	// linear velocity
+	if ( joint->maxVelocityForce > 0.0f )
+	{
+		b3Vec3 cdot = b3Sub( b3Add( vB, b3Cross( wB, rB ) ), b3Add( vA, b3Cross( wA, rA ) ) );
+		cdot = b3Sub( cdot, joint->linearVelocity );
+		//// K = [(1/m1 + 1/m2) * eye(2) - skew(r1) * invI1 * skew(r1) - skew(r2) * invI2 * skew(r2)]
+		b3Matrix3 sA = b3Skew( rA );
+		b3Matrix3 sB = b3Skew( rB );
+		b3Matrix3 kA = b3MulMM( sA, b3MulMM( base->invIA, sA ) );
+		b3Matrix3 kB = b3MulMM( sB, b3MulMM( base->invIB, sB ) );
+		b3Matrix3 k = b3NegateMat3( b3AddMM( kA, kB ) );
+		k.cx.x += mA + mB;
+		k.cy.y += mA + mB;
+		k.cz.z += mA + mB;
+
+		b3Vec3 b = b3Solve3( k, cdot );
+		b3Vec3 impulse = b3Neg( b );
+
+		b3Vec3 oldImpulse = joint->linearVelocityImpulse;
+		float maxImpulse = context->h * joint->maxVelocityForce;
+		joint->linearVelocityImpulse = b3Add( joint->linearVelocityImpulse, impulse );
+
+		if ( b3LengthSquared( joint->linearVelocityImpulse ) > maxImpulse * maxImpulse )
+		{
+			joint->linearVelocityImpulse = b3MulSV( maxImpulse, b3Normalize( joint->linearVelocityImpulse ) );
+		}
+
+		impulse = b3Sub( joint->linearVelocityImpulse, oldImpulse );
+
+		vA = b3MulSub( vA, mA, impulse );
+		wA = b3Sub( wA, b3MulMV( iA, b3Cross( rA, impulse ) ) );
+		vB = b3MulAdd( vB, mB, impulse );
+		wB = b3Add( wB, b3MulMV( iB, b3Cross( rB, impulse ) ) );
+	}
+
+	if ( stateA->flags & b3_dynamicFlag )
+	{
+		stateA->linearVelocity = vA;
+		stateA->angularVelocity = wA;
+	}
+
+	if ( stateB->flags & b3_dynamicFlag )
+	{
+		stateB->linearVelocity = vB;
+		stateB->angularVelocity = wB;
+	}
+}

vendor/box3d/src/src/mover.c

@@ -0,0 +1,70 @@
+// SPDX-FileCopyrightText: 2025 Erin Catto
+// SPDX-License-Identifier: MIT
+
+#include "math_internal.h"
+
+#include "box3d/collision.h"
+#include "box3d/constants.h"
+
+b3PlaneSolverResult b3SolvePlanes( b3Vec3 targetDelta, b3CollisionPlane* planes, int count )
+{
+	for ( int i = 0; i < count; ++i )
+	{
+		planes[i].push = 0.0f;
+	}
+
+	b3Vec3 delta = targetDelta;
+	float tolerance = B3_LINEAR_SLOP;
+
+	int iteration;
+	for ( iteration = 0; iteration < 20; ++iteration )
+	{
+		float totalPush = 0.0f;
+		for ( int planeIndex = 0; planeIndex < count; ++planeIndex )
+		{
+			b3CollisionPlane* plane = planes + planeIndex;
+
+			// Add slop to prevent jitter
+			float separation = b3PlaneSeparation( plane->plane, delta ) + B3_LINEAR_SLOP;
+
+			float push = -separation;
+
+			// Clamp accumulated push
+			float accumulatedPush = plane->push;
+			plane->push = b3ClampFloat( plane->push + push, 0.0f, plane->pushLimit );
+			push = plane->push - accumulatedPush;
+			delta = b3MulAdd( delta, push, plane->plane.normal );
+
+			// Track maximum push for convergence
+			totalPush += b3AbsFloat( push );
+		}
+
+		if ( totalPush < tolerance )
+		{
+			break;
+		}
+	}
+
+	return (b3PlaneSolverResult){
+		.delta = delta,
+		.iterationCount = iteration,
+	};
+}
+
+b3Vec3 b3ClipVector( b3Vec3 vector, const b3CollisionPlane* planes, int count )
+{
+	b3Vec3 v = vector;
+
+	for ( int planeIndex = 0; planeIndex < count; ++planeIndex )
+	{
+		const b3CollisionPlane* plane = planes + planeIndex;
+		if ( plane->push == 0.0f || plane->clipVelocity == false )
+		{
+			continue;
+		}
+
+		v = b3MulSub( v, b3MinFloat( 0.0f, b3Dot( v, plane->plane.normal ) ), plane->plane.normal );
+	}
+
+	return v;
+}

vendor/box3d/src/src/name_cache.c

@@ -0,0 +1,155 @@
+// SPDX-FileCopyrightText: 2026 Erin Catto
+// SPDX-License-Identifier: MIT
+
+#include "name_cache.h"
+
+#include "recording.h"
+
+#include <string.h>
+
+#define NAME b3NameMap
+#define KEY_TY uint32_t
+#define VAL_TY int
+#define HASH_FN vt_hash_integer
+#define CMPR_FN vt_cmpr_integer
+#define MALLOC_FN b3Alloc
+#define FREE_FN b3Free
+#include "verstable.h"
+
+#define FNV32_OFFSET_BASIS 0x811c9dc5u
+#define FNV32_PRIME 0x01000193u
+
+// This is designed to hash small strings and while avoiding collisions
+// for a 32-bit hash.
+// Note: changing this will break recordings
+uint32_t b3Hash32( const void* data, size_t length )
+{
+	// Cast to unsigned to avoid sign extension
+	const unsigned char* p = data;
+
+	// FNV-1a
+	uint32_t h = FNV32_OFFSET_BASIS;
+	for ( size_t i = 0; i < length; ++i )
+	{
+		h ^= p[i];
+		h *= FNV32_PRIME;
+	}
+
+	// Murmur3 finalizer to help with short strings
+	h ^= h >> 16;
+	h *= 0x85ebca6bu;
+	h ^= h >> 13;
+	h *= 0xc2b2ae35u;
+	h ^= h >> 16;
+	return h;
+}
+
+b3NameCache b3CreateNameCache( void )
+{
+	b3NameCache cache = { 0 };
+	b3Array_Create( cache.entries );
+
+	b3NameMap* map = b3Alloc( sizeof( b3NameMap ) );
+	b3NameMap_init( map );
+	cache.map = map;
+	return cache;
+}
+
+void b3DestroyNameCache( b3NameCache* cache )
+{
+	int count = cache->entries.count;
+	for ( int i = 0; i < count; ++i )
+	{
+		b3NameEntry* entry = cache->entries.data + i;
+		b3Free( entry->name, entry->length + 1 );
+	}
+
+	b3Array_Destroy( cache->entries );
+	b3NameMap_cleanup( (b3NameMap*)cache->map );
+	b3Free( cache->map, sizeof( b3NameMap ) );
+}
+
+uint32_t b3AddName( b3NameCache* cache, const char* name )
+{
+	if ( name == NULL || name[0] == 0 )
+	{
+		return B3_NULL_NAME;
+	}
+
+	size_t length = strlen( name );
+	uint32_t id = b3Hash32( name, length );
+
+	id = id == 0 ? 1 : id;
+
+	b3NameMap* map = cache->map;
+	b3NameMap_itr itr = b3NameMap_get( map, id );
+	if ( b3NameMap_is_end( itr ) == false )
+	{
+		if ( strcmp( cache->entries.data[itr.data->val].name, name ) != 0 )
+		{
+			// Different name, same hash
+			b3Log( "Hash collision on %s", name );
+		}
+		return id;
+	}
+
+	char* clone = b3Alloc( length + 1 );
+	memcpy( clone, name, length + 1 );
+	int index = cache->entries.count;
+	b3NameEntry entry = {
+		.hash = id,
+		.name = clone,
+		.length = (int)length,
+	};
+
+	b3Array_Push( cache->entries, entry );
+
+	b3NameMap_insert( map, id, index );
+	return id;
+}
+
+const char* b3FindName( const b3NameCache* cache, uint32_t id )
+{
+	b3NameMap* map = cache->map;
+	b3NameMap_itr itr = b3NameMap_get( map, id );
+	if ( b3NameMap_is_end( itr ) == false )
+	{
+		int index = itr.data->val;
+		const b3NameEntry* entry = b3Array_Get( cache->entries, index );
+		return entry->name;
+	}
+
+	return NULL;
+}
+
+void b3LoadName( b3NameCache* cache, uint32_t id, char* name, int length )
+{
+	if ( name == NULL )
+	{
+		return;
+	}
+
+	if ( name[0] == 0 || id == B3_NULL_NAME )
+	{
+		b3Free( name, length + 1 );
+		return;
+	}
+
+	b3NameMap* map = cache->map;
+	b3NameMap_itr itr = b3NameMap_get( map, id );
+	if ( b3NameMap_is_end( itr ) == false )
+	{
+		b3Free( name, length + 1 );
+		return;
+	}
+
+	int index = cache->entries.count;
+	b3NameEntry entry = {
+		.hash = id,
+		.name = name,
+		.length = length,
+	};
+
+	b3Array_Push( cache->entries, entry );
+	b3NameMap_insert( map, id, index );
+}

vendor/box3d/src/src/name_cache.h

@@ -0,0 +1,42 @@
+// SPDX-FileCopyrightText: 2026 Erin Catto
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include "container.h"
+
+#define B3_NULL_NAME 0u
+
+typedef struct b3NameEntry
+{
+	// Names are used for debugging and I'm favoring simplicity and
+	// minimal storage. Collisions will be logged.
+	uint32_t hash;
+	int length;
+	char* name;
+} b3NameEntry;
+
+b3DeclareArray( b3NameEntry );
+
+typedef struct b3NameCache
+{
+	b3Array( b3NameEntry ) entries;
+	void* map;
+} b3NameCache;
+
+b3NameCache b3CreateNameCache( void );
+void b3DestroyNameCache( b3NameCache* cache );
+
+uint32_t b3AddName( b3NameCache* cache, const char* name );
+const char* b3FindName( const b3NameCache* cache, uint32_t id );
+
+// Load a name from a recording. Name ownership is transferred.
+void b3LoadName( b3NameCache* cache, uint32_t id, char* name, int length );
+
+uint32_t b3Hash32( const void* data, size_t length );
+
+static inline const char* b3FindNameWithDefault( const b3NameCache* cache, uint32_t id, const char* def )
+{
+	const char* name = b3FindName( cache, id );
+	return name == NULL ? def : name;
+}

vendor/box3d/src/src/parallel_for.c

@@ -0,0 +1,135 @@
+// SPDX-FileCopyrightText: 2026 Erin Catto
+// SPDX-License-Identifier: MIT
+
+#include "parallel_for.h"
+
+#include "core.h"
+#include "physics_world.h"
+#include "platform.h"
+
+#include "box3d/base.h"
+#include "box3d/constants.h"
+
+#include <stddef.h>
+
+// Shared state for one b3ParallelFor invocation. Workers race on nextBlock to
+// claim work, so a slow chunk can't strand the other threads.
+typedef struct b3ParallelForShared
+{
+	b3AtomicInt nextBlock;
+	int blockCount;
+	int blockSize;
+	int itemCount;
+	b3ParallelForCallback* callback;
+	void* context;
+} b3ParallelForShared;
+
+typedef struct b3ParallelForTask
+{
+	b3ParallelForShared* shared;
+	int workerIndex;
+} b3ParallelForTask;
+
+static void b3ParallelForTrampoline( void* taskContext )
+{
+	b3ParallelForTask* task = (b3ParallelForTask*)taskContext;
+	b3ParallelForShared* shared = task->shared;
+	int workerIndex = task->workerIndex;
+	void* context = shared->context;
+	b3ParallelForCallback* callback = shared->callback;
+
+	int blockCount = shared->blockCount;
+	int blockSize = shared->blockSize;
+	int itemCount = shared->itemCount;
+
+	for ( ;; )
+	{
+		int blockIndex = b3AtomicFetchAddInt( &shared->nextBlock, 1 );
+		if ( blockIndex >= blockCount )
+		{
+			break;
+		}
+
+		int start = blockIndex * blockSize;
+		int end = start + blockSize;
+		if ( end > itemCount )
+		{
+			end = itemCount;
+		}
+
+		callback( start, end, workerIndex, context );
+	}
+}
+
+void b3ParallelFor( b3World* world, b3ParallelForCallback* callback, int itemCount, int minRange, void* context,
+					const char* name )
+{
+	if ( itemCount <= 0 )
+	{
+		return;
+	}
+
+	B3_ASSERT( minRange > 0 );
+
+	int workerCount = world->workerCount;
+	B3_ASSERT( 0 < workerCount && workerCount <= B3_MAX_WORKERS );
+
+	// Target multiple blocks per worker to reduce thread stalls.
+	// block size grows once items exceed maxBlockCount * minRange
+	// so the block count stays bounded and per-block sync overhead stays low.
+	int blocksPerWorker = 4;
+	int maxBlockCount = blocksPerWorker * workerCount;
+
+	int blockSize;
+	int blockCount;
+	if ( itemCount <= minRange * maxBlockCount )
+	{
+		blockSize = minRange;
+		blockCount = ( itemCount + blockSize - 1 ) / blockSize;
+	}
+	else
+	{
+		blockSize = ( itemCount + maxBlockCount - 1 ) / maxBlockCount;
+		blockCount = ( itemCount + blockSize - 1 ) / blockSize;
+	}
+	B3_ASSERT( blockCount >= 1 );
+	B3_ASSERT( blockSize * blockCount >= itemCount );
+
+	// No point enqueueing more tasks than blocks.
+	int taskCount = workerCount < blockCount ? workerCount : blockCount;
+
+	b3ParallelForShared shared;
+	shared.blockCount = blockCount;
+	shared.blockSize = blockSize;
+	shared.itemCount = itemCount;
+	shared.callback = callback;
+	shared.context = context;
+	b3AtomicStoreInt( &shared.nextBlock, 0 );
+
+	b3ParallelForTask tasks[B3_MAX_WORKERS];
+	void* handles[B3_MAX_WORKERS];
+	for ( int i = 0; i < taskCount; ++i )
+	{
+		tasks[i].shared = &shared;
+		tasks[i].workerIndex = i;
+
+		if (world->taskCount < B3_MAX_TASKS)
+		{
+			handles[i] = world->enqueueTaskFcn( &b3ParallelForTrampoline, tasks + i, world->userTaskContext, name );
+			world->taskCount += 1;
+		}
+		else
+		{
+			handles[i] = NULL;
+			b3ParallelForTrampoline( tasks + i );
+		}
+	}
+
+	for ( int i = 0; i < taskCount; ++i )
+	{
+		if ( handles[i] != NULL )
+		{
+			world->finishTaskFcn( handles[i], world->userTaskContext );
+		}
+	}
+}

vendor/box3d/src/src/parallel_for.h

@@ -0,0 +1,23 @@
+// SPDX-FileCopyrightText: 2026 Erin Catto
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+typedef struct b3World b3World;
+
+// Callback invoked by b3ParallelFor to process a range of items. May be called
+// multiple times per worker: work is divided into blocks that workers claim
+// atomically, so a worker that finishes early picks up the next unclaimed
+// block instead of sitting idle. workerIndex is the worker identity and is
+// stable across all invocations from the same worker, so it is safe to use as
+// an index into per-worker state (e.g. world->taskContexts.data + workerIndex).
+typedef void b3ParallelForCallback( int startIndex, int endIndex, int workerIndex, void* context );
+
+// Divide [0, itemCount) into blocks and process them with cooperative claiming:
+// up to world->workerCount tasks are enqueued, and each task loops, atomically
+// claiming the next unclaimed block until the range is drained. Blocks the
+// caller until all work is complete. minRange is the minimum block size; block
+// size grows once itemCount exceeds 4 * workerCount * minRange so block count
+// stays bounded.
+void b3ParallelFor( b3World* world, b3ParallelForCallback* callback, int itemCount, int minRange, void* context,
+					const char* name );

vendor/box3d/src/src/parallel_joint.c

@@ -0,0 +1,251 @@
+// SPDX-FileCopyrightText: 2025 Erin Catto
+// SPDX-License-Identifier: MIT
+
+#include "body.h"
+#include "core.h"
+#include "joint.h"
+#include "math_internal.h"
+#include "physics_world.h"
+#include "solver.h"
+#include "solver_set.h"
+#include "recording.h"
+
+// needed for dll export
+#include "box3d/box3d.h"
+
+void b3ParallelJoint_SetSpringHertz( b3JointId jointId, float hertz )
+{
+	B3_ASSERT( b3IsValidFloat( hertz ) && hertz >= 0.0f );
+	b3World* world = b3GetWorld( jointId.world0 );
+	B3_REC( world, ParallelJointSetSpringHertz, jointId, hertz );
+	b3JointSim* base = b3GetJointSimCheckType( jointId, b3_parallelJoint );
+	base->parallelJoint.hertz = hertz;
+}
+
+float b3ParallelJoint_GetSpringHertz( b3JointId jointId )
+{
+	b3JointSim* base = b3GetJointSimCheckType( jointId, b3_parallelJoint );
+	return base->parallelJoint.hertz;
+}
+
+void b3ParallelJoint_SetSpringDampingRatio( b3JointId jointId, float dampingRatio )
+{
+	B3_ASSERT( b3IsValidFloat( dampingRatio ) && dampingRatio >= 0.0f );
+	b3World* world = b3GetWorld( jointId.world0 );
+	B3_REC( world, ParallelJointSetSpringDampingRatio, jointId, dampingRatio );
+	b3JointSim* base = b3GetJointSimCheckType( jointId, b3_parallelJoint );
+	base->parallelJoint.dampingRatio = dampingRatio;
+}
+
+float b3ParallelJoint_GetSpringDampingRatio( b3JointId jointId )
+{
+	b3JointSim* base = b3GetJointSimCheckType( jointId, b3_parallelJoint );
+	return base->parallelJoint.dampingRatio;
+}
+
+void b3ParallelJoint_SetMaxTorque( b3JointId jointId, float maxForce )
+{
+	B3_ASSERT( b3IsValidFloat( maxForce ) && maxForce >= 0.0f );
+	b3World* world = b3GetWorld( jointId.world0 );
+	B3_REC( world, ParallelJointSetMaxTorque, jointId, maxForce );
+	b3JointSim* base = b3GetJointSimCheckType( jointId, b3_parallelJoint );
+	base->parallelJoint.maxTorque = maxForce;
+}
+
+float b3ParallelJoint_GetMaxTorque( b3JointId jointId )
+{
+	b3JointSim* base = b3GetJointSimCheckType( jointId, b3_parallelJoint );
+	return base->parallelJoint.maxTorque;
+}
+
+b3Vec3 b3GetParallelJointTorque( b3World* world, b3JointSim* base )
+{
+	b3ParallelJoint* joint = &base->parallelJoint;
+
+	b3Quat relQ = b3InvMulQuat( joint->quatA, joint->quatB );
+	joint->perpAxisX = b3MulSV( 0.5f, b3RotateVector( joint->quatA, b3Add( b3MulSV( relQ.s, b3Vec3_axisX ), b3Cross( relQ.v, b3Vec3_axisX ) ) ) );
+	joint->perpAxisY = b3MulSV( 0.5f, b3RotateVector( joint->quatA, b3Add( b3MulSV( relQ.s, b3Vec3_axisY ), b3Cross( relQ.v, b3Vec3_axisY ) ) ) );
+
+	b3Vec3 angularImpulse = b3Blend2( joint->perpImpulse.x, joint->perpAxisX, joint->perpImpulse.y, joint->perpAxisY );
+	b3Vec3 torque = b3MulSV( world->inv_h, angularImpulse );
+	return torque;
+}
+
+void b3PrepareParallelJoint( b3JointSim* base, b3StepContext* context )
+{
+	B3_ASSERT( base->type == b3_parallelJoint );
+
+	b3World* world = context->world;
+
+	b3Body* bodyA = b3Array_Get( world->bodies, base->bodyIdA  );
+	b3Body* bodyB = b3Array_Get( world->bodies, base->bodyIdB  );
+
+	B3_ASSERT( bodyA->setIndex == b3_awakeSet || bodyB->setIndex == b3_awakeSet );
+	b3SolverSet* setA = b3Array_Get( world->solverSets, bodyA->setIndex  );
+	b3SolverSet* setB = b3Array_Get( world->solverSets, bodyB->setIndex  );
+
+	int localIndexA = bodyA->localIndex;
+	int localIndexB = bodyB->localIndex;
+
+	b3BodySim* bodySimA = b3Array_Get( setA->bodySims, localIndexA  );
+	b3BodySim* bodySimB = b3Array_Get( setB->bodySims, localIndexB  );
+
+	base->invMassA = bodySimA->invMass;
+	base->invMassB = bodySimB->invMass;
+	base->invIA = bodySimA->invInertiaWorld;
+	base->invIB = bodySimB->invInertiaWorld;
+
+	b3Matrix3 invInertiaSum = b3AddMM( base->invIA, base->invIB );
+	base->fixedRotation = b3Det( invInertiaSum ) < 1000.0f * FLT_MIN;
+
+	b3ParallelJoint* joint = &base->parallelJoint;
+	joint->indexA = bodyA->setIndex == b3_awakeSet ? localIndexA : B3_NULL_INDEX;
+	joint->indexB = bodyB->setIndex == b3_awakeSet ? localIndexB : B3_NULL_INDEX;
+
+	// Compute joint anchor frames with world space rotation, relative to center of mass
+	joint->quatA = b3MulQuat( bodySimA->transform.q, base->localFrameA.q );
+	joint->quatB = b3MulQuat( bodySimB->transform.q, base->localFrameB.q );
+
+	b3Quat relQ = b3InvMulQuat( joint->quatA, joint->quatB );
+
+	{
+		// These are needed for warm starting
+		joint->perpAxisX = b3MulSV( 0.5f, b3RotateVector( joint->quatA, b3Add( b3MulSV( relQ.s, b3Vec3_axisX ), b3Cross( relQ.v, b3Vec3_axisX ) ) ) );
+		joint->perpAxisY = b3MulSV( 0.5f, b3RotateVector( joint->quatA, b3Add( b3MulSV( relQ.s, b3Vec3_axisY ), b3Cross( relQ.v, b3Vec3_axisY ) ) ) );
+	}
+
+	joint->softness = b3MakeSoft( joint->hertz, joint->dampingRatio, context->h );
+
+	if ( context->enableWarmStarting == false )
+	{
+		joint->perpImpulse = (b3Vec2){ 0.0f, 0.0f };
+	}
+}
+
+void b3WarmStartParallelJoint( b3JointSim* base, b3StepContext* context )
+{
+	B3_ASSERT( base->type == b3_parallelJoint );
+
+	b3Matrix3 iA = base->invIA;
+	b3Matrix3 iB = base->invIB;
+
+	// dummy state for static bodies
+	b3BodyState dummyState = b3_identityBodyState;
+
+	b3ParallelJoint* joint = &base->parallelJoint;
+
+	b3BodyState* stateA = joint->indexA == B3_NULL_INDEX ? &dummyState : context->states + joint->indexA;
+	b3BodyState* stateB = joint->indexB == B3_NULL_INDEX ? &dummyState : context->states + joint->indexB;
+
+	b3Vec3 wA = stateA->angularVelocity;
+	b3Vec3 wB = stateB->angularVelocity;
+
+	b3Vec3 angularImpulse = b3Blend2( joint->perpImpulse.x, joint->perpAxisX, joint->perpImpulse.y, joint->perpAxisY );
+
+	wA = b3Sub( wA, b3MulMV( iA, angularImpulse ) );
+	wB = b3Add( wB, b3MulMV( iB, angularImpulse ) );
+
+	if ( stateA->flags & b3_dynamicFlag )
+	{
+		stateA->angularVelocity = wA;
+	}
+
+	if ( stateB->flags & b3_dynamicFlag )
+	{
+		stateB->angularVelocity = wB;
+	}
+}
+
+void b3SolveParallelJoint( b3JointSim* base, b3StepContext* context )
+{
+	b3Matrix3 iA = base->invIA;
+	b3Matrix3 iB = base->invIB;
+
+	// dummy state for static bodies
+	b3BodyState dummyState = b3_identityBodyState;
+
+	b3ParallelJoint* joint = &base->parallelJoint;
+	b3BodyState* stateA = joint->indexA == B3_NULL_INDEX ? &dummyState : context->states + joint->indexA;
+	b3BodyState* stateB = joint->indexB == B3_NULL_INDEX ? &dummyState : context->states + joint->indexB;
+
+	b3Vec3 wA = stateA->angularVelocity;
+	b3Vec3 wB = stateB->angularVelocity;
+
+	bool fixedRotation = base->fixedRotation;
+	b3Quat quatA = b3MulQuat( stateA->deltaRotation, joint->quatA );
+	b3Quat quatB = b3MulQuat( stateB->deltaRotation, joint->quatB );
+
+	if ( b3DotQuat( quatA, quatB ) < 0.0f )
+	{
+		// this keeps the rotation angle in the range [-pi, pi]
+		quatB = b3NegateQuat( quatB );
+	}
+
+	b3Quat relQ = b3InvMulQuat( quatA, quatB );
+
+	if ( fixedRotation == false && joint->maxTorque > 0.0f )
+	{
+		b3Vec2 c = { relQ.v.x, relQ.v.y };
+		b3Vec2 bias = { joint->softness.biasRate * c.x, joint->softness.biasRate * c.y };
+		float massScale = joint->softness.massScale;
+		float impulseScale = joint->softness.impulseScale;
+
+		// Collinearity constraint as 2-by-2
+		b3Vec3 perpAxisX = b3MulSV( 0.5f, b3RotateVector( quatA, b3Add( b3MulSV( relQ.s, b3Vec3_axisX ), b3Cross( relQ.v, b3Vec3_axisX ) ) ) );
+		b3Vec3 perpAxisY = b3MulSV( 0.5f, b3RotateVector( quatA, b3Add( b3MulSV( relQ.s, b3Vec3_axisY ), b3Cross( relQ.v, b3Vec3_axisY ) ) ) );
+		joint->perpAxisX = perpAxisX;
+		joint->perpAxisY = perpAxisY;
+
+		b3Matrix3 invInertiaSum = b3AddMM( iA, iB );
+		float kxx = b3Dot( perpAxisX, b3MulMV( invInertiaSum, perpAxisX ) );
+		float kyy = b3Dot( perpAxisY, b3MulMV( invInertiaSum, perpAxisY ) );
+		float kxy = b3Dot( perpAxisX, b3MulMV( invInertiaSum, perpAxisY ) );
+
+		b3Matrix2 k = { { kxx, kxy }, { kxy, kyy } };
+
+		b3Vec3 wRel = b3Sub( wB, wA );
+		b3Vec2 cdot = { b3Dot( wRel, perpAxisX ), b3Dot( wRel, perpAxisY ) };
+
+		float maxImpulse = context->h * joint->maxTorque;
+		b3Vec2 oldImpulse = joint->perpImpulse;
+		b3Vec2 cdotPlusBias = { cdot.x + bias.x, cdot.y + bias.y };
+		b3Vec2 sol = b3Solve2( k, cdotPlusBias );
+		b3Vec2 deltaImpulse = {
+			-massScale * sol.x - impulseScale * oldImpulse.x,
+			-massScale * sol.y - impulseScale * oldImpulse.y,
+		};
+		joint->perpImpulse = (b3Vec2){ oldImpulse.x + deltaImpulse.x, oldImpulse.y + deltaImpulse.y };
+		if ( b3LengthSquared2( joint->perpImpulse ) > maxImpulse * maxImpulse )
+		{
+			float s = maxImpulse / b3Length2( joint->perpImpulse );
+			joint->perpImpulse = (b3Vec2){ s * joint->perpImpulse.x, s * joint->perpImpulse.y };
+		}
+
+		deltaImpulse = b3Sub2( joint->perpImpulse, oldImpulse );
+
+		b3Vec3 angularImpulse = b3Blend2( deltaImpulse.x, perpAxisX, deltaImpulse.y, perpAxisY );
+		wA = b3Sub( wA, b3MulMV( iA, angularImpulse ) );
+		wB = b3Add( wB, b3MulMV( iB, angularImpulse ) );
+	}
+
+	if ( stateA->flags & b3_dynamicFlag )
+	{
+		stateA->angularVelocity = wA;
+	}
+
+	if ( stateB->flags & b3_dynamicFlag )
+	{
+		stateB->angularVelocity = wB;
+	}
+}
+
+void b3DrawParallelJoint( b3DebugDraw* draw, b3JointSim* base, b3WorldTransform transformA, b3WorldTransform transformB, float scale )
+{
+	float length = 0.1f * scale;
+
+	b3WorldTransform frameA = b3MulWorldTransforms( transformA, base->localFrameA );
+	draw->DrawSegmentFcn( frameA.p, b3OffsetPos( frameA.p, b3MulSV( length, b3RotateVector( frameA.q, b3Vec3_axisZ ) ) ), b3_colorGreen, draw->context );
+
+	b3WorldTransform frameB = b3MulWorldTransforms( transformB, base->localFrameB );
+	draw->DrawSegmentFcn( frameB.p, b3OffsetPos( frameB.p, b3MulSV( length, b3RotateVector( frameB.q, b3Vec3_axisZ ) ) ), b3_colorBlue, draw->context );
+}

vendor/box3d/src/src/physics_world.h

@@ -0,0 +1,350 @@
+// SPDX-FileCopyrightText: 2025 Erin Catto
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include "arena_allocator.h"
+#include "bitset.h"
+#include "block_allocator.h"
+#include "broad_phase.h"
+#include "constraint_graph.h"
+#include "id_pool.h"
+#include "name_cache.h"
+
+#include "box3d/types.h"
+
+#define B3_DEBUG_POINT_CAPACITY 64
+#define B3_DEBUG_LINE_CAPACITY 64
+
+typedef struct b3Body b3Body;
+typedef struct b3Recording b3Recording;
+typedef struct b3Contact b3Contact;
+typedef struct b3Island b3Island;
+typedef struct b3Joint b3Joint;
+typedef struct b3Sensor b3Sensor;
+typedef struct b3SensorTaskContext b3SensorTaskContext;
+typedef struct b3SensorHit b3SensorHit;
+typedef struct b3Shape b3Shape;
+typedef struct b3SolverSet b3SolverSet;
+
+b3DeclareArray( b3BlockAllocator );
+b3DeclareArray( b3Body );
+b3DeclareArray( b3SolverSet );
+b3DeclareArray( b3Joint );
+b3DeclareArray( b3Contact );
+b3DeclareArray( b3Island );
+b3DeclareArray( b3Shape );
+b3DeclareArray( b3Sensor );
+b3DeclareArray( b3SensorTaskContext );
+b3DeclareArray( b3SensorHit );
+b3DeclareArray( b3BodyMoveEvent );
+b3DeclareArray( b3SensorBeginTouchEvent );
+b3DeclareArray( b3ContactBeginTouchEvent );
+b3DeclareArray( b3SensorEndTouchEvent );
+b3DeclareArray( b3ContactEndTouchEvent );
+b3DeclareArray( b3ContactHitEvent );
+b3DeclareArray( b3JointEvent );
+
+enum b3SetType
+{
+	b3_staticSet = 0,
+	b3_disabledSet = 1,
+	b3_awakeSet = 2,
+	b3_firstSleepingSet = 3,
+};
+
+typedef struct b3DebugPoint
+{
+	b3Pos p;
+	int label;
+	float value;
+	b3HexColor color;
+} b3DebugPoint;
+
+typedef struct b3DebugLine
+{
+	b3Pos p1, p2;
+	int label;
+	b3HexColor color;
+} b3DebugLine;
+
+// Per thread task storage
+typedef struct b3TaskContext
+{
+	b3Arena arena;
+
+	// Collect per thread sensor continuous hit events.
+	b3Array( b3SensorHit ) sensorHits;
+
+	// These bits align with the b3ConstraintGraph::contactBlocks and signal a change in contact status
+	b3BitSet contactStateBitSet;
+
+	// These bits align with the joint id capacity and signal a change in contact status
+	b3BitSet jointStateBitSet;
+
+	// These bits align with the contact id capacity and signal a hit event.
+	b3BitSet hitEventBitSet;
+
+	// Fast-path flag: true when this worker set at least one bit in hitEventBitSet this step.
+	bool hasHitEvents;
+
+	// 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.
+	b3BitSet enlargedSimBitSet;
+
+	// Used to put islands to sleep
+	b3BitSet awakeIslandBitSet;
+
+	// Per worker split island candidate
+	float splitSleepTime;
+	int splitIslandId;
+
+	// Profiling
+	int satCallCount;
+	int satCacheHitCount;
+	int distanceIterations;
+	int pushBackIterations;
+	int rootIterations;
+
+	// Number of contacts recycled this step (collide pass).
+	int recycledContactCount;
+
+	b3DebugPoint points[B3_DEBUG_POINT_CAPACITY];
+	int pointCount;
+
+	b3DebugLine lines[B3_DEBUG_LINE_CAPACITY];
+	int lineCount;
+
+	int manifoldCounts[B3_CONTACT_MANIFOLD_COUNT_BUCKETS];
+	// Prevent false sharing
+	char cacheLine[64];
+} b3TaskContext;
+
+b3DeclareArray( b3TaskContext );
+
+// The world struct manages all physics entities, dynamic simulation,  and asynchronous queries.
+// The world also contains efficient memory management facilities.
+typedef struct b3World
+{
+	b3Stack stack;
+	b3BroadPhase broadPhase;
+	b3ConstraintGraph constraintGraph;
+
+	// Manifold allocators have one allocator for each manifold count.
+	b3Array( b3BlockAllocator ) manifoldAllocators;
+	b3Mutex* manifoldAllocatorMutex;
+
+	// 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 b3Body.
+	b3IdPool 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.
+	b3Array( b3Body ) bodies;
+
+	// Provides free list for solver sets.
+	b3IdPool 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.
+	b3Array( b3SolverSet ) solverSets;
+
+	// Used to create stable ids for joints
+	b3IdPool jointIdPool;
+
+	// This is a sparse array that maps joint ids to the joint data stored in the constraint graph
+	// or in the solver sets.
+	b3Array( b3Joint ) joints;
+
+	// Used to create stable ids for contacts
+	b3IdPool contactIdPool;
+
+	// This is a sparse array that maps contact ids to the contact data stored in the constraint graph
+	// or in the solver sets.
+	b3Array( b3Contact ) contacts;
+
+	// Used to create stable ids for islands
+	b3IdPool islandIdPool;
+
+	// This is a sparse array that maps island ids to the island data stored in the solver sets.
+	b3Array( b3Island ) islands;
+
+	b3IdPool shapeIdPool;
+
+	// These are sparse arrays that point into the pools above
+	b3Array( b3Shape ) shapes;
+
+	// Reference counted store of shared hull data keyed by content. Shapes hold a
+	// pointer to the owned copy here. Opaque to avoid leaking the verstable map
+	// type into this header.
+	void* hullDatabase;
+
+	// Name cache for shape and body names. This works with recording.
+	b3NameCache names;
+
+	// This is a dense array of sensor data.
+	b3Array( b3Sensor ) sensors;
+
+	// Per thread storage
+	b3Array( b3TaskContext ) taskContexts;
+	b3Array( b3SensorTaskContext ) sensorTaskContexts;
+
+	b3Array( b3BodyMoveEvent ) bodyMoveEvents;
+	b3Array( b3SensorBeginTouchEvent ) sensorBeginEvents;
+	b3Array( b3ContactBeginTouchEvent ) contactBeginEvents;
+
+	// End events are double buffered so that the user doesn't need to flush events
+	b3Array( b3SensorEndTouchEvent ) sensorEndEvents[2];
+	b3Array( b3ContactEndTouchEvent ) contactEndEvents[2];
+	int endEventArrayIndex;
+
+	b3Array( b3ContactHitEvent ) contactHitEvents;
+	b3Array( b3JointEvent ) jointEvents;
+
+	// Used to track debug draw
+	b3BitSet debugBodySet;
+	b3BitSet debugJointSet;
+	b3BitSet debugContactSet;
+	b3BitSet debugIslandSet;
+	b3CreateDebugShapeCallback* createDebugShape;
+	b3DestroyDebugShapeCallback* destroyDebugShape;
+	void* userDebugShapeContext;
+
+	// 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;
+
+	b3Vec3 gravity;
+	float hitEventThreshold;
+	float restitutionThreshold;
+	float maxLinearSpeed;
+	float contactSpeed;
+	float contactHertz;
+	float contactDampingRatio;
+	float contactRecycleDistance;
+
+	b3FrictionCallback* frictionCallback;
+	b3RestitutionCallback* restitutionCallback;
+
+	uint16_t generation;
+
+	b3Profile profile;
+	int satCallCount;
+	int satCacheHitCount;
+	int manifoldCounts[B3_CONTACT_MANIFOLD_COUNT_BUCKETS];
+
+	b3Capacity maxCapacity;
+
+	b3PreSolveFcn* preSolveFcn;
+	void* preSolveContext;
+
+	b3CustomFilterFcn* customFilterFcn;
+	void* customFilterContext;
+
+	int workerCount;
+	b3EnqueueTaskCallback* enqueueTaskFcn;
+	b3FinishTaskCallback* finishTaskFcn;
+	void* userTaskContext;
+	void* userTreeTask;
+
+	struct b3Scheduler* scheduler;
+
+	void* userData;
+
+	// Non-NULL while a recording session is active. Set by b3World_StartRecording,
+	// cleared by b3World_StopRecording. Hooks in mutators check this before writing.
+	struct b3Recording* recording;
+
+	// latest inverse sub-step
+	float inv_h;
+
+	// latest inverse full-step
+	float inv_dt;
+
+	int activeTaskCount;
+	int taskCount;
+
+	uint16_t worldId;
+
+	bool enableSleep;
+
+	// This indicates there is a world write operation in progress. This is for debugging and
+	// not a real mutex. This should have minimal performance impact.
+	bool locked;
+	bool enableWarmStarting;
+	bool enableContinuous;
+	bool enableSpeculative;
+	bool inUse;
+} b3World;
+
+b3World* b3GetUnlockedWorldFromId( b3WorldId id );
+b3World* b3GetWorldFromId( b3WorldId id );
+
+b3World* b3GetUnlockedWorld( int index );
+b3World* b3GetWorld( int index );
+
+void b3ValidateConnectivity( b3World* world );
+void b3ValidateSolverSets( b3World* world );
+void b3ValidateContacts( b3World* world );
+
+// Register a hull in the world database, returning the owned shared copy. Identical hulls
+// share one copy with a reference count. The input may be freed after this call.
+const b3HullData* b3AddHullToDatabase( b3World* world, const b3HullData* src );
+
+// Like b3AddHullToDatabase but takes ownership of a heap hull: inserted directly on a miss,
+// freed on a hit. Avoids cloning data the caller already allocated.
+const b3HullData* b3AddOwnedHullToDatabase( b3World* world, b3HullData* owned );
+
+// Release a reference to a shared hull. The owned copy is freed when the count reaches zero.
+void b3RemoveHullFromDatabase( b3World* world, const b3HullData* data );
+
+static inline b3Manifold* b3AllocateManifolds( b3World* world, int count )
+{
+	if ( count == 0 )
+	{
+		return NULL;
+	}
+
+	int index = count - 1;
+
+	// Need lock because this is called from the parallel narrow phase
+	b3LockMutex( world->manifoldAllocatorMutex );
+	int currentCount = world->manifoldAllocators.count;
+	for ( int i = currentCount; i < count; ++i )
+	{
+		b3BlockAllocator allocator = b3CreateBlockAllocator( ( i + 1 ) * sizeof( b3Manifold ), 2 * B3_BLOCK_SIZE );
+		b3Array_Push( world->manifoldAllocators, allocator );
+	}
+
+	b3BlockAllocator* allocator = b3Array_Get( world->manifoldAllocators, index );
+	b3Manifold* manifolds = (b3Manifold*)b3AllocateElement( allocator );
+	b3UnlockMutex( world->manifoldAllocatorMutex );
+	memset( manifolds, 0, count * sizeof( b3Manifold ) );
+	return manifolds;
+}
+
+static inline void b3FreeManifolds( b3World* world, b3Manifold* manifolds, int count )
+{
+	if ( count == 0 )
+	{
+		return;
+	}
+
+	int index = count - 1;
+	b3LockMutex( world->manifoldAllocatorMutex );
+	b3BlockAllocator* allocator = b3Array_Get( world->manifoldAllocators, index );
+	b3FreeElement( allocator, manifolds );
+	b3UnlockMutex( world->manifoldAllocatorMutex );
+}
+

vendor/box3d/src/src/platform.h

@@ -0,0 +1,104 @@
+// SPDX-FileCopyrightText: 2025 Erin Catto
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include "core.h"
+
+#include <stdbool.h>
+#include <stdint.h>
+
+// Software prefetch hint. T0 brings the line into all cache levels.
+// On x86 MSVC exposes _mm_prefetch, ARM MSVC uses __prefetch instead.
+// clang/gcc provide __builtin_prefetch on every target.
+#if defined( B3_COMPILER_MSVC )
+#include <intrin.h>
+#if defined( B3_CPU_X86_X64 )
+#define b3Prefetch( addr ) _mm_prefetch( (const char*)( addr ), _MM_HINT_T0 )
+#else
+#define b3Prefetch( addr ) __prefetch( (const void*)( addr ) )
+#endif
+#elif defined( B3_COMPILER_CLANG ) || defined( B3_COMPILER_GCC )
+#define b3Prefetch( addr ) __builtin_prefetch( (const void*)( addr ), 0, 3 )
+#else
+#define b3Prefetch( addr ) ( (void)( addr ) )
+#endif
+
+static inline void b3AtomicStoreInt( b3AtomicInt* 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 b3AtomicLoadInt( b3AtomicInt* a )
+{
+#if defined( _MSC_VER ) && !defined( __clang__ )
+	int value = __iso_volatile_load32( (volatile __int32*)&a->value );
+#if defined( _M_ARM ) || defined( _M_ARM64 ) || defined( _M_ARM64EC )
+	__dmb( 0xB );
+#else
+	_ReadWriteBarrier();
+#endif
+	return value;
+#elif defined( __GNUC__ ) || defined( __clang__ )
+	return __atomic_load_n( &a->value, __ATOMIC_SEQ_CST );
+#else
+#error "Unsupported platform"
+#endif
+}
+
+static inline int b3AtomicFetchAddInt( b3AtomicInt* 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 b3AtomicCompareExchangeInt( b3AtomicInt* 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 b3AtomicStoreU32( b3AtomicU32* 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 b3AtomicLoadU32( b3AtomicU32* a )
+{
+#if defined( _MSC_VER ) && !defined( __clang__ )
+	uint32_t value = (uint32_t)__iso_volatile_load32( (volatile __int32*)&a->value );
+#if defined( _M_ARM ) || defined( _M_ARM64 ) || defined( _M_ARM64EC )
+	__dmb( 0xB );
+#else
+	_ReadWriteBarrier();
+#endif
+	return value;
+#elif defined( __GNUC__ ) || defined( __clang__ )
+	return __atomic_load_n( &a->value, __ATOMIC_SEQ_CST );
+#else
+#error "Unsupported platform"
+#endif
+}

vendor/box3d/src/src/prismatic_joint.c

@@ -0,0 +1,725 @@
+// SPDX-FileCopyrightText: 2025 Erin Catto
+// SPDX-License-Identifier: MIT
+
+#include "body.h"
+#include "core.h"
+#include "joint.h"
+#include "math_internal.h"
+#include "physics_world.h"
+#include "solver.h"
+#include "solver_set.h"
+#include "recording.h"
+
+// needed for dll export
+#include "box3d/box3d.h"
+
+// Linear constraint (point-to-line)
+// joint axis is along joint frame A local z-axis
+// perpX and perpY are world vectors fixed in A
+//
+// d = pB - pA = xB + rB - xA - rA
+// Cx = dot(perpX, d)
+// Cy = dot(perpY, d)
+
+// CdotX = dot(d, cross(wA, perpX)) + dot(perpX, vB + cross(wB, rB) - vA - cross(wA, rA))
+//      = -dot(perpX, vA) - dot(cross(d + rA, perpX), wA) + dot(perpX, vB) + dot(cross(rB, perpX), vB)
+// Jx = [-perpX, -cross(d + rA, perpX), perpX, cross(rB, perpX)]
+// similar for perpY
+//
+// Simplification dropping dot(d, cross(wA, perpX)) (todo needs testing)
+// CdotXs = dot(perpX, vB + cross(wB, rB) - vA - cross(wA, rA))
+// Jxs = [-perpX, -cross(rA, perpX), perpX, cross(rB, perpX)]
+
+// Motor/limit/spring linear constraint
+// axis is the world joint axis fixed in A
+
+// C = dot(axis, d)
+// Cdot = dot(d, cross(wA, axis)) + dot(axis, vB + cross(wB, rB) - vA - cross(wA, rA))
+// Cdot = -dot(axis, vA) - dot(cross(d + rA, axis), wA) + dot(axis, vB) + dot(cross(rB, axis), vB)
+// J = [-axis -cross(d + rA, axis) axis cross(rB, axis)]
+//
+// Simplified (todo needs testing)
+// Cdot = -dot(axis, vA) - dot(cross(rA, axis), wA) + dot(axis, vB) + dot(cross(rB, axis), vB)
+// J = [-axis -cross(rA, axis) axis cross(rB, axis)]
+
+// 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
+
+void b3PrismaticJoint_EnableLimit( b3JointId jointId, bool enableLimit )
+{
+	b3World* world = b3GetWorld( jointId.world0 );
+	B3_REC( world, PrismaticJointEnableLimit, jointId, enableLimit );
+	b3JointSim* base = b3GetJointSimCheckType( jointId, b3_prismaticJoint );
+	if ( enableLimit != base->prismaticJoint.enableLimit )
+	{
+		base->prismaticJoint.lowerImpulse = 0.0f;
+		base->prismaticJoint.upperImpulse = 0.0f;
+	}
+	base->prismaticJoint.enableLimit = enableLimit;
+}
+
+bool b3PrismaticJoint_IsLimitEnabled( b3JointId jointId )
+{
+	b3JointSim* base = b3GetJointSimCheckType( jointId, b3_prismaticJoint );
+	return base->prismaticJoint.enableLimit;
+}
+
+float b3PrismaticJoint_GetLowerLimit( b3JointId jointId )
+{
+	b3JointSim* base = b3GetJointSimCheckType( jointId, b3_prismaticJoint );
+	return base->prismaticJoint.lowerTranslation;
+}
+
+float b3PrismaticJoint_GetUpperLimit( b3JointId jointId )
+{
+	b3JointSim* base = b3GetJointSimCheckType( jointId, b3_prismaticJoint );
+	return base->prismaticJoint.upperTranslation;
+}
+
+void b3PrismaticJoint_SetLimits( b3JointId jointId, float lower, float upper )
+{
+	B3_ASSERT( b3IsValidFloat( lower ) && b3IsValidFloat( upper ) );
+	b3World* world = b3GetWorld( jointId.world0 );
+	B3_REC( world, PrismaticJointSetLimits, jointId, lower, upper );
+	float lowerAngle = b3MinFloat( lower, upper );
+	float upperAngle = b3MaxFloat( lower, upper );
+
+	b3JointSim* base = b3GetJointSimCheckType( jointId, b3_prismaticJoint );
+	base->prismaticJoint.lowerTranslation = lowerAngle;
+	base->prismaticJoint.upperTranslation = upperAngle;
+}
+
+float b3PrismaticJoint_GetTranslation( b3JointId jointId )
+{
+	b3World* world = b3GetWorld( jointId.world0 );
+	b3JointSim* base = b3GetJointSimCheckType( jointId, b3_prismaticJoint );
+	b3WorldTransform transformA = b3GetBodyTransform( world, base->bodyIdA );
+	b3WorldTransform transformB = b3GetBodyTransform( world, base->bodyIdB );
+
+	b3Vec3 jointAxis = b3RotateVector( base->localFrameA.q, b3Vec3_axisX );
+	jointAxis = b3RotateVector( transformA.q, jointAxis );
+
+	b3Vec3 anchorA = b3RotateVector( transformA.q, base->localFrameA.p );
+	b3Vec3 anchorB = b3RotateVector( transformB.q, base->localFrameB.p );
+	b3Vec3 d = b3Add( b3SubPos( transformB.p, transformA.p ), b3Sub( anchorB, anchorA ) );
+	float translation = b3Dot( d, jointAxis );
+	return translation;
+}
+
+void b3PrismaticJoint_EnableSpring( b3JointId jointId, bool enableSpring )
+{
+	b3World* world = b3GetWorld( jointId.world0 );
+	B3_REC( world, PrismaticJointEnableSpring, jointId, enableSpring );
+	b3JointSim* base = b3GetJointSimCheckType( jointId, b3_prismaticJoint );
+	if ( enableSpring != base->prismaticJoint.enableSpring )
+	{
+		base->prismaticJoint.springImpulse = 0.0f;
+	}
+	base->prismaticJoint.enableSpring = enableSpring;
+}
+
+bool b3PrismaticJoint_IsSpringEnabled( b3JointId jointId )
+{
+	b3JointSim* base = b3GetJointSimCheckType( jointId, b3_prismaticJoint );
+	return base->prismaticJoint.enableSpring;
+}
+
+void b3PrismaticJoint_SetTargetTranslation( b3JointId jointId, float targetTranslation )
+{
+	B3_ASSERT( b3IsValidFloat( targetTranslation ) );
+	b3World* world = b3GetWorld( jointId.world0 );
+	B3_REC( world, PrismaticJointSetTargetTranslation, jointId, targetTranslation );
+	b3JointSim* base = b3GetJointSimCheckType( jointId, b3_prismaticJoint );
+	base->prismaticJoint.targetTranslation = targetTranslation;
+}
+
+float b3PrismaticJoint_GetTargetTranslation( b3JointId jointId )
+{
+	b3JointSim* base = b3GetJointSimCheckType( jointId, b3_prismaticJoint );
+	return base->prismaticJoint.targetTranslation;
+}
+
+void b3PrismaticJoint_SetSpringHertz( b3JointId jointId, float hertz )
+{
+	B3_ASSERT( b3IsValidFloat( hertz ) && hertz >= 0.0f );
+	b3World* world = b3GetWorld( jointId.world0 );
+	B3_REC( world, PrismaticJointSetSpringHertz, jointId, hertz );
+	b3JointSim* base = b3GetJointSimCheckType( jointId, b3_prismaticJoint );
+	base->prismaticJoint.hertz = hertz;
+}
+
+float b3PrismaticJoint_GetSpringHertz( b3JointId jointId )
+{
+	b3JointSim* base = b3GetJointSimCheckType( jointId, b3_prismaticJoint );
+	return base->prismaticJoint.hertz;
+}
+
+void b3PrismaticJoint_SetSpringDampingRatio( b3JointId jointId, float dampingRatio )
+{
+	B3_ASSERT( b3IsValidFloat( dampingRatio ) && dampingRatio >= 0.0f );
+	b3World* world = b3GetWorld( jointId.world0 );
+	B3_REC( world, PrismaticJointSetSpringDampingRatio, jointId, dampingRatio );
+	b3JointSim* base = b3GetJointSimCheckType( jointId, b3_prismaticJoint );
+	base->prismaticJoint.dampingRatio = dampingRatio;
+}
+
+float b3PrismaticJoint_GetSpringDampingRatio( b3JointId jointId )
+{
+	b3JointSim* base = b3GetJointSimCheckType( jointId, b3_prismaticJoint );
+	return base->prismaticJoint.dampingRatio;
+}
+
+void b3PrismaticJoint_EnableMotor( b3JointId jointId, bool enableMotor )
+{
+	b3World* world = b3GetWorld( jointId.world0 );
+	B3_REC( world, PrismaticJointEnableMotor, jointId, enableMotor );
+	b3JointSim* base = b3GetJointSimCheckType( jointId, b3_prismaticJoint );
+	if ( enableMotor != base->prismaticJoint.enableMotor )
+	{
+		base->prismaticJoint.motorImpulse = 0.0f;
+	}
+	base->prismaticJoint.enableMotor = enableMotor;
+}
+
+bool b3PrismaticJoint_IsMotorEnabled( b3JointId jointId )
+{
+	b3JointSim* base = b3GetJointSimCheckType( jointId, b3_prismaticJoint );
+	return base->prismaticJoint.enableMotor;
+}
+
+void b3PrismaticJoint_SetMotorSpeed( b3JointId jointId, float motorSpeed )
+{
+	B3_ASSERT( b3IsValidFloat( motorSpeed ) );
+	b3World* world = b3GetWorld( jointId.world0 );
+	B3_REC( world, PrismaticJointSetMotorSpeed, jointId, motorSpeed );
+	b3JointSim* base = b3GetJointSimCheckType( jointId, b3_prismaticJoint );
+	base->prismaticJoint.motorSpeed = motorSpeed;
+}
+
+float b3PrismaticJoint_GetMotorSpeed( b3JointId jointId )
+{
+	b3JointSim* base = b3GetJointSimCheckType( jointId, b3_prismaticJoint );
+	return base->prismaticJoint.motorSpeed;
+}
+
+void b3PrismaticJoint_SetMaxMotorForce( b3JointId jointId, float maxForce )
+{
+	B3_ASSERT( b3IsValidFloat( maxForce ) && maxForce >= 0.0f );
+	b3World* world = b3GetWorld( jointId.world0 );
+	B3_REC( world, PrismaticJointSetMaxMotorForce, jointId, maxForce );
+	b3JointSim* base = b3GetJointSimCheckType( jointId, b3_prismaticJoint );
+	base->prismaticJoint.maxMotorForce = maxForce;
+}
+
+float b3PrismaticJoint_GetMaxMotorForce( b3JointId jointId )
+{
+	b3JointSim* base = b3GetJointSimCheckType( jointId, b3_prismaticJoint );
+	return base->prismaticJoint.maxMotorForce;
+}
+
+float b3PrismaticJoint_GetMotorForce( b3JointId jointId )
+{
+	b3World* world = b3GetWorld( jointId.world0 );
+	b3JointSim* base = b3GetJointSimCheckType( jointId, b3_prismaticJoint );
+	return world->inv_h * base->prismaticJoint.motorImpulse;
+}
+
+float b3PrismaticJoint_GetSpeed( b3JointId jointId )
+{
+	b3World* world = b3GetWorld( jointId.world0 );
+	b3JointSim* base = b3GetJointSimCheckType( jointId, b3_prismaticJoint );
+
+	b3Body* bodyA = b3Array_Get( world->bodies, base->bodyIdA );
+	b3Body* bodyB = b3Array_Get( world->bodies, base->bodyIdB );
+	b3BodySim* bodySimA = b3GetBodySim( world, bodyA );
+	b3BodySim* bodySimB = b3GetBodySim( world, bodyB );
+	b3BodyState* stateA = b3GetBodyState( world, bodyA );
+	b3BodyState* stateB = b3GetBodyState( world, bodyB );
+
+	b3Quat qA = bodySimA->transform.q;
+	b3Quat qB = bodySimB->transform.q;
+
+	b3Vec3 axisA = b3RotateVector( qA, b3RotateVector( base->localFrameA.q, b3Vec3_axisX ) );
+	b3Vec3 rA = b3RotateVector( qA, b3Sub( base->localFrameA.p, bodySimA->localCenter ) );
+	b3Vec3 rB = b3RotateVector( qB, b3Sub( base->localFrameB.p, bodySimB->localCenter ) );
+
+	// Difference the centers in double so the speed stays exact far from the origin.
+	b3Vec3 d = b3Add( b3SubPos( bodySimB->center, bodySimA->center ), b3Sub( rB, rA ) );
+
+	b3Vec3 vA = stateA ? stateA->linearVelocity : b3Vec3_zero;
+	b3Vec3 vB = stateB ? stateB->linearVelocity : b3Vec3_zero;
+	b3Vec3 wA = stateA ? stateA->angularVelocity : b3Vec3_zero;
+	b3Vec3 wB = stateB ? stateB->angularVelocity : b3Vec3_zero;
+
+	b3Vec3 vRel = b3Sub( b3Add( vB, b3Cross( wB, rB ) ), b3Add( vA, b3Cross( wA, rA ) ) );
+
+	// The axis moves with body A, so account for its rotation.
+	float speed = b3Dot( d, b3Cross( wA, axisA ) ) + b3Dot( axisA, vRel );
+	return speed;
+}
+
+b3Vec3 b3GetPrismaticJointForce( b3World* world, b3JointSim* base )
+{
+	b3WorldTransform transformA = b3GetBodyTransform( world, base->bodyIdA );
+	b3PrismaticJoint* joint = &base->prismaticJoint;
+
+	// impulse in joint space
+	b3Vec3 impulse = {
+		joint->perpImpulse.x,
+		joint->perpImpulse.y,
+		joint->motorImpulse + joint->lowerImpulse + joint->upperImpulse + joint->springImpulse,
+	};
+
+	// convert impulse to force
+	b3Vec3 force = b3MulSV( world->inv_h, impulse );
+
+	// convert to body space
+	force = b3RotateVector( base->localFrameA.q, force );
+
+	// convert to world space
+	force = b3RotateVector( transformA.q, force );
+	return force;
+}
+
+b3Vec3 b3GetPrismaticJointTorque( b3World* world, b3JointSim* base )
+{
+	b3WorldTransform transformA = b3GetBodyTransform( world, base->bodyIdA );
+	b3PrismaticJoint* joint = &base->prismaticJoint;
+
+	b3Vec3 torque = b3MulSV( world->inv_h, joint->angularImpulse );
+	torque = b3RotateVector( base->localFrameA.q, torque );
+	torque = b3RotateVector( transformA.q, torque );
+	return torque;
+}
+
+void b3PreparePrismaticJoint( b3JointSim* base, b3StepContext* context )
+{
+	B3_ASSERT( base->type == b3_prismaticJoint );
+
+	b3World* world = context->world;
+
+	b3Body* bodyA = b3Array_Get( world->bodies, base->bodyIdA  );
+	b3Body* bodyB = b3Array_Get( world->bodies, base->bodyIdB  );
+
+	B3_ASSERT( bodyB->setIndex == b3_awakeSet );
+	b3SolverSet* setA = b3Array_Get( world->solverSets, bodyA->setIndex  );
+	b3SolverSet* setB = b3Array_Get( world->solverSets, bodyB->setIndex  );
+
+	int localIndexA = bodyA->localIndex;
+	int localIndexB = bodyB->localIndex;
+
+	b3BodySim* bodySimA = b3Array_Get( setA->bodySims, localIndexA  );
+	b3BodySim* bodySimB = b3Array_Get( setB->bodySims, localIndexB  );
+
+	base->invMassA = bodySimA->invMass;
+	base->invMassB = bodySimB->invMass;
+	base->invIA = bodySimA->invInertiaWorld;
+	base->invIB = bodySimB->invInertiaWorld;
+
+	b3Matrix3 invInertiaSum = b3AddMM( base->invIA, base->invIB );
+	base->fixedRotation = b3Det( invInertiaSum ) < 1000.0f * FLT_MIN;
+
+	b3PrismaticJoint* joint = &base->prismaticJoint;
+	joint->indexA = bodyA->setIndex == b3_awakeSet ? localIndexA : B3_NULL_INDEX;
+	joint->indexB = bodyB->setIndex == b3_awakeSet ? localIndexB : B3_NULL_INDEX;
+
+	// Compute joint anchor frames with world space rotation, relative to center of mass
+	joint->frameA.q = b3MulQuat( bodySimA->transform.q, base->localFrameA.q );
+	joint->frameA.p = b3RotateVector( bodySimA->transform.q, b3Sub( base->localFrameA.p, bodySimA->localCenter ) );
+	joint->frameB.q = b3MulQuat( bodySimB->transform.q, base->localFrameB.q );
+	joint->frameB.p = b3RotateVector( bodySimB->transform.q, b3Sub( base->localFrameB.p, bodySimB->localCenter ) );
+
+	joint->deltaCenter = b3SubPos( bodySimB->center, bodySimA->center );
+	joint->rotationMass = b3InvertMatrix( invInertiaSum );
+
+	// Initial joint axes in world space
+	b3Matrix3 matrixA = b3MakeMatrixFromQuat( joint->frameA.q );
+	joint->jointAxis = matrixA.cx;
+	joint->perpAxisY = matrixA.cy;
+	joint->perpAxisZ = matrixA.cz;
+
+	joint->springSoftness = b3MakeSoft( joint->hertz, joint->dampingRatio, context->h );
+
+	if ( context->enableWarmStarting == false )
+	{
+		joint->perpImpulse = (b3Vec2){ 0.0f, 0.0f };
+		joint->angularImpulse = (b3Vec3){ 0.0f, 0.0f, 0.0f };
+		joint->motorImpulse = 0.0f;
+		joint->springImpulse = 0.0f;
+		joint->lowerImpulse = 0.0f;
+		joint->upperImpulse = 0.0f;
+	}
+}
+
+void b3WarmStartPrismaticJoint( b3JointSim* base, b3StepContext* context )
+{
+	B3_ASSERT( base->type == b3_prismaticJoint );
+
+	float mA = base->invMassA;
+	float mB = base->invMassB;
+	b3Matrix3 iA = base->invIA;
+	b3Matrix3 iB = base->invIB;
+
+	// dummy state for static bodies
+	b3BodyState dummyState = b3_identityBodyState;
+
+	b3PrismaticJoint* joint = &base->prismaticJoint;
+
+	b3BodyState* stateA = joint->indexA == B3_NULL_INDEX ? &dummyState : context->states + joint->indexA;
+	b3BodyState* stateB = joint->indexB == B3_NULL_INDEX ? &dummyState : context->states + joint->indexB;
+
+	// todo make this code and the wheel joint more similar
+
+	b3Vec3 rA = b3RotateVector( stateA->deltaRotation, joint->frameA.p );
+	b3Vec3 rB = b3RotateVector( stateB->deltaRotation, joint->frameB.p );
+	b3Vec3 d = b3Add( b3Add( b3Sub( stateB->deltaPosition, stateA->deltaPosition ), joint->deltaCenter ), b3Sub( rB, rA ) );
+	b3Vec3 jointAxis = b3RotateVector( stateA->deltaRotation, joint->jointAxis );
+	b3Vec3 sAx = b3Cross( b3Add( rA, d ), jointAxis );
+	b3Vec3 sBx = b3Cross( rB, jointAxis );
+
+	b3Vec3 perpY = b3RotateVector( stateA->deltaRotation, joint->perpAxisY );
+	b3Vec3 perpZ = b3RotateVector( stateA->deltaRotation, joint->perpAxisZ );
+	b3Vec3 sAy = b3Cross( b3Add( rA, d ), perpY );
+	b3Vec3 sBy = b3Cross( rB, perpY );
+	b3Vec3 sAz = b3Cross( b3Add( rA, d ), perpZ );
+	b3Vec3 sBz = b3Cross( rB, perpZ );
+
+	float axialImpulse = joint->springImpulse + joint->motorImpulse + joint->lowerImpulse - joint->upperImpulse;
+	b3Vec2 perpImpulse = joint->perpImpulse;
+
+	b3Vec3 P = b3Blend3( axialImpulse, jointAxis, perpImpulse.x, perpY, perpImpulse.y, perpZ );
+	b3Vec3 LA = b3Add( b3Blend3( axialImpulse, sAx, perpImpulse.x, sAy, perpImpulse.y, sAz ), joint->angularImpulse );
+	b3Vec3 LB = b3Add( b3Blend3( axialImpulse, sBx, perpImpulse.x, sBy, perpImpulse.y, sBz ), joint->angularImpulse );
+
+	b3Vec3 vA = stateA->linearVelocity;
+	b3Vec3 wA = stateA->angularVelocity;
+	b3Vec3 vB = stateB->linearVelocity;
+	b3Vec3 wB = stateB->angularVelocity;
+	vA = b3MulSub( vA, mA, P );
+	wA = b3Sub( wA, b3MulMV( iA, LA ) );
+	vB = b3MulAdd( vB, mB, P );
+	wB = b3Add( wB, b3MulMV( iB, LB ) );
+
+	if ( stateA->flags & b3_dynamicFlag )
+	{
+		stateA->linearVelocity = vA;
+		stateA->angularVelocity = wA;
+	}
+
+	if ( stateB->flags & b3_dynamicFlag )
+	{
+		stateB->linearVelocity = vB;
+		stateB->angularVelocity = wB;
+	}
+}
+
+void b3SolvePrismaticJoint( b3JointSim* base, b3StepContext* context, bool useBias )
+{
+	float mA = base->invMassA;
+	float mB = base->invMassB;
+	b3Matrix3 iA = base->invIA;
+	b3Matrix3 iB = base->invIB;
+
+	// dummy state for static bodies
+	b3BodyState dummyState = b3_identityBodyState;
+
+	b3PrismaticJoint* joint = &base->prismaticJoint;
+	b3BodyState* stateA = joint->indexA == B3_NULL_INDEX ? &dummyState : context->states + joint->indexA;
+	b3BodyState* stateB = joint->indexB == B3_NULL_INDEX ? &dummyState : context->states + joint->indexB;
+
+	b3Vec3 vA = stateA->linearVelocity;
+	b3Vec3 wA = stateA->angularVelocity;
+	b3Vec3 vB = stateB->linearVelocity;
+	b3Vec3 wB = stateB->angularVelocity;
+
+	bool fixedRotation = base->fixedRotation;
+	b3Vec3 rA = b3RotateVector( stateA->deltaRotation, joint->frameA.p );
+	b3Vec3 rB = b3RotateVector( stateB->deltaRotation, joint->frameB.p );
+
+	b3Vec3 dcA = stateA->deltaPosition;
+	b3Vec3 dcB = stateB->deltaPosition;
+	b3Vec3 d = b3Add( b3Add( b3Sub( dcB, dcA ), joint->deltaCenter ), b3Sub( rB, rA ) );
+
+	b3Vec3 jointAxis = b3RotateVector( stateA->deltaRotation, joint->jointAxis );
+	b3Vec3 sAx = b3Cross( b3Add( rA, d ), jointAxis );
+	b3Vec3 sBx = b3Cross( rB, jointAxis );
+	float jointTranslation = b3Dot( d, jointAxis );
+	float targetTranslation = joint->targetTranslation;
+
+	// The axial effective mass must be fresh to avoid divergence when the joint is stressed
+	float ka = mA + mB + b3Dot( sAx, b3MulMV( iA, sAx ) ) + b3Dot( sBx, b3MulMV( iB, sBx ) );
+	float axialMass = ka > 0.0f ? 1.0f / ka : 0.0f;
+
+	// Solve spring
+	if ( joint->enableSpring && fixedRotation == false )
+	{
+		// Get the substep relative rotation
+		float c = jointTranslation - targetTranslation;
+
+		float bias = joint->springSoftness.biasRate * c;
+		float massScale = joint->springSoftness.massScale;
+		float impulseScale = joint->springSoftness.impulseScale;
+
+		b3Vec3 vRel = b3Sub( b3Sub( b3Add( vB, b3Cross( wB, rB ) ), vA ), b3Cross( wA, b3Add( rA, d ) ) );
+		float cdot = b3Dot( vRel, jointAxis );
+		float deltaImpulse = -massScale * axialMass * ( cdot + bias ) - impulseScale * joint->springImpulse;
+		joint->springImpulse += deltaImpulse;
+
+		b3Vec3 P = b3MulSV( deltaImpulse, jointAxis );
+		b3Vec3 LA = b3MulSV( deltaImpulse, sAx );
+		b3Vec3 LB = b3MulSV( deltaImpulse, sBx );
+
+		vA = b3MulSub( vA, mA, P );
+		wA = b3Sub( wA, b3MulMV( iA, LA ) );
+		vB = b3MulAdd( vB, mB, P );
+		wB = b3Add( wB, b3MulMV( iB, LB ) );
+	}
+
+	if ( joint->enableMotor && fixedRotation == false )
+	{
+		b3Vec3 vRel = b3Sub( b3Sub( b3Add( vB, b3Cross( wB, rB ) ), vA ), b3Cross( wA, b3Add( rA, d ) ) );
+		float cdot = b3Dot( vRel, jointAxis ) - joint->motorSpeed;
+
+		float deltaImpulse = -axialMass * cdot;
+		float newImpulse = joint->motorImpulse + deltaImpulse;
+		float maxImpulse = joint->maxMotorForce * context->h;
+		newImpulse = b3ClampFloat( newImpulse, -maxImpulse, maxImpulse );
+		deltaImpulse = newImpulse - joint->motorImpulse;
+		joint->motorImpulse = newImpulse;
+
+		b3Vec3 P = b3MulSV( deltaImpulse, jointAxis );
+		b3Vec3 LA = b3MulSV( deltaImpulse, sAx );
+		b3Vec3 LB = b3MulSV( deltaImpulse, sBx );
+
+		vA = b3MulSub( vA, mA, P );
+		wA = b3Sub( wA, b3MulMV( iA, LA ) );
+		vB = b3MulAdd( vB, mB, P );
+		wB = b3Add( wB, b3MulMV( iB, LB ) );
+	}
+
+	if ( joint->enableLimit && fixedRotation == false )
+	{
+		float speculativeDistance = 0.25f * ( joint->upperTranslation - joint->lowerTranslation );
+
+		// Lower limit
+		{
+			float C = jointTranslation - joint->lowerTranslation;
+
+			if ( C < speculativeDistance )
+			{
+				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 = base->constraintSoftness.biasRate * C;
+					massScale = base->constraintSoftness.massScale;
+					impulseScale = base->constraintSoftness.impulseScale;
+				}
+
+				b3Vec3 vRel = b3Sub( b3Sub( b3Add( vB, b3Cross( wB, rB ) ), vA ), b3Cross( wA, b3Add( rA, d ) ) );
+				float cdot = b3Dot( vRel, jointAxis );
+				float oldImpulse = joint->lowerImpulse;
+				float deltaImpulse = -massScale * axialMass * ( cdot + bias ) - impulseScale * oldImpulse;
+				joint->lowerImpulse = b3MaxFloat( oldImpulse + deltaImpulse, 0.0f );
+				deltaImpulse = joint->lowerImpulse - oldImpulse;
+
+				b3Vec3 P = b3MulSV( deltaImpulse, jointAxis );
+				b3Vec3 LA = b3MulSV( deltaImpulse, sAx );
+				b3Vec3 LB = b3MulSV( deltaImpulse, sBx );
+
+				vA = b3MulSub( vA, mA, P );
+				wA = b3Sub( wA, b3MulMV( iA, LA ) );
+				vB = b3MulAdd( vB, mB, P );
+				wB = b3Add( wB, b3MulMV( iB, LB ) );
+			}
+			else
+			{
+				joint->lowerImpulse = 0.0f;
+			}
+		}
+
+		// Upper limit
+		{
+			float C = joint->upperTranslation - jointTranslation;
+
+			if ( C < speculativeDistance )
+			{
+				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 = base->constraintSoftness.biasRate * C;
+					massScale = base->constraintSoftness.massScale;
+					impulseScale = base->constraintSoftness.impulseScale;
+				}
+
+				// sign flipped on Cdot
+				b3Vec3 vRel = b3Sub( b3Sub( b3Add( vB, b3Cross( wB, rB ) ), vA ), b3Cross( wA, b3Add( rA, d ) ) );
+				float cdot = -b3Dot( vRel, jointAxis );
+				float oldImpulse = joint->upperImpulse;
+				float deltaImpulse = -massScale * axialMass * ( cdot + bias ) - impulseScale * oldImpulse;
+				joint->upperImpulse = b3MaxFloat( oldImpulse + deltaImpulse, 0.0f );
+
+				// sign flipped on applied impulse
+				float negDeltaImpulse = oldImpulse - joint->upperImpulse;
+				b3Vec3 P = b3MulSV( negDeltaImpulse, jointAxis );
+				b3Vec3 LA = b3MulSV( negDeltaImpulse, sAx );
+				b3Vec3 LB = b3MulSV( negDeltaImpulse, sBx );
+
+				vA = b3MulSub( vA, mA, P );
+				wA = b3Sub( wA, b3MulMV( iA, LA ) );
+				vB = b3MulAdd( vB, mB, P );
+				wB = b3Add( wB, b3MulMV( iB, LB ) );
+			}
+			else
+			{
+				joint->upperImpulse = 0.0f;
+			}
+		}
+	}
+
+	// Rotation constraint
+	if ( fixedRotation == false )
+	{
+		b3Vec3 bias = { 0.0f, 0.0f, 0.0f };
+		float massScale = 1.0f;
+		float impulseScale = 0.0f;
+
+		if ( useBias )
+		{
+			b3Quat quatA = b3MulQuat( stateA->deltaRotation, joint->frameA.q );
+			b3Quat quatB = b3MulQuat( stateB->deltaRotation, joint->frameB.q );
+
+			b3Quat relQ = b3InvMulQuat( quatA, quatB );
+			b3Quat targetQuat = b3Quat_identity;
+			b3Vec3 deltaRotation = b3DeltaQuatToRotation( relQ, targetQuat );
+			b3Vec3 c = b3Neg( b3RotateVector( quatA, deltaRotation ) );
+
+			bias = b3MulSV( base->constraintSoftness.biasRate, c );
+			massScale = base->constraintSoftness.massScale;
+			impulseScale = base->constraintSoftness.impulseScale;
+		}
+
+		b3Vec3 cdot = b3Sub( wB, wA );
+		b3Vec3 impulse = b3Sub(
+			b3MulSV( -massScale, b3MulMV( joint->rotationMass, b3Add( cdot, bias ) ) ),
+			b3MulSV( impulseScale, joint->angularImpulse ) );
+		joint->angularImpulse = b3Add( joint->angularImpulse, impulse );
+
+		wA = b3Sub( wA, b3MulMV( iA, impulse ) );
+		wB = b3Add( wB, b3MulMV( iB, impulse ) );
+	}
+
+	// Solve point-to-line constraint
+	{
+		b3Vec3 perpY = b3RotateVector( stateA->deltaRotation, joint->perpAxisY );
+		b3Vec3 perpZ = b3RotateVector( stateA->deltaRotation, joint->perpAxisZ );
+
+		b3Vec2 bias = { 0.0f, 0.0f };
+		float massScale = 1.0f;
+		float impulseScale = 0.0f;
+		if ( useBias )
+		{
+			b3Vec2 c = { b3Dot( perpY, d ), b3Dot( perpZ, d ) };
+			bias = b3MulSV2( base->constraintSoftness.biasRate, c );
+			massScale = base->constraintSoftness.massScale;
+			impulseScale = base->constraintSoftness.impulseScale;
+		}
+
+		b3Vec3 vRel = b3Sub( b3Sub( b3Add( vB, b3Cross( wB, rB ) ), vA ), b3Cross( wA, b3Add( rA, d ) ) );
+		b3Vec2 cdot = { b3Dot( perpY, vRel ), b3Dot( perpZ, vRel ) };
+
+		// K = [(1/mA + 1/mB) * eye(2) - skew(rA) * invIA * skew(rA) - skew(rB) * invIB * skew(rB)]
+		// Jx = [-perpX, -cross(d + rA, perpX), perpX, cross(rB, perpX)]
+		b3Vec3 sAy = b3Cross( b3Add( rA, d ), perpY );
+		b3Vec3 sBy = b3Cross( rB, perpY );
+		b3Vec3 sAz = b3Cross( b3Add( rA, d ), perpZ );
+		b3Vec3 sBz = b3Cross( rB, perpZ );
+
+		float kyy = mA + mB + b3Dot( sAy, b3MulMV( iA, sAy ) ) + b3Dot( sBy, b3MulMV( iB, sBy ) );
+		float kyz = b3Dot( sAy, b3MulMV( iA, sAz ) ) + b3Dot( sBy, b3MulMV( iB, sBz ) );
+		float kzz = mA + mB + b3Dot( sAz, b3MulMV( iA, sAz ) ) + b3Dot( sBz, b3MulMV( iB, sBz ) );
+
+		b3Matrix2 K = { { kyy, kyz }, { kyz, kzz } };
+
+		b3Vec2 oldImpulse = joint->perpImpulse;
+		b3Vec2 sol = b3Solve2( K, b3Add2( cdot, bias ) );
+		b3Vec2 deltaImpulse = b3Sub2( b3MulSV2( -massScale, sol ), b3MulSV2( impulseScale, oldImpulse ) );
+		joint->perpImpulse = b3Add2( oldImpulse, deltaImpulse );
+
+		b3Vec3 P = b3Blend2( deltaImpulse.x, perpY, deltaImpulse.y, perpZ );
+
+		vA = b3MulSub( vA, mA, P );
+		wA = b3Sub( wA, b3MulMV( iA, b3Blend2( deltaImpulse.x, sAy, deltaImpulse.y, sAz ) ) );
+		vB = b3MulAdd( vB, mB, P );
+		wB = b3Add( wB, b3MulMV( iB, b3Blend2( deltaImpulse.x, sBy, deltaImpulse.y, sBz ) ) );
+	}
+
+	B3_ASSERT( b3IsValidVec3( vA ) );
+	B3_ASSERT( b3IsValidVec3( wA ) );
+	B3_ASSERT( b3IsValidVec3( vB ) );
+	B3_ASSERT( b3IsValidVec3( wB ) );
+
+	if ( stateA->flags & b3_dynamicFlag )
+	{
+		stateA->linearVelocity = vA;
+		stateA->angularVelocity = wA;
+	}
+
+	if ( stateB->flags & b3_dynamicFlag )
+	{
+		stateB->linearVelocity = vB;
+		stateB->angularVelocity = wB;
+	}
+}
+
+void b3DrawPrismaticJoint( b3DebugDraw* draw, b3JointSim* base, b3WorldTransform transformA, b3WorldTransform transformB, float scale )
+{
+	b3WorldTransform frameA = b3MulWorldTransforms( transformA, base->localFrameA );
+	b3WorldTransform frameB = b3MulWorldTransforms( transformB, base->localFrameB );
+
+	b3Matrix3 R = b3MakeMatrixFromQuat( frameA.q );
+	b3Vec3 axis = R.cx;
+	b3Vec3 perpY = R.cy;
+	b3Vec3 perpZ = R.cz;
+
+	float s = 0.2f * scale;
+	draw->DrawSegmentFcn( frameA.p, b3OffsetPos( frameA.p, b3MulSV( s, perpY ) ), b3_colorGreen, draw->context );
+	draw->DrawSegmentFcn( frameA.p, b3OffsetPos( frameA.p, b3MulSV( s, perpZ ) ), b3_colorBlue, draw->context );
+
+	b3PrismaticJoint* joint = &base->prismaticJoint;
+	if ( joint->enableLimit )
+	{
+		b3Pos p1 = b3OffsetPos( frameA.p, b3MulSV( joint->lowerTranslation, axis ) );
+		b3Pos p2 = b3OffsetPos( frameA.p, b3MulSV( joint->upperTranslation, axis ) );
+		draw->DrawSegmentFcn( p1, p2, b3_colorOrange, draw->context );
+		draw->DrawPointFcn( p1, 10.0f, b3_colorGreen, draw->context );
+		draw->DrawPointFcn( p2, 10.0f, b3_colorRed, draw->context );
+	}
+	else
+	{
+		b3Pos p1 = b3OffsetPos( frameA.p, b3MulSV( -0.5f * scale, axis ) );
+		b3Pos p2 = b3OffsetPos( frameA.p, b3MulSV( 0.5f * scale, axis ) );
+		draw->DrawSegmentFcn( p1, p2, b3_colorOrange, draw->context );
+	}
+
+	draw->DrawPointFcn( frameB.p, 8.0f, b3_colorViolet, draw->context );
+}

vendor/box3d/src/src/qsort.h

@@ -0,0 +1,186 @@
+/*
+ * Copyright (c) 2013, 2017 Alexey Tourbin
+ *
+ * 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.
+ */
+
+/*
+ * This is a traditional Quicksort implementation which mostly follows
+ * [Sedgewick 1978].  Sorting is performed entirely on array indices,
+ * while actual access to the array elements is abstracted out with the
+ * user-defined `LESS` and `SWAP` primitives.
+ *
+ * Synopsis:
+ *	QSORT(N, LESS, SWAP);
+ * where
+ *	N - the number of elements in A[];
+ *	LESS(i, j) - compares A[i] to A[j];
+ *	SWAP(i, j) - exchanges A[i] with A[j].
+ */
+
+#ifndef QSORT_H
+#define QSORT_H
+
+/* Sort 3 elements. */
+#define Q_SORT3(q_a1, q_a2, q_a3, Q_LESS, Q_SWAP) \
+do {					\
+    if (Q_LESS(q_a2, q_a1)) {		\
+	if (Q_LESS(q_a3, q_a2))		\
+	    Q_SWAP(q_a1, q_a3);		\
+	else {				\
+	    Q_SWAP(q_a1, q_a2);		\
+	    if (Q_LESS(q_a3, q_a2))	\
+		Q_SWAP(q_a2, q_a3);	\
+	}				\
+    }					\
+    else if (Q_LESS(q_a3, q_a2)) {	\
+	Q_SWAP(q_a2, q_a3);		\
+	if (Q_LESS(q_a2, q_a1))		\
+	    Q_SWAP(q_a1, q_a2);		\
+    }					\
+} while (0)
+
+/* Partition [q_l,q_r] around a pivot.  After partitioning,
+ * [q_l,q_j] are the elements that are less than or equal to the pivot,
+ * while [q_i,q_r] are the elements greater than or equal to the pivot. */
+#define Q_PARTITION(q_l, q_r, q_i, q_j, Q_UINT, Q_LESS, Q_SWAP)		\
+do {									\
+    /* The middle element, not to be confused with the median. */	\
+    Q_UINT q_m = q_l + ((q_r - q_l) >> 1);				\
+    /* Reorder the second, the middle, and the last items.		\
+     * As [Edelkamp Weiss 2016] explain, using the second element	\
+     * instead of the first one helps avoid bad behaviour for		\
+     * decreasingly sorted arrays.  This method is used in recent	\
+     * versions of gcc's std::sort, see gcc bug 58437#c13, although	\
+     * the details are somewhat different (cf. #c14). */		\
+    Q_SORT3(q_l + 1, q_m, q_r, Q_LESS, Q_SWAP);				\
+    /* Place the median at the beginning. */				\
+    Q_SWAP(q_l, q_m);							\
+    /* Partition [q_l+2, q_r-1] around the median which is in q_l.	\
+     * q_i and q_j are initially off by one, they get decremented	\
+     * in the do-while loops. */					\
+    q_i = q_l + 1; q_j = q_r;						\
+    while (1) {								\
+	do q_i++; while (Q_LESS(q_i, q_l));				\
+	do q_j--; while (Q_LESS(q_l, q_j));				\
+	if (q_i >= q_j) break; /* Sedgewick says "until j < i" */	\
+	Q_SWAP(q_i, q_j);						\
+    }									\
+    /* Compensate for the i==j case. */					\
+    q_i = q_j + 1;							\
+    /* Put the median to its final place. */				\
+    Q_SWAP(q_l, q_j);							\
+    /* The median is not part of the left subfile. */			\
+    q_j--;								\
+} while (0)
+
+/* Insertion sort is applied to small subfiles - this is contrary to
+ * Sedgewick's suggestion to run a separate insertion sort pass after
+ * the partitioning is done.  The reason I don't like a separate pass
+ * is that it triggers extra comparisons, because it can't see that the
+ * medians are already in their final positions and need not be rechecked.
+ * Since I do not assume that comparisons are cheap, I also do not try
+ * to eliminate the (q_j > q_l) boundary check. */
+#define Q_INSERTION_SORT(q_l, q_r, Q_UINT, Q_LESS, Q_SWAP)		\
+do {									\
+    Q_UINT q_i, q_j;							\
+    /* For each item starting with the second... */			\
+    for (q_i = q_l + 1; q_i <= q_r; q_i++)				\
+    /* move it down the array so that the first part is sorted. */	\
+    for (q_j = q_i; q_j > q_l && (Q_LESS(q_j, q_j - 1)); q_j--)		\
+	Q_SWAP(q_j, q_j - 1);						\
+} while (0)
+
+/* When the size of [q_l,q_r], i.e. q_r-q_l+1, is greater than or equal to
+ * Q_THRESH, the algorithm performs recursive partitioning.  When the size
+ * drops below Q_THRESH, the algorithm switches to insertion sort.
+ * The minimum valid value is probably 5 (with 5 items, the second and
+ * the middle items, the middle itself being rounded down, are distinct). */
+#define Q_THRESH 16
+
+/* The main loop. */
+#define Q_LOOP(Q_UINT, Q_N, Q_LESS, Q_SWAP)				\
+do {									\
+    Q_UINT q_l = 0;							\
+    Q_UINT q_r = (Q_N) - 1;						\
+    Q_UINT q_sp = 0; /* the number of frames pushed to the stack */	\
+    struct { Q_UINT q_l, q_r; }						\
+	/* On 32-bit platforms, to sort a "char[3GB+]" array,		\
+	 * it may take full 32 stack frames.  On 64-bit CPUs,		\
+	 * though, the address space is limited to 48 bits.		\
+	 * The usage is further reduced if Q_N has a 32-bit type. */	\
+	q_st[sizeof(Q_UINT) > 4 && sizeof(Q_N) > 4 ? 48 : 32];		\
+    while (1) {								\
+	if (q_r - q_l + 1 >= Q_THRESH) {				\
+	    Q_UINT q_i, q_j;						\
+	    Q_PARTITION(q_l, q_r, q_i, q_j, Q_UINT, Q_LESS, Q_SWAP);	\
+	    /* Now have two subfiles: [q_l,q_j] and [q_i,q_r].		\
+	     * Dealing with them depends on which one is bigger. */	\
+	    if (q_j - q_l >= q_r - q_i)					\
+		Q_SUBFILES(q_l, q_j, q_i, q_r);				\
+	    else							\
+		Q_SUBFILES(q_i, q_r, q_l, q_j);				\
+	}								\
+	else {								\
+	    Q_INSERTION_SORT(q_l, q_r, Q_UINT, Q_LESS, Q_SWAP);		\
+	    /* Pop subfiles from the stack, until it gets empty. */	\
+	    if (q_sp == 0) break;					\
+	    q_sp--;							\
+	    q_l = q_st[q_sp].q_l;					\
+	    q_r = q_st[q_sp].q_r;					\
+	}								\
+    }									\
+} while (0)
+
+/* The missing part: dealing with subfiles.
+ * Assumes that the first subfile is not smaller than the second. */
+#define Q_SUBFILES(q_l1, q_r1, q_l2, q_r2)				\
+do {									\
+    /* If the second subfile is only a single element, it needs		\
+     * no further processing.  The first subfile will be processed	\
+     * on the next iteration (both subfiles cannot be only a single	\
+     * element, due to Q_THRESH). */					\
+    if (q_l2 == q_r2) {							\
+	q_l = q_l1;							\
+	q_r = q_r1;							\
+    }									\
+    else {								\
+	/* Otherwise, both subfiles need processing.			\
+	 * Push the larger subfile onto the stack. */			\
+	q_st[q_sp].q_l = q_l1;						\
+	q_st[q_sp].q_r = q_r1;						\
+	q_sp++;								\
+	/* Process the smaller subfile on the next iteration. */	\
+	q_l = q_l2;							\
+	q_r = q_r2;							\
+    }									\
+} while (0)
+
+/* And now, ladies and gentlemen, may I proudly present to you... */
+#define QSORT(Q_N, Q_LESS, Q_SWAP)					\
+do {									\
+    if ((Q_N) > 1)							\
+	/* We could check sizeof(Q_N) and use "unsigned", but at least	\
+	 * on x86_64, this has the performance penalty of up to 5%. */	\
+	Q_LOOP(unsigned long, Q_N, Q_LESS, Q_SWAP);			\
+} while (0)
+
+#endif
+
+/* ex:set ts=8 sts=4 sw=4 noet: */

vendor/box3d/src/src/recording.h

@@ -0,0 +1,386 @@
+// SPDX-FileCopyrightText: 2026 Erin Catto
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include "core.h"
+
+#include "box3d/id.h"
+#include "box3d/math_functions.h"
+#include "box3d/types.h"
+
+#include <stdbool.h>
+#include <stdint.h>
+#include <stdio.h>
+#include <string.h>
+
+// FNV-1a 64-bit constants
+#define B3_SNAP_FNV_INIT 14695981039346656037ull
+#define B3_SNAP_FNV_PRIME 1099511628211ull
+
+// Mix a world position at full width so the determinism gate validates past float precision
+// when the body is far from the origin.
+static inline uint64_t b3FnvMixPosition( uint64_t hash, b3Pos p )
+{
+#if defined( BOX3D_DOUBLE_PRECISION )
+	uint64_t bx, by, bz;
+	memcpy( &bx, &p.x, 8 );
+	memcpy( &by, &p.y, 8 );
+	memcpy( &bz, &p.z, 8 );
+#else
+	uint32_t fx, fy, fz;
+	memcpy( &fx, &p.x, 4 );
+	memcpy( &fy, &p.y, 4 );
+	memcpy( &fz, &p.z, 4 );
+	uint64_t bx = fx, by = fy, bz = fz;
+#endif
+	hash = ( hash ^ bx ) * B3_SNAP_FNV_PRIME;
+	hash = ( hash ^ by ) * B3_SNAP_FNV_PRIME;
+	hash = ( hash ^ bz ) * B3_SNAP_FNV_PRIME;
+	return hash;
+}
+
+typedef struct b3World b3World;
+
+// Magic 'B3RC' in little-endian: bytes B(0x42) 3(0x33) R(0x52) C(0x43)
+#define B3_REC_MAGIC 0x43523342u
+
+// Major recording version is bumped when writers change.
+// Major version 4 added b3ShapeDef::enableSpeculativeContact
+#define B3_REC_VERSION_MAJOR 4
+
+// Minor tracks op-stream additions that keep the 48 byte header shape.
+// Minor version 3 added name cache.
+#define B3_REC_VERSION_MINOR 3
+
+// File header, fixed 48 bytes, little-endian. Contains the registry locator so the player
+// can load geometry before replaying any ops.
+typedef struct b3RecHeader
+{
+	uint32_t magic;			   // 'B3RC' = 0x43523342
+	uint16_t versionMajor;	   // B3_REC_VERSION_MAJOR
+	uint16_t versionMinor;	   // B3_REC_VERSION_MINOR
+	uint8_t pointerWidth;	   // sizeof(void*), gates POD-def struct layout
+	uint8_t bigEndian;		   // 0 on all supported targets
+	uint8_t validationEnabled; // 1 if built with BOX3D_VALIDATE, diagnostic only
+	uint8_t reserved;
+	float lengthScale; // b3GetLengthUnitsPerMeter()
+	uint32_t reserved2;
+	uint32_t reserved3;			// explicit pad so the 64-bit fields align with no implicit gap
+	uint64_t snapshotSize;		// bytes of snapshot blob after the header (0 in Phase 1)
+	uint64_t registryOffset;	// absolute offset to trailing registry block, backpatched at stop
+	uint64_t registryByteCount; // size of the registry block
+} b3RecHeader;
+
+_Static_assert( sizeof( b3RecHeader ) == 48, "recording header must be 48 bytes" );
+
+// Growable append-only byte buffer. Doubles on demand. countOnly mode tallies size without
+// allocating, used to size a buffer cheaply before a second filling pass.
+typedef struct b3RecBuffer
+{
+	uint8_t* data;
+	int capacity;
+	int size;
+	bool countOnly;
+} b3RecBuffer;
+
+// Geometry kinds for the trailing registry section
+typedef enum b3GeometryKind
+{
+	b3_geometryHull,
+	b3_geometryMesh,
+	b3_geometryHeightField,
+	b3_geometryCompound,
+} b3GeometryKind;
+
+// One entry per unique geometry blob. id == index in the entries array.
+// hashNext chains entries that share a content hash so dedup stays exact under a hash collision,
+// which the keyframe registry depends on to never grow during capture. B3_NULL_INDEX ends the chain.
+typedef struct b3GeometryEntry
+{
+	uint64_t contentHash;
+	uint32_t id;
+	b3GeometryKind kind;
+	int byteCount;
+	uint8_t* bytes;
+	int hashNext;
+} b3GeometryEntry;
+
+// Growable array of geometry entries. Ids are array indices, so the array is serialized in order.
+// dedupMap maps content hash to entry id for O(1) dedup; it is opaque here and owned by recording.c.
+typedef struct b3GeometryRegistry
+{
+	b3GeometryEntry* entries;
+	int count;
+	int capacity;
+	void* dedupMap;
+} b3GeometryRegistry;
+
+// Limit the maximum query name length to make recording simpler. Query names longer than this
+// probably indicate a bug in user code.
+#define B3_MAX_QUERY_NAME_LENGTH 64
+
+// Query tag from b3QueryFilter.
+// Stored once per key in the trailing block so a tagged query carries only the 8 byte key on the wire.
+// Shared by the recorder (accumulate) and the player (load).
+typedef struct b3RecTag
+{
+	// hash of (id, queryName)
+	uint64_t key;				  
+	uint64_t id;
+	char queryName[B3_MAX_QUERY_NAME_LENGTH + 1];
+} b3RecTag;
+
+// User-owned recording buffer. The world appends into it while active; the host saves and
+// destroys it. Opaque across the public API.
+typedef struct b3Recording
+{
+	b3RecBuffer buffer;
+	int recordStart; // offset of the 3-byte size field for u24 backpatch
+	b3Mutex* lock;	 // serializes record writes from concurrent threads
+	b3GeometryRegistry registry;
+
+	// Interned query tags accumulated during capture, written to the tail of the registry block at stop.
+	// tagMap maps a tag key to its index for O(1) dedup.
+	b3RecTag* tags;
+	int tagCount;
+	int tagCapacity;
+	void* tagMap;
+
+	// Union of world bounds over every recorded step, written at stop.
+	b3AABB accumulatedBounds;
+	bool haveBounds;
+} b3Recording;
+
+// C type aliases per TAG, used in the X-macro codegen arg structs
+typedef bool b3RecCType_BOOL;
+typedef int32_t b3RecCType_I32;
+typedef uint8_t b3RecCType_U8;
+typedef uint16_t b3RecCType_U16;
+typedef uint32_t b3RecCType_U32;
+typedef uint64_t b3RecCType_U64;
+typedef float b3RecCType_F32;
+typedef double b3RecCType_F64;
+typedef b3Vec3 b3RecCType_VEC3;
+typedef b3Quat b3RecCType_QUAT;
+typedef b3Transform b3RecCType_TRANSFORM;
+typedef b3Pos b3RecCType_POSITION;
+typedef b3WorldTransform b3RecCType_WORLDXF;
+typedef b3Matrix3 b3RecCType_MATRIX3;
+typedef b3AABB b3RecCType_AABB;
+typedef b3Sphere b3RecCType_SPHERE;
+typedef b3Capsule b3RecCType_CAPSULE;
+typedef b3QueryFilter b3RecCType_QUERYFILTER;
+typedef b3ShapeProxy b3RecCType_SHAPEPROXY;
+// Geometry reference: a plain u32 id into the trailing registry
+typedef uint32_t b3RecCType_GEOMID;
+typedef b3Filter b3RecCType_FILTER;
+typedef b3SurfaceMaterial b3RecCType_MATERIAL;
+typedef b3MassData b3RecCType_MASSDATA;
+typedef b3MotionLocks b3RecCType_LOCKS;
+typedef const char* b3RecCType_STR;
+typedef b3WorldId b3RecCType_WORLDID;
+typedef b3BodyId b3RecCType_BODYID;
+typedef b3ShapeId b3RecCType_SHAPEID;
+typedef b3JointId b3RecCType_JOINTID;
+typedef b3BodyDef b3RecCType_BODYDEF;
+typedef b3ShapeDef b3RecCType_SHAPEDEF;
+typedef b3ExplosionDef b3RecCType_EXPLOSIONDEF;
+typedef b3ParallelJointDef b3RecCType_PARALLELJOINTDEF;
+typedef b3DistanceJointDef b3RecCType_DISTANCEJOINTDEF;
+typedef b3FilterJointDef b3RecCType_FILTERJOINTDEF;
+typedef b3MotorJointDef b3RecCType_MOTORJOINTDEF;
+typedef b3PrismaticJointDef b3RecCType_PRISMATICJOINTDEF;
+typedef b3RevoluteJointDef b3RecCType_REVOLUTEJOINTDEF;
+typedef b3SphericalJointDef b3RecCType_SPHERICALJOINTDEF;
+typedef b3WeldJointDef b3RecCType_WELDJOINTDEF;
+typedef b3WheelJointDef b3RecCType_WHEELJOINTDEF;
+
+// Codegen pass 1a: arg structs. Generated here so call sites (body.c, shape.c, etc.) can see them.
+#define ARG( TAG, field ) b3RecCType_##TAG field;
+#define B3_REC_OP( op, Name, RET, ... )                                                                                          \
+	typedef struct                                                                                                               \
+	{                                                                                                                            \
+		__VA_ARGS__                                                                                                              \
+	} b3RecArgs_##Name;
+#include "recording_ops.inl"
+#undef B3_REC_OP
+#undef ARG
+
+// Opcode constants generated from the manifest, so call sites name an op instead of a raw byte and
+// can't drift from the manifest if an op is renumbered.
+enum
+{
+#define B3_REC_OP( op, Name, RET, ... ) b3_recOp##Name = ( op ),
+#include "recording_ops.inl"
+#undef B3_REC_OP
+};
+
+// Low-level buffer helpers
+void b3RecBufAppend( b3RecBuffer* buf, const void* data, int size );
+void b3RecBufFree( b3RecBuffer* buf );
+
+// Write primitives
+void b3RecW_U8( b3RecBuffer* buf, uint8_t v );
+void b3RecW_U16( b3RecBuffer* buf, uint16_t v );
+void b3RecW_U32( b3RecBuffer* buf, uint32_t v );
+void b3RecW_U64( b3RecBuffer* buf, uint64_t v );
+void b3RecW_I32( b3RecBuffer* buf, int32_t v );
+void b3RecW_F32( b3RecBuffer* buf, float v );
+void b3RecW_F64( b3RecBuffer* buf, double v );
+void b3RecW_BOOL( b3RecBuffer* buf, bool v );
+void b3RecW_VEC3( b3RecBuffer* buf, b3Vec3 v );
+void b3RecW_QUAT( b3RecBuffer* buf, b3Quat v );
+void b3RecW_TRANSFORM( b3RecBuffer* buf, b3Transform v );
+// World position: doubles in large-world mode, floats otherwise (wire-identical to VEC3 in float build)
+void b3RecW_POSITION( b3RecBuffer* buf, b3Pos v );
+void b3RecW_WORLDXF( b3RecBuffer* buf, b3WorldTransform v );
+void b3RecW_MATRIX3( b3RecBuffer* buf, b3Matrix3 v );
+void b3RecW_AABB( b3RecBuffer* buf, b3AABB v );
+void b3RecW_QUERYFILTER( b3RecBuffer* buf, b3QueryFilter v );
+void b3RecW_SHAPEPROXY( b3RecBuffer* buf, b3ShapeProxy v );
+void b3RecW_TREESTATS( b3RecBuffer* buf, b3TreeStats v );
+void b3RecW_RAYRESULT( b3RecBuffer* buf, b3RayResult v );
+void b3RecW_PLANERESULT( b3RecBuffer* buf, b3PlaneResult v );
+void b3RecW_WORLDID( b3RecBuffer* buf, b3WorldId v );
+void b3RecW_BODYID( b3RecBuffer* buf, b3BodyId v );
+void b3RecW_SHAPEID( b3RecBuffer* buf, b3ShapeId v );
+void b3RecW_JOINTID( b3RecBuffer* buf, b3JointId v );
+void b3RecW_SPHERE( b3RecBuffer* buf, b3Sphere v );
+void b3RecW_CAPSULE( b3RecBuffer* buf, b3Capsule v );
+void b3RecW_GEOMID( b3RecBuffer* buf, uint32_t v );
+void b3RecW_FILTER( b3RecBuffer* buf, b3Filter v );
+void b3RecW_MATERIAL( b3RecBuffer* buf, b3SurfaceMaterial v );
+void b3RecW_MASSDATA( b3RecBuffer* buf, b3MassData v );
+void b3RecW_LOCKS( b3RecBuffer* buf, b3MotionLocks v );
+void b3RecW_EXPLOSIONDEF( b3RecBuffer* buf, b3ExplosionDef v );
+void b3RecW_BODYDEF( b3RecBuffer* buf, b3BodyDef v );
+void b3RecW_SHAPEDEF( b3RecBuffer* buf, b3ShapeDef v );
+void b3RecW_PARALLELJOINTDEF( b3RecBuffer* buf, b3ParallelJointDef v );
+void b3RecW_DISTANCEJOINTDEF( b3RecBuffer* buf, b3DistanceJointDef v );
+void b3RecW_FILTERJOINTDEF( b3RecBuffer* buf, b3FilterJointDef v );
+void b3RecW_MOTORJOINTDEF( b3RecBuffer* buf, b3MotorJointDef v );
+void b3RecW_PRISMATICJOINTDEF( b3RecBuffer* buf, b3PrismaticJointDef v );
+void b3RecW_REVOLUTEJOINTDEF( b3RecBuffer* buf, b3RevoluteJointDef v );
+void b3RecW_SPHERICALJOINTDEF( b3RecBuffer* buf, b3SphericalJointDef v );
+void b3RecW_WELDJOINTDEF( b3RecBuffer* buf, b3WeldJointDef v );
+void b3RecW_WHEELJOINTDEF( b3RecBuffer* buf, b3WheelJointDef v );
+
+// Record framing
+void b3RecBeginRecord( b3Recording* rec, uint8_t opcode );
+void b3RecEndRecord( b3Recording* rec );
+
+// Per-op arg writers (no framing) and full writers (framing + args), generated from the manifest.
+#define B3_REC_OP( op, Name, RET, ... )                                                                                          \
+	void b3RecWriteArgs_##Name( b3Recording* rec, const b3RecArgs_##Name* a );                                                   \
+	void b3RecWrite_##Name( b3Recording* rec, const b3RecArgs_##Name* a );
+#include "recording_ops.inl"
+#undef B3_REC_OP
+
+// Create ops: declare writers that also append the returned id inside the record.
+#define B3_REC_RETDECL_RET_NONE( Name )
+#define B3_REC_RETDECL_RET_BODYID( Name ) void b3RecWriteRet_##Name( b3Recording* rec, const b3RecArgs_##Name* a, b3BodyId id );
+#define B3_REC_RETDECL_RET_SHAPEID( Name ) void b3RecWriteRet_##Name( b3Recording* rec, const b3RecArgs_##Name* a, b3ShapeId id );
+#define B3_REC_RETDECL_RET_JOINTID( Name ) void b3RecWriteRet_##Name( b3Recording* rec, const b3RecArgs_##Name* a, b3JointId id );
+#define B3_REC_OP( op, Name, RET, ... ) B3_REC_RETDECL_##RET( Name )
+#include "recording_ops.inl"
+#undef B3_REC_OP
+#undef B3_REC_RETDECL_RET_NONE
+#undef B3_REC_RETDECL_RET_BODYID
+#undef B3_REC_RETDECL_RET_SHAPEID
+#undef B3_REC_RETDECL_RET_JOINTID
+
+// Record a void op. The branch is free when recording is off.
+#define B3_REC( world, Name, ... )                                                                                               \
+	do                                                                                                                           \
+	{                                                                                                                            \
+		if ( ( world )->recording != NULL )                                                                                      \
+		{                                                                                                                        \
+			b3RecArgs_##Name recArgs = { __VA_ARGS__ };                                                                          \
+			b3RecWrite_##Name( ( world )->recording, &recArgs );                                                                 \
+		}                                                                                                                        \
+	}                                                                                                                            \
+	while ( 0 )
+
+// Record a create op and its returned id in one framed record. Place after the real create call.
+#define B3_REC_CREATE( world, Name, id, ... )                                                                                    \
+	do                                                                                                                           \
+	{                                                                                                                            \
+		if ( ( world )->recording != NULL )                                                                                      \
+		{                                                                                                                        \
+			b3RecArgs_##Name recCreateArgs = { __VA_ARGS__ };                                                                    \
+			b3RecWriteRet_##Name( ( world )->recording, &recCreateArgs, id );                                                    \
+		}                                                                                                                        \
+	}                                                                                                                            \
+	while ( 0 )
+
+// Patch helpers for the query hit-count backfill
+int b3RecReserveU32( b3RecBuffer* buf );
+void b3RecPatchU32( b3RecBuffer* buf, int offset, uint32_t v );
+
+// Commit a finished query record under the lock. The payload buffer stays owned by the caller.
+void b3RecCommitRecord( b3Recording* rec, uint8_t opcode, const uint8_t* payload, int payloadSize );
+
+// Per-query writer context: holds the user fcn+ctx, the local payload buffer, and the hit counter
+typedef struct b3RecQueryWriter
+{
+	union
+	{
+		b3OverlapResultFcn* overlapFcn;
+		b3CastResultFcn* castFcn;
+		b3PlaneResultFcn* planeFcn;
+		b3MoverFilterFcn* moverFilterFcn;
+	} userFcn;
+	void* userContext;
+	b3RecBuffer buf; // per-call local payload, heap-backed
+	int countOffset; // offset of the reserved u32 hit-count slot
+	uint32_t hitCount;
+	uint64_t tagId;		 // caller query id, 0 = untagged. Emitted as a QueryTag before the record.
+	const char* tagName; // caller query name, interned by id. NULL = none.
+} b3RecQueryWriter;
+
+void b3RecQueryBegin( b3RecQueryWriter* w, void* context, uint64_t tagId, const char* tagName );
+void b3RecQueryCommit( b3Recording* rec, uint8_t opcode, b3RecQueryWriter* w );
+
+// Recording trampolines: replace the user fcn so hits are captured before dispatch. The overlap
+// trampoline doubles for the mover filter, which has the same bool(shapeId, ctx) shape.
+bool b3RecOverlapTrampoline( b3ShapeId id, void* ctx );
+float b3RecCastTrampoline( b3ShapeId id, b3Pos point, b3Vec3 normal, float fraction, uint64_t userMaterialId, int triangleIndex,
+						   int childIndex, void* ctx );
+bool b3RecPlaneTrampoline( b3ShapeId id, const b3PlaneResult* planes, int planeCount, void* ctx );
+
+// Geometry registry
+uint32_t b3InternGeometry( b3GeometryRegistry* reg, b3GeometryKind kind, uint64_t contentHash, uint8_t* bytes, int byteCount );
+// Append an entry unconditionally and return its id, which equals its array index. Unlike
+// b3InternGeometry it never deduplicates, so the keyframe seed can mirror slots 1:1 even when an
+// already-recorded file carries byte-identical duplicate slots (a hash collision wrote them apart).
+uint32_t b3AppendGeometry( b3GeometryRegistry* reg, b3GeometryKind kind, uint64_t contentHash, uint8_t* bytes, int byteCount );
+void b3FreeRegistry( b3GeometryRegistry* reg );
+void b3RecWriteRegistry( b3Recording* rec );
+
+// Hash a query (id, name) pair into the stable key the viewer tracks the query by. Never returns 0,
+// so the key doubles as a tagged/untagged flag.
+uint64_t b3HashQueryTag( uint64_t id, const char* name );
+
+// Record a key->(id, name) mapping once, deduped by key (a repeated key keeps its first id/name).
+void b3RecInternTag( b3Recording* rec, uint64_t key, uint64_t id, const char* name );
+
+// Intern each large geometry kind and return a stable u32 id for use in create ops.
+// Caller does NOT free bytes; b3InternGeometry takes ownership (frees on duplicate).
+uint32_t b3RecInternHull( b3Recording* rec, const b3HullData* hull );
+uint32_t b3RecInternMesh( b3Recording* rec, const b3MeshData* mesh );
+uint32_t b3RecInternHeightField( b3Recording* rec, const b3HeightFieldData* hf );
+uint32_t b3RecInternCompound( b3Recording* rec, const b3CompoundData* compound );
+
+uint64_t b3Hash64Blob( const uint8_t* bytes, int n );
+
+// Lifecycle engine-side hooks
+void b3StartRecordingIntoBuffer( b3World* world, b3Recording* recording );
+void b3StopRecordingInternal( b3World* world );
+
+// Fold one step's world bounds into the running union.
+void b3RecAccumulateBounds( b3Recording* rec, b3AABB bounds );
+
+// Deterministic hash over all body transforms and velocities.
+// Called by both recorder and replayer to verify simulation reproduces exactly.
+uint64_t b3HashWorldState( b3World* world );

vendor/box3d/src/src/recording_ops.inl

@@ -0,0 +1,232 @@
+// SPDX-FileCopyrightText: 2026 Erin Catto
+// SPDX-License-Identifier: MIT
+
+// X-macro manifest for the recording system.
+// Include with B3_REC_OP and ARG defined. No commas between ARG tokens.
+//
+// B3_REC_OP( opcode, Name, RET, ARGS )
+//   RET in { RET_NONE, RET_BODYID, RET_SHAPEID, RET_JOINTID }
+//   ARGS = zero or more ARG( TAG, fieldName ) tokens, NO commas between them
+//
+// Opcode ranges:
+//   0x00-0x0F  world lifecycle and config
+//   0x10-0x1F  body create/destroy
+//   0x20-0x3F  body mutators
+//   0x40-0x4F  shape create/destroy
+//   0x50-0x6F  shape mutators
+//   0x80       step
+//   0x90-0xE7  joints (create, generic, per-type)
+//   0xE8-0xEF  spatial queries
+//   0xF0-0xFF  markers
+
+// Recordings are seeded with a world snapshot, not a CreateWorld op. DestroyWorld stays as the
+// end-of-session marker the viewer reads.
+B3_REC_OP( 0x01, DestroyWorld, RET_NONE, ARG( WORLDID, world ) )
+B3_REC_OP( 0x80, Step, RET_NONE, ARG( WORLDID, world ) ARG( F32, dt ) ARG( I32, subStepCount ) )
+
+// World config. The world arg is informational; replay always targets its own world.
+B3_REC_OP( 0x02, WorldEnableSleeping, RET_NONE, ARG( WORLDID, world ) ARG( BOOL, flag ) )
+B3_REC_OP( 0x03, WorldEnableContinuous, RET_NONE, ARG( WORLDID, world ) ARG( BOOL, flag ) )
+B3_REC_OP( 0x04, WorldSetRestitutionThreshold, RET_NONE, ARG( WORLDID, world ) ARG( F32, value ) )
+B3_REC_OP( 0x05, WorldSetHitEventThreshold, RET_NONE, ARG( WORLDID, world ) ARG( F32, value ) )
+B3_REC_OP( 0x06, WorldSetGravity, RET_NONE, ARG( WORLDID, world ) ARG( VEC3, gravity ) )
+B3_REC_OP( 0x07, WorldExplode, RET_NONE, ARG( WORLDID, world ) ARG( EXPLOSIONDEF, def ) )
+B3_REC_OP( 0x08, WorldSetContactTuning, RET_NONE,
+		   ARG( WORLDID, world ) ARG( F32, hertz ) ARG( F32, dampingRatio ) ARG( F32, contactSpeed ) )
+B3_REC_OP( 0x09, WorldSetContactRecycleDistance, RET_NONE, ARG( WORLDID, world ) ARG( F32, recycleDistance ) )
+B3_REC_OP( 0x0A, WorldSetMaximumLinearSpeed, RET_NONE, ARG( WORLDID, world ) ARG( F32, maximumLinearSpeed ) )
+B3_REC_OP( 0x0B, WorldEnableWarmStarting, RET_NONE, ARG( WORLDID, world ) ARG( BOOL, flag ) )
+B3_REC_OP( 0x0C, WorldRebuildStaticTree, RET_NONE, ARG( WORLDID, world ) )
+B3_REC_OP( 0x0D, WorldEnableSpeculative, RET_NONE, ARG( WORLDID, world ) ARG( BOOL, flag ) )
+
+// Body
+B3_REC_OP( 0x10, CreateBody, RET_BODYID, ARG( WORLDID, world ) ARG( BODYDEF, def ) )
+B3_REC_OP( 0x11, DestroyBody, RET_NONE, ARG( BODYID, body ) )
+B3_REC_OP( 0x20, BodySetTransform, RET_NONE, ARG( BODYID, body ) ARG( POSITION, position ) ARG( QUAT, rotation ) )
+B3_REC_OP( 0x21, BodySetLinearVelocity, RET_NONE, ARG( BODYID, body ) ARG( VEC3, v ) )
+B3_REC_OP( 0x22, BodySetType, RET_NONE, ARG( BODYID, body ) ARG( I32, type ) )
+B3_REC_OP( 0x23, BodySetName, RET_NONE, ARG( BODYID, body ) ARG( STR, name ) )
+B3_REC_OP( 0x24, BodySetAngularVelocity, RET_NONE, ARG( BODYID, body ) ARG( VEC3, w ) )
+B3_REC_OP( 0x25, BodySetTargetTransform, RET_NONE, ARG( BODYID, body ) ARG( WORLDXF, target ) ARG( F32, timeStep ) ARG( BOOL, wake ) )
+B3_REC_OP( 0x26, BodyApplyForce, RET_NONE, ARG( BODYID, body ) ARG( VEC3, force ) ARG( POSITION, point ) ARG( BOOL, wake ) )
+B3_REC_OP( 0x27, BodyApplyForceToCenter, RET_NONE, ARG( BODYID, body ) ARG( VEC3, force ) ARG( BOOL, wake ) )
+B3_REC_OP( 0x28, BodyApplyTorque, RET_NONE, ARG( BODYID, body ) ARG( VEC3, torque ) ARG( BOOL, wake ) )
+B3_REC_OP( 0x29, BodyApplyLinearImpulse, RET_NONE, ARG( BODYID, body ) ARG( VEC3, impulse ) ARG( POSITION, point ) ARG( BOOL, wake ) )
+B3_REC_OP( 0x2A, BodyApplyLinearImpulseToCenter, RET_NONE, ARG( BODYID, body ) ARG( VEC3, impulse ) ARG( BOOL, wake ) )
+B3_REC_OP( 0x2B, BodyApplyAngularImpulse, RET_NONE, ARG( BODYID, body ) ARG( VEC3, impulse ) ARG( BOOL, wake ) )
+B3_REC_OP( 0x2C, BodySetMassData, RET_NONE, ARG( BODYID, body ) ARG( MASSDATA, massData ) )
+B3_REC_OP( 0x2D, BodyApplyMassFromShapes, RET_NONE, ARG( BODYID, body ) )
+B3_REC_OP( 0x2E, BodySetLinearDamping, RET_NONE, ARG( BODYID, body ) ARG( F32, damping ) )
+B3_REC_OP( 0x2F, BodySetAngularDamping, RET_NONE, ARG( BODYID, body ) ARG( F32, damping ) )
+B3_REC_OP( 0x30, BodySetGravityScale, RET_NONE, ARG( BODYID, body ) ARG( F32, scale ) )
+B3_REC_OP( 0x31, BodySetAwake, RET_NONE, ARG( BODYID, body ) ARG( BOOL, awake ) )
+B3_REC_OP( 0x32, BodyEnableSleep, RET_NONE, ARG( BODYID, body ) ARG( BOOL, flag ) )
+B3_REC_OP( 0x33, BodySetSleepThreshold, RET_NONE, ARG( BODYID, body ) ARG( F32, threshold ) )
+B3_REC_OP( 0x34, BodyDisable, RET_NONE, ARG( BODYID, body ) )
+B3_REC_OP( 0x35, BodyEnable, RET_NONE, ARG( BODYID, body ) )
+B3_REC_OP( 0x36, BodySetMotionLocks, RET_NONE, ARG( BODYID, body ) ARG( LOCKS, locks ) )
+B3_REC_OP( 0x37, BodySetBullet, RET_NONE, ARG( BODYID, body ) ARG( BOOL, flag ) )
+B3_REC_OP( 0x38, BodyEnableContactRecycling, RET_NONE, ARG( BODYID, body ) ARG( BOOL, flag ) )
+B3_REC_OP( 0x39, BodyEnableHitEvents, RET_NONE, ARG( BODYID, body ) ARG( BOOL, flag ) )
+
+// Shape create/destroy
+B3_REC_OP( 0x40, CreateSphereShape, RET_SHAPEID, ARG( BODYID, body ) ARG( SHAPEDEF, def ) ARG( SPHERE, sphere ) )
+B3_REC_OP( 0x41, CreateCapsuleShape, RET_SHAPEID, ARG( BODYID, body ) ARG( SHAPEDEF, def ) ARG( CAPSULE, capsule ) )
+B3_REC_OP( 0x42, CreateHullShape, RET_SHAPEID, ARG( BODYID, body ) ARG( SHAPEDEF, def ) ARG( GEOMID, geometryId ) )
+B3_REC_OP( 0x43, CreateMeshShape, RET_SHAPEID, ARG( BODYID, body ) ARG( SHAPEDEF, def ) ARG( GEOMID, geometryId ) ARG( VEC3, scale ) )
+B3_REC_OP( 0x44, CreateHeightFieldShape, RET_SHAPEID, ARG( BODYID, body ) ARG( SHAPEDEF, def ) ARG( GEOMID, geometryId ) )
+B3_REC_OP( 0x45, CreateCompoundShape, RET_SHAPEID, ARG( BODYID, body ) ARG( SHAPEDEF, def ) ARG( GEOMID, geometryId ) )
+B3_REC_OP( 0x46, DestroyShape, RET_NONE, ARG( SHAPEID, shape ) ARG( BOOL, updateBodyMass ) )
+
+// Shape mutators
+B3_REC_OP( 0x50, ShapeSetDensity, RET_NONE, ARG( SHAPEID, shape ) ARG( F32, density ) ARG( BOOL, updateBodyMass ) )
+B3_REC_OP( 0x51, ShapeSetFriction, RET_NONE, ARG( SHAPEID, shape ) ARG( F32, friction ) )
+B3_REC_OP( 0x52, ShapeSetRestitution, RET_NONE, ARG( SHAPEID, shape ) ARG( F32, restitution ) )
+B3_REC_OP( 0x53, ShapeSetSurfaceMaterial, RET_NONE, ARG( SHAPEID, shape ) ARG( MATERIAL, material ) )
+B3_REC_OP( 0x54, ShapeSetFilter, RET_NONE, ARG( SHAPEID, shape ) ARG( FILTER, filter ) ARG( BOOL, invokeContacts ) )
+B3_REC_OP( 0x55, ShapeEnableSensorEvents, RET_NONE, ARG( SHAPEID, shape ) ARG( BOOL, flag ) )
+B3_REC_OP( 0x56, ShapeEnableContactEvents, RET_NONE, ARG( SHAPEID, shape ) ARG( BOOL, flag ) )
+B3_REC_OP( 0x57, ShapeEnablePreSolveEvents, RET_NONE, ARG( SHAPEID, shape ) ARG( BOOL, flag ) )
+B3_REC_OP( 0x58, ShapeEnableHitEvents, RET_NONE, ARG( SHAPEID, shape ) ARG( BOOL, flag ) )
+B3_REC_OP( 0x59, ShapeSetSphere, RET_NONE, ARG( SHAPEID, shape ) ARG( SPHERE, sphere ) )
+B3_REC_OP( 0x5A, ShapeSetCapsule, RET_NONE, ARG( SHAPEID, shape ) ARG( CAPSULE, capsule ) )
+B3_REC_OP( 0x5B, ShapeApplyWind, RET_NONE,
+		   ARG( SHAPEID, shape ) ARG( VEC3, wind ) ARG( F32, drag ) ARG( F32, lift ) ARG( F32, maxSpeed ) ARG( BOOL, wake ) )
+B3_REC_OP( 0x5C, ShapeSetName, RET_NONE, ARG( SHAPEID, shape ) ARG( STR, name ) )
+
+// Joint create and destroy
+B3_REC_OP( 0x90, CreateParallelJoint, RET_JOINTID, ARG( WORLDID, world ) ARG( PARALLELJOINTDEF, def ) )
+B3_REC_OP( 0x91, CreateDistanceJoint, RET_JOINTID, ARG( WORLDID, world ) ARG( DISTANCEJOINTDEF, def ) )
+B3_REC_OP( 0x92, CreateFilterJoint, RET_JOINTID, ARG( WORLDID, world ) ARG( FILTERJOINTDEF, def ) )
+B3_REC_OP( 0x93, CreateMotorJoint, RET_JOINTID, ARG( WORLDID, world ) ARG( MOTORJOINTDEF, def ) )
+B3_REC_OP( 0x94, CreatePrismaticJoint, RET_JOINTID, ARG( WORLDID, world ) ARG( PRISMATICJOINTDEF, def ) )
+B3_REC_OP( 0x95, CreateRevoluteJoint, RET_JOINTID, ARG( WORLDID, world ) ARG( REVOLUTEJOINTDEF, def ) )
+B3_REC_OP( 0x96, CreateSphericalJoint, RET_JOINTID, ARG( WORLDID, world ) ARG( SPHERICALJOINTDEF, def ) )
+B3_REC_OP( 0x97, CreateWeldJoint, RET_JOINTID, ARG( WORLDID, world ) ARG( WELDJOINTDEF, def ) )
+B3_REC_OP( 0x98, CreateWheelJoint, RET_JOINTID, ARG( WORLDID, world ) ARG( WHEELJOINTDEF, def ) )
+B3_REC_OP( 0x99, DestroyJoint, RET_NONE, ARG( JOINTID, joint ) ARG( BOOL, wakeAttached ) )
+
+// Generic joint mutators
+B3_REC_OP( 0x9A, JointSetLocalFrameA, RET_NONE, ARG( JOINTID, joint ) ARG( TRANSFORM, localFrame ) )
+B3_REC_OP( 0x9B, JointSetLocalFrameB, RET_NONE, ARG( JOINTID, joint ) ARG( TRANSFORM, localFrame ) )
+B3_REC_OP( 0x9C, JointSetCollideConnected, RET_NONE, ARG( JOINTID, joint ) ARG( BOOL, shouldCollide ) )
+B3_REC_OP( 0x9D, JointWakeBodies, RET_NONE, ARG( JOINTID, joint ) )
+B3_REC_OP( 0x9E, JointSetConstraintTuning, RET_NONE, ARG( JOINTID, joint ) ARG( F32, hertz ) ARG( F32, dampingRatio ) )
+B3_REC_OP( 0x9F, JointSetForceThreshold, RET_NONE, ARG( JOINTID, joint ) ARG( F32, threshold ) )
+B3_REC_OP( 0xA0, JointSetTorqueThreshold, RET_NONE, ARG( JOINTID, joint ) ARG( F32, threshold ) )
+
+// Parallel joint
+B3_REC_OP( 0xA1, ParallelJointSetSpringHertz, RET_NONE, ARG( JOINTID, joint ) ARG( F32, hertz ) )
+B3_REC_OP( 0xA2, ParallelJointSetSpringDampingRatio, RET_NONE, ARG( JOINTID, joint ) ARG( F32, dampingRatio ) )
+B3_REC_OP( 0xA3, ParallelJointSetMaxTorque, RET_NONE, ARG( JOINTID, joint ) ARG( F32, maxTorque ) )
+
+// Distance joint
+B3_REC_OP( 0xA4, DistanceJointSetLength, RET_NONE, ARG( JOINTID, joint ) ARG( F32, length ) )
+B3_REC_OP( 0xA5, DistanceJointEnableSpring, RET_NONE, ARG( JOINTID, joint ) ARG( BOOL, enableSpring ) )
+B3_REC_OP( 0xA6, DistanceJointSetSpringForceRange, RET_NONE, ARG( JOINTID, joint ) ARG( F32, lowerForce ) ARG( F32, upperForce ) )
+B3_REC_OP( 0xA7, DistanceJointSetSpringHertz, RET_NONE, ARG( JOINTID, joint ) ARG( F32, hertz ) )
+B3_REC_OP( 0xA8, DistanceJointSetSpringDampingRatio, RET_NONE, ARG( JOINTID, joint ) ARG( F32, dampingRatio ) )
+B3_REC_OP( 0xA9, DistanceJointEnableLimit, RET_NONE, ARG( JOINTID, joint ) ARG( BOOL, enableLimit ) )
+B3_REC_OP( 0xAA, DistanceJointSetLengthRange, RET_NONE, ARG( JOINTID, joint ) ARG( F32, minLength ) ARG( F32, maxLength ) )
+B3_REC_OP( 0xAB, DistanceJointEnableMotor, RET_NONE, ARG( JOINTID, joint ) ARG( BOOL, enableMotor ) )
+B3_REC_OP( 0xAC, DistanceJointSetMotorSpeed, RET_NONE, ARG( JOINTID, joint ) ARG( F32, motorSpeed ) )
+B3_REC_OP( 0xAD, DistanceJointSetMaxMotorForce, RET_NONE, ARG( JOINTID, joint ) ARG( F32, force ) )
+
+// Motor joint
+B3_REC_OP( 0xAE, MotorJointSetLinearVelocity, RET_NONE, ARG( JOINTID, joint ) ARG( VEC3, velocity ) )
+B3_REC_OP( 0xAF, MotorJointSetAngularVelocity, RET_NONE, ARG( JOINTID, joint ) ARG( VEC3, velocity ) )
+B3_REC_OP( 0xB0, MotorJointSetMaxVelocityForce, RET_NONE, ARG( JOINTID, joint ) ARG( F32, maxForce ) )
+B3_REC_OP( 0xB1, MotorJointSetMaxVelocityTorque, RET_NONE, ARG( JOINTID, joint ) ARG( F32, maxTorque ) )
+B3_REC_OP( 0xB2, MotorJointSetLinearHertz, RET_NONE, ARG( JOINTID, joint ) ARG( F32, hertz ) )
+B3_REC_OP( 0xB3, MotorJointSetLinearDampingRatio, RET_NONE, ARG( JOINTID, joint ) ARG( F32, damping ) )
+B3_REC_OP( 0xB4, MotorJointSetAngularHertz, RET_NONE, ARG( JOINTID, joint ) ARG( F32, hertz ) )
+B3_REC_OP( 0xB5, MotorJointSetAngularDampingRatio, RET_NONE, ARG( JOINTID, joint ) ARG( F32, damping ) )
+B3_REC_OP( 0xB6, MotorJointSetMaxSpringForce, RET_NONE, ARG( JOINTID, joint ) ARG( F32, maxForce ) )
+B3_REC_OP( 0xB7, MotorJointSetMaxSpringTorque, RET_NONE, ARG( JOINTID, joint ) ARG( F32, maxTorque ) )
+
+// Prismatic joint
+B3_REC_OP( 0xB8, PrismaticJointEnableSpring, RET_NONE, ARG( JOINTID, joint ) ARG( BOOL, enableSpring ) )
+B3_REC_OP( 0xB9, PrismaticJointSetSpringHertz, RET_NONE, ARG( JOINTID, joint ) ARG( F32, hertz ) )
+B3_REC_OP( 0xBA, PrismaticJointSetSpringDampingRatio, RET_NONE, ARG( JOINTID, joint ) ARG( F32, dampingRatio ) )
+B3_REC_OP( 0xBB, PrismaticJointSetTargetTranslation, RET_NONE, ARG( JOINTID, joint ) ARG( F32, translation ) )
+B3_REC_OP( 0xBC, PrismaticJointEnableLimit, RET_NONE, ARG( JOINTID, joint ) ARG( BOOL, enableLimit ) )
+B3_REC_OP( 0xBD, PrismaticJointSetLimits, RET_NONE, ARG( JOINTID, joint ) ARG( F32, lower ) ARG( F32, upper ) )
+B3_REC_OP( 0xBE, PrismaticJointEnableMotor, RET_NONE, ARG( JOINTID, joint ) ARG( BOOL, enableMotor ) )
+B3_REC_OP( 0xBF, PrismaticJointSetMotorSpeed, RET_NONE, ARG( JOINTID, joint ) ARG( F32, motorSpeed ) )
+B3_REC_OP( 0xC0, PrismaticJointSetMaxMotorForce, RET_NONE, ARG( JOINTID, joint ) ARG( F32, force ) )
+
+// Revolute joint
+B3_REC_OP( 0xC1, RevoluteJointEnableSpring, RET_NONE, ARG( JOINTID, joint ) ARG( BOOL, enableSpring ) )
+B3_REC_OP( 0xC2, RevoluteJointSetSpringHertz, RET_NONE, ARG( JOINTID, joint ) ARG( F32, hertz ) )
+B3_REC_OP( 0xC3, RevoluteJointSetSpringDampingRatio, RET_NONE, ARG( JOINTID, joint ) ARG( F32, dampingRatio ) )
+B3_REC_OP( 0xC4, RevoluteJointSetTargetAngle, RET_NONE, ARG( JOINTID, joint ) ARG( F32, angle ) )
+B3_REC_OP( 0xC5, RevoluteJointEnableLimit, RET_NONE, ARG( JOINTID, joint ) ARG( BOOL, enableLimit ) )
+B3_REC_OP( 0xC6, RevoluteJointSetLimits, RET_NONE, ARG( JOINTID, joint ) ARG( F32, lower ) ARG( F32, upper ) )
+B3_REC_OP( 0xC7, RevoluteJointEnableMotor, RET_NONE, ARG( JOINTID, joint ) ARG( BOOL, enableMotor ) )
+B3_REC_OP( 0xC8, RevoluteJointSetMotorSpeed, RET_NONE, ARG( JOINTID, joint ) ARG( F32, motorSpeed ) )
+B3_REC_OP( 0xC9, RevoluteJointSetMaxMotorTorque, RET_NONE, ARG( JOINTID, joint ) ARG( F32, torque ) )
+
+// Spherical joint
+B3_REC_OP( 0xCA, SphericalJointEnableConeLimit, RET_NONE, ARG( JOINTID, joint ) ARG( BOOL, enableLimit ) )
+B3_REC_OP( 0xCB, SphericalJointSetConeLimit, RET_NONE, ARG( JOINTID, joint ) ARG( F32, angleRadians ) )
+B3_REC_OP( 0xCC, SphericalJointEnableTwistLimit, RET_NONE, ARG( JOINTID, joint ) ARG( BOOL, enableLimit ) )
+B3_REC_OP( 0xCD, SphericalJointSetTwistLimits, RET_NONE, ARG( JOINTID, joint ) ARG( F32, lower ) ARG( F32, upper ) )
+B3_REC_OP( 0xCE, SphericalJointEnableSpring, RET_NONE, ARG( JOINTID, joint ) ARG( BOOL, enableSpring ) )
+B3_REC_OP( 0xCF, SphericalJointSetSpringHertz, RET_NONE, ARG( JOINTID, joint ) ARG( F32, hertz ) )
+B3_REC_OP( 0xD0, SphericalJointSetSpringDampingRatio, RET_NONE, ARG( JOINTID, joint ) ARG( F32, dampingRatio ) )
+B3_REC_OP( 0xD1, SphericalJointSetTargetRotation, RET_NONE, ARG( JOINTID, joint ) ARG( QUAT, targetRotation ) )
+B3_REC_OP( 0xD2, SphericalJointEnableMotor, RET_NONE, ARG( JOINTID, joint ) ARG( BOOL, enableMotor ) )
+B3_REC_OP( 0xD3, SphericalJointSetMotorVelocity, RET_NONE, ARG( JOINTID, joint ) ARG( VEC3, motorVelocity ) )
+B3_REC_OP( 0xD4, SphericalJointSetMaxMotorTorque, RET_NONE, ARG( JOINTID, joint ) ARG( F32, torque ) )
+
+// Weld joint
+B3_REC_OP( 0xD5, WeldJointSetLinearHertz, RET_NONE, ARG( JOINTID, joint ) ARG( F32, hertz ) )
+B3_REC_OP( 0xD6, WeldJointSetLinearDampingRatio, RET_NONE, ARG( JOINTID, joint ) ARG( F32, dampingRatio ) )
+B3_REC_OP( 0xD7, WeldJointSetAngularHertz, RET_NONE, ARG( JOINTID, joint ) ARG( F32, hertz ) )
+B3_REC_OP( 0xD8, WeldJointSetAngularDampingRatio, RET_NONE, ARG( JOINTID, joint ) ARG( F32, dampingRatio ) )
+
+// Wheel joint
+B3_REC_OP( 0xD9, WheelJointEnableSuspension, RET_NONE, ARG( JOINTID, joint ) ARG( BOOL, flag ) )
+B3_REC_OP( 0xDA, WheelJointSetSuspensionHertz, RET_NONE, ARG( JOINTID, joint ) ARG( F32, hertz ) )
+B3_REC_OP( 0xDB, WheelJointSetSuspensionDampingRatio, RET_NONE, ARG( JOINTID, joint ) ARG( F32, dampingRatio ) )
+B3_REC_OP( 0xDC, WheelJointEnableSuspensionLimit, RET_NONE, ARG( JOINTID, joint ) ARG( BOOL, flag ) )
+B3_REC_OP( 0xDD, WheelJointSetSuspensionLimits, RET_NONE, ARG( JOINTID, joint ) ARG( F32, lower ) ARG( F32, upper ) )
+B3_REC_OP( 0xDE, WheelJointEnableSpinMotor, RET_NONE, ARG( JOINTID, joint ) ARG( BOOL, flag ) )
+B3_REC_OP( 0xDF, WheelJointSetSpinMotorSpeed, RET_NONE, ARG( JOINTID, joint ) ARG( F32, speed ) )
+
+// Wheel joint continued, overflow past the 0xDF range.
+B3_REC_OP( 0xE0, WheelJointSetMaxSpinTorque, RET_NONE, ARG( JOINTID, joint ) ARG( F32, torque ) )
+B3_REC_OP( 0xE1, WheelJointEnableSteering, RET_NONE, ARG( JOINTID, joint ) ARG( BOOL, flag ) )
+B3_REC_OP( 0xE2, WheelJointSetSteeringHertz, RET_NONE, ARG( JOINTID, joint ) ARG( F32, hertz ) )
+B3_REC_OP( 0xE3, WheelJointSetSteeringDampingRatio, RET_NONE, ARG( JOINTID, joint ) ARG( F32, dampingRatio ) )
+B3_REC_OP( 0xE4, WheelJointSetMaxSteeringTorque, RET_NONE, ARG( JOINTID, joint ) ARG( F32, torque ) )
+B3_REC_OP( 0xE5, WheelJointEnableSteeringLimit, RET_NONE, ARG( JOINTID, joint ) ARG( BOOL, flag ) )
+B3_REC_OP( 0xE6, WheelJointSetSteeringLimits, RET_NONE, ARG( JOINTID, joint ) ARG( F32, lower ) ARG( F32, upper ) )
+B3_REC_OP( 0xE7, WheelJointSetTargetSteeringAngle, RET_NONE, ARG( JOINTID, joint ) ARG( F32, radians ) )
+
+// Spatial queries. Inputs flow through the manifest (reader side). The hit tail and result are
+// hand-written in recording.c / recording_replay.c since they are variable length.
+B3_REC_OP( 0xE8, QueryOverlapAABB, RET_NONE, ARG( WORLDID, world ) ARG( AABB, aabb ) ARG( QUERYFILTER, filter ) )
+B3_REC_OP( 0xE9, QueryOverlapShape, RET_NONE,
+		   ARG( WORLDID, world ) ARG( POSITION, origin ) ARG( SHAPEPROXY, proxy ) ARG( QUERYFILTER, filter ) )
+B3_REC_OP( 0xEA, QueryCastRay, RET_NONE,
+		   ARG( WORLDID, world ) ARG( POSITION, origin ) ARG( VEC3, translation ) ARG( QUERYFILTER, filter ) )
+B3_REC_OP( 0xEB, QueryCastShape, RET_NONE,
+		   ARG( WORLDID, world ) ARG( POSITION, origin ) ARG( SHAPEPROXY, proxy ) ARG( VEC3, translation )
+			   ARG( QUERYFILTER, filter ) )
+B3_REC_OP( 0xEC, QueryCastRayClosest, RET_NONE,
+		   ARG( WORLDID, world ) ARG( POSITION, origin ) ARG( VEC3, translation ) ARG( QUERYFILTER, filter ) )
+B3_REC_OP( 0xED, QueryCastMover, RET_NONE,
+		   ARG( WORLDID, world ) ARG( POSITION, origin ) ARG( CAPSULE, mover ) ARG( VEC3, translation ) ARG( QUERYFILTER, filter ) )
+B3_REC_OP( 0xEE, QueryCollideMover, RET_NONE,
+		   ARG( WORLDID, world ) ARG( POSITION, origin ) ARG( CAPSULE, mover ) ARG( QUERYFILTER, filter ) )
+
+// Identity key (hash of the caller id + label) for the query that immediately follows. Emitted only
+// for a tagged query. The id and label are interned in the trailing tag table, so only the 8 byte key
+// rides the stream.
+B3_REC_OP( 0xEF, QueryTag, RET_NONE, ARG( U64, key ) )
+
+B3_REC_OP( 0xF1, StateHash, RET_NONE, ARG( WORLDID, world ) ARG( U64, hash ) )
+
+// Accumulated world bounds over the whole recording, written once at stop.
+B3_REC_OP( 0xF2, RecordingBounds, RET_NONE, ARG( AABB, bounds ) )

vendor/box3d/src/src/recording_replay.h

@@ -0,0 +1,258 @@
+// SPDX-FileCopyrightText: 2026 Erin Catto
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include "recording.h"
+
+#include <stdbool.h>
+#include <stdint.h>
+
+typedef struct b3RecPlayer b3RecPlayer;
+
+// A single recorded callback hit, used both as reader scratch during a query replay and as the
+// per-frame draw store. For collide-mover, one hit is one plane, with planeCount and userReturnB
+// replicated across a shape's planes so the replay walker can re-group and compare per shape.
+typedef struct b3RecRecordedHit
+{
+	b3ShapeId id;
+	b3Pos point;
+	b3Vec3 normal;
+	float fraction;
+	uint64_t userMaterialId;
+	int triangleIndex;
+	int childIndex;
+	b3PlaneResult plane; // collide-mover: this plane
+	int planeCount;		 // collide-mover: planes in this hit's shape group (replicated)
+	float userReturnF;	 // cast queries
+	bool userReturnB;	 // overlap / collide-mover (per shape, replicated)
+} b3RecRecordedHit;
+
+// Recorded query kind, matching the public b3RecQueryType order.
+typedef enum b3RecQueryKind
+{
+	B3_RECQ_OVERLAP_AABB,
+	B3_RECQ_OVERLAP_SHAPE,
+	B3_RECQ_CAST_RAY,
+	B3_RECQ_CAST_SHAPE,
+	B3_RECQ_CAST_RAY_CLOSEST,
+	B3_RECQ_CAST_MOVER,
+	B3_RECQ_COLLIDE_MOVER,
+} b3RecQueryKind;
+
+// Per-frame draw record for one query call. Self-contained (no aliased pointers) so the player's
+// frameQueries array can grow with a plain memcpy. Geometry is origin relative except the overlap
+// AABB, which is world space in 3D.
+typedef struct b3RecDrawQuery
+{
+	int kind;
+	uint64_t key; // identity key (hash of caller id+name), 0 = untagged
+	b3QueryFilter filter;
+	b3AABB aabb;								  // world-space bounds of the query, swept for casts
+	b3Vec3 proxyPoints[B3_MAX_SHAPE_CAST_POINTS]; // overlap/cast shape proxy, origin relative
+	int proxyCount;
+	float proxyRadius;
+	b3Capsule mover; // cast/collide mover, origin relative
+	b3Pos origin;
+	b3Vec3 translation;
+	float castFraction;	   // cast-mover result fraction
+	b3RayResult rayResult; // cast-ray-closest result
+	b3ShapeId shape;
+	int hitStart; // first hit in the player's frameHits store
+	int hitCount;
+} b3RecDrawQuery;
+
+// One slot in the preloaded geometry registry. Loaded from the trailing block before any
+// ops run; live pointer built lazily on first shape create that references the id.
+typedef struct b3RegistrySlot
+{
+	b3GeometryKind kind;
+	int byteCount;
+	uint8_t* bytes; // raw bytes from the file (always freed at teardown)
+	void* live;		// reconstructed live object, freed after b3DestroyWorld
+} b3RegistrySlot;
+
+// This is used to simplify the scratch buffer lifetime. Names longer than this probably
+// indicate a bug.
+#define B3_MAX_NAME_LENGTH 256
+
+// Reader state threaded through the replay loop and all dispatch functions
+typedef struct b3RecReader
+{
+	const uint8_t* data;
+	int size;
+	int cursor;
+	b3WorldId replayWorldId;
+	bool ok;	   // false on read overrun or id mismatch, fatal stop
+	bool diverged; // a StateHash failed, non-fatal
+
+	// Player that owns this reader, or NULL during a headless b3ValidateReplay. Body
+	// create/destroy and the bounds record fold back into it for the outliner and camera framing.
+	b3RecPlayer* owner;
+
+	// Scratch for per-triangle materials in shape defs (grown on demand, freed at teardown)
+	b3SurfaceMaterial* matScratch;
+	int matScratchCap;
+
+	// Scratch for string reads. Used by b3RecR_STR to pass names to b3BodyDef and b3ShapeDef.
+	char stringBuffers[4][B3_MAX_NAME_LENGTH + 1];
+	int nextString;
+
+	// Preloaded geometry registry
+	b3RegistrySlot* slots;
+	int slotCount;
+
+	// Preloaded query-tag table (key -> id, name), loaded with the registry. Resolves the caller id and
+	// label for the viewer. tagMap maps a key to its tag index for O(1) lookup; opaque, owned by the player.
+	b3RecTag* tags;
+	int tagCount;	 // tags that loaded; a truncated tail loads fewer
+	int tagCapacity; // tags allocated, used to free the array
+	void* tagMap;
+
+	// Key from the QueryTag op preceding the next query, consumed by the next stash. 0 = untagged.
+	uint64_t pendingQueryKey;
+
+	// Scratch for recorded query hits; grown on demand, freed with the player.
+	b3RecRecordedHit* hits;
+	int hitCap;
+
+	// Scratch for a shape-proxy point cloud read from the stream. b3ShapeProxy holds the points
+	// behind a pointer, so a decoded proxy borrows this until the next proxy read or teardown.
+	b3Vec3* proxyScratch;
+	int proxyScratchCap;
+} b3RecReader;
+
+// Stored snapshot for fast backward seek.
+typedef struct b3RecKeyframe
+{
+	uint8_t* image; // serialized world image at the end of this frame
+	int imageSize;
+	int imageCapacity; // allocation size (may exceed imageSize)
+	int frame;		   // frame index this restores to
+	int cursor;		   // op-stream cursor for the frame AFTER this one
+	int divergeFrame;  // divergeFrame state at capture
+	bool diverged;	   // rdr.diverged state at capture
+
+	// Outliner body list as it stood at this frame, restored verbatim so ordinals are stable.
+	b3BodyId* bodyIds;
+	int bodyIdCount;
+} b3RecKeyframe;
+
+typedef struct b3RecPlayer
+{
+	uint8_t* data; // owned copy of recording bytes
+	int size;
+	int headerEnd;	 // first byte of op stream (past header + snapshot blob)
+	int registryEnd; // end of op stream = start of registry block (or size)
+	float lengthScale;
+	float previousLengthScale;
+	int frame;
+	int frameCount;
+	float recordedDt;
+	int recordedSubStepCount;
+	int recordedWorkerCount; // worker count requested for the replay world
+	b3AABB bounds;			 // accumulated world bounds, decoded from the trailing record
+	bool atEnd;
+	// Indicates all ops for the step have been consumed up to the world step op. The next sub-step
+	// will clear this and perform the world step.
+	bool atPreStep;
+	int divergeFrame; // first frame that diverged, -1 until then
+
+	// Outliner body list, indexed by creation ordinal. Holes (null ids) mark destroyed bodies so
+	// later ordinals never shift. Snapshotted into each keyframe and the frame-0 copy, not rebuilt
+	// from the world, so a stored selection survives backward seeks.
+	b3BodyId* bodyIds;
+	int bodyIdCount;
+	int bodyIdCap;
+	b3BodyId* frame0BodyIds;
+	int frame0BodyIdCount;
+
+	// Per-frame query store, reset at the top of each StepFrame and filled by the query dispatchers.
+	// Drawn by b3RecPlayer_DrawFrameQueries and inspected via the public GetFrameQuery API.
+	b3RecDrawQuery* frameQueries;
+	int frameQueryCount;
+	int frameQueryCap;
+	b3RecRecordedHit* frameHits;
+	int frameHitCount;
+	int frameHitCap;
+
+	// Host debug-shape callbacks applied to every world the player creates. The 3D
+	// sample renderer builds GPU meshes here, so a replay world without them draws
+	// nothing. Set once via b3RecPlayer_SetDebugShapeCallbacks; persisted so a world
+	// rebuilt under new callbacks keeps drawing.
+	b3CreateDebugShapeCallback* createDebugShape;
+	b3DestroyDebugShapeCallback* destroyDebugShape;
+	void* debugShapeContext;
+
+	b3RecReader rdr;
+
+	// Frame-0 restore image, points into the owned data copy. Restart and backward seek
+	// deserialize this in place so the replay world id stays stable.
+	const uint8_t* frame0Image;
+	int frame0Size;
+
+	// Keyframe ring
+	b3RecKeyframe* keyframes;
+	int keyframeCount;
+	int keyframeCapacity;
+	size_t keyframeBudget;
+	size_t keyframeBytes;
+	int keyframeMinInterval;
+	int keyframeInterval;
+	int lastKeyframeFrame;
+
+	// Pre-populated recording used by b3SerializeWorld during keyframe capture.
+	// Its registry mirrors rdr.slots so geometry ids stay stable.
+	b3Recording* keyframeRec;
+} b3RecPlayer;
+
+// Read primitives
+uint8_t b3RecR_U8( b3RecReader* rdr );
+uint16_t b3RecR_U16( b3RecReader* rdr );
+uint32_t b3RecR_U24( b3RecReader* rdr );
+uint32_t b3RecR_U32( b3RecReader* rdr );
+uint64_t b3RecR_U64( b3RecReader* rdr );
+int32_t b3RecR_I32( b3RecReader* rdr );
+float b3RecR_F32( b3RecReader* rdr );
+double b3RecR_F64( b3RecReader* rdr );
+bool b3RecR_BOOL( b3RecReader* rdr );
+b3Vec3 b3RecR_VEC3( b3RecReader* rdr );
+b3Quat b3RecR_QUAT( b3RecReader* rdr );
+b3Transform b3RecR_TRANSFORM( b3RecReader* rdr );
+b3Pos b3RecR_POSITION( b3RecReader* rdr );
+b3WorldTransform b3RecR_WORLDXF( b3RecReader* rdr );
+b3Matrix3 b3RecR_MATRIX3( b3RecReader* rdr );
+b3AABB b3RecR_AABB( b3RecReader* rdr );
+b3WorldId b3RecR_WORLDID( b3RecReader* rdr );
+b3BodyId b3RecR_BODYID( b3RecReader* rdr );
+b3ShapeId b3RecR_SHAPEID( b3RecReader* rdr );
+b3JointId b3RecR_JOINTID( b3RecReader* rdr );
+b3Sphere b3RecR_SPHERE( b3RecReader* rdr );
+b3Capsule b3RecR_CAPSULE( b3RecReader* rdr );
+uint32_t b3RecR_GEOMID( b3RecReader* rdr );
+b3Filter b3RecR_FILTER( b3RecReader* rdr );
+b3SurfaceMaterial b3RecR_MATERIAL( b3RecReader* rdr );
+b3MassData b3RecR_MASSDATA( b3RecReader* rdr );
+b3MotionLocks b3RecR_LOCKS( b3RecReader* rdr );
+const char* b3RecR_STR( b3RecReader* rdr );
+b3ExplosionDef b3RecR_EXPLOSIONDEF( b3RecReader* rdr );
+b3BodyDef b3RecR_BODYDEF( b3RecReader* rdr );
+b3ShapeDef b3RecR_SHAPEDEF( b3RecReader* rdr );
+b3ParallelJointDef b3RecR_PARALLELJOINTDEF( b3RecReader* rdr );
+b3DistanceJointDef b3RecR_DISTANCEJOINTDEF( b3RecReader* rdr );
+b3FilterJointDef b3RecR_FILTERJOINTDEF( b3RecReader* rdr );
+b3MotorJointDef b3RecR_MOTORJOINTDEF( b3RecReader* rdr );
+b3PrismaticJointDef b3RecR_PRISMATICJOINTDEF( b3RecReader* rdr );
+b3RevoluteJointDef b3RecR_REVOLUTEJOINTDEF( b3RecReader* rdr );
+b3SphericalJointDef b3RecR_SPHERICALJOINTDEF( b3RecReader* rdr );
+b3WeldJointDef b3RecR_WELDJOINTDEF( b3RecReader* rdr );
+b3WheelJointDef b3RecR_WHEELJOINTDEF( b3RecReader* rdr );
+b3QueryFilter b3RecR_QUERYFILTER( b3RecReader* rdr );
+b3ShapeProxy b3RecR_SHAPEPROXY( b3RecReader* rdr );
+b3TreeStats b3RecR_TREESTATS( b3RecReader* rdr );
+b3RayResult b3RecR_RAYRESULT( b3RecReader* rdr );
+b3PlaneResult b3RecR_PLANERESULT( b3RecReader* rdr );
+
+// Grow the reader's hit scratch to at least n entries, preserving contents. n is bounded by the
+// file size since every recorded hit consumes at least one byte.
+void b3RecEnsureHits( b3RecReader* rdr, int n );

vendor/box3d/src/src/revolute_joint.c

@@ -0,0 +1,673 @@
+// SPDX-FileCopyrightText: 2025 Erin Catto
+// SPDX-License-Identifier: MIT
+
+#include "body.h"
+#include "joint.h"
+#include "math_internal.h"
+#include "physics_world.h"
+#include "solver.h"
+#include "solver_set.h"
+#include "recording.h"
+
+// needed for dll export
+#include "box3d/box3d.h"
+
+// Point-to-point linear constraint
+// C = pB - pA
+// Cdot = vB - vA
+//      = vB + cross(wB, rB) - vA - cross(wA, rA)
+// Cdot = J * v
+// J = [-E -skew(rA) E skew(rB) ]
+//
+// K = J * invM * JT
+//   = [(1/mA + 1/mB) * E - skew(rA) * invIA * skew(rA) - skew(rB) * invIB * skew(rB)]
+
+// Perpendicularity constraint
+// frameA = qA * localFrameA
+// frameB = qB * localFrameB
+// qRel = conj(frameA) * frameB
+// C = [qRel.x; qRel.y]
+// qRelDot = 0.5 * conj(frameA) * (wB - wA) * frameB
+// Cdot = [qRelDot.x, qRelDot.y]
+// Pulling out wB and wA
+// sr = qRel.s
+// vr = qRel.v
+// Jx = 0.5 * rotate(frameA, sr * ex + cross(vr, ex))
+// Jy = 0.5 * rotate(frameA, sr * ey + cross(vr, ey))
+
+// Motor constraint
+// Cdot = wB - wA
+// J = [0 0 -E 0 0 E]
+// K = invIA + invIB
+
+void b3RevoluteJoint_EnableLimit( b3JointId jointId, bool enableLimit )
+{
+	b3World* world = b3GetWorld( jointId.world0 );
+	B3_REC( world, RevoluteJointEnableLimit, jointId, enableLimit );
+	b3JointSim* base = b3GetJointSimCheckType( jointId, b3_revoluteJoint );
+	if ( enableLimit != base->revoluteJoint.enableLimit )
+	{
+		base->revoluteJoint.lowerImpulse = 0.0f;
+		base->revoluteJoint.upperImpulse = 0.0f;
+	}
+	base->revoluteJoint.enableLimit = enableLimit;
+}
+
+bool b3RevoluteJoint_IsLimitEnabled( b3JointId jointId )
+{
+	b3JointSim* base = b3GetJointSimCheckType( jointId, b3_revoluteJoint );
+	return base->revoluteJoint.enableLimit;
+}
+
+float b3RevoluteJoint_GetLowerLimit( b3JointId jointId )
+{
+	b3JointSim* base = b3GetJointSimCheckType( jointId, b3_revoluteJoint );
+	return base->revoluteJoint.lowerAngle;
+}
+
+float b3RevoluteJoint_GetUpperLimit( b3JointId jointId )
+{
+	b3JointSim* base = b3GetJointSimCheckType( jointId, b3_revoluteJoint );
+	return base->revoluteJoint.upperAngle;
+}
+
+void b3RevoluteJoint_SetLimits( b3JointId jointId, float lowerLimitRadians, float upperLimitRadians )
+{
+	B3_ASSERT( b3IsValidFloat( lowerLimitRadians ) && b3IsValidFloat( upperLimitRadians ) );
+	b3World* world = b3GetWorld( jointId.world0 );
+	B3_REC( world, RevoluteJointSetLimits, jointId, lowerLimitRadians, upperLimitRadians );
+
+	float lowerAngle = b3MinFloat( lowerLimitRadians, upperLimitRadians );
+	float upperAngle = b3MaxFloat( lowerLimitRadians, upperLimitRadians );
+
+	b3JointSim* base = b3GetJointSimCheckType( jointId, b3_revoluteJoint );
+	base->revoluteJoint.lowerAngle = b3ClampFloat( lowerAngle, -0.99f * B3_PI, 0.99f * B3_PI );
+	base->revoluteJoint.upperAngle = b3ClampFloat( upperAngle, -0.99f * B3_PI, 0.99f * B3_PI );
+}
+
+float b3RevoluteJoint_GetAngle( b3JointId jointId )
+{
+	b3World* world = b3GetWorld( jointId.world0 );
+	b3JointSim* base = b3GetJointSimCheckType( jointId, b3_revoluteJoint );
+	b3WorldTransform transformA = b3GetBodyTransform( world, base->bodyIdA );
+	b3WorldTransform transformB = b3GetBodyTransform( world, base->bodyIdB );
+
+	b3Quat quatA = b3MulQuat( transformA.q, base->localFrameA.q );
+	b3Quat quatB = b3MulQuat( transformB.q, base->localFrameB.q );
+
+	if ( b3DotQuat( quatA, quatB ) < 0.0f )
+	{
+		// this keeps the twist angle in the range [-pi, pi]
+		quatB = b3NegateQuat( quatB );
+	}
+
+	b3Quat relQ = b3InvMulQuat( quatA, quatB );
+
+	return b3GetTwistAngle( relQ );
+}
+
+void b3RevoluteJoint_EnableSpring( b3JointId jointId, bool enableSpring )
+{
+	b3World* world = b3GetWorld( jointId.world0 );
+	B3_REC( world, RevoluteJointEnableSpring, jointId, enableSpring );
+	b3JointSim* base = b3GetJointSimCheckType( jointId, b3_revoluteJoint );
+	if ( enableSpring != base->revoluteJoint.enableSpring )
+	{
+		base->revoluteJoint.springImpulse = 0.0f;
+	}
+	base->revoluteJoint.enableSpring = enableSpring;
+}
+
+bool b3RevoluteJoint_IsSpringEnabled( b3JointId jointId )
+{
+	b3JointSim* base = b3GetJointSimCheckType( jointId, b3_revoluteJoint );
+	return base->revoluteJoint.enableSpring;
+}
+
+void b3RevoluteJoint_SetTargetAngle( b3JointId jointId, float targetRadians )
+{
+	B3_ASSERT( b3IsValidFloat( targetRadians ) && -B3_PI <= targetRadians && targetRadians <= B3_PI );
+	b3World* world = b3GetWorld( jointId.world0 );
+	B3_REC( world, RevoluteJointSetTargetAngle, jointId, targetRadians );
+	b3JointSim* base = b3GetJointSimCheckType( jointId, b3_revoluteJoint );
+	base->revoluteJoint.targetAngle = targetRadians;
+}
+
+float b3RevoluteJoint_GetTargetAngle( b3JointId jointId )
+{
+	b3JointSim* base = b3GetJointSimCheckType( jointId, b3_revoluteJoint );
+	return base->revoluteJoint.targetAngle;
+}
+
+void b3RevoluteJoint_SetSpringHertz( b3JointId jointId, float hertz )
+{
+	B3_ASSERT( b3IsValidFloat( hertz ) && hertz >= 0.0f );
+	b3World* world = b3GetWorld( jointId.world0 );
+	B3_REC( world, RevoluteJointSetSpringHertz, jointId, hertz );
+	b3JointSim* base = b3GetJointSimCheckType( jointId, b3_revoluteJoint );
+	base->revoluteJoint.hertz = hertz;
+}
+
+float b3RevoluteJoint_GetSpringHertz( b3JointId jointId )
+{
+	b3JointSim* base = b3GetJointSimCheckType( jointId, b3_revoluteJoint );
+	return base->revoluteJoint.hertz;
+}
+
+void b3RevoluteJoint_SetSpringDampingRatio( b3JointId jointId, float dampingRatio )
+{
+	B3_ASSERT( b3IsValidFloat( dampingRatio ) && dampingRatio >= 0.0f );
+	b3World* world = b3GetWorld( jointId.world0 );
+	B3_REC( world, RevoluteJointSetSpringDampingRatio, jointId, dampingRatio );
+	b3JointSim* base = b3GetJointSimCheckType( jointId, b3_revoluteJoint );
+	base->revoluteJoint.dampingRatio = dampingRatio;
+}
+
+float b3RevoluteJoint_GetSpringDampingRatio( b3JointId jointId )
+{
+	b3JointSim* base = b3GetJointSimCheckType( jointId, b3_revoluteJoint );
+	return base->revoluteJoint.dampingRatio;
+}
+
+void b3RevoluteJoint_EnableMotor( b3JointId jointId, bool enableMotor )
+{
+	b3World* world = b3GetWorld( jointId.world0 );
+	B3_REC( world, RevoluteJointEnableMotor, jointId, enableMotor );
+	b3JointSim* base = b3GetJointSimCheckType( jointId, b3_revoluteJoint );
+	if ( enableMotor != base->revoluteJoint.enableMotor )
+	{
+		base->revoluteJoint.motorImpulse = 0.0f;
+	}
+	base->revoluteJoint.enableMotor = enableMotor;
+}
+
+bool b3RevoluteJoint_IsMotorEnabled( b3JointId jointId )
+{
+	b3JointSim* base = b3GetJointSimCheckType( jointId, b3_revoluteJoint );
+	return base->revoluteJoint.enableMotor;
+}
+
+void b3RevoluteJoint_SetMotorSpeed( b3JointId jointId, float motorSpeed )
+{
+	B3_ASSERT( b3IsValidFloat( motorSpeed ) );
+	b3World* world = b3GetWorld( jointId.world0 );
+	B3_REC( world, RevoluteJointSetMotorSpeed, jointId, motorSpeed );
+	b3JointSim* base = b3GetJointSimCheckType( jointId, b3_revoluteJoint );
+	base->revoluteJoint.motorSpeed = motorSpeed;
+}
+
+float b3RevoluteJoint_GetMotorSpeed( b3JointId jointId )
+{
+	b3JointSim* base = b3GetJointSimCheckType( jointId, b3_revoluteJoint );
+	return base->revoluteJoint.motorSpeed;
+}
+
+void b3RevoluteJoint_SetMaxMotorTorque( b3JointId jointId, float maxForce )
+{
+	B3_ASSERT( b3IsValidFloat( maxForce ) && maxForce >= 0.0f );
+	b3World* world = b3GetWorld( jointId.world0 );
+	B3_REC( world, RevoluteJointSetMaxMotorTorque, jointId, maxForce );
+	b3JointSim* base = b3GetJointSimCheckType( jointId, b3_revoluteJoint );
+	base->revoluteJoint.maxMotorTorque = maxForce;
+}
+
+float b3RevoluteJoint_GetMaxMotorTorque( b3JointId jointId )
+{
+	b3JointSim* base = b3GetJointSimCheckType( jointId, b3_revoluteJoint );
+	return base->revoluteJoint.maxMotorTorque;
+}
+
+float b3RevoluteJoint_GetMotorTorque( b3JointId jointId )
+{
+	b3World* world = b3GetWorld( jointId.world0 );
+	b3JointSim* base = b3GetJointSimCheckType( jointId, b3_revoluteJoint );
+	return world->inv_h * base->revoluteJoint.motorImpulse;
+}
+
+b3Vec3 b3GetRevoluteJointForce( b3World* world, b3JointSim* base )
+{
+	b3Vec3 force = b3MulSV( world->inv_h, base->revoluteJoint.linearImpulse );
+	return force;
+}
+
+b3Vec3 b3GetRevoluteJointTorque( b3World* world, b3JointSim* base )
+{
+	b3WorldTransform transformA = b3GetBodyTransform( world, base->bodyIdA );
+	b3RevoluteJoint* joint = &base->revoluteJoint;
+	b3Vec3 axis = b3RotateVector( base->localFrameA.q, b3Vec3_axisZ );
+	axis = b3RotateVector( transformA.q, axis );
+
+	b3Quat relQ = b3InvMulQuat( joint->frameA.q, joint->frameB.q );
+
+	// These are needed for warm starting
+	joint->perpAxisX = b3MulSV(
+		0.5f, b3RotateVector( joint->frameA.q, b3Add( b3MulSV( relQ.s, b3Vec3_axisX ), b3Cross( relQ.v, b3Vec3_axisX ) ) ) );
+	joint->perpAxisY = b3MulSV(
+		0.5f, b3RotateVector( joint->frameA.q, b3Add( b3MulSV( relQ.s, b3Vec3_axisY ), b3Cross( relQ.v, b3Vec3_axisY ) ) ) );
+
+	float axialImpulse = joint->springImpulse + joint->motorImpulse + joint->lowerImpulse - joint->upperImpulse;
+	b3Vec3 angularImpulse =
+		b3Add( b3MulSV( joint->perpImpulse.x, joint->perpAxisX ), b3MulSV( joint->perpImpulse.y, joint->perpAxisY ) );
+	angularImpulse = b3MulAdd( angularImpulse, axialImpulse, joint->rotationAxisZ );
+
+	// todo add pivot torque
+	b3Vec3 impulse = b3MulAdd( angularImpulse,
+							   joint->springImpulse + joint->motorImpulse + joint->lowerImpulse - joint->upperImpulse, axis );
+	b3Vec3 torque = b3MulSV( world->inv_h, impulse );
+	return torque;
+}
+
+void b3PrepareRevoluteJoint( b3JointSim* base, b3StepContext* context )
+{
+	B3_ASSERT( base->type == b3_revoluteJoint );
+
+	b3World* world = context->world;
+
+	b3Body* bodyA = b3Array_Get( world->bodies, base->bodyIdA  );
+	b3Body* bodyB = b3Array_Get( world->bodies, base->bodyIdB  );
+
+	B3_ASSERT( bodyA->setIndex == b3_awakeSet || bodyB->setIndex == b3_awakeSet );
+	b3SolverSet* setA = b3Array_Get( world->solverSets, bodyA->setIndex  );
+	b3SolverSet* setB = b3Array_Get( world->solverSets, bodyB->setIndex  );
+
+	int localIndexA = bodyA->localIndex;
+	int localIndexB = bodyB->localIndex;
+
+	b3BodySim* bodySimA = b3Array_Get( setA->bodySims, localIndexA  );
+	b3BodySim* bodySimB = b3Array_Get( setB->bodySims, localIndexB  );
+
+	base->invMassA = bodySimA->invMass;
+	base->invMassB = bodySimB->invMass;
+	base->invIA = bodySimA->invInertiaWorld;
+	base->invIB = bodySimB->invInertiaWorld;
+
+	b3Matrix3 invInertiaSum = b3AddMM( base->invIA, base->invIB );
+	base->fixedRotation = b3Det( invInertiaSum ) < 1000.0f * FLT_MIN;
+
+	b3RevoluteJoint* joint = &base->revoluteJoint;
+	joint->indexA = bodyA->setIndex == b3_awakeSet ? localIndexA : B3_NULL_INDEX;
+	joint->indexB = bodyB->setIndex == b3_awakeSet ? localIndexB : B3_NULL_INDEX;
+
+	// Compute joint anchor frames with world space rotation, relative to center of mass
+	// Avoid round-off here as much as possible.
+	// b3Vec3 pf = (xf.p - c) + rot(xf.q, f.p)
+	// pf = xf.p - (xf.p + rot(xf.q, lc)) + rot(xf.q, f.p)
+	// pf = rot(xf.q, f.p - lc)
+	joint->frameA.q = b3MulQuat( bodySimA->transform.q, base->localFrameA.q );
+	joint->frameA.p = b3RotateVector( bodySimA->transform.q, b3Sub( base->localFrameA.p, bodySimA->localCenter ) );
+	joint->frameB.q = b3MulQuat( bodySimB->transform.q, base->localFrameB.q );
+	joint->frameB.p = b3RotateVector( bodySimB->transform.q, b3Sub( base->localFrameB.p, bodySimB->localCenter ) );
+
+	joint->deltaCenter = b3SubPos( bodySimB->center, bodySimA->center );
+
+	{
+		// Rotation axis is the z-axis of body A.
+		b3Vec3 rotationAxisZ = b3RotateVector( joint->frameA.q, b3Vec3_axisZ );
+		float k = b3Dot( rotationAxisZ, b3MulMV( invInertiaSum, rotationAxisZ ) );
+		joint->axialMass = k > 0.0f ? 1.0f / k : 0.0f;
+		joint->rotationAxisZ = rotationAxisZ;
+	}
+
+	b3Quat relQ = b3InvMulQuat( joint->frameA.q, joint->frameB.q );
+
+	{
+		// These are needed for warm starting
+		joint->perpAxisX = b3MulSV(
+			0.5f, b3RotateVector( joint->frameA.q, b3Add( b3MulSV( relQ.s, b3Vec3_axisX ), b3Cross( relQ.v, b3Vec3_axisX ) ) ) );
+		joint->perpAxisY = b3MulSV(
+			0.5f, b3RotateVector( joint->frameA.q, b3Add( b3MulSV( relQ.s, b3Vec3_axisY ), b3Cross( relQ.v, b3Vec3_axisY ) ) ) );
+	}
+
+	joint->springSoftness = b3MakeSoft( joint->hertz, joint->dampingRatio, context->h );
+
+	if ( context->enableWarmStarting == false )
+	{
+		joint->linearImpulse = b3Vec3_zero;
+		joint->perpImpulse = (b3Vec2){ 0.0f, 0.0f };
+		joint->motorImpulse = 0.0f;
+		joint->springImpulse = 0.0f;
+		joint->lowerImpulse = 0.0f;
+		joint->upperImpulse = 0.0f;
+	}
+}
+
+void b3WarmStartRevoluteJoint( b3JointSim* base, b3StepContext* context )
+{
+	B3_ASSERT( base->type == b3_revoluteJoint );
+
+	float mA = base->invMassA;
+	float mB = base->invMassB;
+	b3Matrix3 iA = base->invIA;
+	b3Matrix3 iB = base->invIB;
+
+	// dummy state for static bodies
+	b3BodyState dummyState = b3_identityBodyState;
+
+	b3RevoluteJoint* joint = &base->revoluteJoint;
+
+	b3BodyState* stateA = joint->indexA == B3_NULL_INDEX ? &dummyState : context->states + joint->indexA;
+	b3BodyState* stateB = joint->indexB == B3_NULL_INDEX ? &dummyState : context->states + joint->indexB;
+
+	b3Vec3 vA = stateA->linearVelocity;
+	b3Vec3 wA = stateA->angularVelocity;
+	b3Vec3 vB = stateB->linearVelocity;
+	b3Vec3 wB = stateB->angularVelocity;
+
+	b3Vec3 rA = b3RotateVector( stateA->deltaRotation, joint->frameA.p );
+	b3Vec3 rB = b3RotateVector( stateB->deltaRotation, joint->frameB.p );
+
+	float axialImpulse = joint->springImpulse + joint->motorImpulse + joint->lowerImpulse - joint->upperImpulse;
+	b3Vec3 angularImpulse =
+		b3Add( b3MulSV( joint->perpImpulse.x, joint->perpAxisX ), b3MulSV( joint->perpImpulse.y, joint->perpAxisY ) );
+	angularImpulse = b3MulAdd( angularImpulse, axialImpulse, joint->rotationAxisZ );
+
+	vA = b3MulSub( vA, mA, joint->linearImpulse );
+	wA = b3Sub( wA, b3MulMV( iA, b3Add( b3Cross( rA, joint->linearImpulse ), angularImpulse ) ) );
+
+	vB = b3MulAdd( vB, mB, joint->linearImpulse );
+	wB = b3Add( wB, b3MulMV( iB, b3Add( b3Cross( rB, joint->linearImpulse ), angularImpulse ) ) );
+
+	if ( stateA->flags & b3_dynamicFlag )
+	{
+		stateA->linearVelocity = vA;
+		stateA->angularVelocity = wA;
+	}
+
+	if ( stateB->flags & b3_dynamicFlag )
+	{
+		stateB->linearVelocity = vB;
+		stateB->angularVelocity = wB;
+	}
+}
+
+void b3SolveRevoluteJoint( b3JointSim* base, b3StepContext* context, bool useBias )
+{
+	float mA = base->invMassA;
+	float mB = base->invMassB;
+	b3Matrix3 iA = base->invIA;
+	b3Matrix3 iB = base->invIB;
+
+	// dummy state for static bodies
+	b3BodyState dummyState = b3_identityBodyState;
+
+	b3RevoluteJoint* joint = &base->revoluteJoint;
+	b3BodyState* stateA = joint->indexA == B3_NULL_INDEX ? &dummyState : context->states + joint->indexA;
+	b3BodyState* stateB = joint->indexB == B3_NULL_INDEX ? &dummyState : context->states + joint->indexB;
+
+	b3Vec3 vA = stateA->linearVelocity;
+	b3Vec3 wA = stateA->angularVelocity;
+	b3Vec3 vB = stateB->linearVelocity;
+	b3Vec3 wB = stateB->angularVelocity;
+
+	bool fixedRotation = base->fixedRotation;
+	b3Quat quatA = b3MulQuat( stateA->deltaRotation, joint->frameA.q );
+	b3Quat quatB = b3MulQuat( stateB->deltaRotation, joint->frameB.q );
+
+	if ( b3DotQuat( quatA, quatB ) < 0.0f )
+	{
+		// this keeps the rotation angle in the range [-pi, pi]
+		quatB = b3NegateQuat( quatB );
+	}
+
+	b3Quat relQ = b3InvMulQuat( quatA, quatB );
+
+	// Solve spring
+	if ( joint->enableSpring && fixedRotation == false )
+	{
+		// Get the substep relative rotation
+		float targetAngle = joint->targetAngle;
+		float angle = b3GetTwistAngle( relQ );
+		float c = angle - targetAngle;
+
+		float bias = joint->springSoftness.biasRate * c;
+		float massScale = joint->springSoftness.massScale;
+		float impulseScale = joint->springSoftness.impulseScale;
+		float cdot = b3Dot( b3Sub( wB, wA ), joint->rotationAxisZ );
+
+		float deltaImpulse = -massScale * joint->axialMass * ( cdot + bias ) - impulseScale * joint->springImpulse;
+		joint->springImpulse += deltaImpulse;
+
+		wA = b3MulSub( wA, deltaImpulse, b3MulMV( iA, joint->rotationAxisZ ) );
+		wB = b3MulAdd( wB, deltaImpulse, b3MulMV( iB, joint->rotationAxisZ ) );
+	}
+
+	if ( joint->enableMotor && fixedRotation == false )
+	{
+		float cdot = b3Dot( b3Sub( wB, wA ), joint->rotationAxisZ ) - joint->motorSpeed;
+
+		float deltaImpulse = -joint->axialMass * cdot;
+		float newImpulse = joint->motorImpulse + deltaImpulse;
+		float maxImpulse = joint->maxMotorTorque * context->h;
+		newImpulse = b3ClampFloat( newImpulse, -maxImpulse, maxImpulse );
+		deltaImpulse = newImpulse - joint->motorImpulse;
+		joint->motorImpulse = newImpulse;
+
+		wA = b3MulSub( wA, deltaImpulse, b3MulMV( iA, joint->rotationAxisZ ) );
+		wB = b3MulAdd( wB, deltaImpulse, b3MulMV( iB, joint->rotationAxisZ ) );
+	}
+
+	if ( joint->enableLimit && fixedRotation == false )
+	{
+		float angle = b3GetTwistAngle( relQ );
+
+		// todo does an updated twist axis help?
+
+		b3Vec3 axis = joint->rotationAxisZ;
+
+		// Lower limit
+		{
+			float c = angle - 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 = base->constraintSoftness.biasRate * c;
+				massScale = base->constraintSoftness.massScale;
+				impulseScale = base->constraintSoftness.impulseScale;
+			}
+
+			float cdot = b3Dot( b3Sub( wB, wA ), axis );
+			float oldImpulse = joint->lowerImpulse;
+			float deltaImpulse = -massScale * joint->axialMass * ( cdot + bias ) - impulseScale * oldImpulse;
+			joint->lowerImpulse = b3MaxFloat( oldImpulse + deltaImpulse, 0.0f );
+			deltaImpulse = joint->lowerImpulse - oldImpulse;
+
+			wA = b3MulSub( wA, deltaImpulse, b3MulMV( iA, axis ) );
+			wB = b3MulAdd( wB, deltaImpulse, b3MulMV( iB, axis ) );
+		}
+
+		// Upper limit
+		{
+			float c = joint->upperAngle - angle;
+			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 = base->constraintSoftness.biasRate * c;
+				massScale = base->constraintSoftness.massScale;
+				impulseScale = base->constraintSoftness.impulseScale;
+			}
+
+			// sign flipped on Cdot
+			float cdot = b3Dot( b3Sub( wA, wB ), axis );
+			float oldImpulse = joint->upperImpulse;
+			float deltaImpulse = -massScale * joint->axialMass * ( cdot + bias ) - impulseScale * oldImpulse;
+			joint->upperImpulse = b3MaxFloat( oldImpulse + deltaImpulse, 0.0f );
+			deltaImpulse = joint->upperImpulse - oldImpulse;
+
+			// sign flipped on applied impulse
+			wA = b3MulAdd( wA, deltaImpulse, b3MulMV( iA, axis ) );
+			wB = b3MulSub( wB, deltaImpulse, b3MulMV( iB, axis ) );
+		}
+	}
+
+	// Collinearity constraint
+	if ( fixedRotation == false )
+	{
+		b3Vec2 bias = { 0.0f, 0.0f };
+		float massScale = 1.0f;
+		float impulseScale = 0.0f;
+
+		if ( useBias )
+		{
+			b3Vec2 c = { relQ.v.x, relQ.v.y };
+			bias = b3MulSV2( base->constraintSoftness.biasRate, c );
+			massScale = base->constraintSoftness.massScale;
+			impulseScale = base->constraintSoftness.impulseScale;
+		}
+
+		// Collinearity constraint as 2-by-2
+		b3Vec3 perpAxisX =
+			b3MulSV( 0.5f, b3RotateVector( quatA, b3Add( b3MulSV( relQ.s, b3Vec3_axisX ), b3Cross( relQ.v, b3Vec3_axisX ) ) ) );
+		b3Vec3 perpAxisY =
+			b3MulSV( 0.5f, b3RotateVector( quatA, b3Add( b3MulSV( relQ.s, b3Vec3_axisY ), b3Cross( relQ.v, b3Vec3_axisY ) ) ) );
+		joint->perpAxisX = perpAxisX;
+		joint->perpAxisY = perpAxisY;
+
+		b3Matrix3 invInertiaSum = b3AddMM( iA, iB );
+		float kxx = b3Dot( perpAxisX, b3MulMV( invInertiaSum, perpAxisX ) );
+		float kyy = b3Dot( perpAxisY, b3MulMV( invInertiaSum, perpAxisY ) );
+		float kxy = b3Dot( perpAxisX, b3MulMV( invInertiaSum, perpAxisY ) );
+
+		b3Matrix2 k = { { kxx, kxy }, { kxy, kyy } };
+
+		b3Vec3 wRel = b3Sub( wB, wA );
+		b3Vec2 cdot = { b3Dot( wRel, perpAxisX ), b3Dot( wRel, perpAxisY ) };
+		b3Vec2 oldImpulse = joint->perpImpulse;
+		b3Vec2 sol = b3Solve2( k, b3Add2( cdot, bias ) );
+		b3Vec2 deltaImpulse = b3Sub2( b3MulSV2( -massScale, sol ), b3MulSV2( impulseScale, oldImpulse ) );
+		joint->perpImpulse = b3Add2( joint->perpImpulse, deltaImpulse );
+
+		b3Vec3 angularImpulse = b3Add( b3MulSV( deltaImpulse.x, perpAxisX ), b3MulSV( deltaImpulse.y, perpAxisY ) );
+		wA = b3Sub( wA, b3MulMV( iA, angularImpulse ) );
+		wB = b3Add( wB, b3MulMV( iB, angularImpulse ) );
+	}
+
+	// Solve point-to-point constraint
+	{
+		b3Vec3 rA = b3RotateVector( stateA->deltaRotation, joint->frameA.p );
+		b3Vec3 rB = b3RotateVector( stateB->deltaRotation, joint->frameB.p );
+
+		b3Vec3 cdot = b3Sub( b3Sub( b3Add( vB, b3Cross( wB, rB ) ), vA ), b3Cross( wA, rA ) );
+
+		b3Vec3 bias = b3Vec3_zero;
+		float massScale = 1.0f;
+		float impulseScale = 0.0f;
+		if ( useBias )
+		{
+			b3Vec3 dcA = stateA->deltaPosition;
+			b3Vec3 dcB = stateB->deltaPosition;
+
+			b3Vec3 separation = b3Add( b3Add( b3Sub( dcB, dcA ), b3Sub( rB, rA ) ), joint->deltaCenter );
+
+			bias = b3MulSV( base->constraintSoftness.biasRate, separation );
+			massScale = base->constraintSoftness.massScale;
+			impulseScale = base->constraintSoftness.impulseScale;
+		}
+
+		//// K = [(1/m1 + 1/m2) * eye(2) - skew(r1) * invI1 * skew(r1) - skew(r2) * invI2 * skew(r2)]
+		b3Matrix3 sA = b3Skew( rA );
+		b3Matrix3 sB = b3Skew( rB );
+		b3Matrix3 kA = b3MulMM( sA, b3MulMM( base->invIA, sA ) );
+		b3Matrix3 kB = b3MulMM( sB, b3MulMM( base->invIB, sB ) );
+		b3Matrix3 k = b3NegateMat3( b3AddMM( kA, kB ) );
+		k.cx.x += mA + mB;
+		k.cy.y += mA + mB;
+		k.cz.z += mA + mB;
+
+		b3Vec3 b = b3Solve3( k, b3Add( cdot, bias ) );
+
+		b3Vec3 impulse = b3Sub( b3MulSV( -massScale, b ), b3MulSV( impulseScale, joint->linearImpulse ) );
+		joint->linearImpulse = b3Add( joint->linearImpulse, impulse );
+
+		vA = b3MulSub( vA, mA, impulse );
+		wA = b3Sub( wA, b3MulMV( iA, b3Cross( rA, impulse ) ) );
+		vB = b3MulAdd( vB, mB, impulse );
+		wB = b3Add( wB, b3MulMV( iB, b3Cross( rB, impulse ) ) );
+	}
+
+	if ( stateA->flags & b3_dynamicFlag )
+	{
+		stateA->linearVelocity = vA;
+		stateA->angularVelocity = wA;
+	}
+
+	if ( stateB->flags & b3_dynamicFlag )
+	{
+		stateB->linearVelocity = vB;
+		stateB->angularVelocity = wB;
+	}
+}
+
+void b3DrawRevoluteJoint( b3DebugDraw* draw, b3JointSim* base, b3WorldTransform transformA, b3WorldTransform transformB, float scale )
+{
+	b3WorldTransform frameA = b3MulWorldTransforms( transformA, base->localFrameA );
+
+	float length1 = 0.1f * scale;
+	draw->DrawSegmentFcn( frameA.p, b3OffsetPos( frameA.p, b3MulSV( length1, b3RotateVector( frameA.q, b3Vec3_axisX ) ) ), b3_colorRed,
+						  draw->context );
+	draw->DrawSegmentFcn( frameA.p, b3OffsetPos( frameA.p, b3MulSV( length1, b3RotateVector( frameA.q, b3Vec3_axisY ) ) ), b3_colorGreen,
+						  draw->context );
+	draw->DrawSegmentFcn( frameA.p, b3OffsetPos( frameA.p, b3MulSV( length1, b3RotateVector( frameA.q, b3Vec3_axisZ ) ) ), b3_colorBlue,
+						  draw->context );
+
+	b3WorldTransform frameB = b3MulWorldTransforms( transformB, base->localFrameB );
+
+	b3RevoluteJoint* joint = &base->revoluteJoint;
+	enum { kSliceCount = 16 };
+
+	// Twist limit
+	if ( joint->enableLimit )
+	{
+		b3Quat quatA = frameA.q;
+		b3Quat quatB = frameB.q;
+
+		if ( b3DotQuat( quatA, quatB ) < 0.0f )
+		{
+			// this keeps the twist angle in the range [-pi, pi]
+			quatB = b3NegateQuat( quatB );
+		}
+
+		b3Quat relQ = b3InvMulQuat( quatA, quatB );
+
+		const float wedgeRadius = 0.2f * scale;
+		for ( int index = 0; index < kSliceCount; ++index )
+		{
+			float t1 = (float)( index + 0 ) / kSliceCount;
+			float alpha1 = ( 1.0f - t1 ) * joint->lowerAngle + t1 * joint->upperAngle;
+			float t2 = (float)( index + 1 ) / kSliceCount;
+			float alpha2 = ( 1.0f - t2 ) * joint->lowerAngle + t2 * joint->upperAngle;
+
+			b3Vec3 vertex1 = { wedgeRadius * b3Cos( alpha1 ), wedgeRadius * b3Sin( alpha1 ), 0.0f };
+			b3Vec3 vertex2 = { wedgeRadius * b3Cos( alpha2 ), wedgeRadius * b3Sin( alpha2 ), 0.0f };
+
+			if ( index == 0 )
+			{
+				draw->DrawSegmentFcn( frameA.p, b3TransformWorldPoint( frameA, vertex1 ), b3_colorCyan, draw->context );
+			}
+
+			if ( index == kSliceCount - 1 )
+			{
+				draw->DrawSegmentFcn( b3TransformWorldPoint( frameA, vertex2 ), frameA.p, b3_colorCyan, draw->context );
+			}
+			draw->DrawSegmentFcn( b3TransformWorldPoint( frameA, vertex1 ), b3TransformWorldPoint( frameA, vertex2 ), b3_colorCyan,
+								  draw->context );
+		}
+
+		float twistAngle = b3GetTwistAngle( relQ );
+		b3Vec3 p2 = { wedgeRadius * b3Cos( twistAngle ), wedgeRadius * b3Sin( twistAngle ), 0.0f };
+		draw->DrawSegmentFcn( frameA.p, b3TransformWorldPoint( frameA, p2 ), b3_colorYellow, draw->context );
+	}
+}

vendor/box3d/src/src/scheduler.c

@@ -0,0 +1,198 @@
+// SPDX-FileCopyrightText: 2026 Erin Catto
+// SPDX-License-Identifier: MIT
+
+#include "platform.h"
+#include "core.h"
+#include "scheduler.h"
+
+#include "box3d/base.h"
+#include "box3d/constants.h"
+
+#include <stdbool.h>
+#include <stdio.h>
+#include <string.h>
+
+enum b3SchedulerTaskStatus
+{
+	b3_schedulerFree = 0,
+	b3_schedulerPending = 1,
+	b3_schedulerClaimed = 2,
+	b3_schedulerComplete = 3,
+};
+
+typedef struct b3SchedulerTask
+{
+	b3TaskCallback* callback;
+	void* taskContext;
+	b3AtomicInt status;
+} b3SchedulerTask;
+
+typedef struct b3SchedulerWorkerContext
+{
+	struct b3Scheduler* scheduler;
+	int threadIndex;
+} b3SchedulerWorkerContext;
+
+typedef struct b3Scheduler
+{
+	b3Thread* threads[B3_MAX_WORKERS];
+	b3SchedulerWorkerContext workerContexts[B3_MAX_WORKERS];
+
+	// total workers including main thread
+	int workerCount;
+
+	// threads created = workerCount - 1
+	int threadCount;
+
+	b3SchedulerTask tasks[B3_MAX_TASKS];
+	b3AtomicInt nextSlot;
+
+	b3Semaphore* taskSemaphore;
+	b3AtomicInt shutdown;
+} b3Scheduler;
+
+// Try to claim and execute one pending task.
+// Returns true if work was performed, false otherwise.
+static bool b3SchedulerExecuteOne( b3Scheduler* scheduler )
+{
+	int taskCount = b3AtomicLoadInt( &scheduler->nextSlot );
+	for ( int t = 0; t < taskCount; ++t )
+	{
+		b3SchedulerTask* task = scheduler->tasks + t;
+		if ( b3AtomicLoadInt( &task->status ) != b3_schedulerPending )
+		{
+			continue;
+		}
+
+		if ( b3AtomicCompareExchangeInt( &task->status, b3_schedulerPending, b3_schedulerClaimed ) == false )
+		{
+			continue;
+		}
+
+		task->callback( task->taskContext );
+
+		b3AtomicStoreInt( &task->status, b3_schedulerComplete );
+		return true;
+	}
+
+	return false;
+}
+
+// Background worker thread entry point.
+static void b3SchedulerWorkerMain( void* context )
+{
+	b3SchedulerWorkerContext* workerContext = context;
+	b3Scheduler* scheduler = workerContext->scheduler;
+
+	while ( true )
+	{
+		b3WaitSemaphore( scheduler->taskSemaphore );
+
+		if ( b3AtomicLoadInt( &scheduler->shutdown ) != 0 )
+		{
+			break;
+		}
+
+		// Claim and execute all available work
+		while ( b3SchedulerExecuteOne( scheduler ) )
+		{
+		}
+	}
+}
+
+b3Scheduler* b3CreateScheduler( int workerCount )
+{
+	B3_ASSERT( 0 < workerCount && workerCount <= B3_MAX_WORKERS );
+
+	b3Scheduler* scheduler = b3Alloc( sizeof( b3Scheduler ) );
+	memset( scheduler, 0, sizeof( b3Scheduler ) );
+
+	scheduler->workerCount = workerCount;
+	int threadCount = workerCount - 1;
+	scheduler->threadCount = threadCount;
+	scheduler->taskSemaphore = b3CreateSemaphore( 0 );
+	b3AtomicStoreInt( &scheduler->shutdown, 0 );
+	b3AtomicStoreInt( &scheduler->nextSlot, 0 );
+
+	// Background threads use indices 1..workerCount-1.
+	// Main thread uses index 0.
+	for ( int i = 0; i < threadCount; ++i )
+	{
+		scheduler->workerContexts[i].scheduler = scheduler;
+		scheduler->workerContexts[i].threadIndex = i + 1;
+
+		char name[16];
+		snprintf( name, sizeof( name ), "box2d_worker_%02d", i + 1 );
+		scheduler->threads[i] = b3CreateThread( b3SchedulerWorkerMain, scheduler->workerContexts + i, name );
+	}
+
+	return scheduler;
+}
+
+void b3DestroyScheduler( b3Scheduler* scheduler )
+{
+	b3AtomicStoreInt( &scheduler->shutdown, 1 );
+
+	// Wake all background threads so they see the shutdown flag
+	for ( int i = 0; i < scheduler->threadCount; ++i )
+	{
+		b3SignalSemaphore( scheduler->taskSemaphore );
+	}
+
+	for ( int i = 0; i < scheduler->threadCount; ++i )
+	{
+		b3JoinThread( scheduler->threads[i] );
+		scheduler->threads[i] = NULL;
+	}
+
+	b3DestroySemaphore( scheduler->taskSemaphore );
+	b3Free( scheduler, sizeof( b3Scheduler ) );
+}
+
+void b3ResetScheduler( b3Scheduler* scheduler )
+{
+	b3AtomicStoreInt( &scheduler->nextSlot, 0 );
+}
+
+void* b3SchedulerEnqueueTask( b3TaskCallback* task, void* taskContext, void* userContext, const char* name )
+{
+	B3_UNUSED( name );
+	b3Scheduler* scheduler = userContext;
+
+	int slot = b3AtomicFetchAddInt( &scheduler->nextSlot, 1 );
+	B3_ASSERT( slot < B3_MAX_TASKS );
+
+	b3SchedulerTask* schedulerTask = scheduler->tasks + slot;
+	schedulerTask->callback = task;
+	schedulerTask->taskContext = taskContext;
+
+	// Memory fence: status must be published after callback and context are written
+	b3AtomicStoreInt( &schedulerTask->status, b3_schedulerPending );
+
+	// One wake per enqueue is enough: at most one worker picks up each task.
+	b3SignalSemaphore( scheduler->taskSemaphore );
+
+	return schedulerTask;
+}
+
+void b3SchedulerFinishTask( void* userTask, void* userContext )
+{
+	if ( userTask == NULL )
+	{
+		return;
+	}
+
+	b3Scheduler* scheduler = userContext;
+	b3SchedulerTask* waitTask = userTask;
+
+	// Main thread helps execute any available work while waiting for the
+	// target task to complete. This keeps the main thread from idling when
+	// background threads are busy on other tasks from the same phase.
+	while ( b3AtomicLoadInt( &waitTask->status ) != b3_schedulerComplete )
+	{
+		if ( b3SchedulerExecuteOne( scheduler ) == false )
+		{
+			b3Yield();
+		}
+	}
+}

vendor/box3d/src/src/scheduler.h

@@ -0,0 +1,15 @@
+// SPDX-FileCopyrightText: 2026 Erin Catto
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+typedef void b3TaskCallback( void* taskContext );
+typedef struct b3Scheduler b3Scheduler;
+
+b3Scheduler* b3CreateScheduler( int workerCount );
+void b3DestroyScheduler( b3Scheduler* scheduler );
+void b3ResetScheduler( b3Scheduler* scheduler );
+
+// See b3EnqueueTaskCallback and b3FinishTaskCallback
+void* b3SchedulerEnqueueTask( b3TaskCallback* task, void* taskContext, void* userContext, const char* name );
+void b3SchedulerFinishTask( void* userTask, void* userContext );

vendor/box3d/src/src/sensor.c

@@ -0,0 +1,452 @@
+// SPDX-FileCopyrightText: 2025 Erin Catto
+// SPDX-License-Identifier: MIT
+
+#include "sensor.h"
+
+#include "body.h"
+#include "contact.h"
+#include "ctz.h"
+#include "physics_world.h"
+#include "shape.h"
+
+#include "box3d/collision.h"
+
+// for qsort
+#include "parallel_for.h"
+
+#include <stdlib.h>
+
+typedef struct b3SensorQueryContext
+{
+	b3World* world;
+	b3SensorTaskContext* taskContext;
+	b3Sensor* sensor;
+	b3Shape* sensorShape;
+	b3Transform transform;
+} b3SensorQueryContext;
+
+static bool b3OverlapSensor( b3Shape* sensorShape, b3Transform sensorTransform, b3Shape* visitorShape,
+							 b3Transform visitorTransform )
+{
+	b3ShapeType type = sensorShape->type;
+
+	b3ShapeProxy proxy = b3MakeShapeProxy( visitorShape );
+
+	// Get the visitor shape in the frame of the sensor
+	b3Transform relativeTransform = b3InvMulTransforms( sensorTransform, visitorTransform );
+
+	b3Vec3 localPoints[B3_MAX_SHAPE_CAST_POINTS];
+	b3ShapeProxy localProxy;
+
+	localProxy.count = b3MinInt( proxy.count, B3_MAX_SHAPE_CAST_POINTS );
+	for ( int i = 0; i < localProxy.count; ++i )
+	{
+		localPoints[i] = b3TransformPoint( relativeTransform, proxy.points[i] );
+	}
+
+	localProxy.points = localPoints;
+	localProxy.radius = proxy.radius;
+
+	switch ( type )
+	{
+		case b3_capsuleShape:
+			return b3OverlapCapsule( &sensorShape->capsule, b3Transform_identity, &localProxy );
+
+		case b3_compoundShape:
+			return b3OverlapCompound( sensorShape->compound, b3Transform_identity, &localProxy );
+
+		case b3_heightShape:
+			return b3OverlapHeightField( sensorShape->heightField, b3Transform_identity, &localProxy );
+
+		case b3_hullShape:
+			return b3OverlapHull( sensorShape->hull, b3Transform_identity, &localProxy );
+
+		case b3_meshShape:
+			return b3OverlapMesh( &sensorShape->mesh, b3Transform_identity, &localProxy );
+
+		case b3_sphereShape:
+			return b3OverlapSphere( &sensorShape->sphere, b3Transform_identity, &localProxy );
+
+		default:
+			B3_ASSERT( false );
+			return false;
+	}
+}
+
+// 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 b3SensorQueryCallback( int proxyId, uint64_t userData, void* context )
+{
+	B3_UNUSED( proxyId );
+
+	int shapeId = (int)userData;
+
+	b3SensorQueryContext* queryContext = (b3SensorQueryContext*)context;
+	b3Shape* sensorShape = queryContext->sensorShape;
+	int sensorShapeId = sensorShape->id;
+
+	if ( shapeId == sensorShapeId )
+	{
+		return true;
+	}
+
+	b3World* world = queryContext->world;
+	b3Shape* otherShape = b3Array_Get( world->shapes, shapeId );
+
+	// Mesh vs mesh is not supported
+	if ( ( otherShape->type == b3_meshShape || otherShape->type == b3_heightShape ) &&
+		 ( sensorShape->type == b3_meshShape || sensorShape->type == b3_heightShape ) )
+	{
+		return true;
+	}
+
+	// Are sensor events enabled on the other shape?
+	if ( ( otherShape->flags & b3_enableSensorEvents ) == 0 )
+	{
+		return true;
+	}
+
+	// Skip shapes on the same body
+	if ( otherShape->bodyId == sensorShape->bodyId )
+	{
+		return true;
+	}
+
+	// Check filter
+	if ( b3ShouldShapesCollide( sensorShape->filter, otherShape->filter ) == false )
+	{
+		return true;
+	}
+
+	// Custom user filter
+	if ( ( sensorShape->flags & b3_enableCustomFiltering ) || ( otherShape->flags & b3_enableCustomFiltering ) )
+	{
+		b3CustomFilterFcn* customFilterFcn = queryContext->world->customFilterFcn;
+		if ( customFilterFcn != NULL )
+		{
+			b3ShapeId idA = { sensorShapeId + 1, world->worldId, sensorShape->generation };
+			b3ShapeId idB = { shapeId + 1, world->worldId, otherShape->generation };
+			bool shouldCollide = customFilterFcn( idA, idB, queryContext->world->customFilterContext );
+			if ( shouldCollide == false )
+			{
+				return true;
+			}
+		}
+	}
+
+	b3Transform otherTransform = b3ToRelativeTransform( b3GetBodyTransform( world, otherShape->bodyId ), b3Pos_zero );
+
+	bool overlap = b3OverlapSensor( sensorShape, queryContext->transform, otherShape, otherTransform );
+	if ( overlap == false )
+	{
+		return true;
+	}
+
+	// Record the overlap
+	b3Sensor* sensor = queryContext->sensor;
+	b3Visitor* shapeRef = b3Array_Emplace( sensor->overlaps2 );
+	shapeRef->shapeId = shapeId;
+	shapeRef->generation = otherShape->generation;
+
+	return true;
+}
+
+static int b3CompareVisitors( const void* a, const void* b )
+{
+	const b3Visitor* sa = (const b3Visitor*)a;
+	const b3Visitor* sb = (const b3Visitor*)b;
+
+	if ( sa->shapeId < sb->shapeId )
+	{
+		return -1;
+	}
+
+	return 1;
+}
+
+static void b3SensorTask( int startIndex, int endIndex, int workerIndex, void* context )
+{
+	b3TracyCZoneNC( sensor_task, "Overlap", b3_colorBrown, true );
+
+	b3World* world = (b3World*)context;
+	B3_ASSERT( (int)workerIndex < world->workerCount );
+	b3SensorTaskContext* taskContext = world->sensorTaskContexts.data + workerIndex;
+
+	B3_ASSERT( startIndex < endIndex );
+
+	b3DynamicTree* trees = world->broadPhase.trees;
+	for ( int sensorIndex = startIndex; sensorIndex < endIndex; ++sensorIndex )
+	{
+		b3Sensor* sensor = b3Array_Get( world->sensors, sensorIndex );
+		b3Shape* sensorShape = b3Array_Get( world->shapes, sensor->shapeId );
+
+		// Swap overlap arrays
+		b3Array( b3Visitor ) temp = sensor->overlaps1;
+		sensor->overlaps1 = sensor->overlaps2;
+		sensor->overlaps2 = temp;
+		b3Array_Clear( sensor->overlaps2 );
+
+		// Append sensor hits
+		b3Array_Append( sensor->overlaps2, sensor->hits.data, sensor->hits.count );
+
+		// Clear the hits
+		b3Array_Clear( sensor->hits );
+
+		b3Body* body = b3Array_Get( world->bodies, sensorShape->bodyId );
+		if ( body->setIndex == b3_disabledSet || ( sensorShape->flags & b3_enableSensorEvents ) == 0 )
+		{
+			if ( sensor->overlaps1.count != 0 )
+			{
+				// This sensor is dropping all overlaps because it has been disabled.
+				b3SetBit( &taskContext->eventBits, sensorIndex );
+			}
+			continue;
+		}
+
+		b3Transform transform = b3ToRelativeTransform( b3GetBodyTransformQuick( world, body ), b3Pos_zero );
+
+		b3SensorQueryContext queryContext = {
+			.world = world,
+			.taskContext = taskContext,
+			.sensor = sensor,
+			.sensorShape = sensorShape,
+			.transform = transform,
+		};
+
+		B3_ASSERT( sensorShape->sensorIndex == sensorIndex );
+		b3AABB queryBounds = sensorShape->aabb;
+
+		// Query all trees
+		b3DynamicTree_Query( trees + 0, queryBounds, sensorShape->filter.maskBits, false, b3SensorQueryCallback, &queryContext );
+		b3DynamicTree_Query( trees + 1, queryBounds, sensorShape->filter.maskBits, false, b3SensorQueryCallback, &queryContext );
+		b3DynamicTree_Query( trees + 2, queryBounds, sensorShape->filter.maskBits, false, b3SensorQueryCallback, &queryContext );
+
+		// Sort the overlaps to enable finding begin and end events.
+		qsort( sensor->overlaps2.data, sensor->overlaps2.count, sizeof( b3Visitor ), b3CompareVisitors );
+
+		// Remove duplicates from overlaps2 (sorted). Duplicates are possible due to the hit events appended earlier.
+		int uniqueCount = 0;
+		int overlapCount = sensor->overlaps2.count;
+		b3Visitor* overlapData = sensor->overlaps2.data;
+		for ( int i = 0; i < overlapCount; ++i )
+		{
+			if ( uniqueCount == 0 || overlapData[i].shapeId != overlapData[uniqueCount - 1].shapeId )
+			{
+				overlapData[uniqueCount] = overlapData[i];
+				uniqueCount += 1;
+			}
+		}
+		sensor->overlaps2.count = uniqueCount;
+
+		int count1 = sensor->overlaps1.count;
+		int count2 = sensor->overlaps2.count;
+		if ( count1 != count2 )
+		{
+			// something changed
+			b3SetBit( &taskContext->eventBits, sensorIndex );
+		}
+		else
+		{
+			for ( int i = 0; i < count1; ++i )
+			{
+				b3Visitor* s1 = sensor->overlaps1.data + i;
+				b3Visitor* s2 = sensor->overlaps2.data + i;
+
+				if ( s1->shapeId != s2->shapeId || s1->generation != s2->generation )
+				{
+					// something changed
+					b3SetBit( &taskContext->eventBits, sensorIndex );
+					break;
+				}
+			}
+		}
+	}
+
+	b3TracyCZoneEnd( sensor_task );
+}
+
+void b3OverlapSensors( b3World* world )
+{
+	int sensorCount = world->sensors.count;
+	if ( sensorCount == 0 )
+	{
+		return;
+	}
+
+	B3_ASSERT( world->workerCount > 0 );
+
+	b3TracyCZoneNC( overlap_sensors, "Sensors", b3_colorMediumPurple, true );
+
+	for ( int i = 0; i < world->workerCount; ++i )
+	{
+		b3SetBitCountAndClear( &world->sensorTaskContexts.data[i].eventBits, sensorCount );
+	}
+
+	// Parallel-for sensors overlaps
+	int minRange = 16;
+	b3ParallelFor( world, b3SensorTask, sensorCount, minRange, world, "sensors" );
+
+	b3TracyCZoneNC( sensor_state, "Events", b3_colorLightSlateGray, true );
+
+	b3BitSet* bitSet = &world->sensorTaskContexts.data[0].eventBits;
+	for ( int i = 1; i < world->workerCount; ++i )
+	{
+		b3InPlaceUnion( bitSet, &world->sensorTaskContexts.data[i].eventBits );
+	}
+
+	// Iterate sensors bits and publish events
+	// Process sensor 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 = b3CTZ64( word );
+			int sensorIndex = (int)( 64 * k + ctz );
+
+			b3Sensor* sensor = b3Array_Get( world->sensors, sensorIndex );
+			b3Shape* sensorShape = b3Array_Get( world->shapes, sensor->shapeId );
+			b3ShapeId sensorId = { sensor->shapeId + 1, world->worldId, sensorShape->generation };
+
+			int count1 = sensor->overlaps1.count;
+			int count2 = sensor->overlaps2.count;
+			const b3Visitor* refs1 = sensor->overlaps1.data;
+			const b3Visitor* 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 b3Visitor* r1 = refs1 + index1;
+				const b3Visitor* r2 = refs2 + index2;
+				if ( r1->shapeId == r2->shapeId )
+				{
+					if ( r1->generation < r2->generation )
+					{
+						// end
+						b3ShapeId visitorId = { r1->shapeId + 1, world->worldId, r1->generation };
+						b3SensorEndTouchEvent event = {
+							.sensorShapeId = sensorId,
+							.visitorShapeId = visitorId,
+						};
+						b3Array_Push( world->sensorEndEvents[world->endEventArrayIndex], event );
+						index1 += 1;
+					}
+					else if ( r1->generation > r2->generation )
+					{
+						// begin
+						b3ShapeId visitorId = { r2->shapeId + 1, world->worldId, r2->generation };
+						b3SensorBeginTouchEvent event = { sensorId, visitorId };
+						b3Array_Push( world->sensorBeginEvents, event );
+						index2 += 1;
+					}
+					else
+					{
+						// persisted
+						index1 += 1;
+						index2 += 1;
+					}
+				}
+				else if ( r1->shapeId < r2->shapeId )
+				{
+					// end
+					b3ShapeId visitorId = { r1->shapeId + 1, world->worldId, r1->generation };
+					b3SensorEndTouchEvent event = { sensorId, visitorId };
+					b3Array_Push( world->sensorEndEvents[world->endEventArrayIndex], event );
+					index1 += 1;
+				}
+				else
+				{
+					// begin
+					b3ShapeId visitorId = { r2->shapeId + 1, world->worldId, r2->generation };
+					b3SensorBeginTouchEvent event = { sensorId, visitorId };
+					b3Array_Push( world->sensorBeginEvents, event );
+					index2 += 1;
+				}
+			}
+
+			while ( index1 < count1 )
+			{
+				// end
+				const b3Visitor* r1 = refs1 + index1;
+				b3ShapeId visitorId = { r1->shapeId + 1, world->worldId, r1->generation };
+				b3SensorEndTouchEvent event = { sensorId, visitorId };
+				b3Array_Push( world->sensorEndEvents[world->endEventArrayIndex], event );
+				index1 += 1;
+			}
+
+			while ( index2 < count2 )
+			{
+				// begin
+				const b3Visitor* r2 = refs2 + index2;
+				b3ShapeId visitorId = { r2->shapeId + 1, world->worldId, r2->generation };
+				b3SensorBeginTouchEvent event = { sensorId, visitorId };
+				b3Array_Push( world->sensorBeginEvents, event );
+				index2 += 1;
+			}
+
+			// Clear the smallest set bit
+			word = word & ( word - 1 );
+		}
+	}
+
+	b3TracyCZoneEnd( sensor_state );
+	b3TracyCZoneEnd( overlap_sensors );
+}
+
+void b3DestroySensor( b3World* world, b3Shape* sensorShape )
+{
+	b3Sensor* sensor = b3Array_Get( world->sensors, sensorShape->sensorIndex );
+	for ( int i = 0; i < sensor->overlaps2.count; ++i )
+	{
+		b3Visitor* ref = sensor->overlaps2.data + i;
+		b3SensorEndTouchEvent event = {
+			.sensorShapeId =
+				{
+					.index1 = sensorShape->id + 1,
+					.world0 = world->worldId,
+					.generation = sensorShape->generation,
+				},
+			.visitorShapeId =
+				{
+					.index1 = ref->shapeId + 1,
+					.world0 = world->worldId,
+					.generation = ref->generation,
+				},
+		};
+
+		b3Array_Push( world->sensorEndEvents[world->endEventArrayIndex], event );
+	}
+
+	// Destroy sensor
+	b3Array_Destroy( sensor->hits );
+	b3Array_Destroy( sensor->overlaps1 );
+	b3Array_Destroy( sensor->overlaps2 );
+
+	int movedIndex = b3Array_RemoveSwap( world->sensors, sensorShape->sensorIndex );
+	if ( movedIndex != B3_NULL_INDEX )
+	{
+		// Fixup moved sensor
+		b3Sensor* movedSensor = b3Array_Get( world->sensors, sensorShape->sensorIndex );
+		b3Shape* otherSensorShape = b3Array_Get( world->shapes, movedSensor->shapeId );
+		otherSensorShape->sensorIndex = sensorShape->sensorIndex;
+	}
+}

vendor/box3d/src/src/sensor.h

@@ -0,0 +1,41 @@
+// SPDX-FileCopyrightText: 2025 Erin Catto
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include "bitset.h"
+#include "container.h"
+
+typedef struct b3Shape b3Shape;
+typedef struct b3World b3World;
+
+// Used to track shapes that hit sensors using time of impact
+typedef struct b3SensorHit
+{
+	int sensorId;
+	int visitorId;
+} b3SensorHit;
+
+typedef struct b3Visitor
+{
+	int shapeId;
+	uint16_t generation;
+} b3Visitor;
+
+b3DeclareArray( b3Visitor );
+
+typedef struct b3Sensor
+{
+	b3Array( b3Visitor ) hits;
+	b3Array( b3Visitor ) overlaps1;
+	b3Array( b3Visitor ) overlaps2;
+	int shapeId;
+} b3Sensor;
+
+typedef struct b3SensorTaskContext
+{
+	b3BitSet eventBits;
+} b3SensorTaskContext;
+
+void b3OverlapSensors( b3World* world );
+void b3DestroySensor( b3World* world, b3Shape* sensorShape );

vendor/box3d/src/src/shape.h

@@ -0,0 +1,155 @@
+// SPDX-FileCopyrightText: 2025 Erin Catto
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include "math_internal.h"
+
+#include "box3d/types.h"
+
+#include <stdbool.h>
+
+typedef struct b3BroadPhase b3BroadPhase;
+typedef struct b3World b3World;
+
+typedef enum b3ShapeFlags
+{
+	b3_enableSensorEvents = 0x01,
+	b3_enableContactEvents = 0x02,
+	b3_enableCustomFiltering = 0x04,
+	b3_enableHitEvents = 0x08,
+	b3_enablePreSolveEvents = 0x10,
+	b3_enlargedAABB = 0x20,
+	b3_enableSpeculative = 0x40,
+} b3ShapeFlags;
+
+typedef struct b3Shape
+{
+	int id;
+	int bodyId;
+	int prevShapeId;
+	int nextShapeId;
+	int sensorIndex;
+	int proxyKey;
+	b3ShapeType type;
+	float density;
+	float explosionScale;
+	float aabbMargin;
+
+	b3AABB aabb;
+	b3AABB fatAABB;
+	b3Vec3 localCentroid;
+
+	int materialCount;
+	b3SurfaceMaterial material;
+	b3SurfaceMaterial* materials;
+
+	b3Filter filter;
+	void* userData;
+	void* userShape;
+
+	uint32_t nameId;
+	uint16_t generation;
+
+	// b3ShapeFlags
+	uint8_t flags;
+
+	union
+	{
+		b3Capsule capsule;
+		b3Sphere sphere;
+		const b3HullData* hull;
+		b3Mesh mesh;
+		const b3HeightFieldData* heightField;
+		const b3CompoundData* compound;
+	};
+
+} b3Shape;
+
+// A single material shape keeps its material inline. Multi material meshes and compounds own a heap
+// array. Reach the materials the same way for both: a single material shape presents its inline
+// material as a one element array. Do not cache the pointer, the shapes array can move.
+static inline b3SurfaceMaterial* b3GetShapeMaterials( const b3Shape* shape )
+{
+	return shape->materials != NULL ? shape->materials : (b3SurfaceMaterial*)&shape->material;
+}
+
+void b3CreateShapeProxy( b3Shape* shape, b3BroadPhase* bp, b3BodyType type, b3WorldTransform transform, bool forcePairCreation );
+void b3DestroyShapeProxy( b3Shape* shape, b3BroadPhase* bp );
+
+void b3DestroyShapeAllocations( b3World* world, b3Shape* shape );
+
+b3MassData b3ComputeShapeMass( const b3Shape* shape );
+b3ShapeExtent b3ComputeShapeExtent( const b3Shape* shape, b3Vec3 localCenter );
+
+b3AABB b3ComputeSweptSphereAABB( const b3Sphere* shape, b3Transform xf1, b3Transform xf2 );
+b3AABB b3ComputeSweptCapsuleAABB( const b3Capsule* shape, b3Transform xf1, b3Transform xf2 );
+
+b3AABB b3ComputeShapeAABB( const b3Shape* shape, b3Transform transform );
+
+// Conservative world AABB for a shape inflated by extra margin. In double precision mode the
+// box is built in the body local frame, translated by the double origin, and rounded outward.
+b3AABB b3ComputeFatShapeAABB( const b3Shape* shape, b3WorldTransform transform, float extra );
+b3AABB b3ComputeSweptShapeAABB( const b3Shape* shape, const b3Sweep* sweep, float time );
+b3Vec3 b3GetShapeCentroid( const b3Shape* shape );
+float b3GetShapeArea( const b3Shape* shape );
+float b3GetShapeProjectedArea( const b3Shape* shape, b3Vec3 planeNormal );
+uint64_t b3GetShapeUserMaterialId( const b3Shape* shape, int childIndex, int triangleIndex );
+
+b3ShapeProxy b3MakeShapeProxy( const b3Shape* shape );
+b3ShapeProxy b3MakeLocalProxy( const b3ShapeProxy* proxy, b3Transform transform, b3Vec3* buffer );
+b3AABB b3ComputeProxyAABB( const b3ShapeProxy* proxy );
+
+b3CastOutput b3RayCastShape( const b3Shape* shape, b3Transform transform, const b3RayCastInput* input );
+b3CastOutput b3ShapeCastShape( const b3Shape* shape, b3Transform transform, const b3ShapeCastInput* input );
+bool b3OverlapShape( const b3Shape* shape, b3Transform transform, const b3ShapeProxy* proxy );
+
+float b3GetShapeArea( const b3Shape* shape );
+float b3GetShapeProjectedArea( const b3Shape* shape, b3Vec3 planeNormal );
+b3TOIOutput b3ShapeTimeOfImpact( b3Shape* shapeA, b3Shape* shapeB, b3Sweep* sweepA, b3Sweep* sweepB, float maxFraction );
+
+int b3CollideMoverAndSphere( b3PlaneResult* result, const b3Sphere* shape, const b3Capsule* mover );
+int b3CollideMoverAndCapsule( b3PlaneResult* result, const b3Capsule* shape, const b3Capsule* mover );
+int b3CollideMoverAndHull( b3PlaneResult* result, const b3HullData* shape, const b3Capsule* mover );
+int b3CollideMoverAndMesh( b3PlaneResult* planes, int capacity, const b3Mesh* shape, const b3Capsule* mover );
+int b3CollideMoverAndHeightField( b3PlaneResult* results, int capacity, const b3HeightFieldData* shape, const b3Capsule* mover );
+int b3CollideMover( b3PlaneResult* planes, int planeCapacity, const b3Shape* shape, b3Transform transform,
+					const b3Capsule* mover );
+
+// Hull
+int b3FindHullSupportVertex( const b3HullData* hull, b3Vec3 direction );
+int b3FindHullSupportFace( const b3HullData* hull, b3Vec3 direction );
+bool b3IsValidHull( const b3HullData* hull );
+b3AABB b3ComputeSweptHullAABB( const b3HullData* shape, b3Transform xf1, b3Transform xf2 );
+b3ShapeExtent b3ComputeHullExtent( const b3HullData* hull, b3Vec3 origin );
+float b3ComputeHullProjectedArea( const b3HullData* hull, b3Vec3 direction );
+
+// Height field
+b3Triangle b3GetHeightFieldTriangle( const b3HeightFieldData* heightField, int triangleIndex );
+int b3GetHeightFieldMaterial( const b3HeightFieldData* heightField, int triangleIndex );
+
+static inline int b3GetHeightFieldTriangleCount( const b3HeightFieldData* heightField )
+{
+	int cellCount = ( heightField->rowCount - 1 ) * ( heightField->columnCount - 1 );
+	return 2 * cellCount;
+}
+
+// Mesh
+b3Triangle b3GetMeshTriangle( const b3Mesh* mesh, int triangleIndex );
+bool b3IsValidMesh( const b3MeshData* meshData );
+
+static inline bool b3ShouldShapesCollide( b3Filter filterA, b3Filter filterB )
+{
+	if ( filterA.groupIndex == filterB.groupIndex && filterA.groupIndex != 0 )
+	{
+		return filterA.groupIndex > 0;
+	}
+
+	return ( filterA.maskBits & filterB.categoryBits ) != 0 && ( filterA.categoryBits & filterB.maskBits ) != 0;
+}
+
+static inline bool b3ShouldQueryCollide( const b3Filter* shapeFilter, const b3QueryFilter* queryFilter )
+{
+	return ( shapeFilter->categoryBits & queryFilter->maskBits ) != 0 &&
+		   ( shapeFilter->maskBits & queryFilter->categoryBits ) != 0;
+}

vendor/box3d/src/src/simd.c

@@ -0,0 +1,202 @@
+// SPDX-FileCopyrightText: 2026 Erin Catto
+// SPDX-License-Identifier: MIT
+
+// Dirk Gregorius contributed portions of this code
+
+#include "simd.h"
+
+#if defined( B3_SIMD_SSE2 )
+
+#define B3_TRANSPOSE3( C1, C2, C3 )                                                                                              \
+	{                                                                                                                            \
+		b3V32 T1 = _mm_unpacklo_ps( ( C1 ), ( C2 ) );                                                                            \
+		b3V32 T2 = _mm_unpackhi_ps( ( C1 ), ( C2 ) );                                                                            \
+		( C1 ) = _mm_shuffle_ps( ( T1 ), ( C3 ), _MM_SHUFFLE( 0, 0, 1, 0 ) );                                                    \
+		( C2 ) = _mm_shuffle_ps( ( T1 ), ( C3 ), _MM_SHUFFLE( 1, 1, 3, 2 ) );                                                    \
+		( C3 ) = _mm_shuffle_ps( ( T2 ), ( C3 ), _MM_SHUFFLE( 2, 2, 1, 0 ) );                                                    \
+	}
+
+static inline b3V32 b3SplatXV( b3V32 v )
+{
+	return _mm_shuffle_ps( v, v, _MM_SHUFFLE( 0, 0, 0, 0 ) );
+}
+
+static inline b3V32 b3SplatYV( b3V32 v )
+{
+	return _mm_shuffle_ps( v, v, _MM_SHUFFLE( 1, 1, 1, 1 ) );
+}
+
+static inline b3V32 b3SplatZV( b3V32 v )
+{
+	return _mm_shuffle_ps( v, v, _MM_SHUFFLE( 2, 2, 2, 2 ) );
+}
+
+static inline bool b3AnyGreaterEq3V( b3V32 a, b3V32 b )
+{
+	b3V32 v = _mm_cmpge_ps( a, b );
+	return ( _mm_movemask_ps( v ) & 0x07 ) != 0;
+}
+
+static inline b3V32 b3Dot3V( b3V32 a, b3V32 b )
+{
+	b3V32 m = _mm_mul_ps( a, b );
+	b3V32 x = _mm_shuffle_ps( m, m, _MM_SHUFFLE( 0, 0, 0, 0 ) );
+	b3V32 y = _mm_shuffle_ps( m, m, _MM_SHUFFLE( 1, 1, 1, 1 ) );
+	b3V32 z = _mm_shuffle_ps( m, m, _MM_SHUFFLE( 2, 2, 2, 2 ) );
+
+	return _mm_add_ps( _mm_add_ps( x, y ), z );
+}
+
+#else
+
+#define B3_TRANSPOSE3( C1, C2, C3 )                                                                                              \
+	{                                                                                                                            \
+		float temp1 = C1.y;                                                                                                      \
+		float temp2 = C1.z;                                                                                                      \
+		float temp3 = C2.z;                                                                                                      \
+                                                                                                                                 \
+		C1.y = C2.x;                                                                                                             \
+		C1.z = C3.x;                                                                                                             \
+		C2.z = C3.y;                                                                                                             \
+                                                                                                                                 \
+		C2.x = temp1;                                                                                                            \
+		C3.x = temp2;                                                                                                            \
+		C3.y = temp3;                                                                                                            \
+	}
+
+static inline b3V32 b3SplatXV( b3V32 a )
+{
+	return B3_LITERAL( b3V32 ){ a.x, a.x, a.x };
+}
+
+static inline b3V32 b3SplatYV( b3V32 a )
+{
+	return B3_LITERAL( b3V32 ){ a.y, a.y, a.y };
+}
+
+static inline b3V32 b3SplatZV( b3V32 a )
+{
+	return B3_LITERAL( b3V32 ){ a.z, a.z, a.z };
+}
+
+static inline bool b3AnyGreaterEq3V( b3V32 a, b3V32 b )
+{
+	return a.x >= b.x || a.y >= b.y || a.z >= b.z;
+}
+
+static inline b3V32 b3Dot3V( b3V32 a, b3V32 b )
+{
+	float d = a.x * b.x + a.y * b.y + a.z * b.z;
+	return B3_LITERAL( b3V32 ){ d, d, d };
+}
+
+#endif
+
+bool b3TestBoundsTriangleOverlap( b3V32 nodeCenter, b3V32 nodeExtent, b3V32 vertex1, b3V32 vertex2, b3V32 vertex3 )
+{
+	b3V32 two = b3SplatV( 2.0f );
+
+	// Setup triangle
+	vertex1 = b3SubV( vertex1, nodeCenter );
+	vertex2 = b3SubV( vertex2, nodeCenter );
+	vertex3 = b3SubV( vertex3, nodeCenter );
+
+	// Face separation
+	b3V32 triangleMin = b3MinV( vertex1, b3MinV( vertex2, vertex3 ) );
+	b3V32 triangleMax = b3MaxV( vertex1, b3MaxV( vertex2, vertex3 ) );
+
+	b3V32 separation1 = b3SubV( triangleMin, nodeExtent );
+	b3V32 separation2 = b3AddV( triangleMax, nodeExtent );
+
+	b3V32 faceSeparation = b3MaxV( separation1, b3NegV( separation2 ) );
+	if ( b3AnyGreater3V( faceSeparation, b3_zeroV ) )
+	{
+		return false;
+	}
+
+	// SAT: Face separation
+	b3V32 edge1 = b3SubV( vertex2, vertex1 );
+	b3V32 edge2 = b3SubV( vertex3, vertex2 );
+	b3V32 edge3 = b3SubV( vertex1, vertex3 );
+
+	b3V32 normal = b3CrossV( edge1, edge2 );
+
+	b3V32 triangleSeparation = b3SubV( b3AbsV( b3Dot3V( normal, vertex1 ) ), b3Dot3V( b3AbsV( normal ), nodeExtent ) );
+	if ( b3AnyGreater3V( triangleSeparation, b3_zeroV ) )
+	{
+		return false;
+	}
+
+	// SAT: Edge separation
+	b3V32 edgeSeparation1 = b3SubV( b3SubV( b3AbsV( b3CrossV( edge1, b3AddV( vertex1, vertex3 ) ) ), b3AbsV( b3CrossV( edge1, edge3 ) ) ),
+									b3MulV( two, b3ModifiedCrossV( b3AbsV( edge1 ), nodeExtent ) ) );
+	if ( b3AnyGreater3V( edgeSeparation1, b3_zeroV ) )
+	{
+		return false;
+	}
+
+	b3V32 edgeSeparation2 = b3SubV( b3SubV( b3AbsV( b3CrossV( edge2, b3AddV( vertex1, vertex2 ) ) ), b3AbsV( b3CrossV( edge2, edge1 ) ) ),
+									b3MulV( two, b3ModifiedCrossV( b3AbsV( edge2 ), nodeExtent ) ) );
+	if ( b3AnyGreater3V( edgeSeparation2, b3_zeroV ) )
+	{
+		return false;
+	}
+
+	b3V32 edgeSeparation3 = b3SubV( b3SubV( b3AbsV( b3CrossV( edge3, b3AddV( vertex2, vertex3 ) ) ), b3AbsV( b3CrossV( edge3, edge2 ) ) ),
+									b3MulV( two, b3ModifiedCrossV( b3AbsV( edge3 ), nodeExtent ) ) );
+	if ( b3AnyGreater3V( edgeSeparation3, b3_zeroV ) )
+	{
+		return false;
+	}
+
+	return true;
+}
+
+float b3IntersectRayTriangle( b3V32 rayStart, b3V32 rayDelta, b3V32 vertex1, b3V32 vertex2, b3V32 vertex3 )
+{
+	// Test if ray intersects this triangle sharing same calculations for each triangle
+	{
+		b3V32 edge1 = b3SubV( vertex3, vertex2 );
+		b3V32 edge2 = b3SubV( vertex1, vertex3 );
+		b3V32 edge3 = b3SubV( vertex2, vertex1 );
+
+		b3V32 midPoint1 = b3MulV( b3_halfV, b3AddV( vertex2, vertex3 ) );
+		b3V32 midPoint2 = b3MulV( b3_halfV, b3AddV( vertex3, vertex1 ) );
+		b3V32 midPoint3 = b3MulV( b3_halfV, b3AddV( vertex1, vertex2 ) );
+
+		b3V32 normal1 = b3CrossV( edge1, b3SubV( midPoint1, rayStart ) );
+		b3V32 normal2 = b3CrossV( edge2, b3SubV( midPoint2, rayStart ) );
+		b3V32 normal3 = b3CrossV( edge3, b3SubV( midPoint3, rayStart ) );
+		B3_TRANSPOSE3( normal1, normal2, normal3 );
+
+		b3V32 rayDeltaX = b3SplatXV( rayDelta );
+		b3V32 rayDeltaY = b3SplatYV( rayDelta );
+		b3V32 rayDeltaZ = b3SplatZV( rayDelta );
+
+		b3V32 volumes = b3AddV( b3AddV( b3MulV( normal1, rayDeltaX ), b3MulV( normal2, rayDeltaY ) ), b3MulV( normal3, rayDeltaZ ) );
+		if ( b3AnyLess3V( volumes, b3_zeroV ) )
+		{
+			return 1.0f;
+		}
+	}
+
+	// Compute intersection with triangle plane
+	b3V32 edge1 = b3SubV( vertex2, vertex1 );
+	b3V32 edge2 = b3SubV( vertex3, vertex1 );
+	b3V32 normal = b3CrossV( edge1, edge2 );
+
+	b3V32 denominator = b3Dot3V( normal, rayDelta );
+	if ( b3AnyGreaterEq3V( denominator, b3_zeroV ) )
+	{
+		return 1.0f;
+	}
+
+	b3V32 lambda = b3DivV( b3Dot3V( normal, b3SubV( vertex1, rayStart ) ), denominator );
+	if ( b3AnyLessEq3V( lambda, b3_zeroV ) )
+	{
+		return 1.0f;
+	}
+
+	lambda = b3MinV( lambda, b3_oneV );
+	return b3GetXV( lambda );
+}

vendor/box3d/src/src/simd.h

@@ -0,0 +1,357 @@
+// SPDX-FileCopyrightText: 2026 Erin Catto
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include "core.h"
+
+#include <stdbool.h>
+
+#if defined( B3_SIMD_SSE2 )
+
+#include <emmintrin.h>
+
+// wide float holds 4 numbers
+typedef __m128 b3V32;
+
+typedef union b3128
+{
+	b3V32 v;
+	float f[4];
+} b3128;
+
+#if defined( _MSC_VER ) && !defined( __clang__ )
+
+static const b3V32 b3_zeroV = { { 0.0f, 0.0f, 0.0f, 0.0f } };
+static const b3V32 b3_halfV = { { 0.5f, 0.5f, 0.5f, 0.5f } };
+static const b3V32 b3_oneV = { { 1.0f, 1.0f, 1.0f, 1.0f } };
+
+#else
+
+static const b3V32 b3_zeroV = { 0.0f, 0.0f, 0.0f, 0.0f };
+static const b3V32 b3_halfV = { 0.5f, 0.5f, 0.5f, 0.5f };
+static const b3V32 b3_oneV = { 1.0f, 1.0f, 1.0f, 1.0f };
+
+#endif
+
+static inline b3V32 b3AddV( b3V32 a, b3V32 b )
+{
+	return _mm_add_ps( a, b );
+}
+
+static inline b3V32 b3SubV( b3V32 a, b3V32 b )
+{
+	return _mm_sub_ps( a, b );
+}
+
+static inline b3V32 b3MulV( b3V32 a, b3V32 b )
+{
+	return _mm_mul_ps( a, b );
+}
+
+static inline b3V32 b3DivV( b3V32 a, b3V32 b )
+{
+	return _mm_div_ps( a, b );
+}
+
+static inline b3V32 b3NegV( b3V32 a )
+{
+	return _mm_sub_ps( _mm_setzero_ps(), a );
+}
+
+static inline b3V32 b3LoadV( const float* src )
+{
+	// Loads exactly 12 bytes: 8 via movsd, 4 via movss.
+	// Result lane 3 is implicitly zero from the partial loads.
+	__m128 xy = _mm_castpd_ps( _mm_load_sd( (const double*)( src ) ) );
+	__m128 z = _mm_load_ss( src + 2 );
+	return _mm_movelh_ps( xy, z ); // { src[0], src[1], src[2], 0.0f }
+}
+
+static inline b3V32 b3ZeroV( void )
+{
+	return _mm_setzero_ps();
+}
+
+static inline float b3GetXV( b3V32 a )
+{
+	return _mm_cvtss_f32( a );
+}
+
+static inline float b3GetYV( b3V32 a )
+{
+	return _mm_cvtss_f32( _mm_shuffle_ps( a, a, _MM_SHUFFLE( 1, 1, 1, 1 ) ) );
+}
+
+static inline float b3GetZV( b3V32 a )
+{
+	return _mm_cvtss_f32( _mm_shuffle_ps( a, a, _MM_SHUFFLE( 2, 2, 2, 2 ) ) );
+}
+
+static inline float b3GetV( b3V32 a, int index )
+{
+	b3128 b;
+	b.v = a;
+	return b.f[index];
+}
+
+static inline b3V32 b3SplatV( float x )
+{
+	return _mm_set_ps1( x );
+}
+
+static inline b3V32 b3AbsV( b3V32 a )
+{
+	// Abs( V ) = Max( -V, V )
+	b3V32 zero = _mm_setzero_ps();
+	return _mm_max_ps( _mm_sub_ps( zero, a ), a );
+}
+
+static inline b3V32 b3MinV( b3V32 a, b3V32 b )
+{
+	return _mm_min_ps( a, b );
+}
+
+static inline b3V32 b3MaxV( b3V32 a, b3V32 b )
+{
+	return _mm_max_ps( a, b );
+}
+
+static inline b3V32 b3CrossV( b3V32 a, b3V32 b )
+{
+	b3V32 yzX1 = _mm_shuffle_ps( a, a, _MM_SHUFFLE( 3, 0, 2, 1 ) );
+	b3V32 zxY1 = _mm_shuffle_ps( a, a, _MM_SHUFFLE( 3, 1, 0, 2 ) );
+	b3V32 yzX2 = _mm_shuffle_ps( b, b, _MM_SHUFFLE( 3, 0, 2, 1 ) );
+	b3V32 zxY2 = _mm_shuffle_ps( b, b, _MM_SHUFFLE( 3, 1, 0, 2 ) );
+
+	return _mm_sub_ps( _mm_mul_ps( yzX1, zxY2 ), _mm_mul_ps( zxY1, yzX2 ) );
+}
+
+static inline b3V32 b3ModifiedCrossV( b3V32 a, b3V32 b )
+{
+	b3V32 yzX1 = _mm_shuffle_ps( a, a, _MM_SHUFFLE( 3, 0, 2, 1 ) );
+	b3V32 zxY1 = _mm_shuffle_ps( a, a, _MM_SHUFFLE( 3, 1, 0, 2 ) );
+	b3V32 yzX2 = _mm_shuffle_ps( b, b, _MM_SHUFFLE( 3, 0, 2, 1 ) );
+	b3V32 zxY2 = _mm_shuffle_ps( b, b, _MM_SHUFFLE( 3, 1, 0, 2 ) );
+
+	return _mm_add_ps( _mm_mul_ps( yzX1, zxY2 ), _mm_mul_ps( zxY1, yzX2 ) );
+}
+
+static inline bool b3AnyLess3V( b3V32 a, b3V32 b )
+{
+	b3V32 v = _mm_cmplt_ps( a, b );
+	return ( _mm_movemask_ps( v ) & 0x07 ) != 0;
+}
+
+static inline bool b3AnyLessEq3V( b3V32 a, b3V32 b )
+{
+	b3V32 v = _mm_cmple_ps( a, b );
+	return ( _mm_movemask_ps( v ) & 0x07 ) != 0;
+}
+
+static inline bool b3AnyGreater3V( b3V32 a, b3V32 b )
+{
+	b3V32 v = _mm_cmpgt_ps( a, b );
+	return ( _mm_movemask_ps( v ) & 0x07 ) != 0;
+}
+
+static inline bool b3AllLessEq3V( b3V32 a, b3V32 b )
+{
+	b3V32 v = _mm_cmple_ps( a, b );
+	return ( _mm_movemask_ps( v ) & 0x07 ) == 0x07;
+}
+
+#else
+
+// I don't expect the use case of b3V32 to benefit from Neon code.
+// In particular the cross product is very complex in Neon.
+
+// scalar math
+typedef struct b3V32
+{
+	float x, y, z;
+} b3V32;
+
+typedef union b3128
+{
+	b3V32 v;
+	float f[3];
+} b3128;
+
+static const b3V32 b3_zeroV = { 0.0f, 0.0f, 0.0f };
+static const b3V32 b3_halfV = { 0.5f, 0.5f, 0.5f };
+static const b3V32 b3_oneV = { 1.0f, 1.0f, 1.0f };
+
+static inline b3V32 b3AddV( b3V32 a, b3V32 b )
+{
+	return B3_LITERAL( b3V32 ){
+		a.x + b.x,
+		a.y + b.y,
+		a.z + b.z,
+	};
+}
+
+static inline b3V32 b3SubV( b3V32 a, b3V32 b )
+{
+	return B3_LITERAL( b3V32 ){
+		a.x - b.x,
+		a.y - b.y,
+		a.z - b.z,
+	};
+}
+
+static inline b3V32 b3MulV( b3V32 a, b3V32 b )
+{
+	return B3_LITERAL( b3V32 ){
+		a.x * b.x,
+		a.y * b.y,
+		a.z * b.z,
+	};
+}
+
+static inline b3V32 b3DivV( b3V32 a, b3V32 b )
+{
+	return B3_LITERAL( b3V32 ){
+		a.x / b.x,
+		a.y / b.y,
+		a.z / b.z,
+	};
+}
+
+static inline b3V32 b3NegV( b3V32 a )
+{
+	return B3_LITERAL( b3V32 ){
+		-a.x,
+		-a.y,
+		-a.z,
+	};
+}
+
+// Unaligned loads are much faster on recent hardware with little to no penalty
+static inline b3V32 b3LoadV( const float* src )
+{
+	return B3_LITERAL( b3V32 ){ src[0], src[1], src[2] };
+}
+
+static inline b3V32 b3ZeroV( void )
+{
+	return B3_LITERAL( b3V32 ){ 0.0f, 0.0f, 0.0f };
+}
+
+static inline float b3GetXV( b3V32 a )
+{
+	return a.x;
+}
+
+static inline float b3GetYV( b3V32 a )
+{
+	return a.y;
+}
+
+static inline float b3GetZV( b3V32 a )
+{
+	return a.z;
+}
+
+static inline float b3GetV( b3V32 a, int index )
+{
+	b3128 b;
+	b.v = a;
+	return b.f[index];
+}
+
+static inline b3V32 b3SplatV( float x )
+{
+	return B3_LITERAL( b3V32 ){ x, x, x };
+}
+
+static inline b3V32 b3AbsV( b3V32 a )
+{
+	return B3_LITERAL( b3V32 ){
+		a.x < 0.0f ? -a.x : a.x,
+		a.y < 0.0f ? -a.y : a.y,
+		a.z < 0.0f ? -a.z : a.z,
+	};
+}
+
+static inline b3V32 b3MinV( b3V32 a, b3V32 b )
+{
+	return B3_LITERAL( b3V32 ){
+		a.x < b.x ? a.x : b.x,
+		a.y < b.y ? a.y : b.y,
+		a.z < b.z ? a.z : b.z,
+	};
+}
+
+static inline b3V32 b3MaxV( b3V32 a, b3V32 b )
+{
+	return B3_LITERAL( b3V32 ){
+		a.x > b.x ? a.x : b.x,
+		a.y > b.y ? a.y : b.y,
+		a.z > b.z ? a.z : b.z,
+	};
+}
+
+static inline b3V32 b3CrossV( b3V32 a, b3V32 b )
+{
+	b3V32 c;
+	c.x = a.y * b.z - a.z * b.y;
+	c.y = a.z * b.x - a.x * b.z;
+	c.z = a.x * b.y - a.y * b.x;
+	return c;
+}
+
+static inline b3V32 b3ModifiedCrossV( b3V32 a, b3V32 b )
+{
+	b3V32 c;
+	c.x = a.y * b.z + a.z * b.y;
+	c.y = a.z * b.x + a.x * b.z;
+	c.z = a.x * b.y + a.y * b.x;
+	return c;
+}
+
+static inline bool b3AnyLess3V( b3V32 a, b3V32 b )
+{
+	return a.x < b.x || a.y < b.y || a.z < b.z;
+}
+
+static inline bool b3AnyLessEq3V( b3V32 a, b3V32 b )
+{
+	return a.x <= b.x || a.y <= b.y || a.z <= b.z;
+}
+
+static inline bool b3AnyGreater3V( b3V32 a, b3V32 b )
+{
+	return a.x > b.x || a.y > b.y || a.z > b.z;
+}
+
+static inline bool b3AllLessEq3V( b3V32 a, b3V32 b )
+{
+	return a.x <= b.x && a.y <= b.y && a.z <= b.z;
+}
+
+#endif
+
+static inline bool b3TestBoundsOverlap( b3V32 nodeMin1, b3V32 nodeMax1, b3V32 nodeMin2, b3V32 nodeMax2 )
+{
+	b3V32 separation = b3MaxV( b3SubV( nodeMin2, nodeMax1 ), b3SubV( nodeMin1, nodeMax2 ) );
+	return b3AllLessEq3V( separation, b3_zeroV );
+}
+
+// Test a ray for edge separation with an AABB (Gino, p80).
+static inline bool b3TestBoundsRayOverlap( b3V32 nodeMin, b3V32 nodeMax, b3V32 rayStart, b3V32 rayDelta )
+{
+	// Setup node
+	b3V32 nodeCenter = b3MulV( b3_halfV, b3AddV( nodeMin, nodeMax ) );
+	b3V32 nodeExtent = b3SubV( nodeMax, nodeCenter );
+
+	// Setup ray
+	rayStart = b3SubV( rayStart, nodeCenter );
+
+	// SAT: Edge separation
+	b3V32 edgeSeparation = b3SubV( b3AbsV( b3CrossV( rayDelta, rayStart ) ), b3ModifiedCrossV( b3AbsV( rayDelta ), nodeExtent ) );
+	return b3AllLessEq3V( edgeSeparation, b3_zeroV );
+}
+
+bool b3TestBoundsTriangleOverlap( b3V32 nodeCenter, b3V32 nodeExtent, b3V32 vertex1, b3V32 vertex2, b3V32 vertex3 );
+float b3IntersectRayTriangle( b3V32 rayStart, b3V32 rayDelta, b3V32 vertex1, b3V32 vertex2, b3V32 vertex3 );

vendor/box3d/src/src/solver.h

@@ -0,0 +1,306 @@
+// SPDX-FileCopyrightText: 2025 Erin Catto
+// SPDX-License-Identifier: MIT
+
+// Solver work is partitioned into fixed-size blocks that worker threads claim
+// in parallel via atomic CAS on a per-block syncIndex. The descriptor (b3SolverBlock)
+// and the atomic counter sit in a wrapping b3SyncBlock so the CAS-winner can
+// pass the descriptor by value into stage tasks without aliasing the atomic
+// memory other threads are CAS-writing. Three properties of this design
+// matter for performance:
+//
+// 1. Distributed contention. Per-block atomic syncIndex avoids the cache line stampede
+//    that a single shared fetch_add counter would cause. Once a worker
+//    settles into a block range, its CAS targets live in its own L1.
+//
+// 2. Monotonic syncIndex across iterations. Iterative stages (warm start,
+//    solve, relax) reuse the same block array every sub-step iteration.
+//    syncIndex grows each iteration; workers CAS (prev, prev+1), so the
+//    main thread never touches any per-block state between iterations.
+//    Non-iterative stages simply use syncIndex 1.
+//
+// 3. L2 affinity across iterations. Each worker picks a start offset from
+//    its workerIndex, then scans forward and (after wrap) backward:
+//
+//      blocks:   [0] [1] [2] [3] [4] [5] [6] [7]
+//                 ^           ^           ^   ^
+//                 W0          W1          W2  W3   <- start offsets
+//
+//    W0 claims 0,1,2,3 (forward), W1 claims 4,5, etc. Under balanced load
+//    each worker re-hits the same block range every iteration, keeping that
+//    range's hot data resident in its L2. A failed CAS means a neighbour
+//    already claimed the block, so the stealing worker stops -- preserving
+//    locality under mild imbalance while still draining the queue.
+//
+// A graph color stage lays out joint blocks first, then contact blocks:
+//
+//      stage->blocks ->
+//        +------+------+------+------+------+------+------+
+//        |  J0  |  J1  |  J2  |  C0  |  C1  |  C2  |  C3  |
+//        +------+------+------+------+------+------+------+
+//        <-- graphJointBlocks --><---- graphContactBlocks ---->
+//
+// Each block carries its type so the dispatcher routes J-blocks to the joint
+// solver and C-blocks to the SIMD contact solver; both kinds run concurrently
+// within the stage -- no barrier between them. The type tag lives on the
+// block (not the stage) so that mixed-type stages can keep the concurrency.
+//
+// The solver threading model is inspired by https://github.com/bepu/bepuphysics2
+
+#pragma once
+
+#include "container.h"
+#include "core.h"
+
+#include "box3d/math_functions.h"
+
+#include <stdint.h>
+
+typedef struct b3BodySim b3BodySim;
+typedef struct b3BodyState b3BodyState;
+typedef struct b3ContactConstraint b3ContactConstraint;
+typedef struct b3ContactConstraintWide b3ContactConstraintWide;
+typedef struct b3ContactSpec b3ContactSpec;
+typedef struct b3JointSim b3JointSim;
+typedef struct b3Manifold b3Manifold;
+typedef struct b3ManifoldConstraint b3ManifoldConstraint;
+typedef struct b3World b3World;
+
+// Solver stages
+typedef enum b3SolverStageType
+{
+	b3_stagePrepareJoints,
+	b3_stagePrepareWideContacts,
+	b3_stagePrepareContacts,
+	b3_stageIntegrateVelocities,
+	b3_stageWarmStart,
+	b3_stageSolve,
+	b3_stageIntegratePositions,
+	b3_stageRelax,
+	b3_stageRestitution,
+	b3_stageStoreWideImpulses,
+	b3_stageStoreImpulses,
+} b3SolverStageType;
+
+typedef enum b3SolverBlockType
+{
+	// Block for iterating across bodies.
+	b3_bodyBlock,
+
+	// Block for iterating across joints. For prepare.
+	b3_jointBlock,
+
+	// Block for iterating across wide contacts. For prepare and store.
+	b3_wideContactBlock,
+
+	// Block for iterating across contacts. For prepare and store.
+	b3_contactBlock,
+
+	// Block for iterating across joints of a single graph color.
+	b3_graphJointBlock,
+
+	// Block for iterating across wide contacts of a single graph color.
+	b3_graphWideContactBlock,
+
+	// Block for iterating across contacts of a single graph color.
+	b3_graphContactBlock,
+
+	// Block for processing overflow constraints
+	b3_overflowBlock,
+} b3SolverBlockType;
+
+// Solver block describes a multithreaded unit of work.
+typedef struct b3SolverBlock
+{
+	int startIndex;
+	uint16_t count;
+	// b3SolverBlockType
+	uint8_t blockType;
+	uint8_t colorIndex;
+} b3SolverBlock;
+
+// A unit of multithreaded work along with atomic synchronization. The syncIndex grows
+// monotonically allowing the solver block to be re-used across sub-steps.
+typedef struct b3SyncBlock
+{
+	b3SolverBlock block;
+	b3AtomicInt syncIndex;
+} b3SyncBlock;
+
+// 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 b3SolverStage
+{
+	b3SyncBlock* blocks;
+	b3SolverStageType type;
+	int blockCount;
+	uint8_t colorIndex;
+	b3AtomicInt completionCount;
+} b3SolverStage;
+
+// Constraint softness
+typedef struct b3Softness
+{
+	float biasRate;
+	float massScale;
+	float impulseScale;
+} b3Softness;
+
+// Prepare/store run as a flat parallel-for over the whole wide-constraint
+// range. Each span maps a slice of that range back to the owning color's
+// contacts so workers can decode flat wide-slot indices without touching
+// graph state. The spans array has one entry per active color plus a sentinel
+// whose start == wideContactCount.
+typedef struct b3WidePrepareSpan
+{
+	int start;
+	int count;
+	int* contacts;
+} b3WidePrepareSpan;
+
+typedef struct b3ContactPrepareSpan
+{
+	int start;
+	int count;
+	b3ContactSpec* contacts;
+} b3ContactPrepareSpan;
+
+typedef struct b3JointPrepareSpan
+{
+	int start;
+	int count;
+	b3JointSim* joints;
+} b3JointPrepareSpan;
+
+// Context for a time step. Recreated each time step.
+typedef struct b3StepContext
+{
+	// time step
+	float dt;
+
+	// inverse time step (0 if dt == 0).
+	float inv_dt;
+
+	// sub-step
+	float h;
+	float inv_h;
+
+	int subStepCount;
+
+	b3Softness contactSoftness;
+	b3Softness staticSoftness;
+
+	float restitutionThreshold;
+	float maxLinearVelocity;
+
+	struct b3World* world;
+	struct b3ConstraintGraph* graph;
+
+	// shortcut to body states from awake set
+	b3BodyState* states;
+
+	// shortcut to body sims from awake set
+	b3BodySim* 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;
+	b3AtomicInt bulletBodyCount;
+
+	// Contact ids for simplified parallel-for access. Used in narrow-phase.
+	// These contacts may or may not be touching. They are associated with awake bodies.
+	int* awakeContactIndices;
+
+	// Flat view of the wide contact constraint array used by prepare and store.
+	// prepareSpans has activeColorCount + 1 entries, the last being a sentinel
+	// at wideContactCount. wideContactConstraints is the contiguous base
+	// pointer; per-color slices live at colors[i].wideConstraints.
+	struct b3ContactConstraintWide* wideConstraints;
+	b3WidePrepareSpan* widePrepareSpans;
+	int wideContactCount;
+
+	// Similar for mesh/overflow contact constraints
+	struct b3ManifoldConstraint* manifoldConstraints;
+	struct b3ContactConstraint* contactConstraints;
+	b3ContactPrepareSpan* contactPrepareSpans;
+	b3ContactPrepareSpan* overflowSpans;
+	b3JointPrepareSpan* jointPrepareSpans;
+
+	int activeColorCount;
+	int workerCount;
+
+	b3SolverStage* stages;
+	int stageCount;
+	bool enableWarmStarting;
+
+	// padding to prevent false sharing
+	char padding1[64];
+
+	// This atomic is central to multi-threaded solver task synchronization.
+	// It prevents ABA problems by monotonically growing as the solver advances.
+	// This means a delayed worker thread will catch up without repeating already completed
+	// work (causing a race condition).
+	// sync index (16-bits) | stage type (16-bits)
+	b3AtomicU32 atomicSyncBits;
+
+	// padding to prevent false sharing
+	char padding2[64];
+
+	// Race flag claimed by whichever runner reaches b3SolverTask with workerIndex 0 first.
+	// The calling thread of b3World_Step also races for this slot so the orchestrator can
+	// always make progress, regardless of how the user's task system schedules tasks (out
+	// of order, fewer threads than workers, or synchronously inside enqueueTaskFcn). The
+	// loser of the race no-ops as workerIndex 0.
+	b3AtomicInt mainClaimed;
+
+	// padding to prevent false sharing
+	char padding3[64];
+} b3StepContext;
+
+void b3Solve( b3World* world, b3StepContext* stepContext );
+
+static inline b3Softness b3MakeSoft( float hertz, float zeta, float h )
+{
+	if ( hertz == 0.0f )
+	{
+		return B3_LITERAL( b3Softness ){
+			.biasRate = 0.0f,
+			.massScale = 0.0f,
+			.impulseScale = 0.0f,
+		};
+	}
+
+	float omega = 2.0f * B3_PI * hertz;
+	float a1 = 2.0f * zeta + h * omega;
+	float a2 = h * omega * a1;
+	float a3 = 1.0f / ( 1.0f + a2 );
+
+	// bias = w / (2 * z + hw)
+	// massScale = hw * (2 * z + hw) / (1 + hw * (2 * z + hw))
+	// impulseScale = 1 / (1 + hw * (2 * z + hw))
+
+	// If z == 0
+	// bias = 1/h
+	// massScale = hw^2 / (1 + hw^2)
+	// impulseScale = 1 / (1 + hw^2)
+
+	// w -> inf
+	// bias = 1/h
+	// massScale = 1
+	// impulseScale = 0
+
+	// if w = pi / 4  * inv_h
+	// massScale = (pi/4)^2 / (1 + (pi/4)^2) = pi^2 / (16 + pi^2) ~= 0.38
+	// impulseScale = 1 / (1 + (pi/4)^2) = 16 / (16 + pi^2) ~= 0.62
+
+	// In all cases:
+	// massScale + impulseScale == 1
+
+	return ( b3Softness ){
+		.biasRate = omega / a1,
+		.massScale = a2 * a3,
+		.impulseScale = a3,
+	};
+}

vendor/box3d/src/src/solver_set.c

@@ -0,0 +1,633 @@
+// SPDX-FileCopyrightText: 2025 Erin Catto
+// SPDX-License-Identifier: MIT
+
+#include "solver_set.h"
+
+#include "body.h"
+#include "constraint_graph.h"
+#include "contact.h"
+#include "island.h"
+#include "joint.h"
+#include "physics_world.h"
+
+#include <string.h>
+
+void b3DestroySolverSet( b3World* world, int setIndex )
+{
+	b3SolverSet* set = b3Array_Get( world->solverSets, setIndex );
+
+	b3Array_Destroy( set->bodySims );
+	b3Array_Destroy( set->bodyStates );
+	b3Array_Destroy( set->contactIndices );
+	b3Array_Destroy( set->jointSims );
+	b3Array_Destroy( set->islandSims );
+
+	b3FreeId( &world->solverSetIdPool, setIndex );
+	*set = (b3SolverSet){ 0 };
+	set->setIndex = B3_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 b3WakeSolverSet( b3World* world, int setIndex )
+{
+	B3_ASSERT( setIndex >= b3_firstSleepingSet );
+	b3SolverSet* set = b3Array_Get( world->solverSets, setIndex );
+	b3SolverSet* awakeSet = b3Array_Get( world->solverSets, b3_awakeSet );
+	b3SolverSet* disabledSet = b3Array_Get( world->solverSets, b3_disabledSet );
+
+	b3Body* bodies = world->bodies.data;
+
+	int bodyCount = set->bodySims.count;
+	for ( int i = 0; i < bodyCount; ++i )
+	{
+		b3BodySim* simSrc = set->bodySims.data + i;
+
+		b3Body* body = bodies + simSrc->bodyId;
+		B3_ASSERT( body->setIndex == setIndex );
+		body->setIndex = b3_awakeSet;
+		body->localIndex = awakeSet->bodySims.count;
+
+		// Reset sleep timer
+		body->sleepTime = 0.0f;
+
+		b3BodySim* simDst = b3Array_Emplace( awakeSet->bodySims );
+		memcpy( simDst, simSrc, sizeof( b3BodySim ) );
+
+		b3BodyState* state = b3Array_Emplace( awakeSet->bodyStates );
+		*state = b3_identityBodyState;
+		state->flags = body->flags;
+
+		// move non-touching contacts from disabled set to awake set
+		int contactKey = body->headContactKey;
+		while ( contactKey != B3_NULL_INDEX )
+		{
+			int edgeIndex = contactKey & 1;
+			int contactId = contactKey >> 1;
+
+			b3Contact* contact = b3Array_Get( world->contacts, contactId );
+			contactKey = contact->edges[edgeIndex].nextKey;
+
+			if ( contact->setIndex != b3_disabledSet )
+			{
+				B3_ASSERT( contact->setIndex == b3_awakeSet || contact->setIndex == setIndex );
+				continue;
+			}
+
+			int localIndex = contact->localIndex;
+
+			B3_ASSERT( 0 <= localIndex && localIndex < disabledSet->contactIndices.count );
+			B3_ASSERT( disabledSet->contactIndices.data[localIndex] == contactId );
+
+			B3_ASSERT( ( contact->flags & b3_contactTouchingFlag ) == 0 && contact->manifoldCount == 0 );
+
+			contact->setIndex = b3_awakeSet;
+			contact->localIndex = awakeSet->contactIndices.count;
+			b3Array_Push( awakeSet->contactIndices, contactId );
+
+			int movedLocalIndex = b3Array_RemoveSwap( disabledSet->contactIndices, localIndex );
+			if ( movedLocalIndex != B3_NULL_INDEX )
+			{
+				// fix moved element
+				int movedContactIndex = disabledSet->contactIndices.data[localIndex];
+				b3Contact* movedContact = b3Array_Get( world->contacts, movedContactIndex );
+				B3_ASSERT( movedContact->localIndex == movedLocalIndex );
+				movedContact->localIndex = localIndex;
+			}
+		}
+	}
+
+	// Transfer touching contacts from sleeping set to constraint graph.
+	{
+		int contactCount = set->contactIndices.count;
+		for ( int i = 0; i < contactCount; ++i )
+		{
+			int contactIndex = set->contactIndices.data[i];
+			b3Contact* contact = b3Array_Get( world->contacts, contactIndex );
+			B3_ASSERT( contact->flags & b3_contactTouchingFlag );
+			B3_ASSERT( contact->flags & b3_simTouchingFlag );
+			B3_ASSERT( contact->setIndex == setIndex );
+			b3AddContactToGraph( world, contact );
+			contact->setIndex = b3_awakeSet;
+		}
+	}
+
+	// transfer joints from sleeping set to awake set
+	{
+		int jointCount = set->jointSims.count;
+		for ( int i = 0; i < jointCount; ++i )
+		{
+			b3JointSim* jointSim = set->jointSims.data + i;
+			b3Joint* joint = b3Array_Get( world->joints, jointSim->jointId );
+			B3_ASSERT( joint->setIndex == setIndex );
+			b3AddJointToGraph( world, jointSim, joint );
+			joint->setIndex = b3_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 )
+		{
+			b3IslandSim* islandSrc = set->islandSims.data + i;
+			b3Island* island = b3Array_Get( world->islands, islandSrc->islandId );
+			island->setIndex = b3_awakeSet;
+			island->localIndex = awakeSet->islandSims.count;
+			b3IslandSim* islandDst = b3Array_Emplace( awakeSet->islandSims );
+			memcpy( islandDst, islandSrc, sizeof( b3IslandSim ) );
+		}
+	}
+
+	// destroy the sleeping set
+	b3DestroySolverSet( world, setIndex );
+}
+
+void b3TrySleepIsland( b3World* world, int islandId )
+{
+	b3Island* island = b3Array_Get( world->islands, islandId );
+	B3_ASSERT( island->setIndex == b3_awakeSet );
+
+	// Cannot put an island to sleep while it has a pending split and more than one body.
+	if ( island->constraintRemoveCount > 0 && island->bodies.count > 1 )
+	{
+		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 = b3AllocId( &world->solverSetIdPool );
+	if ( sleepSetId == world->solverSets.count )
+	{
+		b3SolverSet set = { 0 };
+		set.setIndex = B3_NULL_INDEX;
+		b3Array_Push( world->solverSets, set );
+	}
+
+	b3SolverSet* sleepSet = b3Array_Get( world->solverSets, sleepSetId );
+	*sleepSet = (b3SolverSet){ 0 };
+
+	// grab awake set after creating the sleep set because the solver set array may have been resized
+	b3SolverSet* awakeSet = b3Array_Get( world->solverSets, b3_awakeSet );
+	b3SolverSet* disabledSet = b3Array_Get( world->solverSets, b3_disabledSet );
+	B3_ASSERT( 0 <= island->localIndex && island->localIndex < awakeSet->islandSims.count );
+
+	sleepSet->setIndex = sleepSetId;
+	b3Array_Reserve( sleepSet->bodySims, island->bodies.count );
+	b3Array_Reserve( sleepSet->contactIndices, island->contacts.count );
+	b3Array_Reserve( sleepSet->jointSims, island->joints.count );
+
+	// move awake bodies to sleeping set
+	// this shuffles around bodies in the awake set
+	{
+		for ( int i = 0; i < island->bodies.count; ++i )
+		{
+			int bodyId = island->bodies.data[i];
+			b3Body* body = b3Array_Get( world->bodies, bodyId );
+			B3_ASSERT( body->setIndex == b3_awakeSet );
+			B3_ASSERT( body->islandId == islandId );
+			B3_ASSERT( body->islandIndex == i );
+
+			// 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 != B3_NULL_INDEX )
+			{
+				b3BodyMoveEvent* moveEvent = b3Array_Get( world->bodyMoveEvents, body->bodyMoveIndex );
+				B3_ASSERT( moveEvent->bodyId.index1 - 1 == bodyId );
+				B3_ASSERT( moveEvent->bodyId.generation == body->generation );
+				moveEvent->fellAsleep = true;
+				body->bodyMoveIndex = B3_NULL_INDEX;
+			}
+
+			int awakeBodyIndex = body->localIndex;
+			b3BodySim* awakeSim = b3Array_Get( awakeSet->bodySims, awakeBodyIndex );
+
+			// move body sim to sleep set
+			int sleepBodyIndex = sleepSet->bodySims.count;
+			b3BodySim* sleepBodySim = b3Array_Emplace( sleepSet->bodySims );
+			memcpy( sleepBodySim, awakeSim, sizeof( b3BodySim ) );
+
+			int movedIndex = b3Array_RemoveSwap( awakeSet->bodySims, awakeBodyIndex );
+			if ( movedIndex != B3_NULL_INDEX )
+			{
+				// fix local index on moved element
+				b3BodySim* movedSim = awakeSet->bodySims.data + awakeBodyIndex;
+				int movedId = movedSim->bodyId;
+				b3Body* movedBody = b3Array_Get( world->bodies, movedId );
+				B3_ASSERT( movedBody->localIndex == movedIndex );
+				movedBody->localIndex = awakeBodyIndex;
+			}
+
+			// destroy state, no need to clone
+			b3Array_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 != B3_NULL_INDEX )
+			{
+				int contactId = contactKey >> 1;
+				int edgeIndex = contactKey & 1;
+
+				b3Contact* contact = b3Array_Get( world->contacts, contactId );
+
+				B3_ASSERT( contact->setIndex == b3_awakeSet || contact->setIndex == b3_disabledSet );
+				contactKey = contact->edges[edgeIndex].nextKey;
+
+				if ( contact->setIndex == b3_disabledSet )
+				{
+					// already moved to disabled set by another body in the island
+					continue;
+				}
+
+				if ( contact->colorIndex != B3_NULL_INDEX )
+				{
+					// contact is touching and will be moved separately
+					B3_ASSERT( ( contact->flags & b3_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;
+				b3Body* otherBody = b3Array_Get( world->bodies, otherBodyId );
+				if ( otherBody->setIndex == b3_awakeSet )
+				{
+					continue;
+				}
+
+				int localIndex = contact->localIndex;
+				B3_ASSERT( awakeSet->contactIndices.data[localIndex] == contactId );
+
+				B3_ASSERT( contact->manifoldCount == 0 );
+				B3_ASSERT( ( contact->flags & b3_contactTouchingFlag ) == 0 );
+
+				// Move the non-touching contact to the disabled set.
+				contact->setIndex = b3_disabledSet;
+
+				// This is mandatory for validation to work correctly
+				contact->localIndex = disabledSet->contactIndices.count;
+				b3Array_Push( disabledSet->contactIndices, contact->contactId );
+
+				int movedLocalIndex = b3Array_RemoveSwap( awakeSet->contactIndices, localIndex );
+				if ( movedLocalIndex != B3_NULL_INDEX )
+				{
+					// fix moved element
+					int movedContactIndex = awakeSet->contactIndices.data[localIndex];
+					b3Contact* movedContact = b3Array_Get( world->contacts, movedContactIndex );
+					B3_ASSERT( movedContact->localIndex == movedLocalIndex );
+					movedContact->localIndex = localIndex;
+				}
+			}
+		}
+	}
+
+	// move touching contacts to sleeping set
+	// this shuffles contacts in the awake set
+	{
+		for ( int i = 0; i < island->contacts.count; ++i )
+		{
+			int contactId = island->contacts.data[i].contactId;
+			b3Contact* contact = b3Array_Get( world->contacts, contactId );
+			B3_ASSERT( contact->setIndex == b3_awakeSet );
+			B3_ASSERT( contact->islandId == islandId );
+			B3_ASSERT( contact->islandIndex == i );
+			int colorIndex = contact->colorIndex;
+			B3_ASSERT( 0 <= colorIndex && colorIndex < B3_GRAPH_COLOR_COUNT );
+
+			b3GraphColor* color = world->constraintGraph.colors + colorIndex;
+
+			// Remove bodies from graph coloring associated with this constraint
+			if ( colorIndex != B3_OVERFLOW_INDEX )
+			{
+				// might clear a bit for a static body, but this has no effect
+				b3ClearBit( &color->bodySet, contact->edges[0].bodyId );
+				b3ClearBit( &color->bodySet, contact->edges[1].bodyId );
+			}
+
+			int sleepContactIndex = sleepSet->contactIndices.count;
+			b3Array_Push( sleepSet->contactIndices, contactId );
+
+			int localIndex = contact->localIndex;
+			if ( ( contact->flags & b3_simMeshContact ) || colorIndex == B3_OVERFLOW_INDEX )
+			{
+				int movedLocalIndex = b3Array_RemoveSwap( color->contacts, localIndex );
+				if ( movedLocalIndex != B3_NULL_INDEX )
+				{
+					// fix moved element
+					int movedContactId = color->contacts.data[localIndex].contactId;
+					b3Contact* movedContact = b3Array_Get( world->contacts, movedContactId );
+					B3_ASSERT( movedContact->localIndex == movedLocalIndex );
+					movedContact->localIndex = localIndex;
+				}
+			}
+			else
+			{
+				int movedLocalIndex = b3Array_RemoveSwap( color->convexContacts, localIndex );
+				if ( movedLocalIndex != B3_NULL_INDEX )
+				{
+					// fix moved element
+					int movedContactId = color->convexContacts.data[localIndex];
+					b3Contact* movedContact = b3Array_Get( world->contacts, movedContactId );
+					B3_ASSERT( movedContact->localIndex == movedLocalIndex );
+					movedContact->localIndex = localIndex;
+				}
+			}
+
+			contact->setIndex = sleepSetId;
+			contact->colorIndex = B3_NULL_INDEX;
+			contact->localIndex = sleepContactIndex;
+		}
+	}
+
+	// move joints
+	// this shuffles joints in the awake set
+	{
+		for ( int i = 0; i < island->joints.count; ++i )
+		{
+			int jointId = island->joints.data[i].jointId;
+			b3Joint* joint = b3Array_Get( world->joints, jointId );
+			B3_ASSERT( joint->setIndex == b3_awakeSet );
+			B3_ASSERT( joint->islandId == islandId );
+			B3_ASSERT( joint->islandIndex == i );
+			int colorIndex = joint->colorIndex;
+			int localIndex = joint->localIndex;
+
+			B3_ASSERT( 0 <= colorIndex && colorIndex < B3_GRAPH_COLOR_COUNT );
+
+			b3GraphColor* color = world->constraintGraph.colors + colorIndex;
+
+			b3JointSim* awakeJointSim = b3Array_Get( color->jointSims, localIndex );
+
+			if ( colorIndex != B3_OVERFLOW_INDEX )
+			{
+				// might clear a bit for a static body, but this has no effect
+				b3ClearBit( &color->bodySet, joint->edges[0].bodyId );
+				b3ClearBit( &color->bodySet, joint->edges[1].bodyId );
+			}
+
+			int sleepJointIndex = sleepSet->jointSims.count;
+			b3JointSim* sleepJointSim = b3Array_Emplace( sleepSet->jointSims );
+			memcpy( sleepJointSim, awakeJointSim, sizeof( b3JointSim ) );
+
+			int movedIndex = b3Array_RemoveSwap( color->jointSims, localIndex );
+			if ( movedIndex != B3_NULL_INDEX )
+			{
+				// fix moved element
+				b3JointSim* movedJointSim = color->jointSims.data + localIndex;
+				int movedId = movedJointSim->jointId;
+				b3Joint* movedJoint = b3Array_Get( world->joints, movedId );
+				B3_ASSERT( movedJoint->localIndex == movedIndex );
+				movedJoint->localIndex = localIndex;
+			}
+
+			joint->setIndex = sleepSetId;
+			joint->colorIndex = B3_NULL_INDEX;
+			joint->localIndex = sleepJointIndex;
+		}
+	}
+
+	// move island struct
+	{
+		B3_ASSERT( island->setIndex == b3_awakeSet );
+
+		int islandIndex = island->localIndex;
+		b3IslandSim* sleepIsland = b3Array_Emplace( sleepSet->islandSims );
+		sleepIsland->islandId = islandId;
+
+		int movedIslandIndex = b3Array_RemoveSwap( awakeSet->islandSims, islandIndex );
+		if ( movedIslandIndex != B3_NULL_INDEX )
+		{
+			// fix index on moved element
+			b3IslandSim* movedIslandSim = awakeSet->islandSims.data + islandIndex;
+			int movedIslandId = movedIslandSim->islandId;
+			b3Island* movedIsland = b3Array_Get( world->islands, movedIslandId );
+			B3_ASSERT( movedIsland->localIndex == movedIslandIndex );
+			movedIsland->localIndex = islandIndex;
+		}
+
+		island->setIndex = sleepSetId;
+		island->localIndex = 0;
+	}
+
+	if ( world->splitIslandId == islandId )
+	{
+		world->splitIslandId = B3_NULL_INDEX;
+	}
+
+	b3ValidateSolverSets( 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 woke.
+void b3MergeSolverSets( b3World* world, int setId1, int setId2 )
+{
+	B3_ASSERT( setId1 >= b3_firstSleepingSet );
+	B3_ASSERT( setId2 >= b3_firstSleepingSet );
+	b3SolverSet* set1 = b3Array_Get( world->solverSets, setId1 );
+	b3SolverSet* set2 = b3Array_Get( world->solverSets, setId2 );
+
+	// Move the fewest number of bodies
+	if ( set1->bodySims.count < set2->bodySims.count )
+	{
+		b3SolverSet* tempSet = set1;
+		set1 = set2;
+		set2 = tempSet;
+
+		int tempId = setId1;
+		setId1 = setId2;
+		setId2 = tempId;
+	}
+
+	// transfer bodies
+	{
+		b3Body* bodies = world->bodies.data;
+		int bodyCount = set2->bodySims.count;
+		for ( int i = 0; i < bodyCount; ++i )
+		{
+			b3BodySim* simSrc = set2->bodySims.data + i;
+
+			b3Body* body = bodies + simSrc->bodyId;
+			B3_ASSERT( body->setIndex == setId2 );
+			body->setIndex = setId1;
+			body->localIndex = set1->bodySims.count;
+
+			b3BodySim* simDst = b3Array_Emplace( set1->bodySims );
+			memcpy( simDst, simSrc, sizeof( b3BodySim ) );
+		}
+	}
+
+	// transfer contacts
+	{
+		int contactCount = set2->contactIndices.count;
+		for ( int i = 0; i < contactCount; ++i )
+		{
+			int contactIndex = set2->contactIndices.data[i];
+			b3Contact* contact = b3Array_Get( world->contacts, contactIndex );
+			B3_ASSERT( contact->setIndex == setId2 );
+			contact->setIndex = setId1;
+			contact->localIndex = set1->contactIndices.count;
+			b3Array_Push( set1->contactIndices, contactIndex );
+		}
+	}
+
+	// transfer joints
+	{
+		int jointCount = set2->jointSims.count;
+		for ( int i = 0; i < jointCount; ++i )
+		{
+			b3JointSim* jointSrc = set2->jointSims.data + i;
+
+			b3Joint* joint = b3Array_Get( world->joints, jointSrc->jointId );
+			B3_ASSERT( joint->setIndex == setId2 );
+			joint->setIndex = setId1;
+			joint->localIndex = set1->jointSims.count;
+
+			b3JointSim* jointDst = b3Array_Emplace( set1->jointSims );
+			memcpy( jointDst, jointSrc, sizeof( b3JointSim ) );
+		}
+	}
+
+	// transfer islands
+	{
+		int islandCount = set2->islandSims.count;
+		for ( int i = 0; i < islandCount; ++i )
+		{
+			b3IslandSim* islandSrc = set2->islandSims.data + i;
+			int islandId = islandSrc->islandId;
+
+			b3Island* island = b3Array_Get( world->islands, islandId );
+			island->setIndex = setId1;
+			island->localIndex = set1->islandSims.count;
+
+			b3IslandSim* islandDst = b3Array_Emplace( set1->islandSims );
+			memcpy( islandDst, islandSrc, sizeof( b3IslandSim ) );
+		}
+	}
+
+	// destroy the merged set
+	// Warning: need to be careful not to destroy things that got transferred, like triangle caches.
+	b3DestroySolverSet( world, setId2 );
+
+	b3ValidateSolverSets( world );
+}
+
+void b3TransferBody( b3World* world, b3SolverSet* targetSet, b3SolverSet* sourceSet, b3Body* body )
+{
+	if ( targetSet == sourceSet )
+	{
+		return;
+	}
+
+	int sourceIndex = body->localIndex;
+	b3BodySim* sourceSim = b3Array_Get( sourceSet->bodySims, sourceIndex );
+
+	int targetIndex = targetSet->bodySims.count;
+	b3BodySim* targetSim = b3Array_Emplace( targetSet->bodySims );
+	memcpy( targetSim, sourceSim, sizeof( b3BodySim ) );
+
+	// Clear transient body flags
+	targetSim->flags &= ~( b3_isFast | b3_isSpeedCapped | b3_hadTimeOfImpact );
+
+	// Remove body sim from solver set that owns it
+	int movedIndex = b3Array_RemoveSwap( sourceSet->bodySims, sourceIndex );
+	if ( movedIndex != B3_NULL_INDEX )
+	{
+		// Fix moved body index
+		b3BodySim* movedSim = sourceSet->bodySims.data + sourceIndex;
+		int movedId = movedSim->bodyId;
+		b3Body* movedBody = b3Array_Get( world->bodies, movedId );
+		B3_ASSERT( movedBody->localIndex == movedIndex );
+		movedBody->localIndex = sourceIndex;
+	}
+
+	if ( sourceSet->setIndex == b3_awakeSet )
+	{
+		b3Array_RemoveSwap( sourceSet->bodyStates, sourceIndex );
+	}
+	else if ( targetSet->setIndex == b3_awakeSet )
+	{
+		b3BodyState* state = b3Array_Emplace( targetSet->bodyStates );
+		*state = b3_identityBodyState;
+		state->flags = body->flags;
+	}
+
+	body->setIndex = targetSet->setIndex;
+	body->localIndex = targetIndex;
+}
+
+void b3TransferJoint( b3World* world, b3SolverSet* targetSet, b3SolverSet* sourceSet, b3Joint* joint )
+{
+	if ( targetSet == sourceSet )
+	{
+		return;
+	}
+
+	int localIndex = joint->localIndex;
+	int colorIndex = joint->colorIndex;
+
+	// Retrieve source.
+	b3JointSim* sourceSim;
+	if ( sourceSet->setIndex == b3_awakeSet )
+	{
+		B3_ASSERT( 0 <= colorIndex && colorIndex < B3_GRAPH_COLOR_COUNT );
+		b3GraphColor* color = world->constraintGraph.colors + colorIndex;
+
+		sourceSim = b3Array_Get( color->jointSims, localIndex );
+	}
+	else
+	{
+		B3_ASSERT( colorIndex == B3_NULL_INDEX );
+		sourceSim = b3Array_Get( sourceSet->jointSims, localIndex );
+	}
+
+	// Create target and copy. Fix joint.
+	if ( targetSet->setIndex == b3_awakeSet )
+	{
+		b3AddJointToGraph( world, sourceSim, joint );
+		joint->setIndex = b3_awakeSet;
+	}
+	else
+	{
+		joint->setIndex = targetSet->setIndex;
+		joint->localIndex = targetSet->jointSims.count;
+		joint->colorIndex = B3_NULL_INDEX;
+
+		b3JointSim* targetSim = b3Array_Emplace( targetSet->jointSims );
+		memcpy( targetSim, sourceSim, sizeof( b3JointSim ) );
+	}
+
+	// Destroy source.
+	if ( sourceSet->setIndex == b3_awakeSet )
+	{
+		b3RemoveJointFromGraph( world, joint->edges[0].bodyId, joint->edges[1].bodyId, colorIndex, localIndex );
+	}
+	else
+	{
+		int movedIndex = b3Array_RemoveSwap( sourceSet->jointSims, localIndex );
+		if ( movedIndex != B3_NULL_INDEX )
+		{
+			// fix swapped element
+			b3JointSim* movedJointSim = sourceSet->jointSims.data + localIndex;
+			int movedId = movedJointSim->jointId;
+			b3Joint* movedJoint = b3Array_Get( world->joints, movedId );
+			movedJoint->localIndex = localIndex;
+		}
+	}
+}

vendor/box3d/src/src/solver_set.h

@@ -0,0 +1,67 @@
+// SPDX-FileCopyrightText: 2025 Erin Catto
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include "container.h"
+#include "contact.h"
+
+typedef struct b3Body b3Body;
+typedef struct b3BodySim b3BodySim;
+typedef struct b3BodyState b3BodyState;
+typedef struct b3IslandSim b3IslandSim;
+typedef struct b3Joint b3Joint;
+typedef struct b3JointSim b3JointSim;
+typedef struct b3World b3World;
+
+b3DeclareArray( b3BodySim );
+b3DeclareArray( b3BodyState );
+b3DeclareArray( b3JointSim );
+b3DeclareArray( b3IslandSim );
+
+// This holds solver set data. The following sets are used:
+// - static set for all static bodies and joints between static bodies
+// - 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 b3SolverSet
+{
+	// Body array. Empty for unused set.
+	b3Array( b3BodySim ) bodySims;
+
+	// Body state only exists for active set
+	b3Array( b3BodyState ) bodyStates;
+
+	// This holds sleeping/disabled joints. Empty for static/active set.
+	b3Array( b3JointSim ) jointSims;
+
+	// This holds all contacts for sleeping sets.
+	// This holds non-touching contacts for the awake set.
+	// This should be empty for the static and disabled sets.
+	b3Array( int ) contactIndices;
+
+	// 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.
+	b3Array( b3IslandSim ) islandSims;
+
+	// Aligns with b3World::solverSetIdPool. Used to create a stable id for body/contact/joint/islands.
+	int setIndex;
+} b3SolverSet;
+
+void b3DestroySolverSet( b3World* world, int setIndex );
+
+void b3WakeSolverSet( b3World* world, int setIndex );
+void b3TrySleepIsland( b3World* world, int islandId );
+
+// Merge set 2 into set 1 then destroy set 2.
+// Warning: any pointers into these sets will be orphaned.
+void b3MergeSolverSets( b3World* world, int setIndex1, int setIndex2 );
+
+void b3TransferBody( b3World* world, b3SolverSet* targetSet, b3SolverSet* sourceSet, b3Body* body );
+void b3TransferJoint( b3World* world, b3SolverSet* targetSet, b3SolverSet* sourceSet, b3Joint* joint );

vendor/box3d/src/src/sphere.c

@@ -0,0 +1,247 @@
+// SPDX-FileCopyrightText: 2026 Erin Catto
+// SPDX-License-Identifier: MIT
+
+// Dirk Gregorius contributed portions of this code
+
+#include "math_internal.h"
+#include "shape.h"
+
+#include "box3d/base.h"
+#include "box3d/collision.h"
+#include "box3d/constants.h"
+
+b3MassData b3ComputeSphereMass( const b3Sphere* shape, float density )
+{
+	b3Vec3 center = shape->center;
+	float radius = shape->radius;
+
+	float volume = 4.0f / 3.0f * B3_PI * radius * radius * radius;
+	float mass = volume * density;
+	float ixx = 0.4f * mass * radius * radius;
+
+	b3MassData out;
+	out.mass = mass;
+	out.center = center;
+
+	// Inertia about the center of mass
+	out.inertia = b3MakeDiagonalMatrix( ixx, ixx, ixx );
+	return out;
+}
+
+b3AABB b3ComputeSphereAABB( const b3Sphere* shape, b3Transform transform )
+{
+	b3Vec3 center = b3TransformPoint( transform, shape->center );
+	float radius = shape->radius;
+	b3Vec3 extent = { radius, radius, radius };
+	return ( b3AABB ){ b3Sub( center, extent ), b3Add( center, extent ) };
+}
+
+b3AABB b3ComputeSweptSphereAABB( const b3Sphere* shape, b3Transform xf1, b3Transform xf2 )
+{
+	b3Vec3 r = { shape->radius, shape->radius, shape->radius };
+	b3Vec3 center1 = b3TransformPoint( xf1, shape->center );
+	b3Vec3 center2 = b3TransformPoint( xf2, shape->center );
+	return ( b3AABB ){ b3Sub( b3Min( center1, center2 ), r ), b3Add( b3Max( center1, center2 ), r ) };
+}
+
+bool b3OverlapSphere( const b3Sphere* shape, b3Transform shapeTransform, const b3ShapeProxy* proxy )
+{
+	b3DistanceInput input;
+	input.proxyA = ( b3ShapeProxy ){ &shape->center, 1, shape->radius };
+	input.proxyB = *proxy;
+	input.transform = b3InvMulTransforms( shapeTransform, b3Transform_identity );
+	input.useRadii = true;
+
+	b3SimplexCache cache = { 0 };
+	b3DistanceOutput output = b3ShapeDistance( &input, &cache, NULL, 0 );
+	return output.distance < B3_OVERLAP_SLOP;
+}
+
+// Precision Improvements for Ray / Sphere Intersection - Ray Tracing Gems 2019
+// http://www.codercorner.com/blog/?p=321
+b3CastOutput b3RayCastSphere(const b3Sphere* shape, const b3RayCastInput* input )
+{
+	B3_ASSERT( b3IsValidRay( input ) );
+	b3CastOutput output = { 0 };
+
+	b3Vec3 p = shape->center;
+
+	// Shift ray so sphere center is the origin
+	b3Vec3 s = b3Sub( input->origin, p );
+
+	float r = shape->radius;
+	float rr = r * r;
+
+	float length;
+	b3Vec3 d = b3GetLengthAndNormalize( &length, input->translation );
+	if ( length == 0.0f )
+	{
+		// zero length ray
+
+		if ( b3LengthSquared( s ) < rr )
+		{
+			// initial overlap
+			output.point = input->origin;
+			output.hit = true;
+		}
+
+		return output;
+	}
+
+	// Find closest point on ray to origin
+
+	// solve: dot(s + t * d, d) = 0
+	float t = -b3Dot( s, d );
+
+	// c is the closest point on the line to the origin
+	b3Vec3 c = b3MulAdd( s, t, d );
+
+	float cc = b3Dot( c, c );
+
+	if ( cc > rr )
+	{
+		// closest point is outside the sphere
+		return output;
+	}
+
+	// Pythagoras
+	float h = sqrtf( rr - cc );
+
+	float fraction = t - h;
+
+	if ( fraction < 0.0f || input->maxFraction * length < fraction )
+	{
+		// intersection is point outside the range of the ray segment
+
+		if ( b3LengthSquared( s ) < rr )
+		{
+			// initial overlap
+			output.point = input->origin;
+			output.hit = true;
+		}
+
+		return output;
+	}
+
+	b3Vec3 hitPoint = b3MulAdd( s, fraction, d );
+
+	output.fraction = fraction / length;
+
+	if ( output.fraction > input->maxFraction )
+	{
+		b3Log( "sphere input fraction = %g, output fraction = %g", input->maxFraction, output.fraction );
+		output.fraction = input->maxFraction;
+	}
+
+	output.normal = b3Normalize( hitPoint );
+	output.point = b3MulAdd( p, shape->radius, output.normal );
+	output.hit = true;
+
+	return output;
+}
+
+// Precision Improvements for Ray / Sphere Intersection - Ray Tracing Gems 2019
+// http://www.codercorner.com/blog/?p=321
+// This will do interior hits.
+b3CastOutput b3RayCastHollowSphere( const b3Sphere* sphere, const b3RayCastInput* input )
+{
+	b3Vec3 p = sphere->center;
+
+	b3CastOutput output = { 0 };
+
+	// Shift ray so sphere center is the origin
+	b3Vec3 s = b3Sub( input->origin, p );
+	b3Vec3 d = b3Normalize( input->translation );
+
+	// Find closest point on ray to origin
+
+	// solve: dot(s + t * d, d) = 0
+	float t = -b3Dot( s, d );
+
+	// c is the closest point on the line to the origin
+	b3Vec3 c = b3MulAdd( s, t, d );
+
+	float cc = b3Dot( c, c );
+	float r = sphere->radius;
+	float rr = r * r;
+
+	if ( cc > rr )
+	{
+		// closest point is outside the sphere
+		return output;
+	}
+
+	// Pythagoras
+	float h = sqrtf( rr - cc );
+
+	float fraction = t - h;
+
+	if ( fraction < 0.0f )
+	{
+		fraction = t + h;
+	}
+
+	if ( fraction < 0.0f )
+	{
+		// behind the ray
+		return output;
+	}
+
+	if (fraction > input->maxFraction)
+	{
+		return output;
+	}
+
+	b3Vec3 hitPoint = b3MulAdd( s, fraction, d );
+
+	output.fraction = fraction;
+	output.normal = b3Normalize( hitPoint );
+	output.point = b3MulAdd( p, sphere->radius, output.normal );
+	output.hit = true;
+
+	return output;
+}
+
+b3CastOutput b3ShapeCastSphere( const b3Sphere* sphere, const b3ShapeCastInput* input )
+{
+	b3ShapeCastPairInput pairInput;
+	pairInput.proxyA = ( b3ShapeProxy ){ &sphere->center, 1, sphere->radius };
+	pairInput.proxyB = input->proxy;
+	pairInput.transform = b3Transform_identity;
+	pairInput.translationB = input->translation;
+	pairInput.maxFraction = input->maxFraction;
+	pairInput.canEncroach = input->canEncroach;
+
+	b3CastOutput output = b3ShapeCast( &pairInput );
+	return output;
+}
+
+int b3CollideMoverAndSphere( b3PlaneResult* result, const b3Sphere* shape, const b3Capsule* mover )
+{
+	float totalRadius = mover->radius + shape->radius;
+	b3Vec3 closest = b3PointToSegmentDistance( mover->center1, mover->center2, shape->center );
+
+	// The normal points from the sphere toward the mover.
+	float distance;
+	b3Vec3 normal = b3GetLengthAndNormalize( &distance, b3Sub( closest, shape->center ) );
+
+	if ( distance > totalRadius )
+	{
+		return 0;
+	}
+
+	float linearSlop = B3_LINEAR_SLOP;
+	if ( distance < linearSlop )
+	{
+		// Deep overlap: the mover axis passes through the sphere center, so no
+		// direction is preferred. Push perpendicular to the mover axis.
+		float length;
+		b3Vec3 axis = b3GetLengthAndNormalize( &length, b3Sub( mover->center2, mover->center1 ) );
+		normal = length > linearSlop ? b3Perp( axis ) : b3Vec3_axisY;
+		distance = 0.0f;
+	}
+
+	b3Plane plane = { normal, totalRadius - distance };
+	*result = ( b3PlaneResult ){ plane, shape->center };
+	return 1;
+}

vendor/box3d/src/src/spherical_joint.c

@@ -0,0 +1,745 @@
+// SPDX-FileCopyrightText: 2025 Erin Catto
+// SPDX-License-Identifier: MIT
+
+#include "body.h"
+#include "core.h"
+#include "joint.h"
+#include "math_internal.h"
+#include "physics_world.h"
+#include "solver.h"
+#include "solver_set.h"
+#include "recording.h"
+
+// needed for dll export
+#include "box3d/box3d.h"
+
+void b3SphericalJoint_EnableConeLimit( b3JointId jointId, bool enableLimit )
+{
+	b3World* world = b3GetWorld( jointId.world0 );
+	B3_REC( world, SphericalJointEnableConeLimit, jointId, enableLimit );
+	b3JointSim* base = b3GetJointSimCheckType( jointId, b3_sphericalJoint );
+	if ( enableLimit != base->sphericalJoint.enableConeLimit )
+	{
+		base->sphericalJoint.swingImpulse = 0.0f;
+	}
+	base->sphericalJoint.enableConeLimit = enableLimit;
+}
+
+bool b3SphericalJoint_IsConeLimitEnabled( b3JointId jointId )
+{
+	b3JointSim* base = b3GetJointSimCheckType( jointId, b3_sphericalJoint );
+	return base->sphericalJoint.enableConeLimit;
+}
+
+float b3SphericalJoint_GetConeLimit( b3JointId jointId )
+{
+	b3JointSim* base = b3GetJointSimCheckType( jointId, b3_sphericalJoint );
+	return base->sphericalJoint.coneAngle;
+}
+
+void b3SphericalJoint_SetConeLimit( b3JointId jointId, float angleRadians )
+{
+	B3_ASSERT( b3IsValidFloat( angleRadians ) && 0 <= angleRadians && angleRadians <= 0.5f * B3_PI );
+	b3World* world = b3GetWorld( jointId.world0 );
+	B3_REC( world, SphericalJointSetConeLimit, jointId, angleRadians );
+	b3JointSim* base = b3GetJointSimCheckType( jointId, b3_sphericalJoint );
+	base->sphericalJoint.coneAngle = angleRadians;
+}
+
+float b3SphericalJoint_GetConeAngle( b3JointId jointId )
+{
+	b3World* world = b3GetWorld( jointId.world0 );
+	b3JointSim* base = b3GetJointSimCheckType( jointId, b3_sphericalJoint );
+	b3WorldTransform transformA = b3GetBodyTransform( world, base->bodyIdA );
+	b3WorldTransform transformB = b3GetBodyTransform( world, base->bodyIdB );
+
+	b3Quat quatA = b3MulQuat( transformA.q, base->localFrameA.q );
+	b3Quat quatB = b3MulQuat( transformB.q, base->localFrameB.q );
+
+	if ( b3DotQuat( quatA, quatB ) < 0.0f )
+	{
+		// this keeps the swing angle in the range [0, pi]
+		quatB = b3NegateQuat( quatB );
+	}
+
+	b3Quat relQ = b3InvMulQuat( quatA, quatB );
+
+	return b3GetSwingAngle( relQ );
+}
+
+void b3SphericalJoint_EnableTwistLimit( b3JointId jointId, bool enableLimit )
+{
+	b3World* world = b3GetWorld( jointId.world0 );
+	B3_REC( world, SphericalJointEnableTwistLimit, jointId, enableLimit );
+	b3JointSim* base = b3GetJointSimCheckType( jointId, b3_sphericalJoint );
+	if ( enableLimit != base->sphericalJoint.enableTwistLimit )
+	{
+		base->sphericalJoint.lowerTwistImpulse = 0.0f;
+		base->sphericalJoint.upperTwistImpulse = 0.0f;
+	}
+	base->sphericalJoint.enableTwistLimit = enableLimit;
+}
+
+bool b3SphericalJoint_IsTwistLimitEnabled( b3JointId jointId )
+{
+	b3JointSim* base = b3GetJointSimCheckType( jointId, b3_sphericalJoint );
+	return base->sphericalJoint.enableTwistLimit;
+}
+
+float b3SphericalJoint_GetLowerTwistLimit( b3JointId jointId )
+{
+	b3JointSim* base = b3GetJointSimCheckType( jointId, b3_sphericalJoint );
+	return base->sphericalJoint.lowerTwistAngle;
+}
+
+float b3SphericalJoint_GetUpperTwistLimit( b3JointId jointId )
+{
+	b3JointSim* base = b3GetJointSimCheckType( jointId, b3_sphericalJoint );
+	return base->sphericalJoint.upperTwistAngle;
+}
+
+void b3SphericalJoint_SetTwistLimits( b3JointId jointId, float lowerLimitRadians, float upperLimitRadians )
+{
+	B3_ASSERT( b3IsValidFloat( lowerLimitRadians ) && b3IsValidFloat( upperLimitRadians ) );
+	b3World* world = b3GetWorld( jointId.world0 );
+	B3_REC( world, SphericalJointSetTwistLimits, jointId, lowerLimitRadians, upperLimitRadians );
+
+	float lowerAngle = b3MinFloat( lowerLimitRadians, upperLimitRadians );
+	float upperAngle = b3MaxFloat( lowerLimitRadians, upperLimitRadians );
+
+	b3JointSim* base = b3GetJointSimCheckType( jointId, b3_sphericalJoint );
+	base->sphericalJoint.lowerTwistAngle = b3ClampFloat( lowerAngle, -0.99f * B3_PI, 0.99f * B3_PI );
+	base->sphericalJoint.upperTwistAngle = b3ClampFloat( upperAngle, -0.99f * B3_PI, 0.99f * B3_PI );
+}
+
+float b3SphericalJoint_GetTwistAngle( b3JointId jointId )
+{
+	b3World* world = b3GetWorld( jointId.world0 );
+	b3JointSim* base = b3GetJointSimCheckType( jointId, b3_sphericalJoint );
+	b3WorldTransform transformA = b3GetBodyTransform( world, base->bodyIdA );
+	b3WorldTransform transformB = b3GetBodyTransform( world, base->bodyIdB );
+
+	b3Quat quatA = b3MulQuat( transformA.q, base->localFrameA.q );
+	b3Quat quatB = b3MulQuat( transformB.q, base->localFrameB.q );
+
+	if ( b3DotQuat( quatA, quatB ) < 0.0f )
+	{
+		// this keeps the twist angle in the range [-pi, pi]
+		quatB = b3NegateQuat( quatB );
+	}
+
+	b3Quat relQ = b3InvMulQuat( quatA, quatB );
+
+	return b3GetTwistAngle( relQ );
+}
+
+void b3SphericalJoint_EnableSpring( b3JointId jointId, bool enableSpring )
+{
+	b3World* world = b3GetWorld( jointId.world0 );
+	B3_REC( world, SphericalJointEnableSpring, jointId, enableSpring );
+	b3JointSim* base = b3GetJointSimCheckType( jointId, b3_sphericalJoint );
+	if ( enableSpring != base->sphericalJoint.enableSpring )
+	{
+		base->sphericalJoint.springImpulse = b3Vec3_zero;
+	}
+	base->sphericalJoint.enableSpring = enableSpring;
+}
+
+bool b3SphericalJoint_IsSpringEnabled( b3JointId jointId )
+{
+	b3JointSim* base = b3GetJointSimCheckType( jointId, b3_sphericalJoint );
+	return base->sphericalJoint.enableSpring;
+}
+
+void b3SphericalJoint_SetTargetRotation( b3JointId jointId, b3Quat targetRotation )
+{
+	B3_ASSERT( b3IsValidQuat( targetRotation ) );
+	b3World* world = b3GetWorld( jointId.world0 );
+	B3_REC( world, SphericalJointSetTargetRotation, jointId, targetRotation );
+	b3JointSim* base = b3GetJointSimCheckType( jointId, b3_sphericalJoint );
+	base->sphericalJoint.targetRotation = targetRotation;
+}
+
+b3Quat b3SphericalJoint_GetTargetRotation( b3JointId jointId )
+{
+	b3JointSim* base = b3GetJointSimCheckType( jointId, b3_sphericalJoint );
+	return base->sphericalJoint.targetRotation;
+}
+
+void b3SphericalJoint_SetSpringHertz( b3JointId jointId, float hertz )
+{
+	B3_ASSERT( b3IsValidFloat( hertz ) && hertz >= 0.0f );
+	b3World* world = b3GetWorld( jointId.world0 );
+	B3_REC( world, SphericalJointSetSpringHertz, jointId, hertz );
+	b3JointSim* base = b3GetJointSimCheckType( jointId, b3_sphericalJoint );
+	base->sphericalJoint.hertz = hertz;
+}
+
+float b3SphericalJoint_GetSpringHertz( b3JointId jointId )
+{
+	b3JointSim* base = b3GetJointSimCheckType( jointId, b3_sphericalJoint );
+	return base->sphericalJoint.hertz;
+}
+
+void b3SphericalJoint_SetSpringDampingRatio( b3JointId jointId, float dampingRatio )
+{
+	B3_ASSERT( b3IsValidFloat( dampingRatio ) && dampingRatio >= 0.0f );
+	b3World* world = b3GetWorld( jointId.world0 );
+	B3_REC( world, SphericalJointSetSpringDampingRatio, jointId, dampingRatio );
+	b3JointSim* base = b3GetJointSimCheckType( jointId, b3_sphericalJoint );
+	base->sphericalJoint.dampingRatio = dampingRatio;
+}
+
+float b3SphericalJoint_GetSpringDampingRatio( b3JointId jointId )
+{
+	b3JointSim* base = b3GetJointSimCheckType( jointId, b3_sphericalJoint );
+	return base->sphericalJoint.dampingRatio;
+}
+
+void b3SphericalJoint_EnableMotor( b3JointId jointId, bool enableMotor )
+{
+	b3World* world = b3GetWorld( jointId.world0 );
+	B3_REC( world, SphericalJointEnableMotor, jointId, enableMotor );
+	b3JointSim* base = b3GetJointSimCheckType( jointId, b3_sphericalJoint );
+	if ( enableMotor != base->sphericalJoint.enableMotor )
+	{
+		base->sphericalJoint.motorImpulse = b3Vec3_zero;
+	}
+	base->sphericalJoint.enableMotor = enableMotor;
+}
+
+bool b3SphericalJoint_IsMotorEnabled( b3JointId jointId )
+{
+	b3JointSim* base = b3GetJointSimCheckType( jointId, b3_sphericalJoint );
+	return base->sphericalJoint.enableMotor;
+}
+
+void b3SphericalJoint_SetMotorVelocity( b3JointId jointId, b3Vec3 motorVelocity )
+{
+	B3_ASSERT( b3IsValidVec3( motorVelocity ) );
+	b3World* world = b3GetWorld( jointId.world0 );
+	B3_REC( world, SphericalJointSetMotorVelocity, jointId, motorVelocity );
+	b3JointSim* base = b3GetJointSimCheckType( jointId, b3_sphericalJoint );
+	base->sphericalJoint.motorVelocity = motorVelocity;
+}
+
+b3Vec3 b3SphericalJoint_GetMotorVelocity( b3JointId jointId )
+{
+	b3JointSim* base = b3GetJointSimCheckType( jointId, b3_sphericalJoint );
+	return base->sphericalJoint.motorVelocity;
+}
+
+void b3SphericalJoint_SetMaxMotorTorque( b3JointId jointId, float maxForce )
+{
+	B3_ASSERT( b3IsValidFloat( maxForce ) && maxForce >= 0.0f );
+	b3World* world = b3GetWorld( jointId.world0 );
+	B3_REC( world, SphericalJointSetMaxMotorTorque, jointId, maxForce );
+	b3JointSim* base = b3GetJointSimCheckType( jointId, b3_sphericalJoint );
+	base->sphericalJoint.maxMotorTorque = maxForce;
+}
+
+float b3SphericalJoint_GetMaxMotorTorque( b3JointId jointId )
+{
+	b3JointSim* base = b3GetJointSimCheckType( jointId, b3_sphericalJoint );
+	return base->sphericalJoint.maxMotorTorque;
+}
+
+b3Vec3 b3SphericalJoint_GetMotorTorque( b3JointId jointId )
+{
+	b3World* world = b3GetWorld( jointId.world0 );
+	b3JointSim* base = b3GetJointSimCheckType( jointId, b3_sphericalJoint );
+	return b3MulSV( world->inv_h, base->sphericalJoint.motorImpulse );
+}
+
+b3Vec3 b3GetSphericalJointForce( b3World* world, b3JointSim* base )
+{
+	b3Vec3 force = b3MulSV( world->inv_h, base->sphericalJoint.linearImpulse );
+	return force;
+}
+
+b3Vec3 b3GetSphericalJointTorque( b3World* world, b3JointSim* base )
+{
+	b3WorldTransform xfA = b3GetBodyTransform( world, base->bodyIdA );
+	b3WorldTransform xfB = b3GetBodyTransform( world, base->bodyIdB );
+	b3Quat qA = b3MulQuat( xfA.q, base->localFrameA.q );
+	b3Quat qB = b3MulQuat( xfB.q, base->localFrameB.q );
+
+	// Cone axis is the z-axis of body A.
+	b3Vec3 coneAxis = b3RotateVector( qA, b3Vec3_axisZ );
+	b3Vec3 twistAxis = b3RotateVector( qB, b3Vec3_axisZ );
+	b3Vec3 swingAxis = b3Normalize( b3Cross( coneAxis, twistAxis ) );
+
+	b3SphericalJoint* joint = &base->sphericalJoint;
+	b3Vec3 impulse = b3Add( joint->springImpulse, joint->motorImpulse );
+	impulse = b3MulAdd( impulse, joint->lowerTwistImpulse - joint->upperTwistImpulse, twistAxis );
+	impulse = b3MulAdd( impulse, joint->swingImpulse, swingAxis );
+	b3Vec3 torque = b3MulSV( world->inv_h, impulse );
+	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 ]
+// K = J * invM * transpose(J)
+// transpose(skew(r)) = -skew(r)
+// K = diag(1/m1 + 1/m2) - r1_skew * invI1 * r1_skew - r2_skew * invI2 * r2_skew
+
+// r_skew = R * skew(r_local) * RT
+// invI = R * invI_local * RT
+// r_skew * invI * r_skew = R * skew(r_local) * RT * R * invI_local * RT * R * r_skew * RT
+//                        = R * ( skew(r_local) * invI_local * skew(r_local) ) * RT
+
+void b3PrepareSphericalJoint( b3JointSim* base, b3StepContext* context )
+{
+	B3_ASSERT( base->type == b3_sphericalJoint );
+
+	b3World* world = context->world;
+
+	b3Body* bodyA = b3Array_Get( world->bodies, base->bodyIdA  );
+	b3Body* bodyB = b3Array_Get( world->bodies, base->bodyIdB  );
+
+	B3_ASSERT( bodyA->setIndex == b3_awakeSet || bodyB->setIndex == b3_awakeSet );
+	b3SolverSet* setA = b3Array_Get( world->solverSets, bodyA->setIndex  );
+	b3SolverSet* setB = b3Array_Get( world->solverSets, bodyB->setIndex  );
+
+	int localIndexA = bodyA->localIndex;
+	int localIndexB = bodyB->localIndex;
+
+	b3BodySim* bodySimA = b3Array_Get( setA->bodySims, localIndexA  );
+	b3BodySim* bodySimB = b3Array_Get( setB->bodySims, localIndexB  );
+
+	base->invMassA = bodySimA->invMass;
+	base->invMassB = bodySimB->invMass;
+	base->invIA = bodySimA->invInertiaWorld;
+	base->invIB = bodySimB->invInertiaWorld;
+
+	b3Matrix3 invInertiaSum = b3AddMM( base->invIA, base->invIB );
+	base->fixedRotation = b3Det( invInertiaSum ) < 1000.0f * FLT_MIN;
+
+	b3SphericalJoint* joint = &base->sphericalJoint;
+	joint->indexA = bodyA->setIndex == b3_awakeSet ? localIndexA : B3_NULL_INDEX;
+	joint->indexB = bodyB->setIndex == b3_awakeSet ? localIndexB : B3_NULL_INDEX;
+
+	// Compute joint anchor frames with world space rotation, relative to center of mass
+	joint->frameA.q = b3MulQuat( bodySimA->transform.q, base->localFrameA.q );
+	joint->frameA.p = b3RotateVector( bodySimA->transform.q, b3Sub( base->localFrameA.p, bodySimA->localCenter ) );
+	joint->frameB.q = b3MulQuat( bodySimB->transform.q, base->localFrameB.q );
+	joint->frameB.p = b3RotateVector( bodySimB->transform.q, b3Sub( base->localFrameB.p, bodySimB->localCenter ) );
+
+	joint->deltaCenter = b3SubPos( bodySimB->center, bodySimA->center );
+
+	// Cone axis is the z-axis of body A.
+	b3Vec3 coneAxis = b3RotateVector( joint->frameA.q, b3Vec3_axisZ );
+
+	// Twist axis is the z-axis of body B.
+	b3Vec3 twistAxis = b3RotateVector( joint->frameB.q, b3Vec3_axisZ );
+
+	if ( joint->enableConeLimit )
+	{
+		// Swing axis may be zero
+		b3Vec3 swingAxis = b3Normalize( b3Cross( coneAxis, twistAxis ) );
+		float k = b3Dot( swingAxis, b3MulMV( invInertiaSum, swingAxis ) );
+		joint->swingMass = k > 0.0f ? 1.0f / k : 0.0f;
+		joint->swingAxis = swingAxis;
+	}
+
+	if ( joint->enableTwistLimit )
+	{
+		b3Quat relQ = b3InvMulQuat( joint->frameA.q, joint->frameB.q );
+		float tanThetaOver2 = sqrtf( ( relQ.v.x * relQ.v.x + relQ.v.y * relQ.v.y ) / ( relQ.v.z * relQ.v.z + relQ.s * relQ.s ) );
+
+		// todo verify this Jacobian using a finite difference, unit test?
+		b3Vec3 swingAxis = b3Normalize( b3Cross( coneAxis, twistAxis ) );
+		b3Vec3 perpAxis = b3Cross( swingAxis, coneAxis );
+		b3Vec3 twistJacobian = b3MulAdd( coneAxis, tanThetaOver2, perpAxis );
+		float k = b3Dot( twistJacobian, b3MulMV( invInertiaSum, twistJacobian ) );
+		joint->twistMass = k > 0.0f ? 1.0f / k : 0.0f;
+		joint->twistJacobian = twistJacobian;
+	}
+
+	if ( base->fixedRotation == false )
+	{
+		joint->rotationMass = b3InvertMatrix( invInertiaSum );
+	}
+	else
+	{
+		joint->rotationMass = b3Mat3_zero;
+	}
+
+	joint->springSoftness = b3MakeSoft( joint->hertz, joint->dampingRatio, context->h );
+
+	if ( context->enableWarmStarting == false )
+	{
+		joint->linearImpulse = b3Vec3_zero;
+		joint->motorImpulse = b3Vec3_zero;
+		joint->springImpulse = b3Vec3_zero;
+		joint->swingImpulse = 0.0f;
+		joint->lowerTwistImpulse = 0.0f;
+		joint->upperTwistImpulse = 0.0f;
+	}
+}
+
+void b3WarmStartSphericalJoint( b3JointSim* base, b3StepContext* context )
+{
+	B3_ASSERT( base->type == b3_sphericalJoint );
+
+	float mA = base->invMassA;
+	float mB = base->invMassB;
+	b3Matrix3 iA = base->invIA;
+	b3Matrix3 iB = base->invIB;
+
+	// dummy state for static bodies
+	b3BodyState dummyState = b3_identityBodyState;
+
+	b3SphericalJoint* joint = &base->sphericalJoint;
+
+	b3BodyState* stateA = joint->indexA == B3_NULL_INDEX ? &dummyState : context->states + joint->indexA;
+	b3BodyState* stateB = joint->indexB == B3_NULL_INDEX ? &dummyState : context->states + joint->indexB;
+
+	b3Vec3 vA = stateA->linearVelocity;
+	b3Vec3 wA = stateA->angularVelocity;
+	b3Vec3 vB = stateB->linearVelocity;
+	b3Vec3 wB = stateB->angularVelocity;
+
+	b3Vec3 rA = b3RotateVector( stateA->deltaRotation, joint->frameA.p );
+	b3Vec3 rB = b3RotateVector( stateB->deltaRotation, joint->frameB.p );
+
+	b3Vec3 angularImpulse = b3Add( joint->springImpulse, joint->motorImpulse );
+	angularImpulse = b3MulSub( angularImpulse, joint->swingImpulse, joint->swingAxis );
+	angularImpulse = b3MulAdd( angularImpulse, joint->lowerTwistImpulse - joint->upperTwistImpulse, joint->twistJacobian );
+
+	vA = b3MulSub( vA, mA, joint->linearImpulse );
+	wA = b3Sub( wA, b3MulMV( iA, b3Add( b3Cross( rA, joint->linearImpulse ), angularImpulse ) ) );
+
+	vB = b3MulAdd( vB, mB, joint->linearImpulse );
+	wB = b3Add( wB, b3MulMV( iB, b3Add( b3Cross( rB, joint->linearImpulse ), angularImpulse ) ) );
+
+	if ( stateA->flags & b3_dynamicFlag )
+	{
+		stateA->linearVelocity = vA;
+		stateA->angularVelocity = wA;
+	}
+
+	if ( stateB->flags & b3_dynamicFlag )
+	{
+		stateB->linearVelocity = vB;
+		stateB->angularVelocity = wB;
+	}
+}
+
+void b3SolveSphericalJoint( b3JointSim* base, b3StepContext* context, bool useBias )
+{
+	float mA = base->invMassA;
+	float mB = base->invMassB;
+	b3Matrix3 iA = base->invIA;
+	b3Matrix3 iB = base->invIB;
+
+	// dummy state for static bodies
+	b3BodyState dummyState = b3_identityBodyState;
+
+	b3SphericalJoint* joint = &base->sphericalJoint;
+	b3BodyState* stateA = joint->indexA == B3_NULL_INDEX ? &dummyState : context->states + joint->indexA;
+	b3BodyState* stateB = joint->indexB == B3_NULL_INDEX ? &dummyState : context->states + joint->indexB;
+
+	b3Vec3 vA = stateA->linearVelocity;
+	b3Vec3 wA = stateA->angularVelocity;
+	b3Vec3 vB = stateB->linearVelocity;
+	b3Vec3 wB = stateB->angularVelocity;
+
+	bool fixedRotation = base->fixedRotation;
+	b3Quat quatA = b3MulQuat( stateA->deltaRotation, joint->frameA.q );
+	b3Quat quatB = b3MulQuat( stateB->deltaRotation, joint->frameB.q );
+
+	b3Quat relQ = b3InvMulQuat( quatA, quatB );
+
+	// Solve spring
+	if ( joint->enableSpring && fixedRotation == false )
+	{
+		// Rotation constraint error
+		b3Vec3 deltaRotation = b3DeltaQuatToRotation( relQ, joint->targetRotation );
+		b3Vec3 c = b3Neg( b3RotateVector( quatA, deltaRotation ) );
+
+		b3Vec3 bias = b3MulSV( joint->springSoftness.biasRate, c );
+		float massScale = joint->springSoftness.massScale;
+		float impulseScale = joint->springSoftness.impulseScale;
+		b3Vec3 cdot = b3Sub( wB, wA );
+
+		b3Vec3 impulse = b3MulSub( b3MulSV( -massScale, b3MulMV( joint->rotationMass, b3Add( cdot, bias ) ) ),
+								   impulseScale, joint->springImpulse );
+		joint->springImpulse = b3Add( joint->springImpulse, impulse );
+
+		wA = b3Sub( wA, b3MulMV( iA, impulse ) );
+		wB = b3Add( wB, b3MulMV( iB, impulse ) );
+	}
+
+	if ( joint->enableMotor && fixedRotation == false )
+	{
+		b3Vec3 cdot = b3Sub( wB, wA );
+
+		b3Vec3 lambda = b3Neg( b3MulMV( joint->rotationMass, b3Sub( cdot, joint->motorVelocity ) ) );
+		b3Vec3 newImpulse = b3Add( joint->motorImpulse, lambda );
+		float length = b3Length( newImpulse );
+		float maxImpulse = joint->maxMotorTorque * context->h;
+		if ( length > maxImpulse )
+		{
+			newImpulse = b3MulSV( maxImpulse / length, newImpulse );
+		}
+
+		lambda = b3Sub( newImpulse, joint->motorImpulse );
+		joint->motorImpulse = newImpulse;
+
+		wA = b3Sub( wA, b3MulMV( iA, lambda ) );
+		wB = b3Add( wB, b3MulMV( iB, lambda ) );
+	}
+
+	if ( joint->enableTwistLimit && fixedRotation == false )
+	{
+		float twistAngle = b3GetTwistAngle( relQ );
+
+		// todo does an updated twist axis help?
+
+		b3Vec3 twistJacobian = joint->twistJacobian;
+
+		// Lower limit
+		{
+			float c = twistAngle - joint->lowerTwistAngle;
+			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 = base->constraintSoftness.biasRate * c;
+				massScale = base->constraintSoftness.massScale;
+				impulseScale = base->constraintSoftness.impulseScale;
+			}
+
+			float cdot = b3Dot( b3Sub( wB, wA ), twistJacobian );
+			float oldImpulse = joint->lowerTwistImpulse;
+			float deltaImpulse = -massScale * joint->twistMass * ( cdot + bias ) - impulseScale * oldImpulse;
+			joint->lowerTwistImpulse = b3MaxFloat( oldImpulse + deltaImpulse, 0.0f );
+			deltaImpulse = joint->lowerTwistImpulse - oldImpulse;
+
+			wA = b3MulSub( wA, deltaImpulse, b3MulMV( iA, twistJacobian ) );
+			wB = b3MulAdd( wB, deltaImpulse, b3MulMV( iB, twistJacobian ) );
+		}
+
+		// Upper limit
+		{
+			float c = joint->upperTwistAngle - twistAngle;
+			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 = base->constraintSoftness.biasRate * c;
+				massScale = base->constraintSoftness.massScale;
+				impulseScale = base->constraintSoftness.impulseScale;
+			}
+
+			// sign flipped on Cdot
+			float cdot = b3Dot( b3Sub( wA, wB ), twistJacobian );
+			float oldImpulse = joint->upperTwistImpulse;
+			float deltaImpulse = -massScale * joint->twistMass * ( cdot + bias ) - impulseScale * oldImpulse;
+			joint->upperTwistImpulse = b3MaxFloat( oldImpulse + deltaImpulse, 0.0f );
+			deltaImpulse = joint->upperTwistImpulse - oldImpulse;
+
+			// sign flipped on applied impulse
+			wA = b3MulAdd( wA, deltaImpulse, b3MulMV( iA, twistJacobian ) );
+			wB = b3MulSub( wB, deltaImpulse, b3MulMV( iB, twistJacobian ) );
+		}
+	}
+
+	if ( joint->enableConeLimit && fixedRotation == false )
+	{
+		float swingAngle = b3GetSwingAngle( relQ );
+
+		// todo does an updated swing axis help?
+		// b3Vec3 axisA = b3RotateVector( quatA, b3Vec3_axisZ );
+		// b3Vec3 axisB = b3RotateVector( quatB, b3Vec3_axisZ );
+		// b3Vec3 swingAxis = b3Normalize( b3Cross( axisA, axisB ) );
+		// joint->swingAxis = swingAxis;
+
+		b3Vec3 swingAxis = joint->swingAxis;
+
+		float c = joint->coneAngle - swingAngle;
+		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 = base->constraintSoftness.biasRate * c;
+			massScale = base->constraintSoftness.massScale;
+			impulseScale = base->constraintSoftness.impulseScale;
+		}
+
+		// sign flipped on Cdot
+		float cdot = b3Dot( b3Sub( wA, wB ), swingAxis );
+		float oldImpulse = joint->swingImpulse;
+		float deltaImpulse = -massScale * joint->swingMass * ( cdot + bias ) - impulseScale * oldImpulse;
+		joint->swingImpulse = b3MaxFloat( oldImpulse + deltaImpulse, 0.0f );
+		deltaImpulse = joint->swingImpulse - oldImpulse;
+
+		// sign flipped on applied impulse
+		wA = b3MulAdd( wA, deltaImpulse, b3MulMV( iA, swingAxis ) );
+		wB = b3MulSub( wB, deltaImpulse, b3MulMV( iB, swingAxis ) );
+	}
+
+	// Solve point-to-point constraint
+	{
+		b3Vec3 rA = b3RotateVector( stateA->deltaRotation, joint->frameA.p );
+		b3Vec3 rB = b3RotateVector( stateB->deltaRotation, joint->frameB.p );
+
+		b3Vec3 cdot = b3Sub( b3Sub( b3Add( vB, b3Cross( wB, rB ) ), vA ), b3Cross( wA, rA ) );
+
+		b3Vec3 bias = b3Vec3_zero;
+		float massScale = 1.0f;
+		float impulseScale = 0.0f;
+		if ( useBias )
+		{
+			b3Vec3 dcA = stateA->deltaPosition;
+			b3Vec3 dcB = stateB->deltaPosition;
+
+			b3Vec3 separation = b3Add( b3Sub( dcB, dcA ), b3Sub( rB, rA ) );
+			separation = b3Add( separation, joint->deltaCenter );
+
+			bias = b3MulSV( base->constraintSoftness.biasRate, separation );
+			massScale = base->constraintSoftness.massScale;
+			impulseScale = base->constraintSoftness.impulseScale;
+		}
+
+		//// K = [(1/m1 + 1/m2) * eye(2) - skew(r1) * invI1 * skew(r1) - skew(r2) * invI2 * skew(r2)]
+		b3Matrix3 sA = b3Skew( rA );
+		b3Matrix3 sB = b3Skew( rB );
+		b3Matrix3 kA = b3MulMM( sA, b3MulMM( base->invIA, sA ) );
+		b3Matrix3 kB = b3MulMM( sB, b3MulMM( base->invIB, sB ) );
+		b3Matrix3 k = b3NegateMat3( b3AddMM( kA, kB ) );
+		k.cx.x += mA + mB;
+		k.cy.y += mA + mB;
+		k.cz.z += mA + mB;
+
+		b3Vec3 b = b3Solve3( k, b3Add( cdot, bias ) );
+
+		b3Vec3 impulse = b3MulSub( b3MulSV( -massScale, b ), impulseScale, joint->linearImpulse );
+		joint->linearImpulse = b3Add( joint->linearImpulse, impulse );
+
+		vA = b3MulSub( vA, mA, impulse );
+		wA = b3Sub( wA, b3MulMV( iA, b3Cross( rA, impulse ) ) );
+		vB = b3MulAdd( vB, mB, impulse );
+		wB = b3Add( wB, b3MulMV( iB, b3Cross( rB, impulse ) ) );
+	}
+
+	if ( stateA->flags & b3_dynamicFlag )
+	{
+		stateA->linearVelocity = vA;
+		stateA->angularVelocity = wA;
+	}
+
+	if ( stateB->flags & b3_dynamicFlag )
+	{
+		stateB->linearVelocity = vB;
+		stateB->angularVelocity = wB;
+	}
+}
+
+void b3DrawSphericalJoint( b3DebugDraw* draw, b3JointSim* base, b3WorldTransform transformA, b3WorldTransform transformB, float scale )
+{
+	b3WorldTransform frameA = b3MulWorldTransforms( transformA, base->localFrameA );
+
+	float length1 = 0.1f * scale;
+	draw->DrawSegmentFcn( frameA.p, b3OffsetPos( frameA.p, b3MulSV( length1, b3RotateVector( frameA.q, b3Vec3_axisX ) ) ), b3_colorRed,
+						  draw->context );
+	draw->DrawSegmentFcn( frameA.p, b3OffsetPos( frameA.p, b3MulSV( length1, b3RotateVector( frameA.q, b3Vec3_axisY ) ) ), b3_colorGreen,
+						  draw->context );
+	draw->DrawSegmentFcn( frameA.p, b3OffsetPos( frameA.p, b3MulSV( length1, b3RotateVector( frameA.q, b3Vec3_axisZ ) ) ), b3_colorBlue,
+						  draw->context );
+
+	b3WorldTransform frameB = b3MulWorldTransforms( transformB, base->localFrameB );
+
+	float length2 = 0.2f * scale;
+	draw->DrawSegmentFcn( frameB.p, b3OffsetPos( frameB.p, b3MulSV( length2, b3RotateVector( frameB.q, b3Vec3_axisZ ) ) ), b3_colorOrange,
+						  draw->context );
+
+	b3SphericalJoint* joint = &base->sphericalJoint;
+	enum { kSliceCount = 16 };
+
+	// Twist limit
+	if ( joint->enableTwistLimit )
+	{
+		b3Quat quatA = frameA.q;
+		b3Quat quatB = frameB.q;
+
+		if ( b3DotQuat( quatA, quatB ) < 0.0f )
+		{
+			// this keeps the twist angle in the range [-pi, pi]
+			quatB = b3NegateQuat( quatB );
+		}
+
+		b3Quat relQ = b3InvMulQuat( quatA, quatB );
+
+		const float wedgeRadius = 0.1f * scale;
+		for ( int index = 0; index < kSliceCount; ++index )
+		{
+			float t1 = (float)( index + 0 ) / kSliceCount;
+			float alpha1 = ( 1.0f - t1 ) * joint->lowerTwistAngle + t1 * joint->upperTwistAngle;
+			float t2 = (float)( index + 1 ) / kSliceCount;
+			float alpha2 = ( 1.0f - t2 ) * joint->lowerTwistAngle + t2 * joint->upperTwistAngle;
+
+			b3Vec3 vertex1 = { wedgeRadius * b3Cos( alpha1 ), wedgeRadius * b3Sin( alpha1 ), 0.0f };
+			b3Vec3 vertex2 = { wedgeRadius * b3Cos( alpha2 ), wedgeRadius * b3Sin( alpha2 ), 0.0f };
+
+			if ( index == 0 )
+			{
+				draw->DrawSegmentFcn( frameA.p, b3TransformWorldPoint( frameA, vertex1 ), b3_colorCyan, draw->context );
+			}
+
+			if ( index == kSliceCount - 1 )
+			{
+				draw->DrawSegmentFcn( b3TransformWorldPoint( frameA, vertex2 ), frameA.p, b3_colorCyan, draw->context );
+			}
+			draw->DrawSegmentFcn( b3TransformWorldPoint( frameA, vertex1 ), b3TransformWorldPoint( frameA, vertex2 ), b3_colorCyan,
+								  draw->context );
+		}
+
+		float twistAngle = b3GetTwistAngle( relQ );
+		b3Vec3 p2 = { wedgeRadius * b3Cos( twistAngle ), wedgeRadius * b3Sin( twistAngle ), 0.0f };
+		draw->DrawSegmentFcn( frameA.p, b3TransformWorldPoint( frameA, p2 ), b3_colorYellow, draw->context );
+	}
+
+	// Swing limit
+	if ( joint->enableConeLimit )
+	{
+		const float radius = 0.1f * scale;
+		float coneRadius = radius * b3Sin( joint->coneAngle );
+		float coneHeight = radius * b3Cos( joint->coneAngle );
+
+		for ( int index = 0; index < kSliceCount; ++index )
+		{
+			float phi1 = 2.0f * ( index + 0 ) / kSliceCount * B3_PI;
+			float phi2 = 2.0f * ( index + 1 ) / kSliceCount * B3_PI;
+
+			b3Vec3 vertex1 = { coneRadius * b3Cos( phi1 ), coneRadius * b3Sin( phi1 ), coneHeight };
+			b3Vec3 vertex2 = { coneRadius * b3Cos( phi2 ), coneRadius * b3Sin( phi2 ), coneHeight };
+
+			draw->DrawSegmentFcn( frameA.p, b3TransformWorldPoint( frameA, vertex1 ), b3_colorCyan, draw->context );
+			draw->DrawSegmentFcn( b3TransformWorldPoint( frameA, vertex1 ), b3TransformWorldPoint( frameA, vertex2 ), b3_colorCyan,
+								  draw->context );
+		}
+	}
+}

vendor/box3d/src/src/table.c

@@ -0,0 +1,248 @@
+// SPDX-FileCopyrightText: 2025 Erin Catto
+// SPDX-License-Identifier: MIT
+
+#include "table.h"
+
+#include "bitset.h"
+#include "core.h"
+#include "ctz.h"
+#include "platform.h"
+
+#include <string.h>
+
+#if B3_DEBUG
+b3AtomicInt b3_probeCount;
+#endif
+
+_Static_assert( 2 * B3_SHAPE_POWER + B3_CHILD_POWER == 64, "compound power" );
+_Static_assert( B3_CHILD_POWER > 8, "compound child power" );
+
+b3HashSet b3CreateSet( int32_t capacity )
+{
+	b3HashSet set = { 0 };
+
+	// Capacity must be a power of 2
+	if ( capacity > 16 )
+	{
+		set.capacity = b3RoundUpPowerOf2( capacity );
+	}
+	else
+	{
+		set.capacity = 16;
+	}
+
+	set.count = 0;
+	set.items = (b3SetItem*)b3Alloc( set.capacity * sizeof( b3SetItem ) );
+	memset( set.items, 0, set.capacity * sizeof( b3SetItem ) );
+
+	return set;
+}
+
+void b3DestroySet( b3HashSet* set )
+{
+	b3Free( set->items, set->capacity * sizeof( b3SetItem ) );
+	set->items = NULL;
+	set->count = 0;
+	set->capacity = 0;
+}
+
+void b3ClearSet( b3HashSet* set )
+{
+	set->count = 0;
+	memset( set->items, 0, set->capacity * sizeof( b3SetItem ) );
+}
+
+// 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 inline uint32_t b3KeyHash( uint64_t key )
+{
+	uint64_t h = key;
+	h ^= h >> 33;
+	h *= 0xff51afd7ed558ccdL;
+	h ^= h >> 33;
+	h *= 0xc4ceb9fe1a85ec53L;
+	h ^= h >> 33;
+
+	return (uint32_t)h;
+}
+
+static int32_t b3FindSlot( const b3HashSet* set, uint64_t key, uint32_t hash )
+{
+	uint32_t capacity = set->capacity;
+	int32_t index = hash & ( capacity - 1 );
+	const b3SetItem* items = set->items;
+	while ( items[index].hash != 0 && items[index].key != key )
+	{
+#if B3_DEBUG
+		b3AtomicFetchAddInt( &b3_probeCount, 1 );
+#endif
+		index = ( index + 1 ) & ( capacity - 1 );
+	}
+
+	return index;
+}
+
+static void b3AddKeyHaveCapacity( b3HashSet* set, uint64_t key, uint32_t hash )
+{
+	int32_t index = b3FindSlot( set, key, hash );
+	b3SetItem* items = set->items;
+	B3_ASSERT( items[index].hash == 0 );
+
+	items[index].key = key;
+	items[index].hash = hash;
+	set->count += 1;
+}
+
+static void b3GrowTable( b3HashSet* set )
+{
+	uint32_t oldCount = set->count;
+	B3_UNUSED( oldCount );
+
+	uint32_t oldCapacity = set->capacity;
+	b3SetItem* oldItems = set->items;
+
+	set->count = 0;
+	// Capacity must be a power of 2
+	set->capacity = 2 * oldCapacity;
+	set->items = (b3SetItem*)b3Alloc( set->capacity * sizeof( b3SetItem ) );
+	memset( set->items, 0, set->capacity * sizeof( b3SetItem ) );
+
+	// Transfer items into new array
+	for ( uint32_t i = 0; i < oldCapacity; ++i )
+	{
+		b3SetItem* item = oldItems + i;
+		if ( item->hash == 0 )
+		{
+			// this item was empty
+			continue;
+		}
+
+		b3AddKeyHaveCapacity( set, item->key, item->hash );
+	}
+
+	B3_ASSERT( set->count == oldCount );
+
+	b3Free( oldItems, oldCapacity * sizeof( b3SetItem ) );
+}
+
+bool b3ContainsKey( const b3HashSet* set, uint64_t key )
+{
+	// key of zero is a sentinel
+	B3_ASSERT( key != 0 );
+	uint32_t hash = b3KeyHash( key );
+	int32_t index = b3FindSlot( set, key, hash );
+	return set->items[index].key == key;
+}
+
+int b3GetHashSetBytes( b3HashSet* set )
+{
+	return set->capacity * (int)sizeof( b3SetItem );
+}
+
+bool b3AddKey( b3HashSet* set, uint64_t key )
+{
+	// key of zero is a sentinel
+	B3_ASSERT( key != 0 );
+
+	uint32_t hash = b3KeyHash( key );
+	B3_ASSERT( hash != 0 );
+
+	int32_t index = b3FindSlot( set, key, hash );
+	if ( set->items[index].hash != 0 )
+	{
+		// Already in set
+		B3_ASSERT( set->items[index].hash == hash && set->items[index].key == key );
+		return true;
+	}
+
+	if ( 2 * set->count >= set->capacity )
+	{
+		b3GrowTable( set );
+	}
+
+	b3AddKeyHaveCapacity( set, key, hash );
+	return false;
+}
+
+// See https://en.wikipedia.org/wiki/Open_addressing
+bool b3RemoveKey( b3HashSet* set, uint64_t key )
+{
+	uint32_t hash = b3KeyHash( key );
+	int32_t i = b3FindSlot( set, key, hash );
+	b3SetItem* 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;
+
+	B3_ASSERT( set->count > 0 );
+	set->count -= 1;
+
+	// Attempt to fill item i
+	int32_t 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
+		int32_t 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;
+}
+
+// This function is here because ctz.h is included by
+// this file but not in bitset.c
+int b3CountSetBits( b3BitSet* bitSet )
+{
+	int popCount = 0;
+	uint32_t blockCount = bitSet->blockCount;
+	for ( uint32_t i = 0; i < blockCount; ++i )
+	{
+		popCount += b3PopCount64( bitSet->bits[i] );
+	}
+
+	return popCount;
+}

vendor/box3d/src/src/table.h

@@ -0,0 +1,53 @@
+// SPDX-FileCopyrightText: 2025 Erin Catto
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include "box3d/constants.h"
+
+#include <stdbool.h>
+#include <stdint.h>
+
+typedef struct b3SetItem
+{
+	uint64_t key;
+	uint32_t hash;
+} b3SetItem;
+
+typedef struct b3HashSet
+{
+	b3SetItem* items;
+	uint32_t capacity;
+	uint32_t count;
+} b3HashSet;
+
+#define B3_SHAPE_MASK ( B3_MAX_SHAPES - 1 )
+#define B3_CHILD_MASK ( B3_MAX_CHILD_SHAPES - 1 )
+
+static inline uint64_t b3ShapePairKey( int s1, int s2, int c )
+{
+	if (s1 < s2)
+	{
+		return ( (uint64_t)( B3_SHAPE_MASK & s1 ) << ( 64 - B3_SHAPE_POWER ) ) |
+			   ( (uint64_t)( B3_SHAPE_MASK & s2 ) << ( 64 - 2 * B3_SHAPE_POWER ) ) | ( (uint64_t)( B3_CHILD_MASK & c ) );
+	}
+
+	return ( (uint64_t)( B3_SHAPE_MASK & s2 ) << ( 64 - B3_SHAPE_POWER ) ) |
+		   ( (uint64_t)( B3_SHAPE_MASK & s1 ) << ( 64 - 2 * B3_SHAPE_POWER ) ) |
+		   ( (uint64_t)( B3_CHILD_MASK & c ) );
+}
+
+b3HashSet b3CreateSet( int32_t capacity );
+void b3DestroySet( b3HashSet* set );
+
+void b3ClearSet( b3HashSet* set );
+
+// Returns true if key was already in set
+bool b3AddKey( b3HashSet* set, uint64_t key );
+
+// Returns true if the key was found
+bool b3RemoveKey( b3HashSet* set, uint64_t key );
+
+bool b3ContainsKey( const b3HashSet* set, uint64_t key );
+
+int b3GetHashSetBytes( b3HashSet* set );

vendor/box3d/src/src/timer.c

@@ -0,0 +1,700 @@
+// SPDX-FileCopyrightText: 2025 Erin Catto
+// SPDX-License-Identifier: MIT
+
+// Required on Linux to expose pthread_setname_np. Must be defined before any
+// system header is included.
+#if defined( __linux__ ) && !defined( _GNU_SOURCE )
+#define _GNU_SOURCE
+#endif
+
+#include "core.h"
+
+#include "box3d/base.h"
+
+#include <stddef.h>
+#include <stdio.h>
+#include <string.h>
+
+#define NAME_LENGTH 16
+
+#if defined( _WIN32 )
+
+#ifndef WIN32_LEAN_AND_MEAN
+#define WIN32_LEAN_AND_MEAN 1
+#endif
+
+#include <Windows.h>
+#include <limits.h>
+
+static double s_invFrequency = 0.0;
+
+uint64_t b3GetTicks( void )
+{
+	LARGE_INTEGER counter;
+	QueryPerformanceCounter( &counter );
+	return (uint64_t)counter.QuadPart;
+}
+
+float b3GetMilliseconds( 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 = b3GetTicks();
+	return (float)( s_invFrequency * ( ticksNow - ticks ) );
+}
+
+float b3GetMillisecondsAndReset( 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 = b3GetTicks();
+	float ms = (float)( s_invFrequency * ( ticksNow - *ticks ) );
+	*ticks = ticksNow;
+	return ms;
+}
+
+void b3Yield( void )
+{
+	SwitchToThread();
+}
+
+void b3Sleep( int milliseconds )
+{
+	Sleep( (DWORD)milliseconds );
+}
+
+typedef struct b3Mutex
+{
+	CRITICAL_SECTION cs;
+} b3Mutex;
+
+b3Mutex* b3CreateMutex( void )
+{
+	b3Mutex* m = b3Alloc( sizeof( b3Mutex ) );
+	InitializeCriticalSection( &m->cs );
+	return m;
+}
+
+void b3DestroyMutex( b3Mutex* m )
+{
+	DeleteCriticalSection( &m->cs );
+	*m = (b3Mutex){ 0 };
+	b3Free( m, sizeof( b3Mutex ) );
+}
+
+void b3LockMutex( b3Mutex* m )
+{
+	EnterCriticalSection( &m->cs );
+}
+
+void b3UnlockMutex( b3Mutex* m )
+{
+	LeaveCriticalSection( &m->cs );
+}
+
+typedef struct b3Semaphore
+{
+	HANDLE semaphore;
+} b3Semaphore;
+
+b3Semaphore* b3CreateSemaphore( int initCount )
+{
+	b3Semaphore* s = b3Alloc( sizeof( b3Semaphore ) );
+	s->semaphore = CreateSemaphoreExW( NULL, initCount, INT_MAX, NULL, 0, SEMAPHORE_ALL_ACCESS );
+	return s;
+}
+
+void b3DestroySemaphore( b3Semaphore* s )
+{
+	CloseHandle( s->semaphore );
+	*s = (b3Semaphore){ 0 };
+	b3Free( s, sizeof( b3Semaphore ) );
+}
+
+void b3WaitSemaphore( b3Semaphore* s )
+{
+	WaitForSingleObjectEx( s->semaphore, INFINITE, FALSE );
+}
+
+void b3SignalSemaphore( b3Semaphore* s )
+{
+	ReleaseSemaphore( s->semaphore, 1, NULL );
+}
+
+typedef struct b3Thread
+{
+	HANDLE thread;
+	b3ThreadFunction* function;
+	void* context;
+	char name[NAME_LENGTH];
+} b3Thread;
+
+typedef HRESULT( WINAPI* b3SetThreadDescriptionFn )( HANDLE, PCWSTR );
+
+// SetThreadDescription exists on Windows 10 1607+. Resolve it dynamically so
+// older Windows versions still link. Resolved once, cached for subsequent calls.
+static void b3SetCurrentThreadName( const char* name )
+{
+	if ( name == NULL || name[0] == 0 )
+	{
+		return;
+	}
+
+	static b3SetThreadDescriptionFn pfn = NULL;
+	static int resolved = 0;
+
+	if ( resolved == 0 )
+	{
+		HMODULE kernel = GetModuleHandleW( L"kernel32.dll" );
+		if ( kernel != NULL )
+		{
+			// MSVC /Wall warns C4191 on every FARPROC function-pointer cast.
+			// This is the intended use of GetProcAddress, so suppress locally.
+#pragma warning( push )
+#pragma warning( disable : 4191 )
+			pfn = (b3SetThreadDescriptionFn)GetProcAddress( kernel, "SetThreadDescription" );
+#pragma warning( pop )
+		}
+		resolved = 1;
+	}
+
+	if ( pfn == NULL )
+	{
+		return;
+	}
+
+	wchar_t wide[NAME_LENGTH];
+	int n = MultiByteToWideChar( CP_UTF8, 0, name, -1, wide, (int)( sizeof( wide ) / sizeof( wide[0] ) ) );
+	if ( n > 0 )
+	{
+		pfn( GetCurrentThread(), wide );
+	}
+}
+
+static DWORD WINAPI b3ThreadStart( LPVOID param )
+{
+	b3Thread* t = (b3Thread*)param;
+	b3SetCurrentThreadName( t->name );
+	t->function( t->context );
+	return 0;
+}
+
+b3Thread* b3CreateThread( b3ThreadFunction* function, void* context, const char* name )
+{
+	b3Thread* t = b3Alloc( sizeof( b3Thread ) );
+	t->function = function;
+	t->context = context;
+	if ( name != NULL )
+	{
+		snprintf( t->name, sizeof( t->name ), "%s", name );
+	}
+	else
+	{
+		t->name[0] = 0;
+	}
+	t->thread = CreateThread( NULL, 0, b3ThreadStart, t, 0, NULL );
+	return t;
+}
+
+void b3JoinThread( b3Thread* t )
+{
+	WaitForSingleObject( t->thread, INFINITE );
+	CloseHandle( t->thread );
+	*t = (b3Thread){ 0 };
+	b3Free( t, sizeof( b3Thread ) );
+}
+
+#elif defined( __linux__ ) || defined( __EMSCRIPTEN__ )
+
+#include <sched.h>
+#include <time.h>
+
+uint64_t b3GetTicks( void )
+{
+	struct timespec ts;
+	clock_gettime( CLOCK_MONOTONIC, &ts );
+	return ts.tv_sec * 1000000000LL + ts.tv_nsec;
+}
+
+float b3GetMilliseconds( uint64_t ticks )
+{
+	uint64_t ticksNow = b3GetTicks();
+	return (float)( ( ticksNow - ticks ) / 1000000.0 );
+}
+
+float b3GetMillisecondsAndReset( uint64_t* ticks )
+{
+	uint64_t ticksNow = b3GetTicks();
+	float ms = (float)( ( ticksNow - *ticks ) / 1000000.0 );
+	*ticks = ticksNow;
+	return ms;
+}
+
+void b3Yield( void )
+{
+	sched_yield();
+}
+
+void b3Sleep( int milliseconds )
+{
+	struct timespec ts;
+	ts.tv_sec = milliseconds / 1000;
+	ts.tv_nsec = ( milliseconds % 1000 ) * 1000000L;
+	nanosleep( &ts, NULL );
+}
+
+#include <pthread.h>
+typedef struct b3Mutex
+{
+	pthread_mutex_t mtx;
+} b3Mutex;
+
+b3Mutex* b3CreateMutex( void )
+{
+	b3Mutex* m = b3Alloc( sizeof( b3Mutex ) );
+	pthread_mutex_init( &m->mtx, NULL );
+	return m;
+}
+
+void b3DestroyMutex( b3Mutex* m )
+{
+	pthread_mutex_destroy( &m->mtx );
+	*m = (b3Mutex){ 0 };
+	b3Free( m, sizeof( b3Mutex ) );
+}
+
+void b3LockMutex( b3Mutex* m )
+{
+	pthread_mutex_lock( &m->mtx );
+}
+
+void b3UnlockMutex( b3Mutex* m )
+{
+	pthread_mutex_unlock( &m->mtx );
+}
+
+#include <semaphore.h>
+
+typedef struct b3Semaphore
+{
+	sem_t semaphore;
+} b3Semaphore;
+
+b3Semaphore* b3CreateSemaphore( int initCount )
+{
+	b3Semaphore* s = b3Alloc( sizeof( b3Semaphore ) );
+	sem_init( &s->semaphore, 0, (unsigned int)initCount );
+	return s;
+}
+
+void b3DestroySemaphore( b3Semaphore* s )
+{
+	sem_destroy( &s->semaphore );
+	*s = (b3Semaphore){ 0 };
+	b3Free( s, sizeof( b3Semaphore ) );
+}
+
+void b3WaitSemaphore( b3Semaphore* s )
+{
+	sem_wait( &s->semaphore );
+}
+
+void b3SignalSemaphore( b3Semaphore* s )
+{
+	sem_post( &s->semaphore );
+}
+
+typedef struct b3Thread
+{
+	pthread_t thread;
+	b3ThreadFunction* function;
+	void* context;
+	char name[NAME_LENGTH];
+} b3Thread;
+
+static void b3SetCurrentThreadName( const char* name )
+{
+	if ( name == NULL || name[0] == 0 )
+	{
+		return;
+	}
+
+#if defined( __linux__ )
+	// Linux caps thread names at 15 chars + null terminator.
+	char truncated[NAME_LENGTH];
+	snprintf( truncated, sizeof( truncated ), "%s", name );
+	pthread_setname_np( pthread_self(), truncated );
+#else
+	(void)name;
+#endif
+}
+
+static void* b3ThreadStart( void* param )
+{
+	b3Thread* t = (b3Thread*)param;
+	b3SetCurrentThreadName( t->name );
+	t->function( t->context );
+	return NULL;
+}
+
+b3Thread* b3CreateThread( b3ThreadFunction* function, void* context, const char* name )
+{
+	b3Thread* t = b3Alloc( sizeof( b3Thread ) );
+	t->function = function;
+	t->context = context;
+	if ( name != NULL )
+	{
+		snprintf( t->name, sizeof( t->name ), "%s", name );
+	}
+	else
+	{
+		t->name[0] = 0;
+	}
+	pthread_create( &t->thread, NULL, b3ThreadStart, t );
+	return t;
+}
+
+void b3JoinThread( b3Thread* t )
+{
+	pthread_join( t->thread, NULL );
+	*t = (b3Thread){ 0 };
+	b3Free( t, sizeof( b3Thread ) );
+}
+
+#elif defined( __APPLE__ )
+
+#include <mach/mach_time.h>
+#include <sched.h>
+#include <sys/time.h>
+#include <time.h>
+
+static double s_invFrequency = 0.0;
+
+uint64_t b3GetTicks( void )
+{
+	return mach_absolute_time();
+}
+
+float b3GetMilliseconds( 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 = b3GetTicks();
+	return (float)( s_invFrequency * ( ticksNow - ticks ) );
+}
+
+float b3GetMillisecondsAndReset( 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 = b3GetTicks();
+	float ms = (float)( s_invFrequency * ( ticksNow - *ticks ) );
+	*ticks = ticksNow;
+	return ms;
+}
+
+void b3Yield( void )
+{
+	sched_yield();
+}
+
+void b3Sleep( int milliseconds )
+{
+	struct timespec ts;
+	ts.tv_sec = milliseconds / 1000;
+	ts.tv_nsec = ( milliseconds % 1000 ) * 1000000L;
+	nanosleep( &ts, NULL );
+}
+
+#include <pthread.h>
+typedef struct b3Mutex
+{
+	pthread_mutex_t mtx;
+} b3Mutex;
+
+b3Mutex* b3CreateMutex( void )
+{
+	b3Mutex* m = b3Alloc( sizeof( b3Mutex ) );
+	pthread_mutex_init( &m->mtx, NULL );
+	return m;
+}
+
+void b3DestroyMutex( b3Mutex* m )
+{
+	pthread_mutex_destroy( &m->mtx );
+	*m = (b3Mutex){ 0 };
+	b3Free( m, sizeof( b3Mutex ) );
+}
+
+void b3LockMutex( b3Mutex* m )
+{
+	pthread_mutex_lock( &m->mtx );
+}
+
+void b3UnlockMutex( b3Mutex* m )
+{
+	pthread_mutex_unlock( &m->mtx );
+}
+
+#include <dispatch/dispatch.h>
+
+typedef struct b3Semaphore
+{
+	dispatch_semaphore_t semaphore;
+	int initialCount;
+} b3Semaphore;
+
+b3Semaphore* b3CreateSemaphore( int initCount )
+{
+	b3Semaphore* s = b3Alloc( sizeof( b3Semaphore ) );
+	s->semaphore = dispatch_semaphore_create( (long)initCount );
+	s->initialCount = initCount;
+	return s;
+}
+
+void b3DestroySemaphore( b3Semaphore* s )
+{
+	// libdispatch aborts if the current count is less than the initial count at release time.
+	// Pad with signals so the invariant always holds; no one is waiting at this point.
+	for ( int i = 0; i < s->initialCount; ++i )
+	{
+		dispatch_semaphore_signal( s->semaphore );
+	}
+	dispatch_release( s->semaphore );
+	*s = (b3Semaphore){ 0 };
+	b3Free( s, sizeof( b3Semaphore ) );
+}
+
+void b3WaitSemaphore( b3Semaphore* s )
+{
+	dispatch_semaphore_wait( s->semaphore, DISPATCH_TIME_FOREVER );
+}
+
+void b3SignalSemaphore( b3Semaphore* s )
+{
+	dispatch_semaphore_signal( s->semaphore );
+}
+
+typedef struct b3Thread
+{
+	pthread_t thread;
+	b3ThreadFunction* function;
+	void* context;
+	char name[NAME_LENGTH];
+} b3Thread;
+
+// macOS pthread_setname_np takes only the name — it always names the calling thread.
+static void b3SetCurrentThreadName( const char* name )
+{
+	if ( name == NULL || name[0] == 0 )
+	{
+		return;
+	}
+	pthread_setname_np( name );
+}
+
+static void* b3ThreadStart( void* param )
+{
+	b3Thread* t = (b3Thread*)param;
+	b3SetCurrentThreadName( t->name );
+	t->function( t->context );
+	return NULL;
+}
+
+b3Thread* b3CreateThread( b3ThreadFunction* function, void* context, const char* name )
+{
+	b3Thread* t = b3Alloc( sizeof( b3Thread ) );
+	t->function = function;
+	t->context = context;
+	if ( name != NULL )
+	{
+		snprintf( t->name, sizeof( t->name ), "%s", name );
+	}
+	else
+	{
+		t->name[0] = 0;
+	}
+	pthread_create( &t->thread, NULL, b3ThreadStart, t );
+	return t;
+}
+
+void b3JoinThread( b3Thread* t )
+{
+	pthread_join( t->thread, NULL );
+	*t = (b3Thread){ 0 };
+	b3Free( t, sizeof( b3Thread ) );
+}
+
+#else
+
+uint64_t b3GetTicks( void )
+{
+	return 0;
+}
+
+float b3GetMilliseconds( uint64_t ticks )
+{
+	( (void)( ticks ) );
+	return 0.0f;
+}
+
+float b3GetMillisecondsAndReset( uint64_t* ticks )
+{
+	( (void)( ticks ) );
+	return 0.0f;
+}
+
+void b3Yield( void )
+{
+}
+
+void b3Sleep( int milliseconds )
+{
+	( (void)( milliseconds ) );
+}
+
+typedef struct b3Mutex
+{
+	int dummy;
+} b3Mutex;
+
+b3Mutex* b3CreateMutex( void )
+{
+	b3Mutex* m = b3Alloc( sizeof( b3Mutex ) );
+	m->dummy = 42;
+	return m;
+}
+
+void b3DestroyMutex( b3Mutex* m )
+{
+	*m = (b3Mutex){ 0 };
+	b3Free( m, sizeof( b3Mutex ) );
+}
+
+void b3LockMutex( b3Mutex* m )
+{
+	(void)m;
+}
+
+void b3UnlockMutex( b3Mutex* m )
+{
+	(void)m;
+}
+
+typedef struct b3Semaphore
+{
+	int dummy;
+} b3Semaphore;
+
+b3Semaphore* b3CreateSemaphore( int initCount )
+{
+	b3Semaphore* s = b3Alloc( sizeof( b3Semaphore ) );
+	(void)initCount;
+	s->dummy = 42;
+	return s;
+}
+
+void b3DestroySemaphore( b3Semaphore* s )
+{
+	*s = (b3Semaphore){ 0 };
+	b3Free( s, sizeof( b3Semaphore ) );
+}
+
+void b3WaitSemaphore( b3Semaphore* s )
+{
+	(void)s;
+}
+
+void b3SignalSemaphore( b3Semaphore* s )
+{
+	(void)s;
+}
+
+typedef struct b3Thread
+{
+	int dummy;
+} b3Thread;
+
+b3Thread* b3CreateThread( b3ThreadFunction* function, void* context, const char* name )
+{
+	(void)name;
+	function( context );
+	b3Thread* t = b3Alloc( sizeof( b3Thread ) );
+	t->dummy = 42;
+	return t;
+}
+
+void b3JoinThread( b3Thread* t )
+{
+	*t = (b3Thread){ 0 };
+	b3Free( t, sizeof( b3Thread ) );
+}
+
+#endif
+
+// djb2 hash, folded 8 bytes per iteration to shorten the dependency chain.
+// memcpy lowers to a single load on most targets; on big-endian we byte-swap so
+// the hash value is identical across endianness (preserving cross-platform determinism).
+// Equivalent to byte-wise djb2 only in spirit; values differ from the original recurrence.
+uint32_t b3Hash( uint32_t hash, const uint8_t* data, int count )
+{
+	uint32_t result = hash;
+	int i = 0;
+
+	while ( i + 8 <= count )
+	{
+		uint64_t word;
+		memcpy( &word, data + i, sizeof( word ) );
+#if defined( __BYTE_ORDER__ ) && __BYTE_ORDER__ == __ORDER_BIG_ENDIAN__
+		word = ( ( word & 0x00000000000000FFULL ) << 56 ) | ( ( word & 0x000000000000FF00ULL ) << 40 ) |
+			   ( ( word & 0x0000000000FF0000ULL ) << 24 ) | ( ( word & 0x00000000FF000000ULL ) << 8 ) |
+			   ( ( word & 0x000000FF00000000ULL ) >> 8 ) | ( ( word & 0x0000FF0000000000ULL ) >> 24 ) |
+			   ( ( word & 0x00FF000000000000ULL ) >> 40 ) | ( ( word & 0xFF00000000000000ULL ) >> 56 );
+#endif
+		result = ( result << 5 ) + result + (uint32_t)word;
+		result = ( result << 5 ) + result + (uint32_t)( word >> 32 );
+		i += 8;
+	}
+
+	while ( i < count )
+	{
+		result = ( result << 5 ) + result + data[i];
+		i++;
+	}
+
+	return result;
+}

vendor/box3d/src/src/types.c

@@ -0,0 +1,155 @@
+// SPDX-FileCopyrightText: 2025 Erin Catto
+// SPDX-License-Identifier: MIT
+
+#include "box3d/types.h"
+
+#include "core.h"
+
+#include "box3d/constants.h"
+
+b3WorldDef b3DefaultWorldDef( void )
+{
+	float lengthUnits = b3GetLengthUnitsPerMeter();
+
+	b3WorldDef def = { 0 };
+	def.gravity.x = 0.0f;
+	def.gravity.y = -10.0f;
+	def.hitEventThreshold = 1.0f * lengthUnits;
+	def.restitutionThreshold = 1.0f * lengthUnits;
+	def.contactSpeed = 3.0f * lengthUnits;
+	def.contactHertz = 30.0f;
+	def.contactDampingRatio = 10.0f;
+
+	// 400 meters per second, faster than the speed of sound
+	def.maximumLinearSpeed = 400.0f * lengthUnits;
+
+	def.enableSleep = true;
+	def.enableContinuous = true;
+	def.internalValue = B3_SECRET_COOKIE;
+	return def;
+}
+
+b3BodyDef b3DefaultBodyDef( void )
+{
+	b3BodyDef def = { 0 };
+	def.type = b3_staticBody;
+	def.rotation = b3Quat_identity;
+	def.sleepThreshold = 0.05f * b3GetLengthUnitsPerMeter();
+	def.gravityScale = 1.0f;
+	def.enableSleep = true;
+	def.isAwake = true;
+	def.isEnabled = true;
+	def.enableContactRecycling = true;
+	def.internalValue = B3_SECRET_COOKIE;
+	return def;
+}
+
+b3Filter b3DefaultFilter( void )
+{
+	b3Filter filter = { B3_DEFAULT_CATEGORY_BITS, B3_DEFAULT_MASK_BITS, 0 };
+	return filter;
+}
+
+b3QueryFilter b3DefaultQueryFilter( void )
+{
+	b3QueryFilter filter = { B3_DEFAULT_CATEGORY_BITS, B3_DEFAULT_MASK_BITS, 0, NULL };
+	return filter;
+}
+
+b3SurfaceMaterial b3DefaultSurfaceMaterial( void )
+{
+	b3SurfaceMaterial surfaceMaterial = { 0 };
+	surfaceMaterial.friction = 0.6f;
+	return surfaceMaterial;
+}
+
+b3ShapeDef b3DefaultShapeDef( void )
+{
+	float lengthUnits = b3GetLengthUnitsPerMeter();
+
+	b3ShapeDef def = { 0 };
+	def.baseMaterial = b3DefaultSurfaceMaterial();
+	// density of water
+	def.density = 1000.0f / ( lengthUnits * lengthUnits * lengthUnits );
+	def.explosionScale = 1.0f;
+	def.filter = b3DefaultFilter();
+	def.updateBodyMass = true;
+	def.invokeContactCreation = true;
+	def.enableSpeculativeContact = true;
+	def.internalValue = B3_SECRET_COOKIE;
+	return def;
+}
+
+static bool b3EmptyDrawShape( void* userShape, b3WorldTransform transform, b3HexColor color, void* context )
+{
+	B3_UNUSED( userShape, transform, color, context );
+	return false;
+}
+
+static void b3EmptyDrawSegment( b3Pos p1, b3Pos p2, b3HexColor color, void* context )
+{
+	B3_UNUSED( p1, p2, color, context );
+}
+
+static void b3EmptyDrawTransform( b3WorldTransform transform, void* context )
+{
+	B3_UNUSED( transform, context );
+}
+
+static void b3EmptyDrawPoint( b3Pos p, float size, b3HexColor color, void* context )
+{
+	B3_UNUSED( p, size, color, context );
+}
+
+static void b3EmptyDrawSphere( b3Pos p, float radius, b3HexColor color, float alpha, void* context )
+{
+	B3_UNUSED( p, radius, color, alpha, context );
+}
+
+static void b3EmptyDrawCapsule( b3Pos p1, b3Pos p2, float radius, b3HexColor color, float alpha, void* context )
+{
+	B3_UNUSED( p1, p2, radius, color, alpha, context );
+}
+
+static void b3EmptyDrawBounds( b3AABB aabb, b3HexColor color, void* context )
+{
+	B3_UNUSED( aabb, color, context );
+}
+
+static void b3EmptyDrawBox( b3Vec3 extents, b3WorldTransform transform, b3HexColor color, void* context )
+{
+	B3_UNUSED( extents, transform, color, context );
+}
+
+static void b3EmptyDrawString( b3Pos p, const char* s, b3HexColor color, void* context )
+{
+	B3_UNUSED( p, s, color, context );
+}
+
+b3DebugDraw b3DefaultDebugDraw( void )
+{
+	b3DebugDraw draw = { 0 };
+
+	// These allow the user to skip some implementations and not hit null exceptions.
+	draw.DrawShapeFcn = b3EmptyDrawShape;
+	draw.DrawSegmentFcn = b3EmptyDrawSegment;
+	draw.DrawTransformFcn = b3EmptyDrawTransform;
+	draw.DrawPointFcn = b3EmptyDrawPoint;
+	draw.DrawSphereFcn = b3EmptyDrawSphere;
+	draw.DrawCapsuleFcn = b3EmptyDrawCapsule;
+	draw.DrawBoundsFcn = b3EmptyDrawBounds;
+	draw.DrawBoxFcn = b3EmptyDrawBox;
+	draw.DrawStringFcn = b3EmptyDrawString;
+
+	// Not too small, not too big.
+	float h = 100.0f * b3GetLengthUnitsPerMeter();
+	draw.drawingBounds = (b3AABB){
+		.lowerBound = { -h, -h, -h },
+		.upperBound = { h, h, h },
+	};
+
+	draw.jointScale = 1.0f;
+	draw.forceScale = 1.0f;
+
+	return draw;
+}

vendor/box3d/src/src/weld_joint.c

@@ -0,0 +1,314 @@
+// SPDX-FileCopyrightText: 2025 Erin Catto
+// SPDX-License-Identifier: MIT
+
+#include "body.h"
+#include "joint.h"
+#include "physics_world.h"
+#include "solver.h"
+#include "solver_set.h"
+#include "recording.h"
+
+// needed for dll export
+#include "box3d/box3d.h"
+
+void b3WeldJoint_SetLinearHertz( b3JointId jointId, float hertz )
+{
+	B3_ASSERT( b3IsValidFloat( hertz ) && hertz >= 0.0f );
+	b3World* world = b3GetWorld( jointId.world0 );
+	B3_REC( world, WeldJointSetLinearHertz, jointId, hertz );
+	b3JointSim* base = b3GetJointSimCheckType( jointId, b3_weldJoint );
+	base->weldJoint.linearHertz = hertz;
+}
+
+float b3WeldJoint_GetLinearHertz( b3JointId jointId )
+{
+	b3JointSim* base = b3GetJointSimCheckType( jointId, b3_weldJoint );
+	return base->weldJoint.linearHertz;
+}
+
+void b3WeldJoint_SetLinearDampingRatio( b3JointId jointId, float dampingRatio )
+{
+	B3_ASSERT( b3IsValidFloat( dampingRatio ) && dampingRatio >= 0.0f );
+	b3World* world = b3GetWorld( jointId.world0 );
+	B3_REC( world, WeldJointSetLinearDampingRatio, jointId, dampingRatio );
+	b3JointSim* base = b3GetJointSimCheckType( jointId, b3_weldJoint );
+	base->weldJoint.linearDampingRatio = dampingRatio;
+}
+
+float b3WeldJoint_GetLinearDampingRatio( b3JointId jointId )
+{
+	b3JointSim* base = b3GetJointSimCheckType( jointId, b3_weldJoint );
+	return base->weldJoint.linearDampingRatio;
+}
+
+void b3WeldJoint_SetAngularHertz( b3JointId jointId, float hertz )
+{
+	B3_ASSERT( b3IsValidFloat( hertz ) && hertz >= 0.0f );
+	b3World* world = b3GetWorld( jointId.world0 );
+	B3_REC( world, WeldJointSetAngularHertz, jointId, hertz );
+	b3JointSim* base = b3GetJointSimCheckType( jointId, b3_weldJoint );
+	base->weldJoint.angularHertz = hertz;
+}
+
+float b3WeldJoint_GetAngularHertz( b3JointId jointId )
+{
+	b3JointSim* base = b3GetJointSimCheckType( jointId, b3_weldJoint );
+	return base->weldJoint.angularHertz;
+}
+
+void b3WeldJoint_SetAngularDampingRatio( b3JointId jointId, float dampingRatio )
+{
+	B3_ASSERT( b3IsValidFloat( dampingRatio ) && dampingRatio >= 0.0f );
+	b3World* world = b3GetWorld( jointId.world0 );
+	B3_REC( world, WeldJointSetAngularDampingRatio, jointId, dampingRatio );
+	b3JointSim* base = b3GetJointSimCheckType( jointId, b3_weldJoint );
+	base->weldJoint.angularDampingRatio = dampingRatio;
+}
+
+float b3WeldJoint_GetAngularDampingRatio( b3JointId jointId )
+{
+	b3JointSim* base = b3GetJointSimCheckType( jointId, b3_weldJoint );
+	return base->weldJoint.angularDampingRatio;
+}
+
+b3Vec3 b3GetWeldJointForce( b3World* world, b3JointSim* base )
+{
+	b3Vec3 force = b3MulSV( world->inv_h, base->weldJoint.linearImpulse );
+	return force;
+}
+
+b3Vec3 b3GetWeldJointTorque( b3World* world, b3JointSim* base )
+{
+	return b3MulSV( world->inv_h, base->weldJoint.angularImpulse );
+}
+
+void b3PrepareWeldJoint( b3JointSim* base, b3StepContext* context )
+{
+	B3_ASSERT( base->type == b3_weldJoint );
+
+	b3World* world = context->world;
+
+	b3Body* bodyA = b3Array_Get( world->bodies, base->bodyIdA  );
+	b3Body* bodyB = b3Array_Get( world->bodies, base->bodyIdB  );
+
+	B3_ASSERT( bodyB->setIndex == b3_awakeSet );
+	b3SolverSet* setA = b3Array_Get( world->solverSets, bodyA->setIndex  );
+	b3SolverSet* setB = b3Array_Get( world->solverSets, bodyB->setIndex  );
+
+	int localIndexA = bodyA->localIndex;
+	int localIndexB = bodyB->localIndex;
+
+	b3BodySim* bodySimA = b3Array_Get( setA->bodySims, localIndexA  );
+	b3BodySim* bodySimB = b3Array_Get( setB->bodySims, localIndexB  );
+
+	base->invMassA = bodySimA->invMass;
+	base->invMassB = bodySimB->invMass;
+	base->invIA = bodySimA->invInertiaWorld;
+	base->invIB = bodySimB->invInertiaWorld;
+
+	b3Matrix3 invInertiaSum = b3AddMM( base->invIA, base->invIB );
+	base->fixedRotation = b3Det( invInertiaSum ) < 1000.0f * FLT_MIN;
+
+	b3WeldJoint* joint = &base->weldJoint;
+	joint->indexA = bodyA->setIndex == b3_awakeSet ? localIndexA : B3_NULL_INDEX;
+	joint->indexB = bodyB->setIndex == b3_awakeSet ? localIndexB : B3_NULL_INDEX;
+
+	// Compute joint anchor frames with world space rotation, relative to center of mass
+	joint->frameA.q = b3MulQuat( bodySimA->transform.q, base->localFrameA.q );
+	joint->frameA.p = b3RotateVector( bodySimA->transform.q, b3Sub( base->localFrameA.p, bodySimA->localCenter ) );
+	joint->frameB.q = b3MulQuat( bodySimB->transform.q, base->localFrameB.q );
+	joint->frameB.p = b3RotateVector( bodySimB->transform.q, b3Sub( base->localFrameB.p, bodySimB->localCenter ) );
+
+	joint->deltaCenter = b3SubPos( bodySimB->center, bodySimA->center );
+	joint->angularMass = b3InvertMatrix( invInertiaSum );
+
+	if ( joint->linearHertz == 0.0f )
+	{
+		joint->linearSpring = base->constraintSoftness;
+	}
+	else
+	{
+		joint->linearSpring = b3MakeSoft( joint->linearHertz, joint->linearDampingRatio, context->h );
+	}
+
+	if ( joint->angularHertz == 0.0f )
+	{
+		joint->angularSpring = base->constraintSoftness;
+	}
+	else
+	{
+		joint->angularSpring = b3MakeSoft( joint->angularHertz, joint->angularDampingRatio, context->h );
+	}
+
+	if ( context->enableWarmStarting == false )
+	{
+		joint->linearImpulse = b3Vec3_zero;
+		joint->angularImpulse = b3Vec3_zero;
+	}
+}
+
+void b3WarmStartWeldJoint( b3JointSim* base, b3StepContext* context )
+{
+	B3_ASSERT( base->type == b3_weldJoint );
+
+	float mA = base->invMassA;
+	float mB = base->invMassB;
+	b3Matrix3 iA = base->invIA;
+	b3Matrix3 iB = base->invIB;
+
+	// dummy state for static bodies
+	b3BodyState dummyState = b3_identityBodyState;
+
+	b3WeldJoint* joint = &base->weldJoint;
+
+	b3BodyState* stateA = joint->indexA == B3_NULL_INDEX ? &dummyState : context->states + joint->indexA;
+	b3BodyState* stateB = joint->indexB == B3_NULL_INDEX ? &dummyState : context->states + joint->indexB;
+
+	b3Vec3 vA = stateA->linearVelocity;
+	b3Vec3 wA = stateA->angularVelocity;
+	b3Vec3 vB = stateB->linearVelocity;
+	b3Vec3 wB = stateB->angularVelocity;
+
+	b3Vec3 rA = b3RotateVector( stateA->deltaRotation, joint->frameA.p );
+	b3Vec3 rB = b3RotateVector( stateB->deltaRotation, joint->frameB.p );
+
+	vA = b3MulSub( vA, mA, joint->linearImpulse );
+	wA = b3Sub( wA, b3MulMV( iA, b3Add( b3Cross( rA, joint->linearImpulse ), joint->angularImpulse ) ) );
+
+	vB = b3MulAdd( vB, mB, joint->linearImpulse );
+	wB = b3Add( wB, b3MulMV( iB, b3Add( b3Cross( rB, joint->linearImpulse ), joint->angularImpulse ) ) );
+
+	if ( stateA->flags & b3_dynamicFlag )
+	{
+		stateA->linearVelocity = vA;
+		stateA->angularVelocity = wA;
+	}
+
+	if ( stateB->flags & b3_dynamicFlag )
+	{
+		stateB->linearVelocity = vB;
+		stateB->angularVelocity = wB;
+	}
+}
+
+void b3SolveWeldJoint( b3JointSim* base, b3StepContext* context, bool useBias )
+{
+	float mA = base->invMassA;
+	float mB = base->invMassB;
+	b3Matrix3 iA = base->invIA;
+	b3Matrix3 iB = base->invIB;
+
+	// dummy state for static bodies
+	b3BodyState dummyState = b3_identityBodyState;
+
+	b3WeldJoint* joint = &base->weldJoint;
+	b3BodyState* stateA = joint->indexA == B3_NULL_INDEX ? &dummyState : context->states + joint->indexA;
+	b3BodyState* stateB = joint->indexB == B3_NULL_INDEX ? &dummyState : context->states + joint->indexB;
+
+	b3Vec3 vA = stateA->linearVelocity;
+	b3Vec3 wA = stateA->angularVelocity;
+	b3Vec3 vB = stateB->linearVelocity;
+	b3Vec3 wB = stateB->angularVelocity;
+
+	bool fixedRotation = base->fixedRotation;
+	b3Quat quatA = b3MulQuat( stateA->deltaRotation, joint->frameA.q );
+	b3Quat quatB = b3MulQuat( stateB->deltaRotation, joint->frameB.q );
+
+	if ( b3DotQuat( quatA, quatB ) < 0.0f )
+	{
+		// this keeps the rotation angle in the range [-pi, pi]
+		quatB = b3NegateQuat( quatB );
+	}
+
+	b3Quat relQ = b3InvMulQuat( quatA, quatB );
+
+	// angular constraint
+	if ( fixedRotation == false )
+	{
+		b3Vec3 bias = b3Vec3_zero;
+		float massScale = 1.0f;
+		float impulseScale = 0.0f;
+		if ( useBias || joint->angularHertz > 0.0f )
+		{
+			b3Quat targetQuat = b3Quat_identity;
+			b3Vec3 deltaRotation = b3DeltaQuatToRotation( relQ, targetQuat );
+			b3Vec3 c = b3Neg( b3RotateVector( quatA, deltaRotation ) );
+
+			bias = b3MulSV( joint->angularSpring.biasRate, c );
+			massScale = joint->angularSpring.massScale;
+			impulseScale = joint->angularSpring.impulseScale;
+		}
+
+		b3Vec3 cdot = b3Sub( wB, wA );
+		b3Vec3 impulse = b3MulSub( b3MulSV( -massScale, b3MulMV( joint->angularMass, b3Add( cdot, bias ) ) ), impulseScale, joint->angularImpulse );
+		joint->angularImpulse = b3Add( joint->angularImpulse, impulse );
+
+		wA = b3Sub( wA, b3MulMV( iA, impulse ) );
+		wB = b3Add( wB, b3MulMV( iB, impulse ) );
+	}
+
+	// linear constraint
+	{
+		b3Vec3 rA = b3RotateVector( stateA->deltaRotation, joint->frameA.p );
+		b3Vec3 rB = b3RotateVector( stateB->deltaRotation, joint->frameB.p );
+
+		b3Vec3 cdot = b3Sub( b3Add( vB, b3Cross( wB, rB ) ), b3Add( vA, b3Cross( wA, rA ) ) );
+
+		b3Vec3 bias = b3Vec3_zero;
+		float massScale = 1.0f;
+		float impulseScale = 0.0f;
+		if ( useBias || joint->linearHertz > 0.0f )
+		{
+			b3Vec3 dcA = stateA->deltaPosition;
+			b3Vec3 dcB = stateB->deltaPosition;
+
+			b3Vec3 separation = b3Add( b3Add( b3Sub( dcB, dcA ), b3Sub( rB, rA ) ), joint->deltaCenter );
+
+			bias = b3MulSV( joint->linearSpring.biasRate, separation );
+			massScale = joint->linearSpring.massScale;
+			impulseScale = joint->linearSpring.impulseScale;
+		}
+
+		//// K = [(1/m1 + 1/m2) * eye(2) - skew(r1) * invI1 * skew(r1) - skew(r2) * invI2 * skew(r2)]
+		b3Matrix3 sA = b3Skew( rA );
+		b3Matrix3 sB = b3Skew( rB );
+		b3Matrix3 kA = b3MulMM( sA, b3MulMM( base->invIA, sA ) );
+		b3Matrix3 kB = b3MulMM( sB, b3MulMM( base->invIB, sB ) );
+		b3Matrix3 k = b3NegateMat3( b3AddMM( kA, kB ) );
+		k.cx.x += mA + mB;
+		k.cy.y += mA + mB;
+		k.cz.z += mA + mB;
+
+		b3Vec3 b = b3Solve3( k, b3Add( cdot, bias ) );
+
+		b3Vec3 impulse = b3MulSub( b3MulSV( -massScale, b ), impulseScale, joint->linearImpulse );
+		joint->linearImpulse = b3Add( joint->linearImpulse, impulse );
+
+		vA = b3MulSub( vA, mA, impulse );
+		wA = b3Sub( wA, b3MulMV( iA, b3Cross( rA, impulse ) ) );
+		vB = b3MulAdd( vB, mB, impulse );
+		wB = b3Add( wB, b3MulMV( iB, b3Cross( rB, impulse ) ) );
+	}
+
+	if ( stateA->flags & b3_dynamicFlag )
+	{
+		stateA->linearVelocity = vA;
+		stateA->angularVelocity = wA;
+	}
+
+	if ( stateB->flags & b3_dynamicFlag )
+	{
+		stateB->linearVelocity = vB;
+		stateB->angularVelocity = wB;
+	}
+}
+
+void b3DrawWeldJoint( b3DebugDraw* draw, b3JointSim* base, b3WorldTransform transformA, b3WorldTransform transformB, float scale )
+{
+	b3WorldTransform frameA = b3MulWorldTransforms( transformA, base->localFrameA );
+	b3WorldTransform frameB = b3MulWorldTransforms( transformB, base->localFrameB );
+
+	b3Vec3 extents = { 0.1f * scale, 0.05f * scale, 0.025f * scale };
+	draw->DrawBoxFcn( extents, frameA, b3_colorDarkOrange, draw->context );
+	draw->DrawBoxFcn( extents, frameB, b3_colorDarkCyan, draw->context );
+}

vendor/box3d/src/src/world_snapshot.h

@@ -0,0 +1,21 @@
+// SPDX-FileCopyrightText: 2026 Erin Catto
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include "recording.h"
+#include "recording_replay.h"
+
+#include <stdbool.h>
+#include <stdint.h>
+
+typedef struct b3World b3World;
+
+// Serialize the live world into buf, interning shape geometry into rec->registry.
+// On success buf holds a self-contained snapshot image. Returns the byte count.
+int b3SerializeWorld( b3World* world, b3RecBuffer* buf, b3Recording* rec );
+
+// Overwrite a freshly-created (shell) world with the simulation state held in the
+// snapshot image [data, size). Geometry references are resolved via the shared
+// registry slots in rdr. Returns false on a corrupt or incompatible image.
+bool b3DeserializeIntoShell( const uint8_t* data, int size, b3World* world, b3RecReader* rdr );