Tree perf (#179)

- Optimizations for falling trees.
- Increased B3_MAX_MESH_CONTACT_TRIANGLES  from 256 to 512.
This commit is contained in:
Erin Catto 2026-10-06 09:51:48 -07:00 • committed by GitHub
commit 16f7f4cf4c
No known key found for this signature in database
GPG key ID: B5690EEEBB952194
36 changed files with 1926 additions and 1110 deletions

View file

@ -1,9 +1,9 @@
threads,ms
1,5092.19
2,2694.4
3,1855.5
4,1450.33
5,1195.15
6,1032.87
7,924.237
8,843.199
1,5092.98
2,2683.53
3,1843.54
4,1439.27
5,1192.14
6,1033.23
7,923.268
8,846.347

1 threads ms
2 1 5092.19 5092.98
3 2 2694.4 2683.53
4 3 1855.5 1843.54
5 4 1450.33 1439.27
6 5 1195.15 1192.14
7 6 1032.87 1033.23
8 7 924.237 923.268
9 8 843.199 846.347

View file

@ -1,9 +1,9 @@
threads,ms
1,1308.56
2,683.543
3,461.149
4,351.313
5,283.848
6,239.34
7,208.293
8,185.43
1,1332.87
2,689.909
3,463.412
4,352.244
5,282.623
6,239.524
7,207.957
8,185.382

1 threads ms
2 1 1308.56 1332.87
3 2 683.543 689.909
4 3 461.149 463.412
5 4 351.313 352.244
6 5 283.848 282.623
7 6 239.34 239.524
8 7 208.293 207.957
9 8 185.43 185.382

View file

@ -1,9 +1,9 @@
threads,ms
1,11252.9
2,5915.73
3,4113.86
4,3194.52
5,2626.44
6,2261.77
7,2009.22
8,1824.68
1,11236.9
2,5885.02
3,4063.23
4,3147.56
5,2601.85
6,2248.15
7,2001.73
8,1822.96

1 threads ms
2 1 11252.9 11236.9
3 2 5915.73 5885.02
4 3 4113.86 4063.23
5 4 3194.52 3147.56
6 5 2626.44 2601.85
7 6 2261.77 2248.15
8 7 2009.22 2001.73
9 8 1824.68 1822.96

View file

@ -1,9 +1,9 @@
threads,ms
1,1067.06
2,558.46
3,390.698
4,300.188
5,249.834
6,218.912
7,193.65
8,175.819
1,1099.9
2,567.505
3,392.791
4,302.607
5,250.85
6,218.705
7,193.731
8,175.831

1 threads ms
2 1 1067.06 1099.9
3 2 558.46 567.505
4 3 390.698 392.791
5 4 300.188 302.607
6 5 249.834 250.85
7 6 218.912 218.705
8 7 193.65 193.731
9 8 175.819 175.831

View file

@ -1,9 +1,9 @@
threads,ms
1,5.0616
2,6.9262
3,7.827
4,8.2244
5,8.7631
6,8.6223
7,8.7636
8,8.9294
1,5.2893
2,6.8593
3,8.2452
4,8.7965
5,8.88
6,8.8702
7,9.0274
8,9.3917

1 threads ms
2 1 5.0616 5.2893
3 2 6.9262 6.8593
4 3 7.827 8.2452
5 4 8.2244 8.7965
6 5 8.7631 8.88
7 6 8.6223 8.8702
8 7 8.7636 9.0274
9 8 8.9294 9.3917

View file

@ -1,9 +1,9 @@
threads,ms
1,1163.37
2,605.215
3,411.543
4,307.527
5,249.396
6,216.535
7,184.334
8,165.442
1,1252.28
2,603.36
3,412.994
4,311.517
5,249.951
6,210.331
7,185.742
8,162.021

1 threads ms
2 1 1163.37 1252.28
3 2 605.215 603.36
4 3 411.543 412.994
5 4 307.527 311.517
6 5 249.396 249.951
7 6 216.535 210.331
8 7 184.334 185.742
9 8 165.442 162.021

View file

@ -0,0 +1,9 @@
threads,ms
1,1287.05
2,665.133
3,463.048
4,360.315
5,300.983
6,262.112
7,236.27
8,214.998
1 threads ms
2 1 1287.05
3 2 665.133
4 3 463.048
5 4 360.315
6 5 300.983
7 6 262.112
8 7 236.27
9 8 214.998

View file

@ -1,9 +1,9 @@
threads,ms
1,2038.39
2,1110.3
3,773.18
4,603.397
5,502.292
6,434.461
7,389.414
8,355.206
1,1832.38
2,995.626
3,699.212
4,547.216
5,456.76
6,396.698
7,356.024
8,325.755

1 threads ms
2 1 2038.39 1832.38
3 2 1110.3 995.626
4 3 773.18 699.212
5 4 603.397 547.216
6 5 502.292 456.76
7 6 434.461 396.698
8 7 389.414 356.024
9 8 355.206 325.755

View file

@ -1,9 +1,9 @@
threads,ms
1,596.258
2,328.927
3,231.57
4,187.904
5,159.347
6,142.58
7,131.034
8,124.711
1,625.709
2,330.685
3,233.065
4,187.897
5,160.823
6,143.489
7,131.729
8,125.636

1 threads ms
2 1 596.258 625.709
3 2 328.927 330.685
4 3 231.57 233.065
5 4 187.904 187.897
6 5 159.347 160.823
7 6 142.58 143.489
8 7 131.034 131.729
9 8 124.711 125.636

View file

@ -1,9 +1,9 @@
threads,ms
1,2919.32
2,1674.26
3,1200.43
4,960.786
5,821.659
6,728.967
7,669.058
8,626.733
1,2274.2
2,1328.84
3,960.76
4,782.728
5,679.039
6,633.552
7,567.926
8,541.742

1 threads ms
2 1 2919.32 2274.2
3 2 1674.26 1328.84
4 3 1200.43 960.76
5 4 960.786 782.728
6 5 821.659 679.039
7 6 728.967 633.552
8 7 669.058 567.926
9 8 626.733 541.742

View file

@ -1,9 +1,9 @@
threads,ms
1,167.468
2,110.54
3,87.5729
4,77.6966
5,73.9776
6,71.7265
7,69.6889
8,69.3954
1,103.803
2,69.6303
3,59.0807
4,53.6692
5,50.927
6,49.0812
7,48.3124
8,47.7709

1 threads ms
2 1 167.468 103.803
3 2 110.54 69.6303
4 3 87.5729 59.0807
5 4 77.6966 53.6692
6 5 73.9776 50.927
7 6 71.7265 49.0812
8 7 69.6889 48.3124
9 8 69.3954 47.7709

View file

@ -1,9 +1,9 @@
threads,ms
1,657.535
2,386.678
3,284.076
4,245.238
5,223.137
6,208.401
7,197.529
8,194.169
1,357.883
2,217.829
3,169.684
4,147.551
5,135.274
6,126.444
7,122.615
8,119.284

1 threads ms
2 1 657.535 357.883
3 2 386.678 217.829
4 3 284.076 169.684
5 4 245.238 147.551
6 5 223.137 135.274
7 6 208.401 126.444
8 7 197.529 122.615
9 8 194.169 119.284

View file

@ -1,9 +1,9 @@
threads,ms
1,285.043
2,176.584
3,135.305
4,117.663
5,109.496
6,104.289
7,101.186
8,98.9602
1,162.522
2,103.266
3,84.4805
4,74.2779
5,70.0241
6,66.3925
7,64.5923
8,63.7022

1 threads ms
2 1 285.043 162.522
3 2 176.584 103.266
4 3 135.305 84.4805
5 4 117.663 74.2779
6 5 109.496 70.0241
7 6 104.289 66.3925
8 7 101.186 64.5923
9 8 98.9602 63.7022

View file

@ -1,9 +1,9 @@
threads,ms
1,15669.7
2,8410.7
3,5793.28
4,4493.58
5,3722.97
6,3219.45
7,2849.15
8,2562.49
1,15659.7
2,8342.79
3,5752.42
4,4424.66
5,3715.88
6,3211.79
7,2848.87
8,2534.36

1 threads ms
2 1 15669.7 15659.7
3 2 8410.7 8342.79
4 3 5793.28 5752.42
5 4 4493.58 4424.66
6 5 3722.97 3715.88
7 6 3219.45 3211.79
8 7 2849.15 2848.87
9 8 2562.49 2534.36

View file

@ -228,6 +228,7 @@ int main( int argc, char** argv )
{ "large_pyramid", NULL, CreateLargePyramid, NULL, NULL, 200 },
{ "large_world", GetLargeWorldCapacity, CreateLargeWorld, NULL, StepLargeWorld, 500 },
{ "many_pyramids", NULL, CreateManyPyramids, NULL, NULL, 100 },
{ "mesh_drop", NULL, CreateMeshDropBenchmark, DestroyMeshDropBenchmark, NULL, 170 },
{ "rain", GetRainCapacity, CreateRain, DestroyRain, StepRain, 400 },
{ "sleep", GetSleepCapacity, CreateSleep, NULL, StepSleep, 300 },
{ "spinner", GetSpinnerCapacity, CreateSpinner, DestroySpinner, NULL, 800 },

View file

@ -14,12 +14,6 @@ B3_API void b3SetLengthUnitsPerMeter( float lengthUnits );
/// Get the current length units per meter.
B3_API float b3GetLengthUnitsPerMeter( void );
/// Set the threshold for logging stalls.
B3_API void b3SetStallThreshold( float seconds );
/// Get the threshold for logging stalls.
B3_API float b3GetStallThreshold( void );
// Used to detect bad values. In float mode positions greater than about 16km have precision
// problems, so 100km is a safe limit. Large world mode keeps coordinates accurate much farther
// from the origin, so the sanity limit widens to keep valid far-field positions from tripping it.
@ -148,5 +142,5 @@ B3_API float b3GetStallThreshold( void );
/// Increasing this will increase stack usage, so be careful. I recommend to simplify your collision data
/// before increasing this. For example, using render mesh for collision often leads to poor performance.
#ifndef B3_MAX_MESH_CONTACT_TRIANGLES
#define B3_MAX_MESH_CONTACT_TRIANGLES 256
#define B3_MAX_MESH_CONTACT_TRIANGLES 512
#endif

View file

@ -735,6 +735,34 @@ public:
static int sampleFallingTrees = RegisterSample( "Benchmark", "Falling Trees", BenchmarkFallingTrees::Create );
class BenchmarkMeshDrop : public Sample
{
public:
explicit BenchmarkMeshDrop( SampleContext* context )
: Sample( context )
{
if ( context->restart == false )
{
m_camera->SetView( 0.0f, 30.0f, 140.0f, b3Pos_zero );
GetGuiDraw()->forceScale = 0.1f;
}
CreateMeshDropBenchmark( m_worldId );
}
~BenchmarkMeshDrop() override
{
DestroyMeshDropBenchmark();
}
static Sample* Create( SampleContext* context )
{
return new BenchmarkMeshDrop( context );
}
};
static int sampleMeshDropBenchmark = RegisterSample( "Benchmark", "Mesh Drop", BenchmarkMeshDrop::Create );
struct ShapeUserData
{
int row;

View file

@ -969,11 +969,6 @@ public:
b3CreateMeshShape( bodyId, &shapeDef, m_mesh, b3Vec3_one );
}
m_savedThreshold = b3GetStallThreshold();
// Log any CCD that takes longer than 1 ms.
b3SetStallThreshold( 0.001f );
Launch();
}
@ -1013,7 +1008,6 @@ public:
~Stall() override
{
b3DestroyMesh( m_mesh );
b3SetStallThreshold( m_savedThreshold );
}
static Sample* Create( SampleContext* context )
@ -1023,7 +1017,6 @@ public:
b3MeshData* m_mesh = nullptr;
b3BodyId m_bulletId = b3_nullBodyId;
float m_savedThreshold;
};
static int sampleStall = RegisterSample( "Continuous", "Stall", Stall::Create );

View file

@ -786,6 +786,77 @@ void DestroyTrees( void )
memset( &g_treeData, 0, sizeof( g_treeData ) );
}
struct
{
b3MeshData* meshData;
} g_meshDropData;
void CreateMeshDropBenchmark( b3WorldId worldId )
{
memset( &g_meshDropData, 0, sizeof( g_meshDropData ) );
int meshGridCount = 120;
int borderCount = 10;
float cellWidth = 1.0f;
{
b3BodyDef bodyDef = b3DefaultBodyDef();
b3BodyId groundId = b3CreateBody( worldId, &bodyDef );
float rowHz = 0.1f;
float columnHz = 0.2f;
float groundAmplitude = 0.5f;
g_meshDropData.meshData =
b3CreateWaveMesh( meshGridCount, meshGridCount, cellWidth, groundAmplitude, rowHz, columnHz );
b3ShapeDef shapeDef = b3DefaultShapeDef();
shapeDef.filter.categoryBits = 1;
b3CreateMeshShape( groundId, &shapeDef, g_meshDropData.meshData, b3Vec3_one );
}
{
b3BoxHull box = b3MakeBoxHull( 0.02f, 0.2f, 0.04f );
b3BodyDef bodyDef = b3DefaultBodyDef();
bodyDef.type = b3_dynamicBody;
b3ShapeDef shapeDef = b3DefaultShapeDef();
shapeDef.baseMaterial.rollingResistance = 0.1f;
// Don't allow shapes to collide with each other.
shapeDef.filter.categoryBits = 2;
shapeDef.filter.maskBits = 1;
g_randomSeed = 3963634789;
int gridCount = BENCHMARK_DEBUG ? 20 : 100;
float spacing = cellWidth * ( meshGridCount - 2 * borderCount ) / gridCount;
float offset = 0.5f * spacing * ( 1 - gridCount );
for ( int i = 0; i < gridCount; ++i )
{
for ( int j = 0; j < gridCount; ++j )
{
b3Vec3 linearVelocity = RandomVec3Uniform( -1.0f, 1.0f );
b3Vec3 angularVelocity = RandomVec3Uniform( -5.0f, 5.0f );
bodyDef.position = (b3Pos){ offset + spacing * i, 5.0f, offset + spacing * j };
bodyDef.linearVelocity = linearVelocity;
bodyDef.angularVelocity = angularVelocity;
b3BodyId bodyId = b3CreateBody( worldId, &bodyDef );
b3CreateHullShape( bodyId, &shapeDef, &box.base );
}
}
}
}
void DestroyMeshDropBenchmark( void )
{
b3DestroyMesh( g_meshDropData.meshData );
memset( &g_meshDropData, 0, sizeof( g_meshDropData ) );
}
struct JunkyardData
{
b3BodyId pusherId;

View file

@ -24,6 +24,8 @@ void CreateTrees25( b3WorldId worldId );
void CreateTrees50( b3WorldId worldId );
void CreateTrees100( b3WorldId worldId );
void DestroyTrees( void );
void CreateMeshDropBenchmark( b3WorldId worldId );
void DestroyMeshDropBenchmark( void );
void CreateJointGrid( b3WorldId worldId );
void CreateJunkyard( b3WorldId worldId );
void GetJunkyardCapacity( b3Capacity* capacity );

View file

@ -53,11 +53,13 @@ typedef struct b3GraphColor
void* wideConstraints;
int wideConstraintCount;
void* wideMeshConstraints;
int* wideMeshManifoldStarts;
int wideMeshConstraintCount;
// These are used for mesh and overflow contacts
struct b3ManifoldConstraint* manifoldConstraints;
int manifoldConstraintCount;
struct b3ContactConstraint* contactConstraints;
int contactConstraintCount;
} b3GraphColor;
typedef struct b3ConstraintGraph

View file

@ -17,6 +17,8 @@
#include "shape.h"
#endif
// todo clean this file up because it is only used for overflow now
// contact separation for sub-stepping
// s = s0 + dot(cB + rB - cA - rA, normal)
// normal is held constant
@ -26,7 +28,7 @@
// s(t) = s0 + dot(cB0 - cA0, normal) + dot(dpB - dpA + rot(dqB, rB0) - rot(dqA, rA0), normal)
// s_base = s0 + dot(cB0 - cA0, normal)
// Prepare a mesh constraints
// Prepare overflow constraints
void b3PrepareContacts_Mesh( b3SolverBlock block, b3StepContext* context )
{
b3TracyCZoneNC( prepare_contact, "Prepare Contact", b3_colorYellow, true );
@ -41,19 +43,11 @@ void b3PrepareContacts_Mesh( b3SolverBlock block, b3StepContext* context )
// Used for friction center weighting.
float invTau = 1.0f / B3_SPECULATIVE_DISTANCE;
// Need to use spans in order to find the associated b2Contact, which is per color
b3ContactPrepareSpan* spans = context->contactPrepareSpans;
b3ManifoldConstraint* manifoldBase = context->manifoldConstraints;
b3ContactConstraint* base = context->contactConstraints;
// Overflow constraints are stored separately
if ( block.blockType == b3_overflowBlock )
{
b3GraphColor* overflow = world->constraintGraph.colors + B3_OVERFLOW_INDEX;
spans = context->overflowSpans;
manifoldBase = overflow->manifoldConstraints;
base = overflow->contactConstraints;
}
B3_ASSERT( block.blockType == b3_overflowBlock );
b3GraphColor* overflow = world->constraintGraph.colors + B3_OVERFLOW_INDEX;
b3ContactPrepareSpan* spans = context->overflowSpans;
b3ManifoldConstraint* manifoldBase = overflow->manifoldConstraints;
b3ContactConstraint* base = overflow->contactConstraints;
int index = block.startIndex;
int endIndex = block.startIndex + block.count;
@ -238,6 +232,10 @@ void b3PrepareContacts_Mesh( b3SolverBlock block, b3StepContext* context )
// should not contribute to the friction center. They are not important for jitter reduction. Closer
// points may begin to touch on and off, so the friction center needs to move smoothly.
// Epsilon to avoid a branch below (or divide by zero). Small enough to get washed out normally.
//
// Dirk suggested to weight the points by their normal impulse. This would use the total impulse from
// the previous time step. I suspect this could jump around if the active point set jitters, leading
// to a limit cycle.
float weight = b3ClampFloat( 2.0f - s * invTau, B3_MIN_FRICTION_WEIGHT, 1.0f );
centerA = b3MulAdd( centerA, weight, rA );
centerB = b3MulAdd( centerB, weight, rB );
@ -313,21 +311,10 @@ void b3WarmStartContacts_Mesh( b3SolverBlock block, b3StepContext* context )
for ( int constraintIndex = startIndex; constraintIndex < endIndex; ++constraintIndex )
{
const b3ContactConstraint* contactConstraint = constraints + constraintIndex;
int indexA = contactConstraint->indexA;
int indexB = contactConstraint->indexB;
b3BodyState* stateA = indexA == B3_NULL_INDEX ? &dummyState : states + indexA;
b3BodyState* stateB = indexB == B3_NULL_INDEX ? &dummyState : states + indexB;
b3Vec3 vA = stateA->linearVelocity;
b3Vec3 wA = stateA->angularVelocity;
b3Vec3 vB = stateB->linearVelocity;
b3Vec3 wB = stateB->angularVelocity;
float mA = contactConstraint->invMassA;
b3Matrix3 iA = contactConstraint->invIA;
float mB = contactConstraint->invMassB;
b3Matrix3 iB = contactConstraint->invIB;
b3Vec3 linearImpulse = b3Vec3_zero;
b3Vec3 angularImpulseA = b3Vec3_zero;
b3Vec3 angularImpulseB = b3Vec3_zero;
int manifoldCount = contactConstraint->manifoldCount;
for ( int manifoldIndex = 0; manifoldIndex < manifoldCount; ++manifoldIndex )
@ -336,60 +323,56 @@ void b3WarmStartContacts_Mesh( b3SolverBlock block, b3StepContext* context )
// Normal impulses
b3Vec3 normal = constraint->normal;
float totalNormalImpulse = 0.0f;
b3Vec3 momentA = b3Vec3_zero;
b3Vec3 momentB = b3Vec3_zero;
int pointCount = constraint->pointCount;
for ( int j = 0; j < pointCount; ++j )
{
const b3ManifoldConstraintPoint* cp = constraint->points + j;
// fixed anchors
b3Vec3 rA = cp->rA;
b3Vec3 rB = cp->rB;
b3Vec3 impulse = b3MulSV( cp->normalImpulse, normal );
wA = b3Sub( wA, b3MulMV( iA, b3Cross( rA, impulse ) ) );
vA = b3MulSub( vA, mA, impulse );
wB = b3Add( wB, b3MulMV( iB, b3Cross( rB, impulse ) ) );
vB = b3MulAdd( vB, mB, impulse );
totalNormalImpulse += cp->normalImpulse;
momentA = b3MulAdd( momentA, cp->normalImpulse, cp->rA );
momentB = b3MulAdd( momentB, cp->normalImpulse, cp->rB );
}
// Central friction
{
b3Vec3 rA = constraint->centerA;
b3Vec3 rB = constraint->centerB;
b3Vec3 impulse = b3MulSV( constraint->frictionImpulse.x, constraint->tangent1 );
impulse = b3Add( impulse, b3MulSV( constraint->frictionImpulse.y, constraint->tangent2 ) );
b3Vec3 frictionImpulse = b3MulSV( constraint->frictionImpulse.x, constraint->tangent1 );
frictionImpulse = b3MulAdd( frictionImpulse, constraint->frictionImpulse.y, constraint->tangent2 );
wA = b3Sub( wA, b3MulMV( iA, b3Cross( rA, impulse ) ) );
vA = b3MulSub( vA, mA, impulse );
wB = b3Add( wB, b3MulMV( iB, b3Cross( rB, impulse ) ) );
vB = b3MulAdd( vB, mB, impulse );
}
linearImpulse = b3Add( linearImpulse, b3MulAdd( frictionImpulse, totalNormalImpulse, normal ) );
// Central twist friction
{
b3Vec3 impulse = b3MulSV( constraint->twistImpulse, constraint->normal );
wA = b3Sub( wA, b3MulMV( iA, impulse ) );
wB = b3Add( wB, b3MulMV( iB, impulse ) );
}
b3Vec3 twistImpulse = b3MulSV( constraint->twistImpulse, normal );
b3Vec3 angularA =
b3Add( b3Add( b3Cross( momentA, normal ), b3Cross( constraint->centerA, frictionImpulse ) ), twistImpulse );
b3Vec3 angularB =
b3Add( b3Add( b3Cross( momentB, normal ), b3Cross( constraint->centerB, frictionImpulse ) ), twistImpulse );
// Rolling resistance
{
b3Vec3 impulse = constraint->rollingImpulse;
wA = b3Sub( wA, b3MulMV( iA, impulse ) );
wB = b3Add( wB, b3MulMV( iB, impulse ) );
}
angularImpulseA = b3Add( angularImpulseA, b3Add( angularA, constraint->rollingImpulse ) );
angularImpulseB = b3Add( angularImpulseB, b3Add( angularB, constraint->rollingImpulse ) );
}
int indexA = contactConstraint->indexA;
int indexB = contactConstraint->indexB;
b3BodyState* stateA = indexA == B3_NULL_INDEX ? &dummyState : states + indexA;
b3BodyState* stateB = indexB == B3_NULL_INDEX ? &dummyState : states + indexB;
if ( stateA->flags & b3_dynamicFlag )
{
stateA->linearVelocity = vA;
stateA->angularVelocity = wA;
stateA->linearVelocity = b3MulSub( stateA->linearVelocity, contactConstraint->invMassA, linearImpulse );
stateA->angularVelocity = b3Sub( stateA->angularVelocity, b3MulMV( contactConstraint->invIA, angularImpulseA ) );
}
if ( stateB->flags & b3_dynamicFlag )
{
stateB->linearVelocity = vB;
stateB->angularVelocity = wB;
stateB->linearVelocity = b3MulAdd( stateB->linearVelocity, contactConstraint->invMassB, linearImpulse );
stateB->angularVelocity = b3Add( stateB->angularVelocity, b3MulMV( contactConstraint->invIB, angularImpulseB ) );
}
}
}
@ -837,17 +820,10 @@ void b3StoreImpulses_Mesh( b3SolverBlock block, b3StepContext* context, int work
{
b3World* world = context->world;
// Mirror b3PrepareContacts_Mesh: the per-color flat arrays and the overflow color
// each have their own (base, spans, manifoldBase).
b3ContactPrepareSpan* spans = context->contactPrepareSpans;
b3ContactConstraint* base = context->contactConstraints;
if ( block.blockType == b3_overflowBlock )
{
b3GraphColor* overflow = world->constraintGraph.colors + B3_OVERFLOW_INDEX;
spans = context->overflowSpans;
base = overflow->contactConstraints;
}
B3_ASSERT( block.blockType == b3_overflowBlock );
b3GraphColor* overflow = world->constraintGraph.colors + B3_OVERFLOW_INDEX;
b3ContactPrepareSpan* spans = context->overflowSpans;
b3ContactConstraint* base = overflow->contactConstraints;
b3TaskContext* taskContext = world->taskContexts.data + workerIndex;
b3BitSet* hitEventBitSet = &taskContext->hitEventBitSet;
@ -1148,3 +1124,109 @@ int b3GetWideContactConstraintByteCount( int simdWidth )
return b3GetWideContactConstraintByteCountW4();
}
void b3PrepareContacts_MeshWide( b3SolverBlock block, b3StepContext* context )
{
#if defined( B3_SIMD_HAS_WIDTH_8 )
if ( context->world->simdWidth == 8 )
{
b3PrepareContacts_MeshWideW8( block, context );
return;
}
#endif
b3PrepareContacts_MeshWideW4( block, context );
}
void b3WarmStartContacts_MeshWide( b3SolverBlock block, b3StepContext* context )
{
#if defined( B3_SIMD_HAS_WIDTH_8 )
if ( context->world->simdWidth == 8 )
{
b3WarmStartContacts_MeshWideW8( block, context );
return;
}
#endif
b3WarmStartContacts_MeshWideW4( block, context );
}
void b3PushContacts_MeshWide( b3SolverBlock block, b3StepContext* context )
{
#if defined( B3_SIMD_HAS_WIDTH_8 )
if ( context->world->simdWidth == 8 )
{
b3PushContacts_MeshWideW8( block, context );
return;
}
#endif
b3PushContacts_MeshWideW4( block, context );
}
void b3SolveContacts_MeshWide( b3SolverBlock block, b3StepContext* context )
{
#if defined( B3_SIMD_HAS_WIDTH_8 )
if ( context->world->simdWidth == 8 )
{
b3SolveContacts_MeshWideW8( block, context );
return;
}
#endif
b3SolveContacts_MeshWideW4( block, context );
}
void b3ApplyRestitution_MeshWide( b3SolverBlock block, b3StepContext* context )
{
#if defined( B3_SIMD_HAS_WIDTH_8 )
if ( context->world->simdWidth == 8 )
{
b3ApplyRestitution_MeshWideW8( block, context );
return;
}
#endif
b3ApplyRestitution_MeshWideW4( block, context );
}
void b3StoreImpulses_MeshWide( b3SolverBlock block, b3StepContext* context, int workerIndex )
{
#if defined( B3_SIMD_HAS_WIDTH_8 )
if ( context->world->simdWidth == 8 )
{
b3StoreImpulses_MeshWideW8( block, context, workerIndex );
return;
}
#endif
b3StoreImpulses_MeshWideW4( block, context, workerIndex );
}
int b3GetWideMeshConstraintByteCount( int simdWidth )
{
#if defined( B3_SIMD_HAS_WIDTH_8 )
if ( simdWidth == 8 )
{
return b3GetWideMeshConstraintByteCountW8();
}
#else
B3_UNUSED( simdWidth );
#endif
return b3GetWideMeshConstraintByteCountW4();
}
int b3GetWideMeshManifoldByteCount( int simdWidth )
{
#if defined( B3_SIMD_HAS_WIDTH_8 )
if ( simdWidth == 8 )
{
return b3GetWideMeshManifoldByteCountW8();
}
#else
B3_UNUSED( simdWidth );
#endif
return b3GetWideMeshManifoldByteCountW4();
}

View file

@ -91,3 +91,30 @@ void b3SolveContacts_ConvexW8( b3SolverBlock block, b3StepContext* context );
void b3ApplyRestitution_ConvexW8( b3SolverBlock block, b3StepContext* context );
void b3StoreImpulses_ConvexW8( b3SolverBlock block, b3StepContext* context, int workerIndex );
int b3GetWideContactConstraintByteCountW8( void );
void b3PrepareContacts_MeshWide( b3SolverBlock block, b3StepContext* context );
void b3WarmStartContacts_MeshWide( b3SolverBlock block, b3StepContext* context );
void b3PushContacts_MeshWide( b3SolverBlock block, b3StepContext* context );
void b3SolveContacts_MeshWide( b3SolverBlock block, b3StepContext* context );
void b3ApplyRestitution_MeshWide( b3SolverBlock block, b3StepContext* context );
void b3StoreImpulses_MeshWide( b3SolverBlock block, b3StepContext* context, int workerIndex );
int b3GetWideMeshConstraintByteCount( int simdWidth );
int b3GetWideMeshManifoldByteCount( int simdWidth );
void b3PrepareContacts_MeshWideW4( b3SolverBlock block, b3StepContext* context );
void b3WarmStartContacts_MeshWideW4( b3SolverBlock block, b3StepContext* context );
void b3PushContacts_MeshWideW4( b3SolverBlock block, b3StepContext* context );
void b3SolveContacts_MeshWideW4( b3SolverBlock block, b3StepContext* context );
void b3ApplyRestitution_MeshWideW4( b3SolverBlock block, b3StepContext* context );
void b3StoreImpulses_MeshWideW4( b3SolverBlock block, b3StepContext* context, int workerIndex );
int b3GetWideMeshConstraintByteCountW4( void );
int b3GetWideMeshManifoldByteCountW4( void );
void b3PrepareContacts_MeshWideW8( b3SolverBlock block, b3StepContext* context );
void b3WarmStartContacts_MeshWideW8( b3SolverBlock block, b3StepContext* context );
void b3PushContacts_MeshWideW8( b3SolverBlock block, b3StepContext* context );
void b3SolveContacts_MeshWideW8( b3SolverBlock block, b3StepContext* context );
void b3ApplyRestitution_MeshWideW8( b3SolverBlock block, b3StepContext* context );
void b3StoreImpulses_MeshWideW8( b3SolverBlock block, b3StepContext* context, int workerIndex );
int b3GetWideMeshConstraintByteCountW8( void );
int b3GetWideMeshManifoldByteCountW8( void );

File diff suppressed because it is too large Load diff

View file

@ -52,19 +52,6 @@ float b3GetLengthUnitsPerMeter( void )
return b3_lengthUnitsPerMeter;
}
static float b3_stallThreshold = FLT_MAX;
void b3SetStallThreshold( float seconds )
{
B3_ASSERT( b3IsValidFloat( seconds ) && seconds > 0.0f );
b3_stallThreshold = seconds;
}
float b3GetStallThreshold( void )
{
return b3_stallThreshold;
}
static int b3DefaultAssertFcn( const char* condition, const char* fileName, int lineNumber )
{
printf( "BOX3D ASSERTION: %s, %s, line %d\n", condition, fileName, lineNumber );

View file

@ -2234,50 +2234,6 @@ b3CastOutput b3ShapeCastMesh( const b3Mesh* mesh, const b3ShapeCastInput* input
return bestOutput;
}
b3Triangle b3GetMeshTriangle( const b3Mesh* mesh, int triangleIndex )
{
B3_ASSERT( 0 <= triangleIndex && triangleIndex < mesh->data->triangleCount );
const b3MeshTriangle* triangles = b3GetMeshTriangles( mesh->data );
const uint8_t* flags = b3GetMeshFlags( mesh->data );
const b3Vec3* vertices = b3GetMeshVertices( mesh->data );
b3Triangle result;
b3MeshTriangle triangle = triangles[triangleIndex];
uint8_t triangleFlags = flags[triangleIndex];
b3Vec3 scale = mesh->scale;
result.vertices[0] = b3Mul( scale, vertices[triangle.index1] );
result.i1 = triangle.index1;
if ( scale.x * scale.y * scale.z < 0.0f )
{
result.vertices[1] = b3Mul( scale, vertices[triangle.index3] );
result.vertices[2] = b3Mul( scale, vertices[triangle.index2] );
result.i2 = triangle.index3;
result.i3 = triangle.index2;
// mesh is inverted, so concave edges are now convex
result.flags = 0;
result.flags |= ( triangleFlags & b3_inverseConcaveEdge1 ) ? b3_concaveEdge1 : 0;
result.flags |= ( triangleFlags & b3_inverseConcaveEdge2 ) ? b3_concaveEdge2 : 0;
result.flags |= ( triangleFlags & b3_inverseConcaveEdge3 ) ? b3_concaveEdge3 : 0;
}
else
{
result.vertices[1] = b3Mul( scale, vertices[triangle.index2] );
result.vertices[2] = b3Mul( scale, vertices[triangle.index3] );
result.i2 = triangle.index2;
result.i3 = triangle.index3;
result.flags = triangleFlags;
}
return result;
}
int b3CollideMoverAndMesh( b3PlaneResult* planes, int capacity, const b3Mesh* shape, const b3Capsule* mover )
{
if ( capacity == 0 )
@ -2436,63 +2392,82 @@ void b3QueryMesh( const b3Mesh* mesh, b3AABB bounds, b3MeshQueryFcn* fcn, void*
const b3MeshTriangle* triangles = b3GetMeshTriangles( data );
const b3Vec3* vertices = b3GetMeshVertices( data );
// Test node overlap in unscaled space
if ( b3TestBoundsOverlap( b3LoadV( &node->lowerBound.x ), b3LoadV( &node->upperBound.x ), invScaledBoundsMin,
invScaledBoundsMax ) == false )
{
return;
}
while ( true )
{
// Test node overlap in unscaled space
b3V32 nodeMin = b3LoadV( &node->lowerBound.x );
b3V32 nodeMax = b3LoadV( &node->upperBound.x );
if ( b3TestBoundsOverlap( nodeMin, nodeMax, invScaledBoundsMin, invScaledBoundsMax ) )
if ( b3IsLeaf( node ) )
{
if ( b3IsLeaf( node ) )
int triangleCount = node->data.asLeaf.triangleCount;
int triangleOffset = node->triangleOffset;
for ( int index = 0; index < triangleCount; ++index )
{
int triangleCount = node->data.asLeaf.triangleCount;
int triangleOffset = node->triangleOffset;
int triangleIndex = triangleOffset + index;
b3MeshTriangle triangle = triangles[triangleIndex];
for ( int index = 0; index < triangleCount; ++index )
b3Vec3 vertex1 = vertices[triangle.index1];
b3Vec3 vertex2 = vertices[triangle.index2];
b3Vec3 vertex3 = vertices[triangle.index3];
b3V32 v1 = b3LoadV( &vertex1.x );
b3V32 v2 = b3LoadV( &vertex2.x );
b3V32 v3 = b3LoadV( &vertex3.x );
// Perform triangle overlap test in unscaled space.
// todo it is possible that some margins are getting scaled
if ( b3TestBoundsTriangleOverlap( invScaledBoundsCenter, invScaledBoundsExtent, v1, v2, v3 ) )
{
int triangleIndex = triangleOffset + index;
b3MeshTriangle triangle = triangles[triangleIndex];
b3Vec3 vertex1 = vertices[triangle.index1];
b3Vec3 vertex2 = vertices[triangle.index2];
b3Vec3 vertex3 = vertices[triangle.index3];
b3V32 v1 = b3LoadV( &vertex1.x );
b3V32 v2 = b3LoadV( &vertex2.x );
b3V32 v3 = b3LoadV( &vertex3.x );
// Perform triangle overlap test in unscaled space.
// todo it is possible that some margins are getting scaled
if ( b3TestBoundsTriangleOverlap( invScaledBoundsCenter, invScaledBoundsExtent, v1, v2, v3 ) )
b3Vec3 a = b3Mul( meshScale, vertex1 );
b3Vec3 b, c;
if ( ccw )
{
b3Vec3 a = b3Mul( meshScale, vertex1 );
b3Vec3 b, c;
if ( ccw )
{
b = b3Mul( meshScale, vertex2 );
c = b3Mul( meshScale, vertex3 );
}
else
{
b = b3Mul( meshScale, vertex3 );
c = b3Mul( meshScale, vertex2 );
}
b = b3Mul( meshScale, vertex2 );
c = b3Mul( meshScale, vertex3 );
}
else
{
b = b3Mul( meshScale, vertex3 );
c = b3Mul( meshScale, vertex2 );
}
bool result = fcn( a, b, c, triangleIndex, context );
if ( result == false )
{
return;
}
bool result = fcn( a, b, c, triangleIndex, context );
if ( result == false )
{
return;
}
}
}
else
{
// Recurse
B3_ASSERT( count <= B3_MESH_STACK_SIZE - 1 );
stack[count++] = b3GetRightChild( node );
node = b3GetLeftChild( node );
}
else
{
const b3MeshNode* leftChild = b3GetLeftChild( node );
const b3MeshNode* rightChild = b3GetRightChild( node );
bool overlapLeft = b3TestBoundsOverlap( b3LoadV( &leftChild->lowerBound.x ), b3LoadV( &leftChild->upperBound.x ),
invScaledBoundsMin, invScaledBoundsMax );
bool overlapRight = b3TestBoundsOverlap( b3LoadV( &rightChild->lowerBound.x ),
b3LoadV( &rightChild->upperBound.x ), invScaledBoundsMin, invScaledBoundsMax );
if ( overlapLeft )
{
if ( overlapRight )
{
B3_ASSERT( count <= B3_MESH_STACK_SIZE - 1 );
stack[count++] = rightChild;
}
node = leftChild;
continue;
}
if ( overlapRight )
{
node = rightChild;
continue;
}
}

View file

@ -2,10 +2,12 @@
// SPDX-License-Identifier: MIT
#include "contact.h"
#include "ctz.h"
#include "manifold.h"
#include "physics_world.h"
#include "qsort.h"
#include "shape.h"
#include "simd.h"
#include "box3d/types.h"
@ -72,7 +74,7 @@ static int b3QueryHeightFieldTriangles( int* indices, int capacity, const b3Heig
return context.count;
}
static void b3RefreshCache( b3Contact* contact, const b3Shape* shapeA, b3WorldTransform xfA, const b3AABB* bounds )
static void b3RefreshCache( b3Contact* contact, const b3Shape* shapeA, b3WorldTransform xfA, const b3AABB* bounds, b3Arena arena )
{
B3_ASSERT( shapeA->type == b3_meshShape || shapeA->type == b3_heightShape );
@ -101,8 +103,7 @@ static void b3RefreshCache( b3Contact* contact, const b3Shape* shapeA, b3WorldTr
// Query triangles
int triangleCapacity = B3_MAX_MESH_CONTACT_TRIANGLES;
int triangleIndices[B3_MAX_MESH_CONTACT_TRIANGLES];
int* triangleIndices = b3Bump( &arena, B3_MAX_MESH_CONTACT_TRIANGLES * sizeof( int ) );
// Bounds are in world space. Convert to the local mesh frame. The broadphase bounds are float,
// so the demoted mesh transform is the matching float world frame (exact in float mode).
@ -133,7 +134,7 @@ static void b3RefreshCache( b3Contact* contact, const b3Shape* shapeA, b3WorldTr
B3_VALIDATE( b3IsSorted( triangleIndices, triangleCount ) );
// Create new contact cache and match with old one
b3ContactCache contactCache[B3_MAX_MESH_CONTACT_TRIANGLES];
b3ContactCache* contactCache = b3Bump( &arena, triangleCount * sizeof( b3ContactCache ) );
int index2 = 0;
for ( int index1 = 0; index1 < triangleCount; ++index1 )
@ -535,7 +536,7 @@ bool b3ComputeMeshManifolds( b3World* world, int workerIndex, b3Contact* contact
b3TaskContext* context = b3Array_Get( world->taskContexts, workerIndex );
b3RefreshCache( contact, shapeA, xfA, &shapeB->aabb );
b3RefreshCache( contact, shapeA, xfA, &shapeB->aabb, arena );
// Collide with triangles and build manifolds
b3MeshContact* meshContact = &contact->meshContact;
@ -578,10 +579,144 @@ bool b3ComputeMeshManifolds( b3World* world, int workerIndex, b3Contact* contact
b3TriangleCache* triangleCaches = meshContact->triangleCache.data;
const b3HullData* hullB = shapeB->type == b3_hullShape ? shapeB->hull : NULL;
float hullSpeculativeDistance = enableSpeculative ? B3_SPECULATIVE_DISTANCE : 0.0f;
b3Vec3 hullMargin = { hullSpeculativeDistance, hullSpeculativeDistance, hullSpeculativeDistance };
for ( int index = 0; index < triangleCount && totalPointCount + 3 < pointBufferCapacity; ++index )
int* survivorIndices = b3Bump( &arena, triangleCount * sizeof( int ) );
b3Vec3* survivorVertices = b3Bump( &arena, 3 * triangleCount * sizeof( b3Vec3 ) );
int survivorCount = 0;
const b3MeshTriangle* meshTriangles = NULL;
const b3Vec3* meshVertices = NULL;
b3Vec3 meshScale = b3Vec3_one;
bool meshFlipped = false;
if ( shapeA->type == b3_meshShape )
{
meshTriangles = b3GetMeshTriangles( shapeA->mesh.data );
meshVertices = b3GetMeshVertices( shapeA->mesh.data );
meshScale = shapeA->mesh.scale;
meshFlipped = meshScale.x * meshScale.y * meshScale.z < 0.0f;
}
b3FloatW4 cxx = b3SplatW4( relativeMatrix.cx.x );
b3FloatW4 cxy = b3SplatW4( relativeMatrix.cx.y );
b3FloatW4 cxz = b3SplatW4( relativeMatrix.cx.z );
b3FloatW4 cyx = b3SplatW4( relativeMatrix.cy.x );
b3FloatW4 cyy = b3SplatW4( relativeMatrix.cy.y );
b3FloatW4 cyz = b3SplatW4( relativeMatrix.cy.z );
b3FloatW4 czx = b3SplatW4( relativeMatrix.cz.x );
b3FloatW4 czy = b3SplatW4( relativeMatrix.cz.y );
b3FloatW4 czz = b3SplatW4( relativeMatrix.cz.z );
b3FloatW4 px = b3SplatW4( transformAtoB.p.x );
b3FloatW4 py = b3SplatW4( transformAtoB.p.y );
b3FloatW4 pz = b3SplatW4( transformAtoB.p.z );
b3FloatW4 marginW = b3SplatW4( hullMargin.x );
b3AABB hullBounds = hullB != NULL ? hullB->aabb : (b3AABB){ 0 };
b3FloatW4 hullLowerX = b3SplatW4( hullBounds.lowerBound.x );
b3FloatW4 hullLowerY = b3SplatW4( hullBounds.lowerBound.y );
b3FloatW4 hullLowerZ = b3SplatW4( hullBounds.lowerBound.z );
b3FloatW4 hullUpperX = b3SplatW4( hullBounds.upperBound.x );
b3FloatW4 hullUpperY = b3SplatW4( hullBounds.upperBound.y );
b3FloatW4 hullUpperZ = b3SplatW4( hullBounds.upperBound.z );
for ( int baseIndex = 0; baseIndex < triangleCount; baseIndex += 4 )
{
int laneCount = b3MinInt( 4, triangleCount - baseIndex );
b3Vec3 lanes[3][4];
for ( int lane = 0; lane < 4; ++lane )
{
int index = baseIndex + b3MinInt( lane, laneCount - 1 );
int triangleIndex = triangleCaches[index].triangleIndex;
if ( meshVertices != NULL )
{
b3MeshTriangle meshTriangle = meshTriangles[triangleIndex];
lanes[0][lane] = b3Mul( meshScale, meshVertices[meshTriangle.index1] );
lanes[1][lane] = b3Mul( meshScale, meshVertices[meshFlipped ? meshTriangle.index3 : meshTriangle.index2] );
lanes[2][lane] = b3Mul( meshScale, meshVertices[meshFlipped ? meshTriangle.index2 : meshTriangle.index3] );
}
else
{
B3_ASSERT( shapeA->type == b3_heightShape );
b3Triangle triangle = b3GetHeightFieldTriangle( shapeA->heightField, triangleIndex );
lanes[0][lane] = triangle.vertices[0];
lanes[1][lane] = triangle.vertices[1];
lanes[2][lane] = triangle.vertices[2];
}
}
// Transform triangle into the shape frame
b3FloatW4 vx[3], vy[3], vz[3];
for ( int k = 0; k < 3; ++k )
{
b3FloatW4 ax = b3SetW4( lanes[k][0].x, lanes[k][1].x, lanes[k][2].x, lanes[k][3].x );
b3FloatW4 ay = b3SetW4( lanes[k][0].y, lanes[k][1].y, lanes[k][2].y, lanes[k][3].y );
b3FloatW4 az = b3SetW4( lanes[k][0].z, lanes[k][1].z, lanes[k][2].z, lanes[k][3].z );
vx[k] = b3AddW4( b3AddW4( b3AddW4( b3MulW4( cxx, ax ), b3MulW4( cyx, ay ) ), b3MulW4( czx, az ) ), px );
vy[k] = b3AddW4( b3AddW4( b3AddW4( b3MulW4( cxy, ax ), b3MulW4( cyy, ay ) ), b3MulW4( czy, az ) ), py );
vz[k] = b3AddW4( b3AddW4( b3AddW4( b3MulW4( cxz, ax ), b3MulW4( cyz, ay ) ), b3MulW4( czz, az ) ), pz );
}
uint32_t keepMask = ( 1u << laneCount ) - 1u;
if ( hullB != NULL )
{
b3FloatW4 lowerX = b3SubW4( b3MinW4( vx[0], b3MinW4( vx[1], vx[2] ) ), marginW );
b3FloatW4 lowerY = b3SubW4( b3MinW4( vy[0], b3MinW4( vy[1], vy[2] ) ), marginW );
b3FloatW4 lowerZ = b3SubW4( b3MinW4( vz[0], b3MinW4( vz[1], vz[2] ) ), marginW );
b3FloatW4 upperX = b3AddW4( b3MaxW4( vx[0], b3MaxW4( vx[1], vx[2] ) ), marginW );
b3FloatW4 upperY = b3AddW4( b3MaxW4( vy[0], b3MaxW4( vy[1], vy[2] ) ), marginW );
b3FloatW4 upperZ = b3AddW4( b3MaxW4( vz[0], b3MaxW4( vz[1], vz[2] ) ), marginW );
b3FloatW4 separated = b3OrW4( b3LessThanW4( upperX, hullLowerX ), b3GreaterThanW4( lowerX, hullUpperX ) );
separated = b3OrW4( separated, b3OrW4( b3LessThanW4( upperY, hullLowerY ), b3GreaterThanW4( lowerY, hullUpperY ) ) );
separated = b3OrW4( separated, b3OrW4( b3LessThanW4( upperZ, hullLowerZ ), b3GreaterThanW4( lowerZ, hullUpperZ ) ) );
uint32_t cullMask = keepMask & (uint32_t)b3MoveMaskW4( separated );
keepMask &= ~cullMask;
while ( cullMask != 0 )
{
uint32_t lane = b3CTZ32( cullMask );
cullMask &= cullMask - 1u;
triangleCaches[baseIndex + (int)lane].cache.satCache = (b3SATCache){ 0 };
}
}
if ( keepMask == 0 )
{
continue;
}
float x[3][4], y[3][4], z[3][4];
for ( int k = 0; k < 3; ++k )
{
b3StoreW4( x[k], vx[k] );
b3StoreW4( y[k], vy[k] );
b3StoreW4( z[k], vz[k] );
}
while ( keepMask != 0 )
{
uint32_t lane = b3CTZ32( keepMask );
keepMask &= keepMask - 1u;
b3Vec3* survivor = survivorVertices + 3 * survivorCount;
survivor[0] = (b3Vec3){ x[0][lane], y[0][lane], z[0][lane] };
survivor[1] = (b3Vec3){ x[1][lane], y[1][lane], z[1][lane] };
survivor[2] = (b3Vec3){ x[2][lane], y[2][lane], z[2][lane] };
survivorIndices[survivorCount] = baseIndex + (int)lane;
survivorCount += 1;
}
}
for ( int survivorIndex = 0; survivorIndex < survivorCount && totalPointCount + 3 < pointBufferCapacity; ++survivorIndex )
{
int index = survivorIndices[survivorIndex];
int triangleIndex = triangleCaches[index].triangleIndex;
const b3Vec3* vertices = survivorVertices + 3 * survivorIndex;
b3Triangle triangle;
if ( shapeA->type == b3_meshShape )
@ -594,12 +729,6 @@ bool b3ComputeMeshManifolds( b3World* world, int workerIndex, b3Contact* contact
triangle = b3GetHeightFieldTriangle( shapeA->heightField, triangleIndex );
}
// Transform triangle into the shape frame
b3Vec3 vertices[3];
vertices[0] = b3Add( b3MulMV( relativeMatrix, triangle.vertices[0] ), transformAtoB.p );
vertices[1] = b3Add( b3MulMV( relativeMatrix, triangle.vertices[1] ), transformAtoB.p );
vertices[2] = b3Add( b3MulMV( relativeMatrix, triangle.vertices[2] ), transformAtoB.p );
b3ContactCache* cache = &triangleCaches[index].cache;
int pointCapacity = pointBufferCapacity - totalPointCount;
b3LocalManifold* manifold = manifoldBuffer + manifoldCount;

View file

@ -831,6 +831,24 @@ b3ShapeExtent b3ComputeShapeExtent( const b3Shape* shape, b3Vec3 localCenter )
return extent;
}
float b3ComputeShapeMinExtent( const b3Shape* shape, b3Vec3 localCenter )
{
switch ( shape->type )
{
case b3_capsuleShape:
return shape->capsule.radius;
case b3_sphereShape:
return shape->sphere.radius;
case b3_hullShape:
return shape->hull->innerRadius;
default:
return b3ComputeShapeExtent( shape, localCenter ).minExtent;
}
}
b3CastOutput b3RayCastShape( const b3Shape* shape, b3Transform transform, const b3RayCastInput* input )
{
b3RayCastInput localInput = *input;
@ -2079,8 +2097,6 @@ typedef struct b3MeshImpactContext
b3Vec3 meshLocalCentroidB1, meshLocalCentroidB2;
float fallbackRadius;
bool isSensor;
int visitCount;
} b3MeshImpactContext;
static bool b3MeshTimeOfImpactFcn( b3Vec3 a, b3Vec3 b, b3Vec3 c, int triangleIndex, void* context )
@ -2089,8 +2105,6 @@ static bool b3MeshTimeOfImpactFcn( b3Vec3 a, b3Vec3 b, b3Vec3 c, int triangleInd
b3MeshImpactContext* toiContext = context;
toiContext->visitCount += 1;
// Early out for parallel movement
b3Vec3 c1 = toiContext->meshLocalCentroidB1;
b3Vec3 c2 = toiContext->meshLocalCentroidB2;
@ -2270,8 +2284,8 @@ b3TOIOutput b3ShapeTimeOfImpact( b3Shape* shapeA, b3Shape* shapeB, b3Sweep* swee
b3Vec3 localCentroidB = b3GetShapeCentroid( shapeB );
context.localCentroidB = localCentroidB;
b3ShapeExtent extents = b3ComputeShapeExtent( shapeB, context.localCentroidB );
context.fallbackRadius = b3MaxFloat( 0.75f * extents.minExtent, B3_SPECULATIVE_DISTANCE );
float minExtent = b3ComputeShapeMinExtent( shapeB, context.localCentroidB );
context.fallbackRadius = b3MaxFloat( 0.75f * minExtent, B3_SPECULATIVE_DISTANCE );
// Swept bounds of shapeB
b3AABB bounds = b3ComputeSweptShapeAABB( shapeB, sweepB, maxFraction );
@ -2290,8 +2304,6 @@ b3TOIOutput b3ShapeTimeOfImpact( b3Shape* shapeA, b3Shape* shapeB, b3Sweep* swee
// todo implement b3MeshTimeOfImpact and b3HeightFieldTimeOfImpact
// Note: assuming mesh is static
uint64_t ticks = b3GetTicks();
b3MeshImpactContext context = { 0 };
context.toiInput.sweepA = *sweepA;
context.toiInput.proxyA.count = 3;
@ -2322,8 +2334,8 @@ b3TOIOutput b3ShapeTimeOfImpact( b3Shape* shapeA, b3Shape* shapeB, b3Sweep* swee
context.meshLocalCentroidB1 = b3InvTransformPoint( xfA, b3TransformPoint( xfB1, localCentroidB ) );
context.meshLocalCentroidB2 = b3InvTransformPoint( xfA, b3TransformPoint( xfB2, localCentroidB ) );
b3ShapeExtent extents = b3ComputeShapeExtent( shapeB, context.localCentroidB );
context.fallbackRadius = b3MaxFloat( 0.5f * extents.minExtent, B3_LINEAR_SLOP );
float minExtent = b3ComputeShapeMinExtent( shapeB, context.localCentroidB );
context.fallbackRadius = b3MaxFloat( 0.5f * minExtent, B3_LINEAR_SLOP );
// Swept bounds of shapeB
// todo pass in xfA to get local bounds directly
@ -2341,12 +2353,6 @@ b3TOIOutput b3ShapeTimeOfImpact( b3Shape* shapeA, b3Shape* shapeB, b3Sweep* swee
b3QueryHeightField( shapeA->heightField, localBounds, b3MeshTimeOfImpactFcn, &context );
}
float ms = b3GetMilliseconds( ticks );
if ( ms > 1000.0f * b3GetStallThreshold() )
{
b3Log( "CCD stall: visited %d triangles", context.visitCount );
}
return context.toiOutput;
}

View file

@ -5,6 +5,7 @@
#include "math_internal.h"
#include "box3d/collision.h"
#include "box3d/types.h"
#include <stdbool.h>
@ -80,6 +81,7 @@ void b3DestroyShapeAllocations( b3World* world, b3Shape* shape );
b3MassData b3ComputeShapeMass( const b3Shape* shape );
b3ShapeExtent b3ComputeShapeExtent( const b3Shape* shape, b3Vec3 localCenter );
float b3ComputeShapeMinExtent( const b3Shape* shape, b3Vec3 localCenter );
b3AABB b3ComputeSweptSphereAABB( const b3Sphere* shape, b3Transform xf1, b3Transform xf2 );
b3AABB b3ComputeSweptCapsuleAABB( const b3Capsule* shape, b3Transform xf1, b3Transform xf2 );
@ -134,7 +136,52 @@ static inline int b3GetHeightFieldTriangleCount( const b3HeightFieldData* height
}
// Mesh
b3Triangle b3GetMeshTriangle( const b3Mesh* mesh, int triangleIndex );
// Making this inline made the tree benchmarks faster.
static inline b3Triangle b3GetMeshTriangle( const b3Mesh* mesh, int triangleIndex )
{
B3_ASSERT( 0 <= triangleIndex && triangleIndex < mesh->data->triangleCount );
const b3MeshTriangle* triangles = b3GetMeshTriangles( mesh->data );
const uint8_t* flags = b3GetMeshFlags( mesh->data );
const b3Vec3* vertices = b3GetMeshVertices( mesh->data );
b3Triangle result;
b3MeshTriangle triangle = triangles[triangleIndex];
uint8_t triangleFlags = flags[triangleIndex];
b3Vec3 scale = mesh->scale;
result.vertices[0] = b3Mul( scale, vertices[triangle.index1] );
result.i1 = triangle.index1;
if ( scale.x * scale.y * scale.z < 0.0f )
{
result.vertices[1] = b3Mul( scale, vertices[triangle.index3] );
result.vertices[2] = b3Mul( scale, vertices[triangle.index2] );
result.i2 = triangle.index3;
result.i3 = triangle.index2;
// mesh is inverted, so concave edges are now convex
result.flags = 0;
result.flags |= ( triangleFlags & b3_inverseConcaveEdge1 ) ? b3_concaveEdge1 : 0;
result.flags |= ( triangleFlags & b3_inverseConcaveEdge2 ) ? b3_concaveEdge2 : 0;
result.flags |= ( triangleFlags & b3_inverseConcaveEdge3 ) ? b3_concaveEdge3 : 0;
}
else
{
result.vertices[1] = b3Mul( scale, vertices[triangle.index2] );
result.vertices[2] = b3Mul( scale, vertices[triangle.index3] );
result.i2 = triangle.index2;
result.i3 = triangle.index3;
result.flags = triangleFlags;
}
return result;
}
bool b3IsValidMesh( const b3MeshData* meshData );
static inline bool b3ShouldShapesCollide( b3Filter filterA, b3Filter filterB )

View file

@ -163,6 +163,14 @@ static inline bool b3AnyTrueW4( b3FloatW4 mask )
return ( vget_lane_u32( p, 0 ) | vget_lane_u32( p, 1 ) ) != 0;
}
static inline int b3MoveMaskW4( b3FloatW4 mask )
{
const int32_t shifts[4] = { 0, 1, 2, 3 };
uint32x4_t bits = vshlq_u32( vshrq_n_u32( vreinterpretq_u32_f32( mask ), 31 ), vld1q_s32( shifts ) );
uint32x2_t p = vorr_u32( vget_low_u32( bits ), vget_high_u32( bits ) );
return (int)( vget_lane_u32( p, 0 ) | vget_lane_u32( p, 1 ) );
}
// component-wise returns mask ? b : a
static inline b3FloatW4 b3BlendW4( b3FloatW4 a, b3FloatW4 b, b3FloatW4 mask )
{

View file

@ -202,6 +202,11 @@ static inline bool b3AnyTrueW4( b3FloatW4 mask )
return mask.x != 0.0f || mask.y != 0.0f || mask.z != 0.0f || mask.w != 0.0f;
}
static inline int b3MoveMaskW4( b3FloatW4 mask )
{
return ( mask.x != 0.0f ? 1 : 0 ) | ( mask.y != 0.0f ? 2 : 0 ) | ( mask.z != 0.0f ? 4 : 0 ) | ( mask.w != 0.0f ? 8 : 0 );
}
// component-wise returns mask ? b : a
static inline b3FloatW4 b3BlendW4( b3FloatW4 a, b3FloatW4 b, b3FloatW4 mask )
{

View file

@ -159,6 +159,11 @@ static inline bool b3AnyTrueW4( b3FloatW4 mask )
return _mm_movemask_ps( mask ) != 0;
}
static inline int b3MoveMaskW4( b3FloatW4 mask )
{
return _mm_movemask_ps( mask );
}
// component-wise returns mask ? b : a
static inline b3FloatW4 b3BlendW4( b3FloatW4 a, b3FloatW4 b, b3FloatW4 mask )
{

View file

@ -8,6 +8,11 @@
// Macros to avoid name clashes between W4 and W8 functions existing in the same code base.
// At the same time this helps to avoid code duplication.
// Help Intellisense from being confused.
#if defined( __INTELLISENSE__ ) && !defined( B3_SIMD_WIDTH )
#define B3_SIMD_WIDTH 8
#endif
#if B3_SIMD_WIDTH == 8
#define B3_WIDE( name ) name##W8
#elif B3_SIMD_WIDTH == 4
@ -18,8 +23,9 @@
#define B3_WIDE_ALIGNMENT ( 4 * B3_SIMD_WIDTH )
#define b3FloatW B3_WIDE( b3Float )
#define b3Vec3W B3_WIDE( b3Vec3 )
typedef B3_WIDE( b3Float ) b3FloatW;
typedef B3_WIDE( b3Vec3 ) b3Vec3W;
#define b3ZeroW B3_WIDE( b3Zero )
#define b3SplatW B3_WIDE( b3Splat )
#define b3SetW B3_WIDE( b3Set )

View file

@ -3,6 +3,7 @@
#include "solver.h"
#include "aabb.h"
#include "arena_allocator.h"
#include "bitset.h"
#include "body.h"
@ -326,8 +327,6 @@ typedef struct b3ContinuousContext
float sensorFractions[B2_MAX_CONTINUOUS_SENSOR_HITS];
int sensorCount;
int visitCount;
int distanceIterations;
int pushBackIterations;
int rootIterations;
@ -340,7 +339,6 @@ static bool b3ContinuousQueryCallback( int proxyId, uint64_t userData, void* con
int shapeId = (int)userData;
b3ContinuousContext* continuousContext = context;
continuousContext->visitCount += 1;
b3Shape* fastShape = continuousContext->fastShape;
b3BodySim* fastBodySim = continuousContext->fastBodySim;
@ -411,8 +409,6 @@ static bool b3ContinuousQueryCallback( int proxyId, uint64_t userData, void* con
}
}
uint64_t ticks = b3GetTicks();
// todo does having a sweep on shapeA help with bullets?
b3Sweep sweepA = b3MakeRelativeSweep( bodySim, continuousContext->base );
@ -457,25 +453,44 @@ static bool b3ContinuousQueryCallback( int proxyId, uint64_t userData, void* con
}
}
float ms = b3GetMilliseconds( ticks );
if ( ms > 1000.0f * b3GetStallThreshold() )
{
const char* nameFast = b3FindNameWithDefault( &world->names, fastBody->nameId, "NULL" );
const char* name = b3FindNameWithDefault( &world->names, body->nameId, "NULL" );
b3Log( "CCD stall: duration %.1f ms for %s versus %s", ms, nameFast, name );
}
// Continue query
return true;
}
static bool b3IsShapeFast( const b3Shape* shape, b3Vec3 centroid1, b3Vec3 centroid2, float rotationChord, float safetyFactor )
{
float radius;
switch ( shape->type )
{
case b3_sphereShape:
radius = shape->sphere.radius;
break;
case b3_capsuleShape:
radius = 0.5f * b3Distance( shape->capsule.center1, shape->capsule.center2 ) + shape->capsule.radius;
break;
case b3_hullShape:
{
b3Vec3 farthestPoint = b3FarthestPointOnAABB( shape->hull->aabb, shape->localCentroid );
radius = b3Distance( farthestPoint, shape->localCentroid );
}
break;
default:
return true;
}
float minExtent = b3ComputeShapeMinExtent( shape, shape->localCentroid );
float maxMotion = b3Distance( centroid1, centroid2 ) + rotationChord * radius;
return maxMotion > safetyFactor * minExtent;
}
// Continuous collision of dynamic versus static
static void b3SolveContinuous( b3World* world, int bodySimIndex, b3TaskContext* taskContext, float dt )
{
b3TracyCZoneNC( ccd, "CCD", b3_colorDarkGoldenRod, true );
uint64_t ticks = b3GetTicks();
b3SolverSet* awakeSet = b3Array_Get( world->solverSets, b3_awakeSet );
b3BodySim* fastBodySim = b3Array_Get( awakeSet->bodySims, bodySimIndex );
B3_VALIDATE( fastBodySim->flags & b3_isFast );
@ -506,6 +521,8 @@ static void b3SolveContinuous( b3World* world, int bodySimIndex, b3TaskContext*
context.fraction = 1.0f;
bool isBullet = ( fastBodySim->flags & b3_isBullet ) != 0;
float safetyFactor = fastBody->safetyFactor;
float rotationChord = 2.0f * b3Length( b3InvMulQuat( sweep.q1, sweep.q2 ).v );
int shapeId = fastBody->headShapeId;
while ( shapeId != B3_NULL_INDEX )
@ -536,6 +553,15 @@ static void b3SolveContinuous( b3World* world, int bodySimIndex, b3TaskContext*
continue;
}
if ( isBullet == false )
{
bool isShapeFast = b3IsShapeFast( fastShape, context.centroid1, context.centroid2, rotationChord, safetyFactor );
if ( isShapeFast == false )
{
continue;
}
}
b3AABB sweptBox = b3AABB_Union( box1, box2 );
b3DynamicTree_Query( staticTree, sweptBox, B3_DEFAULT_MASK_BITS, false, b3ContinuousQueryCallback, &context );
@ -660,17 +686,6 @@ static void b3SolveContinuous( b3World* world, int bodySimIndex, b3TaskContext*
taskContext->pushBackIterations = b3MaxInt( taskContext->pushBackIterations, context.pushBackIterations );
taskContext->rootIterations = b3MaxInt( taskContext->rootIterations, context.rootIterations );
float ms = b3GetMilliseconds( ticks );
if ( ms > 1000.0f * b3GetStallThreshold() )
{
const char* nameFast = b3FindNameWithDefault( &world->names, fastBody->nameId, "NULL" );
b3Vec3 c1 = sweep.c1;
b3Vec3 c2 = sweep.c2;
int vc = context.visitCount;
b3Log( "CCD stall: duration %.1f ms and visit count %d for %s: c1 = (%g, %g, %g), c2 = (%g, %g, %g)", ms, vc, nameFast,
c1.x, c1.y, c1.z, c2.x, c2.y, c2.z );
}
b3TracyCZoneEnd( ccd );
}
@ -985,7 +1000,7 @@ static void b3ExecuteBlock( b3SolverStage* stage, b3StepContext* context, b3Solv
break;
case b3_stagePrepareContacts:
b3PrepareContacts_Mesh( block, context );
b3PrepareContacts_MeshWide( block, context );
break;
case b3_stageIntegrateVelocities:
@ -1003,7 +1018,7 @@ static void b3ExecuteBlock( b3SolverStage* stage, b3StepContext* context, b3Solv
}
else
{
b3WarmStartContacts_Mesh( block, context );
b3WarmStartContacts_MeshWide( block, context );
}
break;
@ -1019,7 +1034,7 @@ static void b3ExecuteBlock( b3SolverStage* stage, b3StepContext* context, b3Solv
}
else
{
b3PushContacts_Mesh( block, context );
b3PushContacts_MeshWide( block, context );
}
break;
@ -1039,7 +1054,7 @@ static void b3ExecuteBlock( b3SolverStage* stage, b3StepContext* context, b3Solv
}
else
{
b3SolveContacts_Mesh( block, context );
b3SolveContacts_MeshWide( block, context );
}
break;
@ -1050,7 +1065,7 @@ static void b3ExecuteBlock( b3SolverStage* stage, b3StepContext* context, b3Solv
}
else if ( blockType == b3_graphContactBlock )
{
b3ApplyRestitution_Mesh( block, context );
b3ApplyRestitution_MeshWide( block, context );
}
break;
@ -1059,7 +1074,7 @@ static void b3ExecuteBlock( b3SolverStage* stage, b3StepContext* context, b3Solv
break;
case b3_stageStoreImpulses:
b3StoreImpulses_Mesh( block, context, workerIndex );
b3StoreImpulses_MeshWide( block, context, workerIndex );
break;
}
}
@ -1456,7 +1471,7 @@ void b3Solve( b3World* world, b3StepContext* stepContext )
// Prepare buffers for continuous collision (fast bodies)
b3AtomicStoreInt( &stepContext->bulletBodyCount, 0 );
stepContext->bulletBodies = (int*)b3StackAlloc( &world->stack, awakeBodyCount * sizeof( int ), "bullet bodies" );
stepContext->bulletBodies = b3StackAlloc( &world->stack, awakeBodyCount * sizeof( int ), "bullet bodies" );
b3ConstraintGraph* graph = &world->constraintGraph;
b3GraphColor* colors = graph->colors;
@ -1493,28 +1508,28 @@ void b3Solve( b3World* world, b3StepContext* stepContext )
// The blocks are a mix of convex contact, mesh contact, and joint blocks
int activeColorIndices[B3_GRAPH_COLOR_COUNT];
int colorWideContactCounts[B3_GRAPH_COLOR_COUNT];
int colorContactCounts[B3_GRAPH_COLOR_COUNT];
// int colorManifoldCounts[B3_GRAPH_COLOR_COUNT];
int colorMeshContactCounts[B3_GRAPH_COLOR_COUNT];
int colorMeshGroupCounts[B3_GRAPH_COLOR_COUNT];
int colorJointCounts[B3_GRAPH_COLOR_COUNT];
b3BlockDim graphWideContactDims[B3_GRAPH_COLOR_COUNT];
b3BlockDim graphContactDims[B3_GRAPH_COLOR_COUNT];
b3BlockDim graphMeshContactDims[B3_GRAPH_COLOR_COUNT];
b3BlockDim graphJointDims[B3_GRAPH_COLOR_COUNT];
int graphBlockCount = 0;
// c is the active color index
int wideContactCount = 0;
int contactCount = 0;
int manifoldCount = 0;
int meshContactCount = 0;
int meshGroupCount = 0;
int jointCount = 0;
int c = 0;
for ( int i = 0; i < B3_GRAPH_COLOR_COUNT - 1; ++i )
{
b3GraphColor* color = colors + i;
int colorConvexContactCount = color->convexContacts.count;
int colorContactCount = color->contacts.count;
int colorMeshContactCount = color->contacts.count;
int colorJointCount = color->jointSims.count;
if ( colorConvexContactCount + colorContactCount + colorJointCount == 0 )
if ( colorConvexContactCount + colorMeshContactCount + colorJointCount == 0 )
{
continue;
}
@ -1526,24 +1541,21 @@ void b3Solve( b3World* world, b3StepContext* stepContext )
wideContactCount += colorWideConstraintCount;
colorWideContactCounts[c] = colorWideConstraintCount;
colorContactCounts[c] = colorContactCount;
contactCount += colorContactCount;
colorMeshContactCounts[c] = colorMeshContactCount;
meshContactCount += colorMeshContactCount;
// Compute manifold starts and accumulate manifold count
for ( int j = 0; j < colorContactCount; ++j )
{
color->contacts.data[j].manifoldStart = manifoldCount;
manifoldCount += color->contacts.data[j].manifoldCount;
}
int colorMeshGroupCount = colorMeshContactCount > 0 ? ( ( colorMeshContactCount - 1 ) >> simdShift ) + 1 : 0;
colorMeshGroupCounts[c] = colorMeshGroupCount;
meshGroupCount += colorMeshGroupCount;
colorJointCounts[c] = colorJointCount;
jointCount += colorJointCount;
// Solver block dimensions
graphWideContactDims[c] = b3ComputeBlockCount( colorWideConstraintCount, minContactsPerBlock, maxBlockCount );
graphContactDims[c] = b3ComputeBlockCount( colorContactCount, minContactsPerBlock, maxBlockCount );
graphMeshContactDims[c] = b3ComputeBlockCount( colorMeshGroupCount, minContactsPerBlock, maxBlockCount );
graphJointDims[c] = b3ComputeBlockCount( colorJointCount, minJointsPerBlock, maxBlockCount );
graphBlockCount += graphWideContactDims[c].count + graphContactDims[c].count + graphJointDims[c].count;
graphBlockCount += graphWideContactDims[c].count + graphMeshContactDims[c].count + graphJointDims[c].count;
c += 1;
}
@ -1553,15 +1565,94 @@ void b3Solve( b3World* world, b3StepContext* stepContext )
// partitioned into uniformly sized blocks. Color info is consulted inside the task via
// a small span array, so blocks do not need to honor color boundaries here.
b3BlockDim convexPrepareDim = b3ComputeBlockCount( wideContactCount, minContactsPerBlock, maxBlockCount );
b3BlockDim meshPrepareDim = b3ComputeBlockCount( contactCount, minContactsPerBlock, maxBlockCount );
b3BlockDim meshPrepareDim = b3ComputeBlockCount( meshGroupCount, minContactsPerBlock, maxBlockCount );
b3BlockDim jointPrepareDim = b3ComputeBlockCount( jointCount, minJointsPerBlock, maxBlockCount );
int wideContactByteCount = b3GetWideContactConstraintByteCount( world->simdWidth );
void* wideConstraints = b3StackAlloc( &world->stack, wideContactCount * wideContactByteCount, "wide contacts" );
b3ContactConstraint* contactConstraints =
(b3ContactConstraint*)b3StackAlloc( &world->stack, contactCount * sizeof( b3ContactConstraint ), "contacts" );
b3ManifoldConstraint* manifoldConstraints = (b3ManifoldConstraint*)b3StackAlloc(
&world->stack, manifoldCount * sizeof( b3ManifoldConstraint ), "manifold constraints" );
int* meshLaneOrder = b3StackAlloc( &world->stack, meshContactCount * sizeof( int ), "mesh lane order" );
int* meshManifoldStarts = b3StackAlloc( &world->stack, ( meshGroupCount + 1 ) * sizeof( int ), "mesh manifold starts" );
// Sort mesh contacts by manifold count so that contacts with more manifolds are solved
// together keeping the SIMD lanes fuller and less ragged.
int meshSlotCount = 0;
{
int maxManifoldCount = 0;
for ( int i = 0; i < activeColorCount; ++i )
{
b3GraphColor* color = colors + activeColorIndices[i];
const b3ContactSpec* specs = color->contacts.data;
int colorContactCount = colorMeshContactCounts[i];
for ( int j = 0; j < colorContactCount; ++j )
{
maxManifoldCount = b3MaxInt( maxManifoldCount, specs[j].manifoldCount );
}
}
int* bucketStarts = b3StackAlloc( &world->stack, ( maxManifoldCount + 1 ) * sizeof( int ), "mesh buckets" );
int orderBase = 0;
int groupBase = 0;
for ( int i = 0; i < activeColorCount; ++i )
{
b3GraphColor* color = colors + activeColorIndices[i];
int colorContactCount = colorMeshContactCounts[i];
if ( colorContactCount == 0 )
{
continue;
}
const b3ContactSpec* specs = color->contacts.data;
int* order = meshLaneOrder + orderBase;
for ( int k = 0; k <= maxManifoldCount; ++k )
{
bucketStarts[k] = 0;
}
for ( int j = 0; j < colorContactCount; ++j )
{
bucketStarts[specs[j].manifoldCount] += 1;
}
int start = 0;
for ( int k = maxManifoldCount; k >= 0; --k )
{
int count = bucketStarts[k];
bucketStarts[k] = start;
start += count;
}
for ( int j = 0; j < colorContactCount; ++j )
{
order[bucketStarts[specs[j].manifoldCount]++] = j;
}
int colorMeshGroupCount = colorMeshGroupCounts[i];
for ( int g = 0; g < colorMeshGroupCount; ++g )
{
int slotCount = specs[order[g << simdShift]].manifoldCount;
meshManifoldStarts[groupBase + g] = meshSlotCount;
meshSlotCount += slotCount;
}
orderBase += colorContactCount;
groupBase += colorMeshGroupCount;
}
B3_ASSERT( orderBase == meshContactCount );
B3_ASSERT( groupBase == meshGroupCount );
meshManifoldStarts[meshGroupCount] = meshSlotCount;
b3StackFree( &world->stack, bucketStarts );
}
int wideMeshConstraintByteCount = b3GetWideMeshConstraintByteCount( world->simdWidth );
int wideMeshManifoldByteCount = b3GetWideMeshManifoldByteCount( world->simdWidth );
void* wideMeshConstraints =
b3StackAlloc( &world->stack, meshGroupCount * wideMeshConstraintByteCount + meshSlotCount * wideMeshManifoldByteCount,
"wide mesh constraints" );
b3GraphColor* overflow = colors + B3_OVERFLOW_INDEX;
int overflowCount = overflow->contacts.count;
@ -1572,23 +1663,23 @@ void b3Solve( b3World* world, b3StepContext* stepContext )
overflowManifoldCount += overflow->contacts.data[i].manifoldCount;
}
overflow->contactConstraints = (b3ContactConstraint*)b3StackAlloc(
&world->stack, overflowCount * sizeof( b3ContactConstraint ), "overflow contacts" );
overflow->manifoldConstraints = (b3ManifoldConstraint*)b3StackAlloc(
&world->stack, overflowManifoldCount * sizeof( b3ManifoldConstraint ), "overflow manifolds" );
overflow->contactConstraints =
b3StackAlloc( &world->stack, overflowCount * sizeof( b3ContactConstraint ), "overflow contacts" );
overflow->manifoldConstraints =
b3StackAlloc( &world->stack, overflowManifoldCount * sizeof( b3ManifoldConstraint ), "overflow manifolds" );
// Build the span table for the flat prepare/store parallel-for while I slice the
// wide constraint buffer across colors. One entry per active color plus a sentinel
// at wideContactCount.
b3WidePrepareSpan widePrepareSpans[B3_GRAPH_COLOR_COUNT + 1];
b3ContactPrepareSpan contactPrepareSpans[B3_GRAPH_COLOR_COUNT + 1];
b3MeshPrepareSpan meshPrepareSpans[B3_GRAPH_COLOR_COUNT + 1];
b3JointPrepareSpan jointPrepareSpans[B3_GRAPH_COLOR_COUNT + 1];
// Distribute transient constraints to each graph color and prepare spans
// todo it might be simpler for solver blocks to index into the global arrays
{
int wideBase = 0;
int contactBase = 0;
int meshGroupBase = 0;
int jointBase = 0;
for ( int i = 0; i < activeColorCount; ++i )
{
@ -1615,19 +1706,25 @@ void b3Solve( b3World* world, b3StepContext* stepContext )
}
int colorContactCount = color->contacts.count;
contactPrepareSpans[i].start = contactBase;
contactPrepareSpans[i].count = colorContactCount;
contactPrepareSpans[i].contacts = color->contacts.data;
meshPrepareSpans[i].start = meshGroupBase;
meshPrepareSpans[i].count = colorContactCount;
meshPrepareSpans[i].contacts = color->contacts.data;
meshPrepareSpans[i].order = meshLaneOrder + contactBase;
if ( colorContactCount == 0 )
{
color->contactConstraints = NULL;
color->contactConstraintCount = 0;
color->wideMeshConstraints = NULL;
color->wideMeshManifoldStarts = NULL;
color->wideMeshConstraintCount = 0;
}
else
{
color->contactConstraints = contactConstraints + contactBase;
color->contactConstraintCount = colorContactCount;
int colorMeshGroupCount = colorMeshGroupCounts[i];
color->wideMeshConstraints = (uint8_t*)wideMeshConstraints + meshGroupBase * wideMeshConstraintByteCount +
meshManifoldStarts[meshGroupBase] * wideMeshManifoldByteCount;
color->wideMeshManifoldStarts = meshManifoldStarts + meshGroupBase;
color->wideMeshConstraintCount = colorMeshGroupCount;
meshGroupBase += colorMeshGroupCount;
contactBase += colorContactCount;
}
@ -1643,10 +1740,12 @@ void b3Solve( b3World* world, b3StepContext* stepContext )
widePrepareSpans[activeColorCount].contacts = NULL;
B3_ASSERT( wideBase == wideContactCount );
contactPrepareSpans[activeColorCount].start = contactCount;
contactPrepareSpans[activeColorCount].count = 0;
contactPrepareSpans[activeColorCount].contacts = NULL;
B3_ASSERT( contactBase == contactCount );
meshPrepareSpans[activeColorCount].start = meshGroupCount;
meshPrepareSpans[activeColorCount].count = 0;
meshPrepareSpans[activeColorCount].contacts = NULL;
meshPrepareSpans[activeColorCount].order = NULL;
B3_ASSERT( contactBase == meshContactCount );
B3_ASSERT( meshGroupBase == meshGroupCount );
jointPrepareSpans[activeColorCount].start = jointCount;
jointPrepareSpans[activeColorCount].count = 0;
@ -1688,17 +1787,13 @@ void b3Solve( b3World* world, b3StepContext* stepContext )
// b3_stageStoreImpulses
stageCount += 1;
b3SolverStage* stages = (b3SolverStage*)b3StackAlloc( &world->stack, stageCount * sizeof( b3SolverStage ), "stages" );
b3SyncBlock* bodyBlocks =
(b3SyncBlock*)b3StackAlloc( &world->stack, bodyDim.count * sizeof( b3SyncBlock ), "body blocks" );
b3SolverStage* stages = b3StackAlloc( &world->stack, stageCount * sizeof( b3SolverStage ), "stages" );
b3SyncBlock* bodyBlocks = b3StackAlloc( &world->stack, bodyDim.count * sizeof( b3SyncBlock ), "body blocks" );
b3SyncBlock* convexBlocks =
(b3SyncBlock*)b3StackAlloc( &world->stack, convexPrepareDim.count * sizeof( b3SyncBlock ), "convex blocks" );
b3SyncBlock* meshBlocks =
(b3SyncBlock*)b3StackAlloc( &world->stack, meshPrepareDim.count * sizeof( b3SyncBlock ), "mesh blocks" );
b3SyncBlock* jointBlocks =
(b3SyncBlock*)b3StackAlloc( &world->stack, jointPrepareDim.count * sizeof( b3SyncBlock ), "joint blocks" );
b3SyncBlock* graphBlocks =
(b3SyncBlock*)b3StackAlloc( &world->stack, graphBlockCount * sizeof( b3SyncBlock ), "graph blocks" );
b3StackAlloc( &world->stack, convexPrepareDim.count * sizeof( b3SyncBlock ), "convex blocks" );
b3SyncBlock* meshBlocks = b3StackAlloc( &world->stack, meshPrepareDim.count * sizeof( b3SyncBlock ), "mesh blocks" );
b3SyncBlock* jointBlocks = b3StackAlloc( &world->stack, jointPrepareDim.count * sizeof( b3SyncBlock ), "joint blocks" );
b3SyncBlock* graphBlocks = b3StackAlloc( &world->stack, graphBlockCount * sizeof( b3SyncBlock ), "graph blocks" );
// Split an awake island. This modifies:
// - stack allocator
@ -1727,7 +1822,7 @@ void b3Solve( b3World* world, b3StepContext* stepContext )
// Prepare blocks as a single flat parallel-for over the whole constraint range.
// The task walks spans to decode flat slot indices back to per-color arrays.
b3InitBlocks( convexBlocks, convexPrepareDim, wideContactCount, b3_wideContactBlock, UINT8_MAX );
b3InitBlocks( meshBlocks, meshPrepareDim, contactCount, b3_contactBlock, UINT8_MAX );
b3InitBlocks( meshBlocks, meshPrepareDim, meshGroupCount, b3_contactBlock, UINT8_MAX );
b3InitBlocks( jointBlocks, jointPrepareDim, jointCount, b3_jointBlock, UINT8_MAX );
// Prepare graph work blocks. Each color gets joint blocks followed by contact blocks.
@ -1746,10 +1841,10 @@ void b3Solve( b3World* world, b3StepContext* stepContext )
colorIndex );
baseGraphBlock += graphWideContactDims[i].count;
b3InitBlocks( baseGraphBlock, graphContactDims[i], colorContactCounts[i], b3_graphContactBlock, colorIndex );
baseGraphBlock += graphContactDims[i].count;
b3InitBlocks( baseGraphBlock, graphMeshContactDims[i], colorMeshGroupCounts[i], b3_graphContactBlock, colorIndex );
baseGraphBlock += graphMeshContactDims[i].count;
graphBlockCounts[i] = graphJointDims[i].count + graphWideContactDims[i].count + graphContactDims[i].count;
graphBlockCounts[i] = graphJointDims[i].count + graphWideContactDims[i].count + graphMeshContactDims[i].count;
}
B3_ASSERT( (ptrdiff_t)( baseGraphBlock - graphBlocks ) == graphBlockCount );
@ -1784,10 +1879,10 @@ void b3Solve( b3World* world, b3StepContext* stepContext )
stepContext->wideConstraints = wideConstraints;
stepContext->widePrepareSpans = widePrepareSpans;
stepContext->wideContactCount = wideContactCount;
stepContext->manifoldConstraints = manifoldConstraints;
stepContext->contactConstraints = contactConstraints;
stepContext->contactPrepareSpans = contactPrepareSpans;
stepContext->overflowSpans = overflowSpans;
stepContext->wideMeshConstraints = wideMeshConstraints;
stepContext->wideMeshManifoldStarts = meshManifoldStarts;
stepContext->meshPrepareSpans = meshPrepareSpans;
stepContext->jointPrepareSpans = jointPrepareSpans;
b3AtomicStoreU32( &stepContext->atomicSyncBits, 0 );
b3AtomicStoreInt( &stepContext->mainClaimed, 0 );
@ -1882,7 +1977,7 @@ void b3Solve( b3World* world, b3StepContext* stepContext )
b3ValidateNoMoved( &world->broadPhase );
// Finalize bodies. Must happen after the constraint solver and after island splitting.
b3ParallelFor( world, &b3FinalizeBodiesTask, awakeBodyCount, 16, stepContext, "ccd" );
b3ParallelFor( world, &b3FinalizeBodiesTask, awakeBodyCount, 8, stepContext, "ccd" );
// Free in reverse order
b3StackFree( &world->stack, graphBlocks );
@ -1893,8 +1988,9 @@ void b3Solve( b3World* world, b3StepContext* stepContext )
b3StackFree( &world->stack, stages );
b3StackFree( &world->stack, overflow->manifoldConstraints );
b3StackFree( &world->stack, overflow->contactConstraints );
b3StackFree( &world->stack, manifoldConstraints );
b3StackFree( &world->stack, contactConstraints );
b3StackFree( &world->stack, wideMeshConstraints );
b3StackFree( &world->stack, meshManifoldStarts );
b3StackFree( &world->stack, meshLaneOrder );
b3StackFree( &world->stack, wideConstraints );
world->profile.transforms = b3GetMilliseconds( transformTicks );

View file

@ -163,6 +163,14 @@ typedef struct b3ContactPrepareSpan
b3ContactSpec* contacts;
} b3ContactPrepareSpan;
typedef struct b3MeshPrepareSpan
{
int start;
int count;
b3ContactSpec* contacts;
int* order;
} b3MeshPrepareSpan;
typedef struct b3JointPrepareSpan
{
int start;
@ -216,16 +224,16 @@ typedef struct b3StepContext
// Flat view of the wide contact constraint array used by prepare and store.
// prepareSpans has activeColorCount + 1 entries, the last being a sentinel
// at wideContactCount. wideContactConstraints is the contiguous base
// pointer; per-color slices live at colors[i].wideConstraints.
// pointer. Per-color slices live at colors[i].wideConstraints.
void* wideConstraints;
b3WidePrepareSpan* widePrepareSpans;
int wideContactCount;
// Similar for mesh/overflow contact constraints
struct b3ManifoldConstraint* manifoldConstraints;
struct b3ContactConstraint* contactConstraints;
b3ContactPrepareSpan* contactPrepareSpans;
// Similar for overflow contact constraints.
b3ContactPrepareSpan* overflowSpans;
void* wideMeshConstraints;
int* wideMeshManifoldStarts;
b3MeshPrepareSpan* meshPrepareSpans;
b3JointPrepareSpan* jointPrepareSpans;
int activeColorCount;

View file

@ -19,27 +19,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 273
#define RAGDOLL_HASH 0x0F4DD1EB
#define WAVE_PILE_SLEEP_STEP 314
#define WAVE_PILE_HASH 0x03B509B6
#define RAGDOLL_SLEEP_STEP 266
#define RAGDOLL_HASH 0x68FC7728
#define WAVE_PILE_SLEEP_STEP 312
#define WAVE_PILE_HASH 0xFD6BD36B
#define QUERY_SPAWN_SLEEP_STEP 242
#define QUERY_SPAWN_HASH 0x9B96A098
#define QUERY_SPAWN_HIT_COUNT 59
#define QUERY_SPAWN_QUERY_HASH 0x5B4429DC
#define MESH_DROP_SLEEP_STEP 217
#define MESH_DROP_HASH 0xCF946437
#define MESH_DROP_SLEEP_STEP 218
#define MESH_DROP_HASH 0x26F9A566
#else
#define RAGDOLL_SLEEP_STEP 274
#define RAGDOLL_HASH 0x773AB8ED
#define WAVE_PILE_SLEEP_STEP 314
#define WAVE_PILE_HASH 0x8CB8F656
#define RAGDOLL_SLEEP_STEP 267
#define RAGDOLL_HASH 0x9BCA269D
#define WAVE_PILE_SLEEP_STEP 270
#define WAVE_PILE_HASH 0xAD7C3901
#define QUERY_SPAWN_SLEEP_STEP 242
#define QUERY_SPAWN_HASH 0xF1EDEF47
#define QUERY_SPAWN_HIT_COUNT 59
#define QUERY_SPAWN_QUERY_HASH 0xE3271F3D
#define MESH_DROP_SLEEP_STEP 217
#define MESH_DROP_HASH 0x256BC40B
#define MESH_DROP_SLEEP_STEP 218
#define MESH_DROP_HASH 0x0A23CFA2
#endif
// The goldens above pin exact values for the default four point manifold. A build that
@ -436,6 +436,118 @@ static int SingleRollingMixTest( int workerCount, DeterminismResult* reference )
return 0;
}
// Mesh contacts are grouped across lanes by manifold count and padded to the largest count in the group. Width 8 groups
// differ from width 4 groups, so mixing large and small shapes on a fine mesh gives each body a width dependent number
// of padded manifolds. The hash covers velocities and impulses bitwise to catch a padded manifold flipping a signed zero.
static int SingleMeshMixTest( int workerCount, DeterminismResult* reference )
{
b3WorldDef worldDef = b3DefaultWorldDef();
worldDef.workerCount = workerCount;
b3WorldId worldId = b3CreateWorld( &worldDef );
b3MeshData* mesh = b3CreateWaveMesh( 40, 40, 0.5f, 0.3f, 0.2f, 0.3f );
{
b3BodyDef groundDef = b3DefaultBodyDef();
groundDef.position = (b3Pos){ -10.0f, 0.0f, -10.0f };
b3BodyId groundId = b3CreateBody( worldId, &groundDef );
b3ShapeDef shapeDef = b3DefaultShapeDef();
b3CreateMeshShape( groundId, &shapeDef, mesh, b3Vec3_one );
}
enum
{
rowCount = 8,
columnCount = 8,
bodyCount = rowCount * columnCount
};
b3BodyId bodyIds[bodyCount];
b3BoxHull smallBox = b3MakeBoxHull( 0.2f, 0.2f, 0.2f );
b3BoxHull largeBox = b3MakeBoxHull( 0.9f, 0.3f, 0.9f );
b3Sphere sphere = { { 0.0f, 0.0f, 0.0f }, 0.3f };
b3Capsule capsule = { { -0.5f, 0.0f, 0.0f }, { 0.5f, 0.0f, 0.0f }, 0.2f };
uint32_t seed = 314159u;
for ( int i = 0; i < bodyCount; ++i )
{
int row = i / columnCount;
int column = i % columnCount;
b3BodyDef bodyDef = b3DefaultBodyDef();
bodyDef.type = b3_dynamicBody;
bodyDef.position = (b3Pos){ 2.2f * (float)column - 7.7f, 1.2f, 2.2f * (float)row - 7.7f };
bodyIds[i] = b3CreateBody( worldId, &bodyDef );
seed = seed * 1664525u + 1013904223u;
b3ShapeDef shapeDef = b3DefaultShapeDef();
shapeDef.density = 1.0f;
shapeDef.baseMaterial.rollingResistance = ( seed >> 28 ) < 5 ? 0.2f : 0.0f;
switch ( ( seed >> 24 ) & 3 )
{
case 0:
b3CreateHullShape( bodyIds[i], &shapeDef, &largeBox.base );
break;
case 1:
b3CreateHullShape( bodyIds[i], &shapeDef, &smallBox.base );
break;
case 2:
b3CreateSphereShape( bodyIds[i], &shapeDef, &sphere );
break;
default:
b3CreateCapsuleShape( bodyIds[i], &shapeDef, &capsule );
break;
}
float sign = ( i & 1 ) ? -1.0f : 1.0f;
b3Body_SetLinearVelocity( bodyIds[i], (b3Vec3){ -0.0f, -0.5f, -0.0f } );
b3Body_SetAngularVelocity( bodyIds[i], (b3Vec3){ -0.0f, sign * 0.5f, -0.0f } );
}
uint32_t hash = B3_HASH_INIT;
int stepCount = 180;
for ( int step = 0; step < stepCount; ++step )
{
b3World_Step( worldId, 1.0f / 60.0f, 4 );
for ( int i = 0; i < bodyCount; ++i )
{
b3WorldTransform xf = b3Body_GetTransform( bodyIds[i] );
hash = b3Hash( hash, (const uint8_t*)&xf, sizeof( b3WorldTransform ) );
b3Vec3 v = b3Body_GetLinearVelocity( bodyIds[i] );
b3Vec3 w = b3Body_GetAngularVelocity( bodyIds[i] );
hash = HashFloats( hash, &v.x, 3 );
hash = HashFloats( hash, &w.x, 3 );
b3ContactData contactData[8];
int contactCount = b3Body_GetContactData( bodyIds[i], contactData, ARRAY_COUNT( contactData ) );
for ( int c = 0; c < contactCount; ++c )
{
for ( int m = 0; m < contactData[c].manifoldCount; ++m )
{
const b3Manifold* manifold = contactData[c].manifolds + m;
hash = HashFloats( hash, &manifold->rollingImpulse.x, 3 );
hash = HashFloats( hash, &manifold->frictionImpulse.x, 3 );
hash = HashFloats( hash, &manifold->twistImpulse, 1 );
for ( int p = 0; p < manifold->pointCount; ++p )
{
hash = HashFloats( hash, &manifold->points[p].normalImpulse, 1 );
hash = HashFloats( hash, &manifold->points[p].totalNormalImpulse, 1 );
}
}
}
}
}
b3DestroyWorld( worldId );
b3DestroyMesh( mesh );
ENSURE( EnsureRepeatable( reference, (DeterminismResult){ .sleepStep = stepCount, .hash = hash } ) == 0 );
return 0;
}
typedef int SceneFcn( int workerCount, DeterminismResult* reference );
static int RunSceneAtWidth( SceneFcn* scene, int width, DeterminismResult* result )
@ -458,7 +570,7 @@ static int SIMDWidthTest( void )
}
SceneFcn* scenes[] = {
SingleMultithreadingTest, SingleWavePileTest, SingleQuerySpawnTest, SingleMeshDropTest, SingleRollingMixTest,
SingleMultithreadingTest, SingleWavePileTest, SingleQuerySpawnTest, SingleMeshDropTest, SingleRollingMixTest, SingleMeshMixTest,
};
for ( int i = 0; i < ARRAY_COUNT( scenes ); ++i )