diff --git a/vendor/box3d/lib/box3d.lib b/vendor/box3d/lib/box3d.lib index 0448b70ec..38b5dad36 100644 Binary files a/vendor/box3d/lib/box3d.lib and b/vendor/box3d/lib/box3d.lib differ diff --git a/vendor/box3d/src/build.bat b/vendor/box3d/src/build.bat new file mode 100644 index 000000000..55145a77c --- /dev/null +++ b/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 diff --git a/vendor/box3d/src/src/CMakeLists.txt b/vendor/box3d/src/src/CMakeLists.txt new file mode 100644 index 000000000..0460f85ba --- /dev/null +++ b/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 + $ + $ + $ + 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 "$<$: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" +) diff --git a/vendor/box3d/src/src/aabb.c b/vendor/box3d/src/src/aabb.c new file mode 100644 index 000000000..6045bd82f --- /dev/null +++ b/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 + +// 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; +} diff --git a/vendor/box3d/src/src/aabb.h b/vendor/box3d/src/src/aabb.h new file mode 100644 index 000000000..97a00b189 --- /dev/null +++ b/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, + }; +} diff --git a/vendor/box3d/src/src/algorithm.h b/vendor/box3d/src/src/algorithm.h new file mode 100644 index 000000000..155bb310d --- /dev/null +++ b/vendor/box3d/src/src/algorithm.h @@ -0,0 +1,19 @@ +// SPDX-FileCopyrightText: 2026 Erin Catto +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include + +// 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 ) diff --git a/vendor/box3d/src/src/arena_allocator.c b/vendor/box3d/src/src/arena_allocator.c new file mode 100644 index 000000000..8543e5782 --- /dev/null +++ b/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 +#include + +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; +} diff --git a/vendor/box3d/src/src/arena_allocator.h b/vendor/box3d/src/src/arena_allocator.h new file mode 100644 index 000000000..41422b91b --- /dev/null +++ b/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 +#include + +#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; +} diff --git a/vendor/box3d/src/src/bitset.c b/vendor/box3d/src/src/bitset.c new file mode 100644 index 000000000..319725bfc --- /dev/null +++ b/vendor/box3d/src/src/bitset.c @@ -0,0 +1,66 @@ +// SPDX-FileCopyrightText: 2025 Erin Catto +// SPDX-License-Identifier: MIT + +#include "bitset.h" + +#include + +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]; + } +} diff --git a/vendor/box3d/src/src/bitset.h b/vendor/box3d/src/src/bitset.h new file mode 100644 index 000000000..ff771d967 --- /dev/null +++ b/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 +#include + +// 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 ); +} diff --git a/vendor/box3d/src/src/block_allocator.c b/vendor/box3d/src/src/block_allocator.c new file mode 100644 index 000000000..4c1f1544e --- /dev/null +++ b/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; +} diff --git a/vendor/box3d/src/src/block_allocator.h b/vendor/box3d/src/src/block_allocator.h new file mode 100644 index 000000000..e9922d012 --- /dev/null +++ b/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 ); diff --git a/vendor/box3d/src/src/body.c b/vendor/box3d/src/src/body.c new file mode 100644 index 000000000..b78642c6c --- /dev/null +++ b/vendor/box3d/src/src/body.c @@ -0,0 +1,2454 @@ +// SPDX-FileCopyrightText: 2025 Erin Catto +// SPDX-License-Identifier: MIT + +#include "body.h" + +#include "aabb.h" +#include "contact.h" +#include "core.h" +#include "id_pool.h" +#include "island.h" +#include "joint.h" +#include "physics_world.h" +#include "recording.h" +#include "sensor.h" +#include "shape.h" +#include "solver_set.h" + +// needed for dll export +#include "box3d/box3d.h" +#include "box3d/id.h" + +#include + +// Get a validated body from a world using an id. +b3Body* b3GetBodyFullId( b3World* world, b3BodyId bodyId ) +{ + B3_ASSERT( b3Body_IsValid( bodyId ) ); + + // id index starts at one so that zero can represent null + // id index starts at one so that zero can represent null + return b3Array_Get( world->bodies, bodyId.index1 - 1 ); +} + +b3WorldTransform b3GetBodyTransformQuick( b3World* world, b3Body* body ) +{ + b3SolverSet* set = b3Array_Get( world->solverSets, body->setIndex ); + b3BodySim* bodySim = b3Array_Get( set->bodySims, body->localIndex ); + return bodySim->transform; +} + +b3WorldTransform b3GetBodyTransform( b3World* world, int bodyId ) +{ + b3Body* body = b3Array_Get( world->bodies, bodyId ); + return b3GetBodyTransformQuick( world, body ); +} + +// Create a b3BodyId from a raw id. +b3BodyId b3MakeBodyId( b3World* world, int bodyId ) +{ + b3Body* body = b3Array_Get( world->bodies, bodyId ); + return (b3BodyId){ bodyId + 1, world->worldId, body->generation }; +} + +b3BodySim* b3GetBodySim( b3World* world, b3Body* body ) +{ + b3SolverSet* set = b3Array_Get( world->solverSets, body->setIndex ); + b3BodySim* bodySim = b3Array_Get( set->bodySims, body->localIndex ); + return bodySim; +} + +b3BodyState* b3GetBodyState( b3World* world, b3Body* body ) +{ + if ( body->setIndex == b3_awakeSet ) + { + b3SolverSet* set = b3Array_Get( world->solverSets, b3_awakeSet ); + return b3Array_Get( set->bodyStates, body->localIndex ); + } + + return NULL; +} + +void b3SyncBodyFlags( b3World* world, b3Body* body ) +{ + // Never sync transient flags + uint32_t flags = body->flags & ~b3_bodyTransientFlags; + + b3BodySim* bodySim = b3GetBodySim( world, body ); + bodySim->flags = flags; + + b3BodyState* bodyState = b3GetBodyState( world, body ); + if ( bodyState != NULL ) + { + bodyState->flags = flags; + } +} + +static void b3CreateIslandForBody( b3World* world, int setIndex, b3Body* body ) +{ + B3_ASSERT( body->islandId == B3_NULL_INDEX ); + B3_ASSERT( setIndex != b3_disabledSet ); + + b3Island* island = b3CreateIsland( world, setIndex ); + b3Array_Push( island->bodies, body->id ); + body->islandId = island->islandId; + body->islandIndex = 0; + + b3ValidateIsland( world, island->islandId ); +} + +static void b3RemoveBodyFromIsland( b3World* world, b3Body* body ) +{ + if ( body->islandId == B3_NULL_INDEX ) + { + B3_ASSERT( body->islandIndex == B3_NULL_INDEX ); + return; + } + + int islandId = body->islandId; + b3Island* island = b3Array_Get( world->islands, islandId ); + { + int localIndex = body->islandIndex; + int movedBodyId = island->bodies.data[island->bodies.count - 1]; + island->bodies.data[localIndex] = movedBodyId; + B3_VALIDATE( world->bodies.data[movedBodyId].islandIndex == island->bodies.count - 1 ); + world->bodies.data[movedBodyId].islandIndex = localIndex; + island->bodies.count -= 1; + } + + if ( island->bodies.count == 0 ) + { + // Destroy empty island + B3_ASSERT( island->contacts.count == 0 ); + B3_ASSERT( island->joints.count == 0 ); + + // Free the island + b3DestroyIsland( world, island->islandId ); + } + else + { + b3ValidateIsland( world, islandId ); + } + + body->islandId = B3_NULL_INDEX; + body->islandIndex = B3_NULL_INDEX; +} + +static void b3DestroyBodyContacts( b3World* world, b3Body* body, bool wakeBodies ) +{ + // Destroy the attached contacts + int edgeKey = body->headContactKey; + while ( edgeKey != B3_NULL_INDEX ) + { + int contactId = edgeKey >> 1; + int edgeIndex = edgeKey & 1; + + b3Contact* contact = b3Array_Get( world->contacts, contactId ); + edgeKey = contact->edges[edgeIndex].nextKey; + b3DestroyContact( world, contact, wakeBodies ); + } + + b3ValidateSolverSets( world ); +} + +b3BodyId b3CreateBody( b3WorldId worldId, const b3BodyDef* def ) +{ + B3_CHECK_DEF( def ); + B3_ASSERT( b3IsValidPosition( def->position ) ); + B3_ASSERT( b3IsValidQuat( def->rotation ) ); + B3_ASSERT( b3IsValidVec3( def->linearVelocity ) ); + B3_ASSERT( b3IsValidVec3( def->angularVelocity ) ); + B3_ASSERT( b3IsValidFloat( def->linearDamping ) && def->linearDamping >= 0.0f ); + B3_ASSERT( b3IsValidFloat( def->angularDamping ) && def->angularDamping >= 0.0f ); + B3_ASSERT( b3IsValidFloat( def->sleepThreshold ) && def->sleepThreshold >= 0.0f ); + B3_ASSERT( b3IsValidFloat( def->gravityScale ) ); + + b3World* world = b3GetUnlockedWorldFromId( worldId ); + + if ( world == NULL ) + { + return b3_nullBodyId; + } + + world->locked = true; + + bool isAwake = ( def->isAwake || def->enableSleep == false ) && def->isEnabled; + + // determine the solver set + int setId; + if ( def->isEnabled == false ) + { + // any body type can be disabled + setId = b3_disabledSet; + } + else if ( def->type == b3_staticBody ) + { + setId = b3_staticSet; + } + else if ( isAwake == true ) + { + setId = b3_awakeSet; + } + else + { + // new set for a sleeping body in its own island + setId = b3AllocId( &world->solverSetIdPool ); + if ( setId == world->solverSets.count ) + { + // Create a zero initialized solver set. All sub-arrays are also zero initialized. + b3Array_Push( world->solverSets, (b3SolverSet){ 0 } ); + } + else + { + B3_ASSERT( world->solverSets.data[setId].setIndex == B3_NULL_INDEX ); + } + + world->solverSets.data[setId].setIndex = setId; + } + + B3_ASSERT( 0 <= setId && setId < world->solverSets.count ); + + int bodyId = b3AllocId( &world->bodyIdPool ); + + uint32_t lockFlags = 0; + lockFlags |= def->motionLocks.linearX ? b3_lockLinearX : 0; + lockFlags |= def->motionLocks.linearY ? b3_lockLinearY : 0; + lockFlags |= def->motionLocks.linearZ ? b3_lockLinearZ : 0; + lockFlags |= def->motionLocks.angularX ? b3_lockAngularX : 0; + lockFlags |= def->motionLocks.angularY ? b3_lockAngularY : 0; + lockFlags |= def->motionLocks.angularZ ? b3_lockAngularZ : 0; + + b3SolverSet* set = b3Array_Get( world->solverSets, setId ); + b3BodySim* bodySim = b3Array_Emplace( set->bodySims ); + *bodySim = (b3BodySim){ 0 }; + bodySim->transform.p = def->position; + bodySim->transform.q = def->rotation; + bodySim->center = def->position; + bodySim->rotation0 = bodySim->transform.q; + bodySim->center0 = bodySim->center; + bodySim->localCenter = b3Vec3_zero; + bodySim->force = b3Vec3_zero; + bodySim->torque = b3Vec3_zero; + bodySim->invMass = 0.0f; + bodySim->invInertiaLocal = b3Mat3_zero; + bodySim->minExtent = B3_HUGE; + bodySim->maxExtent = b3Vec3_zero; + bodySim->linearDamping = def->linearDamping; + bodySim->angularDamping = def->angularDamping; + bodySim->gravityScale = def->gravityScale; + bodySim->bodyId = bodyId; + bodySim->flags = lockFlags; + bodySim->flags |= def->isBullet ? b3_isBullet : 0; + bodySim->flags |= def->allowFastRotation ? b3_allowFastRotation : 0; + bodySim->flags |= def->type == b3_dynamicBody ? b3_dynamicFlag : 0; + bodySim->flags |= def->enableSleep ? b3_enableSleep : 0; + bodySim->flags |= def->enableContactRecycling ? b3_bodyEnableContactRecycling : 0; + + if ( setId == b3_awakeSet ) + { + b3BodyState* bodyState = b3Array_Emplace( set->bodyStates ); + + *bodyState = (b3BodyState){ 0 }; + bodyState->linearVelocity = def->linearVelocity; + bodyState->angularVelocity = def->angularVelocity; + bodyState->deltaRotation = b3Quat_identity; + bodyState->flags = bodySim->flags; + + bodySim->maxAngularVelocity = b3Length( def->angularVelocity ) + 5.0f; + } + + if ( bodyId == world->bodies.count ) + { + b3Array_Push( world->bodies, (b3Body){ 0 } ); + } + else + { + B3_ASSERT( world->bodies.data[bodyId].id == B3_NULL_INDEX ); + } + + b3Body* body = b3Array_Get( world->bodies, bodyId ); + body->userData = def->userData; + body->setIndex = setId; + body->localIndex = set->bodySims.count - 1; + body->generation += 1; + body->headShapeId = B3_NULL_INDEX; + body->shapeCount = 0; + body->headChainId = B3_NULL_INDEX; + body->headContactKey = B3_NULL_INDEX; + body->contactCount = 0; + body->headJointKey = B3_NULL_INDEX; + body->jointCount = 0; + body->islandId = B3_NULL_INDEX; + body->islandIndex = B3_NULL_INDEX; + body->bodyMoveIndex = B3_NULL_INDEX; + body->id = bodyId; + body->sleepThreshold = def->sleepThreshold; + body->sleepTime = 0.0f; + body->sleepVelocity = 0.0f; + body->mass = 0.0f; + body->inertia = b3Mat3_zero; + body->nameId = b3AddName( &world->names, def->name ); + body->type = def->type; + body->flags = bodySim->flags; + + // dynamic and kinematic bodies that are enabled need a island + if ( setId >= b3_awakeSet ) + { + b3CreateIslandForBody( world, setId, body ); + } + + b3ValidateSolverSets( world ); + + b3BodyId id = { bodyId + 1, world->worldId, body->generation }; + + world->locked = false; + + B3_REC_CREATE( world, CreateBody, id, worldId, *def ); + + return id; +} + +bool b3IsBodyAwake( b3World* world, b3Body* body ) +{ + B3_UNUSED( world ); + return body->setIndex == b3_awakeSet; +} + +bool b3WakeBody( b3World* world, b3Body* body ) +{ + if ( body->setIndex >= b3_firstSleepingSet ) + { + b3WakeSolverSet( world, body->setIndex ); + b3ValidateSolverSets( world ); + return true; + } + + return false; +} + +bool b3WakeBodyWithLock( b3World* world, b3Body* body ) +{ + B3_ASSERT( world->locked == false ); + world->locked = true; + bool woke = b3WakeBody( world, body ); + world->locked = false; + return woke; +} + +void b3DestroyBody( b3BodyId bodyId ) +{ + b3World* world = b3GetUnlockedWorld( bodyId.world0 ); + if ( world == NULL ) + { + return; + } + + B3_REC( world, DestroyBody, bodyId ); + + world->locked = true; + + b3Body* body = b3GetBodyFullId( world, bodyId ); + + // Wake bodies attached to this body, even if this body is static. + bool wakeBodies = true; + + // Destroy the attached joints + int edgeKey = body->headJointKey; + while ( edgeKey != B3_NULL_INDEX ) + { + int jointId = edgeKey >> 1; + int edgeIndex = edgeKey & 1; + + b3Joint* joint = b3Array_Get( world->joints, jointId ); + edgeKey = joint->edges[edgeIndex].nextKey; + + // Careful because this modifies the list being traversed + b3DestroyJointInternal( world, joint, wakeBodies ); + } + + // Destroy all contacts attached to this body. + b3DestroyBodyContacts( world, body, wakeBodies ); + + // Destroy the attached shapes and their broad-phase proxies. + int shapeId = body->headShapeId; + while ( shapeId != B3_NULL_INDEX ) + { + b3Shape* shape = b3Array_Get( world->shapes, shapeId ); + + if ( shape->sensorIndex != B3_NULL_INDEX ) + { + b3DestroySensor( world, shape ); + } + + b3DestroyShapeProxy( shape, &world->broadPhase ); + + b3DestroyShapeAllocations( world, shape ); + + // Return shape to free list. + b3FreeId( &world->shapeIdPool, shapeId ); + shape->id = B3_NULL_INDEX; + + shapeId = shape->nextShapeId; + } + + b3RemoveBodyFromIsland( world, body ); + + // Remove body sim from solver set that owns it + b3SolverSet* set = b3Array_Get( world->solverSets, body->setIndex ); + int movedIndex = b3Array_RemoveSwap( set->bodySims, body->localIndex ); + if ( movedIndex != B3_NULL_INDEX ) + { + // Fix moved body index + b3BodySim* movedSim = set->bodySims.data + body->localIndex; + int movedId = movedSim->bodyId; + b3Body* movedBody = b3Array_Get( world->bodies, movedId ); + B3_ASSERT( movedBody->localIndex == movedIndex ); + movedBody->localIndex = body->localIndex; + } + + // Remove body state from awake set + if ( body->setIndex == b3_awakeSet ) + { + int result = b3Array_RemoveSwap( set->bodyStates, body->localIndex ); + B3_UNUSED( result ); + B3_ASSERT( result == movedIndex ); + } + else if ( set->setIndex >= b3_firstSleepingSet && set->bodySims.count == 0 ) + { + // Remove solver set if it's now an orphan. + b3DestroySolverSet( world, set->setIndex ); + } + + // Free body and id (preserve body revision) + b3FreeId( &world->bodyIdPool, body->id ); + + body->setIndex = B3_NULL_INDEX; + body->localIndex = B3_NULL_INDEX; + body->id = B3_NULL_INDEX; + + b3ValidateSolverSets( world ); + + world->locked = false; +} + +int b3Body_GetContactCapacity( b3BodyId bodyId ) +{ + b3World* world = b3GetUnlockedWorld( bodyId.world0 ); + if ( world == NULL ) + { + return 0; + } + + b3Body* body = b3GetBodyFullId( world, bodyId ); + + // Conservative and fast + return body->contactCount; +} + +int b3Body_GetContactData( b3BodyId bodyId, b3ContactData* contactData, int capacity ) +{ + b3World* world = b3GetUnlockedWorld( bodyId.world0 ); + if ( world == NULL ) + { + return 0; + } + + b3Body* body = b3GetBodyFullId( world, bodyId ); + + int contactKey = body->headContactKey; + int index = 0; + while ( contactKey != B3_NULL_INDEX && index < capacity ) + { + int contactId = contactKey >> 1; + int edgeIndex = contactKey & 1; + + b3Contact* contact = b3Array_Get( world->contacts, contactId ); + + // Is contact touching? + if ( contact->flags & b3_contactTouchingFlag ) + { + b3Shape* shapeA = b3Array_Get( world->shapes, contact->shapeIdA ); + b3Shape* shapeB = b3Array_Get( world->shapes, contact->shapeIdB ); + + contactData[index].contactId = (b3ContactId){ contact->contactId + 1, bodyId.world0, 0, contact->generation }; + contactData[index].shapeIdA = (b3ShapeId){ shapeA->id + 1, bodyId.world0, shapeA->generation }; + contactData[index].shapeIdB = (b3ShapeId){ shapeB->id + 1, bodyId.world0, shapeB->generation }; + contactData[index].manifolds = contact->manifolds; + contactData[index].manifoldCount = contact->manifoldCount; + index += 1; + } + + contactKey = contact->edges[edgeIndex].nextKey; + } + + B3_ASSERT( index <= capacity ); + + return index; +} + +b3AABB b3Body_ComputeAABB( b3BodyId bodyId ) +{ + b3World* world = b3GetUnlockedWorld( bodyId.world0 ); + if ( world == NULL ) + { + return (b3AABB){ 0 }; + } + + b3Body* body = b3GetBodyFullId( world, bodyId ); + if ( body->headShapeId == B3_NULL_INDEX ) + { + b3WorldTransform transform = b3GetBodyTransform( world, body->id ); + b3Vec3 p = b3ToVec3( transform.p ); + return (b3AABB){ p, p }; + } + + b3Shape* shape = b3Array_Get( world->shapes, body->headShapeId ); + b3AABB aabb = shape->aabb; + while ( shape->nextShapeId != B3_NULL_INDEX ) + { + shape = b3Array_Get( world->shapes, shape->nextShapeId ); + aabb = b3AABB_Union( aabb, shape->aabb ); + } + + return aabb; +} + +float b3Body_GetClosestPoint( b3BodyId bodyId, b3Vec3* result, b3Vec3 target ) +{ + b3World* world = b3GetUnlockedWorld( bodyId.world0 ); + if ( world == NULL ) + { + *result = (b3Vec3){ 0 }; + return 0.0f; + } + + b3Body* body = b3GetBodyFullId( world, bodyId ); + b3WorldTransform worldTransform = b3GetBodyTransform( world, body->id ); + b3Transform transform = b3ToRelativeTransform( worldTransform, b3Pos_zero ); + + float closestDistance = FLT_MAX; + b3Vec3 closestPoint = transform.p; + + b3DistanceInput input = { 0 }; + input.proxyA = (b3ShapeProxy){ &target, 1, 0.0f }; + + // Target rides in frame A at the origin, so the relative pose of the shape in A is the body transform + input.transform = transform; + input.useRadii = false; + + int shapeId = body->headShapeId; + while ( shapeId != B3_NULL_INDEX ) + { + b3Shape* shape = b3Array_Get( world->shapes, shapeId ); + shapeId = shape->nextShapeId; + + b3ShapeType type = shape->type; + if ( type != b3_sphereShape && type != b3_capsuleShape && type != b3_hullShape ) + { + continue; + } + + input.proxyB = b3MakeShapeProxy( shape ); + + b3SimplexCache cache = { 0 }; + b3DistanceOutput output = b3ShapeDistance( &input, &cache, NULL, 0 ); + if ( output.distance < closestDistance ) + { + closestDistance = output.distance; + closestPoint = output.pointB; + } + } + + *result = closestPoint; + return closestDistance; +} + +b3BodyCastResult b3Body_CastRay( b3BodyId bodyId, b3Pos origin, b3Vec3 translation, b3QueryFilter filter, float maxFraction, + b3WorldTransform bodyTransform ) +{ + b3World* world = b3GetUnlockedWorld( bodyId.world0 ); + if ( world == NULL ) + { + return (b3BodyCastResult){ 0 }; + } + + b3BodyCastResult result = { 0 }; + b3Body* body = b3GetBodyFullId( world, bodyId ); + + // The consistent framing is to center on the ray origin. + b3RayCastInput shapeInput = { 0 }; + shapeInput.origin = b3Vec3_zero; + shapeInput.translation = translation; + shapeInput.maxFraction = maxFraction; + + b3Transform transform = b3ToRelativeTransform( bodyTransform, origin ); + + int shapeId = body->headShapeId; + while ( shapeId != B3_NULL_INDEX ) + { + b3Shape* shape = b3Array_Get( world->shapes, shapeId ); + shapeId = shape->nextShapeId; + + if ( b3ShouldQueryCollide( &shape->filter, &filter ) == false ) + { + continue; + } + + b3CastOutput shapeOutput = b3RayCastShape( shape, transform, &shapeInput ); + + if ( shapeOutput.hit == false ) + { + continue; + } + + if ( shapeOutput.fraction > shapeInput.maxFraction ) + { + continue; + } + + // Careful with id, shapeId is the next shape. + b3ShapeId id = { shape->id + 1, bodyId.world0, shape->generation }; + + int materialIndex = b3ClampInt( shapeOutput.materialIndex, 0, shape->materialCount - 1 ); + uint64_t userMaterialId = b3GetShapeMaterials( shape )[materialIndex].userMaterialId; + + result = (b3BodyCastResult){ + .shapeId = id, + .point = b3OffsetPos( origin, shapeOutput.point ), + .normal = shapeOutput.normal, + .fraction = shapeOutput.fraction, + .triangleIndex = shapeOutput.triangleIndex, + .userMaterialId = userMaterialId, + .iterations = shapeOutput.iterations, + .hit = true, + }; + + shapeInput.maxFraction = shapeOutput.fraction; + } + + return result; +} + +b3BodyCastResult b3Body_CastShape( b3BodyId bodyId, b3Pos origin, const b3ShapeProxy* proxy, b3Vec3 translation, + b3QueryFilter filter, float maxFraction, bool canEncroach, b3WorldTransform bodyTransform ) +{ + b3World* world = b3GetUnlockedWorld( bodyId.world0 ); + if ( world == NULL ) + { + return (b3BodyCastResult){ 0 }; + } + + b3BodyCastResult result = { 0 }; + b3Body* body = b3GetBodyFullId( world, bodyId ); + + b3Transform transform = b3ToRelativeTransform( bodyTransform, origin ); + + b3ShapeCastInput shapeInput = { 0 }; + shapeInput.proxy = *proxy; + shapeInput.translation = translation; + shapeInput.maxFraction = maxFraction; + shapeInput.canEncroach = canEncroach; + + int shapeId = body->headShapeId; + while ( shapeId != B3_NULL_INDEX ) + { + b3Shape* shape = b3Array_Get( world->shapes, shapeId ); + shapeId = shape->nextShapeId; + + if ( b3ShouldQueryCollide( &shape->filter, &filter ) == false ) + { + continue; + } + + b3CastOutput shapeOutput = b3ShapeCastShape( shape, transform, &shapeInput ); + + if ( shapeOutput.hit == false ) + { + continue; + } + + if ( shapeOutput.fraction > shapeInput.maxFraction ) + { + continue; + } + + // Careful with id, shapeId is the next shape. + b3ShapeId id = { shape->id + 1, bodyId.world0, shape->generation }; + int materialIndex = b3ClampInt( shapeOutput.materialIndex, 0, shape->materialCount - 1 ); + uint64_t userMaterialId = b3GetShapeMaterials( shape )[materialIndex].userMaterialId; + + result = (b3BodyCastResult){ + .shapeId = id, + .point = b3OffsetPos( origin, shapeOutput.point ), + .normal = shapeOutput.normal, + .fraction = shapeOutput.fraction, + .triangleIndex = shapeOutput.triangleIndex, + .userMaterialId = userMaterialId, + .iterations = shapeOutput.iterations, + .hit = true, + }; + + shapeInput.maxFraction = shapeOutput.fraction; + } + + return result; +} + +bool b3Body_OverlapShape( b3BodyId bodyId, b3Pos origin, const b3ShapeProxy* proxy, b3QueryFilter filter, + b3WorldTransform bodyTransform ) +{ + b3World* world = b3GetUnlockedWorld( bodyId.world0 ); + if ( world == NULL ) + { + return false; + } + + b3Body* body = b3GetBodyFullId( world, bodyId ); + b3Transform transform = b3ToRelativeTransform( bodyTransform, origin ); + + int shapeId = body->headShapeId; + while ( shapeId != B3_NULL_INDEX ) + { + b3Shape* shape = b3Array_Get( world->shapes, shapeId ); + shapeId = shape->nextShapeId; + + if ( b3ShouldQueryCollide( &shape->filter, &filter ) == false ) + { + continue; + } + + bool overlaps = b3OverlapShape( shape, transform, proxy ); + if ( overlaps ) + { + return true; + } + } + + return false; +} + +int b3Body_CollideMover( b3BodyId bodyId, b3BodyPlaneResult* bodyPlanes, int planeCapacity, b3Pos origin, const b3Capsule* mover, + b3QueryFilter filter, b3WorldTransform bodyTransform ) +{ + b3World* world = b3GetUnlockedWorld( bodyId.world0 ); + if ( world == NULL ) + { + return 0; + } + + if ( planeCapacity == 0 ) + { + return 0; + } + + int resultCount = 0; + b3Body* body = b3GetBodyFullId( world, bodyId ); + + b3Transform transform = b3ToRelativeTransform( bodyTransform, origin ); + + int shapeId = body->headShapeId; + while ( shapeId != B3_NULL_INDEX ) + { + b3Shape* shape = b3Array_Get( world->shapes, shapeId ); + shapeId = shape->nextShapeId; + + if ( b3ShouldQueryCollide( &shape->filter, &filter ) == false ) + { + continue; + } + + b3ShapeType type = shape->type; + if ( type != b3_sphereShape && type != b3_capsuleShape && type != b3_hullShape ) + { + continue; + } + + b3PlaneResult plane; + int count = b3CollideMover( &plane, 1, shape, transform, mover ); + + if ( count > 0 ) + { + b3ShapeId id = { shape->id + 1, bodyId.world0, shape->generation }; + bodyPlanes[resultCount] = (b3BodyPlaneResult){ .shapeId = id, .result = plane }; + resultCount += 1; + if ( resultCount == planeCapacity ) + { + return resultCount; + } + } + } + + return resultCount; +} + +void b3UpdateBodyMassData( b3World* world, b3Body* body ) +{ + b3BodySim* bodySim = b3GetBodySim( world, body ); + + // Mass is no longer dirty + body->flags &= ~b3_dirtyMass; + b3SyncBodyFlags( world, body ); + + // Compute mass data from shapes. Each shape has its own density. + body->mass = 0.0f; + body->inertia = b3Mat3_zero; + + bodySim->invMass = 0.0f; + bodySim->invInertiaLocal = b3Mat3_zero; + bodySim->invInertiaWorld = b3Mat3_zero; + bodySim->localCenter = b3Vec3_zero; + bodySim->minExtent = B3_HUGE; + bodySim->maxExtent = b3Vec3_zero; + + if ( body->headShapeId == B3_NULL_INDEX ) + { + return; + } + + // Static and kinematic sims have zero mass. + if ( body->type != b3_dynamicBody ) + { + bodySim->center = bodySim->transform.p; + bodySim->center0 = bodySim->center; + + // Need extents for kinematic bodies for sleeping to work correctly. + if ( body->type == b3_kinematicBody ) + { + int shapeId = body->headShapeId; + while ( shapeId != B3_NULL_INDEX ) + { + const b3Shape* s = b3Array_Get( world->shapes, shapeId ); + + b3ShapeExtent extent = b3ComputeShapeExtent( s, b3Vec3_zero ); + bodySim->minExtent = b3MinFloat( bodySim->minExtent, extent.minExtent ); + bodySim->maxExtent = b3Max( bodySim->maxExtent, extent.maxExtent ); + + shapeId = s->nextShapeId; + } + } + + return; + } + + int shapeCount = body->shapeCount; + b3MassData* masses = b3StackAlloc( &world->stack, shapeCount * sizeof( b3MassData ), "mass data" ); + + // Accumulate mass over all shapes. + b3Vec3 localCenter = b3Vec3_zero; + int shapeId = body->headShapeId; + int shapeIndex = 0; + while ( shapeId != B3_NULL_INDEX ) + { + const b3Shape* s = b3Array_Get( world->shapes, shapeId ); + shapeId = s->nextShapeId; + + if ( s->density == 0.0f ) + { + masses[shapeIndex] = (b3MassData){ 0 }; + shapeIndex += 1; + continue; + } + + b3MassData massData = b3ComputeShapeMass( s ); + body->mass += massData.mass; + localCenter = b3MulAdd( localCenter, massData.mass, massData.center ); + + masses[shapeIndex] = massData; + shapeIndex += 1; + } + + // Compute center of mass. + if ( body->mass > 0.0f ) + { + bodySim->invMass = 1.0f / body->mass; + localCenter = b3MulSV( bodySim->invMass, localCenter ); + } + + // Second loop to accumulate the rotational inertia about the center of mass + for ( shapeIndex = 0; shapeIndex < shapeCount; ++shapeIndex ) + { + b3MassData massData = masses[shapeIndex]; + if ( massData.mass == 0.0f ) + { + continue; + } + + // Shift to center of mass. This is safe because it can only increase. + b3Vec3 offset = b3Sub( localCenter, massData.center ); + b3Matrix3 inertia = b3AddMM( massData.inertia, b3Steiner( massData.mass, offset ) ); + body->inertia = b3AddMM( body->inertia, inertia ); + } + + b3StackFree( &world->stack, masses ); + masses = NULL; + + float det = b3Det( body->inertia ); + B3_ASSERT( det >= 0.0f ); + + if ( det > 0.0f ) + { + // This call is faster than b3Invert + bodySim->invInertiaLocal = b3InvertT( body->inertia ); + + b3Matrix3 rotationMatrix = b3MakeMatrixFromQuat( bodySim->transform.q ); + bodySim->invInertiaWorld = b3MulMM( b3MulMM( rotationMatrix, bodySim->invInertiaLocal ), b3Transpose( rotationMatrix ) ); + } + + // Move center of mass. + b3Pos oldCenter = bodySim->center; + bodySim->localCenter = localCenter; + bodySim->center = b3TransformWorldPoint( bodySim->transform, bodySim->localCenter ); + bodySim->center0 = bodySim->center; + + // Update center of mass velocity + b3BodyState* state = b3GetBodyState( world, body ); + if ( state != NULL ) + { + b3Vec3 deltaLinear = b3Cross( state->angularVelocity, b3SubPos( bodySim->center, oldCenter ) ); + state->linearVelocity = b3Add( state->linearVelocity, deltaLinear ); + } + + // Compute body extents relative to center of mass + shapeId = body->headShapeId; + while ( shapeId != B3_NULL_INDEX ) + { + b3Shape* s = b3Array_Get( world->shapes, shapeId ); + + b3ShapeExtent extent = b3ComputeShapeExtent( s, localCenter ); + bodySim->minExtent = b3MinFloat( bodySim->minExtent, extent.minExtent ); + bodySim->maxExtent = b3Max( bodySim->maxExtent, extent.maxExtent ); + + shapeId = s->nextShapeId; + } + + // Apply fixed rotation + if ( ( bodySim->flags & b3_fixedRotation ) == b3_fixedRotation ) + { + body->inertia = b3Mat3_zero; + bodySim->invInertiaLocal = b3Mat3_zero; + bodySim->invInertiaWorld = b3Mat3_zero; + } +} + +b3Pos b3Body_GetPosition( b3BodyId bodyId ) +{ + b3World* world = b3GetWorld( bodyId.world0 ); + b3Body* body = b3GetBodyFullId( world, bodyId ); + b3WorldTransform transform = b3GetBodyTransformQuick( world, body ); + return transform.p; +} + +b3Quat b3Body_GetRotation( b3BodyId bodyId ) +{ + b3World* world = b3GetWorld( bodyId.world0 ); + b3Body* body = b3GetBodyFullId( world, bodyId ); + b3WorldTransform transform = b3GetBodyTransformQuick( world, body ); + return transform.q; +} + +b3WorldTransform b3Body_GetTransform( b3BodyId bodyId ) +{ + b3World* world = b3GetWorld( bodyId.world0 ); + b3Body* body = b3GetBodyFullId( world, bodyId ); + return b3GetBodyTransformQuick( world, body ); +} + +b3Vec3 b3Body_GetLocalPoint( b3BodyId bodyId, b3Pos worldPoint ) +{ + b3World* world = b3GetWorld( bodyId.world0 ); + b3Body* body = b3GetBodyFullId( world, bodyId ); + b3WorldTransform transform = b3GetBodyTransformQuick( world, body ); + return b3InvTransformWorldPoint( transform, worldPoint ); +} + +b3Pos b3Body_GetWorldPoint( b3BodyId bodyId, b3Vec3 localPoint ) +{ + b3World* world = b3GetWorld( bodyId.world0 ); + b3Body* body = b3GetBodyFullId( world, bodyId ); + b3WorldTransform transform = b3GetBodyTransformQuick( world, body ); + return b3TransformWorldPoint( transform, localPoint ); +} + +b3Vec3 b3Body_GetLocalVector( b3BodyId bodyId, b3Vec3 worldVector ) +{ + b3World* world = b3GetWorld( bodyId.world0 ); + b3Body* body = b3GetBodyFullId( world, bodyId ); + b3WorldTransform transform = b3GetBodyTransformQuick( world, body ); + return b3InvRotateVector( transform.q, worldVector ); +} + +b3Vec3 b3Body_GetWorldVector( b3BodyId bodyId, b3Vec3 localVector ) +{ + b3World* world = b3GetWorld( bodyId.world0 ); + b3Body* body = b3GetBodyFullId( world, bodyId ); + b3WorldTransform transform = b3GetBodyTransformQuick( world, body ); + return b3RotateVector( transform.q, localVector ); +} + +void b3Body_SetTransform( b3BodyId bodyId, b3Pos position, b3Quat rotation ) +{ + B3_ASSERT( b3IsValidPosition( position ) ); + B3_ASSERT( b3IsValidQuat( rotation ) ); + B3_ASSERT( b3Body_IsValid( bodyId ) ); + b3World* world = b3GetWorld( bodyId.world0 ); + B3_ASSERT( world->locked == false ); + + B3_REC( world, BodySetTransform, bodyId, position, rotation ); + + b3Body* body = b3GetBodyFullId( world, bodyId ); + b3BodySim* bodySim = b3GetBodySim( world, body ); + + bodySim->transform.p = position; + bodySim->transform.q = rotation; + bodySim->center = b3TransformWorldPoint( bodySim->transform, bodySim->localCenter ); + + b3Matrix3 rotationMatrix = b3MakeMatrixFromQuat( bodySim->transform.q ); + bodySim->invInertiaWorld = b3MulMM( b3MulMM( rotationMatrix, bodySim->invInertiaLocal ), b3Transpose( rotationMatrix ) ); + + bodySim->rotation0 = bodySim->transform.q; + bodySim->center0 = bodySim->center; + + b3BroadPhase* broadPhase = &world->broadPhase; + + b3WorldTransform transform = bodySim->transform; + const float speculativeDistance = B3_SPECULATIVE_DISTANCE; + + int shapeId = body->headShapeId; + while ( shapeId != B3_NULL_INDEX ) + { + b3Shape* shape = b3Array_Get( world->shapes, shapeId ); + b3AABB aabb = b3ComputeFatShapeAABB( shape, transform, speculativeDistance ); + shape->aabb = aabb; + + if ( b3AABB_Contains( shape->fatAABB, aabb ) == false ) + { + float margin = shape->aabbMargin; + b3AABB fatAABB; + fatAABB.lowerBound.x = aabb.lowerBound.x - margin; + fatAABB.lowerBound.y = aabb.lowerBound.y - margin; + fatAABB.lowerBound.z = aabb.lowerBound.z - margin; + fatAABB.upperBound.x = aabb.upperBound.x + margin; + fatAABB.upperBound.y = aabb.upperBound.y + margin; + fatAABB.upperBound.z = aabb.upperBound.z + margin; + shape->fatAABB = fatAABB; + + // The body could be disabled + if ( shape->proxyKey != B3_NULL_INDEX ) + { + b3BroadPhase_MoveProxy( broadPhase, shape->proxyKey, fatAABB ); + } + } + + shapeId = shape->nextShapeId; + } +} + +b3Vec3 b3Body_GetLinearVelocity( b3BodyId bodyId ) +{ + b3World* world = b3GetWorld( bodyId.world0 ); + b3Body* body = b3GetBodyFullId( world, bodyId ); + b3BodyState* state = b3GetBodyState( world, body ); + if ( state != NULL ) + { + return state->linearVelocity; + } + return b3Vec3_zero; +} + +b3Vec3 b3Body_GetAngularVelocity( b3BodyId bodyId ) +{ + b3World* world = b3GetWorld( bodyId.world0 ); + b3Body* body = b3GetBodyFullId( world, bodyId ); + b3BodyState* state = b3GetBodyState( world, body ); + if ( state != NULL ) + { + return state->angularVelocity; + } + return b3Vec3_zero; +} + +void b3Body_SetLinearVelocity( b3BodyId bodyId, b3Vec3 linearVelocity ) +{ + B3_ASSERT( b3IsValidVec3( linearVelocity ) ); + + b3World* world = b3GetWorld( bodyId.world0 ); + + B3_REC( world, BodySetLinearVelocity, bodyId, linearVelocity ); + + b3Body* body = b3GetBodyFullId( world, bodyId ); + + if ( body->type == b3_staticBody ) + { + return; + } + + if ( b3LengthSquared( linearVelocity ) > 0.0f ) + { + b3WakeBodyWithLock( world, body ); + } + + b3BodyState* state = b3GetBodyState( world, body ); + if ( state == NULL ) + { + return; + } + + state->linearVelocity = linearVelocity; +} + +void b3Body_SetAngularVelocity( b3BodyId bodyId, b3Vec3 angularVelocity ) +{ + B3_ASSERT( b3IsValidVec3( angularVelocity ) ); + + b3World* world = b3GetWorld( bodyId.world0 ); + + B3_REC( world, BodySetAngularVelocity, bodyId, angularVelocity ); + + b3Body* body = b3GetBodyFullId( world, bodyId ); + + if ( body->type == b3_staticBody ) + { + return; + } + + // Apply locks to avoid waking + b3Vec3 w; + w.x = ( body->flags & b3_lockAngularX ) ? 0.0f : angularVelocity.x; + w.y = ( body->flags & b3_lockAngularY ) ? 0.0f : angularVelocity.y; + w.z = ( body->flags & b3_lockAngularZ ) ? 0.0f : angularVelocity.z; + + if ( b3LengthSquared( w ) != 0.0f ) + { + b3WakeBodyWithLock( world, body ); + } + + b3BodyState* state = b3GetBodyState( world, body ); + if ( state == NULL ) + { + return; + } + + state->angularVelocity = w; +} + +void b3Body_SetTargetTransform( b3BodyId bodyId, b3WorldTransform target, float timeStep, bool wake ) +{ + B3_ASSERT( b3IsValidWorldTransform( target ) ); + + b3World* world = b3GetWorld( bodyId.world0 ); + + B3_REC( world, BodySetTargetTransform, bodyId, target, timeStep, wake ); + + b3Body* body = b3GetBodyFullId( world, bodyId ); + + if ( body->setIndex == b3_disabledSet ) + { + return; + } + + if ( body->type == b3_staticBody || timeStep <= 0.0f ) + { + return; + } + + if ( body->setIndex != b3_awakeSet && wake == false ) + { + return; + } + + b3BodySim* sim = b3GetBodySim( world, body ); + + // Compute linear velocity. The center difference is taken in world precision then demoted. + b3Pos center1 = sim->center; + b3Pos center2 = b3TransformWorldPoint( target, sim->localCenter ); + float invTimeStep = 1.0f / timeStep; + b3Vec3 linearVelocity = b3MulSV( invTimeStep, b3SubPos( center2, center1 ) ); + + // Compute angular velocity: + // q' = 0.5 * w * q + // <~> ( q2 - q1 ) / dt = 0.5 * w * q1 + // <=> w = 2 * ( q2 - q1 ) * Conjugate( q1 ) / dt + b3Quat q1 = sim->transform.q; + b3Quat q2 = target.q; + + // Use the shortest arc quaternion + if ( b3DotQuat( q1, q2 ) < 0.0f ) + { + q2 = b3NegateQuat( q2 ); + } + + b3Quat dq = { b3Sub( q2.v, q1.v ), q2.s - q1.s }; + b3Quat omega = b3MulQuat( dq, b3Conjugate( q1 ) ); + b3Vec3 angularVelocity = b3MulSV( 2.0f * invTimeStep, omega.v ); + + // Early out if the body is asleep already and the desired movement is small + if ( body->setIndex != b3_awakeSet ) + { + float maxVelocity = b3Length( linearVelocity ) + b3Length( b3Mul( angularVelocity, sim->maxExtent ) ); + + // Return if velocity would be sleepy + if ( maxVelocity < body->sleepThreshold ) + { + return; + } + + // Must wake for state to exist + b3WakeBodyWithLock( world, body ); + } + + B3_ASSERT( body->setIndex == b3_awakeSet ); + + b3BodyState* state = b3GetBodyState( world, body ); + state->linearVelocity = linearVelocity; + state->angularVelocity = angularVelocity; +} + +b3Vec3 b3Body_GetLocalPointVelocity( b3BodyId bodyId, b3Vec3 localPoint ) +{ + b3World* world = b3GetWorld( bodyId.world0 ); + b3Body* body = b3GetBodyFullId( world, bodyId ); + b3BodyState* state = b3GetBodyState( world, body ); + if ( state == NULL ) + { + return b3Vec3_zero; + } + + b3SolverSet* set = b3Array_Get( world->solverSets, body->setIndex ); + b3BodySim* bodySim = b3Array_Get( set->bodySims, body->localIndex ); + + b3Vec3 r = b3RotateVector( bodySim->transform.q, b3Sub( localPoint, bodySim->localCenter ) ); + b3Vec3 v = b3Add( state->linearVelocity, b3Cross( state->angularVelocity, r ) ); + return v; +} + +b3Vec3 b3Body_GetWorldPointVelocity( b3BodyId bodyId, b3Pos worldPoint ) +{ + b3World* world = b3GetWorld( bodyId.world0 ); + b3Body* body = b3GetBodyFullId( world, bodyId ); + b3BodyState* state = b3GetBodyState( world, body ); + if ( state == NULL ) + { + return b3Vec3_zero; + } + + b3SolverSet* set = b3Array_Get( world->solverSets, body->setIndex ); + b3BodySim* bodySim = b3Array_Get( set->bodySims, body->localIndex ); + + b3Vec3 r = b3SubPos( worldPoint, bodySim->center ); + b3Vec3 v = b3Add( state->linearVelocity, b3Cross( state->angularVelocity, r ) ); + return v; +} + +void b3Body_ApplyForce( b3BodyId bodyId, b3Vec3 force, b3Pos point, bool wake ) +{ + B3_ASSERT( b3IsValidVec3( force ) ); + + b3World* world = b3GetWorld( bodyId.world0 ); + + B3_REC( world, BodyApplyForce, bodyId, force, point, wake ); + + b3Body* body = b3GetBodyFullId( world, bodyId ); + + if ( wake && body->setIndex >= b3_firstSleepingSet ) + { + b3WakeBodyWithLock( world, body ); + } + + if ( body->setIndex == b3_awakeSet ) + { + b3BodySim* bodySim = b3GetBodySim( world, body ); + bodySim->force = b3Add( bodySim->force, force ); + bodySim->torque = b3Add( bodySim->torque, b3Cross( b3SubPos( point, bodySim->center ), force ) ); + } +} + +void b3Body_ApplyForceToCenter( b3BodyId bodyId, b3Vec3 force, bool wake ) +{ + B3_ASSERT( b3IsValidVec3( force ) ); + + b3World* world = b3GetWorld( bodyId.world0 ); + + B3_REC( world, BodyApplyForceToCenter, bodyId, force, wake ); + + b3Body* body = b3GetBodyFullId( world, bodyId ); + + if ( wake && body->setIndex >= b3_firstSleepingSet ) + { + b3WakeBodyWithLock( world, body ); + } + + if ( body->setIndex == b3_awakeSet ) + { + b3BodySim* bodySim = b3GetBodySim( world, body ); + bodySim->force = b3Add( bodySim->force, force ); + } +} + +void b3Body_ApplyTorque( b3BodyId bodyId, b3Vec3 torque, bool wake ) +{ + B3_ASSERT( b3IsValidVec3( torque ) ); + + b3World* world = b3GetWorld( bodyId.world0 ); + + B3_REC( world, BodyApplyTorque, bodyId, torque, wake ); + + b3Body* body = b3GetBodyFullId( world, bodyId ); + + if ( wake && body->setIndex >= b3_firstSleepingSet ) + { + b3WakeBodyWithLock( world, body ); + } + + if ( body->setIndex == b3_awakeSet ) + { + b3BodySim* bodySim = b3GetBodySim( world, body ); + bodySim->torque = b3Add( bodySim->torque, torque ); + } +} + +void b3Body_ApplyLinearImpulse( b3BodyId bodyId, b3Vec3 impulse, b3Pos point, bool wake ) +{ + B3_ASSERT( b3IsValidVec3( impulse ) ); + B3_ASSERT( b3IsValidPosition( point ) ); + + b3World* world = b3GetWorld( bodyId.world0 ); + + B3_REC( world, BodyApplyLinearImpulse, bodyId, impulse, point, wake ); + + b3Body* body = b3GetBodyFullId( world, bodyId ); + + if ( wake && body->setIndex >= b3_firstSleepingSet ) + { + b3WakeBodyWithLock( world, body ); + } + + if ( body->setIndex == b3_awakeSet ) + { + int localIndex = body->localIndex; + b3SolverSet* set = b3Array_Get( world->solverSets, b3_awakeSet ); + b3BodyState* state = b3Array_Get( set->bodyStates, localIndex ); + b3BodySim* bodySim = b3Array_Get( set->bodySims, localIndex ); + + state->linearVelocity = b3MulAdd( state->linearVelocity, bodySim->invMass, impulse ); + + float maxLinearSpeed = world->maxLinearSpeed; + if ( b3LengthSquared( state->linearVelocity ) > maxLinearSpeed * maxLinearSpeed ) + { + state->linearVelocity = b3MulSV( maxLinearSpeed, b3Normalize( state->linearVelocity ) ); + } + + b3Vec3 delta = b3MulMV( bodySim->invInertiaWorld, b3Cross( b3SubPos( point, bodySim->center ), impulse ) ); + state->angularVelocity = b3Add( state->angularVelocity, delta ); + } +} + +void b3Body_ApplyLinearImpulseToCenter( b3BodyId bodyId, b3Vec3 impulse, bool wake ) +{ + B3_ASSERT( b3IsValidVec3( impulse ) ); + + b3World* world = b3GetWorld( bodyId.world0 ); + + B3_REC( world, BodyApplyLinearImpulseToCenter, bodyId, impulse, wake ); + + b3Body* body = b3GetBodyFullId( world, bodyId ); + + if ( wake && body->setIndex >= b3_firstSleepingSet ) + { + b3WakeBodyWithLock( world, body ); + } + + if ( body->setIndex == b3_awakeSet ) + { + int localIndex = body->localIndex; + b3SolverSet* set = b3Array_Get( world->solverSets, b3_awakeSet ); + b3BodyState* state = b3Array_Get( set->bodyStates, localIndex ); + b3BodySim* bodySim = b3Array_Get( set->bodySims, localIndex ); + state->linearVelocity = b3MulAdd( state->linearVelocity, bodySim->invMass, impulse ); + + float maxLinearSpeed = world->maxLinearSpeed; + if ( b3LengthSquared( state->linearVelocity ) > maxLinearSpeed * maxLinearSpeed ) + { + state->linearVelocity = b3MulSV( maxLinearSpeed, b3Normalize( state->linearVelocity ) ); + } + } +} + +void b3Body_ApplyAngularImpulse( b3BodyId bodyId, b3Vec3 impulse, bool wake ) +{ + B3_ASSERT( b3IsValidVec3( impulse ) ); + B3_ASSERT( b3Body_IsValid( bodyId ) ); + + b3World* world = b3GetWorld( bodyId.world0 ); + + B3_REC( world, BodyApplyAngularImpulse, bodyId, impulse, wake ); + + int id = bodyId.index1 - 1; + b3Body* body = b3Array_Get( world->bodies, id ); + B3_ASSERT( body->generation == bodyId.generation ); + + if ( wake && body->setIndex >= b3_firstSleepingSet ) + { + // this will not invalidate body pointer + b3WakeBodyWithLock( world, body ); + } + + if ( body->setIndex == b3_awakeSet ) + { + int localIndex = body->localIndex; + b3SolverSet* set = b3Array_Get( world->solverSets, b3_awakeSet ); + b3BodyState* state = b3Array_Get( set->bodyStates, localIndex ); + b3BodySim* bodySim = b3Array_Get( set->bodySims, localIndex ); + + b3Vec3 localImpulse = b3InvRotateVector( bodySim->transform.q, impulse ); + b3Vec3 localAngularVelocityDelta = b3MulMV( bodySim->invInertiaLocal, localImpulse ); + state->angularVelocity = + b3Add( state->angularVelocity, b3RotateVector( bodySim->transform.q, localAngularVelocityDelta ) ); + } +} + +b3BodyType b3Body_GetType( b3BodyId bodyId ) +{ + b3World* world = b3GetWorld( bodyId.world0 ); + b3Body* body = b3GetBodyFullId( world, bodyId ); + return body->type; +} + +// This should follow similar steps as you would get destroying and recreating the body, shapes, and joints. +// Contacts are difficult to preserve because the broad-phase pairs change, so I just destroy them. +// todo with a bit more effort I could support an option to let the body sleep +// +// Revised steps: +// 1 Skip disabled bodies +// 2 Destroy all contacts on the body +// 3 Wake the body +// 4 For all joints attached to the body +// - wake attached bodies +// - remove from island +// - move to static set temporarily +// 5 Change the body type and transfer the body +// 6 If the body was static +// - create an island for the body +// Else if the body is becoming static +// - remove it from the island +// 7 For all joints +// - if either body is non-static +// - link into island +// - transfer to constraint graph +// 8 For all shapes +// - Destroy proxy in old tree +// - Create proxy in new tree +// Notes: +// - the implementation below tries to minimize the number of predicates, so some +// operations may have no effect, such as transferring a joint to the same set +void b3Body_SetType( b3BodyId bodyId, b3BodyType type ) +{ + b3World* world = b3GetUnlockedWorld( bodyId.world0 ); + if ( world == NULL ) + { + return; + } + + B3_REC( world, BodySetType, bodyId, (int32_t)type ); + + world->locked = true; + b3Body* body = b3GetBodyFullId( world, bodyId ); + + b3BodyType originalType = body->type; + if ( originalType == type ) + { + world->locked = false; + return; + } + + if ( type != b3_staticBody ) + { + int shapeId = body->headShapeId; + while ( shapeId != B3_NULL_INDEX ) + { + b3Shape* shape = b3Array_Get( world->shapes, shapeId ); + if ( shape->type == b3_compoundShape || shape->type == b3_heightShape ) + { + // Setting the body type is not supported for bodies with compound shapes + return; + } + + shapeId = shape->nextShapeId; + } + } + + // Stage 1: skip disabled bodies + if ( body->setIndex == b3_disabledSet ) + { + // Disabled bodies don't change solver sets or islands when they change type. + body->type = type; + + if ( type == b3_dynamicBody ) + { + body->flags |= b3_dynamicFlag; + } + else + { + body->flags &= ~b3_dynamicFlag; + } + + b3SyncBodyFlags( world, body ); + + // Body type affects the mass properties + b3UpdateBodyMassData( world, body ); + world->locked = false; + return; + } + + // Stage 2: destroy all contacts but don't wake bodies (because we don't need to) + bool wakeBodies = false; + b3DestroyBodyContacts( world, body, wakeBodies ); + + // Stage 3: wake this body (does nothing if body is static), otherwise it will also wake + // all bodies in the same sleeping solver set. + b3WakeBody( world, body ); + + // Stage 4: move joints to temporary storage + b3SolverSet* staticSet = b3Array_Get( world->solverSets, b3_staticSet ); + + int jointKey = body->headJointKey; + while ( jointKey != B3_NULL_INDEX ) + { + int jointId = jointKey >> 1; + int edgeIndex = jointKey & 1; + + b3Joint* joint = b3Array_Get( world->joints, jointId ); + jointKey = joint->edges[edgeIndex].nextKey; + + // Joint may be disabled by other body + if ( joint->setIndex == b3_disabledSet ) + { + continue; + } + + // Wake attached bodies. The b3WakeBody call above does not wake bodies + // attached to a static body. But it is necessary because the body may have + // no joints. + b3Body* bodyA = b3Array_Get( world->bodies, joint->edges[0].bodyId ); + b3Body* bodyB = b3Array_Get( world->bodies, joint->edges[1].bodyId ); + b3WakeBody( world, bodyA ); + b3WakeBody( world, bodyB ); + + // Remove joint from island + b3UnlinkJoint( world, joint ); + + // It is necessary to transfer all joints to the static set + // so they can be added to the constraint graph below and acquire consistent colors. + b3SolverSet* jointSourceSet = b3Array_Get( world->solverSets, joint->setIndex ); + b3TransferJoint( world, staticSet, jointSourceSet, joint ); + } + + // Stage 5: change the body type and transfer body + body->type = type; + + if ( type == b3_dynamicBody ) + { + body->flags |= b3_dynamicFlag; + } + else + { + body->flags &= ~b3_dynamicFlag; + } + + b3SolverSet* awakeSet = b3Array_Get( world->solverSets, b3_awakeSet ); + b3SolverSet* sourceSet = b3Array_Get( world->solverSets, body->setIndex ); + b3SolverSet* targetSet = type == b3_staticBody ? staticSet : awakeSet; + + // Transfer body + b3TransferBody( world, targetSet, sourceSet, body ); + + // Stage 6: update island participation for the body + if ( originalType == b3_staticBody ) + { + // Create island for body + b3CreateIslandForBody( world, b3_awakeSet, body ); + } + else if ( type == b3_staticBody ) + { + // Remove body from island. + b3RemoveBodyFromIsland( world, body ); + } + + // Stage 7: Transfer joints to the target set + jointKey = body->headJointKey; + while ( jointKey != B3_NULL_INDEX ) + { + int jointId = jointKey >> 1; + int edgeIndex = jointKey & 1; + + b3Joint* joint = b3Array_Get( world->joints, jointId ); + + jointKey = joint->edges[edgeIndex].nextKey; + + // Joint may be disabled by other body + if ( joint->setIndex == b3_disabledSet ) + { + continue; + } + + // All joints were transferred to the static set in an earlier stage + B3_ASSERT( joint->setIndex == b3_staticSet ); + + b3Body* bodyA = b3Array_Get( world->bodies, joint->edges[0].bodyId ); + b3Body* bodyB = b3Array_Get( world->bodies, joint->edges[1].bodyId ); + B3_ASSERT( bodyA->setIndex == b3_staticSet || bodyA->setIndex == b3_awakeSet ); + B3_ASSERT( bodyB->setIndex == b3_staticSet || bodyB->setIndex == b3_awakeSet ); + + if ( bodyA->type == b3_dynamicBody || bodyB->type == b3_dynamicBody ) + { + b3TransferJoint( world, awakeSet, staticSet, joint ); + } + } + + // Recreate shape proxies in broadphase + b3WorldTransform transform = b3GetBodyTransformQuick( world, body ); + int shapeId = body->headShapeId; + while ( shapeId != B3_NULL_INDEX ) + { + b3Shape* shape = b3Array_Get( world->shapes, shapeId ); + + // Setting the body type is not supported for bodies with compound shapes + B3_ASSERT( shape->type != b3_compoundShape ); + + shapeId = shape->nextShapeId; + b3DestroyShapeProxy( shape, &world->broadPhase ); + bool forcePairCreation = true; + b3CreateShapeProxy( shape, &world->broadPhase, type, transform, forcePairCreation ); + } + + // Relink all joints + jointKey = body->headJointKey; + while ( jointKey != B3_NULL_INDEX ) + { + int jointId = jointKey >> 1; + int edgeIndex = jointKey & 1; + + b3Joint* joint = b3Array_Get( world->joints, jointId ); + jointKey = joint->edges[edgeIndex].nextKey; + + int otherEdgeIndex = edgeIndex ^ 1; + int otherBodyId = joint->edges[otherEdgeIndex].bodyId; + b3Body* otherBody = b3Array_Get( world->bodies, otherBodyId ); + + if ( otherBody->setIndex == b3_disabledSet ) + { + continue; + } + + if ( body->type != b3_dynamicBody && otherBody->type != b3_dynamicBody ) + { + continue; + } + + b3LinkJoint( world, joint ); + } + + b3SyncBodyFlags( world, body ); + + // Body type affects the mass + b3UpdateBodyMassData( world, body ); + + b3ValidateSolverSets( world ); + b3ValidateIsland( world, body->islandId ); + + world->locked = false; +} + +void b3Body_SetName( b3BodyId bodyId, const char* name ) +{ + b3World* world = b3GetWorld( bodyId.world0 ); + + B3_REC( world, BodySetName, bodyId, name ); + + b3Body* body = b3GetBodyFullId( world, bodyId ); + body->nameId = b3AddName( &world->names, name ); +} + +const char* b3Body_GetName( b3BodyId bodyId ) +{ + b3World* world = b3GetWorld( bodyId.world0 ); + b3Body* body = b3GetBodyFullId( world, bodyId ); + return b3FindNameWithDefault( &world->names, body->nameId, "" ); +} + +void b3Body_SetUserData( b3BodyId bodyId, void* userData ) +{ + b3World* world = b3GetWorld( bodyId.world0 ); + b3Body* body = b3GetBodyFullId( world, bodyId ); + body->userData = userData; +} + +void* b3Body_GetUserData( b3BodyId bodyId ) +{ + b3World* world = b3GetWorld( bodyId.world0 ); + b3Body* body = b3GetBodyFullId( world, bodyId ); + return body->userData; +} + +float b3Body_GetMass( b3BodyId bodyId ) +{ + b3World* world = b3GetWorld( bodyId.world0 ); + b3Body* body = b3GetBodyFullId( world, bodyId ); + return body->mass; +} + +b3Matrix3 b3Body_GetLocalRotationalInertia( b3BodyId bodyId ) +{ + b3World* world = b3GetWorld( bodyId.world0 ); + b3Body* body = b3GetBodyFullId( world, bodyId ); + return body->inertia; +} + +float b3Body_GetInverseMass( b3BodyId bodyId ) +{ + b3World* world = b3GetWorld( bodyId.world0 ); + b3Body* body = b3GetBodyFullId( world, bodyId ); + b3BodySim* sim = b3GetBodySim( world, body ); + return sim->invMass; +} + +b3Matrix3 b3Body_GetWorldInverseRotationalInertia( b3BodyId bodyId ) +{ + b3World* world = b3GetWorld( bodyId.world0 ); + b3Body* body = b3GetBodyFullId( world, bodyId ); + b3BodySim* sim = b3GetBodySim( world, body ); + return sim->invInertiaWorld; +} + +b3Vec3 b3Body_GetLocalCenter( b3BodyId bodyId ) +{ + b3World* world = b3GetWorld( bodyId.world0 ); + b3Body* body = b3GetBodyFullId( world, bodyId ); + b3BodySim* bodySim = b3GetBodySim( world, body ); + return bodySim->localCenter; +} + +b3Pos b3Body_GetWorldCenter( b3BodyId bodyId ) +{ + b3World* world = b3GetWorld( bodyId.world0 ); + b3Body* body = b3GetBodyFullId( world, bodyId ); + b3BodySim* bodySim = b3GetBodySim( world, body ); + return bodySim->center; +} + +void b3Body_SetMassData( b3BodyId bodyId, b3MassData massData ) +{ + B3_ASSERT( b3IsValidFloat( massData.mass ) && massData.mass >= 0.0f ); + B3_ASSERT( b3IsValidMatrix3( massData.inertia ) ); + B3_ASSERT( b3IsValidVec3( massData.center ) ); + + b3World* world = b3GetUnlockedWorld( bodyId.world0 ); + if ( world == NULL ) + { + return; + } + + B3_REC( world, BodySetMassData, bodyId, massData ); + + b3Body* body = b3GetBodyFullId( world, bodyId ); + b3BodySim* bodySim = b3GetBodySim( world, body ); + + // Mass is no longer dirty + body->flags &= ~b3_dirtyMass; + b3SyncBodyFlags( world, body ); + + body->mass = massData.mass; + body->inertia = massData.inertia; + bodySim->localCenter = massData.center; + + b3Pos oldCenter = bodySim->center; + b3Pos center = b3TransformWorldPoint( bodySim->transform, massData.center ); + bodySim->center = center; + bodySim->center0 = center; + bodySim->invMass = body->mass > 0.0f ? 1.0f / body->mass : 0.0f; + + // Update center of mass velocity + b3BodyState* state = b3GetBodyState( world, body ); + if ( state != NULL ) + { + b3Vec3 deltaLinear = b3Cross( state->angularVelocity, b3SubPos( bodySim->center, oldCenter ) ); + state->linearVelocity = b3Add( state->linearVelocity, deltaLinear ); + } + + float det = b3Det( body->inertia ); + B3_ASSERT( det >= 0.0f ); + + if ( det > 0.0f ) + { + // This call is faster than b3Invert + bodySim->invInertiaLocal = b3InvertT( body->inertia ); + + b3Matrix3 rotationMatrix = b3MakeMatrixFromQuat( bodySim->transform.q ); + bodySim->invInertiaWorld = b3MulMM( b3MulMM( rotationMatrix, bodySim->invInertiaLocal ), b3Transpose( rotationMatrix ) ); + } + else + { + bodySim->invInertiaLocal = b3Mat3_zero; + bodySim->invInertiaWorld = b3Mat3_zero; + } + + // Apply fixed rotation + if ( ( bodySim->flags & b3_fixedRotation ) == b3_fixedRotation ) + { + body->inertia = b3Mat3_zero; + bodySim->invInertiaLocal = b3Mat3_zero; + bodySim->invInertiaWorld = b3Mat3_zero; + } + + // Update extents using supplied mass center. + bodySim->minExtent = B3_HUGE; + bodySim->maxExtent = b3Vec3_zero; + int shapeId = body->headShapeId; + while ( shapeId != B3_NULL_INDEX ) + { + const b3Shape* s = b3Array_Get( world->shapes, shapeId ); + b3ShapeExtent extent = b3ComputeShapeExtent( s, massData.center ); + bodySim->minExtent = b3MinFloat( bodySim->minExtent, extent.minExtent ); + bodySim->maxExtent = b3Max( bodySim->maxExtent, extent.maxExtent ); + shapeId = s->nextShapeId; + } +} + +b3MassData b3Body_GetMassData( b3BodyId bodyId ) +{ + b3World* world = b3GetWorld( bodyId.world0 ); + b3Body* body = b3GetBodyFullId( world, bodyId ); + b3BodySim* bodySim = b3GetBodySim( world, body ); + b3MassData massData = { body->mass, bodySim->localCenter, body->inertia }; + return massData; +} + +void b3Body_ApplyMassFromShapes( b3BodyId bodyId ) +{ + b3World* world = b3GetUnlockedWorld( bodyId.world0 ); + if ( world == NULL ) + { + return; + } + + B3_REC( world, BodyApplyMassFromShapes, bodyId ); + + b3Body* body = b3GetBodyFullId( world, bodyId ); + b3UpdateBodyMassData( world, body ); +} + +void b3Body_SetLinearDamping( b3BodyId bodyId, float linearDamping ) +{ + B3_ASSERT( b3IsValidFloat( linearDamping ) && linearDamping >= 0.0f ); + + b3World* world = b3GetUnlockedWorld( bodyId.world0 ); + if ( world == NULL ) + { + return; + } + + B3_REC( world, BodySetLinearDamping, bodyId, linearDamping ); + + b3Body* body = b3GetBodyFullId( world, bodyId ); + b3BodySim* bodySim = b3GetBodySim( world, body ); + bodySim->linearDamping = linearDamping; +} + +float b3Body_GetLinearDamping( b3BodyId bodyId ) +{ + b3World* world = b3GetWorld( bodyId.world0 ); + b3Body* body = b3GetBodyFullId( world, bodyId ); + b3BodySim* bodySim = b3GetBodySim( world, body ); + return bodySim->linearDamping; +} + +void b3Body_SetAngularDamping( b3BodyId bodyId, float angularDamping ) +{ + B3_ASSERT( b3IsValidFloat( angularDamping ) && angularDamping >= 0.0f ); + + b3World* world = b3GetUnlockedWorld( bodyId.world0 ); + if ( world == NULL ) + { + return; + } + + B3_REC( world, BodySetAngularDamping, bodyId, angularDamping ); + + b3Body* body = b3GetBodyFullId( world, bodyId ); + b3BodySim* bodySim = b3GetBodySim( world, body ); + bodySim->angularDamping = angularDamping; +} + +float b3Body_GetAngularDamping( b3BodyId bodyId ) +{ + b3World* world = b3GetWorld( bodyId.world0 ); + b3Body* body = b3GetBodyFullId( world, bodyId ); + b3BodySim* bodySim = b3GetBodySim( world, body ); + return bodySim->angularDamping; +} + +void b3Body_SetGravityScale( b3BodyId bodyId, float gravityScale ) +{ + B3_ASSERT( b3Body_IsValid( bodyId ) ); + B3_ASSERT( b3IsValidFloat( gravityScale ) ); + + b3World* world = b3GetUnlockedWorld( bodyId.world0 ); + if ( world == NULL ) + { + return; + } + + B3_REC( world, BodySetGravityScale, bodyId, gravityScale ); + + b3Body* body = b3GetBodyFullId( world, bodyId ); + b3BodySim* bodySim = b3GetBodySim( world, body ); + bodySim->gravityScale = gravityScale; +} + +float b3Body_GetGravityScale( b3BodyId bodyId ) +{ + B3_ASSERT( b3Body_IsValid( bodyId ) ); + b3World* world = b3GetWorld( bodyId.world0 ); + b3Body* body = b3GetBodyFullId( world, bodyId ); + b3BodySim* bodySim = b3GetBodySim( world, body ); + return bodySim->gravityScale; +} + +bool b3Body_IsAwake( b3BodyId bodyId ) +{ + b3World* world = b3GetWorld( bodyId.world0 ); + b3Body* body = b3GetBodyFullId( world, bodyId ); + return body->setIndex == b3_awakeSet; +} + +void b3Body_SetAwake( b3BodyId bodyId, bool awake ) +{ + b3World* world = b3GetUnlockedWorld( bodyId.world0 ); + if ( world == NULL ) + { + return; + } + + B3_REC( world, BodySetAwake, bodyId, awake ); + + world->locked = true; + + b3Body* body = b3GetBodyFullId( world, bodyId ); + + if ( awake && body->setIndex >= b3_firstSleepingSet ) + { + b3WakeBody( world, body ); + } + else if ( awake == false && body->setIndex == b3_awakeSet ) + { + b3Island* island = b3Array_Get( world->islands, body->islandId ); + if ( island->constraintRemoveCount > 0 ) + { + // Must split the island before sleeping. This is expensive. + b3SplitIsland( world, body->islandId ); + } + + b3TrySleepIsland( world, body->islandId ); + } + + world->locked = false; +} + +bool b3Body_IsEnabled( b3BodyId bodyId ) +{ + b3World* world = b3GetWorld( bodyId.world0 ); + b3Body* body = b3GetBodyFullId( world, bodyId ); + return body->setIndex != b3_disabledSet; +} + +bool b3Body_IsSleepEnabled( b3BodyId bodyId ) +{ + b3World* world = b3GetWorld( bodyId.world0 ); + b3Body* body = b3GetBodyFullId( world, bodyId ); + return ( body->flags & b3_enableSleep ) == b3_enableSleep; +} + +void b3Body_SetSleepThreshold( b3BodyId bodyId, float sleepThreshold ) +{ + b3World* world = b3GetWorld( bodyId.world0 ); + + B3_REC( world, BodySetSleepThreshold, bodyId, sleepThreshold ); + + b3Body* body = b3GetBodyFullId( world, bodyId ); + body->sleepThreshold = sleepThreshold; +} + +float b3Body_GetSleepThreshold( b3BodyId bodyId ) +{ + b3World* world = b3GetWorld( bodyId.world0 ); + b3Body* body = b3GetBodyFullId( world, bodyId ); + return body->sleepThreshold; +} + +void b3Body_EnableSleep( b3BodyId bodyId, bool enableSleep ) +{ + b3World* world = b3GetUnlockedWorld( bodyId.world0 ); + if ( world == NULL ) + { + return; + } + + B3_REC( world, BodyEnableSleep, bodyId, enableSleep ); + + b3Body* body = b3GetBodyFullId( world, bodyId ); + + bool flag = ( body->flags & b3_enableSleep ) == b3_enableSleep; + if ( enableSleep == flag ) + { + return; + } + + world->locked = true; + + body->flags = enableSleep ? body->flags | b3_enableSleep : body->flags & ~b3_enableSleep; + b3SyncBodyFlags( world, body ); + + if ( enableSleep == false ) + { + b3WakeBody( world, body ); + } + + world->locked = false; +} + +// Disabling a body requires a lot of detailed bookkeeping, but it is a valuable feature. +// The most challenging aspect that joints may connect to bodies that are not disabled. +void b3Body_Disable( b3BodyId bodyId ) +{ + b3World* world = b3GetUnlockedWorld( bodyId.world0 ); + if ( world == NULL ) + { + return; + } + + B3_REC( world, BodyDisable, bodyId ); + + world->locked = true; + + b3Body* body = b3GetBodyFullId( world, bodyId ); + if ( body->setIndex == b3_disabledSet ) + { + world->locked = false; + return; + } + + // Destroy contacts and wake bodies touching this body. This avoid floating bodies. + // This is necessary even for static bodies. + bool wakeBodies = true; + b3DestroyBodyContacts( world, body, wakeBodies ); + + // The current solver set of the body + b3SolverSet* set = b3Array_Get( world->solverSets, body->setIndex ); + + // Disabled bodies and connected joints are moved to the disabled set + b3SolverSet* disabledSet = b3Array_Get( world->solverSets, b3_disabledSet ); + + // Unlink joints and transfer them to the disabled set + int jointKey = body->headJointKey; + while ( jointKey != B3_NULL_INDEX ) + { + int jointId = jointKey >> 1; + int edgeIndex = jointKey & 1; + + b3Joint* joint = b3Array_Get( world->joints, jointId ); + jointKey = joint->edges[edgeIndex].nextKey; + + // joint may already be disabled by other body + if ( joint->setIndex == b3_disabledSet ) + { + continue; + } + + B3_ASSERT( joint->setIndex == set->setIndex || set->setIndex == b3_staticSet ); + + // Remove joint from island + b3UnlinkJoint( world, joint ); + + // Transfer joint to disabled set + b3SolverSet* jointSet = b3Array_Get( world->solverSets, joint->setIndex ); + b3TransferJoint( world, disabledSet, jointSet, joint ); + } + + // Remove shapes from broad-phase + int shapeId = body->headShapeId; + while ( shapeId != B3_NULL_INDEX ) + { + b3Shape* shape = b3Array_Get( world->shapes, shapeId ); + shapeId = shape->nextShapeId; + b3DestroyShapeProxy( shape, &world->broadPhase ); + } + + // Disabled bodies are not in an island. If the island becomes empty it will be destroyed. + b3RemoveBodyFromIsland( world, body ); + + // Transfer body sim + b3TransferBody( world, disabledSet, set, body ); + + b3ValidateConnectivity( world ); + b3ValidateSolverSets( world ); + + world->locked = false; +} + +void b3Body_Enable( b3BodyId bodyId ) +{ + b3World* world = b3GetUnlockedWorld( bodyId.world0 ); + if ( world == NULL ) + { + return; + } + + B3_REC( world, BodyEnable, bodyId ); + + b3Body* body = b3GetBodyFullId( world, bodyId ); + if ( body->setIndex != b3_disabledSet ) + { + return; + } + + b3SolverSet* disabledSet = b3Array_Get( world->solverSets, b3_disabledSet ); + int setId = body->type == b3_staticBody ? b3_staticSet : b3_awakeSet; + b3SolverSet* targetSet = b3Array_Get( world->solverSets, setId ); + + b3TransferBody( world, targetSet, disabledSet, body ); + + b3WorldTransform transform = b3GetBodyTransformQuick( world, body ); + + // Add shapes to broad-phase + b3BodyType proxyType = body->type; + bool forcePairCreation = true; + int shapeId = body->headShapeId; + while ( shapeId != B3_NULL_INDEX ) + { + b3Shape* shape = b3Array_Get( world->shapes, shapeId ); + shapeId = shape->nextShapeId; + + b3CreateShapeProxy( shape, &world->broadPhase, proxyType, transform, forcePairCreation ); + } + + if ( setId != b3_staticSet ) + { + b3CreateIslandForBody( world, setId, body ); + } + + // Transfer joints. If the other body is disabled, don't transfer. + // If the other body is sleeping, wake it. + int jointKey = body->headJointKey; + while ( jointKey != B3_NULL_INDEX ) + { + int jointId = jointKey >> 1; + int edgeIndex = jointKey & 1; + + b3Joint* joint = b3Array_Get( world->joints, jointId ); + B3_ASSERT( joint->setIndex == b3_disabledSet ); + B3_ASSERT( joint->islandId == B3_NULL_INDEX ); + + jointKey = joint->edges[edgeIndex].nextKey; + + b3Body* bodyA = b3Array_Get( world->bodies, joint->edges[0].bodyId ); + b3Body* bodyB = b3Array_Get( world->bodies, joint->edges[1].bodyId ); + + if ( bodyA->setIndex == b3_disabledSet || bodyB->setIndex == b3_disabledSet ) + { + // one body is still disabled + continue; + } + + // Transfer joint first + int jointSetId; + if ( bodyA->setIndex == b3_staticSet && bodyB->setIndex == b3_staticSet ) + { + jointSetId = b3_staticSet; + } + else if ( bodyA->setIndex == b3_staticSet ) + { + jointSetId = bodyB->setIndex; + } + else + { + jointSetId = bodyA->setIndex; + } + + b3SolverSet* jointSet = b3Array_Get( world->solverSets, jointSetId ); + b3TransferJoint( world, jointSet, disabledSet, joint ); + + // Now that the joint is in the correct set, I can link the joint in the island. + if ( jointSetId != b3_staticSet ) + { + b3LinkJoint( world, joint ); + } + } + + b3ValidateSolverSets( world ); +} + +void b3Body_SetMotionLocks( b3BodyId bodyId, b3MotionLocks locks ) +{ + b3World* world = b3GetUnlockedWorld( bodyId.world0 ); + if ( world == NULL ) + { + return; + } + + B3_REC( world, BodySetMotionLocks, bodyId, locks ); + + uint32_t newLocks = 0; + newLocks |= locks.linearX ? b3_lockLinearX : 0; + newLocks |= locks.linearY ? b3_lockLinearY : 0; + newLocks |= locks.linearZ ? b3_lockLinearZ : 0; + newLocks |= locks.angularX ? b3_lockAngularX : 0; + newLocks |= locks.angularY ? b3_lockAngularY : 0; + newLocks |= locks.angularZ ? b3_lockAngularZ : 0; + + b3Body* body = b3GetBodyFullId( world, bodyId ); + if ( ( body->flags & b3_allLocks ) == newLocks ) + { + return; + } + + bool fixedRotation1 = ( body->flags & b3_fixedRotation ) == b3_fixedRotation; + bool fixedRotation2 = ( newLocks & b3_fixedRotation ) == b3_fixedRotation; + + body->flags &= ~b3_allLocks; + body->flags |= newLocks; + + b3SyncBodyFlags( world, body ); + + b3BodyState* state = b3GetBodyState( world, body ); + + if ( state != NULL ) + { + if ( locks.linearX ) + { + state->linearVelocity.x = 0.0f; + } + + if ( locks.linearY ) + { + state->linearVelocity.y = 0.0f; + } + + if ( locks.linearZ ) + { + state->linearVelocity.z = 0.0f; + } + + if ( locks.angularX ) + { + state->angularVelocity.x = 0.0f; + } + + if ( locks.angularY ) + { + state->angularVelocity.y = 0.0f; + } + + if ( locks.angularZ ) + { + state->angularVelocity.z = 0.0f; + } + } + + if ( fixedRotation1 != fixedRotation2 ) + { + b3UpdateBodyMassData( world, body ); + } +} + +b3MotionLocks b3Body_GetMotionLocks( b3BodyId bodyId ) +{ + b3World* world = b3GetWorld( bodyId.world0 ); + b3Body* body = b3GetBodyFullId( world, bodyId ); + + b3MotionLocks locks; + locks.linearX = ( body->flags & b3_lockLinearX ); + locks.linearY = ( body->flags & b3_lockLinearY ); + locks.linearZ = ( body->flags & b3_lockLinearZ ); + locks.angularX = ( body->flags & b3_lockAngularX ); + locks.angularY = ( body->flags & b3_lockAngularY ); + locks.angularZ = ( body->flags & b3_lockAngularZ ); + return locks; +} + +void b3Body_SetBullet( b3BodyId bodyId, bool flag ) +{ + b3World* world = b3GetUnlockedWorld( bodyId.world0 ); + if ( world == NULL ) + { + return; + } + + B3_REC( world, BodySetBullet, bodyId, flag ); + + uint32_t newFlag = flag ? b3_isBullet : 0; + + b3Body* body = b3GetBodyFullId( world, bodyId ); + if ( ( body->flags & b3_isBullet ) == newFlag ) + { + return; + } + + body->flags &= ~b3_isBullet; + body->flags |= newFlag; + + b3SyncBodyFlags( world, body ); +} + +bool b3Body_IsBullet( b3BodyId bodyId ) +{ + b3World* world = b3GetWorld( bodyId.world0 ); + b3Body* body = b3GetBodyFullId( world, bodyId ); + return ( body->flags & b3_isBullet ) != 0; +} + +void b3Body_EnableContactRecycling( b3BodyId bodyId, bool flag ) +{ + b3World* world = b3GetUnlockedWorld( bodyId.world0 ); + if ( world == NULL ) + { + return; + } + + B3_REC( world, BodyEnableContactRecycling, bodyId, flag ); + + uint32_t newFlag = flag ? b3_bodyEnableContactRecycling : 0; + + b3Body* body = b3GetBodyFullId( world, bodyId ); + if ( ( body->flags & b3_bodyEnableContactRecycling ) == newFlag ) + { + return; + } + + body->flags &= ~b3_bodyEnableContactRecycling; + body->flags |= newFlag; + + b3SyncBodyFlags( world, body ); +} + +bool b3Body_IsContactRecyclingEnabled( b3BodyId bodyId ) +{ + b3World* world = b3GetWorld( bodyId.world0 ); + b3Body* body = b3GetBodyFullId( world, bodyId ); + return ( body->flags & b3_bodyEnableContactRecycling ) != 0; +} + +void b3Body_EnableHitEvents( b3BodyId bodyId, bool flag ) +{ + b3World* world = b3GetWorld( bodyId.world0 ); + + B3_REC( world, BodyEnableHitEvents, bodyId, flag ); + + b3Body* body = b3GetBodyFullId( world, bodyId ); + int shapeId = body->headShapeId; + while ( shapeId != B3_NULL_INDEX ) + { + b3Shape* shape = b3Array_Get( world->shapes, shapeId ); + shape->flags = flag ? shape->flags | b3_enableHitEvents : shape->flags & ~b3_enableHitEvents; + shapeId = shape->nextShapeId; + } +} + +b3WorldId b3Body_GetWorld( b3BodyId bodyId ) +{ + b3World* world = b3GetWorld( bodyId.world0 ); + return (b3WorldId){ (uint16_t)( bodyId.world0 + 1 ), world->generation }; +} + +int b3Body_GetShapeCount( b3BodyId bodyId ) +{ + b3World* world = b3GetWorld( bodyId.world0 ); + b3Body* body = b3GetBodyFullId( world, bodyId ); + return body->shapeCount; +} + +int b3Body_GetShapes( b3BodyId bodyId, b3ShapeId* shapeArray, int capacity ) +{ + b3World* world = b3GetWorld( bodyId.world0 ); + b3Body* body = b3GetBodyFullId( world, bodyId ); + int shapeId = body->headShapeId; + int shapeCount = 0; + while ( shapeId != B3_NULL_INDEX && shapeCount < capacity ) + { + b3Shape* shape = b3Array_Get( world->shapes, shapeId ); + b3ShapeId id = { shape->id + 1, bodyId.world0, shape->generation }; + shapeArray[shapeCount] = id; + shapeCount += 1; + + shapeId = shape->nextShapeId; + } + + return shapeCount; +} + +int b3Body_GetJointCount( b3BodyId bodyId ) +{ + b3World* world = b3GetWorld( bodyId.world0 ); + b3Body* body = b3GetBodyFullId( world, bodyId ); + return body->jointCount; +} + +int b3Body_GetJoints( b3BodyId bodyId, b3JointId* jointArray, int capacity ) +{ + b3World* world = b3GetWorld( bodyId.world0 ); + b3Body* body = b3GetBodyFullId( world, bodyId ); + int jointKey = body->headJointKey; + + int jointCount = 0; + while ( jointKey != B3_NULL_INDEX && jointCount < capacity ) + { + int jointId = jointKey >> 1; + int edgeIndex = jointKey & 1; + + b3Joint* joint = b3Array_Get( world->joints, jointId ); + + b3JointId id = { jointId + 1, bodyId.world0, joint->generation }; + jointArray[jointCount] = id; + jointCount += 1; + + jointKey = joint->edges[edgeIndex].nextKey; + } + + return jointCount; +} + +bool b3ShouldBodiesCollide( b3World* world, b3Body* bodyA, b3Body* bodyB ) +{ + if ( bodyA->type != b3_dynamicBody && bodyB->type != b3_dynamicBody ) + { + return false; + } + + int jointKey; + int otherBodyId; + if ( bodyA->jointCount < bodyB->jointCount ) + { + jointKey = bodyA->headJointKey; + otherBodyId = bodyB->id; + } + else + { + jointKey = bodyB->headJointKey; + otherBodyId = bodyA->id; + } + + while ( jointKey != B3_NULL_INDEX ) + { + int jointId = jointKey >> 1; + int edgeIndex = jointKey & 1; + int otherEdgeIndex = edgeIndex ^ 1; + + b3Joint* joint = b3Array_Get( world->joints, jointId ); + if ( joint->collideConnected == false && joint->edges[otherEdgeIndex].bodyId == otherBodyId ) + { + return false; + } + + jointKey = joint->edges[edgeIndex].nextKey; + } + + return true; +} diff --git a/vendor/box3d/src/src/body.h b/vendor/box3d/src/src/body.h new file mode 100644 index 000000000..5eebf67ab --- /dev/null +++ b/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; +} + diff --git a/vendor/box3d/src/src/box3d.natvis b/vendor/box3d/src/src/box3d.natvis new file mode 100644 index 000000000..68d36e1c2 --- /dev/null +++ b/vendor/box3d/src/src/box3d.natvis @@ -0,0 +1,147 @@ + + + + + + [ {x}, {y} ] + + + + [ {x}, {y}, {z} ] + + + + [ {v}, {s} ] + + + + + [ {x}, {y}, {z} ] + + + + [ {p}, {q} ] + + + + index: {index1} + + b3_worlds[ index1 - 1 ] + + + + + + {b3_worlds[world0].bodies.data[index1-1].name,na}, + {b3_worlds[world0].bodies.data[index1-1].type} + + + b3_worlds[ world0 ].bodies.data[index1-1] + + + + + + {b3_worlds[world0].shapes.data[index1-1].type} + + + + + + {point}, sep = {separation} + + + + + + {int(owner1)}:{int(index1)}, {int(owner2)}:{int(index2)} + + + + + count: { pointCount }, triangle: { triangleIndex }, cluster: {clusterIndex} + + normal + feature + triangleFlags + + pointCount + points + + + + + + {{{count} : {radius}}} + + count + radius + + count + points + + + + + + type: {type} + + witnessA + witnessB + sweepA + sweepB + proxyA + proxyB + + + + + {{{count} / {capacity}}} + + count + capacity + + count + data + + + + + + + {{{count} / {capacity}}} + + count + capacity + + count + data + + + + + + {{{count} / {capacity}}} + + count + capacity + + count + data + + + + + + {{{count} / {capacity}}} + + count + capacity + + count + data + + + + + diff --git a/vendor/box3d/src/src/broad_phase.c b/vendor/box3d/src/src/broad_phase.c new file mode 100644 index 000000000..a369ca62a --- /dev/null +++ b/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 + +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 +} diff --git a/vendor/box3d/src/src/broad_phase.h b/vendor/box3d/src/src/broad_phase.h new file mode 100644 index 000000000..dcd626b3a --- /dev/null +++ b/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 ); + } +} diff --git a/vendor/box3d/src/src/capsule.c b/vendor/box3d/src/src/capsule.c new file mode 100644 index 000000000..249e8cbf2 --- /dev/null +++ b/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; +} diff --git a/vendor/box3d/src/src/compound.c b/vendor/box3d/src/src/compound.c new file mode 100644 index 000000000..29da3c662 --- /dev/null +++ b/vendor/box3d/src/src/compound.c @@ -0,0 +1,1196 @@ +// SPDX-FileCopyrightText: 2025 Erin Catto +// SPDX-License-Identifier: MIT + +#include "compound.h" + +#include "hull_map.h" +#include "math_internal.h" +#include "shape.h" + +#include "box3d/base.h" +#include "box3d/collision.h" +#include "box3d/constants.h" +#include "box3d/types.h" + +#include +#include + +typedef struct b3SharedHull +{ + const b3HullData* hull; + int hullOffset; +} b3SharedHull; + +typedef struct b3SharedMesh +{ + const b3MeshData* meshData; + int meshOffset; +} b3SharedMesh; + +typedef struct b3HullInstance +{ + b3Transform transform; + uint32_t hullOffset; + uint32_t materialIndex; +} b3HullInstance; + +typedef struct b3MeshInstance +{ + b3Transform transform; + b3Vec3 scale; + uint32_t meshOffset; + uint32_t materialIndices[B3_MAX_COMPOUND_MESH_MATERIALS]; +} b3MeshInstance; + +static inline b3TreeNode* b3GetCompoundNodes( b3CompoundData* compound ) +{ + if ( compound->nodeOffset == 0 ) + { + return NULL; + } + + return (b3TreeNode*)( (intptr_t)compound + compound->nodeOffset ); +} + +const b3SurfaceMaterial* b3GetCompoundMaterials( const b3CompoundData* compound ) +{ + if ( compound->materialOffset == 0 ) + { + return NULL; + } + + return (b3SurfaceMaterial*)( (intptr_t)compound + compound->materialOffset ); +} + +b3CompoundCapsule b3GetCompoundCapsule( const b3CompoundData* compound, int index ) +{ + B3_ASSERT( 0 <= index && index < compound->capsuleCount && compound->capsuleOffset > 0 ); + + b3CompoundCapsule result = { 0 }; + if ( compound->capsuleOffset == 0 ) + { + return result; + } + + const b3CompoundCapsule* capsules = (const b3CompoundCapsule*)( (intptr_t)compound + compound->capsuleOffset ); + return capsules[index]; +} + +b3CompoundHull b3GetCompoundHull( const b3CompoundData* compound, int index ) +{ + B3_ASSERT( 0 <= index && index < compound->hullCount && compound->hullOffset > 0 ); + + b3CompoundHull result = { 0 }; + if ( compound->hullOffset == 0 ) + { + return result; + } + + const b3HullInstance* hullInstances = (const b3HullInstance*)( (intptr_t)compound + compound->hullOffset ); + uint32_t hullOffset = hullInstances[index].hullOffset; + B3_ASSERT( hullOffset >= compound->hullOffset + compound->hullCount * sizeof( b3HullInstance ) ); + result.hull = (const b3HullData*)( (intptr_t)compound + hullOffset ); + result.transform = hullInstances[index].transform; + result.materialIndex = hullInstances[index].materialIndex; + return result; +} + +b3CompoundMesh b3GetCompoundMesh( const b3CompoundData* compound, int index ) +{ + B3_ASSERT( 0 <= index && index < compound->meshCount && compound->meshOffset > 0 ); + + b3CompoundMesh result = { 0 }; + if ( compound->meshOffset == 0 ) + { + return result; + } + + const b3MeshInstance* meshInstances = (const b3MeshInstance*)( (intptr_t)compound + compound->meshOffset ); + uint32_t meshOffset = meshInstances[index].meshOffset; + B3_ASSERT( meshOffset >= compound->meshOffset + compound->meshCount * sizeof( b3HullInstance ) ); + result.meshData = (const b3MeshData*)( (intptr_t)compound + meshOffset ); + result.transform = meshInstances[index].transform; + result.scale = meshInstances[index].scale; + for ( int i = 0; i < B3_MAX_COMPOUND_MESH_MATERIALS; ++i ) + { + result.materialIndices[i] = meshInstances[index].materialIndices[i]; + } + return result; +} + +b3CompoundSphere b3GetCompoundSphere( const b3CompoundData* compound, int index ) +{ + B3_ASSERT( 0 <= index && index < compound->sphereCount && compound->sphereOffset > 0 ); + + b3CompoundSphere result = { 0 }; + if ( compound->sphereOffset == 0 ) + { + return result; + } + + const b3CompoundSphere* spheres = (const b3CompoundSphere*)( (intptr_t)compound + compound->sphereOffset ); + return spheres[index]; +} + +b3ChildShape b3GetCompoundChild( const b3CompoundData* compound, int childIndex ) +{ + // Capsule? + if ( 0 <= childIndex && childIndex < compound->capsuleCount ) + { + b3CompoundCapsule compoundCapsule = b3GetCompoundCapsule( compound, childIndex ); + return (b3ChildShape){ + .capsule = compoundCapsule.capsule, + .transform = b3Transform_identity, + .materialIndices = { compoundCapsule.materialIndex }, + .type = b3_capsuleShape, + }; + } + childIndex -= compound->capsuleCount; + + // Hull? + if ( 0 <= childIndex && childIndex < compound->hullCount ) + { + b3CompoundHull compoundHull = b3GetCompoundHull( compound, childIndex ); + return (b3ChildShape){ + .hull = compoundHull.hull, + .transform = compoundHull.transform, + .materialIndices = { compoundHull.materialIndex }, + .type = b3_hullShape, + }; + } + childIndex -= compound->hullCount; + + // Mesh? + if ( 0 <= childIndex && childIndex < compound->meshCount ) + { + b3CompoundMesh compoundMesh = b3GetCompoundMesh( compound, childIndex ); + const int* m = compoundMesh.materialIndices; + _Static_assert( B3_MAX_COMPOUND_MESH_MATERIALS == 4, "too many materials in compound mesh" ); + + return (b3ChildShape){ + .mesh = + { + .data = compoundMesh.meshData, + .scale = compoundMesh.scale, + }, + .transform = compoundMesh.transform, + .materialIndices = { m[0], m[1], m[2], m[3] }, + .type = b3_meshShape, + }; + } + childIndex -= compound->meshCount; + + B3_ASSERT( 0 <= childIndex && childIndex < compound->sphereCount ); + + // Sphere? + { + b3CompoundSphere compoundSphere = b3GetCompoundSphere( compound, childIndex ); + return (b3ChildShape){ + .sphere = compoundSphere.sphere, + .transform = b3Transform_identity, + .materialIndices = { compoundSphere.materialIndex }, + .type = b3_sphereShape, + }; + } +} + +static inline size_t vt_wyhash( const void* key, size_t len ); + +static inline uint64_t b3HashMesh( const b3MeshData* mesh ) +{ + return vt_wyhash( mesh, mesh->byteCount ); +} + +static bool b3CompareMeshes( const b3MeshData* mesh1, const b3MeshData* mesh2 ) +{ + if ( mesh1 == mesh2 ) + { + return true; + } + + if ( mesh1->byteCount != mesh2->byteCount ) + { + return false; + } + + int result = memcmp( mesh1, mesh2, mesh1->byteCount ); + return result == 0; +} + +#define NAME b3MeshMap +#define KEY_TY const b3MeshData* +#define VAL_TY int +#define HASH_FN b3HashMesh +#define CMPR_FN b3CompareMeshes +#define MALLOC_FN b3Alloc +#define FREE_FN b3Free +#include "verstable.h" + +static inline uint64_t b3HashMaterial( const b3SurfaceMaterial* material ) +{ + return vt_wyhash( material, sizeof( b3SurfaceMaterial ) ); +} + +static bool b3CompareMaterials( const b3SurfaceMaterial* mat1, const b3SurfaceMaterial* mat2 ) +{ + if ( mat1 == mat2 ) + { + return true; + } + + int result = memcmp( mat1, mat2, sizeof( b3SurfaceMaterial ) ); + return result == 0; +} + +#define NAME b3MaterialMap +#define KEY_TY const b3SurfaceMaterial* +#define VAL_TY int +#define HASH_FN b3HashMaterial +#define CMPR_FN b3CompareMaterials +#define MALLOC_FN b3Alloc +#define FREE_FN b3Free +#include "verstable.h" + +b3CompoundData* b3CreateCompound( const b3CompoundDef* def ) +{ + int convexCount = def->capsuleCount + def->hullCount + def->sphereCount; + int shapeCount = convexCount + def->meshCount; + + if ( shapeCount >= B3_MAX_CHILD_SHAPES ) + { + B3_ASSERT( false ); + return NULL; + } + + b3DynamicTree tree = b3DynamicTree_Create( shapeCount ); + + int childIndex = 0; + + // Instances + int capsuleCount = def->capsuleCount; + b3CompoundCapsule* capsuleInstances = b3AllocZeroed( capsuleCount * sizeof( b3CompoundCapsule ) ); + int hullCount = def->hullCount; + b3HullInstance* hullInstances = b3AllocZeroed( hullCount * sizeof( b3HullInstance ) ); + int meshCount = def->meshCount; + b3MeshInstance* meshInstances = b3AllocZeroed( meshCount * sizeof( b3MeshInstance ) ); + int sphereCount = def->sphereCount; + b3CompoundSphere* sphereInstances = b3AllocZeroed( sphereCount * sizeof( b3CompoundSphere ) ); + + // Determine material capacity + int materialCapacity = convexCount; + for ( int i = 0; i < def->meshCount; ++i ) + { + B3_ASSERT( def->meshes[i].materialCount > 0 ); + materialCapacity += def->meshes[i].materialCount; + } + + // Material map for convex material sharing. Mesh materials are not shared for simplicity. + b3MaterialMap materialMap; + b3MaterialMap_init( &materialMap ); + b3MaterialMap_reserve( &materialMap, materialCapacity ); + b3SurfaceMaterial* materials = b3AllocZeroed( materialCapacity * sizeof( b3SurfaceMaterial ) ); + int materialCount = 0; + + for ( int i = 0; i < def->capsuleCount; ++i ) + { + const b3CompoundCapsuleDef* capsuleDef = def->capsules + i; + capsuleInstances[i].capsule = capsuleDef->capsule; + + // Look for an existing material + b3MaterialMap_itr materialItr = b3MaterialMap_get_or_insert( &materialMap, &capsuleDef->material, materialCount ); + + // Get the shared material index + int materialIndex = materialItr.data->val; + capsuleInstances[i].materialIndex = materialIndex; + + // Is this a new material? + if ( materialIndex == materialCount ) + { + materials[materialIndex] = capsuleDef->material; + materialCount += 1; + } + + b3AABB aabb = b3ComputeCapsuleAABB( &capsuleDef->capsule, b3Transform_identity ); + b3DynamicTree_CreateProxy( &tree, aabb, ~0ull, childIndex ); + childIndex += 1; + } + + // Hulls + b3SharedHull* sharedHulls = b3AllocZeroed( hullCount * sizeof( b3SharedHull ) ); + int sharedHullCount = 0; + + if ( hullCount > 0 ) + { + b3HullMap hullMap; + b3HullMap_init( &hullMap ); + b3HullMap_reserve( &hullMap, hullCount ); + + for ( int i = 0; i < hullCount; ++i ) + { + const b3CompoundHullDef* hullDef = def->hulls + i; + const b3HullData* hull = hullDef->hull; + b3AABB aabb = b3ComputeHullAABB( hull, hullDef->transform ); + b3DynamicTree_CreateProxy( &tree, aabb, ~0ull, childIndex ); + childIndex += 1; + + // Look for an existing material + b3MaterialMap_itr materialItr = b3MaterialMap_get_or_insert( &materialMap, &hullDef->material, materialCount ); + + // Get the shared material index + int materialIndex = materialItr.data->val; + hullInstances[i].materialIndex = materialIndex; + + // Is this a new material? + if ( materialIndex == materialCount ) + { + materials[materialIndex] = hullDef->material; + materialCount += 1; + } + + hullInstances[i].transform = hullDef->transform; + + // Look for an existing matching hull + b3HullMap_itr itr = b3HullMap_get_or_insert( &hullMap, hull, sharedHullCount ); + + // Get the unique index for this hull + int sharedHullIndex = itr.data->val; + + // The offset isn't known yet, so store the index of the shared hull + hullInstances[i].hullOffset = sharedHullIndex; + + // Is this a new hull? + if ( sharedHullIndex == sharedHullCount ) + { + // Create a shared hull. The offset is determined below. + sharedHulls[sharedHullIndex].hull = hull; + sharedHulls[sharedHullIndex].hullOffset = B3_NULL_INDEX; + sharedHullCount += 1; + } + } + + b3HullMap_cleanup( &hullMap ); + } + + // Meshes + b3SharedMesh* sharedMeshes = b3AllocZeroed( meshCount * sizeof( b3SharedMesh ) ); + int sharedMeshCount = 0; + + if ( meshCount > 0 ) + { + b3MeshMap meshMap; + b3MeshMap_init( &meshMap ); + b3MeshMap_reserve( &meshMap, meshCount ); + + for ( int i = 0; i < meshCount; ++i ) + { + const b3CompoundMeshDef* meshDef = def->meshes + i; + + const b3MeshData* meshData = meshDef->meshData; + b3AABB aabb = b3ComputeMeshAABB( meshData, meshDef->transform, meshDef->scale ); + b3DynamicTree_CreateProxy( &tree, aabb, ~0ull, childIndex ); + childIndex += 1; + + // No effort to share mesh materials. It would be easier to do if the number of materials was limited. + B3_ASSERT( meshData->materialCount == meshDef->materialCount ); + + for ( int j = 0; j < meshDef->materialCount; ++j ) + { + // Look for an existing material + b3MaterialMap_itr materialItr = + b3MaterialMap_get_or_insert( &materialMap, &meshDef->materials[j], materialCount ); + + // Get the shared material index + int materialIndex = materialItr.data->val; + meshInstances[i].materialIndices[j] = materialIndex; + + // Is this a new material? + if ( materialIndex == materialCount ) + { + materials[materialIndex] = meshDef->materials[j]; + materialCount += 1; + } + } + + // Look for an existing matching mesh + b3MeshMap_itr itr = b3MeshMap_get_or_insert( &meshMap, meshData, sharedMeshCount ); + + // Get the shared mesh index + int sharedMeshIndex = itr.data->val; + + // Create mesh instance + meshInstances[i].transform = def->meshes[i].transform; + meshInstances[i].scale = def->meshes[i].scale; + + // The offset isn't known yet, so store the index of the shared mesh + meshInstances[i].meshOffset = sharedMeshIndex; + + // Is this a new mesh? + if ( sharedMeshIndex == sharedMeshCount ) + { + // Create a shared mesh. The offset is determined below. + sharedMeshes[sharedMeshIndex].meshData = meshData; + sharedMeshes[sharedMeshIndex].meshOffset = B3_NULL_INDEX; + sharedMeshCount += 1; + } + } + + b3MeshMap_cleanup( &meshMap ); + } + + // Spheres + for ( int i = 0; i < def->sphereCount; ++i ) + { + const b3CompoundSphereDef* sphereDef = def->spheres + i; + sphereInstances[i].sphere = sphereDef->sphere; + + // Look for an existing material + b3MaterialMap_itr materialItr = b3MaterialMap_get_or_insert( &materialMap, &sphereDef->material, materialCount ); + + // Get the shared material index + int materialIndex = materialItr.data->val; + sphereInstances[i].materialIndex = materialIndex; + + // Is this a new material? + if ( materialIndex == materialCount ) + { + materials[materialIndex] = sphereDef->material; + materialCount += 1; + } + + b3AABB aabb = b3ComputeSphereAABB( &sphereDef->sphere, b3Transform_identity ); + b3DynamicTree_CreateProxy( &tree, aabb, ~0ull, childIndex ); + childIndex += 1; + } + + B3_ASSERT( materialCount <= materialCapacity ); + B3_ASSERT( tree.nodeCount > 0 ); + + b3DynamicTree_Rebuild( &tree, true ); + + int byteCount = sizeof( b3CompoundData ); + + // Tree nodes - todo 64 byte alignment + int nodeOffset = byteCount; + byteCount += tree.nodeCapacity * sizeof( b3TreeNode ); + + int materialOffset = byteCount; + byteCount += materialCount * sizeof( b3SurfaceMaterial ); + + int capsuleOffset = byteCount; + byteCount += def->capsuleCount * sizeof( b3CompoundCapsule ); + + // Hull data layout has another level of indirection to allow for tight data packing + // 1. hull instance array : hull count array of b3HullInstance with individual hull transforms and offsets + // 2. heterogeneous array of shared hull data : each shared hull can have a different byte count, so direct indexing is not + // possible + int hullArrayOffset = byteCount; + + // Array of hull instances + byteCount += hullCount * sizeof( b3HullInstance ); + + // Packed shared hull blobs + for ( int i = 0; i < sharedHullCount; ++i ) + { + sharedHulls[i].hullOffset = byteCount; + byteCount += sharedHulls[i].hull->byteCount; + } + + // Mesh data layout has another level of indirection to allow for tight data packing + // 1. mesh instance array : mesh count array of b3MeshInstance with individual mesh transform, scale, and offset + // 2. heterogeneous array of shared mesh data : each shared mesh can have a different byte count, so direct indexing is not + // possible + int meshArrayOffset = byteCount; + + // Array of mesh instances + byteCount += meshCount * sizeof( b3MeshInstance ); + + // Packed shared mesh blobs + for ( int i = 0; i < sharedMeshCount; ++i ) + { + sharedMeshes[i].meshOffset = byteCount; + byteCount += sharedMeshes[i].meshData->byteCount; + } + + int sphereOffset = byteCount; + byteCount += def->sphereCount * sizeof( b3CompoundSphere ); + + b3CompoundData* compound = b3Alloc( byteCount ); + memset( compound, 0, byteCount ); + + compound->version = B3_COMPOUND_VERSION; + compound->byteCount = byteCount; + compound->nodeOffset = nodeOffset; + memcpy( &compound->tree, &tree, sizeof( b3DynamicTree ) ); + + // todo clean up this mess + compound->tree.freeList = 0; + compound->tree.leafIndices = NULL; + compound->tree.leafBoxes = NULL; + compound->tree.leafCenters = NULL; + compound->tree.binIndices = NULL; + compound->tree.rebuildCapacity = 0; + + compound->tree.nodes = NULL; + compound->materialOffset = materialOffset; + compound->materialCount = materialCount; + compound->capsuleOffset = capsuleOffset; + compound->capsuleCount = capsuleCount; + compound->hullOffset = hullArrayOffset; + compound->hullCount = hullCount; + compound->meshOffset = meshArrayOffset; + compound->meshCount = meshCount; + compound->sphereOffset = sphereOffset; + compound->sphereCount = sphereCount; + + // Tree nodes + b3TreeNode* nodes = b3GetCompoundNodes( compound ); + memcpy( nodes, tree.nodes, tree.nodeCapacity * sizeof( b3TreeNode ) ); + compound->tree.nodes = nodes; + + // Materials + B3_ASSERT( materialCount > 0 ); + b3SurfaceMaterial* destinationMaterials = (b3SurfaceMaterial*)( (intptr_t)compound + compound->materialOffset ); + if ( materials != NULL ) + { + memcpy( destinationMaterials, materials, materialCount * sizeof( b3SurfaceMaterial ) ); + } + + // Capsules + if ( def->capsuleCount > 0 ) + { + B3_ASSERT( compound->capsuleOffset > 0 ); + b3CompoundCapsule* capsules = (b3CompoundCapsule*)( (intptr_t)compound + compound->capsuleOffset ); + memcpy( capsules, capsuleInstances, capsuleCount * sizeof( b3CompoundCapsule ) ); + } + + // Hulls + for ( int i = 0; i < hullCount; ++i ) + { + // Fix up offsets + int sharedIndex = hullInstances[i].hullOffset; + B3_ASSERT( 0 <= sharedIndex && sharedIndex < sharedHullCount ); + hullInstances[i].hullOffset = sharedHulls[sharedIndex].hullOffset; + } + + b3HullInstance* destinationHullInstances = (b3HullInstance*)( (intptr_t)compound + hullArrayOffset ); + memcpy( destinationHullInstances, hullInstances, hullCount * sizeof( b3HullInstance ) ); + + for ( int i = 0; i < sharedHullCount; ++i ) + { + int offset = sharedHulls[i].hullOffset; + b3HullData* destinationHull = (b3HullData*)( (intptr_t)compound + offset ); + memcpy( destinationHull, sharedHulls[i].hull, sharedHulls[i].hull->byteCount ); + } + + compound->sharedHullCount = sharedHullCount; + + // Meshes + for ( int i = 0; i < meshCount; ++i ) + { + // Fix up offsets + int sharedIndex = meshInstances[i].meshOffset; + B3_ASSERT( 0 <= sharedIndex && sharedIndex < sharedMeshCount ); + meshInstances[i].meshOffset = sharedMeshes[sharedIndex].meshOffset; + } + + b3MeshInstance* destinationMeshInstances = (b3MeshInstance*)( (intptr_t)compound + meshArrayOffset ); + memcpy( destinationMeshInstances, meshInstances, meshCount * sizeof( b3MeshInstance ) ); + + for ( int i = 0; i < sharedMeshCount; ++i ) + { + int offset = sharedMeshes[i].meshOffset; + b3MeshData* destinationMesh = (b3MeshData*)( (intptr_t)compound + offset ); + memcpy( destinationMesh, sharedMeshes[i].meshData, sharedMeshes[i].meshData->byteCount ); + } + + compound->sharedMeshCount = sharedMeshCount; + + // Spheres + if ( def->sphereCount > 0 ) + { + B3_ASSERT( compound->sphereOffset > 0 ); + b3CompoundSphere* spheres = (b3CompoundSphere*)( (intptr_t)compound + compound->sphereOffset ); + memcpy( spheres, sphereInstances, sphereCount * sizeof( b3CompoundSphere ) ); + } + + b3MaterialMap_cleanup( &materialMap ); + b3Free( sharedHulls, hullCount * sizeof( b3SharedHull ) ); + b3Free( sharedMeshes, meshCount * sizeof( b3SharedMesh ) ); + b3Free( capsuleInstances, capsuleCount * sizeof( b3CompoundCapsule ) ); + b3Free( hullInstances, hullCount * sizeof( b3HullInstance ) ); + b3Free( meshInstances, meshCount * sizeof( b3MeshInstance ) ); + b3Free( sphereInstances, sphereCount * sizeof( b3CompoundSphere ) ); + b3Free( materials, materialCapacity * sizeof( b3SurfaceMaterial ) ); + b3DynamicTree_Destroy( &tree ); + + return compound; +} + +void b3DestroyCompound( b3CompoundData* compound ) +{ + b3Free( compound, compound->byteCount ); +} + +uint8_t* b3ConvertCompoundToBytes( b3CompoundData* compound ) +{ + // scrub this pointer before serialization + compound->tree.nodes = NULL; + return (uint8_t*)compound; +} + +b3CompoundData* b3ConvertBytesToCompound( uint8_t* bytes, int byteCount ) +{ + b3CompoundData* compound = (b3CompoundData*)bytes; + if ( compound->version != B3_COMPOUND_VERSION ) + { + return NULL; + } + + if ( compound->byteCount < (int)sizeof( b3CompoundData ) ) + { + return NULL; + } + + if ( byteCount != compound->byteCount ) + { + return NULL; + } + + if ( compound->nodeOffset <= 0 ) + { + return NULL; + } + + // this mutates the input bytes + compound->tree.nodes = (b3TreeNode*)( (intptr_t)compound + compound->nodeOffset ); + return compound; +} + +b3AABB b3ComputeCompoundAABB( const b3CompoundData* shape, b3Transform transform ) +{ + B3_ASSERT( shape->nodeOffset > 0 ); + + const b3TreeNode* nodes = (const b3TreeNode*)( (intptr_t)shape + shape->nodeOffset ); + int root = shape->tree.root; + b3AABB aabb = nodes[root].aabb; + return b3AABB_Transform( transform, aabb ); +} + +struct b3CompoundOverlapContext +{ + const b3CompoundData* compound; + // transform of the compound + b3Transform transform; + b3ShapeProxy proxy; + bool overlap; +}; + +static bool b3CompoundOverlapCallback( int proxyId, uint64_t userData, void* context ) +{ + B3_UNUSED( proxyId ); + + int childIndex = (int)userData; + struct b3CompoundOverlapContext* overlapContext = context; + b3ChildShape child = b3GetCompoundChild( overlapContext->compound, childIndex ); + + b3Transform transform = b3MulTransforms( overlapContext->transform, child.transform ); + + bool overlap = false; + switch ( child.type ) + { + case b3_capsuleShape: + overlap = b3OverlapCapsule( &child.capsule, transform, &overlapContext->proxy ); + break; + + case b3_hullShape: + overlap = b3OverlapHull( child.hull, transform, &overlapContext->proxy ); + break; + + case b3_meshShape: + overlap = b3OverlapMesh( &child.mesh, transform, &overlapContext->proxy ); + break; + + case b3_sphereShape: + overlap = b3OverlapSphere( &child.sphere, transform, &overlapContext->proxy ); + break; + + default: + B3_ASSERT( false ); + break; + } + + if ( overlap ) + { + // Done + overlapContext->overlap = true; + return false; + } + + // Continue the query if there is no overlap + return true; +} + +bool b3OverlapCompound( const b3CompoundData* shape, b3Transform shapeTransform, const b3ShapeProxy* proxy ) +{ + struct b3CompoundOverlapContext context = { + .compound = shape, + .transform = shapeTransform, + .proxy = *proxy, + .overlap = false, + }; + + b3AABB aabb = { proxy->points[0], proxy->points[0] }; + for ( int i = 1; i < proxy->count; ++i ) + { + aabb.lowerBound = b3Min( aabb.lowerBound, proxy->points[i] ); + aabb.upperBound = b3Max( aabb.upperBound, proxy->points[i] ); + } + + b3Vec3 r = { proxy->radius, proxy->radius, proxy->radius }; + aabb.lowerBound = b3Sub( aabb.lowerBound, r ); + aabb.upperBound = b3Add( aabb.upperBound, r ); + + (void)b3DynamicTree_Query( &shape->tree, aabb, ~0ull, false, b3CompoundOverlapCallback, &context ); + + return context.overlap; +} + +struct b3CompoundCastContext +{ + const b3CompoundData* compound; + b3CastOutput* output; + // origin of the shape cast, the box cast callback only carries the advancing fraction + const b3ShapeCastInput* shapeInput; +}; + +static float b3CompoundRayCastCallback( const b3RayCastInput* input, int proxyId, uint64_t userData, void* context ) +{ + B3_UNUSED( proxyId ); + + struct b3CompoundCastContext* castContext = context; + const b3CompoundData* compound = castContext->compound; + + int childIndex = (int)userData; + + b3ChildShape child = b3GetCompoundChild( compound, childIndex ); + + b3RayCastInput localInput = *input; + localInput.origin = b3InvTransformPoint( child.transform, input->origin ); + localInput.translation = b3InvRotateVector( child.transform.q, input->translation ); + + b3CastOutput output = { 0 }; + + switch ( child.type ) + { + case b3_capsuleShape: + output = b3RayCastCapsule( &child.capsule, &localInput ); + output.materialIndex = child.materialIndices[0]; + break; + + case b3_hullShape: + output = b3RayCastHull( child.hull, &localInput ); + output.materialIndex = child.materialIndices[0]; + break; + + case b3_meshShape: + { + output = b3RayCastMesh( &child.mesh, &localInput ); + B3_ASSERT( 0 <= output.materialIndex ); + int childMaterialIndex = b3MinInt( output.materialIndex, B3_MAX_COMPOUND_MESH_MATERIALS - 1 ); + output.materialIndex = child.materialIndices[childMaterialIndex]; + } + break; + + case b3_sphereShape: + output = b3RayCastSphere( &child.sphere, &localInput ); + output.materialIndex = child.materialIndices[0]; + break; + + default: + B3_ASSERT( false ); + break; + } + + if ( output.hit ) + { + output.point = b3TransformPoint( child.transform, output.point ); + output.normal = b3RotateVector( child.transform.q, output.normal ); + output.childIndex = childIndex; + *castContext->output = output; + return output.fraction; + } + + return input->maxFraction; +} + +b3CastOutput b3RayCastCompound( const b3CompoundData* shape, const b3RayCastInput* input ) +{ + b3CastOutput result = { 0 }; + + struct b3CompoundCastContext context = { + .compound = shape, + .output = &result, + }; + (void)b3DynamicTree_RayCast( &shape->tree, input, ~0ull, false, b3CompoundRayCastCallback, &context ); + return result; +} + +static float b3CompoundShapeCastCallback( const b3BoxCastInput* input, int proxyId, uint64_t userData, void* context ) +{ + B3_UNUSED( proxyId ); + + struct b3CompoundCastContext* castContext = context; + const b3CompoundData* compound = castContext->compound; + const b3ShapeCastInput* shapeInput = castContext->shapeInput; + + int childIndex = (int)userData; + + b3ChildShape child = b3GetCompoundChild( compound, childIndex ); + + // Rebuild from the carried shape cast input, taking only the advancing fraction from the tree + b3ShapeCastInput localInput = *shapeInput; + localInput.maxFraction = input->maxFraction; + b3Vec3 localPoints[B3_MAX_SHAPE_CAST_POINTS]; + + localInput.proxy.count = b3MinInt( shapeInput->proxy.count, B3_MAX_SHAPE_CAST_POINTS ); + + b3Transform invTransform = b3InvertTransform( child.transform ); + b3Matrix3 R = b3MakeMatrixFromQuat( invTransform.q ); + + for ( int i = 0; i < localInput.proxy.count; ++i ) + { + localPoints[i] = b3Add( b3MulMV( R, shapeInput->proxy.points[i] ), invTransform.p ); + } + + localInput.proxy.points = localPoints; + localInput.translation = b3MulMV( R, shapeInput->translation ); + + b3CastOutput output = { 0 }; + + switch ( child.type ) + { + case b3_capsuleShape: + output = b3ShapeCastCapsule( &child.capsule, &localInput ); + output.materialIndex = child.materialIndices[0]; + break; + + case b3_hullShape: + output = b3ShapeCastHull( child.hull, &localInput ); + output.materialIndex = child.materialIndices[0]; + break; + + case b3_meshShape: + { + output = b3ShapeCastMesh( &child.mesh, &localInput ); + B3_ASSERT( 0 <= output.materialIndex ); + int childMaterialIndex = b3MinInt( output.materialIndex, B3_MAX_COMPOUND_MESH_MATERIALS - 1 ); + output.materialIndex = child.materialIndices[childMaterialIndex]; + } + break; + + case b3_sphereShape: + output = b3ShapeCastSphere( &child.sphere, &localInput ); + output.materialIndex = child.materialIndices[0]; + break; + + default: + B3_ASSERT( false ); + break; + } + + if ( output.hit ) + { + output.point = b3TransformPoint( child.transform, output.point ); + output.normal = b3RotateVector( child.transform.q, output.normal ); + output.childIndex = childIndex; + *castContext->output = output; + return output.fraction; + } + + return input->maxFraction; +} + +b3CastOutput b3ShapeCastCompound( const b3CompoundData* shape, const b3ShapeCastInput* input ) +{ + b3CastOutput result = { 0 }; + + if ( input->proxy.count == 0 ) + { + return result; + } + + struct b3CompoundCastContext context = { + .compound = shape, + .output = &result, + .shapeInput = input, + }; + + // The compound tree is in the compound local frame, so the proxy box needs no origin offset + b3AABB box = b3MakeAABB( input->proxy.points, input->proxy.count, input->proxy.radius ); + b3BoxCastInput treeInput = { box, input->translation, input->maxFraction }; + (void)b3DynamicTree_BoxCast( &shape->tree, &treeInput, ~0ull, false, b3CompoundShapeCastCallback, &context ); + return result; +} + +struct b3CompoundQueryContext +{ + const b3CompoundData* compound; + b3CompoundQueryFcn* fcn; + void* userContext; +}; + +static bool TreeQueryCallbackFcn( int proxyId, uint64_t userData, void* treeContext ) +{ + B3_UNUSED( proxyId ); + struct b3CompoundQueryContext* context = treeContext; + return context->fcn( context->compound, (int)userData, context->userContext ); +} + +void b3QueryCompound( const b3CompoundData* compound, b3AABB aabb, b3CompoundQueryFcn* fcn, void* context ) +{ + struct b3CompoundQueryContext compoundContext = { + .compound = compound, + .fcn = fcn, + .userContext = context, + }; + + b3DynamicTree_Query( &compound->tree, aabb, B3_DEFAULT_MASK_BITS, false, TreeQueryCallbackFcn, &compoundContext ); +} + +#if 0 +struct b3CompoundImpactContext +{ + b3TOIInput toiInput; + b3TOIOutput toiOutput; + b3Transform compoundTransform; + + // Bounds local to compound + b3AABB localSweepBoundsB; + + // Centroid of shape in body B local space + b3Vec3 localCentroidB; + float fallbackRadius; +}; + +static bool b3CompoundTimeOfImpactFcn( const b3CompoundData* compound, int childIndex, void* context ) +{ + b3CompoundImpactContext* toiContext = (b3CompoundImpactContext*)context; + + b3ChildShape child = b3GetCompoundChild( compound, childIndex ); + + b3TOIOutput output = {0 }; + toiContext->toiInput.sweepA = b3MakeCompoundChildSweep( toiContext->compoundTransform, child.transform ); + + switch ( child.type ) + { + case b3_capsuleShape: + { + toiContext->toiInput.proxyA.points = &child.capsule.center1; + toiContext->toiInput.proxyA.count = 2; + toiContext->toiInput.proxyA.radius = child.capsule.radius; + output = b3TimeOfImpact( &toiContext->toiInput ); + } + break; + + case b3_hullShape: + { + toiContext->toiInput.proxyA.points = b3GetHullPoints( child.hull ); + toiContext->toiInput.proxyA.count = child.hull->vertexCount; + toiContext->toiInput.proxyA.radius = 0.0f; + output = b3TimeOfImpact( &toiContext->toiInput ); + } + break; + + case b3_meshShape: + { + b3MeshImpactContext meshContext = {0}; + meshContext.toiInput = toiContext->toiInput; + meshContext.isSensor = false; + meshContext.localCentroidB = toiContext->localCentroidB; + meshContext.fallbackRadius = toiContext->fallbackRadius; + + b3Transform meshWorldTransform = b3MulTransforms( toiContext->compoundTransform, child.transform ); + + const b3Sweep* sweepB = &toiContext->toiInput.sweepB; + b3Transform xfB1 = { + .p = sweepB->c1 - b3RotateVector( sweepB->q1, sweepB->localCenter ), + .q = sweepB->q1, + }; + + b3Transform xfB2 = { + .p = sweepB->c2 - b3RotateVector( sweepB->q2, sweepB->localCenter ), + .q = sweepB->q2, + }; + + meshContext.meshLocalCentroidB1 = + b3InvTransformPoint( meshWorldTransform, b3TransformPoint( xfB1, meshContext.localCentroidB ) ); + meshContext.meshLocalCentroidB2 = + b3InvTransformPoint( meshWorldTransform, b3TransformPoint( xfB2, meshContext.localCentroidB ) ); + + // Bounds local to mesh + b3AABB localBounds = b3AABB_Transform( b3InvertTransform( child.transform ), toiContext->localSweepBoundsB ); + + b3QueryMesh( &child.mesh, localBounds, b3MeshTimeOfImpactFcn, &meshContext ); + + output = meshContext.toiOutput; + } + break; + + case b3_sphereShape: + { + toiContext->toiInput.proxyA.points = &child.sphere.center; + toiContext->toiInput.proxyA.count = 1; + toiContext->toiInput.proxyA.radius = child.sphere.radius; + output = b3TimeOfImpact( &toiContext->toiInput ); + } + break; + + default: + B3_ASSERT( false ); + break; + } + + if ( 0.0f < output.fraction && output.fraction < toiContext->toiInput.maxFraction ) + { + toiContext->toiOutput = output; + toiContext->toiInput.maxFraction = output.fraction; + } + + // Clear this to be safe + toiContext->toiInput.proxyA = {0}; + + // Continue the query + return true; +} + +b3TOIOutput b3CompoundTimeOfImpact(const b3CompoundData* compound, b3Transform transform, const b3ShapeProxy* proxy, + const b3Sweep* sweep, float maxFraction) +{ + b3CompoundImpactContext context = {0}; + context.toiInput.proxyB = b3MakeShapeProxy( shapeB ); + context.toiInput.sweepB = *sweepB; + context.toiInput.maxFraction = maxFraction; + + context.compoundTransform = { + .p = sweepA->c1, + .q = sweepA->q1, + }; + + b3Vec3 localCentroidB = b3GetShapeCentroid( shapeB ); + context.localCentroidB = localCentroidB; + + b3ShapeExtent extents = b3ComputeShapeExtent( shapeB, context.localCentroidB ); + context.fallbackRadius = b3MaxFloat( 0.5f * extents.minExtent, B3_SPECULATIVE_DISTANCE ); + + // Swept bounds of shapeB + b3AABB aabb = b3ComputeSweptShapeAABB( shapeB, sweepB, maxFraction ); + + // Bounds local to mesh + b3AABB localBounds = b3AABB_Transform( b3InvertTransform( context.compoundTransform ), bounds ); + context.localSweepBoundsB = localBounds; + + b3DynamicTree_Query( &compound->tree, aabb, B3_DEFAULT_MASK_BITS, false, TreeQueryCallbackFcn, &compoundContext ); + + return context.toiOutput; +} +#endif + +// xf = xfP * xfC +b3Sweep b3MakeCompoundChildSweep( b3Transform compoundTransform, b3Transform childTransform ) +{ + b3Transform xf = b3MulTransforms( compoundTransform, childTransform ); + return (b3Sweep){ + .localCenter = b3Vec3_zero, + .c1 = xf.p, + .c2 = xf.p, + .q1 = xf.q, + .q2 = xf.q, + }; +} + +struct b3CompoundMoverContext +{ + const b3CompoundData* compound; + b3PlaneResult* planes; + int planeCapacity; + int planeCount; + b3Capsule mover; +}; + +static bool b3CompoundMoverCallback( int proxyId, uint64_t userData, void* context ) +{ + B3_UNUSED( proxyId ); + + int childIndex = (int)userData; + struct b3CompoundMoverContext* moverContext = context; + b3ChildShape child = b3GetCompoundChild( moverContext->compound, childIndex ); + + // Transform mover to child space + b3Capsule localMover; + localMover.center1 = b3InvTransformPoint( child.transform, moverContext->mover.center1 ); + localMover.center2 = b3InvTransformPoint( child.transform, moverContext->mover.center2 ); + localMover.radius = moverContext->mover.radius; + + int capacity = moverContext->planeCapacity - moverContext->planeCount; + B3_ASSERT( capacity > 0 ); + + b3PlaneResult* planes = moverContext->planes + moverContext->planeCount; + int planeCount = 0; + + switch ( child.type ) + { + case b3_capsuleShape: + planeCount = b3CollideMoverAndCapsule( planes, &child.capsule, &localMover ); + break; + + case b3_hullShape: + planeCount = b3CollideMoverAndHull( planes, child.hull, &localMover ); + break; + + case b3_meshShape: + planeCount = b3CollideMoverAndMesh( planes, capacity, &child.mesh, &localMover ); + break; + + case b3_sphereShape: + planeCount = b3CollideMoverAndSphere( planes, &child.sphere, &localMover ); + break; + + default: + B3_ASSERT( false ); + break; + } + + // Transform results back to shape space + for ( int i = 0; i < planeCount; ++i ) + { + planes[i].plane.normal = b3RotateVector( child.transform.q, planes[i].plane.normal ); + planes[i].point = b3TransformPoint( child.transform, planes[i].point ); + } + + moverContext->planeCount += planeCount; + + // Continue query while there is room for more planes + return moverContext->planeCount < moverContext->planeCapacity; +} + +int b3CollideMoverAndCompound( b3PlaneResult* planes, int capacity, const b3CompoundData* shape, const b3Capsule* mover ) +{ + struct b3CompoundMoverContext context = { + .compound = shape, + .planes = planes, + .planeCapacity = capacity, + .planeCount = 0, + .mover = *mover, + }; + + b3AABB aabb; + aabb.lowerBound = b3Min( mover->center1, mover->center2 ); + aabb.upperBound = b3Max( mover->center1, mover->center2 ); + b3Vec3 r = { mover->radius, mover->radius, mover->radius }; + aabb.lowerBound = b3Sub( aabb.lowerBound, r ); + aabb.upperBound = b3Add( aabb.upperBound, r ); + + (void)b3DynamicTree_Query( &shape->tree, aabb, ~0ull, false, b3CompoundMoverCallback, &context ); + + return context.planeCount; +} diff --git a/vendor/box3d/src/src/compound.h b/vendor/box3d/src/src/compound.h new file mode 100644 index 000000000..1f4d7e508 --- /dev/null +++ b/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 ); diff --git a/vendor/box3d/src/src/constraint_graph.c b/vendor/box3d/src/src/constraint_graph.c new file mode 100644 index 000000000..9f06e9e4e --- /dev/null +++ b/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 + +// 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; + } +} diff --git a/vendor/box3d/src/src/constraint_graph.h b/vendor/box3d/src/src/constraint_graph.h new file mode 100644 index 000000000..ab17e9ac8 --- /dev/null +++ b/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 ); diff --git a/vendor/box3d/src/src/contact.c b/vendor/box3d/src/src/contact.c new file mode 100644 index 000000000..92e38b018 --- /dev/null +++ b/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; +} diff --git a/vendor/box3d/src/src/contact.h b/vendor/box3d/src/src/contact.h new file mode 100644 index 000000000..ad9fa74d6 --- /dev/null +++ b/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 ); diff --git a/vendor/box3d/src/src/contact_solver.c b/vendor/box3d/src/src/contact_solver.c new file mode 100644 index 000000000..501f85876 --- /dev/null +++ b/vendor/box3d/src/src/contact_solver.c @@ -0,0 +1,2472 @@ +// SPDX-FileCopyrightText: 2025 Erin Catto +// SPDX-License-Identifier: MIT + +#include "contact_solver.h" + +#include "body.h" +#include "constraint_graph.h" +#include "contact.h" +#include "core.h" +#include "math_internal.h" +#include "physics_world.h" +#include "platform.h" +#include "solver_set.h" + +#if B3_ENABLE_VALIDATION +#include "shape.h" +#endif + +#define FIXED_ANCHORS 1 + +// contact separation for sub-stepping +// s = s0 + dot(cB + rB - cA - rA, normal) +// normal is held constant +// body positions c can translation and anchors r can rotate +// s(t) = s0 + dot(cB(t) + rB(t) - cA(t) - rA(t), normal) +// s(t) = s0 + dot(cB0 + dpB + rot(dqB, rB0) - cA0 - dpA - rot(dqA, rA0), normal) +// s(t) = s0 + dot(cB0 - cA0, normal) + dot(dpB - dpA + rot(dqB, rB0) - rot(dqA, rA0), normal) +// s_base = s0 + dot(cB0 - cA0, normal) + +// Prepare a mesh constraints +void b3PrepareContacts_Mesh( b3SolverBlock block, b3StepContext* context ) +{ + b3TracyCZoneNC( prepare_contact, "Prepare Contact", b3_colorYellow, true ); + + b3World* world = context->world; + b3BodySim* bodySims = context->sims; + b3BodyState* bodyStates = context->states; + + float warmStartScale = world->enableWarmStarting ? 1.0f : 0.0f; + + // Need to use spans in order to find the associated b2Contact, which is per color + b3ContactPrepareSpan* spans = context->contactPrepareSpans; + b3ManifoldConstraint* manifoldBase = context->manifoldConstraints; + b3ContactConstraint* base = context->contactConstraints; + + // Overflow constraints are stored separately + if ( block.blockType == b3_overflowBlock ) + { + b3GraphColor* overflow = world->constraintGraph.colors + B3_OVERFLOW_INDEX; + spans = context->overflowSpans; + manifoldBase = overflow->manifoldConstraints; + base = overflow->contactConstraints; + } + + int index = block.startIndex; + int endIndex = block.startIndex + block.count; + + // Find color for start index. Linear search but fast. + int colorIndex = 0; + while ( spans[colorIndex + 1].start <= index ) + { + colorIndex += 1; + } + + // Loop over block + while ( index < endIndex ) + { + int colorStart = spans[colorIndex].start; + int colorEndIndex = b3MinInt( spans[colorIndex + 1].start, endIndex ); + b3ContactSpec* specs = spans[colorIndex].contacts; + + // Loop over color + for ( ; index < colorEndIndex; ++index ) + { + b3ContactConstraint* contactConstraint = base + index; + + int localIndex = index - colorStart; + B3_ASSERT( 0 <= localIndex && localIndex < spans[colorIndex].count ); + int contactId = specs[localIndex].contactId; + b3Contact* contact = b3Array_Get( world->contacts, contactId ); + B3_ASSERT( contact->contactId == contactId ); + + int indexA = contact->bodySimIndexA; + int indexB = contact->bodySimIndexB; + +#if B3_ENABLE_VALIDATION + if ( indexA != B3_NULL_INDEX ) + { + b3Body* bodyA = b3Array_Get( world->bodies, contact->edges[0].bodyId ); + B3_ASSERT( indexA == bodyA->localIndex ); + } + + if ( indexB != B3_NULL_INDEX ) + { + b3Body* bodyB = b3Array_Get( world->bodies, contact->edges[1].bodyId ); + B3_ASSERT( indexB == bodyB->localIndex ); + } +#endif + + // Body A data + float mA; + b3Matrix3 iA; + b3Vec3 vA; + b3Vec3 wA; + + if ( indexA == B3_NULL_INDEX ) + { + mA = 0.0f; + iA = b3Mat3_zero; + vA = b3Vec3_zero; + wA = b3Vec3_zero; + } + else + { + b3BodySim* simA = bodySims + indexA; + mA = simA->invMass; + iA = simA->invInertiaWorld; + + b3BodyState* stateA = bodyStates + indexA; + vA = stateA->linearVelocity; + wA = stateA->angularVelocity; + } + + // Body B data + float mB; + b3Matrix3 iB; + b3Vec3 vB; + b3Vec3 wB; + + if ( indexB == B3_NULL_INDEX ) + { + mB = 0.0f; + iB = b3Mat3_zero; + vB = b3Vec3_zero; + wB = b3Vec3_zero; + } + else + { + b3BodySim* simB = bodySims + indexB; + mB = simB->invMass; + iB = simB->invInertiaWorld; + + b3BodyState* stateB = bodyStates + indexB; + vB = stateB->linearVelocity; + wB = stateB->angularVelocity; + } + + int manifoldCount = contact->manifoldCount; + contactConstraint->contact = contact; + contactConstraint->manifoldCount = manifoldCount; + contactConstraint->indexA = indexA; + contactConstraint->indexB = indexB; + contactConstraint->invIA = iA; + contactConstraint->invMassA = mA; + contactConstraint->invIB = iB; + contactConstraint->invMassB = mB; + contactConstraint->rollingMass = b3InvertMatrix( b3AddMM( iA, iB ) ); + contactConstraint->softness = + ( contact->flags & b3_contactStaticFlag ) != 0 ? context->staticSoftness : context->contactSoftness; + contactConstraint->friction = contact->friction; + contactConstraint->restitution = contact->restitution; + contactConstraint->rollingResistance = contact->rollingResistance; + + b3ManifoldConstraint* manifoldConstraints = manifoldBase + specs[localIndex].manifoldStart; + contactConstraint->constraints = manifoldConstraints; + + for ( int manifoldIndex = 0; manifoldIndex < manifoldCount; ++manifoldIndex ) + { + b3Manifold* manifold = contact->manifolds + manifoldIndex; + b3ManifoldConstraint* constraint = manifoldConstraints + manifoldIndex; + int pointCount = manifold->pointCount; + b3Vec3 normal = manifold->normal; + b3Vec3 tangent1 = b3Perp( normal ); + b3Vec3 tangent2 = b3Cross( tangent1, normal ); + + constraint->pointCount = pointCount; + constraint->normal = normal; + constraint->tangent1 = tangent1; + constraint->tangent2 = tangent2; + + // Stiffer for static contacts to avoid bodies getting pushed through the ground + constraint->tangentVelocity1 = b3Dot( contact->tangentVelocity, constraint->tangent1 ); + constraint->tangentVelocity2 = b3Dot( contact->tangentVelocity, constraint->tangent2 ); + + b3Vec3 centerA = b3Vec3_zero; + b3Vec3 centerB = b3Vec3_zero; + + for ( int pointIndex = 0; pointIndex < pointCount; ++pointIndex ) + { + b3ManifoldConstraintPoint* cp = constraint->points + pointIndex; + + // Copy data from manifold point + b3ManifoldPoint* mp = manifold->points + pointIndex; + cp->rA = mp->anchorA; + cp->rB = mp->anchorB; + cp->baseSeparation = mp->separation - b3Dot( b3Sub( cp->rB, cp->rA ), normal ); + cp->normalImpulse = warmStartScale * mp->normalImpulse; + cp->totalNormalImpulse = 0.0f; + + b3Vec3 rA = cp->rA; + b3Vec3 rB = cp->rB; + + b3Vec3 rnA = b3Cross( rA, normal ); + b3Vec3 rnB = b3Cross( rB, normal ); + float kNormal = mA + mB + b3Dot( rnA, b3MulMV( iA, rnA ) ) + b3Dot( rnB, b3MulMV( iB, rnB ) ); + cp->normalMass = kNormal > 0.0f ? 1.0f / kNormal : 0.0f; + + // Save relative velocity for restitution + b3Vec3 vrA = b3Add( vA, b3Cross( wA, rA ) ); + b3Vec3 vrB = b3Add( vB, b3Cross( wB, rB ) ); + cp->relativeVelocity = b3Dot( normal, b3Sub( vrB, vrA ) ); + + centerA = b3Add( centerA, rA ); + centerB = b3Add( centerB, rB ); + } + + float invCount = 1.0f / pointCount; + centerA = b3MulSV( invCount, centerA ); + centerB = b3MulSV( invCount, centerB ); + constraint->originA = centerA; + constraint->originB = centerB; + + for ( int pointIndex = 0; pointIndex < pointCount; ++pointIndex ) + { + b3ManifoldConstraintPoint* cp = constraint->points + pointIndex; + cp->leverArm = b3Distance( cp->rA, centerA ); + } + + b3Vec3 rtA1 = b3Cross( centerA, tangent1 ); + b3Vec3 rtA2 = b3Cross( centerA, tangent2 ); + b3Vec3 rtB1 = b3Cross( centerB, tangent1 ); + b3Vec3 rtB2 = b3Cross( centerB, tangent2 ); + + { + b3Matrix2 k; + k.cx.x = mA + mB + b3Dot( rtA1, b3MulMV( iA, rtA1 ) ) + b3Dot( rtB1, b3MulMV( iB, rtB1 ) ); + k.cy.y = mA + mB + b3Dot( rtA2, b3MulMV( iA, rtA2 ) ) + b3Dot( rtB2, b3MulMV( iB, rtB2 ) ); + k.cx.y = k.cy.x = b3Dot( rtA1, b3MulMV( iA, rtA2 ) ) + b3Dot( rtB1, b3MulMV( iB, rtB2 ) ); + + constraint->tangentMass = b3Invert2( k ); + constraint->frictionImpulse.x = warmStartScale * b3Dot( manifold->frictionImpulse, tangent1 ); + constraint->frictionImpulse.y = warmStartScale * b3Dot( manifold->frictionImpulse, tangent2 ); + } + + { + float k = b3Dot( normal, b3MulMV( b3AddMM( iA, iB ), normal ) ); + constraint->twistMass = k > 0.0f ? 1.0f / k : 0.0f; + constraint->twistImpulse = warmStartScale * manifold->twistImpulse; + } + + { + constraint->rollingImpulse = b3MulSV( warmStartScale, manifold->rollingImpulse ); + } + } + } + + // Advance to next color + colorIndex += 1; + } + + b3TracyCZoneEnd( prepare_contact ); +} + +void b3WarmStartContacts_Mesh( b3SolverBlock block, b3StepContext* context ) +{ + b3World* world = context->world; + b3GraphColor* color = world->constraintGraph.colors + block.colorIndex; + b3SolverSet* awakeSet = b3Array_Get( world->solverSets, b3_awakeSet ); + b3BodyState* states = awakeSet->bodyStates.data; + b3ContactConstraint* constraints = color->contactConstraints; + + // This is a dummy state to represent a static body because static bodies don't have a solver body. + b3BodyState dummyState = b3_identityBodyState; + + int startIndex = block.startIndex; + int endIndex = startIndex + block.count; + + for ( int constraintIndex = startIndex; constraintIndex < endIndex; ++constraintIndex ) + { + const b3ContactConstraint* contactConstraint = constraints + constraintIndex; + int indexA = contactConstraint->indexA; + int indexB = contactConstraint->indexB; + + b3BodyState* stateA = indexA == B3_NULL_INDEX ? &dummyState : states + indexA; + b3BodyState* stateB = indexB == B3_NULL_INDEX ? &dummyState : states + indexB; + + b3Vec3 vA = stateA->linearVelocity; + b3Vec3 wA = stateA->angularVelocity; + b3Vec3 vB = stateB->linearVelocity; + b3Vec3 wB = stateB->angularVelocity; + + float mA = contactConstraint->invMassA; + b3Matrix3 iA = contactConstraint->invIA; + float mB = contactConstraint->invMassB; + b3Matrix3 iB = contactConstraint->invIB; + + int manifoldCount = contactConstraint->manifoldCount; + for ( int manifoldIndex = 0; manifoldIndex < manifoldCount; ++manifoldIndex ) + { + b3ManifoldConstraint* constraint = contactConstraint->constraints + manifoldIndex; + + // Normal impulses + b3Vec3 normal = constraint->normal; + int pointCount = constraint->pointCount; + for ( int j = 0; j < pointCount; ++j ) + { + const b3ManifoldConstraintPoint* cp = constraint->points + j; + + // fixed anchors + b3Vec3 rA = cp->rA; + b3Vec3 rB = cp->rB; + + b3Vec3 impulse = b3MulSV( cp->normalImpulse, normal ); + wA = b3Sub( wA, b3MulMV( iA, b3Cross( rA, impulse ) ) ); + vA = b3MulSub( vA, mA, impulse ); + wB = b3Add( wB, b3MulMV( iB, b3Cross( rB, impulse ) ) ); + vB = b3MulAdd( vB, mB, impulse ); + } + + // Central friction + { + b3Vec3 rA = constraint->originA; + b3Vec3 rB = constraint->originB; + b3Vec3 impulse = b3MulSV( constraint->frictionImpulse.x, constraint->tangent1 ); + impulse = b3Add( impulse, b3MulSV( constraint->frictionImpulse.y, constraint->tangent2 ) ); + + wA = b3Sub( wA, b3MulMV( iA, b3Cross( rA, impulse ) ) ); + vA = b3MulSub( vA, mA, impulse ); + wB = b3Add( wB, b3MulMV( iB, b3Cross( rB, impulse ) ) ); + vB = b3MulAdd( vB, mB, impulse ); + } + + // Central twist friction + { + b3Vec3 impulse = b3MulSV( constraint->twistImpulse, constraint->normal ); + wA = b3Sub( wA, b3MulMV( iA, impulse ) ); + wB = b3Add( wB, b3MulMV( iB, impulse ) ); + } + + // Rolling resistance + { + b3Vec3 impulse = constraint->rollingImpulse; + wA = b3Sub( wA, b3MulMV( iA, impulse ) ); + wB = b3Add( wB, b3MulMV( iB, impulse ) ); + } + } + + if ( stateA->flags & b3_dynamicFlag ) + { + stateA->linearVelocity = vA; + stateA->angularVelocity = wA; + } + + if ( stateB->flags & b3_dynamicFlag ) + { + stateB->linearVelocity = vB; + stateB->angularVelocity = wB; + } + } +} + +// Merged normal and friction loops. This is much more stable for the Jenga stack. +void b3SolveContacts_Mesh( b3SolverBlock block, b3StepContext* context, bool useBias ) +{ + b3World* world = context->world; + b3GraphColor* color = world->constraintGraph.colors + block.colorIndex; + b3ContactConstraint* contactConstraints = color->contactConstraints; + b3BodyState* states = context->states; + + // This is a dummy state to represent a static body because static bodies don't have a solver body. + b3BodyState dummyState = b3_identityBodyState; + + // The last block might not be full + int startIndex = block.startIndex; + int endIndex = startIndex + block.count; + + float inv_h = context->inv_h; + const float contactSpeed = context->world->contactSpeed; + + for ( int i = startIndex; i < endIndex; ++i ) + { + b3ContactConstraint* contactConstraint = contactConstraints + i; + int manifoldCount = contactConstraint->manifoldCount; + + int indexA = contactConstraint->indexA; + int indexB = contactConstraint->indexB; + + float mA = contactConstraint->invMassA; + b3Matrix3 iA = contactConstraint->invIA; + float mB = contactConstraint->invMassB; + b3Matrix3 iB = contactConstraint->invIB; + + b3BodyState* stateA = indexA == B3_NULL_INDEX ? &dummyState : states + indexA; + b3Vec3 vA = stateA->linearVelocity; + b3Vec3 wA = stateA->angularVelocity; + b3Quat dqA = stateA->deltaRotation; + + b3BodyState* stateB = indexB == B3_NULL_INDEX ? &dummyState : states + indexB; + b3Vec3 vB = stateB->linearVelocity; + b3Vec3 wB = stateB->angularVelocity; + b3Quat dqB = stateB->deltaRotation; + + b3Vec3 dp = b3Sub( stateB->deltaPosition, stateA->deltaPosition ); + b3Softness softness = contactConstraint->softness; + float friction = contactConstraint->friction; + float rollingResistance = contactConstraint->rollingResistance; + + for ( int j = 0; j < manifoldCount; ++j ) + { + b3ManifoldConstraint* constraint = contactConstraint->constraints + j; + + int pointCount = constraint->pointCount; + b3Vec3 normal = constraint->normal; + + float totalNormalImpulse = 0.0f; + float totalTwistLimit = 0.0f; + + for ( int pointIndex = 0; pointIndex < pointCount; ++pointIndex ) + { + b3ManifoldConstraintPoint* cp = constraint->points + pointIndex; + + // Fixed anchor points for applying impulses + b3Vec3 rA = cp->rA; + b3Vec3 rB = cp->rB; + + // compute current separation + // this is subject to round-off error if the anchor is far from the body center of mass + b3Vec3 ds = b3Add( dp, b3Sub( b3RotateVector( dqB, rB ), b3RotateVector( dqA, rA ) ) ); + float s = b3Dot( ds, normal ) + cp->baseSeparation; + + float velocityBias = 0.0f; + float massScale = 1.0f; + float impulseScale = 0.0f; + if ( s > 0.0f ) + { + // speculative bias + velocityBias = s * inv_h; + } + else if ( useBias ) + { + velocityBias = b3MaxFloat( softness.massScale * softness.biasRate * s, -contactSpeed ); + massScale = softness.massScale; + impulseScale = softness.impulseScale; + } + + // relative normal velocity at contact + b3Vec3 vrA = b3Add( vA, b3Cross( wA, rA ) ); + b3Vec3 vrB = b3Add( vB, b3Cross( wB, rB ) ); + float vn = b3Dot( b3Sub( vrB, vrA ), normal ); + + // incremental normal impulse + float deltaImpulse = -cp->normalMass * ( massScale * vn + velocityBias ) - impulseScale * cp->normalImpulse; + + // clamp the accumulated impulse + float newImpulse = b3MaxFloat( cp->normalImpulse + deltaImpulse, 0.0f ); + deltaImpulse = newImpulse - cp->normalImpulse; + cp->normalImpulse = newImpulse; + cp->totalNormalImpulse += newImpulse; + + totalNormalImpulse += newImpulse; + totalTwistLimit += cp->leverArm * cp->normalImpulse; + + // apply normal impulse + b3Vec3 P = b3MulSV( deltaImpulse, normal ); + 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 ) ) ); + } + + // No friction when applying bias + if ( useBias == true ) + { + // Go to next manifold + continue; + } + + // Central twist friction + { + float twistSpeed = b3Dot( constraint->normal, b3Sub( wB, wA ) ); + float maxImpulse = friction * totalTwistLimit; + float deltaImpulse = -constraint->twistMass * twistSpeed; + float oldImpulse = constraint->twistImpulse; + constraint->twistImpulse = b3ClampFloat( oldImpulse + deltaImpulse, -maxImpulse, maxImpulse ); + deltaImpulse = constraint->twistImpulse - oldImpulse; + + wA = b3Sub( wA, b3MulMV( iA, b3MulSV( deltaImpulse, constraint->normal ) ) ); + wB = b3Add( wB, b3MulMV( iB, b3MulSV( deltaImpulse, constraint->normal ) ) ); + } + + // Rolling resistance + if ( rollingResistance > 0.0f ) + { + b3Vec3 deltaImpulse = b3Neg( b3MulMV( contactConstraint->rollingMass, b3Sub( wB, wA ) ) ); + b3Vec3 oldImpulse = constraint->rollingImpulse; + constraint->rollingImpulse = b3Add( oldImpulse, deltaImpulse ); + + float maxImpulse = rollingResistance * totalNormalImpulse; + float magSqr = b3Dot( constraint->rollingImpulse, constraint->rollingImpulse ); + if ( magSqr > maxImpulse * maxImpulse + FLT_EPSILON ) + { + constraint->rollingImpulse = b3MulSV( maxImpulse / sqrtf( magSqr ), constraint->rollingImpulse ); + } + + deltaImpulse = b3Sub( constraint->rollingImpulse, oldImpulse ); + + wA = b3Sub( wA, b3MulMV( iA, deltaImpulse ) ); + wB = b3Add( wB, b3MulMV( iB, deltaImpulse ) ); + } + + // Central friction + { + b3Vec3 tangent1 = constraint->tangent1; + b3Vec3 tangent2 = constraint->tangent2; + + // Fixed anchor points for applying impulses + b3Vec3 rA = constraint->originA; + b3Vec3 rB = constraint->originB; + + // Relative tangent velocity at contact + b3Vec3 vrA = b3Add( vA, b3Cross( wA, rA ) ); + b3Vec3 vrB = b3Add( vB, b3Cross( wB, rB ) ); + b3Vec3 vr = b3Sub( vrB, vrA ); + b3Vec2 vt = { + b3Dot( vr, tangent1 ) - constraint->tangentVelocity1, + b3Dot( vr, tangent2 ) - constraint->tangentVelocity2, + }; + + // Incremental tangent impulse + b3Vec2 tm = b3MulMV2( constraint->tangentMass, vt ); + b3Vec2 deltaImpulse = { -tm.x, -tm.y }; + b3Vec2 newImpulse = { + constraint->frictionImpulse.x + deltaImpulse.x, + constraint->frictionImpulse.y + deltaImpulse.y, + }; + + float maxImpulse = friction * totalNormalImpulse; + + // Clamp the accumulated impulse + float lengthSquared = b3Dot2( newImpulse, newImpulse ); + if ( lengthSquared > maxImpulse * maxImpulse ) + { + float scale = maxImpulse / sqrtf( lengthSquared ); + newImpulse.x *= scale; + newImpulse.y *= scale; + } + deltaImpulse = b3Sub2( newImpulse, constraint->frictionImpulse ); + constraint->frictionImpulse = newImpulse; + + // Apply delta impulse + b3Vec3 P = b3Blend2( deltaImpulse.x, tangent1, deltaImpulse.y, tangent2 ); + 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 b3ApplyRestitution_Mesh( b3SolverBlock block, b3StepContext* context ) +{ + b3World* world = context->world; + b3GraphColor* color = world->constraintGraph.colors + block.colorIndex; + b3BodyState* states = context->states; + b3ContactConstraint* constraints = color->contactConstraints; + + // This is a dummy state to represent a static body because static bodies don't have a solver body. + b3BodyState dummyState = b3_identityBodyState; + + int startIndex = block.startIndex; + int endIndex = startIndex + block.count; + + float threshold = context->world->restitutionThreshold; + + for ( int constraintIndex = startIndex; constraintIndex < endIndex; ++constraintIndex ) + { + const b3ContactConstraint* contactConstraint = constraints + constraintIndex; + float restitution = contactConstraint->restitution; + if ( restitution == 0.0f ) + { + continue; + } + + int indexA = contactConstraint->indexA; + int indexB = contactConstraint->indexB; + + b3BodyState* stateA = indexA == B3_NULL_INDEX ? &dummyState : states + indexA; + b3BodyState* stateB = indexB == B3_NULL_INDEX ? &dummyState : states + indexB; + + b3Vec3 vA = stateA->linearVelocity; + b3Vec3 wA = stateA->angularVelocity; + b3Vec3 vB = stateB->linearVelocity; + b3Vec3 wB = stateB->angularVelocity; + + float mA = contactConstraint->invMassA; + b3Matrix3 iA = contactConstraint->invIA; + float mB = contactConstraint->invMassB; + b3Matrix3 iB = contactConstraint->invIB; + + int manifoldCount = contactConstraint->manifoldCount; + for ( int manifoldIndex = 0; manifoldIndex < manifoldCount; ++manifoldIndex ) + { + b3ManifoldConstraint* cm = contactConstraint->constraints + manifoldIndex; + + b3Vec3 normal = cm->normal; + int pointCount = cm->pointCount; + B3_ASSERT( 0 < pointCount && pointCount <= B3_MAX_MANIFOLD_POINTS ); + + for ( int pointIndex = 0; pointIndex < pointCount; ++pointIndex ) + { + b3ManifoldConstraintPoint* cp = cm->points + pointIndex; + + // If the total normal impulse is zero then there was no collision + // this skips speculative contact points that didn't generate an impulse + // The max normal impulse is used in case there was a collision that moved away within the sub-step + // process + if ( cp->relativeVelocity > -threshold || cp->totalNormalImpulse == 0.0f ) + { + continue; + } + + // fixed anchor points + b3Vec3 rA = cp->rA; + b3Vec3 rB = cp->rB; + + // relative normal velocity at contact + b3Vec3 vrB = b3Add( vB, b3Cross( wB, rB ) ); + b3Vec3 vrA = b3Add( vA, b3Cross( wA, rA ) ); + float vn = b3Dot( b3Sub( vrB, vrA ), normal ); + + // compute normal impulse + float impulse = -cp->normalMass * ( vn + restitution * cp->relativeVelocity ); + + // clamp the accumulated impulse + float newImpulse = b3MaxFloat( cp->normalImpulse + impulse, 0.0f ); + impulse = newImpulse - cp->normalImpulse; + cp->normalImpulse = newImpulse; + cp->totalNormalImpulse += impulse; + + // apply contact impulse + b3Vec3 P = b3MulSV( impulse, normal ); + 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; + } + } + } +} + +// Don't need to use spans for colors for this because the constraint to contact association +// is already linked by pointer. +void b3StoreImpulses_Mesh( b3SolverBlock block, b3StepContext* context, int workerIndex ) +{ + b3World* world = context->world; + + // Mirror b3PrepareContacts_Mesh: the per-color flat arrays and the overflow color + // each have their own (base, spans, manifoldBase). + b3ContactPrepareSpan* spans = context->contactPrepareSpans; + b3ContactConstraint* base = context->contactConstraints; + + if ( block.blockType == b3_overflowBlock ) + { + b3GraphColor* overflow = world->constraintGraph.colors + B3_OVERFLOW_INDEX; + spans = context->overflowSpans; + base = overflow->contactConstraints; + } + + b3TaskContext* taskContext = world->taskContexts.data + workerIndex; + b3BitSet* hitEventBitSet = &taskContext->hitEventBitSet; + bool hasHitEvents = taskContext->hasHitEvents; + float negHitThreshold = -world->hitEventThreshold; + + int index = block.startIndex; + int endIndex = block.startIndex + block.count; + + // Find color for start index. Linear search but fast. + int colorIndex = 0; + while ( spans[colorIndex + 1].start <= index ) + { + colorIndex += 1; + } + + // Loop over block + while ( index < endIndex ) + { + int colorStart = spans[colorIndex].start; + int colorEndIndex = b3MinInt( spans[colorIndex + 1].start, endIndex ); + + // Loop over color + for ( ; index < colorEndIndex; ++index ) + { + b3ContactConstraint* contactConstraint = base + index; + + int localIndex = index - colorStart; + B3_UNUSED( localIndex ); + B3_ASSERT( 0 <= localIndex && localIndex < spans[colorIndex].count ); + + // Having this contact pointer simplifies impulse storage + b3Contact* contact = contactConstraint->contact; + B3_ASSERT( contact != NULL ); + + // Catches the wrong-(base, spans) pairing: the contact pointer stashed by + // b3PrepareContacts_Mesh at this flat slot must reference the same contact + // the span at this slot describes. + B3_VALIDATE( contact->contactId == spans[colorIndex].contacts[localIndex].contactId ); + + int manifoldCount = contactConstraint->manifoldCount; + B3_ASSERT( manifoldCount == contact->manifoldCount ); + + bool checkHitEvents = ( contact->flags & b3_simEnableHitEvent ) != 0; + bool flagged = false; + + for ( int manifoldIndex = 0; manifoldIndex < manifoldCount; ++manifoldIndex ) + { + b3Manifold* manifold = contact->manifolds + manifoldIndex; + b3ManifoldConstraint* constraint = contactConstraint->constraints + manifoldIndex; + manifold->twistImpulse = constraint->twistImpulse; + manifold->frictionImpulse = b3Blend2( constraint->frictionImpulse.x, constraint->tangent1, + constraint->frictionImpulse.y, constraint->tangent2 ); + manifold->rollingImpulse = constraint->rollingImpulse; + + int count = constraint->pointCount; + B3_ASSERT( count == manifold->pointCount ); + for ( int pointIndex = 0; pointIndex < count; ++pointIndex ) + { + b3ManifoldConstraintPoint* cp = constraint->points + pointIndex; + b3ManifoldPoint* mp = manifold->points + pointIndex; + mp->normalImpulse = cp->normalImpulse; + mp->totalNormalImpulse = cp->totalNormalImpulse; + mp->normalVelocity = cp->relativeVelocity; + + if ( checkHitEvents && flagged == false && + mp->normalVelocity < negHitThreshold && mp->totalNormalImpulse > 0.0f ) + { + b3SetBit( hitEventBitSet, contact->contactId ); + hasHitEvents = true; + flagged = true; + } + } + } + } + + // Advance to next color + colorIndex += 1; + } + + taskContext->hasHitEvents = hasHitEvents; +} + +#if defined( B3_SIMD_NEON ) + +#include + +// wide float holds 4 numbers +typedef float32x4_t b3FloatW; + +#elif defined( B3_SIMD_SSE2 ) + +#include + +// wide float holds 4 numbers +typedef __m128 b3FloatW; + +#else + +// scalar math +typedef struct b3FloatW +{ + float x, y, z, w; +} b3FloatW; + +#endif + +// Wide vec2 +typedef struct b3Vec2W +{ + b3FloatW x, y; +} b3Vec2W; + +// Wide vec3 +typedef struct b3Vec3W +{ + b3FloatW X, Y, Z; +} b3Vec3W; + +// Wide quaternion +typedef struct b3QuatW +{ + b3Vec3W V; + b3FloatW S; +} b3QuatW; + +// Wide symmetric matrix2 +typedef struct b3SymMatrix2W +{ + b3FloatW cxx, cxy, cyy; +} b3SymMatrix2W; + +// Wide symmetric matrix3 +typedef struct b3SymMatrix3W +{ + b3FloatW cxx, cxy, cxz, cyy, cyz, czz; +} b3SymMatrix3W; + +#if defined( B3_SIMD_NEON ) + +static inline b3FloatW b3ZeroW( void ) +{ + return vdupq_n_f32( 0.0f ); +} + +static inline b3FloatW b3SplatW( float scalar ) +{ + return vdupq_n_f32( scalar ); +} + +static inline b3FloatW b3NegW( b3FloatW a ) +{ + return vnegq_f32( a ); +} + +static inline b3FloatW b3SetW( float a, float b, float c, float d ) +{ + float32_t array[4] = { a, b, c, d }; + return vld1q_f32( array ); +} + +static inline b3FloatW b3AddW( b3FloatW a, b3FloatW b ) +{ + return vaddq_f32( a, b ); +} + +static inline b3FloatW b3SubW( b3FloatW a, b3FloatW b ) +{ + return vsubq_f32( a, b ); +} + +static inline b3FloatW b3MulW( b3FloatW a, b3FloatW b ) +{ + return vmulq_f32( a, b ); +} + +static inline b3FloatW b3DivW( b3FloatW a, b3FloatW b ) +{ + return vdivq_f32( a, b ); +} + +static inline b3FloatW b3SqrtW( b3FloatW a ) +{ + return vsqrtq_f32( a ); +} + +// Cannot use real FMA because it doesn't match the non-SIMD path +static inline b3FloatW b3MulAddW( b3FloatW a, b3FloatW b, b3FloatW c ) +{ + return vaddq_f32( a, vmulq_f32( b, c ) ); +} + +// static inline b3FloatW b3MulSubW( b3FloatW a, b3FloatW b, b3FloatW c ) +//{ +// return vsubq_f32( a, vmulq_f32( b, c ) ); +// } + +static inline b3FloatW b3MinW( b3FloatW a, b3FloatW b ) +{ + return vminq_f32( a, b ); +} + +static inline b3FloatW b3MaxW( b3FloatW a, b3FloatW b ) +{ + return vmaxq_f32( a, b ); +} + +// clamp a to [-b, b] +static inline b3FloatW b3SymClampW( b3FloatW a, b3FloatW b ) +{ + b3FloatW nb = b3NegW( b ); + b3FloatW c = b3MaxW( nb, a ); + return b3MinW( c, b ); +} + +static inline b3FloatW b3OrW( b3FloatW a, b3FloatW b ) +{ + return vreinterpretq_f32_u32( vorrq_u32( vreinterpretq_u32_f32( a ), vreinterpretq_u32_f32( b ) ) ); +} + +static inline b3FloatW b3GreaterThanW( b3FloatW a, b3FloatW b ) +{ + return vreinterpretq_f32_u32( vcgtq_f32( a, b ) ); +} + +static inline b3FloatW b3EqualsW( b3FloatW a, b3FloatW b ) +{ + return vreinterpretq_f32_u32( vceqq_f32( a, b ) ); +} + +static inline bool b3AllZeroW( b3FloatW a ) +{ + // Create a zero vector for comparison + b3FloatW zero = vdupq_n_f32( 0.0f ); + + // Compare the input vector with zero + uint32x4_t cmp_result = vceqq_f32( a, zero ); + +// Check if all comparison results are non-zero using vminvq +#ifdef __ARM_FEATURE_SVE + // ARM v8.2+ has horizontal minimum instruction + return vminvq_u32( cmp_result ) != 0; +#else + // For older ARM architectures, we need to manually check all lanes + return vgetq_lane_u32( cmp_result, 0 ) != 0 && vgetq_lane_u32( cmp_result, 1 ) != 0 && vgetq_lane_u32( cmp_result, 2 ) != 0 && + vgetq_lane_u32( cmp_result, 3 ) != 0; +#endif +} + +// component-wise returns mask ? b : a +static inline b3FloatW b3BlendW( b3FloatW a, b3FloatW b, b3FloatW mask ) +{ + uint32x4_t mask32 = vreinterpretq_u32_f32( mask ); + return vbslq_f32( mask32, b, a ); +} + +#elif defined( B3_SIMD_SSE2 ) + +static inline b3FloatW b3ZeroW( void ) +{ + return _mm_setzero_ps(); +} + +static inline b3FloatW b3SplatW( float scalar ) +{ + return _mm_set1_ps( scalar ); +} + +static inline b3FloatW b3NegW( b3FloatW a ) +{ + // Create a mask with the sign bit set for each element + __m128 mask = _mm_set1_ps( -0.0f ); + + // XOR the input with the mask to negate each element + return _mm_xor_ps( a, mask ); +} + +static inline b3FloatW b3SetW( float a, float b, float c, float d ) +{ + return _mm_setr_ps( a, b, c, d ); +} + +static inline b3FloatW b3AddW( b3FloatW a, b3FloatW b ) +{ + return _mm_add_ps( a, b ); +} + +static inline b3FloatW b3SubW( b3FloatW a, b3FloatW b ) +{ + return _mm_sub_ps( a, b ); +} + +static inline b3FloatW b3MulW( b3FloatW a, b3FloatW b ) +{ + return _mm_mul_ps( a, b ); +} + +static inline b3FloatW b3DivW( b3FloatW a, b3FloatW b ) +{ + return _mm_div_ps( a, b ); +} + +static inline b3FloatW b3SqrtW( b3FloatW a ) +{ + return _mm_sqrt_ps( a ); +} + +static inline b3FloatW b3MulAddW( b3FloatW a, b3FloatW b, b3FloatW c ) +{ + return _mm_add_ps( a, _mm_mul_ps( b, c ) ); +} + +// static inline b3FloatW b3MulSubW( b3FloatW a, b3FloatW b, b3FloatW c ) +//{ +// return _mm_sub_ps( a, _mm_mul_ps( b, c ) ); +// } + +static inline b3FloatW b3MinW( b3FloatW a, b3FloatW b ) +{ + return _mm_min_ps( a, b ); +} + +static inline b3FloatW b3MaxW( b3FloatW a, b3FloatW b ) +{ + return _mm_max_ps( a, b ); +} + +// clamp a to [-b, b] +static inline b3FloatW b3SymClampW( b3FloatW a, b3FloatW b ) +{ + b3FloatW nb = b3NegW( b ); + b3FloatW c = b3MaxW( nb, a ); + return b3MinW( c, b ); +} + +static inline b3FloatW b3OrW( b3FloatW a, b3FloatW b ) +{ + return _mm_or_ps( a, b ); +} + +static inline b3FloatW b3GreaterThanW( b3FloatW a, b3FloatW b ) +{ + return _mm_cmpgt_ps( a, b ); +} + +static inline b3FloatW b3EqualsW( b3FloatW a, b3FloatW b ) +{ + return _mm_cmpeq_ps( a, b ); +} + +static inline bool b3AllZeroW( b3FloatW a ) +{ + // Compare each element with zero + b3FloatW zero = _mm_setzero_ps(); + b3FloatW cmp = _mm_cmpeq_ps( a, zero ); + + // Create a mask from the comparison results + int mask = _mm_movemask_ps( cmp ); + + // If all elements are zero, the mask will be 0xF (1111 in binary) + return mask == 0xF; +} + +// component-wise returns mask ? b : a +static inline b3FloatW b3BlendW( b3FloatW a, b3FloatW b, b3FloatW mask ) +{ + return _mm_or_ps( _mm_and_ps( mask, b ), _mm_andnot_ps( mask, a ) ); +} + +#else + +static inline b3FloatW b3ZeroW( void ) +{ + return (b3FloatW){ 0.0f, 0.0f, 0.0f, 0.0f }; +} + +static inline b3FloatW b3SplatW( float scalar ) +{ + return (b3FloatW){ scalar, scalar, scalar, scalar }; +} + +static inline b3FloatW b3NegW( b3FloatW a ) +{ + return (b3FloatW){ -a.x, -a.y, -a.z, -a.w }; +} + +static inline b3FloatW b3AddW( b3FloatW a, b3FloatW b ) +{ + return (b3FloatW){ a.x + b.x, a.y + b.y, a.z + b.z, a.w + b.w }; +} + +static inline b3FloatW b3SubW( b3FloatW a, b3FloatW b ) +{ + return (b3FloatW){ a.x - b.x, a.y - b.y, a.z - b.z, a.w - b.w }; +} + +static inline b3FloatW b3MulW( b3FloatW a, b3FloatW b ) +{ + return (b3FloatW){ a.x * b.x, a.y * b.y, a.z * b.z, a.w * b.w }; +} + +static inline b3FloatW b3DivW( b3FloatW a, b3FloatW b ) +{ + return (b3FloatW){ a.x / b.x, a.y / b.y, a.z / b.z, a.w / b.w }; +} + +static inline b3FloatW b3SqrtW( b3FloatW a ) +{ + return (b3FloatW){ sqrtf( a.x ), sqrtf( a.y ), sqrtf( a.z ), sqrtf( a.w ) }; +} + +static inline b3FloatW b3MulAddW( b3FloatW a, b3FloatW b, b3FloatW c ) +{ + return (b3FloatW){ a.x + b.x * c.x, a.y + b.y * c.y, a.z + b.z * c.z, a.w + b.w * c.w }; +} + +// static inline b3FloatW b3MulSubW( b3FloatW a, b3FloatW b, b3FloatW c ) +//{ +// return { a.x - b.x * c.x, a.y - b.y * c.y, a.z - b.z * c.z, a.w - b.w * c.w }; +// } + +// static inline b3FloatW b3MinW( b3FloatW a, b3FloatW b ) +//{ +// b3FloatW r; +// r.x = a.x <= b.x ? a.x : b.x; +// r.y = a.y <= b.y ? a.y : b.y; +// r.z = a.z <= b.z ? a.z : b.z; +// r.w = a.w <= b.w ? a.w : b.w; +// return r; +// } + +static inline b3FloatW b3MaxW( b3FloatW a, b3FloatW b ) +{ + b3FloatW r; + r.x = a.x >= b.x ? a.x : b.x; + r.y = a.y >= b.y ? a.y : b.y; + r.z = a.z >= b.z ? a.z : b.z; + r.w = a.w >= b.w ? a.w : b.w; + return r; +} + +// clamp a to [-b, b] +static inline b3FloatW b3SymClampW( b3FloatW a, b3FloatW b ) +{ + b3FloatW r; + r.x = a.x <= b.x ? a.x : b.x; + r.y = a.y <= b.y ? a.y : b.y; + r.z = a.z <= b.z ? a.z : b.z; + r.w = a.w <= b.w ? a.w : b.w; + r.x = r.x <= -b.x ? -b.x : r.x; + r.y = r.y <= -b.y ? -b.y : r.y; + r.z = r.z <= -b.z ? -b.z : r.z; + r.w = r.w <= -b.w ? -b.w : r.w; + return r; +} + +static inline b3FloatW b3OrW( b3FloatW a, b3FloatW b ) +{ + b3FloatW r; + r.x = a.x != 0.0f || b.x != 0.0f ? 1.0f : 0.0f; + r.y = a.y != 0.0f || b.y != 0.0f ? 1.0f : 0.0f; + r.z = a.z != 0.0f || b.z != 0.0f ? 1.0f : 0.0f; + r.w = a.w != 0.0f || b.w != 0.0f ? 1.0f : 0.0f; + return r; +} + +static inline b3FloatW b3GreaterThanW( b3FloatW a, b3FloatW b ) +{ + b3FloatW r; + r.x = a.x > b.x ? 1.0f : 0.0f; + r.y = a.y > b.y ? 1.0f : 0.0f; + r.z = a.z > b.z ? 1.0f : 0.0f; + r.w = a.w > b.w ? 1.0f : 0.0f; + return r; +} + +static inline b3FloatW b3EqualsW( b3FloatW a, b3FloatW b ) +{ + b3FloatW r; + r.x = a.x == b.x ? 1.0f : 0.0f; + r.y = a.y == b.y ? 1.0f : 0.0f; + r.z = a.z == b.z ? 1.0f : 0.0f; + r.w = a.w == b.w ? 1.0f : 0.0f; + return r; +} + +static inline bool b3AllZeroW( b3FloatW a ) +{ + return a.x == 0.0f && a.y == 0.0f && a.z == 0.0f && a.w == 0.0f; +} + +// component-wise returns mask ? b : a +static inline b3FloatW b3BlendW( b3FloatW a, b3FloatW b, b3FloatW mask ) +{ + b3FloatW r; + r.x = mask.x != 0.0f ? b.x : a.x; + r.y = mask.y != 0.0f ? b.y : a.y; + r.z = mask.z != 0.0f ? b.z : a.z; + r.w = mask.w != 0.0f ? b.w : a.w; + return r; +} + +#endif + +// s * a +static inline b3Vec3W b3MulSVW( b3FloatW s, b3Vec3W a ) +{ + return (b3Vec3W){ b3MulW( s, a.X ), b3MulW( s, a.Y ), b3MulW( s, a.Z ) }; +} + +// a - s * b +static inline b3Vec3W b3MulSubSVW( b3Vec3W a, b3FloatW s, b3Vec3W b ) +{ + return (b3Vec3W){ b3SubW( a.X, b3MulW( s, b.X ) ), b3SubW( a.Y, b3MulW( s, b.Y ) ), b3SubW( a.Z, b3MulW( s, b.Z ) ) }; +} + +// a + s * b +static inline b3Vec3W b3MulAddSVW( b3Vec3W a, b3FloatW s, b3Vec3W b ) +{ + return (b3Vec3W){ b3AddW( a.X, b3MulW( s, b.X ) ), b3AddW( a.Y, b3MulW( s, b.Y ) ), b3AddW( a.Z, b3MulW( s, b.Z ) ) }; +} + +// a + b +static inline b3Vec2W b3AddV2W( b3Vec2W a, b3Vec2W b ) +{ + return (b3Vec2W){ + b3AddW( a.x, b.x ), + b3AddW( a.y, b.y ), + }; +} + +// a - b +static inline b3Vec3W b3SubVW( b3Vec3W a, b3Vec3W b ) +{ + return (b3Vec3W){ + b3SubW( a.X, b.X ), + b3SubW( a.Y, b.Y ), + b3SubW( a.Z, b.Z ), + }; +} + +// a + b +static inline b3Vec3W b3AddVW( b3Vec3W a, b3Vec3W b ) +{ + return (b3Vec3W){ + b3AddW( a.X, b.X ), + b3AddW( a.Y, b.Y ), + b3AddW( a.Z, b.Z ), + }; +} + +// m * a +static inline b3Vec2W b3MulMV2W( b3SymMatrix2W m, b3Vec2W a ) +{ + b3Vec2W b = { + b3AddW( b3MulW( m.cxx, a.x ), b3MulW( m.cxy, a.y ) ), + b3AddW( b3MulW( m.cxy, a.x ), b3MulW( m.cyy, a.y ) ), + }; + + return b; +} + +// m * a +static inline b3Vec3W b3MulMVW( b3SymMatrix3W m, b3Vec3W a ) +{ + b3Vec3W b = { + b3AddW( b3MulW( m.cxx, a.X ), b3AddW( b3MulW( m.cxy, a.Y ), b3MulW( m.cxz, a.Z ) ) ), + b3AddW( b3MulW( m.cxy, a.X ), b3AddW( b3MulW( m.cyy, a.Y ), b3MulW( m.cyz, a.Z ) ) ), + b3AddW( b3MulW( m.cxz, a.X ), b3AddW( b3MulW( m.cyz, a.Y ), b3MulW( m.czz, a.Z ) ) ), + }; + + return b; +} + +// a - m * b +static inline b3Vec3W b3MulSubMVW( b3Vec3W a, b3SymMatrix3W m, b3Vec3W b ) +{ + b3Vec3W c = { + b3AddW( b3MulW( m.cxx, b.X ), b3AddW( b3MulW( m.cxy, b.Y ), b3MulW( m.cxz, b.Z ) ) ), + b3AddW( b3MulW( m.cxy, b.X ), b3AddW( b3MulW( m.cyy, b.Y ), b3MulW( m.cyz, b.Z ) ) ), + b3AddW( b3MulW( m.cxz, b.X ), b3AddW( b3MulW( m.cyz, b.Y ), b3MulW( m.czz, b.Z ) ) ), + }; + + return (b3Vec3W){ b3SubW( a.X, c.X ), b3SubW( a.Y, c.Y ), b3SubW( a.Z, c.Z ) }; +} + +// a + m * b +static inline b3Vec3W b3MulAddMVW( b3Vec3W a, b3SymMatrix3W m, b3Vec3W b ) +{ + b3Vec3W c = { + b3AddW( b3MulW( m.cxx, b.X ), b3AddW( b3MulW( m.cxy, b.Y ), b3MulW( m.cxz, b.Z ) ) ), + b3AddW( b3MulW( m.cxy, b.X ), b3AddW( b3MulW( m.cyy, b.Y ), b3MulW( m.cyz, b.Z ) ) ), + b3AddW( b3MulW( m.cxz, b.X ), b3AddW( b3MulW( m.cyz, b.Y ), b3MulW( m.czz, b.Z ) ) ), + }; + + return (b3Vec3W){ b3AddW( a.X, c.X ), b3AddW( a.Y, c.Y ), b3AddW( a.Z, c.Z ) }; +} + +static inline b3FloatW b3DotW( b3Vec3W a, b3Vec3W b ) +{ + return b3AddW( b3AddW( b3MulW( a.X, b.X ), b3MulW( a.Y, b.Y ) ), b3MulW( a.Z, b.Z ) ); +} + +static inline b3Vec3W b3CrossW( b3Vec3W a, b3Vec3W b ) +{ + b3Vec3W c; + c.X = b3SubW( b3MulW( a.Y, b.Z ), b3MulW( a.Z, b.Y ) ); + c.Y = b3SubW( b3MulW( a.Z, b.X ), b3MulW( a.X, b.Z ) ); + c.Z = b3SubW( b3MulW( a.X, b.Y ), b3MulW( a.Y, b.X ) ); + return c; +} + +static inline b3Vec3W b3RotateVectorW( b3QuatW q, b3Vec3W a ) +{ + b3Vec3W t1 = b3CrossW( q.V, a ); + b3Vec3W t2; + t2.X = b3MulAddW( t1.X, q.S, a.X ); + t2.Y = b3MulAddW( t1.Y, q.S, a.Y ); + t2.Z = b3MulAddW( t1.Z, q.S, a.Z ); + b3Vec3W t3 = b3CrossW( q.V, t2 ); + b3FloatW two = b3SplatW( 2.0f ); + b3Vec3W b; + b.X = b3MulAddW( a.X, two, t3.X ); + b.Y = b3MulAddW( a.Y, two, t3.Y ); + b.Z = b3MulAddW( a.Z, two, t3.Z ); + return b; +} + +// Soft contact constraints with sub-stepping support +// Uses fixed anchors for Jacobians for better behavior on rolling shapes (circles & capsules) +// http://mmacklin.com/smallsteps.pdf +// https://box2d.org/files/ErinCatto_SoftConstraints_GDC2011.pdf + +typedef struct b3ContactConstraintPointWide +{ + b3Vec3W anchorAs, anchorBs; + b3FloatW baseSeparations; + b3FloatW normalImpulses; + b3FloatW totalNormalImpulses; + b3FloatW normalMasses; + b3FloatW leverArms; + b3FloatW relativeVelocities; +} b3ContactConstraintPointWide; + +// Solves four points +typedef struct b3ContactConstraintWide +{ + // These are base 1 + int indexA[B3_SIMD_WIDTH]; + int indexB[B3_SIMD_WIDTH]; + + b3FloatW invMassA, invMassB; + b3SymMatrix3W invIA, invIB; + b3Vec3W normal; + + // todo test computing the tangents on the fly, at least tangent2 + b3Vec3W tangent1; + b3Vec3W tangent2; + + b3Vec3W originA, originB; + b3FloatW twistMass; + b3FloatW twistImpulse; + b3SymMatrix2W tangentMass; + b3Vec2W frictionImpulse; + b3SymMatrix3W rollingMass; + b3Vec3W rollingImpulse; + b3FloatW friction; + b3FloatW rollingResistance; + b3FloatW tangentVelocity1; + b3FloatW tangentVelocity2; + + b3FloatW biasRate; + b3FloatW massScale; + b3FloatW impulseScale; + b3FloatW restitution; + + b3Manifold* manifolds[B3_SIMD_WIDTH]; + + // todo store the maximum point count per wide constraint + // to make this work I need zero initialization which is too + // expensive for all the wide constraint data. Instead + // the graph color should store the point count as a compact secondary + // transient array with zero initialization. + b3ContactConstraintPointWide points[B3_MAX_MANIFOLD_POINTS]; + +} b3ContactConstraintWide; + +int b3GetWideContactConstraintByteCount( void ) +{ + return sizeof( b3ContactConstraintWide ); +} + +// wide version of b3BodyState +typedef struct b3BodyStateW +{ + b3Vec3W v; + b3Vec3W w; + b3Vec3W dp; + b3QuatW dq; +} b3BodyStateW; + +#if defined( B3_SIMD_SSE2 ) || defined( B3_SIMD_NEON ) + +static b3BodyStateW b3GatherBodies( const b3BodyState* states, int* indices ) +{ + b3BodyState dummy = { 0 }; + dummy.deltaRotation.s = 1.0f; + + // Indices are 0 for null + b3BodyState b1 = indices[0] == 0 ? dummy : states[indices[0] - 1]; + b3BodyState b2 = indices[1] == 0 ? dummy : states[indices[1] - 1]; + b3BodyState b3 = indices[2] == 0 ? dummy : states[indices[2] - 1]; + b3BodyState b4 = indices[3] == 0 ? dummy : states[indices[3] - 1]; + + b3BodyStateW s; + s.v.X = b3SetW( b1.linearVelocity.x, b2.linearVelocity.x, b3.linearVelocity.x, b4.linearVelocity.x ); + s.v.Y = b3SetW( b1.linearVelocity.y, b2.linearVelocity.y, b3.linearVelocity.y, b4.linearVelocity.y ); + s.v.Z = b3SetW( b1.linearVelocity.z, b2.linearVelocity.z, b3.linearVelocity.z, b4.linearVelocity.z ); + + s.w.X = b3SetW( b1.angularVelocity.x, b2.angularVelocity.x, b3.angularVelocity.x, b4.angularVelocity.x ); + s.w.Y = b3SetW( b1.angularVelocity.y, b2.angularVelocity.y, b3.angularVelocity.y, b4.angularVelocity.y ); + s.w.Z = b3SetW( b1.angularVelocity.z, b2.angularVelocity.z, b3.angularVelocity.z, b4.angularVelocity.z ); + + s.dp.X = b3SetW( b1.deltaPosition.x, b2.deltaPosition.x, b3.deltaPosition.x, b4.deltaPosition.x ); + s.dp.Y = b3SetW( b1.deltaPosition.y, b2.deltaPosition.y, b3.deltaPosition.y, b4.deltaPosition.y ); + s.dp.Z = b3SetW( b1.deltaPosition.z, b2.deltaPosition.z, b3.deltaPosition.z, b4.deltaPosition.z ); + + s.dq.V.X = b3SetW( b1.deltaRotation.v.x, b2.deltaRotation.v.x, b3.deltaRotation.v.x, b4.deltaRotation.v.x ); + s.dq.V.Y = b3SetW( b1.deltaRotation.v.y, b2.deltaRotation.v.y, b3.deltaRotation.v.y, b4.deltaRotation.v.y ); + s.dq.V.Z = b3SetW( b1.deltaRotation.v.z, b2.deltaRotation.v.z, b3.deltaRotation.v.z, b4.deltaRotation.v.z ); + s.dq.S = b3SetW( b1.deltaRotation.s, b2.deltaRotation.s, b3.deltaRotation.s, b4.deltaRotation.s ); + return s; +} + +// This writes only the velocities back to the solver bodies +static void b3ScatterBodies( b3BodyState* states, int* indices, const b3BodyStateW* simdBody ) +{ + const float* vx = (const float*)&simdBody->v.X; + const float* vy = (const float*)&simdBody->v.Y; + const float* vz = (const float*)&simdBody->v.Z; + const float* wx = (const float*)&simdBody->w.X; + const float* wy = (const float*)&simdBody->w.Y; + const float* wz = (const float*)&simdBody->w.Z; + + // I don't use any dummy body in the body array because this will lead to multithreaded sharing and the + // associated cache flushing. + + // Warning: indices start at 1 with 0 indicating null + + if ( indices[0] != 0 && ( states[indices[0] - 1].flags & b3_dynamicFlag ) != 0 ) + { + b3BodyState* s = states + ( indices[0] - 1 ); + + b3Vec3 v = { vx[0], vy[0], vz[0] }; + b3Vec3 w = { wx[0], wy[0], wz[0] }; + + uint32_t flags = s->flags; + if ( flags & b3_allLocks ) + { + v.x = ( flags & b3_lockLinearX ) ? 0.0f : v.x; + v.y = ( flags & b3_lockLinearY ) ? 0.0f : v.y; + v.z = ( flags & b3_lockLinearZ ) ? 0.0f : v.z; + w.x = ( flags & b3_lockAngularX ) ? 0.0f : w.x; + w.y = ( flags & b3_lockAngularY ) ? 0.0f : w.y; + w.z = ( flags & b3_lockAngularZ ) ? 0.0f : w.z; + } + + s->linearVelocity = v; + s->angularVelocity = w; + } + + if ( indices[1] != 0 && ( states[indices[1] - 1].flags & b3_dynamicFlag ) != 0 ) + { + b3BodyState* s = states + ( indices[1] - 1 ); + + b3Vec3 v = { vx[1], vy[1], vz[1] }; + b3Vec3 w = { wx[1], wy[1], wz[1] }; + + uint32_t flags = s->flags; + if ( flags & b3_allLocks ) + { + v.x = ( flags & b3_lockLinearX ) ? 0.0f : v.x; + v.y = ( flags & b3_lockLinearY ) ? 0.0f : v.y; + v.z = ( flags & b3_lockLinearZ ) ? 0.0f : v.z; + w.x = ( flags & b3_lockAngularX ) ? 0.0f : w.x; + w.y = ( flags & b3_lockAngularY ) ? 0.0f : w.y; + w.z = ( flags & b3_lockAngularZ ) ? 0.0f : w.z; + } + + s->linearVelocity = v; + s->angularVelocity = w; + } + + if ( indices[2] != 0 && ( states[indices[2] - 1].flags & b3_dynamicFlag ) != 0 ) + { + b3BodyState* s = states + ( indices[2] - 1 ); + + b3Vec3 v = { vx[2], vy[2], vz[2] }; + b3Vec3 w = { wx[2], wy[2], wz[2] }; + + uint32_t flags = s->flags; + if ( flags & b3_allLocks ) + { + v.x = ( flags & b3_lockLinearX ) ? 0.0f : v.x; + v.y = ( flags & b3_lockLinearY ) ? 0.0f : v.y; + v.z = ( flags & b3_lockLinearZ ) ? 0.0f : v.z; + w.x = ( flags & b3_lockAngularX ) ? 0.0f : w.x; + w.y = ( flags & b3_lockAngularY ) ? 0.0f : w.y; + w.z = ( flags & b3_lockAngularZ ) ? 0.0f : w.z; + } + + s->linearVelocity = v; + s->angularVelocity = w; + } + + if ( indices[3] != 0 && ( states[indices[3] - 1].flags & b3_dynamicFlag ) != 0 ) + { + b3BodyState* s = states + ( indices[3] - 1 ); + + b3Vec3 v = { vx[3], vy[3], vz[3] }; + b3Vec3 w = { wx[3], wy[3], wz[3] }; + + uint32_t flags = s->flags; + if ( flags & b3_allLocks ) + { + v.x = ( flags & b3_lockLinearX ) ? 0.0f : v.x; + v.y = ( flags & b3_lockLinearY ) ? 0.0f : v.y; + v.z = ( flags & b3_lockLinearZ ) ? 0.0f : v.z; + w.x = ( flags & b3_lockAngularX ) ? 0.0f : w.x; + w.y = ( flags & b3_lockAngularY ) ? 0.0f : w.y; + w.z = ( flags & b3_lockAngularZ ) ? 0.0f : w.z; + } + + s->linearVelocity = v; + s->angularVelocity = w; + } +} + +#else // non-simd + +static b3BodyStateW b3GatherBodies( const b3BodyState* states, int* indices ) +{ + b3BodyState identity = b3_identityBodyState; + + b3BodyState s1 = indices[0] == 0 ? identity : states[indices[0] - 1]; + b3BodyState s2 = indices[1] == 0 ? identity : states[indices[1] - 1]; + b3BodyState s3 = indices[2] == 0 ? identity : states[indices[2] - 1]; + b3BodyState s4 = indices[3] == 0 ? identity : states[indices[3] - 1]; + + b3BodyStateW simdBody; + simdBody.v.X = (b3FloatW){ s1.linearVelocity.x, s2.linearVelocity.x, s3.linearVelocity.x, s4.linearVelocity.x }; + simdBody.v.Y = (b3FloatW){ s1.linearVelocity.y, s2.linearVelocity.y, s3.linearVelocity.y, s4.linearVelocity.y }; + simdBody.v.Z = (b3FloatW){ s1.linearVelocity.z, s2.linearVelocity.z, s3.linearVelocity.z, s4.linearVelocity.z }; + simdBody.w.X = (b3FloatW){ s1.angularVelocity.x, s2.angularVelocity.x, s3.angularVelocity.x, s4.angularVelocity.x }; + simdBody.w.Y = (b3FloatW){ s1.angularVelocity.y, s2.angularVelocity.y, s3.angularVelocity.y, s4.angularVelocity.y }; + simdBody.w.Z = (b3FloatW){ s1.angularVelocity.z, s2.angularVelocity.z, s3.angularVelocity.z, s4.angularVelocity.z }; + simdBody.dp.X = (b3FloatW){ s1.deltaPosition.x, s2.deltaPosition.x, s3.deltaPosition.x, s4.deltaPosition.x }; + simdBody.dp.Y = (b3FloatW){ s1.deltaPosition.y, s2.deltaPosition.y, s3.deltaPosition.y, s4.deltaPosition.y }; + simdBody.dp.Z = (b3FloatW){ s1.deltaPosition.z, s2.deltaPosition.z, s3.deltaPosition.z, s4.deltaPosition.z }; + simdBody.dq.V.X = (b3FloatW){ s1.deltaRotation.v.x, s2.deltaRotation.v.x, s3.deltaRotation.v.x, s4.deltaRotation.v.x }; + simdBody.dq.V.Y = (b3FloatW){ s1.deltaRotation.v.y, s2.deltaRotation.v.y, s3.deltaRotation.v.y, s4.deltaRotation.v.y }; + simdBody.dq.V.Z = (b3FloatW){ s1.deltaRotation.v.z, s2.deltaRotation.v.z, s3.deltaRotation.v.z, s4.deltaRotation.v.z }; + simdBody.dq.S = (b3FloatW){ s1.deltaRotation.s, s2.deltaRotation.s, s3.deltaRotation.s, s4.deltaRotation.s }; + + return simdBody; +} + +// This writes only the velocities back to the solver bodies +static void b3ScatterBodies( b3BodyState* states, int* indices, const b3BodyStateW* simdBody ) +{ + int index1 = indices[0] - 1; + if ( index1 != -1 && ( states[index1].flags & b3_dynamicFlag ) != 0 ) + { + b3BodyState* state = states + index1; + state->linearVelocity.x = simdBody->v.X.x; + state->linearVelocity.y = simdBody->v.Y.x; + state->linearVelocity.z = simdBody->v.Z.x; + state->angularVelocity.x = simdBody->w.X.x; + state->angularVelocity.y = simdBody->w.Y.x; + state->angularVelocity.z = simdBody->w.Z.x; + } + + int index2 = indices[1] - 1; + if ( index2 != -1 && ( states[index2].flags & b3_dynamicFlag ) != 0 ) + { + b3BodyState* state = states + index2; + state->linearVelocity.x = simdBody->v.X.y; + state->linearVelocity.y = simdBody->v.Y.y; + state->linearVelocity.z = simdBody->v.Z.y; + state->angularVelocity.x = simdBody->w.X.y; + state->angularVelocity.y = simdBody->w.Y.y; + state->angularVelocity.z = simdBody->w.Z.y; + } + + int index3 = indices[2] - 1; + if ( index3 != -1 && ( states[index3].flags & b3_dynamicFlag ) != 0 ) + { + b3BodyState* state = states + index3; + state->linearVelocity.x = simdBody->v.X.z; + state->linearVelocity.y = simdBody->v.Y.z; + state->linearVelocity.z = simdBody->v.Z.z; + state->angularVelocity.x = simdBody->w.X.z; + state->angularVelocity.y = simdBody->w.Y.z; + state->angularVelocity.z = simdBody->w.Z.z; + } + + int index4 = indices[3] - 1; + if ( index4 != -1 && ( states[index4].flags & b3_dynamicFlag ) != 0 ) + { + b3BodyState* state = states + index4; + state->linearVelocity.x = simdBody->v.X.w; + state->linearVelocity.y = simdBody->v.Y.w; + state->linearVelocity.z = simdBody->v.Z.w; + state->angularVelocity.x = simdBody->w.X.w; + state->angularVelocity.y = simdBody->w.Y.w; + state->angularVelocity.z = simdBody->w.Z.w; + } +} +#endif + +// Prepare convex contact constraints +void b3PrepareContacts_Convex( b3SolverBlock block, b3StepContext* context ) +{ + b3TracyCZoneNC( prepare_contact, "Prepare Contact", b3_colorYellow, true ); + b3World* world = context->world; + b3BodySim* sims = context->sims; + b3BodyState* states = context->states; +#if B3_ENABLE_VALIDATION + b3Body* bodies = world->bodies.data; +#endif + b3WidePrepareSpan* spans = context->widePrepareSpans; + b3ContactConstraintWide* wideBase = context->wideConstraints; + + // Stiffer for static contacts to avoid bodies getting pushed through the ground + b3Softness contactSoftness = context->contactSoftness; + b3Softness staticSoftness = context->staticSoftness; + + float warmStartScale = world->enableWarmStarting ? 1.0f : 0.0f; + + int wideIndex = block.startIndex; + int endWideIndex = block.startIndex + block.count; + + // Find color for start index. Linear search but fast. + int colorIndex = 0; + while ( spans[colorIndex + 1].start <= wideIndex ) + { + colorIndex += 1; + } + + // Loop over block + while ( wideIndex < endWideIndex ) + { + int colorWideStart = spans[colorIndex].start; + int colorWideEndIndex = b3MinInt( spans[colorIndex + 1].start, endWideIndex ); + int colorContactCount = spans[colorIndex].count; + int* contactIds = spans[colorIndex].contacts; + + // Loop over color + for ( ; wideIndex < colorWideEndIndex; ++wideIndex ) + { + b3ContactConstraintWide* constraint = wideBase + wideIndex; + int localWideIndex = wideIndex - colorWideStart; + + for ( int lane = 0; lane < B3_SIMD_WIDTH; ++lane ) + { + int contactIndex = B3_SIMD_WIDTH * localWideIndex + lane; + if ( contactIndex >= colorContactCount ) + { + // Remainder lanes were zeroed in solver setup. + break; + } + + int contactId = contactIds[contactIndex]; + b3Contact* contact = b3Array_Get( world->contacts, contactId ); + B3_ASSERT( contact->manifoldCount == 1 ); + b3Manifold* manifold = contact->manifolds + 0; + + int indexA = contact->bodySimIndexA; + int indexB = contact->bodySimIndexB; + +#if B3_ENABLE_VALIDATION + b3Body* bodyA = bodies + contact->edges[0].bodyId; + int validIndexA = bodyA->setIndex == b3_awakeSet ? bodyA->localIndex : B3_NULL_INDEX; + b3Body* bodyB = bodies + contact->edges[1].bodyId; + int validIndexB = bodyB->setIndex == b3_awakeSet ? bodyB->localIndex : B3_NULL_INDEX; + B3_ASSERT( indexA == validIndexA ); + B3_ASSERT( indexB == validIndexB ); +#endif + + // 0 for null + constraint->indexA[lane] = indexA + 1; + constraint->indexB[lane] = indexB + 1; + constraint->manifolds[lane] = manifold; + + // Body A data + float mA; + b3Matrix3 iA; + b3Vec3 vA; + b3Vec3 wA; + + if ( indexA == B3_NULL_INDEX ) + { + mA = 0.0f; + iA = b3Mat3_zero; + vA = b3Vec3_zero; + wA = b3Vec3_zero; + } + else + { + b3BodySim* simA = sims + indexA; + mA = simA->invMass; + iA = simA->invInertiaWorld; + + b3BodyState* stateA = states + indexA; + vA = stateA->linearVelocity; + wA = stateA->angularVelocity; + } + + // Body B data + float mB; + b3Matrix3 iB; + b3Vec3 vB; + b3Vec3 wB; + + if ( indexB == B3_NULL_INDEX ) + { + mB = 0.0f; + iB = b3Mat3_zero; + vB = b3Vec3_zero; + wB = b3Vec3_zero; + } + else + { + b3BodySim* simB = sims + indexB; + mB = simB->invMass; + iB = simB->invInertiaWorld; + + b3BodyState* stateB = states + indexB; + vB = stateB->linearVelocity; + wB = stateB->angularVelocity; + } + + ( (float*)&constraint->invMassA )[lane] = mA; + ( (float*)&constraint->invMassB )[lane] = mB; + + ( (float*)&constraint->invIA.cxx )[lane] = iA.cx.x; + ( (float*)&constraint->invIA.cxy )[lane] = iA.cx.y; + ( (float*)&constraint->invIA.cxz )[lane] = iA.cx.z; + ( (float*)&constraint->invIA.cyy )[lane] = iA.cy.y; + ( (float*)&constraint->invIA.cyz )[lane] = iA.cy.z; + ( (float*)&constraint->invIA.czz )[lane] = iA.cz.z; + + ( (float*)&constraint->invIB.cxx )[lane] = iB.cx.x; + ( (float*)&constraint->invIB.cxy )[lane] = iB.cx.y; + ( (float*)&constraint->invIB.cxz )[lane] = iB.cx.z; + ( (float*)&constraint->invIB.cyy )[lane] = iB.cy.y; + ( (float*)&constraint->invIB.cyz )[lane] = iB.cy.z; + ( (float*)&constraint->invIB.czz )[lane] = iB.cz.z; + + b3Softness soft = ( indexA == B3_NULL_INDEX || indexB == B3_NULL_INDEX ) ? staticSoftness : contactSoftness; + + b3Vec3 normal = manifold->normal; + ( (float*)&constraint->normal.X )[lane] = normal.x; + ( (float*)&constraint->normal.Y )[lane] = normal.y; + ( (float*)&constraint->normal.Z )[lane] = normal.z; + + b3Vec3 tangent1 = b3Perp( normal ); + ( (float*)&constraint->tangent1.X )[lane] = tangent1.x; + ( (float*)&constraint->tangent1.Y )[lane] = tangent1.y; + ( (float*)&constraint->tangent1.Z )[lane] = tangent1.z; + + b3Vec3 tangent2 = b3Cross( tangent1, normal ); + ( (float*)&constraint->tangent2.X )[lane] = tangent2.x; + ( (float*)&constraint->tangent2.Y )[lane] = tangent2.y; + ( (float*)&constraint->tangent2.Z )[lane] = tangent2.z; + + ( (float*)&constraint->friction )[lane] = contact->friction; + ( (float*)&constraint->restitution )[lane] = contact->restitution; + ( (float*)&constraint->rollingResistance )[lane] = contact->rollingResistance; + + ( (float*)&constraint->tangentVelocity1 )[lane] = b3Dot( contact->tangentVelocity, tangent1 ); + ( (float*)&constraint->tangentVelocity2 )[lane] = b3Dot( contact->tangentVelocity, tangent2 ); + + ( (float*)&constraint->biasRate )[lane] = soft.biasRate; + ( (float*)&constraint->massScale )[lane] = soft.massScale; + ( (float*)&constraint->impulseScale )[lane] = soft.impulseScale; + + int pointCount = manifold->pointCount; + b3Vec3 originA = b3Vec3_zero; + b3Vec3 originB = b3Vec3_zero; + + for ( int pointIndex = 0; pointIndex < pointCount; ++pointIndex ) + { + const b3ManifoldPoint* mp = manifold->points + pointIndex; + b3ContactConstraintPointWide* cp = constraint->points + pointIndex; + + b3Vec3 rA = mp->anchorA; + b3Vec3 rB = mp->anchorB; + originA = b3Add( originA, rA ); + originB = b3Add( originB, rB ); + + ( (float*)&cp->anchorAs.X )[lane] = rA.x; + ( (float*)&cp->anchorAs.Y )[lane] = rA.y; + ( (float*)&cp->anchorAs.Z )[lane] = rA.z; + + ( (float*)&cp->anchorBs.X )[lane] = rB.x; + ( (float*)&cp->anchorBs.Y )[lane] = rB.y; + ( (float*)&cp->anchorBs.Z )[lane] = rB.z; + + float baseSeparation = mp->separation - b3Dot( b3Sub( rB, rA ), normal ); + ( (float*)&cp->baseSeparations )[lane] = baseSeparation; + + ( (float*)&cp->normalImpulses )[lane] = warmStartScale * mp->normalImpulse; + ( (float*)&cp->totalNormalImpulses )[lane] = 0.0f; + + b3Vec3 rnA = b3Cross( rA, normal ); + b3Vec3 rnB = b3Cross( rB, normal ); + float kNormal = mA + mB + b3Dot( rnA, b3MulMV( iA, rnA ) ) + b3Dot( rnB, b3MulMV( iB, rnB ) ); + ( (float*)&cp->normalMasses )[lane] = kNormal > 0.0f ? 1.0f / kNormal : 0.0f; + + // Save relative velocity for restitution + b3Vec3 vrA = b3Add( vA, b3Cross( wA, rA ) ); + b3Vec3 vrB = b3Add( vB, b3Cross( wB, rB ) ); + ( (float*)&cp->relativeVelocities )[lane] = b3Dot( normal, b3Sub( vrB, vrA ) ); + } + + float invCount = 1.0f / pointCount; + originA = b3MulSV( invCount, originA ); + originB = b3MulSV( invCount, originB ); + + ( (float*)&constraint->originA.X )[lane] = originA.x; + ( (float*)&constraint->originA.Y )[lane] = originA.y; + ( (float*)&constraint->originA.Z )[lane] = originA.z; + ( (float*)&constraint->originB.X )[lane] = originB.x; + ( (float*)&constraint->originB.Y )[lane] = originB.y; + ( (float*)&constraint->originB.Z )[lane] = originB.z; + + for ( int pointIndex = 0; pointIndex < pointCount; ++pointIndex ) + { + const b3ManifoldPoint* mp = manifold->points + pointIndex; + b3ContactConstraintPointWide* cp = constraint->points + pointIndex; + ( (float*)&cp->leverArms )[lane] = b3Distance( mp->anchorA, originA ); + } + + b3Vec3 rtA1 = b3Cross( originA, tangent1 ); + b3Vec3 rtA2 = b3Cross( originA, tangent2 ); + + b3Vec3 rtB1 = b3Cross( originB, tangent1 ); + b3Vec3 rtB2 = b3Cross( originB, tangent2 ); + + { + b3Matrix2 k; + k.cx.x = mA + mB + b3Dot( rtA1, b3MulMV( iA, rtA1 ) ) + b3Dot( rtB1, b3MulMV( iB, rtB1 ) ); + k.cy.y = mA + mB + b3Dot( rtA2, b3MulMV( iA, rtA2 ) ) + b3Dot( rtB2, b3MulMV( iB, rtB2 ) ); + k.cx.y = k.cy.x = b3Dot( rtA1, b3MulMV( iA, rtA2 ) ) + b3Dot( rtB1, b3MulMV( iB, rtB2 ) ); + b3Matrix2 tangentMass = b3Invert2( k ); + + ( (float*)&constraint->tangentMass.cxx )[lane] = tangentMass.cx.x; + ( (float*)&constraint->tangentMass.cxy )[lane] = tangentMass.cx.y; + ( (float*)&constraint->tangentMass.cyy )[lane] = tangentMass.cy.y; + + ( (float*)&constraint->frictionImpulse.x )[lane] = + warmStartScale * b3Dot( manifold->frictionImpulse, tangent1 ); + ( (float*)&constraint->frictionImpulse.y )[lane] = + warmStartScale * b3Dot( manifold->frictionImpulse, tangent2 ); + } + + { + float k = b3Dot( normal, b3MulMV( b3AddMM( iA, iB ), normal ) ); + ( (float*)&constraint->twistMass )[lane] = k > 0.0f ? 1.0f / k : 0.0f; + ( (float*)&constraint->twistImpulse )[lane] = warmStartScale * manifold->twistImpulse; + } + + { + b3Matrix3 rollingMass = b3InvertMatrix( b3AddMM( iA, iB ) ); + + ( (float*)&constraint->rollingMass.cxx )[lane] = rollingMass.cx.x; + ( (float*)&constraint->rollingMass.cxy )[lane] = rollingMass.cx.y; + ( (float*)&constraint->rollingMass.cxz )[lane] = rollingMass.cx.z; + ( (float*)&constraint->rollingMass.cyy )[lane] = rollingMass.cy.y; + ( (float*)&constraint->rollingMass.cyz )[lane] = rollingMass.cy.z; + ( (float*)&constraint->rollingMass.czz )[lane] = rollingMass.cz.z; + + ( (float*)&constraint->rollingImpulse.X )[lane] = warmStartScale * manifold->rollingImpulse.x; + ( (float*)&constraint->rollingImpulse.Y )[lane] = warmStartScale * manifold->rollingImpulse.y; + ( (float*)&constraint->rollingImpulse.Z )[lane] = warmStartScale * manifold->rollingImpulse.z; + } + + // zero remaining points + for ( int pointIndex = pointCount; pointIndex < B3_MAX_MANIFOLD_POINTS; ++pointIndex ) + { + b3ContactConstraintPointWide* cp = constraint->points + pointIndex; + ( (float*)&cp->anchorAs.X )[lane] = 0.0f; + ( (float*)&cp->anchorAs.Y )[lane] = 0.0f; + ( (float*)&cp->anchorAs.Z )[lane] = 0.0f; + ( (float*)&cp->anchorBs.X )[lane] = 0.0f; + ( (float*)&cp->anchorBs.Y )[lane] = 0.0f; + ( (float*)&cp->anchorBs.Z )[lane] = 0.0f; + ( (float*)&cp->baseSeparations )[lane] = 0.0f; + ( (float*)&cp->normalImpulses )[lane] = 0.0f; + ( (float*)&cp->totalNormalImpulses )[lane] = 0.0f; + ( (float*)&cp->normalMasses )[lane] = 0.0f; + ( (float*)&cp->relativeVelocities )[lane] = 0.0f; + ( (float*)&cp->leverArms )[lane] = 0.0f; + } + } + } + + // Advance to next color + colorIndex += 1; + } + + b3TracyCZoneEnd( prepare_contact ); +} + +void b3WarmStartContacts_Convex( b3SolverBlock block, b3StepContext* context ) +{ + b3TracyCZoneNC( warm_start_contact, "Warm Start", b3_colorGreen, true ); + + b3BodyState* states = context->states; + b3ContactConstraintWide* constraints = context->graph->colors[block.colorIndex].wideConstraints; + + for ( int i = block.startIndex; i < block.startIndex + block.count; ++i ) + { + b3ContactConstraintWide* c = constraints + i; + b3BodyStateW bA = b3GatherBodies( states, c->indexA ); + b3BodyStateW bB = b3GatherBodies( states, c->indexB ); + + // Normal impulses + for ( int j = 0; j < B3_MAX_MANIFOLD_POINTS; ++j ) + { + b3ContactConstraintPointWide* cp = c->points + j; + + b3Vec3W rA = cp->anchorAs; + b3Vec3W rB = cp->anchorBs; + + b3Vec3W impulse; + impulse.X = b3MulW( cp->normalImpulses, c->normal.X ); + impulse.Y = b3MulW( cp->normalImpulses, c->normal.Y ); + impulse.Z = b3MulW( cp->normalImpulses, c->normal.Z ); + + bA.w = b3MulSubMVW( bA.w, c->invIA, b3CrossW( rA, impulse ) ); + bA.v = b3MulSubSVW( bA.v, c->invMassA, impulse ); + bB.w = b3MulAddMVW( bB.w, c->invIB, b3CrossW( rB, impulse ) ); + bB.v = b3MulAddSVW( bB.v, c->invMassB, impulse ); + } + + // Central friction + { + b3Vec3W rA = c->originA; + b3Vec3W rB = c->originB; + b3Vec3W impulse = b3MulSVW( c->frictionImpulse.x, c->tangent1 ); + impulse = b3MulAddSVW( impulse, c->frictionImpulse.y, c->tangent2 ); + + bA.w = b3MulSubMVW( bA.w, c->invIA, b3CrossW( rA, impulse ) ); + bA.v = b3MulSubSVW( bA.v, c->invMassA, impulse ); + bB.w = b3MulAddMVW( bB.w, c->invIB, b3CrossW( rB, impulse ) ); + bB.v = b3MulAddSVW( bB.v, c->invMassB, impulse ); + } + + // Central twist friction + { + b3Vec3W impulse = b3MulSVW( c->twistImpulse, c->normal ); + bA.w = b3MulSubMVW( bA.w, c->invIA, impulse ); + bB.w = b3MulAddMVW( bB.w, c->invIB, impulse ); + } + + // Rolling resistance + { + b3Vec3W impulse = c->rollingImpulse; + bA.w = b3MulSubMVW( bA.w, c->invIA, impulse ); + bB.w = b3MulAddMVW( bB.w, c->invIB, impulse ); + } + + b3ScatterBodies( states, c->indexA, &bA ); + b3ScatterBodies( states, c->indexB, &bB ); + } + + b3TracyCZoneEnd( warm_start_contact ); +} + +void b3SolveContacts_Convex( b3SolverBlock block, b3StepContext* context, bool useBias ) +{ + b3TracyCZoneNC( solve_contact, "Solve Contact", b3_colorAliceBlue, true ); + + b3BodyState* states = context->states; + b3ContactConstraintWide* constraints = context->graph->colors[block.colorIndex].wideConstraints; + b3FloatW inv_h = b3SplatW( context->inv_h ); + b3FloatW contactSpeed = b3SplatW( -context->world->contactSpeed ); + b3FloatW oneW = b3SplatW( 1.0f ); + b3FloatW epsilonW = b3SplatW( FLT_EPSILON ); + + for ( int wideIndex = block.startIndex; wideIndex < block.startIndex + block.count; ++wideIndex ) + { + b3ContactConstraintWide* c = constraints + wideIndex; + + b3BodyStateW bA = b3GatherBodies( states, c->indexA ); + b3BodyStateW bB = b3GatherBodies( states, c->indexB ); + + b3FloatW biasRate, massScale, impulseScale; + if ( useBias ) + { + biasRate = b3MulW( c->massScale, c->biasRate ); + massScale = c->massScale; + impulseScale = c->impulseScale; + } + else + { + biasRate = b3ZeroW(); + massScale = oneW; + impulseScale = b3ZeroW(); + } + + b3Vec3W dp = b3SubVW( bB.dp, bA.dp ); + + b3FloatW totalNormalImpulse = b3ZeroW(); + b3FloatW totalTwistLimit = b3ZeroW(); + + // todo_erin use the max point count of the four manifolds + for ( int pointIndex = 0; pointIndex < B3_MAX_MANIFOLD_POINTS; ++pointIndex ) + { + b3ContactConstraintPointWide* cp = c->points + pointIndex; + + // Fixed anchor points for applying impulses + b3Vec3W rA = cp->anchorAs; + b3Vec3W rB = cp->anchorBs; + + // Moving anchors for current separation + // todo speed this up using matrices + b3Vec3W rsA = b3RotateVectorW( bA.dq, rA ); + b3Vec3W rsB = b3RotateVectorW( bB.dq, rB ); + + // compute current separation + // this is subject to round-off error if the anchor is far from the body center of mass + b3Vec3W ds = b3AddVW( dp, b3SubVW( rsB, rsA ) ); + b3FloatW s = b3AddW( b3DotW( c->normal, ds ), cp->baseSeparations ); + + // Apply speculative bias if separation is greater than zero, otherwise apply soft constraint bias + b3FloatW mask = b3GreaterThanW( s, b3ZeroW() ); + b3FloatW specBias = b3MulW( s, inv_h ); + b3FloatW softBias = b3MaxW( b3MulW( biasRate, s ), contactSpeed ); + b3FloatW bias = b3BlendW( softBias, specBias, mask ); + + b3FloatW pointMassScale = b3BlendW( massScale, oneW, mask ); + b3FloatW pointImpulseScale = b3BlendW( impulseScale, b3ZeroW(), mask ); + + // Relative velocity at contact + b3Vec3W vrA = b3AddVW( bA.v, b3CrossW( bA.w, rA ) ); + b3Vec3W vrB = b3AddVW( bB.v, b3CrossW( bB.w, rB ) ); + b3FloatW vn = b3DotW( b3SubVW( vrB, vrA ), c->normal ); + + // Compute normal impulse + b3FloatW negImpulse = b3AddW( b3MulW( cp->normalMasses, b3AddW( b3MulW( pointMassScale, vn ), bias ) ), + b3MulW( pointImpulseScale, cp->normalImpulses ) ); + + // Clamp the accumulated impulse + b3FloatW newImpulse = b3MaxW( b3SubW( cp->normalImpulses, negImpulse ), b3ZeroW() ); + b3FloatW deltaImpulse = b3SubW( newImpulse, cp->normalImpulses ); + cp->normalImpulses = newImpulse; + cp->totalNormalImpulses = b3AddW( cp->totalNormalImpulses, newImpulse ); + + totalNormalImpulse = b3AddW( totalNormalImpulse, newImpulse ); + totalTwistLimit = b3AddW( totalTwistLimit, b3MulW( cp->leverArms, newImpulse ) ); + + // Apply contact impulse + b3Vec3W P = b3MulSVW( deltaImpulse, c->normal ); + bA.w = b3MulSubMVW( bA.w, c->invIA, b3CrossW( rA, P ) ); + bA.v = b3MulSubSVW( bA.v, c->invMassA, P ); + bB.w = b3MulAddMVW( bB.w, c->invIB, b3CrossW( rB, P ) ); + bB.v = b3MulAddSVW( bB.v, c->invMassB, P ); + } + + // No friction when applying bias + if ( useBias == false ) + { + // Rolling resistance + if ( b3AllZeroW( c->rollingResistance ) == false ) + { + // flip A/B order to negate + b3Vec3W deltaImpulse = b3MulMVW( c->rollingMass, b3SubVW( bA.w, bB.w ) ); + b3Vec3W oldImpulse = c->rollingImpulse; + c->rollingImpulse = b3AddVW( oldImpulse, deltaImpulse ); + + b3FloatW maxImpulse = b3MulW( c->rollingResistance, totalNormalImpulse ); + b3FloatW lengthSquared = b3DotW( c->rollingImpulse, c->rollingImpulse ); + + // if ( magSqr > maxLambda * maxLambda + FLT_EPSILON ) + //{ + // c->rollingImpulse *= maxLambda / sqrtf( magSqr ); + // } + + b3FloatW mask = b3GreaterThanW( lengthSquared, b3MulAddW( epsilonW, maxImpulse, maxImpulse ) ); + + // No approximate _mm_rsqrt_ps here to maintain cross-platform determinism + b3FloatW normalize = b3DivW( maxImpulse, b3AddW( b3SqrtW( lengthSquared ), epsilonW ) ); + b3FloatW scale = b3BlendW( oneW, normalize, mask ); + + // Ensure zero rolling resistance yields no impulse + b3FloatW rollingMask = b3GreaterThanW( c->rollingResistance, b3ZeroW() ); + scale = b3BlendW( b3ZeroW(), scale, rollingMask ); + + c->rollingImpulse = b3MulSVW( scale, c->rollingImpulse ); + + deltaImpulse = b3SubVW( c->rollingImpulse, oldImpulse ); + + bA.w = b3MulSubMVW( bA.w, c->invIA, deltaImpulse ); + bB.w = b3MulAddMVW( bB.w, c->invIB, deltaImpulse ); + } + + // Central twist friction + { + b3FloatW twistSpeed = b3DotW( c->normal, b3SubVW( bB.w, bA.w ) ); + b3FloatW maxLambda = b3MulW( c->friction, totalTwistLimit ); + b3FloatW deltaImpulse = b3NegW( b3MulW( c->twistMass, twistSpeed ) ); + b3FloatW oldImpulse = c->twistImpulse; + c->twistImpulse = b3SymClampW( b3AddW( oldImpulse, deltaImpulse ), maxLambda ); + deltaImpulse = b3SubW( c->twistImpulse, oldImpulse ); + + b3Vec3W L = b3MulSVW( deltaImpulse, c->normal ); + bA.w = b3MulSubMVW( bA.w, c->invIA, L ); + bB.w = b3MulAddMVW( bB.w, c->invIB, L ); + } + + // Central friction + { + b3Vec3W tangent1 = c->tangent1; + b3Vec3W tangent2 = c->tangent2; + + // Fixed anchor points for applying impulses + b3Vec3W rA = c->originA; + b3Vec3W rB = c->originB; + + // Relative tangent velocity at contact + b3Vec3W vrA = b3AddVW( bA.v, b3CrossW( bA.w, rA ) ); + b3Vec3W vrB = b3AddVW( bB.v, b3CrossW( bB.w, rB ) ); + b3Vec3W vr = b3SubVW( vrB, vrA ); + b3Vec2W vt = { + b3SubW( b3DotW( vr, tangent1 ), c->tangentVelocity1 ), + b3SubW( b3DotW( vr, tangent2 ), c->tangentVelocity2 ), + }; + + // Incremental tangent impulse + b3Vec2W deltaImpulse = b3MulMV2W( c->tangentMass, vt ); + deltaImpulse = (b3Vec2W){ b3NegW( deltaImpulse.x ), b3NegW( deltaImpulse.y ) }; + b3Vec2W newImpulse = b3AddV2W( c->frictionImpulse, deltaImpulse ); + + b3FloatW friction = c->friction; + b3FloatW maxImpulse = b3MulW( friction, totalNormalImpulse ); + + // Clamp the accumulated impulse + b3FloatW lengthSquared = b3AddW( b3MulW( newImpulse.x, newImpulse.x ), b3MulW( newImpulse.y, newImpulse.y ) ); + + // Max impulse can be zero + b3FloatW mask = b3GreaterThanW( lengthSquared, b3MulW( maxImpulse, maxImpulse ) ); + + // No approximate _mm_rsqrt_ps here to maintain cross-platform determinism. Add epsilon to avoid divide by + // zero. + b3FloatW normalize = b3DivW( maxImpulse, b3AddW( b3SqrtW( lengthSquared ), epsilonW ) ); + b3FloatW scale = b3BlendW( oneW, normalize, mask ); + newImpulse = (b3Vec2W){ + b3MulW( scale, newImpulse.x ), + b3MulW( scale, newImpulse.y ), + }; + + deltaImpulse = (b3Vec2W){ + b3SubW( newImpulse.x, c->frictionImpulse.x ), + b3SubW( newImpulse.y, c->frictionImpulse.y ), + }; + + c->frictionImpulse = newImpulse; + + // Apply delta impulse + b3Vec3W P = b3AddVW( b3MulSVW( deltaImpulse.x, tangent1 ), b3MulSVW( deltaImpulse.y, tangent2 ) ); + bA.w = b3MulSubMVW( bA.w, c->invIA, b3CrossW( rA, P ) ); + bA.v = b3MulSubSVW( bA.v, c->invMassA, P ); + bB.w = b3MulAddMVW( bB.w, c->invIB, b3CrossW( rB, P ) ); + bB.v = b3MulAddSVW( bB.v, c->invMassB, P ); + } + } + + b3ScatterBodies( states, c->indexA, &bA ); + b3ScatterBodies( states, c->indexB, &bB ); + } + + b3TracyCZoneEnd( solve_contact ); +} + +void b3ApplyRestitution_Convex( b3SolverBlock block, b3StepContext* context ) +{ + b3TracyCZoneNC( restitution, "Restitution", b3_colorDodgerBlue, true ); + + b3BodyState* states = context->states; + b3ContactConstraintWide* constraints = context->graph->colors[block.colorIndex].wideConstraints; + b3FloatW threshold = b3SplatW( context->world->restitutionThreshold ); + b3FloatW zero = b3ZeroW(); + + for ( int i = block.startIndex; i < block.startIndex + block.count; ++i ) + { + b3ContactConstraintWide* c = constraints + i; + + if ( b3AllZeroW( c->restitution ) ) + { + // No lanes have restitution. Common case. + continue; + } + + // Single gather for all manifolds + b3BodyStateW bA = b3GatherBodies( states, c->indexA ); + b3BodyStateW bB = b3GatherBodies( states, c->indexB ); + + // Create a mask based on restitution so that lanes with no restitution are not affected + // by the calculations below. + b3FloatW restitutionMask = b3EqualsW( c->restitution, zero ); + + for ( int pointIndex = 0; pointIndex < B3_MAX_MANIFOLD_POINTS; ++pointIndex ) + { + b3ContactConstraintPointWide* cp = c->points + pointIndex; + + // Set effective mass to zero if restitution should not be applied + b3FloatW mask1 = b3GreaterThanW( b3AddW( cp->relativeVelocities, threshold ), zero ); + b3FloatW mask2 = b3EqualsW( cp->totalNormalImpulses, zero ); + b3FloatW mask = b3OrW( b3OrW( mask1, mask2 ), restitutionMask ); + b3FloatW mass = b3BlendW( cp->normalMasses, zero, mask ); + + // Fixed anchors for impulses + b3Vec3W rA = cp->anchorAs; + b3Vec3W rB = cp->anchorBs; + + // Relative velocity at contact + b3Vec3W vrA = b3AddVW( bA.v, b3CrossW( bA.w, rA ) ); + b3Vec3W vrB = b3AddVW( bB.v, b3CrossW( bB.w, rB ) ); + b3FloatW vn = b3DotW( b3SubVW( vrB, vrA ), c->normal ); + + // Compute normal impulse + b3FloatW negImpulse = b3MulW( mass, b3AddW( vn, b3MulW( c->restitution, cp->relativeVelocities ) ) ); + + // Clamp the accumulated impulse + b3FloatW newImpulse = b3MaxW( b3SubW( cp->normalImpulses, negImpulse ), b3ZeroW() ); + b3FloatW deltaImpulse = b3SubW( newImpulse, cp->normalImpulses ); + cp->normalImpulses = newImpulse; + cp->totalNormalImpulses = b3AddW( cp->totalNormalImpulses, deltaImpulse ); + + // Apply contact impulse + b3Vec3W P = b3MulSVW( deltaImpulse, c->normal ); + bA.w = b3MulSubMVW( bA.w, c->invIA, b3CrossW( rA, P ) ); + bA.v = b3MulSubSVW( bA.v, c->invMassA, P ); + bB.w = b3MulAddMVW( bB.w, c->invIB, b3CrossW( rB, P ) ); + bB.v = b3MulAddSVW( bB.v, c->invMassB, P ); + } + + b3ScatterBodies( states, c->indexA, &bA ); + b3ScatterBodies( states, c->indexB, &bB ); + } + + b3TracyCZoneEnd( restitution ); +} + +// Store impulses by contact constraint +void b3StoreImpulses_Convex( b3SolverBlock block, b3StepContext* context, int workerIndex ) +{ + b3TracyCZoneNC( store_impulses, "Store", b3_colorFireBrick, true ); + + b3World* world = context->world; + b3WidePrepareSpan* spans = context->widePrepareSpans; + const b3ContactConstraintWide* wideBase = context->wideConstraints; + b3TaskContext* taskContext = world->taskContexts.data + workerIndex; + b3BitSet* hitEventBitSet = &taskContext->hitEventBitSet; + bool hasHitEvents = taskContext->hasHitEvents; + float negHitThreshold = -world->hitEventThreshold; + + int wideIndex = block.startIndex; + int endWideIndex = block.startIndex + block.count; + + // Find color for start index + int colorIndex = 0; + while ( spans[colorIndex + 1].start <= wideIndex ) + { + colorIndex += 1; + } + + while ( wideIndex < endWideIndex ) + { + int colorWideStart = spans[colorIndex].start; + int colorWideEndIndex = b3MinInt( spans[colorIndex + 1].start, endWideIndex ); + int colorContactCount = spans[colorIndex].count; + int* contactIds = spans[colorIndex].contacts; + + for ( ; wideIndex < colorWideEndIndex; ++wideIndex ) + { + const b3ContactConstraintWide* c = wideBase + wideIndex; + const float* frictionImpulse1 = (float*)&c->frictionImpulse.x; + const float* frictionImpulse2 = (float*)&c->frictionImpulse.y; + const float* tangent1X = (float*)&c->tangent1.X; + const float* tangent1Y = (float*)&c->tangent1.Y; + const float* tangent1Z = (float*)&c->tangent1.Z; + const float* tangent2X = (float*)&c->tangent2.X; + const float* tangent2Y = (float*)&c->tangent2.Y; + const float* tangent2Z = (float*)&c->tangent2.Z; + const float* twistImpulse = (float*)&c->twistImpulse; + const float* rollingImpulseX = (float*)&c->rollingImpulse.X; + const float* rollingImpulseY = (float*)&c->rollingImpulse.Y; + const float* rollingImpulseZ = (float*)&c->rollingImpulse.Z; + + int localWideIndex = wideIndex - colorWideStart; + + for ( int lane = 0; lane < B3_SIMD_WIDTH; ++lane ) + { + int contactIndex = B3_SIMD_WIDTH * localWideIndex + lane; + if ( contactIndex >= colorContactCount ) + { + break; + } + + b3Manifold* m = c->manifolds[lane]; + if ( m == NULL ) + { + continue; + } + + float f1 = frictionImpulse1[lane]; + float f2 = frictionImpulse2[lane]; + m->frictionImpulse = (b3Vec3){ + f1 * tangent1X[lane] + f2 * tangent2X[lane], + f1 * tangent1Y[lane] + f2 * tangent2Y[lane], + f1 * tangent1Z[lane] + f2 * tangent2Z[lane], + }; + m->twistImpulse = twistImpulse[lane]; + m->rollingImpulse = (b3Vec3){ + rollingImpulseX[lane], + rollingImpulseY[lane], + rollingImpulseZ[lane], + }; + + int pointCount = m->pointCount; + for ( int pointIndex = 0; pointIndex < pointCount; ++pointIndex ) + { + const b3ContactConstraintPointWide* cp = c->points + pointIndex; + const float* normalImpulse = (float*)&cp->normalImpulses; + const float* totalNormalImpulse = (float*)&cp->totalNormalImpulses; + const float* normalVelocity = (float*)&cp->relativeVelocities; + + b3ManifoldPoint* mp = m->points + pointIndex; + mp->normalImpulse = normalImpulse[lane]; + mp->totalNormalImpulse = totalNormalImpulse[lane]; + mp->normalVelocity = normalVelocity[lane]; + } + + int contactId = contactIds[contactIndex]; + b3Contact* contact = b3Array_Get( world->contacts, contactId ); + if ( ( contact->flags & b3_simEnableHitEvent ) != 0 ) + { + for ( int k = 0; k < pointCount; ++k ) + { + b3ManifoldPoint* mp = m->points + k; + + // Need to check total impulse because the point may be speculative and not colliding + if ( mp->normalVelocity < negHitThreshold && mp->totalNormalImpulse > 0.0f ) + { + b3SetBit( hitEventBitSet, contact->contactId ); + hasHitEvents = true; + break; + } + } + } + } + } + + colorIndex += 1; + } + + taskContext->hasHitEvents = hasHitEvents; + + b3TracyCZoneEnd( store_impulses ); +} + +void b3PrepareContacts_Overflow( b3StepContext* context ) +{ + b3ConstraintGraph* graph = context->graph; + b3GraphColor* color = graph->colors + B3_OVERFLOW_INDEX; + + uint16_t count = (uint16_t)color->contacts.count; + if (count == 0) + { + return; + } + + b3SolverBlock block = { + .startIndex = 0, + .count = count, + .blockType = b3_overflowBlock, + .colorIndex = B3_OVERFLOW_INDEX, + }; + + b3PrepareContacts_Mesh( block, context ); +} + +void b3WarmStartContacts_Overflow( b3StepContext* context ) +{ + b3ConstraintGraph* graph = context->graph; + b3GraphColor* color = graph->colors + B3_OVERFLOW_INDEX; + + uint16_t count = (uint16_t)color->contacts.count; + if ( count == 0 ) + { + return; + } + + b3SolverBlock block = { + .startIndex = 0, + .count = count, + .blockType = b3_overflowBlock, + .colorIndex = B3_OVERFLOW_INDEX, + }; + + b3WarmStartContacts_Mesh( block, context ); +} + +void b3SolveContacts_Overflow( b3StepContext* context, bool useBias ) +{ + b3ConstraintGraph* graph = context->graph; + b3GraphColor* color = graph->colors + B3_OVERFLOW_INDEX; + + uint16_t count = (uint16_t)color->contacts.count; + if ( count == 0 ) + { + return; + } + + b3SolverBlock block = { + .startIndex = 0, + .count = count, + .blockType = b3_overflowBlock, + .colorIndex = B3_OVERFLOW_INDEX, + }; + + b3SolveContacts_Mesh( block, context, useBias ); +} + +void b3ApplyRestitution_Overflow( b3StepContext* context ) +{ + b3ConstraintGraph* graph = context->graph; + b3GraphColor* color = graph->colors + B3_OVERFLOW_INDEX; + + uint16_t count = (uint16_t)color->contacts.count; + if ( count == 0 ) + { + return; + } + + b3SolverBlock block = { + .startIndex = 0, + .count = count, + .blockType = b3_overflowBlock, + .colorIndex = B3_OVERFLOW_INDEX, + }; + + b3ApplyRestitution_Mesh( block, context ); +} + +void b3StoreImpulses_Overflow( b3StepContext* context ) +{ + b3ConstraintGraph* graph = context->graph; + b3GraphColor* color = graph->colors + B3_OVERFLOW_INDEX; + + uint16_t count = (uint16_t)color->contacts.count; + if ( count == 0 ) + { + return; + } + + b3SolverBlock block = { + .startIndex = 0, + .count = count, + .blockType = b3_overflowBlock, + .colorIndex = B3_OVERFLOW_INDEX, + }; + + b3StoreImpulses_Mesh( block, context, 0 ); +} diff --git a/vendor/box3d/src/src/contact_solver.h b/vendor/box3d/src/src/contact_solver.h new file mode 100644 index 000000000..fab17ee53 --- /dev/null +++ b/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 ); diff --git a/vendor/box3d/src/src/container.h b/vendor/box3d/src/src/container.h new file mode 100644 index 000000000..2831cb6ff --- /dev/null +++ b/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 +#include + +#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 ); diff --git a/vendor/box3d/src/src/convex_manifold.c b/vendor/box3d/src/src/convex_manifold.c new file mode 100644 index 000000000..2422567c4 --- /dev/null +++ b/vendor/box3d/src/src/convex_manifold.c @@ -0,0 +1,1600 @@ +// SPDX-FileCopyrightText: 2025 Erin Catto +// SPDX-License-Identifier: MIT + +#include "algorithm.h" +#include "manifold.h" +#include "shape.h" + +#include "box3d/base.h" +#include "box3d/collision.h" +#include "box3d/constants.h" + +#include +#include + +static inline bool b3IsMinkowskiFaceIsolated( b3Vec3 a, b3Vec3 b, b3Vec3 n ) +{ + // An isolated edge (e.g. like in a capsule) defines a circle through the + // origin on the Gauss map. So testing for overlap between this circle and + // the arc AB simplifies to a simple plane test. + float an = b3Dot( a, n ); + float bn = b3Dot( b, n ); + + return an * bn <= 0.0f; +} + +// bxa = cross(b, a) and dxc = cross(d, c) +// but in practice we use the edge vector between the faces for robustness +static inline bool b3IsMinkowskiFace( b3Vec3 a, b3Vec3 b, b3Vec3 bxa, b3Vec3 c, b3Vec3 d, b3Vec3 dxc ) +{ + // Two edges build a face on the Minkowski sum if the associated arcs ab and cd intersect on the Gauss map. + // The associated arcs are defined by the adjacent face normals of each edge. + float cba = b3Dot( c, bxa ); + float dba = b3Dot( d, bxa ); + float adc = b3Dot( a, dxc ); + float bdc = b3Dot( b, dxc ); + + return cba * dba < 0.0f && adc * bdc < 0.0f && cba * bdc > 0.0f; +} + +static int b3ClipSegment( b3ClipVertex segment[2], b3Plane plane ) +{ + int vertexCount = 0; + b3ClipVertex vertex1 = segment[0]; + b3ClipVertex vertex2 = segment[1]; + + float distance1 = b3PlaneSeparation( plane, vertex1.position ); + float distance2 = b3PlaneSeparation( plane, vertex2.position ); + + // If the points are behind the plane + if ( distance1 <= 0.0f ) + { + segment[vertexCount++] = vertex1; + } + if ( distance2 <= 0.0f ) + { + segment[vertexCount++] = vertex2; + } + + // If the points are on different sides of the plane + if ( distance1 * distance2 < 0.0f ) + { + // Find intersection point of edge and plane + float t = distance1 / ( distance1 - distance2 ); + segment[vertexCount].position = b3Add( b3MulSV( 1.0f - t, vertex1.position ), b3MulSV( t, vertex2.position ) ); + segment[vertexCount].pair = distance1 > 0.0f ? vertex1.pair : vertex2.pair; + vertexCount++; + } + + return vertexCount; +} + +static int b3ClipSegmentToHullFace( b3ClipVertex segment[2], const b3HullData* hull, int refFace ) +{ + const b3HullFace* faces = b3GetHullFaces( hull ); + const b3Plane* planes = b3GetHullPlanes( hull ); + const b3HullHalfEdge* edges = b3GetHullEdges( hull ); + const b3Vec3* points = b3GetHullPoints( hull ); + + b3Plane refPlane = planes[refFace]; + + const b3HullFace* face = faces + refFace; + + int edgeIndex = face->edge; + + do + { + const b3HullHalfEdge* edge = edges + edgeIndex; + int nextEdgeIndex = edge->next; + const b3HullHalfEdge* next = edges + nextEdgeIndex; + + b3Vec3 vertex1 = points[edge->origin]; + b3Vec3 vertex2 = points[next->origin]; + b3Vec3 tangent = b3Normalize( b3Sub( vertex2, vertex1 ) ); + b3Vec3 binormal = b3Cross( tangent, refPlane.normal ); + + int pointCount = b3ClipSegment( segment, b3MakePlaneFromNormalAndPoint( binormal, vertex1 ) ); + if ( pointCount < 2 ) + { + return 0; + } + + edgeIndex = nextEdgeIndex; + } + while ( edgeIndex != face->edge ); + + return 2; +} + +static b3FaceQuery b3QueryFaceDirectionHullAndCapsule( const b3HullData* hull, const b3Capsule* capsule, + b3Transform capsuleTransform ) +{ + int maxFaceIndex = -1; + int maxVertexIndex = -1; + float maxFaceSeparation = -FLT_MAX; + const b3Plane* planes = b3GetHullPlanes( hull ); + + b3Vec3 capsulePoints[2] = { + b3TransformPoint( capsuleTransform, capsule->center1 ), + b3TransformPoint( capsuleTransform, capsule->center2 ), + }; + + for ( int faceIndex = 0; faceIndex < hull->faceCount; ++faceIndex ) + { + b3Plane plane = planes[faceIndex]; + + int vertexIndex = b3GetPointSupport( capsulePoints, 2, b3Neg( plane.normal ) ); + b3Vec3 support = capsulePoints[vertexIndex]; + float separation = b3PlaneSeparation( plane, support ); + if ( separation > maxFaceSeparation ) + { + maxVertexIndex = vertexIndex; + maxFaceIndex = faceIndex; + maxFaceSeparation = separation; + } + } + + return (b3FaceQuery){ + .separation = maxFaceSeparation, + .faceIndex = (uint8_t)maxFaceIndex, + .vertexIndex = (uint8_t)maxVertexIndex, + }; +} + +static b3FaceQuery b3QueryFaceDirections( const b3HullData* hullA, const b3HullData* hullB, b3Transform relativeTransform ) +{ + // We perform all computations in local space of the second hull + b3Transform transform = b3InvertTransform( relativeTransform ); + const b3Plane* planesA = b3GetHullPlanes( hullA ); + const b3Vec3* pointsB = b3GetHullPoints( hullB ); + + int maxFaceIndex = -1; + int maxVertexIndex = -1; + float maxFaceSeparation = -FLT_MAX; + + for ( int faceIndex = 0; faceIndex < hullA->faceCount; ++faceIndex ) + { + b3Plane plane = b3TransformPlane( transform, planesA[faceIndex] ); + + int vertexIndex = b3FindHullSupportVertex( hullB, b3Neg( plane.normal ) ); + b3Vec3 support = pointsB[vertexIndex]; + float separation = b3PlaneSeparation( plane, support ); + if ( separation > maxFaceSeparation ) + { + maxFaceIndex = faceIndex; + maxVertexIndex = vertexIndex; + maxFaceSeparation = separation; + } + } + + return (b3FaceQuery){ + .separation = maxFaceSeparation, + .faceIndex = (uint8_t)maxFaceIndex, + .vertexIndex = (uint8_t)maxVertexIndex, + }; +} + +static b3EdgeQuery b3QueryEdgeDirectionHullAndCapsule( const b3HullData* hull, const b3Capsule* capsule, + b3Transform capsuleTransform ) +{ + // Find axis of minimum penetration + float maxSeparation = -FLT_MAX; + int maxIndex1 = -1; + int maxIndex2 = -1; + + // We perform all computations in local space of the hull + b3Vec3 p1 = b3TransformPoint( capsuleTransform, capsule->center1 ); + b3Vec3 q1 = b3TransformPoint( capsuleTransform, capsule->center2 ); + b3Vec3 e1 = b3Sub( q1, p1 ); + + const b3HullHalfEdge* edges = b3GetHullEdges( hull ); + const b3Vec3* points = b3GetHullPoints( hull ); + const b3Plane* planes = b3GetHullPlanes( hull ); + + for ( int index = 0; index < hull->edgeCount; index += 2 ) + { + const b3HullHalfEdge* edge = edges + index; + const b3HullHalfEdge* twin = edges + index + 1; + B3_ASSERT( edge->twin == index + 1 && twin->twin == index ); + + b3Vec3 p2 = points[edge->origin]; + b3Vec3 q2 = points[twin->origin]; + b3Vec3 e2 = b3Sub( q2, p2 ); + + b3Vec3 u2 = planes[edge->face].normal; + b3Vec3 v2 = planes[twin->face].normal; + + if ( b3IsMinkowskiFaceIsolated( u2, v2, e1 ) ) + { + // We can pass any point on the edge and choose + // the edge centers for better numerical precision. + b3Vec3 c1 = b3MulSV( 0.5f, b3Add( q1, p1 ) ); + b3Vec3 c2 = hull->center; + float separation = b3EdgeEdgeSeparation( q1, e1, c1, q2, e2, c2 ); + if ( separation > maxSeparation ) + { + // Note: We don't exit early if we find a separating axis here since we want to + // find the best one for caching and account for the convex radius later. + maxSeparation = separation; + maxIndex1 = 0; + maxIndex2 = index; + } + } + } + + // Save result + return (b3EdgeQuery){ + .separation = maxSeparation, + .indexA = (uint8_t)maxIndex1, + .indexB = (uint8_t)maxIndex2, + }; +} + +static b3EdgeQuery b3QueryEdgeDirections( const b3HullData* hullA, const b3HullData* hullB, b3Transform transformBtoA ) +{ + // Find axis of minimum penetration + float maxSeparation = -FLT_MAX; + int maxIndexA = B3_NULL_INDEX; + int maxIndexB = B3_NULL_INDEX; + + const b3HullHalfEdge* edgesA = b3GetHullEdges( hullA ); + const b3Vec3* pointsA = b3GetHullPoints( hullA ); + const b3Plane* planesA = b3GetHullPlanes( hullA ); + const b3HullHalfEdge* edgesB = b3GetHullEdges( hullB ); + const b3Vec3* pointsB = b3GetHullPoints( hullB ); + const b3Plane* planesB = b3GetHullPlanes( hullB ); + + // Work in frame A + b3Matrix3 matrix = b3MakeMatrixFromQuat( transformBtoA.q ); + + // Arranged to minimize transform operations + for ( int indexB = 0; indexB < hullB->edgeCount; indexB += 2 ) + { + const b3HullHalfEdge* edgeB = edgesB + indexB; + const b3HullHalfEdge* twinB = edgesB + indexB + 1; + B3_ASSERT( edgeB->twin == indexB + 1 && twinB->twin == indexB ); + + b3Vec3 qB = pointsB[twinB->origin]; + b3Vec3 eB = b3MulMV( matrix, b3Sub( qB, pointsB[edgeB->origin] ) ); + qB = b3Add( b3MulMV( matrix, qB ), transformBtoA.p ); + + b3Vec3 uB = b3MulMV( matrix, planesB[edgeB->face].normal ); + b3Vec3 vB = b3MulMV( matrix, planesB[twinB->face].normal ); + + for ( int indexA = 0; indexA < hullA->edgeCount; indexA += 2 ) + { + const b3HullHalfEdge* edgeA = edgesA + indexA; + const b3HullHalfEdge* twinA = edgesA + indexA + 1; + B3_ASSERT( edgeA->twin == indexA + 1 && twinA->twin == indexA ); + + b3Vec3 qA = pointsA[twinA->origin]; + b3Vec3 eA = b3Sub( qA, pointsA[edgeA->origin] ); + b3Vec3 uA = planesA[edgeA->face].normal; + b3Vec3 vA = planesA[twinA->face].normal; + + bool isMinkowski; + { + // Two edges build a face on the Minkowski sum if the associated arcs AB and CD intersect on the Gauss map. + // The associated arcs are defined by the adjacent face normals of each edge. + float cba = b3Dot( uB, eA ); + float dba = b3Dot( vB, eA ); + float adc = -b3Dot( uA, eB ); + float bdc = -b3Dot( vA, eB ); + + isMinkowski = cba * dba < 0.0f && adc * bdc < 0.0f && cba * bdc > 0.0f; + } + + if ( isMinkowski ) + { + b3Vec3 centerA = hullA->center; + b3Vec3 centerB = b3TransformPoint( transformBtoA, hullB->center ); + float separation = b3EdgeEdgeSeparation( qA, eA, centerA, qB, eB, centerB ); + + if ( separation > maxSeparation ) + { + // Continues to find the maximum separating axis + maxSeparation = separation; + maxIndexA = indexA; + maxIndexB = indexB; + } + } + } + } + + return (b3EdgeQuery){ + .separation = maxSeparation, + .indexA = maxIndexA, + .indexB = maxIndexB, + }; +} + +// Reduce the manifold points to a maximum of 4 points. +// Note: this modifies the input point array to improve performance +static void b3ReduceManifoldPoints( b3LocalManifold* manifold, int capacity, b3LocalManifoldPoint* points, int count ) +{ + if ( capacity < 4 ) + { + return; + } + + if ( count <= 4 ) + { + for ( int i = 0; i < count; ++i ) + { + manifold->points[i] = points[i]; + } + + manifold->pointCount = count; + return; + } + + b3Vec3 normal = manifold->normal; + // float linearSlop = B3_LINEAR_SLOP; + float speculativeDistance = B3_SPECULATIVE_DISTANCE; + float tolSqr = speculativeDistance * speculativeDistance; + + // This bias is very important for contact point consistency across time steps. + // It creates a pecking order to avoid flickering between candidates with similar scores. + float bias = 0.95f; + + // Step 1: find extreme point that is touching + int bestIndex = B3_NULL_INDEX; + float bestScore = -FLT_MAX; + + // Arbitrary tangent direction + // b3Vec3 perp1 = b3Perp( normal ); + // b3Vec3 perp2 = b3Cross( perp1, normal ); + // b3Vec3 searchDirection = -0.4535961214255773f * perp1 + 0.8912073600614354f * perp2; + b3Vec3 searchDirection = b3ArbitraryPerp( normal ); + for ( int index = 0; index < count; ++index ) + { + b3LocalManifoldPoint* pt = points + index; + + if ( pt->separation > speculativeDistance ) + { + continue; + } + + // The deeper the better + float score = -pt->separation + b3Dot( searchDirection, pt->point ); + if ( bias * score > bestScore ) + { + bestIndex = index; + bestScore = score; + } + } + + B3_VALIDATE( 0 <= bestIndex && bestIndex < count ); + if ( bestIndex == B3_NULL_INDEX ) + { + manifold->pointCount = 0; + return; + } + + manifold->points[0] = points[bestIndex]; + manifold->pointCount = 1; + + // Remove best point from array + points[bestIndex] = points[count - 1]; + count -= 1; + + b3Vec3 a = manifold->points[0].point; + + // Step 2: Find farthest point in 2D + bestScore = 0.0f; + bestIndex = B3_NULL_INDEX; + float maxDistanceSquared = 0.0f; + + for ( int index = 0; index < count; ++index ) + { + b3Vec3 p = points[index].point; + b3Vec3 d = b3Sub( p, a ); + b3Vec3 v = b3MulSub( d, b3Dot( d, normal ), normal ); + float distanceSquared = b3LengthSquared( v ); + maxDistanceSquared = b3MaxFloat( maxDistanceSquared, distanceSquared ); + float separation = b3MaxFloat( 0.0f, -points[index].separation ); + float score = distanceSquared + 4.0f * separation * separation; + if ( bias * score > bestScore ) + { + bestScore = score; + bestIndex = index; + } + } + + if ( bestScore < tolSqr ) + { + return; + } + + B3_ASSERT( 0 <= bestIndex && bestIndex < count ); + manifold->points[1] = points[bestIndex]; + manifold->pointCount = 2; + + // Remove best point from array + points[bestIndex] = points[count - 1]; + count -= 1; + + b3Vec3 b = manifold->points[1].point; + + // Step 3: Find the point with the maximum triangular area + bestScore = tolSqr; + bestIndex = B3_NULL_INDEX; + float bestSignedArea = 0.0f; + b3Vec3 ba = b3Sub( b, a ); + for ( int index = 0; index < count; ++index ) + { + b3Vec3 p = points[index].point; + float signedArea = b3Dot( normal, b3Cross( ba, b3Sub( p, a ) ) ); + float score = b3AbsFloat( signedArea ); + if ( bias * score >= bestScore ) + { + bestScore = score; + bestIndex = index; + bestSignedArea = signedArea; + } + } + + if ( bestIndex == B3_NULL_INDEX ) + { + return; + } + + B3_ASSERT( bestIndex != B3_NULL_INDEX ); + + manifold->points[2] = points[bestIndex]; + manifold->pointCount = 3; + points[bestIndex] = points[count - 1]; + count -= 1; + + b3Vec3 c = manifold->points[2].point; + + // Step 4: get the point that adds the most area outside the current triangle + bestScore = tolSqr; + bestIndex = B3_NULL_INDEX; + float sign = bestSignedArea < 0.0f ? -1.0f : 1.0f; + for ( int index = 0; index < count; ++index ) + { + b3Vec3 p = points[index].point; + float u1 = sign * b3Dot( normal, b3Cross( b3Sub( p, a ), ba ) ); + float u2 = sign * b3Dot( normal, b3Cross( b3Sub( p, b ), b3Sub( c, b ) ) ); + float u3 = sign * b3Dot( normal, b3Cross( b3Sub( p, c ), b3Sub( a, c ) ) ); + float score = b3MaxFloat( u1, b3MaxFloat( u2, u3 ) ); + + if ( bias * score > bestScore ) + { + bestScore = score; + bestIndex = index; + } + } + + if ( bestIndex != B3_NULL_INDEX ) + { + manifold->points[manifold->pointCount] = points[bestIndex]; + manifold->pointCount += 1; + } +} + +void b3CollideSpheres( b3LocalManifold* manifold, int capacity, const b3Sphere* sphereA, const b3Sphere* sphereB, + b3Transform transformBtoA ) +{ + if ( capacity == 0 ) + { + return; + } + + // Work in shapeB coordinates + b3Vec3 center1 = sphereA->center; + b3Vec3 center2 = b3TransformPoint( transformBtoA, sphereB->center ); + + float totalRadius = sphereA->radius + sphereB->radius; + b3Vec3 offset = b3Sub( center2, center1 ); + float distanceSq = b3LengthSquared( offset ); + + if ( distanceSq > totalRadius * totalRadius ) + { + // We found a separating axis + return; + } + + b3Vec3 normal = { 0.0f, 1.0f, 0.0f }; + float distance = sqrtf( distanceSq ); + if ( distance * distance > 1000.0f * FLT_MIN ) + { + normal = b3MulSV( 1.0f / distance, offset ); + } + + // Contact at the midpoint + // 0.5 * ( ((c1 + rA*n) + c2) - rB*n ) + b3Vec3 point = + b3MulSV( 0.5f, b3MulSub( b3Add( b3MulAdd( center1, sphereA->radius, normal ), center2 ), sphereB->radius, normal ) ); + + // Manifold in frame B + manifold->normal = normal; + manifold->pointCount = 1; + + b3LocalManifoldPoint* pt = manifold->points + 0; + pt->point = point; + pt->separation = distance - totalRadius; + pt->pair = b3FeaturePair_single; +} + +void b3CollideCapsuleAndSphere( b3LocalManifold* manifold, int capacity, const b3Capsule* capsuleA, const b3Sphere* sphereB, + b3Transform transformBtoA ) +{ + manifold->pointCount = 0; + + if ( capacity < 1 ) + { + return; + } + + // Work in shape B coordinates + b3Vec3 center = b3TransformPoint( transformBtoA, sphereB->center ); + b3Vec3 center1 = capsuleA->center1; + b3Vec3 center2 = capsuleA->center2; + + float totalRadius = sphereB->radius + capsuleA->radius; + + b3Vec3 closestPoint = b3PointToSegmentDistance( center1, center2, center ); + b3Vec3 offset = b3Sub( center, closestPoint ); + float distanceSq = b3LengthSquared( offset ); + + if ( distanceSq > totalRadius * totalRadius ) + { + // We found a separating axis + return; + } + + b3Vec3 normal = { 0.0f, 1.0f, 0.0f }; + float distance = sqrtf( distanceSq ); + if ( distance * distance > 1000.0f * FLT_MIN ) + { + normal = b3MulSV( 1.0f / distance, offset ); + } + + // Contact at the midpoint + // 0.5 * (((center - sB*n) + closestPoint) + cA*n) + b3Vec3 point = + b3MulSV( 0.5f, b3MulAdd( b3Add( b3MulSub( center, sphereB->radius, normal ), closestPoint ), capsuleA->radius, normal ) ); + + // Manifold in frame B + manifold->normal = normal; + manifold->pointCount = 1; + + b3LocalManifoldPoint* pt = manifold->points + 0; + pt->point = point; + pt->separation = distance - totalRadius; + pt->pair = b3FeaturePair_single; +} + +void b3CollideHullAndSphere( b3LocalManifold* manifold, int capacity, const b3HullData* hullA, const b3Sphere* sphereB, + b3Transform transformBtoA, b3SimplexCache* cache ) +{ + manifold->pointCount = 0; + + if ( capacity == 0 ) + { + return; + } + + b3Vec3 center = b3TransformPoint( transformBtoA, sphereB->center ); + + const float speculativeDistance = B3_SPECULATIVE_DISTANCE; + + // Work in shapeA coordinates + + b3DistanceInput distanceInput; + distanceInput.proxyA = (b3ShapeProxy){ b3GetHullPoints( hullA ), hullA->vertexCount, 0.0f }; + distanceInput.proxyB = (b3ShapeProxy){ ¢er, 1, 0.0f }; + distanceInput.transform = b3Transform_identity; + distanceInput.useRadii = false; + + float radiusA = 0.0f; + float radiusB = sphereB->radius; + float radius = radiusA + radiusB; + + b3DistanceOutput distanceOutput = b3ShapeDistance( &distanceInput, cache, NULL, 0 ); + + if ( distanceOutput.distance > radius + speculativeDistance ) + { + // We found a separating axis + *cache = (b3SimplexCache){ 0 }; + return; + } + + if ( distanceOutput.distance > 100.0f * FLT_EPSILON ) + { + // Shallow penetration + b3Vec3 normal = b3Normalize( b3Sub( distanceOutput.pointB, distanceOutput.pointA ) ); + + // cA is the projection of the sphere center onto to the hull (pointA if radiusA == 0). + b3Vec3 cA = b3MulAdd( center, radiusA - b3Dot( b3Sub( center, distanceOutput.pointA ), normal ), normal ); + + // cB is the deepest point on the sphere with respect to the reference f + b3Vec3 cB = b3MulSub( center, radiusB, normal ); + + b3Vec3 point = b3Lerp( cA, cB, 0.5f ); + + // Manifold in frame A + manifold->normal = normal; + manifold->pointCount = 1; + + b3LocalManifoldPoint* pt = manifold->points + 0; + pt->point = point; + pt->separation = distanceOutput.distance - radius; + pt->pair = b3FeaturePair_single; + } + else + { + // Deep penetration + int bestIndex = -1; + float bestDistance = -FLT_MAX; + const b3Plane* planes = b3GetHullPlanes( hullA ); + + for ( int index = 0; index < hullA->faceCount; ++index ) + { + b3Plane plane = planes[index]; + + float distance = b3PlaneSeparation( plane, center ); + if ( distance > bestDistance ) + { + bestIndex = index; + bestDistance = distance; + } + } + B3_ASSERT( bestIndex >= 0 ); + + b3Vec3 normal = planes[bestIndex].normal; + + // cA is the projection of the sphere center onto to the hull + b3Vec3 cA = b3MulAdd( center, radiusA - b3Dot( b3Sub( center, distanceOutput.pointA ), normal ), normal ); + + // cB is the deepest point on the sphere with respect to the reference f + b3Vec3 cB = b3MulSub( center, radiusB, normal ); + + b3Vec3 point = b3Lerp( cA, cB, 0.5f ); + + // Manifold in frame A + manifold->normal = normal; + manifold->pointCount = 1; + + b3LocalManifoldPoint* pt = manifold->points + 0; + pt->point = point; + pt->separation = bestDistance - radius; + pt->pair = b3FeaturePair_single; + } +} + +void b3CollideCapsules( b3LocalManifold* manifold, int capacity, const b3Capsule* capsuleA, const b3Capsule* capsuleB, + b3Transform transformBtoA ) +{ + manifold->pointCount = 0; + + if ( capacity < 2 ) + { + return; + } + + // Work in shapeA coordinates + b3Vec3 centerA1 = capsuleA->center1; + b3Vec3 centerA2 = capsuleA->center2; + b3Vec3 centerB1 = b3TransformPoint( transformBtoA, capsuleB->center1 ); + b3Vec3 centerB2 = b3TransformPoint( transformBtoA, capsuleB->center2 ); + + float radius = capsuleA->radius + capsuleB->radius; + float maxDistance = radius + B3_SPECULATIVE_DISTANCE; + + b3SegmentDistanceResult result = b3SegmentDistance( centerA1, centerA2, centerB1, centerB2 ); + b3Vec3 offset = b3Sub( result.point2, result.point1 ); + float distanceSquared = b3LengthSquared( offset ); + float linearSlop = B3_LINEAR_SLOP; + float minDistance = 0.01f * linearSlop; + + if ( distanceSquared > maxDistance * maxDistance || distanceSquared < minDistance * minDistance ) + { + // We found a separating axis + return; + } + + float lengthA; + b3Vec3 segmentA = b3Sub( centerA2, centerA1 ); + b3Vec3 edgeA = b3GetLengthAndNormalize( &lengthA, segmentA ); + if ( lengthA < B3_MIN_CAPSULE_LENGTH ) + { + return; + } + + float lengthB; + b3Vec3 segmentB = b3Sub( centerB2, centerB1 ); + b3Vec3 edgeB = b3GetLengthAndNormalize( &lengthB, segmentB ); + if ( lengthB < B3_MIN_CAPSULE_LENGTH ) + { + return; + } + + // Parallel edges: |eA x eB| = sin(alpha) + const float alphaTol = 0.05f; + const float alphaTolSqr = alphaTol * alphaTol; + b3Vec3 axis = b3Cross( edgeA, edgeB ); + + // Try to create two contact points if the capsules are nearly parallel + if ( b3LengthSquared( axis ) < alphaTolSqr ) + { + // Clip segment B against side planes of segment A + + // Sides planes of A + b3Plane planesA[2]; + planesA[0].normal = b3Neg( edgeA ); + planesA[0].offset = -b3Dot( edgeA, capsuleA->center1 ); + planesA[1].normal = edgeA; + planesA[1].offset = b3Dot( edgeA, capsuleA->center2 ); + + // Clip points for B + b3ClipVertex verticesB[2]; + verticesB[0].position = centerB1; + verticesB[0].separation = 0.0f; + verticesB[0].pair = b3MakeFeaturePair( b3_featureShapeA, 0, b3_featureShapeA, 0 ); + verticesB[1].position = centerB2; + verticesB[1].separation = 0.0f; + verticesB[1].pair = b3MakeFeaturePair( b3_featureShapeA, 1, b3_featureShapeA, 1 ); + + int pointCount = b3ClipSegment( verticesB, planesA[0] ); + if ( pointCount == 2 ) + { + pointCount = b3ClipSegment( verticesB, planesA[1] ); + } + + if ( pointCount == 2 ) + { + // Closest points on A to the clipped points on B. + b3Vec3 closestPoint1 = b3PointToSegmentDistance( centerA1, centerA2, verticesB[0].position ); + b3Vec3 closestPoint2 = b3PointToSegmentDistance( centerA1, centerA2, verticesB[1].position ); + + float distance1 = b3Distance( closestPoint1, verticesB[0].position ); + float distance2 = b3Distance( closestPoint2, verticesB[1].position ); + if ( distance1 <= radius && distance2 <= radius ) + { + if ( distance1 < minDistance || distance2 < minDistance ) + { + // Avoid divide by zero + return; + } + + b3Vec3 normal1 = b3MulSV( 1.0f / distance1, b3Sub( verticesB[0].position, closestPoint1 ) ); + b3Vec3 normal2 = b3MulSV( 1.0f / distance2, b3Sub( verticesB[1].position, closestPoint2 ) ); + b3Vec3 normal = b3Normalize( b3Add( normal1, normal2 ) ); + float radiusA = capsuleA->radius; + float radiusB = capsuleB->radius; + + // Contact is at the midpoint: 0.5 * (((vB.pos + rA*nK) + cP) - rB*n) + b3Vec3 point1 = + b3MulSV( 0.5f, b3MulSub( b3Add( b3MulAdd( verticesB[0].position, radiusA, normal1 ), closestPoint1 ), radiusB, + normal ) ); + b3Vec3 point2 = + b3MulSV( 0.5f, b3MulSub( b3Add( b3MulAdd( verticesB[1].position, radiusA, normal2 ), closestPoint2 ), radiusB, + normal ) ); + + // Manifold in frame A + manifold->normal = normal; + manifold->pointCount = 2; + + b3LocalManifoldPoint* pt1 = manifold->points + 0; + pt1->point = point1; + pt1->separation = distance1 - radius; + pt1->pair = verticesB[0].pair; + + b3LocalManifoldPoint* pt2 = manifold->points + 1; + pt2->point = point2; + pt2->separation = distance2 - radius; + pt2->pair = verticesB[1].pair; + + return; + } + } + } + + float distance; + b3Vec3 normal = b3GetLengthAndNormalize( &distance, offset ); + // Contact at the midpoint 0.5 * (((p1 + rA*n) + p2) - rB*n) + b3Vec3 point = b3MulSV( + 0.5f, b3MulSub( b3Add( b3MulAdd( result.point1, capsuleA->radius, normal ), result.point2 ), capsuleB->radius, normal ) ); + + // Manifold in frame A + manifold->normal = normal; + manifold->pointCount = 1; + + b3LocalManifoldPoint* pt = manifold->points + 0; + pt->point = point; + pt->separation = distance - radius; + pt->pair = b3FeaturePair_single; +} + +static bool b3BuildHullFaceAndCapsuleContact( b3LocalManifold* manifold, const b3HullData* hullA, const b3Capsule* capsuleB, + b3Transform transformBtoA, b3FaceQuery query ) +{ + // Work in shapeA coordinates + const b3Plane* planes = b3GetHullPlanes( hullA ); + + // Clip the capsule edge against the side planes of the reference face + int refFace = query.faceIndex; + b3Plane refPlane = planes[refFace]; + + b3ClipVertex segmentB[2]; + segmentB[0].position = b3TransformPoint( transformBtoA, capsuleB->center1 ); + segmentB[0].separation = 0.0f; + segmentB[0].pair = b3MakeFeaturePair( b3_featureShapeA, 0, b3_featureShapeA, 0 ); + segmentB[1].position = b3TransformPoint( transformBtoA, capsuleB->center2 ); + segmentB[1].separation = 0.0f; + segmentB[1].pair = b3MakeFeaturePair( b3_featureShapeA, 1, b3_featureShapeA, 1 ); + + int pointCount = b3ClipSegmentToHullFace( segmentB, hullA, refFace ); + if ( pointCount < 2 ) + { + return false; + } + + float distance1 = b3PlaneSeparation( refPlane, segmentB[0].position ); + float distance2 = b3PlaneSeparation( refPlane, segmentB[1].position ); + const float speculativeDistance = B3_SPECULATIVE_DISTANCE; + + if ( distance1 <= speculativeDistance || distance2 <= speculativeDistance ) + { + b3Vec3 normal = refPlane.normal; + b3Vec3 point1 = b3MulSub( segmentB[0].position, 0.5f * ( distance1 + capsuleB->radius ), normal ); + b3Vec3 point2 = b3MulSub( segmentB[1].position, 0.5f * ( distance2 + capsuleB->radius ), normal ); + + // Manifold in frame A + manifold->normal = normal; + manifold->pointCount = 2; + + b3LocalManifoldPoint* pt1 = manifold->points + 0; + pt1->point = point1; + pt1->separation = distance1 - capsuleB->radius; + pt1->pair = segmentB[0].pair; + + b3LocalManifoldPoint* pt2 = manifold->points + 1; + pt2->point = point2; + pt2->separation = distance2 - capsuleB->radius; + pt2->pair = segmentB[1].pair; + + return true; + } + + return false; +} + +static inline float b3DeepestPointSeparation( const b3LocalManifold* manifold ) +{ + // Deepest point + float minSeparation = FLT_MAX; + int pointCount = manifold->pointCount; + for ( int i = 0; i < pointCount; ++i ) + { + minSeparation = b3MinFloat( minSeparation, manifold->points[i].separation ); + } + + return minSeparation; +} + +static bool b3BuildHullAndCapsuleEdgeContact( b3LocalManifold* manifold, int capacity, const b3HullData* hullA, + const b3Capsule* capsuleB, b3Transform transformBtoA, b3EdgeQuery query ) +{ + if ( capacity < 1 ) + { + return false; + } + + // Work in shapeA coordinates + + b3Vec3 pc = b3TransformPoint( transformBtoA, capsuleB->center1 ); + b3Vec3 qc = b3TransformPoint( transformBtoA, capsuleB->center2 ); + b3Vec3 ec = b3Sub( qc, pc ); + + const b3HullHalfEdge* edges = b3GetHullEdges( hullA ); + const b3Vec3* points = b3GetHullPoints( hullA ); + + const b3HullHalfEdge* edge2 = edges + query.indexB; + const b3HullHalfEdge* twin2 = edges + edge2->twin; + b3Vec3 ch = hullA->center; + b3Vec3 ph = points[edge2->origin]; + b3Vec3 qh = points[twin2->origin]; + b3Vec3 eh = b3Sub( qh, ph ); + + b3Vec3 normal = b3Cross( ec, eh ); + normal = b3Normalize( normal ); + + // Normal should point outward from hull + if ( b3Dot( normal, b3Sub( ph, ch ) ) < 0.0f ) + { + normal = b3Neg( normal ); + } + + b3SegmentDistanceResult result = b3LineDistance( ph, eh, pc, ec ); + + if ( b3IsWithinSegments( &result ) == false ) + { + // closest point beyond end points + return false; + } + + b3Vec3 point = b3MulSV( 0.5f, b3Add( b3MulSub( result.point1, capsuleB->radius, normal ), result.point2 ) ); + + float separation = b3Dot( normal, b3Sub( result.point2, result.point1 ) ); + B3_VALIDATE( b3AbsFloat( separation - query.separation ) < B3_LINEAR_SLOP ); + + // Manifold in frame A + manifold->normal = normal; + manifold->pointCount = 1; + + b3LocalManifoldPoint* pt = manifold->points + 0; + pt->point = point; + pt->separation = separation - capsuleB->radius; + pt->pair = b3MakeFeaturePair( b3_featureShapeA, query.indexA, b3_featureShapeB, query.indexB ); + return true; +} + +void b3CollideHullAndCapsule( b3LocalManifold* manifold, int capacity, const b3HullData* hullA, const b3Capsule* capsuleB, + b3Transform transformBtoA, b3SimplexCache* cache ) +{ + manifold->pointCount = 0; + + if ( capacity < 2 ) + { + return; + } + + // Work in shapeA coordinates + b3DistanceInput distanceInput; + distanceInput.proxyA = (b3ShapeProxy){ b3GetHullPoints( hullA ), hullA->vertexCount, 0.0f }; + distanceInput.proxyB = (b3ShapeProxy){ &capsuleB->center1, 2, 0.0f }; + distanceInput.transform = transformBtoA; + distanceInput.useRadii = false; + + b3DistanceOutput distanceOutput = b3ShapeDistance( &distanceInput, cache, NULL, 0 ); + const float speculativeDistance = B3_SPECULATIVE_DISTANCE; + + if ( distanceOutput.distance > capsuleB->radius + speculativeDistance ) + { + // We found a separating axis + *cache = (b3SimplexCache){ 0 }; + return; + } + + if ( distanceOutput.distance > 100.0f * FLT_EPSILON ) + { + const b3Plane* planes = b3GetHullPlanes( hullA ); + + // Shallow penetration + b3Vec3 delta = distanceOutput.normal; + int refFace = b3FindHullSupportFace( hullA, delta ); + b3Plane refPlane = planes[refFace]; + + // Try to create two contact points if closest + // points difference is nearly parallel to face normal + const float kTolerance = 0.998f; + if ( b3AbsFloat( b3Dot( refPlane.normal, delta ) ) > kTolerance ) + { + // Clip capsule segment against side planes of reference face + b3ClipVertex verticesB[2]; + verticesB[0].position = b3TransformPoint( transformBtoA, capsuleB->center1 ); + verticesB[0].separation = 0.0f; + verticesB[0].pair = b3MakeFeaturePair( b3_featureShapeA, 0, b3_featureShapeA, 0 ); + verticesB[1].position = b3TransformPoint( transformBtoA, capsuleB->center2 ); + verticesB[1].separation = 0.0f; + verticesB[1].pair = b3MakeFeaturePair( b3_featureShapeA, 1, b3_featureShapeA, 1 ); + + int pointCount = b3ClipSegmentToHullFace( verticesB, hullA, refFace ); + + if ( pointCount == 2 ) + { + float distance1 = b3PlaneSeparation( refPlane, verticesB[0].position ); + float distance2 = b3PlaneSeparation( refPlane, verticesB[1].position ); + if ( distance1 <= capsuleB->radius + speculativeDistance || distance2 <= capsuleB->radius + speculativeDistance ) + { + b3Vec3 normal = refPlane.normal; + b3Vec3 point1 = b3MulSub( verticesB[0].position, 0.5f * ( capsuleB->radius + distance1 ), normal ); + b3Vec3 point2 = b3MulSub( verticesB[1].position, 0.5f * ( capsuleB->radius + distance2 ), normal ); + + // Manifold in frame A + manifold->normal = normal; + manifold->pointCount = 2; + + b3LocalManifoldPoint* pt1 = manifold->points + 0; + pt1->point = point1; + pt1->separation = distance1 - capsuleB->radius; + pt1->pair = verticesB[0].pair; + + b3LocalManifoldPoint* pt2 = manifold->points + 1; + pt2->point = point2; + pt2->separation = distance2 - capsuleB->radius; + pt2->pair = verticesB[1].pair; + + return; + } + } + } + + // Create contact from closest points + b3Vec3 point = + b3MulSV( 0.5f, b3Add( b3MulSub( distanceOutput.pointA, capsuleB->radius, delta ), distanceOutput.pointB ) ); + + // Manifold in frame A + manifold->normal = delta; + manifold->pointCount = 1; + + b3LocalManifoldPoint* pt = manifold->points + 0; + pt->point = point; + pt->separation = distanceOutput.distance - capsuleB->radius; + pt->pair = b3FeaturePair_single; + return; + } + + // Deep penetration + + b3FaceQuery faceQuery = b3QueryFaceDirectionHullAndCapsule( hullA, capsuleB, transformBtoA ); + if ( faceQuery.separation > capsuleB->radius ) + { + // We found a separating axis + return; + } + + b3EdgeQuery edgeQuery = b3QueryEdgeDirectionHullAndCapsule( hullA, capsuleB, transformBtoA ); + if ( edgeQuery.separation > capsuleB->radius ) + { + // We found a separating axis + return; + } + + // Create face contact + float faceSeparation = faceQuery.separation - capsuleB->radius; + b3BuildHullFaceAndCapsuleContact( manifold, hullA, capsuleB, transformBtoA, faceQuery ); + if ( manifold->pointCount > 1 ) + { + // If ( Out.PointCount <= 1 ) -> Compare with unclipped separation + // If ( Out.PointCount > 1 ) -> Be aggressive and compare with clipped separation + // Face contact can be empty if it does not realize the axis of minimum penetration + faceSeparation = b3DeepestPointSeparation( manifold ); + } + B3_VALIDATE( faceSeparation <= 0.0f ); + + // Face contact can be empty if it does not realize the axis of minimum penetration. + // Create edge contact if face contact fails or edge contact is significantly better! + const float kRelEdgeTolerance = 0.90f; + const float kAbsTolerance = 0.5f * B3_LINEAR_SLOP; + float edgeSeparation = edgeQuery.separation - capsuleB->radius; + if ( manifold->pointCount == 0 || edgeSeparation > kRelEdgeTolerance * faceSeparation + kAbsTolerance ) + { + // Edge contact + b3BuildHullAndCapsuleEdgeContact( manifold, capacity, hullA, capsuleB, transformBtoA, edgeQuery ); + } +} + +static int b3BuildPolygon( b3ClipVertex* out, b3Transform transform, const b3HullData* hull, int incFace, b3Plane refPlane ) +{ + const b3HullFace* faces = b3GetHullFaces( hull ); + const b3HullHalfEdge* edges = b3GetHullEdges( hull ); + const b3Vec3* points = b3GetHullPoints( hull ); + + const b3HullFace* face = faces + incFace; + int edgeIndex = face->edge; + B3_ASSERT( edges[edgeIndex].face == incFace ); + + int outCount = 0; + + b3Matrix3 matrix = b3MakeMatrixFromQuat( transform.q ); + + do + { + const b3HullHalfEdge* edge = edges + edgeIndex; + + int nextEdgeIndex = edge->next; + const b3HullHalfEdge* next = edges + nextEdgeIndex; + + b3ClipVertex vertex; + vertex.position = b3Add( b3MulMV( matrix, points[next->origin] ), transform.p ); + vertex.separation = b3PlaneSeparation( refPlane, vertex.position ); + vertex.pair = b3MakeFeaturePair( b3_featureShapeB, edgeIndex, b3_featureShapeB, nextEdgeIndex ); + + out[outCount] = vertex; + outCount += 1; + + edgeIndex = nextEdgeIndex; + } + while ( edgeIndex != face->edge && outCount < B3_MAX_CLIP_POINTS ); + + B3_VALIDATE( b3ValidatePolygon( out, outCount ) ); + + return outCount; +} + +static bool b3BuildFaceAContact( b3LocalManifold* manifold, int capacity, const b3HullData* hullA, const b3HullData* hullB, + b3Transform transformBtoA, b3FaceQuery query, b3SATCache* cache ) +{ + const b3HullFace* facesA = b3GetHullFaces( hullA ); + const b3HullHalfEdge* edgesA = b3GetHullEdges( hullA ); + const b3Plane* planesA = b3GetHullPlanes( hullA ); + const b3Vec3* pointsA = b3GetHullPoints( hullA ); + + // Reference face + int refFace = query.faceIndex; + b3Plane refPlane = planesA[refFace]; + + // Find incident face + b3Vec3 refNormalInB = b3InvRotateVector( transformBtoA.q, refPlane.normal ); + int incFace = b3FindIncidentFace( hullB, refNormalInB, query.vertexIndex ); + + // Build clip polygon from incident face in frame A + b3ClipVertex buffer1[B3_MAX_CLIP_POINTS], buffer2[B3_MAX_CLIP_POINTS]; + int pointCount = b3BuildPolygon( buffer1, transformBtoA, hullB, incFace, refPlane ); + + // Clip incident face against side planes of reference face + b3ClipVertex* input = buffer1; + b3ClipVertex* output = buffer2; + + const b3HullFace* face = facesA + refFace; + int edgeIndex = face->edge; + + do + { + const b3HullHalfEdge* edge = edgesA + edgeIndex; + int nextEdgeIndex = edge->next; + const b3HullHalfEdge* next = edgesA + nextEdgeIndex; + b3Vec3 vertex1 = pointsA[edge->origin]; + b3Vec3 vertex2 = pointsA[next->origin]; + b3Vec3 tangent = b3Normalize( b3Sub( vertex2, vertex1 ) ); + b3Vec3 binormal = b3Cross( tangent, refPlane.normal ); + + b3Plane clipPlane = b3MakePlaneFromNormalAndPoint( binormal, vertex1 ); + + pointCount = b3ClipPolygon( output, input, pointCount, clipPlane, edgeIndex, refPlane ); + B3_ASSERT( pointCount <= B3_MAX_CLIP_POINTS ); + + B3_SWAP( output, input ); + + if ( pointCount < 3 ) + { + *cache = (b3SATCache){ 0 }; + return false; + } + + edgeIndex = nextEdgeIndex; + } + while ( edgeIndex != face->edge ); + + pointCount = b3MinInt( pointCount, B3_MAX_CLIP_POINTS ); + + b3LocalManifoldPoint points[B3_MAX_CLIP_POINTS]; + float minSeparation = FLT_MAX; + + manifold->normal = refPlane.normal; + + for ( int i = 0; i < pointCount; ++i ) + { + b3ClipVertex* clipPoint = input + i; + b3LocalManifoldPoint* pt = points + i; + *pt = (b3LocalManifoldPoint){ 0 }; + + // Using the half-way point keeps the points in the same position when swapping reference face from A to B. + b3Vec3 point = b3MulSub( clipPoint->position, 0.5f * clipPoint->separation, refPlane.normal ); + + // Old way of pushing onto the reference face. + // b3Vec3 point = clipPoint->position - clipPoint->separation * refPlane.normal; + + pt->point = point; + pt->separation = clipPoint->separation; + pt->pair = clipPoint->pair; + + minSeparation = b3MinFloat( minSeparation, clipPoint->separation ); + } + + if ( minSeparation >= B3_SPECULATIVE_DISTANCE ) + { + *cache = (b3SATCache){ 0 }; + return false; + } + + b3ReduceManifoldPoints( manifold, capacity, points, pointCount ); + + // Save cache + cache->separation = minSeparation; + cache->type = (uint8_t)b3_faceAxisA; + cache->indexA = (uint8_t)query.faceIndex; + cache->indexB = (uint8_t)query.vertexIndex; + + return true; +} + +static bool b3BuildFaceBContact( b3LocalManifold* manifold, int capacity, const b3HullData* hullA, const b3HullData* hullB, + b3Transform transformBtoA, b3FaceQuery query, b3SATCache* cache ) +{ + b3Transform transformAtoB = b3InvertTransform( transformBtoA ); + bool touching = b3BuildFaceAContact( manifold, capacity, hullB, hullA, transformAtoB, query, cache ); + if ( touching == false ) + { + return false; + } + + // Results are in frame B, need to transform them into frame A + b3Matrix3 matrix = b3MakeMatrixFromQuat( transformBtoA.q ); + + // Transform and flip normal so it points from A to B, even though the B has the reference face. + manifold->normal = b3Neg( b3MulMV( matrix, manifold->normal ) ); + cache->type = (uint8_t)b3_faceAxisB; + cache->indexA = (uint8_t)query.vertexIndex; + cache->indexB = (uint8_t)query.faceIndex; + + // Transform points from frame B to frame A. + // Also flip the pairs to ensure correct matches. + for ( int i = 0; i < manifold->pointCount; ++i ) + { + b3LocalManifoldPoint* pt = manifold->points + i; + pt->point = b3Add( b3MulMV( matrix, pt->point ), transformBtoA.p ); + pt->pair = b3FlipPair( pt->pair ); + } + + return true; +} + +static bool b3BuildEdgeContact( b3LocalManifold* manifold, const b3HullData* hullA, const b3HullData* hullB, b3Transform transformBtoA, + b3EdgeQuery query, b3SATCache* cache ) +{ + // Work in shapeA coordinates + const b3HullHalfEdge* edgesA = b3GetHullEdges( hullA ); + const b3Vec3* pointsA = b3GetHullPoints( hullA ); + + const b3HullHalfEdge* edgesB = b3GetHullEdges( hullB ); + const b3Vec3* pointsB = b3GetHullPoints( hullB ); + + // B3_VALIDATE( query.separation <= 2.0f * B3_SPECULATIVE_DISTANCE ); + + const b3HullHalfEdge* edgeA = edgesA + query.indexA; + const b3HullHalfEdge* twinA = edgesA + edgeA->twin; + b3Vec3 centerA = hullA->center; + b3Vec3 pA = pointsA[edgeA->origin]; + b3Vec3 qA = pointsA[twinA->origin]; + b3Vec3 eA = b3Sub( qA, pA ); + + const b3HullHalfEdge* edgeB = edgesB + query.indexB; + const b3HullHalfEdge* twinB = edgesB + edgeB->twin; + b3Vec3 pB = b3TransformPoint( transformBtoA, pointsB[edgeB->origin] ); + b3Vec3 qB = b3TransformPoint( transformBtoA, pointsB[twinB->origin] ); + b3Vec3 eB = b3Sub( qB, pB ); + + b3Vec3 normal = b3Cross( eA, eB ); + normal = b3Normalize( normal ); + + if ( b3Dot( normal, b3Sub( pA, centerA ) ) < 0.0f ) + { + normal = b3Neg( normal ); + } + + b3SegmentDistanceResult result = b3LineDistance( pA, eA, pB, eB ); + + if ( b3IsWithinSegments( &result ) == false ) + { + *cache = (b3SATCache){ 0 }; + return false; + } + + // This can slide off the end from caching + float separation = b3Dot( normal, b3Sub( result.point2, result.point1 ) ); + + // todo I suspect this could trip if the cache becomes invalid + // B3_VALIDATE( b3AbsFloat( separation - query.separation ) < B3_LINEAR_SLOP ); + + b3Vec3 point = b3MulSV( 0.5f, b3Add( result.point1, result.point2 ) ); + + // Result in frame A + manifold->normal = normal; + manifold->pointCount = 1; + + b3LocalManifoldPoint* pt = manifold->points + 0; + pt->point = point; + pt->separation = separation; + pt->pair = b3MakeFeaturePair( b3_featureShapeA, query.indexA, b3_featureShapeB, query.indexB ); + + // Save cache + cache->separation = separation; + cache->type = (uint8_t)b3_edgePairAxis; + cache->indexA = (uint8_t)query.indexA; + cache->indexB = (uint8_t)query.indexB; + + return true; +} + +void b3CollideHulls( b3LocalManifold* manifold, int capacity, const b3HullData* hullA, const b3HullData* hullB, b3Transform transformBtoA, + b3SATCache* cache ) +{ + manifold->pointCount = 0; + + if ( capacity < 4 ) + { + return; + } + + // Work in shapeA coordinates + float speculativeDistance = B3_SPECULATIVE_DISTANCE; + + float linearSlop = B3_LINEAR_SLOP; + const b3HullHalfEdge* edgesA = b3GetHullEdges( hullA ); + const b3Plane* planesA = b3GetHullPlanes( hullA ); + const b3Vec3* pointsA = b3GetHullPoints( hullA ); + + const b3HullHalfEdge* edgesB = b3GetHullEdges( hullB ); + const b3Plane* planesB = b3GetHullPlanes( hullB ); + const b3Vec3* pointsB = b3GetHullPoints( hullB ); + + // Attempt to use the cache to speed up collision + switch ( cache->type ) + { + case b3_invalidAxis: + *cache = (b3SATCache){ 0 }; + break; + + case b3_faceAxisA: + { + B3_ASSERT( cache->indexA < hullA->faceCount ); + + // Check for separation using cached face + b3Plane plane = planesA[cache->indexA]; + b3Vec3 searchDirectionInB = b3Neg( b3InvRotateVector( transformBtoA.q, plane.normal ) ); + int vertexIndex = b3FindHullSupportVertex( hullB, searchDirectionInB ); + b3Vec3 support = b3TransformPoint( transformBtoA, pointsB[vertexIndex] ); + float separation = b3PlaneSeparation( plane, support ); + + if ( separation >= speculativeDistance ) + { + // Cache hit, shapes are separated + return; + } + + // if ( cache->separation < speculativeDistance ) + { + // Attempt face contact using cached feature + b3FaceQuery faceQuery; + faceQuery.separation = 0.0f; + faceQuery.faceIndex = cache->indexA; + faceQuery.vertexIndex = vertexIndex; + + b3SATCache localCache = { 0 }; + bool touching = b3BuildFaceAContact( manifold, capacity, hullA, hullB, transformBtoA, faceQuery, &localCache ); + if ( touching == true && b3AbsFloat( cache->separation - localCache.separation ) < linearSlop ) + { + // Cache hit, contact points generated + return; + } + } + } + break; + + case b3_faceAxisB: + { + B3_ASSERT( cache->indexB < hullB->faceCount ); + + // Check for separation using cached face + b3Plane plane = planesB[cache->indexB]; + b3Vec3 searchDirectionInA = b3Neg( b3RotateVector( transformBtoA.q, plane.normal ) ); + int vertexIndex = b3FindHullSupportVertex( hullA, searchDirectionInA ); + b3Vec3 support = b3InvTransformPoint( transformBtoA, pointsA[vertexIndex] ); + float separation = b3PlaneSeparation( plane, support ); + + if ( separation >= speculativeDistance ) + { + // Cache hit, shapes are separated + return; + } + + // if ( cache->separation < speculativeDistance ) + { + // Attempt face contact using cached feature + b3FaceQuery faceQuery; + faceQuery.separation = 0.0f; + faceQuery.faceIndex = cache->indexB; + faceQuery.vertexIndex = vertexIndex; + + b3SATCache localCache = { 0 }; + bool touching = b3BuildFaceBContact( manifold, capacity, hullA, hullB, transformBtoA, faceQuery, &localCache ); + if ( touching == true && b3AbsFloat( cache->separation - localCache.separation ) < linearSlop ) + { + // Cache hit, contact points generated + return; + } + } + } + break; + + case b3_edgePairAxis: + { + int index1 = cache->indexA; + const b3HullHalfEdge* edge1 = edgesA + index1; + const b3HullHalfEdge* twin1 = edgesA + index1 + 1; + B3_ASSERT( edge1->twin == index1 + 1 && twin1->twin == index1 ); + + b3Vec3 p1 = pointsA[edge1->origin]; + b3Vec3 q1 = pointsA[twin1->origin]; + b3Vec3 e1 = b3Sub( q1, p1 ); + + b3Vec3 u1 = planesA[edge1->face].normal; + b3Vec3 v1 = planesA[twin1->face].normal; + + int index2 = cache->indexB; + const b3HullHalfEdge* edge2 = edgesB + index2; + const b3HullHalfEdge* twin2 = edgesB + index2 + 1; + B3_ASSERT( edge2->twin == index2 + 1 && twin2->twin == index2 ); + + b3Vec3 p2 = b3TransformPoint( transformBtoA, pointsB[edge2->origin] ); + b3Vec3 q2 = b3TransformPoint( transformBtoA, pointsB[twin2->origin] ); + b3Vec3 e2 = b3Sub( q2, p2 ); + + b3Vec3 u2 = b3RotateVector( transformBtoA.q, planesB[edge2->face].normal ); + b3Vec3 v2 = b3RotateVector( transformBtoA.q, planesB[twin2->face].normal ); + + // flipping the signs of u2 and v2 + // cross(v2, u2) == cross(-v2, -u2) + // so we still use -e2 + // but we can also use e1 = cross(u1, v1) and e2 = cross(u2, v2) + bool isMinkowski = b3IsMinkowskiFace( u1, v1, e1, b3Neg( u2 ), b3Neg( v2 ), e2 ); + if ( isMinkowski == true ) + { + // Transform reference center of the first hull into local space of the second hull + b3Vec3 c1 = hullA->center; + b3Vec3 c2 = b3TransformPoint( transformBtoA, hullB->center ); + + float separation = b3EdgeEdgeSeparation( p1, e1, c1, p2, e2, c2 ); + if ( separation > speculativeDistance ) + { + // Cache hit, shapes are separated + return; + } + + // if ( cache->separation <= speculativeDistance ) + { + // Try to rebuild contact from last features + b3EdgeQuery edgeQuery; + edgeQuery.indexA = cache->indexA; + edgeQuery.indexB = cache->indexB; + edgeQuery.separation = 0.0f; + + b3SATCache localCache = { 0 }; + bool touching = b3BuildEdgeContact( manifold, hullA, hullB, transformBtoA, edgeQuery, &localCache ); + if ( touching && b3AbsFloat( cache->separation - localCache.separation ) < linearSlop ) + { + // Cache hit, contact point generated + return; + } + } + } + } + break; + + // This case is for testing + case b3_manualFaceAxisA: + { + b3FaceQuery faceQueryA = b3QueryFaceDirections( hullA, hullB, transformBtoA ); + b3BuildFaceAContact( manifold, capacity, hullA, hullB, transformBtoA, faceQueryA, cache ); + return; + } + + // This case is for testing + case b3_manualFaceAxisB: + { + b3FaceQuery faceQueryB = b3QueryFaceDirections( hullB, hullA, b3InvertTransform( transformBtoA ) ); + b3BuildFaceBContact( manifold, capacity, hullA, hullB, transformBtoA, faceQueryB, cache ); + return; + } + + // This case is for testing + case b3_manualEdgePairAxis: + { + b3EdgeQuery edgeQuery = b3QueryEdgeDirections( hullA, hullB, transformBtoA ); + if ( edgeQuery.indexA != B3_NULL_INDEX ) + { + b3BuildEdgeContact( manifold, hullA, hullB, transformBtoA, edgeQuery, cache ); + } + return; + } + + default: + B3_ASSERT( false ); + break; + } + + manifold->pointCount = 0; + *cache = (b3SATCache){ 0 }; + + // Find axis of minimum penetration + b3FaceQuery faceQueryA = b3QueryFaceDirections( hullA, hullB, transformBtoA ); + if ( faceQueryA.separation > speculativeDistance ) + { + B3_ASSERT( faceQueryA.faceIndex < hullA->faceCount ); + B3_ASSERT( faceQueryA.vertexIndex < hullB->vertexCount ); + + // We found a separating axis + cache->separation = faceQueryA.separation; + cache->type = (uint8_t)b3_faceAxisA; + cache->indexA = (uint8_t)faceQueryA.faceIndex; + cache->indexB = (uint8_t)faceQueryA.vertexIndex; + return; + } + + b3FaceQuery faceQueryB = b3QueryFaceDirections( hullB, hullA, b3InvertTransform( transformBtoA ) ); + if ( faceQueryB.separation > speculativeDistance ) + { + B3_ASSERT( faceQueryB.faceIndex < hullB->faceCount ); + B3_ASSERT( faceQueryB.vertexIndex < hullA->vertexCount ); + + // We found a separating axis + cache->separation = faceQueryB.separation; + cache->type = (uint8_t)b3_faceAxisB; + cache->indexA = (uint8_t)faceQueryB.vertexIndex; + cache->indexB = (uint8_t)faceQueryB.faceIndex; + return; + } + + b3EdgeQuery edgeQuery = b3QueryEdgeDirections( hullA, hullB, transformBtoA ); + if ( edgeQuery.separation > speculativeDistance ) + { + // We found a separating axis + cache->separation = edgeQuery.separation; + cache->type = (uint8_t)b3_edgePairAxis; + cache->indexA = (uint8_t)edgeQuery.indexA; + cache->indexB = (uint8_t)edgeQuery.indexB; + return; + } + + // Always build a face contact (e.g. Jenga problem) + float faceSeparationA = faceQueryA.separation; + float faceSeparationB = faceQueryB.separation; + B3_VALIDATE( faceSeparationA <= speculativeDistance && faceSeparationB <= speculativeDistance ); + + if ( faceSeparationB > faceSeparationA + 0.5f * linearSlop ) + { + // Face contact B + b3BuildFaceBContact( manifold, capacity, hullA, hullB, transformBtoA, faceQueryB, cache ); + } + else + { + // Face contact A + b3BuildFaceAContact( manifold, capacity, hullA, hullB, transformBtoA, faceQueryA, cache ); + } + + if ( edgeQuery.indexA == B3_NULL_INDEX ) + { + // There are no valid edge pairs (all edges parallel) + return; + } + + float clippedFaceSeparation = cache->separation; + + B3_VALIDATE( edgeQuery.separation <= speculativeDistance ); + + // todo get rid of relative tolerance + const float kRelEdgeTolerance = 0.90f; + // const float kRelFaceTolerance = 0.98f; + const float kAbsTolerance = 0.5f * linearSlop; + + // Face contact can be empty if it does not realize the axis of minimum penetration. + // Create edge contact if face contact fails or edge contact is significantly better! + if ( manifold->pointCount == 0 || edgeQuery.separation > kRelEdgeTolerance * clippedFaceSeparation + kAbsTolerance ) + { + // Edge contact + b3LocalManifold edgeManifold = { 0 }; + b3LocalManifoldPoint edgePoint = { 0 }; + edgeManifold.points = &edgePoint; + + b3BuildEdgeContact( &edgeManifold, hullA, hullB, transformBtoA, edgeQuery, cache ); + + // It is possible with speculation to have vertex-vertex collision that is missed by SAT. + // todo I doubt this backup scheme matters because I'm using the clipped face separation. + // B3_VALIDATE( edgeManifold.pointCount == 1 ); + + if ( edgeManifold.pointCount == 1 ) + { + // Copy edge manifold out, being careful to preserve manifold point buffer. + b3LocalManifoldPoint* points = manifold->points; + *manifold = edgeManifold; + manifold->points = points; + manifold->points[0] = edgePoint; + } + } +} diff --git a/vendor/box3d/src/src/core.c b/vendor/box3d/src/src/core.c new file mode 100644 index 000000000..6747594b5 --- /dev/null +++ b/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 +#include +#else +#include +#endif + +#include "core.h" + +#include "box3d/constants.h" +#include "box3d/math_functions.h" + +#include +#include + +#ifdef BOX2D_PROFILE + +#include +#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 + +// 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 ); + } +} diff --git a/vendor/box3d/src/src/core.h b/vendor/box3d/src/src/core.h new file mode 100644 index 000000000..3aaf196b1 --- /dev/null +++ b/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 + +// 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 + #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 + #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 ); diff --git a/vendor/box3d/src/src/ctz.h b/vendor/box3d/src/src/ctz.h new file mode 100644 index 000000000..483ce4bec --- /dev/null +++ b/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 +#include + +#if defined( _MSC_VER ) && !defined( __clang__ ) + +#include + +// 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; +} diff --git a/vendor/box3d/src/src/distance.c b/vendor/box3d/src/src/distance.c new file mode 100644 index 000000000..3d558ab34 --- /dev/null +++ b/vendor/box3d/src/src/distance.c @@ -0,0 +1,1904 @@ +// SPDX-FileCopyrightText: 2026 Erin Catto +// SPDX-License-Identifier: MIT + +// Dirk Gregorius contributed portions of this code + +#include "math_internal.h" + +#include "box3d/collision.h" +#include "box3d/constants.h" + +#define B3_MAX_SIMPLEX_VERTICES 4 +#define B3_MAX_GJK_ITERATIONS 32 + +int b3GetProxySupport( const b3ShapeProxy* proxy, b3Vec3 axis ) +{ + int count = proxy->count; + const b3Vec3* points = proxy->points; + + B3_ASSERT( count > 0 ); + B3_ASSERT( points != NULL ); + + // We move the first vertex into the origin for improved precision. + // This is necessary since we don't have shape transforms and + // vertices can potentially be far away from the origin (large). + b3Vec3 origin = points[0]; + int maxIndex = 0; + float maxProjection = 0.0f; + + for ( int index = 1; index < count; ++index ) + { + // We subtract the first vertex since we are shifting into the origin. + float projection = b3Dot( axis, b3Sub( points[index], origin ) ); + if ( projection > maxProjection ) + { + maxIndex = index; + maxProjection = projection; + } + } + + return maxIndex; +} + +int b3GetPointSupport( const b3Vec3* points, int count, b3Vec3 axis ) +{ + B3_ASSERT( count > 0 ); + B3_ASSERT( points != NULL ); + + // We move the first vertex into the origin for improved precision. + // This is necessary since we don't have shape transforms and + // vertices can potentially be far away from the origin (large). + b3Vec3 origin = points[0]; + int maxIndex = 0; + float maxProjection = 0.0f; + + for ( int index = 1; index < count; ++index ) + { + // We subtract the first vertex since we are shifting into the origin. + float projection = b3Dot( axis, b3Sub( points[index], origin ) ); + if ( projection > maxProjection ) + { + maxIndex = index; + maxProjection = projection; + } + } + + return maxIndex; +} + +static void b3BarycentricCoordsEdge( float out[3], b3Vec3 a, b3Vec3 b ) +{ + b3Vec3 ab = b3Sub( b, a ); + + // Last element is divisor + float divisor = b3Dot( ab, ab ); + + out[0] = b3Dot( b, ab ); + out[1] = -b3Dot( a, ab ); + out[2] = divisor; +} + +static void b3BarycentricCoordsTri( float out[4], b3Vec3 a, b3Vec3 b, b3Vec3 c ) +{ + b3Vec3 ab = b3Sub( b, a ); + b3Vec3 ac = b3Sub( c, a ); + + b3Vec3 bXC = b3Cross( b, c ); + b3Vec3 cXA = b3Cross( c, a ); + b3Vec3 aXB = b3Cross( a, b ); + + b3Vec3 abXAc = b3Cross( ab, ac ); + + // Last element is divisor + float divisor = b3Dot( abXAc, abXAc ); + + out[0] = b3Dot( bXC, abXAc ); + out[1] = b3Dot( cXA, abXAc ); + out[2] = b3Dot( aXB, abXAc ); + out[3] = divisor; +} + +static void b3BarycentricCoordsTet( float out[5], b3Vec3 a, b3Vec3 b, b3Vec3 c, b3Vec3 d ) +{ + b3Vec3 ab = b3Sub( b, a ); + b3Vec3 ac = b3Sub( c, a ); + b3Vec3 ad = b3Sub( d, a ); + + // Last element is divisor (forced to be positive) + float divisor = b3ScalarTripleProduct( ab, ac, ad ); + + float sign = divisor < 0.0f ? -1.0f : 1.0f; + out[0] = sign * b3ScalarTripleProduct( b, c, d ); + out[1] = sign * b3ScalarTripleProduct( a, d, c ); + out[2] = sign * b3ScalarTripleProduct( a, b, d ); + out[3] = sign * b3ScalarTripleProduct( a, c, b ); + out[4] = sign * divisor; +} + +static float b3GetMetric( const b3Simplex* simplex ) +{ + int count = simplex->count; + B3_ASSERT( 1 <= count && count <= 4 ); + + const b3SimplexVertex* vertices = simplex->vertices; + + switch ( count ) + { + case 1: + { + return 0.0f; + } + + case 2: + { + b3Vec3 a = vertices[0].w; + b3Vec3 b = vertices[1].w; + return b3Distance( a, b ); + } + + case 3: + { + b3Vec3 a = vertices[0].w; + b3Vec3 b = vertices[1].w; + b3Vec3 c = vertices[2].w; + return b3Length( b3Cross( b3Sub( b, a ), b3Sub( c, a ) ) ) / 2.0f; + } + + case 4: + { + b3Vec3 a = vertices[0].w; + b3Vec3 b = vertices[1].w; + b3Vec3 c = vertices[2].w; + b3Vec3 d = vertices[3].w; + return b3ScalarTripleProduct( b3Sub( b, a ), b3Sub( c, a ), b3Sub( d, a ) ) / 6.0f; + } + + default: + B3_ASSERT( !"Should never get here!" ); + break; + } + + return 0.0f; +} + +static void b3WriteCache( b3SimplexCache* cache, const b3Simplex* simplex ) +{ + int count = simplex->count; + cache->metric = b3GetMetric( simplex ); + cache->count = (uint16_t)count; + for ( int index = 0; index < count; ++index ) + { + cache->indexA[index] = (uint8_t)simplex->vertices[index].indexA; + cache->indexB[index] = (uint8_t)simplex->vertices[index].indexB; + } +} + +static bool b3SolveSimplex2( b3Simplex* simplex ) +{ + b3SimplexVertex* vs = simplex->vertices; + B3_ASSERT( simplex->count == 2 ); + + // Vertex regions + //float wAB[3]; + + b3Vec3 a = vs[0].w; + b3Vec3 b = vs[1].w; + b3Vec3 ab = b3Sub( b, a ); + + // Last element is divisor + float divisor = b3Dot( ab, ab ); + + float u = b3Dot( b, ab ); + float v = -b3Dot( a, ab ); + //wAB[2] = divisor; + + // V( A ) + if ( v <= 0.0f ) + { + // Reduce simplex + simplex->count = 1; + vs[0].a = 1.0f; + + return true; + } + + // V( B ) + if ( u <= 0.0f ) + { + // Reduce simplex + simplex->count = 1; + vs[0] = vs[1]; + vs[0].a = 1.0f; + + return true; + } + + // Edge region + if ( divisor <= 0.0f ) + { + return false; + } + + // VR( AB ) + float denominator = 1.0f / divisor; + vs[0].a = denominator * u; + vs[1].a = denominator * v; + + return true; +} + +static bool b3SolveSimplex3( b3Simplex* simplex ) +{ + b3SimplexVertex* vs = simplex->vertices; + B3_ASSERT( simplex->count == 3 ); + + // Get simplex (be aware of aliasing here!) + b3SimplexVertex v1 = vs[0]; + b3SimplexVertex v2 = vs[1]; + b3SimplexVertex v3 = vs[2]; + + // Vertex regions + float wAB[3], wBC[3], wCA[3]; + b3BarycentricCoordsEdge( wAB, v1.w, v2.w ); + b3BarycentricCoordsEdge( wBC, v2.w, v3.w ); + b3BarycentricCoordsEdge( wCA, v3.w, v1.w ); + + // VR( A ) + if ( wAB[1] <= 0.0f && wCA[0] <= 0.0f ) + { + // Reduce simplex + simplex->count = 1; + vs[0] = v1; + vs[0].a = 1.0f; + + return true; + } + + // VR( B ) + if ( wBC[1] <= 0.0f && wAB[0] <= 0.0f ) + { + // Reduce simplex + simplex->count = 1; + vs[0] = v2; + vs[0].a = 1.0f; + + return true; + } + + // VR( C ) + if ( wCA[1] <= 0.0f && wBC[0] <= 0.0f ) + { + // Reduce simplex + simplex->count = 1; + vs[0] = v3; + vs[0].a = 1.0f; + + return true; + } + + // Edge regions + float wABC[4]; + b3BarycentricCoordsTri( wABC, v1.w, v2.w, v3.w ); + + // VR( AB ) + if ( wABC[2] <= 0.0f && wAB[0] > 0.0f && wAB[1] > 0.0f ) + { + // Reduce simplex + simplex->count = 2; + vs[0] = v1; + vs[1] = v2; + + // Normalize + float divisor = wAB[2]; + if ( divisor <= 0.0f ) + { + return false; + } + + vs[0].a = wAB[0] / divisor; + vs[1].a = wAB[1] / divisor; + + return true; + } + + // VR( BC ) + if ( wABC[0] <= 0.0f && wBC[0] > 0.0f && wBC[1] > 0.0f ) + { + // Reduce simplex + simplex->count = 2; + vs[0] = v2; + vs[1] = v3; + + // Normalize + float divisor = wBC[2]; + if ( divisor <= 0.0f ) + { + return false; + } + + vs[0].a = wBC[0] / divisor; + vs[1].a = wBC[1] / divisor; + + return true; + } + + // VR( CA ) + if ( wABC[1] <= 0.0f && wCA[0] > 0.0f && wCA[1] > 0.0f ) + { + // Reduce simplex + simplex->count = 2; + vs[0] = v3; + vs[1] = v1; + + // Normalize + float divisor = wCA[2]; + if ( divisor <= 0.0f ) + { + return false; + } + + vs[0].a = wCA[0] / divisor; + vs[1].a = wCA[1] / divisor; + + return true; + } + + // Face region + float divisor = wABC[3]; + if ( divisor <= 0.0f ) + { + return false; + } + + // VR( ABC ) + vs[0].a = wABC[0] / divisor; + vs[1].a = wABC[1] / divisor; + vs[2].a = wABC[2] / divisor; + + return true; +} + +static bool b3SolveSimplex4( b3Simplex* simplex ) +{ + b3SimplexVertex* vs = simplex->vertices; + + // Get simplex (be aware of aliasing here!) + B3_ASSERT( simplex->count == 4 ); + b3SimplexVertex vertexA = vs[0]; + b3SimplexVertex vertexB = vs[1]; + b3SimplexVertex vertexC = vs[2]; + b3SimplexVertex vertexD = vs[3]; + + // Vertex region + float wAB[3], wAC[3], wAD[3], wBC[3], wCD[3], wDB[3]; + b3BarycentricCoordsEdge( wAB, vertexA.w, vertexB.w ); + b3BarycentricCoordsEdge( wAC, vertexA.w, vertexC.w ); + b3BarycentricCoordsEdge( wAD, vertexA.w, vertexD.w ); + b3BarycentricCoordsEdge( wBC, vertexB.w, vertexC.w ); + b3BarycentricCoordsEdge( wCD, vertexC.w, vertexD.w ); + b3BarycentricCoordsEdge( wDB, vertexD.w, vertexB.w ); + + // VR( A ) + if ( wAB[1] <= 0.0f && wAC[1] <= 0.0f && wAD[1] <= 0.0f ) + { + // Reduce simplex + simplex->count = 1; + vs[0] = vertexA; + + vs[0].a = 1.0f; + + return true; + } + + // VR( B ) + if ( wAB[0] <= 0.0f && wDB[0] <= 0.0f && wBC[1] <= 0.0f ) + { + // Reduce simplex + simplex->count = 1; + vs[0] = vertexB; + + vs[0].a = 1.0f; + + return true; + } + + // VR( C ) + if ( wAC[0] <= 0.0f && wBC[0] <= 0.0f && wCD[1] <= 0.0f ) + { + // Reduce simplex + simplex->count = 1; + vs[0] = vertexC; + + vs[0].a = 1.0f; + + return true; + } + + // VR( D ) + if ( wAD[0] <= 0.0f && wCD[0] <= 0.0f && wDB[1] <= 0.0f ) + { + // Reduce simplex + simplex->count = 1; + vs[0] = vertexD; + + vs[0].a = 1.0f; + + return true; + } + + // Edge region + float wACB[4], wABD[4], wADC[4], wBCD[4]; + b3BarycentricCoordsTri( wACB, vertexA.w, vertexC.w, vertexB.w ); + b3BarycentricCoordsTri( wABD, vertexA.w, vertexB.w, vertexD.w ); + b3BarycentricCoordsTri( wADC, vertexA.w, vertexD.w, vertexC.w ); + b3BarycentricCoordsTri( wBCD, vertexB.w, vertexC.w, vertexD.w ); + + // VR( AB ) + if ( wABD[2] <= 0.0f && wACB[1] <= 0.0f && wAB[0] > 0.0f && wAB[1] > 0.0f ) + { + // Reduce simplex + simplex->count = 2; + vs[0] = vertexA; + vs[1] = vertexB; + + // Normalize + float divisor = wAB[2]; + if ( divisor <= 0.0f ) + { + return false; + } + + vs[0].a = wAB[0] / divisor; + vs[1].a = wAB[1] / divisor; + + return true; + } + + // VR( AC ) + if ( wACB[2] <= 0.0f && wADC[1] <= 0.0f && wAC[0] > 0.0f && wAC[1] > 0.0f ) + { + // Reduce simplex + simplex->count = 2; + vs[0] = vertexA; + vs[1] = vertexC; + + // Normalize + float divisor = wAC[2]; + if ( divisor <= 0.0f ) + { + return false; + } + + vs[0].a = wAC[0] / divisor; + vs[1].a = wAC[1] / divisor; + + return true; + } + + // VR( AD ) + if ( wADC[2] <= 0.0f && wABD[1] <= 0.0f && wAD[0] > 0.0f && wAD[1] > 0.0f ) + { + // Reduce simplex + simplex->count = 2; + vs[0] = vertexA; + vs[1] = vertexD; + + // Normalize + float divisor = wAD[2]; + if ( divisor <= 0.0f ) + { + return false; + } + + vs[0].a = wAD[0] / divisor; + vs[1].a = wAD[1] / divisor; + + return true; + } + + // VR( BC ) + if ( wACB[0] <= 0.0f && wBCD[2] <= 0.0f && wBC[0] > 0.0f && wBC[1] > 0.0f ) + { + // Reduce simplex + simplex->count = 2; + vs[0] = vertexB; + vs[1] = vertexC; + + // Normalize + float divisor = wBC[2]; + if ( divisor <= 0.0f ) + { + return false; + } + + vs[0].a = wBC[0] / divisor; + vs[1].a = wBC[1] / divisor; + + return true; + } + + // VR( CD ) + if ( wADC[0] <= 0.0f && wBCD[0] <= 0.0f && wCD[0] > 0.0f && wCD[1] > 0.0f ) + { + // Reduce simplex + simplex->count = 2; + vs[0] = vertexC; + vs[1] = vertexD; + + // Normalize + float divisor = wCD[2]; + if ( divisor <= 0.0f ) + { + return false; + } + + vs[0].a = wCD[0] / divisor; + vs[1].a = wCD[1] / divisor; + + return true; + } + + // VR( DB ) + if ( wABD[0] <= 0.0f && wBCD[1] <= 0.0f && wDB[0] > 0.0f && wDB[1] > 0.0f ) + { + // Reduce simplex + simplex->count = 2; + vs[0] = vertexD; + vs[1] = vertexB; + + // Normalize + float divisor = wDB[2]; + if ( divisor <= 0.0f ) + { + return false; + } + + vs[0].a = wDB[0] / divisor; + vs[1].a = wDB[1] / divisor; + + return true; + } + + // Face regions + float wABCD[5]; + b3BarycentricCoordsTet( wABCD, vertexA.w, vertexB.w, vertexC.w, vertexD.w ); + + // VR( ACB ) + if ( wABCD[3] < 0.0f && wACB[0] > 0.0f && wACB[1] > 0.0f && wACB[2] > 0.0f ) + { + // Reduce simplex + simplex->count = 3; + vs[0] = vertexA; + vs[1] = vertexC; + vs[2] = vertexB; + + // Normalize + float divisor = wACB[3]; + if ( divisor <= 0.0f ) + { + return false; + } + + vs[0].a = wACB[0] / divisor; + vs[1].a = wACB[1] / divisor; + vs[2].a = wACB[2] / divisor; + + return true; + } + + // VR( ABD ) + if ( wABCD[2] < 0.0f && wABD[0] > 0.0f && wABD[1] > 0.0f && wABD[2] > 0.0f ) + { + // Reduce simplex + simplex->count = 3; + vs[0] = vertexA; + vs[1] = vertexB; + vs[2] = vertexD; + + // Normalize + float divisor = wABD[3]; + if ( divisor <= 0.0f ) + { + return false; + } + + vs[0].a = wABD[0] / divisor; + vs[1].a = wABD[1] / divisor; + vs[2].a = wABD[2] / divisor; + + return true; + } + + // VR( ADC ) + if ( wABCD[1] < 0.0f && wADC[0] > 0.0f && wADC[1] > 0.0f && wADC[2] > 0.0f ) + { + // Reduce simplex + simplex->count = 3; + vs[0] = vertexA; + vs[1] = vertexD; + vs[2] = vertexC; + + // Normalize + float divisor = wADC[3]; + if ( divisor <= 0.0f ) + { + return false; + } + + vs[0].a = wADC[0] / divisor; + vs[1].a = wADC[1] / divisor; + vs[2].a = wADC[2] / divisor; + + return true; + } + + // VR( BCD ) + if ( wABCD[0] < 0.0f && wBCD[0] > 0.0f && wBCD[1] > 0.0f && wBCD[2] > 0.0f ) + { + // Reduce simplex + simplex->count = 3; + vs[0] = vertexB; + vs[1] = vertexC; + vs[2] = vertexD; + + // Normalize + float divisor = wBCD[3]; + if ( divisor <= 0.0f ) + { + return false; + } + + vs[0].a = wBCD[0] / divisor; + vs[1].a = wBCD[1] / divisor; + vs[2].a = wBCD[2] / divisor; + + return true; + } + + // *** Inside tetrahedron *** + float divisor = wABCD[4]; + if ( divisor <= 0.0f ) + { + return false; + } + + // VR( ABCD ) + vs[0].a = wABCD[0] / divisor; + vs[1].a = wABCD[1] / divisor; + vs[2].a = wABCD[2] / divisor; + vs[3].a = wABCD[3] / divisor; + + return true; +} + +static void b3ComputeWitnessPoints( const b3Simplex* simplex, b3Vec3* vertexA, b3Vec3* vertexB ) +{ + const b3SimplexVertex* vs = simplex->vertices; + int count = simplex->count; + B3_ASSERT( 1 <= count && count <= 4 ); + + switch ( count ) + { + case 1: + *vertexA = vs[0].wA; + *vertexB = vs[0].wB; + break; + + case 2: + *vertexA = b3Blend2( vs[0].a, vs[0].wA, vs[1].a, vs[1].wA ); + *vertexB = b3Blend2( vs[0].a, vs[0].wB, vs[1].a, vs[1].wB ); + break; + + case 3: + *vertexA = b3Blend3( vs[0].a, vs[0].wA, vs[1].a, vs[1].wA, vs[2].a, vs[2].wA ); + *vertexB = b3Blend3( vs[0].a, vs[0].wB, vs[1].a, vs[1].wB, vs[2].a, vs[2].wB ); + break; + + case 4: + { + // Force identical points and *zero* distance + b3Vec3 sum = b3Add( b3Blend2( vs[0].a, vs[0].wA, vs[1].a, vs[1].wA ), + b3Blend2( vs[2].a, vs[2].wA, vs[3].a, vs[3].wA ) ); + *vertexA = sum; + *vertexB = sum; + } + break; + + default: + B3_ASSERT( !"Should never get here!" ); + break; + } +} + +b3DistanceOutput b3ShapeDistance( const b3DistanceInput* input, b3SimplexCache* cache, b3Simplex* simplexes, int simplexCapacity ) +{ + // The query runs in frame A using the relative pose of B in A. + b3Transform xf = input->transform; + + // Use matrices for faster math + b3Matrix3 m = b3MakeMatrixFromQuat( xf.q ); + b3Matrix3 mt = b3Transpose( m ); + + const b3ShapeProxy* proxyA = &input->proxyA; + const b3ShapeProxy* proxyB = &input->proxyB; + + // Compute initial simplex from cache + B3_ASSERT( cache->count <= B3_MAX_SIMPLEX_VERTICES ); + + b3Simplex simplex = { 0 }; + b3SimplexVertex* vs = simplex.vertices; + + simplex.count = cache->count; + for ( int i = 0; i < cache->count; ++i ) + { + int index1 = cache->indexA[i]; + int index2 = cache->indexB[i]; + + B3_ASSERT( 0 <= index1 && index1 < proxyA->count ); + B3_ASSERT( 0 <= index2 && index2 < proxyB->count ); + + b3Vec3 vertex1 = proxyA->points[index1]; + b3Vec3 vertex2 = b3Add( b3MulMV( m, proxyB->points[index2] ), xf.p ); + + vs[i].indexA = index1; + vs[i].indexB = index2; + vs[i].wA = vertex1; + vs[i].wB = vertex2; + vs[i].w = b3Sub( vertex2, vertex1 ); + vs[i].a = 0.0f; + } + + // Compute the new simplex metric, if it is substantially + // different than the old metric flush the simplex. + if ( simplex.count > 0 ) + { + float metric1 = cache->metric; + float metric2 = b3GetMetric( &simplex ); + + // todo the tetrahedron metric can be negative + if ( 2.0f * metric1 < metric2 || metric2 < 0.5f * metric1 || metric2 < FLT_EPSILON ) + { + // Flush the simplex + simplex.count = 0; + } + } + + // If the cache is invalid or empty + if ( simplex.count == 0 ) + { + b3Vec3 vertex1 = proxyA->points[0]; + b3Vec3 vertex2 = b3Add( b3MulMV( m, proxyB->points[0] ), xf.p ); + + simplex.count = 1; + simplex.vertices[0].indexA = 0; + simplex.vertices[0].indexB = 0; + simplex.vertices[0].wA = vertex1; + simplex.vertices[0].wB = vertex2; + simplex.vertices[0].w = b3Sub( vertex2, vertex1 ); + simplex.vertices[0].a = 0.0f; + } + + b3Simplex backup = { 0 }; + + int simplexIndex = 0; + if ( simplexes != NULL && simplexIndex < simplexCapacity ) + { + simplexes[simplexIndex] = simplex; + simplexIndex += 1; + } + + b3DistanceOutput distanceOutput = { 0 }; + + // Keep track of squared distance + float distanceSq = FLT_MAX; + + b3Vec3 normal = b3Vec3_zero; + + // Run GJK + int iteration = 0; + for ( ; iteration < B3_MAX_GJK_ITERATIONS; ++iteration ) + { + // Solve simplex + bool solved = false; + switch ( simplex.count ) + { + case 1: + simplex.vertices[0].a = 1.0f; + solved = true; + break; + + case 2: + solved = b3SolveSimplex2( &simplex ); + break; + + case 3: + solved = b3SolveSimplex3( &simplex ); + break; + + case 4: + solved = b3SolveSimplex4( &simplex ); + break; + + default: + B3_ASSERT( !"Should never get here!" ); + break; + } + + if ( solved == false ) + { + // No progress - reconstruct last simplex + B3_ASSERT( backup.count != 0 ); + simplex = backup; + break; + } + + if ( simplexes != NULL && simplexIndex < simplexCapacity ) + { + simplexes[simplexIndex] = simplex; + simplexIndex += 1; + distanceOutput.iterations = iteration; + distanceOutput.simplexCount = simplexIndex; + } + + if ( simplex.count == B3_MAX_SIMPLEX_VERTICES ) + { + // Overlap + b3Vec3 localPointA, localPointB; + b3ComputeWitnessPoints( &simplex, &localPointA, &localPointB ); + distanceOutput.pointA = localPointA; + distanceOutput.pointB = localPointB; + return distanceOutput; + } + + // Assure distance progression + float oldDistanceSq = distanceSq; + + // Compute closest point + b3Vec3 closestPoint = { 0 }; + + switch ( simplex.count ) + { + case 1: + closestPoint = vs[0].w; + break; + + case 2: + closestPoint = b3Blend2( vs[0].a, vs[0].w, vs[1].a, vs[1].w ); + break; + + case 3: + closestPoint = b3Blend3( vs[0].a, vs[0].w, vs[1].a, vs[1].w, vs[2].a, vs[2].w ); + break; + + case 4: + closestPoint = b3Add( b3Blend2( vs[0].a, vs[0].w, vs[1].a, vs[1].w ), + b3Blend2( vs[2].a, vs[2].w, vs[3].a, vs[3].w ) ); + break; + + default: + B3_ASSERT( !"Should never get here!" ); + break; + } + + distanceSq = b3Dot( closestPoint, closestPoint ); + + if ( distanceSq >= oldDistanceSq ) + { + // No progress - reconstruct last simplex + B3_ASSERT( backup.count != 0 ); + simplex = backup; + break; + } + + // Build new tentative support point + b3Vec3 searchDirection = { 0 }; + + switch ( simplex.count ) + { + case 1: + { + // v = -A + searchDirection = b3Neg( vs[0].w ); + } + break; + + case 2: + { + // v = (AB x AO) x AB + b3Vec3 a = vs[0].w; + b3Vec3 b = vs[1].w; + + b3Vec3 ab = b3Sub( b, a ); + + searchDirection = b3Cross( b3Cross( ab, b3Neg( a ) ), ab ); + } + break; + + case 3: + { + // v = AB x AC or v = AC x AB + b3Vec3 a = vs[0].w; + b3Vec3 b = vs[1].w; + b3Vec3 c = vs[2].w; + + b3Vec3 ab = b3Sub( b, a ); + b3Vec3 ac = b3Sub( c, a ); + + b3Vec3 n = b3Cross( ab, ac ); + + searchDirection = b3Dot( n, a ) < 0.0f ? n : b3Neg( n ); + } + break; + + default: + B3_ASSERT( !"Should never get here!" ); + break; + } + + if ( b3LengthSquared( searchDirection ) < 1000.0f * FLT_MIN ) + { + // The origin is probably contained by a line segment or triangle. + // Thus the shapes are overlapped. + b3Vec3 localPointA, localPointB; + b3ComputeWitnessPoints( &simplex, &localPointA, &localPointB ); + distanceOutput.pointA = localPointA; + distanceOutput.pointB = localPointB; + B3_VALIDATE( b3Distance( localPointA, localPointB ) < FLT_EPSILON ); + return distanceOutput; + } + + normal = b3Neg( searchDirection ); + + // Get new support points + b3Vec3 searchDirection1 = searchDirection; + int indexA = b3GetProxySupport( &input->proxyA, b3Neg( searchDirection1 ) ); + b3Vec3 supportA = input->proxyA.points[indexA]; + b3Vec3 searchDirection2 = b3MulMV( mt, searchDirection ); + int indexB = b3GetProxySupport( &input->proxyB, searchDirection2 ); + b3Vec3 supportB = b3Add( b3MulMV( m, input->proxyB.points[indexB] ), xf.p ); + + // Save current simplex and add new vertex - this can fail if we detect cycling + backup = simplex; + + // Check for duplicate support points. This is the main termination criteria. + bool duplicate = false; + for ( int i = 0; i < simplex.count; ++i ) + { + if ( vs[i].indexA == indexA && vs[i].indexB == indexB ) + { + duplicate = true; + break; + } + } + + if ( duplicate ) + { + break; + } + + vs[simplex.count].indexA = indexA; + vs[simplex.count].indexB = indexB; + vs[simplex.count].wA = supportA; + vs[simplex.count].wB = supportB; + vs[simplex.count].w = b3Sub( supportB, supportA ); + simplex.count += 1; + } + + normal = b3Normalize( normal ); + if ( b3IsNormalized( normal ) == false ) + { + // Treat as overlap + return distanceOutput; + } + + // Build witness points and safe cache + b3Vec3 localPointA, localPointB; + b3ComputeWitnessPoints( &simplex, &localPointA, &localPointB ); + b3WriteCache( cache, &simplex ); + + // Results stay in frame A + distanceOutput.pointA = localPointA; + distanceOutput.pointB = localPointB; + distanceOutput.distance = b3Distance( localPointA, localPointB ); + distanceOutput.normal = normal; + distanceOutput.iterations = iteration; + distanceOutput.simplexCount = simplexIndex; + + // Apply radii if requested + if ( input->useRadii ) + { + float rA = input->proxyA.radius; + float rB = input->proxyB.radius; + distanceOutput.distance = b3MaxFloat( 0.0f, distanceOutput.distance - rA - rB ); + + // Keep closest points on perimeter even if overlapped, this way the points move smoothly. + distanceOutput.pointA = b3Add( distanceOutput.pointA, b3MulSV( rA, normal ) ); + distanceOutput.pointB = b3Sub( distanceOutput.pointB, b3MulSV( rB, normal ) ); + } + + return distanceOutput; +} + + +// Separation function: +// f(t) = (c2 + t * dp2 - c1 - t * dp1 ) * n + +// Root finding : f(t) - target = 0 +// (c2 + t * dp2 - c1 - t * dp1 ) * n - target = 0 +// (c2 - c1) * n + t * (dp2 - dp1) * n - target = 0 +// t = [target - (c2 - c1) * n] / [(dp2 - dp1) * n] +// t = (target - d) / [(dp2 - dp1) * n] + +b3CastOutput b3ShapeCast( const b3ShapeCastPairInput* input ) +{ + // Compute tolerance + float linearSlop = B3_LINEAR_SLOP; + float totalRadius = input->proxyA.radius + input->proxyB.radius; + float target = b3MaxFloat( linearSlop, totalRadius - linearSlop ); + float tolerance = 0.25f * linearSlop; + + B3_ASSERT( target > tolerance ); + + // Prepare input for distance query + b3SimplexCache cache = { 0 }; + + float alpha = 0.0f; + + b3DistanceInput distanceInput = { 0 }; + distanceInput.proxyA = input->proxyA; + distanceInput.proxyB = input->proxyB; + distanceInput.useRadii = false; + + // The whole cast runs in frame A. Advance the relative pose of B in float each iteration, + // which keeps the math near the local origin and avoids re-relativizing world poses. + distanceInput.transform = input->transform; + + b3Vec3 delta2 = input->translationB; + b3DistanceOutput distanceOutput = { 0 }; + b3CastOutput output = { 0 }; + output.triangleIndex = B3_NULL_INDEX; + + int iteration = 0; + const int maxIterations = 20; + + for ( ; iteration < maxIterations; ++iteration ) + { + output.iterations += 1; + + distanceOutput = b3ShapeDistance( &distanceInput, &cache, NULL, 0 ); + + if ( distanceOutput.distance < target + tolerance ) + { + if ( iteration == 0 ) + { + if ( input->canEncroach && distanceOutput.distance > 2.0f * linearSlop ) + { + target = distanceOutput.distance - linearSlop; + } + else + { + // Initial overlap + output.hit = true; + + // Compute a common point + b3Vec3 c1 = b3MulAdd( distanceOutput.pointA, input->proxyA.radius, distanceOutput.normal ); + b3Vec3 c2 = b3MulAdd( distanceOutput.pointB, -input->proxyB.radius, distanceOutput.normal ); + output.point = b3Lerp( c1, c2, 0.5f ); + return output; + } + } + else + { + // Logging for bad input data + if ( distanceOutput.distance > 0.0f && b3IsNormalized( distanceOutput.normal ) == false ) + { + for ( int i = 0; i < input->proxyA.count; ++i ) + { + b3Vec3 p = input->proxyA.points[i]; + b3Log( "pointA[%d] = {%.9f, %.9f, %.9f}", i, p.x, p.y, p.z ); + } + b3Log( "radiusA = %.9f", input->proxyA.radius ); + + for ( int i = 0; i < input->proxyB.count; ++i ) + { + b3Vec3 p = input->proxyB.points[i]; + b3Log( "pointB[%d] = {%.9f, %.9f, %.9f}", i, p.x, p.y, p.z ); + } + b3Log( "radiusB = %.9f", input->proxyB.radius ); + + { + b3Transform xf = input->transform; + b3Log( "transform = {{%.9f, %.9f, %.9f}, {{%.9f, %.9f, %.9f}, %.9f}", xf.p.x, xf.p.y, xf.p.z, xf.q.v.x, + xf.q.v.y, xf.q.v.z, xf.q.s ); + } + + { + b3Vec3 t = input->translationB; + b3Log( "t = {%.9f, %.9f, %.9f}", t.x, t.y, t.z ); + } + + b3Log( "maxFraction = %.9f, canEncroach = %d", input->maxFraction, input->canEncroach ); + + // Numerical problem. Likely extreme input. + return output; + } + + // Hitting this assert implies that the algorithm brought the shapes too close. + // B3_ASSERT( distanceOutput.distance > 0.0f && b3IsNormalized( distanceOutput.normal ) ); + + output.fraction = alpha; + output.point = b3MulAdd( distanceOutput.pointA, input->proxyA.radius, distanceOutput.normal ); + output.normal = distanceOutput.normal; + output.hit = true; + return output; + } + } + + B3_ASSERT( distanceOutput.distance > 0.0f ); + B3_ASSERT( b3IsNormalized( distanceOutput.normal ) ); + + // Check if shapes are approaching each other + float denominator = b3Dot( delta2, distanceOutput.normal ); + if ( denominator >= 0.0f ) + { + // Miss + return output; + } + + // Advance sweep + alpha += ( target - distanceOutput.distance ) / denominator; + if ( alpha >= input->maxFraction ) + { + // Success! + return output; + } + + distanceInput.transform.p = b3MulAdd( input->transform.p, alpha, delta2 ); + } + + // Failure! + return output; +} + +b3Transform b3GetSweepTransform( const b3Sweep* sweep, float time ) +{ + b3Transform transform; + transform.q = b3NLerp( sweep->q1, sweep->q2, time ); + transform.p = b3Sub( b3Lerp( sweep->c1, sweep->c2, time ), b3RotateVector( transform.q, sweep->localCenter ) ); + return transform; +} + +static inline b3Transform b3GetFinalSweepTransform( const b3Sweep* sweep ) +{ + b3Transform transform; + transform.q = sweep->q2; + transform.p = b3Sub( sweep->c2, b3RotateVector( transform.q, sweep->localCenter ) ); + return transform; +} + +static int b3UniqueCount( int vertexCount, int vertices[3] ) +{ + B3_ASSERT( 1 <= vertexCount && vertexCount <= 3 ); + + switch ( vertexCount ) + { + case 1: + return 1; + + case 2: + return vertices[0] != vertices[1] ? 2 : 1; + + case 3: + if ( vertices[0] != vertices[1] && vertices[0] != vertices[2] && vertices[1] != vertices[2] ) + { + // All different + return 3; + } + + if ( vertices[0] == vertices[1] && vertices[0] == vertices[2] && vertices[1] == vertices[2] ) + { + // All equal + return 1; + } + + return 2; + + default: + B3_ASSERT( !"Should never get here!" ); + break; + } + + return 0; +} + +// This checks if the cross product of two edges switches direction. +static inline bool b3CheckFastEdges( b3Transform xfA, b3Vec3 localEdgeA, b3Transform xfB, b3Vec3 localEdgeB, b3Vec3 axis0 ) +{ + // By taking the local witness axes we make sure that we + // get the correct orientations (e.g. if one axis was flipped)! + b3Vec3 edgeA = b3RotateVector( xfA.q, localEdgeA ); + b3Vec3 edgeB = b3RotateVector( xfB.q, localEdgeB ); + b3Vec3 axis = b3Cross( edgeA, edgeB ); + return b3Dot( axis, axis0 ) < 0.0f; +} + +typedef enum b3SeparationType +{ + b3_separationUnknown = 0, + b3_separationVertices, + b3_separationEdges, + b3_separationFaceA, + b3_separationFaceB, +} b3SeparationType; + +typedef struct b3SeparationFunction +{ + const b3ShapeProxy* proxyA; + const b3ShapeProxy* proxyB; + b3Sweep sweepA; + b3Sweep sweepB; + + // These are associated with different bodies depending on the separation function type. + // It could be two local vectors/points on the same body (for example, both on bodyA). + b3Vec3 witness1; + b3Vec3 witness2; + + b3SeparationType type; +} b3SeparationFunction; + +static b3SeparationFunction b3MakeSeparationFunction( const b3SimplexCache cache, const b3ShapeProxy* proxyA, + const b3Sweep* sweepA, const b3ShapeProxy* proxyB, const b3Sweep* sweepB, + b3Vec3 worldNormal, float t1 ) +{ + B3_ASSERT( 1 <= cache.count && cache.count <= 3 ); + B3_VALIDATE( b3IsNormalized( worldNormal ) ); + + b3SeparationFunction fcn = { 0 }; + fcn.proxyA = proxyA; + fcn.proxyB = proxyB; + fcn.sweepA = *sweepA; + fcn.sweepB = *sweepB; + fcn.type = b3_separationUnknown; + + int indexA[3] = { cache.indexA[0], cache.indexA[1], cache.indexA[2] }; + int indexB[3] = { cache.indexB[0], cache.indexB[1], cache.indexB[2] }; + + int uniqueCountA = b3UniqueCount( cache.count, indexA ); + int uniqueCountB = b3UniqueCount( cache.count, indexB ); + + b3Transform xfA1 = b3GetSweepTransform( sweepA, t1 ); + b3Transform xfB1 = b3GetSweepTransform( sweepB, t1 ); + + b3Quat qA = xfA1.q; + b3Quat qB = xfB1.q; + + // Minimize round-off + b3Vec3 deltaP = b3Sub( xfB1.p, xfA1.p ); + + switch ( cache.count ) + { + case 1: + { + // Witness is the world space direction + fcn.type = b3_separationVertices; + fcn.witness1 = worldNormal; + } + break; + + case 2: + { + if ( uniqueCountA == 2 && uniqueCountB == 2 ) + { + // Edge/Edge + b3Vec3 vA1 = proxyA->points[indexA[0]]; + b3Vec3 localEdgeA = b3Sub( proxyA->points[indexA[1]], vA1 ); + localEdgeA = b3Normalize( localEdgeA ); + b3Vec3 edgeA = b3RotateVector( qA, localEdgeA ); + + b3Vec3 vB1 = proxyB->points[indexB[0]]; + b3Vec3 localEdgeB = b3Sub( proxyB->points[indexB[1]], vB1 ); + localEdgeB = b3Normalize( localEdgeB ); + b3Vec3 edgeB = b3RotateVector( qB, localEdgeB ); + + b3Vec3 axis = b3Cross( edgeA, edgeB ); + float lengthSquared = b3LengthSquared( axis ); + + // Skip near parallel edges: |e1 x e1| = sin(alpha) * |e1| * |e2| + const float kToleranceSquared = 0.05f * 0.05f; + if ( lengthSquared < kToleranceSquared ) + { + // The axis is not safe to normalize so we use a world axis instead! + fcn.type = b3_separationVertices; + fcn.witness1 = worldNormal; + } + else + { + b3Vec3 delta = b3Add( b3Sub( b3RotateVector( qB, vB1 ), b3RotateVector( qA, vA1 ) ), deltaP ); + if ( b3Dot( delta, axis ) < 0.0f ) + { + // Make axis point from A to B + axis = b3Neg( axis ); + localEdgeB = b3Neg( localEdgeB ); + } + + // Check for possible sign flip in edge/edge cross product + b3Transform xfA2 = b3GetFinalSweepTransform( sweepA ); + b3Transform xfB2 = b3GetFinalSweepTransform( sweepB ); + bool fastEdges = b3CheckFastEdges( xfA2, localEdgeA, xfB2, localEdgeB, axis ); + if ( fastEdges == true ) + { + // Not safe to use local edges, fall back to initial world space axis instead + fcn.type = b3_separationVertices; + fcn.witness1 = b3Normalize( axis ); + } + else + { + // Edge cross product is safe. This converges faster than a fixed axis. + fcn.type = b3_separationEdges; + fcn.witness1 = localEdgeA; + fcn.witness2 = localEdgeB; + } + } + } + else + { + B3_VALIDATE( b3IsNormalized( worldNormal ) ); + + // Vertex versus edge, use world axis witness + fcn.type = b3_separationVertices; + fcn.witness1 = worldNormal; + } + } + break; + + case 3: + { + if ( uniqueCountA == 3 ) + { + b3Vec3 vA1 = proxyA->points[indexA[0]]; + b3Vec3 vA2 = proxyA->points[indexA[1]]; + b3Vec3 vA3 = proxyA->points[indexA[2]]; + b3Vec3 localAxisA = b3Cross( b3Sub( vA2, vA1 ), b3Sub( vA3, vA1 ) ); + localAxisA = b3Normalize( localAxisA ); + b3Vec3 axisA = b3RotateVector( qA, localAxisA ); + + b3Vec3 localPointA = b3MulSV( 1.0f / 3.0f, b3Add( b3Add( vA1, vA2 ), vA3 ) ); + b3Vec3 localPointB = proxyB->points[indexB[0]]; + b3Vec3 delta = b3Add( b3Sub( b3RotateVector( qB, localPointB ), b3RotateVector( qA, localPointA ) ), deltaP ); + + if ( b3Dot( delta, axisA ) < 0.0f ) + { + // Make axis point from A to B + localAxisA = b3Neg( localAxisA ); + } + + // Witness is the local plane of faceA + fcn.type = b3_separationFaceA; + fcn.witness1 = localAxisA; + fcn.witness2 = localPointA; + } + else if ( uniqueCountB == 3 ) + { + b3Vec3 vB1 = proxyB->points[indexB[0]]; + b3Vec3 vB2 = proxyB->points[indexB[1]]; + b3Vec3 vB3 = proxyB->points[indexB[2]]; + b3Vec3 localAxisB = b3Cross( b3Sub( vB2, vB1 ), b3Sub( vB3, vB1 ) ); + localAxisB = b3Normalize( localAxisB ); + b3Vec3 axisB = b3RotateVector( qB, localAxisB ); + + b3Vec3 localPointA = proxyA->points[indexA[0]]; + b3Vec3 localPointB = b3MulSV( 1.0f / 3.0f, b3Add( b3Add( vB1, vB2 ), vB3 ) ); + b3Vec3 delta = b3Sub( b3Sub( b3RotateVector( qA, localPointA ), b3RotateVector( qB, localPointB ) ), deltaP ); + + if ( b3Dot( delta, axisB ) < 0.0f ) + { + // Make axis point from B to A + localAxisB = b3Neg( localAxisB ); + } + + // Witness is the local plane of faceB + fcn.type = b3_separationFaceB; + fcn.witness1 = localAxisB; + fcn.witness2 = localPointB; + } + else + { + B3_ASSERT( uniqueCountA == 2 && uniqueCountB == 2 ); + + if ( indexA[0] == indexA[1] ) + { + // Make first two indices are unique + indexA[1] = indexA[2]; + B3_ASSERT( indexA[0] != indexA[1] ); + } + + b3Vec3 vA1 = proxyA->points[indexA[0]]; + b3Vec3 vA2 = proxyA->points[indexA[1]]; + b3Vec3 localEdgeA = b3Normalize( b3Sub( vA2, vA1 ) ); + b3Vec3 edgeA = b3RotateVector( qA, localEdgeA ); + + if ( indexB[0] == indexB[1] ) + { + // Make first two indices are unique + indexB[1] = indexB[2]; + B3_ASSERT( indexB[0] != indexB[1] ); + } + + b3Vec3 vB1 = proxyB->points[indexB[0]]; + b3Vec3 vB2 = proxyB->points[indexB[1]]; + b3Vec3 localEdgeB = b3Normalize( b3Sub( vB2, vB1 ) ); + b3Vec3 edgeB = b3RotateVector( qB, localEdgeB ); + + b3Vec3 axis = b3Cross( edgeA, edgeB ); + float lengthSquared = b3LengthSquared( axis ); + + // Skip near parallel edges: |e1 x e1| = sin(alpha) * |e1| * |e2| + const float kToleranceSquared = 0.005f * 0.005f; + if ( lengthSquared < kToleranceSquared ) + { + // The axis is not safe to normalize so we use a world axis instead! + fcn.type = b3_separationVertices; + fcn.witness1 = worldNormal; + } + else + { + b3Vec3 delta = b3Add( b3Sub( b3RotateVector( qB, vB1 ), b3RotateVector( qA, vA1 ) ), deltaP ); + if ( b3Dot( delta, axis ) < 0.0f ) + { + // Make axis point from A to B + axis = b3Neg( axis ); + localEdgeB = b3Neg( localEdgeB ); + } + + // Check for possible sign flip in edge/edge cross product + b3Transform xfA2 = b3GetFinalSweepTransform( sweepA ); + b3Transform xfB2 = b3GetFinalSweepTransform( sweepB ); + bool fastEdges = b3CheckFastEdges( xfA2, localEdgeA, xfB2, localEdgeB, axis ); + if ( fastEdges ) + { + // Not safe to use local edges, fall back to initial world space axis instead + fcn.type = b3_separationVertices; + fcn.witness1 = b3Normalize( axis ); + } + else + { + // Edge cross product is safe. This converges faster than a fixed axis. + fcn.type = b3_separationEdges; + fcn.witness1 = localEdgeA; + fcn.witness2 = localEdgeB; + } + } + } + } + break; + + default: + B3_ASSERT( !"Should never get here!" ); + break; + } + + return fcn; +} + +static float b3FindMinSeparation( b3SeparationFunction* fcn, int* indexA, int* indexB, float t ) +{ + b3Transform xfA = b3GetSweepTransform( &fcn->sweepA, t ); + b3Transform xfB = b3GetSweepTransform( &fcn->sweepB, t ); + + switch ( fcn->type ) + { + case b3_separationVertices: + { + b3Vec3 axis = fcn->witness1; + + b3Vec3 localAxisA = b3InvRotateVector( xfA.q, axis ); + b3Vec3 localAxisB = b3InvRotateVector( xfB.q, b3Neg( axis ) ); + + *indexA = b3GetPointSupport( fcn->proxyA->points, fcn->proxyA->count, localAxisA ); + *indexB = b3GetPointSupport( fcn->proxyB->points, fcn->proxyB->count, localAxisB ); + + b3Vec3 deltaP = b3Sub( xfB.p, xfA.p ); + b3Vec3 localPointA = fcn->proxyA->points[*indexA]; + b3Vec3 localPointB = fcn->proxyB->points[*indexB]; + b3Vec3 delta = b3Add( b3Sub( b3RotateVector( xfB.q, localPointB ), b3RotateVector( xfA.q, localPointA ) ), deltaP ); + return b3Dot( delta, axis ); + } + + case b3_separationEdges: + { + b3Vec3 edgeA = b3RotateVector( xfA.q, fcn->witness1 ); + b3Vec3 edgeB = b3RotateVector( xfB.q, fcn->witness2 ); + b3Vec3 axis = b3Cross( edgeA, edgeB ); + B3_ASSERT( axis.x != 0.0f || axis.y != 0.0f || axis.z != 0.0f ); + axis = b3Normalize( axis ); + + b3Vec3 axisA = b3InvRotateVector( xfA.q, axis ); + *indexA = b3GetPointSupport( fcn->proxyA->points, fcn->proxyA->count, axisA ); + + b3Vec3 axisB = b3InvRotateVector( xfB.q, axis ); + *indexB = b3GetPointSupport( fcn->proxyB->points, fcn->proxyB->count, b3Neg( axisB ) ); + + b3Vec3 deltaP = b3Sub( xfB.p, xfA.p ); + b3Vec3 localPointA = fcn->proxyA->points[*indexA]; + b3Vec3 localPointB = fcn->proxyB->points[*indexB]; + b3Vec3 delta = b3Add( b3Sub( b3RotateVector( xfB.q, localPointB ), b3RotateVector( xfA.q, localPointA ) ), deltaP ); + + return b3Dot( delta, axis ); + } + + case b3_separationFaceA: + { + b3Vec3 normal = b3RotateVector( xfA.q, fcn->witness1 ); + *indexA = -1; + b3Vec3 pointA = b3TransformPoint( xfA, fcn->witness2 ); + + b3Vec3 axisB = b3InvRotateVector( xfB.q, normal ); + *indexB = b3GetPointSupport( fcn->proxyB->points, fcn->proxyB->count, b3Neg( axisB ) ); + b3Vec3 pointB = b3TransformPoint( xfB, fcn->proxyB->points[*indexB] ); + + return b3Dot( b3Sub( pointB, pointA ), normal ); + } + + case b3_separationFaceB: + { + b3Vec3 normal = b3RotateVector( xfB.q, fcn->witness1 ); + + b3Vec3 axisA = b3InvRotateVector( xfA.q, normal ); + *indexA = b3GetPointSupport( fcn->proxyA->points, fcn->proxyA->count, b3Neg( axisA ) ); + b3Vec3 pointA = b3TransformPoint( xfA, fcn->proxyA->points[*indexA] ); + + *indexB = -1; + b3Vec3 pointB = b3TransformPoint( xfB, fcn->witness2 ); + + return b3Dot( b3Sub( pointA, pointB ), normal ); + } + + default: + B3_ASSERT( !"Should never get here!" ); + break; + } + + return 0.0f; +} + +static float b3EvaluateSeparation( b3SeparationFunction* fcn, int index1, int index2, float beta ) +{ + b3Transform transform1 = b3GetSweepTransform( &fcn->sweepA, beta ); + b3Transform transform2 = b3GetSweepTransform( &fcn->sweepB, beta ); + + switch ( fcn->type ) + { + case b3_separationVertices: + { + b3Vec3 axis = fcn->witness1; + + b3Vec3 point1 = b3TransformPoint( transform1, fcn->proxyA->points[index1] ); + b3Vec3 point2 = b3TransformPoint( transform2, fcn->proxyB->points[index2] ); + + return b3Dot( b3Sub( point2, point1 ), axis ); + } + + case b3_separationEdges: + { + b3Vec3 edge1 = b3RotateVector( transform1.q, fcn->witness1 ); + b3Vec3 edge2 = b3RotateVector( transform2.q, fcn->witness2 ); + b3Vec3 axis = b3Cross( edge1, edge2 ); + axis = b3Normalize( axis ); + + b3Vec3 point1 = b3TransformPoint( transform1, fcn->proxyA->points[index1] ); + b3Vec3 point2 = b3TransformPoint( transform2, fcn->proxyB->points[index2] ); + + return b3Dot( b3Sub( point2, point1 ), axis ); + } + + case b3_separationFaceA: + { + b3Vec3 axis = b3RotateVector( transform1.q, fcn->witness1 ); + + b3Vec3 point1 = b3TransformPoint( transform1, fcn->witness2 ); + b3Vec3 point2 = b3TransformPoint( transform2, fcn->proxyB->points[index2] ); + + return b3Dot( b3Sub( point2, point1 ), axis ); + } + + case b3_separationFaceB: + { + b3Vec3 axis = b3RotateVector( transform2.q, fcn->witness1 ); + + b3Vec3 point1 = b3TransformPoint( transform1, fcn->proxyA->points[index1] ); + b3Vec3 point2 = b3TransformPoint( transform2, fcn->witness2 ); + + return b3Dot( b3Sub( point1, point2 ), axis ); + } + + default: + B3_ASSERT( !"Should never get here!" ); + break; + } + + return 0.0f; +} + +static void b3ForceFixedAxis( b3SeparationFunction* fcn, float beta ) +{ + B3_ASSERT( fcn->type == b3_separationEdges ); + + b3Transform transform1 = b3GetSweepTransform( &fcn->sweepA, beta ); + b3Transform transform2 = b3GetSweepTransform( &fcn->sweepB, beta ); + + b3Vec3 edge1 = b3RotateVector( transform1.q, fcn->witness1 ); + b3Vec3 edge2 = b3RotateVector( transform2.q, fcn->witness2 ); + b3Vec3 axis = b3Cross( edge1, edge2 ); + axis = b3Normalize( axis ); + + fcn->type = b3_separationVertices; + fcn->witness1 = axis; + fcn->witness2 = b3Vec3_zero; +} + +// Time of Impact using root finding +b3TOIOutput b3TimeOfImpact( const b3TOIInput* input ) +{ + b3TOIOutput output = { 0 }; + + // Set these to invalid values so they can be validated on exit + output.state = b3_toiStateUnknown; + output.fraction = -1.0f; + + b3Sweep sweepA = input->sweepA; + b3Sweep sweepB = input->sweepB; + + // Shift to origin + b3Vec3 origin = sweepA.c1; + sweepA.c1 = b3Vec3_zero; + sweepA.c2 = b3Sub( sweepA.c2, origin ); + sweepB.c1 = b3Sub( sweepB.c1, origin ); + sweepB.c2 = b3Sub( sweepB.c2, origin ); + + b3ShapeProxy proxyA = input->proxyA; + b3ShapeProxy proxyB = input->proxyB; + + int maxPushBackIterations = proxyA.count + proxyB.count; + float tMax = input->maxFraction; + + // Setup target distance and tolerance + float linearSlop = B3_LINEAR_SLOP; + float totalRadius = proxyA.radius + proxyB.radius; + float target = b3MaxFloat( linearSlop, totalRadius - linearSlop ); + float tolerance = 0.25f * linearSlop; + B3_ASSERT( target > tolerance ); + + float t1 = 0.0f; + const int maxIterations = 25; + int distanceIterations = 0; + + // Prepare input for distance query. + b3SimplexCache cache = { 0 }; + b3DistanceInput distanceInput = { 0 }; + distanceInput.proxyA = proxyA; + distanceInput.proxyB = proxyB; + distanceInput.useRadii = false; + + // The outer loop progressively attempts to compute new separating axes. + // This loop terminates when an axis is repeated (no progress is made). + for ( ;; ) + { + // Get the distance between shapes. We can also use the results to get a separating axis. + b3Transform xfA = b3GetSweepTransform( &sweepA, t1 ); + b3Transform xfB = b3GetSweepTransform( &sweepB, t1 ); + distanceInput.transform = b3InvMulTransforms( xfA, xfB ); + b3DistanceOutput distanceOutput = b3ShapeDistance( &distanceInput, &cache, NULL, 0 ); + output.distance = distanceOutput.distance; + + // The distance query runs in frame A, project the witness data back to the shifted world + b3Vec3 worldNormal = b3RotateVector( xfA.q, distanceOutput.normal ); + b3Vec3 worldPointA = b3TransformPoint( xfA, distanceOutput.pointA ); + b3Vec3 worldPointB = b3TransformPoint( xfA, distanceOutput.pointB ); + + output.distanceIterations += 1; + distanceIterations += 1; + + // If the shapes are overlapped, we give up on continuous collision. + if ( distanceOutput.distance <= 0.0f ) + { + output.state = b3_toiStateOverlapped; + output.fraction = 0.0f; + break; + } + + if ( distanceOutput.distance <= target + tolerance ) + { + // Success! + output.state = b3_toiStateHit; + + // Averaged hit point + b3Vec3 pA = b3MulAdd( worldPointA, proxyA.radius, worldNormal ); + b3Vec3 pB = b3MulAdd( worldPointB, -proxyB.radius, worldNormal ); + output.point = b3Lerp( pA, pB, 0.5f ); + output.point = b3Add( output.point, origin ); + output.normal = worldNormal; + output.fraction = t1; + break; + } + + if ( distanceIterations == maxIterations ) + { + // Progress too slow. This can happen when a capsule rotates around a + // triangle vertex. + output.state = b3_toiStateFailed; + output.fraction = t1; + + // Averaged hit point + b3Vec3 pA = b3MulAdd( worldPointA, input->proxyA.radius, worldNormal ); + b3Vec3 pB = b3MulAdd( worldPointB, -input->proxyB.radius, worldNormal ); + output.point = b3Lerp( pA, pB, 0.5f ); + output.point = b3Add( output.point, origin ); + output.normal = worldNormal; + break; + } + + // Initialize the separating axis. + b3SeparationFunction function = + b3MakeSeparationFunction( cache, &proxyA, &sweepA, &proxyB, &sweepB, worldNormal, t1 ); + +#if B3_ENABLE_VALIDATION && 0 + // todo this can give a negative value for diagonal edge contact on faces, typical GJK problem + // to fix this I think the separation function would need to identify faces + { + int index1, index2; + float minSeparation = b3FindMinSeparation( &function, &index1, &index2, t1 ); + // SAT should give a closer result than GJK + B3_VALIDATE( minSeparation > target - tolerance && minSeparation - distanceOutput.distance < 0.1f * B3_LINEAR_SLOP ); + } +#endif + + // Compute the TOI on the separating axis. We do this by successively resolving the deepest point. + bool done = false; + float t2 = tMax; + int pushBackIterations = 0; + for ( ;; ) + { + int indexA, indexB; + float s2 = b3FindMinSeparation( &function, &indexA, &indexB, t2 ); + + // Dump the function seen by the root finder + // for ( int Step = 0; Step <= 100; ++Step ) + // { + // float Alpha = 0.01f * Step; + // float Separation = Function.Evaluate( Index1, Index2, Alpha ); + // + // b3Report( "s(%4.2g) = %g\n", Alpha, Separation ); + // } + + // Is the final configuration separated? + if ( s2 - target > tolerance ) + { + // Success! + output.state = b3_toiStateSeparated; + output.fraction = input->maxFraction; + done = true; + break; + } + + // Has the separation reached tolerance? + if ( s2 >= target - tolerance ) + { + // Advance the sweeps + t1 = t2; + break; + } + + // Compute the initial separation of the witness points + float s1 = b3EvaluateSeparation( &function, indexA, indexB, t1 ); + + // Check for overlap. This might happen if the root finder runs out of iterations. + if ( s1 < target - tolerance ) + { + // Failed! + B3_VALIDATE( false ); + output.state = b3_toiStateFailed; + output.fraction = t1; + done = true; + break; + } + + // Has the separation reached tolerance? + if ( s1 <= target + tolerance ) + { + // Success! t1 should hold the TOI (could be 0.0) + output.state = b3_toiStateHit; + output.fraction = t1; + done = true; + break; + } + + // Compute 1D root of: f(x) - target = 0 + int rootIterationCount = 0; + int maxRootIterations = 50; + float a1 = t1; + float a2 = t2; + for ( ;; ) + { + // Use a mix of false position and bisection. + float t; + if ( rootIterationCount & 1 ) + { + // False position to improve convergence. + t = a1 + ( target - s1 ) * ( a2 - a1 ) / ( s2 - s1 ); + } + else + { + // Bisection to guarantee progress. + t = 0.5f * ( a1 + a2 ); + } + + output.rootIterations += 1; + rootIterationCount += 1; + + float s = b3EvaluateSeparation( &function, indexA, indexB, t ); + + // Has the separation reached tolerance? + if ( b3AbsFloat( s - target ) <= tolerance ) + { + // t2 holds a tentative value for t1 + t2 = t; + break; + } + + // Ensure we continue to bracket the root. + if ( s > target ) + { + a1 = t; + s1 = s; + } + else + { + a2 = t; + s2 = s; + } + + if ( rootIterationCount == maxRootIterations ) + { + B3_VALIDATE( false ); + break; + } + } + + // Restart the inner loop if we have a failing edge case. + if ( rootIterationCount == maxRootIterations - 1 && function.type == b3_separationEdges ) + { + B3_VALIDATE( false ); + + rootIterationCount = 0; + t2 = input->maxFraction; + b3ForceFixedAxis( &function, t1 ); + B3_ASSERT( function.type != b3_separationEdges ); + } + + output.pushBackIterations += 1; + pushBackIterations += 1; + + if ( pushBackIterations == maxPushBackIterations ) + { + break; + } + } + + if ( done ) + { + // Averaged hit point + b3Vec3 pA = b3MulAdd( worldPointA, input->proxyA.radius, worldNormal ); + b3Vec3 pB = b3MulAdd( worldPointB, -input->proxyB.radius, worldNormal ); + output.point = b3Lerp( pA, pB, 0.5f ); + output.point = b3Add( output.point, origin ); + output.normal = worldNormal; + break; + } + } + + // It is expected that the state and fraction are set before reaching this + B3_ASSERT( output.state != b3_toiStateUnknown ); + B3_ASSERT( output.fraction >= 0.0f ); + + return output; +} diff --git a/vendor/box3d/src/src/distance_joint.c b/vendor/box3d/src/src/distance_joint.c new file mode 100644 index 000000000..9185010ae --- /dev/null +++ b/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 ); + } +} diff --git a/vendor/box3d/src/src/dynamic_tree.c b/vendor/box3d/src/src/dynamic_tree.c new file mode 100644 index 000000000..98370dc05 --- /dev/null +++ b/vendor/box3d/src/src/dynamic_tree.c @@ -0,0 +1,2186 @@ +// SPDX-FileCopyrightText: 2025 Erin Catto +// SPDX-License-Identifier: MIT + +#include "aabb.h" +#include "core.h" +#include "joint.h" +#include "simd.h" + +#include "box3d/collision.h" +#include "box3d/constants.h" +#include "box3d/math_functions.h" + +#include +#include +#include + +#define B3_TREE_STACK_SIZE 1024 + +static b3TreeNode b3_defaultTreeNode = { + .aabb = { { 0.0f, 0.0f, 0.0f }, { 0.0f, 0.0f, 0.0f } }, + .categoryBits = B3_DEFAULT_CATEGORY_BITS, + .children = + { + .child1 = B3_NULL_INDEX, + .child2 = B3_NULL_INDEX, + }, + .parent = B3_NULL_INDEX, + .height = 0, + .flags = b3_allocatedNode, +}; + +static inline bool b3IsLeaf( const b3TreeNode* node ) +{ + return node->flags & b3_leafNode; +} + +static inline bool b3IsAllocated( const b3TreeNode* node ) +{ + return node->flags & b3_allocatedNode; +} + +static inline uint16_t b3MaxUInt16( uint16_t a, uint16_t b ) +{ + return a > b ? a : b; +} + +b3DynamicTree b3DynamicTree_Create( int proxyCapacity ) +{ + int capacity = b3MaxInt( proxyCapacity, 16 ); + + b3DynamicTree tree; + + // memset needed for deterministic serialization + memset( &tree, 0, sizeof( b3DynamicTree ) ); + + tree.version = B3_DYNAMIC_TREE_VERSION; + tree.root = B3_NULL_INDEX; + + // maximum node count for a full binary tree is 2 * leafCount - 1 + tree.nodeCapacity = 2 * capacity - 1; + tree.nodeCount = 0; + + tree.nodes = (b3TreeNode*)b3Alloc( tree.nodeCapacity * sizeof( b3TreeNode ) ); + + memset( tree.nodes, 0, tree.nodeCapacity * sizeof( b3TreeNode ) ); + + // Build a linked list for the free list. + // todo use a bump allocator until the capacity is consumed (see b3PoolAllocator) + for ( int i = 0; i < tree.nodeCapacity - 1; ++i ) + { + tree.nodes[i].next = i + 1; + } + + tree.nodes[tree.nodeCapacity - 1].next = B3_NULL_INDEX; + tree.freeList = 0; + + tree.proxyCount = 0; + + tree.leafIndices = NULL; + tree.leafBoxes = NULL; + tree.leafCenters = NULL; + tree.binIndices = NULL; + tree.rebuildCapacity = 0; + + return tree; +} + +void b3DynamicTree_Destroy( b3DynamicTree* tree ) +{ + b3Free( tree->nodes, tree->nodeCapacity * sizeof( b3TreeNode ) ); + b3Free( tree->leafIndices, tree->rebuildCapacity * sizeof( int ) ); + b3Free( tree->leafBoxes, tree->rebuildCapacity * sizeof( b3AABB ) ); + b3Free( tree->leafCenters, tree->rebuildCapacity * sizeof( b3Vec3 ) ); + b3Free( tree->binIndices, tree->rebuildCapacity * sizeof( int ) ); + + memset( tree, 0, sizeof( b3DynamicTree ) ); +} + +// Allocate a node from the pool. Grow the pool if necessary. +static int b3AllocateNode( b3DynamicTree* tree ) +{ + // Expand the node pool as needed. + if ( tree->freeList == B3_NULL_INDEX ) + { + B3_ASSERT( tree->nodeCount == tree->nodeCapacity ); + + // The free list is empty. Rebuild a bigger pool. + b3TreeNode* oldNodes = tree->nodes; + int oldCapacity = tree->nodeCapacity; + tree->nodeCapacity += oldCapacity >> 1; + tree->nodes = (b3TreeNode*)b3Alloc( tree->nodeCapacity * sizeof( b3TreeNode ) ); + B3_ASSERT( oldNodes != NULL ); + memcpy( tree->nodes, oldNodes, tree->nodeCount * sizeof( b3TreeNode ) ); + memset( tree->nodes + tree->nodeCount, 0, ( tree->nodeCapacity - tree->nodeCount ) * sizeof( b3TreeNode ) ); + b3Free( oldNodes, oldCapacity * sizeof( b3TreeNode ) ); + + // Build a linked list for the free list. The parent pointer becomes the "next" pointer. + // todo avoid building freelist? + for ( int i = tree->nodeCount; i < tree->nodeCapacity - 1; ++i ) + { + tree->nodes[i].next = i + 1; + } + + tree->nodes[tree->nodeCapacity - 1].next = B3_NULL_INDEX; + tree->freeList = tree->nodeCount; + } + + // Peel a node off the free list. + int nodeIndex = tree->freeList; + b3TreeNode* node = tree->nodes + nodeIndex; + tree->freeList = node->next; + *node = b3_defaultTreeNode; + ++tree->nodeCount; + return nodeIndex; +} + +// Return a node to the pool. +static void b3FreeNode( b3DynamicTree* tree, int nodeId ) +{ + B3_ASSERT( 0 <= nodeId && nodeId < tree->nodeCapacity ); + B3_ASSERT( 0 < tree->nodeCount ); + tree->nodes[nodeId].next = tree->freeList; + tree->nodes[nodeId].flags = 0; + tree->freeList = nodeId; + --tree->nodeCount; +} + +// Greedy algorithm for sibling selection using the SAH +// We have three nodes A-(B,C) and want to add a leaf D, there are three choices. +// 1: make a new parent for A and D : E-(A-(B,C), D) +// 2: associate D with B +// a: B is a leaf : A-(E-(B,D), C) +// b: B is an internal node: A-(B{D},C) +// 3: associate D with C +// a: C is a leaf : A-(B, E-(C,D)) +// b: C is an internal node: A-(B, C{D}) +// All of these have a clear cost except when B or C is an internal node. Hence we need to be greedy. + +// The cost for cases 1, 2a, and 3a can be computed using the sibling cost formula. +// cost of sibling H = area(union(H, D)) + increased area of ancestors + +// Suppose B (or C) is an internal node, then the lowest cost would be one of two cases: +// case1: D becomes a sibling of B +// case2: D becomes a descendant of B along with a new internal node of area(D). +static int b3FindBestSibling( const b3DynamicTree* tree, b3AABB boxD ) +{ + b3Vec3 centerD = b3AABB_Center( boxD ); + float areaD = b3Perimeter( boxD ); + + const b3TreeNode* nodes = tree->nodes; + int rootIndex = tree->root; + + b3AABB rootBox = nodes[rootIndex].aabb; + + // Area of current node + float areaBase = b3Perimeter( rootBox ); + + // Area of inflated node + float directCost = b3Perimeter( b3AABB_Union( rootBox, boxD ) ); + float inheritedCost = 0.0f; + + int bestSibling = rootIndex; + float bestCost = directCost; + + // Descend the tree from root, following a single greedy path. + int index = rootIndex; + while ( b3IsLeaf( nodes + index ) == false ) + { + int child1 = nodes[index].children.child1; + int child2 = nodes[index].children.child2; + + // Cost of creating a new parent for this node and the new leaf + float cost = directCost + inheritedCost; + + // Sometimes there are multiple identical costs within tolerance. + // This breaks the ties using the centroid distance. + if ( cost < bestCost ) + { + bestSibling = index; + bestCost = cost; + } + + // Inheritance cost seen by children + inheritedCost += directCost - areaBase; + + bool leaf1 = b3IsLeaf( nodes + child1 ); + bool leaf2 = b3IsLeaf( nodes + child2 ); + + // Cost of descending into child 1 + float lowerCost1 = FLT_MAX; + b3AABB box1 = nodes[child1].aabb; + float directCost1 = b3Perimeter( b3AABB_Union( box1, boxD ) ); + float area1 = 0.0f; + if ( leaf1 ) + { + // Child 1 is a leaf + // Cost of creating new node and increasing area of node P + float cost1 = directCost1 + inheritedCost; + + // Need this here due to while condition above + if ( cost1 < bestCost ) + { + bestSibling = child1; + bestCost = cost1; + } + } + else + { + // Child 1 is an internal node + area1 = b3Perimeter( box1 ); + + // Lower bound cost of inserting under child 1. + lowerCost1 = inheritedCost + directCost1 + b3MinFloat( areaD - area1, 0.0f ); + } + + // Cost of descending into child 2 + float lowerCost2 = FLT_MAX; + b3AABB box2 = nodes[child2].aabb; + float directCost2 = b3Perimeter( b3AABB_Union( box2, boxD ) ); + float area2 = 0.0f; + if ( leaf2 ) + { + // Child 2 is a leaf + // Cost of creating new node and increasing area of node P + float cost2 = directCost2 + inheritedCost; + + // Need this here due to while condition above + if ( cost2 < bestCost ) + { + bestSibling = child2; + bestCost = cost2; + } + } + else + { + // Child 2 is an internal node + area2 = b3Perimeter( box2 ); + + // Lower bound cost of inserting under child 2. This is not the cost + // of child 2, it is the best we can hope for under child 2. + lowerCost2 = inheritedCost + directCost2 + b3MinFloat( areaD - area2, 0.0f ); + } + + if ( leaf1 && leaf2 ) + { + break; + } + + // Can the cost possibly be decreased? + if ( bestCost <= lowerCost1 && bestCost <= lowerCost2 ) + { + break; + } + + if ( lowerCost1 == lowerCost2 && leaf1 == false ) + { + B3_ASSERT( lowerCost1 < FLT_MAX ); + B3_ASSERT( lowerCost2 < FLT_MAX ); + + // No clear choice based on lower bound surface area. This can happen when both + // children fully contain D. Fall back to node distance. + b3Vec3 d1 = b3Sub( b3AABB_Center( box1 ), centerD ); + b3Vec3 d2 = b3Sub( b3AABB_Center( box2 ), centerD ); + lowerCost1 = b3LengthSquared( d1 ); + lowerCost2 = b3LengthSquared( d2 ); + } + + // Descend + if ( lowerCost1 < lowerCost2 && leaf1 == false ) + { + index = child1; + areaBase = area1; + directCost = directCost1; + } + else + { + index = child2; + areaBase = area2; + directCost = directCost2; + } + + B3_ASSERT( b3IsLeaf( nodes + index ) == false ); + } + + return bestSibling; +} + +enum b3RotateType +{ + b3_rotateNone, + b3_rotateBF, + b3_rotateBG, + b3_rotateCD, + b3_rotateCE +}; + +// Perform a left or right rotation if node A is imbalanced. +// Returns the new root index. +static void b3RotateNodes( b3DynamicTree* tree, int iA ) +{ + B3_ASSERT( iA != B3_NULL_INDEX ); + + b3TreeNode* nodes = tree->nodes; + + b3TreeNode* A = nodes + iA; + if ( b3IsLeaf( A ) == true ) + { + return; + } + + int iB = A->children.child1; + int iC = A->children.child2; + B3_ASSERT( 0 <= iB && iB < tree->nodeCapacity ); + B3_ASSERT( 0 <= iC && iC < tree->nodeCapacity ); + + b3TreeNode* B = nodes + iB; + b3TreeNode* C = nodes + iC; + + bool isLeafB = b3IsLeaf( B ); + bool isLeafC = b3IsLeaf( C ); + + if ( isLeafB == true && isLeafC == false ) + { + int iF = C->children.child1; + int iG = C->children.child2; + b3TreeNode* F = nodes + iF; + b3TreeNode* G = nodes + iG; + B3_ASSERT( 0 <= iF && iF < tree->nodeCapacity ); + B3_ASSERT( 0 <= iG && iG < tree->nodeCapacity ); + + // Base cost + float costBase = b3Perimeter( C->aabb ); + + // Cost of swapping B and F + b3AABB aabbBG = b3AABB_Union( B->aabb, G->aabb ); + float costBF = b3Perimeter( aabbBG ); + + // Cost of swapping B and G + b3AABB aabbBF = b3AABB_Union( B->aabb, F->aabb ); + float costBG = b3Perimeter( aabbBF ); + + if ( costBase < costBF && costBase < costBG ) + { + // Rotation does not improve cost + return; + } + + if ( costBF < costBG ) + { + // Swap B and F + A->children.child1 = iF; + C->children.child1 = iB; + + B->parent = iC; + F->parent = iA; + + C->aabb = aabbBG; + + C->height = 1 + b3MaxUInt16( B->height, G->height ); + A->height = 1 + b3MaxUInt16( C->height, F->height ); + C->categoryBits = B->categoryBits | G->categoryBits; + A->categoryBits = C->categoryBits | F->categoryBits; + C->flags |= ( B->flags | G->flags ) & b3_enlargedNode; + A->flags |= ( C->flags | F->flags ) & b3_enlargedNode; + } + else + { + // Swap B and G + A->children.child1 = iG; + C->children.child2 = iB; + + B->parent = iC; + G->parent = iA; + + C->aabb = aabbBF; + + C->height = 1 + b3MaxUInt16( B->height, F->height ); + A->height = 1 + b3MaxUInt16( C->height, G->height ); + C->categoryBits = B->categoryBits | F->categoryBits; + A->categoryBits = C->categoryBits | G->categoryBits; + C->flags |= ( B->flags | F->flags ) & b3_enlargedNode; + A->flags |= ( C->flags | G->flags ) & b3_enlargedNode; + } + } + else if ( isLeafC == true && isLeafB == false ) + { + // C is a leaf and B is internal + + int iD = B->children.child1; + int iE = B->children.child2; + b3TreeNode* D = nodes + iD; + b3TreeNode* E = nodes + iE; + B3_ASSERT( 0 <= iD && iD < tree->nodeCapacity ); + B3_ASSERT( 0 <= iE && iE < tree->nodeCapacity ); + + // Base cost + float costBase = b3Perimeter( B->aabb ); + + // Cost of swapping C and D + b3AABB aabbCE = b3AABB_Union( C->aabb, E->aabb ); + float costCD = b3Perimeter( aabbCE ); + + // Cost of swapping C and E + b3AABB aabbCD = b3AABB_Union( C->aabb, D->aabb ); + float costCE = b3Perimeter( aabbCD ); + + if ( costBase < costCD && costBase < costCE ) + { + // Rotation does not improve cost + return; + } + + if ( costCD < costCE ) + { + // Swap C and D + A->children.child2 = iD; + B->children.child1 = iC; + + C->parent = iB; + D->parent = iA; + + B->aabb = aabbCE; + + B->height = 1 + b3MaxUInt16( C->height, E->height ); + A->height = 1 + b3MaxUInt16( B->height, D->height ); + B->categoryBits = C->categoryBits | E->categoryBits; + A->categoryBits = B->categoryBits | D->categoryBits; + B->flags |= ( C->flags | E->flags ) & b3_enlargedNode; + A->flags |= ( B->flags | D->flags ) & b3_enlargedNode; + } + else + { + // Swap C and E + A->children.child2 = iE; + B->children.child2 = iC; + + C->parent = iB; + E->parent = iA; + + B->aabb = aabbCD; + + B->height = 1 + b3MaxUInt16( C->height, D->height ); + A->height = 1 + b3MaxUInt16( B->height, E->height ); + B->categoryBits = C->categoryBits | D->categoryBits; + A->categoryBits = B->categoryBits | E->categoryBits; + B->flags |= ( C->flags | D->flags ) & b3_enlargedNode; + A->flags |= ( B->flags | E->flags ) & b3_enlargedNode; + } + } + else if ( isLeafB == false && isLeafC == false ) + { + // All grand children exist so there are many options for rotation + int iD = B->children.child1; + int iE = B->children.child2; + int iF = C->children.child1; + int iG = C->children.child2; + + B3_ASSERT( 0 <= iD && iD < tree->nodeCapacity ); + B3_ASSERT( 0 <= iE && iE < tree->nodeCapacity ); + B3_ASSERT( 0 <= iF && iF < tree->nodeCapacity ); + B3_ASSERT( 0 <= iG && iG < tree->nodeCapacity ); + + b3TreeNode* D = nodes + iD; + b3TreeNode* E = nodes + iE; + b3TreeNode* F = nodes + iF; + b3TreeNode* G = nodes + iG; + + // Base cost + float areaB = b3Perimeter( B->aabb ); + float areaC = b3Perimeter( C->aabb ); + float costBase = areaB + areaC; + enum b3RotateType bestRotation = b3_rotateNone; + float bestCost = costBase; + + // Cost of swapping B and F + b3AABB aabbBG = b3AABB_Union( B->aabb, G->aabb ); + float costBF = areaB + b3Perimeter( aabbBG ); + if ( costBF < bestCost ) + { + bestRotation = b3_rotateBF; + bestCost = costBF; + } + + // Cost of swapping B and G + b3AABB aabbBF = b3AABB_Union( B->aabb, F->aabb ); + float costBG = areaB + b3Perimeter( aabbBF ); + if ( costBG < bestCost ) + { + bestRotation = b3_rotateBG; + bestCost = costBG; + } + + // Cost of swapping C and D + b3AABB aabbCE = b3AABB_Union( C->aabb, E->aabb ); + float costCD = areaC + b3Perimeter( aabbCE ); + if ( costCD < bestCost ) + { + bestRotation = b3_rotateCD; + bestCost = costCD; + } + + // Cost of swapping C and E + b3AABB aabbCD = b3AABB_Union( C->aabb, D->aabb ); + float costCE = areaC + b3Perimeter( aabbCD ); + if ( costCE < bestCost ) + { + bestRotation = b3_rotateCE; + // bestCost = costCE; + } + + switch ( bestRotation ) + { + case b3_rotateNone: + break; + + case b3_rotateBF: + A->children.child1 = iF; + C->children.child1 = iB; + + B->parent = iC; + F->parent = iA; + + C->aabb = aabbBG; + + C->height = 1 + b3MaxUInt16( B->height, G->height ); + A->height = 1 + b3MaxUInt16( C->height, F->height ); + C->categoryBits = B->categoryBits | G->categoryBits; + A->categoryBits = C->categoryBits | F->categoryBits; + C->flags |= ( B->flags | G->flags ) & b3_enlargedNode; + A->flags |= ( C->flags | F->flags ) & b3_enlargedNode; + break; + + case b3_rotateBG: + A->children.child1 = iG; + C->children.child2 = iB; + + B->parent = iC; + G->parent = iA; + + C->aabb = aabbBF; + + C->height = 1 + b3MaxUInt16( B->height, F->height ); + A->height = 1 + b3MaxUInt16( C->height, G->height ); + C->categoryBits = B->categoryBits | F->categoryBits; + A->categoryBits = C->categoryBits | G->categoryBits; + C->flags |= ( B->flags | F->flags ) & b3_enlargedNode; + A->flags |= ( C->flags | G->flags ) & b3_enlargedNode; + break; + + case b3_rotateCD: + A->children.child2 = iD; + B->children.child1 = iC; + + C->parent = iB; + D->parent = iA; + + B->aabb = aabbCE; + + B->height = 1 + b3MaxUInt16( C->height, E->height ); + A->height = 1 + b3MaxUInt16( B->height, D->height ); + B->categoryBits = C->categoryBits | E->categoryBits; + A->categoryBits = B->categoryBits | D->categoryBits; + B->flags |= ( C->flags | E->flags ) & b3_enlargedNode; + A->flags |= ( B->flags | D->flags ) & b3_enlargedNode; + break; + + case b3_rotateCE: + A->children.child2 = iE; + B->children.child2 = iC; + + C->parent = iB; + E->parent = iA; + + B->aabb = aabbCD; + + B->height = 1 + b3MaxUInt16( C->height, D->height ); + A->height = 1 + b3MaxUInt16( B->height, E->height ); + B->categoryBits = C->categoryBits | D->categoryBits; + A->categoryBits = B->categoryBits | E->categoryBits; + B->flags |= ( C->flags | D->flags ) & b3_enlargedNode; + A->flags |= ( B->flags | E->flags ) & b3_enlargedNode; + break; + + default: + B3_ASSERT( false ); + break; + } + } +} + +// It would be nicer if the root had zero height but maintaining this would drastically increase +// insertion cost because whole sub-trees would need the height to be updated. +static void b3InsertLeaf( b3DynamicTree* tree, int leaf, bool shouldRotate ) +{ + if ( tree->root == B3_NULL_INDEX ) + { + tree->root = leaf; + tree->nodes[tree->root].parent = B3_NULL_INDEX; + return; + } + + // Stage 1: find the best sibling for this node + b3AABB leafAABB = tree->nodes[leaf].aabb; + int sibling = b3FindBestSibling( tree, leafAABB ); + + // Stage 2: create a new parent for the leaf and sibling + int oldParent = tree->nodes[sibling].parent; + int newParent = b3AllocateNode( tree ); + + // warning: node pointer can change after allocation + b3TreeNode* nodes = tree->nodes; + nodes[newParent].parent = oldParent; + nodes[newParent].userData = UINT64_MAX; + nodes[newParent].aabb = b3AABB_Union( leafAABB, nodes[sibling].aabb ); + nodes[newParent].categoryBits = nodes[leaf].categoryBits | nodes[sibling].categoryBits; + nodes[newParent].height = nodes[sibling].height + 1; + + if ( oldParent != B3_NULL_INDEX ) + { + // The sibling was not the root. + if ( nodes[oldParent].children.child1 == sibling ) + { + nodes[oldParent].children.child1 = newParent; + } + else + { + nodes[oldParent].children.child2 = newParent; + } + + nodes[newParent].children.child1 = sibling; + nodes[newParent].children.child2 = leaf; + nodes[sibling].parent = newParent; + nodes[leaf].parent = newParent; + } + else + { + // The sibling was the root. + nodes[newParent].children.child1 = sibling; + nodes[newParent].children.child2 = leaf; + nodes[sibling].parent = newParent; + nodes[leaf].parent = newParent; + tree->root = newParent; + } + + // Stage 3: walk back up the tree fixing heights and AABBs + int index = nodes[leaf].parent; + while ( index != B3_NULL_INDEX ) + { + int child1 = nodes[index].children.child1; + int child2 = nodes[index].children.child2; + + B3_ASSERT( child1 != B3_NULL_INDEX ); + B3_ASSERT( child2 != B3_NULL_INDEX ); + + nodes[index].aabb = b3AABB_Union( nodes[child1].aabb, nodes[child2].aabb ); + nodes[index].categoryBits = nodes[child1].categoryBits | nodes[child2].categoryBits; + nodes[index].height = 1 + b3MaxUInt16( nodes[child1].height, nodes[child2].height ); + nodes[index].flags |= ( nodes[child1].flags | nodes[child2].flags ) & b3_enlargedNode; + + if ( shouldRotate ) + { + b3RotateNodes( tree, index ); + } + + index = nodes[index].parent; + } +} + +static void b3RemoveLeaf( b3DynamicTree* tree, int leaf ) +{ + if ( leaf == tree->root ) + { + tree->root = B3_NULL_INDEX; + return; + } + + b3TreeNode* nodes = tree->nodes; + + int parent = nodes[leaf].parent; + int grandParent = nodes[parent].parent; + int sibling; + if ( nodes[parent].children.child1 == leaf ) + { + sibling = nodes[parent].children.child2; + } + else + { + sibling = nodes[parent].children.child1; + } + + if ( grandParent != B3_NULL_INDEX ) + { + // Destroy parent and connect sibling to grandParent. + if ( nodes[grandParent].children.child1 == parent ) + { + nodes[grandParent].children.child1 = sibling; + } + else + { + nodes[grandParent].children.child2 = sibling; + } + nodes[sibling].parent = grandParent; + b3FreeNode( tree, parent ); + + // Adjust ancestor bounds. + int index = grandParent; + while ( index != B3_NULL_INDEX ) + { + b3TreeNode* node = nodes + index; + b3TreeNode* child1 = nodes + node->children.child1; + b3TreeNode* child2 = nodes + node->children.child2; + + // Fast union using SSE + //__m128 aabb1 = _mm_load_ps(&child1->aabb.lowerBound.x); + //__m128 aabb2 = _mm_load_ps(&child2->aabb.lowerBound.x); + //__m128 lower = _mm_min_ps(aabb1, aabb2); + //__m128 upper = _mm_max_ps(aabb1, aabb2); + //__m128 aabb = _mm_shuffle_ps(lower, upper, _MM_SHUFFLE(3, 2, 1, 0)); + //_mm_store_ps(&node->aabb.lowerBound.x, aabb); + + node->aabb = b3AABB_Union( child1->aabb, child2->aabb ); + node->categoryBits = child1->categoryBits | child2->categoryBits; + node->height = 1 + b3MaxUInt16( child1->height, child2->height ); + + index = node->parent; + } + } + else + { + tree->root = sibling; + tree->nodes[sibling].parent = B3_NULL_INDEX; + b3FreeNode( tree, parent ); + } +} + +// Create a proxy in the tree as a leaf node. We return the index of the node instead of a pointer so that we can grow +// the node pool. +int b3DynamicTree_CreateProxy( b3DynamicTree* tree, b3AABB aabb, uint64_t categoryBits, uint64_t userData ) +{ + B3_ASSERT( b3IsValidAABB( aabb ) ); + + int proxyId = b3AllocateNode( tree ); + b3TreeNode* node = tree->nodes + proxyId; + + node->aabb = aabb; + node->userData = userData; + node->categoryBits = categoryBits; + node->height = 0; + node->flags = b3_allocatedNode | b3_leafNode; + + bool shouldRotate = true; + b3InsertLeaf( tree, proxyId, shouldRotate ); + + tree->proxyCount += 1; + + return proxyId; +} + +void b3DynamicTree_DestroyProxy( b3DynamicTree* tree, int proxyId ) +{ + B3_ASSERT( 0 <= proxyId && proxyId < tree->nodeCapacity ); + B3_ASSERT( b3IsLeaf( tree->nodes + proxyId ) ); + + b3RemoveLeaf( tree, proxyId ); + b3FreeNode( tree, proxyId ); + + B3_ASSERT( tree->proxyCount > 0 ); + tree->proxyCount -= 1; +} + +int b3DynamicTree_GetProxyCount( const b3DynamicTree* tree ) +{ + return tree->proxyCount; +} + +void b3DynamicTree_MoveProxy( b3DynamicTree* tree, int proxyId, b3AABB aabb ) +{ + B3_ASSERT( b3IsValidAABB( aabb ) ); + B3_ASSERT( 0 <= proxyId && proxyId < tree->nodeCapacity ); + B3_ASSERT( b3IsLeaf( tree->nodes + proxyId ) ); + + b3RemoveLeaf( tree, proxyId ); + + tree->nodes[proxyId].aabb = aabb; + + bool shouldRotate = false; + b3InsertLeaf( tree, proxyId, shouldRotate ); +} + +void b3DynamicTree_EnlargeProxy( b3DynamicTree* tree, int proxyId, b3AABB aabb ) +{ + b3TreeNode* nodes = tree->nodes; + B3_VALIDATE( b3IsValidAABB( aabb ) ); + B3_ASSERT( 0 <= proxyId && proxyId < tree->nodeCapacity ); + B3_VALIDATE( b3IsLeaf( tree->nodes + proxyId ) ); + + // Caller must ensure this + B3_VALIDATE( b3AABB_Contains( nodes[proxyId].aabb, aabb ) == false ); + + b3TreeNode* node = nodes + proxyId; + node->aabb = aabb; + + int parentIndex = node->parent; + while ( parentIndex != B3_NULL_INDEX ) + { + node = nodes + parentIndex; + bool changed = b3EnlargeAABB( &node->aabb, aabb ); + + // todo not sure why this node is marked as enlarged even if it didn't change + node->flags |= b3_enlargedNode; + + parentIndex = node->parent; + + if ( changed == false ) + { + break; + } + } + + while ( parentIndex != B3_NULL_INDEX ) + { + node = nodes + parentIndex; + if ( node->flags & b3_enlargedNode ) + { + // early out because this ancestor was previously ascended and marked as enlarged + break; + } + + node->flags |= b3_enlargedNode; + parentIndex = node->parent; + } +} + +void b3DynamicTree_SetCategoryBits( b3DynamicTree* tree, int proxyId, uint64_t categoryBits ) +{ + b3TreeNode* nodes = tree->nodes; + + B3_ASSERT( b3IsLeaf( nodes + proxyId ) ); + + nodes[proxyId].categoryBits = categoryBits; + + // Fix up category bits in ancestor internal nodes + int nodeIndex = nodes[proxyId].parent; + while ( nodeIndex != B3_NULL_INDEX ) + { + b3TreeNode* node = nodes + nodeIndex; + int child1 = node->children.child1; + B3_ASSERT( child1 != B3_NULL_INDEX ); + int child2 = node->children.child2; + B3_ASSERT( child2 != B3_NULL_INDEX ); + node->categoryBits = nodes[child1].categoryBits | nodes[child2].categoryBits; + + nodeIndex = node->parent; + } +} + +uint64_t b3DynamicTree_GetCategoryBits( b3DynamicTree* tree, int proxyId ) +{ + B3_ASSERT( 0 <= proxyId && proxyId < tree->nodeCapacity ); + return tree->nodes[proxyId].categoryBits; +} + +int b3DynamicTree_GetHeight( const b3DynamicTree* tree ) +{ + if ( tree->root == B3_NULL_INDEX ) + { + return 0; + } + + return tree->nodes[tree->root].height; +} + +float b3DynamicTree_GetAreaRatio( const b3DynamicTree* tree ) +{ + if ( tree->root == B3_NULL_INDEX ) + { + return 0.0f; + } + + const b3TreeNode* root = tree->nodes + tree->root; + float rootArea = b3Perimeter( root->aabb ); + + float totalArea = 0.0f; + for ( int i = 0; i < tree->nodeCapacity; ++i ) + { + const b3TreeNode* node = tree->nodes + i; + if ( b3IsAllocated( node ) == false || b3IsLeaf( node ) || i == tree->root ) + { + continue; + } + + totalArea += b3Perimeter( node->aabb ); + } + + return totalArea / rootArea; +} + +b3AABB b3DynamicTree_GetRootBounds( const b3DynamicTree* tree ) +{ + if ( tree->root != B3_NULL_INDEX ) + { + return tree->nodes[tree->root].aabb; + } + + b3AABB empty = { b3Vec3_zero, b3Vec3_zero }; + return empty; +} + +#if B3_ENABLE_VALIDATION +// Compute the height of a sub-tree. +static int b3ComputeHeightRecurse( const b3DynamicTree* tree, int nodeId ) +{ + B3_ASSERT( 0 <= nodeId && nodeId < tree->nodeCapacity ); + b3TreeNode* node = tree->nodes + nodeId; + + if ( b3IsLeaf( node ) ) + { + return 0; + } + + int height1 = b3ComputeHeightRecurse( tree, node->children.child1 ); + int height2 = b3ComputeHeightRecurse( tree, node->children.child2 ); + return 1 + b3MaxInt( height1, height2 ); +} + +static int b3ComputeHeight( const b3DynamicTree* tree ) +{ + int height = b3ComputeHeightRecurse( tree, tree->root ); + return height; +} + +static void b3ValidateStructure( const b3DynamicTree* tree, int index ) +{ + if ( index == B3_NULL_INDEX ) + { + return; + } + + if ( index == tree->root ) + { + B3_ASSERT( tree->nodes[index].parent == B3_NULL_INDEX ); + } + + const b3TreeNode* node = tree->nodes + index; + + B3_ASSERT( node->flags == 0 || ( node->flags & b3_allocatedNode ) != 0 ); + + if ( b3IsLeaf( node ) ) + { + B3_ASSERT( node->height == 0 ); + return; + } + + int child1 = node->children.child1; + int child2 = node->children.child2; + + B3_ASSERT( 0 <= child1 && child1 < tree->nodeCapacity ); + B3_ASSERT( 0 <= child2 && child2 < tree->nodeCapacity ); + + B3_ASSERT( tree->nodes[child1].parent == index ); + B3_ASSERT( tree->nodes[child2].parent == index ); + + if ( ( tree->nodes[child1].flags | tree->nodes[child2].flags ) & b3_enlargedNode ) + { + B3_ASSERT( node->flags & b3_enlargedNode ); + } + + b3ValidateStructure( tree, child1 ); + b3ValidateStructure( tree, child2 ); +} + +static void b3ValidateMetrics( const b3DynamicTree* tree, int index ) +{ + if ( index == B3_NULL_INDEX ) + { + return; + } + + const b3TreeNode* node = tree->nodes + index; + + B3_VALIDATE( b3IsValidAABB( node->aabb ) ); + + if ( b3IsLeaf( node ) ) + { + B3_ASSERT( node->height == 0 ); + return; + } + + int child1 = node->children.child1; + int child2 = node->children.child2; + + B3_ASSERT( 0 <= child1 && child1 < tree->nodeCapacity ); + B3_ASSERT( 0 <= child2 && child2 < tree->nodeCapacity ); + + int height1 = tree->nodes[child1].height; + int height2 = tree->nodes[child2].height; + int height = 1 + b3MaxInt( height1, height2 ); + B3_ASSERT( node->height == height ); + + // b3AABB aabb = b3AABB_Union(tree->nodes[child1].aabb, tree->nodes[child2].aabb); + + B3_ASSERT( b3AABB_Contains( node->aabb, tree->nodes[child1].aabb ) ); + B3_ASSERT( b3AABB_Contains( node->aabb, tree->nodes[child2].aabb ) ); + + // B3_ASSERT(aabb.lowerBound.x == node->aabb.lowerBound.x); + // B3_ASSERT(aabb.lowerBound.y == node->aabb.lowerBound.y); + // B3_ASSERT(aabb.upperBound.x == node->aabb.upperBound.x); + // B3_ASSERT(aabb.upperBound.y == node->aabb.upperBound.y); + + uint64_t categoryBits = tree->nodes[child1].categoryBits | tree->nodes[child2].categoryBits; + B3_ASSERT( node->categoryBits == categoryBits ); + + b3ValidateMetrics( tree, child1 ); + b3ValidateMetrics( tree, child2 ); +} +#endif + +void b3DynamicTree_Validate( const b3DynamicTree* tree ) +{ +#if B3_ENABLE_VALIDATION + if ( tree->root == B3_NULL_INDEX ) + { + return; + } + + b3ValidateStructure( tree, tree->root ); + b3ValidateMetrics( tree, tree->root ); + + int freeCount = 0; + int freeIndex = tree->freeList; + while ( freeIndex != B3_NULL_INDEX ) + { + B3_ASSERT( 0 <= freeIndex && freeIndex < tree->nodeCapacity ); + freeIndex = tree->nodes[freeIndex].next; + ++freeCount; + } + + int height = b3DynamicTree_GetHeight( tree ); + int computedHeight = b3ComputeHeight( tree ); + B3_ASSERT( height == computedHeight ); + + B3_ASSERT( tree->nodeCount + freeCount == tree->nodeCapacity ); +#else + B3_UNUSED( tree ); +#endif +} + +void b3DynamicTree_ValidateNoEnlarged( const b3DynamicTree* tree ) +{ +#if B3_ENABLE_VALIDATION == 1 + int capacity = tree->nodeCapacity; + const b3TreeNode* nodes = tree->nodes; + for ( int i = 0; i < capacity; ++i ) + { + const b3TreeNode* node = nodes + i; + if ( node->flags & b3_allocatedNode ) + { + B3_ASSERT( ( node->flags & b3_enlargedNode ) == 0 ); + } + } +#else + B3_UNUSED( tree ); +#endif +} + +int b3DynamicTree_GetByteCount( const b3DynamicTree* tree ) +{ + size_t size = sizeof( b3DynamicTree ) + sizeof( b3TreeNode ) * tree->nodeCapacity + + tree->rebuildCapacity * ( sizeof( int ) + sizeof( b3AABB ) + sizeof( b3Vec3 ) + sizeof( int ) ); + + return (int)size; +} + +b3TreeStats b3DynamicTree_Query( const b3DynamicTree* tree, b3AABB aabb, uint64_t maskBits, bool requireAllBits, + b3TreeQueryCallbackFcn* callback, void* context ) +{ + b3TreeStats result = { 0 }; + + if ( tree->nodeCount == 0 ) + { + return result; + } + + int stack[B3_TREE_STACK_SIZE]; + int stackCount = 0; + stack[stackCount++] = tree->root; + + while ( stackCount > 0 ) + { + int nodeId = stack[--stackCount]; + if ( nodeId == B3_NULL_INDEX ) + { + // todo huh? + B3_ASSERT( false ); + continue; + } + + const b3TreeNode* node = tree->nodes + nodeId; + result.nodeVisits += 1; + + // Assuming branch prediction deals with requireAllBits well + uint64_t bitMatch = requireAllBits ? ( node->categoryBits & maskBits ) == maskBits : ( node->categoryBits & maskBits ); + + if ( bitMatch && b3AABB_Overlaps( node->aabb, aabb ) ) + { + if ( b3IsLeaf( node ) ) + { + // callback to user code with proxy id + bool proceed = callback( nodeId, node->userData, context ); + result.leafVisits += 1; + + if ( proceed == false ) + { + return result; + } + } + else + { + B3_ASSERT( stackCount < B3_TREE_STACK_SIZE - 1 ); + if ( stackCount < B3_TREE_STACK_SIZE - 1 ) + { + stack[stackCount++] = node->children.child1; + stack[stackCount++] = node->children.child2; + } + } + } + } + + return result; +} + +B3_FORCE_INLINE float b3DistanceToNodeSqr( b3Vec3 point, const b3TreeNode* node ) +{ + b3Vec3 r = b3Sub( point, b3Clamp( point, node->aabb.lowerBound, node->aabb.upperBound ) ); + return b3Dot( r, r ); +} + +struct b3QueryClosestItem +{ + int nodeIndex; + float distanceToNodeSqr; +}; + +b3TreeStats b3DynamicTree_QueryClosest( const b3DynamicTree* tree, b3Vec3 point, uint64_t maskBits, bool requireAllBits, + b3TreeQueryClosestCallbackFcn* callback, void* context, float* minDistanceSqr ) +{ + b3TreeStats result = { 0 }; + + if ( tree->nodeCount == 0 ) + { + return result; + } + + float minSqr = *minDistanceSqr; + struct b3QueryClosestItem stack[B3_TREE_STACK_SIZE]; + int stackCount = 0; + + float rootDistanceSqr = b3DistanceToNodeSqr( point, tree->nodes + tree->root ); + stack[stackCount++] = (struct b3QueryClosestItem){ + .nodeIndex = tree->root, + .distanceToNodeSqr = rootDistanceSqr, + }; + + while ( stackCount > 0 ) + { + struct b3QueryClosestItem item = stack[--stackCount]; + const b3TreeNode* node = tree->nodes + item.nodeIndex; + result.nodeVisits += 1; + + uint64_t bitMatch = requireAllBits ? ( node->categoryBits & maskBits ) == maskBits : ( node->categoryBits & maskBits ); + + if ( bitMatch ) + { + if ( item.distanceToNodeSqr < minSqr ) + { + if ( b3IsLeaf( node ) ) + { + // callback to user code with minimum distance squared so far and proxy id + float dd = callback( minSqr, item.nodeIndex, node->userData, context ); + + if ( dd < minSqr ) + { + minSqr = dd; + } + + result.leafVisits += 1; + } + else + { + B3_ASSERT( stackCount < B3_TREE_STACK_SIZE - 1 ); + if ( stackCount < B3_TREE_STACK_SIZE - 1 ) + { + int child1 = node->children.child1; + int child2 = node->children.child2; + + // Store the distance to node in the stack instead of recomputing after pop + struct b3QueryClosestItem item1 = { + .nodeIndex = child1, + .distanceToNodeSqr = b3DistanceToNodeSqr( point, tree->nodes + child1 ), + }; + + struct b3QueryClosestItem item2 = { + .nodeIndex = child2, + .distanceToNodeSqr = b3DistanceToNodeSqr( point, tree->nodes + child2 ), + }; + + // Ensure we iterate the closest child first as we pop off the stack + if ( item2.distanceToNodeSqr < item1.distanceToNodeSqr ) + { + stack[stackCount++] = item1; + stack[stackCount++] = item2; + } + else + { + stack[stackCount++] = item2; + stack[stackCount++] = item1; + } + } + } + } + } + } + + *minDistanceSqr = minSqr; + + return result; +} + +b3TreeStats b3DynamicTree_RayCast( const b3DynamicTree* tree, const b3RayCastInput* input, uint64_t maskBits, bool requireAllBits, + b3TreeRayCastCallbackFcn* callback, void* context ) +{ + b3TreeStats result = { 0 }; + + if ( tree->nodeCount == 0 ) + { + return result; + } + + b3Vec3 p1 = input->origin; + b3Vec3 d = input->translation; + + b3V32 pv1 = b3LoadV( &p1.x ); + b3V32 dv = b3LoadV( &d.x ); + + float maxFraction = input->maxFraction; + + b3Vec3 p2 = b3MulAdd( p1, maxFraction, d ); + + // Build a bounding box for the segment. + b3AABB segmentAABB = { b3Min( p1, p2 ), b3Max( p1, p2 ) }; + + int stack[B3_TREE_STACK_SIZE]; + int stackCount = 0; + stack[stackCount++] = tree->root; + + const b3TreeNode* nodes = tree->nodes; + + b3RayCastInput subInput = *input; + + while ( stackCount > 0 ) + { + int nodeId = stack[--stackCount]; + if ( nodeId == B3_NULL_INDEX ) + { + // todo is this possible? + B3_ASSERT( false ); + continue; + } + + const b3TreeNode* node = nodes + nodeId; + result.nodeVisits += 1; + + b3AABB nodeAABB = node->aabb; + + // todo look at disassembly + uint64_t bitMatch = requireAllBits ? ( node->categoryBits & maskBits ) == maskBits : ( node->categoryBits & maskBits ); + + if ( bitMatch == 0 || b3AABB_Overlaps( nodeAABB, segmentAABB ) == false ) + { + continue; + } + + b3V32 lower = b3LoadV( &nodeAABB.lowerBound.x ); + b3V32 upper = b3LoadV( &nodeAABB.upperBound.x ); + + bool edgeOverlap = b3TestBoundsRayOverlap( lower, upper, pv1, dv ); + if ( edgeOverlap == false ) + { + continue; + } + + if ( b3IsLeaf( node ) ) + { + subInput.maxFraction = maxFraction; + + float value = callback( &subInput, nodeId, node->userData, context ); + result.leafVisits += 1; + + // The user may return -1 to indicate this shape should be skipped + + if ( value == 0.0f ) + { + // The client has terminated the ray cast. + return result; + } + + if ( 0.0f < value && value <= maxFraction ) + { + // Update segment bounding box. + maxFraction = value; + p2 = b3MulAdd( p1, maxFraction, d ); + segmentAABB.lowerBound = b3Min( p1, p2 ); + segmentAABB.upperBound = b3Max( p1, p2 ); + } + } + else + { + B3_ASSERT( stackCount < B3_TREE_STACK_SIZE - 1 ); + if ( stackCount < B3_TREE_STACK_SIZE - 1 ) + { + b3Vec3 c1 = b3AABB_Center( nodes[node->children.child1].aabb ); + b3Vec3 c2 = b3AABB_Center( nodes[node->children.child2].aabb ); + if ( b3DistanceSquared( c1, p1 ) < b3DistanceSquared( c2, p1 ) ) + { + stack[stackCount++] = node->children.child2; + stack[stackCount++] = node->children.child1; + } + else + { + stack[stackCount++] = node->children.child1; + stack[stackCount++] = node->children.child2; + } + } + } + } + + return result; +} + +b3TreeStats b3DynamicTree_BoxCast( const b3DynamicTree* tree, const b3BoxCastInput* input, uint64_t maskBits, bool requireAllBits, + b3TreeBoxCastCallbackFcn* callback, void* context ) +{ + b3TreeStats stats = { 0 }; + + if ( tree->nodeCount == 0 ) + { + return stats; + } + + // The caller folds the shape radius and the world origin into the box + b3AABB originAABB = input->box; + + b3Vec3 p1 = b3AABB_Center( originAABB ); + b3Vec3 extension = b3AABB_Extents( originAABB ); + + b3Vec3 d = input->translation; + + b3V32 pv1 = b3LoadV( &p1.x ); + b3V32 dv = b3LoadV( &d.x ); + b3V32 ev = b3LoadV( &extension.x ); + + float maxFraction = input->maxFraction; + + // Build total box for the cast + b3Vec3 t = b3MulSV( maxFraction, input->translation ); + b3AABB totalAABB = { + b3Min( originAABB.lowerBound, b3Add( originAABB.lowerBound, t ) ), + b3Max( originAABB.upperBound, b3Add( originAABB.upperBound, t ) ), + }; + + b3BoxCastInput subInput = *input; + const b3TreeNode* nodes = tree->nodes; + + int stack[B3_TREE_STACK_SIZE]; + int stackCount = 0; + stack[stackCount++] = tree->root; + + while ( stackCount > 0 ) + { + int nodeId = stack[--stackCount]; + if ( nodeId == B3_NULL_INDEX ) + { + B3_ASSERT( false ); + continue; + } + + const b3TreeNode* node = nodes + nodeId; + stats.nodeVisits += 1; + + uint64_t bitMatch = requireAllBits ? ( node->categoryBits & maskBits ) == maskBits : ( node->categoryBits & maskBits ); + + if ( bitMatch == 0 || b3AABB_Overlaps( node->aabb, totalAABB ) == false ) + { + continue; + } + + // radius extension is added to the node in this case + b3V32 lower = b3SubV( b3LoadV( &node->aabb.lowerBound.x ), ev ); + b3V32 upper = b3AddV( b3LoadV( &node->aabb.upperBound.x ), ev ); + bool edgeOverlap = b3TestBoundsRayOverlap( lower, upper, pv1, dv ); + if ( edgeOverlap == false ) + { + continue; + } + + if ( b3IsLeaf( node ) ) + { + subInput.maxFraction = maxFraction; + + float value = callback( &subInput, nodeId, node->userData, context ); + stats.leafVisits += 1; + + if ( value == 0.0f ) + { + // The client has terminated the cast. + return stats; + } + + if ( 0.0f < value && value < maxFraction ) + { + maxFraction = value; + t = b3MulSV( maxFraction, input->translation ); + totalAABB.lowerBound = b3Min( originAABB.lowerBound, b3Add( originAABB.lowerBound, t ) ); + totalAABB.upperBound = b3Max( originAABB.upperBound, b3Add( originAABB.upperBound, t ) ); + } + } + else + { + B3_ASSERT( stackCount < B3_TREE_STACK_SIZE - 1 ); + if ( stackCount < B3_TREE_STACK_SIZE - 1 ) + { + b3Vec3 c1 = b3AABB_Center( nodes[node->children.child1].aabb ); + b3Vec3 c2 = b3AABB_Center( nodes[node->children.child2].aabb ); + if ( b3DistanceSquared( c1, p1 ) < b3DistanceSquared( c2, p1 ) ) + { + stack[stackCount++] = node->children.child2; + stack[stackCount++] = node->children.child1; + } + else + { + stack[stackCount++] = node->children.child1; + stack[stackCount++] = node->children.child2; + } + } + } + } + + return stats; +} + +// Median split == 0, Surface area heuristic == 1 +#define B3_TREE_HEURISTIC 0 + +#if B3_TREE_HEURISTIC == 0 + +// Median split heuristic +static int b3PartitionMid( int* indices, b3Vec3* centers, int count ) +{ + // Handle trivial case + if ( count <= 2 ) + { + return count / 2; + } + + b3Vec3 lowerBound = centers[0]; + b3Vec3 upperBound = centers[0]; + + for ( int i = 1; i < count; ++i ) + { + lowerBound = b3Min( lowerBound, centers[i] ); + upperBound = b3Max( upperBound, centers[i] ); + } + + b3Vec3 d = b3Sub( upperBound, lowerBound ); + b3Vec3 c = b3MulSV( 0.5f, b3Add( lowerBound, upperBound ) ); + + // Partition longest axis using the Hoare partition scheme + // https://en.wikipedia.org/wiki/Quicksort + // https://nicholasvadivelu.com/2021/01/11/array-partition/ + int i1 = 0, i2 = count; + if ( d.x >= d.y && d.x >= d.z ) + { + float pivot = c.x; + + while ( i1 < i2 ) + { + while ( i1 < i2 && centers[i1].x < pivot ) + { + i1 += 1; + }; + + while ( i1 < i2 && centers[i2 - 1].x >= pivot ) + { + i2 -= 1; + }; + + if ( i1 < i2 ) + { + // Swap indices + { + int temp = indices[i1]; + indices[i1] = indices[i2 - 1]; + indices[i2 - 1] = temp; + } + + // Swap centers + { + b3Vec3 temp = centers[i1]; + centers[i1] = centers[i2 - 1]; + centers[i2 - 1] = temp; + } + + i1 += 1; + i2 -= 1; + } + } + } + else if ( d.y >= d.z ) + { + float pivot = c.y; + + while ( i1 < i2 ) + { + while ( i1 < i2 && centers[i1].y < pivot ) + { + i1 += 1; + }; + + while ( i1 < i2 && centers[i2 - 1].y >= pivot ) + { + i2 -= 1; + }; + + if ( i1 < i2 ) + { + // Swap indices + { + int temp = indices[i1]; + indices[i1] = indices[i2 - 1]; + indices[i2 - 1] = temp; + } + + // Swap centers + { + b3Vec3 temp = centers[i1]; + centers[i1] = centers[i2 - 1]; + centers[i2 - 1] = temp; + } + + i1 += 1; + i2 -= 1; + } + } + } + else + { + float pivot = c.z; + + while ( i1 < i2 ) + { + while ( i1 < i2 && centers[i1].z < pivot ) + { + i1 += 1; + }; + + while ( i1 < i2 && centers[i2 - 1].z >= pivot ) + { + i2 -= 1; + }; + + if ( i1 < i2 ) + { + // Swap indices + { + int temp = indices[i1]; + indices[i1] = indices[i2 - 1]; + indices[i2 - 1] = temp; + } + + // Swap centers + { + b3Vec3 temp = centers[i1]; + centers[i1] = centers[i2 - 1]; + centers[i2 - 1] = temp; + } + + i1 += 1; + i2 -= 1; + } + } + } + B3_ASSERT( i1 == i2 ); + + if ( i1 > 0 && i1 < count ) + { + return i1; + } + + return count / 2; +} + +#else + +#define B3_BIN_COUNT 8 + +typedef struct b3TreeBin +{ + b3AABB aabb; + int count; +} b3TreeBin; + +typedef struct b3TreePlane +{ + b3AABB leftAABB; + b3AABB rightAABB; + int leftCount; + int rightCount; +} b3TreePlane; + +// "On Fast Construction of SAH-based Bounding Volume Hierarchies" by Ingo Wald +// Returns the left child count +static int b3PartitionSAH( int* indices, int* binIndices, b3AABB* boxes, int count ) +{ + B3_ASSERT( count > 0 ); + + b3TreeBin bins[B3_BIN_COUNT]; + b3TreePlane planes[B3_BIN_COUNT - 1]; + + b3Vec3 center = b3AABB_Center( boxes[0] ); + b3AABB centroidAABB; + centroidAABB.lowerBound = center; + centroidAABB.upperBound = center; + + for ( int i = 1; i < count; ++i ) + { + center = b3AABB_Center( boxes[i] ); + centroidAABB.lowerBound = b3Min( centroidAABB.lowerBound, center ); + centroidAABB.upperBound = b3Max( centroidAABB.upperBound, center ); + } + + b3Vec3 d = b3Sub( centroidAABB.upperBound, centroidAABB.lowerBound ); + + // Find longest axis + int axisIndex; + float invD; + if ( d.x > d.y ) + { + axisIndex = 0; + invD = d.x; + } + else + { + axisIndex = 1; + invD = d.y; + } + + invD = invD > 0.0f ? 1.0f / invD : 0.0f; + + // Initialize bin bounds and count + for ( int i = 0; i < B3_BIN_COUNT; ++i ) + { + bins[i].aabb.lowerBound = (b3Vec3){ FLT_MAX, FLT_MAX }; + bins[i].aabb.upperBound = (b3Vec3){ -FLT_MAX, -FLT_MAX }; + bins[i].count = 0; + } + + // Assign boxes to bins and compute bin boxes + // TODO_ERIN optimize + float binCount = B3_BIN_COUNT; + float lowerBoundArray[2] = { centroidAABB.lowerBound.x, centroidAABB.lowerBound.y }; + float minC = lowerBoundArray[axisIndex]; + for ( int i = 0; i < count; ++i ) + { + b3Vec3 c = b3AABB_Center( boxes[i] ); + float cArray[2] = { c.x, c.y }; + int binIndex = (int)( binCount * ( cArray[axisIndex] - minC ) * invD ); + binIndex = b3ClampInt( binIndex, 0, B3_BIN_COUNT - 1 ); + binIndices[i] = binIndex; + bins[binIndex].count += 1; + bins[binIndex].aabb = b3AABB_Union( bins[binIndex].aabb, boxes[i] ); + } + + int planeCount = B3_BIN_COUNT - 1; + + // Prepare all the left planes, candidates for left child + planes[0].leftCount = bins[0].count; + planes[0].leftAABB = bins[0].aabb; + for ( int i = 1; i < planeCount; ++i ) + { + planes[i].leftCount = planes[i - 1].leftCount + bins[i].count; + planes[i].leftAABB = b3AABB_Union( planes[i - 1].leftAABB, bins[i].aabb ); + } + + // Prepare all the right planes, candidates for right child + planes[planeCount - 1].rightCount = bins[planeCount].count; + planes[planeCount - 1].rightAABB = bins[planeCount].aabb; + for ( int i = planeCount - 2; i >= 0; --i ) + { + planes[i].rightCount = planes[i + 1].rightCount + bins[i + 1].count; + planes[i].rightAABB = b3AABB_Union( planes[i + 1].rightAABB, bins[i + 1].aabb ); + } + + // Find best split to minimize SAH + float minCost = FLT_MAX; + int bestPlane = 0; + for ( int i = 0; i < planeCount; ++i ) + { + float leftArea = b3Perimeter( planes[i].leftAABB ); + float rightArea = b3Perimeter( planes[i].rightAABB ); + int leftCount = planes[i].leftCount; + int rightCount = planes[i].rightCount; + + float cost = leftCount * leftArea + rightCount * rightArea; + if ( cost < minCost ) + { + bestPlane = i; + minCost = cost; + } + } + + // Partition node indices and boxes using the Hoare partition scheme + // https://en.wikipedia.org/wiki/Quicksort + // https://nicholasvadivelu.com/2021/01/11/array-partition/ + int i1 = 0, i2 = count; + while ( i1 < i2 ) + { + while ( i1 < i2 && binIndices[i1] < bestPlane ) + { + i1 += 1; + }; + + while ( i1 < i2 && binIndices[i2 - 1] >= bestPlane ) + { + i2 -= 1; + }; + + if ( i1 < i2 ) + { + // Swap indices + { + int temp = indices[i1]; + indices[i1] = indices[i2 - 1]; + indices[i2 - 1] = temp; + } + + // Swap boxes + { + b3AABB temp = boxes[i1]; + boxes[i1] = boxes[i2 - 1]; + boxes[i2 - 1] = temp; + } + + i1 += 1; + i2 -= 1; + } + } + B3_ASSERT( i1 == i2 ); + + if ( i1 > 0 && i1 < count ) + { + return i1; + } + else + { + return count / 2; + } +} + +#endif + +// Temporary data used to track the rebuild of a tree node +struct b3RebuildItem +{ + int nodeIndex; + int childCount; + + // Leaf indices + int startIndex; + int splitIndex; + int endIndex; +}; + +// Returns root node index +static int b3BuildTree( b3DynamicTree* tree, int leafCount ) +{ + b3TreeNode* nodes = tree->nodes; + int* leafIndices = tree->leafIndices; + + if ( leafCount == 1 ) + { + nodes[leafIndices[0]].parent = B3_NULL_INDEX; + return leafIndices[0]; + } + +#if B3_TREE_HEURISTIC == 0 + b3Vec3* leafCenters = tree->leafCenters; +#else + b3AABB* leafBoxes = tree->leafBoxes; + int* binIndices = tree->binIndices; +#endif + + // todo large stack item + struct b3RebuildItem stack[B3_TREE_STACK_SIZE]; + int top = 0; + + stack[0].nodeIndex = b3AllocateNode( tree ); + stack[0].childCount = -1; + stack[0].startIndex = 0; + stack[0].endIndex = leafCount; +#if B3_TREE_HEURISTIC == 0 + stack[0].splitIndex = b3PartitionMid( leafIndices, leafCenters, leafCount ); +#else + stack[0].splitIndex = b3PartitionSAH( leafIndices, binIndices, leafBoxes, leafCount ); +#endif + + while ( true ) + { + struct b3RebuildItem* item = stack + top; + + item->childCount += 1; + + if ( item->childCount == 2 ) + { + // This internal node has both children established + + if ( top == 0 ) + { + // all done + break; + } + + struct b3RebuildItem* parentItem = stack + ( top - 1 ); + b3TreeNode* parentNode = nodes + parentItem->nodeIndex; + + if ( parentItem->childCount == 0 ) + { + B3_ASSERT( parentNode->children.child1 == B3_NULL_INDEX ); + parentNode->children.child1 = item->nodeIndex; + } + else + { + B3_ASSERT( parentItem->childCount == 1 ); + B3_ASSERT( parentNode->children.child2 == B3_NULL_INDEX ); + parentNode->children.child2 = item->nodeIndex; + } + + b3TreeNode* node = nodes + item->nodeIndex; + + B3_ASSERT( node->parent == B3_NULL_INDEX ); + node->parent = parentItem->nodeIndex; + + B3_ASSERT( node->children.child1 != B3_NULL_INDEX ); + B3_ASSERT( node->children.child2 != B3_NULL_INDEX ); + b3TreeNode* child1 = nodes + node->children.child1; + b3TreeNode* child2 = nodes + node->children.child2; + + node->aabb = b3AABB_Union( child1->aabb, child2->aabb ); + node->height = 1 + b3MaxUInt16( child1->height, child2->height ); + node->categoryBits = child1->categoryBits | child2->categoryBits; + + // Pop stack + top -= 1; + } + else + { + int startIndex, endIndex; + if ( item->childCount == 0 ) + { + startIndex = item->startIndex; + endIndex = item->splitIndex; + } + else + { + B3_ASSERT( item->childCount == 1 ); + startIndex = item->splitIndex; + endIndex = item->endIndex; + } + + int count = endIndex - startIndex; + + if ( count == 1 ) + { + int childIndex = leafIndices[startIndex]; + b3TreeNode* node = nodes + item->nodeIndex; + + if ( item->childCount == 0 ) + { + B3_ASSERT( node->children.child1 == B3_NULL_INDEX ); + node->children.child1 = childIndex; + } + else + { + B3_ASSERT( item->childCount == 1 ); + B3_ASSERT( node->children.child2 == B3_NULL_INDEX ); + node->children.child2 = childIndex; + } + + b3TreeNode* childNode = nodes + childIndex; + B3_ASSERT( childNode->parent == B3_NULL_INDEX ); + childNode->parent = item->nodeIndex; + } + else + { + B3_ASSERT( count > 0 ); + B3_ASSERT( top < B3_TREE_STACK_SIZE ); + + top += 1; + struct b3RebuildItem* newItem = stack + top; + newItem->nodeIndex = b3AllocateNode( tree ); + newItem->childCount = -1; + newItem->startIndex = startIndex; + newItem->endIndex = endIndex; +#if B3_TREE_HEURISTIC == 0 + newItem->splitIndex = b3PartitionMid( leafIndices + startIndex, leafCenters + startIndex, count ); +#else + newItem->splitIndex = + b3PartitionSAH( leafIndices + startIndex, binIndices + startIndex, leafBoxes + startIndex, count ); +#endif + newItem->splitIndex += startIndex; + } + } + } + + b3TreeNode* rootNode = nodes + stack[0].nodeIndex; + B3_ASSERT( rootNode->parent == B3_NULL_INDEX ); + B3_ASSERT( rootNode->children.child1 != B3_NULL_INDEX ); + B3_ASSERT( rootNode->children.child2 != B3_NULL_INDEX ); + + b3TreeNode* child1 = nodes + rootNode->children.child1; + b3TreeNode* child2 = nodes + rootNode->children.child2; + + rootNode->aabb = b3AABB_Union( child1->aabb, child2->aabb ); + rootNode->height = 1 + b3MaxUInt16( child1->height, child2->height ); + rootNode->categoryBits = child1->categoryBits | child2->categoryBits; + + return stack[0].nodeIndex; +} + +// Not safe to access tree during this operation because it may grow +int b3DynamicTree_Rebuild( b3DynamicTree* tree, bool fullBuild ) +{ + int proxyCount = tree->proxyCount; + if ( proxyCount == 0 ) + { + return 0; + } + + // Ensure capacity for rebuild space + if ( proxyCount > tree->rebuildCapacity ) + { + int newCapacity = proxyCount + proxyCount / 2; + + b3Free( tree->leafIndices, tree->rebuildCapacity * sizeof( int ) ); + tree->leafIndices = (int*)b3Alloc( newCapacity * sizeof( int ) ); + +#if B3_TREE_HEURISTIC == 0 + b3Free( tree->leafCenters, tree->rebuildCapacity * sizeof( b3Vec3 ) ); + tree->leafCenters = (b3Vec3*)b3Alloc( newCapacity * sizeof( b3Vec3 ) ); +#else + b3Free( tree->leafBoxes, tree->rebuildCapacity * sizeof( b3AABB ) ); + tree->leafBoxes = (b3AABB*)b3Alloc( newCapacity * sizeof( b3AABB ) ); + b3Free( tree->binIndices, tree->rebuildCapacity * sizeof( int ) ); + tree->binIndices = (int*)b3Alloc( newCapacity * sizeof( int ) ); +#endif + tree->rebuildCapacity = newCapacity; + } + + int leafCount = 0; + int stack[B3_TREE_STACK_SIZE]; + int stackCount = 0; + + int nodeIndex = tree->root; + b3TreeNode* nodes = tree->nodes; + b3TreeNode* node = nodes + nodeIndex; + + // These are the nodes that get sorted to rebuild the tree. + // I'm using indices because the node pool may grow during the build. + int* leafIndices = tree->leafIndices; + +#if B3_TREE_HEURISTIC == 0 + b3Vec3* leafCenters = tree->leafCenters; +#else + b3AABB* leafBoxes = tree->leafBoxes; +#endif + + // Gather all proxy nodes that have grown and all internal nodes that haven't grown. Both are + // considered leaves in the tree rebuild. + // Free all internal nodes that have grown. + // todo use a node growth metric instead of simply enlarged to reduce rebuild size and frequency + // this should be weighed against B3_MAX_AABB_MARGIN + while ( true ) + { + if ( b3IsLeaf( node ) == true || ( ( node->flags & b3_enlargedNode ) == 0 && fullBuild == false ) ) + { + leafIndices[leafCount] = nodeIndex; +#if B3_TREE_HEURISTIC == 0 + leafCenters[leafCount] = b3AABB_Center( node->aabb ); +#else + leafBoxes[leafCount] = node->aabb; +#endif + leafCount += 1; + + // Detach + node->parent = B3_NULL_INDEX; + } + else + { + int doomedNodeIndex = nodeIndex; + + // Handle children + nodeIndex = node->children.child1; + + B3_ASSERT( stackCount < B3_TREE_STACK_SIZE ); + if ( stackCount < B3_TREE_STACK_SIZE ) + { + stack[stackCount++] = node->children.child2; + } + + node = nodes + nodeIndex; + + // Remove doomed node + b3FreeNode( tree, doomedNodeIndex ); + + continue; + } + + if ( stackCount == 0 ) + { + break; + } + + nodeIndex = stack[--stackCount]; + node = nodes + nodeIndex; + } + +#if B3_ENABLE_VALIDATION == 1 + int capacity = tree->nodeCapacity; + for ( int i = 0; i < capacity; ++i ) + { + if ( nodes[i].flags & b3_allocatedNode ) + { + B3_ASSERT( ( nodes[i].flags & b3_enlargedNode ) == 0 ); + } + } +#endif + + B3_ASSERT( leafCount <= proxyCount ); + + tree->root = b3BuildTree( tree, leafCount ); + + b3DynamicTree_Validate( tree ); + + return leafCount; +} + +static FILE* b3OpenTreeFile( const char* fileName, const char* mode ) +{ + FILE* file = NULL; + +#if defined( _MSC_VER ) + errno_t e = fopen_s( &file, fileName, mode ); + if ( e != 0 ) + { + return NULL; + } +#else + file = fopen( fileName, mode ); + if ( file == NULL ) + { + return NULL; + } +#endif + + return file; +} + +void b3DynamicTree_Save( const b3DynamicTree* tree, const char* fileName ) +{ + FILE* file = b3OpenTreeFile( fileName, "wb" ); + if ( file == NULL ) + { + return; + } + + // Copy to allow setting some fields to zero + b3DynamicTree temp = *tree; + + // Zero pointers and temp data + temp.nodes = NULL; + temp.leafIndices = NULL; + temp.leafBoxes = NULL; + temp.leafCenters = NULL; + temp.binIndices = NULL; + temp.rebuildCapacity = 0; + + // Write tree struct + fwrite( &temp, sizeof( b3DynamicTree ), 1, file ); + + // Write the node array, this includes the free list + if ( tree->nodeCapacity > 0 && tree->nodes != NULL ) + { + fwrite( tree->nodes, sizeof( b3TreeNode ), tree->nodeCapacity, file ); + } + + fclose( file ); +} + +b3DynamicTree b3DynamicTree_Load( const char* fileName, float scale ) +{ + b3DynamicTree tree = { 0 }; + + FILE* file = b3OpenTreeFile( fileName, "rb" ); + if ( file == NULL ) + { + return tree; + } + + int readCount = (int)fread( &tree, sizeof( b3DynamicTree ), 1, file ); + if ( readCount != 1 ) + { + fclose( file ); + return tree; + } + + if ( tree.version != B3_DYNAMIC_TREE_VERSION ) + { + fclose( file ); + memset( &tree, 0, sizeof( b3DynamicTree ) ); + return tree; + } + + if ( tree.nodeCapacity > 0 ) + { + tree.nodes = (b3TreeNode*)b3Alloc( tree.nodeCapacity * sizeof( b3TreeNode ) ); + readCount = (int)fread( tree.nodes, sizeof( b3TreeNode ), tree.nodeCapacity, file ); + if ( readCount != tree.nodeCapacity ) + { + b3Free( tree.nodes, tree.nodeCapacity * sizeof( b3TreeNode ) ); + fclose( file ); + memset( &tree, 0, sizeof( b3DynamicTree ) ); + return tree; + } + + for ( int i = 0; i < tree.nodeCapacity; ++i ) + { + b3TreeNode* node = tree.nodes + i; + node->aabb.lowerBound = b3MulSV( scale, node->aabb.lowerBound ); + node->aabb.upperBound = b3MulSV( scale, node->aabb.upperBound ); + } + } + else + { + tree.nodes = NULL; + } + + // Zero temp data fields + tree.leafIndices = NULL; + tree.leafBoxes = NULL; + tree.leafCenters = NULL; + tree.binIndices = NULL; + tree.rebuildCapacity = 0; + + fclose( file ); + + return tree; +} diff --git a/vendor/box3d/src/src/height_field.c b/vendor/box3d/src/src/height_field.c new file mode 100644 index 000000000..1b0307fe9 --- /dev/null +++ b/vendor/box3d/src/src/height_field.c @@ -0,0 +1,1583 @@ +// SPDX-FileCopyrightText: 2025 Erin Catto +// SPDX-License-Identifier: MIT + +#include "aabb.h" +#include "algorithm.h" +#include "body.h" +#include "core.h" +#include "shape.h" +#include "simd.h" + +#include "box3d/collision.h" +#include "box3d/constants.h" +#include "box3d/math_functions.h" + +#include +#include +#include + +/* + Convention + + index = row * columnCount + column + height = minHeight + heightScale * compressedHeights[index]; + + column = index % columnCount; + row = index / columnCount; + + x-axis : columns + z-axis : rows + + 00 --- 01 --- 02 --- 03 X + | 0 | 1 | 2 | + 04 --- 05 --- 06 --- 07 + | 3 | 4 | 5 | + 08 --- 09 --- 10 --- 11 + | 6 | 7 | 8 | + 12 --- 13 --- 14 --- 15 + Z + + The quads exist before the column and row ends: row < rowCount - 1 and column < columnCount - 1 + quadIndex = row * (columnCount - 1) + column + + Quad origin index from quad index (needs row): + index = quadIndex + row * columnCount + + Triangle index is related to the quad index + triangleIndex = 2 * quadIndex + (0/1) + quadIndex = triangleIndex / 2 + + Row and column from quad index: + row = quadIndex / (columnCount - 1) + column = quadIndex - row * (columnCount - 1) + + The triangle diagonal is fixed. + + triangle0 = {00, 04, 01} -> {11, 21, 12} + triangle1 = {04, 05, 01} -> {22, 12, 21} + + 11 12 + 00 ---- 01 + | / | + | 0 / 1 | 1 + | / | + 04 ---- 05 + 21 22 + + For adjacency we have + + NA + 00 ---- 01 ---- 02 + | / | / | + NA | 0 / 1 | 2 / 3 | + | / | / | + 04 ---- 05 ---- 06 + | / | / | + NA | 6 / 7 | 8 / 9 | + | / | / | + 08 ---- 09 ---- 10 + + 0: NA, NA, 1 + 1: 0, 6, 2 + + Triangle layouts + + 11 3 12 + 1 ---- 3 + | / + 1 | 0 / 2 + | / + 2 + 21 + + 12 + 2 + / | + 2 / 1 | 1 + / | + 3 ---- 1 + 21 3 22 + */ + +b3HeightFieldData* b3CreateHeightField( const b3HeightFieldDef* data ) +{ + int columnCount = data->countX; + int rowCount = data->countZ; + + int heightCount = columnCount * rowCount; + B3_ASSERT( heightCount >= 4 ); + + int cellCount = ( columnCount - 1 ) * ( rowCount - 1 ); + int triangleCount = 2 * cellCount; + + // Single blob: struct followed by the height, material, and flag arrays. Layout + // mirrors b3HullData/b3MeshData so the recording path can copy it with one memcpy. + size_t byteCount = b3AlignUp8( sizeof( b3HeightFieldData ) ); + int heightsOffset = (int)byteCount; + byteCount += b3AlignUp8( heightCount * sizeof( uint16_t ) ); + int materialOffset = (int)byteCount; + byteCount += b3AlignUp8( cellCount * sizeof( uint8_t ) ); + int flagsOffset = (int)byteCount; + byteCount += b3AlignUp8( triangleCount * sizeof( uint8_t ) ); + + // Zero the whole blob so alignment padding is defined. The construction-time hash + // sweeps raw bytes and would otherwise pick up uninitialized padding. + b3HeightFieldData* hf = (b3HeightFieldData*)b3Alloc( byteCount ); + memset( hf, 0, byteCount ); + + hf->version = B3_HEIGHT_FIELD_VERSION; + hf->byteCount = (int)byteCount; + hf->scale = data->scale; + hf->columnCount = columnCount; + hf->rowCount = rowCount; + hf->heightsOffset = heightsOffset; + hf->materialOffset = materialOffset; + hf->flagsOffset = flagsOffset; + hf->clockwise = data->clockwiseWinding; + + uint16_t* compressedHeights = (uint16_t*)( (intptr_t)hf + heightsOffset ); + uint8_t* materialIndices = (uint8_t*)( (intptr_t)hf + materialOffset ); + uint8_t* flags = (uint8_t*)( (intptr_t)hf + flagsOffset ); + + const float* heights = data->heights; + + B3_ASSERT( data->globalMinimumHeight <= data->globalMaximumHeight ); + hf->minHeight = data->globalMinimumHeight; + hf->maxHeight = data->globalMaximumHeight; + + float height = b3MaxFloat( hf->maxHeight - hf->minHeight, B3_LINEAR_SLOP ); + hf->heightScale = height / UINT16_MAX; + + float lowerHeightBound = hf->maxHeight; + float upperHeightBound = hf->minHeight; + + float invHeightScale = 1.0f / hf->heightScale; + for ( int i = 0; i < heightCount; ++i ) + { + float clampedHeight = b3ClampFloat( heights[i], hf->minHeight, hf->maxHeight ); + float scaledHeight = ( clampedHeight - hf->minHeight ) * invHeightScale; + compressedHeights[i] = (uint16_t)( b3MinFloat( scaledHeight, (float)UINT16_MAX ) ); + + lowerHeightBound = b3MinFloat( lowerHeightBound, clampedHeight ); + upperHeightBound = b3MaxFloat( upperHeightBound, clampedHeight ); + } + + // Use decompressed heights for accurate convexity metrics. + float* decompressedHeights = (float*)b3Alloc( heightCount * sizeof( float ) ); + for ( int i = 0; i < heightCount; ++i ) + { + decompressedHeights[i] = hf->minHeight + hf->heightScale * compressedHeights[i]; + } + heights = decompressedHeights; + + if ( data->materialIndices != NULL ) + { + for ( int i = 0; i < cellCount; ++i ) + { + materialIndices[i] = data->materialIndices[i]; + } + } + else + { + for ( int i = 0; i < cellCount; ++i ) + { + materialIndices[i] = 0; + } + } + + hf->aabb.lowerBound = (b3Vec3){ 0.0f, hf->scale.y * lowerHeightBound, 0.0f }; + hf->aabb.upperBound = + (b3Vec3){ hf->scale.x * ( hf->columnCount - 1 ), hf->scale.y * upperHeightBound, hf->scale.z * ( hf->rowCount - 1 ) }; + + float cos5Deg = 0.9962f; + b3Vec3 scale = hf->scale; + + int triangleIndex = 0; + for ( int row = 0; row < rowCount - 1; ++row ) + { + for ( int column = 0; column < columnCount - 1; ++column ) + { + // todo compute convexity flags + // This requires a couple things + // - determine all 3 adjacent triangles for each triangle + // - consider clockwise winding + // - consider borders where there is no adjacent triangle + + int triangleIndex1 = triangleIndex; + int triangleIndex2 = triangleIndex + 1; + triangleIndex += 2; + + int cellIndex = row * ( columnCount - 1 ) + column; + + if ( materialIndices[cellIndex] == B3_HEIGHT_FIELD_HOLE ) + { + continue; + } + + int flags1 = 0; + int flags2 = 0; + + b3Plane plane1, plane2; + b3Vec3 center1, center2; + + int index11 = row * columnCount + column; + int index12 = index11 + 1; + int index21 = ( row + 1 ) * columnCount + column; + int index22 = index21 + 1; + + { + float height11 = heights[index11]; + float height12 = heights[index12]; + float height21 = heights[index21]; + float height22 = heights[index22]; + + float x1 = (float)( column ); + float x2 = (float)( column + 1 ); + float z1 = (float)( row ); + float z2 = (float)( row + 1 ); + + // triangle 0 : 11, 21, 12 + b3Vec3 vs0[3]; + vs0[0] = b3Mul( scale, (b3Vec3){ x1, height11, z1 } ); + vs0[1] = b3Mul( scale, (b3Vec3){ x1, height21, z2 } ); + vs0[2] = b3Mul( scale, (b3Vec3){ x2, height12, z1 } ); + plane1 = b3MakePlaneFromPoints( vs0[0], vs0[1], vs0[2] ); + + center1 = b3MulSV( 1.0f / 3.0f, b3Add( b3Add( vs0[0], vs0[1] ), vs0[2] ) ); + + // triangle 1 : 22, 12, 21 + b3Vec3 vs1[3]; + vs1[0] = b3Mul( scale, (b3Vec3){ x2, height22, z2 } ); + vs1[1] = b3Mul( scale, (b3Vec3){ x2, height12, z1 } ); + vs1[2] = b3Mul( scale, (b3Vec3){ x1, height21, z2 } ); + plane2 = b3MakePlaneFromPoints( vs1[0], vs1[1], vs1[2] ); + + center2 = b3MulSV( 1.0f / 3.0f, b3Add( b3Add( vs1[0], vs1[1] ), vs1[2] ) ); + + float separation = b3PlaneSeparation( plane1, vs1[0] ); + float cosAngle = b3Dot( plane1.normal, plane2.normal ); + if ( separation > 0.0f || cosAngle > cos5Deg ) + { + flags1 |= b3_concaveEdge2; + flags2 |= b3_concaveEdge2; + } + if ( separation < 0.0f || cosAngle > cos5Deg ) + { + flags1 |= b3_inverseConcaveEdge2; + flags2 |= b3_inverseConcaveEdge2; + } + } + + B3_UNUSED( center1 ); + B3_UNUSED( center2 ); + + // top + int topCellIndex = ( row - 1 ) * ( columnCount - 1 ) + column; + if ( row > 0 && materialIndices[topCellIndex] != B3_HEIGHT_FIELD_HOLE ) + { + B3_ASSERT( 0 <= topCellIndex && topCellIndex < cellCount ); + + int r = row - 1; + int c = column; + + int i11 = r * columnCount + c; + int i12 = i11 + 1; + int i21 = ( r + 1 ) * columnCount + c; + int i22 = i21 + 1; + + B3_ASSERT( i21 == index11 ); + B3_ASSERT( i22 == index12 ); + + // float h11 = heights[i11]; + float h12 = heights[i12]; + float h21 = heights[i21]; + float h22 = heights[i22]; + + float x1 = (float)( c ); + float x2 = (float)( c + 1 ); + float z1 = (float)( r ); + float z2 = (float)( r + 1 ); + + // triangle 1 + b3Vec3 vs[3]; + vs[0] = b3Mul( scale, (b3Vec3){ x2, h22, z2 } ); + vs[1] = b3Mul( scale, (b3Vec3){ x2, h12, z1 } ); + vs[2] = b3Mul( scale, (b3Vec3){ x1, h21, z2 } ); + + b3Vec3 n = b3Normalize( b3Cross( b3Sub( vs[1], vs[0] ), b3Sub( vs[2], vs[0] ) ) ); + + float separation = b3PlaneSeparation( plane1, vs[1] ); + float cosAngle = b3Dot( plane1.normal, n ); + if ( separation > 0.0f || cosAngle > cos5Deg ) + { + flags1 |= b3_concaveEdge3; + } + if ( separation < 0.0f || cosAngle > cos5Deg ) + { + flags1 |= b3_inverseConcaveEdge3; + } + } + + int bottomCellIndex = ( row + 1 ) * ( columnCount - 1 ) + column; + if ( row + 1 < rowCount - 1 && materialIndices[bottomCellIndex] != B3_HEIGHT_FIELD_HOLE ) + { + B3_ASSERT( 0 <= bottomCellIndex && bottomCellIndex < cellCount ); + + int r = row + 1; + int c = column; + + int i11 = r * columnCount + c; + int i12 = i11 + 1; + int i21 = ( r + 1 ) * columnCount + c; + // int i22 = i21 + 1; + + B3_ASSERT( i11 == index21 ); + B3_ASSERT( i12 == index22 ); + + float h11 = heights[i11]; + float h12 = heights[i12]; + float h21 = heights[i21]; + // float h22 = heights[i22]; + + float x1 = (float)( c ); + float x2 = (float)( c + 1 ); + float z1 = (float)( r ); + float z2 = (float)( r + 1 ); + + // triangle 0 + b3Vec3 vs[3]; + vs[0] = b3Mul( scale, (b3Vec3){ x1, h11, z1 } ); + vs[1] = b3Mul( scale, (b3Vec3){ x1, h21, z2 } ); + vs[2] = b3Mul( scale, (b3Vec3){ x2, h12, z1 } ); + + b3Vec3 n = b3Normalize( b3Cross( b3Sub( vs[1], vs[0] ), b3Sub( vs[2], vs[0] ) ) ); + + float separation = b3PlaneSeparation( plane2, vs[1] ); + float cosAngle = b3Dot( plane2.normal, n ); + if ( separation > 0.0f || cosAngle > cos5Deg ) + { + flags2 |= b3_concaveEdge3; + } + if ( separation < 0.0f || cosAngle > cos5Deg ) + { + flags2 |= b3_inverseConcaveEdge3; + } + } + + int leftCellIndex = row * ( columnCount - 1 ) + column - 1; + if ( column - 1 >= 0 && materialIndices[leftCellIndex] != B3_HEIGHT_FIELD_HOLE ) + { + B3_ASSERT( 0 <= leftCellIndex && leftCellIndex < cellCount ); + + int r = row; + int c = column - 1; + + int i11 = r * columnCount + c; + int i12 = i11 + 1; + int i21 = ( r + 1 ) * columnCount + c; + int i22 = i21 + 1; + + B3_ASSERT( i12 == index11 ); + B3_ASSERT( i22 == index21 ); + + // float h11 = heights[i11]; + float h12 = heights[i12]; + float h21 = heights[i21]; + float h22 = heights[i22]; + + float x1 = (float)( c ); + float x2 = (float)( c + 1 ); + float z1 = (float)( r ); + float z2 = (float)( r + 1 ); + + // triangle 1 + b3Vec3 vs[3]; + vs[0] = b3Mul( scale, (b3Vec3){ x2, h22, z2 } ); + vs[1] = b3Mul( scale, (b3Vec3){ x2, h12, z1 } ); + vs[2] = b3Mul( scale, (b3Vec3){ x1, h21, z2 } ); + + b3Vec3 n = b3Normalize( b3Cross( b3Sub( vs[1], vs[0] ), b3Sub( vs[2], vs[0] ) ) ); + + float separation = b3PlaneSeparation( plane1, vs[2] ); + float cosAngle = b3Dot( plane1.normal, n ); + if ( separation > 0.0f || cosAngle > cos5Deg ) + { + flags1 |= b3_concaveEdge1; + } + if ( separation < 0.0f || cosAngle > cos5Deg ) + { + flags1 |= b3_inverseConcaveEdge1; + } + } + + int rightCellIndex = row * ( columnCount - 1 ) + column + 1; + if ( column + 1 < columnCount - 1 && materialIndices[rightCellIndex] != B3_HEIGHT_FIELD_HOLE ) + { + B3_ASSERT( 0 <= rightCellIndex && rightCellIndex < cellCount ); + + int r = row; + int c = column + 1; + + int i11 = r * columnCount + c; + int i12 = i11 + 1; + int i21 = ( r + 1 ) * columnCount + c; + // int i22 = i21 + 1; + + B3_ASSERT( i11 == index12 ); + B3_ASSERT( i21 == index22 ); + + float h11 = heights[i11]; + float h12 = heights[i12]; + float h21 = heights[i21]; + // float h22 = heights[i22]; + + float x1 = (float)( c ); + float x2 = (float)( c + 1 ); + float z1 = (float)( r ); + float z2 = (float)( r + 1 ); + + // triangle 0 + b3Vec3 vs[3]; + vs[0] = b3Mul( scale, (b3Vec3){ x1, h11, z1 } ); + vs[1] = b3Mul( scale, (b3Vec3){ x1, h21, z2 } ); + vs[2] = b3Mul( scale, (b3Vec3){ x2, h12, z1 } ); + + b3Vec3 n = b3Normalize( b3Cross( b3Sub( vs[1], vs[0] ), b3Sub( vs[2], vs[0] ) ) ); + + float separation = b3PlaneSeparation( plane2, vs[2] ); + float cosAngle = b3Dot( plane2.normal, n ); + if ( separation > 0.0f || cosAngle > cos5Deg ) + { + flags2 |= b3_concaveEdge1; + } + if ( separation < 0.0f || cosAngle > cos5Deg ) + { + flags2 |= b3_inverseConcaveEdge1; + } + } + + B3_ASSERT( 0 <= flags1 && flags1 <= UINT8_MAX ); + B3_ASSERT( 0 <= flags2 && flags2 <= UINT8_MAX ); + + flags[triangleIndex1] = (uint8_t)flags1; + flags[triangleIndex2] = (uint8_t)flags2; + } + } + + B3_ASSERT( triangleIndex == triangleCount ); + + b3Free( decompressedHeights, heightCount * sizeof( float ) ); + + // Content hash over the whole blob with the hash field zeroed, like b3HullData/b3MeshData. + hf->hash = 0; + hf->hash = b3NonZeroHash( b3Hash( B3_HASH_INIT, (const uint8_t*)hf, hf->byteCount ) ); + + return hf; +} + +_Static_assert( b3_concaveEdge3 == 4 * b3_concaveEdge1, "bit math" ); +_Static_assert( b3_inverseConcaveEdge3 == 4 * b3_inverseConcaveEdge1, "bit math" ); + +// Decode the four corner vertices of a height field cell into local space. +// Output order matches the index naming used throughout this file: +// corners[0] = (column, row), corners[1] = (column + 1, row), +// corners[2] = (column, row + 1), corners[3] = (column + 1, row + 1). +static inline void b3GetHeightFieldCellCorners( const b3HeightFieldData* hf, int row, int column, b3Vec3 corners[4] ) +{ + B3_ASSERT( 0 <= row && row < hf->rowCount - 1 && 0 <= column && column < hf->columnCount - 1 ); + + int columnCount = hf->columnCount; + int index11 = row * columnCount + column; + int index12 = index11 + 1; + int index21 = ( row + 1 ) * columnCount + column; + int index22 = index21 + 1; + + float minHeight = hf->minHeight; + float heightScale = hf->heightScale; + const uint16_t* heights = b3GetHeightFieldCompressedHeights( hf ); + + float height11 = minHeight + heightScale * heights[index11]; + float height12 = minHeight + heightScale * heights[index12]; + float height21 = minHeight + heightScale * heights[index21]; + float height22 = minHeight + heightScale * heights[index22]; + + float x1 = (float)( column ); + float x2 = (float)( column + 1 ); + float z1 = (float)( row ); + float z2 = (float)( row + 1 ); + + b3Vec3 scale = hf->scale; + corners[0] = b3Mul( scale, (b3Vec3){ x1, height11, z1 } ); + corners[1] = b3Mul( scale, (b3Vec3){ x2, height12, z1 } ); + corners[2] = b3Mul( scale, (b3Vec3){ x1, height21, z2 } ); + corners[3] = b3Mul( scale, (b3Vec3){ x2, height22, z2 } ); +} + +b3Triangle b3GetHeightFieldTriangle( const b3HeightFieldData* heightField, int triangleIndex ) +{ + B3_ASSERT( 0 <= triangleIndex ); + B3_ASSERT( triangleIndex < 2 * ( heightField->columnCount - 1 ) * ( heightField->rowCount - 1 ) ); + + b3Triangle triangle; + triangle.flags = b3GetHeightFieldFlags( heightField )[triangleIndex]; + + int columnCount = heightField->columnCount; + int quadIndex = triangleIndex >> 1; + int row = quadIndex / ( columnCount - 1 ); + int column = quadIndex - row * ( columnCount - 1 ); + + int index11 = row * columnCount + column; + int index12 = index11 + 1; + int index21 = ( row + 1 ) * columnCount + column; + int index22 = index21 + 1; + + int cellIndex = row * ( columnCount - 1 ) + column; + + B3_ASSERT( quadIndex == cellIndex ); + B3_ASSERT( b3GetHeightFieldMaterialIndices( heightField )[cellIndex] != B3_HEIGHT_FIELD_HOLE ); + B3_UNUSED( cellIndex ); + + b3Vec3 corners[4]; + b3GetHeightFieldCellCorners( heightField, row, column, corners ); + + if ( ( triangleIndex & 1 ) == 0 ) + { + triangle.vertices[0] = corners[0]; + triangle.vertices[1] = corners[2]; + triangle.vertices[2] = corners[1]; + triangle.i1 = index11; + triangle.i2 = index21; + triangle.i3 = index12; + } + else + { + triangle.vertices[0] = corners[3]; + triangle.vertices[1] = corners[1]; + triangle.vertices[2] = corners[2]; + triangle.i1 = index22; + triangle.i2 = index12; + triangle.i3 = index21; + } + + if ( heightField->clockwise ) + { + B3_SWAP( triangle.vertices[1], triangle.vertices[2] ); + B3_SWAP( triangle.i2, triangle.i3 ); + + // Reversing winding swaps edge1 and edge3; edge2 (the diagonal) is preserved. + int flags = triangle.flags; + int edge1Bits = flags & ( b3_concaveEdge1 | b3_inverseConcaveEdge1 ); + int edge3Bits = flags & ( b3_concaveEdge3 | b3_inverseConcaveEdge3 ); + flags &= ~( b3_concaveEdge1 | b3_concaveEdge3 | b3_inverseConcaveEdge1 | b3_inverseConcaveEdge3 ); + flags |= edge1Bits << 2; + flags |= edge3Bits >> 2; + triangle.flags = flags; + } + + return triangle; +} + +int b3GetHeightFieldMaterial( const b3HeightFieldData* heightField, int triangleIndex ) +{ + B3_ASSERT( 0 <= triangleIndex ); + B3_ASSERT( triangleIndex < 2 * ( heightField->columnCount - 1 ) * ( heightField->rowCount - 1 ) ); + + int cellIndex = triangleIndex >> 1; + return b3GetHeightFieldMaterialIndices( heightField )[cellIndex]; +} + +b3AABB b3ComputeHeightFieldAABB( const b3HeightFieldData* shape, b3Transform transform ) +{ + return b3AABB_Transform( transform, shape->aabb ); +} + +b3CastOutput b3RayCastHeightField( const b3HeightFieldData* heightField, const b3RayCastInput* input ) +{ + b3ShapeCastInput shapeCastInput = { 0 }; + shapeCastInput.proxy = (b3ShapeProxy){ &input->origin, 1, 0.0f }; + shapeCastInput.translation = input->translation; + shapeCastInput.maxFraction = input->maxFraction; + + return b3ShapeCastHeightField( heightField, &shapeCastInput ); +} + +// todo advance cast to the grid border immediately if it starts outside the row/column range +// todo terminate the cast immediately if it leaves the row/column range +b3CastOutput b3ShapeCastHeightField( const b3HeightFieldData* heightField, const b3ShapeCastInput* input ) +{ + b3AABB shapeBounds = b3MakeAABB( input->proxy.points, input->proxy.count, input->proxy.radius ); + b3Vec3 shapeTranslation = input->translation; + b3Vec3 scale = heightField->scale; + + b3Vec3 shapeStart = b3AABB_Center( shapeBounds ); + b3Vec3 shapeDelta = b3MulSV( input->maxFraction, shapeTranslation ); + b3Vec3 shapeEnd = b3Add( shapeStart, shapeDelta ); + + b3CastOutput result = { 0 }; + + b3Vec3 shapeExtents = b3AABB_Extents( shapeBounds ); + b3Vec3 margin = { B3_MAX_AABB_MARGIN, B3_MAX_AABB_MARGIN, B3_MAX_AABB_MARGIN }; + b3AABB combinedBounds = { b3Sub( b3Sub( heightField->aabb.lowerBound, shapeExtents ), margin ), + b3Add( b3Add( heightField->aabb.upperBound, shapeExtents ), margin ) }; + + float minFraction, maxFraction; + bool intersects = b3RayCastAABB( combinedBounds, shapeStart, shapeEnd, &minFraction, &maxFraction ); + if ( intersects == false ) + { + return result; + } + + // These are for walking the grid, not the triangle cast. + // The triangle cast uses the unclamped ray and fraction. + b3Vec3 clampedStart = b3MulAdd( shapeStart, minFraction, shapeDelta ); + b3Vec3 clampedDelta = b3MulSV( maxFraction - minFraction, shapeDelta ); + b3Vec3 clampedEnd = b3Add( clampedStart, clampedDelta ); + + // Preserve the un-shifted center sweep. clampedStart/clampedEnd get pushed out to the + // leading box corner below to drive the grid DDA, but the swept-volume AABB used to + // cull cells must stay centered on the actual shape path. + b3Vec3 centerStart = clampedStart; + b3Vec3 centerEnd = clampedEnd; + + // The grid traversal starts from the leading shape bounds corner + float signX, signZ; + if ( shapeTranslation.x >= 0.0f ) + { + clampedStart.x += shapeExtents.x; + signX = 1.0f; + } + else + { + clampedStart.x -= shapeExtents.x; + signX = -1.0f; + } + + if ( shapeTranslation.z >= 0.0f ) + { + clampedStart.z += shapeExtents.z; + signZ = 1.0f; + } + else + { + clampedStart.z -= shapeExtents.z; + signZ = -1.0f; + } + + // Shift the end as well + clampedEnd = b3Add( clampedStart, clampedDelta ); + + // Row and column range for the shape cast + int columnStart = (int)floorf( clampedStart.x / scale.x ); + int columnEnd = (int)floorf( clampedEnd.x / scale.x ); + int rowStart = (int)floorf( clampedStart.z / scale.z ); + int rowEnd = (int)floorf( clampedEnd.z / scale.z ); + + b3Vec3 absClampedDelta = b3Abs( clampedDelta ); + + // Precompute increments for row and column traversal. + // The ray can be slightly tilted yet remain within a single row or column + // once rasterized. + float deltaAlphaX; + float nextFractionX; + int deltaColumn; + + if ( columnStart < columnEnd ) + { + B3_ASSERT( absClampedDelta.x > 0.0f ); + + // Going forward on x columns + deltaAlphaX = scale.x / absClampedDelta.x; + nextFractionX = ( scale.x * ( columnStart + 1 ) - clampedStart.x ) / absClampedDelta.x; + deltaColumn = 1; + } + else if ( columnEnd < columnStart ) + { + B3_ASSERT( absClampedDelta.x > 0.0f ); + + // Going backwards on x columns + deltaAlphaX = scale.x / absClampedDelta.x; + nextFractionX = ( clampedStart.x - scale.x * columnStart ) / absClampedDelta.x; + deltaColumn = -1; + } + else + { + // Cast stays in a single column + deltaAlphaX = 0.0f; + nextFractionX = FLT_MAX; + deltaColumn = 0; + } + + float deltaAlphaZ; + float nextFractionZ; + int deltaRow; + + if ( rowStart < rowEnd ) + { + B3_ASSERT( absClampedDelta.z > 0.0f ); + + // Going forward on z rows + deltaAlphaZ = scale.z / absClampedDelta.z; + nextFractionZ = ( scale.z * ( rowStart + 1 ) - clampedStart.z ) / absClampedDelta.z; + deltaRow = 1; + } + else if ( rowEnd < rowStart ) + { + B3_ASSERT( absClampedDelta.z > 0.0f ); + + // Going backwards on z rows + deltaAlphaZ = scale.z / absClampedDelta.z; + nextFractionZ = ( clampedStart.z - scale.z * rowStart ) / absClampedDelta.z; + deltaRow = -1; + } + else + { + // Cast stays in a single row + deltaAlphaZ = 0.0f; + nextFractionZ = FLT_MAX; + deltaRow = 0; + } + + // Column and row range for 2D projected initial shape bounds + int boxColumnHead = columnStart; + int boxRowHead = rowStart; + + int boxColumnTail = (int)floorf( ( clampedStart.x - 2.0f * signX * shapeExtents.x ) / scale.x ); + int boxRowTail = (int)floorf( ( clampedStart.z - 2.0f * signZ * shapeExtents.z ) / scale.z ); + + float bestFraction = input->maxFraction; + + // nextFractionX / nextFractionZ advance in units of the clamped sweep + // [minFraction, maxFraction], but bestFraction is a fraction of the full input + // translation. Precompute the affine map from clamped space to input space so + // the loop termination test compares like with like — otherwise it can exit + // early and miss a closer hit in a later cell. + float gridFractionScale = input->maxFraction * ( maxFraction - minFraction ); + float gridFractionOffset = input->maxFraction * minFraction; + + int rowCount = heightField->rowCount; + int columnCount = heightField->columnCount; + int cellCount = ( heightField->rowCount - 1 ) * ( heightField->columnCount - 1 ); + B3_UNUSED( cellCount ); + + b3ShapeCastPairInput pairInput = { 0 }; + pairInput.proxyB = input->proxy; + pairInput.transform = b3Transform_identity; + pairInput.translationB = input->translation; + pairInput.canEncroach = input->canEncroach; + + b3AABB castBounds; + castBounds.lowerBound = b3Sub( b3Min( centerStart, centerEnd ), shapeExtents ); + castBounds.upperBound = b3Add( b3Max( centerStart, centerEnd ), shapeExtents ); + + b3V32 rayOrigin = b3LoadV( &shapeStart.x ); + b3V32 rayTranslation = b3LoadV( &shapeTranslation.x ); + + while ( true ) + { + int column1, column2; + if ( boxColumnTail < boxColumnHead ) + { + column1 = boxColumnTail; + column2 = boxColumnHead; + } + else + { + column1 = boxColumnHead; + column2 = boxColumnTail; + } + + int row1, row2; + if ( boxRowTail < boxRowHead ) + { + row1 = boxRowTail; + row2 = boxRowHead; + } + else + { + row1 = boxRowHead; + row2 = boxRowTail; + } + + for ( int row = row1; row <= row2; ++row ) + { + if ( row < 0 || rowCount - 1 <= row ) + { + continue; + } + + for ( int column = column1; column <= column2; ++column ) + { + if ( column < 0 || columnCount - 1 <= column ) + { + continue; + } + + int cellIndex = row * ( columnCount - 1 ) + column; + B3_ASSERT( cellIndex < cellCount ); + + uint8_t materialIndex = b3GetHeightFieldMaterialIndices( heightField )[cellIndex]; + if ( materialIndex == B3_HEIGHT_FIELD_HOLE ) + { + continue; + } + + b3Vec3 corners[4]; + b3GetHeightFieldCellCorners( heightField, row, column, corners ); + b3Vec3 point11 = corners[0]; + b3Vec3 point12 = corners[1]; + b3Vec3 point21 = corners[2]; + b3Vec3 point22 = corners[3]; + + // I know the min/max x and z values, but not the min/max heights. + b3AABB bounds; + bounds.lowerBound = b3Min( b3Min( point11, point12 ), b3Min( point21, point22 ) ); + bounds.upperBound = b3Max( b3Max( point11, point12 ), b3Max( point21, point22 ) ); + + if ( b3AABB_Overlaps( castBounds, bounds ) == false ) + { + continue; + } + + int quadIndex = row * ( columnCount - 1 ) + column; + int triangleIndex1 = 2 * quadIndex; + int triangleIndex2 = triangleIndex1 + 1; + + if ( input->proxy.count == 1 && input->proxy.radius == 0.0f ) + { + // Ray cast + { + b3V32 vertex1 = b3LoadV( &point11.x ); + b3V32 vertex2, vertex3; + + if ( heightField->clockwise ) + { + vertex2 = b3LoadV( &point12.x ); + vertex3 = b3LoadV( &point21.x ); + } + else + { + vertex2 = b3LoadV( &point21.x ); + vertex3 = b3LoadV( &point12.x ); + } + + float alpha = b3IntersectRayTriangle( rayOrigin, rayTranslation, vertex1, vertex2, vertex3 ); + B3_ASSERT( 0 <= alpha && alpha <= 1.0f ); + + if ( alpha < bestFraction ) + { + b3Vec3 edge1 = b3Sub( point21, point11 ); + b3Vec3 edge2 = b3Sub( point12, point11 ); + b3Vec3 normal = heightField->clockwise ? b3Cross( edge2, edge1 ) : b3Cross( edge1, edge2 ); + + result.point = b3MulAdd( shapeStart, alpha, shapeTranslation ); + result.normal = b3Normalize( normal ); + result.fraction = alpha; + result.triangleIndex = triangleIndex1; + result.materialIndex = materialIndex; + result.hit = true; + bestFraction = alpha; + } + } + + { + b3V32 vertex1 = b3LoadV( &point22.x ); + b3V32 vertex2, vertex3; + + if ( heightField->clockwise ) + { + vertex2 = b3LoadV( &point21.x ); + vertex3 = b3LoadV( &point12.x ); + } + else + { + vertex2 = b3LoadV( &point12.x ); + vertex3 = b3LoadV( &point21.x ); + } + + float alpha = b3IntersectRayTriangle( rayOrigin, rayTranslation, vertex1, vertex2, vertex3 ); + B3_ASSERT( 0 <= alpha && alpha <= 1.0f ); + + if ( alpha < bestFraction ) + { + b3Vec3 edge1 = b3Sub( point22, point21 ); + b3Vec3 edge2 = b3Sub( point12, point21 ); + b3Vec3 normal = heightField->clockwise ? b3Cross( edge2, edge1 ) : b3Cross( edge1, edge2 ); + + result.point = b3MulAdd( shapeStart, alpha, shapeTranslation ); + result.normal = b3Normalize( normal ); + result.fraction = alpha; + result.triangleIndex = triangleIndex2; + result.materialIndex = materialIndex; + result.hit = true; + bestFraction = alpha; + } + } + } + else + { + // Shape cast + // todo back-side culling + { + // Shift origin to first vertex + b3Vec3 origin = point11; + b3Vec3 triangleVertices[] = { b3Vec3_zero, b3Sub( point21, origin ), b3Sub( point12, origin ) }; + pairInput.proxyA = (b3ShapeProxy){ triangleVertices, 3, 0.0f }; + pairInput.maxFraction = bestFraction; + pairInput.transform.p = b3Neg( origin ); + + b3CastOutput pairOutput = b3ShapeCast( &pairInput ); + + if ( pairOutput.hit ) + { + bestFraction = pairOutput.fraction; + result = pairOutput; + result.point = b3Add( result.point, origin ); + result.triangleIndex = triangleIndex1; + result.materialIndex = materialIndex; + } + } + + { + // Shift origin to first vertex + b3Vec3 origin = point21; + b3Vec3 triangleVertices[] = { b3Vec3_zero, b3Sub( point22, origin ), b3Sub( point12, origin ) }; + pairInput.proxyA = (b3ShapeProxy){ triangleVertices, 3, 0.0f }; + pairInput.maxFraction = bestFraction; + pairInput.transform.p = b3Neg( origin ); + + b3CastOutput pairOutput = b3ShapeCast( &pairInput ); + + if ( pairOutput.hit ) + { + bestFraction = pairOutput.fraction; + result = pairOutput; + result.point = b3Add( result.point, origin ); + result.triangleIndex = triangleIndex2; + result.materialIndex = materialIndex; + } + } + } + } + } + + // These fractions always increase to guarantee the loop eventually exits. + // Map them from clamped-sweep space into input-translation space before + // comparing against bestFraction. + float inputFractionX = nextFractionX == FLT_MAX ? FLT_MAX : gridFractionOffset + nextFractionX * gridFractionScale; + float inputFractionZ = nextFractionZ == FLT_MAX ? FLT_MAX : gridFractionOffset + nextFractionZ * gridFractionScale; + if ( inputFractionX > bestFraction && inputFractionZ > bestFraction ) + { + break; + } + + // Advance the cast to the next column or row + if ( nextFractionX <= nextFractionZ ) + { + if ( boxColumnHead == columnEnd ) + { + // Hit the end already + break; + } + + // Advance to next column + boxColumnHead += deltaColumn; + + // Build a single column to cast + boxColumnTail = boxColumnHead; + + if ( shapeExtents.z == 0.0f ) + { + // Single row + boxRowTail = boxRowHead; + } + else + { + // Rasterize shape row + float rowIntercept = clampedStart.z + nextFractionX * clampedDelta.z; + boxRowTail = (int)floorf( ( rowIntercept - 2.0f * signZ * shapeExtents.z ) / scale.z ); + } + + nextFractionX += deltaAlphaX; + } + else + { + if ( boxRowHead == rowEnd ) + { + // Hit the end already + break; + } + + // Advance to next row + boxRowHead += deltaRow; + + // Build a single row to cast + boxRowTail = boxRowHead; + + if ( shapeExtents.x == 0.0f ) + { + // Single column + boxColumnTail = boxColumnHead; + } + else + { + // Rasterize shape column + float columnIntercept = clampedStart.x + nextFractionZ * clampedDelta.x; + boxColumnTail = (int)floorf( ( columnIntercept - 2.0f * signX * shapeExtents.x ) / scale.x ); + } + + nextFractionZ += deltaAlphaZ; + } + } + + return result; +} + +bool b3OverlapHeightField( const b3HeightFieldData* shape, b3Transform shapeTransform, const b3ShapeProxy* proxy ) +{ + b3Vec3 buffer[B3_MAX_SHAPE_CAST_POINTS]; + b3ShapeProxy localProxy = b3MakeLocalProxy( proxy, shapeTransform, buffer ); + b3AABB aabb = b3ComputeProxyAABB( &localProxy ); + + b3Vec3 scale = shape->scale; + int minRow = (int)floorf( aabb.lowerBound.z / scale.z ); + int maxRow = (int)floorf( aabb.upperBound.z / scale.z ); + int minCol = (int)floorf( aabb.lowerBound.x / scale.x ); + int maxCol = (int)floorf( aabb.upperBound.x / scale.x ); + + b3V32 boundsMin = b3LoadV( &aabb.lowerBound.x ); + b3V32 boundsMax = b3LoadV( &aabb.upperBound.x ); + b3V32 boundsCenter = b3MulV( b3_halfV, b3AddV( boundsMin, boundsMax ) ); + b3V32 boundsExtent = b3SubV( boundsMax, boundsCenter ); + + b3DistanceInput input; + input.proxyB = localProxy; + input.transform = b3Transform_identity; + input.useRadii = true; + + b3SimplexCache cache = { 0 }; + + // Outer loop on rows and inner loop on columns so that triangle indices + // increase monotonically. + for ( int row = minRow; row <= maxRow; ++row ) + { + if ( row < 0 || shape->rowCount - 1 <= row ) + { + continue; + } + + for ( int column = minCol; column <= maxCol; ++column ) + { + if ( column < 0 || shape->columnCount - 1 <= column ) + { + continue; + } + + int cellIndex = row * ( shape->columnCount - 1 ) + column; + B3_ASSERT( cellIndex < ( shape->rowCount - 1 ) * ( shape->columnCount - 1 ) ); + uint8_t material = b3GetHeightFieldMaterialIndices( shape )[cellIndex]; + if ( material == B3_HEIGHT_FIELD_HOLE ) + { + continue; + } + + b3Vec3 corners[4]; + b3GetHeightFieldCellCorners( shape, row, column, corners ); + b3Vec3 point11 = corners[0]; + b3Vec3 point12 = corners[1]; + b3Vec3 point21 = corners[2]; + b3Vec3 point22 = corners[3]; + + b3V32 v11 = b3LoadV( &point11.x ); + b3V32 v12 = b3LoadV( &point12.x ); + b3V32 v21 = b3LoadV( &point21.x ); + b3V32 v22 = b3LoadV( &point22.x ); + + if ( b3TestBoundsTriangleOverlap( boundsCenter, boundsExtent, v11, v21, v12 ) ) + { + b3Vec3 triangleVertices[] = { point11, point21, point12 }; + input.proxyA = (b3ShapeProxy){ triangleVertices, 3, 0.0f }; + + // reset the cache + cache.count = 0; + + // get distance between triangle and query shape + b3DistanceOutput output = b3ShapeDistance( &input, &cache, NULL, 0 ); + + float tolerance = 0.1f * B3_LINEAR_SLOP; + if ( output.distance < tolerance ) + { + // overlap detected + return true; + } + } + + if ( b3TestBoundsTriangleOverlap( boundsCenter, boundsExtent, v21, v22, v12 ) ) + { + b3Vec3 triangleVertices[] = { point22, point12, point21 }; + input.proxyA = (b3ShapeProxy){ triangleVertices, 3, 0.0f }; + + // reset the cache + cache.count = 0; + + // get distance between triangle and query shape + b3DistanceOutput output = b3ShapeDistance( &input, &cache, NULL, 0 ); + + float tolerance = 0.1f * B3_LINEAR_SLOP; + if ( output.distance < tolerance ) + { + // overlap detected + return true; + } + } + } + } + + return false; +} + +void b3QueryHeightField( const b3HeightFieldData* heightField, b3AABB bounds, b3MeshQueryFcn* fcn, void* context ) +{ + b3Vec3 scale = heightField->scale; + + int minRow = (int)floorf( bounds.lowerBound.z / scale.z ); + int maxRow = (int)floorf( bounds.upperBound.z / scale.z ); + int minCol = (int)floorf( bounds.lowerBound.x / scale.x ); + int maxCol = (int)floorf( bounds.upperBound.x / scale.x ); + + // Outer loop on rows and inner loop on columns so that triangle indices + // increase monotonically. + for ( int row = minRow; row <= maxRow; ++row ) + { + if ( row < 0 || heightField->rowCount - 1 <= row ) + { + continue; + } + + for ( int column = minCol; column <= maxCol; ++column ) + { + if ( column < 0 || heightField->columnCount - 1 <= column ) + { + continue; + } + + int cellIndex = row * ( heightField->columnCount - 1 ) + column; + B3_ASSERT( cellIndex < ( heightField->rowCount - 1 ) * ( heightField->columnCount - 1 ) ); + uint8_t material = b3GetHeightFieldMaterialIndices( heightField )[cellIndex]; + if ( material == B3_HEIGHT_FIELD_HOLE ) + { + continue; + } + + b3Vec3 corners[4]; + b3GetHeightFieldCellCorners( heightField, row, column, corners ); + b3Vec3 point11 = corners[0]; + b3Vec3 point12 = corners[1]; + b3Vec3 point21 = corners[2]; + b3Vec3 point22 = corners[3]; + + // I know the min/max x and z values, but not the min/max heights. + // This could be done with no branching in SIMD. + b3AABB cellBound; + cellBound.lowerBound = b3Min( b3Min( point11, point12 ), b3Min( point21, point22 ) ); + cellBound.upperBound = b3Max( b3Max( point11, point12 ), b3Max( point21, point22 ) ); + + if ( b3AABB_Overlaps( bounds, cellBound ) ) + { + int quadIndex = row * ( heightField->columnCount - 1 ) + column; + int triangleIndex = 2 * quadIndex; + + if ( heightField->clockwise ) + { + fcn( point11, point12, point21, triangleIndex, context ); + fcn( point22, point21, point12, triangleIndex + 1, context ); + } + else + { + fcn( point11, point21, point12, triangleIndex, context ); + fcn( point22, point12, point21, triangleIndex + 1, context ); + } + } + } + } +} + +int b3CollideMoverAndHeightField( b3PlaneResult* planes, int capacity, const b3HeightFieldData* shape, const b3Capsule* mover ) +{ + b3DistanceInput distanceInput = { 0 }; + distanceInput.proxyB = (b3ShapeProxy){ &mover->center1, 2, 0.0f }; + distanceInput.transform = b3Transform_identity; + distanceInput.useRadii = false; + + b3SimplexCache cache = { 0 }; + + float radius = mover->radius; + b3V32 center1 = b3LoadV( &mover->center1.x ); + b3V32 center2 = b3LoadV( &mover->center2.x ); + b3V32 r = b3SplatV( radius ); + b3V32 boundsMin = b3SubV( b3MinV( center1, center2 ), r ); + b3V32 boundsMax = b3AddV( b3MaxV( center1, center2 ), r ); + b3V32 boundsCenter = b3MulV( b3_halfV, b3AddV( boundsMin, boundsMax ) ); + b3V32 boundsExtent = b3SubV( boundsMax, boundsCenter ); + + float localMinX = b3GetXV( boundsMin ); + float localMinZ = b3GetZV( boundsMin ); + float localMaxX = b3GetXV( boundsMax ); + float localMaxZ = b3GetZV( boundsMax ); + + b3Vec3 scale = shape->scale; + int minRow = (int)floorf( localMinZ / scale.z ); + int maxRow = (int)floorf( localMaxZ / scale.z ); + int minCol = (int)floorf( localMinX / scale.x ); + int maxCol = (int)floorf( localMaxX / scale.x ); + + int planeCount = 0; + + // Outer loop on rows and inner loop on columns so that triangle indices + // increase monotonically. + for ( int row = minRow; row <= maxRow; ++row ) + { + if ( row < 0 || shape->rowCount - 1 <= row ) + { + continue; + } + + for ( int column = minCol; column <= maxCol; ++column ) + { + if ( column < 0 || shape->columnCount - 1 <= column ) + { + continue; + } + + int cellIndex = row * ( shape->columnCount - 1 ) + column; + B3_ASSERT( cellIndex < ( shape->rowCount - 1 ) * ( shape->columnCount - 1 ) ); + uint8_t material = b3GetHeightFieldMaterialIndices( shape )[cellIndex]; + if ( material == B3_HEIGHT_FIELD_HOLE ) + { + continue; + } + + b3Vec3 corners[4]; + b3GetHeightFieldCellCorners( shape, row, column, corners ); + b3Vec3 point11 = corners[0]; + b3Vec3 point12 = corners[1]; + b3Vec3 point21 = corners[2]; + b3Vec3 point22 = corners[3]; + + b3V32 v11 = b3LoadV( &point11.x ); + b3V32 v12 = b3LoadV( &point12.x ); + b3V32 v21 = b3LoadV( &point21.x ); + b3V32 v22 = b3LoadV( &point22.x ); + + if ( b3TestBoundsTriangleOverlap( boundsCenter, boundsExtent, v11, v21, v12 ) ) + { + b3Vec3 triangleVertices[] = { point11, point21, point12 }; + distanceInput.proxyA = (b3ShapeProxy){ triangleVertices, 3, 0.0f }; + + // reset the cache + cache.count = 0; + + // get distance between triangle and mover + b3DistanceOutput distanceOutput = b3ShapeDistance( &distanceInput, &cache, NULL, 0 ); + + if ( distanceOutput.distance == 0.0f ) + { + // todo SAT + } + else if ( distanceOutput.distance <= mover->radius ) + { + b3Plane plane = { distanceOutput.normal, mover->radius - distanceOutput.distance }; + planes[planeCount] = (b3PlaneResult){ plane, distanceOutput.pointA }; + planeCount += 1; + + if ( planeCount == capacity ) + { + return planeCount; + } + } + } + + if ( b3TestBoundsTriangleOverlap( boundsCenter, boundsExtent, v21, v22, v12 ) ) + { + b3Vec3 triangleVertices[] = { point22, point12, point21 }; + distanceInput.proxyA = (b3ShapeProxy){ triangleVertices, 3, 0.0f }; + + // reset the cache + cache.count = 0; + + // get distance between triangle and mover + b3DistanceOutput distanceOutput = b3ShapeDistance( &distanceInput, &cache, NULL, 0 ); + + if ( distanceOutput.distance == 0.0f ) + { + // todo SAT + } + else if ( distanceOutput.distance <= mover->radius ) + { + b3Plane plane = { distanceOutput.normal, mover->radius - distanceOutput.distance }; + planes[planeCount] = (b3PlaneResult){ plane, distanceOutput.pointA }; + planeCount += 1; + + if ( planeCount == capacity ) + { + return planeCount; + } + } + } + } + } + + return planeCount; +} + +b3HeightFieldData* b3CreateGrid( int rowCount, int columnCount, b3Vec3 scale, bool makeHoles ) +{ + int heightCount = rowCount * columnCount; + float* heights = (float*)b3Alloc( heightCount * sizeof( float ) ); + + for ( int i = 0; i < rowCount; ++i ) + { + for ( int j = 0; j < columnCount; ++j ) + { + int k = i * columnCount + j; + heights[k] = 0.0f; + } + } + + int cellCount = ( rowCount - 1 ) * ( columnCount - 1 ); + uint8_t* materialIndices = (uint8_t*)b3Alloc( cellCount * sizeof( uint8_t ) ); + + for ( int i = 0; i < rowCount - 1; ++i ) + { + for ( int j = 0; j < columnCount - 1; ++j ) + { + int k = i * ( columnCount - 1 ) + j; + + if ( makeHoles && k > 0 && k % 16 == 0 ) + { + materialIndices[k] = B3_HEIGHT_FIELD_HOLE; + } + else + { + materialIndices[k] = 0; + } + } + } + + b3HeightFieldDef data = { 0 }; + data.heights = heights; + data.materialIndices = materialIndices; + data.scale = scale; + data.countX = columnCount; + data.countZ = rowCount; + data.globalMinimumHeight = -256.0f; + data.globalMaximumHeight = 256.0f; + data.clockwiseWinding = false; + + b3HeightFieldData* heightField = b3CreateHeightField( &data ); + + b3Free( heights, heightCount * sizeof( float ) ); + b3Free( materialIndices, cellCount * sizeof( uint8_t ) ); + + return heightField; +} + +b3HeightFieldData* b3CreateWave( int rowCount, int columnCount, b3Vec3 scale, float rowFrequency, float columnFrequency, + bool makeHoles ) +{ + int heightCount = rowCount * columnCount; + float* heights = (float*)b3Alloc( heightCount * sizeof( float ) ); + + float omegaZ = 2.0f * B3_PI * rowFrequency; + float omegaX = 2.0f * B3_PI * columnFrequency; + + for ( int i = 0; i < rowCount; ++i ) + { + float rowHeight = sinf( omegaZ * i ); + + for ( int j = 0; j < columnCount; ++j ) + { + int k = i * columnCount + j; + float columnHeight = sinf( omegaX * j ); + heights[k] = rowHeight * columnHeight; + } + } + + int cellCount = ( rowCount - 1 ) * ( columnCount - 1 ); + uint8_t* materialIndices = (uint8_t*)b3Alloc( cellCount * sizeof( uint8_t ) ); + + for ( int i = 0; i < rowCount - 1; ++i ) + { + for ( int j = 0; j < columnCount - 1; ++j ) + { + int k = i * ( columnCount - 1 ) + j; + + if ( makeHoles && k > 0 && k % 16 == 0 ) + { + materialIndices[k] = B3_HEIGHT_FIELD_HOLE; + } + else + { + materialIndices[k] = 0; + } + } + } + + b3HeightFieldDef data = { 0 }; + data.heights = heights; + data.materialIndices = materialIndices; + data.scale = scale; + data.countX = columnCount; + data.countZ = rowCount; + data.globalMinimumHeight = -256.0f; + data.globalMaximumHeight = 256.0f; + data.clockwiseWinding = false; + + b3HeightFieldData* heightField = b3CreateHeightField( &data ); + + b3Free( heights, heightCount * sizeof( float ) ); + b3Free( materialIndices, cellCount * sizeof( uint8_t ) ); + + return heightField; +} + +void b3DestroyHeightField( b3HeightFieldData* heightField ) +{ + b3Free( heightField, heightField->byteCount ); +} + +void b3DumpHeightData( const b3HeightFieldDef* data, const char* fileName ) +{ + FILE* file = NULL; + +#if defined( _MSC_VER ) + errno_t e = fopen_s( &file, fileName, "w" ); + if ( e != 0 ) + { + return; + } +#else + file = fopen( fileName, "w" ); + if ( file == NULL ) + { + return; + } +#endif + + fprintf( file, "%d %d\n", data->countX, data->countZ ); + fprintf( file, "%.9f %.9f %.9f\n", data->scale.x, data->scale.y, data->scale.z ); + fprintf( file, "%.9f %.9f\n", data->globalMinimumHeight, data->globalMaximumHeight ); + fprintf( file, "%d\n", data->clockwiseWinding ); + + int heightCount = data->countX * data->countZ; + for ( int i = 0; i < heightCount; ++i ) + { + fprintf( file, "%.9f\n", data->heights[i] ); + } + + int materialCount = ( data->countX - 1 ) * ( data->countZ - 1 ); + for ( int i = 0; i < materialCount; ++i ) + { + fprintf( file, "%d\n", data->materialIndices[i] ); + } + + fclose( file ); +} + +#if defined( _MSC_VER ) +#define B3_FILE_SCAN fscanf_s +#else +#define B3_FILE_SCAN fscanf +#endif + +b3HeightFieldData* b3LoadHeightField( const char* fileName ) +{ + FILE* file = NULL; + +#if defined( _MSC_VER ) + errno_t e = fopen_s( &file, fileName, "r" ); + if ( e != 0 ) + { + return NULL; + } +#else + file = fopen( fileName, "r" ); + if ( file == NULL ) + { + return NULL; + } +#endif + + b3HeightFieldDef data = { 0 }; + + // Read dimensions + if ( B3_FILE_SCAN( file, "%d %d", &data.countX, &data.countZ ) != 2 ) + { + fclose( file ); + return NULL; + } + + // Read scale + if ( B3_FILE_SCAN( file, "%f %f %f", &data.scale.x, &data.scale.y, &data.scale.z ) != 3 ) + { + fclose( file ); + return NULL; + } + + // Read global height bounds + if ( B3_FILE_SCAN( file, "%f %f", &data.globalMinimumHeight, &data.globalMaximumHeight ) != 2 ) + { + fclose( file ); + return NULL; + } + + // Read clockwise winding + int clockwise; + if ( B3_FILE_SCAN( file, "%d", &clockwise ) != 1 ) + { + fclose( file ); + return NULL; + } + data.clockwiseWinding = clockwise != 0; + + // Allocate and read height data + int heightCount = data.countX * data.countZ; + data.heights = (float*)b3Alloc( heightCount * sizeof( float ) ); + + for ( int i = 0; i < heightCount; ++i ) + { + if ( B3_FILE_SCAN( file, "%f", &data.heights[i] ) != 1 ) + { + b3Free( data.heights, heightCount * sizeof( float ) ); + fclose( file ); + return NULL; + } + } + + // Allocate and read material indices + int materialCount = ( data.countX - 1 ) * ( data.countZ - 1 ); + data.materialIndices = (uint8_t*)b3Alloc( materialCount * sizeof( uint8_t ) ); + + for ( int i = 0; i < materialCount; ++i ) + { + int materialIndex; + if ( B3_FILE_SCAN( file, "%d", &materialIndex ) != 1 ) + { + b3Free( data.heights, heightCount * sizeof( float ) ); + b3Free( data.materialIndices, materialCount * sizeof( uint8_t ) ); + fclose( file ); + return NULL; + } + data.materialIndices[i] = (uint8_t)materialIndex; + } + + fclose( file ); + + // Create height field from loaded data + b3HeightFieldData* heightField = b3CreateHeightField( &data ); + + // Clean up temporary allocations + b3Free( data.heights, heightCount * sizeof( float ) ); + b3Free( data.materialIndices, materialCount * sizeof( uint8_t ) ); + + return heightField; +} diff --git a/vendor/box3d/src/src/hull.c b/vendor/box3d/src/src/hull.c new file mode 100644 index 000000000..75bace9f9 --- /dev/null +++ b/vendor/box3d/src/src/hull.c @@ -0,0 +1,2791 @@ +// SPDX-FileCopyrightText: 2026 Erin Catto +// SPDX-License-Identifier: MIT + +// Dirk Gregorius contributed portions of this code + +#include "algorithm.h" +#include "hull_map.h" +#include "math_internal.h" +#include "shape.h" + +#include "box3d/collision.h" +#include "box3d/constants.h" +#include "box3d/math_functions.h" + +#include +#include +#include +#include +#include +#include + +#define B3_AXIS_X 0 +#define B3_AXIS_Y 1 +#define B3_AXIS_Z 2 + +#define B3_MARK_VISIBLE 0 +#define B3_MARK_DELETE 1 + +// Final hull is index-encoded with uint8_t, so vertex/edge/face counts are capped at UINT8_MAX. +#define B3_HULL_LIMIT UINT8_MAX + +typedef struct b3QHListNode +{ + struct b3QHListNode* prev; + struct b3QHListNode* next; +} b3QHListNode; + +typedef struct b3QHFace b3QHFace; + +typedef struct b3QHVertex +{ + // Intrusive list link. Must be first so (b3QHVertex*)nodePtr is valid. + b3QHListNode link; + + b3QHFace* conflictFace; + b3Vec3 position; + + // Index in the finalized hull, stamped during emit. B3_NULL_INDEX until then. + int finalIndex; + bool reachable; +} b3QHVertex; + +typedef struct b3QHHalfEdge +{ + // Edge ring (CCW) around the owning face. Not an external list. + struct b3QHHalfEdge* prev; + struct b3QHHalfEdge* next; + + b3QHVertex* origin; + b3QHFace* face; + struct b3QHHalfEdge* twin; + + // Index in the finalized hull, stamped during emit. B3_NULL_INDEX until then. + int finalIndex; +} b3QHHalfEdge; + +struct b3QHFace +{ + // Intrusive list link. Must be first so (b3QHFace*)nodePtr is valid. + b3QHListNode link; + + b3QHHalfEdge* edge; + + int mark; + float area; + b3Plane plane; + b3Vec3 centroid; + float maxConflictDistance; + + // Sentinel head for this face's conflict list of b3QHVertex. + b3QHVertex conflictListHead; + + // Cached farthest conflict vertex (above b3HullBuilder::minOutside). + // NULL when no conflict above threshold; maxConflictDistance is then minOutside. + b3QHVertex* maxConflict; + + // Index in the finalized hull, stamped during emit. B3_NULL_INDEX until then. + int finalIndex; + bool flipped; +}; + +// One frame of the iterative horizon DFS. Replaces a recursive call to b3HullBuilder_BuildHorizon. +typedef struct b3HorizonFrame +{ + b3QHFace* face; + b3QHHalfEdge* startEdge; // ring termination sentinel + b3QHHalfEdge* edge; // next edge to process + bool started; // false until the first edge of this ring has been processed +} b3HorizonFrame; + +// All working memory for one hull build, carved from a single b3Alloc block. +typedef struct b3HullBuilder +{ + float tolerance; + float minRadius; + float minOutside; + + b3Vec3 interiorPoint; + + // List sentinels. Only the link is meaningful; other fields are unused. + b3QHVertex orphanedList; + b3QHVertex vertexList; + b3QHFace faceList; + + // Bump-allocated pools with pointer-based free lists for faces and edges. + b3QHVertex* vertexBase; + int vertexCapacity; + int vertexCount; + + b3QHHalfEdge* edgeBase; + int edgeCapacity; + int edgeCount; + b3QHHalfEdge* edgeFreeHead; // LIFO free list; overlays edge->next + + b3QHFace* faceBase; + int faceCapacity; + int faceCount; + b3QHFace* faceFreeHead; // LIFO free list; overlays face->link.next + + // Reusable scratch buffers. + b3QHHalfEdge** horizon; + int horizonCapacity; + int horizonCount; + + b3QHFace** cone; + int coneCapacity; + int coneCount; + + b3QHFace** mergedFaces; + int mergedFacesCapacity; + int mergedFacesCount; + + // DFS stack used by the iterative b3HullBuilder_BuildHorizon. Depth bounded by live faces. + b3HorizonFrame* horizonStack; + int horizonStackCapacity; + + // Final counts of the constructed hull (vertexList / faceList / half-edges around faces). + // Populated by CleanHull; zero until then. + int finalVertexCount; + int finalHalfEdgeCount; + int finalFaceCount; +} b3HullBuilder; + +static inline void b3QHList_Init( b3QHListNode* head ) +{ + head->prev = head; + head->next = head; +} + +#define B3_LIST_EMPTY( A ) ( ( A )->next == ( A ) ) + +static inline bool b3QHList_Contains( const b3QHListNode* node ) +{ + return node->prev != NULL && node->next != NULL; +} + +// Insert node before `where`. +static inline void b3QHList_Insert( b3QHListNode* node, b3QHListNode* where ) +{ + B3_ASSERT( !b3QHList_Contains( node ) && b3QHList_Contains( where ) ); + + node->prev = where->prev; + node->next = where; + + node->prev->next = node; + node->next->prev = node; +} + +static inline void b3QHList_Remove( b3QHListNode* node ) +{ + B3_ASSERT( b3QHList_Contains( node ) ); + + node->prev->next = node->next; + node->next->prev = node->prev; + + node->prev = NULL; + node->next = NULL; +} + +static inline void b3QHList_PushBack( b3QHListNode* head, b3QHListNode* node ) +{ + b3QHList_Insert( node, head->prev ); +} + +static b3QHVertex* b3HullBuilder_NewVertex( b3HullBuilder* b, b3Vec3 position ) +{ + B3_ASSERT( b->vertexCount < b->vertexCapacity ); + b3QHVertex* vertex = b->vertexBase + b->vertexCount++; + + vertex->link.prev = NULL; + vertex->link.next = NULL; + vertex->conflictFace = NULL; + vertex->position = position; + vertex->finalIndex = B3_NULL_INDEX; + vertex->reachable = false; + + return vertex; +} + +static b3QHHalfEdge* b3HullBuilder_NewEdge( b3HullBuilder* b ) +{ + b3QHHalfEdge* edge; + if ( b->edgeFreeHead != NULL ) + { + edge = b->edgeFreeHead; + b->edgeFreeHead = edge->next; + } + else + { + B3_ASSERT( b->edgeCount < b->edgeCapacity ); + edge = b->edgeBase + b->edgeCount++; + } + // All other fields (prev/next/origin/face/twin) are written by NewFace immediately after. + edge->finalIndex = B3_NULL_INDEX; + return edge; +} + +static void b3HullBuilder_RetireEdge( b3HullBuilder* b, b3QHHalfEdge* edge ) +{ + edge->next = b->edgeFreeHead; + b->edgeFreeHead = edge; +} + +static b3QHFace* b3HullBuilder_NewFace( b3HullBuilder* b, b3QHVertex* v1, b3QHVertex* v2, b3QHVertex* v3 ) +{ + b3QHFace* face; + if ( b->faceFreeHead != NULL ) + { + face = b->faceFreeHead; + // link.next was used as free-list pointer; recover next head before we clobber. + b->faceFreeHead = (b3QHFace*)face->link.next; + } + else + { + B3_ASSERT( b->faceCount < b->faceCapacity ); + face = b->faceBase + b->faceCount++; + } + + // link.prev: NULL on retired faces (cleared by Remove); NULL here so PushBack's + // !b3QHList_Contains assert holds for fresh bump slots too. + // link.next: was the free-list pointer on reused slots, now stale; PushBack overwrites. + face->link.prev = NULL; + face->link.next = NULL; + face->maxConflict = NULL; + face->maxConflictDistance = 0.0f; + face->finalIndex = B3_NULL_INDEX; + + b3QHHalfEdge* edge1 = b3HullBuilder_NewEdge( b ); + b3QHHalfEdge* edge2 = b3HullBuilder_NewEdge( b ); + b3QHHalfEdge* edge3 = b3HullBuilder_NewEdge( b ); + + b3Vec3 p1 = v1->position; + b3Vec3 p2 = v2->position; + b3Vec3 p3 = v3->position; + + b3Plane plane; + plane.normal = b3Cross( b3Sub( p2, p1 ), b3Sub( p3, p1 ) ); + float length; + plane.normal = b3GetLengthAndNormalize( &length, plane.normal ); + plane.offset = b3Dot( plane.normal, p1 ); + + float area = 0.5f * length; + + face->edge = edge1; + face->mark = B3_MARK_VISIBLE; + face->area = area; + face->centroid = b3MulSV( 1.0f / 3.0f, b3Add( v1->position, b3Add( v2->position, v3->position ) ) ); + face->plane = plane; + face->flipped = b3PlaneSeparation( plane, b->interiorPoint ) > 0.0f; + b3QHList_Init( &face->conflictListHead.link ); + + edge1->prev = edge3; + edge1->next = edge2; + edge1->origin = v1; + edge1->face = face; + edge1->twin = NULL; + + edge2->prev = edge1; + edge2->next = edge3; + edge2->origin = v2; + edge2->face = face; + edge2->twin = NULL; + + edge3->prev = edge2; + edge3->next = edge1; + edge3->origin = v3; + edge3->face = face; + edge3->twin = NULL; + + return face; +} + +// Remove face from faceList if still linked, clear its edge pointer, then push onto faceFreeHead. +// Uses face->link.next as the free-list next pointer (link.prev stays NULL, so b3QHList_Contains +// returns false on a free slot, as required by the retire-guard in ResolveFaces). +static void b3HullBuilder_RetireFace( b3HullBuilder* b, b3QHFace* face ) +{ + if ( b3QHList_Contains( &face->link ) ) + { + b3QHList_Remove( &face->link ); + } + face->edge = NULL; + // link.prev is already NULL after Remove (or was never set). link.next holds free-list ptr. + face->link.next = (b3QHListNode*)b->faceFreeHead; + b->faceFreeHead = face; +} + +static b3AABB b3BuildBounds( int vertexCount, const b3Vec3* vertices ) +{ + b3AABB bounds = B3_BOUNDS3_EMPTY; + for ( int i = 0; i < vertexCount; ++i ) + { + bounds.lowerBound = b3Min( bounds.lowerBound, vertices[i] ); + bounds.upperBound = b3Max( bounds.upperBound, vertices[i] ); + } + return bounds; +} + +static void b3FindFarthestPointsAlongCardinalAxes( int* index1Out, int* index2Out, float tolerance, int vertexCount, + const b3Vec3* vertexBase ) +{ + *index1Out = B3_NULL_INDEX; + *index2Out = B3_NULL_INDEX; + + b3Vec3 v0 = vertexBase[0]; + b3Vec3 minPt[3] = { v0, v0, v0 }; + b3Vec3 maxPt[3] = { v0, v0, v0 }; + + int minIndex[3] = { 0, 0, 0 }; + int maxIndex[3] = { 0, 0, 0 }; + + for ( int i = 1; i < vertexCount; ++i ) + { + b3Vec3 v = vertexBase[i]; + + if ( v.x < minPt[B3_AXIS_X].x ) + { + minPt[B3_AXIS_X] = v; + minIndex[B3_AXIS_X] = i; + } + else if ( v.x > maxPt[B3_AXIS_X].x ) + { + maxPt[B3_AXIS_X] = v; + maxIndex[B3_AXIS_X] = i; + } + + if ( v.y < minPt[B3_AXIS_Y].y ) + { + minPt[B3_AXIS_Y] = v; + minIndex[B3_AXIS_Y] = i; + } + else if ( v.y > maxPt[B3_AXIS_Y].y ) + { + maxPt[B3_AXIS_Y] = v; + maxIndex[B3_AXIS_Y] = i; + } + + if ( v.z < minPt[B3_AXIS_Z].z ) + { + minPt[B3_AXIS_Z] = v; + minIndex[B3_AXIS_Z] = i; + } + else if ( v.z > maxPt[B3_AXIS_Z].z ) + { + maxPt[B3_AXIS_Z] = v; + maxIndex[B3_AXIS_Z] = i; + } + } + + b3Vec3 distance; + distance.x = maxPt[B3_AXIS_X].x - minPt[B3_AXIS_X].x; + distance.y = maxPt[B3_AXIS_Y].y - minPt[B3_AXIS_Y].y; + distance.z = maxPt[B3_AXIS_Z].z - minPt[B3_AXIS_Z].z; + + float distanceArray[3] = { distance.x, distance.y, distance.z }; + int maxElement = b3MaxElementIndex( distance ); + + if ( distanceArray[maxElement] > 2.0f * tolerance ) + { + *index1Out = minIndex[maxElement]; + *index2Out = maxIndex[maxElement]; + } +} + +static int b3FindFarthestPointFromLine( int index1, int index2, float tolerance, int vertexCount, const b3Vec3* vertexBase ) +{ + b3Vec3 a = vertexBase[index1]; + b3Vec3 b = vertexBase[index2]; + + // |ap x ab|^2 / |ab|^2 is the squared perpendicular distance from p to the line. + // Compares against (2 * tolerance)^2 + b3Vec3 ab = b3Sub( b, a ); + float abLengthSqr = b3Dot( ab, ab ); + B3_ASSERT( abLengthSqr > 0.0f ); + + float invAbLengthSqr = 1.0f / abLengthSqr; + float maxDistanceSqr = 4.0f * tolerance * tolerance; + int maxIndex = B3_NULL_INDEX; + + for ( int i = 0; i < vertexCount; ++i ) + { + if ( i == index1 || i == index2 ) + { + continue; + } + + b3Vec3 ap = b3Sub( vertexBase[i], a ); + b3Vec3 cross = b3Cross( ap, ab ); + float distanceSqr = b3Dot( cross, cross ) * invAbLengthSqr; + if ( distanceSqr > maxDistanceSqr ) + { + maxDistanceSqr = distanceSqr; + maxIndex = i; + } + } + + return maxIndex; +} + +static int b3FindFarthestPointFromPlane( int index1, int index2, int index3, float tolerance, int vertexCount, + const b3Vec3* vertexBase ) +{ + b3Vec3 a = vertexBase[index1]; + b3Vec3 b = vertexBase[index2]; + b3Vec3 c = vertexBase[index3]; + + b3Plane plane = b3MakePlaneFromPoints( a, b, c ); + + float maxDistance = 2.0f * tolerance; + int maxIndex = B3_NULL_INDEX; + + for ( int i = 0; i < vertexCount; ++i ) + { + if ( i == index1 || i == index2 || i == index3 ) + { + continue; + } + + float distance = b3AbsFloat( b3PlaneSeparation( plane, vertexBase[i] ) ); + if ( distance > maxDistance ) + { + maxDistance = distance; + maxIndex = i; + } + } + + return maxIndex; +} + +static bool b3IsEdgeConvex( const b3QHHalfEdge* edge, float tolerance ) +{ + float distance = b3PlaneSeparation( edge->face->plane, edge->twin->face->centroid ); + return distance < -tolerance; +} + +static bool b3IsEdgeConcave( const b3QHHalfEdge* edge, float tolerance ) +{ + float distance = b3PlaneSeparation( edge->face->plane, edge->twin->face->centroid ); + return distance > tolerance; +} + +static int b3VertexCountOfFace( const b3QHFace* face ) +{ + int count = 0; + const b3QHHalfEdge* edge = face->edge; + do + { + count++; + edge = edge->next; + } + while ( edge != face->edge ); + + return count; +} + +static void b3LinkFace( b3QHFace* face, int index, b3QHHalfEdge* twin ) +{ + B3_ASSERT( face != twin->face ); + + b3QHHalfEdge* edge = face->edge; + while ( index-- > 0 ) + { + B3_ASSERT( edge->face == face ); + edge = edge->next; + } + + B3_ASSERT( edge != twin ); + edge->twin = twin; + twin->twin = edge; +} + +static void b3LinkFaces( b3QHFace* face1, int index1, b3QHFace* face2, int index2 ) +{ + B3_ASSERT( face1 != face2 ); + + b3QHHalfEdge* edge1 = face1->edge; + while ( index1-- > 0 ) + { + edge1 = edge1->next; + } + + b3QHHalfEdge* edge2 = face2->edge; + while ( index2-- > 0 ) + { + edge2 = edge2->next; + } + + B3_ASSERT( edge1 != edge2 ); + edge1->twin = edge2; + edge2->twin = edge1; +} + +static void b3NewellPlane( b3QHFace* face ) +{ + int count = 0; + b3Vec3 centroid = b3Vec3_zero; + b3Vec3 normal = b3Vec3_zero; + + b3QHHalfEdge* edge = face->edge; + B3_ASSERT( edge->face == face ); + + // Use the first vertex as the origin to reduce round-off + b3Vec3 origin = edge->origin->position; + + do + { + b3QHHalfEdge* twin = edge->twin; + B3_ASSERT( twin->twin == edge ); + + b3Vec3 v1 = b3Sub( edge->origin->position, origin ); + b3Vec3 v2 = b3Sub( twin->origin->position, origin ); + + count++; + centroid = b3Add( centroid, v1 ); + normal.x += ( v1.y - v2.y ) * ( v1.z + v2.z ); + normal.y += ( v1.z - v2.z ) * ( v1.x + v2.x ); + normal.z += ( v1.x - v2.x ) * ( v1.y + v2.y ); + + edge = edge->next; + } + while ( edge != face->edge ); + + B3_ASSERT( count > 0 ); + centroid = b3MulSV( 1.0f / (float)count, centroid ); + centroid = b3Add( centroid, origin ); + + float length = b3Length( normal ); + B3_VALIDATE( length > 0.0f ); + normal = b3MulSV( 1.0f / length, normal ); + + face->centroid = centroid; + face->plane = b3MakePlaneFromNormalAndPoint( normal, centroid ); + face->area = 0.5f * length; +} + +#if B3_DEBUG +static bool b3CheckConsistency( const b3QHFace* face ) +{ + if ( face->mark == B3_MARK_DELETE ) + { + return false; + } + + if ( b3VertexCountOfFace( face ) < 3 ) + { + return false; + } + + const b3QHHalfEdge* edge = face->edge; + + do + { + const b3QHHalfEdge* twin = edge->twin; + + if ( twin == NULL ) + { + return false; + } + if ( twin->face == NULL ) + { + return false; + } + if ( twin->face == face ) + { + return false; + } + if ( twin->face->mark == B3_MARK_DELETE ) + { + return false; + } + if ( twin->twin != edge ) + { + return false; + } + if ( edge->next->origin != twin->origin ) + { + return false; + } + if ( edge->origin != twin->next->origin ) + { + return false; + } + if ( edge->face != face ) + { + return false; + } + + edge = edge->next; + } + while ( edge != face->edge ); + + return true; +} +#endif + +static void b3HullBuilder_ComputeTolerance( b3HullBuilder* b, int pointCount, const b3Vec3* points ) +{ + b3AABB bounds = b3BuildBounds( pointCount, points ); + b3Vec3 maxAbs = b3Max( b3Abs( bounds.lowerBound ), b3Abs( bounds.upperBound ) ); + + float maxSum = maxAbs.x + maxAbs.y + maxAbs.z; + float maxCoord = b3MaxFloat( maxAbs.x, b3MaxFloat( maxAbs.y, maxAbs.z ) ); + float maxDistance = b3MinFloat( B3_SQRT3 * maxCoord, maxSum ); + + float tolerance = ( 3.0f * maxDistance * 1.01f + maxCoord ) * FLT_EPSILON; + + b->tolerance = tolerance; + b->minRadius = 4.0f * b->tolerance; + b->minOutside = 2.0f * b->minRadius; + B3_ASSERT( b->minRadius < b->minOutside + 3.0f * FLT_EPSILON ); +} + +static bool b3HullBuilder_BuildInitialHull( b3HullBuilder* b, int pointCount, const b3Vec3* points ) +{ + int index1, index2; + b3FindFarthestPointsAlongCardinalAxes( &index1, &index2, b->tolerance, pointCount, points ); + if ( index1 < 0 || index2 < 0 ) + { + return false; + } + + int index3 = b3FindFarthestPointFromLine( index1, index2, b->tolerance, pointCount, points ); + if ( index3 < 0 ) + { + return false; + } + + int index4 = b3FindFarthestPointFromPlane( index1, index2, index3, b->tolerance, pointCount, points ); + if ( index4 < 0 ) + { + return false; + } + + b3Vec3 v1 = b3Sub( points[index1], points[index4] ); + b3Vec3 v2 = b3Sub( points[index2], points[index4] ); + b3Vec3 v3 = b3Sub( points[index3], points[index4] ); + + if ( b3ScalarTripleProduct( v1, v2, v3 ) < 0.0f ) + { + int temp = index2; + index2 = index3; + index3 = temp; + } + + b->interiorPoint = b3Vec3_zero; + b->interiorPoint = b3Add( b->interiorPoint, points[index1] ); + b->interiorPoint = b3Add( b->interiorPoint, points[index2] ); + b->interiorPoint = b3Add( b->interiorPoint, points[index3] ); + b->interiorPoint = b3Add( b->interiorPoint, points[index4] ); + b->interiorPoint = b3MulSV( 0.25f, b->interiorPoint ); + + b3QHVertex* vertex1 = b3HullBuilder_NewVertex( b, points[index1] ); + b3QHList_PushBack( &b->vertexList.link, &vertex1->link ); + b3QHVertex* vertex2 = b3HullBuilder_NewVertex( b, points[index2] ); + b3QHList_PushBack( &b->vertexList.link, &vertex2->link ); + b3QHVertex* vertex3 = b3HullBuilder_NewVertex( b, points[index3] ); + b3QHList_PushBack( &b->vertexList.link, &vertex3->link ); + b3QHVertex* vertex4 = b3HullBuilder_NewVertex( b, points[index4] ); + b3QHList_PushBack( &b->vertexList.link, &vertex4->link ); + + b3QHFace* face1 = b3HullBuilder_NewFace( b, vertex1, vertex2, vertex3 ); + b3QHList_PushBack( &b->faceList.link, &face1->link ); + b3QHFace* face2 = b3HullBuilder_NewFace( b, vertex4, vertex2, vertex1 ); + b3QHList_PushBack( &b->faceList.link, &face2->link ); + b3QHFace* face3 = b3HullBuilder_NewFace( b, vertex4, vertex3, vertex2 ); + b3QHList_PushBack( &b->faceList.link, &face3->link ); + b3QHFace* face4 = b3HullBuilder_NewFace( b, vertex4, vertex1, vertex3 ); + b3QHList_PushBack( &b->faceList.link, &face4->link ); + + b3LinkFaces( face1, 0, face2, 1 ); + b3LinkFaces( face1, 1, face3, 1 ); + b3LinkFaces( face1, 2, face4, 1 ); + + b3LinkFaces( face2, 0, face3, 2 ); + b3LinkFaces( face3, 0, face4, 2 ); + b3LinkFaces( face4, 0, face2, 2 ); + +#if B3_DEBUG + B3_ASSERT( b3CheckConsistency( face1 ) ); + B3_ASSERT( b3CheckConsistency( face2 ) ); + B3_ASSERT( b3CheckConsistency( face3 ) ); + B3_ASSERT( b3CheckConsistency( face4 ) ); +#endif + + for ( int index = 0; index < pointCount; ++index ) + { + if ( index == index1 || index == index2 || index == index3 || index == index4 ) + { + continue; + } + + b3Vec3 point = points[index]; + + float maxDistance = b->minOutside; + b3QHFace* maxFace = NULL; + + for ( b3QHListNode* node = b->faceList.link.next; node != &b->faceList.link; node = node->next ) + { + b3QHFace* face = (b3QHFace*)node; + float distance = b3PlaneSeparation( face->plane, point ); + if ( distance > maxDistance ) + { + maxDistance = distance; + maxFace = face; + } + } + + if ( maxFace != NULL ) + { + b3QHVertex* vertex = b3HullBuilder_NewVertex( b, point ); + vertex->conflictFace = maxFace; + b3QHList_PushBack( &maxFace->conflictListHead.link, &vertex->link ); + if ( maxDistance > maxFace->maxConflictDistance ) + { + maxFace->maxConflictDistance = maxDistance; + maxFace->maxConflict = vertex; + } + } + } + + return true; +} + +// Recompute the farthest-conflict cache after a face's plane changes. +// Walks the existing conflict list once; cost is bounded by that list, not the global pool. +static void b3HullBuilder_RecacheConflicts( b3QHFace* face, float minOutside ) +{ + b3QHVertex* maxVertex = NULL; + float maxDistance = minOutside; + + for ( b3QHListNode* node = face->conflictListHead.link.next; node != &face->conflictListHead.link; node = node->next ) + { + b3QHVertex* vertex = (b3QHVertex*)node; + float distance = b3PlaneSeparation( face->plane, vertex->position ); + if ( distance > maxDistance ) + { + maxDistance = distance; + maxVertex = vertex; + } + } + + face->maxConflict = maxVertex; + face->maxConflictDistance = maxDistance; +} + +static b3QHVertex* b3HullBuilder_NextConflictVertex( const b3HullBuilder* b ) +{ + b3QHVertex* maxVertex = NULL; + float maxDistance = b->minOutside; + + for ( const b3QHListNode* faceNode = b->faceList.link.next; faceNode != &b->faceList.link; faceNode = faceNode->next ) + { + const b3QHFace* face = (const b3QHFace*)faceNode; + if ( face->maxConflict != NULL && face->maxConflictDistance > maxDistance ) + { + maxDistance = face->maxConflictDistance; + maxVertex = face->maxConflict; + } + } + + return maxVertex; +} + +// Move every conflict vertex of `face` onto the orphaned list and clear their conflictFace. +static void b3HullBuilder_DrainConflictList( b3HullBuilder* b, b3QHFace* face ) +{ + b3QHListNode* node = face->conflictListHead.link.next; + while ( node != &face->conflictListHead.link ) + { + b3QHVertex* orphan = (b3QHVertex*)node; + node = node->next; + + orphan->conflictFace = NULL; + b3QHList_Remove( &orphan->link ); + b3QHList_PushBack( &b->orphanedList.link, &orphan->link ); + } + B3_ASSERT( B3_LIST_EMPTY( &face->conflictListHead.link ) ); +} + +// Mark a face for deletion, drain its conflict list, and populate a fresh DFS frame for it. +// `entryEdge` is the half-edge in `face` whose twin lies in the just-deleted parent face, or +// NULL for the seed. The frame skips `entryEdge` on recursive entries (it would be ignored +// anyway since the parent's mark is now DELETE, but skipping saves one iteration). +static void b3HullBuilder_EnterHorizonFace( b3HullBuilder* b, b3QHFace* face, b3QHHalfEdge* entryEdge, b3HorizonFrame* frameOut ) +{ + face->mark = B3_MARK_DELETE; + b3HullBuilder_DrainConflictList( b, face ); + + frameOut->face = face; + frameOut->started = false; + if ( entryEdge != NULL ) + { + frameOut->startEdge = entryEdge; + frameOut->edge = entryEdge->next; + } + else + { + frameOut->startEdge = face->edge; + frameOut->edge = face->edge; + } +} + +static void b3HullBuilder_BuildHorizon( b3HullBuilder* b, b3QHVertex* apex, b3QHFace* seed ) +{ + b3HorizonFrame* stack = b->horizonStack; + int top = 0; + + B3_ASSERT( top < b->horizonStackCapacity ); + b3HullBuilder_EnterHorizonFace( b, seed, NULL, &stack[top++] ); + + while ( top > 0 ) + { + b3HorizonFrame* f = &stack[top - 1]; + + if ( f->started && f->edge == f->startEdge ) + { + top--; + continue; + } + f->started = true; + + b3QHHalfEdge* edge = f->edge; + b3QHHalfEdge* twin = edge->twin; + f->edge = edge->next; + + if ( twin->face->mark != B3_MARK_VISIBLE ) + { + continue; + } + + float distance = b3PlaneSeparation( twin->face->plane, apex->position ); + if ( distance > b->minRadius ) + { + B3_ASSERT( top < b->horizonStackCapacity ); + b3HullBuilder_EnterHorizonFace( b, twin->face, twin, &stack[top++] ); + } + else + { + B3_ASSERT( b->horizonCount < b->horizonCapacity ); + b->horizon[b->horizonCount++] = edge; + } + } +} + +static void b3HullBuilder_BuildCone( b3HullBuilder* b, b3QHVertex* apex ) +{ + for ( int i = 0; i < b->horizonCount; ++i ) + { + b3QHHalfEdge* edge = b->horizon[i]; + B3_ASSERT( edge->twin->twin == edge ); + + b3QHFace* face = b3HullBuilder_NewFace( b, apex, edge->origin, edge->twin->origin ); + B3_ASSERT( b->coneCount < b->coneCapacity ); + b->cone[b->coneCount++] = face; + + b3LinkFace( face, 1, edge->twin ); + } + + b3QHFace* face1 = b->cone[b->coneCount - 1]; + for ( int i = 0; i < b->coneCount; ++i ) + { + b3QHFace* face2 = b->cone[i]; + b3LinkFaces( face1, 2, face2, 0 ); + face1 = face2; + } +} + +// Retire half-edges in the half-open ring range [begin, end) by pushing each onto edgeFreeHead. +// The caller has already detached these edges from the live face ring (rewire happens before +// destroy), so they are unreachable from the live hull. +static void b3HullBuilder_DestroyEdges( b3HullBuilder* b, b3QHHalfEdge* begin, b3QHHalfEdge* end ) +{ + b3QHHalfEdge* edge = begin; + while ( edge != end ) + { + b3QHHalfEdge* next = edge->next; + b3HullBuilder_RetireEdge( b, edge ); + edge = next; + } +} + +static void b3HullBuilder_ConnectEdges( b3HullBuilder* b, b3QHHalfEdge* prev, b3QHHalfEdge* next ) +{ + B3_ASSERT( prev != next ); + B3_ASSERT( prev->face == next->face ); + + // If both shared neighbors are the same face, prev and next together would orphan that face. + if ( prev->twin->face == next->twin->face ) + { + // next is redundant. + if ( next->face->edge == next ) + { + next->face->edge = prev; + } + + b3QHHalfEdge* twin; + if ( b3VertexCountOfFace( prev->twin->face ) == 3 ) + { + // Capture all 3 half-edges of the dead triangle before the rewire overwrites prev->twin. + b3QHHalfEdge* deadEdge0 = prev->twin; // prev->twin (will be rewired below) + b3QHHalfEdge* deadEdge1 = next->twin; // next->twin + b3QHHalfEdge* deadEdge2 = next->twin->prev; // third edge of the dead triangle + + twin = deadEdge2->twin; + B3_ASSERT( twin->face->mark != B3_MARK_DELETE ); + + b3QHFace* opposingFace = prev->twin->face; + opposingFace->mark = B3_MARK_DELETE; + B3_ASSERT( b->mergedFacesCount < b->mergedFacesCapacity ); + b->mergedFaces[b->mergedFacesCount++] = opposingFace; + + prev->next = next->next; + prev->next->prev = prev; + + prev->twin = twin; + twin->twin = prev; + + // Drop the redundant vertex (slot abandoned in the bump allocator). + b3QHList_Remove( &next->origin->link ); + + // Retire the 3 half-edges of the dead triangle now that the rewire is complete. + b3HullBuilder_RetireEdge( b, deadEdge0 ); + b3HullBuilder_RetireEdge( b, deadEdge1 ); + b3HullBuilder_RetireEdge( b, deadEdge2 ); + } + else + { + twin = next->twin; + + if ( twin->face->edge == prev->twin ) + { + twin->face->edge = twin; + } + + twin->next = prev->twin->next; + twin->next->prev = twin; + // prev->twin slot is retired to the edge free list. + b3HullBuilder_RetireEdge( b, prev->twin ); + + prev->next = next->next; + prev->next->prev = prev; + + prev->twin = twin; + twin->twin = prev; + + // Drop the redundant vertex (slot abandoned in the bump allocator). + b3QHList_Remove( &next->origin->link ); + } + + // Twin->face changed shape; recompute its plane and refresh its cached max conflict. + b3NewellPlane( twin->face ); + b3HullBuilder_RecacheConflicts( twin->face, b->minOutside ); + } + else + { + prev->next = next; + next->prev = prev; + } +} + +static void b3HullBuilder_AbsorbFaces( b3HullBuilder* b, b3QHFace* face ) +{ + for ( int i = 0; i < b->mergedFacesCount; ++i ) + { + B3_ASSERT( b->mergedFaces[i]->mark == B3_MARK_DELETE ); + b3QHListNode* head = &b->mergedFaces[i]->conflictListHead.link; + + b3QHListNode* node = head->next; + while ( node != head ) + { + b3QHVertex* vertex = (b3QHVertex*)node; + node = node->next; + + b3QHList_Remove( &vertex->link ); + + float distance = b3PlaneSeparation( face->plane, vertex->position ); + if ( distance > b->minOutside ) + { + b3QHList_PushBack( &face->conflictListHead.link, &vertex->link ); + vertex->conflictFace = face; + if ( distance > face->maxConflictDistance ) + { + face->maxConflictDistance = distance; + face->maxConflict = vertex; + } + } + else + { + b3QHList_PushBack( &b->orphanedList.link, &vertex->link ); + vertex->conflictFace = NULL; + } + } + + B3_ASSERT( B3_LIST_EMPTY( head ) ); + + // Conflict list is now drained. Retire this face to the free list. + b3HullBuilder_RetireFace( b, b->mergedFaces[i] ); + } +} + +static void b3HullBuilder_ConnectFaces( b3HullBuilder* b, b3QHHalfEdge* edge ) +{ + b3QHFace* face = edge->face; + + b3QHHalfEdge* twin = edge->twin; + + b3QHHalfEdge* edgePrev = edge->prev; + b3QHHalfEdge* edgeNext = edge->next; + b3QHHalfEdge* twinPrev = twin->prev; + b3QHHalfEdge* twinNext = twin->next; + + while ( edgePrev->twin->face == twin->face ) + { + B3_ASSERT( edgePrev->twin == twinNext ); + B3_ASSERT( twinNext->twin == edgePrev ); + + edgePrev = edgePrev->prev; + twinNext = twinNext->next; + } + B3_ASSERT( edgePrev->face != twinNext->face ); + + while ( edgeNext->twin->face == twin->face ) + { + B3_ASSERT( edgeNext->twin == twinPrev ); + B3_ASSERT( twinPrev->twin == edgeNext ); + + edgeNext = edgeNext->next; + twinPrev = twinPrev->prev; + } + B3_ASSERT( edgeNext->face != twinPrev->face ); + + face->edge = edgePrev; + + // Discard opposing face. mergedFaces is single-buffered: ConnectFaces does not nest. + b->mergedFacesCount = 0; + B3_ASSERT( b->mergedFacesCount < b->mergedFacesCapacity ); + b->mergedFaces[b->mergedFacesCount++] = twin->face; + twin->face->mark = B3_MARK_DELETE; + twin->face->edge = NULL; + + for ( b3QHHalfEdge* absorbed = twinNext; absorbed != twinPrev->next; absorbed = absorbed->next ) + { + absorbed->face = face; + } + + b3HullBuilder_DestroyEdges( b, edgePrev->next, edgeNext ); + b3HullBuilder_DestroyEdges( b, twinPrev->next, twinNext ); + + b3HullBuilder_ConnectEdges( b, edgePrev, twinNext ); + b3HullBuilder_ConnectEdges( b, twinPrev, edgeNext ); + + b3NewellPlane( face ); + // Existing conflicts now have stale distances under the new plane; AbsorbFaces will then + // add more incrementally and update the cache as it goes. + b3HullBuilder_RecacheConflicts( face, b->minOutside ); +#if B3_DEBUG + B3_ASSERT( b3CheckConsistency( face ) ); +#endif + + b3HullBuilder_AbsorbFaces( b, face ); +} + +static bool b3HullBuilder_MergeConcave( b3HullBuilder* b, b3QHFace* face ) +{ + b3QHHalfEdge* edge = face->edge; + + do + { + b3QHHalfEdge* twin = edge->twin; + + if ( b3IsEdgeConcave( edge, b->minRadius ) || b3IsEdgeConcave( twin, b->minRadius ) ) + { + b3HullBuilder_ConnectFaces( b, edge ); + return true; + } + + edge = edge->next; + } + while ( edge != face->edge ); + + return false; +} + +static bool b3HullBuilder_MergeCoplanar( b3HullBuilder* b, b3QHFace* face ) +{ + b3QHHalfEdge* edge = face->edge; + + do + { + b3QHHalfEdge* twin = edge->twin; + + if ( !b3IsEdgeConvex( edge, b->minRadius ) || !b3IsEdgeConvex( twin, b->minRadius ) ) + { + b3HullBuilder_ConnectFaces( b, edge ); + return true; + } + + edge = edge->next; + } + while ( edge != face->edge ); + + return false; +} + +static void b3HullBuilder_MergeFaces( b3HullBuilder* b ) +{ + for ( int i = 0; i < b->coneCount; ++i ) + { + b3QHFace* face = b->cone[i]; + if ( face->mark == B3_MARK_VISIBLE && face->flipped ) + { + face->flipped = false; + + float bestArea = 0; + b3QHHalfEdge* bestEdge = NULL; + + b3QHHalfEdge* edge = face->edge; + do + { + b3QHHalfEdge* twin = edge->twin; + float area = twin->face->area; + if ( area > bestArea ) + { + bestArea = area; + bestEdge = edge; + } + + edge = edge->next; + } + while ( edge != face->edge ); + + B3_ASSERT( bestEdge != NULL ); + b3HullBuilder_ConnectFaces( b, bestEdge ); + } + } + + for ( int i = 0; i < b->coneCount; ++i ) + { + b3QHFace* face = b->cone[i]; + if ( face->mark == B3_MARK_VISIBLE ) + { + while ( b3HullBuilder_MergeConcave( b, face ) ) + { + } + } + } + + for ( int i = 0; i < b->coneCount; ++i ) + { + b3QHFace* face = b->cone[i]; + if ( face->mark == B3_MARK_VISIBLE ) + { + while ( b3HullBuilder_MergeCoplanar( b, face ) ) + { + } + } + } +} + +static void b3HullBuilder_ResolveVertices( b3HullBuilder* b ) +{ + b3QHListNode* node = b->orphanedList.link.next; + while ( node != &b->orphanedList.link ) + { + b3QHVertex* vertex = (b3QHVertex*)node; + node = node->next; + b3QHList_Remove( &vertex->link ); + + float maxDistance = b->minOutside; + b3QHFace* maxFace = NULL; + + for ( int i = 0; i < b->coneCount; ++i ) + { + if ( b->cone[i]->mark == B3_MARK_VISIBLE ) + { + float distance = b3PlaneSeparation( b->cone[i]->plane, vertex->position ); + if ( distance > maxDistance ) + { + maxDistance = distance; + maxFace = b->cone[i]; + } + } + } + + if ( maxFace != NULL ) + { + B3_ASSERT( maxFace->mark == B3_MARK_VISIBLE ); + b3QHList_PushBack( &maxFace->conflictListHead.link, &vertex->link ); + vertex->conflictFace = maxFace; + if ( maxDistance > maxFace->maxConflictDistance ) + { + maxFace->maxConflictDistance = maxDistance; + maxFace->maxConflict = vertex; + } + } + // Otherwise: vertex is interior to the hull. Its slot in the bump pool is abandoned. + } + + B3_ASSERT( B3_LIST_EMPTY( &b->orphanedList.link ) ); +} + +static void b3HullBuilder_ResolveFaces( b3HullBuilder* b ) +{ + // Splice deleted faces out of the face list. Faces already retired by AbsorbFaces are no + // longer on faceList, so we guard with b3QHList_Contains before removing. + b3QHListNode* node = b->faceList.link.next; + while ( node != &b->faceList.link ) + { + b3QHFace* face = (b3QHFace*)node; + node = node->next; + + if ( face->mark == B3_MARK_DELETE && b3QHList_Contains( &face->link ) ) + { + B3_ASSERT( B3_LIST_EMPTY( &face->conflictListHead.link ) ); + b3QHList_Remove( &face->link ); + } + } + + for ( int i = 0; i < b->coneCount; ++i ) + { + b3QHFace* face = b->cone[i]; + if ( face->mark == B3_MARK_DELETE ) + { + continue; + } + b3QHList_PushBack( &b->faceList.link, &face->link ); + } +} + +static void b3HullBuilder_AddVertexToHull( b3HullBuilder* b, b3QHVertex* vertex ) +{ + b3QHFace* face = vertex->conflictFace; + vertex->conflictFace = NULL; + b3QHList_Remove( &vertex->link ); + b3QHList_PushBack( &b->vertexList.link, &vertex->link ); + + b->horizonCount = 0; + b3HullBuilder_BuildHorizon( b, vertex, face ); + B3_ASSERT( b->horizonCount >= 3 ); + + b->coneCount = 0; + b3HullBuilder_BuildCone( b, vertex ); + B3_ASSERT( b->coneCount >= 3 ); + + b3HullBuilder_MergeFaces( b ); + b3HullBuilder_ResolveVertices( b ); + b3HullBuilder_ResolveFaces( b ); +} + +static void b3HullBuilder_CleanHull( b3HullBuilder* b, b3Vec3 origin ) +{ + int faceCount = 0; + int halfEdgeCount = 0; + + for ( b3QHListNode* faceNode = b->faceList.link.next; faceNode != &b->faceList.link; faceNode = faceNode->next ) + { + b3QHFace* face = (b3QHFace*)faceNode; + b3QHHalfEdge* edge = face->edge; + + do + { + edge->origin->reachable = true; + edge = edge->next; + halfEdgeCount++; + } + while ( edge != face->edge ); + + face->plane.offset += b3Dot( face->plane.normal, origin ); + face->centroid = b3Add( face->centroid, origin ); + faceCount++; + } + + int vertexCount = 0; + b3QHListNode* node = b->vertexList.link.next; + while ( node != &b->vertexList.link ) + { + b3QHVertex* vertex = (b3QHVertex*)node; + node = node->next; + + if ( !vertex->reachable ) + { + b3QHList_Remove( &vertex->link ); + } + else + { + vertex->position = b3Add( vertex->position, origin ); + vertexCount++; + } + } + + b->interiorPoint = b3Add( b->interiorPoint, origin ); + + b->finalVertexCount = vertexCount; + b->finalHalfEdgeCount = halfEdgeCount; + b->finalFaceCount = faceCount; +} + +#if B3_DEBUG +static bool b3HullBuilder_IsConsistent( const b3HullBuilder* b ) +{ + int v = b->finalVertexCount; + int e = b->finalHalfEdgeCount / 2; + int f = b->finalFaceCount; + + if ( v - e + f != 2 ) + { + return false; + } + + for ( const b3QHListNode* faceNode = b->faceList.link.next; faceNode != &b->faceList.link; faceNode = faceNode->next ) + { + const b3QHFace* face = (const b3QHFace*)faceNode; + if ( face->edge->face != face ) + { + return false; + } + + if ( !b3CheckConsistency( face ) ) + { + return false; + } + + if ( b3PlaneSeparation( face->plane, b->interiorPoint ) > 0 ) + { + return false; + } + + if ( face->mark != B3_MARK_VISIBLE ) + { + return false; + } + + const b3QHHalfEdge* edge = face->edge; + + do + { + if ( edge->next->origin != edge->twin->origin ) + { + return false; + } + if ( edge->prev->next != edge ) + { + return false; + } + if ( edge->next->prev != edge ) + { + return false; + } + if ( edge->twin->twin != edge ) + { + return false; + } + if ( edge->face != face ) + { + return false; + } + if ( b3DistanceSquared( edge->origin->position, edge->twin->origin->position ) < 1000.0f * FLT_MIN ) + { + return false; + } + + edge = edge->next; + } + while ( edge != face->edge ); + } + + return true; +} +#endif + +static bool b3HullBuilder_HasHull( const b3HullBuilder* b ) +{ + int v = b->finalVertexCount; + int e = b->finalHalfEdgeCount / 2; + int f = b->finalFaceCount; + return v - e + f == 2 && f >= 4; +} + +// Build the entire hull. Returns true iff the result satisfies Euler's identity. +static bool b3HullBuilder_Construct( b3HullBuilder* b, const b3Vec3* points, int pointCount, int maxVertexCount, b3Vec3 origin, + b3Vec3* shiftedPoints ) +{ + if ( pointCount < 4 ) + { + return false; + } + + for ( int i = 0; i < pointCount; ++i ) + { + shiftedPoints[i] = b3Sub( points[i], origin ); + } + + b3HullBuilder_ComputeTolerance( b, pointCount, shiftedPoints ); + if ( !b3HullBuilder_BuildInitialHull( b, pointCount, shiftedPoints ) ) + { + return false; + } + + int budget = b3ClampInt( maxVertexCount - 4, 0, B3_HULL_LIMIT - 4 ); + + b3QHVertex* vertex = b3HullBuilder_NextConflictVertex( b ); + while ( vertex && budget > 0 ) + { + b3HullBuilder_AddVertexToHull( b, vertex ); + vertex = b3HullBuilder_NextConflictVertex( b ); + budget -= 1; + } + + b3HullBuilder_CleanHull( b, origin ); + +#if B3_DEBUG + B3_ASSERT( b3HullBuilder_IsConsistent( b ) ); +#endif + + return b3HullBuilder_HasHull( b ); +} + +typedef struct b3HullWorkSizes +{ + int N; // pointCount + int M; // clamped maxVertexCount, in [4, B3_HULL_LIMIT] + int vertexCapacity; + int edgeCapacity; + int faceCapacity; + int horizonCapacity; + int coneCapacity; + int mergedFacesCapacity; + int horizonStackCapacity; + size_t totalBytes; + + size_t offsetVertex; + size_t offsetEdge; + size_t offsetFace; + size_t offsetHorizon; + size_t offsetCone; + size_t offsetMergedFaces; + size_t offsetHorizonStack; + size_t offsetShiftedPoints; +} b3HullWorkSizes; + +static b3HullWorkSizes b3ComputeHullWorkSizes( int pointCount, int clampedMaxCount ) +{ + b3HullWorkSizes s; + s.N = pointCount; + s.M = clampedMaxCount; + + // Vertices: 4 initial hull vertices + at most one per remaining input point. No free list. + s.vertexCapacity = pointCount + 4; + + // Edges and faces use free-list recycling; capacity is proportional to live hull size. + // edgeCapacity: peak is ~twice live edges plus cone edges; floor 48. + s.edgeCapacity = 24 * s.M - 48; + if ( s.edgeCapacity < 48 ) + { + s.edgeCapacity = 48; + } + + // faceCapacity: peak intermediate state live faces (<=2*M-4) plus full cone (<=3*M-6); floor 16. + s.faceCapacity = 5 * s.M - 10; + if ( s.faceCapacity < 16 ) + { + s.faceCapacity = 16; + } + + // Horizon/cone bounded by current half-edge count; mergedFaces by face count. + s.horizonCapacity = 3 * s.M - 6; + if ( s.horizonCapacity < 6 ) + { + s.horizonCapacity = 6; + } + s.coneCapacity = s.horizonCapacity; + s.mergedFacesCapacity = 2 * s.M - 4; + if ( s.mergedFacesCapacity < 4 ) + { + s.mergedFacesCapacity = 4; + } + + // Horizon DFS depth is bounded by the number of live faces (Euler: <=2*M-4). + s.horizonStackCapacity = 2 * s.M - 4; + if ( s.horizonStackCapacity < 4 ) + { + s.horizonStackCapacity = 4; + } + + size_t offset = 0; + + s.offsetVertex = offset; + offset = b3AlignUp8( offset + (size_t)s.vertexCapacity * sizeof( b3QHVertex ) ); + + s.offsetEdge = offset; + offset = b3AlignUp8( offset + (size_t)s.edgeCapacity * sizeof( b3QHHalfEdge ) ); + + s.offsetFace = offset; + offset = b3AlignUp8( offset + (size_t)s.faceCapacity * sizeof( b3QHFace ) ); + + s.offsetHorizon = offset; + offset = b3AlignUp8( offset + (size_t)s.horizonCapacity * sizeof( b3QHHalfEdge* ) ); + + s.offsetCone = offset; + offset = b3AlignUp8( offset + (size_t)s.coneCapacity * sizeof( b3QHFace* ) ); + + s.offsetMergedFaces = offset; + offset = b3AlignUp8( offset + (size_t)s.mergedFacesCapacity * sizeof( b3QHFace* ) ); + + s.offsetHorizonStack = offset; + offset = b3AlignUp8( offset + (size_t)s.horizonStackCapacity * sizeof( b3HorizonFrame ) ); + + s.offsetShiftedPoints = offset; + offset += (size_t)pointCount * sizeof( b3Vec3 ); + + s.totalBytes = offset; + return s; +} + +static void b3HullBuilder_Init( b3HullBuilder* b, char* mem, const b3HullWorkSizes* s ) +{ + memset( b, 0, sizeof( *b ) ); + b3QHList_Init( &b->orphanedList.link ); + b3QHList_Init( &b->vertexList.link ); + b3QHList_Init( &b->faceList.link ); + + b->vertexBase = (b3QHVertex*)( mem + s->offsetVertex ); + b->vertexCapacity = s->vertexCapacity; + + b->edgeBase = (b3QHHalfEdge*)( mem + s->offsetEdge ); + b->edgeCapacity = s->edgeCapacity; + + b->faceBase = (b3QHFace*)( mem + s->offsetFace ); + b->faceCapacity = s->faceCapacity; + + b->horizon = (b3QHHalfEdge**)( mem + s->offsetHorizon ); + b->horizonCapacity = s->horizonCapacity; + + b->cone = (b3QHFace**)( mem + s->offsetCone ); + b->coneCapacity = s->coneCapacity; + + b->mergedFaces = (b3QHFace**)( mem + s->offsetMergedFaces ); + b->mergedFacesCapacity = s->mergedFacesCapacity; + + b->horizonStack = (b3HorizonFrame*)( mem + s->offsetHorizonStack ); + b->horizonStackCapacity = s->horizonStackCapacity; +} + +static b3Vec3* b3GetHullPointsWrite( b3HullData* hull ) +{ + if ( hull->pointOffset == 0 ) + { + return NULL; + } + return (b3Vec3*)( (intptr_t)hull + hull->pointOffset ); +} + +static b3Plane* b3GetHullPlanesWrite( b3HullData* hull ) +{ + if ( hull->planeOffset == 0 ) + { + return NULL; + } + return (b3Plane*)( (intptr_t)hull + hull->planeOffset ); +} + +static b3HullVertex* b3GetHullVerticesWrite( b3HullData* hull ) +{ + if ( hull->vertexOffset == 0 ) + { + return NULL; + } + return (b3HullVertex*)( (intptr_t)hull + hull->vertexOffset ); +} + +static b3HullHalfEdge* b3GetHullEdgesWrite( b3HullData* hull ) +{ + if ( hull->edgeOffset == 0 ) + { + return NULL; + } + return (b3HullHalfEdge*)( (intptr_t)hull + hull->edgeOffset ); +} + +int b3FindHullSupportVertex( const b3HullData* hull, b3Vec3 direction ) +{ + int bestIndex = B3_NULL_INDEX; + float bestDot = -FLT_MAX; + + int vertexCount = hull->vertexCount; + const b3Vec3* points = b3GetHullPoints( hull ); + + for ( int index = 0; index < vertexCount; ++index ) + { + float dot = b3Dot( direction, points[index] ); + if ( dot > bestDot ) + { + bestIndex = index; + bestDot = dot; + } + } + B3_ASSERT( bestIndex >= 0 ); + + return bestIndex; +} + +int b3FindHullSupportFace( const b3HullData* hull, b3Vec3 direction ) +{ + int bestIndex = B3_NULL_INDEX; + float bestDot = -FLT_MAX; + + int faceCount = hull->faceCount; + const b3Plane* planes = b3GetHullPlanes( hull ); + + for ( int index = 0; index < faceCount; ++index ) + { + float dot = b3Dot( planes[index].normal, direction ); + if ( dot > bestDot ) + { + bestDot = dot; + bestIndex = index; + } + } + B3_ASSERT( bestIndex >= 0 ); + + return bestIndex; +} + +#if B3_ENABLE_VALIDATION + +bool b3IsValidHull( const b3HullData* hull ) +{ + if ( hull->version != B3_HULL_VERSION ) + { + return false; + } + + int v = hull->vertexCount; + int e = hull->edgeCount / 2; + int f = hull->faceCount; + + if ( v - e + f != 2 ) + { + return false; + } + + const b3HullVertex* vertices = b3GetHullVertices( hull ); + const b3HullHalfEdge* edges = b3GetHullEdges( hull ); + for ( int index = 0; index < hull->vertexCount; ++index ) + { + const b3HullVertex* vertex = vertices + index; + const b3HullHalfEdge* edge = edges + vertex->edge; + + if ( edge->origin != index ) + { + return false; + } + } + + for ( int index = 0; index < hull->edgeCount; index += 2 ) + { + const b3HullHalfEdge* edge = edges + index + 0; + const b3HullHalfEdge* twin = edges + index + 1; + + if ( edge->twin != index + 1 ) + { + return false; + } + + if ( twin->twin != index + 0 ) + { + return false; + } + } + + const b3HullFace* faces = b3GetHullFaces( hull ); + const b3Plane* planes = b3GetHullPlanes( hull ); + for ( int faceIndex = 0; faceIndex < hull->faceCount; ++faceIndex ) + { + const b3HullFace* face = faces + faceIndex; + + int baseEdgeIndex = face->edge; + const b3HullHalfEdge* edge = edges + baseEdgeIndex; + + b3Plane plane = planes[faceIndex]; + if ( b3PlaneSeparation( plane, hull->center ) >= 0.0f ) + { + return false; + } + + int edgeIndex = baseEdgeIndex; + do + { + edge = edges + edgeIndex; + const b3HullHalfEdge* next = edges + edge->next; + const b3HullHalfEdge* twin = edges + edge->twin; + + if ( edge->face != faceIndex ) + { + return false; + } + + if ( twin->twin != edgeIndex ) + { + return false; + } + + if ( next->origin != twin->origin ) + { + return false; + } + + edgeIndex = edge->next; + } + while ( edgeIndex != baseEdgeIndex ); + } + + if ( hull->volume <= 0.0f ) + { + return false; + } + + if ( hull->surfaceArea <= 0.0f ) + { + return false; + } + + if ( hull->innerRadius <= 0.0f ) + { + return false; + } + + return true; +} + +#else + +bool b3IsValidHull( const b3HullData* hull ) +{ + B3_UNUSED( hull ); + return true; +} + +#endif + +b3HullData* b3CreateCylinder( float height, float radius, float yOffset, int sides ) +{ + B3_ASSERT( height > 0.0f ); + B3_ASSERT( radius > 0.0f ); + B3_ASSERT( 3 <= sides && sides <= 32 ); + + int pointCount = 2 * sides; + b3Vec3* points = (b3Vec3*)b3Alloc( pointCount * sizeof( b3Vec3 ) ); + B3_ASSERT( points != NULL ); + + float alpha = 0.0f; + float deltaAlpha = 2.0f * B3_PI / sides; + + for ( int index = 0; index < sides; ++index ) + { + float sinAlpha = b3Sin( alpha ); + float cosAlpha = b3Cos( alpha ); + + points[2 * index + 0] = (b3Vec3){ radius * cosAlpha, yOffset, radius * sinAlpha }; + points[2 * index + 1] = (b3Vec3){ radius * cosAlpha, yOffset + height, radius * sinAlpha }; + + alpha += deltaAlpha; + } + + b3HullData* hull = b3CreateHull( points, pointCount, pointCount ); + B3_ASSERT( hull->vertexCount == pointCount ); + B3_ASSERT( hull->edgeCount == 6 * sides ); + B3_ASSERT( hull->faceCount == sides + 2 ); + + b3Free( points, pointCount * sizeof( b3Vec3 ) ); + + return hull; +} + +b3HullData* b3CreateCone( float height, float radius1, float radius2, int slices ) +{ + B3_ASSERT( height > 0.0f ); + B3_ASSERT( radius1 > 0.0f ); + B3_ASSERT( radius2 > 0.0f ); + B3_ASSERT( 4 <= slices && slices <= 32 ); + + int pointCount = 2 * slices; + b3Vec3* points = (b3Vec3*)b3Alloc( pointCount * sizeof( b3Vec3 ) ); + B3_ASSERT( points != NULL ); + + float alpha = 0.0f; + float deltaAlpha = 2.0f * B3_PI / slices; + + for ( int index = 0; index < slices; ++index ) + { + float sinAlpha = b3Sin( alpha ); + float cosAlpha = b3Cos( alpha ); + + points[2 * index + 0] = (b3Vec3){ radius1 * cosAlpha, 0.0f, radius1 * sinAlpha }; + points[2 * index + 1] = (b3Vec3){ radius2 * cosAlpha, height, radius2 * sinAlpha }; + + alpha += deltaAlpha; + } + + b3HullData* hull = b3CreateHull( points, pointCount, pointCount ); + B3_ASSERT( hull->vertexCount == pointCount ); + B3_ASSERT( hull->edgeCount == 6 * slices ); + B3_ASSERT( hull->faceCount == slices + 2 ); + + b3Free( points, pointCount * sizeof( b3Vec3 ) ); + + return hull; +} + +b3HullData* b3CreateRock( float radius ) +{ + int pointCount = 10; + + // Golden ratio + const float phi = ( 1.0f + sqrtf( 5.0f ) ) / 2.0f; + + // Fibonacci lattice + b3Vec3 points[10]; + + // Azimuthal angle + float theta = 2.0f * B3_PI / phi; + + b3CosSin cs = { 1.0f, 0.0 }; + b3CosSin deltaCS = b3ComputeCosSin( theta ); + + for ( int i = 0; i < pointCount; ++i ) + { + // Z coordinate + float z = 1.0f - ( 2.0f * i + 1.0f ) / pointCount; + // Radius in xy-plane + float radius_XY = sqrtf( 1.0f - z * z ); + + points[i].x = radius * radius_XY * cs.cosine; + points[i].y = radius * radius_XY * cs.sine; + points[i].z = radius * z; + + b3CosSin cs0 = cs; + cs.cosine = deltaCS.cosine * cs0.cosine - deltaCS.sine * cs0.sine; + cs.sine = deltaCS.sine * cs0.cosine + deltaCS.cosine * cs0.sine; + } + + return b3CreateHull( points, pointCount, pointCount ); +} + +static void b3UpdateHullBounds( b3HullData* hull ) +{ + const b3Vec3* points = b3GetHullPoints( hull ); + int vertexCount = hull->vertexCount; + + B3_ASSERT( vertexCount > 0 ); + b3AABB bounds; + bounds.lowerBound = points[0]; + bounds.upperBound = points[0]; + + for ( int i = 1; i < vertexCount; ++i ) + { + b3Vec3 p = points[i]; + bounds.lowerBound = b3Min( bounds.lowerBound, p ); + bounds.upperBound = b3Max( bounds.upperBound, p ); + } + + hull->aabb = bounds; +} + +// M. Kallay - "Computing the Moment of Inertia of a Solid Defined by a Triangle Mesh" +static bool b3UpdateHullBulkProperties( b3HullData* hull ) +{ + const b3Vec3* points = b3GetHullPoints( hull ); + const b3HullFace* faces = b3GetHullFaces( hull ); + const b3HullHalfEdge* edges = b3GetHullEdges( hull ); + const b3Plane* planes = b3GetHullPlanes( hull ); + + float area = 0.0f; + float volume = 0.0f; + b3Vec3 center = b3Vec3_zero; + + // Use the first vertex to reduce round-off errors. + b3Vec3 origin = points[0]; + + float xx = 0.0f; + float xy = 0.0f; + float yy = 0.0f; + float xz = 0.0f; + float zz = 0.0f; + float yz = 0.0f; + + int faceCount = hull->faceCount; + + for ( int faceIndex = 0; faceIndex < faceCount; ++faceIndex ) + { + const b3HullFace* face = faces + faceIndex; + const b3HullHalfEdge* edge1 = edges + face->edge; + const b3HullHalfEdge* edge2 = edges + edge1->next; + const b3HullHalfEdge* edge3 = edges + edge2->next; + + B3_ASSERT( edge1 != edge3 ); + B3_ASSERT( edge1->origin < hull->vertexCount ); + + b3Vec3 v1 = b3Sub( points[edge1->origin], origin ); + + do + { + B3_ASSERT( edge2->origin < hull->vertexCount ); + B3_ASSERT( edge3->origin < hull->vertexCount ); + + b3Vec3 v2 = b3Sub( points[edge2->origin], origin ); + b3Vec3 v3 = b3Sub( points[edge3->origin], origin ); + + area += b3Length( b3Cross( b3Sub( v2, v1 ), b3Sub( v3, v1 ) ) ); + + float det = b3ScalarTripleProduct( v1, v2, v3 ); + + volume += det; + + b3Vec3 v4 = b3Add( v1, b3Add( v2, v3 ) ); + center = b3Add( center, b3MulSV( det, v4 ) ); + + xx += det * ( v1.x * v1.x + v2.x * v2.x + v3.x * v3.x + v4.x * v4.x ); + yy += det * ( v1.y * v1.y + v2.y * v2.y + v3.y * v3.y + v4.y * v4.y ); + zz += det * ( v1.z * v1.z + v2.z * v2.z + v3.z * v3.z + v4.z * v4.z ); + xy += det * ( v1.x * v1.y + v2.x * v2.y + v3.x * v3.y + v4.x * v4.y ); + xz += det * ( v1.x * v1.z + v2.x * v2.z + v3.x * v3.z + v4.x * v4.z ); + yz += det * ( v1.y * v1.z + v2.y * v2.z + v3.y * v3.z + v4.y * v4.z ); + + edge2 = edge3; + edge3 = edges + edge3->next; + } + while ( edge1 != edge3 ); + } + + B3_VALIDATE( volume > 0.0f ); + + b3Vec3 localCenter = volume > 0.0f ? b3MulSV( 0.25f / volume, center ) : b3Vec3_zero; + center = b3Add( localCenter, origin ); + + float radius = FLT_MAX; + for ( int faceIndex = 0; faceIndex < faceCount; ++faceIndex ) + { + b3Plane plane = planes[faceIndex]; + float distance = b3PlaneSeparation( plane, center ); + B3_VALIDATE( distance < 0.0f ); + + radius = b3MinFloat( radius, -distance ); + } + + B3_VALIDATE( 0.0f < radius && radius < FLT_MAX ); + + b3Matrix3 inertia; + inertia.cx.x = yy + zz; + inertia.cy.x = -xy; + inertia.cz.x = -xz; + inertia.cx.y = -xy; + inertia.cy.y = xx + zz; + inertia.cz.y = -yz; + inertia.cx.z = -xz; + inertia.cy.z = -yz; + inertia.cz.z = xx + yy; + + float mass = volume / 6.0f; + + b3Matrix3 centralInertia = b3MulSM( 1.0f / 120.0f, inertia ); + centralInertia = b3SubMM( centralInertia, b3Steiner( mass, localCenter ) ); + + hull->center = center; + hull->centralInertia = centralInertia; + hull->volume = mass; + hull->surfaceArea = 0.5f * area; + hull->innerRadius = radius; + + if ( mass <= 0.0f ) + { + return false; + } + + if ( volume <= 0.0f ) + { + return false; + } + + if ( area <= 0.0f ) + { + return false; + } + + if ( radius <= 0.0f ) + { + return false; + } + + return true; +} + +b3HullData* b3CreateHull( const b3Vec3* points, int pointCount, int maxVertexCount ) +{ + if ( pointCount < 4 ) + { + return NULL; + } + + b3Vec3 origin = points[0]; + int clampedMaxCount = b3ClampInt( maxVertexCount, 4, B3_HULL_LIMIT ); + + // Single allocation for all working memory. + b3HullWorkSizes sizes = b3ComputeHullWorkSizes( pointCount, clampedMaxCount ); + char* work = b3Alloc( sizes.totalBytes ); + + b3HullBuilder builder; + b3HullBuilder_Init( &builder, work, &sizes ); + + b3Vec3* shiftedPoints = (b3Vec3*)( work + sizes.offsetShiftedPoints ); + + bool ok = b3HullBuilder_Construct( &builder, points, pointCount, clampedMaxCount, origin, shiftedPoints ); + if ( !ok ) + { + b3Free( work, sizes.totalBytes ); + return NULL; + } + + if ( builder.finalVertexCount >= B3_HULL_LIMIT ) + { + b3Log( "hull final vertex count of %d exceeds limit of %d", builder.finalVertexCount, B3_HULL_LIMIT ); + b3Free( work, sizes.totalBytes ); + return NULL; + } + + if ( builder.finalFaceCount >= B3_HULL_LIMIT ) + { + b3Log( "hull final face count of %d exceeds limit of %d", builder.finalFaceCount, B3_HULL_LIMIT ); + b3Free( work, sizes.totalBytes ); + return NULL; + } + + if ( builder.finalHalfEdgeCount >= B3_HULL_LIMIT ) + { + b3Log( "hull final half edge count of %d exceeds limit of %d", builder.finalHalfEdgeCount, B3_HULL_LIMIT ); + b3Free( work, sizes.totalBytes ); + return NULL; + } + + // Walk lists into temp arrays bounded by B3_HULL_LIMIT, stamping finalIndex on each node so + // the resolution pass below is O(E + F) instead of O(E^2 + F^2). + const b3QHVertex* tempVertices[B3_HULL_LIMIT]; + int vertexCount = 0; + for ( b3QHListNode* node = builder.vertexList.link.next; node != &builder.vertexList.link; node = node->next ) + { + B3_ASSERT( vertexCount <= B3_HULL_LIMIT - 1 ); + + b3QHVertex* vertex = (b3QHVertex*)node; + vertex->finalIndex = vertexCount; + tempVertices[vertexCount++] = vertex; + } + + // Collect edges in twin-paired order (i, i+1) by stamping each pair as we discover it. + // Replaces b3SortEdges' O(E^2) twin pairing. + const b3QHFace* tempFaces[B3_HULL_LIMIT]; + const b3QHHalfEdge* tempEdges[B3_HULL_LIMIT]; + int faceCount = 0; + int edgeCount = 0; + + for ( b3QHListNode* faceNode = builder.faceList.link.next; faceNode != &builder.faceList.link; faceNode = faceNode->next ) + { + B3_ASSERT( faceCount <= B3_HULL_LIMIT - 1 ); + + b3QHFace* face = (b3QHFace*)faceNode; + face->finalIndex = faceCount; + tempFaces[faceCount++] = face; + + b3QHHalfEdge* edge = face->edge; + do + { + if ( edge->finalIndex < 0 ) + { + B3_ASSERT( edgeCount + 1 <= B3_HULL_LIMIT - 1 ); + + edge->finalIndex = edgeCount; + tempEdges[edgeCount++] = edge; + edge->twin->finalIndex = edgeCount; + tempEdges[edgeCount++] = edge->twin; + } + edge = edge->next; + } + while ( edge != face->edge ); + } + + // Allocate the hull. Arrays hang off the end. + size_t byteCount = b3AlignUp8( sizeof( b3HullData ) ); + int vertexOffset = (int)byteCount; + byteCount += b3AlignUp8( vertexCount * (int)sizeof( b3HullVertex ) ); + int pointOffset = (int)byteCount; + byteCount += b3AlignUp8( vertexCount * (int)sizeof( b3Vec3 ) ); + int edgeOffset = (int)byteCount; + byteCount += b3AlignUp8( edgeCount * (int)sizeof( b3HullHalfEdge ) ); + int faceOffset = (int)byteCount; + byteCount += b3AlignUp8( faceCount * (int)sizeof( b3HullFace ) ); + int planeOffset = (int)byteCount; + byteCount += b3AlignUp8( faceCount * (int)sizeof( b3Plane ) ); + + b3HullData* hull = b3Alloc( byteCount ); + memset( hull, 0, byteCount ); + + hull->version = B3_HULL_VERSION; + hull->vertexOffset = vertexOffset; + hull->pointOffset = pointOffset; + hull->edgeOffset = edgeOffset; + hull->faceOffset = faceOffset; + hull->planeOffset = planeOffset; + + hull->vertexCount = vertexCount; + hull->edgeCount = edgeCount; + hull->faceCount = faceCount; + + hull->byteCount = (int)byteCount; + + b3HullVertex* vertices = b3GetHullVerticesWrite( hull ); + b3HullHalfEdge* edges = b3GetHullEdgesWrite( hull ); + b3HullFace* faces = (b3HullFace*)( (intptr_t)hull + hull->faceOffset ); + b3Vec3* finalPoints = b3GetHullPointsWrite( hull ); + b3Plane* planes = b3GetHullPlanesWrite( hull ); + + for ( int index = 0; index < vertexCount; ++index ) + { + vertices[index].edge = 0; + finalPoints[index] = tempVertices[index]->position; + } + + for ( int index = 0; index < edgeCount; ++index ) + { + const b3QHHalfEdge* edge = tempEdges[index]; + B3_ASSERT( 0 <= edge->next->finalIndex && edge->next->finalIndex <= UINT8_MAX ); + B3_ASSERT( 0 <= edge->twin->finalIndex && edge->twin->finalIndex <= UINT8_MAX ); + B3_ASSERT( 0 <= edge->face->finalIndex && edge->face->finalIndex <= UINT8_MAX ); + B3_ASSERT( 0 <= edge->origin->finalIndex && edge->origin->finalIndex <= UINT8_MAX ); + + edges[index].next = (uint8_t)edge->next->finalIndex; + edges[index].twin = (uint8_t)edge->twin->finalIndex; + edges[index].face = (uint8_t)edge->face->finalIndex; + edges[index].origin = (uint8_t)edge->origin->finalIndex; + + vertices[edge->origin->finalIndex].edge = (uint8_t)index; + } + + for ( int index = 0; index < faceCount; ++index ) + { + const b3QHFace* face = tempFaces[index]; + B3_ASSERT( 0 <= face->edge->finalIndex && face->edge->finalIndex <= UINT8_MAX ); + + faces[index].edge = (uint8_t)face->edge->finalIndex; + planes[index] = face->plane; + } + + // All builder pointers are dead from here on. + b3Free( work, sizes.totalBytes ); + + b3UpdateHullBounds( hull ); + bool success = b3UpdateHullBulkProperties( hull ); + if ( success == false ) + { + b3DestroyHull( hull ); + return NULL; + } + + if ( b3IsValidHull( hull ) == false ) + { + b3DestroyHull( hull ); + return NULL; + } + + hull->hash = 0; + hull->hash = b3NonZeroHash( b3Hash( B3_HASH_INIT, (uint8_t*)hull, hull->byteCount ) ); + + return hull; +} + +b3HullData* b3CloneHull( const b3HullData* hull ) +{ + if ( hull == NULL || b3IsValidHull( hull ) == false ) + { + return NULL; + } + + b3HullData* clone = (b3HullData*)b3Alloc( hull->byteCount ); + memcpy( clone, hull, hull->byteCount ); + + return clone; +} + +uint64_t b3HashHullData( const b3HullData* hull ) +{ + // The baked content hash already covers byteCount. Spread the 32 bits across 64 so the table + // can use the high bits for its fast reject fragment. + return (uint64_t)hull->hash * 0x9E3779B97F4A7C15ull; +} + +bool b3CompareHullData( const b3HullData* hull1, const b3HullData* hull2 ) +{ + if ( hull1 == hull2 ) + { + return true; + } + + if ( hull1->byteCount != hull2->byteCount ) + { + return false; + } + + return memcmp( hull1, hull2, hull1->byteCount ) == 0; +} + +// Hull identity covers every byte, so the structs carry explicit padding. These lock +// the layout, re-audit padding if a size changes. +_Static_assert( sizeof( b3HullData ) == 136, "unexpected hull data size" ); +_Static_assert( sizeof( b3BoxHull ) == 440, "unexpected box hull size" ); + +#define NAME b3HullMap +#define KEY_TY const b3HullData* +#define VAL_TY int +#define HASH_FN b3HashHullData +#define CMPR_FN b3CompareHullData +#define MALLOC_FN b3Alloc +#define FREE_FN b3Free +#define IMPLEMENTATION_MODE +#include "verstable.h" + +size_t b3HullMapByteCount( b3HullMap* map ) +{ + // The map owns a combined bucket and metadata allocation, valid only when buckets exist + size_t byteCount = sizeof( b3HullMap ); + if ( b3HullMap_bucket_count( map ) > 0 ) + { + byteCount += b3HullMap_total_alloc_size( map ); + } + return byteCount; +} + +b3HullData* b3CloneAndTransformHull( const b3HullData* original, b3Transform transform, b3Vec3 scale ) +{ + if ( original == NULL || b3IsValidHull( original ) == false ) + { + return NULL; + } + + b3HullData* hull = (b3HullData*)b3Alloc( original->byteCount ); + memcpy( hull, original, original->byteCount ); + + b3Vec3 safeScale = b3SafeScale( scale ); + + b3HullHalfEdge* edges = b3GetHullEdgesWrite( hull ); + const b3HullFace* faces = b3GetHullFaces( hull ); + int faceCount = hull->faceCount; + int vertexCount = hull->vertexCount; + + if ( safeScale.x * safeScale.y * safeScale.z < 0.0f ) + { + // Reflected: reverse edge winding for each face. + for ( int i = 0; i < faceCount; ++i ) + { + const b3HullFace* face = faces + i; + + uint8_t startEdgeIndex = face->edge; + uint8_t currentEdgeIndex = startEdgeIndex; + uint8_t prevEdgeIndex = UINT8_MAX; + + do + { + b3HullHalfEdge* edge = edges + currentEdgeIndex; + + if ( edge->next == startEdgeIndex ) + { + prevEdgeIndex = currentEdgeIndex; + break; + } + + currentEdgeIndex = edge->next; + } + while ( currentEdgeIndex != startEdgeIndex ); + + B3_ASSERT( prevEdgeIndex != UINT8_MAX ); + + currentEdgeIndex = startEdgeIndex; + + do + { + b3HullHalfEdge* edge = edges + currentEdgeIndex; + uint8_t nextIndex = edge->next; + edge->next = prevEdgeIndex; + + if ( currentEdgeIndex < edge->twin ) + { + b3HullHalfEdge* twin = edges + edge->twin; + B3_SWAP( edge->origin, twin->origin ); + } + + prevEdgeIndex = currentEdgeIndex; + currentEdgeIndex = nextIndex; + } + while ( currentEdgeIndex != startEdgeIndex ); + } + + b3HullVertex* vertices = b3GetHullVerticesWrite( hull ); + + for ( int i = 0; i < vertexCount; ++i ) + { + b3HullVertex* vertex = vertices + i; + const b3HullHalfEdge* edge = edges + vertex->edge; + vertex->edge = edge->twin; + } + } + + b3Matrix3 matrix = b3MakeMatrixFromQuat( transform.q ); + b3Vec3* points = b3GetHullPointsWrite( hull ); + + for ( int i = 0; i < vertexCount; ++i ) + { + points[i] = b3Add( b3MulMV( matrix, b3Mul( safeScale, points[i] ) ), transform.p ); + } + + b3Plane* planes = b3GetHullPlanesWrite( hull ); + + for ( int i = 0; i < faceCount; ++i ) + { + int count = 0; + b3Vec3 centroid = b3Vec3_zero; + b3Vec3 normal = b3Vec3_zero; + + const b3HullFace* face = faces + i; + uint8_t startEdgeIndex = face->edge; + uint8_t currentEdgeIndex = startEdgeIndex; + + const b3HullHalfEdge* startEdge = edges + currentEdgeIndex; + B3_ASSERT( startEdge->face == i ); + B3_ASSERT( startEdge->origin < vertexCount ); + + b3Vec3 origin = points[startEdge->origin]; + + do + { + b3HullHalfEdge* edge = edges + currentEdgeIndex; + b3HullHalfEdge* twin = edges + edge->twin; + B3_ASSERT( twin->twin == currentEdgeIndex ); + + b3Vec3 v1 = b3Sub( points[edge->origin], origin ); + b3Vec3 v2 = b3Sub( points[twin->origin], origin ); + + count++; + centroid = b3Add( centroid, v1 ); + normal.x += ( v1.y - v2.y ) * ( v1.z + v2.z ); + normal.y += ( v1.z - v2.z ) * ( v1.x + v2.x ); + normal.z += ( v1.x - v2.x ) * ( v1.y + v2.y ); + + currentEdgeIndex = edge->next; + } + while ( currentEdgeIndex != startEdgeIndex ); + + B3_ASSERT( count > 0 ); + centroid = b3MulSV( 1.0f / (float)count, centroid ); + centroid = b3Add( centroid, origin ); + + float area = b3Length( normal ); + B3_ASSERT( area > 0.0f ); + normal = b3MulSV( 1.0f / area, normal ); + + planes[i] = b3MakePlaneFromNormalAndPoint( normal, centroid ); + } + + b3UpdateHullBounds( hull ); + bool success = b3UpdateHullBulkProperties( hull ); + if ( success == false ) + { + b3Free( hull, original->byteCount ); + return NULL; + } + + hull->hash = 0; + hull->hash = b3NonZeroHash( b3Hash( B3_HASH_INIT, (uint8_t*)hull, hull->byteCount ) ); + + B3_VALIDATE( b3IsValidHull( hull ) ); + + return hull; +} + +void b3DestroyHull( b3HullData* hull ) +{ + b3Free( hull, hull->byteCount ); +} + +b3MassData b3ComputeHullMass( const b3HullData* shape, float density ) +{ + b3MassData out; + out.mass = density * shape->volume; + out.center = shape->center; + + // Inertia about the center of mass + out.inertia = b3MulSM( density, shape->centralInertia ); + return out; +} + +b3AABB b3ComputeHullAABB( const b3HullData* shape, b3Transform transform ) +{ + return b3AABB_Transform( transform, shape->aabb ); +} + +b3AABB b3ComputeSweptHullAABB( const b3HullData* shape, b3Transform xf1, b3Transform xf2 ) +{ + b3AABB aabb1 = b3AABB_Transform( xf1, shape->aabb ); + b3AABB aabb2 = b3AABB_Transform( xf2, shape->aabb ); + return b3AABB_Union( aabb1, aabb2 ); +} + +bool b3OverlapHull( const b3HullData* shape, b3Transform shapeTransform, const b3ShapeProxy* proxy ) +{ + const b3Vec3* points = b3GetHullPoints( shape ); + + b3DistanceInput input; + input.proxyA = (b3ShapeProxy){ points, shape->vertexCount, 0.0f }; + 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; +} + +b3CastOutput b3RayCastHull( const b3HullData* shape, const b3RayCastInput* input ) +{ + B3_ASSERT( b3IsValidRay( input ) ); + b3CastOutput output = { 0 }; + + float lower = 0.0f; + float upper = input->maxFraction; + int bestFace = B3_NULL_INDEX; + + const b3Plane* planes = b3GetHullPlanes( shape ); + + for ( int faceIndex = 0; faceIndex < shape->faceCount; ++faceIndex ) + { + b3Plane plane = planes[faceIndex]; + + float distance = plane.offset - b3Dot( plane.normal, input->origin ); + float denominator = b3Dot( plane.normal, input->translation ); + + if ( denominator == 0.0f ) + { + if ( distance < 0.0f ) + { + return output; + } + } + else + { + float fraction = distance / denominator; + + if ( denominator < 0.0f ) + { + if ( fraction > lower ) + { + bestFace = faceIndex; + lower = fraction; + } + } + else + { + if ( fraction < upper ) + { + upper = fraction; + } + } + + if ( upper < lower ) + { + return output; + } + } + } + + if ( bestFace >= 0 ) + { + output.point = b3Add( input->origin, b3MulSV( lower, input->translation ) ); + output.normal = planes[bestFace].normal; + output.fraction = lower; + output.hit = true; + } + else + { + output.point = input->origin; + output.hit = true; + } + + return output; +} + +b3CastOutput b3ShapeCastHull( const b3HullData* shape, const b3ShapeCastInput* input ) +{ + const b3Vec3* points = b3GetHullPoints( shape ); + + b3ShapeCastPairInput pairInput; + pairInput.proxyA = (b3ShapeProxy){ points, shape->vertexCount, 0.0f }; + 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 b3CollideMoverAndHull( b3PlaneResult* result, const b3HullData* shape, const b3Capsule* mover ) +{ + const b3Vec3* points = b3GetHullPoints( shape ); + b3DistanceInput distanceInput; + distanceInput.proxyA = (b3ShapeProxy){ points, shape->vertexCount, 0.0f }; + distanceInput.proxyB = (b3ShapeProxy){ &mover->center1, 2, mover->radius }; + distanceInput.transform = b3Transform_identity; + distanceInput.useRadii = false; + + float totalRadius = mover->radius; + + b3SimplexCache cache = { 0 }; + b3DistanceOutput distanceOutput = b3ShapeDistance( &distanceInput, &cache, NULL, 0 ); + + if ( distanceOutput.distance == 0.0f ) + { + // I could handle deep overlap on hulls, but there is no reasonable solution for + // deep overlap on meshes. So if someone converted a hull to a mesh there would be + // different behavior. So I think there is not a good reason to handle deep overlap + // on hulls. + return 0; + } + + if ( distanceOutput.distance <= totalRadius ) + { + b3Plane plane = { distanceOutput.normal, totalRadius - distanceOutput.distance }; + *result = (b3PlaneResult){ plane, distanceOutput.pointA }; + return 1; + } + + return 0; +} + +b3ShapeExtent b3ComputeHullExtent( const b3HullData* hull, b3Vec3 origin ) +{ + const b3Vec3* points = b3GetHullPoints( hull ); + + b3ShapeExtent extent; + extent.minExtent = hull->innerRadius; + extent.maxExtent = b3Vec3_zero; + for ( int index = 0; index < hull->vertexCount; ++index ) + { + b3Vec3 point = points[index]; + extent.maxExtent = b3Max( extent.maxExtent, b3Abs( b3Sub( point, origin ) ) ); + } + + return extent; +} + +float b3ComputeHullProjectedArea( const b3HullData* hull, b3Vec3 direction ) +{ + float area = 0.0f; + + int faceCount = hull->faceCount; + const b3HullFace* hullFaces = b3GetHullFaces( hull ); + const b3HullHalfEdge* hullEdges = b3GetHullEdges( hull ); + const b3Vec3* hullPoints = b3GetHullPoints( hull ); + + for ( int i = 0; i < faceCount; ++i ) + { + const b3HullFace* face = hullFaces + i; + + int baseEdge = face->edge; + const b3HullHalfEdge* edge = hullEdges + baseEdge; + b3Vec3 p1 = hullPoints[edge->origin]; + + int edgeIndex = edge->next; + edge = hullEdges + edgeIndex; + b3Vec3 p2 = hullPoints[edge->origin]; + + edgeIndex = edge->next; + + do + { + edge = hullEdges + edgeIndex; + b3Vec3 p3 = hullPoints[edge->origin]; + + b3Vec3 e1 = b3Sub( p2, p1 ); + b3Vec3 e2 = b3Sub( p3, p1 ); + b3Vec3 n = b3Cross( e1, e2 ); + float a = b3Dot( n, direction ); + area += b3MaxFloat( a, 0.0f ); + + p2 = p3; + edgeIndex = edge->next; + } + while ( edgeIndex != baseEdge ); + } + + return 0.5f * area; +} + +// Constant template box (vertex/edge/face/topology). b3MakeTransformedBoxHull copies and +// fills in the runtime-dependent fields (boxPoints, boxPlanes, aabb, mass properties, hash). +static const b3BoxHull s_boxHull = { + .base = + { + .version = B3_HULL_VERSION, + .byteCount = sizeof( b3BoxHull ), + .hash = 0, + .vertexCount = 8, + .edgeCount = 24, + .faceCount = 6, + .vertexOffset = offsetof( b3BoxHull, boxVertices ), + .pointOffset = offsetof( b3BoxHull, boxPoints ), + .edgeOffset = offsetof( b3BoxHull, boxEdges ), + .faceOffset = offsetof( b3BoxHull, boxFaces ), + .planeOffset = offsetof( b3BoxHull, boxPlanes ), + }, + .boxVertices = + { + [0] = { .edge = 8 }, + [1] = { .edge = 1 }, + [2] = { .edge = 0 }, + [3] = { .edge = 9 }, + [4] = { .edge = 13 }, + [5] = { .edge = 3 }, + [6] = { .edge = 5 }, + [7] = { .edge = 11 }, + }, + .boxEdges = + { + [0] = { 2, 1, 2, 0 }, [1] = { 17, 0, 1, 5 }, [2] = { 4, 3, 1, 0 }, [3] = { 20, 2, 5, 3 }, + [4] = { 6, 5, 5, 0 }, [5] = { 23, 4, 6, 4 }, [6] = { 0, 7, 6, 0 }, [7] = { 18, 6, 2, 2 }, + [8] = { 10, 9, 0, 1 }, [9] = { 21, 8, 3, 5 }, [10] = { 12, 11, 3, 1 }, [11] = { 16, 10, 7, 2 }, + [12] = { 14, 13, 7, 1 }, [13] = { 19, 12, 4, 4 }, [14] = { 8, 15, 4, 1 }, [15] = { 22, 14, 0, 3 }, + [16] = { 7, 17, 3, 2 }, [17] = { 9, 16, 2, 5 }, [18] = { 11, 19, 6, 2 }, [19] = { 5, 18, 7, 4 }, + [20] = { 15, 21, 1, 3 }, [21] = { 1, 20, 0, 5 }, [22] = { 3, 23, 4, 3 }, [23] = { 13, 22, 5, 4 }, + }, + .boxFaces = + { + [0] = { .edge = 0 }, + [1] = { .edge = 8 }, + [2] = { .edge = 16 }, + [3] = { .edge = 20 }, + [4] = { .edge = 19 }, + [5] = { .edge = 21 }, + }, +}; + +b3BoxHull b3MakeTransformedBoxHull( float hx, float hy, float hz, b3Transform transform ) +{ + b3BoxHull boxHull = s_boxHull; + + float minH = 0.2f * B3_LINEAR_SLOP; + b3Vec3 h = b3Max( (b3Vec3){ minH, minH, minH }, (b3Vec3){ hx, hy, hz } ); + + boxHull.base.aabb = b3AABB_Transform( transform, (b3AABB){ b3Neg( h ), h } ); + boxHull.base.surfaceArea = 8.0f * ( h.x * h.y + h.x * h.z + h.y * h.z ); + boxHull.base.volume = 8.0f * h.x * h.y * h.z; + boxHull.base.innerRadius = b3MinFloat( h.x, b3MinFloat( h.y, h.z ) ); + boxHull.base.center = transform.p; + + b3Matrix3 boxInertia = b3BoxInertia( boxHull.base.volume, b3Neg( h ), h ); + boxHull.base.centralInertia = b3RotateInertia( transform.q, boxInertia ); + + b3Vec3 lower = b3Neg( h ); + b3Vec3 upper = h; + + boxHull.boxPlanes[0] = b3TransformPlane( transform, b3MakePlaneFromNormalAndPoint( b3Neg( b3Vec3_axisX ), lower ) ); + boxHull.boxPlanes[1] = b3TransformPlane( transform, b3MakePlaneFromNormalAndPoint( b3Vec3_axisX, upper ) ); + boxHull.boxPlanes[2] = b3TransformPlane( transform, b3MakePlaneFromNormalAndPoint( b3Neg( b3Vec3_axisY ), lower ) ); + boxHull.boxPlanes[3] = b3TransformPlane( transform, b3MakePlaneFromNormalAndPoint( b3Vec3_axisY, upper ) ); + boxHull.boxPlanes[4] = b3TransformPlane( transform, b3MakePlaneFromNormalAndPoint( b3Neg( b3Vec3_axisZ ), lower ) ); + boxHull.boxPlanes[5] = b3TransformPlane( transform, b3MakePlaneFromNormalAndPoint( b3Vec3_axisZ, upper ) ); + + boxHull.boxPoints[0] = b3TransformPoint( transform, (b3Vec3){ h.x, h.y, h.z } ); + boxHull.boxPoints[1] = b3TransformPoint( transform, (b3Vec3){ -h.x, h.y, h.z } ); + boxHull.boxPoints[2] = b3TransformPoint( transform, (b3Vec3){ -h.x, -h.y, h.z } ); + boxHull.boxPoints[3] = b3TransformPoint( transform, (b3Vec3){ h.x, -h.y, h.z } ); + boxHull.boxPoints[4] = b3TransformPoint( transform, (b3Vec3){ h.x, h.y, -h.z } ); + boxHull.boxPoints[5] = b3TransformPoint( transform, (b3Vec3){ -h.x, h.y, -h.z } ); + boxHull.boxPoints[6] = b3TransformPoint( transform, (b3Vec3){ -h.x, -h.y, -h.z } ); + boxHull.boxPoints[7] = b3TransformPoint( transform, (b3Vec3){ h.x, -h.y, -h.z } ); + + boxHull.base.hash = 0; + boxHull.base.hash = b3NonZeroHash( b3Hash( B3_HASH_INIT, (uint8_t*)&boxHull, sizeof( b3BoxHull ) ) ); + + return boxHull; +} + +b3BoxHull b3MakeCubeHull( float halfWidth ) +{ + return b3MakeBoxHull( halfWidth, halfWidth, halfWidth ); +} + +b3BoxHull b3MakeOffsetBoxHull( float hx, float hy, float hz, b3Vec3 offset ) +{ + b3Transform transform = { .p = offset, .q = b3Quat_identity }; + return b3MakeTransformedBoxHull( hx, hy, hz, transform ); +} + +b3BoxHull b3MakeBoxHull( float hx, float hy, float hz ) +{ + return b3MakeTransformedBoxHull( hx, hy, hz, b3Transform_identity ); +} + +void b3ScaleBox( b3Vec3* halfWidths, b3Transform* transform, b3Vec3 postScale, float minHalfWidth ) +{ + B3_ASSERT( b3IsValidFloat( minHalfWidth ) && minHalfWidth > 0.0f ); + + b3Quat q = transform->q; + + if ( postScale.x < 0.0f || postScale.y < 0.0f || postScale.z < 0.0f ) + { + // todo this might be unnecessary if rotation is identity + // todo compare with polar decomposition (much more expensive) + b3Matrix3 m = b3MakeMatrixFromQuat( q ); + m.cx.x *= postScale.x; + m.cy.x *= postScale.x; + m.cz.x *= postScale.x; + m.cx.y *= postScale.y; + m.cy.y *= postScale.y; + m.cz.y *= postScale.y; + m.cx.z *= postScale.z; + m.cy.z *= postScale.z; + m.cz.z *= postScale.z; + m.cx = b3Normalize( m.cx ); + m.cy = b3Normalize( m.cy ); + m.cz = b3Normalize( m.cz ); + m.cx = postScale.x < 0.0f ? b3Neg( m.cx ) : m.cx; + m.cy = postScale.y < 0.0f ? b3Neg( m.cy ) : m.cy; + m.cz = postScale.z < 0.0f ? b3Neg( m.cz ) : m.cz; + q = b3MakeQuatFromMatrix( &m ); + } + + b3Vec3 absScale = b3Abs( postScale ); + + b3Vec3 h = *halfWidths; + b3Vec3 p1 = b3Mul( absScale, b3RotateVector( q, b3Neg( h ) ) ); + b3Vec3 p2 = b3Mul( absScale, b3RotateVector( q, h ) ); + + b3Vec3 localP1 = b3InvRotateVector( q, p1 ); + b3Vec3 localP2 = b3InvRotateVector( q, p2 ); + + b3Vec3 lower = b3Min( localP1, localP2 ); + b3Vec3 upper = b3Max( localP1, localP2 ); + + b3Vec3 scaledHalfWidth = b3MulSV( 0.5f, b3Sub( upper, lower ) ); + + b3Vec3 mLimit = { minHalfWidth, minHalfWidth, minHalfWidth }; + *halfWidths = b3Max( scaledHalfWidth, mLimit ); + transform->p = b3Mul( postScale, transform->p ); + transform->q = q; +} + +// todo use new hull scaling technique +b3BoxHull b3MakeScaledBoxHull( b3Vec3 halfWidths, b3Transform transform, b3Vec3 postScale ) +{ + b3Vec3 h = halfWidths; + b3Transform xf = transform; + b3ScaleBox( &h, &xf, postScale, 4.0f * B3_LINEAR_SLOP ); + return b3MakeTransformedBoxHull( h.x, h.y, h.z, xf ); +} diff --git a/vendor/box3d/src/src/hull_map.h b/vendor/box3d/src/src/hull_map.h new file mode 100644 index 000000000..5b50c56eb --- /dev/null +++ b/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 + +// 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 ); diff --git a/vendor/box3d/src/src/id_pool.c b/vendor/box3d/src/src/id_pool.c new file mode 100644 index 000000000..185b6eebc --- /dev/null +++ b/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 diff --git a/vendor/box3d/src/src/id_pool.h b/vendor/box3d/src/src/id_pool.h new file mode 100644 index 000000000..06106de20 --- /dev/null +++ b/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 ); +} diff --git a/vendor/box3d/src/src/island.c b/vendor/box3d/src/src/island.c new file mode 100644 index 000000000..625a1b345 --- /dev/null +++ b/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 + +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 diff --git a/vendor/box3d/src/src/island.h b/vendor/box3d/src/src/island.h new file mode 100644 index 000000000..297c198dd --- /dev/null +++ b/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 + +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 ); diff --git a/vendor/box3d/src/src/joint.c b/vendor/box3d/src/src/joint.c new file mode 100644 index 000000000..5875f55f9 --- /dev/null +++ b/vendor/box3d/src/src/joint.c @@ -0,0 +1,1748 @@ +// SPDX-FileCopyrightText: 2025 Erin Catto +// SPDX-License-Identifier: MIT + +#include "joint.h" + +#include "body.h" +#include "contact.h" +#include "core.h" +#include "island.h" +#include "physics_world.h" +#include "recording.h" +#include "shape.h" +#include "solver.h" +#include "solver_set.h" + +// needed for dll export +#include "box3d/box3d.h" + +#include +#include +#include + +static b3JointDef b3DefaultJointDef( void ) +{ + b3JointDef def = { 0 }; + def.localFrameA.q = b3Quat_identity; + def.localFrameB.q = b3Quat_identity; + def.forceThreshold = FLT_MAX; + def.torqueThreshold = FLT_MAX; + def.constraintHertz = 60.0f; + def.constraintDampingRatio = 2.0f; + def.drawScale = b3GetLengthUnitsPerMeter(); + def.internalValue = B3_SECRET_COOKIE; + return def; +} + +b3ParallelJointDef b3DefaultParallelJointDef( void ) +{ + b3ParallelJointDef def = { 0 }; + def.base = b3DefaultJointDef(); + def.hertz = 1.0f; + def.dampingRatio = 1.0f; + def.maxTorque = FLT_MAX; + return def; +} + +b3DistanceJointDef b3DefaultDistanceJointDef( void ) +{ + b3DistanceJointDef def = { 0 }; + def.base = b3DefaultJointDef(); + def.lowerSpringForce = -FLT_MAX; + def.upperSpringForce = FLT_MAX; + def.length = 1.0f; + def.maxLength = B3_HUGE; + return def; +} + +b3MotorJointDef b3DefaultMotorJointDef( void ) +{ + b3MotorJointDef def = { 0 }; + def.base = b3DefaultJointDef(); + return def; +} + +b3FilterJointDef b3DefaultFilterJointDef( void ) +{ + b3FilterJointDef def = { 0 }; + def.base = b3DefaultJointDef(); + return def; +} + +b3PrismaticJointDef b3DefaultPrismaticJointDef( void ) +{ + b3PrismaticJointDef def = { 0 }; + def.base = b3DefaultJointDef(); + return def; +} + +b3RevoluteJointDef b3DefaultRevoluteJointDef( void ) +{ + b3RevoluteJointDef def = { 0 }; + def.base = b3DefaultJointDef(); + return def; +} + +b3SphericalJointDef b3DefaultSphericalJointDef( void ) +{ + b3SphericalJointDef def = { 0 }; + def.base = b3DefaultJointDef(); + def.targetRotation = b3Quat_identity; + return def; +} + +b3WeldJointDef b3DefaultWeldJointDef( void ) +{ + b3WeldJointDef def = { 0 }; + def.base = b3DefaultJointDef(); + return def; +} + +b3WheelJointDef b3DefaultWheelJointDef( void ) +{ + b3WheelJointDef def = { 0 }; + def.base = b3DefaultJointDef(); + def.enableSuspensionSpring = true; + def.suspensionHertz = 1.0f; + def.suspensionDampingRatio = 0.7f; + def.steeringHertz = 1.0f; + def.steeringDampingRatio = 0.7f; + return def; +} + +b3ExplosionDef b3DefaultExplosionDef( void ) +{ + b3ExplosionDef def = { 0 }; + def.maskBits = B3_DEFAULT_MASK_BITS; + return def; +} + +b3Joint* b3GetJointFullId( b3World* world, b3JointId jointId ) +{ + int id = jointId.index1 - 1; + b3Joint* joint = b3Array_Get( world->joints, id ); + B3_ASSERT( joint->jointId == id && joint->generation == jointId.generation ); + return joint; +} + +b3JointSim* b3GetJointSim( b3World* world, b3Joint* joint ) +{ + if ( joint->setIndex == b3_awakeSet ) + { + B3_ASSERT( 0 <= joint->colorIndex && joint->colorIndex < B3_GRAPH_COLOR_COUNT ); + b3GraphColor* color = world->constraintGraph.colors + joint->colorIndex; + return b3Array_Get( color->jointSims, joint->localIndex ); + } + + b3SolverSet* set = b3Array_Get( world->solverSets, joint->setIndex ); + return b3Array_Get( set->jointSims, joint->localIndex ); +} + +b3JointSim* b3GetJointSimCheckType( b3JointId jointId, b3JointType type ) +{ + B3_UNUSED( type ); + b3World* world = b3GetWorld( jointId.world0 ); + b3Joint* joint = b3GetJointFullId( world, jointId ); + B3_ASSERT( joint->type == type ); + b3JointSim* jointSim = b3GetJointSim( world, joint ); + B3_ASSERT( jointSim->type == type ); + return jointSim; +} + +typedef struct b3JointPair +{ + b3Joint* joint; + b3JointSim* jointSim; +} b3JointPair; + +static b3JointPair b3CreateJoint( b3World* world, const b3JointDef* def, b3JointType type ) +{ + B3_ASSERT( b3IsValidTransform( def->localFrameA ) ); + B3_ASSERT( b3IsValidTransform( def->localFrameB ) ); + + b3Body* bodyA = b3GetBodyFullId( world, def->bodyIdA ); + b3Body* bodyB = b3GetBodyFullId( world, def->bodyIdB ); + + int bodyIdA = bodyA->id; + int bodyIdB = bodyB->id; + int maxSetIndex = b3MaxInt( bodyA->setIndex, bodyB->setIndex ); + + // Create joint id and joint + int jointId = b3AllocId( &world->jointIdPool ); + if ( jointId == world->joints.count ) + { + b3Array_Push( world->joints, (b3Joint){ 0 } ); + } + + b3Joint* joint = b3Array_Get( world->joints, jointId ); + joint->jointId = jointId; + joint->userData = def->userData; + joint->generation += 1; + joint->setIndex = B3_NULL_INDEX; + joint->colorIndex = B3_NULL_INDEX; + joint->localIndex = B3_NULL_INDEX; + joint->islandId = B3_NULL_INDEX; + joint->islandIndex = B3_NULL_INDEX; + joint->drawScale = def->drawScale; + joint->type = type; + joint->collideConnected = def->collideConnected; + + // Doubly linked list on bodyA + joint->edges[0].bodyId = bodyIdA; + joint->edges[0].prevKey = B3_NULL_INDEX; + joint->edges[0].nextKey = bodyA->headJointKey; + + int keyA = ( jointId << 1 ) | 0; + if ( bodyA->headJointKey != B3_NULL_INDEX ) + { + b3Joint* jointA = b3Array_Get( world->joints, bodyA->headJointKey >> 1 ); + b3JointEdge* edgeA = jointA->edges + ( bodyA->headJointKey & 1 ); + edgeA->prevKey = keyA; + } + bodyA->headJointKey = keyA; + bodyA->jointCount += 1; + + // Doubly linked list on bodyB + joint->edges[1].bodyId = bodyIdB; + joint->edges[1].prevKey = B3_NULL_INDEX; + joint->edges[1].nextKey = bodyB->headJointKey; + + int keyB = ( jointId << 1 ) | 1; + if ( bodyB->headJointKey != B3_NULL_INDEX ) + { + b3Joint* jointB = b3Array_Get( world->joints, bodyB->headJointKey >> 1 ); + b3JointEdge* edgeB = jointB->edges + ( bodyB->headJointKey & 1 ); + edgeB->prevKey = keyB; + } + bodyB->headJointKey = keyB; + bodyB->jointCount += 1; + + b3JointSim* jointSim; + + if ( bodyA->setIndex == b3_disabledSet || bodyB->setIndex == b3_disabledSet ) + { + // if either body is disabled, create in disabled set + b3SolverSet* set = b3Array_Get( world->solverSets, b3_disabledSet ); + joint->setIndex = b3_disabledSet; + joint->localIndex = set->jointSims.count; + + jointSim = b3Array_Emplace( set->jointSims ); + memset( jointSim, 0, sizeof( b3JointSim ) ); + + jointSim->jointId = jointId; + jointSim->bodyIdA = bodyIdA; + jointSim->bodyIdB = bodyIdB; + } + else if ( bodyA->type != b3_dynamicBody && bodyB->type != b3_dynamicBody ) + { + // joint is not attached to a dynamic body + b3SolverSet* set = b3Array_Get( world->solverSets, b3_staticSet ); + joint->setIndex = b3_staticSet; + joint->localIndex = set->jointSims.count; + + jointSim = b3Array_Emplace( set->jointSims ); + memset( jointSim, 0, sizeof( b3JointSim ) ); + + jointSim->jointId = jointId; + jointSim->bodyIdA = bodyIdA; + jointSim->bodyIdB = bodyIdB; + } + else if ( bodyA->setIndex == b3_awakeSet || bodyB->setIndex == b3_awakeSet ) + { + // if either body is sleeping, wake it + if ( maxSetIndex >= b3_firstSleepingSet ) + { + b3WakeSolverSet( world, maxSetIndex ); + } + + joint->setIndex = b3_awakeSet; + + jointSim = b3CreateJointInGraph( world, joint ); + jointSim->jointId = jointId; + jointSim->bodyIdA = bodyIdA; + jointSim->bodyIdB = bodyIdB; + } + else + { + // joint connected between sleeping and/or static bodies + B3_ASSERT( bodyA->setIndex >= b3_firstSleepingSet || bodyB->setIndex >= b3_firstSleepingSet ); + B3_ASSERT( bodyA->setIndex != b3_staticSet || bodyB->setIndex != b3_staticSet ); + + // joint should go into the sleeping set (not static set) + int setIndex = maxSetIndex; + + b3SolverSet* set = b3Array_Get( world->solverSets, setIndex ); + joint->setIndex = setIndex; + joint->localIndex = set->jointSims.count; + + jointSim = b3Array_Emplace( set->jointSims ); + memset( jointSim, 0, sizeof( b3JointSim ) ); + + jointSim->jointId = jointId; + jointSim->bodyIdA = bodyIdA; + jointSim->bodyIdB = bodyIdB; + + if ( bodyA->setIndex != bodyB->setIndex && bodyA->setIndex >= b3_firstSleepingSet && + bodyB->setIndex >= b3_firstSleepingSet ) + { + // merge sleeping sets + b3MergeSolverSets( world, bodyA->setIndex, bodyB->setIndex ); + B3_ASSERT( bodyA->setIndex == bodyB->setIndex ); + + // fix potentially invalid set index + setIndex = bodyA->setIndex; + + b3SolverSet* mergedSet = b3Array_Get( world->solverSets, setIndex ); + + // Careful! The joint sim pointer was orphaned by the set merge. + jointSim = b3Array_Get( mergedSet->jointSims, joint->localIndex ); + } + + B3_ASSERT( joint->setIndex == setIndex ); + } + + jointSim->localFrameA = def->localFrameA; + jointSim->localFrameB = def->localFrameB; + jointSim->type = type; + jointSim->constraintHertz = def->constraintHertz; + jointSim->constraintDampingRatio = def->constraintDampingRatio; + jointSim->constraintSoftness = (b3Softness){ + .biasRate = 0.0f, + .massScale = 1.0f, + .impulseScale = 0.0f, + }; + + B3_ASSERT( b3IsValidFloat( def->forceThreshold ) && def->forceThreshold >= 0.0f ); + B3_ASSERT( b3IsValidFloat( def->torqueThreshold ) && def->torqueThreshold >= 0.0f ); + + jointSim->forceThreshold = def->forceThreshold; + jointSim->torqueThreshold = def->torqueThreshold; + + B3_ASSERT( jointSim->jointId == jointId ); + B3_ASSERT( jointSim->bodyIdA == bodyIdA ); + B3_ASSERT( jointSim->bodyIdB == bodyIdB ); + + if ( joint->setIndex > b3_disabledSet ) + { + // Add edge to island graph + b3LinkJoint( world, joint ); + } + + b3ValidateSolverSets( world ); + + return (b3JointPair){ joint, jointSim }; +} + +static void b3DestroyContactsBetweenBodies( b3World* world, b3Body* bodyA, b3Body* bodyB ) +{ + int contactKey; + int otherBodyId; + + // use the smaller of the two contact lists + if ( bodyA->contactCount < bodyB->contactCount ) + { + contactKey = bodyA->headContactKey; + otherBodyId = bodyB->id; + } + else + { + contactKey = bodyB->headContactKey; + otherBodyId = bodyA->id; + } + + // no need to wake bodies when a joint removes collision between them + bool wakeBodies = false; + + // destroy the contacts + while ( contactKey != B3_NULL_INDEX ) + { + int contactId = contactKey >> 1; + int edgeIndex = contactKey & 1; + + b3Contact* contact = b3Array_Get( world->contacts, contactId ); + contactKey = contact->edges[edgeIndex].nextKey; + + int otherEdgeIndex = edgeIndex ^ 1; + if ( contact->edges[otherEdgeIndex].bodyId == otherBodyId ) + { + // Careful, this removes the contact from the current doubly linked list + b3DestroyContact( world, contact, wakeBodies ); + } + } + + b3ValidateSolverSets( world ); +} + +void b3Joint_SetConstraintTuning( b3JointId jointId, float hertz, float dampingRatio ) +{ + B3_ASSERT( b3IsValidFloat( hertz ) && hertz >= 0.0f ); + B3_ASSERT( b3IsValidFloat( dampingRatio ) && dampingRatio >= 0.0f ); + + b3World* world = b3GetWorld( jointId.world0 ); + B3_REC( world, JointSetConstraintTuning, jointId, hertz, dampingRatio ); + b3Joint* joint = b3GetJointFullId( world, jointId ); + b3JointSim* base = b3GetJointSim( world, joint ); + base->constraintHertz = hertz; + base->constraintDampingRatio = dampingRatio; +} + +void b3Joint_GetConstraintTuning( b3JointId jointId, float* hertz, float* dampingRatio ) +{ + b3World* world = b3GetWorld( jointId.world0 ); + b3Joint* joint = b3GetJointFullId( world, jointId ); + b3JointSim* base = b3GetJointSim( world, joint ); + *hertz = base->constraintHertz; + *dampingRatio = base->constraintDampingRatio; +} + +void b3Joint_SetForceThreshold( b3JointId jointId, float threshold ) +{ + B3_ASSERT( b3IsValidFloat( threshold ) && threshold >= 0.0f ); + + b3World* world = b3GetWorld( jointId.world0 ); + B3_REC( world, JointSetForceThreshold, jointId, threshold ); + b3Joint* joint = b3GetJointFullId( world, jointId ); + b3JointSim* base = b3GetJointSim( world, joint ); + base->forceThreshold = threshold; +} + +float b3Joint_GetForceThreshold( b3JointId jointId ) +{ + b3World* world = b3GetWorld( jointId.world0 ); + b3Joint* joint = b3GetJointFullId( world, jointId ); + b3JointSim* base = b3GetJointSim( world, joint ); + return base->forceThreshold; +} + +void b3Joint_SetTorqueThreshold( b3JointId jointId, float threshold ) +{ + B3_ASSERT( b3IsValidFloat( threshold ) && threshold >= 0.0f ); + + b3World* world = b3GetWorld( jointId.world0 ); + B3_REC( world, JointSetTorqueThreshold, jointId, threshold ); + b3Joint* joint = b3GetJointFullId( world, jointId ); + b3JointSim* base = b3GetJointSim( world, joint ); + base->torqueThreshold = threshold; +} + +float b3Joint_GetTorqueThreshold( b3JointId jointId ) +{ + b3World* world = b3GetWorld( jointId.world0 ); + b3Joint* joint = b3GetJointFullId( world, jointId ); + b3JointSim* base = b3GetJointSim( world, joint ); + return base->torqueThreshold; +} + +b3JointId b3CreateDistanceJoint( b3WorldId worldId, const b3DistanceJointDef* def ) +{ + B3_CHECK_JOINT_DEF( def ); + b3World* world = b3GetUnlockedWorldFromId( worldId ); + if (world == NULL) + { + return (b3JointId){ 0 }; + } + + B3_ASSERT( b3IsValidFloat( def->length ) && def->length > 0.0f ); + B3_ASSERT( def->lowerSpringForce <= def->upperSpringForce ); + + b3JointPair pair = b3CreateJoint( world, &def->base, b3_distanceJoint ); + + b3JointSim* joint = pair.jointSim; + + joint->distanceJoint = (b3DistanceJoint){ 0 }; + joint->distanceJoint.length = b3MaxFloat( def->length, B3_LINEAR_SLOP ); + joint->distanceJoint.hertz = def->hertz; + joint->distanceJoint.dampingRatio = def->dampingRatio; + joint->distanceJoint.lowerSpringForce = def->lowerSpringForce; + joint->distanceJoint.upperSpringForce = def->upperSpringForce; + joint->distanceJoint.minLength = b3MaxFloat( def->minLength, B3_LINEAR_SLOP ); + joint->distanceJoint.maxLength = b3MaxFloat( def->minLength, def->maxLength ); + joint->distanceJoint.maxMotorForce = def->maxMotorForce; + joint->distanceJoint.motorSpeed = def->motorSpeed; + joint->distanceJoint.enableSpring = def->enableSpring; + joint->distanceJoint.enableLimit = def->enableLimit; + joint->distanceJoint.enableMotor = def->enableMotor; + joint->distanceJoint.impulse = 0.0f; + joint->distanceJoint.lowerImpulse = 0.0f; + joint->distanceJoint.upperImpulse = 0.0f; + joint->distanceJoint.motorImpulse = 0.0f; + + b3JointId jointId = { joint->jointId + 1, world->worldId, pair.joint->generation }; + B3_REC_CREATE( world, CreateDistanceJoint, jointId, worldId, *def ); + return jointId; +} + +b3JointId b3CreateMotorJoint( b3WorldId worldId, const b3MotorJointDef* def ) +{ + B3_CHECK_JOINT_DEF( def ); + b3World* world = b3GetUnlockedWorldFromId( worldId ); + if ( world == NULL ) + { + return (b3JointId){ 0 }; + } + + b3JointPair pair = b3CreateJoint( world, &def->base, b3_motorJoint ); + b3JointSim* joint = pair.jointSim; + + joint->motorJoint = (b3MotorJoint){ 0 }; + joint->motorJoint.linearVelocity = def->linearVelocity; + joint->motorJoint.maxVelocityForce = def->maxVelocityForce; + joint->motorJoint.angularVelocity = def->angularVelocity; + joint->motorJoint.maxVelocityTorque = def->maxVelocityTorque; + joint->motorJoint.linearHertz = def->linearHertz; + joint->motorJoint.linearDampingRatio = def->linearDampingRatio; + joint->motorJoint.maxSpringForce = def->maxSpringForce; + joint->motorJoint.angularHertz = def->angularHertz; + joint->motorJoint.angularDampingRatio = def->angularDampingRatio; + joint->motorJoint.maxSpringTorque = def->maxSpringTorque; + + b3JointId jointId = { joint->jointId + 1, world->worldId, pair.joint->generation }; + B3_REC_CREATE( world, CreateMotorJoint, jointId, worldId, *def ); + return jointId; +} + +b3JointId b3CreateFilterJoint( b3WorldId worldId, const b3FilterJointDef* def ) +{ + B3_CHECK_JOINT_DEF( def ); + b3World* world = b3GetUnlockedWorldFromId( worldId ); + if (world == NULL) + { + return (b3JointId){ 0 }; + } + + b3JointPair pair = b3CreateJoint( world, &def->base, b3_filterJoint ); + + b3JointSim* joint = pair.jointSim; + + b3JointId jointId = { joint->jointId + 1, world->worldId, pair.joint->generation }; + B3_REC_CREATE( world, CreateFilterJoint, jointId, worldId, *def ); + return jointId; +} + +b3JointId b3CreateParallelJoint( b3WorldId worldId, const b3ParallelJointDef* def ) +{ + B3_CHECK_JOINT_DEF( def ); + b3World* world = b3GetUnlockedWorldFromId( worldId ); + if (world == NULL) + { + return (b3JointId){ 0 }; + } + + B3_ASSERT( b3IsValidFloat( def->hertz ) && def->hertz >= 0.0f ); + B3_ASSERT( b3IsValidFloat( def->dampingRatio ) && def->dampingRatio >= 0.0f ); + B3_ASSERT( b3IsValidFloat( def->maxTorque ) && def->maxTorque >= 0.0f ); + + b3JointPair pair = b3CreateJoint( world, &def->base, b3_parallelJoint ); + + b3JointSim* joint = pair.jointSim; + + joint->parallelJoint = (b3ParallelJoint){ 0 }; + joint->parallelJoint.hertz = def->hertz; + joint->parallelJoint.dampingRatio = def->dampingRatio; + joint->parallelJoint.maxTorque = def->maxTorque; + + b3JointId jointId = { joint->jointId + 1, world->worldId, pair.joint->generation }; + B3_REC_CREATE( world, CreateParallelJoint, jointId, worldId, *def ); + return jointId; +} + +b3JointId b3CreatePrismaticJoint( b3WorldId worldId, const b3PrismaticJointDef* def ) +{ + B3_CHECK_JOINT_DEF( def ); + B3_ASSERT( def->lowerTranslation <= def->upperTranslation ); + + b3World* world = b3GetUnlockedWorldFromId( worldId ); + if (world == NULL) + { + return (b3JointId){ 0 }; + } + + b3JointPair pair = b3CreateJoint( world, &def->base, b3_prismaticJoint ); + + b3JointSim* joint = pair.jointSim; + + joint->prismaticJoint = (b3PrismaticJoint){ 0 }; + joint->prismaticJoint.hertz = def->hertz; + joint->prismaticJoint.dampingRatio = def->dampingRatio; + joint->prismaticJoint.targetTranslation = def->targetTranslation; + joint->prismaticJoint.lowerTranslation = def->lowerTranslation; + joint->prismaticJoint.upperTranslation = def->upperTranslation; + joint->prismaticJoint.maxMotorForce = def->maxMotorForce; + joint->prismaticJoint.motorSpeed = def->motorSpeed; + joint->prismaticJoint.enableSpring = def->enableSpring; + joint->prismaticJoint.enableLimit = def->enableLimit; + joint->prismaticJoint.enableMotor = def->enableMotor; + + b3JointId jointId = { joint->jointId + 1, world->worldId, pair.joint->generation }; + B3_REC_CREATE( world, CreatePrismaticJoint, jointId, worldId, *def ); + return jointId; +} + +b3JointId b3CreateRevoluteJoint( b3WorldId worldId, const b3RevoluteJointDef* def ) +{ + B3_CHECK_JOINT_DEF( def ); + + b3World* world = b3GetUnlockedWorldFromId( worldId ); + if (world == NULL) + { + return (b3JointId){ 0 }; + } + + b3JointPair pair = b3CreateJoint( world, &def->base, b3_revoluteJoint ); + + b3JointSim* joint = pair.jointSim; + + joint->revoluteJoint = (b3RevoluteJoint){ 0 }; + joint->revoluteJoint.hertz = def->hertz; + joint->revoluteJoint.dampingRatio = def->dampingRatio; + joint->revoluteJoint.targetAngle = b3ClampFloat( def->targetAngle, -B3_PI, B3_PI ); + + float lowerAngle = b3MinFloat( def->lowerAngle, def->upperAngle ); + float upperAngle = b3MaxFloat( def->lowerAngle, def->upperAngle ); + joint->revoluteJoint.lowerAngle = b3ClampFloat( lowerAngle, -0.99f * B3_PI, 0.99f * B3_PI ); + joint->revoluteJoint.upperAngle = b3ClampFloat( upperAngle, -0.99f * B3_PI, 0.99f * B3_PI ); + + joint->revoluteJoint.maxMotorTorque = def->maxMotorTorque; + joint->revoluteJoint.motorSpeed = def->motorSpeed; + joint->revoluteJoint.enableSpring = def->enableSpring; + joint->revoluteJoint.enableLimit = def->enableLimit; + joint->revoluteJoint.enableMotor = def->enableMotor; + + b3JointId jointId = { joint->jointId + 1, world->worldId, pair.joint->generation }; + B3_REC_CREATE( world, CreateRevoluteJoint, jointId, worldId, *def ); + return jointId; +} + +b3JointId b3CreateSphericalJoint( b3WorldId worldId, const b3SphericalJointDef* def ) +{ + B3_CHECK_JOINT_DEF( def ); + B3_ASSERT( 0.0f <= def->coneAngle && def->coneAngle <= 0.99f * B3_PI ); + B3_ASSERT( b3IsValidQuat( def->targetRotation ) ); + + b3World* world = b3GetUnlockedWorldFromId( worldId ); + if (world == NULL) + { + return (b3JointId){ 0 }; + } + + b3JointPair pair = b3CreateJoint( world, &def->base, b3_sphericalJoint ); + + b3JointSim* joint = pair.jointSim; + + joint->sphericalJoint = (b3SphericalJoint){ 0 }; + joint->sphericalJoint.hertz = def->hertz; + joint->sphericalJoint.dampingRatio = def->dampingRatio; + joint->sphericalJoint.targetRotation = def->targetRotation; + joint->sphericalJoint.coneAngle = b3ClampFloat( def->coneAngle, 0.0f, 0.5f * B3_PI ); + + float lowerAngle = b3MinFloat( def->lowerTwistAngle, def->upperTwistAngle ); + float upperAngle = b3MaxFloat( def->lowerTwistAngle, def->upperTwistAngle ); + joint->sphericalJoint.lowerTwistAngle = b3ClampFloat( lowerAngle, -0.99f * B3_PI, 0.99f * B3_PI ); + joint->sphericalJoint.upperTwistAngle = b3ClampFloat( upperAngle, -0.99f * B3_PI, 0.99f * B3_PI ); + + joint->sphericalJoint.maxMotorTorque = def->maxMotorTorque; + joint->sphericalJoint.motorVelocity = def->motorVelocity; + joint->sphericalJoint.enableSpring = def->enableSpring; + joint->sphericalJoint.enableConeLimit = def->enableConeLimit; + joint->sphericalJoint.enableTwistLimit = def->enableTwistLimit; + joint->sphericalJoint.enableMotor = def->enableMotor; + + b3JointId jointId = { joint->jointId + 1, world->worldId, pair.joint->generation }; + B3_REC_CREATE( world, CreateSphericalJoint, jointId, worldId, *def ); + return jointId; +} + +b3JointId b3CreateWeldJoint( b3WorldId worldId, const b3WeldJointDef* def ) +{ + B3_CHECK_JOINT_DEF( def ); + B3_ASSERT( 0.0f <= def->angularHertz ); + B3_ASSERT( 0.0f <= def->angularDampingRatio ); + B3_ASSERT( 0.0f <= def->linearHertz ); + B3_ASSERT( 0.0f <= def->linearDampingRatio ); + + b3World* world = b3GetUnlockedWorldFromId( worldId ); + if (world == NULL) + { + return (b3JointId){ 0 }; + } + + b3JointPair pair = b3CreateJoint( world, &def->base, b3_weldJoint ); + + b3JointSim* joint = pair.jointSim; + + joint->weldJoint = (b3WeldJoint){ 0 }; + joint->weldJoint.linearHertz = def->linearHertz; + joint->weldJoint.linearDampingRatio = def->linearDampingRatio; + joint->weldJoint.angularHertz = def->angularHertz; + joint->weldJoint.angularDampingRatio = def->angularDampingRatio; + + b3JointId jointId = { joint->jointId + 1, world->worldId, pair.joint->generation }; + B3_REC_CREATE( world, CreateWeldJoint, jointId, worldId, *def ); + return jointId; +} + +b3JointId b3CreateWheelJoint( b3WorldId worldId, const b3WheelJointDef* def ) +{ + B3_CHECK_JOINT_DEF( def ); + B3_ASSERT( def->lowerSuspensionLimit <= def->upperSuspensionLimit ); + + b3World* world = b3GetUnlockedWorldFromId( worldId ); + if (world == NULL) + { + return (b3JointId){ 0 }; + } + + b3JointPair pair = b3CreateJoint( world, &def->base, b3_wheelJoint ); + + b3JointSim* joint = pair.jointSim; + + joint->wheelJoint = (b3WheelJoint){ 0 }; + joint->wheelJoint.enableSuspensionSpring = def->enableSuspensionSpring; + joint->wheelJoint.suspensionHertz = def->suspensionHertz; + joint->wheelJoint.suspensionDampingRatio = def->suspensionDampingRatio; + joint->wheelJoint.enableSuspensionLimit = def->enableSuspensionLimit; + joint->wheelJoint.lowerSuspensionLimit = def->lowerSuspensionLimit; + joint->wheelJoint.upperSuspensionLimit = def->upperSuspensionLimit; + joint->wheelJoint.enableSpinMotor = def->enableSpinMotor; + joint->wheelJoint.maxSpinTorque = def->maxSpinTorque; + joint->wheelJoint.spinSpeed = def->spinSpeed; + + joint->wheelJoint.enableSteering = def->enableSteering; + joint->wheelJoint.steeringHertz = def->steeringHertz; + joint->wheelJoint.steeringDampingRatio = def->steeringDampingRatio; + joint->wheelJoint.targetSteeringAngle = def->targetSteeringAngle; + joint->wheelJoint.maxSteeringTorque = def->maxSteeringTorque; + joint->wheelJoint.enableSteeringLimit = def->enableSteeringLimit; + joint->wheelJoint.lowerSteeringLimit = def->lowerSteeringLimit; + joint->wheelJoint.upperSteeringLimit = def->upperSteeringLimit; + + b3JointId jointId = { joint->jointId + 1, world->worldId, pair.joint->generation }; + B3_REC_CREATE( world, CreateWheelJoint, jointId, worldId, *def ); + return jointId; +} + +void b3DestroyJointInternal( b3World* world, b3Joint* joint, bool wakeBodies ) +{ + int jointId = joint->jointId; + + b3JointEdge* edgeA = joint->edges + 0; + b3JointEdge* edgeB = joint->edges + 1; + + int idA = edgeA->bodyId; + int idB = edgeB->bodyId; + b3Body* bodyA = b3Array_Get( world->bodies, idA ); + b3Body* bodyB = b3Array_Get( world->bodies, idB ); + + // Remove from body A + if ( edgeA->prevKey != B3_NULL_INDEX ) + { + b3Joint* prevJoint = b3Array_Get( world->joints, edgeA->prevKey >> 1 ); + b3JointEdge* prevEdge = prevJoint->edges + ( edgeA->prevKey & 1 ); + prevEdge->nextKey = edgeA->nextKey; + } + + if ( edgeA->nextKey != B3_NULL_INDEX ) + { + b3Joint* nextJoint = b3Array_Get( world->joints, edgeA->nextKey >> 1 ); + b3JointEdge* nextEdge = nextJoint->edges + ( edgeA->nextKey & 1 ); + nextEdge->prevKey = edgeA->prevKey; + } + + int edgeKeyA = ( jointId << 1 ) | 0; + if ( bodyA->headJointKey == edgeKeyA ) + { + bodyA->headJointKey = edgeA->nextKey; + } + + bodyA->jointCount -= 1; + + // Remove from body B + if ( edgeB->prevKey != B3_NULL_INDEX ) + { + b3Joint* prevJoint = b3Array_Get( world->joints, edgeB->prevKey >> 1 ); + b3JointEdge* prevEdge = prevJoint->edges + ( edgeB->prevKey & 1 ); + prevEdge->nextKey = edgeB->nextKey; + } + + if ( edgeB->nextKey != B3_NULL_INDEX ) + { + b3Joint* nextJoint = b3Array_Get( world->joints, edgeB->nextKey >> 1 ); + b3JointEdge* nextEdge = nextJoint->edges + ( edgeB->nextKey & 1 ); + nextEdge->prevKey = edgeB->prevKey; + } + + int edgeKeyB = ( jointId << 1 ) | 1; + if ( bodyB->headJointKey == edgeKeyB ) + { + bodyB->headJointKey = edgeB->nextKey; + } + + bodyB->jointCount -= 1; + + if ( joint->islandId != B3_NULL_INDEX ) + { + B3_ASSERT( joint->setIndex > b3_disabledSet ); + b3UnlinkJoint( world, joint ); + } + else + { + B3_ASSERT( joint->setIndex <= b3_disabledSet ); + } + + // Remove joint from solver set that owns it + int setIndex = joint->setIndex; + int localIndex = joint->localIndex; + + if ( setIndex == b3_awakeSet ) + { + b3RemoveJointFromGraph( world, joint->edges[0].bodyId, joint->edges[1].bodyId, joint->colorIndex, localIndex ); + } + else + { + b3SolverSet* set = b3Array_Get( world->solverSets, setIndex ); + int movedIndex = b3Array_RemoveSwap( set->jointSims, localIndex ); + if ( movedIndex != B3_NULL_INDEX ) + { + // Fix moved joint + b3JointSim* movedJointSim = set->jointSims.data + localIndex; + int movedId = movedJointSim->jointId; + b3Joint* movedJoint = b3Array_Get( world->joints, movedId ); + B3_ASSERT( movedJoint->localIndex == movedIndex ); + movedJoint->localIndex = localIndex; + } + } + + // Free joint and id (preserve joint revision) + joint->setIndex = B3_NULL_INDEX; + joint->localIndex = B3_NULL_INDEX; + joint->colorIndex = B3_NULL_INDEX; + joint->jointId = B3_NULL_INDEX; + b3FreeId( &world->jointIdPool, jointId ); + + if ( wakeBodies ) + { + b3WakeBody( world, bodyA ); + b3WakeBody( world, bodyB ); + } + + b3ValidateSolverSets( world ); +} + +void b3DestroyJoint( b3JointId jointId, bool wakeAttached ) +{ + b3World* world = b3GetWorld( jointId.world0 ); + if ( world == NULL ) + { + return; + } + + B3_REC( world, DestroyJoint, jointId, wakeAttached ); + + b3Joint* joint = b3GetJointFullId( world, jointId ); + + b3DestroyJointInternal( world, joint, wakeAttached ); +} + +b3JointType b3Joint_GetType( b3JointId jointId ) +{ + b3World* world = b3GetWorld( jointId.world0 ); + b3Joint* joint = b3GetJointFullId( world, jointId ); + return joint->type; +} + +b3BodyId b3Joint_GetBodyA( b3JointId jointId ) +{ + b3World* world = b3GetWorld( jointId.world0 ); + b3Joint* joint = b3GetJointFullId( world, jointId ); + return b3MakeBodyId( world, joint->edges[0].bodyId ); +} + +b3BodyId b3Joint_GetBodyB( b3JointId jointId ) +{ + b3World* world = b3GetWorld( jointId.world0 ); + b3Joint* joint = b3GetJointFullId( world, jointId ); + return b3MakeBodyId( world, joint->edges[1].bodyId ); +} + +b3WorldId b3Joint_GetWorld( b3JointId jointId ) +{ + b3World* world = b3GetWorld( jointId.world0 ); + return (b3WorldId){ (uint16_t)( jointId.world0 + 1 ), world->generation }; +} + +void b3Joint_SetLocalFrameA( b3JointId jointId, b3Transform localFrame ) +{ + B3_ASSERT( b3IsValidTransform( localFrame ) ); + + b3World* world = b3GetWorld( jointId.world0 ); + B3_REC( world, JointSetLocalFrameA, jointId, localFrame ); + b3Joint* joint = b3GetJointFullId( world, jointId ); + b3JointSim* jointSim = b3GetJointSim( world, joint ); + jointSim->localFrameA = localFrame; +} + +b3Transform b3Joint_GetLocalFrameA( b3JointId jointId ) +{ + b3World* world = b3GetWorld( jointId.world0 ); + b3Joint* joint = b3GetJointFullId( world, jointId ); + b3JointSim* jointSim = b3GetJointSim( world, joint ); + return jointSim->localFrameA; +} + +void b3Joint_SetLocalFrameB( b3JointId jointId, b3Transform localFrame ) +{ + B3_ASSERT( b3IsValidTransform( localFrame ) ); + + b3World* world = b3GetWorld( jointId.world0 ); + B3_REC( world, JointSetLocalFrameB, jointId, localFrame ); + b3Joint* joint = b3GetJointFullId( world, jointId ); + b3JointSim* jointSim = b3GetJointSim( world, joint ); + jointSim->localFrameB = localFrame; +} + +b3Transform b3Joint_GetLocalFrameB( b3JointId jointId ) +{ + b3World* world = b3GetWorld( jointId.world0 ); + b3Joint* joint = b3GetJointFullId( world, jointId ); + b3JointSim* jointSim = b3GetJointSim( world, joint ); + return jointSim->localFrameB; +} + +void b3Joint_SetCollideConnected( b3JointId jointId, bool shouldCollide ) +{ + b3World* world = b3GetUnlockedWorld( jointId.world0 ); + if ( world == NULL ) + { + return; + } + + B3_REC( world, JointSetCollideConnected, jointId, shouldCollide ); + + b3Joint* joint = b3GetJointFullId( world, jointId ); + if ( joint->collideConnected == shouldCollide ) + { + return; + } + + joint->collideConnected = shouldCollide; + + b3Body* bodyA = b3Array_Get( world->bodies, joint->edges[0].bodyId ); + b3Body* bodyB = b3Array_Get( world->bodies, joint->edges[1].bodyId ); + + if ( shouldCollide ) + { + // need to tell the broad-phase to look for new pairs for one of the + // two bodies. Pick the one with the fewest shapes. + int shapeCountA = bodyA->shapeCount; + int shapeCountB = bodyB->shapeCount; + + int shapeId = shapeCountA < shapeCountB ? bodyA->headShapeId : bodyB->headShapeId; + while ( shapeId != B3_NULL_INDEX ) + { + b3Shape* shape = b3Array_Get( world->shapes, shapeId ); + + if ( shape->proxyKey != B3_NULL_INDEX ) + { + b3BufferMove( &world->broadPhase, shape->proxyKey ); + } + + shapeId = shape->nextShapeId; + } + } + else + { + b3DestroyContactsBetweenBodies( world, bodyA, bodyB ); + } +} + +bool b3Joint_GetCollideConnected( b3JointId jointId ) +{ + b3World* world = b3GetWorld( jointId.world0 ); + b3Joint* joint = b3GetJointFullId( world, jointId ); + return joint->collideConnected; +} + +void b3Joint_SetUserData( b3JointId jointId, void* userData ) +{ + b3World* world = b3GetWorld( jointId.world0 ); + b3Joint* joint = b3GetJointFullId( world, jointId ); + joint->userData = userData; +} + +void* b3Joint_GetUserData( b3JointId jointId ) +{ + b3World* world = b3GetWorld( jointId.world0 ); + b3Joint* joint = b3GetJointFullId( world, jointId ); + return joint->userData; +} + +void b3Joint_WakeBodies( b3JointId jointId ) +{ + b3World* world = b3GetUnlockedWorld( jointId.world0 ); + if ( world == NULL ) + { + return; + } + + B3_REC( world, JointWakeBodies, jointId ); + + world->locked = true; + + b3Joint* joint = b3GetJointFullId( world, jointId ); + b3Body* bodyA = b3Array_Get( world->bodies, joint->edges[0].bodyId ); + b3Body* bodyB = b3Array_Get( world->bodies, joint->edges[1].bodyId ); + + b3WakeBody( world, bodyA ); + b3WakeBody( world, bodyB ); + + world->locked = false; +} + +void b3GetJointReaction( b3World* world, b3JointSim* sim, float invTimeStep, float* force, float* torque ) +{ + float linearImpulse = 0.0f; + float angularImpulse = 0.0f; + + switch ( sim->type ) + { + case b3_parallelJoint: + { + b3ParallelJoint* joint = &sim->parallelJoint; + b3Vec3 impulse = { + .x = joint->perpImpulse.x, + .y = joint->perpImpulse.y, + .z = 0.0f, + }; + angularImpulse = b3Length( impulse ); + } + break; + + case b3_distanceJoint: + { + b3DistanceJoint* joint = &sim->distanceJoint; + linearImpulse = b3AbsFloat( joint->impulse + joint->lowerImpulse - joint->upperImpulse + joint->motorImpulse ); + } + break; + + case b3_motorJoint: + { + b3MotorJoint* joint = &sim->motorJoint; + linearImpulse = b3Length( b3Add( joint->linearVelocityImpulse, joint->linearSpringImpulse ) ); + angularImpulse = b3Length( b3Add( joint->angularVelocityImpulse, joint->angularSpringImpulse ) ); + } + break; + + case b3_prismaticJoint: + { + b3PrismaticJoint* joint = &sim->prismaticJoint; + b3Vec3 impulse = { + .x = joint->motorImpulse + joint->lowerImpulse - joint->upperImpulse, + .y = joint->perpImpulse.x, + .z = joint->perpImpulse.y, + }; + linearImpulse = b3Length( impulse ); + angularImpulse = b3Length( joint->angularImpulse ); + } + break; + + case b3_revoluteJoint: + { + b3RevoluteJoint* joint = &sim->revoluteJoint; + linearImpulse = b3Length( joint->linearImpulse ); + b3Vec3 impulse = { + .x = joint->perpImpulse.x, + .y = joint->perpImpulse.y, + .z = joint->motorImpulse + joint->lowerImpulse - joint->upperImpulse, + }; + angularImpulse = b3Length( impulse ); + } + break; + + case b3_sphericalJoint: + { + // todo improve performance + b3SphericalJoint* joint = &sim->sphericalJoint; + linearImpulse = b3Length( joint->linearImpulse ); + + b3WorldTransform xfA = b3GetBodyTransform( world, sim->bodyIdA ); + b3WorldTransform xfB = b3GetBodyTransform( world, sim->bodyIdB ); + b3Quat qA = b3MulQuat( xfA.q, sim->localFrameA.q ); + b3Quat qB = b3MulQuat( xfB.q, sim->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 ) ); + + b3Vec3 impulse = b3Add( joint->springImpulse, joint->motorImpulse ); + impulse = b3MulAdd( impulse, joint->lowerTwistImpulse - joint->upperTwistImpulse, twistAxis ); + impulse = b3MulAdd( impulse, joint->swingImpulse, swingAxis ); + + angularImpulse = b3Length( impulse ); + } + break; + + case b3_weldJoint: + { + b3WeldJoint* joint = &sim->weldJoint; + linearImpulse = b3Length( joint->linearImpulse ); + angularImpulse = b3Length( joint->angularImpulse ); + } + break; + + case b3_wheelJoint: + { + // todo probably wrong + b3WheelJoint* joint = &sim->wheelJoint; + b3Vec2 perpImpulse = joint->linearImpulse; + float axialImpulse = joint->suspensionSpringImpulse + joint->lowerSuspensionImpulse - joint->upperSuspensionImpulse; + linearImpulse = sqrtf( perpImpulse.x * perpImpulse.x + perpImpulse.y * perpImpulse.y + axialImpulse * axialImpulse ); + angularImpulse = b3AbsFloat( joint->spinImpulse ); + } + break; + + default: + break; + } + + *force = linearImpulse * invTimeStep; + *torque = angularImpulse * invTimeStep; +} + +static b3Vec3 b3GetJointConstraintForce( b3World* world, b3Joint* joint ) +{ + b3JointSim* base = b3GetJointSim( world, joint ); + + switch ( joint->type ) + { + case b3_parallelJoint: + return b3Vec3_zero; + + case b3_distanceJoint: + return b3GetDistanceJointForce( world, base ); + + case b3_filterJoint: + return b3Vec3_zero; + + case b3_motorJoint: + return b3GetMotorJointForce( world, base ); + + case b3_prismaticJoint: + return b3GetPrismaticJointForce( world, base ); + + case b3_revoluteJoint: + return b3GetRevoluteJointForce( world, base ); + + case b3_sphericalJoint: + return b3GetSphericalJointForce( world, base ); + + case b3_weldJoint: + return b3GetWeldJointForce( world, base ); + + case b3_wheelJoint: + return b3GetWheelJointForce( world, base ); + + default: + B3_ASSERT( false ); + return b3Vec3_zero; + } +} + +static b3Vec3 b3GetJointConstraintTorque( b3World* world, b3Joint* joint ) +{ + b3JointSim* base = b3GetJointSim( world, joint ); + + switch ( joint->type ) + { + case b3_parallelJoint: + return b3GetParallelJointTorque( world, base ); + + case b3_distanceJoint: + return b3Vec3_zero; + + case b3_filterJoint: + return b3Vec3_zero; + + case b3_motorJoint: + return b3GetMotorJointTorque( world, base ); + + case b3_prismaticJoint: + return b3GetPrismaticJointTorque( world, base ); + + case b3_revoluteJoint: + return b3GetRevoluteJointTorque( world, base ); + + case b3_sphericalJoint: + return b3GetSphericalJointTorque( world, base ); + + case b3_weldJoint: + return b3GetWeldJointTorque( world, base ); + + case b3_wheelJoint: + return b3GetWheelJointTorque( world, base ); + + default: + B3_ASSERT( false ); + return b3Vec3_zero; + } +} + +b3Vec3 b3Joint_GetConstraintForce( b3JointId jointId ) +{ + b3World* world = b3GetWorld( jointId.world0 ); + b3Joint* joint = b3GetJointFullId( world, jointId ); + return b3GetJointConstraintForce( world, joint ); +} + +b3Vec3 b3Joint_GetConstraintTorque( b3JointId jointId ) +{ + b3World* world = b3GetWorld( jointId.world0 ); + b3Joint* joint = b3GetJointFullId( world, jointId ); + return b3GetJointConstraintTorque( world, joint ); +} + +float b3Joint_GetLinearSeparation( b3JointId jointId ) +{ + b3World* world = b3GetWorld( jointId.world0 ); + b3Joint* joint = b3GetJointFullId( world, jointId ); + b3JointSim* base = b3GetJointSim( world, joint ); + + b3WorldTransform xfA = b3GetBodyTransform( world, joint->edges[0].bodyId ); + b3WorldTransform xfB = b3GetBodyTransform( world, joint->edges[1].bodyId ); + + b3Pos pA = b3TransformWorldPoint( xfA, base->localFrameA.p ); + b3Pos pB = b3TransformWorldPoint( xfB, base->localFrameB.p ); + b3Vec3 dp = b3SubPos( pB, pA ); + + switch ( joint->type ) + { + case b3_parallelJoint: + return 0.0f; + + case b3_distanceJoint: + { + b3DistanceJoint* distanceJoint = &base->distanceJoint; + float length = b3Length( dp ); + if ( distanceJoint->enableSpring ) + { + if ( distanceJoint->enableLimit ) + { + if ( length < distanceJoint->minLength ) + { + return distanceJoint->minLength - length; + } + + if ( length > distanceJoint->maxLength ) + { + return length - distanceJoint->maxLength; + } + + return 0.0f; + } + + return 0.0f; + } + + return b3AbsFloat( length - distanceJoint->length ); + } + + case b3_motorJoint: + return 0.0f; + + case b3_filterJoint: + return 0.0f; + + case b3_prismaticJoint: + { + b3PrismaticJoint* prismaticJoint = &base->prismaticJoint; + b3Vec3 axisA = b3RotateVector( xfA.q, b3Vec3_axisX ); + b3Vec3 perpA = b3Perp( axisA ); + float perpendicularSeparation = b3AbsFloat( b3Dot( perpA, dp ) ); + float limitSeparation = 0.0f; + + if ( prismaticJoint->enableLimit ) + { + float translation = b3Dot( axisA, dp ); + if ( translation < prismaticJoint->lowerTranslation ) + { + limitSeparation = prismaticJoint->lowerTranslation - translation; + } + + if ( prismaticJoint->upperTranslation < translation ) + { + limitSeparation = translation - prismaticJoint->upperTranslation; + } + } + + return sqrtf( perpendicularSeparation * perpendicularSeparation + limitSeparation * limitSeparation ); + } + + case b3_revoluteJoint: + return b3Length( dp ); + + case b3_sphericalJoint: + return b3Length( dp ); + + case b3_weldJoint: + { + b3WeldJoint* weldJoint = &base->weldJoint; + if ( weldJoint->linearHertz == 0.0f ) + { + return b3Length( dp ); + } + + return 0.0f; + } + + case b3_wheelJoint: + { + b3WheelJoint* wheelJoint = &base->wheelJoint; + b3Vec3 axisA = b3RotateVector( xfA.q, b3Vec3_axisX ); + b3Vec3 perpA = b3Perp( axisA ); + float perpendicularSeparation = b3AbsFloat( b3Dot( perpA, dp ) ); + float limitSeparation = 0.0f; + + if ( wheelJoint->enableSuspensionLimit ) + { + float translation = b3Dot( axisA, dp ); + if ( translation < wheelJoint->lowerSuspensionLimit ) + { + limitSeparation = wheelJoint->lowerSuspensionLimit - translation; + } + + if ( wheelJoint->upperSuspensionLimit < translation ) + { + limitSeparation = translation - wheelJoint->upperSuspensionLimit; + } + } + + return sqrtf( perpendicularSeparation * perpendicularSeparation + limitSeparation * limitSeparation ); + } + + default: + B3_ASSERT( false ); + return 0.0f; + } +} + +float b3Joint_GetAngularSeparation( b3JointId jointId ) +{ + b3World* world = b3GetWorld( jointId.world0 ); + b3Joint* joint = b3GetJointFullId( world, jointId ); + b3JointSim* base = b3GetJointSim( world, joint ); + + b3WorldTransform xfA = b3GetBodyTransform( world, joint->edges[0].bodyId ); + b3WorldTransform xfB = b3GetBodyTransform( world, joint->edges[1].bodyId ); + + b3Quat relQ = b3InvMulQuat( xfA.q, xfB.q ); + + switch ( joint->type ) + { + case b3_parallelJoint: + { + // Remove hinge angle + relQ.v.z = 0.0f; + return b3GetQuatAngle( relQ ); + } + + case b3_distanceJoint: + return 0.0f; + + case b3_motorJoint: + return 0.0f; + + case b3_filterJoint: + return 0.0f; + + case b3_prismaticJoint: + return b3GetQuatAngle( relQ ); + + case b3_revoluteJoint: + { + b3RevoluteJoint* revoluteJoint = &base->revoluteJoint; + if ( revoluteJoint->enableLimit ) + { + float angle = b3GetTwistAngle( relQ ); + if ( angle < revoluteJoint->lowerAngle ) + { + return b3GetQuatAngle( relQ ); + } + + if ( revoluteJoint->upperAngle < angle ) + { + return b3GetQuatAngle( relQ ); + } + } + + // Remove hinge angle + relQ.v.z = 0.0f; + return b3GetQuatAngle( relQ ); + } + + case b3_sphericalJoint: + { + b3SphericalJoint* sphericalJoint = &base->sphericalJoint; + float sum = 0.0f; + if ( sphericalJoint->enableConeLimit ) + { + float swingAngle = b3GetSwingAngle( relQ ); + sum += b3MaxFloat( 0.0f, swingAngle - sphericalJoint->coneAngle ); + } + + if ( sphericalJoint->enableTwistLimit ) + { + float twistAngle = b3GetTwistAngle( relQ ); + sum += b3MaxFloat( 0.0f, sphericalJoint->lowerTwistAngle - twistAngle ); + sum += b3MaxFloat( 0.0f, twistAngle - sphericalJoint->upperTwistAngle ); + } + + return sum; + } + + case b3_weldJoint: + { + b3WeldJoint* weldJoint = &base->weldJoint; + if ( weldJoint->angularHertz == 0.0f ) + { + return b3GetQuatAngle( relQ ); + } + + return 0.0f; + } + + case b3_wheelJoint: + // todo + B3_ASSERT( false ); + return 0.0f; + + default: + B3_ASSERT( false ); + return 0.0f; + } +} + +#if 0 +void b3Joint_SetSpringRotationTarget( b3JointId jointId, b3Quat relativeBodyRotation, float hertz ) +{ + B3_ASSERT( b3IsValidQuat( relativeBodyRotation ) ); + B3_ASSERT( b3IsValidFloat( hertz ) && hertz > 0.0f ); + + b3World* world = b3GetWorld( jointId.world0 ); + b3Joint* joint = b3GetJointFullId( world, jointId ); + b3JointSim* base = b3GetJointSim( world, joint ); + + b3Quat qA = base->localFrameA.q; + b3Quat qB = b3MulQuat( relativeBodyRotation, base->localFrameB.q ); + + // This keeps the twist angle in the range [-pi, pi] + if ( b3DotQuat( qA, qB ) < 0.0f ) + { + qA = -qA; + } + + b3Quat relQ = b3InvMulQuat( qA, qB ); + + switch ( joint->type ) + { + case b3_revoluteJoint: + base->revoluteJoint.targetAngle = b3GetTwistAngle( relQ ); + base->revoluteJoint.hertz = hertz; + break; + + case b3_sphericalJoint: + base->sphericalJoint.targetRotation = relQ; + base->sphericalJoint.hertz = hertz; + break; + + default: + break; + } +} +#endif + +void b3PrepareJoint( b3JointSim* joint, b3StepContext* context ) +{ + // Clamp joint hertz based on the time step to reduce jitter. + float hertz = b3MinFloat( joint->constraintHertz, 0.25f * context->inv_h ); + joint->constraintSoftness = b3MakeSoft( hertz, joint->constraintDampingRatio, context->h ); + + switch ( joint->type ) + { + case b3_parallelJoint: + b3PrepareParallelJoint( joint, context ); + break; + + case b3_distanceJoint: + b3PrepareDistanceJoint( joint, context ); + break; + + case b3_filterJoint: + break; + + case b3_motorJoint: + b3PrepareMotorJoint( joint, context ); + break; + + case b3_prismaticJoint: + b3PreparePrismaticJoint( joint, context ); + break; + + case b3_revoluteJoint: + b3PrepareRevoluteJoint( joint, context ); + break; + + case b3_sphericalJoint: + b3PrepareSphericalJoint( joint, context ); + break; + + case b3_weldJoint: + b3PrepareWeldJoint( joint, context ); + break; + + case b3_wheelJoint: + b3PrepareWheelJoint( joint, context ); + break; + + default: + B3_ASSERT( false ); + } +} + +void b3WarmStartJoint( b3JointSim* joint, b3StepContext* context ) +{ + switch ( joint->type ) + { + case b3_parallelJoint: + b3WarmStartParallelJoint( joint, context ); + break; + + case b3_distanceJoint: + b3WarmStartDistanceJoint( joint, context ); + break; + + case b3_filterJoint: + break; + + case b3_motorJoint: + b3WarmStartMotorJoint( joint, context ); + break; + + case b3_prismaticJoint: + b3WarmStartPrismaticJoint( joint, context ); + break; + + case b3_revoluteJoint: + b3WarmStartRevoluteJoint( joint, context ); + break; + + case b3_sphericalJoint: + b3WarmStartSphericalJoint( joint, context ); + break; + + case b3_weldJoint: + b3WarmStartWeldJoint( joint, context ); + break; + + case b3_wheelJoint: + b3WarmStartWheelJoint( joint, context ); + break; + + default: + B3_ASSERT( false ); + } +} + +void b3SolveJoint( b3JointSim* joint, b3StepContext* context, bool useBias ) +{ + B3_UNUSED( useBias ); + + switch ( joint->type ) + { + case b3_parallelJoint: + b3SolveParallelJoint( joint, context ); + break; + + case b3_distanceJoint: + b3SolveDistanceJoint( joint, context, useBias ); + break; + + case b3_filterJoint: + break; + + case b3_motorJoint: + b3SolveMotorJoint( joint, context ); + break; + + case b3_prismaticJoint: + b3SolvePrismaticJoint( joint, context, useBias ); + break; + + case b3_revoluteJoint: + b3SolveRevoluteJoint( joint, context, useBias ); + break; + + case b3_sphericalJoint: + b3SolveSphericalJoint( joint, context, useBias ); + break; + + case b3_weldJoint: + b3SolveWeldJoint( joint, context, useBias ); + break; + + case b3_wheelJoint: + b3SolveWheelJoint( joint, context, useBias ); + break; + + default: + B3_ASSERT( false ); + } +} + +void b3PrepareJoints_Overflow( b3StepContext* context ) +{ + b3TracyCZoneNC( prepare_joints, "PrepJoints", b3_colorOldLace, true ); + + b3ConstraintGraph* graph = context->graph; + b3JointSim* joints = graph->colors[B3_OVERFLOW_INDEX].jointSims.data; + int jointCount = graph->colors[B3_OVERFLOW_INDEX].jointSims.count; + + for ( int i = 0; i < jointCount; ++i ) + { + b3JointSim* joint = joints + i; + b3PrepareJoint( joint, context ); + } + + b3TracyCZoneEnd( prepare_joints ); +} + +void b3WarmStartJoints_Overflow( b3StepContext* context ) +{ + b3TracyCZoneNC( prepare_joints, "PrepJoints", b3_colorOldLace, true ); + + b3ConstraintGraph* graph = context->graph; + b3JointSim* joints = graph->colors[B3_OVERFLOW_INDEX].jointSims.data; + int jointCount = graph->colors[B3_OVERFLOW_INDEX].jointSims.count; + + for ( int i = 0; i < jointCount; ++i ) + { + b3JointSim* joint = joints + i; + b3WarmStartJoint( joint, context ); + } + + b3TracyCZoneEnd( prepare_joints ); +} + +void b3SolveJoints_Overflow( b3StepContext* context, bool useBias ) +{ + b3TracyCZoneNC( solve_joints, "SolveJoints", b3_colorLemonChiffon, true ); + + b3ConstraintGraph* graph = context->graph; + b3JointSim* joints = graph->colors[B3_OVERFLOW_INDEX].jointSims.data; + int jointCount = graph->colors[B3_OVERFLOW_INDEX].jointSims.count; + + for ( int i = 0; i < jointCount; ++i ) + { + b3JointSim* joint = joints + i; + b3SolveJoint( joint, context, useBias ); + } + + b3TracyCZoneEnd( solve_joints ); +} + +void b3DrawJoint( b3DebugDraw* draw, b3World* world, b3Joint* joint ) +{ + b3Body* bodyA = b3Array_Get( world->bodies, joint->edges[0].bodyId ); + b3Body* bodyB = b3Array_Get( world->bodies, joint->edges[1].bodyId ); + if ( bodyA->setIndex == b3_disabledSet || bodyB->setIndex == b3_disabledSet ) + { + return; + } + + b3JointSim* jointSim = b3GetJointSim( world, joint ); + + b3WorldTransform transformA = b3GetBodyTransformQuick( world, bodyA ); + b3WorldTransform transformB = b3GetBodyTransformQuick( world, bodyB ); + b3Pos pA = b3TransformWorldPoint( transformA, jointSim->localFrameA.p ); + b3Pos pB = b3TransformWorldPoint( transformB, jointSim->localFrameB.p ); + + b3HexColor color = b3_colorDarkSeaGreen; + + float scale = b3MaxFloat( 0.0001f, draw->jointScale * joint->drawScale ); + + switch ( joint->type ) + { + case b3_parallelJoint: + b3DrawParallelJoint( draw, jointSim, transformA, transformB, scale ); + break; + + case b3_distanceJoint: + b3DrawDistanceJoint( draw, jointSim, transformA, transformB ); + break; + + case b3_filterJoint: + draw->DrawSegmentFcn( pA, pB, b3_colorGold, draw->context ); + break; + + case b3_motorJoint: + draw->DrawSegmentFcn( pA, pB, b3_colorPlum, draw->context ); + draw->DrawPointFcn( pA, 8.0f, b3_colorYellowGreen, draw->context ); + draw->DrawPointFcn( pB, 8.0f, b3_colorPlum, draw->context ); + break; + + case b3_prismaticJoint: + b3DrawPrismaticJoint( draw, jointSim, transformA, transformB, scale ); + break; + + case b3_revoluteJoint: + b3DrawRevoluteJoint( draw, jointSim, transformA, transformB, scale ); + break; + + case b3_sphericalJoint: + b3DrawSphericalJoint( draw, jointSim, transformA, transformB, scale ); + break; + + case b3_weldJoint: + b3DrawWeldJoint( draw, jointSim, transformA, transformB, scale ); + break; + + case b3_wheelJoint: + b3DrawWheelJoint( draw, jointSim, transformA, transformB, scale ); + break; + + default: + draw->DrawSegmentFcn( transformA.p, pA, color, draw->context ); + draw->DrawSegmentFcn( pA, pB, color, draw->context ); + draw->DrawSegmentFcn( transformB.p, pB, color, draw->context ); + break; + } + + if ( draw->drawGraphColors ) + { + b3HexColor graphColors[B3_GRAPH_COLOR_COUNT] = { + b3_colorRed, b3_colorOrange, b3_colorYellow, b3_colorGreen, b3_colorCyan, b3_colorBlue, + b3_colorViolet, b3_colorPink, b3_colorChocolate, b3_colorGoldenRod, b3_colorCoral, b3_colorRosyBrown, + b3_colorAqua, b3_colorPeru, b3_colorLime, b3_colorGold, b3_colorPlum, b3_colorSnow, + b3_colorTeal, b3_colorKhaki, b3_colorSalmon, b3_colorPeachPuff, b3_colorHoneyDew, b3_colorBlack, + }; + + int colorIndex = joint->colorIndex; + if ( colorIndex != B3_NULL_INDEX ) + { + b3Pos p = b3LerpPosition( pA, pB, 0.5f ); + draw->DrawPointFcn( p, 5.0f, graphColors[colorIndex], draw->context ); + } + } + + if ( draw->drawJointExtras ) + { + b3Vec3 force = b3GetJointConstraintForce( world, joint ); + b3Vec3 torque = b3GetJointConstraintTorque( world, joint ); + b3Pos p = b3LerpPosition( pA, pB, 0.5f ); + + draw->DrawSegmentFcn( p, b3OffsetPos( p, b3MulSV( 0.001f, force ) ), b3_colorAzure, draw->context ); + + char buffer[64]; + snprintf( buffer, 64, "f = %g, t = %g", b3Length( force ), b3Length( torque ) ); + draw->DrawStringFcn( p, buffer, b3_colorAzure, draw->context ); + } +} diff --git a/vendor/box3d/src/src/joint.h b/vendor/box3d/src/src/joint.h new file mode 100644 index 000000000..64171b957 --- /dev/null +++ b/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 ); diff --git a/vendor/box3d/src/src/manifold.c b/vendor/box3d/src/src/manifold.c new file mode 100644 index 000000000..40e60ad57 --- /dev/null +++ b/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; +} diff --git a/vendor/box3d/src/src/manifold.h b/vendor/box3d/src/src/manifold.h new file mode 100644 index 000000000..9e529ab97 --- /dev/null +++ b/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; +} diff --git a/vendor/box3d/src/src/math_functions.c b/vendor/box3d/src/src/math_functions.c new file mode 100644 index 000000000..a339d95ff --- /dev/null +++ b/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 +#include + +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; +} diff --git a/vendor/box3d/src/src/math_internal.h b/vendor/box3d/src/src/math_internal.h new file mode 100644 index 000000000..4d152c5fc --- /dev/null +++ b/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 ) }; +} diff --git a/vendor/box3d/src/src/mesh.c b/vendor/box3d/src/src/mesh.c new file mode 100644 index 000000000..c03489955 --- /dev/null +++ b/vendor/box3d/src/src/mesh.c @@ -0,0 +1,2411 @@ +// SPDX-FileCopyrightText: 2026 Erin Catto +// SPDX-License-Identifier: MIT + +// Dirk Gregorius contributed portions of this code + +#include "container.h" +#include "math_internal.h" +#include "shape.h" +#include "simd.h" + +#include "box3d/collision.h" +#include "box3d/constants.h" + +#include + +b3DeclareArray( b3VertexNode ); +b3DeclareArray( b3MeshNode ); +b3DeclareArray( b3MeshTriangle ); +b3DeclareArray( b3Vec3 ); +b3DeclareArray( b3Primitive ); +b3DeclareArrayNative( uint8_t ); + +#define B3_BIN_COUNT 8 +#define B3_DESIRED_TRIANGLES_PER_LEAF 4 +#define B3_LEAF_NODE 3 +#define B3_MAXIMUM_TRIANGLES_PER_LEAF 8 +#define B3_MESH_STACK_SIZE 256 + +static bool b3IsLeaf( const b3MeshNode* node ) +{ + return node->data.asLeaf.type == B3_LEAF_NODE; +} + +static b3MeshNode* b3GetMeshNodesWrite( b3MeshData* mesh ) +{ + if ( mesh->nodeOffset == 0 ) + { + return NULL; + } + + return (b3MeshNode*)( (intptr_t)mesh + mesh->nodeOffset ); +} + +static b3MeshNode* b3GetLeftChildWrite( b3MeshNode* node ) +{ + // The left child follows its parent. + B3_ASSERT( !b3IsLeaf( node ) ); + return node + 1; +} + +static const b3MeshNode* b3GetLeftChild( const b3MeshNode* node ) +{ + // The left child follows its parent. + B3_ASSERT( !b3IsLeaf( node ) ); + return node + 1; +} + +static b3MeshNode* b3GetRightChildWrite( b3MeshNode* node ) +{ + // We store the offset of the right child relative to its parent + B3_ASSERT( !b3IsLeaf( node ) ); + return node + node->data.asNode.childOffset; +} + +static const b3MeshNode* b3GetRightChild( const b3MeshNode* node ) +{ + // We store the offset of the right child relative to its parent + B3_ASSERT( !b3IsLeaf( node ) ); + return node + node->data.asNode.childOffset; +} + +static const b3MeshNode* b3GetRoot( const b3MeshData* mesh ) +{ + // The first node is the root + return b3GetMeshNodes( mesh ); +} + +static b3MeshNode* b3GetRootWrite( b3MeshData* mesh ) +{ + // The first node is the root + return b3GetMeshNodesWrite( mesh ); +} + +static b3MeshTriangle* b3GetMeshTrianglesWrite( b3MeshData* mesh ) +{ + if ( mesh->triangleOffset == 0 ) + { + return NULL; + } + + return (b3MeshTriangle*)( (intptr_t)mesh + mesh->triangleOffset ); +} + +static b3Vec3* b3GetMeshVerticesWrite( b3MeshData* mesh ) +{ + if ( mesh->vertexOffset == 0 ) + { + return NULL; + } + + return (b3Vec3*)( (intptr_t)mesh + mesh->vertexOffset ); +} + +// static b3Vec3 b3GetVertex( b3MeshData& mesh, int vertexIndex ) +//{ +// B3_ASSERT( 0 <= vertexIndex && vertexIndex < mesh.vertexCount ); +// b3Vec3* vertices = b3GetMeshVertices( &mesh ); +// return vertices[vertexIndex]; +// } + +static uint8_t* b3GetMeshMaterialIndicesWrite( b3MeshData* mesh ) +{ + if ( mesh->materialOffset == 0 ) + { + return NULL; + } + + return (uint8_t*)( (intptr_t)mesh + mesh->materialOffset ); +} + +static uint8_t* b3GetMeshFlagsWrite( b3MeshData* mesh ) +{ + if ( mesh->flagsOffset == 0 ) + { + return NULL; + } + + return (uint8_t*)( (intptr_t)mesh + mesh->flagsOffset ); +} + +static int b3GetNodeHeight( const b3MeshNode* node ) +{ + if ( b3IsLeaf( node ) ) + { + return 0; + } + + const b3MeshNode* leftChild = b3GetLeftChild( node ); + int leftHeight = b3GetNodeHeight( leftChild ); + const b3MeshNode* rightChild = b3GetRightChild( node ); + int rightHeight = b3GetNodeHeight( rightChild ); + + return 1 + b3MaxInt( leftHeight, rightHeight ); +} + +int b3GetHeight( const b3MeshData* mesh ) +{ + const b3MeshNode* root = b3GetRoot( mesh ); + if ( root == NULL ) + { + return 0; + } + + return b3GetNodeHeight( root ); +} + +#if B3_ENABLE_VALIDATION == 1 +static bool b3IsDegenerate( b3Vec3 v1, b3Vec3 v2, b3Vec3 v3, float minArea ) +{ + b3Vec3 normal = b3Cross( b3Sub( v2, v1 ), b3Sub( v3, v1 ) ); + float lengthSq = b3LengthSquared( normal ); + return lengthSq < minArea * minArea; +} + +static bool b3IsNonDegenerate( const b3MeshData* mesh, float minArea ) +{ + const b3MeshTriangle* triangles = b3GetMeshTriangles( mesh ); + const b3Vec3* vertices = b3GetMeshVertices( mesh ); + + // Check triangles + for ( int index = 0; index < mesh->triangleCount; ++index ) + { + // Index range + b3MeshTriangle triangle = triangles[index]; + if ( triangle.index1 >= mesh->vertexCount ) + { + return false; + } + + if ( triangle.index2 >= mesh->vertexCount ) + { + return false; + } + + if ( triangle.index3 >= mesh->vertexCount ) + { + return false; + } + + // Degenerate topology + if ( triangle.index1 == triangle.index2 ) + { + return false; + } + if ( triangle.index1 == triangle.index3 ) + { + return false; + } + if ( triangle.index2 == triangle.index3 ) + { + return false; + } + + // Degenerate geometry + b3Vec3 vertex1 = vertices[triangle.index1]; + b3Vec3 vertex2 = vertices[triangle.index2]; + b3Vec3 vertex3 = vertices[triangle.index3]; + if ( b3IsDegenerate( vertex1, vertex2, vertex3, minArea ) ) + { + return false; + } + } + + return true; +} + +static inline b3AABB b3GetNodeAABB( const b3MeshNode* node ) +{ + return (b3AABB){ + node->lowerBound, + node->upperBound, + }; +} + +static bool b3IsConsistent( const b3MeshData* mesh ) +{ + const b3MeshTriangle* triangles = b3GetMeshTriangles( mesh ); + const b3Vec3* vertices = b3GetMeshVertices( mesh ); + + // Check nodes + int count = 0; + const b3MeshNode* stack[64]; + stack[count++] = b3GetRoot( mesh ); + + while ( count > 0 ) + { + const b3MeshNode* node = stack[--count]; + b3AABB nodeBounds = b3GetNodeAABB( node ); + + if ( b3IsLeaf( node ) == false ) + { + const b3MeshNode* child1 = b3GetLeftChild( node ); + b3AABB bounds1 = b3GetNodeAABB( child1 ); + const b3MeshNode* child2 = b3GetRightChild( node ); + b3AABB bounds2 = b3GetNodeAABB( child2 ); + + if ( !b3AABB_Contains( nodeBounds, bounds1 ) ) + { + return false; + } + + if ( !b3AABB_Contains( nodeBounds, bounds2 ) ) + { + return false; + } + + stack[count++] = child2; + stack[count++] = child1; + } + else + { + b3AABB triangleBounds = B3_BOUNDS3_EMPTY; + for ( uint32_t index = 0; index < node->data.asLeaf.triangleCount; ++index ) + { + int triangleIndex = node->triangleOffset + index; + B3_ASSERT( 0 <= triangleIndex && triangleIndex < mesh->triangleCount ); + + b3MeshTriangle triangle = triangles[triangleIndex]; + + b3AABB vertexBounds = B3_BOUNDS3_EMPTY; + vertexBounds = b3AABB_AddPoint( vertexBounds, vertices[triangle.index1] ); + vertexBounds = b3AABB_AddPoint( vertexBounds, vertices[triangle.index2] ); + vertexBounds = b3AABB_AddPoint( vertexBounds, vertices[triangle.index3] ); + + triangleBounds = b3AABB_Union( triangleBounds, vertexBounds ); + } + + if ( !b3AABB_Contains( nodeBounds, triangleBounds ) ) + { + return false; + } + } + } + + return true; +} + +bool b3IsValidMesh( const b3MeshData* meshData ) +{ + if ( meshData == NULL ) + { + return false; + } + + if ( meshData->version != B3_MESH_VERSION ) + { + return false; + } + + if ( meshData->byteCount < (int)sizeof( b3MeshData ) ) + { + return false; + } + + return b3IsConsistent( meshData ); +} + +#else + +bool b3IsValidMesh( const b3MeshData* meshData ) +{ + if ( meshData == NULL ) + { + return false; + } + + if ( meshData->version != B3_MESH_VERSION ) + { + return false; + } + + if ( meshData->byteCount < (int)sizeof( b3MeshData ) ) + { + return false; + } + + return true; +} + +#endif + +// Node for a vertex linked list +typedef struct b3VertexNode +{ + int32_t vertexIndex; + int nextNodeIndex; +} b3VertexNode; + +#define NAME b3VertexMap +#define KEY_TY uint64_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" + +typedef struct b3SpatialHash +{ + b3Array( b3VertexNode ) nodes; + const b3Vec3* vertices; + int vertexCount; + b3VertexMap vertexMap; + float cellSize; + float tolerance; +} b3SpatialHash; + +static void b3SpatialHash_Create( b3SpatialHash* h, const b3Vec3* vertices, int vertexCount, float tolerance ) +{ + h->vertices = vertices; + h->vertexCount = vertexCount; + h->tolerance = tolerance; + h->cellSize = 2.0f * tolerance; + b3Array_CreateN( h->nodes, vertexCount ); + + b3VertexMap_init( &h->vertexMap ); + b3VertexMap_reserve( &h->vertexMap, vertexCount ); + + B3_ASSERT( h->cellSize > 0.0f ); +} + +static void b3SpatialHash_Destroy( b3SpatialHash* h ) +{ + b3VertexMap_cleanup( &h->vertexMap ); + b3Array_Destroy( h->nodes ); +} + +// Welding works by bucketing nearby vertices into identical keys in a hash table. +// Bucketing is done manually with an array. +static int32_t b3SpatialHash_FindDuplicate( b3SpatialHash* h, int32_t currentIndex ) +{ + B3_ASSERT( currentIndex < h->vertexCount ); + b3Vec3 vertex = h->vertices[currentIndex]; + float cellSize = h->cellSize; + float tolerance = h->tolerance; + + // Get the grid coordinates for the current vertex + int32_t baseX = (int32_t)( floorf( vertex.x / cellSize ) ); + int32_t baseY = (int32_t)( floorf( vertex.y / cellSize ) ); + int32_t baseZ = (int32_t)( floorf( vertex.z / cellSize ) ); + + // Check the current cell and all 26 neighboring cells (3x3x3 - 1) + for ( int dx = -1; dx <= 1; ++dx ) + { + for ( int dy = -1; dy <= 1; ++dy ) + { + for ( int dz = -1; dz <= 1; ++dz ) + { + int32_t x = baseX + dx; + int32_t y = baseY + dy; + int32_t z = baseZ + dz; + + // Compute hash for this neighboring cell (this is the key in the map) + uint64_t key = 0; + key ^= (uint64_t)( x ) + 0x9e3779b9 + ( key << 6 ) + ( key >> 2 ); + key ^= (uint64_t)( y ) + 0x9e3779b9 + ( key << 6 ) + ( key >> 2 ); + key ^= (uint64_t)( z ) + 0x9e3779b9 + ( key << 6 ) + ( key >> 2 ); + + b3VertexMap_itr it = b3VertexMap_get( &h->vertexMap, key ); + if ( b3VertexMap_is_end( it ) == false ) + { + // Check all vertices in this key + int nodeIndex = it.data->val; + + while ( nodeIndex != B3_NULL_INDEX ) + { + b3VertexNode node = h->nodes.data[nodeIndex]; + + int32_t existingIndex = node.vertexIndex; + B3_ASSERT( existingIndex < currentIndex ); + B3_ASSERT( existingIndex < h->vertexCount ); + + b3Vec3 other = h->vertices[existingIndex]; + + // IsEqual inlined: check if vertices are within tolerance + if ( fabsf( vertex.x - other.x ) <= tolerance && fabsf( vertex.y - other.y ) <= tolerance && + fabsf( vertex.z - other.z ) <= tolerance ) + { + // Found duplicate + return existingIndex; + } + + nodeIndex = node.nextNodeIndex; + } + } + } + } + } + + // No duplicate found, add to hash table + uint64_t currentKey = 0; + currentKey ^= (uint64_t)( baseX ) + 0x9e3779b9 + ( currentKey << 6 ) + ( currentKey >> 2 ); + currentKey ^= (uint64_t)( baseY ) + 0x9e3779b9 + ( currentKey << 6 ) + ( currentKey >> 2 ); + currentKey ^= (uint64_t)( baseZ ) + 0x9e3779b9 + ( currentKey << 6 ) + ( currentKey >> 2 ); + + b3VertexMap_itr it = b3VertexMap_get( &h->vertexMap, currentKey ); + if ( b3VertexMap_is_end( it ) == false ) + { + int nodeIndex = it.data->val; + + b3VertexNode node = { + .vertexIndex = currentIndex, + .nextNodeIndex = nodeIndex, + }; + + it.data->val = h->nodes.count; + b3Array_Push( h->nodes, node ); + } + else + { + b3VertexNode node = { + .vertexIndex = currentIndex, + .nextNodeIndex = B3_NULL_INDEX, + }; + + b3VertexMap_insert( &h->vertexMap, currentKey, h->nodes.count ); + b3Array_Push( h->nodes, node ); + } + + // Not welded + return B3_NULL_INDEX; +} + +typedef struct b3WeldData +{ + const b3Vec3* srcVertices; + const int32_t* srcIndices; + + b3Vec3* dstVertices; + int32_t* dstIndices; + + int vertexCount; + int indexCount; +} b3WeldData; + +static int b3WeldVertices( b3WeldData* data, float tolerance ) +{ + int vertexCount = data->vertexCount; + int uniqueCount = 0; + + // Create spatial hash and find duplicates + b3SpatialHash spatialHash; + b3SpatialHash_Create( &spatialHash, data->srcVertices, vertexCount, tolerance ); + b3Array( int ) vertexMapping = { 0 }; + b3Array_Resize( vertexMapping, vertexCount ); + + for ( int i = 0; i < vertexCount; ++i ) + { + int32_t duplicateIndex = b3SpatialHash_FindDuplicate( &spatialHash, i ); + + if ( duplicateIndex == B3_NULL_INDEX ) + { + // New unique vertex + vertexMapping.data[i] = uniqueCount; + data->dstVertices[uniqueCount] = data->srcVertices[i]; + uniqueCount += 1; + } + else + { + // Found duplicate, map to existing vertex + vertexMapping.data[i] = vertexMapping.data[duplicateIndex]; + } + } + + // Update indices to reference the new vertex array + int indexCount = data->indexCount; + for ( int i = 0; i < indexCount; ++i ) + { + int srcIndex = data->srcIndices[i]; + B3_ASSERT( srcIndex < vertexCount ); + data->dstIndices[i] = vertexMapping.data[srcIndex]; + } + + b3SpatialHash_Destroy( &spatialHash ); + b3Array_Destroy( vertexMapping ); + + return uniqueCount; +} + +static inline void b3StoreLeaf( b3MeshNode* node, const b3AABB* aabb, int triangleCount, int triangleOffset ) +{ + node->data.asLeaf.type = B3_LEAF_NODE; + node->data.asLeaf.triangleCount = triangleCount; + node->triangleOffset = triangleOffset; + node->lowerBound = aabb->lowerBound; + node->upperBound = aabb->upperBound; +} + +typedef struct b3Primitive +{ + b3AABB aabb; + b3Vec3 center; + int triangleIndex; +} b3Primitive; + +typedef struct b3Bucket +{ + int count; + b3AABB bounds; +} b3Bucket; + +typedef struct b3Split +{ + b3AABB leftBounds; + b3AABB rightBounds; + int axis; + int index; +} b3Split; + +static b3Split b3SplitBinnedSah( int count, b3Primitive* primitives ) +{ + b3Split split; + split.axis = -1; + split.index = -1; + + // Compute bounds of primitive centroids and choose split axis + b3AABB bounds = { primitives[0].center, primitives[0].center }; + for ( int i = 1; i < count; ++i ) + { + bounds = b3AABB_AddPoint( bounds, primitives[i].center ); + } + + // Compute costs for splitting after each bucket and keep track of best split + // This is a small O(n^2) loop. This can be further optimized, but it is already + // very fast and is kept for simplicity right now. + int bestBucket = -1; + float bestCost = FLT_MAX; + + for ( int axis = 0; axis < 3; ++axis ) + { + b3Vec3 extent = b3AABB_Extents( bounds ); + if ( b3GetByIndex( extent, axis ) < B3_LINEAR_SLOP ) + { + continue; + } + + // Initialize buckets + b3Bucket buckets[B3_BIN_COUNT]; + for ( int i = 0; i < B3_BIN_COUNT; ++i ) + { + buckets[i].count = 0; + buckets[i].bounds = B3_BOUNDS3_EMPTY; + } + + // Fill buckets + float factor = B3_BIN_COUNT * ( 1.0f - FLT_EPSILON ) / + ( b3GetByIndex( bounds.upperBound, axis ) - b3GetByIndex( bounds.lowerBound, axis ) ); + for ( int i = 0; i < count; ++i ) + { + b3Vec3 center = primitives[i].center; + int index = (int)( factor * ( b3GetByIndex( center, axis ) - b3GetByIndex( bounds.lowerBound, axis ) ) ); + B3_ASSERT( 0 <= index && index < B3_BIN_COUNT ); + + buckets[index].count++; + buckets[index].bounds = b3AABB_Union( buckets[index].bounds, primitives[i].aabb ); + } + + // Evaluate splits + for ( int i = 0; i < B3_BIN_COUNT - 1; ++i ) + { + int leftCount = 0; + b3AABB leftBounds = B3_BOUNDS3_EMPTY; + for ( int k = 0; k <= i; ++k ) + { + leftCount += buckets[k].count; + leftBounds = b3AABB_Union( leftBounds, buckets[k].bounds ); + } + + int rightCount = 0; + b3AABB rightBounds = B3_BOUNDS3_EMPTY; + for ( int k = i + 1; k < B3_BIN_COUNT; ++k ) + { + rightCount += buckets[k].count; + rightBounds = b3AABB_Union( rightBounds, buckets[k].bounds ); + } + + B3_ASSERT( leftCount + rightCount == count ); + if ( leftCount > 0 && rightCount > 0 ) + { + float cost = leftCount * b3AABB_Area( leftBounds ) + rightCount * b3AABB_Area( rightBounds ); + + if ( cost < bestCost ) + { + bestBucket = i; + bestCost = cost; + + split.axis = axis; + split.index = leftCount; + split.leftBounds = leftBounds; + split.rightBounds = rightBounds; + } + } + } + } + + // Partition + if ( bestBucket >= 0 ) + { + int axis = split.axis; + float factor = B3_BIN_COUNT * ( 1.0f - FLT_EPSILON ) / + ( b3GetByIndex( bounds.upperBound, axis ) - b3GetByIndex( bounds.lowerBound, axis ) ); + + int splitIndex = 0; + for ( int i = 0; i < count; ++i ) + { + b3Vec3 center = primitives[i].center; + int index = (int)( factor * ( b3GetByIndex( center, axis ) - b3GetByIndex( bounds.lowerBound, axis ) ) ); + + if ( index <= bestBucket ) + { + b3Primitive temp = primitives[i]; + primitives[i] = primitives[splitIndex]; + primitives[splitIndex] = temp; + splitIndex++; + } + } + B3_ASSERT( splitIndex == split.index ); + } + + return split; +} + +static b3Split b3SplitHalf( int count, b3Primitive* primitives ) +{ + // Split in the middle + int splitIndex = count / 2; + + b3AABB leftBounds = B3_BOUNDS3_EMPTY; + for ( int i = 0; i < splitIndex; ++i ) + { + leftBounds = b3AABB_Union( leftBounds, primitives[i].aabb ); + } + + b3AABB rightBounds = B3_BOUNDS3_EMPTY; + for ( int i = splitIndex; i < count; ++i ) + { + + rightBounds = b3AABB_Union( rightBounds, primitives[i].aabb ); + } + + b3AABB bounds = b3AABB_Union( leftBounds, rightBounds ); + int axis = b3MajorAxis( b3AABB_Extents( bounds ) ); + + b3Split split; + split.axis = axis; + split.index = splitIndex; + split.leftBounds = leftBounds; + split.rightBounds = rightBounds; + + return split; +} + +static b3Split b3SplitMedian( int count, b3Primitive* primitives ) +{ + B3_ASSERT( count > 2 ); + + b3Vec3 lowerBound = primitives[0].center; + b3Vec3 upperBound = primitives[0].center; + + for ( int i = 1; i < count; ++i ) + { + lowerBound = b3Min( lowerBound, primitives[i].center ); + upperBound = b3Max( upperBound, primitives[i].center ); + } + + b3Vec3 d = b3Sub( upperBound, lowerBound ); + b3Vec3 c = b3MulSV( 0.5f, b3Add( lowerBound, upperBound ) ); + + b3Split split = { 0 }; + split.index = -1; + + // Partition longest axis using the Hoare partition scheme + // https://en.wikipedia.org/wiki/Quicksort + // https://nicholasvadivelu.com/2021/01/11/array-partition/ + int i1 = 0, i2 = count; + if ( d.x >= d.y && d.x >= d.z ) + { + split.axis = 0; + + float pivot = c.x; + + while ( i1 < i2 ) + { + while ( i1 < i2 && primitives[i1].center.x < pivot ) + { + i1 += 1; + }; + + while ( i1 < i2 && primitives[i2 - 1].center.x >= pivot ) + { + i2 -= 1; + }; + + if ( i1 < i2 ) + { + // Swap primitives + b3Primitive temp = primitives[i1]; + primitives[i1] = primitives[i2 - 1]; + primitives[i2 - 1] = temp; + + i1 += 1; + i2 -= 1; + } + } + } + else if ( d.y >= d.z ) + { + split.axis = 1; + + float pivot = c.y; + + while ( i1 < i2 ) + { + while ( i1 < i2 && primitives[i1].center.y < pivot ) + { + i1 += 1; + }; + + while ( i1 < i2 && primitives[i2 - 1].center.y >= pivot ) + { + i2 -= 1; + }; + + if ( i1 < i2 ) + { + // Swap primitives + b3Primitive temp = primitives[i1]; + primitives[i1] = primitives[i2 - 1]; + primitives[i2 - 1] = temp; + + i1 += 1; + i2 -= 1; + } + } + } + else + { + split.axis = 2; + + float pivot = c.z; + + while ( i1 < i2 ) + { + while ( i1 < i2 && primitives[i1].center.z < pivot ) + { + i1 += 1; + }; + + while ( i1 < i2 && primitives[i2 - 1].center.z >= pivot ) + { + i2 -= 1; + }; + + if ( i1 < i2 ) + { + // Swap primitives + b3Primitive temp = primitives[i1]; + primitives[i1] = primitives[i2 - 1]; + primitives[i2 - 1] = temp; + + i1 += 1; + i2 -= 1; + } + } + } + B3_ASSERT( i1 == i2 ); + B3_ASSERT( 0 <= i1 && i1 < count ); + + if ( i1 == 0 || i1 == count - 1 ) + { + // failed to split + i1 = count / 2; + } + + b3AABB leftBounds = B3_BOUNDS3_EMPTY; + for ( int i = 0; i < i1; ++i ) + { + leftBounds = b3AABB_Union( leftBounds, primitives[i].aabb ); + } + + b3AABB rightBounds = B3_BOUNDS3_EMPTY; + for ( int i = i1; i < count; ++i ) + { + rightBounds = b3AABB_Union( rightBounds, primitives[i].aabb ); + } + + split.index = i1; + split.leftBounds = leftBounds; + split.rightBounds = rightBounds; + return split; +} + +#if B3_ENABLE_VALIDATION == 1 + +static bool b3ValidateSplit( int count, b3Primitive* primitives, const b3Split* split ) +{ + if ( split->axis < 0 ) + { + return false; + } + + for ( int i = 0; i < split->index; ++i ) + { + if ( !b3AABB_Contains( split->leftBounds, primitives[i].aabb ) ) + { + return false; + } + } + + for ( int i = split->index; i < count; ++i ) + { + if ( !b3AABB_Contains( split->rightBounds, primitives[i].aabb ) ) + { + return false; + } + } + + return true; +} + +#endif + +static int b3BuildRecursive( b3Array( b3MeshNode ) * nodes, int count, b3Primitive* primitives, b3Primitive* base, + bool useMedianSplit, int* height ) +{ + if ( count > B3_DESIRED_TRIANGLES_PER_LEAF ) + { + // Try to split the input set using the SAH + b3Split split; + if ( useMedianSplit ) + { + split = b3SplitMedian( count, primitives ); + } + else + { + split = b3SplitBinnedSah( count, primitives ); + } + + if ( split.axis < 0 ) + { + if ( count > B3_MAXIMUM_TRIANGLES_PER_LEAF ) + { + // Re-split. This is a less optimal split and can create more false positives! + split = b3SplitHalf( count, primitives ); + } + else + { + b3AABB bounds = B3_BOUNDS3_EMPTY; + for ( int i = 0; i < count; ++i ) + { + bounds = b3AABB_Union( bounds, primitives[i].aabb ); + } + + // We have only a few triangles left. Create a leaf. + int index = b3Array_AddIndex( *nodes ); + b3StoreLeaf( &nodes->data[index], &bounds, count, (int)( primitives - base ) ); + + return index; + } + } + B3_VALIDATE( b3ValidateSplit( count, primitives, &split ) ); + + // Allocate node and recurse + int index = b3Array_AddIndex( *nodes ); + int heightLeft = 0, heightRight = 0; + int leftIndex = b3BuildRecursive( nodes, split.index, primitives, base, useMedianSplit, &heightLeft ); + int rightIndex = + b3BuildRecursive( nodes, count - split.index, primitives + split.index, base, useMedianSplit, &heightRight ); + + *height = b3MaxInt( heightLeft, heightRight ) + 1; + + B3_UNUSED( leftIndex ); + B3_ASSERT( leftIndex - index == 1 && rightIndex - index > 1 ); + + b3AABB aabb = b3AABB_Union( split.leftBounds, split.rightBounds ); + b3MeshNode* node = b3Array_Get( *nodes, index ); + node->data.asNode.axis = split.axis; + node->data.asNode.childOffset = rightIndex - index; + node->lowerBound = aabb.lowerBound; + node->upperBound = aabb.upperBound; + // triangleOffset is leaf-only, but lives outside the union — zero it so mesh->hash is deterministic + node->triangleOffset = 0; + + return index; + } + + b3AABB aabb = B3_BOUNDS3_EMPTY; + for ( int i = 0; i < count; ++i ) + { + aabb = b3AABB_Union( aabb, primitives[i].aabb ); + } + + int index = b3Array_AddIndex( *nodes ); + b3StoreLeaf( &nodes->data[index], &aabb, count, (int)( primitives - base ) ); + + *height = 1; + + return index; +} + +static bool b3SortMeshTriangles( b3MeshData* mesh ) +{ + b3MeshTriangle* triangles = b3GetMeshTrianglesWrite( mesh ); + uint8_t* materialIndices = b3GetMeshMaterialIndicesWrite( mesh ); + + // Sort triangles in depth-first-order + int offset = 0; + b3Array( b3MeshTriangle ) tempTriangles; + b3Array_CreateN( tempTriangles, mesh->triangleCount ); + + b3Array( uint8_t ) tempMaterialIndices; + b3Array_CreateN( tempMaterialIndices, mesh->triangleCount ); + + int count = 0; + b3MeshNode* stack[B3_MESH_STACK_SIZE]; + stack[count++] = b3GetRootWrite( mesh ); + + while ( count > 0 ) + { + b3MeshNode* node = stack[--count]; + + if ( b3IsLeaf( node ) == false ) + { + if ( count >= B3_MESH_STACK_SIZE - 2 ) + { + return false; + } + + stack[count++] = b3GetRightChildWrite( node ); + stack[count++] = b3GetLeftChildWrite( node ); + } + else + { + int triangleCount = node->data.asLeaf.triangleCount; + int triangleOffset = node->triangleOffset; + + for ( int triangle = 0; triangle < triangleCount; ++triangle ) + { + int index = triangleOffset + triangle; + b3Array_Push( tempTriangles, triangles[index] ); + b3Array_Push( tempMaterialIndices, materialIndices[index] ); + } + + node->triangleOffset = offset; + offset += triangleCount; + } + } + + B3_ASSERT( offset == tempTriangles.count ); + B3_ASSERT( tempTriangles.count == mesh->triangleCount ); + B3_ASSERT( tempMaterialIndices.count == mesh->triangleCount ); + + // Copy sorted triangle array back to tree + memcpy( triangles, tempTriangles.data, mesh->triangleCount * sizeof( b3MeshTriangle ) ); + memcpy( materialIndices, tempMaterialIndices.data, mesh->triangleCount * sizeof( uint8_t ) ); + + b3Array_Destroy( tempTriangles ); + b3Array_Destroy( tempMaterialIndices ); + + return true; +} + +typedef struct +{ + int vertex1; + int vertex2; + int triangle1; + int triangle2; + uint16_t triangleCount; + + // The index of an edge within the parent triangle: 0, 1, or 2. 0xFF is unset + uint8_t triangleEdgeIndex1; + uint8_t triangleEdgeIndex2; +} b3MeshEdge; + +#define NAME b3EdgeMap +#define KEY_TY uint64_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" + +#if 0 +// todo this is for testing other hash tables +struct b3TestHash +{ + size_t operator()( uint64_t key ) const + { + key ^= key >> 23; + key *= 0x2127599bf4325c37ull; + key ^= key >> 47; + return (size_t)key; + } +}; +#endif + +// Results for MeshCreationBenchmark +// eastl::hash_map : 4.9703 ms and 5.1445ms with FastHash function +// std::unordered_map : 4.8755 ms and 4.4780ms with FastHash function +// verstable : 3.3968 ms with default FastHash +// no edge identification : 1.7396 ms +static void b3IdentifyEdges( b3MeshData* mesh ) +{ + b3MeshTriangle* triangles = b3GetMeshTrianglesWrite( mesh ); + const b3Vec3* vertices = b3GetMeshVertices( mesh ); + uint8_t* flags = b3GetMeshFlagsWrite( mesh ); + + int triangleCount = mesh->triangleCount; + int edgeCount = 3 * triangleCount; + b3MeshEdge* edges = B3_ALLOC( b3MeshEdge, edgeCount ); + b3Vec3* normals = B3_ALLOC( b3Vec3, triangleCount ); + + for ( int i = 0; i < triangleCount; ++i ) + { + b3MeshTriangle* triangle = triangles + i; + int i1 = triangle->index1; + int i2 = triangle->index2; + int i3 = triangle->index3; + + edges[3 * i + 0].vertex1 = b3MinInt( i1, i2 ); + edges[3 * i + 0].vertex2 = b3MaxInt( i1, i2 ); + edges[3 * i + 0].triangle1 = i; + edges[3 * i + 0].triangle2 = B3_NULL_INDEX; + edges[3 * i + 0].triangleEdgeIndex1 = 0; + edges[3 * i + 0].triangleEdgeIndex2 = 0xFF; + edges[3 * i + 0].triangleCount = 1; + + edges[3 * i + 1].vertex1 = b3MinInt( i2, i3 ); + edges[3 * i + 1].vertex2 = b3MaxInt( i2, i3 ); + edges[3 * i + 1].triangle1 = i; + edges[3 * i + 1].triangle2 = B3_NULL_INDEX; + edges[3 * i + 1].triangleEdgeIndex1 = 1; + edges[3 * i + 1].triangleEdgeIndex2 = 0xFF; + edges[3 * i + 1].triangleCount = 1; + + edges[3 * i + 2].vertex1 = b3MinInt( i3, i1 ); + edges[3 * i + 2].vertex2 = b3MaxInt( i3, i1 ); + edges[3 * i + 2].triangle1 = i; + edges[3 * i + 2].triangle2 = B3_NULL_INDEX; + edges[3 * i + 2].triangleEdgeIndex1 = 2; + edges[3 * i + 2].triangleEdgeIndex2 = 0xFF; + edges[3 * i + 2].triangleCount = 1; + + b3Vec3 v1 = vertices[i1]; + b3Vec3 v2 = vertices[i2]; + b3Vec3 v3 = vertices[i3]; + + b3Vec3 e1 = b3Sub( v2, v1 ); + b3Vec3 e2 = b3Sub( v3, v1 ); + b3Vec3 n = b3Cross( e1, e2 ); + + normals[i] = b3Normalize( n ); + } + + b3EdgeMap map; + b3EdgeMap_init( &map ); + b3EdgeMap_reserve( &map, edgeCount ); + + uint64_t key = (uint64_t)edges[0].vertex1 << 32 | (uint64_t)edges[0].vertex2; + b3EdgeMap_insert( &map, key, 0 ); + + // Find unique edges and assign adjacency + for ( int i = 1; i < edgeCount; ++i ) + { + b3MeshEdge* edge = edges + i; + key = (uint64_t)edge->vertex1 << 32 | (uint64_t)edge->vertex2; + b3EdgeMap_itr itr = b3EdgeMap_get( &map, key ); + + if ( b3EdgeMap_is_end( itr ) ) + { + b3EdgeMap_insert( &map, key, i ); + } + else + { + int otherIndex = itr.data->val; + B3_ASSERT( otherIndex < i ); + + b3MeshEdge* base = edges + otherIndex; + if ( base->triangleCount == 1 ) + { + base->triangle2 = edge->triangle1; + base->triangleEdgeIndex2 = edge->triangleEdgeIndex1; + } + + base->triangleCount += 1; + } + } + + b3EdgeMap_cleanup( &map ); + + for ( int i = 0; i < edgeCount; ++i ) + { + b3MeshEdge* edge = edges + i; + if ( edge->triangleCount != 2 ) + { + continue; + } + + B3_ASSERT( edge->triangleEdgeIndex1 < 3 ); + B3_ASSERT( edge->triangleEdgeIndex2 < 3 ); + + b3MeshTriangle* triangle1 = triangles + edge->triangle1; + b3MeshTriangle* triangle2 = triangles + edge->triangle2; + uint8_t* flag1 = flags + edge->triangle1; + uint8_t* flag2 = flags + edge->triangle2; + + int j1 = triangle2->index1; + int j2 = triangle2->index2; + int j3 = triangle2->index3; + + int opposite = B3_NULL_INDEX; + + switch ( edge->triangleEdgeIndex2 ) + { + case 0: + opposite = j3; + break; + + case 1: + opposite = j1; + break; + + case 2: + opposite = j2; + break; + + default: + B3_ASSERT( false ); + } + + int i1 = triangle1->index1; + int i2 = triangle1->index2; + int i3 = triangle1->index3; + + b3Vec3 v1 = vertices[i1]; + b3Vec3 v2 = vertices[i2]; + b3Vec3 v3 = vertices[i3]; + b3Vec3 p = vertices[opposite]; + + float cos5Deg = 0.9962f; + float signedVolume = b3SignedVolume( v1, v2, v3, p ); + b3Vec3 n1 = normals[edge->triangle1]; + b3Vec3 n2 = normals[edge->triangle2]; + float cosAngle = b3Dot( n1, n2 ); + if ( signedVolume > 0.0f || cosAngle > cos5Deg ) + { + int edgeFlags[3] = { b3_concaveEdge1, b3_concaveEdge2, b3_concaveEdge3 }; + *flag1 |= edgeFlags[edge->triangleEdgeIndex1]; + *flag2 |= edgeFlags[edge->triangleEdgeIndex2]; + } + + if ( signedVolume < 0.0f || cosAngle > cos5Deg ) + { + int edgeFlags[3] = { b3_inverseConcaveEdge1, b3_inverseConcaveEdge2, b3_inverseConcaveEdge3 }; + *flag1 |= edgeFlags[edge->triangleEdgeIndex1]; + *flag2 |= edgeFlags[edge->triangleEdgeIndex2]; + } + } + + B3_FREE( normals, b3Vec3, triangleCount ); + B3_FREE( edges, b3MeshEdge, edgeCount ); +} + +b3MeshData* b3CreateGridMesh( int xCount, int zCount, float cellWidth, int materialCount, bool identifyEdges ) +{ + B3_ASSERT( 0 <= materialCount && materialCount <= UINT8_MAX ); + + // Create vertices + int vertexCount = ( xCount + 1 ) * ( zCount + 1 ); + + b3Array( b3Vec3 ) vertices = { 0 }; + b3Array_Resize( vertices, vertexCount ); + int index = 0; + + float xWidth = cellWidth * xCount; + float zWidth = cellWidth * zCount; + + float x = -0.5f * xWidth; + for ( int ix = 0; ix <= xCount; ++ix ) + { + float z = -0.5f * zWidth; + for ( int iz = 0; iz <= zCount; ++iz ) + { + vertices.data[index] = (b3Vec3){ x, 0.0f, z }; + z += cellWidth; + index += 1; + } + x += cellWidth; + } + B3_ASSERT( index == vertexCount ); + + // Triangles + int triangleCount = 2 * xCount * zCount; + + b3Array( int ) indices = { 0 }; + b3Array_Resize( indices, 3 * triangleCount ); + + b3Array( uint8_t ) materialIndices = { 0 }; + b3Array_Resize( materialIndices, triangleCount ); + + int materialIndex = 0; + index = 0; + for ( int ix = 0; ix < xCount; ++ix ) + { + for ( int iz = 0; iz < zCount; ++iz ) + { + int index1 = iz + ( zCount + 1 ) * ix; + int index2 = index1 + 1; + int index3 = index2 + ( zCount + 1 ); + int index4 = index3 - 1; + + B3_ASSERT( index1 < vertexCount ); + B3_ASSERT( index2 < vertexCount ); + B3_ASSERT( index3 < vertexCount ); + B3_ASSERT( index4 < vertexCount ); + + indices.data[index + 0] = index1; + indices.data[index + 1] = index2; + indices.data[index + 2] = index3; + + indices.data[index + 3] = index3; + indices.data[index + 4] = index4; + indices.data[index + 5] = index1; + + if ( materialCount > 0 ) + { + materialIndices.data[2 * materialIndex + 0] = (uint8_t)( materialIndex % materialCount ); + materialIndices.data[2 * materialIndex + 1] = (uint8_t)( materialIndex % materialCount ); + } + + materialIndex += 1; + index += 6; + } + } + B3_ASSERT( index == 3 * triangleCount ); + + b3MeshDef def = { 0 }; + def.vertexCount = vertices.count; + def.vertices = vertices.data; + def.triangleCount = indices.count / 3; + def.indices = indices.data; + def.materialIndices = materialCount > 0 ? materialIndices.data : NULL; + def.useMedianSplit = true; + def.identifyEdges = identifyEdges; + + b3MeshData* meshData = b3CreateMesh( &def, NULL, 0 ); + + b3Array_Destroy( indices ); + b3Array_Destroy( vertices ); + b3Array_Destroy( materialIndices ); + + return meshData; +} + +b3MeshData* b3CreateWaveMesh( int xCount, int zCount, float cellWidth, float amplitude, float rowFrequency, + float columnFrequency ) +{ + // Create vertices + int vertexCount = ( xCount + 1 ) * ( zCount + 1 ); + + b3Array( b3Vec3 ) vertices = { 0 }; + b3Array_Resize( vertices, vertexCount ); + int index = 0; + + float xWidth = cellWidth * xCount; + float zWidth = cellWidth * zCount; + + float omegaZ = 2.0f * B3_PI * rowFrequency * cellWidth; + float omegaX = 2.0f * B3_PI * columnFrequency * cellWidth; + + float x = -0.5f * xWidth; + for ( int ix = 0; ix <= xCount; ++ix ) + { + float rowHeight = sinf( omegaX * ix ); + + float z = -0.5f * zWidth; + for ( int iz = 0; iz <= zCount; ++iz ) + { + float columnHeight = sinf( omegaZ * iz ); + + float y = amplitude * rowHeight * columnHeight; + vertices.data[index] = (b3Vec3){ x, y, z }; + z += cellWidth; + index += 1; + } + x += cellWidth; + } + B3_ASSERT( index == vertexCount ); + + // Triangles + int triangleCount = 2 * xCount * zCount; + + b3Array( int ) indices = { 0 }; + b3Array_Resize( indices, 3 * triangleCount ); + + index = 0; + for ( int ix = 0; ix < xCount; ++ix ) + { + for ( int iz = 0; iz < zCount; ++iz ) + { + int index1 = iz + ( zCount + 1 ) * ix; + int index2 = index1 + 1; + int index3 = index2 + ( zCount + 1 ); + int index4 = index3 - 1; + + B3_ASSERT( index1 < vertexCount ); + B3_ASSERT( index2 < vertexCount ); + B3_ASSERT( index3 < vertexCount ); + B3_ASSERT( index4 < vertexCount ); + + indices.data[index + 0] = index1; + indices.data[index + 1] = index2; + indices.data[index + 2] = index3; + + indices.data[index + 3] = index3; + indices.data[index + 4] = index4; + indices.data[index + 5] = index1; + + index += 6; + } + } + B3_ASSERT( index == 3 * triangleCount ); + + b3MeshDef def = { 0 }; + def.vertexCount = vertices.count; + def.vertices = vertices.data; + def.triangleCount = indices.count / 3; + def.indices = indices.data; + def.useMedianSplit = true; + def.identifyEdges = true; + + b3MeshData* meshData = b3CreateMesh( &def, NULL, 0 ); + + b3Array_Destroy( indices ); + b3Array_Destroy( vertices ); + + return meshData; +} + +b3MeshData* b3CreateTorusMesh( int radialResolution, int tubularResolution, float radius, float thickness ) +{ + // Create vertices + b3Array( b3Vec3 ) vertices = { 0 }; + + for ( int radialIndex = 0; radialIndex < radialResolution; radialIndex++ ) + { + for ( int tubularIndex = 0; tubularIndex < tubularResolution; tubularIndex++ ) + { + float u = (float)tubularIndex / tubularResolution * B3_TWO_PI; + float v = (float)radialIndex / radialResolution * B3_TWO_PI; + + float x = ( radius + thickness * b3Cos( v ) ) * b3Cos( u ); + float y = ( radius + thickness * b3Cos( v ) ) * b3Sin( u ); + float z = thickness * b3Sin( v ); + + b3Vec3 vertex = { x, y, z }; + b3Array_Push( vertices, vertex ); + } + } + + // Triangles + b3Array( int ) indices = { 0 }; + for ( int radialIndex1 = 0; radialIndex1 < radialResolution; radialIndex1++ ) + { + int radialIndex2 = ( radialIndex1 + 1 ) % radialResolution; + for ( int tubularIndex1 = 0; tubularIndex1 < tubularResolution; tubularIndex1++ ) + { + int tubularIndex2 = ( tubularIndex1 + 1 ) % tubularResolution; + int index1 = radialIndex1 * tubularResolution + tubularIndex1; + int index2 = radialIndex1 * tubularResolution + tubularIndex2; + int index3 = radialIndex2 * tubularResolution + tubularIndex2; + int index4 = radialIndex2 * tubularResolution + tubularIndex1; + + b3Array_Push( indices, index1 ); + b3Array_Push( indices, index2 ); + b3Array_Push( indices, index3 ); + + b3Array_Push( indices, index3 ); + b3Array_Push( indices, index4 ); + b3Array_Push( indices, index1 ); + } + } + + b3MeshDef def = { 0 }; + def.vertexCount = vertices.count; + def.vertices = vertices.data; + def.triangleCount = indices.count / 3; + def.indices = indices.data; + def.useMedianSplit = false; + def.identifyEdges = true; + + b3MeshData* meshData = b3CreateMesh( &def, NULL, 0 ); + + b3Array_Destroy( vertices ); + b3Array_Destroy( indices ); + return meshData; +} + +b3MeshData* b3CreateBoxMesh( b3Vec3 center, b3Vec3 extent, bool identifyEdges ) +{ + float x = extent.x; + float y = extent.y; + float z = extent.z; + b3Vec3 vertices[] = { + { x, y, z }, { -x, y, z }, { -x, -y, z }, { x, -y, z }, { x, y, -z }, { -x, y, -z }, { -x, -y, -z }, { x, -y, -z }, + }; + + for ( int i = 0; i < 8; ++i ) + { + vertices[i] = b3Add( vertices[i], center ); + } + + int indices[] = { + 0, 1, 3, 1, 2, 3, // front + 0, 4, 1, 1, 4, 5, // top + 0, 3, 7, 4, 0, 7, // right + 4, 7, 5, 6, 5, 7, // back + 1, 5, 2, 6, 2, 5, // left + 3, 2, 7, 6, 7, 2, // bottom + }; + + b3MeshDef def = { 0 }; + def.vertexCount = 8; + def.vertices = vertices; + def.triangleCount = 12; + def.indices = indices; + def.useMedianSplit = false; + def.identifyEdges = identifyEdges; + + return b3CreateMesh( &def, NULL, 0 ); +} + +b3MeshData* b3CreateHollowBoxMesh(b3Vec3 center, b3Vec3 extent) +{ + float x = extent.x; + float y = extent.y; + float z = extent.z; + b3Vec3 vertices[] = { + { x, y, z }, { -x, y, z }, { -x, -y, z }, { x, -y, z }, { x, y, -z }, { -x, y, -z }, { -x, -y, -z }, { x, -y, -z }, + }; + + for ( int i = 0; i < 8; ++i ) + { + vertices[i] = b3Add( vertices[i], center ); + } + + int indices[] = { + 3, 1, 0, 3, 2, 1, // front + 1, 4, 0, 5, 4, 1, // top + 7, 3, 0, 7, 0, 4, // right + 5, 7, 4, 7, 5, 6, // back + 2, 5, 1, 5, 2, 6, // left + 7, 2, 3, 2, 7, 6, // bottom + }; + + b3MeshDef def = { 0 }; + def.vertexCount = 8; + def.vertices = vertices; + def.triangleCount = 12; + def.indices = indices; + def.useMedianSplit = false; + def.identifyEdges = true; + + return b3CreateMesh( &def, NULL, 0 ); +} + +b3MeshData* b3CreatePlatformMesh( b3Vec3 center, float height, float topWidth, float bottomWidth ) +{ + float hb = 0.5f * bottomWidth; + float ht = 0.5f * topWidth; + float hy = 0.5f * height; + b3Vec3 vertices[] = { + { ht, hy, ht }, { -ht, hy, ht }, { -hb, -hy, hb }, { hb, -hy, hb }, + { ht, hy, -ht }, { -ht, hy, -ht }, { -hb, -hy, -hb }, { hb, -hy, -hb }, + }; + + for ( int i = 0; i < 8; ++i ) + { + vertices[i] = b3Add( vertices[i], center ); + } + + int indices[] = { + 0, 1, 3, 1, 2, 3, // front + 0, 4, 1, 1, 4, 5, // top + 0, 3, 7, 4, 0, 7, // right + 4, 7, 5, 6, 5, 7, // back + 1, 5, 2, 6, 2, 5, // left + 3, 2, 7, 6, 7, 2, // bottom + }; + + b3MeshDef def = { 0 }; + def.vertexCount = 8; + def.vertices = vertices; + def.triangleCount = 12; + def.indices = indices; + def.useMedianSplit = true; + def.identifyEdges = true; + + return b3CreateMesh( &def, NULL, 0 ); +} + +// todo this should fail if the mesh has a height greater than B3_MESH_STACK_SIZE +b3MeshData* b3CreateMesh( const b3MeshDef* def, int* degenerateTriangleIndices, int degenerateCapacity ) +{ + if ( def->vertexCount < 3 || def->vertices == NULL || def->triangleCount <= 0 || def->indices == NULL ) + { + return NULL; + } + + int triangleCount = def->triangleCount; + if ( triangleCount == 0 ) + { + return NULL; + } + + int vertexCount = def->vertexCount; + + b3AABB meshBounds = B3_BOUNDS3_EMPTY; + + // Clone indices and vertices to support welding + b3Array( int ) indices; + b3Array_CreateN( indices, 3 * triangleCount ); + + b3Array( b3Vec3 ) vertices; + b3Array_CreateN( vertices, vertexCount ); + + if ( def->weldVertices && def->weldTolerance > 0.0f ) + { + b3Array_Resize( vertices, vertexCount ); + b3Array_Resize( indices, 3 * triangleCount ); + b3WeldData data = { + .srcVertices = def->vertices, + .srcIndices = def->indices, + .dstVertices = vertices.data, + .dstIndices = indices.data, + .vertexCount = vertexCount, + .indexCount = 3 * triangleCount, + }; + vertices.count = b3WeldVertices( &data, def->weldTolerance ); + vertexCount = vertices.count; + B3_ASSERT( vertexCount <= def->vertexCount ); + } + else + { + b3Array_Append( vertices, def->vertices, vertexCount ); + b3Array_Append( indices, def->indices, 3 * triangleCount ); + } + + b3Array( b3Primitive ) primitives; + b3Array_CreateN( primitives, triangleCount ); + int degenerateCount = 0; + float minArea = 0.01f * B3_LINEAR_SLOP * B3_LINEAR_SLOP; + float surfaceArea = 0.0f; + int materialCount = 1; + + for ( int index = 0; index < triangleCount; ++index ) + { + int index1 = indices.data[3 * index + 0]; + int index2 = indices.data[3 * index + 1]; + int index3 = indices.data[3 * index + 2]; + + b3Vec3 vertex1 = vertices.data[index1]; + b3Vec3 vertex2 = vertices.data[index2]; + b3Vec3 vertex3 = vertices.data[index3]; + + b3Vec3 normal = b3Cross( b3Sub( vertex2, vertex1 ), b3Sub( vertex3, vertex1 ) ); + float area = 0.5f * b3Length( normal ); + + if ( area < minArea ) + { + // b3Log( "degenerate: %d %d %d\n", index1, index2, index3 ); + + if ( index1 != index2 && index1 != index3 && index2 != index3 ) + { + degenerateCount += 1; + if ( degenerateTriangleIndices != NULL && degenerateCount < degenerateCapacity ) + { + degenerateTriangleIndices[degenerateCount - 1] = index; + } + } + + continue; + } + + surfaceArea += area; + + b3AABB box = { + b3Min( vertex1, b3Min( vertex2, vertex3 ) ), + b3Max( vertex1, b3Max( vertex2, vertex3 ) ), + }; + + b3Vec3 center = b3AABB_Center( box ); + + b3Primitive primitive = { + .aabb = box, + .center = center, + .triangleIndex = index, + }; + b3Array_Push( primitives, primitive ); + + if ( def->materialIndices != NULL ) + { + materialCount = b3MaxInt( materialCount, def->materialIndices[index] + 1 ); + } + + meshBounds = b3AABB_Union( meshBounds, box ); + } + + // Update triangle count due to degenerates being skipped + triangleCount = primitives.count; + + if ( b3IsSaneAABB( meshBounds ) == false ) + { + b3Array_Destroy( primitives ); + return NULL; + } + + // Build the tree (this reorders the builder triangles) + b3Array( b3MeshNode ) tempNodes; + b3Array_CreateN( tempNodes, 2 * triangleCount - 1 ); + + int treeHeight = 0; + b3BuildRecursive( &tempNodes, triangleCount, primitives.data, primitives.data, def->useMedianSplit, &treeHeight ); + + // Allocate the mesh + size_t byteCount = b3AlignUp8( sizeof( b3MeshData ) ); + int nodeOffset = (int)byteCount; + byteCount += b3AlignUp8( tempNodes.count * sizeof( b3MeshNode ) ); + int vertexOffset = (int)byteCount; + byteCount += b3AlignUp8( vertexCount * sizeof( b3Vec3 ) ); + int triangleOffset = (int)byteCount; + byteCount += b3AlignUp8( triangleCount * sizeof( b3MeshTriangle ) ); + int materialIndicesOffset = (int)byteCount; + byteCount += b3AlignUp8( triangleCount * sizeof( uint8_t ) ); + int flagsOffset = (int)byteCount; + byteCount += b3AlignUp8( triangleCount * sizeof( uint8_t ) ); + + b3MeshData* mesh = b3Alloc( byteCount ); + + // zero initialize for determinism + memset( mesh, 0, byteCount ); + + mesh->version = B3_MESH_VERSION; + mesh->byteCount = (int)byteCount; + mesh->bounds = meshBounds; + mesh->surfaceArea = surfaceArea; + mesh->nodeCount = tempNodes.count; + mesh->treeHeight = treeHeight; + mesh->vertexCount = vertexCount; + mesh->triangleCount = triangleCount; + mesh->degenerateCount = degenerateCount; + mesh->nodeOffset = nodeOffset; + mesh->vertexOffset = vertexOffset; + mesh->triangleOffset = triangleOffset; + mesh->materialOffset = materialIndicesOffset; + mesh->materialCount = materialCount; + mesh->flagsOffset = flagsOffset; + + b3MeshNode* nodes = b3GetMeshNodesWrite( mesh ); + b3MeshTriangle* triangles = b3GetMeshTrianglesWrite( mesh ); + b3Vec3* meshVertices = b3GetMeshVerticesWrite( mesh ); + uint8_t* materialIndices = b3GetMeshMaterialIndicesWrite( mesh ); + uint8_t* flags = b3GetMeshFlagsWrite( mesh ); + + memcpy( nodes, tempNodes.data, tempNodes.count * sizeof( b3MeshNode ) ); + memcpy( meshVertices, vertices.data, vertexCount * sizeof( b3Vec3 ) ); + + for ( int index = 0; index < triangleCount; ++index ) + { + b3Primitive primitive = primitives.data[index]; + triangles[index].index1 = indices.data[3 * primitive.triangleIndex + 0]; + triangles[index].index2 = indices.data[3 * primitive.triangleIndex + 1]; + triangles[index].index3 = indices.data[3 * primitive.triangleIndex + 2]; + flags[index] = 0; + + // Copy material indices if they exist. Otherwise the material indices are all zeroes. + if ( def->materialIndices != NULL ) + { + uint8_t materialIndex = def->materialIndices[primitive.triangleIndex]; + materialIndices[index] = materialIndex; + } + } + + // Sort triangle in DFS order. Casts and volume queries will return sorted arrays. + // This also sorts material indices, but not the materials. + // This can fail if the BVH height is too large. + bool success = b3SortMeshTriangles( mesh ); + if ( success == false ) + { + b3Array_Destroy( tempNodes ); + b3Array_Destroy( primitives ); + return NULL; + } + + if ( def->identifyEdges ) + { + b3IdentifyEdges( mesh ); + } + + B3_VALIDATE( b3IsNonDegenerate( mesh, minArea ) ); + B3_VALIDATE( b3IsConsistent( mesh ) ); + + b3Array_Destroy( tempNodes ); + b3Array_Destroy( primitives ); + b3Array_Destroy( indices ); + b3Array_Destroy( vertices ); + + mesh->hash = 0; + mesh->hash = b3NonZeroHash( b3Hash( B3_HASH_INIT, (uint8_t*)mesh, mesh->byteCount ) ); + + return mesh; +} + +void b3DestroyMesh( b3MeshData* mesh ) +{ + b3Free( mesh, mesh->byteCount ); +} + +bool b3OverlapMesh( const b3Mesh* shape, b3Transform shapeTransform, const b3ShapeProxy* proxy ) +{ + B3_ASSERT( proxy->count > 0 ); + b3SimplexCache cache = { 0 }; + + b3Vec3 buffer[B3_MAX_SHAPE_CAST_POINTS]; + b3ShapeProxy localProxy = b3MakeLocalProxy( proxy, shapeTransform, buffer ); + b3AABB aabb = b3ComputeProxyAABB( &localProxy ); + + b3Vec3 meshScale = shape->scale; + + // Scale may have reflection so min/max may become invalid when unscaled + b3V32 scale = b3LoadV( &meshScale.x ); + b3V32 invScale = b3DivV( b3_oneV, scale ); + b3V32 temp1 = b3MulV( invScale, b3LoadV( &aabb.lowerBound.x ) ); + b3V32 temp2 = b3MulV( invScale, b3LoadV( &aabb.upperBound.x ) ); + b3V32 invScaledBoundsMin = b3MinV( temp1, temp2 ); + b3V32 invScaledBoundsMax = b3MaxV( temp1, temp2 ); + b3V32 invScaledBoundsCenter = b3MulV( b3_halfV, b3AddV( invScaledBoundsMin, invScaledBoundsMax ) ); + b3V32 invScaledBoundsExtent = b3SubV( invScaledBoundsMax, invScaledBoundsCenter ); + + b3DistanceInput input; + input.proxyB = localProxy; + input.transform = b3Transform_identity; + input.useRadii = true; + + int count = 0; + const b3MeshNode* stack[B3_MESH_STACK_SIZE]; + const b3MeshNode* node = b3GetRoot( shape->data ); + const b3MeshTriangle* triangles = b3GetMeshTriangles( shape->data ); + const b3Vec3* vertices = b3GetMeshVertices( shape->data ); + + while ( true ) + { + // Test node overlap in unscaled space + b3V32 nodeMin = b3LoadV( &node->lowerBound.x ); + b3V32 nodeMax = b3LoadV( &node->upperBound.x ); + if ( b3TestBoundsOverlap( nodeMin, nodeMax, invScaledBoundsMin, invScaledBoundsMax ) ) + { + if ( b3IsLeaf( node ) ) + { + int triangleCount = node->data.asLeaf.triangleCount; + int triangleOffset = node->triangleOffset; + + for ( int index = 0; index < triangleCount; ++index ) + { + int triangleIndex = triangleOffset + index; + b3MeshTriangle triangle = triangles[triangleIndex]; + + b3Vec3 vertex1 = vertices[triangle.index1]; + b3Vec3 vertex2 = vertices[triangle.index2]; + b3Vec3 vertex3 = vertices[triangle.index3]; + b3V32 v1 = b3LoadV( &vertex1.x ); + b3V32 v2 = b3LoadV( &vertex2.x ); + b3V32 v3 = b3LoadV( &vertex3.x ); + + // Bounding box overlap test in unscaled space + if ( b3TestBoundsTriangleOverlap( invScaledBoundsCenter, invScaledBoundsExtent, v1, v2, v3 ) ) + { + // Shape-triangle overlap test in scaled space. Winding order doesn't matter. + b3Vec3 triangleVertices[] = { b3Mul( meshScale, vertex1 ), b3Mul( meshScale, vertex2 ), + b3Mul( meshScale, vertex3 ) }; + input.proxyA = (b3ShapeProxy){ triangleVertices, 3, 0.0f }; + + // reset the cache + cache.count = 0; + + // get distance between triangle and query shape + b3DistanceOutput output = b3ShapeDistance( &input, &cache, NULL, 0 ); + + float tolerance = 0.1f * B3_LINEAR_SLOP; + if ( output.distance < tolerance ) + { + // overlap detected + return true; + } + } + } + } + else + { + // Recurse + B3_ASSERT( count <= B3_MESH_STACK_SIZE - 1 ); + stack[count++] = b3GetRightChild( node ); + node = b3GetLeftChild( node ); + + continue; + } + } + + if ( count == 0 ) + { + break; + } + node = stack[--count]; + } + + return false; +} + +b3AABB b3ComputeMeshAABB( const b3MeshData* shape, b3Transform transform, b3Vec3 scale ) +{ + b3Vec3 scaledLower = b3Mul( scale, shape->bounds.lowerBound ); + b3Vec3 scaledUpper = b3Mul( scale, shape->bounds.upperBound ); + b3AABB bounds = { b3Min( scaledLower, scaledUpper ), b3Max( scaledLower, scaledUpper ) }; + return b3AABB_Transform( transform, bounds ); +} + +b3CastOutput b3RayCastMesh( const b3Mesh* mesh, const b3RayCastInput* input ) +{ + const b3MeshData* data = mesh->data; + b3Vec3 meshScale = mesh->scale; + + b3CastOutput bestOutput = { 0 }; + bestOutput.fraction = input->maxFraction; + bestOutput.triangleIndex = B3_NULL_INDEX; + + b3V32 lambda = b3SplatV( input->maxFraction ); + + b3V32 rayStart = b3LoadV( &input->origin.x ); + b3V32 rayDelta = b3LoadV( &input->translation.x ); + + b3V32 scale = b3LoadV( &meshScale.x ); + b3V32 invScale = b3DivV( b3_oneV, scale ); + bool clockwise = meshScale.x * meshScale.y * meshScale.z < 0.0f; + + // Use the inverse scaled ray for traversal of the BVH + b3V32 invScaledRayStart = b3MulV( invScale, rayStart ); + b3V32 invScaledRayDelta = b3MulV( invScale, rayDelta ); + b3V32 invScaledRayEnd = b3AddV( invScaledRayStart, b3MulV( lambda, invScaledRayDelta ) ); + b3V32 invScaledRayMin = b3MinV( invScaledRayStart, invScaledRayEnd ); + b3V32 invScaledRayMax = b3MaxV( invScaledRayStart, invScaledRayEnd ); + + int count = 0; + const b3MeshNode* stack[B3_MESH_STACK_SIZE]; + const b3MeshNode* node = b3GetRoot( data ); + const b3MeshTriangle* triangles = b3GetMeshTriangles( data ); + const b3Vec3* vertices = b3GetMeshVertices( data ); + const uint8_t* materialIndices = b3GetMeshMaterialIndices( data ); + + while ( true ) + { + // Test node/ray overlap using SAT + b3V32 nodeMin = b3LoadV( &node->lowerBound.x ); + b3V32 nodeMax = b3LoadV( &node->upperBound.x ); + if ( b3TestBoundsOverlap( nodeMin, nodeMax, invScaledRayMin, invScaledRayMax ) && + b3TestBoundsRayOverlap( nodeMin, nodeMax, invScaledRayStart, invScaledRayDelta ) ) + { + // SAT: The node and ray overlap - process leaf node or recurse + if ( b3IsLeaf( node ) ) + { + int triangleCount = node->data.asLeaf.triangleCount; + int triangleOffset = node->triangleOffset; + + for ( int index = 0; index < triangleCount; ++index ) + { + int triangleIndex = triangleOffset + index; + b3MeshTriangle triangle = triangles[triangleIndex]; + + // Collide ray with triangle in scaled space + b3Vec3 vertex1 = b3Mul( meshScale, vertices[triangle.index1] ); + b3Vec3 vertex2, vertex3; + + // The CPU should predict this branch + if ( clockwise ) + { + vertex2 = b3Mul( meshScale, vertices[triangle.index3] ); + vertex3 = b3Mul( meshScale, vertices[triangle.index2] ); + } + else + { + vertex2 = b3Mul( meshScale, vertices[triangle.index2] ); + vertex3 = b3Mul( meshScale, vertices[triangle.index3] ); + } + + // Collide ray with triangle in scaled space + b3V32 v1 = b3LoadV( &vertex1.x ); + b3V32 v2 = b3LoadV( &vertex2.x ); + b3V32 v3 = b3LoadV( &vertex3.x ); + + float alpha = b3IntersectRayTriangle( rayStart, rayDelta, v1, v2, v3 ); + B3_ASSERT( 0 <= alpha && alpha <= 1.0f ); + + if ( alpha < bestOutput.fraction ) + { + b3Vec3 edge1 = b3Sub( vertex2, vertex1 ); + b3Vec3 edge2 = b3Sub( vertex3, vertex1 ); + bestOutput.normal = b3Normalize( b3Cross( edge1, edge2 ) ); + bestOutput.point = b3Add( input->origin, b3MulSV( alpha, input->translation ) ); + bestOutput.fraction = alpha; + bestOutput.triangleIndex = triangleIndex; + bestOutput.materialIndex = materialIndices[triangleIndex]; + bestOutput.hit = true; + + // Update ray bounds in unscaled space + lambda = b3SplatV( alpha ); + invScaledRayEnd = b3AddV( invScaledRayStart, b3MulV( lambda, invScaledRayDelta ) ); + invScaledRayMin = b3MinV( invScaledRayStart, invScaledRayEnd ); + invScaledRayMax = b3MaxV( invScaledRayStart, invScaledRayEnd ); + } + } + } + else + { + // Determine traversal order (front -> back) and recurse + int axis = node->data.asNode.axis; + if ( b3GetV( invScaledRayDelta, axis ) > 0.0f ) + { + B3_ASSERT( count <= B3_MESH_STACK_SIZE - 1 ); + stack[count++] = b3GetRightChild( node ); + node = b3GetLeftChild( node ); + } + else + { + B3_ASSERT( count <= B3_MESH_STACK_SIZE - 1 ); + stack[count++] = b3GetLeftChild( node ); + node = b3GetRightChild( node ); + } + + continue; + } + } + + if ( count == 0 ) + { + break; + } + node = stack[--count]; + } + + return bestOutput; +} + +b3CastOutput b3ShapeCastMesh( const b3Mesh* mesh, const b3ShapeCastInput* input ) +{ + const b3MeshData* data = mesh->data; + b3Vec3 meshScale = mesh->scale; + + b3CastOutput bestOutput = { 0 }; + bestOutput.fraction = input->maxFraction; + bestOutput.triangleIndex = B3_NULL_INDEX; + + b3V32 lambda = b3SplatV( input->maxFraction ); + + b3AABB shapeBounds = b3MakeAABB( input->proxy.points, input->proxy.count, input->proxy.radius ); + b3Vec3 center = b3AABB_Center( shapeBounds ); + b3Vec3 extents = b3AABB_Extents( shapeBounds ); + b3V32 shapeExtent = b3LoadV( &extents.x ); + + b3V32 rayStart = b3LoadV( ¢er.x ); + b3V32 rayDelta = b3LoadV( &input->translation.x ); + b3V32 rayEnd = b3AddV( rayStart, b3MulV( lambda, rayDelta ) ); + b3V32 rayMin = b3MinV( rayStart, rayEnd ); + b3V32 rayMax = b3MaxV( rayStart, rayEnd ); + + b3V32 scale = b3LoadV( &meshScale.x ); + b3V32 invScale = b3DivV( b3_oneV, scale ); + b3V32 absInvScale = b3AbsV( invScale ); + bool clockwise = meshScale.x * meshScale.y * meshScale.z < 0.0f; + + // Use the inverse scaled shape cast for traversal of the BVH + b3V32 invScaledRayStart = b3MulV( invScale, rayStart ); + b3V32 invScaledRayDelta = b3MulV( invScale, rayDelta ); + b3V32 invScaledRayEnd = b3AddV( invScaledRayStart, b3MulV( lambda, invScaledRayDelta ) ); + b3V32 invScaledRayMin = b3MinV( invScaledRayStart, invScaledRayEnd ); + b3V32 invScaledRayMax = b3MaxV( invScaledRayStart, invScaledRayEnd ); + b3V32 invScaledShapeExtent = b3MulV( absInvScale, shapeExtent ); + + int count = 0; + const b3MeshNode* stack[B3_MESH_STACK_SIZE]; + const b3MeshNode* node = b3GetRoot( data ); + const b3MeshTriangle* triangles = b3GetMeshTriangles( data ); + const b3Vec3* vertices = b3GetMeshVertices( data ); + const uint8_t* materialIndices = b3GetMeshMaterialIndices( data ); + + while ( true ) + { + // Test node/ray overlap using SAT in unscaled space + b3V32 nodeMin = b3SubV( b3LoadV( &node->lowerBound.x ), invScaledShapeExtent ); + b3V32 nodeMax = b3AddV( b3LoadV( &node->upperBound.x ), invScaledShapeExtent ); + + if ( b3TestBoundsOverlap( nodeMin, nodeMax, invScaledRayMin, invScaledRayMax ) && + b3TestBoundsRayOverlap( nodeMin, nodeMax, invScaledRayStart, invScaledRayDelta ) ) + { + // SAT: The node and ray overlap - process leaf node or recurse + if ( b3IsLeaf( node ) ) + { + int triangleCount = node->data.asLeaf.triangleCount; + int triangleOffset = node->triangleOffset; + + for ( int index = 0; index < triangleCount; ++index ) + { + int triangleIndex = triangleOffset + index; + b3MeshTriangle triangle = triangles[triangleIndex]; + + // Collide ray with triangle in scaled space + b3Vec3 vertex1 = b3Mul( meshScale, vertices[triangle.index1] ); + b3Vec3 vertex2, vertex3; + + // The CPU should predict this branch + if ( clockwise ) + { + vertex2 = b3Mul( meshScale, vertices[triangle.index3] ); + vertex3 = b3Mul( meshScale, vertices[triangle.index2] ); + } + else + { + vertex2 = b3Mul( meshScale, vertices[triangle.index2] ); + vertex3 = b3Mul( meshScale, vertices[triangle.index3] ); + } + + b3V32 v1 = b3LoadV( &vertex1.x ); + b3V32 v2 = b3LoadV( &vertex2.x ); + b3V32 v3 = b3LoadV( &vertex3.x ); + + b3V32 triangleMin = b3SubV( b3MinV( v1, b3MinV( v2, v3 ) ), shapeExtent ); + b3V32 triangleMax = b3AddV( b3MaxV( v1, b3MaxV( v2, v3 ) ), shapeExtent ); + + // Test triangle-ray overlap in scaled space + if ( b3TestBoundsOverlap( triangleMin, triangleMax, rayMin, rayMax ) ) + { + // Collide shape with triangle in scaled space + b3Vec3 origin = vertex1; + b3Vec3 triangleVertices[] = { b3Vec3_zero, b3Sub( vertex2, origin ), b3Sub( vertex3, origin ) }; + b3Transform shiftedOrigin = { b3Neg( origin ), b3Quat_identity }; + + b3ShapeCastPairInput pairInput; + pairInput.proxyA = (b3ShapeProxy){ triangleVertices, 3, 0.0f }; + pairInput.proxyB = input->proxy; + pairInput.transform = shiftedOrigin; + pairInput.maxFraction = bestOutput.fraction; + pairInput.translationB = input->translation; + pairInput.canEncroach = input->canEncroach; + + b3CastOutput pairOutput = b3ShapeCast( &pairInput ); + + if ( pairOutput.hit ) + { + pairOutput.point = b3Add( pairOutput.point, origin ); + + bestOutput = pairOutput; + bestOutput.triangleIndex = triangleIndex; + bestOutput.materialIndex = materialIndices[triangleIndex]; + + // Update ray bounds in scaled space + lambda = b3SplatV( pairOutput.fraction ); + rayEnd = b3AddV( rayStart, b3MulV( lambda, rayDelta ) ); + rayMin = b3MinV( rayStart, rayEnd ); + rayMax = b3MaxV( rayStart, rayEnd ); + + // Ray bounds in unscaled space + invScaledRayEnd = b3AddV( invScaledRayStart, b3MulV( lambda, invScaledRayDelta ) ); + invScaledRayMin = b3MinV( invScaledRayStart, invScaledRayEnd ); + invScaledRayMax = b3MaxV( invScaledRayStart, invScaledRayEnd ); + } + } + } + } + else + { + // Determine traversal order (front -> back) and recurse + int axis = node->data.asNode.axis; + if ( b3GetV( invScaledRayDelta, axis ) > 0.0f ) + { + B3_ASSERT( count <= B3_MESH_STACK_SIZE - 1 ); + stack[count++] = b3GetRightChild( node ); + node = b3GetLeftChild( node ); + } + else + { + B3_ASSERT( count <= B3_MESH_STACK_SIZE - 1 ); + stack[count++] = b3GetLeftChild( node ); + node = b3GetRightChild( node ); + } + + continue; + } + } + + if ( count == 0 ) + { + break; + } + node = stack[--count]; + } + + return bestOutput; +} + +b3Triangle b3GetMeshTriangle( const b3Mesh* mesh, int triangleIndex ) +{ + B3_ASSERT( 0 <= triangleIndex && triangleIndex < mesh->data->triangleCount ); + + const b3MeshTriangle* triangles = b3GetMeshTriangles( mesh->data ); + const uint8_t* flags = b3GetMeshFlags( mesh->data ); + const b3Vec3* vertices = b3GetMeshVertices( mesh->data ); + + b3Triangle result; + b3MeshTriangle triangle = triangles[triangleIndex]; + uint8_t triangleFlags = flags[triangleIndex]; + + b3Vec3 scale = mesh->scale; + + result.vertices[0] = b3Mul( scale, vertices[triangle.index1] ); + result.i1 = triangle.index1; + + if ( scale.x * scale.y * scale.z < 0.0f ) + { + result.vertices[1] = b3Mul( scale, vertices[triangle.index3] ); + result.vertices[2] = b3Mul( scale, vertices[triangle.index2] ); + + result.i2 = triangle.index3; + result.i3 = triangle.index2; + + // mesh is inverted, so concave edges are now convex + result.flags = 0; + result.flags |= ( triangleFlags & b3_inverseConcaveEdge1 ) ? b3_concaveEdge1 : 0; + result.flags |= ( triangleFlags & b3_inverseConcaveEdge2 ) ? b3_concaveEdge2 : 0; + result.flags |= ( triangleFlags & b3_inverseConcaveEdge3 ) ? b3_concaveEdge3 : 0; + } + else + { + result.vertices[1] = b3Mul( scale, vertices[triangle.index2] ); + result.vertices[2] = b3Mul( scale, vertices[triangle.index3] ); + + result.i2 = triangle.index2; + result.i3 = triangle.index3; + result.flags = triangleFlags; + } + + return result; +} + +int b3CollideMoverAndMesh( b3PlaneResult* planes, int capacity, const b3Mesh* shape, const b3Capsule* mover ) +{ + if ( capacity == 0 ) + { + return 0; + } + + b3DistanceInput distanceInput = { 0 }; + distanceInput.proxyB = (b3ShapeProxy){ &mover->center1, 2, 0.0f }; + distanceInput.transform = b3Transform_identity; + distanceInput.useRadii = false; + + b3SimplexCache cache = { 0 }; + float radius = mover->radius; + + b3V32 center1 = b3LoadV( &mover->center1.x ); + b3V32 center2 = b3LoadV( &mover->center2.x ); + b3V32 r = b3SplatV( radius ); + b3V32 boundsMin = b3SubV( b3MinV( center1, center2 ), r ); + b3V32 boundsMax = b3AddV( b3MaxV( center1, center2 ), r ); + + // Scale may have reflection so min/max may become invalid when unscaled + b3Vec3 meshScale = shape->scale; + b3V32 scale = b3LoadV( &meshScale.x ); + b3V32 invScale = b3DivV( b3_oneV, scale ); + b3V32 temp1 = b3MulV( invScale, boundsMin ); + b3V32 temp2 = b3MulV( invScale, boundsMax ); + b3V32 invScaledBoundsMin = b3MinV( temp1, temp2 ); + b3V32 invScaledBoundsMax = b3MaxV( temp1, temp2 ); + b3V32 invScaledBoundsCenter = b3MulV( b3_halfV, b3AddV( invScaledBoundsMin, invScaledBoundsMax ) ); + b3V32 invScaledBoundsExtent = b3SubV( invScaledBoundsMax, invScaledBoundsCenter ); + + int count = 0; + const b3MeshNode* stack[B3_MESH_STACK_SIZE]; + const b3MeshNode* node = b3GetRoot( shape->data ); + const b3MeshTriangle* triangles = b3GetMeshTriangles( shape->data ); + const b3Vec3* vertices = b3GetMeshVertices( shape->data ); + + int planeCount = 0; + while ( planeCount < capacity ) + { + // Test node overlap in unscaled space + b3V32 nodeMin = b3LoadV( &node->lowerBound.x ); + b3V32 nodeMax = b3LoadV( &node->upperBound.x ); + if ( b3TestBoundsOverlap( nodeMin, nodeMax, invScaledBoundsMin, invScaledBoundsMax ) ) + { + if ( b3IsLeaf( node ) ) + { + int triangleCount = node->data.asLeaf.triangleCount; + int triangleOffset = node->triangleOffset; + + for ( int index = 0; index < triangleCount; ++index ) + { + int triangleIndex = triangleOffset + index; + b3MeshTriangle triangle = triangles[triangleIndex]; + + b3Vec3 vertex1 = vertices[triangle.index1]; + b3Vec3 vertex2 = vertices[triangle.index2]; + b3Vec3 vertex3 = vertices[triangle.index3]; + b3V32 v1 = b3LoadV( &vertex1.x ); + b3V32 v2 = b3LoadV( &vertex2.x ); + b3V32 v3 = b3LoadV( &vertex3.x ); + + // Test triangle bounds overlap in unscaled space + if ( b3TestBoundsTriangleOverlap( invScaledBoundsCenter, invScaledBoundsExtent, v1, v2, v3 ) ) + { + // Compute shape distance in scaled space. Winding order doesn't matter. + // todo implement one-sided collision? + b3Vec3 triangleVertices[] = { b3Mul( meshScale, vertex1 ), b3Mul( meshScale, vertex2 ), + b3Mul( meshScale, vertex3 ) }; + distanceInput.proxyA = (b3ShapeProxy){ triangleVertices, 3, 0.0f }; + + // reset the cache + cache.count = 0; + + // get distance between triangle and query shape + b3DistanceOutput distanceOutput = b3ShapeDistance( &distanceInput, &cache, NULL, 0 ); + + if ( distanceOutput.distance == 0.0f ) + { + // todo SAT + } + else if ( distanceOutput.distance <= mover->radius ) + { + b3Plane plane = { distanceOutput.normal, mover->radius - distanceOutput.distance }; + planes[planeCount] = (b3PlaneResult){ plane, distanceOutput.pointA }; + planeCount += 1; + + if ( planeCount == capacity ) + { + return planeCount; + } + } + } + } + } + else + { + // Recurse + B3_ASSERT( count <= B3_MESH_STACK_SIZE - 1 ); + stack[count++] = b3GetRightChild( node ); + node = b3GetLeftChild( node ); + + continue; + } + } + + if ( count == 0 ) + { + break; + } + node = stack[--count]; + } + + return planeCount; +} + +void b3QueryMesh( const b3Mesh* mesh, b3AABB bounds, b3MeshQueryFcn* fcn, void* context ) +{ + b3Vec3 meshScale = mesh->scale; + bool clockwise = meshScale.x * meshScale.y * meshScale.z > 0.0f; + + // Scale may have reflection so min/max may become invalid when unscaled + b3V32 scale = b3LoadV( &meshScale.x ); + b3V32 invScale = b3DivV( b3_oneV, scale ); + b3V32 temp1 = b3MulV( invScale, b3LoadV( &bounds.lowerBound.x ) ); + b3V32 temp2 = b3MulV( invScale, b3LoadV( &bounds.upperBound.x ) ); + b3V32 invScaledBoundsMin = b3MinV( temp1, temp2 ); + b3V32 invScaledBoundsMax = b3MaxV( temp1, temp2 ); + b3V32 invScaledBoundsCenter = b3MulV( b3_halfV, b3AddV( invScaledBoundsMin, invScaledBoundsMax ) ); + b3V32 invScaledBoundsExtent = b3SubV( invScaledBoundsMax, invScaledBoundsCenter ); + + const b3MeshData* data = mesh->data; + + int count = 0; + const b3MeshNode* stack[B3_MESH_STACK_SIZE]; + const b3MeshNode* node = b3GetRoot( data ); + const b3MeshTriangle* triangles = b3GetMeshTriangles( data ); + const b3Vec3* vertices = b3GetMeshVertices( data ); + + while ( true ) + { + // Test node overlap in unscaled space + b3V32 nodeMin = b3LoadV( &node->lowerBound.x ); + b3V32 nodeMax = b3LoadV( &node->upperBound.x ); + + if ( b3TestBoundsOverlap( nodeMin, nodeMax, invScaledBoundsMin, invScaledBoundsMax ) ) + { + if ( b3IsLeaf( node ) ) + { + int triangleCount = node->data.asLeaf.triangleCount; + int triangleOffset = node->triangleOffset; + + for ( int index = 0; index < triangleCount; ++index ) + { + int triangleIndex = triangleOffset + index; + b3MeshTriangle triangle = triangles[triangleIndex]; + + b3Vec3 vertex1 = vertices[triangle.index1]; + b3Vec3 vertex2 = vertices[triangle.index2]; + b3Vec3 vertex3 = vertices[triangle.index3]; + b3V32 v1 = b3LoadV( &vertex1.x ); + b3V32 v2 = b3LoadV( &vertex2.x ); + b3V32 v3 = b3LoadV( &vertex3.x ); + + // Perform triangle overlap test in unscaled space. Winding order doesn't matter. + // todo it is possible that some margins are getting scaled + if ( b3TestBoundsTriangleOverlap( invScaledBoundsCenter, invScaledBoundsExtent, v1, v2, v3 ) ) + { + b3Vec3 a = b3Mul( meshScale, vertex1 ); + b3Vec3 b, c; + if ( clockwise ) + { + b = b3Mul( meshScale, vertex2 ); + c = b3Mul( meshScale, vertex3 ); + } + else + { + b = b3Mul( meshScale, vertex3 ); + c = b3Mul( meshScale, vertex2 ); + } + + bool result = fcn( a, b, c, triangleIndex, context ); + if ( result == false ) + { + return; + } + } + } + } + else + { + // Recurse + B3_ASSERT( count <= B3_MESH_STACK_SIZE - 1 ); + stack[count++] = b3GetRightChild( node ); + node = b3GetLeftChild( node ); + + continue; + } + } + + if ( count == 0 ) + { + break; + } + node = stack[--count]; + } +} diff --git a/vendor/box3d/src/src/mesh_contact.c b/vendor/box3d/src/src/mesh_contact.c new file mode 100644 index 000000000..a7163477a --- /dev/null +++ b/vendor/box3d/src/src/mesh_contact.c @@ -0,0 +1,1186 @@ +// SPDX-FileCopyrightText: 2025 Erin Catto +// SPDX-License-Identifier: MIT + +#include "contact.h" +#include "manifold.h" +#include "physics_world.h" +#include "qsort.h" +#include "shape.h" + +#include "box3d/types.h" + +#include + +// This guards against excessive memory usage and complex collision +#define B3_MAX_MESH_CONTACT_TRIANGLES 256 +#define B3_MAX_POINTS_PER_TRIANGLE 32 + +#if B3_ENABLE_VALIDATION +static bool b3IsSorted( const int* array, int count ) +{ + for ( int i = 0; i < count - 1; ++i ) + { + if ( array[i] >= array[i + 1] ) + { + return false; + } + } + + return true; +} +#endif + +typedef struct b3TriangleQueryContext +{ + int* indices; + int capacity; + int count; +} b3TriangleQueryContext; + +static bool b3CollectTriangleIndicesCallback( b3Vec3 a, b3Vec3 b, b3Vec3 c, int triangleIndex, void* context ) +{ + B3_UNUSED( a, b, c ); + b3TriangleQueryContext* triangleContext = (b3TriangleQueryContext*)context; + if ( triangleContext->count == triangleContext->capacity ) + { + return false; + } + triangleContext->indices[triangleContext->count] = triangleIndex; + triangleContext->count += 1; + return triangleContext->count < triangleContext->capacity; +} + +static int b3QueryMeshTriangles( int* indices, int capacity, const b3Mesh* mesh, b3AABB bounds ) +{ + b3TriangleQueryContext context = { + .indices = indices, + .capacity = capacity, + .count = 0, + }; + + b3QueryMesh( mesh, bounds, b3CollectTriangleIndicesCallback, &context ); + return context.count; +} + +static int b3QueryHeightFieldTriangles( int* indices, int capacity, const b3HeightFieldData* heightField, b3AABB bounds ) +{ + b3TriangleQueryContext context = { + .indices = indices, + .capacity = capacity, + .count = 0, + }; + + b3QueryHeightField( heightField, bounds, b3CollectTriangleIndicesCallback, &context ); + return context.count; +} + +static void b3RefreshCache( b3Contact* contact, const b3Shape* shapeA, b3WorldTransform xfA, const b3AABB* bounds ) +{ + B3_ASSERT( shapeA->type == b3_meshShape || shapeA->type == b3_heightShape ); + + b3MeshContact* meshContact = &contact->meshContact; + + // If the dynamic body didn't move out of the cached query bounds we are done! + if ( b3AABB_Contains( meshContact->queryBounds, *bounds ) ) + { + if ( shapeA->type == b3_meshShape ) + { + for ( int i = 0; i < contact->meshContact.triangleCache.count; ++i ) + { + B3_ASSERT( 0 <= contact->meshContact.triangleCache.data[i].triangleIndex && + contact->meshContact.triangleCache.data[i].triangleIndex < shapeA->mesh.data->triangleCount ); + } + } + + return; + } + + // Enlarge to the query bounds to absorb small movement + float radius = B3_MAX_AABB_MARGIN + B3_SPECULATIVE_DISTANCE; + b3Vec3 extension = { radius, radius, radius }; + meshContact->queryBounds.lowerBound = b3Sub( bounds->lowerBound, extension ); + meshContact->queryBounds.upperBound = b3Add( bounds->upperBound, extension ); + + // Query triangles + int triangleCapacity = B3_MAX_MESH_CONTACT_TRIANGLES; + + int triangleIndices[B3_MAX_MESH_CONTACT_TRIANGLES]; + + // Bounds are in world space. Convert to the local mesh frame. The broadphase bounds are float, + // so the demoted mesh transform is the matching float world frame (exact in float mode). + b3Transform meshTransform = b3ToRelativeTransform( xfA, b3Pos_zero ); + b3AABB localBounds = b3AABB_Transform( b3InvertTransform( meshTransform ), meshContact->queryBounds ); + int triangleCount; + if ( shapeA->type == b3_meshShape ) + { + triangleCount = b3QueryMeshTriangles( triangleIndices, triangleCapacity, &shapeA->mesh, localBounds ); + } + else + { + B3_ASSERT( shapeA->type == b3_heightShape ); + triangleCount = b3QueryHeightFieldTriangles( triangleIndices, triangleCapacity, shapeA->heightField, localBounds ); + } + + if ( triangleCount == triangleCapacity ) + { + static bool s_once = false; + if ( s_once == false ) + { + b3Log( "WARNING: complex mesh detected, triangle buffer capacity of %d reached", triangleCapacity ); + s_once = true; + } + } + + // Triangle indices must be sorted to match caches. + B3_VALIDATE( b3IsSorted( triangleIndices, triangleCount ) ); + + // Create new contact cache and match with old one + b3ContactCache contactCache[B3_MAX_MESH_CONTACT_TRIANGLES]; + + int index2 = 0; + for ( int index1 = 0; index1 < triangleCount; ++index1 ) + { + contactCache[index1] = (b3ContactCache){ 0 }; + + while ( index2 < contact->meshContact.triangleCache.count && + contact->meshContact.triangleCache.data[index2].triangleIndex < triangleIndices[index1] ) + { + index2 += 1; + } + + if ( index2 < contact->meshContact.triangleCache.count && + contact->meshContact.triangleCache.data[index2].triangleIndex == triangleIndices[index1] ) + { + contactCache[index1] = contact->meshContact.triangleCache.data[index2].cache; + } + } + + // Save new cache + b3Array_Resize( contact->meshContact.triangleCache, triangleCount ); + for ( int i = 0; i < triangleCount; ++i ) + { + contact->meshContact.triangleCache.data[i] = (b3TriangleCache){ triangleIndices[i], contactCache[i] }; + + if ( shapeA->type == b3_meshShape ) + { + B3_ASSERT( 0 <= contact->meshContact.triangleCache.data[i].triangleIndex && + contact->meshContact.triangleCache.data[i].triangleIndex < shapeA->mesh.data->triangleCount ); + } + } +} + +typedef struct b3TentativeTriangle +{ + float squaredDistance; + int index; +} b3TentativeTriangle; + +#define B3_MAX_EDGE_COUNT 64 + +typedef struct b3FoundEdges +{ + uint64_t keys[B3_MAX_EDGE_COUNT]; + int count; +} b3FoundEdges; + +static inline bool b3AddEdge( b3FoundEdges* edges, int vertex1, int vertex2 ) +{ + uint64_t i1 = (uint64_t)b3MinInt( vertex1, vertex2 ); + uint64_t i2 = (uint64_t)b3MaxInt( vertex1, vertex2 ); + uint64_t key = i1 << 32 | i2; + + int count = edges->count; + for ( int i = 0; i < count; ++i ) + { + if ( edges->keys[i] == key ) + { + return false; + } + } + + if ( count == B3_MAX_EDGE_COUNT ) + { + // This will lead to a potential ghost collision + return true; + } + + edges->keys[count] = key; + edges->count += 1; + + return true; +} + +static inline bool b3FindEdge( b3FoundEdges* edges, int vertex1, int vertex2 ) +{ + uint64_t i1 = (uint64_t)b3MinInt( vertex1, vertex2 ); + uint64_t i2 = (uint64_t)b3MaxInt( vertex1, vertex2 ); + uint64_t key = i1 << 32 | i2; + + int count = edges->count; + for ( int i = 0; i < count; ++i ) + { + if ( edges->keys[i] == key ) + { + return true; + } + } + + return false; +} + +#if 0 +// Two triangles share an edge iff they share at least two vertex indices. +static inline bool b3TrianglesShareEdge( int a1, int a2, int a3, int b1, int b2, int b3 ) +{ + int matches = 0; + matches += ( a1 == b1 || a1 == b2 || a1 == b3 ); + matches += ( a2 == b1 || a2 == b2 || a2 == b3 ); + matches += ( a3 == b1 || a3 == b2 || a3 == b3 ); + return matches >= 2; +} +#endif + +#define B3_MAX_VERTEX_COUNT 64 + +typedef struct b3FoundVertices +{ + int keys[B3_MAX_VERTEX_COUNT]; + int count; +} b3FoundVertices; + +static inline bool b3AddVertex( b3FoundVertices* vertices, int vertex ) +{ + int key = vertex; + + int count = vertices->count; + for ( int i = 0; i < count; ++i ) + { + if ( vertices->keys[i] == key ) + { + return false; + } + } + + if ( count == B3_MAX_VERTEX_COUNT ) + { + // This will lead to a potential ghost collision + return true; + } + + vertices->keys[count] = key; + vertices->count += 1; + + return true; +} + +// Returns true if (score, separation) should replace (bestScore, bestSeparation). +static inline bool b3IsBetterCullCandidate( float score, float separation, float bestScore, float bestSeparation, float scoreTol, + float separationTol ) +{ + if ( score > bestScore + scoreTol ) + { + return true; + } + if ( score < bestScore - scoreTol ) + { + return false; + } + + // Break the tie using separation + return separation < bestSeparation - separationTol; +} + +typedef struct b3Point2D +{ + b3Vec2 p; + float separation; + int originalIndex; +} b3Point2D; + +static int b3CullPoints( b3Point2D* points, int count ) +{ + if ( count <= 1 ) + { + return count; + } + + float tol = 0.25f * B3_LINEAR_SLOP; + float tolSqr = tol * tol; + float separationTol = B3_LINEAR_SLOP; + + b3Point2D finalPoints[4]; + int count1 = count; + + // Step 1: the two points with the largest distance, ties broken by deepest combined separation + float bestScore = 0.0f; + float bestSeparation = FLT_MAX; + int bestIndex1 = B3_NULL_INDEX; + int bestIndex2 = B3_NULL_INDEX; + + for ( int i = 0; i < count1; ++i ) + { + b3Vec2 p1 = points[i].p; + for ( int j = i + 1; j < count1; ++j ) + { + float score = b3DistanceSquared2( p1, points[j].p ); + // Separation sum heuristic + float separation = points[i].separation + points[j].separation; + + if ( b3IsBetterCullCandidate( score, separation, bestScore, bestSeparation, tolSqr, separationTol ) ) + { + bestIndex1 = i; + bestIndex2 = j; + bestScore = score; + bestSeparation = separation; + } + } + } + + if ( bestScore < tolSqr ) + { + // Choose deepest point + int deepestIndex = 0; + for ( int i = 1; i < count1; ++i ) + { + if ( points[i].separation < points[deepestIndex].separation ) + { + deepestIndex = i; + } + } + + if ( deepestIndex != 0 ) + { + points[0] = points[deepestIndex]; + } + return 1; + } + + finalPoints[0] = points[bestIndex1]; + finalPoints[1] = points[bestIndex2]; + + // Cull + points[bestIndex2] = points[count1 - 1]; + points[bestIndex1] = points[count1 - 2]; + count1 -= 2; + + if ( count1 == 0 ) + { + points[0] = finalPoints[0]; + points[1] = finalPoints[1]; + return 2; + } + + // First anchor point + b3Vec2 a = finalPoints[0].p; + + // Second anchor point + b3Vec2 b = finalPoints[1].p; + b3Vec2 ba = b3Sub2( b, a ); + // float length = b3Length2( ba ); + // float areaTol = tol * length; + + // Step 2: find the point with the maximum triangular area, ties broken by deepest separation + bestScore = 0.0f; + bestSeparation = FLT_MAX; + int bestIndex = B3_NULL_INDEX; + float bestSignedArea = 0.0f; + for ( int i = 0; i < count1; ++i ) + { + b3Vec2 p = points[i].p; + float signedArea = b3Cross2( ba, b3Sub2( p, a ) ); + float score = b3AbsFloat( signedArea ); + + if ( b3IsBetterCullCandidate( score, points[i].separation, bestScore, bestSeparation, tolSqr, separationTol ) ) + { + bestSignedArea = signedArea; + bestScore = score; + bestSeparation = points[i].separation; + bestIndex = i; + } + } + + if ( bestIndex == B3_NULL_INDEX ) + { + // All points collinear + points[0] = finalPoints[0]; + points[1] = finalPoints[1]; + return 2; + } + + // Store best point + finalPoints[2] = points[bestIndex]; + + if ( count1 == 1 ) + { + points[0] = finalPoints[0]; + points[1] = finalPoints[1]; + points[2] = finalPoints[2]; + return 3; + } + + // Cull + points[bestIndex] = points[count1 - 1]; + count1 -= 1; + + // Step 4: get the point that adds the most area outside the current triangle + + // Third anchor + b3Vec2 c = finalPoints[2].p; + + // Ensure CCW ordering + if ( bestSignedArea < 0.0f ) + { + B3_SWAP( b, c ); + ba = b3Sub2( b, a ); + } + + b3Vec2 cb = b3Sub2( c, b ); + b3Vec2 ac = b3Sub2( a, c ); + + bestScore = 0.0f; + bestSeparation = FLT_MAX; + bestIndex = B3_NULL_INDEX; + for ( int i = 0; i < count1; ++i ) + { + b3Vec2 p = points[i].p; + float u1 = b3Cross2( b3Sub2( p, a ), ba ); + float u2 = b3Cross2( b3Sub2( p, b ), cb ); + float u3 = b3Cross2( b3Sub2( p, c ), ac ); + float score = b3MaxFloat( u1, b3MaxFloat( u2, u3 ) ); + + // Use the area tolerance for collinear points and hysteresis + if ( b3IsBetterCullCandidate( score, points[i].separation, bestScore, bestSeparation, tolSqr, separationTol ) ) + { + bestScore = score; + bestSeparation = points[i].separation; + bestIndex = i; + } + } + + if ( bestIndex == B3_NULL_INDEX ) + { + // No additional area + points[0] = finalPoints[0]; + points[1] = finalPoints[1]; + points[2] = finalPoints[2]; + return 3; + } + + // Store best point + finalPoints[3] = points[bestIndex]; + + // Full quad + points[0] = finalPoints[0]; + points[1] = finalPoints[1]; + points[2] = finalPoints[2]; + points[3] = finalPoints[3]; + return 4; +} + +static int b3ReduceCluster( b3LocalManifoldPoint* points, int count1, b3Vec3 normal, b3Arena arena ) +{ + int targetCount = 1; + if ( count1 <= targetCount ) + { + return count1; + } + + b3Point2D* pts = b3Bump( &arena, count1 * sizeof( b3Point2D ) ); + b3Vec3 u = b3Perp( normal ); + b3Vec3 v = b3Cross( normal, u ); + b3Vec3 origin = points[0].point; + + for ( int i = 0; i < count1; ++i ) + { + b3Vec3 d = b3Sub( points[i].point, origin ); + pts[i].p = (b3Vec2){ b3Dot( d, u ), b3Dot( d, v ) }; + pts[i].separation = points[i].separation; + pts[i].originalIndex = i; + } + + int count2 = b3CullPoints( pts, count1 ); + B3_ASSERT( count2 <= B3_MAX_MANIFOLD_POINTS ); + + b3LocalManifoldPoint finalPoints[B3_MAX_MANIFOLD_POINTS]; + for ( int i = 0; i < count2; ++i ) + { + int index = pts[i].originalIndex; + B3_ASSERT( 0 <= index && index < count1 ); + finalPoints[i] = points[index]; + } + + memcpy( points, finalPoints, count2 * sizeof( b3LocalManifoldPoint ) ); + return count2; +} + +typedef struct b3Cluster +{ + b3Vec3 manifoldNormal; + b3Vec3 triangleNormal; + b3LocalManifoldPoint* points; + int pointCapacity; + int pointCount; +} b3Cluster; + +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 ) +{ + B3_ASSERT( shapeA->type == b3_meshShape || shapeA->type == b3_heightShape ); + B3_UNUSED( workerIndex ); + B3_UNUSED( isFast ); + B3_UNUSED( materialMap ); + + b3TaskContext* context = b3Array_Get( world->taskContexts, workerIndex ); + + b3RefreshCache( contact, shapeA, xfA, &shapeB->aabb ); + + // Collide with triangles and build manifolds + b3MeshContact* meshContact = &contact->meshContact; + int triangleCount = meshContact->triangleCache.count; + + b3LocalManifold** acceptedManifolds = b3Bump( &arena, triangleCount * sizeof( b3LocalManifold* ) ); + int acceptedManifoldCount = 0; + b3LocalManifold** tentativeManifolds = b3Bump( &arena, triangleCount * sizeof( b3LocalManifold* ) ); + int tentativeManifoldCount = 0; + b3TentativeTriangle* tentativeTriangles = b3Bump( &arena, triangleCount * sizeof( b3TentativeTriangle ) ); + int tentativeTriangleCount = 0; + + b3FoundEdges foundEdges; + b3FoundVertices foundVertices; + foundEdges.count = 0; + foundVertices.count = 0; + + // This transform converts from mesh frame into the shapeB frame + b3Transform transformAtoB = b3InvMulWorldTransforms( xfB, xfA ); + b3Matrix3 relativeMatrix = b3MakeMatrixFromQuat( transformAtoB.q ); + float linearSlop = B3_LINEAR_SLOP; + + // This should push apart shapes after a time of impact event. + // In the past I've called this `polygon skin`, but PhysX and Unreal + // call it `rest offset` which seems appropriate in this case. + // It leads to a small visual gap but seems to improve the quality of mesh + // collision, especially for hull versus mesh. + float restOffset = B3_MESH_REST_OFFSET; + bool enableSpeculative = contact->flags & b3_enableSpeculativePoints; + + // Make room for clip points + int pointBufferCapacity = B3_MAX_POINTS_PER_TRIANGLE * triangleCount; + + b3LocalManifoldPoint* pointBuffer = b3Bump( &arena, pointBufferCapacity * sizeof( b3LocalManifoldPoint ) ); + int totalPointCount = 0; + + b3LocalManifold* manifoldBuffer = b3Bump( &arena, triangleCount * sizeof( b3LocalManifold ) ); + int manifoldCount = 0; + + b3TriangleCache* triangleCaches = meshContact->triangleCache.data; + + const b3HullData* hullB = shapeB->type == b3_hullShape ? shapeB->hull : NULL; + + for ( int index = 0; index < triangleCount && totalPointCount + 3 < pointBufferCapacity; ++index ) + { + int triangleIndex = triangleCaches[index].triangleIndex; + + b3Triangle triangle; + if ( shapeA->type == b3_meshShape ) + { + triangle = b3GetMeshTriangle( &shapeA->mesh, triangleIndex ); + } + else + { + B3_ASSERT( shapeA->type == b3_heightShape ); + triangle = b3GetHeightFieldTriangle( shapeA->heightField, triangleIndex ); + } + + // Transform triangle into the shape frame + b3Vec3 vertices[3]; + vertices[0] = b3Add( b3MulMV( relativeMatrix, triangle.vertices[0] ), transformAtoB.p ); + vertices[1] = b3Add( b3MulMV( relativeMatrix, triangle.vertices[1] ), transformAtoB.p ); + vertices[2] = b3Add( b3MulMV( relativeMatrix, triangle.vertices[2] ), transformAtoB.p ); + + b3ContactCache* cache = &triangleCaches[index].cache; + int pointCapacity = pointBufferCapacity - totalPointCount; + b3LocalManifold* manifold = manifoldBuffer + manifoldCount; + manifold->points = pointBuffer + totalPointCount; + manifold->pointCount = 0; + manifold->triangleFlags = triangle.flags; + manifold->feature = b3_featureNone; + + switch ( shapeB->type ) + { + case b3_capsuleShape: + b3CollideCapsuleAndTriangle( manifold, pointCapacity, &shapeB->capsule, vertices, &cache->simplexCache ); + break; + + case b3_hullShape: + // Cached edge contact is dangerous at high speed because the hull can rotate around the edge and tunnel + // through the triangle. + if ( isFast && cache->satCache.type == b3_edgePairAxis ) + { + cache->satCache = (b3SATCache){ 0 }; + } + + b3CollideHullAndTriangle( manifold, pointCapacity, hullB, vertices[0], vertices[1], vertices[2], triangle.flags, + &cache->satCache, enableSpeculative ); + context->satCallCount += 1; + context->satCacheHitCount += cache->satCache.hit; + break; + + case b3_sphereShape: + b3CollideSphereAndTriangle( manifold, pointCapacity, &shapeB->sphere, vertices ); + break; + + default: + B3_ASSERT( false ); + return false; + } + + int manifoldPointCount = manifold->pointCount; + + if ( manifoldPointCount > 0 ) + { + B3_ASSERT( manifold->feature != b3_featureNone ); + + manifoldCount += 1; + totalPointCount += manifoldPointCount; + manifold->triangleIndex = triangleIndex; + manifold->triangleNormal = b3MakeNormalFromPoints( vertices[0], vertices[1], vertices[2] ); + manifold->i1 = triangle.i1; + manifold->i2 = triangle.i2; + manifold->i3 = triangle.i3; + + if ( manifold->feature == b3_featureTriangleFace || B3_FORCE_GHOST_COLLISIONS ) + { + (void)b3AddEdge( &foundEdges, triangle.i1, triangle.i2 ); + (void)b3AddEdge( &foundEdges, triangle.i2, triangle.i3 ); + (void)b3AddEdge( &foundEdges, triangle.i3, triangle.i1 ); + (void)b3AddVertex( &foundVertices, triangle.i1 ); + (void)b3AddVertex( &foundVertices, triangle.i2 ); + (void)b3AddVertex( &foundVertices, triangle.i3 ); + + acceptedManifolds[acceptedManifoldCount++] = manifold; + } + else if ( manifold->feature == b3_featureHullFace ) + { + float cosNormalAngle = b3Dot( manifold->triangleNormal, manifold->normal ); + if ( cosNormalAngle > 0.5f ) + { + (void)b3AddEdge( &foundEdges, triangle.i1, triangle.i2 ); + (void)b3AddEdge( &foundEdges, triangle.i2, triangle.i3 ); + (void)b3AddEdge( &foundEdges, triangle.i3, triangle.i1 ); + (void)b3AddVertex( &foundVertices, triangle.i1 ); + (void)b3AddVertex( &foundVertices, triangle.i2 ); + (void)b3AddVertex( &foundVertices, triangle.i3 ); + + acceptedManifolds[acceptedManifoldCount++] = manifold; + } + else + { + float minSeparation = manifold->points[0].separation; + for ( int i = 1; i < manifoldPointCount; ++i ) + { + minSeparation = b3MinFloat( minSeparation, manifold->points[i].separation ); + } + + if ( minSeparation < -2.0f * linearSlop ) + { + // Deep overlap + (void)b3AddEdge( &foundEdges, triangle.i1, triangle.i2 ); + (void)b3AddEdge( &foundEdges, triangle.i2, triangle.i3 ); + (void)b3AddEdge( &foundEdges, triangle.i3, triangle.i1 ); + (void)b3AddVertex( &foundVertices, triangle.i1 ); + (void)b3AddVertex( &foundVertices, triangle.i2 ); + (void)b3AddVertex( &foundVertices, triangle.i3 ); + acceptedManifolds[acceptedManifoldCount++] = manifold; + } + else + { + b3TentativeTriangle tentativeTriangle = { .squaredDistance = manifold->squaredDistance, + .index = tentativeManifoldCount }; + tentativeTriangles[tentativeTriangleCount++] = tentativeTriangle; + tentativeManifolds[tentativeManifoldCount++] = manifold; + } + } + } + else + { + b3TentativeTriangle tentativeTriangle = { .squaredDistance = manifold->squaredDistance, + .index = tentativeManifoldCount }; + tentativeTriangles[tentativeTriangleCount++] = tentativeTriangle; + tentativeManifolds[tentativeManifoldCount++] = manifold; + } + } + } + + B3_ASSERT( acceptedManifoldCount <= triangleCount ); + B3_ASSERT( tentativeManifoldCount <= triangleCount ); + B3_ASSERT( tentativeTriangleCount <= triangleCount ); + + if ( shapeB->type == b3_sphereShape ) + { + // Sort triangles so the closest triangles are processed first + { +#define LESS( i, j ) tentativeTriangles[(int)i].squaredDistance < tentativeTriangles[(int)j].squaredDistance +#define SWAP( i, j ) \ + do \ + { \ + b3TentativeTriangle tmp = tentativeTriangles[(int)i]; \ + tentativeTriangles[(int)i] = tentativeTriangles[(int)j]; \ + tentativeTriangles[(int)j] = tmp; \ + } \ + while ( 0 ) + QSORT( tentativeTriangleCount, LESS, SWAP ); +#undef LESS +#undef SWAP + } + + // Add tentative manifolds in sorted order. Avoid adding manifolds that generate ghost collisions. + for ( int i = 0; i < tentativeTriangleCount; ++i ) + { + b3LocalManifold* m = tentativeManifolds[tentativeTriangles[i].index]; + + bool addedEdge1 = b3AddEdge( &foundEdges, m->i1, m->i2 ); + bool addedEdge2 = b3AddEdge( &foundEdges, m->i2, m->i3 ); + bool addedEdge3 = b3AddEdge( &foundEdges, m->i3, m->i1 ); + bool addedVertex1 = b3AddVertex( &foundVertices, m->i1 ); + bool addedVertex2 = b3AddVertex( &foundVertices, m->i2 ); + bool addedVertex3 = b3AddVertex( &foundVertices, m->i3 ); + + b3TriangleFeature feature = m->feature; + bool shouldCollide = false; + switch ( feature ) + { + case b3_featureNone: + case b3_featureTriangleFace: + B3_ASSERT( false ); + break; + + case b3_featureEdge1: + shouldCollide = addedEdge1; + break; + + case b3_featureEdge2: + shouldCollide = addedEdge2; + break; + + case b3_featureEdge3: + shouldCollide = addedEdge3; + break; + + case b3_featureVertex1: + shouldCollide = addedVertex1; + break; + + case b3_featureVertex2: + shouldCollide = addedVertex2; + break; + + case b3_featureVertex3: + shouldCollide = addedVertex3; + break; + + default: + B3_ASSERT( false ); + break; + } + + if ( shouldCollide == true ) + { + acceptedManifolds[acceptedManifoldCount++] = m; + } + } + } + else + { + // Problem: hull can tunnel if time of impact is at concave edge + // Example: flat box sliding down a ramp to a flat bottom + // Solution: only ignore flat edges + for ( int i = 0; i < tentativeManifoldCount; ++i ) + { + b3LocalManifold* m = tentativeManifolds[i]; + int triangleFlags = m->triangleFlags; + + if ( ( triangleFlags & b3_allFlatEdges ) == b3_allFlatEdges ) + { + continue; + } + + if ( ( triangleFlags & b3_flatEdge1 ) == b3_flatEdge1 ) + { + if ( b3FindEdge( &foundEdges, m->i1, m->i2 ) ) + { + continue; + } + } + + if ( ( triangleFlags & b3_flatEdge2 ) == b3_flatEdge2 ) + { + if ( b3FindEdge( &foundEdges, m->i2, m->i3 ) ) + { + continue; + } + } + + if ( ( triangleFlags & b3_flatEdge3 ) == b3_flatEdge3 ) + { + if ( b3FindEdge( &foundEdges, m->i3, m->i1 ) ) + { + continue; + } + } + + acceptedManifolds[acceptedManifoldCount++] = m; + } + } + + B3_ASSERT( acceptedManifoldCount <= triangleCount ); + + if ( acceptedManifoldCount == 0 ) + { + if ( contact->manifoldCount > 0 ) + { + b3FreeManifolds( world, contact->manifolds, contact->manifoldCount ); + contact->manifolds = NULL; + contact->manifoldCount = 0; + } + return false; + } + + b3Cluster* clusters = b3Bump( &arena, acceptedManifoldCount * sizeof( b3Cluster ) ); + int* clusterMemberships = b3Bump( &arena, acceptedManifoldCount * sizeof( int ) ); + + // Cluster tolerance is tighter than the warm starting manifold matching tolerance. These + // serve different purposes. + const float clusterThreshold = 0.996f; + int clusterCount = 0; + int clusterPointCount = 0; + for ( int i = 0; i < acceptedManifoldCount; ++i ) + { + clusterMemberships[i] = B3_NULL_INDEX; + + const b3LocalManifold* manifold = acceptedManifolds[i]; + clusterPointCount += manifold->pointCount; + + // Cluster based on the triangle normal and contact normal. + // The first cluster found is accepted because the tolerance is tight. + // todo consider requiring the triangles to be connect by an edge. + // todo consider looking for the best cluster instead of the first one within tolerance + // This bool is here to allow quick testing with and without clustering. + bool allowClustering = true; + b3Vec3 manifoldNormal = manifold->normal; + b3Vec3 triangleNormal = manifold->triangleNormal; + int clusterIndex = B3_NULL_INDEX; + for ( int j = 0; j < clusterCount && allowClustering; ++j ) + { + float cosManifoldAngle = b3Dot( clusters[j].manifoldNormal, manifoldNormal ); + float cosTriangleAngle = b3Dot( clusters[j].triangleNormal, triangleNormal ); + if ( cosManifoldAngle <= clusterThreshold || cosTriangleAngle <= clusterThreshold ) + { + continue; + } + +#if 0 + // todo there could be later triangles that create the connection + // then failure to cluster breaks greedy impulse warm starting + bool edgeConnected = false; + + for ( int k = 0; k < i; ++k ) + { + if ( clusterMemberships[k] != j ) + { + continue; + } + + const b3LocalManifold* other = acceptedManifolds[k]; + if ( b3TrianglesShareEdge( manifold->i1, manifold->i2, manifold->i3, other->i1, other->i2, other->i3 ) ) + { + edgeConnected = true; + break; + } + } + + if ( edgeConnected ) + { + clusterIndex = j; + break; + } +#else + + // Found a cluster + clusterIndex = j; + break; +#endif + } + + if ( clusterIndex != B3_NULL_INDEX ) + { + clusterMemberships[i] = clusterIndex; + clusters[clusterIndex].pointCapacity += manifold->pointCount; + } + else + { + clusters[clusterCount].manifoldNormal = manifoldNormal; + clusters[clusterCount].triangleNormal = triangleNormal; + clusters[clusterCount].pointCapacity = manifold->pointCount; + clusterMemberships[i] = clusterCount; + clusterCount += 1; + } + } + + if ( clusterPointCount == 0 ) + { + return false; + } + + // Setup clusters + b3LocalManifoldPoint* clusterPoints = b3Bump( &arena, clusterPointCount * sizeof( b3LocalManifoldPoint ) ); + int pointOffset = 0; + + for ( int i = 0; i < clusterCount; ++i ) + { + b3Cluster* cluster = clusters + i; + cluster->points = clusterPoints + pointOffset; + cluster->pointCount = 0; + pointOffset += cluster->pointCapacity; + } + + // Populate clusters + for ( int i = 0; i < acceptedManifoldCount; ++i ) + { + int clusterIndex = clusterMemberships[i]; + if ( clusterIndex == B3_NULL_INDEX ) + { + continue; + } + + B3_ASSERT( 0 <= clusterIndex && clusterIndex < clusterCount ); + + b3LocalManifold* am = acceptedManifolds[i]; + b3Cluster* cm = clusters + clusterIndex; + for ( int j = 0; j < am->pointCount; ++j ) + { + B3_ASSERT( cm->pointCount < cm->pointCapacity ); + b3LocalManifoldPoint* ap = am->points + j; + b3LocalManifoldPoint* cp = cm->points + cm->pointCount; + + cp->triangleIndex = am->triangleIndex; + cp->point = ap->point; + cp->separation = ap->separation; + cp->pair = ap->pair; + cm->pointCount += 1; + } + } + + // Simplify clusters + for ( int i = 0; i < clusterCount; ++i ) + { + b3Cluster* cm = clusters + i; + B3_ASSERT( cm->pointCount == cm->pointCapacity ); + int reducedCount = b3ReduceCluster( cm->points, cm->pointCount, cm->triangleNormal, arena ); + cm->pointCount = reducedCount; + } + + // Make a temporary copy of previous manifolds + int oldManifoldCount = contact->manifoldCount; + b3Manifold* oldManifolds = NULL; + if ( oldManifoldCount > 0 ) + { + oldManifolds = b3Bump( &arena, oldManifoldCount * sizeof( b3Manifold ) ); + memcpy( oldManifolds, contact->manifolds, oldManifoldCount * sizeof( b3Manifold ) ); + } + + // Resize manifolds if needed + if ( oldManifoldCount != clusterCount ) + { + b3FreeManifolds( world, contact->manifolds, contact->manifoldCount ); + contact->manifolds = b3AllocateManifolds( world, clusterCount ); + contact->manifoldCount = (uint16_t)clusterCount; + } + else + { + // Mem zero manifolds + memset( contact->manifolds, 0, contact->manifoldCount * sizeof( b3Manifold ) ); + } + + bool* consumed = NULL; + if ( oldManifoldCount > 0 ) + { + consumed = b3Bump( &arena, oldManifoldCount * sizeof( bool ) ); + memset( consumed, 0, oldManifoldCount * sizeof( bool ) ); + } + + b3Matrix3 matrixB = b3MakeMatrixFromQuat( xfB.q ); + b3Vec3 offsetA = b3SubPos( xfB.p, xfA.p ); + + const float normalMatchTolerance = 0.995f; + for ( int i = 0; i < clusterCount; ++i ) + { + b3Cluster* cm = clusters + i; + int pointCount = cm->pointCount; + B3_ASSERT( 0 < pointCount && pointCount <= B3_MAX_MANIFOLD_POINTS ); + + b3Manifold* manifold = contact->manifolds + i; + manifold->pointCount = pointCount; + manifold->normal = b3MulMV( matrixB, cm->manifoldNormal ); + + b3Vec3 clusterNormal = b3MulMV( matrixB, cm->manifoldNormal ); + float bestDot = normalMatchTolerance; + int bestIndex = B3_NULL_INDEX; + + for ( int j = 0; j < oldManifoldCount; ++j ) + { + if ( consumed[j] == true ) + { + continue; + } + + float dot = b3Dot( oldManifolds[j].normal, clusterNormal ); + if ( dot > bestDot ) + { + bestIndex = j; + bestDot = dot; + } + } + + b3Manifold* matchedManifold = NULL; + if ( bestIndex != B3_NULL_INDEX ) + { + matchedManifold = oldManifolds + bestIndex; + manifold->frictionImpulse = matchedManifold->frictionImpulse; + manifold->rollingImpulse = matchedManifold->rollingImpulse; + manifold->twistImpulse = matchedManifold->twistImpulse; + consumed[bestIndex] = true; + } + + for ( int j = 0; j < pointCount; ++j ) + { + const b3LocalManifoldPoint* source = cm->points + j; + b3ManifoldPoint* target = manifold->points + j; + + // Contact points are computed in frame B + target->anchorB = b3MulMV( matrixB, source->point ); + target->anchorA = b3Add( target->anchorB, offsetA ); + target->separation = source->separation - restOffset; + target->featureId = b3MakeFeatureId( source->pair ); + target->triangleIndex = source->triangleIndex; + + // Preserve normal impulse if possible + if ( matchedManifold != NULL ) + { + int oldPointCount = matchedManifold->pointCount; + for ( int k = 0; k < oldPointCount; ++k ) + { + b3ManifoldPoint* oldPt = matchedManifold->points + k; + + if ( target->featureId == oldPt->featureId && target->triangleIndex == oldPt->triangleIndex ) + { + target->normalImpulse = oldPt->normalImpulse; + target->persisted = true; + + // claimed + oldPt->triangleIndex = B3_NULL_INDEX; + break; + } + } + } + } + } + + const b3SurfaceMaterial* materialsA = b3GetShapeMaterials( shapeA ); + const b3SurfaceMaterial* materialB = b3GetShapeMaterials( shapeB ); + b3Vec3 tangentVelocityA = b3Vec3_zero; + + // Update friction and restitution if the mesh has per triangle material + if ( shapeA->materialCount > 0 ) + { + float friction = 0.0f; + float restitution = 0.0f; + float sampleCount = 0.0f; + + const uint8_t* materialIndices; + if ( shapeA->type == b3_meshShape ) + { + materialIndices = b3GetMeshMaterialIndices( shapeA->mesh.data ); + } + else + { + materialIndices = b3GetHeightFieldMaterialIndices( shapeA->heightField ); + } + + for ( int i = 0; i < clusterCount; ++i ) + { + b3Manifold* manifold = contact->manifolds + i; + int pointCount = manifold->pointCount; + for ( int j = 0; j < pointCount; ++j ) + { + int triangleIndex = manifold->points[j].triangleIndex; + int materialIndex; + if ( shapeA->type == b3_meshShape ) + { + materialIndex = materialIndices[triangleIndex]; + + if ( materialMap != NULL ) + { + materialIndex = materialMap[materialIndex]; + } + } + else + { + materialIndex = materialIndices[triangleIndex >> 1]; + } + + materialIndex = b3ClampInt( materialIndex, 0, shapeA->materialCount - 1 ); + b3SurfaceMaterial material = materialsA[materialIndex]; + friction += world->frictionCallback( material.friction, material.userMaterialId, materialB->friction, + materialB->userMaterialId ); + restitution += world->restitutionCallback( material.restitution, material.userMaterialId, materialB->restitution, + materialB->userMaterialId ); + + tangentVelocityA = b3Add( tangentVelocityA, material.tangentVelocity ); + + sampleCount += 1.0f; + } + } + + if ( sampleCount > 0.0f ) + { + float invCount = 1.0f / sampleCount; + contact->friction = invCount * friction; + contact->restitution = invCount * restitution; + tangentVelocityA = b3MulSV( invCount, tangentVelocityA ); + } + + B3_ASSERT( b3IsValidFloat( contact->friction ) && contact->friction >= 0.0f ); + B3_ASSERT( b3IsValidFloat( contact->restitution ) && contact->restitution >= 0.0f ); + } + else + { + // Keep these updated in case the values on the shapes are modified + contact->friction = world->frictionCallback( materialsA[0].friction, materialsA[0].userMaterialId, materialB->friction, + materialB->userMaterialId ); + contact->restitution = world->restitutionCallback( materialsA[0].restitution, materialsA[0].userMaterialId, + materialB->restitution, materialB->userMaterialId ); + tangentVelocityA = materialsA[0].tangentVelocity; + } + + tangentVelocityA = b3RotateVector( xfA.q, tangentVelocityA ); + + float radiusB = 0.0f; + if ( shapeB->type == b3_sphereShape ) + { + radiusB = shapeB->sphere.radius; + } + else if ( shapeB->type == b3_capsuleShape ) + { + radiusB = shapeB->capsule.radius; + } + else if ( shapeB->type == b3_hullShape ) + { + radiusB = shapeB->hull->innerRadius; + } + + contact->rollingResistance = materialB->rollingResistance * radiusB; + + b3Vec3 tangentVelocityB = b3RotateVector( xfB.q, materialB->tangentVelocity ); + contact->tangentVelocity = b3Sub( tangentVelocityA, tangentVelocityB ); + return true; +} diff --git a/vendor/box3d/src/src/motor_joint.c b/vendor/box3d/src/src/motor_joint.c new file mode 100644 index 000000000..734d49767 --- /dev/null +++ b/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; + } +} diff --git a/vendor/box3d/src/src/mover.c b/vendor/box3d/src/src/mover.c new file mode 100644 index 000000000..209426e97 --- /dev/null +++ b/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; +} diff --git a/vendor/box3d/src/src/name_cache.c b/vendor/box3d/src/src/name_cache.c new file mode 100644 index 000000000..a7833df5a --- /dev/null +++ b/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 + +#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 ); +} diff --git a/vendor/box3d/src/src/name_cache.h b/vendor/box3d/src/src/name_cache.h new file mode 100644 index 000000000..d1535b37a --- /dev/null +++ b/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; +} diff --git a/vendor/box3d/src/src/parallel_for.c b/vendor/box3d/src/src/parallel_for.c new file mode 100644 index 000000000..07d1c3fdc --- /dev/null +++ b/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 + +// 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 ); + } + } +} diff --git a/vendor/box3d/src/src/parallel_for.h b/vendor/box3d/src/src/parallel_for.h new file mode 100644 index 000000000..4a6d2851c --- /dev/null +++ b/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 ); diff --git a/vendor/box3d/src/src/parallel_joint.c b/vendor/box3d/src/src/parallel_joint.c new file mode 100644 index 000000000..7390a2131 --- /dev/null +++ b/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 ); +} diff --git a/vendor/box3d/src/src/physics_world.c b/vendor/box3d/src/src/physics_world.c new file mode 100644 index 000000000..f0dc19e2d --- /dev/null +++ b/vendor/box3d/src/src/physics_world.c @@ -0,0 +1,4099 @@ +// SPDX-FileCopyrightText: 2025 Erin Catto +// SPDX-License-Identifier: MIT + +#include "physics_world.h" + +#include "arena_allocator.h" +#include "bitset.h" +#include "body.h" +#include "broad_phase.h" +#include "constraint_graph.h" +#include "contact.h" +#include "core.h" +#include "ctz.h" +#include "hull_map.h" +#include "island.h" +#include "joint.h" +#include "parallel_for.h" +#include "platform.h" +#include "recording.h" +#include "scheduler.h" +#include "sensor.h" +#include "shape.h" +#include "solver.h" +#include "solver_set.h" + +#include "box3d/box3d.h" +#include "box3d/constants.h" + +#include +#include +#include +#include + +_Static_assert( B3_MAX_WORLDS > 0, "must be 1 or more" ); +_Static_assert( B3_MAX_WORLDS < UINT16_MAX, "B3_MAX_WORLDS limit exceeded" ); +b3World b3_worlds[B3_MAX_WORLDS]; +b3AtomicInt b3_worldCount; +int b3_maxWorldCount; + +const b3HullData* b3AddHullToDatabase( b3World* world, const b3HullData* src ) +{ + b3HullMap* database = world->hullDatabase; + + // Compare by content so an unowned query hull finds the shared copy. + b3HullMap_itr itr = b3HullMap_get( database, src ); + if ( b3HullMap_is_end( itr ) == false ) + { + itr.data->val += 1; + return itr.data->key; + } + + b3HullData* owned = b3CloneHull( src ); + B3_ASSERT( owned != NULL ); + b3HullMap_insert( database, owned, 1 ); + return owned; +} + +const b3HullData* b3AddOwnedHullToDatabase( b3World* world, b3HullData* owned ) +{ + b3HullMap* database = world->hullDatabase; + + b3HullMap_itr itr = b3HullMap_get( database, owned ); + if ( b3HullMap_is_end( itr ) == false ) + { + itr.data->val += 1; + b3DestroyHull( owned ); + return itr.data->key; + } + + // Take ownership of input hull. + b3HullMap_insert( database, owned, 1 ); + return owned; +} + +void b3RemoveHullFromDatabase( b3World* world, const b3HullData* data ) +{ + b3HullMap* database = world->hullDatabase; + + b3HullMap_itr itr = b3HullMap_get( database, data ); + B3_ASSERT( b3HullMap_is_end( itr ) == false ); + + if ( --itr.data->val == 0 ) + { + // Erase through the iterator we already have so the lookup runs once. + b3HullData* owned = (b3HullData*)itr.data->key; + b3HullMap_erase_itr( database, itr ); + b3DestroyHull( owned ); + } +} + +b3World* b3GetUnlockedWorldFromId( b3WorldId id ) +{ + B3_ASSERT( 1 <= id.index1 && id.index1 <= B3_MAX_WORLDS ); + b3World* world = b3_worlds + ( id.index1 - 1 ); + B3_ASSERT( id.index1 == world->worldId + 1 ); + B3_ASSERT( id.generation == world->generation ); + + // A world accessed from an id should not be locked + if ( world->locked ) + { + B3_ASSERT( false ); + return NULL; + } + return world; +} + +b3World* b3GetWorldFromId( b3WorldId id ) +{ + B3_ASSERT( 1 <= id.index1 && id.index1 <= B3_MAX_WORLDS ); + b3World* world = b3_worlds + ( id.index1 - 1 ); + B3_ASSERT( id.index1 == world->worldId + 1 ); + B3_ASSERT( id.generation == world->generation ); + return world; +} + +b3World* b3GetWorld( int index ) +{ + B3_ASSERT( 0 <= index && index < B3_MAX_WORLDS ); + b3World* world = b3_worlds + index; + B3_ASSERT( world->worldId == index ); + return world; +} + +b3World* b3GetUnlockedWorld( int index ) +{ + B3_ASSERT( 0 <= index && index < B3_MAX_WORLDS ); + b3World* world = b3_worlds + index; + B3_ASSERT( world->worldId == index ); + if ( world->locked ) + { + B3_ASSERT( false ); + return NULL; + } + + return world; +} + +static void* b3DefaultAddTaskFcn( b3TaskCallback* task, void* taskContext, void* userContext, const char* name ) +{ + B3_UNUSED( userContext, name ); + task( taskContext ); + return NULL; +} + +static void b3DefaultFinishTaskFcn( void* userTask, void* userContext ) +{ + B3_UNUSED( userTask ); + B3_UNUSED( userContext ); +} + +static float b3DefaultFrictionCallback( float frictionA, uint64_t materialA, float frictionB, uint64_t materialB ) +{ + B3_UNUSED( materialA, materialB ); + return sqrtf( frictionA * frictionB ); +} + +static float b3DefaultRestitutionCallback( float restitutionA, uint64_t materialA, float restitutionB, uint64_t materialB ) +{ + B3_UNUSED( materialA, materialB ); + return b3MaxFloat( restitutionA, restitutionB ); +} + +static void b3CreateWorkerContexts( b3World* world ) +{ + b3Array_Resize( world->taskContexts, world->workerCount ); + b3Array_MemZero( world->taskContexts ); + + b3Array_Resize( world->sensorTaskContexts, world->workerCount ); + b3Array_MemZero( world->sensorTaskContexts ); + + for ( int i = 0; i < world->workerCount; ++i ) + { + world->taskContexts.data[i].arena = b3CreateArena( 128 * 1024 ); + b3Array_Reserve( world->taskContexts.data[i].sensorHits, 8 ); + world->taskContexts.data[i].contactStateBitSet = b3CreateBitSet( 1024 ); + world->taskContexts.data[i].hitEventBitSet = b3CreateBitSet( 1024 ); + world->taskContexts.data[i].hasHitEvents = false; + world->taskContexts.data[i].jointStateBitSet = b3CreateBitSet( 1024 ); + world->taskContexts.data[i].enlargedSimBitSet = b3CreateBitSet( 256 ); + world->taskContexts.data[i].awakeIslandBitSet = b3CreateBitSet( 256 ); + world->taskContexts.data[i].splitIslandId = B3_NULL_INDEX; + + world->sensorTaskContexts.data[i].eventBits = b3CreateBitSet( 128 ); + } +} + +static void b3DestroyWorkerContexts( b3World* world ) +{ + for ( int i = 0; i < world->workerCount; ++i ) + { + b3DestroyArena( &world->taskContexts.data[i].arena ); + b3Array_Destroy( world->taskContexts.data[i].sensorHits ); + b3DestroyBitSet( &world->taskContexts.data[i].contactStateBitSet ); + b3DestroyBitSet( &world->taskContexts.data[i].hitEventBitSet ); + b3DestroyBitSet( &world->taskContexts.data[i].jointStateBitSet ); + b3DestroyBitSet( &world->taskContexts.data[i].enlargedSimBitSet ); + b3DestroyBitSet( &world->taskContexts.data[i].awakeIslandBitSet ); + + b3DestroyBitSet( &world->sensorTaskContexts.data[i].eventBits ); + } + + b3Array_Destroy( world->taskContexts ); + b3Array_Destroy( world->sensorTaskContexts ); +} + +b3WorldId b3CreateWorld( const b3WorldDef* def ) +{ + B3_CHECK_DEF( def ); + + B3_ASSERT( B3_LINEAR_SLOP <= B3_MESH_REST_OFFSET ); + B3_ASSERT( B3_MESH_REST_OFFSET < B3_SPECULATIVE_DISTANCE ); + + int worldId = B3_NULL_INDEX; + for ( int i = 0; i < B3_MAX_WORLDS; ++i ) + { + if ( b3_worlds[i].inUse == false ) + { + worldId = i; + break; + } + } + + if ( worldId == B3_NULL_INDEX ) + { + b3Log( "B3_MAX_WORLDS of %d exceeded!!!", B3_MAX_WORLDS ); + B3_ASSERT( worldId != B3_NULL_INDEX ); + return (b3WorldId){ 0 }; + } + + // b3Log( "b3_lengthUnitsPerMeter = %g", b3_lengthUnitsPerMeter ); + + int oldCount = b3AtomicFetchAddInt( &b3_worldCount, 1 ); + b3_maxWorldCount = b3MaxInt( b3_maxWorldCount, oldCount + 1 ); + + // b3Log( "Created world %d", worldId ); + + b3InitializeContactRegisters(); + + b3World* world = b3_worlds + worldId; + uint16_t revision = world->generation; + + memset( world, 0, sizeof( b3World ) ); + + world->worldId = (uint16_t)worldId; + world->generation = revision; + world->inUse = true; + + world->stack = b3CreateStack( 2048 ); + + b3Array_Reserve( world->manifoldAllocators, 16 ); + world->manifoldAllocatorMutex = b3CreateMutex(); + + b3CreateBroadPhase( &world->broadPhase, &def->capacity ); + b3CreateGraph( &world->constraintGraph, 16 ); + + // pools + world->bodyIdPool = b3CreateIdPool(); + + int bodyCapacity = b3MaxInt( 16, def->capacity.staticBodyCount + def->capacity.dynamicBodyCount ); + b3Array_Reserve( world->bodies, bodyCapacity ); + b3Array_Reserve( world->solverSets, 8 ); + + // add empty static, active, and disabled body sets + world->solverSetIdPool = b3CreateIdPool(); + b3SolverSet set = { 0 }; + + // static set + set.setIndex = b3AllocId( &world->solverSetIdPool ); + b3Array_Push( world->solverSets, set ); + b3Array_Reserve( world->solverSets.data[b3_staticSet].bodySims, b3MaxInt( 16, def->capacity.staticBodyCount ) ); + B3_ASSERT( world->solverSets.data[b3_staticSet].setIndex == b3_staticSet ); + + // disabled set + set.setIndex = b3AllocId( &world->solverSetIdPool ); + b3Array_Push( world->solverSets, set ); + B3_ASSERT( world->solverSets.data[b3_disabledSet].setIndex == b3_disabledSet ); + + // awake set + set.setIndex = b3AllocId( &world->solverSetIdPool ); + b3Array_Push( world->solverSets, set ); + b3Array_Reserve( world->solverSets.data[b3_awakeSet].bodySims, b3MaxInt( 16, def->capacity.dynamicBodyCount ) ); + b3Array_Reserve( world->solverSets.data[b3_awakeSet].bodyStates, b3MaxInt( 16, def->capacity.dynamicBodyCount ) ); + b3Array_Reserve( world->solverSets.data[b3_awakeSet].contactIndices, b3MaxInt( 16, def->capacity.contactCount ) ); + B3_ASSERT( world->solverSets.data[b3_awakeSet].setIndex == b3_awakeSet ); + + world->shapeIdPool = b3CreateIdPool(); + + int shapeCapacity = b3MaxInt( 16, def->capacity.staticShapeCount + def->capacity.dynamicShapeCount ); + b3Array_Reserve( world->shapes, shapeCapacity ); + + world->hullDatabase = b3Alloc( sizeof( b3HullMap ) ); + b3HullMap_init( world->hullDatabase ); + + world->names = b3CreateNameCache(); + + world->contactIdPool = b3CreateIdPool(); + b3Array_Reserve( world->contacts, b3MaxInt( 16, def->capacity.contactCount ) ); + + world->jointIdPool = b3CreateIdPool(); + b3Array_Reserve( world->joints, 16 ); + + world->islandIdPool = b3CreateIdPool(); + b3Array_Reserve( world->islands, b3MaxInt( 16, def->capacity.dynamicBodyCount ) ); + + b3Array_Reserve( world->sensors, 4 ); + + b3Array_Reserve( world->bodyMoveEvents, 4 ); + b3Array_Reserve( world->sensorBeginEvents, 4 ); + b3Array_Reserve( world->sensorEndEvents[0], 4 ); + b3Array_Reserve( world->sensorEndEvents[1], 4 ); + b3Array_Reserve( world->contactBeginEvents, 4 ); + b3Array_Reserve( world->contactEndEvents[0], 4 ); + b3Array_Reserve( world->contactEndEvents[1], 4 ); + b3Array_Reserve( world->contactHitEvents, 4 ); + b3Array_Reserve( world->jointEvents, 4 ); + world->endEventArrayIndex = 0; + + world->stepIndex = 0; + world->splitIslandId = B3_NULL_INDEX; + world->activeTaskCount = 0; + world->taskCount = 0; + world->gravity = def->gravity; + world->hitEventThreshold = def->hitEventThreshold; + world->restitutionThreshold = def->restitutionThreshold; + world->maxLinearSpeed = def->maximumLinearSpeed; + world->contactSpeed = def->contactSpeed; + world->contactHertz = def->contactHertz; + world->contactDampingRatio = def->contactDampingRatio; + world->contactRecycleDistance = B3_CONTACT_RECYCLE_DISTANCE; + + if ( def->frictionCallback == NULL ) + { + world->frictionCallback = b3DefaultFrictionCallback; + } + else + { + world->frictionCallback = def->frictionCallback; + } + + if ( def->restitutionCallback == NULL ) + { + world->restitutionCallback = b3DefaultRestitutionCallback; + } + else + { + world->restitutionCallback = def->restitutionCallback; + } + + world->enableSleep = def->enableSleep; + world->locked = false; + world->enableWarmStarting = true; + world->enableContinuous = def->enableContinuous; + world->enableSpeculative = true; + world->userTreeTask = NULL; + world->userData = def->userData; + + if ( def->workerCount > 0 && def->enqueueTask != NULL && def->finishTask != NULL ) + { + // External task system + world->workerCount = b3MinInt( def->workerCount, B3_MAX_WORKERS ); + world->enqueueTaskFcn = def->enqueueTask; + world->finishTaskFcn = def->finishTask; + world->userTaskContext = def->userTaskContext; + world->scheduler = NULL; + } + else if ( def->workerCount > 1 ) + { + // Built-in scheduler + world->workerCount = b3MinInt( def->workerCount, B3_MAX_WORKERS ); + world->scheduler = b3CreateScheduler( world->workerCount ); + world->enqueueTaskFcn = b3SchedulerEnqueueTask; + world->finishTaskFcn = b3SchedulerFinishTask; + world->userTaskContext = world->scheduler; + } + else + { + // Serial fallback + world->workerCount = 1; + world->enqueueTaskFcn = b3DefaultAddTaskFcn; + world->finishTaskFcn = b3DefaultFinishTaskFcn; + world->userTaskContext = NULL; + world->scheduler = NULL; + } + + b3CreateWorkerContexts( world ); + + world->debugBodySet = b3CreateBitSet( 256 ); + world->debugJointSet = b3CreateBitSet( 256 ); + world->debugContactSet = b3CreateBitSet( 256 ); + world->debugIslandSet = b3CreateBitSet( 256 ); + world->createDebugShape = def->createDebugShape; + world->destroyDebugShape = def->destroyDebugShape; + world->userDebugShapeContext = def->userDebugShapeContext; + + // add one to worldId so that 0 represents a null b3WorldId + return (b3WorldId){ (uint16_t)( worldId + 1 ), world->generation }; +} + +void b3DestroyWorld( b3WorldId worldId ) +{ + b3AtomicFetchAddInt( &b3_worldCount, -1 ); + + b3World* world = b3GetUnlockedWorldFromId( worldId ); + if ( world == NULL ) + { + return; + } + + world->locked = true; + + // Detach any recording before teardown. The user owns and frees the recording buffer. + b3StopRecordingInternal( world ); + + if ( world->scheduler != NULL ) + { + b3DestroyScheduler( world->scheduler ); + world->scheduler = NULL; + } + + b3DestroyBitSet( &world->debugBodySet ); + b3DestroyBitSet( &world->debugJointSet ); + b3DestroyBitSet( &world->debugContactSet ); + b3DestroyBitSet( &world->debugIslandSet ); + + b3DestroyWorkerContexts( world ); + + b3Array_Destroy( world->bodyMoveEvents ); + b3Array_Destroy( world->sensorBeginEvents ); + b3Array_Destroy( world->sensorEndEvents[0] ); + b3Array_Destroy( world->sensorEndEvents[1] ); + b3Array_Destroy( world->contactBeginEvents ); + b3Array_Destroy( world->contactEndEvents[0] ); + b3Array_Destroy( world->contactEndEvents[1] ); + b3Array_Destroy( world->contactHitEvents ); + b3Array_Destroy( world->jointEvents ); + + int sensorCount = world->sensors.count; + for ( int i = 0; i < sensorCount; ++i ) + { + b3Array_Destroy( world->sensors.data[i].hits ); + b3Array_Destroy( world->sensors.data[i].overlaps1 ); + b3Array_Destroy( world->sensors.data[i].overlaps2 ); + } + + b3Array_Destroy( world->sensors ); + b3Array_Destroy( world->bodies ); + + int shapeCapacity = world->shapes.count; + b3Shape* shapes = world->shapes.data; + for ( int i = 0; i < shapeCapacity; ++i ) + { + b3Shape* shape = shapes + i; + if ( shape->id != B3_NULL_INDEX ) + { + b3DestroyShapeAllocations( world, shapes + i ); + } + } + + int contactCapacity = world->contacts.count; + b3Contact* contacts = world->contacts.data; + for ( int i = 0; i < contactCapacity; ++i ) + { + b3Contact* contact = contacts + i; + if ( contact->contactId != B3_NULL_INDEX ) + { + if ( contact->flags & b3_simMeshContact ) + { + b3Array_Destroy( contact->meshContact.triangleCache ); + } + } + } + + // Destroying every shape above released all hull references, so the database is empty. + B3_ASSERT( b3HullMap_size( (b3HullMap*)world->hullDatabase ) == 0 ); + b3HullMap_cleanup( world->hullDatabase ); + b3Free( world->hullDatabase, sizeof( b3HullMap ) ); + world->hullDatabase = NULL; + + b3DestroyNameCache( &world->names ); + + b3Array_Destroy( world->shapes ); + b3Array_Destroy( world->contacts ); + b3Array_Destroy( world->joints ); + + for ( int i = 0; i < world->islands.count; ++i ) + { + b3Array_Destroy( world->islands.data[i].bodies ); + b3Array_Destroy( world->islands.data[i].contacts ); + b3Array_Destroy( world->islands.data[i].joints ); + } + b3Array_Destroy( world->islands ); + + // Destroy solver sets + int setCapacity = world->solverSets.count; + for ( int i = 0; i < setCapacity; ++i ) + { + b3SolverSet* set = world->solverSets.data + i; + if ( set->setIndex != B3_NULL_INDEX ) + { + b3DestroySolverSet( world, i ); + } + } + + b3Array_Destroy( world->solverSets ); + + b3DestroyGraph( &world->constraintGraph ); + b3DestroyBroadPhase( &world->broadPhase ); + + b3DestroyIdPool( &world->bodyIdPool ); + b3DestroyIdPool( &world->shapeIdPool ); + b3DestroyIdPool( &world->contactIdPool ); + b3DestroyIdPool( &world->jointIdPool ); + b3DestroyIdPool( &world->islandIdPool ); + b3DestroyIdPool( &world->solverSetIdPool ); + + for ( int i = 0; i < world->manifoldAllocators.count; ++i ) + { + b3DestroyBlockAllocator( world->manifoldAllocators.data + i ); + } + b3Array_Destroy( world->manifoldAllocators ); + b3DestroyMutex( world->manifoldAllocatorMutex ); + + b3DestroyStack( &world->stack ); + + // Wipe world but preserve generation + uint16_t generation = world->generation; + memset( world, 0, sizeof( b3World ) ); + world->generation = generation + 1; + + // b3Log( "Destroyed world %d", worldId.index1 - 1 ); +} + +int b3GetWorldCount( void ) +{ + return b3AtomicLoadInt( &b3_worldCount ); +} + +int b3GetMaxWorldCount( void ) +{ + return b3_maxWorldCount; +} + +// Issues T0 prefetches across the cache lines of a b3Contact (216 B / 4 lines). +// Used to hide the random-access latency of contact lookups while we work on an +// earlier index. +static inline void b3PrefetchContact( const b3Contact* contact ) +{ + const char* p = (const char*)contact; + b3Prefetch( p ); + b3Prefetch( p + 64 ); + b3Prefetch( p + 128 ); + b3Prefetch( p + 192 ); +} + +static void b3CollideTask( int startIndex, int endIndex, int workerIndex, void* context ) +{ + b3TracyCZoneNC( collide_task, "Collide Task", b3_colorDodgerBlue, true ); + + b3StepContext* stepContext = (b3StepContext*)context; + b3World* world = stepContext->world; + b3ConstraintGraph* graph = &world->constraintGraph; + b3TaskContext* taskContext = world->taskContexts.data + workerIndex; + int* contactIndices = stepContext->awakeContactIndices; + b3Contact* contacts = world->contacts.data; + b3Shape* shapes = world->shapes.data; + b3Body* bodies = world->bodies.data; + b3BodySim* awakeSims = world->solverSets.data[b3_awakeSet].bodySims.data; + b3BodySim* staticSims = world->solverSets.data[b3_staticSet].bodySims.data; + + B3_ASSERT( startIndex < endIndex ); + + float recycleDistance = world->contactRecycleDistance; + float speculativeDistance = B3_SPECULATIVE_DISTANCE; + float recycleDistanceNonTouching = b3MinFloat( recycleDistance, speculativeDistance ); + + // Prefetch contact[i + contactPrefetchDistance] each iteration so the random + // 216 B contact load lands in L1 by the time we reach it. Distance picked to + // cover ~200 cycles of memory latency without overshooting the L1 working set. + const int contactPrefetchDistance = 4; + int prefetchEnd = endIndex - contactPrefetchDistance; + + for ( int i = startIndex; i < endIndex; ++i ) + { + if ( i < prefetchEnd ) + { + b3PrefetchContact( contacts + contactIndices[i + contactPrefetchDistance] ); + } + + int contactIndex = contactIndices[i]; + B3_ASSERT( contactIndex < world->contacts.count ); + + b3Contact* contact = contacts + contactIndex; + B3_VALIDATE( contact->contactId == contactIndex ); + + b3Shape* shapeA = shapes + contact->shapeIdA; + b3Shape* shapeB = shapes + contact->shapeIdB; + + // Do proxies still overlap? + bool overlap = b3AABB_Overlaps( shapeA->fatAABB, shapeB->fatAABB ); + if ( overlap == false ) + { + // This contact will be destroyed + contact->flags |= b3_simDisjoint; + contact->flags &= ~b3_simTouchingFlag; + b3SetBit( &taskContext->contactStateBitSet, contactIndex ); + continue; + } + + // Update contact respecting shape/body order (A,B). Bodies behind awake-set + // contacts are always either awake or static - inline b3GetBodySim with that + // invariant to skip the cross-TU call and per-call solverSets indirection. + b3Body* bodyA = bodies + shapeA->bodyId; + b3Body* bodyB = bodies + shapeB->bodyId; + bool isStaticA = bodyA->type == b3_staticBody; + bool isStaticB = bodyB->type == b3_staticBody; + bool wasTouching = ( contact->flags & b3_simTouchingFlag ); + bool isMeshContact = ( contact->flags & b3_simMeshContact ); + b3BodySim* bodySimA; + b3BodySim* bodySimB; + if ( wasTouching ) + { + B3_ASSERT( bodyA->setIndex == b3_awakeSet || bodyA->setIndex == b3_staticSet ); + B3_ASSERT( bodyB->setIndex == b3_awakeSet || bodyB->setIndex == b3_staticSet ); + bodySimA = ( isStaticA ? staticSims : awakeSims ) + bodyA->localIndex; + bodySimB = ( isStaticB ? staticSims : awakeSims ) + bodyB->localIndex; + } + else + { + // There can be non-touching contacts between awake bodies and sleeping bodies. + { + b3SolverSet* set = b3Array_Get( world->solverSets, bodyA->setIndex ); + bodySimA = b3Array_Get( set->bodySims, bodyA->localIndex ); + } + { + b3SolverSet* set = b3Array_Get( world->solverSets, bodyB->setIndex ); + bodySimB = b3Array_Get( set->bodySims, bodyB->localIndex ); + } + } + + b3WorldTransform transformA = bodySimA->transform; + b3WorldTransform transformB = bodySimB->transform; + + bool isFast = ( bodySimA->flags & b3_isFast ) || ( bodySimB->flags & b3_isFast ); + + // These are used by the contact solver. If the contact is between an awake body + // and a sleeping body and the contact begins to touch, the these will be invalid + // but fixed when linked in the constraint graph. + contact->bodySimIndexA = isStaticA ? B3_NULL_INDEX : bodyA->localIndex; + contact->bodySimIndexB = isStaticB ? B3_NULL_INDEX : bodyB->localIndex; + float recycleTolerance = wasTouching ? recycleDistance : recycleDistanceNonTouching; + + // Contact recycling optimization. Please cite this library if you use this optimization. + // This is inspired by persistent contact manifolds used in some physics engines, such as PhysX. + // However, this allows larger relative motion and has fewer tuning parameters (just one). + if ( ( isFast == false || isMeshContact == false ) && recycleDistance > 0.0f && + ( contact->flags & b3_relativeTransformValid ) && ( contact->flags & b3_contactRecycleFlag ) ) + { + float angleA = b3DotQuat( transformA.q, contact->cachedRotationA ); + float angleB = b3DotQuat( transformB.q, contact->cachedRotationB ); + float angularDistance = b3MinFloat( angleA * angleA, angleB * angleB ); + + b3Transform xf = b3InvMulWorldTransforms( transformA, transformB ); + b3Transform xfc = contact->cachedRelativePose; + b3Vec3 maxExtentA = isStaticA ? b3Vec3_zero : bodySimA->maxExtent; + b3Vec3 maxExtentB = isStaticB ? b3Vec3_zero : bodySimB->maxExtent; + b3Vec3 maxExtent = b3Max( maxExtentA, maxExtentB ); + + // Variation of Conservative Advancement + // distance + 2 * length(modified_cross(|qr.v|, maxExtent)) < recycleTolerance. + // 2*|qr.v| == 2*|sin(theta/2)| ~= theta for small angles. + float distSquared = b3DistanceSquared( xf.p, xfc.p ); + + if ( angularDistance > B3_CONTACT_RECYCLE_ANGULAR_DISTANCE && distSquared < recycleTolerance * recycleTolerance ) + { + float distance = sqrtf( distSquared ); + float slack = recycleTolerance - distance; + + // qr = inv( inv(qA0) * qB0 ) * inv(qA) * qB + // = inv(qB0) * qA0 * inv(qA) * qB + // Suppose A is static + // qr = inv(qB0) * qA0 * inv(qA0) * qB + // = inv(qB0) * qB + // qB = qB0 * qr + // Therefore qr is associated with the local angular velocity of body B when A is static. + b3Quat qr = b3InvMulQuat( xfc.q, xf.q ); + b3Vec3 arc = b3ModifiedCross( b3Abs( qr.v ), maxExtent ); + + float arcSq = 4.0f * b3LengthSquared( arc ); + if ( arcSq < slack * slack ) + { + b3Quat dqA = b3MulQuat( transformA.q, b3Conjugate( contact->cachedRotationA ) ); + b3Quat dqB = b3MulQuat( transformB.q, b3Conjugate( contact->cachedRotationB ) ); + b3Matrix3 matrixA = b3MakeMatrixFromQuat( dqA ); + b3Matrix3 matrixB = b3MakeMatrixFromQuat( dqB ); + + // Minimize round-off + b3Vec3 dc = b3SubPos( bodySimB->center, bodySimA->center ); + + int manifoldCount = contact->manifoldCount; + for ( int manifoldIndex = 0; manifoldIndex < manifoldCount; ++manifoldIndex ) + { + b3Manifold* manifold = contact->manifolds + manifoldIndex; + b3Vec3 normal = manifold->normal; + + int pointCount = manifold->pointCount; + for ( int pointIndex = 0; pointIndex < pointCount; ++pointIndex ) + { + // Keep anchors but update separation, same as sub-stepping. This eliminates jitter. + b3ManifoldPoint* mp = manifold->points + pointIndex; + b3Vec3 rA = b3MulMV( matrixA, mp->anchorA ); + b3Vec3 rB = b3MulMV( matrixB, mp->anchorB ); + b3Vec3 dp = b3Add( dc, b3Sub( rB, rA ) ); + mp->separation = mp->baseSeparation + b3Dot( dp, normal ); + mp->persisted = true; + } + } + + // Diagnostics + taskContext->recycledContactCount += 1; + int bucketIndex = b3MinInt( manifoldCount, B3_CONTACT_MANIFOLD_COUNT_BUCKETS - 1 ); + if ( bucketIndex > 0 ) + { + taskContext->manifoldCounts[bucketIndex - 1] += 1; + } + + // Contact is recycled. This also skips updating other aspects of the contact + // such as material parameters. + continue; + } + } + } + + // Caching for contact recycling. + contact->cachedRotationA = transformA.q; + contact->cachedRotationB = transformB.q; + contact->cachedRelativePose = b3InvMulWorldTransforms( transformA, transformB ); + contact->flags |= b3_relativeTransformValid; + + // This updates solid contacts + bool touching = b3UpdateContact( world, workerIndex, contact, shapeA, bodySimA->localCenter, transformA, shapeB, + bodySimB->localCenter, transformB, isFast, taskContext->arena ); + + int bucketIndex = b3MinInt( contact->manifoldCount, B3_CONTACT_MANIFOLD_COUNT_BUCKETS - 1 ); + if ( bucketIndex > 0 ) + { + taskContext->manifoldCounts[bucketIndex - 1] += 1; + } + + // Update the mesh contact spec + if ( touching == true && wasTouching == true && ( contact->flags & b3_simMeshContact ) ) + { + B3_ASSERT( contact->colorIndex != B3_NULL_INDEX ); + B3_ASSERT( 0 <= contact->colorIndex && contact->colorIndex < B3_GRAPH_COLOR_COUNT ); + b3GraphColor* color = graph->colors + contact->colorIndex; + b3ContactSpec* spec = b3Array_Get( color->contacts, contact->localIndex ); + spec->manifoldCount = (uint16_t)contact->manifoldCount; + } + + // State changes that affect island connectivity. Also affects contact events. + if ( touching == true && wasTouching == false ) + { + contact->flags |= b3_simStartedTouching; + b3SetBit( &taskContext->contactStateBitSet, contactIndex ); + } + else if ( touching == false && wasTouching == true ) + { + contact->flags |= b3_simStoppedTouching; + b3SetBit( &taskContext->contactStateBitSet, contactIndex ); + } + + for ( int manifoldIndex = 0; manifoldIndex < contact->manifoldCount; ++manifoldIndex ) + { + b3Manifold* manifold = contact->manifolds + manifoldIndex; + for ( int pointIndex = 0; pointIndex < manifold->pointCount; ++pointIndex ) + { + // Cache separation + b3ManifoldPoint* mp = manifold->points + pointIndex; + mp->baseSeparation = mp->separation; + } + } + } + + b3TracyCZoneEnd( collide_task ); +} + +static void b3AddNonTouchingContact( b3World* world, b3Contact* contact ) +{ + B3_ASSERT( contact->setIndex == b3_awakeSet ); + b3SolverSet* set = b3Array_Get( world->solverSets, b3_awakeSet ); + contact->colorIndex = B3_NULL_INDEX; + contact->localIndex = set->contactIndices.count; + contact->bodySimIndexA = B3_NULL_INDEX; + contact->bodySimIndexB = B3_NULL_INDEX; + b3Array_Push( set->contactIndices, contact->contactId ); +} + +static void b3RemoveNonTouchingContact( b3World* world, int setIndex, int localIndex ) +{ + b3SolverSet* set = b3Array_Get( world->solverSets, setIndex ); + 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 ); + B3_ASSERT( movedContact->setIndex == setIndex ); + B3_ASSERT( movedContact->colorIndex == B3_NULL_INDEX ); + B3_ASSERT( movedContact->localIndex == movedIndex ); + movedContact->localIndex = localIndex; + } +} + +// Narrow-phase collision +static void b3Collide( b3StepContext* context ) +{ + b3World* world = context->world; + + B3_ASSERT( world->workerCount > 0 ); + + b3TracyCZoneNC( collide, "Collide", b3_colorDarkOrchid, true ); + + // Gather contacts from all the graph colors into a single array for easier parallel-for + int touchingCount = 0; + + b3GraphColor* graphColors = world->constraintGraph.colors; + for ( int i = 0; i < B3_GRAPH_COLOR_COUNT; ++i ) + { + touchingCount += graphColors[i].convexContacts.count + graphColors[i].contacts.count; + } + + b3SolverSet* awakeSet = b3Array_Get( world->solverSets, b3_awakeSet ); + int nonTouchingCount = awakeSet->contactIndices.count; + + int contactCount = touchingCount + nonTouchingCount; + + if ( contactCount == 0 ) + { + b3TracyCZoneEnd( collide ); + return; + } + + int* contactIndices = (int*)b3StackAlloc( &world->stack, contactCount * sizeof( int ), "contact indices" ); + + int contactIndex = 0; + for ( int i = 0; i < B3_GRAPH_COLOR_COUNT; ++i ) + { + b3GraphColor* color = graphColors + i; + int count = color->convexContacts.count; + for ( int j = 0; j < count; ++j ) + { + contactIndices[contactIndex] = color->convexContacts.data[j]; + contactIndex += 1; + } + + count = color->contacts.count; + for ( int j = 0; j < count; ++j ) + { + contactIndices[contactIndex] = color->contacts.data[j].contactId; + contactIndex += 1; + } + } + + B3_ASSERT( contactIndex == touchingCount ); + + if ( nonTouchingCount > 0 ) + { + int* nonTouchingIndices = awakeSet->contactIndices.data; + memcpy( contactIndices + touchingCount, nonTouchingIndices, nonTouchingCount * sizeof( int ) ); + } + + context->awakeContactIndices = contactIndices; + + // Contact bit set on ids because contact pointers are unstable as they move between touching and not touching. + int contactIdCapacity = b3GetIdCapacity( &world->contactIdPool ); + for ( int i = 0; i < world->workerCount; ++i ) + { + b3TaskContext* taskContext = world->taskContexts.data + i; + b3SetBitCountAndClear( &taskContext->contactStateBitSet, contactIdCapacity ); + taskContext->satCallCount = 0; + taskContext->satCacheHitCount = 0; + taskContext->recycledContactCount = 0; + memset( taskContext->manifoldCounts, 0, sizeof( taskContext->manifoldCounts ) ); + } + + // Task should take at least 40us on a 4GHz CPU (10K cycles) + int minRange = 20; + b3ParallelFor( world, b3CollideTask, contactCount, minRange, context, "collide" ); + + b3StackFree( &world->stack, contactIndices ); + context->awakeContactIndices = NULL; + contactIndices = NULL; + + // Serially update contact state + // todo bring this zone together with island merge + b3TracyCZoneNC( contact_state, "Contact State", b3_colorLightSlateGray, true ); + + int satMultiplier = context->dt > 0.0f ? 1 : 0; + + // Bitwise OR all contact bits + b3BitSet* bitSet = &world->taskContexts.data[0].contactStateBitSet; + world->satCallCount = satMultiplier * world->taskContexts.data[0].satCallCount; + world->satCacheHitCount = satMultiplier * world->taskContexts.data[0].satCacheHitCount; + memcpy( world->manifoldCounts, world->taskContexts.data[0].manifoldCounts, + B3_CONTACT_MANIFOLD_COUNT_BUCKETS * sizeof( int ) ); + + for ( int i = 1; i < world->workerCount; ++i ) + { + b3InPlaceUnion( bitSet, &world->taskContexts.data[i].contactStateBitSet ); + world->satCallCount += world->taskContexts.data[i].satCallCount; + world->satCacheHitCount += world->taskContexts.data[i].satCacheHitCount; + for ( int j = 0; j < B3_CONTACT_MANIFOLD_COUNT_BUCKETS; ++j ) + { + world->manifoldCounts[j] += world->taskContexts.data[i].manifoldCounts[j]; + } + } + + // Release per-step overflow blocks and grow the backing capacity if last + // step's demand exceeded it. Contact processing is the only consumer of + // these arenas and is finished by this point. + for ( int i = 0; i < world->workerCount; ++i ) + { + b3ArenaSync( &world->taskContexts.data[i].arena ); + } + + int endEventArrayIndex = world->endEventArrayIndex; + + const b3Shape* shapes = world->shapes.data; + uint16_t worldId = world->worldId; + + // Process contact state changes. Iterate over set bits + for ( uint32_t k = 0; k < bitSet->blockCount; ++k ) + { + uint64_t bits = bitSet->bits[k]; + while ( bits != 0 ) + { + uint32_t ctz = b3CTZ64( bits ); + int contactId = (int)( 64 * k + ctz ); + + b3Contact* contact = b3Array_Get( world->contacts, contactId ); + B3_ASSERT( contact->setIndex == b3_awakeSet ); + + const b3Shape* shapeA = shapes + contact->shapeIdA; + const b3Shape* shapeB = shapes + contact->shapeIdB; + b3ShapeId shapeIdA = { shapeA->id + 1, worldId, shapeA->generation }; + b3ShapeId shapeIdB = { shapeB->id + 1, worldId, shapeB->generation }; + b3ContactId contactFullId = { + .index1 = contactId + 1, + .world0 = worldId, + .padding = 0, + .generation = contact->generation, + }; + uint32_t flags = contact->flags; + + if ( flags & b3_simDisjoint ) + { + // Bounding boxes no longer overlap + b3DestroyContact( world, contact, false ); + contact = NULL; + } + else if ( flags & b3_simStartedTouching ) + { + B3_ASSERT( contact->islandId == B3_NULL_INDEX ); + + if ( flags & b3_contactEnableContactEvents ) + { + b3ContactBeginTouchEvent event = { shapeIdA, shapeIdB, contactFullId }; + b3Array_Push( world->contactBeginEvents, event ); + } + + B3_ASSERT( contact->manifoldCount > 0 ); + B3_ASSERT( contact->setIndex == b3_awakeSet ); + + // Link first because this wakes colliding bodies and ensures the body sims + // are in the correct place. + contact->flags &= ~b3_simStartedTouching; + contact->flags |= b3_contactTouchingFlag; + b3LinkContact( world, contact ); + + // Make sure these didn't change + B3_ASSERT( contact->colorIndex == B3_NULL_INDEX ); + + // Contact sim pointer may have become orphaned due to awake set growth, + // so I just need to refresh it. + + int oldLocalIndex = contact->localIndex; + + b3AddContactToGraph( world, contact ); + b3RemoveNonTouchingContact( world, b3_awakeSet, oldLocalIndex ); + } + else if ( flags & b3_simStoppedTouching ) + { + contact->flags &= ~b3_simStoppedTouching; + contact->flags &= ~b3_contactTouchingFlag; + + if ( contact->flags & b3_contactEnableContactEvents ) + { + b3ContactEndTouchEvent event = { shapeIdA, shapeIdB, contactFullId }; + b3Array_Push( world->contactEndEvents[endEventArrayIndex], event ); + } + + B3_ASSERT( contact->manifoldCount == 0 ); + + // Cache these here for the remove below + int colorIndex = contact->colorIndex; + int localIndex = contact->localIndex; + + b3UnlinkContact( world, contact ); + int bodyIdA = contact->edges[0].bodyId; + int bodyIdB = contact->edges[1].bodyId; + + b3AddNonTouchingContact( world, contact ); + + bool isMeshContact = contact->flags & b3_simMeshContact; + b3RemoveContactFromGraph( world, bodyIdA, bodyIdB, colorIndex, localIndex, isMeshContact ); + contact = NULL; + } + + // Clear the smallest set bit + bits = bits & ( bits - 1 ); + } + } + + b3ValidateSolverSets( world ); + b3ValidateContacts( world ); + + b3TracyCZoneEnd( contact_state ); + b3TracyCZoneEnd( collide ); +} + +void b3World_Step( b3WorldId worldId, float timeStep, int subStepCount ) +{ + b3World* world = b3GetUnlockedWorldFromId( worldId ); + if ( world == NULL ) + { + return; + } + + B3_REC( world, Step, worldId, timeStep, subStepCount ); + + world->locked = true; + + b3TracyCZoneNC( world_step, "Step", b3_colorBox2DGreen, true ); + + // Clear debug buffers + for ( int i = 0; i < world->workerCount; ++i ) + { + world->taskContexts.data[i].pointCount = 0; + world->taskContexts.data[i].lineCount = 0; + } + + // Prepare to capture events + // Ensure user does not access stale data if there is an early return + b3Array_Clear( world->bodyMoveEvents ); + b3Array_Clear( world->sensorBeginEvents ); + b3Array_Clear( world->contactBeginEvents ); + b3Array_Clear( world->contactHitEvents ); + b3Array_Clear( world->jointEvents ); + + world->profile = (b3Profile){ 0 }; + + world->activeTaskCount = 0; + world->taskCount = 0; + + if ( world->scheduler != NULL ) + { + b3ResetScheduler( world->scheduler ); + } + + uint64_t stepTicks = b3GetTicks(); + + { + b3Capacity* c = &world->maxCapacity; + c->staticShapeCount = b3MaxInt( c->staticShapeCount, world->broadPhase.trees[b3_staticBody].proxyCount ); + c->dynamicShapeCount = b3MaxInt( c->dynamicShapeCount, world->broadPhase.trees[b3_dynamicBody].proxyCount ); + + int staticBodyCount = world->solverSets.data[b3_staticSet].bodySims.count; + c->staticBodyCount = b3MaxInt( c->staticBodyCount, staticBodyCount ); + + // this includes kinematic bodies + int totalBodyCount = b3GetIdCount( &world->bodyIdPool ); + c->dynamicBodyCount = b3MaxInt( c->dynamicBodyCount, totalBodyCount - staticBodyCount ); + + int totalContactCount = b3GetIdCount( &world->contactIdPool ); + c->contactCount = b3MaxInt( c->contactCount, totalContactCount ); + } + + // Update collision pairs and create contacts + { + uint64_t pairTicks = b3GetTicks(); + b3UpdateBroadPhasePairs( world ); + world->profile.pairs = b3GetMilliseconds( pairTicks ); + } + + b3SolverSet* awakeSet = b3Array_Get( world->solverSets, b3_awakeSet ); + + b3StepContext context = { 0 }; + context.world = world; + context.states = awakeSet->bodyStates.data; + context.dt = timeStep; + context.subStepCount = b3MaxInt( 1, subStepCount ); + + if ( timeStep > 0.0f ) + { + context.inv_dt = 1.0f / timeStep; + context.h = timeStep / context.subStepCount; + context.inv_h = context.subStepCount * context.inv_dt; + } + else + { + context.inv_dt = 0.0f; + context.h = 0.0f; + context.inv_h = 0.0f; + } + + world->inv_h = context.inv_h; + world->inv_dt = context.inv_dt; + + // Hertz values get reduced for large time steps + float contactHertz = b3MinFloat( world->contactHertz, 0.125f * context.inv_h ); + context.contactSoftness = b3MakeSoft( contactHertz, world->contactDampingRatio, context.h ); + context.staticSoftness = b3MakeSoft( 2.0f * contactHertz, 0.5f * world->contactDampingRatio, context.h ); + + context.restitutionThreshold = world->restitutionThreshold; + context.maxLinearVelocity = world->maxLinearSpeed; + context.enableWarmStarting = world->enableWarmStarting; + + // Narrow phase : update contacts + { + uint64_t collideTicks = b3GetTicks(); + b3Collide( &context ); + world->profile.collide = b3GetMilliseconds( collideTicks ); + } + + // Integrate velocities, solve velocity constraints, and integrate positions. + if ( timeStep > 0.0f ) + { + uint64_t solveTicks = b3GetTicks(); + b3Solve( world, &context ); + world->profile.solve = b3GetMilliseconds( solveTicks ); + } + + // Finish the tree task in case b3Solve didn't finish it + if ( world->userTreeTask ) + { + world->finishTaskFcn( world->userTreeTask, world->userTaskContext ); + world->userTreeTask = NULL; + world->activeTaskCount -= 1; + } + + // Update sensors + { + uint64_t sensorTicks = b3GetTicks(); + b3OverlapSensors( world ); + world->profile.sensors = b3GetMilliseconds( sensorTicks ); + } + + world->profile.step = b3GetMilliseconds( stepTicks ); + + B3_ASSERT( world->stack.allocation == 0 ); + + // Ensure stack is large enough + b3GrowStack( &world->stack ); + + // Make sure all tasks that were started were also finished + B3_ASSERT( world->activeTaskCount == 0 ); + + // Swap end event array buffers + world->endEventArrayIndex = 1 - world->endEventArrayIndex; + b3Array_Clear( world->sensorEndEvents[world->endEventArrayIndex] ); + b3Array_Clear( world->contactEndEvents[world->endEventArrayIndex] ); + world->locked = false; + + if ( world->recording != NULL ) + { + uint64_t hash = b3HashWorldState( world ); + b3RecArgs_StateHash stateHash = { worldId, hash }; + b3RecWrite_StateHash( world->recording, &stateHash ); + + // Fold this step's world bounds into the recording so a viewer can frame the whole motion. + b3AABB worldBounds = { 0 }; + bool haveBounds = false; + for ( int i = 0; i < b3_bodyTypeCount; ++i ) + { + b3DynamicTree* tree = world->broadPhase.trees + i; + if ( b3DynamicTree_GetProxyCount( tree ) == 0 ) + { + continue; + } + b3AABB bounds = b3DynamicTree_GetRootBounds( tree ); + worldBounds = haveBounds ? b3AABB_Union( worldBounds, bounds ) : bounds; + haveBounds = true; + } + if ( haveBounds ) + { + b3RecAccumulateBounds( world->recording, worldBounds ); + } + } + + b3TracyCZoneEnd( world_step ); + b3TracyCFrame; +} + +typedef struct DrawContext +{ + b3World* world; + b3DebugDraw* draw; +} DrawContext; + +static bool DrawQueryCallback( int proxyId, uint64_t userData, void* context ) +{ + int shapeId = (int)userData; + + B3_UNUSED( proxyId ); + + struct DrawContext* drawContext = (DrawContext*)context; + b3World* world = drawContext->world; + b3DebugDraw* draw = drawContext->draw; + + b3Shape* shape = b3Array_Get( world->shapes, shapeId ); + B3_ASSERT( shape->id == shapeId ); + + b3SetBit( &world->debugBodySet, shape->bodyId ); + + if ( draw->drawShapes ) + { + b3Body* body = b3Array_Get( world->bodies, shape->bodyId ); + b3BodySim* bodySim = b3GetBodySim( world, body ); + + b3HexColor color; + + const b3SurfaceMaterial* surfaceMaterial = b3GetShapeMaterials( shape ); + if ( surfaceMaterial[0].customColor != 0 ) + { + // May already carry a packed material preset, pass through unchanged + color = (b3HexColor)surfaceMaterial[0].customColor; + } + else + { + // Hue carries the state, material carries its energy. Calm matte for the + // resting masses, glossy for fast bodies, metallic for the driven kinematic. + // Diagnostic states keep a saturated hue and the default material so they pop. + b3HexColor rgb; + b3DebugMaterial material = b3_debugMaterialDefault; + + if ( body->type == b3_dynamicBody && body->mass == 0.0f ) + { + // Bad body + rgb = b3_colorRed; + } + else if ( body->setIndex == b3_disabledSet ) + { + rgb = b3_colorSlateGray; + } + else if ( shape->sensorIndex != B3_NULL_INDEX ) + { + rgb = b3_colorWheat; + } + else if ( body->flags & b3_hadTimeOfImpact ) + { + rgb = b3_colorLime; + } + else if ( ( bodySim->flags & b3_isBullet ) && body->setIndex == b3_awakeSet ) + { + rgb = b3_colorTurquoise; + } + else if ( body->flags & b3_isSpeedCapped ) + { + rgb = b3_colorYellow; + } + else if ( bodySim->flags & b3_isFast ) + { + rgb = b3_colorOrange; + material = b3_debugMaterialGlossy; + } + else if ( body->type == b3_staticBody ) + { + rgb = b3_colorDarkGray; + material = b3_debugMaterialMatte; + } + else if ( body->type == b3_kinematicBody ) + { + if ( body->setIndex == b3_awakeSet ) + { + rgb = b3_colorSteelBlue; + material = b3_debugMaterialMetallic; + } + else + { + rgb = b3_colorLightSteelBlue; + material = b3_debugMaterialMatte; + } + } + else if ( body->setIndex == b3_awakeSet ) + { + rgb = b3_colorTan; + material = b3_debugMaterialSoft; + } + else + { + rgb = b3_colorLightSlateGray; + material = b3_debugMaterialDead; + } + + color = (b3HexColor)b3MakeDebugColor( rgb, material ); + } + + if ( shape->userShape == NULL && world->createDebugShape != NULL ) + { + b3DebugShape debugShape = { 0 }; + debugShape.shapeId = (b3ShapeId){ + .world0 = world->worldId, + .index1 = shapeId + 1, + .generation = shape->generation, + }; + debugShape.type = shape->type; + + switch ( shape->type ) + { + case b3_capsuleShape: + debugShape.capsule = &shape->capsule; + shape->userShape = world->createDebugShape( &debugShape, world->userDebugShapeContext ); + break; + case b3_compoundShape: + debugShape.compound = shape->compound; + shape->userShape = world->createDebugShape( &debugShape, world->userDebugShapeContext ); + break; + case b3_heightShape: + debugShape.heightField = shape->heightField; + shape->userShape = world->createDebugShape( &debugShape, world->userDebugShapeContext ); + break; + case b3_hullShape: + debugShape.hull = shape->hull; + shape->userShape = world->createDebugShape( &debugShape, world->userDebugShapeContext ); + break; + case b3_meshShape: + debugShape.mesh = &shape->mesh; + shape->userShape = world->createDebugShape( &debugShape, world->userDebugShapeContext ); + break; + case b3_sphereShape: + debugShape.sphere = &shape->sphere; + shape->userShape = world->createDebugShape( &debugShape, world->userDebugShapeContext ); + break; + default: + B3_ASSERT( false ); + break; + } + } + + if ( shape->userShape != NULL ) + { + draw->DrawShapeFcn( shape->userShape, bodySim->transform, color, draw->context ); + } + } + + if ( draw->drawBounds ) + { + draw->DrawBoundsFcn( shape->fatAABB, b3_colorGold, draw->context ); + } + + return true; +} + +void b3World_Draw( b3WorldId worldId, b3DebugDraw* draw, uint64_t maskBits ) +{ + b3World* world = b3GetUnlockedWorldFromId( worldId ); + if ( world == NULL ) + { + return; + } + + B3_ASSERT( b3IsValidAABB( draw->drawingBounds ) ); + + float lengthScale = b3GetLengthUnitsPerMeter(); + float axisScale = 0.3f * lengthScale; + b3HexColor farColor = b3_colorDarkBlue; + b3HexColor speculativeColor = b3_colorBlue; + b3HexColor addColor = b3_colorLimeGreen; + b3HexColor persistColor = b3_colorLightBlue; + // b3HexColor normalColors[3] = { b3_colorLightGray, b3_colorLightSalmon, b3_colorLightSeaGreen }; + b3HexColor impulseColor = b3_colorMagenta; + b3HexColor frictionColor = b3_colorYellow; + + b3HexColor graphColors[B3_GRAPH_COLOR_COUNT] = { + b3_colorRed, b3_colorOrange, b3_colorYellow, b3_colorGreen, b3_colorCyan, b3_colorBlue, + b3_colorViolet, b3_colorPink, b3_colorChocolate, b3_colorGoldenRod, b3_colorCoral, b3_colorRosyBrown, + b3_colorAqua, b3_colorPeru, b3_colorLime, b3_colorGold, b3_colorPlum, b3_colorSnow, + b3_colorTeal, b3_colorKhaki, b3_colorSalmon, b3_colorPeachPuff, b3_colorHoneyDew, b3_colorBlack, + }; + + int bodyCapacity = b3GetIdCapacity( &world->bodyIdPool ); + b3SetBitCountAndClear( &world->debugBodySet, bodyCapacity ); + + int jointCapacity = b3GetIdCapacity( &world->jointIdPool ); + b3SetBitCountAndClear( &world->debugJointSet, jointCapacity ); + + int contactCapacity = b3GetIdCapacity( &world->contactIdPool ); + b3SetBitCountAndClear( &world->debugContactSet, contactCapacity ); + + int islandCapacity = b3GetIdCapacity( &world->islandIdPool ); + b3SetBitCountAndClear( &world->debugIslandSet, islandCapacity ); + + struct DrawContext drawContext = { world, draw }; + + for ( int i = 0; i < b3_bodyTypeCount; ++i ) + { + b3DynamicTree_Query( world->broadPhase.trees + i, draw->drawingBounds, maskBits, false, DrawQueryCallback, &drawContext ); + } + + uint32_t wordCount = world->debugBodySet.blockCount; + uint64_t* bits = world->debugBodySet.bits; + for ( uint32_t wordIndex = 0; wordIndex < wordCount; ++wordIndex ) + { + uint64_t word = bits[wordIndex]; + while ( word != 0 ) + { + uint32_t ctz = b3CTZ64( word ); + int bodyId = (int)( 64 * wordIndex + ctz ); + + b3Body* body = b3Array_Get( world->bodies, bodyId ); + b3BodySim* bodySim = b3GetBodySim( world, body ); + + if ( draw->drawBodyNames && body->nameId != B3_NULL_NAME ) + { + b3Vec3 offset = { 0.05f, 0.05f, 0.05f }; + b3WorldTransform transform = { bodySim->center, bodySim->transform.q }; + b3Pos p = b3TransformWorldPoint( transform, offset ); + const char* name = b3FindName( &world->names, body->nameId ); + if ( name != NULL ) + { + draw->DrawStringFcn( p, name, b3_colorOrange, draw->context ); + } + } + + if ( draw->drawMass && body->type == b3_dynamicBody ) + { + b3Vec3 offset = { 0.1f, 0.1f, 0.1f }; + + b3WorldTransform transform = { bodySim->center, bodySim->transform.q }; + draw->DrawTransformFcn( transform, draw->context ); + b3Pos p = b3TransformWorldPoint( transform, offset ); + + char buffer[32]; + snprintf( buffer, 32, " %.2f", body->mass ); + draw->DrawStringFcn( p, buffer, b3_colorWhite, draw->context ); + } + + if ( draw->drawSleep ) + { + b3BodyState* bodyState = b3GetBodyState( world, body ); + + if ( bodyState != NULL ) + { + b3HexColor colors[4] = { b3_colorBlue, b3_colorSkyBlue, b3_colorOrange, b3_colorRed }; + + b3HexColor color = b3_colorBlack; + if ( body->sleepThreshold > 0.0f ) + { + float ratio = body->sleepVelocity / body->sleepThreshold; + int index = b3ClampInt( (int)ratio, 0, 3 ); + color = colors[index]; + } + + b3Pos center = bodySim->center; + draw->DrawPointFcn( center, 10.0f, color, draw->context ); + + b3Vec3 offset = { 0.1f, 0.1f, 0.1f }; + b3Pos p = b3OffsetPos( center, offset ); + + char buffer[32]; + snprintf( buffer, 32, " %.3f", body->sleepVelocity ); + draw->DrawStringFcn( p, buffer, color, draw->context ); + } + } + + if ( draw->drawJoints ) + { + int jointKey = body->headJointKey; + while ( jointKey != B3_NULL_INDEX ) + { + int jointId = jointKey >> 1; + int edgeIndex = jointKey & 1; + b3Joint* joint = b3Array_Get( world->joints, jointId ); + + // avoid double draw + if ( b3GetBit( &world->debugJointSet, jointId ) == false ) + { + b3DrawJoint( draw, world, joint ); + b3SetBit( &world->debugJointSet, jointId ); + } + + jointKey = joint->edges[edgeIndex].nextKey; + } + } + + const float speculativeDistance = B3_SPECULATIVE_DISTANCE; + if ( draw->drawContacts && body->type == b3_dynamicBody && body->setIndex == b3_awakeSet ) + { + int contactKey = body->headContactKey; + while ( contactKey != B3_NULL_INDEX ) + { + int contactId = contactKey >> 1; + int edgeIndex = contactKey & 1; + b3Contact* contact = b3Array_Get( world->contacts, contactId ); + contactKey = contact->edges[edgeIndex].nextKey; + + if ( contact->setIndex != b3_awakeSet || contact->colorIndex == B3_NULL_INDEX ) + { + continue; + } + + // avoid double draw + if ( b3GetBit( &world->debugContactSet, contactId ) == false ) + { + b3Body* bodyA = b3Array_Get( world->bodies, contact->edges[0].bodyId ); + b3BodySim* bodySimA = b3GetBodySim( world, bodyA ); + b3Body* bodyB = b3Array_Get( world->bodies, contact->edges[1].bodyId ); + b3BodySim* bodySimB = b3GetBodySim( world, bodyB ); + + for ( int manifoldIndex = 0; manifoldIndex < contact->manifoldCount; ++manifoldIndex ) + { + const b3Manifold* manifold = contact->manifolds + manifoldIndex; + B3_ASSERT( manifold->pointCount > 0 ); + + b3Vec3 normal = manifold->normal; + + // Average the anchors not the world points so the friction center stays exact far from the origin + b3Pos contactCenter = draw->drawAnchorA == 1 ? bodySimA->center : bodySimB->center; + b3Vec3 anchorSum = b3Vec3_zero; + + const b3ManifoldPoint* points = manifold->points; + for ( int pointIndex = 0; pointIndex < manifold->pointCount; ++pointIndex ) + { + const b3ManifoldPoint* mp = points + pointIndex; + + char buffer[32]; + + b3Vec3 anchor = draw->drawAnchorA == 1 ? mp->anchorA : mp->anchorB; + b3Pos p = b3OffsetPos( contactCenter, anchor ); + + anchorSum = b3Add( anchorSum, anchor ); + + if ( draw->drawContactNormals ) + { + b3Pos p1 = p; + b3Pos p2 = b3OffsetPos( p1, b3MulSV( axisScale, normal ) ); + draw->DrawSegmentFcn( p1, p2, b3_colorLightGray, draw->context ); + + snprintf( buffer, B3_ARRAY_COUNT( buffer ), " %.2f", 100.0f * mp->separation ); + draw->DrawStringFcn( p, buffer, b3_colorWhite, draw->context ); + } + else if ( draw->drawContactForces ) + { + // Hack inv_dt for single step debugging + float inv_dt = world->inv_dt > 0.0f ? world->inv_dt : 60.0f; + // todo validate + // multiply by one-half due to relax iteration + float force = 0.5f * mp->totalNormalImpulse * inv_dt; + b3Pos p1 = p; + b3Pos p2 = b3OffsetPos( p1, b3MulSV( draw->forceScale * force, normal ) ); + draw->DrawSegmentFcn( p1, p2, impulseColor, draw->context ); + snprintf( buffer, B3_ARRAY_COUNT( buffer ), " %.1f", force ); + draw->DrawStringFcn( p1, buffer, b3_colorWhite, draw->context ); + } + else if ( draw->drawContactFeatures ) + { + snprintf( buffer, B3_ARRAY_COUNT( buffer ), " %#X", mp->featureId ); + draw->DrawStringFcn( p, buffer, b3_colorWhite, draw->context ); + } + + if ( draw->drawGraphColors ) + { + // graph color + float pointSize = contact->colorIndex == B3_OVERFLOW_INDEX ? 15.0f : 10.0f; + draw->DrawPointFcn( p, pointSize, graphColors[contact->colorIndex], draw->context ); + // g_draw.DrawString(point->position, "%d", point->color); + } + else if ( mp->persisted == false ) + { + // Add + draw->DrawPointFcn( p, 20.0f, addColor, draw->context ); + } + else if ( mp->separation > speculativeDistance ) + { + // Speculative + draw->DrawPointFcn( p, 10.0f, farColor, draw->context ); + } + else if ( mp->separation > 0.0f ) + { + // Speculative + draw->DrawPointFcn( p, 10.0f, speculativeColor, draw->context ); + } + else + { + // Persist + draw->DrawPointFcn( p, 10.0f, persistColor, draw->context ); + } + } + + if ( draw->drawContactForces ) + { + // Hack inv_dt for single step debugging + float inv_dt = world->inv_dt > 0.0f ? world->inv_dt : 60.0f; + + b3Vec3 avgAnchor = b3MulSV( 1.0f / manifold->pointCount, anchorSum ); + b3Pos p1 = b3OffsetPos( contactCenter, avgAnchor ); + b3Vec3 frictionForce = b3MulSV( 0.5f * inv_dt, manifold->frictionImpulse ); + b3Pos p2 = b3OffsetPos( p1, b3MulSV( draw->forceScale, frictionForce ) ); + draw->DrawSegmentFcn( p1, p2, frictionColor, draw->context ); + draw->DrawPointFcn( p1, 5.0f, frictionColor, draw->context ); + + p1 = b3OffsetPos( p1, b3MulSV( 0.05f * lengthScale, normal ) ); + char buffer[32]; + snprintf( buffer, B3_ARRAY_COUNT( buffer ), "%.2f", b3Length( frictionForce ) ); + draw->DrawStringFcn( p1, buffer, b3_colorWhite, draw->context ); + } + } + + b3SetBit( &world->debugContactSet, contactId ); + } + + contactKey = contact->edges[edgeIndex].nextKey; + } + } + + if ( draw->drawIslands ) + { + int islandId = body->islandId; + if ( islandId != B3_NULL_INDEX && b3GetBit( &world->debugIslandSet, islandId ) == false ) + { + b3Island* island = world->islands.data + islandId; + if ( island->setIndex == B3_NULL_INDEX ) + { + continue; + } + + int shapeCount = 0; + b3AABB aabb = { + .lowerBound = { FLT_MAX, FLT_MAX, FLT_MAX }, + .upperBound = { -FLT_MAX, -FLT_MAX, -FLT_MAX }, + }; + + for ( int b = 0; b < island->bodies.count; ++b ) + { + int islandBodyId = island->bodies.data[b]; + b3Body* islandBody = b3Array_Get( world->bodies, islandBodyId ); + int shapeId = islandBody->headShapeId; + while ( shapeId != B3_NULL_INDEX ) + { + b3Shape* shape = b3Array_Get( world->shapes, shapeId ); + aabb = b3AABB_Union( aabb, shape->fatAABB ); + shapeCount += 1; + shapeId = shape->nextShapeId; + } + } + + if ( shapeCount > 0 ) + { + draw->DrawBoundsFcn( aabb, b3_colorOrangeRed, draw->context ); + } + + b3SetBit( &world->debugIslandSet, islandId ); + } + } + + // Clear the smallest set bit + word = word & ( word - 1 ); + } + } + + char buffer[32] = { 0 }; + float lengthUnits = b3GetLengthUnitsPerMeter(); + b3Vec3 offset = { 0.002f * lengthUnits, 0.002f * lengthUnits, 0.002f * lengthUnits }; + for ( int i = 0; i < world->workerCount; ++i ) + { + int pointCount = world->taskContexts.data[i].pointCount; + b3DebugPoint* points = world->taskContexts.data[i].points; + for ( int j = 0; j < pointCount; ++j ) + { + b3DebugPoint* point = points + j; + b3Pos p = point->p; + draw->DrawPointFcn( p, 5.0f, point->color, draw->context ); + snprintf( buffer, 32, " %d, %.2f", point->label, point->value ); + b3Pos ps = b3OffsetPos( p, offset ); + draw->DrawStringFcn( ps, buffer, point->color, draw->context ); + } + + int lineCount = world->taskContexts.data[i].lineCount; + b3DebugLine* lines = world->taskContexts.data[i].lines; + for ( int j = 0; j < lineCount; ++j ) + { + b3DebugLine* line = lines + j; + b3Pos p1 = line->p1; + b3Pos p2 = line->p2; + draw->DrawSegmentFcn( p1, p2, line->color, draw->context ); + draw->DrawPointFcn( p1, 10.0f, line->color, draw->context ); + snprintf( buffer, 32, "%d", line->label ); + b3Pos ps = b3OffsetPos( p1, offset ); + draw->DrawStringFcn( ps, buffer, line->color, draw->context ); + } + } +} + +b3AABB b3World_GetBounds( b3WorldId worldId ) +{ + b3World* world = b3GetUnlockedWorldFromId( worldId ); + if ( world == NULL ) + { + return (b3AABB){ 0 }; + } + + b3AABB worldBounds = { 0 }; + bool haveBounds = false; + + for ( int i = 0; i < b3_bodyTypeCount; ++i ) + { + b3DynamicTree* tree = world->broadPhase.trees + i; + if ( b3DynamicTree_GetProxyCount( tree ) == 0 ) + { + continue; + } + + b3AABB bounds = b3DynamicTree_GetRootBounds( world->broadPhase.trees + i ); + + if ( haveBounds ) + { + worldBounds = b3AABB_Union( worldBounds, bounds ); + } + else + { + worldBounds = bounds; + haveBounds = true; + } + } + + return worldBounds; +} + +b3BodyEvents b3World_GetBodyEvents( b3WorldId worldId ) +{ + b3World* world = b3GetUnlockedWorldFromId( worldId ); + if ( world == NULL ) + { + return (b3BodyEvents){ 0 }; + } + + int count = world->bodyMoveEvents.count; + b3BodyEvents events = { world->bodyMoveEvents.data, count }; + return events; +} + +b3SensorEvents b3World_GetSensorEvents( b3WorldId worldId ) +{ + b3World* world = b3GetUnlockedWorldFromId( worldId ); + if ( world == NULL ) + { + return (b3SensorEvents){ 0 }; + } + + // Careful to use previous buffer + int endEventArrayIndex = 1 - world->endEventArrayIndex; + + int beginCount = world->sensorBeginEvents.count; + int endCount = world->sensorEndEvents[endEventArrayIndex].count; + + b3SensorEvents events = { + .beginEvents = world->sensorBeginEvents.data, + .endEvents = world->sensorEndEvents[endEventArrayIndex].data, + .beginCount = beginCount, + .endCount = endCount, + }; + return events; +} + +b3ContactEvents b3World_GetContactEvents( b3WorldId worldId ) +{ + b3World* world = b3GetUnlockedWorldFromId( worldId ); + if ( world == NULL ) + { + return (b3ContactEvents){ 0 }; + } + + // Careful to use previous buffer + int endEventArrayIndex = 1 - world->endEventArrayIndex; + + int beginCount = world->contactBeginEvents.count; + int endCount = world->contactEndEvents[endEventArrayIndex].count; + int hitCount = world->contactHitEvents.count; + + b3ContactEvents events = { + .beginEvents = world->contactBeginEvents.data, + .endEvents = world->contactEndEvents[endEventArrayIndex].data, + .hitEvents = world->contactHitEvents.data, + .beginCount = beginCount, + .endCount = endCount, + .hitCount = hitCount, + }; + + return events; +} + +b3JointEvents b3World_GetJointEvents( b3WorldId worldId ) +{ + b3World* world = b3GetUnlockedWorldFromId( worldId ); + if ( world == NULL ) + { + return (b3JointEvents){ 0 }; + } + + int count = world->jointEvents.count; + b3JointEvents events = { world->jointEvents.data, count }; + return events; +} + +bool b3World_IsValid( b3WorldId id ) +{ + if ( id.index1 < 1 || B3_MAX_WORLDS < id.index1 ) + { + return false; + } + + b3World* world = b3_worlds + ( id.index1 - 1 ); + + if ( world->worldId != id.index1 - 1 ) + { + // world is not allocated + return false; + } + + return id.generation == world->generation; +} + +bool b3Body_IsValid( b3BodyId id ) +{ + if ( B3_MAX_WORLDS <= id.world0 ) + { + // invalid world + return false; + } + + b3World* world = b3_worlds + id.world0; + if ( world->worldId != id.world0 ) + { + // world is free + return false; + } + + if ( id.index1 < 1 || world->bodies.count < id.index1 ) + { + // invalid index + return false; + } + + b3Body* body = world->bodies.data + ( id.index1 - 1 ); + if ( body->setIndex == B3_NULL_INDEX ) + { + // this was freed + return false; + } + + B3_ASSERT( body->localIndex != B3_NULL_INDEX ); + + if ( body->generation != id.generation ) + { + // this id is orphaned + return false; + } + + return true; +} + +bool b3Shape_IsValid( b3ShapeId id ) +{ + if ( B3_MAX_WORLDS <= id.world0 ) + { + return false; + } + + b3World* world = b3_worlds + id.world0; + if ( world->worldId != id.world0 ) + { + // world is free + return false; + } + + int shapeId = id.index1 - 1; + if ( shapeId < 0 || world->shapes.count <= shapeId ) + { + return false; + } + + b3Shape* shape = world->shapes.data + shapeId; + if ( shape->id == B3_NULL_INDEX ) + { + // shape is free + return false; + } + + B3_ASSERT( shape->id == shapeId ); + + return id.generation == shape->generation; +} + +bool b3Joint_IsValid( b3JointId id ) +{ + if ( B3_MAX_WORLDS <= id.world0 ) + { + return false; + } + + b3World* world = b3_worlds + id.world0; + if ( world->worldId != id.world0 ) + { + // world is free + return false; + } + + int jointId = id.index1 - 1; + if ( jointId < 0 || world->joints.count <= jointId ) + { + return false; + } + + b3Joint* joint = world->joints.data + jointId; + if ( joint->jointId == B3_NULL_INDEX ) + { + // joint is free + return false; + } + + B3_ASSERT( joint->jointId == jointId ); + + return id.generation == joint->generation; +} + +bool b3Contact_IsValid( b3ContactId id ) +{ + if ( B3_MAX_WORLDS <= id.world0 ) + { + return false; + } + + b3World* world = b3_worlds + id.world0; + if ( world->worldId != id.world0 ) + { + // world is free + return false; + } + + int contactId = id.index1 - 1; + if ( contactId < 0 || world->contacts.count <= contactId ) + { + return false; + } + + b3Contact* contact = world->contacts.data + contactId; + if ( contact->contactId == B3_NULL_INDEX ) + { + // contact is free + return false; + } + + B3_ASSERT( contact->contactId == contactId ); + + return id.generation == contact->generation; +} + +void b3World_EnableSleeping( b3WorldId worldId, bool flag ) +{ + b3World* world = b3GetUnlockedWorldFromId( worldId ); + if ( world == NULL ) + { + return; + } + + if ( flag == world->enableSleep ) + { + return; + } + + B3_REC( world, WorldEnableSleeping, worldId, flag ); + + world->enableSleep = flag; + + if ( flag == false ) + { + int setCount = world->solverSets.count; + for ( int i = b3_firstSleepingSet; i < setCount; ++i ) + { + b3SolverSet* set = b3Array_Get( world->solverSets, i ); + if ( set->bodySims.count > 0 ) + { + b3WakeSolverSet( world, i ); + } + } + } +} + +bool b3World_IsSleepingEnabled( b3WorldId worldId ) +{ + b3World* world = b3GetWorldFromId( worldId ); + return world->enableSleep; +} + +void b3World_EnableWarmStarting( b3WorldId worldId, bool flag ) +{ + b3World* world = b3GetUnlockedWorldFromId( worldId ); + if ( world == NULL ) + { + return; + } + + B3_REC( world, WorldEnableWarmStarting, worldId, flag ); + + world->enableWarmStarting = flag; +} + +bool b3World_IsWarmStartingEnabled( b3WorldId worldId ) +{ + b3World* world = b3GetWorldFromId( worldId ); + return world->enableWarmStarting; +} + +int b3World_GetAwakeBodyCount( b3WorldId worldId ) +{ + b3World* world = b3GetUnlockedWorldFromId( worldId ); + if ( world == NULL ) + { + return 0; + } + b3SolverSet* awakeSet = b3Array_Get( world->solverSets, b3_awakeSet ); + return awakeSet->bodySims.count; +} + +void b3World_EnableContinuous( b3WorldId worldId, bool flag ) +{ + b3World* world = b3GetUnlockedWorldFromId( worldId ); + if ( world == NULL ) + { + return; + } + + B3_REC( world, WorldEnableContinuous, worldId, flag ); + + world->enableContinuous = flag; +} + +bool b3World_IsContinuousEnabled( b3WorldId worldId ) +{ + b3World* world = b3GetWorldFromId( worldId ); + return world->enableContinuous; +} + +void b3World_SetRestitutionThreshold( b3WorldId worldId, float value ) +{ + b3World* world = b3GetUnlockedWorldFromId( worldId ); + if ( world == NULL ) + { + return; + } + + B3_REC( world, WorldSetRestitutionThreshold, worldId, value ); + + world->restitutionThreshold = b3ClampFloat( value, 0.0f, FLT_MAX ); +} + +float b3World_GetRestitutionThreshold( b3WorldId worldId ) +{ + b3World* world = b3GetWorldFromId( worldId ); + return world->restitutionThreshold; +} + +void b3World_SetHitEventThreshold( b3WorldId worldId, float value ) +{ + b3World* world = b3GetUnlockedWorldFromId( worldId ); + if ( world == NULL ) + { + return; + } + + B3_REC( world, WorldSetHitEventThreshold, worldId, value ); + + world->hitEventThreshold = b3ClampFloat( value, 0.0f, FLT_MAX ); +} + +float b3World_GetHitEventThreshold( b3WorldId worldId ) +{ + b3World* world = b3GetWorldFromId( worldId ); + return world->hitEventThreshold; +} + +void b3World_SetContactTuning( b3WorldId worldId, float hertz, float dampingRatio, float contactSpeed ) +{ + b3World* world = b3GetUnlockedWorldFromId( worldId ); + if ( world == NULL ) + { + return; + } + + B3_REC( world, WorldSetContactTuning, worldId, hertz, dampingRatio, contactSpeed ); + + world->contactHertz = b3ClampFloat( hertz, 0.0f, FLT_MAX ); + world->contactDampingRatio = b3ClampFloat( dampingRatio, 0.0f, FLT_MAX ); + world->contactSpeed = b3ClampFloat( contactSpeed, 0.0f, FLT_MAX ); +} + +void b3World_SetContactRecycleDistance( b3WorldId worldId, float recycleDistance ) +{ + b3World* world = b3GetWorldFromId( worldId ); + B3_ASSERT( world->locked == false ); + if ( world->locked ) + { + return; + } + + B3_REC( world, WorldSetContactRecycleDistance, worldId, recycleDistance ); + + world->contactRecycleDistance = b3ClampFloat( recycleDistance, 0.0f, FLT_MAX ); +} + +float b3World_GetContactRecycleDistance( b3WorldId worldId ) +{ + b3World* world = b3GetWorldFromId( worldId ); + return world->contactRecycleDistance; +} + +void b3World_SetMaximumLinearSpeed( b3WorldId worldId, float maximumLinearSpeed ) +{ + B3_ASSERT( b3IsValidFloat( maximumLinearSpeed ) && maximumLinearSpeed > 0.0f ); + + b3World* world = b3GetUnlockedWorldFromId( worldId ); + if ( world == NULL ) + { + return; + } + + B3_REC( world, WorldSetMaximumLinearSpeed, worldId, maximumLinearSpeed ); + + world->maxLinearSpeed = maximumLinearSpeed; +} + +float b3World_GetMaximumLinearSpeed( b3WorldId worldId ) +{ + b3World* world = b3GetWorldFromId( worldId ); + return world->maxLinearSpeed; +} + +b3Profile b3World_GetProfile( b3WorldId worldId ) +{ + b3World* world = b3GetUnlockedWorldFromId( worldId ); + if ( world == NULL ) + { + return (b3Profile){ 0 }; + } + return world->profile; +} + +b3Counters b3World_GetCounters( b3WorldId worldId ) +{ + b3World* world = b3GetUnlockedWorldFromId( worldId ); + if ( world == NULL ) + { + return (b3Counters){ 0 }; + } + + b3Counters s = { 0 }; + s.bodyCount = b3GetIdCount( &world->bodyIdPool ); + s.shapeCount = b3GetIdCount( &world->shapeIdPool ); + s.contactCount = b3GetIdCount( &world->contactIdPool ); + s.jointCount = b3GetIdCount( &world->jointIdPool ); + s.islandCount = b3GetIdCount( &world->islandIdPool ); + + b3DynamicTree* staticTree = world->broadPhase.trees + b3_staticBody; + s.staticTreeHeight = b3DynamicTree_GetHeight( staticTree ); + + b3DynamicTree* dynamicTree = world->broadPhase.trees + b3_dynamicBody; + b3DynamicTree* kinematicTree = world->broadPhase.trees + b3_kinematicBody; + s.treeHeight = b3MaxInt( b3DynamicTree_GetHeight( dynamicTree ), b3DynamicTree_GetHeight( kinematicTree ) ); + + s.satCallCount = world->satCallCount; + s.satCacheHitCount = world->satCacheHitCount; + memcpy( s.manifoldCounts, world->manifoldCounts, B3_CONTACT_MANIFOLD_COUNT_BUCKETS * sizeof( int ) ); + s.stackUsed = world->stack.maxAllocation; + s.byteCount = b3GetByteCount(); + s.taskCount = world->taskCount; + + _Static_assert( B3_GRAPH_COLOR_COUNT == sizeof( s.colorCounts ) / sizeof( s.colorCounts[0] ), "colorCounts size mismatch" ); + + s.awakeContactCount = 0; + for ( int i = 0; i < B3_GRAPH_COLOR_COUNT; ++i ) + { + b3GraphColor* color = world->constraintGraph.colors + i; + int colorContactCount = color->convexContacts.count + color->contacts.count; + s.colorCounts[i] = colorContactCount + color->jointSims.count; + s.awakeContactCount += colorContactCount; + } + s.awakeContactCount += world->solverSets.data[b3_awakeSet].contactIndices.count; + + s.recycledContactCount = 0; + s.arenaCapacity = 0; + s.distanceIterations = 0; + s.pushBackIterations = 0; + s.rootIterations = 0; + for ( int i = 0; i < world->workerCount; ++i ) + { + s.recycledContactCount += world->taskContexts.data[i].recycledContactCount; + + s.distanceIterations = b3MaxInt( s.distanceIterations, world->taskContexts.data[i].distanceIterations ); + s.pushBackIterations = b3MaxInt( s.pushBackIterations, world->taskContexts.data[i].pushBackIterations ); + s.rootIterations = b3MaxInt( s.rootIterations, world->taskContexts.data[i].rootIterations ); + + int peak = world->taskContexts.data[i].arena.shared->peakDemand; + if ( peak > s.arenaCapacity ) + { + s.arenaCapacity = peak; + } + } + + return s; +} + +b3Capacity b3World_GetMaxCapacity( b3WorldId worldId ) +{ + b3World* world = b3GetUnlockedWorldFromId( worldId ); + if ( world == NULL ) + { + return (b3Capacity){ 0 }; + } + return world->maxCapacity; +} + +void b3World_SetUserData( b3WorldId worldId, void* userData ) +{ + b3World* world = b3GetWorldFromId( worldId ); + world->userData = userData; +} + +void* b3World_GetUserData( b3WorldId worldId ) +{ + b3World* world = b3GetWorldFromId( worldId ); + return world->userData; +} + +void b3World_SetFrictionCallback( b3WorldId worldId, b3FrictionCallback* callback ) +{ + b3World* world = b3GetUnlockedWorldFromId( worldId ); + if ( world == NULL ) + { + return; + } + + world->frictionCallback = callback != NULL ? callback : b3DefaultFrictionCallback; +} + +void b3World_SetRestitutionCallback( b3WorldId worldId, b3RestitutionCallback* callback ) +{ + b3World* world = b3GetUnlockedWorldFromId( worldId ); + if ( world == NULL ) + { + return; + } + + world->restitutionCallback = callback != NULL ? callback : b3DefaultRestitutionCallback; +} + +void b3World_SetWorkerCount( b3WorldId worldId, int count ) +{ + b3World* world = b3GetUnlockedWorldFromId( worldId ); + if ( world == NULL ) + { + return; + } + + if ( count == world->workerCount ) + { + return; + } + + b3DestroyWorkerContexts( world ); + world->workerCount = b3ClampInt( count, 1, B3_MAX_WORKERS ); + b3CreateWorkerContexts( world ); +} + +int b3World_GetWorkerCount( b3WorldId worldId ) +{ + b3World* world = b3GetUnlockedWorldFromId( worldId ); + if ( world == NULL ) + { + return 0; + } + + return world->workerCount; +} + +void b3World_StartRecording( b3WorldId worldId, b3Recording* recording ) +{ + // Must be a step boundary, so refuse a locked world + b3World* world = b3GetUnlockedWorldFromId( worldId ); + + if ( world == NULL || recording == NULL || world->recording != NULL ) + { + return; + } + + b3StartRecordingIntoBuffer( world, recording ); +} + +void b3World_StopRecording( b3WorldId worldId ) +{ + b3World* world = b3GetUnlockedWorldFromId( worldId ); + if ( world == NULL ) + { + return; + } + + b3StopRecordingInternal( world ); +} + +void b3World_DumpMemoryStats( b3WorldId worldId ) +{ + b3World* world = b3GetUnlockedWorldFromId( worldId ); + if ( world == NULL ) + { + return; + } + + // Large worlds can exceed 2GB, sum in 64 bits + uint64_t total = 0; + + // id pools + int bodyIdBytes = b3GetIdBytes( &world->bodyIdPool ); + int solverSetIdBytes = b3GetIdBytes( &world->solverSetIdPool ); + int jointIdBytes = b3GetIdBytes( &world->jointIdPool ); + int contactIdBytes = b3GetIdBytes( &world->contactIdPool ); + int islandIdBytes = b3GetIdBytes( &world->islandIdPool ); + int shapeIdBytes = b3GetIdBytes( &world->shapeIdPool ); + total += (uint64_t)bodyIdBytes + solverSetIdBytes + jointIdBytes + contactIdBytes + islandIdBytes + shapeIdBytes; + + b3Log( "id pools" ); + b3Log( "body ids: %d", bodyIdBytes ); + b3Log( "solver set ids: %d", solverSetIdBytes ); + b3Log( "joint ids: %d", jointIdBytes ); + b3Log( "contact ids: %d", contactIdBytes ); + b3Log( "island ids: %d", islandIdBytes ); + b3Log( "shape ids: %d", shapeIdBytes ); + + // Islands own per-island body/contact/joint link arrays + int islandLinkBytes = 0; + for ( int i = 0; i < world->islands.count; ++i ) + { + b3Island* island = world->islands.data + i; + islandLinkBytes += b3Array_ByteCount( island->bodies ); + islandLinkBytes += b3Array_ByteCount( island->contacts ); + islandLinkBytes += b3Array_ByteCount( island->joints ); + } + + // world arrays + int bodyArrayBytes = b3Array_ByteCount( world->bodies ); + int solverSetArrayBytes = b3Array_ByteCount( world->solverSets ); + int jointArrayBytes = b3Array_ByteCount( world->joints ); + int contactArrayBytes = b3Array_ByteCount( world->contacts ); + int islandArrayBytes = b3Array_ByteCount( world->islands ); + int shapeArrayBytes = b3Array_ByteCount( world->shapes ); + int sensorArrayBytes = b3Array_ByteCount( world->sensors ); + total += (uint64_t)bodyArrayBytes + solverSetArrayBytes + jointArrayBytes + contactArrayBytes + islandArrayBytes + + islandLinkBytes + shapeArrayBytes + sensorArrayBytes; + + b3Log( "world arrays" ); + b3Log( "bodies: %d", bodyArrayBytes ); + b3Log( "solver sets: %d", solverSetArrayBytes ); + b3Log( "joints: %d", jointArrayBytes ); + b3Log( "contacts: %d", contactArrayBytes ); + b3Log( "islands: %d", islandArrayBytes ); + b3Log( "island links: %d", islandLinkBytes ); + b3Log( "shapes: %d", shapeArrayBytes ); + b3Log( "sensors: %d", sensorArrayBytes ); + + // Sensors own overlap tracking arrays. The sensor array is dense. + int sensorOverlapBytes = 0; + for ( int i = 0; i < world->sensors.count; ++i ) + { + b3Sensor* sensor = world->sensors.data + i; + sensorOverlapBytes += b3Array_ByteCount( sensor->hits ); + sensorOverlapBytes += b3Array_ByteCount( sensor->overlaps1 ); + sensorOverlapBytes += b3Array_ByteCount( sensor->overlaps2 ); + } + total += sensorOverlapBytes; + + b3Log( "owned arrays" ); + b3Log( "sensor overlaps: %d", sensorOverlapBytes ); + + // Shared hull database. The map owns a combined bucket and metadata allocation + // plus the small map struct. Each stored key is an owned clone sized by byteCount. + b3HullMap* hullDatabase = world->hullDatabase; + int hullCount = (int)b3HullMap_size( hullDatabase ); + int hullBucketCount = (int)b3HullMap_bucket_count( hullDatabase ); + uint64_t hullMapBytes = b3HullMapByteCount( hullDatabase ); + uint64_t hullDataBytes = 0; + for ( b3HullMap_itr itr = b3HullMap_first( hullDatabase ); b3HullMap_is_end( itr ) == false; itr = b3HullMap_next( itr ) ) + { + hullDataBytes += itr.data->key->byteCount; + } + total += hullMapBytes + hullDataBytes; + + b3Log( "hulls" ); + b3Log( "database: %d (%d, %d)", (int)hullMapBytes, hullCount, hullBucketCount ); + b3Log( "hull data: %d", (int)hullDataBytes ); + + // broad-phase + int staticTreeBytes = b3DynamicTree_GetByteCount( world->broadPhase.trees + b3_staticBody ); + int kinematicTreeBytes = b3DynamicTree_GetByteCount( world->broadPhase.trees + b3_kinematicBody ); + int dynamicTreeBytes = b3DynamicTree_GetByteCount( world->broadPhase.trees + b3_dynamicBody ); + int movedBytes = 0; + for ( int i = 0; i < b3_bodyTypeCount; ++i ) + { + movedBytes += b3GetBitSetBytes( &world->broadPhase.movedProxies[i] ); + } + int moveArrayBytes = b3Array_ByteCount( world->broadPhase.moveArray ); + b3HashSet* pairSet = &world->broadPhase.pairSet; + int pairSetBytes = b3GetHashSetBytes( pairSet ); + total += (uint64_t)staticTreeBytes + kinematicTreeBytes + dynamicTreeBytes + movedBytes + moveArrayBytes + pairSetBytes; + + b3Log( "broad-phase" ); + b3Log( "static tree: %d", staticTreeBytes ); + b3Log( "kinematic tree: %d", kinematicTreeBytes ); + b3Log( "dynamic tree: %d", dynamicTreeBytes ); + b3Log( "movedProxies: %d", movedBytes ); + b3Log( "moveArray: %d", moveArrayBytes ); + b3Log( "pairSet: %d (%d, %d)", pairSetBytes, pairSet->count, pairSet->capacity ); + + // Manifold block allocators, one per manifold point count + int manifoldArrayBytes = b3Array_ByteCount( world->manifoldAllocators ); + int manifoldBlockBytes = 0; + for ( int i = 0; i < world->manifoldAllocators.count; ++i ) + { + b3BlockAllocator* allocator = world->manifoldAllocators.data + i; + manifoldBlockBytes += b3Array_ByteCount( allocator->blocks ); + manifoldBlockBytes += allocator->blocks.count * B3_BLOCK_SIZE * allocator->elementSize; + } + total += (uint64_t)manifoldArrayBytes + manifoldBlockBytes; + + b3Log( "manifold allocators" ); + b3Log( "allocator array: %d", manifoldArrayBytes ); + b3Log( "blocks: %d", manifoldBlockBytes ); + + // solver sets + int bodySimCapacity = 0; + int bodyStateCapacity = 0; + int jointSimCapacity = 0; + int contactIndexCapacity = 0; + int islandSimCapacity = 0; + int solverSetCapacity = world->solverSets.count; + for ( int i = 0; i < solverSetCapacity; ++i ) + { + b3SolverSet* set = world->solverSets.data + i; + if ( set->setIndex == B3_NULL_INDEX ) + { + continue; + } + + bodySimCapacity += set->bodySims.capacity; + bodyStateCapacity += set->bodyStates.capacity; + jointSimCapacity += set->jointSims.capacity; + contactIndexCapacity += set->contactIndices.capacity; + islandSimCapacity += set->islandSims.capacity; + } + + int setBodySimBytes = bodySimCapacity * (int)sizeof( b3BodySim ); + int setBodyStateBytes = bodyStateCapacity * (int)sizeof( b3BodyState ); + int setJointSimBytes = jointSimCapacity * (int)sizeof( b3JointSim ); + int setContactSimBytes = contactIndexCapacity * (int)sizeof( int ); + int setIslandSimBytes = islandSimCapacity * (int)sizeof( b3IslandSim ); + total += (uint64_t)setBodySimBytes + setBodyStateBytes + setJointSimBytes + setContactSimBytes + setIslandSimBytes; + + b3Log( "solver sets" ); + b3Log( "body sim: %d", setBodySimBytes ); + b3Log( "body state: %d", setBodyStateBytes ); + b3Log( "joint sim: %d", setJointSimBytes ); + b3Log( "contact sim: %d", setContactSimBytes ); + b3Log( "island sim: %d", setIslandSimBytes ); + + // constraint graph + int bodyBitSetBytes = 0; + int graphContactBytes = 0; + int graphJointSimBytes = 0; + for ( int i = 0; i < B3_GRAPH_COLOR_COUNT; ++i ) + { + b3GraphColor* c = world->constraintGraph.colors + i; + bodyBitSetBytes += b3GetBitSetBytes( &c->bodySet ); + graphContactBytes += b3Array_ByteCount( c->convexContacts ) + b3Array_ByteCount( c->contacts ); + graphJointSimBytes += b3Array_ByteCount( c->jointSims ); + } + total += (uint64_t)bodyBitSetBytes + graphJointSimBytes + graphContactBytes; + + b3Log( "constraint graph" ); + b3Log( "body bit sets: %d", bodyBitSetBytes ); + b3Log( "joint sim: %d", graphJointSimBytes ); + b3Log( "contact sim: %d", graphContactBytes ); + + // Per worker task storage and its bit sets + int taskContextBytes = b3Array_ByteCount( world->taskContexts ); + for ( int i = 0; i < world->taskContexts.count; ++i ) + { + b3TaskContext* taskContext = world->taskContexts.data + i; + taskContextBytes += b3Array_ByteCount( taskContext->sensorHits ); + taskContextBytes += b3GetBitSetBytes( &taskContext->contactStateBitSet ); + taskContextBytes += b3GetBitSetBytes( &taskContext->jointStateBitSet ); + taskContextBytes += b3GetBitSetBytes( &taskContext->hitEventBitSet ); + taskContextBytes += b3GetBitSetBytes( &taskContext->enlargedSimBitSet ); + taskContextBytes += b3GetBitSetBytes( &taskContext->awakeIslandBitSet ); + } + + int sensorTaskContextBytes = b3Array_ByteCount( world->sensorTaskContexts ); + for ( int i = 0; i < world->sensorTaskContexts.count; ++i ) + { + b3SensorTaskContext* taskContext = world->sensorTaskContexts.data + i; + sensorTaskContextBytes += b3GetBitSetBytes( &taskContext->eventBits ); + } + total += (uint64_t)taskContextBytes + sensorTaskContextBytes; + + b3Log( "task contexts" ); + b3Log( "worker: %d", taskContextBytes ); + b3Log( "sensor: %d", sensorTaskContextBytes ); + + // Double buffered event arrays + int eventBytes = 0; + eventBytes += b3Array_ByteCount( world->bodyMoveEvents ); + eventBytes += b3Array_ByteCount( world->sensorBeginEvents ); + eventBytes += b3Array_ByteCount( world->contactBeginEvents ); + eventBytes += b3Array_ByteCount( world->sensorEndEvents[0] ); + eventBytes += b3Array_ByteCount( world->sensorEndEvents[1] ); + eventBytes += b3Array_ByteCount( world->contactEndEvents[0] ); + eventBytes += b3Array_ByteCount( world->contactEndEvents[1] ); + eventBytes += b3Array_ByteCount( world->contactHitEvents ); + eventBytes += b3Array_ByteCount( world->jointEvents ); + total += eventBytes; + + b3Log( "events: %d", eventBytes ); + + // Debug draw bit sets + int debugBytes = 0; + debugBytes += b3GetBitSetBytes( &world->debugBodySet ); + debugBytes += b3GetBitSetBytes( &world->debugJointSet ); + debugBytes += b3GetBitSetBytes( &world->debugContactSet ); + debugBytes += b3GetBitSetBytes( &world->debugIslandSet ); + total += debugBytes; + + b3Log( "debug draw: %d", debugBytes ); + + // stack allocator + total += world->stack.capacity; + b3Log( "stack allocator: %d", world->stack.capacity ); + + b3Log( "total: %u KB", (uint32_t)( total / 1024 ) ); +} + +typedef struct WorldQueryContext +{ + b3World* world; + b3OverlapResultFcn* fcn; + b3QueryFilter filter; + void* userContext; +} WorldQueryContext; + +static bool TreeQueryCallback( int proxyId, uint64_t userData, void* context ) +{ + B3_UNUSED( proxyId ); + + int shapeId = (int)userData; + WorldQueryContext* worldContext = (WorldQueryContext*)context; + b3World* world = worldContext->world; + + b3Shape* shape = b3Array_Get( world->shapes, shapeId ); + + b3Filter shapeFilter = shape->filter; + b3QueryFilter queryFilter = worldContext->filter; + + if ( b3ShouldQueryCollide( &shapeFilter, &queryFilter ) == false ) + { + return true; + } + + b3ShapeId id = { shapeId + 1, world->worldId, shape->generation }; + bool result = worldContext->fcn( id, worldContext->userContext ); + return result; +} + +b3TreeStats b3World_OverlapAABB( b3WorldId worldId, b3AABB aabb, b3QueryFilter filter, b3OverlapResultFcn* fcn, void* context ) +{ + b3TreeStats treeStats = { 0 }; + + b3World* world = b3GetUnlockedWorldFromId( worldId ); + if ( world == NULL ) + { + return treeStats; + } + + B3_ASSERT( b3IsValidAABB( aabb ) ); + + b3RecQueryWriter recWriter = { 0 }; + if ( world->recording != NULL ) + { + b3RecQueryBegin( &recWriter, context, filter.id, filter.name ); + recWriter.userFcn.overlapFcn = fcn; + b3RecW_WORLDID( &recWriter.buf, worldId ); + b3RecW_AABB( &recWriter.buf, aabb ); + b3RecW_QUERYFILTER( &recWriter.buf, filter ); + recWriter.countOffset = b3RecReserveU32( &recWriter.buf ); + fcn = b3RecOverlapTrampoline; + context = &recWriter; + } + + WorldQueryContext worldContext = { world, fcn, filter, context }; + + for ( int i = 0; i < b3_bodyTypeCount; ++i ) + { + b3TreeStats treeResult = + b3DynamicTree_Query( world->broadPhase.trees + i, aabb, filter.maskBits, false, TreeQueryCallback, &worldContext ); + + treeStats.nodeVisits += treeResult.nodeVisits; + treeStats.leafVisits += treeResult.leafVisits; + } + + if ( world->recording != NULL ) + { + b3RecPatchU32( &recWriter.buf, recWriter.countOffset, recWriter.hitCount ); + b3RecW_TREESTATS( &recWriter.buf, treeStats ); + b3RecQueryCommit( world->recording, b3_recOpQueryOverlapAABB, &recWriter ); + } + + return treeStats; +} + +typedef struct WorldOverlapContext +{ + b3World* world; + b3OverlapResultFcn* fcn; + b3QueryFilter filter; + b3ShapeProxy proxy; + b3Pos origin; + void* userContext; +} WorldOverlapContext; + +static bool b3TreeOverlapCallback( int proxyId, uint64_t userData, void* context ) +{ + B3_UNUSED( proxyId ); + + int shapeId = (int)userData; + WorldOverlapContext* worldContext = (WorldOverlapContext*)context; + b3World* world = worldContext->world; + + b3Shape* shape = b3Array_Get( world->shapes, shapeId ); + + b3Filter shapeFilter = shape->filter; + b3QueryFilter queryFilter = worldContext->filter; + + if ( b3ShouldQueryCollide( &shapeFilter, &queryFilter ) == false ) + { + return true; + } + + // Re-center on the query origin so the overlap test stays in float precision far from the origin + b3Body* body = b3Array_Get( world->bodies, shape->bodyId ); + b3Transform transform = b3ToRelativeTransform( b3GetBodyTransformQuick( world, body ), worldContext->origin ); + + bool overlapping = b3OverlapShape( shape, transform, &worldContext->proxy ); + if ( overlapping == false ) + { + return true; + } + + b3ShapeId id = { shape->id + 1, world->worldId, shape->generation }; + bool result = worldContext->fcn( id, worldContext->userContext ); + return result; +} + +b3TreeStats b3World_OverlapShape( b3WorldId worldId, b3Pos origin, const b3ShapeProxy* proxy, b3QueryFilter filter, + b3OverlapResultFcn* fcn, void* context ) +{ + b3TreeStats treeStats = { 0 }; + + b3World* world = b3GetUnlockedWorldFromId( worldId ); + if ( world == NULL ) + { + return treeStats; + } + + B3_ASSERT( b3IsValidPosition( origin ) ); + + b3RecQueryWriter recWriter = { 0 }; + if ( world->recording != NULL ) + { + b3RecQueryBegin( &recWriter, context, filter.id, filter.name ); + recWriter.userFcn.overlapFcn = fcn; + b3RecW_WORLDID( &recWriter.buf, worldId ); + b3RecW_POSITION( &recWriter.buf, origin ); + b3RecW_SHAPEPROXY( &recWriter.buf, *proxy ); + b3RecW_QUERYFILTER( &recWriter.buf, filter ); + recWriter.countOffset = b3RecReserveU32( &recWriter.buf ); + fcn = b3RecOverlapTrampoline; + context = &recWriter; + } + + // Bound the proxy in origin relative space then lift to a conservative world float box + b3AABB aabb = b3OffsetAABB( b3MakeAABB( proxy->points, proxy->count, proxy->radius ), origin ); + WorldOverlapContext worldContext = { + world, fcn, filter, *proxy, origin, context, + }; + + for ( int i = 0; i < b3_bodyTypeCount; ++i ) + { + b3TreeStats treeResult = b3DynamicTree_Query( world->broadPhase.trees + i, aabb, filter.maskBits, false, + b3TreeOverlapCallback, &worldContext ); + + treeStats.nodeVisits += treeResult.nodeVisits; + treeStats.leafVisits += treeResult.leafVisits; + } + + if ( world->recording != NULL ) + { + b3RecPatchU32( &recWriter.buf, recWriter.countOffset, recWriter.hitCount ); + b3RecW_TREESTATS( &recWriter.buf, treeStats ); + b3RecQueryCommit( world->recording, b3_recOpQueryOverlapShape, &recWriter ); + } + + return treeStats; +} + +typedef struct WorldMoverContext +{ + b3World* world; + b3PlaneResultFcn* fcn; + b3QueryFilter filter; + b3Capsule mover; + b3Pos origin; + void* userContext; +} WorldMoverContext; + +static bool TreeCollideCallback( int proxyId, uint64_t userData, void* context ) +{ + B3_UNUSED( proxyId ); + + int shapeId = (int)userData; + WorldMoverContext* worldContext = (WorldMoverContext*)context; + b3World* world = worldContext->world; + + b3Shape* shape = b3Array_Get( world->shapes, shapeId ); + + b3Filter shapeFilter = shape->filter; + b3QueryFilter queryFilter = worldContext->filter; + + if ( b3ShouldQueryCollide( &shapeFilter, &queryFilter ) == false ) + { + return true; + } + + // Re-center on the query origin, the mover and the resulting planes are origin relative + b3Body* body = b3Array_Get( world->bodies, shape->bodyId ); + b3WorldTransform bodyTransform = b3GetBodyTransformQuick( world, body ); + b3Transform transform = b3ToRelativeTransform( bodyTransform, worldContext->origin ); + + b3PlaneResult buffer[64]; + int count = b3CollideMover( buffer, 64, shape, transform, &worldContext->mover ); + + if ( count > 0 ) + { + b3ShapeId id = { shape->id + 1, world->worldId, shape->generation }; + return worldContext->fcn( id, buffer, count, worldContext->userContext ); + } + + return true; +} + +// It is tempting to use a shape proxy for the mover, but this makes handling deep overlap difficult and the generality may +// not be worth it. +void b3World_CollideMover( b3WorldId worldId, b3Pos origin, const b3Capsule* mover, b3QueryFilter filter, b3PlaneResultFcn* fcn, + void* context ) +{ + b3World* world = b3GetUnlockedWorldFromId( worldId ); + if ( world == NULL ) + { + return; + } + + B3_ASSERT( b3IsValidPosition( origin ) ); + + b3RecQueryWriter recWriter = { 0 }; + if ( world->recording != NULL ) + { + b3RecQueryBegin( &recWriter, context, filter.id, filter.name ); + recWriter.userFcn.planeFcn = fcn; + b3RecW_WORLDID( &recWriter.buf, worldId ); + b3RecW_POSITION( &recWriter.buf, origin ); + b3RecW_CAPSULE( &recWriter.buf, *mover ); + b3RecW_QUERYFILTER( &recWriter.buf, filter ); + recWriter.countOffset = b3RecReserveU32( &recWriter.buf ); + fcn = b3RecPlaneTrampoline; + context = &recWriter; + } + + b3Vec3 r = { mover->radius, mover->radius, mover->radius }; + + // Relative box lifted to world float with outward rounding, conservative for the tree + b3AABB relBox; + relBox.lowerBound = b3Sub( b3Min( mover->center1, mover->center2 ), r ); + relBox.upperBound = b3Add( b3Max( mover->center1, mover->center2 ), r ); + b3AABB aabb = b3OffsetAABB( relBox, origin ); + + WorldMoverContext worldContext = { + world, fcn, filter, *mover, origin, context, + }; + + for ( int i = 0; i < b3_bodyTypeCount; ++i ) + { + b3DynamicTree_Query( world->broadPhase.trees + i, aabb, filter.maskBits, false, TreeCollideCallback, &worldContext ); + } + + if ( world->recording != NULL ) + { + // CollideMover returns void: no treestats tail, just the per-shape plane batches. + b3RecPatchU32( &recWriter.buf, recWriter.countOffset, recWriter.hitCount ); + b3RecQueryCommit( world->recording, b3_recOpQueryCollideMover, &recWriter ); + } +} + +typedef struct WorldRayCastContext +{ + b3World* world; + b3CastResultFcn* fcn; + b3QueryFilter filter; + float fraction; + b3Pos origin; + void* userContext; +} WorldRayCastContext; + +static float RayCastCallback( const b3RayCastInput* input, int proxyId, uint64_t userData, void* context ) +{ + B3_UNUSED( proxyId ); + + int shapeId = (int)userData; + WorldRayCastContext* worldContext = (WorldRayCastContext*)context; + b3World* world = worldContext->world; + + b3Shape* shape = b3Array_Get( world->shapes, shapeId ); + b3Filter shapeFilter = shape->filter; + b3QueryFilter queryFilter = worldContext->filter; + + if ( b3ShouldQueryCollide( &shapeFilter, &queryFilter ) == false ) + { + return input->maxFraction; + } + + b3Body* body = b3Array_Get( world->bodies, shape->bodyId ); + b3WorldTransform bodyTransform = b3GetBodyTransformQuick( world, body ); + b3Transform transform = b3ToRelativeTransform( bodyTransform, worldContext->origin ); + + b3RayCastInput localInput = *input; + localInput.origin = b3Vec3_zero; + b3CastOutput output = b3RayCastShape( shape, transform, &localInput ); + + if ( output.hit ) + { + B3_ASSERT( output.fraction <= input->maxFraction ); + + b3ShapeId id = { shapeId + 1, world->worldId, shape->generation }; + b3Pos point = b3OffsetPos( worldContext->origin, output.point ); + int materialIndex = b3ClampInt( output.materialIndex, 0, shape->materialCount - 1 ); + uint64_t userMaterialId = b3GetShapeMaterials( shape )[materialIndex].userMaterialId; + + int triangleIndex = output.triangleIndex; + int childIndex = output.childIndex; + float fraction = worldContext->fcn( id, point, output.normal, output.fraction, userMaterialId, triangleIndex, childIndex, + worldContext->userContext ); + + // The user may return -1 to skip this shape + if ( 0.0f <= fraction && fraction <= 1.0f ) + { + worldContext->fraction = fraction; + } + + return fraction; + } + + return input->maxFraction; +} + +b3TreeStats b3World_CastRay( b3WorldId worldId, b3Pos origin, b3Vec3 translation, b3QueryFilter filter, b3CastResultFcn* fcn, + void* context ) +{ + b3TreeStats treeStats = { 0 }; + + b3World* world = b3GetUnlockedWorldFromId( worldId ); + if ( world == NULL ) + { + return treeStats; + } + + B3_ASSERT( b3IsValidPosition( origin ) ); + B3_ASSERT( b3IsValidVec3( translation ) ); + + b3RecQueryWriter recWriter = { 0 }; + if ( world->recording != NULL ) + { + b3RecQueryBegin( &recWriter, context, filter.id, filter.name ); + recWriter.userFcn.castFcn = fcn; + b3RecW_WORLDID( &recWriter.buf, worldId ); + b3RecW_POSITION( &recWriter.buf, origin ); + b3RecW_VEC3( &recWriter.buf, translation ); + b3RecW_QUERYFILTER( &recWriter.buf, filter ); + recWriter.countOffset = b3RecReserveU32( &recWriter.buf ); + fcn = b3RecCastTrampoline; + context = &recWriter; + } + + // The tree traverses in float relative to the world origin. Each shape is then re-differenced at + // full precision against the origin, so a hit stays accurate far from the origin. + b3RayCastInput input = { b3ToVec3( origin ), translation, 1.0f }; + + WorldRayCastContext worldContext = { world, fcn, filter, 1.0f, origin, context }; + + for ( int i = 0; i < b3_bodyTypeCount; ++i ) + { + b3TreeStats treeResult = + b3DynamicTree_RayCast( world->broadPhase.trees + i, &input, filter.maskBits, false, RayCastCallback, &worldContext ); + treeStats.nodeVisits += treeResult.nodeVisits; + treeStats.leafVisits += treeResult.leafVisits; + + if ( worldContext.fraction == 0.0f ) + { + break; + } + + input.maxFraction = worldContext.fraction; + } + + if ( world->recording != NULL ) + { + b3RecPatchU32( &recWriter.buf, recWriter.countOffset, recWriter.hitCount ); + b3RecW_TREESTATS( &recWriter.buf, treeStats ); + b3RecQueryCommit( world->recording, b3_recOpQueryCastRay, &recWriter ); + } + + return treeStats; +} + +// This callback finds the closest hit. This is the most common callback used in games. +static float b3RayCastClosestFcn( b3ShapeId shapeId, b3Pos point, b3Vec3 normal, float fraction, uint64_t userMaterialId, + int triangleIndex, int childIndex, void* context ) +{ + // Ignore initial overlap + if ( fraction == 0.0f ) + { + return -1.0f; + } + + b3RayResult* rayResult = (b3RayResult*)context; + rayResult->shapeId = shapeId; + rayResult->point = point; + rayResult->normal = normal; + rayResult->fraction = fraction; + rayResult->userMaterialId = userMaterialId; + rayResult->triangleIndex = triangleIndex; + rayResult->childIndex = childIndex; + rayResult->hit = true; + return fraction; +} + +b3RayResult b3World_CastRayClosest( b3WorldId worldId, b3Pos origin, b3Vec3 translation, b3QueryFilter filter ) +{ + b3RayResult result = { 0 }; + + b3World* world = b3GetUnlockedWorldFromId( worldId ); + if ( world == NULL ) + { + return result; + } + + B3_ASSERT( b3IsValidPosition( origin ) ); + B3_ASSERT( b3IsValidVec3( translation ) ); + + // The tree traverses in float relative to the world origin. Each shape is then re-differenced at + // full precision against its body, so a hit stays accurate far from the origin. + b3RayCastInput input = { b3ToVec3( origin ), translation, 1.0f }; + WorldRayCastContext worldContext = { + .world = world, + .fcn = b3RayCastClosestFcn, + .filter = filter, + .fraction = 1.0f, + .origin = origin, + .userContext = &result, + }; + + for ( int i = 0; i < b3_bodyTypeCount; ++i ) + { + b3TreeStats treeResult = + b3DynamicTree_RayCast( world->broadPhase.trees + i, &input, filter.maskBits, false, RayCastCallback, &worldContext ); + result.nodeVisits += treeResult.nodeVisits; + result.leafVisits += treeResult.leafVisits; + + if ( worldContext.fraction == 0.0f ) + { + break; + } + + input.maxFraction = worldContext.fraction; + } + + // Closed query, no user callback: record the inputs and the single result for the replay compare. + if ( world->recording != NULL ) + { + b3RecQueryWriter recWriter = { 0 }; + b3RecQueryBegin( &recWriter, NULL, filter.id, filter.name ); + b3RecW_WORLDID( &recWriter.buf, worldId ); + b3RecW_POSITION( &recWriter.buf, origin ); + b3RecW_VEC3( &recWriter.buf, translation ); + b3RecW_QUERYFILTER( &recWriter.buf, filter ); + b3RecW_RAYRESULT( &recWriter.buf, result ); + b3RecQueryCommit( world->recording, b3_recOpQueryCastRayClosest, &recWriter ); + } + + return result; +} + +typedef struct WorldShapeCastContext +{ + b3World* world; + b3CastResultFcn* fcn; + b3QueryFilter filter; + float fraction; + b3Pos origin; + // origin relative input + b3ShapeCastInput input; + void* userContext; +} WorldShapeCastContext; + +static float b3ShapeCastCallback( const b3BoxCastInput* input, int proxyId, uint64_t userData, void* context ) +{ + B3_UNUSED( proxyId ); + + int shapeId = (int)userData; + WorldShapeCastContext* worldContext = (WorldShapeCastContext*)context; + b3World* world = worldContext->world; + + b3Shape* shape = b3Array_Get( world->shapes, shapeId ); + b3Filter shapeFilter = shape->filter; + b3QueryFilter queryFilter = worldContext->filter; + + if ( b3ShouldQueryCollide( &shapeFilter, &queryFilter ) == false ) + { + return input->maxFraction; + } + + // Rebuild from the origin relative input, taking only the advancing fraction from the tree. + // The tree box is world float and would lose the cast far from the origin. + b3ShapeCastInput localInput = worldContext->input; + localInput.maxFraction = input->maxFraction; + + // Re-center on the query origin so the per-shape cast stays in float precision far from the origin + b3Body* body = b3Array_Get( world->bodies, shape->bodyId ); + b3Transform transform = b3ToRelativeTransform( b3GetBodyTransformQuick( world, body ), worldContext->origin ); + + b3CastOutput output = b3ShapeCastShape( shape, transform, &localInput ); + + if ( output.hit ) + { + b3ShapeId id = { shapeId + 1, world->worldId, shape->generation }; + int materialIndex = b3ClampInt( output.materialIndex, 0, shape->materialCount - 1 ); + uint64_t userMaterialId = b3GetShapeMaterials( shape )[materialIndex].userMaterialId; + + int triangleIndex = output.triangleIndex; + int childIndex = output.childIndex; + float fraction = worldContext->fcn( id, b3OffsetPos( worldContext->origin, output.point ), output.normal, output.fraction, + userMaterialId, triangleIndex, childIndex, worldContext->userContext ); + + // The user may return -1 to skip this shape + if ( 0.0f <= fraction && fraction <= 1.0f ) + { + worldContext->fraction = fraction; + } + + return fraction; + } + + return input->maxFraction; +} + +b3TreeStats b3World_CastShape( b3WorldId worldId, b3Pos origin, const b3ShapeProxy* proxy, b3Vec3 translation, + b3QueryFilter filter, b3CastResultFcn* fcn, void* context ) +{ + b3TreeStats treeStats = { 0 }; + + b3World* world = b3GetUnlockedWorldFromId( worldId ); + if ( world == NULL ) + { + return treeStats; + } + + B3_ASSERT( b3IsValidPosition( origin ) ); + B3_ASSERT( b3IsValidVec3( translation ) ); + + b3RecQueryWriter recWriter = { 0 }; + if ( world->recording != NULL ) + { + b3RecQueryBegin( &recWriter, context, filter.id, filter.name ); + recWriter.userFcn.castFcn = fcn; + b3RecW_WORLDID( &recWriter.buf, worldId ); + b3RecW_POSITION( &recWriter.buf, origin ); + b3RecW_SHAPEPROXY( &recWriter.buf, *proxy ); + b3RecW_VEC3( &recWriter.buf, translation ); + b3RecW_QUERYFILTER( &recWriter.buf, filter ); + recWriter.countOffset = b3RecReserveU32( &recWriter.buf ); + fcn = b3RecCastTrampoline; + context = &recWriter; + } + + WorldShapeCastContext worldContext = { 0 }; + worldContext.world = world; + worldContext.fcn = fcn; + worldContext.filter = filter; + worldContext.fraction = 1.0f; + worldContext.origin = origin; + worldContext.input.proxy = *proxy; + worldContext.input.translation = translation; + worldContext.input.maxFraction = 1.0f; + worldContext.input.canEncroach = false; + worldContext.userContext = context; + + // Bound the proxy in origin relative space then lift to a conservative world float box. The tree + // node boxes use the same directed rounding, so the swept box never clips a shape far from the + // origin. Per shape casts re-difference at full precision against the carried origin. + b3AABB localBox = b3MakeAABB( proxy->points, proxy->count, proxy->radius ); + b3BoxCastInput treeInput = { b3OffsetAABB( localBox, origin ), translation, 1.0f }; + + for ( int i = 0; i < b3_bodyTypeCount; ++i ) + { + b3TreeStats treeResult = b3DynamicTree_BoxCast( world->broadPhase.trees + i, &treeInput, filter.maskBits, false, + b3ShapeCastCallback, &worldContext ); + treeStats.nodeVisits += treeResult.nodeVisits; + treeStats.leafVisits += treeResult.leafVisits; + + if ( worldContext.fraction == 0.0f ) + { + break; + } + + treeInput.maxFraction = worldContext.fraction; + } + + if ( world->recording != NULL ) + { + b3RecPatchU32( &recWriter.buf, recWriter.countOffset, recWriter.hitCount ); + b3RecW_TREESTATS( &recWriter.buf, treeStats ); + b3RecQueryCommit( world->recording, b3_recOpQueryCastShape, &recWriter ); + } + + return treeStats; +} + +typedef struct WorldMoverCastContext +{ + b3World* world; + b3MoverFilterFcn* fcn; + b3QueryFilter filter; + float fraction; + b3Pos origin; + // origin relative input + b3ShapeCastInput input; + void* userContext; +} WorldMoverCastContext; + +static float MoverCastCallback( const b3BoxCastInput* input, int proxyId, uint64_t userData, void* context ) +{ + B3_UNUSED( proxyId ); + + int shapeId = (int)userData; + WorldMoverCastContext* worldContext = (WorldMoverCastContext*)context; + b3World* world = worldContext->world; + + b3Shape* shape = b3Array_Get( world->shapes, shapeId ); + b3Filter shapeFilter = shape->filter; + b3QueryFilter queryFilter = worldContext->filter; + + if ( b3ShouldQueryCollide( &shapeFilter, &queryFilter ) == false ) + { + return worldContext->fraction; + } + + if ( worldContext->fcn != NULL ) + { + b3ShapeId id = { shapeId + 1, world->worldId, shape->generation }; + bool shouldCollide = worldContext->fcn( id, worldContext->userContext ); + if ( shouldCollide == false ) + { + return worldContext->fraction; + } + } + + // Rebuild from the origin relative input, taking only the advancing fraction from the tree + b3ShapeCastInput localInput = worldContext->input; + localInput.maxFraction = input->maxFraction; + + // Re-center on the query origin so the per-shape cast stays in float precision far from the origin + b3Body* body = b3Array_Get( world->bodies, shape->bodyId ); + b3Transform transform = b3ToRelativeTransform( b3GetBodyTransformQuick( world, body ), worldContext->origin ); + + b3CastOutput output = b3ShapeCastShape( shape, transform, &localInput ); + if ( output.fraction == 0.0f ) + { + // Ignore overlapping shapes + return worldContext->fraction; + } + + worldContext->fraction = output.fraction; + return output.fraction; +} + +float b3World_CastMover( b3WorldId worldId, b3Pos origin, const b3Capsule* mover, b3Vec3 translation, b3QueryFilter filter, + b3MoverFilterFcn* fcn, void* context ) +{ + B3_ASSERT( b3IsValidPosition( origin ) ); + B3_ASSERT( b3IsValidVec3( translation ) ); + + b3World* world = b3GetUnlockedWorldFromId( worldId ); + if ( world == NULL ) + { + return 1.0f; + } + + // The mover filter is a per-shape bool(shapeId, ctx) decision, the same shape as an overlap + // callback, so the overlap trampoline captures it. Installing it even when the user passed no + // filter records an accept-all stream that replays identically. + b3RecQueryWriter recWriter = { 0 }; + if ( world->recording != NULL ) + { + b3RecQueryBegin( &recWriter, context, filter.id, filter.name ); + recWriter.userFcn.moverFilterFcn = fcn; + b3RecW_WORLDID( &recWriter.buf, worldId ); + b3RecW_POSITION( &recWriter.buf, origin ); + b3RecW_CAPSULE( &recWriter.buf, *mover ); + b3RecW_VEC3( &recWriter.buf, translation ); + b3RecW_QUERYFILTER( &recWriter.buf, filter ); + recWriter.countOffset = b3RecReserveU32( &recWriter.buf ); + fcn = b3RecOverlapTrampoline; + context = &recWriter; + } + + WorldMoverCastContext worldContext = { + .world = world, + .fcn = fcn, + .filter = filter, + .fraction = 1.0f, + .origin = origin, + .userContext = context, + }; + worldContext.input.proxy = (b3ShapeProxy){ &mover->center1, 2, mover->radius }; + worldContext.input.translation = translation; + worldContext.input.maxFraction = 1.0f; + worldContext.input.canEncroach = mover->radius > 0.0f; + + // Bound the capsule in origin relative space then lift to a conservative world float box + b3Vec3 centers[2] = { mover->center1, mover->center2 }; + b3BoxCastInput treeInput = { b3OffsetAABB( b3MakeAABB( centers, 2, mover->radius ), origin ), translation, 1.0f }; + + for ( int i = 0; i < b3_bodyTypeCount; ++i ) + { + b3DynamicTree_BoxCast( world->broadPhase.trees + i, &treeInput, filter.maskBits, false, MoverCastCallback, + &worldContext ); + + if ( worldContext.fraction == 0.0f ) + { + break; + } + + treeInput.maxFraction = worldContext.fraction; + } + + if ( world->recording != NULL ) + { + // The mover filter type aliases the overlap trampoline, so the user fcn lands in the same + // union slot. Backpatch the accept count, then record the returned fraction as the tail. + b3RecPatchU32( &recWriter.buf, recWriter.countOffset, recWriter.hitCount ); + b3RecW_F32( &recWriter.buf, worldContext.fraction ); + b3RecQueryCommit( world->recording, b3_recOpQueryCastMover, &recWriter ); + } + + return worldContext.fraction; +} + +void b3World_SetCustomFilterCallback( b3WorldId worldId, b3CustomFilterFcn* fcn, void* context ) +{ + b3World* world = b3GetUnlockedWorldFromId( worldId ); + if ( world == NULL ) + { + return; + } + world->customFilterFcn = fcn; + world->customFilterContext = context; +} + +void b3World_SetPreSolveCallback( b3WorldId worldId, b3PreSolveFcn* fcn, void* context ) +{ + b3World* world = b3GetUnlockedWorldFromId( worldId ); + if ( world == NULL ) + { + return; + } + world->preSolveFcn = fcn; + world->preSolveContext = context; +} + +void b3World_SetGravity( b3WorldId worldId, b3Vec3 gravity ) +{ + b3World* world = b3GetWorldFromId( worldId ); + + B3_REC( world, WorldSetGravity, worldId, gravity ); + + world->gravity = gravity; +} + +b3Vec3 b3World_GetGravity( b3WorldId worldId ) +{ + b3World* world = b3GetWorldFromId( worldId ); + return world->gravity; +} + +typedef struct ExplosionContext +{ + b3World* world; + b3Pos position; + float radius; + float falloff; + float impulsePerArea; +} ExplosionContext; + +static bool ExplosionCallback( int proxyId, uint64_t userData, void* context ) +{ + B3_UNUSED( proxyId ); + + int shapeId = (int)userData; + ExplosionContext* explosionContext = (ExplosionContext*)context; + b3World* world = explosionContext->world; + + b3Shape* shape = b3Array_Get( world->shapes, shapeId ); + if ( shape->explosionScale == 0.0f ) + { + return true; + } + + b3Body* body = b3Array_Get( world->bodies, shape->bodyId ); + B3_ASSERT( body->type == b3_dynamicBody ); + + b3WorldTransform xf = b3GetBodyTransformQuick( world, body ); + + // Re-center the explosion into the shape local frame so distance and direction stay precise + // far from the origin. Everything below runs in that near-origin frame. + b3Vec3 localPosition = b3InvTransformWorldPoint( xf, explosionContext->position ); + + b3DistanceInput input; + input.proxyA = b3MakeShapeProxy( shape ); + input.proxyB = (b3ShapeProxy){ &localPosition, 1, 0.0f }; + input.transform = b3Transform_identity; + input.useRadii = true; + + b3SimplexCache cache = { 0 }; + b3DistanceOutput output = b3ShapeDistance( &input, &cache, NULL, 0 ); + + float radius = explosionContext->radius; + float falloff = explosionContext->falloff; + if ( output.distance > radius + falloff ) + { + return true; + } + + b3WakeBody( world, body ); + + if ( body->setIndex != b3_awakeSet ) + { + return true; + } + + // Witness point is already in the body local query frame + b3Vec3 closestPoint = output.pointA; + if ( output.distance == 0.0f ) + { + closestPoint = b3GetShapeCentroid( shape ); + } + + b3Vec3 direction = b3Sub( closestPoint, localPosition ); + if ( b3LengthSquared( direction ) > 100.0f * FLT_EPSILON * FLT_EPSILON ) + { + direction = b3Normalize( direction ); + } + else + { + direction = (b3Vec3){ 1.0f, 0.0f, 0.0f }; + } + + float area = b3GetShapeProjectedArea( shape, direction ); + float scale = 1.0f; + if ( output.distance > radius && falloff > 0.0f ) + { + scale = b3ClampFloat( ( radius + falloff - output.distance ) / falloff, 0.0f, 1.0f ); + } + + float magnitude = explosionContext->impulsePerArea * area * scale * shape->explosionScale; + b3Vec3 impulse = b3MulSV( magnitude, b3RotateVector( xf.q, direction ) ); + + int localIndex = body->localIndex; + b3SolverSet* set = b3Array_Get( world->solverSets, b3_awakeSet ); + b3BodyState* state = b3Array_Get( set->bodyStates, localIndex ); + b3BodySim* bodySim = b3Array_Get( set->bodySims, localIndex ); + state->linearVelocity = b3MulAdd( state->linearVelocity, bodySim->invMass, impulse ); + + // Lever arm from the center of mass to the closest point, rotated to world + b3Vec3 r = b3RotateVector( xf.q, b3Sub( closestPoint, bodySim->localCenter ) ); + state->angularVelocity = b3Add( state->angularVelocity, b3MulMV( bodySim->invInertiaWorld, b3Cross( r, impulse ) ) ); + + return true; +} + +void b3World_Explode( b3WorldId worldId, const b3ExplosionDef* explosionDef ) +{ + uint64_t maskBits = explosionDef->maskBits; + b3Pos position = explosionDef->position; + float radius = explosionDef->radius; + float falloff = explosionDef->falloff; + float impulsePerArea = explosionDef->impulsePerArea; + + B3_ASSERT( b3IsValidPosition( position ) ); + B3_ASSERT( b3IsValidFloat( radius ) && radius >= 0.0f ); + B3_ASSERT( b3IsValidFloat( falloff ) && falloff >= 0.0f ); + B3_ASSERT( b3IsValidFloat( impulsePerArea ) ); + + b3World* world = b3GetUnlockedWorldFromId( worldId ); + if ( world == NULL ) + { + return; + } + + B3_REC( world, WorldExplode, worldId, *explosionDef ); + + // Locked due to waking + world->locked = true; + + struct ExplosionContext explosionContext = { world, position, radius, falloff, impulsePerArea }; + + // The broad-phase tree is float, so translate a local query box out to world with outward rounding + float extent = radius + falloff; + b3AABB localBox = { { -extent, -extent, -extent }, { extent, extent, extent } }; + b3AABB aabb = b3OffsetAABB( localBox, position ); + + b3DynamicTree_Query( world->broadPhase.trees + b3_dynamicBody, aabb, maskBits, false, ExplosionCallback, &explosionContext ); + + world->locked = false; +} + +void b3World_RebuildStaticTree( b3WorldId worldId ) +{ + b3World* world = b3GetUnlockedWorldFromId( worldId ); + if ( world == NULL ) + { + return; + } + + B3_REC( world, WorldRebuildStaticTree, worldId ); + + b3DynamicTree* staticTree = world->broadPhase.trees + b3_staticBody; + b3DynamicTree_Rebuild( staticTree, true ); +} + +void b3World_EnableSpeculative( b3WorldId worldId, bool flag ) +{ + b3World* world = b3GetUnlockedWorldFromId( worldId ); + if ( world == NULL ) + { + return; + } + + B3_REC( world, WorldEnableSpeculative, worldId, flag ); + + world->enableSpeculative = flag; +} + +#if B3_ENABLE_VALIDATION +// This validates island graph connectivity for each body +void b3ValidateConnectivity( b3World* world ) +{ + b3Body* bodies = world->bodies.data; + int bodyCapacity = world->bodies.count; + + for ( int bodyIndex = 0; bodyIndex < bodyCapacity; ++bodyIndex ) + { + b3Body* body = bodies + bodyIndex; + if ( body->id == B3_NULL_INDEX ) + { + b3ValidateFreeId( &world->bodyIdPool, bodyIndex ); + continue; + } + + B3_ASSERT( bodyIndex == body->id ); + + // Need to get the root island because islands are not merged until the next time step + int bodyIslandId = body->islandId; + int bodySetIndex = body->setIndex; + + int contactKey = body->headContactKey; + while ( contactKey != B3_NULL_INDEX ) + { + int contactId = contactKey >> 1; + int edgeIndex = contactKey & 1; + + b3Contact* contact = b3Array_Get( world->contacts, contactId ); + + bool touching = ( contact->flags & b3_contactTouchingFlag ) != 0; + if ( touching ) + { + if ( bodySetIndex != b3_staticSet ) + { + int contactIslandId = contact->islandId; + B3_ASSERT( contactIslandId == bodyIslandId ); + } + } + else + { + B3_ASSERT( contact->islandId == B3_NULL_INDEX ); + } + + contactKey = contact->edges[edgeIndex].nextKey; + } + + int jointKey = body->headJointKey; + while ( jointKey != B3_NULL_INDEX ) + { + int jointId = jointKey >> 1; + int edgeIndex = jointKey & 1; + + b3Joint* joint = b3Array_Get( world->joints, jointId ); + + int otherEdgeIndex = edgeIndex ^ 1; + + b3Body* otherBody = b3Array_Get( world->bodies, joint->edges[otherEdgeIndex].bodyId ); + + if ( bodySetIndex == b3_disabledSet || otherBody->setIndex == b3_disabledSet ) + { + B3_ASSERT( joint->islandId == B3_NULL_INDEX ); + } + else if ( bodySetIndex == b3_staticSet ) + { + // Intentional nesting + if ( otherBody->setIndex == b3_staticSet ) + { + B3_ASSERT( joint->islandId == B3_NULL_INDEX ); + } + } + else if ( body->type != b3_dynamicBody && otherBody->type != b3_dynamicBody ) + { + B3_ASSERT( joint->islandId == B3_NULL_INDEX ); + } + else + { + int jointIslandId = joint->islandId; + B3_ASSERT( jointIslandId == bodyIslandId ); + } + + jointKey = joint->edges[edgeIndex].nextKey; + } + } +} + +// Validates solver sets, but not island connectivity +void b3ValidateSolverSets( b3World* world ) +{ + B3_ASSERT( b3GetIdCapacity( &world->bodyIdPool ) == world->bodies.count ); + B3_ASSERT( b3GetIdCapacity( &world->contactIdPool ) == world->contacts.count ); + B3_ASSERT( b3GetIdCapacity( &world->jointIdPool ) == world->joints.count ); + B3_ASSERT( b3GetIdCapacity( &world->islandIdPool ) == world->islands.count ); + B3_ASSERT( b3GetIdCapacity( &world->solverSetIdPool ) == world->solverSets.count ); + + int activeSetCount = 0; + int totalBodyCount = 0; + int totalJointCount = 0; + int totalContactCount = 0; + int totalIslandCount = 0; + + // Validate all solver sets + int setCount = world->solverSets.count; + for ( int setIndex = 0; setIndex < setCount; ++setIndex ) + { + b3SolverSet* set = world->solverSets.data + setIndex; + if ( set->setIndex != B3_NULL_INDEX ) + { + activeSetCount += 1; + + if ( setIndex == b3_staticSet ) + { + B3_ASSERT( set->contactIndices.count == 0 ); + B3_ASSERT( set->islandSims.count == 0 ); + B3_ASSERT( set->bodyStates.count == 0 ); + } + else if ( setIndex == b3_disabledSet ) + { + B3_ASSERT( set->islandSims.count == 0 ); + B3_ASSERT( set->bodyStates.count == 0 ); + } + else if ( setIndex == b3_awakeSet ) + { + B3_ASSERT( set->bodySims.count == set->bodyStates.count ); + B3_ASSERT( set->jointSims.count == 0 ); + } + else + { + B3_ASSERT( set->bodyStates.count == 0 ); + } + + // Validate bodies + { + b3Body* bodies = world->bodies.data; + B3_ASSERT( set->bodySims.count >= 0 ); + totalBodyCount += set->bodySims.count; + for ( int i = 0; i < set->bodySims.count; ++i ) + { + b3BodySim* bodySim = set->bodySims.data + i; + + int bodyId = bodySim->bodyId; + B3_ASSERT( 0 <= bodyId && bodyId < world->bodies.count ); + b3Body* body = bodies + bodyId; + B3_ASSERT( body->setIndex == setIndex ); + B3_ASSERT( body->localIndex == i ); + + uint32_t syncedFlags = body->flags & ~b3_bodyTransientFlags; + B3_ASSERT( ( bodySim->flags & syncedFlags ) == syncedFlags ); + + b3BodyState* bodyState = b3GetBodyState( world, body ); + if ( bodyState != NULL ) + { + B3_ASSERT( ( bodyState->flags & syncedFlags ) == syncedFlags ); + } + + if ( body->type == b3_dynamicBody ) + { + B3_ASSERT( body->flags & b3_dynamicFlag ); + } + + if ( setIndex == b3_disabledSet ) + { + B3_ASSERT( body->headContactKey == B3_NULL_INDEX ); + } + + // Validate body shapes + int prevShapeId = B3_NULL_INDEX; + int shapeId = body->headShapeId; + while ( shapeId != B3_NULL_INDEX ) + { + b3Shape* shape = b3Array_Get( world->shapes, shapeId ); + B3_ASSERT( shape->id == shapeId ); + B3_ASSERT( shape->prevShapeId == prevShapeId ); + + if ( setIndex == b3_disabledSet ) + { + B3_ASSERT( shape->proxyKey == B3_NULL_INDEX ); + } + else if ( setIndex == b3_staticSet ) + { + B3_ASSERT( B3_PROXY_TYPE( shape->proxyKey ) == b3_staticBody ); + } + else + { + b3BodyType proxyType = B3_PROXY_TYPE( shape->proxyKey ); + B3_ASSERT( proxyType == b3_kinematicBody || proxyType == b3_dynamicBody ); + } + + prevShapeId = shapeId; + shapeId = shape->nextShapeId; + } + + // Validate body contacts + 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_staticSet ); + B3_ASSERT( contact->edges[0].bodyId == bodyId || contact->edges[1].bodyId == bodyId ); + contactKey = contact->edges[edgeIndex].nextKey; + } + + // Validate body joints + int jointKey = body->headJointKey; + while ( jointKey != B3_NULL_INDEX ) + { + int jointId = jointKey >> 1; + int edgeIndex = jointKey & 1; + + b3Joint* joint = b3Array_Get( world->joints, jointId ); + + int otherEdgeIndex = edgeIndex ^ 1; + + b3Body* otherBody = b3Array_Get( world->bodies, joint->edges[otherEdgeIndex].bodyId ); + + if ( setIndex == b3_disabledSet || otherBody->setIndex == b3_disabledSet ) + { + B3_ASSERT( joint->setIndex == b3_disabledSet ); + } + else if ( setIndex == b3_staticSet && otherBody->setIndex == b3_staticSet ) + { + B3_ASSERT( joint->setIndex == b3_staticSet ); + } + else if ( body->type != b3_dynamicBody && otherBody->type != b3_dynamicBody ) + { + B3_ASSERT( joint->setIndex == b3_staticSet ); + } + else if ( setIndex == b3_awakeSet ) + { + B3_ASSERT( joint->setIndex == b3_awakeSet ); + } + else if ( setIndex >= b3_firstSleepingSet ) + { + B3_ASSERT( joint->setIndex == setIndex ); + } + + b3JointSim* jointSim = b3GetJointSim( world, joint ); + B3_ASSERT( jointSim->jointId == jointId ); + B3_ASSERT( jointSim->bodyIdA == joint->edges[0].bodyId ); + B3_ASSERT( jointSim->bodyIdB == joint->edges[1].bodyId ); + + jointKey = joint->edges[edgeIndex].nextKey; + } + } + } + + // Validate contacts + { + B3_ASSERT( set->contactIndices.count >= 0 ); + totalContactCount += set->contactIndices.count; + for ( int i = 0; i < set->contactIndices.count; ++i ) + { + int contactIndex = set->contactIndices.data[i]; + b3Contact* contact = b3Array_Get( world->contacts, contactIndex ); + if ( setIndex == b3_awakeSet ) + { + // contact should be non-touching if awake + // or it could be this contact hasn't been transferred yet + B3_ASSERT( contact->manifoldCount == 0 || ( contact->flags & b3_simStartedTouching ) != 0 ); + } + B3_ASSERT( contact->setIndex == setIndex ); + B3_ASSERT( contact->colorIndex == B3_NULL_INDEX ); + B3_ASSERT( contact->localIndex == i ); + } + } + + // Validate joints + { + B3_ASSERT( set->jointSims.count >= 0 ); + totalJointCount += set->jointSims.count; + for ( int i = 0; i < set->jointSims.count; ++i ) + { + b3JointSim* jointSim = set->jointSims.data + i; + b3Joint* joint = b3Array_Get( world->joints, jointSim->jointId ); + B3_ASSERT( joint->setIndex == setIndex ); + B3_ASSERT( joint->colorIndex == B3_NULL_INDEX ); + B3_ASSERT( joint->localIndex == i ); + } + } + + // Validate islands + { + B3_ASSERT( set->islandSims.count >= 0 ); + totalIslandCount += set->islandSims.count; + for ( int i = 0; i < set->islandSims.count; ++i ) + { + b3IslandSim* islandSim = set->islandSims.data + i; + b3Island* island = b3Array_Get( world->islands, islandSim->islandId ); + B3_ASSERT( island->setIndex == setIndex ); + B3_ASSERT( island->localIndex == i ); + } + } + } + else + { + B3_ASSERT( set->bodySims.count == 0 ); + B3_ASSERT( set->contactIndices.count == 0 ); + B3_ASSERT( set->jointSims.count == 0 ); + B3_ASSERT( set->islandSims.count == 0 ); + B3_ASSERT( set->bodyStates.count == 0 ); + } + } + + int setIdCount = b3GetIdCount( &world->solverSetIdPool ); + B3_ASSERT( activeSetCount == setIdCount ); + + int bodyIdCount = b3GetIdCount( &world->bodyIdPool ); + B3_ASSERT( totalBodyCount == bodyIdCount ); + + int islandIdCount = b3GetIdCount( &world->islandIdPool ); + B3_ASSERT( totalIslandCount == islandIdCount ); + + // Validate constraint graph + for ( int colorIndex = 0; colorIndex < B3_GRAPH_COLOR_COUNT; ++colorIndex ) + { + b3GraphColor* color = world->constraintGraph.colors + colorIndex; + int bitCount = 0; + + B3_ASSERT( color->convexContacts.count >= 0 ); + totalContactCount += color->convexContacts.count; + for ( int i = 0; i < color->convexContacts.count; ++i ) + { + int contactId = color->convexContacts.data[i]; + b3Contact* contact = b3Array_Get( world->contacts, contactId ); + // contact should be touching in the constraint graph or awaiting transfer to non-touching + B3_ASSERT( contact->manifoldCount > 0 || ( contact->flags & ( b3_simStoppedTouching | b3_simDisjoint ) ) != 0 ); + B3_ASSERT( contact->setIndex == b3_awakeSet ); + B3_ASSERT( contact->colorIndex == colorIndex ); + B3_ASSERT( contact->localIndex == i ); + + int bodyIdA = contact->edges[0].bodyId; + int bodyIdB = contact->edges[1].bodyId; + + if ( colorIndex < B3_OVERFLOW_INDEX ) + { + b3Body* bodyA = b3Array_Get( world->bodies, bodyIdA ); + b3Body* bodyB = b3Array_Get( world->bodies, bodyIdB ); + B3_ASSERT( b3GetBit( &color->bodySet, bodyIdA ) == ( bodyA->type == b3_dynamicBody ) ); + B3_ASSERT( b3GetBit( &color->bodySet, bodyIdB ) == ( bodyB->type == b3_dynamicBody ) ); + + bitCount += bodyA->type == b3_dynamicBody ? 1 : 0; + bitCount += bodyB->type == b3_dynamicBody ? 1 : 0; + } + } + + totalContactCount += color->contacts.count; + for ( int i = 0; i < color->contacts.count; ++i ) + { + int contactId = color->contacts.data[i].contactId; + b3Contact* contact = b3Array_Get( world->contacts, contactId ); + // contact should be touching in the constraint graph or awaiting transfer to non-touching + B3_ASSERT( contact->manifoldCount > 0 || ( contact->flags & ( b3_simStoppedTouching | b3_simDisjoint ) ) != 0 ); + B3_ASSERT( contact->setIndex == b3_awakeSet ); + B3_ASSERT( contact->colorIndex == colorIndex ); + B3_ASSERT( contact->localIndex == i ); + + int bodyIdA = contact->edges[0].bodyId; + int bodyIdB = contact->edges[1].bodyId; + + if ( colorIndex < B3_OVERFLOW_INDEX ) + { + b3Body* bodyA = b3Array_Get( world->bodies, bodyIdA ); + b3Body* bodyB = b3Array_Get( world->bodies, bodyIdB ); + B3_ASSERT( b3GetBit( &color->bodySet, bodyIdA ) == ( bodyA->type == b3_dynamicBody ) ); + B3_ASSERT( b3GetBit( &color->bodySet, bodyIdB ) == ( bodyB->type == b3_dynamicBody ) ); + + bitCount += bodyA->type == b3_dynamicBody ? 1 : 0; + bitCount += bodyB->type == b3_dynamicBody ? 1 : 0; + } + } + + B3_ASSERT( color->jointSims.count >= 0 ); + totalJointCount += color->jointSims.count; + for ( int i = 0; i < color->jointSims.count; ++i ) + { + b3JointSim* jointSim = color->jointSims.data + i; + b3Joint* joint = b3Array_Get( world->joints, jointSim->jointId ); + B3_ASSERT( joint->setIndex == b3_awakeSet ); + B3_ASSERT( joint->colorIndex == colorIndex ); + B3_ASSERT( joint->localIndex == i ); + + int bodyIdA = joint->edges[0].bodyId; + int bodyIdB = joint->edges[1].bodyId; + + if ( colorIndex < B3_OVERFLOW_INDEX ) + { + b3Body* bodyA = b3Array_Get( world->bodies, bodyIdA ); + b3Body* bodyB = b3Array_Get( world->bodies, bodyIdB ); + B3_ASSERT( b3GetBit( &color->bodySet, bodyIdA ) == ( bodyA->type == b3_dynamicBody ) ); + B3_ASSERT( b3GetBit( &color->bodySet, bodyIdB ) == ( bodyB->type == b3_dynamicBody ) ); + + bitCount += bodyA->type == b3_dynamicBody ? 1 : 0; + bitCount += bodyB->type == b3_dynamicBody ? 1 : 0; + } + } + + // Validate the bit population for this graph color + B3_ASSERT( bitCount == b3CountSetBits( &color->bodySet ) ); + } + + int contactIdCount = b3GetIdCount( &world->contactIdPool ); + B3_ASSERT( totalContactCount == contactIdCount ); + B3_ASSERT( totalContactCount == (int)world->broadPhase.pairSet.count ); + + int jointIdCount = b3GetIdCount( &world->jointIdPool ); + B3_ASSERT( totalJointCount == jointIdCount ); + +// Validate shapes +// This is very slow on compounds +#if 0 + int shapeCapacity = b3Array(world->shapeArray).count; + for (int shapeIndex = 0; shapeIndex < shapeCapacity; shapeIndex += 1) + { + b3Shape* shape = world->shapeArray + shapeIndex; + if (shape->id != shapeIndex) + { + continue; + } + + B3_ASSERT(0 <= shape->bodyId && shape->bodyId < b3Array(world->bodyArray).count); + + b3Body* body = world->bodyArray + shape->bodyId; + B3_ASSERT(0 <= body->setIndex && body->setIndex < b3Array(world->solverSetArray).count); + + b3SolverSet* set = world->solverSetArray + body->setIndex; + B3_ASSERT(0 <= body->localIndex && body->localIndex < set->sims.count); + + b3BodySim* bodySim = set->sims.mData + body->localIndex; + B3_ASSERT(bodySim->bodyId == shape->bodyId); + + bool found = false; + int shapeCount = 0; + int index = body->headShapeId; + while (index != B3_NULL_INDEX) + { + b3CheckId(world->shapeArray, index); + b3Shape* s = world->shapeArray + index; + if (index == shapeIndex) + { + found = true; + } + + index = s->nextShapeId; + shapeCount += 1; + } + + B3_ASSERT(found); + B3_ASSERT(shapeCount == body->shapeCount); + } +#endif +} + +// Validate contact touching status. +void b3ValidateContacts( b3World* world ) +{ + b3ConstraintGraph* graph = &world->constraintGraph; + int contactCount = world->contacts.count; + B3_ASSERT( contactCount == b3GetIdCapacity( &world->contactIdPool ) ); + int allocatedContactCount = 0; + + for ( int contactIndex = 0; contactIndex < contactCount; ++contactIndex ) + { + b3Contact* contact = b3Array_Get( world->contacts, contactIndex ); + if ( contact->contactId == B3_NULL_INDEX ) + { + continue; + } + + B3_ASSERT( contact->contactId == contactIndex ); + + allocatedContactCount += 1; + + bool touching = ( contact->flags & b3_contactTouchingFlag ) != 0; + + int setId = contact->setIndex; + b3SolverSet* set = b3Array_Get( world->solverSets, setId ); + + if ( setId == b3_awakeSet ) + { + if ( touching ) + { + B3_ASSERT( 0 <= contact->colorIndex && contact->colorIndex < B3_GRAPH_COLOR_COUNT ); + // Validate body sim indices + b3Shape* shapeA = b3Array_Get( world->shapes, contact->shapeIdA ); + b3Shape* shapeB = b3Array_Get( world->shapes, contact->shapeIdB ); + + b3Body* bodyA = b3Array_Get( world->bodies, shapeA->bodyId ); + b3Body* bodyB = b3Array_Get( world->bodies, shapeB->bodyId ); + + if ( bodyA->type == b3_staticBody ) + { + B3_ASSERT( contact->bodySimIndexA == B3_NULL_INDEX ); + } + else + { + B3_ASSERT( contact->bodySimIndexA == bodyA->localIndex ); + } + + if ( bodyB->type == b3_staticBody ) + { + B3_ASSERT( contact->bodySimIndexB == B3_NULL_INDEX ); + } + else + { + B3_ASSERT( contact->bodySimIndexB == bodyB->localIndex ); + } + + if ( ( contact->flags & b3_simMeshContact ) != 0 || contact->colorIndex == B3_OVERFLOW_INDEX ) + { + b3GraphColor* color = graph->colors + contact->colorIndex; + int contactId = b3Array_Get( color->contacts, contact->localIndex )->contactId; + B3_ASSERT( contactId == contactIndex ); + } + else + { + b3GraphColor* color = graph->colors + contact->colorIndex; + int contactId = *b3Array_Get( color->convexContacts, contact->localIndex ); + B3_ASSERT( contactId == contactIndex ); + } + } + else + { + B3_ASSERT( contact->colorIndex == B3_NULL_INDEX ); + B3_ASSERT( contact->manifolds == NULL ); + B3_ASSERT( contact->manifoldCount == 0 ); + + int* index = b3Array_Get( set->contactIndices, contact->localIndex ); + B3_ASSERT( *index == contactIndex ); + } + } + else if ( setId >= b3_firstSleepingSet ) + { + // Only touching contacts allowed in a sleeping set + B3_ASSERT( touching == true ); + B3_ASSERT( contact->manifolds != NULL ); + B3_ASSERT( contact->manifoldCount > 0 ); + int* index = b3Array_Get( set->contactIndices, contact->localIndex ); + B3_ASSERT( *index == contactIndex ); + } + else + { + // Sleeping and non-touching contacts belong in the disabled set + B3_ASSERT( touching == false && setId == b3_disabledSet ); + B3_ASSERT( contact->manifolds == NULL ); + B3_ASSERT( contact->manifoldCount == 0 ); + int* index = b3Array_Get( set->contactIndices, contact->localIndex ); + B3_ASSERT( *index == contactIndex ); + } + + if ( contact->flags & b3_simMeshContact ) + { + int cacheCount = contact->meshContact.triangleCache.count; + if ( cacheCount > 0 ) + { + B3_ASSERT( contact->meshContact.triangleCache.data != NULL ); + B3_ASSERT( contact->meshContact.triangleCache.capacity >= cacheCount ); + + b3Shape* shapeA = b3Array_Get( world->shapes, contact->shapeIdA ); + if ( shapeA->type == b3_meshShape ) + { + int triangleCount = shapeA->mesh.data->triangleCount; + for ( int i = 0; i < cacheCount; ++i ) + { + int triangleIndex = contact->meshContact.triangleCache.data[i].triangleIndex; + B3_ASSERT( 0 <= triangleIndex && triangleIndex < triangleCount ); + } + } + else if ( shapeA->type == b3_heightShape ) + { + int triangleCount = b3GetHeightFieldTriangleCount( shapeA->heightField ); + for ( int i = 0; i < cacheCount; ++i ) + { + int triangleIndex = contact->meshContact.triangleCache.data[i].triangleIndex; + B3_ASSERT( 0 <= triangleIndex && triangleIndex < triangleCount ); + } + } + else + { + B3_ASSERT( shapeA->type == b3_compoundShape ); + b3ChildShape child = b3GetCompoundChild( shapeA->compound, contact->childIndex ); + B3_ASSERT( child.type == b3_meshShape ); + + int triangleCount = child.mesh.data->triangleCount; + for ( int i = 0; i < cacheCount; ++i ) + { + int triangleIndex = contact->meshContact.triangleCache.data[i].triangleIndex; + B3_ASSERT( 0 <= triangleIndex && triangleIndex < triangleCount ); + } + } + } + } + } + + int contactIdCount = b3GetIdCount( &world->contactIdPool ); + B3_ASSERT( allocatedContactCount == contactIdCount ); +} + +#else + +void b3ValidateConnectivity( b3World* world ) +{ + B3_UNUSED( world ); +} + +void b3ValidateSolverSets( b3World* world ) +{ + B3_UNUSED( world ); +} + +void b3ValidateContacts( b3World* world ) +{ + B3_UNUSED( world ); +} + +#endif diff --git a/vendor/box3d/src/src/physics_world.h b/vendor/box3d/src/src/physics_world.h new file mode 100644 index 000000000..12f169561 --- /dev/null +++ b/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 ); +} + diff --git a/vendor/box3d/src/src/platform.h b/vendor/box3d/src/src/platform.h new file mode 100644 index 000000000..2bc37a591 --- /dev/null +++ b/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 +#include + +// 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 +#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 +} diff --git a/vendor/box3d/src/src/prismatic_joint.c b/vendor/box3d/src/src/prismatic_joint.c new file mode 100644 index 000000000..c96e6d643 --- /dev/null +++ b/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 ); +} diff --git a/vendor/box3d/src/src/qsort.h b/vendor/box3d/src/src/qsort.h new file mode 100644 index 000000000..9ab426bac --- /dev/null +++ b/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: */ diff --git a/vendor/box3d/src/src/recording.c b/vendor/box3d/src/src/recording.c new file mode 100644 index 000000000..4d5c5995d --- /dev/null +++ b/vendor/box3d/src/src/recording.c @@ -0,0 +1,1279 @@ +// SPDX-FileCopyrightText: 2026 Erin Catto +// SPDX-License-Identifier: MIT + +#if defined( _MSC_VER ) && !defined( _CRT_SECURE_NO_WARNINGS ) +#define _CRT_SECURE_NO_WARNINGS +#endif + +#include "recording.h" + +#include "body.h" +#include "compound.h" +#include "physics_world.h" +#include "world_snapshot.h" + +#include "box3d/box3d.h" +#include "box3d/constants.h" + +#include +#include + +// Buffer helpers + +void b3RecBufAppend( b3RecBuffer* buf, const void* data, int size ) +{ + if ( size <= 0 ) + { + return; + } + + if ( buf->countOnly ) + { + buf->size += size; + return; + } + + if ( buf->size + size > buf->capacity ) + { + int newCap = buf->capacity * 2; + if ( newCap < buf->size + size + 64 ) + { + newCap = buf->size + size + 64; + } + if ( buf->data == NULL ) + { + buf->data = b3Alloc( (size_t)newCap ); + } + else + { + buf->data = b3GrowAlloc( buf->data, buf->capacity, newCap ); + } + buf->capacity = newCap; + } + + memcpy( buf->data + buf->size, data, (size_t)size ); + buf->size += size; +} + +void b3RecBufFree( b3RecBuffer* buf ) +{ + if ( buf->data != NULL ) + { + b3Free( buf->data, (size_t)buf->capacity ); + buf->data = NULL; + buf->capacity = 0; + buf->size = 0; + } +} + +// Write primitives + +void b3RecW_U8( b3RecBuffer* buf, uint8_t v ) +{ + b3RecBufAppend( buf, &v, 1 ); +} + +void b3RecW_U16( b3RecBuffer* buf, uint16_t v ) +{ + uint8_t b[2] = { (uint8_t)v, (uint8_t)( v >> 8 ) }; + b3RecBufAppend( buf, b, 2 ); +} + +void b3RecW_U32( b3RecBuffer* buf, uint32_t v ) +{ + uint8_t b[4] = { (uint8_t)v, (uint8_t)( v >> 8 ), (uint8_t)( v >> 16 ), (uint8_t)( v >> 24 ) }; + b3RecBufAppend( buf, b, 4 ); +} + +void b3RecW_U64( b3RecBuffer* buf, uint64_t v ) +{ + uint8_t b[8] = { (uint8_t)v, (uint8_t)( v >> 8 ), (uint8_t)( v >> 16 ), (uint8_t)( v >> 24 ), + (uint8_t)( v >> 32 ), (uint8_t)( v >> 40 ), (uint8_t)( v >> 48 ), (uint8_t)( v >> 56 ) }; + b3RecBufAppend( buf, b, 8 ); +} + +void b3RecW_I32( b3RecBuffer* buf, int32_t v ) +{ + b3RecW_U32( buf, (uint32_t)v ); +} + +void b3RecW_F32( b3RecBuffer* buf, float v ) +{ + uint32_t bits; + memcpy( &bits, &v, 4 ); + b3RecW_U32( buf, bits ); +} + +void b3RecW_F64( b3RecBuffer* buf, double v ) +{ + uint64_t bits; + memcpy( &bits, &v, 8 ); + b3RecW_U64( buf, bits ); +} + +void b3RecW_BOOL( b3RecBuffer* buf, bool v ) +{ + b3RecW_U8( buf, v ? 1u : 0u ); +} + +void b3RecW_VEC3( b3RecBuffer* buf, b3Vec3 v ) +{ + b3RecW_F32( buf, v.x ); + b3RecW_F32( buf, v.y ); + b3RecW_F32( buf, v.z ); +} + +void b3RecW_QUAT( b3RecBuffer* buf, b3Quat v ) +{ + b3RecW_F32( buf, v.v.x ); + b3RecW_F32( buf, v.v.y ); + b3RecW_F32( buf, v.v.z ); + b3RecW_F32( buf, v.s ); +} + +void b3RecW_TRANSFORM( b3RecBuffer* buf, b3Transform v ) +{ + b3RecW_VEC3( buf, v.p ); + b3RecW_QUAT( buf, v.q ); +} + +// World position at full precision so recordings reproduce the simulation far from the origin. +// In the float build this is three floats, wire-identical to VEC3. +void b3RecW_POSITION( b3RecBuffer* buf, b3Pos v ) +{ +#if defined( BOX3D_DOUBLE_PRECISION ) + b3RecW_F64( buf, v.x ); + b3RecW_F64( buf, v.y ); + b3RecW_F64( buf, v.z ); +#else + b3RecW_F32( buf, v.x ); + b3RecW_F32( buf, v.y ); + b3RecW_F32( buf, v.z ); +#endif +} + +void b3RecW_WORLDXF( b3RecBuffer* buf, b3WorldTransform v ) +{ + b3RecW_POSITION( buf, v.p ); + b3RecW_QUAT( buf, v.q ); +} + +void b3RecW_MATRIX3( b3RecBuffer* buf, b3Matrix3 v ) +{ + b3RecW_VEC3( buf, v.cx ); + b3RecW_VEC3( buf, v.cy ); + b3RecW_VEC3( buf, v.cz ); +} + +void b3RecW_AABB( b3RecBuffer* buf, b3AABB v ) +{ + b3RecW_VEC3( buf, v.lowerBound ); + b3RecW_VEC3( buf, v.upperBound ); +} + +void b3RecW_QUERYFILTER( b3RecBuffer* buf, b3QueryFilter v ) +{ + b3RecW_U64( buf, v.categoryBits ); + b3RecW_U64( buf, v.maskBits ); +} + +// Variable length: count, then count points, then radius. The point cloud lives behind a pointer so +// it cannot ride along as POD. +void b3RecW_SHAPEPROXY( b3RecBuffer* buf, b3ShapeProxy v ) +{ + int count = v.count; + if ( count < 0 ) + count = 0; + if ( count > B3_MAX_SHAPE_CAST_POINTS ) + count = B3_MAX_SHAPE_CAST_POINTS; + b3RecW_I32( buf, count ); + for ( int i = 0; i < count; ++i ) + { + b3RecW_VEC3( buf, v.points[i] ); + } + b3RecW_F32( buf, v.radius ); +} + +void b3RecW_TREESTATS( b3RecBuffer* buf, b3TreeStats v ) +{ + b3RecW_I32( buf, v.nodeVisits ); + b3RecW_I32( buf, v.leafVisits ); +} + +void b3RecW_RAYRESULT( b3RecBuffer* buf, b3RayResult v ) +{ + b3RecW_SHAPEID( buf, v.shapeId ); + b3RecW_POSITION( buf, v.point ); + b3RecW_VEC3( buf, v.normal ); + b3RecW_U64( buf, v.userMaterialId ); + b3RecW_F32( buf, v.fraction ); + b3RecW_I32( buf, v.triangleIndex ); + b3RecW_I32( buf, v.childIndex ); + b3RecW_BOOL( buf, v.hit ); +} + +void b3RecW_PLANERESULT( b3RecBuffer* buf, b3PlaneResult v ) +{ + b3RecW_VEC3( buf, v.plane.normal ); + b3RecW_F32( buf, v.plane.offset ); + b3RecW_VEC3( buf, v.point ); +} + +void b3RecW_WORLDID( b3RecBuffer* buf, b3WorldId v ) +{ + b3RecW_U32( buf, b3StoreWorldId( v ) ); +} + +void b3RecW_BODYID( b3RecBuffer* buf, b3BodyId v ) +{ + b3RecW_U64( buf, b3StoreBodyId( v ) ); +} + +void b3RecW_SHAPEID( b3RecBuffer* buf, b3ShapeId v ) +{ + b3RecW_U64( buf, b3StoreShapeId( v ) ); +} + +void b3RecW_JOINTID( b3RecBuffer* buf, b3JointId v ) +{ + b3RecW_U64( buf, b3StoreJointId( v ) ); +} + +// Pointer-free POD; pointerWidth in the header gates the layout on replay +void b3RecW_SPHERE( b3RecBuffer* buf, b3Sphere v ) +{ + b3RecBufAppend( buf, &v, (int)sizeof( b3Sphere ) ); +} + +void b3RecW_CAPSULE( b3RecBuffer* buf, b3Capsule v ) +{ + b3RecBufAppend( buf, &v, (int)sizeof( b3Capsule ) ); +} + +void b3RecW_GEOMID( b3RecBuffer* buf, uint32_t v ) +{ + b3RecW_U32( buf, v ); +} + +void b3RecW_FILTER( b3RecBuffer* buf, b3Filter v ) +{ + b3RecW_U64( buf, v.categoryBits ); + b3RecW_U64( buf, v.maskBits ); + b3RecW_I32( buf, v.groupIndex ); +} + +void b3RecW_MATERIAL( b3RecBuffer* buf, b3SurfaceMaterial v ) +{ + b3RecW_F32( buf, v.friction ); + b3RecW_F32( buf, v.restitution ); + b3RecW_F32( buf, v.rollingResistance ); + b3RecW_VEC3( buf, v.tangentVelocity ); + b3RecW_U64( buf, v.userMaterialId ); + b3RecW_U32( buf, v.customColor ); +} + +void b3RecW_MASSDATA( b3RecBuffer* buf, b3MassData v ) +{ + b3RecW_F32( buf, v.mass ); + b3RecW_VEC3( buf, v.center ); + b3RecW_MATRIX3( buf, v.inertia ); +} + +void b3RecW_LOCKS( b3RecBuffer* buf, b3MotionLocks v ) +{ + b3RecW_BOOL( buf, v.linearX ); + b3RecW_BOOL( buf, v.linearY ); + b3RecW_BOOL( buf, v.linearZ ); + b3RecW_BOOL( buf, v.angularX ); + b3RecW_BOOL( buf, v.angularY ); + b3RecW_BOOL( buf, v.angularZ ); +} + +static void b3RecW_STR( b3RecBuffer* buf, const char* s ) +{ + if ( s == NULL ) + { + b3RecW_U16( buf, 0xFFFFu ); + return; + } + int len = 0; + while ( s[len] != '\0' && len < 65534 ) + { + len++; + } + b3RecW_U16( buf, (uint16_t)len ); + if ( len > 0 ) + { + b3RecBufAppend( buf, s, len ); + } +} + +// Hand-written def helpers. Zero pointer and cookie fields before serializing. +// Readers call b3Default*Def() first to get the cookie, then overwrite fields. + +// Tripwire: each def serializer below is paired with a reader in recording_replay.c, and the two must +// stay field-for-field in sync. Add a field to a def and the size changes, firing the matching assert +// so the writer and reader both get updated. Only enforced on the 64-bit target; each def lists the +// single-precision and double-precision sizes (equal for most), so either build configuration passes. +_Static_assert( sizeof( void* ) != 8 || sizeof( b3ExplosionDef ) == 32 || sizeof( b3ExplosionDef ) == 48, + "b3ExplosionDef changed: update b3RecW_EXPLOSIONDEF and b3RecR_EXPLOSIONDEF together" ); +_Static_assert( sizeof( void* ) != 8 || sizeof( b3BodyDef ) == 104 || sizeof( b3BodyDef ) == 120, + "b3BodyDef changed: update b3RecW_BODYDEF and b3RecR_BODYDEF together" ); +_Static_assert( sizeof( void* ) != 8 || sizeof( b3ShapeDef ) == 120, + "b3ShapeDef changed: update b3RecW_SHAPEDEF and b3RecR_SHAPEDEF together" ); +_Static_assert( sizeof( void* ) != 8 || sizeof( b3ParallelJointDef ) == 128, + "b3ParallelJointDef changed: update b3RecW_PARALLELJOINTDEF and its reader together" ); +_Static_assert( sizeof( void* ) != 8 || sizeof( b3DistanceJointDef ) == 160, + "b3DistanceJointDef changed: update b3RecW_DISTANCEJOINTDEF and its reader together" ); +_Static_assert( sizeof( void* ) != 8 || sizeof( b3FilterJointDef ) == 112, + "b3FilterJointDef changed: update b3RecW_FILTERJOINTDEF and its reader together" ); +_Static_assert( sizeof( void* ) != 8 || sizeof( b3MotorJointDef ) == 168, + "b3MotorJointDef changed: update b3RecW_MOTORJOINTDEF and its reader together" ); +_Static_assert( sizeof( void* ) != 8 || sizeof( b3PrismaticJointDef ) == 152, + "b3PrismaticJointDef changed: update b3RecW_PRISMATICJOINTDEF and its reader together" ); +_Static_assert( sizeof( void* ) != 8 || sizeof( b3RevoluteJointDef ) == 152, + "b3RevoluteJointDef changed: update b3RecW_REVOLUTEJOINTDEF and its reader together" ); +_Static_assert( sizeof( void* ) != 8 || sizeof( b3SphericalJointDef ) == 184, + "b3SphericalJointDef changed: update b3RecW_SPHERICALJOINTDEF and its reader together" ); +_Static_assert( sizeof( void* ) != 8 || sizeof( b3WeldJointDef ) == 128, + "b3WeldJointDef changed: update b3RecW_WELDJOINTDEF and its reader together" ); +_Static_assert( sizeof( void* ) != 8 || sizeof( b3WheelJointDef ) == 184, + "b3WheelJointDef changed: update b3RecW_WHEELJOINTDEF and its reader together" ); + +void b3RecW_EXPLOSIONDEF( b3RecBuffer* buf, b3ExplosionDef v ) +{ + b3RecW_U64( buf, v.maskBits ); + b3RecW_POSITION( buf, v.position ); + b3RecW_F32( buf, v.radius ); + b3RecW_F32( buf, v.falloff ); + b3RecW_F32( buf, v.impulsePerArea ); +} + +void b3RecW_BODYDEF( b3RecBuffer* buf, b3BodyDef v ) +{ + b3RecW_I32( buf, (int32_t)v.type ); + b3RecW_POSITION( buf, v.position ); + b3RecW_QUAT( buf, v.rotation ); + b3RecW_VEC3( buf, v.linearVelocity ); + b3RecW_VEC3( buf, v.angularVelocity ); + b3RecW_F32( buf, v.linearDamping ); + b3RecW_F32( buf, v.angularDamping ); + b3RecW_F32( buf, v.gravityScale ); + b3RecW_F32( buf, v.sleepThreshold ); + b3RecW_STR( buf, v.name ); + // userData: not preserved + b3RecW_U64( buf, 0u ); + b3RecW_LOCKS( buf, v.motionLocks ); + b3RecW_BOOL( buf, v.enableSleep ); + b3RecW_BOOL( buf, v.isAwake ); + b3RecW_BOOL( buf, v.isBullet ); + b3RecW_BOOL( buf, v.isEnabled ); + b3RecW_BOOL( buf, v.allowFastRotation ); + b3RecW_BOOL( buf, v.enableContactRecycling ); + // internalValue omitted +} + +void b3RecW_SHAPEDEF( b3RecBuffer* buf, b3ShapeDef v ) +{ + b3RecW_STR( buf, v.name ); + + // userData: not preserved + b3RecW_U64( buf, 0u ); + // Per-triangle materials: length-prefixed so the reader can rebuild the array. + // Guard NULL so a default def (materialCount=0, materials=NULL) round-trips cleanly. + int matCount = ( v.materials != NULL ) ? v.materialCount : 0; + b3RecW_I32( buf, matCount ); + for ( int i = 0; i < matCount; ++i ) + { + b3RecW_MATERIAL( buf, v.materials[i] ); + } + b3RecW_MATERIAL( buf, v.baseMaterial ); + b3RecW_F32( buf, v.density ); + b3RecW_F32( buf, v.explosionScale ); + b3RecW_FILTER( buf, v.filter ); + b3RecW_BOOL( buf, v.enableCustomFiltering ); + b3RecW_BOOL( buf, v.isSensor ); + b3RecW_BOOL( buf, v.enableSensorEvents ); + b3RecW_BOOL( buf, v.enableContactEvents ); + b3RecW_BOOL( buf, v.enableHitEvents ); + b3RecW_BOOL( buf, v.enablePreSolveEvents ); + b3RecW_BOOL( buf, v.invokeContactCreation ); + b3RecW_BOOL( buf, v.updateBodyMass ); + b3RecW_BOOL( buf, v.enableSpeculativeContact ); + // internalValue omitted +} + +// Joint defs share a base. Body ids are written as packed ids for replay remapping. +static void b3RecW_JointBase( b3RecBuffer* buf, const b3JointDef* base ) +{ + // userData: not preserved + b3RecW_U64( buf, 0u ); + b3RecW_BODYID( buf, base->bodyIdA ); + b3RecW_BODYID( buf, base->bodyIdB ); + b3RecW_TRANSFORM( buf, base->localFrameA ); + b3RecW_TRANSFORM( buf, base->localFrameB ); + b3RecW_F32( buf, base->forceThreshold ); + b3RecW_F32( buf, base->torqueThreshold ); + b3RecW_F32( buf, base->constraintHertz ); + b3RecW_F32( buf, base->constraintDampingRatio ); + b3RecW_F32( buf, base->drawScale ); + b3RecW_BOOL( buf, base->collideConnected ); + // internalValue omitted +} + +void b3RecW_PARALLELJOINTDEF( b3RecBuffer* buf, b3ParallelJointDef v ) +{ + b3RecW_JointBase( buf, &v.base ); + b3RecW_F32( buf, v.hertz ); + b3RecW_F32( buf, v.dampingRatio ); + b3RecW_F32( buf, v.maxTorque ); +} + +void b3RecW_DISTANCEJOINTDEF( b3RecBuffer* buf, b3DistanceJointDef v ) +{ + b3RecW_JointBase( buf, &v.base ); + b3RecW_F32( buf, v.length ); + b3RecW_BOOL( buf, v.enableSpring ); + b3RecW_F32( buf, v.lowerSpringForce ); + b3RecW_F32( buf, v.upperSpringForce ); + b3RecW_F32( buf, v.hertz ); + b3RecW_F32( buf, v.dampingRatio ); + b3RecW_BOOL( buf, v.enableLimit ); + b3RecW_F32( buf, v.minLength ); + b3RecW_F32( buf, v.maxLength ); + b3RecW_BOOL( buf, v.enableMotor ); + b3RecW_F32( buf, v.maxMotorForce ); + b3RecW_F32( buf, v.motorSpeed ); +} + +void b3RecW_FILTERJOINTDEF( b3RecBuffer* buf, b3FilterJointDef v ) +{ + b3RecW_JointBase( buf, &v.base ); +} + +void b3RecW_MOTORJOINTDEF( b3RecBuffer* buf, b3MotorJointDef v ) +{ + b3RecW_JointBase( buf, &v.base ); + b3RecW_VEC3( buf, v.linearVelocity ); + b3RecW_F32( buf, v.maxVelocityForce ); + b3RecW_VEC3( buf, v.angularVelocity ); + b3RecW_F32( buf, v.maxVelocityTorque ); + b3RecW_F32( buf, v.linearHertz ); + b3RecW_F32( buf, v.linearDampingRatio ); + b3RecW_F32( buf, v.maxSpringForce ); + b3RecW_F32( buf, v.angularHertz ); + b3RecW_F32( buf, v.angularDampingRatio ); + b3RecW_F32( buf, v.maxSpringTorque ); +} + +void b3RecW_PRISMATICJOINTDEF( b3RecBuffer* buf, b3PrismaticJointDef v ) +{ + b3RecW_JointBase( buf, &v.base ); + b3RecW_BOOL( buf, v.enableSpring ); + b3RecW_F32( buf, v.hertz ); + b3RecW_F32( buf, v.dampingRatio ); + b3RecW_F32( buf, v.targetTranslation ); + b3RecW_BOOL( buf, v.enableLimit ); + b3RecW_F32( buf, v.lowerTranslation ); + b3RecW_F32( buf, v.upperTranslation ); + b3RecW_BOOL( buf, v.enableMotor ); + b3RecW_F32( buf, v.maxMotorForce ); + b3RecW_F32( buf, v.motorSpeed ); +} + +void b3RecW_REVOLUTEJOINTDEF( b3RecBuffer* buf, b3RevoluteJointDef v ) +{ + b3RecW_JointBase( buf, &v.base ); + b3RecW_F32( buf, v.targetAngle ); + b3RecW_BOOL( buf, v.enableSpring ); + b3RecW_F32( buf, v.hertz ); + b3RecW_F32( buf, v.dampingRatio ); + b3RecW_BOOL( buf, v.enableLimit ); + b3RecW_F32( buf, v.lowerAngle ); + b3RecW_F32( buf, v.upperAngle ); + b3RecW_BOOL( buf, v.enableMotor ); + b3RecW_F32( buf, v.maxMotorTorque ); + b3RecW_F32( buf, v.motorSpeed ); +} + +void b3RecW_SPHERICALJOINTDEF( b3RecBuffer* buf, b3SphericalJointDef v ) +{ + b3RecW_JointBase( buf, &v.base ); + b3RecW_BOOL( buf, v.enableSpring ); + b3RecW_F32( buf, v.hertz ); + b3RecW_F32( buf, v.dampingRatio ); + b3RecW_QUAT( buf, v.targetRotation ); + b3RecW_BOOL( buf, v.enableConeLimit ); + b3RecW_F32( buf, v.coneAngle ); + b3RecW_BOOL( buf, v.enableTwistLimit ); + b3RecW_F32( buf, v.lowerTwistAngle ); + b3RecW_F32( buf, v.upperTwistAngle ); + b3RecW_BOOL( buf, v.enableMotor ); + b3RecW_F32( buf, v.maxMotorTorque ); + b3RecW_VEC3( buf, v.motorVelocity ); +} + +void b3RecW_WELDJOINTDEF( b3RecBuffer* buf, b3WeldJointDef v ) +{ + b3RecW_JointBase( buf, &v.base ); + b3RecW_F32( buf, v.linearHertz ); + b3RecW_F32( buf, v.angularHertz ); + b3RecW_F32( buf, v.linearDampingRatio ); + b3RecW_F32( buf, v.angularDampingRatio ); +} + +void b3RecW_WHEELJOINTDEF( b3RecBuffer* buf, b3WheelJointDef v ) +{ + b3RecW_JointBase( buf, &v.base ); + b3RecW_BOOL( buf, v.enableSuspensionSpring ); + b3RecW_F32( buf, v.suspensionHertz ); + b3RecW_F32( buf, v.suspensionDampingRatio ); + b3RecW_BOOL( buf, v.enableSuspensionLimit ); + b3RecW_F32( buf, v.lowerSuspensionLimit ); + b3RecW_F32( buf, v.upperSuspensionLimit ); + b3RecW_BOOL( buf, v.enableSpinMotor ); + b3RecW_F32( buf, v.maxSpinTorque ); + b3RecW_F32( buf, v.spinSpeed ); + b3RecW_BOOL( buf, v.enableSteering ); + b3RecW_F32( buf, v.steeringHertz ); + b3RecW_F32( buf, v.steeringDampingRatio ); + b3RecW_F32( buf, v.targetSteeringAngle ); + b3RecW_F32( buf, v.maxSteeringTorque ); + b3RecW_BOOL( buf, v.enableSteeringLimit ); + b3RecW_F32( buf, v.lowerSteeringLimit ); + b3RecW_F32( buf, v.upperSteeringLimit ); +} + +// Query recording. A query collects a variable number of hits through a user callback, so the count +// is not known until the callback stops firing. The record is built in a local buffer with a +// reserved hit-count slot, then committed whole under the lock so concurrent query threads never +// interleave records in the shared buffer. + +int b3RecReserveU32( b3RecBuffer* buf ) +{ + int offset = buf->size; + uint8_t zero[4] = { 0, 0, 0, 0 }; + b3RecBufAppend( buf, zero, 4 ); + return offset; +} + +void b3RecPatchU32( b3RecBuffer* buf, int offset, uint32_t v ) +{ + B3_ASSERT( offset >= 0 && offset + 4 <= buf->size ); + uint8_t* p = buf->data + offset; + p[0] = (uint8_t)v; + p[1] = (uint8_t)( v >> 8 ); + p[2] = (uint8_t)( v >> 16 ); + p[3] = (uint8_t)( v >> 24 ); +} + +// Frame and append one record into the buffer. Caller holds rec->lock. +static void b3RecCommitRecordLocked( b3Recording* rec, uint8_t opcode, const uint8_t* payload, int payloadSize ) +{ + B3_ASSERT( payloadSize >= 0 && payloadSize < ( 1 << 24 ) ); + b3RecW_U8( &rec->buffer, opcode ); + uint8_t sz[3] = { (uint8_t)payloadSize, (uint8_t)( payloadSize >> 8 ), (uint8_t)( payloadSize >> 16 ) }; + b3RecBufAppend( &rec->buffer, sz, 3 ); + b3RecBufAppend( &rec->buffer, payload, payloadSize ); +} + +void b3RecCommitRecord( b3Recording* rec, uint8_t opcode, const uint8_t* payload, int payloadSize ) +{ + b3LockMutex( rec->lock ); + b3RecCommitRecordLocked( rec, opcode, payload, payloadSize ); + b3UnlockMutex( rec->lock ); +} + +void b3RecQueryBegin( b3RecQueryWriter* w, void* context, uint64_t tagId, const char* tagName ) +{ + w->buf = (b3RecBuffer){ 0 }; + w->userFcn.overlapFcn = NULL; + w->userContext = context; + w->hitCount = 0; + w->countOffset = 0; + w->tagId = tagId; + w->tagName = tagName; +} + +void b3RecQueryCommit( b3Recording* rec, uint8_t opcode, b3RecQueryWriter* w ) +{ + b3LockMutex( rec->lock ); + // A tagged query writes its identity key right before the query record, under one lock so the pair + // stays adjacent even with concurrent queries. The key is the hash of the caller (id, name), which + // are interned once into the trailing tag table so the viewer can show them. + bool tagged = w->tagId != 0 || ( w->tagName != NULL && w->tagName[0] != '\0' ); + if ( tagged ) + { + uint64_t key = b3HashQueryTag( w->tagId, w->tagName ); + b3RecInternTag( rec, key, w->tagId, w->tagName ); + b3RecBuffer tagBuf = { 0 }; + b3RecW_U64( &tagBuf, key ); + b3RecCommitRecordLocked( rec, b3_recOpQueryTag, tagBuf.data, tagBuf.size ); + b3RecBufFree( &tagBuf ); + } + b3RecCommitRecordLocked( rec, opcode, w->buf.data, w->buf.size ); + b3UnlockMutex( rec->lock ); + b3RecBufFree( &w->buf ); +} + +bool b3RecOverlapTrampoline( b3ShapeId id, void* ctx ) +{ + b3RecQueryWriter* w = (b3RecQueryWriter*)ctx; + // The user fcn is NULL for an unfiltered mover cast: accept all, still record the decision so + // replay reproduces the same per-shape accept stream. + bool ret = w->userFcn.overlapFcn != NULL ? w->userFcn.overlapFcn( id, w->userContext ) : true; + b3RecW_SHAPEID( &w->buf, id ); + b3RecW_BOOL( &w->buf, ret ); + w->hitCount++; + return ret; +} + +float b3RecCastTrampoline( b3ShapeId id, b3Pos point, b3Vec3 normal, float fraction, uint64_t userMaterialId, int triangleIndex, + int childIndex, void* ctx ) +{ + b3RecQueryWriter* w = (b3RecQueryWriter*)ctx; + float ret = w->userFcn.castFcn( id, point, normal, fraction, userMaterialId, triangleIndex, childIndex, w->userContext ); + b3RecW_SHAPEID( &w->buf, id ); + b3RecW_POSITION( &w->buf, point ); + b3RecW_VEC3( &w->buf, normal ); + b3RecW_F32( &w->buf, fraction ); + b3RecW_U64( &w->buf, userMaterialId ); + b3RecW_I32( &w->buf, triangleIndex ); + b3RecW_I32( &w->buf, childIndex ); + b3RecW_F32( &w->buf, ret ); + w->hitCount++; + return ret; +} + +// 3D delivers every plane for one shape in a single call. Record the shape, its plane count, each +// plane, then the user return so replay reproduces the same per-shape batch. One hit per shape. +bool b3RecPlaneTrampoline( b3ShapeId id, const b3PlaneResult* planes, int planeCount, void* ctx ) +{ + b3RecQueryWriter* w = (b3RecQueryWriter*)ctx; + bool ret = w->userFcn.planeFcn( id, planes, planeCount, w->userContext ); + b3RecW_SHAPEID( &w->buf, id ); + b3RecW_I32( &w->buf, planeCount ); + for ( int i = 0; i < planeCount; ++i ) + { + b3RecW_PLANERESULT( &w->buf, planes[i] ); + } + b3RecW_BOOL( &w->buf, ret ); + w->hitCount++; + return ret; +} + +// Record framing + +void b3RecBeginRecord( b3Recording* rec, uint8_t opcode ) +{ + b3RecW_U8( &rec->buffer, opcode ); + rec->recordStart = rec->buffer.size; + // Reserve 3 bytes for the u24 payload size, backpatched in b3RecEndRecord. + uint8_t zero[3] = { 0, 0, 0 }; + b3RecBufAppend( &rec->buffer, zero, 3 ); +} + +void b3RecEndRecord( b3Recording* rec ) +{ + int payloadSize = rec->buffer.size - rec->recordStart - 3; + B3_ASSERT( payloadSize >= 0 && payloadSize < ( 1 << 24 ) ); + uint8_t* p = rec->buffer.data + rec->recordStart; + p[0] = (uint8_t)payloadSize; + p[1] = (uint8_t)( payloadSize >> 8 ); + p[2] = (uint8_t)( payloadSize >> 16 ); +} + +// Codegen pass 1b: arg writers +#define ARG( TAG, field ) b3RecW_##TAG( &rec->buffer, a->field ); +#define B3_REC_OP( op, Name, RET, ... ) \ + void b3RecWriteArgs_##Name( b3Recording* rec, const b3RecArgs_##Name* a ) \ + { \ + __VA_ARGS__ \ + } +#include "recording_ops.inl" +#undef B3_REC_OP +#undef ARG + +// Codegen: full writers. Setters may run on threads that each own a distinct object, +// so hold the lock across the whole record. Without it a concurrent writer splices its bytes between +// our begin and end and the record desyncs replay. Same lock the query commit path takes. +#define B3_REC_OP( op, Name, RET, ... ) \ + void b3RecWrite_##Name( b3Recording* rec, const b3RecArgs_##Name* a ) \ + { \ + b3LockMutex( rec->lock ); \ + b3RecBeginRecord( rec, (uint8_t)( op ) ); \ + b3RecWriteArgs_##Name( rec, a ); \ + b3RecEndRecord( rec ); \ + b3UnlockMutex( rec->lock ); \ + } +#include "recording_ops.inl" +#undef B3_REC_OP + +// Codegen: create-op writers that append the returned id inside the record +#define B3_REC_RETWRITE( op, Name, idType, idW ) \ + void b3RecWriteRet_##Name( b3Recording* rec, const b3RecArgs_##Name* a, idType id ) \ + { \ + b3LockMutex( rec->lock ); \ + b3RecBeginRecord( rec, (uint8_t)( op ) ); \ + b3RecWriteArgs_##Name( rec, a ); \ + idW( &rec->buffer, id ); \ + b3RecEndRecord( rec ); \ + b3UnlockMutex( rec->lock ); \ + } +#define B3_REC_RETWRITE_RET_NONE( op, Name ) +#define B3_REC_RETWRITE_RET_BODYID( op, Name ) B3_REC_RETWRITE( op, Name, b3BodyId, b3RecW_BODYID ) +#define B3_REC_RETWRITE_RET_SHAPEID( op, Name ) B3_REC_RETWRITE( op, Name, b3ShapeId, b3RecW_SHAPEID ) +#define B3_REC_RETWRITE_RET_JOINTID( op, Name ) B3_REC_RETWRITE( op, Name, b3JointId, b3RecW_JOINTID ) +#define B3_REC_OP( op, Name, RET, ... ) B3_REC_RETWRITE_##RET( op, Name ) +#include "recording_ops.inl" +#undef B3_REC_OP +#undef B3_REC_RETWRITE_RET_NONE +#undef B3_REC_RETWRITE_RET_BODYID +#undef B3_REC_RETWRITE_RET_SHAPEID +#undef B3_REC_RETWRITE_RET_JOINTID +#undef B3_REC_RETWRITE + +// Geometry registry + +// Full 64-bit content hash, so distinct blobs of the same length get independent bits. A reseeded +// 32-bit djb2 cannot: djb2 is affine in its seed, so a same-length collision survives every seed and +// the high word would just track the low one. Word folded for speed, byte order normalized on +// big-endian to match b3Hash, then a splitmix64 finalizer so tiny inputs still spread across all bits. +// From Fowler/Noll/Vo FNV-1a salted by length, then the splitmix64 mix. +uint64_t b3Hash64Blob( const uint8_t* bytes, int n ) +{ + uint64_t h = 0xcbf29ce484222325ull ^ (uint64_t)(uint32_t)n; + const uint64_t prime = 0x100000001b3ull; + int i = 0; + + while ( i + 8 <= n ) + { + uint64_t word; + memcpy( &word, bytes + 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 + h = ( h ^ word ) * prime; + i += 8; + } + + while ( i < n ) + { + h = ( h ^ (uint64_t)bytes[i] ) * prime; + i += 1; + } + + h ^= h >> 30; + h *= 0xbf58476d1ce4e5b9ull; + h ^= h >> 27; + h *= 0x94d049bb133111ebull; + h ^= h >> 31; + return h; +} + +// Content hash to chain head, so dedup is near O(1). Colliding hashes share a head and are walked +// through b3GeometryEntry::hashNext, so the byteCount + memcmp check always finds an existing blob. +#define NAME b3GeometryHashMap +#define KEY_TY uint64_t +#define VAL_TY uint32_t +#define HASH_FN vt_hash_integer +#define CMPR_FN vt_cmpr_integer +#define MALLOC_FN b3Alloc +#define FREE_FN b3Free +#include "verstable.h" + +// Tag key to tag index, so interning a query tag is O(1) rather than a linear scan over the tag table. +#define NAME b3RecTagMap +#define KEY_TY uint64_t +#define VAL_TY uint32_t +#define HASH_FN vt_hash_integer +#define CMPR_FN vt_cmpr_integer +#define MALLOC_FN b3Alloc +#define FREE_FN b3Free +#include "verstable.h" + +// Append a fresh entry and splice it onto the front of its hash chain. The map value is the chain head. +static uint32_t b3RegistryPush( b3GeometryRegistry* reg, b3GeometryHashMap* map, b3GeometryHashMap_itr itr, bool hashPresent, + b3GeometryKind kind, uint64_t contentHash, uint8_t* bytes, int byteCount ) +{ + if ( reg->count >= reg->capacity ) + { + int newCap = reg->capacity < 8 ? 8 : reg->capacity * 2; + reg->entries = (b3GeometryEntry*)b3GrowAlloc( reg->entries, reg->capacity * (int)sizeof( b3GeometryEntry ), + newCap * (int)sizeof( b3GeometryEntry ) ); + reg->capacity = newCap; + } + + uint32_t id = (uint32_t)reg->count; + b3GeometryEntry* entry = reg->entries + reg->count; + entry->contentHash = contentHash; + entry->id = id; + entry->kind = kind; + entry->byteCount = byteCount; + entry->bytes = bytes; // take ownership + entry->hashNext = hashPresent ? (int)itr.data->val : B3_NULL_INDEX; + reg->count++; + + if ( hashPresent ) + { + itr.data->val = id; + } + else + { + b3GeometryHashMap_insert( map, contentHash, id ); + } + return id; +} + +static b3GeometryHashMap* b3RegistryMap( b3GeometryRegistry* reg ) +{ + if ( reg->dedupMap == NULL ) + { + b3GeometryHashMap* fresh = (b3GeometryHashMap*)b3Alloc( sizeof( b3GeometryHashMap ) ); + b3GeometryHashMap_init( fresh ); + reg->dedupMap = fresh; + } + return (b3GeometryHashMap*)reg->dedupMap; +} + +uint32_t b3InternGeometry( b3GeometryRegistry* reg, b3GeometryKind kind, uint64_t contentHash, uint8_t* bytes, int byteCount ) +{ + b3GeometryHashMap* map = b3RegistryMap( reg ); + + b3GeometryHashMap_itr itr = b3GeometryHashMap_get( map, contentHash ); + bool hashPresent = b3GeometryHashMap_is_end( itr ) == false; + if ( hashPresent ) + { + // Walk every entry sharing this hash so a collision still finds the identical blob. + for ( int idx = (int)itr.data->val; idx != B3_NULL_INDEX; idx = reg->entries[idx].hashNext ) + { + b3GeometryEntry* e = reg->entries + idx; + if ( e->byteCount == byteCount && memcmp( e->bytes, bytes, (size_t)byteCount ) == 0 ) + { + // Duplicate: the caller transferred ownership; return existing id + b3Free( bytes, (size_t)byteCount ); + return e->id; + } + } + } + + return b3RegistryPush( reg, map, itr, hashPresent, kind, contentHash, bytes, byteCount ); +} + +uint32_t b3AppendGeometry( b3GeometryRegistry* reg, b3GeometryKind kind, uint64_t contentHash, uint8_t* bytes, int byteCount ) +{ + b3GeometryHashMap* map = b3RegistryMap( reg ); + b3GeometryHashMap_itr itr = b3GeometryHashMap_get( map, contentHash ); + bool hashPresent = b3GeometryHashMap_is_end( itr ) == false; + return b3RegistryPush( reg, map, itr, hashPresent, kind, contentHash, bytes, byteCount ); +} + +void b3FreeRegistry( b3GeometryRegistry* reg ) +{ + for ( int i = 0; i < reg->count; ++i ) + { + b3Free( reg->entries[i].bytes, (size_t)reg->entries[i].byteCount ); + } + if ( reg->entries != NULL ) + { + b3Free( reg->entries, (size_t)( reg->capacity * (int)sizeof( b3GeometryEntry ) ) ); + } + if ( reg->dedupMap != NULL ) + { + b3GeometryHashMap_cleanup( (b3GeometryHashMap*)reg->dedupMap ); + b3Free( reg->dedupMap, sizeof( b3GeometryHashMap ) ); + } + reg->entries = NULL; + reg->count = 0; + reg->capacity = 0; + reg->dedupMap = NULL; +} + +uint64_t b3HashQueryTag( uint64_t id, const char* name ) +{ + uint64_t h = B3_SNAP_FNV_INIT; + for ( int i = 0; i < 8; ++i ) + { + h = ( h ^ ( ( id >> ( 8 * i ) ) & 0xFFu ) ) * B3_SNAP_FNV_PRIME; + } + if ( name != NULL ) + { + for ( int i = 0; name[i] != '\0'; ++i ) + { + h = ( h ^ (uint8_t)name[i] ) * B3_SNAP_FNV_PRIME; + } + } + // Never 0 so the key doubles as the tagged flag. + return h != 0 ? h : 1; +} + +static b3RecTagMap* b3RecTags( b3Recording* rec ) +{ + if ( rec->tagMap == NULL ) + { + b3RecTagMap* fresh = b3Alloc( sizeof( b3RecTagMap ) ); + b3RecTagMap_init( fresh ); + rec->tagMap = fresh; + } + return rec->tagMap; +} + +void b3RecInternTag( b3Recording* rec, uint64_t key, uint64_t id, const char* name ) +{ + b3RecTagMap* map = b3RecTags( rec ); + if ( b3RecTagMap_is_end( b3RecTagMap_get( map, key ) ) == false ) + { + return; // first id/name for a key wins + } + + if ( rec->tagCount == rec->tagCapacity ) + { + int newCap = rec->tagCapacity == 0 ? 8 : 2 * rec->tagCapacity; + rec->tags = b3GrowAlloc( rec->tags, rec->tagCapacity * (int)sizeof( b3RecTag ), newCap * (int)sizeof( b3RecTag ) ); + rec->tagCapacity = newCap; + } + + uint32_t index = (uint32_t)rec->tagCount; + b3RecTag* tag = &rec->tags[rec->tagCount++]; + tag->key = key; + tag->id = id; + int n = 0; + while ( name != NULL && name[n] != '\0' && n < B3_MAX_QUERY_NAME_LENGTH ) + { + tag->queryName[n] = name[n]; + n++; + } + tag->queryName[n] = '\0'; + b3RecTagMap_insert( map, key, index ); +} + +// Write the trailing registry block: u32 entryCount then per-entry { u8 kind, u32 byteCount, bytes }, +// followed by the query-tag table { u32 tagCount, per-tag uu64 id, STR name }. A reader built before +// the tag table stops after the geometry entries and ignores the trailing tag bytes. +void b3RecWriteRegistry( b3Recording* rec ) +{ + b3RecW_U32( &rec->buffer, (uint32_t)rec->registry.count ); + for ( int i = 0; i < rec->registry.count; ++i ) + { + b3GeometryEntry* e = rec->registry.entries + i; + b3RecW_U8( &rec->buffer, (uint8_t)e->kind ); + b3RecW_U32( &rec->buffer, (uint32_t)e->byteCount ); + b3RecBufAppend( &rec->buffer, e->bytes, e->byteCount ); + } + + b3RecW_U32( &rec->buffer, (uint32_t)rec->tagCount ); + for ( int i = 0; i < rec->tagCount; ++i ) + { + b3RecW_U64( &rec->buffer, rec->tags[i].key ); + b3RecW_U64( &rec->buffer, rec->tags[i].id ); + b3RecW_STR( &rec->buffer, rec->tags[i].queryName ); + } +} + +// Lifecycle + +b3Recording* b3CreateRecording( int byteCapacity ) +{ + b3Recording* rec = (b3Recording*)b3Alloc( sizeof( b3Recording ) ); + *rec = (b3Recording){ 0 }; + + int initCap = byteCapacity > 0 ? byteCapacity : 65536; + rec->buffer.data = (uint8_t*)b3Alloc( (size_t)initCap ); + rec->buffer.capacity = initCap; + rec->buffer.size = 0; + rec->lock = b3CreateMutex(); + return rec; +} + +void b3DestroyRecording( b3Recording* recording ) +{ + if ( recording == NULL ) + { + return; + } + + b3RecBufFree( &recording->buffer ); + b3FreeRegistry( &recording->registry ); + if ( recording->tags != NULL ) + { + b3Free( recording->tags, (size_t)recording->tagCapacity * sizeof( b3RecTag ) ); + } + if ( recording->tagMap != NULL ) + { + b3RecTagMap_cleanup( (b3RecTagMap*)recording->tagMap ); + b3Free( recording->tagMap, sizeof( b3RecTagMap ) ); + } + b3DestroyMutex( recording->lock ); + b3Free( recording, sizeof( b3Recording ) ); +} + +const uint8_t* b3Recording_GetData( const b3Recording* recording ) +{ + return recording->buffer.data; +} + +int b3Recording_GetSize( const b3Recording* recording ) +{ + return recording->buffer.size; +} + +void b3RecAccumulateBounds( b3Recording* rec, b3AABB bounds ) +{ + rec->accumulatedBounds = rec->haveBounds ? b3AABB_Union( rec->accumulatedBounds, bounds ) : bounds; + rec->haveBounds = true; +} + +void b3StartRecordingIntoBuffer( b3World* world, b3Recording* recording ) +{ + // Reset so a recording handle can be reused for a fresh session + recording->buffer.size = 0; + recording->recordStart = 0; + recording->haveBounds = false; + b3FreeRegistry( &recording->registry ); + if ( recording->tags != NULL ) + { + b3Free( recording->tags, (size_t)recording->tagCapacity * sizeof( b3RecTag ) ); + recording->tags = NULL; + } + if ( recording->tagMap != NULL ) + { + b3RecTagMap_cleanup( (b3RecTagMap*)recording->tagMap ); + b3Free( recording->tagMap, sizeof( b3RecTagMap ) ); + recording->tagMap = NULL; + } + recording->tagCount = 0; + recording->tagCapacity = 0; + + b3RecHeader hdr = { 0 }; + hdr.magic = B3_REC_MAGIC; + hdr.versionMajor = B3_REC_VERSION_MAJOR; + hdr.versionMinor = B3_REC_VERSION_MINOR; + hdr.pointerWidth = (uint8_t)sizeof( void* ); + hdr.bigEndian = 0; + hdr.validationEnabled = B3_ENABLE_VALIDATION ? 1u : 0u; + hdr.lengthScale = b3GetLengthUnitsPerMeter(); + hdr.registryOffset = 0; // backpatched in b3StopRecordingInternal + hdr.registryByteCount = 0; + + world->recording = recording; + + // Every recording is snapshot-seeded. The seed blob follows the header so replay restores in + // place and the world id stays stable across a restart or backward scrub. An empty world still + // serializes a valid blob, so there is no from-creation special case. + b3RecBuffer snapBuf = { 0 }; + b3SerializeWorld( world, &snapBuf, recording ); + hdr.snapshotSize = (uint64_t)snapBuf.size; + + b3RecBufAppend( &recording->buffer, &hdr, (int)sizeof( hdr ) ); + b3RecBufAppend( &recording->buffer, snapBuf.data, snapBuf.size ); + b3RecBufFree( &snapBuf ); + + // Anchor the recording with the current world state hash so replay can assert + // determinism from the very first step. + b3WorldId worldId = { (uint16_t)( world->worldId + 1 ), world->generation }; + b3RecArgs_StateHash stateHash = { worldId, b3HashWorldState( world ) }; + b3RecWrite_StateHash( recording, &stateHash ); +} + +void b3StopRecordingInternal( b3World* world ) +{ + if ( world->recording == NULL ) + { + return; + } + + b3Recording* rec = world->recording; + world->recording = NULL; + + // Write accumulated bounds so a viewer can frame the whole recorded motion + b3RecArgs_RecordingBounds rb = { 0 }; + if ( rec->haveBounds ) + { + rb.bounds = rec->accumulatedBounds; + } + b3RecWrite_RecordingBounds( rec, &rb ); + + // End-of-stream marker; the buffer is now self-contained + b3WorldId wid = { (uint16_t)( world->worldId + 1 ), world->generation }; + b3RecArgs_DestroyWorld a = { wid }; + b3RecWrite_DestroyWorld( rec, &a ); + + // Write the trailing registry block + int registryOffset = rec->buffer.size; + b3RecWriteRegistry( rec ); + int registryByteCount = rec->buffer.size - registryOffset; + + // Backpatch registryOffset and registryByteCount into the header + uint8_t* hdrBytes = rec->buffer.data; + uint64_t regOff = (uint64_t)registryOffset; + uint64_t regSz = (uint64_t)registryByteCount; + // Little-endian backpatch in place; offsetof keeps this correct if the header layout shifts + uint8_t* pOff = hdrBytes + offsetof( b3RecHeader, registryOffset ); + uint8_t* pSz = hdrBytes + offsetof( b3RecHeader, registryByteCount ); + for ( int i = 0; i < 8; ++i ) + { + pOff[i] = (uint8_t)( regOff >> ( 8 * i ) ); + pSz[i] = (uint8_t)( regSz >> ( 8 * i ) ); + } +} + +// Convenience file I/O + +bool b3SaveRecordingToFile( const b3Recording* recording, const char* path ) +{ + if ( recording == NULL || path == NULL ) + { + return false; + } + + FILE* f = fopen( path, "wb" ); + if ( f == NULL ) + { + return false; + } + + size_t written = fwrite( recording->buffer.data, 1, (size_t)recording->buffer.size, f ); + fclose( f ); + return (int)written == recording->buffer.size; +} + +b3Recording* b3LoadRecordingFromFile( const char* path ) +{ + if ( path == NULL ) + { + return NULL; + } + + FILE* f = fopen( path, "rb" ); + if ( f == NULL ) + { + return NULL; + } + + if ( fseek( f, 0, SEEK_END ) != 0 ) + { + fclose( f ); + return NULL; + } + + long fileSize = ftell( f ); + // Anything smaller than the fixed header can't be a recording + if ( fileSize < (long)sizeof( b3RecHeader ) || fileSize > INT_MAX ) + { + fclose( f ); + return NULL; + } + fseek( f, 0, SEEK_SET ); + + b3Recording* rec = b3CreateRecording( (int)fileSize ); + size_t readSize = fread( rec->buffer.data, 1, (size_t)fileSize, f ); + fclose( f ); + + if ( (long)readSize != fileSize ) + { + b3DestroyRecording( rec ); + return NULL; + } + + // Validate magic so a wrong file fails at load rather than deep in the player + b3RecHeader hdr; + memcpy( &hdr, rec->buffer.data, sizeof( hdr ) ); + if ( hdr.magic != B3_REC_MAGIC ) + { + b3DestroyRecording( rec ); + return NULL; + } + + rec->buffer.size = (int)fileSize; + return rec; +} + +// Geometry interning helpers + +uint32_t b3RecInternHull( b3Recording* rec, const b3HullData* hull ) +{ + int byteCount = hull->byteCount; + uint8_t* bytes = (uint8_t*)b3Alloc( (size_t)byteCount ); + memcpy( bytes, hull, (size_t)byteCount ); + uint64_t h = b3Hash64Blob( bytes, byteCount ); + return b3InternGeometry( &rec->registry, b3_geometryHull, h, bytes, byteCount ); +} + +uint32_t b3RecInternMesh( b3Recording* rec, const b3MeshData* mesh ) +{ + int byteCount = mesh->byteCount; + uint8_t* bytes = (uint8_t*)b3Alloc( (size_t)byteCount ); + memcpy( bytes, mesh, (size_t)byteCount ); + uint64_t h = b3Hash64Blob( bytes, byteCount ); + return b3InternGeometry( &rec->registry, b3_geometryMesh, h, bytes, byteCount ); +} + +uint32_t b3RecInternHeightField( b3Recording* rec, const b3HeightFieldData* hf ) +{ + int byteCount = hf->byteCount; + uint8_t* bytes = (uint8_t*)b3Alloc( (size_t)byteCount ); + memcpy( bytes, hf, (size_t)byteCount ); + uint64_t h = b3Hash64Blob( bytes, byteCount ); + return b3InternGeometry( &rec->registry, b3_geometryHeightField, h, bytes, byteCount ); +} + +uint32_t b3RecInternCompound( b3Recording* rec, const b3CompoundData* compound ) +{ + int byteCount = compound->byteCount; + uint8_t* bytes = (uint8_t*)b3Alloc( (size_t)byteCount ); + memcpy( bytes, compound, (size_t)byteCount ); + // Null the tree node pointer in the copy so the canonical bytes are pointer-free. + // b3ConvertBytesToCompound fixes it back on load via nodeOffset. + ( (b3CompoundData*)bytes )->tree.nodes = NULL; + uint64_t h = b3Hash64Blob( bytes, byteCount ); + return b3InternGeometry( &rec->registry, b3_geometryCompound, h, bytes, byteCount ); +} + +uint64_t b3HashWorldState( b3World* world ) +{ + uint64_t hash = B3_SNAP_FNV_INIT; + const uint64_t prime = B3_SNAP_FNV_PRIME; + + int bodyCount = world->bodies.count; + for ( int i = 0; i < bodyCount; ++i ) + { + b3Body* body = world->bodies.data + i; + if ( body->id != i ) + { + // Free or never-used slot + continue; + } + + b3BodySim* sim = b3GetBodySim( world, body ); + + uint32_t bits; + +#define B3_HASH_FLOAT( f ) \ + memcpy( &bits, &( f ), 4 ); \ + hash = ( hash ^ (uint64_t)bits ) * prime; + + hash = b3FnvMixPosition( hash, sim->transform.p ); + B3_HASH_FLOAT( sim->transform.q.v.x ) + B3_HASH_FLOAT( sim->transform.q.v.y ) + B3_HASH_FLOAT( sim->transform.q.v.z ) + B3_HASH_FLOAT( sim->transform.q.s ) + + b3BodyState* state = b3GetBodyState( world, body ); + if ( state != NULL ) + { + B3_HASH_FLOAT( state->linearVelocity.x ) + B3_HASH_FLOAT( state->linearVelocity.y ) + B3_HASH_FLOAT( state->linearVelocity.z ) + B3_HASH_FLOAT( state->angularVelocity.x ) + B3_HASH_FLOAT( state->angularVelocity.y ) + B3_HASH_FLOAT( state->angularVelocity.z ) + } + +#undef B3_HASH_FLOAT + } + + return hash; +} diff --git a/vendor/box3d/src/src/recording.h b/vendor/box3d/src/src/recording.h new file mode 100644 index 000000000..3ba2fd3bb --- /dev/null +++ b/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 +#include +#include +#include + +// 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 ); diff --git a/vendor/box3d/src/src/recording_ops.inl b/vendor/box3d/src/src/recording_ops.inl new file mode 100644 index 000000000..b8a3625da --- /dev/null +++ b/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 ) ) diff --git a/vendor/box3d/src/src/recording_replay.c b/vendor/box3d/src/src/recording_replay.c new file mode 100644 index 000000000..584912d59 --- /dev/null +++ b/vendor/box3d/src/src/recording_replay.c @@ -0,0 +1,3634 @@ +// SPDX-FileCopyrightText: 2026 Erin Catto +// SPDX-License-Identifier: MIT + +#if defined( _MSC_VER ) && !defined( _CRT_SECURE_NO_WARNINGS ) +#define _CRT_SECURE_NO_WARNINGS +#endif + +#include "recording_replay.h" + +#include "body.h" +#include "compound.h" +#include "physics_world.h" +#include "world_snapshot.h" + +#include "box3d/box3d.h" + +#include +#include +#include +#include + +// Read primitives + +static void b3RecRdrCheck( b3RecReader* rdr, int size ) +{ + if ( size < 0 || (int64_t)rdr->cursor + (int64_t)size > (int64_t)rdr->size ) + { + rdr->ok = false; + } +} + +static void b3RecRdrBlob( b3RecReader* rdr, void* out, int size ) +{ + b3RecRdrCheck( rdr, size ); + if ( !rdr->ok ) + { + memset( out, 0, (size_t)size ); + return; + } + memcpy( out, rdr->data + rdr->cursor, (size_t)size ); + rdr->cursor += size; +} + +uint8_t b3RecR_U8( b3RecReader* rdr ) +{ + b3RecRdrCheck( rdr, 1 ); + if ( !rdr->ok ) + { + return 0; + } + return rdr->data[rdr->cursor++]; +} + +uint16_t b3RecR_U16( b3RecReader* rdr ) +{ + b3RecRdrCheck( rdr, 2 ); + if ( !rdr->ok ) + { + return 0; + } + uint16_t v = (uint16_t)rdr->data[rdr->cursor] | ( (uint16_t)rdr->data[rdr->cursor + 1] << 8 ); + rdr->cursor += 2; + return v; +} + +uint32_t b3RecR_U24( b3RecReader* rdr ) +{ + b3RecRdrCheck( rdr, 3 ); + if ( !rdr->ok ) + { + return 0; + } + uint32_t v = (uint32_t)rdr->data[rdr->cursor] | ( (uint32_t)rdr->data[rdr->cursor + 1] << 8 ) | + ( (uint32_t)rdr->data[rdr->cursor + 2] << 16 ); + rdr->cursor += 3; + return v; +} + +uint32_t b3RecR_U32( b3RecReader* rdr ) +{ + b3RecRdrCheck( rdr, 4 ); + if ( !rdr->ok ) + { + return 0; + } + uint32_t v = (uint32_t)rdr->data[rdr->cursor] | ( (uint32_t)rdr->data[rdr->cursor + 1] << 8 ) | + ( (uint32_t)rdr->data[rdr->cursor + 2] << 16 ) | ( (uint32_t)rdr->data[rdr->cursor + 3] << 24 ); + rdr->cursor += 4; + return v; +} + +uint64_t b3RecR_U64( b3RecReader* rdr ) +{ + b3RecRdrCheck( rdr, 8 ); + if ( !rdr->ok ) + { + return 0; + } + uint64_t v = (uint64_t)rdr->data[rdr->cursor] | ( (uint64_t)rdr->data[rdr->cursor + 1] << 8 ) | + ( (uint64_t)rdr->data[rdr->cursor + 2] << 16 ) | ( (uint64_t)rdr->data[rdr->cursor + 3] << 24 ) | + ( (uint64_t)rdr->data[rdr->cursor + 4] << 32 ) | ( (uint64_t)rdr->data[rdr->cursor + 5] << 40 ) | + ( (uint64_t)rdr->data[rdr->cursor + 6] << 48 ) | ( (uint64_t)rdr->data[rdr->cursor + 7] << 56 ); + rdr->cursor += 8; + return v; +} + +int32_t b3RecR_I32( b3RecReader* rdr ) +{ + return (int32_t)b3RecR_U32( rdr ); +} + +float b3RecR_F32( b3RecReader* rdr ) +{ + uint32_t bits = b3RecR_U32( rdr ); + float v; + memcpy( &v, &bits, 4 ); + return v; +} + +double b3RecR_F64( b3RecReader* rdr ) +{ + uint64_t bits = b3RecR_U64( rdr ); + double v; + memcpy( &v, &bits, 8 ); + return v; +} + +bool b3RecR_BOOL( b3RecReader* rdr ) +{ + return b3RecR_U8( rdr ) != 0u; +} + +b3Vec3 b3RecR_VEC3( b3RecReader* rdr ) +{ + b3Vec3 v; + v.x = b3RecR_F32( rdr ); + v.y = b3RecR_F32( rdr ); + v.z = b3RecR_F32( rdr ); + return v; +} + +b3Quat b3RecR_QUAT( b3RecReader* rdr ) +{ + b3Quat q; + q.v.x = b3RecR_F32( rdr ); + q.v.y = b3RecR_F32( rdr ); + q.v.z = b3RecR_F32( rdr ); + q.s = b3RecR_F32( rdr ); + return q; +} + +b3Transform b3RecR_TRANSFORM( b3RecReader* rdr ) +{ + b3Transform t; + t.p = b3RecR_VEC3( rdr ); + t.q = b3RecR_QUAT( rdr ); + return t; +} + +b3Pos b3RecR_POSITION( b3RecReader* rdr ) +{ + b3Pos p; +#if defined( BOX3D_DOUBLE_PRECISION ) + p.x = b3RecR_F64( rdr ); + p.y = b3RecR_F64( rdr ); + p.z = b3RecR_F64( rdr ); +#else + p.x = b3RecR_F32( rdr ); + p.y = b3RecR_F32( rdr ); + p.z = b3RecR_F32( rdr ); +#endif + return p; +} + +b3WorldTransform b3RecR_WORLDXF( b3RecReader* rdr ) +{ + b3WorldTransform t; + t.p = b3RecR_POSITION( rdr ); + t.q = b3RecR_QUAT( rdr ); + return t; +} + +b3Matrix3 b3RecR_MATRIX3( b3RecReader* rdr ) +{ + b3Matrix3 m; + m.cx = b3RecR_VEC3( rdr ); + m.cy = b3RecR_VEC3( rdr ); + m.cz = b3RecR_VEC3( rdr ); + return m; +} + +b3AABB b3RecR_AABB( b3RecReader* rdr ) +{ + b3AABB v; + v.lowerBound = b3RecR_VEC3( rdr ); + v.upperBound = b3RecR_VEC3( rdr ); + return v; +} + +b3QueryFilter b3RecR_QUERYFILTER( b3RecReader* rdr ) +{ + // id and name are not on the wire here, they ride the separate QueryTag op. Start from the default + // so they keep the untagged sentinel instead of garbage. + b3QueryFilter f = b3DefaultQueryFilter(); + f.categoryBits = b3RecR_U64( rdr ); + f.maskBits = b3RecR_U64( rdr ); + return f; +} + +// Reserve reader scratch for a count taken from an untrusted file. Every recorded element +// consumes at least one byte, so a valid count can never exceed the bytes left in the file. +// Reject anything larger (or negative, or that would overflow the byte size) by failing the read +// rather than allocating wildly. A grow keeps the old contents so callers can accumulate across +// reserves, as the collide-mover dispatcher does one shape group at a time. +static bool b3RecReserveScratch( b3RecReader* rdr, void** data, int* cap, int need, int elemSize ) +{ + int remaining = rdr->size - rdr->cursor; + if ( need < 0 || remaining < 0 || need > remaining || need > INT_MAX / elemSize ) + { + rdr->ok = false; + return false; + } + if ( need <= *cap ) + { + return true; + } + int newCap = need <= INT_MAX / elemSize - 8 ? need + 8 : need; + void* grown = b3Alloc( (size_t)newCap * (size_t)elemSize ); + if ( *data != NULL ) + { + memcpy( grown, *data, (size_t)*cap * (size_t)elemSize ); + b3Free( *data, (size_t)*cap * (size_t)elemSize ); + } + *data = grown; + *cap = newCap; + return true; +} + +// Variable length, mirrors b3RecW_SHAPEPROXY: count, count points, radius. The decoded proxy borrows +// the reader's scratch for its point cloud, valid until the next proxy read. +b3ShapeProxy b3RecR_SHAPEPROXY( b3RecReader* rdr ) +{ + b3ShapeProxy p = { 0 }; + int count = b3RecR_I32( rdr ); + if ( count < 0 ) + count = 0; + if ( count > B3_MAX_SHAPE_CAST_POINTS ) + count = B3_MAX_SHAPE_CAST_POINTS; + if ( count > 0 && + b3RecReserveScratch( rdr, (void**)&rdr->proxyScratch, &rdr->proxyScratchCap, count, (int)sizeof( b3Vec3 ) ) ) + { + for ( int i = 0; i < count; ++i ) + { + rdr->proxyScratch[i] = b3RecR_VEC3( rdr ); + } + p.points = rdr->proxyScratch; + p.count = count; + } + p.radius = b3RecR_F32( rdr ); + return p; +} + +b3TreeStats b3RecR_TREESTATS( b3RecReader* rdr ) +{ + b3TreeStats v; + v.nodeVisits = b3RecR_I32( rdr ); + v.leafVisits = b3RecR_I32( rdr ); + return v; +} + +b3RayResult b3RecR_RAYRESULT( b3RecReader* rdr ) +{ + b3RayResult v = { 0 }; + // shapeId keeps the recorded world0; b3RecMakeShapeId is applied at compare time + v.shapeId = b3RecR_SHAPEID( rdr ); + v.point = b3RecR_POSITION( rdr ); + v.normal = b3RecR_VEC3( rdr ); + v.userMaterialId = b3RecR_U64( rdr ); + v.fraction = b3RecR_F32( rdr ); + v.triangleIndex = b3RecR_I32( rdr ); + v.childIndex = b3RecR_I32( rdr ); + v.hit = b3RecR_BOOL( rdr ); + return v; +} + +b3PlaneResult b3RecR_PLANERESULT( b3RecReader* rdr ) +{ + b3PlaneResult v; + v.plane.normal = b3RecR_VEC3( rdr ); + v.plane.offset = b3RecR_F32( rdr ); + v.point = b3RecR_VEC3( rdr ); + return v; +} + +b3WorldId b3RecR_WORLDID( b3RecReader* rdr ) +{ + return b3LoadWorldId( b3RecR_U32( rdr ) ); +} + +b3BodyId b3RecR_BODYID( b3RecReader* rdr ) +{ + return b3LoadBodyId( b3RecR_U64( rdr ) ); +} + +b3ShapeId b3RecR_SHAPEID( b3RecReader* rdr ) +{ + return b3LoadShapeId( b3RecR_U64( rdr ) ); +} + +b3JointId b3RecR_JOINTID( b3RecReader* rdr ) +{ + return b3LoadJointId( b3RecR_U64( rdr ) ); +} + +b3Sphere b3RecR_SPHERE( b3RecReader* rdr ) +{ + b3Sphere s; + b3RecRdrBlob( rdr, &s, (int)sizeof( s ) ); + return s; +} + +b3Capsule b3RecR_CAPSULE( b3RecReader* rdr ) +{ + b3Capsule c; + b3RecRdrBlob( rdr, &c, (int)sizeof( c ) ); + return c; +} + +uint32_t b3RecR_GEOMID( b3RecReader* rdr ) +{ + return b3RecR_U32( rdr ); +} + +b3Filter b3RecR_FILTER( b3RecReader* rdr ) +{ + b3Filter f; + f.categoryBits = b3RecR_U64( rdr ); + f.maskBits = b3RecR_U64( rdr ); + f.groupIndex = b3RecR_I32( rdr ); + return f; +} + +b3SurfaceMaterial b3RecR_MATERIAL( b3RecReader* rdr ) +{ + b3SurfaceMaterial m = b3DefaultSurfaceMaterial(); + m.friction = b3RecR_F32( rdr ); + m.restitution = b3RecR_F32( rdr ); + m.rollingResistance = b3RecR_F32( rdr ); + m.tangentVelocity = b3RecR_VEC3( rdr ); + m.userMaterialId = b3RecR_U64( rdr ); + m.customColor = b3RecR_U32( rdr ); + return m; +} + +b3MassData b3RecR_MASSDATA( b3RecReader* rdr ) +{ + b3MassData md; + md.mass = b3RecR_F32( rdr ); + md.center = b3RecR_VEC3( rdr ); + md.inertia = b3RecR_MATRIX3( rdr ); + return md; +} + +b3MotionLocks b3RecR_LOCKS( b3RecReader* rdr ) +{ + b3MotionLocks locks; + locks.linearX = b3RecR_BOOL( rdr ); + locks.linearY = b3RecR_BOOL( rdr ); + locks.linearZ = b3RecR_BOOL( rdr ); + locks.angularX = b3RecR_BOOL( rdr ); + locks.angularY = b3RecR_BOOL( rdr ); + locks.angularZ = b3RecR_BOOL( rdr ); + return locks; +} + +// Rotating set of static string buffers, valid until the next 4 STR reads. +const char* b3RecR_STR( b3RecReader* rdr ) +{ + char* buf = rdr->stringBuffers[rdr->nextString]; + rdr->nextString = ( rdr->nextString + 1 ) & 3; + + uint16_t len = b3RecR_U16( rdr ); + if ( len == 0xFFFFu ) + { + return NULL; + } + + int n = (int)len; + if ( n > B3_MAX_NAME_LENGTH ) + { + n = B3_MAX_NAME_LENGTH; + } + b3RecRdrCheck( rdr, (int)len ); + if ( rdr->ok && n > 0 ) + { + memcpy( buf, rdr->data + rdr->cursor, (size_t)n ); + } + rdr->cursor += (int)len; + buf[n] = '\0'; + return buf; +} + +// Def readers: start from b3Default*Def() then overlay each serialized field +// in the exact order the writer produced them. + +b3ExplosionDef b3RecR_EXPLOSIONDEF( b3RecReader* rdr ) +{ + b3ExplosionDef def = b3DefaultExplosionDef(); + def.maskBits = b3RecR_U64( rdr ); + def.position = b3RecR_POSITION( rdr ); + def.radius = b3RecR_F32( rdr ); + def.falloff = b3RecR_F32( rdr ); + def.impulsePerArea = b3RecR_F32( rdr ); + return def; +} + +b3BodyDef b3RecR_BODYDEF( b3RecReader* rdr ) +{ + b3BodyDef def = b3DefaultBodyDef(); + def.type = (b3BodyType)b3RecR_I32( rdr ); + def.position = b3RecR_POSITION( rdr ); + def.rotation = b3RecR_QUAT( rdr ); + def.linearVelocity = b3RecR_VEC3( rdr ); + def.angularVelocity = b3RecR_VEC3( rdr ); + def.linearDamping = b3RecR_F32( rdr ); + def.angularDamping = b3RecR_F32( rdr ); + def.gravityScale = b3RecR_F32( rdr ); + def.sleepThreshold = b3RecR_F32( rdr ); + def.name = b3RecR_STR( rdr ); + (void)b3RecR_U64( rdr ); // userData placeholder + def.motionLocks = b3RecR_LOCKS( rdr ); + def.enableSleep = b3RecR_BOOL( rdr ); + def.isAwake = b3RecR_BOOL( rdr ); + def.isBullet = b3RecR_BOOL( rdr ); + def.isEnabled = b3RecR_BOOL( rdr ); + def.allowFastRotation = b3RecR_BOOL( rdr ); + def.enableContactRecycling = b3RecR_BOOL( rdr ); + def.userData = NULL; + return def; +} + +b3ShapeDef b3RecR_SHAPEDEF( b3RecReader* rdr ) +{ + b3ShapeDef def = b3DefaultShapeDef(); + + def.name = b3RecR_STR( rdr ); + (void)b3RecR_U64( rdr ); // userData placeholder + + int matCount = b3RecR_I32( rdr ); + if ( matCount < 0 ) + { + matCount = 0; + } + if ( matCount > 0 && + b3RecReserveScratch( rdr, (void**)&rdr->matScratch, &rdr->matScratchCap, matCount, (int)sizeof( b3SurfaceMaterial ) ) ) + { + for ( int i = 0; i < matCount; ++i ) + { + rdr->matScratch[i] = b3RecR_MATERIAL( rdr ); + } + def.materials = rdr->matScratch; + def.materialCount = matCount; + } + else + { + for ( int i = 0; i < matCount; ++i ) + { + (void)b3RecR_MATERIAL( rdr ); + } + def.materials = NULL; + def.materialCount = 0; + } + + def.baseMaterial = b3RecR_MATERIAL( rdr ); + def.density = b3RecR_F32( rdr ); + def.explosionScale = b3RecR_F32( rdr ); + def.filter = b3RecR_FILTER( rdr ); + def.enableCustomFiltering = b3RecR_BOOL( rdr ); + def.isSensor = b3RecR_BOOL( rdr ); + def.enableSensorEvents = b3RecR_BOOL( rdr ); + def.enableContactEvents = b3RecR_BOOL( rdr ); + def.enableHitEvents = b3RecR_BOOL( rdr ); + def.enablePreSolveEvents = b3RecR_BOOL( rdr ); + def.invokeContactCreation = b3RecR_BOOL( rdr ); + def.updateBodyMass = b3RecR_BOOL( rdr ); + def.enableSpeculativeContact = b3RecR_BOOL( rdr ); + def.userData = NULL; + return def; +} + +// Shared base for all joint defs. Body ids come in with recorded world0; callers remap them. +static void b3RecR_JointBase( b3RecReader* rdr, b3JointDef* base ) +{ + (void)b3RecR_U64( rdr ); // userData + base->bodyIdA = b3RecR_BODYID( rdr ); + base->bodyIdB = b3RecR_BODYID( rdr ); + base->localFrameA = b3RecR_TRANSFORM( rdr ); + base->localFrameB = b3RecR_TRANSFORM( rdr ); + base->forceThreshold = b3RecR_F32( rdr ); + base->torqueThreshold = b3RecR_F32( rdr ); + base->constraintHertz = b3RecR_F32( rdr ); + base->constraintDampingRatio = b3RecR_F32( rdr ); + base->drawScale = b3RecR_F32( rdr ); + base->collideConnected = b3RecR_BOOL( rdr ); + base->userData = NULL; +} + +b3ParallelJointDef b3RecR_PARALLELJOINTDEF( b3RecReader* rdr ) +{ + b3ParallelJointDef def = b3DefaultParallelJointDef(); + b3RecR_JointBase( rdr, &def.base ); + def.hertz = b3RecR_F32( rdr ); + def.dampingRatio = b3RecR_F32( rdr ); + def.maxTorque = b3RecR_F32( rdr ); + return def; +} + +b3DistanceJointDef b3RecR_DISTANCEJOINTDEF( b3RecReader* rdr ) +{ + b3DistanceJointDef def = b3DefaultDistanceJointDef(); + b3RecR_JointBase( rdr, &def.base ); + def.length = b3RecR_F32( rdr ); + def.enableSpring = b3RecR_BOOL( rdr ); + def.lowerSpringForce = b3RecR_F32( rdr ); + def.upperSpringForce = b3RecR_F32( rdr ); + def.hertz = b3RecR_F32( rdr ); + def.dampingRatio = b3RecR_F32( rdr ); + def.enableLimit = b3RecR_BOOL( rdr ); + def.minLength = b3RecR_F32( rdr ); + def.maxLength = b3RecR_F32( rdr ); + def.enableMotor = b3RecR_BOOL( rdr ); + def.maxMotorForce = b3RecR_F32( rdr ); + def.motorSpeed = b3RecR_F32( rdr ); + return def; +} + +b3FilterJointDef b3RecR_FILTERJOINTDEF( b3RecReader* rdr ) +{ + b3FilterJointDef def = b3DefaultFilterJointDef(); + b3RecR_JointBase( rdr, &def.base ); + return def; +} + +b3MotorJointDef b3RecR_MOTORJOINTDEF( b3RecReader* rdr ) +{ + b3MotorJointDef def = b3DefaultMotorJointDef(); + b3RecR_JointBase( rdr, &def.base ); + def.linearVelocity = b3RecR_VEC3( rdr ); + def.maxVelocityForce = b3RecR_F32( rdr ); + def.angularVelocity = b3RecR_VEC3( rdr ); + def.maxVelocityTorque = b3RecR_F32( rdr ); + def.linearHertz = b3RecR_F32( rdr ); + def.linearDampingRatio = b3RecR_F32( rdr ); + def.maxSpringForce = b3RecR_F32( rdr ); + def.angularHertz = b3RecR_F32( rdr ); + def.angularDampingRatio = b3RecR_F32( rdr ); + def.maxSpringTorque = b3RecR_F32( rdr ); + return def; +} + +b3PrismaticJointDef b3RecR_PRISMATICJOINTDEF( b3RecReader* rdr ) +{ + b3PrismaticJointDef def = b3DefaultPrismaticJointDef(); + b3RecR_JointBase( rdr, &def.base ); + def.enableSpring = b3RecR_BOOL( rdr ); + def.hertz = b3RecR_F32( rdr ); + def.dampingRatio = b3RecR_F32( rdr ); + def.targetTranslation = b3RecR_F32( rdr ); + def.enableLimit = b3RecR_BOOL( rdr ); + def.lowerTranslation = b3RecR_F32( rdr ); + def.upperTranslation = b3RecR_F32( rdr ); + def.enableMotor = b3RecR_BOOL( rdr ); + def.maxMotorForce = b3RecR_F32( rdr ); + def.motorSpeed = b3RecR_F32( rdr ); + return def; +} + +b3RevoluteJointDef b3RecR_REVOLUTEJOINTDEF( b3RecReader* rdr ) +{ + b3RevoluteJointDef def = b3DefaultRevoluteJointDef(); + b3RecR_JointBase( rdr, &def.base ); + def.targetAngle = b3RecR_F32( rdr ); + def.enableSpring = b3RecR_BOOL( rdr ); + def.hertz = b3RecR_F32( rdr ); + def.dampingRatio = b3RecR_F32( rdr ); + def.enableLimit = b3RecR_BOOL( rdr ); + def.lowerAngle = b3RecR_F32( rdr ); + def.upperAngle = b3RecR_F32( rdr ); + def.enableMotor = b3RecR_BOOL( rdr ); + def.maxMotorTorque = b3RecR_F32( rdr ); + def.motorSpeed = b3RecR_F32( rdr ); + return def; +} + +b3SphericalJointDef b3RecR_SPHERICALJOINTDEF( b3RecReader* rdr ) +{ + b3SphericalJointDef def = b3DefaultSphericalJointDef(); + b3RecR_JointBase( rdr, &def.base ); + def.enableSpring = b3RecR_BOOL( rdr ); + def.hertz = b3RecR_F32( rdr ); + def.dampingRatio = b3RecR_F32( rdr ); + def.targetRotation = b3RecR_QUAT( rdr ); + def.enableConeLimit = b3RecR_BOOL( rdr ); + def.coneAngle = b3RecR_F32( rdr ); + def.enableTwistLimit = b3RecR_BOOL( rdr ); + def.lowerTwistAngle = b3RecR_F32( rdr ); + def.upperTwistAngle = b3RecR_F32( rdr ); + def.enableMotor = b3RecR_BOOL( rdr ); + def.maxMotorTorque = b3RecR_F32( rdr ); + def.motorVelocity = b3RecR_VEC3( rdr ); + return def; +} + +b3WeldJointDef b3RecR_WELDJOINTDEF( b3RecReader* rdr ) +{ + b3WeldJointDef def = b3DefaultWeldJointDef(); + b3RecR_JointBase( rdr, &def.base ); + def.linearHertz = b3RecR_F32( rdr ); + def.angularHertz = b3RecR_F32( rdr ); + def.linearDampingRatio = b3RecR_F32( rdr ); + def.angularDampingRatio = b3RecR_F32( rdr ); + return def; +} + +b3WheelJointDef b3RecR_WHEELJOINTDEF( b3RecReader* rdr ) +{ + b3WheelJointDef def = b3DefaultWheelJointDef(); + b3RecR_JointBase( rdr, &def.base ); + def.enableSuspensionSpring = b3RecR_BOOL( rdr ); + def.suspensionHertz = b3RecR_F32( rdr ); + def.suspensionDampingRatio = b3RecR_F32( rdr ); + def.enableSuspensionLimit = b3RecR_BOOL( rdr ); + def.lowerSuspensionLimit = b3RecR_F32( rdr ); + def.upperSuspensionLimit = b3RecR_F32( rdr ); + def.enableSpinMotor = b3RecR_BOOL( rdr ); + def.maxSpinTorque = b3RecR_F32( rdr ); + def.spinSpeed = b3RecR_F32( rdr ); + def.enableSteering = b3RecR_BOOL( rdr ); + def.steeringHertz = b3RecR_F32( rdr ); + def.steeringDampingRatio = b3RecR_F32( rdr ); + def.targetSteeringAngle = b3RecR_F32( rdr ); + def.maxSteeringTorque = b3RecR_F32( rdr ); + def.enableSteeringLimit = b3RecR_BOOL( rdr ); + def.lowerSteeringLimit = b3RecR_F32( rdr ); + def.upperSteeringLimit = b3RecR_F32( rdr ); + return def; +} + +// Outliner body tracking. Defined after the player struct; forward declared here because the +// create/destroy dispatch sits above that struct. +static void b3RecTrackBodyCreate( b3RecPlayer* player, b3BodyId id ); +static void b3RecTrackBodyDestroy( b3RecPlayer* player, b3BodyId id ); + +// Id retargeting: replace world0 with the replay world's slot index. + +static b3BodyId b3RecMakeBodyId( b3RecReader* rdr, b3BodyId recorded ) +{ + b3BodyId id; + id.index1 = recorded.index1; + id.world0 = (uint16_t)( rdr->replayWorldId.index1 - 1u ); + id.generation = recorded.generation; + return id; +} + +static b3ShapeId b3RecMakeShapeId( b3RecReader* rdr, b3ShapeId recorded ) +{ + b3ShapeId id; + id.index1 = recorded.index1; + id.world0 = (uint16_t)( rdr->replayWorldId.index1 - 1u ); + id.generation = recorded.generation; + return id; +} + +static b3JointId b3RecMakeJointId( b3RecReader* rdr, b3JointId recorded ) +{ + b3JointId id; + id.index1 = recorded.index1; + id.world0 = (uint16_t)( rdr->replayWorldId.index1 - 1u ); + id.generation = recorded.generation; + return id; +} + +// A create op appends the returned id after args. index1 and generation must match; +// world0 always differs so we ignore it. +static void b3RecCheckId( b3RecReader* rdr, const char* kind, int gotIndex, unsigned gotGen, int recIndex, unsigned recGen ) +{ + if ( gotIndex != recIndex || gotGen != recGen ) + { + printf( "b3ReplayFile: %s id mismatch (rec index1=%d gen=%u, got index1=%d gen=%u)\n", kind, recIndex, recGen, gotIndex, + gotGen ); + rdr->ok = false; + } +} + +static void b3RecCheckBodyId( b3RecReader* rdr, b3BodyId got, b3BodyId rec ) +{ + b3RecCheckId( rdr, "body", got.index1, got.generation, rec.index1, rec.generation ); +} + +static void b3RecCheckShapeId( b3RecReader* rdr, b3ShapeId got, b3ShapeId rec ) +{ + b3RecCheckId( rdr, "shape", got.index1, got.generation, rec.index1, rec.generation ); +} + +static void b3RecCheckJointId( b3RecReader* rdr, b3JointId got, b3JointId rec ) +{ + b3RecCheckId( rdr, "joint", got.index1, got.generation, rec.index1, rec.generation ); +} + +// Registry slot reconstruction. Returns the live pointer for the given slot, building it +// on first use. The hull case is handled inline at the call site since it doesn't cache. + +static void* b3RecGetLiveMesh( b3RegistrySlot* slot ) +{ + // Mesh is a self-contained blob used by reference, with no pointer fixup. Hand back the pristine + // bytes directly: they already outlive the world and are freed at teardown, so a copy would just + // double the memory. Compound can't do this, see b3RecGetLiveCompound. + return slot->bytes; +} + +static void* b3RecGetLiveHeightField( b3RegistrySlot* slot ) +{ + // Self-contained blob used by reference, like b3RecGetLiveMesh. The bytes already are a + // valid b3HeightFieldData with no pointer fixup, so hand them back directly. + return slot->bytes; +} + +static void* b3RecGetLiveCompound( b3RegistrySlot* slot ) +{ + if ( slot->live != NULL ) + { + return slot->live; + } + // The copy is unavoidable here: b3ConvertBytesToCompound rewrites its input in place, while the + // pristine bytes must survive for keyframe registry seeding (b3RecSeedKeyframeRegistry). So we + // keep both the serialized bytes and a separate converted live object. + slot->live = b3Alloc( (size_t)slot->byteCount ); + memcpy( slot->live, slot->bytes, (size_t)slot->byteCount ); + b3ConvertBytesToCompound( (uint8_t*)slot->live, slot->byteCount ); + return slot->live; +} + +// Dispatch functions, one per op + +static void b3RecDispatch_DestroyWorld( const b3RecArgs_DestroyWorld* a, b3RecReader* rdr ) +{ + (void)a; + (void)rdr; + // End-of-session marker. The replay world is torn down in b3ValidateReplay, not here. +} + +static void b3RecDispatch_Step( const b3RecArgs_Step* a, b3RecReader* rdr ) +{ + (void)a; + b3World_Step( rdr->replayWorldId, a->dt, a->subStepCount ); +} + +static void b3RecDispatch_WorldEnableSleeping( const b3RecArgs_WorldEnableSleeping* a, b3RecReader* rdr ) +{ + b3World_EnableSleeping( rdr->replayWorldId, a->flag ); +} + +static void b3RecDispatch_WorldEnableContinuous( const b3RecArgs_WorldEnableContinuous* a, b3RecReader* rdr ) +{ + b3World_EnableContinuous( rdr->replayWorldId, a->flag ); +} + +static void b3RecDispatch_WorldSetRestitutionThreshold( const b3RecArgs_WorldSetRestitutionThreshold* a, b3RecReader* rdr ) +{ + b3World_SetRestitutionThreshold( rdr->replayWorldId, a->value ); +} + +static void b3RecDispatch_WorldSetHitEventThreshold( const b3RecArgs_WorldSetHitEventThreshold* a, b3RecReader* rdr ) +{ + b3World_SetHitEventThreshold( rdr->replayWorldId, a->value ); +} + +static void b3RecDispatch_WorldSetGravity( const b3RecArgs_WorldSetGravity* a, b3RecReader* rdr ) +{ + b3World_SetGravity( rdr->replayWorldId, a->gravity ); +} + +static void b3RecDispatch_WorldExplode( const b3RecArgs_WorldExplode* a, b3RecReader* rdr ) +{ + b3World_Explode( rdr->replayWorldId, &a->def ); +} + +static void b3RecDispatch_WorldSetContactTuning( const b3RecArgs_WorldSetContactTuning* a, b3RecReader* rdr ) +{ + b3World_SetContactTuning( rdr->replayWorldId, a->hertz, a->dampingRatio, a->contactSpeed ); +} + +static void b3RecDispatch_WorldSetContactRecycleDistance( const b3RecArgs_WorldSetContactRecycleDistance* a, b3RecReader* rdr ) +{ + b3World_SetContactRecycleDistance( rdr->replayWorldId, a->recycleDistance ); +} + +static void b3RecDispatch_WorldSetMaximumLinearSpeed( const b3RecArgs_WorldSetMaximumLinearSpeed* a, b3RecReader* rdr ) +{ + b3World_SetMaximumLinearSpeed( rdr->replayWorldId, a->maximumLinearSpeed ); +} + +static void b3RecDispatch_WorldEnableWarmStarting( const b3RecArgs_WorldEnableWarmStarting* a, b3RecReader* rdr ) +{ + b3World_EnableWarmStarting( rdr->replayWorldId, a->flag ); +} + +static void b3RecDispatch_WorldRebuildStaticTree( const b3RecArgs_WorldRebuildStaticTree* a, b3RecReader* rdr ) +{ + (void)a; + b3World_RebuildStaticTree( rdr->replayWorldId ); +} + +static void b3RecDispatch_WorldEnableSpeculative( const b3RecArgs_WorldEnableSpeculative* a, b3RecReader* rdr ) +{ + b3World_EnableSpeculative( rdr->replayWorldId, a->flag ); +} + +static void b3RecDispatch_CreateBody( const b3RecArgs_CreateBody* a, b3RecReader* rdr ) +{ + b3BodyId recId = b3RecR_BODYID( rdr ); + b3BodyId gotId = b3CreateBody( rdr->replayWorldId, &a->def ); + b3RecCheckBodyId( rdr, gotId, recId ); + if ( rdr->owner != NULL ) + { + b3RecTrackBodyCreate( rdr->owner, gotId ); + } +} + +static void b3RecDispatch_DestroyBody( const b3RecArgs_DestroyBody* a, b3RecReader* rdr ) +{ + b3BodyId id = b3RecMakeBodyId( rdr, a->body ); + if ( rdr->owner != NULL ) + { + b3RecTrackBodyDestroy( rdr->owner, id ); + } + b3DestroyBody( id ); +} + +static void b3RecDispatch_BodySetTransform( const b3RecArgs_BodySetTransform* a, b3RecReader* rdr ) +{ + b3Body_SetTransform( b3RecMakeBodyId( rdr, a->body ), a->position, a->rotation ); +} + +static void b3RecDispatch_BodySetLinearVelocity( const b3RecArgs_BodySetLinearVelocity* a, b3RecReader* rdr ) +{ + b3Body_SetLinearVelocity( b3RecMakeBodyId( rdr, a->body ), a->v ); +} + +static void b3RecDispatch_BodySetType( const b3RecArgs_BodySetType* a, b3RecReader* rdr ) +{ + b3Body_SetType( b3RecMakeBodyId( rdr, a->body ), (b3BodyType)a->type ); +} + +static void b3RecDispatch_BodySetName( const b3RecArgs_BodySetName* a, b3RecReader* rdr ) +{ + b3Body_SetName( b3RecMakeBodyId( rdr, a->body ), a->name ); +} + +static void b3RecDispatch_BodySetAngularVelocity( const b3RecArgs_BodySetAngularVelocity* a, b3RecReader* rdr ) +{ + b3Body_SetAngularVelocity( b3RecMakeBodyId( rdr, a->body ), a->w ); +} + +static void b3RecDispatch_BodySetTargetTransform( const b3RecArgs_BodySetTargetTransform* a, b3RecReader* rdr ) +{ + b3Body_SetTargetTransform( b3RecMakeBodyId( rdr, a->body ), a->target, a->timeStep, a->wake ); +} + +static void b3RecDispatch_BodyApplyForce( const b3RecArgs_BodyApplyForce* a, b3RecReader* rdr ) +{ + b3Body_ApplyForce( b3RecMakeBodyId( rdr, a->body ), a->force, a->point, a->wake ); +} + +static void b3RecDispatch_BodyApplyForceToCenter( const b3RecArgs_BodyApplyForceToCenter* a, b3RecReader* rdr ) +{ + b3Body_ApplyForceToCenter( b3RecMakeBodyId( rdr, a->body ), a->force, a->wake ); +} + +static void b3RecDispatch_BodyApplyTorque( const b3RecArgs_BodyApplyTorque* a, b3RecReader* rdr ) +{ + b3Body_ApplyTorque( b3RecMakeBodyId( rdr, a->body ), a->torque, a->wake ); +} + +static void b3RecDispatch_BodyApplyLinearImpulse( const b3RecArgs_BodyApplyLinearImpulse* a, b3RecReader* rdr ) +{ + b3Body_ApplyLinearImpulse( b3RecMakeBodyId( rdr, a->body ), a->impulse, a->point, a->wake ); +} + +static void b3RecDispatch_BodyApplyLinearImpulseToCenter( const b3RecArgs_BodyApplyLinearImpulseToCenter* a, b3RecReader* rdr ) +{ + b3Body_ApplyLinearImpulseToCenter( b3RecMakeBodyId( rdr, a->body ), a->impulse, a->wake ); +} + +static void b3RecDispatch_BodyApplyAngularImpulse( const b3RecArgs_BodyApplyAngularImpulse* a, b3RecReader* rdr ) +{ + b3Body_ApplyAngularImpulse( b3RecMakeBodyId( rdr, a->body ), a->impulse, a->wake ); +} + +static void b3RecDispatch_BodySetMassData( const b3RecArgs_BodySetMassData* a, b3RecReader* rdr ) +{ + b3Body_SetMassData( b3RecMakeBodyId( rdr, a->body ), a->massData ); +} + +static void b3RecDispatch_BodyApplyMassFromShapes( const b3RecArgs_BodyApplyMassFromShapes* a, b3RecReader* rdr ) +{ + b3Body_ApplyMassFromShapes( b3RecMakeBodyId( rdr, a->body ) ); +} + +static void b3RecDispatch_BodySetLinearDamping( const b3RecArgs_BodySetLinearDamping* a, b3RecReader* rdr ) +{ + b3Body_SetLinearDamping( b3RecMakeBodyId( rdr, a->body ), a->damping ); +} + +static void b3RecDispatch_BodySetAngularDamping( const b3RecArgs_BodySetAngularDamping* a, b3RecReader* rdr ) +{ + b3Body_SetAngularDamping( b3RecMakeBodyId( rdr, a->body ), a->damping ); +} + +static void b3RecDispatch_BodySetGravityScale( const b3RecArgs_BodySetGravityScale* a, b3RecReader* rdr ) +{ + b3Body_SetGravityScale( b3RecMakeBodyId( rdr, a->body ), a->scale ); +} + +static void b3RecDispatch_BodySetAwake( const b3RecArgs_BodySetAwake* a, b3RecReader* rdr ) +{ + b3Body_SetAwake( b3RecMakeBodyId( rdr, a->body ), a->awake ); +} + +static void b3RecDispatch_BodyEnableSleep( const b3RecArgs_BodyEnableSleep* a, b3RecReader* rdr ) +{ + b3Body_EnableSleep( b3RecMakeBodyId( rdr, a->body ), a->flag ); +} + +static void b3RecDispatch_BodySetSleepThreshold( const b3RecArgs_BodySetSleepThreshold* a, b3RecReader* rdr ) +{ + b3Body_SetSleepThreshold( b3RecMakeBodyId( rdr, a->body ), a->threshold ); +} + +static void b3RecDispatch_BodyDisable( const b3RecArgs_BodyDisable* a, b3RecReader* rdr ) +{ + b3Body_Disable( b3RecMakeBodyId( rdr, a->body ) ); +} + +static void b3RecDispatch_BodyEnable( const b3RecArgs_BodyEnable* a, b3RecReader* rdr ) +{ + b3Body_Enable( b3RecMakeBodyId( rdr, a->body ) ); +} + +static void b3RecDispatch_BodySetMotionLocks( const b3RecArgs_BodySetMotionLocks* a, b3RecReader* rdr ) +{ + b3Body_SetMotionLocks( b3RecMakeBodyId( rdr, a->body ), a->locks ); +} + +static void b3RecDispatch_BodySetBullet( const b3RecArgs_BodySetBullet* a, b3RecReader* rdr ) +{ + b3Body_SetBullet( b3RecMakeBodyId( rdr, a->body ), a->flag ); +} + +static void b3RecDispatch_BodyEnableContactRecycling( const b3RecArgs_BodyEnableContactRecycling* a, b3RecReader* rdr ) +{ + b3Body_EnableContactRecycling( b3RecMakeBodyId( rdr, a->body ), a->flag ); +} + +static void b3RecDispatch_BodyEnableHitEvents( const b3RecArgs_BodyEnableHitEvents* a, b3RecReader* rdr ) +{ + b3Body_EnableHitEvents( b3RecMakeBodyId( rdr, a->body ), a->flag ); +} + +static void b3RecDispatch_CreateSphereShape( const b3RecArgs_CreateSphereShape* a, b3RecReader* rdr ) +{ + b3ShapeId recId = b3RecR_SHAPEID( rdr ); + b3BodyId bodyId = b3RecMakeBodyId( rdr, a->body ); + b3ShapeId gotId = b3CreateSphereShape( bodyId, &a->def, &a->sphere ); + b3RecCheckShapeId( rdr, gotId, recId ); +} + +static void b3RecDispatch_CreateCapsuleShape( const b3RecArgs_CreateCapsuleShape* a, b3RecReader* rdr ) +{ + b3ShapeId recId = b3RecR_SHAPEID( rdr ); + b3BodyId bodyId = b3RecMakeBodyId( rdr, a->body ); + b3ShapeId gotId = b3CreateCapsuleShape( bodyId, &a->def, &a->capsule ); + b3RecCheckShapeId( rdr, gotId, recId ); +} + +static void b3RecDispatch_CreateHullShape( const b3RecArgs_CreateHullShape* a, b3RecReader* rdr ) +{ + b3ShapeId recId = b3RecR_SHAPEID( rdr ); + if ( !rdr->ok ) + { + return; + } + uint32_t id = a->geometryId; + if ( id >= (uint32_t)rdr->slotCount ) + { + printf( "b3ReplayFile: hull geometryId %u out of range\n", id ); + rdr->ok = false; + return; + } + b3RegistrySlot* slot = rdr->slots + id; + b3BodyId bodyId = b3RecMakeBodyId( rdr, a->body ); + // Hull is cloned by b3CreateHullShape into the world DB; no caching needed. + b3ShapeId gotId = b3CreateHullShape( bodyId, &a->def, (const b3HullData*)slot->bytes ); + b3RecCheckShapeId( rdr, gotId, recId ); +} + +static void b3RecDispatch_CreateMeshShape( const b3RecArgs_CreateMeshShape* a, b3RecReader* rdr ) +{ + b3ShapeId recId = b3RecR_SHAPEID( rdr ); + if ( !rdr->ok ) + { + return; + } + uint32_t id = a->geometryId; + if ( id >= (uint32_t)rdr->slotCount ) + { + printf( "b3ReplayFile: mesh geometryId %u out of range\n", id ); + rdr->ok = false; + return; + } + b3RegistrySlot* slot = rdr->slots + id; + const b3MeshData* mesh = (const b3MeshData*)b3RecGetLiveMesh( slot ); + b3BodyId bodyId = b3RecMakeBodyId( rdr, a->body ); + b3ShapeId gotId = b3CreateMeshShape( bodyId, &a->def, mesh, a->scale ); + b3RecCheckShapeId( rdr, gotId, recId ); +} + +static void b3RecDispatch_CreateHeightFieldShape( const b3RecArgs_CreateHeightFieldShape* a, b3RecReader* rdr ) +{ + b3ShapeId recId = b3RecR_SHAPEID( rdr ); + if ( !rdr->ok ) + { + return; + } + uint32_t id = a->geometryId; + if ( id >= (uint32_t)rdr->slotCount ) + { + printf( "b3ReplayFile: heightfield geometryId %u out of range\n", id ); + rdr->ok = false; + return; + } + b3RegistrySlot* slot = rdr->slots + id; + const b3HeightFieldData* hf = (const b3HeightFieldData*)b3RecGetLiveHeightField( slot ); + if ( hf == NULL ) + { + printf( "b3ReplayFile: heightfield geometry %u is corrupt\n", id ); + rdr->ok = false; + return; + } + b3BodyId bodyId = b3RecMakeBodyId( rdr, a->body ); + b3ShapeId gotId = b3CreateHeightFieldShape( bodyId, &a->def, hf ); + b3RecCheckShapeId( rdr, gotId, recId ); +} + +static void b3RecDispatch_CreateCompoundShape( const b3RecArgs_CreateCompoundShape* a, b3RecReader* rdr ) +{ + b3ShapeId recId = b3RecR_SHAPEID( rdr ); + if ( !rdr->ok ) + { + return; + } + uint32_t id = a->geometryId; + if ( id >= (uint32_t)rdr->slotCount ) + { + printf( "b3ReplayFile: compound geometryId %u out of range\n", id ); + rdr->ok = false; + return; + } + b3RegistrySlot* slot = rdr->slots + id; + const b3CompoundData* compound = (const b3CompoundData*)b3RecGetLiveCompound( slot ); + b3BodyId bodyId = b3RecMakeBodyId( rdr, a->body ); + // b3CreateCompoundShape takes a non-const def pointer; cast away const for the scratch def + b3ShapeDef shapeDef = a->def; + b3ShapeId gotId = b3CreateCompoundShape( bodyId, &shapeDef, compound ); + b3RecCheckShapeId( rdr, gotId, recId ); +} + +static void b3RecDispatch_DestroyShape( const b3RecArgs_DestroyShape* a, b3RecReader* rdr ) +{ + b3DestroyShape( b3RecMakeShapeId( rdr, a->shape ), a->updateBodyMass ); +} + +static void b3RecDispatch_ShapeSetName( const b3RecArgs_ShapeSetName* a, b3RecReader* rdr ) +{ + b3Shape_SetName( b3RecMakeShapeId( rdr, a->shape ), a->name ); +} + +static void b3RecDispatch_ShapeSetDensity( const b3RecArgs_ShapeSetDensity* a, b3RecReader* rdr ) +{ + b3Shape_SetDensity( b3RecMakeShapeId( rdr, a->shape ), a->density, a->updateBodyMass ); +} + +static void b3RecDispatch_ShapeSetFriction( const b3RecArgs_ShapeSetFriction* a, b3RecReader* rdr ) +{ + b3Shape_SetFriction( b3RecMakeShapeId( rdr, a->shape ), a->friction ); +} + +static void b3RecDispatch_ShapeSetRestitution( const b3RecArgs_ShapeSetRestitution* a, b3RecReader* rdr ) +{ + b3Shape_SetRestitution( b3RecMakeShapeId( rdr, a->shape ), a->restitution ); +} + +static void b3RecDispatch_ShapeSetSurfaceMaterial( const b3RecArgs_ShapeSetSurfaceMaterial* a, b3RecReader* rdr ) +{ + b3Shape_SetSurfaceMaterial( b3RecMakeShapeId( rdr, a->shape ), a->material ); +} + +static void b3RecDispatch_ShapeSetFilter( const b3RecArgs_ShapeSetFilter* a, b3RecReader* rdr ) +{ + b3Shape_SetFilter( b3RecMakeShapeId( rdr, a->shape ), a->filter, a->invokeContacts ); +} + +static void b3RecDispatch_ShapeEnableSensorEvents( const b3RecArgs_ShapeEnableSensorEvents* a, b3RecReader* rdr ) +{ + b3Shape_EnableSensorEvents( b3RecMakeShapeId( rdr, a->shape ), a->flag ); +} + +static void b3RecDispatch_ShapeEnableContactEvents( const b3RecArgs_ShapeEnableContactEvents* a, b3RecReader* rdr ) +{ + b3Shape_EnableContactEvents( b3RecMakeShapeId( rdr, a->shape ), a->flag ); +} + +static void b3RecDispatch_ShapeEnablePreSolveEvents( const b3RecArgs_ShapeEnablePreSolveEvents* a, b3RecReader* rdr ) +{ + b3Shape_EnablePreSolveEvents( b3RecMakeShapeId( rdr, a->shape ), a->flag ); +} + +static void b3RecDispatch_ShapeEnableHitEvents( const b3RecArgs_ShapeEnableHitEvents* a, b3RecReader* rdr ) +{ + b3Shape_EnableHitEvents( b3RecMakeShapeId( rdr, a->shape ), a->flag ); +} + +static void b3RecDispatch_ShapeSetSphere( const b3RecArgs_ShapeSetSphere* a, b3RecReader* rdr ) +{ + b3Shape_SetSphere( b3RecMakeShapeId( rdr, a->shape ), &a->sphere ); +} + +static void b3RecDispatch_ShapeSetCapsule( const b3RecArgs_ShapeSetCapsule* a, b3RecReader* rdr ) +{ + b3Shape_SetCapsule( b3RecMakeShapeId( rdr, a->shape ), &a->capsule ); +} + +static void b3RecDispatch_ShapeApplyWind( const b3RecArgs_ShapeApplyWind* a, b3RecReader* rdr ) +{ + b3Shape_ApplyWind( b3RecMakeShapeId( rdr, a->shape ), a->wind, a->drag, a->lift, a->maxSpeed, a->wake ); +} + +// Joint creates: remap body ids in the def before calling the API. + +static void b3RecDispatch_CreateParallelJoint( const b3RecArgs_CreateParallelJoint* a, b3RecReader* rdr ) +{ + b3JointId recId = b3RecR_JOINTID( rdr ); + b3ParallelJointDef def = a->def; + def.base.bodyIdA = b3RecMakeBodyId( rdr, def.base.bodyIdA ); + def.base.bodyIdB = b3RecMakeBodyId( rdr, def.base.bodyIdB ); + b3RecCheckJointId( rdr, b3CreateParallelJoint( rdr->replayWorldId, &def ), recId ); +} + +static void b3RecDispatch_CreateDistanceJoint( const b3RecArgs_CreateDistanceJoint* a, b3RecReader* rdr ) +{ + b3JointId recId = b3RecR_JOINTID( rdr ); + b3DistanceJointDef def = a->def; + def.base.bodyIdA = b3RecMakeBodyId( rdr, def.base.bodyIdA ); + def.base.bodyIdB = b3RecMakeBodyId( rdr, def.base.bodyIdB ); + b3RecCheckJointId( rdr, b3CreateDistanceJoint( rdr->replayWorldId, &def ), recId ); +} + +static void b3RecDispatch_CreateFilterJoint( const b3RecArgs_CreateFilterJoint* a, b3RecReader* rdr ) +{ + b3JointId recId = b3RecR_JOINTID( rdr ); + b3FilterJointDef def = a->def; + def.base.bodyIdA = b3RecMakeBodyId( rdr, def.base.bodyIdA ); + def.base.bodyIdB = b3RecMakeBodyId( rdr, def.base.bodyIdB ); + b3RecCheckJointId( rdr, b3CreateFilterJoint( rdr->replayWorldId, &def ), recId ); +} + +static void b3RecDispatch_CreateMotorJoint( const b3RecArgs_CreateMotorJoint* a, b3RecReader* rdr ) +{ + b3JointId recId = b3RecR_JOINTID( rdr ); + b3MotorJointDef def = a->def; + def.base.bodyIdA = b3RecMakeBodyId( rdr, def.base.bodyIdA ); + def.base.bodyIdB = b3RecMakeBodyId( rdr, def.base.bodyIdB ); + b3RecCheckJointId( rdr, b3CreateMotorJoint( rdr->replayWorldId, &def ), recId ); +} + +static void b3RecDispatch_CreatePrismaticJoint( const b3RecArgs_CreatePrismaticJoint* a, b3RecReader* rdr ) +{ + b3JointId recId = b3RecR_JOINTID( rdr ); + b3PrismaticJointDef def = a->def; + def.base.bodyIdA = b3RecMakeBodyId( rdr, def.base.bodyIdA ); + def.base.bodyIdB = b3RecMakeBodyId( rdr, def.base.bodyIdB ); + b3RecCheckJointId( rdr, b3CreatePrismaticJoint( rdr->replayWorldId, &def ), recId ); +} + +static void b3RecDispatch_CreateRevoluteJoint( const b3RecArgs_CreateRevoluteJoint* a, b3RecReader* rdr ) +{ + b3JointId recId = b3RecR_JOINTID( rdr ); + b3RevoluteJointDef def = a->def; + def.base.bodyIdA = b3RecMakeBodyId( rdr, def.base.bodyIdA ); + def.base.bodyIdB = b3RecMakeBodyId( rdr, def.base.bodyIdB ); + b3RecCheckJointId( rdr, b3CreateRevoluteJoint( rdr->replayWorldId, &def ), recId ); +} + +static void b3RecDispatch_CreateSphericalJoint( const b3RecArgs_CreateSphericalJoint* a, b3RecReader* rdr ) +{ + b3JointId recId = b3RecR_JOINTID( rdr ); + b3SphericalJointDef def = a->def; + def.base.bodyIdA = b3RecMakeBodyId( rdr, def.base.bodyIdA ); + def.base.bodyIdB = b3RecMakeBodyId( rdr, def.base.bodyIdB ); + b3RecCheckJointId( rdr, b3CreateSphericalJoint( rdr->replayWorldId, &def ), recId ); +} + +static void b3RecDispatch_CreateWeldJoint( const b3RecArgs_CreateWeldJoint* a, b3RecReader* rdr ) +{ + b3JointId recId = b3RecR_JOINTID( rdr ); + b3WeldJointDef def = a->def; + def.base.bodyIdA = b3RecMakeBodyId( rdr, def.base.bodyIdA ); + def.base.bodyIdB = b3RecMakeBodyId( rdr, def.base.bodyIdB ); + b3RecCheckJointId( rdr, b3CreateWeldJoint( rdr->replayWorldId, &def ), recId ); +} + +static void b3RecDispatch_CreateWheelJoint( const b3RecArgs_CreateWheelJoint* a, b3RecReader* rdr ) +{ + b3JointId recId = b3RecR_JOINTID( rdr ); + b3WheelJointDef def = a->def; + def.base.bodyIdA = b3RecMakeBodyId( rdr, def.base.bodyIdA ); + def.base.bodyIdB = b3RecMakeBodyId( rdr, def.base.bodyIdB ); + b3RecCheckJointId( rdr, b3CreateWheelJoint( rdr->replayWorldId, &def ), recId ); +} + +static void b3RecDispatch_DestroyJoint( const b3RecArgs_DestroyJoint* a, b3RecReader* rdr ) +{ + b3DestroyJoint( b3RecMakeJointId( rdr, a->joint ), a->wakeAttached ); +} + +static void b3RecDispatch_JointSetLocalFrameA( const b3RecArgs_JointSetLocalFrameA* a, b3RecReader* rdr ) +{ + b3Joint_SetLocalFrameA( b3RecMakeJointId( rdr, a->joint ), a->localFrame ); +} + +static void b3RecDispatch_JointSetLocalFrameB( const b3RecArgs_JointSetLocalFrameB* a, b3RecReader* rdr ) +{ + b3Joint_SetLocalFrameB( b3RecMakeJointId( rdr, a->joint ), a->localFrame ); +} + +static void b3RecDispatch_JointSetCollideConnected( const b3RecArgs_JointSetCollideConnected* a, b3RecReader* rdr ) +{ + b3Joint_SetCollideConnected( b3RecMakeJointId( rdr, a->joint ), a->shouldCollide ); +} + +static void b3RecDispatch_JointWakeBodies( const b3RecArgs_JointWakeBodies* a, b3RecReader* rdr ) +{ + b3Joint_WakeBodies( b3RecMakeJointId( rdr, a->joint ) ); +} + +static void b3RecDispatch_JointSetConstraintTuning( const b3RecArgs_JointSetConstraintTuning* a, b3RecReader* rdr ) +{ + b3Joint_SetConstraintTuning( b3RecMakeJointId( rdr, a->joint ), a->hertz, a->dampingRatio ); +} + +static void b3RecDispatch_JointSetForceThreshold( const b3RecArgs_JointSetForceThreshold* a, b3RecReader* rdr ) +{ + b3Joint_SetForceThreshold( b3RecMakeJointId( rdr, a->joint ), a->threshold ); +} + +static void b3RecDispatch_JointSetTorqueThreshold( const b3RecArgs_JointSetTorqueThreshold* a, b3RecReader* rdr ) +{ + b3Joint_SetTorqueThreshold( b3RecMakeJointId( rdr, a->joint ), a->threshold ); +} + +static void b3RecDispatch_ParallelJointSetSpringHertz( const b3RecArgs_ParallelJointSetSpringHertz* a, b3RecReader* rdr ) +{ + b3ParallelJoint_SetSpringHertz( b3RecMakeJointId( rdr, a->joint ), a->hertz ); +} + +static void b3RecDispatch_ParallelJointSetSpringDampingRatio( const b3RecArgs_ParallelJointSetSpringDampingRatio* a, + b3RecReader* rdr ) +{ + b3ParallelJoint_SetSpringDampingRatio( b3RecMakeJointId( rdr, a->joint ), a->dampingRatio ); +} + +static void b3RecDispatch_ParallelJointSetMaxTorque( const b3RecArgs_ParallelJointSetMaxTorque* a, b3RecReader* rdr ) +{ + b3ParallelJoint_SetMaxTorque( b3RecMakeJointId( rdr, a->joint ), a->maxTorque ); +} + +static void b3RecDispatch_DistanceJointSetLength( const b3RecArgs_DistanceJointSetLength* a, b3RecReader* rdr ) +{ + b3DistanceJoint_SetLength( b3RecMakeJointId( rdr, a->joint ), a->length ); +} + +static void b3RecDispatch_DistanceJointEnableSpring( const b3RecArgs_DistanceJointEnableSpring* a, b3RecReader* rdr ) +{ + b3DistanceJoint_EnableSpring( b3RecMakeJointId( rdr, a->joint ), a->enableSpring ); +} + +static void b3RecDispatch_DistanceJointSetSpringForceRange( const b3RecArgs_DistanceJointSetSpringForceRange* a, + b3RecReader* rdr ) +{ + b3DistanceJoint_SetSpringForceRange( b3RecMakeJointId( rdr, a->joint ), a->lowerForce, a->upperForce ); +} + +static void b3RecDispatch_DistanceJointSetSpringHertz( const b3RecArgs_DistanceJointSetSpringHertz* a, b3RecReader* rdr ) +{ + b3DistanceJoint_SetSpringHertz( b3RecMakeJointId( rdr, a->joint ), a->hertz ); +} + +static void b3RecDispatch_DistanceJointSetSpringDampingRatio( const b3RecArgs_DistanceJointSetSpringDampingRatio* a, + b3RecReader* rdr ) +{ + b3DistanceJoint_SetSpringDampingRatio( b3RecMakeJointId( rdr, a->joint ), a->dampingRatio ); +} + +static void b3RecDispatch_DistanceJointEnableLimit( const b3RecArgs_DistanceJointEnableLimit* a, b3RecReader* rdr ) +{ + b3DistanceJoint_EnableLimit( b3RecMakeJointId( rdr, a->joint ), a->enableLimit ); +} + +static void b3RecDispatch_DistanceJointSetLengthRange( const b3RecArgs_DistanceJointSetLengthRange* a, b3RecReader* rdr ) +{ + b3DistanceJoint_SetLengthRange( b3RecMakeJointId( rdr, a->joint ), a->minLength, a->maxLength ); +} + +static void b3RecDispatch_DistanceJointEnableMotor( const b3RecArgs_DistanceJointEnableMotor* a, b3RecReader* rdr ) +{ + b3DistanceJoint_EnableMotor( b3RecMakeJointId( rdr, a->joint ), a->enableMotor ); +} + +static void b3RecDispatch_DistanceJointSetMotorSpeed( const b3RecArgs_DistanceJointSetMotorSpeed* a, b3RecReader* rdr ) +{ + b3DistanceJoint_SetMotorSpeed( b3RecMakeJointId( rdr, a->joint ), a->motorSpeed ); +} + +static void b3RecDispatch_DistanceJointSetMaxMotorForce( const b3RecArgs_DistanceJointSetMaxMotorForce* a, b3RecReader* rdr ) +{ + b3DistanceJoint_SetMaxMotorForce( b3RecMakeJointId( rdr, a->joint ), a->force ); +} + +static void b3RecDispatch_MotorJointSetLinearVelocity( const b3RecArgs_MotorJointSetLinearVelocity* a, b3RecReader* rdr ) +{ + b3MotorJoint_SetLinearVelocity( b3RecMakeJointId( rdr, a->joint ), a->velocity ); +} + +static void b3RecDispatch_MotorJointSetAngularVelocity( const b3RecArgs_MotorJointSetAngularVelocity* a, b3RecReader* rdr ) +{ + b3MotorJoint_SetAngularVelocity( b3RecMakeJointId( rdr, a->joint ), a->velocity ); +} + +static void b3RecDispatch_MotorJointSetMaxVelocityForce( const b3RecArgs_MotorJointSetMaxVelocityForce* a, b3RecReader* rdr ) +{ + b3MotorJoint_SetMaxVelocityForce( b3RecMakeJointId( rdr, a->joint ), a->maxForce ); +} + +static void b3RecDispatch_MotorJointSetMaxVelocityTorque( const b3RecArgs_MotorJointSetMaxVelocityTorque* a, b3RecReader* rdr ) +{ + b3MotorJoint_SetMaxVelocityTorque( b3RecMakeJointId( rdr, a->joint ), a->maxTorque ); +} + +static void b3RecDispatch_MotorJointSetLinearHertz( const b3RecArgs_MotorJointSetLinearHertz* a, b3RecReader* rdr ) +{ + b3MotorJoint_SetLinearHertz( b3RecMakeJointId( rdr, a->joint ), a->hertz ); +} + +static void b3RecDispatch_MotorJointSetLinearDampingRatio( const b3RecArgs_MotorJointSetLinearDampingRatio* a, b3RecReader* rdr ) +{ + b3MotorJoint_SetLinearDampingRatio( b3RecMakeJointId( rdr, a->joint ), a->damping ); +} + +static void b3RecDispatch_MotorJointSetAngularHertz( const b3RecArgs_MotorJointSetAngularHertz* a, b3RecReader* rdr ) +{ + b3MotorJoint_SetAngularHertz( b3RecMakeJointId( rdr, a->joint ), a->hertz ); +} + +static void b3RecDispatch_MotorJointSetAngularDampingRatio( const b3RecArgs_MotorJointSetAngularDampingRatio* a, + b3RecReader* rdr ) +{ + b3MotorJoint_SetAngularDampingRatio( b3RecMakeJointId( rdr, a->joint ), a->damping ); +} + +static void b3RecDispatch_MotorJointSetMaxSpringForce( const b3RecArgs_MotorJointSetMaxSpringForce* a, b3RecReader* rdr ) +{ + b3MotorJoint_SetMaxSpringForce( b3RecMakeJointId( rdr, a->joint ), a->maxForce ); +} + +static void b3RecDispatch_MotorJointSetMaxSpringTorque( const b3RecArgs_MotorJointSetMaxSpringTorque* a, b3RecReader* rdr ) +{ + b3MotorJoint_SetMaxSpringTorque( b3RecMakeJointId( rdr, a->joint ), a->maxTorque ); +} + +static void b3RecDispatch_PrismaticJointEnableSpring( const b3RecArgs_PrismaticJointEnableSpring* a, b3RecReader* rdr ) +{ + b3PrismaticJoint_EnableSpring( b3RecMakeJointId( rdr, a->joint ), a->enableSpring ); +} + +static void b3RecDispatch_PrismaticJointSetSpringHertz( const b3RecArgs_PrismaticJointSetSpringHertz* a, b3RecReader* rdr ) +{ + b3PrismaticJoint_SetSpringHertz( b3RecMakeJointId( rdr, a->joint ), a->hertz ); +} + +static void b3RecDispatch_PrismaticJointSetSpringDampingRatio( const b3RecArgs_PrismaticJointSetSpringDampingRatio* a, + b3RecReader* rdr ) +{ + b3PrismaticJoint_SetSpringDampingRatio( b3RecMakeJointId( rdr, a->joint ), a->dampingRatio ); +} + +static void b3RecDispatch_PrismaticJointSetTargetTranslation( const b3RecArgs_PrismaticJointSetTargetTranslation* a, + b3RecReader* rdr ) +{ + b3PrismaticJoint_SetTargetTranslation( b3RecMakeJointId( rdr, a->joint ), a->translation ); +} + +static void b3RecDispatch_PrismaticJointEnableLimit( const b3RecArgs_PrismaticJointEnableLimit* a, b3RecReader* rdr ) +{ + b3PrismaticJoint_EnableLimit( b3RecMakeJointId( rdr, a->joint ), a->enableLimit ); +} + +static void b3RecDispatch_PrismaticJointSetLimits( const b3RecArgs_PrismaticJointSetLimits* a, b3RecReader* rdr ) +{ + b3PrismaticJoint_SetLimits( b3RecMakeJointId( rdr, a->joint ), a->lower, a->upper ); +} + +static void b3RecDispatch_PrismaticJointEnableMotor( const b3RecArgs_PrismaticJointEnableMotor* a, b3RecReader* rdr ) +{ + b3PrismaticJoint_EnableMotor( b3RecMakeJointId( rdr, a->joint ), a->enableMotor ); +} + +static void b3RecDispatch_PrismaticJointSetMotorSpeed( const b3RecArgs_PrismaticJointSetMotorSpeed* a, b3RecReader* rdr ) +{ + b3PrismaticJoint_SetMotorSpeed( b3RecMakeJointId( rdr, a->joint ), a->motorSpeed ); +} + +static void b3RecDispatch_PrismaticJointSetMaxMotorForce( const b3RecArgs_PrismaticJointSetMaxMotorForce* a, b3RecReader* rdr ) +{ + b3PrismaticJoint_SetMaxMotorForce( b3RecMakeJointId( rdr, a->joint ), a->force ); +} + +static void b3RecDispatch_RevoluteJointEnableSpring( const b3RecArgs_RevoluteJointEnableSpring* a, b3RecReader* rdr ) +{ + b3RevoluteJoint_EnableSpring( b3RecMakeJointId( rdr, a->joint ), a->enableSpring ); +} + +static void b3RecDispatch_RevoluteJointSetSpringHertz( const b3RecArgs_RevoluteJointSetSpringHertz* a, b3RecReader* rdr ) +{ + b3RevoluteJoint_SetSpringHertz( b3RecMakeJointId( rdr, a->joint ), a->hertz ); +} + +static void b3RecDispatch_RevoluteJointSetSpringDampingRatio( const b3RecArgs_RevoluteJointSetSpringDampingRatio* a, + b3RecReader* rdr ) +{ + b3RevoluteJoint_SetSpringDampingRatio( b3RecMakeJointId( rdr, a->joint ), a->dampingRatio ); +} + +static void b3RecDispatch_RevoluteJointSetTargetAngle( const b3RecArgs_RevoluteJointSetTargetAngle* a, b3RecReader* rdr ) +{ + b3RevoluteJoint_SetTargetAngle( b3RecMakeJointId( rdr, a->joint ), a->angle ); +} + +static void b3RecDispatch_RevoluteJointEnableLimit( const b3RecArgs_RevoluteJointEnableLimit* a, b3RecReader* rdr ) +{ + b3RevoluteJoint_EnableLimit( b3RecMakeJointId( rdr, a->joint ), a->enableLimit ); +} + +static void b3RecDispatch_RevoluteJointSetLimits( const b3RecArgs_RevoluteJointSetLimits* a, b3RecReader* rdr ) +{ + b3RevoluteJoint_SetLimits( b3RecMakeJointId( rdr, a->joint ), a->lower, a->upper ); +} + +static void b3RecDispatch_RevoluteJointEnableMotor( const b3RecArgs_RevoluteJointEnableMotor* a, b3RecReader* rdr ) +{ + b3RevoluteJoint_EnableMotor( b3RecMakeJointId( rdr, a->joint ), a->enableMotor ); +} + +static void b3RecDispatch_RevoluteJointSetMotorSpeed( const b3RecArgs_RevoluteJointSetMotorSpeed* a, b3RecReader* rdr ) +{ + b3RevoluteJoint_SetMotorSpeed( b3RecMakeJointId( rdr, a->joint ), a->motorSpeed ); +} + +static void b3RecDispatch_RevoluteJointSetMaxMotorTorque( const b3RecArgs_RevoluteJointSetMaxMotorTorque* a, b3RecReader* rdr ) +{ + b3RevoluteJoint_SetMaxMotorTorque( b3RecMakeJointId( rdr, a->joint ), a->torque ); +} + +static void b3RecDispatch_SphericalJointEnableConeLimit( const b3RecArgs_SphericalJointEnableConeLimit* a, b3RecReader* rdr ) +{ + b3SphericalJoint_EnableConeLimit( b3RecMakeJointId( rdr, a->joint ), a->enableLimit ); +} + +static void b3RecDispatch_SphericalJointSetConeLimit( const b3RecArgs_SphericalJointSetConeLimit* a, b3RecReader* rdr ) +{ + b3SphericalJoint_SetConeLimit( b3RecMakeJointId( rdr, a->joint ), a->angleRadians ); +} + +static void b3RecDispatch_SphericalJointEnableTwistLimit( const b3RecArgs_SphericalJointEnableTwistLimit* a, b3RecReader* rdr ) +{ + b3SphericalJoint_EnableTwistLimit( b3RecMakeJointId( rdr, a->joint ), a->enableLimit ); +} + +static void b3RecDispatch_SphericalJointSetTwistLimits( const b3RecArgs_SphericalJointSetTwistLimits* a, b3RecReader* rdr ) +{ + b3SphericalJoint_SetTwistLimits( b3RecMakeJointId( rdr, a->joint ), a->lower, a->upper ); +} + +static void b3RecDispatch_SphericalJointEnableSpring( const b3RecArgs_SphericalJointEnableSpring* a, b3RecReader* rdr ) +{ + b3SphericalJoint_EnableSpring( b3RecMakeJointId( rdr, a->joint ), a->enableSpring ); +} + +static void b3RecDispatch_SphericalJointSetSpringHertz( const b3RecArgs_SphericalJointSetSpringHertz* a, b3RecReader* rdr ) +{ + b3SphericalJoint_SetSpringHertz( b3RecMakeJointId( rdr, a->joint ), a->hertz ); +} + +static void b3RecDispatch_SphericalJointSetSpringDampingRatio( const b3RecArgs_SphericalJointSetSpringDampingRatio* a, + b3RecReader* rdr ) +{ + b3SphericalJoint_SetSpringDampingRatio( b3RecMakeJointId( rdr, a->joint ), a->dampingRatio ); +} + +static void b3RecDispatch_SphericalJointSetTargetRotation( const b3RecArgs_SphericalJointSetTargetRotation* a, b3RecReader* rdr ) +{ + b3SphericalJoint_SetTargetRotation( b3RecMakeJointId( rdr, a->joint ), a->targetRotation ); +} + +static void b3RecDispatch_SphericalJointEnableMotor( const b3RecArgs_SphericalJointEnableMotor* a, b3RecReader* rdr ) +{ + b3SphericalJoint_EnableMotor( b3RecMakeJointId( rdr, a->joint ), a->enableMotor ); +} + +static void b3RecDispatch_SphericalJointSetMotorVelocity( const b3RecArgs_SphericalJointSetMotorVelocity* a, b3RecReader* rdr ) +{ + b3SphericalJoint_SetMotorVelocity( b3RecMakeJointId( rdr, a->joint ), a->motorVelocity ); +} + +static void b3RecDispatch_SphericalJointSetMaxMotorTorque( const b3RecArgs_SphericalJointSetMaxMotorTorque* a, b3RecReader* rdr ) +{ + b3SphericalJoint_SetMaxMotorTorque( b3RecMakeJointId( rdr, a->joint ), a->torque ); +} + +static void b3RecDispatch_WeldJointSetLinearHertz( const b3RecArgs_WeldJointSetLinearHertz* a, b3RecReader* rdr ) +{ + b3WeldJoint_SetLinearHertz( b3RecMakeJointId( rdr, a->joint ), a->hertz ); +} + +static void b3RecDispatch_WeldJointSetLinearDampingRatio( const b3RecArgs_WeldJointSetLinearDampingRatio* a, b3RecReader* rdr ) +{ + b3WeldJoint_SetLinearDampingRatio( b3RecMakeJointId( rdr, a->joint ), a->dampingRatio ); +} + +static void b3RecDispatch_WeldJointSetAngularHertz( const b3RecArgs_WeldJointSetAngularHertz* a, b3RecReader* rdr ) +{ + b3WeldJoint_SetAngularHertz( b3RecMakeJointId( rdr, a->joint ), a->hertz ); +} + +static void b3RecDispatch_WeldJointSetAngularDampingRatio( const b3RecArgs_WeldJointSetAngularDampingRatio* a, b3RecReader* rdr ) +{ + b3WeldJoint_SetAngularDampingRatio( b3RecMakeJointId( rdr, a->joint ), a->dampingRatio ); +} + +static void b3RecDispatch_WheelJointEnableSuspension( const b3RecArgs_WheelJointEnableSuspension* a, b3RecReader* rdr ) +{ + b3WheelJoint_EnableSuspension( b3RecMakeJointId( rdr, a->joint ), a->flag ); +} + +static void b3RecDispatch_WheelJointSetSuspensionHertz( const b3RecArgs_WheelJointSetSuspensionHertz* a, b3RecReader* rdr ) +{ + b3WheelJoint_SetSuspensionHertz( b3RecMakeJointId( rdr, a->joint ), a->hertz ); +} + +static void b3RecDispatch_WheelJointSetSuspensionDampingRatio( const b3RecArgs_WheelJointSetSuspensionDampingRatio* a, + b3RecReader* rdr ) +{ + b3WheelJoint_SetSuspensionDampingRatio( b3RecMakeJointId( rdr, a->joint ), a->dampingRatio ); +} + +static void b3RecDispatch_WheelJointEnableSuspensionLimit( const b3RecArgs_WheelJointEnableSuspensionLimit* a, b3RecReader* rdr ) +{ + b3WheelJoint_EnableSuspensionLimit( b3RecMakeJointId( rdr, a->joint ), a->flag ); +} + +static void b3RecDispatch_WheelJointSetSuspensionLimits( const b3RecArgs_WheelJointSetSuspensionLimits* a, b3RecReader* rdr ) +{ + b3WheelJoint_SetSuspensionLimits( b3RecMakeJointId( rdr, a->joint ), a->lower, a->upper ); +} + +static void b3RecDispatch_WheelJointEnableSpinMotor( const b3RecArgs_WheelJointEnableSpinMotor* a, b3RecReader* rdr ) +{ + b3WheelJoint_EnableSpinMotor( b3RecMakeJointId( rdr, a->joint ), a->flag ); +} + +static void b3RecDispatch_WheelJointSetSpinMotorSpeed( const b3RecArgs_WheelJointSetSpinMotorSpeed* a, b3RecReader* rdr ) +{ + b3WheelJoint_SetSpinMotorSpeed( b3RecMakeJointId( rdr, a->joint ), a->speed ); +} + +static void b3RecDispatch_WheelJointSetMaxSpinTorque( const b3RecArgs_WheelJointSetMaxSpinTorque* a, b3RecReader* rdr ) +{ + b3WheelJoint_SetMaxSpinTorque( b3RecMakeJointId( rdr, a->joint ), a->torque ); +} + +static void b3RecDispatch_WheelJointEnableSteering( const b3RecArgs_WheelJointEnableSteering* a, b3RecReader* rdr ) +{ + b3WheelJoint_EnableSteering( b3RecMakeJointId( rdr, a->joint ), a->flag ); +} + +static void b3RecDispatch_WheelJointSetSteeringHertz( const b3RecArgs_WheelJointSetSteeringHertz* a, b3RecReader* rdr ) +{ + b3WheelJoint_SetSteeringHertz( b3RecMakeJointId( rdr, a->joint ), a->hertz ); +} + +static void b3RecDispatch_WheelJointSetSteeringDampingRatio( const b3RecArgs_WheelJointSetSteeringDampingRatio* a, + b3RecReader* rdr ) +{ + b3WheelJoint_SetSteeringDampingRatio( b3RecMakeJointId( rdr, a->joint ), a->dampingRatio ); +} + +static void b3RecDispatch_WheelJointSetMaxSteeringTorque( const b3RecArgs_WheelJointSetMaxSteeringTorque* a, b3RecReader* rdr ) +{ + b3WheelJoint_SetMaxSteeringTorque( b3RecMakeJointId( rdr, a->joint ), a->torque ); +} + +static void b3RecDispatch_WheelJointEnableSteeringLimit( const b3RecArgs_WheelJointEnableSteeringLimit* a, b3RecReader* rdr ) +{ + b3WheelJoint_EnableSteeringLimit( b3RecMakeJointId( rdr, a->joint ), a->flag ); +} + +static void b3RecDispatch_WheelJointSetSteeringLimits( const b3RecArgs_WheelJointSetSteeringLimits* a, b3RecReader* rdr ) +{ + b3WheelJoint_SetSteeringLimits( b3RecMakeJointId( rdr, a->joint ), a->lower, a->upper ); +} + +static void b3RecDispatch_WheelJointSetTargetSteeringAngle( const b3RecArgs_WheelJointSetTargetSteeringAngle* a, + b3RecReader* rdr ) +{ + b3WheelJoint_SetTargetSteeringAngle( b3RecMakeJointId( rdr, a->joint ), a->radians ); +} + +static void b3RecDispatch_StateHash( const b3RecArgs_StateHash* a, b3RecReader* rdr ) +{ + b3World* world = b3GetWorldFromId( rdr->replayWorldId ); + uint64_t computed = b3HashWorldState( world ); + if ( computed != a->hash ) + { + printf( "b3ReplayFile: StateHash mismatch (recorded=0x%" PRIx64 ", computed=0x%" PRIx64 ")\n", a->hash, computed ); + rdr->diverged = true; + } +} + +static void b3RecDispatch_RecordingBounds( const b3RecArgs_RecordingBounds* a, b3RecReader* rdr ) +{ + // Primary resolve is the open-time scan, this keeps the value right if it ever moves earlier + if ( rdr->owner != NULL ) + { + rdr->owner->bounds = a->bounds; + } +} + +// Spatial query replay. The recorded inputs come through the manifest; here the variable-length hit +// tail is read back, the query re-issued against the replay world, and each callback hit compared to +// what was recorded. Any mismatch latches rdr->diverged. When a player owns the reader the hits are +// also stashed for the viewer overlay. The stash helpers dereference the player struct, defined later +// in this file, so they are forward declared and implemented in Block B below. + +static void b3RecGrow( void** data, int* capacity, int need, int keep, int elemSize ); +static b3RecDrawQuery* b3RecStashQueryBegin( b3RecPlayer* player, int kind, const b3RecRecordedHit* hits, int hitCount ); + +// 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, so a corrupt count fails the read. +void b3RecEnsureHits( b3RecReader* rdr, int n ) +{ + b3RecReserveScratch( rdr, (void**)&rdr->hits, &rdr->hitCap, n, (int)sizeof( b3RecRecordedHit ) ); +} + +// Bitwise float compare so the determinism check is exact, not within a tolerance. +static bool b3RecF32Differs( float a, float b ) +{ + uint32_t ua, ub; + memcpy( &ua, &a, 4 ); + memcpy( &ub, &b, 4 ); + return ua != ub; +} + +static bool b3RecVec3Differs( b3Vec3 a, b3Vec3 b ) +{ + return b3RecF32Differs( a.x, b.x ) || b3RecF32Differs( a.y, b.y ) || b3RecF32Differs( a.z, b.z ); +} + +// Shared context for the replay trampolines: walks recorded hits in order, flagging any divergence +// from the re-issued query. +typedef struct b3RecReplayQueryCtx +{ + b3RecReader* rdr; + const b3RecRecordedHit* hits; + int count; + int cursor; +} b3RecReplayQueryCtx; + +static bool b3RecReplayOverlapTrampoline( b3ShapeId id, void* ctx ) +{ + b3RecReplayQueryCtx* rc = ctx; + if ( rc->cursor >= rc->count ) + { + rc->rdr->diverged = true; + return false; + } + const b3RecRecordedHit* h = &rc->hits[rc->cursor++]; + if ( id.index1 != h->id.index1 || id.generation != h->id.generation ) + { + rc->rdr->diverged = true; + } + return h->userReturnB; +} + +// The mover filter has the same bool(shapeId, ctx) shape as an overlap callback, so it replays the +// same way. A distinct typed wrapper keeps the function-pointer types clean. +static bool b3RecReplayMoverFilterTrampoline( b3ShapeId id, void* ctx ) +{ + return b3RecReplayOverlapTrampoline( id, ctx ); +} + +static float b3RecReplayCastTrampoline( b3ShapeId id, b3Pos point, b3Vec3 normal, float fraction, uint64_t userMaterialId, + int triangleIndex, int childIndex, void* ctx ) +{ + b3RecReplayQueryCtx* rc = ctx; + if ( rc->cursor >= rc->count ) + { + rc->rdr->diverged = true; + return 0.0f; + } + const b3RecRecordedHit* h = &rc->hits[rc->cursor++]; + // Positions compared through a full-width delta, truncating both sides would pass vacuously far + // from the origin. + if ( id.index1 != h->id.index1 || id.generation != h->id.generation || + b3RecVec3Differs( b3SubPos( point, h->point ), b3Vec3_zero ) || b3RecVec3Differs( normal, h->normal ) || + b3RecF32Differs( fraction, h->fraction ) || userMaterialId != h->userMaterialId || triangleIndex != h->triangleIndex || + childIndex != h->childIndex ) + { + rc->rdr->diverged = true; + } + return h->userReturnF; +} + +// 3D delivers a whole shape's planes in one call. The recorded group starts at the cursor with its +// plane count and user return replicated on each hit, so compare the batch and advance by the +// recorded count to stay aligned with the stream even when the count itself diverged. +static bool b3RecReplayPlaneTrampoline( b3ShapeId id, const b3PlaneResult* planes, int planeCount, void* ctx ) +{ + b3RecReplayQueryCtx* rc = ctx; + if ( rc->cursor >= rc->count ) + { + rc->rdr->diverged = true; + return true; + } + const b3RecRecordedHit* head = &rc->hits[rc->cursor]; + int recordedCount = head->planeCount; + bool ret = head->userReturnB; + if ( id.index1 != head->id.index1 || id.generation != head->id.generation || recordedCount != planeCount ) + { + rc->rdr->diverged = true; + } + int n = recordedCount < planeCount ? recordedCount : planeCount; + for ( int i = 0; i < n; ++i ) + { + const b3RecRecordedHit* h = &rc->hits[rc->cursor + i]; + if ( b3RecVec3Differs( h->plane.plane.normal, planes[i].plane.normal ) || + b3RecF32Differs( h->plane.plane.offset, planes[i].plane.offset ) || + b3RecVec3Differs( h->plane.point, planes[i].point ) ) + { + rc->rdr->diverged = true; + } + } + rc->cursor += recordedCount; + return ret; +} + +// Copy a decoded proxy's points into a draw record so the overlay does not depend on reader scratch. +static void b3RecStashProxy( b3RecDrawQuery* q, const b3ShapeProxy* proxy ) +{ + int count = proxy->count; + if ( count > B3_MAX_SHAPE_CAST_POINTS ) + count = B3_MAX_SHAPE_CAST_POINTS; + q->proxyCount = count; + q->proxyRadius = proxy->radius; + for ( int i = 0; i < count; ++i ) + { + q->proxyPoints[i] = proxy->points[i]; + } +} + +// Tight world-space bounds of a query's swept geometry, so the viewer can frame any query and not +// just the overlap AABB. Mover and proxy points are origin relative. A cast sweeps the shape from the +// origin to origin plus translation. The overlap AABB is already a world-space box. +static void b3RecComputeQueryBounds( b3RecDrawQuery* q ) +{ + if ( q->kind == B3_RECQ_OVERLAP_AABB ) + { + return; + } + + // Shape points relative to the origin, plus the fattening radius. A ray has no shape, so it falls + // through to a single point at the origin. + b3Vec3 local[B3_MAX_SHAPE_CAST_POINTS]; + int count = 0; + float radius = 0.0f; + switch ( q->kind ) + { + case B3_RECQ_CAST_MOVER: + case B3_RECQ_COLLIDE_MOVER: + local[0] = q->mover.center1; + local[1] = q->mover.center2; + count = 2; + radius = q->mover.radius; + break; + + case B3_RECQ_OVERLAP_SHAPE: + case B3_RECQ_CAST_SHAPE: + count = q->proxyCount; + for ( int i = 0; i < count; ++i ) + { + local[i] = q->proxyPoints[i]; + } + radius = q->proxyRadius; + break; + + default: + break; + } + if ( count == 0 ) + { + local[0] = b3Vec3_zero; + count = 1; + } + + // Sweep each point across the translation. A non-cast query has zero translation, so both ends + // coincide and the duplicates fold away. + b3Pos end = b3OffsetPos( q->origin, q->translation ); + b3Vec3 world[2 * B3_MAX_SHAPE_CAST_POINTS]; + int n = 0; + for ( int i = 0; i < count; ++i ) + { + world[n++] = b3ToVec3( b3OffsetPos( q->origin, local[i] ) ); + world[n++] = b3ToVec3( b3OffsetPos( end, local[i] ) ); + } + q->aabb = b3MakeAABB( world, n, radius ); +} + +static void b3RecDispatch_QueryOverlapAABB( const b3RecArgs_QueryOverlapAABB* a, b3RecReader* rdr ) +{ + uint32_t n = b3RecR_U32( rdr ); + b3RecEnsureHits( rdr, (int)n ); + if ( !rdr->ok ) + return; + for ( uint32_t i = 0; i < n; ++i ) + { + rdr->hits[i].id = b3RecMakeShapeId( rdr, b3RecR_SHAPEID( rdr ) ); + rdr->hits[i].userReturnB = b3RecR_BOOL( rdr ); + } + (void)b3RecR_TREESTATS( rdr ); + if ( !rdr->ok ) + return; + b3RecReplayQueryCtx rc = { rdr, rdr->hits, (int)n, 0 }; + b3World_OverlapAABB( rdr->replayWorldId, a->aabb, a->filter, b3RecReplayOverlapTrampoline, &rc ); + if ( rc.cursor != (int)n ) + rdr->diverged = true; + if ( rdr->owner ) + { + b3RecDrawQuery* q = b3RecStashQueryBegin( rdr->owner, B3_RECQ_OVERLAP_AABB, rdr->hits, (int)n ); + q->filter = a->filter; + q->aabb = a->aabb; + b3RecComputeQueryBounds( q ); + } +} + +static void b3RecDispatch_QueryOverlapShape( const b3RecArgs_QueryOverlapShape* a, b3RecReader* rdr ) +{ + uint32_t n = b3RecR_U32( rdr ); + b3RecEnsureHits( rdr, (int)n ); + if ( !rdr->ok ) + return; + for ( uint32_t i = 0; i < n; ++i ) + { + rdr->hits[i].id = b3RecMakeShapeId( rdr, b3RecR_SHAPEID( rdr ) ); + rdr->hits[i].userReturnB = b3RecR_BOOL( rdr ); + } + (void)b3RecR_TREESTATS( rdr ); + if ( !rdr->ok ) + return; + b3RecReplayQueryCtx rc = { rdr, rdr->hits, (int)n, 0 }; + b3World_OverlapShape( rdr->replayWorldId, a->origin, &a->proxy, a->filter, b3RecReplayOverlapTrampoline, &rc ); + if ( rc.cursor != (int)n ) + rdr->diverged = true; + if ( rdr->owner ) + { + b3RecDrawQuery* q = b3RecStashQueryBegin( rdr->owner, B3_RECQ_OVERLAP_SHAPE, rdr->hits, (int)n ); + q->filter = a->filter; + q->origin = a->origin; + b3RecStashProxy( q, &a->proxy ); + b3RecComputeQueryBounds( q ); + } +} + +static void b3RecDispatch_QueryCastRay( const b3RecArgs_QueryCastRay* a, b3RecReader* rdr ) +{ + uint32_t n = b3RecR_U32( rdr ); + b3RecEnsureHits( rdr, (int)n ); + if ( !rdr->ok ) + return; + for ( uint32_t i = 0; i < n; ++i ) + { + rdr->hits[i].id = b3RecMakeShapeId( rdr, b3RecR_SHAPEID( rdr ) ); + rdr->hits[i].point = b3RecR_POSITION( rdr ); + rdr->hits[i].normal = b3RecR_VEC3( rdr ); + rdr->hits[i].fraction = b3RecR_F32( rdr ); + rdr->hits[i].userMaterialId = b3RecR_U64( rdr ); + rdr->hits[i].triangleIndex = b3RecR_I32( rdr ); + rdr->hits[i].childIndex = b3RecR_I32( rdr ); + rdr->hits[i].userReturnF = b3RecR_F32( rdr ); + } + (void)b3RecR_TREESTATS( rdr ); + if ( !rdr->ok ) + return; + b3RecReplayQueryCtx rc = { rdr, rdr->hits, (int)n, 0 }; + b3World_CastRay( rdr->replayWorldId, a->origin, a->translation, a->filter, b3RecReplayCastTrampoline, &rc ); + if ( rc.cursor != (int)n ) + rdr->diverged = true; + if ( rdr->owner ) + { + b3RecDrawQuery* q = b3RecStashQueryBegin( rdr->owner, B3_RECQ_CAST_RAY, rdr->hits, (int)n ); + q->filter = a->filter; + q->origin = a->origin; + q->translation = a->translation; + b3RecComputeQueryBounds( q ); + } +} + +static void b3RecDispatch_QueryCastShape( const b3RecArgs_QueryCastShape* a, b3RecReader* rdr ) +{ + uint32_t n = b3RecR_U32( rdr ); + b3RecEnsureHits( rdr, (int)n ); + if ( !rdr->ok ) + return; + for ( uint32_t i = 0; i < n; ++i ) + { + rdr->hits[i].id = b3RecMakeShapeId( rdr, b3RecR_SHAPEID( rdr ) ); + rdr->hits[i].point = b3RecR_POSITION( rdr ); + rdr->hits[i].normal = b3RecR_VEC3( rdr ); + rdr->hits[i].fraction = b3RecR_F32( rdr ); + rdr->hits[i].userMaterialId = b3RecR_U64( rdr ); + rdr->hits[i].triangleIndex = b3RecR_I32( rdr ); + rdr->hits[i].childIndex = b3RecR_I32( rdr ); + rdr->hits[i].userReturnF = b3RecR_F32( rdr ); + } + (void)b3RecR_TREESTATS( rdr ); + if ( !rdr->ok ) + return; + b3RecReplayQueryCtx rc = { rdr, rdr->hits, (int)n, 0 }; + b3World_CastShape( rdr->replayWorldId, a->origin, &a->proxy, a->translation, a->filter, b3RecReplayCastTrampoline, &rc ); + if ( rc.cursor != (int)n ) + rdr->diverged = true; + if ( rdr->owner ) + { + b3RecDrawQuery* q = b3RecStashQueryBegin( rdr->owner, B3_RECQ_CAST_SHAPE, rdr->hits, (int)n ); + q->filter = a->filter; + q->origin = a->origin; + q->translation = a->translation; + b3RecStashProxy( q, &a->proxy ); + b3RecComputeQueryBounds( q ); + } +} + +static void b3RecDispatch_QueryCastRayClosest( const b3RecArgs_QueryCastRayClosest* a, b3RecReader* rdr ) +{ + b3RayResult rec = b3RecR_RAYRESULT( rdr ); + if ( !rdr->ok ) + return; + b3RayResult got = b3World_CastRayClosest( rdr->replayWorldId, a->origin, a->translation, a->filter ); + b3ShapeId recId = b3RecMakeShapeId( rdr, rec.shapeId ); + if ( got.hit != rec.hit || + ( got.hit && + ( got.shapeId.index1 != recId.index1 || got.shapeId.generation != recId.generation || + b3RecVec3Differs( b3SubPos( got.point, rec.point ), b3Vec3_zero ) || b3RecVec3Differs( got.normal, rec.normal ) || + b3RecF32Differs( got.fraction, rec.fraction ) || got.userMaterialId != rec.userMaterialId ) ) ) + { + rdr->diverged = true; + } + if ( rdr->owner ) + { + // Stash the closest result as a single pooled hit so the shared draw loop renders its point. + b3RecRecordedHit h = { 0 }; + h.id = recId; + h.point = rec.point; + h.normal = rec.normal; + h.fraction = rec.fraction; + b3RecDrawQuery* q = b3RecStashQueryBegin( rdr->owner, B3_RECQ_CAST_RAY_CLOSEST, &h, rec.hit ? 1 : 0 ); + q->filter = a->filter; + q->origin = a->origin; + q->translation = a->translation; + q->rayResult = rec; + b3RecComputeQueryBounds( q ); + } +} + +static void b3RecDispatch_QueryCastMover( const b3RecArgs_QueryCastMover* a, b3RecReader* rdr ) +{ + uint32_t n = b3RecR_U32( rdr ); + b3RecEnsureHits( rdr, (int)n ); + if ( !rdr->ok ) + return; + for ( uint32_t i = 0; i < n; ++i ) + { + rdr->hits[i].id = b3RecMakeShapeId( rdr, b3RecR_SHAPEID( rdr ) ); + rdr->hits[i].userReturnB = b3RecR_BOOL( rdr ); + } + float recFraction = b3RecR_F32( rdr ); + if ( !rdr->ok ) + return; + b3RecReplayQueryCtx rc = { rdr, rdr->hits, (int)n, 0 }; + float got = b3World_CastMover( rdr->replayWorldId, a->origin, &a->mover, a->translation, a->filter, + b3RecReplayMoverFilterTrampoline, &rc ); + if ( rc.cursor != (int)n || b3RecF32Differs( got, recFraction ) ) + rdr->diverged = true; + if ( rdr->owner ) + { + b3RecDrawQuery* q = b3RecStashQueryBegin( rdr->owner, B3_RECQ_CAST_MOVER, NULL, 0 ); + q->filter = a->filter; + q->origin = a->origin; + q->mover = a->mover; + q->translation = a->translation; + q->castFraction = recFraction; + b3RecComputeQueryBounds( q ); + } +} + +static void b3RecDispatch_QueryCollideMover( const b3RecArgs_QueryCollideMover* a, b3RecReader* rdr ) +{ + // Recorded as shapeCount groups, each: shapeId, planeCount, planeCount planes, user return. Flatten + // into one hit per plane with the group's count and return replicated, so the replay walker can + // re-group per shape. + uint32_t shapeCount = b3RecR_U32( rdr ); + int total = 0; + for ( uint32_t s = 0; s < shapeCount; ++s ) + { + b3ShapeId id = b3RecMakeShapeId( rdr, b3RecR_SHAPEID( rdr ) ); + int planeCount = b3RecR_I32( rdr ); + if ( planeCount < 0 ) + planeCount = 0; + b3RecEnsureHits( rdr, total + planeCount ); + if ( !rdr->ok ) + return; + for ( int i = 0; i < planeCount; ++i ) + { + rdr->hits[total + i].plane = b3RecR_PLANERESULT( rdr ); + } + bool ret = b3RecR_BOOL( rdr ); + for ( int i = 0; i < planeCount; ++i ) + { + rdr->hits[total + i].id = id; + rdr->hits[total + i].planeCount = planeCount; + rdr->hits[total + i].userReturnB = ret; + } + total += planeCount; + } + if ( !rdr->ok ) + return; + b3RecReplayQueryCtx rc = { rdr, rdr->hits, total, 0 }; + b3World_CollideMover( rdr->replayWorldId, a->origin, &a->mover, a->filter, b3RecReplayPlaneTrampoline, &rc ); + if ( rc.cursor != total ) + rdr->diverged = true; + if ( rdr->owner ) + { + b3RecDrawQuery* q = b3RecStashQueryBegin( rdr->owner, B3_RECQ_COLLIDE_MOVER, rdr->hits, total ); + q->filter = a->filter; + q->origin = a->origin; + q->mover = a->mover; + b3RecComputeQueryBounds( q ); + } +} + +// Stash the identity key of the query that immediately follows. Consumed by the next stash. +static void b3RecDispatch_QueryTag( const b3RecArgs_QueryTag* a, b3RecReader* rdr ) +{ + rdr->pendingQueryKey = a->key; +} + +// X-macro dispatch switch: read opcode+u24 payloadSize, dispatch, skip unknown ops. +// Returns the opcode dispatched, or -1 when the stream is exhausted or broken. + +static int b3RecDispatchOne( b3RecReader* rdr ) +{ + if ( rdr->cursor >= rdr->size || !rdr->ok ) + { + return -1; + } + uint8_t opcode = b3RecR_U8( rdr ); + uint32_t payloadSize = b3RecR_U24( rdr ); + if ( !rdr->ok ) + { + return -1; + } + + int payloadStart = rdr->cursor; + + switch ( opcode ) + { +#define ARG( TAG, field ) a.field = b3RecR_##TAG( rdr ); +#define B3_REC_OP( op, Name, RET, ... ) \ + case op: \ + { \ + b3RecArgs_##Name a; \ + memset( &a, 0, sizeof( a ) ); \ + __VA_ARGS__ \ + if ( rdr->ok ) \ + { \ + b3RecDispatch_##Name( &a, rdr ); \ + } \ + break; \ + } +#include "recording_ops.inl" +#undef B3_REC_OP +#undef ARG + default: + printf( "b3ReplayFile: unknown opcode 0x%02X, skipping %u bytes\n", opcode, payloadSize ); + // payloadStart is in bounds, so size - payloadStart is the bytes left to skip over + if ( payloadSize > (uint32_t)( rdr->size - payloadStart ) ) + { + rdr->ok = false; + } + else + { + rdr->cursor = payloadStart + (int)payloadSize; + } + break; + } + return (int)(unsigned)opcode; +} + +// Public entry point + +bool b3ValidateReplay( const void* data, int size, int workerCount ) +{ + b3RecPlayer* player = b3RecPlayer_Create( data, size, workerCount ); + if ( player == NULL ) + { + return false; + } + + while ( b3RecPlayer_StepFrame( player ) ) + { + if ( player->rdr.diverged ) + { + break; + } + } + + bool ok = player->rdr.ok && player->rdr.diverged == false; + b3RecPlayer_Destroy( player ); + return ok; +} + +// b3RecPlayer implementation + +#define B3_REC_KEYFRAME_INTERVAL_DEFAULT 16 +#define B3_REC_KEYFRAME_BUDGET_DEFAULT ( (size_t)512 * 1024 * 1024 ) + +// Overflow-safe growth for the player's accumulating arrays. Counts come from the replay itself, +// not the file, so this only guards the byte-size multiply. Preserves keep elements. +static void b3RecGrow( void** data, int* capacity, int need, int keep, int elemSize ) +{ + if ( need <= *capacity ) + { + return; + } + int newCap = *capacity == 0 ? 8 : 2 * *capacity; + if ( newCap < need ) + { + newCap = need; + } + void* grown = b3Alloc( (size_t)newCap * (size_t)elemSize ); + if ( *data != NULL ) + { + if ( keep > 0 ) + { + memcpy( grown, *data, (size_t)keep * (size_t)elemSize ); + } + b3Free( *data, (size_t)*capacity * (size_t)elemSize ); + } + *data = grown; + *capacity = newCap; +} + +// Block B: per-frame query store helpers. Forward declared above the query dispatchers, which run +// before the player struct is defined, so the player-dereferencing code lives here. + +static void b3RecGrowFrameQueries( b3RecPlayer* player ) +{ + b3RecGrow( (void**)&player->frameQueries, &player->frameQueryCap, player->frameQueryCount + 1, player->frameQueryCount, + (int)sizeof( b3RecDrawQuery ) ); +} + +static void b3RecGrowFrameHits( b3RecPlayer* player, int need ) +{ + b3RecGrow( (void**)&player->frameHits, &player->frameHitCap, player->frameHitCount + need, player->frameHitCount, + (int)sizeof( b3RecRecordedHit ) ); +} + +// Push a draw record for one query and copy its hits into the per-frame store. Ids in hits[] are +// already remapped to the replay world by the dispatcher. +static b3RecDrawQuery* b3RecStashQueryBegin( b3RecPlayer* player, int kind, const b3RecRecordedHit* hits, int hitCount ) +{ + b3RecGrowFrameQueries( player ); + b3RecDrawQuery* q = &player->frameQueries[player->frameQueryCount]; + memset( q, 0, sizeof( *q ) ); + q->kind = kind; + // Pair the query with the key from its preceding QueryTag op, if any, then clear it so the next + // untagged query reads 0. + q->key = player->rdr.pendingQueryKey; + player->rdr.pendingQueryKey = 0; + q->hitStart = player->frameHitCount; + q->hitCount = hitCount; + b3RecGrowFrameHits( player, hitCount ); + for ( int i = 0; i < hitCount; ++i ) + { + player->frameHits[player->frameHitCount + i] = hits[i]; + } + player->frameHitCount += hitCount; + player->frameQueryCount++; + return q; +} + +// Append a created body to the outliner list. Ordinals are creation order and never reused. +static void b3RecTrackBodyCreate( b3RecPlayer* player, b3BodyId id ) +{ + b3RecGrow( (void**)&player->bodyIds, &player->bodyIdCap, player->bodyIdCount + 1, player->bodyIdCount, + (int)sizeof( b3BodyId ) ); + player->bodyIds[player->bodyIdCount] = id; + player->bodyIdCount += 1; +} + +// Leave a hole so later ordinals do not shift, keeping a stored selection stable. +static void b3RecTrackBodyDestroy( b3RecPlayer* player, b3BodyId id ) +{ + for ( int i = 0; i < player->bodyIdCount; ++i ) + { + if ( B3_ID_EQUALS( player->bodyIds[i], id ) ) + { + player->bodyIds[i] = b3_nullBodyId; + return; + } + } +} + +// Snapshot bodies are restored as a struct image and never hit the CreateBody hook the tracker keys +// on, so the seed world must be walked once to populate the outliner list. Slot order is stable. +static void b3RecSeedBodyIds( b3RecPlayer* player ) +{ + b3World* world = b3GetWorldFromId( player->rdr.replayWorldId ); + player->bodyIdCount = 0; + int count = world->bodies.count; + for ( int i = 0; i < count; ++i ) + { + if ( world->bodies.data[i].id != i ) + { + continue; // free slot + } + b3RecTrackBodyCreate( player, b3MakeBodyId( world, i ) ); + } +} + +// Seed the outliner list from the current world and save it as the frame-0 restore copy. +static void b3RecSeedFrame0BodyIds( b3RecPlayer* player ) +{ + b3RecSeedBodyIds( player ); + if ( player->frame0BodyIds != NULL ) + { + b3Free( player->frame0BodyIds, (size_t)player->frame0BodyIdCount * sizeof( b3BodyId ) ); + player->frame0BodyIds = NULL; + } + player->frame0BodyIdCount = player->bodyIdCount; + if ( player->bodyIdCount > 0 ) + { + player->frame0BodyIds = (b3BodyId*)b3Alloc( (size_t)player->bodyIdCount * sizeof( b3BodyId ) ); + memcpy( player->frame0BodyIds, player->bodyIds, (size_t)player->bodyIdCount * sizeof( b3BodyId ) ); + } +} + +// Tag key to tag index, so the viewer resolves a query's caller id and label in O(1) instead of a +// linear scan over the tag table. +#define NAME b3RecTagLookup +#define KEY_TY uint64_t +#define VAL_TY uint32_t +#define HASH_FN vt_hash_integer +#define CMPR_FN vt_cmpr_integer +#define MALLOC_FN b3Alloc +#define FREE_FN b3Free +#include "verstable.h" + +// Read the optional query-tag table trailing the geometry entries: u32 tagCount then per tag +// { u64 key, u64 id, u16 len, name bytes }. A recording written before the tag table leaves rp at +// dataEnd, so nothing loads. Bounds-checked; tagCount reflects only the tags that fully fit, so a +// truncated tail loads what it can and reports the real count. +static void b3RecLoadTags( b3RecReader* rdr, const uint8_t* rp, const uint8_t* dataEnd ) +{ + b3RecReader sub = { 0 }; + sub.data = rp; + sub.size = (int)( dataEnd - rp ); + sub.ok = true; + + uint32_t count = b3RecR_U32( &sub ); + if ( sub.ok == false || count == 0 ) + { + return; + } + + // Each tag is at least 18 bytes (8 key + 8 id + 2 length). Reject a count that cannot fit the + // remaining bytes so a corrupt table cannot request a wild allocation. + if ( (size_t)count > (size_t)( sub.size - sub.cursor ) / 18 ) + { + return; + } + + b3RecTag* tags = (b3RecTag*)b3Alloc( (size_t)count * sizeof( b3RecTag ) ); + memset( tags, 0, (size_t)count * sizeof( b3RecTag ) ); + + b3RecTagLookup* map = (b3RecTagLookup*)b3Alloc( sizeof( b3RecTagLookup ) ); + b3RecTagLookup_init( map ); + + uint32_t loaded = 0; + for ( uint32_t i = 0; i < count; ++i ) + { + uint64_t key = b3RecR_U64( &sub ); + uint64_t id = b3RecR_U64( &sub ); + uint16_t len = b3RecR_U16( &sub ); + if ( sub.ok == false ) + { + break; + } + if ( len == 0xFFFFu ) + { + len = 0; // a null name is written as 0xFFFF + } + if ( (int64_t)sub.cursor + (int64_t)len > (int64_t)sub.size ) + { + break; + } + int n = len > B3_MAX_QUERY_NAME_LENGTH ? B3_MAX_QUERY_NAME_LENGTH : (int)len; + tags[loaded].key = key; + tags[loaded].id = id; + if ( n > 0 ) + { + memcpy( tags[loaded].queryName, sub.data + sub.cursor, (size_t)n ); + } + tags[loaded].queryName[n] = '\0'; + sub.cursor += len; + b3RecTagLookup_insert( map, key, loaded ); + loaded += 1; + } + + rdr->tags = tags; + rdr->tagCount = (int)loaded; + rdr->tagCapacity = (int)count; + rdr->tagMap = map; +} + +// Load the trailing registry block and fill rdr->slots/slotCount, then the optional tag table. +// Returns true on success. On failure sets rdr->ok = false and returns false. +static bool b3RecLoadSlots( b3RecReader* rdr, const void* data, int size, uint64_t registryOffset, uint64_t registryByteCount ) +{ + if ( registryOffset == 0 || registryByteCount == 0 ) + { + rdr->slots = NULL; + rdr->slotCount = 0; + return true; + } + + int regStart = (int)registryOffset; + int regEnd = regStart + (int)registryByteCount; + if ( regEnd > size ) + { + printf( "b3ReplayFile: registry block out of bounds\n" ); + return false; + } + if ( regStart + 4 > size ) + { + printf( "b3ReplayFile: registry too small\n" ); + return false; + } + + const uint8_t* dataEnd = (const uint8_t*)data + regEnd; + const uint8_t* rp = (const uint8_t*)data + regStart; + uint32_t count = (uint32_t)rp[0] | ( (uint32_t)rp[1] << 8 ) | ( (uint32_t)rp[2] << 16 ) | ( (uint32_t)rp[3] << 24 ); + rp += 4; + + if ( count == 0 ) + { + rdr->slots = NULL; + rdr->slotCount = 0; + b3RecLoadTags( rdr, rp, dataEnd ); + return true; + } + + // Each entry is at least 5 bytes (kind + 4-byte length). A count that cannot fit the remaining + // registry bytes is a corrupt header, so reject it before allocating. + if ( rp > dataEnd || (size_t)count > (size_t)( dataEnd - rp ) / 5 ) + { + printf( "b3ReplayFile: registry count out of range\n" ); + return false; + } + + b3RegistrySlot* slots = (b3RegistrySlot*)b3Alloc( (size_t)count * sizeof( b3RegistrySlot ) ); + memset( slots, 0, (size_t)count * sizeof( b3RegistrySlot ) ); + + for ( uint32_t i = 0; i < count; ++i ) + { + if ( rp + 5 > dataEnd ) + { + printf( "b3ReplayFile: registry truncated at entry %u\n", i ); + for ( uint32_t j = 0; j < i; ++j ) + { + if ( slots[j].bytes != NULL ) + { + b3Free( slots[j].bytes, (size_t)slots[j].byteCount ); + } + } + b3Free( slots, (size_t)count * sizeof( b3RegistrySlot ) ); + return false; + } + uint8_t kind = rp[0]; + uint32_t byteCount = (uint32_t)rp[1] | ( (uint32_t)rp[2] << 8 ) | ( (uint32_t)rp[3] << 16 ) | ( (uint32_t)rp[4] << 24 ); + rp += 5; + if ( rp + byteCount > dataEnd ) + { + printf( "b3ReplayFile: registry entry %u bytes out of bounds\n", i ); + for ( uint32_t j = 0; j < i; ++j ) + { + if ( slots[j].bytes != NULL ) + { + b3Free( slots[j].bytes, (size_t)slots[j].byteCount ); + } + } + b3Free( slots, (size_t)count * sizeof( b3RegistrySlot ) ); + return false; + } + uint8_t* bytes = (uint8_t*)b3Alloc( byteCount > 0 ? (size_t)byteCount : 1u ); + if ( byteCount > 0 ) + { + memcpy( bytes, rp, (size_t)byteCount ); + } + rp += byteCount; + slots[i].kind = (b3GeometryKind)kind; + slots[i].byteCount = (int)byteCount; + slots[i].bytes = bytes; + slots[i].live = NULL; + } + + rdr->slots = slots; + rdr->slotCount = (int)count; + b3RecLoadTags( rdr, rp, dataEnd ); + return true; +} + +// Free slots loaded by b3RecLoadSlots. +static void b3RecFreeSlots( b3RegistrySlot* slots, int slotCount ) +{ + if ( slots == NULL ) + { + return; + } + for ( int i = 0; i < slotCount; ++i ) + { + b3RegistrySlot* slot = slots + i; + if ( slot->live != NULL ) + { + switch ( slot->kind ) + { + // Mesh and height field have no separate live object; they borrow the bytes freed below. + case b3_geometryCompound: + b3Free( slot->live, (size_t)slot->byteCount ); + break; + default: + break; + } + } + if ( slot->bytes != NULL ) + { + b3Free( slot->bytes, slot->byteCount > 0 ? (size_t)slot->byteCount : 1u ); + } + } + b3Free( slots, (size_t)slotCount * sizeof( b3RegistrySlot ) ); +} + +// Walk the op stream once without dispatching: count Step ops and grab the first step's tuning. +static void b3RecScanFile( b3RecPlayer* player ) +{ + const uint8_t* data = player->data; + int size = player->registryEnd; + int cursor = player->headerEnd; + int frameCount = 0; + bool gotStep = false; + + while ( cursor + 4 <= size ) + { + uint8_t opcode = data[cursor]; + uint32_t payloadSize = + (uint32_t)data[cursor + 1] | ( (uint32_t)data[cursor + 2] << 8 ) | ( (uint32_t)data[cursor + 3] << 16 ); + int payloadStart = cursor + 4; + if ( payloadStart + (int)payloadSize > size ) + { + break; + } + if ( opcode == b3_recOpStep ) + { + frameCount += 1; + if ( !gotStep && payloadSize >= 12 ) + { + uint32_t dtBits = (uint32_t)data[payloadStart + 4] | ( (uint32_t)data[payloadStart + 5] << 8 ) | + ( (uint32_t)data[payloadStart + 6] << 16 ) | ( (uint32_t)data[payloadStart + 7] << 24 ); + memcpy( &player->recordedDt, &dtBits, 4 ); + player->recordedSubStepCount = + (int)( (uint32_t)data[payloadStart + 8] | ( (uint32_t)data[payloadStart + 9] << 8 ) | + ( (uint32_t)data[payloadStart + 10] << 16 ) | ( (uint32_t)data[payloadStart + 11] << 24 ) ); + gotStep = true; + } + } + else if ( opcode == 0xF2 && payloadSize >= (uint32_t)sizeof( b3AABB ) ) // RecordingBounds + { + // Payload is a single b3AABB (lower xyz, upper xyz as f32), written at stop so the + // viewer can frame the whole recorded motion without playing to the end. + memcpy( &player->bounds, data + payloadStart, sizeof( b3AABB ) ); + } + cursor = payloadStart + (int)payloadSize; + } + player->frameCount = frameCount; +} + +// Free one keyframe's heap. +static void b3FreeKeyframe( b3RecKeyframe* kf ) +{ + if ( kf->image != NULL ) + { + b3Free( kf->image, (size_t)kf->imageCapacity ); + } + if ( kf->bodyIds != NULL ) + { + b3Free( kf->bodyIds, (size_t)kf->bodyIdCount * sizeof( b3BodyId ) ); + } +} + +// Pre-populate keyframeRec's registry to mirror rdr.slots so geometry ids stay stable during +// b3SerializeWorld. Each slot becomes one entry with id == slot index, even byte-identical slots that +// a hash collision left undeduplicated in an already-recorded file. b3SerializeWorld then resolves a +// live blob back to a valid slot index via the registry's exact dedup, so capture never grows it. +static void b3RecSeedKeyframeRegistry( b3RecPlayer* player ) +{ + b3GeometryRegistry* reg = &player->keyframeRec->registry; + for ( int i = 0; i < player->rdr.slotCount; ++i ) + { + b3RegistrySlot* slot = player->rdr.slots + i; + // Copy so the registry can take ownership. + int n = slot->byteCount > 0 ? slot->byteCount : 1; + uint8_t* copy = (uint8_t*)b3Alloc( (size_t)n ); + if ( slot->byteCount > 0 ) + { + memcpy( copy, slot->bytes, (size_t)slot->byteCount ); + } + uint64_t h = b3Hash64Blob( slot->bytes, slot->byteCount ); + uint32_t id = b3AppendGeometry( reg, slot->kind, h, copy, slot->byteCount ); + // Seeding in order without dedup keeps id == slot index. + B3_ASSERT( id == (uint32_t)i ); + (void)id; + } +} + +// Capture a restore-point keyframe for the just-completed frame. rdr.cursor already points to +// the next frame's Step op. +static void b3RecCaptureKeyframe( b3RecPlayer* player ) +{ + b3World* world = b3GetWorldFromId( player->rdr.replayWorldId ); + b3RecBuffer buf = { 0 }; + + int regCountBefore = player->keyframeRec->registry.count; + B3_UNUSED( regCountBefore ); + + b3SerializeWorld( world, &buf, player->keyframeRec ); + // Registry must not grow: all geometry was pre-seeded and the registry dedups exactly. + B3_ASSERT( player->keyframeRec->registry.count == regCountBefore ); + + size_t bodyBytes = (size_t)player->bodyIdCount * sizeof( b3BodyId ); + size_t newBytes = (size_t)buf.capacity + bodyBytes; + + // Make room under the budget by doubling the spacing and evicting off-grid keyframes. + while ( player->keyframeCount > 0 && player->keyframeBytes + newBytes > player->keyframeBudget ) + { + player->keyframeInterval *= 2; + int kept = 0; + size_t keptBytes = 0; + for ( int i = 0; i < player->keyframeCount; ++i ) + { + b3RecKeyframe* kf = player->keyframes + i; + if ( kf->frame % player->keyframeInterval == 0 ) + { + player->keyframes[kept] = *kf; + keptBytes += (size_t)kf->imageCapacity + (size_t)kf->bodyIdCount * sizeof( b3BodyId ); + kept += 1; + } + else + { + b3FreeKeyframe( kf ); + } + } + bool progress = ( kept < player->keyframeCount ); + player->keyframeCount = kept; + player->keyframeBytes = keptBytes; + if ( !progress ) + { + break; + } + } + + // Grow the keyframe ring if needed. + if ( player->keyframeCount >= player->keyframeCapacity ) + { + int newCap = player->keyframeCapacity < 8 ? 8 : player->keyframeCapacity * 2; + player->keyframes = (b3RecKeyframe*)b3GrowAlloc( + player->keyframes, player->keyframeCapacity * (int)sizeof( b3RecKeyframe ), newCap * (int)sizeof( b3RecKeyframe ) ); + player->keyframeCapacity = newCap; + } + + b3RecKeyframe* kf = player->keyframes + player->keyframeCount; + kf->image = buf.data; + kf->imageSize = buf.size; + kf->imageCapacity = buf.capacity; + kf->frame = player->frame; + kf->cursor = player->rdr.cursor; + kf->divergeFrame = player->divergeFrame; + kf->diverged = player->rdr.diverged; + kf->bodyIdCount = player->bodyIdCount; + kf->bodyIds = NULL; + if ( bodyBytes > 0 ) + { + kf->bodyIds = (b3BodyId*)b3Alloc( bodyBytes ); + memcpy( kf->bodyIds, player->bodyIds, bodyBytes ); + } + + player->keyframeBytes += newBytes; + player->keyframeCount += 1; + player->lastKeyframeFrame = player->frame; +} + +// Restore the world in-place from a keyframe image. +static void b3RecPlayerRestoreKeyframe( b3RecPlayer* player, const b3RecKeyframe* kf ) +{ + b3World* world = b3GetWorldFromId( player->rdr.replayWorldId ); + if ( b3DeserializeIntoShell( kf->image, kf->imageSize, world, &player->rdr ) == false ) + { + player->rdr.ok = false; + return; + } + player->rdr.cursor = kf->cursor; + player->rdr.ok = true; + player->rdr.diverged = kf->diverged; + player->frame = kf->frame; + player->divergeFrame = kf->divergeFrame; + player->atEnd = false; + player->atPreStep = false; + + // Restore the outliner list verbatim so ordinals match this frame. + b3RecGrow( (void**)&player->bodyIds, &player->bodyIdCap, kf->bodyIdCount, 0, (int)sizeof( b3BodyId ) ); + player->bodyIdCount = kf->bodyIdCount; + if ( kf->bodyIdCount > 0 ) + { + memcpy( player->bodyIds, kf->bodyIds, (size_t)kf->bodyIdCount * sizeof( b3BodyId ) ); + } +} + +// Create a replay world carrying the host debug-shape callbacks. Every world the player +// stands up funnels through here so the sample renderer can draw replayed shapes. +static b3WorldId b3RecPlayerCreateWorld( const b3RecPlayer* player ) +{ + b3WorldDef worldDef = b3DefaultWorldDef(); + worldDef.createDebugShape = player->createDebugShape; + worldDef.destroyDebugShape = player->destroyDebugShape; + worldDef.userDebugShapeContext = player->debugShapeContext; + // Carry the requested worker count so a rebuild on Restart or backward seek keeps the same + // graph partitioning. Replaying at a different count than recorded is a determinism check. + worldDef.workerCount = b3MaxInt( 1, player->recordedWorkerCount ); + return b3CreateWorld( &worldDef ); +} + +b3RecPlayer* b3RecPlayer_Create( const void* data, int size, int workerCount ) +{ + if ( data == NULL || size < (int)sizeof( b3RecHeader ) ) + { + printf( "b3RecPlayer_Create: recording too small\n" ); + return NULL; + } + + b3RecHeader hdr; + memcpy( &hdr, data, sizeof( hdr ) ); + + if ( hdr.magic != B3_REC_MAGIC ) + { + printf( "b3RecPlayer_Create: bad magic 0x%08X\n", hdr.magic ); + return NULL; + } + // Only the major version is breaking. Minor bumps are additive op-stream changes that keep the + // header shape, and the dispatcher skips opcodes it doesn't know, so a minor mismatch still loads. + if ( hdr.versionMajor != B3_REC_VERSION_MAJOR ) + { + printf( "b3RecPlayer_Create: version mismatch %u.%u vs %u.%u\n", hdr.versionMajor, hdr.versionMinor, B3_REC_VERSION_MAJOR, + B3_REC_VERSION_MINOR ); + return NULL; + } + if ( hdr.pointerWidth != (uint8_t)sizeof( void* ) ) + { + printf( "b3RecPlayer_Create: pointer width mismatch %u vs %u\n", hdr.pointerWidth, (unsigned)sizeof( void* ) ); + return NULL; + } + if ( hdr.bigEndian != 0 ) + { + printf( "b3RecPlayer_Create: big-endian recording not supported\n" ); + return NULL; + } + + // Every recording is snapshot-seeded: the seed blob sits between the header and the op stream. + if ( hdr.snapshotSize == 0 ) + { + printf( "b3RecPlayer_Create: missing snapshot seed\n" ); + return NULL; + } + + // snapshotSize and registryOffset are 64-bit and come from the file. Validate in 64-bit so a + // hostile value can't wrap when narrowed to int, then narrow once the bounds are known good. + uint64_t headerEnd64 = (uint64_t)sizeof( b3RecHeader ) + hdr.snapshotSize; + uint64_t registryEnd64 = ( hdr.registryOffset != 0 ) ? hdr.registryOffset : (uint64_t)size; + + if ( headerEnd64 < sizeof( b3RecHeader ) || headerEnd64 > registryEnd64 || registryEnd64 > (uint64_t)size ) + { + printf( "b3RecPlayer_Create: corrupt offsets\n" ); + return NULL; + } + + int headerEnd = (int)headerEnd64; + int registryEnd = (int)registryEnd64; + + // Own a private copy so the caller can free their buffer right away. + uint8_t* copy = b3Alloc( (size_t)size ); + memcpy( copy, data, (size_t)size ); + + b3RecPlayer* player = b3Alloc( sizeof( b3RecPlayer ) ); + memset( player, 0, sizeof( b3RecPlayer ) ); + + player->data = copy; + player->size = size; + player->headerEnd = headerEnd; + player->registryEnd = registryEnd; + player->lengthScale = hdr.lengthScale; + player->previousLengthScale = b3GetLengthUnitsPerMeter(); + player->frame = 0; + player->frameCount = 0; + player->recordedDt = 0.0f; + player->recordedSubStepCount = 0; + player->recordedWorkerCount = workerCount; + player->atEnd = false; + player->atPreStep = false; + player->divergeFrame = -1; + player->keyframeMinInterval = B3_REC_KEYFRAME_INTERVAL_DEFAULT; + player->keyframeInterval = B3_REC_KEYFRAME_INTERVAL_DEFAULT; + player->keyframeBudget = B3_REC_KEYFRAME_BUDGET_DEFAULT; + player->lastKeyframeFrame = 0; + + // Set length scale so replay reproduces the same tuning constants. + if ( hdr.lengthScale > 0.0f ) + { + b3SetLengthUnitsPerMeter( hdr.lengthScale ); + } + + // Count frames and read first step's dt so the viewer can show hz up front. + b3RecScanFile( player ); + + // Create the replay world. Debug-shape callbacks are NULL here; the sample wires + // them right after Create via b3RecPlayer_SetDebugShapeCallbacks, which rebuilds. + b3WorldId worldId = b3RecPlayerCreateWorld( player ); + + // Initialize the reader. + player->rdr.data = copy; + player->rdr.size = size; + player->rdr.cursor = headerEnd; + player->rdr.replayWorldId = worldId; + player->rdr.ok = true; + player->rdr.diverged = false; + player->rdr.owner = player; + + // Load the trailing geometry registry. + if ( !b3RecLoadSlots( &player->rdr, copy, size, hdr.registryOffset, hdr.registryByteCount ) ) + { + b3DestroyWorld( worldId ); + b3Free( copy, (size_t)size ); + b3Free( player, sizeof( b3RecPlayer ) ); + return NULL; + } + + // Restore the seed snapshot to stand up the replay world. The blob doubles as the frame-0 + // restore image, owned by the copy held above. + { + int snapStart = (int)sizeof( b3RecHeader ); + int snapSize = (int)hdr.snapshotSize; + b3World* replayWorld = b3GetWorldFromId( worldId ); + if ( b3DeserializeIntoShell( copy + snapStart, snapSize, replayWorld, &player->rdr ) == false ) + { + printf( "b3RecPlayer_Create: snapshot deserialization failed\n" ); + b3DestroyWorld( worldId ); + b3RecFreeSlots( player->rdr.slots, player->rdr.slotCount ); + if ( player->rdr.tags != NULL ) + { + b3Free( player->rdr.tags, (size_t)player->rdr.tagCapacity * sizeof( b3RecTag ) ); + } + if ( player->rdr.tagMap != NULL ) + { + b3RecTagLookup_cleanup( (b3RecTagLookup*)player->rdr.tagMap ); + b3Free( player->rdr.tagMap, sizeof( b3RecTagLookup ) ); + } + b3Free( copy, (size_t)size ); + b3Free( player, sizeof( b3RecPlayer ) ); + return NULL; + } + player->rdr.cursor = headerEnd; + player->frame0Image = copy + snapStart; + player->frame0Size = snapSize; + } + + // Seed the outliner from the restored world (snapshot bodies bypass the create hook) and save + // the frame-0 restore copy. + b3RecSeedFrame0BodyIds( player ); + + // Build the keyframe recording with a pre-seeded registry that mirrors rdr.slots, + // so b3SerializeWorld geometry ids stay stable across captures. + player->keyframeRec = b3CreateRecording( 0 ); + b3RecSeedKeyframeRegistry( player ); + + return player; +} + +void b3RecPlayer_Destroy( b3RecPlayer* player ) +{ + if ( player == NULL ) + { + return; + } + + if ( b3World_IsValid( player->rdr.replayWorldId ) ) + { + b3DestroyWorld( player->rdr.replayWorldId ); + } + + // Free live geometry after destroying the world (slot->live may be used by the world). + b3RecFreeSlots( player->rdr.slots, player->rdr.slotCount ); + + // Free reader scratch. + if ( player->rdr.matScratch != NULL ) + { + b3Free( player->rdr.matScratch, (size_t)player->rdr.matScratchCap * sizeof( b3SurfaceMaterial ) ); + } + if ( player->rdr.proxyScratch != NULL ) + { + b3Free( player->rdr.proxyScratch, (size_t)player->rdr.proxyScratchCap * sizeof( b3Vec3 ) ); + } + if ( player->rdr.hits != NULL ) + { + b3Free( player->rdr.hits, (size_t)player->rdr.hitCap * sizeof( b3RecRecordedHit ) ); + } + if ( player->rdr.tags != NULL ) + { + b3Free( player->rdr.tags, (size_t)player->rdr.tagCapacity * sizeof( b3RecTag ) ); + } + if ( player->rdr.tagMap != NULL ) + { + b3RecTagLookup_cleanup( (b3RecTagLookup*)player->rdr.tagMap ); + b3Free( player->rdr.tagMap, sizeof( b3RecTagLookup ) ); + } + + // Free the per-frame query store. + if ( player->frameQueries != NULL ) + { + b3Free( player->frameQueries, (size_t)player->frameQueryCap * sizeof( b3RecDrawQuery ) ); + } + if ( player->frameHits != NULL ) + { + b3Free( player->frameHits, (size_t)player->frameHitCap * sizeof( b3RecRecordedHit ) ); + } + + // Free keyframe ring. + for ( int i = 0; i < player->keyframeCount; ++i ) + { + b3FreeKeyframe( player->keyframes + i ); + } + if ( player->keyframes != NULL ) + { + b3Free( player->keyframes, (size_t)player->keyframeCapacity * sizeof( b3RecKeyframe ) ); + } + + // The keyframe recording owns only its buffer and registry; b3DestroyRecording frees both. + if ( player->keyframeRec != NULL ) + { + b3DestroyRecording( player->keyframeRec ); + } + + // Free the outliner body lists. + if ( player->bodyIds != NULL ) + { + b3Free( player->bodyIds, (size_t)player->bodyIdCap * sizeof( b3BodyId ) ); + } + if ( player->frame0BodyIds != NULL ) + { + b3Free( player->frame0BodyIds, (size_t)player->frame0BodyIdCount * sizeof( b3BodyId ) ); + } + + // frame0Image points into the owned data copy, not separately allocated. + + b3Free( player->data, (size_t)player->size ); + + // Restore the global length scale. + b3SetLengthUnitsPerMeter( player->previousLengthScale ); + + b3Free( player, sizeof( b3RecPlayer ) ); +} + +bool b3RecPlayer_StepFrame( b3RecPlayer* player ) +{ + // This is never true when full stepping + player->atPreStep = false; + + if ( player->atEnd ) + { + return false; + } + + // Reset the per-frame query store before this frame's records are dispatched. + player->frameQueryCount = 0; + player->frameHitCount = 0; + + // A frame is its leading inputs (queries and between-step mutators), one Step, and the Step's + // trailing StateHash. Queries are recorded before the Step they belong to, so they stash here + // against the world state they were computed for. + bool stepped = false; + for ( ;; ) + { + if ( player->rdr.cursor >= player->registryEnd || !player->rdr.ok ) + { + player->atEnd = true; + return stepped; + } + + // Once stepped, the StateHash is the only record still belonging to this frame. Anything else + // begins the next frame, so stop and let the next StepFrame consume it. Capture a keyframe at + // the boundary. + if ( stepped && player->rdr.data[player->rdr.cursor] != b3_recOpStateHash ) + { + if ( player->frame > player->lastKeyframeFrame && player->frame % player->keyframeInterval == 0 ) + { + b3RecCaptureKeyframe( player ); + } + return true; + } + + int op = b3RecDispatchOne( &player->rdr ); + if ( op < 0 ) + { + player->atEnd = true; + return stepped; + } + if ( op == b3_recOpDestroyWorld ) // end of recording + { + player->atEnd = true; + return stepped; + } + if ( op == b3_recOpStep ) + { + player->frame += 1; + stepped = true; + } + else if ( op == b3_recOpStateHash ) // trailing record of the frame just stepped + { + // Latch the first frame whose state hash diverged. The hash belongs to the frame Step just + // advanced, so latch against the current frame, not the next Step which would be one late. + if ( player->divergeFrame < 0 && player->rdr.diverged ) + { + player->divergeFrame = player->frame; + } + } + } +} + +void b3RecPlayer_SubStepFrame( b3RecPlayer* player ) +{ + if ( player->atEnd ) + { + return; + } + + // Reset the per-frame query store before this frame's records are dispatched. + if ( player->atPreStep == false ) + { + player->frameQueryCount = 0; + player->frameHitCount = 0; + } + + // A frame is its leading inputs (queries and between-step mutators), one Step, and the Step's + // trailing StateHash. Queries are recorded before the Step they belong to, so they stash here + // against the world state they were computed for. + bool stepped = false; + bool haveCreateBodyOp = false; + for ( ;; ) + { + if ( player->rdr.cursor >= player->registryEnd || !player->rdr.ok ) + { + player->atEnd = true; + player->atPreStep = false; + return; + } + + // Once stepped, the StateHash is the only record still belonging to this frame. Anything else + // begins the next frame, so stop and let the next StepFrame consume it. Capture a keyframe at + // the boundary. + uint8_t currentOpCode = player->rdr.data[player->rdr.cursor]; + if ( stepped && currentOpCode != b3_recOpStateHash ) + { + if ( player->frame > player->lastKeyframeFrame && player->frame % player->keyframeInterval == 0 ) + { + b3RecCaptureKeyframe( player ); + } + return; + } + + if ( player->atPreStep == false && haveCreateBodyOp == true && currentOpCode == b3_recOpStep ) + { + player->atPreStep = true; + return; + } + + int op = b3RecDispatchOne( &player->rdr ); + if ( op < 0 ) + { + player->atEnd = true; + player->atPreStep = false; + return; + } + if ( op == b3_recOpDestroyWorld ) // end of recording + { + player->atEnd = true; + player->atPreStep = false; + return; + } + + if ( op == b3_recOpCreateBody ) + { + B3_ASSERT( player->atPreStep == false ); + haveCreateBodyOp = true; + } + + if ( op == b3_recOpStep ) + { + player->atPreStep = false; + player->frame += 1; + stepped = true; + } + else if ( op == b3_recOpStateHash ) // trailing record of the frame just stepped + { + // Latch the first frame whose state hash diverged. The hash belongs to the frame Step just + // advanced, so latch against the current frame, not the next Step which would be one late. + if ( player->divergeFrame < 0 && player->rdr.diverged ) + { + player->divergeFrame = player->frame; + } + } + } +} + +void b3RecPlayer_Restart( b3RecPlayer* player ) +{ + // Restore the frame-0 image in place so the replay world id stays stable across a restart or + // backward scrub. Stepping resumes at the first Step, which rebuilds the body list deterministically. + b3World* world = b3GetWorldFromId( player->rdr.replayWorldId ); + if ( b3DeserializeIntoShell( player->frame0Image, player->frame0Size, world, &player->rdr ) == false ) + { + player->rdr.ok = false; + return; + } + player->rdr.cursor = player->headerEnd; + player->rdr.ok = true; + player->rdr.diverged = false; + player->frame = 0; + player->divergeFrame = -1; + player->atEnd = false; + player->atPreStep = false; + + // Frame 0 is the pre-step snapshot with no recorded queries, so clear the per-frame store. This + // keeps the last stepped frame's queries from lingering on a restart or a backward scrub to 0. + player->frameQueryCount = 0; + player->frameHitCount = 0; + + // Roll the outliner body list back to its frame-0 contents. + b3RecGrow( (void**)&player->bodyIds, &player->bodyIdCap, player->frame0BodyIdCount, 0, (int)sizeof( b3BodyId ) ); + player->bodyIdCount = player->frame0BodyIdCount; + if ( player->frame0BodyIdCount > 0 ) + { + memcpy( player->bodyIds, player->frame0BodyIds, (size_t)player->frame0BodyIdCount * sizeof( b3BodyId ) ); + } +} + +void b3RecPlayer_SeekFrame( b3RecPlayer* player, int targetFrame ) +{ + if ( player == NULL ) + { + return; + } + + player->atPreStep = false; + + if ( targetFrame < 0 ) + { + targetFrame = 0; + } + + // Find the best keyframe strictly before the target. + const b3RecKeyframe* best = NULL; + for ( int i = 0; i < player->keyframeCount; ++i ) + { + const b3RecKeyframe* kf = player->keyframes + i; + if ( kf->frame < targetFrame && ( best == NULL || kf->frame > best->frame ) ) + { + best = kf; + } + } + + if ( targetFrame < player->frame ) + { + // Backward seek: restore keyframe or restart from frame 0. + if ( best != NULL ) + { + b3RecPlayerRestoreKeyframe( player, best ); + } + else + { + b3RecPlayer_Restart( player ); + } + } + else if ( best != NULL && best->frame > player->frame ) + { + // Forward seek that can skip ahead via a keyframe. + b3RecPlayerRestoreKeyframe( player, best ); + } + + while ( player->frame < targetFrame && b3RecPlayer_StepFrame( player ) ) + { + } +} + +b3WorldId b3RecPlayer_GetWorldId( const b3RecPlayer* player ) +{ + return player != NULL ? player->rdr.replayWorldId : b3_nullWorldId; +} + +int b3RecPlayer_GetFrame( const b3RecPlayer* player ) +{ + return player != NULL ? player->frame : 0; +} + +int b3RecPlayer_GetFrameCount( const b3RecPlayer* player ) +{ + return player != NULL ? player->frameCount : 0; +} + +bool b3RecPlayer_IsAtEnd( const b3RecPlayer* player ) +{ + return player != NULL ? player->atEnd : true; +} + +bool b3RecPlayer_IsAtPreStep( const b3RecPlayer* player ) +{ + return player != NULL ? player->atPreStep : false; +} + +bool b3RecPlayer_HasDiverged( const b3RecPlayer* player ) +{ + return player != NULL ? player->rdr.diverged : false; +} + +b3RecPlayerInfo b3RecPlayer_GetInfo( const b3RecPlayer* player ) +{ + b3RecPlayerInfo info = { 0 }; + if ( player != NULL ) + { + info.frameCount = player->frameCount; + info.workerCount = player->recordedWorkerCount; + info.timeStep = player->recordedDt; + info.subStepCount = player->recordedSubStepCount; + info.lengthScale = player->lengthScale; + info.bounds = player->bounds; + } + return info; +} + +int b3RecPlayer_GetDivergeFrame( const b3RecPlayer* player ) +{ + return player != NULL ? player->divergeFrame : -1; +} + +void b3RecPlayer_SetWorkerCount( b3RecPlayer* player, int count ) +{ + if ( player == NULL ) + { + return; + } + + player->recordedWorkerCount = b3ClampInt( count, 1, B3_MAX_WORKERS ); + + // Apply to the live world now so the next steps re-partition without a rebuild. Worker count is + // host state, not part of a keyframe image, so it survives an in-place restore. A rebuild on + // Restart or deep backward seek picks the count back up through b3RecPlayerCreateWorld. + if ( b3World_IsValid( player->rdr.replayWorldId ) ) + { + b3World_SetWorkerCount( player->rdr.replayWorldId, player->recordedWorkerCount ); + } +} + +void b3RecPlayer_SetKeyframePolicy( b3RecPlayer* player, size_t budgetBytes, int minIntervalFrames ) +{ + if ( player == NULL ) + { + return; + } + if ( budgetBytes > 0 ) + { + player->keyframeBudget = budgetBytes; + } + if ( minIntervalFrames > 0 ) + { + player->keyframeMinInterval = minIntervalFrames; + } + + // Drop the ring so it repopulates under the new policy on the next replay. + for ( int i = 0; i < player->keyframeCount; ++i ) + { + b3FreeKeyframe( player->keyframes + i ); + } + player->keyframeCount = 0; + player->keyframeBytes = 0; + player->keyframeInterval = player->keyframeMinInterval; + player->lastKeyframeFrame = 0; +} + +size_t b3RecPlayer_GetKeyframeBudget( const b3RecPlayer* player ) +{ + return player != NULL ? player->keyframeBudget : 0; +} + +int b3RecPlayer_GetKeyframeMinInterval( const b3RecPlayer* player ) +{ + return player != NULL ? player->keyframeMinInterval : 0; +} + +int b3RecPlayer_GetKeyframeInterval( const b3RecPlayer* player ) +{ + return player != NULL ? player->keyframeInterval : 0; +} + +size_t b3RecPlayer_GetKeyframeBytes( const b3RecPlayer* player ) +{ + return player != NULL ? player->keyframeBytes : 0; +} + +int b3RecPlayer_GetBodyCount( const b3RecPlayer* player ) +{ + return player != NULL ? player->bodyIdCount : 0; +} + +b3BodyId b3RecPlayer_GetBodyId( const b3RecPlayer* player, int index ) +{ + if ( player == NULL || index < 0 || index >= player->bodyIdCount ) + { + return b3_nullBodyId; + } + return player->bodyIds[index]; +} + +// A selected query draws in one reserved color so it stands out when every query is drawn at once. +static b3HexColor b3RecQuerySelColor( bool selected, b3HexColor base ) +{ + return selected ? b3_colorPlum : base; +} + +// Highlight each reported overlap shape by its AABB. Skip any destroyed since the query, per the +// b3Shape_GetAABB contract that overlap results may contain stale shapes. +static void b3RecDrawHitBounds( const b3RecPlayer* player, const b3RecDrawQuery* q, b3DebugDraw* draw, b3HexColor color ) +{ + if ( draw->DrawBoundsFcn == NULL ) + { + return; + } + for ( int hi = q->hitStart; hi < q->hitStart + q->hitCount; ++hi ) + { + b3ShapeId id = player->frameHits[hi].id; + if ( b3Shape_IsValid( id ) == false ) + { + continue; + } + draw->DrawBoundsFcn( b3Shape_GetAABB( id ), color, draw->context ); + } +} + +// Draw a recorded shape proxy at basePos. A lone point with no radius draws a fat point, a lone point +// with a radius draws a translucent sphere, and a multi-point cloud draws its points. Capsule and hull +// proxies are rare and fall through to the point cloud for now. +static void b3RecDrawProxy( b3DebugDraw* draw, b3Pos basePos, const b3RecDrawQuery* q, b3HexColor color ) +{ + if ( q->proxyCount == 1 ) + { + b3Pos p = b3OffsetPos( basePos, q->proxyPoints[0] ); + if ( q->proxyRadius > 0.0f ) + { + if ( draw->DrawSphereFcn ) + { + draw->DrawSphereFcn( p, q->proxyRadius, color, 0.5f, draw->context ); + } + } + else if ( draw->DrawPointFcn ) + { + draw->DrawPointFcn( p, 10.0f, color, draw->context ); + } + } + else if ( q->proxyCount == 2 && q->proxyRadius > 0.0f ) + { + if ( draw->DrawCapsuleFcn ) + { + b3Pos p1 = b3OffsetPos( basePos, q->proxyPoints[0] ); + b3Pos p2 = b3OffsetPos( basePos, q->proxyPoints[1] ); + draw->DrawCapsuleFcn( p1, p2, q->proxyRadius, color, 0.5f, draw->context ); + } + } + else if ( q->proxyCount >= 2 && draw->DrawPointFcn ) + { + for ( int i = 0; i < q->proxyCount; ++i ) + { + draw->DrawPointFcn( b3OffsetPos( basePos, q->proxyPoints[i] ), 6.0f, color, draw->context ); + } + } +} + +void b3RecPlayer_DrawFrameQueries( b3RecPlayer* player, b3DebugDraw* draw, int queryIndex, int selectedIndex ) +{ + if ( player == NULL || draw == NULL ) + { + return; + } + + // queryIndex < 0 draws every query, otherwise just the one selected in the viewer. The query at + // selectedIndex draws in one reserved color and is labeled, so it stands out among the rest. + for ( int qi = 0; qi < player->frameQueryCount; ++qi ) + { + if ( queryIndex >= 0 && qi != queryIndex ) + { + continue; + } + + const b3RecDrawQuery* q = &player->frameQueries[qi]; + bool selected = ( qi == selectedIndex ); + + switch ( q->kind ) + { + case B3_RECQ_CAST_RAY: + case B3_RECQ_CAST_RAY_CLOSEST: + { + b3Pos origin = q->origin; + b3Pos end = b3OffsetPos( origin, q->translation ); + if ( draw->DrawSegmentFcn ) + { + draw->DrawSegmentFcn( origin, end, b3RecQuerySelColor( selected, b3_colorYellow ), draw->context ); + } + for ( int hi = q->hitStart; hi < q->hitStart + q->hitCount; ++hi ) + { + const b3RecRecordedHit* h = &player->frameHits[hi]; + if ( draw->DrawPointFcn ) + { + draw->DrawPointFcn( h->point, 4.0f, b3RecQuerySelColor( selected, b3_colorYellow ), draw->context ); + } + if ( draw->DrawSegmentFcn ) + { + draw->DrawSegmentFcn( h->point, b3OffsetPos( h->point, b3MulSV( 0.2f, h->normal ) ), + b3RecQuerySelColor( selected, b3_colorYellowGreen ), draw->context ); + } + } + break; + } + case B3_RECQ_CAST_SHAPE: + { + // Draw the cast line and the proxy at its start, then each hit point and normal. + if ( draw->DrawSegmentFcn ) + { + draw->DrawSegmentFcn( q->origin, b3OffsetPos( q->origin, q->translation ), + b3RecQuerySelColor( selected, b3_colorSkyBlue ), draw->context ); + } + b3RecDrawProxy( draw, q->origin, q, b3RecQuerySelColor( selected, b3_colorLightGreen ) ); + for ( int hi = q->hitStart; hi < q->hitStart + q->hitCount; ++hi ) + { + const b3RecRecordedHit* h = &player->frameHits[hi]; + if ( draw->DrawPointFcn ) + { + draw->DrawPointFcn( h->point, 4.0f, b3RecQuerySelColor( selected, b3_colorSkyBlue ), draw->context ); + } + if ( draw->DrawSegmentFcn ) + { + draw->DrawSegmentFcn( h->point, b3OffsetPos( h->point, b3MulSV( 0.2f, h->normal ) ), + b3RecQuerySelColor( selected, b3_colorLightSkyBlue ), draw->context ); + } + if ( draw->DrawSphereFcn ) + { + b3Pos p = b3OffsetPos( q->origin, b3MulSV( h->fraction, q->translation ) ); + b3RecDrawProxy( draw, p, q, b3RecQuerySelColor( selected, b3_colorSkyBlue ) ); + } + } + break; + } + case B3_RECQ_CAST_MOVER: + { + b3Pos c1 = b3OffsetPos( q->origin, q->mover.center1 ); + b3Pos c2 = b3OffsetPos( q->origin, q->mover.center2 ); + b3HexColor c = b3_colorLightSkyBlue; + if ( draw->DrawCapsuleFcn ) + { + draw->DrawCapsuleFcn( c1, c2, q->mover.radius, b3RecQuerySelColor( selected, c ), 0.6f, draw->context ); + + if ( q->castFraction > 0.01f ) + { + b3Vec3 d = b3MulSV( q->castFraction, q->translation ); + c1 = b3OffsetPos( c1, d ); + c2 = b3OffsetPos( c2, d ); + draw->DrawCapsuleFcn( c1, c2, q->mover.radius, c, 0.3f, draw->context ); + } + } + break; + } + + case B3_RECQ_COLLIDE_MOVER: + { + b3Pos c1 = b3OffsetPos( q->origin, q->mover.center1 ); + b3Pos c2 = b3OffsetPos( q->origin, q->mover.center2 ); + b3HexColor c = b3_colorTan; + if ( draw->DrawCapsuleFcn ) + { + draw->DrawCapsuleFcn( c1, c2, q->mover.radius, b3RecQuerySelColor( selected, c ), 0.6f, draw->context ); + } + + for ( int hi = q->hitStart; hi < q->hitStart + q->hitCount; ++hi ) + { + const b3RecRecordedHit* h = &player->frameHits[hi]; + b3Pos point = b3OffsetPos( q->origin, h->plane.point ); + if ( draw->DrawSegmentFcn ) + { + draw->DrawSegmentFcn( point, b3OffsetPos( point, b3MulSV( 0.2f, h->plane.plane.normal ) ), + b3RecQuerySelColor( selected, b3_colorOrange ), draw->context ); + } + } + break; + } + + case B3_RECQ_OVERLAP_AABB: + { + if ( draw->DrawBoundsFcn ) + { + draw->DrawBoundsFcn( q->aabb, b3RecQuerySelColor( selected, b3_colorLimeGreen ), draw->context ); + } + b3RecDrawHitBounds( player, q, draw, b3RecQuerySelColor( selected, b3_colorMagenta ) ); + break; + } + case B3_RECQ_OVERLAP_SHAPE: + { + // The overlap proxy sits at the origin; draw it, then the overlapping shape bounds. + b3RecDrawProxy( draw, q->origin, q, b3RecQuerySelColor( selected, b3_colorLimeGreen ) ); + b3RecDrawHitBounds( player, q, draw, b3RecQuerySelColor( selected, b3_colorMagenta ) ); + break; + } + default: + break; + } + + // Label the selected query at its origin so it reads by name among the others. The overlap AABB + // has no origin, so anchor at the box center. Untagged queries (no key) rely on the color alone. + if ( selected && q->key != 0 && draw->DrawStringFcn != NULL ) + { + const char* name = NULL; + uint64_t id = 0; + if ( player->rdr.tagMap != NULL ) + { + b3RecTagLookup_itr it = b3RecTagLookup_get( (b3RecTagLookup*)player->rdr.tagMap, q->key ); + if ( b3RecTagLookup_is_end( it ) == false ) + { + const b3RecTag* tag = &player->rdr.tags[it.data->val]; + name = tag->queryName; + id = tag->id; + } + } + char label[64]; + if ( name != NULL && name[0] != '\0' && id != 0 ) + { + snprintf( label, sizeof( label ), "%.40s (%" PRIu64 ")", name, id ); + } + else if ( name != NULL && name[0] != '\0' ) + { + snprintf( label, sizeof( label ), "%.40s", name ); + } + else + { + snprintf( label, sizeof( label ), "#%" PRIu64, id ); + } + b3Pos labelPos = q->origin; + if ( q->kind == B3_RECQ_OVERLAP_AABB ) + { + labelPos = b3ToPos( b3AABB_Center( q->aabb ) ); + } + else if ( q->kind == B3_RECQ_CAST_MOVER || q->kind == B3_RECQ_COLLIDE_MOVER ) + { + // Sit the label just past the center2 end cap, which for an upright mover is above it. + b3Pos c1 = b3OffsetPos( q->origin, q->mover.center1 ); + b3Pos c2 = b3OffsetPos( q->origin, q->mover.center2 ); + b3Vec3 dir = b3Normalize( b3SubPos( c2, c1 ) ); + labelPos = b3OffsetPos( c2, b3MulSV( 1.25f * q->mover.radius, dir ) ); + } + draw->DrawStringFcn( labelPos, label, b3_colorWhite, draw->context ); + } + } +} + +// The internal b3RecQueryKind values match the public b3RecQueryType, so the kind copies across as a +// plain cast. Pin the first and last kinds to catch enum drift. +_Static_assert( b3_recQueryOverlapAABB == 0 && B3_RECQ_OVERLAP_AABB == 0, "query type enum drift" ); +_Static_assert( b3_recQueryCollideMover == 6 && B3_RECQ_COLLIDE_MOVER == 6, "query type enum drift" ); + +int b3RecPlayer_GetFrameQueryCount( const b3RecPlayer* player ) +{ + return player != NULL ? player->frameQueryCount : 0; +} + +b3RecQueryInfo b3RecPlayer_GetFrameQuery( const b3RecPlayer* player, int index ) +{ + b3RecQueryInfo info = { 0 }; + if ( player == NULL || index < 0 || index >= player->frameQueryCount ) + { + return info; + } + + const b3RecDrawQuery* q = &player->frameQueries[index]; + info.type = (b3RecQueryType)q->kind; + info.filter = q->filter; + info.aabb = q->aabb; + info.origin = q->origin; + info.translation = q->translation; + info.hitCount = q->hitCount; + info.key = q->key; + info.id = 0; + info.name = NULL; + if ( q->key != 0 && player->rdr.tagMap != NULL ) + { + b3RecTagLookup_itr it = b3RecTagLookup_get( (b3RecTagLookup*)player->rdr.tagMap, q->key ); + if ( b3RecTagLookup_is_end( it ) == false ) + { + const b3RecTag* tag = &player->rdr.tags[it.data->val]; + info.id = tag->id; + // An id-only tag interns an empty name; report it as none so the viewer shows the id alone. + info.name = tag->queryName[0] != '\0' ? tag->queryName : NULL; + } + } + return info; +} + +b3RecQueryHit b3RecPlayer_GetFrameQueryHit( const b3RecPlayer* player, int queryIndex, int hitIndex ) +{ + b3RecQueryHit hit = { 0 }; + if ( player == NULL || queryIndex < 0 || queryIndex >= player->frameQueryCount ) + { + return hit; + } + + const b3RecDrawQuery* q = &player->frameQueries[queryIndex]; + if ( hitIndex < 0 || hitIndex >= q->hitCount ) + { + return hit; + } + + const b3RecRecordedHit* h = &player->frameHits[q->hitStart + hitIndex]; + hit.shape = h->id; + hit.point = h->point; + hit.normal = h->normal; + hit.fraction = h->fraction; + return hit; +} + +void b3RecPlayer_SetDebugShapeCallbacks( b3RecPlayer* player, b3CreateDebugShapeCallback* createDebugShape, + b3DestroyDebugShapeCallback* destroyDebugShape, void* context ) +{ + if ( player == NULL ) + { + return; + } + + player->createDebugShape = createDebugShape; + player->destroyDebugShape = destroyDebugShape; + player->debugShapeContext = context; + + // A world fixes its debug-shape callbacks at creation, so rebuild frame 0 under the new + // wiring. The old world held no adapter shapes (its callbacks were NULL), so the tear-down + // is balanced. Geometry slots are byte blobs reused as-is, the same path Restart relies on. + if ( b3World_IsValid( player->rdr.replayWorldId ) ) + { + b3DestroyWorld( player->rdr.replayWorldId ); + } + player->rdr.replayWorldId = b3RecPlayerCreateWorld( player ); + player->rdr.cursor = player->headerEnd; + player->rdr.ok = true; + player->rdr.diverged = false; + player->frame = 0; + player->divergeFrame = -1; + player->atEnd = false; + + // Re-seed the world so its shapes are recreated through the new callbacks. + b3World* world = b3GetWorldFromId( player->rdr.replayWorldId ); + if ( b3DeserializeIntoShell( player->frame0Image, player->frame0Size, world, &player->rdr ) == false ) + { + player->rdr.ok = false; + return; + } + player->rdr.cursor = player->headerEnd; + + // Rebuild the outliner from the frame-0 world that was just stood up under the new callbacks. + b3RecSeedFrame0BodyIds( player ); +} diff --git a/vendor/box3d/src/src/recording_replay.h b/vendor/box3d/src/src/recording_replay.h new file mode 100644 index 000000000..68760f6d0 --- /dev/null +++ b/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 +#include + +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 ); diff --git a/vendor/box3d/src/src/revolute_joint.c b/vendor/box3d/src/src/revolute_joint.c new file mode 100644 index 000000000..7856a83d8 --- /dev/null +++ b/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 ); + } +} diff --git a/vendor/box3d/src/src/scheduler.c b/vendor/box3d/src/src/scheduler.c new file mode 100644 index 000000000..7bcde42f1 --- /dev/null +++ b/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 +#include +#include + +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(); + } + } +} diff --git a/vendor/box3d/src/src/scheduler.h b/vendor/box3d/src/src/scheduler.h new file mode 100644 index 000000000..f9aeff7b2 --- /dev/null +++ b/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 ); diff --git a/vendor/box3d/src/src/sensor.c b/vendor/box3d/src/src/sensor.c new file mode 100644 index 000000000..0cd23a460 --- /dev/null +++ b/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 + +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; + } +} diff --git a/vendor/box3d/src/src/sensor.h b/vendor/box3d/src/src/sensor.h new file mode 100644 index 000000000..1a17366b7 --- /dev/null +++ b/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 ); diff --git a/vendor/box3d/src/src/shape.c b/vendor/box3d/src/src/shape.c new file mode 100644 index 000000000..e4d9d9e0c --- /dev/null +++ b/vendor/box3d/src/src/shape.c @@ -0,0 +1,2393 @@ +// SPDX-FileCopyrightText: 2025 Erin Catto +// SPDX-License-Identifier: MIT + +#include "shape.h" + +#include "body.h" +#include "broad_phase.h" +#include "contact.h" +#include "physics_world.h" +#include "recording.h" +#include "sensor.h" + +// needed for dll export +#include "aabb.h" +#include "compound.h" + +#include "box3d/box3d.h" + +static b3Shape* b3GetShape( b3World* world, b3ShapeId shapeId ) +{ + int id = shapeId.index1 - 1; + b3Shape* shape = b3Array_Get( world->shapes, id ); + B3_ASSERT( shape->id == id && shape->generation == shapeId.generation ); + return shape; +} + +static float b3ComputeShapeMargin( b3Shape* shape ) +{ + float margin = 0.0f; + + switch ( shape->type ) + { + case b3_sphereShape: + { + margin = shape->sphere.radius; + } + break; + + case b3_capsuleShape: + { + margin = 0.5f * b3Distance( shape->capsule.center2, shape->capsule.center1 ) + shape->capsule.radius; + } + break; + + case b3_hullShape: + { + const b3HullData* hull = shape->hull; + const b3Vec3* points = b3GetHullPoints( hull ); + float maxExtentSqr = 0.0f; + int count = hull->vertexCount; + for ( int i = 0; i < count; ++i ) + { + float distSqr = b3DistanceSquared( points[i], hull->center ); + maxExtentSqr = b3MaxFloat( maxExtentSqr, distSqr ); + } + margin = sqrtf( maxExtentSqr ); + } + break; + + case b3_meshShape: + case b3_heightShape: + case b3_compoundShape: + { + // Static-only shapes: broadphase uses speculative distance for static + // proxies, so the per-shape margin is never consumed in practice. + // Return the cap so any incidental use is generous. + return B3_MAX_AABB_MARGIN; + } + + default: + B3_VALIDATE( false ); + return B3_MAX_AABB_MARGIN; + } + + return b3MinFloat( B3_MAX_AABB_MARGIN, B3_AABB_MARGIN_FRACTION * margin ); +} + +static void b3UpdateShapeAABBs( b3Shape* shape, b3WorldTransform transform, b3BodyType proxyType ) +{ + // Compute a bounding box with a speculative margin + const float speculativeDistance = B3_SPECULATIVE_DISTANCE; + const float aabbMargin = shape->aabbMargin; + + b3AABB aabb = b3ComputeFatShapeAABB( shape, transform, speculativeDistance ); + shape->aabb = aabb; + + // Smaller margin for static bodies. Cannot be zero due to TOI tolerance. + float margin = proxyType == b3_staticBody ? speculativeDistance : aabbMargin; + b3AABB fatAABB; + fatAABB.lowerBound.x = aabb.lowerBound.x - margin; + fatAABB.lowerBound.y = aabb.lowerBound.y - margin; + fatAABB.lowerBound.z = aabb.lowerBound.z - margin; + fatAABB.upperBound.x = aabb.upperBound.x + margin; + fatAABB.upperBound.y = aabb.upperBound.y + margin; + fatAABB.upperBound.z = aabb.upperBound.z + margin; + shape->fatAABB = fatAABB; +} + +static b3Shape* b3CreateShapeInternal( b3World* world, b3Body* body, b3WorldTransform bodyTransform, const b3ShapeDef* def, + const void* geometry, b3ShapeType shapeType, b3Transform shapeTransform, b3Vec3 scale, + bool haveShapeTransform ) +{ + int shapeId = b3AllocId( &world->shapeIdPool ); + + if ( shapeId == world->shapes.count ) + { + b3Array_Push( world->shapes, (b3Shape){ 0 } ); + } + else + { + B3_ASSERT( world->shapes.data[shapeId].id == B3_NULL_INDEX ); + } + + b3Shape* shape = b3Array_Get( world->shapes, shapeId ); + + switch ( shapeType ) + { + case b3_capsuleShape: + shape->capsule = *(b3Capsule*)geometry; + break; + + case b3_compoundShape: + // Compounds must be a static and not a sensor + B3_ASSERT( body->type == b3_staticBody ); + B3_ASSERT( def->isSensor == false ); + shape->compound = (b3CompoundData*)geometry; + break; + + case b3_sphereShape: + shape->sphere = *(b3Sphere*)geometry; + break; + + case b3_hullShape: + if ( haveShapeTransform ) + { + // The transform and non-uniform scale are baked into fresh data, then shared. + b3HullData* baked = b3CloneAndTransformHull( (b3HullData*)geometry, shapeTransform, scale ); + if ( baked == NULL ) + { + // This can fail to produce a valid hull in extreme cases + b3FreeId( &world->shapeIdPool, shapeId ); + shape->id = B3_NULL_INDEX; + return NULL; + } + + shape->hull = b3AddOwnedHullToDatabase( world, baked ); + } + else + { + shape->hull = b3AddHullToDatabase( world, (const b3HullData*)geometry ); + } + break; + + case b3_meshShape: + { + shape->mesh.data = (b3MeshData*)geometry; + shape->mesh.scale = b3SafeScale( scale ); + } + break; + + case b3_heightShape: + shape->heightField = (b3HeightFieldData*)geometry; + break; + + default: + B3_ASSERT( false ); + break; + } + + shape->id = shapeId; + shape->bodyId = body->id; + shape->type = shapeType; + shape->density = def->density; + shape->explosionScale = def->explosionScale; + shape->filter = def->filter; + shape->userData = def->userData; + shape->userShape = NULL; + shape->flags = 0; + shape->flags |= def->enableSensorEvents ? b3_enableSensorEvents : 0; + shape->flags |= def->enableContactEvents ? b3_enableContactEvents : 0; + shape->flags |= def->enableCustomFiltering ? b3_enableCustomFiltering : 0; + shape->flags |= def->enableHitEvents ? b3_enableHitEvents : 0; + shape->flags |= def->enablePreSolveEvents ? b3_enablePreSolveEvents : 0; + shape->flags |= def->enableSpeculativeContact ? b3_enableSpeculative : 0; + shape->proxyKey = B3_NULL_INDEX; + shape->localCentroid = b3GetShapeCentroid( shape ); + shape->aabbMargin = b3ComputeShapeMargin( shape ); + shape->aabb = (b3AABB){ b3Vec3_zero, b3Vec3_zero }; + shape->fatAABB = (b3AABB){ b3Vec3_zero, b3Vec3_zero }; + shape->nameId = b3AddName( &world->names, def->name ); + shape->generation += 1; + + if ( shape->type == b3_compoundShape ) + { + // Own a copy of the compound materials so every shape frees its array the same way. Compounds + // are few, so the copy is cheap and avoids aliasing the geometry blob. + int materialCount = shape->compound->materialCount; + shape->materialCount = materialCount; + shape->materials = b3Alloc( materialCount * sizeof( b3SurfaceMaterial ) ); + memcpy( shape->materials, b3GetCompoundMaterials( shape->compound ), materialCount * sizeof( b3SurfaceMaterial ) ); + } + else if ( def->materialCount > 1 && def->materials != NULL ) + { + // Per triangle materials need a heap array. + shape->materialCount = def->materialCount; + shape->materials = b3Alloc( def->materialCount * sizeof( b3SurfaceMaterial ) ); + memcpy( shape->materials, def->materials, def->materialCount * sizeof( b3SurfaceMaterial ) ); + } + else + { + // The common case is one material, stored inline with no allocation. + shape->material = ( def->materialCount == 1 && def->materials != NULL ) ? def->materials[0] : def->baseMaterial; + shape->materialCount = 1; + shape->materials = NULL; + } + + if ( body->setIndex != b3_disabledSet ) + { + b3BodyType proxyType = body->type; + bool forcePairCreation = def->invokeContactCreation && shape->type != b3_compoundShape; + b3CreateShapeProxy( shape, &world->broadPhase, proxyType, bodyTransform, forcePairCreation ); + } + + // Add to shape doubly linked list + if ( body->headShapeId != B3_NULL_INDEX ) + { + b3Shape* headShape = b3Array_Get( world->shapes, body->headShapeId ); + headShape->prevShapeId = shapeId; + } + + shape->prevShapeId = B3_NULL_INDEX; + shape->nextShapeId = body->headShapeId; + body->headShapeId = shapeId; + body->shapeCount += 1; + + if ( def->isSensor ) + { + shape->sensorIndex = world->sensors.count; + b3Sensor* sensor = b3Array_Emplace( world->sensors ); + b3Array_CreateN( sensor->hits, 4 ); + b3Array_CreateN( sensor->overlaps1, 16 ); + b3Array_CreateN( sensor->overlaps2, 16 ); + sensor->shapeId = shapeId; + } + else + { + shape->sensorIndex = B3_NULL_INDEX; + } + + b3ValidateSolverSets( world ); + + return shape; +} + +static b3ShapeId b3CreateShape( b3BodyId bodyId, const b3ShapeDef* def, const void* geometry, b3ShapeType shapeType, + b3Transform transform, b3Vec3 scale, bool haveTransform ) +{ + B3_CHECK_DEF( def ); + B3_ASSERT( b3IsValidFloat( def->density ) && def->density >= 0.0f ); + B3_ASSERT( b3IsValidFloat( def->baseMaterial.friction ) && def->baseMaterial.friction >= 0.0f ); + B3_ASSERT( b3IsValidFloat( def->baseMaterial.restitution ) && def->baseMaterial.restitution >= 0.0f ); + + b3World* world = b3GetUnlockedWorld( bodyId.world0 ); + if ( world == NULL ) + { + return (b3ShapeId){ 0 }; + } + + if ( world->shapes.count == B3_MAX_SHAPES && world->shapeIdPool.freeArray.count == 0 ) + { + B3_ASSERT( false ); + return b3_nullShapeId; + } + + b3Body* body = b3GetBodyFullId( world, bodyId ); + if ( body->type != b3_staticBody && ( shapeType == b3_compoundShape || shapeType == b3_heightShape ) ) + { + // Compound and height shapes must be on static bodies. + return b3_nullShapeId; + } + + world->locked = true; + + b3WorldTransform bodyTransform = b3GetBodyTransformQuick( world, body ); + + b3Shape* shape = + b3CreateShapeInternal( world, body, bodyTransform, def, geometry, shapeType, transform, scale, haveTransform ); + + if ( shape == NULL ) + { + world->locked = false; + return b3_nullShapeId; + } + + if ( def->updateBodyMass == true ) + { + b3UpdateBodyMassData( world, body ); + } + else if ( ( body->flags & b3_dirtyMass ) == 0 ) + { + body->flags |= b3_dirtyMass; + b3SyncBodyFlags( world, body ); + } + + b3ValidateSolverSets( world ); + + b3ShapeId id = { shape->id + 1, bodyId.world0, shape->generation }; + + world->locked = false; + + return id; +} + +b3ShapeId b3CreateSphereShape( b3BodyId bodyId, const b3ShapeDef* def, const b3Sphere* sphere ) +{ + b3ShapeId shapeId = b3CreateShape( bodyId, def, sphere, b3_sphereShape, b3Transform_identity, b3Vec3_one, false ); + if ( shapeId.index1 != 0 ) + { + b3World* world = b3GetUnlockedWorld( bodyId.world0 ); + if ( world != NULL ) + { + B3_REC_CREATE( world, CreateSphereShape, shapeId, bodyId, *def, *sphere ); + } + } + return shapeId; +} + +b3ShapeId b3CreateCapsuleShape( b3BodyId bodyId, const b3ShapeDef* def, const b3Capsule* capsule ) +{ + float lengthSqr = b3DistanceSquared( capsule->center1, capsule->center2 ); + b3ShapeId shapeId; + if ( lengthSqr <= B3_LINEAR_SLOP * B3_LINEAR_SLOP ) + { + b3Sphere sphere = { b3Lerp( capsule->center1, capsule->center2, 0.5f ), capsule->radius }; + shapeId = b3CreateShape( bodyId, def, &sphere, b3_sphereShape, b3Transform_identity, b3Vec3_one, false ); + } + else + { + shapeId = b3CreateShape( bodyId, def, capsule, b3_capsuleShape, b3Transform_identity, b3Vec3_one, false ); + } + if ( shapeId.index1 != 0 ) + { + b3World* world = b3GetUnlockedWorld( bodyId.world0 ); + if ( world != NULL ) + { + B3_REC_CREATE( world, CreateCapsuleShape, shapeId, bodyId, *def, *capsule ); + } + } + return shapeId; +} + +b3ShapeId b3CreateHullShape( b3BodyId bodyId, const b3ShapeDef* def, const b3HullData* hull ) +{ + B3_VALIDATE( b3IsValidHull( hull ) ); + B3_VALIDATE( hull->hash != 0 ); + b3ShapeId shapeId = b3CreateShape( bodyId, def, hull, b3_hullShape, b3Transform_identity, b3Vec3_one, false ); + if ( shapeId.index1 != 0 ) + { + b3World* world = b3GetUnlockedWorld( bodyId.world0 ); + if ( world != NULL && world->recording != NULL ) + { + uint32_t geometryId = b3RecInternHull( world->recording, hull ); + b3RecArgs_CreateHullShape createArgs = { bodyId, *def, geometryId }; + b3RecWriteRet_CreateHullShape( world->recording, &createArgs, shapeId ); + } + } + return shapeId; +} + +b3ShapeId b3CreateTransformedHullShape( b3BodyId bodyId, const b3ShapeDef* def, const b3HullData* hull, b3Transform transform, + b3Vec3 scale ) +{ + B3_VALIDATE( b3IsValidHull( hull ) ); + b3ShapeId shapeId = b3CreateShape( bodyId, def, hull, b3_hullShape, transform, scale, true ); + if ( shapeId.index1 != 0 ) + { + b3World* world = b3GetUnlockedWorld( bodyId.world0 ); + if ( world != NULL && world->recording != NULL ) + { + // The transform and scale are baked into fresh hull data at create time. Record the baked hull + // as a plain hull shape so replay rebuilds identical geometry with no rebake, and the keyframe + // registry, which interns the live baked hull, stays seeded. + b3Shape* shape = b3Array_Get( world->shapes, shapeId.index1 - 1 ); + uint32_t geometryId = b3RecInternHull( world->recording, shape->hull ); + b3RecArgs_CreateHullShape createArgs = { bodyId, *def, geometryId }; + b3RecWriteRet_CreateHullShape( world->recording, &createArgs, shapeId ); + } + } + return shapeId; +} + +b3ShapeId b3CreateMeshShape( b3BodyId bodyId, const b3ShapeDef* def, const b3MeshData* mesh, b3Vec3 scale ) +{ + B3_VALIDATE( b3IsValidMesh( mesh ) ); + B3_VALIDATE( mesh->hash != 0 ); + b3ShapeId shapeId = b3CreateShape( bodyId, def, mesh, b3_meshShape, b3Transform_identity, scale, true ); + if ( shapeId.index1 != 0 ) + { + b3World* world = b3GetUnlockedWorld( bodyId.world0 ); + if ( world != NULL && world->recording != NULL ) + { + uint32_t geometryId = b3RecInternMesh( world->recording, mesh ); + b3RecArgs_CreateMeshShape createArgs = { bodyId, *def, geometryId, scale }; + b3RecWriteRet_CreateMeshShape( world->recording, &createArgs, shapeId ); + } + } + return shapeId; +} + +b3ShapeId b3CreateHeightFieldShape( b3BodyId bodyId, const b3ShapeDef* def, const b3HeightFieldData* heightField ) +{ + B3_VALIDATE( heightField->hash != 0 ); + b3ShapeId shapeId = b3CreateShape( bodyId, def, heightField, b3_heightShape, b3Transform_identity, b3Vec3_one, false ); + if ( shapeId.index1 != 0 ) + { + b3World* world = b3GetUnlockedWorld( bodyId.world0 ); + if ( world != NULL && world->recording != NULL ) + { + uint32_t geometryId = b3RecInternHeightField( world->recording, heightField ); + b3RecArgs_CreateHeightFieldShape createArgs = { bodyId, *def, geometryId }; + b3RecWriteRet_CreateHeightFieldShape( world->recording, &createArgs, shapeId ); + } + } + return shapeId; +} + +b3ShapeId b3CreateCompoundShape( b3BodyId bodyId, b3ShapeDef* def, const b3CompoundData* compound ) +{ + b3ShapeId shapeId = b3CreateShape( bodyId, def, compound, b3_compoundShape, b3Transform_identity, b3Vec3_one, false ); + if ( shapeId.index1 != 0 ) + { + b3World* world = b3GetUnlockedWorld( bodyId.world0 ); + if ( world != NULL && world->recording != NULL ) + { + uint32_t geometryId = b3RecInternCompound( world->recording, compound ); + b3RecArgs_CreateCompoundShape createArgs = { bodyId, *def, geometryId }; + b3RecWriteRet_CreateCompoundShape( world->recording, &createArgs, shapeId ); + } + } + return shapeId; +} + +// Destroy a shape on a body. This doesn't need to be called when destroying a body. +static void b3DestroyShapeInternal( b3World* world, b3Shape* shape, b3Body* body, bool wakeBodies ) +{ + int shapeId = shape->id; + + // Remove the shape from the body's doubly linked list. + if ( shape->prevShapeId != B3_NULL_INDEX ) + { + b3Shape* prevShape = b3Array_Get( world->shapes, shape->prevShapeId ); + prevShape->nextShapeId = shape->nextShapeId; + } + + if ( shape->nextShapeId != B3_NULL_INDEX ) + { + b3Shape* nextShape = b3Array_Get( world->shapes, shape->nextShapeId ); + nextShape->prevShapeId = shape->prevShapeId; + } + + if ( shapeId == body->headShapeId ) + { + body->headShapeId = shape->nextShapeId; + } + + body->shapeCount -= 1; + + // Remove from broad-phase. + b3DestroyShapeProxy( shape, &world->broadPhase ); + + // Destroy any contacts associated with the shape. + int contactKey = body->headContactKey; + while ( contactKey != B3_NULL_INDEX ) + { + int contactId = contactKey >> 1; + int edgeIndex = contactKey & 1; + + b3Contact* contact = b3Array_Get( world->contacts, contactId ); + contactKey = contact->edges[edgeIndex].nextKey; + + if ( contact->shapeIdA == shapeId || contact->shapeIdB == shapeId ) + { + b3DestroyContact( world, contact, wakeBodies ); + } + } + + if ( shape->sensorIndex != B3_NULL_INDEX ) + { + b3Sensor* sensor = b3Array_Get( world->sensors, shape->sensorIndex ); + for ( int i = 0; i < sensor->overlaps2.count; ++i ) + { + b3Visitor* ref = sensor->overlaps2.data + i; + b3SensorEndTouchEvent event = { + .sensorShapeId = + { + .index1 = shapeId + 1, + .world0 = world->worldId, + .generation = shape->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, shape->sensorIndex ); + if ( movedIndex != B3_NULL_INDEX ) + { + // Fixup moved sensor + b3Sensor* movedSensor = b3Array_Get( world->sensors, shape->sensorIndex ); + b3Shape* otherSensorShape = b3Array_Get( world->shapes, movedSensor->shapeId ); + otherSensorShape->sensorIndex = shape->sensorIndex; + } + } + + // Destroy every shape member from b3Alloc + b3DestroyShapeAllocations( world, shape ); + + // Return shape to free list. + b3FreeId( &world->shapeIdPool, shapeId ); + shape->id = B3_NULL_INDEX; + + b3ValidateSolverSets( world ); +} + +void b3DestroyShape( b3ShapeId shapeId, bool updateBodyMass ) +{ + b3World* world = b3GetUnlockedWorld( shapeId.world0 ); + if ( world == NULL ) + { + return; + } + + B3_REC( world, DestroyShape, shapeId, updateBodyMass ); + + world->locked = true; + + b3Shape* shape = b3GetShape( world, shapeId ); + + // need to wake bodies because this might be a static body + bool wakeBodies = true; + + b3Body* body = b3Array_Get( world->bodies, shape->bodyId ); + b3DestroyShapeInternal( world, shape, body, wakeBodies ); + + if ( updateBodyMass == true ) + { + b3UpdateBodyMassData( world, body ); + } + + world->locked = false; +} + +b3AABB b3ComputeShapeAABB( const b3Shape* shape, b3Transform transform ) +{ + switch ( shape->type ) + { + case b3_capsuleShape: + return b3ComputeCapsuleAABB( &shape->capsule, transform ); + + case b3_compoundShape: + return b3ComputeCompoundAABB( shape->compound, transform ); + + case b3_heightShape: + return b3ComputeHeightFieldAABB( shape->heightField, transform ); + + case b3_hullShape: + return b3ComputeHullAABB( shape->hull, transform ); + + case b3_meshShape: + return b3ComputeMeshAABB( shape->mesh.data, transform, shape->mesh.scale ); + + case b3_sphereShape: + return b3ComputeSphereAABB( &shape->sphere, transform ); + + default: + { + B3_ASSERT( false ); + b3AABB empty = { transform.p, transform.p }; + return empty; + } + } +} + +b3AABB b3ComputeFatShapeAABB( const b3Shape* shape, b3WorldTransform transform, float extra ) +{ + b3Vec3 r = { extra, extra, extra }; +#if defined( BOX3D_DOUBLE_PRECISION ) + // Build the box in the body local frame, inflate, then translate by the double origin and + // round outward. Inflating before the single rounding matters far from the origin where the + // float margin would otherwise vanish. + b3Transform rotation = { b3Vec3_zero, transform.q }; + b3AABB localBox = b3ComputeShapeAABB( shape, rotation ); + localBox.lowerBound = b3Sub( localBox.lowerBound, r ); + localBox.upperBound = b3Add( localBox.upperBound, r ); + return b3OffsetAABB( localBox, transform.p ); +#else + b3AABB aabb = b3ComputeShapeAABB( shape, transform ); + aabb.lowerBound = b3Sub( aabb.lowerBound, r ); + aabb.upperBound = b3Add( aabb.upperBound, r ); + return aabb; +#endif +} + +b3AABB b3ComputeSweptShapeAABB( const b3Shape* shape, const b3Sweep* sweep, float time ) +{ + B3_ASSERT( 0.0f <= time && time <= 1.0f ); + b3Transform xf1 = { b3Sub( sweep->c1, b3RotateVector( sweep->q1, sweep->localCenter ) ), sweep->q1 }; + b3Transform xf2 = b3GetSweepTransform( sweep, time ); + + switch ( shape->type ) + { + case b3_capsuleShape: + return b3ComputeSweptCapsuleAABB( &shape->capsule, xf1, xf2 ); + + case b3_hullShape: + return b3ComputeSweptHullAABB( shape->hull, xf1, xf2 ); + + case b3_sphereShape: + return b3ComputeSweptSphereAABB( &shape->sphere, xf1, xf2 ); + + default: + B3_ASSERT( false ); + return (b3AABB){ xf1.p, xf1.p }; + } +} + +b3Vec3 b3GetShapeCentroid( const b3Shape* shape ) +{ + switch ( shape->type ) + { + case b3_capsuleShape: + return b3Lerp( shape->capsule.center1, shape->capsule.center2, 0.5f ); + case b3_compoundShape: + { + b3AABB aabb = b3ComputeCompoundAABB( shape->compound, b3Transform_identity ); + return b3AABB_Center( aabb ); + } + case b3_sphereShape: + return shape->sphere.center; + case b3_hullShape: + return shape->hull->center; + case b3_meshShape: + { + b3AABB aabb = b3ComputeMeshAABB( shape->mesh.data, b3Transform_identity, shape->mesh.scale ); + return b3AABB_Center( aabb ); + } + case b3_heightShape: + { + b3AABB aabb = b3ComputeHeightFieldAABB( shape->heightField, b3Transform_identity ); + return b3AABB_Center( aabb ); + } + default: + return b3Vec3_zero; + } +} + +float b3GetShapeArea( const b3Shape* shape ) +{ + // todo_erin fix these + switch ( shape->type ) + { + case b3_capsuleShape: + return 2.0f * b3Length( b3Sub( shape->capsule.center1, shape->capsule.center2 ) ) + + 2.0f * B3_PI * shape->capsule.radius; + + case b3_hullShape: + return shape->hull->surfaceArea; + + case b3_sphereShape: + return 2.0f * B3_PI * shape->sphere.radius; + + default: + return 0.0f; + } +} + +// This projects the shape surface area onto a plane +float b3GetShapeProjectedArea( const b3Shape* shape, b3Vec3 planeNormal ) +{ + switch ( shape->type ) + { + case b3_capsuleShape: + { + float radius = shape->capsule.radius; + b3Vec3 axis = b3Sub( shape->capsule.center2, shape->capsule.center1 ); + float projectedLength = b3Length( b3Cross( axis, planeNormal ) ); + float cylinderArea = 2.0f * radius * projectedLength; + float sphereArea = B3_PI * radius * radius; + return sphereArea + cylinderArea; + } + + case b3_hullShape: + return b3ComputeHullProjectedArea( shape->hull, planeNormal ); + + case b3_sphereShape: + return B3_PI * shape->sphere.radius * shape->sphere.radius; + + default: + return 0.0f; + } +} + +b3MassData b3ComputeShapeMass( const b3Shape* shape ) +{ + switch ( shape->type ) + { + case b3_capsuleShape: + return b3ComputeCapsuleMass( &shape->capsule, shape->density ); + + case b3_hullShape: + return b3ComputeHullMass( shape->hull, shape->density ); + + case b3_sphereShape: + return b3ComputeSphereMass( &shape->sphere, shape->density ); + + default: + return (b3MassData){ 0 }; + } +} + +b3ShapeExtent b3ComputeShapeExtent( const b3Shape* shape, b3Vec3 localCenter ) +{ + b3ShapeExtent extent = { 0 }; + + switch ( shape->type ) + { + case b3_capsuleShape: + { + float radius = shape->capsule.radius; + extent.minExtent = radius; + b3Vec3 c1 = b3Sub( shape->capsule.center1, localCenter ); + b3Vec3 c2 = b3Sub( shape->capsule.center2, localCenter ); + b3Vec3 r = { radius, radius, radius }; + extent.maxExtent = b3Add( b3Max( c1, c2 ), r ); + } + break; + + case b3_compoundShape: + { + // This is shouldn't be needed but here for completeness + b3AABB aabb = b3ComputeCompoundAABB( shape->compound, b3Transform_identity ); + float r1 = b3Length( b3Sub( aabb.lowerBound, localCenter ) ); + float r2 = b3Length( b3Sub( aabb.upperBound, localCenter ) ); + extent.minExtent = b3MinFloat( r1, r2 ); + b3Vec3 p = b3FarthestPointOnAABB( aabb, localCenter ); + extent.maxExtent = b3Abs( b3Sub( p, localCenter ) ); + } + break; + + case b3_sphereShape: + { + float radius = shape->sphere.radius; + extent.minExtent = radius; + b3Vec3 r = { radius, radius, radius }; + b3Vec3 p = b3Add( b3Sub( shape->sphere.center, localCenter ), r ); + extent.maxExtent = b3Abs( b3Sub( p, localCenter ) ); + } + break; + + case b3_hullShape: + extent = b3ComputeHullExtent( shape->hull, localCenter ); + break; + + case b3_meshShape: + { + // This is needed for kinematic mesh sleeping + b3AABB aabb = b3ComputeMeshAABB( shape->mesh.data, b3Transform_identity, shape->mesh.scale ); + float r1 = b3Length( b3Sub( aabb.lowerBound, localCenter ) ); + float r2 = b3Length( b3Sub( aabb.upperBound, localCenter ) ); + extent.minExtent = b3MinFloat( r1, r2 ); + b3Vec3 p = b3FarthestPointOnAABB( aabb, localCenter ); + extent.maxExtent = b3Abs( p ); + } + break; + + default: + break; + } + + return extent; +} + +b3CastOutput b3RayCastShape( const b3Shape* shape, b3Transform transform, const b3RayCastInput* input ) +{ + b3RayCastInput localInput = *input; + localInput.origin = b3InvTransformPoint( transform, input->origin ); + localInput.translation = b3InvRotateVector( transform.q, input->translation ); + + b3CastOutput output = { 0 }; + switch ( shape->type ) + { + case b3_capsuleShape: + output = b3RayCastCapsule( &shape->capsule, &localInput ); + break; + case b3_compoundShape: + output = b3RayCastCompound( shape->compound, &localInput ); + break; + case b3_sphereShape: + output = b3RayCastSphere( &shape->sphere, &localInput ); + break; + case b3_hullShape: + output = b3RayCastHull( shape->hull, &localInput ); + break; + case b3_meshShape: + output = b3RayCastMesh( &shape->mesh, &localInput ); + break; + case b3_heightShape: + output = b3RayCastHeightField( shape->heightField, &localInput ); + break; + default: + return output; + } + + output.point = b3TransformPoint( transform, output.point ); + output.normal = b3RotateVector( transform.q, output.normal ); + return output; +} + +b3CastOutput b3ShapeCastShape( const b3Shape* shape, b3Transform transform, const b3ShapeCastInput* input ) +{ + b3ShapeCastInput localInput = *input; + b3Vec3 localPoints[B3_MAX_SHAPE_CAST_POINTS]; + + localInput.proxy.count = b3MinInt( input->proxy.count, B3_MAX_SHAPE_CAST_POINTS ); + for ( int i = 0; i < localInput.proxy.count; ++i ) + { + localPoints[i] = b3InvTransformPoint( transform, input->proxy.points[i] ); + } + + localInput.proxy.points = localPoints; + localInput.translation = b3InvRotateVector( transform.q, input->translation ); + + b3CastOutput output = { 0 }; + switch ( shape->type ) + { + case b3_capsuleShape: + output = b3ShapeCastCapsule( &shape->capsule, &localInput ); + break; + + case b3_compoundShape: + output = b3ShapeCastCompound( shape->compound, &localInput ); + break; + + case b3_heightShape: + output = b3ShapeCastHeightField( shape->heightField, &localInput ); + break; + + case b3_hullShape: + output = b3ShapeCastHull( shape->hull, &localInput ); + break; + + case b3_meshShape: + output = b3ShapeCastMesh( &shape->mesh, &localInput ); + break; + + case b3_sphereShape: + output = b3ShapeCastSphere( &shape->sphere, &localInput ); + break; + default: + return output; + } + + output.point = b3TransformPoint( transform, output.point ); + output.normal = b3RotateVector( transform.q, output.normal ); + return output; +} + +bool b3OverlapShape( const b3Shape* shape, b3Transform transform, const b3ShapeProxy* proxy ) +{ + b3ShapeType type = shape->type; + switch ( type ) + { + case b3_capsuleShape: + return b3OverlapCapsule( &shape->capsule, transform, proxy ); + + case b3_compoundShape: + return b3OverlapCompound( shape->compound, transform, proxy ); + + case b3_heightShape: + return b3OverlapHeightField( shape->heightField, transform, proxy ); + + case b3_hullShape: + return b3OverlapHull( shape->hull, transform, proxy ); + + case b3_meshShape: + return b3OverlapMesh( &shape->mesh, transform, proxy ); + + case b3_sphereShape: + return b3OverlapSphere( &shape->sphere, transform, proxy ); + + default: + B3_ASSERT( false ); + return false; + } + +#if 0 + b3Vec3 localPoints[B3_MAX_SHAPE_CAST_POINTS]; + b3ShapeProxy localProxy; + + b3Transform invTransform = b3InvertTransform( transform ); + b3Matrix3 R = b3MakeMatrixFromQuat( invTransform.q ); + + localProxy.count = b3MinInt( proxy->count, B3_MAX_SHAPE_CAST_POINTS ); + for ( int i = 0; i < localProxy.count; ++i ) + { + localPoints[i] = b3Add( b3MulMV( R, proxy->points[i] ), invTransform.p ); + } + + localProxy.points = localPoints; + localProxy.radius = proxy->radius; + + if ( type == b3_meshShape ) + { + return b3OverlapMesh( &localProxy, shape->mesh.data, shape->mesh.scale ); + } + + B3_ASSERT( type == b3_heightShape ); + + return b3OverlapHeightField( &localProxy, shape->heightField ); +#endif +} + +int b3CollideMover( b3PlaneResult* planes, int planeCapacity, const b3Shape* shape, b3Transform transform, + const b3Capsule* mover ) +{ + if ( planeCapacity == 0 ) + { + return 0; + } + + b3Capsule localMover; + localMover.center1 = b3InvTransformPoint( transform, mover->center1 ); + localMover.center2 = b3InvTransformPoint( transform, mover->center2 ); + localMover.radius = mover->radius; + + int planeCount = 0; + switch ( shape->type ) + { + case b3_capsuleShape: + planeCount = b3CollideMoverAndCapsule( planes, &shape->capsule, &localMover ); + break; + + case b3_compoundShape: + planeCount = b3CollideMoverAndCompound( planes, planeCapacity, shape->compound, &localMover ); + break; + + case b3_sphereShape: + planeCount = b3CollideMoverAndSphere( planes, &shape->sphere, &localMover ); + break; + + case b3_hullShape: + planeCount = b3CollideMoverAndHull( planes, shape->hull, &localMover ); + break; + + case b3_meshShape: + planeCount = b3CollideMoverAndMesh( planes, planeCapacity, &shape->mesh, &localMover ); + break; + + case b3_heightShape: + planeCount = b3CollideMoverAndHeightField( planes, planeCapacity, shape->heightField, &localMover ); + break; + + default: + B3_ASSERT( false ); + break; + } + + for ( int i = 0; i < planeCount; ++i ) + { + planes[i].plane.normal = b3RotateVector( transform.q, planes[i].plane.normal ); + planes[i].point = b3TransformPoint( transform, planes[i].point ); + } + + return planeCount; +} + +void b3CreateShapeProxy( b3Shape* shape, b3BroadPhase* bp, b3BodyType type, b3WorldTransform transform, bool forcePairCreation ) +{ + B3_ASSERT( shape->proxyKey == B3_NULL_INDEX ); + + b3UpdateShapeAABBs( shape, transform, type ); + + // Create proxies in the broad-phase. + shape->proxyKey = + b3BroadPhase_CreateProxy( bp, type, shape->fatAABB, shape->filter.categoryBits, shape->id, forcePairCreation ); + B3_ASSERT( B3_PROXY_TYPE( shape->proxyKey ) < b3_bodyTypeCount ); +} + +void b3DestroyShapeProxy( b3Shape* shape, b3BroadPhase* bp ) +{ + if ( shape->proxyKey != B3_NULL_INDEX ) + { + b3BroadPhase_DestroyProxy( bp, shape->proxyKey ); + shape->proxyKey = B3_NULL_INDEX; + } +} + +static void b3DestroyShapeAllocationForShapeChange( b3World* world, b3Shape* shape ) +{ + b3ShapeType type = shape->type; + switch ( type ) + { + case b3_hullShape: + b3RemoveHullFromDatabase( world, shape->hull ); + shape->hull = NULL; + break; + + default: + break; + } + + if ( shape->userShape != NULL ) + { + world->destroyDebugShape( shape->userShape, world->userDebugShapeContext ); + shape->userShape = NULL; + } +} + +void b3DestroyShapeAllocations( b3World* world, b3Shape* shape ) +{ + b3DestroyShapeAllocationForShapeChange( world, shape ); + + if ( shape->materials != NULL ) + { + B3_ASSERT( shape->materialCount > 0 ); + b3Free( shape->materials, shape->materialCount * sizeof( b3SurfaceMaterial ) ); + shape->materials = NULL; + shape->materialCount = 0; + } + + // Name is stored inline. Sensor data is destroyed elsewhere +} + +b3ShapeProxy b3MakeShapeProxy( const b3Shape* shape ) +{ + switch ( shape->type ) + { + case b3_capsuleShape: + return (b3ShapeProxy){ &shape->capsule.center1, 2, shape->capsule.radius }; + + case b3_sphereShape: + return (b3ShapeProxy){ &shape->sphere.center, 1, shape->sphere.radius }; + + case b3_hullShape: + { + const b3HullData* hull = shape->hull; + const b3Vec3* points = b3GetHullPoints( hull ); + return (b3ShapeProxy){ points, hull->vertexCount, 0.0f }; + } + + default: + { + B3_ASSERT( false ); + return (b3ShapeProxy){ 0 }; + } + } +} + +b3ShapeProxy b3MakeLocalProxy( const b3ShapeProxy* proxy, b3Transform transform, b3Vec3* buffer ) +{ + b3Transform invTransform = b3InvertTransform( transform ); + b3Matrix3 R = b3MakeMatrixFromQuat( invTransform.q ); + + int count = b3MinInt( proxy->count, B3_MAX_SHAPE_CAST_POINTS ); + for ( int i = 0; i < count; ++i ) + { + buffer[i] = b3Add( b3MulMV( R, proxy->points[i] ), invTransform.p ); + } + + return (b3ShapeProxy){ + .points = buffer, + .count = count, + .radius = proxy->radius, + }; +} + +b3AABB b3ComputeProxyAABB( const b3ShapeProxy* proxy ) +{ + const b3Vec3* points = proxy->points; + b3AABB aabb = { + .lowerBound = points[0], + .upperBound = points[0], + }; + + for ( int i = 1; i < proxy->count; ++i ) + { + aabb.lowerBound = b3Min( aabb.lowerBound, points[i] ); + aabb.upperBound = b3Max( aabb.upperBound, points[i] ); + } + + b3Vec3 r = { proxy->radius, proxy->radius, proxy->radius }; + aabb.lowerBound = b3Sub( aabb.lowerBound, r ); + aabb.upperBound = b3Add( aabb.upperBound, r ); + return aabb; +} + +b3BodyId b3Shape_GetBody( b3ShapeId shapeId ) +{ + b3World* world = b3GetWorld( shapeId.world0 ); + b3Shape* shape = b3GetShape( world, shapeId ); + return b3MakeBodyId( world, shape->bodyId ); +} + +b3WorldId b3Shape_GetWorld( b3ShapeId shapeId ) +{ + b3World* world = b3GetWorld( shapeId.world0 ); + return (b3WorldId){ (uint16_t)( shapeId.world0 + 1 ), world->generation }; +} + +void b3Shape_SetUserData( b3ShapeId shapeId, void* userData ) +{ + b3World* world = b3GetWorld( shapeId.world0 ); + b3Shape* shape = b3GetShape( world, shapeId ); + shape->userData = userData; +} + +void* b3Shape_GetUserData( b3ShapeId shapeId ) +{ + b3World* world = b3GetWorld( shapeId.world0 ); + b3Shape* shape = b3GetShape( world, shapeId ); + return shape->userData; +} + +void b3Shape_SetName( b3ShapeId shapeId, const char* name ) +{ + b3World* world = b3GetWorld( shapeId.world0 ); + + B3_REC( world, ShapeSetName, shapeId, name ); + + b3Shape* shape = b3GetShape( world, shapeId ); + shape->nameId = b3AddName( &world->names, name ); +} + +const char* b3Shape_GetName( b3ShapeId shapeId ) +{ + b3World* world = b3GetWorld( shapeId.world0 ); + b3Shape* shape = b3GetShape( world, shapeId ); + return b3FindNameWithDefault( &world->names, shape->nameId, "" ); +} + +bool b3Shape_IsSensor( b3ShapeId shapeId ) +{ + b3World* world = b3GetWorld( shapeId.world0 ); + b3Shape* shape = b3GetShape( world, shapeId ); + return shape->sensorIndex != B3_NULL_INDEX; +} + +// todo no tests +b3WorldCastOutput b3Shape_RayCast( b3ShapeId shapeId, b3Pos origin, b3Vec3 translation ) +{ + B3_ASSERT( b3IsValidPosition( origin ) ); + B3_ASSERT( b3IsValidVec3( translation ) ); + + b3World* world = b3GetWorld( shapeId.world0 ); + b3Shape* shape = b3GetShape( world, shapeId ); + + // Re-center on the origin so the cast runs in float precision far from the world origin + b3Transform transform = b3ToRelativeTransform( b3GetBodyTransform( world, shape->bodyId ), origin ); + + // The ray starts at the origin, so its origin in the re-centered frame is zero + b3RayCastInput input = { b3Vec3_zero, translation, 1.0f }; + + // Lift the re-centered float result back to a world position + b3CastOutput local = b3RayCastShape( shape, transform, &input ); + b3WorldCastOutput output; + output.normal = local.normal; + output.point = b3OffsetPos( origin, local.point ); + output.fraction = local.fraction; + output.iterations = local.iterations; + output.triangleIndex = local.triangleIndex; + output.childIndex = local.childIndex; + output.materialIndex = local.materialIndex; + output.hit = local.hit; + + return output; +} + +void b3Shape_SetDensity( b3ShapeId shapeId, float density, bool updateBodyMass ) +{ + B3_ASSERT( b3IsValidFloat( density ) && density >= 0.0f ); + + b3World* world = b3GetUnlockedWorld( shapeId.world0 ); + if ( world == NULL ) + { + return; + } + + B3_REC( world, ShapeSetDensity, shapeId, density, updateBodyMass ); + + b3Shape* shape = b3GetShape( world, shapeId ); + if ( density == shape->density ) + { + // early return to avoid expensive function + return; + } + + shape->density = density; + + if ( updateBodyMass == true ) + { + b3Body* body = b3Array_Get( world->bodies, shape->bodyId ); + b3UpdateBodyMassData( world, body ); + } +} + +float b3Shape_GetDensity( b3ShapeId shapeId ) +{ + b3World* world = b3GetWorld( shapeId.world0 ); + b3Shape* shape = b3GetShape( world, shapeId ); + return shape->density; +} + +void b3Shape_SetFriction( b3ShapeId shapeId, float friction ) +{ + B3_ASSERT( b3IsValidFloat( friction ) && friction >= 0.0f ); + b3World* world = b3GetWorld( shapeId.world0 ); + B3_REC( world, ShapeSetFriction, shapeId, friction ); + b3Shape* shape = b3GetShape( world, shapeId ); + B3_ASSERT( shape->type != b3_compoundShape ); + b3GetShapeMaterials( shape )[0].friction = friction; +} + +float b3Shape_GetFriction( b3ShapeId shapeId ) +{ + b3World* world = b3GetWorld( shapeId.world0 ); + b3Shape* shape = b3GetShape( world, shapeId ); + return b3GetShapeMaterials( shape )[0].friction; +} + +void b3Shape_SetRestitution( b3ShapeId shapeId, float restitution ) +{ + B3_ASSERT( b3IsValidFloat( restitution ) && restitution >= 0.0f ); + b3World* world = b3GetWorld( shapeId.world0 ); + B3_REC( world, ShapeSetRestitution, shapeId, restitution ); + b3Shape* shape = b3GetShape( world, shapeId ); + B3_ASSERT( shape->type != b3_compoundShape ); + b3GetShapeMaterials( shape )[0].restitution = restitution; +} + +float b3Shape_GetRestitution( b3ShapeId shapeId ) +{ + b3World* world = b3GetWorld( shapeId.world0 ); + b3Shape* shape = b3GetShape( world, shapeId ); + return b3GetShapeMaterials( shape )[0].restitution; +} + +void b3Shape_SetSurfaceMaterial( b3ShapeId shapeId, b3SurfaceMaterial surfaceMaterial ) +{ + B3_ASSERT( b3IsValidFloat( surfaceMaterial.friction ) && surfaceMaterial.friction >= 0.0f ); + B3_ASSERT( b3IsValidFloat( surfaceMaterial.restitution ) && surfaceMaterial.restitution >= 0.0f ); + B3_ASSERT( b3IsValidFloat( surfaceMaterial.rollingResistance ) && surfaceMaterial.rollingResistance >= 0.0f ); + B3_ASSERT( b3IsValidVec3( surfaceMaterial.tangentVelocity ) ); + + b3World* world = b3GetWorld( shapeId.world0 ); + B3_REC( world, ShapeSetSurfaceMaterial, shapeId, surfaceMaterial ); + b3Shape* shape = b3GetShape( world, shapeId ); + B3_ASSERT( shape->type != b3_compoundShape ); + b3GetShapeMaterials( shape )[0] = surfaceMaterial; +} + +b3SurfaceMaterial b3Shape_GetSurfaceMaterial( b3ShapeId shapeId ) +{ + b3World* world = b3GetWorld( shapeId.world0 ); + b3Shape* shape = b3GetShape( world, shapeId ); + return b3GetShapeMaterials( shape )[0]; +} + +int b3Shape_GetMeshMaterialCount( b3ShapeId shapeId ) +{ + b3World* world = b3GetWorld( shapeId.world0 ); + b3Shape* shape = b3GetShape( world, shapeId ); + return shape->materialCount; +} + +void b3Shape_SetMeshMaterial( b3ShapeId shapeId, b3SurfaceMaterial surfaceMaterial, int index ) +{ + B3_ASSERT( b3IsValidFloat( surfaceMaterial.friction ) && surfaceMaterial.friction >= 0.0f ); + B3_ASSERT( b3IsValidFloat( surfaceMaterial.restitution ) && surfaceMaterial.restitution >= 0.0f ); + B3_ASSERT( b3IsValidFloat( surfaceMaterial.rollingResistance ) && surfaceMaterial.rollingResistance >= 0.0f ); + B3_ASSERT( b3IsValidVec3( surfaceMaterial.tangentVelocity ) ); + + b3World* world = b3GetWorld( shapeId.world0 ); + b3Shape* shape = b3GetShape( world, shapeId ); + + B3_ASSERT( 0 <= index && index < shape->materialCount ); + B3_ASSERT( shape->type != b3_compoundShape ); + b3GetShapeMaterials( shape )[index] = surfaceMaterial; +} + +b3SurfaceMaterial b3Shape_GetMeshSurfaceMaterial( b3ShapeId shapeId, int index ) +{ + b3World* world = b3GetWorld( shapeId.world0 ); + b3Shape* shape = b3GetShape( world, shapeId ); + B3_ASSERT( 0 <= index && index < shape->materialCount ); + return b3GetShapeMaterials( shape )[index]; +} + +b3Filter b3Shape_GetFilter( b3ShapeId shapeId ) +{ + b3World* world = b3GetWorld( shapeId.world0 ); + b3Shape* shape = b3GetShape( world, shapeId ); + return shape->filter; +} + +static void b3ResetProxy( b3World* world, b3Shape* shape, bool wakeBodies, bool destroyProxy ) +{ + b3Body* body = b3Array_Get( world->bodies, shape->bodyId ); + + int shapeId = shape->id; + + // destroy all contacts associated with this shape + int contactKey = body->headContactKey; + while ( contactKey != B3_NULL_INDEX ) + { + int contactId = contactKey >> 1; + int edgeIndex = contactKey & 1; + + b3Contact* contact = b3Array_Get( world->contacts, contactId ); + contactKey = contact->edges[edgeIndex].nextKey; + + if ( contact->shapeIdA == shapeId || contact->shapeIdB == shapeId ) + { + b3DestroyContact( world, contact, wakeBodies ); + } + } + + b3WorldTransform transform = b3GetBodyTransformQuick( world, body ); + if ( shape->proxyKey != B3_NULL_INDEX ) + { + b3BodyType proxyType = B3_PROXY_TYPE( shape->proxyKey ); + b3UpdateShapeAABBs( shape, transform, proxyType ); + + if ( destroyProxy ) + { + b3BroadPhase_DestroyProxy( &world->broadPhase, shape->proxyKey ); + + bool forcePairCreation = true; + shape->proxyKey = b3BroadPhase_CreateProxy( &world->broadPhase, proxyType, shape->fatAABB, shape->filter.categoryBits, + shapeId, forcePairCreation ); + } + else + { + b3BroadPhase_MoveProxy( &world->broadPhase, shape->proxyKey, shape->fatAABB ); + } + } + else + { + b3BodyType proxyType = body->type; + b3UpdateShapeAABBs( shape, transform, proxyType ); + } + + b3ValidateSolverSets( world ); +} + +void b3Shape_SetFilter( b3ShapeId shapeId, b3Filter filter, bool invokeContacts ) +{ + b3World* world = b3GetUnlockedWorld( shapeId.world0 ); + if ( world == NULL ) + { + return; + } + + B3_REC( world, ShapeSetFilter, shapeId, filter, invokeContacts ); + + b3Shape* shape = b3GetShape( world, shapeId ); + if ( filter.maskBits == shape->filter.maskBits && filter.categoryBits == shape->filter.categoryBits && + filter.groupIndex == shape->filter.groupIndex ) + { + return; + } + + shape->filter = filter; + + if ( invokeContacts ) + { + world->locked = true; + bool wakeBodies = true; + + // If the category bits change, I need to destroy the proxy because it affects the tree sorting. + bool destroyProxy = filter.categoryBits == shape->filter.categoryBits; + + // need to wake bodies because a filter change may destroy contacts + b3ResetProxy( world, shape, wakeBodies, destroyProxy ); + world->locked = false; + } + + // note: this does not immediately update sensor overlaps. Instead sensor + // overlaps are updated the next time step +} + +void b3Shape_EnableSensorEvents( b3ShapeId shapeId, bool flag ) +{ + b3World* world = b3GetUnlockedWorld( shapeId.world0 ); + if ( world == NULL ) + { + return; + } + + B3_REC( world, ShapeEnableSensorEvents, shapeId, flag ); + + b3Shape* shape = b3GetShape( world, shapeId ); + shape->flags = flag ? shape->flags | b3_enableSensorEvents : shape->flags & ~b3_enableSensorEvents; +} + +bool b3Shape_AreSensorEventsEnabled( b3ShapeId shapeId ) +{ + b3World* world = b3GetWorld( shapeId.world0 ); + b3Shape* shape = b3GetShape( world, shapeId ); + return shape->flags & b3_enableSensorEvents; +} + +void b3Shape_EnableContactEvents( b3ShapeId shapeId, bool flag ) +{ + b3World* world = b3GetUnlockedWorld( shapeId.world0 ); + if ( world == NULL ) + { + return; + } + + B3_REC( world, ShapeEnableContactEvents, shapeId, flag ); + + b3Shape* shape = b3GetShape( world, shapeId ); + shape->flags = flag ? shape->flags | b3_enableContactEvents : shape->flags & ~b3_enableContactEvents; +} + +bool b3Shape_AreContactEventsEnabled( b3ShapeId shapeId ) +{ + b3World* world = b3GetWorld( shapeId.world0 ); + b3Shape* shape = b3GetShape( world, shapeId ); + return shape->flags & b3_enableContactEvents; +} + +void b3Shape_EnablePreSolveEvents( b3ShapeId shapeId, bool flag ) +{ + b3World* world = b3GetUnlockedWorld( shapeId.world0 ); + if ( world == NULL ) + { + return; + } + + B3_REC( world, ShapeEnablePreSolveEvents, shapeId, flag ); + + b3Shape* shape = b3GetShape( world, shapeId ); + shape->flags = flag ? shape->flags | b3_enablePreSolveEvents : shape->flags & ~b3_enablePreSolveEvents; +} + +bool b3Shape_ArePreSolveEventsEnabled( b3ShapeId shapeId ) +{ + b3World* world = b3GetWorld( shapeId.world0 ); + b3Shape* shape = b3GetShape( world, shapeId ); + return shape->flags & b3_enablePreSolveEvents; +} + +void b3Shape_EnableHitEvents( b3ShapeId shapeId, bool flag ) +{ + b3World* world = b3GetUnlockedWorld( shapeId.world0 ); + if ( world == NULL ) + { + return; + } + + B3_REC( world, ShapeEnableHitEvents, shapeId, flag ); + + b3Shape* shape = b3GetShape( world, shapeId ); + shape->flags = flag ? shape->flags | b3_enableHitEvents : shape->flags & ~b3_enableHitEvents; +} + +bool b3Shape_AreHitEventsEnabled( b3ShapeId shapeId ) +{ + b3World* world = b3GetWorld( shapeId.world0 ); + b3Shape* shape = b3GetShape( world, shapeId ); + return shape->flags & b3_enableHitEvents; +} + +b3ShapeType b3Shape_GetType( b3ShapeId shapeId ) +{ + b3World* world = b3GetWorld( shapeId.world0 ); + b3Shape* shape = b3GetShape( world, shapeId ); + return shape->type; +} + +b3Sphere b3Shape_GetSphere( b3ShapeId shapeId ) +{ + b3World* world = b3GetWorld( shapeId.world0 ); + b3Shape* shape = b3GetShape( world, shapeId ); + B3_ASSERT( shape->type == b3_sphereShape ); + return shape->sphere; +} + +b3Capsule b3Shape_GetCapsule( b3ShapeId shapeId ) +{ + b3World* world = b3GetWorld( shapeId.world0 ); + b3Shape* shape = b3GetShape( world, shapeId ); + B3_ASSERT( shape->type == b3_capsuleShape ); + return shape->capsule; +} + +const b3HullData* b3Shape_GetHull( b3ShapeId shapeId ) +{ + b3World* world = b3GetWorld( shapeId.world0 ); + b3Shape* shape = b3GetShape( world, shapeId ); + B3_ASSERT( shape->type == b3_hullShape ); + return shape->hull; +} + +b3Mesh b3Shape_GetMesh( b3ShapeId shapeId ) +{ + b3World* world = b3GetWorld( shapeId.world0 ); + b3Shape* shape = b3GetShape( world, shapeId ); + B3_ASSERT( shape->type == b3_meshShape ); + return shape->mesh; +} + +const b3HeightFieldData* b3Shape_GetHeightField( b3ShapeId shapeId ) +{ + b3World* world = b3GetWorld( shapeId.world0 ); + b3Shape* shape = b3GetShape( world, shapeId ); + B3_ASSERT( shape->type == b3_heightShape ); + return shape->heightField; +} + +void b3Shape_SetSphere( b3ShapeId shapeId, const b3Sphere* sphere ) +{ + b3World* world = b3GetUnlockedWorld( shapeId.world0 ); + if ( world == NULL ) + { + return; + } + + B3_REC( world, ShapeSetSphere, shapeId, *sphere ); + + world->locked = true; + + b3Shape* shape = b3GetShape( world, shapeId ); + + b3DestroyShapeAllocationForShapeChange( world, shape ); + + shape->sphere = *sphere; + shape->type = b3_sphereShape; + shape->aabbMargin = b3ComputeShapeMargin( shape ); + + // need to wake bodies so they can react to the shape change + bool wakeBodies = true; + bool destroyProxy = true; + b3ResetProxy( world, shape, wakeBodies, destroyProxy ); + + world->locked = false; +} + +void b3Shape_SetCapsule( b3ShapeId shapeId, const b3Capsule* capsule ) +{ + b3World* world = b3GetUnlockedWorld( shapeId.world0 ); + if ( world == NULL ) + { + return; + } + + B3_REC( world, ShapeSetCapsule, shapeId, *capsule ); + + world->locked = true; + + b3Shape* shape = b3GetShape( world, shapeId ); + + b3DestroyShapeAllocationForShapeChange( world, shape ); + + shape->capsule = *capsule; + shape->type = b3_capsuleShape; + shape->aabbMargin = b3ComputeShapeMargin( shape ); + + // need to wake bodies so they can react to the shape change + bool wakeBodies = true; + bool destroyProxy = true; + b3ResetProxy( world, shape, wakeBodies, destroyProxy ); + + world->locked = false; +} + +void b3Shape_SetHull( b3ShapeId shapeId, const b3HullData* hull ) +{ + B3_VALIDATE( b3IsValidHull( hull ) ); + B3_VALIDATE( hull->hash != 0 ); + + b3World* world = b3GetUnlockedWorld( shapeId.world0 ); + if ( world == NULL ) + { + return; + } + + world->locked = true; + + b3Shape* shape = b3GetShape( world, shapeId ); + + // Acquire the new hull before releasing the old so the input may safely alias + // the shape's current shared data. + const b3HullData* data = b3AddHullToDatabase( world, hull ); + + // Same shared hull, avoid destroying contacts and recreating the proxy + if ( shape->type == b3_hullShape && data == shape->hull ) + { + b3RemoveHullFromDatabase( world, data ); + world->locked = false; + return; + } + + b3DestroyShapeAllocationForShapeChange( world, shape ); + + shape->hull = data; + shape->type = b3_hullShape; + shape->aabbMargin = b3ComputeShapeMargin( shape ); + + // need to wake bodies so they can react to the shape change + bool wakeBodies = true; + bool destroyProxy = true; + b3ResetProxy( world, shape, wakeBodies, destroyProxy ); + + world->locked = false; +} + +void b3Shape_SetMesh( b3ShapeId shapeId, const b3MeshData* meshData, b3Vec3 scale ) +{ + B3_ASSERT( b3IsValidVec3( scale ) ); + B3_ASSERT( meshData != NULL && b3IsValidMesh( meshData ) ); + + b3World* world = b3GetUnlockedWorld( shapeId.world0 ); + if ( world == NULL ) + { + return; + } + + world->locked = true; + + b3Shape* shape = b3GetShape( world, shapeId ); + + b3DestroyShapeAllocationForShapeChange( world, shape ); + + shape->mesh.data = meshData; + shape->mesh.scale = b3SafeScale( scale ); + shape->type = b3_meshShape; + shape->aabbMargin = b3ComputeShapeMargin( shape ); + + // need to wake bodies so they can react to the shape change + bool wakeBodies = true; + bool destroyProxy = true; + b3ResetProxy( world, shape, wakeBodies, destroyProxy ); + + world->locked = false; +} + +int b3Shape_GetContactCapacity( b3ShapeId shapeId ) +{ + b3World* world = b3GetUnlockedWorld( shapeId.world0 ); + if ( world == NULL ) + { + return 0; + } + + b3Shape* shape = b3GetShape( world, shapeId ); + if ( shape->sensorIndex != B3_NULL_INDEX ) + { + return 0; + } + + b3Body* body = b3Array_Get( world->bodies, shape->bodyId ); + + // Conservative and fast + return body->contactCount; +} + +int b3Shape_GetContactData( b3ShapeId shapeId, b3ContactData* contactData, int capacity ) +{ + b3World* world = b3GetUnlockedWorld( shapeId.world0 ); + if ( world == NULL ) + { + return 0; + } + + b3Shape* shape = b3GetShape( world, shapeId ); + if ( shape->sensorIndex != B3_NULL_INDEX ) + { + return 0; + } + + b3Body* body = b3Array_Get( world->bodies, shape->bodyId ); + int contactKey = body->headContactKey; + int index = 0; + while ( contactKey != B3_NULL_INDEX && index < capacity ) + { + int contactId = contactKey >> 1; + int edgeIndex = contactKey & 1; + + b3Contact* contact = b3Array_Get( world->contacts, contactId ); + + // Does contact involve this shape and is it touching? + if ( ( contact->shapeIdA == shapeId.index1 - 1 || contact->shapeIdB == shapeId.index1 - 1 ) && + ( contact->flags & b3_contactTouchingFlag ) != 0 ) + { + b3Shape* shapeA = world->shapes.data + contact->shapeIdA; + b3Shape* shapeB = world->shapes.data + contact->shapeIdB; + + contactData[index].contactId = (b3ContactId){ contact->contactId + 1, shapeId.world0, 0, contact->generation }; + contactData[index].shapeIdA = (b3ShapeId){ shapeA->id + 1, shapeId.world0, shapeA->generation }; + contactData[index].shapeIdB = (b3ShapeId){ shapeB->id + 1, shapeId.world0, shapeB->generation }; + contactData[index].manifolds = contact->manifolds; + contactData[index].manifoldCount = contact->manifoldCount; + index += 1; + } + + contactKey = contact->edges[edgeIndex].nextKey; + } + + B3_ASSERT( index <= capacity ); + + return index; +} + +int b3Shape_GetSensorCapacity( b3ShapeId shapeId ) +{ + b3World* world = b3GetUnlockedWorld( shapeId.world0 ); + if ( world == NULL ) + { + return 0; + } + + b3Shape* shape = b3GetShape( world, shapeId ); + if ( shape->sensorIndex == B3_NULL_INDEX ) + { + return 0; + } + + b3Sensor* sensor = b3Array_Get( world->sensors, shape->sensorIndex ); + return sensor->overlaps2.count; +} + +int b3Shape_GetSensorData( b3ShapeId shapeId, b3ShapeId* visitorIds, int capacity ) +{ + b3World* world = b3GetUnlockedWorld( shapeId.world0 ); + if ( world == NULL ) + { + return 0; + } + + b3Shape* shape = b3GetShape( world, shapeId ); + if ( shape->sensorIndex == B3_NULL_INDEX ) + { + return 0; + } + + b3Sensor* sensor = b3Array_Get( world->sensors, shape->sensorIndex ); + + int count = b3MinInt( sensor->overlaps2.count, capacity ); + b3Visitor* refs = sensor->overlaps2.data; + for ( int i = 0; i < count; ++i ) + { + b3ShapeId visitorId = { + .index1 = refs[i].shapeId + 1, + .world0 = shapeId.world0, + .generation = refs[i].generation, + }; + + visitorIds[i] = visitorId; + } + + return count; +} + +b3AABB b3Shape_GetAABB( b3ShapeId shapeId ) +{ + b3World* world = b3GetWorld( shapeId.world0 ); + if ( world == NULL ) + { + return (b3AABB){ 0 }; + } + + b3Shape* shape = b3GetShape( world, shapeId ); + return shape->aabb; +} + +b3MassData b3Shape_ComputeMassData( b3ShapeId shapeId ) +{ + b3World* world = b3GetWorld( shapeId.world0 ); + if ( world == NULL ) + { + return (b3MassData){ 0 }; + } + + b3Shape* shape = b3GetShape( world, shapeId ); + return b3ComputeShapeMass( shape ); +} + +b3Vec3 b3Shape_GetClosestPoint( b3ShapeId shapeId, b3Vec3 target ) +{ + b3World* world = b3GetWorld( shapeId.world0 ); + if ( world == NULL ) + { + return b3Vec3_zero; + } + + b3Shape* shape = b3GetShape( world, shapeId ); + b3Body* body = b3Array_Get( world->bodies, shape->bodyId ); + // Low level closest point query is a documented float carve-out far from the origin + b3Transform transform = b3ToRelativeTransform( b3GetBodyTransformQuick( world, body ), b3Pos_zero ); + + b3DistanceInput input; + input.proxyA = b3MakeShapeProxy( shape ); + input.proxyB = (b3ShapeProxy){ &target, 1, 0.0f }; + input.transform = b3InvMulTransforms( transform, b3Transform_identity ); + input.useRadii = true; + + b3SimplexCache cache = { 0 }; + b3DistanceOutput output = b3ShapeDistance( &input, &cache, NULL, 0 ); + + // Witness point comes back in frame A, lift it back to the query frame + return b3TransformPoint( transform, output.pointA ); +} + +#define B3_DEBUG_WIND 0 + +// https://en.wikipedia.org/wiki/Density_of_air +// https://www.engineeringtoolbox.com/wind-load-d_1775.html +// force = 0.5 * air_density * velocity^2 * area +// https://en.wikipedia.org/wiki/Lift_(force) +void b3Shape_ApplyWind( b3ShapeId shapeId, b3Vec3 wind, float drag, float lift, float maxSpeed, bool wake ) +{ + b3World* world = b3GetWorld( shapeId.world0 ); + if ( world == NULL ) + { + return; + } + + B3_REC( world, ShapeApplyWind, shapeId, wind, drag, lift, maxSpeed, wake ); + + b3Shape* shape = b3GetShape( world, shapeId ); + + b3ShapeType shapeType = shape->type; + if ( shapeType != b3_sphereShape && shapeType != b3_capsuleShape && shapeType != b3_hullShape ) + { + return; + } + + b3Body* body = b3Array_Get( world->bodies, shape->bodyId ); + + if ( body->type != b3_dynamicBody ) + { + return; + } + + if ( body->setIndex == b3_disabledSet ) + { + return; + } + + if ( body->setIndex >= b3_firstSleepingSet && wake == false ) + { + return; + } + + b3BodySim* sim = b3GetBodySim( world, body ); + + if ( body->setIndex != b3_awakeSet ) + { + // Must wake for state to exist + b3WakeBodyWithLock( world, body ); + } + + B3_ASSERT( body->setIndex == b3_awakeSet ); + + b3BodyState* state = b3GetBodyState( world, body ); + // Only the rotation is used below, so the demoted world transform is exact + b3Transform transform = b3ToRelativeTransform( sim->transform, b3Pos_zero ); + + float lengthUnits = b3GetLengthUnitsPerMeter(); + float volumeUnits = lengthUnits * lengthUnits * lengthUnits; + + float airDensity = 1.2250f / ( volumeUnits ); + + b3Vec3 force = { 0 }; + b3Vec3 torque = { 0 }; + + switch ( shape->type ) + { + case b3_sphereShape: + { + float radius = shape->sphere.radius; + b3Vec3 centroid = shape->localCentroid; + b3Vec3 lever = b3RotateVector( transform.q, b3Sub( centroid, sim->localCenter ) ); + b3Vec3 shapeVelocity = b3Add( state->linearVelocity, b3Cross( state->angularVelocity, lever ) ); + b3Vec3 relativeVelocity = b3MulSub( wind, drag, shapeVelocity ); + float speed; + b3Vec3 direction = b3GetLengthAndNormalize( &speed, relativeVelocity ); + speed = b3MinFloat( speed, maxSpeed ); + float projectedArea = B3_PI * radius * radius; + force = b3MulSV( 0.5f * airDensity * projectedArea * speed * speed, direction ); + torque = b3Cross( lever, force ); + } + break; + + case b3_capsuleShape: + { + b3Vec3 centroid = shape->localCentroid; + b3Vec3 lever = b3RotateVector( transform.q, b3Sub( centroid, sim->localCenter ) ); + b3Vec3 shapeVelocity = b3Add( state->linearVelocity, b3Cross( state->angularVelocity, lever ) ); + b3Vec3 relativeVelocity = b3MulSub( wind, drag, shapeVelocity ); + float speed; + b3Vec3 direction = b3GetLengthAndNormalize( &speed, relativeVelocity ); + speed = b3MinFloat( speed, maxSpeed ); + + b3Vec3 d = b3Sub( shape->capsule.center2, shape->capsule.center1 ); + d = b3RotateVector( transform.q, d ); + + float radius = shape->capsule.radius; + float projectedArea = B3_PI * radius * radius + 2.0f * radius * b3Length( b3Cross( d, direction ) ); + + // Normal that opposes the wind + b3Vec3 e = b3Normalize( d ); + b3Vec3 normal = b3Sub( b3MulSV( b3Dot( direction, e ), e ), direction ); + + // portion of wind that is perpendicular to surface + b3Vec3 liftDirection = b3Cross( b3Cross( normal, direction ), direction ); + + float forceMagnitude = 0.5f * airDensity * projectedArea * speed * speed; + force = b3MulSV( forceMagnitude, b3MulAdd( direction, lift, liftDirection ) ); + + b3Vec3 edgeLever = b3MulAdd( lever, radius, normal ); + torque = b3Cross( edgeLever, force ); + } + break; + + case b3_hullShape: + { + b3Matrix3 matrix = b3MakeMatrixFromQuat( transform.q ); + + int faceCount = shape->hull->faceCount; + const b3Vec3* points = b3GetHullPoints( shape->hull ); + const b3HullFace* faces = b3GetHullFaces( shape->hull ); + const b3HullHalfEdge* edges = b3GetHullEdges( shape->hull ); + const b3Plane* planes = b3GetHullPlanes( shape->hull ); + + b3Vec3 linearVelocity = state->linearVelocity; + b3Vec3 angularVelocity = state->angularVelocity; + b3Vec3 localCenterOfMass = sim->localCenter; + + for ( int i = 0; i < faceCount; ++i ) + { + const b3HullFace* face = faces + i; + const b3HullHalfEdge* edge1 = edges + face->edge; + const b3HullHalfEdge* edge2 = edges + edge1->next; + const b3HullHalfEdge* edge3 = edges + edge2->next; + + B3_ASSERT( edge1 != edge3 ); + B3_ASSERT( edge1->origin < shape->hull->vertexCount ); + B3_ASSERT( edge2->origin < shape->hull->vertexCount ); + + b3Vec3 localPoint1 = points[edge1->origin]; + b3Vec3 localPoint2 = points[edge2->origin]; + b3Vec3 v1 = b3MulMV( matrix, localPoint1 ); + b3Vec3 v2 = b3MulMV( matrix, localPoint2 ); + b3Vec3 normal = b3MulMV( matrix, planes[i].normal ); + + do + { + B3_ASSERT( edge3->origin < shape->hull->vertexCount ); + b3Vec3 localPoint3 = points[edge3->origin]; + b3Vec3 v3 = b3MulMV( matrix, localPoint3 ); + + // Triangle center + b3Vec3 localCenter = b3MulSV( 0.333333f, b3Add( localPoint1, b3Add( localPoint2, localPoint3 ) ) ); + + // Lever arm from center of mass to triangle center in world space + b3Vec3 lever = b3MulMV( matrix, b3Sub( localCenter, localCenterOfMass ) ); + + // Velocity of the triangle center in world space + b3Vec3 centerVelocity = b3Add( linearVelocity, b3Cross( angularVelocity, lever ) ); + + b3Vec3 relativeVelocity = b3MulSub( wind, drag, centerVelocity ); + float speed; + b3Vec3 direction = b3GetLengthAndNormalize( &speed, relativeVelocity ); + + // Check for back-side + if ( b3Dot( normal, direction ) < -FLT_EPSILON ) + { + float projectedArea = -0.5f * b3Dot( b3Cross( b3Sub( v2, v1 ), b3Sub( v3, v1 ) ), direction ); + B3_VALIDATE( projectedArea >= -FLT_EPSILON ); + + b3Vec3 liftDirection = b3Cross( b3Cross( normal, direction ), direction ); + + speed = b3MinFloat( speed, maxSpeed ); + + float forceMagnitude = 0.5f * airDensity * projectedArea * speed * speed; + b3Vec3 deltaForce = b3MulSV( forceMagnitude, b3MulAdd( direction, lift, liftDirection ) ); + b3Vec3 deltaTorque = b3Cross( lever, deltaForce ); + + force = b3Add( force, deltaForce ); + torque = b3Add( torque, deltaTorque ); + +#if B3_DEBUG_WIND + int lineIndex = world->taskContexts.data[0].lineCount; + if ( lineIndex < B3_DEBUG_LINE_CAPACITY ) + { + b3DebugLine* line = world->taskContexts.data[0].lines + lineIndex; + line->p1 = b3OffsetPos( sim->transform.p, b3MulMV( matrix, localCenter ) ); + line->p2 = b3OffsetPos( line->p1, deltaForce ); + line->label = i; + line->color = b3_colorBlanchedAlmond; + world->taskContexts.data[0].lineCount += 1; + } +#endif + } + + edge2 = edge3; + edge3 = edges + edge3->next; + v2 = v3; + localPoint2 = localPoint3; + } + while ( edge1 != edge3 ); + } + } + break; + + default: + break; + } + + sim->force = b3Add( sim->force, force ); + sim->torque = b3Add( sim->torque, torque ); +} + +typedef struct b3MeshImpactContext +{ + b3TOIInput toiInput; + b3TOIOutput toiOutput; + // Centroid of shape in body B local space + b3Vec3 localCentroidB; + // Centroid of shape at beginning and end of sweep in mesh local space. Used for early out. + b3Vec3 meshLocalCentroidB1, meshLocalCentroidB2; + float fallbackRadius; + bool isSensor; + + int visitCount; +} b3MeshImpactContext; + +static bool b3MeshTimeOfImpactFcn( b3Vec3 a, b3Vec3 b, b3Vec3 c, int triangleIndex, void* context ) +{ + B3_UNUSED( triangleIndex ); + + b3MeshImpactContext* toiContext = context; + + toiContext->visitCount += 1; + + // Early out for parallel movement + b3Vec3 c1 = toiContext->meshLocalCentroidB1; + b3Vec3 c2 = toiContext->meshLocalCentroidB2; + + b3Vec3 n = b3Normalize( b3Cross( b3Sub( b, a ), b3Sub( c, a ) ) ); + float offset1 = b3Dot( n, b3Sub( c1, a ) ); + float offset2 = b3Dot( n, b3Sub( c2, a ) ); + + if ( offset1 < 0.0f ) + { + // Started behind or finished in front + return true; + } + + if ( toiContext->isSensor == false && offset1 - offset2 < toiContext->fallbackRadius && offset2 > toiContext->fallbackRadius ) + { + // Finished in front + return true; + } + + b3Vec3 triangle[3] = { a, b, c }; + toiContext->toiInput.proxyA.points = triangle; + toiContext->toiInput.proxyA.count = 3; + + b3TOIOutput output = b3TimeOfImpact( &toiContext->toiInput ); + + // It is possible for a hit at fraction == 0 + + if ( 0.0f < output.fraction && output.fraction < toiContext->toiInput.maxFraction ) + { + toiContext->toiOutput = output; + toiContext->toiInput.maxFraction = output.fraction; + } + else if ( 0.0f == output.fraction ) + { + // fallback to TOI of a small circle around the fast shape centroid + b3TOIInput fallbackInput = toiContext->toiInput; + fallbackInput.proxyB = (b3ShapeProxy){ &toiContext->localCentroidB, 1, toiContext->fallbackRadius + B3_LINEAR_SLOP }; + output = b3TimeOfImpact( &fallbackInput ); + + if ( 0.0f < output.fraction && output.fraction < toiContext->toiInput.maxFraction ) + { + toiContext->toiOutput = output; + toiContext->toiInput.maxFraction = output.fraction; + toiContext->toiOutput.usedFallback = true; + } + } + + // Continue the query + return true; +} + +typedef struct b3CompoundImpactContext +{ + b3TOIInput toiInput; + b3TOIOutput toiOutput; + b3Transform compoundTransform; + + // Bounds local to compound + b3AABB localSweepBoundsB; + + // Centroid of shape in body B local space + b3Vec3 localCentroidB; + float fallbackRadius; +} b3CompoundImpactContext; + +// Implements b3CompoundQueryFcn +static bool b3CompoundTimeOfImpactFcn( const b3CompoundData* compound, int childIndex, void* context ) +{ + b3CompoundImpactContext* toiContext = (b3CompoundImpactContext*)context; + + b3ChildShape child = b3GetCompoundChild( compound, childIndex ); + + b3TOIOutput output = { 0 }; + toiContext->toiInput.sweepA = b3MakeCompoundChildSweep( toiContext->compoundTransform, child.transform ); + + switch ( child.type ) + { + case b3_capsuleShape: + { + toiContext->toiInput.proxyA.points = &child.capsule.center1; + toiContext->toiInput.proxyA.count = 2; + toiContext->toiInput.proxyA.radius = child.capsule.radius; + output = b3TimeOfImpact( &toiContext->toiInput ); + } + break; + + case b3_hullShape: + { + toiContext->toiInput.proxyA.points = b3GetHullPoints( child.hull ); + toiContext->toiInput.proxyA.count = child.hull->vertexCount; + toiContext->toiInput.proxyA.radius = 0.0f; + output = b3TimeOfImpact( &toiContext->toiInput ); + } + break; + + case b3_meshShape: + { + b3MeshImpactContext meshContext = { 0 }; + meshContext.toiInput = toiContext->toiInput; + meshContext.isSensor = false; + meshContext.localCentroidB = toiContext->localCentroidB; + meshContext.fallbackRadius = toiContext->fallbackRadius; + + b3Transform meshWorldTransform = b3MulTransforms( toiContext->compoundTransform, child.transform ); + + const b3Sweep* sweepB = &toiContext->toiInput.sweepB; + b3Transform xfB1 = { + .p = b3Sub( sweepB->c1, b3RotateVector( sweepB->q1, sweepB->localCenter ) ), + .q = sweepB->q1, + }; + + b3Transform xfB2 = { + .p = b3Sub( sweepB->c2, b3RotateVector( sweepB->q2, sweepB->localCenter ) ), + .q = sweepB->q2, + }; + + meshContext.meshLocalCentroidB1 = + b3InvTransformPoint( meshWorldTransform, b3TransformPoint( xfB1, meshContext.localCentroidB ) ); + meshContext.meshLocalCentroidB2 = + b3InvTransformPoint( meshWorldTransform, b3TransformPoint( xfB2, meshContext.localCentroidB ) ); + + // Bounds local to mesh + b3AABB localBounds = b3AABB_Transform( b3InvertTransform( child.transform ), toiContext->localSweepBoundsB ); + + b3QueryMesh( &child.mesh, localBounds, b3MeshTimeOfImpactFcn, &meshContext ); + + output = meshContext.toiOutput; + } + break; + + case b3_sphereShape: + { + toiContext->toiInput.proxyA.points = &child.sphere.center; + toiContext->toiInput.proxyA.count = 1; + toiContext->toiInput.proxyA.radius = child.sphere.radius; + output = b3TimeOfImpact( &toiContext->toiInput ); + } + break; + + default: + B3_ASSERT( false ); + break; + } + + if ( 0.0f < output.fraction && output.fraction < toiContext->toiInput.maxFraction ) + { + toiContext->toiOutput = output; + toiContext->toiInput.maxFraction = output.fraction; + } + + // Clear this to be safe + toiContext->toiInput.proxyA = (b3ShapeProxy){ 0 }; + + // Continue the query + return true; +} + +b3TOIOutput b3ShapeTimeOfImpact( b3Shape* shapeA, b3Shape* shapeB, b3Sweep* sweepA, b3Sweep* sweepB, float maxFraction ) +{ + bool isSensor = shapeA->sensorIndex != B3_NULL_INDEX; + + b3ShapeType typeA = shapeA->type; + if ( typeA == b3_compoundShape ) + { + // todo implement b3CompoundTimeOfImpact + b3CompoundImpactContext context = { 0 }; + context.toiInput.proxyB = b3MakeShapeProxy( shapeB ); + context.toiInput.sweepB = *sweepB; + context.toiInput.maxFraction = maxFraction; + + context.compoundTransform = (b3Transform){ + .p = sweepA->c1, + .q = sweepA->q1, + }; + + b3Vec3 localCentroidB = b3GetShapeCentroid( shapeB ); + context.localCentroidB = localCentroidB; + + b3ShapeExtent extents = b3ComputeShapeExtent( shapeB, context.localCentroidB ); + context.fallbackRadius = b3MaxFloat( 0.75f * extents.minExtent, B3_SPECULATIVE_DISTANCE ); + + // Swept bounds of shapeB + b3AABB bounds = b3ComputeSweptShapeAABB( shapeB, sweepB, maxFraction ); + + // Bounds local to mesh + b3AABB localBounds = b3AABB_Transform( b3InvertTransform( context.compoundTransform ), bounds ); + context.localSweepBoundsB = localBounds; + + b3QueryCompound( shapeA->compound, localBounds, b3CompoundTimeOfImpactFcn, &context ); + + return context.toiOutput; + } + + if ( typeA == b3_heightShape || typeA == b3_meshShape ) + { + // todo implement b3MeshTimeOfImpact and b3HeightFieldTimeOfImpact + // Note: assuming mesh is static + + uint64_t ticks = b3GetTicks(); + + b3MeshImpactContext context = { 0 }; + context.toiInput.sweepA = *sweepA; + context.toiInput.proxyA.count = 3; + context.toiInput.proxyB = b3MakeShapeProxy( shapeB ); + context.toiInput.sweepB = *sweepB; + context.toiInput.maxFraction = maxFraction; + context.isSensor = isSensor; + + b3Vec3 localCentroidB = b3GetShapeCentroid( shapeB ); + context.localCentroidB = localCentroidB; + + // Assume mesh is static + b3Transform xfA = { + .p = b3Sub( sweepA->c1, b3RotateVector( sweepA->q1, sweepA->localCenter ) ), + .q = sweepA->q1, + }; + + b3Transform xfB1 = { + .p = b3Sub( sweepB->c1, b3RotateVector( sweepB->q1, sweepB->localCenter ) ), + .q = sweepB->q1, + }; + + b3Transform xfB2 = { + .p = b3Sub( sweepB->c2, b3RotateVector( sweepB->q2, sweepB->localCenter ) ), + .q = sweepB->q2, + }; + + context.meshLocalCentroidB1 = b3InvTransformPoint( xfA, b3TransformPoint( xfB1, localCentroidB ) ); + context.meshLocalCentroidB2 = b3InvTransformPoint( xfA, b3TransformPoint( xfB2, localCentroidB ) ); + + b3ShapeExtent extents = b3ComputeShapeExtent( shapeB, context.localCentroidB ); + context.fallbackRadius = b3MaxFloat( 0.5f * extents.minExtent, B3_LINEAR_SLOP ); + + // Swept bounds of shapeB + // todo pass in xfA to get local bounds directly + b3AABB bounds = b3ComputeSweptShapeAABB( shapeB, sweepB, maxFraction ); + + // Bounds local to mesh + b3AABB localBounds = b3AABB_Transform( b3InvertTransform( xfA ), bounds ); + + if ( typeA == b3_meshShape ) + { + b3QueryMesh( &shapeA->mesh, localBounds, b3MeshTimeOfImpactFcn, &context ); + } + else if ( typeA == b3_heightShape ) + { + b3QueryHeightField( shapeA->heightField, localBounds, b3MeshTimeOfImpactFcn, &context ); + } + + float ms = b3GetMilliseconds( ticks ); + if ( ms > 1000.0f * b3GetStallThreshold() ) + { + b3Log( "CCD stall: visited %d triangles", context.visitCount ); + } + + return context.toiOutput; + } + + B3_ASSERT( shapeB->type != b3_compoundShape && shapeB->type != b3_meshShape && shapeB->type != b3_heightShape ); + + b3TOIInput input; + input.proxyA = b3MakeShapeProxy( shapeA ); + input.proxyB = b3MakeShapeProxy( shapeB ); + input.sweepA = *sweepA; + input.sweepB = *sweepB; + input.maxFraction = maxFraction; + + b3TOIOutput output = b3TimeOfImpact( &input ); + +#if 0 + // todo I'm not sure this is worth it for convex vs convex. + if (0.0f < output.fraction && output.fraction < maxFraction) + { + return output; + } + + if (0.0f == output.fraction) + { + // fallback to TOI of a small circle around the fast shape centroid + b3Vec3 centroid = b3GetShapeCentroid( shapeB ); + input.proxyB = ( b3ShapeProxy ){ ¢roid, 1, B3_SPECULATIVE_DISTANCE }; + output = b3TimeOfImpact( &input ); + return output; + } +#endif + + return output; +} + +// Resolve the user material id for a hit point on the given shape. Mesh/heightfield shapes +// use the manifold-point triangleIndex to pick a per-triangle material. Compound shapes use +// the contact's childIndex to find the participating child, then for a mesh child apply the +// child's materialIndices indirection on top of the per-triangle index. Convex shapes fall +// back to materials[0]. childIndex is unused for non-compound shapes. +uint64_t b3GetShapeUserMaterialId( const b3Shape* shape, int childIndex, int triangleIndex ) +{ + if ( shape->materialCount == 0 ) + { + return 0; + } + + int materialIndex = 0; + if ( shape->type == b3_meshShape ) + { + const uint8_t* indices = b3GetMeshMaterialIndices( shape->mesh.data ); + if ( indices != NULL ) + { + materialIndex = indices[triangleIndex]; + } + } + else if ( shape->type == b3_heightShape ) + { + materialIndex = b3GetHeightFieldMaterial( shape->heightField, triangleIndex ); + } + else if ( shape->type == b3_compoundShape ) + { + b3ChildShape child = b3GetCompoundChild( shape->compound, childIndex ); + if ( child.type == b3_meshShape ) + { + const uint8_t* indices = b3GetMeshMaterialIndices( child.mesh.data ); + int meshMaterialIndex = indices != NULL ? indices[triangleIndex] : 0; + meshMaterialIndex = b3ClampInt( meshMaterialIndex, 0, B3_MAX_COMPOUND_MESH_MATERIALS - 1 ); + materialIndex = child.materialIndices[meshMaterialIndex]; + } + else + { + materialIndex = child.materialIndices[0]; + } + } + + materialIndex = b3ClampInt( materialIndex, 0, shape->materialCount - 1 ); + return b3GetShapeMaterials( shape )[materialIndex].userMaterialId; +} diff --git a/vendor/box3d/src/src/shape.h b/vendor/box3d/src/src/shape.h new file mode 100644 index 000000000..519ce8694 --- /dev/null +++ b/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 + +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; +} diff --git a/vendor/box3d/src/src/simd.c b/vendor/box3d/src/src/simd.c new file mode 100644 index 000000000..6a9faace0 --- /dev/null +++ b/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 ); +} diff --git a/vendor/box3d/src/src/simd.h b/vendor/box3d/src/src/simd.h new file mode 100644 index 000000000..ac7e9e8b5 --- /dev/null +++ b/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 + +#if defined( B3_SIMD_SSE2 ) + +#include + +// 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 ); diff --git a/vendor/box3d/src/src/solver.c b/vendor/box3d/src/src/solver.c new file mode 100644 index 000000000..a2b1be977 --- /dev/null +++ b/vendor/box3d/src/src/solver.c @@ -0,0 +1,2328 @@ +// SPDX-FileCopyrightText: 2025 Erin Catto +// SPDX-License-Identifier: MIT + +#include "solver.h" + +#include "arena_allocator.h" +#include "bitset.h" +#include "body.h" +#include "compound.h" +#include "contact.h" +#include "contact_solver.h" +#include "core.h" +#include "ctz.h" +#include "island.h" +#include "joint.h" +#include "parallel_for.h" +#include "physics_world.h" +#include "platform.h" +#include "sensor.h" +#include "shape.h" +#include "solver_set.h" + +#include +#include +#include + +// these are useful for solver testing +#define ITERATIONS 1 +#define RELAX_ITERATIONS 1 + +#if ( defined( __GNUC__ ) || defined( __clang__ ) ) && ( defined( __i386__ ) || defined( __x86_64__ ) ) +static void b3Pause( void ) +{ + __asm__ __volatile__( "pause\n" ); +} +#elif ( defined( __arm__ ) && defined( __ARM_ARCH ) && __ARM_ARCH >= 7 ) || defined( __aarch64__ ) +static void b3Pause( void ) +{ + __asm__ __volatile__( "yield" ::: "memory" ); +} +#elif defined( _MSC_VER ) && ( defined( _M_IX86 ) || defined( _M_X64 ) ) +static void b3Pause( void ) +{ + _mm_pause(); +} +#elif defined( _MSC_VER ) && ( defined( _M_ARM ) || defined( _M_ARM64 ) ) +static void b3Pause( void ) +{ + __yield(); +} +#else +static void b3Pause( void ) +{ +} +#endif + +typedef struct b3WorkerContext +{ + b3StepContext* context; + int workerIndex; + void* userTask; +} b3WorkerContext; + +// Integrate velocities, apply damping, and gyroscopic torque +static void b3IntegrateVelocitiesTask( b3SolverBlock block, b3StepContext* context ) +{ + b3TracyCZoneNC( integrate_velocity, "IntVel", b3_colorDeepPink, true ); + + B3_VALIDATE( block.startIndex + block.count <= context->world->solverSets.data[b3_awakeSet].bodyStates.count ); + + b3BodyState* states = context->states; + b3BodySim* sims = context->sims; + + b3Vec3 gravity = context->world->gravity; + float h = context->h; + + for ( int i = block.startIndex; i < block.startIndex + block.count; ++i ) + { + b3BodySim* sim = sims + i; + b3BodyState* state = states + i; + + b3Vec3 v = state->linearVelocity; + b3Vec3 w = state->angularVelocity; + + // Damping math + // Differential equation: dv/dt + c * v = 0 + // Solution: v(t) = v0 * exp(-c * t) + // Time step: v(t + dt) = v0 * exp(-c * (t + dt)) = v0 * exp(-c * t) * exp(-c * dt) = v(t) * exp(-c * dt) + // v2 = exp(-c * dt) * v1 + // Pade approximation: + // v2 = v1 * 1 / (1 + c * dt) + float linearDamping = 1.0f / ( 1.0f + h * sim->linearDamping ); + float angularDamping = 1.0f / ( 1.0f + h * sim->angularDamping ); + + // Gravity scale will be zero for kinematic bodies + float gravityScale = sim->invMass > 0.0f ? sim->gravityScale : 0.0f; + + b3Vec3 linearVelocityDelta = b3Blend2( h * sim->invMass, sim->force, h * gravityScale, gravity ); + v = b3MulAdd( linearVelocityDelta, linearDamping, v ); + + b3Vec3 angularVelocityDelta = b3MulSV( h, b3MulMV( sim->invInertiaWorld, sim->torque ) ); + w = b3MulAdd( angularVelocityDelta, angularDamping, w ); + + // Gyroscopic torque by solving this nonlinear equation using Newton-Raphson. + // I * (w2 - w1) + h * cross(w2, I * w2) = 0 + // This is all done in local coordinates where the Jacobian is easier to compute. + // This improves the simulation of long skinny bodies. + { + // Get current rotation. + b3Quat q0 = sim->transform.q; + b3Quat q = b3MulQuat( state->deltaRotation, q0 ); + + // todo wasteful computation + b3Matrix3 inertiaLocal = b3InvertMatrix( sim->invInertiaLocal ); + + // Compute local angular velocity + b3Vec3 omega1 = b3InvRotateVector( q, w ); + b3Vec3 omega2 = omega1; + + // Symmetric inertia tensor: 6 unique entries (column-major) + const float i00 = inertiaLocal.cx.x; + const float i01 = inertiaLocal.cy.x; + const float i02 = inertiaLocal.cz.x; + const float i11 = inertiaLocal.cy.y; + const float i12 = inertiaLocal.cz.y; + const float i22 = inertiaLocal.cz.z; + + for ( int gyroIteration = 0; gyroIteration < 1; ++gyroIteration ) + { + const float w1 = omega2.x; + const float w2 = omega2.y; + const float w3 = omega2.z; + + // Iw = I * omega2 (shared between residual and Jacobian) + const float Iw1 = i00 * w1 + i01 * w2 + i02 * w3; + const float Iw2 = i01 * w1 + i11 * w2 + i12 * w3; + const float Iw3 = i02 * w1 + i12 * w2 + i22 * w3; + + // Residual: b = I*(omega2 - omega1) + h * (omega2 × I*omega2) + const b3Vec3 dw = b3Sub( omega2, omega1 ); + b3Vec3 b = { + i00 * dw.x + i01 * dw.y + i02 * dw.z + h * ( w2 * Iw3 - w3 * Iw2 ), + i01 * dw.x + i11 * dw.y + i12 * dw.z + h * ( w3 * Iw1 - w1 * Iw3 ), + i02 * dw.x + i12 * dw.y + i22 * dw.z + h * ( w1 * Iw2 - w2 * Iw1 ), + }; + + // Jacobian J = I + h * (skew(omega2) * I - skew(I*omega2)) + // Jacobian derived by Erin Catto, Ph.D. Do not attempt to do this without a Ph.D. + // Doubled inertia terms above fold into Iw, e.g. row 2 col 1: i00*w3 - i02*w1 - Iw3. + b3Matrix3 J = { + { i00 + h * ( w2 * i02 - w3 * i01 ), i01 + h * ( w3 * i00 - w1 * i02 - Iw3 ), + i02 + h * ( w1 * i01 - w2 * i00 + Iw2 ) }, + { i01 + h * ( w2 * i12 - w3 * i11 + Iw3 ), i11 + h * ( w3 * i01 - w1 * i12 ), + i12 + h * ( w1 * i11 - w2 * i01 - Iw1 ) }, + { i02 + h * ( w2 * i22 - w3 * i12 - Iw2 ), i12 + h * ( w3 * i02 - w1 * i22 + Iw1 ), + i22 + h * ( w1 * i12 - w2 * i02 ) }, + }; + + omega2 = b3Sub( omega2, b3Solve3( J, b ) ); + } + + w = b3RotateVector( q, omega2 ); + } + + state->linearVelocity = v; + state->angularVelocity = w; + } + + b3TracyCZoneEnd( integrate_velocity ); +} + +static void b3IntegratePositionsTask( b3SolverBlock block, b3StepContext* context ) +{ + b3TracyCZoneNC( integrate_positions, "IntPos", b3_colorDarkSeaGreen, true ); + + B3_VALIDATE( block.startIndex + block.count <= context->world->solverSets.data[b3_awakeSet].bodyStates.count ); + + b3BodyState* states = context->states; + float h = context->h; + float maxLinearSpeed = context->maxLinearVelocity; + float maxAngularSpeed = B3_MAX_ROTATION * context->inv_dt; + float maxLinearSpeedSquared = maxLinearSpeed * maxLinearSpeed; + float maxAngularSpeedSquared = maxAngularSpeed * maxAngularSpeed; + + for ( int i = block.startIndex; i < block.startIndex + block.count; ++i ) + { + b3BodyState* state = states + i; + + b3Vec3 v = state->linearVelocity; + b3Vec3 w = state->angularVelocity; + + // Motion locks - these can be viewed as a constraint that come last + v.x = ( state->flags & b3_lockLinearX ) ? 0.0f : v.x; + v.y = ( state->flags & b3_lockLinearY ) ? 0.0f : v.y; + v.z = ( state->flags & b3_lockLinearZ ) ? 0.0f : v.z; + w.x = ( state->flags & b3_lockAngularX ) ? 0.0f : w.x; + w.y = ( state->flags & b3_lockAngularY ) ? 0.0f : w.y; + w.z = ( state->flags & b3_lockAngularZ ) ? 0.0f : w.z; + + // Clamp to max linear speed + if ( b3Dot( v, v ) > maxLinearSpeedSquared ) + { + float ratio = maxLinearSpeed / b3Length( v ); + v = b3MulSV( ratio, v ); + state->flags |= b3_isSpeedCapped; + } + + // Clamp to max angular speed + if ( b3Dot( w, w ) > maxAngularSpeedSquared && ( state->flags & b3_allowFastRotation ) == 0 ) + { + float ratio = maxAngularSpeed / b3Length( w ); + w = b3MulSV( ratio, w ); + state->flags |= b3_isSpeedCapped; + } + + state->linearVelocity = v; + state->angularVelocity = w; + state->deltaPosition = b3MulAdd( state->deltaPosition, h, v ); + state->deltaRotation = b3IntegrateRotation( state->deltaRotation, b3MulSV( h, w ) ); + } + + b3TracyCZoneEnd( integrate_positions ); +} + +static void b3PrepareJointsTask( b3SolverBlock block, b3StepContext* context ) +{ + b3TracyCZoneNC( prepare_joints, "PrepJoints", b3_colorOldLace, true ); + + b3JointPrepareSpan* spans = context->jointPrepareSpans; + + int index = block.startIndex; + int endIndex = block.startIndex + block.count; + + // Find color for start index. Linear search but fast. + int colorIndex = 0; + while ( spans[colorIndex + 1].start <= index ) + { + colorIndex += 1; + } + + // Loop over block + while ( index < endIndex ) + { + int colorStart = spans[colorIndex].start; + int colorEndIndex = b3MinInt( spans[colorIndex + 1].start, endIndex ); + b3JointSim* joints = spans[colorIndex].joints; + + // Loop over color + for ( ; index < colorEndIndex; ++index ) + { + B3_ASSERT( 0 <= index - colorStart && index - colorStart < spans[colorIndex].count ); + b3JointSim* joint = joints + ( index - colorStart ); + b3PrepareJoint( joint, context ); + } + + // Advance to next color + colorIndex += 1; + } + + b3TracyCZoneEnd( prepare_joints ); +} + +static void b3WarmStartJointsTask( b3SolverBlock block, b3StepContext* context ) +{ + b3TracyCZoneNC( warm_joints, "WarmJoints", b3_colorGold, true ); + + b3GraphColor* color = context->graph->colors + block.colorIndex; + b3JointSim* joints = color->jointSims.data; + + for ( int i = block.startIndex; i < block.startIndex + block.count; ++i ) + { + b3JointSim* joint = joints + i; + b3WarmStartJoint( joint, context ); + } + + b3TracyCZoneEnd( warm_joints ); +} + +static void b3SolveJointsTask( b3SolverBlock block, b3StepContext* context, bool useBias, int workerIndex ) +{ + b3TracyCZoneNC( solve_joints, "SolveJoints", b3_colorLemonChiffon, true ); + + b3GraphColor* color = context->graph->colors + block.colorIndex; + b3JointSim* joints = color->jointSims.data; + + B3_ASSERT( 0 <= block.startIndex && block.startIndex + block.count <= color->jointSims.count ); + + b3BitSet* jointStateBitSet = &context->world->taskContexts.data[workerIndex].jointStateBitSet; + + for ( int i = block.startIndex; i < block.startIndex + block.count; ++i ) + { + b3JointSim* joint = joints + i; + b3SolveJoint( joint, context, useBias ); + + if ( useBias && ( joint->forceThreshold < FLT_MAX || joint->torqueThreshold < FLT_MAX ) && + b3GetBit( jointStateBitSet, joint->jointId ) == false ) + { + float force, torque; + b3GetJointReaction( context->world, joint, context->inv_h, &force, &torque ); + + // Check thresholds. A zero threshold means all awake joints get reported. + if ( force >= joint->forceThreshold || torque >= joint->torqueThreshold ) + { + // Flag this joint for processing. + b3SetBit( jointStateBitSet, joint->jointId ); + } + } + } + + b3TracyCZoneEnd( solve_joints ); +} + +#define B2_MAX_CONTINUOUS_SENSOR_HITS 8 + +typedef struct b3ContinuousContext +{ + b3World* world; + b3BodySim* fastBodySim; + b3Shape* fastShape; + b3Vec3 centroid1, centroid2; + b3Sweep sweep; + // World base for re-centering sweeps. Keeps TOI in float precision far from the origin. + b3Pos base; + float fraction; + b3SensorHit sensorHits[B2_MAX_CONTINUOUS_SENSOR_HITS]; + float sensorFractions[B2_MAX_CONTINUOUS_SENSOR_HITS]; + int sensorCount; + + int visitCount; + + int distanceIterations; + int pushBackIterations; + int rootIterations; +} b3ContinuousContext; + +// This is called from b3DynamicTree_Query for continuous collision +static bool b3ContinuousQueryCallback( int proxyId, uint64_t userData, void* context ) +{ + B3_UNUSED( proxyId ); + + int shapeId = (int)userData; + b3ContinuousContext* continuousContext = context; + continuousContext->visitCount += 1; + + b3Shape* fastShape = continuousContext->fastShape; + b3BodySim* fastBodySim = continuousContext->fastBodySim; + + B3_ASSERT( fastShape->sensorIndex == B3_NULL_INDEX ); + + // Skip same shape + if ( shapeId == fastShape->id ) + { + return true; + } + + b3World* world = continuousContext->world; + b3Shape* shape = b3Array_Get( world->shapes, shapeId ); + + // Skip same body + if ( shape->bodyId == fastShape->bodyId ) + { + return true; + } + + // Skip sensors unless both shapes want sensor events + bool isSensor = shape->sensorIndex != B3_NULL_INDEX; + if ( isSensor && ( ( shape->flags & b3_enableSensorEvents ) == 0 || ( fastShape->flags & b3_enableSensorEvents ) == 0 ) ) + { + return true; + } + + // Skip filtered shapes + bool canCollide = b3ShouldShapesCollide( fastShape->filter, shape->filter ); + if ( canCollide == false ) + { + return true; + } + + b3Body* body = b3Array_Get( world->bodies, shape->bodyId ); + + b3BodySim* bodySim = b3GetBodySim( world, body ); + B3_ASSERT( body->type == b3_staticBody || ( fastBodySim->flags & b3_isBullet ) ); + + // Skip bullets + if ( bodySim->flags & b3_isBullet ) + { + return true; + } + + // Skip filtered bodies + b3Body* fastBody = b3Array_Get( world->bodies, fastBodySim->bodyId ); + canCollide = b3ShouldBodiesCollide( world, fastBody, body ); + if ( canCollide == false ) + { + return true; + } + + // Custom user filtering + if ( ( shape->flags & b3_enableCustomFiltering ) != 0 || ( fastShape->flags & b3_enableCustomFiltering ) != 0 ) + { + b3CustomFilterFcn* customFilterFcn = world->customFilterFcn; + if ( customFilterFcn != NULL ) + { + b3ShapeId idA = { shape->id + 1, world->worldId, shape->generation }; + b3ShapeId idB = { fastShape->id + 1, world->worldId, fastShape->generation }; + canCollide = customFilterFcn( idA, idB, world->customFilterContext ); + if ( canCollide == false ) + { + return true; + } + } + } + + uint64_t ticks = b3GetTicks(); + + // todo does having a sweep on shapeA help with bullets? + b3Sweep sweepA = b3MakeRelativeSweep( bodySim, continuousContext->base ); + + // Time of impact versus shape. Supports all shape types + b3TOIOutput output = b3ShapeTimeOfImpact( shape, fastShape, &sweepA, &continuousContext->sweep, continuousContext->fraction ); + if ( isSensor ) + { + // Only accept a sensor hit that is sooner than the current solid hit. + if ( output.fraction <= continuousContext->fraction && continuousContext->sensorCount < B2_MAX_CONTINUOUS_SENSOR_HITS ) + { + int index = continuousContext->sensorCount; + + // The hit shape is a sensor + b3SensorHit sensorHit = { + .sensorId = shape->id, + .visitorId = fastShape->id, + }; + + continuousContext->sensorHits[index] = sensorHit; + continuousContext->sensorFractions[index] = output.fraction; + continuousContext->sensorCount += 1; + } + } + else if ( 0.0f < output.fraction && output.fraction < continuousContext->fraction ) + { + bool didHit = true; + + if ( didHit && ( ( shape->flags & b3_enablePreSolveEvents ) || ( fastShape->flags & b3_enablePreSolveEvents ) ) ) + { + b3ShapeId shapeIdA = { shape->id + 1, world->worldId, shape->generation }; + b3ShapeId shapeIdB = { fastShape->id + 1, world->worldId, fastShape->generation }; + b3Pos point = b3OffsetPos( continuousContext->base, output.point ); + didHit = world->preSolveFcn( shapeIdA, shapeIdB, point, output.normal, world->preSolveContext ); + } + + if ( didHit ) + { + fastBodySim->flags |= b3_hadTimeOfImpact; + continuousContext->fraction = output.fraction; + continuousContext->distanceIterations = b3MaxInt( continuousContext->distanceIterations, output.distanceIterations ); + continuousContext->pushBackIterations = b3MaxInt( continuousContext->pushBackIterations, output.pushBackIterations ); + continuousContext->rootIterations = b3MaxInt( continuousContext->rootIterations, output.rootIterations ); + } + } + + float ms = b3GetMilliseconds( ticks ); + if ( ms > 1000.0f * b3GetStallThreshold() ) + { + const char* nameFast = b3FindNameWithDefault( &world->names, fastBody->nameId, "NULL" ); + const char* name = b3FindNameWithDefault( &world->names, body->nameId, "NULL" ); + b3Log( "CCD stall: duration %.1f ms for %s versus %s", ms, nameFast, name ); + } + + // Continue query + return true; +} + +// Continuous collision of dynamic versus static +static void b3SolveContinuous( b3World* world, int bodySimIndex, b3TaskContext* taskContext ) +{ + b3TracyCZoneNC( ccd, "CCD", b3_colorDarkGoldenRod, true ); + + uint64_t ticks = b3GetTicks(); + + b3SolverSet* awakeSet = b3Array_Get( world->solverSets, b3_awakeSet ); + b3BodySim* fastBodySim = b3Array_Get( awakeSet->bodySims, bodySimIndex ); + B3_ASSERT( fastBodySim->flags & b3_isFast ); + + // Re-center the sweep on the fast body so the TOI and the swept query stay in float precision + b3Pos base = fastBodySim->center0; + + b3Sweep sweep = b3MakeRelativeSweep( fastBodySim, base ); + + b3Transform xf1; + xf1.q = sweep.q1; + xf1.p = b3Sub( sweep.c1, b3RotateVector( sweep.q1, sweep.localCenter ) ); + + b3Transform xf2; + xf2.q = sweep.q2; + xf2.p = b3Sub( sweep.c2, b3RotateVector( sweep.q2, sweep.localCenter ) ); + + b3DynamicTree* staticTree = world->broadPhase.trees + b3_staticBody; + b3DynamicTree* kinematicTree = world->broadPhase.trees + b3_kinematicBody; + b3DynamicTree* dynamicTree = world->broadPhase.trees + b3_dynamicBody; + b3Body* fastBody = b3Array_Get( world->bodies, fastBodySim->bodyId ); + + b3ContinuousContext context = { 0 }; + context.world = world; + context.sweep = sweep; + context.base = base; + context.fastBodySim = fastBodySim; + context.fraction = 1.0f; + + bool isBullet = ( fastBodySim->flags & b3_isBullet ) != 0; + + int shapeId = fastBody->headShapeId; + while ( shapeId != B3_NULL_INDEX ) + { + b3Shape* fastShape = b3Array_Get( world->shapes, shapeId ); + shapeId = fastShape->nextShapeId; + + context.fastShape = fastShape; + context.centroid1 = b3TransformPoint( xf1, fastShape->localCentroid ); + context.centroid2 = b3TransformPoint( xf2, fastShape->localCentroid ); + + b3AABB box1 = fastShape->aabb; + // xf2 is relative to the base, so translate the box back to world space, rounding outward + b3AABB box2 = b3OffsetAABB( b3ComputeShapeAABB( fastShape, xf2 ), base ); + + // Store this to avoid double computation in the case there is no impact event + fastShape->aabb = box2; + + // No continuous collision for meshes + if ( fastShape->type == b3_meshShape || fastShape->type == b3_heightShape ) + { + continue; + } + + // No continuous collision for sensors + if ( fastShape->sensorIndex != B3_NULL_INDEX ) + { + continue; + } + + b3AABB sweptBox = b3AABB_Union( box1, box2 ); + b3DynamicTree_Query( staticTree, sweptBox, B3_DEFAULT_MASK_BITS, false, b3ContinuousQueryCallback, &context ); + + if ( isBullet ) + { + b3DynamicTree_Query( kinematicTree, sweptBox, B3_DEFAULT_MASK_BITS, false, b3ContinuousQueryCallback, &context ); + b3DynamicTree_Query( dynamicTree, sweptBox, B3_DEFAULT_MASK_BITS, false, b3ContinuousQueryCallback, &context ); + } + } + + const float speculativeScalar = B3_SPECULATIVE_DISTANCE; + + if ( context.fraction < 1.0f ) + { + // Handle time of impact event. The sweep is relative to the base, so re-add the base + // to return the advanced pose to world space. + b3Quat q = b3NLerp( sweep.q1, sweep.q2, context.fraction ); + b3Vec3 c = b3Lerp( sweep.c1, sweep.c2, context.fraction ); + b3Vec3 origin = b3Sub( c, b3RotateVector( q, sweep.localCenter ) ); + + // Advance body + b3WorldTransform transform = { b3OffsetPos( base, origin ), q }; + b3Pos center = b3OffsetPos( base, c ); + fastBodySim->transform = transform; + fastBodySim->center = center; + fastBodySim->rotation0 = q; + fastBodySim->center0 = center; + + // The move event was written before CCD, so correct it with the impact pose + b3BodyMoveEvent* event = b3Array_Get( world->bodyMoveEvents, bodySimIndex ); + event->transform = fastBodySim->transform; + + // Prepare AABBs for broad-phase. + // Even though a body is fast, it may not move much. So the AABB may not need enlargement. + + shapeId = fastBody->headShapeId; + while ( shapeId != B3_NULL_INDEX ) + { + b3Shape* shape = b3Array_Get( world->shapes, shapeId ); + + // Must recompute aabb at the interpolated transform + b3AABB aabb = b3ComputeFatShapeAABB( shape, transform, speculativeScalar ); + shape->aabb = aabb; + + if ( b3AABB_Contains( shape->fatAABB, aabb ) == false ) + { + float marginScalar = shape->aabbMargin; + b3Vec3 aabbMargin = { marginScalar, marginScalar, marginScalar }; + shape->fatAABB = (b3AABB){ b3Sub( aabb.lowerBound, aabbMargin ), b3Add( aabb.upperBound, aabbMargin ) }; + + shape->flags |= b3_enlargedAABB; + fastBodySim->flags |= b3_enlargeBounds; + } + + shapeId = shape->nextShapeId; + } + } + else + { + // No time of impact event + + // Advance body + fastBodySim->rotation0 = fastBodySim->transform.q; + fastBodySim->center0 = fastBodySim->center; + + // Prepare AABBs for broad-phase + shapeId = fastBody->headShapeId; + while ( shapeId != B3_NULL_INDEX ) + { + b3Shape* shape = b3Array_Get( world->shapes, shapeId ); + + // shape->aabb is still valid from above + + if ( b3AABB_Contains( shape->fatAABB, shape->aabb ) == false ) + { + float marginScalar = shape->aabbMargin; + b3Vec3 aabbMargin = { marginScalar, marginScalar, marginScalar }; + shape->fatAABB = (b3AABB){ + .lowerBound = b3Sub( shape->aabb.lowerBound, aabbMargin ), + .upperBound = b3Add( shape->aabb.upperBound, aabbMargin ), + }; + + shape->flags |= b3_enlargedAABB; + fastBodySim->flags |= b3_enlargeBounds; + } + + shapeId = shape->nextShapeId; + } + } + + // Push sensor hits on the the task context for serial processing. + for ( int i = 0; i < context.sensorCount; ++i ) + { + // Skip any sensor hits that occurred after a solid hit + if ( context.sensorFractions[i] < context.fraction ) + { + b3Array_Push( taskContext->sensorHits, context.sensorHits[i] ); + } + } + + taskContext->distanceIterations = b3MaxInt( taskContext->distanceIterations, context.distanceIterations ); + taskContext->pushBackIterations = b3MaxInt( taskContext->pushBackIterations, context.pushBackIterations ); + taskContext->rootIterations = b3MaxInt( taskContext->rootIterations, context.rootIterations ); + + float ms = b3GetMilliseconds( ticks ); + if ( ms > 1000.0f * b3GetStallThreshold() ) + { + const char* nameFast = b3FindNameWithDefault( &world->names, fastBody->nameId, "NULL" ); + b3Vec3 c1 = sweep.c1; + b3Vec3 c2 = sweep.c2; + int vc = context.visitCount; + b3Log( "CCD stall: duration %.1f ms and visit count %d for %s: c1 = (%g, %g, %g), c2 = (%g, %g, %g)", ms, vc, nameFast, + c1.x, c1.y, c1.z, c2.x, c2.y, c2.z ); + } + + b3TracyCZoneEnd( ccd ); +} + +// Implements b3ParallelForCallback +static void b3FinalizeBodiesTask( int startIndex, int endIndex, int workerIndex, void* context ) +{ + b3TracyCZoneNC( finalize_bodies, "Finalize", b3_colorMediumSeaGreen, true ); + + b3StepContext* stepContext = (b3StepContext*)context; + b3World* world = stepContext->world; + b3Body* bodies = world->bodies.data; + b3BodySim* sims = stepContext->sims; + b3BodyState* states = stepContext->states; + + B3_ASSERT( endIndex <= world->bodyMoveEvents.count ); + + bool enableSleep = world->enableSleep; + bool enableContinuous = world->enableContinuous; + float timeStep = stepContext->dt; + float invTimeStep = stepContext->inv_dt; + uint16_t worldId = world->worldId; + + // The body move event array has should already have the correct size + b3BodyMoveEvent* moveEvents = world->bodyMoveEvents.data; + + b3TaskContext* taskContext = world->taskContexts.data + workerIndex; + b3BitSet* enlargedSimBitSet = &taskContext->enlargedSimBitSet; + b3BitSet* awakeIslandBitSet = &taskContext->awakeIslandBitSet; + + const float speculativeScalar = B3_SPECULATIVE_DISTANCE; + + for ( int simIndex = startIndex; simIndex < endIndex; ++simIndex ) + { + b3BodyState* state = states + simIndex; + b3BodySim* sim = sims + simIndex; + + b3Vec3 v = state->linearVelocity; + b3Vec3 w = state->angularVelocity; + b3Vec3 localOmega = b3InvRotateVector( sim->transform.q, w ); + b3Vec3 localDeltaRotation = b3InvRotateVector( sim->transform.q, state->deltaRotation.v ); + + if ( b3IsValidVec3( v ) == false || b3IsValidVec3( w ) == false ) + { + const char* name = b3FindNameWithDefault( &world->names, bodies[sim->bodyId].nameId, "NULL" ); + b3Log( "unstable: %s", name ); + } + + B3_ASSERT( b3IsValidVec3( v ) ); + B3_ASSERT( b3IsValidVec3( w ) ); + + sim->center = b3OffsetPos( sim->center, state->deltaPosition ); + sim->transform.q = b3NormalizeQuat( b3MulQuat( state->deltaRotation, sim->transform.q ) ); + + // Use the velocity of the farthest point on the body to account for rotation. + b3Vec3 velocityArc = b3ModifiedCross( b3Abs( localOmega ), sim->maxExtent ); + float maxVelocity = b3Length( v ) + b3Length( velocityArc ); + + // Sleep needs to observe position correction as well as true velocity. + // q = [sin(theta/2) * v, cos(theta/2)] + // for small angles abs(theta) ~= 2 * length(sin(theta/2) * v) + b3Vec3 rotationArc = b3ModifiedCross( b3Abs( localDeltaRotation ), sim->maxExtent ); + float maxDeltaPosition = b3Length( state->deltaPosition ) + 2.0f * b3Length( rotationArc ); + + // Position correction is not as important for sleep as true velocity. + float positionSleepFactor = 0.5f; + float sleepVelocity = b3MaxFloat( maxVelocity, positionSleepFactor * invTimeStep * maxDeltaPosition ); + + // reset state deltas + state->deltaPosition = b3Vec3_zero; + state->deltaRotation = b3Quat_identity; + + sim->transform.p = b3OffsetPos( sim->center, b3Neg( b3RotateVector( sim->transform.q, sim->localCenter ) ) ); + + // cache miss here, however I need the shape list below + b3Body* body = bodies + sim->bodyId; + body->bodyMoveIndex = simIndex; + body->sleepVelocity = sleepVelocity; + + moveEvents[simIndex].userData = body->userData; + moveEvents[simIndex].transform = sim->transform; + moveEvents[simIndex].bodyId = (b3BodyId){ sim->bodyId + 1, worldId, body->generation }; + moveEvents[simIndex].fellAsleep = false; + + // reset applied force and torque + sim->force = b3Vec3_zero; + sim->torque = b3Vec3_zero; + + // If you hit this then it means you deferred mass computation but never called b3Body_ApplyMassFromShapes + // or b3Body_SetMassData. + B3_ASSERT( ( body->flags & b3_dirtyMass ) == 0 ); + + body->flags &= ~b3_bodyTransientFlags; + body->flags |= ( sim->flags & ( b3_isSpeedCapped | b3_hadTimeOfImpact ) ); + body->flags |= ( state->flags & ( b3_isSpeedCapped | b3_hadTimeOfImpact ) ); + sim->flags &= ~b3_bodyTransientFlags; + state->flags &= ~b3_bodyTransientFlags; + + if ( enableSleep == false || ( body->flags & b3_enableSleep ) == 0 || sleepVelocity > body->sleepThreshold ) + { + // Body is not sleepy + body->sleepTime = 0.0f; + + const float safetyFactor = 0.5f; + float maxMotion = b3MaxFloat( maxDeltaPosition, maxVelocity * timeStep ); + if ( body->type == b3_dynamicBody && enableContinuous && maxMotion > safetyFactor * sim->minExtent ) + { + // This flag is only retained for debug draw + sim->flags |= b3_isFast; + + // Store in fast array for the continuous collision stage + // This is deterministic because the order of TOI sweeps doesn't matter + if ( sim->flags & b3_isBullet ) + { + int bulletIndex = b3AtomicFetchAddInt( &stepContext->bulletBodyCount, 1 ); + stepContext->bulletBodies[bulletIndex] = simIndex; + } + else + { + b3SolveContinuous( world, simIndex, taskContext ); + } + } + else + { + // Body is safe to advance + sim->center0 = sim->center; + sim->rotation0 = sim->transform.q; + } + } + else + { + // Body is safe to advance and is falling asleep + sim->center0 = sim->center; + sim->rotation0 = sim->transform.q; + body->sleepTime += timeStep; + } + + // Update world space inverse inertia tensor. + b3Matrix3 rotationMatrix = b3MakeMatrixFromQuat( sim->transform.q ); + sim->invInertiaWorld = b3MulMM( b3MulMM( rotationMatrix, sim->invInertiaLocal ), b3Transpose( rotationMatrix ) ); + + // Any single body in an island can keep it awake + b3Island* island = b3Array_Get( world->islands, body->islandId ); + if ( body->sleepTime < B3_TIME_TO_SLEEP ) + { + // keep island awake + int islandIndex = island->localIndex; + b3SetBit( awakeIslandBitSet, islandIndex ); + } + else if ( island->constraintRemoveCount > 0 ) + { + // Body wants to sleep but its island needs splitting first. Track the sleepiest candidate. + // Break sleep time ties using the island id to ensure determinism. The cross worker reduction + // breaks ties the same way. + if ( body->sleepTime > taskContext->splitSleepTime || + ( body->sleepTime == taskContext->splitSleepTime && body->islandId > taskContext->splitIslandId ) ) + { + // pick the sleepiest candidate + taskContext->splitIslandId = body->islandId; + taskContext->splitSleepTime = body->sleepTime; + } + } + + // Update shapes AABBs + b3WorldTransform transform = sim->transform; + bool isFast = ( sim->flags & b3_isFast ) != 0; + int shapeId = body->headShapeId; + while ( shapeId != B3_NULL_INDEX ) + { + b3Shape* shape = b3Array_Get( world->shapes, shapeId ); + + if ( isFast ) + { + // For fast non-bullet bodies the AABB has already been updated in b3SolveContinuous + // For fast bullet bodies the AABB will be updated at a later stage + + // Add to enlarged shapes regardless of AABB changes. + // Bit-set to keep the move array sorted + b3SetBit( enlargedSimBitSet, simIndex ); + } + else + { + b3AABB aabb = b3ComputeFatShapeAABB( shape, transform, speculativeScalar ); + shape->aabb = aabb; + + B3_ASSERT( ( shape->flags & b3_enlargedAABB ) == 0 ); + + if ( b3AABB_Contains( shape->fatAABB, aabb ) == false ) + { + float marginScalar = shape->aabbMargin; + b3Vec3 aabbMargin = { marginScalar, marginScalar, marginScalar }; + shape->fatAABB = (b3AABB){ b3Sub( aabb.lowerBound, aabbMargin ), b3Add( aabb.upperBound, aabbMargin ) }; + shape->flags |= b3_enlargedAABB; + + // Bit-set to keep the move array sorted + b3SetBit( enlargedSimBitSet, simIndex ); + } + } + + shapeId = shape->nextShapeId; + } + } + + b3TracyCZoneEnd( finalize_bodies ); +} + +typedef struct b3BlockDim +{ + // number of items per block (except last block) + int size; + + // total number of blocks + int count; +} b3BlockDim; + +// A block is a range of tasks, a start index and count as a sub-array. Each worker receives at +// most M blocks of work. The workers may receive less blocks if there is not sufficient work. +// Each block of work has a minimum number of elements (block size). This in turn may limit the +// number of blocks. If there are many elements then the block size is increased so there are +// still at most M blocks of work per worker. M is a tunable number that has two goals: +// 1. keep M small to reduce overhead +// 2. keep M large enough for other workers to be able to steal work +// The block size is a power of two to make math efficient. +static inline b3BlockDim b3ComputeBlockCount( int itemCount, int minSize, int maxBlockCount ) +{ + b3BlockDim dim = { 0 }; + if ( itemCount == 0 ) + { + return dim; + } + + if ( itemCount <= minSize * maxBlockCount ) + { + dim.size = minSize; + } + else + { + dim.size = ( itemCount + maxBlockCount - 1 ) / maxBlockCount; + } + + dim.count = ( itemCount + dim.size - 1 ) / dim.size; + + B3_ASSERT( dim.count >= 1 ); + B3_ASSERT( dim.size * dim.count >= itemCount ); + + return dim; +} + +// Initialize solver blocks for a contiguous range of items. Computes block size internally +// from the same parameters used by b3ComputeBlockCount. The atomic claim counter is zeroed +// so workers can CAS (0, 1) on the first stage that owns these blocks. +static void b3InitBlocks( b3SyncBlock* blocks, b3BlockDim dim, int itemCount, uint8_t blockType, uint8_t colorIndex ) +{ + if ( dim.count == 0 ) + { + return; + } + + B3_ASSERT( itemCount >= dim.count ); + + // Compute the number of elements per block + int blockSize = dim.size; + + // Simulation too big + B3_ASSERT( blockSize <= UINT16_MAX ); + + for ( int i = 0; i < dim.count; ++i ) + { + blocks[i].block.startIndex = i * blockSize; + blocks[i].block.count = (uint16_t)blockSize; + blocks[i].block.blockType = blockType; + blocks[i].block.colorIndex = colorIndex; + b3AtomicStoreInt( &blocks[i].syncIndex, 0 ); + } + + // The last block may not be full + blocks[dim.count - 1].block.count = (uint16_t)( itemCount - ( dim.count - 1 ) * blockSize ); + + B3_VALIDATE( blocks[dim.count - 1].block.count <= blockSize ); + B3_VALIDATE( ( dim.count - 1 ) * dim.size + blocks[dim.count - 1].block.count == itemCount ); +} + +static inline b3SolverStage* b3InitStage( b3SolverStage* stage, b3SolverStageType type, b3SyncBlock* blocks, int blockCount, + uint8_t colorIndex ) +{ + stage->type = type; + stage->blocks = blocks; + stage->blockCount = blockCount; + stage->colorIndex = colorIndex; + b3AtomicStoreInt( &stage->completionCount, 0 ); + return stage + 1; +} + +// Initialize one stage per color for each iteration. Used for warm start, solve, relax, and restitution. +// All iterations of a given color share the same b3SyncBlock array so the per-block syncIndex +// grows monotonically across stages within that color. +static b3SolverStage* b3InitColorStages( b3SolverStage* stage, b3SolverStageType type, int iterations, int activeColorCount, + b3SyncBlock** colorBlocks, int* colorBlockCounts, int* activeColorIndices ) +{ + for ( int j = 0; j < iterations; ++j ) + { + for ( int i = 0; i < activeColorCount; ++i ) + { + stage = b3InitStage( stage, type, colorBlocks[i], colorBlockCounts[i], (uint8_t)activeColorIndices[i] ); + } + } + return stage; +} + +static void b3ExecuteBlock( b3SolverStage* stage, b3StepContext* context, b3SolverBlock block, int workerIndex ) +{ + b3SolverStageType stageType = stage->type; + b3SolverBlockType blockType = (b3SolverBlockType)block.blockType; + + switch ( stageType ) + { + case b3_stagePrepareJoints: + b3PrepareJointsTask( block, context ); + break; + + case b3_stagePrepareWideContacts: + b3PrepareContacts_Convex( block, context ); + break; + + case b3_stagePrepareContacts: + b3PrepareContacts_Mesh( block, context ); + break; + + case b3_stageIntegrateVelocities: + b3IntegrateVelocitiesTask( block, context ); + break; + + case b3_stageWarmStart: + if ( blockType == b3_graphJointBlock ) + { + b3WarmStartJointsTask( block, context ); + } + else if ( blockType == b3_graphWideContactBlock ) + { + b3WarmStartContacts_Convex( block, context ); + } + else + { + b3WarmStartContacts_Mesh( block, context ); + } + break; + + case b3_stageSolve: + if ( blockType == b3_graphJointBlock ) + { + bool useBias = true; + b3SolveJointsTask( block, context, useBias, workerIndex ); + } + else if ( blockType == b3_graphWideContactBlock ) + { + bool useBias = true; + b3SolveContacts_Convex( block, context, useBias ); + } + else + { + bool useBias = true; + b3SolveContacts_Mesh( block, context, useBias ); + } + break; + + case b3_stageIntegratePositions: + b3IntegratePositionsTask( block, context ); + break; + + case b3_stageRelax: + if ( blockType == b3_graphJointBlock ) + { + bool useBias = false; + b3SolveJointsTask( block, context, useBias, workerIndex ); + } + else if ( blockType == b3_graphWideContactBlock ) + { + bool useBias = false; + b3SolveContacts_Convex( block, context, useBias ); + } + else + { + bool useBias = false; + b3SolveContacts_Mesh( block, context, useBias ); + } + break; + + case b3_stageRestitution: + if ( blockType == b3_graphWideContactBlock ) + { + b3ApplyRestitution_Convex( block, context ); + } + else if ( blockType == b3_graphContactBlock ) + { + b3ApplyRestitution_Mesh( block, context ); + } + break; + + case b3_stageStoreWideImpulses: + b3StoreImpulses_Convex( block, context, workerIndex ); + break; + + case b3_stageStoreImpulses: + b3StoreImpulses_Mesh( block, context, workerIndex ); + break; + } +} + +// This staggers the worker start indices so they avoid touching the same solver blocks +static inline int GetWorkerStartIndex( int workerIndex, int blockCount, int workerCount ) +{ + if ( blockCount <= workerCount ) + { + return workerIndex < blockCount ? workerIndex : B3_NULL_INDEX; + } + + int blocksPerWorker = blockCount / workerCount; + int remainder = blockCount - blocksPerWorker * workerCount; + return blocksPerWorker * workerIndex + b3MinInt( remainder, workerIndex ); +} + +// Execute a stage, which is an array of solver blocks, each controlled with an atomic sync index. +// Each worker starts at its home index and sweeps the ring, CAS-claiming any unclaimed blocks. +static void b3ExecuteStage( b3SolverStage* stage, b3StepContext* context, int previousSyncIndex, int syncIndex, int workerIndex ) +{ + int completedCount = 0; + b3SyncBlock* blocks = stage->blocks; + int blockCount = stage->blockCount; + + int startIndex = GetWorkerStartIndex( workerIndex, blockCount, context->workerCount ); + if ( startIndex == B3_NULL_INDEX ) + { + return; + } + + B3_ASSERT( 0 <= startIndex && startIndex < blockCount ); + + int blockIndex = startIndex; + for ( int i = 0; i < blockCount; ++i ) + { + if ( b3AtomicCompareExchangeInt( &blocks[blockIndex].syncIndex, previousSyncIndex, syncIndex ) ) + { + B3_ASSERT( completedCount < blockCount ); + + // Pass the descriptor by value -- the wrapping b3SyncBlock holds the atomic + // syncIndex but we only copy .block, so the struct copy never aliases the CAS target. + b3ExecuteBlock( stage, context, blocks[blockIndex].block, workerIndex ); + completedCount += 1; + } + + blockIndex += 1; + if ( blockIndex >= blockCount ) + { + blockIndex = 0; + } + } + + (void)b3AtomicFetchAddInt( &stage->completionCount, completedCount ); +} + +// Execute a stage on worker 0 (main thread). +static void b3ExecuteMainStage( b3SolverStage* stage, b3StepContext* context, uint32_t syncBits ) +{ + int blockCount = stage->blockCount; + if ( blockCount == 0 ) + { + return; + } + + const int workerIndex = 0; + + if ( blockCount == 1 ) + { + b3ExecuteBlock( stage, context, stage->blocks[0].block, workerIndex ); + } + else + { + b3AtomicStoreU32( &context->atomicSyncBits, syncBits ); + + int syncIndex = ( syncBits >> 16 ) & 0xFFFF; + B3_ASSERT( syncIndex > 0 ); + int previousSyncIndex = syncIndex - 1; + + b3ExecuteStage( stage, context, previousSyncIndex, syncIndex, workerIndex ); + + // Spin waiting for thieves to finish + while ( b3AtomicLoadInt( &stage->completionCount ) != blockCount ) + { + b3Pause(); + } + + b3AtomicStoreInt( &stage->completionCount, 0 ); + } +} + +// Parallel solver task +static void b3SolverTask( void* taskContext ) +{ + b3WorkerContext* workerContext = (b3WorkerContext*)taskContext; + int workerIndex = workerContext->workerIndex; + b3StepContext* context = workerContext->context; + int activeColorCount = context->activeColorCount; + b3SolverStage* stages = context->stages; + b3Profile* profile = &context->world->profile; + + if ( workerIndex == 0 ) + { + // The orchestrator slot is a race. The calling thread of b3World_Step also enters here + // as worker 0, so progress is guaranteed even if the user's task system schedules tasks + // out of order, has fewer threads than workerCount, or runs the task synchronously + // inside enqueueTaskFcn. Whoever wins the CAS becomes the orchestrator; the loser + // returns and lets the spinner-only path handle workers >0. + if ( b3AtomicCompareExchangeInt( &context->mainClaimed, 0, 1 ) == false ) + { + return; + } + + // Main thread synchronizes the workers and does work itself. + // + // This needs to be a task for the main thread because the user's task system may execute + // the tasks serially and this is the first task. This single task is able to fully + // complete all work even if all other workers are blocked. + + // Stages are re-used by loops so that I don't need more stages for large substep counts. + // The sync indices grow monotonically for the body/graph/constraint groupings because they share solver blocks. + // The stage index and sync indices are combined in to sync bits for atomic synchronization. + // The workers need to compute the previous sync index for a given stage so that CAS works correctly. This + // setup makes this easy to do. + + /* + b3_stagePrepareJoints, + b3_stagePrepareContacts, + b3_stageIntegrateVelocities, + b3_stageWarmStart, + b3_stageSolve, + b3_stageIntegratePositions, + b3_stageRelax, + b3_stageRestitution, + b3_stageStoreImpulses + */ + + uint64_t ticks = b3GetTicks(); + + int bodySyncIndex = 1; + int stageIndex = 0; + + // Prepare joint constraints + uint32_t jointSyncIndex = 1; + uint32_t syncBits = ( jointSyncIndex << 16 ) | stageIndex; + B3_ASSERT( stages[stageIndex].type == b3_stagePrepareJoints ); + b3ExecuteMainStage( stages + stageIndex, context, syncBits ); + stageIndex += 1; + jointSyncIndex += 1; + + // Prepare convex contact constraints + uint32_t convexSyncIndex = 1; + syncBits = ( convexSyncIndex << 16 ) | stageIndex; + B3_ASSERT( stages[stageIndex].type == b3_stagePrepareWideContacts ); + b3ExecuteMainStage( stages + stageIndex, context, syncBits ); + stageIndex += 1; + convexSyncIndex += 1; + + // Prepare mesh contact constraints + uint32_t meshSyncIndex = 1; + syncBits = ( meshSyncIndex << 16 ) | stageIndex; + B3_ASSERT( stages[stageIndex].type == b3_stagePrepareContacts ); + b3ExecuteMainStage( stages + stageIndex, context, syncBits ); + stageIndex += 1; + meshSyncIndex += 1; + + // Single-threaded overflow work. These constraints don't fit in the graph coloring. + b3PrepareJoints_Overflow( context ); + b3PrepareContacts_Overflow( context ); + + profile->prepareConstraints += b3GetMillisecondsAndReset( &ticks ); + + int graphSyncIndex = 1; + int subStepCount = context->subStepCount; + for ( int subStepIndex = 0; subStepIndex < subStepCount; ++subStepIndex ) + { + // stageIndex restarted each iteration + // syncBits still increases monotonically because the upper bits increase each iteration + int iterationStageIndex = stageIndex; + + // Integrate velocities + syncBits = ( bodySyncIndex << 16 ) | iterationStageIndex; + B3_ASSERT( stages[iterationStageIndex].type == b3_stageIntegrateVelocities ); + b3ExecuteMainStage( stages + iterationStageIndex, context, syncBits ); + iterationStageIndex += 1; + bodySyncIndex += 1; + + profile->integrateVelocities += b3GetMillisecondsAndReset( &ticks ); + + // Warm start constraints + b3WarmStartJoints_Overflow( context ); + b3WarmStartContacts_Overflow( context ); + + for ( int colorIndex = 0; colorIndex < activeColorCount; ++colorIndex ) + { + syncBits = ( graphSyncIndex << 16 ) | iterationStageIndex; + B3_ASSERT( stages[iterationStageIndex].type == b3_stageWarmStart ); + b3ExecuteMainStage( stages + iterationStageIndex, context, syncBits ); + iterationStageIndex += 1; + } + graphSyncIndex += 1; + + profile->warmStart += b3GetMillisecondsAndReset( &ticks ); + + // Solve constraints + bool useBias = true; + for ( int j = 0; j < ITERATIONS; ++j ) + { + // Overflow constraints have lower priority. Typically these are dynamic-vs-dynamic. + b3SolveJoints_Overflow( context, useBias ); + b3SolveContacts_Overflow( context, useBias ); + + for ( int colorIndex = 0; colorIndex < activeColorCount; ++colorIndex ) + { + syncBits = ( graphSyncIndex << 16 ) | iterationStageIndex; + B3_ASSERT( stages[iterationStageIndex].type == b3_stageSolve ); + b3ExecuteMainStage( stages + iterationStageIndex, context, syncBits ); + iterationStageIndex += 1; + } + graphSyncIndex += 1; + } + + profile->solveImpulses += b3GetMillisecondsAndReset( &ticks ); + + // Integrate positions + B3_ASSERT( stages[iterationStageIndex].type == b3_stageIntegratePositions ); + syncBits = ( bodySyncIndex << 16 ) | iterationStageIndex; + b3ExecuteMainStage( stages + iterationStageIndex, context, syncBits ); + iterationStageIndex += 1; + bodySyncIndex += 1; + + profile->integratePositions += b3GetMillisecondsAndReset( &ticks ); + + // Relax constraints + useBias = false; + for ( int j = 0; j < RELAX_ITERATIONS; ++j ) + { + b3SolveJoints_Overflow( context, useBias ); + b3SolveContacts_Overflow( context, useBias ); + + for ( int colorIndex = 0; colorIndex < activeColorCount; ++colorIndex ) + { + syncBits = ( graphSyncIndex << 16 ) | iterationStageIndex; + B3_ASSERT( stages[iterationStageIndex].type == b3_stageRelax ); + b3ExecuteMainStage( stages + iterationStageIndex, context, syncBits ); + iterationStageIndex += 1; + } + graphSyncIndex += 1; + } + + profile->relaxImpulses += b3GetMillisecondsAndReset( &ticks ); + } + + // Advance the stage according to the sub-stepping tasks just completed + // integrate velocities / warm start / solve / integrate positions / relax + stageIndex += 1 + activeColorCount + ITERATIONS * activeColorCount + 1 + RELAX_ITERATIONS * activeColorCount; + + // Restitution + { + b3ApplyRestitution_Overflow( context ); + + int iterStageIndex = stageIndex; + for ( int colorIndex = 0; colorIndex < activeColorCount; ++colorIndex ) + { + syncBits = ( graphSyncIndex << 16 ) | iterStageIndex; + B3_ASSERT( stages[iterStageIndex].type == b3_stageRestitution ); + b3ExecuteMainStage( stages + iterStageIndex, context, syncBits ); + iterStageIndex += 1; + } + // graphSyncIndex += 1; + stageIndex += activeColorCount; + } + + profile->applyRestitution += b3GetMillisecondsAndReset( &ticks ); + + // Store impulses + b3StoreImpulses_Overflow( context ); + + syncBits = ( convexSyncIndex << 16 ) | stageIndex; + B3_ASSERT( stages[stageIndex].type == b3_stageStoreWideImpulses ); + b3ExecuteMainStage( stages + stageIndex, context, syncBits ); + stageIndex += 1; + + syncBits = ( meshSyncIndex << 16 ) | stageIndex; + B3_ASSERT( stages[stageIndex].type == b3_stageStoreImpulses ); + b3ExecuteMainStage( stages + stageIndex, context, syncBits ); + stageIndex += 1; + + profile->storeImpulses += b3GetMillisecondsAndReset( &ticks ); + + // Signal workers to finish + b3AtomicStoreU32( &context->atomicSyncBits, UINT_MAX ); + + B3_ASSERT( stageIndex == context->stageCount ); + return; + } + + // Worker spins and waits for work + uint32_t lastSyncBits = 0; + // uint64_t maxSpinTime = 10; + while ( true ) + { + // Spin until main thread bumps changes the sync bits. This can waste significant time overall, but it is necessary for + // parallel simulation with graph coloring. + // todo improve this spinner + uint32_t syncBits; + int spinCount = 0; + while ( ( syncBits = b3AtomicLoadU32( &context->atomicSyncBits ) ) == lastSyncBits ) + { + if ( spinCount > 5 ) + { + b3Yield(); + spinCount = 0; + } + else + { + // Using the cycle counter helps to account for variation in mm_pause timing across different + // CPUs. However, this is X64 only. + // uint64_t prev = __rdtsc(); + // do + //{ + // b3Pause(); + //} + // while ((__rdtsc() - prev) < maxSpinTime); + // maxSpinTime += 10; + b3Pause(); + b3Pause(); + spinCount += 1; + } + } + + if ( syncBits == UINT_MAX ) + { + // sentinel hit + break; + } + + int stageIndex = syncBits & 0xFFFF; + B3_ASSERT( stageIndex < context->stageCount ); + + int syncIndex = ( syncBits >> 16 ) & 0xFFFF; + B3_ASSERT( syncIndex > 0 ); + + int previousSyncIndex = syncIndex - 1; + + b3SolverStage* stage = stages + stageIndex; + b3ExecuteStage( stage, context, previousSyncIndex, syncIndex, workerIndex ); + + lastSyncBits = syncBits; + } +} + +static void b3BulletBodyTask( int startIndex, int endIndex, int workerIndex, void* context ) +{ + b3TracyCZoneNC( bullet_body_task, "Bullet Body Task", b3_colorLightSkyBlue, true ); + + b3StepContext* stepContext = (b3StepContext*)context; + b3TaskContext* taskContext = b3Array_Get( stepContext->world->taskContexts, workerIndex ); + + B3_ASSERT( startIndex <= endIndex ); + + for ( int i = startIndex; i < endIndex; ++i ) + { + int simIndex = stepContext->bulletBodies[i]; + b3SolveContinuous( stepContext->world, simIndex, taskContext ); + } + + b3TracyCZoneEnd( bullet_body_task ); +} + +#if B3_SIMD_WIDTH == 4 +#define B3_SIMD_SHIFT 2 +#else +#define B3_SIMD_SHIFT 0 +#endif + +// Solve with graph coloring +void b3Solve( b3World* world, b3StepContext* stepContext ) +{ + // Only count steps that advance the simulation + world->stepIndex += 1; + + b3SolverSet* awakeSet = b3Array_Get( world->solverSets, b3_awakeSet ); + int awakeBodyCount = awakeSet->bodySims.count; + if ( awakeBodyCount == 0 ) + { + b3ValidateNoEnlarged( &world->broadPhase ); + return; + } + + // Solve constraints using graph coloring + { + b3TracyCZoneNC( solver_setup, "Solver Setup", b3_colorDarkOrange, true ); + uint64_t setupTicks = b3GetTicks(); + + // Prepare buffers for continuous collision (fast bodies) + b3AtomicStoreInt( &stepContext->bulletBodyCount, 0 ); + stepContext->bulletBodies = (int*)b3StackAlloc( &world->stack, awakeBodyCount * sizeof( int ), "bullet bodies" ); + + b3ConstraintGraph* graph = &world->constraintGraph; + b3GraphColor* colors = graph->colors; + + stepContext->sims = awakeSet->bodySims.data; + stepContext->states = awakeSet->bodyStates.data; + + // count contacts, joints, and colors + int activeColorCount = 0; + for ( int i = 0; i < B3_GRAPH_COLOR_COUNT - 1; ++i ) + { + int perColorContactCount = colors[i].convexContacts.count + colors[i].contacts.count; + int perColorJointCount = colors[i].jointSims.count; + int occupancyCount = perColorContactCount + perColorJointCount; + activeColorCount += occupancyCount > 0 ? 1 : 0; + } + + // prepare for move events + b3Array_Resize( world->bodyMoveEvents, awakeBodyCount ); + + int workerCount = world->workerCount; + + // Target 4 blocks per worker to allow work stealing + const int maxBlockCount = 4 * workerCount; + + // Body blocks are for parallel iteration over bodies directly (integration, update transforms) + int minBodiesPerBlock = 32; + b3BlockDim bodyDim = b3ComputeBlockCount( awakeBodyCount, minBodiesPerBlock, maxBlockCount ); + + const int minContactsPerBlock = 4; + const int minJointsPerBlock = 4; + + // Configure blocks for tasks parallel-for each active graph color + // The blocks are a mix of convex contact, mesh contact, and joint blocks + int activeColorIndices[B3_GRAPH_COLOR_COUNT]; + int colorWideContactCounts[B3_GRAPH_COLOR_COUNT]; + int colorContactCounts[B3_GRAPH_COLOR_COUNT]; + // int colorManifoldCounts[B3_GRAPH_COLOR_COUNT]; + int colorJointCounts[B3_GRAPH_COLOR_COUNT]; + b3BlockDim graphWideContactDims[B3_GRAPH_COLOR_COUNT]; + b3BlockDim graphContactDims[B3_GRAPH_COLOR_COUNT]; + b3BlockDim graphJointDims[B3_GRAPH_COLOR_COUNT]; + int graphBlockCount = 0; + + // c is the active color index + int wideContactCount = 0; + int contactCount = 0; + int manifoldCount = 0; + int jointCount = 0; + int c = 0; + for ( int i = 0; i < B3_GRAPH_COLOR_COUNT - 1; ++i ) + { + b3GraphColor* color = colors + i; + int colorConvexContactCount = color->convexContacts.count; + int colorContactCount = color->contacts.count; + int colorJointCount = color->jointSims.count; + + if ( colorConvexContactCount + colorContactCount + colorJointCount == 0 ) + { + continue; + } + + activeColorIndices[c] = i; + + // Ceiling for wide constraint count + int colorWideConstraintCount = + colorConvexContactCount > 0 ? ( ( colorConvexContactCount - 1 ) >> B3_SIMD_SHIFT ) + 1 : 0; + wideContactCount += colorWideConstraintCount; + colorWideContactCounts[c] = colorWideConstraintCount; + + colorContactCounts[c] = colorContactCount; + contactCount += colorContactCount; + + // Compute manifold starts and accumulate manifold count + for ( int j = 0; j < colorContactCount; ++j ) + { + color->contacts.data[j].manifoldStart = manifoldCount; + manifoldCount += color->contacts.data[j].manifoldCount; + } + + colorJointCounts[c] = colorJointCount; + jointCount += colorJointCount; + + // Solver block dimensions + graphWideContactDims[c] = b3ComputeBlockCount( colorWideConstraintCount, minContactsPerBlock, maxBlockCount ); + graphContactDims[c] = b3ComputeBlockCount( colorContactCount, minContactsPerBlock, maxBlockCount ); + graphJointDims[c] = b3ComputeBlockCount( colorJointCount, minJointsPerBlock, maxBlockCount ); + graphBlockCount += graphWideContactDims[c].count + graphContactDims[c].count + graphJointDims[c].count; + + c += 1; + } + activeColorCount = c; + + // Prepare and store run as one flat parallel-for over the entire wide constraint range, + // partitioned into uniformly sized blocks. Color info is consulted inside the task via + // a small span array, so blocks do not need to honor color boundaries here. + b3BlockDim convexPrepareDim = b3ComputeBlockCount( wideContactCount, minContactsPerBlock, maxBlockCount ); + b3BlockDim meshPrepareDim = b3ComputeBlockCount( contactCount, minContactsPerBlock, maxBlockCount ); + b3BlockDim jointPrepareDim = b3ComputeBlockCount( jointCount, minJointsPerBlock, maxBlockCount ); + + int wideContactByteCount = b3GetWideContactConstraintByteCount(); + b3ContactConstraintWide* wideConstraints = + (b3ContactConstraintWide*)b3StackAlloc( &world->stack, wideContactCount * wideContactByteCount, "wide contacts" ); + b3ContactConstraint* contactConstraints = + (b3ContactConstraint*)b3StackAlloc( &world->stack, contactCount * sizeof( b3ContactConstraint ), "contacts" ); + b3ManifoldConstraint* manifoldConstraints = (b3ManifoldConstraint*)b3StackAlloc( + &world->stack, manifoldCount * sizeof( b3ManifoldConstraint ), "manifold constraints" ); + + b3GraphColor* overflow = colors + B3_OVERFLOW_INDEX; + int overflowCount = overflow->contacts.count; + int overflowManifoldCount = 0; + for ( int i = 0; i < overflowCount; ++i ) + { + overflow->contacts.data[i].manifoldStart = overflowManifoldCount; + overflowManifoldCount += overflow->contacts.data[i].manifoldCount; + } + + overflow->contactConstraints = (b3ContactConstraint*)b3StackAlloc( + &world->stack, overflowCount * sizeof( b3ContactConstraint ), "overflow contacts" ); + overflow->manifoldConstraints = (b3ManifoldConstraint*)b3StackAlloc( + &world->stack, overflowManifoldCount * sizeof( b3ManifoldConstraint ), "overflow manifolds" ); + + // Build the span table for the flat prepare/store parallel-for while I slice the + // wide constraint buffer across colors. One entry per active color plus a sentinel + // at wideContactCount. + b3WidePrepareSpan widePrepareSpans[B3_GRAPH_COLOR_COUNT + 1]; + b3ContactPrepareSpan contactPrepareSpans[B3_GRAPH_COLOR_COUNT + 1]; + b3JointPrepareSpan jointPrepareSpans[B3_GRAPH_COLOR_COUNT + 1]; + + // Distribute transient constraints to each graph color and prepare spans + // todo it might be simpler for solver blocks to index into the global arrays + { + int wideBase = 0; + int contactBase = 0; + int jointBase = 0; + for ( int i = 0; i < activeColorCount; ++i ) + { + int j = activeColorIndices[i]; + b3GraphColor* color = colors + j; + + int colorConvexContactCount = color->convexContacts.count; + widePrepareSpans[i].start = wideBase; + widePrepareSpans[i].count = colorConvexContactCount; + widePrepareSpans[i].contacts = color->convexContacts.data; + + if ( colorConvexContactCount == 0 ) + { + color->wideConstraints = NULL; + color->wideConstraintCount = 0; + } + else + { + color->wideConstraints = + (b3ContactConstraintWide*)( (uint8_t*)wideConstraints + wideBase * wideContactByteCount ); + + int colorContactCountW = ( ( colorConvexContactCount - 1 ) >> B3_SIMD_SHIFT ) + 1; + color->wideConstraintCount = colorContactCountW; + + // Zero remainder lanes in the tail wide slot so prepare workers don't need to + // initialize them. + if ( ( colorConvexContactCount & ( B3_SIMD_WIDTH - 1 ) ) != 0 ) + { + memset( (uint8_t*)color->wideConstraints + ( colorContactCountW - 1 ) * wideContactByteCount, 0, + wideContactByteCount ); + } + + wideBase += colorContactCountW; + } + + int colorContactCount = color->contacts.count; + contactPrepareSpans[i].start = contactBase; + contactPrepareSpans[i].count = colorContactCount; + contactPrepareSpans[i].contacts = color->contacts.data; + + if ( colorContactCount == 0 ) + { + color->contactConstraints = NULL; + color->contactConstraintCount = 0; + } + else + { + color->contactConstraints = contactConstraints + contactBase; + color->contactConstraintCount = colorContactCount; + contactBase += colorContactCount; + } + + jointPrepareSpans[i].start = jointBase; + jointPrepareSpans[i].count = color->jointSims.count; + jointPrepareSpans[i].joints = color->jointSims.data; + jointBase += color->jointSims.count; + } + + // Sentinels + widePrepareSpans[activeColorCount].start = wideContactCount; + widePrepareSpans[activeColorCount].count = 0; + widePrepareSpans[activeColorCount].contacts = NULL; + B3_ASSERT( wideBase == wideContactCount ); + + contactPrepareSpans[activeColorCount].start = contactCount; + contactPrepareSpans[activeColorCount].count = 0; + contactPrepareSpans[activeColorCount].contacts = NULL; + B3_ASSERT( contactBase == contactCount ); + + jointPrepareSpans[activeColorCount].start = jointCount; + jointPrepareSpans[activeColorCount].count = 0; + jointPrepareSpans[activeColorCount].joints = NULL; + B3_ASSERT( jointBase == jointCount ); + } + + //// Special span for overflow to allow for function re-use + b3ContactPrepareSpan overflowSpans[2] = { 0 }; + overflowSpans[0].start = 0; + overflowSpans[0].count = overflow->contacts.count; + overflowSpans[0].contacts = overflow->contacts.data; + overflowSpans[1].start = overflow->contacts.count; + overflowSpans[1].count = 0; + overflowSpans[1].contacts = NULL; + + int stageCount = 0; + + // b3_stagePrepareJoints + stageCount += 1; + // b3_stagePrepareWideContacts + stageCount += 1; + // b3_stagePrepareContacts + stageCount += 1; + // b3_stageIntegrateVelocities + stageCount += 1; + // b3_stageWarmStart + stageCount += activeColorCount; + // b3_stageSolve + stageCount += ITERATIONS * activeColorCount; + // b3_stageIntegratePositions + stageCount += 1; + // b3_stageRelax + stageCount += RELAX_ITERATIONS * activeColorCount; + // b3_stageRestitution + stageCount += activeColorCount; + // b3_stageStoreWideImpulses + stageCount += 1; + // b3_stageStoreImpulses + stageCount += 1; + + b3SolverStage* stages = (b3SolverStage*)b3StackAlloc( &world->stack, stageCount * sizeof( b3SolverStage ), "stages" ); + b3SyncBlock* bodyBlocks = + (b3SyncBlock*)b3StackAlloc( &world->stack, bodyDim.count * sizeof( b3SyncBlock ), "body blocks" ); + b3SyncBlock* convexBlocks = + (b3SyncBlock*)b3StackAlloc( &world->stack, convexPrepareDim.count * sizeof( b3SyncBlock ), "convex blocks" ); + b3SyncBlock* meshBlocks = + (b3SyncBlock*)b3StackAlloc( &world->stack, meshPrepareDim.count * sizeof( b3SyncBlock ), "mesh blocks" ); + b3SyncBlock* jointBlocks = + (b3SyncBlock*)b3StackAlloc( &world->stack, jointPrepareDim.count * sizeof( b3SyncBlock ), "joint blocks" ); + b3SyncBlock* graphBlocks = + (b3SyncBlock*)b3StackAlloc( &world->stack, graphBlockCount * sizeof( b3SyncBlock ), "graph blocks" ); + + // Split an awake island. This modifies: + // - stack allocator + // - world island array and solver set + // - island indices on bodies, contacts, and joints + // I'm squeezing this task in here because it may be expensive and this is a safe place to put it. + // Note: cannot split islands in parallel with FinalizeBodies + void* splitIslandTask = NULL; + if ( world->splitIslandId != B3_NULL_INDEX ) + { + if ( world->taskCount < B3_MAX_TASKS ) + { + splitIslandTask = world->enqueueTaskFcn( &b3SplitIslandTask, world, world->userTaskContext, "split" ); + world->taskCount += 1; + world->activeTaskCount += splitIslandTask == NULL ? 0 : 1; + } + else + { + b3SplitIslandTask( world ); + } + } + + // Prepare body blocks + b3InitBlocks( bodyBlocks, bodyDim, awakeBodyCount, b3_bodyBlock, UINT8_MAX ); + + // Prepare blocks as a single flat parallel-for over the whole constraint range. + // The task walks spans to decode flat slot indices back to per-color arrays. + b3InitBlocks( convexBlocks, convexPrepareDim, wideContactCount, b3_wideContactBlock, UINT8_MAX ); + b3InitBlocks( meshBlocks, meshPrepareDim, contactCount, b3_contactBlock, UINT8_MAX ); + b3InitBlocks( jointBlocks, jointPrepareDim, jointCount, b3_jointBlock, UINT8_MAX ); + + // Prepare graph work blocks. Each color gets joint blocks followed by contact blocks. + b3SyncBlock* graphColorBlocks[B3_GRAPH_COLOR_COUNT] = { 0 }; + b3SyncBlock* baseGraphBlock = graphBlocks; + int graphBlockCounts[B3_GRAPH_COLOR_COUNT] = { 0 }; + for ( int i = 0; i < activeColorCount; ++i ) + { + graphColorBlocks[i] = baseGraphBlock; + + uint8_t colorIndex = (uint8_t)activeColorIndices[i]; + b3InitBlocks( baseGraphBlock, graphJointDims[i], colorJointCounts[i], b3_graphJointBlock, colorIndex ); + baseGraphBlock += graphJointDims[i].count; + + b3InitBlocks( baseGraphBlock, graphWideContactDims[i], colorWideContactCounts[i], b3_graphWideContactBlock, + colorIndex ); + baseGraphBlock += graphWideContactDims[i].count; + + b3InitBlocks( baseGraphBlock, graphContactDims[i], colorContactCounts[i], b3_graphContactBlock, colorIndex ); + baseGraphBlock += graphContactDims[i].count; + + graphBlockCounts[i] = graphJointDims[i].count + graphWideContactDims[i].count + graphContactDims[i].count; + } + + B3_ASSERT( (ptrdiff_t)( baseGraphBlock - graphBlocks ) == graphBlockCount ); + + b3SolverStage* stage = stages; + stage = b3InitStage( stage, b3_stagePrepareJoints, jointBlocks, jointPrepareDim.count, UINT8_MAX ); + stage = b3InitStage( stage, b3_stagePrepareWideContacts, convexBlocks, convexPrepareDim.count, UINT8_MAX ); + stage = b3InitStage( stage, b3_stagePrepareContacts, meshBlocks, meshPrepareDim.count, UINT8_MAX ); + stage = b3InitStage( stage, b3_stageIntegrateVelocities, bodyBlocks, bodyDim.count, UINT8_MAX ); + stage = b3InitColorStages( stage, b3_stageWarmStart, 1, activeColorCount, graphColorBlocks, graphBlockCounts, + activeColorIndices ); + stage = b3InitColorStages( stage, b3_stageSolve, ITERATIONS, activeColorCount, graphColorBlocks, graphBlockCounts, + activeColorIndices ); + stage = b3InitStage( stage, b3_stageIntegratePositions, bodyBlocks, bodyDim.count, UINT8_MAX ); + stage = b3InitColorStages( stage, b3_stageRelax, RELAX_ITERATIONS, activeColorCount, graphColorBlocks, graphBlockCounts, + activeColorIndices ); + // Note: joint blocks mixed in, could have joint limit restitution + stage = b3InitColorStages( stage, b3_stageRestitution, 1, activeColorCount, graphColorBlocks, graphBlockCounts, + activeColorIndices ); + stage = b3InitStage( stage, b3_stageStoreWideImpulses, convexBlocks, convexPrepareDim.count, UINT8_MAX ); + stage = b3InitStage( stage, b3_stageStoreImpulses, meshBlocks, meshPrepareDim.count, UINT8_MAX ); + + B3_ASSERT( (int)( stage - stages ) == stageCount ); + + B3_ASSERT( workerCount <= B3_MAX_WORKERS ); + b3WorkerContext workerContext[B3_MAX_WORKERS]; + + stepContext->graph = graph; + stepContext->activeColorCount = activeColorCount; + stepContext->workerCount = workerCount; + stepContext->stageCount = stageCount; + stepContext->stages = stages; + stepContext->wideConstraints = wideConstraints; + stepContext->widePrepareSpans = widePrepareSpans; + stepContext->wideContactCount = wideContactCount; + stepContext->manifoldConstraints = manifoldConstraints; + stepContext->contactConstraints = contactConstraints; + stepContext->contactPrepareSpans = contactPrepareSpans; + stepContext->overflowSpans = overflowSpans; + stepContext->jointPrepareSpans = jointPrepareSpans; + b3AtomicStoreU32( &stepContext->atomicSyncBits, 0 ); + b3AtomicStoreInt( &stepContext->mainClaimed, 0 ); + + world->profile.solverSetup = b3GetMillisecondsAndReset( &setupTicks ); + b3TracyCZoneEnd( solver_setup ); + + b3TracyCZoneNC( solve_constraints, "Solve Constraints", b3_colorIndigo, true ); + uint64_t constraintTicks = b3GetTicks(); + + int jointIdCapacity = b3GetIdCapacity( &world->jointIdPool ); + int contactIdCapacity = b3GetIdCapacity( &world->contactIdPool ); + for ( int i = 0; i < workerCount; ++i ) + { + b3TaskContext* taskContext = b3Array_Get( world->taskContexts, i ); + b3SetBitCountAndClear( &taskContext->jointStateBitSet, jointIdCapacity ); + b3SetBitCountAndClear( &taskContext->hitEventBitSet, contactIdCapacity ); + taskContext->hasHitEvents = false; + + workerContext[i].context = stepContext; + workerContext[i].workerIndex = i; + + if ( world->taskCount < B3_MAX_TASKS ) + { + char buffer[16]; + snprintf( buffer, sizeof( buffer ), "solve[%d]", i ); + workerContext[i].userTask = + world->enqueueTaskFcn( &b3SolverTask, workerContext + i, world->userTaskContext, buffer ); + world->taskCount += 1; + world->activeTaskCount += workerContext[i].userTask == NULL ? 0 : 1; + } + else + { + workerContext[i].userTask = NULL; + b3SolverTask( workerContext + i ); + } + } + + // The calling thread of b3World_Step also enters b3SolverTask as worker 0 and races for the + // orchestrator slot via the CAS inside. This guarantees progress even when the user's task + // system can't run the queued worker 0 promptly: it might schedule out of order, have fewer + // threads than workerCount, or invert priority by parking the calling thread in finishTaskFcn. + // Whoever wins the CAS becomes the orchestrator; the loser returns and lets the spinner-only + // path handle workers >0. + b3WorkerContext callerContext = { stepContext, 0, NULL }; + b3SolverTask( &callerContext ); + + // Finish constraint solve + for ( int i = 0; i < workerCount; ++i ) + { + if ( workerContext[i].userTask != NULL ) + { + world->finishTaskFcn( workerContext[i].userTask, world->userTaskContext ); + world->activeTaskCount -= 1; + } + } + + // Finish island split + if ( splitIslandTask != NULL ) + { + world->finishTaskFcn( splitIslandTask, world->userTaskContext ); + world->activeTaskCount -= 1; + } + world->splitIslandId = B3_NULL_INDEX; + + world->profile.constraints = b3GetMillisecondsAndReset( &constraintTicks ); + b3TracyCZoneEnd( solve_constraints ); + + b3TracyCZoneNC( update_transforms, "Update Transforms", b3_colorMediumSeaGreen, true ); + uint64_t transformTicks = b3GetTicks(); + + // Prepare contact, enlarged body, and island bit sets used in body finalization. + int awakeIslandCount = awakeSet->islandSims.count; + for ( int i = 0; i < world->workerCount; ++i ) + { + b3TaskContext* taskContext = world->taskContexts.data + i; + b3Array_Clear( taskContext->sensorHits ); + b3SetBitCountAndClear( &taskContext->enlargedSimBitSet, awakeBodyCount ); + b3SetBitCountAndClear( &taskContext->awakeIslandBitSet, awakeIslandCount ); + taskContext->splitIslandId = B3_NULL_INDEX; + taskContext->splitSleepTime = 0.0f; + } + + // Finalize bodies. Must happen after the constraint solver and after island splitting. + b3ParallelFor( world, &b3FinalizeBodiesTask, awakeBodyCount, 16, stepContext, "ccd" ); + + // Free in reverse order + b3StackFree( &world->stack, graphBlocks ); + b3StackFree( &world->stack, jointBlocks ); + b3StackFree( &world->stack, meshBlocks ); + b3StackFree( &world->stack, convexBlocks ); + b3StackFree( &world->stack, bodyBlocks ); + b3StackFree( &world->stack, stages ); + b3StackFree( &world->stack, overflow->manifoldConstraints ); + b3StackFree( &world->stack, overflow->contactConstraints ); + b3StackFree( &world->stack, manifoldConstraints ); + b3StackFree( &world->stack, contactConstraints ); + b3StackFree( &world->stack, wideConstraints ); + + world->profile.transforms = b3GetMilliseconds( transformTicks ); + b3TracyCZoneEnd( update_transforms ); + } + + // Report joint events + { + b3TracyCZoneNC( joint_events, "Joint Events", b3_colorPeru, true ); + uint64_t jointEventTicks = b3GetTicks(); + + // Gather bits for all joints that have force/torque events + b3BitSet* jointStateBitSet = &world->taskContexts.data[0].jointStateBitSet; + for ( int i = 1; i < world->workerCount; ++i ) + { + b3InPlaceUnion( jointStateBitSet, &world->taskContexts.data[i].jointStateBitSet ); + } + + { + uint32_t wordCount = jointStateBitSet->blockCount; + uint64_t* bits = jointStateBitSet->bits; + + b3Joint* jointArray = world->joints.data; + uint16_t worldIndex0 = world->worldId; + + for ( uint32_t k = 0; k < wordCount; ++k ) + { + uint64_t word = bits[k]; + while ( word != 0 ) + { + uint32_t ctz = b3CTZ64( word ); + int jointId = (int)( 64 * k + ctz ); + + B3_ASSERT( jointId < world->joints.capacity ); + + b3Joint* joint = jointArray + jointId; + + B3_ASSERT( joint->setIndex == b3_awakeSet ); + + b3JointEvent event = { + .jointId = + { + .index1 = jointId + 1, + .world0 = worldIndex0, + .generation = joint->generation, + }, + .userData = joint->userData, + }; + + b3Array_Push( world->jointEvents, event ); + + // Clear the smallest set bit + word = word & ( word - 1 ); + } + } + } + + world->profile.jointEvents = b3GetMilliseconds( jointEventTicks ); + b3TracyCZoneEnd( joint_events ); + } + + // Report hit events + { + b3TracyCZoneNC( hit_events, "Hit Events", b3_colorRosyBrown, true ); + uint64_t hitTicks = b3GetTicks(); + + B3_ASSERT( world->contactHitEvents.count == 0 ); + + // Fast path: if no worker flagged any hit-event candidates during b2StoreImpulsesTask, skip entirely. + bool anyHitEvents = false; + for ( int i = 0; i < world->workerCount; ++i ) + { + if ( world->taskContexts.data[i].hasHitEvents ) + { + anyHitEvents = true; + break; + } + } + + if ( anyHitEvents ) + { + // Union per-worker bits into worker 0's bit set. + b3BitSet* hitEventBitSet = &world->taskContexts.data[0].hitEventBitSet; + for ( int i = 1; i < world->workerCount; ++i ) + { + if ( world->taskContexts.data[i].hasHitEvents ) + { + b3InPlaceUnion( hitEventBitSet, &world->taskContexts.data[i].hitEventBitSet ); + } + } + + float threshold = world->hitEventThreshold; + b3Contact* contactArray = world->contacts.data; + uint16_t worldId = world->worldId; + + uint32_t wordCount = hitEventBitSet->blockCount; + uint64_t* bits = hitEventBitSet->bits; + for ( uint32_t k = 0; k < wordCount; ++k ) + { + uint64_t word = bits[k]; + while ( word != 0 ) + { + uint32_t ctz = b3CTZ64( word ); + int contactId = (int)( 64 * k + ctz ); + + b3Contact* contact = contactArray + contactId; + B3_ASSERT( contact->setIndex == b3_awakeSet && contact->colorIndex != B3_NULL_INDEX ); + + b3Shape* shapeA = b3Array_Get( world->shapes, contact->shapeIdA ); + b3Shape* shapeB = b3Array_Get( world->shapes, contact->shapeIdB ); + b3Body* bodyA = b3Array_Get( world->bodies, shapeA->bodyId ); + b3Body* bodyB = b3Array_Get( world->bodies, shapeB->bodyId ); + b3BodySim* simA = b3GetBodySim( world, bodyA ); + b3BodySim* simB = b3GetBodySim( world, bodyB ); + b3Pos midCenter = b3LerpPosition( simA->center, simB->center, 0.5f ); + + b3ContactHitEvent event = { 0 }; + event.approachSpeed = threshold; + + bool found = false; + int triangleIndex = 0; + int manifoldCount = contact->manifoldCount; + for ( int i = 0; i < manifoldCount; ++i ) + { + b3Manifold* manifold = contact->manifolds + i; + int pointCount = manifold->pointCount; + for ( int p = 0; p < pointCount; ++p ) + { + b3ManifoldPoint* mp = manifold->points + p; + float approachSpeed = -mp->normalVelocity; + + // Need to check total impulse because the point may be speculative and not colliding + if ( approachSpeed > event.approachSpeed && mp->totalNormalImpulse > 0.0f ) + { + event.approachSpeed = approachSpeed; + event.point = b3OffsetPos( midCenter, b3Lerp( mp->anchorA, mp->anchorB, 0.5f ) ); + event.normal = manifold->normal; + triangleIndex = mp->triangleIndex; + found = true; + } + } + } + + if ( found == true ) + { + event.shapeIdA = (b3ShapeId){ shapeA->id + 1, worldId, shapeA->generation }; + event.shapeIdB = (b3ShapeId){ shapeB->id + 1, worldId, shapeB->generation }; + + event.contactId = (b3ContactId){ + .index1 = contact->contactId + 1, + .world0 = worldId, + .padding = 0, + .generation = contact->generation, + }; + + // shapeB is never a compound today (asserted in b3CreateContact), so the + // childIndex argument is irrelevant for it. shapeA carries the compound. + event.userMaterialIdA = b3GetShapeUserMaterialId( shapeA, contact->childIndex, triangleIndex ); + event.userMaterialIdB = b3GetShapeUserMaterialId( shapeB, 0, triangleIndex ); + + b3Array_Push( world->contactHitEvents, event ); + } + + // Clear the smallest set bit + word = word & ( word - 1 ); + } + } + } + + world->profile.hitEvents = b3GetMilliseconds( hitTicks ); + b3TracyCZoneEnd( hit_events ); + } + + { + b3TracyCZoneNC( refit_bvh, "Refit BVH", b3_colorFireBrick, true ); + uint64_t refitTicks = b3GetTicks(); + + // Finish the user tree task that was queued earlier in the time step. This must be complete before touching the + // broad-phase. + if ( world->userTreeTask != NULL ) + { + world->finishTaskFcn( world->userTreeTask, world->userTaskContext ); + world->userTreeTask = NULL; + world->activeTaskCount -= 1; + } + + b3ValidateNoEnlarged( &world->broadPhase ); + + // Gather bits for all sim bodies that have enlarged AABBs + b3BitSet* enlargedBodyBitSet = &world->taskContexts.data[0].enlargedSimBitSet; + for ( int i = 1; i < world->workerCount; ++i ) + { + b3InPlaceUnion( enlargedBodyBitSet, &world->taskContexts.data[i].enlargedSimBitSet ); + } + + // Enlarge broad-phase proxies and build move array + // Apply shape AABB changes to broad-phase. This also create the move array which must be + // in deterministic order. I'm tracking sim bodies because the number of shape ids can be huge. + // This has to happen before bullets are processed. + { + b3BroadPhase* broadPhase = &world->broadPhase; + uint32_t wordCount = enlargedBodyBitSet->blockCount; + uint64_t* bits = enlargedBodyBitSet->bits; + + // Fast array access is important here + b3Body* bodyArray = world->bodies.data; + b3BodySim* bodySimArray = awakeSet->bodySims.data; + b3Shape* shapeArray = world->shapes.data; + + for ( uint32_t k = 0; k < wordCount; ++k ) + { + uint64_t word = bits[k]; + while ( word != 0 ) + { + uint32_t ctz = b3CTZ64( word ); + uint32_t bodySimIndex = 64 * k + ctz; + + b3BodySim* bodySim = bodySimArray + bodySimIndex; + + b3Body* body = bodyArray + bodySim->bodyId; + + int shapeId = body->headShapeId; + if ( ( bodySim->flags & ( b3_isBullet | b3_isFast ) ) == ( b3_isBullet | b3_isFast ) ) + { + // Fast bullet bodies don't have their final AABB yet + while ( shapeId != B3_NULL_INDEX ) + { + b3Shape* shape = shapeArray + shapeId; + + // Shape is fast. It's aabb will be enlarged in continuous collision. + // Update the move array here for determinism because bullets are processed + // below in non-deterministic order. + b3BufferMove( broadPhase, shape->proxyKey ); + + shapeId = shape->nextShapeId; + } + } + else + { + while ( shapeId != B3_NULL_INDEX ) + { + b3Shape* shape = shapeArray + shapeId; + + // The AABB may not have been enlarged, despite the body being flagged as enlarged. + // For example, a body with multiple shapes may have not have all shapes enlarged. + // A fast body may have been flagged as enlarged despite having no shapes enlarged. + if ( shape->flags & b3_enlargedAABB ) + { + b3BroadPhase_EnlargeProxy( broadPhase, shape->proxyKey, shape->fatAABB ); + shape->flags &= ~b3_enlargedAABB; + } + + shapeId = shape->nextShapeId; + } + } + + // Clear the smallest set bit + word = word & ( word - 1 ); + } + } + } + + b3ValidateBroadPhase( &world->broadPhase ); + + world->profile.refit = b3GetMilliseconds( refitTicks ); + b3TracyCZoneEnd( refit_bvh ); + } + + int bulletBodyCount = b3AtomicLoadInt( &stepContext->bulletBodyCount ); + if ( bulletBodyCount > 0 ) + { + b3TracyCZoneNC( bullets, "Bullets", b3_colorDarkGoldenRod, true ); + uint64_t bulletTicks = b3GetTicks(); + + // Fast bullet bodies + // Note: a bullet body may be moving slow + int minRange = 8; + b3ParallelFor( world, &b3BulletBodyTask, bulletBodyCount, minRange, stepContext, "bullets" ); + + // Serially enlarge broad-phase proxies for bullet shapes + b3BroadPhase* broadPhase = &world->broadPhase; + b3DynamicTree* dynamicTree = broadPhase->trees + b3_dynamicBody; + + // Fast array access is important here + b3Body* bodyArray = world->bodies.data; + b3BodySim* bodySimArray = awakeSet->bodySims.data; + b3Shape* shapeArray = world->shapes.data; + + // Serially enlarge broad-phase proxies for bullet shapes + int* bulletBodySimIndices = stepContext->bulletBodies; + + // This loop has non-deterministic order but it shouldn't affect the result + for ( int i = 0; i < bulletBodyCount; ++i ) + { + b3BodySim* bulletBodySim = bodySimArray + bulletBodySimIndices[i]; + if ( ( bulletBodySim->flags & b3_enlargeBounds ) == 0 ) + { + continue; + } + + // Clear flag + bulletBodySim->flags &= ~b3_enlargeBounds; + + int bodyId = bulletBodySim->bodyId; + B3_ASSERT( 0 <= bodyId && bodyId < world->bodies.count ); + b3Body* bulletBody = bodyArray + bodyId; + + int shapeId = bulletBody->headShapeId; + while ( shapeId != B3_NULL_INDEX ) + { + b3Shape* shape = shapeArray + shapeId; + if ( ( shape->flags & b3_enlargedAABB ) == 0 ) + { + shapeId = shape->nextShapeId; + continue; + } + + // clear flag + shape->flags &= ~b3_enlargedAABB; + + int proxyKey = shape->proxyKey; + int proxyId = B3_PROXY_ID( proxyKey ); + B3_ASSERT( B3_PROXY_TYPE( proxyKey ) == b3_dynamicBody ); + + // all fast bullet shapes should already be in the move buffer + B3_ASSERT( b3GetBit( &broadPhase->movedProxies[b3_dynamicBody], proxyId ) ); + + b3DynamicTree_EnlargeProxy( dynamicTree, proxyId, shape->fatAABB ); + + shapeId = shape->nextShapeId; + } + } + + world->profile.bullets = b3GetMilliseconds( bulletTicks ); + b3TracyCZoneEnd( bullets ); + } + + b3StackFree( &world->stack, stepContext->bulletBodies ); + stepContext->bulletBodies = NULL; + b3AtomicStoreInt( &stepContext->bulletBodyCount, 0 ); + + // Report sensor hits. This may include bullets sensor hits. + { + b3TracyCZoneNC( sensor_hits, "Sensor Hits", b3_colorPowderBlue, true ); + uint64_t sensorHitTicks = b3GetTicks(); + + int workerCount = world->workerCount; + B3_ASSERT( workerCount == world->taskContexts.count ); + + for ( int i = 0; i < workerCount; ++i ) + { + b3TaskContext* taskContext = world->taskContexts.data + i; + int hitCount = taskContext->sensorHits.count; + b3SensorHit* hits = taskContext->sensorHits.data; + + for ( int j = 0; j < hitCount; ++j ) + { + b3SensorHit hit = hits[j]; + b3Shape* sensorShape = b3Array_Get( world->shapes, hit.sensorId ); + b3Shape* visitor = b3Array_Get( world->shapes, hit.visitorId ); + + b3Sensor* sensor = b3Array_Get( world->sensors, sensorShape->sensorIndex ); + b3Visitor shapeRef = { + .shapeId = hit.visitorId, + .generation = visitor->generation, + }; + b3Array_Push( sensor->hits, shapeRef ); + } + } + + world->profile.sensorHits = b3GetMilliseconds( sensorHitTicks ); + b3TracyCZoneEnd( sensor_hits ); + } + + // Island sleeping + // This must be done last because putting islands to sleep invalidates the enlarged body bits. + // todo_erin figure out how to do this in parallel with tree refit + if ( world->enableSleep == true ) + { + b3TracyCZoneNC( sleep_islands, "Island Sleep", b3_colorLightSlateGray, true ); + uint64_t sleepTicks = b3GetTicks(); + + // Collect split island candidate for the next time step. No need to split if sleeping is disabled. + B3_ASSERT( world->splitIslandId == B3_NULL_INDEX ); + float splitSleepTimer = 0.0f; + for ( int i = 0; i < world->workerCount; ++i ) + { + b3TaskContext* taskContext = world->taskContexts.data + i; + if ( taskContext->splitIslandId != B3_NULL_INDEX && taskContext->splitSleepTime >= splitSleepTimer ) + { + B3_ASSERT( taskContext->splitSleepTime > 0.0f ); + + // Tie breaking for determinism. Largest island id wins. Needed due to work stealing. + if ( taskContext->splitSleepTime == splitSleepTimer && taskContext->splitIslandId < world->splitIslandId ) + { + continue; + } + + world->splitIslandId = taskContext->splitIslandId; + splitSleepTimer = taskContext->splitSleepTime; + } + } + + b3BitSet* awakeIslandBitSet = &world->taskContexts.data[0].awakeIslandBitSet; + for ( int i = 1; i < world->workerCount; ++i ) + { + b3InPlaceUnion( awakeIslandBitSet, &world->taskContexts.data[i].awakeIslandBitSet ); + } + + // Need to process in reverse because this moves islands to sleeping solver sets. + b3IslandSim* islands = awakeSet->islandSims.data; + int count = awakeSet->islandSims.count; + for ( int islandIndex = count - 1; islandIndex >= 0; islandIndex -= 1 ) + { + if ( b3GetBit( awakeIslandBitSet, islandIndex ) == true ) + { + // this island is still awake + continue; + } + + b3IslandSim* island = islands + islandIndex; + int islandId = island->islandId; + + b3TrySleepIsland( world, islandId ); + } + + b3ValidateSolverSets( world ); + + world->profile.sleepIslands = b3GetMilliseconds( sleepTicks ); + b3TracyCZoneEnd( sleep_islands ); + } +} diff --git a/vendor/box3d/src/src/solver.h b/vendor/box3d/src/src/solver.h new file mode 100644 index 000000000..7c8a1c71b --- /dev/null +++ b/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 + +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, + }; +} diff --git a/vendor/box3d/src/src/solver_set.c b/vendor/box3d/src/src/solver_set.c new file mode 100644 index 000000000..f050a869d --- /dev/null +++ b/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 + +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; + } + } +} diff --git a/vendor/box3d/src/src/solver_set.h b/vendor/box3d/src/src/solver_set.h new file mode 100644 index 000000000..bf78ab04c --- /dev/null +++ b/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 ); diff --git a/vendor/box3d/src/src/sphere.c b/vendor/box3d/src/src/sphere.c new file mode 100644 index 000000000..9e4bdaac0 --- /dev/null +++ b/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; +} diff --git a/vendor/box3d/src/src/spherical_joint.c b/vendor/box3d/src/src/spherical_joint.c new file mode 100644 index 000000000..3f699ac18 --- /dev/null +++ b/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 ); + } + } +} diff --git a/vendor/box3d/src/src/table.c b/vendor/box3d/src/src/table.c new file mode 100644 index 000000000..fb066b807 --- /dev/null +++ b/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 + +#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; +} diff --git a/vendor/box3d/src/src/table.h b/vendor/box3d/src/src/table.h new file mode 100644 index 000000000..ec8f39782 --- /dev/null +++ b/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 +#include + +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 ); diff --git a/vendor/box3d/src/src/timer.c b/vendor/box3d/src/src/timer.c new file mode 100644 index 000000000..826b1495c --- /dev/null +++ b/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 +#include +#include + +#define NAME_LENGTH 16 + +#if defined( _WIN32 ) + +#ifndef WIN32_LEAN_AND_MEAN +#define WIN32_LEAN_AND_MEAN 1 +#endif + +#include +#include + +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 +#include + +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 +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 + +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 +#include +#include +#include + +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 +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 + +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; +} diff --git a/vendor/box3d/src/src/triangle_manifold.c b/vendor/box3d/src/src/triangle_manifold.c new file mode 100644 index 000000000..b87973add --- /dev/null +++ b/vendor/box3d/src/src/triangle_manifold.c @@ -0,0 +1,1274 @@ +// SPDX-FileCopyrightText: 2025 Erin Catto +// SPDX-License-Identifier: MIT + +#include "algorithm.h" +#include "contact.h" +#include "core.h" +#include "manifold.h" +#include "shape.h" + +#include "box3d/base.h" +#include "box3d/collision.h" +#include "box3d/constants.h" + +#include +#include + +typedef struct b3TriangleData +{ + b3Vec3 v1, v2, v3; + b3Vec3 e1, e2, e3; + b3Vec3 center; + b3Plane plane; + int flags; +} b3TriangleData; + +// Indexed by the 3-bit vertex mask +static const b3TriangleFeature s_triangleFeatures[8] = { + b3_featureNone, // 000 (unreachable) + b3_featureVertex1, // 001 + b3_featureVertex2, // 010 + b3_featureEdge1, // 011 v1,v2 + b3_featureVertex3, // 100 + b3_featureEdge3, // 101 v1,v3 + b3_featureEdge2, // 110 v2,v3 + b3_featureTriangleFace, // 111 +}; + +static b3TriangleFeature b3GetTriangleFeature( const b3SimplexCache* cache ) +{ + int count = cache->count; + B3_ASSERT( 0 < count && count < 4 ); + + // Bit i set means triangle vertex i participates in the simplex. + int mask = 0; + for ( int i = 0; i < count; ++i ) + { + B3_ASSERT( cache->indexA[i] < 3 ); + mask |= 1 << cache->indexA[i]; + } + + return s_triangleFeatures[mask]; +} + +void b3CollideSphereAndTriangle( b3LocalManifold* manifold, int capacity, const b3Sphere* sphereA, const b3Vec3* triangleB ) +{ + manifold->pointCount = 0; + + if ( capacity == 0 ) + { + return; + } + + b3Vec3 center = sphereA->center; + b3Vec3 v1 = triangleB[0], v2 = triangleB[1], v3 = triangleB[2]; + b3Plane plane = b3MakePlaneFromPoints( v1, v2, v3 ); + + float offset = b3PlaneSeparation( plane, center ); + if ( offset < 0.0f ) + { + // Cull back side collision + return; + } + + // Closest point on triangle to sphere center + b3TrianglePoint closest = b3ClosestPointOnTriangle( v1, v2, v3, center ); + + // Test separating axis + float squaredDistance = b3DistanceSquared( closest.point, center ); + float speculativeDistance = B3_SPECULATIVE_DISTANCE; + float maxDistance = sphereA->radius + speculativeDistance; + if ( squaredDistance > maxDistance * maxDistance ) + { + return; + } + + float distance = sqrtf( squaredDistance ); + b3Vec3 normal; + if ( distance * distance > 1000.0f * FLT_MIN ) + { + normal = b3MulSV( 1.0f / distance, b3Sub( center, closest.point ) ); + } + else + { + normal = b3Normalize( b3Cross( b3Sub( v2, v1 ), b3Sub( v3, v1 ) ) ); + } + + // contact point mid-way + b3Vec3 contactPoint = b3MulSV( 0.5f, b3Add( b3Sub( center, b3MulSV( sphereA->radius, normal ) ), closest.point ) ); + + manifold->normal = normal; + manifold->pointCount = 1; + manifold->feature = closest.feature; + manifold->squaredDistance = squaredDistance; + + b3LocalManifoldPoint* mp = manifold->points + 0; + mp->point = contactPoint; + mp->separation = distance - sphereA->radius; + mp->pair = b3FeaturePair_single; +} + +static bool b3ClipSegmentToTriangleFace( b3ClipVertex segment[2], const b3Vec3* points, b3Plane plane ) +{ + b3Vec3 vertex1 = points[2]; + for ( int i = 0; i < 3; ++i ) + { + b3Vec3 vertex2 = points[i]; + b3Vec3 tangent = b3Normalize( b3Sub( vertex2, vertex1 ) ); + b3Vec3 binormal = b3Cross( tangent, plane.normal ); + + b3Plane clipPlane = b3MakePlaneFromNormalAndPoint( binormal, vertex1 ); + + int vertexCount = 0; + b3ClipVertex p1 = segment[0]; + b3ClipVertex p2 = segment[1]; + + float distance1 = b3PlaneSeparation( clipPlane, p1.position ); + float distance2 = b3PlaneSeparation( clipPlane, p2.position ); + + // If the points are behind the plane + if ( distance1 <= 0.0f ) + { + segment[vertexCount++] = p1; + } + if ( distance2 <= 0.0f ) + { + segment[vertexCount++] = p2; + } + + // If the points are on different sides of the plane + if ( distance1 * distance2 < 0.0f ) + { + // Find intersection point of edge and plane + float t = distance1 / ( distance1 - distance2 ); + segment[vertexCount].position = b3Lerp( p1.position, p2.position, t ); + segment[vertexCount].pair = distance1 > 0.0f ? p1.pair : p2.pair; + vertexCount++; + } + + if ( vertexCount != 2 ) + { + return false; + } + + vertex1 = vertex2; + } + + return true; +} + +static b3FaceQuery b3QueryTriangleFaceAndCapsule( b3Plane plane, const b3Capsule* capsule ) +{ + float separation1 = b3PlaneSeparation( plane, capsule->center1 ); + float separation2 = b3PlaneSeparation( plane, capsule->center2 ); + + if ( separation1 < separation2 ) + { + return (b3FaceQuery){ + .separation = separation1, + .faceIndex = 0, + .vertexIndex = 0, + }; + } + + return (b3FaceQuery){ + .separation = separation2, + .faceIndex = 0, + .vertexIndex = 1, + }; +} + +static b3EdgeQuery b3QueryTriangleAndCapsuleEdges( const b3Vec3* vertices, const b3Capsule* capsule ) +{ + // Work in the local space of the capsule + b3Vec3 p1 = capsule->center1; + b3Vec3 p2 = capsule->center2; + b3Vec3 capsuleEdge = b3Sub( p2, p1 ); + + b3Vec3 capsuleCenter = b3Lerp( p1, p2, 0.5f ); + + b3Vec3 triangleCenter = b3MulSV( 1.0f / 3.0f, b3Add( vertices[0], b3Add( vertices[1], vertices[2] ) ) ); + + // Find axis of minimum penetration + float maxSeparation = -FLT_MAX; + int maxIndex1 = UINT8_MAX; + int maxIndex2 = UINT8_MAX; + + int edgeIndex = 2; + b3Vec3 v1 = vertices[2]; + for ( int index = 0; index < 3; ++index ) + { + b3Vec3 v2 = vertices[index]; + + b3Vec3 triangleEdge = b3Sub( v2, v1 ); + + float separation = b3EdgeEdgeSeparation( p1, capsuleEdge, capsuleCenter, v1, triangleEdge, triangleCenter ); + if ( separation > maxSeparation ) + { + // Note: We don't exit early if we find a separating axis here since we want to + // find the best one for caching and account for the convex radius later. + maxSeparation = separation; + maxIndex1 = edgeIndex; + maxIndex2 = 0; + } + + v1 = v2; + edgeIndex = index; + } + + // Save result + return (b3EdgeQuery){ + .separation = maxSeparation, + .indexA = (uint8_t)maxIndex1, + .indexB = (uint8_t)maxIndex2, + }; +} + +static void b3BuildTriangleAndCapsuleFaceContact( b3LocalManifold* manifold, const b3Vec3* triangle, b3Plane plane, + const b3Capsule* capsule ) +{ + B3_ASSERT( manifold->pointCount == 0 ); + + b3ClipVertex segment[2]; + segment[0].position = capsule->center1; + segment[0].separation = 0.0f; + segment[0].pair = b3MakeFeaturePair( b3_featureShapeA, 0, b3_featureShapeA, 0 ); + segment[1].position = capsule->center2; + segment[1].separation = 0.0f; + segment[1].pair = b3MakeFeaturePair( b3_featureShapeA, 1, b3_featureShapeA, 1 ); + + bool havePoints = b3ClipSegmentToTriangleFace( segment, triangle, plane ); + if ( havePoints == false ) + { + return; + } + + float radius = capsule->radius; + float distance1 = b3PlaneSeparation( plane, segment[0].position ); + float distance2 = b3PlaneSeparation( plane, segment[1].position ); + + float speculativeDistance = B3_SPECULATIVE_DISTANCE; + if ( distance1 > speculativeDistance + radius && distance2 > speculativeDistance + radius ) + { + return; + } + + // Average points. Half-way between capsule bottom and triangle plane. + b3Vec3 point1 = b3MulSub( segment[0].position, 0.5f * ( distance1 + capsule->radius ), plane.normal ); + b3Vec3 point2 = b3MulSub( segment[1].position, 0.5f * ( distance2 + capsule->radius ), plane.normal ); + + manifold->normal = plane.normal; + manifold->feature = b3_featureTriangleFace; + manifold->pointCount = 2; + + b3LocalManifoldPoint* pt = manifold->points + 0; + pt->point = point1; + pt->separation = distance1 - capsule->radius; + pt->pair = segment[0].pair; + + pt = manifold->points + 1; + pt->point = point2; + pt->separation = distance2 - capsule->radius; + pt->pair = segment[1].pair; +} + +static void b3BuildTriangleAndCapsuleEdgeContact( b3LocalManifold* manifold, const b3Vec3* triangle, const b3Capsule* capsule, + b3EdgeQuery query ) +{ + B3_ASSERT( 0 <= query.indexA && query.indexA < 3 ); + + b3Vec3 p1 = capsule->center1; + b3Vec3 p2 = capsule->center2; + b3Vec3 capsuleEdge = b3Sub( p2, p1 ); + + const b3Vec3* vs = triangle; + + b3Vec3 triangleCenter = b3MulSV( 1.0f / 3.0f, b3Add( vs[0], b3Add( vs[1], vs[2] ) ) ); + b3Vec3 v1 = vs[query.indexA]; + b3Vec3 v2 = vs[( query.indexA + 1 ) % 3]; + b3Vec3 triangleEdge = b3Sub( v2, v1 ); + + b3Vec3 normal = b3Cross( capsuleEdge, triangleEdge ); + normal = b3Normalize( normal ); + + // Normal should point away from triangle center + if ( b3Dot( normal, b3Sub( v1, triangleCenter ) ) < 0.0f ) + { + normal = b3Neg( normal ); + } + + b3SegmentDistanceResult result = b3LineDistance( v1, triangleEdge, p1, capsuleEdge ); + + if ( result.fraction1 < 0.0f || 1.0f < result.fraction1 || result.fraction2 < 0.0f || 1.0f < result.fraction2 ) + { + // closest point beyond end points + return; + } + + b3Vec3 point = b3Lerp( b3MulSub( result.point1, capsule->radius, normal ), result.point2, 0.5f ); + + float separation = b3Dot( normal, b3Sub( result.point2, result.point1 ) ); + B3_VALIDATE( b3AbsFloat( separation - query.separation ) < B3_LINEAR_SLOP ); + + manifold->normal = normal; + manifold->pointCount = 1; + + b3TriangleFeature edgesFeatures[] = { b3_featureEdge1, b3_featureEdge2, b3_featureEdge3 }; + manifold->feature = edgesFeatures[query.indexA]; + + b3LocalManifoldPoint* pt = manifold->points + 0; + pt->point = point; + pt->separation = separation - capsule->radius; + pt->pair = b3MakeFeaturePair( b3_featureShapeA, query.indexA, b3_featureShapeB, query.indexB ); +} + +void b3CollideCapsuleAndTriangle( b3LocalManifold* manifold, int capacity, const b3Capsule* capsuleA, const b3Vec3* triangleB, + b3SimplexCache* cache ) +{ + manifold->pointCount = 0; + + if ( capacity < 2 ) + { + return; + } + + b3Vec3 v1 = triangleB[0], v2 = triangleB[1], v3 = triangleB[2]; + b3Plane plane = b3MakePlaneFromPoints( v1, v2, v3 ); + b3Vec3 capsuleCenter = b3Lerp( capsuleA->center1, capsuleA->center2, 0.5f ); + + float offset = b3PlaneSeparation( plane, capsuleCenter ); + if ( offset < 0.0f ) + { + // Cull back side collision + return; + } + + b3DistanceInput distanceInput; + distanceInput.proxyA = (b3ShapeProxy){ triangleB, 3, 0.0f }; + distanceInput.proxyB = (b3ShapeProxy){ &capsuleA->center1, 2, 0.0f }; + distanceInput.transform = b3Transform_identity; + distanceInput.useRadii = false; + + b3DistanceOutput distanceOutput = b3ShapeDistance( &distanceInput, cache, NULL, 0 ); + + float radius = capsuleA->radius; + if ( distanceOutput.distance > radius + B3_SPECULATIVE_DISTANCE ) + { + // Shapes are separated, persist the cache + return; + } + + if ( distanceOutput.distance > 100.0f * FLT_EPSILON ) + { + // Shallow penetration + b3Vec3 delta = b3Normalize( b3Sub( distanceOutput.pointB, distanceOutput.pointA ) ); + + // Try to create two contact points if closest points difference is nearly parallel to face normal + const float kTolerance = 0.2f; + float cosAngle = b3AbsFloat( b3Dot( plane.normal, delta ) ); + if ( cosAngle > kTolerance ) + { + // Clip capsule segment against side planes of reference face + b3ClipVertex segment[2]; + segment[0].position = capsuleA->center1; + segment[0].separation = 0.0f; + segment[0].pair = b3MakeFeaturePair( b3_featureShapeA, 0, b3_featureShapeA, 0 ); + segment[1].position = capsuleA->center2; + segment[1].separation = 0.0f; + segment[1].pair = b3MakeFeaturePair( b3_featureShapeA, 1, b3_featureShapeA, 1 ); + + bool havePoints = b3ClipSegmentToTriangleFace( segment, triangleB, plane ); + + if ( havePoints == true ) + { + float distance1 = b3PlaneSeparation( plane, segment[0].position ); + float distance2 = b3PlaneSeparation( plane, segment[1].position ); + + b3Vec3 normal = plane.normal; + b3Vec3 point1 = b3MulSub( segment[0].position, 0.5f * ( radius + distance1 ), normal ); + b3Vec3 point2 = b3MulSub( segment[1].position, 0.5f * ( radius + distance2 ), normal ); + + manifold->normal = normal; + manifold->feature = b3_featureTriangleFace; + manifold->pointCount = 2; + + b3LocalManifoldPoint* mp = manifold->points + 0; + mp->point = point1; + mp->separation = distance1 - radius; + mp->pair = segment[0].pair; + + mp = manifold->points + 1; + mp->point = point2; + mp->separation = distance2 - radius; + mp->pair = segment[1].pair; + + return; + } + } + + // Create contact from closest points + b3Vec3 point = b3MulSV( 0.5f, b3Add( b3Sub( distanceOutput.pointA, b3MulSV( radius, delta ) ), distanceOutput.pointB ) ); + + manifold->normal = delta; + manifold->pointCount = 1; + manifold->feature = b3GetTriangleFeature( cache ); + + b3LocalManifoldPoint* mp = manifold->points + 0; + mp->point = point; + mp->separation = distanceOutput.distance - radius; + mp->pair = b3FeaturePair_single; + + return; + } + + // Deep penetration + + b3FaceQuery faceQuery = b3QueryTriangleFaceAndCapsule( plane, capsuleA ); + if ( faceQuery.separation > radius ) + { + // Shapes are separated + return; + } + + b3EdgeQuery edgeQuery = b3QueryTriangleAndCapsuleEdges( triangleB, capsuleA ); + if ( edgeQuery.separation > radius ) + { + // Shapes are separated + return; + } + + // Create face contact + float faceSeparation = faceQuery.separation - radius; + b3BuildTriangleAndCapsuleFaceContact( manifold, triangleB, plane, capsuleA ); + if ( manifold->pointCount == 2 ) + { + faceSeparation = b3MinFloat( manifold->points[0].separation, manifold->points[1].separation ); + } + B3_VALIDATE( faceSeparation <= 0.0f ); + + // Face contact can be empty if it does not realize the axis of minimum penetration. + // Create edge contact if face contact fails or edge contact is significantly better! + const float kRelEdgeTolerance = 0.50f; + const float kAbsTolerance = 1.0f * B3_LINEAR_SLOP; + float edgeSeparation = edgeQuery.separation - radius; + if ( manifold->pointCount == 0 || edgeSeparation > kRelEdgeTolerance * faceSeparation + kAbsTolerance ) + { + // Edge contact + b3BuildTriangleAndCapsuleEdgeContact( manifold, triangleB, capsuleA, edgeQuery ); + } +} + +static inline int b3GetTriangleSupport( b3Vec3* points, b3Vec3 direction ) +{ + int index = 0; + float distance = b3Dot( points[0], direction ); + + float d = b3Dot( points[1], direction ); + if ( d > distance ) + { + distance = d; + index = 1; + } + + d = b3Dot( points[2], direction ); + if ( d > distance ) + { + return 2; + } + + return index; +} + +static b3FaceQuery b3QueryTriangleFace( const b3TriangleData* triangle, const b3HullData* hull ) +{ + const b3Vec3* hullPoints = b3GetHullPoints( hull ); + b3Plane plane = triangle->plane; + int vertexIndex = b3FindHullSupportVertex( hull, b3Neg( plane.normal ) ); + b3Vec3 support = hullPoints[vertexIndex]; + float separation = b3PlaneSeparation( plane, support ); + + return (b3FaceQuery){ + .separation = separation, + .faceIndex = 0, + .vertexIndex = (uint8_t)vertexIndex, + }; +} + +static b3FaceQuery b3QueryHullFace( const b3TriangleData* triangle, const b3HullData* hull ) +{ + const b3Plane* hullPlanes = b3GetHullPlanes( hull ); + int faceCount = hull->faceCount; + + b3Vec3 trianglePoints[] = { triangle->v1, triangle->v2, triangle->v3 }; + + int maxFaceIndex = -1; + int maxVertexIndex = -1; + float maxFaceSeparation = -FLT_MAX; + + for ( int faceIndex = 0; faceIndex < faceCount; ++faceIndex ) + { + b3Plane plane = hullPlanes[faceIndex]; + + int vertexIndex = b3GetTriangleSupport( trianglePoints, b3Neg( plane.normal ) ); + b3Vec3 support = trianglePoints[vertexIndex]; + float separation = b3PlaneSeparation( plane, support ); + if ( separation > maxFaceSeparation ) + { + maxFaceIndex = faceIndex; + maxVertexIndex = vertexIndex; + maxFaceSeparation = separation; + } + } + + return (b3FaceQuery){ + .separation = maxFaceSeparation, + .faceIndex = maxFaceIndex, + .vertexIndex = maxVertexIndex, + }; +} + +static b3EdgeQuery b3TestEdgePairs( const b3TriangleData* triangle, const b3HullData* hull ) +{ + b3EdgeQuery result = { + .separation = -FLT_MAX, + .indexA = B3_NULL_INDEX, + .indexB = B3_NULL_INDEX, + }; + + b3Vec3 trianglePoints[] = { triangle->v1, triangle->v2, triangle->v3 }; + b3Vec3 triangleEdges[] = { triangle->e1, triangle->e2, triangle->e3 }; + // int edgeFlags[] = { b3_concaveEdge1, b3_concaveEdge1, b3_concaveEdge3 }; + +#if B3_FORCE_GHOST_COLLISIONS + int triangleFlags = 0xFF; +#else + int triangleFlags = triangle->flags; +#endif + (void)triangleFlags; + + b3Vec3 triNormal = triangle->plane.normal; + + const b3HullHalfEdge* hullEdges = b3GetHullEdges( hull ); + const b3Vec3* hullPoints = b3GetHullPoints( hull ); + const b3Plane* hullPlanes = b3GetHullPlanes( hull ); + int edgeCount = hull->edgeCount; + + for ( int i = 0; i < edgeCount; i += 2 ) + { + const b3HullHalfEdge* edge = hullEdges + i; + const b3HullHalfEdge* twin = hullEdges + i + 1; + B3_ASSERT( edge->twin == i + 1 && twin->twin == i ); + + b3Vec3 hullPoint = hullPoints[edge->origin]; + b3Vec3 hullEdge = b3Sub( hullPoints[twin->origin], hullPoint ); + + b3Vec3 hullNormal1 = hullPlanes[edge->face].normal; + b3Vec3 hullNormal2 = hullPlanes[twin->face].normal; + + for ( int j = 0; j < 3; ++j ) + { + b3Vec3 triEdge = triangleEdges[j]; + + float cab = b3Dot( hullNormal1, triEdge ); + float dab = b3Dot( hullNormal2, triEdge ); + float bcd = b3Dot( triNormal, hullEdge ); + if ( cab * dab >= 0.0f || cab * bcd <= 0.0f ) + { + continue; + } + + b3Vec3 triPoint = trianglePoints[j]; + float separation = b3EdgeEdgeSeparation( triPoint, triEdge, triangle->center, hullPoint, hullEdge, hull->center ); + + // if ( separation > result.separation && ( edgeFlags[j] & triangleFlags ) == 0 ) + if ( separation > result.separation ) + { + // Note: We don't exit early if we find a separating axis here since we want to + // find the best one for caching. + result.separation = separation; + result.indexA = j; + result.indexB = i; + } + } + } + + return result; +} + +static float b3CollideHullFace( b3LocalManifold* manifold, int pointCapacity, const b3TriangleData* triangle, + const b3HullData* hull, b3FaceQuery query, b3SATCache* cache, bool enableSpeculative ) +{ + manifold->pointCount = 0; + + const b3HullFace* hullFaces = b3GetHullFaces( hull ); + const b3HullHalfEdge* hullEdges = b3GetHullEdges( hull ); + const b3Plane* hullPlanes = b3GetHullPlanes( hull ); + const b3Vec3* hullPoints = b3GetHullPoints( hull ); + + // Reference hull face + int refFace = query.faceIndex; + b3Plane refPlane = hullPlanes[refFace]; + + // Build clip polygon from triangle face (the incident face) + b3ClipVertex buffer1[B3_MAX_CLIP_POINTS], buffer2[B3_MAX_CLIP_POINTS]; + + b3Vec3 v1 = triangle->v1; + b3Vec3 v2 = triangle->v2; + b3Vec3 v3 = triangle->v3; + buffer1[0].position = v1; + buffer1[0].separation = b3PlaneSeparation( refPlane, v1 ); + buffer1[0].pair = b3MakeFeaturePair( b3_featureShapeB, 2, b3_featureShapeB, 0 ); + buffer1[1].position = v2; + buffer1[1].separation = b3PlaneSeparation( refPlane, v2 ); + buffer1[1].pair = b3MakeFeaturePair( b3_featureShapeB, 0, b3_featureShapeB, 1 ); + buffer1[2].position = v3; + buffer1[2].separation = b3PlaneSeparation( refPlane, v3 ); + buffer1[2].pair = b3MakeFeaturePair( b3_featureShapeB, 1, b3_featureShapeB, 2 ); + int pointCount = 3; + + // Clip triangle face against side planes of reference face + b3ClipVertex* input = buffer1; + b3ClipVertex* output = buffer2; + + const b3HullFace* face = hullFaces + refFace; + int edgeIndex = face->edge; + + do + { + const b3HullHalfEdge* edge = hullEdges + edgeIndex; + int nextEdgeIndex = edge->next; + const b3HullHalfEdge* next = hullEdges + nextEdgeIndex; + b3Vec3 vertex1 = hullPoints[edge->origin]; + b3Vec3 vertex2 = hullPoints[next->origin]; + b3Vec3 tangent = b3Normalize( b3Sub( vertex2, vertex1 ) ); + b3Vec3 binormal = b3Cross( tangent, refPlane.normal ); + + b3Plane clipPlane = b3MakePlaneFromNormalAndPoint( binormal, vertex1 ); + + pointCount = b3ClipPolygon( output, input, pointCount, clipPlane, edgeIndex, refPlane ); + B3_ASSERT( pointCount <= B3_MAX_CLIP_POINTS ); + + if ( pointCount < 3 ) + { + // Using a stale cache + *cache = (b3SATCache){ 0 }; + return query.separation; + } + + // Swap buffers, output becomes input for the next clipping plane + B3_SWAP( output, input ); + edgeIndex = nextEdgeIndex; + } + while ( edgeIndex != face->edge ); + + pointCount = b3MinInt( pointCount, pointCapacity ); + float minSeparation = FLT_MAX; + int finalPointCount = 0; + + for ( int i = 0; i < pointCount; ++i ) + { + b3ClipVertex* clipPoint = input + i; + minSeparation = b3MinFloat( minSeparation, clipPoint->separation ); + + if ( enableSpeculative == false && clipPoint->separation > 0.0f ) + { + continue; + } + + // Move point onto hull face improved culling + b3Vec3 point = b3MulSub( clipPoint->position, clipPoint->separation, refPlane.normal ); + + b3LocalManifoldPoint* pt = manifold->points + finalPointCount; + pt->point = point; + pt->separation = clipPoint->separation; + pt->pair = b3FlipPair( clipPoint->pair ); + + finalPointCount += 1; + } + + float speculativeDistance = enableSpeculative ? B3_SPECULATIVE_DISTANCE : 0.0f; + if ( minSeparation > speculativeDistance ) + { + // This can occur with a stale SAT cache + manifold->pointCount = 0; + *cache = (b3SATCache){ 0 }; + return minSeparation; + } + + manifold->pointCount = finalPointCount; + manifold->normal = b3Neg( refPlane.normal ); + manifold->feature = b3_featureHullFace; + + // Save cache + cache->separation = minSeparation; + cache->type = b3_faceAxisB; + cache->indexA = (uint8_t)query.vertexIndex; + cache->indexB = (uint8_t)query.faceIndex; + return minSeparation; +} + +static float b3CollideTriangleFace( b3LocalManifold* manifold, int pointCapacity, const b3TriangleData* triangle, + const b3HullData* hull, b3FaceQuery query, b3SATCache* cache, bool enableSpeculative ) +{ + B3_VALIDATE( manifold->pointCount == 0 ); + + const b3HullFace* hullFaces = b3GetHullFaces( hull ); + const b3HullHalfEdge* hullEdges = b3GetHullEdges( hull ); + const b3Vec3* hullPoints = b3GetHullPoints( hull ); + + // Find incident face + B3_ASSERT( query.faceIndex == 0 ); + b3Plane refPlane = triangle->plane; + + int incFace = b3FindIncidentFace( hull, refPlane.normal, query.vertexIndex ); + + // Build clip polygon from incident face + b3ClipVertex buffer1[2 * B3_MAX_CLIP_POINTS], buffer2[2 * B3_MAX_CLIP_POINTS]; + int pointCount = 0; + const b3HullFace* face = hullFaces + incFace; + int hullEdgeIndex = face->edge; + + do + { + const b3HullHalfEdge* edge = hullEdges + hullEdgeIndex; + + int nextEdgeIndex = edge->next; + const b3HullHalfEdge* next = hullEdges + nextEdgeIndex; + + b3Vec3 hullPoint = hullPoints[next->origin]; + buffer1[pointCount].position = hullPoint; + buffer1[pointCount].separation = b3PlaneSeparation( refPlane, hullPoint ); + buffer1[pointCount].pair = b3MakeFeaturePair( b3_featureShapeB, hullEdgeIndex, b3_featureShapeB, nextEdgeIndex ); + + pointCount += 1; + hullEdgeIndex = nextEdgeIndex; + } + while ( hullEdgeIndex != face->edge && pointCount < 2 * B3_MAX_CLIP_POINTS ); + + B3_ASSERT( pointCount >= 3 ); + + // Clip incident face against side planes of reference face (triangle) + b3ClipVertex* input = buffer1; + b3ClipVertex* output = buffer2; + + b3Vec3 trianglePoints[] = { triangle->v1, triangle->v2, triangle->v3 }; + b3Vec3 triangleEdges[] = { triangle->e1, triangle->e2, triangle->e3 }; + + for ( int i = 0; i < 3 && pointCount > 0; ++i ) + { + b3Vec3 sideNormal = b3Cross( triangleEdges[i], refPlane.normal ); + sideNormal = b3Normalize( sideNormal ); + + b3Plane clipPlane = b3MakePlaneFromNormalAndPoint( sideNormal, trianglePoints[i] ); + + pointCount = b3ClipPolygon( output, input, pointCount, clipPlane, i, refPlane ); + B3_ASSERT( pointCount <= 2 * B3_MAX_CLIP_POINTS ); + + B3_SWAP( output, input ); + } + + if ( pointCount == 0 ) + { + // Triangle face clipped away. Invalidate cache. + *cache = (b3SATCache){ 0 }; + return FLT_MAX; + } + + pointCount = b3MinInt( pointCount, pointCapacity ); + + float minSeparation = FLT_MAX; + + int finalPointCount = 0; + for ( int i = 0; i < pointCount; ++i ) + { + b3ClipVertex* clipPoint = input + i; + minSeparation = b3MinFloat( minSeparation, clipPoint->separation ); + + if ( enableSpeculative == false && clipPoint->separation > 0.0f ) + { + continue; + } + + // Move point onto triangle surface for improved culling + // b3Vec3 point = b3MulSub( clipPoint->position, clipPoint->separation, refPlane.normal ); + b3Vec3 point = clipPoint->position; + + b3LocalManifoldPoint* pt = manifold->points + finalPointCount; + pt->point = point; + pt->separation = clipPoint->separation; + pt->pair = clipPoint->pair; + + finalPointCount += 1; + } + + float speculativeDistance = enableSpeculative ? B3_SPECULATIVE_DISTANCE : 0.0f; + if ( minSeparation >= speculativeDistance ) + { + // This can happens if the objects move a part while re-using a cached axis + *cache = (b3SATCache){ 0 }; + return minSeparation; + } + + manifold->pointCount = finalPointCount; + manifold->normal = refPlane.normal; + manifold->feature = b3_featureTriangleFace; + + // Save cache + cache->separation = minSeparation; + cache->type = b3_faceAxisA; + cache->indexA = (uint8_t)query.faceIndex; + cache->indexB = (uint8_t)query.vertexIndex; + return minSeparation; +} + +static void b3CollideHullAndTriangleEdges( b3LocalManifold* manifold, int capacity, b3Vec3 trianglePoint, b3Vec3 triangleEdge, + b3Vec3 triangleCenter, const b3HullData* hull, b3EdgeQuery query, b3SATCache* cache ) +{ + B3_VALIDATE( query.separation <= 2.0f * B3_SPECULATIVE_DISTANCE ); + B3_ASSERT( query.indexA < 3 ); + + b3Vec3 cA = triangleCenter; + b3Vec3 pA = trianglePoint; + b3Vec3 eA = triangleEdge; + + const b3HullHalfEdge* edgesB = b3GetHullEdges( hull ); + const b3Vec3* pointsB = b3GetHullPoints( hull ); + const b3HullHalfEdge* edgeB = edgesB + query.indexB; + const b3HullHalfEdge* twinB = edgesB + edgeB->twin; + b3Vec3 pB = pointsB[edgeB->origin]; + b3Vec3 qB = pointsB[twinB->origin]; + b3Vec3 eB = b3Sub( qB, pB ); + + b3Vec3 normal = b3Cross( eA, eB ); + normal = b3Normalize( normal ); + + // Ensure normal points outward from triangle center + float outwardA = b3Dot( normal, b3Sub( pA, cA ) ); + + // Ensure normal points towards hull center + float outwardB = b3Dot( normal, b3Sub( hull->center, pB ) ); + + // Use the largest magnitude. The triangle outward value + // may be unreliable as some angles. + if ( b3AbsFloat( outwardA ) > b3AbsFloat( outwardB ) ) + { + if ( outwardA < 0.0f ) + { + normal = b3Neg( normal ); + } + } + else + { + if ( outwardB < 0.0f ) + { + normal = b3Neg( normal ); + } + } + + // Get the closest points between the infinite edge lines + b3SegmentDistanceResult result = b3LineDistance( pA, eA, pB, eB ); + + // Is one of the closest points outside of the associated edge segment? + if ( capacity == 0 || result.fraction1 < 0.0f || 1.0f < result.fraction1 || result.fraction2 < 0.0f || + 1.0f < result.fraction2 ) + { + // Invalid edge pair, no points generated + B3_ASSERT( manifold->pointCount == 0 ); + *cache = (b3SATCache){ 0 }; + return; + } + + // This can slide off the end from caching + float separation = b3Dot( normal, b3Sub( result.point2, result.point1 ) ); + B3_VALIDATE( b3AbsFloat( separation - query.separation ) < B3_LINEAR_SLOP ); + + b3Vec3 point = b3MulSV( 0.5f, b3Add( result.point1, result.point2 ) ); + + b3LocalManifoldPoint* pt = manifold->points + 0; + pt->point = point; + pt->separation = separation; + pt->pair = b3MakeFeaturePair( b3_featureShapeA, query.indexA, b3_featureShapeB, query.indexB ); + + // Save cache + cache->separation = separation; + cache->type = b3_edgePairAxis; + cache->indexA = (uint8_t)query.indexA; + cache->indexB = (uint8_t)query.indexB; + + manifold->normal = normal; + manifold->pointCount = 1; + + b3TriangleFeature edgesFeatures[] = { b3_featureEdge1, b3_featureEdge2, b3_featureEdge3 }; + manifold->feature = edgesFeatures[query.indexA]; +} + +// See "Collision Detection of Convex Polyhedra Based on Duality Transformation" +// Simplified for triangle versus hull +static inline bool b3IsTriangleMinkowskiFace( b3Vec3 triNormal, b3Vec3 triEdge, b3Vec3 hullNormal1, b3Vec3 hullNormal2, + b3Vec3 hullEdge ) +{ + float cab = b3Dot( hullNormal1, triEdge ); + float dab = b3Dot( hullNormal2, triEdge ); + float bcd = b3Dot( triNormal, hullEdge ); + return cab * dab < 0.0f && cab * bcd > 0.0f; +} + +b3AtomicInt b3_triangleConvexCalls; +b3AtomicInt b3_triangleCacheHits; + +// Computes the manifold in the local space of the hull +void b3CollideHullAndTriangle( b3LocalManifold* manifold, int capacity, const b3HullData* hullA, b3Vec3 v1, b3Vec3 v2, b3Vec3 v3, + int triangleFlags, b3SATCache* cache, bool enableSpeculative ) +{ + manifold->pointCount = 0; + manifold->feature = b3_featureNone; + + if ( capacity < 4 ) + { + return; + } + + b3Plane trianglePlane = b3MakePlaneFromPoints( v1, v2, v3 ); + float linearSlop = B3_LINEAR_SLOP; + + float offset = b3PlaneSeparation( trianglePlane, hullA->center ); + if ( cache->type == b3_backsideAxis ) + { + // Use hysteresis to avoid jitter on wavy meshes + if ( b3AbsFloat( cache->separation - offset ) < linearSlop ) + { + return; + } + + cache->type = b3_invalidAxis; + } + + if ( offset < -linearSlop ) + { + // Cull back side collision. Cache offset to add hysteresis. + cache->type = b3_backsideAxis; + cache->separation = offset; + return; + } + + b3Vec3 triangleCenter = b3MulSV( 1.0f / 3.0f, b3Add( v1, b3Add( v2, v3 ) ) ); + b3Vec3 trianglePoints[] = { v1, v2, v3 }; + b3Vec3 triangleEdges[] = { b3Sub( v2, v1 ), b3Sub( v3, v2 ), b3Sub( v1, v3 ) }; + + b3TriangleData triangle = { + .v1 = v1, + .v2 = v2, + .v3 = v3, + .e1 = triangleEdges[0], + .e2 = triangleEdges[1], + .e3 = triangleEdges[2], + .center = triangleCenter, + .plane = trianglePlane, + .flags = triangleFlags, + }; + + const b3HullHalfEdge* edges = b3GetHullEdges( hullA ); + const b3Plane* hullPlanes = b3GetHullPlanes( hullA ); + const b3Vec3* hullPoints = b3GetHullPoints( hullA ); + + float speculativeDistance = enableSpeculative ? B3_SPECULATIVE_DISTANCE : 0.0f; + cache->hit = 1; + + // Attempt to use the cache to speed up collision + switch ( cache->type ) + { + case b3_faceAxisA: + { + B3_ASSERT( cache->indexA == 0 ); + + int vertexIndex = b3FindHullSupportVertex( hullA, b3Neg( trianglePlane.normal ) ); + b3Vec3 support = hullPoints[vertexIndex]; + float separation = b3PlaneSeparation( trianglePlane, support ); + if ( separation > speculativeDistance ) + { + // Cache hit, shapes are separated + return; + } + + b3FaceQuery faceQuery; + faceQuery.separation = separation; + faceQuery.faceIndex = cache->indexA; + faceQuery.vertexIndex = vertexIndex; + + // Read cache but don't modify it + b3SATCache localCache = *cache; + float clippedSeparation = + b3CollideTriangleFace( manifold, capacity, &triangle, hullA, faceQuery, &localCache, enableSpeculative ); + + if ( manifold->pointCount > 0 && b3AbsFloat( cache->separation - clippedSeparation ) < linearSlop ) + { + // Cache hit, contact points generated + return; + } + + // Invalidate cache and fall through + manifold->pointCount = 0; + *cache = (b3SATCache){ 0 }; + } + break; + + case b3_faceAxisB: + { + B3_ASSERT( cache->indexB < hullA->faceCount ); + + b3Plane plane = hullPlanes[cache->indexB]; + + // Get triangle support point + int vertexIndex = 0; + float distance = -b3Dot( v1, plane.normal ); + for ( int i = 1; i < 3; ++i ) + { + float d = -b3Dot( trianglePoints[i], plane.normal ); + if ( d > distance ) + { + distance = d; + vertexIndex = i; + } + } + + b3Vec3 support = trianglePoints[vertexIndex]; + + // Separation of triangle support point with hull plane + float separation = b3PlaneSeparation( plane, support ); + if ( separation > speculativeDistance ) + { + // Cache hit, shapes are separated + return; + } + + // Deep overlap may lead to an invalid cache + // todo confirm + bool isDeep = separation < -2.0f * linearSlop; + + // Don't persist deep cache or allow separation to change too much + if ( isDeep == false ) + { + // Try to rebuild contact from last features + b3FaceQuery faceQuery; + faceQuery.separation = separation; + faceQuery.faceIndex = cache->indexB; + faceQuery.vertexIndex = vertexIndex; + + // Read cache but don't modify it + b3SATCache localCache = *cache; + float clippedSeparation = + b3CollideHullFace( manifold, capacity, &triangle, hullA, faceQuery, &localCache, enableSpeculative ); + + // Cache reuse is only successful if it creates contact points and the clipped separation didn't change much. + if ( manifold->pointCount > 0 && b3AbsFloat( cache->separation - clippedSeparation ) < linearSlop ) + { + // Cache hit, contact points generated + return; + } + } + + // Invalidate cache and fall through + manifold->pointCount = 0; + *cache = (b3SATCache){ 0 }; + } + break; + + case b3_edgePairAxis: + { + B3_ASSERT( cache->indexA < 3 ); + int indexA = cache->indexA; + + b3Vec3 triPoint = trianglePoints[indexA]; + b3Vec3 triEdge = triangleEdges[indexA]; + + B3_ASSERT( cache->indexB < hullA->edgeCount - 1 ); + int indexB = cache->indexB; + + const b3HullHalfEdge* edge2 = edges + indexB; + const b3HullHalfEdge* twin2 = edges + indexB + 1; + B3_ASSERT( edge2->twin == indexB + 1 && twin2->twin == indexB ); + + b3Vec3 hullPoint = hullPoints[edge2->origin]; + b3Vec3 hullEdge = b3Sub( hullPoints[twin2->origin], hullPoint ); + b3Vec3 hullNormal1 = hullPlanes[edge2->face].normal; + b3Vec3 hullNormal2 = hullPlanes[twin2->face].normal; + + // Confirm the edge pair is still a Minkowski face + bool isMinkowski = b3IsTriangleMinkowskiFace( trianglePlane.normal, triEdge, hullNormal1, hullNormal2, hullEdge ); + if ( isMinkowski ) + { + // Transform reference center of the first hull into local space of the second hull + float separation = b3EdgeEdgeSeparation( triPoint, triEdge, triangleCenter, hullPoint, hullEdge, hullA->center ); + if ( separation > speculativeDistance ) + { + // Cache hit, shapes are separated + return; + } + + if ( b3AbsFloat( cache->separation - separation ) < linearSlop ) + { + // Try to rebuild contact from last features + b3EdgeQuery edgeQuery; + edgeQuery.indexA = indexA; + edgeQuery.indexB = indexB; + edgeQuery.separation = separation; + + // Read cache but don't modify it + b3SATCache localCache = *cache; + b3CollideHullAndTriangleEdges( manifold, capacity, triPoint, triEdge, triangleCenter, hullA, edgeQuery, + &localCache ); + + if ( manifold->pointCount > 0 ) + { + // Cache hit, contact point generated + return; + } + } + } + + // Invalidate cache and fall through + *cache = (b3SATCache){ 0 }; + } + break; + + // This case is for testing + case b3_manualFaceAxisA: + { + b3FaceQuery faceQueryA = b3QueryTriangleFace( &triangle, hullA ); + b3CollideTriangleFace( manifold, capacity, &triangle, hullA, faceQueryA, cache, enableSpeculative ); + return; + } + + // This case is for testing + case b3_manualFaceAxisB: + { + b3FaceQuery faceQueryB = b3QueryHullFace( &triangle, hullA ); + b3CollideHullFace( manifold, capacity, &triangle, hullA, faceQueryB, cache, enableSpeculative ); + return; + } + + // This case is for testing + case b3_manualEdgePairAxis: + { + b3EdgeQuery edgeQuery = b3TestEdgePairs( &triangle, hullA ); + if ( edgeQuery.indexA != B3_NULL_INDEX ) + { + b3Vec3 trianglePoint = trianglePoints[edgeQuery.indexA]; + b3Vec3 triangleEdge = triangleEdges[edgeQuery.indexA]; + b3CollideHullAndTriangleEdges( manifold, capacity, trianglePoint, triangleEdge, triangleCenter, hullA, edgeQuery, + cache ); + } + return; + } + + default: + B3_ASSERT( cache->type == b3_invalidAxis ); + break; + } + + // Cache miss + cache->hit = 0; + + // Find axis of minimum penetration + b3FaceQuery faceQueryA = b3QueryTriangleFace( &triangle, hullA ); + if ( faceQueryA.separation > speculativeDistance ) + { + // Separating axis found + cache->separation = faceQueryA.separation; + cache->type = b3_faceAxisA; + cache->indexA = 0; + cache->indexB = UINT8_MAX; + return; + } + + b3FaceQuery faceQueryB = b3QueryHullFace( &triangle, hullA ); + if ( faceQueryB.separation > speculativeDistance ) + { + // Separating axis found + cache->separation = faceQueryB.separation; + cache->type = b3_faceAxisB; + cache->indexA = UINT8_MAX; + cache->indexB = (uint8_t)faceQueryB.faceIndex; + return; + } + + b3EdgeQuery edgeQuery = b3TestEdgePairs( &triangle, hullA ); + if ( edgeQuery.separation > speculativeDistance ) + { + // Separating axis found + cache->separation = edgeQuery.separation; + cache->type = b3_edgePairAxis; + cache->indexA = (uint8_t)edgeQuery.indexA; + cache->indexB = (uint8_t)edgeQuery.indexB; + return; + } + + float clippedFaceSeparation; + + // Don't admit a hull face significantly opposed to the triangle face. + // Need a tolerance to avoid ghost collisions. + // todo hull query skips faces that point along the triangle normal + b3Vec3 hullNormal = hullPlanes[faceQueryB.faceIndex].normal; + bool pushingDown = b3Dot( hullNormal, trianglePlane.normal ) > 0.25f; + if ( faceQueryB.separation > faceQueryA.separation + linearSlop && pushingDown == false ) + { + clippedFaceSeparation = b3CollideHullFace( manifold, capacity, &triangle, hullA, faceQueryB, cache, enableSpeculative ); + } + else + { + clippedFaceSeparation = + b3CollideTriangleFace( manifold, capacity, &triangle, hullA, faceQueryA, cache, enableSpeculative ); + } + + // Does an edge axis exist? + if ( edgeQuery.indexA != B3_NULL_INDEX ) + { + // When axes are aligned the edge separation can be garbage. + // If a face axis has positive separation there may be no points. + float maxFaceSeparation = b3MaxFloat( faceQueryA.separation, faceQueryB.separation ); + + if ( ( manifold->pointCount == 0 && edgeQuery.separation > maxFaceSeparation ) || + ( manifold->pointCount == 1 && edgeQuery.separation > clippedFaceSeparation + linearSlop ) ) + { + B3_ASSERT( 0 <= edgeQuery.indexA && edgeQuery.indexA < 3 ); + b3Vec3 trianglePoint = trianglePoints[edgeQuery.indexA]; + b3Vec3 triangleEdge = triangleEdges[edgeQuery.indexA]; + manifold->pointCount = 0; + b3CollideHullAndTriangleEdges( manifold, capacity, trianglePoint, triangleEdge, triangleCenter, hullA, edgeQuery, + cache ); + } + } + + // Using the speculative distance means that sometimes there are no valid contact points from SAT. + // In this fall back to GJK. This is important to prevent tunneling in rare cases. + if ( manifold->pointCount == 0 ) + { + b3Vec3 triangleB[] = { v1, v2, v3 }; + b3DistanceInput input = { 0 }; + input.proxyA = (b3ShapeProxy){ + .points = triangleB, + .count = 3, + .radius = 0.0f, + }; + input.proxyB = (b3ShapeProxy){ .points = hullPoints, .count = hullA->vertexCount, .radius = 0.0f }; + input.transform = b3Transform_identity; + input.useRadii = false; + + b3SimplexCache simplexCache = { 0 }; + b3DistanceOutput output = b3ShapeDistance( &input, &simplexCache, NULL, 0 ); + + if ( output.distance > 0.0f ) + { + B3_ASSERT( 0 < simplexCache.count && simplexCache.count <= 3 ); + + manifold->pointCount = 1; + manifold->feature = b3GetTriangleFeature( &simplexCache ); + manifold->normal = output.normal; + manifold->points[0].point = output.pointB; + manifold->points[0].separation = output.distance; + + // This feature pair not accurate but maybe it doesn't matter + manifold->points[0].pair = b3FeaturePair_single; + } + } +} diff --git a/vendor/box3d/src/src/types.c b/vendor/box3d/src/src/types.c new file mode 100644 index 000000000..b199cd15d --- /dev/null +++ b/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; +} diff --git a/vendor/box3d/src/src/verstable.h b/vendor/box3d/src/src/verstable.h new file mode 100644 index 000000000..c8601e6d4 --- /dev/null +++ b/vendor/box3d/src/src/verstable.h @@ -0,0 +1,2069 @@ +/*------------------------------------------------- VERSTABLE v2.2.1 --------------------------------------------------- + +Verstable is a C99-compatible, open-addressing hash table using quadratic probing and the following additions: + +* All keys that hash (i.e. "belong") to the same bucket (their "home bucket") are linked together by an 11-bit integer + specifying the quadratic displacement, relative to that bucket, of the next key in the chain. + +* If a chain of keys exists for a given bucket, then it always begins at that bucket. To maintain this policy, a 1-bit + flag is used to mark whether the key occupying a bucket belongs there. When inserting a new key, if the bucket it + belongs to is occupied by a key that does not belong there, then the occupying key is evicted and the new key takes + the bucket. + +* A 4-bit fragment of each key's hash code is also stored. + +* The aforementioned metadata associated with each bucket (the 4-bit hash fragment, the 1-bit flag, and the 11-bit link + to the next key in the chain) are stored together in a uint16_t array rather than in the bucket alongside the key and + (optionally) the value. + +One way to conceptualize this scheme is as a chained hash table in which overflowing keys are stored not in separate +memory allocations but in otherwise unused buckets. In this regard, it shares similarities with Malte Skarupke's Bytell +hash table (https://www.youtube.com/watch?v=M2fKMP47slQ) and traditional "coalesced hashing". + +Advantages of this scheme include: + +* Fast lookups impervious to load factor: If the table contains any key belonging to the lookup key's home bucket, then + that bucket contains the first in a traversable chain of all keys belonging to it. Hence, only the home bucket and + other buckets containing keys belonging to it are ever probed. Moreover, the stored hash fragments allow skipping most + non-matching keys in the chain without accessing the actual buckets array or calling the (potentially expensive) key + comparison function. + +* Fast insertions: Insertions are faster than they are in other schemes that move keys around (e.g. Robin Hood) because + they only move, at most, one existing key. + +* Fast, tombstone-free deletions: Deletions, which usually require tombstones in quadratic-probing hash tables, are + tombstone-free and only move, at most, one existing key. + +* Fast iteration: The separate metadata array allows keys in sparsely populated tables to be found without incurring the + frequent cache misses that would result from traversing the buckets array. + +Usage example: + + +---------------------------------------------------------+----------------------------------------------------------+ + | Using the generic macro API (C11 and later): | Using the prefixed functions API (C99 and later): | + +---------------------------------------------------------+----------------------------------------------------------+ + | #include | #include | + | | | + | // Instantiating a set template. | // Instantiating a set template. | + | #define NAME int_set | #define NAME int_set | + | #define KEY_TY int | #define KEY_TY int | + | #include "verstable.h" | #define HASH_FN vt_hash_integer | + | | #define CMPR_FN vt_cmpr_integer | + | // Instantiating a map template. | #include "verstable.h" | + | #define NAME int_int_map | | + | #define KEY_TY int | // Instantiating a map template. | + | #define VAL_TY int | #define NAME int_int_map | + | #include "verstable.h" | #define KEY_TY int | + | | #define VAL_TY int | + | int main( void ) | #define HASH_FN vt_hash_integer | + | { | #define CMPR_FN vt_cmpr_integer | + | // Set. | #include "verstable.h" | + | | | + | int_set our_set; | int main( void ) | + | vt_init( &our_set ); | { | + | | // Set. | + | // Inserting keys. | | + | for( int i = 0; i < 10; ++i ) | int_set our_set; | + | { | int_set_init( &our_set ); | + | int_set_itr itr = vt_insert( &our_set, i ); | | + | if( vt_is_end( itr ) ) | // Inserting keys. | + | { | for( int i = 0; i < 10; ++i ) | + | // Out of memory, so abort. | { | + | vt_cleanup( &our_set ); | int_set_itr itr = | + | return 1; | int_set_insert( &our_set, i ); | + | } | if( int_set_is_end( itr ) ) | + | } | { | + | | // Out of memory, so abort. | + | // Erasing keys. | int_set_cleanup( &our_set ); | + | for( int i = 0; i < 10; i += 3 ) | return 1; | + | vt_erase( &our_set, i ); | } | + | | } | + | // Retrieving keys. | | + | for( int i = 0; i < 10; ++i ) | // Erasing keys. | + | { | for( int i = 0; i < 10; i += 3 ) | + | int_set_itr itr = vt_get( &our_set, i ); | int_set_erase( &our_set, i ); | + | if( !vt_is_end( itr ) ) | | + | printf( "%d ", itr.data->key ); | // Retrieving keys. | + | } | for( int i = 0; i < 10; ++i ) | + | // Printed: 1 2 4 5 7 8 | { | + | | int_set_itr itr = int_set_get( &our_set, i ); | + | // Iteration. | if( !int_set_is_end( itr ) ) | + | for( | printf( "%d ", itr.data->key ); | + | int_set_itr itr = vt_first( &our_set ); | } | + | !vt_is_end( itr ); | // Printed: 1 2 4 5 7 8 | + | itr = vt_next( itr ) | | + | ) | // Iteration. | + | printf( "%d ", itr.data->key ); | for( | + | // Printed: 2 4 7 1 5 8 | int_set_itr itr = | + | | int_set_first( &our_set ); | + | vt_cleanup( &our_set ); | !int_set_is_end( itr ); | + | | itr = int_set_next( itr ) | + | // Map. | ) | + | | printf( "%d ", itr.data->key ); | + | int_int_map our_map; | // Printed: 2 4 7 1 5 8 | + | vt_init( &our_map ); | | + | | int_set_cleanup( &our_set ); | + | // Inserting keys and values. | | + | for( int i = 0; i < 10; ++i ) | // Map. | + | { | | + | int_int_map_itr itr = | int_int_map our_map; | + | vt_insert( &our_map, i, i + 1 ); | int_int_map_init( &our_map ); | + | if( vt_is_end( itr ) ) | | + | { | // Inserting keys and values. | + | // Out of memory, so abort. | for( int i = 0; i < 10; ++i ) | + | vt_cleanup( &our_map ); | { | + | return 1; | int_int_map_itr itr = | + | } | int_int_map_insert( &our_map, i, i + 1 ); | + | } | if( int_int_map_is_end( itr ) ) | + | | { | + | // Erasing keys and values. | // Out of memory, so abort. | + | for( int i = 0; i < 10; i += 3 ) | int_int_map_cleanup( &our_map ); | + | vt_erase( &our_map, i ); | return 1; | + | | } | + | // Retrieving keys and values. | } | + | for( int i = 0; i < 10; ++i ) | | + | { | // Erasing keys and values. | + | int_int_map_itr itr = vt_get( &our_map, i ); | for( int i = 0; i < 10; i += 3 ) | + | if( !vt_is_end( itr ) ) | int_int_map_erase( &our_map, i ); | + | printf( | | + | "%d:%d ", | // Retrieving keys and values. | + | itr.data->key, | for( int i = 0; i < 10; ++i ) | + | itr.data->val | { | + | ); | int_int_map_itr itr = | + | } | int_int_map_get( &our_map, i ); | + | // Printed: 1:2 2:3 4:5 5:6 7:8 8:9 | if( !int_int_map_is_end( itr ) ) | + | | printf( | + | // Iteration. | "%d:%d ", | + | for( | itr.data->key, | + | int_int_map_itr itr = vt_first( &our_map ); | itr.data->val | + | !vt_is_end( itr ); | ); | + | itr = vt_next( itr ) | } | + | ) | // Printed: 1:2 2:3 4:5 5:6 7:8 8:9 | + | printf( | | + | "%d:%d ", | // Iteration. | + | itr.data->key, | for( | + | itr.data->val | int_int_map_itr itr = | + | ); | int_int_map_first( &our_map ); | + | // Printed: 2:3 4:5 7:8 1:2 5:6 8:9 | !int_int_map_is_end( itr ); | + | | itr = int_int_map_next( itr ) | + | vt_cleanup( &our_map ); | ) | + | } | printf( | + | | "%d:%d ", | + | | itr.data->key, | + | | itr.data->val | + | | ); | + | | // Printed: 2:3 4:5 7:8 1:2 5:6 8:9 | + | | | + | | int_int_map_cleanup( &our_map ); | + | | } | + | | | + +---------------------------------------------------------+----------------------------------------------------------+ + +API: + + Instantiating a hash table template: + + Create a new hash table type in the following manner: + + #define NAME + #define KEY_TY + #include "verstable.h" + + The NAME macro specifies the name of hash table type that the library will declare, the prefix for the functions + associated with it, and the prefix for the associated iterator type. + + The KEY_TY macro specifies the key type. + + In C99, it is also always necessary to define HASH_FN and CMPR_FN (see below) before including the header. + + The following macros may also be defined before including the header: + + #define VAL_TY + + The type of the value associated with each key. + If this macro is defined, the hash table acts as a map associating keys with values. + Otherwise, it acts as a set containing only keys. + + #define HASH_FN + + The name of the existing function used to hash each key. + The function should have the signature uint64_t ( KEY_TY key ) and return a 64-bit hash code. + For best performance, the hash function should provide a high level of entropy across all bits. + There are two default hash functions: vt_hash_integer for all integer types up to 64 bits in size, and + vt_hash_string for NULL-terminated strings (i.e. char *). + When KEY_TY is one of such types and the compiler is in C11 mode or later, HASH_FN may be left undefined, in + which case the appropriate default function is inferred from KEY_TY. + Otherwise, HASH_FN must be defined. + + #define CMPR_FN + + The name of the existing function used to compare two keys. + The function should have the signature bool ( KEY_TY key_1, KEY_TY key_2 ) and return true if the two keys are + equal. + There are two default comparison functions: vt_cmpr_integer for all integer types up to 64 bits in size, and + vt_cmpr_string for NULL-terminated strings (i.e. char *). + As with the default hash functions, in C11 or later the appropriate default comparison function is inferred if + KEY_TY is one of such types and CMPR_FN is left undefined. + Otherwise, CMPR_FN must be defined. + + #define MAX_LOAD + + The floating-point load factor at which the hash table automatically doubles the size of its internal buckets + array. + The default is 0.9, i.e. 90%. + + #define KEY_DTOR_FN + + The name of the existing destructor function, with the signature void ( KEY_TY key ), called on a key when it is + erased from the table or replaced by a newly inserted key. + The API functions that may call the key destructor are NAME_insert, NAME_erase, NAME_erase_itr, NAME_clear, + and NAME_cleanup. + + #define VAL_DTOR_FN + + The name of the existing destructor function, with the signature void ( VAL_TY val ), called on a value when it + is erased from the table or replaced by a newly inserted value. + The API functions that may call the value destructor are NAME_insert, NAME_erase, NAME_erase_itr, NAME_clear, + and NAME_cleanup. + + #define CTX_TY + + The type of the hash table type's ctx (context) member. + This member only exists if CTX_TY was defined. + It is intended to be used in conjunction with MALLOC_FN and FREE_FN (see below). + + #define MALLOC_FN + + The name of the existing function used to allocate memory. + If CTX_TY was defined, the signature should be void *( size_t size, CTX_TY *ctx ), where size is the number of + bytes to allocate and ctx points to the table's ctx member. + Otherwise, the signature should be void *( size_t size ). + The default wraps stdlib.h's malloc. + + #define FREE_FN + + The name of the existing function used to free memory. + If CTX_TY was defined, the signature should be void ( void *ptr, size_t size, CTX_TY *ctx ), where ptr points to + the memory to free, size is the number of bytes that were allocated, and ctx points to the table's ctx member. + Otherwise, the signature should be void ( void *ptr, size_t size ). + The default wraps stdlib.h's free. + + #define HEADER_MODE + #define IMPLEMENTATION_MODE + + By default, all hash table functions are defined as static inline functions, the intent being that a given hash + table template should be instantiated once per translation unit; for best performance, this is the recommended + way to use the library. + However, it is also possible to separate the struct definitions and function declarations from the function + definitions such that one implementation can be shared across all translation units (as in a traditional header + and source file pair). + In that case, instantiate a template wherever it is needed by defining HEADER_MODE, along with only NAME, + KEY_TY, and (optionally) VAL_TY, CTX_TY, and header guards, and including the library, e.g.: + + #ifndef INT_INT_MAP_H + #define INT_INT_MAP_H + #define NAME int_int_map + #define KEY_TY int + #define VAL_TY int + #define HEADER_MODE + #include "verstable.h" + #endif + + In one source file, define IMPLEMENTATION_MODE, along with NAME, KEY_TY, and any of the aforementioned optional + macros, and include the library, e.g.: + + #define NAME int_int_map + #define KEY_TY int + #define VAL_TY int + #define HASH_FN vt_hash_integer // C99. + #define CMPR_FN vt_cmpr_integer // C99. + #define MAX_LOAD 0.8 + #define IMPLEMENTATION_MODE + #include "verstable.h" + + Including the library automatically undefines all the aforementioned macros after they have been used to instantiate + the template. + + Functions: + + The functions associated with a hash table type are all prefixed with the name the user supplied via the NAME macro. + In C11 and later, the generic "vt_"-prefixed macros may be used to automatically select the correct version of the + specified function based on the arguments. + + void NAME_init( NAME *table ) + void NAME_init( NAME *table, CTX_TY ctx ) + // C11 generic macro: vt_init. + + Initializes the table for use. + If CTX_TY was defined, ctx sets the table's ctx member. + + bool NAME_init_clone( NAME *table, NAME *source ) + bool NAME_init_clone( NAME *table, NAME *source, CTX_TY ctx ) + // C11 generic macro: vt_init_clone. + + Initializes the table as a shallow copy of the specified source table. + If CTX_TY was defined, ctx sets the table's ctx member. + Returns false in the case of memory allocation failure. + + size_t NAME_size( NAME *table ) // C11 generic macro: vt_size. + + Returns the number of keys currently in the table. + + size_t NAME_bucket_count( NAME *table ) // C11 generic macro: vt_bucket_count. + + Returns the table's current bucket count. + + NAME_itr NAME_insert( NAME *table, KEY_TY key ) + NAME_itr NAME_insert( NAME *table, KEY_TY key, VAL_TY val ) + // C11 generic macro: vt_insert. + + Inserts the specified key (and value, if VAL_TY was defined) into the hash table. + If the same key already exists, then the new key (and value) replaces the existing key (and value). + Returns an iterator to the new key, or an end iterator in the case of memory allocation failure. + + NAME_itr NAME_get_or_insert( NAME *table, KEY_TY key ) + NAME_itr NAME_get_or_insert( NAME *table, KEY_TY key, VAL_TY val ) + // C11 generic macro: vt_get_or_insert. + + Inserts the specified key (and value, if VAL_TY was defined) if it does not already exist in the table. + Returns an iterator to the new key if it was inserted, or an iterator to the existing key, or an end iterator if + the key did not exist but the new key could not be inserted because of memory allocation failure. + Determine whether the key was inserted by comparing the table's size before and after the call. + + NAME_itr NAME_get( NAME *table, KEY_TY key ) // C11 generic macro: vt_get. + + Returns a iterator to the specified key, or an end iterator if no such key exists. + + bool NAME_erase( NAME *table, KEY_TY key ) // C11 generic macro: vt_erase. + + Erases the specified key (and associated value, if VAL_TY was defined), if it exists. + Returns true if a key was erased. + + NAME_itr NAME_erase_itr( NAME *table, NAME_itr itr ) // C11 generic macro: vt_erase_itr. + + Erases the key (and associated value, if VAL_TY was defined) pointed to by the specified iterator. + Returns an iterator to the next key in the table, or an end iterator if the erased key was the last one. + + bool NAME_reserve( NAME *table, size_t size ) // C11 generic macro: vt_reserve. + + Ensures that the bucket count is large enough to support the specified key count (i.e. size) without rehashing. + Returns false if unsuccessful due to memory allocation failure. + + bool NAME_shrink( NAME *table ) // C11 generic macro: vt_shrink. + + Shrinks the bucket count to best accommodate the current size. + Returns false if unsuccessful due to memory allocation failure. + + NAME_itr NAME_first( NAME *table ) // C11 generic macro: vt_first. + + Returns an iterator to the first key in the table, or an end iterator if the table is empty. + + bool NAME_is_end( NAME_itr itr ) // C11 generic macro: vt_is_end. + + Returns true if the iterator is an end iterator. + + NAME_itr NAME_next( NAME_itr itr ) // C11 generic macro: vt_next. + + Returns an iterator to the key after the one pointed to by the specified iterator, or an end iterator if the + specified iterator points to the last key in the table. + + void NAME_clear( NAME *table ) // C11 generic macro: vt_clear. + + Erases all keys (and values, if VAL_TY was defined) in the table. + + void NAME_cleanup( NAME *table ) // C11 generic macro: vt_cleanup. + + Erases all keys (and values, if VAL_TY was defined) in the table, frees all memory associated with it, and + initializes it for reuse. + + Iterators: + + Access the key (and value, if VAL_TY was defined) that an iterator points to using the NAME_itr struct's data + member: + + itr.data->key + itr.data->val + + Functions that may insert new keys (NAME_insert and NAME_get_or_insert), erase keys (NAME_erase and NAME_erase_itr), + or reallocate the internal bucket array (NAME_reserve and NAME_shrink) invalidate all existing iterators. + To delete keys during iteration and resume iterating, use the return value of NAME_erase_itr. + +Version history: + + 06/05/2025 2.2.1: Fixed incorrect signature of NAME_is_end in documentation. + 18/04/2025 2.2.0: Added const qualifier to the table parameter of NAME_size, NAME_bucket_count, NAME_get, and + NAME_first and to the source parameter of NAME_init_clone. + Added default support for const char * strings. + Added support for -Wextra. + Replaced FNV-1a with Wyhash as the default string hash function. + 18/06/2024 2.1.1: Fixed a bug affecting iteration on big-endian platforms under MSVC. + 27/05/2024 2.1.0: Replaced the Murmur3 mixer with the fast-hash mixer as the default integer hash function. + Fixed a bug that could theoretically cause a crash on rehash (triggerable in testing using + NAME_shrink with a maximum load factor significantly higher than 1.0). + 06/02/2024 2.0.0: Improved custom allocator support by introducing the CTX_TY option and allowing user-supplied free + functions to receive the allocation size. + Improved documentation. + Introduced various optimizations, including storing the buckets-array size mask instead of the + bucket count, eliminating empty-table checks, combining the buckets memory and metadata memory into + one allocation, and adding branch prediction macros. + Fixed a bug that caused a key to be used after destruction during erasure. + 12/12/2023 1.0.0: Initial release. + +License (MIT): + + Copyright (c) 2023-2025 Jackson L. Allan + + 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. + +*/ + +/*--------------------------------------------------------------------------------------------------------------------*/ +/* Common header section */ +/*--------------------------------------------------------------------------------------------------------------------*/ + +#ifndef VERSTABLE_H +#define VERSTABLE_H + +#include +#include +#include +#include +#include +#include + +// Two-way concatenation macro. +#define VT_CAT_( a, b ) a##b +#define VT_CAT( a, b ) VT_CAT_( a, b ) + +// Branch optimization macros. +#ifdef __GNUC__ +#define VT_LIKELY( expression ) __builtin_expect( (bool)( expression ), true ) +#define VT_UNLIKELY( expression ) __builtin_expect( (bool)( expression ), false ) +#else +#define VT_LIKELY( expression ) ( expression ) +#define VT_UNLIKELY( expression ) ( expression ) +#endif + +// Masks for manipulating and extracting data from a bucket's uint16_t metadatum. +#define VT_EMPTY 0x0000 +#define VT_HASH_FRAG_MASK 0xF000 // 0b1111000000000000. +#define VT_IN_HOME_BUCKET_MASK 0x0800 // 0b0000100000000000. +#define VT_DISPLACEMENT_MASK 0x07FF // 0b0000011111111111, also denotes the displacement limit. Set to VT_LOAD to 1.0 + // to test proper handling of encroachment on the displacement limit during + // inserts. + +// Extracts a hash fragment from a uint64_t hash code. +// We take the highest four bits so that keys that map (via modulo) to the same bucket have distinct hash fragments. +static inline uint16_t vt_hashfrag( uint64_t hash ) +{ + return ( hash >> 48 ) & VT_HASH_FRAG_MASK; +} + +// Standard quadratic probing formula that guarantees that all buckets are visited when the bucket count is a power of +// two (at least in theory, because the displacement limit could terminate the search early when the bucket count is +// high). +static inline size_t vt_quadratic( uint16_t displacement ) +{ + return ( (size_t)displacement * displacement + displacement ) / 2; +} + +#define VT_MIN_NONZERO_BUCKET_COUNT 8 // Must be a power of two. + +// Function to find the left-most non-zero uint16_t in a uint64_t. +// This function is used when we scan four buckets at a time while iterating and relies on compiler intrinsics wherever +// possible. + +#if defined( __GNUC__ ) && ULLONG_MAX == 0xFFFFFFFFFFFFFFFF + +static inline int vt_first_nonzero_uint16( uint64_t val ) +{ + const uint16_t endian_checker = 0x0001; + if( *(const char *)&endian_checker ) // Little-endian (the compiler will optimize away the check at -O1 and above). + return __builtin_ctzll( val ) / 16; + + return __builtin_clzll( val ) / 16; +} + +#elif defined( _MSC_VER ) && ( defined( _M_X64 ) || defined( _M_ARM64 ) ) + +#include +#pragma intrinsic(_BitScanForward64) +#pragma intrinsic(_BitScanReverse64) + +static inline int vt_first_nonzero_uint16( uint64_t val ) +{ + unsigned long result; + + const uint16_t endian_checker = 0x0001; + if( *(const char *)&endian_checker ) + _BitScanForward64( &result, val ); + else + { + _BitScanReverse64( &result, val ); + result = 63 - result; + } + + return result / 16; +} + +#else + +static inline int vt_first_nonzero_uint16( uint64_t val ) +{ + int result = 0; + + uint32_t half; + memcpy( &half, &val, sizeof( uint32_t ) ); + if( !half ) + result += 2; + + uint16_t quarter; + memcpy( &quarter, (char *)&val + result * sizeof( uint16_t ), sizeof( uint16_t ) ); + if( !quarter ) + result += 1; + + return result; +} + +#endif + +// When the bucket count is zero, setting the metadata pointer to point to a VT_EMPTY placeholder, rather than NULL, +// allows us to avoid checking for a zero bucket count during insertion and lookup. +static const uint16_t vt_empty_placeholder_metadatum = VT_EMPTY; + +// Default hash and comparison functions. + +// Fast-hash, as described by https://jonkagstrom.com/bit-mixer-construction and +// https://code.google.com/archive/p/fast-hash. +// In testing, this hash function provided slightly better performance than the Murmur3 mixer. +static inline uint64_t vt_hash_integer( uint64_t key ) +{ + key ^= key >> 23; + key *= 0x2127599bf4325c37ull; + key ^= key >> 47; + return key; +} + +// For hashing strings, we use the public-domain Wyhash +// (https://github.com/wangyi-fudan/wyhash) with the following modifications: +// * We use a fixed seed and secret (the defaults suggested in the Wyhash repository). +// * We do not handle endianness, so the result will differ depending on the platform. +// * We omit the code optimized for 32-bit platforms. + +static inline void vt_wymum( uint64_t *a, uint64_t *b ) +{ +#if defined( __SIZEOF_INT128__ ) + __uint128_t r = *a; + r *= *b; + *a = (uint64_t)r; + *b = (uint64_t)( r >> 64 ); +#elif defined( _MSC_VER ) && defined( _M_X64 ) + *a = _umul128( *a, *b, b ); +#else + uint64_t ha = *a >> 32; + uint64_t hb = *b >> 32; + uint64_t la = (uint32_t)*a; + uint64_t lb = (uint32_t)*b; + uint64_t rh = ha * hb; + uint64_t rm0 = ha * lb; + uint64_t rm1 = hb * la; + uint64_t rl = la * lb; + uint64_t t = rl + ( rm0 << 32 ); + uint64_t c = t < rl; + uint64_t lo = t + ( rm1 << 32 ); + c += lo < t; + uint64_t hi = rh + ( rm0 >> 32 ) + ( rm1 >> 32 ) + c; + *a = lo; + *b = hi; +#endif +} + +static inline uint64_t vt_wymix( uint64_t a, uint64_t b ) +{ + vt_wymum( &a, &b ); + return a ^ b; +} + +static inline uint64_t vt_wyr8( const unsigned char *p ) +{ + uint64_t v; + memcpy( &v, p, 8 ); + return v; +} + +static inline uint64_t vt_wyr4( const unsigned char *p ) +{ + uint32_t v; + memcpy( &v, p, 4 ); + return v; +} + +static inline uint64_t vt_wyr3( const unsigned char *p, size_t k ) +{ + return ( ( (uint64_t)p[ 0 ] ) << 16 ) | ( ( (uint64_t)p[ k >> 1 ] ) << 8 ) | p[ k - 1 ]; +} + +static inline size_t vt_wyhash( const void *key, size_t len ) +{ + const unsigned char *p = (const unsigned char *)key; + uint64_t seed = 0xca813bf4c7abf0a9ull; + uint64_t a; + uint64_t b; + if( VT_LIKELY( len <= 16 ) ) + { + if( VT_LIKELY( len >= 4 ) ) + { + a = ( vt_wyr4( p ) << 32 ) | vt_wyr4( p + ( ( len >> 3 ) << 2 ) ); + b = ( vt_wyr4( p + len - 4 ) << 32 ) | vt_wyr4( p + len - 4 - ( ( len >> 3 ) << 2 ) ); + } + else if( VT_LIKELY( len > 0 ) ) + { + a = vt_wyr3( p, len ); + b = 0; + } + else + { + a = 0; + b = 0; + } + } + else + { + size_t i = len; + if( VT_UNLIKELY( i >= 48 ) ) + { + uint64_t see1 = seed; + uint64_t see2 = seed; + do{ + seed = vt_wymix( vt_wyr8( p ) ^ 0x8bb84b93962eacc9ull, vt_wyr8( p + 8 ) ^ seed ); + see1 = vt_wymix( vt_wyr8( p + 16 ) ^ 0x4b33a62ed433d4a3ull, vt_wyr8( p + 24 ) ^ see1 ); + see2 = vt_wymix( vt_wyr8( p + 32 ) ^ 0x4d5a2da51de1aa47ull, vt_wyr8( p + 40 ) ^ see2 ); + p += 48; + i -= 48; + } + while( VT_LIKELY( i >= 48 ) ); + seed ^= see1 ^ see2; + } + + while( VT_UNLIKELY( i > 16 ) ) + { + seed = vt_wymix( vt_wyr8( p ) ^ 0x8bb84b93962eacc9ull, vt_wyr8( p + 8 ) ^ seed ); + i -= 16; + p += 16; + } + + a = vt_wyr8( p + i - 16 ); + b = vt_wyr8( p + i - 8 ); + } + + a ^= 0x8bb84b93962eacc9ull; + b ^= seed; + vt_wymum( &a, &b ); + return (size_t)vt_wymix( a ^ 0x2d358dccaa6c78a5ull ^ len, b ^ 0x8bb84b93962eacc9ull ); +} + +static inline uint64_t vt_hash_string( const char *key ) +{ + return vt_wyhash( key, strlen( key ) ); +} + +static inline bool vt_cmpr_integer( uint64_t key_1, uint64_t key_2 ) +{ + return key_1 == key_2; +} + +static inline bool vt_cmpr_string( const char *key_1, const char *key_2 ) +{ + return strcmp( key_1, key_2 ) == 0; +} + +// Default allocation and free functions. + +static inline void *vt_malloc( size_t size ) +{ + return malloc( size ); +} + +static inline void vt_free( void *ptr, size_t size ) +{ + (void)size; + free( ptr ); +} + +static inline void *vt_malloc_with_ctx( size_t size, void *ctx ) +{ + (void)ctx; + return malloc( size ); +} + +static inline void vt_free_with_ctx( void *ptr, size_t size, void *ctx ) +{ + (void)size; + (void)ctx; + free( ptr ); +} + +// The rest of the common header section pertains to the C11 generic macro API. +// This interface is based on the extendible-_Generic mechanism documented in detail at +// https://github.com/JacksonAllan/CC/blob/main/articles/Better_C_Generics_Part_1_The_Extendible_Generic.md. +// In summary, instantiating a template also defines wrappers for the template's types and functions with names in the +// pattern of vt_table_NNNN and vt_init_NNNN, where NNNN is an automatically generated integer unique to the template +// instance in the current translation unit. +// These wrappers plug into _Generic-based API macros, which use preprocessor magic to automatically generate _Generic +// slots for every existing template instance. +#if defined(__STDC_VERSION__) && __STDC_VERSION__ >= 201112L && !defined( VT_NO_C11_GENERIC_API ) + +// Octal counter that supports up to 511 hash table templates. +#define VT_TEMPLATE_COUNT_D1 0 // Digit 1, i.e. least significant digit. +#define VT_TEMPLATE_COUNT_D2 0 +#define VT_TEMPLATE_COUNT_D3 0 + +// Four-way concatenation macro. +#define VT_CAT_4_( a, b, c, d ) a##b##c##d +#define VT_CAT_4( a, b, c, d ) VT_CAT_4_( a, b, c, d ) + +// Provides the current value of the counter as a three-digit octal number preceded by 0. +#define VT_TEMPLATE_COUNT VT_CAT_4( 0, VT_TEMPLATE_COUNT_D3, VT_TEMPLATE_COUNT_D2, VT_TEMPLATE_COUNT_D1 ) + +// _Generic-slot generation macros. + +#define VT_GENERIC_SLOT( ty, fn, n ) , VT_CAT( ty, n ): VT_CAT( fn, n ) +#define VT_R1_0( ty, fn, d3, d2 ) +#define VT_R1_1( ty, fn, d3, d2 ) VT_GENERIC_SLOT( ty, fn, VT_CAT_4( 0, d3, d2, 0 ) ) +#define VT_R1_2( ty, fn, d3, d2 ) VT_GENERIC_SLOT( ty, fn, VT_CAT_4( 0, d3, d2, 1 ) ) VT_R1_1( ty, fn, d3, d2 ) +#define VT_R1_3( ty, fn, d3, d2 ) VT_GENERIC_SLOT( ty, fn, VT_CAT_4( 0, d3, d2, 2 ) ) VT_R1_2( ty, fn, d3, d2 ) +#define VT_R1_4( ty, fn, d3, d2 ) VT_GENERIC_SLOT( ty, fn, VT_CAT_4( 0, d3, d2, 3 ) ) VT_R1_3( ty, fn, d3, d2 ) +#define VT_R1_5( ty, fn, d3, d2 ) VT_GENERIC_SLOT( ty, fn, VT_CAT_4( 0, d3, d2, 4 ) ) VT_R1_4( ty, fn, d3, d2 ) +#define VT_R1_6( ty, fn, d3, d2 ) VT_GENERIC_SLOT( ty, fn, VT_CAT_4( 0, d3, d2, 5 ) ) VT_R1_5( ty, fn, d3, d2 ) +#define VT_R1_7( ty, fn, d3, d2 ) VT_GENERIC_SLOT( ty, fn, VT_CAT_4( 0, d3, d2, 6 ) ) VT_R1_6( ty, fn, d3, d2 ) +#define VT_R1_8( ty, fn, d3, d2 ) VT_GENERIC_SLOT( ty, fn, VT_CAT_4( 0, d3, d2, 7 ) ) VT_R1_7( ty, fn, d3, d2 ) +#define VT_R2_0( ty, fn, d3 ) +#define VT_R2_1( ty, fn, d3 ) VT_R1_8( ty, fn, d3, 0 ) +#define VT_R2_2( ty, fn, d3 ) VT_R1_8( ty, fn, d3, 1 ) VT_R2_1( ty, fn, d3 ) +#define VT_R2_3( ty, fn, d3 ) VT_R1_8( ty, fn, d3, 2 ) VT_R2_2( ty, fn, d3 ) +#define VT_R2_4( ty, fn, d3 ) VT_R1_8( ty, fn, d3, 3 ) VT_R2_3( ty, fn, d3 ) +#define VT_R2_5( ty, fn, d3 ) VT_R1_8( ty, fn, d3, 4 ) VT_R2_4( ty, fn, d3 ) +#define VT_R2_6( ty, fn, d3 ) VT_R1_8( ty, fn, d3, 5 ) VT_R2_5( ty, fn, d3 ) +#define VT_R2_7( ty, fn, d3 ) VT_R1_8( ty, fn, d3, 6 ) VT_R2_6( ty, fn, d3 ) +#define VT_R2_8( ty, fn, d3 ) VT_R1_8( ty, fn, d3, 7 ) VT_R2_7( ty, fn, d3 ) +#define VT_R3_0( ty, fn ) +#define VT_R3_1( ty, fn ) VT_R2_8( ty, fn, 0 ) +#define VT_R3_2( ty, fn ) VT_R2_8( ty, fn, 1 ) VT_R3_1( ty, fn ) +#define VT_R3_3( ty, fn ) VT_R2_8( ty, fn, 2 ) VT_R3_2( ty, fn ) +#define VT_R3_4( ty, fn ) VT_R2_8( ty, fn, 3 ) VT_R3_3( ty, fn ) +#define VT_R3_5( ty, fn ) VT_R2_8( ty, fn, 4 ) VT_R3_4( ty, fn ) +#define VT_R3_6( ty, fn ) VT_R2_8( ty, fn, 5 ) VT_R3_5( ty, fn ) +#define VT_R3_7( ty, fn ) VT_R2_8( ty, fn, 6 ) VT_R3_6( ty, fn ) + +#define VT_GENERIC_SLOTS( ty, fn ) \ +VT_CAT( VT_R1_, VT_TEMPLATE_COUNT_D1 )( ty, fn, VT_TEMPLATE_COUNT_D3, VT_TEMPLATE_COUNT_D2 ) \ +VT_CAT( VT_R2_, VT_TEMPLATE_COUNT_D2 )( ty, fn, VT_TEMPLATE_COUNT_D3 ) \ +VT_CAT( VT_R3_, VT_TEMPLATE_COUNT_D3 )( ty, fn ) \ + +// Actual generic API macros. + +// vt_init must be handled as a special case because it could take one or two arguments, depending on whether CTX_TY +// was defined. +#define VT_ARG_3( _1, _2, _3, ... ) _3 +#define vt_init( ... ) VT_ARG_3( __VA_ARGS__, vt_init_with_ctx, vt_init_without_ctx, )( __VA_ARGS__ ) +#define vt_init_without_ctx( table ) _Generic( *( table ) VT_GENERIC_SLOTS( vt_table_, vt_init_ ) )( table ) +#define vt_init_with_ctx( table, ... ) _Generic( *( table ) \ + VT_GENERIC_SLOTS( vt_table_, vt_init_ ) \ +)( table, __VA_ARGS__ ) \ + +#define vt_init_clone( table, ... ) _Generic( *( table ) \ + VT_GENERIC_SLOTS( vt_table_, vt_init_clone_ ) \ +)( table, __VA_ARGS__ ) \ + +#define vt_size( table )_Generic( *( table ) VT_GENERIC_SLOTS( vt_table_, vt_size_ ) )( table ) + +#define vt_bucket_count( table ) _Generic( *( table ) VT_GENERIC_SLOTS( vt_table_, vt_bucket_count_ ) )( table ) + +#define vt_is_end( itr ) _Generic( itr VT_GENERIC_SLOTS( vt_table_itr_, vt_is_end_ ) )( itr ) + +#define vt_insert( table, ... ) _Generic( *( table ) VT_GENERIC_SLOTS( vt_table_, vt_insert_ ) )( table, __VA_ARGS__ ) + +#define vt_get_or_insert( table, ... ) _Generic( *( table ) \ + VT_GENERIC_SLOTS( vt_table_, vt_get_or_insert_ ) \ +)( table, __VA_ARGS__ ) \ + +#define vt_get( table, ... ) _Generic( *( table ) VT_GENERIC_SLOTS( vt_table_, vt_get_ ) )( table, __VA_ARGS__ ) + +#define vt_erase( table, ... ) _Generic( *( table ) VT_GENERIC_SLOTS( vt_table_, vt_erase_ ) )( table, __VA_ARGS__ ) + +#define vt_next( itr ) _Generic( itr VT_GENERIC_SLOTS( vt_table_itr_, vt_next_ ) )( itr ) + +#define vt_erase_itr( table, ... ) _Generic( *( table ) \ + VT_GENERIC_SLOTS( vt_table_, vt_erase_itr_ ) \ +)( table, __VA_ARGS__ ) \ + +#define vt_reserve( table, ... ) _Generic( *( table ) VT_GENERIC_SLOTS( vt_table_, vt_reserve_ ) )( table, __VA_ARGS__ ) + +#define vt_shrink( table ) _Generic( *( table ) VT_GENERIC_SLOTS( vt_table_, vt_shrink_ ) )( table ) + +#define vt_first( table ) _Generic( *( table ) VT_GENERIC_SLOTS( vt_table_, vt_first_ ) )( table ) + +#define vt_clear( table ) _Generic( *( table ) VT_GENERIC_SLOTS( vt_table_, vt_clear_ ) )( table ) + +#define vt_cleanup( table ) _Generic( *( table ) VT_GENERIC_SLOTS( vt_table_, vt_cleanup_ ) )( table ) + +#endif + +#endif + +/*--------------------------------------------------------------------------------------------------------------------*/ +/* Prefixed structs */ +/*--------------------------------------------------------------------------------------------------------------------*/ + +#ifndef IMPLEMENTATION_MODE + +typedef struct +{ + KEY_TY key; + #ifdef VAL_TY + VAL_TY val; + #endif +} VT_CAT( NAME, _bucket ); + +typedef struct +{ + VT_CAT( NAME, _bucket ) *data; + uint16_t *metadatum; + uint16_t *metadata_end; // Iterators carry an internal end pointer so that NAME_is_end does not need the table to be + // passed in as an argument. + // This also allows for the zero-bucket-count check to occur once in NAME_first, rather than + // repeatedly in NAME_is_end. + size_t home_bucket; // SIZE_MAX if home bucket is unknown. +} VT_CAT( NAME, _itr ); + +typedef struct +{ + size_t key_count; + size_t buckets_mask; // Rather than storing the bucket count directly, we store the bit mask used to reduce a hash + // code or displacement-derived bucket index to the buckets array, i.e. the bucket count minus + // one. + // Consequently, a zero bucket count (i.e. when .metadata points to the placeholder) constitutes + // a special case, represented by all bits unset (i.e. zero). + VT_CAT( NAME, _bucket ) *buckets; + uint16_t *metadata; // As described above, each metadatum consists of a 4-bit hash-code fragment (X), a 1-bit flag + // indicating whether the key in this bucket begins a chain associated with the bucket (Y), and + // an 11-bit value indicating the quadratic displacement of the next key in the chain (Z): + // XXXXYZZZZZZZZZZZ. + #ifdef CTX_TY + CTX_TY ctx; + #endif +} NAME; + +#endif + +/*--------------------------------------------------------------------------------------------------------------------*/ +/* Function prototypes */ +/*--------------------------------------------------------------------------------------------------------------------*/ + +#if defined( HEADER_MODE ) || defined( IMPLEMENTATION_MODE ) +#define VT_API_FN_QUALIFIERS +#else +#define VT_API_FN_QUALIFIERS static inline +#endif + +#ifndef IMPLEMENTATION_MODE + +VT_API_FN_QUALIFIERS void VT_CAT( NAME, _init )( + NAME * + #ifdef CTX_TY + , CTX_TY + #endif +); + +VT_API_FN_QUALIFIERS bool VT_CAT( NAME, _init_clone )( + NAME *, + const NAME * + #ifdef CTX_TY + , CTX_TY + #endif +); + +VT_API_FN_QUALIFIERS size_t VT_CAT( NAME, _size )( const NAME * ); + +VT_API_FN_QUALIFIERS size_t VT_CAT( NAME, _bucket_count )( const NAME * ); + +VT_API_FN_QUALIFIERS bool VT_CAT( NAME, _is_end )( VT_CAT( NAME, _itr ) ); + +VT_API_FN_QUALIFIERS VT_CAT( NAME, _itr ) VT_CAT( NAME, _insert )( + NAME *, + KEY_TY + #ifdef VAL_TY + , VAL_TY + #endif +); + +VT_API_FN_QUALIFIERS VT_CAT( NAME, _itr ) VT_CAT( NAME, _get_or_insert )( + NAME *, + KEY_TY + #ifdef VAL_TY + , VAL_TY + #endif +); + +VT_API_FN_QUALIFIERS VT_CAT( NAME, _itr ) VT_CAT( NAME, _get )( + const NAME *table, + KEY_TY key +); + +VT_API_FN_QUALIFIERS bool VT_CAT( NAME, _erase )( NAME *, KEY_TY ); + +VT_API_FN_QUALIFIERS VT_CAT( NAME, _itr ) VT_CAT( NAME, _next )( VT_CAT( NAME, _itr ) ); + +VT_API_FN_QUALIFIERS bool VT_CAT( NAME, _reserve )( NAME *, size_t ); + +VT_API_FN_QUALIFIERS bool VT_CAT( NAME, _shrink )( NAME * ); + +VT_API_FN_QUALIFIERS VT_CAT( NAME, _itr ) VT_CAT( NAME, _first )( const NAME * ); + +VT_API_FN_QUALIFIERS void VT_CAT( NAME, _clear )( NAME * ); + +VT_API_FN_QUALIFIERS void VT_CAT( NAME, _cleanup )( NAME * ); + +// Not an API function, but must be prototyped anyway because it is called by the inline NAME_erase_itr below. +VT_API_FN_QUALIFIERS bool VT_CAT( NAME, _erase_itr_raw ) ( NAME *, VT_CAT( NAME, _itr ) ); + +// Erases the key pointed to by itr and returns an iterator to the next key in the table. +// This function must be inlined to ensure that the compiler optimizes away the NAME_fast_forward call if the returned +// iterator is discarded. +#ifdef __GNUC__ +static inline __attribute__((always_inline)) +#elif defined( _MSC_VER ) +static __forceinline +#else +static inline +#endif +VT_CAT( NAME, _itr ) VT_CAT( NAME, _erase_itr )( NAME *table, VT_CAT( NAME, _itr ) itr ) +{ + if( VT_CAT( NAME, _erase_itr_raw )( table, itr ) ) + return VT_CAT( NAME, _next )( itr ); + + return itr; +} + +#endif + +/*--------------------------------------------------------------------------------------------------------------------*/ +/* Function implementations */ +/*--------------------------------------------------------------------------------------------------------------------*/ + +#ifndef HEADER_MODE + +// Default settings. + +#ifndef MAX_LOAD +#define MAX_LOAD 0.9 +#endif + +#ifndef MALLOC_FN +#ifdef CTX_TY +#define MALLOC_FN vt_malloc_with_ctx +#else +#define MALLOC_FN vt_malloc +#endif +#endif + +#ifndef FREE_FN +#ifdef CTX_TY +#define FREE_FN vt_free_with_ctx +#else +#define FREE_FN vt_free +#endif +#endif + +#ifndef HASH_FN +#if defined(__STDC_VERSION__) && __STDC_VERSION__ >= 201112L +#ifdef _MSC_VER // In MSVC, the compound literal in the _Generic triggers a warning about unused local variables at /W4. +#define HASH_FN \ +_Pragma( "warning( push )" ) \ +_Pragma( "warning( disable: 4189 )" ) \ +_Generic( ( KEY_TY ){ 0 }, char *: vt_hash_string, const char*: vt_hash_string, default: vt_hash_integer ) \ +_Pragma( "warning( pop )" ) +#else +#define HASH_FN _Generic( ( KEY_TY ){ 0 }, \ + char *: vt_hash_string, \ + const char*: vt_hash_string, \ + default: vt_hash_integer \ +) +#endif +#else +#error Hash function inference is only available in C11 and later. In C99, you need to define HASH_FN manually to \ +vt_hash_integer, vt_hash_string, or your own custom function with the signature uint64_t ( KEY_TY ). +#endif +#endif + +#ifndef CMPR_FN +#if defined(__STDC_VERSION__) && __STDC_VERSION__ >= 201112L +#ifdef _MSC_VER +#define CMPR_FN \ +_Pragma( "warning( push )" ) \ +_Pragma( "warning( disable: 4189 )" ) \ +_Generic( ( KEY_TY ){ 0 }, char *: vt_cmpr_string, const char*: vt_cmpr_string, default: vt_cmpr_integer ) \ +_Pragma( "warning( pop )" ) +#else +#define CMPR_FN _Generic( ( KEY_TY ){ 0 }, \ + char *: vt_cmpr_string, \ + const char*: vt_cmpr_string, \ + default: vt_cmpr_integer \ +) +#endif +#else +#error Comparison function inference is only available in C11 and later. In C99, you need to define CMPR_FN manually \ +to vt_cmpr_integer, vt_cmpr_string, or your own custom function with the signature bool ( KEY_TY, KEY_TY ). +#endif +#endif + +VT_API_FN_QUALIFIERS void VT_CAT( NAME, _init )( + NAME *table + #ifdef CTX_TY + , CTX_TY ctx + #endif +) +{ + table->key_count = 0; + table->buckets_mask = 0x0000000000000000ull; + table->buckets = NULL; + table->metadata = (uint16_t *)&vt_empty_placeholder_metadatum; + #ifdef CTX_TY + table->ctx = ctx; + #endif +} + +// For efficiency, especially in the case of a small table, the buckets array and metadata share the same dynamic memory +// allocation: +// +-----------------------------+-----+----------------+--------+ +// | Buckets | Pad | Metadata | Excess | +// +-----------------------------+-----+----------------+--------+ +// Any allocated metadata array requires four excess elements to ensure that iteration functions, which read four +// metadata at a time, never read beyond the end of it. +// This function returns the offset of the beginning of the metadata, i.e. the size of the buckets array plus the +// (usually zero) padding. +// It assumes that the bucket count is not zero. +static inline size_t VT_CAT( NAME, _metadata_offset )( NAME *table ) +{ + // Use sizeof, rather than alignof, for C99 compatibility. + return ( ( ( table->buckets_mask + 1 ) * sizeof( VT_CAT( NAME, _bucket ) ) + sizeof( uint16_t ) - 1 ) / + sizeof( uint16_t ) ) * sizeof( uint16_t ); +} + +// Returns the total allocation size, including the buckets array, padding, metadata, and excess metadata. +// As above, this function assumes that the bucket count is not zero. +static inline size_t VT_CAT( NAME, _total_alloc_size )( NAME *table ) +{ + return VT_CAT( NAME, _metadata_offset )( table ) + ( table->buckets_mask + 1 + 4 ) * sizeof( uint16_t ); +} + +VT_API_FN_QUALIFIERS bool VT_CAT( NAME, _init_clone )( + NAME *table, + const NAME *source + #ifdef CTX_TY + , CTX_TY ctx + #endif +) +{ + table->key_count = source->key_count; + table->buckets_mask = source->buckets_mask; + #ifdef CTX_TY + table->ctx = ctx; + #endif + + if( !source->buckets_mask ) + { + table->metadata = (uint16_t *)&vt_empty_placeholder_metadatum; + table->buckets = NULL; + return true; + } + + void *allocation = MALLOC_FN( + VT_CAT( NAME, _total_alloc_size )( table ) + #ifdef CTX_TY + , &table->ctx + #endif + ); + + if( VT_UNLIKELY( !allocation ) ) + return false; + + table->buckets = (VT_CAT( NAME, _bucket ) *)allocation; + table->metadata = (uint16_t *)( (unsigned char *)allocation + VT_CAT( NAME, _metadata_offset )( table ) ); + memcpy( allocation, source->buckets, VT_CAT( NAME, _total_alloc_size )( table ) ); + + return true; +} + +VT_API_FN_QUALIFIERS size_t VT_CAT( NAME, _size )( const NAME *table ) +{ + return table->key_count; +} + +VT_API_FN_QUALIFIERS size_t VT_CAT( NAME, _bucket_count )( const NAME *table ) +{ + // If the bucket count is zero, buckets_mask will be zero, not the bucket count minus one. + // We account for this special case by adding (bool)buckets_mask rather than one. + return table->buckets_mask + (bool)table->buckets_mask; +} + +VT_API_FN_QUALIFIERS bool VT_CAT( NAME, _is_end )( VT_CAT( NAME, _itr ) itr ) +{ + return itr.metadatum == itr.metadata_end; +} + +// Finds the earliest empty bucket in which a key belonging to home_bucket can be placed, assuming that home_bucket +// is already occupied. +// The reason to begin the search at home_bucket, rather than the end of the existing chain, is that keys deleted from +// other chains might have freed up buckets that could fall in this chain before the final key. +// Returns true if an empty bucket within the range of the displacement limit was found, in which case the final two +// pointer arguments contain the index of the empty bucket and its quadratic displacement from home_bucket. +static inline bool VT_CAT( NAME, _find_first_empty )( + NAME *table, + size_t home_bucket, + size_t *empty, + uint16_t *displacement +) +{ + *displacement = 1; + size_t linear_dispacement = 1; + + while( true ) + { + *empty = ( home_bucket + linear_dispacement ) & table->buckets_mask; + if( table->metadata[ *empty ] == VT_EMPTY ) + return true; + + if( VT_UNLIKELY( ++*displacement == VT_DISPLACEMENT_MASK ) ) + return false; + + linear_dispacement += *displacement; + } +} + +// Finds the key in the chain beginning in home_bucket after which to link a new key with displacement_to_empty +// quadratic displacement and returns the index of the bucket containing that key. +// Although the new key could simply be linked to the end of the chain, keeping the chain ordered by displacement +// theoretically improves cache locality during lookups. +static inline size_t VT_CAT( NAME, _find_insert_location_in_chain )( + NAME *table, + size_t home_bucket, + uint16_t displacement_to_empty +) +{ + size_t candidate = home_bucket; + while( true ) + { + uint16_t displacement = table->metadata[ candidate ] & VT_DISPLACEMENT_MASK; + + if( displacement > displacement_to_empty ) + return candidate; + + candidate = ( home_bucket + vt_quadratic( displacement ) ) & table->buckets_mask; + } +} + +// Frees up a bucket occupied by a key not belonging there so that a new key belonging there can be placed there as the +// beginning of a new chain. +// This requires: +// * Finding the previous key in the chain to which the occupying key belongs by rehashing it and then traversing the +// chain. +// * Disconnecting the key from the chain. +// * Finding the appropriate empty bucket to which to move the key. +// * Moving the key (and value) data to the empty bucket. +// * Re-linking the key to the chain. +// Returns true if the eviction succeeded, or false if no empty bucket to which to evict the occupying key could be +// found within the displacement limit. +static inline bool VT_CAT( NAME, _evict )( NAME *table, size_t bucket ) +{ + // Find the previous key in chain. + size_t home_bucket = HASH_FN( table->buckets[ bucket ].key ) & table->buckets_mask; + size_t prev = home_bucket; + while( true ) + { + size_t next = ( home_bucket + vt_quadratic( table->metadata[ prev ] & VT_DISPLACEMENT_MASK ) ) & + table->buckets_mask; + + if( next == bucket ) + break; + + prev = next; + } + + // Disconnect the key from chain. + table->metadata[ prev ] = ( table->metadata[ prev ] & ~VT_DISPLACEMENT_MASK ) | ( table->metadata[ bucket ] & + VT_DISPLACEMENT_MASK ); + + // Find the empty bucket to which to move the key. + size_t empty; + uint16_t displacement; + if( VT_UNLIKELY( !VT_CAT( NAME, _find_first_empty )( table, home_bucket, &empty, &displacement ) ) ) + return false; + + // Find the key in the chain after which to link the moved key. + prev = VT_CAT( NAME, _find_insert_location_in_chain )( table, home_bucket, displacement ); + + // Move the key (and value) data. + table->buckets[ empty ] = table->buckets[ bucket ]; + + // Re-link the key to the chain from its new bucket. + table->metadata[ empty ] = ( table->metadata[ bucket ] & VT_HASH_FRAG_MASK ) | ( table->metadata[ prev ] & + VT_DISPLACEMENT_MASK ); + table->metadata[ prev ] = ( table->metadata[ prev ] & ~VT_DISPLACEMENT_MASK ) | displacement; + + return true; +} + +// Returns an end iterator, i.e. any iterator for which .metadatum == .metadata_end. +// This function just cleans up the library code in functions that return an end iterator as a failure indicator. +static inline VT_CAT( NAME, _itr ) VT_CAT( NAME, _end_itr )( void ) +{ + VT_CAT( NAME, _itr ) itr = { NULL, NULL, NULL, 0 }; + return itr; +} + +// Inserts a key, optionally replacing the existing key if it already exists. +// There are two main cases that must be handled: +// * If the key's home bucket is empty or occupied by a key that does not belong there, then the key is inserted there, +// evicting the occupying key if there is one. +// * Otherwise, the chain of keys beginning at the home bucket is (if unique is false) traversed in search of a matching +// key. +// If none is found, then the new key is inserted at the earliest available bucket, per quadratic probing from the +// home bucket, and then linked to the chain in a manner that maintains its quadratic order. +// The unique argument tells the function whether to skip searching for the key before inserting it (on rehashing, this +// step is unnecessary). +// The replace argument tells the function whether to replace an existing key. +// If replace is true, the function returns an iterator to the inserted key, or an end iterator if the key was not +// inserted because of the maximum load factor or displacement limit constraints. +// If replace is false, then the return value is as described above, except that if the key already exists, the function +// returns an iterator to the existing key. +static inline VT_CAT( NAME, _itr ) VT_CAT( NAME, _insert_raw )( + NAME *table, + KEY_TY key, + #ifdef VAL_TY + VAL_TY *val, + #endif + bool unique, + bool replace +) +{ + uint64_t hash = HASH_FN( key ); + uint16_t hashfrag = vt_hashfrag( hash ); + size_t home_bucket = hash & table->buckets_mask; + + // Case 1: The home bucket is empty or contains a key that doesn't belong there. + // This case also implicitly handles the case of a zero bucket count, since home_bucket will be zero and metadata[ 0 ] + // will be the empty placeholder. + // In that scenario, the zero buckets_mask triggers the below load-factor check. + if( !( table->metadata[ home_bucket ] & VT_IN_HOME_BUCKET_MASK ) ) + { + if( + // Load-factor check. + VT_UNLIKELY( table->key_count + 1 > VT_CAT( NAME, _bucket_count )( table ) * MAX_LOAD ) || + // Vacate the home bucket if it contains a key. + ( table->metadata[ home_bucket ] != VT_EMPTY && VT_UNLIKELY( !VT_CAT( NAME, _evict )( table, home_bucket ) ) ) + ) + return VT_CAT( NAME, _end_itr )(); + + table->buckets[ home_bucket ].key = key; + #ifdef VAL_TY + table->buckets[ home_bucket ].val = *val; + #endif + table->metadata[ home_bucket ] = hashfrag | VT_IN_HOME_BUCKET_MASK | VT_DISPLACEMENT_MASK; + + ++table->key_count; + + VT_CAT( NAME, _itr ) itr = { + table->buckets + home_bucket, + table->metadata + home_bucket, + table->metadata + table->buckets_mask + 1, // Iteration stopper (i.e. the first of the four excess metadata). + home_bucket + }; + return itr; + } + + // Case 2: The home bucket contains the beginning of a chain. + + // Optionally, check the existing chain. + if( !unique ) + { + size_t bucket = home_bucket; + while( true ) + { + if( + ( table->metadata[ bucket ] & VT_HASH_FRAG_MASK ) == hashfrag && + VT_LIKELY( CMPR_FN( table->buckets[ bucket ].key, key ) ) + ) + { + if( replace ) + { + #ifdef KEY_DTOR_FN + KEY_DTOR_FN( table->buckets[ bucket ].key ); + #endif + table->buckets[ bucket ].key = key; + + #ifdef VAL_TY + #ifdef VAL_DTOR_FN + VAL_DTOR_FN( table->buckets[ bucket ].val ); + #endif + table->buckets[ bucket ].val = *val; + #endif + } + + VT_CAT( NAME, _itr ) itr = { + table->buckets + bucket, + table->metadata + bucket, + table->metadata + table->buckets_mask + 1, + home_bucket + }; + return itr; + } + + uint16_t displacement = table->metadata[ bucket ] & VT_DISPLACEMENT_MASK; + if( displacement == VT_DISPLACEMENT_MASK ) + break; + + bucket = ( home_bucket + vt_quadratic( displacement ) ) & table->buckets_mask; + } + } + + size_t empty; + uint16_t displacement; + if( + VT_UNLIKELY( + // Load-factor check. + table->key_count + 1 > VT_CAT( NAME, _bucket_count )( table ) * MAX_LOAD || + // Find the earliest empty bucket, per quadratic probing. + !VT_CAT( NAME, _find_first_empty )( table, home_bucket, &empty, &displacement ) + ) + ) + return VT_CAT( NAME, _end_itr )(); + + // Insert the new key (and value) in the empty bucket and link it to the chain. + + size_t prev = VT_CAT( NAME, _find_insert_location_in_chain )( table, home_bucket, displacement ); + + table->buckets[ empty ].key = key; + #ifdef VAL_TY + table->buckets[ empty ].val = *val; + #endif + table->metadata[ empty ] = hashfrag | ( table->metadata[ prev ] & VT_DISPLACEMENT_MASK ); + table->metadata[ prev ] = ( table->metadata[ prev ] & ~VT_DISPLACEMENT_MASK ) | displacement; + + ++table->key_count; + + VT_CAT( NAME, _itr ) itr = { + table->buckets + empty, + table->metadata + empty, + table->metadata + table->buckets_mask + 1, + home_bucket + }; + return itr; +} + +// Resizes the bucket array. +// This function assumes that bucket_count is a power of two and large enough to accommodate all keys without violating +// the maximum load factor. +// Returns false in the case of allocation failure. +// As this function is called very rarely in _insert and _get_or_insert, ideally it should not be inlined into those +// functions. +// In testing, the no-inline approach showed a performance benefit when inserting existing keys (i.e. replacing). +#ifdef __GNUC__ +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wattributes" // Silence warning about combining noinline with static inline. +__attribute__((noinline)) static inline +#elif defined( _MSC_VER ) +__declspec(noinline) static inline +#else +static inline +#endif +bool VT_CAT( NAME, _rehash )( NAME *table, size_t bucket_count ) +{ + // The attempt to resize the bucket array and rehash the keys must occur inside a loop that incrementally doubles the + // target bucket count because a failure could theoretically occur at any load factor due to the displacement limit. + while( true ) + { + NAME new_table = { + 0, + bucket_count - 1, + NULL, + NULL + #ifdef CTX_TY + , table->ctx + #endif + }; + + void *allocation = MALLOC_FN( + VT_CAT( NAME, _total_alloc_size )( &new_table ) + #ifdef CTX_TY + , &new_table.ctx + #endif + ); + + if( VT_UNLIKELY( !allocation ) ) + return false; + + new_table.buckets = (VT_CAT( NAME, _bucket ) *)allocation; + new_table.metadata = (uint16_t *)( (unsigned char *)allocation + VT_CAT( NAME, _metadata_offset )( &new_table ) ); + + memset( new_table.metadata, 0x00, ( bucket_count + 4 ) * sizeof( uint16_t ) ); + + // Iteration stopper at the end of the actual metadata array (i.e. the first of the four excess metadata). + new_table.metadata[ bucket_count ] = 0x01; + + for( size_t bucket = 0; bucket < VT_CAT( NAME, _bucket_count )( table ); ++bucket ) + if( table->metadata[ bucket ] != VT_EMPTY ) + { + VT_CAT( NAME, _itr ) itr = VT_CAT( NAME, _insert_raw )( + &new_table, + table->buckets[ bucket ].key, + #ifdef VAL_TY + &table->buckets[ bucket ].val, + #endif + true, + false + ); + + if( VT_UNLIKELY( VT_CAT( NAME, _is_end )( itr ) ) ) + break; + } + + // If a key could not be reinserted due to the displacement limit, double the bucket count and retry. + if( VT_UNLIKELY( new_table.key_count < table->key_count ) ) + { + FREE_FN( + new_table.buckets, + VT_CAT( NAME, _total_alloc_size )( &new_table ) + #ifdef CTX_TY + , &new_table.ctx + #endif + ); + + bucket_count *= 2; + continue; + } + + if( table->buckets_mask ) + FREE_FN( + table->buckets, + VT_CAT( NAME, _total_alloc_size )( table ) + #ifdef CTX_TY + , &table->ctx + #endif + ); + + *table = new_table; + return true; + } +} +#ifdef __GNUC__ +#pragma GCC diagnostic pop +#endif + +// Inserts a key, replacing the existing key if it already exists. +// This function wraps insert_raw in a loop that handles growing and rehashing the table if a new key cannot be inserted +// because of the maximum load factor or displacement limit constraints. +// Returns an iterator to the inserted key, or an end iterator in the case of allocation failure. +VT_API_FN_QUALIFIERS VT_CAT( NAME, _itr ) VT_CAT( NAME, _insert )( + NAME *table, + KEY_TY key + #ifdef VAL_TY + , VAL_TY val + #endif +) +{ + while( true ) + { + VT_CAT( NAME, _itr ) itr = VT_CAT( NAME, _insert_raw )( + table, + key, + #ifdef VAL_TY + &val, + #endif + false, + true + ); + + if( + // Lookup succeeded, in which case itr points to the found key. + VT_LIKELY( !VT_CAT( NAME, _is_end )( itr ) ) || + // Lookup failed and rehash also fails, in which case itr is an end iterator. + VT_UNLIKELY( + !VT_CAT( NAME, _rehash )( + table, table->buckets_mask ? VT_CAT( NAME, _bucket_count )( table ) * 2 : VT_MIN_NONZERO_BUCKET_COUNT + ) + ) + ) + return itr; + } +} + +// Same as NAME_insert, except that if the key already exists, no insertion occurs and the function returns an iterator +// to the existing key. +VT_API_FN_QUALIFIERS VT_CAT( NAME, _itr ) VT_CAT( NAME, _get_or_insert )( + NAME *table, + KEY_TY key + #ifdef VAL_TY + , VAL_TY val + #endif +) +{ + while( true ) + { + VT_CAT( NAME, _itr ) itr = VT_CAT( NAME, _insert_raw )( + table, + key, + #ifdef VAL_TY + &val, + #endif + false, + false + ); + + if( + // Lookup succeeded, in which case itr points to the found key. + VT_LIKELY( !VT_CAT( NAME, _is_end )( itr ) ) || + // Lookup failed and rehash also fails, in which case itr is an end iterator. + VT_UNLIKELY( + !VT_CAT( NAME, _rehash )( + table, table->buckets_mask ? VT_CAT( NAME, _bucket_count )( table ) * 2 : VT_MIN_NONZERO_BUCKET_COUNT + ) + ) + ) + return itr; + } +} + +// Returns an iterator pointing to the specified key, or an end iterator if the key does not exist. +VT_API_FN_QUALIFIERS VT_CAT( NAME, _itr ) VT_CAT( NAME, _get )( const NAME *table, KEY_TY key ) +{ + uint64_t hash = HASH_FN( key ); + size_t home_bucket = hash & table->buckets_mask; + + // If the home bucket is empty or contains a key that does not belong there, then our key does not exist. + // This check also implicitly handles the case of a zero bucket count, since home_bucket will be zero and + // metadata[ 0 ] will be the empty placeholder. + if( !( table->metadata[ home_bucket ] & VT_IN_HOME_BUCKET_MASK ) ) + return VT_CAT( NAME, _end_itr )(); + + // Traverse the chain of keys belonging to the home bucket. + uint16_t hashfrag = vt_hashfrag( hash ); + size_t bucket = home_bucket; + while( true ) + { + if( + ( table->metadata[ bucket ] & VT_HASH_FRAG_MASK ) == hashfrag && + VT_LIKELY( CMPR_FN( table->buckets[ bucket ].key, key ) ) + ) + { + VT_CAT( NAME, _itr ) itr = { + table->buckets + bucket, + table->metadata + bucket, + table->metadata + table->buckets_mask + 1, + home_bucket + }; + return itr; + } + + uint16_t displacement = table->metadata[ bucket ] & VT_DISPLACEMENT_MASK; + if( displacement == VT_DISPLACEMENT_MASK ) + return VT_CAT( NAME, _end_itr )(); + + bucket = ( home_bucket + vt_quadratic( displacement ) ) & table->buckets_mask; + } +} + +// Erases the key pointed to by the specified iterator. +// The erasure always occurs at the end of the chain to which the key belongs. +// If the key to be erased is not the last in the chain, it is swapped with the last so that erasure occurs at the end. +// This helps keep a chain's keys close to their home bucket for the sake of cache locality. +// Returns true if, in the case of iteration from first to end, NAME_next should now be called on the iterator to find +// the next key. +// This return value is necessary because at the iterator location, the erasure could result in an empty bucket, a +// bucket containing a moved key already visited during the iteration, or a bucket containing a moved key not yet +// visited. +VT_API_FN_QUALIFIERS bool VT_CAT( NAME, _erase_itr_raw )( NAME *table, VT_CAT( NAME, _itr ) itr ) +{ + --table->key_count; + size_t itr_bucket = itr.metadatum - table->metadata; + + // For now, we only call the value's destructor because the key may need to be hashed below to determine the home + // bucket. + #ifdef VAL_DTOR_FN + VAL_DTOR_FN( table->buckets[ itr_bucket ].val ); + #endif + + // Case 1: The key is the only one in its chain, so just remove it. + if( + table->metadata[ itr_bucket ] & VT_IN_HOME_BUCKET_MASK && + ( table->metadata[ itr_bucket ] & VT_DISPLACEMENT_MASK ) == VT_DISPLACEMENT_MASK + ) + { + #ifdef KEY_DTOR_FN + KEY_DTOR_FN( table->buckets[ itr_bucket ].key ); + #endif + table->metadata[ itr_bucket ] = VT_EMPTY; + return true; + } + + // Case 2 and 3 require that we know the key's home bucket, which the iterator may not have recorded. + if( itr.home_bucket == SIZE_MAX ) + { + if( table->metadata[ itr_bucket ] & VT_IN_HOME_BUCKET_MASK ) + itr.home_bucket = itr_bucket; + else + itr.home_bucket = HASH_FN( table->buckets[ itr_bucket ].key ) & table->buckets_mask; + } + + // The key can now be safely destructed for cases 2 and 3. + #ifdef KEY_DTOR_FN + KEY_DTOR_FN( table->buckets[ itr_bucket ].key ); + #endif + + // Case 2: The key is the last in a multi-key chain. + // Traverse the chain from the beginning and find the penultimate key. + // Then disconnect the key and erase. + if( ( table->metadata[ itr_bucket ] & VT_DISPLACEMENT_MASK ) == VT_DISPLACEMENT_MASK ) + { + size_t bucket = itr.home_bucket; + while( true ) + { + uint16_t displacement = table->metadata[ bucket ] & VT_DISPLACEMENT_MASK; + size_t next = ( itr.home_bucket + vt_quadratic( displacement ) ) & table->buckets_mask; + if( next == itr_bucket ) + { + table->metadata[ bucket ] |= VT_DISPLACEMENT_MASK; + table->metadata[ itr_bucket ] = VT_EMPTY; + return true; + } + + bucket = next; + } + } + + // Case 3: The chain has multiple keys, and the key is not the last one. + // Traverse the chain from the key to be erased and find the last and penultimate keys. + // Disconnect the last key from the chain, and swap it with the key to erase. + size_t bucket = itr_bucket; + while( true ) + { + size_t prev = bucket; + bucket = ( itr.home_bucket + vt_quadratic( table->metadata[ bucket ] & VT_DISPLACEMENT_MASK ) ) & + table->buckets_mask; + + if( ( table->metadata[ bucket ] & VT_DISPLACEMENT_MASK ) == VT_DISPLACEMENT_MASK ) + { + table->buckets[ itr_bucket ] = table->buckets[ bucket ]; + + table->metadata[ itr_bucket ] = ( table->metadata[ itr_bucket ] & ~VT_HASH_FRAG_MASK ) | ( + table->metadata[ bucket ] & VT_HASH_FRAG_MASK ); + + table->metadata[ prev ] |= VT_DISPLACEMENT_MASK; + table->metadata[ bucket ] = VT_EMPTY; + + // Whether the iterator should be advanced depends on whether the key moved to the iterator bucket came from + // before or after that bucket. + // In the former case, the iteration would already have hit the moved key, so the iterator should still be + // advanced. + if( bucket > itr_bucket ) + return false; + + return true; + } + } +} + +// Erases the specified key, if it exists. +// Returns true if a key was erased. +VT_API_FN_QUALIFIERS bool VT_CAT( NAME, _erase )( NAME *table, KEY_TY key ) +{ + VT_CAT( NAME, _itr ) itr = VT_CAT( NAME, _get)( table, key ); + if( VT_CAT( NAME, _is_end )( itr ) ) + return false; + + VT_CAT( NAME, _erase_itr_raw )( table, itr ); + return true; +} + +// Finds the first occupied bucket at or after the bucket pointed to by itr. +// This function scans four buckets at a time, ideally using intrinsics. +static inline void VT_CAT( NAME, _fast_forward )( VT_CAT( NAME, _itr ) *itr ) +{ + while( true ) + { + uint64_t metadata; + memcpy( &metadata, itr->metadatum, sizeof( uint64_t ) ); + if( metadata ) + { + int offset = vt_first_nonzero_uint16( metadata ); + itr->data += offset; + itr->metadatum += offset; + itr->home_bucket = SIZE_MAX; + return; + } + + itr->data += 4; + itr->metadatum += 4; + } +} + +VT_API_FN_QUALIFIERS VT_CAT( NAME, _itr ) VT_CAT( NAME, _next )( VT_CAT( NAME, _itr ) itr ) +{ + ++itr.data; + ++itr.metadatum; + VT_CAT( NAME, _fast_forward )( &itr ); + return itr; +} + +// Returns the minimum bucket count required to accommodate a certain number of keys, which is governed by the maximum +// load factor. +static inline size_t VT_CAT( NAME, _min_bucket_count_for_size )( size_t size ) +{ + if( size == 0 ) + return 0; + + // Round up to a power of two. + size_t bucket_count = VT_MIN_NONZERO_BUCKET_COUNT; + while( size > bucket_count * MAX_LOAD ) + bucket_count *= 2; + + return bucket_count; +} + +VT_API_FN_QUALIFIERS bool VT_CAT( NAME, _reserve )( NAME *table, size_t size ) +{ + size_t bucket_count = VT_CAT( NAME, _min_bucket_count_for_size )( size ); + + if( bucket_count <= VT_CAT( NAME, _bucket_count )( table ) ) + return true; + + return VT_CAT( NAME, _rehash )( table, bucket_count ); +} + +VT_API_FN_QUALIFIERS bool VT_CAT( NAME, _shrink )( NAME *table ) +{ + size_t bucket_count = VT_CAT( NAME, _min_bucket_count_for_size )( table->key_count ); + + if( bucket_count == VT_CAT( NAME, _bucket_count )( table ) ) // Shrink unnecessary. + return true; + + if( bucket_count == 0 ) + { + FREE_FN( + table->buckets, + VT_CAT( NAME, _total_alloc_size )( table ) + #ifdef CTX_TY + , &table->ctx + #endif + ); + + table->buckets_mask = 0x0000000000000000ull; + table->metadata = (uint16_t *)&vt_empty_placeholder_metadatum; + return true; + } + + return VT_CAT( NAME, _rehash )( table, bucket_count ); +} + +VT_API_FN_QUALIFIERS VT_CAT( NAME, _itr ) VT_CAT( NAME, _first )( const NAME *table ) +{ + if( !table->key_count ) + return VT_CAT( NAME, _end_itr )(); + + VT_CAT( NAME, _itr ) itr = { table->buckets, table->metadata, table->metadata + table->buckets_mask + 1, SIZE_MAX }; + VT_CAT( NAME, _fast_forward )( &itr ); + return itr; +} + +VT_API_FN_QUALIFIERS void VT_CAT( NAME, _clear )( NAME *table ) +{ + if( !table->key_count ) + return; + + for( size_t i = 0; i < VT_CAT( NAME, _bucket_count )( table ); ++i ) + { + if( table->metadata[ i ] != VT_EMPTY ) + { + #ifdef KEY_DTOR_FN + KEY_DTOR_FN( table->buckets[ i ].key ); + #endif + #ifdef VAL_DTOR_FN + VAL_DTOR_FN( table->buckets[ i ].val ); + #endif + } + + table->metadata[ i ] = VT_EMPTY; + } + + table->key_count = 0; +} + +VT_API_FN_QUALIFIERS void VT_CAT( NAME, _cleanup )( NAME *table ) +{ + if( !table->buckets_mask ) + return; + + #if defined( KEY_DTOR_FN ) || defined( VAL_DTOR_FN ) + VT_CAT( NAME, _clear )( table ); + #endif + + FREE_FN( + table->buckets, + VT_CAT( NAME, _total_alloc_size )( table ) + #ifdef CTX_TY + , &table->ctx + #endif + ); + + VT_CAT( NAME, _init )( + table + #ifdef CTX_TY + , table->ctx + #endif + ); +} + +#endif + +/*--------------------------------------------------------------------------------------------------------------------*/ +/* Wrapper types and functions for the C11 generic API */ +/*--------------------------------------------------------------------------------------------------------------------*/ + +#if defined(__STDC_VERSION__) && \ + __STDC_VERSION__ >= 201112L && \ + !defined( IMPLEMENTATION_MODE ) && \ + !defined( VT_NO_C11_GENERIC_API ) \ + +typedef NAME VT_CAT( vt_table_, VT_TEMPLATE_COUNT ); +typedef VT_CAT( NAME, _itr ) VT_CAT( vt_table_itr_, VT_TEMPLATE_COUNT ); + +static inline void VT_CAT( vt_init_, VT_TEMPLATE_COUNT )( + NAME *table + #ifdef CTX_TY + , CTX_TY ctx + #endif +) +{ + VT_CAT( NAME, _init )( + table + #ifdef CTX_TY + , ctx + #endif + ); +} + +static inline bool VT_CAT( vt_init_clone_, VT_TEMPLATE_COUNT )( + NAME *table, + const NAME* source + #ifdef CTX_TY + , CTX_TY ctx + #endif +) +{ + return VT_CAT( NAME, _init_clone )( + table, + source + #ifdef CTX_TY + , ctx + #endif + ); +} + +static inline size_t VT_CAT( vt_size_, VT_TEMPLATE_COUNT )( const NAME *table ) +{ + return VT_CAT( NAME, _size )( table ); +} + +static inline size_t VT_CAT( vt_bucket_count_, VT_TEMPLATE_COUNT )( const NAME *table ) +{ + return VT_CAT( NAME, _bucket_count )( table ); +} + +static inline bool VT_CAT( vt_is_end_, VT_TEMPLATE_COUNT )( VT_CAT( NAME, _itr ) itr ) +{ + return VT_CAT( NAME, _is_end )( itr ); +} + +static inline VT_CAT( NAME, _itr ) VT_CAT( vt_insert_, VT_TEMPLATE_COUNT )( + NAME *table, + KEY_TY key + #ifdef VAL_TY + , VAL_TY val + #endif +) +{ + return VT_CAT( NAME, _insert )( + table, + key + #ifdef VAL_TY + , val + #endif + ); +} + +static inline VT_CAT( NAME, _itr ) VT_CAT( vt_get_or_insert_, VT_TEMPLATE_COUNT )( + NAME *table, + KEY_TY key + #ifdef VAL_TY + , VAL_TY val + #endif +) +{ + return VT_CAT( NAME, _get_or_insert )( + table, + key + #ifdef VAL_TY + , val + #endif + ); +} + +static inline VT_CAT( NAME, _itr ) VT_CAT( vt_get_, VT_TEMPLATE_COUNT )( const NAME *table, KEY_TY key ) +{ + return VT_CAT( NAME, _get )( table, key ); +} + +static inline bool VT_CAT( vt_erase_, VT_TEMPLATE_COUNT )( NAME *table, KEY_TY key ) +{ + return VT_CAT( NAME, _erase )( table, key ); +} + +static inline VT_CAT( NAME, _itr ) VT_CAT( vt_next_, VT_TEMPLATE_COUNT )( VT_CAT( NAME, _itr ) itr ) +{ + return VT_CAT( NAME, _next )( itr ); +} + +static inline VT_CAT( NAME, _itr ) VT_CAT( vt_erase_itr_, VT_TEMPLATE_COUNT )( NAME *table, VT_CAT( NAME, _itr ) itr ) +{ + return VT_CAT( NAME, _erase_itr )( table, itr ); +} + +static inline bool VT_CAT( vt_reserve_, VT_TEMPLATE_COUNT )( NAME *table, size_t bucket_count ) +{ + return VT_CAT( NAME, _reserve )( table, bucket_count ); +} + +static inline bool VT_CAT( vt_shrink_, VT_TEMPLATE_COUNT )( NAME *table ) +{ + return VT_CAT( NAME, _shrink )( table ); +} + +static inline VT_CAT( NAME, _itr ) VT_CAT( vt_first_, VT_TEMPLATE_COUNT )( const NAME *table ) +{ + return VT_CAT( NAME, _first )( table ); +} + +static inline void VT_CAT( vt_clear_, VT_TEMPLATE_COUNT )( NAME *table ) +{ + VT_CAT( NAME, _clear )( table ); +} + +static inline void VT_CAT( vt_cleanup_, VT_TEMPLATE_COUNT )( NAME *table ) +{ + VT_CAT( NAME, _cleanup )( table ); +} + +// Increment the template counter. +#if VT_TEMPLATE_COUNT_D1 == 0 +#undef VT_TEMPLATE_COUNT_D1 +#define VT_TEMPLATE_COUNT_D1 1 +#elif VT_TEMPLATE_COUNT_D1 == 1 +#undef VT_TEMPLATE_COUNT_D1 +#define VT_TEMPLATE_COUNT_D1 2 +#elif VT_TEMPLATE_COUNT_D1 == 2 +#undef VT_TEMPLATE_COUNT_D1 +#define VT_TEMPLATE_COUNT_D1 3 +#elif VT_TEMPLATE_COUNT_D1 == 3 +#undef VT_TEMPLATE_COUNT_D1 +#define VT_TEMPLATE_COUNT_D1 4 +#elif VT_TEMPLATE_COUNT_D1 == 4 +#undef VT_TEMPLATE_COUNT_D1 +#define VT_TEMPLATE_COUNT_D1 5 +#elif VT_TEMPLATE_COUNT_D1 == 5 +#undef VT_TEMPLATE_COUNT_D1 +#define VT_TEMPLATE_COUNT_D1 6 +#elif VT_TEMPLATE_COUNT_D1 == 6 +#undef VT_TEMPLATE_COUNT_D1 +#define VT_TEMPLATE_COUNT_D1 7 +#elif VT_TEMPLATE_COUNT_D1 == 7 +#undef VT_TEMPLATE_COUNT_D1 +#define VT_TEMPLATE_COUNT_D1 0 +#if VT_TEMPLATE_COUNT_D2 == 0 +#undef VT_TEMPLATE_COUNT_D2 +#define VT_TEMPLATE_COUNT_D2 1 +#elif VT_TEMPLATE_COUNT_D2 == 1 +#undef VT_TEMPLATE_COUNT_D2 +#define VT_TEMPLATE_COUNT_D2 2 +#elif VT_TEMPLATE_COUNT_D2 == 2 +#undef VT_TEMPLATE_COUNT_D2 +#define VT_TEMPLATE_COUNT_D2 3 +#elif VT_TEMPLATE_COUNT_D2 == 3 +#undef VT_TEMPLATE_COUNT_D2 +#define VT_TEMPLATE_COUNT_D2 4 +#elif VT_TEMPLATE_COUNT_D2 == 4 +#undef VT_TEMPLATE_COUNT_D2 +#define VT_TEMPLATE_COUNT_D2 5 +#elif VT_TEMPLATE_COUNT_D2 == 5 +#undef VT_TEMPLATE_COUNT_D2 +#define VT_TEMPLATE_COUNT_D2 6 +#elif VT_TEMPLATE_COUNT_D2 == 6 +#undef VT_TEMPLATE_COUNT_D2 +#define VT_TEMPLATE_COUNT_D2 7 +#elif VT_TEMPLATE_COUNT_D2 == 7 +#undef VT_TEMPLATE_COUNT_D2 +#define VT_TEMPLATE_COUNT_D2 0 +#if VT_TEMPLATE_COUNT_D3 == 0 +#undef VT_TEMPLATE_COUNT_D3 +#define VT_TEMPLATE_COUNT_D3 1 +#elif VT_TEMPLATE_COUNT_D3 == 1 +#undef VT_TEMPLATE_COUNT_D3 +#define VT_TEMPLATE_COUNT_D3 2 +#elif VT_TEMPLATE_COUNT_D3 == 2 +#undef VT_TEMPLATE_COUNT_D3 +#define VT_TEMPLATE_COUNT_D3 3 +#elif VT_TEMPLATE_COUNT_D3 == 3 +#undef VT_TEMPLATE_COUNT_D3 +#define VT_TEMPLATE_COUNT_D3 4 +#elif VT_TEMPLATE_COUNT_D3 == 4 +#undef VT_TEMPLATE_COUNT_D3 +#define VT_TEMPLATE_COUNT_D3 5 +#elif VT_TEMPLATE_COUNT_D3 == 5 +#undef VT_TEMPLATE_COUNT_D3 +#define VT_TEMPLATE_COUNT_D3 6 +#elif VT_TEMPLATE_COUNT_D3 == 6 +#undef VT_TEMPLATE_COUNT_D3 +#define VT_TEMPLATE_COUNT_D3 7 +#elif VT_TEMPLATE_COUNT_D3 == 7 +#error Sorry, the number of template instances is limited to 511. Define VT_NO_C11_GENERIC_API globally and use the \ +C99 prefixed function API to circumvent this restriction. +#endif +#endif +#endif + +#endif + +#undef NAME +#undef KEY_TY +#undef VAL_TY +#undef HASH_FN +#undef CMPR_FN +#undef MAX_LOAD +#undef KEY_DTOR_FN +#undef VAL_DTOR_FN +#undef CTX_TY +#undef MALLOC_FN +#undef FREE_FN +#undef HEADER_MODE +#undef IMPLEMENTATION_MODE +#undef VT_API_FN_QUALIFIERS diff --git a/vendor/box3d/src/src/weld_joint.c b/vendor/box3d/src/src/weld_joint.c new file mode 100644 index 000000000..611b714fa --- /dev/null +++ b/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 ); +} diff --git a/vendor/box3d/src/src/wheel_joint.c b/vendor/box3d/src/src/wheel_joint.c new file mode 100644 index 000000000..05665e9f0 --- /dev/null +++ b/vendor/box3d/src/src/wheel_joint.c @@ -0,0 +1,1105 @@ +// 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 b3WheelJoint_EnableSuspension( b3JointId jointId, bool enableSpring ) +{ + b3World* world = b3GetWorld( jointId.world0 ); + B3_REC( world, WheelJointEnableSuspension, jointId, enableSpring ); + b3JointSim* joint = b3GetJointSimCheckType( jointId, b3_wheelJoint ); + + if ( enableSpring != joint->wheelJoint.enableSuspensionSpring ) + { + joint->wheelJoint.enableSuspensionSpring = enableSpring; + joint->wheelJoint.suspensionSpringImpulse = 0.0f; + } +} + +bool b3WheelJoint_IsSuspensionEnabled( b3JointId jointId ) +{ + b3JointSim* joint = b3GetJointSimCheckType( jointId, b3_wheelJoint ); + return joint->wheelJoint.enableSuspensionSpring; +} + +void b3WheelJoint_SetSuspensionHertz( b3JointId jointId, float hertz ) +{ + b3World* world = b3GetWorld( jointId.world0 ); + B3_REC( world, WheelJointSetSuspensionHertz, jointId, hertz ); + b3JointSim* joint = b3GetJointSimCheckType( jointId, b3_wheelJoint ); + joint->wheelJoint.suspensionHertz = hertz; +} + +float b3WheelJoint_GetSuspensionHertz( b3JointId jointId ) +{ + b3JointSim* joint = b3GetJointSimCheckType( jointId, b3_wheelJoint ); + return joint->wheelJoint.suspensionHertz; +} + +void b3WheelJoint_SetSuspensionDampingRatio( b3JointId jointId, float dampingRatio ) +{ + b3World* world = b3GetWorld( jointId.world0 ); + B3_REC( world, WheelJointSetSuspensionDampingRatio, jointId, dampingRatio ); + b3JointSim* joint = b3GetJointSimCheckType( jointId, b3_wheelJoint ); + joint->wheelJoint.suspensionDampingRatio = dampingRatio; +} + +float b3WheelJoint_GetSuspensionDampingRatio( b3JointId jointId ) +{ + b3JointSim* joint = b3GetJointSimCheckType( jointId, b3_wheelJoint ); + return joint->wheelJoint.suspensionDampingRatio; +} + +void b3WheelJoint_EnableSuspensionLimit( b3JointId jointId, bool enableLimit ) +{ + b3World* world = b3GetWorld( jointId.world0 ); + B3_REC( world, WheelJointEnableSuspensionLimit, jointId, enableLimit ); + b3JointSim* joint = b3GetJointSimCheckType( jointId, b3_wheelJoint ); + if ( joint->wheelJoint.enableSuspensionLimit != enableLimit ) + { + joint->wheelJoint.lowerSuspensionImpulse = 0.0f; + joint->wheelJoint.upperSuspensionImpulse = 0.0f; + joint->wheelJoint.enableSuspensionLimit = enableLimit; + } +} + +bool b3WheelJoint_IsSuspensionLimitEnabled( b3JointId jointId ) +{ + b3JointSim* joint = b3GetJointSimCheckType( jointId, b3_wheelJoint ); + return joint->wheelJoint.enableSuspensionLimit; +} + +float b3WheelJoint_GetLowerSuspensionLimit( b3JointId jointId ) +{ + b3JointSim* joint = b3GetJointSimCheckType( jointId, b3_wheelJoint ); + return joint->wheelJoint.lowerSuspensionLimit; +} + +float b3WheelJoint_GetUpperSuspensionLimit( b3JointId jointId ) +{ + b3JointSim* joint = b3GetJointSimCheckType( jointId, b3_wheelJoint ); + return joint->wheelJoint.upperSuspensionLimit; +} + +void b3WheelJoint_SetSuspensionLimits( b3JointId jointId, float lower, float upper ) +{ + B3_ASSERT( lower <= upper ); + b3World* world = b3GetWorld( jointId.world0 ); + B3_REC( world, WheelJointSetSuspensionLimits, jointId, lower, upper ); + b3JointSim* joint = b3GetJointSimCheckType( jointId, b3_wheelJoint ); + if ( lower != joint->wheelJoint.lowerSuspensionLimit || upper != joint->wheelJoint.upperSuspensionLimit ) + { + joint->wheelJoint.lowerSuspensionLimit = lower; + joint->wheelJoint.upperSuspensionLimit = upper; + joint->wheelJoint.lowerSuspensionImpulse = 0.0f; + joint->wheelJoint.upperSuspensionImpulse = 0.0f; + } +} + +void b3WheelJoint_EnableSpinMotor( b3JointId jointId, bool enableMotor ) +{ + b3World* world = b3GetWorld( jointId.world0 ); + B3_REC( world, WheelJointEnableSpinMotor, jointId, enableMotor ); + b3JointSim* joint = b3GetJointSimCheckType( jointId, b3_wheelJoint ); + if ( joint->wheelJoint.enableSpinMotor != enableMotor ) + { + joint->wheelJoint.spinImpulse = 0.0f; + joint->wheelJoint.enableSpinMotor = enableMotor; + } +} + +bool b3WheelJoint_IsSpinMotorEnabled( b3JointId jointId ) +{ + b3JointSim* joint = b3GetJointSimCheckType( jointId, b3_wheelJoint ); + return joint->wheelJoint.enableSpinMotor; +} + +void b3WheelJoint_SetSpinMotorSpeed( b3JointId jointId, float motorSpeed ) +{ + b3World* world = b3GetWorld( jointId.world0 ); + B3_REC( world, WheelJointSetSpinMotorSpeed, jointId, motorSpeed ); + b3JointSim* joint = b3GetJointSimCheckType( jointId, b3_wheelJoint ); + joint->wheelJoint.spinSpeed = motorSpeed; +} + +float b3WheelJoint_GetSpinMotorSpeed( b3JointId jointId ) +{ + b3JointSim* joint = b3GetJointSimCheckType( jointId, b3_wheelJoint ); + return joint->wheelJoint.spinSpeed; +} + +void b3WheelJoint_SetMaxSpinTorque( b3JointId jointId, float torque ) +{ + b3World* world = b3GetWorld( jointId.world0 ); + B3_REC( world, WheelJointSetMaxSpinTorque, jointId, torque ); + b3JointSim* joint = b3GetJointSimCheckType( jointId, b3_wheelJoint ); + joint->wheelJoint.maxSpinTorque = torque; +} + +float b3WheelJoint_GetMaxSpinTorque( b3JointId jointId ) +{ + b3JointSim* joint = b3GetJointSimCheckType( jointId, b3_wheelJoint ); + return joint->wheelJoint.maxSpinTorque; +} + +void b3WheelJoint_EnableSteering( b3JointId jointId, bool flag ) +{ + b3World* world = b3GetWorld( jointId.world0 ); + B3_REC( world, WheelJointEnableSteering, jointId, flag ); + b3JointSim* joint = b3GetJointSimCheckType( jointId, b3_wheelJoint ); + if ( joint->wheelJoint.enableSteering != flag ) + { + joint->wheelJoint.angularImpulse = (b3Vec2){ 0.0f, 0.0f }; + joint->wheelJoint.enableSteering = flag; + } +} + +bool b3WheelJoint_IsSteeringEnabled( b3JointId jointId ) +{ + b3JointSim* joint = b3GetJointSimCheckType( jointId, b3_wheelJoint ); + return joint->wheelJoint.enableSteering; +} + +void b3WheelJoint_SetSteeringHertz( b3JointId jointId, float hertz ) +{ + b3World* world = b3GetWorld( jointId.world0 ); + B3_REC( world, WheelJointSetSteeringHertz, jointId, hertz ); + b3JointSim* joint = b3GetJointSimCheckType( jointId, b3_wheelJoint ); + joint->wheelJoint.steeringHertz = hertz; +} + +float b3WheelJoint_GetSteeringHertz( b3JointId jointId ) +{ + b3JointSim* joint = b3GetJointSimCheckType( jointId, b3_wheelJoint ); + return joint->wheelJoint.steeringHertz; +} + +void b3WheelJoint_SetSteeringDampingRatio( b3JointId jointId, float dampingRatio ) +{ + b3World* world = b3GetWorld( jointId.world0 ); + B3_REC( world, WheelJointSetSteeringDampingRatio, jointId, dampingRatio ); + b3JointSim* joint = b3GetJointSimCheckType( jointId, b3_wheelJoint ); + joint->wheelJoint.steeringDampingRatio = dampingRatio; +} + +float b3WheelJoint_GetSteeringDampingRatio( b3JointId jointId ) +{ + b3JointSim* joint = b3GetJointSimCheckType( jointId, b3_wheelJoint ); + return joint->wheelJoint.steeringDampingRatio; +} + +void b3WheelJoint_SetMaxSteeringTorque( b3JointId jointId, float maxTorque ) +{ + b3World* world = b3GetWorld( jointId.world0 ); + B3_REC( world, WheelJointSetMaxSteeringTorque, jointId, maxTorque ); + b3JointSim* joint = b3GetJointSimCheckType( jointId, b3_wheelJoint ); + joint->wheelJoint.maxSteeringTorque = maxTorque; +} + +float b3WheelJoint_GetMaxSteeringTorque( b3JointId jointId ) +{ + b3JointSim* joint = b3GetJointSimCheckType( jointId, b3_wheelJoint ); + return joint->wheelJoint.maxSteeringTorque; +} + +void b3WheelJoint_EnableSteeringLimit( b3JointId jointId, bool flag ) +{ + b3World* world = b3GetWorld( jointId.world0 ); + B3_REC( world, WheelJointEnableSteeringLimit, jointId, flag ); + b3JointSim* joint = b3GetJointSimCheckType( jointId, b3_wheelJoint ); + if ( joint->wheelJoint.enableSteeringLimit != flag ) + { + joint->wheelJoint.lowerSteeringImpulse = 0.0f; + joint->wheelJoint.upperSteeringImpulse = 0.0f; + joint->wheelJoint.enableSteeringLimit = flag; + } +} + +bool b3WheelJoint_IsSteeringLimitEnabled( b3JointId jointId ) +{ + b3JointSim* joint = b3GetJointSimCheckType( jointId, b3_wheelJoint ); + return joint->wheelJoint.enableSteeringLimit; +} + +float b3WheelJoint_GetLowerSteeringLimit( b3JointId jointId ) +{ + b3JointSim* joint = b3GetJointSimCheckType( jointId, b3_wheelJoint ); + return joint->wheelJoint.lowerSteeringLimit; +} + +float b3WheelJoint_GetUpperSteeringLimit( b3JointId jointId ) +{ + b3JointSim* joint = b3GetJointSimCheckType( jointId, b3_wheelJoint ); + return joint->wheelJoint.upperSteeringLimit; +} + +void b3WheelJoint_SetSteeringLimits( b3JointId jointId, float lowerRadians, float upperRadians ) +{ + B3_ASSERT( lowerRadians <= upperRadians ); + b3World* world = b3GetWorld( jointId.world0 ); + B3_REC( world, WheelJointSetSteeringLimits, jointId, lowerRadians, upperRadians ); + b3JointSim* joint = b3GetJointSimCheckType( jointId, b3_wheelJoint ); + joint->wheelJoint.lowerSteeringLimit = lowerRadians; + joint->wheelJoint.upperSteeringLimit = upperRadians; +} + +void b3WheelJoint_SetTargetSteeringAngle( b3JointId jointId, float radians ) +{ + b3World* world = b3GetWorld( jointId.world0 ); + B3_REC( world, WheelJointSetTargetSteeringAngle, jointId, radians ); + b3JointSim* joint = b3GetJointSimCheckType( jointId, b3_wheelJoint ); + joint->wheelJoint.targetSteeringAngle = radians; +} + +float b3WheelJoint_GetTargetSteeringAngle( b3JointId jointId ) +{ + b3JointSim* joint = b3GetJointSimCheckType( jointId, b3_wheelJoint ); + return joint->wheelJoint.targetSteeringAngle; +} + +float b3WheelJoint_GetSpinSpeed( b3JointId jointId ) +{ + b3World* world = b3GetWorld( jointId.world0 ); + b3JointSim* base = b3GetJointSimCheckType( jointId, b3_wheelJoint ); + + int idA = base->bodyIdA; + int idB = base->bodyIdB; + + b3Body* bodyA = b3Array_Get( world->bodies, idA ); + b3Body* bodyB = b3Array_Get( world->bodies, idB ); + + b3SolverSet* setB = b3Array_Get( world->solverSets, bodyB->setIndex ); + int localIndexB = bodyB->localIndex; + b3BodySim* bodySimB = b3Array_Get( setB->bodySims, localIndexB ); + + b3Quat quatB = b3MulQuat( bodySimB->transform.q, base->localFrameB.q ); + b3Vec3 spinAxis = b3RotateVector( quatB, b3Vec3_axisZ ); + + b3Vec3 wA = b3Vec3_zero; + b3BodyState* stateA = b3GetBodyState( world, bodyA ); + if ( stateA != NULL ) + { + wA = stateA->angularVelocity; + } + + b3Vec3 wB = b3Vec3_zero; + b3BodyState* stateB = b3GetBodyState( world, bodyB ); + if ( stateB != NULL ) + { + wB = stateB->angularVelocity; + } + + float speed = b3Dot( b3Sub( wB, wA ), spinAxis ); + return speed; +} + +float b3WheelJoint_GetSpinTorque( b3JointId jointId ) +{ + b3World* world = b3GetWorld( jointId.world0 ); + b3JointSim* joint = b3GetJointSimCheckType( jointId, b3_wheelJoint ); + return world->inv_h * joint->wheelJoint.spinImpulse; +} + +float b3WheelJoint_GetSteeringAngle( b3JointId jointId ) +{ + b3World* world = b3GetWorld( jointId.world0 ); + b3JointSim* base = b3GetJointSimCheckType( jointId, b3_wheelJoint ); + + int idA = base->bodyIdA; + int idB = base->bodyIdB; + + b3Body* bodyA = b3Array_Get( world->bodies, idA ); + b3Body* bodyB = b3Array_Get( world->bodies, idB ); + + 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 ); + + b3Quat quatA = b3MulQuat( bodySimA->transform.q, base->localFrameA.q ); + b3Quat quatB = b3MulQuat( bodySimB->transform.q, base->localFrameB.q ); + + b3Matrix3 matrixA = b3MakeMatrixFromQuat( quatA ); + b3Matrix3 matrixB = b3MakeMatrixFromQuat( quatB ); + + // Twist around x-axis + float cs = b3Dot( matrixB.cz, matrixA.cz ); + float ss = -b3Dot( matrixB.cz, matrixA.cy ); + + return b3Atan2( ss, cs ); +} + +float b3WheelJoint_GetSteeringTorque( b3JointId jointId ) +{ + b3World* world = b3GetWorld( jointId.world0 ); + b3JointSim* joint = b3GetJointSimCheckType( jointId, b3_wheelJoint ); + return world->inv_h * joint->wheelJoint.steeringSpringImpulse; +} + +b3Vec3 b3GetWheelJointForce( b3World* world, b3JointSim* base ) +{ + b3WorldTransform transformA = b3GetBodyTransform( world, base->bodyIdA ); + b3WheelJoint* joint = &base->wheelJoint; + + // impulse in joint space + b3Vec3 impulse = { + joint->linearImpulse.x, + joint->linearImpulse.y, + joint->lowerSuspensionLimit + joint->upperSuspensionImpulse + joint->suspensionSpringImpulse, + }; + + // 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 b3GetWheelJointTorque( b3World* world, b3JointSim* base ) +{ + B3_ASSERT( base->type == b3_wheelJoint ); + + // chase body id to the solver set where the body lives + int idA = base->bodyIdA; + // int idB = base->bodyIdB; + + b3Body* bodyA = b3Array_Get( world->bodies, idA ); + // b3Body* bodyB = b3Array_Get( world->bodies, idB ); + + 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 ); + + b3Quat qA = b3MulQuat( bodySimA->transform.q, base->localFrameA.q ); + + b3Matrix3 matrixA = b3MakeMatrixFromQuat( qA ); + + return b3MulSV( world->inv_h * base->wheelJoint.spinImpulse, matrixA.cz ); +} + +// See constraints.pdf + +void b3PrepareWheelJoint( b3JointSim* base, b3StepContext* context ) +{ + B3_ASSERT( base->type == b3_wheelJoint ); + + // 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; + + b3WheelJoint* joint = &base->wheelJoint; + + 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 ); + + b3Vec3 rA = joint->frameA.p; + b3Vec3 rB = joint->frameB.p; + + b3Matrix3 matrixA = b3MakeMatrixFromQuat( joint->frameA.q ); + b3Matrix3 matrixB = b3MakeMatrixFromQuat( joint->frameB.q ); + + // todo use fresh effective masses in the sub-step to avoid divergence like I saw for the prismatic joint + + { + b3Vec3 suspensionAxis = matrixA.cx; + b3Vec3 rAn = b3Cross( rA, suspensionAxis ); + b3Vec3 rBn = b3Cross( rB, suspensionAxis ); + + float k = base->invMassA + base->invMassB + b3Dot( rAn, b3MulMV( base->invIA, rAn ) ) + + b3Dot( rBn, b3MulMV( base->invIB, rBn ) ); + joint->suspensionMass = k > 0.0f ? 1.0f / k : 0.0f; + } + + joint->suspensionSoftness = b3MakeSoft( joint->suspensionHertz, joint->suspensionDampingRatio, context->h ); + joint->steeringSoftness = b3MakeSoft( joint->steeringHertz, joint->steeringDampingRatio, context->h ); + + { + // Rotation axis is the z-axis of body A. + b3Vec3 spinAxis = matrixB.cz; + float k = b3Dot( spinAxis, b3MulMV( invInertiaSum, spinAxis ) ); + joint->spinMass = k > 0.0f ? 1.0f / k : 0.0f; + } + + { + // Twist constraint around x-axis + float cs = b3Dot( matrixB.cz, matrixA.cz ); + float ss = -b3Dot( matrixB.cz, matrixA.cy ); + float den = cs * cs + ss * ss; + den = den > 0.0f ? 1.0f / den : 0.0f; + b3Vec3 steeringAxis = + b3MulSV( den, b3Cross( matrixB.cz, b3Sub( b3MulSV( -cs, matrixA.cy ), b3MulSV( ss, matrixA.cz ) ) ) ); + + float k = b3Dot( steeringAxis, b3MulMV( invInertiaSum, steeringAxis ) ); + joint->steeringMass = k > 0.0f ? 1.0f / k : 0.0f; + } + + if ( context->enableWarmStarting == false ) + { + joint->linearImpulse = (b3Vec2){ 0.0f, 0.0f }; + joint->angularImpulse = (b3Vec2){ 0.0f, 0.0f }; + joint->spinImpulse = 0.0f; + joint->suspensionSpringImpulse = 0.0f; + joint->lowerSuspensionImpulse = 0.0f; + joint->upperSuspensionImpulse = 0.0f; + joint->steeringSpringImpulse = 0.0f; + joint->lowerSteeringImpulse = 0.0f; + joint->upperSteeringImpulse = 0.0f; + } +} + +void b3WarmStartWheelJoint( b3JointSim* base, b3StepContext* context ) +{ + B3_ASSERT( base->type == b3_wheelJoint ); + + 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; + + b3WheelJoint* joint = &base->wheelJoint; + + 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 d = b3Add( b3Add( b3Sub( stateB->deltaPosition, stateA->deltaPosition ), joint->deltaCenter ), b3Sub( rB, rA ) ); + + 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 ); + } + + b3Matrix3 matrixA = b3MakeMatrixFromQuat( quatA ); + b3Matrix3 matrixB = b3MakeMatrixFromQuat( quatB ); + + b3Vec3 sAx = b3Cross( b3Add( d, rA ), matrixA.cx ); + b3Vec3 sBx = b3Cross( rB, matrixA.cx ); + b3Vec3 sAy = b3Cross( b3Add( d, rA ), matrixA.cy ); + b3Vec3 sBy = b3Cross( rB, matrixA.cy ); + b3Vec3 sAz = b3Cross( b3Add( d, rA ), matrixA.cz ); + b3Vec3 sBz = b3Cross( rB, matrixA.cz ); + + float suspensionImpulse = joint->suspensionSpringImpulse + joint->lowerSuspensionImpulse - joint->upperSuspensionImpulse; + + float linearImpulseY = joint->linearImpulse.x; + float linearImpulseZ = joint->linearImpulse.y; + float angularImpulseX = joint->angularImpulse.x; + float angularImpulseY = joint->angularImpulse.y; + + b3Vec3 linearImpulse = b3Blend3( suspensionImpulse, matrixA.cx, linearImpulseY, matrixA.cy, linearImpulseZ, matrixA.cz ); + b3Vec3 angularImpulseA = b3Blend3( suspensionImpulse, sAx, linearImpulseY, sAy, linearImpulseZ, sAz ); + b3Vec3 angularImpulseB = b3Blend3( suspensionImpulse, sBx, linearImpulseY, sBy, linearImpulseZ, sBz ); + b3Vec3 angularImpulse = b3MulSV( joint->spinImpulse, matrixA.cz ); + + b3Vec3 spinAxis = matrixB.cz; + + if ( joint->enableSteering ) + { + // Twist constraint around x-axis + float cs = b3Dot( matrixB.cz, matrixA.cz ); + float ss = -b3Dot( matrixB.cz, matrixA.cy ); + float den = cs * cs + ss * ss; + den = den > 0.0f ? 1.0f / den : 0.0f; + b3Vec3 steeringAxis = + b3MulSV( den, b3Cross( matrixB.cz, b3Sub( b3MulSV( -cs, matrixA.cy ), b3MulSV( ss, matrixA.cz ) ) ) ); + + b3Vec3 perpAxis = b3Cross( spinAxis, matrixA.cx ); + float steeringImpulse = joint->steeringSpringImpulse + joint->lowerSteeringImpulse - joint->upperSteeringImpulse; + angularImpulse = b3Blend3( angularImpulseX, perpAxis, joint->spinImpulse, spinAxis, steeringImpulse, steeringAxis ); + } + else + { + b3Quat relQ = b3InvMulQuat( quatA, quatB ); + 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 ) ) ) ); + angularImpulse = b3Add( + angularImpulse, + b3Blend3( angularImpulseX, perpAxisX, angularImpulseY, perpAxisY, joint->spinImpulse, spinAxis ) ); + } + + if ( stateA->flags & b3_dynamicFlag ) + { + stateA->linearVelocity = b3MulSub( stateA->linearVelocity, mA, linearImpulse ); + stateA->angularVelocity = b3Sub( stateA->angularVelocity, b3MulMV( iA, b3Add( angularImpulseA, angularImpulse ) ) ); + } + + if ( stateB->flags & b3_dynamicFlag ) + { + stateB->linearVelocity = b3MulAdd( stateB->linearVelocity, mB, linearImpulse ); + stateB->angularVelocity = b3Add( stateB->angularVelocity, b3MulMV( iB, b3Add( angularImpulseB, angularImpulse ) ) ); + } +} + +void b3SolveWheelJoint( b3JointSim* base, b3StepContext* context, bool useBias ) +{ + B3_ASSERT( base->type == b3_wheelJoint ); + + 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; + + b3WheelJoint* joint = &base->wheelJoint; + + 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; + + // current anchors + b3Vec3 rA = b3RotateVector( stateA->deltaRotation, joint->frameA.p ); + b3Vec3 rB = b3RotateVector( stateB->deltaRotation, joint->frameB.p ); + + 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 ); + b3Matrix3 matrixA = b3MakeMatrixFromQuat( quatA ); + b3Matrix3 matrixB = b3MakeMatrixFromQuat( quatB ); + + // b3Vec3 spinAxis = b3RotateVector( quatB, b3Vec3_axisZ ); + + b3Vec3 d = b3Add( b3Add( b3Sub( stateB->deltaPosition, stateA->deltaPosition ), joint->deltaCenter ), b3Sub( rB, rA ) ); + b3Vec3 sAx = b3Cross( b3Add( d, rA ), matrixA.cx ); + b3Vec3 sBx = b3Cross( rB, matrixA.cx ); + b3Vec3 sAy = b3Cross( b3Add( d, rA ), matrixA.cy ); + b3Vec3 sBy = b3Cross( rB, matrixA.cy ); + b3Vec3 sAz = b3Cross( b3Add( d, rA ), matrixA.cz ); + b3Vec3 sBz = b3Cross( rB, matrixA.cz ); + + float translation = b3Dot( matrixA.cx, d ); + + // Steering param ib = cz_b, ia = cz_a, ja = -cy_a + float cs = b3Dot( matrixB.cz, matrixA.cz ); + float ss = -b3Dot( matrixB.cz, matrixA.cy ); + float den = cs * cs + ss * ss; + den = den > 0.0f ? 1.0f / den : 0.0f; + b3Vec3 steeringAxis = + b3MulSV( den, b3Cross( matrixB.cz, b3Sub( b3MulSV( -cs, matrixA.cy ), b3MulSV( ss, matrixA.cz ) ) ) ); + + // motor constraint + if ( joint->enableSpinMotor && fixedRotation == false ) + { + b3Vec3 spinAxis = matrixB.cz; + float cdot = b3Dot( b3Sub( wB, wA ), spinAxis ) - joint->spinSpeed; + float impulse = -joint->spinMass * cdot; + float oldImpulse = joint->spinImpulse; + float maxImpulse = context->h * joint->maxSpinTorque; + joint->spinImpulse = b3ClampFloat( joint->spinImpulse + impulse, -maxImpulse, maxImpulse ); + impulse = joint->spinImpulse - oldImpulse; + + wA = b3Sub( wA, b3MulMV( iA, b3MulSV( impulse, spinAxis ) ) ); + wB = b3Add( wB, b3MulMV( iB, b3MulSV( impulse, spinAxis ) ) ); + } + + // suspension + if ( joint->enableSuspensionSpring ) + { + // This is a real spring and should be applied even during relax + float c = translation; + float bias = joint->suspensionSoftness.biasRate * c; + float massScale = joint->suspensionSoftness.massScale; + float impulseScale = joint->suspensionSoftness.impulseScale; + + float cdot = b3Dot( matrixA.cx, b3Sub( vB, vA ) ) + b3Dot( sBx, wB ) - b3Dot( sAx, wA ); + float impulse = -massScale * joint->suspensionMass * ( cdot + bias ) - impulseScale * joint->suspensionSpringImpulse; + joint->suspensionSpringImpulse += impulse; + + b3Vec3 linearImpulse = b3MulSV( impulse, matrixA.cx ); + b3Vec3 angularImpulseA = b3MulSV( impulse, sAx ); + b3Vec3 angularImpulseB = b3MulSV( impulse, sBx ); + + vA = b3MulSub( vA, mA, linearImpulse ); + wA = b3Sub( wA, b3MulMV( iA, angularImpulseA ) ); + vB = b3MulAdd( vB, mB, linearImpulse ); + wB = b3Add( wB, b3MulMV( iB, angularImpulseB ) ); + } + + // steering + if ( joint->enableSteering && fixedRotation == false ) + { + float steeringAngle = b3Atan2( ss, cs ); + + { + // This is a real spring and should be applied even during relax + float c = steeringAngle - joint->targetSteeringAngle; + float bias = joint->steeringSoftness.biasRate * c; + float massScale = joint->steeringSoftness.massScale; + float impulseScale = joint->steeringSoftness.impulseScale; + + float cdot = b3Dot( steeringAxis, b3Sub( wB, wA ) ); + float oldImpulse = joint->steeringSpringImpulse; + float impulse = -massScale * joint->steeringMass * ( cdot + bias ) - impulseScale * oldImpulse; + float maxImpulse = context->h * joint->maxSteeringTorque; + joint->steeringSpringImpulse = b3ClampFloat( oldImpulse + impulse, -maxImpulse, maxImpulse ); + impulse = joint->steeringSpringImpulse - oldImpulse; + + wA = b3Sub( wA, b3MulMV( iA, b3MulSV( impulse, steeringAxis ) ) ); + wB = b3Add( wB, b3MulMV( iB, b3MulSV( impulse, steeringAxis ) ) ); + } + + if ( joint->enableSteeringLimit ) + { + // Lower limit + { + float c = steeringAngle - joint->lowerSteeringLimit; + 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( steeringAxis, b3Sub( wB, wA ) ); + float oldImpulse = joint->lowerSteeringImpulse; + float impulse = -massScale * joint->steeringMass * ( cdot + bias ) - impulseScale * oldImpulse; + joint->lowerSteeringImpulse = b3MaxFloat( oldImpulse + impulse, 0.0f ); + impulse = joint->lowerSteeringImpulse - oldImpulse; + + wA = b3Sub( wA, b3MulMV( iA, b3MulSV( impulse, steeringAxis ) ) ); + wB = b3Add( wB, b3MulMV( iB, b3MulSV( impulse, steeringAxis ) ) ); + } + + // Upper limit + // Note: signs are flipped to keep c positive when the constraint is satisfied. + // This also keeps the impulse positive when the limit is active. + { + // sign flipped + float c = joint->upperSteeringLimit - steeringAngle; + 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( steeringAxis, b3Sub( wA, wB ) ); + float oldImpulse = joint->upperSteeringImpulse; + float impulse = -massScale * joint->steeringMass * ( cdot + bias ) - impulseScale * oldImpulse; + joint->upperSteeringImpulse = b3MaxFloat( oldImpulse + impulse, 0.0f ); + impulse = joint->upperSteeringImpulse - oldImpulse; + + // sign flipped on applied impulse + wA = b3Add( wA, b3MulMV( iA, b3MulSV( impulse, steeringAxis ) ) ); + wB = b3Sub( wB, b3MulMV( iB, b3MulSV( impulse, steeringAxis ) ) ); + } + } + } + + if ( joint->enableSuspensionLimit ) + { + // Lower limit + { + float c = translation - joint->lowerSuspensionLimit; + 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( matrixA.cx, b3Sub( vB, vA ) ) + b3Dot( sBx, wB ) - b3Dot( sAx, wA ); + float impulse = -massScale * joint->suspensionMass * ( cdot + bias ) - impulseScale * joint->lowerSuspensionImpulse; + float oldImpulse = joint->lowerSuspensionImpulse; + joint->lowerSuspensionImpulse = b3MaxFloat( oldImpulse + impulse, 0.0f ); + impulse = joint->lowerSuspensionImpulse - oldImpulse; + + b3Vec3 linearImpulse = b3MulSV( impulse, matrixA.cx ); + b3Vec3 angularImpulseA = b3MulSV( impulse, sAx ); + b3Vec3 angularImpulseB = b3MulSV( impulse, sBx ); + + vA = b3MulSub( vA, mA, linearImpulse ); + wA = b3Sub( wA, b3MulMV( iA, angularImpulseA ) ); + vB = b3MulAdd( vB, mB, linearImpulse ); + wB = b3Add( wB, b3MulMV( iB, angularImpulseB ) ); + } + + // Upper limit + // Note: signs are flipped to keep c positive when the constraint is satisfied. + // This also keeps the impulse positive when the limit is active. + { + // sign flipped + float c = joint->upperSuspensionLimit - translation; + float bias = 0.0f; + float massScale = 1.0f; + float impulseScale = 0.0f; + + if ( c > 0.0f ) + { + // speculation + bias = c * context->inv_h; + } + else if ( useBias ) + { + bias = base->constraintSoftness.biasRate * c; + massScale = base->constraintSoftness.massScale; + impulseScale = base->constraintSoftness.impulseScale; + } + + // sign flipped on cdot + float cdot = b3Dot( matrixA.cx, b3Sub( vA, vB ) ) + b3Dot( sAx, wA ) - b3Dot( sBx, wB ); + float impulse = -massScale * joint->suspensionMass * ( cdot + bias ) - impulseScale * joint->upperSuspensionImpulse; + float oldImpulse = joint->upperSuspensionImpulse; + joint->upperSuspensionImpulse = b3MaxFloat( oldImpulse + impulse, 0.0f ); + impulse = joint->upperSuspensionImpulse - oldImpulse; + + b3Vec3 linearImpulse = b3MulSV( impulse, matrixA.cx ); + b3Vec3 angularImpulseA = b3MulSV( impulse, sAx ); + b3Vec3 angularImpulseB = b3MulSV( impulse, sBx ); + + // sign flipped on applied impulse + vA = b3MulAdd( vA, mA, linearImpulse ); + wA = b3Add( wA, b3MulMV( iA, angularImpulseA ) ); + vB = b3MulSub( vB, mB, linearImpulse ); + wB = b3Sub( wB, b3MulMV( iB, angularImpulseB ) ); + } + } + + // Collinearity constraint + if ( fixedRotation == false ) + { + if ( joint->enableSteering == true ) + { + float bias = 0.0f; + float massScale = 1.0f; + float impulseScale = 0.0f; + if ( useBias ) + { + float c = b3Dot( matrixA.cx, matrixB.cz ); + + bias = base->constraintSoftness.biasRate * c; + massScale = base->constraintSoftness.massScale; + impulseScale = base->constraintSoftness.impulseScale; + } + + b3Vec3 u = b3Cross( matrixB.cz, matrixA.cx ); + float cdot = b3Dot( b3Sub( wB, wA ), u ); + + b3Matrix3 invInertiaSum = b3AddMM( iA, iB ); + float k = b3Dot( u, b3MulMV( invInertiaSum, u ) ); + float perpMass = k > 0.0f ? 1.0f / k : 0.0f; + + float deltaImpulse = -massScale * perpMass * ( cdot + bias ) - impulseScale * joint->angularImpulse.x; + joint->angularImpulse.x += deltaImpulse; + + wA = b3MulSub( wA, deltaImpulse, b3MulMV( iA, u ) ); + wB = b3MulAdd( wB, deltaImpulse, b3MulMV( iB, u ) ); + } + else + { + 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 = (b3Vec2){ base->constraintSoftness.biasRate * c.x, base->constraintSoftness.biasRate * c.y }; + 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 ) ) ) ); + + 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->angularImpulse; + 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->angularImpulse = (b3Vec2){ oldImpulse.x + deltaImpulse.x, oldImpulse.y + deltaImpulse.y }; + + b3Vec3 angularImpulse = b3Blend2( deltaImpulse.x, perpAxisX, deltaImpulse.y, perpAxisY ); + wA = b3Sub( wA, b3MulMV( iA, angularImpulse ) ); + wB = b3Add( wB, b3MulMV( iB, angularImpulse ) ); + } + } + + // Solve point-to-line constraint + { + b3Vec3 perpY = matrixA.cy; + b3Vec3 perpZ = matrixA.cz; + + 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 = (b3Vec2){ base->constraintSoftness.biasRate * c.x, base->constraintSoftness.biasRate * c.y }; + 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/m1 + 1/m2) * eye(2) - skew(r1) * invI1 * skew(r1) - skew(r2) * invI2 * skew(r2)] + ///// Jx = [-perpX, -cross(d + rA, perpX), perpX, cross(rB, perpX)] + + 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->linearImpulse; + 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->linearImpulse = (b3Vec2){ oldImpulse.x + deltaImpulse.x, oldImpulse.y + deltaImpulse.y }; + + b3Vec3 linearImpulse = b3Blend2( deltaImpulse.x, perpY, deltaImpulse.y, perpZ ); + + vA = b3MulSub( vA, mA, linearImpulse ); + wA = b3Sub( wA, b3MulMV( iA, b3Blend2( deltaImpulse.x, sAy, deltaImpulse.y, sAz ) ) ); + vB = b3MulAdd( vB, mB, linearImpulse ); + wB = b3Add( wB, b3MulMV( iB, b3Blend2( deltaImpulse.x, sBy, deltaImpulse.y, sBz ) ) ); + } + + if ( stateA->flags & b3_dynamicFlag ) + { + stateA->linearVelocity = vA; + stateA->angularVelocity = wA; + } + + if ( stateB->flags & b3_dynamicFlag ) + { + stateB->linearVelocity = vB; + stateB->angularVelocity = wB; + } +} + +#if 0 +void b3WheelJoint_Dump() +{ + int32 indexA = joint->bodyA->joint->islandIndex; + int32 indexB = joint->bodyB->joint->islandIndex; + + b3Dump(" b3WheelJointDef jd;\n"); + b3Dump(" jd.bodyA = sims[%d];\n", indexA); + b3Dump(" jd.bodyB = sims[%d];\n", indexB); + b3Dump(" jd.collideConnected = bool(%d);\n", joint->collideConnected); + b3Dump(" jd.localAnchorA.Set(%.9g, %.9g);\n", joint->localAnchorA.x, joint->localAnchorA.y); + b3Dump(" jd.localAnchorB.Set(%.9g, %.9g);\n", joint->localAnchorB.x, joint->localAnchorB.y); + b3Dump(" jd.referenceAngle = %.9g;\n", joint->referenceAngle); + b3Dump(" jd.enableLimit = bool(%d);\n", joint->enableLimit); + b3Dump(" jd.lowerAngle = %.9g;\n", joint->lowerAngle); + b3Dump(" jd.upperAngle = %.9g;\n", joint->upperAngle); + b3Dump(" jd.enableMotor = bool(%d);\n", joint->enableMotor); + b3Dump(" jd.motorSpeed = %.9g;\n", joint->motorSpeed); + b3Dump(" jd.maxMotorTorque = %.9g;\n", joint->maxMotorTorque); + b3Dump(" joints[%d] = joint->world->CreateJoint(&jd);\n", joint->index); +} +#endif + +void b3DrawWheelJoint( b3DebugDraw* draw, b3JointSim* base, b3WorldTransform transformA, b3WorldTransform transformB, float scale ) +{ + B3_ASSERT( base->type == b3_wheelJoint ); + + b3WheelJoint* joint = &base->wheelJoint; + + b3WorldTransform frameA = b3MulWorldTransforms( transformA, base->localFrameA ); + b3WorldTransform frameB = b3MulWorldTransforms( transformB, base->localFrameB ); + + b3Matrix3 matrixA = b3MakeMatrixFromQuat( frameA.q ); + b3Matrix3 matrixB = b3MakeMatrixFromQuat( frameB.q ); + + draw->DrawSegmentFcn( frameA.p, frameB.p, b3_colorBlue, draw->context ); + + if ( joint->enableSuspensionLimit ) + { + b3Pos lower = b3OffsetPos( frameA.p, b3MulSV( joint->lowerSuspensionLimit, matrixA.cx ) ); + b3Pos upper = b3OffsetPos( frameA.p, b3MulSV( joint->upperSuspensionLimit, matrixA.cx ) ); + b3Vec3 perp = matrixA.cy; + draw->DrawSegmentFcn( lower, upper, b3_colorGray, draw->context ); + draw->DrawSegmentFcn( b3OffsetPos( lower, b3MulSV( -0.1f * scale, perp ) ), b3OffsetPos( lower, b3MulSV( 0.1f * scale, perp ) ), + b3_colorGreen, draw->context ); + draw->DrawSegmentFcn( b3OffsetPos( upper, b3MulSV( -0.1f * scale, perp ) ), b3OffsetPos( upper, b3MulSV( 0.1f * scale, perp ) ), + b3_colorRed, draw->context ); + } + else + { + draw->DrawSegmentFcn( b3OffsetPos( frameA.p, b3MulSV( -1.0f * scale, matrixA.cx ) ), + b3OffsetPos( frameA.p, b3MulSV( 1.0f * scale, matrixA.cx ) ), b3_colorGray, draw->context ); + } + + if ( joint->enableSteering && joint->enableSteeringLimit ) + { + // 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 = -quatB; + // } + + // b3Quat relQ = b3InvMulQuat( quatA, quatB ); + + b3WorldTransform frame = { + .p = frameB.p, + .q = frameA.q, + }; + + const float radius = 0.5f * scale; + const int sliceCount = 16; + float lower = joint->lowerSteeringLimit; + float upper = joint->upperSteeringLimit; + + b3CosSin cs = b3ComputeCosSin( lower ); + b3Pos vertex1 = b3TransformWorldPoint( frame, (b3Vec3){ 0.0f, -radius * cs.sine, radius * cs.cosine } ); + + for ( int index = 0; index < sliceCount; ++index ) + { + float t2 = ( index + 1.0f ) / sliceCount; + float phi = b3LerpFloat( lower, upper, t2 ); + + cs = b3ComputeCosSin( phi ); + b3Pos vertex2 = b3TransformWorldPoint( frame, (b3Vec3){ 0.0f, -radius * cs.sine, radius * cs.cosine } ); + + if ( index == 0 ) + { + draw->DrawSegmentFcn( frame.p, vertex1, b3_colorCyan, draw->context ); + } + + if ( index == sliceCount - 1 ) + { + draw->DrawSegmentFcn( vertex2, frame.p, b3_colorCyan, draw->context ); + } + draw->DrawSegmentFcn( vertex1, vertex2, b3_colorCyan, draw->context ); + + vertex1 = vertex2; + } + } + + draw->DrawSegmentFcn( b3OffsetPos( frameB.p, b3MulSV( -0.5f * scale, matrixB.cz ) ), + b3OffsetPos( frameB.p, b3MulSV( 0.5f * scale, matrixB.cz ) ), b3_colorMagenta, draw->context ); + + draw->DrawPointFcn( frameA.p, 5.0f, b3_colorGray, draw->context ); + draw->DrawPointFcn( frameB.p, 5.0f, b3_colorDimGray, draw->context ); +} diff --git a/vendor/box3d/src/src/world_snapshot.c b/vendor/box3d/src/src/world_snapshot.c new file mode 100644 index 000000000..9efaee9fd --- /dev/null +++ b/vendor/box3d/src/src/world_snapshot.c @@ -0,0 +1,1335 @@ +// SPDX-FileCopyrightText: 2026 Erin Catto +// SPDX-License-Identifier: MIT + +#if defined( _MSC_VER ) && !defined( _CRT_SECURE_NO_WARNINGS ) +#define _CRT_SECURE_NO_WARNINGS +#endif + +#include "world_snapshot.h" + +#include "bitset.h" +#include "body.h" +#include "broad_phase.h" +#include "compound.h" +#include "constraint_graph.h" +#include "contact.h" +#include "container.h" +#include "core.h" +#include "id_pool.h" +#include "island.h" +#include "joint.h" +#include "physics_world.h" +#include "recording.h" +#include "sensor.h" +#include "shape.h" +#include "solver_set.h" +#include "table.h" + +#include "box3d/box3d.h" +#include "box3d/collision.h" +#include "box3d/types.h" + +#include + +// Snapshot image magic 'BNS3' and version +#define B3_SNAP_MAGIC 0x33534E42u +#define B3_SNAP_VERSION 2u + +#define B3_SNAP_FLAG_VALIDATION 0x1u +#define B3_SNAP_FLAG_DOUBLE_PRECISION 0x2u + +// Layout hash over all POD-copied structs + key constants. +// Changing a struct size updates this, catching ABI drift early. +static uint32_t b3ComputeLayoutHash( void ) +{ + uint32_t h = 2166136261u; +#define MIX( x ) \ + h ^= (uint32_t)( x ); \ + h *= 16777619u; + MIX( sizeof( b3Body ) ) + MIX( sizeof( b3BodySim ) ) + MIX( sizeof( b3BodyState ) ) + MIX( sizeof( b3Shape ) ) + MIX( sizeof( b3Contact ) ) + MIX( sizeof( b3Manifold ) ) + MIX( sizeof( b3Joint ) ) + MIX( sizeof( b3JointSim ) ) + MIX( sizeof( b3Island ) ) + MIX( sizeof( b3IslandSim ) ) + MIX( sizeof( b3ContactLink ) ) + MIX( sizeof( b3JointLink ) ) + MIX( sizeof( b3Sensor ) ) + MIX( sizeof( b3Visitor ) ) + MIX( sizeof( b3SolverSet ) ) + MIX( sizeof( b3GraphColor ) ) + MIX( sizeof( b3DynamicTree ) ) + MIX( sizeof( b3TreeNode ) ) + MIX( sizeof( b3SetItem ) ) + MIX( sizeof( b3IdPool ) ) + MIX( sizeof( b3SurfaceMaterial ) ) + MIX( sizeof( b3ContactSpec ) ) + MIX( sizeof( b3TriangleCache ) ) + MIX( B3_GRAPH_COLOR_COUNT ) + MIX( b3_bodyTypeCount ) + MIX( sizeof( void* ) ) +#undef MIX + return h; +} + +typedef struct b3SnapHeader +{ + uint32_t magic; + uint32_t version; + uint32_t layoutHash; + uint32_t flags; +} b3SnapHeader; + +// Bounds-checked read cursor +typedef struct b3SnapReader +{ + const uint8_t* data; + int cursor; + int size; + bool ok; +} b3SnapReader; + +static void b3SnapRCheck( b3SnapReader* r, int need ) +{ + if ( need < 0 || (int64_t)r->cursor + (int64_t)need > (int64_t)r->size ) + { + r->ok = false; + } +} + +static void b3SnapR_Bytes( b3SnapReader* r, void* dst, int n ) +{ + b3SnapRCheck( r, n ); + if ( !r->ok ) + { + return; + } + memcpy( dst, r->data + r->cursor, n ); + r->cursor += n; +} + +static int b3SnapR_I32( b3SnapReader* r ) +{ + int32_t v = 0; + b3SnapR_Bytes( r, &v, 4 ); + return (int)v; +} + +static uint32_t b3SnapR_U32( b3SnapReader* r ) +{ + uint32_t v = 0; + b3SnapR_Bytes( r, &v, 4 ); + return v; +} + +static void b3SnapW_I32( b3RecBuffer* buf, int v ) +{ + int32_t w = (int32_t)v; + b3RecBufAppend( buf, &w, 4 ); +} + +static void b3SnapW_U32( b3RecBuffer* buf, uint32_t v ) +{ + b3RecBufAppend( buf, &v, 4 ); +} + +static void b3SnapW_Bytes( b3RecBuffer* buf, const void* src, int n ) +{ + b3RecBufAppend( buf, src, n ); +} + +// Bounds check before allocating from image +static bool b3SnapCheckCount( const b3SnapReader* r, int count, int memSize, int minStreamBytes ) +{ + if ( count < 0 || memSize < 0 || minStreamBytes < 0 ) + { + return false; + } + if ( memSize > 0 && count > 0x7FFFFFFF / memSize ) + { + return false; + } + int64_t remaining = (int64_t)r->size - (int64_t)r->cursor; + return (int64_t)count * (int64_t)minStreamBytes <= remaining; +} + +// POD array: count + raw bytes +#define b3SerPodArray( buf, arr ) \ + do \ + { \ + b3SnapW_I32( buf, ( arr ).count ); \ + if ( ( arr ).count > 0 ) \ + { \ + b3SnapW_Bytes( buf, ( arr ).data, ( arr ).count * (int)sizeof( *( arr ).data ) ); \ + } \ + } \ + while ( 0 ) + +#define b3DesPodArray( r, arr ) \ + do \ + { \ + int cnt = b3SnapR_I32( r ); \ + int elemSize = (int)sizeof( *( arr ).data ); \ + if ( ( r )->ok && b3SnapCheckCount( r, cnt, elemSize, elemSize ) == false ) \ + { \ + ( r )->ok = false; \ + } \ + if ( ( r )->ok && cnt > 0 ) \ + { \ + b3Array_Resize( arr, cnt ); \ + b3SnapR_Bytes( r, ( arr ).data, cnt * elemSize ); \ + } \ + else if ( ( r )->ok ) \ + { \ + ( arr ).count = 0; \ + } \ + } \ + while ( 0 ) + +// Id pool: nextIndex + freeArray +static void b3SerIdPool( b3RecBuffer* buf, const b3IdPool* pool ) +{ + b3SnapW_I32( buf, pool->nextIndex ); + b3SerPodArray( buf, pool->freeArray ); +} + +static void b3DesIdPool( b3SnapReader* r, b3IdPool* pool ) +{ + pool->nextIndex = b3SnapR_I32( r ); + b3DesPodArray( r, pool->freeArray ); +} + +// BitSet: blockCount + raw words +static void b3SerBitSet( b3RecBuffer* buf, const b3BitSet* bs ) +{ + b3SnapW_U32( buf, bs->blockCount ); + if ( bs->blockCount > 0 ) + { + b3SnapW_Bytes( buf, bs->bits, (int)( bs->blockCount * sizeof( uint64_t ) ) ); + } +} + +static void b3DesBitSet( b3SnapReader* r, b3BitSet* bs ) +{ + uint32_t blockCount = b3SnapR_U32( r ); + if ( r->ok && b3SnapCheckCount( r, (int)blockCount, (int)sizeof( uint64_t ), (int)sizeof( uint64_t ) ) == false ) + { + r->ok = false; + } + b3DestroyBitSet( bs ); + if ( !r->ok ) + { + return; + } + uint32_t blockCapacity = blockCount > 0 ? blockCount : 1; + bs->bits = (uint64_t*)b3Alloc( blockCapacity * sizeof( uint64_t ) ); + memset( bs->bits, 0, blockCapacity * sizeof( uint64_t ) ); + bs->blockCapacity = blockCapacity; + bs->blockCount = blockCount; + if ( blockCount > 0 ) + { + b3SnapR_Bytes( r, bs->bits, (int)( blockCount * sizeof( uint64_t ) ) ); + } +} + +// HashSet: capacity + count + raw items (probe order depends on layout) +static void b3SerHashSet( b3RecBuffer* buf, const b3HashSet* hs ) +{ + b3SnapW_U32( buf, hs->capacity ); + b3SnapW_U32( buf, hs->count ); + if ( hs->capacity > 0 ) + { + b3SnapW_Bytes( buf, hs->items, (int)( hs->capacity * sizeof( b3SetItem ) ) ); + } +} + +static void b3DesHashSet( b3SnapReader* r, b3HashSet* hs ) +{ + uint32_t cap = b3SnapR_U32( r ); + uint32_t cnt = b3SnapR_U32( r ); + bool valid = b3SnapCheckCount( r, (int)cap, (int)sizeof( b3SetItem ), (int)sizeof( b3SetItem ) ) && + ( cap & ( cap - 1 ) ) == 0 && cnt <= cap; + if ( r->ok && valid == false && ( cap != 0 || cnt != 0 ) ) + { + r->ok = false; + } + b3DestroySet( hs ); + if ( !r->ok ) + { + return; + } + if ( cap > 0 ) + { + hs->items = (b3SetItem*)b3Alloc( cap * sizeof( b3SetItem ) ); + hs->capacity = cap; + hs->count = cnt; + b3SnapR_Bytes( r, hs->items, (int)( cap * sizeof( b3SetItem ) ) ); + } + else + { + hs->items = NULL; + hs->capacity = 0; + hs->count = 0; + } +} + +// DynamicTree: version, scalars, full nodeCapacity nodes (rebuild scratch excluded) +static void b3SerTree( b3RecBuffer* buf, const b3DynamicTree* tree ) +{ + b3SnapW_Bytes( buf, &tree->version, sizeof( uint64_t ) ); + b3SnapW_I32( buf, tree->root ); + b3SnapW_I32( buf, tree->nodeCount ); + b3SnapW_I32( buf, tree->nodeCapacity ); + b3SnapW_I32( buf, tree->freeList ); + b3SnapW_I32( buf, tree->proxyCount ); + if ( tree->nodeCapacity > 0 ) + { + b3SnapW_Bytes( buf, tree->nodes, tree->nodeCapacity * (int)sizeof( b3TreeNode ) ); + } +} + +static void b3DesTree( b3SnapReader* r, b3DynamicTree* tree ) +{ + uint64_t version; + b3SnapR_Bytes( r, &version, sizeof( uint64_t ) ); + int root = b3SnapR_I32( r ); + int nodeCount = b3SnapR_I32( r ); + int nodeCapacity = b3SnapR_I32( r ); + int freeList = b3SnapR_I32( r ); + int proxyCount = b3SnapR_I32( r ); + + if ( r->ok && b3SnapCheckCount( r, nodeCapacity, (int)sizeof( b3TreeNode ), (int)sizeof( b3TreeNode ) ) == false ) + { + r->ok = false; + } + + // Free existing allocation including any rebuild scratch + b3Free( tree->nodes, tree->nodeCapacity * (int)sizeof( b3TreeNode ) ); + b3Free( tree->leafIndices, tree->rebuildCapacity * (int)sizeof( int ) ); + b3Free( tree->leafBoxes, tree->rebuildCapacity * (int)sizeof( b3AABB ) ); + b3Free( tree->leafCenters, tree->rebuildCapacity * (int)sizeof( b3Vec3 ) ); + b3Free( tree->binIndices, tree->rebuildCapacity * (int)sizeof( int ) ); + tree->nodes = NULL; + tree->leafIndices = NULL; + tree->leafBoxes = NULL; + tree->leafCenters = NULL; + tree->binIndices = NULL; + tree->nodeCapacity = 0; + tree->rebuildCapacity = 0; + + if ( !r->ok ) + { + return; + } + + tree->version = version; + tree->root = root; + tree->nodeCount = nodeCount; + tree->nodeCapacity = nodeCapacity; + tree->freeList = freeList; + tree->proxyCount = proxyCount; + + if ( nodeCapacity > 0 ) + { + tree->nodes = (b3TreeNode*)b3Alloc( nodeCapacity * (int)sizeof( b3TreeNode ) ); + b3SnapR_Bytes( r, tree->nodes, nodeCapacity * (int)sizeof( b3TreeNode ) ); + } +} + +// Solver set: setIndex + 4 arrays (note: contactIndices is int array, not contactSims) +static void b3SerSolverSet( b3RecBuffer* buf, const b3SolverSet* set ) +{ + b3SnapW_I32( buf, set->setIndex ); + b3SerPodArray( buf, set->bodySims ); + b3SerPodArray( buf, set->bodyStates ); + b3SerPodArray( buf, set->jointSims ); + b3SerPodArray( buf, set->contactIndices ); + b3SerPodArray( buf, set->islandSims ); +} + +static void b3DesSolverSet( b3SnapReader* r, b3SolverSet* set ) +{ + set->setIndex = b3SnapR_I32( r ); + b3DesPodArray( r, set->bodySims ); + b3DesPodArray( r, set->bodyStates ); + b3DesPodArray( r, set->jointSims ); + b3DesPodArray( r, set->contactIndices ); + b3DesPodArray( r, set->islandSims ); +} + +static void b3SerNames( b3RecBuffer* buf, const b3NameCache* cache ) +{ + b3SnapW_I32( buf, cache->entries.count ); + int count = cache->entries.count; + for ( int i = 0; i < count; ++i ) + { + const b3NameEntry* entry = cache->entries.data + i; + b3SnapW_U32( buf, entry->hash ); + b3SnapW_I32( buf, entry->length ); + b3RecBufAppend( buf, entry->name, entry->length ); + } +} + +static void b3DesNames( b3SnapReader* r, b3NameCache* cache ) +{ + int count = b3SnapR_I32( r ); + + if ( r->ok && b3SnapCheckCount( r, count, (int)sizeof( b3NameEntry ), 8 ) == false ) + { + r->ok = false; + } + + if ( r->ok == false ) + { + return; + } + + b3Array_Reserve( cache->entries, count ); + for ( int i = 0; i < count; ++i ) + { + uint32_t hash = b3SnapR_U32( r ); + int length = b3SnapR_I32( r ); + if ( r->ok == false || length < 0 || length > r->size - r->cursor ) + { + r->ok = false; + return; + } + char* name = b3Alloc( length + 1 ); + b3SnapR_Bytes( r, name, length ); + name[length] = 0; + b3LoadName( cache, hash, name, length ); + } +} + +// Graph color: bodySet (non-overflow only) + jointSims + convexContacts + contacts +static void b3SerGraphColor( b3RecBuffer* buf, const b3GraphColor* color, bool isOverflow ) +{ + if ( !isOverflow ) + { + b3SerBitSet( buf, &color->bodySet ); + } + b3SerPodArray( buf, color->jointSims ); + b3SerPodArray( buf, color->convexContacts ); + b3SerPodArray( buf, color->contacts ); + // wideConstraints / manifoldConstraints / contactConstraints are transient, not serialized +} + +static void b3DesGraphColor( b3SnapReader* r, b3GraphColor* color, bool isOverflow ) +{ + if ( !isOverflow ) + { + b3DesBitSet( r, &color->bodySet ); + } + b3DesPodArray( r, color->jointSims ); + b3DesPodArray( r, color->convexContacts ); + b3DesPodArray( r, color->contacts ); + // Transient pointers left at NULL/0 from shell +} + +// World simulation scalars (never host/callback/worker state) +static void b3SerWorldConfig( b3RecBuffer* buf, const b3World* world ) +{ + b3SnapW_Bytes( buf, &world->gravity, sizeof( b3Vec3 ) ); + b3SnapW_Bytes( buf, &world->hitEventThreshold, sizeof( float ) ); + b3SnapW_Bytes( buf, &world->restitutionThreshold, sizeof( float ) ); + b3SnapW_Bytes( buf, &world->maxLinearSpeed, sizeof( float ) ); + b3SnapW_Bytes( buf, &world->contactSpeed, sizeof( float ) ); + b3SnapW_Bytes( buf, &world->contactHertz, sizeof( float ) ); + b3SnapW_Bytes( buf, &world->contactDampingRatio, sizeof( float ) ); + b3SnapW_Bytes( buf, &world->contactRecycleDistance, sizeof( float ) ); + b3SnapW_Bytes( buf, &world->stepIndex, sizeof( uint64_t ) ); + b3SnapW_I32( buf, world->splitIslandId ); + b3SnapW_Bytes( buf, &world->inv_h, sizeof( float ) ); + b3SnapW_Bytes( buf, &world->inv_dt, sizeof( float ) ); + b3SnapW_I32( buf, world->endEventArrayIndex ); + b3SnapW_Bytes( buf, &world->maxCapacity, sizeof( b3Capacity ) ); + uint8_t flags = 0; + flags |= world->enableSleep ? 0x01u : 0u; + flags |= world->enableWarmStarting ? 0x02u : 0u; + flags |= world->enableContinuous ? 0x04u : 0u; + flags |= world->enableSpeculative ? 0x08u : 0u; + b3RecBufAppend( buf, &flags, 1 ); +} + +static void b3DesWorldConfig( b3SnapReader* r, b3World* world ) +{ + b3SnapR_Bytes( r, &world->gravity, sizeof( b3Vec3 ) ); + b3SnapR_Bytes( r, &world->hitEventThreshold, sizeof( float ) ); + b3SnapR_Bytes( r, &world->restitutionThreshold, sizeof( float ) ); + b3SnapR_Bytes( r, &world->maxLinearSpeed, sizeof( float ) ); + b3SnapR_Bytes( r, &world->contactSpeed, sizeof( float ) ); + b3SnapR_Bytes( r, &world->contactHertz, sizeof( float ) ); + b3SnapR_Bytes( r, &world->contactDampingRatio, sizeof( float ) ); + b3SnapR_Bytes( r, &world->contactRecycleDistance, sizeof( float ) ); + b3SnapR_Bytes( r, &world->stepIndex, sizeof( uint64_t ) ); + world->splitIslandId = b3SnapR_I32( r ); + b3SnapR_Bytes( r, &world->inv_h, sizeof( float ) ); + b3SnapR_Bytes( r, &world->inv_dt, sizeof( float ) ); + world->endEventArrayIndex = b3SnapR_I32( r ); + b3SnapR_Bytes( r, &world->maxCapacity, sizeof( b3Capacity ) ); + uint8_t flags = 0; + b3SnapR_Bytes( r, &flags, 1 ); + world->enableSleep = ( flags & 0x01u ) != 0; + world->enableWarmStarting = ( flags & 0x02u ) != 0; + world->enableContinuous = ( flags & 0x04u ) != 0; + world->enableSpeculative = ( flags & 0x08u ) != 0; +} + +// Shapes carry pointer fields: materials, userData, userShape, and the geometry union. +// Serialize the POD scalars with pointers nulled, then the owned materials array, then geometry. +// A single material lives inline in the struct image. +// Hull/mesh/heightField/compound are interned into the recording registry; sphere/capsule inline. +static void b3SerShapes( b3RecBuffer* buf, b3World* world, b3Recording* rec ) +{ + int count = world->shapes.count; + b3SnapW_I32( buf, count ); + + for ( int i = 0; i < count; ++i ) + { + b3Shape shape = world->shapes.data[i]; + bool isLive = ( shape.id == i ); + + // Null out pointer fields before writing the raw struct + shape.materials = NULL; + shape.userData = NULL; + shape.userShape = NULL; + // Zero the geometry union so free-slot images have deterministic bytes + if ( !isLive ) + { + memset( &shape.capsule, 0, sizeof( shape.capsule ) ); + } + b3SnapW_Bytes( buf, &shape, sizeof( b3Shape ) ); + + if ( !isLive ) + { + // Free slot: no materials or geometry + b3SnapW_I32( buf, 0 ); // materialCount + b3SnapW_I32( buf, -1 ); // geometry kind sentinel + continue; + } + + // Owned material array. Only multi material meshes and compounds have one. A single material + // already rode along inline in the struct image, so write a zero length for it. + const b3Shape* src = world->shapes.data + i; + if ( src->materials != NULL ) + { + b3SnapW_I32( buf, src->materialCount ); + b3SnapW_Bytes( buf, src->materials, src->materialCount * (int)sizeof( b3SurfaceMaterial ) ); + } + else + { + b3SnapW_I32( buf, 0 ); + } + + // Geometry + switch ( src->type ) + { + case b3_sphereShape: + b3SnapW_I32( buf, (int)b3_sphereShape ); + b3SnapW_Bytes( buf, &src->sphere, sizeof( b3Sphere ) ); + break; + case b3_capsuleShape: + b3SnapW_I32( buf, (int)b3_capsuleShape ); + b3SnapW_Bytes( buf, &src->capsule, sizeof( b3Capsule ) ); + break; + case b3_hullShape: + { + b3SnapW_I32( buf, (int)b3_hullShape ); + uint32_t gid = b3RecInternHull( rec, src->hull ); + b3SnapW_U32( buf, gid ); + break; + } + case b3_meshShape: + { + b3SnapW_I32( buf, (int)b3_meshShape ); + uint32_t gid = b3RecInternMesh( rec, src->mesh.data ); + b3SnapW_U32( buf, gid ); + b3SnapW_Bytes( buf, &src->mesh.scale, sizeof( b3Vec3 ) ); + break; + } + case b3_heightShape: + { + b3SnapW_I32( buf, (int)b3_heightShape ); + uint32_t gid = b3RecInternHeightField( rec, src->heightField ); + b3SnapW_U32( buf, gid ); + break; + } + case b3_compoundShape: + { + b3SnapW_I32( buf, (int)b3_compoundShape ); + uint32_t gid = b3RecInternCompound( rec, src->compound ); + b3SnapW_U32( buf, gid ); + break; + } + default: + // A live shape must have a known geometry type. Fail loudly rather than emit a shape + // with no geometry that would silently lose its collision on restore. + B3_ASSERT( false ); + b3SnapW_I32( buf, -1 ); + break; + } + } +} + +static void b3DesShapes( b3SnapReader* r, b3World* world, b3RecReader* rdr ) +{ + int count = b3SnapR_I32( r ); + if ( r->ok && b3SnapCheckCount( r, count, (int)sizeof( b3Shape ), (int)sizeof( b3Shape ) ) == false ) + { + r->ok = false; + } + if ( !r->ok ) + { + return; + } + + // Save renderer handles before the array is wiped. A keyframe restore is a deterministic replay + // state, so a live shape that still occupies the same slot with the same generation is the same + // shape with the same geometry. Carrying its handle over avoids tearing down and rebuilding every + // GPU mesh on each seek, which the host (a 3D renderer) would otherwise pay for. Handles that are + // not reclaimed below belong to shapes that are gone or were replaced, and get released so the host + // pool does not leak across seeks. Box2D has no such handles, so its restore skips all of this. + int oldShapeCount = world->shapes.count; + void** savedUserShape = NULL; + uint16_t* savedGeneration = NULL; + if ( oldShapeCount > 0 ) + { + savedUserShape = (void**)b3Alloc( (size_t)oldShapeCount * sizeof( void* ) ); + savedGeneration = (uint16_t*)b3Alloc( (size_t)oldShapeCount * sizeof( uint16_t ) ); + for ( int i = 0; i < oldShapeCount; ++i ) + { + b3Shape* old = world->shapes.data + i; + bool oldLive = ( old->id == i ); + savedUserShape[i] = oldLive ? old->userShape : NULL; + savedGeneration[i] = old->generation; + } + } + + b3Array_Resize( world->shapes, count ); + memset( world->shapes.data, 0, (size_t)count * sizeof( b3Shape ) ); + + for ( int i = 0; i < count && r->ok; ++i ) + { + b3Shape* dst = world->shapes.data + i; + b3SnapR_Bytes( r, dst, sizeof( b3Shape ) ); + // Pointer fields were written as NULL; set them cleanly + dst->materials = NULL; + dst->userData = NULL; + dst->userShape = NULL; + memset( &dst->capsule, 0, sizeof( dst->capsule ) ); + + bool isLive = ( dst->id == i ); + + // Carry the renderer handle over when the same shape still occupies this slot. Consumed + // handles are nulled so the teardown sweep only releases the ones that vanished. + if ( isLive && i < oldShapeCount && savedUserShape != NULL && savedUserShape[i] != NULL && + savedGeneration[i] == dst->generation ) + { + dst->userShape = savedUserShape[i]; + savedUserShape[i] = NULL; + } + + // Serializer writes: matCount, matData, geoKind, geoData + int matCount = b3SnapR_I32( r ); + + if ( !r->ok ) + { + break; + } + + if ( !isLive ) + { + // Free slot: matCount=0, geoKind=-1 + (void)matCount; + b3SnapR_I32( r ); // consume the geoKind sentinel + continue; + } + + // Owned material array (written before geoKind in serializer). A zero length means the single + // material is already inline in the restored struct image, so leave materialCount as restored. + if ( matCount > 0 ) + { + if ( b3SnapCheckCount( r, matCount, (int)sizeof( b3SurfaceMaterial ), (int)sizeof( b3SurfaceMaterial ) ) == false ) + { + r->ok = false; + break; + } + dst->materialCount = matCount; + dst->materials = (b3SurfaceMaterial*)b3Alloc( (size_t)matCount * sizeof( b3SurfaceMaterial ) ); + b3SnapR_Bytes( r, dst->materials, matCount * (int)sizeof( b3SurfaceMaterial ) ); + } + else + { + dst->materials = NULL; + } + + int geoKind = b3SnapR_I32( r ); + + // Geometry + switch ( (b3ShapeType)geoKind ) + { + case b3_sphereShape: + b3SnapR_Bytes( r, &dst->sphere, sizeof( b3Sphere ) ); + break; + case b3_capsuleShape: + b3SnapR_Bytes( r, &dst->capsule, sizeof( b3Capsule ) ); + break; + case b3_hullShape: + { + uint32_t gid = b3SnapR_U32( r ); + if ( !r->ok ) + { + break; + } + if ( rdr == NULL || gid >= (uint32_t)rdr->slotCount ) + { + r->ok = false; + break; + } + // Hull is cloned into the world DB; pass raw bytes directly + b3RegistrySlot* slot = rdr->slots + gid; + dst->hull = b3AddHullToDatabase( world, (const b3HullData*)slot->bytes ); + break; + } + case b3_meshShape: + { + uint32_t gid = b3SnapR_U32( r ); + b3Vec3 scale; + b3SnapR_Bytes( r, &scale, sizeof( b3Vec3 ) ); + if ( !r->ok ) + { + break; + } + if ( rdr == NULL || gid >= (uint32_t)rdr->slotCount ) + { + r->ok = false; + break; + } + b3RegistrySlot* slot = rdr->slots + gid; + // Mesh is a self-contained blob used by reference; point straight at the pristine bytes. + dst->mesh.data = (const b3MeshData*)slot->bytes; + dst->mesh.scale = scale; + break; + } + case b3_heightShape: + { + uint32_t gid = b3SnapR_U32( r ); + if ( !r->ok ) + { + break; + } + if ( rdr == NULL || gid >= (uint32_t)rdr->slotCount ) + { + r->ok = false; + break; + } + b3RegistrySlot* slot = rdr->slots + gid; + // Self-contained blob used by reference; point straight at the pristine bytes. + dst->heightField = (const b3HeightFieldData*)slot->bytes; + break; + } + case b3_compoundShape: + { + uint32_t gid = b3SnapR_U32( r ); + if ( !r->ok ) + { + break; + } + if ( rdr == NULL || gid >= (uint32_t)rdr->slotCount ) + { + r->ok = false; + break; + } + b3RegistrySlot* slot = rdr->slots + gid; + if ( slot->live == NULL ) + { + slot->live = b3Alloc( (size_t)slot->byteCount ); + memcpy( slot->live, slot->bytes, (size_t)slot->byteCount ); + b3ConvertBytesToCompound( (uint8_t*)slot->live, slot->byteCount ); + } + dst->compound = (const b3CompoundData*)slot->live; + break; + } + default: + // Unknown geometry kind means a corrupt or unsupported snapshot. Fail the load instead + // of leaving a shape with no geometry. + r->ok = false; + break; + } + } + + // Release handles for shapes that are gone or were replaced this restore, so the host pool and any + // GPU resources they pinned do not leak across seeks. + if ( savedUserShape != NULL ) + { + for ( int i = 0; i < oldShapeCount; ++i ) + { + if ( savedUserShape[i] != NULL && world->destroyDebugShape != NULL ) + { + world->destroyDebugShape( savedUserShape[i], world->userDebugShapeContext ); + } + } + b3Free( savedUserShape, (size_t)oldShapeCount * sizeof( void* ) ); + b3Free( savedGeneration, (size_t)oldShapeCount * sizeof( uint16_t ) ); + } +} + +// Contact serialization. b3Contact is not fully POD: +// - manifolds: heap array of b3Manifold, allocated via b3AllocateManifolds +// - meshContact.triangleCache: heap b3Array, active when b3_simMeshContact flag is set +// Serialize: raw struct (with nulled manifolds/triangleCache), then manifolds, then triangleCache. +static void b3SerContacts( b3RecBuffer* buf, b3World* world ) +{ + int count = world->contacts.count; + b3SnapW_I32( buf, count ); + + for ( int i = 0; i < count; ++i ) + { + const b3Contact* c = world->contacts.data + i; + bool isLive = ( c->contactId == i ); + + // Write raw struct with pointer fields zeroed + b3Contact copy = *c; + copy.manifolds = NULL; + copy.bodySimIndexA = B3_NULL_INDEX; + copy.bodySimIndexB = B3_NULL_INDEX; + if ( copy.flags & b3_simMeshContact ) + { + copy.meshContact.triangleCache.data = NULL; + copy.meshContact.triangleCache.count = 0; + copy.meshContact.triangleCache.capacity = 0; + } + b3SnapW_Bytes( buf, ©, sizeof( b3Contact ) ); + + if ( !isLive ) + { + // Free slot: no heap data + b3SnapW_I32( buf, 0 ); // manifoldCount + // No triangleCache + continue; + } + + // Manifolds + b3SnapW_I32( buf, c->manifoldCount ); + if ( c->manifoldCount > 0 && c->manifolds != NULL ) + { + b3SnapW_Bytes( buf, c->manifolds, c->manifoldCount * (int)sizeof( b3Manifold ) ); + } + + // Mesh triangleCache + if ( c->flags & b3_simMeshContact ) + { + b3SnapW_I32( buf, c->meshContact.triangleCache.count ); + if ( c->meshContact.triangleCache.count > 0 ) + { + b3SnapW_Bytes( buf, c->meshContact.triangleCache.data, + c->meshContact.triangleCache.count * (int)sizeof( b3TriangleCache ) ); + } + } + } +} + +static void b3DesContacts( b3SnapReader* r, b3World* world ) +{ + int count = b3SnapR_I32( r ); + if ( r->ok && b3SnapCheckCount( r, count, (int)sizeof( b3Contact ), (int)sizeof( b3Contact ) ) == false ) + { + r->ok = false; + } + if ( !r->ok ) + { + return; + } + + b3Array_Resize( world->contacts, count ); + memset( world->contacts.data, 0, (size_t)count * sizeof( b3Contact ) ); + + for ( int i = 0; i < count && r->ok; ++i ) + { + b3Contact* dst = world->contacts.data + i; + b3SnapR_Bytes( r, dst, sizeof( b3Contact ) ); + dst->manifolds = NULL; + dst->bodySimIndexA = B3_NULL_INDEX; + dst->bodySimIndexB = B3_NULL_INDEX; + if ( dst->flags & b3_simMeshContact ) + { + dst->meshContact.triangleCache.data = NULL; + dst->meshContact.triangleCache.count = 0; + dst->meshContact.triangleCache.capacity = 0; + } + + bool isLive = ( dst->contactId == i ); + + int manifoldCount = b3SnapR_I32( r ); + + if ( !r->ok ) + { + break; + } + + if ( isLive && manifoldCount > 0 ) + { + if ( b3SnapCheckCount( r, manifoldCount, (int)sizeof( b3Manifold ), (int)sizeof( b3Manifold ) ) == false ) + { + r->ok = false; + break; + } + dst->manifolds = b3AllocateManifolds( world, manifoldCount ); + dst->manifoldCount = manifoldCount; + b3SnapR_Bytes( r, dst->manifolds, manifoldCount * (int)sizeof( b3Manifold ) ); + } + else + { + dst->manifolds = NULL; + dst->manifoldCount = 0; + } + + // Mesh triangleCache + if ( isLive && ( dst->flags & b3_simMeshContact ) ) + { + int cacheCount = b3SnapR_I32( r ); + if ( !r->ok ) + { + break; + } + if ( cacheCount > 0 ) + { + if ( b3SnapCheckCount( r, cacheCount, (int)sizeof( b3TriangleCache ), (int)sizeof( b3TriangleCache ) ) == false ) + { + r->ok = false; + break; + } + b3Array_Resize( dst->meshContact.triangleCache, cacheCount ); + b3SnapR_Bytes( r, dst->meshContact.triangleCache.data, cacheCount * (int)sizeof( b3TriangleCache ) ); + } + } + } +} + +// Free per-object heap that b3DeserializeIntoShell will overwrite, +// so restoring over a populated world doesn't leak. +static void b3FreeLiveSimElements( b3World* world ) +{ + // Shape heap: materials and hull DB references + for ( int i = 0; i < world->shapes.count; ++i ) + { + b3Shape* s = world->shapes.data + i; + if ( s->id != i ) + { + continue; + } + // A single material lives inline (materials == NULL). Multi material meshes and compounds own + // the array, so free it exactly as b3DestroyShapeAllocations does. + if ( s->materials != NULL ) + { + b3Free( s->materials, (size_t)s->materialCount * sizeof( b3SurfaceMaterial ) ); + s->materials = NULL; + s->materialCount = 0; + } + // Hull is ref-counted in the world DB; release before overwrite so re-adding is ref-neutral. + if ( s->type == b3_hullShape && s->hull != NULL ) + { + b3RemoveHullFromDatabase( world, s->hull ); + s->hull = NULL; + } + // name / userData / userShape are host-owned; do not free + } + + // Contact heap: manifolds + mesh triangleCache + for ( int i = 0; i < world->contacts.count; ++i ) + { + b3Contact* c = world->contacts.data + i; + if ( c->contactId == i ) + { + if ( c->manifolds != NULL ) + { + b3FreeManifolds( world, c->manifolds, c->manifoldCount ); + c->manifolds = NULL; + c->manifoldCount = 0; + } + if ( c->flags & b3_simMeshContact ) + { + b3Array_Destroy( c->meshContact.triangleCache ); + } + } + } + + // Sensor heap: inner arrays + for ( int i = 0; i < world->sensors.count; ++i ) + { + b3Sensor* sensor = world->sensors.data + i; + b3Array_Destroy( sensor->hits ); + b3Array_Destroy( sensor->overlaps1 ); + b3Array_Destroy( sensor->overlaps2 ); + } + + // Island heap: inner arrays + for ( int i = 0; i < world->islands.count; ++i ) + { + b3Island* island = world->islands.data + i; + b3Array_Destroy( island->bodies ); + b3Array_Destroy( island->contacts ); + b3Array_Destroy( island->joints ); + } +} + +int b3SerializeWorld( b3World* world, b3RecBuffer* buf, b3Recording* rec ) +{ + int startSize = buf->size; + + // Snapshot header + b3SnapHeader hdr; + hdr.magic = B3_SNAP_MAGIC; + hdr.version = B3_SNAP_VERSION; + hdr.layoutHash = b3ComputeLayoutHash(); + hdr.flags = B3_ENABLE_VALIDATION ? B3_SNAP_FLAG_VALIDATION : 0u; +#if defined( BOX3D_DOUBLE_PRECISION ) + hdr.flags |= B3_SNAP_FLAG_DOUBLE_PRECISION; +#endif + b3SnapW_Bytes( buf, &hdr, (int)sizeof( hdr ) ); + + // World scalars + b3SerWorldConfig( buf, world ); + + // 6 id pools (Box3D has no chainIdPool) + b3SerIdPool( buf, &world->bodyIdPool ); + b3SerIdPool( buf, &world->shapeIdPool ); + b3SerIdPool( buf, &world->contactIdPool ); + b3SerIdPool( buf, &world->jointIdPool ); + b3SerIdPool( buf, &world->islandIdPool ); + b3SerIdPool( buf, &world->solverSetIdPool ); + + // Solver sets + int setCount = world->solverSets.count; + b3SnapW_I32( buf, setCount ); + for ( int i = 0; i < setCount; ++i ) + { + b3SerSolverSet( buf, world->solverSets.data + i ); + } + + // Sparse body array (userData is host wiring, zero it on the copy) + { + int bodyCount = world->bodies.count; + b3SnapW_I32( buf, bodyCount ); + for ( int i = 0; i < bodyCount; ++i ) + { + b3Body elem = world->bodies.data[i]; + elem.userData = NULL; + b3SnapW_Bytes( buf, &elem, sizeof( b3Body ) ); + } + } + + // Shape sparse array with geometry interning + b3SerShapes( buf, world, rec ); + + // Contact sparse array with manifold and mesh triangleCache + b3SerContacts( buf, world ); + + // Joint sparse array (userData scrubbed) + { + int jointCount = world->joints.count; + b3SnapW_I32( buf, jointCount ); + for ( int i = 0; i < jointCount; ++i ) + { + b3Joint elem = world->joints.data[i]; + elem.userData = NULL; + b3SnapW_Bytes( buf, &elem, sizeof( b3Joint ) ); + } + } + + // Sensors: shapeId + 3 inner arrays each + { + int sensorCount = world->sensors.count; + b3SnapW_I32( buf, sensorCount ); + for ( int i = 0; i < sensorCount; ++i ) + { + b3Sensor* s = world->sensors.data + i; + b3SnapW_I32( buf, s->shapeId ); + b3SerPodArray( buf, s->hits ); + b3SerPodArray( buf, s->overlaps1 ); + b3SerPodArray( buf, s->overlaps2 ); + } + } + + // Islands: 4 scalars + 3 inner arrays each + { + int islandCount = world->islands.count; + b3SnapW_I32( buf, islandCount ); + for ( int i = 0; i < islandCount; ++i ) + { + b3Island* island = world->islands.data + i; + b3SnapW_I32( buf, island->setIndex ); + b3SnapW_I32( buf, island->localIndex ); + b3SnapW_I32( buf, island->islandId ); + b3SnapW_I32( buf, island->constraintRemoveCount ); + b3SerPodArray( buf, island->bodies ); + b3SerPodArray( buf, island->contacts ); + b3SerPodArray( buf, island->joints ); + } + } + + // Broad phase + b3BroadPhase* bp = &world->broadPhase; + for ( int t = 0; t < b3_bodyTypeCount; ++t ) + { + b3SerTree( buf, &bp->trees[t] ); + } + for ( int t = 0; t < b3_bodyTypeCount; ++t ) + { + b3SerBitSet( buf, &bp->movedProxies[t] ); + } + b3SerPodArray( buf, bp->moveArray ); + b3SerHashSet( buf, &bp->pairSet ); + + // Constraint graph + b3ConstraintGraph* graph = &world->constraintGraph; + for ( int c = 0; c < B3_GRAPH_COLOR_COUNT; ++c ) + { + b3SerGraphColor( buf, &graph->colors[c], c == B3_OVERFLOW_INDEX ); + } + + b3SerNames( buf, &world->names ); + + return buf->size - startSize; +} + +bool b3DeserializeIntoShell( const uint8_t* data, int size, b3World* world, b3RecReader* rdr ) +{ + if ( data == NULL || size < (int)sizeof( b3SnapHeader ) ) + { + return false; + } + + // Validate header + b3SnapHeader hdr; + memcpy( &hdr, data, sizeof( hdr ) ); + if ( hdr.magic != B3_SNAP_MAGIC || hdr.version != B3_SNAP_VERSION ) + { + printf( "b3DeserializeIntoShell: bad magic/version\n" ); + return false; + } + bool imageDouble = ( hdr.flags & B3_SNAP_FLAG_DOUBLE_PRECISION ) != 0; +#if defined( BOX3D_DOUBLE_PRECISION ) + bool buildDouble = true; +#else + bool buildDouble = false; +#endif + if ( imageDouble != buildDouble ) + { + printf( "b3DeserializeIntoShell: precision mismatch\n" ); + return false; + } + if ( hdr.layoutHash != b3ComputeLayoutHash() ) + { + printf( "b3DeserializeIntoShell: layout hash mismatch\n" ); + return false; + } + + b3SnapReader readerStorage; + b3SnapReader* r = &readerStorage; + r->data = data; + r->cursor = (int)sizeof( b3SnapHeader ); + r->size = size; + r->ok = true; + + // Free existing per-object heap before overwriting + b3FreeLiveSimElements( world ); + + // 1. World scalars + b3DesWorldConfig( r, world ); + + // 2. 6 id pools; destroy the pre-created sets' pool state first + b3DesIdPool( r, &world->bodyIdPool ); + b3DesIdPool( r, &world->shapeIdPool ); + b3DesIdPool( r, &world->contactIdPool ); + b3DesIdPool( r, &world->jointIdPool ); + b3DesIdPool( r, &world->islandIdPool ); + b3DesIdPool( r, &world->solverSetIdPool ); + + // 3. Solver sets: destroy inner arrays of existing sets first + for ( int i = 0; i < world->solverSets.count; ++i ) + { + b3SolverSet* set = world->solverSets.data + i; + b3Array_Destroy( set->bodySims ); + b3Array_Destroy( set->bodyStates ); + b3Array_Destroy( set->jointSims ); + b3Array_Destroy( set->contactIndices ); + b3Array_Destroy( set->islandSims ); + } + + int setCount = b3SnapR_I32( r ); + if ( r->ok && b3SnapCheckCount( r, setCount, (int)sizeof( b3SolverSet ), 6 * (int)sizeof( int ) ) == false ) + { + r->ok = false; + } + if ( r->ok ) + { + b3Array_Resize( world->solverSets, setCount ); + memset( world->solverSets.data, 0, (size_t)setCount * sizeof( b3SolverSet ) ); + for ( int i = 0; i < setCount; ++i ) + { + b3DesSolverSet( r, world->solverSets.data + i ); + } + } + + if ( !r->ok ) + { + return false; + } + + // 4. Body sparse array + { + int bodyCount = b3SnapR_I32( r ); + if ( r->ok && b3SnapCheckCount( r, bodyCount, (int)sizeof( b3Body ), (int)sizeof( b3Body ) ) == false ) + { + r->ok = false; + } + if ( r->ok ) + { + b3Array_Resize( world->bodies, bodyCount ); + for ( int i = 0; i < bodyCount; ++i ) + { + b3SnapR_Bytes( r, world->bodies.data + i, sizeof( b3Body ) ); + world->bodies.data[i].userData = NULL; + } + } + } + + if ( !r->ok ) + { + return false; + } + + // 5. Shape sparse array + b3DesShapes( r, world, rdr ); + + if ( !r->ok ) + { + return false; + } + + // 6. Contact sparse array + b3DesContacts( r, world ); + + if ( !r->ok ) + { + return false; + } + + // 7. Joint sparse array + { + int jointCount = b3SnapR_I32( r ); + if ( r->ok && b3SnapCheckCount( r, jointCount, (int)sizeof( b3Joint ), (int)sizeof( b3Joint ) ) == false ) + { + r->ok = false; + } + if ( r->ok ) + { + b3Array_Resize( world->joints, jointCount ); + for ( int i = 0; i < jointCount; ++i ) + { + b3SnapR_Bytes( r, world->joints.data + i, sizeof( b3Joint ) ); + world->joints.data[i].userData = NULL; + } + } + } + + // 8. Sensors + { + b3Array_Destroy( world->sensors ); + b3Array_Create( world->sensors ); + + int sensorCount = b3SnapR_I32( r ); + if ( r->ok && b3SnapCheckCount( r, sensorCount, (int)sizeof( b3Sensor ), 4 * (int)sizeof( int ) ) == false ) + { + r->ok = false; + } + if ( r->ok ) + { + b3Array_Resize( world->sensors, sensorCount ); + memset( world->sensors.data, 0, (size_t)sensorCount * sizeof( b3Sensor ) ); + } + + for ( int i = 0; i < sensorCount && r->ok; ++i ) + { + b3Sensor* s = world->sensors.data + i; + s->shapeId = b3SnapR_I32( r ); + b3Array_Create( s->hits ); + b3Array_Create( s->overlaps1 ); + b3Array_Create( s->overlaps2 ); + b3DesPodArray( r, s->hits ); + b3DesPodArray( r, s->overlaps1 ); + b3DesPodArray( r, s->overlaps2 ); + } + } + + // 9. Islands + { + b3Array_Destroy( world->islands ); + b3Array_Create( world->islands ); + + int islandCount = b3SnapR_I32( r ); + if ( r->ok && b3SnapCheckCount( r, islandCount, (int)sizeof( b3Island ), 7 * (int)sizeof( int ) ) == false ) + { + r->ok = false; + } + if ( r->ok ) + { + b3Array_Resize( world->islands, islandCount ); + memset( world->islands.data, 0, (size_t)islandCount * sizeof( b3Island ) ); + } + + for ( int i = 0; i < islandCount && r->ok; ++i ) + { + b3Island* island = world->islands.data + i; + island->setIndex = b3SnapR_I32( r ); + island->localIndex = b3SnapR_I32( r ); + island->islandId = b3SnapR_I32( r ); + island->constraintRemoveCount = b3SnapR_I32( r ); + b3Array_Create( island->bodies ); + b3Array_Create( island->contacts ); + b3Array_Create( island->joints ); + b3DesPodArray( r, island->bodies ); + b3DesPodArray( r, island->contacts ); + b3DesPodArray( r, island->joints ); + } + } + + // 10. Broad phase + { + b3BroadPhase* bp = &world->broadPhase; + + for ( int t = 0; t < b3_bodyTypeCount; ++t ) + { + b3DesTree( r, &bp->trees[t] ); + } + for ( int t = 0; t < b3_bodyTypeCount; ++t ) + { + b3DesBitSet( r, &bp->movedProxies[t] ); + } + + b3Array_Destroy( bp->moveArray ); + b3Array_Create( bp->moveArray ); + b3DesPodArray( r, bp->moveArray ); + + b3DesHashSet( r, &bp->pairSet ); + // Transient moveResults/movePairs stay at shell's NULL/0 + } + + // 11. Constraint graph + { + b3ConstraintGraph* graph = &world->constraintGraph; + for ( int c = 0; c < B3_GRAPH_COLOR_COUNT; ++c ) + { + b3DesGraphColor( r, &graph->colors[c], c == B3_OVERFLOW_INDEX ); + } + } + + b3DesNames( r, &world->names ); + + return r->ok; +} diff --git a/vendor/box3d/src/src/world_snapshot.h b/vendor/box3d/src/src/world_snapshot.h new file mode 100644 index 000000000..7bf0065a1 --- /dev/null +++ b/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 +#include + +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 );