< Summary

Information
Class: Gravitas.CollisionHandling.ExactContactLever3D
Assembly: Gravitas
File(s): /home/runner/work/Gravitas/Gravitas/src/Gravitas/CollisionHandling/Response/3D/ExactContactLever3D.cs
Line coverage
100%
Covered lines: 123
Uncovered lines: 0
Coverable lines: 123
Total lines: 234
Line coverage: 100%
Branch coverage
100%
Covered branches: 18
Total branches: 18
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/ExactContactLever3D.cs

#LineLine coverage
 1//=======================================================================
 2// ExactContactLever3D.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/// <summary>
 13/// Evaluates solver products for a semantic lever that cannot be narrowed to
 14/// <see cref="Vector3d"/>.
 15/// </summary>
 16internal static class ExactContactLever3D
 17{
 18    internal static bool TryGetNormalResponse(
 19        SolidBody? bodyA,
 20        Vector3d linearVelocityA,
 21        Vector3d angularVelocityA,
 22        in ExactLever3D relativeContactPointA,
 23        SolidBody? bodyB,
 24        Vector3d linearVelocityB,
 25        Vector3d angularVelocityB,
 26        in ExactLever3D relativeContactPointB,
 27        Vector3d normal,
 28        Fixed64 restitution,
 29        Fixed64 restitutionVelocityThreshold,
 30        out ExactNormalResponse3D response)
 31    {
 6232        ExactContactResponseOperand3D first = CreateResponseOperand(
 6233            bodyA,
 6234            linearVelocityA,
 6235            angularVelocityA,
 6236            relativeContactPointA,
 6237            -normal);
 6238        ExactContactResponseOperand3D second = CreateResponseOperand(
 6239            bodyB,
 6240            linearVelocityB,
 6241            angularVelocityB,
 6242            relativeContactPointB,
 6243            normal);
 6244        return ExactContactResponseKernel.TryGetNormalResponse(
 6245            first,
 6246            second,
 6247            normal,
 6248            restitution,
 6249            restitutionVelocityThreshold,
 6250            out response);
 51    }
 52
 53    internal static bool TryGetAccumulatedNormalResponse(
 54        SolidBody? bodyA,
 55        Vector3d linearVelocityA,
 56        Vector3d angularVelocityA,
 57        in ExactLever3D relativeContactPointA,
 58        SolidBody? bodyB,
 59        Vector3d linearVelocityB,
 60        Vector3d angularVelocityB,
 61        in ExactLever3D relativeContactPointB,
 62        Vector3d normal,
 63        Fixed64 restitution,
 64        Fixed64 restitutionVelocityThreshold,
 65        Fixed64 accumulatedImpulse,
 66        Fixed64 positiveImpulseScale,
 67        Fixed64 negativeImpulseScale,
 68        out ExactNormalResponse3D response,
 69        in ContactResponseSnapshot impact = default)
 70    {
 1130871        ExactContactResponseOperand3D first = CreateResponseOperand(
 1130872            bodyA,
 1130873            linearVelocityA,
 1130874            angularVelocityA,
 1130875            relativeContactPointA,
 1130876            -normal);
 1130877        ExactContactResponseOperand3D second = CreateResponseOperand(
 1130878            bodyB,
 1130879            linearVelocityB,
 1130880            angularVelocityB,
 1130881            relativeContactPointB,
 1130882            normal);
 1130883        return ExactContactResponseKernel.TryGetAccumulatedNormalResponse(
 1130884            first,
 1130885            second,
 1130886            normal,
 1130887            restitution,
 1130888            restitutionVelocityThreshold,
 1130889            accumulatedImpulse,
 1130890            positiveImpulseScale,
 1130891            negativeImpulseScale,
 1130892            out response, impact);
 93    }
 94
 95    internal static ExactContactResponseOperand3D CreateResponseOperand(
 96        SolidBody? body,
 97        Vector3d linearVelocity,
 98        Vector3d angularVelocity,
 99        in ExactLever3D relativeContactPoint,
 100        Vector3d signedNormal) =>
 29188101        new(
 29188102            relativeContactPoint,
 29188103            linearVelocity,
 29188104            angularVelocity,
 29188105            body?.ProjectLinearMotion(signedNormal) ?? Vector3d.Zero,
 29188106            body?.EffectiveInverseMass ?? Fixed64.Zero,
 29188107            body?.GetConstrainedInverseInertiaTensor() ?? Fixed3x3.Zero);
 108
 109    internal static bool TryGetAngularVelocityDelta(
 110        SolidBody? body,
 111        in ExactLever3D relativeContactPoint,
 112        Vector3d impulse,
 113        out Vector3d velocityDelta)
 114    {
 17115        velocityDelta = Vector3d.Zero;
 17116        return body?.CanRotate != true
 17117            || ExactLever3D.TryGetTransformedScaledCrossProduct(
 17118                relativeContactPoint,
 17119                impulse,
 17120                body.GetConstrainedInverseInertiaTensor(),
 17121                Fixed64.One,
 17122                Fixed64.One,
 17123                Fixed64.One,
 17124                out velocityDelta);
 125    }
 126
 127    internal static bool TryGetImpulseCombinationVelocityDeltas(
 128        SolidBody? bodyA,
 129        in ExactLever3D relativeContactPointA,
 130        SolidBody? bodyB,
 131        in ExactLever3D relativeContactPointB,
 132        Vector3d firstAxis,
 133        Fixed64 firstScale,
 134        Vector3d secondAxis,
 135        Fixed64 secondScale,
 136        Vector3d thirdAxis,
 137        Fixed64 thirdScale,
 138        out Vector3d linearVelocityDeltaA,
 139        out Vector3d angularVelocityDeltaA,
 140        out Vector3d linearVelocityDeltaB,
 141        out Vector3d angularVelocityDeltaB)
 142    {
 10143        bool linearAResolved = TryGetLinearCombinationVelocityDelta(
 10144            bodyA,
 10145            -firstAxis,
 10146            firstScale,
 10147            -secondAxis,
 10148            secondScale,
 10149            -thirdAxis,
 10150            thirdScale,
 10151            out linearVelocityDeltaA);
 10152        bool angularAResolved = TryGetAngularCombinationVelocityDelta(
 10153            bodyA,
 10154            relativeContactPointA,
 10155            -firstAxis,
 10156            firstScale,
 10157            -secondAxis,
 10158            secondScale,
 10159            -thirdAxis,
 10160            thirdScale,
 10161            out angularVelocityDeltaA);
 10162        bool linearBResolved = TryGetLinearCombinationVelocityDelta(
 10163            bodyB,
 10164            firstAxis,
 10165            firstScale,
 10166            secondAxis,
 10167            secondScale,
 10168            thirdAxis,
 10169            thirdScale,
 10170            out linearVelocityDeltaB);
 10171        bool angularBResolved = TryGetAngularCombinationVelocityDelta(
 10172            bodyB,
 10173            relativeContactPointB,
 10174            firstAxis,
 10175            firstScale,
 10176            secondAxis,
 10177            secondScale,
 10178            thirdAxis,
 10179            thirdScale,
 10180            out angularVelocityDeltaB);
 10181        return linearAResolved
 10182            & angularAResolved
 10183            & linearBResolved
 10184            & angularBResolved;
 185    }
 186
 187    private static bool TryGetLinearCombinationVelocityDelta(
 188        SolidBody? body,
 189        Vector3d firstAxis,
 190        Fixed64 firstScale,
 191        Vector3d secondAxis,
 192        Fixed64 secondScale,
 193        Vector3d thirdAxis,
 194        Fixed64 thirdScale,
 195        out Vector3d velocityDelta)
 196    {
 20197        velocityDelta = Vector3d.Zero;
 20198        return body?.CanTranslate != true
 20199            || Vector3d.TryScaledLinearCombination(
 20200                body.ProjectLinearMotion(firstAxis),
 20201                firstScale,
 20202                body.ProjectLinearMotion(secondAxis),
 20203                secondScale,
 20204                body.ProjectLinearMotion(thirdAxis),
 20205                thirdScale,
 20206                body.EffectiveInverseMass,
 20207                out velocityDelta);
 208    }
 209
 210    private static bool TryGetAngularCombinationVelocityDelta(
 211        SolidBody? body,
 212        in ExactLever3D relativeContactPoint,
 213        Vector3d firstAxis,
 214        Fixed64 firstScale,
 215        Vector3d secondAxis,
 216        Fixed64 secondScale,
 217        Vector3d thirdAxis,
 218        Fixed64 thirdScale,
 219        out Vector3d velocityDelta)
 220    {
 20221        velocityDelta = Vector3d.Zero;
 20222        return body?.CanRotate != true
 20223            || ExactLever3D.TryGetTransformedWeightedCrossProduct(
 20224                    relativeContactPoint,
 20225                    firstAxis,
 20226                    firstScale,
 20227                    secondAxis,
 20228                    secondScale,
 20229                    thirdAxis,
 20230                    thirdScale,
 20231                    body.GetConstrainedInverseInertiaTensor(),
 20232                    out velocityDelta);
 233    }
 234}

Methods/Properties

TryGetNormalResponse(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.ExactNormalResponse3D&)
TryGetAccumulatedNormalResponse(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.ExactNormalResponse3D&,Gravitas.CollisionHandling.ContactResponseSnapshot&)
CreateResponseOperand(Gravitas.SolidBody,FixedMathSharp.Vector3d,FixedMathSharp.Vector3d,Gravitas.CollisionHandling.ExactLever3D&,FixedMathSharp.Vector3d)
TryGetAngularVelocityDelta(Gravitas.SolidBody,Gravitas.CollisionHandling.ExactLever3D&,FixedMathSharp.Vector3d,FixedMathSharp.Vector3d&)
TryGetImpulseCombinationVelocityDeltas(Gravitas.SolidBody,Gravitas.CollisionHandling.ExactLever3D&,Gravitas.SolidBody,Gravitas.CollisionHandling.ExactLever3D&,FixedMathSharp.Vector3d,FixedMathSharp.Fixed64,FixedMathSharp.Vector3d,FixedMathSharp.Fixed64,FixedMathSharp.Vector3d,FixedMathSharp.Fixed64,FixedMathSharp.Vector3d&,FixedMathSharp.Vector3d&,FixedMathSharp.Vector3d&,FixedMathSharp.Vector3d&)
TryGetLinearCombinationVelocityDelta(Gravitas.SolidBody,FixedMathSharp.Vector3d,FixedMathSharp.Fixed64,FixedMathSharp.Vector3d,FixedMathSharp.Fixed64,FixedMathSharp.Vector3d,FixedMathSharp.Fixed64,FixedMathSharp.Vector3d&)
TryGetAngularCombinationVelocityDelta(Gravitas.SolidBody,Gravitas.CollisionHandling.ExactLever3D&,FixedMathSharp.Vector3d,FixedMathSharp.Fixed64,FixedMathSharp.Vector3d,FixedMathSharp.Fixed64,FixedMathSharp.Vector3d,FixedMathSharp.Fixed64,FixedMathSharp.Vector3d&)