Optimizations (#168)
- Optimize contact prepare - Back to post restitution stage - Optimize solver math - Bug fixes
This commit is contained in:
parent
9e5a4cde86
commit
79fc55976a
55 changed files with 3248 additions and 1519 deletions
12
.github/workflows/build.yml
vendored
12
.github/workflows/build.yml
vendored
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
|
@ -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
|
||||
|
|
|
|||
|
|
|
@ -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
|
||||
|
|
|
|||
|
|
|
@ -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
|
||||
|
|
|
|||
|
|
|
@ -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
|
||||
|
|
|
|||
|
|
|
@ -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
|
||||
|
|
|
|||
|
|
|
@ -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
|
||||
|
|
|
|||
|
|
|
@ -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
|
||||
|
|
|
|||
|
|
|
@ -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
|
||||
|
|
|
|||
|
|
|
@ -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
|
||||
|
|
|
|||
|
|
|
@ -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
|
||||
|
|
|
|||
|
|
|
@ -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
|
||||
|
|
|
|||
|
|
|
@ -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
|
||||
|
|
|
|||
|
|
|
@ -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,
|
||||
|
|
|
|||
|
|
@ -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 );
|
||||
|
||||
|
|
|
|||
|
|
@ -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.
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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 );
|
||||
|
|
|
|||
|
|
@ -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" ) )
|
||||
{
|
||||
|
|
|
|||
|
|
@ -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.
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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 );
|
||||
|
|
|
|||
1219
samples/sample_restitution.cpp
Normal file
1219
samples/sample_restitution.cpp
Normal file
File diff suppressed because it is too large
Load diff
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
||||
|
|
|
|||
|
|
@ -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 );
|
||||
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
}
|
||||
|
|
|
|||
31
src/body.h
31
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.
|
||||
|
|
|
|||
1352
src/contact_solver.c
1352
src/contact_solver.c
File diff suppressed because it is too large
Load diff
|
|
@ -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 );
|
||||
|
|
|
|||
|
|
@ -19,7 +19,7 @@
|
|||
#include <stdarg.h>
|
||||
#include <string.h>
|
||||
|
||||
#ifdef BOX2D_PROFILE
|
||||
#ifdef BOX3D_PROFILE
|
||||
|
||||
#include <tracy/TracyC.h>
|
||||
#define b3TracyCAlloc( ptr, size ) TracyCAlloc( ptr, size )
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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 );
|
||||
|
|
|
|||
|
|
@ -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 );
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
|
|
|||
|
|
@ -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.
|
||||
|
|
|
|||
|
|
@ -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 ) )
|
||||
|
|
|
|||
|
|
@ -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 );
|
||||
|
|
|
|||
|
|
@ -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 );
|
||||
}
|
||||
|
||||
|
|
|
|||
58
src/simd.h
58
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
|
||||
|
|
|
|||
51
src/solver.c
51
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 );
|
||||
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
|
|
|||
|
|
@ -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 );
|
||||
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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 );
|
||||
|
|
|
|||
|
|
@ -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 )
|
||||
|
|
|
|||
488
test/test_broad_phase.c
Normal file
488
test/test_broad_phase.c
Normal file
|
|
@ -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 <stdio.h>
|
||||
|
||||
// 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;
|
||||
}
|
||||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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 );
|
||||
|
|
|
|||
|
|
@ -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 );
|
||||
|
||||
|
|
|
|||
Loading…
Reference in a new issue