Add support for AVX2 and runtime feature detection (#174)

- [x] run unit tests
- [x] run all samples
This commit is contained in:
Ethan Rutherford 2026-10-04 00:14:34 +02:00 • committed by GitHub
commit e77352cd60
No known key found for this signature in database
GPG key ID: B5690EEEBB952194
53 changed files with 4681 additions and 3376 deletions

View file

@ -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

View file

@ -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}"
# "currentDir": "${workspaceRoot}"

View file

@ -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.

View file

@ -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
1 threads ms
2 1 5092.19
3 2 2694.4
4 3 1855.5
5 4 1450.33
6 5 1195.15
7 6 1032.87
8 7 924.237
9 8 843.199

View file

@ -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
1 threads ms
2 1 1308.56
3 2 683.543
4 3 461.149
5 4 351.313
6 5 283.848
7 6 239.34
8 7 208.293
9 8 185.43

View file

@ -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
1 threads ms
2 1 11252.9
3 2 5915.73
4 3 4113.86
5 4 3194.52
6 5 2626.44
7 6 2261.77
8 7 2009.22
9 8 1824.68

View file

@ -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
1 threads ms
2 1 1067.06
3 2 558.46
4 3 390.698
5 4 300.188
6 5 249.834
7 6 218.912
8 7 193.65
9 8 175.819

View file

@ -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
1 threads ms
2 1 5.0616
3 2 6.9262
4 3 7.827
5 4 8.2244
6 5 8.7631
7 6 8.6223
8 7 8.7636
9 8 8.9294

View file

@ -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
1 threads ms
2 1 1163.37
3 2 605.215
4 3 411.543
5 4 307.527
6 5 249.396
7 6 216.535
8 7 184.334
9 8 165.442

View file

@ -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
1 threads ms
2 1 2038.39
3 2 1110.3
4 3 773.18
5 4 603.397
6 5 502.292
7 6 434.461
8 7 389.414
9 8 355.206

View file

@ -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
1 threads ms
2 1 596.258
3 2 328.927
4 3 231.57
5 4 187.904
6 5 159.347
7 6 142.58
8 7 131.034
9 8 124.711

View file

@ -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
1 threads ms
2 1 2919.32
3 2 1674.26
4 3 1200.43
5 4 960.786
6 5 821.659
7 6 728.967
8 7 669.058
9 8 626.733

View file

@ -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
1 threads ms
2 1 167.468
3 2 110.54
4 3 87.5729
5 4 77.6966
6 5 73.9776
7 6 71.7265
8 7 69.6889
9 8 69.3954

View file

@ -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
1 threads ms
2 1 657.535
3 2 386.678
4 3 284.076
5 4 245.238
6 5 223.137
7 6 208.401
8 7 197.529
9 8 194.169

View file

@ -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
1 threads ms
2 1 285.043
3 2 176.584
4 3 135.305
5 4 117.663
6 5 109.496
7 6 104.289
8 7 101.186
9 8 98.9602

View file

@ -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
1 threads ms
2 1 15669.7
3 2 8410.7
4 3 5793.28
5 4 4493.58
6 5 3722.97
7 6 3219.45
8 7 2849.15
9 8 2562.49

View file

@ -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 )
{

View file

@ -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

View file

@ -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 );

View file

@ -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 )

View file

@ -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;

View file

@ -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 );

View file

@ -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;

View file

@ -9,6 +9,7 @@
#include "box3d/math_functions.h"
#include "manifold.h"
#include "simd.h"
#include <stdlib.h>
@ -185,3 +186,13 @@ SatBenchmarkResult RunSatBenchmark( const SatBenchmarkData* data, int repeatCoun
};
return result;
}
void SetSimdWidth( int width )
{
b3SetSIMDWidth( width );
}
int GetSimdWidth( void )
{
return b3GetSIMDWidth();
}

View file

@ -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

View file

@ -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)

View file

@ -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

View file

@ -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;
}

File diff suppressed because it is too large Load diff

View file

@ -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 );

1426
src/contact_solver_wide.inl Normal file

File diff suppressed because it is too large Load diff

File diff suppressed because it is too large Load diff

View file

@ -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<eps and ADC*BDC<eps and CBA*BDC<eps
b3FloatW m1 = b3LessThanW( b3MulW( CBA, DBA ), EPS );
b3FloatW m2 = b3LessThanW( b3MulW( ADC, BDC ), EPS );
b3FloatW m3 = b3LessThanW( b3MulW( CBA, BDC ), EPS );
// Reject near parallel edges. The arc lerp is ill conditioned when both of B's normals are nearly
// perpendicular to edge A, a scale invariant sine threshold relative to the edge length.
b3FloatW maxCD = b3MaxW( b3MulW( CBA, CBA ), b3MulW( DBA, DBA ) );
b3FloatW notParallel = b3GreaterThanW( maxCD, tol );
b3FloatW mask = b3AndW( b3AndW( m1, m2 ), b3AndW( m3, notParallel ) );
// Most A-edges fail the Gauss test, so skip the divide, sqrt and support work when no
// lane passed.
if ( b3AnyTrueW( mask ) == false )
{
continue;
}
// t = -CBA / (DBA - CBA)
b3FloatW t = b3DivW( b3NegW( CBA ), b3SubW( DBA, CBA ) );
// normal = lerp(t, C, D) = C + (D-C)*t
b3Vec3W n = b3MulAddSVW( C, t, b3SubVW( D, C ) );
// normalize
b3FloatW len2 = b3DotW( n, n );
b3FloatW inv = b3DivW( b3SplatW( 1.0f ), b3SqrtW( len2 ) );
n = b3MulSVW( inv, n );
// support = dot(normal, av0 + bv0)
b3FloatW support = b3DotW( b3AddVW( av0, bv0 ), n );
// Lanes that fail the Gauss test can never win.
support = b3BlendW( INF, support, mask );
b3FloatW separation = b3NegW( support );
// Test all supports against the running best at once. If none beats it, skip the
// store and scalar reduction.
b3FloatW improves = b3GreaterThanW( separation, b3SplatW( res.edge.separation ) );
if ( b3AnyTrueW( improves ) == false )
{
continue;
}
_Alignas( B3_WIDE_ALIGNMENT ) float sA[B3_SIMD_WIDTH];
_Alignas( B3_WIDE_ALIGNMENT ) float nxA[B3_SIMD_WIDTH];
_Alignas( B3_WIDE_ALIGNMENT ) float nyA[B3_SIMD_WIDTH];
_Alignas( B3_WIDE_ALIGNMENT ) float nzA[B3_SIMD_WIDTH];
b3StoreW( sA, separation );
b3StoreVW( nxA, nyA, nzA, n );
// Reduce in lane order so ties keep the first edge and the early out takes the first
// improving support below zero. Padded tail lanes carry +INF support, so they never
// update or index edges out of range.
for ( int lane = 0; lane < B3_SIMD_WIDTH; lane++ )
{
int ei = i + lane;
float s = sA[lane];
if ( s > 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

View file

@ -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

View file

@ -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;

View file

@ -7,6 +7,14 @@
#include <stddef.h>
// 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 );

View file

@ -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 );

View file

@ -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 );

View file

@ -262,6 +262,7 @@ typedef struct b3World
void* customFilterContext;
int workerCount;
int simdWidth;
b3EnqueueTaskCallback* enqueueTaskFcn;
b3FinishTaskCallback* finishTaskFcn;
void* userTaskContext;

View file

@ -5,6 +5,114 @@
#include "simd.h"
#include "platform.h"
#include <stdbool.h>
#include <stdint.h>
static b3AtomicInt b3_simdWidth;
#if defined( B3_SIMD_HAS_WIDTH_8 )
#if defined( _MSC_VER )
#include <intrin.h>
#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 ) \

1235
src/simd.h

File diff suppressed because it is too large Load diff

216
src/simd_avx2.h Normal file
View file

@ -0,0 +1,216 @@
// SPDX-FileCopyrightText: 2026 Erin Catto
// SPDX-License-Identifier: MIT
#pragma once
#include "core.h"
#include "box3d/types.h"
#include <immintrin.h>
// 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 );
}

265
src/simd_neon.h Normal file
View file

@ -0,0 +1,265 @@
// SPDX-FileCopyrightText: 2026 Erin Catto
// SPDX-License-Identifier: MIT
#pragma once
#include "core.h"
#include "box3d/types.h"
#include <arm_neon.h>
// 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 );
}

296
src/simd_scalar.h Normal file
View file

@ -0,0 +1,296 @@
// SPDX-FileCopyrightText: 2026 Erin Catto
// SPDX-License-Identifier: MIT
#pragma once
#include "core.h"
#include "box3d/types.h"
#include <math.h>
#include <string.h>
// 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 };
}

414
src/simd_sse2.h Normal file
View file

@ -0,0 +1,414 @@
// SPDX-FileCopyrightText: 2026 Erin Catto
// SPDX-License-Identifier: MIT
#pragma once
#include "core.h"
#include "box3d/types.h"
#include <emmintrin.h>
// 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 ) ) );
}

62
src/simd_wide.h Normal file
View file

@ -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 )

View file

@ -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;
}

View file

@ -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;

6
src/wide4.c Normal file
View file

@ -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"

23
src/wide8.c Normal file
View file

@ -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 <immintrin.h>
B3_AVX2_BEGIN
#include "contact_solver_wide.inl"
#include "convex_manifold_wide.inl"
B3_AVX2_END
#endif

View file

@ -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})

View file

@ -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;
}

View file

@ -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 );