diff --git a/benchmark/amd7950x_avx2/convex_pile.csv b/benchmark/amd7950x_avx2/convex_pile.csv index 1fa06e8..19ef32e 100644 --- a/benchmark/amd7950x_avx2/convex_pile.csv +++ b/benchmark/amd7950x_avx2/convex_pile.csv @@ -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 diff --git a/benchmark/amd7950x_avx2/joint_grid.csv b/benchmark/amd7950x_avx2/joint_grid.csv index 926e3f4..31fc1f6 100644 --- a/benchmark/amd7950x_avx2/joint_grid.csv +++ b/benchmark/amd7950x_avx2/joint_grid.csv @@ -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 diff --git a/benchmark/amd7950x_avx2/junkyard.csv b/benchmark/amd7950x_avx2/junkyard.csv index 7cb8746..8846ec1 100644 --- a/benchmark/amd7950x_avx2/junkyard.csv +++ b/benchmark/amd7950x_avx2/junkyard.csv @@ -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 diff --git a/benchmark/amd7950x_avx2/large_pyramid.csv b/benchmark/amd7950x_avx2/large_pyramid.csv index b06f305..8809f8f 100644 --- a/benchmark/amd7950x_avx2/large_pyramid.csv +++ b/benchmark/amd7950x_avx2/large_pyramid.csv @@ -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 diff --git a/benchmark/amd7950x_avx2/large_world.csv b/benchmark/amd7950x_avx2/large_world.csv index 75041ab..757e17c 100644 --- a/benchmark/amd7950x_avx2/large_world.csv +++ b/benchmark/amd7950x_avx2/large_world.csv @@ -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 diff --git a/benchmark/amd7950x_avx2/many_pyramids.csv b/benchmark/amd7950x_avx2/many_pyramids.csv index c68aba0..d950ecf 100644 --- a/benchmark/amd7950x_avx2/many_pyramids.csv +++ b/benchmark/amd7950x_avx2/many_pyramids.csv @@ -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 diff --git a/benchmark/amd7950x_avx2/mesh_drop.csv b/benchmark/amd7950x_avx2/mesh_drop.csv new file mode 100644 index 0000000..bcfa419 --- /dev/null +++ b/benchmark/amd7950x_avx2/mesh_drop.csv @@ -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 diff --git a/benchmark/amd7950x_avx2/rain.csv b/benchmark/amd7950x_avx2/rain.csv index cfa7c5f..1ead3d8 100644 --- a/benchmark/amd7950x_avx2/rain.csv +++ b/benchmark/amd7950x_avx2/rain.csv @@ -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 diff --git a/benchmark/amd7950x_avx2/sleep.csv b/benchmark/amd7950x_avx2/sleep.csv index 20320b2..8296ea7 100644 --- a/benchmark/amd7950x_avx2/sleep.csv +++ b/benchmark/amd7950x_avx2/sleep.csv @@ -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 diff --git a/benchmark/amd7950x_avx2/spinner.csv b/benchmark/amd7950x_avx2/spinner.csv index 920a029..ae89955 100644 --- a/benchmark/amd7950x_avx2/spinner.csv +++ b/benchmark/amd7950x_avx2/spinner.csv @@ -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 diff --git a/benchmark/amd7950x_avx2/trees100.csv b/benchmark/amd7950x_avx2/trees100.csv index ad34128..caf3120 100644 --- a/benchmark/amd7950x_avx2/trees100.csv +++ b/benchmark/amd7950x_avx2/trees100.csv @@ -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 diff --git a/benchmark/amd7950x_avx2/trees25.csv b/benchmark/amd7950x_avx2/trees25.csv index fd12855..d3968d4 100644 --- a/benchmark/amd7950x_avx2/trees25.csv +++ b/benchmark/amd7950x_avx2/trees25.csv @@ -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 diff --git a/benchmark/amd7950x_avx2/trees50.csv b/benchmark/amd7950x_avx2/trees50.csv index 6f91345..b90215e 100644 --- a/benchmark/amd7950x_avx2/trees50.csv +++ b/benchmark/amd7950x_avx2/trees50.csv @@ -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 diff --git a/benchmark/amd7950x_avx2/washer.csv b/benchmark/amd7950x_avx2/washer.csv index f671a36..75cdfdf 100644 --- a/benchmark/amd7950x_avx2/washer.csv +++ b/benchmark/amd7950x_avx2/washer.csv @@ -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 diff --git a/benchmark/main.c b/benchmark/main.c index 3474139..429cf78 100644 --- a/benchmark/main.c +++ b/benchmark/main.c @@ -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 }, diff --git a/include/box3d/constants.h b/include/box3d/constants.h index cdc1910..259dcb9 100644 --- a/include/box3d/constants.h +++ b/include/box3d/constants.h @@ -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 diff --git a/samples/sample_benchmark.cpp b/samples/sample_benchmark.cpp index 35e235e..d3dc640 100644 --- a/samples/sample_benchmark.cpp +++ b/samples/sample_benchmark.cpp @@ -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; diff --git a/samples/sample_continuous.cpp b/samples/sample_continuous.cpp index ee2137a..6ff45dd 100644 --- a/samples/sample_continuous.cpp +++ b/samples/sample_continuous.cpp @@ -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 ); diff --git a/shared/benchmarks.c b/shared/benchmarks.c index a128fbb..29535a6 100644 --- a/shared/benchmarks.c +++ b/shared/benchmarks.c @@ -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; diff --git a/shared/benchmarks.h b/shared/benchmarks.h index d74f1dc..35ef0be 100644 --- a/shared/benchmarks.h +++ b/shared/benchmarks.h @@ -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 ); diff --git a/src/constraint_graph.h b/src/constraint_graph.h index fcda3a8..56bfdff 100644 --- a/src/constraint_graph.h +++ b/src/constraint_graph.h @@ -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 diff --git a/src/contact_solver.c b/src/contact_solver.c index 7b64af2..ad41b41 100644 --- a/src/contact_solver.c +++ b/src/contact_solver.c @@ -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(); +} diff --git a/src/contact_solver.h b/src/contact_solver.h index c1cec85..d7773db 100644 --- a/src/contact_solver.h +++ b/src/contact_solver.h @@ -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 ); diff --git a/src/contact_solver_wide.inl b/src/contact_solver_wide.inl index e00e571..8573e83 100644 --- a/src/contact_solver_wide.inl +++ b/src/contact_solver_wide.inl @@ -7,41 +7,32 @@ #include "simd.h" #include "simd_wide.h" -#define b3Vec2W B3_WIDE( b3Vec2 ) -#define b3QuatW B3_WIDE( b3Quat ) -#define b3SymMatrix2W B3_WIDE( b3SymMatrix2 ) -#define b3SymMatrix3W B3_WIDE( b3SymMatrix3 ) -#define b3Matrix3W B3_WIDE( b3Matrix3 ) -#define b3BodyStateW B3_WIDE( b3BodyState ) -#define b3ContactConstraintPointWide B3_WIDE( b3ContactConstraintPoint ) -#define b3ContactConstraintWide B3_WIDE( b3ContactConstraint ) - // Wide vec2 -typedef struct b3Vec2W +typedef struct B3_WIDE( b3Vec2 ) { b3FloatW x, y; } b3Vec2W; // Wide quaternion -typedef struct b3QuatW +typedef struct B3_WIDE( b3Quat ) { b3Vec3W V; b3FloatW S; } b3QuatW; // Wide symmetric matrix2 -typedef struct b3SymMatrix2W +typedef struct B3_WIDE( b3SymMatrix2 ) { b3FloatW cxx, cxy, cyy; } b3SymMatrix2W; // Wide symmetric matrix3 -typedef struct b3SymMatrix3W +typedef struct B3_WIDE( b3SymMatrix3 ) { b3FloatW cxx, cxy, cxz, cyy, cyz, czz; } b3SymMatrix3W; -typedef struct b3Matrix3W +typedef struct B3_WIDE( b3Matrix3 ) { b3Vec3W cx, cy, cz; } b3Matrix3W; @@ -102,35 +93,6 @@ static inline b3Vec3W b3MulAddMVW( b3Vec3W a, b3SymMatrix3W m, b3Vec3W b ) return (b3Vec3W){ b3AddW( a.X, c.X ), b3AddW( a.Y, c.Y ), b3AddW( a.Z, c.Z ) }; } -static inline b3Matrix3W b3MakeMatrixFromQuatW( b3QuatW q ) -{ - b3FloatW x2 = b3AddW( q.V.X, q.V.X ); - b3FloatW y2 = b3AddW( q.V.Y, q.V.Y ); - b3FloatW z2 = b3AddW( q.V.Z, q.V.Z ); - b3FloatW xx2 = b3MulW( q.V.X, x2 ); - b3FloatW yy2 = b3MulW( q.V.Y, y2 ); - b3FloatW zz2 = b3MulW( q.V.Z, z2 ); - b3FloatW xy2 = b3MulW( q.V.X, y2 ); - b3FloatW xz2 = b3MulW( q.V.X, z2 ); - b3FloatW yz2 = b3MulW( q.V.Y, z2 ); - b3FloatW xw2 = b3MulW( q.S, x2 ); - b3FloatW yw2 = b3MulW( q.S, y2 ); - b3FloatW zw2 = b3MulW( q.S, z2 ); - b3FloatW one = b3SplatW( 1.0f ); - - b3Matrix3W m; - m.cx.X = b3SubW( one, b3AddW( yy2, zz2 ) ); - m.cx.Y = b3AddW( xy2, zw2 ); - m.cx.Z = b3SubW( xz2, yw2 ); - m.cy.X = b3SubW( xy2, zw2 ); - m.cy.Y = b3SubW( one, b3AddW( xx2, zz2 ) ); - m.cy.Z = b3AddW( yz2, xw2 ); - m.cz.X = b3AddW( xz2, yw2 ); - m.cz.Y = b3SubW( yz2, xw2 ); - m.cz.Z = b3SubW( one, b3AddW( xx2, yy2 ) ); - return m; -} - static inline b3Vec3W b3MulM3VW( b3Matrix3W m, b3Vec3W a ) { b3Vec3W b = { @@ -159,7 +121,7 @@ static inline b3Vec3W b3InvRotateVectorW( b3QuatW q, b3Vec3W a ) // http://mmacklin.com/smallsteps.pdf // https://box2d.org/files/ErinCatto_SoftConstraints_GDC2011.pdf -typedef struct b3ContactConstraintPointWide +typedef struct B3_WIDE( b3ContactConstraintPoint ) { b3Vec3W anchorAs, anchorBs; b3FloatW baseSeparations; @@ -171,20 +133,10 @@ typedef struct b3ContactConstraintPointWide b3FloatW restitutionImpulses; } b3ContactConstraintPointWide; -// Solves one contact per lane -typedef struct b3ContactConstraintWide +typedef struct B3_WIDE( b3ContactManifold ) { - // These are base 1 - int indexA[B3_SIMD_WIDTH]; - int indexB[B3_SIMD_WIDTH]; + _Alignas( B3_WIDE_ALIGNMENT ) b3Vec3W normal; - int pointCounts[B3_SIMD_WIDTH]; - - b3FloatW invMassA, invMassB; - b3SymMatrix3W invIA, invIB; - b3Vec3W normal; - - // todo test computing the tangents on the fly, at least tangent2 b3Vec3W tangent1; b3Vec3W tangent2; @@ -194,32 +146,62 @@ typedef struct b3ContactConstraintWide b3FloatW twistImpulse; b3SymMatrix2W tangentMass; b3Vec2W frictionImpulse; - b3SymMatrix3W rollingMass; b3Vec3W rollingImpulse; - b3FloatW friction; - b3FloatW rollingResistance; + b3FloatW rollingMask; b3FloatW tangentVelocity1; b3FloatW tangentVelocity2; + + int pointCount; + + b3ContactConstraintPointWide points[B3_MAX_MANIFOLD_POINTS]; +} b3ContactManifoldWide; + +// Solves one contact per lane +typedef struct B3_WIDE( b3ContactConstraint ) +{ + // These are base 1 + _Alignas( B3_WIDE_ALIGNMENT ) int indexA[B3_SIMD_WIDTH]; + int indexB[B3_SIMD_WIDTH]; + + b3Contact* contacts[B3_SIMD_WIDTH]; + + b3FloatW invMassA, invMassB; + b3SymMatrix3W invIA, invIB; + b3SymMatrix3W rollingMass; + b3FloatW friction; + b3FloatW rollingResistance; b3FloatW restitution; - b3Manifold* manifolds[B3_SIMD_WIDTH]; - - // todo store the maximum point count per wide constraint - // to make this work I need zero initialization which is too - // expensive for all the wide constraint data. Instead - // the graph color should store the point count as a compact secondary - // transient array with zero initialization. - b3ContactConstraintPointWide points[B3_MAX_MANIFOLD_POINTS]; - + int manifoldCount; } b3ContactConstraintWide; +typedef struct B3_WIDE( b3ConvexConstraint ) +{ + b3ContactConstraintWide base; + b3ContactManifoldWide manifold; +} b3ConvexConstraintWide; + +_Static_assert( offsetof( b3ConvexConstraintWide, manifold ) == sizeof( b3ContactConstraintWide ), "wide constraint layout" ); +_Static_assert( sizeof( b3ContactConstraintWide ) % B3_WIDE_ALIGNMENT == 0, "wide constraint layout" ); +_Static_assert( sizeof( b3ContactManifoldWide ) % B3_WIDE_ALIGNMENT == 0, "wide constraint layout" ); + int B3_WIDE( b3GetWideContactConstraintByteCount )( void ) +{ + return sizeof( b3ConvexConstraintWide ); +} + +int B3_WIDE( b3GetWideMeshConstraintByteCount )( void ) { return sizeof( b3ContactConstraintWide ); } +int B3_WIDE( b3GetWideMeshManifoldByteCount )( void ) +{ + return sizeof( b3ContactManifoldWide ); +} + // wide version of b3BodyState -typedef struct b3BodyStateW +typedef struct B3_WIDE( b3BodyState ) { b3Vec3W v; b3Vec3W w; @@ -469,7 +451,7 @@ static const b3BodySim b3_zeroBodySim = { 0 }; #define B3_GATHER_LANES( wide, lanes, field ) wide = b3SetW( lanes[0]->field, lanes[1]->field, lanes[2]->field, lanes[3]->field ) #endif -static inline b3SymMatrix3W b3GatherInvInertiaW( const b3BodySim* const* simLanes ) +static inline b3SymMatrix3W b3GatherInvInertiaW( const b3BodySim* simLanes[B3_SIMD_WIDTH] ) { const float* i0 = &simLanes[0]->invInertiaWorld.cx.x; const float* i1 = &simLanes[1]->invInertiaWorld.cx.x; @@ -539,26 +521,378 @@ static inline b3Vec3W b3PerpW( b3Vec3W a ) b3FloatW s = b3BlendW( zero, b3DivW( b3SplatW( 1.0f ), b3SqrtW( lengthSquared ) ), valid ); return b3MulSVW( s, p ); } - -void B3_WIDE( b3PrepareContacts_Convex )( b3SolverBlock block, b3StepContext* context ) +static inline b3FloatW b3IntsToFloatW( const int* v ) +{ +#if B3_SIMD_WIDTH == 8 + return b3SetW( (float)v[0], (float)v[1], (float)v[2], (float)v[3], (float)v[4], (float)v[5], (float)v[6], (float)v[7] ); +#else + return b3SetW( (float)v[0], (float)v[1], (float)v[2], (float)v[3] ); +#endif +} + +// Get the contiguous manifold array that trails the constraint. +static inline b3ContactManifoldWide* b3GetManifoldsW( b3ContactConstraintWide* c ) +{ + return (b3ContactManifoldWide*)( c + 1 ); +} + +// fixedManifoldCount is 1 to indicate a convex contact with one manifold. +B3_FORCE_INLINE b3ContactConstraintWide* b3GetConstraintW( void* base, const int* manifoldStarts, int index, + int fixedManifoldCount ) +{ + if ( fixedManifoldCount > 0 ) + { + size_t stride = sizeof( b3ContactConstraintWide ) + fixedManifoldCount * sizeof( b3ContactManifoldWide ); + return (b3ContactConstraintWide*)( (uint8_t*)base + index * stride ); + } + + size_t offset = index * sizeof( b3ContactConstraintWide ) + + ( manifoldStarts[index] - manifoldStarts[0] ) * sizeof( b3ContactManifoldWide ); + return (b3ContactConstraintWide*)( (uint8_t*)base + offset ); +} + +B3_FORCE_INLINE b3ContactConstraintWide* b3NextConstraintW( b3ContactConstraintWide* c, int manifoldCount ) +{ + return (b3ContactConstraintWide*)( (uint8_t*)c + sizeof( b3ContactConstraintWide ) + + manifoldCount * sizeof( b3ContactManifoldWide ) ); +} + +// Contact constraint data is stored in a heterogious stream. Constraints followed by an array of constraint manifolds. +// [c1 m11 m12 c2 m21 c3 m31 m32 m33 ... ] +B3_FORCE_INLINE bool b3PrepareConstraintW( b3ContactConstraintWide* c, b3Contact* const* contacts, int manifoldCount, + b3StepContext* context, b3FloatW warmStartScale ) { - b3TracyCZoneNC( prepare_contact, "Prepare Contact", b3_colorYellow, true ); - b3World* world = context->world; b3BodySim* sims = context->sims; b3BodyState* states = context->states; #if B3_ENABLE_VALIDATION - b3Body* bodies = world->bodies.data; + b3Body* bodies = context->world->bodies.data; #endif - b3WidePrepareSpan* spans = context->widePrepareSpans; - b3ContactConstraintWide* wideBase = context->wideConstraints; b3FloatW zeroW = b3ZeroW(); b3FloatW oneW = b3SplatW( 1.0f ); b3FloatW twoW = b3SplatW( 2.0f ); - b3FloatW warmStartScale = world->enableWarmStarting ? oneW : zeroW; b3FloatW invTau = b3SplatW( 1.0f / B3_SPECULATIVE_DISTANCE ); b3FloatW minFrictionWeight = b3SplatW( B3_MIN_FRICTION_WEIGHT ); b3FloatW minDet = b3SplatW( 1000.0f * FLT_MIN ); + + const b3Contact* contactLanes[B3_SIMD_WIDTH]; + const b3BodySim* simLanesA[B3_SIMD_WIDTH]; + const b3BodySim* simLanesB[B3_SIMD_WIDTH]; + int manifoldCounts[B3_SIMD_WIDTH]; + int hitEventLanes = 0; + int maxManifoldCount = 0; + + for ( int lane = 0; lane < B3_SIMD_WIDTH; ++lane ) + { + b3Contact* contact = contacts[lane]; + c->contacts[lane] = contact; + + if ( contact != NULL ) + { + int indexA = b3DecodeAwakeIndex( contact->encodedBodySimA ); + int indexB = b3DecodeAwakeIndex( contact->encodedBodySimB ); + +#if B3_ENABLE_VALIDATION + b3Body* bodyA = bodies + contact->edges[0].bodyId; + b3Body* bodyB = bodies + contact->edges[1].bodyId; + B3_ASSERT( contact->encodedBodySimA == b3EncodeBodySimIndex( bodyA ) ); + B3_ASSERT( contact->encodedBodySimB == b3EncodeBodySimIndex( bodyB ) ); +#endif + + c->indexA[lane] = indexA + 1; + c->indexB[lane] = indexB + 1; + manifoldCounts[lane] = contact->manifoldCount; + + contactLanes[lane] = contact; + simLanesA[lane] = indexA == B3_NULL_INDEX ? &b3_zeroBodySim : sims + indexA; + simLanesB[lane] = indexB == B3_NULL_INDEX ? &b3_zeroBodySim : sims + indexB; + hitEventLanes |= ( contact->flags & b3_simEnableHitEvent ) != 0 ? 1 << lane : 0; + maxManifoldCount = b3MaxInt( maxManifoldCount, contact->manifoldCount ); + } + else + { + c->indexA[lane] = 0; + c->indexB[lane] = 0; + manifoldCounts[lane] = 0; + + contactLanes[lane] = &b3_zeroContact; + simLanesA[lane] = &b3_zeroBodySim; + simLanesB[lane] = &b3_zeroBodySim; + } + } + + B3_VALIDATE( maxManifoldCount == manifoldCount ); + B3_UNUSED( maxManifoldCount ); + c->manifoldCount = manifoldCount; + + b3FloatW mA, mB; + B3_GATHER_LANES( mA, simLanesA, invMass ); + B3_GATHER_LANES( mB, simLanesB, invMass ); + b3SymMatrix3W iA = b3GatherInvInertiaW( simLanesA ); + b3SymMatrix3W iB = b3GatherInvInertiaW( simLanesB ); + c->invMassA = mA; + c->invMassB = mB; + c->invIA = iA; + c->invIB = iB; + + b3Vec3W tangentVelocity; + B3_GATHER_LANES( c->friction, contactLanes, friction ); + B3_GATHER_LANES( c->rollingResistance, contactLanes, rollingResistance ); + B3_GATHER_LANES( c->restitution, contactLanes, restitution ); + B3_GATHER_LANES( tangentVelocity.X, contactLanes, tangentVelocity.x ); + B3_GATHER_LANES( tangentVelocity.Y, contactLanes, tangentVelocity.y ); + B3_GATHER_LANES( tangentVelocity.Z, contactLanes, tangentVelocity.z ); + + b3SymMatrix3W invIAB = b3AddSymW( iA, iB ); + if ( b3AllZeroW( c->rollingResistance ) == false ) + { + c->rollingMass = b3InvertSymW( invIAB ); + } + else + { + c->rollingMass = (b3SymMatrix3W){ zeroW, zeroW, zeroW, zeroW, zeroW, zeroW }; + } + + b3FloatW rollingResistanceMask = b3GreaterThanW( c->rollingResistance, zeroW ); + + b3ContactManifoldWide* manifolds = b3GetManifoldsW( c ); + for ( int manifoldIndex = 0; manifoldIndex < manifoldCount; ++manifoldIndex ) + { + b3ContactManifoldWide* cm = manifolds + manifoldIndex; + + const b3Manifold* manifoldLanes[B3_SIMD_WIDTH]; + int activeLanes[B3_SIMD_WIDTH]; + int pointCounts[B3_SIMD_WIDTH]; + int pointCount = 0; + for ( int lane = 0; lane < B3_SIMD_WIDTH; ++lane ) + { + if ( manifoldIndex < manifoldCounts[lane] ) + { + const b3Manifold* manifold = contactLanes[lane]->manifolds + manifoldIndex; + manifoldLanes[lane] = manifold; + activeLanes[lane] = 1; + pointCounts[lane] = manifold->pointCount; + } + else + { + manifoldLanes[lane] = &b3_zeroManifold; + activeLanes[lane] = 0; + pointCounts[lane] = 0; + } + + pointCount = b3MaxInt( pointCount, pointCounts[lane] ); + } + + B3_VALIDATE( 0 <= pointCount && pointCount <= B3_MAX_MANIFOLD_POINTS ); + cm->pointCount = pointCount; + cm->rollingMask = b3AndW( rollingResistanceMask, b3GreaterThanW( b3IntsToFloatW( activeLanes ), zeroW ) ); + + b3Vec3W normal, frictionImpulse, rollingImpulse; + b3FloatW twistImpulse; + { + const float* m0 = &manifoldLanes[0]->normal.x; + const float* m1 = &manifoldLanes[1]->normal.x; + const float* m2 = &manifoldLanes[2]->normal.x; + const float* m3 = &manifoldLanes[3]->normal.x; +#if B3_SIMD_WIDTH == 8 + const float* m4 = &manifoldLanes[4]->normal.x; + const float* m5 = &manifoldLanes[5]->normal.x; + const float* m6 = &manifoldLanes[6]->normal.x; + const float* m7 = &manifoldLanes[7]->normal.x; + b3TransposeW( b3LoadW( m0 ), b3LoadW( m1 ), b3LoadW( m2 ), b3LoadW( m3 ), b3LoadW( m4 ), b3LoadW( m5 ), b3LoadW( m6 ), + b3LoadW( m7 ), &normal.X, &normal.Y, &normal.Z, &twistImpulse, &frictionImpulse.X, &frictionImpulse.Y, + &frictionImpulse.Z, &rollingImpulse.X ); + rollingImpulse.Y = b3SetW( m0[8], m1[8], m2[8], m3[8], m4[8], m5[8], m6[8], m7[8] ); + rollingImpulse.Z = b3SetW( m0[9], m1[9], m2[9], m3[9], m4[9], m5[9], m6[9], m7[9] ); +#else + b3TransposeW( b3LoadW( m0 ), b3LoadW( m1 ), b3LoadW( m2 ), b3LoadW( m3 ), &normal.X, &normal.Y, &normal.Z, + &twistImpulse ); + b3TransposeW( b3LoadW( m0 + 4 ), b3LoadW( m1 + 4 ), b3LoadW( m2 + 4 ), b3LoadW( m3 + 4 ), &frictionImpulse.X, + &frictionImpulse.Y, &frictionImpulse.Z, &rollingImpulse.X ); + rollingImpulse.Y = b3SetW( m0[8], m1[8], m2[8], m3[8] ); + rollingImpulse.Z = b3SetW( m0[9], m1[9], m2[9], m3[9] ); +#endif + } + + b3Vec3W tangent1 = b3PerpW( normal ); + b3Vec3W tangent2 = b3CrossW( tangent1, normal ); + cm->normal = normal; + cm->tangent1 = tangent1; + cm->tangent2 = tangent2; + cm->tangentVelocity1 = b3DotW( tangentVelocity, tangent1 ); + cm->tangentVelocity2 = b3DotW( tangentVelocity, tangent2 ); + + cm->twistImpulse = b3MulW( warmStartScale, twistImpulse ); + cm->rollingImpulse.X = b3BlendW( zeroW, b3MulW( warmStartScale, rollingImpulse.X ), cm->rollingMask ); + cm->rollingImpulse.Y = b3BlendW( zeroW, b3MulW( warmStartScale, rollingImpulse.Y ), cm->rollingMask ); + cm->rollingImpulse.Z = b3BlendW( zeroW, b3MulW( warmStartScale, rollingImpulse.Z ), cm->rollingMask ); + cm->frictionImpulse.x = b3MulW( warmStartScale, b3DotW( frictionImpulse, tangent1 ) ); + cm->frictionImpulse.y = b3MulW( warmStartScale, b3DotW( frictionImpulse, tangent2 ) ); + + b3FloatW pointCountW = b3IntsToFloatW( pointCounts ); + + b3Vec3W centerA = { zeroW, zeroW, zeroW }; + b3Vec3W centerB = { zeroW, zeroW, zeroW }; + b3FloatW totalFrictionWeight = zeroW; + + for ( int pointIndex = 0; pointIndex < pointCount; ++pointIndex ) + { + b3ContactConstraintPointWide* cp = cm->points + pointIndex; + b3FloatW pointMask = b3GreaterThanW( pointCountW, b3SplatW( (float)pointIndex ) ); + + const float* p0 = (const float*)( manifoldLanes[0]->points + pointIndex ); + const float* p1 = (const float*)( manifoldLanes[1]->points + pointIndex ); + const float* p2 = (const float*)( manifoldLanes[2]->points + pointIndex ); + const float* p3 = (const float*)( manifoldLanes[3]->points + pointIndex ); +#if B3_SIMD_WIDTH == 8 + const float* p4 = (const float*)( manifoldLanes[4]->points + pointIndex ); + const float* p5 = (const float*)( manifoldLanes[5]->points + pointIndex ); + const float* p6 = (const float*)( manifoldLanes[6]->points + pointIndex ); + const float* p7 = (const float*)( manifoldLanes[7]->points + pointIndex ); +#endif + + b3Vec3W rA, rB; + b3FloatW separation, normalImpulse; +#if B3_SIMD_WIDTH == 8 + b3TransposeW( b3LoadW( p0 ), b3LoadW( p1 ), b3LoadW( p2 ), b3LoadW( p3 ), b3LoadW( p4 ), b3LoadW( p5 ), b3LoadW( p6 ), + b3LoadW( p7 ), &rA.X, &rA.Y, &rA.Z, &rB.X, &rB.Y, &rB.Z, &separation, &normalImpulse ); +#else + b3TransposeW( b3LoadW( p0 ), b3LoadW( p1 ), b3LoadW( p2 ), b3LoadW( p3 ), &rA.X, &rA.Y, &rA.Z, &rB.X ); + b3TransposeW( b3LoadW( p0 + 4 ), b3LoadW( p1 + 4 ), b3LoadW( p2 + 4 ), b3LoadW( p3 + 4 ), &rB.Y, &rB.Z, &separation, + &normalImpulse ); +#endif + + rA.X = b3BlendW( zeroW, rA.X, pointMask ); + rA.Y = b3BlendW( zeroW, rA.Y, pointMask ); + rA.Z = b3BlendW( zeroW, rA.Z, pointMask ); + rB.X = b3BlendW( zeroW, rB.X, pointMask ); + rB.Y = b3BlendW( zeroW, rB.Y, pointMask ); + rB.Z = b3BlendW( zeroW, rB.Z, pointMask ); + separation = b3BlendW( zeroW, separation, pointMask ); + normalImpulse = b3BlendW( zeroW, normalImpulse, pointMask ); + + // C0 friction center decay. Needed to prevent spinning top drift (GyroscopicPrecession sample). + // See details in b3PrepareContacts_Mesh. This code should stay in sync. + b3FloatW weight = b3MinW( b3MaxW( b3SubW( twoW, b3MulW( separation, invTau ) ), minFrictionWeight ), oneW ); + weight = b3BlendW( zeroW, weight, pointMask ); + centerA = b3MulAddSVW( centerA, weight, rA ); + centerB = b3MulAddSVW( centerB, weight, rB ); + totalFrictionWeight = b3AddW( totalFrictionWeight, weight ); + + cp->anchorAs = rA; + cp->anchorBs = rB; + cp->baseSeparations = b3SubW( separation, b3DotW( b3SubVW( rB, rA ), normal ) ); + cp->normalImpulses = b3MulW( warmStartScale, normalImpulse ); + cp->totalNormalImpulses = zeroW; + cp->relativeVelocities = zeroW; + cp->restitutionImpulses = zeroW; + + b3Vec3W rnA = b3CrossW( rA, normal ); + b3Vec3W rnB = b3CrossW( rB, normal ); + b3FloatW kNormal = b3AddW( mA, mB ); + kNormal = b3AddW( kNormal, b3DotW( rnA, b3MulMVW( iA, rnA ) ) ); + kNormal = b3AddW( kNormal, b3DotW( rnB, b3MulMVW( iB, rnB ) ) ); + b3FloatW valid = b3AndW( b3GreaterThanW( kNormal, zeroW ), pointMask ); + cp->normalMasses = b3BlendW( zeroW, b3DivW( oneW, kNormal ), valid ); + } + + b3FloatW frictionValid = b3GreaterThanW( totalFrictionWeight, zeroW ); + b3FloatW invWeight = b3BlendW( zeroW, b3DivW( oneW, totalFrictionWeight ), frictionValid ); + centerA = b3MulSVW( invWeight, centerA ); + centerB = b3MulSVW( invWeight, centerB ); + cm->centerA = centerA; + cm->centerB = centerB; + + for ( int pointIndex = 0; pointIndex < pointCount; ++pointIndex ) + { + b3ContactConstraintPointWide* cp = cm->points + pointIndex; + b3FloatW pointMask = b3GreaterThanW( pointCountW, b3SplatW( (float)pointIndex ) ); + b3Vec3W d = b3SubVW( cp->anchorAs, centerA ); + cp->leverArms = b3BlendW( zeroW, b3SqrtW( b3DotW( d, d ) ), pointMask ); + } + + { + b3Vec3W rtA1 = b3CrossW( centerA, tangent1 ); + b3Vec3W rtA2 = b3CrossW( centerA, tangent2 ); + b3Vec3W rtB1 = b3CrossW( centerB, tangent1 ); + b3Vec3W rtB2 = b3CrossW( centerB, tangent2 ); + b3Vec3W iArtA1 = b3MulMVW( iA, rtA1 ); + b3Vec3W iArtA2 = b3MulMVW( iA, rtA2 ); + b3Vec3W iBrtB1 = b3MulMVW( iB, rtB1 ); + b3Vec3W iBrtB2 = b3MulMVW( iB, rtB2 ); + + b3FloatW kxx = b3AddW( b3AddW( b3AddW( mA, mB ), b3DotW( rtA1, iArtA1 ) ), b3DotW( rtB1, iBrtB1 ) ); + b3FloatW kyy = b3AddW( b3AddW( b3AddW( mA, mB ), b3DotW( rtA2, iArtA2 ) ), b3DotW( rtB2, iBrtB2 ) ); + b3FloatW kxy = b3AddW( b3DotW( rtA1, iArtA2 ), b3DotW( rtB1, iBrtB2 ) ); + + b3FloatW det = b3SubW( b3MulW( kxx, kyy ), b3MulW( kxy, kxy ) ); + b3FloatW valid = b3AndW( b3GreaterThanW( b3AbsW( det ), minDet ), frictionValid ); + b3FloatW invDet = b3BlendW( zeroW, b3DivW( oneW, det ), valid ); + cm->tangentMass.cxx = b3MulW( invDet, kyy ); + cm->tangentMass.cxy = b3NegW( b3MulW( invDet, kxy ) ); + cm->tangentMass.cyy = b3MulW( invDet, kxx ); + } + + { + b3FloatW kTwist = b3DotW( normal, b3MulMVW( invIAB, normal ) ); + cm->twistMass = b3BlendW( zeroW, b3DivW( oneW, kTwist ), b3GreaterThanW( kTwist, zeroW ) ); + } + } + + // Only sample contact point normal velocity if needed. + bool haveRestitution = b3AnyTrueW( b3GreaterThanW( c->restitution, zeroW ) ); + if ( hitEventLanes != 0 || haveRestitution ) + { + b3BodyStateW bA = b3GatherBodies( states, c->indexA ); + b3BodyStateW bB = b3GatherBodies( states, c->indexB ); + + for ( int manifoldIndex = 0; manifoldIndex < manifoldCount; ++manifoldIndex ) + { + b3ContactManifoldWide* cm = manifolds + manifoldIndex; + + for ( int pointIndex = 0; pointIndex < cm->pointCount; ++pointIndex ) + { + b3ContactConstraintPointWide* cp = cm->points + pointIndex; + + b3Vec3W vrA = b3AddVW( bA.v, b3CrossW( bA.w, cp->anchorAs ) ); + b3Vec3W vrB = b3AddVW( bB.v, b3CrossW( bB.w, cp->anchorBs ) ); + b3FloatW vn = b3DotW( cm->normal, b3SubVW( vrB, vrA ) ); + cp->relativeVelocities = vn; + + if ( hitEventLanes != 0 ) + { + float normalVelocities[B3_SIMD_WIDTH]; + b3StoreW( normalVelocities, vn ); + + for ( int lane = 0; lane < B3_SIMD_WIDTH; ++lane ) + { + if ( ( hitEventLanes & ( 1 << lane ) ) == 0 || manifoldIndex >= manifoldCounts[lane] ) + { + continue; + } + + b3Manifold* manifold = c->contacts[lane]->manifolds + manifoldIndex; + if ( pointIndex < manifold->pointCount ) + { + manifold->points[pointIndex].normalVelocity = normalVelocities[lane]; + } + } + } + } + } + } + + return haveRestitution; +} + +void B3_WIDE( b3PrepareContacts_Convex )( b3SolverBlock block, b3StepContext* context ) +{ + b3TracyCZoneNC( prepare_contact, "Prepare Contact", b3_colorYellow, true ); + + b3World* world = context->world; + b3WidePrepareSpan* spans = context->widePrepareSpans; + void* wideBase = context->wideConstraints; + b3FloatW warmStartScale = world->enableWarmStarting ? b3SplatW( 1.0f ) : b3ZeroW(); bool anyRestitution = false; int wideIndex = block.startIndex; @@ -582,283 +916,26 @@ void B3_WIDE( b3PrepareContacts_Convex )( b3SolverBlock block, b3StepContext* co // Loop over color for ( ; wideIndex < colorWideEndIndex; ++wideIndex ) { - b3ContactConstraintWide* c = wideBase + wideIndex; + b3ContactConstraintWide* c = b3GetConstraintW( wideBase, NULL, wideIndex, 1 ); int localWideIndex = wideIndex - colorWideStart; - const b3Contact* contactLanes[B3_SIMD_WIDTH]; - const b3Manifold* manifoldLanes[B3_SIMD_WIDTH]; - const b3BodySim* simLanesA[B3_SIMD_WIDTH]; - const b3BodySim* simLanesB[B3_SIMD_WIDTH]; - int hitEventLanes = 0; - + b3Contact* contacts[B3_SIMD_WIDTH]; for ( int lane = 0; lane < B3_SIMD_WIDTH; ++lane ) { int contactIndex = B3_SIMD_WIDTH * localWideIndex + lane; if ( contactIndex < colorContactCount ) { - b3Contact* contact = b3Array_Get( world->contacts, contactIds[contactIndex] ); - B3_ASSERT( contact->manifoldCount == 1 ); - b3Manifold* manifold = contact->manifolds; - - int indexA = b3DecodeAwakeIndex( contact->encodedBodySimA ); - int indexB = b3DecodeAwakeIndex( contact->encodedBodySimB ); - -#if B3_ENABLE_VALIDATION - b3Body* bodyA = bodies + contact->edges[0].bodyId; - b3Body* bodyB = bodies + contact->edges[1].bodyId; - B3_ASSERT( contact->encodedBodySimA == b3EncodeBodySimIndex( bodyA ) ); - B3_ASSERT( contact->encodedBodySimB == b3EncodeBodySimIndex( bodyB ) ); -#endif - - c->indexA[lane] = indexA + 1; - c->indexB[lane] = indexB + 1; - c->pointCounts[lane] = manifold->pointCount; - c->manifolds[lane] = manifold; - - contactLanes[lane] = contact; - manifoldLanes[lane] = manifold; - simLanesA[lane] = indexA == B3_NULL_INDEX ? &b3_zeroBodySim : sims + indexA; - simLanesB[lane] = indexB == B3_NULL_INDEX ? &b3_zeroBodySim : sims + indexB; - hitEventLanes |= ( contact->flags & b3_simEnableHitEvent ) != 0 ? 1 << lane : 0; + contacts[lane] = b3Array_Get( world->contacts, contactIds[contactIndex] ); + B3_ASSERT( contacts[lane]->manifoldCount == 1 ); } else { - c->indexA[lane] = 0; - c->indexB[lane] = 0; - c->pointCounts[lane] = 0; - c->manifolds[lane] = NULL; - - contactLanes[lane] = &b3_zeroContact; - manifoldLanes[lane] = &b3_zeroManifold; - simLanesA[lane] = &b3_zeroBodySim; - simLanesB[lane] = &b3_zeroBodySim; + contacts[lane] = NULL; } } - b3FloatW mA, mB; - B3_GATHER_LANES( mA, simLanesA, invMass ); - B3_GATHER_LANES( mB, simLanesB, invMass ); - b3SymMatrix3W iA = b3GatherInvInertiaW( simLanesA ); - b3SymMatrix3W iB = b3GatherInvInertiaW( simLanesB ); - c->invMassA = mA; - c->invMassB = mB; - c->invIA = iA; - c->invIB = iB; - - b3Vec3W tangentVelocity; - B3_GATHER_LANES( c->friction, contactLanes, friction ); - B3_GATHER_LANES( c->rollingResistance, contactLanes, rollingResistance ); - B3_GATHER_LANES( c->restitution, contactLanes, restitution ); - B3_GATHER_LANES( tangentVelocity.X, contactLanes, tangentVelocity.x ); - B3_GATHER_LANES( tangentVelocity.Y, contactLanes, tangentVelocity.y ); - B3_GATHER_LANES( tangentVelocity.Z, contactLanes, tangentVelocity.z ); - - b3Vec3W normal, frictionImpulse, rollingImpulse; - b3FloatW twistImpulse; - { - const float* m0 = &manifoldLanes[0]->normal.x; - const float* m1 = &manifoldLanes[1]->normal.x; - const float* m2 = &manifoldLanes[2]->normal.x; - const float* m3 = &manifoldLanes[3]->normal.x; -#if B3_SIMD_WIDTH == 8 - const float* m4 = &manifoldLanes[4]->normal.x; - const float* m5 = &manifoldLanes[5]->normal.x; - const float* m6 = &manifoldLanes[6]->normal.x; - const float* m7 = &manifoldLanes[7]->normal.x; - b3TransposeW( b3LoadW( m0 ), b3LoadW( m1 ), b3LoadW( m2 ), b3LoadW( m3 ), b3LoadW( m4 ), b3LoadW( m5 ), - b3LoadW( m6 ), b3LoadW( m7 ), &normal.X, &normal.Y, &normal.Z, &twistImpulse, &frictionImpulse.X, - &frictionImpulse.Y, &frictionImpulse.Z, &rollingImpulse.X ); -#else - b3TransposeW( b3LoadW( m0 ), b3LoadW( m1 ), b3LoadW( m2 ), b3LoadW( m3 ), &normal.X, &normal.Y, &normal.Z, - &twistImpulse ); - b3TransposeW( b3LoadW( m0 + 4 ), b3LoadW( m1 + 4 ), b3LoadW( m2 + 4 ), b3LoadW( m3 + 4 ), &frictionImpulse.X, - &frictionImpulse.Y, &frictionImpulse.Z, &rollingImpulse.X ); -#endif -#if B3_SIMD_WIDTH == 8 - rollingImpulse.Y = b3SetW( m0[8], m1[8], m2[8], m3[8], m4[8], m5[8], m6[8], m7[8] ); - rollingImpulse.Z = b3SetW( m0[9], m1[9], m2[9], m3[9], m4[9], m5[9], m6[9], m7[9] ); -#else - rollingImpulse.Y = b3SetW( m0[8], m1[8], m2[8], m3[8] ); - rollingImpulse.Z = b3SetW( m0[9], m1[9], m2[9], m3[9] ); -#endif - } - - b3Vec3W tangent1 = b3PerpW( normal ); - b3Vec3W tangent2 = b3CrossW( tangent1, normal ); - c->normal = normal; - c->tangent1 = tangent1; - c->tangent2 = tangent2; - c->tangentVelocity1 = b3DotW( tangentVelocity, tangent1 ); - c->tangentVelocity2 = b3DotW( tangentVelocity, tangent2 ); - - b3FloatW rollingMask = b3GreaterThanW( c->rollingResistance, zeroW ); - c->twistImpulse = b3MulW( warmStartScale, twistImpulse ); - c->rollingImpulse.X = b3BlendW( zeroW, b3MulW( warmStartScale, rollingImpulse.X ), rollingMask ); - c->rollingImpulse.Y = b3BlendW( zeroW, b3MulW( warmStartScale, rollingImpulse.Y ), rollingMask ); - c->rollingImpulse.Z = b3BlendW( zeroW, b3MulW( warmStartScale, rollingImpulse.Z ), rollingMask ); - c->frictionImpulse.x = b3MulW( warmStartScale, b3DotW( frictionImpulse, tangent1 ) ); - c->frictionImpulse.y = b3MulW( warmStartScale, b3DotW( frictionImpulse, tangent2 ) ); - - b3FloatW pointCountW = -#if B3_SIMD_WIDTH == 8 - b3SetW( (float)c->pointCounts[0], (float)c->pointCounts[1], (float)c->pointCounts[2], (float)c->pointCounts[3], - (float)c->pointCounts[4], (float)c->pointCounts[5], (float)c->pointCounts[6], (float)c->pointCounts[7] ); -#else - b3SetW( (float)c->pointCounts[0], (float)c->pointCounts[1], (float)c->pointCounts[2], (float)c->pointCounts[3] ); -#endif - - b3Vec3W centerA = { zeroW, zeroW, zeroW }; - b3Vec3W centerB = { zeroW, zeroW, zeroW }; - b3FloatW totalFrictionWeight = zeroW; - - for ( int pointIndex = 0; pointIndex < B3_MAX_MANIFOLD_POINTS; ++pointIndex ) - { - b3ContactConstraintPointWide* cp = c->points + pointIndex; - b3FloatW pointMask = b3GreaterThanW( pointCountW, b3SplatW( (float)pointIndex ) ); - - const float* p0 = (const float*)( manifoldLanes[0]->points + pointIndex ); - const float* p1 = (const float*)( manifoldLanes[1]->points + pointIndex ); - const float* p2 = (const float*)( manifoldLanes[2]->points + pointIndex ); - const float* p3 = (const float*)( manifoldLanes[3]->points + pointIndex ); -#if B3_SIMD_WIDTH == 8 - const float* p4 = (const float*)( manifoldLanes[4]->points + pointIndex ); - const float* p5 = (const float*)( manifoldLanes[5]->points + pointIndex ); - const float* p6 = (const float*)( manifoldLanes[6]->points + pointIndex ); - const float* p7 = (const float*)( manifoldLanes[7]->points + pointIndex ); -#endif - - b3Vec3W rA, rB; - b3FloatW separation, normalImpulse; -#if B3_SIMD_WIDTH == 8 - b3TransposeW( b3LoadW( p0 ), b3LoadW( p1 ), b3LoadW( p2 ), b3LoadW( p3 ), b3LoadW( p4 ), b3LoadW( p5 ), - b3LoadW( p6 ), b3LoadW( p7 ), &rA.X, &rA.Y, &rA.Z, &rB.X, &rB.Y, &rB.Z, &separation, - &normalImpulse ); -#else - b3TransposeW( b3LoadW( p0 ), b3LoadW( p1 ), b3LoadW( p2 ), b3LoadW( p3 ), &rA.X, &rA.Y, &rA.Z, &rB.X ); - b3TransposeW( b3LoadW( p0 + 4 ), b3LoadW( p1 + 4 ), b3LoadW( p2 + 4 ), b3LoadW( p3 + 4 ), &rB.Y, &rB.Z, - &separation, &normalImpulse ); -#endif - - rA.X = b3BlendW( zeroW, rA.X, pointMask ); - rA.Y = b3BlendW( zeroW, rA.Y, pointMask ); - rA.Z = b3BlendW( zeroW, rA.Z, pointMask ); - rB.X = b3BlendW( zeroW, rB.X, pointMask ); - rB.Y = b3BlendW( zeroW, rB.Y, pointMask ); - rB.Z = b3BlendW( zeroW, rB.Z, pointMask ); - separation = b3BlendW( zeroW, separation, pointMask ); - normalImpulse = b3BlendW( zeroW, normalImpulse, pointMask ); - - // C0 friction center decay. Needed to prevent spinning top drift (GyroscopicPrecession sample). - // See details in b3PrepareContacts_Mesh. This code should stay in sync. - b3FloatW weight = b3MinW( b3MaxW( b3SubW( twoW, b3MulW( separation, invTau ) ), minFrictionWeight ), oneW ); - weight = b3BlendW( zeroW, weight, pointMask ); - centerA = b3MulAddSVW( centerA, weight, rA ); - centerB = b3MulAddSVW( centerB, weight, rB ); - totalFrictionWeight = b3AddW( totalFrictionWeight, weight ); - - cp->anchorAs = rA; - cp->anchorBs = rB; - cp->baseSeparations = b3SubW( separation, b3DotW( b3SubVW( rB, rA ), normal ) ); - cp->normalImpulses = b3MulW( warmStartScale, normalImpulse ); - cp->totalNormalImpulses = zeroW; - cp->relativeVelocities = zeroW; - cp->restitutionImpulses = zeroW; - - b3Vec3W rnA = b3CrossW( rA, normal ); - b3Vec3W rnB = b3CrossW( rB, normal ); - b3FloatW kNormal = b3AddW( mA, mB ); - kNormal = b3AddW( kNormal, b3DotW( rnA, b3MulMVW( iA, rnA ) ) ); - kNormal = b3AddW( kNormal, b3DotW( rnB, b3MulMVW( iB, rnB ) ) ); - b3FloatW valid = b3AndW( b3GreaterThanW( kNormal, zeroW ), pointMask ); - cp->normalMasses = b3BlendW( zeroW, b3DivW( oneW, kNormal ), valid ); - } - - b3FloatW invWeight = - b3BlendW( zeroW, b3DivW( oneW, totalFrictionWeight ), b3GreaterThanW( totalFrictionWeight, zeroW ) ); - centerA = b3MulSVW( invWeight, centerA ); - centerB = b3MulSVW( invWeight, centerB ); - c->centerA = centerA; - c->centerB = centerB; - - for ( int pointIndex = 0; pointIndex < B3_MAX_MANIFOLD_POINTS; ++pointIndex ) - { - b3ContactConstraintPointWide* cp = c->points + pointIndex; - b3FloatW pointMask = b3GreaterThanW( pointCountW, b3SplatW( (float)pointIndex ) ); - b3Vec3W d = b3SubVW( cp->anchorAs, centerA ); - cp->leverArms = b3BlendW( zeroW, b3SqrtW( b3DotW( d, d ) ), pointMask ); - } - - { - b3Vec3W rtA1 = b3CrossW( centerA, tangent1 ); - b3Vec3W rtA2 = b3CrossW( centerA, tangent2 ); - b3Vec3W rtB1 = b3CrossW( centerB, tangent1 ); - b3Vec3W rtB2 = b3CrossW( centerB, tangent2 ); - b3Vec3W iArtA1 = b3MulMVW( iA, rtA1 ); - b3Vec3W iArtA2 = b3MulMVW( iA, rtA2 ); - b3Vec3W iBrtB1 = b3MulMVW( iB, rtB1 ); - b3Vec3W iBrtB2 = b3MulMVW( iB, rtB2 ); - - b3FloatW kxx = b3AddW( b3AddW( b3AddW( mA, mB ), b3DotW( rtA1, iArtA1 ) ), b3DotW( rtB1, iBrtB1 ) ); - b3FloatW kyy = b3AddW( b3AddW( b3AddW( mA, mB ), b3DotW( rtA2, iArtA2 ) ), b3DotW( rtB2, iBrtB2 ) ); - b3FloatW kxy = b3AddW( b3DotW( rtA1, iArtA2 ), b3DotW( rtB1, iBrtB2 ) ); - - b3FloatW det = b3SubW( b3MulW( kxx, kyy ), b3MulW( kxy, kxy ) ); - b3FloatW valid = b3GreaterThanW( b3AbsW( det ), minDet ); - b3FloatW invDet = b3BlendW( zeroW, b3DivW( oneW, det ), valid ); - c->tangentMass.cxx = b3MulW( invDet, kyy ); - c->tangentMass.cxy = b3NegW( b3MulW( invDet, kxy ) ); - c->tangentMass.cyy = b3MulW( invDet, kxx ); - } - - b3SymMatrix3W invIAB = b3AddSymW( iA, iB ); - - { - b3FloatW kTwist = b3DotW( normal, b3MulMVW( invIAB, normal ) ); - c->twistMass = b3BlendW( zeroW, b3DivW( oneW, kTwist ), b3GreaterThanW( kTwist, zeroW ) ); - } - - if ( b3AllZeroW( c->rollingResistance ) == false ) - { - c->rollingMass = b3InvertSymW( invIAB ); - } - else - { - c->rollingMass = (b3SymMatrix3W){ zeroW, zeroW, zeroW, zeroW, zeroW, zeroW }; - } - - // Only sample contact point normal velocity if needed. - bool haveRestitution = b3AnyTrueW( b3GreaterThanW( c->restitution, zeroW ) ); + bool haveRestitution = b3PrepareConstraintW( c, contacts, 1, context, warmStartScale ); anyRestitution = anyRestitution || haveRestitution; - - if ( hitEventLanes != 0 || haveRestitution ) - { - b3BodyStateW bA = b3GatherBodies( states, c->indexA ); - b3BodyStateW bB = b3GatherBodies( states, c->indexB ); - - for ( int pointIndex = 0; pointIndex < B3_MAX_MANIFOLD_POINTS; ++pointIndex ) - { - b3ContactConstraintPointWide* cp = c->points + pointIndex; - - b3Vec3W vrA = b3AddVW( bA.v, b3CrossW( bA.w, cp->anchorAs ) ); - b3Vec3W vrB = b3AddVW( bB.v, b3CrossW( bB.w, cp->anchorBs ) ); - b3FloatW vn = b3DotW( normal, b3SubVW( vrB, vrA ) ); - cp->relativeVelocities = vn; - - if ( hitEventLanes != 0 ) - { - float normalVelocities[B3_SIMD_WIDTH]; - b3StoreW( normalVelocities, vn ); - - for ( int lane = 0; lane < B3_SIMD_WIDTH; ++lane ) - { - if ( ( hitEventLanes & ( 1 << lane ) ) != 0 && pointIndex < c->pointCounts[lane] ) - { - c->manifolds[lane]->points[pointIndex].normalVelocity = normalVelocities[lane]; - } - } - } - } - } } // Advance to next color @@ -873,459 +950,20 @@ void B3_WIDE( b3PrepareContacts_Convex )( b3SolverBlock block, b3StepContext* co b3TracyCZoneEnd( prepare_contact ); } -void B3_WIDE( b3WarmStartContacts_Convex )( b3SolverBlock block, b3StepContext* context ) +void B3_WIDE( b3PrepareContacts_MeshWide )( b3SolverBlock block, b3StepContext* context ) { - b3TracyCZoneNC( warm_start_contact, "Warm Start", b3_colorGreen, true ); - - b3BodyState* states = context->states; - b3ContactConstraintWide* constraints = context->graph->colors[block.colorIndex].wideConstraints; - - for ( int i = block.startIndex; i < block.startIndex + block.count; ++i ) - { - b3ContactConstraintWide* c = constraints + i; - b3BodyStateW bA = b3GatherBodies( states, c->indexA ); - b3BodyStateW bB = b3GatherBodies( states, c->indexB ); - - int pointCount = 0; - for ( int lane = 0; lane < B3_SIMD_WIDTH; ++lane ) - { - pointCount = b3MaxInt( pointCount, c->pointCounts[lane] ); - } - B3_VALIDATE( 0 < pointCount && pointCount <= B3_MAX_MANIFOLD_POINTS ); - - b3FloatW zeroW = b3ZeroW(); - b3FloatW totalNormalImpulse = zeroW; - b3Vec3W momentA = { zeroW, zeroW, zeroW }; - b3Vec3W momentB = { zeroW, zeroW, zeroW }; - - for ( int j = 0; j < pointCount; ++j ) - { - b3ContactConstraintPointWide* cp = c->points + j; - b3FloatW normalImpulse = cp->normalImpulses; - totalNormalImpulse = b3AddW( totalNormalImpulse, normalImpulse ); - momentA = b3MulAddSVW( momentA, normalImpulse, cp->anchorAs ); - momentB = b3MulAddSVW( momentB, normalImpulse, cp->anchorBs ); - } - - b3Vec3W normal = c->normal; - b3Vec3W frictionImpulse = b3MulSVW( c->frictionImpulse.x, c->tangent1 ); - frictionImpulse = b3MulAddSVW( frictionImpulse, c->frictionImpulse.y, c->tangent2 ); - - b3Vec3W linearImpulse = b3MulAddSVW( frictionImpulse, totalNormalImpulse, normal ); - - b3Vec3W twistImpulse = b3MulSVW( c->twistImpulse, normal ); - b3Vec3W angularImpulseA = - b3AddVW( b3AddVW( b3CrossW( momentA, normal ), b3CrossW( c->centerA, frictionImpulse ) ), twistImpulse ); - b3Vec3W angularImpulseB = - b3AddVW( b3AddVW( b3CrossW( momentB, normal ), b3CrossW( c->centerB, frictionImpulse ) ), twistImpulse ); - - if ( b3AllZeroW( c->rollingResistance ) == false ) - { - angularImpulseA = b3AddVW( angularImpulseA, c->rollingImpulse ); - angularImpulseB = b3AddVW( angularImpulseB, c->rollingImpulse ); - } - - bA.w = b3MulSubMVW( bA.w, c->invIA, angularImpulseA ); - bA.v = b3MulSubSVW( bA.v, c->invMassA, linearImpulse ); - bB.w = b3MulAddMVW( bB.w, c->invIB, angularImpulseB ); - bB.v = b3MulAddSVW( bB.v, c->invMassB, linearImpulse ); - - b3ScatterBodies( states, c->indexA, &bA ); - b3ScatterBodies( states, c->indexB, &bB ); - } - - b3TracyCZoneEnd( warm_start_contact ); -} - -// Solve the non-penetration constraints with the soft bias. No friction and no restitution. -void B3_WIDE( b3PushContacts_Convex )( b3SolverBlock block, b3StepContext* context ) -{ - b3TracyCZoneNC( push_contact, "Push Contact", b3_colorAliceBlue, true ); - - b3BodyState* states = context->states; - b3ContactConstraintWide* constraints = context->graph->colors[block.colorIndex].wideConstraints; - b3FloatW inv_h = b3SplatW( context->inv_h ); - b3FloatW contactSpeed = b3SplatW( -context->world->contactSpeed ); - b3FloatW oneW = b3SplatW( 1.0f ); - - // Stiffer for static contacts to avoid bodies getting pushed through the ground. Selected per - // lane from the null body index instead of stored per constraint. - b3FloatW dynamicBiasRate = b3SplatW( context->contactSoftness.massScale * context->contactSoftness.biasRate ); - b3FloatW dynamicMassScale = b3SplatW( context->contactSoftness.massScale ); - b3FloatW dynamicImpulseScale = b3SplatW( context->contactSoftness.impulseScale ); - b3FloatW staticBiasRate = b3SplatW( context->staticSoftness.massScale * context->staticSoftness.biasRate ); - b3FloatW staticMassScale = b3SplatW( context->staticSoftness.massScale ); - b3FloatW staticImpulseScale = b3SplatW( context->staticSoftness.impulseScale ); - - for ( int wideIndex = block.startIndex; wideIndex < block.startIndex + block.count; ++wideIndex ) - { - b3ContactConstraintWide* c = constraints + wideIndex; - - int pointCount = 0; - for ( int lane = 0; lane < B3_SIMD_WIDTH; ++lane ) - { - pointCount = b3MaxInt( pointCount, c->pointCounts[lane] ); - } - B3_VALIDATE( 0 < pointCount && pointCount <= B3_MAX_MANIFOLD_POINTS ); - - b3BodyStateW bA = b3GatherBodies( states, c->indexA ); - b3BodyStateW bB = b3GatherBodies( states, c->indexB ); - - b3FloatW softMask = b3SoftMaskW( c->indexA, c->indexB ); - b3FloatW biasRate = b3BlendW( dynamicBiasRate, staticBiasRate, softMask ); - b3FloatW massScale = b3BlendW( dynamicMassScale, staticMassScale, softMask ); - b3FloatW impulseScale = b3BlendW( dynamicImpulseScale, staticImpulseScale, softMask ); - - b3Vec3W dp = b3SubVW( bB.dp, bA.dp ); - - // Convert to normals to local space to reduce transform math. - b3FloatW normalSeparation = b3DotW( c->normal, dp ); - b3Vec3W normalA = b3InvRotateVectorW( bA.dq, c->normal ); - b3Vec3W normalB = b3InvRotateVectorW( bB.dq, c->normal ); - - for ( int pointIndex = 0; pointIndex < pointCount; ++pointIndex ) - { - b3ContactConstraintPointWide* cp = c->points + pointIndex; - - // Fixed anchor points for applying impulses - b3Vec3W rA = cp->anchorAs; - b3Vec3W rB = cp->anchorBs; - - b3FloatW s = - b3AddW( b3AddW( normalSeparation, b3SubW( b3DotW( normalB, rB ), b3DotW( normalA, rA ) ) ), cp->baseSeparations ); - - // Apply speculative bias if separation is greater than zero, otherwise apply soft constraint bias - b3FloatW separated = b3GreaterThanW( s, b3ZeroW() ); - - // Speculative bias - positive - b3FloatW specBias = b3MulW( s, inv_h ); - - // Overlap bias - negative - b3FloatW overlapBias = b3MaxW( b3MulW( biasRate, s ), contactSpeed ); - b3FloatW velocityBias = b3BlendW( overlapBias, specBias, separated ); - - b3FloatW pointMassScale = b3BlendW( massScale, oneW, separated ); - b3FloatW pointImpulseScale = b3BlendW( impulseScale, b3ZeroW(), separated ); - - // Relative velocity at contact - b3Vec3W vrA = b3AddVW( bA.v, b3CrossW( bA.w, rA ) ); - b3Vec3W vrB = b3AddVW( bB.v, b3CrossW( bB.w, rB ) ); - b3FloatW vn = b3DotW( b3SubVW( vrB, vrA ), c->normal ); - - // Compute normal impulse - b3FloatW negImpulse = b3AddW( b3MulW( cp->normalMasses, b3AddW( b3MulW( pointMassScale, vn ), velocityBias ) ), - b3MulW( pointImpulseScale, cp->normalImpulses ) ); - - // Clamp the accumulated impulse - b3FloatW newImpulse = b3MaxW( b3SubW( cp->normalImpulses, negImpulse ), b3ZeroW() ); - b3FloatW deltaImpulse = b3SubW( newImpulse, cp->normalImpulses ); - cp->normalImpulses = newImpulse; - - // Apply contact impulse - b3Vec3W P = b3MulSVW( deltaImpulse, c->normal ); - bA.w = b3MulSubMVW( bA.w, c->invIA, b3CrossW( rA, P ) ); - bA.v = b3MulSubSVW( bA.v, c->invMassA, P ); - bB.w = b3MulAddMVW( bB.w, c->invIB, b3CrossW( rB, P ) ); - bB.v = b3MulAddSVW( bB.v, c->invMassB, P ); - } - - b3ScatterBodies( states, c->indexA, &bA ); - b3ScatterBodies( states, c->indexB, &bB ); - } - - b3TracyCZoneEnd( push_contact ); -} - -// Solve the normal constraint, friction, and rolling resistance. -void B3_WIDE( b3SolveContacts_Convex )( b3SolverBlock block, b3StepContext* context ) -{ - b3TracyCZoneNC( solve_contact, "Solve Contact", b3_colorAliceBlue, true ); - - b3BodyState* states = context->states; - b3ContactConstraintWide* constraints = context->graph->colors[block.colorIndex].wideConstraints; - b3FloatW inv_h = b3SplatW( context->inv_h ); - b3FloatW oneW = b3SplatW( 1.0f ); - b3FloatW epsilonW = b3SplatW( FLT_EPSILON ); - - for ( int wideIndex = block.startIndex; wideIndex < block.startIndex + block.count; ++wideIndex ) - { - b3ContactConstraintWide* c = constraints + wideIndex; - - int pointCount = 0; - for ( int lane = 0; lane < B3_SIMD_WIDTH; ++lane ) - { - pointCount = b3MaxInt( pointCount, c->pointCounts[lane] ); - } - B3_VALIDATE( 0 < pointCount && pointCount <= B3_MAX_MANIFOLD_POINTS ); - - b3BodyStateW bA = b3GatherBodies( states, c->indexA ); - b3BodyStateW bB = b3GatherBodies( states, c->indexB ); - - b3Vec3W dp = b3SubVW( bB.dp, bA.dp ); - b3FloatW normalSeparation = b3DotW( c->normal, dp ); - b3Vec3W normalA = b3InvRotateVectorW( bA.dq, c->normal ); - b3Vec3W normalB = b3InvRotateVectorW( bB.dq, c->normal ); - - b3FloatW totalNormalImpulse = b3ZeroW(); - b3FloatW totalTwistLimit = b3ZeroW(); - - for ( int pointIndex = 0; pointIndex < pointCount; ++pointIndex ) - { - b3ContactConstraintPointWide* cp = c->points + pointIndex; - - // Fixed anchor points for applying impulses - b3Vec3W rA = cp->anchorAs; - b3Vec3W rB = cp->anchorBs; - - b3FloatW s = - b3AddW( b3AddW( normalSeparation, b3SubW( b3DotW( normalB, rB ), b3DotW( normalA, rA ) ) ), cp->baseSeparations ); - - // Speculative bias, positive if separated and zero if overlapped - b3FloatW velocityBias = b3MaxW( b3ZeroW(), b3MulW( s, inv_h ) ); - - // Relative velocity at contact - b3Vec3W vrA = b3AddVW( bA.v, b3CrossW( bA.w, rA ) ); - b3Vec3W vrB = b3AddVW( bB.v, b3CrossW( bB.w, rB ) ); - b3FloatW vn = b3DotW( b3SubVW( vrB, vrA ), c->normal ); - - // Compute normal impulse - b3FloatW negImpulse = b3MulW( cp->normalMasses, b3AddW( vn, velocityBias ) ); - - // Clamp the accumulated impulse - b3FloatW newImpulse = b3MaxW( b3SubW( cp->normalImpulses, negImpulse ), b3ZeroW() ); - b3FloatW deltaImpulse = b3SubW( newImpulse, cp->normalImpulses ); - cp->normalImpulses = newImpulse; - cp->totalNormalImpulses = b3AddW( cp->totalNormalImpulses, newImpulse ); - totalNormalImpulse = b3AddW( totalNormalImpulse, newImpulse ); - totalTwistLimit = b3AddW( totalTwistLimit, b3MulW( cp->leverArms, newImpulse ) ); - - // Apply contact impulse - b3Vec3W P = b3MulSVW( deltaImpulse, c->normal ); - bA.w = b3MulSubMVW( bA.w, c->invIA, b3CrossW( rA, P ) ); - bA.v = b3MulSubSVW( bA.v, c->invMassA, P ); - bB.w = b3MulAddMVW( bB.w, c->invIB, b3CrossW( rB, P ) ); - bB.v = b3MulAddSVW( bB.v, c->invMassB, P ); - } - - // Rolling resistance - if ( b3AllZeroW( c->rollingResistance ) == false ) - { - // flip A/B order to negate - b3Vec3W deltaImpulse = b3MulMVW( c->rollingMass, b3SubVW( bA.w, bB.w ) ); - b3Vec3W oldImpulse = c->rollingImpulse; - c->rollingImpulse = b3AddVW( oldImpulse, deltaImpulse ); - - b3FloatW maxImpulse = b3MulW( c->rollingResistance, totalNormalImpulse ); - b3FloatW lengthSquared = b3DotW( c->rollingImpulse, c->rollingImpulse ); - - // if ( magSqr > maxLambda * maxLambda + FLT_EPSILON ) - //{ - // c->rollingImpulse *= maxLambda / sqrtf( magSqr ); - // } - - b3FloatW mask = b3GreaterThanW( lengthSquared, b3MulAddW( epsilonW, maxImpulse, maxImpulse ) ); - - // No approximate _mm_rsqrt_ps here to maintain cross-platform determinism - b3FloatW normalize = b3DivW( maxImpulse, b3AddW( b3SqrtW( lengthSquared ), epsilonW ) ); - b3FloatW scale = b3BlendW( oneW, normalize, mask ); - - // Ensure zero rolling resistance yields no impulse - b3FloatW rollingMask = b3GreaterThanW( c->rollingResistance, b3ZeroW() ); - - // Blend the result to ensure there are no +/-0 determinism problems, since 0 * -1 == -0. - b3Vec3W scaledImpulse = b3MulSVW( scale, c->rollingImpulse ); - c->rollingImpulse.X = b3BlendW( b3ZeroW(), scaledImpulse.X, rollingMask ); - c->rollingImpulse.Y = b3BlendW( b3ZeroW(), scaledImpulse.Y, rollingMask ); - c->rollingImpulse.Z = b3BlendW( b3ZeroW(), scaledImpulse.Z, rollingMask ); - - deltaImpulse = b3SubVW( c->rollingImpulse, oldImpulse ); - - bA.w = b3MulSubMVW( bA.w, c->invIA, deltaImpulse ); - bB.w = b3MulAddMVW( bB.w, c->invIB, deltaImpulse ); - } - - // Central twist friction - { - b3FloatW twistSpeed = b3DotW( c->normal, b3SubVW( bB.w, bA.w ) ); - b3FloatW maxLambda = b3MulW( c->friction, totalTwistLimit ); - b3FloatW deltaImpulse = b3NegW( b3MulW( c->twistMass, twistSpeed ) ); - b3FloatW oldImpulse = c->twistImpulse; - c->twistImpulse = b3SymClampW( b3AddW( oldImpulse, deltaImpulse ), maxLambda ); - deltaImpulse = b3SubW( c->twistImpulse, oldImpulse ); - - b3Vec3W L = b3MulSVW( deltaImpulse, c->normal ); - bA.w = b3MulSubMVW( bA.w, c->invIA, L ); - bB.w = b3MulAddMVW( bB.w, c->invIB, L ); - } - - // Central friction - { - b3Vec3W tangent1 = c->tangent1; - b3Vec3W tangent2 = c->tangent2; - - // Fixed anchor points for applying impulses - b3Vec3W rA = c->centerA; - b3Vec3W rB = c->centerB; - - // Relative tangent velocity at contact - b3Vec3W vrA = b3AddVW( bA.v, b3CrossW( bA.w, rA ) ); - b3Vec3W vrB = b3AddVW( bB.v, b3CrossW( bB.w, rB ) ); - b3Vec3W vr = b3SubVW( vrB, vrA ); - b3Vec2W vt = { - b3SubW( b3DotW( vr, tangent1 ), c->tangentVelocity1 ), - b3SubW( b3DotW( vr, tangent2 ), c->tangentVelocity2 ), - }; - - // Incremental tangent impulse - b3Vec2W deltaImpulse = b3MulMV2W( c->tangentMass, vt ); - deltaImpulse = (b3Vec2W){ b3NegW( deltaImpulse.x ), b3NegW( deltaImpulse.y ) }; - b3Vec2W newImpulse = b3AddV2W( c->frictionImpulse, deltaImpulse ); - - b3FloatW friction = c->friction; - b3FloatW maxImpulse = b3MulW( friction, totalNormalImpulse ); - - // Clamp the accumulated impulse - b3FloatW lengthSquared = b3AddW( b3MulW( newImpulse.x, newImpulse.x ), b3MulW( newImpulse.y, newImpulse.y ) ); - - // Max impulse can be zero - b3FloatW mask = b3GreaterThanW( lengthSquared, b3MulW( maxImpulse, maxImpulse ) ); - - // No approximate _mm_rsqrt_ps here to maintain cross-platform determinism. Add epsilon to avoid divide by - // zero. - b3FloatW normalize = b3DivW( maxImpulse, b3AddW( b3SqrtW( lengthSquared ), epsilonW ) ); - b3FloatW scale = b3BlendW( oneW, normalize, mask ); - newImpulse = (b3Vec2W){ - b3MulW( scale, newImpulse.x ), - b3MulW( scale, newImpulse.y ), - }; - - deltaImpulse = (b3Vec2W){ - b3SubW( newImpulse.x, c->frictionImpulse.x ), - b3SubW( newImpulse.y, c->frictionImpulse.y ), - }; - - c->frictionImpulse = newImpulse; - - // Apply delta impulse - b3Vec3W P = b3AddVW( b3MulSVW( deltaImpulse.x, tangent1 ), b3MulSVW( deltaImpulse.y, tangent2 ) ); - bA.w = b3MulSubMVW( bA.w, c->invIA, b3CrossW( rA, P ) ); - bA.v = b3MulSubSVW( bA.v, c->invMassA, P ); - bB.w = b3MulAddMVW( bB.w, c->invIB, b3CrossW( rB, P ) ); - bB.v = b3MulAddSVW( bB.v, c->invMassB, P ); - } - - b3ScatterBodies( states, c->indexA, &bA ); - b3ScatterBodies( states, c->indexB, &bB ); - } - - b3TracyCZoneEnd( solve_contact ); -} - -void B3_WIDE( b3ApplyRestitution_Convex )( b3SolverBlock block, b3StepContext* context ) -{ - b3TracyCZoneNC( restitution, "Restitution", b3_colorDodgerBlue, true ); - - b3BodyState* states = context->states; - b3ContactConstraintWide* constraints = context->graph->colors[block.colorIndex].wideConstraints; - b3FloatW inv_h = b3SplatW( context->inv_h ); - b3FloatW negRestitutionThreshold = b3SplatW( -context->world->restitutionThreshold ); - b3FloatW zeroW = b3ZeroW(); - bool propagate = context->world->enableRestitutionPropagation; - - for ( int wideIndex = block.startIndex; wideIndex < block.startIndex + block.count; ++wideIndex ) - { - b3ContactConstraintWide* c = constraints + wideIndex; - if ( propagate == false && b3AllZeroW( c->restitution ) ) - { - continue; - } - - int pointCount = 0; - for ( int lane = 0; lane < B3_SIMD_WIDTH; ++lane ) - { - pointCount = b3MaxInt( pointCount, c->pointCounts[lane] ); - } - B3_VALIDATE( 0 < pointCount && pointCount <= B3_MAX_MANIFOLD_POINTS ); - - b3BodyStateW bA = b3GatherBodies( states, c->indexA ); - b3BodyStateW bB = b3GatherBodies( states, c->indexB ); - - b3FloatW restitutionMask = b3GreaterThanW( c->restitution, zeroW ); - b3Vec3W dp = b3SubVW( bB.dp, bA.dp ); - b3Matrix3W dqA = b3MakeMatrixFromQuatW( bA.dq ); - b3Matrix3W dqB = b3MakeMatrixFromQuatW( bB.dq ); - - for ( int pointIndex = 0; pointIndex < pointCount; ++pointIndex ) - { - b3ContactConstraintPointWide* cp = c->points + pointIndex; - - b3Vec3W rA = cp->anchorAs; - b3Vec3W rB = cp->anchorBs; - - b3FloatW normalMass = propagate ? cp->normalMasses : b3BlendW( zeroW, cp->normalMasses, restitutionMask ); - - b3FloatW compressionImpulse = b3SubW( cp->totalNormalImpulses, cp->restitutionImpulses ); - b3FloatW armed = b3AndW( b3AndW( restitutionMask, b3LessThanW( cp->relativeVelocities, negRestitutionThreshold ) ), - b3GreaterThanW( compressionImpulse, zeroW ) ); - - b3Vec3W rsA = b3MulM3VW( dqA, rA ); - b3Vec3W rsB = b3MulM3VW( dqB, rB ); - b3Vec3W ds = b3AddVW( dp, b3SubVW( rsB, rsA ) ); - b3FloatW s = b3AddW( b3DotW( c->normal, ds ), cp->baseSeparations ); - - b3FloatW specBias = b3MaxW( zeroW, b3MulW( s, inv_h ) ); - b3FloatW velocityBias = b3BlendW( specBias, b3MulW( c->restitution, cp->relativeVelocities ), armed ); - - b3Vec3W vrA = b3AddVW( bA.v, b3CrossW( bA.w, rA ) ); - b3Vec3W vrB = b3AddVW( bB.v, b3CrossW( bB.w, rB ) ); - b3FloatW vn = b3DotW( b3SubVW( vrB, vrA ), c->normal ); - - b3FloatW negImpulse = b3MulW( normalMass, b3AddW( vn, velocityBias ) ); - - b3FloatW newImpulse = b3MaxW( b3SubW( cp->normalImpulses, negImpulse ), zeroW ); - b3FloatW impulse = b3SubW( newImpulse, cp->normalImpulses ); - - b3FloatW approachImpulse = b3MinW( b3MaxW( b3NegW( b3MulW( normalMass, vn ) ), zeroW ), b3MaxW( impulse, zeroW ) ); - b3FloatW allowance = - b3SubW( b3MulW( c->restitution, b3AddW( compressionImpulse, approachImpulse ) ), cp->restitutionImpulses ); - b3FloatW maxImpulse = b3AddW( approachImpulse, b3MaxW( allowance, zeroW ) ); - impulse = b3BlendW( impulse, b3MinW( impulse, maxImpulse ), armed ); - - cp->normalImpulses = b3AddW( cp->normalImpulses, impulse ); - cp->restitutionImpulses = b3AddW( cp->restitutionImpulses, b3SubW( impulse, approachImpulse ) ); - cp->totalNormalImpulses = b3AddW( cp->totalNormalImpulses, impulse ); - - b3Vec3W P = b3MulSVW( impulse, c->normal ); - bA.w = b3MulSubMVW( bA.w, c->invIA, b3CrossW( rA, P ) ); - bA.v = b3MulSubSVW( bA.v, c->invMassA, P ); - bB.w = b3MulAddMVW( bB.w, c->invIB, b3CrossW( rB, P ) ); - bB.v = b3MulAddSVW( bB.v, c->invMassB, P ); - } - - b3ScatterBodies( states, c->indexA, &bA ); - b3ScatterBodies( states, c->indexB, &bB ); - } - - b3TracyCZoneEnd( restitution ); -} - -// Store impulses by contact constraint -void B3_WIDE( b3StoreImpulses_Convex )( b3SolverBlock block, b3StepContext* context, int workerIndex ) -{ - b3TracyCZoneNC( store_impulses, "Store", b3_colorFireBrick, true ); + b3TracyCZoneNC( prepare_mesh_wide, "Prepare Mesh", b3_colorYellow, true ); b3World* world = context->world; - b3WidePrepareSpan* spans = context->widePrepareSpans; - const b3ContactConstraintWide* wideBase = context->wideConstraints; - b3TaskContext* taskContext = world->taskContexts.data + workerIndex; - b3BitSet* hitEventBitSet = &taskContext->hitEventBitSet; - bool hasHitEvents = taskContext->hasHitEvents; - float negHitThreshold = -world->hitEventThreshold; + b3MeshPrepareSpan* spans = context->meshPrepareSpans; + void* wideBase = context->wideMeshConstraints; + const int* manifoldStarts = context->wideMeshManifoldStarts; + b3FloatW warmStartScale = world->enableWarmStarting ? b3SplatW( 1.0f ) : b3ZeroW(); + bool anyRestitution = false; int wideIndex = block.startIndex; int endWideIndex = block.startIndex + block.count; - // Find color for start index int colorIndex = 0; while ( spans[colorIndex + 1].start <= wideIndex ) { @@ -1337,39 +975,545 @@ void B3_WIDE( b3StoreImpulses_Convex )( b3SolverBlock block, b3StepContext* cont int colorWideStart = spans[colorIndex].start; int colorWideEndIndex = b3MinInt( spans[colorIndex + 1].start, endWideIndex ); int colorContactCount = spans[colorIndex].count; - int* contactIds = spans[colorIndex].contacts; + b3ContactSpec* specs = spans[colorIndex].contacts; + + // This order is used to group contacts with similar manifold counts to keep the lanes full. + const int* order = spans[colorIndex].order; for ( ; wideIndex < colorWideEndIndex; ++wideIndex ) { - const b3ContactConstraintWide* c = wideBase + wideIndex; - const float* frictionImpulse1 = (float*)&c->frictionImpulse.x; - const float* frictionImpulse2 = (float*)&c->frictionImpulse.y; - const float* tangent1X = (float*)&c->tangent1.X; - const float* tangent1Y = (float*)&c->tangent1.Y; - const float* tangent1Z = (float*)&c->tangent1.Z; - const float* tangent2X = (float*)&c->tangent2.X; - const float* tangent2Y = (float*)&c->tangent2.Y; - const float* tangent2Z = (float*)&c->tangent2.Z; - const float* twistImpulse = (float*)&c->twistImpulse; - const float* rollingImpulseX = (float*)&c->rollingImpulse.X; - const float* rollingImpulseY = (float*)&c->rollingImpulse.Y; - const float* rollingImpulseZ = (float*)&c->rollingImpulse.Z; - + b3ContactConstraintWide* c = b3GetConstraintW( wideBase, manifoldStarts, wideIndex, 0 ); int localWideIndex = wideIndex - colorWideStart; + b3Contact* contacts[B3_SIMD_WIDTH]; for ( int lane = 0; lane < B3_SIMD_WIDTH; ++lane ) { int contactIndex = B3_SIMD_WIDTH * localWideIndex + lane; - if ( contactIndex >= colorContactCount ) + if ( contactIndex < colorContactCount ) { - break; + b3ContactSpec* spec = specs + order[contactIndex]; + b3Contact* contact = b3Array_Get( world->contacts, spec->contactId ); + B3_ASSERT( contact->contactId == spec->contactId ); + B3_ASSERT( contact->manifoldCount == spec->manifoldCount ); + contacts[lane] = contact; } + else + { + contacts[lane] = NULL; + } + } - b3Manifold* m = c->manifolds[lane]; - if ( m == NULL ) - { - continue; - } + int manifoldCount = manifoldStarts[wideIndex + 1] - manifoldStarts[wideIndex]; + bool haveRestitution = b3PrepareConstraintW( c, contacts, manifoldCount, context, warmStartScale ); + anyRestitution = anyRestitution || haveRestitution; + } + + colorIndex += 1; + } + + if ( anyRestitution ) + { + b3AtomicStoreInt( &context->anyRestitution, 1 ); + } + + b3TracyCZoneEnd( prepare_mesh_wide ); +} + +B3_FORCE_INLINE void b3WarmStartContactsW( b3SolverBlock block, b3StepContext* context, void* base, const int* manifoldStarts, + int fixedManifoldCount ) +{ + b3BodyState* states = context->states; + b3FloatW zeroW = b3ZeroW(); + + b3ContactConstraintWide* c = b3GetConstraintW( base, manifoldStarts, block.startIndex, fixedManifoldCount ); + for ( int i = 0; i < block.count; ++i ) + { + int manifoldCount = fixedManifoldCount > 0 ? fixedManifoldCount : c->manifoldCount; + bool anyRolling = b3AllZeroW( c->rollingResistance ) == false; + + b3Vec3W linearImpulse = { zeroW, zeroW, zeroW }; + b3Vec3W angularImpulseA = { zeroW, zeroW, zeroW }; + b3Vec3W angularImpulseB = { zeroW, zeroW, zeroW }; + + const b3ContactManifoldWide* manifolds = b3GetManifoldsW( c ); + for ( int manifoldIndex = 0; manifoldIndex < manifoldCount; ++manifoldIndex ) + { + const b3ContactManifoldWide* cm = manifolds + manifoldIndex; + + b3FloatW totalNormalImpulse = zeroW; + b3Vec3W momentA = { zeroW, zeroW, zeroW }; + b3Vec3W momentB = { zeroW, zeroW, zeroW }; + + for ( int pointIndex = 0; pointIndex < cm->pointCount; ++pointIndex ) + { + const b3ContactConstraintPointWide* cp = cm->points + pointIndex; + b3FloatW normalImpulse = cp->normalImpulses; + totalNormalImpulse = b3AddW( totalNormalImpulse, normalImpulse ); + momentA = b3MulAddSVW( momentA, normalImpulse, cp->anchorAs ); + momentB = b3MulAddSVW( momentB, normalImpulse, cp->anchorBs ); + } + + b3Vec3W normal = cm->normal; + b3Vec3W frictionImpulse = b3MulSVW( cm->frictionImpulse.x, cm->tangent1 ); + frictionImpulse = b3MulAddSVW( frictionImpulse, cm->frictionImpulse.y, cm->tangent2 ); + + b3Vec3W linear = b3MulAddSVW( frictionImpulse, totalNormalImpulse, normal ); + + b3Vec3W twistImpulse = b3MulSVW( cm->twistImpulse, normal ); + b3Vec3W angularA = + b3AddVW( b3AddVW( b3CrossW( momentA, normal ), b3CrossW( cm->centerA, frictionImpulse ) ), twistImpulse ); + b3Vec3W angularB = + b3AddVW( b3AddVW( b3CrossW( momentB, normal ), b3CrossW( cm->centerB, frictionImpulse ) ), twistImpulse ); + + if ( anyRolling ) + { + angularA = b3AddVW( angularA, cm->rollingImpulse ); + angularB = b3AddVW( angularB, cm->rollingImpulse ); + } + + linearImpulse = b3AddVW( linearImpulse, linear ); + angularImpulseA = b3AddVW( angularImpulseA, angularA ); + angularImpulseB = b3AddVW( angularImpulseB, angularB ); + } + + b3BodyStateW bA = b3GatherBodies( states, c->indexA ); + b3BodyStateW bB = b3GatherBodies( states, c->indexB ); + + bA.w = b3MulSubMVW( bA.w, c->invIA, angularImpulseA ); + bA.v = b3MulSubSVW( bA.v, c->invMassA, linearImpulse ); + bB.w = b3MulAddMVW( bB.w, c->invIB, angularImpulseB ); + bB.v = b3MulAddSVW( bB.v, c->invMassB, linearImpulse ); + + b3ScatterBodies( states, c->indexA, &bA ); + b3ScatterBodies( states, c->indexB, &bB ); + + c = b3NextConstraintW( c, manifoldCount ); + } +} + +// Solve the non-penetration constraints with the soft bias. No friction and no restitution. +B3_FORCE_INLINE void b3PushContactsW( b3SolverBlock block, b3StepContext* context, void* base, const int* manifoldStarts, + int fixedManifoldCount ) +{ + b3BodyState* states = context->states; + b3FloatW inv_h = b3SplatW( context->inv_h ); + b3FloatW contactSpeed = b3SplatW( -context->world->contactSpeed ); + b3FloatW oneW = b3SplatW( 1.0f ); + b3FloatW zeroW = b3ZeroW(); + + // Stiffer for static contacts to avoid bodies getting pushed through the ground. Selected per + // lane from the null body index instead of stored per constraint. + b3FloatW dynamicBiasRate = b3SplatW( context->contactSoftness.massScale * context->contactSoftness.biasRate ); + b3FloatW dynamicMassScale = b3SplatW( context->contactSoftness.massScale ); + b3FloatW dynamicImpulseScale = b3SplatW( context->contactSoftness.impulseScale ); + b3FloatW staticBiasRate = b3SplatW( context->staticSoftness.massScale * context->staticSoftness.biasRate ); + b3FloatW staticMassScale = b3SplatW( context->staticSoftness.massScale ); + b3FloatW staticImpulseScale = b3SplatW( context->staticSoftness.impulseScale ); + + b3ContactConstraintWide* c = b3GetConstraintW( base, manifoldStarts, block.startIndex, fixedManifoldCount ); + for ( int i = 0; i < block.count; ++i ) + { + int manifoldCount = fixedManifoldCount > 0 ? fixedManifoldCount : c->manifoldCount; + b3BodyStateW bA = b3GatherBodies( states, c->indexA ); + b3BodyStateW bB = b3GatherBodies( states, c->indexB ); + + b3FloatW softMask = b3SoftMaskW( c->indexA, c->indexB ); + b3FloatW biasRate = b3BlendW( dynamicBiasRate, staticBiasRate, softMask ); + b3FloatW massScale = b3BlendW( dynamicMassScale, staticMassScale, softMask ); + b3FloatW impulseScale = b3BlendW( dynamicImpulseScale, staticImpulseScale, softMask ); + + b3Vec3W dp = b3SubVW( bB.dp, bA.dp ); + + b3ContactManifoldWide* manifolds = b3GetManifoldsW( c ); + for ( int manifoldIndex = 0; manifoldIndex < manifoldCount; ++manifoldIndex ) + { + b3ContactManifoldWide* cm = manifolds + manifoldIndex; + + // Convert to normals to local space to reduce transform math. + // Read the normal rather than put on stack to avoid register spills. + b3FloatW normalSeparation = b3DotW( cm->normal, dp ); + b3Vec3W normalA = b3InvRotateVectorW( bA.dq, cm->normal ); + b3Vec3W normalB = b3InvRotateVectorW( bB.dq, cm->normal ); + + for ( int pointIndex = 0; pointIndex < cm->pointCount; ++pointIndex ) + { + b3ContactConstraintPointWide* cp = cm->points + pointIndex; + + // Fixed anchor points for applying impulses + b3Vec3W rA = cp->anchorAs; + b3Vec3W rB = cp->anchorBs; + + b3FloatW s = b3AddW( b3AddW( normalSeparation, b3SubW( b3DotW( normalB, rB ), b3DotW( normalA, rA ) ) ), + cp->baseSeparations ); + + // Apply speculative bias if separation is greater than zero, otherwise apply soft constraint bias + b3FloatW separated = b3GreaterThanW( s, zeroW ); + + // Speculative bias - positive + b3FloatW specBias = b3MulW( s, inv_h ); + + // Overlap bias - negative + b3FloatW overlapBias = b3MaxW( b3MulW( biasRate, s ), contactSpeed ); + b3FloatW velocityBias = b3BlendW( overlapBias, specBias, separated ); + + b3FloatW pointMassScale = b3BlendW( massScale, oneW, separated ); + b3FloatW pointImpulseScale = b3BlendW( impulseScale, zeroW, separated ); + + // Relative velocity at contact + b3Vec3W vrA = b3AddVW( bA.v, b3CrossW( bA.w, rA ) ); + b3Vec3W vrB = b3AddVW( bB.v, b3CrossW( bB.w, rB ) ); + b3FloatW vn = b3DotW( b3SubVW( vrB, vrA ), cm->normal ); + + // Compute normal impulse + b3FloatW negImpulse = b3AddW( b3MulW( cp->normalMasses, b3AddW( b3MulW( pointMassScale, vn ), velocityBias ) ), + b3MulW( pointImpulseScale, cp->normalImpulses ) ); + + // Clamp the accumulated impulse + b3FloatW newImpulse = b3MaxW( b3SubW( cp->normalImpulses, negImpulse ), zeroW ); + b3FloatW deltaImpulse = b3SubW( newImpulse, cp->normalImpulses ); + cp->normalImpulses = newImpulse; + + // Apply contact impulse + b3Vec3W P = b3MulSVW( deltaImpulse, cm->normal ); + bA.w = b3MulSubMVW( bA.w, c->invIA, b3CrossW( rA, P ) ); + bA.v = b3MulSubSVW( bA.v, c->invMassA, P ); + bB.w = b3MulAddMVW( bB.w, c->invIB, b3CrossW( rB, P ) ); + bB.v = b3MulAddSVW( bB.v, c->invMassB, P ); + } + } + + b3ScatterBodies( states, c->indexA, &bA ); + b3ScatterBodies( states, c->indexB, &bB ); + + c = b3NextConstraintW( c, manifoldCount ); + } +} + +// Solve the normal constraint, friction, and rolling resistance. +B3_FORCE_INLINE void b3SolveContactsW( b3SolverBlock block, b3StepContext* context, void* base, const int* manifoldStarts, + int fixedManifoldCount ) +{ + b3BodyState* states = context->states; + b3FloatW inv_h = b3SplatW( context->inv_h ); + b3FloatW oneW = b3SplatW( 1.0f ); + b3FloatW zeroW = b3ZeroW(); + b3FloatW epsilonW = b3SplatW( FLT_EPSILON ); + + b3ContactConstraintWide* c = b3GetConstraintW( base, manifoldStarts, block.startIndex, fixedManifoldCount ); + for ( int i = 0; i < block.count; ++i ) + { + int manifoldCount = fixedManifoldCount > 0 ? fixedManifoldCount : c->manifoldCount; + b3BodyStateW bA = b3GatherBodies( states, c->indexA ); + b3BodyStateW bB = b3GatherBodies( states, c->indexB ); + + bool anyRolling = b3AllZeroW( c->rollingResistance ) == false; + b3Vec3W dp = b3SubVW( bB.dp, bA.dp ); + + b3ContactManifoldWide* manifolds = b3GetManifoldsW( c ); + for ( int manifoldIndex = 0; manifoldIndex < manifoldCount; ++manifoldIndex ) + { + b3ContactManifoldWide* cm = manifolds + manifoldIndex; + + // Read the normal rather than put on stack to avoid register spills. + b3FloatW normalSeparation = b3DotW( cm->normal, dp ); + b3Vec3W normalA = b3InvRotateVectorW( bA.dq, cm->normal ); + b3Vec3W normalB = b3InvRotateVectorW( bB.dq, cm->normal ); + + b3FloatW totalNormalImpulse = zeroW; + b3FloatW totalTwistLimit = zeroW; + + // Normal contraints + for ( int pointIndex = 0; pointIndex < cm->pointCount; ++pointIndex ) + { + b3ContactConstraintPointWide* cp = cm->points + pointIndex; + + // Fixed anchor points for applying impulses + b3Vec3W rA = cp->anchorAs; + b3Vec3W rB = cp->anchorBs; + + b3FloatW s = b3AddW( b3AddW( normalSeparation, b3SubW( b3DotW( normalB, rB ), b3DotW( normalA, rA ) ) ), + cp->baseSeparations ); + + // Speculative bias, positive if separated and zero if overlapped + b3FloatW velocityBias = b3MaxW( zeroW, b3MulW( s, inv_h ) ); + + // Relative velocity at contact + b3Vec3W vrA = b3AddVW( bA.v, b3CrossW( bA.w, rA ) ); + b3Vec3W vrB = b3AddVW( bB.v, b3CrossW( bB.w, rB ) ); + b3FloatW vn = b3DotW( b3SubVW( vrB, vrA ), cm->normal ); + + // Compute normal impulse + b3FloatW negImpulse = b3MulW( cp->normalMasses, b3AddW( vn, velocityBias ) ); + + // Clamp the accumulated impulse + b3FloatW newImpulse = b3MaxW( b3SubW( cp->normalImpulses, negImpulse ), zeroW ); + b3FloatW deltaImpulse = b3SubW( newImpulse, cp->normalImpulses ); + cp->normalImpulses = newImpulse; + cp->totalNormalImpulses = b3AddW( cp->totalNormalImpulses, newImpulse ); + totalNormalImpulse = b3AddW( totalNormalImpulse, newImpulse ); + totalTwistLimit = b3AddW( totalTwistLimit, b3MulW( cp->leverArms, newImpulse ) ); + + // Apply contact impulse + b3Vec3W P = b3MulSVW( deltaImpulse, cm->normal ); + bA.w = b3MulSubMVW( bA.w, c->invIA, b3CrossW( rA, P ) ); + bA.v = b3MulSubSVW( bA.v, c->invMassA, P ); + bB.w = b3MulAddMVW( bB.w, c->invIB, b3CrossW( rB, P ) ); + bB.v = b3MulAddSVW( bB.v, c->invMassB, P ); + } + + // Rolling resistance. After normal constraints because it uses the normal impulse. + if ( anyRolling ) + { + // flip A/B order to negate + b3Vec3W deltaImpulse = b3MulMVW( c->rollingMass, b3SubVW( bA.w, bB.w ) ); + b3Vec3W oldImpulse = cm->rollingImpulse; + cm->rollingImpulse = b3AddVW( oldImpulse, deltaImpulse ); + + b3FloatW maxImpulse = b3MulW( c->rollingResistance, totalNormalImpulse ); + b3FloatW lengthSquared = b3DotW( cm->rollingImpulse, cm->rollingImpulse ); + + // if ( magSqr > maxLambda * maxLambda + FLT_EPSILON ) + //{ + // c->rollingImpulse *= maxLambda / sqrtf( magSqr ); + // } + + b3FloatW mask = b3GreaterThanW( lengthSquared, b3MulAddW( epsilonW, maxImpulse, maxImpulse ) ); + + // No approximate _mm_rsqrt_ps here to maintain cross-platform determinism + b3FloatW normalize = b3DivW( maxImpulse, b3AddW( b3SqrtW( lengthSquared ), epsilonW ) ); + b3FloatW scale = b3BlendW( oneW, normalize, mask ); + + // Blend the result to ensure there are no +/-0 determinism problems, since 0 * -1 == -0. + b3Vec3W scaledImpulse = b3MulSVW( scale, cm->rollingImpulse ); + cm->rollingImpulse.X = b3BlendW( zeroW, scaledImpulse.X, cm->rollingMask ); + cm->rollingImpulse.Y = b3BlendW( zeroW, scaledImpulse.Y, cm->rollingMask ); + cm->rollingImpulse.Z = b3BlendW( zeroW, scaledImpulse.Z, cm->rollingMask ); + + deltaImpulse = b3SubVW( cm->rollingImpulse, oldImpulse ); + + bA.w = b3MulSubMVW( bA.w, c->invIA, deltaImpulse ); + bB.w = b3MulAddMVW( bB.w, c->invIB, deltaImpulse ); + } + + // Central twist friction. Friction goes last to improve stacking stability. + { + b3FloatW twistSpeed = b3DotW( cm->normal, b3SubVW( bB.w, bA.w ) ); + b3FloatW maxLambda = b3MulW( c->friction, totalTwistLimit ); + b3FloatW deltaImpulse = b3NegW( b3MulW( cm->twistMass, twistSpeed ) ); + b3FloatW oldImpulse = cm->twistImpulse; + cm->twistImpulse = b3SymClampW( b3AddW( oldImpulse, deltaImpulse ), maxLambda ); + deltaImpulse = b3SubW( cm->twistImpulse, oldImpulse ); + + b3Vec3W L = b3MulSVW( deltaImpulse, cm->normal ); + bA.w = b3MulSubMVW( bA.w, c->invIA, L ); + bB.w = b3MulAddMVW( bB.w, c->invIB, L ); + } + + // Central friction. Friction goes last to improve stacking stability. + { + b3Vec3W tangent1 = cm->tangent1; + b3Vec3W tangent2 = cm->tangent2; + + // Fixed anchor points for applying impulses + b3Vec3W rA = cm->centerA; + b3Vec3W rB = cm->centerB; + + // Relative tangent velocity at contact + b3Vec3W vrA = b3AddVW( bA.v, b3CrossW( bA.w, rA ) ); + b3Vec3W vrB = b3AddVW( bB.v, b3CrossW( bB.w, rB ) ); + b3Vec3W vr = b3SubVW( vrB, vrA ); + b3Vec2W vt = { + b3SubW( b3DotW( vr, tangent1 ), cm->tangentVelocity1 ), + b3SubW( b3DotW( vr, tangent2 ), cm->tangentVelocity2 ), + }; + + // Incremental tangent impulse + b3Vec2W deltaImpulse = b3MulMV2W( cm->tangentMass, vt ); + deltaImpulse = (b3Vec2W){ b3NegW( deltaImpulse.x ), b3NegW( deltaImpulse.y ) }; + b3Vec2W newImpulse = b3AddV2W( cm->frictionImpulse, deltaImpulse ); + + b3FloatW maxImpulse = b3MulW( c->friction, totalNormalImpulse ); + + // Clamp the accumulated impulse + b3FloatW lengthSquared = b3AddW( b3MulW( newImpulse.x, newImpulse.x ), b3MulW( newImpulse.y, newImpulse.y ) ); + + // Max impulse can be zero + b3FloatW mask = b3GreaterThanW( lengthSquared, b3MulW( maxImpulse, maxImpulse ) ); + + // No approximate _mm_rsqrt_ps here to maintain cross-platform determinism. Add epsilon to avoid divide by + // zero. + b3FloatW normalize = b3DivW( maxImpulse, b3AddW( b3SqrtW( lengthSquared ), epsilonW ) ); + b3FloatW scale = b3BlendW( oneW, normalize, mask ); + newImpulse = (b3Vec2W){ + b3MulW( scale, newImpulse.x ), + b3MulW( scale, newImpulse.y ), + }; + + deltaImpulse = (b3Vec2W){ + b3SubW( newImpulse.x, cm->frictionImpulse.x ), + b3SubW( newImpulse.y, cm->frictionImpulse.y ), + }; + + cm->frictionImpulse = newImpulse; + + // Apply delta impulse + b3Vec3W P = b3AddVW( b3MulSVW( deltaImpulse.x, tangent1 ), b3MulSVW( deltaImpulse.y, tangent2 ) ); + bA.w = b3MulSubMVW( bA.w, c->invIA, b3CrossW( rA, P ) ); + bA.v = b3MulSubSVW( bA.v, c->invMassA, P ); + bB.w = b3MulAddMVW( bB.w, c->invIB, b3CrossW( rB, P ) ); + bB.v = b3MulAddSVW( bB.v, c->invMassB, P ); + } + } + + b3ScatterBodies( states, c->indexA, &bA ); + b3ScatterBodies( states, c->indexB, &bB ); + + c = b3NextConstraintW( c, manifoldCount ); + } +} + +B3_FORCE_INLINE void b3ApplyRestitutionW( b3SolverBlock block, b3StepContext* context, void* base, const int* manifoldStarts, + int fixedManifoldCount ) +{ + b3BodyState* states = context->states; + b3FloatW inv_h = b3SplatW( context->inv_h ); + b3FloatW negRestitutionThreshold = b3SplatW( -context->world->restitutionThreshold ); + b3FloatW zeroW = b3ZeroW(); + bool propagate = context->world->enableRestitutionPropagation; + + b3ContactConstraintWide* next = b3GetConstraintW( base, manifoldStarts, block.startIndex, fixedManifoldCount ); + for ( int i = 0; i < block.count; ++i ) + { + b3ContactConstraintWide* c = next; + int manifoldCount = fixedManifoldCount > 0 ? fixedManifoldCount : c->manifoldCount; + next = b3NextConstraintW( c, manifoldCount ); + + if ( propagate == false && b3AllZeroW( c->restitution ) ) + { + continue; + } + + b3BodyStateW bA = b3GatherBodies( states, c->indexA ); + b3BodyStateW bB = b3GatherBodies( states, c->indexB ); + + b3FloatW restitutionMask = b3GreaterThanW( c->restitution, zeroW ); + b3Vec3W dp = b3SubVW( bB.dp, bA.dp ); + + b3ContactManifoldWide* manifolds = b3GetManifoldsW( c ); + for ( int manifoldIndex = 0; manifoldIndex < manifoldCount; ++manifoldIndex ) + { + b3ContactManifoldWide* cm = manifolds + manifoldIndex; + + b3FloatW normalSeparation = b3DotW( cm->normal, dp ); + b3Vec3W normalA = b3InvRotateVectorW( bA.dq, cm->normal ); + b3Vec3W normalB = b3InvRotateVectorW( bB.dq, cm->normal ); + + // Read the point count rather than put on stack to avoid register spills. + for ( int pointIndex = 0; pointIndex < cm->pointCount; ++pointIndex ) + { + b3ContactConstraintPointWide* cp = cm->points + pointIndex; + + b3Vec3W rA = cp->anchorAs; + b3Vec3W rB = cp->anchorBs; + + b3FloatW s = b3AddW( b3AddW( normalSeparation, b3SubW( b3DotW( normalB, rB ), b3DotW( normalA, rA ) ) ), + cp->baseSeparations ); + + b3FloatW normalMass = propagate ? cp->normalMasses : b3BlendW( zeroW, cp->normalMasses, restitutionMask ); + + b3FloatW compressionImpulse = b3SubW( cp->totalNormalImpulses, cp->restitutionImpulses ); + b3FloatW armed = + b3AndW( b3AndW( restitutionMask, b3LessThanW( cp->relativeVelocities, negRestitutionThreshold ) ), + b3GreaterThanW( compressionImpulse, zeroW ) ); + + b3FloatW specBias = b3MaxW( zeroW, b3MulW( s, inv_h ) ); + b3FloatW velocityBias = b3BlendW( specBias, b3MulW( c->restitution, cp->relativeVelocities ), armed ); + + b3Vec3W vrA = b3AddVW( bA.v, b3CrossW( bA.w, rA ) ); + b3Vec3W vrB = b3AddVW( bB.v, b3CrossW( bB.w, rB ) ); + b3FloatW vn = b3DotW( b3SubVW( vrB, vrA ), cm->normal ); + + b3FloatW negImpulse = b3MulW( normalMass, b3AddW( vn, velocityBias ) ); + + b3FloatW newImpulse = b3MaxW( b3SubW( cp->normalImpulses, negImpulse ), zeroW ); + b3FloatW impulse = b3SubW( newImpulse, cp->normalImpulses ); + + b3FloatW approachImpulse = + b3MinW( b3MaxW( b3NegW( b3MulW( normalMass, vn ) ), zeroW ), b3MaxW( impulse, zeroW ) ); + b3FloatW allowance = + b3SubW( b3MulW( c->restitution, b3AddW( compressionImpulse, approachImpulse ) ), cp->restitutionImpulses ); + b3FloatW maxImpulse = b3AddW( approachImpulse, b3MaxW( allowance, zeroW ) ); + impulse = b3BlendW( impulse, b3MinW( impulse, maxImpulse ), armed ); + + cp->normalImpulses = b3AddW( cp->normalImpulses, impulse ); + cp->restitutionImpulses = b3AddW( cp->restitutionImpulses, b3SubW( impulse, approachImpulse ) ); + cp->totalNormalImpulses = b3AddW( cp->totalNormalImpulses, impulse ); + + b3Vec3W P = b3MulSVW( impulse, cm->normal ); + bA.w = b3MulSubMVW( bA.w, c->invIA, b3CrossW( rA, P ) ); + bA.v = b3MulSubSVW( bA.v, c->invMassA, P ); + bB.w = b3MulAddMVW( bB.w, c->invIB, b3CrossW( rB, P ) ); + bB.v = b3MulAddSVW( bB.v, c->invMassB, P ); + } + } + + b3ScatterBodies( states, c->indexA, &bA ); + b3ScatterBodies( states, c->indexB, &bB ); + } +} + +// Store impulses by contact constraint +B3_FORCE_INLINE void b3StoreImpulsesW( b3SolverBlock block, b3StepContext* context, int workerIndex, void* base, + const int* manifoldStarts, int fixedManifoldCount ) +{ + b3World* world = context->world; + b3TaskContext* taskContext = world->taskContexts.data + workerIndex; + b3BitSet* hitEventBitSet = &taskContext->hitEventBitSet; + bool hasHitEvents = taskContext->hasHitEvents; + float negHitThreshold = -world->hitEventThreshold; + + b3ContactConstraintWide* c = b3GetConstraintW( base, manifoldStarts, block.startIndex, fixedManifoldCount ); + for ( int i = 0; i < block.count; ++i ) + { + int manifoldCount = fixedManifoldCount > 0 ? fixedManifoldCount : c->manifoldCount; + const b3ContactManifoldWide* manifolds = b3GetManifoldsW( c ); + + // Pay the cache misses up front. + b3Manifold* laneManifolds[B3_SIMD_WIDTH]; + for ( int lane = 0; lane < B3_SIMD_WIDTH; ++lane ) + { + b3Contact* contact = c->contacts[lane]; + laneManifolds[lane] = contact != NULL ? contact->manifolds : NULL; + } + + for ( int lane = 0; lane < B3_SIMD_WIDTH; ++lane ) + { + b3Contact* contact = c->contacts[lane]; + if ( contact == NULL ) + { + continue; + } + + int laneManifoldCount = fixedManifoldCount > 0 ? fixedManifoldCount : contact->manifoldCount; + B3_ASSERT( laneManifoldCount == contact->manifoldCount && laneManifoldCount <= manifoldCount ); + + for ( int manifoldIndex = 0; manifoldIndex < laneManifoldCount; ++manifoldIndex ) + { + const b3ContactManifoldWide* cm = manifolds + manifoldIndex; + b3Manifold* m = laneManifolds[lane] + manifoldIndex; + + const float* frictionImpulse1 = (const float*)&cm->frictionImpulse.x; + const float* frictionImpulse2 = (const float*)&cm->frictionImpulse.y; + const float* tangent1X = (const float*)&cm->tangent1.X; + const float* tangent1Y = (const float*)&cm->tangent1.Y; + const float* tangent1Z = (const float*)&cm->tangent1.Z; + const float* tangent2X = (const float*)&cm->tangent2.X; + const float* tangent2Y = (const float*)&cm->tangent2.Y; + const float* tangent2Z = (const float*)&cm->tangent2.Z; + const float* twistImpulse = (const float*)&cm->twistImpulse; + const float* rollingImpulseX = (const float*)&cm->rollingImpulse.X; + const float* rollingImpulseY = (const float*)&cm->rollingImpulse.Y; + const float* rollingImpulseZ = (const float*)&cm->rollingImpulse.Z; float f1 = frictionImpulse1[lane]; float f2 = frictionImpulse2[lane]; @@ -1386,30 +1530,35 @@ void B3_WIDE( b3StoreImpulses_Convex )( b3SolverBlock block, b3StepContext* cont }; int pointCount = m->pointCount; + B3_ASSERT( pointCount <= cm->pointCount ); for ( int pointIndex = 0; pointIndex < pointCount; ++pointIndex ) { - const b3ContactConstraintPointWide* cp = c->points + pointIndex; - const float* normalImpulse = (float*)&cp->normalImpulses; - const float* totalNormalImpulse = (float*)&cp->totalNormalImpulses; + const b3ContactConstraintPointWide* cp = cm->points + pointIndex; + const float* normalImpulse = (const float*)&cp->normalImpulses; + const float* totalNormalImpulse = (const float*)&cp->totalNormalImpulses; b3ManifoldPoint* mp = m->points + pointIndex; mp->normalImpulse = normalImpulse[lane]; mp->totalNormalImpulse = totalNormalImpulse[lane]; } + } - int contactId = contactIds[contactIndex]; - b3Contact* contact = b3Array_Get( world->contacts, contactId ); - if ( ( contact->flags & b3_simEnableHitEvent ) != 0 ) + if ( ( contact->flags & b3_simEnableHitEvent ) != 0 ) + { + bool flagged = false; + for ( int manifoldIndex = 0; manifoldIndex < laneManifoldCount && flagged == false; ++manifoldIndex ) { - for ( int k = 0; k < pointCount; ++k ) + const b3Manifold* m = contact->manifolds + manifoldIndex; + for ( int pointIndex = 0; pointIndex < m->pointCount; ++pointIndex ) { - b3ManifoldPoint* mp = m->points + k; + const b3ManifoldPoint* mp = m->points + pointIndex; // Need to check total impulse because the point may be speculative and not colliding if ( mp->normalVelocity < negHitThreshold && mp->totalNormalImpulse > 0.0f ) { b3SetBit( hitEventBitSet, contact->contactId ); hasHitEvents = true; + flagged = true; break; } } @@ -1417,10 +1566,84 @@ void B3_WIDE( b3StoreImpulses_Convex )( b3SolverBlock block, b3StepContext* cont } } - colorIndex += 1; + c = b3NextConstraintW( c, manifoldCount ); } taskContext->hasHitEvents = hasHitEvents; +} +// These wrappers allow the implementation be shared between convex and mesh contacts. + +void B3_WIDE( b3WarmStartContacts_Convex )( b3SolverBlock block, b3StepContext* context ) +{ + b3TracyCZoneNC( warm_start_contact, "Warm Start", b3_colorGreen, true ); + b3WarmStartContactsW( block, context, context->graph->colors[block.colorIndex].wideConstraints, NULL, 1 ); + b3TracyCZoneEnd( warm_start_contact ); +} + +void B3_WIDE( b3PushContacts_Convex )( b3SolverBlock block, b3StepContext* context ) +{ + b3TracyCZoneNC( push_contact, "Push Contact", b3_colorAliceBlue, true ); + b3PushContactsW( block, context, context->graph->colors[block.colorIndex].wideConstraints, NULL, 1 ); + b3TracyCZoneEnd( push_contact ); +} + +void B3_WIDE( b3SolveContacts_Convex )( b3SolverBlock block, b3StepContext* context ) +{ + b3TracyCZoneNC( solve_contact, "Solve Contact", b3_colorAliceBlue, true ); + b3SolveContactsW( block, context, context->graph->colors[block.colorIndex].wideConstraints, NULL, 1 ); + b3TracyCZoneEnd( solve_contact ); +} + +void B3_WIDE( b3ApplyRestitution_Convex )( b3SolverBlock block, b3StepContext* context ) +{ + b3TracyCZoneNC( restitution, "Restitution", b3_colorDodgerBlue, true ); + b3ApplyRestitutionW( block, context, context->graph->colors[block.colorIndex].wideConstraints, NULL, 1 ); + b3TracyCZoneEnd( restitution ); +} + +void B3_WIDE( b3StoreImpulses_Convex )( b3SolverBlock block, b3StepContext* context, int workerIndex ) +{ + b3TracyCZoneNC( store_impulses, "Store", b3_colorFireBrick, true ); + b3StoreImpulsesW( block, context, workerIndex, context->wideConstraints, NULL, 1 ); b3TracyCZoneEnd( store_impulses ); } + +void B3_WIDE( b3WarmStartContacts_MeshWide )( b3SolverBlock block, b3StepContext* context ) +{ + b3TracyCZoneNC( warm_start_mesh_wide, "Warm Start Mesh", b3_colorGreen, true ); + b3GraphColor* color = context->graph->colors + block.colorIndex; + b3WarmStartContactsW( block, context, color->wideMeshConstraints, color->wideMeshManifoldStarts, 0 ); + b3TracyCZoneEnd( warm_start_mesh_wide ); +} + +void B3_WIDE( b3PushContacts_MeshWide )( b3SolverBlock block, b3StepContext* context ) +{ + b3TracyCZoneNC( push_mesh_wide, "Push Mesh", b3_colorAliceBlue, true ); + b3GraphColor* color = context->graph->colors + block.colorIndex; + b3PushContactsW( block, context, color->wideMeshConstraints, color->wideMeshManifoldStarts, 0 ); + b3TracyCZoneEnd( push_mesh_wide ); +} + +void B3_WIDE( b3SolveContacts_MeshWide )( b3SolverBlock block, b3StepContext* context ) +{ + b3TracyCZoneNC( solve_mesh_wide, "Solve Mesh", b3_colorAliceBlue, true ); + b3GraphColor* color = context->graph->colors + block.colorIndex; + b3SolveContactsW( block, context, color->wideMeshConstraints, color->wideMeshManifoldStarts, 0 ); + b3TracyCZoneEnd( solve_mesh_wide ); +} + +void B3_WIDE( b3ApplyRestitution_MeshWide )( b3SolverBlock block, b3StepContext* context ) +{ + b3TracyCZoneNC( restitution_mesh_wide, "Restitution Mesh", b3_colorDodgerBlue, true ); + b3GraphColor* color = context->graph->colors + block.colorIndex; + b3ApplyRestitutionW( block, context, color->wideMeshConstraints, color->wideMeshManifoldStarts, 0 ); + b3TracyCZoneEnd( restitution_mesh_wide ); +} + +void B3_WIDE( b3StoreImpulses_MeshWide )( b3SolverBlock block, b3StepContext* context, int workerIndex ) +{ + b3TracyCZoneNC( store_mesh_wide, "Store Mesh", b3_colorFireBrick, true ); + b3StoreImpulsesW( block, context, workerIndex, context->wideMeshConstraints, context->wideMeshManifoldStarts, 0 ); + b3TracyCZoneEnd( store_mesh_wide ); +} diff --git a/src/core.c b/src/core.c index fee546a..0073d8b 100644 --- a/src/core.c +++ b/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 ); diff --git a/src/mesh.c b/src/mesh.c index 312dbab..7b8d617 100644 --- a/src/mesh.c +++ b/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; } } diff --git a/src/mesh_contact.c b/src/mesh_contact.c index 67c9b6f..86f7ed0 100644 --- a/src/mesh_contact.c +++ b/src/mesh_contact.c @@ -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; diff --git a/src/shape.c b/src/shape.c index 951e2c3..56513f1 100644 --- a/src/shape.c +++ b/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; } diff --git a/src/shape.h b/src/shape.h index 91e8eef..a9164d4 100644 --- a/src/shape.h +++ b/src/shape.h @@ -5,6 +5,7 @@ #include "math_internal.h" +#include "box3d/collision.h" #include "box3d/types.h" #include @@ -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 ) diff --git a/src/simd_neon.h b/src/simd_neon.h index 97bd8d1..c35f39b 100644 --- a/src/simd_neon.h +++ b/src/simd_neon.h @@ -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 ) { diff --git a/src/simd_scalar.h b/src/simd_scalar.h index 144acc1..56cb045 100644 --- a/src/simd_scalar.h +++ b/src/simd_scalar.h @@ -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 ) { diff --git a/src/simd_sse2.h b/src/simd_sse2.h index 22ae9a5..b09a17c 100644 --- a/src/simd_sse2.h +++ b/src/simd_sse2.h @@ -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 ) { diff --git a/src/simd_wide.h b/src/simd_wide.h index 21c9cba..fe0bb9c 100644 --- a/src/simd_wide.h +++ b/src/simd_wide.h @@ -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 ) diff --git a/src/solver.c b/src/solver.c index 9cdc02b..17367bb 100644 --- a/src/solver.c +++ b/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 ); diff --git a/src/solver.h b/src/solver.h index a0d2f25..297213d 100644 --- a/src/solver.h +++ b/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; diff --git a/test/test_determinism.c b/test/test_determinism.c index aca4312..fee1efd 100644 --- a/test/test_determinism.c +++ b/test/test_determinism.c @@ -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 )