< Summary

Information
Class: Gravitas.CollisionHandling.ExactContactLever2D
Assembly: Gravitas
File(s): /home/runner/work/Gravitas/Gravitas/src/Gravitas/CollisionHandling/Response/2D/ExactContactLever2D.cs
Line coverage
100%
Covered lines: 149
Uncovered lines: 0
Coverable lines: 149
Total lines: 279
Line coverage: 100%
Branch coverage
100%
Covered branches: 34
Total branches: 34
Branch coverage: 100%
Method coverage

Feature is only available for sponsors

Upgrade to PRO version

Metrics

MethodBranch coverage Crap Score Cyclomatic complexity Line coverage
GetDetectedNormalAnchor(...)100%1414100%
CanUseCompactResponse(...)100%44100%
CreateResponseOperand(...)100%44100%
TryGetNormalResponse(...)100%11100%
TryGetAccumulatedNormalResponse(...)100%11100%
ToSpatial(...)100%11100%
ToPlanar(...)100%11100%
ToPlanarAngular(...)100%11100%
TryGetImpulseVelocityDeltas(...)100%11100%
TryGetParticipantVelocityDeltas(...)100%88100%
CreateInverseInertia(...)100%44100%

File(s)

/home/runner/work/Gravitas/Gravitas/src/Gravitas/CollisionHandling/Response/2D/ExactContactLever2D.cs

#LineLine coverage
 1//=======================================================================
 2// ExactContactLever2D.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;
 9using Gravitas.Colliders;
 10
 11namespace Gravitas.CollisionHandling;
 12
 13/// <summary>
 14/// Adapts planar body mobility and signed yaw to the shared exact 3D response
 15/// kernel.
 16/// </summary>
 17internal static class ExactContactLever2D
 18{
 19    internal static ContactAnchor GetDetectedNormalAnchor(LSCollider2D owner, int featureNamespace,
 20        ContactAnchor bodyCenter, ContactAnchor surfaceAnchor)
 21    {
 29370522        if (owner.Body == null) return surfaceAnchor;
 7780123        LSCollider2D leaf = owner is LSCompoundCollider2D compound && featureNamespace < 0
 7780124            ? compound.GetPartCollider(-featureNamespace - 1) : owner;
 7780125        if (leaf is not LSCircleCollider2D
 7780126            && !(leaf is LSCapsuleCollider2D capsule && capsule.AxisLength == Fixed64.Zero))
 4872627            return surfaceAnchor;
 28
 29        // A circular slab's normal line passes through its shape center. Its
 30        // rounded radial surface offset must not create a spurious yaw Jacobian:
 31        // with translation locked, even one raw lever unit can stop all spin.
 32        // Retain center-to-COM torque for offset circles and compound leaves;
 33        // friction continues to use the original finite surface anchor.
 2907534        if (leaf.TransformRelativeMassPropertyPointExact(Vector2d.Zero).TryGetPoint(out Vector2d localCenter))
 2907335            return new ContactAnchor(bodyCenter.Origin, bodyCenter.Rotation, ToSpatial(localCenter));
 36
 37        // A compound-local center may exceed Q32.32 while its world center is
 38        // finite. Detected circular side/support anchors retain that frozen
 39        // center as their origin. Cap normals have no planar yaw coupling.
 240        return ContactAnchor.FromWorldPoint(surfaceAnchor.Origin);
 41    }
 42
 43    internal static bool CanUseCompactResponse(
 44        SolidBody2D? bodyA,
 45        Vector2d linearVelocityA,
 46        Fixed64 angularVelocityA,
 47        Vector2d relativeContactPointA,
 48        SolidBody2D? bodyB,
 49        Vector2d linearVelocityB,
 50        Fixed64 angularVelocityB,
 51        Vector2d relativeContactPointB,
 52        Vector2d axis)
 53    {
 3254054        Vector3d spatialAxis = ToSpatial(axis);
 3254055        Vector3d leverA = ToSpatial(relativeContactPointA);
 3254056        Vector3d leverB = ToSpatial(relativeContactPointB);
 3254057        return ContactResponseArithmetic3D.CanUseFastPointVelocity(
 3254058                ToSpatial(linearVelocityA),
 3254059                new Vector3d(
 3254060                    Fixed64.Zero,
 3254061                    -angularVelocityA,
 3254062                    Fixed64.Zero),
 3254063                leverA,
 3254064                ToSpatial(linearVelocityB),
 3254065                new Vector3d(
 3254066                    Fixed64.Zero,
 3254067                    -angularVelocityB,
 3254068                    Fixed64.Zero),
 3254069                leverB,
 3254070                spatialAxis)
 3254071            && ContactResponseArithmetic3D.CanUseFastAngularResponse(
 3254072                leverA,
 3254073                spatialAxis,
 3254074                CreateInverseInertia(bodyA))
 3254075            && ContactResponseArithmetic3D.CanUseFastAngularResponse(
 3254076                leverB,
 3254077                spatialAxis,
 3254078                CreateInverseInertia(bodyB));
 79    }
 80
 81    internal static ExactContactResponseOperand3D CreateResponseOperand(
 82        SolidBody2D? body,
 83        Vector2d linearVelocity,
 84        Fixed64 angularVelocity,
 85        in ExactLever3D lever,
 86        Vector3d signedAxis) =>
 487687        new(
 487688            lever,
 487689            ToSpatial(linearVelocity),
 487690            new Vector3d(
 487691                Fixed64.Zero,
 487692                -angularVelocity,
 487693                Fixed64.Zero),
 487694            body == null
 487695                ? Vector3d.Zero
 487696                : ToSpatial(body.ProjectLinearMotion(ToPlanar(signedAxis))),
 487697            body?.EffectiveInverseMass ?? Fixed64.Zero,
 487698            CreateInverseInertia(body), angularVelocity);
 99
 100    internal static bool TryGetNormalResponse(
 101        SolidBody2D? bodyA,
 102        Vector2d linearVelocityA,
 103        Fixed64 angularVelocityA,
 104        in ExactLever3D relativeContactPointA,
 105        SolidBody2D? bodyB,
 106        Vector2d linearVelocityB,
 107        Fixed64 angularVelocityB,
 108        in ExactLever3D relativeContactPointB,
 109        Vector2d normal,
 110        Fixed64 restitution,
 111        Fixed64 restitutionVelocityThreshold,
 112        out ExactNormalResponse3D response)
 113    {
 34114        Vector3d spatialNormal = ToSpatial(normal);
 34115        ExactContactResponseOperand3D first = CreateResponseOperand(
 34116            bodyA,
 34117            linearVelocityA,
 34118            angularVelocityA,
 34119            relativeContactPointA,
 34120            -spatialNormal);
 34121        ExactContactResponseOperand3D second = CreateResponseOperand(
 34122            bodyB,
 34123            linearVelocityB,
 34124            angularVelocityB,
 34125            relativeContactPointB,
 34126            spatialNormal);
 34127        return ExactContactResponseKernel.TryGetNormalResponse(
 34128            first,
 34129            second,
 34130            spatialNormal,
 34131            restitution,
 34132            restitutionVelocityThreshold,
 34133            out response);
 134    }
 135
 136    internal static bool TryGetAccumulatedNormalResponse(
 137        SolidBody2D? bodyA,
 138        Vector2d linearVelocityA,
 139        Fixed64 angularVelocityA,
 140        in ExactLever3D relativeContactPointA,
 141        SolidBody2D? bodyB,
 142        Vector2d linearVelocityB,
 143        Fixed64 angularVelocityB,
 144        in ExactLever3D relativeContactPointB,
 145        Vector2d normal,
 146        Fixed64 restitution,
 147        Fixed64 restitutionVelocityThreshold,
 148        Fixed64 accumulatedImpulse,
 149        Fixed64 positiveImpulseScale,
 150        Fixed64 negativeImpulseScale,
 151        out ExactNormalResponse3D response,
 152        in ContactResponseSnapshot impact = default)
 153    {
 370154        Vector3d spatialNormal = ToSpatial(normal);
 370155        ExactContactResponseOperand3D first = CreateResponseOperand(
 370156            bodyA,
 370157            linearVelocityA,
 370158            angularVelocityA,
 370159            relativeContactPointA,
 370160            -spatialNormal);
 370161        ExactContactResponseOperand3D second = CreateResponseOperand(
 370162            bodyB,
 370163            linearVelocityB,
 370164            angularVelocityB,
 370165            relativeContactPointB,
 370166            spatialNormal);
 370167        return ExactContactResponseKernel.TryGetAccumulatedNormalResponse(
 370168            first,
 370169            second,
 370170            spatialNormal,
 370171            restitution,
 370172            restitutionVelocityThreshold,
 370173            accumulatedImpulse,
 370174            positiveImpulseScale,
 370175            negativeImpulseScale,
 370176            out response, impact);
 177    }
 178
 179    internal static Vector3d ToSpatial(Vector2d vector) =>
 1334418180        new(vector.X, Fixed64.Zero, vector.Y);
 181
 182    internal static Vector2d ToPlanar(Vector3d vector) =>
 179555183        new(vector.X, vector.Z);
 184
 185    internal static Fixed64 ToPlanarAngular(Vector3d vector)
 186    {
 187        // Exact response operands preflight the planar delta before publication.
 188        System.Diagnostics.Debug.Assert(vector.Y != Fixed64.MinValue);
 177277189        return -vector.Y;
 190    }
 191
 192    internal static bool TryGetImpulseVelocityDeltas(
 193        SolidBody2D? bodyA,
 194        in ExactLever3D relativeContactPointA,
 195        SolidBody2D? bodyB,
 196        in ExactLever3D relativeContactPointB,
 197        Vector2d firstAxis,
 198        Fixed64 firstScale,
 199        Vector2d secondAxis,
 200        Fixed64 secondScale,
 201        out Vector2d linearVelocityDeltaA,
 202        out Fixed64 angularVelocityDeltaA,
 203        out Vector2d linearVelocityDeltaB,
 204        out Fixed64 angularVelocityDeltaB)
 205    {
 4206        Vector3d first = ToSpatial(firstAxis);
 4207        Vector3d second = ToSpatial(secondAxis);
 4208        bool firstResolved = TryGetParticipantVelocityDeltas(
 4209            bodyA,
 4210            relativeContactPointA,
 4211            -first,
 4212            firstScale,
 4213            -second,
 4214            secondScale,
 4215            out linearVelocityDeltaA,
 4216            out angularVelocityDeltaA);
 4217        bool secondResolved = TryGetParticipantVelocityDeltas(
 4218            bodyB,
 4219            relativeContactPointB,
 4220            first,
 4221            firstScale,
 4222            second,
 4223            secondScale,
 4224            out linearVelocityDeltaB,
 4225            out angularVelocityDeltaB);
 4226        return firstResolved & secondResolved;
 227    }
 228
 229    internal static bool TryGetParticipantVelocityDeltas(
 230        SolidBody2D? body,
 231        in ExactLever3D lever,
 232        Vector3d firstAxis,
 233        Fixed64 firstScale,
 234        Vector3d secondAxis,
 235        Fixed64 secondScale,
 236        out Vector2d linearVelocityDelta,
 237        out Fixed64 angularVelocityDelta)
 238    {
 14239        linearVelocityDelta = Vector2d.Zero;
 14240        angularVelocityDelta = Fixed64.Zero;
 14241        if (body?.HasSolverMobility != true)
 6242            return true;
 243
 8244        Vector3d spatialLinear = Vector3d.Zero;
 8245        bool linearResolved = !body.CanTranslate
 8246            || Vector3d.TryScaledLinearCombination(
 8247                ToSpatial(body.ProjectLinearMotion(ToPlanar(firstAxis))),
 8248                firstScale,
 8249                ToSpatial(body.ProjectLinearMotion(ToPlanar(secondAxis))),
 8250                secondScale,
 8251                Vector3d.Zero,
 8252                Fixed64.Zero,
 8253                body.EffectiveInverseMass,
 8254                out spatialLinear);
 8255        Vector3d spatialAngular = Vector3d.Zero;
 8256        bool angularResolved = !body.CanRotate
 8257            || ExactLever3D.TryGetTransformedWeightedCrossProduct(
 8258                lever,
 8259                firstAxis,
 8260                firstScale,
 8261                secondAxis,
 8262                secondScale,
 8263                Vector3d.Zero,
 8264                Fixed64.Zero,
 8265                CreateInverseInertia(body),
 8266                out spatialAngular);
 8267        linearVelocityDelta = ToPlanar(spatialLinear);
 8268        bool planarResolved = Fixed64.TrySubtract(Fixed64.Zero, spatialAngular.Y, out angularVelocityDelta);
 8269        return linearResolved & angularResolved & planarResolved;
 270    }
 271
 272    internal static Fixed3x3 CreateInverseInertia(SolidBody2D? body) =>
 254256273        body?.CanRotate == true
 254256274            ? new Fixed3x3(
 254256275                Fixed64.Zero, Fixed64.Zero, Fixed64.Zero,
 254256276                Fixed64.Zero, body.EffectiveInverseMomentOfInertia, Fixed64.Zero,
 254256277                Fixed64.Zero, Fixed64.Zero, Fixed64.Zero)
 254256278            : Fixed3x3.Zero;
 279}

Methods/Properties

GetDetectedNormalAnchor(Gravitas.Colliders.LSCollider2D,System.Int32,Gravitas.CollisionHandling.ContactAnchor,Gravitas.CollisionHandling.ContactAnchor)
CanUseCompactResponse(Gravitas.SolidBody2D,FixedMathSharp.Vector2d,FixedMathSharp.Fixed64,FixedMathSharp.Vector2d,Gravitas.SolidBody2D,FixedMathSharp.Vector2d,FixedMathSharp.Fixed64,FixedMathSharp.Vector2d,FixedMathSharp.Vector2d)
CreateResponseOperand(Gravitas.SolidBody2D,FixedMathSharp.Vector2d,FixedMathSharp.Fixed64,Gravitas.CollisionHandling.ExactLever3D&,FixedMathSharp.Vector3d)
TryGetNormalResponse(Gravitas.SolidBody2D,FixedMathSharp.Vector2d,FixedMathSharp.Fixed64,Gravitas.CollisionHandling.ExactLever3D&,Gravitas.SolidBody2D,FixedMathSharp.Vector2d,FixedMathSharp.Fixed64,Gravitas.CollisionHandling.ExactLever3D&,FixedMathSharp.Vector2d,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,Gravitas.CollisionHandling.ExactNormalResponse3D&)
TryGetAccumulatedNormalResponse(Gravitas.SolidBody2D,FixedMathSharp.Vector2d,FixedMathSharp.Fixed64,Gravitas.CollisionHandling.ExactLever3D&,Gravitas.SolidBody2D,FixedMathSharp.Vector2d,FixedMathSharp.Fixed64,Gravitas.CollisionHandling.ExactLever3D&,FixedMathSharp.Vector2d,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,Gravitas.CollisionHandling.ExactNormalResponse3D&,Gravitas.CollisionHandling.ContactResponseSnapshot&)
ToSpatial(FixedMathSharp.Vector2d)
ToPlanar(FixedMathSharp.Vector3d)
ToPlanarAngular(FixedMathSharp.Vector3d)
TryGetImpulseVelocityDeltas(Gravitas.SolidBody2D,Gravitas.CollisionHandling.ExactLever3D&,Gravitas.SolidBody2D,Gravitas.CollisionHandling.ExactLever3D&,FixedMathSharp.Vector2d,FixedMathSharp.Fixed64,FixedMathSharp.Vector2d,FixedMathSharp.Fixed64,FixedMathSharp.Vector2d&,FixedMathSharp.Fixed64&,FixedMathSharp.Vector2d&,FixedMathSharp.Fixed64&)
TryGetParticipantVelocityDeltas(Gravitas.SolidBody2D,Gravitas.CollisionHandling.ExactLever3D&,FixedMathSharp.Vector3d,FixedMathSharp.Fixed64,FixedMathSharp.Vector3d,FixedMathSharp.Fixed64,FixedMathSharp.Vector2d&,FixedMathSharp.Fixed64&)
CreateInverseInertia(Gravitas.SolidBody2D)