< Summary

Line coverage
100%
Covered lines: 985
Uncovered lines: 0
Coverable lines: 985
Total lines: 1603
Line coverage: 100%
Branch coverage
100%
Covered branches: 404
Total branches: 404
Branch coverage: 100%
Method coverage

Feature is only available for sponsors

Upgrade to PRO version

Metrics

MethodBranch coverage Crap Score Cyclomatic complexity Line coverage
File 1: .cctor()100%11100%
File 1: CalculateImpulse(...)100%88100%
File 1: PrepareSolve(...)100%22100%
File 1: CalculateImpulse(...)100%3838100%
File 1: IsCorrectionOwner(...)100%1616100%
File 1: TryCreateBodyPair(...)100%88100%
File 1: TryCreateContact(...)100%1818100%
File 1: TryCalculateExactNormalResult(...)100%11100%
File 1: ApplyPositionCorrection(...)100%1414100%
File 1: TryApplyCachedImpulse(...)100%66100%
File 1: TryApplyContactImpulseCombination(...)100%44100%
File 1: TryApplyContactImpulse(...)100%66100%
File 1: TryApplyContactImpulseExact(...)100%11100%
File 1: TryApplyContactImpulseCombinationExact(...)100%22100%
File 1: TryApplyNormalImpulse(...)100%1616100%
File 1: ResolveLinearVelocity(...)100%44100%
File 1: ResolveAngularVelocity(...)100%44100%
File 1: TryApplyExactImpulse(...)100%22100%
File 1: TryApplyCompactImpulse(...)100%22100%
File 1: CanNegate(...)100%44100%
File 1: TryGetLinearVelocityDelta(...)100%44100%
File 1: TryGetAngularVelocityDelta(...)100%1010100%
File 1: TryPrepareVelocityStates(...)100%44100%
File 1: TryApplyVelocityDeltas(...)100%22100%
File 1: ApplyVelocityStates(...)100%44100%
File 1: GetExactLevers(...)100%44100%
File 1: RejectResponse(...)100%11100%
File 1: ClearWarmStartImpulse(...)100%11100%
File 1: IsWarmStartCompatible(...)100%11100%
File 1: ResolveContactNormal(...)100%44100%
File 2: TrySolveFrictionImpulse(...)100%1212100%
File 2: TryGetCompactFrictionResponse(...)100%2626100%
File 2: TrySolveFrictionImpulseExact(...)100%44100%
File 2: TryGetCompactTangentImpulseDelta(...)100%1212100%
File 2: TryGetCompactConstrainedInverseMass(...)100%44100%
File 2: TryGetFrictionLimit(...)100%66100%
File 2: TryGetMagnitudeSquared(...)100%22100%
File 2: TryGetSquare(...)100%44100%
File 3: TrySeedFlatSupportNormals(...)100%2020100%
File 3: IsFlatCuboidPatch(...)100%11100%
File 3: HasOnlyNormalMotion(...)100%11100%
File 3: HasFlatNormalRows(...)100%44100%
File 3: TrySeedSupportedIslandNormals(...)100%2020100%
File 3: TryPrepareIslandNormalVelocities(...)100%2020100%
File 3: TryGetSupportedBodies(...)100%2020100%
File 3: IsFixedIslandSupport(...)100%44100%
File 3: HasUnconstrainedIslandMobility(...)100%66100%
File 3: TryReadIslandNormals(...)100%2020100%
File 3: TryReadIslandFriction(...)100%44100%
File 3: TryPrepareIslandFriction(...)100%44100%
File 3: TryPrepareIslandVelocity(...)100%88100%
File 3: ArePreparedIslandNormalsFeasible(...)100%44100%
File 3: TryPrepareIslandPositions(...)100%22100%
File 3: TryGetTranslationDepth(...)100%44100%
File 3: TryPrepareIslandTranslation(...)100%11100%
File 3: StoreIslandNormals(...)100%66100%
File 3: .ctor(...)100%11100%
File 3: TryAdd(...)100%22100%
File 3: TryDot(...)100%11100%
File 3: TryGetPreparedSpeed(...)100%11100%
File 3: .ctor(...)100%11100%
File 3: TryTransform(...)100%11100%

File(s)

/home/runner/work/Gravitas/Gravitas/src/Gravitas/CollisionHandling/Response/3D/CollisionResponse.cs

#LineLine coverage
 1//=======================================================================
 2// CollisionResponse.cs
 3//=======================================================================
 4// MIT License, Copyright (c) 2026-present David Oravsky (mrdav30)
 5// See LICENSE file in the project root for full license information.
 6//=======================================================================
 7
 8using System.Runtime.CompilerServices;
 9using FixedMathSharp;
 10using Gravitas.Materials;
 11
 12namespace Gravitas.CollisionHandling;
 13
 14/// <summary>
 15/// Solves deterministic contact response for one collision pair manifold.
 16/// </summary>
 17/// <content>
 18/// Streams deterministic warm starts, independent normal rows and translation correction.
 19/// </content>
 20public static partial class CollisionResponse
 21{
 22    /// <summary>
 23    /// Penetration depth below this value is treated as contact slop and does not
 24    /// produce positional correction.
 25    /// </summary>
 126    public static readonly Fixed64 PenetrationSlop = (Fixed64)0.01f;
 27
 28    /// <summary>
 29    /// Fraction of penetration above slop corrected per solver call.
 30    /// </summary>
 131    public static readonly Fixed64 PenetrationCorrectionPercent = (Fixed64)0.8f;
 32
 133    private static readonly Fixed64 WarmStartNormalCompatibilityThreshold = Fixed64.FromFraction(63, 64);
 34
 35    /// <summary>
 36    /// Applies positional correction, normal impulses, and Coulomb friction for
 37    /// the collision pair's current deterministic contact manifold.
 38    /// </summary>
 39    /// <exception cref="System.OverflowException">A corrected body position cannot be represented.</exception>
 40    public static void CalculateImpulse(CollisionPair pair)
 41    {
 3873642        PrepareSolve(pair);
 3873643        bool seededNormals = TrySeedFlatSupportNormals(pair);
 3873644        int iterations = pair.Manifold.Count > 1 ? pair.Context.Settings.DiscreteSolverIterations : 1;
 33917845        for (int iteration = 0; iteration < iterations; iteration++)
 13085546            CalculateImpulse(pair, applyCachedImpulse: iteration == 0 && !seededNormals,
 13085547                applyPositionCorrection: iteration == 0 && !seededNormals);
 3873448        pair.ResponseSnapshot = default;
 3873449    }
 50
 51    internal static void PrepareSolve(CollisionPair pair)
 52    {
 4762553        pair.ResponseSnapshot = default;
 54        // Also cover callers that author the public manifold without narrow phase.
 4762555        pair.RetainWarmStarts();
 4762556        if (TryCreateBodyPair(pair, out ResponseBody3D first, out ResponseBody3D second))
 4762057            pair.ResponseSnapshot = new ContactResponseSnapshot(first.Body, second.Body,
 4762058                ResolveLinearVelocity(first.Body), ResolveAngularVelocity(first.Body),
 4762059                ResolveLinearVelocity(second.Body), ResolveAngularVelocity(second.Body));
 4762560    }
 61
 62    internal static void CalculateImpulse(
 63        CollisionPair pair,
 64        bool applyCachedImpulse,
 65        bool applyPositionCorrection)
 66    {
 21654367        if (!TryCreateBodyPair(pair, out ResponseBody3D bodyA, out ResponseBody3D bodyB))
 1568            return;
 69
 21659170        if (!pair.ResponseSnapshot.IsValid) PrepareSolve(pair);
 21652871        ref readonly ContactResponseSnapshot impact = ref pair.ResponseSnapshot;
 21652872        ContactAnchor responseCenterA = impact.CenterA;
 21652873        ContactAnchor responseCenterB = impact.CenterB;
 21652874        Vector3d responsePositionA = impact.PositionA;
 21652875        Vector3d responsePositionB = impact.PositionB;
 76
 77        // Warm every admitted row before solving. Updating one cache must not
 78        // change which impulse another row uses for this warm-start pass.
 21652879        if (applyCachedImpulse || applyPositionCorrection)
 80        {
 20688081            for (int group = 0; group < pair.Manifold.GroupCount; group++)
 82            {
 33438483                for (int point = 0; point < pair.Manifold.GetGroup(group).Count; point++)
 84                {
 11031285                    if (!TryCreateContact(pair, bodyA, bodyB, responseCenterA,
 11031286                            responseCenterB, group, point, out SolverContact3D contact))
 87                        continue;
 11031188                    if (applyPositionCorrection && IsCorrectionOwner(pair, contact))
 4659989                        ApplyPositionCorrection(contact, Fixed64.One);
 11030990                    if (applyCachedImpulse && !TryApplyCachedImpulse(pair, contact,
 11030991                            responsePositionA, responsePositionB))
 892                        ClearWarmStartImpulse(pair, contact);
 93                }
 94            }
 95
 96        }
 97
 21652698        Fixed64 threshold = pair.Context.Settings.RestitutionVelocityThreshold;
 98998499        for (int group = 0; group < pair.Manifold.GroupCount; group++)
 100        {
 1761306101            for (int point = 0; point < pair.Manifold.GetGroup(group).Count; point++)
 102            {
 602187103                if (!TryCreateContact(pair, bodyA, bodyB, responseCenterA,
 602187104                        responseCenterB, group, point, out SolverContact3D contact))
 105                    continue;
 602186106                ContactNormalImpulseResult3D normalResult = default;
 602186107                bool resolved = !contact.RelativeA.IsExact && !contact.RelativeB.IsExact
 602186108                    && ContactNormalImpulse3D.TryCalculateAccumulatedDeltaWithImpact(
 602186109                        bodyA.Body, ResolveLinearVelocity(bodyA.Body), ResolveAngularVelocity(bodyA.Body),
 602186110                        contact.RelativeA.Vector, bodyB.Body, ResolveLinearVelocity(bodyB.Body),
 602186111                        ResolveAngularVelocity(bodyB.Body), contact.RelativeB.Vector,
 602186112                        contact.Normal, contact.Restitution, threshold, contact.CachedNormalImpulse,
 602186113                        impact, out normalResult);
 602186114                if (!resolved)
 11301115                    resolved = TryCalculateExactNormalResult(pair, contact, responsePositionA,
 11301116                        responsePositionB, threshold, Fixed64.One, out normalResult);
 602186117                if (!resolved || !TryApplyNormalImpulse(pair, contact, normalResult))
 118                {
 4119                    RejectResponse(pair, contact);
 4120                    continue;
 121                }
 602182122                Fixed64 normalImpulse = contact.CachedNormalImpulse + normalResult.ImpulseScalar;
 602182123                if (!TrySolveFrictionImpulse(pair, contact, responsePositionA, responsePositionB,
 602182124                        normalImpulse, out Fixed64 tangentImpulse, out Fixed64 secondaryTangentImpulse))
 125                {
 1126                    RejectResponse(pair, contact);
 1127                    continue;
 128                }
 602181129                pair.StoreWarmStartImpulse(pair.Manifold.GetGroup(group).Key, contact.ContactId,
 602181130                    contact.Normal, normalImpulse, tangentImpulse, secondaryTangentImpulse);
 131            }
 132        }
 216526133    }
 134
 135    private static bool IsCorrectionOwner(CollisionPair pair, SolverContact3D contact)
 136    {
 137        // Position correction is translational: repeated samples of the same
 138        // direction must not multiply it. Velocity rows keep their own levers.
 139        // ponytail: quadratic scan over bounded group samples; index directions
 140        // only if measured large multi-surface workloads justify that scratch.
 335252141        for (int group = 0; group < pair.Manifold.GroupCount; group++)
 142        {
 121027143            ref ContactGroup current = ref pair.Manifold.GetGroup(group);
 465458144            for (int point = 0; point < current.Count; point++)
 145            {
 175405146                ManifoldContact other = current[point];
 175405147                if (ResolveContactNormal(other.Normal, pair.ColliderB.Center - pair.ColliderA.Center) != contact.Normal)
 175761148                if (other.Depth > contact.Depth) return false;
 173531149                if (other.Depth == contact.Depth && (group < contact.GroupIndex
 236119150                    || group == contact.GroupIndex && point < contact.PointIndex)) return false;
 151            }
 152        }
 46599153        return true;
 154    }
 155
 156    private static bool TryCreateBodyPair(CollisionPair pair, out ResponseBody3D bodyA, out ResponseBody3D bodyB)
 157    {
 264168158        bodyA = default;
 264168159        bodyB = default;
 160
 264168161        if (pair.ColliderA.IsTrigger || pair.ColliderB.IsTrigger)
 2162            return false;
 163
 264166164        if (!pair.Manifold.HasContact)
 2165            return false;
 166
 264164167        bodyA = ResponseBody3D.Create(pair.ColliderA);
 264164168        bodyB = ResponseBody3D.Create(pair.ColliderB);
 264164169        return bodyA.HasSolverMobility || bodyB.HasSolverMobility;
 170    }
 171
 172    private static bool TryCreateContact(
 173        CollisionPair pair,
 174        ResponseBody3D bodyA,
 175        ResponseBody3D bodyB,
 176        in ContactAnchor responseCenterA,
 177        in ContactAnchor responseCenterB,
 178        int groupIndex,
 179        int pointIndex,
 180        out SolverContact3D contact,
 181        bool requireMatchingTangentBasis = false)
 182    {
 714434183        contact = default;
 714434184        ref ContactGroup group = ref pair.Manifold.GetGroup(groupIndex);
 714434185        ManifoldContact manifoldContact = group[pointIndex];
 714434186        Vector3d normal = ResolveContactNormal(manifoldContact.Normal, pair.ColliderB.Center - pair.ColliderA.Center);
 714434187        if (normal == Vector3d.Zero)
 2188            return false;
 189        // A bodyless static endpoint has no torque arm or mutable solver state.
 714432190        ContactLever3D relativeA = bodyA.Body == null ? default : ContactLever3D.Create(
 714432191            manifoldContact.AnchorA,
 714432192            responseCenterA);
 714432193        ContactLever3D relativeB = bodyB.Body == null ? default : ContactLever3D.Create(
 714432194            manifoldContact.AnchorB,
 714432195            responseCenterB);
 714432196        PhysicsMaterial materialA = manifoldContact.HasMaterialOverride
 714432197            ? manifoldContact.MaterialA
 714432198            : pair.ColliderA.Material;
 714432199        PhysicsMaterial materialB = manifoldContact.HasMaterialOverride
 714432200            ? manifoldContact.MaterialB
 714432201            : pair.ColliderB.Material;
 202
 714432203        Fixed64 cachedNormalImpulse = Fixed64.Zero;
 714432204        Fixed64 cachedTangentImpulse = Fixed64.Zero;
 714432205        Fixed64 cachedSecondaryTangentImpulse = Fixed64.Zero;
 714432206        if (pair.TryGetWarmStartImpulse(group.Key, manifoldContact.ContactId, out ContactWarmStartImpulse cached))
 207        {
 571036208            if (IsWarmStartCompatible(cached.Normal, normal))
 209            {
 571034210                cachedNormalImpulse = cached.NormalImpulse;
 571034211                if (!requireMatchingTangentBasis || cached.Normal == normal)
 212                {
 571033213                    cachedTangentImpulse = cached.TangentImpulse;
 571033214                    cachedSecondaryTangentImpulse = cached.SecondaryTangentImpulse;
 215                }
 216            }
 217        }
 218
 714432219        contact = new SolverContact3D(
 714432220            groupIndex,
 714432221            pointIndex,
 714432222            manifoldContact.ContactId,
 714432223            bodyA,
 714432224            bodyB,
 714432225            relativeA,
 714432226            relativeB,
 714432227            manifoldContact.Depth,
 714432228            normal,
 714432229            materialA,
 714432230            materialB,
 714432231            cachedNormalImpulse,
 714432232            cachedTangentImpulse,
 714432233            cachedSecondaryTangentImpulse);
 714432234        return true;
 235    }
 236
 237    [MethodImpl(MethodImplOptions.NoInlining)]
 238    private static bool TryCalculateExactNormalResult(
 239        CollisionPair pair,
 240        SolverContact3D contact,
 241        Vector3d responsePositionA,
 242        Vector3d responsePositionB,
 243        Fixed64 restitutionVelocityThreshold,
 244        Fixed64 contactShare,
 245        out ContactNormalImpulseResult3D result)
 246    {
 11301247        result = default;
 11301248        GetExactLevers(
 11301249            pair,
 11301250            contact,
 11301251            responsePositionA,
 11301252            responsePositionB,
 11301253            out ExactLever3D exactA,
 11301254            out ExactLever3D exactB);
 11301255        return ContactNormalImpulse3D.TryCalculateAccumulatedDeltaExact(
 11301256            contact.A.Body,
 11301257            ResolveLinearVelocity(contact.A.Body),
 11301258            ResolveAngularVelocity(contact.A.Body),
 11301259            exactA,
 11301260            contact.B.Body,
 11301261            ResolveLinearVelocity(contact.B.Body),
 11301262            ResolveAngularVelocity(contact.B.Body),
 11301263            exactB,
 11301264            contact.Normal,
 11301265            contact.Restitution,
 11301266            restitutionVelocityThreshold,
 11301267            contact.CachedNormalImpulse,
 11301268            contactShare,
 11301269            Fixed64.One,
 11301270            out result, pair.ResponseSnapshot);
 271    }
 272
 273    private static void ApplyPositionCorrection(SolverContact3D contact, Fixed64 contactShare)
 274    {
 46608275        Fixed64 correctionDepth = contact.Depth - PenetrationSlop;
 46608276        if (correctionDepth <= Fixed64.Zero)
 1561277            return;
 278
 45047279        Fixed64 inverseMassA = contact.A.GetConstrainedInverseMass(contact.Normal);
 45047280        Fixed64 inverseMassB = contact.B.GetConstrainedInverseMass(contact.Normal);
 45047281        Fixed64 totalInverseMass = inverseMassA + inverseMassB;
 45047282        if (totalInverseMass <= Fixed64.Zero)
 16283            return;
 284
 45031285        Fixed64 weightedDepth = correctionDepth * PenetrationCorrectionPercent * contactShare;
 286        Vector3d correctionA, correctionB;
 45031287        if (CollisionPositionCorrection.TryGetCompactScale(
 45031288                weightedDepth, inverseMassA, inverseMassB, out Fixed64 scale))
 289        {
 44995290            Vector3d correction = contact.Normal * scale;
 44995291            correctionA = -correction * inverseMassA;
 44995292            correctionB = correction * inverseMassB;
 293        }
 294        else
 295        {
 36296            correctionA = -contact.Normal * CollisionPositionCorrection.GetWeightedDistance(
 36297                weightedDepth, inverseMassA, inverseMassA, inverseMassB);
 36298            correctionB = contact.Normal * CollisionPositionCorrection.GetWeightedDistance(
 36299                weightedDepth, inverseMassB, inverseMassA, inverseMassB);
 300        }
 301        // Prepare both endpoints before publishing either participant's correction.
 45031302        Vector3d positionA = contact.A.Body?.PrepareCollisionPositionCorrection(correctionA) ?? default;
 45031303        Vector3d positionB = contact.B.Body?.PrepareCollisionPositionCorrection(correctionB) ?? default;
 45029304        contact.A.Body?.ApplyCollisionPositionState(positionA);
 45029305        contact.B.Body?.ApplyCollisionPositionState(positionB);
 37511306    }
 307
 308    private static bool TryApplyCachedImpulse(
 309        CollisionPair pair,
 310        SolverContact3D contact,
 311        Vector3d responsePositionA,
 312        Vector3d responsePositionB)
 313    {
 110308314        if (contact.CachedNormalImpulse == Fixed64.Zero
 110308315            && contact.CachedTangentImpulse == Fixed64.Zero
 110308316            && contact.CachedSecondaryTangentImpulse == Fixed64.Zero)
 317        {
 91358318            return true;
 319        }
 320
 18950321        return TryApplyContactImpulseCombination(
 18950322            pair,
 18950323            contact,
 18950324            responsePositionA,
 18950325            responsePositionB,
 18950326            contact.Normal,
 18950327            contact.CachedNormalImpulse,
 18950328            contact.Tangent,
 18950329            contact.CachedTangentImpulse,
 18950330            contact.SecondaryTangent,
 18950331            contact.CachedSecondaryTangentImpulse);
 332    }
 333
 334    private static bool TryApplyContactImpulseCombination(
 335        CollisionPair pair,
 336        SolverContact3D contact,
 337        Vector3d responsePositionA,
 338        Vector3d responsePositionB,
 339        Vector3d firstAxis,
 340        Fixed64 firstScale,
 341        Vector3d secondAxis,
 342        Fixed64 secondScale,
 343        Vector3d thirdAxis,
 344        Fixed64 thirdScale)
 345    {
 36660346        if (ContactResponseArithmetic3D.TryLinearCombination(
 36660347                firstAxis,
 36660348                firstScale,
 36660349                secondAxis,
 36660350                secondScale,
 36660351                thirdAxis,
 36660352                thirdScale,
 36660353                out Vector3d impulse)
 36660354            && CanNegate(impulse))
 355        {
 36654356            return TryApplyContactImpulse(
 36654357                pair,
 36654358                contact,
 36654359                responsePositionA,
 36654360                responsePositionB,
 36654361                impulse);
 362        }
 363
 6364        return TryApplyContactImpulseCombinationExact(
 6365            pair,
 6366            contact,
 6367            responsePositionA,
 6368            responsePositionB,
 6369            firstAxis,
 6370            firstScale,
 6371            secondAxis,
 6372            secondScale,
 6373            thirdAxis,
 6374            thirdScale);
 375    }
 376
 377    private static bool TryApplyContactImpulse(
 378        CollisionPair pair,
 379        SolverContact3D contact,
 380        Vector3d responsePositionA,
 381        Vector3d responsePositionB,
 382        Vector3d impulse)
 383    {
 36654384        if (!contact.RelativeA.IsExact
 36654385            && !contact.RelativeB.IsExact
 36654386            && TryApplyCompactImpulse(contact, impulse))
 387        {
 36647388            return true;
 389        }
 390
 7391        return TryApplyContactImpulseExact(
 7392            pair,
 7393            contact,
 7394            responsePositionA,
 7395            responsePositionB,
 7396            impulse);
 397    }
 398
 399    [MethodImpl(MethodImplOptions.NoInlining)]
 400    private static bool TryApplyContactImpulseExact(
 401        CollisionPair pair,
 402        SolverContact3D contact,
 403        Vector3d responsePositionA,
 404        Vector3d responsePositionB,
 405        Vector3d impulse)
 406    {
 7407        GetExactLevers(
 7408            pair,
 7409            contact,
 7410            responsePositionA,
 7411            responsePositionB,
 7412            out ExactLever3D exactA,
 7413            out ExactLever3D exactB);
 7414        return TryApplyExactImpulse(
 7415            contact,
 7416            exactA,
 7417            exactB,
 7418            impulse);
 419    }
 420
 421    [MethodImpl(MethodImplOptions.NoInlining)]
 422    private static bool TryApplyContactImpulseCombinationExact(
 423        CollisionPair pair,
 424        SolverContact3D contact,
 425        Vector3d responsePositionA,
 426        Vector3d responsePositionB,
 427        Vector3d firstAxis,
 428        Fixed64 firstScale,
 429        Vector3d secondAxis,
 430        Fixed64 secondScale,
 431        Vector3d thirdAxis,
 432        Fixed64 thirdScale)
 433    {
 6434        GetExactLevers(
 6435            pair,
 6436            contact,
 6437            responsePositionA,
 6438            responsePositionB,
 6439            out ExactLever3D exactA,
 6440            out ExactLever3D exactB);
 6441        if (!ExactContactLever3D
 6442                .TryGetImpulseCombinationVelocityDeltas(
 6443                    contact.A.Body,
 6444                    exactA,
 6445                    contact.B.Body,
 6446                    exactB,
 6447                    firstAxis,
 6448                    firstScale,
 6449                    secondAxis,
 6450                    secondScale,
 6451                    thirdAxis,
 6452                    thirdScale,
 6453                    out Vector3d linearA,
 6454                    out Vector3d angularA,
 6455                    out Vector3d linearB,
 6456                    out Vector3d angularB))
 457        {
 4458            return false;
 459        }
 460
 2461        return TryApplyVelocityDeltas(
 2462            contact,
 2463            linearA,
 2464            angularA,
 2465            linearB,
 2466            angularB);
 467    }
 468
 469    private static bool TryApplyNormalImpulse(
 470        CollisionPair pair,
 471        SolverContact3D contact,
 472        ContactNormalImpulseResult3D result)
 473    {
 602184474        if (result.LinearVelocityDeltaA == Vector3d.Zero
 602184475            && result.AngularVelocityDeltaA == Vector3d.Zero
 602184476            && result.LinearVelocityDeltaB == Vector3d.Zero
 602184477            && result.AngularVelocityDeltaB == Vector3d.Zero)
 478        {
 544871479            return true;
 480        }
 481
 57313482        if (!TryPrepareVelocityStates(
 57313483                contact,
 57313484                result.LinearVelocityDeltaA,
 57313485                result.AngularVelocityDeltaA,
 57313486                result.LinearVelocityDeltaB,
 57313487                result.AngularVelocityDeltaB,
 57313488                out Vector3d linearA,
 57313489                out Vector3d angularA,
 57313490                out Vector3d linearB,
 57313491                out Vector3d angularB))
 492        {
 2493            return false;
 494        }
 495
 57311496        if (result.HasRepresentableAppliedImpulse
 57311497            && result.HasRepresentableNormalVelocity
 57311498            && result.AppliedImpulseScalar != Fixed64.Zero)
 499        {
 57270500            Vector3d impulse =
 57270501                contact.Normal * result.AppliedImpulseScalar;
 57270502            pair.Context.Diagnostics.EmitResponseImpulse(
 57270503                pair,
 57270504                impulse,
 57270505                result.NormalVelocity);
 506        }
 57311507        ApplyVelocityStates(
 57311508            contact,
 57311509            linearA,
 57311510            angularA,
 57311511            linearB,
 57311512            angularB);
 57311513        return true;
 514    }
 515
 516    [MethodImpl(MethodImplOptions.AggressiveInlining)]
 517    private static Vector3d ResolveLinearVelocity(SolidBody? body) =>
 1478774518        body == null ? Vector3d.Zero : body.ProjectLinearMotion(
 1478774519            body.IsKinematic
 1478774520                ? body.SampleContinuousCollisionLinearVelocity(Fixed64.One)
 1478774521                : body.LinearVelocity);
 522
 523    [MethodImpl(MethodImplOptions.AggressiveInlining)]
 524    private static Vector3d ResolveAngularVelocity(SolidBody? body) =>
 1478774525        body == null ? Vector3d.Zero : body.ProjectAngularMotion(
 1478774526            body.IsKinematic
 1478774527                ? body.SampleContinuousCollisionAngularVelocity(Fixed64.One)
 1478774528                : body.AngularVelocity);
 529
 530    private static bool TryApplyExactImpulse(
 531        SolverContact3D contact,
 532        in ExactLever3D exactA,
 533        in ExactLever3D exactB,
 534        Vector3d impulseB)
 535    {
 7536        Vector3d impulseA = -impulseB;
 7537        bool linearAResolved = TryGetLinearVelocityDelta(
 7538            contact.A,
 7539            impulseA,
 7540            out Vector3d linearA);
 7541        bool angularAResolved =
 7542            ExactContactLever3D.TryGetAngularVelocityDelta(
 7543            contact.A.Body,
 7544            exactA,
 7545            impulseA,
 7546            out Vector3d angularA);
 7547        bool linearBResolved = TryGetLinearVelocityDelta(
 7548            contact.B,
 7549            impulseB,
 7550            out Vector3d linearB);
 7551        bool angularBResolved =
 7552            ExactContactLever3D.TryGetAngularVelocityDelta(
 7553            contact.B.Body,
 7554            exactB,
 7555            impulseB,
 7556            out Vector3d angularB);
 7557        if (!(linearAResolved
 7558            & angularAResolved
 7559            & linearBResolved
 7560            & angularBResolved))
 561        {
 4562            return false;
 563        }
 564
 3565        return TryApplyVelocityDeltas(
 3566            contact,
 3567            linearA,
 3568            angularA,
 3569            linearB,
 3570            angularB);
 571    }
 572
 573    private static bool TryApplyCompactImpulse(
 574        SolverContact3D contact,
 575        Vector3d impulseB)
 576    {
 36650577        Vector3d impulseA = -impulseB;
 36650578        bool linearAResolved = TryGetLinearVelocityDelta(
 36650579            contact.A,
 36650580            impulseA,
 36650581            out Vector3d linearA);
 36650582        bool angularAResolved = TryGetAngularVelocityDelta(
 36650583            contact.A,
 36650584            contact.RelativeA.Vector,
 36650585            impulseA,
 36650586            out Vector3d angularA);
 36650587        bool linearBResolved = TryGetLinearVelocityDelta(
 36650588            contact.B,
 36650589            impulseB,
 36650590            out Vector3d linearB);
 36650591        bool angularBResolved = TryGetAngularVelocityDelta(
 36650592            contact.B,
 36650593            contact.RelativeB.Vector,
 36650594            impulseB,
 36650595            out Vector3d angularB);
 36650596        if (!(linearAResolved
 36650597            & angularAResolved
 36650598            & linearBResolved
 36650599            & angularBResolved))
 600        {
 3601            return false;
 602        }
 603
 36647604        return TryApplyVelocityDeltas(
 36647605            contact,
 36647606            linearA,
 36647607            angularA,
 36647608            linearB,
 36647609            angularB);
 610    }
 611
 612    [MethodImpl(MethodImplOptions.AggressiveInlining)]
 613    private static bool CanNegate(Vector3d value) =>
 36656614        value.X != Fixed64.MinValue
 36656615        && value.Y != Fixed64.MinValue
 36656616        && value.Z != Fixed64.MinValue;
 617
 618    private static bool TryGetLinearVelocityDelta(
 619        ResponseBody3D body,
 620        Vector3d impulse,
 621        out Vector3d velocityDelta)
 622    {
 73314623        if (!body.HasSolverMobility || !body.Body!.CanTranslate)
 624        {
 35302625            velocityDelta = Vector3d.Zero;
 35302626            return true;
 627        }
 628
 38012629        return ContinuousCollisionImpulsePolicy.TryResolveVelocityDelta(
 38012630            body.Body.ProjectLinearMotion(impulse),
 38012631            Fixed64.One,
 38012632            body.InverseMass,
 38012633            Fixed64.One,
 38012634            out velocityDelta);
 635    }
 636
 637    private static bool TryGetAngularVelocityDelta(
 638        ResponseBody3D body,
 639        Vector3d relativeContactPoint,
 640        Vector3d impulse,
 641        out Vector3d velocityDelta)
 642    {
 73300643        velocityDelta = Vector3d.Zero;
 73300644        if (!body.HasSolverMobility || !body.Body!.CanRotate)
 46701645            return true;
 646
 26599647        Fixed3x3 inverseInertia =
 26599648            body.Body!.GetConstrainedInverseInertiaTensor();
 26599649        if (ContactResponseArithmetic3D.CanUseFastAngularResponse(
 26599650                relativeContactPoint,
 26599651                impulse,
 26599652                inverseInertia))
 653        {
 26594654            Vector3d fastTorqueAxis =
 26594655                Vector3d.Cross(relativeContactPoint, impulse);
 26594656            velocityDelta = Fixed3x3.TransformDirection(
 26594657                inverseInertia,
 26594658                fastTorqueAxis);
 26594659            return ContactResponseArithmetic3D
 26594660                    .PreservesNonzeroCrossProduct(
 26594661                        relativeContactPoint,
 26594662                        impulse,
 26594663                        fastTorqueAxis)
 26594664                && ContactResponseArithmetic3D
 26594665                    .PreservesNonzeroTransformDirection(
 26594666                        inverseInertia,
 26594667                        fastTorqueAxis,
 26594668                        velocityDelta);
 669        }
 670
 5671        if (!ContactResponseArithmetic3D.TryCross(
 5672                relativeContactPoint,
 5673                impulse,
 5674                out Vector3d torqueAxis))
 675        {
 1676            return false;
 677        }
 678
 4679        return ContactResponseArithmetic3D.TryTransformDirection(
 4680            inverseInertia,
 4681            torqueAxis,
 4682            out velocityDelta);
 683    }
 684
 685    private static bool TryPrepareVelocityStates(
 686        SolverContact3D contact,
 687        Vector3d linearA,
 688        Vector3d angularA,
 689        Vector3d linearB,
 690        Vector3d angularB,
 691        out Vector3d preparedLinearA,
 692        out Vector3d preparedAngularA,
 693        out Vector3d preparedLinearB,
 694        out Vector3d preparedAngularB)
 695    {
 94589696        preparedLinearA = preparedAngularA = preparedLinearB = preparedAngularB = default;
 94589697        bool firstPrepared =
 94589698            contact.A.Body == null || contact.A.Body.TryPrepareCollisionVelocityState(
 94589699                linearA,
 94589700                angularA,
 94589701                out preparedLinearA,
 94589702                out preparedAngularA);
 94589703        bool secondPrepared =
 94589704            contact.B.Body == null || contact.B.Body.TryPrepareCollisionVelocityState(
 94589705                linearB,
 94589706                angularB,
 94589707                out preparedLinearB,
 94589708                out preparedAngularB);
 94589709        return firstPrepared & secondPrepared;
 710    }
 711
 712    private static bool TryApplyVelocityDeltas(
 713        SolverContact3D contact,
 714        Vector3d linearA,
 715        Vector3d angularA,
 716        Vector3d linearB,
 717        Vector3d angularB)
 718    {
 37276719        if (!TryPrepareVelocityStates(
 37276720                contact,
 37276721                linearA,
 37276722                angularA,
 37276723                linearB,
 37276724                angularB,
 37276725                out Vector3d preparedLinearA,
 37276726                out Vector3d preparedAngularA,
 37276727                out Vector3d preparedLinearB,
 37276728                out Vector3d preparedAngularB))
 729        {
 1730            return false;
 731        }
 732
 37275733        ApplyVelocityStates(
 37275734            contact,
 37275735            preparedLinearA,
 37275736            preparedAngularA,
 37275737            preparedLinearB,
 37275738            preparedAngularB);
 37275739        return true;
 740    }
 741
 742    private static void ApplyVelocityStates(
 743        SolverContact3D contact,
 744        Vector3d linearA,
 745        Vector3d angularA,
 746        Vector3d linearB,
 747        Vector3d angularB)
 748    {
 94586749        contact.A.Body?.ApplyCollisionVelocityState(linearA, angularA);
 94586750        contact.B.Body?.ApplyCollisionVelocityState(linearB, angularB);
 87063751    }
 752
 753    private static void GetExactLevers(
 754        CollisionPair pair,
 755        SolverContact3D contact,
 756        Vector3d responsePositionA,
 757        Vector3d responsePositionB,
 758        out ExactLever3D exactA,
 759        out ExactLever3D exactB)
 760    {
 11970761        ManifoldContact manifoldContact =
 11970762            pair.Manifold.GetGroup(contact.GroupIndex)[contact.PointIndex];
 11970763        exactA = contact.A.Body == null
 11970764            ? ExactLever3D.Zero
 11970765            : manifoldContact.AnchorA.GetLeverFrom(new ContactAnchor(responsePositionA,
 11970766                contact.A.Body.Rotation, contact.A.Body.LocalCenterOfMassOffset));
 11970767        exactB = contact.B.Body == null
 11970768            ? ExactLever3D.Zero
 11970769            : manifoldContact.AnchorB.GetLeverFrom(new ContactAnchor(responsePositionB,
 11970770                contact.B.Body.Rotation, contact.B.Body.LocalCenterOfMassOffset));
 11970771    }
 772
 773    private static void RejectResponse(CollisionPair pair, SolverContact3D contact)
 774    {
 5775        ClearWarmStartImpulse(pair, contact);
 5776        GravitasLogger.Channel.Error($"Contact response is outside the representable velocity domain.");
 5777    }
 778
 779    private static void ClearWarmStartImpulse(
 780        CollisionPair pair,
 781        SolverContact3D contact) =>
 13782        pair.RemoveWarmStartImpulse(pair.Manifold.GetGroup(contact.GroupIndex).Key, contact.ContactId);
 783
 784    private static bool IsWarmStartCompatible(Vector3d cachedNormal, Vector3d normal) =>
 571036785        Vector3d.Dot(cachedNormal, normal) >= WarmStartNormalCompatibilityThreshold;
 786
 787    private static Vector3d ResolveContactNormal(Vector3d normal, Vector3d fallbackDirection) =>
 889839788        normal.MagnitudeSquared > Fixed64.Epsilon ? normal
 889839789            : fallbackDirection.MagnitudeSquared > Fixed64.Epsilon ? fallbackDirection.Normalized
 889839790            : Vector3d.Zero;
 791}

/home/runner/work/Gravitas/Gravitas/src/Gravitas/CollisionHandling/Response/3D/CollisionResponse.Friction.cs

#LineLine coverage
 1//=======================================================================
 2// CollisionResponse.Friction.cs
 3//=======================================================================
 4// MIT License, Copyright (c) 2026-present David Oravsky (mrdav30)
 5// See LICENSE file in the project root for full license information.
 6//=======================================================================
 7
 8using System.Runtime.CompilerServices;
 9using FixedMathSharp;
 10
 11namespace Gravitas.CollisionHandling;
 12
 13/// <content>Owns compact and exact two-axis Coulomb disk response.</content>
 14public static partial class CollisionResponse
 15{
 16    private static bool TrySolveFrictionImpulse(
 17        CollisionPair pair,
 18        SolverContact3D contact,
 19        Vector3d responsePositionA,
 20        Vector3d responsePositionB,
 21        Fixed64 normalImpulseScalar,
 22        out Fixed64 tangentImpulse,
 23        out Fixed64 secondaryTangentImpulse)
 24    {
 60218225        if (!contact.RelativeA.IsExact
 60218226            && !contact.RelativeB.IsExact
 60218227            && TryGetCompactFrictionResponse(
 60218228                contact,
 60218229                normalImpulseScalar,
 60218230                out tangentImpulse,
 60218231                out secondaryTangentImpulse,
 60218232                out Fixed64 tangentDelta,
 60218233                out Fixed64 secondaryTangentDelta)
 60218234            && ((tangentDelta == Fixed64.Zero
 60218235                    && secondaryTangentDelta == Fixed64.Zero)
 60218236                || TryApplyContactImpulseCombination(
 60218237                    pair,
 60218238                    contact,
 60218239                    responsePositionA,
 60218240                    responsePositionB,
 60218241                    contact.Normal,
 60218242                    Fixed64.Zero,
 60218243                    contact.Tangent,
 60218244                    tangentDelta,
 60218245                    contact.SecondaryTangent,
 60218246                    secondaryTangentDelta)))
 47        {
 60152648            return true;
 49        }
 50
 65651        return TrySolveFrictionImpulseExact(
 65652            pair,
 65653            contact,
 65654            responsePositionA,
 65655            responsePositionB,
 65656            normalImpulseScalar,
 65657            out tangentImpulse,
 65658            out secondaryTangentImpulse);
 59    }
 60
 61    private static bool TryGetCompactFrictionResponse(
 62        SolverContact3D contact,
 63        Fixed64 normalImpulseScalar,
 64        out Fixed64 tangentImpulse,
 65        out Fixed64 secondaryTangentImpulse,
 66        out Fixed64 tangentDelta,
 67        out Fixed64 secondaryTangentDelta)
 68    {
 60214469        tangentImpulse = default;
 60214470        secondaryTangentImpulse = default;
 60214471        tangentDelta = default;
 60214472        secondaryTangentDelta = default;
 60214473        bool limitsResolved = TryGetFrictionLimit(
 60214474            normalImpulseScalar,
 60214475            contact.StaticFriction,
 60214476            out Fixed64 staticFrictionLimit);
 60214477        limitsResolved &= TryGetFrictionLimit(
 60214478            normalImpulseScalar,
 60214479            contact.DynamicFriction,
 60214480            out Fixed64 dynamicFrictionLimit);
 60214481        if (!limitsResolved)
 282            return false;
 83
 60214284        if (staticFrictionLimit == Fixed64.Zero
 60214285            && dynamicFrictionLimit == Fixed64.Zero)
 86        {
 52447887            return Fixed64.TrySubtract(
 52447888                    Fixed64.Zero,
 52447889                    contact.CachedTangentImpulse,
 52447890                    out tangentDelta)
 52447891                & Fixed64.TrySubtract(
 52447892                    Fixed64.Zero,
 52447893                    contact.CachedSecondaryTangentImpulse,
 52447894                    out secondaryTangentDelta);
 95        }
 96
 7766497        Vector3d linearA = ResolveLinearVelocity(contact.A.Body);
 7766498        Vector3d angularA = ResolveAngularVelocity(contact.A.Body);
 7766499        Vector3d linearB = ResolveLinearVelocity(contact.B.Body);
 77664100        Vector3d angularB = ResolveAngularVelocity(contact.B.Body);
 77664101        if (!ContactResponseArithmetic3D.TryGetRelativePointVelocity(
 77664102                linearA,
 77664103                angularA,
 77664104                contact.RelativeA.Vector,
 77664105                linearB,
 77664106                angularB,
 77664107                contact.RelativeB.Vector,
 77664108                contact.Tangent,
 77664109                out Vector3d relativeVelocity))
 110        {
 3111            return false;
 112        }
 113
 77661114        bool deltasResolved = TryGetCompactTangentImpulseDelta(
 77661115            contact,
 77661116            relativeVelocity,
 77661117            contact.Tangent,
 77661118            out Fixed64 desiredTangentDelta);
 77661119        deltasResolved &= TryGetCompactTangentImpulseDelta(
 77661120            contact,
 77661121            relativeVelocity,
 77661122            contact.SecondaryTangent,
 77661123            out Fixed64 desiredSecondaryTangentDelta);
 77661124        bool diskResolved = Fixed64.TryAdd(
 77661125                contact.CachedTangentImpulse,
 77661126                desiredTangentDelta,
 77661127                out tangentImpulse)
 77661128            & Fixed64.TryAdd(
 77661129                contact.CachedSecondaryTangentImpulse,
 77661130                desiredSecondaryTangentDelta,
 77661131                out secondaryTangentImpulse)
 77661132            & TryGetMagnitudeSquared(
 77661133                tangentImpulse,
 77661134                secondaryTangentImpulse,
 77661135                out Fixed64 desiredMagnitudeSquared)
 77661136            & TryGetSquare(
 77661137                staticFrictionLimit,
 77661138                out Fixed64 staticLimitSquared);
 77661139        if (!(deltasResolved & diskResolved))
 140        {
 610141            return false;
 142        }
 143
 77051144        if (desiredMagnitudeSquared > staticLimitSquared)
 145        {
 431146            Fixed64 magnitude = FixedMath.Sqrt(desiredMagnitudeSquared);
 431147            Fixed64 scale = dynamicFrictionLimit / magnitude;
 431148            if (dynamicFrictionLimit != Fixed64.Zero
 431149                && scale == Fixed64.Zero)
 150            {
 1151                return false;
 152            }
 153
 430154            Fixed64 desiredTangentImpulse = tangentImpulse;
 430155            Fixed64 desiredSecondaryTangentImpulse =
 430156                secondaryTangentImpulse;
 430157            tangentImpulse *= scale;
 430158            secondaryTangentImpulse *= scale;
 430159            if (dynamicFrictionLimit != Fixed64.Zero
 430160                && ((desiredTangentImpulse != Fixed64.Zero
 430161                        && tangentImpulse == Fixed64.Zero)
 430162                    || (desiredSecondaryTangentImpulse != Fixed64.Zero
 430163                        && secondaryTangentImpulse == Fixed64.Zero)))
 164            {
 1165                return false;
 166            }
 167        }
 168
 77049169        return Fixed64.TrySubtract(
 77049170                tangentImpulse,
 77049171                contact.CachedTangentImpulse,
 77049172                out tangentDelta)
 77049173            & Fixed64.TrySubtract(
 77049174                secondaryTangentImpulse,
 77049175                contact.CachedSecondaryTangentImpulse,
 77049176                out secondaryTangentDelta);
 177    }
 178
 179    [MethodImpl(MethodImplOptions.NoInlining)]
 180    private static bool TrySolveFrictionImpulseExact(
 181        CollisionPair pair,
 182        SolverContact3D contact,
 183        Vector3d responsePositionA,
 184        Vector3d responsePositionB,
 185        Fixed64 normalImpulseScalar,
 186        out Fixed64 tangentImpulse,
 187        out Fixed64 secondaryTangentImpulse)
 188    {
 656189        GetExactLevers(
 656190            pair,
 656191            contact,
 656192            responsePositionA,
 656193            responsePositionB,
 656194            out ExactLever3D exactA,
 656195            out ExactLever3D exactB);
 656196        Vector3d linearA = ResolveLinearVelocity(contact.A.Body);
 656197        Vector3d angularA = ResolveAngularVelocity(contact.A.Body);
 656198        Vector3d linearB = ResolveLinearVelocity(contact.B.Body);
 656199        Vector3d angularB = ResolveAngularVelocity(contact.B.Body);
 656200        ExactContactResponseOperand3D primaryFirst =
 656201            ExactContactLever3D.CreateResponseOperand(
 656202                contact.A.Body,
 656203                linearA,
 656204                angularA,
 656205                exactA,
 656206                -contact.Tangent);
 656207        ExactContactResponseOperand3D primarySecond =
 656208            ExactContactLever3D.CreateResponseOperand(
 656209                contact.B.Body,
 656210                linearB,
 656211                angularB,
 656212                exactB,
 656213                contact.Tangent);
 656214        ExactContactResponseOperand3D secondaryFirst =
 656215            ExactContactLever3D.CreateResponseOperand(
 656216                contact.A.Body,
 656217                linearA,
 656218                angularA,
 656219                exactA,
 656220                -contact.SecondaryTangent);
 656221        ExactContactResponseOperand3D secondarySecond =
 656222            ExactContactLever3D.CreateResponseOperand(
 656223                contact.B.Body,
 656224                linearB,
 656225                angularB,
 656226                exactB,
 656227                contact.SecondaryTangent);
 656228        if (!ExactContactResponseKernel.TryGetCoulombDiskResponse(
 656229                contact.Normal,
 656230                normalImpulseScalar,
 656231                primaryFirst,
 656232                primarySecond,
 656233                contact.Tangent,
 656234                contact.CachedTangentImpulse,
 656235                secondaryFirst,
 656236                secondarySecond,
 656237                contact.SecondaryTangent,
 656238                contact.CachedSecondaryTangentImpulse,
 656239                contact.StaticFriction,
 656240                contact.DynamicFriction,
 656241                out ExactCoulombResponse3D response))
 242        {
 1243            tangentImpulse = default;
 1244            secondaryTangentImpulse = default;
 1245            return false;
 246        }
 247
 655248        _ = response.TryGetPrimaryAccumulatedImpulse(out tangentImpulse);
 655249        _ = response.TryGetSecondaryAccumulatedImpulse(
 655250            out secondaryTangentImpulse);
 655251        return !response.HasAppliedImpulse
 655252            || TryApplyVelocityDeltas(
 655253                contact,
 655254                response.FirstLinearVelocityDelta,
 655255                response.FirstAngularVelocityDelta,
 655256                response.SecondLinearVelocityDelta,
 655257                response.SecondAngularVelocityDelta);
 258    }
 259
 260    private static bool TryGetCompactTangentImpulseDelta(
 261        SolverContact3D contact,
 262        Vector3d relativeVelocity,
 263        Vector3d tangent,
 264        out Fixed64 impulseDelta)
 265    {
 155322266        impulseDelta = Fixed64.Zero;
 155322267        if (!ContactResponseArithmetic3D.TryDot(
 155322268                relativeVelocity,
 155322269                tangent,
 155322270                out Fixed64 tangentVelocity))
 271        {
 1272            return false;
 273        }
 274
 155321275        if (tangentVelocity >= -Fixed64.Epsilon
 155321276            && tangentVelocity <= Fixed64.Epsilon)
 277        {
 136580278            return true;
 279        }
 280
 18741281        bool denominatorsResolved =
 18741282            ContactNormalImpulse3D.TryComputeAngularDenominator(
 18741283                contact.A.Body,
 18741284                contact.RelativeA.Vector,
 18741285                tangent,
 18741286                out Fixed64 angularA);
 18741287        denominatorsResolved &=
 18741288            ContactNormalImpulse3D.TryComputeAngularDenominator(
 18741289                contact.B.Body,
 18741290                contact.RelativeB.Vector,
 18741291                tangent,
 18741292                out Fixed64 angularB);
 18741293        denominatorsResolved &= TryGetCompactConstrainedInverseMass(
 18741294            contact.A,
 18741295            tangent,
 18741296            out Fixed64 linearA);
 18741297        denominatorsResolved &= TryGetCompactConstrainedInverseMass(
 18741298            contact.B,
 18741299            tangent,
 18741300            out Fixed64 linearB);
 18741301        var denominatorTerms = new ContactEffectiveMassTerms3D(
 18741302            linearA,
 18741303            linearB,
 18741304            angularA,
 18741305            angularB);
 18741306        bool denominatorResolved = denominatorsResolved
 18741307            & denominatorTerms.TryGetValue(out Fixed64 denominator);
 18741308        if (!denominatorResolved)
 309        {
 3310            return false;
 311        }
 312
 18738313        if (denominator <= Fixed64.Epsilon)
 35314            return true;
 315
 18703316        return Fixed64.TryMultiplyDivide(
 18703317                tangentVelocity,
 18703318                -Fixed64.One,
 18703319                denominator,
 18703320                out impulseDelta)
 18703321            && impulseDelta != Fixed64.Zero;
 322    }
 323
 324    private static bool TryGetCompactConstrainedInverseMass(
 325        ResponseBody3D body,
 326        Vector3d axis,
 327        out Fixed64 inverseMass)
 328    {
 37482329        inverseMass = body.GetConstrainedInverseMass(axis);
 37482330        return inverseMass != Fixed64.Zero
 37482331            || body.InverseMass == Fixed64.Zero
 37482332            || body.Body!.ProjectLinearMotion(axis) == Vector3d.Zero;
 333    }
 334
 335    private static bool TryGetFrictionLimit(
 336        Fixed64 normalImpulse,
 337        Fixed64 friction,
 338        out Fixed64 limit)
 339    {
 1204288340        if (normalImpulse <= Fixed64.Zero || friction <= Fixed64.Zero)
 341        {
 1048957342            limit = Fixed64.Zero;
 1048957343            return true;
 344        }
 345
 155331346        return Fixed64.TryMultiplyDivide(
 155331347                normalImpulse,
 155331348                friction,
 155331349                Fixed64.One,
 155331350                out limit)
 155331351            && limit != Fixed64.Zero;
 352    }
 353
 354    private static bool TryGetMagnitudeSquared(
 355        Fixed64 first,
 356        Fixed64 second,
 357        out Fixed64 result)
 358    {
 77661359        bool resolved = TryGetSquare(first, out Fixed64 firstSquared)
 77661360            & TryGetSquare(second, out Fixed64 secondSquared);
 77661361        if (!resolved)
 362        {
 600363            result = default;
 600364            return false;
 365        }
 366
 77061367        return Fixed64.TryAdd(
 77061368            firstSquared,
 77061369            secondSquared,
 77061370            out result);
 371    }
 372
 373    private static bool TryGetSquare(Fixed64 value, out Fixed64 square) =>
 232983374        Fixed64.TryMultiplyDivide(
 232983375            value,
 232983376            value,
 232983377            Fixed64.One,
 232983378            out square)
 232983379        && (value == Fixed64.Zero || square != Fixed64.Zero);
 380
 381}

/home/runner/work/Gravitas/Gravitas/src/Gravitas/CollisionHandling/Response/3D/CollisionResponse.SupportedIsland.cs

#LineLine coverage
 1//=======================================================================
 2// CollisionResponse.SupportedIsland.cs
 3//=======================================================================
 4// MIT License, Copyright (c) 2026-present David Oravsky (mrdav30)
 5// See LICENSE file in the project root for full license information.
 6//=======================================================================
 7
 8using System;
 9using FixedMathSharp;
 10using Gravitas.Colliders;
 11
 12namespace Gravitas.CollisionHandling;
 13
 14/// <content>Seeds coupled normal loads for cuboid support.</content>
 15public static partial class CollisionResponse
 16{
 17    private static bool TrySeedFlatSupportNormals(CollisionPair pair)
 18    {
 3873619        if (!IsFlatCuboidPatch(pair))
 3844920            return false;
 21
 57422        SolidBody? a = pair.ColliderA.Body, b = pair.ColliderB.Body;
 28723        bool firstIsSupport = IsFixedIslandSupport(a);
 28724        SolidBody? body = firstIsSupport ? b : IsFixedIslandSupport(b) ? a : null;
 28725        if (body == null || !HasUnconstrainedIslandMobility(body))
 26426            return false;
 27
 2328        Vector3d normal = pair.Manifold.GetGroup(0)[0].Normal;
 2329        Vector3d outward = firstIsSupport ? normal : -normal;
 2330        if (!HasOnlyNormalMotion(body, outward)
 2331            | !Vector3d.TryDot(body.LinearVelocity, outward, out Fixed64 approach) | approach >= Fixed64.Zero)
 832            return false;
 33
 1534        Span<IslandNormal> rows = stackalloc IslandNormal[ContactManifold.MaxContactsPerGroup];
 1535        if (!TryReadIslandNormals(pair, body, null, rows) || !HasFlatNormalRows(rows, normal))
 436            return false;
 37
 1138        Span<Fixed64> speeds = stackalloc Fixed64[ContactManifold.MaxContactsPerGroup];
 1139        Span<Fixed64> impulses = stackalloc Fixed64[ContactManifold.MaxContactsPerGroup];
 1140        if (!TryPrepareIslandNormalVelocities(rows, body, null, speeds, impulses, requireRest: true,
 1141                out IslandMotion state, out _))
 242            return false;
 43
 44        // Equal depths make the first row the ordinary correction owner.
 945        ref readonly ContactResponseSnapshot snapshot = ref pair.ResponseSnapshot;
 946        _ = TryCreateContact(pair, ResponseBody3D.Create(pair.ColliderA), ResponseBody3D.Create(pair.ColliderB),
 947            snapshot.CenterA, snapshot.CenterB, 0, 0, out SolverContact3D correction);
 948        ApplyPositionCorrection(correction, Fixed64.One);
 949        StoreIslandNormals(pair, rows, speeds, impulses);
 950        body.ApplyCollisionVelocityState(state.Linear, state.Angular);
 951        return true;
 52    }
 53
 3874854    private static bool IsFlatCuboidPatch(CollisionPair pair) => pair.ColliderA.Shape == ColliderType.AABox
 3874855        & pair.ColliderB.Shape == ColliderType.AABox & pair.Manifold.GroupCount == 1
 3874856        & pair.Manifold.Count == ContactManifold.MaxContactsPerGroup;
 57
 3358    private static bool HasOnlyNormalMotion(SolidBody body, Vector3d normal) => body.AngularVelocity == Vector3d.Zero
 3359        & Vector3d.TryCross(body.LinearVelocity, normal, out Vector3d tangent) & tangent == Vector3d.Zero;
 60
 61    private static bool HasFlatNormalRows(ReadOnlySpan<IslandNormal> rows, Vector3d normal)
 62    {
 21363        foreach (IslandNormal row in rows)
 64        {
 8565            if (row.Normal != normal | row.Depth != rows[0].Depth
 8566                | row.Tangent != Fixed64.Zero | row.SecondaryTangent != Fixed64.Zero)
 167                return false;
 68        }
 2169        return true;
 70    }
 71
 72    internal static bool TrySeedSupportedIslandNormals(CollisionPair first, CollisionPair second, out bool correctedPosi
 73    {
 117374        correctedPositions = false;
 117375        if (!TryGetSupportedBodies(first, second, out SolidBody lower, out SolidBody upper)
 117376            && !TryGetSupportedBodies(second, first, out lower, out upper))
 43777            return false;
 78
 73679        Span<IslandNormal> rows = stackalloc IslandNormal[SmallContactNormalSolver.MaxRows];
 73680        int firstCount = first.Manifold.Count;
 73681        int count = firstCount + second.Manifold.Count;
 82        System.Diagnostics.Debug.Assert(count <= rows.Length);
 73683        if (!(TryReadIslandNormals(first, lower, upper, rows[..firstCount])
 73684            & TryReadIslandNormals(second, lower, upper, rows[firstCount..count])))
 24385            return false;
 86
 49387        bool requireRest = upper.Mass <= lower.Mass;
 49388        if (requireRest)
 89        {
 590            bool floorFirst = rows[0].Upper.Linear == Vector3d.Zero;
 591            Vector3d supportAxis = (floorFirst ? rows[0] : rows[firstCount]).Lower.Linear;
 592            Vector3d loadAxis = (floorFirst ? rows[firstCount] : rows[0]).Upper.Linear;
 593            if (!(supportAxis == loadAxis & HasOnlyNormalMotion(lower, supportAxis) & HasOnlyNormalMotion(upper, loadAxi
 594                & HasFlatNormalRows(rows[..firstCount], rows[0].Normal)
 595                & HasFlatNormalRows(rows[firstCount..count], rows[firstCount].Normal)))
 196                return false;
 97        }
 98
 49299        Span<Fixed64> speeds = stackalloc Fixed64[SmallContactNormalSolver.MaxRows];
 492100        Span<Fixed64> impulses = stackalloc Fixed64[SmallContactNormalSolver.MaxRows];
 492101        if (!TryPrepareIslandNormalVelocities(rows[..count], lower, upper, speeds[..count], impulses[..count], requireRe
 492102                out IslandMotion lowerState, out IslandMotion upperState))
 4103            return false;
 104
 488105        correctedPositions = TryPrepareIslandPositions(rows[..firstCount], rows[firstCount..count], lower, upper,
 488106            out Vector3d lowerPosition, out Vector3d upperPosition);
 488107        if (requireRest & !correctedPositions)
 1108            return false;
 109
 110        // Commit only after both endpoints and every normal impulse were prepared.
 111        // The subsequent row sweep refines friction against the new normal loads.
 487112        if (correctedPositions)
 113        {
 485114            lower.ApplyCollisionPositionState(lowerPosition);
 485115            upper.ApplyCollisionPositionState(upperPosition);
 116        }
 487117        StoreIslandNormals(first, rows[..firstCount], speeds[..firstCount], impulses[..firstCount]);
 487118        StoreIslandNormals(second, rows[firstCount..count], speeds[firstCount..count], impulses[firstCount..count]);
 487119        lower.ApplyCollisionVelocityState(lowerState.Linear, lowerState.Angular);
 487120        upper.ApplyCollisionVelocityState(upperState.Linear, upperState.Angular);
 487121        return true;
 122    }
 123
 124    private static bool TryPrepareIslandNormalVelocities(Span<IslandNormal> rows, SolidBody lower,
 125        SolidBody? upper, Span<Fixed64> speeds, Span<Fixed64> impulses, bool requireRest,
 126        out IslandMotion lowerState, out IslandMotion upperState)
 127    {
 503128        int count = rows.Length;
 503129        Span<Fixed64> matrix = stackalloc Fixed64[count * count];
 503130        lowerState = upperState = default;
 503131        var lowerMobility = new IslandMobility(lower);
 503132        var upperMobility = upper == null ? default : new IslandMobility(upper);
 503133        bool preparedSystem = TryPrepareIslandFriction(rows, lowerMobility, upperMobility,
 503134            out IslandMotion lowerFriction, out IslandMotion upperFriction);
 3858135        for (int i = 0; i < count; i++)
 136        {
 1426137            ref IslandNormal row = ref rows[i];
 1426138            bool prepared = row.Lower.TryDot(lowerFriction, out Fixed64 lowerSpeed)
 1426139                & row.Upper.TryDot(upperFriction, out Fixed64 upperSpeed)
 1426140                & lowerMobility.TryTransform(row.Lower, out row.LowerResponse)
 1426141                & upperMobility.TryTransform(row.Upper, out row.UpperResponse);
 1426142            prepared &= Fixed64.TryAdd(lowerSpeed, upperSpeed, out Fixed64 warmSpeed);
 1426143            preparedSystem &= prepared & Fixed64.TryAdd(row.Speed, warmSpeed, out speeds[i]);
 10392144            for (int j = 0; j <= i; j++)
 145            {
 3770146                bool terms = rows[j].Lower.TryDot(row.LowerResponse, out Fixed64 low)
 3770147                    & rows[j].Upper.TryDot(row.UpperResponse, out Fixed64 high);
 3770148                preparedSystem &= terms & Fixed64.TryAdd(low, high, out Fixed64 coefficient);
 3770149                matrix[i * count + j] = matrix[j * count + i] = coefficient;
 150            }
 151        }
 152
 503153        if (!preparedSystem)
 1154            return false;
 502155        if (!SmallContactNormalSolver.TrySolve(count, matrix, speeds, impulses))
 2156            return false;
 500157        Fixed64 deadband = lower.Context.Environment.MinSpeed;
 500158        if (requireRest)
 14159            deadband = FixedMath.Min(deadband, PhysicsEnvironment.DefaultMinSpeed);
 500160        bool preparedVelocities = TryPrepareIslandVelocity(lower, rows, impulses, lowerFriction,
 500161                isLower: true, deadband, out lowerState)
 500162            & (upper == null || TryPrepareIslandVelocity(upper, rows, impulses, upperFriction,
 500163                isLower: false, deadband, out upperState));
 500164        if (requireRest && (lowerState.Linear != Vector3d.Zero | lowerState.Angular != Vector3d.Zero
 500165            | upperState.Linear != Vector3d.Zero | upperState.Angular != Vector3d.Zero))
 1166            return false;
 499167        return preparedVelocities && ArePreparedIslandNormalsFeasible(rows, impulses, lowerState, upperState);
 168    }
 169
 170    private static bool TryGetSupportedBodies(CollisionPair support, CollisionPair load,
 171        out SolidBody lower, out SolidBody upper)
 172    {
 1613173        lower = null!;
 1613174        upper = null!;
 1613175        if (support.CollisionType != CollisionType.Cuboid_Cuboid | load.CollisionType != CollisionType.Cuboid_Cuboid
 1613176            | support.Manifold.GroupCount != 1 | load.Manifold.GroupCount != 1)
 854177            return false;
 178
 759179        SolidBody? a = support.ColliderA.Body;
 759180        SolidBody? b = support.ColliderB.Body;
 759181        SolidBody? supported = IsFixedIslandSupport(a) ? b : IsFixedIslandSupport(b) ? a : null;
 759182        if (supported == null || !HasUnconstrainedIslandMobility(supported))
 19183            return false;
 184        // Two contacts in this connected island must share the only moving support endpoint.
 185        System.Diagnostics.Debug.Assert(load.ColliderA.Body == supported || load.ColliderB.Body == supported);
 740186        SolidBody? loaded = load.ColliderA.Body == supported ? load.ColliderB.Body : load.ColliderA.Body;
 740187        if (loaded == null || !HasUnconstrainedIslandMobility(loaded))
 3188            return false;
 737189        if (loaded.Mass <= supported.Mass && !(IsFlatCuboidPatch(support) & IsFlatCuboidPatch(load)))
 1190            return false;
 736191        lower = supported;
 736192        upper = loaded;
 736193        return true;
 194    }
 195
 1261196    private static bool IsFixedIslandSupport(SolidBody? body) => body == null || body.IsStatic && body.IsAtRest;
 197
 1771198    private static bool HasUnconstrainedIslandMobility(SolidBody body) => body.IsDynamic
 1771199        && body.FreezeAxes == BodyFreezeAxes3D.None && body.CanTranslate && body.CanRotate;
 200
 201    private static bool TryReadIslandNormals(CollisionPair pair, SolidBody lower, SolidBody? upper, Span<IslandNormal> r
 202    {
 1487203        if (!pair.ResponseSnapshot.IsValid)
 1204            return false;
 1486205        ResponseBody3D a = ResponseBody3D.Create(pair.ColliderA);
 1486206        ResponseBody3D b = ResponseBody3D.Create(pair.ColliderB);
 1486207        ref readonly ContactResponseSnapshot snapshot = ref pair.ResponseSnapshot;
 4458208        bool lowerIsA = a.Body == lower, upperIsA = upper != null & a.Body == upper, upperIsB = upper != null & b.Body =
 5852209        for (int i = 0; i < rows.Length; i++)
 210        {
 1926211            bool contactPrepared = TryCreateContact(pair, a, b, snapshot.CenterA, snapshot.CenterB, 0, i,
 1926212                out SolverContact3D contact, requireMatchingTangentBasis: true);
 1926213            if (!(contactPrepared & !contact.RelativeA.IsExact & !contact.RelativeB.IsExact & (contact.Restitution == Fi
 485214                return false;
 1441215            bool prepared = ContactNormalImpulse3D.TryComputeNormalVelocity(snapshot.LinearA, snapshot.AngularA,
 1441216                    contact.RelativeA.Vector, snapshot.LinearB, snapshot.AngularB,
 1441217                    contact.RelativeB.Vector, contact.Normal, out Fixed64 speed)
 1441218                & ContactResponseArithmetic3D.TryCross(contact.RelativeA.Vector, -contact.Normal, out Vector3d torqueA)
 1441219                & ContactResponseArithmetic3D.TryCross(contact.RelativeB.Vector, contact.Normal, out Vector3d torqueB);
 1441220            var first = new IslandMotion(-contact.Normal, torqueA);
 1441221            var second = new IslandMotion(contact.Normal, torqueB);
 1441222            if (!(prepared & TryReadIslandFriction(contact, out IslandMotion firstFriction, out IslandMotion secondFrict
 1223                return false;
 1440224            rows[i] = new IslandNormal
 1440225            {
 1440226                Lower = lowerIsA ? first : second,
 1440227                Upper = upperIsA ? first : upperIsB ? second : default,
 1440228                LowerFriction = lowerIsA ? firstFriction : secondFriction,
 1440229                UpperFriction = upperIsA ? firstFriction : upperIsB ? secondFriction : default,
 1440230                Tangent = contact.CachedTangentImpulse,
 1440231                SecondaryTangent = contact.CachedSecondaryTangentImpulse,
 1440232                Speed = speed,
 1440233                Normal = contact.Normal,
 1440234                Depth = contact.Depth
 1440235            };
 236        }
 1000237        return true;
 238    }
 239
 240    private static bool TryReadIslandFriction(SolverContact3D contact, out IslandMotion first, out IslandMotion second)
 241    {
 1441242        first = second = default;
 1441243        if (contact.CachedTangentImpulse == Fixed64.Zero && contact.CachedSecondaryTangentImpulse == Fixed64.Zero)
 1436244            return true;
 5245        bool prepared = ContactResponseArithmetic3D.TryLinearCombination(contact.Tangent, contact.CachedTangentImpulse,
 5246            contact.SecondaryTangent, contact.CachedSecondaryTangentImpulse, Vector3d.Zero, Fixed64.Zero, out Vector3d f
 5247        prepared &= Vector3d.TrySubtract(Vector3d.Zero, force, out Vector3d opposite);
 5248        prepared &= ContactResponseArithmetic3D.TryCross(contact.RelativeA.Vector, opposite, out Vector3d firstTorque)
 5249            & ContactResponseArithmetic3D.TryCross(contact.RelativeB.Vector, force, out Vector3d secondTorque);
 5250        first = new IslandMotion(opposite, firstTorque);
 5251        second = new IslandMotion(force, secondTorque);
 5252        return prepared;
 253    }
 254
 255    private static bool TryPrepareIslandFriction(ReadOnlySpan<IslandNormal> rows,
 256        IslandMobility lowerMobility, IslandMobility upperMobility, out IslandMotion lower, out IslandMotion upper)
 257    {
 503258        lower = upper = default;
 1006259        IslandMotion lowerImpulse = default, upperImpulse = default;
 3855260        foreach (IslandNormal row in rows)
 261        {
 1425262            if (!(lowerImpulse.TryAdd(row.LowerFriction, out lowerImpulse)
 1425263                & upperImpulse.TryAdd(row.UpperFriction, out upperImpulse)))
 1264                return false;
 265        }
 502266        return lowerMobility.TryTransform(lowerImpulse, out lower) & upperMobility.TryTransform(upperImpulse, out upper)
 267    }
 268
 269    private static bool TryPrepareIslandVelocity(SolidBody body, ReadOnlySpan<IslandNormal> rows,
 270        ReadOnlySpan<Fixed64> impulses, IslandMotion friction, bool isLower, Fixed64 deadband, out IslandMotion state)
 271    {
 990272        state = default;
 1980273        Vector3d linear = friction.Linear, angular = friction.Angular;
 990274        bool prepared = true;
 7568275        for (int i = 0; i < rows.Length; i++)
 276        {
 2794277            IslandMotion response = isLower ? rows[i].LowerResponse : rows[i].UpperResponse;
 2794278            bool scaled = ContactResponseArithmetic3D.TryScale(response.Linear, impulses[i], out Vector3d linearDelta)
 2794279                & ContactResponseArithmetic3D.TryScale(response.Angular, impulses[i], out Vector3d angularDelta);
 2794280            prepared &= scaled & Vector3d.TryAdd(linear, linearDelta, out linear)
 2794281                & Vector3d.TryAdd(angular, angularDelta, out angular);
 282        }
 990283        prepared &= body.TryPrepareCollisionVelocityState(linear, angular, out linear, out angular);
 990284        PhysicsEnvironment environment = body.Context.Environment;
 1980285        Fixed64 linearSpeed = linear.Magnitude, angularSpeed = angular.Magnitude;
 990286        state = new IslandMotion(linearSpeed <= deadband ? Vector3d.Zero : linear,
 990287            angularSpeed <= deadband ? Vector3d.Zero : angular);
 990288        return prepared & (linearSpeed <= environment.MaxSpeed) & (angularSpeed <= environment.MaxSpeed);
 289    }
 290
 291    private static bool ArePreparedIslandNormalsFeasible(ReadOnlySpan<IslandNormal> rows, ReadOnlySpan<Fixed64> impulses
 292        IslandMotion lowerState, IslandMotion upperState)
 293    {
 294        // Materializing the body deltas adds rounding beyond the matrix solve.
 295        // Use the ordinary motion deadband, independent of a host's configured limit.
 498296        Fixed64 tolerance = PhysicsEnvironment.DefaultMinSpeed;
 3816297        for (int i = 0; i < rows.Length; i++)
 298        {
 1411299            bool prepared = rows[i].Lower.TryGetPreparedSpeed(lowerState, out Fixed64 lowerSpeed)
 1411300                & rows[i].Upper.TryGetPreparedSpeed(upperState, out Fixed64 upperSpeed);
 1411301            prepared &= Fixed64.TryAdd(lowerSpeed, upperSpeed, out Fixed64 speed);
 1411302            if (!(prepared & (speed >= -tolerance) & ((impulses[i] == Fixed64.Zero) | (speed <= tolerance))))
 1303                return false;
 304        }
 497305        return true;
 306    }
 307
 308    private static bool TryPrepareIslandPositions(ReadOnlySpan<IslandNormal> first, ReadOnlySpan<IslandNormal> second,
 309        SolidBody lower, SolidBody upper, out Vector3d lowerPosition, out Vector3d upperPosition)
 310    {
 488311        lowerPosition = upperPosition = default;
 488312        if (!(TryGetTranslationDepth(first, out Fixed64 firstDepth) & TryGetTranslationDepth(second, out Fixed64 secondD
 1313            return false;
 314
 315        // Project one depth per interface, without using angular inertia for a translation-only correction.
 487316        Span<Vector3d> lowerAxes = stackalloc Vector3d[] { first[0].Lower.Linear, second[0].Lower.Linear };
 487317        Span<Vector3d> upperAxes = stackalloc Vector3d[] { first[0].Upper.Linear, second[0].Upper.Linear };
 487318        Span<Fixed64> depths = stackalloc Fixed64[] { PenetrationSlop - firstDepth, PenetrationSlop - secondDepth };
 487319        Span<Fixed64> impulses = stackalloc Fixed64[2];
 487320        impulses.Clear();
 487321        bool prepared = SmallContactNormalSolver.TrySolveTranslations(lowerAxes, upperAxes,
 487322            lower.EffectiveInverseMass, upper.EffectiveInverseMass, depths, impulses);
 487323        return prepared & TryPrepareIslandTranslation(lower, lowerAxes, impulses, out lowerPosition)
 487324            & TryPrepareIslandTranslation(upper, upperAxes, impulses, out upperPosition);
 325    }
 326
 327    private static bool TryGetTranslationDepth(ReadOnlySpan<IslandNormal> rows, out Fixed64 depth)
 328    {
 976329        depth = Fixed64.Zero;
 4697330        foreach (IslandNormal row in rows)
 331        {
 1373332            if (row.Normal != rows[0].Normal)
 1333                return false;
 1372334            depth = FixedMath.Max(depth, row.Depth);
 335        }
 975336        return true;
 337    }
 338
 339    private static bool TryPrepareIslandTranslation(SolidBody body, ReadOnlySpan<Vector3d> axes,
 340        ReadOnlySpan<Fixed64> impulses, out Vector3d position)
 341    {
 974342        position = default;
 974343        Fixed64 inverseMass = PenetrationCorrectionPercent * body.EffectiveInverseMass;
 974344        bool prepared = ContactResponseArithmetic3D.TryLinearCombination(axes[0], impulses[0], axes[1], impulses[1],
 974345            Vector3d.Zero, Fixed64.Zero, out Vector3d translation);
 974346        prepared &= ContactResponseArithmetic3D.TryScale(translation, inverseMass, out translation);
 974347        return prepared & (inverseMass > Fixed64.Zero) & Vector3d.TryAdd(body.Position3d, translation, out position);
 348    }
 349
 350    private static void StoreIslandNormals(CollisionPair pair, ReadOnlySpan<IslandNormal> rows,
 351        ReadOnlySpan<Fixed64> speeds, ReadOnlySpan<Fixed64> impulses)
 352    {
 983353        ref ContactGroup group = ref pair.Manifold.GetGroup(0);
 983354        var diagnostics = pair.Context.Diagnostics;
 4768355        for (int i = 0; i < rows.Length; i++)
 356        {
 1401357            pair.StoreWarmStartImpulse(group.Key, group[i].ContactId, rows[i].Normal, impulses[i],
 1401358                rows[i].Tangent, rows[i].SecondaryTangent);
 1401359            if (diagnostics.Enabled && impulses[i] != Fixed64.Zero)
 4360                diagnostics.EmitResponseImpulse(pair, rows[i].Normal * impulses[i], speeds[i], group[i]);
 361        }
 983362    }
 363
 364    private struct IslandNormal
 365    {
 366        internal IslandMotion Lower, Upper, LowerResponse, UpperResponse, LowerFriction, UpperFriction;
 367        internal Vector3d Normal;
 368        internal Fixed64 Speed, Depth, Tangent, SecondaryTangent;
 369    }
 370
 371    private readonly struct IslandMotion
 372    {
 373        internal IslandMotion(Vector3d linear, Vector3d angular)
 374        {
 10587375            Linear = linear;
 10587376            Angular = angular;
 10587377        }
 378
 379        internal readonly Vector3d Linear, Angular;
 380
 381        internal bool TryAdd(IslandMotion other, out IslandMotion result)
 382        {
 2850383            if (!(Vector3d.TryAdd(Linear, other.Linear, out Vector3d linear)
 2850384                & Vector3d.TryAdd(Angular, other.Angular, out Vector3d angular)))
 385            {
 1386                result = default;
 1387                return false;
 388            }
 2849389            result = new IslandMotion(linear, angular);
 2849390            return true;
 391        }
 392
 393        internal bool TryDot(IslandMotion other, out Fixed64 result)
 394        {
 10392395            result = default;
 10392396            return ContactResponseArithmetic3D.TryDot(Linear, other.Linear, out Fixed64 linear)
 10392397                & ContactResponseArithmetic3D.TryDot(Angular, other.Angular, out Fixed64 angular)
 10392398                & Fixed64.TryAdd(linear, angular, out result);
 399        }
 400
 401        internal bool TryGetPreparedSpeed(IslandMotion other, out Fixed64 result)
 402        {
 2822403            result = default;
 404            // These are already-materialized velocities. Rounding a sub-LSB term
 405            // to zero here cannot amplify it through a subsequent impulse scale.
 2822406            return Vector3d.TryDot(Linear, other.Linear, out Fixed64 linear)
 2822407                & Vector3d.TryDot(Angular, other.Angular, out Fixed64 angular)
 2822408                & Fixed64.TryAdd(linear, angular, out result);
 409        }
 410    }
 411
 412    private readonly struct IslandMobility
 413    {
 414        private readonly Fixed64 _inverseMass;
 415        private readonly Fixed3x3 _inverseInertia;
 416
 417        internal IslandMobility(SolidBody body)
 418        {
 995419            _inverseMass = body.EffectiveInverseMass;
 995420            _inverseInertia = body.GetConstrainedInverseInertiaTensor();
 995421        }
 422
 423        internal bool TryTransform(IslandMotion jacobian, out IslandMotion response)
 424        {
 3856425            bool prepared = ContactResponseArithmetic3D.TryScale(jacobian.Linear, _inverseMass, out Vector3d linear)
 3856426                & ContactResponseArithmetic3D.TryTransformDirection(_inverseInertia, jacobian.Angular, out Vector3d angu
 3856427            response = new IslandMotion(linear, angular);
 3856428            return prepared;
 429        }
 430    }
 431}

Methods/Properties

.cctor()
CalculateImpulse(Gravitas.CollisionHandling.CollisionPair)
PrepareSolve(Gravitas.CollisionHandling.CollisionPair)
CalculateImpulse(Gravitas.CollisionHandling.CollisionPair,System.Boolean,System.Boolean)
IsCorrectionOwner(Gravitas.CollisionHandling.CollisionPair,Gravitas.CollisionHandling.SolverContact3D)
TryCreateBodyPair(Gravitas.CollisionHandling.CollisionPair,Gravitas.CollisionHandling.ResponseBody3D&,Gravitas.CollisionHandling.ResponseBody3D&)
TryCreateContact(Gravitas.CollisionHandling.CollisionPair,Gravitas.CollisionHandling.ResponseBody3D,Gravitas.CollisionHandling.ResponseBody3D,Gravitas.CollisionHandling.ContactAnchor&,Gravitas.CollisionHandling.ContactAnchor&,System.Int32,System.Int32,Gravitas.CollisionHandling.SolverContact3D&,System.Boolean)
TryCalculateExactNormalResult(Gravitas.CollisionHandling.CollisionPair,Gravitas.CollisionHandling.SolverContact3D,FixedMathSharp.Vector3d,FixedMathSharp.Vector3d,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,Gravitas.CollisionHandling.ContactNormalImpulseResult3D&)
ApplyPositionCorrection(Gravitas.CollisionHandling.SolverContact3D,FixedMathSharp.Fixed64)
TryApplyCachedImpulse(Gravitas.CollisionHandling.CollisionPair,Gravitas.CollisionHandling.SolverContact3D,FixedMathSharp.Vector3d,FixedMathSharp.Vector3d)
TryApplyContactImpulseCombination(Gravitas.CollisionHandling.CollisionPair,Gravitas.CollisionHandling.SolverContact3D,FixedMathSharp.Vector3d,FixedMathSharp.Vector3d,FixedMathSharp.Vector3d,FixedMathSharp.Fixed64,FixedMathSharp.Vector3d,FixedMathSharp.Fixed64,FixedMathSharp.Vector3d,FixedMathSharp.Fixed64)
TryApplyContactImpulse(Gravitas.CollisionHandling.CollisionPair,Gravitas.CollisionHandling.SolverContact3D,FixedMathSharp.Vector3d,FixedMathSharp.Vector3d,FixedMathSharp.Vector3d)
TryApplyContactImpulseExact(Gravitas.CollisionHandling.CollisionPair,Gravitas.CollisionHandling.SolverContact3D,FixedMathSharp.Vector3d,FixedMathSharp.Vector3d,FixedMathSharp.Vector3d)
TryApplyContactImpulseCombinationExact(Gravitas.CollisionHandling.CollisionPair,Gravitas.CollisionHandling.SolverContact3D,FixedMathSharp.Vector3d,FixedMathSharp.Vector3d,FixedMathSharp.Vector3d,FixedMathSharp.Fixed64,FixedMathSharp.Vector3d,FixedMathSharp.Fixed64,FixedMathSharp.Vector3d,FixedMathSharp.Fixed64)
TryApplyNormalImpulse(Gravitas.CollisionHandling.CollisionPair,Gravitas.CollisionHandling.SolverContact3D,Gravitas.CollisionHandling.ContactNormalImpulseResult3D)
ResolveLinearVelocity(Gravitas.SolidBody)
ResolveAngularVelocity(Gravitas.SolidBody)
TryApplyExactImpulse(Gravitas.CollisionHandling.SolverContact3D,Gravitas.CollisionHandling.ExactLever3D&,Gravitas.CollisionHandling.ExactLever3D&,FixedMathSharp.Vector3d)
TryApplyCompactImpulse(Gravitas.CollisionHandling.SolverContact3D,FixedMathSharp.Vector3d)
CanNegate(FixedMathSharp.Vector3d)
TryGetLinearVelocityDelta(Gravitas.CollisionHandling.ResponseBody3D,FixedMathSharp.Vector3d,FixedMathSharp.Vector3d&)
TryGetAngularVelocityDelta(Gravitas.CollisionHandling.ResponseBody3D,FixedMathSharp.Vector3d,FixedMathSharp.Vector3d,FixedMathSharp.Vector3d&)
TryPrepareVelocityStates(Gravitas.CollisionHandling.SolverContact3D,FixedMathSharp.Vector3d,FixedMathSharp.Vector3d,FixedMathSharp.Vector3d,FixedMathSharp.Vector3d,FixedMathSharp.Vector3d&,FixedMathSharp.Vector3d&,FixedMathSharp.Vector3d&,FixedMathSharp.Vector3d&)
TryApplyVelocityDeltas(Gravitas.CollisionHandling.SolverContact3D,FixedMathSharp.Vector3d,FixedMathSharp.Vector3d,FixedMathSharp.Vector3d,FixedMathSharp.Vector3d)
ApplyVelocityStates(Gravitas.CollisionHandling.SolverContact3D,FixedMathSharp.Vector3d,FixedMathSharp.Vector3d,FixedMathSharp.Vector3d,FixedMathSharp.Vector3d)
GetExactLevers(Gravitas.CollisionHandling.CollisionPair,Gravitas.CollisionHandling.SolverContact3D,FixedMathSharp.Vector3d,FixedMathSharp.Vector3d,Gravitas.CollisionHandling.ExactLever3D&,Gravitas.CollisionHandling.ExactLever3D&)
RejectResponse(Gravitas.CollisionHandling.CollisionPair,Gravitas.CollisionHandling.SolverContact3D)
ClearWarmStartImpulse(Gravitas.CollisionHandling.CollisionPair,Gravitas.CollisionHandling.SolverContact3D)
IsWarmStartCompatible(FixedMathSharp.Vector3d,FixedMathSharp.Vector3d)
ResolveContactNormal(FixedMathSharp.Vector3d,FixedMathSharp.Vector3d)
TrySolveFrictionImpulse(Gravitas.CollisionHandling.CollisionPair,Gravitas.CollisionHandling.SolverContact3D,FixedMathSharp.Vector3d,FixedMathSharp.Vector3d,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64&,FixedMathSharp.Fixed64&)
TryGetCompactFrictionResponse(Gravitas.CollisionHandling.SolverContact3D,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64&,FixedMathSharp.Fixed64&,FixedMathSharp.Fixed64&,FixedMathSharp.Fixed64&)
TrySolveFrictionImpulseExact(Gravitas.CollisionHandling.CollisionPair,Gravitas.CollisionHandling.SolverContact3D,FixedMathSharp.Vector3d,FixedMathSharp.Vector3d,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64&,FixedMathSharp.Fixed64&)
TryGetCompactTangentImpulseDelta(Gravitas.CollisionHandling.SolverContact3D,FixedMathSharp.Vector3d,FixedMathSharp.Vector3d,FixedMathSharp.Fixed64&)
TryGetCompactConstrainedInverseMass(Gravitas.CollisionHandling.ResponseBody3D,FixedMathSharp.Vector3d,FixedMathSharp.Fixed64&)
TryGetFrictionLimit(FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64&)
TryGetMagnitudeSquared(FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64&)
TryGetSquare(FixedMathSharp.Fixed64,FixedMathSharp.Fixed64&)
TrySeedFlatSupportNormals(Gravitas.CollisionHandling.CollisionPair)
IsFlatCuboidPatch(Gravitas.CollisionHandling.CollisionPair)
HasOnlyNormalMotion(Gravitas.SolidBody,FixedMathSharp.Vector3d)
HasFlatNormalRows(System.ReadOnlySpan`1<Gravitas.CollisionHandling.CollisionResponse/IslandNormal>,FixedMathSharp.Vector3d)
TrySeedSupportedIslandNormals(Gravitas.CollisionHandling.CollisionPair,Gravitas.CollisionHandling.CollisionPair,System.Boolean&)
TryPrepareIslandNormalVelocities(System.Span`1<Gravitas.CollisionHandling.CollisionResponse/IslandNormal>,Gravitas.SolidBody,Gravitas.SolidBody,System.Span`1<FixedMathSharp.Fixed64>,System.Span`1<FixedMathSharp.Fixed64>,System.Boolean,Gravitas.CollisionHandling.CollisionResponse/IslandMotion&,Gravitas.CollisionHandling.CollisionResponse/IslandMotion&)
TryGetSupportedBodies(Gravitas.CollisionHandling.CollisionPair,Gravitas.CollisionHandling.CollisionPair,Gravitas.SolidBody&,Gravitas.SolidBody&)
IsFixedIslandSupport(Gravitas.SolidBody)
HasUnconstrainedIslandMobility(Gravitas.SolidBody)
TryReadIslandNormals(Gravitas.CollisionHandling.CollisionPair,Gravitas.SolidBody,Gravitas.SolidBody,System.Span`1<Gravitas.CollisionHandling.CollisionResponse/IslandNormal>)
TryReadIslandFriction(Gravitas.CollisionHandling.SolverContact3D,Gravitas.CollisionHandling.CollisionResponse/IslandMotion&,Gravitas.CollisionHandling.CollisionResponse/IslandMotion&)
TryPrepareIslandFriction(System.ReadOnlySpan`1<Gravitas.CollisionHandling.CollisionResponse/IslandNormal>,Gravitas.CollisionHandling.CollisionResponse/IslandMobility,Gravitas.CollisionHandling.CollisionResponse/IslandMobility,Gravitas.CollisionHandling.CollisionResponse/IslandMotion&,Gravitas.CollisionHandling.CollisionResponse/IslandMotion&)
TryPrepareIslandVelocity(Gravitas.SolidBody,System.ReadOnlySpan`1<Gravitas.CollisionHandling.CollisionResponse/IslandNormal>,System.ReadOnlySpan`1<FixedMathSharp.Fixed64>,Gravitas.CollisionHandling.CollisionResponse/IslandMotion,System.Boolean,FixedMathSharp.Fixed64,Gravitas.CollisionHandling.CollisionResponse/IslandMotion&)
ArePreparedIslandNormalsFeasible(System.ReadOnlySpan`1<Gravitas.CollisionHandling.CollisionResponse/IslandNormal>,System.ReadOnlySpan`1<FixedMathSharp.Fixed64>,Gravitas.CollisionHandling.CollisionResponse/IslandMotion,Gravitas.CollisionHandling.CollisionResponse/IslandMotion)
TryPrepareIslandPositions(System.ReadOnlySpan`1<Gravitas.CollisionHandling.CollisionResponse/IslandNormal>,System.ReadOnlySpan`1<Gravitas.CollisionHandling.CollisionResponse/IslandNormal>,Gravitas.SolidBody,Gravitas.SolidBody,FixedMathSharp.Vector3d&,FixedMathSharp.Vector3d&)
TryGetTranslationDepth(System.ReadOnlySpan`1<Gravitas.CollisionHandling.CollisionResponse/IslandNormal>,FixedMathSharp.Fixed64&)
TryPrepareIslandTranslation(Gravitas.SolidBody,System.ReadOnlySpan`1<FixedMathSharp.Vector3d>,System.ReadOnlySpan`1<FixedMathSharp.Fixed64>,FixedMathSharp.Vector3d&)
StoreIslandNormals(Gravitas.CollisionHandling.CollisionPair,System.ReadOnlySpan`1<Gravitas.CollisionHandling.CollisionResponse/IslandNormal>,System.ReadOnlySpan`1<FixedMathSharp.Fixed64>,System.ReadOnlySpan`1<FixedMathSharp.Fixed64>)
.ctor(FixedMathSharp.Vector3d,FixedMathSharp.Vector3d)
TryAdd(Gravitas.CollisionHandling.CollisionResponse/IslandMotion,Gravitas.CollisionHandling.CollisionResponse/IslandMotion&)
TryDot(Gravitas.CollisionHandling.CollisionResponse/IslandMotion,FixedMathSharp.Fixed64&)
TryGetPreparedSpeed(Gravitas.CollisionHandling.CollisionResponse/IslandMotion,FixedMathSharp.Fixed64&)
.ctor(Gravitas.SolidBody)
TryTransform(Gravitas.CollisionHandling.CollisionResponse/IslandMotion,Gravitas.CollisionHandling.CollisionResponse/IslandMotion&)