diff --git a/.github/workflows/build.yml b/.github/workflows/build.yml index 5e75d2c..32b91ed 100644 --- a/.github/workflows/build.yml +++ b/.github/workflows/build.yml @@ -28,11 +28,13 @@ jobs: os: ubuntu-latest args: '-DCMAKE_CXX_COMPILER=clang++-18 -DCMAKE_C_COMPILER=clang-18 -DCMAKE_C_FLAGS="-fsanitize=thread -fno-omit-frame-pointer" -DCMAKE_CXX_FLAGS="-fsanitize=thread -fno-omit-frame-pointer" -DCMAKE_EXE_LINKER_FLAGS="-fsanitize=thread"' test: ./bin/test + # Runner speed varies about 3x by region and MSan at -O0 can exceed the timeout on the slow ones. + # -O1 is the level MSan recommends and Debug keeps asserts. Line buffering shows per test timing. - name: ubuntu-clang-msan os: ubuntu-latest timeout: 10 - args: '-DCMAKE_CXX_COMPILER=clang++-18 -DCMAKE_C_COMPILER=clang-18 -DCMAKE_C_FLAGS="-fsanitize=memory -fsanitize-memory-track-origins=1 -fno-omit-frame-pointer" -DCMAKE_CXX_FLAGS="-fsanitize=memory -fsanitize-memory-track-origins=1 -fno-omit-frame-pointer" -DCMAKE_EXE_LINKER_FLAGS="-fsanitize=memory"' - test: ./bin/test + args: '-DCMAKE_CXX_COMPILER=clang++-18 -DCMAKE_C_COMPILER=clang-18 -DCMAKE_C_FLAGS="-O1 -fsanitize=memory -fsanitize-memory-track-origins=1 -fno-omit-frame-pointer" -DCMAKE_CXX_FLAGS="-O1 -fsanitize=memory -fsanitize-memory-track-origins=1 -fno-omit-frame-pointer" -DCMAKE_EXE_LINKER_FLAGS="-fsanitize=memory"' + test: lscpu | grep 'Model name'; nproc; stdbuf -oL ./bin/test msan-options: abort_on_error=1:halt_on_error=1 - name: macos os: macos-latest @@ -88,7 +90,7 @@ jobs: run: cmake -B ${{github.workspace}}/build -DCMAKE_BUILD_TYPE=${{ matrix.config || env.BUILD_TYPE }} -DBOX3D_SAMPLES=OFF -DBUILD_SHARED_LIBS=OFF -DBOX3D_VALIDATE=ON -DCMAKE_COMPILE_WARNING_AS_ERROR=ON ${{ matrix.args }} - name: Build - run: cmake --build ${{github.workspace}}/build --config ${{ matrix.config || env.BUILD_TYPE }} + run: cmake --build ${{github.workspace}}/build --config ${{ matrix.config || env.BUILD_TYPE }} --parallel - name: Test working-directory: ${{github.workspace}}/build @@ -126,7 +128,7 @@ jobs: run: cmake -B build -G Ninja -DCMAKE_BUILD_TYPE=RelWithDebInfo -DBOX3D_SAMPLES=OFF -DBUILD_SHARED_LIBS=OFF -DBOX3D_VALIDATE=ON -DCMAKE_COMPILE_WARNING_AS_ERROR=ON - name: Build - run: cmake --build build --config RelWithDebInfo + run: cmake --build build --config RelWithDebInfo --parallel - name: Test working-directory: build @@ -148,7 +150,7 @@ jobs: run: emcmake cmake -B build -DCMAKE_BUILD_TYPE=Debug -DBOX3D_SAMPLES=OFF -DBUILD_SHARED_LIBS=OFF -DBOX3D_VALIDATE=ON - name: Build - run: cmake --build build --config Debug + run: cmake --build build --config Debug --parallel samples: name: ${{ matrix.name }} @@ -199,4 +201,4 @@ jobs: run: cmake -B ${{github.workspace}}/build -DCMAKE_BUILD_TYPE=Release -DBOX3D_SAMPLES=ON -DBOX3D_UNIT_TESTS=OFF ${{ matrix.args }} - name: Build - run: cmake --build ${{github.workspace}}/build --config Release + run: cmake --build ${{github.workspace}}/build --config Release --parallel diff --git a/CMakeLists.txt b/CMakeLists.txt index e5d603b..dc3ecb6 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -89,6 +89,7 @@ if (CMAKE_C_COMPILER_ID MATCHES "GNU|Clang") endif() option(BOX3D_DISABLE_SIMD "Disable SIMD math (slower)" OFF) +option(BOX3D_AVX2 "Compile AVX2 kernels, selected at runtime on x86 CPUs that support them" ON) option(BOX3D_COMPILE_WARNING_AS_ERROR "Compile warnings as errors" OFF) option(BOX3D_DOUBLE_PRECISION "Enable double precision for large worlds" OFF) @@ -188,4 +189,4 @@ endif() # https://stackoverflow.com/questions/72635402/how-to-set-working-directory-in-cmake-visual-studio-2022 # launch.vs.json -# "currentDir": "${workspaceRoot}" \ No newline at end of file +# "currentDir": "${workspaceRoot}" diff --git a/README.md b/README.md index cfe16e4..ab36b8c 100644 --- a/README.md +++ b/README.md @@ -149,6 +149,8 @@ You will need a compiler that supports C++20 to build the samples. Box3D uses SSE2 and Neon SIMD math to improve performance. SIMD can be disabled by defining `BOX3D_DISABLE_SIMD`. +On x86, AVX2 kernels are selected at runtime when the CPU supports them. Configure with `-DBOX3D_AVX2=OFF` to disable them. + ## Documentation The user manual lives in [`docs/`](docs/) and is built with Doxygen. Enable the `BOX3D_DOCS` CMake option and build the `doc` target. diff --git a/benchmark/amd7950x_avx2/convex_pile.csv b/benchmark/amd7950x_avx2/convex_pile.csv new file mode 100644 index 0000000..1fa06e8 --- /dev/null +++ b/benchmark/amd7950x_avx2/convex_pile.csv @@ -0,0 +1,9 @@ +threads,ms +1,5092.19 +2,2694.4 +3,1855.5 +4,1450.33 +5,1195.15 +6,1032.87 +7,924.237 +8,843.199 diff --git a/benchmark/amd7950x_avx2/joint_grid.csv b/benchmark/amd7950x_avx2/joint_grid.csv new file mode 100644 index 0000000..926e3f4 --- /dev/null +++ b/benchmark/amd7950x_avx2/joint_grid.csv @@ -0,0 +1,9 @@ +threads,ms +1,1308.56 +2,683.543 +3,461.149 +4,351.313 +5,283.848 +6,239.34 +7,208.293 +8,185.43 diff --git a/benchmark/amd7950x_avx2/junkyard.csv b/benchmark/amd7950x_avx2/junkyard.csv new file mode 100644 index 0000000..7cb8746 --- /dev/null +++ b/benchmark/amd7950x_avx2/junkyard.csv @@ -0,0 +1,9 @@ +threads,ms +1,11252.9 +2,5915.73 +3,4113.86 +4,3194.52 +5,2626.44 +6,2261.77 +7,2009.22 +8,1824.68 diff --git a/benchmark/amd7950x_avx2/large_pyramid.csv b/benchmark/amd7950x_avx2/large_pyramid.csv new file mode 100644 index 0000000..b06f305 --- /dev/null +++ b/benchmark/amd7950x_avx2/large_pyramid.csv @@ -0,0 +1,9 @@ +threads,ms +1,1067.06 +2,558.46 +3,390.698 +4,300.188 +5,249.834 +6,218.912 +7,193.65 +8,175.819 diff --git a/benchmark/amd7950x_avx2/large_world.csv b/benchmark/amd7950x_avx2/large_world.csv new file mode 100644 index 0000000..75041ab --- /dev/null +++ b/benchmark/amd7950x_avx2/large_world.csv @@ -0,0 +1,9 @@ +threads,ms +1,5.0616 +2,6.9262 +3,7.827 +4,8.2244 +5,8.7631 +6,8.6223 +7,8.7636 +8,8.9294 diff --git a/benchmark/amd7950x_avx2/many_pyramids.csv b/benchmark/amd7950x_avx2/many_pyramids.csv new file mode 100644 index 0000000..c68aba0 --- /dev/null +++ b/benchmark/amd7950x_avx2/many_pyramids.csv @@ -0,0 +1,9 @@ +threads,ms +1,1163.37 +2,605.215 +3,411.543 +4,307.527 +5,249.396 +6,216.535 +7,184.334 +8,165.442 diff --git a/benchmark/amd7950x_avx2/rain.csv b/benchmark/amd7950x_avx2/rain.csv new file mode 100644 index 0000000..cfa7c5f --- /dev/null +++ b/benchmark/amd7950x_avx2/rain.csv @@ -0,0 +1,9 @@ +threads,ms +1,2038.39 +2,1110.3 +3,773.18 +4,603.397 +5,502.292 +6,434.461 +7,389.414 +8,355.206 diff --git a/benchmark/amd7950x_avx2/sleep.csv b/benchmark/amd7950x_avx2/sleep.csv new file mode 100644 index 0000000..20320b2 --- /dev/null +++ b/benchmark/amd7950x_avx2/sleep.csv @@ -0,0 +1,9 @@ +threads,ms +1,596.258 +2,328.927 +3,231.57 +4,187.904 +5,159.347 +6,142.58 +7,131.034 +8,124.711 diff --git a/benchmark/amd7950x_avx2/spinner.csv b/benchmark/amd7950x_avx2/spinner.csv new file mode 100644 index 0000000..920a029 --- /dev/null +++ b/benchmark/amd7950x_avx2/spinner.csv @@ -0,0 +1,9 @@ +threads,ms +1,2919.32 +2,1674.26 +3,1200.43 +4,960.786 +5,821.659 +6,728.967 +7,669.058 +8,626.733 diff --git a/benchmark/amd7950x_avx2/trees100.csv b/benchmark/amd7950x_avx2/trees100.csv new file mode 100644 index 0000000..ad34128 --- /dev/null +++ b/benchmark/amd7950x_avx2/trees100.csv @@ -0,0 +1,9 @@ +threads,ms +1,167.468 +2,110.54 +3,87.5729 +4,77.6966 +5,73.9776 +6,71.7265 +7,69.6889 +8,69.3954 diff --git a/benchmark/amd7950x_avx2/trees25.csv b/benchmark/amd7950x_avx2/trees25.csv new file mode 100644 index 0000000..fd12855 --- /dev/null +++ b/benchmark/amd7950x_avx2/trees25.csv @@ -0,0 +1,9 @@ +threads,ms +1,657.535 +2,386.678 +3,284.076 +4,245.238 +5,223.137 +6,208.401 +7,197.529 +8,194.169 diff --git a/benchmark/amd7950x_avx2/trees50.csv b/benchmark/amd7950x_avx2/trees50.csv new file mode 100644 index 0000000..6f91345 --- /dev/null +++ b/benchmark/amd7950x_avx2/trees50.csv @@ -0,0 +1,9 @@ +threads,ms +1,285.043 +2,176.584 +3,135.305 +4,117.663 +5,109.496 +6,104.289 +7,101.186 +8,98.9602 diff --git a/benchmark/amd7950x_avx2/washer.csv b/benchmark/amd7950x_avx2/washer.csv new file mode 100644 index 0000000..f671a36 --- /dev/null +++ b/benchmark/amd7950x_avx2/washer.csv @@ -0,0 +1,9 @@ +threads,ms +1,15669.7 +2,8410.7 +3,5793.28 +4,4493.58 +5,3722.97 +6,3219.45 +7,2849.15 +8,2562.49 diff --git a/benchmark/main.c b/benchmark/main.c index 38f8651..3474139 100644 --- a/benchmark/main.c +++ b/benchmark/main.c @@ -125,7 +125,7 @@ static void RunSatBenchmarks( int runCount ) int repeatCount = 1; #endif - printf( "benchmark: sat, pairs = %d, repeats = %d\n", SAT_PAIR_COUNT, repeatCount ); + printf( "benchmark: sat, pairs = %d, repeats = %d, simd width = %d\n", SAT_PAIR_COUNT, repeatCount, GetSimdWidth() ); printf( "time per query (us), sat full has the inscribed sphere bound disabled\n" ); printf( "%-20s %10s %10s %10s %10s\n", "pair", "gjk cold", "gjk warm", "sat", "sat full" ); @@ -284,6 +284,7 @@ int main( int argc, char** argv ) bool enableContinuous = true; bool recordStepTimes = false; bool runSat = false; + int simdWidth = 0; assert( maxThreadCount <= B3_MAX_WORKERS ); @@ -338,6 +339,20 @@ int main( int argc, char** argv ) #else printf( "The SAT benchmark requires static linkage\n" ); exit( 1 ); +#endif + } + else if ( ( value = MatchValue( arg, "-simd=", "--simd-width=" ) ) != NULL ) + { + simdWidth = atoi( value ); + if ( simdWidth != 4 && simdWidth != 8 ) + { + printf( "The SIMD width must be 4 or 8\n" ); + exit( 1 ); + } + +#ifndef BOX3D_INTERNAL_BENCHMARKS + printf( "Forcing the SIMD width requires static linkage\n" ); + exit( 1 ); #endif } else if ( strcmp( arg, "-l" ) == 0 || strcmp( arg, "--list" ) == 0 ) @@ -355,6 +370,7 @@ int main( int argc, char** argv ) "-nc, --no-continuous: disable continuous collision\n" "-s, --record-steps: record step times\n" "-sat, --sat: compare GJK and SAT on hull pairs, then exit\n" + "-simd, --simd-width=<4|8>: force the SIMD width (static Box3D only)\n" "-l, --list: list the registered benchmarks\n" "-h, --help: print this help\n" ); exit( 0 ); @@ -371,6 +387,11 @@ int main( int argc, char** argv ) } #ifdef BOX3D_INTERNAL_BENCHMARKS + if ( simdWidth != 0 ) + { + SetSimdWidth( simdWidth ); + } + if ( runSat ) { RunSatBenchmarks( runCount ); @@ -381,6 +402,9 @@ int main( int argc, char** argv ) printf( "Starting benchmarks\n" ); printf( "======================================\n" ); +#ifdef BOX3D_INTERNAL_BENCHMARKS + printf( "simd width = %d\n", GetSimdWidth() ); +#endif for ( int benchmarkIndex = 0; benchmarkIndex < benchmarkCount; ++benchmarkIndex ) { diff --git a/include/box3d/base.h b/include/box3d/base.h index 8fcf19d..eb85047 100644 --- a/include/box3d/base.h +++ b/include/box3d/base.h @@ -177,6 +177,9 @@ B3_API bool b3IsDoublePrecision( void ); /// an ABI test. B3_API int b3GetMaxManifoldPoints( void ); +/// @return true if the current CPU has the AVX2 instruction set. +B3_API bool b3IsAVX2Available( void ); + /**@}*/ //! @cond diff --git a/include/box3d/box3d.h b/include/box3d/box3d.h index a48a15d..7385973 100644 --- a/include/box3d/box3d.h +++ b/include/box3d/box3d.h @@ -219,6 +219,10 @@ B3_API void b3World_EnableWarmStarting( b3WorldId worldId, bool flag ); /// Is constraint warm starting enabled? B3_API bool b3World_IsWarmStartingEnabled( b3WorldId worldId ); +/// Enable the SSE2 fallback even when AVX2 is present. This is for testing. +/// Normally you should use the CMake build settings to disable AVX2. +B3_API void b3World_EnableSSE2Fallback( b3WorldId worldId, bool flag ); + /// Get the number of awake bodies B3_API int b3World_GetAwakeBodyCount( b3WorldId worldId ); diff --git a/include/box3d/collision.h b/include/box3d/collision.h index ab1e86f..61b3782 100644 --- a/include/box3d/collision.h +++ b/include/box3d/collision.h @@ -188,7 +188,7 @@ B3_INLINE const b3HullFace* b3GetHullFaces( const b3HullData* hull ) /// Get read only SOA vertices. This is an array of vertices with all x values, /// y values, and z values as separate arrays. The array lengths are padded to -/// a multiple of 4. The padded values are repeats of the first value. +/// a multiple of 8. The padded values are repeats of the first value. B3_INLINE const float* b3GetHullSoaVertices( const b3HullData* hull ) { if ( hull->soaVertexOffset == 0 ) @@ -201,7 +201,7 @@ B3_INLINE const float* b3GetHullSoaVertices( const b3HullData* hull ) /// Get read only SOA unit normal vectors. This is an array of normals with all x values, /// y values, and z values as separate arrays. The array lengths are padded to -/// a multiple of 4. The padded values are 0. +/// a multiple of 8. The padded values are 0. B3_INLINE const float* b3GetHullSoaNormals( const b3HullData* hull ) { if ( hull->soaNormalOffset == 0 ) diff --git a/include/box3d/types.h b/include/box3d/types.h index 09552f7..20c9745 100644 --- a/include/box3d/types.h +++ b/include/box3d/types.h @@ -2019,7 +2019,7 @@ typedef struct b3HullFace } b3HullFace; /// 64-bit hull version. Useful for validating serialized data. -#define B3_HULL_VERSION 0x9D3E61B7C2A4F085ull +#define B3_HULL_VERSION 0x6B1E39D4A87C25F3ull /// A convex hull. /// @note This data structure has data hanging off the end and cannot be directly copied. @@ -2105,9 +2105,9 @@ typedef struct b3BoxHull float vx[8]; ///< vertex x float vy[8]; ///< vertex y float vz[8]; ///< vertex z - float nx[8]; ///< normal x, padded to multiple of 4 - float ny[8]; ///< normal y, padded to multiple of 4 - float nz[8]; ///< normal z, padded to multiple of 4 + float nx[8]; ///< normal x, padded to multiple of 8 + float ny[8]; ///< normal y, padded to multiple of 8 + float nz[8]; ///< normal z, padded to multiple of 8 float edgeCosines[12]; ///< dot(n1, n2) for each full edge. } b3BoxHull; diff --git a/samples/sample.cpp b/samples/sample.cpp index 1d8dcbf..5abea6b 100644 --- a/samples/sample.cpp +++ b/samples/sample.cpp @@ -467,6 +467,7 @@ void Sample::Step() b3World_EnableContinuous( m_worldId, m_context->enableContinuous ); b3World_SetRestitutionIterations( m_worldId, m_context->restitutionIterations ); b3World_EnableRestitutionPropagation( m_worldId, m_context->enableRestitutionPropagation ); + b3World_EnableSSE2Fallback( m_worldId, m_context->enableSSE2Fallback ); if ( timeStep > 0.0f || m_stepWhilePaused ) { @@ -2183,6 +2184,13 @@ static void DrawInfoPanel( SampleContext* context ) ImGui::Checkbox( "Bounce Propagation##Solver", &context->enableRestitutionPropagation ); HelpMarker( "Enable restitution solver propagation across all touching contacts points" ); + static bool avx2Available = b3IsAVX2Available(); + if ( avx2Available ) + { + ImGui::Checkbox( "SSE2 Fallback##Solver", &context->enableSSE2Fallback ); + HelpMarker( "Use the 4 wide SSE2 path instead of the 8 wide AVX2 path.\nThe simulation is bitwise identical either way." ); + } + if ( ImGui::Shortcut( ImGuiKey_R ) || ImGui::Button( "Restart" ) ) { SelectSample( context, context->sampleIndex, true ); diff --git a/samples/sample.h b/samples/sample.h index d457894..6d09f41 100644 --- a/samples/sample.h +++ b/samples/sample.h @@ -76,6 +76,7 @@ struct SampleContext bool enableWarmStarting = true; bool enableContinuous = true; bool enableRestitutionPropagation = false; + bool enableSSE2Fallback = false; bool enableSleep = true; bool pause = false; int singleStep = 0; diff --git a/shared/sat_benchmark.c b/shared/sat_benchmark.c index 740e872..c20943b 100644 --- a/shared/sat_benchmark.c +++ b/shared/sat_benchmark.c @@ -9,6 +9,7 @@ #include "box3d/math_functions.h" #include "manifold.h" +#include "simd.h" #include @@ -185,3 +186,13 @@ SatBenchmarkResult RunSatBenchmark( const SatBenchmarkData* data, int repeatCoun }; return result; } + +void SetSimdWidth( int width ) +{ + b3SetSIMDWidth( width ); +} + +int GetSimdWidth( void ) +{ + return b3GetSIMDWidth(); +} diff --git a/shared/sat_benchmark.h b/shared/sat_benchmark.h index 70d1a36..66b2b2d 100644 --- a/shared/sat_benchmark.h +++ b/shared/sat_benchmark.h @@ -65,6 +65,9 @@ void EnableSatInscribedSphere( SatBenchmarkData* data, bool flag ); SatBenchmarkResult RunSatBenchmark( const SatBenchmarkData* data, int repeatCount, bool warmStartDistance ); +void SetSimdWidth( int width ); +int GetSimdWidth( void ); + #ifdef __cplusplus } #endif diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index 401d236..566b72f 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -21,9 +21,11 @@ set(BOX3D_SOURCE_FILES constraint_graph.h contact.c contact.h + contact_solver_wide.inl contact_solver.c contact_solver.h container.h + convex_manifold_wide.inl convex_manifold.c core.c core.h @@ -76,6 +78,11 @@ set(BOX3D_SOURCE_FILES shape.h simd.c simd.h + simd_avx2.h + simd_neon.h + simd_scalar.h + simd_sse2.h + simd_wide.h solver.c solver.h solver_set.c @@ -90,6 +97,8 @@ set(BOX3D_SOURCE_FILES verstable.h weld_joint.c wheel_joint.c + wide4.c + wide8.c box3d.natvis ) @@ -167,6 +176,11 @@ if (BOX3D_DISABLE_SIMD) target_compile_definitions(box3d PRIVATE BOX3D_DISABLE_SIMD) endif() +if (NOT BOX3D_AVX2) + message(STATUS "Box3D AVX2 disabled") + target_compile_definitions(box3d PRIVATE BOX3D_DISABLE_AVX2) +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 @@ -197,6 +211,10 @@ if (MSVC) target_compile_options(box3d PRIVATE -Wmissing-prototypes) endif() + if (BOX3D_AVX2 AND NOT CMAKE_C_COMPILER_ID STREQUAL "Clang" AND CMAKE_C_COMPILER_ARCHITECTURE_ID MATCHES "^(x64|X86)$") + set_source_files_properties(wide8.c PROPERTIES COMPILE_OPTIONS "/arch:AVX2") + endif() + elseif (MINGW) message(STATUS "Box3D on MinGW") elseif (APPLE) diff --git a/src/constraint_graph.h b/src/constraint_graph.h index ab17e9a..fcda3a8 100644 --- a/src/constraint_graph.h +++ b/src/constraint_graph.h @@ -50,7 +50,7 @@ typedef struct b3GraphColor b3Array( b3ContactSpec ) contacts; // These are used for convex contacts - struct b3ContactConstraintWide* wideConstraints; + void* wideConstraints; int wideConstraintCount; // These are used for mesh and overflow contacts diff --git a/src/contact.c b/src/contact.c index dab7f1e..903c8b1 100644 --- a/src/contact.c +++ b/src/contact.c @@ -529,7 +529,8 @@ static bool b3ComputeConvexManifold( b3World* world, int workerIndex, b3Contact* else { B3_ASSERT( typeB == b3_hullShape ); - b3CollideHulls( &geomManifold, pointCapacity, shapeA->hull, shapeB->hull, transformBtoA, &cache->satCache ); + b3CollideHullsAtWidth( &geomManifold, pointCapacity, shapeA->hull, shapeB->hull, transformBtoA, &cache->satCache, + world->simdWidth ); world->taskContexts.data[workerIndex].satCallCount += 1; world->taskContexts.data[workerIndex].satCacheHitCount += cache->satCache.hit; } diff --git a/src/contact_solver.c b/src/contact_solver.c index 0998848..7b64af2 100644 --- a/src/contact_solver.c +++ b/src/contact_solver.c @@ -930,1285 +930,6 @@ void b3StoreImpulses_Mesh( b3SolverBlock block, b3StepContext* context, int work taskContext->hasHitEvents = hasHitEvents; } -// Wide vec2 -typedef struct b3Vec2W -{ - b3FloatW x, y; -} b3Vec2W; - -// 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; - -typedef struct b3Matrix3W -{ - b3Vec3W cx, cy, cz; -} b3Matrix3W; - -// a + b -static inline b3Vec2W b3AddV2W( b3Vec2W a, b3Vec2W b ) -{ - return (b3Vec2W){ - b3AddW( a.x, b.x ), - b3AddW( a.y, b.y ), - }; -} - -// 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 b3Matrix3W b3MakeMatrixFromQuatW( b3QuatW q ) -{ - b3FloatW x2 = b3AddW( q.V.X, q.V.X ); - b3FloatW y2 = b3AddW( q.V.Y, q.V.Y ); - b3FloatW z2 = b3AddW( q.V.Z, q.V.Z ); - b3FloatW xx2 = b3MulW( q.V.X, x2 ); - b3FloatW yy2 = b3MulW( q.V.Y, y2 ); - b3FloatW zz2 = b3MulW( q.V.Z, z2 ); - b3FloatW xy2 = b3MulW( q.V.X, y2 ); - b3FloatW xz2 = b3MulW( q.V.X, z2 ); - b3FloatW yz2 = b3MulW( q.V.Y, z2 ); - b3FloatW xw2 = b3MulW( q.S, x2 ); - b3FloatW yw2 = b3MulW( q.S, y2 ); - b3FloatW zw2 = b3MulW( q.S, z2 ); - b3FloatW one = b3SplatW( 1.0f ); - - b3Matrix3W m; - m.cx.X = b3SubW( one, b3AddW( yy2, zz2 ) ); - m.cx.Y = b3AddW( xy2, zw2 ); - m.cx.Z = b3SubW( xz2, yw2 ); - m.cy.X = b3SubW( xy2, zw2 ); - m.cy.Y = b3SubW( one, b3AddW( xx2, zz2 ) ); - m.cy.Z = b3AddW( yz2, xw2 ); - m.cz.X = b3AddW( xz2, yw2 ); - m.cz.Y = b3SubW( yz2, xw2 ); - m.cz.Z = b3SubW( one, b3AddW( xx2, yy2 ) ); - return m; -} - -static inline b3Vec3W b3MulM3VW( b3Matrix3W m, b3Vec3W a ) -{ - b3Vec3W b = { - b3AddW( b3MulW( m.cx.X, a.X ), b3AddW( b3MulW( m.cy.X, a.Y ), b3MulW( m.cz.X, a.Z ) ) ), - b3AddW( b3MulW( m.cx.Y, a.X ), b3AddW( b3MulW( m.cy.Y, a.Y ), b3MulW( m.cz.Y, a.Z ) ) ), - b3AddW( b3MulW( m.cx.Z, a.X ), b3AddW( b3MulW( m.cy.Z, a.Y ), b3MulW( m.cz.Z, a.Z ) ) ), - }; - - return b; -} - -static inline b3Vec3W b3InvRotateVectorW( b3QuatW q, b3Vec3W a ) -{ - b3Vec3W t = b3CrossW( q.V, a ); - t = (b3Vec3W){ b3AddW( t.X, t.X ), b3AddW( t.Y, t.Y ), b3AddW( t.Z, t.Z ) }; - b3Vec3W u = b3CrossW( q.V, t ); - return (b3Vec3W){ - b3AddW( b3SubW( a.X, b3MulW( q.S, t.X ) ), u.X ), - b3AddW( b3SubW( a.Y, b3MulW( q.S, t.Y ) ), u.Y ), - b3AddW( b3SubW( a.Z, b3MulW( q.S, t.Z ) ), u.Z ), - }; -} - -// 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; - b3FloatW restitutionImpulses; -} b3ContactConstraintPointWide; - -// Solves four points -typedef struct b3ContactConstraintWide -{ - // These are base 1 - int indexA[B3_SIMD_WIDTH]; - int indexB[B3_SIMD_WIDTH]; - - int pointCounts[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; - - // Friction centers - b3Vec3W centerA, centerB; - b3FloatW twistMass; - b3FloatW twistImpulse; - b3SymMatrix2W tangentMass; - b3Vec2W frictionImpulse; - b3SymMatrix3W rollingMass; - b3Vec3W rollingImpulse; - b3FloatW friction; - b3FloatW rollingResistance; - b3FloatW tangentVelocity1; - b3FloatW tangentVelocity2; - 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_assert( sizeof( b3BodyState ) == 64, "body state layout" ); -_Static_assert( offsetof( b3BodyState, linearVelocity ) == 0 && offsetof( b3BodyState, angularVelocity ) == 16 && - offsetof( b3BodyState, deltaPosition ) == 32 && offsetof( b3BodyState, deltaRotation ) == 48, - "body state layout" ); - -B3_FORCE_INLINE b3BodyStateW b3GatherBodies( const b3BodyState* B3_RESTRICT states, const int* B3_RESTRICT indices ) -{ - const float* identity = (const float*)&b3_identityBodyState; - - // Indices are 0 for null - const float* p1 = indices[0] == 0 ? identity : (const float*)( states + indices[0] - 1 ); - const float* p2 = indices[1] == 0 ? identity : (const float*)( states + indices[1] - 1 ); - const float* p3 = indices[2] == 0 ? identity : (const float*)( states + indices[2] - 1 ); - const float* p4 = indices[3] == 0 ? identity : (const float*)( states + indices[3] - 1 ); - - b3BodyStateW s; - b3FloatW pad; - b3TransposeW( b3LoadW( p1 ), b3LoadW( p2 ), b3LoadW( p3 ), b3LoadW( p4 ), &s.v.X, &s.v.Y, &s.v.Z, &pad ); - b3TransposeW( b3LoadW( p1 + 4 ), b3LoadW( p2 + 4 ), b3LoadW( p3 + 4 ), b3LoadW( p4 + 4 ), &s.w.X, &s.w.Y, &s.w.Z, &pad ); - b3TransposeW( b3LoadW( p1 + 8 ), b3LoadW( p2 + 8 ), b3LoadW( p3 + 8 ), b3LoadW( p4 + 8 ), &s.dp.X, &s.dp.Y, &s.dp.Z, &pad ); - b3TransposeW( b3LoadW( p1 + 12 ), b3LoadW( p2 + 12 ), b3LoadW( p3 + 12 ), b3LoadW( p4 + 12 ), &s.dq.V.X, &s.dq.V.Y, - &s.dq.V.Z, &s.dq.S ); - return s; -} - -B3_FORCE_INLINE void b3StoreBodyVelocity( b3BodyState* B3_RESTRICT states, int index, b3FloatW v, b3FloatW w ) -{ - // Indices are 0 for null - if ( index == 0 ) - { - return; - } - - b3BodyState* state = states + index - 1; - uint32_t flags = state->flags; - if ( ( flags & b3_dynamicFlag ) == 0 ) - { - return; - } - - b3StoreW( (float*)state, v ); - b3StoreW( (float*)state + 4, w ); -} - -// This writes only the velocities back to the solver bodies -B3_FORCE_INLINE void b3ScatterBodies( b3BodyState* B3_RESTRICT states, const int* B3_RESTRICT indices, - const b3BodyStateW* B3_RESTRICT simdBody ) -{ - // I don't use any dummy body in the body array because this will lead to multithreaded sharing and the - // associated cache flushing. - b3FloatW zero = b3ZeroW(); - b3FloatW v1, v2, v3, v4; - b3TransposeW( simdBody->v.X, simdBody->v.Y, simdBody->v.Z, zero, &v1, &v2, &v3, &v4 ); - b3FloatW w1, w2, w3, w4; - b3TransposeW( simdBody->w.X, simdBody->w.Y, simdBody->w.Z, zero, &w1, &w2, &w3, &w4 ); - - b3StoreBodyVelocity( states, indices[0], v1, w1 ); - b3StoreBodyVelocity( states, indices[1], v2, w2 ); - b3StoreBodyVelocity( states, indices[2], v3, w3 ); - b3StoreBodyVelocity( states, indices[3], v4, w4 ); -} - -#else // non-simd - -B3_FORCE_INLINE b3BodyStateW b3GatherBodies( const b3BodyState* B3_RESTRICT states, const int* B3_RESTRICT 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 -B3_FORCE_INLINE void b3ScatterBodies( b3BodyState* B3_RESTRICT states, const int* B3_RESTRICT indices, - const b3BodyStateW* B3_RESTRICT 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 - -_Static_assert( offsetof( b3ManifoldPoint, anchorA ) == 0, "manifold point layout" ); -_Static_assert( offsetof( b3ManifoldPoint, anchorB ) == 12, "manifold point layout" ); -_Static_assert( offsetof( b3ManifoldPoint, separation ) == 24, "manifold point layout" ); -_Static_assert( offsetof( b3ManifoldPoint, normalImpulse ) == 28, "manifold point layout" ); -_Static_assert( offsetof( b3Manifold, twistImpulse ) == offsetof( b3Manifold, normal ) + 12, "manifold layout" ); -_Static_assert( offsetof( b3Manifold, frictionImpulse ) == offsetof( b3Manifold, normal ) + 16, "manifold layout" ); -_Static_assert( offsetof( b3Manifold, rollingImpulse ) == offsetof( b3Manifold, normal ) + 28, "manifold layout" ); -_Static_assert( offsetof( b3Manifold, pointCount ) == offsetof( b3Manifold, normal ) + 40, "manifold layout" ); -_Static_assert( offsetof( b3Matrix3, cy ) == 12 && offsetof( b3Matrix3, cz ) == 24 && sizeof( b3Matrix3 ) == 36, - "matrix layout" ); -_Static_assert( B3_SIMD_WIDTH == 4, "width" ); - -static const b3Contact b3_zeroContact = { 0 }; -static const b3Manifold b3_zeroManifold = { 0 }; -static const b3BodySim b3_zeroBodySim = { 0 }; - -#define B3_GATHER_LANES( wide, lanes, field ) wide = b3SetW( lanes[0]->field, lanes[1]->field, lanes[2]->field, lanes[3]->field ) - -static inline b3SymMatrix3W b3GatherInvInertiaW( const b3BodySim* const* simLanes ) -{ - const float* i0 = &simLanes[0]->invInertiaWorld.cx.x; - const float* i1 = &simLanes[1]->invInertiaWorld.cx.x; - const float* i2 = &simLanes[2]->invInertiaWorld.cx.x; - const float* i3 = &simLanes[3]->invInertiaWorld.cx.x; - - b3SymMatrix3W m; - b3FloatW unused; - b3TransposeW( b3LoadW( i0 ), b3LoadW( i1 ), b3LoadW( i2 ), b3LoadW( i3 ), &m.cxx, &m.cxy, &m.cxz, &unused ); - b3TransposeW( b3LoadW( i0 + 4 ), b3LoadW( i1 + 4 ), b3LoadW( i2 + 4 ), b3LoadW( i3 + 4 ), &m.cyy, &m.cyz, &unused, &unused ); - m.czz = b3SetW( i0[8], i1[8], i2[8], i3[8] ); - return m; -} - -static inline b3SymMatrix3W b3AddSymW( b3SymMatrix3W a, b3SymMatrix3W b ) -{ - return (b3SymMatrix3W){ - b3AddW( a.cxx, b.cxx ), b3AddW( a.cxy, b.cxy ), b3AddW( a.cxz, b.cxz ), - b3AddW( a.cyy, b.cyy ), b3AddW( a.cyz, b.cyz ), b3AddW( a.czz, b.czz ), - }; -} - -static inline b3SymMatrix3W b3InvertSymW( b3SymMatrix3W m ) -{ - b3FloatW cxx = b3SubW( b3MulW( m.cyy, m.czz ), b3MulW( m.cyz, m.cyz ) ); - b3FloatW cxy = b3SubW( b3MulW( m.cxz, m.cyz ), b3MulW( m.cxy, m.czz ) ); - b3FloatW cxz = b3SubW( b3MulW( m.cxy, m.cyz ), b3MulW( m.cxz, m.cyy ) ); - b3FloatW cyy = b3SubW( b3MulW( m.cxx, m.czz ), b3MulW( m.cxz, m.cxz ) ); - b3FloatW cyz = b3SubW( b3MulW( m.cxy, m.cxz ), b3MulW( m.cxx, m.cyz ) ); - b3FloatW czz = b3SubW( b3MulW( m.cxx, m.cyy ), b3MulW( m.cxy, m.cxy ) ); - - b3FloatW det = b3AddW( b3MulW( m.cxx, cxx ), b3AddW( b3MulW( m.cxy, cxy ), b3MulW( m.cxz, cxz ) ) ); - b3FloatW valid = b3GreaterThanW( b3AbsW( det ), b3SplatW( 1000.0f * FLT_MIN ) ); - b3FloatW invDet = b3BlendW( b3ZeroW(), b3DivW( b3SplatW( 1.0f ), det ), valid ); - - return (b3SymMatrix3W){ - b3MulW( invDet, cxx ), b3MulW( invDet, cxy ), b3MulW( invDet, cxz ), - b3MulW( invDet, cyy ), b3MulW( invDet, cyz ), b3MulW( invDet, czz ), - }; -} - -static inline b3Vec3W b3PerpW( b3Vec3W a ) -{ - b3FloatW zero = b3ZeroW(); - b3FloatW half = b3SplatW( 0.5f ); - b3FloatW mask = b3OrW( b3LessThanW( a.X, b3NegW( half ) ), b3GreaterThanW( a.X, half ) ); - - b3Vec3W p; - p.X = b3BlendW( zero, a.Y, mask ); - p.Y = b3BlendW( a.Z, b3NegW( a.X ), mask ); - p.Z = b3BlendW( b3NegW( a.Y ), zero, mask ); - - b3FloatW lengthSquared = b3DotW( p, p ); - b3FloatW valid = b3GreaterThanW( lengthSquared, b3SplatW( 1000.0f * FLT_MIN ) ); - b3FloatW s = b3BlendW( zero, b3DivW( b3SplatW( 1.0f ), b3SqrtW( lengthSquared ) ), valid ); - return b3MulSVW( s, p ); -} - -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; - - b3FloatW zeroW = b3ZeroW(); - b3FloatW oneW = b3SplatW( 1.0f ); - b3FloatW twoW = b3SplatW( 2.0f ); - b3FloatW warmStartScale = world->enableWarmStarting ? oneW : zeroW; - b3FloatW invTau = b3SplatW( 1.0f / B3_SPECULATIVE_DISTANCE ); - b3FloatW minFrictionWeight = b3SplatW( B3_MIN_FRICTION_WEIGHT ); - b3FloatW minDet = b3SplatW( 1000.0f * FLT_MIN ); - bool anyRestitution = false; - - 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* c = wideBase + wideIndex; - int localWideIndex = wideIndex - colorWideStart; - - const b3Contact* contactLanes[B3_SIMD_WIDTH]; - const b3Manifold* manifoldLanes[B3_SIMD_WIDTH]; - const b3BodySim* simLanesA[B3_SIMD_WIDTH]; - const b3BodySim* simLanesB[B3_SIMD_WIDTH]; - int hitEventLanes = 0; - - for ( int lane = 0; lane < B3_SIMD_WIDTH; ++lane ) - { - int contactIndex = B3_SIMD_WIDTH * localWideIndex + lane; - if ( contactIndex < colorContactCount ) - { - b3Contact* contact = b3Array_Get( world->contacts, contactIds[contactIndex] ); - B3_ASSERT( contact->manifoldCount == 1 ); - b3Manifold* manifold = contact->manifolds; - - int indexA = b3DecodeAwakeIndex( contact->encodedBodySimA ); - int indexB = b3DecodeAwakeIndex( contact->encodedBodySimB ); - -#if B3_ENABLE_VALIDATION - b3Body* bodyA = bodies + contact->edges[0].bodyId; - b3Body* bodyB = bodies + contact->edges[1].bodyId; - B3_ASSERT( contact->encodedBodySimA == b3EncodeBodySimIndex( bodyA ) ); - B3_ASSERT( contact->encodedBodySimB == b3EncodeBodySimIndex( bodyB ) ); -#endif - - c->indexA[lane] = indexA + 1; - c->indexB[lane] = indexB + 1; - c->pointCounts[lane] = manifold->pointCount; - c->manifolds[lane] = manifold; - - contactLanes[lane] = contact; - manifoldLanes[lane] = manifold; - simLanesA[lane] = indexA == B3_NULL_INDEX ? &b3_zeroBodySim : sims + indexA; - simLanesB[lane] = indexB == B3_NULL_INDEX ? &b3_zeroBodySim : sims + indexB; - hitEventLanes |= ( contact->flags & b3_simEnableHitEvent ) != 0 ? 1 << lane : 0; - } - else - { - c->indexA[lane] = 0; - c->indexB[lane] = 0; - c->pointCounts[lane] = 0; - c->manifolds[lane] = NULL; - - contactLanes[lane] = &b3_zeroContact; - manifoldLanes[lane] = &b3_zeroManifold; - simLanesA[lane] = &b3_zeroBodySim; - simLanesB[lane] = &b3_zeroBodySim; - } - } - - b3FloatW mA, mB; - B3_GATHER_LANES( mA, simLanesA, invMass ); - B3_GATHER_LANES( mB, simLanesB, invMass ); - b3SymMatrix3W iA = b3GatherInvInertiaW( simLanesA ); - b3SymMatrix3W iB = b3GatherInvInertiaW( simLanesB ); - c->invMassA = mA; - c->invMassB = mB; - c->invIA = iA; - c->invIB = iB; - - b3Vec3W tangentVelocity; - B3_GATHER_LANES( c->friction, contactLanes, friction ); - B3_GATHER_LANES( c->rollingResistance, contactLanes, rollingResistance ); - B3_GATHER_LANES( c->restitution, contactLanes, restitution ); - B3_GATHER_LANES( tangentVelocity.X, contactLanes, tangentVelocity.x ); - B3_GATHER_LANES( tangentVelocity.Y, contactLanes, tangentVelocity.y ); - B3_GATHER_LANES( tangentVelocity.Z, contactLanes, tangentVelocity.z ); - - b3Vec3W normal, frictionImpulse, rollingImpulse; - b3FloatW twistImpulse; - { - const float* m0 = &manifoldLanes[0]->normal.x; - const float* m1 = &manifoldLanes[1]->normal.x; - const float* m2 = &manifoldLanes[2]->normal.x; - const float* m3 = &manifoldLanes[3]->normal.x; - b3TransposeW( b3LoadW( m0 ), b3LoadW( m1 ), b3LoadW( m2 ), b3LoadW( m3 ), &normal.X, &normal.Y, &normal.Z, - &twistImpulse ); - b3TransposeW( b3LoadW( m0 + 4 ), b3LoadW( m1 + 4 ), b3LoadW( m2 + 4 ), b3LoadW( m3 + 4 ), &frictionImpulse.X, - &frictionImpulse.Y, &frictionImpulse.Z, &rollingImpulse.X ); - rollingImpulse.Y = b3SetW( m0[8], m1[8], m2[8], m3[8] ); - rollingImpulse.Z = b3SetW( m0[9], m1[9], m2[9], m3[9] ); - } - - b3Vec3W tangent1 = b3PerpW( normal ); - b3Vec3W tangent2 = b3CrossW( tangent1, normal ); - c->normal = normal; - c->tangent1 = tangent1; - c->tangent2 = tangent2; - c->tangentVelocity1 = b3DotW( tangentVelocity, tangent1 ); - c->tangentVelocity2 = b3DotW( tangentVelocity, tangent2 ); - - b3FloatW rollingMask = b3GreaterThanW( c->rollingResistance, zeroW ); - c->twistImpulse = b3MulW( warmStartScale, twistImpulse ); - c->rollingImpulse.X = b3BlendW( zeroW, b3MulW( warmStartScale, rollingImpulse.X ), rollingMask ); - c->rollingImpulse.Y = b3BlendW( zeroW, b3MulW( warmStartScale, rollingImpulse.Y ), rollingMask ); - c->rollingImpulse.Z = b3BlendW( zeroW, b3MulW( warmStartScale, rollingImpulse.Z ), rollingMask ); - c->frictionImpulse.x = b3MulW( warmStartScale, b3DotW( frictionImpulse, tangent1 ) ); - c->frictionImpulse.y = b3MulW( warmStartScale, b3DotW( frictionImpulse, tangent2 ) ); - - b3FloatW pointCountW = - b3SetW( (float)c->pointCounts[0], (float)c->pointCounts[1], (float)c->pointCounts[2], (float)c->pointCounts[3] ); - - b3Vec3W centerA = { zeroW, zeroW, zeroW }; - b3Vec3W centerB = { zeroW, zeroW, zeroW }; - b3FloatW totalFrictionWeight = zeroW; - - for ( int pointIndex = 0; pointIndex < B3_MAX_MANIFOLD_POINTS; ++pointIndex ) - { - b3ContactConstraintPointWide* cp = c->points + pointIndex; - b3FloatW pointMask = b3GreaterThanW( pointCountW, b3SplatW( (float)pointIndex ) ); - - const float* p0 = (const float*)( manifoldLanes[0]->points + pointIndex ); - const float* p1 = (const float*)( manifoldLanes[1]->points + pointIndex ); - const float* p2 = (const float*)( manifoldLanes[2]->points + pointIndex ); - const float* p3 = (const float*)( manifoldLanes[3]->points + pointIndex ); - - b3Vec3W rA, rB; - b3FloatW separation, normalImpulse; - b3TransposeW( b3LoadW( p0 ), b3LoadW( p1 ), b3LoadW( p2 ), b3LoadW( p3 ), &rA.X, &rA.Y, &rA.Z, &rB.X ); - b3TransposeW( b3LoadW( p0 + 4 ), b3LoadW( p1 + 4 ), b3LoadW( p2 + 4 ), b3LoadW( p3 + 4 ), &rB.Y, &rB.Z, - &separation, &normalImpulse ); - - rA.X = b3BlendW( zeroW, rA.X, pointMask ); - rA.Y = b3BlendW( zeroW, rA.Y, pointMask ); - rA.Z = b3BlendW( zeroW, rA.Z, pointMask ); - rB.X = b3BlendW( zeroW, rB.X, pointMask ); - rB.Y = b3BlendW( zeroW, rB.Y, pointMask ); - rB.Z = b3BlendW( zeroW, rB.Z, pointMask ); - separation = b3BlendW( zeroW, separation, pointMask ); - normalImpulse = b3BlendW( zeroW, normalImpulse, pointMask ); - - // C0 friction center decay. Needed to prevent spinning top drift (GyroscopicPrecession sample). - // See details in b3PrepareContacts_Mesh. This code should stay in sync. - b3FloatW weight = b3MinW( b3MaxW( b3SubW( twoW, b3MulW( separation, invTau ) ), minFrictionWeight ), oneW ); - weight = b3BlendW( zeroW, weight, pointMask ); - centerA = b3MulAddSVW( centerA, weight, rA ); - centerB = b3MulAddSVW( centerB, weight, rB ); - totalFrictionWeight = b3AddW( totalFrictionWeight, weight ); - - cp->anchorAs = rA; - cp->anchorBs = rB; - cp->baseSeparations = b3SubW( separation, b3DotW( b3SubVW( rB, rA ), normal ) ); - cp->normalImpulses = b3MulW( warmStartScale, normalImpulse ); - cp->totalNormalImpulses = zeroW; - cp->relativeVelocities = zeroW; - cp->restitutionImpulses = zeroW; - - b3Vec3W rnA = b3CrossW( rA, normal ); - b3Vec3W rnB = b3CrossW( rB, normal ); - b3FloatW kNormal = b3AddW( mA, mB ); - kNormal = b3AddW( kNormal, b3DotW( rnA, b3MulMVW( iA, rnA ) ) ); - kNormal = b3AddW( kNormal, b3DotW( rnB, b3MulMVW( iB, rnB ) ) ); - b3FloatW valid = b3AndW( b3GreaterThanW( kNormal, zeroW ), pointMask ); - cp->normalMasses = b3BlendW( zeroW, b3DivW( oneW, kNormal ), valid ); - } - - b3FloatW invWeight = - b3BlendW( zeroW, b3DivW( oneW, totalFrictionWeight ), b3GreaterThanW( totalFrictionWeight, zeroW ) ); - centerA = b3MulSVW( invWeight, centerA ); - centerB = b3MulSVW( invWeight, centerB ); - c->centerA = centerA; - c->centerB = centerB; - - for ( int pointIndex = 0; pointIndex < B3_MAX_MANIFOLD_POINTS; ++pointIndex ) - { - b3ContactConstraintPointWide* cp = c->points + pointIndex; - b3FloatW pointMask = b3GreaterThanW( pointCountW, b3SplatW( (float)pointIndex ) ); - b3Vec3W d = b3SubVW( cp->anchorAs, centerA ); - cp->leverArms = b3BlendW( zeroW, b3SqrtW( b3DotW( d, d ) ), pointMask ); - } - - { - b3Vec3W rtA1 = b3CrossW( centerA, tangent1 ); - b3Vec3W rtA2 = b3CrossW( centerA, tangent2 ); - b3Vec3W rtB1 = b3CrossW( centerB, tangent1 ); - b3Vec3W rtB2 = b3CrossW( centerB, tangent2 ); - b3Vec3W iArtA1 = b3MulMVW( iA, rtA1 ); - b3Vec3W iArtA2 = b3MulMVW( iA, rtA2 ); - b3Vec3W iBrtB1 = b3MulMVW( iB, rtB1 ); - b3Vec3W iBrtB2 = b3MulMVW( iB, rtB2 ); - - b3FloatW kxx = b3AddW( b3AddW( b3AddW( mA, mB ), b3DotW( rtA1, iArtA1 ) ), b3DotW( rtB1, iBrtB1 ) ); - b3FloatW kyy = b3AddW( b3AddW( b3AddW( mA, mB ), b3DotW( rtA2, iArtA2 ) ), b3DotW( rtB2, iBrtB2 ) ); - b3FloatW kxy = b3AddW( b3DotW( rtA1, iArtA2 ), b3DotW( rtB1, iBrtB2 ) ); - - b3FloatW det = b3SubW( b3MulW( kxx, kyy ), b3MulW( kxy, kxy ) ); - b3FloatW valid = b3GreaterThanW( b3AbsW( det ), minDet ); - b3FloatW invDet = b3BlendW( zeroW, b3DivW( oneW, det ), valid ); - c->tangentMass.cxx = b3MulW( invDet, kyy ); - c->tangentMass.cxy = b3NegW( b3MulW( invDet, kxy ) ); - c->tangentMass.cyy = b3MulW( invDet, kxx ); - } - - b3SymMatrix3W invIAB = b3AddSymW( iA, iB ); - - { - b3FloatW kTwist = b3DotW( normal, b3MulMVW( invIAB, normal ) ); - c->twistMass = b3BlendW( zeroW, b3DivW( oneW, kTwist ), b3GreaterThanW( kTwist, zeroW ) ); - } - - if ( b3AllZeroW( c->rollingResistance ) == false ) - { - c->rollingMass = b3InvertSymW( invIAB ); - } - else - { - c->rollingMass = (b3SymMatrix3W){ zeroW, zeroW, zeroW, zeroW, zeroW, zeroW }; - } - - // Only sample contact point normal velocity if needed. - bool haveRestitution = b3AnyTrueW( b3GreaterThanW( c->restitution, zeroW ) ); - anyRestitution = anyRestitution || haveRestitution; - - if ( hitEventLanes != 0 || haveRestitution ) - { - b3BodyStateW bA = b3GatherBodies( states, c->indexA ); - b3BodyStateW bB = b3GatherBodies( states, c->indexB ); - - for ( int pointIndex = 0; pointIndex < B3_MAX_MANIFOLD_POINTS; ++pointIndex ) - { - b3ContactConstraintPointWide* cp = c->points + pointIndex; - - b3Vec3W vrA = b3AddVW( bA.v, b3CrossW( bA.w, cp->anchorAs ) ); - b3Vec3W vrB = b3AddVW( bB.v, b3CrossW( bB.w, cp->anchorBs ) ); - b3FloatW vn = b3DotW( normal, b3SubVW( vrB, vrA ) ); - cp->relativeVelocities = vn; - - if ( hitEventLanes != 0 ) - { - float normalVelocities[B3_SIMD_WIDTH]; - b3StoreW( normalVelocities, vn ); - - for ( int lane = 0; lane < B3_SIMD_WIDTH; ++lane ) - { - if ( ( hitEventLanes & ( 1 << lane ) ) != 0 && pointIndex < c->pointCounts[lane] ) - { - c->manifolds[lane]->points[pointIndex].normalVelocity = normalVelocities[lane]; - } - } - } - } - } - } - - // Advance to next color - colorIndex += 1; - } - - if ( anyRestitution ) - { - b3AtomicStoreInt( &context->anyRestitution, 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 ); - - _Static_assert( B3_SIMD_WIDTH == 4, "width" ); - int pointCount1 = b3MaxInt( c->pointCounts[0], c->pointCounts[1] ); - int pointCount2 = b3MaxInt( c->pointCounts[2], c->pointCounts[3] ); - int pointCount = b3MaxInt( pointCount1, pointCount2 ); - B3_VALIDATE( 0 < pointCount && pointCount <= B3_MAX_MANIFOLD_POINTS ); - - b3FloatW zeroW = b3ZeroW(); - b3FloatW totalNormalImpulse = zeroW; - b3Vec3W momentA = { zeroW, zeroW, zeroW }; - b3Vec3W momentB = { zeroW, zeroW, zeroW }; - - for ( int j = 0; j < pointCount; ++j ) - { - b3ContactConstraintPointWide* cp = c->points + j; - b3FloatW normalImpulse = cp->normalImpulses; - totalNormalImpulse = b3AddW( totalNormalImpulse, normalImpulse ); - momentA = b3MulAddSVW( momentA, normalImpulse, cp->anchorAs ); - momentB = b3MulAddSVW( momentB, normalImpulse, cp->anchorBs ); - } - - b3Vec3W normal = c->normal; - b3Vec3W frictionImpulse = b3MulSVW( c->frictionImpulse.x, c->tangent1 ); - frictionImpulse = b3MulAddSVW( frictionImpulse, c->frictionImpulse.y, c->tangent2 ); - - b3Vec3W linearImpulse = b3MulAddSVW( frictionImpulse, totalNormalImpulse, normal ); - - b3Vec3W twistImpulse = b3MulSVW( c->twistImpulse, normal ); - b3Vec3W angularImpulseA = b3AddVW( b3AddVW( b3CrossW( momentA, normal ), b3CrossW( c->centerA, frictionImpulse ) ), twistImpulse ); - b3Vec3W angularImpulseB = b3AddVW( b3AddVW( b3CrossW( momentB, normal ), b3CrossW( c->centerB, frictionImpulse ) ), twistImpulse ); - - if ( b3AllZeroW( c->rollingResistance ) == false ) - { - angularImpulseA = b3AddVW( angularImpulseA, c->rollingImpulse ); - angularImpulseB = b3AddVW( angularImpulseB, c->rollingImpulse ); - } - - bA.w = b3MulSubMVW( bA.w, c->invIA, angularImpulseA ); - bA.v = b3MulSubSVW( bA.v, c->invMassA, linearImpulse ); - bB.w = b3MulAddMVW( bB.w, c->invIB, angularImpulseB ); - bB.v = b3MulAddSVW( bB.v, c->invMassB, linearImpulse ); - - b3ScatterBodies( states, c->indexA, &bA ); - b3ScatterBodies( states, c->indexB, &bB ); - } - - b3TracyCZoneEnd( warm_start_contact ); -} - -// Solve the non-penetration constraints with the soft bias. No friction and no restitution. -void b3PushContacts_Convex( b3SolverBlock block, b3StepContext* context ) -{ - b3TracyCZoneNC( push_contact, "Push 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 ); - - // Stiffer for static contacts to avoid bodies getting pushed through the ground. Selected per - // lane from the null body index instead of stored per constraint. - b3FloatW dynamicBiasRate = b3SplatW( context->contactSoftness.massScale * context->contactSoftness.biasRate ); - b3FloatW dynamicMassScale = b3SplatW( context->contactSoftness.massScale ); - b3FloatW dynamicImpulseScale = b3SplatW( context->contactSoftness.impulseScale ); - b3FloatW staticBiasRate = b3SplatW( context->staticSoftness.massScale * context->staticSoftness.biasRate ); - b3FloatW staticMassScale = b3SplatW( context->staticSoftness.massScale ); - b3FloatW staticImpulseScale = b3SplatW( context->staticSoftness.impulseScale ); - - for ( int wideIndex = block.startIndex; wideIndex < block.startIndex + block.count; ++wideIndex ) - { - b3ContactConstraintWide* c = constraints + wideIndex; - - _Static_assert( B3_SIMD_WIDTH == 4, "width" ); - int pointCount1 = b3MaxInt( c->pointCounts[0], c->pointCounts[1] ); - int pointCount2 = b3MaxInt( c->pointCounts[2], c->pointCounts[3] ); - int pointCount = b3MaxInt( pointCount1, pointCount2 ); - B3_VALIDATE( 0 < pointCount && pointCount <= B3_MAX_MANIFOLD_POINTS ); - - b3BodyStateW bA = b3GatherBodies( states, c->indexA ); - b3BodyStateW bB = b3GatherBodies( states, c->indexB ); - - b3FloatW softMask = b3SoftMaskW( c->indexA, c->indexB ); - b3FloatW biasRate = b3BlendW( dynamicBiasRate, staticBiasRate, softMask ); - b3FloatW massScale = b3BlendW( dynamicMassScale, staticMassScale, softMask ); - b3FloatW impulseScale = b3BlendW( dynamicImpulseScale, staticImpulseScale, softMask ); - - b3Vec3W dp = b3SubVW( bB.dp, bA.dp ); - - // Convert to normals to local space to reduce transform math. - b3FloatW normalSeparation = b3DotW( c->normal, dp ); - b3Vec3W normalA = b3InvRotateVectorW( bA.dq, c->normal ); - b3Vec3W normalB = b3InvRotateVectorW( bB.dq, c->normal ); - - for ( int pointIndex = 0; pointIndex < pointCount; ++pointIndex ) - { - b3ContactConstraintPointWide* cp = c->points + pointIndex; - - // Fixed anchor points for applying impulses - b3Vec3W rA = cp->anchorAs; - b3Vec3W rB = cp->anchorBs; - - b3FloatW s = b3AddW( b3AddW( normalSeparation, b3SubW( b3DotW( normalB, rB ), b3DotW( normalA, rA ) ) ), - cp->baseSeparations ); - - // Apply speculative bias if separation is greater than zero, otherwise apply soft constraint bias - b3FloatW separated = b3GreaterThanW( s, b3ZeroW() ); - - // Speculative bias - positive - b3FloatW specBias = b3MulW( s, inv_h ); - - // Overlap bias - negative - b3FloatW overlapBias = b3MaxW( b3MulW( biasRate, s ), contactSpeed ); - b3FloatW velocityBias = b3BlendW( overlapBias, specBias, separated ); - - b3FloatW pointMassScale = b3BlendW( massScale, oneW, separated ); - b3FloatW pointImpulseScale = b3BlendW( impulseScale, b3ZeroW(), separated ); - - // 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 ), velocityBias ) ), - 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; - - // 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( push_contact ); -} - -// Solve the normal constraint, friction, and rolling resistance. -void b3SolveContacts_Convex( b3SolverBlock block, b3StepContext* context ) -{ - 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 oneW = b3SplatW( 1.0f ); - b3FloatW epsilonW = b3SplatW( FLT_EPSILON ); - - for ( int wideIndex = block.startIndex; wideIndex < block.startIndex + block.count; ++wideIndex ) - { - b3ContactConstraintWide* c = constraints + wideIndex; - - _Static_assert( B3_SIMD_WIDTH == 4, "width" ); - int pointCount1 = b3MaxInt( c->pointCounts[0], c->pointCounts[1] ); - int pointCount2 = b3MaxInt( c->pointCounts[2], c->pointCounts[3] ); - int pointCount = b3MaxInt( pointCount1, pointCount2 ); - B3_VALIDATE( 0 < pointCount && pointCount <= B3_MAX_MANIFOLD_POINTS ); - - b3BodyStateW bA = b3GatherBodies( states, c->indexA ); - b3BodyStateW bB = b3GatherBodies( states, c->indexB ); - - b3Vec3W dp = b3SubVW( bB.dp, bA.dp ); - b3FloatW normalSeparation = b3DotW( c->normal, dp ); - b3Vec3W normalA = b3InvRotateVectorW( bA.dq, c->normal ); - b3Vec3W normalB = b3InvRotateVectorW( bB.dq, c->normal ); - - b3FloatW totalNormalImpulse = b3ZeroW(); - b3FloatW totalTwistLimit = b3ZeroW(); - - for ( int pointIndex = 0; pointIndex < pointCount; ++pointIndex ) - { - b3ContactConstraintPointWide* cp = c->points + pointIndex; - - // Fixed anchor points for applying impulses - b3Vec3W rA = cp->anchorAs; - b3Vec3W rB = cp->anchorBs; - - b3FloatW s = b3AddW( b3AddW( normalSeparation, b3SubW( b3DotW( normalB, rB ), b3DotW( normalA, rA ) ) ), - cp->baseSeparations ); - - // Speculative bias, positive if separated and zero if overlapped - b3FloatW velocityBias = b3MaxW( b3ZeroW(), b3MulW( s, inv_h ) ); - - // 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( cp->normalMasses, b3AddW( vn, velocityBias ) ); - - // 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 ); - } - - // 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->centerA; - b3Vec3W rB = c->centerB; - - // 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 inv_h = b3SplatW( context->inv_h ); - b3FloatW negRestitutionThreshold = b3SplatW( -context->world->restitutionThreshold ); - b3FloatW zeroW = b3ZeroW(); - bool propagate = context->world->enableRestitutionPropagation; - - for ( int wideIndex = block.startIndex; wideIndex < block.startIndex + block.count; ++wideIndex ) - { - b3ContactConstraintWide* c = constraints + wideIndex; - if ( propagate == false && b3AllZeroW( c->restitution ) ) - { - continue; - } - - _Static_assert( B3_SIMD_WIDTH == 4, "width" ); - int pointCount1 = b3MaxInt( c->pointCounts[0], c->pointCounts[1] ); - int pointCount2 = b3MaxInt( c->pointCounts[2], c->pointCounts[3] ); - int pointCount = b3MaxInt( pointCount1, pointCount2 ); - B3_VALIDATE( 0 < pointCount && pointCount <= B3_MAX_MANIFOLD_POINTS ); - - b3BodyStateW bA = b3GatherBodies( states, c->indexA ); - b3BodyStateW bB = b3GatherBodies( states, c->indexB ); - - b3FloatW restitutionMask = b3GreaterThanW( c->restitution, zeroW ); - b3Vec3W dp = b3SubVW( bB.dp, bA.dp ); - b3Matrix3W dqA = b3MakeMatrixFromQuatW( bA.dq ); - b3Matrix3W dqB = b3MakeMatrixFromQuatW( bB.dq ); - - for ( int pointIndex = 0; pointIndex < pointCount; ++pointIndex ) - { - b3ContactConstraintPointWide* cp = c->points + pointIndex; - - b3Vec3W rA = cp->anchorAs; - b3Vec3W rB = cp->anchorBs; - - b3FloatW normalMass = propagate ? cp->normalMasses : b3BlendW( zeroW, cp->normalMasses, restitutionMask ); - - b3FloatW compressionImpulse = b3SubW( cp->totalNormalImpulses, cp->restitutionImpulses ); - b3FloatW armed = b3AndW( b3AndW( restitutionMask, b3LessThanW( cp->relativeVelocities, negRestitutionThreshold ) ), - b3GreaterThanW( compressionImpulse, zeroW ) ); - - b3Vec3W rsA = b3MulM3VW( dqA, rA ); - b3Vec3W rsB = b3MulM3VW( dqB, rB ); - b3Vec3W ds = b3AddVW( dp, b3SubVW( rsB, rsA ) ); - b3FloatW s = b3AddW( b3DotW( c->normal, ds ), cp->baseSeparations ); - - b3FloatW specBias = b3MaxW( zeroW, b3MulW( s, inv_h ) ); - b3FloatW velocityBias = b3BlendW( specBias, b3MulW( c->restitution, cp->relativeVelocities ), armed ); - - 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 ); - - b3FloatW negImpulse = b3MulW( normalMass, b3AddW( vn, velocityBias ) ); - - b3FloatW newImpulse = b3MaxW( b3SubW( cp->normalImpulses, negImpulse ), zeroW ); - b3FloatW impulse = b3SubW( newImpulse, cp->normalImpulses ); - - b3FloatW approachImpulse = b3MinW( b3MaxW( b3NegW( b3MulW( normalMass, vn ) ), zeroW ), b3MaxW( impulse, zeroW ) ); - b3FloatW allowance = - b3SubW( b3MulW( c->restitution, b3AddW( compressionImpulse, approachImpulse ) ), cp->restitutionImpulses ); - b3FloatW maxImpulse = b3AddW( approachImpulse, b3MaxW( allowance, zeroW ) ); - impulse = b3BlendW( impulse, b3MinW( impulse, maxImpulse ), armed ); - - cp->normalImpulses = b3AddW( cp->normalImpulses, impulse ); - cp->restitutionImpulses = b3AddW( cp->restitutionImpulses, b3SubW( impulse, approachImpulse ) ); - cp->totalNormalImpulses = b3AddW( cp->totalNormalImpulses, impulse ); - - b3Vec3W P = b3MulSVW( impulse, 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; - - b3ManifoldPoint* mp = m->points + pointIndex; - mp->normalImpulse = normalImpulse[lane]; - mp->totalNormalImpulse = totalNormalImpulse[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; @@ -2335,3 +1056,95 @@ void b3StoreImpulses_Overflow( b3StepContext* context ) b3StoreImpulses_Mesh( block, context, 0 ); } + +void b3PrepareContacts_Convex( b3SolverBlock block, b3StepContext* context ) +{ +#if defined( B3_SIMD_HAS_WIDTH_8 ) + if ( context->world->simdWidth == 8 ) + { + b3PrepareContacts_ConvexW8( block, context ); + return; + } +#endif + + b3PrepareContacts_ConvexW4( block, context ); +} + +void b3WarmStartContacts_Convex( b3SolverBlock block, b3StepContext* context ) +{ +#if defined( B3_SIMD_HAS_WIDTH_8 ) + if ( context->world->simdWidth == 8 ) + { + b3WarmStartContacts_ConvexW8( block, context ); + return; + } +#endif + + b3WarmStartContacts_ConvexW4( block, context ); +} + +void b3PushContacts_Convex( b3SolverBlock block, b3StepContext* context ) +{ +#if defined( B3_SIMD_HAS_WIDTH_8 ) + if ( context->world->simdWidth == 8 ) + { + b3PushContacts_ConvexW8( block, context ); + return; + } +#endif + + b3PushContacts_ConvexW4( block, context ); +} + +void b3SolveContacts_Convex( b3SolverBlock block, b3StepContext* context ) +{ +#if defined( B3_SIMD_HAS_WIDTH_8 ) + if ( context->world->simdWidth == 8 ) + { + b3SolveContacts_ConvexW8( block, context ); + return; + } +#endif + + b3SolveContacts_ConvexW4( block, context ); +} + +void b3ApplyRestitution_Convex( b3SolverBlock block, b3StepContext* context ) +{ +#if defined( B3_SIMD_HAS_WIDTH_8 ) + if ( context->world->simdWidth == 8 ) + { + b3ApplyRestitution_ConvexW8( block, context ); + return; + } +#endif + + b3ApplyRestitution_ConvexW4( block, context ); +} + +void b3StoreImpulses_Convex( b3SolverBlock block, b3StepContext* context, int workerIndex ) +{ +#if defined( B3_SIMD_HAS_WIDTH_8 ) + if ( context->world->simdWidth == 8 ) + { + b3StoreImpulses_ConvexW8( block, context, workerIndex ); + return; + } +#endif + + b3StoreImpulses_ConvexW4( block, context, workerIndex ); +} + +int b3GetWideContactConstraintByteCount( int simdWidth ) +{ +#if defined( B3_SIMD_HAS_WIDTH_8 ) + if ( simdWidth == 8 ) + { + return b3GetWideContactConstraintByteCountW8(); + } +#else + B3_UNUSED( simdWidth ); +#endif + + return b3GetWideContactConstraintByteCountW4(); +} diff --git a/src/contact_solver.h b/src/contact_solver.h index 2ca02f1..c1cec85 100644 --- a/src/contact_solver.h +++ b/src/contact_solver.h @@ -52,7 +52,7 @@ typedef struct b3ContactConstraint int manifoldCount; } b3ContactConstraint; -int b3GetWideContactConstraintByteCount( void ); +int b3GetWideContactConstraintByteCount( int simdWidth ); // Overflow contacts don't fit into the constraint graph coloring void b3PrepareContacts_Overflow( b3StepContext* context ); @@ -75,3 +75,19 @@ void b3PushContacts_Convex( b3SolverBlock block, b3StepContext* context ); void b3SolveContacts_Convex( b3SolverBlock block, b3StepContext* context ); void b3ApplyRestitution_Convex( b3SolverBlock block, b3StepContext* context ); void b3StoreImpulses_Convex( b3SolverBlock block, b3StepContext* context, int workerIndex ); + +void b3PrepareContacts_ConvexW4( b3SolverBlock block, b3StepContext* context ); +void b3WarmStartContacts_ConvexW4( b3SolverBlock block, b3StepContext* context ); +void b3PushContacts_ConvexW4( b3SolverBlock block, b3StepContext* context ); +void b3SolveContacts_ConvexW4( b3SolverBlock block, b3StepContext* context ); +void b3ApplyRestitution_ConvexW4( b3SolverBlock block, b3StepContext* context ); +void b3StoreImpulses_ConvexW4( b3SolverBlock block, b3StepContext* context, int workerIndex ); +int b3GetWideContactConstraintByteCountW4( void ); + +void b3PrepareContacts_ConvexW8( b3SolverBlock block, b3StepContext* context ); +void b3WarmStartContacts_ConvexW8( b3SolverBlock block, b3StepContext* context ); +void b3PushContacts_ConvexW8( b3SolverBlock block, b3StepContext* context ); +void b3SolveContacts_ConvexW8( b3SolverBlock block, b3StepContext* context ); +void b3ApplyRestitution_ConvexW8( b3SolverBlock block, b3StepContext* context ); +void b3StoreImpulses_ConvexW8( b3SolverBlock block, b3StepContext* context, int workerIndex ); +int b3GetWideContactConstraintByteCountW8( void ); diff --git a/src/contact_solver_wide.inl b/src/contact_solver_wide.inl new file mode 100644 index 0000000..e00e571 --- /dev/null +++ b/src/contact_solver_wide.inl @@ -0,0 +1,1426 @@ +// SPDX-FileCopyrightText: 2026 Erin Catto +// SPDX-License-Identifier: MIT + +#include "body.h" +#include "contact_solver.h" +#include "platform.h" +#include "simd.h" +#include "simd_wide.h" + +#define b3Vec2W B3_WIDE( b3Vec2 ) +#define b3QuatW B3_WIDE( b3Quat ) +#define b3SymMatrix2W B3_WIDE( b3SymMatrix2 ) +#define b3SymMatrix3W B3_WIDE( b3SymMatrix3 ) +#define b3Matrix3W B3_WIDE( b3Matrix3 ) +#define b3BodyStateW B3_WIDE( b3BodyState ) +#define b3ContactConstraintPointWide B3_WIDE( b3ContactConstraintPoint ) +#define b3ContactConstraintWide B3_WIDE( b3ContactConstraint ) + +// Wide vec2 +typedef struct b3Vec2W +{ + b3FloatW x, y; +} b3Vec2W; + +// 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; + +typedef struct b3Matrix3W +{ + b3Vec3W cx, cy, cz; +} b3Matrix3W; + +// a + b +static inline b3Vec2W b3AddV2W( b3Vec2W a, b3Vec2W b ) +{ + return (b3Vec2W){ + b3AddW( a.x, b.x ), + b3AddW( a.y, b.y ), + }; +} + +// 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 b3Matrix3W b3MakeMatrixFromQuatW( b3QuatW q ) +{ + b3FloatW x2 = b3AddW( q.V.X, q.V.X ); + b3FloatW y2 = b3AddW( q.V.Y, q.V.Y ); + b3FloatW z2 = b3AddW( q.V.Z, q.V.Z ); + b3FloatW xx2 = b3MulW( q.V.X, x2 ); + b3FloatW yy2 = b3MulW( q.V.Y, y2 ); + b3FloatW zz2 = b3MulW( q.V.Z, z2 ); + b3FloatW xy2 = b3MulW( q.V.X, y2 ); + b3FloatW xz2 = b3MulW( q.V.X, z2 ); + b3FloatW yz2 = b3MulW( q.V.Y, z2 ); + b3FloatW xw2 = b3MulW( q.S, x2 ); + b3FloatW yw2 = b3MulW( q.S, y2 ); + b3FloatW zw2 = b3MulW( q.S, z2 ); + b3FloatW one = b3SplatW( 1.0f ); + + b3Matrix3W m; + m.cx.X = b3SubW( one, b3AddW( yy2, zz2 ) ); + m.cx.Y = b3AddW( xy2, zw2 ); + m.cx.Z = b3SubW( xz2, yw2 ); + m.cy.X = b3SubW( xy2, zw2 ); + m.cy.Y = b3SubW( one, b3AddW( xx2, zz2 ) ); + m.cy.Z = b3AddW( yz2, xw2 ); + m.cz.X = b3AddW( xz2, yw2 ); + m.cz.Y = b3SubW( yz2, xw2 ); + m.cz.Z = b3SubW( one, b3AddW( xx2, yy2 ) ); + return m; +} + +static inline b3Vec3W b3MulM3VW( b3Matrix3W m, b3Vec3W a ) +{ + b3Vec3W b = { + b3AddW( b3MulW( m.cx.X, a.X ), b3AddW( b3MulW( m.cy.X, a.Y ), b3MulW( m.cz.X, a.Z ) ) ), + b3AddW( b3MulW( m.cx.Y, a.X ), b3AddW( b3MulW( m.cy.Y, a.Y ), b3MulW( m.cz.Y, a.Z ) ) ), + b3AddW( b3MulW( m.cx.Z, a.X ), b3AddW( b3MulW( m.cy.Z, a.Y ), b3MulW( m.cz.Z, a.Z ) ) ), + }; + + return b; +} + +static inline b3Vec3W b3InvRotateVectorW( b3QuatW q, b3Vec3W a ) +{ + b3Vec3W t = b3CrossW( q.V, a ); + t = (b3Vec3W){ b3AddW( t.X, t.X ), b3AddW( t.Y, t.Y ), b3AddW( t.Z, t.Z ) }; + b3Vec3W u = b3CrossW( q.V, t ); + return (b3Vec3W){ + b3AddW( b3SubW( a.X, b3MulW( q.S, t.X ) ), u.X ), + b3AddW( b3SubW( a.Y, b3MulW( q.S, t.Y ) ), u.Y ), + b3AddW( b3SubW( a.Z, b3MulW( q.S, t.Z ) ), u.Z ), + }; +} + +// 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; + b3FloatW restitutionImpulses; +} b3ContactConstraintPointWide; + +// Solves one contact per lane +typedef struct b3ContactConstraintWide +{ + // These are base 1 + int indexA[B3_SIMD_WIDTH]; + int indexB[B3_SIMD_WIDTH]; + + int pointCounts[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; + + // Friction centers + b3Vec3W centerA, centerB; + b3FloatW twistMass; + b3FloatW twistImpulse; + b3SymMatrix2W tangentMass; + b3Vec2W frictionImpulse; + b3SymMatrix3W rollingMass; + b3Vec3W rollingImpulse; + b3FloatW friction; + b3FloatW rollingResistance; + b3FloatW tangentVelocity1; + b3FloatW tangentVelocity2; + 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 B3_WIDE( 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_NONE ) + +B3_FORCE_INLINE b3BodyStateW b3GatherBodies( const b3BodyState* B3_RESTRICT states, const int* B3_RESTRICT 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 +B3_FORCE_INLINE void b3ScatterBodies( b3BodyState* B3_RESTRICT states, const int* B3_RESTRICT indices, + const b3BodyStateW* B3_RESTRICT 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; + } +} + +#else + +_Static_assert( sizeof( b3BodyState ) == 64, "body state layout" ); +_Static_assert( offsetof( b3BodyState, linearVelocity ) == 0 && offsetof( b3BodyState, angularVelocity ) == 16 && + offsetof( b3BodyState, deltaPosition ) == 32 && offsetof( b3BodyState, deltaRotation ) == 48, + "body state layout" ); + +#if B3_SIMD_WIDTH == 8 + +B3_FORCE_INLINE b3BodyStateW b3GatherBodies( const b3BodyState* B3_RESTRICT states, const int* B3_RESTRICT indices ) +{ + const float* identity = (const float*)&b3_identityBodyState; + + // Indices are 0 for null + const float* p1 = indices[0] == 0 ? identity : (const float*)( states + indices[0] - 1 ); + const float* p2 = indices[1] == 0 ? identity : (const float*)( states + indices[1] - 1 ); + const float* p3 = indices[2] == 0 ? identity : (const float*)( states + indices[2] - 1 ); + const float* p4 = indices[3] == 0 ? identity : (const float*)( states + indices[3] - 1 ); + const float* p5 = indices[4] == 0 ? identity : (const float*)( states + indices[4] - 1 ); + const float* p6 = indices[5] == 0 ? identity : (const float*)( states + indices[5] - 1 ); + const float* p7 = indices[6] == 0 ? identity : (const float*)( states + indices[6] - 1 ); + const float* p8 = indices[7] == 0 ? identity : (const float*)( states + indices[7] - 1 ); + + b3BodyStateW s; + b3FloatW pad; + + b3TransposeW( b3LoadW( p1 ), b3LoadW( p2 ), b3LoadW( p3 ), b3LoadW( p4 ), b3LoadW( p5 ), b3LoadW( p6 ), b3LoadW( p7 ), + b3LoadW( p8 ), &s.v.X, &s.v.Y, &s.v.Z, &pad, &s.w.X, &s.w.Y, &s.w.Z, &pad ); + b3TransposeW( b3LoadW( p1 + 8 ), b3LoadW( p2 + 8 ), b3LoadW( p3 + 8 ), b3LoadW( p4 + 8 ), b3LoadW( p5 + 8 ), + b3LoadW( p6 + 8 ), b3LoadW( p7 + 8 ), b3LoadW( p8 + 8 ), &s.dp.X, &s.dp.Y, &s.dp.Z, &pad, &s.dq.V.X, &s.dq.V.Y, + &s.dq.V.Z, &s.dq.S ); + + return s; +} + +B3_FORCE_INLINE void b3StoreBodyVelocity( b3BodyState* B3_RESTRICT states, int index, b3FloatW vw ) +{ + // Indices are 0 for null + if ( index == 0 ) + { + return; + } + + b3BodyState* state = states + index - 1; + uint32_t flags = state->flags; + if ( ( flags & b3_dynamicFlag ) == 0 ) + { + return; + } + + b3StoreW( (float*)state, vw ); +} + +// This writes only the velocities back to the solver bodies +B3_FORCE_INLINE void b3ScatterBodies( b3BodyState* B3_RESTRICT states, const int* B3_RESTRICT indices, + const b3BodyStateW* B3_RESTRICT simdBody ) +{ + // I don't use any dummy body in the body array because this will lead to multithreaded sharing and the + // associated cache flushing. + b3FloatW zero = b3ZeroW(); + b3FloatW vw1, vw2, vw3, vw4, vw5, vw6, vw7, vw8; + b3TransposeW( simdBody->v.X, simdBody->v.Y, simdBody->v.Z, zero, simdBody->w.X, simdBody->w.Y, simdBody->w.Z, zero, &vw1, + &vw2, &vw3, &vw4, &vw5, &vw6, &vw7, &vw8 ); + + b3StoreBodyVelocity( states, indices[0], vw1 ); + b3StoreBodyVelocity( states, indices[1], vw2 ); + b3StoreBodyVelocity( states, indices[2], vw3 ); + b3StoreBodyVelocity( states, indices[3], vw4 ); + b3StoreBodyVelocity( states, indices[4], vw5 ); + b3StoreBodyVelocity( states, indices[5], vw6 ); + b3StoreBodyVelocity( states, indices[6], vw7 ); + b3StoreBodyVelocity( states, indices[7], vw8 ); +} + +#else + +B3_FORCE_INLINE b3BodyStateW b3GatherBodies( const b3BodyState* B3_RESTRICT states, const int* B3_RESTRICT indices ) +{ + const float* identity = (const float*)&b3_identityBodyState; + + // Indices are 0 for null + const float* p1 = indices[0] == 0 ? identity : (const float*)( states + indices[0] - 1 ); + const float* p2 = indices[1] == 0 ? identity : (const float*)( states + indices[1] - 1 ); + const float* p3 = indices[2] == 0 ? identity : (const float*)( states + indices[2] - 1 ); + const float* p4 = indices[3] == 0 ? identity : (const float*)( states + indices[3] - 1 ); + + b3BodyStateW s; + b3FloatW pad; + b3TransposeW( b3LoadW( p1 ), b3LoadW( p2 ), b3LoadW( p3 ), b3LoadW( p4 ), &s.v.X, &s.v.Y, &s.v.Z, &pad ); + b3TransposeW( b3LoadW( p1 + 4 ), b3LoadW( p2 + 4 ), b3LoadW( p3 + 4 ), b3LoadW( p4 + 4 ), &s.w.X, &s.w.Y, &s.w.Z, &pad ); + b3TransposeW( b3LoadW( p1 + 8 ), b3LoadW( p2 + 8 ), b3LoadW( p3 + 8 ), b3LoadW( p4 + 8 ), &s.dp.X, &s.dp.Y, &s.dp.Z, &pad ); + b3TransposeW( b3LoadW( p1 + 12 ), b3LoadW( p2 + 12 ), b3LoadW( p3 + 12 ), b3LoadW( p4 + 12 ), &s.dq.V.X, &s.dq.V.Y, &s.dq.V.Z, + &s.dq.S ); + return s; +} + +B3_FORCE_INLINE void b3StoreBodyVelocity( b3BodyState* B3_RESTRICT states, int index, b3FloatW v, b3FloatW w ) +{ + // Indices are 0 for null + if ( index == 0 ) + { + return; + } + + b3BodyState* state = states + index - 1; + uint32_t flags = state->flags; + if ( ( flags & b3_dynamicFlag ) == 0 ) + { + return; + } + + b3StoreW( (float*)state, v ); + b3StoreW( (float*)state + 4, w ); +} + +// This writes only the velocities back to the solver bodies +B3_FORCE_INLINE void b3ScatterBodies( b3BodyState* B3_RESTRICT states, const int* B3_RESTRICT indices, + const b3BodyStateW* B3_RESTRICT simdBody ) +{ + // I don't use any dummy body in the body array because this will lead to multithreaded sharing and the + // associated cache flushing. + b3FloatW zero = b3ZeroW(); + b3FloatW v1, v2, v3, v4; + b3TransposeW( simdBody->v.X, simdBody->v.Y, simdBody->v.Z, zero, &v1, &v2, &v3, &v4 ); + b3FloatW w1, w2, w3, w4; + b3TransposeW( simdBody->w.X, simdBody->w.Y, simdBody->w.Z, zero, &w1, &w2, &w3, &w4 ); + + b3StoreBodyVelocity( states, indices[0], v1, w1 ); + b3StoreBodyVelocity( states, indices[1], v2, w2 ); + b3StoreBodyVelocity( states, indices[2], v3, w3 ); + b3StoreBodyVelocity( states, indices[3], v4, w4 ); +} + +#endif + +#endif + +_Static_assert( offsetof( b3ManifoldPoint, anchorA ) == 0, "manifold point layout" ); +_Static_assert( offsetof( b3ManifoldPoint, anchorB ) == 12, "manifold point layout" ); +_Static_assert( offsetof( b3ManifoldPoint, separation ) == 24, "manifold point layout" ); +_Static_assert( offsetof( b3ManifoldPoint, normalImpulse ) == 28, "manifold point layout" ); +_Static_assert( offsetof( b3Manifold, twistImpulse ) == offsetof( b3Manifold, normal ) + 12, "manifold layout" ); +_Static_assert( offsetof( b3Manifold, frictionImpulse ) == offsetof( b3Manifold, normal ) + 16, "manifold layout" ); +_Static_assert( offsetof( b3Manifold, rollingImpulse ) == offsetof( b3Manifold, normal ) + 28, "manifold layout" ); +_Static_assert( offsetof( b3Manifold, pointCount ) == offsetof( b3Manifold, normal ) + 40, "manifold layout" ); +_Static_assert( offsetof( b3Matrix3, cy ) == 12 && offsetof( b3Matrix3, cz ) == 24 && sizeof( b3Matrix3 ) == 36, + "matrix layout" ); + +static const b3Contact b3_zeroContact = { 0 }; +static const b3Manifold b3_zeroManifold = { 0 }; +static const b3BodySim b3_zeroBodySim = { 0 }; + +#if B3_SIMD_WIDTH == 8 +#define B3_GATHER_LANES( wide, lanes, field ) \ + wide = b3SetW( lanes[0]->field, lanes[1]->field, lanes[2]->field, lanes[3]->field, lanes[4]->field, lanes[5]->field, \ + lanes[6]->field, lanes[7]->field ) +#else +#define B3_GATHER_LANES( wide, lanes, field ) wide = b3SetW( lanes[0]->field, lanes[1]->field, lanes[2]->field, lanes[3]->field ) +#endif + +static inline b3SymMatrix3W b3GatherInvInertiaW( const b3BodySim* const* simLanes ) +{ + const float* i0 = &simLanes[0]->invInertiaWorld.cx.x; + const float* i1 = &simLanes[1]->invInertiaWorld.cx.x; + const float* i2 = &simLanes[2]->invInertiaWorld.cx.x; + const float* i3 = &simLanes[3]->invInertiaWorld.cx.x; +#if B3_SIMD_WIDTH == 8 + const float* i4 = &simLanes[4]->invInertiaWorld.cx.x; + const float* i5 = &simLanes[5]->invInertiaWorld.cx.x; + const float* i6 = &simLanes[6]->invInertiaWorld.cx.x; + const float* i7 = &simLanes[7]->invInertiaWorld.cx.x; +#endif + + b3SymMatrix3W m; + b3FloatW unused; +#if B3_SIMD_WIDTH == 8 + b3TransposeW( b3LoadW( i0 ), b3LoadW( i1 ), b3LoadW( i2 ), b3LoadW( i3 ), b3LoadW( i4 ), b3LoadW( i5 ), b3LoadW( i6 ), + b3LoadW( i7 ), &m.cxx, &m.cxy, &m.cxz, &unused, &m.cyy, &m.cyz, &unused, &unused ); + m.czz = b3SetW( i0[8], i1[8], i2[8], i3[8], i4[8], i5[8], i6[8], i7[8] ); +#else + b3TransposeW( b3LoadW( i0 ), b3LoadW( i1 ), b3LoadW( i2 ), b3LoadW( i3 ), &m.cxx, &m.cxy, &m.cxz, &unused ); + b3TransposeW( b3LoadW( i0 + 4 ), b3LoadW( i1 + 4 ), b3LoadW( i2 + 4 ), b3LoadW( i3 + 4 ), &m.cyy, &m.cyz, &unused, &unused ); + m.czz = b3SetW( i0[8], i1[8], i2[8], i3[8] ); +#endif + return m; +} + +static inline b3SymMatrix3W b3AddSymW( b3SymMatrix3W a, b3SymMatrix3W b ) +{ + return (b3SymMatrix3W){ + b3AddW( a.cxx, b.cxx ), b3AddW( a.cxy, b.cxy ), b3AddW( a.cxz, b.cxz ), + b3AddW( a.cyy, b.cyy ), b3AddW( a.cyz, b.cyz ), b3AddW( a.czz, b.czz ), + }; +} + +static inline b3SymMatrix3W b3InvertSymW( b3SymMatrix3W m ) +{ + b3FloatW cxx = b3SubW( b3MulW( m.cyy, m.czz ), b3MulW( m.cyz, m.cyz ) ); + b3FloatW cxy = b3SubW( b3MulW( m.cxz, m.cyz ), b3MulW( m.cxy, m.czz ) ); + b3FloatW cxz = b3SubW( b3MulW( m.cxy, m.cyz ), b3MulW( m.cxz, m.cyy ) ); + b3FloatW cyy = b3SubW( b3MulW( m.cxx, m.czz ), b3MulW( m.cxz, m.cxz ) ); + b3FloatW cyz = b3SubW( b3MulW( m.cxy, m.cxz ), b3MulW( m.cxx, m.cyz ) ); + b3FloatW czz = b3SubW( b3MulW( m.cxx, m.cyy ), b3MulW( m.cxy, m.cxy ) ); + + b3FloatW det = b3AddW( b3MulW( m.cxx, cxx ), b3AddW( b3MulW( m.cxy, cxy ), b3MulW( m.cxz, cxz ) ) ); + b3FloatW valid = b3GreaterThanW( b3AbsW( det ), b3SplatW( 1000.0f * FLT_MIN ) ); + b3FloatW invDet = b3BlendW( b3ZeroW(), b3DivW( b3SplatW( 1.0f ), det ), valid ); + + return (b3SymMatrix3W){ + b3MulW( invDet, cxx ), b3MulW( invDet, cxy ), b3MulW( invDet, cxz ), + b3MulW( invDet, cyy ), b3MulW( invDet, cyz ), b3MulW( invDet, czz ), + }; +} + +static inline b3Vec3W b3PerpW( b3Vec3W a ) +{ + b3FloatW zero = b3ZeroW(); + b3FloatW half = b3SplatW( 0.5f ); + b3FloatW mask = b3OrW( b3LessThanW( a.X, b3NegW( half ) ), b3GreaterThanW( a.X, half ) ); + + b3Vec3W p; + p.X = b3BlendW( zero, a.Y, mask ); + p.Y = b3BlendW( a.Z, b3NegW( a.X ), mask ); + p.Z = b3BlendW( b3NegW( a.Y ), zero, mask ); + + b3FloatW lengthSquared = b3DotW( p, p ); + b3FloatW valid = b3GreaterThanW( lengthSquared, b3SplatW( 1000.0f * FLT_MIN ) ); + b3FloatW s = b3BlendW( zero, b3DivW( b3SplatW( 1.0f ), b3SqrtW( lengthSquared ) ), valid ); + return b3MulSVW( s, p ); +} + +void B3_WIDE( 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; + + b3FloatW zeroW = b3ZeroW(); + b3FloatW oneW = b3SplatW( 1.0f ); + b3FloatW twoW = b3SplatW( 2.0f ); + b3FloatW warmStartScale = world->enableWarmStarting ? oneW : zeroW; + b3FloatW invTau = b3SplatW( 1.0f / B3_SPECULATIVE_DISTANCE ); + b3FloatW minFrictionWeight = b3SplatW( B3_MIN_FRICTION_WEIGHT ); + b3FloatW minDet = b3SplatW( 1000.0f * FLT_MIN ); + bool anyRestitution = false; + + 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* c = wideBase + wideIndex; + int localWideIndex = wideIndex - colorWideStart; + + const b3Contact* contactLanes[B3_SIMD_WIDTH]; + const b3Manifold* manifoldLanes[B3_SIMD_WIDTH]; + const b3BodySim* simLanesA[B3_SIMD_WIDTH]; + const b3BodySim* simLanesB[B3_SIMD_WIDTH]; + int hitEventLanes = 0; + + for ( int lane = 0; lane < B3_SIMD_WIDTH; ++lane ) + { + int contactIndex = B3_SIMD_WIDTH * localWideIndex + lane; + if ( contactIndex < colorContactCount ) + { + b3Contact* contact = b3Array_Get( world->contacts, contactIds[contactIndex] ); + B3_ASSERT( contact->manifoldCount == 1 ); + b3Manifold* manifold = contact->manifolds; + + int indexA = b3DecodeAwakeIndex( contact->encodedBodySimA ); + int indexB = b3DecodeAwakeIndex( contact->encodedBodySimB ); + +#if B3_ENABLE_VALIDATION + b3Body* bodyA = bodies + contact->edges[0].bodyId; + b3Body* bodyB = bodies + contact->edges[1].bodyId; + B3_ASSERT( contact->encodedBodySimA == b3EncodeBodySimIndex( bodyA ) ); + B3_ASSERT( contact->encodedBodySimB == b3EncodeBodySimIndex( bodyB ) ); +#endif + + c->indexA[lane] = indexA + 1; + c->indexB[lane] = indexB + 1; + c->pointCounts[lane] = manifold->pointCount; + c->manifolds[lane] = manifold; + + contactLanes[lane] = contact; + manifoldLanes[lane] = manifold; + simLanesA[lane] = indexA == B3_NULL_INDEX ? &b3_zeroBodySim : sims + indexA; + simLanesB[lane] = indexB == B3_NULL_INDEX ? &b3_zeroBodySim : sims + indexB; + hitEventLanes |= ( contact->flags & b3_simEnableHitEvent ) != 0 ? 1 << lane : 0; + } + else + { + c->indexA[lane] = 0; + c->indexB[lane] = 0; + c->pointCounts[lane] = 0; + c->manifolds[lane] = NULL; + + contactLanes[lane] = &b3_zeroContact; + manifoldLanes[lane] = &b3_zeroManifold; + simLanesA[lane] = &b3_zeroBodySim; + simLanesB[lane] = &b3_zeroBodySim; + } + } + + b3FloatW mA, mB; + B3_GATHER_LANES( mA, simLanesA, invMass ); + B3_GATHER_LANES( mB, simLanesB, invMass ); + b3SymMatrix3W iA = b3GatherInvInertiaW( simLanesA ); + b3SymMatrix3W iB = b3GatherInvInertiaW( simLanesB ); + c->invMassA = mA; + c->invMassB = mB; + c->invIA = iA; + c->invIB = iB; + + b3Vec3W tangentVelocity; + B3_GATHER_LANES( c->friction, contactLanes, friction ); + B3_GATHER_LANES( c->rollingResistance, contactLanes, rollingResistance ); + B3_GATHER_LANES( c->restitution, contactLanes, restitution ); + B3_GATHER_LANES( tangentVelocity.X, contactLanes, tangentVelocity.x ); + B3_GATHER_LANES( tangentVelocity.Y, contactLanes, tangentVelocity.y ); + B3_GATHER_LANES( tangentVelocity.Z, contactLanes, tangentVelocity.z ); + + b3Vec3W normal, frictionImpulse, rollingImpulse; + b3FloatW twistImpulse; + { + const float* m0 = &manifoldLanes[0]->normal.x; + const float* m1 = &manifoldLanes[1]->normal.x; + const float* m2 = &manifoldLanes[2]->normal.x; + const float* m3 = &manifoldLanes[3]->normal.x; +#if B3_SIMD_WIDTH == 8 + const float* m4 = &manifoldLanes[4]->normal.x; + const float* m5 = &manifoldLanes[5]->normal.x; + const float* m6 = &manifoldLanes[6]->normal.x; + const float* m7 = &manifoldLanes[7]->normal.x; + b3TransposeW( b3LoadW( m0 ), b3LoadW( m1 ), b3LoadW( m2 ), b3LoadW( m3 ), b3LoadW( m4 ), b3LoadW( m5 ), + b3LoadW( m6 ), b3LoadW( m7 ), &normal.X, &normal.Y, &normal.Z, &twistImpulse, &frictionImpulse.X, + &frictionImpulse.Y, &frictionImpulse.Z, &rollingImpulse.X ); +#else + b3TransposeW( b3LoadW( m0 ), b3LoadW( m1 ), b3LoadW( m2 ), b3LoadW( m3 ), &normal.X, &normal.Y, &normal.Z, + &twistImpulse ); + b3TransposeW( b3LoadW( m0 + 4 ), b3LoadW( m1 + 4 ), b3LoadW( m2 + 4 ), b3LoadW( m3 + 4 ), &frictionImpulse.X, + &frictionImpulse.Y, &frictionImpulse.Z, &rollingImpulse.X ); +#endif +#if B3_SIMD_WIDTH == 8 + rollingImpulse.Y = b3SetW( m0[8], m1[8], m2[8], m3[8], m4[8], m5[8], m6[8], m7[8] ); + rollingImpulse.Z = b3SetW( m0[9], m1[9], m2[9], m3[9], m4[9], m5[9], m6[9], m7[9] ); +#else + rollingImpulse.Y = b3SetW( m0[8], m1[8], m2[8], m3[8] ); + rollingImpulse.Z = b3SetW( m0[9], m1[9], m2[9], m3[9] ); +#endif + } + + b3Vec3W tangent1 = b3PerpW( normal ); + b3Vec3W tangent2 = b3CrossW( tangent1, normal ); + c->normal = normal; + c->tangent1 = tangent1; + c->tangent2 = tangent2; + c->tangentVelocity1 = b3DotW( tangentVelocity, tangent1 ); + c->tangentVelocity2 = b3DotW( tangentVelocity, tangent2 ); + + b3FloatW rollingMask = b3GreaterThanW( c->rollingResistance, zeroW ); + c->twistImpulse = b3MulW( warmStartScale, twistImpulse ); + c->rollingImpulse.X = b3BlendW( zeroW, b3MulW( warmStartScale, rollingImpulse.X ), rollingMask ); + c->rollingImpulse.Y = b3BlendW( zeroW, b3MulW( warmStartScale, rollingImpulse.Y ), rollingMask ); + c->rollingImpulse.Z = b3BlendW( zeroW, b3MulW( warmStartScale, rollingImpulse.Z ), rollingMask ); + c->frictionImpulse.x = b3MulW( warmStartScale, b3DotW( frictionImpulse, tangent1 ) ); + c->frictionImpulse.y = b3MulW( warmStartScale, b3DotW( frictionImpulse, tangent2 ) ); + + b3FloatW pointCountW = +#if B3_SIMD_WIDTH == 8 + b3SetW( (float)c->pointCounts[0], (float)c->pointCounts[1], (float)c->pointCounts[2], (float)c->pointCounts[3], + (float)c->pointCounts[4], (float)c->pointCounts[5], (float)c->pointCounts[6], (float)c->pointCounts[7] ); +#else + b3SetW( (float)c->pointCounts[0], (float)c->pointCounts[1], (float)c->pointCounts[2], (float)c->pointCounts[3] ); +#endif + + b3Vec3W centerA = { zeroW, zeroW, zeroW }; + b3Vec3W centerB = { zeroW, zeroW, zeroW }; + b3FloatW totalFrictionWeight = zeroW; + + for ( int pointIndex = 0; pointIndex < B3_MAX_MANIFOLD_POINTS; ++pointIndex ) + { + b3ContactConstraintPointWide* cp = c->points + pointIndex; + b3FloatW pointMask = b3GreaterThanW( pointCountW, b3SplatW( (float)pointIndex ) ); + + const float* p0 = (const float*)( manifoldLanes[0]->points + pointIndex ); + const float* p1 = (const float*)( manifoldLanes[1]->points + pointIndex ); + const float* p2 = (const float*)( manifoldLanes[2]->points + pointIndex ); + const float* p3 = (const float*)( manifoldLanes[3]->points + pointIndex ); +#if B3_SIMD_WIDTH == 8 + const float* p4 = (const float*)( manifoldLanes[4]->points + pointIndex ); + const float* p5 = (const float*)( manifoldLanes[5]->points + pointIndex ); + const float* p6 = (const float*)( manifoldLanes[6]->points + pointIndex ); + const float* p7 = (const float*)( manifoldLanes[7]->points + pointIndex ); +#endif + + b3Vec3W rA, rB; + b3FloatW separation, normalImpulse; +#if B3_SIMD_WIDTH == 8 + b3TransposeW( b3LoadW( p0 ), b3LoadW( p1 ), b3LoadW( p2 ), b3LoadW( p3 ), b3LoadW( p4 ), b3LoadW( p5 ), + b3LoadW( p6 ), b3LoadW( p7 ), &rA.X, &rA.Y, &rA.Z, &rB.X, &rB.Y, &rB.Z, &separation, + &normalImpulse ); +#else + b3TransposeW( b3LoadW( p0 ), b3LoadW( p1 ), b3LoadW( p2 ), b3LoadW( p3 ), &rA.X, &rA.Y, &rA.Z, &rB.X ); + b3TransposeW( b3LoadW( p0 + 4 ), b3LoadW( p1 + 4 ), b3LoadW( p2 + 4 ), b3LoadW( p3 + 4 ), &rB.Y, &rB.Z, + &separation, &normalImpulse ); +#endif + + rA.X = b3BlendW( zeroW, rA.X, pointMask ); + rA.Y = b3BlendW( zeroW, rA.Y, pointMask ); + rA.Z = b3BlendW( zeroW, rA.Z, pointMask ); + rB.X = b3BlendW( zeroW, rB.X, pointMask ); + rB.Y = b3BlendW( zeroW, rB.Y, pointMask ); + rB.Z = b3BlendW( zeroW, rB.Z, pointMask ); + separation = b3BlendW( zeroW, separation, pointMask ); + normalImpulse = b3BlendW( zeroW, normalImpulse, pointMask ); + + // C0 friction center decay. Needed to prevent spinning top drift (GyroscopicPrecession sample). + // See details in b3PrepareContacts_Mesh. This code should stay in sync. + b3FloatW weight = b3MinW( b3MaxW( b3SubW( twoW, b3MulW( separation, invTau ) ), minFrictionWeight ), oneW ); + weight = b3BlendW( zeroW, weight, pointMask ); + centerA = b3MulAddSVW( centerA, weight, rA ); + centerB = b3MulAddSVW( centerB, weight, rB ); + totalFrictionWeight = b3AddW( totalFrictionWeight, weight ); + + cp->anchorAs = rA; + cp->anchorBs = rB; + cp->baseSeparations = b3SubW( separation, b3DotW( b3SubVW( rB, rA ), normal ) ); + cp->normalImpulses = b3MulW( warmStartScale, normalImpulse ); + cp->totalNormalImpulses = zeroW; + cp->relativeVelocities = zeroW; + cp->restitutionImpulses = zeroW; + + b3Vec3W rnA = b3CrossW( rA, normal ); + b3Vec3W rnB = b3CrossW( rB, normal ); + b3FloatW kNormal = b3AddW( mA, mB ); + kNormal = b3AddW( kNormal, b3DotW( rnA, b3MulMVW( iA, rnA ) ) ); + kNormal = b3AddW( kNormal, b3DotW( rnB, b3MulMVW( iB, rnB ) ) ); + b3FloatW valid = b3AndW( b3GreaterThanW( kNormal, zeroW ), pointMask ); + cp->normalMasses = b3BlendW( zeroW, b3DivW( oneW, kNormal ), valid ); + } + + b3FloatW invWeight = + b3BlendW( zeroW, b3DivW( oneW, totalFrictionWeight ), b3GreaterThanW( totalFrictionWeight, zeroW ) ); + centerA = b3MulSVW( invWeight, centerA ); + centerB = b3MulSVW( invWeight, centerB ); + c->centerA = centerA; + c->centerB = centerB; + + for ( int pointIndex = 0; pointIndex < B3_MAX_MANIFOLD_POINTS; ++pointIndex ) + { + b3ContactConstraintPointWide* cp = c->points + pointIndex; + b3FloatW pointMask = b3GreaterThanW( pointCountW, b3SplatW( (float)pointIndex ) ); + b3Vec3W d = b3SubVW( cp->anchorAs, centerA ); + cp->leverArms = b3BlendW( zeroW, b3SqrtW( b3DotW( d, d ) ), pointMask ); + } + + { + b3Vec3W rtA1 = b3CrossW( centerA, tangent1 ); + b3Vec3W rtA2 = b3CrossW( centerA, tangent2 ); + b3Vec3W rtB1 = b3CrossW( centerB, tangent1 ); + b3Vec3W rtB2 = b3CrossW( centerB, tangent2 ); + b3Vec3W iArtA1 = b3MulMVW( iA, rtA1 ); + b3Vec3W iArtA2 = b3MulMVW( iA, rtA2 ); + b3Vec3W iBrtB1 = b3MulMVW( iB, rtB1 ); + b3Vec3W iBrtB2 = b3MulMVW( iB, rtB2 ); + + b3FloatW kxx = b3AddW( b3AddW( b3AddW( mA, mB ), b3DotW( rtA1, iArtA1 ) ), b3DotW( rtB1, iBrtB1 ) ); + b3FloatW kyy = b3AddW( b3AddW( b3AddW( mA, mB ), b3DotW( rtA2, iArtA2 ) ), b3DotW( rtB2, iBrtB2 ) ); + b3FloatW kxy = b3AddW( b3DotW( rtA1, iArtA2 ), b3DotW( rtB1, iBrtB2 ) ); + + b3FloatW det = b3SubW( b3MulW( kxx, kyy ), b3MulW( kxy, kxy ) ); + b3FloatW valid = b3GreaterThanW( b3AbsW( det ), minDet ); + b3FloatW invDet = b3BlendW( zeroW, b3DivW( oneW, det ), valid ); + c->tangentMass.cxx = b3MulW( invDet, kyy ); + c->tangentMass.cxy = b3NegW( b3MulW( invDet, kxy ) ); + c->tangentMass.cyy = b3MulW( invDet, kxx ); + } + + b3SymMatrix3W invIAB = b3AddSymW( iA, iB ); + + { + b3FloatW kTwist = b3DotW( normal, b3MulMVW( invIAB, normal ) ); + c->twistMass = b3BlendW( zeroW, b3DivW( oneW, kTwist ), b3GreaterThanW( kTwist, zeroW ) ); + } + + if ( b3AllZeroW( c->rollingResistance ) == false ) + { + c->rollingMass = b3InvertSymW( invIAB ); + } + else + { + c->rollingMass = (b3SymMatrix3W){ zeroW, zeroW, zeroW, zeroW, zeroW, zeroW }; + } + + // Only sample contact point normal velocity if needed. + bool haveRestitution = b3AnyTrueW( b3GreaterThanW( c->restitution, zeroW ) ); + anyRestitution = anyRestitution || haveRestitution; + + if ( hitEventLanes != 0 || haveRestitution ) + { + b3BodyStateW bA = b3GatherBodies( states, c->indexA ); + b3BodyStateW bB = b3GatherBodies( states, c->indexB ); + + for ( int pointIndex = 0; pointIndex < B3_MAX_MANIFOLD_POINTS; ++pointIndex ) + { + b3ContactConstraintPointWide* cp = c->points + pointIndex; + + b3Vec3W vrA = b3AddVW( bA.v, b3CrossW( bA.w, cp->anchorAs ) ); + b3Vec3W vrB = b3AddVW( bB.v, b3CrossW( bB.w, cp->anchorBs ) ); + b3FloatW vn = b3DotW( normal, b3SubVW( vrB, vrA ) ); + cp->relativeVelocities = vn; + + if ( hitEventLanes != 0 ) + { + float normalVelocities[B3_SIMD_WIDTH]; + b3StoreW( normalVelocities, vn ); + + for ( int lane = 0; lane < B3_SIMD_WIDTH; ++lane ) + { + if ( ( hitEventLanes & ( 1 << lane ) ) != 0 && pointIndex < c->pointCounts[lane] ) + { + c->manifolds[lane]->points[pointIndex].normalVelocity = normalVelocities[lane]; + } + } + } + } + } + } + + // Advance to next color + colorIndex += 1; + } + + if ( anyRestitution ) + { + b3AtomicStoreInt( &context->anyRestitution, 1 ); + } + + b3TracyCZoneEnd( prepare_contact ); +} + +void B3_WIDE( 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 ); + + int pointCount = 0; + for ( int lane = 0; lane < B3_SIMD_WIDTH; ++lane ) + { + pointCount = b3MaxInt( pointCount, c->pointCounts[lane] ); + } + B3_VALIDATE( 0 < pointCount && pointCount <= B3_MAX_MANIFOLD_POINTS ); + + b3FloatW zeroW = b3ZeroW(); + b3FloatW totalNormalImpulse = zeroW; + b3Vec3W momentA = { zeroW, zeroW, zeroW }; + b3Vec3W momentB = { zeroW, zeroW, zeroW }; + + for ( int j = 0; j < pointCount; ++j ) + { + b3ContactConstraintPointWide* cp = c->points + j; + b3FloatW normalImpulse = cp->normalImpulses; + totalNormalImpulse = b3AddW( totalNormalImpulse, normalImpulse ); + momentA = b3MulAddSVW( momentA, normalImpulse, cp->anchorAs ); + momentB = b3MulAddSVW( momentB, normalImpulse, cp->anchorBs ); + } + + b3Vec3W normal = c->normal; + b3Vec3W frictionImpulse = b3MulSVW( c->frictionImpulse.x, c->tangent1 ); + frictionImpulse = b3MulAddSVW( frictionImpulse, c->frictionImpulse.y, c->tangent2 ); + + b3Vec3W linearImpulse = b3MulAddSVW( frictionImpulse, totalNormalImpulse, normal ); + + b3Vec3W twistImpulse = b3MulSVW( c->twistImpulse, normal ); + b3Vec3W angularImpulseA = + b3AddVW( b3AddVW( b3CrossW( momentA, normal ), b3CrossW( c->centerA, frictionImpulse ) ), twistImpulse ); + b3Vec3W angularImpulseB = + b3AddVW( b3AddVW( b3CrossW( momentB, normal ), b3CrossW( c->centerB, frictionImpulse ) ), twistImpulse ); + + if ( b3AllZeroW( c->rollingResistance ) == false ) + { + angularImpulseA = b3AddVW( angularImpulseA, c->rollingImpulse ); + angularImpulseB = b3AddVW( angularImpulseB, c->rollingImpulse ); + } + + bA.w = b3MulSubMVW( bA.w, c->invIA, angularImpulseA ); + bA.v = b3MulSubSVW( bA.v, c->invMassA, linearImpulse ); + bB.w = b3MulAddMVW( bB.w, c->invIB, angularImpulseB ); + bB.v = b3MulAddSVW( bB.v, c->invMassB, linearImpulse ); + + b3ScatterBodies( states, c->indexA, &bA ); + b3ScatterBodies( states, c->indexB, &bB ); + } + + b3TracyCZoneEnd( warm_start_contact ); +} + +// Solve the non-penetration constraints with the soft bias. No friction and no restitution. +void B3_WIDE( b3PushContacts_Convex )( b3SolverBlock block, b3StepContext* context ) +{ + b3TracyCZoneNC( push_contact, "Push 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 ); + + // Stiffer for static contacts to avoid bodies getting pushed through the ground. Selected per + // lane from the null body index instead of stored per constraint. + b3FloatW dynamicBiasRate = b3SplatW( context->contactSoftness.massScale * context->contactSoftness.biasRate ); + b3FloatW dynamicMassScale = b3SplatW( context->contactSoftness.massScale ); + b3FloatW dynamicImpulseScale = b3SplatW( context->contactSoftness.impulseScale ); + b3FloatW staticBiasRate = b3SplatW( context->staticSoftness.massScale * context->staticSoftness.biasRate ); + b3FloatW staticMassScale = b3SplatW( context->staticSoftness.massScale ); + b3FloatW staticImpulseScale = b3SplatW( context->staticSoftness.impulseScale ); + + for ( int wideIndex = block.startIndex; wideIndex < block.startIndex + block.count; ++wideIndex ) + { + b3ContactConstraintWide* c = constraints + wideIndex; + + int pointCount = 0; + for ( int lane = 0; lane < B3_SIMD_WIDTH; ++lane ) + { + pointCount = b3MaxInt( pointCount, c->pointCounts[lane] ); + } + B3_VALIDATE( 0 < pointCount && pointCount <= B3_MAX_MANIFOLD_POINTS ); + + b3BodyStateW bA = b3GatherBodies( states, c->indexA ); + b3BodyStateW bB = b3GatherBodies( states, c->indexB ); + + b3FloatW softMask = b3SoftMaskW( c->indexA, c->indexB ); + b3FloatW biasRate = b3BlendW( dynamicBiasRate, staticBiasRate, softMask ); + b3FloatW massScale = b3BlendW( dynamicMassScale, staticMassScale, softMask ); + b3FloatW impulseScale = b3BlendW( dynamicImpulseScale, staticImpulseScale, softMask ); + + b3Vec3W dp = b3SubVW( bB.dp, bA.dp ); + + // Convert to normals to local space to reduce transform math. + b3FloatW normalSeparation = b3DotW( c->normal, dp ); + b3Vec3W normalA = b3InvRotateVectorW( bA.dq, c->normal ); + b3Vec3W normalB = b3InvRotateVectorW( bB.dq, c->normal ); + + for ( int pointIndex = 0; pointIndex < pointCount; ++pointIndex ) + { + b3ContactConstraintPointWide* cp = c->points + pointIndex; + + // Fixed anchor points for applying impulses + b3Vec3W rA = cp->anchorAs; + b3Vec3W rB = cp->anchorBs; + + b3FloatW s = + b3AddW( b3AddW( normalSeparation, b3SubW( b3DotW( normalB, rB ), b3DotW( normalA, rA ) ) ), cp->baseSeparations ); + + // Apply speculative bias if separation is greater than zero, otherwise apply soft constraint bias + b3FloatW separated = b3GreaterThanW( s, b3ZeroW() ); + + // Speculative bias - positive + b3FloatW specBias = b3MulW( s, inv_h ); + + // Overlap bias - negative + b3FloatW overlapBias = b3MaxW( b3MulW( biasRate, s ), contactSpeed ); + b3FloatW velocityBias = b3BlendW( overlapBias, specBias, separated ); + + b3FloatW pointMassScale = b3BlendW( massScale, oneW, separated ); + b3FloatW pointImpulseScale = b3BlendW( impulseScale, b3ZeroW(), separated ); + + // 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 ), velocityBias ) ), + 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; + + // 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( push_contact ); +} + +// Solve the normal constraint, friction, and rolling resistance. +void B3_WIDE( b3SolveContacts_Convex )( b3SolverBlock block, b3StepContext* context ) +{ + 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 oneW = b3SplatW( 1.0f ); + b3FloatW epsilonW = b3SplatW( FLT_EPSILON ); + + for ( int wideIndex = block.startIndex; wideIndex < block.startIndex + block.count; ++wideIndex ) + { + b3ContactConstraintWide* c = constraints + wideIndex; + + int pointCount = 0; + for ( int lane = 0; lane < B3_SIMD_WIDTH; ++lane ) + { + pointCount = b3MaxInt( pointCount, c->pointCounts[lane] ); + } + B3_VALIDATE( 0 < pointCount && pointCount <= B3_MAX_MANIFOLD_POINTS ); + + b3BodyStateW bA = b3GatherBodies( states, c->indexA ); + b3BodyStateW bB = b3GatherBodies( states, c->indexB ); + + b3Vec3W dp = b3SubVW( bB.dp, bA.dp ); + b3FloatW normalSeparation = b3DotW( c->normal, dp ); + b3Vec3W normalA = b3InvRotateVectorW( bA.dq, c->normal ); + b3Vec3W normalB = b3InvRotateVectorW( bB.dq, c->normal ); + + b3FloatW totalNormalImpulse = b3ZeroW(); + b3FloatW totalTwistLimit = b3ZeroW(); + + for ( int pointIndex = 0; pointIndex < pointCount; ++pointIndex ) + { + b3ContactConstraintPointWide* cp = c->points + pointIndex; + + // Fixed anchor points for applying impulses + b3Vec3W rA = cp->anchorAs; + b3Vec3W rB = cp->anchorBs; + + b3FloatW s = + b3AddW( b3AddW( normalSeparation, b3SubW( b3DotW( normalB, rB ), b3DotW( normalA, rA ) ) ), cp->baseSeparations ); + + // Speculative bias, positive if separated and zero if overlapped + b3FloatW velocityBias = b3MaxW( b3ZeroW(), b3MulW( s, inv_h ) ); + + // 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( cp->normalMasses, b3AddW( vn, velocityBias ) ); + + // 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 ); + } + + // 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() ); + + // Blend the result to ensure there are no +/-0 determinism problems, since 0 * -1 == -0. + b3Vec3W scaledImpulse = b3MulSVW( scale, c->rollingImpulse ); + c->rollingImpulse.X = b3BlendW( b3ZeroW(), scaledImpulse.X, rollingMask ); + c->rollingImpulse.Y = b3BlendW( b3ZeroW(), scaledImpulse.Y, rollingMask ); + c->rollingImpulse.Z = b3BlendW( b3ZeroW(), scaledImpulse.Z, rollingMask ); + + 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->centerA; + b3Vec3W rB = c->centerB; + + // 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 B3_WIDE( 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 inv_h = b3SplatW( context->inv_h ); + b3FloatW negRestitutionThreshold = b3SplatW( -context->world->restitutionThreshold ); + b3FloatW zeroW = b3ZeroW(); + bool propagate = context->world->enableRestitutionPropagation; + + for ( int wideIndex = block.startIndex; wideIndex < block.startIndex + block.count; ++wideIndex ) + { + b3ContactConstraintWide* c = constraints + wideIndex; + if ( propagate == false && b3AllZeroW( c->restitution ) ) + { + continue; + } + + int pointCount = 0; + for ( int lane = 0; lane < B3_SIMD_WIDTH; ++lane ) + { + pointCount = b3MaxInt( pointCount, c->pointCounts[lane] ); + } + B3_VALIDATE( 0 < pointCount && pointCount <= B3_MAX_MANIFOLD_POINTS ); + + b3BodyStateW bA = b3GatherBodies( states, c->indexA ); + b3BodyStateW bB = b3GatherBodies( states, c->indexB ); + + b3FloatW restitutionMask = b3GreaterThanW( c->restitution, zeroW ); + b3Vec3W dp = b3SubVW( bB.dp, bA.dp ); + b3Matrix3W dqA = b3MakeMatrixFromQuatW( bA.dq ); + b3Matrix3W dqB = b3MakeMatrixFromQuatW( bB.dq ); + + for ( int pointIndex = 0; pointIndex < pointCount; ++pointIndex ) + { + b3ContactConstraintPointWide* cp = c->points + pointIndex; + + b3Vec3W rA = cp->anchorAs; + b3Vec3W rB = cp->anchorBs; + + b3FloatW normalMass = propagate ? cp->normalMasses : b3BlendW( zeroW, cp->normalMasses, restitutionMask ); + + b3FloatW compressionImpulse = b3SubW( cp->totalNormalImpulses, cp->restitutionImpulses ); + b3FloatW armed = b3AndW( b3AndW( restitutionMask, b3LessThanW( cp->relativeVelocities, negRestitutionThreshold ) ), + b3GreaterThanW( compressionImpulse, zeroW ) ); + + b3Vec3W rsA = b3MulM3VW( dqA, rA ); + b3Vec3W rsB = b3MulM3VW( dqB, rB ); + b3Vec3W ds = b3AddVW( dp, b3SubVW( rsB, rsA ) ); + b3FloatW s = b3AddW( b3DotW( c->normal, ds ), cp->baseSeparations ); + + b3FloatW specBias = b3MaxW( zeroW, b3MulW( s, inv_h ) ); + b3FloatW velocityBias = b3BlendW( specBias, b3MulW( c->restitution, cp->relativeVelocities ), armed ); + + 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 ); + + b3FloatW negImpulse = b3MulW( normalMass, b3AddW( vn, velocityBias ) ); + + b3FloatW newImpulse = b3MaxW( b3SubW( cp->normalImpulses, negImpulse ), zeroW ); + b3FloatW impulse = b3SubW( newImpulse, cp->normalImpulses ); + + b3FloatW approachImpulse = b3MinW( b3MaxW( b3NegW( b3MulW( normalMass, vn ) ), zeroW ), b3MaxW( impulse, zeroW ) ); + b3FloatW allowance = + b3SubW( b3MulW( c->restitution, b3AddW( compressionImpulse, approachImpulse ) ), cp->restitutionImpulses ); + b3FloatW maxImpulse = b3AddW( approachImpulse, b3MaxW( allowance, zeroW ) ); + impulse = b3BlendW( impulse, b3MinW( impulse, maxImpulse ), armed ); + + cp->normalImpulses = b3AddW( cp->normalImpulses, impulse ); + cp->restitutionImpulses = b3AddW( cp->restitutionImpulses, b3SubW( impulse, approachImpulse ) ); + cp->totalNormalImpulses = b3AddW( cp->totalNormalImpulses, impulse ); + + b3Vec3W P = b3MulSVW( impulse, 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 B3_WIDE( 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; + + b3ManifoldPoint* mp = m->points + pointIndex; + mp->normalImpulse = normalImpulse[lane]; + mp->totalNormalImpulse = totalNormalImpulse[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 ); +} diff --git a/src/convex_manifold.c b/src/convex_manifold.c index e450bd0..a5dd0bd 100644 --- a/src/convex_manifold.c +++ b/src/convex_manifold.c @@ -1,8 +1,9 @@ // SPDX-FileCopyrightText: 2026 Erin Catto // SPDX-License-Identifier: MIT -#include "algorithm.h" #include "manifold.h" + +#include "algorithm.h" #include "shape.h" #include "simd.h" @@ -204,229 +205,6 @@ static b3SeparatingAxis b3QueryEdgeDirectionHullAndCapsule( const b3HullData* hu }; } -_Static_assert( B3_MAX_MANIFOLD_POINTS >= 4, "must be 4 or more" ); - -#if B3_MAX_MANIFOLD_POINTS == 4 - -// 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; - - 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 ); - 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; - } -} - -#else - -static void b3ReduceManifoldPoints( b3LocalManifold* manifold, int capacity, b3LocalManifoldPoint* points, int count ) -{ - if ( capacity < 4 ) - { - return; - } - - B3_ASSERT( count <= B3_MAX_CLIP_POINTS ); - - int target = b3MinInt( capacity, B3_MAX_MANIFOLD_POINTS ); - - if ( count <= target ) - { - for ( int i = 0; i < count; ++i ) - { - manifold->points[i] = points[i]; - } - - manifold->pointCount = count; - return; - } - - b3Vec3 normal = manifold->normal; - b3Vec3 u = b3Perp( normal ); - b3Vec3 v = b3Cross( normal, u ); - b3Vec3 origin = points[0].point; - - b3Point2D pts[B3_MAX_CLIP_POINTS]; - for ( int i = 0; i < count; ++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; - } - - b3Point2D hull[2 * B3_MAX_CLIP_POINTS]; - int hullCount = b3Hull2D( pts, count, hull ); - int finalCount = b3SimplifyHull2D( hull, hullCount, target ); - B3_ASSERT( 0 < finalCount && finalCount <= target ); - - for ( int i = 0; i < finalCount; ++i ) - { - int index = hull[i].originalIndex; - B3_ASSERT( 0 <= index && index < count ); - manifold->points[i] = points[index]; - } - - manifold->pointCount = finalCount; -} - -#endif - void b3CollideSpheres( b3LocalManifold* manifold, int capacity, const b3Sphere* sphereA, const b3Sphere* sphereB, b3Transform transformBtoA ) { @@ -1003,6 +781,229 @@ void b3CollideHullAndCapsule( b3LocalManifold* manifold, int capacity, const b3H } } +_Static_assert( B3_MAX_MANIFOLD_POINTS >= 4, "must be 4 or more" ); + +#if B3_MAX_MANIFOLD_POINTS == 4 + +// 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; + + 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 ); + 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; + } +} + +#else + +static void b3ReduceManifoldPoints( b3LocalManifold* manifold, int capacity, b3LocalManifoldPoint* points, int count ) +{ + if ( capacity < 4 ) + { + return; + } + + B3_ASSERT( count <= B3_MAX_CLIP_POINTS ); + + int target = b3MinInt( capacity, B3_MAX_MANIFOLD_POINTS ); + + if ( count <= target ) + { + for ( int i = 0; i < count; ++i ) + { + manifold->points[i] = points[i]; + } + + manifold->pointCount = count; + return; + } + + b3Vec3 normal = manifold->normal; + b3Vec3 u = b3Perp( normal ); + b3Vec3 v = b3Cross( normal, u ); + b3Vec3 origin = points[0].point; + + b3Point2D pts[B3_MAX_CLIP_POINTS]; + for ( int i = 0; i < count; ++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; + } + + b3Point2D hull[2 * B3_MAX_CLIP_POINTS]; + int hullCount = b3Hull2D( pts, count, hull ); + int finalCount = b3SimplifyHull2D( hull, hullCount, target ); + B3_ASSERT( 0 < finalCount && finalCount <= target ); + + for ( int i = 0; i < finalCount; ++i ) + { + int index = hull[i].originalIndex; + B3_ASSERT( 0 <= index && index < count ); + manifold->points[i] = points[index]; + } + + manifold->pointCount = finalCount; +} + +#endif + static int b3BuildPolygon( b3ClipVertex* out, b3Transform transform, const b3HullData* hull, int incFace, b3Plane refPlane ) { const b3HullFace* faces = b3GetHullFaces( hull ); @@ -1245,814 +1246,28 @@ static bool b3BuildEdgeContact( b3LocalManifold* manifold, const b3HullData* hul return true; } -// Transform a SoA point/normal stream (already split into X/Y/Z) by out = -(R*v (+t)). -// The inputs come straight from the hull's stored SoA arrays, so there's no transpose here. -static inline void b3NegativeTransformFromSoA( b3Matrix3 R, b3Vec3 p, const float* inX, const float* inY, const float* inZ, - int n, float* outX, float* outY, float* outZ, bool isPoint ) +b3AxisQuery b3ComputeSeparatingAxisAtWidth( const b3HullData* hullA, const b3HullData* hullB, b3Transform xfB, bool earlyReturn, + int simdWidth ) { - // row-column - b3Vec3W r0 = { b3SplatW( R.cx.x ), b3SplatW( R.cy.x ), b3SplatW( R.cz.x ) }; - b3Vec3W r1 = { b3SplatW( R.cx.y ), b3SplatW( R.cy.y ), b3SplatW( R.cz.y ) }; - b3Vec3W r2 = { b3SplatW( R.cx.z ), b3SplatW( R.cy.z ), b3SplatW( R.cz.z ) }; - - b3Vec3W t = { b3ZeroW(), b3ZeroW(), b3ZeroW() }; - if ( isPoint ) +#if defined( B3_SIMD_HAS_WIDTH_8 ) + if ( simdWidth == 8 ) { - t = b3SplatVW( p ); + return b3ComputeSeparatingAxisW8( hullA, hullB, xfB, earlyReturn ); } - - for ( int i = 0; i < n; i += 4 ) - { - b3Vec3W v = b3LoadVW( inX + i, inY + i, inZ + i ); - - // Rotate four vectors at a time - b3Vec3W out = { b3DotW( r0, v ), b3DotW( r1, v ), b3DotW( r2, v ) }; - - if ( isPoint ) - { - out = b3AddVW( out, t ); - } - - b3StoreVW( outX + i, outY + i, outZ + i, b3NegVW( out ) ); - } -} - -_Static_assert( B3_MAX_HULL_VERTICES == 128, "must be 128" ); - -#define B3_HULL_BIT_COUNT 7 - -// SIMD support point calculation using a SoA vertex array padded with repeats of the first vertex -// to a multiple of 4. -// -// This minimizes (bias - dot), where the caller is expected to provide a bias that makes this always positive. -// It can be direction dependent. The bias should be just big enough to ensure the value is positive because -// an excessive bias causes a precision loss in the support calculation. -// -// The vertex index is embedded in the low B3_HULL_BIT_COUNT mantissa bits of the value. By minimizing a value that -// is always positive, the minimum carries the smallest index so that padded SoA values will never win. This is the -// purpose of using the bias instead of maximizing the dot directly. -// -// The support is then recomputed exactly as dot(normal, vertex), without the embedded index. -// todo consider using this for GJK -static inline void b3GetSupportWide( b3Vec3 normal, const float* vx, const float* vy, const float* vz, int n, float bias, - float* support, int* vertexIndex ) -{ - const b3Vec3W normalW = b3SplatVW( normal ); - const b3FloatW biasV = b3SplatW( bias ); - - // Start the minimum at a large value. - b3FloatW minValue = b3SplatW( INFINITY ); - - // Tail lanes hold vertex 0 with index bits >= vertexCount, so they never become the min value. - for ( int i = 0; i < n; i += 4 ) - { - b3Vec3W v = b3LoadVW( vx + i, vy + i, vz + i ); - b3FloatW d = b3DotW( normalW, v ); - - // This is always positive. - b3FloatW value = b3SubW( biasV, d ); - b3FloatW augmentedValue = b3EmbedIndexW( value, i, B3_HULL_BIT_COUNT ); - minValue = b3MinW( minValue, augmentedValue ); - } - - // One horizontal min, the winning lane's value and index bits ride through. - int vi = b3MinIndexW( minValue, B3_HULL_BIT_COUNT ); - - // Exact support for the chosen vertex. - *vertexIndex = vi; - - // Dot product - *support = normal.x * vx[vi] + normal.y * vy[vi] + normal.z * vz[vi]; -} - -static inline float b3GetFaceSeparation( b3Vec3 direction, float planeSeparation, const float* vx, const float* vy, - const float* vz, int n, b3Vec3 center, b3Vec3 extents, int* vertexIndex ) -{ - float bias = b3Dot( direction, center ) + 1.0625f * b3Dot( b3Abs( direction ), extents ); - float support; - b3GetSupportWide( direction, vx, vy, vz, n, bias, &support, vertexIndex ); - return planeSeparation - support; -} - -// Wide dot(n, d) for all face normals n of the hull, padded to the SIMD width. -static inline void b3GetFaceDots( const b3HullData* hull, b3Vec3 d, float* dots ) -{ - int soaFaceCount = ( hull->faceCount + 3 ) & ~3; - const float* nx = b3GetHullSoaNormals( hull ); - const float* ny = nx + soaFaceCount; - const float* nz = ny + soaFaceCount; - - b3Vec3W dW = b3SplatVW( d ); - - for ( int i = 0; i < soaFaceCount; i += 4 ) - { - // dot product per lane - b3FloatW m = b3DotW( b3LoadVW( nx + i, ny + i, nz + i ), dW ); - b3StoreW( dots + i, m ); - } -} - -#define B3_PARALLEL_TOL 1e-4f - -// Inscribed sphere edge test. https://box2d.org/posts/2026/09/inscribed-spheres/ -// The implementation here is a mix of the versions from Cairn Overturf and Dirk Gregorius: -// https://gist.github.com/cairnc/dee7a2866da0709f2d9a77b6493b57b5 -// https://gist.github.com/dgregorius/e6751b5c00937cd21af63cba3c53c861 -// This benchmarks faster than the version from the blog post. -static inline int b3TestEdgeCandidateSorted( float a1, float a2, float c, float bound ) -{ - // We want to maximize - // - // f(t) = dot(d, nlerp(n1, n2, t)) - // - // over t in [0, 1], where - // - // a1 = dot(n1, d) - // a2 = dot(n2, d) - // c = dot(n1, n2). - // - // The maximum can occur either at an endpoint or at an interior - // critical point where f'(t) = 0. - - // Since the endpoint values are a1 and a2, only the larger endpoint - // needs to be tested against the bound. - float hi = b3MaxFloat( a1, a2 ); - float lo = b3MinFloat( a1, a2 ); - int exterior = hi >= bound; - - // Differentiating f(t) gives - // - // a2 - c*a1 - (1 - c)*(a1 + a2)*t - // f'(t) = -------------------------------------- - // ((1 - t)^2 + t^2 + 2*c*t*(1 - t))^(3/2) - // - // The denominator is positive and the numerator is linear in t. - // An interior maximum therefore requires - // - // f'(0) >= 0 -> a2 >= c*a1 - // f'(1) <= 0 -> a1 >= c*a2. - // - // After sorting these become - // - // hi >= c*lo - // lo >= c*hi. - // - // The second is always the stricter condition since - // - // (hi - c*lo) - (lo - c*hi) - // = (1 + c)*(hi - lo) >= 0. - // - // Thus both conditions reduce to u >= 0. - float u = lo - c * hi; - int maxIsInterior = u >= 0.0f; - - // Solving f'(t) = 0 and evaluating f(t) at tmax gives - // - // f(tmax)^2 = - // (a1^2 + a2^2 - 2*c*a1*a2) / (1 - c^2). - // - // After sorting, - // - // hi^2 + lo^2 - 2*c*hi*lo - // = hi^2*(1 - c^2) + u^2. - // - // Let s = 1 - c^2 and rearrange f(tmax) >= bound: - // - // u^2 >= (bound^2 - hi^2)*s. - float s = 1.0f - c * c; - float boundTerm = ( bound - hi ) * ( bound + hi ); - float lhs = u * u; - float rhs = boundTerm * s; - int maxBeatsBound = lhs >= rhs; - - // The interior expression contains 1 - c^2 in its denominator. - // When this approaches zero the arc is degenerate or ill-conditioned, - // so conservatively keep the edge as a candidate. - int nearlyParallel = s < B3_PARALLEL_TOL; - - int interior = maxIsInterior & ( maxBeatsBound | nearlyParallel ); - - return exterior | interior; -} - -// dot(n, otherCenter) - offset for all faces of the hull, padded to the SIMD width. -static inline void b3GetFacePlaneSeparations( const b3HullData* hull, b3Vec3 otherCenter, float* separations ) -{ - b3GetFaceDots( hull, otherCenter, separations ); - - const b3Plane* planes = b3GetHullPlanes( hull ); - int faceCount = hull->faceCount; - for ( int i = 0; i < faceCount; ++i ) - { - separations[i] -= planes[i].offset; - } -} - -// The number of edge pair tests needed to make the extra culling pass worthwhile. -#define B3_EDGE_PROBE_MIN_TESTS 10 - -// Re-test edge candidates against the plane separations of a probe point on the other hull -static inline int b3FilterEdgeCandidates( const b3HullData* hull, const float* planeSeparations, float bound, int* edgeIndices, - int count ) -{ - const b3HullHalfEdge* halfEdges = b3GetHullEdges( hull ); - const float* cosines = b3GetHullEdgeCosines( hull ); - int keptCount = 0; - - for ( int k = 0; k < count; ++k ) - { - int i = edgeIndices[k]; - int i1 = halfEdges[i].face; - int i2 = halfEdges[i + 1].face; - float c = cosines[i >> 1]; - edgeIndices[keptCount] = i; - keptCount += b3TestEdgeCandidateSorted( planeSeparations[i1], planeSeparations[i2], c, bound ); - } - - return keptCount; -} - -// Temporary abbreviations for convenience. -#define NE ( B3_MAX_HULL_EDGES + B3_SIMD_WIDTH ) -#define NF ( B3_MAX_HULL_FACES + B3_SIMD_WIDTH ) -#define NV ( B3_MAX_HULL_VERTICES + B3_SIMD_WIDTH ) - -// SIMD separating axis test based on an implementation developed by Cairn Overturf. -// See his article: https://cairnc.github.io/posts/improvements-to-the-separating-axis/ -b3AxisQuery b3ComputeSeparatingAxis( const b3HullData* hullA, const b3HullData* hullB, b3Transform xfB, bool earlyReturn ) -{ - b3Matrix3 R = b3MakeMatrixFromQuat( xfB.q ); - b3Matrix3 invR = b3Transpose( R ); - - float speculativeDistance = B3_SPECULATIVE_DISTANCE; - - b3AxisQuery res = { - .faceA = - { - .normal = b3Vec3_zero, - .separation = -INFINITY, - .indexA = B3_NULL_INDEX, - .indexB = B3_NULL_INDEX, - .type = b3_faceAxisA, - }, - .faceB = - { - .normal = b3Vec3_zero, - .separation = -INFINITY, - .indexA = B3_NULL_INDEX, - .indexB = B3_NULL_INDEX, - .type = b3_faceAxisB, - }, - .edge = - { - .normal = b3Vec3_zero, - .separation = -INFINITY, - .indexA = B3_NULL_INDEX, - .indexB = B3_NULL_INDEX, - .type = b3_edgePairAxis, - }, - .separatedFeature = b3_invalidAxis, - }; - - int faceCountA = hullA->faceCount; - const b3Plane* planesA = b3GetHullPlanes( hullA ); - - int soaVertexCountB = ( hullB->vertexCount + 3 ) & ~3; - const float* vxB = b3GetHullSoaVertices( hullB ); - const float* vyB = vxB + soaVertexCountB; - const float* vzB = vyB + soaVertexCountB; - - b3Vec3 cB = b3AABB_Center( hullB->aabb ); - b3Vec3 hB = b3AABB_Extents( hullB->aabb ); - - // The hulls have a precomputed inner radius and centroid. - // A given axis cannot achieve a separation larger than: - // dot(axis, centerB - centerA) - innerRadiusA - innerRadiusB - // So this is the upper bound for the separation of a candidate axis. - // An axis can be skipped if it has an upper bound that is less than the current - // best separation. This lets me skip many of the candidates without computing - // support points. - - b3Vec3 deltaCenter = b3Sub( b3Add( b3MulMV( R, hullB->center ), xfB.p ), hullA->center ); - float centerDistance = b3Length( deltaCenter ); - float radius = hullA->innerRadius + hullB->innerRadius; - - // Adjust the radius to ensure the best axis isn't skipped. - float radiusBound = radius - ( B3_LINEAR_SLOP + 0.001f * ( centerDistance + radius ) ); - - // Compute dot(normalA, centerDelta) for all face normals of hullA. - _Alignas( 16 ) float dotA[NF]; - b3GetFaceDots( hullA, deltaCenter, dotA ); - - // Find the face of hullA that most aligns with centerDelta. - int seedIndexA = 0; - float maxDotA = dotA[0]; - for ( int i = 1; i < faceCountA; ++i ) - { - if ( dotA[i] > maxDotA ) - { - maxDotA = dotA[i]; - seedIndexA = i; - } - } - - // Use the seed to get a lower bound on the separation for the faces of hullA. - float floorA = -INFINITY; - float seedSeparationA = -INFINITY; - int seedVertexB = 0; - if ( earlyReturn ) - { - b3Plane plane = planesA[seedIndexA]; - b3Vec3 direction = b3Neg( b3MulMV( invR, plane.normal ) ); - float planeSeparation = b3Dot( plane.normal, xfB.p ) - plane.offset; - seedSeparationA = - b3GetFaceSeparation( direction, planeSeparation, vxB, vyB, vzB, soaVertexCountB, cB, hB, &seedVertexB ); - floorA = b3MinFloat( seedSeparationA, speculativeDistance ); - } - - // Test A's face planes against B's vertices. - for ( int i = 0; i < faceCountA; ++i ) - { - // The bound offset ensures the seed will be evaluated. - if ( dotA[i] - radiusBound < b3MaxFloat( floorA, res.faceA.separation ) ) - { - continue; - } - - b3Plane plane = planesA[i]; - int vertexIndex = seedVertexB; - float separation = seedSeparationA; - - // Avoid recomputing the seed face separation. - if ( earlyReturn == false || i != seedIndexA ) - { - b3Vec3 direction = b3Neg( b3MulMV( invR, plane.normal ) ); - float planeSeparation = b3Dot( plane.normal, xfB.p ) - plane.offset; - separation = - b3GetFaceSeparation( direction, planeSeparation, vxB, vyB, vzB, soaVertexCountB, cB, hB, &vertexIndex ); - } - - if ( separation > res.faceA.separation ) - { - res.faceA.normal = plane.normal; - res.faceA.separation = separation; - res.faceA.indexA = i; - res.faceA.indexB = vertexIndex; - if ( separation > speculativeDistance && earlyReturn ) - { - res.separatedFeature = b3_faceAxisA; - return res; - } - } - } - - B3_VALIDATE( res.faceA.indexA != B3_NULL_INDEX ); - - int faceCountB = hullB->faceCount; - const b3Plane* planesB = b3GetHullPlanes( hullB ); - - int soaVertexCountA = ( hullA->vertexCount + 3 ) & ~3; - const float* vxA = b3GetHullSoaVertices( hullA ); - const float* vyA = vxA + soaVertexCountA; - const float* vzA = vyA + soaVertexCountA; - - b3Vec3 cA = b3AABB_Center( hullA->aabb ); - b3Vec3 hA = b3AABB_Extents( hullA->aabb ); - - // Similarly, find the face of hullB that most aligns with the vector pointing from centerB to centerA. - _Alignas( 16 ) float dotB[NF]; - b3GetFaceDots( hullB, b3Neg( b3MulMV( invR, deltaCenter ) ), dotB ); - - int seedIndexB = 0; - float maxDotB = dotB[0]; - for ( int i = 1; i < faceCountB; ++i ) - { - if ( dotB[i] > maxDotB ) - { - maxDotB = dotB[i]; - seedIndexB = i; - } - } - - // Get a lower bound on the separation for the faces of hullB. - float floorB = -INFINITY; - float seedSeparationB = -INFINITY; - int seedVertexA = 0; - if ( earlyReturn ) - { - b3Plane plane = planesB[seedIndexB]; - b3Vec3 direction = b3Neg( b3MulMV( R, plane.normal ) ); - float planeSeparation = b3Dot( direction, xfB.p ) - plane.offset; - seedSeparationB = - b3GetFaceSeparation( direction, planeSeparation, vxA, vyA, vzA, soaVertexCountA, cA, hA, &seedVertexA ); - - // Include the floor set by hull A faces. - floorB = b3MaxFloat( seedSeparationB, res.faceA.separation ); - floorB = b3MinFloat( floorB, speculativeDistance ); - } - - // Test B's face planes against A's vertices. - for ( int i = 0; i < faceCountB; ++i ) - { - if ( dotB[i] - radiusBound < b3MaxFloat( floorB, res.faceB.separation ) ) - { - continue; - } - - b3Plane plane = planesB[i]; - b3Vec3 direction = b3Neg( b3MulMV( R, plane.normal ) ); - int vertexIndex = seedVertexA; - float separation = seedSeparationB; - - // Avoid recomputing the seed face separation. - if ( earlyReturn == false || i != seedIndexB ) - { - float planeSeparation = b3Dot( direction, xfB.p ) - plane.offset; - separation = - b3GetFaceSeparation( direction, planeSeparation, vxA, vyA, vzA, soaVertexCountA, cA, hA, &vertexIndex ); - } - - if ( separation > res.faceB.separation ) - { - res.faceB.normal = direction; - res.faceB.separation = separation; - res.faceB.indexA = vertexIndex; - res.faceB.indexB = i; - if ( separation > speculativeDistance && earlyReturn ) - { - res.separatedFeature = b3_faceAxisB; - return res; - } - } - } - - // Transform B into A's space once, into SoA arrays. Extra space so tail can be set to zero. - _Static_assert( ( B3_MAX_HULL_EDGES & ( B3_SIMD_WIDTH - 1 ) ) == 0, "must be multiple of SIMD width" ); - _Static_assert( ( B3_MAX_HULL_FACES & ( B3_SIMD_WIDTH - 1 ) ) == 0, "must be multiple of SIMD width" ); - _Static_assert( ( B3_MAX_HULL_VERTICES & ( B3_SIMD_WIDTH - 1 ) ) == 0, "must be multiple of SIMD width" ); - - B3_VALIDATE( earlyReturn == false || - centerDistance >= b3MaxFloat( res.faceA.separation, res.faceB.separation ) + radiusBound ); - - // Gather edges of A that can feasibly create a separating axis that beats the maximum face separation. - int halfEdgeCountA = hullA->edgeCount; - const b3HullHalfEdge* halfEdgesA = b3GetHullEdges( hullA ); - int edgeIndicesA[NE]; - int na = 0; - - int halfEdgeCountB = hullB->edgeCount; - const b3HullHalfEdge* halfEdgesB = b3GetHullEdges( hullB ); - int edgeIndicesB[B3_MAX_HULL_EDGES]; - int nb = 0; - - float maxFaceSeparation = b3MaxFloat( res.faceA.separation, res.faceB.separation ); - float boundSlack = radius - radiusBound; - float thresholdA = earlyReturn ? maxFaceSeparation + hullB->innerRadius - boundSlack : -INFINITY; - float thresholdB = earlyReturn ? maxFaceSeparation + hullA->innerRadius - boundSlack : -INFINITY; - - b3Vec3 centerBinA = b3Add( b3MulMV( R, hullB->center ), xfB.p ); - b3Vec3 centerAinB = b3MulMV( invR, b3Sub( hullA->center, xfB.p ) ); - - _Alignas( 16 ) float planeDotA[NF]; - _Alignas( 16 ) float planeDotB[NF]; - b3GetFacePlaneSeparations( hullA, centerBinA, planeDotA ); - b3GetFacePlaneSeparations( hullB, centerAinB, planeDotB ); - - const float* cosinesA = b3GetHullEdgeCosines( hullA ); - const float* cosinesB = b3GetHullEdgeCosines( hullB ); - - for ( int i = 0; i < halfEdgeCountA; i += 2 ) - { - int i1 = halfEdgesA[i].face; - int i2 = halfEdgesA[i + 1].face; - float c = cosinesA[i >> 1]; - edgeIndicesA[na] = i; - na += b3TestEdgeCandidateSorted( planeDotA[i1], planeDotA[i2], c, thresholdA ); - } - - for ( int i = 0; i < halfEdgeCountB; i += 2 ) - { - int i1 = halfEdgesB[i].face; - int i2 = halfEdgesB[i + 1].face; - float c = cosinesB[i >> 1]; - edgeIndicesB[nb] = i; - nb += b3TestEdgeCandidateSorted( planeDotB[i1], planeDotB[i2], c, thresholdB ); - } - - // Apply additional culling use the support points for the best face axes. - // The support vertices of the best faces are points on the other hull, so an edge pair axis cannot - // have a larger separation than the plane separation of these points over the arc of the edge. - // This can slow down boxes so skip this if there are not enough edge candidates. - if ( earlyReturn && nb * ( ( na + 3 ) >> 2 ) >= B3_EDGE_PROBE_MIN_TESTS ) - { - float probeBound = maxFaceSeparation - ( 0.1f * B3_LINEAR_SLOP + 0.001f * b3AbsFloat( centerDistance + radius ) ); - - int vertexB = res.faceA.indexB != B3_NULL_INDEX ? res.faceA.indexB : seedVertexB; - b3Vec3 probeB = { vxB[vertexB], vyB[vertexB], vzB[vertexB] }; - b3GetFacePlaneSeparations( hullA, b3Add( b3MulMV( R, probeB ), xfB.p ), planeDotA ); - na = b3FilterEdgeCandidates( hullA, planeDotA, probeBound, edgeIndicesA, na ); - - int vertexA = res.faceB.indexA != B3_NULL_INDEX ? res.faceB.indexA : seedVertexA; - b3Vec3 probeA = { vxA[vertexA], vyA[vertexA], vzA[vertexA] }; - b3GetFacePlaneSeparations( hullB, b3MulMV( invR, b3Sub( probeA, xfB.p ) ), planeDotB ); - nb = b3FilterEdgeCandidates( hullB, planeDotB, probeBound, edgeIndicesB, nb ); - } - - if ( na == 0 || nb == 0 ) - { - // No edge candidates found. - return res; - } - - // The alignments below are not necessary, but they don't hurt. - - // B face normals in A space, negated. - _Alignas( 16 ) float bFNx[NF]; - _Alignas( 16 ) float bFNy[NF]; - _Alignas( 16 ) float bFNz[NF]; - - // B vertices in A space, negated. - _Alignas( 16 ) float bWx[NV]; - _Alignas( 16 ) float bWy[NV]; - _Alignas( 16 ) float bWz[NV]; - - int soaFaceCountB = ( faceCountB + 3 ) & ~3; - const float* nxB = b3GetHullSoaNormals( hullB ); - const float* nyB = nxB + soaFaceCountB; - const float* nzB = nyB + soaFaceCountB; - - b3NegativeTransformFromSoA( R, xfB.p, nxB, nyB, nzB, soaFaceCountB, bFNx, bFNy, bFNz, false ); - b3NegativeTransformFromSoA( R, xfB.p, vxB, vyB, vzB, soaVertexCountB, bWx, bWy, bWz, true ); - - // Per A edge data, already in A's space so just gathered. n0 and n1 are the two face - // normals, d the edge vector av1-av0, v0 the first vertex. Tol is the - // parallel edge tolerance, scaled by the edge length. - _Alignas( 16 ) float aN0x[NE]; - _Alignas( 16 ) float aN0y[NE]; - _Alignas( 16 ) float aN0z[NE]; - _Alignas( 16 ) float aN1x[NE]; - _Alignas( 16 ) float aN1y[NE]; - _Alignas( 16 ) float aN1z[NE]; - // dir = av1 - av0 - _Alignas( 16 ) float aDx[NE]; - _Alignas( 16 ) float aDy[NE]; - _Alignas( 16 ) float aDz[NE]; - _Alignas( 16 ) float aV0x[NE]; - _Alignas( 16 ) float aV0y[NE]; - _Alignas( 16 ) float aV0z[NE]; - _Alignas( 16 ) float aTol[NE]; - - float squaredTol = B3_PARALLEL_EDGE_TOL * B3_PARALLEL_EDGE_TOL; - for ( int k = 0; k < na; ++k ) - { - const b3HullHalfEdge* edge = halfEdgesA + edgeIndicesA[k]; - const b3HullHalfEdge* twin = edge + 1; - - b3Vec3 A = planesA[edge->face].normal; - b3Vec3 B = planesA[twin->face].normal; - aN0x[k] = A.x; - aN0y[k] = A.y; - aN0z[k] = A.z; - aN1x[k] = B.x; - aN1y[k] = B.y; - aN1z[k] = B.z; - - int v0 = edge->origin; - int v1 = twin->origin; - - aDx[k] = vxA[v1] - vxA[v0]; - aDy[k] = vyA[v1] - vyA[v0]; - aDz[k] = vzA[v1] - vzA[v0]; - aV0x[k] = vxA[v0]; - aV0y[k] = vyA[v0]; - aV0z[k] = vzA[v0]; - - aTol[k] = squaredTol * ( aDx[k] * aDx[k] + aDy[k] * aDy[k] + aDz[k] * aDz[k] ); - } - - // Zero the tail lanes. - b3FloatW zero = b3ZeroW(); - b3Vec3W zeroV = { zero, zero, zero }; - b3StoreVW( aN0x + na, aN0y + na, aN0z + na, zeroV ); - b3StoreVW( aN1x + na, aN1y + na, aN1z + na, zeroV ); - b3StoreVW( aDx + na, aDy + na, aDz + na, zeroV ); - b3StoreVW( aV0x + na, aV0y + na, aV0z + na, zeroV ); - b3StoreW( aTol + na, zero ); - - float linearSlop = B3_LINEAR_SLOP; - -#if defined( B3_SIMD_NONE ) - - // The SIMD emulated version of this code is very slow. This is a purely scalar version - // for platforms that don't have SIMD capability. It is much faster than SIMD emulation. - // WARNING: this math needs to match the SIMD version for cross platform determinism. - - const float EPS = -linearSlop * linearSlop; - - for ( int j = 0; j < nb; ++j ) - { - const b3HullHalfEdge* edge = halfEdgesB + edgeIndicesB[j]; - const b3HullHalfEdge* twin = edge + 1; - int f0 = edge->face; - int f1 = twin->face; - int v0 = edge->origin; - int v1 = twin->origin; - - b3Vec3 C = { bFNx[f0], bFNy[f0], bFNz[f0] }; - b3Vec3 D = { bFNx[f1], bFNy[f1], bFNz[f1] }; - b3Vec3 bv0 = { bWx[v0], bWy[v0], bWz[v0] }; - b3Vec3 bv1 = { bWx[v1], bWy[v1], bWz[v1] }; - b3Vec3 DC = b3Sub( bv1, bv0 ); - - for ( int i = 0; i < na; ++i ) - { - b3Vec3 d = { aDx[i], aDy[i], aDz[i] }; - - // CBA = C.dir, DBA = D.dir, where dir = B_x_A - float CBA = b3Dot( C, d ); - float DBA = b3Dot( D, d ); - if ( CBA * DBA >= EPS ) - { - continue; - } - - b3Vec3 n0 = { aN0x[i], aN0y[i], aN0z[i] }; - b3Vec3 n1 = { aN1x[i], aN1y[i], aN1z[i] }; - - // ADC = n0.DC, BDC = n1.DC, where DC = D_x_C - float ADC = b3Dot( n0, DC ); - float BDC = b3Dot( n1, DC ); - if ( ADC * BDC >= EPS || CBA * BDC >= EPS ) - { - continue; - } - - // Reject near parallel edges - float maxCD = b3MaxFloat( CBA * CBA, DBA * DBA ); - if ( maxCD <= aTol[i] ) - { - continue; - } - - // t = -CBA / (DBA - CBA) - float t = -CBA / ( DBA - CBA ); - - // normal = lerp(t, C, D) = C + (D-C)*t - b3Vec3 n = b3MulAdd( C, t, b3Sub( D, C ) ); - float len2 = b3Dot( n, n ); - float inv = 1.0f / sqrtf( len2 ); - n = b3MulSV( inv, n ); - - // separation = -dot(normal, av0 + bv0) - b3Vec3 av0 = { aV0x[i], aV0y[i], aV0z[i] }; - float separation = -b3Dot( b3Add( av0, bv0 ), n ); - if ( separation > res.edge.separation ) - { - res.edge.normal = n; - res.edge.separation = separation; - - // Half edge index - res.edge.indexA = edgeIndicesA[i]; - res.edge.indexB = edgeIndicesB[j]; - - if ( separation > speculativeDistance && earlyReturn ) - { - res.separatedFeature = b3_edgePairAxis; - return res; - } - } - } - } - #else - - // Edge phase, one B edge against four A edges at a time, no transforms in the loop. - - // This tolerance can skip edges shorter than 1cm. - const b3FloatW EPS = b3SplatW( -linearSlop * linearSlop ); - const b3FloatW INF = b3SplatW( INFINITY ); - - for ( int j = 0; j < nb; ++j ) - { - const b3HullHalfEdge* edge = halfEdgesB + edgeIndicesB[j]; - const b3HullHalfEdge* twin = edge + 1; - int f0 = edge->face; - int f1 = twin->face; - int v0 = edge->origin; - int v1 = twin->origin; - - b3Vec3 nC = { bFNx[f0], bFNy[f0], bFNz[f0] }; - b3Vec3 nD = { bFNx[f1], bFNy[f1], bFNz[f1] }; - b3Vec3 pB = { bWx[v0], bWy[v0], bWz[v0] }; - b3Vec3 qB = { bWx[v1], bWy[v1], bWz[v1] }; - - const b3Vec3W C = b3SplatVW( nC ); - const b3Vec3W D = b3SplatVW( nD ); - const b3Vec3W DC = b3SplatVW( b3Sub( qB, pB ) ); - const b3Vec3W bv0 = b3SplatVW( pB ); - - for ( int i = 0; i < na; i += 4 ) - { - b3Vec3W n0 = b3LoadVW( aN0x + i, aN0y + i, aN0z + i ); - b3Vec3W n1 = b3LoadVW( aN1x + i, aN1y + i, aN1z + i ); - b3Vec3W d = b3LoadVW( aDx + i, aDy + i, aDz + i ); - b3Vec3W av0 = b3LoadVW( aV0x + i, aV0y + i, aV0z + i ); - b3FloatW tol = b3LoadW( aTol + i ); - - // CBA = C.dir, DBA = D.dir, where dir = B_x_A - b3FloatW CBA = b3DotW( C, d ); - b3FloatW DBA = b3DotW( D, d ); - // ADC = n0.DC, BDC = n1.DC, where DC = D_x_C - b3FloatW ADC = b3DotW( n0, DC ); - b3FloatW BDC = b3DotW( n1, DC ); - - // Gauss map arc crossing test, CBA*DBA res.edge.separation ) - { - res.edge.normal = (b3Vec3){ nxA[lane], nyA[lane], nzA[lane] }; - res.edge.separation = s; - - // Half edge index - res.edge.indexA = edgeIndicesA[ei]; - res.edge.indexB = edgeIndicesB[j]; - - if ( s > speculativeDistance && earlyReturn ) - { - res.separatedFeature = b3_edgePairAxis; - return res; - } - } - } - } - } + B3_UNUSED( simdWidth ); #endif - return res; + return b3ComputeSeparatingAxisW4( hullA, hullB, xfB, earlyReturn ); } -#undef NE -#undef NF -#undef NV +b3AxisQuery b3ComputeSeparatingAxis( const b3HullData* hullA, const b3HullData* hullB, b3Transform xfB, bool earlyReturn ) +{ + return b3ComputeSeparatingAxisAtWidth( hullA, hullB, xfB, earlyReturn, b3GetSIMDWidth() ); +} -void b3CollideHulls( b3LocalManifold* manifold, int capacity, const b3HullData* hullA, const b3HullData* hullB, - b3Transform transformBtoA, b3SATCache* cache ) +void b3CollideHullsAtWidth( b3LocalManifold* manifold, int capacity, const b3HullData* hullA, const b3HullData* hullB, + b3Transform transformBtoA, b3SATCache* cache, int simdWidth ) { manifold->pointCount = 0; @@ -2239,7 +1454,7 @@ void b3CollideHulls( b3LocalManifold* manifold, int capacity, const b3HullData* // This case is for testing case b3_manualFaceAxisA: { - b3AxisQuery axisQuery = b3ComputeSeparatingAxis( hullA, hullB, transformBtoA, false ); + b3AxisQuery axisQuery = b3ComputeSeparatingAxisAtWidth( hullA, hullB, transformBtoA, false, simdWidth ); b3SeparatingAxis faceQuery = axisQuery.faceA; b3BuildFaceAContact( manifold, capacity, hullA, hullB, transformBtoA, faceQuery, cache ); return; @@ -2248,7 +1463,7 @@ void b3CollideHulls( b3LocalManifold* manifold, int capacity, const b3HullData* // This case is for testing case b3_manualFaceAxisB: { - b3AxisQuery axisQuery = b3ComputeSeparatingAxis( hullA, hullB, transformBtoA, false ); + b3AxisQuery axisQuery = b3ComputeSeparatingAxisAtWidth( hullA, hullB, transformBtoA, false, simdWidth ); b3SeparatingAxis faceQuery = axisQuery.faceB; b3BuildFaceBContact( manifold, capacity, hullA, hullB, transformBtoA, faceQuery, cache ); return; @@ -2257,7 +1472,7 @@ void b3CollideHulls( b3LocalManifold* manifold, int capacity, const b3HullData* // This case is for testing case b3_manualEdgePairAxis: { - b3AxisQuery axisQuery = b3ComputeSeparatingAxis( hullA, hullB, transformBtoA, false ); + b3AxisQuery axisQuery = b3ComputeSeparatingAxisAtWidth( hullA, hullB, transformBtoA, false, simdWidth ); b3SeparatingAxis edgeQuery = axisQuery.edge; if ( edgeQuery.indexA != B3_NULL_INDEX ) { @@ -2274,7 +1489,7 @@ void b3CollideHulls( b3LocalManifold* manifold, int capacity, const b3HullData* manifold->pointCount = 0; *cache = (b3SATCache){ 0 }; - b3AxisQuery axisQuery = b3ComputeSeparatingAxis( hullA, hullB, transformBtoA, true ); + b3AxisQuery axisQuery = b3ComputeSeparatingAxisAtWidth( hullA, hullB, transformBtoA, true, simdWidth ); if ( axisQuery.separatedFeature != b3_invalidAxis ) { @@ -2377,3 +1592,9 @@ void b3CollideHulls( b3LocalManifold* manifold, int capacity, const b3HullData* } } } + +void b3CollideHulls( b3LocalManifold* manifold, int capacity, const b3HullData* hullA, const b3HullData* hullB, + b3Transform transformBtoA, b3SATCache* cache ) +{ + b3CollideHullsAtWidth( manifold, capacity, hullA, hullB, transformBtoA, cache, b3GetSIMDWidth() ); +} diff --git a/src/convex_manifold_wide.inl b/src/convex_manifold_wide.inl new file mode 100644 index 0000000..cbb293b --- /dev/null +++ b/src/convex_manifold_wide.inl @@ -0,0 +1,825 @@ +// SPDX-FileCopyrightText: 2026 Erin Catto +// SPDX-License-Identifier: MIT + +#include "algorithm.h" +#include "hull.h" +#include "manifold.h" +#include "shape.h" +#include "simd.h" +#include "simd_wide.h" + +_Static_assert( B3_SIMD_WIDTH <= B3_HULL_SOA_PADDING, "hull SoA padding must cover the SIMD width" ); + +static inline int b3RoundUpToSIMDWidth( int count ) +{ + return ( count + B3_SIMD_WIDTH - 1 ) & ~( B3_SIMD_WIDTH - 1 ); +} + +// Transform a SoA point/normal stream (already split into X/Y/Z) by out = -(R*v (+t)). +// The inputs come straight from the hull's stored SoA arrays, so there's no transpose here. +static inline void b3NegativeTransformFromSoA( b3Matrix3 R, b3Vec3 p, const float* inX, const float* inY, const float* inZ, int n, + float* outX, float* outY, float* outZ, bool isPoint ) +{ + // row-column + b3Vec3W r0 = { b3SplatW( R.cx.x ), b3SplatW( R.cy.x ), b3SplatW( R.cz.x ) }; + b3Vec3W r1 = { b3SplatW( R.cx.y ), b3SplatW( R.cy.y ), b3SplatW( R.cz.y ) }; + b3Vec3W r2 = { b3SplatW( R.cx.z ), b3SplatW( R.cy.z ), b3SplatW( R.cz.z ) }; + + b3Vec3W t = { b3ZeroW(), b3ZeroW(), b3ZeroW() }; + if ( isPoint ) + { + t = b3SplatVW( p ); + } + + for ( int i = 0; i < n; i += B3_SIMD_WIDTH ) + { + b3Vec3W v = b3LoadVW( inX + i, inY + i, inZ + i ); + + // Rotate one vector per lane + b3Vec3W out = { b3DotW( r0, v ), b3DotW( r1, v ), b3DotW( r2, v ) }; + + if ( isPoint ) + { + out = b3AddVW( out, t ); + } + + b3StoreVW( outX + i, outY + i, outZ + i, b3NegVW( out ) ); + } +} + +_Static_assert( B3_MAX_HULL_VERTICES == 128, "must be 128" ); + +#define B3_HULL_BIT_COUNT 7 + +// SIMD support point calculation using a SoA vertex array padded with repeats of the first vertex +// to a multiple of 8. +// +// This minimizes (bias - dot), where the caller is expected to provide a bias that makes this always positive. +// It can be direction dependent. The bias should be just big enough to ensure the value is positive because +// an excessive bias causes a precision loss in the support calculation. +// +// The vertex index is embedded in the low B3_HULL_BIT_COUNT mantissa bits of the value. By minimizing a value that +// is always positive, the minimum carries the smallest index so that padded SoA values will never win. This is the +// purpose of using the bias instead of maximizing the dot directly. +// +// The support is then recomputed exactly as dot(normal, vertex), without the embedded index. +// todo consider using this for GJK +static inline void b3GetSupportWide( b3Vec3 normal, const float* vx, const float* vy, const float* vz, int n, float bias, + float* support, int* vertexIndex ) +{ + const b3Vec3W normalW = b3SplatVW( normal ); + const b3FloatW biasV = b3SplatW( bias ); + + // Start the minimum at a large value. + b3FloatW minValue = b3SplatW( INFINITY ); + + // Tail lanes hold vertex 0 with index bits >= vertexCount, so they never become the min value. + for ( int i = 0; i < n; i += B3_SIMD_WIDTH ) + { + b3Vec3W v = b3LoadVW( vx + i, vy + i, vz + i ); + b3FloatW d = b3DotW( normalW, v ); + + // This is always positive. + b3FloatW value = b3SubW( biasV, d ); + b3FloatW augmentedValue = b3EmbedIndexW( value, i, B3_HULL_BIT_COUNT ); + minValue = b3MinW( minValue, augmentedValue ); + } + + // One horizontal min, the winning lane's value and index bits ride through. + int vi = b3MinIndexW( minValue, B3_HULL_BIT_COUNT ); + + // Exact support for the chosen vertex. + *vertexIndex = vi; + + // Dot product + *support = normal.x * vx[vi] + normal.y * vy[vi] + normal.z * vz[vi]; +} + +static inline float b3GetFaceSeparation( b3Vec3 direction, float planeSeparation, const float* vx, const float* vy, + const float* vz, int n, b3Vec3 center, b3Vec3 extents, int* vertexIndex ) +{ + float bias = b3Dot( direction, center ) + 1.0625f * b3Dot( b3Abs( direction ), extents ); + float support; + b3GetSupportWide( direction, vx, vy, vz, n, bias, &support, vertexIndex ); + return planeSeparation - support; +} + +// Wide dot(n, d) for all face normals n of the hull, padded to the SIMD width. +static inline void b3GetFaceDots( const b3HullData* hull, b3Vec3 d, float* dots ) +{ + int faceStride = b3GetHullSoaStride( hull->faceCount ); + const float* nx = b3GetHullSoaNormals( hull ); + const float* ny = nx + faceStride; + const float* nz = ny + faceStride; + + b3Vec3W dW = b3SplatVW( d ); + + int wideFaceCount = b3RoundUpToSIMDWidth( hull->faceCount ); + for ( int i = 0; i < wideFaceCount; i += B3_SIMD_WIDTH ) + { + // dot product per lane + b3FloatW m = b3DotW( b3LoadVW( nx + i, ny + i, nz + i ), dW ); + b3StoreW( dots + i, m ); + } +} + +#define B3_PARALLEL_TOL 1e-4f + +// Inscribed sphere edge test. https://box2d.org/posts/2026/09/inscribed-spheres/ +// The implementation here is a mix of the versions from Cairn Overturf and Dirk Gregorius: +// https://gist.github.com/cairnc/dee7a2866da0709f2d9a77b6493b57b5 +// https://gist.github.com/dgregorius/e6751b5c00937cd21af63cba3c53c861 +// This benchmarks faster than the version from the blog post. +static inline int b3TestEdgeCandidateSorted( float a1, float a2, float c, float bound ) +{ + // We want to maximize + // + // f(t) = dot(d, nlerp(n1, n2, t)) + // + // over t in [0, 1], where + // + // a1 = dot(n1, d) + // a2 = dot(n2, d) + // c = dot(n1, n2). + // + // The maximum can occur either at an endpoint or at an interior + // critical point where f'(t) = 0. + + // Since the endpoint values are a1 and a2, only the larger endpoint + // needs to be tested against the bound. + float hi = b3MaxFloat( a1, a2 ); + float lo = b3MinFloat( a1, a2 ); + int exterior = hi >= bound; + + // Differentiating f(t) gives + // + // a2 - c*a1 - (1 - c)*(a1 + a2)*t + // f'(t) = -------------------------------------- + // ((1 - t)^2 + t^2 + 2*c*t*(1 - t))^(3/2) + // + // The denominator is positive and the numerator is linear in t. + // An interior maximum therefore requires + // + // f'(0) >= 0 -> a2 >= c*a1 + // f'(1) <= 0 -> a1 >= c*a2. + // + // After sorting these become + // + // hi >= c*lo + // lo >= c*hi. + // + // The second is always the stricter condition since + // + // (hi - c*lo) - (lo - c*hi) + // = (1 + c)*(hi - lo) >= 0. + // + // Thus both conditions reduce to u >= 0. + float u = lo - c * hi; + int maxIsInterior = u >= 0.0f; + + // Solving f'(t) = 0 and evaluating f(t) at tmax gives + // + // f(tmax)^2 = + // (a1^2 + a2^2 - 2*c*a1*a2) / (1 - c^2). + // + // After sorting, + // + // hi^2 + lo^2 - 2*c*hi*lo + // = hi^2*(1 - c^2) + u^2. + // + // Let s = 1 - c^2 and rearrange f(tmax) >= bound: + // + // u^2 >= (bound^2 - hi^2)*s. + float s = 1.0f - c * c; + float boundTerm = ( bound - hi ) * ( bound + hi ); + float lhs = u * u; + float rhs = boundTerm * s; + int maxBeatsBound = lhs >= rhs; + + // The interior expression contains 1 - c^2 in its denominator. + // When this approaches zero the arc is degenerate or ill-conditioned, + // so conservatively keep the edge as a candidate. + int nearlyParallel = s < B3_PARALLEL_TOL; + + int interior = maxIsInterior & ( maxBeatsBound | nearlyParallel ); + + return exterior | interior; +} + +// dot(n, otherCenter) - offset for all faces of the hull, padded to the SIMD width. +static inline void b3GetFacePlaneSeparations( const b3HullData* hull, b3Vec3 otherCenter, float* separations ) +{ + b3GetFaceDots( hull, otherCenter, separations ); + + const b3Plane* planes = b3GetHullPlanes( hull ); + int faceCount = hull->faceCount; + for ( int i = 0; i < faceCount; ++i ) + { + separations[i] -= planes[i].offset; + } +} + +// The number of edge pair tests needed to make the extra culling pass worthwhile. +#define B3_EDGE_PROBE_MIN_TESTS 10 + +// Re-test edge candidates against the plane separations of a probe point on the other hull +static inline int b3FilterEdgeCandidates( const b3HullData* hull, const float* planeSeparations, float bound, int* edgeIndices, + int count ) +{ + const b3HullHalfEdge* halfEdges = b3GetHullEdges( hull ); + const float* cosines = b3GetHullEdgeCosines( hull ); + int keptCount = 0; + + for ( int k = 0; k < count; ++k ) + { + int i = edgeIndices[k]; + int i1 = halfEdges[i].face; + int i2 = halfEdges[i + 1].face; + float c = cosines[i >> 1]; + edgeIndices[keptCount] = i; + keptCount += b3TestEdgeCandidateSorted( planeSeparations[i1], planeSeparations[i2], c, bound ); + } + + return keptCount; +} + +// Temporary abbreviations for convenience. +#define NE ( B3_MAX_HULL_EDGES + B3_SIMD_WIDTH ) +#define NF ( B3_MAX_HULL_FACES + B3_SIMD_WIDTH ) +#define NV ( B3_MAX_HULL_VERTICES + B3_SIMD_WIDTH ) + +// SIMD separating axis test based on an implementation developed by Cairn Overturf. +// See his article: https://cairnc.github.io/posts/improvements-to-the-separating-axis/ +b3AxisQuery B3_WIDE( b3ComputeSeparatingAxis )( const b3HullData* hullA, const b3HullData* hullB, b3Transform xfB, + bool earlyReturn ) +{ + b3Matrix3 R = b3MakeMatrixFromQuat( xfB.q ); + b3Matrix3 invR = b3Transpose( R ); + + float speculativeDistance = B3_SPECULATIVE_DISTANCE; + + b3AxisQuery res = { + .faceA = + { + .normal = b3Vec3_zero, + .separation = -INFINITY, + .indexA = B3_NULL_INDEX, + .indexB = B3_NULL_INDEX, + .type = b3_faceAxisA, + }, + .faceB = + { + .normal = b3Vec3_zero, + .separation = -INFINITY, + .indexA = B3_NULL_INDEX, + .indexB = B3_NULL_INDEX, + .type = b3_faceAxisB, + }, + .edge = + { + .normal = b3Vec3_zero, + .separation = -INFINITY, + .indexA = B3_NULL_INDEX, + .indexB = B3_NULL_INDEX, + .type = b3_edgePairAxis, + }, + .separatedFeature = b3_invalidAxis, + }; + + int faceCountA = hullA->faceCount; + const b3Plane* planesA = b3GetHullPlanes( hullA ); + + int vertexStrideB = b3GetHullSoaStride( hullB->vertexCount ); + const float* vxB = b3GetHullSoaVertices( hullB ); + const float* vyB = vxB + vertexStrideB; + const float* vzB = vyB + vertexStrideB; + int wideVertexCountB = b3RoundUpToSIMDWidth( hullB->vertexCount ); + + b3Vec3 cB = b3AABB_Center( hullB->aabb ); + b3Vec3 hB = b3AABB_Extents( hullB->aabb ); + + // The hulls have a precomputed inner radius and centroid. + // A given axis cannot achieve a separation larger than: + // dot(axis, centerB - centerA) - innerRadiusA - innerRadiusB + // So this is the upper bound for the separation of a candidate axis. + // An axis can be skipped if it has an upper bound that is less than the current + // best separation. This lets me skip many of the candidates without computing + // support points. + + b3Vec3 deltaCenter = b3Sub( b3Add( b3MulMV( R, hullB->center ), xfB.p ), hullA->center ); + float centerDistance = b3Length( deltaCenter ); + float radius = hullA->innerRadius + hullB->innerRadius; + + // Adjust the radius to ensure the best axis isn't skipped. + float radiusBound = radius - ( B3_LINEAR_SLOP + 0.001f * ( centerDistance + radius ) ); + + // Compute dot(normalA, centerDelta) for all face normals of hullA. + _Alignas( B3_WIDE_ALIGNMENT ) float dotA[NF]; + b3GetFaceDots( hullA, deltaCenter, dotA ); + + // Find the face of hullA that most aligns with centerDelta. + int seedIndexA = 0; + float maxDotA = dotA[0]; + for ( int i = 1; i < faceCountA; ++i ) + { + if ( dotA[i] > maxDotA ) + { + maxDotA = dotA[i]; + seedIndexA = i; + } + } + + // Use the seed to get a lower bound on the separation for the faces of hullA. + float floorA = -INFINITY; + float seedSeparationA = -INFINITY; + int seedVertexB = 0; + if ( earlyReturn ) + { + b3Plane plane = planesA[seedIndexA]; + b3Vec3 direction = b3Neg( b3MulMV( invR, plane.normal ) ); + float planeSeparation = b3Dot( plane.normal, xfB.p ) - plane.offset; + seedSeparationA = b3GetFaceSeparation( direction, planeSeparation, vxB, vyB, vzB, wideVertexCountB, cB, hB, &seedVertexB ); + floorA = b3MinFloat( seedSeparationA, speculativeDistance ); + } + + // Test A's face planes against B's vertices. + for ( int i = 0; i < faceCountA; ++i ) + { + // The bound offset ensures the seed will be evaluated. + if ( dotA[i] - radiusBound < b3MaxFloat( floorA, res.faceA.separation ) ) + { + continue; + } + + b3Plane plane = planesA[i]; + int vertexIndex = seedVertexB; + float separation = seedSeparationA; + + // Avoid recomputing the seed face separation. + if ( earlyReturn == false || i != seedIndexA ) + { + b3Vec3 direction = b3Neg( b3MulMV( invR, plane.normal ) ); + float planeSeparation = b3Dot( plane.normal, xfB.p ) - plane.offset; + separation = b3GetFaceSeparation( direction, planeSeparation, vxB, vyB, vzB, wideVertexCountB, cB, hB, &vertexIndex ); + } + + if ( separation > res.faceA.separation ) + { + res.faceA.normal = plane.normal; + res.faceA.separation = separation; + res.faceA.indexA = i; + res.faceA.indexB = vertexIndex; + if ( separation > speculativeDistance && earlyReturn ) + { + res.separatedFeature = b3_faceAxisA; + return res; + } + } + } + + B3_VALIDATE( res.faceA.indexA != B3_NULL_INDEX ); + + int faceCountB = hullB->faceCount; + const b3Plane* planesB = b3GetHullPlanes( hullB ); + + int vertexStrideA = b3GetHullSoaStride( hullA->vertexCount ); + const float* vxA = b3GetHullSoaVertices( hullA ); + const float* vyA = vxA + vertexStrideA; + const float* vzA = vyA + vertexStrideA; + int wideVertexCountA = b3RoundUpToSIMDWidth( hullA->vertexCount ); + + b3Vec3 cA = b3AABB_Center( hullA->aabb ); + b3Vec3 hA = b3AABB_Extents( hullA->aabb ); + + // Similarly, find the face of hullB that most aligns with the vector pointing from centerB to centerA. + _Alignas( B3_WIDE_ALIGNMENT ) float dotB[NF]; + b3GetFaceDots( hullB, b3Neg( b3MulMV( invR, deltaCenter ) ), dotB ); + + int seedIndexB = 0; + float maxDotB = dotB[0]; + for ( int i = 1; i < faceCountB; ++i ) + { + if ( dotB[i] > maxDotB ) + { + maxDotB = dotB[i]; + seedIndexB = i; + } + } + + // Get a lower bound on the separation for the faces of hullB. + float floorB = -INFINITY; + float seedSeparationB = -INFINITY; + int seedVertexA = 0; + if ( earlyReturn ) + { + b3Plane plane = planesB[seedIndexB]; + b3Vec3 direction = b3Neg( b3MulMV( R, plane.normal ) ); + float planeSeparation = b3Dot( direction, xfB.p ) - plane.offset; + seedSeparationB = b3GetFaceSeparation( direction, planeSeparation, vxA, vyA, vzA, wideVertexCountA, cA, hA, &seedVertexA ); + + // Include the floor set by hull A faces. + floorB = b3MaxFloat( seedSeparationB, res.faceA.separation ); + floorB = b3MinFloat( floorB, speculativeDistance ); + } + + // Test B's face planes against A's vertices. + for ( int i = 0; i < faceCountB; ++i ) + { + if ( dotB[i] - radiusBound < b3MaxFloat( floorB, res.faceB.separation ) ) + { + continue; + } + + b3Plane plane = planesB[i]; + b3Vec3 direction = b3Neg( b3MulMV( R, plane.normal ) ); + int vertexIndex = seedVertexA; + float separation = seedSeparationB; + + // Avoid recomputing the seed face separation. + if ( earlyReturn == false || i != seedIndexB ) + { + float planeSeparation = b3Dot( direction, xfB.p ) - plane.offset; + separation = b3GetFaceSeparation( direction, planeSeparation, vxA, vyA, vzA, wideVertexCountA, cA, hA, &vertexIndex ); + } + + if ( separation > res.faceB.separation ) + { + res.faceB.normal = direction; + res.faceB.separation = separation; + res.faceB.indexA = vertexIndex; + res.faceB.indexB = i; + if ( separation > speculativeDistance && earlyReturn ) + { + res.separatedFeature = b3_faceAxisB; + return res; + } + } + } + + // Transform B into A's space once, into SoA arrays. Extra space so tail can be set to zero. + _Static_assert( ( B3_MAX_HULL_EDGES & ( B3_SIMD_WIDTH - 1 ) ) == 0, "must be multiple of SIMD width" ); + _Static_assert( ( B3_MAX_HULL_FACES & ( B3_HULL_SOA_PADDING - 1 ) ) == 0, "must be multiple of hull SoA padding" ); + _Static_assert( ( B3_MAX_HULL_VERTICES & ( B3_HULL_SOA_PADDING - 1 ) ) == 0, "must be multiple of hull SoA padding" ); + + B3_VALIDATE( earlyReturn == false || + centerDistance >= b3MaxFloat( res.faceA.separation, res.faceB.separation ) + radiusBound ); + + // Gather edges of A that can feasibly create a separating axis that beats the maximum face separation. + int halfEdgeCountA = hullA->edgeCount; + const b3HullHalfEdge* halfEdgesA = b3GetHullEdges( hullA ); + int edgeIndicesA[NE]; + int na = 0; + + int halfEdgeCountB = hullB->edgeCount; + const b3HullHalfEdge* halfEdgesB = b3GetHullEdges( hullB ); + int edgeIndicesB[B3_MAX_HULL_EDGES]; + int nb = 0; + + float maxFaceSeparation = b3MaxFloat( res.faceA.separation, res.faceB.separation ); + float boundSlack = radius - radiusBound; + float thresholdA = earlyReturn ? maxFaceSeparation + hullB->innerRadius - boundSlack : -INFINITY; + float thresholdB = earlyReturn ? maxFaceSeparation + hullA->innerRadius - boundSlack : -INFINITY; + + b3Vec3 centerBinA = b3Add( b3MulMV( R, hullB->center ), xfB.p ); + b3Vec3 centerAinB = b3MulMV( invR, b3Sub( hullA->center, xfB.p ) ); + + _Alignas( B3_WIDE_ALIGNMENT ) float planeDotA[NF]; + _Alignas( B3_WIDE_ALIGNMENT ) float planeDotB[NF]; + b3GetFacePlaneSeparations( hullA, centerBinA, planeDotA ); + b3GetFacePlaneSeparations( hullB, centerAinB, planeDotB ); + + const float* cosinesA = b3GetHullEdgeCosines( hullA ); + const float* cosinesB = b3GetHullEdgeCosines( hullB ); + + for ( int i = 0; i < halfEdgeCountA; i += 2 ) + { + int i1 = halfEdgesA[i].face; + int i2 = halfEdgesA[i + 1].face; + float c = cosinesA[i >> 1]; + edgeIndicesA[na] = i; + na += b3TestEdgeCandidateSorted( planeDotA[i1], planeDotA[i2], c, thresholdA ); + } + + for ( int i = 0; i < halfEdgeCountB; i += 2 ) + { + int i1 = halfEdgesB[i].face; + int i2 = halfEdgesB[i + 1].face; + float c = cosinesB[i >> 1]; + edgeIndicesB[nb] = i; + nb += b3TestEdgeCandidateSorted( planeDotB[i1], planeDotB[i2], c, thresholdB ); + } + + // Apply additional culling use the support points for the best face axes. + // The support vertices of the best faces are points on the other hull, so an edge pair axis cannot + // have a larger separation than the plane separation of these points over the arc of the edge. + // This can slow down boxes so skip this if there are not enough edge candidates. + if ( earlyReturn && nb * ( ( na + 3 ) >> 2 ) >= B3_EDGE_PROBE_MIN_TESTS ) + { + float probeBound = maxFaceSeparation - ( 0.1f * B3_LINEAR_SLOP + 0.001f * b3AbsFloat( centerDistance + radius ) ); + + int vertexB = res.faceA.indexB != B3_NULL_INDEX ? res.faceA.indexB : seedVertexB; + b3Vec3 probeB = { vxB[vertexB], vyB[vertexB], vzB[vertexB] }; + b3GetFacePlaneSeparations( hullA, b3Add( b3MulMV( R, probeB ), xfB.p ), planeDotA ); + na = b3FilterEdgeCandidates( hullA, planeDotA, probeBound, edgeIndicesA, na ); + + int vertexA = res.faceB.indexA != B3_NULL_INDEX ? res.faceB.indexA : seedVertexA; + b3Vec3 probeA = { vxA[vertexA], vyA[vertexA], vzA[vertexA] }; + b3GetFacePlaneSeparations( hullB, b3MulMV( invR, b3Sub( probeA, xfB.p ) ), planeDotB ); + nb = b3FilterEdgeCandidates( hullB, planeDotB, probeBound, edgeIndicesB, nb ); + } + + if ( na == 0 || nb == 0 ) + { + // No edge candidates found. + return res; + } + + // The alignments below are not necessary, but they don't hurt. + + // B face normals in A space, negated. + _Alignas( B3_WIDE_ALIGNMENT ) float bFNx[NF]; + _Alignas( B3_WIDE_ALIGNMENT ) float bFNy[NF]; + _Alignas( B3_WIDE_ALIGNMENT ) float bFNz[NF]; + + // B vertices in A space, negated. + _Alignas( B3_WIDE_ALIGNMENT ) float bWx[NV]; + _Alignas( B3_WIDE_ALIGNMENT ) float bWy[NV]; + _Alignas( B3_WIDE_ALIGNMENT ) float bWz[NV]; + + int faceStrideB = b3GetHullSoaStride( faceCountB ); + const float* nxB = b3GetHullSoaNormals( hullB ); + const float* nyB = nxB + faceStrideB; + const float* nzB = nyB + faceStrideB; + + int wideFaceCountB = b3RoundUpToSIMDWidth( faceCountB ); + b3NegativeTransformFromSoA( R, xfB.p, nxB, nyB, nzB, wideFaceCountB, bFNx, bFNy, bFNz, false ); + b3NegativeTransformFromSoA( R, xfB.p, vxB, vyB, vzB, wideVertexCountB, bWx, bWy, bWz, true ); + + // Per A edge data, already in A's space so just gathered. n0 and n1 are the two face + // normals, d the edge vector av1-av0, v0 the first vertex. Tol is the + // parallel edge tolerance, scaled by the edge length. + _Alignas( B3_WIDE_ALIGNMENT ) float aN0x[NE]; + _Alignas( B3_WIDE_ALIGNMENT ) float aN0y[NE]; + _Alignas( B3_WIDE_ALIGNMENT ) float aN0z[NE]; + _Alignas( B3_WIDE_ALIGNMENT ) float aN1x[NE]; + _Alignas( B3_WIDE_ALIGNMENT ) float aN1y[NE]; + _Alignas( B3_WIDE_ALIGNMENT ) float aN1z[NE]; + // dir = av1 - av0 + _Alignas( B3_WIDE_ALIGNMENT ) float aDx[NE]; + _Alignas( B3_WIDE_ALIGNMENT ) float aDy[NE]; + _Alignas( B3_WIDE_ALIGNMENT ) float aDz[NE]; + _Alignas( B3_WIDE_ALIGNMENT ) float aV0x[NE]; + _Alignas( B3_WIDE_ALIGNMENT ) float aV0y[NE]; + _Alignas( B3_WIDE_ALIGNMENT ) float aV0z[NE]; + _Alignas( B3_WIDE_ALIGNMENT ) float aTol[NE]; + + float squaredTol = B3_PARALLEL_EDGE_TOL * B3_PARALLEL_EDGE_TOL; + for ( int k = 0; k < na; ++k ) + { + const b3HullHalfEdge* edge = halfEdgesA + edgeIndicesA[k]; + const b3HullHalfEdge* twin = edge + 1; + + b3Vec3 A = planesA[edge->face].normal; + b3Vec3 B = planesA[twin->face].normal; + aN0x[k] = A.x; + aN0y[k] = A.y; + aN0z[k] = A.z; + aN1x[k] = B.x; + aN1y[k] = B.y; + aN1z[k] = B.z; + + int v0 = edge->origin; + int v1 = twin->origin; + + aDx[k] = vxA[v1] - vxA[v0]; + aDy[k] = vyA[v1] - vyA[v0]; + aDz[k] = vzA[v1] - vzA[v0]; + aV0x[k] = vxA[v0]; + aV0y[k] = vyA[v0]; + aV0z[k] = vzA[v0]; + + aTol[k] = squaredTol * ( aDx[k] * aDx[k] + aDy[k] * aDy[k] + aDz[k] * aDz[k] ); + } + + // Zero the tail lanes. + b3FloatW zero = b3ZeroW(); + b3Vec3W zeroV = { zero, zero, zero }; + b3StoreVW( aN0x + na, aN0y + na, aN0z + na, zeroV ); + b3StoreVW( aN1x + na, aN1y + na, aN1z + na, zeroV ); + b3StoreVW( aDx + na, aDy + na, aDz + na, zeroV ); + b3StoreVW( aV0x + na, aV0y + na, aV0z + na, zeroV ); + + b3StoreW( aTol + na, zero ); + + float linearSlop = B3_LINEAR_SLOP; + +#if defined( B3_SIMD_NONE ) + + // The SIMD emulated version of this code is very slow. This is a purely scalar version + // for platforms that don't have SIMD capability. It is much faster than SIMD emulation. + // WARNING: this math needs to match the SIMD version for cross platform determinism. + + const float EPS = -linearSlop * linearSlop; + + for ( int j = 0; j < nb; ++j ) + { + const b3HullHalfEdge* edge = halfEdgesB + edgeIndicesB[j]; + const b3HullHalfEdge* twin = edge + 1; + int f0 = edge->face; + int f1 = twin->face; + int v0 = edge->origin; + int v1 = twin->origin; + + b3Vec3 C = { bFNx[f0], bFNy[f0], bFNz[f0] }; + b3Vec3 D = { bFNx[f1], bFNy[f1], bFNz[f1] }; + b3Vec3 bv0 = { bWx[v0], bWy[v0], bWz[v0] }; + b3Vec3 bv1 = { bWx[v1], bWy[v1], bWz[v1] }; + b3Vec3 DC = b3Sub( bv1, bv0 ); + + for ( int i = 0; i < na; ++i ) + { + b3Vec3 d = { aDx[i], aDy[i], aDz[i] }; + + // CBA = C.dir, DBA = D.dir, where dir = B_x_A + float CBA = b3Dot( C, d ); + float DBA = b3Dot( D, d ); + if ( CBA * DBA >= EPS ) + { + continue; + } + + b3Vec3 n0 = { aN0x[i], aN0y[i], aN0z[i] }; + b3Vec3 n1 = { aN1x[i], aN1y[i], aN1z[i] }; + + // ADC = n0.DC, BDC = n1.DC, where DC = D_x_C + float ADC = b3Dot( n0, DC ); + float BDC = b3Dot( n1, DC ); + if ( ADC * BDC >= EPS || CBA * BDC >= EPS ) + { + continue; + } + + // Reject near parallel edges + float maxCD = b3MaxFloat( CBA * CBA, DBA * DBA ); + if ( maxCD <= aTol[i] ) + { + continue; + } + + // t = -CBA / (DBA - CBA) + float t = -CBA / ( DBA - CBA ); + + // normal = lerp(t, C, D) = C + (D-C)*t + b3Vec3 n = b3MulAdd( C, t, b3Sub( D, C ) ); + float len2 = b3Dot( n, n ); + + float inv = 1.0f / sqrtf( len2 ); + n = b3MulSV( inv, n ); + + // separation = -dot(normal, av0 + bv0) + b3Vec3 av0 = { aV0x[i], aV0y[i], aV0z[i] }; + float separation = -b3Dot( b3Add( av0, bv0 ), n ); + + if ( separation > res.edge.separation ) + { + res.edge.normal = n; + res.edge.separation = separation; + + // Half edge index + res.edge.indexA = edgeIndicesA[i]; + res.edge.indexB = edgeIndicesB[j]; + + if ( separation > speculativeDistance && earlyReturn ) + { + res.separatedFeature = b3_edgePairAxis; + return res; + } + } + } + } + +#else + + // This tolerance can skip edges shorter than 1cm. + const b3FloatW EPS = b3SplatW( -linearSlop * linearSlop ); + const b3FloatW INF = b3SplatW( INFINITY ); + + for ( int j = 0; j < nb; ++j ) + { + const b3HullHalfEdge* edge = halfEdgesB + edgeIndicesB[j]; + const b3HullHalfEdge* twin = edge + 1; + int f0 = edge->face; + int f1 = twin->face; + int v0 = edge->origin; + int v1 = twin->origin; + + b3Vec3 nC = { bFNx[f0], bFNy[f0], bFNz[f0] }; + b3Vec3 nD = { bFNx[f1], bFNy[f1], bFNz[f1] }; + b3Vec3 pB = { bWx[v0], bWy[v0], bWz[v0] }; + b3Vec3 qB = { bWx[v1], bWy[v1], bWz[v1] }; + + const b3Vec3W C = b3SplatVW( nC ); + const b3Vec3W D = b3SplatVW( nD ); + const b3Vec3W DC = b3SplatVW( b3Sub( qB, pB ) ); + const b3Vec3W bv0 = b3SplatVW( pB ); + + for ( int i = 0; i < na; i += B3_SIMD_WIDTH ) + { + b3Vec3W n0 = b3LoadVW( aN0x + i, aN0y + i, aN0z + i ); + b3Vec3W n1 = b3LoadVW( aN1x + i, aN1y + i, aN1z + i ); + b3Vec3W d = b3LoadVW( aDx + i, aDy + i, aDz + i ); + b3Vec3W av0 = b3LoadVW( aV0x + i, aV0y + i, aV0z + i ); + + b3FloatW tol = b3LoadW( aTol + i ); + + // CBA = C.dir, DBA = D.dir, where dir = B_x_A + b3FloatW CBA = b3DotW( C, d ); + b3FloatW DBA = b3DotW( D, d ); + // ADC = n0.DC, BDC = n1.DC, where DC = D_x_C + b3FloatW ADC = b3DotW( n0, DC ); + b3FloatW BDC = b3DotW( n1, DC ); + + // Gauss map arc crossing test, CBA*DBA res.edge.separation ) + { + res.edge.normal = (b3Vec3){ nxA[lane], nyA[lane], nzA[lane] }; + res.edge.separation = s; + + // Half edge index + res.edge.indexA = edgeIndicesA[ei]; + res.edge.indexB = edgeIndicesB[j]; + + if ( s > speculativeDistance && earlyReturn ) + { + res.separatedFeature = b3_edgePairAxis; + return res; + } + } + } + } + } +#endif + + return res; +} + +#undef NE +#undef NF +#undef NV diff --git a/src/core.h b/src/core.h index 36d64f8..94de040 100644 --- a/src/core.h +++ b/src/core.h @@ -49,34 +49,48 @@ // 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") + #if !defined( BOX3D_DISABLE_AVX2 ) + #define B3_SIMD_HAS_WIDTH_8 + #if defined( __AVX2__ ) || ( defined( B3_SIMD_WIDTH ) && B3_SIMD_WIDTH == 8 ) + #define B3_SIMD_AVX2 + #endif + #endif #elif defined( B3_CPU_ARM ) - // ARMv7 Neon doesn't have divide or sqrt so cannot be used. - #if defined( __aarch64__ ) || defined( _M_ARM64 ) - #define B3_SIMD_NEON - #else - #define B3_SIMD_NONE - #endif - #define B3_SIMD_WIDTH 4 + // ARMv7 Neon doesn't have divide or sqrt so cannot be used. + #if defined( __aarch64__ ) || defined( _M_ARM64 ) + #define B3_SIMD_NEON + #else + #define B3_SIMD_NONE + #endif //#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 +#if !defined( B3_SIMD_NONE ) + #define B3_SIMD_ENABLED +#endif + +#if defined( B3_SIMD_AVX2 ) && defined( __clang__ ) + #define B3_AVX2_BEGIN _Pragma( "clang attribute push( __attribute__( ( target( \"avx2\" ) ) ), apply_to = function )" ) + #define B3_AVX2_END _Pragma( "clang attribute pop" ) +#elif defined( B3_SIMD_AVX2 ) && defined( __GNUC__ ) + #define B3_AVX2_BEGIN _Pragma( "GCC push_options" ) _Pragma( "GCC target( \"avx2\" )" ) + #define B3_AVX2_END _Pragma( "GCC pop_options" ) +#else + #define B3_AVX2_BEGIN + #define B3_AVX2_END +#endif + // Define compiler #if defined( __clang__ ) #define B3_COMPILER_CLANG @@ -120,7 +134,7 @@ typedef struct b3AtomicI64 } b3AtomicI64; // Minimum memory alignment used for all allocations -#define B3_ALIGNMENT 16 +#define B3_ALIGNMENT 32 #define B3_RESTRICT restrict diff --git a/src/hull.c b/src/hull.c index 58c37bc..b0392d8 100644 --- a/src/hull.c +++ b/src/hull.c @@ -1649,32 +1649,34 @@ int b3FindHullSupportVertex( const b3HullData* hull, b3Vec3 direction ) int vertexCount = hull->vertexCount; const float* vx = b3GetHullSoaVertices( hull ); - int soaVertexCount = ( vertexCount + 3 ) & ~3; + int soaVertexCount = b3GetHullSoaStride( vertexCount ); const float* vy = vx + soaVertexCount; const float* vz = vy + soaVertexCount; - b3FloatW dx = b3SplatW( direction.x ); - b3FloatW dy = b3SplatW( direction.y ); - b3FloatW dz = b3SplatW( direction.z ); - b3FloatW four = b3SplatW( 4.0f ); - b3FloatW index = b3SetW( 0.0f, 1.0f, 2.0f, 3.0f ); - b3FloatW bestDotW = b3SplatW( -FLT_MAX ); - b3FloatW bestIndexW = b3SplatW( -1.0f ); + b3FloatW4 dx = b3SplatW4( direction.x ); + b3FloatW4 dy = b3SplatW4( direction.y ); + b3FloatW4 dz = b3SplatW4( direction.z ); + b3FloatW4 four = b3SplatW4( 4.0f ); + b3FloatW4 index = b3SetW4( 0.0f, 1.0f, 2.0f, 3.0f ); + b3FloatW4 bestDotW = b3SplatW4( -FLT_MAX ); + b3FloatW4 bestIndexW = b3SplatW4( -1.0f ); - for ( int i = 0; i < soaVertexCount; i += 4 ) + // Keep this at 4 wide to avoid regressing 4 wide platforms. + int loopCount = ( vertexCount + 3 ) & ~3; + for ( int i = 0; i < loopCount; i += 4 ) { - b3FloatW dot = - b3AddW( b3AddW( b3MulW( dx, b3LoadW( vx + i ) ), b3MulW( dy, b3LoadW( vy + i ) ) ), b3MulW( dz, b3LoadW( vz + i ) ) ); - b3FloatW mask = b3GreaterThanW( dot, bestDotW ); - bestDotW = b3BlendW( bestDotW, dot, mask ); - bestIndexW = b3BlendW( bestIndexW, index, mask ); - index = b3AddW( index, four ); + b3FloatW4 dot = b3AddW4( b3AddW4( b3MulW4( dx, b3LoadW4( vx + i ) ), b3MulW4( dy, b3LoadW4( vy + i ) ) ), + b3MulW4( dz, b3LoadW4( vz + i ) ) ); + b3FloatW4 mask = b3GreaterThanW4( dot, bestDotW ); + bestDotW = b3BlendW4( bestDotW, dot, mask ); + bestIndexW = b3BlendW4( bestIndexW, index, mask ); + index = b3AddW4( index, four ); } _Alignas( 16 ) float dots[4]; _Alignas( 16 ) float indices[4]; - b3StoreW( dots, bestDotW ); - b3StoreW( indices, bestIndexW ); + b3StoreW4( dots, bestDotW ); + b3StoreW4( indices, bestIndexW ); for ( int lane = 0; lane < 4; ++lane ) { @@ -2218,8 +2220,8 @@ b3HullData* b3CreateHull( const b3Vec3* points, int pointCount, int maxVertexCou while ( edge != face->edge ); } - int soaVertexCount = ( vertexCount + 3 ) & ~3; - int soaNormalCount = ( faceCount + 3 ) & ~3; + int soaVertexCount = b3GetHullSoaStride( vertexCount ); + int soaNormalCount = b3GetHullSoaStride( faceCount ); // Allocate the hull. Arrays hang off the end. size_t byteCount = b3AlignUp8( sizeof( b3HullData ) ); @@ -2492,7 +2494,7 @@ b3HullData* b3CloneAndTransformHull( const b3HullData* original, b3Transform tra b3Matrix3 matrix = b3MakeMatrixFromQuat( transform.q ); b3Vec3* points = b3GetHullPointsWrite( hull ); - int soaVertexCount = ( vertexCount + 3 ) & ~3; + int soaVertexCount = b3GetHullSoaStride( vertexCount ); float* vx = b3GetHullSoaVerticesWrite( hull ); float* vy = vx + soaVertexCount; float* vz = vy + soaVertexCount; @@ -2514,7 +2516,7 @@ b3HullData* b3CloneAndTransformHull( const b3HullData* original, b3Transform tra } b3Plane* planes = b3GetHullPlanesWrite( hull ); - int soaNormalCount = ( faceCount + 3 ) & ~3; + int soaNormalCount = b3GetHullSoaStride( faceCount ); float* nx = b3GetHullSoaNormalsWrite( hull ); float* ny = nx + soaNormalCount; float* nz = ny + soaNormalCount; diff --git a/src/hull.h b/src/hull.h index cb47c30..23ebbe4 100644 --- a/src/hull.h +++ b/src/hull.h @@ -7,6 +7,14 @@ #include +// Big enough to handle AVX2. +#define B3_HULL_SOA_PADDING 8 + +static inline int b3GetHullSoaStride( int count ) +{ + return ( count + B3_HULL_SOA_PADDING - 1 ) & ~( B3_HULL_SOA_PADDING - 1 ); +} + uint64_t b3HashHullData( const b3HullData* hull ); bool b3CompareHullData( const b3HullData* hull1, const b3HullData* hull2 ); diff --git a/src/manifold.h b/src/manifold.h index fca3cde..577c5d0 100644 --- a/src/manifold.h +++ b/src/manifold.h @@ -46,6 +46,13 @@ b3FeaturePair b3FlipPair( b3FeaturePair pair ); int b3ClipPolygon( b3ClipVertex* out, b3ClipVertex* polygon, int count, b3Plane clipPlane, int edge, b3Plane refPlane ); b3AxisQuery b3ComputeSeparatingAxis( const b3HullData* hullA, const b3HullData* hullB, b3Transform xfB, bool earlyReturn ); +b3AxisQuery b3ComputeSeparatingAxisAtWidth( const b3HullData* hullA, const b3HullData* hullB, b3Transform xfB, bool earlyReturn, + int simdWidth ); +void b3CollideHullsAtWidth( b3LocalManifold* manifold, int capacity, const b3HullData* hullA, const b3HullData* hullB, + b3Transform transformBtoA, b3SATCache* cache, int simdWidth ); + +b3AxisQuery b3ComputeSeparatingAxisW4( const b3HullData* hullA, const b3HullData* hullB, b3Transform xfB, bool earlyReturn ); +b3AxisQuery b3ComputeSeparatingAxisW8( const b3HullData* hullA, const b3HullData* hullB, b3Transform xfB, bool earlyReturn ); #if B3_ENABLE_VALIDATION bool b3ValidatePolygon( b3ClipVertex* polygon, int count ); diff --git a/src/physics_world.c b/src/physics_world.c index c91f946..c48253d 100644 --- a/src/physics_world.c +++ b/src/physics_world.c @@ -245,6 +245,8 @@ b3WorldId b3CreateWorld( const b3WorldDef* def ) memset( world, 0, sizeof( b3World ) ); + world->simdWidth = b3GetSIMDWidth(); + world->worldId = (uint16_t)worldId; world->generation = revision; world->inUse = true; @@ -2029,6 +2031,17 @@ bool b3World_IsWarmStartingEnabled( b3WorldId worldId ) return world->enableWarmStarting; } +void b3World_EnableSSE2Fallback( b3WorldId worldId, bool flag ) +{ + b3World* world = b3GetUnlockedWorldFromId( worldId ); + if ( world == NULL ) + { + return; + } + + world->simdWidth = flag ? 4 : b3GetSIMDWidth(); +} + int b3World_GetAwakeBodyCount( b3WorldId worldId ) { b3World* world = b3GetUnlockedWorldFromId( worldId ); diff --git a/src/physics_world.h b/src/physics_world.h index a302aa3..af419de 100644 --- a/src/physics_world.h +++ b/src/physics_world.h @@ -262,6 +262,7 @@ typedef struct b3World void* customFilterContext; int workerCount; + int simdWidth; b3EnqueueTaskCallback* enqueueTaskFcn; b3FinishTaskCallback* finishTaskFcn; void* userTaskContext; diff --git a/src/simd.c b/src/simd.c index 6a9faac..30ad2e6 100644 --- a/src/simd.c +++ b/src/simd.c @@ -5,6 +5,114 @@ #include "simd.h" +#include "platform.h" + +#include +#include + +static b3AtomicInt b3_simdWidth; + +#if defined( B3_SIMD_HAS_WIDTH_8 ) + +#if defined( _MSC_VER ) +#include +#endif + +// This code enables runtime dispatch to AVX2. +// Inspired by https://github.com/simdjson/simdjson/blob/master/src/internal/isadetection.h + +static void b3CpuId( unsigned int leaf, unsigned int subLeaf, unsigned int registers[4] ) +{ +#if defined( _MSC_VER ) + int info[4]; + __cpuidex( info, (int)leaf, (int)subLeaf ); + registers[0] = (unsigned int)info[0]; + registers[1] = (unsigned int)info[1]; + registers[2] = (unsigned int)info[2]; + registers[3] = (unsigned int)info[3]; +#else + // clang cpuid.h can give the rbx swap register the same register as the leaf input, clobbering rbx + __asm__ volatile( "cpuid" + : "=a"( registers[0] ), "=b"( registers[1] ), "=c"( registers[2] ), "=d"( registers[3] ) + : "a"( leaf ), "c"( subLeaf ) ); +#endif +} + +static uint64_t b3GetExtendedControlRegister( void ) +{ +#if defined( _MSC_VER ) && !defined( __clang__ ) + return _xgetbv( 0 ); +#else + uint32_t low, high; + __asm__ volatile( "xgetbv" : "=a"( low ), "=d"( high ) : "c"( 0 ) ); + return ( (uint64_t)high << 32 ) | low; +#endif +} + +static bool b3HasAVX2( void ) +{ + unsigned int registers[4]; + b3CpuId( 0, 0, registers ); + if ( registers[0] < 7 ) + { + return false; + } + + b3CpuId( 1, 0, registers ); + unsigned int osxsaveAndAvx = ( 1u << 27 ) | ( 1u << 28 ); + if ( ( registers[2] & osxsaveAndAvx ) != osxsaveAndAvx ) + { + return false; + } + + if ( ( b3GetExtendedControlRegister() & 6 ) != 6 ) + { + return false; + } + + b3CpuId( 7, 0, registers ); + return ( registers[1] & ( 1u << 5 ) ) != 0; +} + +#endif + +static int b3DetectSIMDWidth( void ) +{ +#if defined( B3_SIMD_HAS_WIDTH_8 ) + return b3HasAVX2() ? 8 : 4; +#else + return 4; +#endif +} + +int b3GetSIMDWidth( void ) +{ + int width = b3AtomicLoadInt( &b3_simdWidth ); + if ( width == 0 ) + { + width = b3DetectSIMDWidth(); + b3AtomicStoreInt( &b3_simdWidth, width ); + } + + return width; +} + +bool b3IsAVX2Available( void ) +{ + return b3DetectSIMDWidth() == 8; +} + +void b3SetSIMDWidth( int width ) +{ + B3_ASSERT( width == 0 || width == 4 || width == 8 ); + if ( width == 8 && b3DetectSIMDWidth() != 8 ) + { + width = 4; + } + + b3AtomicStoreInt( &b3_simdWidth, width ); +} + #if defined( B3_SIMD_SSE2 ) #define B3_TRANSPOSE3( C1, C2, C3 ) \ diff --git a/src/simd.h b/src/simd.h index 07ee15f..b4da76a 100644 --- a/src/simd.h +++ b/src/simd.h @@ -5,196 +5,38 @@ #include "core.h" -#include "box3d/math_functions.h" #include "box3d/types.h" #include +#if defined( B3_SIMD_ENABLED ) + #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 - -#include -#include - -// scalar math -typedef struct b3FloatW -{ - float x, y, z, w; -} b3FloatW; - +#include "simd_neon.h" #endif #if defined( B3_SIMD_SSE2 ) +#include "simd_sse2.h" +#endif -// 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 } }; +#if defined( B3_SIMD_AVX2 ) +#include "simd_avx2.h" +#endif #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 }; +#include "simd_scalar.h" #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 +#if !defined( B3_SIMD_SSE2 ) // 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 +#include +#include + typedef struct b3V32 { float x, y, z; @@ -360,6 +202,173 @@ static inline bool b3AllLessEq3V( b3V32 a, b3V32 b ) #endif +int b3GetSIMDWidth( void ); +void b3SetSIMDWidth( int width ); + +// Wide vec3 +typedef struct b3Vec3W4 +{ + b3FloatW4 X, Y, Z; +} b3Vec3W4; + +static inline b3Vec3W4 b3LoadVW4( const float* x, const float* y, const float* z ) +{ + return (b3Vec3W4){ b3LoadW4( x ), b3LoadW4( y ), b3LoadW4( z ) }; +} + +static inline void b3StoreVW4( float* x, float* y, float* z, b3Vec3W4 v ) +{ + b3StoreW4( x, v.X ); + b3StoreW4( y, v.Y ); + b3StoreW4( z, v.Z ); +} + +static inline b3Vec3W4 b3SplatVW4( b3Vec3 v ) +{ + return (b3Vec3W4){ b3SplatW4( v.x ), b3SplatW4( v.y ), b3SplatW4( v.z ) }; +} + +static inline b3Vec3W4 b3NegVW4( b3Vec3W4 a ) +{ + return (b3Vec3W4){ b3NegW4( a.X ), b3NegW4( a.Y ), b3NegW4( a.Z ) }; +} + +// s * a +static inline b3Vec3W4 b3MulSVW4( b3FloatW4 s, b3Vec3W4 a ) +{ + return (b3Vec3W4){ b3MulW4( s, a.X ), b3MulW4( s, a.Y ), b3MulW4( s, a.Z ) }; +} + +// a - s * b +static inline b3Vec3W4 b3MulSubSVW4( b3Vec3W4 a, b3FloatW4 s, b3Vec3W4 b ) +{ + return (b3Vec3W4){ b3SubW4( a.X, b3MulW4( s, b.X ) ), b3SubW4( a.Y, b3MulW4( s, b.Y ) ), b3SubW4( a.Z, b3MulW4( s, b.Z ) ) }; +} + +// a + s * b +static inline b3Vec3W4 b3MulAddSVW4( b3Vec3W4 a, b3FloatW4 s, b3Vec3W4 b ) +{ + return (b3Vec3W4){ b3AddW4( a.X, b3MulW4( s, b.X ) ), b3AddW4( a.Y, b3MulW4( s, b.Y ) ), b3AddW4( a.Z, b3MulW4( s, b.Z ) ) }; +} + +// a - b +static inline b3Vec3W4 b3SubVW4( b3Vec3W4 a, b3Vec3W4 b ) +{ + return (b3Vec3W4){ + b3SubW4( a.X, b.X ), + b3SubW4( a.Y, b.Y ), + b3SubW4( a.Z, b.Z ), + }; +} + +// a + b +static inline b3Vec3W4 b3AddVW4( b3Vec3W4 a, b3Vec3W4 b ) +{ + return (b3Vec3W4){ + b3AddW4( a.X, b.X ), + b3AddW4( a.Y, b.Y ), + b3AddW4( a.Z, b.Z ), + }; +} + +static inline b3FloatW4 b3DotW4( b3Vec3W4 a, b3Vec3W4 b ) +{ + return b3AddW4( b3AddW4( b3MulW4( a.X, b.X ), b3MulW4( a.Y, b.Y ) ), b3MulW4( a.Z, b.Z ) ); +} + +static inline b3Vec3W4 b3CrossW4( b3Vec3W4 a, b3Vec3W4 b ) +{ + b3Vec3W4 c; + c.X = b3SubW4( b3MulW4( a.Y, b.Z ), b3MulW4( a.Z, b.Y ) ); + c.Y = b3SubW4( b3MulW4( a.Z, b.X ), b3MulW4( a.X, b.Z ) ); + c.Z = b3SubW4( b3MulW4( a.X, b.Y ), b3MulW4( a.Y, b.X ) ); + return c; +} + +#if defined( B3_SIMD_AVX2 ) + +// Wide vec3 +typedef struct b3Vec3W8 +{ + b3FloatW8 X, Y, Z; +} b3Vec3W8; + +static inline b3Vec3W8 b3LoadVW8( const float* x, const float* y, const float* z ) +{ + return (b3Vec3W8){ b3LoadW8( x ), b3LoadW8( y ), b3LoadW8( z ) }; +} + +static inline void b3StoreVW8( float* x, float* y, float* z, b3Vec3W8 v ) +{ + b3StoreW8( x, v.X ); + b3StoreW8( y, v.Y ); + b3StoreW8( z, v.Z ); +} + +static inline b3Vec3W8 b3SplatVW8( b3Vec3 v ) +{ + return (b3Vec3W8){ b3SplatW8( v.x ), b3SplatW8( v.y ), b3SplatW8( v.z ) }; +} + +static inline b3Vec3W8 b3NegVW8( b3Vec3W8 a ) +{ + return (b3Vec3W8){ b3NegW8( a.X ), b3NegW8( a.Y ), b3NegW8( a.Z ) }; +} + +// s * a +static inline b3Vec3W8 b3MulSVW8( b3FloatW8 s, b3Vec3W8 a ) +{ + return (b3Vec3W8){ b3MulW8( s, a.X ), b3MulW8( s, a.Y ), b3MulW8( s, a.Z ) }; +} + +// a - s * b +static inline b3Vec3W8 b3MulSubSVW8( b3Vec3W8 a, b3FloatW8 s, b3Vec3W8 b ) +{ + return (b3Vec3W8){ b3SubW8( a.X, b3MulW8( s, b.X ) ), b3SubW8( a.Y, b3MulW8( s, b.Y ) ), b3SubW8( a.Z, b3MulW8( s, b.Z ) ) }; +} + +// a + s * b +static inline b3Vec3W8 b3MulAddSVW8( b3Vec3W8 a, b3FloatW8 s, b3Vec3W8 b ) +{ + return (b3Vec3W8){ b3AddW8( a.X, b3MulW8( s, b.X ) ), b3AddW8( a.Y, b3MulW8( s, b.Y ) ), b3AddW8( a.Z, b3MulW8( s, b.Z ) ) }; +} + +// a - b +static inline b3Vec3W8 b3SubVW8( b3Vec3W8 a, b3Vec3W8 b ) +{ + return (b3Vec3W8){ + b3SubW8( a.X, b.X ), + b3SubW8( a.Y, b.Y ), + b3SubW8( a.Z, b.Z ), + }; +} + +// a + b +static inline b3Vec3W8 b3AddVW8( b3Vec3W8 a, b3Vec3W8 b ) +{ + return (b3Vec3W8){ + b3AddW8( a.X, b.X ), + b3AddW8( a.Y, b.Y ), + b3AddW8( a.Z, b.Z ), + }; +} + +static inline b3FloatW8 b3DotW8( b3Vec3W8 a, b3Vec3W8 b ) +{ + return b3AddW8( b3AddW8( b3MulW8( a.X, b.X ), b3MulW8( a.Y, b.Y ) ), b3MulW8( a.Z, b.Z ) ); +} + +static inline b3Vec3W8 b3CrossW8( b3Vec3W8 a, b3Vec3W8 b ) +{ + b3Vec3W8 c; + c.X = b3SubW8( b3MulW8( a.Y, b.Z ), b3MulW8( a.Z, b.Y ) ); + c.Y = b3SubW8( b3MulW8( a.Z, b.X ), b3MulW8( a.X, b.Z ) ); + c.Z = b3SubW8( b3MulW8( a.X, b.Y ), b3MulW8( a.Y, b.X ) ); + return c; +} + +#endif + static inline bool b3TestBoundsOverlap( b3V32 nodeMin1, b3V32 nodeMax1, b3V32 nodeMin2, b3V32 nodeMax2 ) { b3V32 separation = b3MaxV( b3SubV( nodeMin2, nodeMax1 ), b3SubV( nodeMin1, nodeMax2 ) ); @@ -384,890 +393,6 @@ static inline bool b3TestBoundsRayOverlap( b3V32 nodeMin, b3V32 nodeMax, b3V32 r bool b3TestBoundsTriangleOverlap( b3V32 nodeCenter, b3V32 nodeExtent, b3V32 vertex1, b3V32 vertex2, b3V32 vertex3 ); float b3IntersectRayTriangle( b3V32 rayStart, b3V32 rayDelta, b3V32 vertex1, b3V32 vertex2, b3V32 vertex3 ); -#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 b3SetW( float a, float b, float c, float d ) -{ - float32_t array[4] = { a, b, c, d }; - return vld1q_f32( array ); -} - -static inline b3FloatW b3LoadW( const float* data ) -{ - return vld1q_f32( data ); -} - -static inline void b3StoreW( float* data, b3FloatW a ) -{ - vst1q_f32( data, a ); -} - -static inline b3FloatW b3UnpackLoW( b3FloatW a, b3FloatW b ) -{ - return vzip1q_f32( a, b ); -} - -static inline b3FloatW b3UnpackHiW( b3FloatW a, b3FloatW b ) -{ - return vzip2q_f32( a, b ); -} - -static inline b3FloatW b3NegW( b3FloatW a ) -{ - return vnegq_f32( a ); -} - -static inline b3FloatW b3AbsW( b3FloatW a ) -{ - return vabsq_f32( a ); -} - -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 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 b3AndW( b3FloatW a, b3FloatW b ) -{ - return vreinterpretq_f32_u32( vandq_u32( vreinterpretq_u32_f32( a ), vreinterpretq_u32_f32( b ) ) ); -} - -static inline b3FloatW b3OrW( b3FloatW a, b3FloatW b ) -{ - return vreinterpretq_f32_u32( vorrq_u32( vreinterpretq_u32_f32( a ), vreinterpretq_u32_f32( b ) ) ); -} - -// a & ~b -static inline b3FloatW b3AndNotW( b3FloatW a, b3FloatW b ) -{ - return vreinterpretq_f32_u32( vbicq_u32( vreinterpretq_u32_f32( a ), vreinterpretq_u32_f32( b ) ) ); -} - -static inline b3FloatW b3SoftMaskW( const int* indexA, const int* indexB ) -{ - int32x4_t zero = vdupq_n_s32( 0 ); - uint32x4_t a = vceqq_s32( vld1q_s32( (const int32_t*)indexA ), zero ); - uint32x4_t b = vceqq_s32( vld1q_s32( (const int32_t*)indexB ), zero ); - return vreinterpretq_f32_u32( vorrq_u32( a, b ) ); -} - -static inline b3FloatW b3GreaterThanW( b3FloatW a, b3FloatW b ) -{ - return vreinterpretq_f32_u32( vcgtq_f32( a, b ) ); -} - -static inline b3FloatW b3LessThanW( b3FloatW a, b3FloatW b ) -{ - return vreinterpretq_f32_u32( vcltq_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 -} - -// _mm_movemask_ps equivalent, compatible with ARM v7. -static inline bool b3AnyTrueW( b3FloatW mask ) -{ - uint32x4_t m = vreinterpretq_u32_f32( mask ); - uint32x2_t p = vorr_u32( vget_low_u32( m ), vget_high_u32( m ) ); - return ( vget_lane_u32( p, 0 ) | vget_lane_u32( p, 1 ) ) != 0; -} - -// 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 ); -} - -static inline b3FloatW b3EmbedIndexW( b3FloatW value, int baseIndex, int bitCount ) -{ - uint32_t mask = ( 1u << bitCount ) - 1; - const int32_t lanes[4] = { 0, 1, 2, 3 }; - int32x4_t index = vaddq_s32( vdupq_n_s32( baseIndex ), vld1q_s32( lanes ) ); - uint32x4_t clearLow = vdupq_n_u32( ~mask ); - uint32x4_t bits = vorrq_u32( vandq_u32( vreinterpretq_u32_f32( value ), clearLow ), vreinterpretq_u32_s32( index ) ); - return vreinterpretq_f32_u32( bits ); -} - -static inline int b3MinIndexW( b3FloatW a, int bitCount ) -{ - float32x2_t m = vmin_f32( vget_low_f32( a ), vget_high_f32( a ) ); - m = vpmin_f32( m, m ); - uint32_t bits = vget_lane_u32( vreinterpret_u32_f32( m ), 0 ); - return (int)( bits & ( ( 1u << bitCount ) - 1 ) ); -} - -#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 b3SetW( float a, float b, float c, float d ) -{ - return _mm_setr_ps( a, b, c, d ); -} - -static inline b3FloatW b3LoadW( const float* data ) -{ - return _mm_loadu_ps( data ); -} - -static inline void b3StoreW( float* data, b3FloatW a ) -{ - _mm_storeu_ps( data, a ); -} - -static inline b3FloatW b3UnpackLoW( b3FloatW a, b3FloatW b ) -{ - return _mm_unpacklo_ps( a, b ); -} - -static inline b3FloatW b3UnpackHiW( b3FloatW a, b3FloatW b ) -{ - return _mm_unpackhi_ps( a, b ); -} - -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 b3AbsW( b3FloatW a ) -{ - return _mm_andnot_ps( _mm_set1_ps( -0.0f ), a ); -} - -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 ); -} - -// a + b * c -static inline b3FloatW b3MulAddW( b3FloatW a, b3FloatW b, b3FloatW c ) -{ - return _mm_add_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 ); -} - -// Horizontal min over a 4-lane vector (result broadcast to all lanes). -static inline b3FloatW b3HorizontalMinW( b3FloatW v ) -{ - v = _mm_min_ps( v, _mm_shuffle_ps( v, v, _MM_SHUFFLE( 2, 3, 0, 1 ) ) ); - return _mm_min_ps( v, _mm_shuffle_ps( v, v, _MM_SHUFFLE( 1, 0, 3, 2 ) ) ); -} - -// 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 b3AndW( b3FloatW a, b3FloatW b ) -{ - return _mm_and_ps( a, b ); -} - -static inline b3FloatW b3OrW( b3FloatW a, b3FloatW b ) -{ - return _mm_or_ps( a, b ); -} - -// a & ~b -static inline b3FloatW b3AndNotW( b3FloatW a, b3FloatW b ) -{ - // Arguments are reversed - return _mm_andnot_ps( b, a ); -} - -// This is used to optimize selection of contact softness. -static inline b3FloatW b3SoftMaskW( const int* indexA, const int* indexB ) -{ - __m128i zero = _mm_setzero_si128(); - __m128i a = _mm_cmpeq_epi32( _mm_loadu_si128( (const __m128i*)indexA ), zero ); - __m128i b = _mm_cmpeq_epi32( _mm_loadu_si128( (const __m128i*)indexB ), zero ); - return _mm_castsi128_ps( _mm_or_si128( a, b ) ); -} - -static inline b3FloatW b3GreaterThanW( b3FloatW a, b3FloatW b ) -{ - return _mm_cmpgt_ps( a, b ); -} - -static inline b3FloatW b3LessThanW( b3FloatW a, b3FloatW b ) -{ - return _mm_cmplt_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; -} - -static inline bool b3AnyTrueW( b3FloatW mask ) -{ - return _mm_movemask_ps( mask ) != 0; -} - -// 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 ) ); -} - -// Replace the low bitCount mantissa bits of each lane with baseIndex + lane. The value must be -// positive so the embedded index sorts with the value, and ties fall to the lower index. -static inline b3FloatW b3EmbedIndexW( b3FloatW value, int baseIndex, int bitCount ) -{ - int mask = ( 1 << bitCount ) - 1; - __m128i index = _mm_add_epi32( _mm_set1_epi32( baseIndex ), _mm_setr_epi32( 0, 1, 2, 3 ) ); - __m128 clearLow = _mm_castsi128_ps( _mm_set1_epi32( ~mask ) ); - return _mm_or_ps( _mm_and_ps( value, clearLow ), _mm_castsi128_ps( index ) ); -} - -// Recovers the index embedded by b3EmbedIndexW from the lane holding the minimum. -static inline int b3MinIndexW( b3FloatW a, int bitCount ) -{ - a = _mm_min_ps( a, _mm_shuffle_ps( a, a, _MM_SHUFFLE( 2, 3, 0, 1 ) ) ); - a = _mm_min_ps( a, _mm_shuffle_ps( a, a, _MM_SHUFFLE( 1, 0, 3, 2 ) ) ); - return _mm_cvtsi128_si32( _mm_castps_si128( a ) ) & ( ( 1 << bitCount ) - 1 ); -} - -#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 b3SetW( float a, float b, float c, float d ) -{ - return (b3FloatW){ a, b, c, d }; -} - -static inline b3FloatW b3LoadW( const float* data ) -{ - return (b3FloatW){ data[0], data[1], data[2], data[3] }; -} - -static inline void b3StoreW( float* data, b3FloatW a ) -{ - data[0] = a.x; - data[1] = a.y; - data[2] = a.z; - data[3] = a.w; -} - -static inline b3FloatW b3UnpackLoW( b3FloatW a, b3FloatW b ) -{ - return (b3FloatW){ a.x, b.x, a.y, b.y }; -} - -static inline b3FloatW b3UnpackHiW( b3FloatW a, b3FloatW b ) -{ - return (b3FloatW){ a.z, b.z, a.w, b.w }; -} - -static inline b3FloatW b3NegW( b3FloatW a ) -{ - return (b3FloatW){ -a.x, -a.y, -a.z, -a.w }; -} - -static inline b3FloatW b3AbsW( b3FloatW a ) -{ - return (b3FloatW){ a.x < 0.0f ? -a.x : a.x, a.y < 0.0f ? -a.y : a.y, a.z < 0.0f ? -a.z : a.z, a.w < 0.0f ? -a.w : 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 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; -} - -// Logical operations on the scalar path are 0/1 float values. Not bit-wise like SIMD. - -static inline b3FloatW b3AndW( 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 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; -} - -// a & ~b -static inline b3FloatW b3AndNotW( 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 b3SoftMaskW( const int* indexA, const int* indexB ) -{ - b3FloatW r; - r.x = indexA[0] == 0 || indexB[0] == 0 ? 1.0f : 0.0f; - r.y = indexA[1] == 0 || indexB[1] == 0 ? 1.0f : 0.0f; - r.z = indexA[2] == 0 || indexB[2] == 0 ? 1.0f : 0.0f; - r.w = indexA[3] == 0 || indexB[3] == 0 ? 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 b3LessThanW( 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; -} - -static inline bool b3AnyTrueW( b3FloatW mask ) -{ - return mask.x != 0.0f || mask.y != 0.0f || mask.z != 0.0f || mask.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; -} - -static inline b3FloatW b3EmbedIndexW( b3FloatW value, int baseIndex, int bitCount ) -{ - uint32_t mask = ( 1u << bitCount ) - 1; - float lanes[4] = { value.x, value.y, value.z, value.w }; - - for ( int i = 0; i < 4; ++i ) - { - uint32_t bits; - memcpy( &bits, lanes + i, sizeof( bits ) ); - bits = ( bits & ~mask ) | (uint32_t)( baseIndex + i ); - memcpy( lanes + i, &bits, sizeof( bits ) ); - } - - return (b3FloatW){ lanes[0], lanes[1], lanes[2], lanes[3] }; -} - -static inline int b3MinIndexW( b3FloatW a, int bitCount ) -{ - float m = a.x; - m = a.y < m ? a.y : m; - m = a.z < m ? a.z : m; - m = a.w < m ? a.w : m; - - uint32_t bits; - memcpy( &bits, &m, sizeof( bits ) ); - return (int)( bits & ( ( 1u << bitCount ) - 1 ) ); -} - -#endif - -static inline void b3TransposeW( b3FloatW r0, b3FloatW r1, b3FloatW r2, b3FloatW r3, b3FloatW* c0, b3FloatW* c1, b3FloatW* c2, - b3FloatW* c3 ) -{ - b3FloatW t0 = b3UnpackLoW( r0, r2 ); - b3FloatW t1 = b3UnpackLoW( r1, r3 ); - b3FloatW t2 = b3UnpackHiW( r0, r2 ); - b3FloatW t3 = b3UnpackHiW( r1, r3 ); - *c0 = b3UnpackLoW( t0, t1 ); - *c1 = b3UnpackHiW( t0, t1 ); - *c2 = b3UnpackLoW( t2, t3 ); - *c3 = b3UnpackHiW( t2, t3 ); -} - -// Wide vec3 -typedef struct b3Vec3W -{ - b3FloatW X, Y, Z; -} b3Vec3W; - -static inline b3Vec3W b3LoadVW( const float* x, const float* y, const float* z ) -{ - return (b3Vec3W){ b3LoadW( x ), b3LoadW( y ), b3LoadW( z ) }; -} - -static inline void b3StoreVW( float* x, float* y, float* z, b3Vec3W v ) -{ - b3StoreW( x, v.X ); - b3StoreW( y, v.Y ); - b3StoreW( z, v.Z ); -} - -static inline b3Vec3W b3SplatVW( b3Vec3 v ) -{ - return (b3Vec3W){ b3SplatW( v.x ), b3SplatW( v.y ), b3SplatW( v.z ) }; -} - -static inline b3Vec3W b3NegVW( b3Vec3W a ) -{ - return (b3Vec3W){ b3NegW( a.X ), b3NegW( a.Y ), b3NegW( a.Z ) }; -} - -// 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 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 ), - }; -} - -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; -} - -#if defined( B3_SIMD_NEON ) - -typedef struct b3AABBV -{ - float32x4_t lower; - float32x4_t upper; -} b3AABBV; - -B3_FORCE_INLINE b3AABBV b3LoadAABBV( const b3AABB* aabb ) -{ - const float* base = &aabb->lowerBound.x; - - // Offset to avoid reading off the end (avoid UB). - // [lz ux uy uz] - float32x4_t v1 = vld1q_f32( base + 2 ); - b3AABBV result; - // [lx ly lz -] - result.lower = vld1q_f32( base ); - // [ux uy uz -] - result.upper = vextq_f32( v1, v1, 1 ); - return result; -} - -B3_FORCE_INLINE bool b3OverlapAABBV( b3AABBV a, b3AABBV b ) -{ - static const uint32_t laneMask[4] = { 0, 0, 0, 0xFFFFFFFFu }; - uint32x4_t test = vandq_u32( vcleq_f32( a.lower, b.upper ), vcleq_f32( b.lower, a.upper ) ); - return vminvq_u32( vorrq_u32( test, vld1q_u32( laneMask ) ) ) != 0; -} - -B3_FORCE_INLINE bool b3OverlapNode( b3AABBV av, const b3TreeNode* node ) -{ - return b3OverlapAABBV( av, b3LoadAABBV( &node->aabb ) ); -} - -B3_FORCE_INLINE bool b3OverlapV( const b3AABB* a, const b3AABB* b ) -{ - return b3OverlapAABBV( b3LoadAABBV( a ), b3LoadAABBV( b ) ); -} - -B3_FORCE_INLINE b3AABBV b3UnionAABBV( b3AABBV a, b3AABBV b ) -{ - b3AABBV result; - result.lower = vminq_f32( a.lower, b.lower ); - result.upper = vmaxq_f32( a.upper, b.upper ); - return result; -} - -B3_FORCE_INLINE b3AABBV b3UnionPairV( const b3TreeNode* pair ) -{ - return b3UnionAABBV( b3LoadAABBV( &pair[0].aabb ), b3LoadAABBV( &pair[1].aabb ) ); -} - -B3_FORCE_INLINE void b3StoreAABBV( b3AABB* aabb, b3AABBV value, bool condition ) -{ - float32x4_t raw0 = vsetq_lane_f32( vgetq_lane_f32( value.upper, 0 ), value.lower, 3 ); - float32x4_t rotated = vextq_f32( value.upper, value.upper, 1 ); - float32x4_t raw1 = vcombine_f32( vget_high_f32( raw0 ), vget_low_f32( rotated ) ); - - float* base = &aabb->lowerBound.x; - uint32x4_t mask = vdupq_n_u32( condition ? 0xFFFFFFFFu : 0u ); - vst1q_f32( base, vbslq_f32( mask, raw0, vld1q_f32( base ) ) ); - vst1q_f32( base + 2, vbslq_f32( mask, raw1, vld1q_f32( base + 2 ) ) ); -} - -#elif defined( B3_SIMD_SSE2 ) - -typedef struct b3AABBV -{ - __m128 lower; - __m128 upper; -} b3AABBV; - -B3_FORCE_INLINE b3AABBV b3LoadAABBV( const b3AABB* aabb ) -{ - const float* base = &aabb->lowerBound.x; - // Same offset as Neon to avoid UB. - __m128 v1 = _mm_loadu_ps( base + 2 ); - b3AABBV result; - result.lower = _mm_loadu_ps( base ); - result.upper = _mm_shuffle_ps( v1, v1, _MM_SHUFFLE( 3, 3, 2, 1 ) ); - return result; -} - -B3_FORCE_INLINE bool b3OverlapAABBV( b3AABBV a, b3AABBV b ) -{ - __m128 test = _mm_and_ps( _mm_cmple_ps( a.lower, b.upper ), _mm_cmple_ps( b.lower, a.upper ) ); - return ( _mm_movemask_ps( test ) & 0x7 ) == 0x7; -} - -B3_FORCE_INLINE bool b3OverlapNode( b3AABBV av, const b3TreeNode* node ) -{ - return b3OverlapAABBV( av, b3LoadAABBV( &node->aabb ) ); -} - -B3_FORCE_INLINE bool b3OverlapV( const b3AABB* a, const b3AABB* b ) -{ - return b3OverlapAABBV( b3LoadAABBV( a ), b3LoadAABBV( b ) ); -} - -B3_FORCE_INLINE b3AABBV b3UnionAABBV( b3AABBV a, b3AABBV b ) -{ - b3AABBV result; - result.lower = _mm_min_ps( a.lower, b.lower ); - result.upper = _mm_max_ps( a.upper, b.upper ); - return result; -} - -B3_FORCE_INLINE b3AABBV b3UnionPairV( const b3TreeNode* pair ) -{ - return b3UnionAABBV( b3LoadAABBV( &pair[0].aabb ), b3LoadAABBV( &pair[1].aabb ) ); -} - -// Conditionally store an AABB. Avoids a branch. -B3_FORCE_INLINE void b3StoreAABBV( b3AABB* aabb, b3AABBV value, bool condition ) -{ - __m128 lane3 = _mm_castsi128_ps( _mm_set_epi32( -1, 0, 0, 0 ) ); - __m128 uxSplat = _mm_shuffle_ps( value.upper, value.upper, _MM_SHUFFLE( 0, 0, 0, 0 ) ); - __m128 raw0 = _mm_or_ps( _mm_andnot_ps( lane3, value.lower ), _mm_and_ps( lane3, uxSplat ) ); - __m128 raw1 = _mm_shuffle_ps( raw0, value.upper, _MM_SHUFFLE( 2, 1, 3, 2 ) ); - - float* base = &aabb->lowerBound.x; - __m128 mask = _mm_castsi128_ps( _mm_set1_epi32( condition ? -1 : 0 ) ); - __m128 old0 = _mm_loadu_ps( base ); - __m128 old1 = _mm_loadu_ps( base + 2 ); - _mm_storeu_ps( base, _mm_or_ps( _mm_and_ps( mask, raw0 ), _mm_andnot_ps( mask, old0 ) ) ); - _mm_storeu_ps( base + 2, _mm_or_ps( _mm_and_ps( mask, raw1 ), _mm_andnot_ps( mask, old1 ) ) ); -} - -#else - -typedef b3AABB b3AABBV; - -B3_FORCE_INLINE b3AABBV b3LoadAABBV( const b3AABB* aabb ) -{ - return *aabb; -} - -B3_FORCE_INLINE bool b3OverlapAABBV( b3AABBV a, b3AABBV b ) -{ - return a.lowerBound.x <= b.upperBound.x && a.lowerBound.y <= b.upperBound.y && a.lowerBound.z <= b.upperBound.z && - b.lowerBound.x <= a.upperBound.x && b.lowerBound.y <= a.upperBound.y && b.lowerBound.z <= a.upperBound.z; -} - -B3_FORCE_INLINE bool b3OverlapNode( b3AABBV av, const b3TreeNode* node ) -{ - return b3OverlapAABBV( av, node->aabb ); -} - -B3_FORCE_INLINE bool b3OverlapV( const b3AABB* a, const b3AABB* b ) -{ - return b3OverlapAABBV( *a, *b ); -} - -B3_FORCE_INLINE b3AABBV b3UnionAABBV( b3AABBV a, b3AABBV b ) -{ - return b3AABB_Union( a, b ); -} - -B3_FORCE_INLINE b3AABBV b3UnionPairV( const b3TreeNode* pair ) -{ - return b3AABB_Union( pair[0].aabb, pair[1].aabb ); -} - -B3_FORCE_INLINE void b3StoreAABBV( b3AABB* aabb, b3AABBV value, bool condition ) -{ - if ( condition ) - { - *aabb = value; - } -} - -#endif - B3_FORCE_INLINE b3AABB b3UnionV( b3AABB a, b3AABB b ) { b3AABB result; diff --git a/src/simd_avx2.h b/src/simd_avx2.h new file mode 100644 index 0000000..a751249 --- /dev/null +++ b/src/simd_avx2.h @@ -0,0 +1,216 @@ +// SPDX-FileCopyrightText: 2026 Erin Catto +// SPDX-License-Identifier: MIT + +#pragma once + +#include "core.h" + +#include "box3d/types.h" + +#include + +// wide float holds 8 numbers +typedef __m256 b3FloatW8; + +static inline b3FloatW8 b3ZeroW8( void ) +{ + return _mm256_setzero_ps(); +} + +static inline b3FloatW8 b3SplatW8( float scalar ) +{ + return _mm256_set1_ps( scalar ); +} + +static inline b3FloatW8 b3SetW8( float a, float b, float c, float d, float e, float f, float g, float h ) +{ + return _mm256_setr_ps( a, b, c, d, e, f, g, h ); +} + +static inline b3FloatW8 b3LoadW8( const float* data ) +{ + return _mm256_loadu_ps( data ); +} + +static inline void b3StoreW8( float* data, b3FloatW8 a ) +{ + _mm256_storeu_ps( data, a ); +} + +static inline b3FloatW8 b3NegW8( b3FloatW8 a ) +{ + // Create a mask with the sign bit set for each element. + __m256 mask = _mm256_set1_ps( -0.0f ); + + // XOR the input with the mask to negate each element. + return _mm256_xor_ps( a, mask ); +} + +static inline b3FloatW8 b3AbsW8( b3FloatW8 a ) +{ + return _mm256_andnot_ps( _mm256_set1_ps( -0.0f ), a ); +} + +static inline b3FloatW8 b3AddW8( b3FloatW8 a, b3FloatW8 b ) +{ + return _mm256_add_ps( a, b ); +} + +static inline b3FloatW8 b3SubW8( b3FloatW8 a, b3FloatW8 b ) +{ + return _mm256_sub_ps( a, b ); +} + +static inline b3FloatW8 b3MulW8( b3FloatW8 a, b3FloatW8 b ) +{ + return _mm256_mul_ps( a, b ); +} + +static inline b3FloatW8 b3DivW8( b3FloatW8 a, b3FloatW8 b ) +{ + return _mm256_div_ps( a, b ); +} + +static inline b3FloatW8 b3SqrtW8( b3FloatW8 a ) +{ + return _mm256_sqrt_ps( a ); +} + +// a + b * c +static inline b3FloatW8 b3MulAddW8( b3FloatW8 a, b3FloatW8 b, b3FloatW8 c ) +{ + return _mm256_add_ps( a, _mm256_mul_ps( b, c ) ); +} + +static inline b3FloatW8 b3MinW8( b3FloatW8 a, b3FloatW8 b ) +{ + return _mm256_min_ps( a, b ); +} + +static inline b3FloatW8 b3MaxW8( b3FloatW8 a, b3FloatW8 b ) +{ + return _mm256_max_ps( a, b ); +} + +// Clamp a to [-b, b] +static inline b3FloatW8 b3SymClampW8( b3FloatW8 a, b3FloatW8 b ) +{ + b3FloatW8 nb = b3NegW8( b ); + b3FloatW8 c = b3MaxW8( nb, a ); + return b3MinW8( c, b ); +} + +static inline b3FloatW8 b3AndW8( b3FloatW8 a, b3FloatW8 b ) +{ + return _mm256_and_ps( a, b ); +} + +static inline b3FloatW8 b3OrW8( b3FloatW8 a, b3FloatW8 b ) +{ + return _mm256_or_ps( a, b ); +} + +// a & ~b +static inline b3FloatW8 b3AndNotW8( b3FloatW8 a, b3FloatW8 b ) +{ + // Arguments are reversed + return _mm256_andnot_ps( b, a ); +} + +// This is used to optimize selection of contact softness. +static inline b3FloatW8 b3SoftMaskW8( const int* indexA, const int* indexB ) +{ + __m256i zero = _mm256_setzero_si256(); + __m256i a = _mm256_cmpeq_epi32( _mm256_loadu_si256( (const __m256i*)indexA ), zero ); + __m256i b = _mm256_cmpeq_epi32( _mm256_loadu_si256( (const __m256i*)indexB ), zero ); + return _mm256_castsi256_ps( _mm256_or_si256( a, b ) ); +} + +static inline b3FloatW8 b3GreaterThanW8( b3FloatW8 a, b3FloatW8 b ) +{ + return _mm256_cmp_ps( a, b, _CMP_GT_OQ ); +} + +static inline b3FloatW8 b3LessThanW8( b3FloatW8 a, b3FloatW8 b ) +{ + return _mm256_cmp_ps( a, b, _CMP_LT_OQ ); +} + +static inline b3FloatW8 b3EqualsW8( b3FloatW8 a, b3FloatW8 b ) +{ + return _mm256_cmp_ps( a, b, _CMP_EQ_OQ ); +} + +static inline bool b3AllZeroW8( b3FloatW8 a ) +{ + // Compare each element with zero + b3FloatW8 zero = _mm256_setzero_ps(); + b3FloatW8 cmp = _mm256_cmp_ps( a, zero, _CMP_EQ_OQ ); + + // Create a mask from the comparison results + int mask = _mm256_movemask_ps( cmp ); + + // If all elements are zero, the mask will be 0xFF (11111111 in binary) + return mask == 0xFF; +} + +static inline bool b3AnyTrueW8( b3FloatW8 mask ) +{ + return _mm256_movemask_ps( mask ) != 0; +} + +// Component-wise returns mask ? b : a +static inline b3FloatW8 b3BlendW8( b3FloatW8 a, b3FloatW8 b, b3FloatW8 mask ) +{ + return _mm256_or_ps( _mm256_and_ps( mask, b ), _mm256_andnot_ps( mask, a ) ); +} + +// Replace the low bitCount mantissa bits of each lane with baseIndex + lane. The value must be +// positive so the embedded index sorts with the value, and ties fall to the lower index. +static inline b3FloatW8 b3EmbedIndexW8( b3FloatW8 value, int baseIndex, int bitCount ) +{ + int mask = ( 1 << bitCount ) - 1; + __m256i index = _mm256_add_epi32( _mm256_set1_epi32( baseIndex ), _mm256_setr_epi32( 0, 1, 2, 3, 4, 5, 6, 7 ) ); + __m256 clearLow = _mm256_castsi256_ps( _mm256_set1_epi32( ~mask ) ); + return _mm256_or_ps( _mm256_and_ps( value, clearLow ), _mm256_castsi256_ps( index ) ); +} + +// Recovers the index embedded by b3EmbedIndexW8 from the lane holding the minimum. +static inline int b3MinIndexW8( b3FloatW8 a, int bitCount ) +{ + __m128 min = _mm_min_ps( _mm256_castps256_ps128( a ), _mm256_extractf128_ps( a, 1 ) ); + min = _mm_min_ps( _mm_shuffle_ps( min, min, _MM_SHUFFLE( 2, 3, 0, 1 ) ), min ); + min = _mm_min_ps( _mm_shuffle_ps( min, min, _MM_SHUFFLE( 1, 0, 3, 2 ) ), min ); + return _mm_cvtsi128_si32( _mm_castps_si128( min ) ) & ( ( 1 << bitCount ) - 1 ); +} + +B3_FORCE_INLINE void b3TransposeW8( b3FloatW8 r0, b3FloatW8 r1, b3FloatW8 r2, b3FloatW8 r3, b3FloatW8 r4, b3FloatW8 r5, + b3FloatW8 r6, b3FloatW8 r7, b3FloatW8* c0, b3FloatW8* c1, b3FloatW8* c2, b3FloatW8* c3, + b3FloatW8* c4, b3FloatW8* c5, b3FloatW8* c6, b3FloatW8* c7 ) +{ + b3FloatW8 t0 = _mm256_unpacklo_ps( r0, r1 ); + b3FloatW8 t1 = _mm256_unpackhi_ps( r0, r1 ); + b3FloatW8 t2 = _mm256_unpacklo_ps( r2, r3 ); + b3FloatW8 t3 = _mm256_unpackhi_ps( r2, r3 ); + b3FloatW8 t4 = _mm256_unpacklo_ps( r4, r5 ); + b3FloatW8 t5 = _mm256_unpackhi_ps( r4, r5 ); + b3FloatW8 t6 = _mm256_unpacklo_ps( r6, r7 ); + b3FloatW8 t7 = _mm256_unpackhi_ps( r6, r7 ); + b3FloatW8 tt0 = _mm256_shuffle_ps( t0, t2, _MM_SHUFFLE( 1, 0, 1, 0 ) ); + b3FloatW8 tt1 = _mm256_shuffle_ps( t0, t2, _MM_SHUFFLE( 3, 2, 3, 2 ) ); + b3FloatW8 tt2 = _mm256_shuffle_ps( t1, t3, _MM_SHUFFLE( 1, 0, 1, 0 ) ); + b3FloatW8 tt3 = _mm256_shuffle_ps( t1, t3, _MM_SHUFFLE( 3, 2, 3, 2 ) ); + b3FloatW8 tt4 = _mm256_shuffle_ps( t4, t6, _MM_SHUFFLE( 1, 0, 1, 0 ) ); + b3FloatW8 tt5 = _mm256_shuffle_ps( t4, t6, _MM_SHUFFLE( 3, 2, 3, 2 ) ); + b3FloatW8 tt6 = _mm256_shuffle_ps( t5, t7, _MM_SHUFFLE( 1, 0, 1, 0 ) ); + b3FloatW8 tt7 = _mm256_shuffle_ps( t5, t7, _MM_SHUFFLE( 3, 2, 3, 2 ) ); + + *c0 = _mm256_permute2f128_ps( tt0, tt4, 0x20 ); + *c1 = _mm256_permute2f128_ps( tt1, tt5, 0x20 ); + *c2 = _mm256_permute2f128_ps( tt2, tt6, 0x20 ); + *c3 = _mm256_permute2f128_ps( tt3, tt7, 0x20 ); + *c4 = _mm256_permute2f128_ps( tt0, tt4, 0x31 ); + *c5 = _mm256_permute2f128_ps( tt1, tt5, 0x31 ); + *c6 = _mm256_permute2f128_ps( tt2, tt6, 0x31 ); + *c7 = _mm256_permute2f128_ps( tt3, tt7, 0x31 ); +} diff --git a/src/simd_neon.h b/src/simd_neon.h new file mode 100644 index 0000000..97bd8d1 --- /dev/null +++ b/src/simd_neon.h @@ -0,0 +1,265 @@ +// SPDX-FileCopyrightText: 2026 Erin Catto +// SPDX-License-Identifier: MIT + +#pragma once + +#include "core.h" + +#include "box3d/types.h" + +#include + +// wide float holds 4 numbers +typedef float32x4_t b3FloatW4; + +static inline b3FloatW4 b3ZeroW4( void ) +{ + return vdupq_n_f32( 0.0f ); +} + +static inline b3FloatW4 b3SplatW4( float scalar ) +{ + return vdupq_n_f32( scalar ); +} + +static inline b3FloatW4 b3SetW4( float a, float b, float c, float d ) +{ + float32_t array[4] = { a, b, c, d }; + return vld1q_f32( array ); +} + +static inline b3FloatW4 b3LoadW4( const float* data ) +{ + return vld1q_f32( data ); +} + +static inline void b3StoreW4( float* data, b3FloatW4 a ) +{ + vst1q_f32( data, a ); +} + +static inline b3FloatW4 b3NegW4( b3FloatW4 a ) +{ + return vnegq_f32( a ); +} + +static inline b3FloatW4 b3AbsW4( b3FloatW4 a ) +{ + return vabsq_f32( a ); +} + +static inline b3FloatW4 b3AddW4( b3FloatW4 a, b3FloatW4 b ) +{ + return vaddq_f32( a, b ); +} + +static inline b3FloatW4 b3SubW4( b3FloatW4 a, b3FloatW4 b ) +{ + return vsubq_f32( a, b ); +} + +static inline b3FloatW4 b3MulW4( b3FloatW4 a, b3FloatW4 b ) +{ + return vmulq_f32( a, b ); +} + +static inline b3FloatW4 b3DivW4( b3FloatW4 a, b3FloatW4 b ) +{ + return vdivq_f32( a, b ); +} + +static inline b3FloatW4 b3SqrtW4( b3FloatW4 a ) +{ + return vsqrtq_f32( a ); +} + +// Cannot use real FMA because it doesn't match the non-SIMD path +static inline b3FloatW4 b3MulAddW4( b3FloatW4 a, b3FloatW4 b, b3FloatW4 c ) +{ + return vaddq_f32( a, vmulq_f32( b, c ) ); +} + +static inline b3FloatW4 b3MinW4( b3FloatW4 a, b3FloatW4 b ) +{ + return vminq_f32( a, b ); +} + +static inline b3FloatW4 b3MaxW4( b3FloatW4 a, b3FloatW4 b ) +{ + return vmaxq_f32( a, b ); +} + +// clamp a to [-b, b] +static inline b3FloatW4 b3SymClampW4( b3FloatW4 a, b3FloatW4 b ) +{ + b3FloatW4 nb = b3NegW4( b ); + b3FloatW4 c = b3MaxW4( nb, a ); + return b3MinW4( c, b ); +} + +static inline b3FloatW4 b3AndW4( b3FloatW4 a, b3FloatW4 b ) +{ + return vreinterpretq_f32_u32( vandq_u32( vreinterpretq_u32_f32( a ), vreinterpretq_u32_f32( b ) ) ); +} + +static inline b3FloatW4 b3OrW4( b3FloatW4 a, b3FloatW4 b ) +{ + return vreinterpretq_f32_u32( vorrq_u32( vreinterpretq_u32_f32( a ), vreinterpretq_u32_f32( b ) ) ); +} + +// a & ~b +static inline b3FloatW4 b3AndNotW4( b3FloatW4 a, b3FloatW4 b ) +{ + return vreinterpretq_f32_u32( vbicq_u32( vreinterpretq_u32_f32( a ), vreinterpretq_u32_f32( b ) ) ); +} + +static inline b3FloatW4 b3SoftMaskW4( const int* indexA, const int* indexB ) +{ + int32x4_t zero = vdupq_n_s32( 0 ); + uint32x4_t a = vceqq_s32( vld1q_s32( (const int32_t*)indexA ), zero ); + uint32x4_t b = vceqq_s32( vld1q_s32( (const int32_t*)indexB ), zero ); + return vreinterpretq_f32_u32( vorrq_u32( a, b ) ); +} + +static inline b3FloatW4 b3GreaterThanW4( b3FloatW4 a, b3FloatW4 b ) +{ + return vreinterpretq_f32_u32( vcgtq_f32( a, b ) ); +} + +static inline b3FloatW4 b3LessThanW4( b3FloatW4 a, b3FloatW4 b ) +{ + return vreinterpretq_f32_u32( vcltq_f32( a, b ) ); +} + +static inline b3FloatW4 b3EqualsW4( b3FloatW4 a, b3FloatW4 b ) +{ + return vreinterpretq_f32_u32( vceqq_f32( a, b ) ); +} + +static inline bool b3AllZeroW4( b3FloatW4 a ) +{ + // Create a zero vector for comparison + b3FloatW4 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 +} + +// _mm_movemask_ps equivalent, compatible with ARM v7. +static inline bool b3AnyTrueW4( b3FloatW4 mask ) +{ + uint32x4_t m = vreinterpretq_u32_f32( mask ); + uint32x2_t p = vorr_u32( vget_low_u32( m ), vget_high_u32( m ) ); + return ( vget_lane_u32( p, 0 ) | vget_lane_u32( p, 1 ) ) != 0; +} + +// component-wise returns mask ? b : a +static inline b3FloatW4 b3BlendW4( b3FloatW4 a, b3FloatW4 b, b3FloatW4 mask ) +{ + uint32x4_t mask32 = vreinterpretq_u32_f32( mask ); + return vbslq_f32( mask32, b, a ); +} + +static inline b3FloatW4 b3EmbedIndexW4( b3FloatW4 value, int baseIndex, int bitCount ) +{ + uint32_t mask = ( 1u << bitCount ) - 1; + const int32_t lanes[4] = { 0, 1, 2, 3 }; + int32x4_t index = vaddq_s32( vdupq_n_s32( baseIndex ), vld1q_s32( lanes ) ); + uint32x4_t clearLow = vdupq_n_u32( ~mask ); + uint32x4_t bits = vorrq_u32( vandq_u32( vreinterpretq_u32_f32( value ), clearLow ), vreinterpretq_u32_s32( index ) ); + return vreinterpretq_f32_u32( bits ); +} + +static inline int b3MinIndexW4( b3FloatW4 a, int bitCount ) +{ + float32x2_t m = vmin_f32( vget_low_f32( a ), vget_high_f32( a ) ); + m = vpmin_f32( m, m ); + uint32_t bits = vget_lane_u32( vreinterpret_u32_f32( m ), 0 ); + return (int)( bits & ( ( 1u << bitCount ) - 1 ) ); +} + +typedef struct b3AABBV +{ + float32x4_t lower; + float32x4_t upper; +} b3AABBV; + +B3_FORCE_INLINE b3AABBV b3LoadAABBV( const b3AABB* aabb ) +{ + const float* base = &aabb->lowerBound.x; + + // Offset to avoid reading off the end (avoid UB). + // [lz ux uy uz] + float32x4_t v1 = vld1q_f32( base + 2 ); + b3AABBV result; + // [lx ly lz -] + result.lower = vld1q_f32( base ); + // [ux uy uz -] + result.upper = vextq_f32( v1, v1, 1 ); + return result; +} + +B3_FORCE_INLINE bool b3OverlapAABBV( b3AABBV a, b3AABBV b ) +{ + static const uint32_t laneMask[4] = { 0, 0, 0, 0xFFFFFFFFu }; + uint32x4_t test = vandq_u32( vcleq_f32( a.lower, b.upper ), vcleq_f32( b.lower, a.upper ) ); + return vminvq_u32( vorrq_u32( test, vld1q_u32( laneMask ) ) ) != 0; +} + +B3_FORCE_INLINE bool b3OverlapNode( b3AABBV av, const b3TreeNode* node ) +{ + return b3OverlapAABBV( av, b3LoadAABBV( &node->aabb ) ); +} + +B3_FORCE_INLINE bool b3OverlapV( const b3AABB* a, const b3AABB* b ) +{ + return b3OverlapAABBV( b3LoadAABBV( a ), b3LoadAABBV( b ) ); +} + +B3_FORCE_INLINE b3AABBV b3UnionAABBV( b3AABBV a, b3AABBV b ) +{ + b3AABBV result; + result.lower = vminq_f32( a.lower, b.lower ); + result.upper = vmaxq_f32( a.upper, b.upper ); + return result; +} + +B3_FORCE_INLINE b3AABBV b3UnionPairV( const b3TreeNode* pair ) +{ + return b3UnionAABBV( b3LoadAABBV( &pair[0].aabb ), b3LoadAABBV( &pair[1].aabb ) ); +} + +B3_FORCE_INLINE void b3StoreAABBV( b3AABB* aabb, b3AABBV value, bool condition ) +{ + float32x4_t raw0 = vsetq_lane_f32( vgetq_lane_f32( value.upper, 0 ), value.lower, 3 ); + float32x4_t rotated = vextq_f32( value.upper, value.upper, 1 ); + float32x4_t raw1 = vcombine_f32( vget_high_f32( raw0 ), vget_low_f32( rotated ) ); + + float* base = &aabb->lowerBound.x; + uint32x4_t mask = vdupq_n_u32( condition ? 0xFFFFFFFFu : 0u ); + vst1q_f32( base, vbslq_f32( mask, raw0, vld1q_f32( base ) ) ); + vst1q_f32( base + 2, vbslq_f32( mask, raw1, vld1q_f32( base + 2 ) ) ); +} + +B3_FORCE_INLINE void b3TransposeW4( b3FloatW4 r0, b3FloatW4 r1, b3FloatW4 r2, b3FloatW4 r3, b3FloatW4* c0, b3FloatW4* c1, + b3FloatW4* c2, b3FloatW4* c3 ) +{ + b3FloatW4 t0 = vzip1q_f32( r0, r2 ); + b3FloatW4 t1 = vzip1q_f32( r1, r3 ); + b3FloatW4 t2 = vzip2q_f32( r0, r2 ); + b3FloatW4 t3 = vzip2q_f32( r1, r3 ); + *c0 = vzip1q_f32( t0, t1 ); + *c1 = vzip2q_f32( t0, t1 ); + *c2 = vzip1q_f32( t2, t3 ); + *c3 = vzip2q_f32( t2, t3 ); +} diff --git a/src/simd_scalar.h b/src/simd_scalar.h new file mode 100644 index 0000000..144acc1 --- /dev/null +++ b/src/simd_scalar.h @@ -0,0 +1,296 @@ +// SPDX-FileCopyrightText: 2026 Erin Catto +// SPDX-License-Identifier: MIT + +#pragma once + +#include "core.h" + +#include "box3d/types.h" + +#include +#include + +// scalar math +typedef struct b3FloatW4 +{ + float x, y, z, w; +} b3FloatW4; + +static inline b3FloatW4 b3ZeroW4( void ) +{ + return (b3FloatW4){ 0.0f, 0.0f, 0.0f, 0.0f }; +} + +static inline b3FloatW4 b3SplatW4( float scalar ) +{ + return (b3FloatW4){ scalar, scalar, scalar, scalar }; +} + +static inline b3FloatW4 b3SetW4( float a, float b, float c, float d ) +{ + return (b3FloatW4){ a, b, c, d }; +} + +static inline b3FloatW4 b3LoadW4( const float* data ) +{ + return (b3FloatW4){ data[0], data[1], data[2], data[3] }; +} + +static inline void b3StoreW4( float* data, b3FloatW4 a ) +{ + data[0] = a.x; + data[1] = a.y; + data[2] = a.z; + data[3] = a.w; +} + +static inline b3FloatW4 b3NegW4( b3FloatW4 a ) +{ + return (b3FloatW4){ -a.x, -a.y, -a.z, -a.w }; +} + +static inline b3FloatW4 b3AbsW4( b3FloatW4 a ) +{ + return (b3FloatW4){ a.x < 0.0f ? -a.x : a.x, a.y < 0.0f ? -a.y : a.y, a.z < 0.0f ? -a.z : a.z, a.w < 0.0f ? -a.w : a.w }; +} + +static inline b3FloatW4 b3AddW4( b3FloatW4 a, b3FloatW4 b ) +{ + return (b3FloatW4){ a.x + b.x, a.y + b.y, a.z + b.z, a.w + b.w }; +} + +static inline b3FloatW4 b3SubW4( b3FloatW4 a, b3FloatW4 b ) +{ + return (b3FloatW4){ a.x - b.x, a.y - b.y, a.z - b.z, a.w - b.w }; +} + +static inline b3FloatW4 b3MulW4( b3FloatW4 a, b3FloatW4 b ) +{ + return (b3FloatW4){ a.x * b.x, a.y * b.y, a.z * b.z, a.w * b.w }; +} + +static inline b3FloatW4 b3DivW4( b3FloatW4 a, b3FloatW4 b ) +{ + return (b3FloatW4){ a.x / b.x, a.y / b.y, a.z / b.z, a.w / b.w }; +} + +static inline b3FloatW4 b3SqrtW4( b3FloatW4 a ) +{ + return (b3FloatW4){ sqrtf( a.x ), sqrtf( a.y ), sqrtf( a.z ), sqrtf( a.w ) }; +} + +static inline b3FloatW4 b3MulAddW4( b3FloatW4 a, b3FloatW4 b, b3FloatW4 c ) +{ + return (b3FloatW4){ 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 b3FloatW4 b3MinW4( b3FloatW4 a, b3FloatW4 b ) +{ + b3FloatW4 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 b3FloatW4 b3MaxW4( b3FloatW4 a, b3FloatW4 b ) +{ + b3FloatW4 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 b3FloatW4 b3SymClampW4( b3FloatW4 a, b3FloatW4 b ) +{ + b3FloatW4 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; +} + +// Logical operations on the scalar path are 0/1 float values. Not bit-wise like SIMD. + +static inline b3FloatW4 b3AndW4( b3FloatW4 a, b3FloatW4 b ) +{ + b3FloatW4 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 b3FloatW4 b3OrW4( b3FloatW4 a, b3FloatW4 b ) +{ + b3FloatW4 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; +} + +// a & ~b +static inline b3FloatW4 b3AndNotW4( b3FloatW4 a, b3FloatW4 b ) +{ + b3FloatW4 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 b3FloatW4 b3SoftMaskW4( const int* indexA, const int* indexB ) +{ + b3FloatW4 r; + r.x = indexA[0] == 0 || indexB[0] == 0 ? 1.0f : 0.0f; + r.y = indexA[1] == 0 || indexB[1] == 0 ? 1.0f : 0.0f; + r.z = indexA[2] == 0 || indexB[2] == 0 ? 1.0f : 0.0f; + r.w = indexA[3] == 0 || indexB[3] == 0 ? 1.0f : 0.0f; + return r; +} + +static inline b3FloatW4 b3GreaterThanW4( b3FloatW4 a, b3FloatW4 b ) +{ + b3FloatW4 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 b3FloatW4 b3LessThanW4( b3FloatW4 a, b3FloatW4 b ) +{ + b3FloatW4 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 b3FloatW4 b3EqualsW4( b3FloatW4 a, b3FloatW4 b ) +{ + b3FloatW4 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 b3AllZeroW4( b3FloatW4 a ) +{ + return a.x == 0.0f && a.y == 0.0f && a.z == 0.0f && a.w == 0.0f; +} + +static inline bool b3AnyTrueW4( b3FloatW4 mask ) +{ + return mask.x != 0.0f || mask.y != 0.0f || mask.z != 0.0f || mask.w != 0.0f; +} + +// component-wise returns mask ? b : a +static inline b3FloatW4 b3BlendW4( b3FloatW4 a, b3FloatW4 b, b3FloatW4 mask ) +{ + b3FloatW4 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; +} + +static inline b3FloatW4 b3EmbedIndexW4( b3FloatW4 value, int baseIndex, int bitCount ) +{ + uint32_t mask = ( 1u << bitCount ) - 1; + float lanes[4] = { value.x, value.y, value.z, value.w }; + + for ( int i = 0; i < 4; ++i ) + { + uint32_t bits; + memcpy( &bits, lanes + i, sizeof( bits ) ); + bits = ( bits & ~mask ) | (uint32_t)( baseIndex + i ); + memcpy( lanes + i, &bits, sizeof( bits ) ); + } + + return (b3FloatW4){ lanes[0], lanes[1], lanes[2], lanes[3] }; +} + +static inline int b3MinIndexW4( b3FloatW4 a, int bitCount ) +{ + float m = a.x; + m = a.y < m ? a.y : m; + m = a.z < m ? a.z : m; + m = a.w < m ? a.w : m; + + uint32_t bits; + memcpy( &bits, &m, sizeof( bits ) ); + return (int)( bits & ( ( 1u << bitCount ) - 1 ) ); +} + +typedef b3AABB b3AABBV; + +B3_FORCE_INLINE b3AABBV b3LoadAABBV( const b3AABB* aabb ) +{ + return *aabb; +} + +B3_FORCE_INLINE bool b3OverlapAABBV( b3AABBV a, b3AABBV b ) +{ + return a.lowerBound.x <= b.upperBound.x && a.lowerBound.y <= b.upperBound.y && a.lowerBound.z <= b.upperBound.z && + b.lowerBound.x <= a.upperBound.x && b.lowerBound.y <= a.upperBound.y && b.lowerBound.z <= a.upperBound.z; +} + +B3_FORCE_INLINE bool b3OverlapNode( b3AABBV av, const b3TreeNode* node ) +{ + return b3OverlapAABBV( av, node->aabb ); +} + +B3_FORCE_INLINE bool b3OverlapV( const b3AABB* a, const b3AABB* b ) +{ + return b3OverlapAABBV( *a, *b ); +} + +B3_FORCE_INLINE b3AABBV b3UnionAABBV( b3AABBV a, b3AABBV b ) +{ + return b3AABB_Union( a, b ); +} + +B3_FORCE_INLINE b3AABBV b3UnionPairV( const b3TreeNode* pair ) +{ + return b3AABB_Union( pair[0].aabb, pair[1].aabb ); +} + +B3_FORCE_INLINE void b3StoreAABBV( b3AABB* aabb, b3AABBV value, bool condition ) +{ + if ( condition ) + { + *aabb = value; + } +} + +B3_FORCE_INLINE void b3TransposeW4( b3FloatW4 r0, b3FloatW4 r1, b3FloatW4 r2, b3FloatW4 r3, b3FloatW4* c0, b3FloatW4* c1, + b3FloatW4* c2, b3FloatW4* c3 ) +{ + b3FloatW4 t0 = (b3FloatW4){ r0.x, r2.x, r0.y, r2.y }; + b3FloatW4 t1 = (b3FloatW4){ r1.x, r3.x, r1.y, r3.y }; + b3FloatW4 t2 = (b3FloatW4){ r0.z, r2.z, r0.w, r2.w }; + b3FloatW4 t3 = (b3FloatW4){ r1.z, r3.z, r1.w, r3.w }; + *c0 = (b3FloatW4){ t0.x, t1.x, t0.y, t1.y }; + *c1 = (b3FloatW4){ t0.z, t1.z, t0.w, t1.w }; + *c2 = (b3FloatW4){ t2.x, t3.x, t2.y, t3.y }; + *c3 = (b3FloatW4){ t2.z, t3.z, t2.w, t3.w }; +} diff --git a/src/simd_sse2.h b/src/simd_sse2.h new file mode 100644 index 0000000..22ae9a5 --- /dev/null +++ b/src/simd_sse2.h @@ -0,0 +1,414 @@ +// SPDX-FileCopyrightText: 2026 Erin Catto +// SPDX-License-Identifier: MIT + +#pragma once + +#include "core.h" + +#include "box3d/types.h" + +#include + +// wide float holds 4 numbers +typedef __m128 b3FloatW4; + +static inline b3FloatW4 b3ZeroW4( void ) +{ + return _mm_setzero_ps(); +} + +static inline b3FloatW4 b3SplatW4( float scalar ) +{ + return _mm_set1_ps( scalar ); +} + +static inline b3FloatW4 b3SetW4( float a, float b, float c, float d ) +{ + return _mm_setr_ps( a, b, c, d ); +} + +static inline b3FloatW4 b3LoadW4( const float* data ) +{ + return _mm_loadu_ps( data ); +} + +static inline void b3StoreW4( float* data, b3FloatW4 a ) +{ + _mm_storeu_ps( data, a ); +} + +static inline b3FloatW4 b3NegW4( b3FloatW4 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 b3FloatW4 b3AbsW4( b3FloatW4 a ) +{ + return _mm_andnot_ps( _mm_set1_ps( -0.0f ), a ); +} + +static inline b3FloatW4 b3AddW4( b3FloatW4 a, b3FloatW4 b ) +{ + return _mm_add_ps( a, b ); +} + +static inline b3FloatW4 b3SubW4( b3FloatW4 a, b3FloatW4 b ) +{ + return _mm_sub_ps( a, b ); +} + +static inline b3FloatW4 b3MulW4( b3FloatW4 a, b3FloatW4 b ) +{ + return _mm_mul_ps( a, b ); +} + +static inline b3FloatW4 b3DivW4( b3FloatW4 a, b3FloatW4 b ) +{ + return _mm_div_ps( a, b ); +} + +static inline b3FloatW4 b3SqrtW4( b3FloatW4 a ) +{ + return _mm_sqrt_ps( a ); +} + +// a + b * c +static inline b3FloatW4 b3MulAddW4( b3FloatW4 a, b3FloatW4 b, b3FloatW4 c ) +{ + return _mm_add_ps( a, _mm_mul_ps( b, c ) ); +} + +static inline b3FloatW4 b3MinW4( b3FloatW4 a, b3FloatW4 b ) +{ + return _mm_min_ps( a, b ); +} + +static inline b3FloatW4 b3MaxW4( b3FloatW4 a, b3FloatW4 b ) +{ + return _mm_max_ps( a, b ); +} + +// clamp a to [-b, b] +static inline b3FloatW4 b3SymClampW4( b3FloatW4 a, b3FloatW4 b ) +{ + b3FloatW4 nb = b3NegW4( b ); + b3FloatW4 c = b3MaxW4( nb, a ); + return b3MinW4( c, b ); +} + +static inline b3FloatW4 b3AndW4( b3FloatW4 a, b3FloatW4 b ) +{ + return _mm_and_ps( a, b ); +} + +static inline b3FloatW4 b3OrW4( b3FloatW4 a, b3FloatW4 b ) +{ + return _mm_or_ps( a, b ); +} + +// a & ~b +static inline b3FloatW4 b3AndNotW4( b3FloatW4 a, b3FloatW4 b ) +{ + // Arguments are reversed + return _mm_andnot_ps( b, a ); +} + +// This is used to optimize selection of contact softness. +static inline b3FloatW4 b3SoftMaskW4( const int* indexA, const int* indexB ) +{ + __m128i zero = _mm_setzero_si128(); + __m128i a = _mm_cmpeq_epi32( _mm_loadu_si128( (const __m128i*)indexA ), zero ); + __m128i b = _mm_cmpeq_epi32( _mm_loadu_si128( (const __m128i*)indexB ), zero ); + return _mm_castsi128_ps( _mm_or_si128( a, b ) ); +} + +static inline b3FloatW4 b3GreaterThanW4( b3FloatW4 a, b3FloatW4 b ) +{ + return _mm_cmpgt_ps( a, b ); +} + +static inline b3FloatW4 b3LessThanW4( b3FloatW4 a, b3FloatW4 b ) +{ + return _mm_cmplt_ps( a, b ); +} + +static inline b3FloatW4 b3EqualsW4( b3FloatW4 a, b3FloatW4 b ) +{ + return _mm_cmpeq_ps( a, b ); +} + +static inline bool b3AllZeroW4( b3FloatW4 a ) +{ + // Compare each element with zero + b3FloatW4 zero = _mm_setzero_ps(); + b3FloatW4 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; +} + +static inline bool b3AnyTrueW4( b3FloatW4 mask ) +{ + return _mm_movemask_ps( mask ) != 0; +} + +// component-wise returns mask ? b : a +static inline b3FloatW4 b3BlendW4( b3FloatW4 a, b3FloatW4 b, b3FloatW4 mask ) +{ + return _mm_or_ps( _mm_and_ps( mask, b ), _mm_andnot_ps( mask, a ) ); +} + +// Replace the low bitCount mantissa bits of each lane with baseIndex + lane. The value must be +// positive so the embedded index sorts with the value, and ties fall to the lower index. +static inline b3FloatW4 b3EmbedIndexW4( b3FloatW4 value, int baseIndex, int bitCount ) +{ + int mask = ( 1 << bitCount ) - 1; + __m128i index = _mm_add_epi32( _mm_set1_epi32( baseIndex ), _mm_setr_epi32( 0, 1, 2, 3 ) ); + __m128 clearLow = _mm_castsi128_ps( _mm_set1_epi32( ~mask ) ); + return _mm_or_ps( _mm_and_ps( value, clearLow ), _mm_castsi128_ps( index ) ); +} + +// Recovers the index embedded by b3EmbedIndexW4 from the lane holding the minimum. +static inline int b3MinIndexW4( b3FloatW4 a, int bitCount ) +{ + a = _mm_min_ps( a, _mm_shuffle_ps( a, a, _MM_SHUFFLE( 2, 3, 0, 1 ) ) ); + a = _mm_min_ps( a, _mm_shuffle_ps( a, a, _MM_SHUFFLE( 1, 0, 3, 2 ) ) ); + return _mm_cvtsi128_si32( _mm_castps_si128( a ) ) & ( ( 1 << bitCount ) - 1 ); +} + +B3_FORCE_INLINE void b3TransposeW4( b3FloatW4 r0, b3FloatW4 r1, b3FloatW4 r2, b3FloatW4 r3, b3FloatW4* c0, b3FloatW4* c1, + b3FloatW4* c2, b3FloatW4* c3 ) +{ + b3FloatW4 t0 = _mm_unpacklo_ps( r0, r2 ); + b3FloatW4 t1 = _mm_unpacklo_ps( r1, r3 ); + b3FloatW4 t2 = _mm_unpackhi_ps( r0, r2 ); + b3FloatW4 t3 = _mm_unpackhi_ps( r1, r3 ); + *c0 = _mm_unpacklo_ps( t0, t1 ); + *c1 = _mm_unpackhi_ps( t0, t1 ); + *c2 = _mm_unpacklo_ps( t2, t3 ); + *c3 = _mm_unpackhi_ps( t2, t3 ); +} + +// The following are SIMD definitions for operations where a wide register is +// used to store a single 3D vector, rather than SOA data for proper "wide" SIMD. +// For such uses, there's no benefit to using AVX2 8-wide registers; we're only using 3 lanes. +// For that reason, we unconditionally use SSE, even if AVX2 is available. + +// 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; +} + +typedef struct b3AABBV +{ + __m128 lower; + __m128 upper; +} b3AABBV; + +B3_FORCE_INLINE b3AABBV b3LoadAABBV( const b3AABB* aabb ) +{ + const float* base = &aabb->lowerBound.x; + // Same offset as Neon to avoid UB. + __m128 v1 = _mm_loadu_ps( base + 2 ); + b3AABBV result; + result.lower = _mm_loadu_ps( base ); + result.upper = _mm_shuffle_ps( v1, v1, _MM_SHUFFLE( 3, 3, 2, 1 ) ); + return result; +} + +B3_FORCE_INLINE bool b3OverlapAABBV( b3AABBV a, b3AABBV b ) +{ + __m128 test = _mm_and_ps( _mm_cmple_ps( a.lower, b.upper ), _mm_cmple_ps( b.lower, a.upper ) ); + return ( _mm_movemask_ps( test ) & 0x7 ) == 0x7; +} + +B3_FORCE_INLINE bool b3OverlapNode( b3AABBV av, const b3TreeNode* node ) +{ + return b3OverlapAABBV( av, b3LoadAABBV( &node->aabb ) ); +} + +B3_FORCE_INLINE bool b3OverlapV( const b3AABB* a, const b3AABB* b ) +{ + return b3OverlapAABBV( b3LoadAABBV( a ), b3LoadAABBV( b ) ); +} + +B3_FORCE_INLINE b3AABBV b3UnionAABBV( b3AABBV a, b3AABBV b ) +{ + b3AABBV result; + result.lower = _mm_min_ps( a.lower, b.lower ); + result.upper = _mm_max_ps( a.upper, b.upper ); + return result; +} + +B3_FORCE_INLINE b3AABBV b3UnionPairV( const b3TreeNode* pair ) +{ + return b3UnionAABBV( b3LoadAABBV( &pair[0].aabb ), b3LoadAABBV( &pair[1].aabb ) ); +} + +// Conditionally store an AABB. Avoids a branch. +B3_FORCE_INLINE void b3StoreAABBV( b3AABB* aabb, b3AABBV value, bool condition ) +{ + __m128 lane3 = _mm_castsi128_ps( _mm_set_epi32( -1, 0, 0, 0 ) ); + __m128 uxSplat = _mm_shuffle_ps( value.upper, value.upper, _MM_SHUFFLE( 0, 0, 0, 0 ) ); + __m128 raw0 = _mm_or_ps( _mm_andnot_ps( lane3, value.lower ), _mm_and_ps( lane3, uxSplat ) ); + __m128 raw1 = _mm_shuffle_ps( raw0, value.upper, _MM_SHUFFLE( 2, 1, 3, 2 ) ); + + float* base = &aabb->lowerBound.x; + __m128 mask = _mm_castsi128_ps( _mm_set1_epi32( condition ? -1 : 0 ) ); + __m128 old0 = _mm_loadu_ps( base ); + __m128 old1 = _mm_loadu_ps( base + 2 ); + _mm_storeu_ps( base, _mm_or_ps( _mm_and_ps( mask, raw0 ), _mm_andnot_ps( mask, old0 ) ) ); + _mm_storeu_ps( base + 2, _mm_or_ps( _mm_and_ps( mask, raw1 ), _mm_andnot_ps( mask, old1 ) ) ); +} diff --git a/src/simd_wide.h b/src/simd_wide.h new file mode 100644 index 0000000..21c9cba --- /dev/null +++ b/src/simd_wide.h @@ -0,0 +1,62 @@ +// SPDX-FileCopyrightText: 2026 Erin Catto +// SPDX-License-Identifier: MIT + +#pragma once + +#include "simd.h" + +// Macros to avoid name clashes between W4 and W8 functions existing in the same code base. +// At the same time this helps to avoid code duplication. + +#if B3_SIMD_WIDTH == 8 +#define B3_WIDE( name ) name##W8 +#elif B3_SIMD_WIDTH == 4 +#define B3_WIDE( name ) name##W4 +#else +#error "B3_SIMD_WIDTH must be 4 or 8" +#endif + +#define B3_WIDE_ALIGNMENT ( 4 * B3_SIMD_WIDTH ) + +#define b3FloatW B3_WIDE( b3Float ) +#define b3Vec3W B3_WIDE( b3Vec3 ) +#define b3ZeroW B3_WIDE( b3Zero ) +#define b3SplatW B3_WIDE( b3Splat ) +#define b3SetW B3_WIDE( b3Set ) +#define b3LoadW B3_WIDE( b3Load ) +#define b3StoreW B3_WIDE( b3Store ) +#define b3NegW B3_WIDE( b3Neg ) +#define b3AbsW B3_WIDE( b3Abs ) +#define b3AddW B3_WIDE( b3Add ) +#define b3SubW B3_WIDE( b3Sub ) +#define b3MulW B3_WIDE( b3Mul ) +#define b3DivW B3_WIDE( b3Div ) +#define b3SqrtW B3_WIDE( b3Sqrt ) +#define b3MulAddW B3_WIDE( b3MulAdd ) +#define b3MinW B3_WIDE( b3Min ) +#define b3MaxW B3_WIDE( b3Max ) +#define b3SymClampW B3_WIDE( b3SymClamp ) +#define b3AndW B3_WIDE( b3And ) +#define b3OrW B3_WIDE( b3Or ) +#define b3AndNotW B3_WIDE( b3AndNot ) +#define b3SoftMaskW B3_WIDE( b3SoftMask ) +#define b3GreaterThanW B3_WIDE( b3GreaterThan ) +#define b3LessThanW B3_WIDE( b3LessThan ) +#define b3EqualsW B3_WIDE( b3Equals ) +#define b3AllZeroW B3_WIDE( b3AllZero ) +#define b3AnyTrueW B3_WIDE( b3AnyTrue ) +#define b3BlendW B3_WIDE( b3Blend ) +#define b3EmbedIndexW B3_WIDE( b3EmbedIndex ) +#define b3MinIndexW B3_WIDE( b3MinIndex ) +#define b3TransposeW B3_WIDE( b3Transpose ) +#define b3LoadVW B3_WIDE( b3LoadV ) +#define b3StoreVW B3_WIDE( b3StoreV ) +#define b3SplatVW B3_WIDE( b3SplatV ) +#define b3NegVW B3_WIDE( b3NegV ) +#define b3MulSVW B3_WIDE( b3MulSV ) +#define b3MulSubSVW B3_WIDE( b3MulSubSV ) +#define b3MulAddSVW B3_WIDE( b3MulAddSV ) +#define b3SubVW B3_WIDE( b3SubV ) +#define b3AddVW B3_WIDE( b3AddV ) +#define b3DotW B3_WIDE( b3Dot ) +#define b3CrossW B3_WIDE( b3Cross ) diff --git a/src/solver.c b/src/solver.c index bbb1175..9cdc02b 100644 --- a/src/solver.c +++ b/src/solver.c @@ -1434,15 +1434,11 @@ static void b3BulletBodyTask( int startIndex, int endIndex, int workerIndex, voi 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 ) { + int simdShift = world->simdWidth == 8 ? 3 : 2; + // Only count steps that advance the simulation world->stepIndex += 1; @@ -1526,8 +1522,7 @@ void b3Solve( b3World* world, b3StepContext* stepContext ) activeColorIndices[c] = i; // Ceiling for wide constraint count - int colorWideConstraintCount = - colorConvexContactCount > 0 ? ( ( colorConvexContactCount - 1 ) >> B3_SIMD_SHIFT ) + 1 : 0; + int colorWideConstraintCount = colorConvexContactCount > 0 ? ( ( colorConvexContactCount - 1 ) >> simdShift ) + 1 : 0; wideContactCount += colorWideConstraintCount; colorWideContactCounts[c] = colorWideConstraintCount; @@ -1561,9 +1556,8 @@ void b3Solve( b3World* world, b3StepContext* stepContext ) 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" ); + int wideContactByteCount = b3GetWideContactConstraintByteCount( world->simdWidth ); + void* wideConstraints = b3StackAlloc( &world->stack, wideContactCount * wideContactByteCount, "wide contacts" ); b3ContactConstraint* contactConstraints = (b3ContactConstraint*)b3StackAlloc( &world->stack, contactCount * sizeof( b3ContactConstraint ), "contacts" ); b3ManifoldConstraint* manifoldConstraints = (b3ManifoldConstraint*)b3StackAlloc( @@ -1613,10 +1607,9 @@ void b3Solve( b3World* world, b3StepContext* stepContext ) } else { - color->wideConstraints = - (b3ContactConstraintWide*)( (uint8_t*)wideConstraints + wideBase * wideContactByteCount ); + color->wideConstraints = ( (uint8_t*)wideConstraints + wideBase * wideContactByteCount ); - int colorContactCountW = ( ( colorConvexContactCount - 1 ) >> B3_SIMD_SHIFT ) + 1; + int colorContactCountW = ( ( colorConvexContactCount - 1 ) >> simdShift ) + 1; color->wideConstraintCount = colorContactCountW; wideBase += colorContactCountW; } diff --git a/src/solver.h b/src/solver.h index b8f8950..a0d2f25 100644 --- a/src/solver.h +++ b/src/solver.h @@ -58,7 +58,6 @@ 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; @@ -218,7 +217,7 @@ typedef struct b3StepContext // 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; + void* wideConstraints; b3WidePrepareSpan* widePrepareSpans; int wideContactCount; diff --git a/src/wide4.c b/src/wide4.c new file mode 100644 index 0000000..e2132ec --- /dev/null +++ b/src/wide4.c @@ -0,0 +1,6 @@ +// SPDX-FileCopyrightText: 2026 Erin Catto +// SPDX-License-Identifier: MIT + +#define B3_SIMD_WIDTH 4 +#include "contact_solver_wide.inl" +#include "convex_manifold_wide.inl" diff --git a/src/wide8.c b/src/wide8.c new file mode 100644 index 0000000..cb89f92 --- /dev/null +++ b/src/wide8.c @@ -0,0 +1,23 @@ +// SPDX-FileCopyrightText: 2026 Erin Catto +// SPDX-License-Identifier: MIT + +#define B3_SIMD_WIDTH 8 + +#include "core.h" + +#if defined( B3_SIMD_AVX2 ) + +#if defined( _MSC_VER ) && !defined( __clang__ ) && !defined( __AVX2__ ) +#error "MSVC must compile this file with /arch:AVX2, or define BOX3D_DISABLE_AVX2" +#endif + +#include + +B3_AVX2_BEGIN + +#include "contact_solver_wide.inl" +#include "convex_manifold_wide.inl" + +B3_AVX2_END + +#endif diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt index 510e343..8b3e192 100644 --- a/test/CMakeLists.txt +++ b/test/CMakeLists.txt @@ -52,6 +52,10 @@ if(BOX3D_DISABLE_SIMD) target_compile_definitions(test PRIVATE BOX3D_DISABLE_SIMD) endif() +if(NOT BOX3D_AVX2) + target_compile_definitions(test PRIVATE BOX3D_DISABLE_AVX2) +endif() + target_link_libraries(test PRIVATE box3d shared) source_group(TREE "${CMAKE_CURRENT_SOURCE_DIR}" PREFIX "" FILES ${BOX3D_TEST_FILES}) diff --git a/test/test_determinism.c b/test/test_determinism.c index 23e74ad..aca4312 100644 --- a/test/test_determinism.c +++ b/test/test_determinism.c @@ -3,6 +3,7 @@ #include "box3d/box3d.h" #include "determinism.h" +#include "simd.h" #include "stability.h" #include "test_macros.h" @@ -331,6 +332,155 @@ static int MeshDropTest( void ) return 0; } +static uint32_t HashFloats( uint32_t hash, const float* values, int count ) +{ + return b3Hash( hash, (const uint8_t*)values, count * (int)sizeof( float ) ); +} + +// Rolling resistance is mixed per contact and the manifolds have one, two, and four points, so a width 8 +// constraint can mix lanes that width 4 keeps in separate constraints. The hash covers the stored contact +// impulses bitwise, including the sign of zero, because those survive into warm starting and contact data. +static int SingleRollingMixTest( int workerCount, DeterminismResult* reference ) +{ + b3WorldDef worldDef = b3DefaultWorldDef(); + worldDef.workerCount = workerCount; + b3WorldId worldId = b3CreateWorld( &worldDef ); + + b3BodyDef groundDef = b3DefaultBodyDef(); + b3BodyId groundId = b3CreateBody( worldId, &groundDef ); + b3BoxHull groundBox = b3MakeBoxHull( 40.0f, 1.0f, 40.0f ); + b3ShapeDef groundShapeDef = b3DefaultShapeDef(); + b3CreateHullShape( groundId, &groundShapeDef, &groundBox.base ); + + enum + { + rowCount = 8, + columnCount = 8, + bodyCount = rowCount * columnCount + }; + + b3BodyId bodyIds[bodyCount]; + b3BoxHull box = b3MakeBoxHull( 0.4f, 0.4f, 0.4f ); + b3Sphere sphere = { { 0.0f, 0.0f, 0.0f }, 0.4f }; + b3Capsule capsule = { { -0.3f, 0.0f, 0.0f }, { 0.3f, 0.0f, 0.0f }, 0.25f }; + + uint32_t seed = 271828u; + for ( int i = 0; i < bodyCount; ++i ) + { + int row = i / columnCount; + int column = i % columnCount; + + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.type = b3_dynamicBody; + bodyDef.position = (b3Pos){ 2.0f * (float)column - 7.0f, 1.45f, 2.0f * (float)row - 7.0f }; + bodyIds[i] = b3CreateBody( worldId, &bodyDef ); + + seed = seed * 1664525u + 1013904223u; + b3ShapeDef shapeDef = b3DefaultShapeDef(); + shapeDef.density = 1.0f; + shapeDef.baseMaterial.rollingResistance = ( seed >> 28 ) < 5 ? 0.2f : 0.0f; + + switch ( i % 3 ) + { + case 0: + b3CreateSphereShape( bodyIds[i], &shapeDef, &sphere ); + break; + case 1: + b3CreateCapsuleShape( bodyIds[i], &shapeDef, &capsule ); + break; + default: + b3CreateHullShape( bodyIds[i], &shapeDef, &box.base ); + break; + } + + float sign = ( i & 1 ) ? -1.0f : 1.0f; + b3Body_SetLinearVelocity( bodyIds[i], (b3Vec3){ -0.0f, -0.5f, sign * 0.5f } ); + b3Body_SetAngularVelocity( bodyIds[i], (b3Vec3){ sign * 3.0f, -0.0f, -2.0f * sign } ); + } + + uint32_t hash = B3_HASH_INIT; + int stepCount = 120; + for ( int step = 0; step < stepCount; ++step ) + { + b3World_Step( worldId, 1.0f / 60.0f, 4 ); + + for ( int i = 0; i < bodyCount; ++i ) + { + b3WorldTransform xf = b3Body_GetTransform( bodyIds[i] ); + hash = b3Hash( hash, (const uint8_t*)&xf, sizeof( b3WorldTransform ) ); + + b3ContactData contactData[4]; + int contactCount = b3Body_GetContactData( bodyIds[i], contactData, ARRAY_COUNT( contactData ) ); + for ( int c = 0; c < contactCount; ++c ) + { + for ( int m = 0; m < contactData[c].manifoldCount; ++m ) + { + const b3Manifold* manifold = contactData[c].manifolds + m; + hash = HashFloats( hash, &manifold->rollingImpulse.x, 3 ); + hash = HashFloats( hash, &manifold->frictionImpulse.x, 3 ); + hash = HashFloats( hash, &manifold->twistImpulse, 1 ); + + for ( int p = 0; p < manifold->pointCount; ++p ) + { + hash = HashFloats( hash, &manifold->points[p].normalImpulse, 1 ); + hash = HashFloats( hash, &manifold->points[p].totalNormalImpulse, 1 ); + } + } + } + } + } + + b3DestroyWorld( worldId ); + + ENSURE( EnsureRepeatable( reference, (DeterminismResult){ .sleepStep = stepCount, .hash = hash } ) == 0 ); + return 0; +} + +typedef int SceneFcn( int workerCount, DeterminismResult* reference ); + +static int RunSceneAtWidth( SceneFcn* scene, int width, DeterminismResult* result ) +{ + b3SetSIMDWidth( width ); + int status = scene( 1, result ); + b3SetSIMDWidth( 0 ); + return status; +} + +// Width 4 and the native width must produce bitwise identical simulations. +static int SIMDWidthTest( void ) +{ + b3SetSIMDWidth( 0 ); + int nativeWidth = b3GetSIMDWidth(); + if ( nativeWidth == 4 ) + { + printf( " subtest skipped: SIMDWidthTest, native SIMD width is 4\n" ); + return 0; + } + + SceneFcn* scenes[] = { + SingleMultithreadingTest, SingleWavePileTest, SingleQuerySpawnTest, SingleMeshDropTest, SingleRollingMixTest, + }; + + for ( int i = 0; i < ARRAY_COUNT( scenes ); ++i ) + { + DeterminismResult narrow = { 0 }; + DeterminismResult wide = { 0 }; + + int narrowStatus = RunSceneAtWidth( scenes[i], 4, &narrow ); + int wideStatus = RunSceneAtWidth( scenes[i], nativeWidth, &wide ); + + ENSURE( narrowStatus == 0 ); + ENSURE( wideStatus == 0 ); + ENSURE( narrow.seeded && wide.seeded ); + ENSURE( narrow.sleepStep == wide.sleepStep ); + ENSURE( narrow.hash == wide.hash ); + ENSURE( narrow.queryHitCount == wide.queryHitCount ); + ENSURE( narrow.queryHash == wide.queryHash ); + } + + return 0; +} + int DeterminismTest( void ) { RUN_SUBTEST( MultithreadingTest ); @@ -338,6 +488,7 @@ int DeterminismTest( void ) RUN_SUBTEST( WavePileTest ); RUN_SUBTEST( QuerySpawnTest ); RUN_SUBTEST( MeshDropTest ); + RUN_SUBTEST( SIMDWidthTest ); return 0; } diff --git a/test/test_recording.c b/test/test_recording.c index 42acb95..0f0e3a6 100644 --- a/test/test_recording.c +++ b/test/test_recording.c @@ -7,6 +7,7 @@ #include "physics_world.h" #include "recording.h" +#include "simd.h" #include "test_macros.h" #include "box3d/box3d.h" @@ -133,6 +134,106 @@ static int HullDedup( void ) return 0; } +static int ValidateReplayAtWidth( const b3Recording* rec, int width ) +{ + b3SetSIMDWidth( width ); + bool valid = b3ValidateReplay( b3Recording_GetData( rec ), b3Recording_GetSize( rec ), 1 ); + b3SetSIMDWidth( 0 ); + return valid ? 0 : 1; +} + +// A recording made at the native SIMD width must replay at width 4 and vice versa. +static int CrossWidthReplay( void ) +{ + b3SetSIMDWidth( 0 ); + int nativeWidth = b3GetSIMDWidth(); + if ( nativeWidth == 4 ) + { + printf( " subtest skipped: CrossWidthReplay, native SIMD width is 4\n" ); + return 0; + } + + enum + { + hullCount = 6 + }; + + b3HullData* hulls[hullCount] = { 0 }; + hulls[0] = b3CreateRock( 0.5f ); + hulls[1] = b3CreateComplexHull( 0.5f ); + hulls[2] = b3CreateCylinder( 0.8f, 0.4f, 0.0f, 8 ); + hulls[3] = b3CreateCone( 0.8f, 0.5f, 0.2f, 7 ); + + uint32_t seed = 12345u; + for ( int hullIndex = 4; hullIndex < hullCount; ++hullIndex ) + { + b3Vec3 points[20]; + for ( int i = 0; i < 20; ++i ) + { + float v[3]; + for ( int k = 0; k < 3; ++k ) + { + seed = seed * 1664525u + 1013904223u; + v[k] = ( (float)( seed >> 8 ) / (float)( 1 << 24 ) - 0.5f ) * ( k == 1 ? 0.6f : 1.0f ); + } + points[i] = (b3Vec3){ v[0], v[1], v[2] }; + } + hulls[hullIndex] = b3CreateHull( points, 20, 20 ); + } + + b3Recording* rec = b3CreateRecording( 0 ); + + b3SetSIMDWidth( nativeWidth ); + + b3WorldDef worldDef = b3DefaultWorldDef(); + b3WorldId worldId = b3CreateWorld( &worldDef ); + + b3World_StartRecording( worldId, rec ); + + b3BodyDef groundDef = b3DefaultBodyDef(); + groundDef.type = b3_staticBody; + b3BodyId groundId = b3CreateBody( worldId, &groundDef ); + b3BoxHull groundBox = b3MakeBoxHull( 20.0f, 1.0f, 20.0f ); + b3ShapeDef groundShape = b3DefaultShapeDef(); + b3CreateHullShape( groundId, &groundShape, &groundBox.base ); + + b3ShapeDef dynamicShape = b3DefaultShapeDef(); + dynamicShape.density = 1.0f; + + for ( int i = 0; i < 18; ++i ) + { + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.type = b3_dynamicBody; + bodyDef.position = (b3Pos){ 0.4f * (float)( i % 3 ) - 0.4f, 1.5f + 0.9f * (float)i, 0.3f * (float)( i % 2 ) }; + bodyDef.rotation = b3MakeQuatFromAxisAngle( (b3Vec3){ 0.0f, 1.0f, 0.0f }, 0.37f * (float)i ); + b3BodyId bodyId = b3CreateBody( worldId, &bodyDef ); + b3CreateHullShape( bodyId, &dynamicShape, hulls[i % hullCount] ); + } + + float timeStep = 1.0f / 60.0f; + for ( int i = 0; i < 120; ++i ) + { + b3World_Step( worldId, timeStep, 4 ); + } + + b3World_StopRecording( worldId ); + b3DestroyWorld( worldId ); + b3SetSIMDWidth( 0 ); + + int narrowStatus = ValidateReplayAtWidth( rec, 4 ); + int nativeStatus = ValidateReplayAtWidth( rec, nativeWidth ); + + b3DestroyRecording( rec ); + for ( int i = 0; i < hullCount; ++i ) + { + b3DestroyHull( hulls[i] ); + } + + ENSURE( narrowStatus == 0 ); + ENSURE( nativeStatus == 0 ); + return 0; +} + // Mid-stream snapshot with only dynamic bodies floating in air (no contacts). static int MidStreamNoContacts( void ) { @@ -2147,6 +2248,7 @@ int RecordingTest( void ) RUN_SUBTEST( HullDedup ); RUN_SUBTEST( MidStreamNoContacts ); RUN_SUBTEST( MidStreamContacts ); + RUN_SUBTEST( CrossWidthReplay ); RUN_SUBTEST( StagedStepCreationPose ); RUN_SUBTEST( ScrubBackward ); RUN_SUBTEST( SeekWithHull );