Optimizations (#161)

- cache body sim indices and split out fat aabb
- deferred restitution
- the manifold point normal velocity is only computed for restitution or
hit events, otherwise it is zero
This commit is contained in:
Erin Catto 2026-09-20 15:54:52 -07:00 • committed by GitHub
commit 9e5a4cde86
No known key found for this signature in database
GPG key ID: B5690EEEBB952194
49 changed files with 3395 additions and 864 deletions

View file

@ -1,9 +1,9 @@
threads,ms
1,14441.9
2,7238.43
3,4870.13
4,3697.34
5,2998.67
6,2533.13
7,2209.12
8,1983.89
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 threads ms
2 1 14441.9 13564.7
3 2 7238.43 7058.44
4 3 4870.13 4749.26
5 4 3697.34 3596.92
6 5 2998.67 2921.35
7 6 2533.13 2478.9
8 7 2209.12 2163.02
9 8 1983.89 1919.1

View file

@ -1,9 +1,9 @@
threads,ms
1,1323.84
2,691.063
3,463.698
4,353.723
5,284.09
6,240.864
7,209.11
8,185.953
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 threads ms
2 1 1323.84 1341.05
3 2 691.063 697.405
4 3 463.698 467.98
5 4 353.723 355.631
6 5 284.09 285.912
7 6 240.864 240.761
8 7 209.11 210.048
9 8 185.953 187.1

View file

@ -1,9 +1,9 @@
threads,ms
1,16467.5
2,8687.63
3,5966.06
4,4569.55
5,3727.89
6,3175.51
7,2801.72
8,2512.32
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 threads ms
2 1 16467.5 14783.1
3 2 8687.63 7859.85
4 3 5966.06 5367.48
5 4 4569.55 4115.03
6 5 3727.89 3368.76
7 6 3175.51 2874.23
8 7 2801.72 2535.89
9 8 2512.32 2277.2

View file

@ -1,9 +1,9 @@
threads,ms
1,1993.6
2,1049.01
3,712.917
4,544.335
5,443.056
6,384.22
7,331.626
8,297.982
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 threads ms
2 1 1993.6 1852.7
3 2 1049.01 978.38
4 3 712.917 665.353
5 4 544.335 505.116
6 5 443.056 411.56
7 6 384.22 356.776
8 7 331.626 307.866
9 8 297.982 276.957

View file

@ -1,9 +1,9 @@
threads,ms
1,7.6329
2,9.7115
3,10.7648
4,11.2398
5,12.029
6,12.1204
7,12.2289
8,12.5125
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 threads ms
2 1 7.6329 7.1447
3 2 9.7115 9.0736
4 3 10.7648 10.1672
5 4 11.2398 10.5088
6 5 12.029 11.0298
7 6 12.1204 11.2858
8 7 12.2289 11.378
9 8 12.5125 11.4947

View file

@ -1,9 +1,9 @@
threads,ms
1,2075.59
2,1084.54
3,722.057
4,549.644
5,437.772
6,372.242
7,319.13
8,283.131
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 threads ms
2 1 2075.59 1947.41
3 2 1084.54 1002.95
4 3 722.057 663.846
5 4 549.644 499.942
6 5 437.772 405.97
7 6 372.242 336.977
8 7 319.13 291.483
9 8 283.131 255.782

View file

@ -1,9 +1,9 @@
threads,ms
1,2102.47
2,1147.85
3,803.547
4,630.96
5,524.168
6,452.1
7,404.499
8,370.325
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 threads ms
2 1 2102.47 2061.72
3 2 1147.85 1139.07
4 3 803.547 794.05
5 4 630.96 622.555
6 5 524.168 516.441
7 6 452.1 446.451
8 7 404.499 399.084
9 8 370.325 364.345

View file

@ -1,9 +1,9 @@
threads,ms
1,1144.18
2,613.407
3,423.032
4,329.512
5,272.901
6,239.65
7,216.5
8,200.217
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 threads ms
2 1 1144.18 1051.05
3 2 613.407 568.185
4 3 423.032 390.055
5 4 329.512 304.593
6 5 272.901 251.978
7 6 239.65 221.25
8 7 216.5 200.171
9 8 200.217 185.485

View file

@ -1,9 +1,9 @@
threads,ms
1,3173.87
2,1841.25
3,1325.07
4,1061.04
5,902.193
6,803.715
7,734.266
8,689.781
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 threads ms
2 1 3173.87 3152.3
3 2 1841.25 1832.83
4 3 1325.07 1315.75
5 4 1061.04 1058.49
6 5 902.193 903.367
7 6 803.715 801.254
8 7 734.266 738.93
9 8 689.781 695.326

View file

@ -1,9 +1,9 @@
threads,ms
1,170.276
2,112.037
3,88.2708
4,78.4451
5,74.3577
6,71.868
7,70.6687
8,69.1284
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 threads ms
2 1 170.276 170.933
3 2 112.037 112.916
4 3 88.2708 89.6041
5 4 78.4451 79.6461
6 5 74.3577 75.7754
7 6 71.868 73.2911
8 7 70.6687 71.7742
9 8 69.1284 70.7292

View file

@ -1,9 +1,9 @@
threads,ms
1,673.356
2,395.07
3,291.336
4,249.378
5,225.709
6,211.1
7,201.452
8,194.379
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 threads ms
2 1 673.356 680.981
3 2 395.07 400.129
4 3 291.336 296.389
5 4 249.378 252.676
6 5 225.709 229.879
7 6 211.1 214.117
8 7 201.452 204.083
9 8 194.379 196.839

View file

@ -1,9 +1,9 @@
threads,ms
1,285.24
2,174.635
3,132.161
4,116.509
5,107.948
6,102.503
7,98.9163
8,96.5196
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 threads ms
2 1 285.24 295.508
3 2 174.635 183.122
4 3 132.161 139.634
5 4 116.509 121.466
6 5 107.948 113.262
7 6 102.503 106.697
8 7 98.9163 103.854
9 8 96.5196 101.959

View file

@ -1,9 +1,9 @@
threads,ms
1,21430.4
2,11407.7
3,7908.09
4,6102.55
5,5012.68
6,4272.59
7,3783.33
8,3415.5
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 threads ms
2 1 21430.4 19931.1
3 2 11407.7 10626.7
4 3 7908.09 7300.85
5 4 6102.55 5627.16
6 5 5012.68 4603.18
7 6 4272.59 3931.89
8 7 3783.33 3454.58
9 8 3415.5 3273.54

View file

@ -188,7 +188,6 @@ int main( int argc, char** argv )
.solveImpulses = FLT_MAX,
.integratePositions = FLT_MAX,
.relaxImpulses = FLT_MAX,
.applyRestitution = FLT_MAX,
.storeImpulses = FLT_MAX,
.splitIslands = FLT_MAX,
.transforms = FLT_MAX,

1
data/hulls/axe.txt Normal file

File diff suppressed because one or more lines are too long

View file

@ -4,6 +4,7 @@
#pragma once
#include <stdbool.h>
#include <stddef.h>
#include <stdint.h>
// Compile-time options. Edit box3d/config.h, or define BOX3D_USER_CONFIG to
@ -93,11 +94,12 @@
/// Prototype for user allocation function.
/// @param size the allocation size in bytes
/// @param alignment the required alignment, guaranteed to be a power of 2
typedef void* b3AllocFcn( int32_t size, int32_t alignment );
typedef void* b3AllocFcn( size_t size, int32_t alignment );
/// Prototype for user free function.
/// @param mem the memory previously allocated through `b3AllocFcn`
typedef void b3FreeFcn( void* mem );
/// @param size the size of the allocation in bytes.
typedef void b3FreeFcn( void* mem, size_t size );
/// Prototype for the user assert callback. Return 0 to skip the debugger break.
typedef int b3AssertFcn( const char* condition, const char* fileName, int lineNumber );
@ -110,7 +112,7 @@ typedef void b3LogFcn( const char* message );
B3_API void b3SetAllocator( b3AllocFcn* allocFcn, b3FreeFcn* freeFcn );
/// Total bytes allocated by Box3D
B3_API int b3GetByteCount( void );
B3_API int64_t b3GetByteCount( void );
/// Override the default assert callback.
/// @param assertFcn a non-null assert callback

View file

@ -31,5 +31,3 @@
// Enable assertions when NDEBUG is defined
// #define B3_ENABLE_ASSERT
// #define B3_RESTITUTION_ITERATIONS

View file

@ -141,11 +141,6 @@ B3_API float b3GetStallThreshold( void );
#define B3_MAX_SHAPES ( 1 << B3_SHAPE_POWER )
#define B3_MAX_CHILD_SHAPES ( 1 << B3_CHILD_POWER )
/// Increase this if your application needs more accurate restitution. Doing so will
/// slow down the simulation. Must be 1 or more.
#ifndef B3_RESTITUTION_ITERATIONS
#define B3_RESTITUTION_ITERATIONS 1
#endif
/// 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

View file

@ -537,69 +537,70 @@ typedef struct b3ShapeDef
/// @ingroup shape
B3_API b3ShapeDef b3DefaultShapeDef( void );
//! @cond
/// Profiling data. Times are in milliseconds.
/// @ingroup world
typedef struct b3Profile
{
float step;
float pairs;
float collide;
float solve;
float solverSetup;
float constraints;
float prepareConstraints;
float integrateVelocities;
float warmStart;
float solveImpulses;
float integratePositions;
float relaxImpulses;
float applyRestitution;
float storeImpulses;
float splitIslands;
float transforms;
float sensorHits;
float jointEvents;
float hitEvents;
float refit;
float bullets;
float sleepIslands;
float sensors;
float step; //
float pairs; //
float collide; //
float solve; //
float solverSetup; //
float constraints; //
float prepareConstraints; //
float integrateVelocities; //
float warmStart; //
float solveImpulses; //
float integratePositions; //
float relaxImpulses; //
float storeImpulses; //
float splitIslands; //
float transforms; //
float sensorHits; //
float jointEvents; //
float hitEvents; //
float refit; //
float bullets; //
float sleepIslands; //
float sensors; //
} b3Profile;
/// Counters that give details of the simulation size.
/// @ingroup world
typedef struct b3Counters
{
int bodyCount;
int shapeCount;
int contactCount;
int jointCount;
int islandCount;
int stackUsed;
int arenaCapacity;
int staticTreeHeight;
int treeHeight;
int satCallCount;
int satCacheHitCount;
int byteCount;
int taskCount;
int colorCounts[24];
int manifoldCounts[B3_CONTACT_MANIFOLD_COUNT_BUCKETS];
int64_t byteCount; //
int bodyCount; //
int shapeCount; //
int contactCount; //
int jointCount; //
int islandCount; //
int stackUsed; //
int arenaCapacity; //
int staticTreeHeight; //
int treeHeight; //
int satCallCount; //
int satCacheHitCount; //
int taskCount; //
int colorCounts[24]; //
int manifoldCounts[B3_CONTACT_MANIFOLD_COUNT_BUCKETS]; //
/// Number of contacts touched by the collide pass
/// graph contacts + awake-set non-touching
int awakeContactCount;
int awakeContactCount; //
/// Number of contacts recycled in the most recent step.
int recycledContactCount;
/// Maximum number of time of impact iterations
/// Maximum number of time of impact outer iterations
int distanceIterations;
/// Maximum TOI push backs.
int pushBackIterations;
/// Maximum TOI root solver iterations.
int rootIterations;
} b3Counters;
//! @endcond
/// Joint type enumeration. This is useful because all joint types use b3JointId and sometimes you
/// want to get the type of a joint.
@ -2651,12 +2652,14 @@ typedef struct b3ManifoldPoint
/// result from the final sub-step.
float normalImpulse;
/// The total normal impulse applied during sub-stepping. This is important
/// The total normal impulse applied across sub-stepping and restitution. This is important
/// to identify speculative contact points that had an interaction in the time step.
/// This includes the warm starting impulse, the sub-step delta impulse, and the restitution
/// impulse.
float totalNormalImpulse;
/// Relative normal velocity pre-solve. Used for hit events. If the normal impulse is
/// zero then there was no hit. Negative means shapes are approaching.
/// Relative normal velocity pre-solve. Negative when approaching. This is only
/// computed if hit events are enabled.
float normalVelocity;
/// Local point for matching

View file

@ -621,7 +621,7 @@ void Sample::DrawMetrics()
int count = m_profileWriteIndex - m_profileReadIndex;
// Unroll ring buffer into per-field histories.
constexpr int kRowCount = 22;
constexpr int kRowCount = 21;
float histories[kRowCount][m_profileCapacity];
float totals[kRowCount] = {};
for ( int i = 0; i < count; ++i )
@ -640,16 +640,15 @@ void Sample::DrawMetrics()
histories[9][i] = p.solveImpulses;
histories[10][i] = p.integratePositions;
histories[11][i] = p.relaxImpulses;
histories[12][i] = p.applyRestitution;
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;
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;
totals[0] += p.step;
totals[1] += p.pairs;
@ -663,16 +662,15 @@ void Sample::DrawMetrics()
totals[9] += p.solveImpulses;
totals[10] += p.integratePositions;
totals[11] += p.relaxImpulses;
totals[12] += p.applyRestitution;
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;
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;
}
// Smoothed over the last few frames so bars don't jitter visibly.
@ -728,13 +726,13 @@ void Sample::DrawMetrics()
const ImU32 colorDefault = IM_COL32( 220, 220, 220, 255 );
const RowDef rows[kRowCount] = {
{ "step", 0, colorStep }, { "pairs", 0, colorPairs }, { "collide", 0, colorCollide },
{ "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 },
{ "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 },
{ "step", 0, colorStep }, { "pairs", 0, colorPairs }, { "collide", 0, colorCollide },
{ "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 },
{ "transforms", 1, colorDefault }, { "joint events", 1, colorDefault }, { "hit events", 1, colorDefault },
{ "refit BVH", 1, colorDefault }, { "sleep", 1, colorDefault }, { "bullets", 1, colorDefault },
{ "sensors", 0, colorSensors },
};
@ -955,7 +953,7 @@ void Sample::DrawMetrics()
ImGui::Text( "toi d/p/r = %d/%d/%d", s.distanceIterations, s.pushBackIterations, s.rootIterations );
ImGui::Text( "stack allocator size = %d K", s.stackUsed / 1024 );
ImGui::Text( "arena capacity = %d K", s.arenaCapacity / 1024 );
ImGui::Text( "total allocation = %d K", s.byteCount / 1024 );
ImGui::Text( "total allocation = %d K", (int)(s.byteCount / 1024) );
ImGui::Separator();
b3Capacity c = b3World_GetMaxCapacity( m_worldId );
@ -2175,3 +2173,24 @@ float CastClosestCallback( b3ShapeId shapeId, b3Pos point, b3Vec3 normal, float
rayContext->hit = true;
return fraction;
}
MechanicalEnergy MeasureEnergy( b3WorldId worldId, const b3BodyId* bodyIds, int count )
{
b3Vec3 gravity = b3World_GetGravity( worldId );
MechanicalEnergy energy = {};
for ( int i = 0; i < count; ++i )
{
b3MassData massData = b3Body_GetMassData( bodyIds[i] );
b3Vec3 v = b3Body_GetLinearVelocity( bodyIds[i] );
// The inertia tensor is in the body frame, so bring the angular velocity back to it
b3Vec3 w = b3InvRotateVector( b3Body_GetRotation( bodyIds[i] ), b3Body_GetAngularVelocity( bodyIds[i] ) );
energy.linear += 0.5f * massData.mass * b3Dot( v, v );
energy.angular += 0.5f * b3Dot( w, b3MulMV( massData.inertia, w ) );
energy.potential -= massData.mass * b3Dot( gravity, b3ToVec3( b3Body_GetWorldCenter( bodyIds[i] ) ) );
}
return energy;
}

View file

@ -294,3 +294,19 @@ struct CastClosestContext
float CastClosestCallback( b3ShapeId shapeId, b3Pos point, b3Vec3 normal, float fraction, uint64_t materialId, int triangleIndex,
int childIndex, void* context );
struct MechanicalEnergy
{
float Total() const
{
return linear + angular + potential;
}
float linear;
float angular;
float potential;
};
// Kinetic and gravitational potential energy summed over the bodies. Restitution samples
// use the total as the invariant that may only decrease.
MechanicalEnergy MeasureEnergy( b3WorldId worldId, const b3BodyId* bodyIds, int count );

View file

@ -8,6 +8,10 @@
#include "box3d/box3d.h"
#include <ctype.h>
#include <iterator>
#include <stdio.h>
#include <stdlib.h>
#include <vector>
class Crash : public Sample
@ -132,65 +136,164 @@ public:
static int sampleMultiplePrismatic = RegisterSample( "Issues", "Multiple Prismatic", MultiplePrismatic::Create );
// bad hull SM_Waterfall_MED_Wide_01
static const b3Vec3 waterfallPoints[] = {
{ 0.100183107, -498.925385, -1275.39966 }, { 0.100183107, -498.925415, 0.125000000 },
{ 0.100183107, 486.343750, 0.125000000 }, { 0.100183107, 486.343719, -1275.39966 },
{ -395.117462, 486.343781, -1462.43750 }, { -395.117462, 486.343750, -96.7426758 },
{ -395.117462, -498.925415, -96.7424469 }, { -395.117462, -498.925446, -1462.52612 },
{ -186.979691, 486.294891, -1462.47949 }, { -298.250000, 486.294891, 0.125000000 },
{ -395.121216, 486.294891, -1462.52612 }, { -186.984360, -498.913361, -1462.48413 },
{ -298.250000, -498.913361, 0.125000000 },
};
static const b3Vec3 quadPoints[] = {
{ 100.000000, -142.292389, 130.826111 }, { 99.5354385, -71.3011093, 130.826111 },
{ 99.5930862, -80.1112213, -100.000000 }, { 100.000000, -142.292389, -100.000000 },
{ 99.5930862, -80.1112213, 130.826111 },
};
static const b3Vec3 thinBoxPoints[] = {
{ -11.3861933, -24.2451687, -12.0037909 }, { -11.3889809, -24.2466526, -11.9013014 },
{ -11.3804407, -24.3151531, -12.0046492 }, { -11.3832273, -24.3166409, -11.9021587 },
{ -14.4396200, -24.3636723, -12.1324549 }, { -14.4432650, -24.3655701, -12.0299988 },
{ -14.4356947, -24.4337788, -12.1336164 }, { -14.4393377, -24.4356804, -12.0311594 },
};
// Inline point sets are scaled from centimeters. A null point array is a file in data/hulls.
struct HullSource
{
const char* name;
const b3Vec3* points;
int count;
float scale;
};
static const HullSource hullSources[] = {
{ "waterfall", waterfallPoints, (int)std::size( waterfallPoints ), 0.01f },
{ "quad", quadPoints, (int)std::size( quadPoints ), 0.01f },
{ "thin box", thinBoxPoints, (int)std::size( thinBoxPoints ), 0.01f },
{ "axe.txt", nullptr, 0, 1.0f },
};
static constexpr int hullSourceCount = (int)std::size( hullSources );
// Reads x, y, z triples separated by commas or whitespace. A C source repro is
// also accepted: only the first initializer list is parsed, so counts and other
// numbers in the surrounding code are ignored.
static bool LoadHullPoints( const char* path, std::vector<b3Vec3>* points )
{
points->clear();
FILE* file = fopen( path, "rb" );
if ( file == nullptr )
{
fprintf( stderr, "Failed to open '%s'\n", path );
return false;
}
fseek( file, 0, SEEK_END );
long size = ftell( file );
fseek( file, 0, SEEK_SET );
if ( size < 0 )
{
fprintf( stderr, "Failed to read '%s'\n", path );
fclose( file );
return false;
}
std::vector<char> text( size + 1 );
size_t readCount = fread( text.data(), 1, size, file );
fclose( file );
text[readCount] = '\0';
char* begin = text.data();
char* end = begin + readCount;
for ( char* c = begin; *c != '\0'; ++c )
{
if ( *c != '{' )
{
continue;
}
char* prev = c - 1;
while ( prev >= begin && isspace( (unsigned char)*prev ) )
{
--prev;
}
if ( prev < begin || *prev != '=' )
{
continue;
}
begin = c + 1;
int depth = 1;
for ( end = begin; *end != '\0'; ++end )
{
if ( *end == '{' )
{
depth += 1;
}
else if ( *end == '}' && --depth == 0 )
{
break;
}
}
break;
}
*end = '\0';
std::vector<float> values;
char* cursor = begin;
while ( *cursor != '\0' )
{
char* stop;
float value = strtof( cursor, &stop );
if ( stop == cursor )
{
cursor += 1;
continue;
}
values.push_back( value );
cursor = stop;
}
if ( values.size() % 3 != 0 )
{
fprintf( stderr, "'%s' has %d values, not a multiple of 3\n", path, (int)values.size() );
}
int count = (int)values.size() / 3;
points->resize( count );
for ( int i = 0; i < count; ++i )
{
( *points )[i] = { values[3 * i], values[3 * i + 1], values[3 * i + 2] };
}
return count > 0;
}
class HullCrash : public Sample
{
public:
explicit HullCrash( SampleContext* context )
: Sample( context )
{
m_hull = nullptr;
m_sourceIndex = 3;
m_maxVertexCount = B3_MAX_HULL_VERTICES;
LoadPoints();
CreateHull();
if ( m_context->restart == false )
{
m_camera->SetView( 0.0f, 15.0f, 5.0f, b3Pos_zero );
FrameView();
}
m_hull = nullptr;
#if 0
// bad hull SM_Waterfall_MED_Wide_01
b3Vec3 points[] = {
{ 0.100183107, -498.925385, -1275.39966 }, { 0.100183107, -498.925415, 0.125000000 },
{ 0.100183107, 486.343750, 0.125000000 }, { 0.100183107, 486.343719, -1275.39966 },
{ -395.117462, 486.343781, -1462.43750 }, { -395.117462, 486.343750, -96.7426758 },
{ -395.117462, -498.925415, -96.7424469 }, { -395.117462, -498.925446, -1462.52612 },
{ -186.979691, 486.294891, -1462.47949 }, { -298.250000, 486.294891, 0.125000000 },
{ -395.121216, 486.294891, -1462.52612 }, { -186.984360, -498.913361, -1462.48413 },
{ -298.250000, -498.913361, 0.125000000 },
};
#elif 1
b3Vec3 points[] = {
{ 100.000000, -142.292389, 130.826111 }, { 99.5354385, -71.3011093, 130.826111 },
{ 99.5930862, -80.1112213, -100.000000 }, { 100.000000, -142.292389, -100.000000 },
{ 99.5930862, -80.1112213, 130.826111 },
};
#else
b3Vec3 points[] = {
{ -11.3861933, -24.2451687, -12.0037909 }, { -11.3889809, -24.2466526, -11.9013014 },
{ -11.3804407, -24.3151531, -12.0046492 }, { -11.3832273, -24.3166409, -11.9021587 },
{ -14.4396200, -24.3636723, -12.1324549 }, { -14.4432650, -24.3655701, -12.0299988 },
{ -14.4356947, -24.4337788, -12.1336164 }, { -14.4393377, -24.4356804, -12.0311594 },
};
#endif
static_assert( sizeof( points ) / sizeof( points[0] ) < m_capacity, "bad" );
m_count = sizeof( points ) / sizeof( points[0] );
for ( int i = 0; i < m_count; ++i )
{
m_points[i] = 0.01f * points[i];
}
// This shift shouldn't be necessary but I'm doing it so the hull
// appears on the screen.
// for ( int i = 0; i < m_count; ++i )
//{
// m_points[i] -= m_points[0];
// m_points[i] *= 0.01f;
//}
m_hull = b3CreateHull( m_points, m_count, m_count );
(void)m_hull;
}
~HullCrash() override
@ -201,6 +304,94 @@ public:
}
}
void LoadPoints()
{
const HullSource& source = hullSources[m_sourceIndex];
if ( source.points != nullptr )
{
m_points.assign( source.points, source.points + source.count );
}
else
{
char path[64];
snprintf( path, sizeof( path ), "data/hulls/%s", source.name );
LoadHullPoints( path, &m_points );
}
for ( b3Vec3& point : m_points )
{
point = source.scale * point;
}
// The reported failure used the point count as the limit
m_maxVertexCount = b3ClampInt( (int)m_points.size(), 4, B3_MAX_HULL_VERTICES );
}
void CreateHull()
{
if ( m_hull != nullptr )
{
b3DestroyHull( m_hull );
m_hull = nullptr;
}
int count = (int)m_points.size();
if ( count >= 4 )
{
m_hull = b3CreateHull( m_points.data(), count, m_maxVertexCount );
}
}
void FrameView()
{
if ( m_points.empty() )
{
return;
}
b3AABB bounds = b3MakeAABB( m_points.data(), (int)m_points.size(), 0.0f );
float radius = b3Length( b3AABB_Extents( bounds ) );
m_camera->SetView( 0.0f, 15.0f, b3MaxFloat( 3.0f * radius, 1.0f ), b3ToPos( b3AABB_Center( bounds ) ) );
}
bool DrawControls() override
{
DrawTextLine( "points = %d", (int)m_points.size() );
if ( m_hull != nullptr )
{
DrawTextLine( "hull: vertices = %d, edges = %d, faces = %d", m_hull->vertexCount, m_hull->edgeCount / 2,
m_hull->faceCount );
}
else
{
DrawTextLine( "hull creation failed" );
}
if ( ImGui::BeginCombo( "Source", hullSources[m_sourceIndex].name ) )
{
for ( int i = 0; i < hullSourceCount; ++i )
{
if ( ImGui::Selectable( hullSources[i].name, i == m_sourceIndex ) )
{
m_sourceIndex = i;
LoadPoints();
CreateHull();
FrameView();
}
}
ImGui::EndCombo();
}
if ( ImGui::SliderInt( "Max Vertices", &m_maxVertexCount, 4, B3_MAX_HULL_VERTICES ) )
{
CreateHull();
}
return true;
}
void Render() override
{
if ( m_hull != nullptr )
@ -209,9 +400,9 @@ public:
}
else
{
for ( int i = 0; i < m_count; ++i )
for ( const b3Vec3& point : m_points )
{
DrawPoint( b3ToPos( m_points[i] ), 5.0f, MakeColor( b3_colorWhite ) );
DrawPoint( b3ToPos( point ), 5.0f, MakeColor( b3_colorWhite ) );
}
}
@ -225,10 +416,10 @@ public:
return new HullCrash( sampleContext );
}
static constexpr int m_capacity = 64;
std::vector<b3Vec3> m_points;
b3HullData* m_hull;
b3Vec3 m_points[m_capacity];
int m_count;
int m_sourceIndex;
int m_maxVertexCount;
};
static int sampleHullCrash = RegisterSample( "Issues", "Hull Crash", HullCrash::Create );
@ -1146,6 +1337,7 @@ public:
static int sampleWheelStack = RegisterSample( "Issues", "GMod Wheel Stack", WheelStack::Create );
// There was a problem where this bouncing box could gain energy. This has been fixed by the deferred restitution update.
class RestitutionOvershoot : public Sample
{
public:
@ -1176,6 +1368,7 @@ public:
b3BodyDef boxDef = b3DefaultBodyDef();
boxDef.type = b3_dynamicBody;
boxDef.position = { 0.0f, m_dropHeight, 0.0f };
boxDef.safetyFactor = 0.1f;
m_boxBody = b3CreateBody( m_worldId, &boxDef );
b3ShapeDef boxShape = b3DefaultShapeDef();
@ -1186,6 +1379,9 @@ public:
m_maxBounceY = 0.0f;
m_bounced = false;
m_failed = false;
m_startEnergy = MeasureEnergy( m_worldId, &m_boxBody, 1 ).Total();
m_maxEnergy = m_startEnergy;
m_energyFailed = false;
}
void Step() override
@ -1221,9 +1417,29 @@ public:
b3Pos markerPoint = { 0.0f, m_dropHeight + m_boxHalf, 0.0f };
DrawPlane( b3Vec3_axisY, markerPoint, MakeColor( b3_colorYellow ) );
// Perfect restitution and no friction, so the total energy may only decrease. It is the honest
// invariant here: the height check cannot tell a bounce that gained potential energy from one
// that converted it into spin, and this drop does both.
MechanicalEnergy energy = MeasureEnergy( m_worldId, &m_boxBody, 1 );
float total = energy.Total();
if ( total > m_maxEnergy )
{
m_maxEnergy = total;
}
if ( m_bounced && total > m_startEnergy * ( 1.0f + m_energyTolerance ) )
{
m_energyFailed = true;
}
DrawTextLine( "drop height = %.2f m", m_dropHeight );
DrawTextLine( "current y = %.2f m", m_currentY );
DrawTextLine( "max bounce = %.2f m", m_maxBounceY );
DrawTextLine( "kinetic = %.0f J linear + %.0f J angular", energy.linear, energy.angular );
DrawTextLine( "potential = %.0f J", energy.potential );
DrawTextLine( "total = %.0f J (%.2f%% of start, peak %.2f%%)", total, 100.0f * total / m_startEnergy,
100.0f * m_maxEnergy / m_startEnergy );
if ( m_bounced == false )
{
@ -1237,6 +1453,15 @@ public:
{
DrawTextLine( "PASS: bounce stays at or below drop height" );
}
if ( m_energyFailed )
{
DrawTextLine( "FAIL: total energy increased" );
}
else if ( m_bounced )
{
DrawTextLine( "PASS: total energy did not increase" );
}
}
static Sample* Create( SampleContext* context )
@ -1244,11 +1469,16 @@ public:
return new RestitutionOvershoot( context );
}
static constexpr float m_energyTolerance = 0.001f;
b3BodyId m_boxBody = {};
float m_currentY = 0.0f;
float m_maxBounceY = 0.0f;
float m_startEnergy = 0.0f;
float m_maxEnergy = 0.0f;
bool m_bounced = false;
bool m_failed = false;
bool m_energyFailed = false;
};
static int sampleRestitutionOvershoot = RegisterSample( "Issues", "Restitution Overshoot", RestitutionOvershoot::Create );

View file

@ -1,6 +1,7 @@
// SPDX-FileCopyrightText: 2025 Erin Catto
// SPDX-License-Identifier: MIT
#include "gfx/debug_adapter.h"
#include "gfx/draw.h"
#include "human.h"
#include "imgui.h"
@ -336,6 +337,528 @@ public:
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

View file

@ -69,18 +69,41 @@ b3BodyState* b3GetBodyState( b3World* world, b3Body* body )
return NULL;
}
void b3RefreshBodyContactIndices( b3World* world, b3Body* body )
{
int encodedBodySimIndex = b3EncodeBodySimIndex( body );
int contactKey = body->headContactKey;
while ( contactKey != B3_NULL_INDEX )
{
int edgeIndex = contactKey & 1;
int contactId = contactKey >> 1;
b3Contact* contact = b3Array_Get( world->contacts, contactId );
contactKey = contact->edges[edgeIndex].nextKey;
if ( edgeIndex == 0 )
{
contact->encodedBodySimA = encodedBodySimIndex;
}
else
{
contact->encodedBodySimB = encodedBodySimIndex;
}
}
}
void b3SyncBodyFlags( b3World* world, b3Body* body )
{
// Never sync transient flags
uint32_t flags = body->flags & ~b3_bodyTransientFlags;
b3BodySim* bodySim = b3GetBodySim( world, body );
bodySim->flags = flags;
// Preserve the fast flag for contact recycling.
bodySim->flags = ( bodySim->flags & b3_isFast ) | ( body->flags & ~b3_bodyTransientFlags );
b3BodyState* bodyState = b3GetBodyState( world, body );
if ( bodyState != NULL )
{
bodyState->flags = flags;
bodyState->flags = body->flags & ~b3_bodyTransientFlags;
}
}
@ -410,6 +433,7 @@ void b3DestroyBody( b3BodyId bodyId )
b3Body* movedBody = b3Array_Get( world->bodies, movedId );
B3_ASSERT( movedBody->localIndex == movedIndex );
movedBody->localIndex = body->localIndex;
b3RefreshBodyContactIndices( world, movedBody );
}
// Remove body state from awake set
@ -1120,17 +1144,11 @@ void b3Body_SetTransform( b3BodyId bodyId, b3Pos position, b3Quat rotation )
b3AABB aabb = b3ComputeFatShapeAABB( shape, transform, speculativeDistance );
shape->aabb = aabb;
if ( b3AABB_Contains( shape->fatAABB, aabb ) == false )
b3AABB* shapeFatAABB = world->fatAABBs.data + shapeId;
if ( b3AABB_Contains( *shapeFatAABB, aabb ) == false )
{
float margin = shape->aabbMargin;
b3AABB fatAABB;
fatAABB.lowerBound.x = aabb.lowerBound.x - margin;
fatAABB.lowerBound.y = aabb.lowerBound.y - margin;
fatAABB.lowerBound.z = aabb.lowerBound.z - margin;
fatAABB.upperBound.x = aabb.upperBound.x + margin;
fatAABB.upperBound.y = aabb.upperBound.y + margin;
fatAABB.upperBound.z = aabb.upperBound.z + margin;
shape->fatAABB = fatAABB;
b3AABB fatAABB = b3AABB_Inflate( aabb, shape->aabbMargin );
*shapeFatAABB = fatAABB;
// The body could be disabled
if ( shape->proxyKey != B3_NULL_INDEX )
@ -1717,7 +1735,7 @@ void b3Body_SetType( b3BodyId bodyId, b3BodyType type )
shapeId = shape->nextShapeId;
b3DestroyShapeProxy( shape, &world->broadPhase );
bool forcePairCreation = true;
b3CreateShapeProxy( shape, &world->broadPhase, type, transform, forcePairCreation );
b3CreateShapeProxy( world, shape, type, transform, forcePairCreation );
}
// Relink all joints
@ -2263,7 +2281,7 @@ void b3Body_Enable( b3BodyId bodyId )
b3Shape* shape = b3Array_Get( world->shapes, shapeId );
shapeId = shape->nextShapeId;
b3CreateShapeProxy( shape, &world->broadPhase, proxyType, transform, forcePairCreation );
b3CreateShapeProxy( world, shape, proxyType, transform, forcePairCreation );
}
if ( setId != b3_staticSet )

View file

@ -3,6 +3,8 @@
#pragma once
#include "physics_world.h"
#include "box3d/constants.h"
#include "box3d/math_functions.h"
#include "box3d/types.h"
@ -29,7 +31,7 @@ enum b3BodyFlags
// This body has fixed rotation around the z-axis
b3_lockAngularZ = 0x00000020,
// This flag is used for debug draw
// This flag is used for debug draw and contact recycling. Only lives on b3BodySim.
b3_isFast = 0x00000040,
// This dynamic body does a final CCD pass against all body types, but not other bullets
@ -44,8 +46,9 @@ enum b3BodyFlags
// This body has no limit on angular velocity
b3_allowFastRotation = 0x00000400,
// This body need's to have its AABB increased
b3_enlargeBounds = 0x00000800,
// This bullet body needs to have its AABB increased. Needed because bullets don't follow
// the standard broad-phase update.
b3_enlargeBulletBounds = 0x00000800,
// This body is dynamic so the solver should write to it.
// This prevents writing to kinematic bodies that causes a multithreaded sharing
@ -68,7 +71,7 @@ enum b3BodyFlags
b3_fixedRotation = b3_lockAngularX | b3_lockAngularY | b3_lockAngularZ,
// These flags are transient per time step. These may be different across b3Body, b3BodySim, and b3BodyState.
b3_bodyTransientFlags = b3_isFast | b3_isSpeedCapped | b3_hadTimeOfImpact,
b3_bodyTransientFlags = b3_isSpeedCapped | b3_hadTimeOfImpact,
};
// Body organizational details that are not used in the solver.
@ -241,6 +244,38 @@ bool b3WakeBodyWithLock( b3World* world, b3Body* body );
void b3UpdateBodyMassData( b3World* world, b3Body* body );
void b3SyncBodyFlags( b3World* world, b3Body* body );
void b3RefreshBodyContactIndices( b3World* world, b3Body* body );
// Encode the body sim index for storage in the contact.
static inline int b3EncodeBodySimIndex( const b3Body* body )
{
if ( body->setIndex == b3_awakeSet )
{
return body->localIndex;
}
if ( body->setIndex == b3_staticSet )
{
return -( body->localIndex + 2 );
}
return B3_NULL_INDEX;
}
static inline bool b3IsStaticSimIndex( int encodedBodySimIndex )
{
return encodedBodySimIndex < B3_NULL_INDEX;
}
static inline int b3DecodeAwakeIndex( int encodedBodySimIndex )
{
return encodedBodySimIndex >= 0 ? encodedBodySimIndex : B3_NULL_INDEX;
}
static inline int b3SleepBodySimIndex( int encodedBodySimIndex )
{
return encodedBodySimIndex >= 0 ? B3_NULL_INDEX : encodedBodySimIndex;
}
// Make a sweep relative to a base position to keep TOI in float precision far from the origin.
static inline b3Sweep b3MakeRelativeSweep( const b3BodySim* bodySim, b3Pos base )

View file

@ -149,8 +149,8 @@ void b3AddContactToGraph( b3World* world, b3Contact* contact )
b3GraphColor* color = graph->colors + colorIndex;
contact->colorIndex = colorIndex;
contact->localIndex = isScalar ? color->contacts.count : color->convexContacts.count;
contact->bodySimIndexA = bodyA->type == b3_staticBody ? B3_NULL_INDEX : bodyA->localIndex;
contact->bodySimIndexB = bodyB->type == b3_staticBody ? B3_NULL_INDEX : bodyB->localIndex;
contact->encodedBodySimA = b3EncodeBodySimIndex( bodyA );
contact->encodedBodySimB = b3EncodeBodySimIndex( bodyB );
if ( isScalar )
{

View file

@ -210,6 +210,8 @@ void b3CreateContact( b3World* world, b3Shape* shapeA, b3Shape* shapeB, int chil
contact->shapeIdA = shapeIdA;
contact->shapeIdB = shapeIdB;
contact->childIndex = childIndex;
contact->encodedBodySimA = b3EncodeBodySimIndex( bodyA );
contact->encodedBodySimB = b3EncodeBodySimIndex( bodyB );
// Both bodies must enable recycling
if ( ( bodyA->flags & b3_bodyEnableContactRecycling ) != 0 && ( bodyB->flags & b3_bodyEnableContactRecycling ) != 0 )

View file

@ -126,9 +126,9 @@ typedef struct b3Contact
// Back index into b3World::contacts
int contactId;
// These are transient and cached for improved performance. B3_NULL_INDEX for static bodies.
int bodySimIndexA;
int bodySimIndexB;
// Encoded body sim indices to avoid a cache miss accessing the body in the narrow phase.
int encodedBodySimA;
int encodedBodySimB;
// b3ContactFlags
uint32_t flags;

File diff suppressed because it is too large Load diff

View file

@ -10,7 +10,7 @@ typedef struct b3ManifoldConstraintPoint
{
b3Vec3 rA, rB;
float baseSeparation;
float relativeVelocity;
float restitutionVelocity;
float normalImpulse;
float totalNormalImpulse;
float normalMass;
@ -46,7 +46,6 @@ typedef struct b3ContactConstraint
b3Softness softness;
b3Matrix3 rollingMass;
float friction;
float restitution;
float rollingResistance;
int manifoldCount;
} b3ContactConstraint;
@ -56,18 +55,18 @@ int b3GetWideContactConstraintByteCount( void );
// Overflow contacts don't fit into the constraint graph coloring
void b3PrepareContacts_Overflow( b3StepContext* context );
void b3WarmStartContacts_Overflow( b3StepContext* context );
void b3SolveContacts_Overflow( b3StepContext* context, bool useBias );
void b3ApplyRestitution_Overflow( b3StepContext* context );
void b3PushContacts_Overflow( b3StepContext* context );
void b3SolveContacts_Overflow( b3StepContext* context );
void b3StoreImpulses_Overflow( b3StepContext* context );
void b3PrepareContacts_Mesh( b3SolverBlock block, b3StepContext* context );
void b3WarmStartContacts_Mesh( b3SolverBlock block, b3StepContext* context );
void b3SolveContacts_Mesh( b3SolverBlock block, b3StepContext* context, bool useBias );
void b3ApplyRestitution_Mesh( b3SolverBlock block, b3StepContext* context );
void b3PushContacts_Mesh( b3SolverBlock block, b3StepContext* context );
void b3SolveContacts_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 b3SolveContacts_Convex( b3SolverBlock block, b3StepContext* context, bool useBias );
void b3ApplyRestitution_Convex( b3SolverBlock block, b3StepContext* context );
void b3PushContacts_Convex( b3SolverBlock block, b3StepContext* context );
void b3SolveContacts_Convex( b3SolverBlock block, b3StepContext* context );
void b3StoreImpulses_Convex( b3SolverBlock block, b3StepContext* context, int workerIndex );

View file

@ -138,7 +138,7 @@ int b3GetMaxManifoldPoints( void )
static b3AllocFcn* b3_allocFcn = NULL;
static b3FreeFcn* b3_freeFcn = NULL;
b3AtomicInt b3_byteCount;
static b3AtomicI64 b3_byteCount;
void b3SetAllocator( b3AllocFcn* allocFcn, b3FreeFcn* freeFcn )
{
@ -153,13 +153,11 @@ void* b3Alloc( size_t size )
return NULL;
}
// This could cause some sharing issues, however Box3D rarely calls b3Alloc.
// todo this is not true, Box3D allocates a lot.
b3AtomicFetchAddInt( &b3_byteCount, (int)size );
b3AtomicFetchAddI64( &b3_byteCount, (int)size );
// Allocation must be a multiple of B3_ALIGNMENT (required by spec).
// https://en.cppreference.com/w/c/memory/aligned_alloc
int alignedSize = ( ( (int)size - 1 ) | ( B3_ALIGNMENT - 1 ) ) + 1;
size_t alignedSize = ( ( size - 1 ) | ( B3_ALIGNMENT - 1 ) ) + 1;
if ( b3_allocFcn != NULL )
{
@ -204,7 +202,8 @@ void b3Free( void* mem, size_t size )
if ( b3_freeFcn != NULL )
{
b3_freeFcn( mem );
size_t alignedSize = ( ( size - 1 ) | ( B3_ALIGNMENT - 1 ) ) + 1;
b3_freeFcn( mem, alignedSize );
}
else
{
@ -215,7 +214,7 @@ void b3Free( void* mem, size_t size )
#endif
}
b3AtomicFetchAddInt( &b3_byteCount, -(int)size );
b3AtomicFetchAddI64( &b3_byteCount, -(int64_t)size );
}
void* b3GrowAlloc( void* oldMem, int oldSize, int newSize )
@ -237,9 +236,9 @@ void* b3GrowAllocZeroed( void* oldMem, int oldSize, int newSize )
return newMem;
}
int b3GetByteCount( void )
int64_t b3GetByteCount( void )
{
return b3AtomicLoadInt( &b3_byteCount );
return b3AtomicLoadI64( &b3_byteCount );
}
void* b3AllocZero( size_t size )

View file

@ -113,9 +113,17 @@ typedef struct b3AtomicU32
uint32_t value;
} b3AtomicU32;
typedef struct b3AtomicI64
{
// 64-bit atomic wants 8-byte alignment
_Alignas( 8 ) int64_t value;
} b3AtomicI64;
// Minimum memory alignment used for all allocations
#define B3_ALIGNMENT 16
#define B3_RESTRICT restrict
// Returns the number of elements of an array
#define B3_ARRAY_COUNT( A ) (int)( sizeof( A ) / sizeof( A[0] ) )

View file

@ -20,6 +20,7 @@
#include "scheduler.h"
#include "sensor.h"
#include "shape.h"
#include "simd.h"
#include "solver.h"
#include "solver_set.h"
@ -290,6 +291,7 @@ b3WorldId b3CreateWorld( const b3WorldDef* def )
int shapeCapacity = b3MaxInt( 16, def->capacity.staticShapeCount + def->capacity.dynamicShapeCount );
b3Array_Reserve( world->shapes, shapeCapacity );
b3Array_Reserve( world->fatAABBs, shapeCapacity );
world->hullDatabase = b3Alloc( sizeof( b3HullMap ) );
b3HullMap_init( world->hullDatabase );
@ -482,6 +484,7 @@ void b3DestroyWorld( b3WorldId worldId )
b3DestroyNameCache( &world->names );
b3Array_Destroy( world->shapes );
b3Array_Destroy( world->fatAABBs );
b3Array_Destroy( world->contacts );
b3Array_Destroy( world->joints );
@ -555,6 +558,24 @@ static inline void b3PrefetchContact( const b3Contact* contact )
b3Prefetch( p + 192 );
}
static inline b3BodySim* b3ResolveContactBodySim( b3World* world, b3BodySim* awakeSims, b3BodySim* staticSims, int encodedIndex,
int bodyId )
{
if ( encodedIndex >= 0 )
{
return awakeSims + encodedIndex;
}
if ( b3IsStaticSimIndex( encodedIndex ) )
{
return staticSims - encodedIndex - 2;
}
b3Body* body = b3Array_Get( world->bodies, bodyId );
b3SolverSet* set = b3Array_Get( world->solverSets, body->setIndex );
return b3Array_Get( set->bodySims, body->localIndex );
}
static void b3CollideTask( int startIndex, int endIndex, int workerIndex, void* context )
{
b3TracyCZoneNC( collide_task, "Collide Task", b3_colorDodgerBlue, true );
@ -566,7 +587,7 @@ static void b3CollideTask( int startIndex, int endIndex, int workerIndex, void*
int* contactIndices = stepContext->awakeContactIndices;
b3Contact* contacts = world->contacts.data;
b3Shape* shapes = world->shapes.data;
b3Body* bodies = world->bodies.data;
const b3AABB* fatAABBs = world->fatAABBs.data;
b3BodySim* awakeSims = world->solverSets.data[b3_awakeSet].bodySims.data;
b3BodySim* staticSims = world->solverSets.data[b3_staticSet].bodySims.data;
@ -595,11 +616,11 @@ static void b3CollideTask( int startIndex, int endIndex, int workerIndex, void*
b3Contact* contact = contacts + contactIndex;
B3_VALIDATE( contact->contactId == contactIndex );
b3Shape* shapeA = shapes + contact->shapeIdA;
b3Shape* shapeB = shapes + contact->shapeIdB;
int shapeIdA = contact->shapeIdA;
int shapeIdB = contact->shapeIdB;
// Do proxies still overlap?
bool overlap = b3AABB_Overlaps( shapeA->fatAABB, shapeB->fatAABB );
bool overlap = b3OverlapV( fatAABBs + shapeIdA, fatAABBs + shapeIdB );
if ( overlap == false )
{
// This contact will be destroyed
@ -609,51 +630,29 @@ static void b3CollideTask( int startIndex, int endIndex, int workerIndex, void*
continue;
}
b3Shape* shapeA = shapes + shapeIdA;
b3Shape* shapeB = shapes + shapeIdB;
// Update contact respecting shape/body order (A,B)
b3Body* bodyA = bodies + shapeA->bodyId;
b3Body* bodyB = bodies + shapeB->bodyId;
bool isStaticA = bodyA->type == b3_staticBody;
bool isStaticB = bodyB->type == b3_staticBody;
int encodedA = contact->encodedBodySimA;
int encodedB = contact->encodedBodySimB;
b3BodySim* bodySimA = b3ResolveContactBodySim( world, awakeSims, staticSims, encodedA, contact->edges[0].bodyId );
b3BodySim* bodySimB = b3ResolveContactBodySim( world, awakeSims, staticSims, encodedB, contact->edges[1].bodyId );
bool wasTouching = ( contact->flags & b3_simTouchingFlag );
b3BodySim* bodySimA;
b3BodySim* bodySimB;
if ( wasTouching )
{
B3_ASSERT( bodyA->setIndex == b3_awakeSet || bodyA->setIndex == b3_staticSet );
B3_ASSERT( bodyB->setIndex == b3_awakeSet || bodyB->setIndex == b3_staticSet );
bodySimA = ( isStaticA ? staticSims : awakeSims ) + bodyA->localIndex;
bodySimB = ( isStaticB ? staticSims : awakeSims ) + bodyB->localIndex;
}
else
{
// There can be non-touching contacts between awake bodies and sleeping bodies.
{
b3SolverSet* set = b3Array_Get( world->solverSets, bodyA->setIndex );
bodySimA = b3Array_Get( set->bodySims, bodyA->localIndex );
}
{
b3SolverSet* set = b3Array_Get( world->solverSets, bodyB->setIndex );
bodySimB = b3Array_Get( set->bodySims, bodyB->localIndex );
}
}
b3WorldTransform transformA = bodySimA->transform;
b3WorldTransform transformB = bodySimB->transform;
bool isFast = ( bodySimA->flags & b3_isFast ) || ( bodySimB->flags & b3_isFast );
bool isFast = ( ( bodySimA->flags | bodySimB->flags ) & b3_isFast ) != 0;
// These are used by the contact solver. If the contact is between an awake body
// and a sleeping body and the contact begins to touch, the these will be invalid
// but fixed when linked in the constraint graph.
contact->bodySimIndexA = isStaticA ? B3_NULL_INDEX : bodyA->localIndex;
contact->bodySimIndexB = isStaticB ? B3_NULL_INDEX : bodyB->localIndex;
float recycleTolerance = wasTouching ? recycleDistance : recycleDistanceNonTouching;
// Contact recycling optimization. Please cite this library if you use this optimization.
// This is inspired by persistent contact manifolds used in some physics engines, such as PhysX.
// However, this allows larger relative motion and has fewer tuning parameters (just one).
if ( isFast == false && recycleDistance > 0.0f &&
( contact->flags & b3_relativeTransformValid ) && ( contact->flags & b3_contactRecycleFlag ) )
if ( isFast == false && recycleDistance > 0.0f && ( contact->flags & b3_relativeTransformValid ) &&
( contact->flags & b3_contactRecycleFlag ) )
{
// The scalar part of b3InvMulQuat is just the quaternion dot product.
// cos(relative_angle/2) = scalar(conj(q1) * q2) = dot(q1, q2)
@ -665,8 +664,8 @@ static void b3CollideTask( int startIndex, int endIndex, int workerIndex, void*
b3Transform xf = b3InvMulWorldTransforms( transformA, transformB );
b3Transform xfc = contact->cachedRelativePose;
b3Vec3 maxExtentA = isStaticA ? b3Vec3_zero : bodySimA->maxExtent;
b3Vec3 maxExtentB = isStaticB ? b3Vec3_zero : bodySimB->maxExtent;
b3Vec3 maxExtentA = b3IsStaticSimIndex( encodedA ) ? b3Vec3_zero : bodySimA->maxExtent;
b3Vec3 maxExtentB = b3IsStaticSimIndex( encodedB ) ? b3Vec3_zero : bodySimB->maxExtent;
b3Vec3 maxExtent = b3Max( maxExtentA, maxExtentB );
// Variation of Conservative Advancement
@ -772,14 +771,19 @@ static void b3CollideTask( int startIndex, int endIndex, int workerIndex, void*
b3SetBit( &taskContext->contactStateBitSet, contactIndex );
}
for ( int manifoldIndex = 0; manifoldIndex < contact->manifoldCount; ++manifoldIndex )
if ( touching )
{
b3Manifold* manifold = contact->manifolds + manifoldIndex;
for ( int pointIndex = 0; pointIndex < manifold->pointCount; ++pointIndex )
int manifoldCount = contact->manifoldCount;
for ( int manifoldIndex = 0; manifoldIndex < manifoldCount; ++manifoldIndex )
{
// Cache separation
b3ManifoldPoint* mp = manifold->points + pointIndex;
mp->baseSeparation = mp->separation;
b3Manifold* manifold = contact->manifolds + manifoldIndex;
int pointCount = manifold->pointCount;
for ( int pointIndex = 0; pointIndex < pointCount; ++pointIndex )
{
// Cache separation
b3ManifoldPoint* mp = manifold->points + pointIndex;
mp->baseSeparation = mp->separation;
}
}
}
}
@ -793,8 +797,6 @@ static void b3AddNonTouchingContact( b3World* world, b3Contact* contact )
b3SolverSet* set = b3Array_Get( world->solverSets, b3_awakeSet );
contact->colorIndex = B3_NULL_INDEX;
contact->localIndex = set->contactIndices.count;
contact->bodySimIndexA = B3_NULL_INDEX;
contact->bodySimIndexB = B3_NULL_INDEX;
b3Array_Push( set->contactIndices, contact->contactId );
}
@ -1358,7 +1360,7 @@ static bool DrawQueryCallback( int proxyId, uint64_t userData, void* context )
if ( draw->drawBounds )
{
draw->DrawBoundsFcn( shape->fatAABB, b3_colorGold, draw->context );
draw->DrawBoundsFcn( world->fatAABBs.data[shapeId], b3_colorGold, draw->context );
}
return true;
@ -1440,7 +1442,7 @@ void b3World_Draw( b3WorldId worldId, b3DebugDraw* draw, uint64_t maskBits )
b3WorldTransform transform = { bodySim->center, bodySim->transform.q };
draw->DrawTransformFcn( transform, draw->context );
if (body->type == b3_dynamicBody)
if ( body->type == b3_dynamicBody )
{
b3Vec3 offset = { 0.05f, 0.05f, 0.05f };
b3Pos p = b3TransformWorldPoint( transform, offset );
@ -1563,9 +1565,7 @@ void b3World_Draw( b3WorldId worldId, b3DebugDraw* draw, uint64_t maskBits )
{
// Hack inv_dt for single step debugging
float inv_dt = world->inv_dt > 0.0f ? world->inv_dt : 60.0f;
// todo validate
// multiply by one-half due to relax iteration
float force = 0.5f * mp->totalNormalImpulse * inv_dt;
float force = mp->totalNormalImpulse * inv_dt;
b3Pos p1 = p;
b3Pos p2 = b3OffsetPos( p1, b3MulSV( draw->forceScale * force, normal ) );
draw->DrawSegmentFcn( p1, p2, impulseColor, draw->context );
@ -1657,7 +1657,7 @@ void b3World_Draw( b3WorldId worldId, b3DebugDraw* draw, uint64_t maskBits )
while ( shapeId != B3_NULL_INDEX )
{
b3Shape* shape = b3Array_Get( world->shapes, shapeId );
aabb = b3AABB_Union( aabb, shape->fatAABB );
aabb = b3AABB_Union( aabb, world->fatAABBs.data[shapeId] );
shapeCount += 1;
shapeId = shape->nextShapeId;
}
@ -3575,6 +3575,21 @@ void b3ValidateSolverSets( b3World* world )
B3_ASSERT( b3GetIdCapacity( &world->islandIdPool ) == world->islands.count );
B3_ASSERT( b3GetIdCapacity( &world->solverSetIdPool ) == world->solverSets.count );
// Ensure the encoded body sim indices are correct.
for ( int contactIndex = 0; contactIndex < world->contacts.count; ++contactIndex )
{
b3Contact* contact = world->contacts.data + contactIndex;
if ( contact->contactId == B3_NULL_INDEX )
{
continue;
}
b3Body* bodyA = b3Array_Get( world->bodies, contact->edges[0].bodyId );
b3Body* bodyB = b3Array_Get( world->bodies, contact->edges[1].bodyId );
B3_ASSERT( contact->encodedBodySimA == b3EncodeBodySimIndex( bodyA ) );
B3_ASSERT( contact->encodedBodySimB == b3EncodeBodySimIndex( bodyB ) );
}
int activeSetCount = 0;
int totalBodyCount = 0;
int totalJointCount = 0;
@ -3626,7 +3641,7 @@ void b3ValidateSolverSets( b3World* world )
B3_ASSERT( body->setIndex == setIndex );
B3_ASSERT( body->localIndex == i );
uint32_t syncedFlags = body->flags & ~b3_bodyTransientFlags;
uint32_t syncedFlags = body->flags & ~( b3_isFast | b3_bodyTransientFlags );
B3_ASSERT( ( bodySim->flags & syncedFlags ) == syncedFlags );
b3BodyState* bodyState = b3GetBodyState( world, body );
@ -3965,30 +3980,6 @@ void b3ValidateContacts( b3World* world )
if ( touching )
{
B3_ASSERT( 0 <= contact->colorIndex && contact->colorIndex < B3_GRAPH_COLOR_COUNT );
// Validate body sim indices
b3Shape* shapeA = b3Array_Get( world->shapes, contact->shapeIdA );
b3Shape* shapeB = b3Array_Get( world->shapes, contact->shapeIdB );
b3Body* bodyA = b3Array_Get( world->bodies, shapeA->bodyId );
b3Body* bodyB = b3Array_Get( world->bodies, shapeB->bodyId );
if ( bodyA->type == b3_staticBody )
{
B3_ASSERT( contact->bodySimIndexA == B3_NULL_INDEX );
}
else
{
B3_ASSERT( contact->bodySimIndexA == bodyA->localIndex );
}
if ( bodyB->type == b3_staticBody )
{
B3_ASSERT( contact->bodySimIndexB == B3_NULL_INDEX );
}
else
{
B3_ASSERT( contact->bodySimIndexB == bodyB->localIndex );
}
if ( ( contact->flags & b3_simMeshContact ) != 0 || contact->colorIndex == B3_OVERFLOW_INDEX )
{

View file

@ -33,6 +33,7 @@ b3DeclareArray( b3SolverSet );
b3DeclareArray( b3Joint );
b3DeclareArray( b3Contact );
b3DeclareArray( b3Island );
b3DeclareArray( b3AABB );
b3DeclareArray( b3Shape );
b3DeclareArray( b3Sensor );
b3DeclareArray( b3SensorTaskContext );
@ -176,6 +177,9 @@ typedef struct b3World
// These are sparse arrays that point into the pools above
b3Array( b3Shape ) shapes;
// This follows the shape array and is split out for the hot path in the narrow phase.
b3Array( b3AABB ) fatAABBs;
// Compound shapes are static only, so only the broad-phase static pass can meet one. This
// lets a world without compounds skip the shape type read in the pair batch.
int compoundShapeCount;

View file

@ -132,3 +132,34 @@ static inline uint32_t b3AtomicFetchOrU32( uint32_t* a, uint32_t mask )
#error "Unsupported platform"
#endif
}
static inline int64_t b3AtomicFetchAddI64( b3AtomicI64* a, int64_t increment )
{
#if defined( _MSC_VER )
return (int64_t)_InterlockedExchangeAdd64( (__int64*)&a->value, (__int64)increment );
#elif defined( __GNUC__ ) || defined( __clang__ )
return __atomic_fetch_add( &a->value, increment, __ATOMIC_SEQ_CST );
#else
#error "Unsupported platform"
#endif
}
static inline int64_t b3AtomicLoadI64( b3AtomicI64* a )
{
#if defined( _MSC_VER ) && !defined( __clang__ ) && !defined( _M_ARM )
int64_t value = __iso_volatile_load64( (volatile __int64*)&a->value );
#if defined( _M_ARM64 ) || defined( _M_ARM64EC )
__dmb( 0xB );
#else
_ReadWriteBarrier();
#endif
return value;
#elif defined( _MSC_VER ) && !defined( __clang__ )
// 32-bit ARM has no plain atomic 64-bit load
return _InterlockedOr64( (__int64*)&a->value, 0 );
#elif defined( __GNUC__ ) || defined( __clang__ )
return __atomic_load_n( &a->value, __ATOMIC_SEQ_CST );
#else
#error "Unsupported platform"
#endif
}

View file

@ -51,8 +51,8 @@ typedef struct b3World b3World;
#define B3_REC_VERSION_MAJOR 6
// Minor tracks op-stream additions that keep the 48 byte header shape.
// Minor version 6 moved the broad phase to the tree against tree pair traversal
#define B3_REC_VERSION_MINOR 6
// Minor version 7 made the contact body sim locators persistent
#define B3_REC_VERSION_MINOR 7
// File header, fixed 48 bytes. Contains the registry locator so the player
// can load geometry before replaying any ops.

View file

@ -75,7 +75,7 @@ static float b3ComputeShapeMargin( b3Shape* shape )
return b3MinFloat( B3_MAX_AABB_MARGIN, B3_AABB_MARGIN_FRACTION * margin );
}
static void b3UpdateShapeAABBs( b3Shape* shape, b3WorldTransform transform, b3BodyType proxyType )
static void b3UpdateShapeAABBs( b3Shape* shape, b3AABB* fatAABB, b3WorldTransform transform, b3BodyType proxyType )
{
// Compute a bounding box with a speculative margin
const float speculativeDistance = B3_SPECULATIVE_DISTANCE;
@ -86,14 +86,7 @@ static void b3UpdateShapeAABBs( b3Shape* shape, b3WorldTransform transform, b3Bo
// Smaller margin for static bodies. Cannot be zero due to TOI tolerance.
float margin = proxyType == b3_staticBody ? speculativeDistance : aabbMargin;
b3AABB fatAABB;
fatAABB.lowerBound.x = aabb.lowerBound.x - margin;
fatAABB.lowerBound.y = aabb.lowerBound.y - margin;
fatAABB.lowerBound.z = aabb.lowerBound.z - margin;
fatAABB.upperBound.x = aabb.upperBound.x + margin;
fatAABB.upperBound.y = aabb.upperBound.y + margin;
fatAABB.upperBound.z = aabb.upperBound.z + margin;
shape->fatAABB = fatAABB;
*fatAABB = b3AABB_Inflate( aabb, margin );
}
static b3Shape* b3CreateShapeInternal( b3World* world, b3Body* body, b3WorldTransform bodyTransform, const b3ShapeDef* def,
@ -105,12 +98,15 @@ static b3Shape* b3CreateShapeInternal( b3World* world, b3Body* body, b3WorldTran
if ( shapeId == world->shapes.count )
{
b3Array_Push( world->shapes, (b3Shape){ 0 } );
b3Array_Push( world->fatAABBs, (b3AABB){ 0 } );
}
else
{
B3_ASSERT( world->shapes.data[shapeId].id == B3_NULL_INDEX );
}
B3_ASSERT( world->fatAABBs.count == world->shapes.count );
b3Shape* shape = b3Array_Get( world->shapes, shapeId );
switch ( shapeType )
@ -186,7 +182,7 @@ static b3Shape* b3CreateShapeInternal( b3World* world, b3Body* body, b3WorldTran
shape->localCentroid = b3GetShapeCentroid( shape );
shape->aabbMargin = b3ComputeShapeMargin( shape );
shape->aabb = (b3AABB){ b3Vec3_zero, b3Vec3_zero };
shape->fatAABB = (b3AABB){ b3Vec3_zero, b3Vec3_zero };
world->fatAABBs.data[shapeId] = (b3AABB){ b3Vec3_zero, b3Vec3_zero };
shape->nameId = b3AddName( &world->names, def->name );
shape->generation += 1;
@ -220,7 +216,7 @@ static b3Shape* b3CreateShapeInternal( b3World* world, b3Body* body, b3WorldTran
{
b3BodyType proxyType = body->type;
bool forcePairCreation = def->invokeContactCreation && shape->type != b3_compoundShape;
b3CreateShapeProxy( shape, &world->broadPhase, proxyType, bodyTransform, forcePairCreation );
b3CreateShapeProxy( world, shape, proxyType, bodyTransform, forcePairCreation );
}
// Add to shape doubly linked list
@ -1004,15 +1000,16 @@ int b3CollideMover( b3PlaneResult* planes, int planeCapacity, const b3Shape* sha
return planeCount;
}
void b3CreateShapeProxy( b3Shape* shape, b3BroadPhase* bp, b3BodyType type, b3WorldTransform transform, bool forcePairCreation )
void b3CreateShapeProxy( b3World* world, b3Shape* shape, b3BodyType type, b3WorldTransform transform, bool forcePairCreation )
{
B3_ASSERT( shape->proxyKey == B3_NULL_INDEX );
b3UpdateShapeAABBs( shape, transform, type );
b3AABB* fatAABB = world->fatAABBs.data + shape->id;
b3UpdateShapeAABBs( shape, fatAABB, transform, type );
// Create proxies in the broad-phase.
shape->proxyKey =
b3BroadPhase_CreateProxy( bp, type, shape->fatAABB, shape->filter.categoryBits, shape->id, forcePairCreation );
shape->proxyKey = b3BroadPhase_CreateProxy( &world->broadPhase, type, *fatAABB, shape->filter.categoryBits, shape->id,
forcePairCreation );
B3_ASSERT( B3_PROXY_TYPE( shape->proxyKey ) < b3_bodyTypeCount );
}
@ -1360,25 +1357,26 @@ static void b3ResetProxy( b3World* world, b3Shape* shape, bool wakeBodies, bool
if ( shape->proxyKey != B3_NULL_INDEX )
{
b3BodyType proxyType = B3_PROXY_TYPE( shape->proxyKey );
b3UpdateShapeAABBs( shape, transform, proxyType );
b3AABB* fatAABB = world->fatAABBs.data + shapeId;
b3UpdateShapeAABBs( shape, fatAABB, transform, proxyType );
if ( destroyProxy )
{
b3BroadPhase_DestroyProxy( &world->broadPhase, shape->proxyKey );
bool forcePairCreation = true;
shape->proxyKey = b3BroadPhase_CreateProxy( &world->broadPhase, proxyType, shape->fatAABB, shape->filter.categoryBits,
shape->proxyKey = b3BroadPhase_CreateProxy( &world->broadPhase, proxyType, *fatAABB, shape->filter.categoryBits,
shapeId, forcePairCreation );
}
else
{
b3BroadPhase_MoveProxy( &world->broadPhase, shape->proxyKey, shape->fatAABB );
b3BroadPhase_MoveProxy( &world->broadPhase, shape->proxyKey, *fatAABB );
}
}
else
{
b3BodyType proxyType = body->type;
b3UpdateShapeAABBs( shape, transform, proxyType );
b3UpdateShapeAABBs( shape, world->fatAABBs.data + shapeId, transform, proxyType );
}
b3ValidateSolverSets( world );

View file

@ -37,7 +37,6 @@ typedef struct b3Shape
float aabbMargin;
b3AABB aabb;
b3AABB fatAABB;
b3Vec3 localCentroid;
int materialCount;
@ -74,7 +73,7 @@ static inline b3SurfaceMaterial* b3GetShapeMaterials( const b3Shape* shape )
return shape->materials != NULL ? shape->materials : (b3SurfaceMaterial*)&shape->material;
}
void b3CreateShapeProxy( b3Shape* shape, b3BroadPhase* bp, b3BodyType type, b3WorldTransform transform, bool forcePairCreation );
void b3CreateShapeProxy( b3World* world, b3Shape* shape, b3BodyType type, b3WorldTransform transform, bool forcePairCreation );
void b3DestroyShapeProxy( b3Shape* shape, b3BroadPhase* bp );
void b3DestroyShapeAllocations( b3World* world, b3Shape* shape );

View file

@ -476,6 +476,20 @@ static inline b3FloatW b3OrW( b3FloatW a, b3FloatW b )
return vreinterpretq_f32_u32( vorrq_u32( vreinterpretq_u32_f32( a ), vreinterpretq_u32_f32( b ) ) );
}
// a & ~b
static inline b3FloatW b3AndNotW( b3FloatW a, b3FloatW b )
{
return vreinterpretq_f32_u32( vbicq_u32( vreinterpretq_u32_f32( a ), vreinterpretq_u32_f32( b ) ) );
}
static inline b3FloatW b3SoftMaskW( const int* indexA, const int* indexB )
{
int32x4_t zero = vdupq_n_s32( 0 );
uint32x4_t a = vceqq_s32( vld1q_s32( (const int32_t*)indexA ), zero );
uint32x4_t b = vceqq_s32( vld1q_s32( (const int32_t*)indexB ), zero );
return vreinterpretq_f32_u32( vorrq_u32( a, b ) );
}
static inline b3FloatW b3GreaterThanW( b3FloatW a, b3FloatW b )
{
return vreinterpretq_f32_u32( vcgtq_f32( a, b ) );
@ -650,6 +664,22 @@ static inline b3FloatW b3OrW( b3FloatW a, b3FloatW b )
return _mm_or_ps( a, b );
}
// a & ~b
static inline b3FloatW b3AndNotW( b3FloatW a, b3FloatW b )
{
// Arguments are reversed
return _mm_andnot_ps( b, a );
}
// This is used to optimize selection of contact softness.
static inline b3FloatW b3SoftMaskW( const int* indexA, const int* indexB )
{
__m128i zero = _mm_setzero_si128();
__m128i a = _mm_cmpeq_epi32( _mm_loadu_si128( (const __m128i*)indexA ), zero );
__m128i b = _mm_cmpeq_epi32( _mm_loadu_si128( (const __m128i*)indexB ), zero );
return _mm_castsi128_ps( _mm_or_si128( a, b ) );
}
static inline b3FloatW b3GreaterThanW( b3FloatW a, b3FloatW b )
{
return _mm_cmpgt_ps( a, b );
@ -834,6 +864,27 @@ static inline b3FloatW b3OrW( b3FloatW a, b3FloatW b )
return r;
}
// a & ~b
static inline b3FloatW b3AndNotW( b3FloatW a, b3FloatW b )
{
b3FloatW r;
r.x = a.x != 0.0f && b.x == 0.0f ? 1.0f : 0.0f;
r.y = a.y != 0.0f && b.y == 0.0f ? 1.0f : 0.0f;
r.z = a.z != 0.0f && b.z == 0.0f ? 1.0f : 0.0f;
r.w = a.w != 0.0f && b.w == 0.0f ? 1.0f : 0.0f;
return r;
}
static inline b3FloatW b3SoftMaskW( const int* indexA, const int* indexB )
{
b3FloatW r;
r.x = indexA[0] == 0 || indexB[0] == 0 ? 1.0f : 0.0f;
r.y = indexA[1] == 0 || indexB[1] == 0 ? 1.0f : 0.0f;
r.z = indexA[2] == 0 || indexB[2] == 0 ? 1.0f : 0.0f;
r.w = indexA[3] == 0 || indexB[3] == 0 ? 1.0f : 0.0f;
return r;
}
static inline b3FloatW b3GreaterThanW( b3FloatW a, b3FloatW b )
{
b3FloatW r;

View file

@ -24,8 +24,6 @@
#include <stddef.h>
#include <stdio.h>
_Static_assert( B3_RESTITUTION_ITERATIONS >= 1, "must be 1 or more" );
// these are useful for solver testing
#define ITERATIONS 1
#define RELAX_ITERATIONS 1
@ -473,7 +471,7 @@ static bool b3ContinuousQueryCallback( int proxyId, uint64_t userData, void* con
}
// Continuous collision of dynamic versus static
static void b3SolveContinuous( b3World* world, int bodySimIndex, b3TaskContext* taskContext )
static void b3SolveContinuous( b3World* world, int bodySimIndex, b3TaskContext* taskContext, float dt )
{
b3TracyCZoneNC( ccd, "CCD", b3_colorDarkGoldenRod, true );
@ -481,7 +479,7 @@ static void b3SolveContinuous( b3World* world, int bodySimIndex, b3TaskContext*
b3SolverSet* awakeSet = b3Array_Get( world->solverSets, b3_awakeSet );
b3BodySim* fastBodySim = b3Array_Get( awakeSet->bodySims, bodySimIndex );
B3_ASSERT( fastBodySim->flags & b3_isFast );
B3_VALIDATE( fastBodySim->flags & b3_isFast );
// Re-center the sweep on the fast body so the TOI and the swept query stay in float precision
b3Pos base = fastBodySim->center0;
@ -567,6 +565,16 @@ static void b3SolveContinuous( b3World* world, int bodySimIndex, b3TaskContext*
fastBodySim->rotation0 = q;
fastBodySim->center0 = center;
// Timeloss means there is a lost gravity contribution. Other forces and torques are ignored for now.
b3BodyState* fastBodyState = b3Array_Get( awakeSet->bodyStates, bodySimIndex );
b3Vec3 v = fastBodyState->linearVelocity;
float timeLoss = ( 1.0f - context.fraction ) * dt;
b3Vec3 dv = b3MulSV( -timeLoss * fastBodySim->gravityScale, world->gravity );
dv.x = ( fastBodyState->flags & b3_lockLinearX ) ? 0.0f : dv.x;
dv.y = ( fastBodyState->flags & b3_lockLinearY ) ? 0.0f : dv.y;
dv.z = ( fastBodyState->flags & b3_lockLinearZ ) ? 0.0f : dv.z;
fastBodyState->linearVelocity = b3Add( v, dv );
// The move event was written before CCD, so correct it with the impact pose
b3BodyMoveEvent* event = b3Array_Get( world->bodyMoveEvents, bodySimIndex );
event->transform = fastBodySim->transform;
@ -583,19 +591,20 @@ static void b3SolveContinuous( b3World* world, int bodySimIndex, b3TaskContext*
b3AABB aabb = b3ComputeFatShapeAABB( shape, transform, speculativeScalar );
shape->aabb = aabb;
if ( b3AABB_Contains( shape->fatAABB, aabb ) == false )
b3AABB* shapeFatAABB = world->fatAABBs.data + shapeId;
if ( b3AABB_Contains( *shapeFatAABB, aabb ) == false )
{
float marginScalar = shape->aabbMargin;
b3Vec3 aabbMargin = { marginScalar, marginScalar, marginScalar };
shape->fatAABB = (b3AABB){ b3Sub( aabb.lowerBound, aabbMargin ), b3Add( aabb.upperBound, aabbMargin ) };
fastBodySim->flags |= b3_enlargeBounds;
*shapeFatAABB = b3AABB_Inflate( aabb, shape->aabbMargin );
// Regular bodies mark the hierarchy as moved using atomic operations.
// Bullets are handled separately at a later stage.
if ( isBullet == false )
{
b3BroadPhase_MarkProxyMoved( &world->broadPhase, shape->proxyKey, shape->fatAABB );
b3BroadPhase_MarkProxyMoved( &world->broadPhase, shape->proxyKey, *shapeFatAABB );
}
else
{
fastBodySim->flags |= b3_enlargeBulletBounds;
}
}
@ -618,20 +627,18 @@ static void b3SolveContinuous( b3World* world, int bodySimIndex, b3TaskContext*
// shape->aabb is still valid from above
if ( b3AABB_Contains( shape->fatAABB, shape->aabb ) == false )
b3AABB* shapeFatAABB = world->fatAABBs.data + shapeId;
if ( b3AABB_Contains( *shapeFatAABB, shape->aabb ) == false )
{
float marginScalar = shape->aabbMargin;
b3Vec3 aabbMargin = { marginScalar, marginScalar, marginScalar };
shape->fatAABB = (b3AABB){
.lowerBound = b3Sub( shape->aabb.lowerBound, aabbMargin ),
.upperBound = b3Add( shape->aabb.upperBound, aabbMargin ),
};
fastBodySim->flags |= b3_enlargeBounds;
*shapeFatAABB = b3AABB_Inflate( shape->aabb, shape->aabbMargin );
if ( isBullet == false )
{
b3BroadPhase_MarkProxyMoved( &world->broadPhase, shape->proxyKey, shape->fatAABB );
b3BroadPhase_MarkProxyMoved( &world->broadPhase, shape->proxyKey, *shapeFatAABB );
}
else
{
fastBodySim->flags |= b3_enlargeBulletBounds;
}
}
@ -754,10 +761,13 @@ static void b3FinalizeBodiesTask( int startIndex, int endIndex, int workerIndex,
// or b3Body_SetMassData.
B3_ASSERT( ( body->flags & b3_dirtyMass ) == 0 );
// Clear the transient flags (fast, speed capped, had TOI). These flags are conditionally set
// as part of the code below.
body->flags &= ~b3_bodyTransientFlags;
body->flags |= ( sim->flags & ( b3_isSpeedCapped | b3_hadTimeOfImpact ) );
body->flags |= ( state->flags & ( b3_isSpeedCapped | b3_hadTimeOfImpact ) );
sim->flags &= ~b3_bodyTransientFlags;
sim->flags &= ~( b3_isFast | b3_bodyTransientFlags );
// The body state flag knows about speed capping (used for debug draw).
body->flags |= ( state->flags & b3_isSpeedCapped );
state->flags &= ~b3_bodyTransientFlags;
if ( enableSleep == false || ( body->flags & b3_enableSleep ) == 0 || sleepVelocity > body->sleepThreshold )
@ -781,7 +791,7 @@ static void b3FinalizeBodiesTask( int startIndex, int endIndex, int workerIndex,
}
else
{
b3SolveContinuous( world, simIndex, taskContext );
b3SolveContinuous( world, simIndex, taskContext, timeStep );
}
}
else
@ -839,14 +849,13 @@ static void b3FinalizeBodiesTask( int startIndex, int endIndex, int workerIndex,
b3AABB aabb = b3ComputeFatShapeAABB( shape, transform, speculativeScalar );
shape->aabb = aabb;
if ( b3AABB_Contains( shape->fatAABB, aabb ) == false )
b3AABB* shapeFatAABB = world->fatAABBs.data + shapeId;
if ( b3AABB_Contains( *shapeFatAABB, aabb ) == false )
{
float marginScalar = shape->aabbMargin;
b3Vec3 aabbMargin = { marginScalar, marginScalar, marginScalar };
shape->fatAABB = (b3AABB){ b3Sub( aabb.lowerBound, aabbMargin ), b3Add( aabb.upperBound, aabbMargin ) };
*shapeFatAABB = b3AABB_Inflate( aabb, shape->aabbMargin );
// Mark the hierarchy as moved using atomic operations.
b3BroadPhase_MarkProxyMoved( &world->broadPhase, shape->proxyKey, shape->fatAABB );
b3BroadPhase_MarkProxyMoved( &world->broadPhase, shape->proxyKey, *shapeFatAABB );
}
shapeId = shape->nextShapeId;
@ -1006,13 +1015,11 @@ static void b3ExecuteBlock( b3SolverStage* stage, b3StepContext* context, b3Solv
}
else if ( blockType == b3_graphWideContactBlock )
{
bool useBias = true;
b3SolveContacts_Convex( block, context, useBias );
b3PushContacts_Convex( block, context );
}
else
{
bool useBias = true;
b3SolveContacts_Mesh( block, context, useBias );
b3PushContacts_Mesh( block, context );
}
break;
@ -1028,24 +1035,11 @@ static void b3ExecuteBlock( b3SolverStage* stage, b3StepContext* context, b3Solv
}
else if ( blockType == b3_graphWideContactBlock )
{
bool useBias = false;
b3SolveContacts_Convex( block, context, useBias );
b3SolveContacts_Convex( block, context );
}
else
{
bool useBias = false;
b3SolveContacts_Mesh( block, context, useBias );
}
break;
case b3_stageRestitution:
if ( blockType == b3_graphWideContactBlock )
{
b3ApplyRestitution_Convex( block, context );
}
else if ( blockType == b3_graphContactBlock )
{
b3ApplyRestitution_Mesh( block, context );
b3SolveContacts_Mesh( block, context );
}
break;
@ -1188,7 +1182,6 @@ static void b3SolverTask( void* taskContext )
b3_stageSolve,
b3_stageIntegratePositions,
b3_stageRelax,
b3_stageRestitution,
b3_stageStoreImpulses
*/
@ -1265,7 +1258,7 @@ static void b3SolverTask( void* taskContext )
{
// Overflow constraints have lower priority. Typically these are dynamic-vs-dynamic.
b3SolveJoints_Overflow( context, useBias );
b3SolveContacts_Overflow( context, useBias );
b3PushContacts_Overflow( context );
for ( int colorIndex = 0; colorIndex < activeColorCount; ++colorIndex )
{
@ -1293,7 +1286,7 @@ static void b3SolverTask( void* taskContext )
for ( int j = 0; j < RELAX_ITERATIONS; ++j )
{
b3SolveJoints_Overflow( context, useBias );
b3SolveContacts_Overflow( context, useBias );
b3SolveContacts_Overflow( context );
for ( int colorIndex = 0; colorIndex < activeColorCount; ++colorIndex )
{
@ -1312,25 +1305,6 @@ static void b3SolverTask( void* taskContext )
// integrate velocities / warm start / solve / integrate positions / relax
stageIndex += 1 + activeColorCount + ITERATIONS * activeColorCount + 1 + RELAX_ITERATIONS * activeColorCount;
// Restitution
for ( int iteration = 0; iteration < B3_RESTITUTION_ITERATIONS; ++iteration )
{
b3ApplyRestitution_Overflow( context );
int iterStageIndex = stageIndex;
for ( int colorIndex = 0; colorIndex < activeColorCount; ++colorIndex )
{
syncBits = ( graphSyncIndex << 16 ) | iterStageIndex;
B3_ASSERT( stages[iterStageIndex].type == b3_stageRestitution );
b3ExecuteMainStage( stages + iterStageIndex, context, syncBits );
iterStageIndex += 1;
}
graphSyncIndex += 1;
stageIndex += activeColorCount;
}
profile->applyRestitution += b3GetMillisecondsAndReset( &ticks );
// Store impulses
b3StoreImpulses_Overflow( context );
@ -1420,7 +1394,7 @@ static void b3BulletBodyTask( int startIndex, int endIndex, int workerIndex, voi
for ( int i = startIndex; i < endIndex; ++i )
{
int simIndex = stepContext->bulletBodies[i];
b3SolveContinuous( stepContext->world, simIndex, taskContext );
b3SolveContinuous( stepContext->world, simIndex, taskContext, stepContext->dt );
}
b3TracyCZoneEnd( bullet_body_task );
@ -1689,8 +1663,6 @@ void b3Solve( b3World* world, b3StepContext* stepContext )
stageCount += 1;
// b3_stageRelax
stageCount += RELAX_ITERATIONS * activeColorCount;
// b3_stageRestitution
stageCount += B3_RESTITUTION_ITERATIONS * activeColorCount;
// b3_stageStoreWideImpulses
stageCount += 1;
// b3_stageStoreImpulses
@ -1774,9 +1746,6 @@ 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 );
// Note: joint blocks mixed in, could have joint limit restitution
stage = b3InitColorStages( stage, b3_stageRestitution, B3_RESTITUTION_ITERATIONS, activeColorCount, graphColorBlocks,
graphBlockCounts, activeColorIndices );
stage = b3InitStage( stage, b3_stageStoreWideImpulses, convexBlocks, convexPrepareDim.count, UINT8_MAX );
stage = b3InitStage( stage, b3_stageStoreImpulses, meshBlocks, meshPrepareDim.count, UINT8_MAX );
@ -2117,13 +2086,13 @@ void b3Solve( b3World* world, b3StepContext* stepContext )
for ( int i = 0; i < bulletBodyCount; ++i )
{
b3BodySim* bulletBodySim = bodySimArray + bulletBodySimIndices[i];
if ( ( bulletBodySim->flags & b3_enlargeBounds ) == 0 )
if ( ( bulletBodySim->flags & b3_enlargeBulletBounds ) == 0 )
{
continue;
}
// Clear flag
bulletBodySim->flags &= ~b3_enlargeBounds;
bulletBodySim->flags &= ~b3_enlargeBulletBounds;
int bodyId = bulletBodySim->bodyId;
B3_ASSERT( 0 <= bodyId && bodyId < world->bodies.count );
@ -2139,10 +2108,11 @@ void b3Solve( b3World* world, b3StepContext* stepContext )
B3_VALIDATE( B3_PROXY_TYPE( proxyKey ) == b3_dynamicBody );
b3AABB treeAABB = b3DynamicTree_GetAABB( dynamicTree, proxyId );
b3AABB shapeFatAABB = world->fatAABBs.data[shapeId];
if ( b3AABB_Contains( treeAABB, shape->fatAABB ) == false )
if ( b3AABB_Contains( treeAABB, shapeFatAABB ) == false )
{
b3DynamicTree_EnlargeProxy( dynamicTree, proxyId, shape->fatAABB );
b3DynamicTree_EnlargeProxy( dynamicTree, proxyId, shapeFatAABB );
}
shapeId = shape->nextShapeId;

View file

@ -76,7 +76,6 @@ typedef enum b3SolverStageType
b3_stageSolve,
b3_stageIntegratePositions,
b3_stageRelax,
b3_stageRestitution,
b3_stageStoreWideImpulses,
b3_stageStoreImpulses,
} b3SolverStageType;

View file

@ -62,7 +62,8 @@ void b3WakeSolverSet( b3World* world, int setIndex )
*state = b3_identityBodyState;
state->flags = body->flags;
// move non-touching contacts from disabled set to awake set
// Move non-touching contacts from disabled set to awake set.
int encodedBodySimIndex = body->localIndex;
int contactKey = body->headContactKey;
while ( contactKey != B3_NULL_INDEX )
{
@ -75,6 +76,21 @@ void b3WakeSolverSet( b3World* world, int setIndex )
if ( contact->setIndex != b3_disabledSet )
{
B3_ASSERT( contact->setIndex == b3_awakeSet || contact->setIndex == setIndex );
// If this contact is awake but not touching I need to fixup the body sim index here.
if ( contact->setIndex == b3_awakeSet )
{
B3_ASSERT( contact->colorIndex == B3_NULL_INDEX );
if ( edgeIndex == 0 )
{
contact->encodedBodySimA = encodedBodySimIndex;
}
else
{
contact->encodedBodySimB = encodedBodySimIndex;
}
}
continue;
}
@ -89,6 +105,15 @@ void b3WakeSolverSet( b3World* world, int setIndex )
contact->localIndex = awakeSet->contactIndices.count;
b3Array_Push( awakeSet->contactIndices, contactId );
if ( edgeIndex == 0 )
{
contact->encodedBodySimA = encodedBodySimIndex;
}
else
{
contact->encodedBodySimB = encodedBodySimIndex;
}
int movedLocalIndex = b3Array_RemoveSwap( disabledSet->contactIndices, localIndex );
if ( movedLocalIndex != B3_NULL_INDEX )
{
@ -227,6 +252,11 @@ void b3TrySleepIsland( b3World* world, int islandId )
b3Body* movedBody = b3Array_Get( world->bodies, movedId );
B3_ASSERT( movedBody->localIndex == movedIndex );
movedBody->localIndex = awakeBodyIndex;
if ( movedBody->islandId != islandId )
{
b3RefreshBodyContactIndices( world, movedBody );
}
}
// destroy state, no need to clone
@ -245,14 +275,9 @@ void b3TrySleepIsland( b3World* world, int islandId )
b3Contact* contact = b3Array_Get( world->contacts, contactId );
B3_ASSERT( contact->setIndex == b3_awakeSet || contact->setIndex == b3_disabledSet );
contactKey = contact->edges[edgeIndex].nextKey;
B3_ASSERT( contact->setIndex == b3_awakeSet );
if ( contact->setIndex == b3_disabledSet )
{
// already moved to disabled set by another body in the island
continue;
}
contactKey = contact->edges[edgeIndex].nextKey;
if ( contact->colorIndex != B3_NULL_INDEX )
{
@ -261,6 +286,15 @@ void b3TrySleepIsland( b3World* world, int islandId )
continue;
}
if ( edgeIndex == 0 )
{
contact->encodedBodySimA = B3_NULL_INDEX;
}
else
{
contact->encodedBodySimB = B3_NULL_INDEX;
}
// the other body may still be awake, it still may go to sleep and then it will be responsible
// for moving this contact to the disabled set.
int otherEdgeIndex = edgeIndex ^ 1;
@ -352,6 +386,8 @@ void b3TrySleepIsland( b3World* world, int islandId )
contact->setIndex = sleepSetId;
contact->colorIndex = B3_NULL_INDEX;
contact->localIndex = sleepContactIndex;
contact->encodedBodySimA = b3SleepBodySimIndex( contact->encodedBodySimA );
contact->encodedBodySimB = b3SleepBodySimIndex( contact->encodedBodySimB );
}
}
@ -530,6 +566,8 @@ void b3MergeSolverSets( b3World* world, int setId1, int setId2 )
void b3TransferBody( b3World* world, b3SolverSet* targetSet, b3SolverSet* sourceSet, b3Body* body )
{
B3_ASSERT( body->headContactKey == B3_NULL_INDEX );
if ( targetSet == sourceSet )
{
return;
@ -543,7 +581,8 @@ void b3TransferBody( b3World* world, b3SolverSet* targetSet, b3SolverSet* source
memcpy( targetSim, sourceSim, sizeof( b3BodySim ) );
// Clear transient body flags
targetSim->flags &= ~( b3_isFast | b3_isSpeedCapped | b3_hadTimeOfImpact );
body->flags &= ~b3_bodyTransientFlags;
targetSim->flags &= ~(b3_isFast | b3_bodyTransientFlags);
// Remove body sim from solver set that owns it
int movedIndex = b3Array_RemoveSwap( sourceSet->bodySims, sourceIndex );
@ -555,6 +594,7 @@ void b3TransferBody( b3World* world, b3SolverSet* targetSet, b3SolverSet* source
b3Body* movedBody = b3Array_Get( world->bodies, movedId );
B3_ASSERT( movedBody->localIndex == movedIndex );
movedBody->localIndex = sourceIndex;
b3RefreshBodyContactIndices( world, movedBody );
}
if ( sourceSet->setIndex == b3_awakeSet )

View file

@ -33,7 +33,7 @@
// Snapshot image magic 'BNS3' and version
#define B3_SNAP_MAGIC 0x33534E42u
#define B3_SNAP_VERSION 5u // broad-phase pair traversal, move buffer removed
#define B3_SNAP_VERSION 7u // same-step restitution
#define B3_SNAP_FLAG_VALIDATION 0x1u
#define B3_SNAP_FLAG_DOUBLE_PRECISION 0x2u
@ -831,8 +831,6 @@ static void b3SerContacts( b3RecBuffer* buf, b3World* world )
// Write raw struct with pointer fields zeroed
b3Contact copy = *c;
copy.manifolds = NULL;
copy.bodySimIndexA = B3_NULL_INDEX;
copy.bodySimIndexB = B3_NULL_INDEX;
if ( copy.flags & b3_simMeshContact )
{
copy.meshContact.triangleCache.data = NULL;
@ -889,8 +887,6 @@ static void b3DesContacts( b3SnapReader* r, b3World* world )
b3Contact* dst = world->contacts.data + i;
b3SnapR_Bytes( r, dst, sizeof( b3Contact ) );
dst->manifolds = NULL;
dst->bodySimIndexA = B3_NULL_INDEX;
dst->bodySimIndexB = B3_NULL_INDEX;
if ( dst->flags & b3_simMeshContact )
{
dst->meshContact.triangleCache.data = NULL;
@ -1062,6 +1058,8 @@ int b3SerializeWorld( b3World* world, b3RecBuffer* buf, b3Recording* rec )
// Shape sparse array with geometry interning
b3SerShapes( buf, world, rec );
b3SerPodArray( buf, world->fatAABBs );
// Contact sparse array with manifold and mesh triangleCache
b3SerContacts( buf, world );
@ -1243,6 +1241,13 @@ bool b3DeserializeIntoShell( const uint8_t* data, int size, b3World* world, b3Re
return false;
}
b3DesPodArray( r, world->fatAABBs );
if ( !r->ok )
{
return false;
}
// 6. Contact sparse array
b3DesContacts( r, world );

View file

@ -25,6 +25,7 @@ set(BOX3D_TEST_FILES
test_mover.c
test_name_cache.c
test_recording.c
test_restitution.c
test_sat.c
test_shape.c
test_table.c

View file

@ -47,6 +47,7 @@ extern int MeshTest( void );
extern int MoverTest( void );
extern int NameCacheTest( void );
extern int RecordingTest( void );
extern int RestitutionTest( void );
extern int SeparatingAxisTest( void );
extern int ShapeTest( void );
extern int TableTest( void );
@ -123,6 +124,7 @@ int main( int argc, char** argv )
MAYBE_RUN_TEST( MoverTest );
MAYBE_RUN_TEST( NameCacheTest );
MAYBE_RUN_TEST( RecordingTest );
MAYBE_RUN_TEST( RestitutionTest );
MAYBE_RUN_TEST( SeparatingAxisTest );
MAYBE_RUN_TEST( ShapeTest );
MAYBE_RUN_TEST( TableTest );

View file

@ -18,27 +18,27 @@
// 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 277
#define RAGDOLL_HASH 0xF7B5B2D1
#define RAGDOLL_SLEEP_STEP 272
#define RAGDOLL_HASH 0xC049067A
#define WAVE_PILE_SLEEP_STEP 323
#define WAVE_PILE_HASH 0x2200A547
#define QUERY_SPAWN_SLEEP_STEP 242
#define QUERY_SPAWN_HASH 0x1737F5BC
#define QUERY_SPAWN_HIT_COUNT 59
#define QUERY_SPAWN_QUERY_HASH 0x5B4429DC
#define MESH_DROP_SLEEP_STEP 251
#define MESH_DROP_HASH 0x465381C5
#define MESH_DROP_SLEEP_STEP 217
#define MESH_DROP_HASH 0xCF946437
#else
#define RAGDOLL_SLEEP_STEP 272
#define RAGDOLL_HASH 0xF4992B41
#define RAGDOLL_SLEEP_STEP 271
#define RAGDOLL_HASH 0x805719D3
#define WAVE_PILE_SLEEP_STEP 316
#define WAVE_PILE_HASH 0xAA7D15A7
#define QUERY_SPAWN_SLEEP_STEP 242
#define QUERY_SPAWN_HASH 0xB9F993A5
#define QUERY_SPAWN_HIT_COUNT 59
#define QUERY_SPAWN_QUERY_HASH 0xE3271F3D
#define MESH_DROP_SLEEP_STEP 251
#define MESH_DROP_HASH 0xE58C7240
#define MESH_DROP_SLEEP_STEP 217
#define MESH_DROP_HASH 0x256BC40B
#endif
// The goldens above pin exact values for the default four point manifold. A build that

View file

@ -12,7 +12,7 @@
// Direct unit test of the cache: dedup, reverse lookup, empty/null, arbitrary length, no leak.
static int CacheUnit( void )
{
int32_t base = b3GetByteCount();
int64_t base = b3GetByteCount();
b3NameCache cache = b3CreateNameCache();
@ -155,7 +155,7 @@ static int NameRoundTrip( void )
// the tracked byte count to return to baseline so a re-load that drops duplicate copies leaks nothing.
static int RollbackNames( void )
{
int32_t base = b3GetByteCount();
int64_t base = b3GetByteCount();
b3Recording* rec = b3CreateRecording( 0 );
ENSURE( rec != NULL );

1135
test/test_restitution.c Normal file

File diff suppressed because it is too large Load diff

View file

@ -2,9 +2,11 @@
// SPDX-License-Identifier: MIT
#include "benchmarks.h"
#include "body.h"
#include "dynamic_tree.h"
#include "overflow_color.h"
#include "physics_world.h"
#include "recording.h"
#include "test_macros.h"
#include "box3d/box3d.h"
@ -1276,6 +1278,348 @@ static int TestContinuousMoveEvent( void )
return 0;
}
// The contact body sim locators are persistent state. Every contact in every set carries the
// current encoding for both of its bodies: the awake local index, a static tag, or null for a
// body that fell asleep. The scenes below drive each site that can invalidate one. The gate is
// b3ValidateSolverSets, which asserts the invariant on every mutation and every step, so a
// dropped refresh trips there rather than producing a plausible but wrong simulation. Each scene
// also runs at worker counts 1 and 4 with the state hashes compared.
static b3WorldId CreateLocatorWorld( int workerCount )
{
b3WorldDef worldDef = b3DefaultWorldDef();
worldDef.workerCount = workerCount;
return b3CreateWorld( &worldDef );
}
static b3BodyId CreateLocatorBody( b3WorldId worldId, b3BodyType type, b3Pos position, float hx, float hy, float hz )
{
b3BodyDef bodyDef = b3DefaultBodyDef();
bodyDef.type = type;
bodyDef.position = position;
b3BodyId bodyId = b3CreateBody( worldId, &bodyDef );
b3ShapeDef shapeDef = b3DefaultShapeDef();
b3BoxHull box = b3MakeBoxHull( hx, hy, hz );
b3CreateHullShape( bodyId, &shapeDef, &box.base );
return bodyId;
}
static void StepLocatorWorld( b3WorldId worldId, int stepCount )
{
for ( int i = 0; i < stepCount; ++i )
{
b3World_Step( worldId, 1.0f / 60.0f, 4 );
}
}
// Steps until the body sleeps or the budget runs out. Returns the steps taken.
static int StepUntilAsleep( b3WorldId worldId, b3BodyId bodyId, int maxSteps )
{
for ( int i = 0; i < maxSteps; ++i )
{
b3World_Step( worldId, 1.0f / 60.0f, 4 );
if ( b3Body_IsAwake( bodyId ) == false )
{
return i + 1;
}
}
return maxSteps;
}
// Sleep and wake: first touch puts contacts in the graph, sleeping nulls the locators of the
// island bodies, waking restores their awake indices. The plate beside the stack holds a
// non-touching contact across the whole cycle, and that is the contact the wake path has to fix up
// itself. A touching contact would get a fresh encoding from the graph add and hide a dropped
// refresh.
static uint64_t LocatorSleepWakeScene( int workerCount, int* failed )
{
b3WorldId worldId = CreateLocatorWorld( workerCount );
CreateLocatorBody( worldId, b3_staticBody, (b3Pos){ 0.0f, -1.0f, 0.0f }, 5.0f, 1.0f, 5.0f );
b3BodyId stackId[3];
for ( int i = 0; i < 3; ++i )
{
stackId[i] = CreateLocatorBody( worldId, b3_dynamicBody, (b3Pos){ 0.0f, 0.5f + i, 0.0f }, 0.5f, 0.5f, 0.5f );
}
// Beside the top box, close enough that the fat bounds overlap and far enough that the
// shapes never touch. Kinematic and awake, so the contact stays in the awake set while the
// stack sleeps. Off to the side so it does not catch the body dropped later.
b3BodyId plateId = CreateLocatorBody( worldId, b3_kinematicBody, (b3Pos){ 1.04f, 2.5f, 0.0f }, 0.5f, 0.5f, 0.5f );
b3Body_EnableSleep( plateId, false );
int steps = StepUntilAsleep( worldId, stackId[0], 300 );
if ( steps == 300 || b3Body_IsAwake( stackId[2] ) == true )
{
*failed = 1;
}
// Wake without contact so the non-touching contact keeps its stale locator if the wake
// path forgets to refresh
b3Body_SetAwake( stackId[0], true );
StepLocatorWorld( worldId, 5 );
if ( b3Body_IsAwake( stackId[2] ) == false )
{
*failed = 1;
}
steps = StepUntilAsleep( worldId, stackId[0], 300 );
if ( steps == 300 )
{
*failed = 1;
}
// Dropping a body on the pile drives the wake path through first touch. The pile can settle
// and sleep again, so catch the wake as it happens.
CreateLocatorBody( worldId, b3_dynamicBody, (b3Pos){ 0.0f, 8.0f, 0.0f }, 0.5f, 0.5f, 0.5f );
bool woke = false;
for ( int i = 0; i < 180; ++i )
{
b3World_Step( worldId, 1.0f / 60.0f, 4 );
woke = woke || b3Body_IsAwake( stackId[0] );
}
if ( woke == false )
{
*failed = 1;
}
uint64_t hash = b3HashWorldState( b3GetWorldFromId( worldId ) );
b3DestroyWorld( worldId );
return hash;
}
// An island falling asleep and a body leaving the awake set both swap the last awake sim into
// the hole. The pile that never sleeps supplies a moved body that still owns contacts.
static uint64_t LocatorAwakeNeighborScene( int workerCount, int* failed )
{
b3WorldId worldId = CreateLocatorWorld( workerCount );
CreateLocatorBody( worldId, b3_staticBody, (b3Pos){ 0.0f, -1.0f, 0.0f }, 30.0f, 1.0f, 5.0f );
// This island sleeps
b3BodyId sleeperId[2];
for ( int i = 0; i < 2; ++i )
{
sleeperId[i] = CreateLocatorBody( worldId, b3_dynamicBody, (b3Pos){ -10.0f, 0.5f + i, 0.0f }, 0.5f, 0.5f, 0.5f );
}
// Created after the sleepers so its sims sit at the end of the awake set and get swapped
// into the holes the sleeping island leaves behind
b3BodyId spareId = CreateLocatorBody( worldId, b3_dynamicBody, (b3Pos){ 20.0f, 0.5f, 0.0f }, 0.5f, 0.5f, 0.5f );
b3Body_EnableSleep( spareId, false );
b3BodyId awakeId[3];
for ( int i = 0; i < 3; ++i )
{
awakeId[i] = CreateLocatorBody( worldId, b3_dynamicBody, (b3Pos){ 10.0f, 0.5f + i, 0.0f }, 0.5f, 0.5f, 0.5f );
b3Body_EnableSleep( awakeId[i], false );
}
int steps = StepUntilAsleep( worldId, sleeperId[0], 300 );
if ( steps == 300 || b3Body_IsAwake( awakeId[0] ) == false )
{
*failed = 1;
}
StepLocatorWorld( worldId, 10 );
// Leaving the awake set swaps a body that owns contacts into the spare's slot
b3Body_Disable( spareId );
StepLocatorWorld( worldId, 10 );
b3Body_Enable( spareId );
StepLocatorWorld( worldId, 10 );
uint64_t hash = b3HashWorldState( b3GetWorldFromId( worldId ) );
b3DestroyWorld( worldId );
return hash;
}
// Destroying a body swaps the last sim of its set into the hole. The moved body keeps its
// contacts, so its side of each one has to follow it.
static uint64_t LocatorDestroyScene( int workerCount, int* failed )
{
b3WorldId worldId = CreateLocatorWorld( workerCount );
CreateLocatorBody( worldId, b3_staticBody, (b3Pos){ 0.0f, -1.0f, 0.0f }, 5.0f, 1.0f, 5.0f );
b3BodyId stackId[4];
for ( int i = 0; i < 4; ++i )
{
stackId[i] = CreateLocatorBody( worldId, b3_dynamicBody, (b3Pos){ 0.0f, 0.5f + i, 0.0f }, 0.5f, 0.5f, 0.5f );
}
StepLocatorWorld( worldId, 30 );
// The last awake sim moves into this hole while its contacts live
b3DestroyBody( stackId[0] );
StepLocatorWorld( worldId, 30 );
// Three static boxes, one resting box each. Destroying the first moves the last static sim.
b3BodyId staticId[3];
for ( int i = 0; i < 3; ++i )
{
float x = 10.0f + 4.0f * i;
staticId[i] = CreateLocatorBody( worldId, b3_staticBody, (b3Pos){ x, -1.0f, 0.0f }, 1.0f, 1.0f, 1.0f );
CreateLocatorBody( worldId, b3_dynamicBody, (b3Pos){ x, 0.5f, 0.0f }, 0.5f, 0.5f, 0.5f );
}
StepLocatorWorld( worldId, 30 );
b3DestroyBody( staticId[0] );
StepLocatorWorld( worldId, 30 );
B3_UNUSED( failed );
uint64_t hash = b3HashWorldState( b3GetWorldFromId( worldId ) );
b3DestroyWorld( worldId );
return hash;
}
// A kinematic body parked a hair above a pile keeps a non-touching contact to a body that falls
// asleep, which is the one case the locator cannot name and has to leave to the cold path.
static uint64_t LocatorKinematicScene( int workerCount, int* failed )
{
b3WorldId worldId = CreateLocatorWorld( workerCount );
CreateLocatorBody( worldId, b3_staticBody, (b3Pos){ 0.0f, -1.0f, 0.0f }, 5.0f, 1.0f, 5.0f );
b3BodyId boxId = CreateLocatorBody( worldId, b3_dynamicBody, (b3Pos){ 0.0f, 0.5f, 0.0f }, 0.5f, 0.5f, 0.5f );
// Close enough that the fat bounds overlap and a non-touching contact exists
b3BodyId plateId = CreateLocatorBody( worldId, b3_kinematicBody, (b3Pos){ 0.0f, 1.52f, 0.0f }, 1.0f, 0.5f, 1.0f );
int steps = StepUntilAsleep( worldId, boxId, 300 );
if ( steps == 300 )
{
*failed = 1;
}
// Drive the plate down to touch and wake the sleeper
b3Body_SetLinearVelocity( plateId, (b3Vec3){ 0.0f, -1.0f, 0.0f } );
StepLocatorWorld( worldId, 60 );
if ( b3Body_IsAwake( boxId ) == false )
{
*failed = 1;
}
uint64_t hash = b3HashWorldState( b3GetWorldFromId( worldId ) );
b3DestroyWorld( worldId );
return hash;
}
// Set type and disable move a body between solver sets, which changes the encoding of every
// contact it has. A joint across two sleeping sets drives the merge path.
static uint64_t LocatorTransferScene( int workerCount, int* failed )
{
b3WorldId worldId = CreateLocatorWorld( workerCount );
CreateLocatorBody( worldId, b3_staticBody, (b3Pos){ 0.0f, -1.0f, 0.0f }, 20.0f, 1.0f, 5.0f );
b3BodyId boxId = CreateLocatorBody( worldId, b3_dynamicBody, (b3Pos){ 0.0f, 0.5f, 0.0f }, 0.5f, 0.5f, 0.5f );
StepLocatorWorld( worldId, 30 );
b3Body_SetType( boxId, b3_staticBody );
StepLocatorWorld( worldId, 5 );
b3Body_SetType( boxId, b3_dynamicBody );
StepLocatorWorld( worldId, 5 );
b3Body_Disable( boxId );
StepLocatorWorld( worldId, 5 );
b3Body_Enable( boxId );
StepLocatorWorld( worldId, 30 );
// Two piles far enough apart to sleep in separate sets
b3BodyId sleeperId[2];
sleeperId[0] = CreateLocatorBody( worldId, b3_dynamicBody, (b3Pos){ -12.0f, 0.5f, 0.0f }, 0.5f, 0.5f, 0.5f );
sleeperId[1] = CreateLocatorBody( worldId, b3_dynamicBody, (b3Pos){ 12.0f, 0.5f, 0.0f }, 0.5f, 0.5f, 0.5f );
int steps = StepUntilAsleep( worldId, sleeperId[0], 300 );
if ( steps == 300 || b3Body_IsAwake( sleeperId[1] ) == true )
{
*failed = 1;
}
b3SphericalJointDef jointDef = b3DefaultSphericalJointDef();
jointDef.base.bodyIdA = sleeperId[0];
jointDef.base.bodyIdB = sleeperId[1];
b3CreateSphericalJoint( worldId, &jointDef );
StepLocatorWorld( worldId, 30 );
uint64_t hash = b3HashWorldState( b3GetWorldFromId( worldId ) );
b3DestroyWorld( worldId );
return hash;
}
// The fast flag lives on the body sim and gates contact recycling, but a flag sync rebuilds the
// sim flags from the body row, which never carries it. A public setter called on a body in
// flight must not drop it, or the recycle path can take a fast pair.
static int LocatorFastFlagTest( void )
{
b3WorldDef worldDef = b3DefaultWorldDef();
b3WorldId worldId = b3CreateWorld( &worldDef );
CreateLocatorBody( worldId, b3_staticBody, (b3Pos){ 0.0f, -1.0f, 0.0f }, 5.0f, 1.0f, 5.0f );
b3BodyDef bodyDef = b3DefaultBodyDef();
bodyDef.type = b3_dynamicBody;
bodyDef.position = (b3Pos){ -20.0f, 2.0f, 0.0f };
bodyDef.linearVelocity = (b3Vec3){ 400.0f, 0.0f, 0.0f };
b3BodyId bulletId = b3CreateBody( worldId, &bodyDef );
b3ShapeDef shapeDef = b3DefaultShapeDef();
b3Sphere sphere = { { 0.0f, 0.0f, 0.0f }, 0.1f };
b3CreateSphereShape( bulletId, &shapeDef, &sphere );
b3World_Step( worldId, 1.0f / 60.0f, 4 );
b3World* world = b3GetWorldFromId( worldId );
b3Body* body = b3GetBodyFullId( world, bulletId );
b3BodySim* sim = b3GetBodySim( world, body );
ENSURE( ( sim->flags & b3_isFast ) != 0 );
// Any public setter that syncs flags. The locks have to actually change or the setter
// returns early and never syncs.
b3MotionLocks locks = { 0 };
locks.angularX = true;
b3Body_SetMotionLocks( bulletId, locks );
sim = b3GetBodySim( world, b3GetBodyFullId( world, bulletId ) );
ENSURE( ( sim->flags & b3_isFast ) != 0 );
b3World_Step( worldId, 1.0f / 60.0f, 4 );
b3DestroyWorld( worldId );
return 0;
}
static int BodySimLocatorTest( void )
{
typedef uint64_t ( *LocatorScene )( int workerCount, int* failed );
LocatorScene scenes[5] = {
LocatorSleepWakeScene,
LocatorAwakeNeighborScene,
LocatorDestroyScene,
LocatorKinematicScene,
LocatorTransferScene,
};
for ( int i = 0; i < 5; ++i )
{
int failed = 0;
uint64_t hash1 = scenes[i]( 1, &failed );
uint64_t hash4 = scenes[i]( 4, &failed );
ENSURE( failed == 0 );
ENSURE( hash1 == hash4 );
}
return LocatorFastFlagTest();
}
int WorldTest( void )
{
RUN_SUBTEST( HelloWorld );
@ -1300,6 +1644,7 @@ int WorldTest( void )
RUN_SUBTEST( TestHullDatabase );
RUN_SUBTEST( TestEnlargedProxyDestroyed );
RUN_SUBTEST( TestCompoundShapeCount );
RUN_SUBTEST( BodySimLocatorTest );
return 0;
}