Tree perf (#179)
- Optimizations for falling trees. - Increased B3_MAX_MESH_CONTACT_TRIANGLES from 256 to 512.
This commit is contained in:
parent
e77352cd60
commit
16f7f4cf4c
36 changed files with 1926 additions and 1110 deletions
|
|
@ -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,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,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,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,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,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
|
||||
|
|
|
|||
|
9
benchmark/amd7950x_avx2/mesh_drop.csv
Normal file
9
benchmark/amd7950x_avx2/mesh_drop.csv
Normal 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,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,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,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,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,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,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,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
|
||||
|
|
|
|||
|
|
|
@ -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 },
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
|
|
|||
|
|
@ -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 );
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
|
|
|||
|
|
@ -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 );
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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();
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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
13
src/core.c
13
src/core.c
|
|
@ -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 );
|
||||
|
|
|
|||
153
src/mesh.c
153
src/mesh.c
|
|
@ -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;
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
|
|
|||
38
src/shape.c
38
src/shape.c
|
|
@ -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;
|
||||
}
|
||||
|
||||
|
|
|
|||
49
src/shape.h
49
src/shape.h
|
|
@ -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 )
|
||||
|
|
|
|||
|
|
@ -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 )
|
||||
{
|
||||
|
|
|
|||
|
|
@ -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 )
|
||||
{
|
||||
|
|
|
|||
|
|
@ -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 )
|
||||
{
|
||||
|
|
|
|||
|
|
@ -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 )
|
||||
|
|
|
|||
280
src/solver.c
280
src/solver.c
|
|
@ -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 );
|
||||
|
|
|
|||
18
src/solver.h
18
src/solver.h
|
|
@ -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;
|
||||
|
|
|
|||
|
|
@ -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 )
|
||||
|
|
|
|||
Loading…
Reference in a new issue