Add support for AVX2 and runtime feature detection (#174)
- [x] run unit tests - [x] run all samples
This commit is contained in:
parent
51f056e0a9
commit
e77352cd60
53 changed files with 4681 additions and 3376 deletions
14
.github/workflows/build.yml
vendored
14
.github/workflows/build.yml
vendored
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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}"
|
||||
|
|
|
|||
|
|
@ -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.
|
||||
|
|
|
|||
9
benchmark/amd7950x_avx2/convex_pile.csv
Normal file
9
benchmark/amd7950x_avx2/convex_pile.csv
Normal 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
|
||||
|
9
benchmark/amd7950x_avx2/joint_grid.csv
Normal file
9
benchmark/amd7950x_avx2/joint_grid.csv
Normal 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
|
||||
|
9
benchmark/amd7950x_avx2/junkyard.csv
Normal file
9
benchmark/amd7950x_avx2/junkyard.csv
Normal 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
|
||||
|
9
benchmark/amd7950x_avx2/large_pyramid.csv
Normal file
9
benchmark/amd7950x_avx2/large_pyramid.csv
Normal 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
|
||||
|
9
benchmark/amd7950x_avx2/large_world.csv
Normal file
9
benchmark/amd7950x_avx2/large_world.csv
Normal 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
|
||||
|
9
benchmark/amd7950x_avx2/many_pyramids.csv
Normal file
9
benchmark/amd7950x_avx2/many_pyramids.csv
Normal 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
|
||||
|
9
benchmark/amd7950x_avx2/rain.csv
Normal file
9
benchmark/amd7950x_avx2/rain.csv
Normal 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
|
||||
|
9
benchmark/amd7950x_avx2/sleep.csv
Normal file
9
benchmark/amd7950x_avx2/sleep.csv
Normal 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
|
||||
|
9
benchmark/amd7950x_avx2/spinner.csv
Normal file
9
benchmark/amd7950x_avx2/spinner.csv
Normal 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
|
||||
|
9
benchmark/amd7950x_avx2/trees100.csv
Normal file
9
benchmark/amd7950x_avx2/trees100.csv
Normal 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
|
||||
|
9
benchmark/amd7950x_avx2/trees25.csv
Normal file
9
benchmark/amd7950x_avx2/trees25.csv
Normal 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
|
||||
|
9
benchmark/amd7950x_avx2/trees50.csv
Normal file
9
benchmark/amd7950x_avx2/trees50.csv
Normal 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
|
||||
|
9
benchmark/amd7950x_avx2/washer.csv
Normal file
9
benchmark/amd7950x_avx2/washer.csv
Normal 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
|
||||
|
|
|
@ -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 )
|
||||
{
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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 );
|
||||
|
||||
|
|
|
|||
|
|
@ -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 )
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
||||
|
|
|
|||
|
|
@ -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 );
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
|
|
|||
|
|
@ -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();
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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)
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
}
|
||||
|
|
|
|||
1371
src/contact_solver.c
1371
src/contact_solver.c
File diff suppressed because it is too large
Load diff
|
|
@ -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
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
825
src/convex_manifold_wide.inl
Normal file
825
src/convex_manifold_wide.inl
Normal 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
|
||||
42
src/core.h
42
src/core.h
|
|
@ -49,34 +49,48 @@
|
|||
// Define SIMD
|
||||
#if defined( BOX3D_DISABLE_SIMD )
|
||||
#define B3_SIMD_NONE
|
||||
#define B3_SIMD_WIDTH 4
|
||||
//#pragma message("B3_SIMD_NONE")
|
||||
#else
|
||||
#if defined( B3_CPU_X86_X64 )
|
||||
#define B3_SIMD_SSE2
|
||||
#define B3_SIMD_WIDTH 4
|
||||
//#pragma message("B3_SIMD_SSE2")
|
||||
#if !defined( BOX3D_DISABLE_AVX2 )
|
||||
#define B3_SIMD_HAS_WIDTH_8
|
||||
#if defined( __AVX2__ ) || ( defined( B3_SIMD_WIDTH ) && B3_SIMD_WIDTH == 8 )
|
||||
#define B3_SIMD_AVX2
|
||||
#endif
|
||||
#endif
|
||||
#elif defined( B3_CPU_ARM )
|
||||
// ARMv7 Neon doesn't have divide or sqrt so cannot be used.
|
||||
#if defined( __aarch64__ ) || defined( _M_ARM64 )
|
||||
#define B3_SIMD_NEON
|
||||
#else
|
||||
#define B3_SIMD_NONE
|
||||
#endif
|
||||
#define B3_SIMD_WIDTH 4
|
||||
// ARMv7 Neon doesn't have divide or sqrt so cannot be used.
|
||||
#if defined( __aarch64__ ) || defined( _M_ARM64 )
|
||||
#define B3_SIMD_NEON
|
||||
#else
|
||||
#define B3_SIMD_NONE
|
||||
#endif
|
||||
//#pragma message("B3_SIMD_NEON")
|
||||
#elif defined( B3_CPU_WASM )
|
||||
#define B3_CPU_WASM
|
||||
#define B3_SIMD_SSE2
|
||||
#define B3_SIMD_WIDTH 4
|
||||
//#pragma message("B3_SIMD_SSE2")
|
||||
#else
|
||||
#define B3_SIMD_NONE
|
||||
#define B3_SIMD_WIDTH 4
|
||||
//#pragma message("B3_SIMD_NONE")
|
||||
#endif
|
||||
#endif
|
||||
|
||||
#if !defined( B3_SIMD_NONE )
|
||||
#define B3_SIMD_ENABLED
|
||||
#endif
|
||||
|
||||
#if defined( B3_SIMD_AVX2 ) && defined( __clang__ )
|
||||
#define B3_AVX2_BEGIN _Pragma( "clang attribute push( __attribute__( ( target( \"avx2\" ) ) ), apply_to = function )" )
|
||||
#define B3_AVX2_END _Pragma( "clang attribute pop" )
|
||||
#elif defined( B3_SIMD_AVX2 ) && defined( __GNUC__ )
|
||||
#define B3_AVX2_BEGIN _Pragma( "GCC push_options" ) _Pragma( "GCC target( \"avx2\" )" )
|
||||
#define B3_AVX2_END _Pragma( "GCC pop_options" )
|
||||
#else
|
||||
#define B3_AVX2_BEGIN
|
||||
#define B3_AVX2_END
|
||||
#endif
|
||||
|
||||
// Define compiler
|
||||
#if defined( __clang__ )
|
||||
#define B3_COMPILER_CLANG
|
||||
|
|
@ -120,7 +134,7 @@ typedef struct b3AtomicI64
|
|||
} b3AtomicI64;
|
||||
|
||||
// Minimum memory alignment used for all allocations
|
||||
#define B3_ALIGNMENT 16
|
||||
#define B3_ALIGNMENT 32
|
||||
|
||||
#define B3_RESTRICT restrict
|
||||
|
||||
|
|
|
|||
44
src/hull.c
44
src/hull.c
|
|
@ -1649,32 +1649,34 @@ int b3FindHullSupportVertex( const b3HullData* hull, b3Vec3 direction )
|
|||
int vertexCount = hull->vertexCount;
|
||||
const float* vx = b3GetHullSoaVertices( hull );
|
||||
|
||||
int soaVertexCount = ( vertexCount + 3 ) & ~3;
|
||||
int soaVertexCount = b3GetHullSoaStride( vertexCount );
|
||||
const float* vy = vx + soaVertexCount;
|
||||
const float* vz = vy + soaVertexCount;
|
||||
|
||||
b3FloatW dx = b3SplatW( direction.x );
|
||||
b3FloatW dy = b3SplatW( direction.y );
|
||||
b3FloatW dz = b3SplatW( direction.z );
|
||||
b3FloatW four = b3SplatW( 4.0f );
|
||||
b3FloatW index = b3SetW( 0.0f, 1.0f, 2.0f, 3.0f );
|
||||
b3FloatW bestDotW = b3SplatW( -FLT_MAX );
|
||||
b3FloatW bestIndexW = b3SplatW( -1.0f );
|
||||
b3FloatW4 dx = b3SplatW4( direction.x );
|
||||
b3FloatW4 dy = b3SplatW4( direction.y );
|
||||
b3FloatW4 dz = b3SplatW4( direction.z );
|
||||
b3FloatW4 four = b3SplatW4( 4.0f );
|
||||
b3FloatW4 index = b3SetW4( 0.0f, 1.0f, 2.0f, 3.0f );
|
||||
b3FloatW4 bestDotW = b3SplatW4( -FLT_MAX );
|
||||
b3FloatW4 bestIndexW = b3SplatW4( -1.0f );
|
||||
|
||||
for ( int i = 0; i < soaVertexCount; i += 4 )
|
||||
// Keep this at 4 wide to avoid regressing 4 wide platforms.
|
||||
int loopCount = ( vertexCount + 3 ) & ~3;
|
||||
for ( int i = 0; i < loopCount; i += 4 )
|
||||
{
|
||||
b3FloatW dot =
|
||||
b3AddW( b3AddW( b3MulW( dx, b3LoadW( vx + i ) ), b3MulW( dy, b3LoadW( vy + i ) ) ), b3MulW( dz, b3LoadW( vz + i ) ) );
|
||||
b3FloatW mask = b3GreaterThanW( dot, bestDotW );
|
||||
bestDotW = b3BlendW( bestDotW, dot, mask );
|
||||
bestIndexW = b3BlendW( bestIndexW, index, mask );
|
||||
index = b3AddW( index, four );
|
||||
b3FloatW4 dot = b3AddW4( b3AddW4( b3MulW4( dx, b3LoadW4( vx + i ) ), b3MulW4( dy, b3LoadW4( vy + i ) ) ),
|
||||
b3MulW4( dz, b3LoadW4( vz + i ) ) );
|
||||
b3FloatW4 mask = b3GreaterThanW4( dot, bestDotW );
|
||||
bestDotW = b3BlendW4( bestDotW, dot, mask );
|
||||
bestIndexW = b3BlendW4( bestIndexW, index, mask );
|
||||
index = b3AddW4( index, four );
|
||||
}
|
||||
|
||||
_Alignas( 16 ) float dots[4];
|
||||
_Alignas( 16 ) float indices[4];
|
||||
b3StoreW( dots, bestDotW );
|
||||
b3StoreW( indices, bestIndexW );
|
||||
b3StoreW4( dots, bestDotW );
|
||||
b3StoreW4( indices, bestIndexW );
|
||||
|
||||
for ( int lane = 0; lane < 4; ++lane )
|
||||
{
|
||||
|
|
@ -2218,8 +2220,8 @@ b3HullData* b3CreateHull( const b3Vec3* points, int pointCount, int maxVertexCou
|
|||
while ( edge != face->edge );
|
||||
}
|
||||
|
||||
int soaVertexCount = ( vertexCount + 3 ) & ~3;
|
||||
int soaNormalCount = ( faceCount + 3 ) & ~3;
|
||||
int soaVertexCount = b3GetHullSoaStride( vertexCount );
|
||||
int soaNormalCount = b3GetHullSoaStride( faceCount );
|
||||
|
||||
// Allocate the hull. Arrays hang off the end.
|
||||
size_t byteCount = b3AlignUp8( sizeof( b3HullData ) );
|
||||
|
|
@ -2492,7 +2494,7 @@ b3HullData* b3CloneAndTransformHull( const b3HullData* original, b3Transform tra
|
|||
b3Matrix3 matrix = b3MakeMatrixFromQuat( transform.q );
|
||||
b3Vec3* points = b3GetHullPointsWrite( hull );
|
||||
|
||||
int soaVertexCount = ( vertexCount + 3 ) & ~3;
|
||||
int soaVertexCount = b3GetHullSoaStride( vertexCount );
|
||||
float* vx = b3GetHullSoaVerticesWrite( hull );
|
||||
float* vy = vx + soaVertexCount;
|
||||
float* vz = vy + soaVertexCount;
|
||||
|
|
@ -2514,7 +2516,7 @@ b3HullData* b3CloneAndTransformHull( const b3HullData* original, b3Transform tra
|
|||
}
|
||||
|
||||
b3Plane* planes = b3GetHullPlanesWrite( hull );
|
||||
int soaNormalCount = ( faceCount + 3 ) & ~3;
|
||||
int soaNormalCount = b3GetHullSoaStride( faceCount );
|
||||
float* nx = b3GetHullSoaNormalsWrite( hull );
|
||||
float* ny = nx + soaNormalCount;
|
||||
float* nz = ny + soaNormalCount;
|
||||
|
|
|
|||
|
|
@ -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 );
|
||||
|
||||
|
|
|
|||
|
|
@ -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 );
|
||||
|
|
|
|||
|
|
@ -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 );
|
||||
|
|
|
|||
|
|
@ -262,6 +262,7 @@ typedef struct b3World
|
|||
void* customFilterContext;
|
||||
|
||||
int workerCount;
|
||||
int simdWidth;
|
||||
b3EnqueueTaskCallback* enqueueTaskFcn;
|
||||
b3FinishTaskCallback* finishTaskFcn;
|
||||
void* userTaskContext;
|
||||
|
|
|
|||
108
src/simd.c
108
src/simd.c
|
|
@ -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
1235
src/simd.h
File diff suppressed because it is too large
Load diff
216
src/simd_avx2.h
Normal file
216
src/simd_avx2.h
Normal 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
265
src/simd_neon.h
Normal 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
296
src/simd_scalar.h
Normal 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
414
src/simd_sse2.h
Normal 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
62
src/simd_wide.h
Normal 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 )
|
||||
21
src/solver.c
21
src/solver.c
|
|
@ -1434,15 +1434,11 @@ static void b3BulletBodyTask( int startIndex, int endIndex, int workerIndex, voi
|
|||
b3TracyCZoneEnd( bullet_body_task );
|
||||
}
|
||||
|
||||
#if B3_SIMD_WIDTH == 4
|
||||
#define B3_SIMD_SHIFT 2
|
||||
#else
|
||||
#define B3_SIMD_SHIFT 0
|
||||
#endif
|
||||
|
||||
// Solve with graph coloring
|
||||
void b3Solve( b3World* world, b3StepContext* stepContext )
|
||||
{
|
||||
int simdShift = world->simdWidth == 8 ? 3 : 2;
|
||||
|
||||
// Only count steps that advance the simulation
|
||||
world->stepIndex += 1;
|
||||
|
||||
|
|
@ -1526,8 +1522,7 @@ void b3Solve( b3World* world, b3StepContext* stepContext )
|
|||
activeColorIndices[c] = i;
|
||||
|
||||
// Ceiling for wide constraint count
|
||||
int colorWideConstraintCount =
|
||||
colorConvexContactCount > 0 ? ( ( colorConvexContactCount - 1 ) >> B3_SIMD_SHIFT ) + 1 : 0;
|
||||
int colorWideConstraintCount = colorConvexContactCount > 0 ? ( ( colorConvexContactCount - 1 ) >> simdShift ) + 1 : 0;
|
||||
wideContactCount += colorWideConstraintCount;
|
||||
colorWideContactCounts[c] = colorWideConstraintCount;
|
||||
|
||||
|
|
@ -1561,9 +1556,8 @@ void b3Solve( b3World* world, b3StepContext* stepContext )
|
|||
b3BlockDim meshPrepareDim = b3ComputeBlockCount( contactCount, minContactsPerBlock, maxBlockCount );
|
||||
b3BlockDim jointPrepareDim = b3ComputeBlockCount( jointCount, minJointsPerBlock, maxBlockCount );
|
||||
|
||||
int wideContactByteCount = b3GetWideContactConstraintByteCount();
|
||||
b3ContactConstraintWide* wideConstraints =
|
||||
(b3ContactConstraintWide*)b3StackAlloc( &world->stack, wideContactCount * wideContactByteCount, "wide contacts" );
|
||||
int wideContactByteCount = b3GetWideContactConstraintByteCount( world->simdWidth );
|
||||
void* wideConstraints = b3StackAlloc( &world->stack, wideContactCount * wideContactByteCount, "wide contacts" );
|
||||
b3ContactConstraint* contactConstraints =
|
||||
(b3ContactConstraint*)b3StackAlloc( &world->stack, contactCount * sizeof( b3ContactConstraint ), "contacts" );
|
||||
b3ManifoldConstraint* manifoldConstraints = (b3ManifoldConstraint*)b3StackAlloc(
|
||||
|
|
@ -1613,10 +1607,9 @@ void b3Solve( b3World* world, b3StepContext* stepContext )
|
|||
}
|
||||
else
|
||||
{
|
||||
color->wideConstraints =
|
||||
(b3ContactConstraintWide*)( (uint8_t*)wideConstraints + wideBase * wideContactByteCount );
|
||||
color->wideConstraints = ( (uint8_t*)wideConstraints + wideBase * wideContactByteCount );
|
||||
|
||||
int colorContactCountW = ( ( colorConvexContactCount - 1 ) >> B3_SIMD_SHIFT ) + 1;
|
||||
int colorContactCountW = ( ( colorConvexContactCount - 1 ) >> simdShift ) + 1;
|
||||
color->wideConstraintCount = colorContactCountW;
|
||||
wideBase += colorContactCountW;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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
6
src/wide4.c
Normal 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
23
src/wide8.c
Normal 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
|
||||
|
|
@ -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})
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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 );
|
||||
|
|
|
|||
Loading…
Reference in a new issue