< Summary

Line coverage
100%
Covered lines: 893
Uncovered lines: 0
Coverable lines: 893
Total lines: 1383
Line coverage: 100%
Branch coverage
100%
Covered branches: 370
Total branches: 370
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: PrepareSolve(...)100%1414100%
File 1: Resolve(...)100%44100%
File 1: Resolve(...)100%11100%
File 1: Resolve(...)100%1414100%
File 1: ResolveGroups(...)100%2222100%
File 1: ResolveGroup(...)100%5858100%
File 1: IsCorrectionOwner(...)100%1616100%
File 1: TryCreateBodyPair(...)100%88100%
File 1: BuildContactBuffer(...)100%44100%
File 1: TryCreateContact(...)100%1212100%
File 1: TryCalculateExactNormalResult(...)100%11100%
File 1: ApplyPositionCorrections(...)100%22100%
File 1: ApplyPositionCorrection(...)100%1414100%
File 1: TryApplyCachedImpulse(...)100%44100%
File 1: TryApplyNormalImpulse(...)100%88100%
File 1: ApplyVelocityDelta(...)100%66100%
File 1: TrySolveFrictionImpulse(...)100%3636100%
File 1: TrySolveFrictionImpulseExact(...)100%66100%
File 1: CanUseCompactAxisResponse(...)100%11100%
File 1: TryApplyContactImpulseCombination(...)100%1010100%
File 1: TryApplyCompactImpulse(...)100%22100%
File 1: TryGetLinearVelocityDelta(...)100%22100%
File 1: TryGetAngularVelocityDelta(...)100%44100%
File 1: TryApplyVelocityDeltas(...)100%66100%
File 1: ResolveLinearVelocity(...)100%44100%
File 1: ResolveAngularVelocity(...)100%44100%
File 1: ResolveContactNormal(...)100%44100%
File 1: ResolveContactNormal(...)100%1010100%
File 1: GetExactLevers(...)100%44100%
File 1: HasFailedResponse(...)100%11100%
File 1: RejectResponse(...)100%11100%
File 2: CorrectSeededSupportPositions(...)100%22100%
File 2: TryCorrectSeededSupportPositions(...)100%1212100%
File 2: TryReadTranslationInterface(...)100%88100%
File 2: TryPrepareSupportTranslation(...)100%44100%
File 2: CorrectSeededSupportPosition(...)100%44100%
File 2: TrySeedSupportedIslandNormals(...)100%1818100%
File 2: IsSupportPair(...)100%11100%
File 2: TryGetSupportedBodies(...)100%1212100%
File 2: IsFixedSupport(...)100%22100%
File 2: HasSupportMobility(...)100%22100%
File 2: TryReadSupportRows(...)100%1212100%
File 2: TryReadSupportJacobian(...)100%22100%
File 2: TryPrepareSupportVelocities(...)100%44100%
File 2: TryPreparePublishedSupportMotion(...)100%88100%
File 2: TryProjectSupportSpeed(...)100%11100%
File 2: TryAccumulateSupportDelta(...)100%11100%
File 2: ReadMotion(...)100%11100%
File 2: ApplySupportDelta(...)100%11100%
File 2: StoreSupportNormals(...)100%22100%
File 2: .ctor(...)100%11100%

File(s)

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

#LineLine coverage
 1//=======================================================================
 2// CollisionResponse2D.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/// Deterministic pure 2D manifold contact response.
 16/// </summary>
 17public static partial class CollisionResponse2D
 18{
 19    /// <summary>Penetration tolerated before positional correction is applied.</summary>
 120    public static readonly Fixed64 PenetrationSlop = (Fixed64)0.01f;
 21
 22    /// <summary>Fraction of excess penetration corrected by the solver.</summary>
 123    public static readonly Fixed64 PenetrationCorrectionPercent = Fixed64.One;
 24
 25
 26
 27    internal static void PrepareSolve(CollisionPair2D pair)
 28    {
 11829        pair.ClearResponseSnapshot();
 11830        if (pair.Manifold.GroupCount <= 1
 12231            || !TryCreateBodyPair(pair, out ResponseBody2D bodyA, out ResponseBody2D bodyB)) return;
 11432        pair.RetainWarmStart();
 11433        GroupedSolveState state = pair.ResponseState ??= new GroupedSolveState();
 11434        state.CenterA = bodyA.Body?.GetCenterOfMassAnchor() ?? default;
 11435        state.CenterB = bodyB.Body?.GetCenterOfMassAnchor() ?? default;
 11436        state.Impact = new ContactResponseSnapshot(
 11437            ExactContactLever2D.ToSpatial(bodyA.Body?.Position ?? Vector2d.Zero),
 11438            ExactContactLever2D.ToSpatial(bodyB.Body?.Position ?? Vector2d.Zero), default, default,
 11439            ExactContactLever2D.ToSpatial(ResolveLinearVelocity(bodyA.Body)), Vector3d.Zero,
 11440            ExactContactLever2D.ToSpatial(ResolveLinearVelocity(bodyB.Body)), Vector3d.Zero,
 11441            planarAngularB: ResolveAngularVelocity(bodyB.Body),
 11442            planarAngularA: ResolveAngularVelocity(bodyA.Body));
 11443    }
 44
 45    internal static void Resolve(CollisionPair2D pair)
 46    {
 507347        bool grouped = pair.Manifold.GroupCount > 1;
 517748        if (grouped) PrepareSolve(pair);
 507349        Resolve(pair, applyCachedImpulse: true, applyPositionCorrection: true, iterateGroups: true);
 516750        if (grouped) pair.ClearResponseSnapshot();
 506351    }
 52
 53    internal static void Resolve(CollisionPair2D pair, bool applyCachedImpulse, bool applyPositionCorrection) =>
 460354        Resolve(pair, applyCachedImpulse, applyPositionCorrection, iterateGroups: false);
 55
 56    private static void Resolve(CollisionPair2D pair, bool applyCachedImpulse,
 57        bool applyPositionCorrection, bool iterateGroups)
 58    {
 1534159        if (applyPositionCorrection) pair.ApplyRecovery();
 966860        if (!TryCreateBodyPair(pair, out ResponseBody2D bodyA, out ResponseBody2D bodyB))
 1561            return;
 62
 965363        if (pair.Manifold.GroupCount == 1)
 64        {
 952965            pair.RetainWarmStart();
 952966            ResolveGroup(pair, bodyA, bodyB, 0, applyCachedImpulse, applyPositionCorrection,
 952967                bodyA.Body?.GetCenterOfMassAnchor() ?? default,
 952968                bodyB.Body?.GetCenterOfMassAnchor() ?? default,
 952969                bodyA.Body?.Position ?? Vector2d.Zero,
 952970                bodyB.Body?.Position ?? Vector2d.Zero, default);
 952771            return;
 72        }
 12473        ResolveGroups(pair, bodyA, bodyB, applyCachedImpulse, applyPositionCorrection, iterateGroups);
 12474    }
 75
 76    private static void ResolveGroups(CollisionPair2D pair, ResponseBody2D bodyA, ResponseBody2D bodyB,
 77        bool applyCachedImpulse, bool applyPositionCorrection, bool iterateGroups)
 78    {
 79        // Standalone and island owners prepare the grouped incoming state
 80        // before any row or joint changes a participating body's velocity.
 12481        GroupedSolveState state = pair.ResponseState!;
 24882        ContactAnchor2D centerA = state.CenterA, centerB = state.CenterB;
 12483        ref readonly ContactResponseSnapshot impact = ref state.Impact;
 12484        Vector2d positionA = ExactContactLever2D.ToPlanar(impact.PositionA);
 12485        Vector2d positionB = ExactContactLever2D.ToPlanar(impact.PositionB);
 12486        if (applyCachedImpulse || applyPositionCorrection)
 87        {
 88            // Warm all groups before solving any row: later warm starts must
 89            // not overwrite the state against which an earlier group was solved.
 99890            for (int g = 0; g < pair.Manifold.GroupCount; g++)
 91            {
 38992                ref ContactGroup2D group = ref pair.Manifold.GetGroup(g);
 179093                for (int point = 0; point < group.Count; point++)
 94                {
 50695                    if (!TryCreateContact(pair, bodyA, bodyB, centerA, centerB,
 50696                            g, point, out SolverContact2D contact)) continue;
 50597                    if (applyPositionCorrection && IsCorrectionOwner(pair, contact))
 13198                        ApplyPositionCorrection(contact, Fixed64.One);
 50599                    if (applyCachedImpulse && !TryApplyCachedImpulse(pair, contact, positionA, positionB))
 1100                        pair.RemoveWarmStartImpulse(contact.Group, contact.ContactId);
 101                }
 102            }
 103        }
 124104        int iterations = iterateGroups
 124105            ? pair.ColliderA.Context.Settings.DiscreteSolverIterations : 1;
 1496106        for (int iteration = 0; iteration < iterations; iteration++)
 107        {
 5774108            for (int group = 0; group < pair.Manifold.GroupCount; group++)
 109            {
 2263110                ResolveGroup(pair, bodyA, bodyB, group,
 2263111                    applyCachedImpulse: false, applyPositionCorrection: false,
 2263112                    centerA, centerB, positionA, positionB, impact);
 113            }
 114        }
 124115    }
 116
 117    private static void ResolveGroup(
 118        CollisionPair2D pair,
 119        ResponseBody2D bodyA,
 120        ResponseBody2D bodyB,
 121        int groupIndex,
 122        bool applyCachedImpulse,
 123        bool applyPositionCorrection,
 124        in ContactAnchor2D responseCenterA,
 125        in ContactAnchor2D responseCenterB,
 126        Vector2d responsePositionA,
 127        Vector2d responsePositionB,
 128        in ContactResponseSnapshot impact)
 129    {
 11792130        SolverContactBuffer2D contacts = BuildContactBuffer(
 11792131            pair,
 11792132            bodyA,
 11792133            bodyB,
 11792134            responseCenterA,
 11792135            responseCenterB, groupIndex);
 11792136        if (contacts.Count == 0)
 5137            return;
 138
 11787139        byte failedResponseMask = 0;
 11787140        Fixed64 contactShare = impact.IsValid ? Fixed64.One : Fixed64.One / (Fixed64)contacts.Count;
 11787141        if (applyPositionCorrection)
 5532142            ApplyPositionCorrections(ref contacts, contactShare);
 143
 11785144        if (applyCachedImpulse)
 145        {
 5535146            bool rebuildContacts = false;
 23210147            for (int i = 0; i < contacts.Count; i++)
 148            {
 6070149                SolverContact2D contact = contacts.GetContact(i);
 6070150                if (TryApplyCachedImpulse(
 6070151                        pair,
 6070152                        contact,
 6070153                        responsePositionA,
 6070154                        responsePositionB))
 155                {
 156                    continue;
 157                }
 158
 2159                pair.RemoveWarmStartImpulse(contact.Group, contact.ContactId);
 2160                rebuildContacts = true;
 161            }
 162
 5535163            if (rebuildContacts)
 164            {
 2165                contacts = BuildContactBuffer(
 2166                    pair,
 2167                    bodyA,
 2168                    bodyB,
 2169                    responseCenterA,
 2170                    responseCenterB, groupIndex);
 171            }
 172        }
 173
 11785174        Vector2d normalLinearVelocityA =
 11785175            ResolveLinearVelocity(bodyA.Body);
 11785176        Fixed64 normalAngularVelocityA =
 11785177            ResolveAngularVelocity(bodyA.Body);
 11785178        Vector2d normalLinearVelocityB =
 11785179            ResolveLinearVelocity(bodyB.Body);
 11785180        Fixed64 normalAngularVelocityB =
 11785181            ResolveAngularVelocity(bodyB.Body);
 11785182        Fixed64 restitutionVelocityThreshold = pair.ColliderA.Context.Settings.RestitutionVelocityThreshold;
 56382183        for (int i = 0; i < contacts.Count; i++)
 184        {
 16406185            SolverContact2D contact = contacts.GetContact(i);
 16406186            if (impact.IsValid)
 187            {
 2939188                normalLinearVelocityA = ResolveLinearVelocity(bodyA.Body);
 2939189                normalAngularVelocityA = ResolveAngularVelocity(bodyA.Body);
 2939190                normalLinearVelocityB = ResolveLinearVelocity(bodyB.Body);
 2939191                normalAngularVelocityB = ResolveAngularVelocity(bodyB.Body);
 192            }
 16406193            SolidBody2D? contactBodyA = contact.A.Body;
 16406194            SolidBody2D? contactBodyB = contact.B.Body;
 195            bool normalResolved;
 16406196            ContactNormalImpulseResult2D normalResult = default;
 16406197            if (contact.RelativeA.IsExact || contact.RelativeB.IsExact
 16406198                || impact.IsValid && contact.Restitution != Fixed64.Zero)
 199            {
 296200                normalResolved = TryCalculateExactNormalResult(
 296201                    pair,
 296202                    contact,
 296203                    responsePositionA,
 296204                    responsePositionB,
 296205                    normalLinearVelocityA,
 296206                    normalAngularVelocityA,
 296207                    normalLinearVelocityB,
 296208                    normalAngularVelocityB,
 296209                    restitutionVelocityThreshold,
 296210                    contactShare,
 296211                    out normalResult, impact);
 212            }
 213            else
 214            {
 16110215                normalResolved = CanUseCompactAxisResponse(
 16110216                        contact,
 16110217                        contact.Normal)
 16110218                    && ContactNormalImpulse2D.TryCalculateAccumulatedDelta(
 16110219                        contactBodyA,
 16110220                        normalLinearVelocityA,
 16110221                        normalAngularVelocityA,
 16110222                        contact.RelativeA.Vector,
 16110223                        contactBodyB,
 16110224                        normalLinearVelocityB,
 16110225                        normalAngularVelocityB,
 16110226                        contact.RelativeB.Vector,
 16110227                        contact.Normal,
 16110228                        contact.Restitution,
 16110229                        restitutionVelocityThreshold,
 16110230                        contact.CachedNormalImpulse,
 16110231                        contactShare,
 16110232                        contactShare,
 16110233                        out normalResult);
 16110234                if (!normalResolved)
 235                {
 72236                    normalResolved = TryCalculateExactNormalResult(
 72237                        pair,
 72238                        contact,
 72239                        responsePositionA,
 72240                        responsePositionB,
 72241                        normalLinearVelocityA,
 72242                        normalAngularVelocityA,
 72243                        normalLinearVelocityB,
 72244                        normalAngularVelocityB,
 72245                        restitutionVelocityThreshold,
 72246                        contactShare,
 72247                        out normalResult, impact);
 248                }
 249            }
 16406250            if (!normalResolved)
 251            {
 2252                RejectResponse(pair, contact, i, ref failedResponseMask);
 2253                continue;
 254            }
 16404255            if (impact.IsValid && !TryApplyNormalImpulse(contact, normalResult))
 256            {
 1257                RejectResponse(pair, contact, i, ref failedResponseMask);
 1258                continue;
 259            }
 16403260            Fixed64 normalImpulse = contact.CachedNormalImpulse + normalResult.ImpulseScalar;
 16403261            contacts.SetNormalImpulse(
 16403262                i,
 16403263                normalImpulse,
 16403264                normalResult);
 16403265            if (impact.IsValid)
 266            {
 2938267                if (!TrySolveFrictionImpulse(pair, contact, responsePositionA, responsePositionB,
 2938268                        normalLinearVelocityA, normalAngularVelocityA, normalLinearVelocityB,
 2938269                        normalAngularVelocityB, restitutionVelocityThreshold, Fixed64.One,
 2938270                        normalResult, normalImpulse, out Fixed64 tangentImpulse, impact))
 271                {
 2272                    RejectResponse(pair, contact, i, ref failedResponseMask);
 2273                    continue;
 274                }
 2936275                pair.StoreWarmStartImpulse(contact.Group, contact.ContactId, normalImpulse, tangentImpulse);
 276            }
 277        }
 14044278        if (impact.IsValid) return;
 279
 45986280        for (int i = 0; i < contacts.Count; i++)
 281        {
 13467282            if (HasFailedResponse(failedResponseMask, i))
 283                continue;
 284
 13465285            if (!TryApplyNormalImpulse(
 13465286                    contacts.GetContact(i),
 13465287                    contacts.GetNormalResult(i)))
 288            {
 1289                RejectResponse(
 1290                    pair,
 1291                    contacts.GetContact(i),
 1292                    i,
 1293                    ref failedResponseMask);
 294            }
 295        }
 296
 45986297        for (int i = 0; i < contacts.Count; i++)
 298        {
 13467299            if (HasFailedResponse(failedResponseMask, i))
 300                continue;
 301
 13464302            SolverContact2D contact = contacts.GetContact(i);
 13464303            if (!TrySolveFrictionImpulse(
 13464304                    pair,
 13464305                    contact,
 13464306                    responsePositionA,
 13464307                    responsePositionB,
 13464308                    normalLinearVelocityA,
 13464309                    normalAngularVelocityA,
 13464310                    normalLinearVelocityB,
 13464311                    normalAngularVelocityB,
 13464312                    restitutionVelocityThreshold,
 13464313                    contactShare,
 13464314                    contacts.GetNormalResult(i),
 13464315                    contacts.GetNormalImpulse(i),
 13464316                    out Fixed64 tangentImpulse))
 317            {
 2318                RejectResponse(pair, contact, i, ref failedResponseMask);
 2319                continue;
 320            }
 13462321            contacts.SetTangentImpulse(
 13462322                i,
 13462323                tangentImpulse);
 324        }
 325
 45986326        for (int i = 0; i < contacts.Count; i++)
 327        {
 13467328            if (HasFailedResponse(failedResponseMask, i))
 329                continue;
 330
 13462331            SolverContact2D contact = contacts.GetContact(i);
 13462332            pair.StoreWarmStartImpulse(
 13462333                contact.Group, contact.ContactId,
 13462334                contacts.GetNormalImpulse(i),
 13462335                contacts.GetTangentImpulse(i));
 336        }
 9526337    }
 338
 339    private static bool IsCorrectionOwner(CollisionPair2D pair, SolverContact2D contact)
 340    {
 341        // Translation is shared by equal-normal samples; retain every velocity
 342        // row, but apply the deepest directional correction only once.
 1930343        for (int g = 0; g < pair.Manifold.GroupCount; g++)
 344        {
 834345            ref ContactGroup2D group = ref pair.Manifold.GetGroup(g);
 2866346            for (int point = 0; point < group.Count; point++)
 347            {
 971348                ManifoldContact2D other = group[point];
 971349                if (ResolveContactNormal(pair, group.Key, other.Normal) != contact.Normal) continue;
 885350                if (other.Depth > contact.Depth
 885351                    || other.Depth == contact.Depth
 1257352                    && (g < contact.GroupIndex || g == contact.GroupIndex && point < contact.PointIndex)) return false;
 353            }
 354        }
 131355        return true;
 356    }
 357
 358    private static bool TryCreateBodyPair(
 359        CollisionPair2D pair,
 360        out ResponseBody2D bodyA,
 361        out ResponseBody2D bodyB)
 362    {
 9782363        bodyA = default;
 9782364        bodyB = default;
 365
 9782366        if (pair.ColliderA.IsTrigger || pair.ColliderB.IsTrigger || !pair.Manifold.HasContact)
 13367            return false;
 368
 9769369        bodyA = ResponseBody2D.Create(pair.ColliderA);
 9769370        bodyB = ResponseBody2D.Create(pair.ColliderB);
 9769371        return bodyA.HasSolverMobility || bodyB.HasSolverMobility;
 372    }
 373
 374    private static SolverContactBuffer2D BuildContactBuffer(
 375        CollisionPair2D pair,
 376        ResponseBody2D bodyA,
 377        ResponseBody2D bodyB,
 378        in ContactAnchor2D responseCenterA,
 379        in ContactAnchor2D responseCenterB,
 380        int groupIndex)
 381    {
 11798382        SolverContactBuffer2D contacts = default;
 56440383        for (int i = 0; i < pair.Manifold.GetGroup(groupIndex).Count; i++)
 384        {
 16422385            if (TryCreateContact(
 16422386                    pair,
 16422387                    bodyA,
 16422388                    bodyB,
 16422389                    responseCenterA,
 16422390                    responseCenterB,
 16422391                    groupIndex, i,
 16422392                    out SolverContact2D contact))
 16417393                contacts.Add(contact);
 394        }
 395
 11798396        return contacts;
 397    }
 398
 399    private static bool TryCreateContact(
 400        CollisionPair2D pair,
 401        ResponseBody2D bodyA,
 402        ResponseBody2D bodyB,
 403        in ContactAnchor2D responseCenterA,
 404        in ContactAnchor2D responseCenterB,
 405        int groupIndex,
 406        int pointIndex,
 407        out SolverContact2D contact)
 408    {
 17754409        contact = default;
 17754410        ref ContactGroup2D surface = ref pair.Manifold.GetGroup(groupIndex);
 17754411        ManifoldContact2D manifoldContact = surface[pointIndex];
 17754412        ContactGroupKey group = surface.Key;
 17754413        Vector2d normal = ResolveContactNormal(pair, group, manifoldContact.Normal);
 17754414        if (normal == Vector2d.Zero)
 6415            return false;
 17748416        ContactLever2D relativeA = bodyA.Body == null
 17748417            ? ContactLever2D.Zero
 17748418            : ContactLever2D.Create(
 17748419                manifoldContact.AnchorA,
 17748420                responseCenterA);
 17748421        ContactLever2D relativeB = bodyB.Body == null
 17748422            ? ContactLever2D.Zero
 17748423            : ContactLever2D.Create(
 17748424                manifoldContact.AnchorB,
 17748425                responseCenterB);
 17748426        PhysicsMaterial materialA = manifoldContact.HasMaterialOverride
 17748427            ? manifoldContact.MaterialA
 17748428            : pair.ColliderA.Material;
 17748429        PhysicsMaterial materialB = manifoldContact.HasMaterialOverride
 17748430            ? manifoldContact.MaterialB
 17748431            : pair.ColliderB.Material;
 432
 17748433        Fixed64 cachedNormalImpulse = Fixed64.Zero;
 17748434        Fixed64 cachedTangentImpulse = Fixed64.Zero;
 17748435        if (pair.TryGetWarmStartImpulse(group, manifoldContact.ContactId, out ContactWarmStartImpulse cached))
 436        {
 13489437            cachedNormalImpulse = cached.NormalImpulse;
 13489438            cachedTangentImpulse = cached.TangentImpulse;
 439        }
 440
 17748441        contact = new SolverContact2D(
 17748442            groupIndex, pointIndex,
 17748443            manifoldContact.ContactId,
 17748444            bodyA,
 17748445            bodyB,
 17748446            relativeA,
 17748447            relativeB,
 17748448            manifoldContact.Depth,
 17748449            normal,
 17748450            materialA,
 17748451            materialB,
 17748452            cachedNormalImpulse,
 17748453            cachedTangentImpulse, group);
 17748454        return true;
 455    }
 456
 457    [MethodImpl(MethodImplOptions.NoInlining)]
 458    private static bool TryCalculateExactNormalResult(
 459        CollisionPair2D pair,
 460        SolverContact2D contact,
 461        Vector2d responsePositionA,
 462        Vector2d responsePositionB,
 463        Vector2d linearVelocityA,
 464        Fixed64 angularVelocityA,
 465        Vector2d linearVelocityB,
 466        Fixed64 angularVelocityB,
 467        Fixed64 restitutionVelocityThreshold,
 468        Fixed64 contactShare,
 469        out ContactNormalImpulseResult2D result,
 470        in ContactResponseSnapshot impact = default)
 471    {
 368472        GetExactLevers(
 368473            pair,
 368474            contact,
 368475            responsePositionA,
 368476            responsePositionB,
 368477            out ExactLever3D exactA,
 368478            out ExactLever3D exactB);
 368479        return ContactNormalImpulse2D.TryCalculateAccumulatedDeltaExact(
 368480            contact.A.Body,
 368481            linearVelocityA,
 368482            angularVelocityA,
 368483            exactA,
 368484            contact.B.Body,
 368485            linearVelocityB,
 368486            angularVelocityB,
 368487            exactB,
 368488            contact.Normal,
 368489            contact.Restitution,
 368490            restitutionVelocityThreshold,
 368491            contact.CachedNormalImpulse,
 368492            contactShare,
 368493            contactShare,
 368494            out result, impact);
 495    }
 496
 497    private static void ApplyPositionCorrections(ref SolverContactBuffer2D contacts, Fixed64 contactShare)
 498    {
 23212499        for (int i = 0; i < contacts.Count; i++)
 6072500            ApplyPositionCorrection(contacts.GetContact(i), contactShare);
 5534501    }
 502
 503    private static void ApplyPositionCorrection(SolverContact2D contact, Fixed64 contactShare)
 504    {
 6203505        Fixed64 correctionDepth = contact.Depth - PenetrationSlop;
 6203506        if (correctionDepth <= Fixed64.Zero)
 5120507            return;
 508
 1083509        Fixed64 inverseMassA = contact.A.GetConstrainedInverseMass(contact.Normal);
 1083510        Fixed64 inverseMassB = contact.B.GetConstrainedInverseMass(contact.Normal);
 1083511        Fixed64 totalInverseMass = inverseMassA + inverseMassB;
 1083512        if (totalInverseMass <= Fixed64.Zero)
 11513            return;
 514
 1072515        Fixed64 weightedDepth = correctionDepth * PenetrationCorrectionPercent * contactShare;
 516        Vector2d correctionA, correctionB;
 1072517        if (CollisionPositionCorrection.TryGetCompactScale(
 1072518                weightedDepth, inverseMassA, inverseMassB, out Fixed64 scale))
 519        {
 1032520            Vector2d correction = contact.Normal * scale;
 1032521            correctionA = -correction * inverseMassA;
 1032522            correctionB = correction * inverseMassB;
 523        }
 524        else
 525        {
 40526            correctionA = -contact.Normal * CollisionPositionCorrection.GetWeightedDistance(
 40527                weightedDepth, inverseMassA, inverseMassA, inverseMassB);
 40528            correctionB = contact.Normal * CollisionPositionCorrection.GetWeightedDistance(
 40529                weightedDepth, inverseMassB, inverseMassA, inverseMassB);
 530        }
 531        // Prepare both endpoints before publishing either participant's correction.
 1072532        Vector2d positionA = contact.A.Body?.PrepareCollisionPositionCorrection(correctionA) ?? default;
 1072533        Vector2d positionB = contact.B.Body?.PrepareCollisionPositionCorrection(correctionB) ?? default;
 1070534        contact.A.Body?.ApplyCollisionPositionState(positionA);
 1070535        contact.B.Body?.ApplyCollisionPositionState(positionB);
 1062536    }
 537
 538    private static bool TryApplyCachedImpulse(
 539        CollisionPair2D pair,
 540        SolverContact2D contact,
 541        Vector2d responsePositionA,
 542        Vector2d responsePositionB)
 543    {
 6573544        if (contact.CachedNormalImpulse == Fixed64.Zero && contact.CachedTangentImpulse == Fixed64.Zero)
 6316545            return true;
 546
 257547        return TryApplyContactImpulseCombination(
 257548            pair,
 257549            contact,
 257550            responsePositionA,
 257551            responsePositionB,
 257552            contact.Normal,
 257553            contact.CachedNormalImpulse,
 257554            contact.Tangent,
 257555            contact.CachedTangentImpulse);
 556    }
 557
 558    private static bool TryApplyNormalImpulse(
 559        SolverContact2D contact,
 560        ContactNormalImpulseResult2D result)
 561    {
 16404562        if (result.LinearVelocityDeltaA == Vector2d.Zero
 16404563            && result.AngularVelocityDeltaA == Fixed64.Zero
 16404564            && result.LinearVelocityDeltaB == Vector2d.Zero
 16404565            && result.AngularVelocityDeltaB == Fixed64.Zero)
 566        {
 15966567            return true;
 568        }
 569
 438570        return TryApplyVelocityDeltas(
 438571            contact,
 438572            result.LinearVelocityDeltaA,
 438573            result.AngularVelocityDeltaA,
 438574            result.LinearVelocityDeltaB,
 438575            result.AngularVelocityDeltaB);
 576    }
 577
 578    private static void ApplyVelocityDelta(
 579        ResponseBody2D body,
 580        Vector2d linearVelocityDelta,
 581        Fixed64 angularVelocityDelta)
 582    {
 1534583        if (!body.HasSolverMobility)
 559584            return;
 585
 975586        if (body.CanTranslate)
 971587            body.Body!.ApplyCollisionLinearVelocityDelta(linearVelocityDelta);
 975588        if (body.CanRotate)
 780589            body.Body!.ApplyCollisionAngularVelocityDelta(angularVelocityDelta);
 975590    }
 591
 592    private static bool TrySolveFrictionImpulse(
 593        CollisionPair2D pair,
 594        SolverContact2D contact,
 595        Vector2d responsePositionA,
 596        Vector2d responsePositionB,
 597        Vector2d normalLinearVelocityA,
 598        Fixed64 normalAngularVelocityA,
 599        Vector2d normalLinearVelocityB,
 600        Fixed64 normalAngularVelocityB,
 601        Fixed64 restitutionVelocityThreshold,
 602        Fixed64 contactShare,
 603        ContactNormalImpulseResult2D normalResult,
 604        Fixed64 normalImpulseScalar,
 605        out Fixed64 accumulated,
 606        in ContactResponseSnapshot impact = default)
 607    {
 16402608        accumulated = default;
 16402609        if (contact.RelativeA.IsExact
 16402610            || contact.RelativeB.IsExact
 16402611            || !normalResult.HasRepresentableAccumulatedImpulse
 16402612            || !CanUseCompactAxisResponse(contact, contact.Tangent)
 16402613            || !Fixed64.TryMultiplyDivide(
 16402614                normalImpulseScalar,
 16402615                contact.StaticFriction,
 16402616                Fixed64.One,
 16402617                out Fixed64 staticFrictionLimit)
 16402618            || !Fixed64.TryMultiplyDivide(
 16402619                normalImpulseScalar,
 16402620                contact.DynamicFriction,
 16402621                Fixed64.One,
 16402622                out Fixed64 dynamicFrictionLimit))
 623        {
 52624            return TrySolveFrictionImpulseExact(
 52625                pair,
 52626                contact,
 52627                responsePositionA,
 52628                responsePositionB,
 52629                normalLinearVelocityA,
 52630                normalAngularVelocityA,
 52631                normalLinearVelocityB,
 52632                normalAngularVelocityB,
 52633                restitutionVelocityThreshold,
 52634                contactShare,
 52635                out accumulated, impact);
 636        }
 637
 16350638        Fixed64 impulseScalar = Fixed64.Zero;
 16350639        if (staticFrictionLimit > Fixed64.Zero || dynamicFrictionLimit > Fixed64.Zero)
 640        {
 2300641            Vector3d spatialTangent =
 2300642                ExactContactLever2D.ToSpatial(contact.Tangent);
 2300643            Fixed64 tangentVelocity = default;
 2300644            bool tangentVelocityResolved =
 2300645                ContactResponseArithmetic3D.TryGetRelativePointVelocity(
 2300646                    ExactContactLever2D.ToSpatial(
 2300647                        ResolveLinearVelocity(contact.A.Body)),
 2300648                    new Vector3d(
 2300649                        Fixed64.Zero,
 2300650                        -ResolveAngularVelocity(contact.A.Body),
 2300651                        Fixed64.Zero),
 2300652                    ExactContactLever2D.ToSpatial(contact.RelativeA.Vector),
 2300653                    ExactContactLever2D.ToSpatial(
 2300654                        ResolveLinearVelocity(contact.B.Body)),
 2300655                    new Vector3d(
 2300656                        Fixed64.Zero,
 2300657                        -ResolveAngularVelocity(contact.B.Body),
 2300658                        Fixed64.Zero),
 2300659                    ExactContactLever2D.ToSpatial(contact.RelativeB.Vector),
 2300660                    spatialTangent,
 2300661                    out Vector3d relativeVelocity)
 2300662                && ContactResponseArithmetic3D.TryDot(
 2300663                    relativeVelocity,
 2300664                    spatialTangent,
 2300665                    out tangentVelocity);
 2300666            bool denominatorResolved =
 2300667                ContactNormalImpulse2D.TryComputeDenominator(
 2300668                    contact.A.Body,
 2300669                    contact.RelativeA.Vector,
 2300670                    contact.B.Body,
 2300671                    contact.RelativeB.Vector,
 2300672                    contact.Tangent,
 2300673                    out Fixed64 denominator);
 2300674            if (!(tangentVelocityResolved & denominatorResolved))
 675            {
 3676                return TrySolveFrictionImpulseExact(
 3677                    pair,
 3678                    contact,
 3679                    responsePositionA,
 3680                    responsePositionB,
 3681                    normalLinearVelocityA,
 3682                    normalAngularVelocityA,
 3683                    normalLinearVelocityB,
 3684                    normalAngularVelocityB,
 3685                    restitutionVelocityThreshold,
 3686                    contactShare,
 3687                    out accumulated, impact);
 688            }
 2297689            if (tangentVelocity.Abs() > Fixed64.Epsilon
 2297690                && denominator > Fixed64.Zero
 2297691                && !Fixed64.TryMultiplyDivide(
 2297692                    -tangentVelocity,
 2297693                    Fixed64.One,
 2297694                    denominator,
 2297695                    out impulseScalar))
 696            {
 1697                return TrySolveFrictionImpulseExact(
 1698                    pair,
 1699                    contact,
 1700                    responsePositionA,
 1701                    responsePositionB,
 1702                    normalLinearVelocityA,
 1703                    normalAngularVelocityA,
 1704                    normalLinearVelocityB,
 1705                    normalAngularVelocityB,
 1706                    restitutionVelocityThreshold,
 1707                    contactShare,
 1708                    out accumulated, impact);
 709            }
 710        }
 711
 16346712        if (!Fixed64.TryAdd(
 16346713                contact.CachedTangentImpulse,
 16346714                impulseScalar,
 16346715                out Fixed64 desiredAccumulated))
 716        {
 3717            return TrySolveFrictionImpulseExact(
 3718                pair,
 3719                contact,
 3720                responsePositionA,
 3721                responsePositionB,
 3722                normalLinearVelocityA,
 3723                normalAngularVelocityA,
 3724                normalLinearVelocityB,
 3725                normalAngularVelocityB,
 3726                restitutionVelocityThreshold,
 3727                contactShare,
 3728                out accumulated, impact);
 729        }
 16343730        accumulated = desiredAccumulated >= -staticFrictionLimit
 16343731            && desiredAccumulated <= staticFrictionLimit
 16343732            ? desiredAccumulated
 16343733            : FixedMath.Clamp(
 16343734                desiredAccumulated,
 16343735                -dynamicFrictionLimit,
 16343736                dynamicFrictionLimit);
 16343737        if (!Fixed64.TrySubtract(
 16343738                accumulated,
 16343739                contact.CachedTangentImpulse,
 16343740                out impulseScalar))
 741        {
 2742            return TrySolveFrictionImpulseExact(
 2743                pair,
 2744                contact,
 2745                responsePositionA,
 2746                responsePositionB,
 2747                normalLinearVelocityA,
 2748                normalAngularVelocityA,
 2749                normalLinearVelocityB,
 2750                normalAngularVelocityB,
 2751                restitutionVelocityThreshold,
 2752                contactShare,
 2753                out accumulated, impact);
 754        }
 16341755        return impulseScalar == Fixed64.Zero
 16341756            || TryApplyContactImpulseCombination(
 16341757                pair,
 16341758                contact,
 16341759                responsePositionA,
 16341760                responsePositionB,
 16341761                contact.Normal,
 16341762                Fixed64.Zero,
 16341763                contact.Tangent,
 16341764                impulseScalar);
 765    }
 766
 767    [MethodImpl(MethodImplOptions.NoInlining)]
 768    private static bool TrySolveFrictionImpulseExact(
 769        CollisionPair2D pair,
 770        SolverContact2D contact,
 771        Vector2d responsePositionA,
 772        Vector2d responsePositionB,
 773        Vector2d normalLinearVelocityA,
 774        Fixed64 normalAngularVelocityA,
 775        Vector2d normalLinearVelocityB,
 776        Fixed64 normalAngularVelocityB,
 777        Fixed64 restitutionVelocityThreshold,
 778        Fixed64 contactShare,
 779        out Fixed64 accumulated,
 780        in ContactResponseSnapshot impact = default)
 781    {
 61782        accumulated = default;
 61783        GetExactLevers(
 61784            pair,
 61785            contact,
 61786            responsePositionA,
 61787            responsePositionB,
 61788            out ExactLever3D exactA,
 61789            out ExactLever3D exactB);
 61790        Vector3d spatialNormal =
 61791            ExactContactLever2D.ToSpatial(contact.Normal);
 61792        var normalConstraint = new ExactNormalConstraint3D(
 61793            ExactContactLever2D.CreateResponseOperand(
 61794                contact.A.Body,
 61795                normalLinearVelocityA,
 61796                normalAngularVelocityA,
 61797                exactA,
 61798                -spatialNormal),
 61799            ExactContactLever2D.CreateResponseOperand(
 61800                contact.B.Body,
 61801                normalLinearVelocityB,
 61802                normalAngularVelocityB,
 61803                exactB,
 61804                spatialNormal),
 61805            spatialNormal,
 61806            contact.Restitution,
 61807            restitutionVelocityThreshold,
 61808            contact.CachedNormalImpulse,
 61809            contactShare,
 61810            contactShare);
 61811        Vector3d spatialTangent =
 61812            ExactContactLever2D.ToSpatial(contact.Tangent);
 61813        if (!ExactContactResponseKernel.TryGetCoulombLineResponse(
 61814                normalConstraint,
 61815                ExactContactLever2D.CreateResponseOperand(
 61816                    contact.A.Body,
 61817                    ResolveLinearVelocity(contact.A.Body),
 61818                    ResolveAngularVelocity(contact.A.Body),
 61819                    exactA,
 61820                    -spatialTangent),
 61821                ExactContactLever2D.CreateResponseOperand(
 61822                    contact.B.Body,
 61823                    ResolveLinearVelocity(contact.B.Body),
 61824                    ResolveAngularVelocity(contact.B.Body),
 61825                    exactB,
 61826                    spatialTangent),
 61827                spatialTangent,
 61828                contact.CachedTangentImpulse,
 61829                contact.StaticFriction,
 61830                contact.DynamicFriction,
 61831                out ExactCoulombResponse3D response, impact))
 832        {
 4833            return false;
 834        }
 835
 57836        accumulated = response.TryGetPrimaryAccumulatedImpulse(
 57837                out Fixed64 projectedAccumulated)
 57838            ? projectedAccumulated
 57839            : Fixed64.Zero;
 57840        return !response.HasAppliedImpulse
 57841            || TryApplyVelocityDeltas(
 57842                contact,
 57843                ExactContactLever2D.ToPlanar(
 57844                    response.FirstLinearVelocityDelta),
 57845                ExactContactLever2D.ToPlanarAngular(
 57846                    response.FirstAngularVelocityDelta),
 57847                ExactContactLever2D.ToPlanar(
 57848                    response.SecondLinearVelocityDelta),
 57849                ExactContactLever2D.ToPlanarAngular(
 57850                    response.SecondAngularVelocityDelta));
 851    }
 852
 853    private static bool CanUseCompactAxisResponse(
 854        SolverContact2D contact,
 855        Vector2d axis) =>
 32491856        ExactContactLever2D.CanUseCompactResponse(
 32491857            contact.A.Body,
 32491858            ResolveLinearVelocity(contact.A.Body),
 32491859            ResolveAngularVelocity(contact.A.Body),
 32491860            contact.RelativeA.Vector,
 32491861            contact.B.Body,
 32491862            ResolveLinearVelocity(contact.B.Body),
 32491863            ResolveAngularVelocity(contact.B.Body),
 32491864            contact.RelativeB.Vector,
 32491865            axis);
 866
 867    private static bool TryApplyContactImpulseCombination(
 868        CollisionPair2D pair,
 869        SolverContact2D contact,
 870        Vector2d responsePositionA,
 871        Vector2d responsePositionB,
 872        Vector2d firstAxis,
 873        Fixed64 firstScale,
 874        Vector2d secondAxis,
 875        Fixed64 secondScale)
 876    {
 311877        if (!contact.RelativeA.IsExact
 311878            && !contact.RelativeB.IsExact
 311879            && Vector3d.TryLinearCombination(
 311880                ExactContactLever2D.ToSpatial(firstAxis),
 311881                firstScale,
 311882                ExactContactLever2D.ToSpatial(secondAxis),
 311883                secondScale,
 311884                Vector3d.Zero,
 311885                Fixed64.Zero,
 311886                out Vector3d spatialImpulse)
 311887            && TryApplyCompactImpulse(
 311888                contact,
 311889                ExactContactLever2D.ToPlanar(spatialImpulse)))
 890        {
 307891            return true;
 892        }
 893
 4894        GetExactLevers(
 4895            pair,
 4896            contact,
 4897            responsePositionA,
 4898            responsePositionB,
 4899            out ExactLever3D exactA,
 4900            out ExactLever3D exactB);
 4901        return ExactContactLever2D.TryGetImpulseVelocityDeltas(
 4902                contact.A.Body,
 4903                exactA,
 4904                contact.B.Body,
 4905                exactB,
 4906                firstAxis,
 4907                firstScale,
 4908                secondAxis,
 4909                secondScale,
 4910                out Vector2d linearA,
 4911                out Fixed64 angularA,
 4912                out Vector2d linearB,
 4913                out Fixed64 angularB)
 4914            && TryApplyVelocityDeltas(
 4915                contact,
 4916                linearA,
 4917                angularA,
 4918                linearB,
 4919                angularB);
 920    }
 921
 922    private static bool TryApplyCompactImpulse(
 923        SolverContact2D contact,
 924        Vector2d impulseB)
 925    {
 310926        Vector2d impulseA = -impulseB;
 310927        bool linearAResolved = TryGetLinearVelocityDelta(
 310928            contact.A,
 310929            impulseA,
 310930            out Vector2d linearA);
 310931        bool angularAResolved = TryGetAngularVelocityDelta(
 310932            contact.A,
 310933            contact.RelativeA.Vector,
 310934            impulseA,
 310935            out Fixed64 angularA);
 310936        bool linearBResolved = TryGetLinearVelocityDelta(
 310937            contact.B,
 310938            impulseB,
 310939            out Vector2d linearB);
 310940        bool angularBResolved = TryGetAngularVelocityDelta(
 310941            contact.B,
 310942            contact.RelativeB.Vector,
 310943            impulseB,
 310944            out Fixed64 angularB);
 310945        return linearAResolved
 310946            & angularAResolved
 310947            & linearBResolved
 310948            & angularBResolved
 310949            && TryApplyVelocityDeltas(
 310950                contact,
 310951                linearA,
 310952                angularA,
 310953                linearB,
 310954                angularB);
 955    }
 956
 957    private static bool TryGetLinearVelocityDelta(
 958        ResponseBody2D body,
 959        Vector2d impulse,
 960        out Vector2d velocityDelta)
 961    {
 620962        if (!body.CanTranslate)
 963        {
 296964            velocityDelta = Vector2d.Zero;
 296965            return true;
 966        }
 967
 324968        return ContinuousCollisionImpulsePolicy.TryResolveVelocityDelta(
 324969            body.Body!.ProjectLinearMotion(impulse),
 324970            Fixed64.One,
 324971            body.InverseMass,
 324972            Fixed64.One,
 324973            out velocityDelta);
 974    }
 975
 976    private static bool TryGetAngularVelocityDelta(
 977        ResponseBody2D body,
 978        Vector2d relativeContactPoint,
 979        Vector2d impulse,
 980        out Fixed64 velocityDelta)
 981    {
 620982        velocityDelta = Fixed64.Zero;
 620983        if (!body.CanRotate)
 353984            return true;
 985
 267986        return ContactResponseArithmetic3D.TryCross(
 267987                ExactContactLever2D.ToSpatial(relativeContactPoint),
 267988                ExactContactLever2D.ToSpatial(impulse),
 267989                out Vector3d torque)
 267990            && Fixed64.TryMultiplyDivide(
 267991                -torque.Y,
 267992                body.InverseMoment,
 267993                Fixed64.One,
 267994                out velocityDelta);
 995    }
 996
 997    private static bool TryApplyVelocityDeltas(
 998        SolverContact2D contact,
 999        Vector2d linearA,
 1000        Fixed64 angularA,
 1001        Vector2d linearB,
 1002        Fixed64 angularB)
 1003    {
 7691004        bool firstFits = contact.A.Body?.CanApplyCollisionVelocityDeltas(
 7691005                linearA,
 7691006                angularA)
 7691007            ?? true;
 7691008        bool secondFits = contact.B.Body?.CanApplyCollisionVelocityDeltas(
 7691009                linearB,
 7691010                angularB)
 7691011            ?? true;
 7691012        if (!(firstFits & secondFits))
 21013            return false;
 1014
 7671015        ApplyVelocityDelta(contact.A, linearA, angularA);
 7671016        ApplyVelocityDelta(contact.B, linearB, angularB);
 7671017        return true;
 1018    }
 1019
 1020    [MethodImpl(MethodImplOptions.AggressiveInlining)]
 1021    private static Vector2d ResolveLinearVelocity(SolidBody2D? body) =>
 993801022        body == null
 993801023            ? Vector2d.Zero
 993801024            : body.ProjectLinearMotion(
 993801025                body.IsKinematic
 993801026                    ? body.SampleContinuousCollisionLinearVelocity(Fixed64.One)
 993801027                    : body.LinearVelocity);
 1028
 1029    [MethodImpl(MethodImplOptions.AggressiveInlining)]
 1030    private static Fixed64 ResolveAngularVelocity(SolidBody2D? body) =>
 993801031        body == null
 993801032            ? Fixed64.Zero
 993801033            : body.IsKinematic
 993801034                ? body.SampleContinuousCollisionAngularVelocity(Fixed64.One)
 993801035                : body.AngularVelocity;
 1036
 1037    private static Vector2d ResolveContactNormal(CollisionPair2D pair,
 1038        in ContactGroupKey group, Vector2d normal) =>
 190871039        ResolveContactNormal(normal, group.SourceIdA != 0 || group.SourceIdB != 0
 190871040            ? Vector2d.Zero : pair.ColliderB.Center - pair.ColliderA.Center);
 1041
 1042    private static Vector2d ResolveContactNormal(Vector2d normal, Vector2d fallbackDirection)
 1043    {
 190871044        Vector2d resolved = normal.MagnitudeSquared > Fixed64.Epsilon
 190871045            ? normal.Normalized
 190871046            : fallbackDirection.MagnitudeSquared > Fixed64.Epsilon
 190871047                ? fallbackDirection.Normalized
 190871048                : Vector2d.Zero;
 1049
 190871050        if (resolved == Vector2d.Zero)
 61051            return resolved;
 1052
 190811053        return fallbackDirection.MagnitudeSquared > Fixed64.Epsilon
 190811054            && Vector2d.Dot(resolved, fallbackDirection) < Fixed64.Zero
 190811055                ? -resolved
 190811056                : resolved;
 1057    }
 1058
 1059    private static void GetExactLevers(
 1060        CollisionPair2D pair,
 1061        SolverContact2D contact,
 1062        Vector2d responsePositionA,
 1063        Vector2d responsePositionB,
 1064        out ExactLever3D exactA,
 1065        out ExactLever3D exactB)
 1066    {
 4331067        ManifoldContact2D manifoldContact =
 4331068            pair.Manifold.GetGroup(contact.GroupIndex)[contact.PointIndex];
 4331069        ContactAnchor2D zero =
 4331070            ContactAnchor2D.FromWorldPoint(Vector2d.Zero);
 4331071        exactA = contact.A.Body == null
 4331072            ? zero.GetXZLeverFrom(zero)
 4331073            : manifoldContact.AnchorA.GetXZLeverFrom(
 4331074                new ContactAnchor2D(
 4331075                    responsePositionA,
 4331076                    contact.A.Body.Rotation,
 4331077                    contact.A.Body.LocalCenterOfMassOffset));
 4331078        exactB = contact.B.Body == null
 4331079            ? zero.GetXZLeverFrom(zero)
 4331080            : manifoldContact.AnchorB.GetXZLeverFrom(
 4331081                new ContactAnchor2D(
 4331082                    responsePositionB,
 4331083                    contact.B.Body.Rotation,
 4331084                    contact.B.Body.LocalCenterOfMassOffset));
 4331085    }
 1086
 1087    [MethodImpl(MethodImplOptions.AggressiveInlining)]
 1088    private static bool HasFailedResponse(
 1089        byte failedResponseMask,
 1090        int contactIndex) =>
 404011091        (failedResponseMask & (1 << contactIndex)) != 0;
 1092
 1093    private static void RejectResponse(
 1094        CollisionPair2D pair,
 1095        SolverContact2D contact,
 1096        int contactIndex,
 1097        ref byte failedResponseMask)
 1098    {
 81099        failedResponseMask |= (byte)(1 << contactIndex);
 81100        pair.RemoveWarmStartImpulse(contact.Group, contact.ContactId);
 81101        GravitasLogger.Channel.Error(
 81102            $"2D contact response is outside the representable velocity domain.");
 81103    }
 1104}

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

#LineLine coverage
 1//=======================================================================
 2// CollisionResponse2D.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 in a two-interface planar support island.</content>
 15public static partial class CollisionResponse2D
 16{
 17    private const int MaxSupportRows = 2 * ContactManifold2D.MaxContactsPerGroup;
 18
 19    internal static void CorrectSeededSupportPositions(CollisionPair2D first, CollisionPair2D second)
 20    {
 8521        if (TryCorrectSeededSupportPositions(first, second))
 8322            return;
 223        CorrectSeededSupportPosition(first);
 224        CorrectSeededSupportPosition(second);
 225    }
 26
 27    internal static bool TryCorrectSeededSupportPositions(CollisionPair2D first, CollisionPair2D second)
 28    {
 9529        if (!TryGetSupportedBodies(first, second, out SolidBody2D lower, out SolidBody2D upper)
 9530            && !TryGetSupportedBodies(second, first, out lower, out upper))
 131            return false;
 32
 9433        Span<Vector3d> lowerAxes = stackalloc Vector3d[2];
 9434        Span<Vector3d> upperAxes = stackalloc Vector3d[2];
 9435        if (!(TryReadTranslationInterface(first, lower, upper, out lowerAxes[0], out upperAxes[0], out Fixed64 firstDept
 9436            & TryReadTranslationInterface(second, lower, upper, out lowerAxes[1], out upperAxes[1], out Fixed64 secondDe
 437            return false;
 9038        if (firstDepth <= PenetrationSlop & secondDepth <= PenetrationSlop)
 139            return true;
 40
 41        // Keep the signed slack of each interface. Correcting one can deepen the other.
 8942        Span<Fixed64> depths = stackalloc Fixed64[] { PenetrationSlop - firstDepth, PenetrationSlop - secondDepth };
 8943        Span<Fixed64> impulses = stackalloc Fixed64[2];
 8944        if (!SmallContactNormalSolver.TrySolveTranslations(lowerAxes, upperAxes,
 8945                lower.EffectiveInverseMass, upper.EffectiveInverseMass, depths, impulses)
 8946            || !(TryPrepareSupportTranslation(lower, lowerAxes, impulses, out Vector2d lowerPosition)
 8947                & TryPrepareSupportTranslation(upper, upperAxes, impulses, out Vector2d upperPosition)))
 348            return false;
 49
 8650        lower.ApplyCollisionPositionState(lowerPosition);
 8651        upper.ApplyCollisionPositionState(upperPosition);
 8652        return true;
 53    }
 54
 55    private static bool TryReadTranslationInterface(CollisionPair2D pair, SolidBody2D lower, SolidBody2D upper,
 56        out Vector3d lowerAxis, out Vector3d upperAxis, out Fixed64 depth)
 57    {
 18858        ref ContactGroup2D group = ref pair.Manifold.GetGroup(0);
 18859        Vector2d normal = ResolveContactNormal(pair, group.Key, group[0].Normal);
 18860        bool lowerIsA = pair.ColliderA.Body == lower;
 18861        var axis = new Vector3d(normal.X, normal.Y, Fixed64.Zero);
 18862        lowerAxis = lowerIsA ? -axis : axis;
 18863        upperAxis = (lowerIsA ? pair.ColliderB.Body : pair.ColliderA.Body) == upper ? -lowerAxis : default;
 18864        depth = group[0].Depth;
 18865        bool compatible = !group[0].DepthIsClamped;
 72466        for (int i = 1; i < group.Count; i++)
 67        {
 17468            ManifoldContact2D row = group[i];
 17469            compatible &= !row.DepthIsClamped & (ResolveContactNormal(pair, group.Key, row.Normal) == normal);
 17470            depth = FixedMath.Max(depth, row.Depth);
 71        }
 18872        return compatible;
 73    }
 74
 75    private static bool TryPrepareSupportTranslation(SolidBody2D body, ReadOnlySpan<Vector3d> axes,
 76        ReadOnlySpan<Fixed64> impulses, out Vector2d position)
 77    {
 17678        bool prepared = Vector3d.TryScaledLinearCombination(axes[0], impulses[0], axes[1], impulses[1],
 17679            default, Fixed64.Zero, body.EffectiveInverseMass * PenetrationCorrectionPercent, out Vector3d delta);
 17680        prepared &= Vector2d.TryAdd(body.Position, new Vector2d(delta.X, delta.Y), out position);
 17681        return prepared && (position == body.Position || body.Collider.TryPrepareBodyPose(position, body.Rotation));
 82    }
 83
 84    private static void CorrectSeededSupportPosition(CollisionPair2D pair)
 85    {
 886        ResponseBody2D a = ResponseBody2D.Create(pair.ColliderA), b = ResponseBody2D.Create(pair.ColliderB);
 487        SolverContactBuffer2D contacts = BuildContactBuffer(pair, a, b,
 488            a.Body?.GetCenterOfMassAnchor() ?? default, b.Body?.GetCenterOfMassAnchor() ?? default, 0);
 489        ApplyPositionCorrections(ref contacts, Fixed64.One / (Fixed64)Math.Max(1, contacts.Count));
 490    }
 91
 92    internal static bool TrySeedSupportedIslandNormals(CollisionPair2D first, CollisionPair2D second)
 93    {
 37694        if (!IsSupportPair(first) | !IsSupportPair(second))
 4495            return false;
 33296        if (!TryGetSupportedBodies(first, second, out SolidBody2D lower, out SolidBody2D upper)
 33297            && !TryGetSupportedBodies(second, first, out lower, out upper))
 298            return false;
 99
 330100        int firstCount = first.Manifold.Count;
 330101        int count = firstCount + second.Manifold.Count;
 330102        Span<SupportNormal> rows = stackalloc SupportNormal[count];
 330103        if (!(TryReadSupportRows(first, lower, rows[..firstCount])
 330104            & TryReadSupportRows(second, lower, rows[firstCount..])))
 242105            return false;
 106
 88107        Span<Fixed64> matrix = stackalloc Fixed64[MaxSupportRows * MaxSupportRows];
 88108        Span<Fixed64> speeds = stackalloc Fixed64[MaxSupportRows];
 88109        Span<Fixed64> impulses = stackalloc Fixed64[MaxSupportRows];
 176110        Vector3d lowerMotion = ReadMotion(lower), upperMotion = ReadMotion(upper);
 856111        for (int i = 0; i < count; i++)
 112        {
 341113            ref SupportNormal row = ref rows[i];
 341114            bool prepared = TryProjectSupportSpeed(row, lowerMotion, upperMotion, out speeds[i]);
 2368115            for (int j = 0; j <= i; j++)
 116            {
 843117                bool terms = Vector3d.TryDot(rows[j].Lower, row.LowerResponse, out Fixed64 low)
 843118                    & Vector3d.TryDot(rows[j].Upper, row.UpperResponse, out Fixed64 high);
 843119                prepared &= terms & Fixed64.TryAdd(low, high, out Fixed64 coefficient);
 843120                matrix[i * count + j] = matrix[j * count + i] = coefficient;
 121            }
 341122            if (!prepared)
 1123                return false;
 124        }
 125
 87126        if (!SmallContactNormalSolver.TrySolve(count, matrix, speeds, impulses)
 87127            || !TryPrepareSupportVelocities(rows, impulses, lowerMotion, upperMotion,
 87128                out Vector3d lowerDelta, out Vector3d upperDelta))
 2129            return false;
 130
 85131        StoreSupportNormals(first, impulses[..firstCount]);
 85132        StoreSupportNormals(second, impulses[firstCount..count]);
 85133        ApplySupportDelta(lower, lowerDelta);
 85134        ApplySupportDelta(upper, upperDelta);
 85135        return true;
 136    }
 137
 752138    private static bool IsSupportPair(CollisionPair2D pair) => pair.CollisionType == CollisionType2D.Convex_Convex
 752139        & pair.Manifold.GroupCount == 1 & !pair.HasRecovery;
 140
 141    private static bool TryGetSupportedBodies(CollisionPair2D support, CollisionPair2D load,
 142        out SolidBody2D lower, out SolidBody2D upper)
 143    {
 706144        lower = upper = null!;
 1412145        SolidBody2D? a = support.ColliderA.Body, b = support.ColliderB.Body;
 706146        SolidBody2D? supported = IsFixedSupport(a) ? b : IsFixedSupport(b) ? a : null;
 706147        if (!HasSupportMobility(supported))
 276148            return false;
 149
 150        // Both contacts belong to the same two-pair island and share the movable lower body.
 151        System.Diagnostics.Debug.Assert(load.ColliderA.Body == supported || load.ColliderB.Body == supported);
 430152        SolidBody2D? loaded = load.ColliderA.Body == supported ? load.ColliderB.Body : load.ColliderA.Body;
 430153        if (!HasSupportMobility(loaded) || loaded!.Mass <= supported!.Mass)
 6154            return false;
 424155        lower = supported;
 424156        upper = loaded;
 424157        return true;
 158    }
 159
 1260160    private static bool IsFixedSupport(SolidBody2D? body) => body == null
 1260161        || (body.IsStatic & (body.LinearVelocity == Vector2d.Zero) & (body.AngularVelocity == Fixed64.Zero));
 162
 1136163    private static bool HasSupportMobility(SolidBody2D? body) => body != null
 1136164        && (body.IsDynamic & body.CanTranslate & body.CanRotate & body.FreezeAxes == BodyFreezeAxes2D.None);
 165
 166    private static bool TryReadSupportRows(CollisionPair2D pair, SolidBody2D lower, Span<SupportNormal> rows)
 167    {
 1320168        ResponseBody2D a = ResponseBody2D.Create(pair.ColliderA), b = ResponseBody2D.Create(pair.ColliderB);
 660169        ContactAnchor2D centerA = a.Body?.GetCenterOfMassAnchor() ?? default;
 660170        ContactAnchor2D centerB = b.Body?.GetCenterOfMassAnchor() ?? default;
 660171        bool lowerIsA = a.Body == lower;
 2006172        for (int i = 0; i < rows.Length; i++)
 173        {
 826174            bool read = TryCreateContact(pair, a, b, centerA, centerB, 0, i, out SolverContact2D contact);
 826175            if (!(read & !contact.RelativeA.IsExact & !contact.RelativeB.IsExact & (contact.Restitution == Fixed64.Zero)
 482176                return false;
 177
 344178            bool prepared = TryReadSupportJacobian(contact.RelativeA.Vector, -contact.Normal, a, out Vector3d rowA, out 
 344179                & TryReadSupportJacobian(contact.RelativeB.Vector, contact.Normal, b, out Vector3d rowB, out Vector3d re
 344180            if (!prepared)
 1181                return false;
 343182            rows[i] = lowerIsA ? new SupportNormal(rowA, rowB, responseA, responseB)
 343183                : new SupportNormal(rowB, rowA, responseB, responseA);
 184        }
 177185        return true;
 186    }
 187
 188    private static bool TryReadSupportJacobian(Vector2d lever, Vector2d direction, ResponseBody2D body,
 189        out Vector3d row, out Vector3d response)
 190    {
 688191        if (body.InverseMass == Fixed64.Zero)
 192        {
 172193            row = response = default;
 172194            return true;
 195        }
 196
 516197        bool valid = Fixed64.TrySubtractProducts(lever.X, direction.Y, lever.Y, direction.X, out Fixed64 torque);
 516198        row = new Vector3d(direction.X, direction.Y, torque);
 516199        valid &= Fixed64.TryMultiplyDivide(direction.X, body.InverseMass, Fixed64.One, out Fixed64 x)
 516200            & Fixed64.TryMultiplyDivide(direction.Y, body.InverseMass, Fixed64.One, out Fixed64 y)
 516201            & Fixed64.TryMultiplyDivide(torque, body.InverseMoment, Fixed64.One, out Fixed64 angular);
 516202        response = new Vector3d(x, y, angular);
 516203        return valid;
 204    }
 205
 206    private static bool TryPrepareSupportVelocities(ReadOnlySpan<SupportNormal> rows, ReadOnlySpan<Fixed64> impulses,
 207        Vector3d lowerMotion, Vector3d upperMotion, out Vector3d lowerDelta, out Vector3d upperDelta)
 208    {
 86209        lowerDelta = upperDelta = default;
 86210        bool valid = true;
 848211        for (int i = 0; i < rows.Length; i++)
 212        {
 338213            valid &= TryAccumulateSupportDelta(lowerDelta, rows[i].LowerResponse, impulses[i], out lowerDelta)
 338214                & TryAccumulateSupportDelta(upperDelta, rows[i].UpperResponse, impulses[i], out upperDelta);
 215        }
 216
 86217        valid &= TryPreparePublishedSupportMotion(lowerMotion, lowerDelta, out Vector3d lowerState)
 86218            & TryPreparePublishedSupportMotion(upperMotion, upperDelta, out Vector3d upperState);
 86219        Fixed64 tolerance = PhysicsEnvironment.DefaultMinSpeed;
 848220        for (int i = 0; i < rows.Length; i++)
 221        {
 338222            valid &= TryProjectSupportSpeed(rows[i], lowerState, upperState, out Fixed64 speed)
 338223                & (speed >= -tolerance) & ((impulses[i] == Fixed64.Zero) | (speed <= tolerance));
 224        }
 86225        return valid;
 226    }
 227
 228    private static bool TryPreparePublishedSupportMotion(Vector3d motion, Vector3d delta, out Vector3d state)
 229    {
 172230        bool fits = Vector3d.TryAdd(motion, delta, out state);
 231        // Applying a nonzero delta refreshes the corresponding body's motion deadband.
 172232        var linear = new Vector2d(state.X, state.Y);
 172233        if ((delta.X != Fixed64.Zero | delta.Y != Fixed64.Zero) && linear.Magnitude <= Fixed64.Epsilon)
 168234            linear = Vector2d.Zero;
 172235        Fixed64 angular = delta.Z != Fixed64.Zero && state.Z.Abs() <= Fixed64.Epsilon ? Fixed64.Zero : state.Z;
 172236        state = new Vector3d(linear.X, linear.Y, angular);
 172237        return fits;
 238    }
 239
 240    private static bool TryProjectSupportSpeed(in SupportNormal row, Vector3d lower, Vector3d upper, out Fixed64 speed)
 241    {
 679242        bool valid = Vector3d.TryDot(row.Lower, lower, out Fixed64 first)
 679243            & Vector3d.TryDot(row.Upper, upper, out Fixed64 second);
 679244        return valid & Fixed64.TryAdd(first, second, out speed);
 245    }
 246
 247    private static bool TryAccumulateSupportDelta(Vector3d sum, Vector3d response, Fixed64 impulse, out Vector3d delta) 
 676248        Vector3d.TryScaledLinearCombination(sum, Fixed64.One, response, impulse, default, Fixed64.Zero, Fixed64.One, out
 249
 176250    private static Vector3d ReadMotion(SolidBody2D body) => new(body.LinearVelocity.X, body.LinearVelocity.Y, body.Angul
 251
 252    private static void ApplySupportDelta(SolidBody2D body, Vector3d delta)
 253    {
 170254        body.ApplyCollisionLinearVelocityDelta(new Vector2d(delta.X, delta.Y));
 170255        body.ApplyCollisionAngularVelocityDelta(delta.Z);
 170256    }
 257
 258    private static void StoreSupportNormals(CollisionPair2D pair, ReadOnlySpan<Fixed64> impulses)
 259    {
 170260        pair.RetainWarmStart();
 170261        ref ContactGroup2D group = ref pair.Manifold.GetGroup(0);
 262        // Friction starts cold on the seeded path. The existing row sweeps solve it against these normal loads.
 1012263        for (int i = 0; i < impulses.Length; i++)
 336264            pair.StoreWarmStartImpulse(group.Key, group[i].ContactId, impulses[i], Fixed64.Zero);
 170265    }
 266
 267    private readonly struct SupportNormal
 268    {
 269        internal SupportNormal(Vector3d lower, Vector3d upper, Vector3d lowerResponse, Vector3d upperResponse)
 270        {
 343271            Lower = lower;
 343272            Upper = upper;
 343273            LowerResponse = lowerResponse;
 343274            UpperResponse = upperResponse;
 343275        }
 276
 277        internal readonly Vector3d Lower, Upper, LowerResponse, UpperResponse;
 278    }
 279}

Methods/Properties

.cctor()
PrepareSolve(Gravitas.CollisionPair2D)
Resolve(Gravitas.CollisionPair2D)
Resolve(Gravitas.CollisionPair2D,System.Boolean,System.Boolean)
Resolve(Gravitas.CollisionPair2D,System.Boolean,System.Boolean,System.Boolean)
ResolveGroups(Gravitas.CollisionPair2D,Gravitas.CollisionHandling.ResponseBody2D,Gravitas.CollisionHandling.ResponseBody2D,System.Boolean,System.Boolean,System.Boolean)
ResolveGroup(Gravitas.CollisionPair2D,Gravitas.CollisionHandling.ResponseBody2D,Gravitas.CollisionHandling.ResponseBody2D,System.Int32,System.Boolean,System.Boolean,Gravitas.CollisionHandling.ContactAnchor2D&,Gravitas.CollisionHandling.ContactAnchor2D&,FixedMathSharp.Vector2d,FixedMathSharp.Vector2d,Gravitas.CollisionHandling.ContactResponseSnapshot&)
IsCorrectionOwner(Gravitas.CollisionPair2D,Gravitas.CollisionHandling.SolverContact2D)
TryCreateBodyPair(Gravitas.CollisionPair2D,Gravitas.CollisionHandling.ResponseBody2D&,Gravitas.CollisionHandling.ResponseBody2D&)
BuildContactBuffer(Gravitas.CollisionPair2D,Gravitas.CollisionHandling.ResponseBody2D,Gravitas.CollisionHandling.ResponseBody2D,Gravitas.CollisionHandling.ContactAnchor2D&,Gravitas.CollisionHandling.ContactAnchor2D&,System.Int32)
TryCreateContact(Gravitas.CollisionPair2D,Gravitas.CollisionHandling.ResponseBody2D,Gravitas.CollisionHandling.ResponseBody2D,Gravitas.CollisionHandling.ContactAnchor2D&,Gravitas.CollisionHandling.ContactAnchor2D&,System.Int32,System.Int32,Gravitas.CollisionHandling.SolverContact2D&)
TryCalculateExactNormalResult(Gravitas.CollisionPair2D,Gravitas.CollisionHandling.SolverContact2D,FixedMathSharp.Vector2d,FixedMathSharp.Vector2d,FixedMathSharp.Vector2d,FixedMathSharp.Fixed64,FixedMathSharp.Vector2d,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,Gravitas.CollisionHandling.ContactNormalImpulseResult2D&,Gravitas.CollisionHandling.ContactResponseSnapshot&)
ApplyPositionCorrections(Gravitas.CollisionHandling.SolverContactBuffer2D&,FixedMathSharp.Fixed64)
ApplyPositionCorrection(Gravitas.CollisionHandling.SolverContact2D,FixedMathSharp.Fixed64)
TryApplyCachedImpulse(Gravitas.CollisionPair2D,Gravitas.CollisionHandling.SolverContact2D,FixedMathSharp.Vector2d,FixedMathSharp.Vector2d)
TryApplyNormalImpulse(Gravitas.CollisionHandling.SolverContact2D,Gravitas.CollisionHandling.ContactNormalImpulseResult2D)
ApplyVelocityDelta(Gravitas.CollisionHandling.ResponseBody2D,FixedMathSharp.Vector2d,FixedMathSharp.Fixed64)
TrySolveFrictionImpulse(Gravitas.CollisionPair2D,Gravitas.CollisionHandling.SolverContact2D,FixedMathSharp.Vector2d,FixedMathSharp.Vector2d,FixedMathSharp.Vector2d,FixedMathSharp.Fixed64,FixedMathSharp.Vector2d,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,Gravitas.CollisionHandling.ContactNormalImpulseResult2D,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64&,Gravitas.CollisionHandling.ContactResponseSnapshot&)
TrySolveFrictionImpulseExact(Gravitas.CollisionPair2D,Gravitas.CollisionHandling.SolverContact2D,FixedMathSharp.Vector2d,FixedMathSharp.Vector2d,FixedMathSharp.Vector2d,FixedMathSharp.Fixed64,FixedMathSharp.Vector2d,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64&,Gravitas.CollisionHandling.ContactResponseSnapshot&)
CanUseCompactAxisResponse(Gravitas.CollisionHandling.SolverContact2D,FixedMathSharp.Vector2d)
TryApplyContactImpulseCombination(Gravitas.CollisionPair2D,Gravitas.CollisionHandling.SolverContact2D,FixedMathSharp.Vector2d,FixedMathSharp.Vector2d,FixedMathSharp.Vector2d,FixedMathSharp.Fixed64,FixedMathSharp.Vector2d,FixedMathSharp.Fixed64)
TryApplyCompactImpulse(Gravitas.CollisionHandling.SolverContact2D,FixedMathSharp.Vector2d)
TryGetLinearVelocityDelta(Gravitas.CollisionHandling.ResponseBody2D,FixedMathSharp.Vector2d,FixedMathSharp.Vector2d&)
TryGetAngularVelocityDelta(Gravitas.CollisionHandling.ResponseBody2D,FixedMathSharp.Vector2d,FixedMathSharp.Vector2d,FixedMathSharp.Fixed64&)
TryApplyVelocityDeltas(Gravitas.CollisionHandling.SolverContact2D,FixedMathSharp.Vector2d,FixedMathSharp.Fixed64,FixedMathSharp.Vector2d,FixedMathSharp.Fixed64)
ResolveLinearVelocity(Gravitas.SolidBody2D)
ResolveAngularVelocity(Gravitas.SolidBody2D)
ResolveContactNormal(Gravitas.CollisionPair2D,Gravitas.CollisionHandling.ContactGroupKey&,FixedMathSharp.Vector2d)
ResolveContactNormal(FixedMathSharp.Vector2d,FixedMathSharp.Vector2d)
GetExactLevers(Gravitas.CollisionPair2D,Gravitas.CollisionHandling.SolverContact2D,FixedMathSharp.Vector2d,FixedMathSharp.Vector2d,Gravitas.CollisionHandling.ExactLever3D&,Gravitas.CollisionHandling.ExactLever3D&)
HasFailedResponse(System.Byte,System.Int32)
RejectResponse(Gravitas.CollisionPair2D,Gravitas.CollisionHandling.SolverContact2D,System.Int32,System.Byte&)
CorrectSeededSupportPositions(Gravitas.CollisionPair2D,Gravitas.CollisionPair2D)
TryCorrectSeededSupportPositions(Gravitas.CollisionPair2D,Gravitas.CollisionPair2D)
TryReadTranslationInterface(Gravitas.CollisionPair2D,Gravitas.SolidBody2D,Gravitas.SolidBody2D,FixedMathSharp.Vector3d&,FixedMathSharp.Vector3d&,FixedMathSharp.Fixed64&)
TryPrepareSupportTranslation(Gravitas.SolidBody2D,System.ReadOnlySpan`1<FixedMathSharp.Vector3d>,System.ReadOnlySpan`1<FixedMathSharp.Fixed64>,FixedMathSharp.Vector2d&)
CorrectSeededSupportPosition(Gravitas.CollisionPair2D)
TrySeedSupportedIslandNormals(Gravitas.CollisionPair2D,Gravitas.CollisionPair2D)
IsSupportPair(Gravitas.CollisionPair2D)
TryGetSupportedBodies(Gravitas.CollisionPair2D,Gravitas.CollisionPair2D,Gravitas.SolidBody2D&,Gravitas.SolidBody2D&)
IsFixedSupport(Gravitas.SolidBody2D)
HasSupportMobility(Gravitas.SolidBody2D)
TryReadSupportRows(Gravitas.CollisionPair2D,Gravitas.SolidBody2D,System.Span`1<Gravitas.CollisionHandling.CollisionResponse2D/SupportNormal>)
TryReadSupportJacobian(FixedMathSharp.Vector2d,FixedMathSharp.Vector2d,Gravitas.CollisionHandling.ResponseBody2D,FixedMathSharp.Vector3d&,FixedMathSharp.Vector3d&)
TryPrepareSupportVelocities(System.ReadOnlySpan`1<Gravitas.CollisionHandling.CollisionResponse2D/SupportNormal>,System.ReadOnlySpan`1<FixedMathSharp.Fixed64>,FixedMathSharp.Vector3d,FixedMathSharp.Vector3d,FixedMathSharp.Vector3d&,FixedMathSharp.Vector3d&)
TryPreparePublishedSupportMotion(FixedMathSharp.Vector3d,FixedMathSharp.Vector3d,FixedMathSharp.Vector3d&)
TryProjectSupportSpeed(Gravitas.CollisionHandling.CollisionResponse2D/SupportNormal&,FixedMathSharp.Vector3d,FixedMathSharp.Vector3d,FixedMathSharp.Fixed64&)
TryAccumulateSupportDelta(FixedMathSharp.Vector3d,FixedMathSharp.Vector3d,FixedMathSharp.Fixed64,FixedMathSharp.Vector3d&)
ReadMotion(Gravitas.SolidBody2D)
ApplySupportDelta(Gravitas.SolidBody2D,FixedMathSharp.Vector3d)
StoreSupportNormals(Gravitas.CollisionPair2D,System.ReadOnlySpan`1<FixedMathSharp.Fixed64>)
.ctor(FixedMathSharp.Vector3d,FixedMathSharp.Vector3d,FixedMathSharp.Vector3d,FixedMathSharp.Vector3d)