From e961bfb7bf9123188eb0addc71194c9d7af60e41 Mon Sep 17 00:00:00 2001 From: Erin Catto Date: Sat, 11 Jul 2026 23:26:31 -0700 Subject: [PATCH] Friction center weighted average (#71) The central friction anchors now have C0 continuity by using a weighted average. Reduces jitter while letting the spinning top function correctly. Fixes #65 --- .github/workflows/build.yml | 2 +- CONTRIBUTING.md | 9 ++- docs/loose_ends.md | 1 - include/box3d/constants.h | 6 ++ include/box3d/types.h | 3 - samples/gfx/debug_adapter.c | 1 - samples/host/camera.h | 5 +- samples/sample.cpp | 3 +- samples/sample_bodies.cpp | 3 - src/contact_solver.c | 112 +++++++++++++++++++++--------------- src/contact_solver.h | 4 +- src/convex_manifold.c | 6 ++ src/height_field.c | 2 +- src/island.c | 2 +- src/mesh.c | 3 +- src/physics_world.c | 14 +++-- src/solver.c | 30 +++++----- src/timer.c | 2 +- test/test_compound.c | 14 ++--- test/test_determinism.c | 8 +-- test/test_height_field.c | 8 +-- 21 files changed, 136 insertions(+), 102 deletions(-) diff --git a/.github/workflows/build.yml b/.github/workflows/build.yml index 1f20d68..19ba414 100644 --- a/.github/workflows/build.yml +++ b/.github/workflows/build.yml @@ -1,5 +1,5 @@ # https://docs.github.com/en/actions -name: CI Build +name: build_test on: push: diff --git a/CONTRIBUTING.md b/CONTRIBUTING.md index b9cf5d7..35f53d0 100644 --- a/CONTRIBUTING.md +++ b/CONTRIBUTING.md @@ -15,7 +15,8 @@ before opening a pull request — **PRs that skip these steps will be closed.** check enforces this. 4. **Follow the style.** C17, no compiler warnings, formatted with `clang-format`. -5. **Follow the LLM guidelines.** See the LLM guidelines in README.md +5. **Comments** Comment your code with human words. Create Doxygen comments for public APIs. +6. **Follow the LLM guidelines.** See the LLM guidelines in README.md --- @@ -95,6 +96,12 @@ conventions and use clang format. --- +## 6. LLM Policy + +- Follow the LLM policy in README.md + +--- + ## Questions? Open a [Discussion](../../discussions) — that's the best place for anything not diff --git a/docs/loose_ends.md b/docs/loose_ends.md index 47892b9..d7f2bbd 100644 --- a/docs/loose_ends.md +++ b/docs/loose_ends.md @@ -91,7 +91,6 @@ Category flags on the struct control what gets drawn: | `drawContacts` | Contact points and anchors | | `drawContactNormals` | Contact normal directions | | `drawContactForces` | Normal impulse magnitudes | -| `drawFrictionForces` | Friction impulse magnitudes | | `drawGraphColors` | Constraint-graph color assignment | | `drawContactFeatures` | Raw contact feature indices | | `drawIslands` | Island bounding boxes | diff --git a/include/box3d/constants.h b/include/box3d/constants.h index 87dd56d..55427ba 100644 --- a/include/box3d/constants.h +++ b/include/box3d/constants.h @@ -52,8 +52,14 @@ B3_API float b3GetStallThreshold( void ); // @warning modifying this can have a significant impact on stability #define B3_LINEAR_SLOP ( 0.005f * b3GetLengthUnitsPerMeter() ) +/// The minimum length of a capsules. Very short capsules should be created as spheres +/// to avoid numerical problems. #define B3_MIN_CAPSULE_LENGTH ( B3_LINEAR_SLOP ) +/// Minimum contact point friction weight, lower bound for speculative points. Made small +/// enough to be washed away by weights that hit 1. +#define B3_MIN_FRICTION_WEIGHT ( 1e-10f ) + /// The distance between shapes where they are considered overlapped. This is needed /// because GJK may return small positive values for overlapped shapes in degenerate /// configurations. diff --git a/include/box3d/types.h b/include/box3d/types.h index 40e6853..b0a8bb8 100644 --- a/include/box3d/types.h +++ b/include/box3d/types.h @@ -3032,9 +3032,6 @@ typedef struct b3DebugDraw /// Option to draw contact normal forces bool drawContactForces; - /// Option to draw contact friction forces - bool drawFrictionForces; - /// Option to draw islands as bounding boxes bool drawIslands; diff --git a/samples/gfx/debug_adapter.c b/samples/gfx/debug_adapter.c index 4e3f93c..68a21e1 100644 --- a/samples/gfx/debug_adapter.c +++ b/samples/gfx/debug_adapter.c @@ -227,7 +227,6 @@ void ApplyGuiFlags( b3DebugDraw* out ) out->drawContactFeatures = s_adapter.guiDraw.drawContactFeatures; out->drawContactNormals = s_adapter.guiDraw.drawContactNormals; out->drawContactForces = s_adapter.guiDraw.drawContactForces; - out->drawFrictionForces = s_adapter.guiDraw.drawFrictionForces; out->drawIslands = s_adapter.guiDraw.drawIslands; } diff --git a/samples/host/camera.h b/samples/host/camera.h index dd91efc..72e89c4 100644 --- a/samples/host/camera.h +++ b/samples/host/camera.h @@ -217,9 +217,8 @@ public: b3Vec3 m_up; b3Vec3 m_forward; - // All four matrices are produced together — view / viewInv from the basis, - // proj / projInv from fov/aspect/near/far — so the renderer never inverts - // at runtime. + // All four matrices are produced together: view / viewInv from the basis, + // proj / projInv from fov/aspect/near/far. Mat4 m_view; Mat4 m_viewInv; Mat4 m_proj; diff --git a/samples/sample.cpp b/samples/sample.cpp index 362b9d6..16d6c99 100644 --- a/samples/sample.cpp +++ b/samples/sample.cpp @@ -958,7 +958,7 @@ void Sample::DrawMetrics() int count = s.colorCounts[i]; bool isOverflow = ( i == overflowIndex ); - // Skip empty slots, but always show overflow — a non-zero overflow row is the signal we care about. + // Skip empty slots, but always show overflow. if ( count == 0 && !isOverflow ) { continue; @@ -1630,7 +1630,6 @@ static void DrawMenuBar( SampleContext* context ) ImGui::MenuItem( "Contact Normals", nullptr, &gd->drawContactNormals ); ImGui::MenuItem( "Contact Features", nullptr, &gd->drawContactFeatures ); ImGui::MenuItem( "Contact Forces", nullptr, &gd->drawContactForces ); - ImGui::MenuItem( "Friction Forces", nullptr, &gd->drawFrictionForces ); if ( ImGui::BeginMenu( "Anchor" ) ) { if ( ImGui::MenuItem( "Anchor A", nullptr, gd->drawAnchorA != 0 ) ) diff --git a/samples/sample_bodies.cpp b/samples/sample_bodies.cpp index 3581b9c..a0f3ef1 100644 --- a/samples/sample_bodies.cpp +++ b/samples/sample_bodies.cpp @@ -372,8 +372,6 @@ static int sampleGyroscopicTorque = RegisterSample( "Bodies", "Gyroscopic Torque // Spinning tops. Ported from PEEL. // Each top is tilted and spun about the world up axis. Offsetting the spin axis from the // symmetry axis is what makes them precess under gravity instead of spinning true. -// todo disabling this until issue #65 is fixed -#if 0 class GyroscopicPrecession : public Sample { public: @@ -437,7 +435,6 @@ public: }; static int sampleGyroscopicPrecession = RegisterSample( "Bodies", "Gyroscopic Precession", GyroscopicPrecession::Create ); -#endif class Weeble : public Sample { diff --git a/src/contact_solver.c b/src/contact_solver.c index 501f858..7ada73c 100644 --- a/src/contact_solver.c +++ b/src/contact_solver.c @@ -16,8 +16,6 @@ #include "shape.h" #endif -#define FIXED_ANCHORS 1 - // contact separation for sub-stepping // s = s0 + dot(cB + rB - cA - rA, normal) // normal is held constant @@ -38,6 +36,9 @@ void b3PrepareContacts_Mesh( b3SolverBlock block, b3StepContext* context ) float warmStartScale = world->enableWarmStarting ? 1.0f : 0.0f; + // 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; @@ -77,7 +78,7 @@ void b3PrepareContacts_Mesh( b3SolverBlock block, b3StepContext* context ) int localIndex = index - colorStart; B3_ASSERT( 0 <= localIndex && localIndex < spans[colorIndex].count ); int contactId = specs[localIndex].contactId; - b3Contact* contact = b3Array_Get( world->contacts, contactId ); + b3Contact* contact = b3Array_Get( world->contacts, contactId ); B3_ASSERT( contact->contactId == contactId ); int indexA = contact->bodySimIndexA; @@ -86,13 +87,13 @@ void b3PrepareContacts_Mesh( b3SolverBlock block, b3StepContext* context ) #if B3_ENABLE_VALIDATION if ( indexA != B3_NULL_INDEX ) { - b3Body* bodyA = b3Array_Get( world->bodies, contact->edges[0].bodyId ); + b3Body* bodyA = b3Array_Get( world->bodies, contact->edges[0].bodyId ); B3_ASSERT( indexA == bodyA->localIndex ); } if ( indexB != B3_NULL_INDEX ) { - b3Body* bodyB = b3Array_Get( world->bodies, contact->edges[1].bodyId ); + b3Body* bodyB = b3Array_Get( world->bodies, contact->edges[1].bodyId ); B3_ASSERT( indexB == bodyB->localIndex ); } #endif @@ -184,6 +185,7 @@ void b3PrepareContacts_Mesh( b3SolverBlock block, b3StepContext* context ) b3Vec3 centerA = b3Vec3_zero; b3Vec3 centerB = b3Vec3_zero; + float totalFrictionWeight = 0.0f; for ( int pointIndex = 0; pointIndex < pointCount; ++pointIndex ) { @@ -193,7 +195,9 @@ void b3PrepareContacts_Mesh( b3SolverBlock block, b3StepContext* context ) b3ManifoldPoint* mp = manifold->points + pointIndex; cp->rA = mp->anchorA; cp->rB = mp->anchorB; - cp->baseSeparation = mp->separation - b3Dot( b3Sub( cp->rB, cp->rA ), normal ); + + float s = mp->separation; + cp->baseSeparation = s - b3Dot( b3Sub( cp->rB, cp->rA ), normal ); cp->normalImpulse = warmStartScale * mp->normalImpulse; cp->totalNormalImpulse = 0.0f; @@ -210,15 +214,22 @@ void b3PrepareContacts_Mesh( b3SolverBlock block, b3StepContext* context ) b3Vec3 vrB = b3Add( vB, b3Cross( wB, rB ) ); cp->relativeVelocity = b3Dot( normal, b3Sub( vrB, vrA ) ); - centerA = b3Add( centerA, rA ); - centerB = b3Add( centerB, rB ); + // C0 friction center decay. Needed to prevent spinning top drift (GyroscopicPrecession sample). + // Contacts with separation greater than twice the speculative distance only matter for CCD and + // 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. + float weight = b3ClampFloat( 2.0f - s * invTau, B3_MIN_FRICTION_WEIGHT, 1.0f ); + centerA = b3MulAdd( centerA, weight, rA ); + centerB = b3MulAdd( centerB, weight, rB ); + totalFrictionWeight += weight; } - float invCount = 1.0f / pointCount; - centerA = b3MulSV( invCount, centerA ); - centerB = b3MulSV( invCount, centerB ); - constraint->originA = centerA; - constraint->originB = centerB; + float invWeight = 1.0f / totalFrictionWeight; + centerA = b3MulSV( invWeight, centerA ); + centerB = b3MulSV( invWeight, centerB ); + constraint->centerA = centerA; + constraint->centerB = centerB; for ( int pointIndex = 0; pointIndex < pointCount; ++pointIndex ) { @@ -265,7 +276,7 @@ void b3WarmStartContacts_Mesh( b3SolverBlock block, b3StepContext* context ) { b3World* world = context->world; b3GraphColor* color = world->constraintGraph.colors + block.colorIndex; - b3SolverSet* awakeSet = b3Array_Get( world->solverSets, b3_awakeSet ); + b3SolverSet* awakeSet = b3Array_Get( world->solverSets, b3_awakeSet ); b3BodyState* states = awakeSet->bodyStates.data; b3ContactConstraint* constraints = color->contactConstraints; @@ -319,8 +330,8 @@ void b3WarmStartContacts_Mesh( b3SolverBlock block, b3StepContext* context ) // Central friction { - b3Vec3 rA = constraint->originA; - b3Vec3 rB = constraint->originB; + 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 ) ); @@ -515,8 +526,8 @@ void b3SolveContacts_Mesh( b3SolverBlock block, b3StepContext* context, bool use b3Vec3 tangent2 = constraint->tangent2; // Fixed anchor points for applying impulses - b3Vec3 rA = constraint->originA; - b3Vec3 rB = constraint->originB; + b3Vec3 rA = constraint->centerA; + b3Vec3 rB = constraint->centerB; // Relative tangent velocity at contact b3Vec3 vrA = b3Add( vA, b3Cross( wA, rA ) ); @@ -756,8 +767,8 @@ void b3StoreImpulses_Mesh( b3SolverBlock block, b3StepContext* context, int work mp->totalNormalImpulse = cp->totalNormalImpulse; mp->normalVelocity = cp->relativeVelocity; - if ( checkHitEvents && flagged == false && - mp->normalVelocity < negHitThreshold && mp->totalNormalImpulse > 0.0f ) + if ( checkHitEvents && flagged == false && mp->normalVelocity < negHitThreshold && + mp->totalNormalImpulse > 0.0f ) { b3SetBit( hitEventBitSet, contact->contactId ); hasHitEvents = true; @@ -1350,7 +1361,8 @@ typedef struct b3ContactConstraintWide b3Vec3W tangent1; b3Vec3W tangent2; - b3Vec3W originA, originB; + // Friction centers + b3Vec3W centerA, centerB; b3FloatW twistMass; b3FloatW twistImpulse; b3SymMatrix2W tangentMass; @@ -1629,6 +1641,7 @@ void b3PrepareContacts_Convex( b3SolverBlock block, b3StepContext* context ) b3Softness staticSoftness = context->staticSoftness; float warmStartScale = world->enableWarmStarting ? 1.0f : 0.0f; + float invTau = 1.0f / B3_SPECULATIVE_DISTANCE; int wideIndex = block.startIndex; int endWideIndex = block.startIndex + block.count; @@ -1664,7 +1677,7 @@ void b3PrepareContacts_Convex( b3SolverBlock block, b3StepContext* context ) } int contactId = contactIds[contactIndex]; - b3Contact* contact = b3Array_Get( world->contacts, contactId ); + b3Contact* contact = b3Array_Get( world->contacts, contactId ); B3_ASSERT( contact->manifoldCount == 1 ); b3Manifold* manifold = contact->manifolds + 0; @@ -1779,8 +1792,9 @@ void b3PrepareContacts_Convex( b3SolverBlock block, b3StepContext* context ) ( (float*)&constraint->impulseScale )[lane] = soft.impulseScale; int pointCount = manifold->pointCount; - b3Vec3 originA = b3Vec3_zero; - b3Vec3 originB = b3Vec3_zero; + b3Vec3 centerA = b3Vec3_zero; + b3Vec3 centerB = b3Vec3_zero; + float totalFrictionWeight = 0.0f; for ( int pointIndex = 0; pointIndex < pointCount; ++pointIndex ) { @@ -1789,8 +1803,14 @@ void b3PrepareContacts_Convex( b3SolverBlock block, b3StepContext* context ) b3Vec3 rA = mp->anchorA; b3Vec3 rB = mp->anchorB; - originA = b3Add( originA, rA ); - originB = b3Add( originB, rB ); + float s = mp->separation; + + // C0 friction center decay. Needed to prevent spinning top drift (GyroscopicPrecession sample). + // See details in b3PrepareContacts_Mesh. This code should stay in sync. + float weight = b3ClampFloat( 2.0f - s * invTau, B3_MIN_FRICTION_WEIGHT, 1.0f ); + centerA = b3MulAdd( centerA, weight, rA ); + centerB = b3MulAdd( centerB, weight, rB ); + totalFrictionWeight += weight; ( (float*)&cp->anchorAs.X )[lane] = rA.x; ( (float*)&cp->anchorAs.Y )[lane] = rA.y; @@ -1800,7 +1820,7 @@ void b3PrepareContacts_Convex( b3SolverBlock block, b3StepContext* context ) ( (float*)&cp->anchorBs.Y )[lane] = rB.y; ( (float*)&cp->anchorBs.Z )[lane] = rB.z; - float baseSeparation = mp->separation - b3Dot( b3Sub( rB, rA ), normal ); + float baseSeparation = s - b3Dot( b3Sub( rB, rA ), normal ); ( (float*)&cp->baseSeparations )[lane] = baseSeparation; ( (float*)&cp->normalImpulses )[lane] = warmStartScale * mp->normalImpulse; @@ -1817,29 +1837,29 @@ void b3PrepareContacts_Convex( b3SolverBlock block, b3StepContext* context ) ( (float*)&cp->relativeVelocities )[lane] = b3Dot( normal, b3Sub( vrB, vrA ) ); } - float invCount = 1.0f / pointCount; - originA = b3MulSV( invCount, originA ); - originB = b3MulSV( invCount, originB ); + float invWeight = 1.0f / totalFrictionWeight; + centerA = b3MulSV( invWeight, centerA ); + centerB = b3MulSV( invWeight, centerB ); - ( (float*)&constraint->originA.X )[lane] = originA.x; - ( (float*)&constraint->originA.Y )[lane] = originA.y; - ( (float*)&constraint->originA.Z )[lane] = originA.z; - ( (float*)&constraint->originB.X )[lane] = originB.x; - ( (float*)&constraint->originB.Y )[lane] = originB.y; - ( (float*)&constraint->originB.Z )[lane] = originB.z; + ( (float*)&constraint->centerA.X )[lane] = centerA.x; + ( (float*)&constraint->centerA.Y )[lane] = centerA.y; + ( (float*)&constraint->centerA.Z )[lane] = centerA.z; + ( (float*)&constraint->centerB.X )[lane] = centerB.x; + ( (float*)&constraint->centerB.Y )[lane] = centerB.y; + ( (float*)&constraint->centerB.Z )[lane] = centerB.z; for ( int pointIndex = 0; pointIndex < pointCount; ++pointIndex ) { const b3ManifoldPoint* mp = manifold->points + pointIndex; b3ContactConstraintPointWide* cp = constraint->points + pointIndex; - ( (float*)&cp->leverArms )[lane] = b3Distance( mp->anchorA, originA ); + ( (float*)&cp->leverArms )[lane] = b3Distance( mp->anchorA, centerA ); } - b3Vec3 rtA1 = b3Cross( originA, tangent1 ); - b3Vec3 rtA2 = b3Cross( originA, tangent2 ); + b3Vec3 rtA1 = b3Cross( centerA, tangent1 ); + b3Vec3 rtA2 = b3Cross( centerA, tangent2 ); - b3Vec3 rtB1 = b3Cross( originB, tangent1 ); - b3Vec3 rtB2 = b3Cross( originB, tangent2 ); + b3Vec3 rtB1 = b3Cross( centerB, tangent1 ); + b3Vec3 rtB2 = b3Cross( centerB, tangent2 ); { b3Matrix2 k; @@ -1940,8 +1960,8 @@ void b3WarmStartContacts_Convex( b3SolverBlock block, b3StepContext* context ) // Central friction { - b3Vec3W rA = c->originA; - b3Vec3W rB = c->originB; + b3Vec3W rA = c->centerA; + b3Vec3W rB = c->centerB; b3Vec3W impulse = b3MulSVW( c->frictionImpulse.x, c->tangent1 ); impulse = b3MulAddSVW( impulse, c->frictionImpulse.y, c->tangent2 ); @@ -2120,8 +2140,8 @@ void b3SolveContacts_Convex( b3SolverBlock block, b3StepContext* context, bool u b3Vec3W tangent2 = c->tangent2; // Fixed anchor points for applying impulses - b3Vec3W rA = c->originA; - b3Vec3W rB = c->originB; + b3Vec3W rA = c->centerA; + b3Vec3W rB = c->centerB; // Relative tangent velocity at contact b3Vec3W vrA = b3AddVW( bA.v, b3CrossW( bA.w, rA ) ); @@ -2372,7 +2392,7 @@ void b3PrepareContacts_Overflow( b3StepContext* context ) b3GraphColor* color = graph->colors + B3_OVERFLOW_INDEX; uint16_t count = (uint16_t)color->contacts.count; - if (count == 0) + if ( count == 0 ) { return; } diff --git a/src/contact_solver.h b/src/contact_solver.h index fab17ee..d85f233 100644 --- a/src/contact_solver.h +++ b/src/contact_solver.h @@ -19,13 +19,13 @@ typedef struct b3ManifoldConstraintPoint typedef struct b3ManifoldConstraint { - // todo use pointer buffer b3ManifoldConstraintPoint points[4]; int pointCount; b3Vec3 normal; b3Vec3 tangent1; b3Vec3 tangent2; - b3Vec3 originA, originB; + // Friction centers + b3Vec3 centerA, centerB; float twistMass; float twistImpulse; b3Matrix2 tangentMass; diff --git a/src/convex_manifold.c b/src/convex_manifold.c index e97f58d..ff55e10 100644 --- a/src/convex_manifold.c +++ b/src/convex_manifold.c @@ -1082,6 +1082,12 @@ void b3CollideHullAndCapsule( b3LocalManifold* manifold, int capacity, const b3H } B3_VALIDATE( faceSeparation <= 0.0f ); + // Is there a valid edge-edge axis? + if ( edgeQuery.indexA == B3_NULL_INDEX ) + { + return; + } + // Face contact can be empty if it does not realize the axis of minimum penetration. // Create edge contact if face contact fails or edge contact is significantly better! const float kRelEdgeTolerance = 0.90f; diff --git a/src/height_field.c b/src/height_field.c index 1b0307f..2bf5d19 100644 --- a/src/height_field.c +++ b/src/height_field.c @@ -749,7 +749,7 @@ b3CastOutput b3ShapeCastHeightField( const b3HeightFieldData* heightField, const // nextFractionX / nextFractionZ advance in units of the clamped sweep // [minFraction, maxFraction], but bestFraction is a fraction of the full input // translation. Precompute the affine map from clamped space to input space so - // the loop termination test compares like with like — otherwise it can exit + // the loop termination test compares like with like. Otherwise it can exit // early and miss a closer hit in a later cell. float gridFractionScale = input->maxFraction * ( maxFraction - minFraction ); float gridFractionOffset = input->maxFraction * minFraction; diff --git a/src/island.c b/src/island.c index 625a1b3..6a17909 100644 --- a/src/island.c +++ b/src/island.c @@ -498,7 +498,7 @@ void b3SplitIsland( b3World* world, int baseId ) } } - // Early return — island is still fully connected, no split needed. + // Island is still fully connected, no split needed. if ( componentCount == 1 ) { baseIsland->constraintRemoveCount = 0; diff --git a/src/mesh.c b/src/mesh.c index c034899..a6628a8 100644 --- a/src/mesh.c +++ b/src/mesh.c @@ -928,7 +928,8 @@ static int b3BuildRecursive( b3Array( b3MeshNode ) * nodes, int count, b3Primiti node->data.asNode.childOffset = rightIndex - index; node->lowerBound = aabb.lowerBound; node->upperBound = aabb.upperBound; - // triangleOffset is leaf-only, but lives outside the union — zero it so mesh->hash is deterministic + + // Zero so mesh->hash is deterministic node->triangleOffset = 0; return index; diff --git a/src/physics_world.c b/src/physics_world.c index f0dc19e..2a065aa 100644 --- a/src/physics_world.c +++ b/src/physics_world.c @@ -1524,7 +1524,9 @@ void b3World_Draw( b3WorldId worldId, b3DebugDraw* draw, uint64_t maskBits ) // Average the anchors not the world points so the friction center stays exact far from the origin b3Pos contactCenter = draw->drawAnchorA == 1 ? bodySimA->center : bodySimB->center; - b3Vec3 anchorSum = b3Vec3_zero; + b3Vec3 frictionAnchor = b3Vec3_zero; + float totalWeight = 0.0f; + float invTau = 1.0f / B3_SPECULATIVE_DISTANCE; const b3ManifoldPoint* points = manifold->points; for ( int pointIndex = 0; pointIndex < manifold->pointCount; ++pointIndex ) @@ -1536,7 +1538,10 @@ void b3World_Draw( b3WorldId worldId, b3DebugDraw* draw, uint64_t maskBits ) b3Vec3 anchor = draw->drawAnchorA == 1 ? mp->anchorA : mp->anchorB; b3Pos p = b3OffsetPos( contactCenter, anchor ); - anchorSum = b3Add( anchorSum, anchor ); + // See similar friction anchor weights in b3PrepareContacts_Mesh. + float weight = b3ClampFloat( 2.0f - mp->separation * invTau, B3_MIN_FRICTION_WEIGHT, 1.0f ); + frictionAnchor = b3MulAdd( frictionAnchor, weight, anchor ); + totalWeight += weight; if ( draw->drawContactNormals ) { @@ -1599,9 +1604,8 @@ void b3World_Draw( b3WorldId worldId, b3DebugDraw* draw, uint64_t maskBits ) { // Hack inv_dt for single step debugging float inv_dt = world->inv_dt > 0.0f ? world->inv_dt : 60.0f; - - b3Vec3 avgAnchor = b3MulSV( 1.0f / manifold->pointCount, anchorSum ); - b3Pos p1 = b3OffsetPos( contactCenter, avgAnchor ); + frictionAnchor = b3MulSV( 1.0f / totalWeight, frictionAnchor ); + b3Pos p1 = b3OffsetPos( contactCenter, frictionAnchor ); b3Vec3 frictionForce = b3MulSV( 0.5f * inv_dt, manifold->frictionImpulse ); b3Pos p2 = b3OffsetPos( p1, b3MulSV( draw->forceScale, frictionForce ) ); draw->DrawSegmentFcn( p1, p2, frictionColor, draw->context ); diff --git a/src/solver.c b/src/solver.c index a2b1be9..0fde035 100644 --- a/src/solver.c +++ b/src/solver.c @@ -118,33 +118,33 @@ static void b3IntegrateVelocitiesTask( b3SolverBlock block, b3StepContext* conte b3Vec3 omega2 = omega1; // Symmetric inertia tensor: 6 unique entries (column-major) - const float i00 = inertiaLocal.cx.x; - const float i01 = inertiaLocal.cy.x; - const float i02 = inertiaLocal.cz.x; - const float i11 = inertiaLocal.cy.y; - const float i12 = inertiaLocal.cz.y; - const float i22 = inertiaLocal.cz.z; + float i00 = inertiaLocal.cx.x; + float i01 = inertiaLocal.cy.x; + float i02 = inertiaLocal.cz.x; + float i11 = inertiaLocal.cy.y; + float i12 = inertiaLocal.cz.y; + float i22 = inertiaLocal.cz.z; for ( int gyroIteration = 0; gyroIteration < 1; ++gyroIteration ) { - const float w1 = omega2.x; - const float w2 = omega2.y; - const float w3 = omega2.z; + float w1 = omega2.x; + float w2 = omega2.y; + float w3 = omega2.z; // Iw = I * omega2 (shared between residual and Jacobian) - const float Iw1 = i00 * w1 + i01 * w2 + i02 * w3; - const float Iw2 = i01 * w1 + i11 * w2 + i12 * w3; - const float Iw3 = i02 * w1 + i12 * w2 + i22 * w3; + float Iw1 = i00 * w1 + i01 * w2 + i02 * w3; + float Iw2 = i01 * w1 + i11 * w2 + i12 * w3; + float Iw3 = i02 * w1 + i12 * w2 + i22 * w3; - // Residual: b = I*(omega2 - omega1) + h * (omega2 × I*omega2) - const b3Vec3 dw = b3Sub( omega2, omega1 ); + // Residual: b = I * (omega2 - omega1) + h * cross(omega2, I * omega2) + b3Vec3 dw = b3Sub( omega2, omega1 ); b3Vec3 b = { i00 * dw.x + i01 * dw.y + i02 * dw.z + h * ( w2 * Iw3 - w3 * Iw2 ), i01 * dw.x + i11 * dw.y + i12 * dw.z + h * ( w3 * Iw1 - w1 * Iw3 ), i02 * dw.x + i12 * dw.y + i22 * dw.z + h * ( w1 * Iw2 - w2 * Iw1 ), }; - // Jacobian J = I + h * (skew(omega2) * I - skew(I*omega2)) + // Jacobian J = I + h * (skew(omega2) * I - skew(I * omega2)) // Jacobian derived by Erin Catto, Ph.D. Do not attempt to do this without a Ph.D. // Doubled inertia terms above fold into Iw, e.g. row 2 col 1: i00*w3 - i02*w1 - Iw3. b3Matrix3 J = { diff --git a/src/timer.c b/src/timer.c index 826b149..726fa18 100644 --- a/src/timer.c +++ b/src/timer.c @@ -517,7 +517,7 @@ typedef struct b3Thread char name[NAME_LENGTH]; } b3Thread; -// macOS pthread_setname_np takes only the name — it always names the calling thread. +// macOS pthread_setname_np takes only the name, it always names the calling thread. static void b3SetCurrentThreadName( const char* name ) { if ( name == NULL || name[0] == 0 ) diff --git a/test/test_compound.c b/test/test_compound.c index 7fac23f..f0aaa1f 100644 --- a/test/test_compound.c +++ b/test/test_compound.c @@ -188,7 +188,7 @@ static int CompoundMaterialDistinct( void ) static int CompoundMaterialCrossShape( void ) { - // One material shared across capsule, hull, and sphere → 1 material slot. + // One material shared across capsule, hull, and sphere -> 1 material slot. b3SurfaceMaterial mat = MakeMaterial( 0.5f, 99 ); b3CompoundCapsuleDef cap = { .capsule = { { 0, 0, 0 }, { 1, 0, 0 }, 0.25f }, .material = mat }; @@ -422,14 +422,14 @@ static int CompoundChildDispatch( void ) }; b3CompoundData* c = b3CreateCompound( &def ); - // Index ordering is capsules → hulls → meshes → spheres. + // Index ordering is capsules -> hulls -> meshes -> spheres. ENSURE( b3GetCompoundChild( c, 0 ).type == b3_capsuleShape ); ENSURE( b3GetCompoundChild( c, 1 ).type == b3_capsuleShape ); ENSURE( b3GetCompoundChild( c, 2 ).type == b3_hullShape ); ENSURE( b3GetCompoundChild( c, 3 ).type == b3_meshShape ); ENSURE( b3GetCompoundChild( c, 4 ).type == b3_sphereShape ); - // Capsule and sphere children always report identity transform — the position + // Capsule and sphere children always report identity transform. The position // is encoded in the shape itself (capsule->center{1,2}, sphere->center). b3ChildShape cap0 = b3GetCompoundChild( c, 0 ); ENSURE_SMALL( cap0.transform.p.x, FLT_EPSILON ); @@ -514,7 +514,7 @@ static int CompoundRayCastClosest( void ) b3CastOutput out = b3RayCastCompound( c, &input ); ENSURE( out.hit == true ); - // Front face of the nearer sphere is at x=4 → fraction 4/20 = 0.2. + // Front face of the nearer sphere is at x=4 -> fraction 4/20 = 0.2. ENSURE_SMALL( out.fraction - 0.2f, 1e-4f ); ENSURE_SMALL( out.normal.x + 1.0f, 1e-4f ); ENSURE( out.childIndex == 0 ); @@ -528,7 +528,7 @@ static int CompoundRayCastClosest( void ) static int CompoundRayCastHullNormalRotation( void ) { - // A unit box rotated 90° about Z, placed at compound +X. The ray hits the + // A unit box rotated 90 degrees about Z, placed at compound +X. The ray hits the // face that, in compound space, points back toward -X. Verifies that the // normal returned by the cast has been rotated from hull-local space back // into compound space. @@ -575,7 +575,7 @@ static int CompoundShapeCastClosest( void ) }; b3CastOutput out = b3ShapeCastCompound( c, &input ); ENSURE( out.hit == true ); - // Closest contact: caster radius 0.25 + sphere radius 1.0 → first contact at x ≈ 3.75. + // Closest contact: caster radius 0.25 + sphere radius 1.0 -> first contact at x ~= 3.75. ENSURE_SMALL( out.fraction - 3.75f / 20.0f, 1e-3f ); ENSURE( out.childIndex == 0 ); @@ -640,7 +640,7 @@ static int CompoundQuery( void ) b3CompoundDef def = { .spheres = spheres, .sphereCount = 3 }; b3CompoundData* c = b3CreateCompound( &def ); - // Tight box around the middle sphere — only it should be reported. + // Tight box around the middle sphere. Only it should be reported. b3AABB middle = { { -1, -1, -1 }, { 1, 1, 1 } }; struct QueryAccumulator acc = { .stopAfter = -1 }; b3QueryCompound( c, middle, QueryCallback, &acc ); diff --git a/test/test_determinism.c b/test/test_determinism.c index 888a0d6..e497d0b 100644 --- a/test/test_determinism.c +++ b/test/test_determinism.c @@ -17,11 +17,11 @@ // 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 EXPECTED_SLEEP_STEP 301 -#define EXPECTED_HASH 0xE4844A97 +#define EXPECTED_SLEEP_STEP 297 +#define EXPECTED_HASH 0x27FF38C1 #else -#define EXPECTED_SLEEP_STEP 269 -#define EXPECTED_HASH 0x50313037 +#define EXPECTED_SLEEP_STEP 308 +#define EXPECTED_HASH 0x1E5EDD79 #endif static int SingleMultithreadingTest( int workerCount ) diff --git a/test/test_height_field.c b/test/test_height_field.c index 2dd3fc6..b293579 100644 --- a/test/test_height_field.c +++ b/test/test_height_field.c @@ -143,7 +143,7 @@ static int RayCastFlatField( void ) b3HeightFieldData* hf = b3CreateHeightField( &def ); - // Origin sits clearly inside triangle 0 of cell (1, 1) — off the cell + // Origin sits clearly inside triangle 0 of cell (1, 1). Off the cell // diagonal x+z = 3. The translation overshoots the surface so the hit // fraction is strictly less than maxFraction. b3RayCastInput input = { 0 }; @@ -168,13 +168,13 @@ static int OverlapAtSurface( void ) b3Vec3 scale = { 1.0f, 1.0f, 1.0f }; b3HeightFieldData* hf = b3CreateGrid( 4, 4, scale, false ); - // Sphere center 1.0 above the surface, radius 0.5 — clear gap. + // Sphere center 1.0 above the surface, radius 0.5. Clear gap. b3Vec3 above = { 1.5f, 1.0f, 1.5f }; b3ShapeProxy proxyAbove = { &above, 1, 0.5f }; bool hitAbove = b3OverlapHeightField( hf, b3Transform_identity, &proxyAbove ); ENSURE( hitAbove == false ); - // Sphere centered on the surface — radius pokes through. + // Sphere centered on the surface. Radius pokes through. b3Vec3 through = { 1.5f, 0.0f, 1.5f }; b3ShapeProxy proxyThrough = { &through, 1, 0.5f }; bool hitThrough = b3OverlapHeightField( hf, b3Transform_identity, &proxyThrough ); @@ -431,7 +431,7 @@ static int ShapeCastBruteForce( void ) // Brute-force ray cast: cast the ray against every (non-hole) triangle and keep // the closest hit. b3GetHeightFieldTriangle returns vertices in the same winding // order that b3ShapeCastHeightField feeds to b3IntersectRayTriangle, so this is a -// pure traversal/culling check — the per-triangle math is identical. +// pure traversal/culling check. The per-triangle math is identical. static b3CastOutput BruteForceRayCast( const b3HeightFieldData* hf, const b3RayCastInput* input ) { b3CastOutput best = { 0 };