< Summary

Line coverage
100%
Covered lines: 523
Uncovered lines: 0
Coverable lines: 523
Total lines: 838
Line coverage: 100%
Branch coverage
100%
Covered branches: 124
Total branches: 124
Branch coverage: 100%
Method coverage

Feature is only available for sponsors

Upgrade to PRO version

Metrics

File(s)

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

#LineLine coverage
 1//=======================================================================
 2// ContactNormalImpulse3D.Accumulation.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 FixedMathSharp;
 9
 10namespace Gravitas.CollisionHandling;
 11
 12/// <content>Owns accumulated unilateral impulses and frozen incoming restitution targets.</content>
 13internal static partial class ContactNormalImpulse3D
 14{
 15    internal static ContactNormalImpulseResult3D CalculateAccumulatedDelta(
 16        SolidBody? bodyA,
 17        Vector3d linearVelocityA,
 18        Vector3d angularVelocityA,
 19        Vector3d relativeContactPointA,
 20        SolidBody? bodyB,
 21        Vector3d linearVelocityB,
 22        Vector3d angularVelocityB,
 23        Vector3d relativeContactPointB,
 24        Vector3d normal,
 25        Fixed64 restitution,
 26        Fixed64 restitutionVelocityThreshold,
 27        Fixed64 accumulatedImpulse,
 28        Fixed64 positiveImpulseScale,
 29        Fixed64 negativeImpulseScale)
 30    {
 131        _ = TryCalculateAccumulatedDelta(
 132            bodyA,
 133            linearVelocityA,
 134            angularVelocityA,
 135            relativeContactPointA,
 136            bodyB,
 137            linearVelocityB,
 138            angularVelocityB,
 139            relativeContactPointB,
 140            normal,
 141            restitution,
 142            restitutionVelocityThreshold,
 143            accumulatedImpulse,
 144            positiveImpulseScale,
 145            negativeImpulseScale,
 146            out ContactNormalImpulseResult3D result);
 147        return result;
 48    }
 49
 50    internal static bool TryCalculateAccumulatedDelta(
 51        SolidBody? bodyA,
 52        Vector3d linearVelocityA,
 53        Vector3d angularVelocityA,
 54        Vector3d relativeContactPointA,
 55        SolidBody? bodyB,
 56        Vector3d linearVelocityB,
 57        Vector3d angularVelocityB,
 58        Vector3d relativeContactPointB,
 59        Vector3d normal,
 60        Fixed64 restitution,
 61        Fixed64 restitutionVelocityThreshold,
 62        Fixed64 accumulatedImpulse,
 63        Fixed64 positiveImpulseScale,
 64        Fixed64 negativeImpulseScale,
 65        out ContactNormalImpulseResult3D result)
 66    {
 51262467        result = default;
 51262468        if (!TryComputeNormalVelocity(
 51262469                linearVelocityA,
 51262470                angularVelocityA,
 51262471                relativeContactPointA,
 51262472                linearVelocityB,
 51262473                angularVelocityB,
 51262474                relativeContactPointB,
 51262475                normal,
 51262476                out Fixed64 normalVelocity)
 51262477            || !TryComputeDenominator(
 51262478                bodyA,
 51262479                relativeContactPointA,
 51262480                bodyB,
 51262481                relativeContactPointB,
 51262482                normal,
 51262483                out ContactEffectiveMassTerms3D denominator))
 84        {
 558685            return false;
 86        }
 87
 50703888        if (denominator.SaturatedSum <= Fixed64.Zero)
 89        {
 3790            result = Zero(normalVelocity);
 3791            return true;
 92        }
 93
 50700194        if (!TryCalculateAccumulatedImpulseDelta(
 50700195                normalVelocity,
 50700196                denominator,
 50700197                restitution,
 50700198                restitutionVelocityThreshold,
 50700199                accumulatedImpulse,
 507001100                positiveImpulseScale,
 507001101                negativeImpulseScale,
 507001102                out Fixed64 impulseScalar))
 103        {
 4104            return false;
 105        }
 106
 506997107        return TryCreateAccumulatedResult(bodyA, relativeContactPointA,
 506997108            bodyB, relativeContactPointB, normal, normalVelocity, impulseScalar, out result);
 109    }
 110
 111    private static bool TryCreateAccumulatedResult(SolidBody? bodyA,
 112        Vector3d relativeContactPointA, SolidBody? bodyB, Vector3d relativeContactPointB,
 113        Vector3d normal, Fixed64 normalVelocity, Fixed64 impulseScalar,
 114        out ContactNormalImpulseResult3D result)
 115    {
 594793116        result = default;
 594793117        if (impulseScalar == Fixed64.Zero)
 118        {
 543741119            result = Zero(normalVelocity);
 543741120            return true;
 121        }
 122
 51052123        Vector3d impulseB = normal * impulseScalar;
 51052124        Vector3d impulseA = -impulseB;
 51052125        bool linearAResolved = TryComputeLinearVelocityDelta(
 51052126            bodyA,
 51052127            impulseA,
 51052128            out Vector3d linearVelocityDeltaA);
 51052129        bool angularAResolved = TryComputeAngularVelocityDelta(
 51052130            bodyA,
 51052131            relativeContactPointA,
 51052132            impulseA,
 51052133            out Vector3d angularVelocityDeltaA);
 51052134        bool linearBResolved = TryComputeLinearVelocityDelta(
 51052135            bodyB,
 51052136            impulseB,
 51052137            out Vector3d linearVelocityDeltaB);
 51052138        bool angularBResolved = TryComputeAngularVelocityDelta(
 51052139            bodyB,
 51052140            relativeContactPointB,
 51052141            impulseB,
 51052142            out Vector3d angularVelocityDeltaB);
 51052143        if (!(linearAResolved
 51052144            & angularAResolved
 51052145            & linearBResolved
 51052146            & angularBResolved))
 147        {
 3942148            return false;
 149        }
 150
 47110151        result = new ContactNormalImpulseResult3D(
 47110152            normalVelocity,
 47110153            impulseScalar,
 47110154            linearVelocityDeltaA,
 47110155            angularVelocityDeltaA,
 47110156            linearVelocityDeltaB,
 47110157            angularVelocityDeltaB);
 47110158        return true;
 159    }
 160
 161    internal static bool TryCalculateAccumulatedDeltaWithImpact(
 162        SolidBody? bodyA, Vector3d linearVelocityA, Vector3d angularVelocityA,
 163        Vector3d relativeContactPointA, SolidBody? bodyB, Vector3d linearVelocityB,
 164        Vector3d angularVelocityB, Vector3d relativeContactPointB, Vector3d normal,
 165        Fixed64 restitution, Fixed64 restitutionVelocityThreshold, Fixed64 accumulatedImpulse,
 166        in ContactResponseSnapshot impact, out ContactNormalImpulseResult3D result)
 167    {
 602153168        if (restitution == Fixed64.Zero)
 512615169            return TryCalculateAccumulatedDelta(bodyA, linearVelocityA, angularVelocityA,
 512615170                relativeContactPointA, bodyB, linearVelocityB, angularVelocityB,
 512615171                relativeContactPointB, normal, restitution, restitutionVelocityThreshold,
 512615172                accumulatedImpulse, Fixed64.One, Fixed64.One, out result);
 89538173        result = default;
 89538174        if (!TryComputeNormalVelocity(linearVelocityA, angularVelocityA, relativeContactPointA,
 89538175                linearVelocityB, angularVelocityB, relativeContactPointB, normal, out Fixed64 velocity)
 89538176            || !TryComputeNormalVelocity(impact.LinearA, impact.AngularA, relativeContactPointA,
 89538177                impact.LinearB, impact.AngularB, relativeContactPointB, normal, out Fixed64 incoming)
 89538178            || !TryComputeDenominator(bodyA, relativeContactPointA, bodyB,
 89538179                relativeContactPointB, normal, out ContactEffectiveMassTerms3D denominator))
 1737180            return false;
 87801181        if (denominator.SaturatedSum <= Fixed64.Zero)
 182        {
 3183            result = Zero(velocity);
 3184            return true;
 185        }
 87798186        Fixed64 bounce = incoming < -restitutionVelocityThreshold ? restitution : Fixed64.Zero;
 187        // Fuse v_current + e*v_incoming before division. Materializing a rounded
 188        // bounce speed here would add another rounding to the completed impulse.
 87798189        Signed320 numerator = WideArithmetic.Negate(WideArithmetic.AddSigned320(
 87798190            WideArithmetic.MultiplySigned192(Signed192.Raw(velocity), Signed192.One),
 87798191            WideArithmetic.MultiplySigned192(Signed192.Raw(incoming), Signed192.Raw(bounce))));
 87798192        Signed192 sum = WideArithmetic.AddSigned192(
 87798193            WideArithmetic.AddSigned192(Signed192.Raw(denominator.LinearA), Signed192.Raw(denominator.LinearB)),
 87798194            WideArithmetic.AddSigned192(Signed192.Raw(denominator.AngularA), Signed192.Raw(denominator.AngularB)));
 195        Fixed64 impulse;
 87798196        if (!Fixed64.TryGetSignedRawRatio(Signed832.ExtendValue(Signed576.ExtendValue(numerator)), Signed832.ExtendValue
 197        {
 3198            if (numerator.Sign >= 0) return false;
 1199            impulse = -accumulatedImpulse;
 200        }
 87796201        else if (!Fixed64.TryAdd(accumulatedImpulse, delta, out Fixed64 accumulated)
 87796202            || !Fixed64.TrySubtract(FixedMath.Max(Fixed64.Zero, accumulated), accumulatedImpulse, out impulse))
 1203            return false;
 87796204        return TryCreateAccumulatedResult(bodyA, relativeContactPointA, bodyB,
 87796205            relativeContactPointB, normal, velocity, impulse, out result);
 206    }
 207
 208    internal static bool TryCalculateAccumulatedDeltaExact(
 209        SolidBody? bodyA,
 210        Vector3d linearVelocityA,
 211        Vector3d angularVelocityA,
 212        in ExactLever3D relativeContactPointA,
 213        SolidBody? bodyB,
 214        Vector3d linearVelocityB,
 215        Vector3d angularVelocityB,
 216        in ExactLever3D relativeContactPointB,
 217        Vector3d normal,
 218        Fixed64 restitution,
 219        Fixed64 restitutionVelocityThreshold,
 220        Fixed64 accumulatedImpulse,
 221        Fixed64 positiveImpulseScale,
 222        Fixed64 negativeImpulseScale,
 223        out ContactNormalImpulseResult3D result,
 224        in ContactResponseSnapshot impact = default)
 225    {
 11308226        result = default;
 11308227        if (!ExactContactLever3D.TryGetAccumulatedNormalResponse(
 11308228                bodyA,
 11308229                linearVelocityA,
 11308230                angularVelocityA,
 11308231                relativeContactPointA,
 11308232                bodyB,
 11308233                linearVelocityB,
 11308234                angularVelocityB,
 11308235                relativeContactPointB,
 11308236                normal,
 11308237                restitution,
 11308238                restitutionVelocityThreshold,
 11308239                accumulatedImpulse,
 11308240                positiveImpulseScale,
 11308241                negativeImpulseScale,
 11308242                out ExactNormalResponse3D response, impact))
 243        {
 5244            return false;
 245        }
 246
 11303247        bool hasNormalVelocity =
 11303248            response.TryGetNormalVelocity(out Fixed64 normalVelocity);
 11303249        bool hasAppliedImpulse =
 11303250            response.TryGetAppliedImpulse(out Fixed64 appliedImpulse);
 251        Fixed64 impulseScalar;
 11303252        if (response.TryGetAccumulatedImpulse(
 11303253                out Fixed64 newAccumulatedImpulse))
 254        {
 255            // Both values are nonnegative, so their difference is always in
 256            // [-Fixed64.MaxValue, Fixed64.MaxValue].
 11299257            impulseScalar = newAccumulatedImpulse - accumulatedImpulse;
 258        }
 259        else
 260        {
 4261            impulseScalar = -accumulatedImpulse;
 262        }
 263
 11303264        result = new ContactNormalImpulseResult3D(
 11303265            normalVelocity,
 11303266            impulseScalar,
 11303267            appliedImpulse,
 11303268            response.FirstLinearVelocityDelta,
 11303269            response.FirstAngularVelocityDelta,
 11303270            response.SecondLinearVelocityDelta,
 11303271            response.SecondAngularVelocityDelta,
 11303272            hasNormalVelocity,
 11303273            hasAppliedImpulse);
 11303274        return true;
 275    }
 276
 277    private static bool TryCalculateAccumulatedImpulseDelta(
 278        Fixed64 normalVelocity,
 279        in ContactEffectiveMassTerms3D denominator,
 280        Fixed64 restitution,
 281        Fixed64 restitutionVelocityThreshold,
 282        Fixed64 accumulatedImpulse,
 283        Fixed64 positiveImpulseScale,
 284        Fixed64 negativeImpulseScale,
 285        out Fixed64 impulseDelta)
 286    {
 507001287        Fixed64 appliedRestitution =
 507001288            normalVelocity < -restitutionVelocityThreshold
 507001289                ? restitution
 507001290                : Fixed64.Zero;
 507001291        Fixed64 responseFactor = -(Fixed64.One + appliedRestitution);
 507001292        Fixed64 impulseScale = normalVelocity < Fixed64.Zero
 507001293            ? positiveImpulseScale
 507001294            : negativeImpulseScale;
 507001295        if (!Fixed64.TryMultiplyDivideBySum(
 507001296                normalVelocity,
 507001297                responseFactor,
 507001298                impulseScale,
 507001299                Fixed64.One,
 507001300                denominator.LinearA,
 507001301                denominator.LinearB,
 507001302                denominator.AngularA,
 507001303                denominator.AngularB,
 507001304                out Fixed64 scaledImpulse))
 305        {
 2306            impulseDelta = default;
 2307            return normalVelocity >= Fixed64.Zero
 2308                && responseFactor <= Fixed64.Zero
 2309                && impulseScale >= Fixed64.Zero
 2310                && accumulatedImpulse >= Fixed64.Zero
 2311                && Fixed64.TrySubtract(
 2312                    Fixed64.Zero,
 2313                    accumulatedImpulse,
 2314                    out impulseDelta);
 315        }
 316
 506999317        if (!Fixed64.TryAdd(
 506999318                accumulatedImpulse,
 506999319                scaledImpulse,
 506999320                out Fixed64 accumulated))
 321        {
 1322            impulseDelta = default;
 1323            return false;
 324        }
 325
 506998326        return Fixed64.TrySubtract(
 506998327            FixedMath.Max(Fixed64.Zero, accumulated),
 506998328            accumulatedImpulse,
 506998329            out impulseDelta);
 330    }
 331
 332}

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

#LineLine coverage
 1//=======================================================================
 2// ContactNormalImpulse3D.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
 14
 15
 16
 17
 18
 19/// <summary>
 20/// Calculates allocation-free 3D contact-point normal response without mutating either body.
 21/// </summary>
 22/// <content>Owns checked normal velocity, effective mass and final body deltas.</content>
 23internal static partial class ContactNormalImpulse3D
 24{
 25    internal static bool TryCalculateVelocityDeltas(
 26        SolidBody? bodyA,
 27        Vector3d linearVelocityA,
 28        Vector3d angularVelocityA,
 29        Vector3d relativeContactPointA,
 30        SolidBody? bodyB,
 31        Vector3d linearVelocityB,
 32        Vector3d angularVelocityB,
 33        Vector3d relativeContactPointB,
 34        Vector3d normal,
 35        Fixed64 restitution,
 36        Fixed64 restitutionVelocityThreshold,
 37        out ContactNormalVelocityDeltaResult3D result)
 38    {
 8839        result = default;
 8840        if (!TryComputeNormalVelocity(
 8841                linearVelocityA,
 8842                angularVelocityA,
 8843                relativeContactPointA,
 8844                linearVelocityB,
 8845                angularVelocityB,
 8846                relativeContactPointB,
 8847                normal,
 8848                out Fixed64 normalVelocity))
 49        {
 450            return false;
 51        }
 52
 8453        if (normalVelocity >= Fixed64.Zero)
 54        {
 355            result = ZeroVelocityDelta(normalVelocity);
 356            return true;
 57        }
 58
 8159        if (!TryComputeDenominator(
 8160                bodyA,
 8161                relativeContactPointA,
 8162                bodyB,
 8163                relativeContactPointB,
 8164                normal,
 8165                out ContactEffectiveMassTerms3D denominator)
 8166            || denominator.SaturatedSum <= Fixed64.Zero)
 67        {
 5468            return false;
 69        }
 70
 2771        Fixed64 appliedRestitution = normalVelocity < -restitutionVelocityThreshold
 2772            ? restitution
 2773            : Fixed64.Zero;
 2774        Fixed64 responseFactor = -(Fixed64.One + appliedRestitution);
 2775        bool linearAResolved = TryResolveVelocityDelta(
 2776            bodyA?.ProjectLinearMotion(-normal) ?? Vector3d.Zero,
 2777            normalVelocity,
 2778            responseFactor,
 2779            bodyA?.EffectiveInverseMass ?? Fixed64.Zero,
 2780            denominator,
 2781            out Vector3d linearVelocityDeltaA);
 2782        bool angularAResolved = TryResolveAngularVelocityDelta(
 2783                bodyA,
 2784                relativeContactPointA,
 2785                -normal,
 2786                normalVelocity,
 2787                responseFactor,
 2788                denominator,
 2789                out Vector3d angularVelocityDeltaA);
 2790        bool linearBResolved = TryResolveVelocityDelta(
 2791            bodyB?.ProjectLinearMotion(normal) ?? Vector3d.Zero,
 2792            normalVelocity,
 2793            responseFactor,
 2794            bodyB?.EffectiveInverseMass ?? Fixed64.Zero,
 2795            denominator,
 2796            out Vector3d linearVelocityDeltaB);
 2797        bool angularBResolved = TryResolveAngularVelocityDelta(
 2798                bodyB,
 2799                relativeContactPointB,
 27100                normal,
 27101                normalVelocity,
 27102                responseFactor,
 27103                denominator,
 27104                out Vector3d angularVelocityDeltaB);
 27105        if (!(linearAResolved
 27106                & angularAResolved
 27107                & linearBResolved
 27108                & angularBResolved))
 109        {
 2110            return false;
 111        }
 112
 25113        result = new ContactNormalVelocityDeltaResult3D(
 25114            normalVelocity,
 25115            linearVelocityDeltaA,
 25116            angularVelocityDeltaA,
 25117            linearVelocityDeltaB,
 25118            angularVelocityDeltaB);
 25119        return true;
 120    }
 121
 122    internal static bool TryCalculateVelocityDeltasExact(
 123        SolidBody? bodyA,
 124        Vector3d linearVelocityA,
 125        Vector3d angularVelocityA,
 126        in ExactLever3D relativeContactPointA,
 127        SolidBody? bodyB,
 128        Vector3d linearVelocityB,
 129        Vector3d angularVelocityB,
 130        in ExactLever3D relativeContactPointB,
 131        Vector3d normal,
 132        Fixed64 restitution,
 133        Fixed64 restitutionVelocityThreshold,
 134        out ContactNormalVelocityDeltaResult3D result)
 135    {
 62136        result = default;
 62137        if (!ExactContactLever3D.TryGetNormalResponse(
 62138                bodyA,
 62139                linearVelocityA,
 62140                angularVelocityA,
 62141                relativeContactPointA,
 62142                bodyB,
 62143                linearVelocityB,
 62144                angularVelocityB,
 62145                relativeContactPointB,
 62146                normal,
 62147                restitution,
 62148                restitutionVelocityThreshold,
 62149                out ExactNormalResponse3D response))
 150        {
 5151            return false;
 152        }
 153
 57154        bool hasNormalVelocity =
 57155            response.TryGetNormalVelocity(out Fixed64 normalVelocity);
 57156        result = new ContactNormalVelocityDeltaResult3D(
 57157            normalVelocity,
 57158            response.FirstLinearVelocityDelta,
 57159            response.FirstAngularVelocityDelta,
 57160            response.SecondLinearVelocityDelta,
 57161            response.SecondAngularVelocityDelta,
 57162            response.IsClosing,
 57163            hasNormalVelocity);
 57164        return true;
 165    }
 166
 167    [MethodImpl(MethodImplOptions.AggressiveInlining)]
 168    private static Fixed64 GetConstrainedInverseMass(SolidBody? body, Vector3d axis) =>
 1189736169        body?.GetConstrainedInverseMass(axis) ?? Fixed64.Zero;
 170
 171    internal static bool TryComputeNormalVelocity(
 172        Vector3d linearVelocityA,
 173        Vector3d angularVelocityA,
 174        Vector3d relativeContactPointA,
 175        Vector3d linearVelocityB,
 176        Vector3d angularVelocityB,
 177        Vector3d relativeContactPointB,
 178        Vector3d normal,
 179        out Fixed64 normalVelocity)
 180    {
 877558181        if (ContactResponseArithmetic3D.CanUseFastPointVelocity(
 877558182                linearVelocityA,
 877558183                angularVelocityA,
 877558184                relativeContactPointA,
 877558185                linearVelocityB,
 877558186                angularVelocityB,
 877558187                relativeContactPointB,
 877558188                normal))
 189        {
 877495190            Vector3d fastPointVelocityA = linearVelocityA
 877495191                + Vector3d.Cross(
 877495192                    angularVelocityA,
 877495193                    relativeContactPointA);
 877495194            Vector3d fastPointVelocityB = linearVelocityB
 877495195                + Vector3d.Cross(
 877495196                    angularVelocityB,
 877495197                    relativeContactPointB);
 877495198            normalVelocity = Vector3d.Dot(
 877495199                fastPointVelocityB - fastPointVelocityA,
 877495200                normal);
 877495201            return true;
 202        }
 203
 63204        bool resolved = ContactResponseArithmetic3D.TryCross(
 63205                angularVelocityA,
 63206                relativeContactPointA,
 63207                out Vector3d angularA)
 63208            & ContactResponseArithmetic3D.TryCross(
 63209                angularVelocityB,
 63210                relativeContactPointB,
 63211                out Vector3d angularB);
 63212        if (!resolved
 63213            || !Vector3d.TryAdd(
 63214                linearVelocityA,
 63215                angularA,
 63216                out Vector3d pointVelocityA)
 63217            || !Vector3d.TryAdd(
 63218                linearVelocityB,
 63219                angularB,
 63220                out Vector3d pointVelocityB)
 63221            || !Vector3d.TrySubtract(
 63222                pointVelocityB,
 63223                pointVelocityA,
 63224                out Vector3d relativeVelocity))
 225        {
 12226            normalVelocity = default;
 12227            return false;
 228        }
 229
 51230        return ContactResponseArithmetic3D.TryDot(
 51231            relativeVelocity,
 51232            normal,
 51233            out normalVelocity);
 234    }
 235
 236    private static bool TryComputeDenominator(
 237        SolidBody? bodyA,
 238        Vector3d relativeContactPointA,
 239        SolidBody? bodyB,
 240        Vector3d relativeContactPointB,
 241        Vector3d normal,
 242        out ContactEffectiveMassTerms3D denominator)
 243    {
 602237244        denominator = default;
 602237245        bool angularAResolved = TryComputeAngularDenominator(
 602237246            bodyA,
 602237247            relativeContactPointA,
 602237248            normal,
 602237249            out Fixed64 angularA);
 602237250        bool angularBResolved = TryComputeAngularDenominator(
 602237251            bodyB,
 602237252            relativeContactPointB,
 602237253            normal,
 602237254            out Fixed64 angularB);
 602237255        if (!(angularAResolved & angularBResolved))
 256        {
 7369257            return false;
 258        }
 259
 594868260        denominator = new ContactEffectiveMassTerms3D(
 594868261            GetConstrainedInverseMass(bodyA, normal),
 594868262            GetConstrainedInverseMass(bodyB, normal),
 594868263            angularA,
 594868264            angularB);
 594868265        return true;
 266    }
 267
 268    internal static bool TryComputeAngularDenominator(
 269        SolidBody? body,
 270        Vector3d relativeContactPoint,
 271        Vector3d axis,
 272        out Fixed64 denominator)
 273    {
 1426312274        denominator = Fixed64.Zero;
 1426312275        if (body?.CanRotate != true)
 1273868276            return true;
 277
 152444278        Fixed3x3 inverseInertia =
 152444279            body.GetConstrainedInverseInertiaTensor();
 152444280        if (ContactResponseArithmetic3D.CanUseFastAngularResponse(
 152444281                relativeContactPoint,
 152444282                axis,
 152444283                inverseInertia))
 284        {
 152339285            Vector3d fastTorqueAxis =
 152339286                Vector3d.Cross(relativeContactPoint, axis);
 152339287            Vector3d fastAngularVelocityDelta =
 152339288                Fixed3x3.TransformDirection(
 152339289                    inverseInertia,
 152339290                    fastTorqueAxis);
 152339291            Vector3d fastAngular = Vector3d.Cross(
 152339292                fastAngularVelocityDelta,
 152339293                relativeContactPoint);
 152339294            denominator = Vector3d.Dot(fastAngular, axis);
 152339295            bool responsePreserved =
 152339296                ContactResponseArithmetic3D.PreservesNonzeroCrossProduct(
 152339297                    relativeContactPoint,
 152339298                    axis,
 152339299                    fastTorqueAxis);
 152339300            responsePreserved &=
 152339301                ContactResponseArithmetic3D
 152339302                .PreservesNonzeroTransformDirection(
 152339303                    inverseInertia,
 152339304                    fastTorqueAxis,
 152339305                    fastAngularVelocityDelta);
 152339306            responsePreserved &=
 152339307                ContactResponseArithmetic3D
 152339308                .PreservesNonzeroCrossProduct(
 152339309                    fastAngularVelocityDelta,
 152339310                    relativeContactPoint,
 152339311                    fastAngular);
 152339312            responsePreserved &=
 152339313                ContactResponseArithmetic3D.PreservesNonzeroDotProduct(
 152339314                    fastAngular,
 152339315                    axis,
 152339316                    denominator);
 152339317            if (!responsePreserved)
 318            {
 9361319                denominator = default;
 9361320                return false;
 321            }
 322
 142978323            denominator = FixedMath.Max(denominator, Fixed64.Zero);
 142978324            return true;
 325        }
 326
 105327        if (!ContactResponseArithmetic3D.TryCross(
 105328                relativeContactPoint,
 105329                axis,
 105330                out Vector3d torqueAxis)
 105331            || !ContactResponseArithmetic3D.TryTransformDirection(
 105332                inverseInertia,
 105333                torqueAxis,
 105334                out Vector3d angularVelocityDelta)
 105335            || !ContactResponseArithmetic3D.TryCross(
 105336                angularVelocityDelta,
 105337                relativeContactPoint,
 105338                out Vector3d angular)
 105339            || !ContactResponseArithmetic3D.TryDot(
 105340                angular,
 105341                axis,
 105342                out denominator))
 343        {
 56344            denominator = default;
 56345            return false;
 346        }
 347
 49348        denominator = FixedMath.Max(denominator, Fixed64.Zero);
 49349        return true;
 350    }
 351
 352    internal static bool TryComputeLinearVelocityDelta(
 353        SolidBody? body,
 354        Vector3d impulse,
 355        out Vector3d velocityDelta)
 356    {
 120030357        if (body != null)
 74121358            impulse = body.ProjectLinearMotion(impulse);
 359
 120030360        if (!ContactResponseArithmetic3D.TryScale(
 120030361                impulse,
 120030362                body?.EffectiveInverseMass ?? Fixed64.Zero,
 120030363                out velocityDelta))
 364        {
 2365            return false;
 366        }
 367
 120028368        return true;
 369    }
 370
 371    internal static bool TryComputeAngularVelocityDelta(
 372        SolidBody? body,
 373        Vector3d relativeContactPoint,
 374        Vector3d impulse,
 375        out Vector3d velocityDelta)
 376    {
 120067377        velocityDelta = Vector3d.Zero;
 120067378        if (body?.CanRotate != true)
 80504379            return true;
 380
 39563381        Fixed3x3 inverseInertia =
 39563382            body.GetConstrainedInverseInertiaTensor();
 39563383        if (ContactResponseArithmetic3D.CanUseFastAngularResponse(
 39563384                relativeContactPoint,
 39563385                impulse,
 39563386                inverseInertia))
 387        {
 39533388            if (!ContactResponseArithmetic3D.TryCross(
 39533389                    relativeContactPoint,
 39533390                    impulse,
 39533391                    out Vector3d torqueAxis))
 1950392                return false;
 393
 37583394            velocityDelta = Fixed3x3.TransformDirection(
 37583395                inverseInertia,
 37583396                torqueAxis);
 37583397            return ContactResponseArithmetic3D
 37583398                .PreservesNonzeroTransformDirection(
 37583399                    inverseInertia,
 37583400                    torqueAxis,
 37583401                    velocityDelta);
 402        }
 403
 30404        bool torqueResolved = ContactResponseArithmetic3D.TryCross(
 30405                relativeContactPoint,
 30406                impulse,
 30407                out Vector3d checkedTorqueAxis);
 30408        bool transformResolved =
 30409            ContactResponseArithmetic3D.TryTransformDirection(
 30410                inverseInertia,
 30411                checkedTorqueAxis,
 30412                out velocityDelta);
 30413        return torqueResolved & transformResolved;
 414    }
 415
 416    private static bool TryResolveAngularVelocityDelta(
 417        SolidBody? body,
 418        Vector3d relativeContactPoint,
 419        Vector3d signedNormal,
 420        Fixed64 normalVelocity,
 421        Fixed64 responseFactor,
 422        in ContactEffectiveMassTerms3D denominator,
 423        out Vector3d velocityDelta)
 424    {
 54425        velocityDelta = Vector3d.Zero;
 54426        if (body?.CanRotate != true)
 18427            return true;
 428
 36429        bool responseResolved = TryComputeAngularVelocityDelta(
 36430            body,
 36431            relativeContactPoint,
 36432            signedNormal,
 36433            out Vector3d response);
 36434        bool deltaResolved = TryResolveVelocityDelta(
 36435            response,
 36436            normalVelocity,
 36437            responseFactor,
 36438            Fixed64.One,
 36439            denominator,
 36440            out velocityDelta);
 36441        return responseResolved & deltaResolved;
 442    }
 443
 444    private static bool TryResolveVelocityDelta(
 445        Vector3d response,
 446        Fixed64 firstMultiplier,
 447        Fixed64 secondMultiplier,
 448        Fixed64 thirdMultiplier,
 449        in ContactEffectiveMassTerms3D denominator,
 450        out Vector3d velocityDelta)
 451    {
 90452        bool xResolved = Fixed64.TryMultiplyDivideBySum(
 90453            response.X,
 90454            firstMultiplier,
 90455            secondMultiplier,
 90456            thirdMultiplier,
 90457            denominator.LinearA,
 90458            denominator.LinearB,
 90459            denominator.AngularA,
 90460            denominator.AngularB,
 90461            out Fixed64 x);
 90462        bool yResolved = Fixed64.TryMultiplyDivideBySum(
 90463            response.Y,
 90464            firstMultiplier,
 90465            secondMultiplier,
 90466            thirdMultiplier,
 90467            denominator.LinearA,
 90468            denominator.LinearB,
 90469            denominator.AngularA,
 90470            denominator.AngularB,
 90471            out Fixed64 y);
 90472        bool zResolved = Fixed64.TryMultiplyDivideBySum(
 90473            response.Z,
 90474            firstMultiplier,
 90475            secondMultiplier,
 90476            thirdMultiplier,
 90477            denominator.LinearA,
 90478            denominator.LinearB,
 90479            denominator.AngularA,
 90480            denominator.AngularB,
 90481            out Fixed64 z);
 90482        velocityDelta = xResolved & yResolved & zResolved
 90483            ? new Vector3d(x, y, z)
 90484            : default;
 90485        return xResolved & yResolved & zResolved;
 486    }
 487
 488    [MethodImpl(MethodImplOptions.AggressiveInlining)]
 489    private static ContactNormalImpulseResult3D Zero(Fixed64 normalVelocity) =>
 543781490        new(
 543781491            normalVelocity,
 543781492            Fixed64.Zero,
 543781493            Vector3d.Zero,
 543781494            Vector3d.Zero,
 543781495            Vector3d.Zero,
 543781496            Vector3d.Zero);
 497
 498    [MethodImpl(MethodImplOptions.AggressiveInlining)]
 499    private static ContactNormalVelocityDeltaResult3D ZeroVelocityDelta(Fixed64 normalVelocity) =>
 3500        new(
 3501            normalVelocity,
 3502            Vector3d.Zero,
 3503            Vector3d.Zero,
 3504            Vector3d.Zero,
 3505            Vector3d.Zero);
 506}

Methods/Properties

CalculateAccumulatedDelta(Gravitas.SolidBody,FixedMathSharp.Vector3d,FixedMathSharp.Vector3d,FixedMathSharp.Vector3d,Gravitas.SolidBody,FixedMathSharp.Vector3d,FixedMathSharp.Vector3d,FixedMathSharp.Vector3d,FixedMathSharp.Vector3d,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64)
TryCalculateAccumulatedDelta(Gravitas.SolidBody,FixedMathSharp.Vector3d,FixedMathSharp.Vector3d,FixedMathSharp.Vector3d,Gravitas.SolidBody,FixedMathSharp.Vector3d,FixedMathSharp.Vector3d,FixedMathSharp.Vector3d,FixedMathSharp.Vector3d,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,Gravitas.CollisionHandling.ContactNormalImpulseResult3D&)
TryCreateAccumulatedResult(Gravitas.SolidBody,FixedMathSharp.Vector3d,Gravitas.SolidBody,FixedMathSharp.Vector3d,FixedMathSharp.Vector3d,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,Gravitas.CollisionHandling.ContactNormalImpulseResult3D&)
TryCalculateAccumulatedDeltaWithImpact(Gravitas.SolidBody,FixedMathSharp.Vector3d,FixedMathSharp.Vector3d,FixedMathSharp.Vector3d,Gravitas.SolidBody,FixedMathSharp.Vector3d,FixedMathSharp.Vector3d,FixedMathSharp.Vector3d,FixedMathSharp.Vector3d,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,Gravitas.CollisionHandling.ContactResponseSnapshot&,Gravitas.CollisionHandling.ContactNormalImpulseResult3D&)
TryCalculateAccumulatedDeltaExact(Gravitas.SolidBody,FixedMathSharp.Vector3d,FixedMathSharp.Vector3d,Gravitas.CollisionHandling.ExactLever3D&,Gravitas.SolidBody,FixedMathSharp.Vector3d,FixedMathSharp.Vector3d,Gravitas.CollisionHandling.ExactLever3D&,FixedMathSharp.Vector3d,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,Gravitas.CollisionHandling.ContactNormalImpulseResult3D&,Gravitas.CollisionHandling.ContactResponseSnapshot&)
TryCalculateAccumulatedImpulseDelta(FixedMathSharp.Fixed64,Gravitas.CollisionHandling.ContactEffectiveMassTerms3D&,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64&)
TryCalculateVelocityDeltas(Gravitas.SolidBody,FixedMathSharp.Vector3d,FixedMathSharp.Vector3d,FixedMathSharp.Vector3d,Gravitas.SolidBody,FixedMathSharp.Vector3d,FixedMathSharp.Vector3d,FixedMathSharp.Vector3d,FixedMathSharp.Vector3d,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,Gravitas.CollisionHandling.ContactNormalVelocityDeltaResult3D&)
TryCalculateVelocityDeltasExact(Gravitas.SolidBody,FixedMathSharp.Vector3d,FixedMathSharp.Vector3d,Gravitas.CollisionHandling.ExactLever3D&,Gravitas.SolidBody,FixedMathSharp.Vector3d,FixedMathSharp.Vector3d,Gravitas.CollisionHandling.ExactLever3D&,FixedMathSharp.Vector3d,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,Gravitas.CollisionHandling.ContactNormalVelocityDeltaResult3D&)
GetConstrainedInverseMass(Gravitas.SolidBody,FixedMathSharp.Vector3d)
TryComputeNormalVelocity(FixedMathSharp.Vector3d,FixedMathSharp.Vector3d,FixedMathSharp.Vector3d,FixedMathSharp.Vector3d,FixedMathSharp.Vector3d,FixedMathSharp.Vector3d,FixedMathSharp.Vector3d,FixedMathSharp.Fixed64&)
TryComputeDenominator(Gravitas.SolidBody,FixedMathSharp.Vector3d,Gravitas.SolidBody,FixedMathSharp.Vector3d,FixedMathSharp.Vector3d,Gravitas.CollisionHandling.ContactEffectiveMassTerms3D&)
TryComputeAngularDenominator(Gravitas.SolidBody,FixedMathSharp.Vector3d,FixedMathSharp.Vector3d,FixedMathSharp.Fixed64&)
TryComputeLinearVelocityDelta(Gravitas.SolidBody,FixedMathSharp.Vector3d,FixedMathSharp.Vector3d&)
TryComputeAngularVelocityDelta(Gravitas.SolidBody,FixedMathSharp.Vector3d,FixedMathSharp.Vector3d,FixedMathSharp.Vector3d&)
TryResolveAngularVelocityDelta(Gravitas.SolidBody,FixedMathSharp.Vector3d,FixedMathSharp.Vector3d,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,Gravitas.CollisionHandling.ContactEffectiveMassTerms3D&,FixedMathSharp.Vector3d&)
TryResolveVelocityDelta(FixedMathSharp.Vector3d,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,Gravitas.CollisionHandling.ContactEffectiveMassTerms3D&,FixedMathSharp.Vector3d&)
Zero(FixedMathSharp.Fixed64)
ZeroVelocityDelta(FixedMathSharp.Fixed64)