From c37cfe4cd78f555fcea4a10c7aed2ecf883a3271 Mon Sep 17 00:00:00 2001 From: Erin Catto Date: Sat, 18 Jul 2026 22:43:03 -0700 Subject: [PATCH] SIMD hull collision (#93) SIMD based hull collision inspired by example code from @cairnc in #54 image - Fixed #78 - Fixed #85 and fixed CI to error on UB. - Updated benchmarks - Added AMD scalar benchmarks - Added Neon benchmarks - More determinism unit tests - Sample leak detection - Added b3Body_AllowFastRotation - Added b3Body_IsFastRotationAllowed - Bumped b3HullData version - Bumped b3CompoundData version --- .github/workflows/build.yml | 1 + .gitignore | 1 + CMakeLists.txt | 7 +- benchmark/amd7950x_scalar/convex_pile.csv | 9 + benchmark/amd7950x_scalar/joint_grid.csv | 9 + benchmark/amd7950x_scalar/junkyard.csv | 9 + benchmark/amd7950x_scalar/large_pyramid.csv | 9 + benchmark/amd7950x_scalar/large_world.csv | 9 + benchmark/amd7950x_scalar/many_pyramids.csv | 9 + benchmark/amd7950x_scalar/rain.csv | 9 + benchmark/amd7950x_scalar/trees100.csv | 9 + benchmark/amd7950x_scalar/trees25.csv | 9 + benchmark/amd7950x_scalar/trees50.csv | 9 + benchmark/amd7950x_scalar/washer.csv | 16 +- benchmark/amd7950x_sse2/convex_pile.csv | 16 +- benchmark/amd7950x_sse2/joint_grid.csv | 16 +- benchmark/amd7950x_sse2/junkyard.csv | 16 +- benchmark/amd7950x_sse2/large_pyramid.csv | 16 +- benchmark/amd7950x_sse2/large_world.csv | 16 +- benchmark/amd7950x_sse2/many_pyramids.csv | 16 +- benchmark/amd7950x_sse2/rain.csv | 16 +- benchmark/amd7950x_sse2/trees100.csv | 16 +- benchmark/amd7950x_sse2/trees25.csv | 16 +- benchmark/amd7950x_sse2/trees50.csv | 16 +- benchmark/amd7950x_sse2/washer.csv | 16 +- benchmark/m2air_neon/convex_pile.csv | 5 + benchmark/m2air_neon/joint_grid.csv | 5 + benchmark/m2air_neon/junkyard.csv | 5 + benchmark/m2air_neon/large_pyramid.csv | 5 + benchmark/m2air_neon/large_world.csv | 5 + benchmark/m2air_neon/many_pyramids.csv | 5 + benchmark/m2air_neon/rain.csv | 5 + benchmark/m2air_neon/trees100.csv | 5 + benchmark/m2air_neon/trees25.csv | 5 + benchmark/m2air_neon/trees50.csv | 5 + benchmark/m2air_neon/washer.csv | 5 + benchmark/main.c | 4 +- data/dumps/single_box/box3d_dump.inl | 85 -- include/box3d/base.h | 3 +- include/box3d/box3d.h | 27 +- include/box3d/collision.h | 44 +- include/box3d/constants.h | 17 + include/box3d/types.h | 45 +- samples/gfx/CMakeLists.txt | 6 + samples/host/sokol_app_impl.c | 9 +- samples/main.cpp | 67 +- samples/sample_benchmark.cpp | 8 + samples/sample_bodies.cpp | 147 ++- samples/sample_continuous.cpp | 55 - samples/sample_determinism.cpp | 207 ++++ samples/sample_issues.cpp | 39 - samples/sample_shapes.cpp | 2 +- samples/sample_stacking.cpp | 84 +- shared/benchmarks.c | 25 + shared/benchmarks.h | 2 + shared/determinism.c | 274 +++++ shared/determinism.h | 49 + shared/stability.c | 26 +- shared/stability.h | 9 + shared/utils.h | 20 +- src/block_allocator.c | 5 + src/block_allocator.h | 2 +- src/body.c | 33 +- src/body.h | 1 - src/compound.c | 67 +- src/contact_solver.c | 391 +----- src/convex_manifold.c | 1219 ++++++++++++++++--- src/height_field.c | 10 +- src/hull.c | 187 ++- src/manifold.h | 11 + src/math_internal.h | 1 + src/mesh.c | 9 +- src/parallel_for.c | 3 +- src/recording_ops.inl | 1 + src/recording_replay.c | 5 + src/simd.h | 568 ++++++++- src/solver.c | 2 +- test/CMakeLists.txt | 1 + test/main.c | 2 + test/test_allocator.c | 50 + test/test_determinism.c | 175 +++ test/test_hull.c | 15 +- test/test_sat.c | 486 ++++++++ test/test_world.c | 43 - 84 files changed, 3817 insertions(+), 1070 deletions(-) create mode 100644 benchmark/amd7950x_scalar/convex_pile.csv create mode 100644 benchmark/amd7950x_scalar/joint_grid.csv create mode 100644 benchmark/amd7950x_scalar/junkyard.csv create mode 100644 benchmark/amd7950x_scalar/large_pyramid.csv create mode 100644 benchmark/amd7950x_scalar/large_world.csv create mode 100644 benchmark/amd7950x_scalar/many_pyramids.csv create mode 100644 benchmark/amd7950x_scalar/rain.csv create mode 100644 benchmark/amd7950x_scalar/trees100.csv create mode 100644 benchmark/amd7950x_scalar/trees25.csv create mode 100644 benchmark/amd7950x_scalar/trees50.csv create mode 100644 benchmark/m2air_neon/convex_pile.csv create mode 100644 benchmark/m2air_neon/joint_grid.csv create mode 100644 benchmark/m2air_neon/junkyard.csv create mode 100644 benchmark/m2air_neon/large_pyramid.csv create mode 100644 benchmark/m2air_neon/large_world.csv create mode 100644 benchmark/m2air_neon/many_pyramids.csv create mode 100644 benchmark/m2air_neon/rain.csv create mode 100644 benchmark/m2air_neon/trees100.csv create mode 100644 benchmark/m2air_neon/trees25.csv create mode 100644 benchmark/m2air_neon/trees50.csv create mode 100644 benchmark/m2air_neon/washer.csv delete mode 100644 data/dumps/single_box/box3d_dump.inl create mode 100644 test/test_sat.c diff --git a/.github/workflows/build.yml b/.github/workflows/build.yml index 19ba414..06e89d1 100644 --- a/.github/workflows/build.yml +++ b/.github/workflows/build.yml @@ -81,6 +81,7 @@ jobs: working-directory: ${{github.workspace}}/build env: MSAN_OPTIONS: ${{ matrix['msan-options'] || '' }} + UBSAN_OPTIONS: print_stacktrace=1 run: ${{ matrix.test }} # mingw needs some extra setup so keeping it separate from the matrix. diff --git a/.gitignore b/.gitignore index 242d3b4..dc1cd05 100644 --- a/.gitignore +++ b/.gitignore @@ -13,3 +13,4 @@ CMakeUserPresets.json .fetchcontent-cache/ *.b3rec Superluminal/ +build-dp/ diff --git a/CMakeLists.txt b/CMakeLists.txt index cd1322f..69a167d 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -34,15 +34,16 @@ if (MSVC OR APPLE OR UNIX) add_link_options("$<$:/INCREMENTAL:NO>") elseif(APPLE) # more sanitizers on Apple clang + # UBSan recovers by default, no-recover makes the first report fatal so CI fails if(BOX3D_SANITIZER_TYPE STREQUAL "thread") add_compile_options(-fsanitize=thread -fno-omit-frame-pointer) add_link_options(-fsanitize=thread) elseif(BOX3D_SANITIZER_TYPE STREQUAL "undefined") - add_compile_options(-fsanitize=undefined -fno-omit-frame-pointer) + add_compile_options(-fsanitize=undefined -fno-sanitize-recover=all -fno-omit-frame-pointer) add_link_options(-fsanitize=undefined) else() # default to address sanitizer - add_compile_options(-fsanitize=address -fsanitize-address-use-after-scope -fsanitize=undefined) + add_compile_options(-fsanitize=address -fsanitize-address-use-after-scope -fsanitize=undefined -fno-sanitize-recover=all) add_link_options(-fsanitize=address -fsanitize-address-use-after-scope -fsanitize=undefined) endif() elseif(UNIX) @@ -53,7 +54,7 @@ if (MSVC OR APPLE OR UNIX) add_compile_options(-fsanitize=thread -fno-omit-frame-pointer -fPIE) add_link_options(-fsanitize=thread -pie) elseif(BOX3D_SANITIZER_TYPE STREQUAL "undefined") - add_compile_options(-fsanitize=undefined -fno-omit-frame-pointer) + add_compile_options(-fsanitize=undefined -fno-sanitize-recover=all -fno-omit-frame-pointer) add_link_options(-fsanitize=undefined) elseif(BOX3D_SANITIZER_TYPE STREQUAL "memory") add_compile_options(-fsanitize=memory -fno-omit-frame-pointer) diff --git a/benchmark/amd7950x_scalar/convex_pile.csv b/benchmark/amd7950x_scalar/convex_pile.csv new file mode 100644 index 0000000..6134458 --- /dev/null +++ b/benchmark/amd7950x_scalar/convex_pile.csv @@ -0,0 +1,9 @@ +threads,ms +1,40706 +2,20798.5 +3,13788.7 +4,10323.8 +5,8358.62 +6,6957.96 +7,6005.53 +8,5292.26 diff --git a/benchmark/amd7950x_scalar/joint_grid.csv b/benchmark/amd7950x_scalar/joint_grid.csv new file mode 100644 index 0000000..83d11be --- /dev/null +++ b/benchmark/amd7950x_scalar/joint_grid.csv @@ -0,0 +1,9 @@ +threads,ms +1,1525.94 +2,780.126 +3,524.776 +4,401.402 +5,325.756 +6,273.225 +7,238.761 +8,213.639 diff --git a/benchmark/amd7950x_scalar/junkyard.csv b/benchmark/amd7950x_scalar/junkyard.csv new file mode 100644 index 0000000..df2f358 --- /dev/null +++ b/benchmark/amd7950x_scalar/junkyard.csv @@ -0,0 +1,9 @@ +threads,ms +1,31501.3 +2,16601.6 +3,11227.1 +4,8563.17 +5,6942.15 +6,5857.2 +7,5070.54 +8,4517.74 diff --git a/benchmark/amd7950x_scalar/large_pyramid.csv b/benchmark/amd7950x_scalar/large_pyramid.csv new file mode 100644 index 0000000..930f930 --- /dev/null +++ b/benchmark/amd7950x_scalar/large_pyramid.csv @@ -0,0 +1,9 @@ +threads,ms +1,5026.74 +2,2674.22 +3,1794.82 +4,1368.46 +5,1105.67 +6,939.709 +7,808.349 +8,720.536 diff --git a/benchmark/amd7950x_scalar/large_world.csv b/benchmark/amd7950x_scalar/large_world.csv new file mode 100644 index 0000000..61bc940 --- /dev/null +++ b/benchmark/amd7950x_scalar/large_world.csv @@ -0,0 +1,9 @@ +threads,ms +1,24.5593 +2,26.5894 +3,27.5323 +4,27.8544 +5,27.8073 +6,28.2759 +7,28.1201 +8,28.7588 diff --git a/benchmark/amd7950x_scalar/many_pyramids.csv b/benchmark/amd7950x_scalar/many_pyramids.csv new file mode 100644 index 0000000..8abe613 --- /dev/null +++ b/benchmark/amd7950x_scalar/many_pyramids.csv @@ -0,0 +1,9 @@ +threads,ms +1,4932.85 +2,2561.79 +3,1704.06 +4,1272.41 +5,1017.39 +6,850.793 +7,724.552 +8,635.926 diff --git a/benchmark/amd7950x_scalar/rain.csv b/benchmark/amd7950x_scalar/rain.csv new file mode 100644 index 0000000..41d830e --- /dev/null +++ b/benchmark/amd7950x_scalar/rain.csv @@ -0,0 +1,9 @@ +threads,ms +1,2540.8 +2,1420.77 +3,992.02 +4,783.73 +5,650.823 +6,570.124 +7,510.213 +8,467.848 diff --git a/benchmark/amd7950x_scalar/trees100.csv b/benchmark/amd7950x_scalar/trees100.csv new file mode 100644 index 0000000..e4f26e3 --- /dev/null +++ b/benchmark/amd7950x_scalar/trees100.csv @@ -0,0 +1,9 @@ +threads,ms +1,221.556 +2,143.138 +3,113.761 +4,100.076 +5,93.3681 +6,89.6276 +7,88.2381 +8,85.6611 diff --git a/benchmark/amd7950x_scalar/trees25.csv b/benchmark/amd7950x_scalar/trees25.csv new file mode 100644 index 0000000..0d566ff --- /dev/null +++ b/benchmark/amd7950x_scalar/trees25.csv @@ -0,0 +1,9 @@ +threads,ms +1,815.263 +2,468.217 +3,344.646 +4,296.781 +5,269.952 +6,252.229 +7,240.146 +8,234.044 diff --git a/benchmark/amd7950x_scalar/trees50.csv b/benchmark/amd7950x_scalar/trees50.csv new file mode 100644 index 0000000..b44d02b --- /dev/null +++ b/benchmark/amd7950x_scalar/trees50.csv @@ -0,0 +1,9 @@ +threads,ms +1,346.159 +2,201.958 +3,154.425 +4,134.719 +5,126.168 +6,118.983 +7,114.768 +8,113.253 diff --git a/benchmark/amd7950x_scalar/washer.csv b/benchmark/amd7950x_scalar/washer.csv index 83bc876..350d561 100644 --- a/benchmark/amd7950x_scalar/washer.csv +++ b/benchmark/amd7950x_scalar/washer.csv @@ -1,9 +1,9 @@ threads,ms -1,0 -2,0 -3,0 -4,0 -5,0 -6,0 -7,0 -8,5488.14 +1,42026.4 +2,22031.7 +3,15189.6 +4,11643.4 +5,9501.71 +6,8106.17 +7,7077.93 +8,6338.72 diff --git a/benchmark/amd7950x_sse2/convex_pile.csv b/benchmark/amd7950x_sse2/convex_pile.csv index b1584e4..bc51d9a 100644 --- a/benchmark/amd7950x_sse2/convex_pile.csv +++ b/benchmark/amd7950x_sse2/convex_pile.csv @@ -1,9 +1,9 @@ threads,ms -1,50240.2 -2,37216.3 -3,31420.6 -4,23466.2 -5,19266 -6,16078.2 -7,13874.8 -8,12405.8 +1,17336.6 +2,8857.22 +3,5945.65 +4,4508.71 +5,3674.55 +6,3105.97 +7,2696.86 +8,2410.11 diff --git a/benchmark/amd7950x_sse2/joint_grid.csv b/benchmark/amd7950x_sse2/joint_grid.csv index 5dee352..f5645ca 100644 --- a/benchmark/amd7950x_sse2/joint_grid.csv +++ b/benchmark/amd7950x_sse2/joint_grid.csv @@ -1,9 +1,9 @@ threads,ms -1,1488.56 -2,760.38 -3,512.213 -4,389.109 -5,316.481 -6,268.681 -7,234.538 -8,208.135 +1,1586.48 +2,787.633 +3,531.491 +4,401.5 +5,325.625 +6,275.095 +7,238.344 +8,214.504 diff --git a/benchmark/amd7950x_sse2/junkyard.csv b/benchmark/amd7950x_sse2/junkyard.csv index b6f3fd7..1d7791b 100644 --- a/benchmark/amd7950x_sse2/junkyard.csv +++ b/benchmark/amd7950x_sse2/junkyard.csv @@ -1,9 +1,9 @@ threads,ms -1,24363.8 -2,13463.4 -3,9229.2 -4,7110.22 -5,5784.18 -6,4952.28 -7,4305.77 -8,3859.87 +1,20044.1 +2,10386.4 +3,7087.11 +4,5437.88 +5,4451.58 +6,3792.59 +7,3293.41 +8,2970.84 diff --git a/benchmark/amd7950x_sse2/large_pyramid.csv b/benchmark/amd7950x_sse2/large_pyramid.csv index 1ba6824..0c7d8e6 100644 --- a/benchmark/amd7950x_sse2/large_pyramid.csv +++ b/benchmark/amd7950x_sse2/large_pyramid.csv @@ -1,9 +1,9 @@ threads,ms -1,2182.19 -2,1111.43 -3,770.761 -4,588.373 -5,487.242 -6,412.618 -7,361.125 -8,327.371 +1,2071.23 +2,1143.38 +3,778.58 +4,597.287 +5,487.523 +6,419.766 +7,366.281 +8,333.042 diff --git a/benchmark/amd7950x_sse2/large_world.csv b/benchmark/amd7950x_sse2/large_world.csv index f22c3aa..47edee3 100644 --- a/benchmark/amd7950x_sse2/large_world.csv +++ b/benchmark/amd7950x_sse2/large_world.csv @@ -1,9 +1,9 @@ threads,ms -1,10.0177 -2,12.2186 -3,13.1469 -4,13.9029 -5,14.4534 -6,14.2852 -7,14.2133 -8,14.433 +1,10.938 +2,12.76 +3,13.5526 +4,13.9525 +5,15.3005 +6,15.6679 +7,15.4235 +8,14.7126 diff --git a/benchmark/amd7950x_sse2/many_pyramids.csv b/benchmark/amd7950x_sse2/many_pyramids.csv index dc673a0..ac3b0d1 100644 --- a/benchmark/amd7950x_sse2/many_pyramids.csv +++ b/benchmark/amd7950x_sse2/many_pyramids.csv @@ -1,9 +1,9 @@ threads,ms -1,2415.01 -2,1205.83 -3,797.971 -4,608.846 -5,493.836 -6,413.397 -7,352.727 -8,311.869 +1,2331.14 +2,1259.61 +3,827.118 +4,616.691 +5,495.451 +6,411.55 +7,355.527 +8,316.039 diff --git a/benchmark/amd7950x_sse2/rain.csv b/benchmark/amd7950x_sse2/rain.csv index 32d2c9d..8b0058d 100644 --- a/benchmark/amd7950x_sse2/rain.csv +++ b/benchmark/amd7950x_sse2/rain.csv @@ -1,9 +1,9 @@ threads,ms -1,2295.15 -2,1266.67 -3,891.229 -4,705.264 -5,586.454 -6,511.619 -7,460.223 -8,424.157 +1,2434.57 +2,1325.14 +3,937.234 +4,737.677 +5,615.764 +6,536.387 +7,479.755 +8,444.147 diff --git a/benchmark/amd7950x_sse2/trees100.csv b/benchmark/amd7950x_sse2/trees100.csv index 3006d9f..50fa9b4 100644 --- a/benchmark/amd7950x_sse2/trees100.csv +++ b/benchmark/amd7950x_sse2/trees100.csv @@ -1,9 +1,9 @@ threads,ms -1,209.402 -2,136.201 -3,109.042 -4,96.4747 -5,90.4057 -6,86.776 -7,84.5495 -8,82.5579 +1,225.438 +2,141.14 +3,115.727 +4,101.289 +5,93.6898 +6,90.61 +7,86.1046 +8,86.7291 diff --git a/benchmark/amd7950x_sse2/trees25.csv b/benchmark/amd7950x_sse2/trees25.csv index f444483..73c74eb 100644 --- a/benchmark/amd7950x_sse2/trees25.csv +++ b/benchmark/amd7950x_sse2/trees25.csv @@ -1,9 +1,9 @@ threads,ms -1,717.735 -2,409.163 -3,304.326 -4,258.89 -5,234.893 -6,218.016 -7,206.416 -8,200.162 +1,727.276 +2,433.755 +3,315.505 +4,270.352 +5,246.82 +6,228.451 +7,216.217 +8,210.749 diff --git a/benchmark/amd7950x_sse2/trees50.csv b/benchmark/amd7950x_sse2/trees50.csv index 9feb624..1bc17fd 100644 --- a/benchmark/amd7950x_sse2/trees50.csv +++ b/benchmark/amd7950x_sse2/trees50.csv @@ -1,9 +1,9 @@ threads,ms -1,325.527 -2,199.714 -3,152.192 -4,133.217 -5,123.004 -6,115.157 -7,111.613 -8,108.696 +1,334.365 +2,191.522 +3,144.41 +4,127.041 +5,119.247 +6,113.65 +7,108.433 +8,105.905 diff --git a/benchmark/amd7950x_sse2/washer.csv b/benchmark/amd7950x_sse2/washer.csv index 9865102..fc18745 100644 --- a/benchmark/amd7950x_sse2/washer.csv +++ b/benchmark/amd7950x_sse2/washer.csv @@ -1,9 +1,9 @@ threads,ms -1,27144.2 -2,14602.3 -3,10240.3 -4,8022.88 -5,6649.81 -6,5770.75 -7,5112.17 -8,4749.63 +1,27059.6 +2,14569.2 +3,10081.7 +4,7825.43 +5,6480.34 +6,5518.81 +7,4901.6 +8,4256.68 diff --git a/benchmark/m2air_neon/convex_pile.csv b/benchmark/m2air_neon/convex_pile.csv new file mode 100644 index 0000000..652d979 --- /dev/null +++ b/benchmark/m2air_neon/convex_pile.csv @@ -0,0 +1,5 @@ +threads,ms +1,15032.6 +2,7877.68 +3,5546.68 +4,4296.53 diff --git a/benchmark/m2air_neon/joint_grid.csv b/benchmark/m2air_neon/joint_grid.csv new file mode 100644 index 0000000..6439c4e --- /dev/null +++ b/benchmark/m2air_neon/joint_grid.csv @@ -0,0 +1,5 @@ +threads,ms +1,1089.82 +2,569.935 +3,399.883 +4,309.318 diff --git a/benchmark/m2air_neon/junkyard.csv b/benchmark/m2air_neon/junkyard.csv new file mode 100644 index 0000000..9b73cc6 --- /dev/null +++ b/benchmark/m2air_neon/junkyard.csv @@ -0,0 +1,5 @@ +threads,ms +1,13501.4 +2,7288.19 +3,5229.53 +4,4253.67 diff --git a/benchmark/m2air_neon/large_pyramid.csv b/benchmark/m2air_neon/large_pyramid.csv new file mode 100644 index 0000000..db51073 --- /dev/null +++ b/benchmark/m2air_neon/large_pyramid.csv @@ -0,0 +1,5 @@ +threads,ms +1,1810.73 +2,976.492 +3,687.311 +4,563.518 diff --git a/benchmark/m2air_neon/large_world.csv b/benchmark/m2air_neon/large_world.csv new file mode 100644 index 0000000..48ea598 --- /dev/null +++ b/benchmark/m2air_neon/large_world.csv @@ -0,0 +1,5 @@ +threads,ms +1,8.9855 +2,10.7568 +3,14.1527 +4,14.9816 diff --git a/benchmark/m2air_neon/many_pyramids.csv b/benchmark/m2air_neon/many_pyramids.csv new file mode 100644 index 0000000..6070afb --- /dev/null +++ b/benchmark/m2air_neon/many_pyramids.csv @@ -0,0 +1,5 @@ +threads,ms +1,1917.44 +2,1025.05 +3,727.266 +4,621.283 diff --git a/benchmark/m2air_neon/rain.csv b/benchmark/m2air_neon/rain.csv new file mode 100644 index 0000000..5abbe15 --- /dev/null +++ b/benchmark/m2air_neon/rain.csv @@ -0,0 +1,5 @@ +threads,ms +1,1954.85 +2,1079.88 +3,789.64 +4,626.161 diff --git a/benchmark/m2air_neon/trees100.csv b/benchmark/m2air_neon/trees100.csv new file mode 100644 index 0000000..44df54a --- /dev/null +++ b/benchmark/m2air_neon/trees100.csv @@ -0,0 +1,5 @@ +threads,ms +1,186.082 +2,119.994 +3,103.081 +4,91.2481 diff --git a/benchmark/m2air_neon/trees25.csv b/benchmark/m2air_neon/trees25.csv new file mode 100644 index 0000000..9ef25d8 --- /dev/null +++ b/benchmark/m2air_neon/trees25.csv @@ -0,0 +1,5 @@ +threads,ms +1,609.865 +2,348.635 +3,272.314 +4,236.167 diff --git a/benchmark/m2air_neon/trees50.csv b/benchmark/m2air_neon/trees50.csv new file mode 100644 index 0000000..04448b0 --- /dev/null +++ b/benchmark/m2air_neon/trees50.csv @@ -0,0 +1,5 @@ +threads,ms +1,288.292 +2,184.331 +3,144.592 +4,156.867 diff --git a/benchmark/m2air_neon/washer.csv b/benchmark/m2air_neon/washer.csv new file mode 100644 index 0000000..47a7ad1 --- /dev/null +++ b/benchmark/m2air_neon/washer.csv @@ -0,0 +1,5 @@ +threads,ms +1,21065.2 +2,11419.4 +3,8383.39 +4,7675.16 diff --git a/benchmark/main.c b/benchmark/main.c index 16df1fe..8e8cf1c 100644 --- a/benchmark/main.c +++ b/benchmark/main.c @@ -150,9 +150,9 @@ int main( int argc, char** argv ) #endif Benchmark benchmarks[] = { - { "convex_pile", NULL, CreateConvexPile, NULL, NULL, 500 }, + { "convex_pile", GetConvexPileCapacity, CreateConvexPile, NULL, NULL, 500 }, { "joint_grid", NULL, CreateJointGrid, NULL, NULL, 100 }, - { "junkyard", NULL, CreateJunkyard, NULL, StepJunkyard, 500 }, + { "junkyard", GetJunkyardCapacity, CreateJunkyard, NULL, StepJunkyard, 500 }, { "large_pyramid", NULL, CreateLargePyramid, NULL, NULL, 200 }, { "large_world", GetLargeWorldCapacity, CreateLargeWorld, NULL, StepLargeWorld, 500 }, { "many_pyramids", NULL, CreateManyPyramids, NULL, NULL, 100 }, diff --git a/data/dumps/single_box/box3d_dump.inl b/data/dumps/single_box/box3d_dump.inl deleted file mode 100644 index 9b60dea..0000000 --- a/data/dumps/single_box/box3d_dump.inl +++ /dev/null @@ -1,85 +0,0 @@ -b3Vec3 gravity = {0, -10, 0}; -b3World_SetGravity(m_worldId, gravity); -std::vector bodies; -std::vector joints; - -{ - b3BodyDef bd = b3DefaultBodyDef(); - bd.name = "cube"; - bd.type = b3BodyType(2); - bd.position = {-7.98573353e-07, 0.499929696, -9.86034479e-07}; - bd.rotation = {{-4.03613463e-08, 0.825855613, 4.68083634e-08}, 0.563881755}; - bd.linearVelocity = {2.47239775e-08, -1.7695136e-08, 8.65693384e-09}; - bd.angularVelocity = {1.73163173e-08, -4.2026933e-15, -4.94549042e-08}; - bd.linearDamping = 0; - bd.angularDamping = 0; - bd.enableSleep = bool(1); - bd.isAwake = bool(1); - bd.gravityScale = 1; - b3BodyId bodyId = b3CreateBody(m_worldId, &bd); - bodies.push_back(bodyId); - - { - b3ShapeDef sd = b3DefaultShapeDef(); - sd.density = 1000; - sd.isSensor = bool(0); - sd.filter.categoryBits = 0x1; - sd.filter.maskBits = 0xffffffffffffffff; - sd.filter.groupIndex = 0; - sd.baseMaterial.friction = 0.600000024; - sd.baseMaterial.restitution = 0; - sd.baseMaterial.rollingResistance = 0; - b3Vec3 vs[8]; - vs[0] = {0.5, 0.5, 0.5}; - vs[1] = {-0.5, 0.5, 0.5}; - vs[2] = {-0.5, -0.5, 0.5}; - vs[3] = {0.5, -0.5, 0.5}; - vs[4] = {0.5, 0.5, -0.5}; - vs[5] = {-0.5, 0.5, -0.5}; - vs[6] = {-0.5, -0.5, -0.5}; - vs[7] = {0.5, -0.5, -0.5}; - b3HullData* hullData = b3CreateHull(vs, 8, 8); - b3CreateHullShape(bodyId, &sd, hullData); - b3DestroyHull(hullData); - } -} -{ - b3BodyDef bd = b3DefaultBodyDef(); - bd.name = "ground"; - bd.type = b3BodyType(0); - bd.position = {0, -1, 0}; - bd.rotation = {{0, 0, 0}, 1}; - bd.linearVelocity = {0, 0, 0}; - bd.angularVelocity = {0, 0, 0}; - bd.linearDamping = 0; - bd.angularDamping = 0; - bd.enableSleep = bool(1); - bd.isAwake = bool(0); - bd.gravityScale = 1; - b3BodyId bodyId = b3CreateBody(m_worldId, &bd); - bodies.push_back(bodyId); - - { - b3ShapeDef sd = b3DefaultShapeDef(); - sd.density = 1000; - sd.isSensor = bool(0); - sd.filter.categoryBits = 0x1; - sd.filter.maskBits = 0xffffffffffffffff; - sd.filter.groupIndex = 0; - sd.baseMaterial.friction = 0.600000024; - sd.baseMaterial.restitution = 0; - sd.baseMaterial.rollingResistance = 0; - b3Vec3 vs[8]; - vs[0] = {15, 1, 15}; - vs[1] = {-15, 1, 15}; - vs[2] = {-15, -1, 15}; - vs[3] = {15, -1, 15}; - vs[4] = {15, 1, -15}; - vs[5] = {-15, 1, -15}; - vs[6] = {-15, -1, -15}; - vs[7] = {15, -1, -15}; - b3HullData* hullData = b3CreateHull(vs, 8, 8); - b3CreateHullShape(bodyId, &sd, hullData); - b3DestroyHull(hullData); - } -} diff --git a/include/box3d/base.h b/include/box3d/base.h index 25828b3..6878cb6 100644 --- a/include/box3d/base.h +++ b/include/box3d/base.h @@ -38,7 +38,7 @@ #ifdef __cplusplus #define B3_API extern "C" BOX3D_EXPORT #define B3_INLINE inline - + #define B3_ALIGN_AS(N) alignas(N) #if defined( _MSC_VER ) #define B3_FORCE_INLINE __forceinline #elif defined( __GNUC__ ) || defined( __clang__ ) @@ -52,6 +52,7 @@ #else #define B3_API BOX3D_EXPORT #define B3_INLINE static inline + #define B3_ALIGN_AS(N) _Alignas(N) #if defined( _MSC_VER ) #define B3_FORCE_INLINE static __forceinline diff --git a/include/box3d/box3d.h b/include/box3d/box3d.h index 569117f..d993810 100644 --- a/include/box3d/box3d.h +++ b/include/box3d/box3d.h @@ -524,31 +524,31 @@ B3_API b3WorldTransform b3Body_GetTransform( b3BodyId bodyId ); /// Set the world transform of a body. This acts as a teleport and is fairly expensive. /// @note Generally you should create a body with the intended transform. -/// @see b3BodyDef::position and b3BodyDef::rotation +/// @see b3BodyDef::position and b3BodyDef::rotation. B3_API void b3Body_SetTransform( b3BodyId bodyId, b3Pos position, b3Quat rotation ); -/// Get a local point on a body given a world point +/// Get a local point on a body given a world point. B3_API b3Vec3 b3Body_GetLocalPoint( b3BodyId bodyId, b3Pos worldPoint ); -/// Get a world point on a body given a local point +/// Get a world point on a body given a local point. B3_API b3Pos b3Body_GetWorldPoint( b3BodyId bodyId, b3Vec3 localPoint ); -/// Get a local vector on a body given a world vector +/// Get a local vector on a body given a world vector. B3_API b3Vec3 b3Body_GetLocalVector( b3BodyId bodyId, b3Vec3 worldVector ); -/// Get a world vector on a body given a local vector +/// Get a world vector on a body given a local vector. B3_API b3Vec3 b3Body_GetWorldVector( b3BodyId bodyId, b3Vec3 localVector ); /// Get the linear velocity of a body's center of mass. Usually in meters per second. B3_API b3Vec3 b3Body_GetLinearVelocity( b3BodyId bodyId ); -/// Get the angular velocity of a body in radians per second +/// Get the angular velocity of a body in radians per second. B3_API b3Vec3 b3Body_GetAngularVelocity( b3BodyId bodyId ); -/// Set the linear velocity of a body. Usually in meters per second. +/// Set the linear velocity of a body at the center of mass. Usually in meters per second. B3_API void b3Body_SetLinearVelocity( b3BodyId bodyId, b3Vec3 linearVelocity ); -/// Set the angular velocity of a body in radians per second +/// Set the angular velocity of a body in radians per second. B3_API void b3Body_SetAngularVelocity( b3BodyId bodyId, b3Vec3 angularVelocity ); /// Set the velocity to reach the given transform after a given time step. @@ -710,6 +710,14 @@ B3_API void b3Body_SetBullet( b3BodyId bodyId, bool flag ); /// Is this body a bullet? B3_API bool b3Body_IsBullet( b3BodyId bodyId ); +/// Allow this body to rotate fast. Useful for axially symmetric bodies, such as vehicle wheels. +/// Normally rotation speed is clamped to improve CCD. However, this clamping is unnecessary for +/// bodies that only rotate fast around an axis of symmetry. +B3_API void b3Body_AllowFastRotation( b3BodyId bodyId, bool flag ); + +/// Is this body allowed to rotate fast? +B3_API bool b3Body_IsFastRotationAllowed( b3BodyId bodyId ); + /// Enable or disable contact recycling for this body. Contact recycling is a performance optimization /// that reuses contact manifolds when bodies move slightly. Disabling it can avoid ghost collisions /// on characters at the cost of higher per-step work. Existing contacts retain their prior setting; @@ -778,6 +786,9 @@ B3_API int b3Body_CollideMover( b3BodyId bodyId, b3BodyPlaneResult* bodyPlanes, * Functions to create, destroy, and access. * Shapes bind raw geometry to bodies and hold material properties including friction and restitution. * You may add multiple shapes to a single body. There are no hard limits on shape count per body. + * + * When you create a shape on a body the center of mass moves. This can lead to the body linear velocity + * changing if the angular velocity is non-zero. * @{ */ diff --git a/include/box3d/collision.h b/include/box3d/collision.h index 1ba0070..3854cb2 100644 --- a/include/box3d/collision.h +++ b/include/box3d/collision.h @@ -163,6 +163,17 @@ B3_INLINE const b3HullHalfEdge* b3GetHullEdges( const b3HullData* hull ) return (const b3HullHalfEdge*)( (intptr_t)hull + hull->edgeOffset ); } +/// Get read only hull planes. +B3_INLINE const b3Plane* b3GetHullPlanes( const b3HullData* hull ) +{ + if ( hull->planeOffset == 0 ) + { + return NULL; + } + + return (const b3Plane*)( (intptr_t)hull + hull->planeOffset ); +} + /// Get read only hull faces. B3_INLINE const b3HullFace* b3GetHullFaces( const b3HullData* hull ) { @@ -174,15 +185,30 @@ B3_INLINE const b3HullFace* b3GetHullFaces( const b3HullData* hull ) return (const b3HullFace*)( (intptr_t)hull + hull->faceOffset ); } -/// Get read only hull planes. -B3_INLINE const b3Plane* b3GetHullPlanes( 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. +B3_INLINE const float* b3GetHullSoaVertices( const b3HullData* hull ) { - if ( hull->planeOffset == 0 ) + if ( hull->soaVertexOffset == 0 ) { return NULL; } - return (const b3Plane*)( (intptr_t)hull + hull->planeOffset ); + return (const float*)( (intptr_t)hull + hull->soaVertexOffset ); +} + +/// 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 repeats of the first value. +B3_INLINE const float* b3GetHullSoaNormals( const b3HullData* hull ) +{ + if ( hull->soaNormalOffset == 0 ) + { + return NULL; + } + + return (const float*)( (intptr_t)hull + hull->soaNormalOffset ); } /// Create a tessellated cylinder as a hull. @@ -426,14 +452,12 @@ B3_API b3CompoundData* b3CreateCompound( const b3CompoundDef* def ); /// Destroy a compound shape. B3_API void b3DestroyCompound( b3CompoundData* compound ); -/// If bytes is null then this returns the number of required bytes. This clones all the -/// data into the bytes buffer. This is expected to run offline or asynchronously. -/// This mutates the compound to nullify pointers, leaving the compound in an unusable state. +/// Cast the provided compound data to bytes, setting the internal pointers to null. +/// Use this before serializing the compound bytes. B3_API uint8_t* b3ConvertCompoundToBytes( b3CompoundData* compound ); -/// Convert bytes to compound. This does not clone. The bytes must remain in scope while the -/// compound is used. This is done to improve run-time performance and allow for instancing. -/// The bytes are mutated to fixup pointers. +/// Cast the provided bytes to compound data, setting up internal pointers. +/// Use this after de-serializing the compound bytes. B3_API b3CompoundData* b3ConvertBytesToCompound( uint8_t* bytes, int byteCount ); /**@}*/ // compound diff --git a/include/box3d/constants.h b/include/box3d/constants.h index 55427ba..de8da49 100644 --- a/include/box3d/constants.h +++ b/include/box3d/constants.h @@ -109,6 +109,23 @@ B3_API float b3GetStallThreshold( void ); /// The maximum number points to use for shape cast proxies (swept point cloud). #define B3_MAX_SHAPE_CAST_POINTS 64 +/// The number of iterations for gyroscopic torques. +#ifndef B3_GYROSCOPIC_ITERATIONS +#define B3_GYROSCOPIC_ITERATIONS 1 +#endif + +/// The maximum number of convex hull vertices. This is fixed for performance reasons. +#define B3_MAX_HULL_VERTICES 128 + +/// The maximum number of convex hull faces. +#define B3_MAX_HULL_FACES 128 + +/// The maximum number of convex hull edges. Full edges, not half-edges. +#define B3_MAX_HULL_EDGES 128 + +/// Relative tolerance used to determine if two edges are parallel. +#define B3_PARALLEL_EDGE_TOL 0.005f + /// These generous limits allow for easy hashing. See b3ShapePairKey. #define B3_SHAPE_POWER 22 #define B3_CHILD_POWER ( 64 - 2 * B3_SHAPE_POWER ) diff --git a/include/box3d/types.h b/include/box3d/types.h index 65918dc..96df9df 100644 --- a/include/box3d/types.h +++ b/include/box3d/types.h @@ -1930,8 +1930,7 @@ typedef struct b3Capsule * @{ */ -/// A hull vertex. Identified by a half-edge with this -/// vertex as its tail. +/// A hull vertex. Identified by a half-edge with this vertex as its tail. typedef struct b3HullVertex { /// A half-edge that has this vertex as the origin @@ -1965,7 +1964,7 @@ typedef struct b3HullFace } b3HullFace; /// 64-bit hull version. Useful for validating serialized data. -#define B3_HULL_VERSION 0x9D4716CE3793900Eull +#define B3_HULL_VERSION 0xDA5150191B994C01ull /// A convex hull. /// @note This data structure has data hanging off the end and cannot be directly copied. @@ -2016,11 +2015,17 @@ typedef struct b3HullData /// The face count. Hulls faces are convex polygons. int faceCount; + /// Offset of the face plane array in bytes from the struct address. + int planeOffset; + /// Offset of the face array in bytes from the struct address. int faceOffset; - /// Offset of the face plane array in bytes from the struct address. - int planeOffset; + /// Offset of structure of array (SOA) vertices + int soaVertexOffset; + + /// Offset of structure of array (SOA) unit normal vectors + int soaNormalOffset; /// Explicit padding. Hull identity is a content hash and memcmp over raw bytes, /// so there must be no unnamed padding for struct copies to scramble. @@ -2035,9 +2040,15 @@ typedef struct b3BoxHull b3HullVertex boxVertices[8]; ///< Box vertices. b3Vec3 boxPoints[8]; ///< Box points. b3HullHalfEdge boxEdges[24]; ///< Box half-edges. - b3HullFace boxFaces[6]; ///< Box faces. - uint8_t padding[2]; ///< Explicit padding, see b3HullData::padding. b3Plane boxPlanes[6]; ///< Box face planes. + b3HullFace boxFaces[6]; ///< Box faces. + uint8_t padding[10]; ///< Explicit padding, see b3HullData::padding. + 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 } b3BoxHull; /**@}*/ // hull @@ -2048,13 +2059,13 @@ typedef struct b3BoxHull * @{ */ -/// This is used to create a re-usable collision mesh +/// This is used to create a re-usable collision mesh. typedef struct b3MeshDef { /// Triangle vertices b3Vec3* vertices; - /// Triangle vertex indices. 3 for each triangle. + /// Triangle vertex indices. 3 for each triangle. CCW winding. int32_t* indices; /// Triangle material index. 1 per triangle. Indexes into b3ShapeDef::materials. @@ -2408,7 +2419,7 @@ typedef struct b3CompoundDef } b3CompoundDef; /// The baked compound version depends on the tree, mesh, and hull versions. -#define B3_COMPOUND_VERSION ( 0x830778DB07086EB4ull ^ B3_DYNAMIC_TREE_VERSION ^ B3_MESH_VERSION ^ B3_HULL_VERSION ) +#define B3_COMPOUND_VERSION ( 0xB11DCE70FAD5622Bull ^ B3_DYNAMIC_TREE_VERSION ^ B3_MESH_VERSION ^ B3_HULL_VERSION ) /// Meshes used in compounds have limited space for materials. If you have /// a mesh with many materials, you can use it outside of the compound. @@ -2530,10 +2541,10 @@ typedef struct b3ChildShape /// Tagged union. union { - b3Capsule capsule; ///< Capsule. + b3Capsule capsule; ///< Capsule. const b3HullData* hull; ///< Hull. - b3Mesh mesh; ///< Mesh. - b3Sphere sphere; ///< Sphere. + b3Mesh mesh; ///< Mesh. + b3Sphere sphere; ///< Sphere. }; /// Transform of the shape into compound local space. @@ -2943,12 +2954,12 @@ typedef struct b3DebugShape /// Tagged union. union { - const b3Capsule* capsule; ///< Capsule shape. + const b3Capsule* capsule; ///< Capsule shape. const b3CompoundData* compound; ///< Compound shape. const b3HeightFieldData* heightField; ///< Height-field shape. - const b3HullData* hull; ///< Convex hull shape. - const b3Mesh* mesh; ///< Mesh shape with scale. - const b3Sphere* sphere; ///< Sphere shape. + const b3HullData* hull; ///< Convex hull shape. + const b3Mesh* mesh; ///< Mesh shape with scale. + const b3Sphere* sphere; ///< Sphere shape. }; } b3DebugShape; diff --git a/samples/gfx/CMakeLists.txt b/samples/gfx/CMakeLists.txt index e2594e2..6036cef 100644 --- a/samples/gfx/CMakeLists.txt +++ b/samples/gfx/CMakeLists.txt @@ -44,6 +44,12 @@ set(GFX_FILES add_library(gfx STATIC ${GFX_FILES}) +set_target_properties(gfx PROPERTIES + C_STANDARD 17 + C_STANDARD_REQUIRED YES + C_EXTENSIONS YES +) + if(APPLE) set_source_files_properties( sokol_impl.c diff --git a/samples/host/sokol_app_impl.c b/samples/host/sokol_app_impl.c index 3113529..398e2d3 100644 --- a/samples/host/sokol_app_impl.c +++ b/samples/host/sokol_app_impl.c @@ -8,9 +8,10 @@ // `sokol` INTERFACE target. On Apple this file is compiled as Objective-C // with ARC (see src/app/CMakeLists.txt) so sokol_app's Cocoa backend builds. // -// SOKOL_WIN32_FORCE_MAIN: sokol_app defaults to WinMain (subsystem:windows) -// on Windows. Our binaries are console-subsystem so the smoke test can -// surface stderr to ctest, so force the int main variant. +// SOKOL_NO_ENTRY: samples/main.cpp supplies its own main() so it can install +// MSVC CRT leak reporting and dump after sapp_run returns, past sokol's own +// teardown. Our main is console-subsystem, which also keeps stderr flowing to +// ctest for the smoke test. // // _POSIX_C_SOURCE: glibc gates clock_gettime / CLOCK_MONOTONIC behind a // POSIX feature-test macro under -std=c17. Define before any system headers. @@ -21,7 +22,7 @@ #define SOKOL_APP_IMPL #define SOKOL_GLUE_IMPL #define SOKOL_LOG_IMPL -#define SOKOL_WIN32_FORCE_MAIN +#define SOKOL_NO_ENTRY #include "sokol_gfx.h" #include "sokol_app.h" diff --git a/samples/main.cpp b/samples/main.cpp index 9ad9f83..361223b 100644 --- a/samples/main.cpp +++ b/samples/main.cpp @@ -1,6 +1,16 @@ // SPDX-FileCopyrightText: 2025 Erin Catto // SPDX-License-Identifier: MIT +#if defined( _MSC_VER ) +#ifndef _CRT_SECURE_NO_WARNINGS +#define _CRT_SECURE_NO_WARNINGS +#endif +#define _CRTDBG_MAP_ALLOC +// stdlib before crtdbg so the malloc->_malloc_dbg remap lands after the real prototype +#include +#include +#endif + #include "gfx/debug_adapter.h" #include "gfx/keycodes.h" #include "gfx/renderer.h" @@ -18,6 +28,12 @@ #include #include +#ifdef TRACY_ENABLE +#include +#else +#define FrameMark +#endif + #if defined( _WIN32 ) #define WIN32_LEAN_AND_MEAN #define NOMINMAX @@ -32,6 +48,7 @@ static SampleContext s_context; static int s_frame = 0; static int s_frameLimit = -1; static int s_sampleOverride = -1; +static int s_exitCode = 0; static int CompareSamples( const void* a, const void* b ) { @@ -75,6 +92,10 @@ static void OnDrawUI( void ) static void OnInit( void ) { +#ifdef TRACY_ENABLE + tracy::StartupProfiler(); +#endif + #if defined( _WIN32 ) timeBeginPeriod( 1 ); #endif @@ -307,12 +328,17 @@ static void OnEvent( const sapp_event* e ) } } -// Pace the loop to 60 Hz so the fixed 1/60 physics step plays at real time on any -// display. Sleep the bulk of the idle time, then spin the last bit since sleep wakes -// are only accurate to about a millisecond. +// This limits the render rate to the simulation rate. Physics debugging +// ergonomics require render frames and simulation frames to be one to one. +// I don't recommend this setup for games, where a decoupled render +// and simulation rate is desirable. static void LimitFrameRate( uint64_t frameStart ) { - const float targetMs = 1000.0f / 60.0f; + // By limiting to the simulation hertz, this allows me to run the simulation + // at 240Hz and render at that rate when using a 240Hz monitor. It also allows + // me to run a 10Hz simulation in at the wall clock rate. + float hertz = b3ClampFloat(s_context.hertz, 5.0f, 1000.0f); + const float targetMs = 1000.0f / hertz; const float spinMs = 2.0f; int sleepMs = (int)( targetMs - spinMs - b3GetMilliseconds( frameStart ) ); @@ -424,6 +450,9 @@ static void OnFrame( void ) sg_commit(); ++s_frame; + // For the Tracy profiler + FrameMark; + if ( s_frameLimit < 0 ) { LimitFrameRate( frameStart ); @@ -447,7 +476,13 @@ static void OnCleanup( void ) timeEndPeriod( 1 ); #endif - exit( errors == 0 ? 0 : 1 ); + #ifdef TRACY_ENABLE + tracy::ShutdownProfiler(); +#endif + + // Return instead of exit so sokol frees its own window and context. main + // dumps CRT leaks once sapp_run returns, past that teardown. + s_exitCode = errors == 0 ? 0 : 1; } static void OnAppLog( const char* tag, uint32_t logLevel, uint32_t logItemId, const char* message, uint32_t lineNumber, @@ -469,7 +504,7 @@ static void OnAppLog( const char* tag, uint32_t logLevel, uint32_t logItemId, co } } -sapp_desc sokol_main( int argc, char** argv ) +static sapp_desc BuildAppDesc( int argc, char** argv ) { for ( int i = 1; i < argc; ++i ) { @@ -528,3 +563,23 @@ sapp_desc sokol_main( int argc, char** argv ) return desc; } + +// We own main (SOKOL_NO_ENTRY) so the leak dump can run after sapp_run returns, +// once sokol has torn down its window and context. On Windows sapp_run returns +// after that teardown; on macOS it may not return, so the dump is Windows only. +int main( int argc, char** argv ) +{ +#if defined( _MSC_VER ) + _CrtSetReportMode( _CRT_WARN, _CRTDBG_MODE_DEBUG | _CRTDBG_MODE_FILE ); + _CrtSetReportFile( _CRT_WARN, _CRTDBG_FILE_STDOUT ); +#endif + + sapp_desc desc = BuildAppDesc( argc, argv ); + sapp_run( &desc ); + +#if defined( _MSC_VER ) + _CrtDumpMemoryLeaks(); +#endif + + return s_exitCode; +} diff --git a/samples/sample_benchmark.cpp b/samples/sample_benchmark.cpp index 41e37c1..c2b0584 100644 --- a/samples/sample_benchmark.cpp +++ b/samples/sample_benchmark.cpp @@ -1427,6 +1427,10 @@ public: GetGuiDraw()->drawJoints = false; } + b3Capacity capacity = {}; + GetJunkyardCapacity( &capacity ); + CreateWorld( &capacity ); + CreateJunkyard( m_worldId ); SetGroundShape( GetGroundShapeId() ); @@ -1461,6 +1465,10 @@ public: m_camera->SetView( 45.0f, 20.0f, 150.0f, { 0.0f, 15.0f, 0.0f } ); } + b3Capacity capacity = {}; + GetConvexPileCapacity( &capacity ); + CreateWorld( &capacity ); + CreateConvexPile( m_worldId ); SetGroundShape( GetGroundShapeId() ); diff --git a/samples/sample_bodies.cpp b/samples/sample_bodies.cpp index a0f3ef1..362f457 100644 --- a/samples/sample_bodies.cpp +++ b/samples/sample_bodies.cpp @@ -370,8 +370,9 @@ public: static int sampleGyroscopicTorque = RegisterSample( "Bodies", "Gyroscopic Torque", GyroscopicTorque::Create ); // Spinning tops. Ported from PEEL. -// Each top is tilted and spun about the world up axis. Offsetting the spin axis from the -// symmetry axis is what makes them precess under gravity instead of spinning true. +// Each top is tilted and spun about its symmetry axis. Gravity torque about the tip is what makes +// them precess instead of toppling. The first top carries a diagnostic that compares the measured +// precession rate against the classical heavy top solution. class GyroscopicPrecession : public Sample { public: @@ -400,7 +401,7 @@ public: b3ShapeDef shapeDef = b3DefaultShapeDef(); - // Tilt the symmetry axis, then spin about the world up axis so the offset drives precession. + // Tilt the top, then spin it about its own symmetry axis. Gravity does the rest. b3Quat rotation = b3MakeQuatFromAxisAngle( b3Vec3_axisZ, 15.0f * B3_PI / 180.0f ); b3Vec3 angularVelocity = b3RotateVector( rotation, { 0.0f, 75.0f, 0.0f } ); @@ -422,16 +423,156 @@ public: b3CreateHullShape( bodyId, &shapeDef, hull ); b3Body_SetAngularVelocity( bodyId, angularVelocity ); + + if ( x == 0 && z == 0 ) + { + m_topId = bodyId; + } } } b3DestroyHull( hull ); + + // Mass properties of the measured top. The tip sits at the body origin, so the pivot distance is + // just the height of the center of mass, and the symmetry axis is the local up axis. + b3MassData massData = b3Body_GetMassData( m_topId ); + m_mass = massData.mass; + m_pivotDistance = massData.center.y; + m_spinInertia = massData.inertia.cy.y; + + // Transverse inertia belongs about the pivot, not the center of mass + float transverse = 0.5f * ( massData.inertia.cx.x + massData.inertia.cz.z ); + m_transverseInertia = transverse + m_mass * m_pivotDistance * m_pivotDistance; + + m_gravity = b3Length( b3World_GetGravity( m_worldId ) ); + + m_azimuth = 0.0f; + m_actualAngle = 0.0f; + m_expectedAngle = 0.0f; + m_elapsed = 0.0f; + m_groundTime = 0.0f; + m_rate = 0.0f; + m_measuring = false; + } + + // Steady precession rate of a heavy symmetric top about the vertical. The slow root of + // I1 * W^2 * cos(tilt) - I3 * spin * W + M * g * d = 0. Goldstein 5.7. + // Collapses to torque over spin momentum for a fast top. + float ExpectedRate( float spin, float cosTilt ) const + { + float momentum = m_spinInertia * spin; + float torque = m_mass * m_gravity * m_pivotDistance; + if ( momentum <= FLT_EPSILON ) + { + return 0.0f; + } + + float a = m_transverseInertia * cosTilt; + float discriminant = momentum * momentum - 4.0f * a * torque; + if ( a <= FLT_EPSILON || discriminant < 0.0f ) + { + // Axis at or past horizontal, or spinning too slowly to precess steadily + return torque / momentum; + } + + return ( momentum - sqrtf( discriminant ) ) / ( 2.0f * a ); + } + + void Step() override + { + Sample::Step(); + + bool isAwake = b3Body_IsAwake( m_topId ); + if ( isAwake == false ) + { + DrawTextLine( "top is sleeping" ); + return; + } + + b3Pos tip = b3Body_GetPosition( m_topId ); + b3Quat quat = b3Body_GetRotation( m_topId ); + b3Vec3 axis = b3RotateVector( quat, b3Vec3_axisY ); + b3Vec3 omega = b3Body_GetAngularVelocity( m_topId ); + float spin = b3Dot( omega , axis ); + float cosTilt = b3ClampFloat( axis.y, -1.0f, 1.0f ); + float expected = ExpectedRate( spin, cosTilt ); + + if ( m_didStep ) + { + float timeStep = 1.0f / m_context->hertz; + + // Gravity exerts no torque about the center of mass while the top is airborne, so the pivot + // solution only applies once the tip lands. Give the landing impulse time to wash out too, + // otherwise it biases the average for the rest of the run. + if ( tip.y < 0.05f ) + { + m_groundTime += timeStep; + } + + if ( m_measuring == false ) + { + if ( m_groundTime > 0.5f ) + { + m_measuring = true; + m_azimuth = b3Atan2( -axis.z, axis.x ); + } + } + else + { + // Right handed angle of the symmetry axis about the world up axis + float azimuth = b3Atan2( -axis.z, axis.x ); + float delta = b3UnwindAngle( azimuth - m_azimuth ); + m_azimuth = azimuth; + + m_actualAngle += delta; + m_expectedAngle += expected * timeStep; + m_elapsed += timeStep; + + // Nutation swings the instantaneous rate between zero and twice the mean, so filter it + // with a one second time constant. + float alpha = timeStep / ( timeStep + 1.0f ); + m_rate += alpha * ( delta / timeStep - m_rate ); + } + } + + DrawLine( tip, b3OffsetPos( tip, 5.0f * axis ), MakeColor( b3_colorYellow ) ); + + DrawTextLine( "spin %.1f rad/s, tilt %.1f deg", spin, ( 180.0f / B3_PI ) * acosf( cosTilt ) ); + DrawTextLine( "precession: expected %.4f rad/s, actual %.4f rad/s", expected, m_rate ); + + if ( m_elapsed > 0.0f ) + { + // Average both sides over the same window. The spin decays, so the expected rate drifts with it. + float expectedAverage = m_expectedAngle / m_elapsed; + float actualAverage = m_actualAngle / m_elapsed; + float error = expectedAverage != 0.0f ? 100.0f * ( actualAverage - expectedAverage ) / expectedAverage : 0.0f; + DrawTextLine( "%.1f s average: expected %.4f, actual %.4f, error %+.1f%%", m_elapsed, expectedAverage, actualAverage, + error ); + } + else + { + DrawTextLine( "waiting for the tip to land" ); + } } static Sample* Create( SampleContext* context ) { return new GyroscopicPrecession( context ); } + + b3BodyId m_topId; + float m_mass; + float m_gravity; + float m_pivotDistance; + float m_spinInertia; + float m_transverseInertia; + float m_azimuth; + float m_actualAngle; + float m_expectedAngle; + float m_elapsed; + float m_groundTime; + float m_rate; + bool m_measuring; }; static int sampleGyroscopicPrecession = RegisterSample( "Bodies", "Gyroscopic Precession", GyroscopicPrecession::Create ); diff --git a/samples/sample_continuous.cpp b/samples/sample_continuous.cpp index 4872ddb..76afe53 100644 --- a/samples/sample_continuous.cpp +++ b/samples/sample_continuous.cpp @@ -4,7 +4,6 @@ #include "imgui.h" #include "sample.h" #include "gfx/draw.h" -#include "stability.h" #include "utils.h" #include "gfx/debug_adapter.h" @@ -741,60 +740,6 @@ public: static int sampleMeshDrop = RegisterSample( "Continuous", "Mesh Drop", MeshDrop::Create ); -class MeshDropUnitTest : public Sample -{ -public: - explicit MeshDropUnitTest( SampleContext* context ) - : Sample( context ) - { - if ( context->restart == false ) - { - m_camera->SetView( 0.0f, 30.0f, 20.0f, b3Pos_zero ); - GetGuiDraw()->forceScale = 0.1f; - } - - m_data = CreateMeshDrop( m_worldId, b3Pos_zero ); - m_failed = false; - } - - ~MeshDropUnitTest() override - { - DestroyMeshDrop( &m_data ); - } - - void Step() override - { - Sample::Step(); - - if ( m_failed == false ) - { - if ( m_stepCount >= 300 ) - { - b3BodyEvents bodyEvents = b3World_GetBodyEvents( m_worldId ); - if ( bodyEvents.moveCount > 0 ) - { - m_failed = true; - } - } - } - - if ( m_failed == true ) - { - DrawTextLine( "failed!" ); - } - } - - static Sample* Create( SampleContext* context ) - { - return new MeshDropUnitTest( context ); - } - - MeshDropData m_data; - bool m_failed; -}; - -static int sampleMeshDropUnitTest = RegisterSample( "Continuous", "Mesh Drop Unit Test", MeshDropUnitTest::Create ); - // This sample shows that clustering based on the manifold normal can lead to clipping and/or tunneling. class HumpMesh : public Sample { diff --git a/samples/sample_determinism.cpp b/samples/sample_determinism.cpp index 98a850c..7720f99 100644 --- a/samples/sample_determinism.cpp +++ b/samples/sample_determinism.cpp @@ -2,7 +2,10 @@ // SPDX-License-Identifier: MIT #include "determinism.h" +#include "gfx/debug_adapter.h" +#include "gfx/draw.h" #include "sample.h" +#include "stability.h" #include "box3d/box3d.h" @@ -57,3 +60,207 @@ public: }; static int sampleFallingRagdolls = RegisterSample( "Determinism", "Falling Ragdolls", FallingRagdolls::Create ); + +class WavePile : public Sample +{ +public: + explicit WavePile( SampleContext* context ) + : Sample( context ) + { + if ( context->restart == false ) + { + m_camera->SetView( 45.0f, 25.0f, 25.0f, b3Pos_zero ); + } + + m_data = CreateWavePile( m_worldId ); + m_done = false; + } + + ~WavePile() override + { + DestroyWavePile( &m_data ); + } + + void Step() override + { + Sample::Step(); + + if ( m_done == false ) + { + // Only advance the scenario on real steps, else pausing would skew the step count + if ( m_didStep ) + { + m_done = UpdateWavePile( m_worldId, &m_data ); + if ( m_done ) + { + printf( "sleep step = %d, hash = 0x%08X\n", m_data.sleepStep, m_data.hash ); + } + } + } + else + { + DrawTextLine( "sleep step = %d, hash = 0x%08X", m_data.sleepStep, m_data.hash ); + } + } + + static Sample* Create( SampleContext* context ) + { + return new WavePile( context ); + } + + WavePileData m_data; + bool m_done; +}; + +static int sampleWavePile = RegisterSample( "Determinism", "Wave Pile", WavePile::Create ); + +class QuerySpawn : public Sample +{ +public: + explicit QuerySpawn( SampleContext* context ) + : Sample( context ) + { + if ( context->restart == false ) + { + m_camera->SetView( 45.0f, 25.0f, 30.0f, b3Pos_zero ); + } + + m_data = CreateQuerySpawn( m_worldId ); + m_frameCount = 0; + m_done = false; + } + + ~QuerySpawn() override + { + DestroyQuerySpawn( &m_data ); + } + + void Step() override + { + Sample::Step(); + + if ( m_done == false ) + { + // Advance every 10th step so each query lingers on screen. This diverges from + // the unit test cadence, so the hashes differ from the pinned constants. + if ( m_didStep ) + { + m_frameCount += 1; + if ( m_frameCount % 10 == 1 ) + { + m_done = UpdateQuerySpawn( m_worldId, &m_data ); + if ( m_done ) + { + printf( "sleep step = %d, hash = 0x%08X, hits = %d, query hash = 0x%08X\n", m_data.sleepStep, + m_data.hash, m_data.queryHitCount, m_data.queryHash ); + } + } + } + + DrawTextLine( "spawned = %d of %d, query hits = %d", m_data.spawnCount, QUERY_SPAWN_COUNT, m_data.queryHitCount ); + DrawTextLine( "ray = yellow, overlap box = magenta, sphere cast = cyan, spawn = white" ); + } + else + { + DrawTextLine( "sleep step = %d, hash = 0x%08X", m_data.sleepStep, m_data.hash ); + DrawTextLine( "query hits = %d, query hash = 0x%08X", m_data.queryHitCount, m_data.queryHash ); + } + } + + void Render() override + { + Sample::Render(); + + if ( m_data.spawnCount == 0 ) + { + return; + } + + // Ray with hit point and normal, red end point on a miss + DrawLine( m_data.rayOrigin, m_data.rayPoint, MakeColor( b3_colorYellow ) ); + DrawPoint( m_data.rayOrigin, 5.0f, MakeColor( b3_colorYellow ) ); + if ( m_data.rayDidHit ) + { + DrawPoint( m_data.rayPoint, 8.0f, MakeColor( b3_colorGreen ) ); + DrawLine( m_data.rayPoint, b3OffsetPos( m_data.rayPoint, m_data.rayNormal ), MakeColor( b3_colorGreen ) ); + } + else + { + DrawPoint( m_data.rayPoint, 5.0f, MakeColor( b3_colorRed ) ); + } + + DrawBounds( m_data.overlapBounds, 0.0f, MakeColor( b3_colorMagenta ) ); + + // Swept sphere stopped at its cast fraction, gray when it reached full length + b3Pos castCenter = b3OffsetPos( m_data.rayOrigin, b3MulSV( m_data.castFraction, m_data.rayTranslation ) ); + b3Sphere castSphere = { b3Vec3_zero, QUERY_SPAWN_CAST_RADIUS }; + Vec4 castColor = m_data.castFraction < 1.0f ? MakeColor( b3_colorCyan ) : MakeColor( b3_colorGray ); + DrawWireSphere( { castCenter, b3Quat_identity }, &castSphere, 24, castColor ); + + DrawCross( m_data.lastSpawnPosition, 0.5f, MakeColor( b3_colorWhite ) ); + } + + static Sample* Create( SampleContext* context ) + { + return new QuerySpawn( context ); + } + + QuerySpawnData m_data; + int m_frameCount; + bool m_done; +}; + +static int sampleQuerySpawn = RegisterSample( "Determinism", "Query Spawn", QuerySpawn::Create ); + +class MeshDropDeterminism : public Sample +{ +public: + explicit MeshDropDeterminism( SampleContext* context ) + : Sample( context ) + { + if ( context->restart == false ) + { + m_camera->SetView( 0.0f, 30.0f, 20.0f, b3Pos_zero ); + GetGuiDraw()->forceScale = 0.1f; + } + + m_data = CreateMeshDrop( m_worldId, b3Pos_zero ); + m_done = false; + } + + ~MeshDropDeterminism() override + { + DestroyMeshDrop( &m_data ); + } + + void Step() override + { + Sample::Step(); + + if ( m_done == false ) + { + if ( m_didStep ) + { + m_done = UpdateMeshDrop( m_worldId, &m_data ); + if ( m_done ) + { + printf( "sleep step = %d, hash = 0x%08X\n", m_data.sleepStep, m_data.hash ); + } + } + } + else + { + DrawTextLine( "sleep step = %d, hash = 0x%08X", m_data.sleepStep, m_data.hash ); + } + } + + static Sample* Create( SampleContext* context ) + { + return new MeshDropDeterminism( context ); + } + + MeshDropData m_data; + bool m_done; +}; + +static int sampleMeshDropDeterminism = RegisterSample( "Determinism", "Mesh Drop", MeshDropDeterminism::Create ); diff --git a/samples/sample_issues.cpp b/samples/sample_issues.cpp index ec6edbd..d0892c6 100644 --- a/samples/sample_issues.cpp +++ b/samples/sample_issues.cpp @@ -10,45 +10,6 @@ #include -class DumpLoader : public Sample -{ -public: - explicit DumpLoader( SampleContext* context ) - : Sample( context ) - { - if ( context->restart == false ) - { - m_camera->SetView( 45.0f, 30.0f, 15.0f, { 0.0f, 2.0f, 0.0f } ); - // m_camera->SetView( 45.0f, 30.0f, 300.0f, { 3910.62109f, 9862.50293f, 875.395081f } ); - } - - b3SetLengthUnitsPerMeter( 1.0f ); - - const char* dumpPrefix = "data/dumps/single_box/"; - -#include "dumps/single_box/box3d_dump.inl" - } - - ~DumpLoader() override - { - for ( b3MeshData* md : m_meshes ) - { - b3DestroyMesh( md ); - } - - b3SetLengthUnitsPerMeter( 1.0f ); - } - - static Sample* Create( SampleContext* context ) - { - return new DumpLoader( context ); - } - - std::vector m_meshes; -}; - -static int sampleDumpLoader = RegisterSample( "Issues", "Dump Loader", DumpLoader::Create ); - class Crash : public Sample { public: diff --git a/samples/sample_shapes.cpp b/samples/sample_shapes.cpp index 564c9d9..8d07830 100644 --- a/samples/sample_shapes.cpp +++ b/samples/sample_shapes.cpp @@ -36,7 +36,7 @@ public: bodyDef.type = b3_dynamicBody; for ( int index = 0; index < m_boxCount; ++index ) { - bodyDef.position = { -10.0f + 5.0f * index, 15.75f, -10.6f }; + bodyDef.position = { -10.0f + 5.0f * index, 14.25f, -10.6f }; b3BodyId boxBody = b3CreateBody( m_worldId, &bodyDef ); shapeDef.baseMaterial.friction = ( index + 1 ) * ( index + 1 ) * 0.04f; b3CreateHullShape( boxBody, &shapeDef, &box.base ); diff --git a/samples/sample_stacking.cpp b/samples/sample_stacking.cpp index bb48ed9..6462d35 100644 --- a/samples/sample_stacking.cpp +++ b/samples/sample_stacking.cpp @@ -11,84 +11,6 @@ #include -class CardHouseThick : public Sample -{ -public: - explicit CardHouseThick( SampleContext* context ) - : Sample( context ) - { - if ( context->restart == false ) - { - m_camera->SetView( 0.0f, 25.0f, 10.0f, { 0.0f, 2.0f, 0.0f } ); - } - - AddGroundBox( 10.0f ); - - const float alpha = 25.0f * B3_DEG_TO_RAD; - const float width = 0.38f; - const float height = 0.98f; - const float depth = 0.08f; - - float offsetX = 0.5f * height * b3Sin( alpha ) + 0.045f; - float offsetY = 0.5f * height * b3Cos( alpha ) + 0.035f; - - b3BoxHull box = b3MakeBoxHull( 0.5f * depth, 0.5f * height, 0.5f * width ); - AddVerticalRow( 4, -6.0f * offsetX, offsetX, offsetY, alpha, box ); - AddHorizontalRow( 3, -4.0f * offsetX, 4.0f * offsetX, 2.0f * offsetY + 0.04f, box ); - AddVerticalRow( 3, -4.0f * offsetX, offsetX, 3.0f * offsetY + 0.08f, alpha, box ); - AddHorizontalRow( 2, -2.0f * offsetX, 4.0f * offsetX, 4.0f * offsetY + 0.12f, box ); - AddVerticalRow( 2, -2.0f * offsetX, offsetX, 5.0f * offsetY + 0.16f, alpha, box ); - AddHorizontalRow( 1, -0.0f * offsetX, 4.0f * offsetX, 6.0f * offsetY + 0.20f, box ); - AddVerticalRow( 1, -0.0f * offsetX, offsetX, 7.0f * offsetY + 0.24f, alpha, box ); - } - - void AddVerticalRow( int n, float startX, float offsetX, float startY, float alpha, const b3BoxHull& box ) - { - b3BodyDef bodyDef = b3DefaultBodyDef(); - bodyDef.type = b3_dynamicBody; - b3ShapeDef shapeDef = b3DefaultShapeDef(); - shapeDef.baseMaterial.friction = 0.8f; - - for ( int index = 0; index < n; ++index ) - { - bodyDef.position = { startX - offsetX, startY, 0.0f }; - bodyDef.rotation = b3MakeQuatFromAxisAngle( b3Vec3_axisZ, -alpha ); - b3BodyId body1 = b3CreateBody( m_worldId, &bodyDef ); - b3CreateHullShape( body1, &shapeDef, &box.base ); - - bodyDef.position = { startX + offsetX, startY, 0.0f }; - bodyDef.rotation = b3MakeQuatFromAxisAngle( b3Vec3_axisZ, alpha ); - b3BodyId body2 = b3CreateBody( m_worldId, &bodyDef ); - b3CreateHullShape( body2, &shapeDef, &box.base ); - - startX += 4.0f * offsetX; - } - } - - void AddHorizontalRow( int n, float startX, float offsetX, float startY, const b3BoxHull& box ) - { - b3BodyDef bodyDef = b3DefaultBodyDef(); - bodyDef.type = b3_dynamicBody; - b3ShapeDef shapeDef = b3DefaultShapeDef(); - shapeDef.baseMaterial.friction = 0.8f; - - for ( int index = 0; index < n; ++index ) - { - bodyDef.position = { startX + index * offsetX, startY, 0.0f }; - bodyDef.rotation = b3MakeQuatFromAxisAngle( b3Vec3_axisZ, 0.5f * B3_PI ); - b3BodyId body = b3CreateBody( m_worldId, &bodyDef ); - b3CreateHullShape( body, &shapeDef, &box.base ); - } - } - - static Sample* Create( SampleContext* context ) - { - return new CardHouseThick( context ); - } -}; - -static int sampleCardHouseThick = RegisterSample( "Stacking", "Card House Thick", CardHouseThick::Create ); - // From PEEL class CardHouse : public Sample { @@ -104,6 +26,7 @@ public: AddGroundBox( 10.0f ); b3ShapeDef shapeDef = b3DefaultShapeDef(); + shapeDef.baseMaterial.rollingResistance = 0.05f; shapeDef.baseMaterial.friction = 0.7f; float cardHeight = 0.2f; @@ -287,7 +210,7 @@ public: bodyDef.name = "cube"; bodyDef.type = b3_dynamicBody; bodyDef.position = { 0.0f, 0.5f, 0.0f }; - bodyDef.angularVelocity = { 0.0f, 10.0f, 0.0f }; + //bodyDef.angularVelocity = { 0.0f, 10.0f, 0.0f }; m_bodyId = b3CreateBody( m_worldId, &bodyDef ); b3ShapeDef shapeDef = b3DefaultShapeDef(); @@ -430,7 +353,7 @@ public: { if ( context->restart == false ) { - m_camera->SetView( 0.0f, 15.0f, 50.0f, { 0.0f, 20.0f, 0.0f } ); + m_camera->SetView( 40.0f, 15.0f, 50.0f, { 0.0f, 20.0f, 0.0f } ); } AddGroundBox( 40.0f ); @@ -441,7 +364,6 @@ public: bodyDef.type = b3_dynamicBody; b3BoxHull cube = b3MakeBoxHull( a, a, a ); - // b3Quat q = b3MakeQuatFromAxisAngle( { 1.0f, 0.0f, 0.0f }, 0.5f * B3_PI ); for ( int i = 0; i < 40; ++i ) { diff --git a/shared/benchmarks.c b/shared/benchmarks.c index a04cf9a..6975f7c 100644 --- a/shared/benchmarks.c +++ b/shared/benchmarks.c @@ -869,6 +869,22 @@ void CreateJunkyard( b3WorldId worldId ) b3DestroyHull( hull ); } +void GetJunkyardCapacity(b3Capacity* capacity) +{ +#ifdef NDEBUG + capacity->staticShapeCount = 16; + capacity->dynamicShapeCount = 20 * 20 * 24 + 1; + capacity->staticBodyCount = 16; + capacity->dynamicBodyCount = 20 * 20 * 24 + 1; + capacity->contactCount = 250 * 1024; +#else + capacity->staticShapeCount = 16; + capacity->dynamicShapeCount = 20 * 20 * 2 + 1; + capacity->staticBodyCount = 16; + capacity->dynamicBodyCount = 20 * 20 * 2 + 1; +#endif +} + void StepJunkyard( b3WorldId worldId, int stepCount ) { (void)worldId; @@ -887,6 +903,15 @@ void StepJunkyard( b3WorldId worldId, int stepCount ) // Huge pile of large convexes, ported from PEEL. Each convex is the hull of 32 random points on a // sphere. A fixed LCG seed makes the hull identical across runs so results compare directly. +void GetConvexPileCapacity( b3Capacity* capacity ) +{ +#ifdef NDEBUG + capacity->dynamicShapeCount = 5120; + capacity->dynamicBodyCount = 5120; + capacity->contactCount = 50 * 1024; +#endif +} + // PEEL's BasicRandom, kept verbatim so the point set matches the original typedef struct ConvexPileRandom { diff --git a/shared/benchmarks.h b/shared/benchmarks.h index 2dcd396..eb6c055 100644 --- a/shared/benchmarks.h +++ b/shared/benchmarks.h @@ -26,6 +26,7 @@ void CreateTrees100( b3WorldId worldId ); void DestroyTrees( void ); void CreateJointGrid( b3WorldId worldId ); void CreateJunkyard( b3WorldId worldId ); +void GetJunkyardCapacity( b3Capacity* capacity ); void StepJunkyard( b3WorldId worldId, int stepCount ); void CreateLargePyramid( b3WorldId worldId ); void CreateWidePyramid( b3WorldId worldId ); @@ -40,6 +41,7 @@ void StepLargeWorld( b3WorldId worldId, int stepCount ); void GetWasherCapacity( b3Capacity* capacity ); void CreateWasher( b3WorldId worldId ); void CreateConvexPile( b3WorldId worldId ); +void GetConvexPileCapacity( b3Capacity* capacity ); // void CreateSpinner( b3WorldId worldId ); // float StepSpinner( b3WorldId worldId, int stepCount ); diff --git a/shared/determinism.c b/shared/determinism.c index 0034afb..e3121bb 100644 --- a/shared/determinism.c +++ b/shared/determinism.c @@ -4,6 +4,7 @@ #include "determinism.h" #include "human.h" +#include "utils.h" #include "box3d/box3d.h" @@ -122,3 +123,276 @@ void DestroyFallingRagdolls( FallingRagdollData* data ) data->gridMesh = NULL; data->torusMesh = NULL; } + +#define WAVE_PILE_GRID 5 +#define WAVE_PILE_LAYERS 4 + +WavePileData CreateWavePile( b3WorldId worldId ) +{ + WavePileData data = { 0 }; + + g_randomSeed = 52977; + + // Height fields grow from a corner, offset the body to center the patch. + int fieldCount = 21; + b3Vec3 fieldScale = { 1.0f, 0.6f, 1.0f }; + data.heightField = b3CreateWave( fieldCount, fieldCount, fieldScale, 0.08f, 0.06f, false ); + + { + float extent = fieldScale.x * ( fieldCount - 1 ); + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.position.x = -0.5f * extent; + bodyDef.position.z = -0.5f * extent; + b3BodyId groundId = b3CreateBody( worldId, &bodyDef ); + + b3ShapeDef shapeDef = b3DefaultShapeDef(); + b3CreateHeightFieldShape( groundId, &shapeDef, data.heightField ); + } + + b3HullData* rock = b3CreateRock( 0.55f ); + b3BoxHull box = b3MakeBoxHull( 0.45f, 0.3f, 0.55f ); + b3Sphere sphere = { b3Vec3_zero, 0.5f }; + b3Capsule capsule = { { 0.0f, -0.3f, 0.0f }, { 0.0f, 0.3f, 0.0f }, 0.35f }; + + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.type = b3_dynamicBody; + + // Rolling resistance so the pile sleeps quickly. + b3ShapeDef shapeDef = b3DefaultShapeDef(); + shapeDef.baseMaterial.rollingResistance = 0.3f; + + float spacing = 1.7f; + int index = 0; + for ( int layer = 0; layer < WAVE_PILE_LAYERS; ++layer ) + { + for ( int i = 0; i < WAVE_PILE_GRID; ++i ) + { + for ( int j = 0; j < WAVE_PILE_GRID; ++j ) + { + b3Vec3 jitter = RandomVec3Uniform( -0.3f, 0.3f ); + bodyDef.position.x = spacing * ( i - 0.5f * ( WAVE_PILE_GRID - 1 ) ) + jitter.x; + bodyDef.position.y = 2.5f + 1.6f * layer + 0.3f * jitter.y; + bodyDef.position.z = spacing * ( j - 0.5f * ( WAVE_PILE_GRID - 1 ) ) + jitter.z; + bodyDef.rotation = RandomQuat(); + + b3BodyId bodyId = b3CreateBody( worldId, &bodyDef ); + data.bodies[index] = bodyId; + + switch ( index % 4 ) + { + case 0: + b3CreateSphereShape( bodyId, &shapeDef, &sphere ); + break; + case 1: + b3CreateCapsuleShape( bodyId, &shapeDef, &capsule ); + break; + case 2: + b3CreateHullShape( bodyId, &shapeDef, &box.base ); + break; + default: + b3CreateHullShape( bodyId, &shapeDef, rock ); + break; + } + + index += 1; + } + } + } + + assert( index == WAVE_PILE_BODY_COUNT ); + + // The world keeps its own copy. + b3DestroyHull( rock ); + + return data; +} + +bool UpdateWavePile( b3WorldId worldId, WavePileData* data ) +{ + if ( data->hash == 0 ) + { + if ( b3World_GetAwakeBodyCount( worldId ) == 0 ) + { + data->hash = B3_HASH_INIT; + for ( int i = 0; i < WAVE_PILE_BODY_COUNT; ++i ) + { + b3WorldTransform xf = b3Body_GetTransform( data->bodies[i] ); + data->hash = b3Hash( data->hash, (uint8_t*)&xf, sizeof( b3WorldTransform ) ); + } + + data->sleepStep = data->stepCount; + } + } + + data->stepCount += 1; + + return data->hash != 0; +} + +void DestroyWavePile( WavePileData* data ) +{ + b3DestroyHeightField( data->heightField ); + data->heightField = NULL; +} + +typedef struct QuerySpawnOverlapContext +{ + QuerySpawnData* data; + int count; +} QuerySpawnOverlapContext; + +static bool QuerySpawnOverlapCallback( b3ShapeId shapeId, void* context ) +{ + QuerySpawnOverlapContext* overlap = context; + overlap->count += 1; + overlap->data->queryHitCount += 1; + overlap->data->queryHash = b3Hash( overlap->data->queryHash, (uint8_t*)&shapeId.index1, sizeof( shapeId.index1 ) ); + return true; +} + +static float QuerySpawnCastCallback( b3ShapeId shapeId, b3Pos point, b3Vec3 normal, float fraction, uint64_t userMaterialId, + int triangleIndex, int childIndex, void* context ) +{ + (void)shapeId, (void)point, (void)normal, (void)userMaterialId, (void)triangleIndex, (void)childIndex; + float* closest = context; + *closest = fraction; + return fraction; +} + +static void QuerySpawnOnce( b3WorldId worldId, QuerySpawnData* data ) +{ + b3QueryFilter filter = b3DefaultQueryFilter(); + + // Ray result decides the spawn position, a miss seeds the cloud near the origin. + b3Pos rayOrigin = RandomPos( (b3Vec3){ -12.0f, -12.0f, -12.0f }, (b3Vec3){ 12.0f, 12.0f, 12.0f } ); + b3Vec3 rayTranslation = b3MulSV( 30.0f, RandomUnitVector() ); + + b3Pos spawnPosition; + b3RayResult ray = b3World_CastRayClosest( worldId, rayOrigin, rayTranslation, filter ); + if ( ray.hit ) + { + data->queryHitCount += 1; + data->queryHash = b3Hash( data->queryHash, (uint8_t*)&ray.fraction, sizeof( ray.fraction ) ); + data->queryHash = b3Hash( data->queryHash, (uint8_t*)&ray.normal, sizeof( ray.normal ) ); + spawnPosition = b3OffsetPos( ray.point, b3MulSV( 1.2f, ray.normal ) ); + } + else + { + spawnPosition = RandomPos( (b3Vec3){ -6.0f, -6.0f, -6.0f }, (b3Vec3){ 6.0f, 6.0f, 6.0f } ); + } + + data->rayOrigin = rayOrigin; + data->rayTranslation = rayTranslation; + data->rayDidHit = ray.hit; + data->rayPoint = ray.hit ? ray.point : b3OffsetPos( rayOrigin, rayTranslation ); + data->rayNormal = ray.hit ? ray.normal : b3Vec3_zero; + + // Overlap count picks the shape type. + b3Vec3 center = RandomVec3Uniform( -10.0f, 10.0f ); + float extent = RandomFloatRange( 1.0f, 4.0f ); + b3AABB aabb; + aabb.lowerBound = (b3Vec3){ center.x - extent, center.y - extent, center.z - extent }; + aabb.upperBound = (b3Vec3){ center.x + extent, center.y + extent, center.z + extent }; + + QuerySpawnOverlapContext overlap = { data, 0 }; + b3World_OverlapAABB( worldId, aabb, filter, QuerySpawnOverlapCallback, &overlap ); + + data->overlapBounds = aabb; + data->overlapCount = overlap.count; + + // Sphere cast fraction sets the spawn size. + float fraction = 1.0f; + b3Vec3 proxyPoint = b3Vec3_zero; + b3ShapeProxy proxy = { &proxyPoint, 1, QUERY_SPAWN_CAST_RADIUS }; + b3World_CastShape( worldId, rayOrigin, &proxy, rayTranslation, filter, QuerySpawnCastCallback, &fraction ); + if ( fraction < 1.0f ) + { + data->queryHitCount += 1; + data->queryHash = b3Hash( data->queryHash, (uint8_t*)&fraction, sizeof( fraction ) ); + } + + data->castFraction = fraction; + + float size = 0.3f + 0.2f * fraction; + + // Damping guarantees everything comes to rest. + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.type = b3_dynamicBody; + bodyDef.position = spawnPosition; + bodyDef.rotation = RandomQuat(); + bodyDef.linearVelocity = RandomVec3Uniform( -0.2f, 0.2f ); + bodyDef.angularVelocity = RandomVec3Uniform( -0.5f, 0.5f ); + bodyDef.linearDamping = 1.0f; + bodyDef.angularDamping = 1.0f; + + b3BodyId bodyId = b3CreateBody( worldId, &bodyDef ); + + b3ShapeDef shapeDef = b3DefaultShapeDef(); + shapeDef.baseMaterial.rollingResistance = 0.2f; + + switch ( ( data->spawnCount + overlap.count ) % 3 ) + { + case 0: + { + b3Sphere sphere = { b3Vec3_zero, size }; + b3CreateSphereShape( bodyId, &shapeDef, &sphere ); + break; + } + case 1: + { + b3Capsule capsule = { { 0.0f, -size, 0.0f }, { 0.0f, size, 0.0f }, 0.7f * size }; + b3CreateCapsuleShape( bodyId, &shapeDef, &capsule ); + break; + } + default: + { + b3BoxHull box = b3MakeBoxHull( size, 0.7f * size, 0.5f * size ); + b3CreateHullShape( bodyId, &shapeDef, &box.base ); + break; + } + } + + data->bodies[data->spawnCount] = bodyId; + data->spawnCount += 1; + data->lastSpawnPosition = spawnPosition; +} + +QuerySpawnData CreateQuerySpawn( b3WorldId worldId ) +{ + QuerySpawnData data = { 0 }; + + g_randomSeed = 71689; + + // Empty space, motion comes only from spawn velocities and overlap pushes. + b3World_SetGravity( worldId, b3Vec3_zero ); + + return data; +} + +bool UpdateQuerySpawn( b3WorldId worldId, QuerySpawnData* data ) +{ + if ( data->spawnCount < QUERY_SPAWN_COUNT ) + { + QuerySpawnOnce( worldId, data ); + } + else if ( data->hash == 0 && b3World_GetAwakeBodyCount( worldId ) == 0 ) + { + data->hash = B3_HASH_INIT; + for ( int i = 0; i < QUERY_SPAWN_COUNT; ++i ) + { + b3WorldTransform xf = b3Body_GetTransform( data->bodies[i] ); + data->hash = b3Hash( data->hash, (uint8_t*)&xf, sizeof( b3WorldTransform ) ); + } + + data->sleepStep = data->stepCount; + } + + data->stepCount += 1; + + return data->hash != 0; +} + +void DestroyQuerySpawn( QuerySpawnData* data ) +{ + (void)data; +} diff --git a/shared/determinism.h b/shared/determinism.h index 0839fbf..78925a1 100644 --- a/shared/determinism.h +++ b/shared/determinism.h @@ -36,6 +36,55 @@ FallingRagdollData CreateFallingRagdolls( b3WorldId worldId ); bool UpdateFallingRagdolls( b3WorldId worldId, FallingRagdollData* data ); void DestroyFallingRagdolls( FallingRagdollData* data ); +// Convex pile dropped on a wave height field. Spheres, capsules, boxes, and rocks with +// rolling resistance so the pile sleeps quickly. +#define WAVE_PILE_BODY_COUNT 100 + +typedef struct WavePileData +{ + b3BodyId bodies[WAVE_PILE_BODY_COUNT]; + b3HeightFieldData* heightField; + int stepCount; + int sleepStep; + uint32_t hash; +} WavePileData; + +WavePileData CreateWavePile( b3WorldId worldId ); +bool UpdateWavePile( b3WorldId worldId, WavePileData* data ); +void DestroyWavePile( WavePileData* data ); + +// Query driven spawning in an empty zero gravity world. Each step casts a ray, overlaps an +// AABB, and casts a sphere, then spawns a shape whose position, type, and size depend on the +// query results. Any query divergence cascades into the final state. +#define QUERY_SPAWN_COUNT 50 +#define QUERY_SPAWN_CAST_RADIUS 0.5f + +typedef struct QuerySpawnData +{ + b3BodyId bodies[QUERY_SPAWN_COUNT]; + int spawnCount; + int queryHitCount; + uint32_t queryHash; + int stepCount; + int sleepStep; + uint32_t hash; + + // Last query cycle, recorded for visualization + b3Pos rayOrigin; + b3Vec3 rayTranslation; + b3Pos rayPoint; + b3Vec3 rayNormal; + bool rayDidHit; + b3AABB overlapBounds; + int overlapCount; + float castFraction; + b3Pos lastSpawnPosition; +} QuerySpawnData; + +QuerySpawnData CreateQuerySpawn( b3WorldId worldId ); +bool UpdateQuerySpawn( b3WorldId worldId, QuerySpawnData* data ); +void DestroyQuerySpawn( QuerySpawnData* data ); + #ifdef __cplusplus } #endif diff --git a/shared/stability.c b/shared/stability.c index 395c93e..404f911 100644 --- a/shared/stability.c +++ b/shared/stability.c @@ -10,8 +10,6 @@ #include #include -#define MESH_DROP_GRID_COUNT 32 - MeshDropData CreateMeshDrop( b3WorldId worldId, b3Pos origin ) { MeshDropData data = { 0 }; @@ -61,6 +59,7 @@ MeshDropData CreateMeshDrop( b3WorldId worldId, b3Pos origin ) bodyDef.linearVelocity = linearVelocity; bodyDef.angularVelocity = angularVelocity; b3BodyId bodyId = b3CreateBody( worldId, &bodyDef ); + data.bodies[i * gridCount + j] = bodyId; b3CreateHullShape( bodyId, &shapeDef, &box.base ); } @@ -70,6 +69,29 @@ MeshDropData CreateMeshDrop( b3WorldId worldId, b3Pos origin ) return data; } +bool UpdateMeshDrop( b3WorldId worldId, MeshDropData* data ) +{ + if ( data->hash == 0 ) + { + if ( b3World_GetAwakeBodyCount( worldId ) == 0 ) + { + data->hash = B3_HASH_INIT; + int bodyCount = MESH_DROP_GRID_COUNT * MESH_DROP_GRID_COUNT; + for ( int i = 0; i < bodyCount; ++i ) + { + b3WorldTransform xf = b3Body_GetTransform( data->bodies[i] ); + data->hash = b3Hash( data->hash, (uint8_t*)&xf, sizeof( b3WorldTransform ) ); + } + + data->sleepStep = data->stepCount; + } + } + + data->stepCount += 1; + + return data->hash != 0; +} + void DestroyMeshDrop( MeshDropData* data ) { b3DestroyMesh( data->mesh ); diff --git a/shared/stability.h b/shared/stability.h index cc5e5e5..f8e105c 100644 --- a/shared/stability.h +++ b/shared/stability.h @@ -12,12 +12,21 @@ extern "C" { #endif +#define MESH_DROP_GRID_COUNT 20 + +// Thin fast boxes dropped on a wave mesh. Stresses continuous collision and mesh contact +// stability, and doubles as a determinism scenario via the sleep hash. typedef struct MeshDropData { struct b3MeshData* mesh; + b3BodyId bodies[MESH_DROP_GRID_COUNT * MESH_DROP_GRID_COUNT]; + int stepCount; + int sleepStep; + uint32_t hash; } MeshDropData; MeshDropData CreateMeshDrop( b3WorldId worldId, b3Pos origin ); +bool UpdateMeshDrop( b3WorldId worldId, MeshDropData* data ); void DestroyMeshDrop( MeshDropData* data ); #ifdef __cplusplus diff --git a/shared/utils.h b/shared/utils.h index f376614..5997cbc 100644 --- a/shared/utils.h +++ b/shared/utils.h @@ -102,10 +102,13 @@ B3_INLINE b3Vec3 RandomUnitVector() float sqrt1MinusU1 = sqrtf( 1.0f - u1 ); float sqrtU1 = sqrtf( u1 ); + b3CosSin cs2 = b3ComputeCosSin( u2 ); + b3CosSin cs3 = b3ComputeCosSin( u3 ); + b3Vec3 v; - v.x = sqrt1MinusU1 * sinf( u2 ); - v.y = sqrt1MinusU1 * cosf( u2 ); - v.z = sqrtU1 * sinf( u3 ); + v.x = sqrt1MinusU1 * cs2.sine; + v.y = sqrt1MinusU1 * cs2.cosine; + v.z = sqrtU1 * cs3.sine; return v; } @@ -122,11 +125,14 @@ B3_INLINE b3Quat RandomQuat() float sqrt1MinusU1 = sqrtf( 1.0f - u1 ); float sqrtU1 = sqrtf( u1 ); + b3CosSin cs2 = b3ComputeCosSin( u2 ); + b3CosSin cs3 = b3ComputeCosSin( u3 ); + b3Quat q; - q.v.x = sqrt1MinusU1 * sinf( u2 ); - q.v.y = sqrt1MinusU1 * cosf( u2 ); - q.v.z = sqrtU1 * sinf( u3 ); - q.s = sqrtU1 * cosf( u3 ); + q.v.x = sqrt1MinusU1 * cs2.sine; + q.v.y = sqrt1MinusU1 * cs2.cosine; + q.v.z = sqrtU1 * cs3.sine; + q.s = sqrtU1 * cs3.cosine; return q; } diff --git a/src/block_allocator.c b/src/block_allocator.c index 4c1f154..ebc6d79 100644 --- a/src/block_allocator.c +++ b/src/block_allocator.c @@ -9,6 +9,10 @@ b3BlockAllocator b3CreateBlockAllocator( int elementSize, int initialCount ) { B3_ASSERT( elementSize >= (int)sizeof( void* ) ); + // The free list stores a pointer inside each element, so the stride must keep every element aligned. + // Blocks come from b3Alloc, so a stride that is a multiple of B3_ALIGNMENT aligns all elements. + elementSize = ( ( elementSize - 1 ) | ( B3_ALIGNMENT - 1 ) ) + 1; + b3BlockAllocator allocator = { 0 }; b3Array_Create( allocator.blocks ); @@ -81,6 +85,7 @@ void b3FreeElement( b3BlockAllocator* allocator, void* element ) B3_ASSERT( allocator != NULL ); B3_ASSERT( element != NULL ); B3_ASSERT( allocator->allocationCount > 0 ); + B3_ASSERT( ( (uintptr_t)element & ( B3_ALIGNMENT - 1 ) ) == 0 ); allocator->allocationCount -= 1; diff --git a/src/block_allocator.h b/src/block_allocator.h index e9922d0..1c62ed8 100644 --- a/src/block_allocator.h +++ b/src/block_allocator.h @@ -26,7 +26,7 @@ typedef struct b3BlockAllocator int allocationCount; } b3BlockAllocator; -// Element must be large enough to hold a pointer +// Element must be large enough to hold a pointer. The element size is rounded up to B3_ALIGNMENT. b3BlockAllocator b3CreateBlockAllocator( int elementSize, int initialCount ); void b3DestroyBlockAllocator( b3BlockAllocator* allocator ); diff --git a/src/body.c b/src/body.c index b78642c..bce16de 100644 --- a/src/body.c +++ b/src/body.c @@ -253,8 +253,6 @@ b3BodyId b3CreateBody( b3WorldId worldId, const b3BodyDef* def ) bodyState->angularVelocity = def->angularVelocity; bodyState->deltaRotation = b3Quat_identity; bodyState->flags = bodySim->flags; - - bodySim->maxAngularVelocity = b3Length( def->angularVelocity ) + 5.0f; } if ( bodyId == world->bodies.count ) @@ -2305,6 +2303,37 @@ bool b3Body_IsBullet( b3BodyId bodyId ) return ( body->flags & b3_isBullet ) != 0; } +void b3Body_AllowFastRotation(b3BodyId bodyId, bool flag) +{ + b3World* world = b3GetUnlockedWorld( bodyId.world0 ); + if ( world == NULL ) + { + return; + } + + B3_REC( world, BodyAllowFastRotation, bodyId, flag ); + + uint32_t newFlag = flag ? b3_allowFastRotation : 0; + + b3Body* body = b3GetBodyFullId( world, bodyId ); + if ( ( body->flags & b3_allowFastRotation ) == newFlag ) + { + return; + } + + body->flags &= ~b3_allowFastRotation; + body->flags |= newFlag; + + b3SyncBodyFlags( world, body ); +} + +bool b3Body_IsFastRotationAllowed(b3BodyId bodyId) +{ + b3World* world = b3GetWorld( bodyId.world0 ); + b3Body* body = b3GetBodyFullId( world, bodyId ); + return ( body->flags & b3_allowFastRotation ) != 0; +} + void b3Body_EnableContactRecycling( b3BodyId bodyId, bool flag ) { b3World* world = b3GetUnlockedWorld( bodyId.world0 ); diff --git a/src/body.h b/src/body.h index 5eebf67..ae90105 100644 --- a/src/body.h +++ b/src/body.h @@ -208,7 +208,6 @@ typedef struct b3BodySim float minExtent; b3Vec3 maxExtent; - float maxAngularVelocity; float linearDamping; float angularDamping; float gravityScale; diff --git a/src/compound.c b/src/compound.c index 29da3c6..1816475 100644 --- a/src/compound.c +++ b/src/compound.c @@ -276,7 +276,7 @@ b3CompoundData* b3CreateCompound( const b3CompoundDef* def ) int sphereCount = def->sphereCount; b3CompoundSphere* sphereInstances = b3AllocZeroed( sphereCount * sizeof( b3CompoundSphere ) ); - // Determine material capacity + // Determine material capacity. int materialCapacity = convexCount; for ( int i = 0; i < def->meshCount; ++i ) { @@ -296,10 +296,10 @@ b3CompoundData* b3CreateCompound( const b3CompoundDef* def ) const b3CompoundCapsuleDef* capsuleDef = def->capsules + i; capsuleInstances[i].capsule = capsuleDef->capsule; - // Look for an existing material + // Look for an existing material. b3MaterialMap_itr materialItr = b3MaterialMap_get_or_insert( &materialMap, &capsuleDef->material, materialCount ); - // Get the shared material index + // Get the shared material index. int materialIndex = materialItr.data->val; capsuleInstances[i].materialIndex = materialIndex; @@ -333,10 +333,10 @@ b3CompoundData* b3CreateCompound( const b3CompoundDef* def ) b3DynamicTree_CreateProxy( &tree, aabb, ~0ull, childIndex ); childIndex += 1; - // Look for an existing material + // Look for an existing material. b3MaterialMap_itr materialItr = b3MaterialMap_get_or_insert( &materialMap, &hullDef->material, materialCount ); - // Get the shared material index + // Get the shared material index. int materialIndex = materialItr.data->val; hullInstances[i].materialIndex = materialIndex; @@ -395,11 +395,11 @@ b3CompoundData* b3CreateCompound( const b3CompoundDef* def ) for ( int j = 0; j < meshDef->materialCount; ++j ) { - // Look for an existing material + // Look for an existing material. b3MaterialMap_itr materialItr = b3MaterialMap_get_or_insert( &materialMap, &meshDef->materials[j], materialCount ); - // Get the shared material index + // Get the shared material index. int materialIndex = materialItr.data->val; meshInstances[i].materialIndices[j] = materialIndex; @@ -411,17 +411,17 @@ b3CompoundData* b3CreateCompound( const b3CompoundDef* def ) } } - // Look for an existing matching mesh + // Look for an existing matching mesh. b3MeshMap_itr itr = b3MeshMap_get_or_insert( &meshMap, meshData, sharedMeshCount ); - // Get the shared mesh index + // Get the shared mesh index. int sharedMeshIndex = itr.data->val; - // Create mesh instance + // Create mesh instance. meshInstances[i].transform = def->meshes[i].transform; meshInstances[i].scale = def->meshes[i].scale; - // The offset isn't known yet, so store the index of the shared mesh + // The offset isn't known yet, so store the index of the shared mesh. meshInstances[i].meshOffset = sharedMeshIndex; // Is this a new mesh? @@ -443,10 +443,10 @@ b3CompoundData* b3CreateCompound( const b3CompoundDef* def ) const b3CompoundSphereDef* sphereDef = def->spheres + i; sphereInstances[i].sphere = sphereDef->sphere; - // Look for an existing material + // Look for an existing material. b3MaterialMap_itr materialItr = b3MaterialMap_get_or_insert( &materialMap, &sphereDef->material, materialCount ); - // Get the shared material index + // Get the shared material index. int materialIndex = materialItr.data->val; sphereInstances[i].materialIndex = materialIndex; @@ -467,58 +467,55 @@ b3CompoundData* b3CreateCompound( const b3CompoundDef* def ) b3DynamicTree_Rebuild( &tree, true ); - int byteCount = sizeof( b3CompoundData ); - - // Tree nodes - todo 64 byte alignment - int nodeOffset = byteCount; - byteCount += tree.nodeCapacity * sizeof( b3TreeNode ); - - int materialOffset = byteCount; - byteCount += materialCount * sizeof( b3SurfaceMaterial ); - - int capsuleOffset = byteCount; - byteCount += def->capsuleCount * sizeof( b3CompoundCapsule ); + // Tree nodes + size_t byteCount = b3AlignUp8( sizeof( b3CompoundData ) ); + int nodeOffset = (int)byteCount; + byteCount += b3AlignUp8( tree.nodeCapacity * sizeof( b3TreeNode ) ); + int materialOffset = (int)byteCount; + byteCount += b3AlignUp8( materialCount * sizeof( b3SurfaceMaterial ) ); + int capsuleOffset = (int)byteCount; + byteCount += b3AlignUp8( def->capsuleCount * sizeof( b3CompoundCapsule ) ); // Hull data layout has another level of indirection to allow for tight data packing // 1. hull instance array : hull count array of b3HullInstance with individual hull transforms and offsets // 2. heterogeneous array of shared hull data : each shared hull can have a different byte count, so direct indexing is not // possible - int hullArrayOffset = byteCount; + int hullArrayOffset = (int)byteCount; // Array of hull instances - byteCount += hullCount * sizeof( b3HullInstance ); + byteCount += b3AlignUp8( hullCount * sizeof( b3HullInstance ) ); // Packed shared hull blobs for ( int i = 0; i < sharedHullCount; ++i ) { - sharedHulls[i].hullOffset = byteCount; - byteCount += sharedHulls[i].hull->byteCount; + sharedHulls[i].hullOffset = (int)byteCount; + byteCount += b3AlignUp8( sharedHulls[i].hull->byteCount ); } // Mesh data layout has another level of indirection to allow for tight data packing // 1. mesh instance array : mesh count array of b3MeshInstance with individual mesh transform, scale, and offset // 2. heterogeneous array of shared mesh data : each shared mesh can have a different byte count, so direct indexing is not // possible - int meshArrayOffset = byteCount; + int meshArrayOffset = (int)byteCount; // Array of mesh instances - byteCount += meshCount * sizeof( b3MeshInstance ); + byteCount += b3AlignUp8( meshCount * sizeof( b3MeshInstance ) ); // Packed shared mesh blobs for ( int i = 0; i < sharedMeshCount; ++i ) { - sharedMeshes[i].meshOffset = byteCount; - byteCount += sharedMeshes[i].meshData->byteCount; + sharedMeshes[i].meshOffset = (int)byteCount; + byteCount += b3AlignUp8( sharedMeshes[i].meshData->byteCount ); } - int sphereOffset = byteCount; - byteCount += def->sphereCount * sizeof( b3CompoundSphere ); + int sphereOffset = (int)byteCount; + byteCount += b3AlignUp8( def->sphereCount * sizeof( b3CompoundSphere ) ); b3CompoundData* compound = b3Alloc( byteCount ); memset( compound, 0, byteCount ); compound->version = B3_COMPOUND_VERSION; - compound->byteCount = byteCount; + compound->byteCount = (int)byteCount; compound->nodeOffset = nodeOffset; memcpy( &compound->tree, &tree, sizeof( b3DynamicTree ) ); diff --git a/src/contact_solver.c b/src/contact_solver.c index 7ada73c..5ee2eaf 100644 --- a/src/contact_solver.c +++ b/src/contact_solver.c @@ -10,6 +10,7 @@ #include "math_internal.h" #include "physics_world.h" #include "platform.h" +#include "simd.h" #include "solver_set.h" #if B3_ENABLE_VALIDATION @@ -785,30 +786,6 @@ void b3StoreImpulses_Mesh( b3SolverBlock block, b3StepContext* context, int work taskContext->hasHitEvents = hasHitEvents; } -#if defined( B3_SIMD_NEON ) - -#include - -// wide float holds 4 numbers -typedef float32x4_t b3FloatW; - -#elif defined( B3_SIMD_SSE2 ) - -#include - -// wide float holds 4 numbers -typedef __m128 b3FloatW; - -#else - -// scalar math -typedef struct b3FloatW -{ - float x, y, z, w; -} b3FloatW; - -#endif - // Wide vec2 typedef struct b3Vec2W { @@ -840,372 +817,6 @@ typedef struct b3SymMatrix3W b3FloatW cxx, cxy, cxz, cyy, cyz, czz; } b3SymMatrix3W; -#if defined( B3_SIMD_NEON ) - -static inline b3FloatW b3ZeroW( void ) -{ - return vdupq_n_f32( 0.0f ); -} - -static inline b3FloatW b3SplatW( float scalar ) -{ - return vdupq_n_f32( scalar ); -} - -static inline b3FloatW b3NegW( b3FloatW a ) -{ - return vnegq_f32( a ); -} - -static inline b3FloatW b3SetW( float a, float b, float c, float d ) -{ - float32_t array[4] = { a, b, c, d }; - return vld1q_f32( array ); -} - -static inline b3FloatW b3AddW( b3FloatW a, b3FloatW b ) -{ - return vaddq_f32( a, b ); -} - -static inline b3FloatW b3SubW( b3FloatW a, b3FloatW b ) -{ - return vsubq_f32( a, b ); -} - -static inline b3FloatW b3MulW( b3FloatW a, b3FloatW b ) -{ - return vmulq_f32( a, b ); -} - -static inline b3FloatW b3DivW( b3FloatW a, b3FloatW b ) -{ - return vdivq_f32( a, b ); -} - -static inline b3FloatW b3SqrtW( b3FloatW a ) -{ - return vsqrtq_f32( a ); -} - -// Cannot use real FMA because it doesn't match the non-SIMD path -static inline b3FloatW b3MulAddW( b3FloatW a, b3FloatW b, b3FloatW c ) -{ - return vaddq_f32( a, vmulq_f32( b, c ) ); -} - -// static inline b3FloatW b3MulSubW( b3FloatW a, b3FloatW b, b3FloatW c ) -//{ -// return vsubq_f32( a, vmulq_f32( b, c ) ); -// } - -static inline b3FloatW b3MinW( b3FloatW a, b3FloatW b ) -{ - return vminq_f32( a, b ); -} - -static inline b3FloatW b3MaxW( b3FloatW a, b3FloatW b ) -{ - return vmaxq_f32( a, b ); -} - -// clamp a to [-b, b] -static inline b3FloatW b3SymClampW( b3FloatW a, b3FloatW b ) -{ - b3FloatW nb = b3NegW( b ); - b3FloatW c = b3MaxW( nb, a ); - return b3MinW( c, b ); -} - -static inline b3FloatW b3OrW( b3FloatW a, b3FloatW b ) -{ - return vreinterpretq_f32_u32( vorrq_u32( vreinterpretq_u32_f32( a ), vreinterpretq_u32_f32( b ) ) ); -} - -static inline b3FloatW b3GreaterThanW( b3FloatW a, b3FloatW b ) -{ - return vreinterpretq_f32_u32( vcgtq_f32( a, b ) ); -} - -static inline b3FloatW b3EqualsW( b3FloatW a, b3FloatW b ) -{ - return vreinterpretq_f32_u32( vceqq_f32( a, b ) ); -} - -static inline bool b3AllZeroW( b3FloatW a ) -{ - // Create a zero vector for comparison - b3FloatW zero = vdupq_n_f32( 0.0f ); - - // Compare the input vector with zero - uint32x4_t cmp_result = vceqq_f32( a, zero ); - -// Check if all comparison results are non-zero using vminvq -#ifdef __ARM_FEATURE_SVE - // ARM v8.2+ has horizontal minimum instruction - return vminvq_u32( cmp_result ) != 0; -#else - // For older ARM architectures, we need to manually check all lanes - return vgetq_lane_u32( cmp_result, 0 ) != 0 && vgetq_lane_u32( cmp_result, 1 ) != 0 && vgetq_lane_u32( cmp_result, 2 ) != 0 && - vgetq_lane_u32( cmp_result, 3 ) != 0; -#endif -} - -// component-wise returns mask ? b : a -static inline b3FloatW b3BlendW( b3FloatW a, b3FloatW b, b3FloatW mask ) -{ - uint32x4_t mask32 = vreinterpretq_u32_f32( mask ); - return vbslq_f32( mask32, b, a ); -} - -#elif defined( B3_SIMD_SSE2 ) - -static inline b3FloatW b3ZeroW( void ) -{ - return _mm_setzero_ps(); -} - -static inline b3FloatW b3SplatW( float scalar ) -{ - return _mm_set1_ps( scalar ); -} - -static inline b3FloatW b3NegW( b3FloatW a ) -{ - // Create a mask with the sign bit set for each element - __m128 mask = _mm_set1_ps( -0.0f ); - - // XOR the input with the mask to negate each element - return _mm_xor_ps( a, mask ); -} - -static inline b3FloatW b3SetW( float a, float b, float c, float d ) -{ - return _mm_setr_ps( a, b, c, d ); -} - -static inline b3FloatW b3AddW( b3FloatW a, b3FloatW b ) -{ - return _mm_add_ps( a, b ); -} - -static inline b3FloatW b3SubW( b3FloatW a, b3FloatW b ) -{ - return _mm_sub_ps( a, b ); -} - -static inline b3FloatW b3MulW( b3FloatW a, b3FloatW b ) -{ - return _mm_mul_ps( a, b ); -} - -static inline b3FloatW b3DivW( b3FloatW a, b3FloatW b ) -{ - return _mm_div_ps( a, b ); -} - -static inline b3FloatW b3SqrtW( b3FloatW a ) -{ - return _mm_sqrt_ps( a ); -} - -static inline b3FloatW b3MulAddW( b3FloatW a, b3FloatW b, b3FloatW c ) -{ - return _mm_add_ps( a, _mm_mul_ps( b, c ) ); -} - -// static inline b3FloatW b3MulSubW( b3FloatW a, b3FloatW b, b3FloatW c ) -//{ -// return _mm_sub_ps( a, _mm_mul_ps( b, c ) ); -// } - -static inline b3FloatW b3MinW( b3FloatW a, b3FloatW b ) -{ - return _mm_min_ps( a, b ); -} - -static inline b3FloatW b3MaxW( b3FloatW a, b3FloatW b ) -{ - return _mm_max_ps( a, b ); -} - -// clamp a to [-b, b] -static inline b3FloatW b3SymClampW( b3FloatW a, b3FloatW b ) -{ - b3FloatW nb = b3NegW( b ); - b3FloatW c = b3MaxW( nb, a ); - return b3MinW( c, b ); -} - -static inline b3FloatW b3OrW( b3FloatW a, b3FloatW b ) -{ - return _mm_or_ps( a, b ); -} - -static inline b3FloatW b3GreaterThanW( b3FloatW a, b3FloatW b ) -{ - return _mm_cmpgt_ps( a, b ); -} - -static inline b3FloatW b3EqualsW( b3FloatW a, b3FloatW b ) -{ - return _mm_cmpeq_ps( a, b ); -} - -static inline bool b3AllZeroW( b3FloatW a ) -{ - // Compare each element with zero - b3FloatW zero = _mm_setzero_ps(); - b3FloatW cmp = _mm_cmpeq_ps( a, zero ); - - // Create a mask from the comparison results - int mask = _mm_movemask_ps( cmp ); - - // If all elements are zero, the mask will be 0xF (1111 in binary) - return mask == 0xF; -} - -// component-wise returns mask ? b : a -static inline b3FloatW b3BlendW( b3FloatW a, b3FloatW b, b3FloatW mask ) -{ - return _mm_or_ps( _mm_and_ps( mask, b ), _mm_andnot_ps( mask, a ) ); -} - -#else - -static inline b3FloatW b3ZeroW( void ) -{ - return (b3FloatW){ 0.0f, 0.0f, 0.0f, 0.0f }; -} - -static inline b3FloatW b3SplatW( float scalar ) -{ - return (b3FloatW){ scalar, scalar, scalar, scalar }; -} - -static inline b3FloatW b3NegW( b3FloatW a ) -{ - return (b3FloatW){ -a.x, -a.y, -a.z, -a.w }; -} - -static inline b3FloatW b3AddW( b3FloatW a, b3FloatW b ) -{ - return (b3FloatW){ a.x + b.x, a.y + b.y, a.z + b.z, a.w + b.w }; -} - -static inline b3FloatW b3SubW( b3FloatW a, b3FloatW b ) -{ - return (b3FloatW){ a.x - b.x, a.y - b.y, a.z - b.z, a.w - b.w }; -} - -static inline b3FloatW b3MulW( b3FloatW a, b3FloatW b ) -{ - return (b3FloatW){ a.x * b.x, a.y * b.y, a.z * b.z, a.w * b.w }; -} - -static inline b3FloatW b3DivW( b3FloatW a, b3FloatW b ) -{ - return (b3FloatW){ a.x / b.x, a.y / b.y, a.z / b.z, a.w / b.w }; -} - -static inline b3FloatW b3SqrtW( b3FloatW a ) -{ - return (b3FloatW){ sqrtf( a.x ), sqrtf( a.y ), sqrtf( a.z ), sqrtf( a.w ) }; -} - -static inline b3FloatW b3MulAddW( b3FloatW a, b3FloatW b, b3FloatW c ) -{ - return (b3FloatW){ a.x + b.x * c.x, a.y + b.y * c.y, a.z + b.z * c.z, a.w + b.w * c.w }; -} - -// static inline b3FloatW b3MulSubW( b3FloatW a, b3FloatW b, b3FloatW c ) -//{ -// return { a.x - b.x * c.x, a.y - b.y * c.y, a.z - b.z * c.z, a.w - b.w * c.w }; -// } - -// static inline b3FloatW b3MinW( b3FloatW a, b3FloatW b ) -//{ -// b3FloatW r; -// r.x = a.x <= b.x ? a.x : b.x; -// r.y = a.y <= b.y ? a.y : b.y; -// r.z = a.z <= b.z ? a.z : b.z; -// r.w = a.w <= b.w ? a.w : b.w; -// return r; -// } - -static inline b3FloatW b3MaxW( b3FloatW a, b3FloatW b ) -{ - b3FloatW r; - r.x = a.x >= b.x ? a.x : b.x; - r.y = a.y >= b.y ? a.y : b.y; - r.z = a.z >= b.z ? a.z : b.z; - r.w = a.w >= b.w ? a.w : b.w; - return r; -} - -// clamp a to [-b, b] -static inline b3FloatW b3SymClampW( b3FloatW a, b3FloatW b ) -{ - b3FloatW r; - r.x = a.x <= b.x ? a.x : b.x; - r.y = a.y <= b.y ? a.y : b.y; - r.z = a.z <= b.z ? a.z : b.z; - r.w = a.w <= b.w ? a.w : b.w; - r.x = r.x <= -b.x ? -b.x : r.x; - r.y = r.y <= -b.y ? -b.y : r.y; - r.z = r.z <= -b.z ? -b.z : r.z; - r.w = r.w <= -b.w ? -b.w : r.w; - return r; -} - -static inline b3FloatW b3OrW( b3FloatW a, b3FloatW b ) -{ - b3FloatW r; - r.x = a.x != 0.0f || b.x != 0.0f ? 1.0f : 0.0f; - r.y = a.y != 0.0f || b.y != 0.0f ? 1.0f : 0.0f; - r.z = a.z != 0.0f || b.z != 0.0f ? 1.0f : 0.0f; - r.w = a.w != 0.0f || b.w != 0.0f ? 1.0f : 0.0f; - return r; -} - -static inline b3FloatW b3GreaterThanW( b3FloatW a, b3FloatW b ) -{ - b3FloatW r; - r.x = a.x > b.x ? 1.0f : 0.0f; - r.y = a.y > b.y ? 1.0f : 0.0f; - r.z = a.z > b.z ? 1.0f : 0.0f; - r.w = a.w > b.w ? 1.0f : 0.0f; - return r; -} - -static inline b3FloatW b3EqualsW( b3FloatW a, b3FloatW b ) -{ - b3FloatW r; - r.x = a.x == b.x ? 1.0f : 0.0f; - r.y = a.y == b.y ? 1.0f : 0.0f; - r.z = a.z == b.z ? 1.0f : 0.0f; - r.w = a.w == b.w ? 1.0f : 0.0f; - return r; -} - -static inline bool b3AllZeroW( b3FloatW a ) -{ - return a.x == 0.0f && a.y == 0.0f && a.z == 0.0f && a.w == 0.0f; -} - -// component-wise returns mask ? b : a -static inline b3FloatW b3BlendW( b3FloatW a, b3FloatW b, b3FloatW mask ) -{ - b3FloatW r; - r.x = mask.x != 0.0f ? b.x : a.x; - r.y = mask.y != 0.0f ? b.y : a.y; - r.z = mask.z != 0.0f ? b.z : a.z; - r.w = mask.w != 0.0f ? b.w : a.w; - return r; -} - -#endif - // s * a static inline b3Vec3W b3MulSVW( b3FloatW s, b3Vec3W a ) { diff --git a/src/convex_manifold.c b/src/convex_manifold.c index ff55e10..bb3fb08 100644 --- a/src/convex_manifold.c +++ b/src/convex_manifold.c @@ -4,6 +4,7 @@ #include "algorithm.h" #include "manifold.h" #include "shape.h" +#include "simd.h" #include "box3d/base.h" #include "box3d/collision.h" @@ -116,39 +117,6 @@ static b3FaceQuery b3QueryFaceDirectionHullAndCapsule( const b3HullData* hull, c }; } -static b3FaceQuery b3QueryFaceDirections( const b3HullData* hullA, const b3HullData* hullB, b3Transform relativeTransform ) -{ - // We perform all computations in local space of the second hull - b3Transform transform = b3InvertTransform( relativeTransform ); - const b3Plane* planesA = b3GetHullPlanes( hullA ); - const b3Vec3* pointsB = b3GetHullPoints( hullB ); - - int maxFaceIndex = -1; - int maxVertexIndex = -1; - float maxFaceSeparation = -FLT_MAX; - - for ( int faceIndex = 0; faceIndex < hullA->faceCount; ++faceIndex ) - { - b3Plane plane = b3TransformPlane( transform, planesA[faceIndex] ); - - int vertexIndex = b3FindHullSupportVertex( hullB, b3Neg( plane.normal ) ); - b3Vec3 support = pointsB[vertexIndex]; - float separation = b3PlaneSeparation( plane, support ); - if ( separation > maxFaceSeparation ) - { - maxFaceIndex = faceIndex; - maxVertexIndex = vertexIndex; - maxFaceSeparation = separation; - } - } - - return (b3FaceQuery){ - .separation = maxFaceSeparation, - .faceIndex = (uint8_t)maxFaceIndex, - .vertexIndex = (uint8_t)maxVertexIndex, - }; -} - static b3EdgeQuery b3QueryEdgeDirectionHullAndCapsule( const b3HullData* hull, const b3Capsule* capsule, b3Transform capsuleTransform ) { @@ -166,7 +134,7 @@ static b3EdgeQuery b3QueryEdgeDirectionHullAndCapsule( const b3HullData* hull, c const b3HullHalfEdge* edges = b3GetHullEdges( hull ); const b3Vec3* points = b3GetHullPoints( hull ); const b3Plane* planes = b3GetHullPlanes( hull ); - float squaredTolerance = 0.005f * 0.005f; + float squaredTolerance = B3_PARALLEL_EDGE_TOL * B3_PARALLEL_EDGE_TOL; for ( int index = 0; index < hull->edgeCount; index += 2 ) { @@ -234,114 +202,6 @@ static b3EdgeQuery b3QueryEdgeDirectionHullAndCapsule( const b3HullData* hull, c }; } -static b3EdgeQuery b3QueryEdgeDirections( const b3HullData* hullA, const b3HullData* hullB, b3Transform transformBtoA ) -{ - // Find axis of minimum penetration - b3Vec3 maxNormal = b3Vec3_zero; - float maxSeparation = -FLT_MAX; - int maxIndexA = B3_NULL_INDEX; - int maxIndexB = B3_NULL_INDEX; - - const b3HullHalfEdge* edgesA = b3GetHullEdges( hullA ); - const b3Vec3* pointsA = b3GetHullPoints( hullA ); - const b3Plane* planesA = b3GetHullPlanes( hullA ); - const b3HullHalfEdge* edgesB = b3GetHullEdges( hullB ); - const b3Vec3* pointsB = b3GetHullPoints( hullB ); - const b3Plane* planesB = b3GetHullPlanes( hullB ); - - // Work in frame A - b3Matrix3 matrix = b3MakeMatrixFromQuat( transformBtoA.q ); - - float squaredTolerance = 0.005f * 0.005f; - - // Arranged to minimize transform operations - for ( int indexB = 0; indexB < hullB->edgeCount; indexB += 2 ) - { - const b3HullHalfEdge* edgeB = edgesB + indexB; - const b3HullHalfEdge* twinB = edgesB + indexB + 1; - B3_ASSERT( edgeB->twin == indexB + 1 && twinB->twin == indexB ); - - b3Vec3 qB = pointsB[twinB->origin]; - b3Vec3 eB = b3MulMV( matrix, b3Sub( qB, pointsB[edgeB->origin] ) ); - qB = b3Add( b3MulMV( matrix, qB ), transformBtoA.p ); - - b3Vec3 uB = b3MulMV( matrix, planesB[edgeB->face].normal ); - b3Vec3 vB = b3MulMV( matrix, planesB[twinB->face].normal ); - - for ( int indexA = 0; indexA < hullA->edgeCount; indexA += 2 ) - { - const b3HullHalfEdge* edgeA = edgesA + indexA; - const b3HullHalfEdge* twinA = edgesA + indexA + 1; - B3_ASSERT( edgeA->twin == indexA + 1 && twinA->twin == indexA ); - - b3Vec3 qA = pointsA[twinA->origin]; - b3Vec3 eA = b3Sub( qA, pointsA[edgeA->origin] ); - b3Vec3 uA = planesA[edgeA->face].normal; - b3Vec3 vA = planesA[twinA->face].normal; - - // See "Collision Detection of Convex Polyhedra Based on Duality Transformation" - // Two edges build a face on the Minkowski sum if the associated arcs AB and CD intersect on the Gauss map. - // The associated arcs are defined by the adjacent face normals of each edge. - - // These are signed volumes with an edge optimization to avoid cross products - // eA parallel to cross(vA, uA) - // eB parallel to cross(vB, uB) - // Since only signs are tested, length doesn't matter. - - float cba = b3Dot( uB, eA ); - float dba = b3Dot( vB, eA ); - float adc = -b3Dot( uA, eB ); - float bdc = -b3Dot( vA, eB ); - - if ( cba * dba < 0.0f && adc * bdc < 0.0f && cba * bdc > 0.0f ) - { - // Avoid nearly parallel edges that may lead to invalid separation values at the noise floor. - if ( b3MaxFloat( cba * cba, dba * dba ) < squaredTolerance * b3LengthSquared( eA ) ) - { - continue; - } - - // The intersection of the arcs on the Gauss map is the edge pair axis. Cast the - // arc of hull B (from uB to vB) against the plane containing the arc of hull A: - // dot(uB + t * (vB - uB), eA) == 0 - // then - // t = cba / (cba - dba) - // - // The signs of cba and dba differ (Minkowski test), so the division is safe. - // - // The axis generated points from B to A by construction since it lands between - // two face normals on B. This removes the need to orient the separation axis - // using the hull centers. - // - // The axis is perpendicular to both edges so I can use qA and qB as arbitrary - // points on edgeA and edgeB to measure the separation. - float t = cba / ( cba - dba ); - b3Vec3 axis = b3Lerp( uB, vB, t ); - B3_VALIDATE( b3LengthSquared( axis ) > 1000.0f * FLT_MIN ); - axis = b3Normalize( axis ); - float separation = b3Dot( axis, b3Sub( qA, qB ) ); - - if ( separation > maxSeparation ) - { - // Continues to find the maximum separating axis - // Flip normal so it points from A to B - maxNormal = b3Neg( axis ); - maxSeparation = separation; - maxIndexA = indexA; - maxIndexB = indexB; - } - } - } - } - - return (b3EdgeQuery){ - .normal = maxNormal, - .separation = maxSeparation, - .indexA = maxIndexA, - .indexB = maxIndexB, - }; -} - // Reduce the manifold points to a maximum of 4 points. // Note: this modifies the input point array to improve performance static void b3ReduceManifoldPoints( b3LocalManifold* manifold, int capacity, b3LocalManifoldPoint* points, int count ) @@ -1321,6 +1181,1077 @@ static bool b3BuildEdgeContact( b3LocalManifold* manifold, const b3HullData* hul return true; } +// Transform a SoA point/normal stream (already split into X/Y/Z) by out = -(R*v (+t)). +// The inputs come straight from the hull's stored SoA arrays, so there's no transpose here. +// IsPoint is a template arg so the translation add is only emitted for points. +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 ) +{ + B3_VALIDATE( ( (uintptr_t)outX & 0xF ) == 0 ); + B3_VALIDATE( ( (uintptr_t)outY & 0xF ) == 0 ); + B3_VALIDATE( ( (uintptr_t)outZ & 0xF ) == 0 ); + + // row-column + b3FloatW r00 = b3SplatW( R->cx.x ); + b3FloatW r01 = b3SplatW( R->cy.x ); + b3FloatW r02 = b3SplatW( R->cz.x ); + b3FloatW r10 = b3SplatW( R->cx.y ); + b3FloatW r11 = b3SplatW( R->cy.y ); + b3FloatW r12 = b3SplatW( R->cz.y ); + b3FloatW r20 = b3SplatW( R->cx.z ); + b3FloatW r21 = b3SplatW( R->cy.z ); + b3FloatW r22 = b3SplatW( R->cz.z ); + + b3FloatW tx = b3ZeroW(); + b3FloatW ty = b3ZeroW(); + b3FloatW tz = b3ZeroW(); + + if ( isPoint ) + { + tx = b3SplatW( p.x ); + ty = b3SplatW( p.y ); + tz = b3SplatW( p.z ); + } + + for ( int i = 0; i < n; i += 4 ) + { + b3FloatW x = b3LoadW( inX + i ); + b3FloatW y = b3LoadW( inY + i ); + b3FloatW z = b3LoadW( inZ + i ); + + // Rotate four vectors at a time + b3FloatW ox = b3Dot3W( r00, r01, r02, x, y, z ); + b3FloatW oy = b3Dot3W( r10, r11, r12, x, y, z ); + b3FloatW oz = b3Dot3W( r20, r21, r22, x, y, z ); + + if ( isPoint ) + { + ox = b3AddW( ox, tx ); + oy = b3AddW( oy, ty ); + oz = b3AddW( oz, tz ); + } + + b3StoreW( outX + i, b3NegW( ox ) ); + b3StoreW( outY + i, b3NegW( oy ) ); + b3StoreW( outZ + i, b3NegW( oz ) ); + } +} + +_Static_assert( B3_MAX_HULL_VERTICES == 128, "must be 128" ); + +#define B3_HULL_BIT_COUNT 7 + +// SIMD support point calculation using a SoA vertex array padded with repeats of the first vertex +// to a multiple of 4. +// +// This minimizes (bias - dot), where the caller is expected to provide a bias that makes this always positive. +// It can be direction dependent. The bias should be just big enough to ensure the value if 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 +// 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 b3FloatW nx = b3SplatW( normal.x ); + const b3FloatW ny = b3SplatW( normal.y ); + const b3FloatW nz = b3SplatW( normal.z ); + const b3FloatW biasV = b3SplatW( bias ); + + // Start the minimum at a large value. + b3FloatW minValue = b3SplatW( B3_HUGE ); + + // Tail lanes hold vertex 0 with index bits >= vertexCount, so they never become the min value. + for ( int i = 0; i < n; i += 4 ) + { + b3FloatW x = b3LoadW( vx + i ); + b3FloatW y = b3LoadW( vy + i ); + b3FloatW z = b3LoadW( vz + i ); + b3FloatW d = b3AddW( b3MulW( nz, z ), b3AddW( b3MulW( ny, y ), b3MulW( nx, x ) ) ); + + // 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]; +} + +#define NE ( B3_MAX_HULL_EDGES + 4 ) +#define NF ( B3_MAX_HULL_FACES + 4 ) +#define NV ( B3_MAX_HULL_VERTICES + 4 ) + +// SIMD separating axis test based on an implementation developed by Cairn Overturf. +// See his article: https://cairno.substack.com/p/improvements-to-the-separating-axis +b3AxisQuery b3ComputeSeparatingAxis( const b3HullData* hullA, const b3HullData* hullB, b3Transform xfB, int axisOverride ) +{ + B3_VALIDATE( axisOverride == b3_invalidAxis || axisOverride == b3_manualFaceAxisA || axisOverride == b3_manualFaceAxisB || + axisOverride == b3_manualEdgePairAxis ); + + b3Matrix3 R = b3MakeMatrixFromQuat( xfB.q ); + b3Matrix3 invR = b3Transpose( R ); + + float speculativeDistance = B3_SPECULATIVE_DISTANCE; + + b3AxisQuery res = { + .normal = b3Vec3_zero, + .separation = -INFINITY, + .indexA = B3_NULL_INDEX, + .indexB = B3_NULL_INDEX, + .type = b3_invalidAxis, + }; + + int faceCountA = hullA->faceCount; + const b3Plane* planesA = b3GetHullPlanes( hullA ); + + int soaVertexCountB = ( hullB->vertexCount + 3 ) & ~3; + const float* vxB = b3GetHullSoaVertices( hullB ); + const float* vyB = vxB + soaVertexCountB; + const float* vzB = vyB + soaVertexCountB; + + b3Vec3 cB = b3AABB_Center( hullB->aabb ); + b3Vec3 hB = b3AABB_Extents( hullB->aabb ); + + // Test A's face planes against B's vertices. + if ( axisOverride != b3_manualFaceAxisB && axisOverride != b3_manualEdgePairAxis ) + { + for ( int i = 0; i < faceCountA; ++i ) + { + b3Plane plane = planesA[i]; + b3Vec3 direction = b3Neg( b3MulMV( invR, plane.normal ) ); + float planeSeparation = b3Dot( plane.normal, xfB.p ) - plane.offset; + float biasB = b3Dot( direction, cB ) + 1.0625f * b3Dot( b3Abs( direction ), hB ); + float support; + int vertexIndex; + b3GetSupportWide( direction, vxB, vyB, vzB, soaVertexCountB, biasB, &support, &vertexIndex ); + float separation = planeSeparation - support; + if ( separation > res.separation ) + { + res.type = b3_faceAxisA; + res.separation = separation; + res.indexA = i; + res.indexB = vertexIndex; + res.normal = plane.normal; + if ( separation > speculativeDistance ) + { + return res; + } + } + } + } + + if ( axisOverride == b3_manualFaceAxisA ) + { + return res; + } + + int faceCountB = hullB->faceCount; + const b3Plane* planesB = b3GetHullPlanes( hullB ); + + int soaVertexCountA = ( hullA->vertexCount + 3 ) & ~3; + const float* vxA = b3GetHullSoaVertices( hullA ); + const float* vyA = vxA + soaVertexCountA; + const float* vzA = vyA + soaVertexCountA; + + b3Vec3 cA = b3AABB_Center( hullA->aabb ); + b3Vec3 hA = b3AABB_Extents( hullA->aabb ); + + // Test B's face planes against A's vertices. + if ( axisOverride != b3_manualEdgePairAxis ) + { + for ( int i = 0; i < faceCountB; ++i ) + { + b3Plane plane = planesB[i]; + b3Vec3 direction = b3Neg( b3MulMV( R, plane.normal ) ); + float planeSeparation = b3Dot( direction, xfB.p ) - plane.offset; + float biasA = b3Dot( direction, cA ) + 1.0625f * b3Dot( b3Abs( direction ), hA ); + float support; + int vertexIndex; + b3GetSupportWide( direction, vxA, vyA, vzA, soaVertexCountA, biasA, &support, &vertexIndex ); + float separation = planeSeparation - support; + if ( separation > res.separation ) + { + res.type = b3_faceAxisB; + res.separation = separation; + res.indexA = vertexIndex; + res.indexB = i; + // This points from A to B and is in frame A + res.normal = direction; + if ( separation > speculativeDistance ) + { + return res; + } + } + } + } + + if ( axisOverride == b3_manualFaceAxisB ) + { + return res; + } + + // Transform B into A's space once, into SoA arrays. Extra space so + // tail can be set to zero in all cases. + _Static_assert( ( B3_MAX_HULL_EDGES & 3 ) == 0, "must be multiple of 4" ); + _Static_assert( ( B3_MAX_HULL_FACES & 3 ) == 0, "must be multiple of 4" ); + _Static_assert( ( B3_MAX_HULL_VERTICES & 3 ) == 0, "must be multiple of 4" ); + + // The alignments below are not necessary, but they don't hurt. + + // B face normals in A space, negated. + _Alignas( 16 ) float bFNx[NF]; + _Alignas( 16 ) float bFNy[NF]; + _Alignas( 16 ) float bFNz[NF]; + + // B vertices in A space, negated. + _Alignas( 16 ) float bWx[NV]; + _Alignas( 16 ) float bWy[NV]; + _Alignas( 16 ) float bWz[NV]; + + int soaFaceCountB = ( faceCountB + 3 ) & ~3; + const float* nxB = b3GetHullSoaNormals( hullB ); + const float* nyB = nxB + soaFaceCountB; + const float* nzB = nyB + soaFaceCountB; + + b3NegativeTransformFromSoA( &R, xfB.p, nxB, nyB, nzB, soaFaceCountB, bFNx, bFNy, bFNz, false ); + b3NegativeTransformFromSoA( &R, xfB.p, vxB, vyB, vzB, soaVertexCountB, bWx, bWy, bWz, true ); + + // Per B edge data. C and D are the two face normals, v0 a vertex, DC the edge vector. + _Alignas( 16 ) float bCx[NE]; + _Alignas( 16 ) float bCy[NE]; + _Alignas( 16 ) float bCz[NE]; + _Alignas( 16 ) float bDx[NE]; + _Alignas( 16 ) float bDy[NE]; + _Alignas( 16 ) float bDz[NE]; + _Alignas( 16 ) float bV0x[NE]; + _Alignas( 16 ) float bV0y[NE]; + _Alignas( 16 ) float bV0z[NE]; + _Alignas( 16 ) float bDCx[NE]; + _Alignas( 16 ) float bDCy[NE]; + _Alignas( 16 ) float bDCz[NE]; + + int halfEdgeCountB = hullB->edgeCount; + const b3HullHalfEdge* halfEdgesB = b3GetHullEdges( hullB ); + int nb = 0; + for ( int i = 0; i < halfEdgeCountB; i += 2 ) + { + const b3HullHalfEdge* edge = halfEdgesB + i; + const b3HullHalfEdge* twin = edge + 1; + int f0 = edge->face; + int f1 = twin->face; + int v0 = edge->origin; + int v1 = twin->origin; + + bCx[nb] = bFNx[f0]; + bCy[nb] = bFNy[f0]; + bCz[nb] = bFNz[f0]; + bDx[nb] = bFNx[f1]; + bDy[nb] = bFNy[f1]; + bDz[nb] = bFNz[f1]; + bV0x[nb] = bWx[v0]; + bV0y[nb] = bWy[v0]; + bV0z[nb] = bWz[v0]; + bDCx[nb] = bWx[v1] - bWx[v0]; + bDCy[nb] = bWy[v1] - bWy[v0]; + bDCz[nb] = bWz[v1] - bWz[v0]; + nb += 1; + } + + // Per A edge data, already in A's space so just gathered. n0 and n1 are the two face + // normals, d the edge vector av1-av0, v0 the first vertex. Tol is the + // parallel edge tolerance, scaled by the edge length. + _Alignas( 16 ) float aN0x[NE]; + _Alignas( 16 ) float aN0y[NE]; + _Alignas( 16 ) float aN0z[NE]; + _Alignas( 16 ) float aN1x[NE]; + _Alignas( 16 ) float aN1y[NE]; + _Alignas( 16 ) float aN1z[NE]; + // dir = av1 - av0 + _Alignas( 16 ) float aDx[NE]; + _Alignas( 16 ) float aDy[NE]; + _Alignas( 16 ) float aDz[NE]; + _Alignas( 16 ) float aV0x[NE]; + _Alignas( 16 ) float aV0y[NE]; + _Alignas( 16 ) float aV0z[NE]; + _Alignas( 16 ) float aTol[NE]; + + int halfEdgeCountA = hullA->edgeCount; + const b3HullHalfEdge* halfEdgesA = b3GetHullEdges( hullA ); + int na = 0; + + float squaredTol = B3_PARALLEL_EDGE_TOL * B3_PARALLEL_EDGE_TOL; + for ( int i = 0; i < halfEdgeCountA; i += 2 ) + { + const b3HullHalfEdge* edge = halfEdgesA + i; + const b3HullHalfEdge* twin = edge + 1; + + b3Vec3 A = planesA[edge->face].normal; + b3Vec3 B = planesA[twin->face].normal; + aN0x[na] = A.x; + aN0y[na] = A.y; + aN0z[na] = A.z; + aN1x[na] = B.x; + aN1y[na] = B.y; + aN1z[na] = B.z; + + int v0 = edge->origin; + int v1 = twin->origin; + + aDx[na] = vxA[v1] - vxA[v0]; + aDy[na] = vyA[v1] - vyA[v0]; + aDz[na] = vzA[v1] - vzA[v0]; + aV0x[na] = vxA[v0]; + aV0y[na] = vyA[v0]; + aV0z[na] = vzA[v0]; + + aTol[na] = squaredTol * ( aDx[na] * aDx[na] + aDy[na] * aDy[na] + aDz[na] * aDz[na] ); + na += 1; + } + + // Zero the tail lanes. + b3FloatW zero = b3ZeroW(); + b3StoreW( aN0x + na, zero ); + b3StoreW( aN0y + na, zero ); + b3StoreW( aN0z + na, zero ); + b3StoreW( aN1x + na, zero ); + b3StoreW( aN1y + na, zero ); + b3StoreW( aN1z + na, zero ); + b3StoreW( aDx + na, zero ); + b3StoreW( aDy + na, zero ); + b3StoreW( aDz + na, zero ); + b3StoreW( aV0x + na, zero ); + b3StoreW( aV0y + na, zero ); + b3StoreW( aV0z + na, zero ); + b3StoreW( aTol + na, zero ); + + // Prefer face contact for more contact points. + float absFaceBias = 0.1f * B3_LINEAR_SLOP; + + int edgeCountB = halfEdgeCountB / 2; + +#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 = -0.0001f; + + for ( int j = 0; j < edgeCountB; ++j ) + { + float Cx = bCx[j]; + float Cy = bCy[j]; + float Cz = bCz[j]; + float Dx = bDx[j]; + float Dy = bDy[j]; + float Dz = bDz[j]; + float DCx = bDCx[j]; + float DCy = bDCy[j]; + float DCz = bDCz[j]; + float bv0x = bV0x[j]; + float bv0y = bV0y[j]; + float bv0z = bV0z[j]; + + for ( int i = 0; i < na; ++i ) + { + // CBA = C.dir, DBA = D.dir, where dir = B_x_A + float CBA = Cx * aDx[i] + ( Cy * aDy[i] + Cz * aDz[i] ); + float DBA = Dx * aDx[i] + ( Dy * aDy[i] + Dz * aDz[i] ); + if ( CBA * DBA >= EPS ) + { + continue; + } + + // ADC = n0.DC, BDC = n1.DC, where DC = D_x_C + float ADC = aN0x[i] * DCx + ( aN0y[i] * DCy + aN0z[i] * DCz ); + float BDC = aN1x[i] * DCx + ( aN1y[i] * DCy + aN1z[i] * DCz ); + 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 + float nx = Cx + t * ( Dx - Cx ); + float ny = Cy + t * ( Dy - Cy ); + float nz = Cz + t * ( Dz - Cz ); + float len2 = nx * nx + ( ny * ny + nz * nz ); + float inv = 1.0f / sqrtf( len2 ); + nx *= inv; + ny *= inv; + nz *= inv; + + // separation = -dot(normal, av0 + bv0) + float sx = aV0x[i] + bv0x; + float sy = aV0y[i] + bv0y; + float sz = aV0z[i] + bv0z; + + float separation = -( sx * nx + ( sy * ny + sz * nz ) ); + if ( separation > res.separation + absFaceBias ) + { + res.normal = (b3Vec3){ nx, ny, nz }; + res.separation = separation; + res.type = b3_edgePairAxis; + + // Half edge index + res.indexA = 2 * i; + res.indexB = 2 * j; + + // Edge beats face, remove bias + absFaceBias = 0.0f; + if ( separation > speculativeDistance ) + { + return res; + } + } + } + } + +#else + + // Edge phase, one B edge against four A edges at a time, no transforms in the loop. + const b3FloatW EPS = b3SplatW( -0.0001f ); + const b3FloatW INF = b3SplatW( INFINITY ); + + for ( int j = 0; j < edgeCountB; ++j ) + { + const b3FloatW Cx = b3SplatW( bCx[j] ); + const b3FloatW Cy = b3SplatW( bCy[j] ); + const b3FloatW Cz = b3SplatW( bCz[j] ); + const b3FloatW Dx = b3SplatW( bDx[j] ); + const b3FloatW Dy = b3SplatW( bDy[j] ); + const b3FloatW Dz = b3SplatW( bDz[j] ); + const b3FloatW DCx = b3SplatW( bDCx[j] ); + const b3FloatW DCy = b3SplatW( bDCy[j] ); + const b3FloatW DCz = b3SplatW( bDCz[j] ); + const b3FloatW bv0x = b3SplatW( bV0x[j] ); + const b3FloatW bv0y = b3SplatW( bV0y[j] ); + const b3FloatW bv0z = b3SplatW( bV0z[j] ); + + for ( int i = 0; i < na; i += 4 ) + { + b3FloatW n0x = b3LoadW( aN0x + i ); + b3FloatW n0y = b3LoadW( aN0y + i ); + b3FloatW n0z = b3LoadW( aN0z + i ); + b3FloatW n1x = b3LoadW( aN1x + i ); + b3FloatW n1y = b3LoadW( aN1y + i ); + b3FloatW n1z = b3LoadW( aN1z + i ); + b3FloatW dx = b3LoadW( aDx + i ); + b3FloatW dy = b3LoadW( aDy + i ); + b3FloatW dz = b3LoadW( aDz + i ); + b3FloatW v0x = b3LoadW( aV0x + i ); + b3FloatW v0y = b3LoadW( aV0y + i ); + b3FloatW v0z = b3LoadW( aV0z + i ); + b3FloatW tol = b3LoadW( aTol + i ); + + // CBA = C.dir, DBA = D.dir, where dir = B_x_A + b3FloatW CBA = b3Dot3W( Cx, Cy, Cz, dx, dy, dz ); + b3FloatW DBA = b3Dot3W( Dx, Dy, Dz, dx, dy, dz ); + // ADC = n0.DC, BDC = n1.DC, where DC = D_x_C + b3FloatW ADC = b3Dot3W( n0x, n0y, n0z, DCx, DCy, DCz ); + b3FloatW BDC = b3Dot3W( n1x, n1y, n1z, DCx, DCy, DCz ); + + // Gauss map arc crossing test, CBA*DBAsupport only turns negative just before returning, + // so this never skips a lane that would trigger the early out. + b3FloatW improves = b3GreaterThanW( separation, b3SplatW( res.separation ) ); + if ( b3AnyTrueW( improves ) == false ) + { + continue; + } + + _Alignas( 16 ) float sA[4]; + _Alignas( 16 ) float nxA[4]; + _Alignas( 16 ) float nyA[4]; + _Alignas( 16 ) float nzA[4]; + b3StoreW( sA, separation ); + b3StoreW( nxA, nx ); + b3StoreW( nyA, ny ); + b3StoreW( nzA, nz ); + + // 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 a->edges out of range. + for ( int lane = 0; lane < 4; lane++ ) + { + int ei = i + lane; + float s = sA[lane]; + if ( s > res.separation + absFaceBias ) + { + res.normal = (b3Vec3){ nxA[lane], nyA[lane], nzA[lane] }; + res.separation = s; + res.type = b3_edgePairAxis; + // Half edge index + res.indexA = 2 * ei; + res.indexB = 2 * j; + + // Edge beats face, remove bias + absFaceBias = 0.0f; + + if ( s > speculativeDistance ) + { + return res; + } + } + } + } + } +#endif + + return res; +} + +#undef NE +#undef NF +#undef NV + +#define B3_SIMD_COLLIDE_HULLS 1 + +#if B3_SIMD_COLLIDE_HULLS == 1 + +void b3CollideHulls( b3LocalManifold* manifold, int capacity, const b3HullData* hullA, const b3HullData* hullB, + b3Transform transformBtoA, b3SATCache* cache ) +{ + manifold->pointCount = 0; + + if ( capacity < 4 ) + { + return; + } + + // Work in shapeA coordinates + float speculativeDistance = B3_SPECULATIVE_DISTANCE; + + float linearSlop = B3_LINEAR_SLOP; + const b3HullHalfEdge* edgesA = b3GetHullEdges( hullA ); + const b3Plane* planesA = b3GetHullPlanes( hullA ); + const b3Vec3* pointsA = b3GetHullPoints( hullA ); + + const b3HullHalfEdge* edgesB = b3GetHullEdges( hullB ); + const b3Plane* planesB = b3GetHullPlanes( hullB ); + const b3Vec3* pointsB = b3GetHullPoints( hullB ); + + cache->hit = 0; + + // Attempt to use the cache to speed up collision + switch ( cache->type ) + { + case b3_invalidAxis: + break; + + case b3_faceAxisA: + { + B3_ASSERT( cache->indexA < hullA->faceCount ); + + // Check for separation using cached face + b3Plane plane = planesA[cache->indexA]; + b3Vec3 searchDirectionInB = b3Neg( b3InvRotateVector( transformBtoA.q, plane.normal ) ); + + // todo use b3GetSupportWide + int vertexIndex = b3FindHullSupportVertex( hullB, searchDirectionInB ); + b3Vec3 support = b3TransformPoint( transformBtoA, pointsB[vertexIndex] ); + float separation = b3PlaneSeparation( plane, support ); + + if ( separation >= speculativeDistance ) + { + // Cache hit, shapes are separated + cache->hit = 1; + return; + } + + // Attempt face contact using cached feature + b3FaceQuery faceQuery; + faceQuery.separation = 0.0f; + faceQuery.faceIndex = cache->indexA; + faceQuery.vertexIndex = vertexIndex; + + b3SATCache localCache = { 0 }; + bool touching = b3BuildFaceAContact( manifold, capacity, hullA, hullB, transformBtoA, faceQuery, &localCache ); + if ( touching == true && b3AbsFloat( cache->separation - localCache.separation ) < linearSlop ) + { + // Cache hit, contact points generated + cache->hit = 1; + return; + } + } + break; + + case b3_faceAxisB: + { + B3_ASSERT( cache->indexB < hullB->faceCount ); + + // Check for separation using cached face + b3Plane plane = planesB[cache->indexB]; + b3Vec3 searchDirectionInA = b3Neg( b3RotateVector( transformBtoA.q, plane.normal ) ); + + // todo use b3GetSupportWide + int vertexIndex = b3FindHullSupportVertex( hullA, searchDirectionInA ); + b3Vec3 support = b3InvTransformPoint( transformBtoA, pointsA[vertexIndex] ); + float separation = b3PlaneSeparation( plane, support ); + + if ( separation >= speculativeDistance ) + { + // Cache hit, shapes are separated + cache->hit = 1; + return; + } + + // Attempt face contact using cached feature + b3FaceQuery faceQuery; + faceQuery.separation = 0.0f; + faceQuery.faceIndex = cache->indexB; + faceQuery.vertexIndex = vertexIndex; + + b3SATCache localCache = { 0 }; + bool touching = b3BuildFaceBContact( manifold, capacity, hullA, hullB, transformBtoA, faceQuery, &localCache ); + if ( touching == true && b3AbsFloat( cache->separation - localCache.separation ) < linearSlop ) + { + // Cache hit, contact points generated + cache->hit = 1; + return; + } + } + break; + + case b3_edgePairAxis: + { + int indexA = cache->indexA; + const b3HullHalfEdge* edge1 = edgesA + indexA; + const b3HullHalfEdge* twin1 = edgesA + indexA + 1; + B3_ASSERT( edge1->twin == indexA + 1 && twin1->twin == indexA ); + + b3Vec3 pA = pointsA[edge1->origin]; + b3Vec3 qA = pointsA[twin1->origin]; + b3Vec3 eA = b3Sub( qA, pA ); + + b3Vec3 uA = planesA[edge1->face].normal; + b3Vec3 vA = planesA[twin1->face].normal; + + int indexB = cache->indexB; + const b3HullHalfEdge* edge2 = edgesB + indexB; + const b3HullHalfEdge* twin2 = edgesB + indexB + 1; + B3_ASSERT( edge2->twin == indexB + 1 && twin2->twin == indexB ); + + b3Vec3 pB = b3TransformPoint( transformBtoA, pointsB[edge2->origin] ); + b3Vec3 qB = b3TransformPoint( transformBtoA, pointsB[twin2->origin] ); + b3Vec3 eB = b3Sub( qB, pB ); + + b3Vec3 uB = b3RotateVector( transformBtoA.q, planesB[edge2->face].normal ); + b3Vec3 vB = b3RotateVector( transformBtoA.q, planesB[twin2->face].normal ); + + // flipping the signs of u2 and v2 + // cross(v2, u2) == cross(-v2, -u2) + // so we still use -e2 + // but we can also use e1 = cross(u1, v1) and e2 = cross(u2, v2) + float cba = b3Dot( uB, eA ); + float dba = b3Dot( vB, eA ); + float adc = -b3Dot( uA, eB ); + float bdc = -b3Dot( vA, eB ); + + if ( cba * dba < 0.0f && adc * bdc < 0.0f && cba * bdc > 0.0f ) + { + // Avoid nearly parallel edges that may lead to invalid separation values at the noise floor. + float squaredTolerance = B3_PARALLEL_EDGE_TOL * B3_PARALLEL_EDGE_TOL; + if ( b3MaxFloat( cba * cba, dba * dba ) >= squaredTolerance * b3LengthSquared( eA ) ) + { + // Transform reference center of the first hull into local space of the second hull + float t = cba / ( cba - dba ); + b3Vec3 axis = b3Lerp( uB, vB, t ); + B3_VALIDATE( b3LengthSquared( axis ) > 1000.0f * FLT_MIN ); + axis = b3Normalize( axis ); + float separation = b3Dot( axis, b3Sub( qA, qB ) ); + + if ( separation > speculativeDistance ) + { + // Cache hit, shapes are separated + cache->hit = 1; + return; + } + + // Try to rebuild contact from last features + b3EdgeQuery edgeQuery = { 0 }; + edgeQuery.normal = b3Neg( axis ); + edgeQuery.separation = 0.0f; + edgeQuery.indexA = cache->indexA; + edgeQuery.indexB = cache->indexB; + + b3SATCache localCache = { 0 }; + bool touching = b3BuildEdgeContact( manifold, hullA, hullB, transformBtoA, edgeQuery, &localCache ); + + // This separation tolerance may have a big impact on performance in some benchmarks. + if ( touching && b3AbsFloat( cache->separation - localCache.separation ) < linearSlop ) + { + // Cache hit, contact point generated + cache->hit = 1; + return; + } + } + } + } + break; + + // This case is for testing + case b3_manualFaceAxisA: + { + b3AxisQuery axisQuery = b3ComputeSeparatingAxis( hullA, hullB, transformBtoA, b3_manualFaceAxisA ); + b3FaceQuery faceQuery = { + .separation = axisQuery.separation, + .faceIndex = axisQuery.indexA, + .vertexIndex = axisQuery.indexB, + }; + + b3BuildFaceAContact( manifold, capacity, hullA, hullB, transformBtoA, faceQuery, cache ); + return; + } + + // This case is for testing + case b3_manualFaceAxisB: + { + b3AxisQuery axisQuery = b3ComputeSeparatingAxis( hullA, hullB, transformBtoA, b3_manualFaceAxisB ); + b3FaceQuery faceQuery = { + .separation = axisQuery.separation, + .faceIndex = axisQuery.indexB, + .vertexIndex = axisQuery.indexA, + }; + b3BuildFaceBContact( manifold, capacity, hullA, hullB, transformBtoA, faceQuery, cache ); + return; + } + + // This case is for testing + case b3_manualEdgePairAxis: + { + b3AxisQuery axisQuery = b3ComputeSeparatingAxis( hullA, hullB, transformBtoA, b3_manualEdgePairAxis ); + b3EdgeQuery edgeQuery = { + .normal = axisQuery.normal, + .indexA = axisQuery.indexA, + .indexB = axisQuery.indexB, + .separation = axisQuery.separation, + }; + + if ( edgeQuery.indexA != B3_NULL_INDEX ) + { + b3BuildEdgeContact( manifold, hullA, hullB, transformBtoA, edgeQuery, cache ); + } + return; + } + + default: + B3_ASSERT( false ); + break; + } + + manifold->pointCount = 0; + *cache = (b3SATCache){ 0 }; + + b3AxisQuery axisQuery = b3ComputeSeparatingAxis( hullA, hullB, transformBtoA, b3_invalidAxis ); + + B3_VALIDATE( 0 <= axisQuery.indexA && axisQuery.indexA <= UINT8_MAX ); + B3_VALIDATE( 0 <= axisQuery.indexB && axisQuery.indexB <= UINT8_MAX ); + B3_ASSERT( axisQuery.type != b3_invalidAxis ); + + cache->separation = axisQuery.separation; + cache->type = (uint8_t)axisQuery.type; + cache->indexA = (uint8_t)axisQuery.indexA; + cache->indexB = (uint8_t)axisQuery.indexB; + + if ( axisQuery.separation > speculativeDistance ) + { + // We found a separating axis + return; + } + + if ( axisQuery.type == b3_faceAxisA ) + { + B3_ASSERT( axisQuery.indexA < hullA->faceCount ); + B3_ASSERT( axisQuery.indexB < hullB->vertexCount ); + + b3FaceQuery faceQuery = { + .separation = axisQuery.separation, + .faceIndex = axisQuery.indexA, + .vertexIndex = axisQuery.indexB, + }; + + // Face contact A + b3BuildFaceAContact( manifold, capacity, hullA, hullB, transformBtoA, faceQuery, cache ); + + return; + } + + if ( axisQuery.type == b3_faceAxisB ) + { + B3_ASSERT( axisQuery.indexA < hullA->vertexCount ); + B3_ASSERT( axisQuery.indexB < hullB->faceCount ); + + b3FaceQuery faceQuery = { + .separation = axisQuery.separation, + .faceIndex = axisQuery.indexB, + .vertexIndex = axisQuery.indexA, + }; + + // Face contact B + b3BuildFaceBContact( manifold, capacity, hullA, hullB, transformBtoA, faceQuery, cache ); + + return; + } + + B3_ASSERT( axisQuery.type == b3_edgePairAxis ); + + { + // Edge contact + b3LocalManifold edgeManifold = { 0 }; + b3LocalManifoldPoint edgePoint = { 0 }; + edgeManifold.points = &edgePoint; + + b3EdgeQuery edgeQuery = { + .normal = axisQuery.normal, + .indexA = axisQuery.indexA, + .indexB = axisQuery.indexB, + .separation = axisQuery.separation, + }; + + b3BuildEdgeContact( &edgeManifold, hullA, hullB, transformBtoA, edgeQuery, cache ); + + if ( edgeManifold.pointCount == 1 ) + { + // Copy edge manifold out, being careful to preserve manifold point buffer. + b3LocalManifoldPoint* points = manifold->points; + *manifold = edgeManifold; + manifold->points = points; + manifold->points[0] = edgePoint; + } + } +} + +#else + +// Old non-SIMD version. Keeping this for testing and comparisons + +static b3FaceQuery b3QueryFaceDirections( const b3HullData* hullA, const b3HullData* hullB, b3Transform relativeTransform ) +{ + // We perform all computations in local space of the second hull + b3Transform transform = b3InvertTransform( relativeTransform ); + const b3Plane* planesA = b3GetHullPlanes( hullA ); + const b3Vec3* pointsB = b3GetHullPoints( hullB ); + + int maxFaceIndex = -1; + int maxVertexIndex = -1; + float maxFaceSeparation = -FLT_MAX; + float speculativeDistance = B3_SPECULATIVE_DISTANCE; + + for ( int faceIndex = 0; faceIndex < hullA->faceCount; ++faceIndex ) + { + b3Plane plane = b3TransformPlane( transform, planesA[faceIndex] ); + int vertexIndex = b3FindHullSupportVertex( hullB, b3Neg( plane.normal ) ); + b3Vec3 support = pointsB[vertexIndex]; + + float separation = b3PlaneSeparation( plane, support ); + if ( separation > maxFaceSeparation ) + { + maxFaceIndex = faceIndex; + maxVertexIndex = vertexIndex; + maxFaceSeparation = separation; + + if ( separation >= speculativeDistance ) + { + return (b3FaceQuery){ + .separation = maxFaceSeparation, + .faceIndex = (uint8_t)maxFaceIndex, + .vertexIndex = (uint8_t)maxVertexIndex, + }; + } + } + } + + return (b3FaceQuery){ + .separation = maxFaceSeparation, + .faceIndex = (uint8_t)maxFaceIndex, + .vertexIndex = (uint8_t)maxVertexIndex, + }; +} + +static b3EdgeQuery b3QueryEdgeDirections( const b3HullData* hullA, const b3HullData* hullB, b3Transform transformBtoA ) +{ + // Find axis of minimum penetration + b3Vec3 maxNormal = b3Vec3_zero; + float maxSeparation = -FLT_MAX; + int maxIndexA = B3_NULL_INDEX; + int maxIndexB = B3_NULL_INDEX; + + const b3HullHalfEdge* edgesA = b3GetHullEdges( hullA ); + const b3Vec3* pointsA = b3GetHullPoints( hullA ); + const b3Plane* planesA = b3GetHullPlanes( hullA ); + const b3HullHalfEdge* edgesB = b3GetHullEdges( hullB ); + const b3Vec3* pointsB = b3GetHullPoints( hullB ); + const b3Plane* planesB = b3GetHullPlanes( hullB ); + + // Work in frame A + b3Matrix3 matrix = b3MakeMatrixFromQuat( transformBtoA.q ); + float speculativeDistance = B3_SPECULATIVE_DISTANCE; + float squaredTolerance = B3_PARALLEL_EDGE_TOL * B3_PARALLEL_EDGE_TOL; + + // Arranged to minimize transform operations + for ( int indexB = 0; indexB < hullB->edgeCount; indexB += 2 ) + { + const b3HullHalfEdge* edgeB = edgesB + indexB; + const b3HullHalfEdge* twinB = edgesB + indexB + 1; + B3_ASSERT( edgeB->twin == indexB + 1 && twinB->twin == indexB ); + + b3Vec3 qB = pointsB[twinB->origin]; + b3Vec3 eB = b3MulMV( matrix, b3Sub( qB, pointsB[edgeB->origin] ) ); + qB = b3Add( b3MulMV( matrix, qB ), transformBtoA.p ); + + b3Vec3 uB = b3MulMV( matrix, planesB[edgeB->face].normal ); + b3Vec3 vB = b3MulMV( matrix, planesB[twinB->face].normal ); + + for ( int indexA = 0; indexA < hullA->edgeCount; indexA += 2 ) + { + const b3HullHalfEdge* edgeA = edgesA + indexA; + const b3HullHalfEdge* twinA = edgesA + indexA + 1; + B3_ASSERT( edgeA->twin == indexA + 1 && twinA->twin == indexA ); + + b3Vec3 qA = pointsA[twinA->origin]; + b3Vec3 eA = b3Sub( qA, pointsA[edgeA->origin] ); + b3Vec3 uA = planesA[edgeA->face].normal; + b3Vec3 vA = planesA[twinA->face].normal; + + // See "Collision Detection of Convex Polyhedra Based on Duality Transformation" + // Two edges build a face on the Minkowski sum if the associated arcs AB and CD intersect on the Gauss map. + // The associated arcs are defined by the adjacent face normals of each edge. + + // These are signed volumes with an edge optimization to avoid cross products + // eA parallel to cross(vA, uA) + // eB parallel to cross(vB, uB) + // Since only signs are tested, length doesn't matter. + + float cba = b3Dot( uB, eA ); + float dba = b3Dot( vB, eA ); + float adc = -b3Dot( uA, eB ); + float bdc = -b3Dot( vA, eB ); + + if ( cba * dba < 0.0f && adc * bdc < 0.0f && cba * bdc > 0.0f ) + { + // Avoid nearly parallel edges that may lead to invalid separation values at the noise floor. + if ( b3MaxFloat( cba * cba, dba * dba ) < squaredTolerance * b3LengthSquared( eA ) ) + { + continue; + } + + // The intersection of the arcs on the Gauss map is the edge pair axis. Cast the + // arc of hull B (from uB to vB) against the plane containing the arc of hull A: + // dot(uB + t * (vB - uB), eA) == 0 + // then + // t = cba / (cba - dba) + // + // The signs of cba and dba differ (Minkowski test), so the division is safe. + // + // The axis generated points from B to A by construction since it lands between + // two face normals on B. This removes the need to orient the separation axis + // using the hull centers. + // + // The axis is perpendicular to both edges so I can use qA and qB as arbitrary + // points on edgeA and edgeB to measure the separation. + float t = cba / ( cba - dba ); + b3Vec3 axis = b3Lerp( uB, vB, t ); + B3_VALIDATE( b3LengthSquared( axis ) > 1000.0f * FLT_MIN ); + axis = b3Normalize( axis ); + float separation = b3Dot( axis, b3Sub( qA, qB ) ); + + if ( separation > maxSeparation ) + { + // Continues to find the maximum separating axis + // Flip normal so it points from A to B + maxNormal = b3Neg( axis ); + maxSeparation = separation; + maxIndexA = indexA; + maxIndexB = indexB; + + if ( separation >= speculativeDistance ) + { + // Cache hit, shapes are separated + return (b3EdgeQuery){ + .normal = maxNormal, + .separation = maxSeparation, + .indexA = maxIndexA, + .indexB = maxIndexB, + }; + } + } + } + } + } + + return (b3EdgeQuery){ + .normal = maxNormal, + .separation = maxSeparation, + .indexA = maxIndexA, + .indexB = maxIndexB, + }; +} + void b3CollideHulls( b3LocalManifold* manifold, int capacity, const b3HullData* hullA, const b3HullData* hullB, b3Transform transformBtoA, b3SATCache* cache ) { @@ -1460,7 +2391,7 @@ void b3CollideHulls( b3LocalManifold* manifold, int capacity, const b3HullData* if ( cba * dba < 0.0f && adc * bdc < 0.0f && cba * bdc > 0.0f ) { // Avoid nearly parallel edges that may lead to invalid separation values at the noise floor. - float squaredTolerance = 0.005f * 0.005f; + float squaredTolerance = B3_PARALLEL_EDGE_TOL * B3_PARALLEL_EDGE_TOL; if ( b3MaxFloat( cba * cba, dba * dba ) >= squaredTolerance * b3LengthSquared( eA ) ) { // Transform reference center of the first hull into local space of the second hull @@ -1626,3 +2557,5 @@ void b3CollideHulls( b3LocalManifold* manifold, int capacity, const b3HullData* } } } + +#endif diff --git a/src/height_field.c b/src/height_field.c index 2bf5d19..69c2474 100644 --- a/src/height_field.c +++ b/src/height_field.c @@ -1382,7 +1382,7 @@ b3HeightFieldData* b3CreateGrid( int rowCount, int columnCount, b3Vec3 scale, bo } b3HeightFieldData* b3CreateWave( int rowCount, int columnCount, b3Vec3 scale, float rowFrequency, float columnFrequency, - bool makeHoles ) + bool makeHoles ) { int heightCount = rowCount * columnCount; float* heights = (float*)b3Alloc( heightCount * sizeof( float ) ); @@ -1390,14 +1390,18 @@ b3HeightFieldData* b3CreateWave( int rowCount, int columnCount, b3Vec3 scale, fl float omegaZ = 2.0f * B3_PI * rowFrequency; float omegaX = 2.0f * B3_PI * columnFrequency; + b3CosSin cs = { 0 }; + for ( int i = 0; i < rowCount; ++i ) { - float rowHeight = sinf( omegaZ * i ); + cs = b3ComputeCosSin( omegaZ * i ); + float rowHeight = cs.sine; for ( int j = 0; j < columnCount; ++j ) { int k = i * columnCount + j; - float columnHeight = sinf( omegaX * j ); + cs = b3ComputeCosSin( omegaX * j ); + float columnHeight = cs.sine; heights[k] = rowHeight * columnHeight; } } diff --git a/src/hull.c b/src/hull.c index 75bace9..12d3f1e 100644 --- a/src/hull.c +++ b/src/hull.c @@ -1421,7 +1421,8 @@ static bool b3HullBuilder_Construct( b3HullBuilder* b, const b3Vec3* points, int } b3HullBuilder_ComputeTolerance( b, pointCount, shiftedPoints ); - if ( !b3HullBuilder_BuildInitialHull( b, pointCount, shiftedPoints ) ) + bool haveInitialHull = b3HullBuilder_BuildInitialHull( b, pointCount, shiftedPoints ); + if ( haveInitialHull == false ) { return false; } @@ -1607,6 +1608,24 @@ static b3HullHalfEdge* b3GetHullEdgesWrite( b3HullData* hull ) return (b3HullHalfEdge*)( (intptr_t)hull + hull->edgeOffset ); } +static float* b3GetHullSoaVerticesWrite( b3HullData* hull ) +{ + if ( hull->soaVertexOffset == 0 ) + { + return NULL; + } + return (float*)( (intptr_t)hull + hull->soaVertexOffset ); +} + +static float* b3GetHullSoaNormalsWrite( b3HullData* hull ) +{ + if ( hull->soaNormalOffset == 0 ) + { + return NULL; + } + return (float*)( (intptr_t)hull + hull->soaNormalOffset ); +} + int b3FindHullSupportVertex( const b3HullData* hull, b3Vec3 direction ) { int bestIndex = B3_NULL_INDEX; @@ -2028,7 +2047,7 @@ b3HullData* b3CreateHull( const b3Vec3* points, int pointCount, int maxVertexCou } b3Vec3 origin = points[0]; - int clampedMaxCount = b3ClampInt( maxVertexCount, 4, B3_HULL_LIMIT ); + int clampedMaxCount = b3ClampInt( maxVertexCount, 4, B3_MAX_HULL_VERTICES ); // Single allocation for all working memory. b3HullWorkSizes sizes = b3ComputeHullWorkSizes( pointCount, clampedMaxCount ); @@ -2040,29 +2059,30 @@ b3HullData* b3CreateHull( const b3Vec3* points, int pointCount, int maxVertexCou b3Vec3* shiftedPoints = (b3Vec3*)( work + sizes.offsetShiftedPoints ); bool ok = b3HullBuilder_Construct( &builder, points, pointCount, clampedMaxCount, origin, shiftedPoints ); - if ( !ok ) + if ( ok == false ) { b3Free( work, sizes.totalBytes ); return NULL; } - if ( builder.finalVertexCount >= B3_HULL_LIMIT ) + if ( builder.finalVertexCount >= B3_MAX_HULL_VERTICES ) { - b3Log( "hull final vertex count of %d exceeds limit of %d", builder.finalVertexCount, B3_HULL_LIMIT ); + b3Log( "hull final vertex count of %d exceeds limit of %d", builder.finalVertexCount, B3_MAX_HULL_VERTICES ); b3Free( work, sizes.totalBytes ); return NULL; } - if ( builder.finalFaceCount >= B3_HULL_LIMIT ) + if ( builder.finalFaceCount >= B3_MAX_HULL_FACES ) { - b3Log( "hull final face count of %d exceeds limit of %d", builder.finalFaceCount, B3_HULL_LIMIT ); + b3Log( "hull final face count of %d exceeds limit of %d", builder.finalFaceCount, B3_MAX_HULL_FACES ); b3Free( work, sizes.totalBytes ); return NULL; } - if ( builder.finalHalfEdgeCount >= B3_HULL_LIMIT ) + int maxHalfEdgeCount = 2 * B3_MAX_HULL_EDGES; + if ( builder.finalHalfEdgeCount >= maxHalfEdgeCount ) { - b3Log( "hull final half edge count of %d exceeds limit of %d", builder.finalHalfEdgeCount, B3_HULL_LIMIT ); + b3Log( "hull final half edge count of %d exceeds limit of %d", builder.finalHalfEdgeCount, maxHalfEdgeCount ); b3Free( work, sizes.totalBytes ); return NULL; } @@ -2112,6 +2132,9 @@ b3HullData* b3CreateHull( const b3Vec3* points, int pointCount, int maxVertexCou while ( edge != face->edge ); } + int soaVertexCount = ( vertexCount + 3 ) & ~3; + int soaNormalCount = ( faceCount + 3 ) & ~3; + // Allocate the hull. Arrays hang off the end. size_t byteCount = b3AlignUp8( sizeof( b3HullData ) ); int vertexOffset = (int)byteCount; @@ -2120,10 +2143,14 @@ b3HullData* b3CreateHull( const b3Vec3* points, int pointCount, int maxVertexCou byteCount += b3AlignUp8( vertexCount * (int)sizeof( b3Vec3 ) ); int edgeOffset = (int)byteCount; byteCount += b3AlignUp8( edgeCount * (int)sizeof( b3HullHalfEdge ) ); - int faceOffset = (int)byteCount; - byteCount += b3AlignUp8( faceCount * (int)sizeof( b3HullFace ) ); int planeOffset = (int)byteCount; byteCount += b3AlignUp8( faceCount * (int)sizeof( b3Plane ) ); + int faceOffset = (int)byteCount; + byteCount += b3AlignUp8( faceCount * (int)sizeof( b3HullFace ) ); + int soaVertexOffset = (int)byteCount; + byteCount += b3AlignUp8( 3 * soaVertexCount * (int)sizeof( float ) ); + int soaNormalOffset = (int)byteCount; + byteCount += b3AlignUp8( 3 * soaNormalCount * (int)sizeof( float ) ); b3HullData* hull = b3Alloc( byteCount ); memset( hull, 0, byteCount ); @@ -2132,8 +2159,10 @@ b3HullData* b3CreateHull( const b3Vec3* points, int pointCount, int maxVertexCou hull->vertexOffset = vertexOffset; hull->pointOffset = pointOffset; hull->edgeOffset = edgeOffset; - hull->faceOffset = faceOffset; hull->planeOffset = planeOffset; + hull->faceOffset = faceOffset; + hull->soaVertexOffset = soaVertexOffset; + hull->soaNormalOffset = soaNormalOffset; hull->vertexCount = vertexCount; hull->edgeCount = edgeCount; @@ -2146,11 +2175,29 @@ b3HullData* b3CreateHull( const b3Vec3* points, int pointCount, int maxVertexCou b3HullFace* faces = (b3HullFace*)( (intptr_t)hull + hull->faceOffset ); b3Vec3* finalPoints = b3GetHullPointsWrite( hull ); b3Plane* planes = b3GetHullPlanesWrite( hull ); + float* soaVertices = b3GetHullSoaVerticesWrite( hull ); + float* soaNormals = b3GetHullSoaNormalsWrite( hull ); + + float* vx = soaVertices; + float* vy = vx + soaVertexCount; + float* vz = vy + soaVertexCount; for ( int index = 0; index < vertexCount; ++index ) { vertices[index].edge = 0; - finalPoints[index] = tempVertices[index]->position; + b3Vec3 p = tempVertices[index]->position; + finalPoints[index] = p; + + vx[index] = p.x; + vy[index] = p.y; + vz[index] = p.z; + } + + for ( int index = vertexCount; index < soaVertexCount; ++index ) + { + vx[index] = vx[0]; + vy[index] = vy[0]; + vz[index] = vz[0]; } for ( int index = 0; index < edgeCount; ++index ) @@ -2169,6 +2216,10 @@ b3HullData* b3CreateHull( const b3Vec3* points, int pointCount, int maxVertexCou vertices[edge->origin->finalIndex].edge = (uint8_t)index; } + float* nx = soaNormals; + float* ny = nx + soaNormalCount; + float* nz = ny + soaNormalCount; + for ( int index = 0; index < faceCount; ++index ) { const b3QHFace* face = tempFaces[index]; @@ -2176,6 +2227,18 @@ b3HullData* b3CreateHull( const b3Vec3* points, int pointCount, int maxVertexCou faces[index].edge = (uint8_t)face->edge->finalIndex; planes[index] = face->plane; + + b3Vec3 n = face->plane.normal; + nx[index] = n.x; + ny[index] = n.y; + nz[index] = n.z; + } + + for ( int index = faceCount; index < soaNormalCount; ++index ) + { + nx[index] = 0.0f; + ny[index] = 0.0f; + nz[index] = 0.0f; } // All builder pointers are dead from here on. @@ -2238,8 +2301,8 @@ bool b3CompareHullData( const b3HullData* hull1, const b3HullData* hull2 ) // Hull identity covers every byte, so the structs carry explicit padding. These lock // the layout, re-audit padding if a size changes. -_Static_assert( sizeof( b3HullData ) == 136, "unexpected hull data size" ); -_Static_assert( sizeof( b3BoxHull ) == 440, "unexpected box hull size" ); +_Static_assert( sizeof( b3HullData ) == 144, "unexpected hull data size" ); +_Static_assert( sizeof( b3BoxHull ) == 648, "unexpected box hull size" ); #define NAME b3HullMap #define KEY_TY const b3HullData* @@ -2269,7 +2332,7 @@ b3HullData* b3CloneAndTransformHull( const b3HullData* original, b3Transform tra return NULL; } - b3HullData* hull = (b3HullData*)b3Alloc( original->byteCount ); + b3HullData* hull = b3Alloc( original->byteCount ); memcpy( hull, original, original->byteCount ); b3Vec3 safeScale = b3SafeScale( scale ); @@ -2339,12 +2402,32 @@ b3HullData* b3CloneAndTransformHull( const b3HullData* original, b3Transform tra b3Matrix3 matrix = b3MakeMatrixFromQuat( transform.q ); b3Vec3* points = b3GetHullPointsWrite( hull ); + int soaVertexCount = ( vertexCount + 3 ) & ~3; + float* vx = b3GetHullSoaVerticesWrite( hull ); + float* vy = vx + soaVertexCount; + float* vz = vy + soaVertexCount; + for ( int i = 0; i < vertexCount; ++i ) { - points[i] = b3Add( b3MulMV( matrix, b3Mul( safeScale, points[i] ) ), transform.p ); + b3Vec3 p = b3Add( b3MulMV( matrix, b3Mul( safeScale, points[i] ) ), transform.p ); + points[i] = p; + vx[i] = p.x; + vy[i] = p.y; + vz[i] = p.z; + } + + for ( int i = vertexCount; i < soaVertexCount; ++i ) + { + vx[i] = vx[0]; + vy[i] = vy[0]; + vz[i] = vz[0]; } b3Plane* planes = b3GetHullPlanesWrite( hull ); + int soaNormalCount = ( faceCount + 3 ) & ~3; + float* nx = b3GetHullSoaNormalsWrite( hull ); + float* ny = nx + soaNormalCount; + float* nz = ny + soaNormalCount; for ( int i = 0; i < faceCount; ++i ) { @@ -2390,6 +2473,17 @@ b3HullData* b3CloneAndTransformHull( const b3HullData* original, b3Transform tra normal = b3MulSV( 1.0f / area, normal ); planes[i] = b3MakePlaneFromNormalAndPoint( normal, centroid ); + + nx[i] = normal.x; + ny[i] = normal.y; + nz[i] = normal.z; + } + + for ( int i = faceCount; i < soaNormalCount; ++i ) + { + nx[i] = 0.0f; + ny[i] = 0.0f; + nz[i] = 0.0f; } b3UpdateHullBounds( hull ); @@ -2641,8 +2735,10 @@ static const b3BoxHull s_boxHull = { .vertexOffset = offsetof( b3BoxHull, boxVertices ), .pointOffset = offsetof( b3BoxHull, boxPoints ), .edgeOffset = offsetof( b3BoxHull, boxEdges ), - .faceOffset = offsetof( b3BoxHull, boxFaces ), .planeOffset = offsetof( b3BoxHull, boxPlanes ), + .faceOffset = offsetof( b3BoxHull, boxFaces ), + .soaVertexOffset = offsetof( b3BoxHull, vx ), + .soaNormalOffset = offsetof( b3BoxHull, nx ), }, .boxVertices = { @@ -2710,6 +2806,61 @@ b3BoxHull b3MakeTransformedBoxHull( float hx, float hy, float hz, b3Transform tr boxHull.boxPoints[6] = b3TransformPoint( transform, (b3Vec3){ -h.x, -h.y, -h.z } ); boxHull.boxPoints[7] = b3TransformPoint( transform, (b3Vec3){ h.x, -h.y, -h.z } ); + // SOA + boxHull.vx[0] = boxHull.boxPoints[0].x; + boxHull.vx[1] = boxHull.boxPoints[1].x; + boxHull.vx[2] = boxHull.boxPoints[2].x; + boxHull.vx[3] = boxHull.boxPoints[3].x; + boxHull.vx[4] = boxHull.boxPoints[4].x; + boxHull.vx[5] = boxHull.boxPoints[5].x; + boxHull.vx[6] = boxHull.boxPoints[6].x; + boxHull.vx[7] = boxHull.boxPoints[7].x; + + boxHull.vy[0] = boxHull.boxPoints[0].y; + boxHull.vy[1] = boxHull.boxPoints[1].y; + boxHull.vy[2] = boxHull.boxPoints[2].y; + boxHull.vy[3] = boxHull.boxPoints[3].y; + boxHull.vy[4] = boxHull.boxPoints[4].y; + boxHull.vy[5] = boxHull.boxPoints[5].y; + boxHull.vy[6] = boxHull.boxPoints[6].y; + boxHull.vy[7] = boxHull.boxPoints[7].y; + + boxHull.vz[0] = boxHull.boxPoints[0].z; + boxHull.vz[1] = boxHull.boxPoints[1].z; + boxHull.vz[2] = boxHull.boxPoints[2].z; + boxHull.vz[3] = boxHull.boxPoints[3].z; + boxHull.vz[4] = boxHull.boxPoints[4].z; + boxHull.vz[5] = boxHull.boxPoints[5].z; + boxHull.vz[6] = boxHull.boxPoints[6].z; + boxHull.vz[7] = boxHull.boxPoints[7].z; + + boxHull.nx[0] = boxHull.boxPlanes[0].normal.x; + boxHull.nx[1] = boxHull.boxPlanes[1].normal.x; + boxHull.nx[2] = boxHull.boxPlanes[2].normal.x; + boxHull.nx[3] = boxHull.boxPlanes[3].normal.x; + boxHull.nx[4] = boxHull.boxPlanes[4].normal.x; + boxHull.nx[5] = boxHull.boxPlanes[5].normal.x; + boxHull.nx[6] = 0.0f; + boxHull.nx[7] = 0.0f; + + boxHull.ny[0] = boxHull.boxPlanes[0].normal.y; + boxHull.ny[1] = boxHull.boxPlanes[1].normal.y; + boxHull.ny[2] = boxHull.boxPlanes[2].normal.y; + boxHull.ny[3] = boxHull.boxPlanes[3].normal.y; + boxHull.ny[4] = boxHull.boxPlanes[4].normal.y; + boxHull.ny[5] = boxHull.boxPlanes[5].normal.y; + boxHull.ny[6] = 0.0f; + boxHull.ny[7] = 0.0f; + + boxHull.nz[0] = boxHull.boxPlanes[0].normal.z; + boxHull.nz[1] = boxHull.boxPlanes[1].normal.z; + boxHull.nz[2] = boxHull.boxPlanes[2].normal.z; + boxHull.nz[3] = boxHull.boxPlanes[3].normal.z; + boxHull.nz[4] = boxHull.boxPlanes[4].normal.z; + boxHull.nz[5] = boxHull.boxPlanes[5].normal.z; + boxHull.nz[6] = 0.0f; + boxHull.nz[7] = 0.0f; + boxHull.base.hash = 0; boxHull.base.hash = b3NonZeroHash( b3Hash( B3_HASH_INIT, (uint8_t*)&boxHull, sizeof( b3BoxHull ) ) ); diff --git a/src/manifold.h b/src/manifold.h index 5ed0a5f..2eebbda 100644 --- a/src/manifold.h +++ b/src/manifold.h @@ -23,6 +23,15 @@ typedef struct b3EdgeQuery int indexB; } b3EdgeQuery; +typedef struct b3AxisQuery +{ + b3Vec3 normal; + float separation; + int indexA; + int indexB; + b3SeparatingFeature type; +} b3AxisQuery; + typedef struct b3ClipVertex { b3Vec3 position; @@ -43,6 +52,8 @@ 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, int axisOverride ); + #if B3_ENABLE_VALIDATION bool b3ValidatePolygon( b3ClipVertex* polygon, int count ); #endif diff --git a/src/math_internal.h b/src/math_internal.h index 4d152c5..d975c65 100644 --- a/src/math_internal.h +++ b/src/math_internal.h @@ -61,6 +61,7 @@ b3Matrix3 b3BoxInertia( float mass, b3Vec3 min, b3Vec3 max ); int b3GetProxySupport( const b3ShapeProxy* proxy, b3Vec3 axis ); int b3GetPointSupport( const b3Vec3* points, int count, b3Vec3 axis ); +// Align up to 8 byte alignment. static inline size_t b3AlignUp8( size_t x ) { return ( x + 7u ) & ~(size_t)7u; diff --git a/src/mesh.c b/src/mesh.c index a6628a8..acfbda0 100644 --- a/src/mesh.c +++ b/src/mesh.c @@ -1322,14 +1322,17 @@ b3MeshData* b3CreateWaveMesh( int xCount, int zCount, float cellWidth, float amp float omegaX = 2.0f * B3_PI * columnFrequency * cellWidth; float x = -0.5f * xWidth; + b3CosSin cs = { 0 }; for ( int ix = 0; ix <= xCount; ++ix ) { - float rowHeight = sinf( omegaX * ix ); + cs = b3ComputeCosSin( omegaX * ix ); + float rowHeight = cs.sine; float z = -0.5f * zWidth; for ( int iz = 0; iz <= zCount; ++iz ) { - float columnHeight = sinf( omegaZ * iz ); + cs = b3ComputeCosSin( omegaZ * iz ); + float columnHeight = cs.sine; float y = amplitude * rowHeight * columnHeight; vertices.data[index] = (b3Vec3){ x, y, z }; @@ -1483,7 +1486,7 @@ b3MeshData* b3CreateBoxMesh( b3Vec3 center, b3Vec3 extent, bool identifyEdges ) return b3CreateMesh( &def, NULL, 0 ); } -b3MeshData* b3CreateHollowBoxMesh(b3Vec3 center, b3Vec3 extent) +b3MeshData* b3CreateHollowBoxMesh( b3Vec3 center, b3Vec3 extent ) { float x = extent.x; float y = extent.y; diff --git a/src/parallel_for.c b/src/parallel_for.c index 07d1c3f..c16ac18 100644 --- a/src/parallel_for.c +++ b/src/parallel_for.c @@ -77,7 +77,8 @@ void b3ParallelFor( b3World* world, b3ParallelForCallback* callback, int itemCou // Target multiple blocks per worker to reduce thread stalls. // block size grows once items exceed maxBlockCount * minRange // so the block count stays bounded and per-block sync overhead stays low. - int blocksPerWorker = 4; + // Benchmarking shows 32 is optimal for the convex pile benchmark and others. + int blocksPerWorker = 32; int maxBlockCount = blocksPerWorker * workerCount; int blockSize; diff --git a/src/recording_ops.inl b/src/recording_ops.inl index b8a3625..88acf0a 100644 --- a/src/recording_ops.inl +++ b/src/recording_ops.inl @@ -68,6 +68,7 @@ B3_REC_OP( 0x36, BodySetMotionLocks, RET_NONE, ARG( BODYID, body ) ARG( LOCKS, l B3_REC_OP( 0x37, BodySetBullet, RET_NONE, ARG( BODYID, body ) ARG( BOOL, flag ) ) B3_REC_OP( 0x38, BodyEnableContactRecycling, RET_NONE, ARG( BODYID, body ) ARG( BOOL, flag ) ) B3_REC_OP( 0x39, BodyEnableHitEvents, RET_NONE, ARG( BODYID, body ) ARG( BOOL, flag ) ) +B3_REC_OP( 0x3A, BodyAllowFastRotation, RET_NONE, ARG( BODYID, body ) ARG( BOOL, flag ) ) // Shape create/destroy B3_REC_OP( 0x40, CreateSphereShape, RET_SHAPEID, ARG( BODYID, body ) ARG( SHAPEDEF, def ) ARG( SPHERE, sphere ) ) diff --git a/src/recording_replay.c b/src/recording_replay.c index adf4ec8..c3cc491 100644 --- a/src/recording_replay.c +++ b/src/recording_replay.c @@ -957,6 +957,11 @@ static void b3RecDispatch_BodySetBullet( const b3RecArgs_BodySetBullet* a, b3Rec b3Body_SetBullet( b3RecMakeBodyId( rdr, a->body ), a->flag ); } +static void b3RecDispatch_BodyAllowFastRotation( const b3RecArgs_BodyAllowFastRotation* a, b3RecReader* rdr ) +{ + b3Body_AllowFastRotation( b3RecMakeBodyId( rdr, a->body ), a->flag ); +} + static void b3RecDispatch_BodyEnableContactRecycling( const b3RecArgs_BodyEnableContactRecycling* a, b3RecReader* rdr ) { b3Body_EnableContactRecycling( b3RecMakeBodyId( rdr, a->body ), a->flag ); diff --git a/src/simd.h b/src/simd.h index ac7e9e8..6e313a0 100644 --- a/src/simd.h +++ b/src/simd.h @@ -7,10 +7,35 @@ #include -#if defined( B3_SIMD_SSE2 ) +#if defined( B3_SIMD_NEON ) + +#include + +// wide float holds 4 numbers +typedef float32x4_t b3FloatW; + +#elif defined( B3_SIMD_SSE2 ) #include +// wide float holds 4 numbers +typedef __m128 b3FloatW; + +#else + +#include +#include + +// scalar math +typedef struct b3FloatW +{ + float x, y, z, w; +} b3FloatW; + +#endif + +#if defined( B3_SIMD_SSE2 ) + // wide float holds 4 numbers typedef __m128 b3V32; @@ -355,3 +380,544 @@ static inline bool b3TestBoundsRayOverlap( b3V32 nodeMin, b3V32 nodeMax, b3V32 r bool b3TestBoundsTriangleOverlap( b3V32 nodeCenter, b3V32 nodeExtent, b3V32 vertex1, b3V32 vertex2, b3V32 vertex3 ); float b3IntersectRayTriangle( b3V32 rayStart, b3V32 rayDelta, b3V32 vertex1, b3V32 vertex2, b3V32 vertex3 ); + +#if defined( B3_SIMD_NEON ) + +static inline b3FloatW b3ZeroW( void ) +{ + return vdupq_n_f32( 0.0f ); +} + +static inline b3FloatW b3SplatW( float scalar ) +{ + return vdupq_n_f32( scalar ); +} + +static inline b3FloatW b3SetW( float a, float b, float c, float d ) +{ + float32_t array[4] = { a, b, c, d }; + return vld1q_f32( array ); +} + +static inline b3FloatW b3LoadW( const float* data ) +{ + return vld1q_f32( data ); +} + +static inline void b3StoreW( float* data, b3FloatW a ) +{ + vst1q_f32( data, a ); +} + +static inline b3FloatW b3NegW( b3FloatW a ) +{ + return vnegq_f32( a ); +} + +static inline b3FloatW b3AddW( b3FloatW a, b3FloatW b ) +{ + return vaddq_f32( a, b ); +} + +static inline b3FloatW b3SubW( b3FloatW a, b3FloatW b ) +{ + return vsubq_f32( a, b ); +} + +static inline b3FloatW b3MulW( b3FloatW a, b3FloatW b ) +{ + return vmulq_f32( a, b ); +} + +static inline b3FloatW b3DivW( b3FloatW a, b3FloatW b ) +{ + return vdivq_f32( a, b ); +} + +static inline b3FloatW b3SqrtW( b3FloatW a ) +{ + return vsqrtq_f32( a ); +} + +// Cannot use real FMA because it doesn't match the non-SIMD path +static inline b3FloatW b3MulAddW( b3FloatW a, b3FloatW b, b3FloatW c ) +{ + return vaddq_f32( a, vmulq_f32( b, c ) ); +} + +static inline b3FloatW b3MinW( b3FloatW a, b3FloatW b ) +{ + return vminq_f32( a, b ); +} + +static inline b3FloatW b3MaxW( b3FloatW a, b3FloatW b ) +{ + return vmaxq_f32( a, b ); +} + +// clamp a to [-b, b] +static inline b3FloatW b3SymClampW( b3FloatW a, b3FloatW b ) +{ + b3FloatW nb = b3NegW( b ); + b3FloatW c = b3MaxW( nb, a ); + return b3MinW( c, b ); +} + +static inline b3FloatW b3AndW( b3FloatW a, b3FloatW b ) +{ + return vreinterpretq_f32_u32( vandq_u32( vreinterpretq_u32_f32( a ), vreinterpretq_u32_f32( b ) ) ); +} + +static inline b3FloatW b3OrW( b3FloatW a, b3FloatW b ) +{ + return vreinterpretq_f32_u32( vorrq_u32( vreinterpretq_u32_f32( a ), vreinterpretq_u32_f32( b ) ) ); +} + +static inline b3FloatW b3GreaterThanW( b3FloatW a, b3FloatW b ) +{ + return vreinterpretq_f32_u32( vcgtq_f32( a, b ) ); +} + +static inline b3FloatW b3LessThanW( b3FloatW a, b3FloatW b ) +{ + return vreinterpretq_f32_u32( vcltq_f32( a, b ) ); +} + +static inline b3FloatW b3EqualsW( b3FloatW a, b3FloatW b ) +{ + return vreinterpretq_f32_u32( vceqq_f32( a, b ) ); +} + +static inline bool b3AllZeroW( b3FloatW a ) +{ + // Create a zero vector for comparison + b3FloatW zero = vdupq_n_f32( 0.0f ); + + // Compare the input vector with zero + uint32x4_t cmp_result = vceqq_f32( a, zero ); + +// Check if all comparison results are non-zero using vminvq +#ifdef __ARM_FEATURE_SVE + // ARM v8.2+ has horizontal minimum instruction + return vminvq_u32( cmp_result ) != 0; +#else + // For older ARM architectures, we need to manually check all lanes + return vgetq_lane_u32( cmp_result, 0 ) != 0 && vgetq_lane_u32( cmp_result, 1 ) != 0 && vgetq_lane_u32( cmp_result, 2 ) != 0 && + vgetq_lane_u32( cmp_result, 3 ) != 0; +#endif +} + +// _mm_movemask_ps equivalent, compatible with ARM v7. +static inline bool b3AnyTrueW( b3FloatW mask ) +{ + uint32x4_t m = vreinterpretq_u32_f32( mask ); + uint32x2_t p = vorr_u32( vget_low_u32( m ), vget_high_u32( m ) ); + return ( vget_lane_u32( p, 0 ) | vget_lane_u32( p, 1 ) ) != 0; +} + +// component-wise returns mask ? b : a +static inline b3FloatW b3BlendW( b3FloatW a, b3FloatW b, b3FloatW mask ) +{ + uint32x4_t mask32 = vreinterpretq_u32_f32( mask ); + return vbslq_f32( mask32, b, a ); +} + +static inline b3FloatW b3Dot3W( b3FloatW ax, b3FloatW ay, b3FloatW az, b3FloatW bx, b3FloatW by, b3FloatW bz ) +{ + return vaddq_f32( vmulq_f32( ax, bx ), vaddq_f32( vmulq_f32( ay, by ), vmulq_f32( az, bz ) ) ); +} + +static inline b3FloatW b3EmbedIndexW( b3FloatW value, int baseIndex, int bitCount ) +{ + uint32_t mask = ( 1u << bitCount ) - 1; + const int32_t lanes[4] = { 0, 1, 2, 3 }; + int32x4_t index = vaddq_s32( vdupq_n_s32( baseIndex ), vld1q_s32( lanes ) ); + uint32x4_t clearLow = vdupq_n_u32( ~mask ); + uint32x4_t bits = vorrq_u32( vandq_u32( vreinterpretq_u32_f32( value ), clearLow ), vreinterpretq_u32_s32( index ) ); + return vreinterpretq_f32_u32( bits ); +} + +static inline int b3MinIndexW( b3FloatW a, int bitCount ) +{ + float32x2_t m = vmin_f32( vget_low_f32( a ), vget_high_f32( a ) ); + m = vpmin_f32( m, m ); + uint32_t bits = vget_lane_u32( vreinterpret_u32_f32( m ), 0 ); + return (int)( bits & ( ( 1u << bitCount ) - 1 ) ); +} + +#elif defined( B3_SIMD_SSE2 ) + +static inline b3FloatW b3ZeroW( void ) +{ + return _mm_setzero_ps(); +} + +static inline b3FloatW b3SplatW( float scalar ) +{ + return _mm_set1_ps( scalar ); +} + +static inline b3FloatW b3SetW( float a, float b, float c, float d ) +{ + return _mm_setr_ps( a, b, c, d ); +} + +static inline b3FloatW b3LoadW( const float* data ) +{ + return _mm_loadu_ps( data ); +} + +static inline void b3StoreW( float* data, b3FloatW a ) +{ + _mm_storeu_ps( data, a ); +} + +static inline b3FloatW b3NegW( b3FloatW a ) +{ + // Create a mask with the sign bit set for each element + __m128 mask = _mm_set1_ps( -0.0f ); + + // XOR the input with the mask to negate each element + return _mm_xor_ps( a, mask ); +} + +static inline b3FloatW b3AddW( b3FloatW a, b3FloatW b ) +{ + return _mm_add_ps( a, b ); +} + +static inline b3FloatW b3SubW( b3FloatW a, b3FloatW b ) +{ + return _mm_sub_ps( a, b ); +} + +static inline b3FloatW b3MulW( b3FloatW a, b3FloatW b ) +{ + return _mm_mul_ps( a, b ); +} + +static inline b3FloatW b3DivW( b3FloatW a, b3FloatW b ) +{ + return _mm_div_ps( a, b ); +} + +static inline b3FloatW b3SqrtW( b3FloatW a ) +{ + return _mm_sqrt_ps( a ); +} + +// a + b * c +static inline b3FloatW b3MulAddW( b3FloatW a, b3FloatW b, b3FloatW c ) +{ + return _mm_add_ps( a, _mm_mul_ps( b, c ) ); +} + +static inline b3FloatW b3MinW( b3FloatW a, b3FloatW b ) +{ + return _mm_min_ps( a, b ); +} + +static inline b3FloatW b3MaxW( b3FloatW a, b3FloatW b ) +{ + return _mm_max_ps( a, b ); +} + +// Horizontal min over a 4-lane vector (result broadcast to all lanes). +static inline b3FloatW b3HorizontalMinW( b3FloatW v ) +{ + v = _mm_min_ps( v, _mm_shuffle_ps( v, v, _MM_SHUFFLE( 2, 3, 0, 1 ) ) ); + return _mm_min_ps( v, _mm_shuffle_ps( v, v, _MM_SHUFFLE( 1, 0, 3, 2 ) ) ); +} + +// clamp a to [-b, b] +static inline b3FloatW b3SymClampW( b3FloatW a, b3FloatW b ) +{ + b3FloatW nb = b3NegW( b ); + b3FloatW c = b3MaxW( nb, a ); + return b3MinW( c, b ); +} + +static inline b3FloatW b3AndW( b3FloatW a, b3FloatW b ) +{ + return _mm_and_ps( a, b ); +} + +static inline b3FloatW b3OrW( b3FloatW a, b3FloatW b ) +{ + return _mm_or_ps( a, b ); +} + +static inline b3FloatW b3GreaterThanW( b3FloatW a, b3FloatW b ) +{ + return _mm_cmpgt_ps( a, b ); +} + +static inline b3FloatW b3LessThanW( b3FloatW a, b3FloatW b ) +{ + return _mm_cmplt_ps( a, b ); +} + +static inline b3FloatW b3EqualsW( b3FloatW a, b3FloatW b ) +{ + return _mm_cmpeq_ps( a, b ); +} + +static inline bool b3AllZeroW( b3FloatW a ) +{ + // Compare each element with zero + b3FloatW zero = _mm_setzero_ps(); + b3FloatW cmp = _mm_cmpeq_ps( a, zero ); + + // Create a mask from the comparison results + int mask = _mm_movemask_ps( cmp ); + + // If all elements are zero, the mask will be 0xF (1111 in binary) + return mask == 0xF; +} + +static inline bool b3AnyTrueW( b3FloatW mask ) +{ + return _mm_movemask_ps( mask ) != 0; +} + +// component-wise returns mask ? b : a +static inline b3FloatW b3BlendW( b3FloatW a, b3FloatW b, b3FloatW mask ) +{ + return _mm_or_ps( _mm_and_ps( mask, b ), _mm_andnot_ps( mask, a ) ); +} + +static inline b3FloatW b3Dot3W( b3FloatW ax, b3FloatW ay, b3FloatW az, b3FloatW bx, b3FloatW by, b3FloatW bz ) +{ + return _mm_add_ps( _mm_mul_ps( ax, bx ), _mm_add_ps( _mm_mul_ps( ay, by ), _mm_mul_ps( az, bz ) ) ); +} + +// Replace the low bitCount mantissa bits of each lane with baseIndex + lane. The value must be +// positive so the embedded index sorts with the value, and ties fall to the lower index. +static inline b3FloatW b3EmbedIndexW( b3FloatW value, int baseIndex, int bitCount ) +{ + int mask = ( 1 << bitCount ) - 1; + __m128i index = _mm_add_epi32( _mm_set1_epi32( baseIndex ), _mm_setr_epi32( 0, 1, 2, 3 ) ); + __m128 clearLow = _mm_castsi128_ps( _mm_set1_epi32( ~mask ) ); + return _mm_or_ps( _mm_and_ps( value, clearLow ), _mm_castsi128_ps( index ) ); +} + +// Recovers the index embedded by b3EmbedIndexW from the lane holding the minimum. +static inline int b3MinIndexW( b3FloatW a, int bitCount ) +{ + a = _mm_min_ps( a, _mm_shuffle_ps( a, a, _MM_SHUFFLE( 2, 3, 0, 1 ) ) ); + a = _mm_min_ps( a, _mm_shuffle_ps( a, a, _MM_SHUFFLE( 1, 0, 3, 2 ) ) ); + return _mm_cvtsi128_si32( _mm_castps_si128( a ) ) & ( ( 1 << bitCount ) - 1 ); +} + +#else + +static inline b3FloatW b3ZeroW( void ) +{ + return (b3FloatW){ 0.0f, 0.0f, 0.0f, 0.0f }; +} + +static inline b3FloatW b3SplatW( float scalar ) +{ + return (b3FloatW){ scalar, scalar, scalar, scalar }; +} + +static inline b3FloatW b3SetW( float a, float b, float c, float d ) +{ + return (b3FloatW){ a, b, c, d }; +} + +static inline b3FloatW b3LoadW( const float* data ) +{ + return (b3FloatW){ data[0], data[1], data[2], data[3] }; +} + +static inline void b3StoreW( float* data, b3FloatW a ) +{ + data[0] = a.x; + data[1] = a.y; + data[2] = a.z; + data[3] = a.w; +} + +static inline b3FloatW b3NegW( b3FloatW a ) +{ + return (b3FloatW){ -a.x, -a.y, -a.z, -a.w }; +} + +static inline b3FloatW b3AddW( b3FloatW a, b3FloatW b ) +{ + return (b3FloatW){ a.x + b.x, a.y + b.y, a.z + b.z, a.w + b.w }; +} + +static inline b3FloatW b3SubW( b3FloatW a, b3FloatW b ) +{ + return (b3FloatW){ a.x - b.x, a.y - b.y, a.z - b.z, a.w - b.w }; +} + +static inline b3FloatW b3MulW( b3FloatW a, b3FloatW b ) +{ + return (b3FloatW){ a.x * b.x, a.y * b.y, a.z * b.z, a.w * b.w }; +} + +static inline b3FloatW b3DivW( b3FloatW a, b3FloatW b ) +{ + return (b3FloatW){ a.x / b.x, a.y / b.y, a.z / b.z, a.w / b.w }; +} + +static inline b3FloatW b3SqrtW( b3FloatW a ) +{ + return (b3FloatW){ sqrtf( a.x ), sqrtf( a.y ), sqrtf( a.z ), sqrtf( a.w ) }; +} + +static inline b3FloatW b3MulAddW( b3FloatW a, b3FloatW b, b3FloatW c ) +{ + return (b3FloatW){ a.x + b.x * c.x, a.y + b.y * c.y, a.z + b.z * c.z, a.w + b.w * c.w }; +} + +static inline b3FloatW b3MinW( b3FloatW a, b3FloatW b ) +{ + b3FloatW r; + r.x = a.x <= b.x ? a.x : b.x; + r.y = a.y <= b.y ? a.y : b.y; + r.z = a.z <= b.z ? a.z : b.z; + r.w = a.w <= b.w ? a.w : b.w; + return r; +} + +static inline b3FloatW b3MaxW( b3FloatW a, b3FloatW b ) +{ + b3FloatW r; + r.x = a.x >= b.x ? a.x : b.x; + r.y = a.y >= b.y ? a.y : b.y; + r.z = a.z >= b.z ? a.z : b.z; + r.w = a.w >= b.w ? a.w : b.w; + return r; +} + +// clamp a to [-b, b] +static inline b3FloatW b3SymClampW( b3FloatW a, b3FloatW b ) +{ + b3FloatW r; + r.x = a.x <= b.x ? a.x : b.x; + r.y = a.y <= b.y ? a.y : b.y; + r.z = a.z <= b.z ? a.z : b.z; + r.w = a.w <= b.w ? a.w : b.w; + r.x = r.x <= -b.x ? -b.x : r.x; + r.y = r.y <= -b.y ? -b.y : r.y; + r.z = r.z <= -b.z ? -b.z : r.z; + r.w = r.w <= -b.w ? -b.w : r.w; + return r; +} + +// Logical operations on the scalar path are 0/1 float values. Not bit-wise like SIMD. + +static inline b3FloatW b3AndW( b3FloatW a, b3FloatW b ) +{ + b3FloatW r; + r.x = a.x != 0.0f && b.x != 0.0f ? 1.0f : 0.0f; + r.y = a.y != 0.0f && b.y != 0.0f ? 1.0f : 0.0f; + r.z = a.z != 0.0f && b.z != 0.0f ? 1.0f : 0.0f; + r.w = a.w != 0.0f && b.w != 0.0f ? 1.0f : 0.0f; + return r; +} + +static inline b3FloatW b3OrW( b3FloatW a, b3FloatW b ) +{ + b3FloatW r; + r.x = a.x != 0.0f || b.x != 0.0f ? 1.0f : 0.0f; + r.y = a.y != 0.0f || b.y != 0.0f ? 1.0f : 0.0f; + r.z = a.z != 0.0f || b.z != 0.0f ? 1.0f : 0.0f; + r.w = a.w != 0.0f || b.w != 0.0f ? 1.0f : 0.0f; + return r; +} + +static inline b3FloatW b3GreaterThanW( b3FloatW a, b3FloatW b ) +{ + b3FloatW r; + r.x = a.x > b.x ? 1.0f : 0.0f; + r.y = a.y > b.y ? 1.0f : 0.0f; + r.z = a.z > b.z ? 1.0f : 0.0f; + r.w = a.w > b.w ? 1.0f : 0.0f; + return r; +} + +static inline b3FloatW b3LessThanW( b3FloatW a, b3FloatW b ) +{ + b3FloatW r; + r.x = a.x < b.x ? 1.0f : 0.0f; + r.y = a.y < b.y ? 1.0f : 0.0f; + r.z = a.z < b.z ? 1.0f : 0.0f; + r.w = a.w < b.w ? 1.0f : 0.0f; + return r; +} + +static inline b3FloatW b3EqualsW( b3FloatW a, b3FloatW b ) +{ + b3FloatW r; + r.x = a.x == b.x ? 1.0f : 0.0f; + r.y = a.y == b.y ? 1.0f : 0.0f; + r.z = a.z == b.z ? 1.0f : 0.0f; + r.w = a.w == b.w ? 1.0f : 0.0f; + return r; +} + +static inline bool b3AllZeroW( b3FloatW a ) +{ + return a.x == 0.0f && a.y == 0.0f && a.z == 0.0f && a.w == 0.0f; +} + +static inline bool b3AnyTrueW( b3FloatW mask ) +{ + return mask.x != 0.0f || mask.y != 0.0f || mask.z != 0.0f || mask.w != 0.0f; +} + +// component-wise returns mask ? b : a +static inline b3FloatW b3BlendW( b3FloatW a, b3FloatW b, b3FloatW mask ) +{ + b3FloatW r; + r.x = mask.x != 0.0f ? b.x : a.x; + r.y = mask.y != 0.0f ? b.y : a.y; + r.z = mask.z != 0.0f ? b.z : a.z; + r.w = mask.w != 0.0f ? b.w : a.w; + return r; +} + +static inline b3FloatW b3Dot3W( b3FloatW ax, b3FloatW ay, b3FloatW az, b3FloatW bx, b3FloatW by, b3FloatW bz ) +{ + b3FloatW r; + r.x = ax.x * bx.x + ( ay.x * by.x + az.x * bz.x ); + r.y = ax.y * bx.y + ( ay.y * by.y + az.y * bz.y ); + r.z = ax.z * bx.z + ( ay.z * by.z + az.z * bz.z ); + r.w = ax.w * bx.w + ( ay.w * by.w + az.w * bz.w ); + return r; +} + +static inline b3FloatW b3EmbedIndexW( b3FloatW value, int baseIndex, int bitCount ) +{ + uint32_t mask = ( 1u << bitCount ) - 1; + float lanes[4] = { value.x, value.y, value.z, value.w }; + + for ( int i = 0; i < 4; ++i ) + { + uint32_t bits; + memcpy( &bits, lanes + i, sizeof( bits ) ); + bits = ( bits & ~mask ) | (uint32_t)( baseIndex + i ); + memcpy( lanes + i, &bits, sizeof( bits ) ); + } + + return (b3FloatW){ lanes[0], lanes[1], lanes[2], lanes[3] }; +} + +static inline int b3MinIndexW( b3FloatW a, int bitCount ) +{ + float m = a.x; + m = a.y < m ? a.y : m; + m = a.z < m ? a.z : m; + m = a.w < m ? a.w : m; + + uint32_t bits; + memcpy( &bits, &m, sizeof( bits ) ); + return (int)( bits & ( ( 1u << bitCount ) - 1 ) ); +} + +#endif diff --git a/src/solver.c b/src/solver.c index 0fde035..c0db8eb 100644 --- a/src/solver.c +++ b/src/solver.c @@ -125,7 +125,7 @@ static void b3IntegrateVelocitiesTask( b3SolverBlock block, b3StepContext* conte float i12 = inertiaLocal.cz.y; float i22 = inertiaLocal.cz.z; - for ( int gyroIteration = 0; gyroIteration < 1; ++gyroIteration ) + for ( int gyroIteration = 0; gyroIteration < B3_GYROSCOPIC_ITERATIONS; ++gyroIteration ) { float w1 = omega2.x; float w2 = omega2.y; diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt index 92dcbb1..22ccc2d 100644 --- a/test/CMakeLists.txt +++ b/test/CMakeLists.txt @@ -22,6 +22,7 @@ set(BOX3D_TEST_FILES test_mover.c test_name_cache.c test_recording.c + test_sat.c test_shape.c test_table.c test_world.c diff --git a/test/main.c b/test/main.c index 8edcf96..3843689 100644 --- a/test/main.c +++ b/test/main.c @@ -44,6 +44,7 @@ extern int MathTest( void ); extern int MoverTest( void ); extern int NameCacheTest( void ); extern int RecordingTest( void ); +extern int SeparatingAxisTest( void ); extern int ShapeTest( void ); extern int TableTest( void ); extern int WorldTest( void ); @@ -116,6 +117,7 @@ int main( int argc, char** argv ) MAYBE_RUN_TEST( MoverTest ); MAYBE_RUN_TEST( NameCacheTest ); MAYBE_RUN_TEST( RecordingTest ); + MAYBE_RUN_TEST( SeparatingAxisTest ); MAYBE_RUN_TEST( ShapeTest ); MAYBE_RUN_TEST( TableTest ); MAYBE_RUN_TEST( WorldTest ); diff --git a/test/test_allocator.c b/test/test_allocator.c index ca7acaf..1e78b7d 100644 --- a/test/test_allocator.c +++ b/test/test_allocator.c @@ -2,8 +2,11 @@ // SPDX-License-Identifier: MIT #include "block_allocator.h" +#include "core.h" #include "test_macros.h" +#include "box3d/types.h" + typedef struct { int value1; @@ -40,10 +43,57 @@ static int TestBlockClear( void ) return 0; } +// The free list link is a pointer stored in the element, so a stride that is not a multiple +// of the alignment leaves odd elements on a bad boundary. See issue #78. +static int TestBlockAlignment( void ) +{ + // The manifold sizes are the reported case + int elementSizes[] = { 12, (int)sizeof( b3Manifold ), 3 * (int)sizeof( b3Manifold ) }; + + for ( int i = 0; i < ARRAY_COUNT( elementSizes ); ++i ) + { + b3BlockAllocator allocator = b3CreateBlockAllocator( elementSizes[i], 0 ); + + // Hold the elements so the indices advance and a block boundary is crossed + void* elements[B3_BLOCK_SIZE + 8]; + int count = ARRAY_COUNT( elements ); + + for ( int j = 0; j < count; ++j ) + { + elements[j] = b3AllocateElement( &allocator ); + ENSURE( elements[j] != NULL ); + ENSURE( ( (uintptr_t)elements[j] & ( B3_ALIGNMENT - 1 ) ) == 0 ); + } + + for ( int j = 0; j < count; ++j ) + { + b3FreeElement( &allocator, elements[j] ); + } + + // Now drain the free list + for ( int j = 0; j < count; ++j ) + { + elements[j] = b3AllocateElement( &allocator ); + ENSURE( elements[j] != NULL ); + ENSURE( ( (uintptr_t)elements[j] & ( B3_ALIGNMENT - 1 ) ) == 0 ); + } + + for ( int j = 0; j < count; ++j ) + { + b3FreeElement( &allocator, elements[j] ); + } + + b3DestroyBlockAllocator( &allocator ); + } + + return 0; +} + int AllocatorTest( void ) { RUN_SUBTEST( TestBlockAllocate ); RUN_SUBTEST( TestBlockClear ); + RUN_SUBTEST( TestBlockAlignment ); return 0; } diff --git a/test/test_determinism.c b/test/test_determinism.c index e497d0b..0c92a90 100644 --- a/test/test_determinism.c +++ b/test/test_determinism.c @@ -3,6 +3,7 @@ #include "box3d/box3d.h" #include "determinism.h" +#include "stability.h" #include "test_macros.h" #include @@ -19,9 +20,25 @@ #if defined( BOX3D_DOUBLE_PRECISION ) #define EXPECTED_SLEEP_STEP 297 #define EXPECTED_HASH 0x27FF38C1 +#define WAVE_PILE_SLEEP_STEP 287 +#define WAVE_PILE_HASH 0xFFC8DA49 +#define QUERY_SPAWN_SLEEP_STEP 242 +#define QUERY_SPAWN_HASH 0x1737F5BC +#define QUERY_SPAWN_HIT_COUNT 59 +#define QUERY_SPAWN_QUERY_HASH 0x31F090DC +#define MESH_DROP_SLEEP_STEP 206 +#define MESH_DROP_HASH 0xB6A9E1DE #else #define EXPECTED_SLEEP_STEP 308 #define EXPECTED_HASH 0x1E5EDD79 +#define WAVE_PILE_SLEEP_STEP 285 +#define WAVE_PILE_HASH 0x4057173C +#define QUERY_SPAWN_SLEEP_STEP 242 +#define QUERY_SPAWN_HASH 0xB9F993A5 +#define QUERY_SPAWN_HIT_COUNT 59 +#define QUERY_SPAWN_QUERY_HASH 0xB9D3863D +#define MESH_DROP_SLEEP_STEP 205 +#define MESH_DROP_HASH 0x8F55FB2D #endif static int SingleMultithreadingTest( int workerCount ) @@ -111,10 +128,168 @@ static int CrossPlatformTest( void ) return 0; } +static int SingleWavePileTest( int workerCount ) +{ + b3WorldDef worldDef = b3DefaultWorldDef(); + worldDef.workerCount = workerCount; + + b3WorldId worldId = b3CreateWorld( &worldDef ); + + WavePileData data = CreateWavePile( worldId ); + + float timeStep = 1.0f / 60.0f; + + // Rolling resistance must put the pile to sleep within 500 steps + bool done = false; + for ( int i = 0; i < 500 && done == false; ++i ) + { + int subStepCount = 4; + b3World_Step( worldId, timeStep, subStepCount ); + TracyCFrameMark; + + done = UpdateWavePile( worldId, &data ); + } + + b3DestroyWorld( worldId ); + + if ( data.sleepStep != WAVE_PILE_SLEEP_STEP || data.hash != WAVE_PILE_HASH ) + { + printf( " wave pile workers=%d sleepStep=%d hash=0x%08X\n", workerCount, data.sleepStep, data.hash ); + } + + ENSURE( done == true ); + ENSURE( data.sleepStep == WAVE_PILE_SLEEP_STEP ); + ENSURE( data.hash == WAVE_PILE_HASH ); + + DestroyWavePile( &data ); + + return 0; +} + +// Test multithreaded determinism of a mixed convex pile on a wave height field. +static int WavePileTest( void ) +{ + for ( int workerCount = 1; workerCount <= 4; ++workerCount ) + { + int result = SingleWavePileTest( workerCount ); + ENSURE( result == 0 ); + } + + return 0; +} + +static int SingleQuerySpawnTest( int workerCount ) +{ + b3WorldDef worldDef = b3DefaultWorldDef(); + worldDef.workerCount = workerCount; + + b3WorldId worldId = b3CreateWorld( &worldDef ); + + QuerySpawnData data = CreateQuerySpawn( worldId ); + + float timeStep = 1.0f / 60.0f; + + bool done = false; + for ( int i = 0; i < 1000 && done == false; ++i ) + { + int subStepCount = 4; + b3World_Step( worldId, timeStep, subStepCount ); + TracyCFrameMark; + + done = UpdateQuerySpawn( worldId, &data ); + } + + b3DestroyWorld( worldId ); + + if ( data.sleepStep != QUERY_SPAWN_SLEEP_STEP || data.hash != QUERY_SPAWN_HASH || data.queryHitCount != QUERY_SPAWN_HIT_COUNT || + data.queryHash != QUERY_SPAWN_QUERY_HASH ) + { + printf( " query spawn workers=%d sleepStep=%d hash=0x%08X hits=%d queryHash=0x%08X\n", workerCount, data.sleepStep, + data.hash, data.queryHitCount, data.queryHash ); + } + + ENSURE( done == true ); + ENSURE( data.spawnCount == QUERY_SPAWN_COUNT ); + ENSURE( data.sleepStep == QUERY_SPAWN_SLEEP_STEP ); + ENSURE( data.hash == QUERY_SPAWN_HASH ); + ENSURE( data.queryHitCount == QUERY_SPAWN_HIT_COUNT ); + ENSURE( data.queryHash == QUERY_SPAWN_QUERY_HASH ); + + DestroyQuerySpawn( &data ); + + return 0; +} + +// Test determinism of world queries by feeding their results back into the simulation. +static int QuerySpawnTest( void ) +{ + for ( int workerCount = 1; workerCount <= 4; ++workerCount ) + { + int result = SingleQuerySpawnTest( workerCount ); + ENSURE( result == 0 ); + } + + return 0; +} + +static int SingleMeshDropTest( int workerCount ) +{ + b3WorldDef worldDef = b3DefaultWorldDef(); + worldDef.workerCount = workerCount; + + b3WorldId worldId = b3CreateWorld( &worldDef ); + + MeshDropData data = CreateMeshDrop( worldId, b3Pos_zero ); + + float timeStep = 1.0f / 60.0f; + + bool done = false; + for ( int i = 0; i < 400 && done == false; ++i ) + { + int subStepCount = 4; + b3World_Step( worldId, timeStep, subStepCount ); + TracyCFrameMark; + + done = UpdateMeshDrop( worldId, &data ); + } + + b3DestroyWorld( worldId ); + + if ( data.sleepStep != MESH_DROP_SLEEP_STEP || data.hash != MESH_DROP_HASH ) + { + printf( " mesh drop workers=%d sleepStep=%d hash=0x%08X\n", workerCount, data.sleepStep, data.hash ); + } + + ENSURE( done == true ); + ENSURE( data.sleepStep == MESH_DROP_SLEEP_STEP ); + ENSURE( data.hash == MESH_DROP_HASH ); + + DestroyMeshDrop( &data ); + + return 0; +} + +// Test continuous collision determinism. Thin fast boxes need CCD against the wave mesh. +// The scene is large, so only the single threaded and widest schedules run. +static int MeshDropTest( void ) +{ + int workerCounts[2] = { 1, 4 }; + for ( int i = 0; i < 2; ++i ) + { + int result = SingleMeshDropTest( workerCounts[i] ); + ENSURE( result == 0 ); + } + + return 0; +} + int DeterminismTest( void ) { RUN_SUBTEST( MultithreadingTest ); RUN_SUBTEST( CrossPlatformTest ); + RUN_SUBTEST( WavePileTest ); + RUN_SUBTEST( QuerySpawnTest ); + RUN_SUBTEST( MeshDropTest ); return 0; } diff --git a/test/test_hull.c b/test/test_hull.c index 46ccc84..9860d97 100644 --- a/test/test_hull.c +++ b/test/test_hull.c @@ -146,13 +146,13 @@ static int CreateHullMaxVertexTest( void ) // Below the floor: clamps up to 4. b3HullData* h2 = b3CreateHull( points, count, 1 ); ENSURE( h2 != NULL ); - ENSURE( h2->vertexCount >= 4 && h2->vertexCount <= 255 ); + ENSURE( h2->vertexCount >= 4 && h2->vertexCount <= B3_MAX_HULL_VERTICES ); b3DestroyHull( h2 ); - // Above the ceiling: clamps down to 255. + // Above the ceiling: clamps down to B3_MAX_HULL_VERTICES. b3HullData* h3 = b3CreateHull( points, count, 1000 ); ENSURE( h3 != NULL ); - ENSURE( h3->vertexCount >= 4 && h3->vertexCount <= 255 ); + ENSURE( h3->vertexCount >= 4 && h3->vertexCount <= B3_MAX_HULL_VERTICES ); b3DestroyHull( h3 ); return 0; @@ -341,9 +341,8 @@ static void FillCubeSample( b3Vec3* points, int count, uint32_t seed ) // 24*M - 48 in src/hull.c::b3ComputeHullWorkSizes) close to their peak by sweeping M // on a dense random sphere. If the peak face/edge count exceeds the cap and the free // list fails to reclaim slots in time, the assertion at src/hull.c::NewFace/NewEdge -// fires (exit code 3). M is held <= 80 because the finalization stage caps the final -// hull's face/edge count at B3_HULL_LIMIT (255), and a fully-triangulated hull with -// V > 87 vertices has 3V - 6 > 255 edges (src/hull.c:2010 returns NULL). +// fires (exit code 3). A dense sphere hull is fully triangulated, so F = 2M - 4 and the +// final face limit B3_MAX_HULL_FACES binds well before the vertex or edge limits. static int CreateHullSphereStressTest( void ) { enum { N = 512 }; @@ -352,8 +351,8 @@ static int CreateHullSphereStressTest( void ) // Multiple seeds exercise different horizon-size / merge-cascade sequences. const uint32_t seeds[] = { 12345u, 1u, 0xdeadbeefu, 0xcafef00du }; - // M kept <= 40: random sphere inputs at higher M exceed the half edge limit of UINT8_MAX - const int M_values[] = { 16, 24, 32, 40 }; + // M kept <= 32 so the final face count 2M - 4 stays under B3_MAX_HULL_FACES + const int M_values[] = { 16, 24, 32 }; for ( int s = 0; s < ARRAY_COUNT( seeds ); ++s ) { diff --git a/test/test_sat.c b/test/test_sat.c new file mode 100644 index 0000000..037846f --- /dev/null +++ b/test/test_sat.c @@ -0,0 +1,486 @@ +// SPDX-FileCopyrightText: 2026 Erin Catto +// SPDX-License-Identifier: MIT + +#include "test_macros.h" + +#include "manifold.h" + +#include "box3d/collision.h" +#include "box3d/constants.h" +#include "box3d/math_functions.h" + +#include +#include + +static const float kRoot2 = 1.41421356f; +static const b3Vec3 kAxisY = { 0.0f, 1.0f, 0.0f }; +static const b3Vec3 kAxisZ = { 0.0f, 0.0f, 1.0f }; + +// b3ComputeCosSin is a coarse rational approximation, so the fixtures build their rotations from +// libm to keep the analytic edge and face positions exact. +static b3Quat ExactQuat( b3Vec3 axis, float radians ) +{ + float half = 0.5f * radians; + float s = sinf( half ); + return (b3Quat){ { s * axis.x, s * axis.y, s * axis.z }, cosf( half ) }; +} + +static b3Transform ExactRotation( b3Vec3 axis, float radians ) +{ + return (b3Transform){ b3Vec3_zero, ExactQuat( axis, radians ) }; +} + +// Directed separation along a unit axis n pointing from A to B. Positive means B clears A along n. +// This is the projection gap min over B minus max over A, which is exactly what a valid separating +// axis measures, so the winning face or edge axis must reproduce it. +static float SepAlong( const b3HullData* hullA, const b3HullData* hullB, b3Transform xfB, b3Vec3 n ) +{ + const b3Vec3* pointsA = b3GetHullPoints( hullA ); + float maxA = -FLT_MAX; + for ( int i = 0; i < hullA->vertexCount; ++i ) + { + maxA = b3MaxFloat( maxA, b3Dot( n, pointsA[i] ) ); + } + + const b3Vec3* pointsB = b3GetHullPoints( hullB ); + float minB = FLT_MAX; + for ( int i = 0; i < hullB->vertexCount; ++i ) + { + minB = b3MinFloat( minB, b3Dot( n, b3TransformPoint( xfB, pointsB[i] ) ) ); + } + + return minB - maxA; +} + +static void TryAxis( const b3HullData* hullA, const b3HullData* hullB, b3Transform xfB, b3Vec3 n, float* best, + b3Vec3* bestNormal ) +{ + float s = SepAlong( hullA, hullB, xfB, n ); + if ( s > *best ) + { + *best = s; + *bestNormal = n; + } +} + +// Brute force SAT. The maximum separation over every face normal and every edge cross product is +// the true answer the SIMD query has to match. Each undirected axis is tried both ways so the +// winner comes out oriented from A to B with no sign bookkeeping. +static float OracleSeparation( const b3HullData* hullA, const b3HullData* hullB, b3Transform xfB, b3Vec3* outNormal ) +{ + float best = -FLT_MAX; + b3Vec3 bestNormal = { 0.0f, 0.0f, 0.0f }; + + const b3Plane* planesA = b3GetHullPlanes( hullA ); + for ( int i = 0; i < hullA->faceCount; ++i ) + { + TryAxis( hullA, hullB, xfB, planesA[i].normal, &best, &bestNormal ); + TryAxis( hullA, hullB, xfB, b3Neg( planesA[i].normal ), &best, &bestNormal ); + } + + const b3Plane* planesB = b3GetHullPlanes( hullB ); + for ( int i = 0; i < hullB->faceCount; ++i ) + { + b3Vec3 n = b3RotateVector( xfB.q, planesB[i].normal ); + TryAxis( hullA, hullB, xfB, n, &best, &bestNormal ); + TryAxis( hullA, hullB, xfB, b3Neg( n ), &best, &bestNormal ); + } + + const b3HullHalfEdge* edgesA = b3GetHullEdges( hullA ); + const b3Vec3* pointsA = b3GetHullPoints( hullA ); + const b3HullHalfEdge* edgesB = b3GetHullEdges( hullB ); + const b3Vec3* pointsB = b3GetHullPoints( hullB ); + + // Half edges are stored as adjacent twin pairs, so the even index and its successor bound one edge. + for ( int i = 0; i < hullA->edgeCount; i += 2 ) + { + b3Vec3 dirA = b3Sub( pointsA[edgesA[i + 1].origin], pointsA[edgesA[i].origin] ); + for ( int j = 0; j < hullB->edgeCount; j += 2 ) + { + b3Vec3 dirB = b3RotateVector( xfB.q, b3Sub( pointsB[edgesB[j + 1].origin], pointsB[edgesB[j].origin] ) ); + b3Vec3 cross = b3Cross( dirA, dirB ); + + // Near parallel edges give no useful axis and cannot be the true maximum. + if ( b3Dot( cross, cross ) < 1e-10f ) + { + continue; + } + + cross = b3Normalize( cross ); + TryAxis( hullA, hullB, xfB, cross, &best, &bestNormal ); + TryAxis( hullA, hullB, xfB, b3Neg( cross ), &best, &bestNormal ); + } + } + + *outNormal = bestNormal; + return best; +} + +// Two axis aligned cubes pushed apart along x. A's plus x face is the separating axis, the gap is +// large enough to trip the speculative early out, and the reference vertex on B is its minus x side. +static int FaceAxisASeparatedTest( void ) +{ + b3BoxHull hullA = b3MakeBoxHull( 0.5f, 0.5f, 0.5f ); + b3BoxHull hullB = b3MakeBoxHull( 0.5f, 0.5f, 0.5f ); + + b3Transform xfB = { { 1.2f, 0.0f, 0.0f }, b3Quat_identity }; + b3AxisQuery q = b3ComputeSeparatingAxis( &hullA.base, &hullB.base, xfB, b3_invalidAxis ); + + ENSURE( q.type == b3_faceAxisA ); + ENSURE_SMALL( q.separation - 0.2f, 1e-5f ); + ENSURE_SMALL( q.normal.x - 1.0f, 1e-6f ); + ENSURE_SMALL( q.normal.y, 1e-6f ); + ENSURE_SMALL( q.normal.z, 1e-6f ); + + const b3Plane* planesA = b3GetHullPlanes( &hullA.base ); + ENSURE_SMALL( planesA[q.indexA].normal.x - 1.0f, 1e-6f ); + + const b3Vec3* pointsB = b3GetHullPoints( &hullB.base ); + ENSURE_SMALL( pointsB[q.indexB].x + 0.5f, 1e-6f ); + + ENSURE_SMALL( SepAlong( &hullA.base, &hullB.base, xfB, q.normal ) - q.separation, 1e-5f ); + return 0; +} + +// A yawed 45 degrees about z so all of its faces sit oblique to x. Only B still has a face square to +// the gap, so the reference has to be B's minus x face. This is the case that ties when both hulls +// are aligned, and the tie is what a face A bias would hide. +static int FaceAxisBSeparatedTest( void ) +{ + b3BoxHull hullA = b3MakeTransformedBoxHull( 0.5f, 0.5f, 0.5f, ExactRotation( kAxisZ, 0.25f * B3_PI ) ); + b3BoxHull hullB = b3MakeBoxHull( 0.5f, 0.5f, 0.5f ); + + float aExtent = 0.5f * kRoot2; + float gap = 0.2f; + float d = aExtent + 0.5f + gap; + b3Transform xfB = { { d, 0.0f, 0.0f }, b3Quat_identity }; + + b3AxisQuery q = b3ComputeSeparatingAxis( &hullA.base, &hullB.base, xfB, b3_invalidAxis ); + + ENSURE( q.type == b3_faceAxisB ); + ENSURE_SMALL( q.separation - gap, 1e-5f ); + ENSURE_SMALL( q.normal.x - 1.0f, 1e-5f ); + ENSURE_SMALL( q.normal.y, 1e-5f ); + ENSURE_SMALL( q.normal.z, 1e-5f ); + + const b3Plane* planesB = b3GetHullPlanes( &hullB.base ); + ENSURE_SMALL( planesB[q.indexB].normal.x + 1.0f, 1e-5f ); + + const b3Vec3* pointsA = b3GetHullPoints( &hullA.base ); + ENSURE_SMALL( pointsA[q.indexA].x - aExtent, 1e-4f ); + + ENSURE_SMALL( SepAlong( &hullA.base, &hullB.base, xfB, q.normal ) - q.separation, 1e-4f ); + return 0; +} + +// Well beyond the speculative distance the face phase must report the axis and separation without +// running the edge phase. A sign slip in the early out would read as deep overlap. +static int FaceFarSeparatedTest( void ) +{ + b3BoxHull hullA = b3MakeBoxHull( 0.5f, 0.5f, 0.5f ); + b3BoxHull hullB = b3MakeBoxHull( 0.5f, 0.5f, 0.5f ); + + b3Transform xfB = { { 3.0f, 0.0f, 0.0f }, b3Quat_identity }; + b3AxisQuery q = b3ComputeSeparatingAxis( &hullA.base, &hullB.base, xfB, b3_invalidAxis ); + + ENSURE( q.type == b3_faceAxisA ); + ENSURE_SMALL( q.separation - 2.0f, 1e-5f ); + ENSURE_SMALL( q.normal.x - 1.0f, 1e-6f ); + + b3Vec3 oracleNormal; + float oracleSep = OracleSeparation( &hullA.base, &hullB.base, xfB, &oracleNormal ); + ENSURE_SMALL( q.separation - oracleSep, 1e-5f ); + return 0; +} + +// The support bias is derived from each hull's AABB, not from an origin assumed to sit inside it. A +// box built far from its own local origin is the case the old diagonal bias got wrong. A is yawed +// and pushed out to x = 5, so the face B query has to run getSupport over vertices clustered near +// x = 5 and still pick the right one. +static int OffsetFaceAxisBTest( void ) +{ + float cx = 5.0f; + b3Transform placeA = { { cx, 0.0f, 0.0f }, ExactQuat( kAxisZ, 0.25f * B3_PI ) }; + b3BoxHull hullA = b3MakeTransformedBoxHull( 0.5f, 0.5f, 0.5f, placeA ); + b3BoxHull hullB = b3MakeBoxHull( 0.5f, 0.5f, 0.5f ); + + float aExtent = cx + 0.5f * kRoot2; + float gap = 0.2f; + float d = aExtent + 0.5f + gap; + b3Transform xfB = { { d, 0.0f, 0.0f }, b3Quat_identity }; + + b3AxisQuery q = b3ComputeSeparatingAxis( &hullA.base, &hullB.base, xfB, b3_invalidAxis ); + + ENSURE( q.type == b3_faceAxisB ); + ENSURE_SMALL( q.separation - gap, 1e-4f ); + ENSURE_SMALL( q.normal.x - 1.0f, 1e-5f ); + ENSURE_SMALL( q.normal.y, 1e-5f ); + ENSURE_SMALL( q.normal.z, 1e-5f ); + + const b3Plane* planesB = b3GetHullPlanes( &hullB.base ); + ENSURE_SMALL( planesB[q.indexB].normal.x + 1.0f, 1e-5f ); + + const b3Vec3* pointsA = b3GetHullPoints( &hullA.base ); + ENSURE_SMALL( pointsA[q.indexA].x - aExtent, 1e-4f ); + + b3Vec3 oracleNormal; + float oracleSep = OracleSeparation( &hullA.base, &hullB.base, xfB, &oracleNormal ); + ENSURE_SMALL( q.separation - oracleSep, 1e-4f ); + ENSURE_SMALL( SepAlong( &hullA.base, &hullB.base, xfB, q.normal ) - q.separation, 1e-4f ); + return 0; +} + +// Cube A yawed 45 about y presents an edge along y at x = h*root2. Cube B rolled 45 about z presents +// an edge along z at x = -h*root2. Sliding B along x makes those edges the closest features, so the +// axis is x and the separation is d - root2. The sweep straddles contact into overlap while staying +// under the speculative distance, so no face axis can short circuit the edge phase. The overlap rows +// pin the maximum separation, which is where a premature edge early out shows up. +static int EdgePairSweepTest( void ) +{ + b3BoxHull hullA = b3MakeTransformedBoxHull( 0.5f, 0.5f, 0.5f, ExactRotation( kAxisY, 0.25f * B3_PI ) ); + b3BoxHull hullB = b3MakeTransformedBoxHull( 0.5f, 0.5f, 0.5f, ExactRotation( kAxisZ, 0.25f * B3_PI ) ); + + float distances[] = { 1.43f, 1.42f, kRoot2, 1.40f, 1.38f, 1.35f, 1.30f }; + + for ( int i = 0; i < ARRAY_COUNT( distances ); ++i ) + { + float d = distances[i]; + float expected = d - kRoot2; + + b3Transform xfB = { { d, 0.0f, 0.0f }, b3Quat_identity }; + b3AxisQuery q = b3ComputeSeparatingAxis( &hullA.base, &hullB.base, xfB, b3_invalidAxis ); + + ENSURE( q.type == b3_edgePairAxis ); + ENSURE_SMALL( q.separation - expected, 1e-4f ); + ENSURE_SMALL( q.normal.x - 1.0f, 1e-4f ); + ENSURE_SMALL( q.normal.y, 1e-4f ); + ENSURE_SMALL( q.normal.z, 1e-4f ); + + // Half edge indices are the even twin of each pair. + ENSURE( ( q.indexA & 1 ) == 0 ); + ENSURE( ( q.indexB & 1 ) == 0 ); + + b3Vec3 oracleNormal; + float oracleSep = OracleSeparation( &hullA.base, &hullB.base, xfB, &oracleNormal ); + ENSURE_SMALL( q.separation - oracleSep, 1e-4f ); + ENSURE_SMALL( SepAlong( &hullA.base, &hullB.base, xfB, q.normal ) - q.separation, 1e-4f ); + } + + return 0; +} + +static uint32_t g_seed = 987654321u; + +static float NextFloat( float lower, float upper ) +{ + g_seed = 1664525u * g_seed + 1013904223u; + float t = (float)( g_seed >> 8 ) / (float)( 1 << 24 ); + return lower + t * ( upper - lower ); +} + +static b3Vec3 NextDirection( void ) +{ + b3Vec3 v = { NextFloat( -1.0f, 1.0f ), NextFloat( -1.0f, 1.0f ), NextFloat( -1.0f, 1.0f ) }; + return b3Normalize( v ); +} + +// A wide net over randomly oriented box pairs, from clearly separated to deep overlap. The brute +// force oracle is the source of truth. Whatever axis the query returns it must be a unit vector that +// reproduces its own separation, it must not exceed the true maximum, and it must not fall short of +// it. The short fall bound is what a suboptimal edge pick trips. Thin boxes are mixed in because a +// crossed pair of long edges stays the axis of minimum penetration even deep in overlap, which is +// the hardest case for the edge phase. +static int SeparatingAxisOracleTest( void ) +{ + g_seed = 987654321u; + + int separated = 0; + int penetrating = 0; + int edgeWins = 0; + float worstBandShortfall = 0.0f; + float worstExcess = 0.0f; + + for ( int i = 0; i < 4000; ++i ) + { + b3Vec3 halfA, halfB; + float reach; + + if ( i & 1 ) + { + // Thin crossed beams driven into deep overlap. + halfA = (b3Vec3){ NextFloat( 1.0f, 1.6f ), NextFloat( 0.08f, 0.15f ), NextFloat( 0.08f, 0.15f ) }; + halfB = (b3Vec3){ NextFloat( 1.0f, 1.6f ), NextFloat( 0.08f, 0.15f ), NextFloat( 0.08f, 0.15f ) }; + reach = NextFloat( 0.0f, 0.4f ); + } + else + { + // Chunky boxes across the whole separated to overlapping range. + halfA = (b3Vec3){ NextFloat( 0.3f, 0.8f ), NextFloat( 0.3f, 0.8f ), NextFloat( 0.3f, 0.8f ) }; + halfB = (b3Vec3){ NextFloat( 0.3f, 0.8f ), NextFloat( 0.3f, 0.8f ), NextFloat( 0.3f, 0.8f ) }; + reach = NextFloat( 0.0f, 1.7f ); + } + + // Rotate A in place so it stays centered on the origin, which keeps the support bias valid. + b3BoxHull hullA = + b3MakeTransformedBoxHull( halfA.x, halfA.y, halfA.z, ExactRotation( NextDirection(), NextFloat( 0.0f, B3_PI ) ) ); + b3BoxHull hullB = b3MakeBoxHull( halfB.x, halfB.y, halfB.z ); + + b3Transform xfB = { b3MulSV( reach, NextDirection() ), ExactQuat( NextDirection(), NextFloat( 0.0f, B3_PI ) ) }; + + b3AxisQuery q = b3ComputeSeparatingAxis( &hullA.base, &hullB.base, xfB, b3_invalidAxis ); + + b3Vec3 oracleNormal; + float oracleSep = OracleSeparation( &hullA.base, &hullB.base, xfB, &oracleNormal ); + + ENSURE_SMALL( b3Length( q.normal ) - 1.0f, 1e-3f ); + ENSURE_SMALL( SepAlong( &hullA.base, &hullB.base, xfB, q.normal ) - q.separation, 2e-3f ); + + worstExcess = b3MaxFloat( worstExcess, q.separation - oracleSep ); + + // Never overstate the true maximum. This holds on every path since the reported axis is real. + ENSURE( q.separation <= oracleSep + 2e-3f ); + + // The scan stops at the first axis that clears the speculative band, so a widely separated pair + // reports some separating axis rather than the deepest one. That is all the collide path needs, + // since past the band it drops the pair either way. Inside the band nothing stops the scan, so + // the result there is the exhaustive maximum, and that is what a manifold gets built from. + if ( q.separation <= B3_SPECULATIVE_DISTANCE ) + { + worstBandShortfall = b3MaxFloat( worstBandShortfall, oracleSep - q.separation ); + + // A suboptimal axis pick lands a discrete step below the true maximum. The bound sits well + // under that step yet clear of the edge normalization noise floor near 6e-4. + ENSURE( q.separation >= oracleSep - 4e-3f ); + } + + if ( oracleSep > 0.0f ) + { + separated += 1; + } + else + { + penetrating += 1; + } + + if ( q.type == b3_edgePairAxis ) + { + edgeWins += 1; + } + } + + printf( " oracle: separated=%d penetrating=%d edgeWins=%d worstBandShortfall=%.2e worstExcess=%.2e\n", separated, + penetrating, edgeWins, worstBandShortfall, worstExcess ); + + // The sweep has to cover both regimes and actually drive the edge path. + ENSURE( separated > 100 ); + ENSURE( penetrating > 100 ); + ENSURE( edgeWins > 20 ); + return 0; +} + +// The oracle over hulls whose vertices sit far from their own local origin, which the AABB based +// bias must handle. Half the pairs offset A in its own frame, half offset B, and B is placed so the +// two centers land near each other across the separated to overlapping range. Any bias that assumed +// the origin was inside the hull would pick the wrong support and blow the separation. +static int OffsetHullOracleTest( void ) +{ + g_seed = 24681012u; + + int separated = 0; + int penetrating = 0; + int edgeWins = 0; + float worstBandShortfall = 0.0f; + float worstExcess = 0.0f; + float worstConsistency = 0.0f; + + for ( int i = 0; i < 3000; ++i ) + { + b3Vec3 halfA = { NextFloat( 0.3f, 0.8f ), NextFloat( 0.3f, 0.8f ), NextFloat( 0.3f, 0.8f ) }; + b3Vec3 halfB = { NextFloat( 0.3f, 0.8f ), NextFloat( 0.3f, 0.8f ), NextFloat( 0.3f, 0.8f ) }; + + b3Vec3 offset = b3MulSV( NextFloat( 2.0f, 6.0f ), NextDirection() ); + float reach = NextFloat( 0.0f, 1.7f ); + b3Vec3 dir = NextDirection(); + + b3BoxHull hullA; + b3BoxHull hullB; + b3Transform xfB; + + if ( i & 1 ) + { + // A carries the offset in its own frame. B is centered and dropped near A's center. + b3Transform placeA = { offset, ExactQuat( NextDirection(), NextFloat( 0.0f, B3_PI ) ) }; + hullA = b3MakeTransformedBoxHull( halfA.x, halfA.y, halfA.z, placeA ); + hullB = b3MakeBoxHull( halfB.x, halfB.y, halfB.z ); + xfB = (b3Transform){ b3MulAdd( offset, reach, dir ), ExactQuat( NextDirection(), NextFloat( 0.0f, B3_PI ) ) }; + } + else + { + // B carries the offset in its own frame. Place it so its world center lands near the origin. + b3Quat qB = ExactQuat( NextDirection(), NextFloat( 0.0f, B3_PI ) ); + hullA = b3MakeBoxHull( halfA.x, halfA.y, halfA.z ); + hullB = b3MakeOffsetBoxHull( halfB.x, halfB.y, halfB.z, offset ); + xfB = (b3Transform){ b3Sub( b3MulSV( reach, dir ), b3RotateVector( qB, offset ) ), qB }; + } + + b3AxisQuery q = b3ComputeSeparatingAxis( &hullA.base, &hullB.base, xfB, b3_invalidAxis ); + + b3Vec3 oracleNormal; + float oracleSep = OracleSeparation( &hullA.base, &hullB.base, xfB, &oracleNormal ); + + float consistency = b3AbsFloat( SepAlong( &hullA.base, &hullB.base, xfB, q.normal ) - q.separation ); + worstConsistency = b3MaxFloat( worstConsistency, consistency ); + worstExcess = b3MaxFloat( worstExcess, q.separation - oracleSep ); + + // Consistency is the sharp bias check: a wrong support pick on an offset hull would throw the + // returned normal off its own separation by a vertex spacing, not a noise floor. It holds on + // every path, early out or not, so it stays unconditional. The oracle bounds are looser since + // differencing coordinates out at radius six costs a few more digits. + ENSURE_SMALL( b3Length( q.normal ) - 1.0f, 1e-3f ); + ENSURE( consistency < 1e-3f ); + ENSURE( q.separation <= oracleSep + 3e-3f ); + + // Only the in band result feeds a manifold, and only there does the scan run to completion. + if ( q.separation <= B3_SPECULATIVE_DISTANCE ) + { + worstBandShortfall = b3MaxFloat( worstBandShortfall, oracleSep - q.separation ); + ENSURE( q.separation >= oracleSep - 8e-3f ); + } + + if ( oracleSep > 0.0f ) + { + separated += 1; + } + else + { + penetrating += 1; + } + + if ( q.type == b3_edgePairAxis ) + { + edgeWins += 1; + } + } + + printf( + " offset: separated=%d penetrating=%d edgeWins=%d worstBandShortfall=%.2e worstExcess=%.2e worstConsistency=%.2e\n", + separated, penetrating, edgeWins, worstBandShortfall, worstExcess, worstConsistency ); + + ENSURE( separated > 100 ); + ENSURE( penetrating > 100 ); + ENSURE( edgeWins > 20 ); + return 0; +} + +int SeparatingAxisTest( void ) +{ + RUN_SUBTEST( FaceAxisASeparatedTest ); + RUN_SUBTEST( FaceAxisBSeparatedTest ); + RUN_SUBTEST( FaceFarSeparatedTest ); + RUN_SUBTEST( OffsetFaceAxisBTest ); + RUN_SUBTEST( EdgePairSweepTest ); + RUN_SUBTEST( SeparatingAxisOracleTest ); + RUN_SUBTEST( OffsetHullOracleTest ); + + return 0; +} diff --git a/test/test_world.c b/test/test_world.c index a94070e..69d0f37 100644 --- a/test/test_world.c +++ b/test/test_world.c @@ -3,7 +3,6 @@ #include "benchmarks.h" #include "overflow_color.h" -#include "stability.h" #include "test_macros.h" #include "box3d/box3d.h" @@ -659,47 +658,6 @@ static int TestSetWorkerCount( void ) return 0; } -// This tests continuous collision and mesh contact stability -static int TestMeshDrop( void ) -{ - b3WorldDef worldDef = b3DefaultWorldDef(); - // todo GetCoreCount - worldDef.workerCount = 4; - - b3WorldId worldId = b3CreateWorld( &worldDef ); - - MeshDropData data = CreateMeshDrop( worldId, b3Pos_zero ); - - float timeStep = 1.0f / 60.0f; - - int stepIndex = 0; - int stepLimit = 400; - - while ( stepIndex < stepLimit ) - { - int subStepCount = 4; - b3World_Step( worldId, timeStep, subStepCount ); - - b3BodyEvents events = b3World_GetBodyEvents( worldId ); - if ( events.moveCount == 0 ) - { - // All bodies sleeping - break; - } - - stepIndex += 1; - } - - printf( " TestMeshDrop stepIndex = %d\n", stepIndex ); - - DestroyMeshDrop( &data ); - b3DestroyWorld( worldId ); - - ENSURE( stepIndex < stepLimit ); - - return 0; -} - // Verifies the b3*_Overflow solver path. The scene puts >B3_DYNAMIC_COLOR_COUNT // dyn-dyn contacts on a single hub body so several land in the overflow color. // The new Prepare/Store contactId-pairing asserts in contact_solver.c fire here @@ -1080,7 +1038,6 @@ int WorldTest( void ) RUN_SUBTEST( TestContactEvents ); RUN_SUBTEST( TestHitEvents ); RUN_SUBTEST( TestCompoundHitEvents ); - RUN_SUBTEST( TestMeshDrop ); RUN_SUBTEST( TestOverflowColorPile ); RUN_SUBTEST( SetBulletDriftTest ); RUN_SUBTEST( EnableSleepFlagSyncTest );