From 79fc55976a041d63f160da1de3ef8d224249ce7e Mon Sep 17 00:00:00 2001 From: Erin Catto Date: Thu, 24 Sep 2026 12:21:17 -0700 Subject: [PATCH] Optimizations (#168) - Optimize contact prepare - Back to post restitution stage - Optimize solver math - Bug fixes --- .github/workflows/build.yml | 12 +- 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/sleep.csv | 16 +- benchmark/amd7950x_sse2/spinner.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/main.c | 1 + include/box3d/box3d.h | 16 +- include/box3d/constants.h | 4 +- include/box3d/types.h | 14 +- samples/CMakeLists.txt | 1 + samples/main.cpp | 40 +- samples/sample.cpp | 105 +- samples/sample.h | 18 +- samples/sample_continuous.cpp | 2 +- samples/sample_replay.cpp | 17 +- samples/sample_restitution.cpp | 1219 +++++++++++++++++++ samples/sample_shapes.cpp | 620 ---------- src/block_allocator.c | 6 +- src/block_allocator.h | 3 +- src/body.c | 8 +- src/body.h | 31 +- src/contact_solver.c | 1352 ++++++++++++--------- src/contact_solver.h | 7 +- src/core.c | 2 +- src/dynamic_tree.c | 15 +- src/physics_world.c | 55 +- src/physics_world.h | 4 + src/prismatic_joint.c | 4 +- src/recording.h | 2 +- src/recording_ops.inl | 2 + src/recording_replay.c | 11 + src/scheduler.c | 2 +- src/simd.h | 58 + src/solver.c | 51 +- src/solver.h | 4 +- src/types.c | 2 + src/weld_joint.c | 2 +- src/world_snapshot.c | 8 +- test/CMakeLists.txt | 1 + test/main.c | 2 + test/test_allocator.c | 4 +- test/test_broad_phase.c | 488 ++++++++ test/test_determinism.c | 20 +- test/test_joint.c | 60 + test/test_recording.c | 5 +- test/test_restitution.c | 307 +++-- 55 files changed, 3261 insertions(+), 1532 deletions(-) create mode 100644 samples/sample_restitution.cpp create mode 100644 test/test_broad_phase.c diff --git a/.github/workflows/build.yml b/.github/workflows/build.yml index 917430e..5e75d2c 100644 --- a/.github/workflows/build.yml +++ b/.github/workflows/build.yml @@ -15,7 +15,7 @@ jobs: test: name: ${{ matrix.name }} runs-on: ${{ matrix.os }} - timeout-minutes: ${{ matrix.timeout || 4 }} + timeout-minutes: ${{ matrix.timeout || 10 }} if: github.event_name != 'pull_request' || github.event.pull_request.draft == false strategy: fail-fast: false @@ -51,6 +51,8 @@ jobs: test: ./bin/Debug/test - name: windows-arm64 os: windows-11-arm + msvc: true + msvc-arch: arm64 args: -A arm64 -DBOX3D_DISABLE_SIMD=TRUE -DBOX3D_SANITIZE=ON test: ./bin/Debug/test # Optimized runs. The determinism golden hashes are otherwise only checked at -O0, @@ -80,7 +82,7 @@ jobs: if: ${{ matrix.msvc }} uses: TheMrMilchmann/setup-msvc-dev@v4 with: - arch: x64 + arch: ${{ matrix['msvc-arch'] || 'x64' }} - name: Configure CMake run: cmake -B ${{github.workspace}}/build -DCMAKE_BUILD_TYPE=${{ matrix.config || env.BUILD_TYPE }} -DBOX3D_SAMPLES=OFF -DBUILD_SHARED_LIBS=OFF -DBOX3D_VALIDATE=ON -DCMAKE_COMPILE_WARNING_AS_ERROR=ON ${{ matrix.args }} @@ -102,7 +104,7 @@ jobs: build-windows-mingw: name: windows-mingw runs-on: windows-latest - timeout-minutes: 4 + timeout-minutes: 10 if: github.event_name != 'pull_request' || github.event.pull_request.draft == false defaults: run: @@ -135,7 +137,7 @@ jobs: emscripten: name: emscripten runs-on: ubuntu-latest - timeout-minutes: 8 + timeout-minutes: 10 if: github.event_name != 'pull_request' || github.event.pull_request.draft == false steps: - uses: actions/checkout@v6 @@ -151,7 +153,7 @@ jobs: samples: name: ${{ matrix.name }} runs-on: ${{ matrix.os }} - timeout-minutes: 8 + timeout-minutes: 10 if: github.event_name != 'pull_request' || github.event.pull_request.draft == false strategy: fail-fast: false diff --git a/benchmark/amd7950x_sse2/convex_pile.csv b/benchmark/amd7950x_sse2/convex_pile.csv index ed6f055..a5f0847 100644 --- a/benchmark/amd7950x_sse2/convex_pile.csv +++ b/benchmark/amd7950x_sse2/convex_pile.csv @@ -1,9 +1,9 @@ threads,ms -1,13564.7 -2,7058.44 -3,4749.26 -4,3596.92 -5,2921.35 -6,2478.9 -7,2163.02 -8,1919.1 +1,13748 +2,6965.15 +3,4680.52 +4,3551.71 +5,2881.12 +6,2436.97 +7,2128.86 +8,1895.85 diff --git a/benchmark/amd7950x_sse2/joint_grid.csv b/benchmark/amd7950x_sse2/joint_grid.csv index a173bdc..fe1c753 100644 --- a/benchmark/amd7950x_sse2/joint_grid.csv +++ b/benchmark/amd7950x_sse2/joint_grid.csv @@ -1,9 +1,9 @@ threads,ms -1,1341.05 -2,697.405 -3,467.98 -4,355.631 -5,285.912 -6,240.761 -7,210.048 -8,187.1 +1,1367.75 +2,693.994 +3,464.069 +4,350.134 +5,282.582 +6,238.946 +7,207.608 +8,185.253 diff --git a/benchmark/amd7950x_sse2/junkyard.csv b/benchmark/amd7950x_sse2/junkyard.csv index 89de3af..d5a3c79 100644 --- a/benchmark/amd7950x_sse2/junkyard.csv +++ b/benchmark/amd7950x_sse2/junkyard.csv @@ -1,9 +1,9 @@ threads,ms -1,14783.1 -2,7859.85 -3,5367.48 -4,4115.03 -5,3368.76 -6,2874.23 -7,2535.89 -8,2277.2 +1,13672.8 +2,7175.79 +3,4913.8 +4,3784.71 +5,3108 +6,2664.34 +7,2355.17 +8,2130.17 diff --git a/benchmark/amd7950x_sse2/large_pyramid.csv b/benchmark/amd7950x_sse2/large_pyramid.csv index c1a9fc3..291be77 100644 --- a/benchmark/amd7950x_sse2/large_pyramid.csv +++ b/benchmark/amd7950x_sse2/large_pyramid.csv @@ -1,9 +1,9 @@ threads,ms -1,1852.7 -2,978.38 -3,665.353 -4,505.116 -5,411.56 -6,356.776 -7,307.866 -8,276.957 +1,1494.53 +2,777.388 +3,534.802 +4,410.022 +5,336.317 +6,293.644 +7,255.579 +8,231.247 diff --git a/benchmark/amd7950x_sse2/large_world.csv b/benchmark/amd7950x_sse2/large_world.csv index 9ff7d9a..0fd1d11 100644 --- a/benchmark/amd7950x_sse2/large_world.csv +++ b/benchmark/amd7950x_sse2/large_world.csv @@ -1,9 +1,9 @@ threads,ms -1,7.1447 -2,9.0736 -3,10.1672 -4,10.5088 -5,11.0298 -6,11.2858 -7,11.378 -8,11.4947 +1,6.3724 +2,8.1894 +3,9.2282 +4,9.5863 +5,9.7645 +6,9.7135 +7,10.0949 +8,10.2191 diff --git a/benchmark/amd7950x_sse2/many_pyramids.csv b/benchmark/amd7950x_sse2/many_pyramids.csv index dd14013..a301ab3 100644 --- a/benchmark/amd7950x_sse2/many_pyramids.csv +++ b/benchmark/amd7950x_sse2/many_pyramids.csv @@ -1,9 +1,9 @@ threads,ms -1,1947.41 -2,1002.95 -3,663.846 -4,499.942 -5,405.97 -6,336.977 -7,291.483 -8,255.782 +1,1716.24 +2,863.727 +3,564.346 +4,431.892 +5,350.505 +6,294.929 +7,249.838 +8,217.224 diff --git a/benchmark/amd7950x_sse2/rain.csv b/benchmark/amd7950x_sse2/rain.csv index 908ea42..b1ff82b 100644 --- a/benchmark/amd7950x_sse2/rain.csv +++ b/benchmark/amd7950x_sse2/rain.csv @@ -1,9 +1,9 @@ threads,ms -1,2061.72 -2,1139.07 -3,794.05 -4,622.555 -5,516.441 -6,446.451 -7,399.084 -8,364.345 +1,2058.5 +2,1111.94 +3,774.168 +4,613.062 +5,506.603 +6,441.273 +7,393.697 +8,364.168 diff --git a/benchmark/amd7950x_sse2/sleep.csv b/benchmark/amd7950x_sse2/sleep.csv index da3e5a9..3c9c52b 100644 --- a/benchmark/amd7950x_sse2/sleep.csv +++ b/benchmark/amd7950x_sse2/sleep.csv @@ -1,9 +1,9 @@ threads,ms -1,1051.05 -2,568.185 -3,390.055 -4,304.593 -5,251.978 -6,221.25 -7,200.171 -8,185.485 +1,841.682 +2,452.523 +3,317.294 +4,244.096 +5,209.411 +6,186.295 +7,169.097 +8,159.324 diff --git a/benchmark/amd7950x_sse2/spinner.csv b/benchmark/amd7950x_sse2/spinner.csv index 04d05dd..6ac177c 100644 --- a/benchmark/amd7950x_sse2/spinner.csv +++ b/benchmark/amd7950x_sse2/spinner.csv @@ -1,9 +1,9 @@ threads,ms -1,3152.3 -2,1832.83 -3,1315.75 -4,1058.49 -5,903.367 -6,801.254 -7,738.93 -8,695.326 +1,3133.28 +2,1789.12 +3,1283.52 +4,1028.48 +5,879.763 +6,792.053 +7,725.787 +8,700.573 diff --git a/benchmark/amd7950x_sse2/trees100.csv b/benchmark/amd7950x_sse2/trees100.csv index c276a5f..209bfb6 100644 --- a/benchmark/amd7950x_sse2/trees100.csv +++ b/benchmark/amd7950x_sse2/trees100.csv @@ -1,9 +1,9 @@ threads,ms -1,170.933 -2,112.916 -3,89.6041 -4,79.6461 -5,75.7754 -6,73.2911 -7,71.7742 -8,70.7292 +1,169.469 +2,108.928 +3,87.0377 +4,78.2135 +5,74.7472 +6,72.3567 +7,71.2555 +8,69.4727 diff --git a/benchmark/amd7950x_sse2/trees25.csv b/benchmark/amd7950x_sse2/trees25.csv index 00c842b..186e441 100644 --- a/benchmark/amd7950x_sse2/trees25.csv +++ b/benchmark/amd7950x_sse2/trees25.csv @@ -1,9 +1,9 @@ threads,ms -1,680.981 -2,400.129 -3,296.389 -4,252.676 -5,229.879 -6,214.117 -7,204.083 -8,196.839 +1,705.33 +2,395.666 +3,288.224 +4,250.774 +5,227.915 +6,213.331 +7,199.236 +8,197.301 diff --git a/benchmark/amd7950x_sse2/trees50.csv b/benchmark/amd7950x_sse2/trees50.csv index aafde5d..bd82cba 100644 --- a/benchmark/amd7950x_sse2/trees50.csv +++ b/benchmark/amd7950x_sse2/trees50.csv @@ -1,9 +1,9 @@ threads,ms -1,295.508 -2,183.122 -3,139.634 -4,121.466 -5,113.262 -6,106.697 -7,103.854 -8,101.959 +1,292.4 +2,176.437 +3,136.132 +4,119.305 +5,112.324 +6,104.356 +7,100.668 +8,101.573 diff --git a/benchmark/amd7950x_sse2/washer.csv b/benchmark/amd7950x_sse2/washer.csv index bf0a124..549fde1 100644 --- a/benchmark/amd7950x_sse2/washer.csv +++ b/benchmark/amd7950x_sse2/washer.csv @@ -1,9 +1,9 @@ threads,ms -1,19931.1 -2,10626.7 -3,7300.85 -4,5627.16 -5,4603.18 -6,3931.89 -7,3454.58 -8,3273.54 +1,19731.4 +2,10458 +3,7085.75 +4,5383.4 +5,4383.12 +6,3769.83 +7,3321.09 +8,3064.85 diff --git a/benchmark/main.c b/benchmark/main.c index fba4067..09a1f97 100644 --- a/benchmark/main.c +++ b/benchmark/main.c @@ -188,6 +188,7 @@ int main( int argc, char** argv ) .solveImpulses = FLT_MAX, .integratePositions = FLT_MAX, .relaxImpulses = FLT_MAX, + .restitution = FLT_MAX, .storeImpulses = FLT_MAX, .splitIslands = FLT_MAX, .transforms = FLT_MAX, diff --git a/include/box3d/box3d.h b/include/box3d/box3d.h index ad9933d..a48a15d 100644 --- a/include/box3d/box3d.h +++ b/include/box3d/box3d.h @@ -151,6 +151,19 @@ B3_API void b3World_SetRestitutionThreshold( b3WorldId worldId, float value ); /// Get the restitution speed threshold. Usually in meters per second. B3_API float b3World_GetRestitutionThreshold( b3WorldId worldId ); +/// Set the number of iterations used by the restitution solver. +/// @see B3_MAX_RESTITUTION_ITERATIONS +B3_API void b3World_SetRestitutionIterations( b3WorldId worldId, int iterations ); + +/// Get the number of iterations used by the restitution solver. +B3_API int b3World_GetRestitutionIterations( b3WorldId worldId ); + +/// Enable restitution propagation. +B3_API void b3World_EnableRestitutionPropagation( b3WorldId worldId, bool flag ); + +/// Is restitution propagation enabled? +B3_API bool b3World_IsRestitutionPropagationEnabled( b3WorldId worldId ); + /// Adjust the hit event threshold. This controls the collision speed needed to generate a b3ContactHitEvent. /// Usually in meters per second. /// @see b3WorldDef::hitEventThreshold @@ -242,9 +255,6 @@ B3_API void b3World_DumpMemoryStats( b3WorldId worldId ); /// Dump shape bounds to box3d_bounds.txt B3_API void b3World_DumpShapeBounds( b3WorldId worldId, b3BodyType type ); -/// This is for internal testing -B3_API void b3World_RebuildStaticTree( b3WorldId worldId ); - /// This is for internal testing B3_API void b3World_EnableSpeculative( b3WorldId worldId, bool flag ); diff --git a/include/box3d/constants.h b/include/box3d/constants.h index 288b5d8..cdc1910 100644 --- a/include/box3d/constants.h +++ b/include/box3d/constants.h @@ -105,6 +105,9 @@ B3_API float b3GetStallThreshold( void ); /// The time that a body must be still before it will go to sleep. In seconds. #define B3_TIME_TO_SLEEP 0.5f +/// The maximum number of restitution iterations. Needed to avoid a solver overflow. +#define B3_MAX_RESTITUTION_ITERATIONS 63 + /// The maximum number of contact points between two touching shapes. /// The default and minimum is 4 and this case uses a fast approximate hull to reduce the /// point count. A larger value builds a 2D convex hull of the candidate points and then @@ -141,7 +144,6 @@ B3_API float b3GetStallThreshold( void ); #define B3_MAX_SHAPES ( 1 << B3_SHAPE_POWER ) #define B3_MAX_CHILD_SHAPES ( 1 << B3_CHILD_POWER ) - /// This is the limit on how many mesh or heightfield triangles a single convex shape can collide with. /// Increasing this will increase stack usage, so be careful. I recommend to simplify your collision data /// before increasing this. For example, using render mesh for collision often leads to poor performance. diff --git a/include/box3d/types.h b/include/box3d/types.h index 08358b5..a06689e 100644 --- a/include/box3d/types.h +++ b/include/box3d/types.h @@ -147,6 +147,13 @@ typedef struct b3WorldDef /// speed have restitution applied (will bounce). float restitutionThreshold; + /// Number of iterations of the restitution solver. More iterations can lead to less box spinning. + /// @see B3_MAX_RESTITUTION_ITERATIONS + int restitutionIterations; + + /// Enable full contact propagation in the restitution solver. Expensive. + bool enableRestitutionPropagation; + /// Hit event speed threshold, usually in m/s. Collisions above this /// speed can generate hit events if the shape also enables hit events. float hitEventThreshold; @@ -553,6 +560,7 @@ typedef struct b3Profile float solveImpulses; // float integratePositions; // float relaxImpulses; // + float restitution; // float storeImpulses; // float splitIslands; // float transforms; // @@ -2645,9 +2653,6 @@ typedef struct b3ManifoldPoint /// The separation of the contact point, negative if penetrating float separation; - /// Cached separation used for contact recycling - float baseSeparation; - /// The impulse along the manifold normal vector. Since Box3D uses sub-stepping, this is /// result from the final sub-step. float normalImpulse; @@ -2662,6 +2667,9 @@ typedef struct b3ManifoldPoint /// computed if hit events are enabled. float normalVelocity; + /// Cached separation used for contact recycling + float baseSeparation; + /// Local point for matching /// Uniquely identifies a contact point between two shapes uint32_t featureId; diff --git a/samples/CMakeLists.txt b/samples/CMakeLists.txt index 9c9ca12..5a628e8 100644 --- a/samples/CMakeLists.txt +++ b/samples/CMakeLists.txt @@ -177,6 +177,7 @@ set(SAMPLE_FILES sample_mesh.cpp sample_ragdoll.cpp sample_replay.cpp + sample_restitution.cpp sample_robustness.cpp sample_shapes.cpp sample_stacking.cpp diff --git a/samples/main.cpp b/samples/main.cpp index 8f31d25..d5b32d3 100644 --- a/samples/main.cpp +++ b/samples/main.cpp @@ -194,7 +194,10 @@ static void OnEvent( const sapp_event* e ) } else { + // The replay viewer redraws the highlight from its own selection every + // frame, so it has to clear that too ClearSelection(); + s_context.sample->Keyboard( e->key_code, ACTION_PRESS, mods ); } break; @@ -238,41 +241,20 @@ static void OnEvent( const sapp_event* e ) break; case SAPP_KEYCODE_LEFT_BRACKET: - SelectSample( &s_context, b3MaxInt( 0, s_context.sampleIndex - 1 ), false ); + SelectSample( &s_context, ( s_context.sampleIndex + g_sampleCount - 1 ) % g_sampleCount, false ); break; case SAPP_KEYCODE_RIGHT_BRACKET: - SelectSample( &s_context, b3MinInt( g_sampleCount - 1, s_context.sampleIndex + 1 ), false ); + SelectSample( &s_context, ( s_context.sampleIndex + 1 ) % g_sampleCount, false ); break; case SAPP_KEYCODE_F: - { - // Frame the selection, or let the sample frame its whole scene when nothing is - // selected. A non-body selection such as a recorded query supplies its own bounds and - // takes priority over the hovered body. The replay viewer's scene lives in a - // player-owned world, not the base world, so the whole-scene case routes through - // FocusHome. - Camera& cam = s_context.camera; - float aspect = cam.m_height > 0 ? (float)cam.m_width / (float)cam.m_height : 1.0f; - b3AABB bounds; - if ( s_context.sample->FocusBounds( &bounds ) ) - { - cam.Frame( bounds, aspect, 1.5f ); - } - else - { - b3BodyId bodyId = s_context.sample->FocusBody(); - if ( B3_IS_NON_NULL( bodyId ) ) - { - cam.Frame( b3Body_ComputeAABB( bodyId ), aspect, 1.5f ); - } - else - { - s_context.sample->FocusHome(); - } - } - } - break; + FrameSelection( &s_context ); + break; + + case SAPP_KEYCODE_HOME: + s_context.sample->FocusHome(); + break; default: s_context.sample->Keyboard( e->key_code, ACTION_PRESS, mods ); diff --git a/samples/sample.cpp b/samples/sample.cpp index caf879d..8ddd8f7 100644 --- a/samples/sample.cpp +++ b/samples/sample.cpp @@ -442,6 +442,8 @@ void Sample::Step() b3World_EnableSleeping( m_worldId, m_context->enableSleep ); b3World_EnableWarmStarting( m_worldId, m_context->enableWarmStarting ); b3World_EnableContinuous( m_worldId, m_context->enableContinuous ); + b3World_SetRestitutionIterations( m_worldId, m_context->restitutionIterations ); + b3World_EnableRestitutionPropagation( m_worldId, m_context->enableRestitutionPropagation ); if ( timeStep > 0.0f || m_stepWhilePaused ) { @@ -538,9 +540,33 @@ b3BodyId Sample::FocusBody() const void Sample::FocusHome() { - b3AABB aabb = b3World_GetBounds( m_worldId ); - float aspect = m_camera->m_height > 0 ? (float)m_camera->m_width / (float)m_camera->m_height : 1.0f; - m_camera->Frame( aabb, aspect, 0.75f ); + m_camera->m_pivot = m_context->homePivot; + m_camera->SetOrbit( m_context->homeYaw, m_context->homePitch, m_context->homeRadius ); +} + +void FrameSelection( SampleContext* context ) +{ + // A non-body selection such as a recorded query supplies its own bounds and takes priority over + // the hovered body. With nothing selected the sample decides what home is. The replay viewer fits + // its recording, which lives in a player-owned world rather than the base world. + Sample* sample = context->sample; + Camera& cam = context->camera; + float aspect = cam.m_height > 0 ? (float)cam.m_width / (float)cam.m_height : 1.0f; + b3AABB bounds; + if ( sample->FocusBounds( &bounds ) ) + { + cam.Frame( bounds, aspect, 1.5f ); + return; + } + + b3BodyId bodyId = sample->FocusBody(); + if ( B3_IS_NON_NULL( bodyId ) ) + { + cam.Frame( b3Body_ComputeAABB( bodyId ), aspect, 1.5f ); + return; + } + + sample->FocusHome(); } void Sample::ResetProfile() @@ -621,7 +647,7 @@ void Sample::DrawMetrics() int count = m_profileWriteIndex - m_profileReadIndex; // Unroll ring buffer into per-field histories. - constexpr int kRowCount = 21; + constexpr int kRowCount = 22; float histories[kRowCount][m_profileCapacity]; float totals[kRowCount] = {}; for ( int i = 0; i < count; ++i ) @@ -640,15 +666,16 @@ void Sample::DrawMetrics() histories[9][i] = p.solveImpulses; histories[10][i] = p.integratePositions; histories[11][i] = p.relaxImpulses; - histories[12][i] = p.storeImpulses; - histories[13][i] = p.splitIslands; - histories[14][i] = p.transforms; - histories[15][i] = p.jointEvents; - histories[16][i] = p.hitEvents; - histories[17][i] = p.refit; - histories[18][i] = p.sleepIslands; - histories[19][i] = p.bullets; - histories[20][i] = p.sensors; + histories[12][i] = p.restitution; + histories[13][i] = p.storeImpulses; + histories[14][i] = p.splitIslands; + histories[15][i] = p.transforms; + histories[16][i] = p.jointEvents; + histories[17][i] = p.hitEvents; + histories[18][i] = p.refit; + histories[19][i] = p.sleepIslands; + histories[20][i] = p.bullets; + histories[21][i] = p.sensors; totals[0] += p.step; totals[1] += p.pairs; @@ -662,15 +689,16 @@ void Sample::DrawMetrics() totals[9] += p.solveImpulses; totals[10] += p.integratePositions; totals[11] += p.relaxImpulses; - totals[12] += p.storeImpulses; - totals[13] += p.splitIslands; - totals[14] += p.transforms; - totals[15] += p.jointEvents; - totals[16] += p.hitEvents; - totals[17] += p.refit; - totals[18] += p.sleepIslands; - totals[19] += p.bullets; - totals[20] += p.sensors; + totals[12] += p.restitution; + totals[13] += p.storeImpulses; + totals[14] += p.splitIslands; + totals[15] += p.transforms; + totals[16] += p.jointEvents; + totals[17] += p.hitEvents; + totals[18] += p.refit; + totals[19] += p.sleepIslands; + totals[20] += p.bullets; + totals[21] += p.sensors; } // Smoothed over the last few frames so bars don't jitter visibly. @@ -730,7 +758,7 @@ void Sample::DrawMetrics() { "solve", 0, colorSolve }, { "setup", 1, colorDefault }, { "constraints", 1, colorDefault }, { "prepare", 2, colorDefault }, { "velocities", 2, colorDefault }, { "warm start", 2, colorDefault }, { "bias", 2, colorDefault }, { "positions", 2, colorDefault }, { "relax", 2, colorDefault }, - { "store", 2, colorDefault }, { "split islands", 2, colorDefault }, + { "restitution", 2, colorDefault }, { "store", 2, colorDefault }, { "split islands", 2, colorDefault }, { "transforms", 1, colorDefault }, { "joint events", 1, colorDefault }, { "hit events", 1, colorDefault }, { "refit BVH", 1, colorDefault }, { "sleep", 1, colorDefault }, { "bullets", 1, colorDefault }, { "sensors", 0, colorSensors }, @@ -1354,6 +1382,15 @@ void SelectSample( SampleContext* context, int selection, bool restart ) context->restart = restart; context->sample = g_sampleEntries[selection].CreateFcn( context ); + // A restart keeps the camera where it was, so the original starting view stays home + if ( restart == false ) + { + context->homePivot = context->camera.m_pivot; + context->homeYaw = context->camera.m_yaw; + context->homePitch = context->camera.m_pitch; + context->homeRadius = context->camera.m_radius; + } + // A sample that knows where its content sits relative to its camera says // so, and is taken at its word. The ceiling below guards a guess, not a // measurement, so capping an explicit request would only hide it. @@ -1678,11 +1715,11 @@ static void DrawMenuBar( SampleContext* context ) ImGui::Separator(); if ( ImGui::MenuItem( "Previous Sample", "[" ) ) { - SelectSample( context, b3MaxInt( 0, context->sampleIndex - 1 ), false ); + SelectSample( context, ( context->sampleIndex + g_sampleCount - 1 ) % g_sampleCount, false ); } if ( ImGui::MenuItem( "Next Sample", "]" ) ) { - SelectSample( context, b3MinInt( g_sampleCount - 1, context->sampleIndex + 1 ), false ); + SelectSample( context, ( context->sampleIndex + 1 ) % g_sampleCount, false ); } ImGui::Separator(); if ( ImGui::MenuItem( "Reset Profile" ) ) @@ -1694,7 +1731,7 @@ static void DrawMenuBar( SampleContext* context ) b3World_DumpMemoryStats( context->sample->m_worldId ); } ImGui::Separator(); - if ( ImGui::MenuItem( "Quit", "Esc" ) ) + if ( ImGui::MenuItem( "Quit", "Ctrl+Q" ) ) { sapp_request_quit(); } @@ -1707,12 +1744,13 @@ static void DrawMenuBar( SampleContext* context ) { context->showUI = false; } - if ( ImGui::MenuItem( "Frame Camera" ) ) + if ( ImGui::MenuItem( "Frame Selection", "F" ) ) { - b3AABB aabb = b3World_GetBounds( context->sample->m_worldId ); - Camera& cam = context->camera; - float aspect = cam.m_height > 0 ? (float)cam.m_width / (float)cam.m_height : 1.0f; - cam.Frame( aabb, aspect, 0.75f ); + FrameSelection( context ); + } + if ( ImGui::MenuItem( "Reset Camera", "Home" ) ) + { + context->sample->FocusHome(); } ImGui::MenuItem( "Shapes", nullptr, &gd->drawShapes ); if ( ImGui::BeginMenu( "Transparency" ) ) @@ -1844,7 +1882,8 @@ static void DrawMenuBar( SampleContext* context ) DrawRow( "R", "Restart sample" ); DrawRow( "[ ]", "Previous / next sample" ); DrawRow( "Ctrl+O", "Open sample picker" ); - DrawRow( "F", "Frame selection / world" ); + DrawRow( "F", "Frame selection" ); + DrawRow( "Home", "Reset camera" ); DrawRow( "?", "Show / hide controls" ); DrawRow( "Esc", "Cancel / close" ); DrawRow( "Ctrl+Q", "Quit" ); @@ -2046,6 +2085,7 @@ static void DrawInfoPanel( SampleContext* context ) { ImGui::PushItemWidth( 6.0f * fontSize ); ImGui::SliderInt( "Sub-steps##Solver", &context->subStepCount, 1, 50 ); + ImGui::SliderInt( "Rest Iters##Solver", &context->restitutionIterations, 0, 8 ); ImGui::SliderFloat( "Hertz##Solver", &context->hertz, 5.0f, 240.0f, "%.0f hz" ); if ( ImGui::SliderInt( "Workers##Solver", &context->workerCount, 1, B3_MAX_WORKERS ) ) @@ -2065,6 +2105,7 @@ static void DrawInfoPanel( SampleContext* context ) ImGui::Checkbox( "Sleep##Solver", &context->enableSleep ); ImGui::Checkbox( "Warm Starting##Solver", &context->enableWarmStarting ); ImGui::Checkbox( "Continuous##Solver", &context->enableContinuous ); + ImGui::Checkbox( "Rest Prop##Solver", &context->enableRestitutionPropagation ); if ( ImGui::Shortcut( ImGuiKey_R ) || ImGui::Button( "Restart" ) ) { diff --git a/samples/sample.h b/samples/sample.h index b283b3b..ded069a 100644 --- a/samples/sample.h +++ b/samples/sample.h @@ -57,12 +57,21 @@ struct SampleContext float hertz = 60.0f; float recycleDistance = 0.05f; float drawDistance = 100.0f; // meters, view/cull box half extent, persisted + + // Camera the sample set up on creation. Home returns to it. Fitting the world bounds instead + // can put a scene with a large ground plate beyond the draw distance. + b3Pos homePivot = {}; + float homeYaw = 0.0f; + float homePitch = 0.0f; + float homeRadius = 0.0f; int subStepCount = 4; + int restitutionIterations = 2; int workerCount = 1; bool transparentDynamic = false; bool transparentKinematic = false; bool enableWarmStarting = true; bool enableContinuous = true; + bool enableRestitutionPropagation = false; bool enableSleep = true; bool pause = false; int singleStep = 0; @@ -150,9 +159,9 @@ public: // framing works regardless of where the cursor sits. virtual b3BodyId FocusBody() const; - // Frame shortcut with nothing selected: fit the whole scene. Defaults to the live world bounds. - // The replay viewer overrides this to fit the recording, whose world is player-owned and separate - // from the empty base world. + // Home, and the frame shortcut with nothing selected. Defaults to the camera the sample set up on + // creation. The replay viewer overrides this to fit the recording, whose world is player-owned and + // separate from the empty base world. virtual void FocusHome(); // Arm recording on the live world, snapshotting it as the seed so capture can begin at any @@ -271,6 +280,9 @@ int RegisterReplay( const char* category, const char* name, SampleCreateFcn* fcn // by leaving the restart flag set while the new sample constructs. void SelectSample( SampleContext* context, int selection, bool restart ); +// Frame the selection, or go home when nothing is selected +void FrameSelection( SampleContext* context ); + // Run the native "open replay" file picker and, on success, hand the chosen // file to the Replay viewer. Must be called outside the frame (the dialog spins // a blocking nested run loop), driven by SampleContext::openReplayPicker. diff --git a/samples/sample_continuous.cpp b/samples/sample_continuous.cpp index 4911b7b..ee2137a 100644 --- a/samples/sample_continuous.cpp +++ b/samples/sample_continuous.cpp @@ -1033,7 +1033,7 @@ static int sampleStall = RegisterSample( "Continuous", "Stall", Stall::Create ); // The best way to run this sample is: // 1. press pause (P) // 2. restart (R) or press the Drop button -// 3. then single step (O) +// 3. then single step (.) // Then look at the metrics and overlap. The shape is drawn orange while it is using // continuous collision detection. class SafetyFactor : public Sample diff --git a/samples/sample_replay.cpp b/samples/sample_replay.cpp index 0f7c87f..328020c 100644 --- a/samples/sample_replay.cpp +++ b/samples/sample_replay.cpp @@ -395,6 +395,14 @@ public: m_camera->SetRenderTransform( b3GetLengthUnitsPerMeter(), m_context->viewZUp ); float aspect = m_camera->m_height > 0 ? (float)m_camera->m_width / (float)m_camera->m_height : 1.0f; m_camera->Frame( m_info.bounds, aspect, 0.75f ); + + // The bounds include static geometry, and a large ground plate can push the fit past the + // draw distance, which culls the whole scene. Pull in so the middle of the scene still draws. + float maxRadius = 0.75f * m_context->drawDistance; + if ( m_camera->m_radius > maxRadius ) + { + m_camera->SetOrbit( m_camera->m_yaw, m_camera->m_pitch, maxRadius ); + } } } @@ -523,7 +531,8 @@ public: } // , steps backward. Forward is the global single step on . so it works in every sample, and the - // step count lands in the context. Shift moves five frames, matching that key. + // step count lands in the context. Shift moves five frames, matching that key. Esc drops the + // selection. void Keyboard( int key, int action, int mods ) override { if ( m_generating ) @@ -536,7 +545,11 @@ public: return; } - if ( key == KEY_COMMA ) + if ( key == KEY_ESCAPE ) + { + m_selKind = SelNone; + } + else if ( key == KEY_COMMA ) { int back = ( mods & MOD_SHIFT ) ? 5 : 1; SeekTo( b3RecPlayer_GetFrame( m_player ) - back ); diff --git a/samples/sample_restitution.cpp b/samples/sample_restitution.cpp new file mode 100644 index 0000000..d4be176 --- /dev/null +++ b/samples/sample_restitution.cpp @@ -0,0 +1,1219 @@ +// SPDX-FileCopyrightText: 2026 Erin Catto +// SPDX-License-Identifier: MIT + +#include "gfx/debug_adapter.h" +#include "gfx/draw.h" +#include "imgui.h" +#include "sample.h" + +#include "box3d/box3d.h" + +#include + +// Restitution is approximate since Box3D uses speculative collision +class VaryingRestitution : public Sample +{ +public: + enum ShapeType + { + e_sphereShape = 0, + e_boxShape + }; + + explicit VaryingRestitution( SampleContext* context ) + : Sample( context ) + { + if ( context->restart == false ) + { + m_camera->SetView( 0.0f, 25.0f, 85.0f, { 0.0f, 20.0f, 0.0f } ); + } + + AddGroundBox( 50.0f ); + + m_shapeType = e_sphereShape; + + CreateBodies(); + } + + void CreateBodies() + { + for ( int i = 0; i < m_count; ++i ) + { + if ( B3_IS_NON_NULL( m_bodyIds[i] ) ) + { + b3DestroyBody( m_bodyIds[i] ); + m_bodyIds[i] = b3_nullBodyId; + } + } + + b3Sphere sphere = { b3Vec3_zero, 0.5f }; + b3BoxHull box = b3MakeCubeHull( 0.5f ); + + b3ShapeDef shapeDef = b3DefaultShapeDef(); + shapeDef.density = 1.0f; + shapeDef.baseMaterial.restitution = 0.0f; + shapeDef.baseMaterial.friction = 0.0f; + + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.type = b3_dynamicBody; + + float dr = 1.0f / ( m_count > 1 ? m_count - 1 : 1 ); + float x = -1.0f * ( m_count - 1 ); + float dx = 2.0f; + + for ( int i = 0; i < m_count; ++i ) + { + char buffer[32]; + snprintf( buffer, 32, "%.2f", shapeDef.baseMaterial.restitution ); + bodyDef.name = buffer; + bodyDef.position = { x, 40.0f, 0.0f }; + b3BodyId bodyId = b3CreateBody( m_worldId, &bodyDef ); + + m_bodyIds[i] = bodyId; + + if ( m_shapeType == e_sphereShape ) + { + b3CreateSphereShape( bodyId, &shapeDef, &sphere ); + } + else + { + b3CreateHullShape( bodyId, &shapeDef, &box.base ); + } + + shapeDef.baseMaterial.restitution += dr; + x += dx; + } + } + + bool DrawControls() override + { + ImGui::PushItemWidth( 6.0f * ImGui::GetFontSize() ); + + bool changed = false; + const char* shapeTypes[] = { "Sphere", "Box" }; + + int shapeType = int( m_shapeType ); + changed = changed || ImGui::Combo( "Shape", &shapeType, shapeTypes, IM_ARRAYSIZE( shapeTypes ) ); + m_shapeType = ShapeType( shapeType ); + + ImGui::PopItemWidth(); + + changed = changed || ImGui::Button( "Reset" ); + + if ( changed ) + { + CreateBodies(); + } + + return true; + } + + void Step() override + { + Sample::Step(); + + float h = 1.0f * m_count; + DrawLine( { -h, 40.5f, 0.0f }, { h, 40.5f, 0.0f }, MakeColor( b3_colorRed ) ); + } + + static Sample* Create( SampleContext* context ) + { + return new VaryingRestitution( context ); + } + + static constexpr int m_count = 40; + + b3BodyId m_bodyIds[m_count] = {}; + ShapeType m_shapeType; +}; + +static int sampleVaryingRestitution = RegisterSample( "Restitution", "Varying", VaryingRestitution::Create ); + +// Tests how a single bouncing box behaves. +class SingleBoxRestitution : public Sample +{ +public: + explicit SingleBoxRestitution( SampleContext* context ) + : Sample( context ) + { + if ( context->restart == false ) + { + m_camera->SetView( 20.0f, 10.0f, 16.0f, { 0.0f, 3.0f, 0.0f } ); + } + + AddGroundBox( 4.0f ); + + m_height = 5.0f; + m_maxY = 5.0f; + + b3BoxHull box = b3MakeCubeHull( 0.5f ); + + b3ShapeDef shapeDef = b3DefaultShapeDef(); + shapeDef.density = 1.0f; + shapeDef.baseMaterial.restitution = 1.0f; + shapeDef.baseMaterial.friction = 0.0f; + shapeDef.enableHitEvents = true; + + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.type = b3_dynamicBody; + bodyDef.position = { 0.0f, m_height, 0.0f }; + bodyDef.safetyFactor = 0.01f; + m_bodyId = b3CreateBody( m_worldId, &bodyDef ); + b3CreateHullShape( m_bodyId, &shapeDef, &box.base ); + + m_startEnergy = MeasureEnergy( m_worldId, &m_bodyId, 1 ).Total(); + m_peakEnergy = m_startEnergy; + } + + void Step() override + { + Sample::Step(); + + DrawLine( { -4.0f, m_height + 0.5f, 0.0f }, { 4.0f, m_height + 0.5f, 0.0f }, MakeColor( b3_colorRed ) ); + + b3ContactEvents events = b3World_GetContactEvents( m_worldId ); + + b3Pos p = b3Body_GetPosition( m_bodyId ); + if ( events.hitCount == 1 ) + { + m_maxY = (float)p.y; + } + else + { + m_maxY = b3MaxFloat( m_maxY, (float)p.y ); + } + + DrawTextLine( "maxY = %.2f", m_maxY ); + + MechanicalEnergy energy = MeasureEnergy( m_worldId, &m_bodyId, 1 ); + float total = energy.Total(); + m_peakEnergy = b3MaxFloat( m_peakEnergy, total ); + + float scale = m_startEnergy != 0.0f ? 100.0f / m_startEnergy : 0.0f; + DrawTextLine( "kinetic = %.3f J linear + %.3f J angular", energy.linear, energy.angular ); + DrawTextLine( "potential = %.3f J", energy.potential ); + DrawTextLine( "total = %.3f J (%.2f%% of start, peak %.2f%%)", total, scale * total, scale * m_peakEnergy ); + } + + static Sample* Create( SampleContext* context ) + { + return new SingleBoxRestitution( context ); + } + + b3BodyId m_bodyId; + float m_maxY; + float m_startEnergy = 0.0f; + float m_peakEnergy = 0.0f; + float m_height; +}; + +static int sampleSingleBoxRestitution = RegisterSample( "Restitution", "Single Box Restitution", SingleBoxRestitution::Create ); + +class SingleSphereRestitution : public Sample +{ +public: + explicit SingleSphereRestitution( SampleContext* context ) + : Sample( context ) + { + if ( context->restart == false ) + { + m_camera->SetView( 20.0f, 10.0f, 24.0f, { 0.0f, 5.0f, 0.0f } ); + } + + AddGroundBox( 4.0f ); + + b3Sphere sphere = { b3Vec3_zero, 0.5f }; + + b3ShapeDef shapeDef = b3DefaultShapeDef(); + shapeDef.density = 1.0f; + shapeDef.baseMaterial.restitution = 1.0f; + shapeDef.baseMaterial.friction = 0.0f; + shapeDef.enableHitEvents = true; + + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.type = b3_dynamicBody; + bodyDef.position = { 0.0f, 10.0f, 0.0f }; + bodyDef.safetyFactor = 0.01f; + m_bodyId = b3CreateBody( m_worldId, &bodyDef ); + b3CreateSphereShape( m_bodyId, &shapeDef, &sphere ); + + m_maxY = (float)bodyDef.position.y; + + m_startEnergy = MeasureEnergy( m_worldId, &m_bodyId, 1 ).Total(); + m_peakEnergy = m_startEnergy; + } + + void Step() override + { + Sample::Step(); + + b3ContactEvents events = b3World_GetContactEvents( m_worldId ); + + b3Pos p = b3Body_GetPosition( m_bodyId ); + if ( events.hitCount == 1 ) + { + m_maxY = (float)p.y; + } + else + { + m_maxY = b3MaxFloat( m_maxY, (float)p.y ); + } + + DrawLine( { -4.0f, 10.5f, 0.0f }, { 4.0f, 10.5f, 0.0f }, MakeColor( b3_colorRed ) ); + + DrawTextLine( "maxY = %.2f", m_maxY ); + + MechanicalEnergy energy = MeasureEnergy( m_worldId, &m_bodyId, 1 ); + float total = energy.Total(); + m_peakEnergy = b3MaxFloat( m_peakEnergy, total ); + + float scale = m_startEnergy != 0.0f ? 100.0f / m_startEnergy : 0.0f; + DrawTextLine( "kinetic = %.3f J linear + %.3f J angular", energy.linear, energy.angular ); + DrawTextLine( "potential = %.3f J", energy.potential ); + DrawTextLine( "total = %.3f J (%.2f%% of start, peak %.2f%%)", total, scale * total, scale * m_peakEnergy ); + } + + static Sample* Create( SampleContext* context ) + { + return new SingleSphereRestitution( context ); + } + + b3BodyId m_bodyId; + float m_maxY; + float m_startEnergy = 0.0f; + float m_peakEnergy = 0.0f; +}; + +static int sampleSingleSphereRestitution = + RegisterSample( "Restitution", "Single Sphere Restitution", SingleSphereRestitution::Create ); + +// Similar to the MeasureSupportedBounce unit test +class SphereStackRestitution : public Sample +{ +public: + static constexpr float m_impactSpeed = 5.0f; + static constexpr int m_maxStackCount = 3; + + explicit SphereStackRestitution( SampleContext* context ) + : Sample( context ) + { + if ( context->restart == false ) + { + m_camera->SetView( 20.0f, 10.0f, 12.0f, { 0.0f, 2.5f, 0.0f } ); + } + + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.position = { 0.0f, -1.0f, 0.0f }; + b3BodyId groundId = b3CreateBody( m_worldId, &bodyDef ); + + b3ShapeDef shapeDef = b3DefaultShapeDef(); + shapeDef.baseMaterial.friction = 0.0f; + shapeDef.baseMaterial.restitution = 0.0f; + b3BoxHull box = b3MakeBoxHull( 40.0f, 1.0f, 40.0f ); + b3ShapeId groundShapeId = b3CreateHullShape( groundId, &shapeDef, &box.base ); + SetGroundShape( groundShapeId ); + + CreateScene(); + } + + b3BodyId CreateBall( float y, float velocityY, float restitution, bool hitEvents ) + { + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.type = b3_dynamicBody; + bodyDef.position = { 0.0f, y, 0.0f }; + bodyDef.linearVelocity = { 0.0f, velocityY, 0.0f }; + bodyDef.enableSleep = false; + b3BodyId bodyId = b3CreateBody( m_worldId, &bodyDef ); + + b3ShapeDef shapeDef = b3DefaultShapeDef(); + shapeDef.baseMaterial.friction = 0.0f; + shapeDef.baseMaterial.restitution = restitution; + shapeDef.enableHitEvents = hitEvents; + b3Sphere sphere = { b3Vec3_zero, 0.5f }; + b3CreateSphereShape( bodyId, &shapeDef, &sphere ); + + return bodyId; + } + + void CreateScene() + { + for ( int i = 0; i < m_bodyCount; ++i ) + { + b3DestroyBody( m_bodyIds[i] ); + } + m_bodyCount = 0; + + if ( m_gravity ) + { + b3World_SetGravity( m_worldId, { 0.0f, -10.0f, 0.0f } ); + } + else + { + b3World_SetGravity( m_worldId, b3Vec3_zero ); + } + + for ( int i = 0; i < m_stackCount; ++i ) + { + m_bodyIds[m_bodyCount] = CreateBall( 0.5f + 1.0f * i, 0.0f, 0.0f, false ); + m_bodyCount += 1; + } + + // Start half a step of travel above contact so the impact lands mid step, matching the test + m_startHeight = 0.5f + 1.0f * m_stackCount + 0.5f * m_impactSpeed * ( 1.0f / 60.0f ); + + m_impactorId = CreateBall( m_startHeight, -m_impactSpeed, m_restitution, true ); + m_bodyIds[m_bodyCount] = m_impactorId; + m_bodyCount += 1; + + m_startEnergy = MeasureEnergy( m_worldId, m_bodyIds, m_bodyCount ).Total(); + m_peakEnergy = m_startEnergy; + + m_coefficient = 0.0f; + m_latched = false; + m_hit = false; + } + + bool DrawControls() override + { + bool rebuild = false; + + ImGui::PushItemWidth( 6.0f * ImGui::GetFontSize() ); + + if ( ImGui::SliderFloat( "Restitution", &m_restitution, 0.0f, 1.0f, "%.2f" ) ) + { + rebuild = true; + } + + if ( ImGui::SliderInt( "Stack", &m_stackCount, 0, m_maxStackCount ) ) + { + rebuild = true; + } + + ImGui::PopItemWidth(); + + if ( ImGui::Checkbox( "Gravity", &m_gravity ) ) + { + rebuild = true; + } + + if ( ImGui::Button( "Reset" ) ) + { + rebuild = true; + } + + if ( rebuild ) + { + CreateScene(); + } + + return true; + } + + void Step() override + { + Sample::Step(); + + b3ContactEvents events = b3World_GetContactEvents( m_worldId ); + if ( events.hitCount > 0 ) + { + m_hit = true; + } + + float vy = b3Body_GetLinearVelocity( m_impactorId ).y; + + // The rebound only means something once the impact has happened and the impactor is leaving + if ( m_latched == false && m_hit && vy > 0.0f ) + { + m_coefficient = vy / m_impactSpeed; + m_latched = true; + } + + float supportSpeed = 0.0f; + for ( int i = 0; i < m_stackCount; ++i ) + { + supportSpeed = b3MaxFloat( supportSpeed, b3Length( b3Body_GetLinearVelocity( m_bodyIds[i] ) ) ); + } + + DrawLine( { -2.0f, m_startHeight, 0.0f }, { 2.0f, m_startHeight, 0.0f }, MakeColor( b3_colorRed ) ); + + MechanicalEnergy energy = MeasureEnergy( m_worldId, m_bodyIds, m_bodyCount ); + float total = energy.Total(); + m_peakEnergy = b3MaxFloat( m_peakEnergy, total ); + + DrawTextLine( "impactor vy = %.3f m/s", vy ); + + DrawTextLine( "coefficient = %.4f live (target %.2f)", vy / m_impactSpeed, m_restitution ); + + if ( m_latched ) + { + DrawTextLine( "coefficient = %.4f at first rebound", m_coefficient ); + } + + DrawTextLine( "support speed = %.4f m/s", supportSpeed ); + + float scale = m_startEnergy != 0.0f ? 100.0f / m_startEnergy : 0.0f; + DrawTextLine( "kinetic = %.3f J linear + %.3f J angular", energy.linear, energy.angular ); + DrawTextLine( "potential = %.3f J", energy.potential ); + DrawTextLine( "total = %.3f J (%.2f%% of start, peak %.2f%%)", total, scale * total, scale * m_peakEnergy ); + } + + static Sample* Create( SampleContext* context ) + { + return new SphereStackRestitution( context ); + } + + b3BodyId m_bodyIds[m_maxStackCount + 1] = {}; + b3BodyId m_impactorId = b3_nullBodyId; + int m_bodyCount = 0; + int m_stackCount = 2; + float m_restitution = 1.0f; + float m_startHeight = 0.0f; + float m_coefficient = 0.0f; + float m_startEnergy = 0.0f; + float m_peakEnergy = 0.0f; + bool m_gravity = false; + bool m_latched = false; + bool m_hit = false; +}; + +static int sampleSphereStackRestitution = RegisterSample( "Restitution", "Sphere Stack Restitution", SphereStackRestitution::Create ); + +class RestitutionPropagation : public Sample +{ +public: + static constexpr int m_columnCount = 3; + static constexpr float m_dropHeight = 5.0f; + + explicit RestitutionPropagation( SampleContext* context ) + : Sample( context ) + { + if ( context->restart == false ) + { + m_camera->SetView( 0.0f, 10.0f, 24.0f, { 0.0f, 4.0f, 0.0f } ); + } + + AddGroundBox( 40.0f ); + + CreateScene(); + } + + void CreateScene() + { + for ( int i = 0; i < m_bodyCount; ++i ) + { + b3DestroyBody( m_bodyIds[i] ); + } + m_bodyCount = 0; + m_crateCount = 0; + m_kick = 0.0f; + + b3BoxHull box = b3MakeCubeHull( 0.5f ); + b3Sphere sphere = { b3Vec3_zero, 0.5f }; + + for ( int column = 0; column < m_columnCount; ++column ) + { + float x = 4.0f * ( column - 1 ); + + for ( int k = 0; k < column; ++k ) + { + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.type = b3_dynamicBody; + bodyDef.position = { x, 0.5f + 1.0f * k, 0.0f }; + bodyDef.enableSleep = false; + b3BodyId crateId = b3CreateBody( m_worldId, &bodyDef ); + + b3ShapeDef shapeDef = b3DefaultShapeDef(); + b3CreateHullShape( crateId, &shapeDef, &box.base ); + + m_bodyIds[m_bodyCount++] = crateId; + m_crateIds[m_crateCount++] = crateId; + } + + m_restHeight[column] = 0.5f + 1.0f * column; + + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.type = b3_dynamicBody; + bodyDef.position = { x, m_restHeight[column] + m_dropHeight, 0.0f }; + bodyDef.enableSleep = false; + b3BodyId ballId = b3CreateBody( m_worldId, &bodyDef ); + + b3ShapeDef shapeDef = b3DefaultShapeDef(); + shapeDef.baseMaterial.restitution = m_restitution; + b3CreateSphereShape( ballId, &shapeDef, &sphere ); + + m_bodyIds[m_bodyCount++] = ballId; + m_ballIds[column] = ballId; + m_apex[column] = 0.0f; + m_rebounding[column] = false; + m_done[column] = false; + } + } + + bool DrawControls() override + { + ImGui::PushItemWidth( 6.0f * ImGui::GetFontSize() ); + bool rebuild = ImGui::SliderFloat( "Restitution", &m_restitution, 0.0f, 1.0f, "%.2f" ); + ImGui::PopItemWidth(); + + if ( ImGui::Button( "Reset" ) ) + { + rebuild = true; + } + + if ( rebuild ) + { + CreateScene(); + } + + return true; + } + + void Step() override + { + Sample::Step(); + + static const char* labels[m_columnCount] = { "ground", "one crate", "two crates" }; + + for ( int column = 0; column < m_columnCount; ++column ) + { + b3Vec3 v = b3Body_GetLinearVelocity( m_ballIds[column] ); + float height = float( b3Body_GetPosition( m_ballIds[column] ).y ) - m_restHeight[column]; + + // The ball starts at rest and falls, so upward velocity means it bounced + if ( m_rebounding[column] == false && v.y > 0.0f ) + { + m_rebounding[column] = true; + } + + if ( m_rebounding[column] && m_done[column] == false ) + { + m_apex[column] = b3MaxFloat( m_apex[column], height ); + if ( v.y < 0.0f ) + { + m_done[column] = true; + } + } + + float x = 4.0f * ( column - 1 ); + float dropY = m_restHeight[column] + m_dropHeight; + DrawLine( { x - 1.0f, dropY, 0.0f }, { x + 1.0f, dropY, 0.0f }, MakeColor( b3_colorRed ) ); + if ( m_apex[column] > 0.0f ) + { + float apexY = m_restHeight[column] + m_apex[column]; + DrawLine( { x - 1.0f, apexY, 0.0f }, { x + 1.0f, apexY, 0.0f }, MakeColor( b3_colorGreen ) ); + } + + DrawTextLine( "%s: apex %.2f m (%.0f%% of drop)", labels[column], m_apex[column], + 100.0f * m_apex[column] / m_dropHeight ); + } + + for ( int i = 0; i < m_crateCount; ++i ) + { + m_kick = b3MaxFloat( m_kick, b3Length( b3Body_GetLinearVelocity( m_crateIds[i] ) ) ); + } + + DrawTextLine( "peak crate speed %.2f m/s", m_kick ); + } + + static Sample* Create( SampleContext* context ) + { + return new RestitutionPropagation( context ); + } + + b3BodyId m_bodyIds[2 * m_columnCount] = {}; + b3BodyId m_ballIds[m_columnCount] = {}; + b3BodyId m_crateIds[m_columnCount] = {}; + float m_restHeight[m_columnCount] = {}; + float m_apex[m_columnCount] = {}; + bool m_rebounding[m_columnCount] = {}; + bool m_done[m_columnCount] = {}; + int m_bodyCount = 0; + int m_crateCount = 0; + float m_restitution = 0.9f; + float m_kick = 0.0f; +}; + +static int sampleRestitutionPropagation = RegisterSample( "Restitution", "Restitution Propagation", RestitutionPropagation::Create ); + +// Similar to the MeasureFlatBounce and SpinTest unit tests +class BoxRestitution : public Sample +{ +public: + static constexpr float m_impactSpeed = 5.0f; + + explicit BoxRestitution( SampleContext* context ) + : Sample( context ) + { + if ( context->restart == false ) + { + m_camera->SetView( 20.0f, 10.0f, 14.0f, { 0.0f, 3.0f, 0.0f } ); + } + + // Gravity would bias the measured coefficient + b3World_SetGravity( m_worldId, b3Vec3_zero ); + + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.position = { 0.0f, -1.0f, 0.0f }; + b3BodyId groundId = b3CreateBody( m_worldId, &bodyDef ); + + b3ShapeDef shapeDef = b3DefaultShapeDef(); + shapeDef.baseMaterial.friction = 0.0f; + shapeDef.baseMaterial.restitution = 0.0f; + b3BoxHull box = b3MakeBoxHull( 40.0f, 1.0f, 40.0f ); + b3ShapeId groundShapeId = b3CreateHullShape( groundId, &shapeDef, &box.base ); + SetGroundShape( groundShapeId ); + + CreateScene(); + } + + void CreateScene() + { + if ( B3_IS_NON_NULL( m_boxId ) ) + { + b3DestroyBody( m_boxId ); + } + + // Start half a step of travel above contact so the impact lands mid step, matching the test + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.type = b3_dynamicBody; + bodyDef.position = { 0.0f, 0.25f + 0.5f * m_impactSpeed * ( 1.0f / 60.0f ), 0.0f }; + bodyDef.linearVelocity = { 0.0f, -m_impactSpeed, 0.0f }; + bodyDef.angularVelocity = { 0.0f, 0.0f, m_spin }; + bodyDef.enableSleep = false; + m_boxId = b3CreateBody( m_worldId, &bodyDef ); + + b3ShapeDef shapeDef = b3DefaultShapeDef(); + shapeDef.density = 1.0f; + shapeDef.baseMaterial.friction = 0.0f; + shapeDef.baseMaterial.restitution = m_restitution; + shapeDef.enableHitEvents = true; + b3BoxHull box = b3MakeBoxHull( 1.0f, 0.25f, 1.0f ); + b3CreateHullShape( m_boxId, &shapeDef, &box.base ); + + m_startEnergy = MeasureEnergy( m_worldId, &m_boxId, 1 ).Total(); + m_peakEnergy = m_startEnergy; + + m_coefficient = 0.0f; + m_latched = false; + m_hit = false; + } + + bool DrawControls() override + { + bool rebuild = false; + + ImGui::PushItemWidth( 6.0f * ImGui::GetFontSize() ); + + if ( ImGui::SliderFloat( "Restitution", &m_restitution, 0.0f, 1.0f, "%.2f" ) ) + { + rebuild = true; + } + + if ( ImGui::SliderFloat( "Spin", &m_spin, 0.0f, 2.0f, "%.2f" ) ) + { + rebuild = true; + } + + ImGui::PopItemWidth(); + + if ( ImGui::Button( "Reset" ) ) + { + rebuild = true; + } + + if ( rebuild ) + { + CreateScene(); + } + + return true; + } + + void Step() override + { + Sample::Step(); + + b3ContactEvents events = b3World_GetContactEvents( m_worldId ); + if ( events.hitCount > 0 ) + { + m_hit = true; + } + + float vy = b3Body_GetLinearVelocity( m_boxId ).y; + b3Vec3 w = b3Body_GetAngularVelocity( m_boxId ); + + if ( m_latched == false && m_hit && vy > 0.0f ) + { + m_coefficient = vy / m_impactSpeed; + m_latched = true; + } + + Vec4 yellow = MakeColor( b3_colorYellow ); + DrawPoint( b3Body_GetWorldPoint( m_boxId, { -1.0f, -0.25f, -1.0f } ), 8.0f, yellow ); + DrawPoint( b3Body_GetWorldPoint( m_boxId, { 1.0f, -0.25f, -1.0f } ), 8.0f, yellow ); + DrawPoint( b3Body_GetWorldPoint( m_boxId, { -1.0f, -0.25f, 1.0f } ), 8.0f, yellow ); + DrawPoint( b3Body_GetWorldPoint( m_boxId, { 1.0f, -0.25f, 1.0f } ), 8.0f, yellow ); + + MechanicalEnergy energy = MeasureEnergy( m_worldId, &m_boxId, 1 ); + float total = energy.Total(); + m_peakEnergy = b3MaxFloat( m_peakEnergy, total ); + + DrawTextLine( "vy = %.3f m/s", vy ); + + DrawTextLine( "coefficient = %.4f live (target %.2f)", vy / m_impactSpeed, m_restitution ); + + if ( m_latched ) + { + DrawTextLine( "coefficient = %.4f at first rebound", m_coefficient ); + } + + // The spin is about z, so any x or y component is residual from the sequential point solve + DrawTextLine( "spin in = %.2f, spin out = %.4f (ideal %.2f at e = 1)", m_spin, w.z, -m_spin ); + DrawTextLine( "off axis spin = %.4f rad/s", sqrtf( w.x * w.x + w.y * w.y ) ); + + float scale = m_startEnergy != 0.0f ? 100.0f / m_startEnergy : 0.0f; + DrawTextLine( "kinetic = %.3f J linear + %.3f J angular", energy.linear, energy.angular ); + DrawTextLine( "potential = %.3f J", energy.potential ); + DrawTextLine( "total = %.3f J (%.2f%% of start, peak %.2f%%)", total, scale * total, scale * m_peakEnergy ); + } + + static Sample* Create( SampleContext* context ) + { + return new BoxRestitution( context ); + } + + b3BodyId m_boxId = b3_nullBodyId; + float m_restitution = 0.9f; + float m_spin = 0.0f; + float m_coefficient = 0.0f; + float m_startEnergy = 0.0f; + float m_peakEnergy = 0.0f; + bool m_latched = false; + bool m_hit = false; +}; + +static int sampleBoxRestitution = RegisterSample( "Restitution", "Box Restitution", BoxRestitution::Create ); + +class RotatedBoxRestitution : public Sample +{ +public: + static constexpr int m_count = 12; + static constexpr float m_dropHeight = 10.0f; + + explicit RotatedBoxRestitution( SampleContext* context ) + : Sample( context ) + { + if ( context->restart == false ) + { + m_camera->SetView( 0.0f, 15.0f, 36.0f, { 0.0f, 5.0f, 0.0f } ); + } + + AddGroundBox( 2.0f * m_count + 1.0f ); + + CreateScene(); + } + + void CreateScene() + { + for ( int i = 0; i < m_count; ++i ) + { + if ( B3_IS_NON_NULL( m_bodyIds[i] ) ) + { + b3DestroyBody( m_bodyIds[i] ); + m_bodyIds[i] = b3_nullBodyId; + } + } + + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.type = b3_dynamicBody; + bodyDef.safetyFactor = m_safetyFactor; + + b3ShapeDef shapeDef = b3DefaultShapeDef(); + shapeDef.baseMaterial.friction = m_friction; + shapeDef.baseMaterial.restitution = m_restitution; + + // Boxes in a negative group never collide with each other + shapeDef.filter.groupIndex = -1; + + b3BoxHull box = b3MakeCubeHull( 0.5f ); + + // A cube repeats every quarter turn + float dq = 0.5f * B3_PI / m_count; + float x = -1.0f * ( m_count - 1 ); + + for ( int i = 0; i < m_count; ++i ) + { + bodyDef.position = { x, m_dropHeight, 0.0f }; + bodyDef.rotation = b3MakeQuatFromAxisAngle( b3Vec3_axisZ, i * dq ); + m_bodyIds[i] = b3CreateBody( m_worldId, &bodyDef ); + b3CreateHullShape( m_bodyIds[i], &shapeDef, &box.base ); + + m_startEnergy[i] = MeasureEnergy( m_worldId, &m_bodyIds[i], 1 ).Total(); + + x += 2.0f; + } + } + + bool DrawControls() override + { + bool rebuild = false; + + ImGui::PushItemWidth( 6.0f * ImGui::GetFontSize() ); + + if ( ImGui::SliderFloat( "Restitution", &m_restitution, 0.0f, 1.0f, "%.1f" ) ) + { + rebuild = true; + } + + if ( ImGui::SliderFloat( "Friction", &m_friction, 0.0f, 1.0f, "%.1f" ) ) + { + rebuild = true; + } + + if ( ImGui::SliderFloat( "Safety Factor", &m_safetyFactor, 0.01f, 0.5f, "%.2f" ) ) + { + rebuild = true; + } + + ImGui::PopItemWidth(); + + if ( ImGui::Button( "Reset" ) ) + { + rebuild = true; + } + + if ( rebuild ) + { + CreateScene(); + } + + return true; + } + + void Step() override + { + Sample::Step(); + + float h = 1.0f * m_count; + DrawLine( { -h, m_dropHeight + 0.5f, 0.0f }, { h, m_dropHeight + 0.5f, 0.0f }, MakeColor( b3_colorRed ) ); + + float minPercent = 0.0f; + float maxPercent = 0.0f; + float x = -1.0f * ( m_count - 1 ); + Vec4 white = MakeColor( b3_colorWhite ); + + for ( int i = 0; i < m_count; ++i ) + { + float total = MeasureEnergy( m_worldId, &m_bodyIds[i], 1 ).Total(); + + float percent = m_startEnergy[i] != 0.0f ? 100.0f * total / m_startEnergy[i] : 0.0f; + minPercent = i == 0 ? percent : b3MinFloat( minPercent, percent ); + maxPercent = i == 0 ? percent : b3MaxFloat( maxPercent, percent ); + + DrawString3D( { x - 0.5f, m_dropHeight + 1.5f, 0.0f }, white, "%.1f%%", percent ); + x += 2.0f; + } + + DrawTextLine( "energy: min %.2f%%, max %.2f%% of start", minPercent, maxPercent ); + } + + static Sample* Create( SampleContext* context ) + { + return new RotatedBoxRestitution( context ); + } + + b3BodyId m_bodyIds[m_count] = {}; + float m_startEnergy[m_count] = {}; + float m_restitution = 0.9f; + float m_friction = 0.0f; + float m_safetyFactor = 0.5f; +}; + +static int sampleRotatedBoxRestitution = RegisterSample( "Restitution", "Rotated Box Restitution", RotatedBoxRestitution::Create ); + +// Based on https://github.com/erincatto/box2d/discussions/957 +class TwoBoxRestitution : public Sample +{ +public: + explicit TwoBoxRestitution( SampleContext* context ) + : Sample( context ) + { + if ( context->restart == false ) + { + m_camera->SetView( 20.0f, 10.0f, 14.0f, { 0.0f, 3.0f, 0.0f } ); + } + + AddGroundBox( 10.0f ); + + b3BoxHull box = b3MakeCubeHull( 0.5f ); + + b3ShapeDef shapeDef = b3DefaultShapeDef(); + shapeDef.baseMaterial.restitution = 0.9f; + + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.type = b3_dynamicBody; + + bodyDef.position.y = 0.5f; + m_bodyIds[0] = b3CreateBody( m_worldId, &bodyDef ); + b3CreateHullShape( m_bodyIds[0], &shapeDef, &box.base ); + + bodyDef.position.x += 0.1f; + bodyDef.position.y = 6.0f; + m_bodyIds[1] = b3CreateBody( m_worldId, &bodyDef ); + b3CreateHullShape( m_bodyIds[1], &shapeDef, &box.base ); + + m_startEnergy = MeasureEnergy( m_worldId, m_bodyIds, 2 ).Total(); + m_peakEnergy = m_startEnergy; + } + + void Step() override + { + Sample::Step(); + + MechanicalEnergy energy = MeasureEnergy( m_worldId, m_bodyIds, 2 ); + float total = energy.Total(); + m_peakEnergy = b3MaxFloat( m_peakEnergy, total ); + + float scale = m_startEnergy != 0.0f ? 100.0f / m_startEnergy : 0.0f; + DrawTextLine( "kinetic = %.3f J linear + %.3f J angular", energy.linear, energy.angular ); + DrawTextLine( "potential = %.3f J", energy.potential ); + DrawTextLine( "total = %.3f J (%.2f%% of start, peak %.2f%%)", total, scale * total, scale * m_peakEnergy ); + } + + static Sample* Create( SampleContext* context ) + { + return new TwoBoxRestitution( context ); + } + + b3BodyId m_bodyIds[2] = {}; + float m_startEnergy = 0.0f; + float m_peakEnergy = 0.0f; +}; + +static int sampleTwoBoxRestitution = RegisterSample( "Restitution", "Two Box Restitution", TwoBoxRestitution::Create ); + +class SphereRestitution : public Sample +{ +public: + explicit SphereRestitution( SampleContext* context ) + : Sample( context ) + { + if ( context->restart == false ) + { + m_camera->SetView( 0.0f, 15.0f, 30.0f, { 0.0f, 3.0f, 0.0f } ); + } + + AddGroundBox( 2.0f * m_count ); + + b3Sphere sphere = { b3Vec3_zero, 0.5f }; + + b3ShapeDef shapeDef = b3DefaultShapeDef(); + shapeDef.density = 1.0f; + shapeDef.baseMaterial.restitution = 0.0f; + + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.type = b3_dynamicBody; + + float dr = 1.0f / ( m_count > 1 ? m_count - 1 : 1 ); + float x = -1.0f * ( m_count - 1 ); + float dx = 2.0f; + + for ( int i = 0; i < m_count; ++i ) + { + char buffer[32]; + snprintf( buffer, 32, "%.2f", shapeDef.baseMaterial.restitution ); + bodyDef.name = buffer; + + bodyDef.position = { x, 1.0f, 0.0f }; + b3BodyId bodyId = b3CreateBody( m_worldId, &bodyDef ); + b3CreateSphereShape( bodyId, &shapeDef, &sphere ); + + bodyDef.position = { x, 4.0f, 0.0f }; + bodyId = b3CreateBody( m_worldId, &bodyDef ); + b3CreateSphereShape( bodyId, &shapeDef, &sphere ); + + shapeDef.baseMaterial.restitution += dr; + x += dx; + } + } + + static Sample* Create( SampleContext* context ) + { + return new SphereRestitution( context ); + } + + static constexpr int m_count = 10; +}; + +static int sampleSphereRestitution = RegisterSample( "Restitution", "Sphere Restitution", SphereRestitution::Create ); + +// Similar to the ImpulseTest unit test. The hit event marks the bounce and carries the approach speed, +// so the impulse the contact reports can be checked against reversing that speed at the coefficient. +// The total runs a little over because the contact also carries the weight for the part of the step +// it is active. +class BounceImpulse : public Sample +{ +public: + explicit BounceImpulse( SampleContext* context ) + : Sample( context ) + { + if ( context->restart == false ) + { + m_camera->SetView( 20.0f, 10.0f, 36.0f, { 0.0f, 8.0f, 0.0f } ); + } + + b3World_SetGravity( m_worldId, { 0.0f, -10.0f, 0.0f } ); + + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.position = { 0.0f, -1.0f, 0.0f }; + b3BodyId groundId = b3CreateBody( m_worldId, &bodyDef ); + + b3ShapeDef shapeDef = b3DefaultShapeDef(); + shapeDef.baseMaterial.friction = 0.0f; + shapeDef.baseMaterial.restitution = 0.0f; + b3BoxHull box = b3MakeBoxHull( 40.0f, 1.0f, 40.0f ); + b3ShapeId groundShapeId = b3CreateHullShape( groundId, &shapeDef, &box.base ); + SetGroundShape( groundShapeId ); + + CreateScene(); + } + + void CreateScene() + { + if ( B3_IS_NON_NULL( m_ballId ) ) + { + b3DestroyBody( m_ballId ); + } + + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.type = b3_dynamicBody; + bodyDef.position = { 0.0f, 0.5f + m_dropHeight, 0.0f }; + bodyDef.enableSleep = false; + m_ballId = b3CreateBody( m_worldId, &bodyDef ); + + b3ShapeDef shapeDef = b3DefaultShapeDef(); + shapeDef.baseMaterial.friction = 0.0f; + shapeDef.baseMaterial.restitution = m_restitution; + shapeDef.enableHitEvents = true; + b3Sphere sphere = { b3Vec3_zero, 0.5f }; + b3CreateSphereShape( m_ballId, &shapeDef, &sphere ); + + m_mass = b3Body_GetMass( m_ballId ); + + m_approachSpeed = 0.0f; + m_totalImpulse = 0.0f; + m_expectedImpulse = 0.0f; + m_contactSteps = 0; + m_hit = false; + m_bouncing = false; + } + + bool DrawControls() override + { + bool rebuild = false; + + ImGui::PushItemWidth( 6.0f * ImGui::GetFontSize() ); + + if ( ImGui::SliderFloat( "Restitution", &m_restitution, 0.0f, 1.0f, "%.2f" ) ) + { + rebuild = true; + } + + if ( ImGui::SliderFloat( "Height", &m_dropHeight, 0.5f, 50.0f, "%.1f" ) ) + { + rebuild = true; + } + + ImGui::PopItemWidth(); + + if ( ImGui::Button( "Reset" ) ) + { + rebuild = true; + } + + if ( rebuild ) + { + CreateScene(); + } + + return true; + } + + void Step() override + { + Sample::Step(); + + if ( m_didStep ) + { + b3ContactEvents events = b3World_GetContactEvents( m_worldId ); + if ( m_hit == false && events.hitCount > 0 ) + { + m_hit = true; + m_bouncing = true; + m_approachSpeed = events.hitEvents[0].approachSpeed; + m_expectedImpulse = ( 1.0f + m_restitution ) * m_mass * m_approachSpeed; + } + + // Keep summing until the contact lets go, a slow bounce can take a few steps to leave + if ( m_bouncing ) + { + b3ContactData contactData[4]; + int contactCount = b3Body_GetContactData( m_ballId, contactData, 4 ); + for ( int c = 0; c < contactCount; ++c ) + { + for ( int m = 0; m < contactData[c].manifoldCount; ++m ) + { + const b3Manifold* manifold = contactData[c].manifolds + m; + for ( int p = 0; p < manifold->pointCount; ++p ) + { + m_totalImpulse += manifold->points[p].totalNormalImpulse; + } + } + } + + if ( contactCount > 0 ) + { + m_contactSteps += 1; + } + else + { + m_bouncing = false; + } + } + } + + float startHeight = 0.5f + m_dropHeight; + DrawLine( { -2.0f, startHeight, 0.0f }, { 2.0f, startHeight, 0.0f }, MakeColor( b3_colorRed ) ); + + DrawTextLine( "vy = %.3f m/s", b3Body_GetLinearVelocity( m_ballId ).y ); + + if ( m_hit ) + { + DrawTextLine( "approach speed = %.3f m/s", m_approachSpeed ); + DrawTextLine( "total impulse = %.2f N s over %d steps", m_totalImpulse, m_contactSteps ); + DrawTextLine( "expected impulse = %.2f N s", m_expectedImpulse ); + } + else + { + DrawTextLine( "waiting for the hit" ); + } + } + + static Sample* Create( SampleContext* context ) + { + return new BounceImpulse( context ); + } + + b3BodyId m_ballId = b3_nullBodyId; + float m_restitution = 0.5f; + float m_dropHeight = 20.0f; + float m_mass = 0.0f; + float m_approachSpeed = 0.0f; + float m_totalImpulse = 0.0f; + float m_expectedImpulse = 0.0f; + int m_contactSteps = 0; + bool m_hit = false; + bool m_bouncing = false; +}; + +static int sampleBounceImpulse = RegisterSample( "Restitution", "Bounce Impulse", BounceImpulse::Create ); diff --git a/samples/sample_shapes.cpp b/samples/sample_shapes.cpp index ba9b834..6bcd458 100644 --- a/samples/sample_shapes.cpp +++ b/samples/sample_shapes.cpp @@ -239,626 +239,6 @@ public: static int sampleSlideTwist = RegisterSample( "Shapes", "Slide Twist", SlideTwist::Create ); -class Restitution : public Sample -{ -public: - enum ShapeType - { - e_sphereShape = 0, - e_boxShape - }; - - explicit Restitution( SampleContext* context ) - : Sample( context ) - { - if ( context->restart == false ) - { - m_camera->SetView( 0.0f, 25.0f, 85.0f, { 0.0f, 20.0f, 0.0f } ); - } - - AddGroundBox( 50.0f ); - - m_shapeType = e_sphereShape; - - CreateBodies(); - } - - void CreateBodies() - { - for ( int i = 0; i < m_count; ++i ) - { - if ( B3_IS_NON_NULL( m_bodyIds[i] ) ) - { - b3DestroyBody( m_bodyIds[i] ); - m_bodyIds[i] = b3_nullBodyId; - } - } - - b3Sphere sphere = { b3Vec3_zero, 0.5f }; - b3BoxHull box = b3MakeBoxHull( 0.5f, 0.5f, 0.5f ); - - b3ShapeDef shapeDef = b3DefaultShapeDef(); - - b3BodyDef bodyDef = b3DefaultBodyDef(); - bodyDef.type = b3_dynamicBody; - - float dr = 1.0f / ( m_count > 1 ? m_count - 1 : 1 ); - float x = -1.0f * ( m_count - 1 ); - float dx = 2.0f; - - for ( int i = 0; i < m_count; ++i ) - { - bodyDef.position = { x, 40.0f, 0.0f }; - b3BodyId bodyId = b3CreateBody( m_worldId, &bodyDef ); - - m_bodyIds[i] = bodyId; - - if ( m_shapeType == e_sphereShape ) - { - b3CreateSphereShape( bodyId, &shapeDef, &sphere ); - } - else - { - b3CreateHullShape( bodyId, &shapeDef, &box.base ); - } - - shapeDef.baseMaterial.restitution += dr; - x += dx; - } - } - - bool DrawControls() override - { - if ( ImGui::RadioButton( "Sphere", m_shapeType == e_sphereShape ) ) - { - m_shapeType = e_sphereShape; - CreateBodies(); - } - - if ( ImGui::RadioButton( "Box", m_shapeType == e_boxShape ) ) - { - m_shapeType = e_boxShape; - CreateBodies(); - } - - return true; - } - - static Sample* Create( SampleContext* context ) - { - return new Restitution( context ); - } - - static constexpr int m_count = 40; - - b3BodyId m_bodyIds[m_count] = {}; - ShapeType m_shapeType; -}; - -static int sampleRestitution = RegisterSample( "Shapes", "Restitution", Restitution::Create ); - - -// Similar to the MeasureSupportedBounce unit test -class SphereStackRestitution : public Sample -{ -public: - static constexpr float m_impactSpeed = 5.0f; - static constexpr int m_maxStackCount = 3; - - explicit SphereStackRestitution( SampleContext* context ) - : Sample( context ) - { - if ( context->restart == false ) - { - m_camera->SetView( 20.0f, 10.0f, 12.0f, { 0.0f, 2.5f, 0.0f } ); - } - - b3BodyDef bodyDef = b3DefaultBodyDef(); - bodyDef.position = { 0.0f, -1.0f, 0.0f }; - b3BodyId groundId = b3CreateBody( m_worldId, &bodyDef ); - - b3ShapeDef shapeDef = b3DefaultShapeDef(); - shapeDef.baseMaterial.friction = 0.0f; - shapeDef.baseMaterial.restitution = 0.0f; - b3BoxHull box = b3MakeBoxHull( 40.0f, 1.0f, 40.0f ); - b3ShapeId groundShapeId = b3CreateHullShape( groundId, &shapeDef, &box.base ); - SetGroundShape( groundShapeId ); - - CreateScene(); - } - - b3BodyId CreateBall( float y, float velocityY, float restitution, bool hitEvents ) - { - b3BodyDef bodyDef = b3DefaultBodyDef(); - bodyDef.type = b3_dynamicBody; - bodyDef.position = { 0.0f, y, 0.0f }; - bodyDef.linearVelocity = { 0.0f, velocityY, 0.0f }; - bodyDef.enableSleep = false; - b3BodyId bodyId = b3CreateBody( m_worldId, &bodyDef ); - - b3ShapeDef shapeDef = b3DefaultShapeDef(); - shapeDef.baseMaterial.friction = 0.0f; - shapeDef.baseMaterial.restitution = restitution; - shapeDef.enableHitEvents = hitEvents; - b3Sphere sphere = { b3Vec3_zero, 0.5f }; - b3CreateSphereShape( bodyId, &shapeDef, &sphere ); - - return bodyId; - } - - void CreateScene() - { - for ( int i = 0; i < m_bodyCount; ++i ) - { - b3DestroyBody( m_bodyIds[i] ); - } - m_bodyCount = 0; - - if ( m_gravity ) - { - b3World_SetGravity( m_worldId, { 0.0f, -10.0f, 0.0f } ); - } - else - { - b3World_SetGravity( m_worldId, b3Vec3_zero ); - } - - for ( int i = 0; i < m_stackCount; ++i ) - { - m_bodyIds[m_bodyCount] = CreateBall( 0.5f + 1.0f * i, 0.0f, 0.0f, false ); - m_bodyCount += 1; - } - - // Start half a step of travel above contact so the impact lands mid step, matching the test - m_startHeight = 0.5f + 1.0f * m_stackCount + 0.5f * m_impactSpeed * ( 1.0f / 60.0f ); - - m_impactorId = CreateBall( m_startHeight, -m_impactSpeed, m_restitution, true ); - m_bodyIds[m_bodyCount] = m_impactorId; - m_bodyCount += 1; - - m_startEnergy = MeasureEnergy( m_worldId, m_bodyIds, m_bodyCount ).Total(); - m_peakEnergy = m_startEnergy; - - m_coefficient = 0.0f; - m_latched = false; - m_hit = false; - } - - bool DrawControls() override - { - bool rebuild = false; - - ImGui::PushItemWidth( 6.0f * ImGui::GetFontSize() ); - - if ( ImGui::SliderFloat( "Restitution", &m_restitution, 0.0f, 1.0f, "%.2f" ) ) - { - rebuild = true; - } - - if ( ImGui::SliderInt( "Stack", &m_stackCount, 0, m_maxStackCount ) ) - { - rebuild = true; - } - - ImGui::PopItemWidth(); - - if ( ImGui::Checkbox( "Gravity", &m_gravity ) ) - { - rebuild = true; - } - - if ( ImGui::Button( "Reset" ) ) - { - rebuild = true; - } - - if ( rebuild ) - { - CreateScene(); - } - - return true; - } - - void Step() override - { - Sample::Step(); - - b3ContactEvents events = b3World_GetContactEvents( m_worldId ); - if ( events.hitCount > 0 ) - { - m_hit = true; - } - - float vy = b3Body_GetLinearVelocity( m_impactorId ).y; - - // The rebound only means something once the impact has happened and the impactor is leaving - if ( m_latched == false && m_hit && vy > 0.0f ) - { - m_coefficient = vy / m_impactSpeed; - m_latched = true; - } - - float supportSpeed = 0.0f; - for ( int i = 0; i < m_stackCount; ++i ) - { - supportSpeed = b3MaxFloat( supportSpeed, b3Length( b3Body_GetLinearVelocity( m_bodyIds[i] ) ) ); - } - - DrawLine( { -2.0f, m_startHeight, 0.0f }, { 2.0f, m_startHeight, 0.0f }, MakeColor( b3_colorRed ) ); - - MechanicalEnergy energy = MeasureEnergy( m_worldId, m_bodyIds, m_bodyCount ); - float total = energy.Total(); - m_peakEnergy = b3MaxFloat( m_peakEnergy, total ); - - DrawTextLine( "impactor vy = %.3f m/s", vy ); - - DrawTextLine( "coefficient = %.4f live (target %.2f)", vy / m_impactSpeed, m_restitution ); - - if ( m_latched ) - { - DrawTextLine( "coefficient = %.4f at first rebound", m_coefficient ); - } - - DrawTextLine( "support speed = %.4f m/s", supportSpeed ); - - float scale = m_startEnergy != 0.0f ? 100.0f / m_startEnergy : 0.0f; - DrawTextLine( "kinetic = %.3f J linear + %.3f J angular", energy.linear, energy.angular ); - DrawTextLine( "potential = %.3f J", energy.potential ); - DrawTextLine( "total = %.3f J (%.2f%% of start, peak %.2f%%)", total, scale * total, scale * m_peakEnergy ); - } - - static Sample* Create( SampleContext* context ) - { - return new SphereStackRestitution( context ); - } - - b3BodyId m_bodyIds[m_maxStackCount + 1] = {}; - b3BodyId m_impactorId = b3_nullBodyId; - int m_bodyCount = 0; - int m_stackCount = 2; - float m_restitution = 1.0f; - float m_startHeight = 0.0f; - float m_coefficient = 0.0f; - float m_startEnergy = 0.0f; - float m_peakEnergy = 0.0f; - bool m_gravity = false; - bool m_latched = false; - bool m_hit = false; -}; - -static int sampleSphereStackRestitution = RegisterSample( "Shapes", "Sphere Stack Restitution", SphereStackRestitution::Create ); - -// Similar to the MeasureFlatBounce and SpinTest unit tests -class BoxRestitution : public Sample -{ -public: - static constexpr float m_impactSpeed = 5.0f; - - explicit BoxRestitution( SampleContext* context ) - : Sample( context ) - { - if ( context->restart == false ) - { - m_camera->SetView( 20.0f, 10.0f, 14.0f, { 0.0f, 3.0f, 0.0f } ); - } - - // Gravity would bias the measured coefficient - b3World_SetGravity( m_worldId, b3Vec3_zero ); - - b3BodyDef bodyDef = b3DefaultBodyDef(); - bodyDef.position = { 0.0f, -1.0f, 0.0f }; - b3BodyId groundId = b3CreateBody( m_worldId, &bodyDef ); - - b3ShapeDef shapeDef = b3DefaultShapeDef(); - shapeDef.baseMaterial.friction = 0.0f; - shapeDef.baseMaterial.restitution = 0.0f; - b3BoxHull box = b3MakeBoxHull( 40.0f, 1.0f, 40.0f ); - b3ShapeId groundShapeId = b3CreateHullShape( groundId, &shapeDef, &box.base ); - SetGroundShape( groundShapeId ); - - CreateScene(); - } - - void CreateScene() - { - if ( B3_IS_NON_NULL( m_boxId ) ) - { - b3DestroyBody( m_boxId ); - } - - // Start half a step of travel above contact so the impact lands mid step, matching the test - b3BodyDef bodyDef = b3DefaultBodyDef(); - bodyDef.type = b3_dynamicBody; - bodyDef.position = { 0.0f, 0.25f + 0.5f * m_impactSpeed * ( 1.0f / 60.0f ), 0.0f }; - bodyDef.linearVelocity = { 0.0f, -m_impactSpeed, 0.0f }; - bodyDef.angularVelocity = { 0.0f, 0.0f, m_spin }; - bodyDef.enableSleep = false; - m_boxId = b3CreateBody( m_worldId, &bodyDef ); - - b3ShapeDef shapeDef = b3DefaultShapeDef(); - shapeDef.density = 1.0f; - shapeDef.baseMaterial.friction = 0.0f; - shapeDef.baseMaterial.restitution = m_restitution; - shapeDef.enableHitEvents = true; - b3BoxHull box = b3MakeBoxHull( 1.0f, 0.25f, 1.0f ); - b3CreateHullShape( m_boxId, &shapeDef, &box.base ); - - m_startEnergy = MeasureEnergy( m_worldId, &m_boxId, 1 ).Total(); - m_peakEnergy = m_startEnergy; - - m_coefficient = 0.0f; - m_latched = false; - m_hit = false; - } - - bool DrawControls() override - { - bool rebuild = false; - - ImGui::PushItemWidth( 6.0f * ImGui::GetFontSize() ); - - if ( ImGui::SliderFloat( "Restitution", &m_restitution, 0.0f, 1.0f, "%.2f" ) ) - { - rebuild = true; - } - - if ( ImGui::SliderFloat( "Spin", &m_spin, 0.0f, 2.0f, "%.2f" ) ) - { - rebuild = true; - } - - ImGui::PopItemWidth(); - - if ( ImGui::Button( "Reset" ) ) - { - rebuild = true; - } - - if ( rebuild ) - { - CreateScene(); - } - - return true; - } - - void Step() override - { - Sample::Step(); - - b3ContactEvents events = b3World_GetContactEvents( m_worldId ); - if ( events.hitCount > 0 ) - { - m_hit = true; - } - - float vy = b3Body_GetLinearVelocity( m_boxId ).y; - b3Vec3 w = b3Body_GetAngularVelocity( m_boxId ); - - if ( m_latched == false && m_hit && vy > 0.0f ) - { - m_coefficient = vy / m_impactSpeed; - m_latched = true; - } - - Vec4 yellow = MakeColor( b3_colorYellow ); - DrawPoint( b3Body_GetWorldPoint( m_boxId, { -1.0f, -0.25f, -1.0f } ), 8.0f, yellow ); - DrawPoint( b3Body_GetWorldPoint( m_boxId, { 1.0f, -0.25f, -1.0f } ), 8.0f, yellow ); - DrawPoint( b3Body_GetWorldPoint( m_boxId, { -1.0f, -0.25f, 1.0f } ), 8.0f, yellow ); - DrawPoint( b3Body_GetWorldPoint( m_boxId, { 1.0f, -0.25f, 1.0f } ), 8.0f, yellow ); - - MechanicalEnergy energy = MeasureEnergy( m_worldId, &m_boxId, 1 ); - float total = energy.Total(); - m_peakEnergy = b3MaxFloat( m_peakEnergy, total ); - - DrawTextLine( "vy = %.3f m/s", vy ); - - DrawTextLine( "coefficient = %.4f live (target %.2f)", vy / m_impactSpeed, m_restitution ); - - if ( m_latched ) - { - DrawTextLine( "coefficient = %.4f at first rebound", m_coefficient ); - } - - // The spin is about z, so any x or y component is residual from the sequential point solve - DrawTextLine( "spin in = %.2f, spin out = %.4f (ideal %.2f at e = 1)", m_spin, w.z, -m_spin ); - DrawTextLine( "off axis spin = %.4f rad/s", sqrtf( w.x * w.x + w.y * w.y ) ); - - float scale = m_startEnergy != 0.0f ? 100.0f / m_startEnergy : 0.0f; - DrawTextLine( "kinetic = %.3f J linear + %.3f J angular", energy.linear, energy.angular ); - DrawTextLine( "potential = %.3f J", energy.potential ); - DrawTextLine( "total = %.3f J (%.2f%% of start, peak %.2f%%)", total, scale * total, scale * m_peakEnergy ); - } - - static Sample* Create( SampleContext* context ) - { - return new BoxRestitution( context ); - } - - b3BodyId m_boxId = b3_nullBodyId; - float m_restitution = 0.9f; - float m_spin = 0.0f; - float m_coefficient = 0.0f; - float m_startEnergy = 0.0f; - float m_peakEnergy = 0.0f; - bool m_latched = false; - bool m_hit = false; -}; - -static int sampleBoxRestitution = RegisterSample( "Shapes", "Box Restitution", BoxRestitution::Create ); - -// Similar to the ImpulseTest unit test. The hit event marks the bounce and carries the approach speed, -// so the impulse the contact reports can be checked against reversing that speed at the coefficient. -// The total runs a little over because the contact also carries the weight for the part of the step -// it is active. -class BounceImpulse : public Sample -{ -public: - explicit BounceImpulse( SampleContext* context ) - : Sample( context ) - { - if ( context->restart == false ) - { - m_camera->SetView( 20.0f, 10.0f, 36.0f, { 0.0f, 8.0f, 0.0f } ); - } - - b3World_SetGravity( m_worldId, { 0.0f, -10.0f, 0.0f } ); - - b3BodyDef bodyDef = b3DefaultBodyDef(); - bodyDef.position = { 0.0f, -1.0f, 0.0f }; - b3BodyId groundId = b3CreateBody( m_worldId, &bodyDef ); - - b3ShapeDef shapeDef = b3DefaultShapeDef(); - shapeDef.baseMaterial.friction = 0.0f; - shapeDef.baseMaterial.restitution = 0.0f; - b3BoxHull box = b3MakeBoxHull( 40.0f, 1.0f, 40.0f ); - b3ShapeId groundShapeId = b3CreateHullShape( groundId, &shapeDef, &box.base ); - SetGroundShape( groundShapeId ); - - CreateScene(); - } - - void CreateScene() - { - if ( B3_IS_NON_NULL( m_ballId ) ) - { - b3DestroyBody( m_ballId ); - } - - b3BodyDef bodyDef = b3DefaultBodyDef(); - bodyDef.type = b3_dynamicBody; - bodyDef.position = { 0.0f, 0.5f + m_dropHeight, 0.0f }; - bodyDef.enableSleep = false; - m_ballId = b3CreateBody( m_worldId, &bodyDef ); - - b3ShapeDef shapeDef = b3DefaultShapeDef(); - shapeDef.baseMaterial.friction = 0.0f; - shapeDef.baseMaterial.restitution = m_restitution; - shapeDef.enableHitEvents = true; - b3Sphere sphere = { b3Vec3_zero, 0.5f }; - b3CreateSphereShape( m_ballId, &shapeDef, &sphere ); - - m_mass = b3Body_GetMass( m_ballId ); - - m_approachSpeed = 0.0f; - m_totalImpulse = 0.0f; - m_expectedImpulse = 0.0f; - m_contactSteps = 0; - m_hit = false; - m_bouncing = false; - } - - bool DrawControls() override - { - bool rebuild = false; - - ImGui::PushItemWidth( 6.0f * ImGui::GetFontSize() ); - - if ( ImGui::SliderFloat( "Restitution", &m_restitution, 0.0f, 1.0f, "%.2f" ) ) - { - rebuild = true; - } - - if ( ImGui::SliderFloat( "Height", &m_dropHeight, 0.5f, 50.0f, "%.1f" ) ) - { - rebuild = true; - } - - ImGui::PopItemWidth(); - - if ( ImGui::Button( "Reset" ) ) - { - rebuild = true; - } - - if ( rebuild ) - { - CreateScene(); - } - - return true; - } - - void Step() override - { - Sample::Step(); - - if ( m_didStep ) - { - b3ContactEvents events = b3World_GetContactEvents( m_worldId ); - if ( m_hit == false && events.hitCount > 0 ) - { - m_hit = true; - m_bouncing = true; - m_approachSpeed = events.hitEvents[0].approachSpeed; - m_expectedImpulse = ( 1.0f + m_restitution ) * m_mass * m_approachSpeed; - } - - // Keep summing until the contact lets go, a slow bounce can take a few steps to leave - if ( m_bouncing ) - { - b3ContactData contactData[4]; - int contactCount = b3Body_GetContactData( m_ballId, contactData, 4 ); - for ( int c = 0; c < contactCount; ++c ) - { - for ( int m = 0; m < contactData[c].manifoldCount; ++m ) - { - const b3Manifold* manifold = contactData[c].manifolds + m; - for ( int p = 0; p < manifold->pointCount; ++p ) - { - m_totalImpulse += manifold->points[p].totalNormalImpulse; - } - } - } - - if ( contactCount > 0 ) - { - m_contactSteps += 1; - } - else - { - m_bouncing = false; - } - } - } - - float startHeight = 0.5f + m_dropHeight; - DrawLine( { -2.0f, startHeight, 0.0f }, { 2.0f, startHeight, 0.0f }, MakeColor( b3_colorRed ) ); - - DrawTextLine( "vy = %.3f m/s", b3Body_GetLinearVelocity( m_ballId ).y ); - - if ( m_hit ) - { - DrawTextLine( "approach speed = %.3f m/s", m_approachSpeed ); - DrawTextLine( "total impulse = %.2f N s over %d steps", m_totalImpulse, m_contactSteps ); - DrawTextLine( "expected impulse = %.2f N s", m_expectedImpulse ); - } - else - { - DrawTextLine( "waiting for the hit" ); - } - } - - static Sample* Create( SampleContext* context ) - { - return new BounceImpulse( context ); - } - - b3BodyId m_ballId = b3_nullBodyId; - float m_restitution = 0.5f; - float m_dropHeight = 20.0f; - float m_mass = 0.0f; - float m_approachSpeed = 0.0f; - float m_totalImpulse = 0.0f; - float m_expectedImpulse = 0.0f; - int m_contactSteps = 0; - bool m_hit = false; - bool m_bouncing = false; -}; - -static int sampleBounceImpulse = RegisterSample( "Shapes", "Bounce Impulse", BounceImpulse::Create ); - // This shows an optimization when creating many static shapes you can skip having them invoke collision, assuming // dynamic bodies are added after the static bodies. class StaticInvoke : public Sample diff --git a/src/block_allocator.c b/src/block_allocator.c index ebc6d79..b3eeea8 100644 --- a/src/block_allocator.c +++ b/src/block_allocator.c @@ -10,8 +10,8 @@ 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; + // Blocks come from b3Alloc, so a stride that is a multiple of B3_BLOCK_ALIGNMENT aligns all elements. + elementSize = ( ( elementSize - 1 ) | ( B3_BLOCK_ALIGNMENT - 1 ) ) + 1; b3BlockAllocator allocator = { 0 }; @@ -85,7 +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 ); + B3_ASSERT( ( (uintptr_t)element & ( B3_BLOCK_ALIGNMENT - 1 ) ) == 0 ); allocator->allocationCount -= 1; diff --git a/src/block_allocator.h b/src/block_allocator.h index 1c62ed8..d271304 100644 --- a/src/block_allocator.h +++ b/src/block_allocator.h @@ -9,6 +9,7 @@ #define B3_BLOCK_EXPONENT 8 #define B3_BLOCK_SIZE ( 1 << B3_BLOCK_EXPONENT ) +#define B3_BLOCK_ALIGNMENT 16 typedef struct b3Block { @@ -26,7 +27,7 @@ typedef struct b3BlockAllocator int allocationCount; } b3BlockAllocator; -// Element must be large enough to hold a pointer. The element size is rounded up to B3_ALIGNMENT. +// Element must be large enough to hold a pointer. The element size is rounded up to B3_BLOCK_ALIGNMENT. b3BlockAllocator b3CreateBlockAllocator( int elementSize, int initialCount ); void b3DestroyBlockAllocator( b3BlockAllocator* allocator ); diff --git a/src/body.c b/src/body.c index c9cf3a8..4b96393 100644 --- a/src/body.c +++ b/src/body.c @@ -97,8 +97,8 @@ void b3SyncBodyFlags( b3World* world, b3Body* body ) { b3BodySim* bodySim = b3GetBodySim( world, body ); - // Preserve the fast flag for contact recycling. - bodySim->flags = ( bodySim->flags & b3_isFast ) | ( body->flags & ~b3_bodyTransientFlags ); + // Preserve the sim only flags: fast for contact recycling, time of impact for debug draw. + bodySim->flags = ( bodySim->flags & ( b3_isFast | b3_hadTimeOfImpact ) ) | ( body->flags & ~b3_bodyTransientFlags ); b3BodyState* bodyState = b3GetBodyState( world, body ); if ( bodyState != NULL ) @@ -2217,7 +2217,7 @@ void b3Body_Disable( b3BodyId bodyId ) continue; } - B3_ASSERT( joint->setIndex == set->setIndex || set->setIndex == b3_staticSet ); + B3_ASSERT( joint->setIndex == set->setIndex || set->setIndex == b3_staticSet || joint->setIndex == b3_staticSet ); // Remove joint from island b3UnlinkJoint( world, joint ); @@ -2314,7 +2314,7 @@ void b3Body_Enable( b3BodyId bodyId ) // Transfer joint first int jointSetId; - if ( bodyA->setIndex == b3_staticSet && bodyB->setIndex == b3_staticSet ) + if ( bodyA->type != b3_dynamicBody && bodyB->type != b3_dynamicBody ) { jointSetId = b3_staticSet; } diff --git a/src/body.h b/src/body.h index 0ff4337..3e1c6d4 100644 --- a/src/body.h +++ b/src/body.h @@ -40,7 +40,7 @@ enum b3BodyFlags // This body was speed capped in the current time step b3_isSpeedCapped = 0x00000100, - // This body had a time of impact event in the current time step + // This body had a time of impact event in the current time step (body sim only) b3_hadTimeOfImpact = 0x00000200, // This body has no limit on angular velocity @@ -159,28 +159,30 @@ typedef struct b3Body // according to substep progress. Contacts have reduced stability when anchors are rotated during substeps, especially for // round shapes. // -// 56 bytes -// todo_erin measure perf padding to 64 bytes +// 64 bytes typedef struct b3BodyState { - b3Vec3 linearVelocity; // 12 + b3Vec3 linearVelocity; // 12 + float padding1; + b3Vec3 angularVelocity; // 12 + float padding2; // Using delta position reduces round-off error far from the origin b3Vec3 deltaPosition; // 12 - // Using delta rotation because I cannot access the full rotation on static bodies in - // the solver and must use zero delta rotation for static bodies (c,s) = (1,0) - b3Quat deltaRotation; // 16 - // b3BodyFlags // Important flags: locking, dynamic uint32_t flags; // 4 + + // Using delta rotation because I cannot access the full rotation on static bodies in + // the solver and must use zero delta rotation for static bodies (c,s) = (1,0) + b3Quat deltaRotation; // 16 } b3BodyState; // Identity body state, notice the deltaRotation is identity static const b3BodyState b3_identityBodyState = { - { 0.0f, 0.0f, 0.0f }, { 0.0f, 0.0f, 0.0f }, { 0.0f, 0.0f, 0.0f }, { { 0.0f, 0.0f, 0.0f }, 1.0f }, 0, + { 0.0f, 0.0f, 0.0f }, 0.0f, { 0.0f, 0.0f, 0.0f }, 0.0f, { 0.0f, 0.0f, 0.0f }, 0, { { 0.0f, 0.0f, 0.0f }, 1.0f }, }; // Body simulation data used for integration of position and velocity @@ -193,6 +195,12 @@ typedef struct b3BodySim // center of mass position in world space b3Pos center; + // b3BodyFlags + uint32_t flags; + + float minExtent; + b3Vec3 maxExtent; + // previous rotation and COM for TOI b3Quat rotation0; b3Pos center0; @@ -210,17 +218,12 @@ typedef struct b3BodySim b3Matrix3 invInertiaLocal; b3Matrix3 invInertiaWorld; - float minExtent; - b3Vec3 maxExtent; float linearDamping; float angularDamping; float gravityScale; // Index of b3Body int bodyId; - - // b3BodyFlags - uint32_t flags; } b3BodySim; // Get a validated body from a world using an id. diff --git a/src/contact_solver.c b/src/contact_solver.c index d0a9ddc..ce1cbd2 100644 --- a/src/contact_solver.c +++ b/src/contact_solver.c @@ -35,9 +35,8 @@ void b3PrepareContacts_Mesh( b3SolverBlock block, b3StepContext* context ) b3BodySim* bodySims = context->sims; b3BodyState* states = context->states; - float negRestitutionThreshold = -world->restitutionThreshold; - float warmStartScale = world->enableWarmStarting ? 1.0f : 0.0f; + bool anyRestitution = false; // Used for friction center weighting. float invTau = 1.0f / B3_SPECULATIVE_DISTANCE; @@ -141,12 +140,14 @@ void b3PrepareContacts_Mesh( b3SolverBlock block, b3StepContext* context ) contactConstraint->softness = ( contact->flags & b3_contactStaticFlag ) != 0 ? context->staticSoftness : context->contactSoftness; contactConstraint->friction = contact->friction; + contactConstraint->restitution = contact->restitution; contactConstraint->rollingResistance = contact->rollingResistance; // Only sample contact point normal velocity if needed. - float restitution = contact->restitution; + bool haveRestitution = contact->restitution > 0.0f; bool hitEvents = ( contact->flags & b3_simEnableHitEvent ) != 0; - bool sampleVelocity = restitution > 0.0f || hitEvents; + bool sampleVelocity = haveRestitution || hitEvents; + anyRestitution = anyRestitution || haveRestitution; b3Vec3 vA = b3Vec3_zero; b3Vec3 wA = b3Vec3_zero; @@ -206,6 +207,7 @@ void b3PrepareContacts_Mesh( b3SolverBlock block, b3StepContext* context ) cp->baseSeparation = s - b3Dot( b3Sub( cp->rB, cp->rA ), normal ); cp->normalImpulse = warmStartScale * mp->normalImpulse; cp->totalNormalImpulse = 0.0f; + cp->restitutionImpulse = 0.0f; b3Vec3 rA = cp->rA; b3Vec3 rB = cp->rB; @@ -222,12 +224,12 @@ void b3PrepareContacts_Mesh( b3SolverBlock block, b3StepContext* context ) b3Vec3 vrB = b3Add( vB, b3Cross( wB, rB ) ); float vn = b3Dot( normal, b3Sub( vrB, vrA ) ); - cp->restitutionVelocity = vn < negRestitutionThreshold ? -restitution * vn : 0.0f; + cp->relativeVelocity = vn; mp->normalVelocity = hitEvents ? vn : 0.0f; } else { - cp->restitutionVelocity = 0.0f; + cp->relativeVelocity = 0.0f; mp->normalVelocity = 0.0f; } @@ -286,6 +288,11 @@ void b3PrepareContacts_Mesh( b3SolverBlock block, b3StepContext* context ) colorIndex += 1; } + if ( anyRestitution ) + { + b3AtomicStoreInt( &context->anyRestitution, 1 ); + } + b3TracyCZoneEnd( prepare_contact ); } @@ -507,7 +514,7 @@ void b3PushContacts_Mesh( b3SolverBlock block, b3StepContext* context ) } } -// Solve contacts: normal, friction, rolling resistance, and restitution. +// Solve contacts: normal, friction and rolling resistance. void b3SolveContacts_Mesh( b3SolverBlock block, b3StepContext* context ) { b3World* world = context->world; @@ -560,95 +567,46 @@ void b3SolveContacts_Mesh( b3SolverBlock block, b3StepContext* context ) float totalNormalImpulse = 0.0f; float totalTwistLimit = 0.0f; - bool stopRestitution = true; - // Restitution with more than one point requires iteration to reduce spin. - int iterationCount = 1; for ( int pointIndex = 0; pointIndex < pointCount; ++pointIndex ) { - if ( constraint->points[pointIndex].restitutionVelocity > 0.0f ) - { - iterationCount = pointCount; - break; - } - } + b3ManifoldConstraintPoint* cp = constraint->points + pointIndex; - float velocityBiases[B3_MAX_MANIFOLD_POINTS]; + // Fixed anchor points for applying impulses + b3Vec3 rA = cp->rA; + b3Vec3 rB = cp->rB; - for ( int iteration = 0; iteration < iterationCount; ++iteration ) - { - bool accumulate = iteration + 1 == iterationCount; + // compute current separation + // this is subject to round-off error if the anchor is far from the body center of mass + b3Vec3 ds = b3Add( dp, b3Sub( b3RotateVector( dqB, rB ), b3RotateVector( dqA, rA ) ) ); + float s = b3Dot( ds, normal ) + cp->baseSeparation; - for ( int pointIndex = 0; pointIndex < pointCount; ++pointIndex ) - { - b3ManifoldConstraintPoint* cp = constraint->points + pointIndex; + // speculative bias, zero when overlapped + float velocityBias = s > 0.0f ? s * inv_h : 0.0f; - // Fixed anchor points for applying impulses - b3Vec3 rA = cp->rA; - b3Vec3 rB = cp->rB; + // relative normal velocity at contact + b3Vec3 vrA = b3Add( vA, b3Cross( wA, rA ) ); + b3Vec3 vrB = b3Add( vB, b3Cross( wB, rB ) ); + float vn = b3Dot( b3Sub( vrB, vrA ), normal ); - float velocityBias; - if ( iteration == 0 ) - { - // compute current separation - // this is subject to round-off error if the anchor is far from the body center of mass - b3Vec3 ds = b3Add( dp, b3Sub( b3RotateVector( dqB, rB ), b3RotateVector( dqA, rA ) ) ); - float s = b3Dot( ds, normal ) + cp->baseSeparation; + // incremental normal impulse + float deltaImpulse = -cp->normalMass * ( vn + velocityBias ); - // speculative bias, zero when overlapped - velocityBias = s > 0.0f ? s * inv_h : 0.0f; + // clamp the accumulated impulse + float newImpulse = b3MaxFloat( cp->normalImpulse + deltaImpulse, 0.0f ); + deltaImpulse = newImpulse - cp->normalImpulse; + cp->normalImpulse = newImpulse; + cp->totalNormalImpulse += newImpulse; + totalNormalImpulse += newImpulse; + totalTwistLimit += cp->leverArm * newImpulse; - // Apply restitution. - if ( cp->restitutionVelocity > 0.0f ) - { - velocityBias = b3MinFloat( velocityBias, -cp->restitutionVelocity ); - stopRestitution = stopRestitution && s > 0.0f; - } + // apply normal impulse + b3Vec3 P = b3MulSV( deltaImpulse, normal ); + vA = b3MulSub( vA, mA, P ); + wA = b3Sub( wA, b3MulMV( iA, b3Cross( rA, P ) ) ); - velocityBiases[pointIndex] = velocityBias; - } - else - { - velocityBias = velocityBiases[pointIndex]; - } - - // relative normal velocity at contact - b3Vec3 vrA = b3Add( vA, b3Cross( wA, rA ) ); - b3Vec3 vrB = b3Add( vB, b3Cross( wB, rB ) ); - float vn = b3Dot( b3Sub( vrB, vrA ), normal ); - - // incremental normal impulse - float deltaImpulse = -cp->normalMass * ( vn + velocityBias ); - - // clamp the accumulated impulse - float newImpulse = b3MaxFloat( cp->normalImpulse + deltaImpulse, 0.0f ); - deltaImpulse = newImpulse - cp->normalImpulse; - cp->normalImpulse = newImpulse; - - if ( accumulate ) - { - cp->totalNormalImpulse += newImpulse; - totalNormalImpulse += newImpulse; - totalTwistLimit += cp->leverArm * cp->normalImpulse; - } - - // apply normal impulse - b3Vec3 P = b3MulSV( deltaImpulse, normal ); - vA = b3MulSub( vA, mA, P ); - wA = b3Sub( wA, b3MulMV( iA, b3Cross( rA, P ) ) ); - - vB = b3MulAdd( vB, mB, P ); - wB = b3Add( wB, b3MulMV( iB, b3Cross( rB, P ) ) ); - } - } - - // Stop restitution once all bouncing points are separated. - if ( stopRestitution ) - { - for ( int pointIndex = 0; pointIndex < pointCount; ++pointIndex ) - { - constraint->points[pointIndex].restitutionVelocity = 0.0f; - } + vB = b3MulAdd( vB, mB, P ); + wB = b3Add( wB, b3MulMV( iB, b3Cross( rB, P ) ) ); } // Central twist friction @@ -746,6 +704,133 @@ void b3SolveContacts_Mesh( b3SolverBlock block, b3StepContext* context ) } } +void b3ApplyRestitution_Mesh( b3SolverBlock block, b3StepContext* context ) +{ + b3TracyCZoneNC( restitution_mesh, "Restitution Mesh", b3_colorViolet, true ); + + b3World* world = context->world; + b3GraphColor* color = world->constraintGraph.colors + block.colorIndex; + b3ContactConstraint* contactConstraints = color->contactConstraints; + b3BodyState* states = context->states; + + float threshold = world->restitutionThreshold; + float inv_h = context->inv_h; + bool propagate = world->enableRestitutionPropagation; + + b3BodyState dummyState = b3_identityBodyState; + + int startIndex = block.startIndex; + int endIndex = startIndex + block.count; + + for ( int i = startIndex; i < endIndex; ++i ) + { + b3ContactConstraint* contactConstraint = contactConstraints + i; + float restitution = contactConstraint->restitution; + if ( propagate == false && restitution == 0.0f ) + { + continue; + } + + int manifoldCount = contactConstraint->manifoldCount; + + int indexA = contactConstraint->indexA; + int indexB = contactConstraint->indexB; + + float mA = contactConstraint->invMassA; + b3Matrix3 iA = contactConstraint->invIA; + float mB = contactConstraint->invMassB; + b3Matrix3 iB = contactConstraint->invIB; + + b3BodyState* stateA = indexA == B3_NULL_INDEX ? &dummyState : states + indexA; + b3Vec3 vA = stateA->linearVelocity; + b3Vec3 wA = stateA->angularVelocity; + b3Quat dqA = stateA->deltaRotation; + + b3BodyState* stateB = indexB == B3_NULL_INDEX ? &dummyState : states + indexB; + b3Vec3 vB = stateB->linearVelocity; + b3Vec3 wB = stateB->angularVelocity; + b3Quat dqB = stateB->deltaRotation; + + b3Vec3 dp = b3Sub( stateB->deltaPosition, stateA->deltaPosition ); + + for ( int j = 0; j < manifoldCount; ++j ) + { + b3ManifoldConstraint* constraint = contactConstraint->constraints + j; + + int pointCount = constraint->pointCount; + b3Vec3 normal = constraint->normal; + + for ( int pointIndex = 0; pointIndex < pointCount; ++pointIndex ) + { + b3ManifoldConstraintPoint* cp = constraint->points + pointIndex; + + b3Vec3 rA = cp->rA; + b3Vec3 rB = cp->rB; + + // The total normal impulse is 0 for speculative points. + float compressionImpulse = cp->totalNormalImpulse - cp->restitutionImpulse; + bool armed = restitution > 0.0f && cp->relativeVelocity < -threshold && compressionImpulse > 0.0f; + + float velocityBias; + if ( armed ) + { + velocityBias = restitution * cp->relativeVelocity; + } + else + { + b3Vec3 ds = b3Add( dp, b3Sub( b3RotateVector( dqB, rB ), b3RotateVector( dqA, rA ) ) ); + float s = b3Dot( ds, normal ) + cp->baseSeparation; + + velocityBias = s > 0.0f ? s * inv_h : 0.0f; + } + + b3Vec3 vrA = b3Add( vA, b3Cross( wA, rA ) ); + b3Vec3 vrB = b3Add( vB, b3Cross( wB, rB ) ); + float vn = b3Dot( b3Sub( vrB, vrA ), normal ); + + float impulse = -cp->normalMass * ( vn + velocityBias ); + + float newImpulse = b3MaxFloat( cp->normalImpulse + impulse, 0.0f ); + impulse = newImpulse - cp->normalImpulse; + + float approachImpulse = b3MinFloat( b3MaxFloat( -cp->normalMass * vn, 0.0f ), b3MaxFloat( impulse, 0.0f ) ); + + if ( armed ) + { + // Poisson kinetic restitution guarantees no energy gain. + float allowance = restitution * ( compressionImpulse + approachImpulse ) - cp->restitutionImpulse; + impulse = b3MinFloat( impulse, approachImpulse + b3MaxFloat( allowance, 0.0f ) ); + } + + cp->normalImpulse += impulse; + cp->restitutionImpulse += impulse - approachImpulse; + cp->totalNormalImpulse += impulse; + + b3Vec3 P = b3MulSV( impulse, normal ); + vA = b3MulSub( vA, mA, P ); + wA = b3Sub( wA, b3MulMV( iA, b3Cross( rA, P ) ) ); + + vB = b3MulAdd( vB, mB, P ); + wB = b3Add( wB, b3MulMV( iB, b3Cross( rB, P ) ) ); + } + } + + if ( stateA->flags & b3_dynamicFlag ) + { + stateA->linearVelocity = vA; + stateA->angularVelocity = wA; + } + + if ( stateB->flags & b3_dynamicFlag ) + { + stateB->linearVelocity = vB; + stateB->angularVelocity = wB; + } + } + + b3TracyCZoneEnd( restitution_mesh ); +} + // Don't need to use spans for colors for this because the constraint to contact association // is already linked by pointer. void b3StoreImpulses_Mesh( b3SolverBlock block, b3StepContext* context, int workerIndex ) @@ -876,6 +961,11 @@ typedef struct b3SymMatrix3W b3FloatW cxx, cxy, cxz, cyy, cyz, czz; } b3SymMatrix3W; +typedef struct b3Matrix3W +{ + b3Vec3W cx, cy, cz; +} b3Matrix3W; + // s * a static inline b3Vec3W b3MulSVW( b3FloatW s, b3Vec3W a ) { @@ -984,22 +1074,58 @@ static inline b3Vec3W b3CrossW( b3Vec3W a, b3Vec3W b ) return c; } -static inline b3Vec3W b3RotateVectorW( b3QuatW q, b3Vec3W a ) +static inline b3Matrix3W b3MakeMatrixFromQuatW( b3QuatW q ) { - b3Vec3W t1 = b3CrossW( q.V, a ); - b3Vec3W t2; - t2.X = b3MulAddW( t1.X, q.S, a.X ); - t2.Y = b3MulAddW( t1.Y, q.S, a.Y ); - t2.Z = b3MulAddW( t1.Z, q.S, a.Z ); - b3Vec3W t3 = b3CrossW( q.V, t2 ); - b3FloatW two = b3SplatW( 2.0f ); - b3Vec3W b; - b.X = b3MulAddW( a.X, two, t3.X ); - b.Y = b3MulAddW( a.Y, two, t3.Y ); - b.Z = b3MulAddW( a.Z, two, t3.Z ); + b3FloatW x2 = b3AddW( q.V.X, q.V.X ); + b3FloatW y2 = b3AddW( q.V.Y, q.V.Y ); + b3FloatW z2 = b3AddW( q.V.Z, q.V.Z ); + b3FloatW xx2 = b3MulW( q.V.X, x2 ); + b3FloatW yy2 = b3MulW( q.V.Y, y2 ); + b3FloatW zz2 = b3MulW( q.V.Z, z2 ); + b3FloatW xy2 = b3MulW( q.V.X, y2 ); + b3FloatW xz2 = b3MulW( q.V.X, z2 ); + b3FloatW yz2 = b3MulW( q.V.Y, z2 ); + b3FloatW xw2 = b3MulW( q.S, x2 ); + b3FloatW yw2 = b3MulW( q.S, y2 ); + b3FloatW zw2 = b3MulW( q.S, z2 ); + b3FloatW one = b3SplatW( 1.0f ); + + b3Matrix3W m; + m.cx.X = b3SubW( one, b3AddW( yy2, zz2 ) ); + m.cx.Y = b3AddW( xy2, zw2 ); + m.cx.Z = b3SubW( xz2, yw2 ); + m.cy.X = b3SubW( xy2, zw2 ); + m.cy.Y = b3SubW( one, b3AddW( xx2, zz2 ) ); + m.cy.Z = b3AddW( yz2, xw2 ); + m.cz.X = b3AddW( xz2, yw2 ); + m.cz.Y = b3SubW( yz2, xw2 ); + m.cz.Z = b3SubW( one, b3AddW( xx2, yy2 ) ); + return m; +} + +static inline b3Vec3W b3MulM3VW( b3Matrix3W m, b3Vec3W a ) +{ + b3Vec3W b = { + b3AddW( b3MulW( m.cx.X, a.X ), b3AddW( b3MulW( m.cy.X, a.Y ), b3MulW( m.cz.X, a.Z ) ) ), + b3AddW( b3MulW( m.cx.Y, a.X ), b3AddW( b3MulW( m.cy.Y, a.Y ), b3MulW( m.cz.Y, a.Z ) ) ), + b3AddW( b3MulW( m.cx.Z, a.X ), b3AddW( b3MulW( m.cy.Z, a.Y ), b3MulW( m.cz.Z, a.Z ) ) ), + }; + return b; } +static inline b3Vec3W b3InvRotateVectorW( b3QuatW q, b3Vec3W a ) +{ + b3Vec3W t = b3CrossW( q.V, a ); + t = (b3Vec3W){ b3AddW( t.X, t.X ), b3AddW( t.Y, t.Y ), b3AddW( t.Z, t.Z ) }; + b3Vec3W u = b3CrossW( q.V, t ); + return (b3Vec3W){ + b3AddW( b3SubW( a.X, b3MulW( q.S, t.X ) ), u.X ), + b3AddW( b3SubW( a.Y, b3MulW( q.S, t.Y ) ), u.Y ), + b3AddW( b3SubW( a.Z, b3MulW( q.S, t.Z ) ), u.Z ), + }; +} + // Soft contact constraints with sub-stepping support // Uses fixed anchors for Jacobians for better behavior on rolling shapes (circles & capsules) // http://mmacklin.com/smallsteps.pdf @@ -1013,7 +1139,8 @@ typedef struct b3ContactConstraintPointWide b3FloatW totalNormalImpulses; b3FloatW normalMasses; b3FloatW leverArms; - b3FloatW negRestitutionVelocities; + b3FloatW relativeVelocities; + b3FloatW restitutionImpulses; } b3ContactConstraintPointWide; // Solves four points @@ -1045,6 +1172,7 @@ typedef struct b3ContactConstraintWide b3FloatW rollingResistance; b3FloatW tangentVelocity1; b3FloatW tangentVelocity2; + b3FloatW restitution; b3Manifold* manifolds[B3_SIMD_WIDTH]; @@ -1073,140 +1201,66 @@ typedef struct b3BodyStateW #if defined( B3_SIMD_SSE2 ) || defined( B3_SIMD_NEON ) +_Static_assert( sizeof( b3BodyState ) == 64, "body state layout" ); +_Static_assert( offsetof( b3BodyState, linearVelocity ) == 0 && offsetof( b3BodyState, angularVelocity ) == 16 && + offsetof( b3BodyState, deltaPosition ) == 32 && offsetof( b3BodyState, deltaRotation ) == 48, + "body state layout" ); + B3_FORCE_INLINE b3BodyStateW b3GatherBodies( const b3BodyState* B3_RESTRICT states, const int* B3_RESTRICT indices ) { - b3BodyState dummy = { 0 }; - dummy.deltaRotation.s = 1.0f; + const float* identity = (const float*)&b3_identityBodyState; // Indices are 0 for null - b3BodyState b1 = indices[0] == 0 ? dummy : states[indices[0] - 1]; - b3BodyState b2 = indices[1] == 0 ? dummy : states[indices[1] - 1]; - b3BodyState b3 = indices[2] == 0 ? dummy : states[indices[2] - 1]; - b3BodyState b4 = indices[3] == 0 ? dummy : states[indices[3] - 1]; + const float* p1 = indices[0] == 0 ? identity : (const float*)( states + indices[0] - 1 ); + const float* p2 = indices[1] == 0 ? identity : (const float*)( states + indices[1] - 1 ); + const float* p3 = indices[2] == 0 ? identity : (const float*)( states + indices[2] - 1 ); + const float* p4 = indices[3] == 0 ? identity : (const float*)( states + indices[3] - 1 ); b3BodyStateW s; - s.v.X = b3SetW( b1.linearVelocity.x, b2.linearVelocity.x, b3.linearVelocity.x, b4.linearVelocity.x ); - s.v.Y = b3SetW( b1.linearVelocity.y, b2.linearVelocity.y, b3.linearVelocity.y, b4.linearVelocity.y ); - s.v.Z = b3SetW( b1.linearVelocity.z, b2.linearVelocity.z, b3.linearVelocity.z, b4.linearVelocity.z ); - - s.w.X = b3SetW( b1.angularVelocity.x, b2.angularVelocity.x, b3.angularVelocity.x, b4.angularVelocity.x ); - s.w.Y = b3SetW( b1.angularVelocity.y, b2.angularVelocity.y, b3.angularVelocity.y, b4.angularVelocity.y ); - s.w.Z = b3SetW( b1.angularVelocity.z, b2.angularVelocity.z, b3.angularVelocity.z, b4.angularVelocity.z ); - - s.dp.X = b3SetW( b1.deltaPosition.x, b2.deltaPosition.x, b3.deltaPosition.x, b4.deltaPosition.x ); - s.dp.Y = b3SetW( b1.deltaPosition.y, b2.deltaPosition.y, b3.deltaPosition.y, b4.deltaPosition.y ); - s.dp.Z = b3SetW( b1.deltaPosition.z, b2.deltaPosition.z, b3.deltaPosition.z, b4.deltaPosition.z ); - - s.dq.V.X = b3SetW( b1.deltaRotation.v.x, b2.deltaRotation.v.x, b3.deltaRotation.v.x, b4.deltaRotation.v.x ); - s.dq.V.Y = b3SetW( b1.deltaRotation.v.y, b2.deltaRotation.v.y, b3.deltaRotation.v.y, b4.deltaRotation.v.y ); - s.dq.V.Z = b3SetW( b1.deltaRotation.v.z, b2.deltaRotation.v.z, b3.deltaRotation.v.z, b4.deltaRotation.v.z ); - s.dq.S = b3SetW( b1.deltaRotation.s, b2.deltaRotation.s, b3.deltaRotation.s, b4.deltaRotation.s ); + b3FloatW pad; + b3TransposeW( b3LoadW( p1 ), b3LoadW( p2 ), b3LoadW( p3 ), b3LoadW( p4 ), &s.v.X, &s.v.Y, &s.v.Z, &pad ); + b3TransposeW( b3LoadW( p1 + 4 ), b3LoadW( p2 + 4 ), b3LoadW( p3 + 4 ), b3LoadW( p4 + 4 ), &s.w.X, &s.w.Y, &s.w.Z, &pad ); + b3TransposeW( b3LoadW( p1 + 8 ), b3LoadW( p2 + 8 ), b3LoadW( p3 + 8 ), b3LoadW( p4 + 8 ), &s.dp.X, &s.dp.Y, &s.dp.Z, &pad ); + b3TransposeW( b3LoadW( p1 + 12 ), b3LoadW( p2 + 12 ), b3LoadW( p3 + 12 ), b3LoadW( p4 + 12 ), &s.dq.V.X, &s.dq.V.Y, + &s.dq.V.Z, &s.dq.S ); return s; } +B3_FORCE_INLINE void b3StoreBodyVelocity( b3BodyState* B3_RESTRICT states, int index, b3FloatW v, b3FloatW w ) +{ + // Indices are 0 for null + if ( index == 0 ) + { + return; + } + + b3BodyState* state = states + index - 1; + uint32_t flags = state->flags; + if ( ( flags & b3_dynamicFlag ) == 0 ) + { + return; + } + + b3StoreW( (float*)state, v ); + b3StoreW( (float*)state + 4, w ); +} + // This writes only the velocities back to the solver bodies B3_FORCE_INLINE void b3ScatterBodies( b3BodyState* B3_RESTRICT states, const int* B3_RESTRICT indices, const b3BodyStateW* B3_RESTRICT simdBody ) { - const float* vx = (const float*)&simdBody->v.X; - const float* vy = (const float*)&simdBody->v.Y; - const float* vz = (const float*)&simdBody->v.Z; - const float* wx = (const float*)&simdBody->w.X; - const float* wy = (const float*)&simdBody->w.Y; - const float* wz = (const float*)&simdBody->w.Z; - // I don't use any dummy body in the body array because this will lead to multithreaded sharing and the // associated cache flushing. + b3FloatW zero = b3ZeroW(); + b3FloatW v1, v2, v3, v4; + b3TransposeW( simdBody->v.X, simdBody->v.Y, simdBody->v.Z, zero, &v1, &v2, &v3, &v4 ); + b3FloatW w1, w2, w3, w4; + b3TransposeW( simdBody->w.X, simdBody->w.Y, simdBody->w.Z, zero, &w1, &w2, &w3, &w4 ); - // Warning: indices start at 1 with 0 indicating null - - if ( indices[0] != 0 && ( states[indices[0] - 1].flags & b3_dynamicFlag ) != 0 ) - { - b3BodyState* s = states + ( indices[0] - 1 ); - - b3Vec3 v = { vx[0], vy[0], vz[0] }; - b3Vec3 w = { wx[0], wy[0], wz[0] }; - - uint32_t flags = s->flags; - if ( flags & b3_allLocks ) - { - v.x = ( flags & b3_lockLinearX ) ? 0.0f : v.x; - v.y = ( flags & b3_lockLinearY ) ? 0.0f : v.y; - v.z = ( flags & b3_lockLinearZ ) ? 0.0f : v.z; - w.x = ( flags & b3_lockAngularX ) ? 0.0f : w.x; - w.y = ( flags & b3_lockAngularY ) ? 0.0f : w.y; - w.z = ( flags & b3_lockAngularZ ) ? 0.0f : w.z; - } - - s->linearVelocity = v; - s->angularVelocity = w; - } - - if ( indices[1] != 0 && ( states[indices[1] - 1].flags & b3_dynamicFlag ) != 0 ) - { - b3BodyState* s = states + ( indices[1] - 1 ); - - b3Vec3 v = { vx[1], vy[1], vz[1] }; - b3Vec3 w = { wx[1], wy[1], wz[1] }; - - uint32_t flags = s->flags; - if ( flags & b3_allLocks ) - { - v.x = ( flags & b3_lockLinearX ) ? 0.0f : v.x; - v.y = ( flags & b3_lockLinearY ) ? 0.0f : v.y; - v.z = ( flags & b3_lockLinearZ ) ? 0.0f : v.z; - w.x = ( flags & b3_lockAngularX ) ? 0.0f : w.x; - w.y = ( flags & b3_lockAngularY ) ? 0.0f : w.y; - w.z = ( flags & b3_lockAngularZ ) ? 0.0f : w.z; - } - - s->linearVelocity = v; - s->angularVelocity = w; - } - - if ( indices[2] != 0 && ( states[indices[2] - 1].flags & b3_dynamicFlag ) != 0 ) - { - b3BodyState* s = states + ( indices[2] - 1 ); - - b3Vec3 v = { vx[2], vy[2], vz[2] }; - b3Vec3 w = { wx[2], wy[2], wz[2] }; - - uint32_t flags = s->flags; - if ( flags & b3_allLocks ) - { - v.x = ( flags & b3_lockLinearX ) ? 0.0f : v.x; - v.y = ( flags & b3_lockLinearY ) ? 0.0f : v.y; - v.z = ( flags & b3_lockLinearZ ) ? 0.0f : v.z; - w.x = ( flags & b3_lockAngularX ) ? 0.0f : w.x; - w.y = ( flags & b3_lockAngularY ) ? 0.0f : w.y; - w.z = ( flags & b3_lockAngularZ ) ? 0.0f : w.z; - } - - s->linearVelocity = v; - s->angularVelocity = w; - } - - if ( indices[3] != 0 && ( states[indices[3] - 1].flags & b3_dynamicFlag ) != 0 ) - { - b3BodyState* s = states + ( indices[3] - 1 ); - - b3Vec3 v = { vx[3], vy[3], vz[3] }; - b3Vec3 w = { wx[3], wy[3], wz[3] }; - - uint32_t flags = s->flags; - if ( flags & b3_allLocks ) - { - v.x = ( flags & b3_lockLinearX ) ? 0.0f : v.x; - v.y = ( flags & b3_lockLinearY ) ? 0.0f : v.y; - v.z = ( flags & b3_lockLinearZ ) ? 0.0f : v.z; - w.x = ( flags & b3_lockAngularX ) ? 0.0f : w.x; - w.y = ( flags & b3_lockAngularY ) ? 0.0f : w.y; - w.z = ( flags & b3_lockAngularZ ) ? 0.0f : w.z; - } - - s->linearVelocity = v; - s->angularVelocity = w; - } + b3StoreBodyVelocity( states, indices[0], v1, w1 ); + b3StoreBodyVelocity( states, indices[1], v2, w2 ); + b3StoreBodyVelocity( states, indices[2], v3, w3 ); + b3StoreBodyVelocity( states, indices[3], v4, w4 ); } #else // non-simd @@ -1292,22 +1346,103 @@ B3_FORCE_INLINE void b3ScatterBodies( b3BodyState* B3_RESTRICT states, const int } #endif -// Prepare convex contact constraints +_Static_assert( offsetof( b3ManifoldPoint, anchorA ) == 0, "manifold point layout" ); +_Static_assert( offsetof( b3ManifoldPoint, anchorB ) == 12, "manifold point layout" ); +_Static_assert( offsetof( b3ManifoldPoint, separation ) == 24, "manifold point layout" ); +_Static_assert( offsetof( b3ManifoldPoint, normalImpulse ) == 28, "manifold point layout" ); +_Static_assert( offsetof( b3Manifold, twistImpulse ) == offsetof( b3Manifold, normal ) + 12, "manifold layout" ); +_Static_assert( offsetof( b3Manifold, frictionImpulse ) == offsetof( b3Manifold, normal ) + 16, "manifold layout" ); +_Static_assert( offsetof( b3Manifold, rollingImpulse ) == offsetof( b3Manifold, normal ) + 28, "manifold layout" ); +_Static_assert( offsetof( b3Manifold, pointCount ) == offsetof( b3Manifold, normal ) + 40, "manifold layout" ); +_Static_assert( offsetof( b3Matrix3, cy ) == 12 && offsetof( b3Matrix3, cz ) == 24 && sizeof( b3Matrix3 ) == 36, + "matrix layout" ); +_Static_assert( B3_SIMD_WIDTH == 4, "width" ); + +static const b3Contact b3_zeroContact = { 0 }; +static const b3Manifold b3_zeroManifold = { 0 }; +static const b3BodySim b3_zeroBodySim = { 0 }; + +#define B3_GATHER_LANES( wide, lanes, field ) wide = b3SetW( lanes[0]->field, lanes[1]->field, lanes[2]->field, lanes[3]->field ) + +static inline b3SymMatrix3W b3GatherInvInertiaW( const b3BodySim* const* simLanes ) +{ + const float* i0 = &simLanes[0]->invInertiaWorld.cx.x; + const float* i1 = &simLanes[1]->invInertiaWorld.cx.x; + const float* i2 = &simLanes[2]->invInertiaWorld.cx.x; + const float* i3 = &simLanes[3]->invInertiaWorld.cx.x; + + b3SymMatrix3W m; + b3FloatW unused; + b3TransposeW( b3LoadW( i0 ), b3LoadW( i1 ), b3LoadW( i2 ), b3LoadW( i3 ), &m.cxx, &m.cxy, &m.cxz, &unused ); + b3TransposeW( b3LoadW( i0 + 4 ), b3LoadW( i1 + 4 ), b3LoadW( i2 + 4 ), b3LoadW( i3 + 4 ), &m.cyy, &m.cyz, &unused, &unused ); + m.czz = b3SetW( i0[8], i1[8], i2[8], i3[8] ); + return m; +} + +static inline b3SymMatrix3W b3AddSymW( b3SymMatrix3W a, b3SymMatrix3W b ) +{ + return (b3SymMatrix3W){ + b3AddW( a.cxx, b.cxx ), b3AddW( a.cxy, b.cxy ), b3AddW( a.cxz, b.cxz ), + b3AddW( a.cyy, b.cyy ), b3AddW( a.cyz, b.cyz ), b3AddW( a.czz, b.czz ), + }; +} + +static inline b3SymMatrix3W b3InvertSymW( b3SymMatrix3W m ) +{ + b3FloatW cxx = b3SubW( b3MulW( m.cyy, m.czz ), b3MulW( m.cyz, m.cyz ) ); + b3FloatW cxy = b3SubW( b3MulW( m.cxz, m.cyz ), b3MulW( m.cxy, m.czz ) ); + b3FloatW cxz = b3SubW( b3MulW( m.cxy, m.cyz ), b3MulW( m.cxz, m.cyy ) ); + b3FloatW cyy = b3SubW( b3MulW( m.cxx, m.czz ), b3MulW( m.cxz, m.cxz ) ); + b3FloatW cyz = b3SubW( b3MulW( m.cxy, m.cxz ), b3MulW( m.cxx, m.cyz ) ); + b3FloatW czz = b3SubW( b3MulW( m.cxx, m.cyy ), b3MulW( m.cxy, m.cxy ) ); + + b3FloatW det = b3AddW( b3MulW( m.cxx, cxx ), b3AddW( b3MulW( m.cxy, cxy ), b3MulW( m.cxz, cxz ) ) ); + b3FloatW valid = b3GreaterThanW( b3AbsW( det ), b3SplatW( 1000.0f * FLT_MIN ) ); + b3FloatW invDet = b3BlendW( b3ZeroW(), b3DivW( b3SplatW( 1.0f ), det ), valid ); + + return (b3SymMatrix3W){ + b3MulW( invDet, cxx ), b3MulW( invDet, cxy ), b3MulW( invDet, cxz ), + b3MulW( invDet, cyy ), b3MulW( invDet, cyz ), b3MulW( invDet, czz ), + }; +} + +static inline b3Vec3W b3PerpW( b3Vec3W a ) +{ + b3FloatW zero = b3ZeroW(); + b3FloatW half = b3SplatW( 0.5f ); + b3FloatW mask = b3OrW( b3LessThanW( a.X, b3NegW( half ) ), b3GreaterThanW( a.X, half ) ); + + b3Vec3W p; + p.X = b3BlendW( zero, a.Y, mask ); + p.Y = b3BlendW( a.Z, b3NegW( a.X ), mask ); + p.Z = b3BlendW( b3NegW( a.Y ), zero, mask ); + + b3FloatW lengthSquared = b3DotW( p, p ); + b3FloatW valid = b3GreaterThanW( lengthSquared, b3SplatW( 1000.0f * FLT_MIN ) ); + b3FloatW s = b3BlendW( zero, b3DivW( b3SplatW( 1.0f ), b3SqrtW( lengthSquared ) ), valid ); + return b3MulSVW( s, p ); +} + void b3PrepareContacts_Convex( b3SolverBlock block, b3StepContext* context ) { b3TracyCZoneNC( prepare_contact, "Prepare Contact", b3_colorYellow, true ); b3World* world = context->world; b3BodySim* sims = context->sims; + b3BodyState* states = context->states; #if B3_ENABLE_VALIDATION b3Body* bodies = world->bodies.data; #endif b3WidePrepareSpan* spans = context->widePrepareSpans; b3ContactConstraintWide* wideBase = context->wideConstraints; - b3BodyState* states = context->states; - float warmStartScale = world->enableWarmStarting ? 1.0f : 0.0f; - float invTau = 1.0f / B3_SPECULATIVE_DISTANCE; - float negRestitutionThreshold = -world->restitutionThreshold; + b3FloatW zeroW = b3ZeroW(); + b3FloatW oneW = b3SplatW( 1.0f ); + b3FloatW twoW = b3SplatW( 2.0f ); + b3FloatW warmStartScale = world->enableWarmStarting ? oneW : zeroW; + b3FloatW invTau = b3SplatW( 1.0f / B3_SPECULATIVE_DISTANCE ); + b3FloatW minFrictionWeight = b3SplatW( B3_MIN_FRICTION_WEIGHT ); + b3FloatW minDet = b3SplatW( 1000.0f * FLT_MIN ); + bool anyRestitution = false; int wideIndex = block.startIndex; int endWideIndex = block.startIndex + block.count; @@ -1330,270 +1465,249 @@ void b3PrepareContacts_Convex( b3SolverBlock block, b3StepContext* context ) // Loop over color for ( ; wideIndex < colorWideEndIndex; ++wideIndex ) { - b3ContactConstraintWide* constraint = wideBase + wideIndex; + b3ContactConstraintWide* c = wideBase + wideIndex; int localWideIndex = wideIndex - colorWideStart; + const b3Contact* contactLanes[B3_SIMD_WIDTH]; + const b3Manifold* manifoldLanes[B3_SIMD_WIDTH]; + const b3BodySim* simLanesA[B3_SIMD_WIDTH]; + const b3BodySim* simLanesB[B3_SIMD_WIDTH]; + int hitEventLanes = 0; + for ( int lane = 0; lane < B3_SIMD_WIDTH; ++lane ) { int contactIndex = B3_SIMD_WIDTH * localWideIndex + lane; - if ( contactIndex >= colorContactCount ) + if ( contactIndex < colorContactCount ) { - // Remainder lanes were zeroed in solver setup. - break; - } + b3Contact* contact = b3Array_Get( world->contacts, contactIds[contactIndex] ); + B3_ASSERT( contact->manifoldCount == 1 ); + b3Manifold* manifold = contact->manifolds; - int contactId = contactIds[contactIndex]; - b3Contact* contact = b3Array_Get( world->contacts, contactId ); - B3_ASSERT( contact->manifoldCount == 1 ); - b3Manifold* manifold = contact->manifolds + 0; - - int indexA = b3DecodeAwakeIndex( contact->encodedBodySimA ); - int indexB = b3DecodeAwakeIndex( contact->encodedBodySimB ); + int indexA = b3DecodeAwakeIndex( contact->encodedBodySimA ); + int indexB = b3DecodeAwakeIndex( contact->encodedBodySimB ); #if B3_ENABLE_VALIDATION - b3Body* bodyA = bodies + contact->edges[0].bodyId; - b3Body* bodyB = bodies + contact->edges[1].bodyId; - B3_ASSERT( contact->encodedBodySimA == b3EncodeBodySimIndex( bodyA ) ); - B3_ASSERT( contact->encodedBodySimB == b3EncodeBodySimIndex( bodyB ) ); + b3Body* bodyA = bodies + contact->edges[0].bodyId; + b3Body* bodyB = bodies + contact->edges[1].bodyId; + B3_ASSERT( contact->encodedBodySimA == b3EncodeBodySimIndex( bodyA ) ); + B3_ASSERT( contact->encodedBodySimB == b3EncodeBodySimIndex( bodyB ) ); #endif - // 0 for null - constraint->indexA[lane] = indexA + 1; - constraint->indexB[lane] = indexB + 1; - constraint->manifolds[lane] = manifold; + c->indexA[lane] = indexA + 1; + c->indexB[lane] = indexB + 1; + c->pointCounts[lane] = manifold->pointCount; + c->manifolds[lane] = manifold; - // Body A data - float mA; - b3Matrix3 iA; - - if ( indexA == B3_NULL_INDEX ) - { - mA = 0.0f; - iA = b3Mat3_zero; + contactLanes[lane] = contact; + manifoldLanes[lane] = manifold; + simLanesA[lane] = indexA == B3_NULL_INDEX ? &b3_zeroBodySim : sims + indexA; + simLanesB[lane] = indexB == B3_NULL_INDEX ? &b3_zeroBodySim : sims + indexB; + hitEventLanes |= ( contact->flags & b3_simEnableHitEvent ) != 0 ? 1 << lane : 0; } else { - b3BodySim* simA = sims + indexA; - mA = simA->invMass; - iA = simA->invInertiaWorld; + c->indexA[lane] = 0; + c->indexB[lane] = 0; + c->pointCounts[lane] = 0; + c->manifolds[lane] = NULL; + + contactLanes[lane] = &b3_zeroContact; + manifoldLanes[lane] = &b3_zeroManifold; + simLanesA[lane] = &b3_zeroBodySim; + simLanesB[lane] = &b3_zeroBodySim; } + } - // Body B data - float mB; - b3Matrix3 iB; + b3FloatW mA, mB; + B3_GATHER_LANES( mA, simLanesA, invMass ); + B3_GATHER_LANES( mB, simLanesB, invMass ); + b3SymMatrix3W iA = b3GatherInvInertiaW( simLanesA ); + b3SymMatrix3W iB = b3GatherInvInertiaW( simLanesB ); + c->invMassA = mA; + c->invMassB = mB; + c->invIA = iA; + c->invIB = iB; - if ( indexB == B3_NULL_INDEX ) + b3Vec3W tangentVelocity; + B3_GATHER_LANES( c->friction, contactLanes, friction ); + B3_GATHER_LANES( c->rollingResistance, contactLanes, rollingResistance ); + B3_GATHER_LANES( c->restitution, contactLanes, restitution ); + B3_GATHER_LANES( tangentVelocity.X, contactLanes, tangentVelocity.x ); + B3_GATHER_LANES( tangentVelocity.Y, contactLanes, tangentVelocity.y ); + B3_GATHER_LANES( tangentVelocity.Z, contactLanes, tangentVelocity.z ); + + b3Vec3W normal, frictionImpulse, rollingImpulse; + b3FloatW twistImpulse; + { + const float* m0 = &manifoldLanes[0]->normal.x; + const float* m1 = &manifoldLanes[1]->normal.x; + const float* m2 = &manifoldLanes[2]->normal.x; + const float* m3 = &manifoldLanes[3]->normal.x; + b3TransposeW( b3LoadW( m0 ), b3LoadW( m1 ), b3LoadW( m2 ), b3LoadW( m3 ), &normal.X, &normal.Y, &normal.Z, + &twistImpulse ); + b3TransposeW( b3LoadW( m0 + 4 ), b3LoadW( m1 + 4 ), b3LoadW( m2 + 4 ), b3LoadW( m3 + 4 ), &frictionImpulse.X, + &frictionImpulse.Y, &frictionImpulse.Z, &rollingImpulse.X ); + rollingImpulse.Y = b3SetW( m0[8], m1[8], m2[8], m3[8] ); + rollingImpulse.Z = b3SetW( m0[9], m1[9], m2[9], m3[9] ); + } + + b3Vec3W tangent1 = b3PerpW( normal ); + b3Vec3W tangent2 = b3CrossW( tangent1, normal ); + c->normal = normal; + c->tangent1 = tangent1; + c->tangent2 = tangent2; + c->tangentVelocity1 = b3DotW( tangentVelocity, tangent1 ); + c->tangentVelocity2 = b3DotW( tangentVelocity, tangent2 ); + + b3FloatW rollingMask = b3GreaterThanW( c->rollingResistance, zeroW ); + c->twistImpulse = b3MulW( warmStartScale, twistImpulse ); + c->rollingImpulse.X = b3BlendW( zeroW, b3MulW( warmStartScale, rollingImpulse.X ), rollingMask ); + c->rollingImpulse.Y = b3BlendW( zeroW, b3MulW( warmStartScale, rollingImpulse.Y ), rollingMask ); + c->rollingImpulse.Z = b3BlendW( zeroW, b3MulW( warmStartScale, rollingImpulse.Z ), rollingMask ); + c->frictionImpulse.x = b3MulW( warmStartScale, b3DotW( frictionImpulse, tangent1 ) ); + c->frictionImpulse.y = b3MulW( warmStartScale, b3DotW( frictionImpulse, tangent2 ) ); + + b3FloatW pointCountW = + b3SetW( (float)c->pointCounts[0], (float)c->pointCounts[1], (float)c->pointCounts[2], (float)c->pointCounts[3] ); + + b3Vec3W centerA = { zeroW, zeroW, zeroW }; + b3Vec3W centerB = { zeroW, zeroW, zeroW }; + b3FloatW totalFrictionWeight = zeroW; + + for ( int pointIndex = 0; pointIndex < B3_MAX_MANIFOLD_POINTS; ++pointIndex ) + { + b3ContactConstraintPointWide* cp = c->points + pointIndex; + b3FloatW pointMask = b3GreaterThanW( pointCountW, b3SplatW( (float)pointIndex ) ); + + const float* p0 = (const float*)( manifoldLanes[0]->points + pointIndex ); + const float* p1 = (const float*)( manifoldLanes[1]->points + pointIndex ); + const float* p2 = (const float*)( manifoldLanes[2]->points + pointIndex ); + const float* p3 = (const float*)( manifoldLanes[3]->points + pointIndex ); + + b3Vec3W rA, rB; + b3FloatW separation, normalImpulse; + b3TransposeW( b3LoadW( p0 ), b3LoadW( p1 ), b3LoadW( p2 ), b3LoadW( p3 ), &rA.X, &rA.Y, &rA.Z, &rB.X ); + b3TransposeW( b3LoadW( p0 + 4 ), b3LoadW( p1 + 4 ), b3LoadW( p2 + 4 ), b3LoadW( p3 + 4 ), &rB.Y, &rB.Z, + &separation, &normalImpulse ); + + rA.X = b3BlendW( zeroW, rA.X, pointMask ); + rA.Y = b3BlendW( zeroW, rA.Y, pointMask ); + rA.Z = b3BlendW( zeroW, rA.Z, pointMask ); + rB.X = b3BlendW( zeroW, rB.X, pointMask ); + rB.Y = b3BlendW( zeroW, rB.Y, pointMask ); + rB.Z = b3BlendW( zeroW, rB.Z, pointMask ); + separation = b3BlendW( zeroW, separation, pointMask ); + normalImpulse = b3BlendW( zeroW, normalImpulse, pointMask ); + + // C0 friction center decay. Needed to prevent spinning top drift (GyroscopicPrecession sample). + // See details in b3PrepareContacts_Mesh. This code should stay in sync. + b3FloatW weight = b3MinW( b3MaxW( b3SubW( twoW, b3MulW( separation, invTau ) ), minFrictionWeight ), oneW ); + weight = b3BlendW( zeroW, weight, pointMask ); + centerA = b3MulAddSVW( centerA, weight, rA ); + centerB = b3MulAddSVW( centerB, weight, rB ); + totalFrictionWeight = b3AddW( totalFrictionWeight, weight ); + + cp->anchorAs = rA; + cp->anchorBs = rB; + cp->baseSeparations = b3SubW( separation, b3DotW( b3SubVW( rB, rA ), normal ) ); + cp->normalImpulses = b3MulW( warmStartScale, normalImpulse ); + cp->totalNormalImpulses = zeroW; + cp->relativeVelocities = zeroW; + cp->restitutionImpulses = zeroW; + + b3Vec3W rnA = b3CrossW( rA, normal ); + b3Vec3W rnB = b3CrossW( rB, normal ); + b3FloatW kNormal = b3AddW( mA, mB ); + kNormal = b3AddW( kNormal, b3DotW( rnA, b3MulMVW( iA, rnA ) ) ); + kNormal = b3AddW( kNormal, b3DotW( rnB, b3MulMVW( iB, rnB ) ) ); + b3FloatW valid = b3AndW( b3GreaterThanW( kNormal, zeroW ), pointMask ); + cp->normalMasses = b3BlendW( zeroW, b3DivW( oneW, kNormal ), valid ); + } + + b3FloatW invWeight = + b3BlendW( zeroW, b3DivW( oneW, totalFrictionWeight ), b3GreaterThanW( totalFrictionWeight, zeroW ) ); + centerA = b3MulSVW( invWeight, centerA ); + centerB = b3MulSVW( invWeight, centerB ); + c->centerA = centerA; + c->centerB = centerB; + + for ( int pointIndex = 0; pointIndex < B3_MAX_MANIFOLD_POINTS; ++pointIndex ) + { + b3ContactConstraintPointWide* cp = c->points + pointIndex; + b3FloatW pointMask = b3GreaterThanW( pointCountW, b3SplatW( (float)pointIndex ) ); + b3Vec3W d = b3SubVW( cp->anchorAs, centerA ); + cp->leverArms = b3BlendW( zeroW, b3SqrtW( b3DotW( d, d ) ), pointMask ); + } + + { + b3Vec3W rtA1 = b3CrossW( centerA, tangent1 ); + b3Vec3W rtA2 = b3CrossW( centerA, tangent2 ); + b3Vec3W rtB1 = b3CrossW( centerB, tangent1 ); + b3Vec3W rtB2 = b3CrossW( centerB, tangent2 ); + b3Vec3W iArtA1 = b3MulMVW( iA, rtA1 ); + b3Vec3W iArtA2 = b3MulMVW( iA, rtA2 ); + b3Vec3W iBrtB1 = b3MulMVW( iB, rtB1 ); + b3Vec3W iBrtB2 = b3MulMVW( iB, rtB2 ); + + b3FloatW kxx = b3AddW( b3AddW( b3AddW( mA, mB ), b3DotW( rtA1, iArtA1 ) ), b3DotW( rtB1, iBrtB1 ) ); + b3FloatW kyy = b3AddW( b3AddW( b3AddW( mA, mB ), b3DotW( rtA2, iArtA2 ) ), b3DotW( rtB2, iBrtB2 ) ); + b3FloatW kxy = b3AddW( b3DotW( rtA1, iArtA2 ), b3DotW( rtB1, iBrtB2 ) ); + + b3FloatW det = b3SubW( b3MulW( kxx, kyy ), b3MulW( kxy, kxy ) ); + b3FloatW valid = b3GreaterThanW( b3AbsW( det ), minDet ); + b3FloatW invDet = b3BlendW( zeroW, b3DivW( oneW, det ), valid ); + c->tangentMass.cxx = b3MulW( invDet, kyy ); + c->tangentMass.cxy = b3NegW( b3MulW( invDet, kxy ) ); + c->tangentMass.cyy = b3MulW( invDet, kxx ); + } + + b3SymMatrix3W invIAB = b3AddSymW( iA, iB ); + + { + b3FloatW kTwist = b3DotW( normal, b3MulMVW( invIAB, normal ) ); + c->twistMass = b3BlendW( zeroW, b3DivW( oneW, kTwist ), b3GreaterThanW( kTwist, zeroW ) ); + } + + if ( b3AllZeroW( c->rollingResistance ) == false ) + { + c->rollingMass = b3InvertSymW( invIAB ); + } + else + { + c->rollingMass = (b3SymMatrix3W){ zeroW, zeroW, zeroW, zeroW, zeroW, zeroW }; + } + + // Only sample contact point normal velocity if needed. + bool haveRestitution = b3AnyTrueW( b3GreaterThanW( c->restitution, zeroW ) ); + anyRestitution = anyRestitution || haveRestitution; + + if ( hitEventLanes != 0 || haveRestitution ) + { + b3BodyStateW bA = b3GatherBodies( states, c->indexA ); + b3BodyStateW bB = b3GatherBodies( states, c->indexB ); + + for ( int pointIndex = 0; pointIndex < B3_MAX_MANIFOLD_POINTS; ++pointIndex ) { - mB = 0.0f; - iB = b3Mat3_zero; - } - else - { - b3BodySim* simB = sims + indexB; - mB = simB->invMass; - iB = simB->invInertiaWorld; - } + b3ContactConstraintPointWide* cp = c->points + pointIndex; - ( (float*)&constraint->invMassA )[lane] = mA; - ( (float*)&constraint->invMassB )[lane] = mB; + b3Vec3W vrA = b3AddVW( bA.v, b3CrossW( bA.w, cp->anchorAs ) ); + b3Vec3W vrB = b3AddVW( bB.v, b3CrossW( bB.w, cp->anchorBs ) ); + b3FloatW vn = b3DotW( normal, b3SubVW( vrB, vrA ) ); + cp->relativeVelocities = vn; - ( (float*)&constraint->invIA.cxx )[lane] = iA.cx.x; - ( (float*)&constraint->invIA.cxy )[lane] = iA.cx.y; - ( (float*)&constraint->invIA.cxz )[lane] = iA.cx.z; - ( (float*)&constraint->invIA.cyy )[lane] = iA.cy.y; - ( (float*)&constraint->invIA.cyz )[lane] = iA.cy.z; - ( (float*)&constraint->invIA.czz )[lane] = iA.cz.z; - - ( (float*)&constraint->invIB.cxx )[lane] = iB.cx.x; - ( (float*)&constraint->invIB.cxy )[lane] = iB.cx.y; - ( (float*)&constraint->invIB.cxz )[lane] = iB.cx.z; - ( (float*)&constraint->invIB.cyy )[lane] = iB.cy.y; - ( (float*)&constraint->invIB.cyz )[lane] = iB.cy.z; - ( (float*)&constraint->invIB.czz )[lane] = iB.cz.z; - - b3Vec3 normal = manifold->normal; - ( (float*)&constraint->normal.X )[lane] = normal.x; - ( (float*)&constraint->normal.Y )[lane] = normal.y; - ( (float*)&constraint->normal.Z )[lane] = normal.z; - - b3Vec3 tangent1 = b3Perp( normal ); - ( (float*)&constraint->tangent1.X )[lane] = tangent1.x; - ( (float*)&constraint->tangent1.Y )[lane] = tangent1.y; - ( (float*)&constraint->tangent1.Z )[lane] = tangent1.z; - - b3Vec3 tangent2 = b3Cross( tangent1, normal ); - ( (float*)&constraint->tangent2.X )[lane] = tangent2.x; - ( (float*)&constraint->tangent2.Y )[lane] = tangent2.y; - ( (float*)&constraint->tangent2.Z )[lane] = tangent2.z; - - ( (float*)&constraint->friction )[lane] = contact->friction; - ( (float*)&constraint->rollingResistance )[lane] = contact->rollingResistance; - - ( (float*)&constraint->tangentVelocity1 )[lane] = b3Dot( contact->tangentVelocity, tangent1 ); - ( (float*)&constraint->tangentVelocity2 )[lane] = b3Dot( contact->tangentVelocity, tangent2 ); - - // Only sample contact point normal velocity if needed. - float restitution = contact->restitution; - bool hitEvents = ( contact->flags & b3_simEnableHitEvent ) != 0; - bool sampleVelocity = restitution > 0.0f || hitEvents; - - b3Vec3 vA = b3Vec3_zero; - b3Vec3 wA = b3Vec3_zero; - b3Vec3 vB = b3Vec3_zero; - b3Vec3 wB = b3Vec3_zero; - - if ( sampleVelocity ) - { - if ( indexA != B3_NULL_INDEX ) + if ( hitEventLanes != 0 ) { - vA = states[indexA].linearVelocity; - wA = states[indexA].angularVelocity; + float normalVelocities[B3_SIMD_WIDTH]; + b3StoreW( normalVelocities, vn ); + + for ( int lane = 0; lane < B3_SIMD_WIDTH; ++lane ) + { + if ( ( hitEventLanes & ( 1 << lane ) ) != 0 && pointIndex < c->pointCounts[lane] ) + { + c->manifolds[lane]->points[pointIndex].normalVelocity = normalVelocities[lane]; + } + } } - - if ( indexB != B3_NULL_INDEX ) - { - vB = states[indexB].linearVelocity; - wB = states[indexB].angularVelocity; - } - } - - int pointCount = manifold->pointCount; - constraint->pointCounts[lane] = pointCount; - - b3Vec3 centerA = b3Vec3_zero; - b3Vec3 centerB = b3Vec3_zero; - float totalFrictionWeight = 0.0f; - - for ( int pointIndex = 0; pointIndex < pointCount; ++pointIndex ) - { - b3ManifoldPoint* mp = manifold->points + pointIndex; - b3ContactConstraintPointWide* cp = constraint->points + pointIndex; - - b3Vec3 rA = mp->anchorA; - b3Vec3 rB = mp->anchorB; - float s = mp->separation; - - // C0 friction center decay. Needed to prevent spinning top drift (GyroscopicPrecession sample). - // See details in b3PrepareContacts_Mesh. This code should stay in sync. - float weight = b3ClampFloat( 2.0f - s * invTau, B3_MIN_FRICTION_WEIGHT, 1.0f ); - centerA = b3MulAdd( centerA, weight, rA ); - centerB = b3MulAdd( centerB, weight, rB ); - totalFrictionWeight += weight; - - ( (float*)&cp->anchorAs.X )[lane] = rA.x; - ( (float*)&cp->anchorAs.Y )[lane] = rA.y; - ( (float*)&cp->anchorAs.Z )[lane] = rA.z; - - ( (float*)&cp->anchorBs.X )[lane] = rB.x; - ( (float*)&cp->anchorBs.Y )[lane] = rB.y; - ( (float*)&cp->anchorBs.Z )[lane] = rB.z; - - float baseSeparation = s - b3Dot( b3Sub( rB, rA ), normal ); - ( (float*)&cp->baseSeparations )[lane] = baseSeparation; - - ( (float*)&cp->normalImpulses )[lane] = warmStartScale * mp->normalImpulse; - ( (float*)&cp->totalNormalImpulses )[lane] = 0.0f; - - b3Vec3 rnA = b3Cross( rA, normal ); - b3Vec3 rnB = b3Cross( rB, normal ); - float kNormal = mA + mB + b3Dot( rnA, b3MulMV( iA, rnA ) ) + b3Dot( rnB, b3MulMV( iB, rnB ) ); - ( (float*)&cp->normalMasses )[lane] = kNormal > 0.0f ? 1.0f / kNormal : 0.0f; - - // Get the normal velocity if needed for restitution or hit events. - if ( sampleVelocity ) - { - b3Vec3 vrA = b3Add( vA, b3Cross( wA, rA ) ); - b3Vec3 vrB = b3Add( vB, b3Cross( wB, rB ) ); - float vn = b3Dot( normal, b3Sub( vrB, vrA ) ); - - ( (float*)&cp->negRestitutionVelocities )[lane] = vn < negRestitutionThreshold ? restitution * vn : 0.0f; - mp->normalVelocity = hitEvents ? vn : 0.0f; - } - else - { - ( (float*)&cp->negRestitutionVelocities )[lane] = 0.0f; - mp->normalVelocity = 0.0f; - } - } - - float invWeight = 1.0f / totalFrictionWeight; - centerA = b3MulSV( invWeight, centerA ); - centerB = b3MulSV( invWeight, centerB ); - - ( (float*)&constraint->centerA.X )[lane] = centerA.x; - ( (float*)&constraint->centerA.Y )[lane] = centerA.y; - ( (float*)&constraint->centerA.Z )[lane] = centerA.z; - ( (float*)&constraint->centerB.X )[lane] = centerB.x; - ( (float*)&constraint->centerB.Y )[lane] = centerB.y; - ( (float*)&constraint->centerB.Z )[lane] = centerB.z; - - for ( int pointIndex = 0; pointIndex < pointCount; ++pointIndex ) - { - const b3ManifoldPoint* mp = manifold->points + pointIndex; - b3ContactConstraintPointWide* cp = constraint->points + pointIndex; - ( (float*)&cp->leverArms )[lane] = b3Distance( mp->anchorA, centerA ); - } - - b3Vec3 rtA1 = b3Cross( centerA, tangent1 ); - b3Vec3 rtA2 = b3Cross( centerA, tangent2 ); - - b3Vec3 rtB1 = b3Cross( centerB, tangent1 ); - b3Vec3 rtB2 = b3Cross( centerB, tangent2 ); - - { - b3Matrix2 k; - k.cx.x = mA + mB + b3Dot( rtA1, b3MulMV( iA, rtA1 ) ) + b3Dot( rtB1, b3MulMV( iB, rtB1 ) ); - k.cy.y = mA + mB + b3Dot( rtA2, b3MulMV( iA, rtA2 ) ) + b3Dot( rtB2, b3MulMV( iB, rtB2 ) ); - k.cx.y = k.cy.x = b3Dot( rtA1, b3MulMV( iA, rtA2 ) ) + b3Dot( rtB1, b3MulMV( iB, rtB2 ) ); - b3Matrix2 tangentMass = b3Invert2( k ); - - ( (float*)&constraint->tangentMass.cxx )[lane] = tangentMass.cx.x; - ( (float*)&constraint->tangentMass.cxy )[lane] = tangentMass.cx.y; - ( (float*)&constraint->tangentMass.cyy )[lane] = tangentMass.cy.y; - - ( (float*)&constraint->frictionImpulse.x )[lane] = - warmStartScale * b3Dot( manifold->frictionImpulse, tangent1 ); - ( (float*)&constraint->frictionImpulse.y )[lane] = - warmStartScale * b3Dot( manifold->frictionImpulse, tangent2 ); - } - - { - float k = b3Dot( normal, b3MulMV( b3AddMM( iA, iB ), normal ) ); - ( (float*)&constraint->twistMass )[lane] = k > 0.0f ? 1.0f / k : 0.0f; - ( (float*)&constraint->twistImpulse )[lane] = warmStartScale * manifold->twistImpulse; - } - - { - b3Matrix3 rollingMass = b3InvertMatrix( b3AddMM( iA, iB ) ); - - ( (float*)&constraint->rollingMass.cxx )[lane] = rollingMass.cx.x; - ( (float*)&constraint->rollingMass.cxy )[lane] = rollingMass.cx.y; - ( (float*)&constraint->rollingMass.cxz )[lane] = rollingMass.cx.z; - ( (float*)&constraint->rollingMass.cyy )[lane] = rollingMass.cy.y; - ( (float*)&constraint->rollingMass.cyz )[lane] = rollingMass.cy.z; - ( (float*)&constraint->rollingMass.czz )[lane] = rollingMass.cz.z; - - ( (float*)&constraint->rollingImpulse.X )[lane] = warmStartScale * manifold->rollingImpulse.x; - ( (float*)&constraint->rollingImpulse.Y )[lane] = warmStartScale * manifold->rollingImpulse.y; - ( (float*)&constraint->rollingImpulse.Z )[lane] = warmStartScale * manifold->rollingImpulse.z; - } - - // zero remaining points - for ( int pointIndex = pointCount; pointIndex < B3_MAX_MANIFOLD_POINTS; ++pointIndex ) - { - b3ContactConstraintPointWide* cp = constraint->points + pointIndex; - ( (float*)&cp->anchorAs.X )[lane] = 0.0f; - ( (float*)&cp->anchorAs.Y )[lane] = 0.0f; - ( (float*)&cp->anchorAs.Z )[lane] = 0.0f; - ( (float*)&cp->anchorBs.X )[lane] = 0.0f; - ( (float*)&cp->anchorBs.Y )[lane] = 0.0f; - ( (float*)&cp->anchorBs.Z )[lane] = 0.0f; - ( (float*)&cp->baseSeparations )[lane] = 0.0f; - ( (float*)&cp->normalImpulses )[lane] = 0.0f; - ( (float*)&cp->totalNormalImpulses )[lane] = 0.0f; - ( (float*)&cp->normalMasses )[lane] = 0.0f; - ( (float*)&cp->negRestitutionVelocities )[lane] = 0.0f; - ( (float*)&cp->leverArms )[lane] = 0.0f; } } } @@ -1602,6 +1716,11 @@ void b3PrepareContacts_Convex( b3SolverBlock block, b3StepContext* context ) colorIndex += 1; } + if ( anyRestitution ) + { + b3AtomicStoreInt( &context->anyRestitution, 1 ); + } + b3TracyCZoneEnd( prepare_contact ); } @@ -1624,51 +1743,40 @@ void b3WarmStartContacts_Convex( b3SolverBlock block, b3StepContext* context ) int pointCount = b3MaxInt( pointCount1, pointCount2 ); B3_VALIDATE( 0 < pointCount && pointCount <= B3_MAX_MANIFOLD_POINTS ); - // Normal impulses + b3FloatW zeroW = b3ZeroW(); + b3FloatW totalNormalImpulse = zeroW; + b3Vec3W momentA = { zeroW, zeroW, zeroW }; + b3Vec3W momentB = { zeroW, zeroW, zeroW }; + for ( int j = 0; j < pointCount; ++j ) { b3ContactConstraintPointWide* cp = c->points + j; - - b3Vec3W rA = cp->anchorAs; - b3Vec3W rB = cp->anchorBs; - - b3Vec3W impulse; - impulse.X = b3MulW( cp->normalImpulses, c->normal.X ); - impulse.Y = b3MulW( cp->normalImpulses, c->normal.Y ); - impulse.Z = b3MulW( cp->normalImpulses, c->normal.Z ); - - bA.w = b3MulSubMVW( bA.w, c->invIA, b3CrossW( rA, impulse ) ); - bA.v = b3MulSubSVW( bA.v, c->invMassA, impulse ); - bB.w = b3MulAddMVW( bB.w, c->invIB, b3CrossW( rB, impulse ) ); - bB.v = b3MulAddSVW( bB.v, c->invMassB, impulse ); + b3FloatW normalImpulse = cp->normalImpulses; + totalNormalImpulse = b3AddW( totalNormalImpulse, normalImpulse ); + momentA = b3MulAddSVW( momentA, normalImpulse, cp->anchorAs ); + momentB = b3MulAddSVW( momentB, normalImpulse, cp->anchorBs ); } - // Central friction + b3Vec3W normal = c->normal; + b3Vec3W frictionImpulse = b3MulSVW( c->frictionImpulse.x, c->tangent1 ); + frictionImpulse = b3MulAddSVW( frictionImpulse, c->frictionImpulse.y, c->tangent2 ); + + b3Vec3W linearImpulse = b3MulAddSVW( frictionImpulse, totalNormalImpulse, normal ); + + b3Vec3W twistImpulse = b3MulSVW( c->twistImpulse, normal ); + b3Vec3W angularImpulseA = b3AddVW( b3AddVW( b3CrossW( momentA, normal ), b3CrossW( c->centerA, frictionImpulse ) ), twistImpulse ); + b3Vec3W angularImpulseB = b3AddVW( b3AddVW( b3CrossW( momentB, normal ), b3CrossW( c->centerB, frictionImpulse ) ), twistImpulse ); + + if ( b3AllZeroW( c->rollingResistance ) == false ) { - b3Vec3W rA = c->centerA; - b3Vec3W rB = c->centerB; - b3Vec3W impulse = b3MulSVW( c->frictionImpulse.x, c->tangent1 ); - impulse = b3MulAddSVW( impulse, c->frictionImpulse.y, c->tangent2 ); - - bA.w = b3MulSubMVW( bA.w, c->invIA, b3CrossW( rA, impulse ) ); - bA.v = b3MulSubSVW( bA.v, c->invMassA, impulse ); - bB.w = b3MulAddMVW( bB.w, c->invIB, b3CrossW( rB, impulse ) ); - bB.v = b3MulAddSVW( bB.v, c->invMassB, impulse ); + angularImpulseA = b3AddVW( angularImpulseA, c->rollingImpulse ); + angularImpulseB = b3AddVW( angularImpulseB, c->rollingImpulse ); } - // Central twist friction - { - b3Vec3W impulse = b3MulSVW( c->twistImpulse, c->normal ); - bA.w = b3MulSubMVW( bA.w, c->invIA, impulse ); - bB.w = b3MulAddMVW( bB.w, c->invIB, impulse ); - } - - // Rolling resistance - { - b3Vec3W impulse = c->rollingImpulse; - bA.w = b3MulSubMVW( bA.w, c->invIA, impulse ); - bB.w = b3MulAddMVW( bB.w, c->invIB, impulse ); - } + bA.w = b3MulSubMVW( bA.w, c->invIA, angularImpulseA ); + bA.v = b3MulSubSVW( bA.v, c->invMassA, linearImpulse ); + bB.w = b3MulAddMVW( bB.w, c->invIB, angularImpulseB ); + bB.v = b3MulAddSVW( bB.v, c->invMassB, linearImpulse ); b3ScatterBodies( states, c->indexA, &bA ); b3ScatterBodies( states, c->indexB, &bB ); @@ -1717,6 +1825,11 @@ void b3PushContacts_Convex( b3SolverBlock block, b3StepContext* context ) b3Vec3W dp = b3SubVW( bB.dp, bA.dp ); + // Convert to normals to local space to reduce transform math. + b3FloatW normalSeparation = b3DotW( c->normal, dp ); + b3Vec3W normalA = b3InvRotateVectorW( bA.dq, c->normal ); + b3Vec3W normalB = b3InvRotateVectorW( bB.dq, c->normal ); + for ( int pointIndex = 0; pointIndex < pointCount; ++pointIndex ) { b3ContactConstraintPointWide* cp = c->points + pointIndex; @@ -1725,15 +1838,8 @@ void b3PushContacts_Convex( b3SolverBlock block, b3StepContext* context ) b3Vec3W rA = cp->anchorAs; b3Vec3W rB = cp->anchorBs; - // Moving anchors for current separation - // todo speed this up using matrices - b3Vec3W rsA = b3RotateVectorW( bA.dq, rA ); - b3Vec3W rsB = b3RotateVectorW( bB.dq, rB ); - - // compute current separation - // this is subject to round-off error if the anchor is far from the body center of mass - b3Vec3W ds = b3AddVW( dp, b3SubVW( rsB, rsA ) ); - b3FloatW s = b3AddW( b3DotW( c->normal, ds ), cp->baseSeparations ); + b3FloatW s = b3AddW( b3AddW( normalSeparation, b3SubW( b3DotW( normalB, rB ), b3DotW( normalA, rA ) ) ), + cp->baseSeparations ); // Apply speculative bias if separation is greater than zero, otherwise apply soft constraint bias b3FloatW separated = b3GreaterThanW( s, b3ZeroW() ); @@ -1777,8 +1883,7 @@ void b3PushContacts_Convex( b3SolverBlock block, b3StepContext* context ) b3TracyCZoneEnd( push_contact ); } -// Solve the normal constraint, friction, and rolling resistance. Deferred restitution augments -// the normal constraint. +// Solve the normal constraint, friction, and rolling resistance. void b3SolveContacts_Convex( b3SolverBlock block, b3StepContext* context ) { b3TracyCZoneNC( solve_contact, "Solve Contact", b3_colorAliceBlue, true ); @@ -1802,106 +1907,50 @@ void b3SolveContacts_Convex( b3SolverBlock block, b3StepContext* context ) b3BodyStateW bA = b3GatherBodies( states, c->indexA ); b3BodyStateW bB = b3GatherBodies( states, c->indexB ); - b3FloatW anyRestitution = c->points[0].negRestitutionVelocities; - for ( int pointIndex = 1; pointIndex < pointCount; ++pointIndex ) - { - anyRestitution = b3OrW( anyRestitution, c->points[pointIndex].negRestitutionVelocities ); - } - bool haveRestitution = b3AllZeroW( anyRestitution ) == false; - b3FloatW keepRestitution = b3ZeroW(); - b3Vec3W dp = b3SubVW( bB.dp, bA.dp ); + b3FloatW normalSeparation = b3DotW( c->normal, dp ); + b3Vec3W normalA = b3InvRotateVectorW( bA.dq, c->normal ); + b3Vec3W normalB = b3InvRotateVectorW( bB.dq, c->normal ); b3FloatW totalNormalImpulse = b3ZeroW(); b3FloatW totalTwistLimit = b3ZeroW(); - // Restitution with more than one point needs iteration to reduce spin. - // This makes restitution have a significant cost. - int iterationCount = haveRestitution ? pointCount : 1; - b3FloatW velocityBiases[B3_MAX_MANIFOLD_POINTS]; - - for ( int iteration = 0; iteration < iterationCount; ++iteration ) + for ( int pointIndex = 0; pointIndex < pointCount; ++pointIndex ) { - bool accumulate = iteration + 1 == iterationCount; + b3ContactConstraintPointWide* cp = c->points + pointIndex; - for ( int pointIndex = 0; pointIndex < pointCount; ++pointIndex ) - { - b3ContactConstraintPointWide* cp = c->points + pointIndex; + // Fixed anchor points for applying impulses + b3Vec3W rA = cp->anchorAs; + b3Vec3W rB = cp->anchorBs; - // Fixed anchor points for applying impulses - b3Vec3W rA = cp->anchorAs; - b3Vec3W rB = cp->anchorBs; + b3FloatW s = b3AddW( b3AddW( normalSeparation, b3SubW( b3DotW( normalB, rB ), b3DotW( normalA, rA ) ) ), + cp->baseSeparations ); - b3FloatW velocityBias; - if ( iteration == 0 ) - { - // Moving anchors for current separation - // todo speed this up using matrices - b3Vec3W rsA = b3RotateVectorW( bA.dq, rA ); - b3Vec3W rsB = b3RotateVectorW( bB.dq, rB ); + // Speculative bias, positive if separated and zero if overlapped + b3FloatW velocityBias = b3MaxW( b3ZeroW(), b3MulW( s, inv_h ) ); - // compute current separation - // this is subject to round-off error if the anchor is far from the body center of mass - b3Vec3W ds = b3AddVW( dp, b3SubVW( rsB, rsA ) ); - b3FloatW s = b3AddW( b3DotW( c->normal, ds ), cp->baseSeparations ); + // Relative velocity at contact + b3Vec3W vrA = b3AddVW( bA.v, b3CrossW( bA.w, rA ) ); + b3Vec3W vrB = b3AddVW( bB.v, b3CrossW( bB.w, rB ) ); + b3FloatW vn = b3DotW( b3SubVW( vrB, vrA ), c->normal ); - // Speculative bias, positive if separated and zero if overlapped - velocityBias = b3MaxW( b3ZeroW(), b3MulW( s, inv_h ) ); + // Compute normal impulse + b3FloatW negImpulse = b3MulW( cp->normalMasses, b3AddW( vn, velocityBias ) ); - if ( haveRestitution ) - { - // A lane with no restitution keeps the speculative bias. A bouncing lane stops - // bouncing once every point on its manifold has separated. - b3FloatW restitutionMask = b3GreaterThanW( b3ZeroW(), cp->negRestitutionVelocities ); - b3FloatW separated = b3GreaterThanW( s, b3ZeroW() ); - velocityBias = b3BlendW( velocityBias, cp->negRestitutionVelocities, restitutionMask ); - keepRestitution = b3OrW( keepRestitution, b3AndNotW( restitutionMask, separated ) ); - } + // Clamp the accumulated impulse + b3FloatW newImpulse = b3MaxW( b3SubW( cp->normalImpulses, negImpulse ), b3ZeroW() ); + b3FloatW deltaImpulse = b3SubW( newImpulse, cp->normalImpulses ); + cp->normalImpulses = newImpulse; + cp->totalNormalImpulses = b3AddW( cp->totalNormalImpulses, newImpulse ); + totalNormalImpulse = b3AddW( totalNormalImpulse, newImpulse ); + totalTwistLimit = b3AddW( totalTwistLimit, b3MulW( cp->leverArms, newImpulse ) ); - velocityBiases[pointIndex] = velocityBias; - } - else - { - velocityBias = velocityBiases[pointIndex]; - } - - // Relative velocity at contact - b3Vec3W vrA = b3AddVW( bA.v, b3CrossW( bA.w, rA ) ); - b3Vec3W vrB = b3AddVW( bB.v, b3CrossW( bB.w, rB ) ); - b3FloatW vn = b3DotW( b3SubVW( vrB, vrA ), c->normal ); - - // Compute normal impulse - b3FloatW negImpulse = b3MulW( cp->normalMasses, b3AddW( vn, velocityBias ) ); - - // Clamp the accumulated impulse - b3FloatW newImpulse = b3MaxW( b3SubW( cp->normalImpulses, negImpulse ), b3ZeroW() ); - b3FloatW deltaImpulse = b3SubW( newImpulse, cp->normalImpulses ); - cp->normalImpulses = newImpulse; - - // Don't accumulate the total impulse while bouncing. - if ( accumulate ) - { - cp->totalNormalImpulses = b3AddW( cp->totalNormalImpulses, newImpulse ); - totalNormalImpulse = b3AddW( totalNormalImpulse, newImpulse ); - totalTwistLimit = b3AddW( totalTwistLimit, b3MulW( cp->leverArms, newImpulse ) ); - } - - // Apply contact impulse - b3Vec3W P = b3MulSVW( deltaImpulse, c->normal ); - bA.w = b3MulSubMVW( bA.w, c->invIA, b3CrossW( rA, P ) ); - bA.v = b3MulSubSVW( bA.v, c->invMassA, P ); - bB.w = b3MulAddMVW( bB.w, c->invIB, b3CrossW( rB, P ) ); - bB.v = b3MulAddSVW( bB.v, c->invMassB, P ); - } - } - - if ( haveRestitution ) - { - for ( int pointIndex = 0; pointIndex < pointCount; ++pointIndex ) - { - b3ContactConstraintPointWide* cp = c->points + pointIndex; - cp->negRestitutionVelocities = b3BlendW( b3ZeroW(), cp->negRestitutionVelocities, keepRestitution ); - } + // Apply contact impulse + b3Vec3W P = b3MulSVW( deltaImpulse, c->normal ); + bA.w = b3MulSubMVW( bA.w, c->invIA, b3CrossW( rA, P ) ); + bA.v = b3MulSubSVW( bA.v, c->invMassA, P ); + bB.w = b3MulAddMVW( bB.w, c->invIB, b3CrossW( rB, P ) ); + bB.v = b3MulAddSVW( bB.v, c->invMassB, P ); } // Rolling resistance @@ -2015,6 +2064,93 @@ void b3SolveContacts_Convex( b3SolverBlock block, b3StepContext* context ) b3TracyCZoneEnd( solve_contact ); } +void b3ApplyRestitution_Convex( b3SolverBlock block, b3StepContext* context ) +{ + b3TracyCZoneNC( restitution, "Restitution", b3_colorDodgerBlue, true ); + + b3BodyState* states = context->states; + b3ContactConstraintWide* constraints = context->graph->colors[block.colorIndex].wideConstraints; + b3FloatW inv_h = b3SplatW( context->inv_h ); + b3FloatW negRestitutionThreshold = b3SplatW( -context->world->restitutionThreshold ); + b3FloatW zeroW = b3ZeroW(); + bool propagate = context->world->enableRestitutionPropagation; + + for ( int wideIndex = block.startIndex; wideIndex < block.startIndex + block.count; ++wideIndex ) + { + b3ContactConstraintWide* c = constraints + wideIndex; + if ( propagate == false && b3AllZeroW( c->restitution ) ) + { + continue; + } + + _Static_assert( B3_SIMD_WIDTH == 4, "width" ); + int pointCount1 = b3MaxInt( c->pointCounts[0], c->pointCounts[1] ); + int pointCount2 = b3MaxInt( c->pointCounts[2], c->pointCounts[3] ); + int pointCount = b3MaxInt( pointCount1, pointCount2 ); + B3_VALIDATE( 0 < pointCount && pointCount <= B3_MAX_MANIFOLD_POINTS ); + + b3BodyStateW bA = b3GatherBodies( states, c->indexA ); + b3BodyStateW bB = b3GatherBodies( states, c->indexB ); + + b3FloatW restitutionMask = b3GreaterThanW( c->restitution, zeroW ); + b3Vec3W dp = b3SubVW( bB.dp, bA.dp ); + b3Matrix3W dqA = b3MakeMatrixFromQuatW( bA.dq ); + b3Matrix3W dqB = b3MakeMatrixFromQuatW( bB.dq ); + + for ( int pointIndex = 0; pointIndex < pointCount; ++pointIndex ) + { + b3ContactConstraintPointWide* cp = c->points + pointIndex; + + b3Vec3W rA = cp->anchorAs; + b3Vec3W rB = cp->anchorBs; + + b3FloatW normalMass = propagate ? cp->normalMasses : b3BlendW( zeroW, cp->normalMasses, restitutionMask ); + + b3FloatW compressionImpulse = b3SubW( cp->totalNormalImpulses, cp->restitutionImpulses ); + b3FloatW armed = b3AndW( b3AndW( restitutionMask, b3LessThanW( cp->relativeVelocities, negRestitutionThreshold ) ), + b3GreaterThanW( compressionImpulse, zeroW ) ); + + b3Vec3W rsA = b3MulM3VW( dqA, rA ); + b3Vec3W rsB = b3MulM3VW( dqB, rB ); + b3Vec3W ds = b3AddVW( dp, b3SubVW( rsB, rsA ) ); + b3FloatW s = b3AddW( b3DotW( c->normal, ds ), cp->baseSeparations ); + + b3FloatW specBias = b3MaxW( zeroW, b3MulW( s, inv_h ) ); + b3FloatW velocityBias = b3BlendW( specBias, b3MulW( c->restitution, cp->relativeVelocities ), armed ); + + b3Vec3W vrA = b3AddVW( bA.v, b3CrossW( bA.w, rA ) ); + b3Vec3W vrB = b3AddVW( bB.v, b3CrossW( bB.w, rB ) ); + b3FloatW vn = b3DotW( b3SubVW( vrB, vrA ), c->normal ); + + b3FloatW negImpulse = b3MulW( normalMass, b3AddW( vn, velocityBias ) ); + + b3FloatW newImpulse = b3MaxW( b3SubW( cp->normalImpulses, negImpulse ), zeroW ); + b3FloatW impulse = b3SubW( newImpulse, cp->normalImpulses ); + + b3FloatW approachImpulse = b3MinW( b3MaxW( b3NegW( b3MulW( normalMass, vn ) ), zeroW ), b3MaxW( impulse, zeroW ) ); + b3FloatW allowance = + b3SubW( b3MulW( c->restitution, b3AddW( compressionImpulse, approachImpulse ) ), cp->restitutionImpulses ); + b3FloatW maxImpulse = b3AddW( approachImpulse, b3MaxW( allowance, zeroW ) ); + impulse = b3BlendW( impulse, b3MinW( impulse, maxImpulse ), armed ); + + cp->normalImpulses = b3AddW( cp->normalImpulses, impulse ); + cp->restitutionImpulses = b3AddW( cp->restitutionImpulses, b3SubW( impulse, approachImpulse ) ); + cp->totalNormalImpulses = b3AddW( cp->totalNormalImpulses, impulse ); + + b3Vec3W P = b3MulSVW( impulse, c->normal ); + bA.w = b3MulSubMVW( bA.w, c->invIA, b3CrossW( rA, P ) ); + bA.v = b3MulSubSVW( bA.v, c->invMassA, P ); + bB.w = b3MulAddMVW( bB.w, c->invIB, b3CrossW( rB, P ) ); + bB.v = b3MulAddSVW( bB.v, c->invMassB, P ); + } + + b3ScatterBodies( states, c->indexA, &bA ); + b3ScatterBodies( states, c->indexB, &bB ); + } + + b3TracyCZoneEnd( restitution ); +} + // Store impulses by contact constraint void b3StoreImpulses_Convex( b3SolverBlock block, b3StepContext* context, int workerIndex ) { @@ -2136,6 +2272,7 @@ void b3PrepareContacts_Overflow( b3StepContext* context ) b3ConstraintGraph* graph = context->graph; b3GraphColor* color = graph->colors + B3_OVERFLOW_INDEX; + B3_ASSERT( color->contacts.count <= UINT16_MAX ); uint16_t count = (uint16_t)color->contacts.count; if ( count == 0 ) { @@ -2215,6 +2352,27 @@ void b3SolveContacts_Overflow( b3StepContext* context ) b3SolveContacts_Mesh( block, context ); } +void b3ApplyRestitution_Overflow( b3StepContext* context ) +{ + b3ConstraintGraph* graph = context->graph; + b3GraphColor* color = graph->colors + B3_OVERFLOW_INDEX; + + uint16_t count = (uint16_t)color->contacts.count; + if ( count == 0 ) + { + return; + } + + b3SolverBlock block = { + .startIndex = 0, + .count = count, + .blockType = b3_overflowBlock, + .colorIndex = B3_OVERFLOW_INDEX, + }; + + b3ApplyRestitution_Mesh( block, context ); +} + void b3StoreImpulses_Overflow( b3StepContext* context ) { b3ConstraintGraph* graph = context->graph; diff --git a/src/contact_solver.h b/src/contact_solver.h index 523e70b..2ca02f1 100644 --- a/src/contact_solver.h +++ b/src/contact_solver.h @@ -10,9 +10,10 @@ typedef struct b3ManifoldConstraintPoint { b3Vec3 rA, rB; float baseSeparation; - float restitutionVelocity; + float relativeVelocity; float normalImpulse; float totalNormalImpulse; + float restitutionImpulse; float normalMass; float leverArm; } b3ManifoldConstraintPoint; @@ -46,6 +47,7 @@ typedef struct b3ContactConstraint b3Softness softness; b3Matrix3 rollingMass; float friction; + float restitution; float rollingResistance; int manifoldCount; } b3ContactConstraint; @@ -57,16 +59,19 @@ void b3PrepareContacts_Overflow( b3StepContext* context ); void b3WarmStartContacts_Overflow( b3StepContext* context ); void b3PushContacts_Overflow( b3StepContext* context ); void b3SolveContacts_Overflow( b3StepContext* context ); +void b3ApplyRestitution_Overflow( b3StepContext* context ); void b3StoreImpulses_Overflow( b3StepContext* context ); void b3PrepareContacts_Mesh( b3SolverBlock block, b3StepContext* context ); void b3WarmStartContacts_Mesh( b3SolverBlock block, b3StepContext* context ); void b3PushContacts_Mesh( b3SolverBlock block, b3StepContext* context ); void b3SolveContacts_Mesh( b3SolverBlock block, b3StepContext* context ); +void b3ApplyRestitution_Mesh( b3SolverBlock block, b3StepContext* context ); void b3StoreImpulses_Mesh( b3SolverBlock block, b3StepContext* context, int workerIndex ); void b3PrepareContacts_Convex( b3SolverBlock block, b3StepContext* context ); void b3WarmStartContacts_Convex( b3SolverBlock block, b3StepContext* context ); void b3PushContacts_Convex( b3SolverBlock block, b3StepContext* context ); void b3SolveContacts_Convex( b3SolverBlock block, b3StepContext* context ); +void b3ApplyRestitution_Convex( b3SolverBlock block, b3StepContext* context ); void b3StoreImpulses_Convex( b3SolverBlock block, b3StepContext* context, int workerIndex ); diff --git a/src/core.c b/src/core.c index c17e073..fee546a 100644 --- a/src/core.c +++ b/src/core.c @@ -19,7 +19,7 @@ #include #include -#ifdef BOX2D_PROFILE +#ifdef BOX3D_PROFILE #include #define b3TracyCAlloc( ptr, size ) TracyCAlloc( ptr, size ) diff --git a/src/dynamic_tree.c b/src/dynamic_tree.c index 3ec9d36..d9d07d8 100644 --- a/src/dynamic_tree.c +++ b/src/dynamic_tree.c @@ -1929,7 +1929,10 @@ int b3DynamicTree_Rebuild( b3DynamicTree* tree, bool fullBuild ) else { b3TreeNode node = *root; - node.flagIndex &= ~B3_MOVED_NODE; + if ( fullBuild == false ) + { + node.flagIndex &= ~B3_MOVED_NODE; + } leafIndices[0] = 0; leaves[0] = node; #if B3_TREE_HEURISTIC == 0 @@ -1954,7 +1957,10 @@ int b3DynamicTree_Rebuild( b3DynamicTree* tree, bool fullBuild ) continue; } - node.flagIndex &= ~B3_MOVED_NODE; + if ( fullBuild == false ) + { + node.flagIndex &= ~B3_MOVED_NODE; + } leafIndices[leafCount] = leafCount; leaves[leafCount] = node; #if B3_TREE_HEURISTIC == 0 @@ -1977,7 +1983,10 @@ int b3DynamicTree_Rebuild( b3DynamicTree* tree, bool fullBuild ) tree->dfsOrdered = true; b3DynamicTree_Validate( tree ); - b3DynamicTree_ValidateNoMoved( tree ); + if ( fullBuild == false ) + { + b3DynamicTree_ValidateNoMoved( tree ); + } return leafCount; } diff --git a/src/physics_world.c b/src/physics_world.c index d954633..8dbc2f7 100644 --- a/src/physics_world.c +++ b/src/physics_world.c @@ -327,6 +327,8 @@ b3WorldId b3CreateWorld( const b3WorldDef* def ) world->gravity = def->gravity; world->hitEventThreshold = def->hitEventThreshold; world->restitutionThreshold = def->restitutionThreshold; + world->restitutionIterations = b3ClampInt( def->restitutionIterations, 0, B3_MAX_RESTITUTION_ITERATIONS ); + world->enableRestitutionPropagation = def->enableRestitutionPropagation; world->maxLinearSpeed = def->maximumLinearSpeed; world->contactSpeed = def->contactSpeed; world->contactHertz = def->contactHertz; @@ -714,6 +716,7 @@ static void b3CollideTask( int startIndex, int endIndex, int workerIndex, void* b3Vec3 rB = b3MulMV( matrixB, mp->anchorB ); b3Vec3 dp = b3Add( dc, b3Sub( rB, rA ) ); mp->separation = mp->baseSeparation + b3Dot( dp, normal ); + mp->normalVelocity = 0.0f; mp->persisted = true; } } @@ -1125,7 +1128,6 @@ void b3World_Step( b3WorldId worldId, float timeStep, int subStepCount ) context.contactSoftness = b3MakeSoft( contactHertz, world->contactDampingRatio, context.h ); context.staticSoftness = b3MakeSoft( 2.0f * contactHertz, 0.5f * world->contactDampingRatio, context.h ); - context.restitutionThreshold = world->restitutionThreshold; context.maxLinearVelocity = world->maxLinearSpeed; context.enableWarmStarting = world->enableWarmStarting; @@ -1261,7 +1263,7 @@ static bool DrawQueryCallback( int proxyId, uint64_t userData, void* context ) { rgb = b3_colorWheat; } - else if ( body->flags & b3_hadTimeOfImpact ) + else if ( bodySim->flags & b3_hadTimeOfImpact ) { rgb = b3_colorLime; } @@ -2079,6 +2081,55 @@ float b3World_GetRestitutionThreshold( b3WorldId worldId ) return world->restitutionThreshold; } +void b3World_SetRestitutionIterations( b3WorldId worldId, int iterations ) +{ + b3World* world = b3GetUnlockedWorldFromId( worldId ); + if ( world == NULL ) + { + return; + } + + iterations = b3ClampInt( iterations, 0, B3_MAX_RESTITUTION_ITERATIONS ); + if ( iterations == world->restitutionIterations ) + { + return; + } + + B3_REC( world, WorldSetRestitutionIterations, worldId, iterations ); + + world->restitutionIterations = iterations; +} + +int b3World_GetRestitutionIterations( b3WorldId worldId ) +{ + b3World* world = b3GetWorldFromId( worldId ); + return world->restitutionIterations; +} + +void b3World_EnableRestitutionPropagation( b3WorldId worldId, bool flag ) +{ + b3World* world = b3GetUnlockedWorldFromId( worldId ); + if ( world == NULL ) + { + return; + } + + if ( flag == world->enableRestitutionPropagation ) + { + return; + } + + B3_REC( world, WorldEnableRestitutionPropagation, worldId, flag ); + + world->enableRestitutionPropagation = flag; +} + +bool b3World_IsRestitutionPropagationEnabled( b3WorldId worldId ) +{ + b3World* world = b3GetWorldFromId( worldId ); + return world->enableRestitutionPropagation; +} + void b3World_SetHitEventThreshold( b3WorldId worldId, float value ) { b3World* world = b3GetUnlockedWorldFromId( worldId ); diff --git a/src/physics_world.h b/src/physics_world.h index fcfee9f..a302aa3 100644 --- a/src/physics_world.h +++ b/src/physics_world.h @@ -235,6 +235,8 @@ typedef struct b3World b3Vec3 gravity; float hitEventThreshold; float restitutionThreshold; + int restitutionIterations; + bool enableRestitutionPropagation; float maxLinearSpeed; float contactSpeed; float contactHertz; @@ -301,6 +303,8 @@ b3World* b3GetWorldFromId( b3WorldId id ); b3World* b3GetUnlockedWorld( int index ); b3World* b3GetWorld( int index ); +void b3World_RebuildStaticTree( b3WorldId worldId ); + void b3ValidateConnectivity( b3World* world ); void b3ValidateSolverSets( b3World* world ); void b3ValidateContacts( b3World* world ); diff --git a/src/prismatic_joint.c b/src/prismatic_joint.c index c96e6d6..b380d37 100644 --- a/src/prismatic_joint.c +++ b/src/prismatic_joint.c @@ -307,8 +307,8 @@ void b3PreparePrismaticJoint( b3JointSim* base, b3StepContext* context ) b3Body* bodyA = b3Array_Get( world->bodies, base->bodyIdA ); b3Body* bodyB = b3Array_Get( world->bodies, base->bodyIdB ); - B3_ASSERT( bodyB->setIndex == b3_awakeSet ); - b3SolverSet* setA = b3Array_Get( world->solverSets, bodyA->setIndex ); + B3_ASSERT( bodyA->setIndex == b3_awakeSet || bodyB->setIndex == b3_awakeSet ); + b3SolverSet* setA = b3Array_Get( world->solverSets, bodyA->setIndex ); b3SolverSet* setB = b3Array_Get( world->solverSets, bodyB->setIndex ); int localIndexA = bodyA->localIndex; diff --git a/src/recording.h b/src/recording.h index e7c0cd2..bcc6608 100644 --- a/src/recording.h +++ b/src/recording.h @@ -52,7 +52,7 @@ typedef struct b3World b3World; // Minor tracks op-stream additions that keep the 48 byte header shape. // Minor version 7 made the contact body sim locators persistent -#define B3_REC_VERSION_MINOR 7 +#define B3_REC_VERSION_MINOR 8 // File header, fixed 48 bytes. Contains the registry locator so the player // can load geometry before replaying any ops. diff --git a/src/recording_ops.inl b/src/recording_ops.inl index 6b6973d..d2c992b 100644 --- a/src/recording_ops.inl +++ b/src/recording_ops.inl @@ -38,6 +38,8 @@ B3_REC_OP( 0x0A, WorldSetMaximumLinearSpeed, RET_NONE, ARG( WORLDID, world ) ARG B3_REC_OP( 0x0B, WorldEnableWarmStarting, RET_NONE, ARG( WORLDID, world ) ARG( BOOL, flag ) ) B3_REC_OP( 0x0C, WorldRebuildStaticTree, RET_NONE, ARG( WORLDID, world ) ) B3_REC_OP( 0x0D, WorldEnableSpeculative, RET_NONE, ARG( WORLDID, world ) ARG( BOOL, flag ) ) +B3_REC_OP( 0x0E, WorldSetRestitutionIterations, RET_NONE, ARG( WORLDID, world ) ARG( I32, iterations ) ) +B3_REC_OP( 0x0F, WorldEnableRestitutionPropagation, RET_NONE, ARG( WORLDID, world ) ARG( BOOL, flag ) ) // Body B3_REC_OP( 0x10, CreateBody, RET_BODYID, ARG( WORLDID, world ) ARG( BODYDEF, def ) ) diff --git a/src/recording_replay.c b/src/recording_replay.c index dc23de2..9c0247c 100644 --- a/src/recording_replay.c +++ b/src/recording_replay.c @@ -819,6 +819,17 @@ static void b3RecDispatch_WorldEnableSpeculative( const b3RecArgs_WorldEnableSpe b3World_EnableSpeculative( rdr->replayWorldId, a->flag ); } +static void b3RecDispatch_WorldSetRestitutionIterations( const b3RecArgs_WorldSetRestitutionIterations* a, b3RecReader* rdr ) +{ + b3World_SetRestitutionIterations( rdr->replayWorldId, a->iterations ); +} + +static void b3RecDispatch_WorldEnableRestitutionPropagation( const b3RecArgs_WorldEnableRestitutionPropagation* a, + b3RecReader* rdr ) +{ + b3World_EnableRestitutionPropagation( rdr->replayWorldId, a->flag ); +} + static void b3RecDispatch_CreateBody( const b3RecArgs_CreateBody* a, b3RecReader* rdr ) { b3BodyId recId = b3RecR_BODYID( rdr ); diff --git a/src/scheduler.c b/src/scheduler.c index 7bcde42..ed942f2 100644 --- a/src/scheduler.c +++ b/src/scheduler.c @@ -122,7 +122,7 @@ b3Scheduler* b3CreateScheduler( int workerCount ) scheduler->workerContexts[i].threadIndex = i + 1; char name[16]; - snprintf( name, sizeof( name ), "box2d_worker_%02d", i + 1 ); + snprintf( name, sizeof( name ), "box3d_worker_%02d", i + 1 ); scheduler->threads[i] = b3CreateThread( b3SchedulerWorkerMain, scheduler->workerContexts + i, name ); } diff --git a/src/simd.h b/src/simd.h index 92ba2bf..e7b80bd 100644 --- a/src/simd.h +++ b/src/simd.h @@ -412,11 +412,26 @@ static inline void b3StoreW( float* data, b3FloatW a ) vst1q_f32( data, a ); } +static inline b3FloatW b3UnpackLoW( b3FloatW a, b3FloatW b ) +{ + return vzip1q_f32( a, b ); +} + +static inline b3FloatW b3UnpackHiW( b3FloatW a, b3FloatW b ) +{ + return vzip2q_f32( a, b ); +} + static inline b3FloatW b3NegW( b3FloatW a ) { return vnegq_f32( a ); } +static inline b3FloatW b3AbsW( b3FloatW a ) +{ + return vabsq_f32( a ); +} + static inline b3FloatW b3AddW( b3FloatW a, b3FloatW b ) { return vaddq_f32( a, b ); @@ -589,6 +604,16 @@ static inline void b3StoreW( float* data, b3FloatW a ) _mm_storeu_ps( data, a ); } +static inline b3FloatW b3UnpackLoW( b3FloatW a, b3FloatW b ) +{ + return _mm_unpacklo_ps( a, b ); +} + +static inline b3FloatW b3UnpackHiW( b3FloatW a, b3FloatW b ) +{ + return _mm_unpackhi_ps( a, b ); +} + static inline b3FloatW b3NegW( b3FloatW a ) { // Create a mask with the sign bit set for each element @@ -598,6 +623,11 @@ static inline b3FloatW b3NegW( b3FloatW a ) return _mm_xor_ps( a, mask ); } +static inline b3FloatW b3AbsW( b3FloatW a ) +{ + return _mm_andnot_ps( _mm_set1_ps( -0.0f ), a ); +} + static inline b3FloatW b3AddW( b3FloatW a, b3FloatW b ) { return _mm_add_ps( a, b ); @@ -772,11 +802,26 @@ static inline void b3StoreW( float* data, b3FloatW a ) data[3] = a.w; } +static inline b3FloatW b3UnpackLoW( b3FloatW a, b3FloatW b ) +{ + return (b3FloatW){ a.x, b.x, a.y, b.y }; +} + +static inline b3FloatW b3UnpackHiW( b3FloatW a, b3FloatW b ) +{ + return (b3FloatW){ a.z, b.z, a.w, b.w }; +} + static inline b3FloatW b3NegW( b3FloatW a ) { return (b3FloatW){ -a.x, -a.y, -a.z, -a.w }; } +static inline b3FloatW b3AbsW( b3FloatW a ) +{ + return (b3FloatW){ a.x < 0.0f ? -a.x : a.x, a.y < 0.0f ? -a.y : a.y, a.z < 0.0f ? -a.z : a.z, a.w < 0.0f ? -a.w : a.w }; +} + static inline b3FloatW b3AddW( b3FloatW a, b3FloatW b ) { return (b3FloatW){ a.x + b.x, a.y + b.y, a.z + b.z, a.w + b.w }; @@ -976,6 +1021,19 @@ static inline int b3MinIndexW( b3FloatW a, int bitCount ) #endif +static inline void b3TransposeW( b3FloatW r0, b3FloatW r1, b3FloatW r2, b3FloatW r3, b3FloatW* c0, b3FloatW* c1, b3FloatW* c2, + b3FloatW* c3 ) +{ + b3FloatW t0 = b3UnpackLoW( r0, r2 ); + b3FloatW t1 = b3UnpackLoW( r1, r3 ); + b3FloatW t2 = b3UnpackHiW( r0, r2 ); + b3FloatW t3 = b3UnpackHiW( r1, r3 ); + *c0 = b3UnpackLoW( t0, t1 ); + *c1 = b3UnpackHiW( t0, t1 ); + *c2 = b3UnpackLoW( t2, t3 ); + *c3 = b3UnpackHiW( t2, t3 ); +} + #if defined( B3_SIMD_NEON ) typedef struct b3AABBV diff --git a/src/solver.c b/src/solver.c index 2773ea2..bbb1175 100644 --- a/src/solver.c +++ b/src/solver.c @@ -382,7 +382,7 @@ static bool b3ContinuousQueryCallback( int proxyId, uint64_t userData, void* con B3_ASSERT( body->type == b3_staticBody || ( fastBodySim->flags & b3_isBullet ) ); // Skip bullets - if ( bodySim->flags & b3_isBullet ) + if ( body->flags & b3_isBullet ) { return true; } @@ -450,7 +450,6 @@ static bool b3ContinuousQueryCallback( int proxyId, uint64_t userData, void* con if ( didHit ) { - fastBodySim->flags |= b3_hadTimeOfImpact; continuousContext->fraction = output.fraction; continuousContext->distanceIterations = b3MaxInt( continuousContext->distanceIterations, output.distanceIterations ); continuousContext->pushBackIterations = b3MaxInt( continuousContext->pushBackIterations, output.pushBackIterations ); @@ -564,6 +563,7 @@ static void b3SolveContinuous( b3World* world, int bodySimIndex, b3TaskContext* fastBodySim->center = center; fastBodySim->rotation0 = q; fastBodySim->center0 = center; + fastBodySim->flags |= b3_hadTimeOfImpact; // Timeloss means there is a lost gravity contribution. Other forces and torques are ignored for now. b3BodyState* fastBodyState = b3Array_Get( awakeSet->bodyStates, bodySimIndex ); @@ -1043,6 +1043,17 @@ static void b3ExecuteBlock( b3SolverStage* stage, b3StepContext* context, b3Solv } break; + case b3_stageRestitution: + if ( blockType == b3_graphWideContactBlock ) + { + b3ApplyRestitution_Convex( block, context ); + } + else if ( blockType == b3_graphContactBlock ) + { + b3ApplyRestitution_Mesh( block, context ); + } + break; + case b3_stageStoreWideImpulses: b3StoreImpulses_Convex( block, context, workerIndex ); break; @@ -1305,6 +1316,29 @@ static void b3SolverTask( void* taskContext ) // integrate velocities / warm start / solve / integrate positions / relax stageIndex += 1 + activeColorCount + ITERATIONS * activeColorCount + 1 + RELAX_ITERATIONS * activeColorCount; + int restitutionIterations = context->world->restitutionIterations; + if ( restitutionIterations > 0 && b3AtomicLoadInt( &context->anyRestitution ) != 0 ) + { + for ( int j = 0; j < restitutionIterations; ++j ) + { + b3ApplyRestitution_Overflow( context ); + + int iterationStageIndex = stageIndex; + for ( int colorIndex = 0; colorIndex < activeColorCount; ++colorIndex ) + { + syncBits = ( graphSyncIndex << 16 ) | iterationStageIndex; + B3_ASSERT( stages[iterationStageIndex].type == b3_stageRestitution ); + b3ExecuteMainStage( stages + iterationStageIndex, context, syncBits ); + iterationStageIndex += 1; + } + graphSyncIndex += 1; + } + + profile->restitution += b3GetMillisecondsAndReset( &ticks ); + } + + stageIndex += activeColorCount; + // Store impulses b3StoreImpulses_Overflow( context ); @@ -1584,15 +1618,6 @@ void b3Solve( b3World* world, b3StepContext* stepContext ) int colorContactCountW = ( ( colorConvexContactCount - 1 ) >> B3_SIMD_SHIFT ) + 1; color->wideConstraintCount = colorContactCountW; - - // Zero remainder lanes in the tail wide slot so prepare workers don't need to - // initialize them. - if ( ( colorConvexContactCount & ( B3_SIMD_WIDTH - 1 ) ) != 0 ) - { - memset( (uint8_t*)color->wideConstraints + ( colorContactCountW - 1 ) * wideContactByteCount, 0, - wideContactByteCount ); - } - wideBase += colorContactCountW; } @@ -1663,6 +1688,8 @@ void b3Solve( b3World* world, b3StepContext* stepContext ) stageCount += 1; // b3_stageRelax stageCount += RELAX_ITERATIONS * activeColorCount; + // b3_stageRestitution + stageCount += activeColorCount; // b3_stageStoreWideImpulses stageCount += 1; // b3_stageStoreImpulses @@ -1746,6 +1773,8 @@ void b3Solve( b3World* world, b3StepContext* stepContext ) stage = b3InitStage( stage, b3_stageIntegratePositions, bodyBlocks, bodyDim.count, UINT8_MAX ); stage = b3InitColorStages( stage, b3_stageRelax, RELAX_ITERATIONS, activeColorCount, graphColorBlocks, graphBlockCounts, activeColorIndices ); + stage = b3InitColorStages( stage, b3_stageRestitution, 1, activeColorCount, graphColorBlocks, graphBlockCounts, + activeColorIndices ); stage = b3InitStage( stage, b3_stageStoreWideImpulses, convexBlocks, convexPrepareDim.count, UINT8_MAX ); stage = b3InitStage( stage, b3_stageStoreImpulses, meshBlocks, meshPrepareDim.count, UINT8_MAX ); diff --git a/src/solver.h b/src/solver.h index 3f96a65..b8f8950 100644 --- a/src/solver.h +++ b/src/solver.h @@ -76,6 +76,7 @@ typedef enum b3SolverStageType b3_stageSolve, b3_stageIntegratePositions, b3_stageRelax, + b3_stageRestitution, b3_stageStoreWideImpulses, b3_stageStoreImpulses, } b3SolverStageType; @@ -188,7 +189,6 @@ typedef struct b3StepContext b3Softness contactSoftness; b3Softness staticSoftness; - float restitutionThreshold; float maxLinearVelocity; struct b3World* world; @@ -208,6 +208,8 @@ typedef struct b3StepContext int* bulletBodies; b3AtomicInt bulletBodyCount; + b3AtomicInt anyRestitution; + // Contact ids for simplified parallel-for access. Used in narrow-phase. // These contacts may or may not be touching. They are associated with awake bodies. int* awakeContactIndices; diff --git a/src/types.c b/src/types.c index 13d947b..12f4c09 100644 --- a/src/types.c +++ b/src/types.c @@ -16,6 +16,8 @@ b3WorldDef b3DefaultWorldDef( void ) def.gravity.y = -10.0f; def.hitEventThreshold = 1.0f * lengthUnits; def.restitutionThreshold = 1.0f * lengthUnits; + def.restitutionIterations = 2; + def.enableRestitutionPropagation = false; def.contactSpeed = 3.0f * lengthUnits; def.contactHertz = 30.0f; def.contactDampingRatio = 10.0f; diff --git a/src/weld_joint.c b/src/weld_joint.c index 611b714..86b62ed 100644 --- a/src/weld_joint.c +++ b/src/weld_joint.c @@ -91,7 +91,7 @@ void b3PrepareWeldJoint( b3JointSim* base, b3StepContext* context ) b3Body* bodyA = b3Array_Get( world->bodies, base->bodyIdA ); b3Body* bodyB = b3Array_Get( world->bodies, base->bodyIdB ); - B3_ASSERT( bodyB->setIndex == b3_awakeSet ); + B3_ASSERT( bodyA->setIndex == b3_awakeSet || bodyB->setIndex == b3_awakeSet ); b3SolverSet* setA = b3Array_Get( world->solverSets, bodyA->setIndex ); b3SolverSet* setB = b3Array_Get( world->solverSets, bodyB->setIndex ); diff --git a/src/world_snapshot.c b/src/world_snapshot.c index b7c1a21..c3f0271 100644 --- a/src/world_snapshot.c +++ b/src/world_snapshot.c @@ -33,7 +33,7 @@ // Snapshot image magic 'BNS3' and version #define B3_SNAP_MAGIC 0x33534E42u -#define B3_SNAP_VERSION 7u // same-step restitution +#define B3_SNAP_VERSION 10u // post solve restitution #define B3_SNAP_FLAG_VALIDATION 0x1u #define B3_SNAP_FLAG_DOUBLE_PRECISION 0x2u @@ -470,6 +470,7 @@ static void b3SerWorldConfig( b3RecBuffer* buf, const b3World* world ) b3SnapW_Bytes( buf, &world->gravity, sizeof( b3Vec3 ) ); b3SnapW_Bytes( buf, &world->hitEventThreshold, sizeof( float ) ); b3SnapW_Bytes( buf, &world->restitutionThreshold, sizeof( float ) ); + b3SnapW_I32( buf, world->restitutionIterations ); b3SnapW_Bytes( buf, &world->maxLinearSpeed, sizeof( float ) ); b3SnapW_Bytes( buf, &world->contactSpeed, sizeof( float ) ); b3SnapW_Bytes( buf, &world->contactHertz, sizeof( float ) ); @@ -487,6 +488,7 @@ static void b3SerWorldConfig( b3RecBuffer* buf, const b3World* world ) flags |= world->enableWarmStarting ? 0x02u : 0u; flags |= world->enableContinuous ? 0x04u : 0u; flags |= world->enableSpeculative ? 0x08u : 0u; + flags |= world->enableRestitutionPropagation ? 0x10u : 0u; b3RecBufAppend( buf, &flags, 1 ); } @@ -495,6 +497,7 @@ static void b3DesWorldConfig( b3SnapReader* r, b3World* world ) b3SnapR_Bytes( r, &world->gravity, sizeof( b3Vec3 ) ); b3SnapR_Bytes( r, &world->hitEventThreshold, sizeof( float ) ); b3SnapR_Bytes( r, &world->restitutionThreshold, sizeof( float ) ); + world->restitutionIterations = b3SnapR_I32( r ); b3SnapR_Bytes( r, &world->maxLinearSpeed, sizeof( float ) ); b3SnapR_Bytes( r, &world->contactSpeed, sizeof( float ) ); b3SnapR_Bytes( r, &world->contactHertz, sizeof( float ) ); @@ -513,6 +516,7 @@ static void b3DesWorldConfig( b3SnapReader* r, b3World* world ) world->enableWarmStarting = ( flags & 0x02u ) != 0; world->enableContinuous = ( flags & 0x04u ) != 0; world->enableSpeculative = ( flags & 0x08u ) != 0; + world->enableRestitutionPropagation = ( flags & 0x10u ) != 0; } // Shapes carry pointer fields: materials, userData, userShape, and the geometry union. @@ -628,7 +632,7 @@ static void b3DesShapes( b3SnapReader* r, b3World* world, b3RecReader* rdr ) // shape with the same geometry. Carrying its handle over avoids tearing down and rebuilding every // GPU mesh on each seek, which the host (a 3D renderer) would otherwise pay for. Handles that are // not reclaimed below belong to shapes that are gone or were replaced, and get released so the host - // pool does not leak across seeks. Box2D has no such handles, so its restore skips all of this. + // pool does not leak across seeks. int oldShapeCount = world->shapes.count; void** savedUserShape = NULL; uint16_t* savedGeneration = NULL; diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt index 6be43e1..510e343 100644 --- a/test/CMakeLists.txt +++ b/test/CMakeLists.txt @@ -6,6 +6,7 @@ set(BOX3D_TEST_FILES test_bitset.c test_body.c test_body_query.c + test_broad_phase.c test_collision.c test_compound.c test_container.c diff --git a/test/main.c b/test/main.c index 9539d51..df993cf 100644 --- a/test/main.c +++ b/test/main.c @@ -29,6 +29,7 @@ extern int AllocatorTest( void ); extern int BitTest( void ); extern int BodyTest( void ); extern int BodyQueryTest( void ); +extern int BroadPhaseTest( void ); extern int CollisionTest( void ); extern int CompoundTest( void ); extern int ContainerTest( void ); @@ -106,6 +107,7 @@ int main( int argc, char** argv ) MAYBE_RUN_TEST( BitTest ); MAYBE_RUN_TEST( BodyTest ); MAYBE_RUN_TEST( BodyQueryTest ); + MAYBE_RUN_TEST( BroadPhaseTest ); MAYBE_RUN_TEST( CollisionTest ); MAYBE_RUN_TEST( CompoundTest ); MAYBE_RUN_TEST( ContainerTest ); diff --git a/test/test_allocator.c b/test/test_allocator.c index 1e78b7d..b8bd2bd 100644 --- a/test/test_allocator.c +++ b/test/test_allocator.c @@ -62,7 +62,7 @@ static int TestBlockAlignment( void ) { elements[j] = b3AllocateElement( &allocator ); ENSURE( elements[j] != NULL ); - ENSURE( ( (uintptr_t)elements[j] & ( B3_ALIGNMENT - 1 ) ) == 0 ); + ENSURE( ( (uintptr_t)elements[j] & ( B3_BLOCK_ALIGNMENT - 1 ) ) == 0 ); } for ( int j = 0; j < count; ++j ) @@ -75,7 +75,7 @@ static int TestBlockAlignment( void ) { elements[j] = b3AllocateElement( &allocator ); ENSURE( elements[j] != NULL ); - ENSURE( ( (uintptr_t)elements[j] & ( B3_ALIGNMENT - 1 ) ) == 0 ); + ENSURE( ( (uintptr_t)elements[j] & ( B3_BLOCK_ALIGNMENT - 1 ) ) == 0 ); } for ( int j = 0; j < count; ++j ) diff --git a/test/test_broad_phase.c b/test/test_broad_phase.c new file mode 100644 index 0000000..85adbf6 --- /dev/null +++ b/test/test_broad_phase.c @@ -0,0 +1,488 @@ +// SPDX-FileCopyrightText: 2026 Erin Catto +// SPDX-License-Identifier: MIT + +#include "body.h" +#include "broad_phase.h" +#include "physics_world.h" +#include "shape.h" +#include "table.h" +#include "test_macros.h" + +#include "box3d/box3d.h" +#include "box3d/math_functions.h" + +#include + +// Brute force check of the broad-phase pair set against every live shape pair. Must run right +// after a zero time step so the pair update and the disjoint contact purge have both happened +// against the current fat boxes. +static int CheckPairSet( b3WorldId worldId ) +{ + b3World* world = b3GetWorldFromId( worldId ); + b3BroadPhase* bp = &world->broadPhase; + const b3Shape* shapes = world->shapes.data; + const b3AABB* fatAABBs = world->fatAABBs.data; + int shapeCount = world->shapes.count; + + for ( int i = 0; i < shapeCount; ++i ) + { + const b3Shape* shapeA = shapes + i; + if ( shapeA->id == B3_NULL_INDEX || shapeA->proxyKey == B3_NULL_INDEX || shapeA->sensorIndex != B3_NULL_INDEX ) + { + continue; + } + + b3Body* bodyA = b3Array_Get( world->bodies, shapeA->bodyId ); + + for ( int j = i + 1; j < shapeCount; ++j ) + { + const b3Shape* shapeB = shapes + j; + if ( shapeB->id == B3_NULL_INDEX || shapeB->proxyKey == B3_NULL_INDEX || shapeB->sensorIndex != B3_NULL_INDEX ) + { + continue; + } + + if ( shapeA->bodyId == shapeB->bodyId ) + { + continue; + } + + b3Body* bodyB = b3Array_Get( world->bodies, shapeB->bodyId ); + + bool expected = b3ShouldShapesCollide( shapeA->filter, shapeB->filter ) && + b3ShouldBodiesCollide( world, bodyA, bodyB ) && b3AABB_Overlaps( fatAABBs[i], fatAABBs[j] ); + + bool actual = b3ContainsKey( &bp->pairSet, b3ShapePairKey( i, j, 0 ) ); + + // A sleeping contact is never refreshed, so a stale pair can outlive the overlap when + // the other body is teleported, and a set woken by the narrow phase carries its stale + // contacts into the awake set one collide late. With sleeping on only a missing pair + // is a bug. + if ( world->enableSleep && actual && expected == false ) + { + continue; + } + + if ( expected != actual ) + { + printf( " pair mismatch: shapes %d (body %d type %d set %d) and %d (body %d type %d set %d), expected %d, " + "actual %d\n", + i, shapeA->bodyId, bodyA->type, bodyA->setIndex, j, shapeB->bodyId, bodyB->type, bodyB->setIndex, + expected, actual ); + fflush( stdout ); + return 1; + } + } + } + + return 0; +} + +typedef struct RandomState +{ + uint32_t seed; +} RandomState; + +static uint32_t NextRandom( RandomState* state ) +{ + // xorshift32 + uint32_t x = state->seed; + x ^= x << 13; + x ^= x >> 17; + x ^= x << 5; + state->seed = x; + return x; +} + +static float RandomFloat( RandomState* state, float lower, float upper ) +{ + float u = (float)( NextRandom( state ) & 0xFFFFFF ) / (float)0x1000000; + return lower + ( upper - lower ) * u; +} + +static int RandomInt( RandomState* state, int count ) +{ + return (int)( NextRandom( state ) % (uint32_t)count ); +} + +#define MAX_BODIES 64 +#define STATIC_COUNT 60 +#define TILE_COUNT 9 + +typedef struct BodySlot +{ + b3BodyId bodyId; + bool active; +} BodySlot; + +typedef struct StressContext +{ + b3WorldId worldId; + RandomState random; + b3Pos origin; + float extent; + BodySlot slots[MAX_BODIES]; + b3JointId joints[4]; + int jointCount; + b3BoxHull box; +} StressContext; + +static b3Pos RandomPosition( StressContext* context ) +{ + float e = context->extent; + b3Vec3 offset = { RandomFloat( &context->random, -e, e ), RandomFloat( &context->random, -e, e ), + RandomFloat( &context->random, -e, e ) }; + return b3OffsetPos( context->origin, offset ); +} + +static b3Vec3 RandomVelocity( StressContext* context ) +{ + // Some bodies outrun their margin every step and go through continuous collision + float s = RandomInt( &context->random, 4 ) == 0 ? 40.0f : 12.0f; + return (b3Vec3){ RandomFloat( &context->random, -s, s ), RandomFloat( &context->random, -s, s ), + RandomFloat( &context->random, -s, s ) }; +} + +static void CreateSlotBody( StressContext* context, int slot ) +{ + RandomState* random = &context->random; + + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.type = RandomInt( random, 10 ) == 0 ? b3_kinematicBody : b3_dynamicBody; + bodyDef.position = RandomPosition( context ); + bodyDef.linearVelocity = RandomVelocity( context ); + bodyDef.isBullet = RandomInt( random, 3 ) == 0; + bodyDef.gravityScale = 0.0f; + b3BodyId bodyId = b3CreateBody( context->worldId, &bodyDef ); + + b3ShapeDef shapeDef = b3DefaultShapeDef(); + shapeDef.filter.categoryBits = 1ull << RandomInt( random, 3 ); + shapeDef.filter.maskBits = RandomInt( random, 4 ) == 0 ? 0x5ull : B3_DEFAULT_MASK_BITS; + + int shapeCount = 1 + RandomInt( random, 2 ); + for ( int i = 0; i < shapeCount; ++i ) + { + if ( RandomInt( random, 2 ) == 0 ) + { + b3Sphere sphere = { { RandomFloat( random, -1.0f, 1.0f ), 0.0f, 0.0f }, RandomFloat( random, 0.2f, 1.5f ) }; + b3CreateSphereShape( bodyId, &shapeDef, &sphere ); + } + else + { + b3CreateHullShape( bodyId, &shapeDef, &context->box.base ); + } + } + + context->slots[slot].bodyId = bodyId; + context->slots[slot].active = true; +} + +// A swarm of bodies churning through every proxy mutation the world API offers, with the pair +// set checked against a brute force scan after every step. +static int RunBroadPhaseStress( int workerCount, uint32_t seed, bool enableSleep ) +{ + b3WorldDef worldDef = b3DefaultWorldDef(); + worldDef.workerCount = workerCount; + worldDef.enableSleep = enableSleep; + worldDef.gravity = b3Vec3_zero; + b3WorldId worldId = b3CreateWorld( &worldDef ); + + StressContext context = { 0 }; + context.worldId = worldId; + context.random.seed = seed; + context.origin = (b3Pos){ -1900.0, 0.0, 1330.0 }; + context.extent = 12.0f; + context.box = b3MakeBoxHull( 0.95f, 0.6f, 2.2f ); + + // Kilometre tiles far from the origin under the swarm, like a streamed terrain + b3HeightFieldData* tile = b3CreateGrid( 65, 65, (b3Vec3){ 16.0f, 1.0f, 16.0f }, false ); + for ( int i = 0; i < TILE_COUNT; ++i ) + { + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.type = b3_staticBody; + bodyDef.position = (b3Pos){ -3072.0 + 1024.0 * ( i % 3 ), -30.0, 1024.0 * ( i / 3 ) }; + b3BodyId tileId = b3CreateBody( worldId, &bodyDef ); + b3ShapeDef shapeDef = b3DefaultShapeDef(); + shapeDef.enableSpeculativeContact = false; + b3CreateHeightFieldShape( tileId, &shapeDef, tile ); + } + + // Static clutter inside the swarm. Some pieces ask for contacts on creation. + b3BodyId staticIds[STATIC_COUNT]; + b3HeightFieldData* grid = b3CreateGrid( 5, 5, (b3Vec3){ 2.0f, 1.0f, 2.0f }, false ); + b3BoxHull staticBox = b3MakeBoxHull( 3.0f, 0.5f, 3.0f ); + for ( int i = 0; i < STATIC_COUNT; ++i ) + { + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.type = b3_staticBody; + bodyDef.position = RandomPosition( &context ); + staticIds[i] = b3CreateBody( worldId, &bodyDef ); + + b3ShapeDef shapeDef = b3DefaultShapeDef(); + shapeDef.invokeContactCreation = ( i % 4 ) == 0; + shapeDef.enableSpeculativeContact = ( i % 2 ) == 0; + + if ( i % 3 == 0 ) + { + b3CreateHeightFieldShape( staticIds[i], &shapeDef, grid ); + } + else + { + b3CreateHullShape( staticIds[i], &shapeDef, &staticBox.base ); + } + } + + for ( int i = 0; i < MAX_BODIES; ++i ) + { + if ( RandomInt( &context.random, 4 ) != 0 ) + { + CreateSlotBody( &context, i ); + } + } + + float timeStep = 1.0f / 60.0f; + int stepCount = 400; + + for ( int step = 0; step < stepCount; ++step ) + { + RandomState* random = &context.random; + + int mutationCount = RandomInt( random, 4 ); + for ( int m = 0; m < mutationCount; ++m ) + { + int slot = RandomInt( random, MAX_BODIES ); + BodySlot* body = context.slots + slot; + int action = RandomInt( random, 12 ); + + if ( body->active == false ) + { + if ( action < 6 ) + { + CreateSlotBody( &context, slot ); + } + continue; + } + + switch ( action ) + { + case 0: + b3DestroyBody( body->bodyId ); + body->active = false; + break; + + case 1: + case 2: + b3Body_SetTransform( body->bodyId, RandomPosition( &context ), b3Quat_identity ); + break; + + case 3: + b3Body_SetLinearVelocity( body->bodyId, RandomVelocity( &context ) ); + break; + + case 4: + b3Body_Disable( body->bodyId ); + break; + + case 5: + b3Body_Enable( body->bodyId ); + break; + + case 6: + b3Body_SetBullet( body->bodyId, RandomInt( random, 2 ) == 0 ); + break; + + case 7: + { + b3BodyType types[3] = { b3_staticBody, b3_kinematicBody, b3_dynamicBody }; + b3Body_SetType( body->bodyId, types[RandomInt( random, 3 )] ); + if ( b3Body_GetType( body->bodyId ) != b3_staticBody ) + { + b3Body_SetLinearVelocity( body->bodyId, RandomVelocity( &context ) ); + } + } + break; + + case 8: + { + // Add a joint that blocks collision, or toggle one + if ( context.jointCount < 4 ) + { + int otherSlot = RandomInt( random, MAX_BODIES ); + BodySlot* other = context.slots + otherSlot; + if ( otherSlot != slot && other->active && b3Body_GetType( body->bodyId ) == b3_dynamicBody && + b3Body_GetType( other->bodyId ) == b3_dynamicBody ) + { + b3WeldJointDef jointDef = b3DefaultWeldJointDef(); + jointDef.base.bodyIdA = body->bodyId; + jointDef.base.bodyIdB = other->bodyId; + jointDef.base.collideConnected = false; + context.joints[context.jointCount++] = b3CreateWeldJoint( worldId, &jointDef ); + } + } + else + { + b3JointId jointId = context.joints[RandomInt( random, context.jointCount )]; + if ( b3Joint_IsValid( jointId ) ) + { + b3Joint_SetCollideConnected( jointId, RandomInt( random, 2 ) == 0 ); + } + } + } + break; + + case 9: + { + int staticIndex = RandomInt( random, STATIC_COUNT ); + b3Body_SetTransform( staticIds[staticIndex], RandomPosition( &context ), b3Quat_identity ); + } + break; + + case 10: + b3World_RebuildStaticTree( worldId ); + break; + + default: + break; + } + } + + // Joints whose bodies were destroyed are gone too + for ( int j = 0; j < context.jointCount; ) + { + if ( b3Joint_IsValid( context.joints[j] ) == false ) + { + context.joints[j] = context.joints[context.jointCount - 1]; + context.jointCount -= 1; + } + else + { + j += 1; + } + } + + // Keep the swarm inside the clutter + for ( int i = 0; i < MAX_BODIES; ++i ) + { + BodySlot* body = context.slots + i; + if ( body->active == false || b3Body_IsEnabled( body->bodyId ) == false ) + { + continue; + } + + b3Pos p = b3Body_GetPosition( body->bodyId ); + b3Vec3 d = b3SubPos( p, context.origin ); + if ( b3AbsFloat( d.x ) > 2.0f * context.extent || b3AbsFloat( d.y ) > 2.0f * context.extent || + b3AbsFloat( d.z ) > 2.0f * context.extent ) + { + b3Body_SetTransform( body->bodyId, RandomPosition( &context ), b3Quat_identity ); + b3Body_SetLinearVelocity( body->bodyId, RandomVelocity( &context ) ); + } + } + + b3World_Step( worldId, timeStep, 4 ); + + // A zero step finds pairs and purges disjoint contacts without moving anything + b3World_Step( worldId, 0.0f, 1 ); + + if ( CheckPairSet( worldId ) != 0 ) + { + printf( " workers=%d seed=%u sleep=%d step=%d\n", workerCount, seed, enableSleep, step ); + fflush( stdout ); + b3DestroyWorld( worldId ); + b3DestroyHeightField( grid ); + b3DestroyHeightField( tile ); + return 1; + } + } + + b3DestroyWorld( worldId ); + b3DestroyHeightField( grid ); + b3DestroyHeightField( tile ); + return 0; +} + +static int BroadPhaseStressTest( void ) +{ + uint32_t seeds[3] = { 12345u, 0xC0FFEEu, 777u }; + for ( int i = 0; i < 3; ++i ) + { + ENSURE( RunBroadPhaseStress( 1, seeds[i], false ) == 0 ); + ENSURE( RunBroadPhaseStress( 4, seeds[i], false ) == 0 ); + ENSURE( RunBroadPhaseStress( 3, seeds[i], true ) == 0 ); + } + + return 0; +} + +// The broad-phase keeps its pending pair marks inside the static tree, so a full rebuild between +// a static mutation and the next step used to wipe them and the contact never appeared. Covers +// both ways a static proxy gets marked: a teleport and a shape created with invokeContactCreation. +static int RunStaticRebuildKeepsMarks( bool useInvoke ) +{ + b3WorldDef worldDef = b3DefaultWorldDef(); + worldDef.gravity = b3Vec3_zero; + b3WorldId worldId = b3CreateWorld( &worldDef ); + + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.type = b3_dynamicBody; + bodyDef.position = (b3Pos){ 0.0, 1.0, 0.0 }; + b3BodyId sphereId = b3CreateBody( worldId, &bodyDef ); + b3ShapeDef shapeDef = b3DefaultShapeDef(); + b3Sphere sphere = { { 0.0f, 0.0f, 0.0f }, 0.5f }; + b3CreateSphereShape( sphereId, &shapeDef, &sphere ); + + bodyDef.type = b3_staticBody; + bodyDef.position = (b3Pos){ 100.0, 0.0, 0.0 }; + b3BodyId groundId = b3CreateBody( worldId, &bodyDef ); + b3BoxHull box = b3MakeBoxHull( 2.0f, 0.5f, 2.0f ); + if ( useInvoke == false ) + { + b3CreateHullShape( groundId, &shapeDef, &box.base ); + } + + // Settle so the resting sphere carries no mark of its own + for ( int i = 0; i < 3; ++i ) + { + b3World_Step( worldId, 1.0f / 60.0f, 4 ); + } + + b3Body_SetTransform( groundId, b3Pos_zero, b3Quat_identity ); + if ( useInvoke ) + { + b3ShapeDef invokeDef = b3DefaultShapeDef(); + invokeDef.invokeContactCreation = true; + b3CreateHullShape( groundId, &invokeDef, &box.base ); + } + + b3World_RebuildStaticTree( worldId ); + b3World_Step( worldId, 1.0f / 60.0f, 4 ); + + ENSURE( b3Body_GetContactCapacity( sphereId ) == 1 ); + + b3DestroyWorld( worldId ); + return 0; +} + +static int StaticRebuildKeepsMarks( void ) +{ + ENSURE( RunStaticRebuildKeepsMarks( false ) == 0 ); + ENSURE( RunStaticRebuildKeepsMarks( true ) == 0 ); + return 0; +} + +static int FlushingAssert( const char* condition, const char* fileName, int lineNumber ) +{ + printf( "BOX3D ASSERTION: %s, %s, line %d\n", condition, fileName, lineNumber ); + fflush( stdout ); + return 1; +} + +int BroadPhaseTest( void ) +{ + // A crash through a pipe loses buffered output, so flush the assertion first + b3SetAssertFcn( FlushingAssert ); + + RUN_SUBTEST( StaticRebuildKeepsMarks ); + RUN_SUBTEST( BroadPhaseStressTest ); + return 0; +} diff --git a/test/test_determinism.c b/test/test_determinism.c index 01fa0d5..9333e03 100644 --- a/test/test_determinism.c +++ b/test/test_determinism.c @@ -18,23 +18,23 @@ // Double precision accumulates body positions in double, so the settle/sleep step and the // state hash differ from the float build. Both modes are internally deterministic. #if defined( BOX3D_DOUBLE_PRECISION ) -#define RAGDOLL_SLEEP_STEP 272 -#define RAGDOLL_HASH 0xC049067A -#define WAVE_PILE_SLEEP_STEP 323 -#define WAVE_PILE_HASH 0x2200A547 +#define RAGDOLL_SLEEP_STEP 273 +#define RAGDOLL_HASH 0x0F4DD1EB +#define WAVE_PILE_SLEEP_STEP 314 +#define WAVE_PILE_HASH 0xD9B88124 #define QUERY_SPAWN_SLEEP_STEP 242 -#define QUERY_SPAWN_HASH 0x1737F5BC +#define QUERY_SPAWN_HASH 0x9B96A098 #define QUERY_SPAWN_HIT_COUNT 59 #define QUERY_SPAWN_QUERY_HASH 0x5B4429DC #define MESH_DROP_SLEEP_STEP 217 #define MESH_DROP_HASH 0xCF946437 #else -#define RAGDOLL_SLEEP_STEP 271 -#define RAGDOLL_HASH 0x805719D3 -#define WAVE_PILE_SLEEP_STEP 316 -#define WAVE_PILE_HASH 0xAA7D15A7 +#define RAGDOLL_SLEEP_STEP 274 +#define RAGDOLL_HASH 0x773AB8ED +#define WAVE_PILE_SLEEP_STEP 330 +#define WAVE_PILE_HASH 0x205D6055 #define QUERY_SPAWN_SLEEP_STEP 242 -#define QUERY_SPAWN_HASH 0xB9F993A5 +#define QUERY_SPAWN_HASH 0xF1EDEF47 #define QUERY_SPAWN_HIT_COUNT 59 #define QUERY_SPAWN_QUERY_HASH 0xE3271F3D #define MESH_DROP_SLEEP_STEP 217 diff --git a/test/test_joint.c b/test/test_joint.c index 8057a13..4e38d75 100644 --- a/test/test_joint.c +++ b/test/test_joint.c @@ -608,6 +608,65 @@ static int TestCreateJointClearsContacts( void ) return 0; } +// Issue #164: a joint with no dynamic body lives in the static set even when one body is an awake +// kinematic. Disabling and enabling that body has to respect this, whether the joint was created +// that way or got there through body type changes. +static int TestNonDynamicJointDisable( void ) +{ + b3WorldDef worldDef = b3DefaultWorldDef(); + worldDef.gravity = b3Vec3_zero; + b3WorldId worldId = b3CreateWorld( &worldDef ); + + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.type = b3_dynamicBody; + b3BodyId bodyIdA = b3CreateBody( worldId, &bodyDef ); + bodyDef.position = (b3Pos){ 2.0f, 0.0f, 0.0f }; + b3BodyId bodyIdB = b3CreateBody( worldId, &bodyDef ); + + b3ShapeDef shapeDef = b3DefaultShapeDef(); + b3BoxHull box = b3MakeCubeHull( 0.5f ); + b3CreateHullShape( bodyIdA, &shapeDef, &box.base ); + b3CreateHullShape( bodyIdB, &shapeDef, &box.base ); + + b3WeldJointDef jointDef = b3DefaultWeldJointDef(); + jointDef.base.bodyIdA = bodyIdA; + jointDef.base.bodyIdB = bodyIdB; + b3JointId weldId = b3CreateWeldJoint( worldId, &jointDef ); + + b3Body_SetType( bodyIdA, b3_staticBody ); + b3Body_SetType( bodyIdB, b3_kinematicBody ); + b3World_Step( worldId, 1.0f / 60.0f, 4 ); + + b3Body_Disable( bodyIdB ); + b3World_Step( worldId, 1.0f / 60.0f, 4 ); + b3Body_Enable( bodyIdB ); + b3World_Step( worldId, 1.0f / 60.0f, 4 ); + ENSURE( b3Joint_IsValid( weldId ) ); + + b3Body_SetType( bodyIdB, b3_dynamicBody ); + b3World_Step( worldId, 1.0f / 60.0f, 4 ); + ENSURE( b3Joint_IsAwake( weldId ) ); + + // Created directly between a static and a kinematic body + bodyDef.type = b3_kinematicBody; + bodyDef.position = (b3Pos){ 4.0f, 0.0f, 0.0f }; + b3BodyId bodyIdC = b3CreateBody( worldId, &bodyDef ); + b3CreateHullShape( bodyIdC, &shapeDef, &box.base ); + + b3RevoluteJointDef revoluteDef = b3DefaultRevoluteJointDef(); + revoluteDef.base.bodyIdA = bodyIdA; + revoluteDef.base.bodyIdB = bodyIdC; + b3JointId revoluteId = b3CreateRevoluteJoint( worldId, &revoluteDef ); + + b3Body_Disable( bodyIdC ); + b3Body_Enable( bodyIdC ); + b3World_Step( worldId, 1.0f / 60.0f, 4 ); + ENSURE( b3Joint_IsAwake( revoluteId ) == false ); + + b3DestroyWorld( worldId ); + return 0; +} + int JointTest( void ) { RUN_SUBTEST( TestParallelJoint ); @@ -620,6 +679,7 @@ int JointTest( void ) RUN_SUBTEST( TestWeldJoint ); RUN_SUBTEST( TestWheelJoint ); RUN_SUBTEST( TestCreateJointClearsContacts ); + RUN_SUBTEST( TestNonDynamicJointDisable ); return 0; } diff --git a/test/test_recording.c b/test/test_recording.c index 594d865..3c6274a 100644 --- a/test/test_recording.c +++ b/test/test_recording.c @@ -1028,8 +1028,7 @@ static float ReplaySafetyFactor( b3RecPlayer* player, const char* name ) } // Exercise every recorded op in a single session, then validate replay at two worker -// counts, round-trip through a file, and drive the incremental player. Mirrors the -// comprehensive RecordingTest in Box2D's test suite (box2d/test/test_recording.c). +// counts, round-trip through a file, and drive the incremental player. static int AllOps( void ) { b3Recording* rec = b3CreateRecording( 0 ); @@ -1440,6 +1439,8 @@ static int AllOps( void ) b3World_EnableWarmStarting( worldId, true ); b3World_EnableSpeculative( worldId, true ); b3World_SetRestitutionThreshold( worldId, 1.5f ); + b3World_SetRestitutionIterations( worldId, 3 ); + b3World_EnableRestitutionPropagation( worldId, true ); b3World_SetHitEventThreshold( worldId, 2.0f ); b3World_SetContactTuning( worldId, 30.0f, 10.0f, 3.0f ); b3World_SetContactRecycleDistance( worldId, 0.05f ); diff --git a/test/test_restitution.c b/test/test_restitution.c index 18aea9e..3c1b6b2 100644 --- a/test/test_restitution.c +++ b/test/test_restitution.c @@ -17,6 +17,10 @@ #define TIME_STEP ( 1.0f / 60.0f ) #define SUB_STEP_COUNT 4 +// A flat landing converges its Gauss-Seidel coupling in four stage iterations and the default is two, +// so the tests that read the coefficient or the spin of a flat landing pin the count +#define RESTITUTION_ITERATIONS 4 + // Impact speed shared by every scenario that measures a coefficient. Well above the default // restitution threshold so the bounce is always armed. #define IMPACT_SPEED 5.0f @@ -46,6 +50,15 @@ static b3WorldId MakeWorld( float gravityY ) return b3CreateWorld( &worldDef ); } +static b3WorldId MakeWorldIterations( float gravityY, int restitutionIterations ) +{ + b3WorldDef worldDef = b3DefaultWorldDef(); + worldDef.gravity = (b3Vec3){ 0.0f, gravityY, 0.0f }; + worldDef.enableSleep = false; + worldDef.restitutionIterations = restitutionIterations; + return b3CreateWorld( &worldDef ); +} + // Ground with its top surface at y = 0 static void MakeGround( b3WorldId worldId, float restitution ) { @@ -142,9 +155,14 @@ static float MeasureGroundBounce( float restitution, float gap ) // Impactor driven onto a column of resting balls pinned against the ground. The support balls are // dead so only the top contact can bounce. A supported target has infinite effective mass along the // normal, so the coefficient must not depend on the column height. -static float MeasureSupportedBounce( float restitution, int supportCount ) +static float MeasureSupportedBounce( float restitution, int supportCount, bool propagate ) { - b3WorldId worldId = MakeWorld( 0.0f ); + b3WorldDef worldDef = b3DefaultWorldDef(); + worldDef.gravity = b3Vec3_zero; + worldDef.enableSleep = false; + worldDef.enableRestitutionPropagation = propagate; + worldDef.restitutionIterations = RESTITUTION_ITERATIONS; + b3WorldId worldId = b3CreateWorld( &worldDef ); MakeGround( worldId, 0.0f ); for ( int i = 0; i < supportCount; ++i ) @@ -168,7 +186,7 @@ static float MeasureSupportedBounce( float restitution, int supportCount ) // Flat box landing on all four corners at once with no gravity static float MeasureFlatBounce( float restitution, int subStepCount, float* spin ) { - b3WorldId worldId = MakeWorld( 0.0f ); + b3WorldId worldId = MakeWorldIterations( 0.0f, RESTITUTION_ITERATIONS ); MakeGround( worldId, 0.0f ); b3BodyDef bodyDef = b3DefaultBodyDef(); @@ -307,32 +325,35 @@ static int PhaseTest( void ) return failed; } -// The bounce is solved as a constraint alongside the support contacts, so the column can supply the -// reaction. A terminal restitution pass has nothing after it to do that and the coefficient decays -// with the column height, which the tolerance is chosen to reject. +// By default the restitution stage sweeps only the contacts that carry restitution, so the ball +// pushes the column down and the ground under it gets no say until the next step: e = 0.9 gives +// about 0.52 on one support. With propagation enabled the stage sweeps every contact, the ground +// contact supplies the reaction within the stage and e = 0.9 gives 0.85 on one support and 0.72 on +// three at the pinned four iterations, decaying with depth because the column needs an iteration per +// level. Propagation costs a normal pass over every contact per iteration, hence the option. Both +// modes are gated: never above the free coefficient, which would be energy from nowhere, and never +// collapsed. static int SupportedTest( void ) { static const float restitutions[] = { 0.5f, 0.9f }; - const float tolerance = 0.2f; int failed = 0; - for ( int j = 0; j < ARRAY_COUNT( restitutions ); ++j ) + for ( int propagate = 0; propagate <= 1; ++propagate ) { - for ( int n = 0; n <= 3; ++n ) + float floor = propagate ? 0.75f : 0.5f; + + for ( int j = 0; j < ARRAY_COUNT( restitutions ); ++j ) { - float measured = MeasureSupportedBounce( restitutions[j], n ); - printf( " supported e %.2f supports %d -> %.4f\n", restitutions[j], n, measured ); - - if ( b3AbsFloat( measured - restitutions[j] ) > tolerance ) + for ( int n = 0; n <= 3; ++n ) { - failed = 1; - } + float measured = MeasureSupportedBounce( restitutions[j], n, propagate == 1 ); + printf( " supported e %.2f supports %d propagation %d -> %.4f\n", restitutions[j], n, propagate, measured ); - // Rebounding faster than the impact is energy from nowhere, whatever the coefficient - if ( measured > 1.01f ) - { - failed = 1; + if ( measured > restitutions[j] + 0.01f || measured < floor * restitutions[j] ) + { + failed = 1; + } } } } @@ -340,15 +361,16 @@ static int SupportedTest( void ) return failed; } -// A symmetric four point landing. The points are solved in sequence within a relax pass and the -// bounce retires once they all separate, so a small residual spin is expected. The tolerance admits -// that residual and rejects the gross asymmetry of one point taking the whole bounce. +// A symmetric four point landing. The stage solves the points in sequence, so a single sweep leaves +// the box spinning: the first impulse tilts it and the next point sees a larger closing speed. Four +// stage iterations converge the coupling and leave a small residual. The tolerance admits that +// residual and rejects the single sweep behavior. static int FlatLandingTest( void ) { static const float restitutions[] = { 0.5f, 0.9f }; static const int subStepCounts[] = { 4, 8 }; - const float speedTolerance = 0.1f; - const float spinTolerance = 0.25f; + const float speedTolerance = 0.01f; + const float spinTolerance = 0.03f; int failed = 0; @@ -379,8 +401,7 @@ static int FlatLandingTest( void ) // A flat box carrying spin about a horizontal axis. All four corners stay in contact and the normal // constraints are linear in the body velocity, so perfect restitution reverses linear and angular -// velocity. Sweeping the normal solve once per manifold point makes it exact to float precision. One -// sweep left up to 0.03 of residual in each. +// velocity. Four stage iterations bring both within a few thousandths. static int SpinTest( void ) { static const float spins[] = { 0.0f, 0.5f, 1.0f, 2.0f }; @@ -389,7 +410,7 @@ static int SpinTest( void ) for ( int j = 0; j < ARRAY_COUNT( spins ); ++j ) { - b3WorldId worldId = MakeWorld( 0.0f ); + b3WorldId worldId = MakeWorldIterations( 0.0f, RESTITUTION_ITERATIONS ); MakeGround( worldId, 0.0f ); b3BodyDef bodyDef = b3DefaultBodyDef(); @@ -418,7 +439,7 @@ static int SpinTest( void ) printf( " spin in %+.2f -> vy %+.4f (want %+.4f) wz %+.4f (want %+.4f)\n", spins[j], speed, IMPACT_SPEED, spin, -spins[j] ); - if ( b3AbsFloat( speed - IMPACT_SPEED ) > 0.25f || b3AbsFloat( spin + spins[j] ) > 0.25f ) + if ( b3AbsFloat( speed - IMPACT_SPEED ) > 0.05f || b3AbsFloat( spin + spins[j] ) > 0.05f ) { failed = 1; } @@ -430,10 +451,10 @@ static int SpinTest( void ) // Perfectly elastic ball under gravity. Only heights where continuous collision engages are used. // Continuous collision lands the ball on the surface, so the bounce is armed from the true impact // speed and the apex holds. Slower drops resolve the impact inside the overlap and the penetration -// recovery adds height, by design. +// recovery adds height, by design. The 10 m case is the long horizon read. static int DropTest( void ) { - static const float heights[] = { 40.0f, 20.0f }; + static const float heights[] = { 40.0f, 20.0f, 10.0f }; int failed = 0; @@ -461,7 +482,8 @@ static int DropTest( void ) highest = b3MaxFloat( highest, apexes[i] ); } - if ( highest > 1.02f * apexes[0] || apexes[apexCount - 1] < 0.8f * apexes[0] ) + // A later apex above the first, or a first apex above the drop, is energy from nowhere + if ( highest > 1.001f * apexes[0] || apexes[0] > 1.001f * heights[j] || apexes[apexCount - 1] < 0.8f * apexes[0] ) { failed = 1; } @@ -471,21 +493,14 @@ static int DropTest( void ) } // A cube dropped flat with an aggressive continuous safety factor so it lands square on the surface. -// Perfectly elastic and frictionless, so it should come back to the drop height with no rotation and -// keep doing that. It now does: the four apexes sit within a centimetre of each other and the spin -// after the landing is zero to float precision. -// -// This used to be the worst subtest in the file and its history is worth keeping. The four points of -// a flat landing are solved in sequence, so the first impulse tilts the cube, the next point sees a -// larger closing speed and delivers more, and the sum overshot the rigid body answer. One sweep gave -// 0.89 rad/s of spin and a 6 percent high first apex, after which the cube tumbled and either shed -// almost all its energy (the deferred scheme collapsed to 0.71, the height of a cube rolling on its -// edges) or gained it. The normal solve is now swept once per manifold point when a bounce is armed, -// which converges the coupling: spin fell to 0.13 rad/s at two sweeps and to zero at three. The -// bounds below reject every one of those earlier behaviors. +// Perfectly elastic and frictionless, so it should come back to the drop height with no rotation. A +// flat landing solves four coplanar points in sequence and one sweep over-delivers: the first impulse +// tilts the cube, the next point sees a larger closing speed and the sum overshoots the rigid body +// answer, leaving spin that tilts the cube for the next landing. Four stage iterations converge the +// coupling. The bounds below reject the single sweep behavior and any apex above the drop. static int SingleBoxTest( void ) { - b3WorldId worldId = MakeWorld( -10.0f ); + b3WorldId worldId = MakeWorldIterations( -10.0f, RESTITUTION_ITERATIONS ); MakeGround( worldId, 0.0f ); b3ShapeDef shapeDef = b3DefaultShapeDef(); @@ -543,14 +558,14 @@ static int SingleBoxTest( void ) int failed = 0; - if ( firstSpin > 0.1f ) + if ( firstSpin > 0.01f ) { failed = 1; } for ( int i = 0; i < apexCount; ++i ) { - if ( apexes[i] < 0.95f * dropHeight || apexes[i] > 1.02f * dropHeight ) + if ( apexes[i] < 0.95f * dropHeight || apexes[i] > 1.001f * dropHeight ) { failed = 1; } @@ -680,7 +695,6 @@ static uint64_t RunWorkerScene( int workerCount ) return hash; } -// The armed bounce is per manifold point state, so it must survive the split across workers // The manifold point approach speed is published only for contacts that opted into hit events. It // is not a stale value otherwise, it is zero, so a reader can tell "not measured" from "measured // zero" by whether the shape enables the events. Restitution no longer reads it at all, so this is @@ -745,33 +759,27 @@ static int NormalVelocityTest( void ) return failed; } -// Mirrors the Restitution Overshoot sample in samples/sample_issues.cpp: a unit cube dropped flat on -// a floor narrower than itself, perfectly elastic, so the four contact points sit inboard of the box -// corners. A perfectly elastic bounce cannot come back higher than it was dropped from. -// -// Run twice, because two different effects used to be tangled together here. The impulse itself must -// never return more speed than it received, which is the restitution invariant and is checked in both -// cases. The apex is a weaker statement: without continuous collision the cube moves 0.23 m in the -// step before contact, just under the 0.25 m that would trigger it, so the first manifold appears -// with the cube already 0.10 m deep. It then bounces from down there and the soft position correction -// lifts it back to the surface for free, which is worth about a tenth of a metre of extra height and -// has nothing to do with restitution. With continuous collision the cube lands on the surface and the -// apex comes in under the drop height. -// Total mechanical energy of a body. Perfect restitution and no friction means this may only -// decrease, which is a stronger statement than any height bound: it also catches a bounce that -// manufactures spin rather than height. +// Mechanical energy of a body as symplectic Euler conserves it. The integrator updates velocity +// before position, so the quantity that is exactly constant in free flight is not kinetic plus +// potential but that plus half a sub-step of the gravity power, m * dot(v, g) * h / 2. Plain +// kinetic plus potential sampled at step boundaries jumps by about g * v * h at every bounce and +// drifts down by g^2 h^2 / 2 each sub-step, which would hide a real gain or fake one. With this +// measure a contact that never does positive work leaves the energy flat to round-off. static float MeasureEnergy( b3BodyId bodyId, b3WorldId worldId ) { b3MassData massData = b3Body_GetMassData( bodyId ); b3Vec3 v = b3Body_GetLinearVelocity( bodyId ); + b3Vec3 gravity = b3MulSV( b3Body_GetGravityScale( bodyId ), b3World_GetGravity( worldId ) ); + float h = TIME_STEP / SUB_STEP_COUNT; // The inertia tensor is in the body frame, so bring the angular velocity back to it b3Vec3 wLocal = b3InvRotateVector( b3Body_GetRotation( bodyId ), b3Body_GetAngularVelocity( bodyId ) ); float kinetic = 0.5f * massData.mass * b3Dot( v, v ) + 0.5f * b3Dot( wLocal, b3MulMV( massData.inertia, wLocal ) ); - float potential = -massData.mass * b3Dot( b3World_GetGravity( worldId ), b3ToVec3( b3Body_GetWorldCenter( bodyId ) ) ); + float potential = -massData.mass * b3Dot( gravity, b3ToVec3( b3Body_GetWorldCenter( bodyId ) ) ); + float phase = 0.5f * h * massData.mass * b3Dot( v, gravity ); - return kinetic + potential; + return kinetic + potential + phase; } typedef struct OvershootResult @@ -789,7 +797,7 @@ typedef struct OvershootResult // gain energy. static OvershootResult MeasureOvershoot( bool continuous ) { - b3WorldId worldId = MakeWorld( -10.0f ); + b3WorldId worldId = MakeWorldIterations( -10.0f, RESTITUTION_ITERATIONS ); b3BodyDef floorDef = b3DefaultBodyDef(); floorDef.position = (b3Pos){ 0.0f, -0.25f, 0.0f }; @@ -901,9 +909,13 @@ static int OvershootTest( void ) OvershootResult discrete = MeasureOvershoot( false ); OvershootResult continuous = MeasureOvershoot( true ); + // Perfectly elastic returns exactly the speed it received, so any excess is round-off. The + // overflow and colored solvers round differently and either can land an ulp over one. + const float speedTolerance = 1.0e-5f; + int failed = 0; - if ( discrete.speedRatio > 1.0f || continuous.speedRatio > 1.0f ) + if ( discrete.speedRatio > 1.0f + speedTolerance || continuous.speedRatio > 1.0f + speedTolerance ) { failed = 1; } @@ -918,7 +930,10 @@ static int OvershootTest( void ) failed = 1; } - if ( discrete.bounceCount < 2 || continuous.bounceCount < 2 ) + // The continuous landing leaves the cube sliding sideways a few tenths of a metre per second, from + // friction during the sub-steps, and a floor narrower than the cube does not catch it on the way + // back down. That is sub-step friction, not restitution, so only the first bounce is required. + if ( discrete.bounceCount < 2 || continuous.bounceCount < 1 ) { failed = 1; } @@ -981,7 +996,7 @@ static ImpulseResult MeasureDropImpulse( float restitution, float dropHeight ) } } - // The sim flag is the one written this step, the body flag lags a step behind + // Transient body flags are cleared and rewritten every step b3Body* ball = b3GetBodyFullId( world, ballId ); b3BodySim* ballSim = b3GetBodySim( world, ball ); if ( ballSim->flags & b3_hadTimeOfImpact ) @@ -1029,8 +1044,8 @@ static ImpulseResult MeasureDropImpulse( float restitution, float dropHeight ) // // The step balance is the real gate. The bounce total is bracketed as well so the restitution sweep // means something: the contact has to reverse the approach at the coefficient and may carry the -// weight for at most as long as it lasted. The bounce retires inside the step once the point -// separates, so the ball can lose up to a step of gravity below the reversal. +// weight for at most as long as it lasted. The stage runs after the sub-steps, so the reversal +// lands a step late and the ball can come in up to a step of gravity under it. static int ImpulseTest( void ) { static const float restitutions[] = { 0.0f, 0.25f, 0.5f, 0.75f, 1.0f }; @@ -1099,6 +1114,157 @@ static int ImpulseTest( void ) return failed; } +typedef struct EnergyScene +{ + const char* name; + float safetyFactor; + float friction; + float angle; + float spin; + float height; + float velocityY; + float halfWidth; + float halfHeight; + bool bullet; + bool stack; + int stepCount; +} EnergyScene; + +// Peak energy over the run relative to the start, for every dynamic body in the scene. Perfectly +// elastic and the energy may still only go down: friction and the soft contact position correction +// dissipate, and a bounce may return at most what the contact absorbed. +static float MeasurePeakEnergy( const EnergyScene* scene ) +{ + b3WorldId worldId = MakeWorld( -10.0f ); + + b3BodyDef groundDef = b3DefaultBodyDef(); + groundDef.position = (b3Pos){ 0.0f, -1.0f, 0.0f }; + b3BodyId groundId = b3CreateBody( worldId, &groundDef ); + b3ShapeDef groundShape = b3DefaultShapeDef(); + groundShape.baseMaterial.friction = scene->friction; + b3BoxHull ground = b3MakeBoxHull( 40.0f, 1.0f, 40.0f ); + b3CreateHullShape( groundId, &groundShape, &ground.base ); + + b3ShapeDef shapeDef = b3DefaultShapeDef(); + shapeDef.density = 1.0f; + shapeDef.baseMaterial.friction = scene->friction; + shapeDef.baseMaterial.restitution = 1.0f; + b3BoxHull box = b3MakeBoxHull( scene->halfWidth, scene->halfHeight, scene->halfWidth ); + + b3BodyId bodyIds[4]; + int bodyCount = 0; + + if ( scene->stack ) + { + // Resting box under the dropped one, like the TwoBoxRestitution sample + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.type = b3_dynamicBody; + bodyDef.position = (b3Pos){ 0.0f, scene->halfHeight, 0.0f }; + bodyDef.safetyFactor = scene->safetyFactor; + bodyIds[bodyCount] = b3CreateBody( worldId, &bodyDef ); + b3CreateHullShape( bodyIds[bodyCount], &shapeDef, &box.base ); + bodyCount += 1; + } + + { + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.type = b3_dynamicBody; + bodyDef.position = (b3Pos){ 0.0f, scene->height, 0.0f }; + bodyDef.rotation = b3MakeQuatFromAxisAngle( b3Vec3_axisZ, scene->angle ); + bodyDef.angularVelocity = (b3Vec3){ 0.0f, 0.0f, scene->spin }; + bodyDef.linearVelocity = (b3Vec3){ 0.0f, scene->velocityY, 0.0f }; + bodyDef.safetyFactor = scene->safetyFactor; + bodyDef.isBullet = scene->bullet; + bodyIds[bodyCount] = b3CreateBody( worldId, &bodyDef ); + b3CreateHullShape( bodyIds[bodyCount], &shapeDef, &box.base ); + bodyCount += 1; + } + + float start = 0.0f; + for ( int k = 0; k < bodyCount; ++k ) + { + start += MeasureEnergy( bodyIds[k], worldId ); + } + + float peak = start; + int peakStep = -1; + + for ( int i = 0; i < scene->stepCount; ++i ) + { + b3World_Step( worldId, TIME_STEP, SUB_STEP_COUNT ); + + float energy = 0.0f; + for ( int k = 0; k < bodyCount; ++k ) + { + energy += MeasureEnergy( bodyIds[k], worldId ); + } + + if ( energy > peak ) + { + peak = energy; + peakStep = i; + } + } + + b3DestroyWorld( worldId ); + + float ratio = peak / start; + printf( " energy %-24s peak %.5f at step %d\n", scene->name, ratio, peakStep ); + return ratio; +} + +// Perfectly elastic material with friction, dropped every way that used to gain energy: a spinning +// edge landing, tumbling, a flat landing with spin, a box falling on a resting box, and a tall box +// landing flat on its narrow end. Energy must never rise above the start. This is the load bearing +// property of restitution. Accuracy of the coefficient and freedom from spin are secondary. +// +// The tall boxes guard the stage iteration count. Their contact points sit close together while the +// inertia is long, so the off diagonal of the multi point normal mass is large and one sweep +// overshoots. +static int EnergyTest( void ) +{ + static const EnergyScene scenes[] = { + { "spinning edge", 0.01f, 0.6f, -1.43f, 7.9f, 0.7f, -1.9f, 0.5f, 0.5f, false, false, 300 }, + { "spinning edge fast", 0.01f, 0.6f, -0.8f, 12.0f, 3.0f, -8.0f, 0.5f, 0.5f, false, false, 300 }, + { "tumble", 0.01f, 0.6f, -0.3f, 3.0f, 8.0f, 0.0f, 0.5f, 0.5f, false, false, 600 }, + { "tumble discrete", 0.5f, 0.6f, -0.3f, 3.0f, 8.0f, 0.0f, 0.5f, 0.5f, false, false, 600 }, + { "flat with spin", 0.01f, 0.6f, 0.0f, 2.0f, 2.0f, -5.0f, 0.5f, 0.5f, false, false, 300 }, + { "flat frictionless", 0.01f, 0.0f, 0.0f, 0.0f, 10.0f, 0.0f, 0.5f, 0.5f, false, false, 1200 }, + { "edge", 0.01f, 0.6f, -0.2f, 0.0f, 5.0f, 0.0f, 0.5f, 0.5f, false, false, 600 }, + { "two box", 0.02f, 0.6f, 0.0f, 0.0f, 4.0f, 0.0f, 0.5f, 0.5f, true, true, 1500 }, + { "two box discrete", 0.5f, 0.6f, 0.0f, 0.0f, 4.0f, 0.0f, 0.5f, 0.5f, false, true, 1500 }, + { "tall flat frictionless", 0.01f, 0.0f, 0.0f, 0.0f, 10.0f, 0.0f, 0.5f, 1.0f, false, false, 1200 }, + { "tall flat friction", 0.01f, 0.6f, 0.0f, 0.0f, 10.0f, 0.0f, 0.5f, 1.0f, false, false, 1200 }, + { "narrow flat frictionless", 0.01f, 0.0f, 0.0f, 0.0f, 10.0f, 0.0f, 0.25f, 1.0f, false, false, 1200 }, + { "narrow flat friction", 0.01f, 0.6f, 0.0f, 0.0f, 10.0f, 0.0f, 0.25f, 1.0f, false, false, 1200 }, + }; + + // The soft contact position correction lifts a body that lands inside the ground, which is + // potential energy from nowhere at any restitution. Continuous collision keeps landings on the + // surface, so the tolerance only has to cover the stage. A four point flat landing does not + // converge at the default two stage iterations the way the two point landing does in 2D: the cube + // comes back with 0.2% extra. The in-relax scheme this replaced gained 2.5% to 29% on these scenes, + // so the tolerance still separates the two. + const float tolerance = 5.0e-3f; + const float discreteTolerance = 2.0e-2f; + + int failed = 0; + + for ( int i = 0; i < ARRAY_COUNT( scenes ); ++i ) + { + float ratio = MeasurePeakEnergy( scenes + i ); + float limit = scenes[i].safetyFactor < 0.1f ? tolerance : discreteTolerance; + if ( ratio > 1.0f + limit ) + { + failed = 1; + } + } + + return failed; +} + +// The sampled approach speed and the Poisson accumulator are per manifold point state, so they must +// survive the split across workers static int WorkerParityTest( void ) { uint64_t hash1 = RunWorkerScene( 1 ); @@ -1128,6 +1294,7 @@ int RestitutionTest( void ) RUN_MEASUREMENT( RestingTest ); RUN_MEASUREMENT( NormalVelocityTest ); RUN_MEASUREMENT( OvershootTest ); + RUN_MEASUREMENT( EnergyTest ); RUN_MEASUREMENT( ImpulseTest ); RUN_MEASUREMENT( WorkerParityTest );