< Summary

Line coverage
100%
Covered lines: 1971
Uncovered lines: 0
Coverable lines: 1971
Total lines: 2944
Line coverage: 100%
Branch coverage
100%
Covered branches: 386
Total branches: 386
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: TryGetCoulombLineResponse(...)100%1212100%
File 1: TryGetCoulombDiskResponse(...)100%11100%
File 1: TryGetCoulombDiskResponse(...)100%11100%
File 1: TryGetCoulombDiskResponseRetained(...)100%88100%
File 1: TryGetCoulombDiskResponse(...)100%44100%
File 1: TryGetCoulombDiskResponseCore(...)100%5050100%
File 1: AreCoulombDiskInputsValid(...)100%11100%
File 1: TryGetCompletedNormalAccumulatorRatio(...)100%2020100%
File 1: GetBilateralImpulseRatio(...)100%1010100%
File 1: GetFrictionLimit(...)100%11100%
File 1: AddFixedToRatio(...)100%44100%
File 1: SubtractFixedFromRatio(...)100%44100%
File 1: CompareRatios(...)100%11100%
File 1: IsVectorWithinLimit(...)100%11100%
File 1: AddSignedMagnitudes(...)100%44100%
File 1: TryGetSignedRatioOverSquareRoot(...)100%1414100%
File 1: CompareRatioOverSquareRoot(...)100%11100%
File 1: CompareRatioOverSquareRoot(...)100%11100%
File 1: HaveMatchingParticipants(...)100%11100%
File 2: AddCachedTangentToRatio(...)100%88100%
File 2: SubtractCachedTangentFromRatio(...)100%88100%
File 2: RoundCachedRatio(...)100%66100%
File 2: RoundCachedRadialRatio(...)100%1212100%
File 2: IsRadialProjectionEqualToFixed(...)100%2020100%
File 2: TryGetSignedRadicalAndRationalSum(...)100%2424100%
File 2: TryGetSignedSumIntegerParts(...)100%66100%
File 2: GetRadialQuotientBitBound(...)100%11100%
File 2: ComparePreparedSignedSum(...)100%11100%
File 2: GetWeightedFixedSum(...)100%44100%
File 2: GetWeightedSignedWideSum(...)100%44100%
File 2: GetProductSign(...)100%11100%
File 2: TrimMagnitude(...)100%11100%
File 2: TryGetDiskVelocityDeltas(...)100%11100%
File 2: TryGetDiskLinearVelocityDelta(...)100%22100%
File 2: TryGetDiskLinearComponent(...)100%1414100%
File 2: TryGetDiskAngularVelocityDelta(...)100%22100%
File 2: TryGetDiskAngularComponent(...)100%88100%
File 2: GetWeightedSignedSum(...)100%88100%
File 3: TryGetNormalResponse(...)100%11100%
File 3: TryGetAccumulatedNormalResponse(...)100%11100%
File 3: TryGetAccumulatedNormalResponse(...)100%11100%
File 3: TryGetNormalResponseCore(...)100%6464100%
File 3: CreateZeroResponse(...)100%66100%
File 3: GetEffectiveMassRatio(...)100%88100%
File 3: BuildImpactImpulseRatio(...)100%1212100%
File 3: BuildImpulseRatio(...)100%11100%
File 3: ResolveUnilateralImpulse(...)100%2020100%
File 3: TryGetLinearVelocityDelta(...)100%88100%
File 3: TryGetLinearComponent(...)100%22100%
File 3: TryGetAngularVelocityDelta(...)100%66100%
File 3: TryGetAngularComponent(...)100%22100%
File 3: GetRawDot(...)100%11100%
File 3: SetNonNegativeRatio(...)100%22100%
File 3: SetUnsignedSum(...)100%11100%
File 3: SetMagnitude(...)100%11100%
File 3: SetMagnitude(...)100%11100%
File 3: SetMagnitude(...)100%11100%
File 3: SetMagnitude(...)100%11100%
File 3: SetMagnitude(...)100%11100%
File 3: Multiply3(...)100%11100%
File 3: Multiply4(...)100%11100%

File(s)

/home/runner/work/Gravitas/Gravitas/src/Gravitas/CollisionHandling/Response/Exact/ExactContactResponseKernel.Coulomb.cs

#LineLine coverage
 1//=======================================================================
 2// ExactContactResponseKernel.Coulomb.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 System.Runtime.CompilerServices;
 10
 11using FixedMathSharp;
 12
 13namespace Gravitas.CollisionHandling;
 14
 15/// <content>
 16/// Owns exact line and disk Coulomb-friction policy and materialization.
 17/// </content>
 18internal static partial class ExactContactResponseKernel
 19{
 20    private const int MaxCoulombWords = MaxResponseWords * 2;
 21    private const int MaxCoulombSquareWords = MaxCoulombWords * 2;
 22    // Normal-response ratios use fewer than 46 active words. A common tangent
 23    // denominator therefore uses fewer than 92, its squared magnitude fewer
 24    // than 185 including carry, and the largest radial comparison fewer than
 25    // 307. The remaining words prevent silent product truncation.
 26    private const int MaxCoulombComparisonWords = 320;
 27
 28    [MethodImpl(MethodImplOptions.NoInlining)]
 29    internal static bool TryGetCoulombLineResponse(
 30        in ExactNormalConstraint3D normalConstraint,
 31        in ExactContactResponseOperand3D firstTangent,
 32        in ExactContactResponseOperand3D secondTangent,
 33        Vector3d tangent,
 34        Fixed64 accumulatedTangentImpulse,
 35        Fixed64 staticFriction,
 36        Fixed64 dynamicFriction,
 37        out ExactCoulombResponse3D response,
 38        in ContactResponseSnapshot impact = default)
 39    {
 9340        response = default;
 9341        bool inputsValid =
 9342            staticFriction >= Fixed64.Zero
 9343            & dynamicFriction >= Fixed64.Zero
 9344            & tangent.IsNormalized();
 9345        if (!inputsValid)
 346            return false;
 47
 9048        bool constraintValid =
 9049            FixedMath.Abs(Vector3d.Dot(
 9050                normalConstraint.Normal,
 9051                tangent)) <= Fixed64.Epsilon
 9052            & HaveMatchingParticipants(
 9053                normalConstraint.First,
 9054                firstTangent)
 9055            & HaveMatchingParticipants(
 9056                normalConstraint.Second,
 9057                secondTangent);
 9058        if (!constraintValid)
 259            return false;
 60
 8861        Span<ulong> normalNumerator = stackalloc ulong[MaxResponseWords];
 8862        Span<ulong> normalDenominator = stackalloc ulong[MaxResponseWords];
 8863        Span<ulong> tangentNumerator = stackalloc ulong[MaxResponseWords];
 8864        Span<ulong> tangentDenominator = stackalloc ulong[MaxResponseWords];
 8865        if (!TryGetCompletedNormalAccumulatorRatio(
 8866                normalConstraint,
 8867                normalNumerator,
 8868                normalDenominator, impact))
 169            return false;
 8770        GetBilateralImpulseRatio(
 8771            firstTangent,
 8772            secondTangent,
 8773            tangent,
 8774            Fixed64.Zero,
 8775            tangentNumerator,
 8776            tangentDenominator,
 8777            out int tangentSign);
 78
 8779        Span<ulong> desiredNumerator = stackalloc ulong[MaxResponseWords];
 8780        AddFixedToRatio(
 8781            tangentNumerator,
 8782            tangentDenominator,
 8783            tangentSign,
 8784            accumulatedTangentImpulse,
 8785            desiredNumerator,
 8786            out int desiredSign);
 87
 8788        Span<ulong> staticNumerator = stackalloc ulong[MaxResponseWords];
 8789        Span<ulong> staticDenominator = stackalloc ulong[MaxResponseWords];
 8790        GetFrictionLimit(
 8791            normalNumerator,
 8792            normalDenominator,
 8793            staticFriction,
 8794            staticNumerator,
 8795            staticDenominator);
 96
 8797        Span<ulong> appliedNumerator = stackalloc ulong[MaxCoulombWords];
 8798        Span<ulong> appliedDenominator = stackalloc ulong[MaxCoulombWords];
 99        int appliedSign;
 87100        Span<ulong> accumulatedNumerator = stackalloc ulong[MaxCoulombWords];
 87101        Span<ulong> accumulatedDenominator = stackalloc ulong[MaxCoulombWords];
 102        int accumulatedSign;
 87103        appliedNumerator.Clear();
 87104        appliedDenominator.Clear();
 87105        accumulatedNumerator.Clear();
 87106        accumulatedDenominator.Clear();
 87107        Span<ulong> clampedNumerator = stackalloc ulong[MaxCoulombWords];
 87108        Span<ulong> clampedDenominator = stackalloc ulong[MaxResponseWords];
 87109        if (CompareRatios(
 87110                desiredNumerator,
 87111                tangentDenominator,
 87112                staticNumerator,
 87113                staticDenominator) <= 0)
 114        {
 68115            tangentNumerator.CopyTo(appliedNumerator);
 68116            tangentDenominator.CopyTo(appliedDenominator);
 68117            appliedSign = tangentSign;
 68118            desiredNumerator.CopyTo(accumulatedNumerator);
 68119            tangentDenominator.CopyTo(accumulatedDenominator);
 68120            accumulatedSign = desiredSign;
 121        }
 122        else
 123        {
 19124            GetFrictionLimit(
 19125                normalNumerator,
 19126                normalDenominator,
 19127                dynamicFriction,
 19128                clampedNumerator,
 19129                clampedDenominator);
 19130            clampedNumerator.CopyTo(accumulatedNumerator);
 19131            clampedDenominator.CopyTo(accumulatedDenominator);
 19132            accumulatedSign = WideArithmetic.IsZeroMagnitude(
 19133                clampedNumerator)
 19134                ? 0
 19135                : desiredSign;
 19136            SubtractFixedFromRatio(
 19137                clampedNumerator,
 19138                clampedDenominator,
 19139                accumulatedSign,
 19140                accumulatedTangentImpulse,
 19141                appliedNumerator,
 19142                appliedDenominator,
 19143                out appliedSign);
 144        }
 145
 87146        bool resolved = TryGetLinearVelocityDelta(
 87147            firstTangent.LinearImpulseAxis,
 87148            firstTangent.InverseMass,
 87149            appliedNumerator,
 87150            appliedDenominator,
 87151            appliedSign,
 87152            out Vector3d firstLinear);
 87153        resolved &= TryGetAngularVelocityDelta(
 87154            firstTangent.Lever,
 87155            -tangent,
 87156            firstTangent.InverseInertia,
 87157            appliedNumerator,
 87158            appliedDenominator,
 87159            appliedSign,
 87160            out Vector3d firstAngular);
 87161        resolved &= TryGetLinearVelocityDelta(
 87162            secondTangent.LinearImpulseAxis,
 87163            secondTangent.InverseMass,
 87164            appliedNumerator,
 87165            appliedDenominator,
 87166            appliedSign,
 87167            out Vector3d secondLinear);
 87168        resolved &= TryGetAngularVelocityDelta(
 87169            secondTangent.Lever,
 87170            tangent,
 87171            secondTangent.InverseInertia,
 87172            appliedNumerator,
 87173            appliedDenominator,
 87174            appliedSign,
 87175            out Vector3d secondAngular);
 87176        resolved &= firstTangent.CanRepresentAngularDelta(firstAngular);
 87177        resolved &= secondTangent.CanRepresentAngularDelta(secondAngular);
 87178        if (!resolved)
 7179            return false;
 180
 80181        bool hasAccumulatedProjection = Fixed64.TryGetSignedRawRatio(
 80182            accumulatedNumerator,
 80183            accumulatedDenominator,
 80184            accumulatedSign < 0,
 80185            out Fixed64 accumulatedProjection);
 80186        response = new ExactCoulombResponse3D(
 80187            !WideArithmetic.IsZeroMagnitude(appliedNumerator),
 80188            firstLinear,
 80189            firstAngular,
 80190            secondLinear,
 80191            secondAngular,
 80192            hasAccumulatedProjection,
 80193            accumulatedProjection,
 80194            hasSecondaryAccumulatedImpulse: false,
 80195            secondaryAccumulatedImpulse: default);
 80196        return true;
 197    }
 198
 199    [MethodImpl(MethodImplOptions.AggressiveInlining)]
 200    internal static bool TryGetCoulombDiskResponse(
 201        Vector3d normal,
 202        Fixed64 completedNormalImpulse,
 203        in ExactContactResponseOperand3D primaryFirst,
 204        in ExactContactResponseOperand3D primarySecond,
 205        Vector3d primaryTangent,
 206        Fixed64 accumulatedPrimaryTangentImpulse,
 207        in ExactContactResponseOperand3D secondaryFirst,
 208        in ExactContactResponseOperand3D secondarySecond,
 209        Vector3d secondaryTangent,
 210        Fixed64 accumulatedSecondaryTangentImpulse,
 211        Fixed64 staticFriction,
 212        Fixed64 dynamicFriction,
 213        out ExactCoulombResponse3D response)
 214    {
 662215        ExactContactImpulseState? unused = null;
 662216        return TryGetCoulombDiskResponseRetained(normal, completedNormalImpulse, primaryFirst, primarySecond, primaryTan
 662217            accumulatedPrimaryTangentImpulse, secondaryFirst, secondarySecond, secondaryTangent,
 662218            accumulatedSecondaryTangentImpulse, staticFriction, dynamicFriction, out response,
 662219            ref unused, retainOversized: false);
 220    }
 221
 222    [MethodImpl(MethodImplOptions.AggressiveInlining)]
 223    internal static bool TryGetCoulombDiskResponse(
 224        Vector3d normal,
 225        Fixed64 completedNormalImpulse,
 226        in ExactContactResponseOperand3D primaryFirst,
 227        in ExactContactResponseOperand3D primarySecond,
 228        Vector3d primaryTangent,
 229        Fixed64 accumulatedPrimaryTangentImpulse,
 230        in ExactContactResponseOperand3D secondaryFirst,
 231        in ExactContactResponseOperand3D secondarySecond,
 232        Vector3d secondaryTangent,
 233        Fixed64 accumulatedSecondaryTangentImpulse,
 234        Fixed64 staticFriction,
 235        Fixed64 dynamicFriction,
 236        out ExactCoulombResponse3D response,
 237        ref ExactContactImpulseState? exactImpulse) =>
 354238        TryGetCoulombDiskResponseRetained(normal, completedNormalImpulse, primaryFirst, primarySecond, primaryTangent,
 354239            accumulatedPrimaryTangentImpulse, secondaryFirst, secondarySecond, secondaryTangent,
 354240            accumulatedSecondaryTangentImpulse, staticFriction, dynamicFriction, out response,
 354241            ref exactImpulse, retainOversized: true);
 242
 243
 244    [MethodImpl(MethodImplOptions.NoInlining)]
 245    private static bool TryGetCoulombDiskResponseRetained(
 246        Vector3d normal,
 247        Fixed64 completedNormalImpulse,
 248        in ExactContactResponseOperand3D primaryFirst,
 249        in ExactContactResponseOperand3D primarySecond,
 250        Vector3d primaryTangent,
 251        Fixed64 accumulatedPrimaryTangentImpulse,
 252        in ExactContactResponseOperand3D secondaryFirst,
 253        in ExactContactResponseOperand3D secondarySecond,
 254        Vector3d secondaryTangent,
 255        Fixed64 accumulatedSecondaryTangentImpulse,
 256        Fixed64 staticFriction,
 257        Fixed64 dynamicFriction,
 258        out ExactCoulombResponse3D response,
 259        ref ExactContactImpulseState? exactImpulse,
 260        bool retainOversized)
 261    {
 1016262        response = default;
 1016263        if (completedNormalImpulse < Fixed64.Zero
 1016264            || !AreCoulombDiskInputsValid(
 1016265                normal,
 1016266                primaryFirst,
 1016267                primarySecond,
 1016268                primaryTangent,
 1016269                secondaryFirst,
 1016270                secondarySecond,
 1016271                secondaryTangent,
 1016272                staticFriction,
 1016273                dynamicFriction))
 274        {
 2275            return false;
 276        }
 277
 1014278        Span<ulong> normalNumerator = stackalloc ulong[MaxResponseWords];
 1014279        Span<ulong> normalDenominator = stackalloc ulong[MaxResponseWords];
 1014280        if (exactImpulse?.HasNormal == true)
 281        {
 10282            exactImpulse.NormalNumerator.CopyTo(normalNumerator);
 10283            exactImpulse.NormalDenominator.CopyTo(normalDenominator);
 284        }
 285        else
 286        {
 1004287            SetMagnitude(completedNormalImpulse, normalNumerator);
 1004288            normalDenominator.Clear();
 1004289            normalDenominator[0] = 1UL;
 290        }
 1014291        return TryGetCoulombDiskResponseCore(
 1014292            normalNumerator,
 1014293            normalDenominator,
 1014294            primaryFirst,
 1014295            primarySecond,
 1014296            primaryTangent,
 1014297            accumulatedPrimaryTangentImpulse,
 1014298            secondaryFirst,
 1014299            secondarySecond,
 1014300            secondaryTangent,
 1014301            accumulatedSecondaryTangentImpulse,
 1014302            staticFriction,
 1014303            dynamicFriction,
 1014304            Fixed64.Epsilon,
 1014305            out response, ref exactImpulse, retainOversized);
 306    }
 307
 308    [MethodImpl(MethodImplOptions.NoInlining)]
 309    internal static bool TryGetCoulombDiskResponse(
 310        in ExactNormalConstraint3D normalConstraint,
 311        in ExactContactResponseOperand3D primaryFirst,
 312        in ExactContactResponseOperand3D primarySecond,
 313        Vector3d primaryTangent,
 314        Fixed64 accumulatedPrimaryTangentImpulse,
 315        in ExactContactResponseOperand3D secondaryFirst,
 316        in ExactContactResponseOperand3D secondarySecond,
 317        Vector3d secondaryTangent,
 318        Fixed64 accumulatedSecondaryTangentImpulse,
 319        Fixed64 staticFriction,
 320        Fixed64 dynamicFriction,
 321        out ExactCoulombResponse3D response)
 322    {
 60323        response = default;
 60324        bool inputsValid = AreCoulombDiskInputsValid(
 60325                normalConstraint.Normal,
 60326                primaryFirst,
 60327                primarySecond,
 60328                primaryTangent,
 60329                secondaryFirst,
 60330                secondarySecond,
 60331                secondaryTangent,
 60332                staticFriction,
 60333                dynamicFriction)
 60334            & HaveMatchingParticipants(
 60335                normalConstraint.First,
 60336                primaryFirst)
 60337            & HaveMatchingParticipants(
 60338                normalConstraint.Second,
 60339                primarySecond)
 60340            & HaveMatchingParticipants(primaryFirst, secondaryFirst)
 60341            & HaveMatchingParticipants(primarySecond, secondarySecond);
 60342        if (!inputsValid)
 3343            return false;
 344
 57345        Span<ulong> normalNumerator = stackalloc ulong[MaxResponseWords];
 57346        Span<ulong> normalDenominator = stackalloc ulong[MaxResponseWords];
 57347        if (!TryGetCompletedNormalAccumulatorRatio(
 57348                normalConstraint,
 57349                normalNumerator,
 57350                normalDenominator))
 1351            return false;
 352
 56353        ExactContactImpulseState? unused = null;
 56354        return TryGetCoulombDiskResponseCore(
 56355            normalNumerator,
 56356            normalDenominator,
 56357            primaryFirst,
 56358            primarySecond,
 56359            primaryTangent,
 56360            accumulatedPrimaryTangentImpulse,
 56361            secondaryFirst,
 56362            secondarySecond,
 56363            secondaryTangent,
 56364            accumulatedSecondaryTangentImpulse,
 56365            staticFriction,
 56366            dynamicFriction,
 56367            Fixed64.Zero,
 56368            out response, ref unused, retainOversized: false);
 369    }
 370
 371    private static bool TryGetCoulombDiskResponseCore(
 372        ReadOnlySpan<ulong> normalNumerator,
 373        ReadOnlySpan<ulong> normalDenominator,
 374        in ExactContactResponseOperand3D primaryFirst,
 375        in ExactContactResponseOperand3D primarySecond,
 376        Vector3d primaryTangent,
 377        Fixed64 accumulatedPrimaryTangentImpulse,
 378        in ExactContactResponseOperand3D secondaryFirst,
 379        in ExactContactResponseOperand3D secondarySecond,
 380        Vector3d secondaryTangent,
 381        Fixed64 accumulatedSecondaryTangentImpulse,
 382        Fixed64 staticFriction,
 383        Fixed64 dynamicFriction,
 384        Fixed64 velocityDeadzone,
 385        out ExactCoulombResponse3D response,
 386        ref ExactContactImpulseState? exactImpulse,
 387        bool retainOversized)
 388    {
 1070389        response = default;
 1070390        bool wideTangents = exactImpulse?.HasTangents == true;
 1070391        int desiredWords = wideTangents ? MaxCoulombWords : MaxResponseWords;
 1070392        Span<ulong> primaryNumerator = stackalloc ulong[MaxResponseWords];
 1070393        Span<ulong> primaryDenominator = stackalloc ulong[MaxResponseWords];
 1070394        Span<ulong> secondaryNumerator = stackalloc ulong[MaxResponseWords];
 1070395        Span<ulong> secondaryDenominator = stackalloc ulong[MaxResponseWords];
 1070396        GetBilateralImpulseRatio(
 1070397            primaryFirst,
 1070398            primarySecond,
 1070399            primaryTangent,
 1070400            velocityDeadzone,
 1070401            primaryNumerator,
 1070402            primaryDenominator,
 1070403            out int primarySign);
 1070404        GetBilateralImpulseRatio(
 1070405            secondaryFirst,
 1070406            secondarySecond,
 1070407            secondaryTangent,
 1070408            velocityDeadzone,
 1070409            secondaryNumerator,
 1070410            secondaryDenominator,
 1070411            out int secondarySign);
 412
 1070413        Span<ulong> desiredPrimaryNumerator =
 1070414            stackalloc ulong[desiredWords];
 1070415        Span<ulong> desiredSecondaryNumerator =
 1070416            stackalloc ulong[desiredWords];
 1070417        AddCachedTangentToRatio(
 1070418            primaryNumerator,
 1070419            primaryDenominator,
 1070420            primarySign,
 1070421            accumulatedPrimaryTangentImpulse, exactImpulse, primary: true,
 1070422            desiredPrimaryNumerator,
 1070423            out int desiredPrimarySign);
 1070424        AddCachedTangentToRatio(
 1070425            secondaryNumerator,
 1070426            secondaryDenominator,
 1070427            secondarySign,
 1070428            accumulatedSecondaryTangentImpulse, exactImpulse, primary: false,
 1070429            desiredSecondaryNumerator,
 1070430            out int desiredSecondarySign);
 431
 1070432        Span<ulong> commonDenominator = stackalloc ulong[MaxCoulombWords];
 1070433        Span<ulong> primaryAtCommon = stackalloc ulong[MaxCoulombWords];
 1070434        Span<ulong> secondaryAtCommon = stackalloc ulong[MaxCoulombWords];
 1070435        WideArithmetic.MultiplyMagnitudes(
 1070436            primaryDenominator,
 1070437            secondaryDenominator,
 1070438            commonDenominator);
 1070439        WideArithmetic.MultiplyMagnitudes(
 1070440            desiredPrimaryNumerator,
 1070441            secondaryDenominator,
 1070442            primaryAtCommon);
 1070443        WideArithmetic.MultiplyMagnitudes(
 1070444            desiredSecondaryNumerator,
 1070445            primaryDenominator,
 1070446            secondaryAtCommon);
 447
 1070448        Span<ulong> magnitudeSquared = stackalloc ulong[MaxCoulombSquareWords];
 1070449        Span<ulong> square = stackalloc ulong[MaxCoulombSquareWords];
 1070450        WideArithmetic.MultiplyMagnitudes(
 1070451            primaryAtCommon,
 1070452            primaryAtCommon,
 1070453            magnitudeSquared);
 1070454        WideArithmetic.MultiplyMagnitudes(
 1070455            secondaryAtCommon,
 1070456            secondaryAtCommon,
 1070457            square);
 1070458        WideArithmetic.AddMagnitudeInto(square, magnitudeSquared);
 459
 1070460        Span<ulong> staticNumerator = stackalloc ulong[MaxResponseWords];
 1070461        Span<ulong> staticDenominator = stackalloc ulong[MaxResponseWords];
 1070462        GetFrictionLimit(
 1070463            normalNumerator,
 1070464            normalDenominator,
 1070465            staticFriction,
 1070466            staticNumerator,
 1070467            staticDenominator);
 1070468        bool withinStaticLimit = IsVectorWithinLimit(
 1070469            magnitudeSquared,
 1070470            commonDenominator,
 1070471            staticNumerator,
 1070472            staticDenominator);
 473
 1070474        Span<ulong> dynamicNumerator = stackalloc ulong[MaxResponseWords];
 1070475        Span<ulong> dynamicDenominator = stackalloc ulong[MaxResponseWords];
 1070476        if (!withinStaticLimit)
 477        {
 87478            GetFrictionLimit(
 87479                normalNumerator,
 87480                normalDenominator,
 87481                dynamicFriction,
 87482                dynamicNumerator,
 87483                dynamicDenominator);
 484        }
 485
 1070486        bool useDynamicProjection =
 1070487            !withinStaticLimit
 1070488            && !WideArithmetic.IsZeroMagnitude(magnitudeSquared)
 1070489            && !WideArithmetic.IsZeroMagnitude(dynamicNumerator);
 490        bool hasAppliedImpulse;
 491        bool resolved;
 492        Vector3d firstLinear;
 493        Vector3d firstAngular;
 494        Vector3d secondLinear;
 495        Vector3d secondAngular;
 1070496        if (useDynamicProjection)
 497        {
 56498            hasAppliedImpulse =
 56499                !IsRadialProjectionEqualToFixed(
 56500                    primaryAtCommon,
 56501                    desiredPrimarySign,
 56502                    dynamicNumerator,
 56503                    dynamicDenominator,
 56504                    magnitudeSquared,
 56505                    accumulatedPrimaryTangentImpulse, exactImpulse, primary: true)
 56506                || !IsRadialProjectionEqualToFixed(
 56507                    secondaryAtCommon,
 56508                    desiredSecondarySign,
 56509                    dynamicNumerator,
 56510                    dynamicDenominator,
 56511                    magnitudeSquared,
 56512                    accumulatedSecondaryTangentImpulse, exactImpulse, primary: false);
 56513            resolved = TryGetDiskVelocityDeltas(
 56514                primaryFirst,
 56515                primarySecond,
 56516                secondaryFirst,
 56517                secondarySecond,
 56518                primaryTangent,
 56519                secondaryTangent,
 56520                primaryAtCommon,
 56521                desiredPrimarySign,
 56522                secondaryAtCommon,
 56523                desiredSecondarySign,
 56524                commonDenominator,
 56525                rational: false,
 56526                dynamicNumerator,
 56527                dynamicDenominator,
 56528                magnitudeSquared,
 56529                accumulatedPrimaryTangentImpulse,
 56530                accumulatedSecondaryTangentImpulse,
 56531                out firstLinear,
 56532                out firstAngular,
 56533                out secondLinear,
 56534                out secondAngular, exactImpulse);
 535        }
 536        else
 537        {
 1014538            Span<ulong> zero = stackalloc ulong[MaxCoulombWords];
 1014539            Span<ulong> appliedPrimaryNumerator =
 1014540                stackalloc ulong[MaxCoulombWords];
 1014541            Span<ulong> appliedDenominator =
 1014542                stackalloc ulong[MaxCoulombWords];
 1014543            Span<ulong> appliedSecondaryNumerator =
 1014544                stackalloc ulong[MaxCoulombWords];
 1014545            zero.Clear();
 1014546            SubtractCachedTangentFromRatio(
 1014547                withinStaticLimit ? primaryAtCommon : zero,
 1014548                commonDenominator,
 1014549                withinStaticLimit ? desiredPrimarySign : 0,
 1014550                accumulatedPrimaryTangentImpulse, exactImpulse, primary: true,
 1014551                appliedPrimaryNumerator,
 1014552                appliedDenominator,
 1014553                out int appliedPrimarySign);
 1014554            SubtractCachedTangentFromRatio(
 1014555                withinStaticLimit ? secondaryAtCommon : zero,
 1014556                commonDenominator,
 1014557                withinStaticLimit ? desiredSecondarySign : 0,
 1014558                accumulatedSecondaryTangentImpulse, exactImpulse, primary: false,
 1014559                appliedSecondaryNumerator,
 1014560                appliedDenominator,
 1014561                out int appliedSecondarySign);
 1014562            hasAppliedImpulse =
 1014563                appliedPrimarySign != 0
 1014564                || appliedSecondarySign != 0;
 1014565            resolved = TryGetDiskVelocityDeltas(
 1014566                primaryFirst,
 1014567                primarySecond,
 1014568                secondaryFirst,
 1014569                secondarySecond,
 1014570                primaryTangent,
 1014571                secondaryTangent,
 1014572                appliedPrimaryNumerator,
 1014573                appliedPrimarySign,
 1014574                appliedSecondaryNumerator,
 1014575                appliedSecondarySign,
 1014576                appliedDenominator,
 1014577                rational: true,
 1014578                dynamicNumerator,
 1014579                dynamicDenominator,
 1014580                magnitudeSquared,
 1014581                Fixed64.Zero,
 1014582                Fixed64.Zero,
 1014583                out firstLinear,
 1014584                out firstAngular,
 1014585                out secondLinear,
 1014586                out secondAngular, exactImpulse);
 587        }
 1070588        resolved &= primaryFirst.CanRepresentAngularDelta(firstAngular);
 1070589        resolved &= primarySecond.CanRepresentAngularDelta(secondAngular);
 1070590        if (!resolved)
 8591            return false;
 592
 593        bool hasPrimaryProjection;
 594        bool hasSecondaryProjection;
 595        Fixed64 primaryProjection;
 596        Fixed64 secondaryProjection;
 1062597        if (withinStaticLimit)
 598        {
 980599            hasPrimaryProjection = Fixed64.TryGetSignedRawRatio(
 980600                primaryAtCommon,
 980601                commonDenominator,
 980602                desiredPrimarySign < 0,
 980603                out primaryProjection);
 980604            hasSecondaryProjection = Fixed64.TryGetSignedRawRatio(
 980605                secondaryAtCommon,
 980606                commonDenominator,
 980607                desiredSecondarySign < 0,
 980608                out secondaryProjection);
 609        }
 82610        else if (useDynamicProjection)
 611        {
 53612            Span<ulong> projectedNumerator =
 53613                stackalloc ulong[MaxCoulombWords + MaxResponseWords];
 53614            WideArithmetic.MultiplyMagnitudes(
 53615                primaryAtCommon,
 53616                dynamicNumerator,
 53617                projectedNumerator);
 53618            hasPrimaryProjection = TryGetSignedRatioOverSquareRoot(
 53619                projectedNumerator,
 53620                dynamicDenominator,
 53621                magnitudeSquared,
 53622                desiredPrimarySign < 0,
 53623                out primaryProjection);
 53624            WideArithmetic.MultiplyMagnitudes(
 53625                secondaryAtCommon,
 53626                dynamicNumerator,
 53627                projectedNumerator);
 53628            hasSecondaryProjection = TryGetSignedRatioOverSquareRoot(
 53629                projectedNumerator,
 53630                dynamicDenominator,
 53631                magnitudeSquared,
 53632                desiredSecondarySign < 0,
 53633                out secondaryProjection);
 634        }
 635        else
 636        {
 29637            hasPrimaryProjection = true;
 29638            hasSecondaryProjection = true;
 29639            primaryProjection = Fixed64.Zero;
 29640            secondaryProjection = Fixed64.Zero;
 641        }
 642
 1062643        if (retainOversized
 1062644            && (!hasPrimaryProjection || !hasSecondaryProjection || wideTangents))
 645        {
 29646            if (!Vector3d.TryAdd(primaryFirst.LinearVelocity, firstLinear, out _)
 29647                || !primaryFirst.CanApplyAngularDelta(firstAngular)
 29648                || !Vector3d.TryAdd(primarySecond.LinearVelocity, secondLinear, out _)
 34649                || !primarySecond.CanApplyAngularDelta(secondAngular)) return false;
 24650            Span<ulong> widePrimary = stackalloc ulong[MaxResponseWords];
 24651            Span<ulong> wideSecondary = stackalloc ulong[MaxResponseWords];
 24652            if (withinStaticLimit)
 653            {
 12654                RoundCachedRatio(primaryAtCommon, commonDenominator, widePrimary);
 12655                RoundCachedRatio(secondaryAtCommon, commonDenominator, wideSecondary);
 656            }
 12657            else if (useDynamicProjection)
 658            {
 8659                Span<ulong> projected = stackalloc ulong[MaxCoulombWords + MaxResponseWords];
 8660                WideArithmetic.MultiplyMagnitudes(primaryAtCommon, dynamicNumerator, projected);
 8661                RoundCachedRadialRatio(projected, dynamicDenominator, magnitudeSquared, widePrimary);
 8662                WideArithmetic.MultiplyMagnitudes(secondaryAtCommon, dynamicNumerator, projected);
 8663                RoundCachedRadialRatio(projected, dynamicDenominator, magnitudeSquared, wideSecondary);
 664            }
 665            else
 666            {
 4667                widePrimary.Clear();
 4668                wideSecondary.Clear();
 669            }
 24670            (exactImpulse ??= new ExactContactImpulseState()).SetTangents(
 24671                widePrimary, desiredPrimarySign, wideSecondary, desiredSecondarySign);
 672        }
 673
 1057674        response = new ExactCoulombResponse3D(
 1057675            hasAppliedImpulse,
 1057676            firstLinear,
 1057677            firstAngular,
 1057678            secondLinear,
 1057679            secondAngular,
 1057680            hasPrimaryProjection,
 1057681            primaryProjection,
 1057682            hasSecondaryProjection,
 1057683            secondaryProjection);
 1057684        return true;
 685    }
 686
 687    private static bool AreCoulombDiskInputsValid(
 688        Vector3d normal,
 689        in ExactContactResponseOperand3D primaryFirst,
 690        in ExactContactResponseOperand3D primarySecond,
 691        Vector3d primaryTangent,
 692        in ExactContactResponseOperand3D secondaryFirst,
 693        in ExactContactResponseOperand3D secondarySecond,
 694        Vector3d secondaryTangent,
 695        Fixed64 staticFriction,
 696        Fixed64 dynamicFriction) =>
 1075697        staticFriction >= Fixed64.Zero
 1075698        & dynamicFriction >= Fixed64.Zero
 1075699        & primaryFirst.Lever.Denominator.Sign != 0
 1075700        & primarySecond.Lever.Denominator.Sign != 0
 1075701        & primaryFirst.InverseMass >= Fixed64.Zero
 1075702        & primarySecond.InverseMass >= Fixed64.Zero
 1075703        & normal.IsNormalized()
 1075704        & primaryTangent.IsNormalized()
 1075705        & secondaryTangent.IsNormalized()
 1075706        & FixedMath.Abs(Vector3d.Dot(
 1075707            primaryTangent,
 1075708            secondaryTangent)) <= Fixed64.Epsilon
 1075709        & FixedMath.Abs(Vector3d.Dot(
 1075710            normal,
 1075711            primaryTangent)) <= Fixed64.Epsilon
 1075712        & FixedMath.Abs(Vector3d.Dot(
 1075713            normal,
 1075714            secondaryTangent)) <= Fixed64.Epsilon
 1075715        & HaveMatchingParticipants(primaryFirst, secondaryFirst)
 1075716        & HaveMatchingParticipants(primarySecond, secondarySecond);
 717
 718    private static bool TryGetCompletedNormalAccumulatorRatio(
 719        in ExactNormalConstraint3D constraint,
 720        Span<ulong> numerator,
 721        Span<ulong> denominator,
 722        in ContactResponseSnapshot impact = default)
 723    {
 145724        numerator.Clear();
 145725        denominator.Clear();
 145726        ExactContactResponseOperand3D first = constraint.First;
 145727        ExactContactResponseOperand3D second = constraint.Second;
 145728        bool inputsValid =
 145729            first.Lever.Denominator.Sign != 0
 145730            & second.Lever.Denominator.Sign != 0
 145731            & constraint.Restitution >= Fixed64.Zero
 145732            & constraint.RestitutionVelocityThreshold >= Fixed64.Zero
 145733            & constraint.AccumulatedImpulse >= Fixed64.Zero
 145734            & constraint.PositiveImpulseScale >= Fixed64.Zero
 145735            & constraint.NegativeImpulseScale >= Fixed64.Zero
 145736            & first.InverseMass >= Fixed64.Zero
 145737            & second.InverseMass >= Fixed64.Zero
 145738            & constraint.Normal.IsNormalized();
 145739        if (!inputsValid)
 2740            return false;
 741
 143742        ExactLever3D.GetRelativePointVelocityRatio(
 143743            first.LinearVelocity,
 143744            first.AngularVelocity,
 143745            first.Lever,
 143746            second.LinearVelocity,
 143747            second.AngularVelocity,
 143748            second.Lever,
 143749            constraint.Normal,
 143750            out Signed832 velocityNumerator,
 143751            out Signed832 velocityDenominator, first.PlanarAngularVelocity, second.PlanarAngularVelocity);
 752
 143753        Span<ulong> effectiveNumerator = stackalloc ulong[MaxResponseWords];
 143754        Span<ulong> effectiveDenominator = stackalloc ulong[MaxResponseWords];
 143755        GetEffectiveMassRatio(
 143756            first,
 143757            second,
 143758            constraint.Normal,
 143759            effectiveNumerator,
 143760            effectiveDenominator);
 143761        int velocitySign =
 143762            velocityNumerator.Sign * velocityDenominator.Sign;
 143763        if (velocitySign == 0 && !impact.IsValid
 143764            || WideArithmetic.IsZeroMagnitude(effectiveNumerator))
 765        {
 38766            SetMagnitude(constraint.AccumulatedImpulse, numerator);
 38767            denominator[0] = 1UL;
 38768            return true;
 769        }
 770
 105771        bool hasVelocityProjection = Fixed64.TryGetSignedRawRatio(
 105772            velocityNumerator,
 105773            velocityDenominator,
 105774            0,
 105775            out Fixed64 velocityProjection);
 105776        bool applyRestitution = velocitySign < 0
 105777            & (!hasVelocityProjection
 105778                | velocityProjection
 105779                    < -constraint.RestitutionVelocityThreshold);
 105780        Fixed64 scale = velocitySign < 0
 105781            ? constraint.PositiveImpulseScale
 105782            : constraint.NegativeImpulseScale;
 105783        Span<ulong> impulseNumerator = stackalloc ulong[MaxResponseWords];
 105784        Span<ulong> impulseDenominator = stackalloc ulong[MaxResponseWords];
 785        int impulseSign;
 105786        if (impact.IsValid)
 787        {
 4788            impulseSign = BuildImpactImpulseRatio(first, second, constraint.Normal, impact,
 4789                constraint.Restitution, constraint.RestitutionVelocityThreshold,
 4790                velocityNumerator, velocityDenominator, effectiveNumerator, effectiveDenominator,
 4791                impulseNumerator, impulseDenominator);
 792        }
 793        else
 794        {
 101795            BuildImpulseRatio(
 101796                velocityNumerator,
 101797                velocityDenominator,
 101798                effectiveNumerator,
 101799                effectiveDenominator,
 101800                applyRestitution ? constraint.Restitution : Fixed64.Zero,
 101801                scale,
 101802                impulseNumerator,
 101803                impulseDenominator);
 101804            impulseSign = -velocitySign;
 805        }
 806
 105807        Span<ulong> accumulatorMagnitude =
 105808            stackalloc ulong[MaxResponseWords];
 105809        Span<ulong> accumulatorAtImpulseDenominator =
 105810            stackalloc ulong[MaxResponseWords];
 105811        SetMagnitude(constraint.AccumulatedImpulse, accumulatorMagnitude);
 105812        WideArithmetic.MultiplyMagnitudes(
 105813            accumulatorMagnitude,
 105814            impulseDenominator,
 105815            accumulatorAtImpulseDenominator);
 105816        if (impulseSign < 0
 105817            && WideArithmetic.CompareMagnitudeEqualLength(
 105818                impulseNumerator,
 105819                accumulatorAtImpulseDenominator) >= 0)
 820        {
 1821            denominator[0] = 1UL;
 1822            return true;
 823        }
 824
 104825        accumulatorAtImpulseDenominator.CopyTo(numerator);
 104826        if (impulseSign > 0)
 102827            WideArithmetic.AddMagnitudeInto(impulseNumerator, numerator);
 828        else
 2829            WideArithmetic.SubtractEqualMagnitudes(
 2830                accumulatorAtImpulseDenominator,
 2831                impulseNumerator,
 2832                numerator);
 104833        impulseDenominator.CopyTo(denominator);
 104834        return true;
 835    }
 836
 837    private static void GetBilateralImpulseRatio(
 838        in ExactContactResponseOperand3D first,
 839        in ExactContactResponseOperand3D second,
 840        Vector3d tangent,
 841        Fixed64 velocityDeadzone,
 842        Span<ulong> numerator,
 843        Span<ulong> denominator,
 844        out int sign)
 845    {
 2227846        numerator.Clear();
 2227847        denominator.Clear();
 2227848        sign = 0;
 2227849        ExactLever3D.GetRelativePointVelocityRatio(
 2227850            first.LinearVelocity,
 2227851            first.AngularVelocity,
 2227852            first.Lever,
 2227853            second.LinearVelocity,
 2227854            second.AngularVelocity,
 2227855            second.Lever,
 2227856            tangent,
 2227857            out Signed832 velocityNumerator,
 2227858            out Signed832 velocityDenominator, first.PlanarAngularVelocity, second.PlanarAngularVelocity);
 859
 2227860        Span<ulong> effectiveNumerator = stackalloc ulong[MaxResponseWords];
 2227861        Span<ulong> effectiveDenominator = stackalloc ulong[MaxResponseWords];
 2227862        GetEffectiveMassRatio(
 2227863            first,
 2227864            second,
 2227865            tangent,
 2227866            effectiveNumerator,
 2227867            effectiveDenominator);
 2227868        int velocitySign =
 2227869            velocityNumerator.Sign * velocityDenominator.Sign;
 2227870        bool suppressVelocity = velocityDeadzone != Fixed64.Zero
 2227871            && Fixed64.TryGetSignedRawRatio(
 2227872                velocityNumerator,
 2227873                velocityDenominator,
 2227874                0,
 2227875                out Fixed64 velocityProjection)
 2227876            && velocityProjection >= -velocityDeadzone
 2227877            && velocityProjection <= velocityDeadzone;
 2227878        if (velocitySign == 0
 2227879            || suppressVelocity
 2227880            || WideArithmetic.IsZeroMagnitude(effectiveNumerator))
 881        {
 1283882            denominator[0] = 1UL;
 1283883            return;
 884        }
 885
 944886        BuildImpulseRatio(
 944887            velocityNumerator,
 944888            velocityDenominator,
 944889            effectiveNumerator,
 944890            effectiveDenominator,
 944891            Fixed64.Zero,
 944892            Fixed64.One,
 944893            numerator,
 944894        denominator);
 944895        sign = -velocitySign;
 944896    }
 897
 898    private static void GetFrictionLimit(
 899        ReadOnlySpan<ulong> normalNumerator,
 900        ReadOnlySpan<ulong> normalDenominator,
 901        Fixed64 friction,
 902        Span<ulong> numerator,
 903        Span<ulong> denominator)
 904    {
 1263905        Span<ulong> frictionMagnitude =
 1263906            stackalloc ulong[3];
 1263907        Span<ulong> fixedScale =
 1263908            stackalloc ulong[1];
 1263909        SetMagnitude(friction, frictionMagnitude);
 1263910        fixedScale.Clear();
 1263911        fixedScale[0] = (ulong)FixedMath.ONE_L;
 1263912        WideArithmetic.MultiplyMagnitudes(
 1263913            normalNumerator,
 1263914            frictionMagnitude,
 1263915            numerator);
 1263916        WideArithmetic.MultiplyMagnitudes(
 1263917            normalDenominator,
 1263918            fixedScale,
 1263919            denominator);
 1263920    }
 921
 922    private static void AddFixedToRatio(
 923        ReadOnlySpan<ulong> ratioNumerator,
 924        ReadOnlySpan<ulong> ratioDenominator,
 925        int ratioSign,
 926        Fixed64 value,
 927        Span<ulong> resultNumerator,
 928        out int resultSign)
 929    {
 2199930        Span<ulong> valueMagnitude = stackalloc ulong[3];
 2199931        Span<ulong> valueAtDenominator =
 2199932            stackalloc ulong[MaxResponseWords];
 2199933        SetMagnitude(value, valueMagnitude);
 2199934        WideArithmetic.MultiplyMagnitudes(
 2199935            valueMagnitude,
 2199936            ratioDenominator,
 2199937            valueAtDenominator);
 2199938        AddSignedMagnitudes(
 2199939            ratioNumerator,
 2199940            ratioSign,
 2199941            valueAtDenominator,
 2199942            value == Fixed64.Zero ? 0 : value < Fixed64.Zero ? -1 : 1,
 2199943            resultNumerator,
 2199944            out resultSign);
 2199945    }
 946
 947    private static void SubtractFixedFromRatio(
 948        ReadOnlySpan<ulong> ratioNumerator,
 949        ReadOnlySpan<ulong> ratioDenominator,
 950        int ratioSign,
 951        Fixed64 value,
 952        Span<ulong> resultNumerator,
 953        Span<ulong> resultDenominator,
 954        out int resultSign)
 955    {
 2023956        Span<ulong> valueMagnitude = stackalloc ulong[3];
 2023957        Span<ulong> valueAtDenominator =
 2023958            stackalloc ulong[MaxCoulombWords];
 2023959        SetMagnitude(value, valueMagnitude);
 2023960        WideArithmetic.MultiplyMagnitudes(
 2023961            valueMagnitude,
 2023962            ratioDenominator,
 2023963            valueAtDenominator);
 2023964        AddSignedMagnitudes(
 2023965            ratioNumerator,
 2023966            ratioSign,
 2023967            valueAtDenominator,
 2023968            value == Fixed64.Zero ? 0 : value < Fixed64.Zero ? 1 : -1,
 2023969            resultNumerator,
 2023970            out resultSign);
 2023971        resultDenominator.Clear();
 2023972        ratioDenominator.CopyTo(resultDenominator);
 2023973    }
 974
 975    private static int CompareRatios(
 976        ReadOnlySpan<ulong> firstNumerator,
 977        ReadOnlySpan<ulong> firstDenominator,
 978        ReadOnlySpan<ulong> secondNumerator,
 979        ReadOnlySpan<ulong> secondDenominator)
 980    {
 87981        Span<ulong> first = stackalloc ulong[MaxCoulombWords];
 87982        Span<ulong> second = stackalloc ulong[MaxCoulombWords];
 87983        WideArithmetic.MultiplyMagnitudes(
 87984            firstNumerator,
 87985            secondDenominator,
 87986            first);
 87987        WideArithmetic.MultiplyMagnitudes(
 87988            secondNumerator,
 87989            firstDenominator,
 87990            second);
 87991        return WideArithmetic.CompareMagnitudeEqualLength(first, second);
 992    }
 993
 994    private static bool IsVectorWithinLimit(
 995        ReadOnlySpan<ulong> vectorMagnitudeSquared,
 996        ReadOnlySpan<ulong> vectorDenominator,
 997        ReadOnlySpan<ulong> limitNumerator,
 998        ReadOnlySpan<ulong> limitDenominator)
 999    {
 10701000        Span<ulong> denominatorSquared =
 10701001            stackalloc ulong[MaxCoulombSquareWords];
 10701002        Span<ulong> limitNumeratorSquared =
 10701003            stackalloc ulong[MaxCoulombSquareWords];
 10701004        Span<ulong> limitDenominatorSquared =
 10701005            stackalloc ulong[MaxCoulombSquareWords];
 10701006        Span<ulong> left = stackalloc ulong[MaxCoulombComparisonWords];
 10701007        Span<ulong> right = stackalloc ulong[MaxCoulombComparisonWords];
 10701008        WideArithmetic.MultiplyMagnitudes(
 10701009            vectorDenominator,
 10701010            vectorDenominator,
 10701011            denominatorSquared);
 10701012        WideArithmetic.MultiplyMagnitudes(
 10701013            limitNumerator,
 10701014            limitNumerator,
 10701015            limitNumeratorSquared);
 10701016        WideArithmetic.MultiplyMagnitudes(
 10701017            limitDenominator,
 10701018            limitDenominator,
 10701019            limitDenominatorSquared);
 10701020        WideArithmetic.MultiplyMagnitudes(
 10701021            vectorMagnitudeSquared,
 10701022            limitDenominatorSquared,
 10701023            left);
 10701024        WideArithmetic.MultiplyMagnitudes(
 10701025            limitNumeratorSquared,
 10701026            denominatorSquared,
 10701027            right);
 10701028        return WideArithmetic.CompareMagnitudeEqualLength(left, right) <= 0;
 1029    }
 1030
 1031    private static void AddSignedMagnitudes(
 1032        ReadOnlySpan<ulong> first,
 1033        int firstSign,
 1034        ReadOnlySpan<ulong> second,
 1035        int secondSign,
 1036        Span<ulong> result,
 1037        out int resultSign)
 1038    {
 338501039        result.Clear();
 338501040        resultSign = 0;
 338501041        if (firstSign != 0 & !WideArithmetic.IsZeroMagnitude(first))
 177051042            WideArithmetic.AddShiftedSignedMagnitude(
 177051043                first,
 177051044                firstSign,
 177051045                0,
 177051046                result,
 177051047                ref resultSign);
 338501048        if (secondSign != 0 & !WideArithmetic.IsZeroMagnitude(second))
 50901049            WideArithmetic.AddShiftedSignedMagnitude(
 50901050                second,
 50901051                secondSign,
 50901052                0,
 50901053                result,
 50901054                ref resultSign);
 338501055    }
 1056
 1057    private static bool TryGetSignedRatioOverSquareRoot(
 1058        ReadOnlySpan<ulong> numerator,
 1059        ReadOnlySpan<ulong> denominator,
 1060        ReadOnlySpan<ulong> radicand,
 1061        bool negative,
 1062        out Fixed64 result)
 1063    {
 1061064        if (WideArithmetic.IsZeroMagnitude(numerator))
 1065        {
 341066            result = Fixed64.Zero;
 341067            return true;
 1068        }
 1069
 721070        ulong limit = negative ? 1UL << 63 : (ulong)long.MaxValue;
 721071        if (CompareRatioOverSquareRoot(
 721072                numerator,
 721073                denominator,
 721074                radicand,
 721075                limit) > 0)
 1076        {
 111077            result = default;
 111078            return false;
 1079        }
 1080
 611081        ulong low = 0UL;
 611082        ulong high = limit;
 39071083        while (low < high)
 1084        {
 38461085            ulong difference = high - low;
 38461086            ulong middle = low + (difference >> 1) + (difference & 1UL);
 38461087            if (CompareRatioOverSquareRoot(
 38461088                    numerator,
 38461089                    denominator,
 38461090                    radicand,
 38461091                    middle) >= 0)
 1092            {
 10471093                low = middle;
 1094            }
 1095            else
 1096            {
 27991097                high = middle - 1UL;
 1098            }
 1099        }
 1100
 611101        ulong quotient = low;
 611102        Span<ulong> doubledQuotient = stackalloc ulong[2];
 611103        doubledQuotient[0] = (quotient << 1) | 1UL;
 611104        doubledQuotient[1] = quotient >> 63;
 611105        int midpointComparison = CompareRatioOverSquareRoot(
 611106            numerator,
 611107            denominator,
 611108            radicand,
 611109            doubledQuotient,
 611110            numeratorMultiplier: 2UL);
 611111        bool roundUp = midpointComparison > 0
 611112            | (midpointComparison == 0 & (quotient & 1UL) != 0UL);
 611113        if (roundUp & quotient < limit)
 1114        {
 191115            quotient++;
 1116        }
 1117
 611118        result = new Fixed64(
 611119            negative ? unchecked(-(long)quotient) : (long)quotient);
 611120        return true;
 1121    }
 1122
 1123    private static int CompareRatioOverSquareRoot(
 1124        ReadOnlySpan<ulong> numerator,
 1125        ReadOnlySpan<ulong> denominator,
 1126        ReadOnlySpan<ulong> radicand,
 1127        ulong candidate)
 1128    {
 39181129        Span<ulong> candidateMagnitude = stackalloc ulong[1];
 39181130        candidateMagnitude.Clear();
 39181131        candidateMagnitude[0] = candidate;
 39181132        return CompareRatioOverSquareRoot(
 39181133            numerator,
 39181134            denominator,
 39181135            radicand,
 39181136            candidateMagnitude,
 39181137            numeratorMultiplier: 1UL);
 1138    }
 1139
 1140    private static int CompareRatioOverSquareRoot(
 1141        ReadOnlySpan<ulong> numerator,
 1142        ReadOnlySpan<ulong> denominator,
 1143        ReadOnlySpan<ulong> radicand,
 1144        ReadOnlySpan<ulong> candidate,
 1145        ulong numeratorMultiplier)
 1146    {
 46511147        Span<ulong> multiplier = stackalloc ulong[1];
 46511148        multiplier.Clear();
 46511149        multiplier[0] = numeratorMultiplier;
 46511150        Span<ulong> scaledNumerator =
 46511151            stackalloc ulong[(MaxCoulombComparisonWords / 2) + 1];
 46511152        Span<ulong> left = stackalloc ulong[MaxCoulombComparisonWords];
 46511153        Span<ulong> denominatorTimesCandidate =
 46511154            stackalloc ulong[(MaxCoulombComparisonWords / 2) + 1];
 46511155        Span<ulong> rightBase =
 46511156            stackalloc ulong[MaxCoulombComparisonWords];
 46511157        Span<ulong> right = stackalloc ulong[MaxCoulombComparisonWords];
 46511158        WideArithmetic.MultiplyMagnitudes(
 46511159            numerator,
 46511160            multiplier,
 46511161            scaledNumerator);
 46511162        WideArithmetic.MultiplyMagnitudes(
 46511163            scaledNumerator,
 46511164            scaledNumerator,
 46511165            left);
 46511166        WideArithmetic.MultiplyMagnitudes(
 46511167            denominator,
 46511168            candidate,
 46511169            denominatorTimesCandidate);
 46511170        WideArithmetic.MultiplyMagnitudes(
 46511171            denominatorTimesCandidate,
 46511172            denominatorTimesCandidate,
 46511173            rightBase);
 46511174        WideArithmetic.MultiplyMagnitudes(rightBase, radicand, right);
 46511175        return WideArithmetic.CompareMagnitudeEqualLength(left, right);
 1176    }
 1177
 1178    private static bool HaveMatchingParticipants(
 1179        in ExactContactResponseOperand3D first,
 1180        in ExactContactResponseOperand3D second) =>
 25701181        first.Lever.XNumerator.Equals(second.Lever.XNumerator)
 25701182        & first.Lever.YNumerator.Equals(second.Lever.YNumerator)
 25701183        & first.Lever.ZNumerator.Equals(second.Lever.ZNumerator)
 25701184        & first.Lever.Denominator.Equals(second.Lever.Denominator)
 25701185        & first.InverseMass == second.InverseMass
 25701186        & first.InverseInertia == second.InverseInertia;
 1187}

/home/runner/work/Gravitas/Gravitas/src/Gravitas/CollisionHandling/Response/Exact/ExactContactResponseKernel.CoulombDiskCache.cs

#LineLine coverage
 1//=======================================================================
 2// ExactContactResponseKernel.CoulombDiskCache.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;
 9
 10using FixedMathSharp;
 11
 12namespace Gravitas.CollisionHandling;
 13
 14/// <content>
 15/// Owns exact cached-disk projection comparison and materialization.
 16/// </content>
 17internal static partial class ExactContactResponseKernel
 18{
 19    // With anchor denominators <202 bits, an angular effective-mass denominator
 20    // is <532 bits; effective mass is <1432/<1130 bits and point-velocity D <468.
 21    // A normal impulse is therefore <2088/<1932 bits even with maximum scales.
 22    // Int32 iterations add at most 31 carry bits. Multiplying by maximum friction
 23    // bounds cached tangent raw magnitudes below 2150 bits. A desired tangent
 24    // at the common denominator is <6014 bits, within the existing 96 words.
 25    // Disk squares and radial comparisons remain below the existing 320 words.
 26    private static void AddCachedTangentToRatio(ReadOnlySpan<ulong> numerator,
 27        ReadOnlySpan<ulong> denominator, int sign, Fixed64 scalar,
 28        ExactContactImpulseState? state, bool primary, Span<ulong> result, out int resultSign)
 29    {
 214030        if (state?.HasTangents != true)
 31        {
 211232            AddFixedToRatio(numerator, denominator, sign, scalar, result, out resultSign);
 211233            return;
 34        }
 2835        Span<ulong> cached = stackalloc ulong[MaxCoulombWords];
 2836        WideArithmetic.MultiplyMagnitudes(primary ? state.PrimaryMagnitude : state.SecondaryMagnitude,
 2837            denominator, cached);
 2838        AddSignedMagnitudes(numerator, sign, cached,
 2839            primary ? state.PrimarySign : state.SecondarySign, result, out resultSign);
 2840    }
 41
 42    private static void SubtractCachedTangentFromRatio(ReadOnlySpan<ulong> numerator,
 43        ReadOnlySpan<ulong> denominator, int sign, Fixed64 scalar,
 44        ExactContactImpulseState? state, bool primary, Span<ulong> result,
 45        Span<ulong> resultDenominator, out int resultSign)
 46    {
 202847        if (state?.HasTangents != true)
 48        {
 200449            SubtractFixedFromRatio(numerator, denominator, sign, scalar,
 200450                result, resultDenominator, out resultSign);
 200451            return;
 52        }
 2453        Span<ulong> cached = stackalloc ulong[MaxCoulombWords];
 2454        WideArithmetic.MultiplyMagnitudes(primary ? state.PrimaryMagnitude : state.SecondaryMagnitude,
 2455            denominator, cached);
 2456        AddSignedMagnitudes(numerator, sign, cached,
 2457            -(primary ? state.PrimarySign : state.SecondarySign), result, out resultSign);
 2458        resultDenominator.Clear();
 2459        denominator.CopyTo(resultDenominator);
 2460    }
 61
 62    private static void RoundCachedRatio(ReadOnlySpan<ulong> numerator,
 63        ReadOnlySpan<ulong> denominator, Span<ulong> result)
 64    {
 2465        Span<ulong> quotient = stackalloc ulong[numerator.Length];
 2466        Span<ulong> remainder = stackalloc ulong[denominator.Length];
 2467        Span<ulong> division = stackalloc ulong[numerator.Length + denominator.Length + 1];
 2468        WideArithmetic.DivideMagnitudes(numerator, denominator, quotient, remainder, division);
 69        // Compare r with D-r to decide the midpoint without overflowing 2*r.
 2470        Span<ulong> distance = stackalloc ulong[denominator.Length];
 2471        WideArithmetic.SubtractEqualMagnitudes(denominator, remainder, distance);
 2472        int midpoint = WideArithmetic.CompareMagnitudeEqualLength(remainder, distance);
 2473        if (midpoint > 0 || midpoint == 0 && (quotient[0] & 1UL) != 0)
 74        {
 275            Span<ulong> one = stackalloc ulong[1];
 276            one[0] = 1;
 277            WideArithmetic.AddMagnitudeInto(one, quotient);
 78        }
 79#if DEBUG
 80        // The retained tangent bound is <2150 bits, below the 3072-bit cache.
 81        System.Diagnostics.Debug.Assert(WideArithmetic.IsZeroMagnitude(quotient[result.Length..]));
 82#endif
 2483        quotient[..result.Length].CopyTo(result);
 2484    }
 85
 86    private static void RoundCachedRadialRatio(ReadOnlySpan<ulong> numerator,
 87        ReadOnlySpan<ulong> denominator, ReadOnlySpan<ulong> radicand, Span<ulong> result)
 88    {
 1689        result.Clear();
 2290        if (WideArithmetic.IsZeroMagnitude(numerator)) return;
 91        // The bit enclosure only bounds the integer search. Every admitted bit
 92        // and the final midpoint are compared against the exact radical.
 1093        int bits = Math.Max(0, GetRadialQuotientBitBound(numerator, denominator, radicand));
 94#if DEBUG
 95        // The enclosure adds at most two bits to the <2150-bit tangent bound.
 96        System.Diagnostics.Debug.Assert(bits <= result.Length * 64);
 97#endif
 134498        for (int bit = bits - 1; bit >= 0; bit--)
 99        {
 662100            ulong mask = 1UL << (bit & 63);
 662101            result[bit >> 6] |= mask;
 662102            if (CompareRatioOverSquareRoot(numerator, denominator, radicand, result,
 1005103                    numeratorMultiplier: 1) < 0) result[bit >> 6] &= ~mask;
 104        }
 10105        Span<ulong> midpoint = stackalloc ulong[result.Length];
 10106        Span<ulong> two = stackalloc ulong[1];
 10107        two[0] = 2;
 10108        WideArithmetic.MultiplyMagnitudes(result, two, midpoint);
 10109        midpoint[0] |= 1UL;
 10110        int comparison = CompareRatioOverSquareRoot(numerator, denominator, radicand, midpoint,
 10111            numeratorMultiplier: 2);
 10112        if (comparison > 0 || comparison == 0 && (result[0] & 1UL) != 0)
 113        {
 3114            two[0] = 1;
 3115            WideArithmetic.AddMagnitudeInto(two, result);
 116        }
 10117    }
 118
 119    private static bool IsRadialProjectionEqualToFixed(
 120        ReadOnlySpan<ulong> directionNumerator,
 121        int directionSign,
 122        ReadOnlySpan<ulong> radialNumerator,
 123        ReadOnlySpan<ulong> radialDenominator,
 124        ReadOnlySpan<ulong> radicand,
 125        Fixed64 value,
 126        ExactContactImpulseState? state = null,
 127        bool primary = true)
 128    {
 66129        Span<ulong> projectedNumerator =
 66130            stackalloc ulong[MaxCoulombComparisonWords / 2];
 66131        Span<ulong> valueMagnitude = stackalloc ulong[state?.HasTangents == true ? MaxResponseWords : 3];
 66132        WideArithmetic.MultiplyMagnitudes(
 66133            directionNumerator,
 66134            radialNumerator,
 66135            projectedNumerator);
 66136        if (state?.HasTangents == true)
 3137            (primary ? state.PrimaryMagnitude : state.SecondaryMagnitude).CopyTo(valueMagnitude);
 138        else
 63139            SetMagnitude(value, valueMagnitude);
 66140        int valueSign =
 66141            state?.HasTangents == true ? primary ? state.PrimarySign : state.SecondarySign
 66142                : value == Fixed64.Zero ? 0 : value < Fixed64.Zero ? -1 : 1;
 66143        Span<ulong> valueCoefficient = stackalloc ulong[MaxCoulombComparisonWords];
 66144        WideArithmetic.MultiplyMagnitudes(valueMagnitude, radialDenominator, valueCoefficient);
 66145        return WideArithmetic.GetSignedMagnitudeAndSquareRootSign(
 66146                projectedNumerator,
 66147                directionSign,
 66148                valueCoefficient,
 66149                -valueSign,
 66150                radicand)
 66151            == 0;
 152    }
 153
 154    // The radical denominator is the supplied factor times the rational denominator.
 155    // Both factors and the radicand are positive.
 156    internal static bool TryGetSignedRadicalAndRationalSum(
 157        ReadOnlySpan<ulong> radicalNumerator,
 158        ReadOnlySpan<ulong> radicalDenominatorFactor,
 159        ReadOnlySpan<ulong> radicand,
 160        int radicalSign,
 161        ReadOnlySpan<ulong> rationalNumerator,
 162        ReadOnlySpan<ulong> rationalDenominator,
 163        int rationalSign,
 164        out Fixed64 result)
 165    {
 582166        radicalNumerator = TrimMagnitude(radicalNumerator);
 582167        radicalDenominatorFactor = TrimMagnitude(radicalDenominatorFactor);
 582168        rationalNumerator = TrimMagnitude(rationalNumerator);
 582169        rationalDenominator = TrimMagnitude(rationalDenominator);
 582170        radicand = TrimMagnitude(radicand);
 171
 172        // A/(B*D*sqrt(R)) + N/D compared to c/2 has the sign of
 173        // 2*A + (2*N*B - c*B*D)*sqrt(R). Prepare the invariant products once.
 582174        ReadOnlySpan<ulong> two = stackalloc ulong[] { 2UL };
 582175        Span<ulong> doubledRadical = stackalloc ulong[radicalNumerator.Length + 1];
 582176        Span<ulong> doubledCoefficient =
 582177            stackalloc ulong[rationalNumerator.Length + radicalDenominatorFactor.Length + 1];
 582178        Span<ulong> denominator =
 582179            stackalloc ulong[radicalDenominatorFactor.Length + rationalDenominator.Length];
 582180        WideArithmetic.MultiplyMagnitudes(radicalNumerator, two, doubledRadical);
 582181        Multiply3(rationalNumerator, radicalDenominatorFactor, two, doubledCoefficient);
 582182        WideArithmetic.MultiplyMagnitudes(radicalDenominatorFactor, rationalDenominator, denominator);
 582183        int resultSign = WideArithmetic.GetSignedMagnitudeAndSquareRootSign(
 582184            doubledRadical, radicalSign, doubledCoefficient, rationalSign, radicand);
 582185        if (resultSign == 0)
 186        {
 430187            result = Fixed64.Zero;
 430188            return true;
 189        }
 190
 152191        int normalizedRadicalSign =
 152192            resultSign < 0 ? -radicalSign : radicalSign;
 152193        int normalizedRationalSign =
 152194            resultSign < 0 ? -rationalSign : rationalSign;
 152195        ulong limit =
 152196            resultSign < 0 ? 1UL << 63 : (ulong)long.MaxValue;
 152197        int limitMidpointComparison =
 152198            ComparePreparedSignedSum(doubledRadical, normalizedRadicalSign, doubledCoefficient,
 152199                normalizedRationalSign, denominator, radicand, limit, midpoint: true);
 152200        if (limitMidpointComparison > 0
 152201            || (limitMidpointComparison == 0
 152202                && (limit & 1UL) != 0UL))
 203        {
 6204            result = default;
 6205            return false;
 206        }
 207
 292208        ulong low = 0UL, high = limit;
 146209        if (TryGetSignedSumIntegerParts(radicalNumerator, TrimMagnitude(denominator), radicand,
 146210                normalizedRadicalSign, rationalNumerator, rationalDenominator, normalizedRationalSign,
 146211                out long integerParts))
 212        {
 213            // The normalized sum is positive. With opposite signs the fractional
 214            // correction is in (-1, 1), and with positive terms it is in [0, 2).
 129215            low = (ulong)Math.Max(0L, integerParts - 1L);
 129216            high = (ulong)(integerParts + 1L);
 217        }
 1439218        while (low < high)
 219        {
 1293220            ulong difference = high - low;
 1293221            ulong middle =
 1293222                low + (difference >> 1) + (difference & 1UL);
 1293223            if (ComparePreparedSignedSum(doubledRadical, normalizedRadicalSign, doubledCoefficient,
 1293224                    normalizedRationalSign, denominator, radicand, middle, midpoint: false) >= 0)
 225            {
 503226                low = middle;
 227            }
 228            else
 229            {
 790230                high = middle - 1UL;
 231            }
 232        }
 233
 146234        ulong quotient = low;
 146235        int midpointComparison =
 146236            ComparePreparedSignedSum(doubledRadical, normalizedRadicalSign, doubledCoefficient,
 146237                normalizedRationalSign, denominator, radicand, quotient, midpoint: true);
 146238        bool roundUp = midpointComparison > 0
 146239            | (midpointComparison == 0 & (quotient & 1UL) != 0UL);
 146240        if (roundUp & quotient < limit)
 77241            quotient++;
 242
 146243        result = new Fixed64(
 146244            resultSign < 0
 146245                ? unchecked(-(long)quotient)
 146246                : (long)quotient);
 146247        return true;
 248    }
 249
 250    private static bool TryGetSignedSumIntegerParts(
 251        ReadOnlySpan<ulong> radicalNumerator,
 252        ReadOnlySpan<ulong> radicalDenominator,
 253        ReadOnlySpan<ulong> radicand,
 254        int radicalSign,
 255        ReadOnlySpan<ulong> rationalNumerator,
 256        ReadOnlySpan<ulong> rationalDenominator,
 257        int rationalSign,
 258        out long integerParts)
 259    {
 260        // Two unclipped parts and a one-unit search margin fit in Int64.
 261        const ulong cap = 1UL << 62;
 146262        Span<ulong> rationalFloor = stackalloc ulong[Math.Max(1, rationalNumerator.Length)];
 146263        Span<ulong> remainder = stackalloc ulong[rationalDenominator.Length];
 146264        Span<ulong> division = stackalloc ulong[rationalNumerator.Length + rationalDenominator.Length + 1];
 146265        WideArithmetic.DivideMagnitudes(rationalNumerator, rationalDenominator, rationalFloor, remainder, division);
 146266        integerParts = 0;
 146267        if (rationalFloor[0] >= cap || !WideArithmetic.IsZeroMagnitude(rationalFloor[1..]))
 13268            return false;
 269
 133270        int words = Math.Max(radicalNumerator.Length * 2, radicalDenominator.Length * 2 + radicand.Length);
 133271        Span<ulong> squareNumerator = stackalloc ulong[words];
 133272        Span<ulong> squareDenominator = stackalloc ulong[words];
 133273        WideArithmetic.MultiplyMagnitudes(radicalNumerator, radicalNumerator, squareNumerator);
 133274        Multiply3(radicalDenominator, radicalDenominator, radicand, squareDenominator);
 133275        ulong radicalFloor = WideArithmetic.GetRatioFloorSquareRoot(squareNumerator, squareDenominator, cap);
 133276        if (radicalFloor >= cap)
 4277            return false;
 278
 129279        integerParts = radicalSign * (long)radicalFloor + rationalSign * (long)rationalFloor[0];
 129280        return true;
 281    }
 282
 283    private static int GetRadialQuotientBitBound(
 284        ReadOnlySpan<ulong> numerator,
 285        ReadOnlySpan<ulong> denominator,
 286        ReadOnlySpan<ulong> radicand) =>
 10287        WideArithmetic.GetMagnitudeBitLength(numerator)
 10288        - WideArithmetic.GetMagnitudeBitLength(denominator)
 10289        - (WideArithmetic.GetMagnitudeBitLength(radicand) - 1) / 2 + 2;
 290
 291    private static int ComparePreparedSignedSum(
 292        ReadOnlySpan<ulong> doubledRadical,
 293        int radicalSign,
 294        ReadOnlySpan<ulong> doubledCoefficient,
 295        int rationalSign,
 296        ReadOnlySpan<ulong> denominator,
 297        ReadOnlySpan<ulong> radicand,
 298        ulong candidate,
 299        bool midpoint)
 300    {
 1591301        ReadOnlySpan<ulong> doubledCandidate =
 1591302            stackalloc ulong[] { (candidate << 1) | (midpoint ? 1UL : 0UL), candidate >> 63 };
 1591303        Span<ulong> threshold = stackalloc ulong[denominator.Length + doubledCandidate.Length];
 1591304        Span<ulong> adjusted = stackalloc ulong[Math.Max(doubledCoefficient.Length, threshold.Length) + 1];
 1591305        WideArithmetic.MultiplyMagnitudes(doubledCandidate, denominator, threshold);
 1591306        AddSignedMagnitudes(doubledCoefficient, rationalSign, threshold, -1, adjusted, out int adjustedSign);
 1591307        return WideArithmetic.GetSignedMagnitudeAndSquareRootSign(
 1591308            doubledRadical, radicalSign, adjusted, adjustedSign, radicand);
 309    }
 310
 311    private static void GetWeightedFixedSum(
 312        Fixed64 firstFactor,
 313        Fixed64 firstValue,
 314        Fixed64 secondFactor,
 315        Fixed64 secondValue,
 316        Span<ulong> result,
 317        out int resultSign,
 318        ExactContactImpulseState? state = null)
 319    {
 225320        if (state?.HasTangents == true)
 321        {
 6322            GetWeightedSignedSum(firstFactor, state.PrimaryMagnitude, state.PrimarySign,
 6323                secondFactor, state.SecondaryMagnitude, state.SecondarySign, result, out resultSign);
 6324            return;
 325        }
 219326        Span<ulong> factorMagnitude = stackalloc ulong[3];
 219327        Span<ulong> valueMagnitude = stackalloc ulong[3];
 219328        Span<ulong> first = stackalloc ulong[3];
 219329        Span<ulong> second = stackalloc ulong[3];
 219330        SetMagnitude(firstFactor, factorMagnitude);
 219331        SetMagnitude(firstValue, valueMagnitude);
 219332        WideArithmetic.MultiplyMagnitudes(
 219333            factorMagnitude,
 219334            valueMagnitude,
 219335            first);
 219336        SetMagnitude(secondFactor, factorMagnitude);
 219337        SetMagnitude(secondValue, valueMagnitude);
 219338        WideArithmetic.MultiplyMagnitudes(
 219339            factorMagnitude,
 219340            valueMagnitude,
 219341            second);
 219342        AddSignedMagnitudes(
 219343            first,
 219344            GetProductSign(firstFactor, firstValue),
 219345            second,
 219346            GetProductSign(secondFactor, secondValue),
 219347            result,
 219348            out resultSign);
 219349    }
 350
 351    private static void GetWeightedSignedWideSum(
 352        Signed832 firstFactor,
 353        Fixed64 firstValue,
 354        Signed832 secondFactor,
 355        Fixed64 secondValue,
 356        Span<ulong> result,
 357        out int resultSign,
 358        ExactContactImpulseState? state = null)
 359    {
 336360        if (state?.HasTangents == true)
 361        {
 12362            Span<ulong> firstMagnitude = stackalloc ulong[13];
 12363            Span<ulong> secondMagnitude = stackalloc ulong[13];
 12364            Span<ulong> firstProduct = stackalloc ulong[64];
 12365            Span<ulong> secondProduct = stackalloc ulong[64];
 12366            SetMagnitude(firstFactor, firstMagnitude);
 12367            SetMagnitude(secondFactor, secondMagnitude);
 12368            WideArithmetic.MultiplyMagnitudes(firstMagnitude, state.PrimaryMagnitude, firstProduct);
 12369            WideArithmetic.MultiplyMagnitudes(secondMagnitude, state.SecondaryMagnitude, secondProduct);
 12370            AddSignedMagnitudes(firstProduct, firstFactor.Sign * state.PrimarySign,
 12371                secondProduct, secondFactor.Sign * state.SecondarySign, result, out resultSign);
 12372            return;
 373        }
 324374        Span<ulong> factorMagnitude = stackalloc ulong[13];
 324375        Span<ulong> valueMagnitude = stackalloc ulong[3];
 324376        Span<ulong> first = stackalloc ulong[15];
 324377        Span<ulong> second = stackalloc ulong[15];
 324378        SetMagnitude(firstFactor, factorMagnitude);
 324379        SetMagnitude(firstValue, valueMagnitude);
 324380        WideArithmetic.MultiplyMagnitudes(
 324381            factorMagnitude,
 324382            valueMagnitude,
 324383            first);
 324384        SetMagnitude(secondFactor, factorMagnitude);
 324385        SetMagnitude(secondValue, valueMagnitude);
 324386        WideArithmetic.MultiplyMagnitudes(
 324387            factorMagnitude,
 324388            valueMagnitude,
 324389            second);
 324390        AddSignedMagnitudes(
 324391            first,
 324392            firstFactor.Sign * Math.Sign(firstValue.m_rawValue),
 324393            second,
 324394            secondFactor.Sign * Math.Sign(secondValue.m_rawValue),
 324395            result,
 324396            out resultSign);
 324397    }
 398
 399    private static int GetProductSign(Fixed64 first, Fixed64 second) =>
 438400        Math.Sign(first.m_rawValue) * Math.Sign(second.m_rawValue);
 401
 402    private static ReadOnlySpan<ulong> TrimMagnitude(ReadOnlySpan<ulong> value) =>
 3056403        value[..WideArithmetic.GetActiveMagnitudeLength(value)];
 404
 405    private static bool TryGetDiskVelocityDeltas(
 406        in ExactContactResponseOperand3D primaryFirst,
 407        in ExactContactResponseOperand3D primarySecond,
 408        in ExactContactResponseOperand3D secondaryFirst,
 409        in ExactContactResponseOperand3D secondarySecond,
 410        Vector3d primaryTangent,
 411        Vector3d secondaryTangent,
 412        ReadOnlySpan<ulong> primaryNumerator,
 413        int primarySign,
 414        ReadOnlySpan<ulong> secondaryNumerator,
 415        int secondarySign,
 416        ReadOnlySpan<ulong> commonDenominator,
 417        bool rational,
 418        ReadOnlySpan<ulong> radialNumerator,
 419        ReadOnlySpan<ulong> radialDenominator,
 420        ReadOnlySpan<ulong> radicand,
 421        Fixed64 accumulatedPrimaryTangentImpulse,
 422        Fixed64 accumulatedSecondaryTangentImpulse,
 423        out Vector3d firstLinear,
 424        out Vector3d firstAngular,
 425        out Vector3d secondLinear,
 426        out Vector3d secondAngular,
 427        ExactContactImpulseState? state = null)
 428    {
 1070429        bool resolved = TryGetDiskLinearVelocityDelta(
 1070430            primaryFirst.LinearImpulseAxis,
 1070431            secondaryFirst.LinearImpulseAxis,
 1070432            primaryFirst.InverseMass,
 1070433            primaryNumerator,
 1070434            primarySign,
 1070435            secondaryNumerator,
 1070436            secondarySign,
 1070437            commonDenominator,
 1070438            rational,
 1070439            radialNumerator,
 1070440            radialDenominator,
 1070441            radicand,
 1070442            accumulatedPrimaryTangentImpulse,
 1070443            accumulatedSecondaryTangentImpulse,
 1070444            out firstLinear, state);
 1070445        resolved &= TryGetDiskAngularVelocityDelta(
 1070446            primaryFirst.Lever,
 1070447            -primaryTangent,
 1070448            -secondaryTangent,
 1070449            primaryFirst.InverseInertia,
 1070450            primaryNumerator,
 1070451            primarySign,
 1070452            secondaryNumerator,
 1070453            secondarySign,
 1070454            commonDenominator,
 1070455            rational,
 1070456            radialNumerator,
 1070457            radialDenominator,
 1070458            radicand,
 1070459            accumulatedPrimaryTangentImpulse,
 1070460            accumulatedSecondaryTangentImpulse,
 1070461            out firstAngular, state);
 1070462        resolved &= TryGetDiskLinearVelocityDelta(
 1070463            primarySecond.LinearImpulseAxis,
 1070464            secondarySecond.LinearImpulseAxis,
 1070465            primarySecond.InverseMass,
 1070466            primaryNumerator,
 1070467            primarySign,
 1070468            secondaryNumerator,
 1070469            secondarySign,
 1070470            commonDenominator,
 1070471            rational,
 1070472            radialNumerator,
 1070473            radialDenominator,
 1070474            radicand,
 1070475            accumulatedPrimaryTangentImpulse,
 1070476            accumulatedSecondaryTangentImpulse,
 1070477            out secondLinear, state);
 1070478        resolved &= TryGetDiskAngularVelocityDelta(
 1070479            primarySecond.Lever,
 1070480            primaryTangent,
 1070481            secondaryTangent,
 1070482            primarySecond.InverseInertia,
 1070483            primaryNumerator,
 1070484            primarySign,
 1070485            secondaryNumerator,
 1070486            secondarySign,
 1070487            commonDenominator,
 1070488            rational,
 1070489            radialNumerator,
 1070490            radialDenominator,
 1070491            radicand,
 1070492            accumulatedPrimaryTangentImpulse,
 1070493            accumulatedSecondaryTangentImpulse,
 1070494            out secondAngular, state);
 1070495        return resolved;
 496    }
 497
 498    private static bool TryGetDiskLinearVelocityDelta(
 499        Vector3d primaryAxis,
 500        Vector3d secondaryAxis,
 501        Fixed64 inverseMass,
 502        ReadOnlySpan<ulong> primaryNumerator,
 503        int primarySign,
 504        ReadOnlySpan<ulong> secondaryNumerator,
 505        int secondarySign,
 506        ReadOnlySpan<ulong> commonDenominator,
 507        bool rational,
 508        ReadOnlySpan<ulong> radialNumerator,
 509        ReadOnlySpan<ulong> radialDenominator,
 510        ReadOnlySpan<ulong> radicand,
 511        Fixed64 accumulatedPrimaryTangentImpulse,
 512        Fixed64 accumulatedSecondaryTangentImpulse,
 513        out Vector3d result,
 514        ExactContactImpulseState? state = null)
 515    {
 2140516        bool xResolved = TryGetDiskLinearComponent(
 2140517            primaryAxis.X,
 2140518            secondaryAxis.X,
 2140519            inverseMass,
 2140520            primaryNumerator,
 2140521            primarySign,
 2140522            secondaryNumerator,
 2140523            secondarySign,
 2140524            commonDenominator,
 2140525            rational,
 2140526            radialNumerator,
 2140527            radialDenominator,
 2140528            radicand,
 2140529            accumulatedPrimaryTangentImpulse,
 2140530            accumulatedSecondaryTangentImpulse,
 2140531            out Fixed64 x, state);
 2140532        bool yResolved = TryGetDiskLinearComponent(
 2140533            primaryAxis.Y,
 2140534            secondaryAxis.Y,
 2140535            inverseMass,
 2140536            primaryNumerator,
 2140537            primarySign,
 2140538            secondaryNumerator,
 2140539            secondarySign,
 2140540            commonDenominator,
 2140541            rational,
 2140542            radialNumerator,
 2140543            radialDenominator,
 2140544            radicand,
 2140545            accumulatedPrimaryTangentImpulse,
 2140546            accumulatedSecondaryTangentImpulse,
 2140547            out Fixed64 y, state);
 2140548        bool zResolved = TryGetDiskLinearComponent(
 2140549            primaryAxis.Z,
 2140550            secondaryAxis.Z,
 2140551            inverseMass,
 2140552            primaryNumerator,
 2140553            primarySign,
 2140554            secondaryNumerator,
 2140555            secondarySign,
 2140556            commonDenominator,
 2140557            rational,
 2140558            radialNumerator,
 2140559            radialDenominator,
 2140560            radicand,
 2140561            accumulatedPrimaryTangentImpulse,
 2140562            accumulatedSecondaryTangentImpulse,
 2140563            out Fixed64 z, state);
 2140564        result = xResolved & yResolved & zResolved
 2140565            ? new Vector3d(x, y, z)
 2140566            : default;
 2140567        return xResolved & yResolved & zResolved;
 568    }
 569
 570    private static bool TryGetDiskLinearComponent(
 571        Fixed64 primaryAxis,
 572        Fixed64 secondaryAxis,
 573        Fixed64 inverseMass,
 574        ReadOnlySpan<ulong> primaryNumerator,
 575        int primarySign,
 576        ReadOnlySpan<ulong> secondaryNumerator,
 577        int secondarySign,
 578        ReadOnlySpan<ulong> commonDenominator,
 579        bool rational,
 580        ReadOnlySpan<ulong> radialNumerator,
 581        ReadOnlySpan<ulong> radialDenominator,
 582        ReadOnlySpan<ulong> radicand,
 583        Fixed64 accumulatedPrimaryTangentImpulse,
 584        Fixed64 accumulatedSecondaryTangentImpulse,
 585        out Fixed64 result,
 586        ExactContactImpulseState? state = null)
 587    {
 6420588        Span<ulong> combined = stackalloc ulong[MaxCoulombWords + 2];
 6420589        GetWeightedSignedSum(
 6420590            primaryAxis,
 6420591            primaryNumerator,
 6420592            primarySign,
 6420593            secondaryAxis,
 6420594            secondaryNumerator,
 6420595            secondarySign,
 6420596            combined,
 6420597            out int combinedSign);
 6420598        if (inverseMass == Fixed64.Zero)
 599        {
 2442600            result = Fixed64.Zero;
 2442601            return true;
 602        }
 603
 3978604        Span<ulong> inverseMassMagnitude =
 3978605            stackalloc ulong[3];
 3978606        Span<ulong> fixedScale = stackalloc ulong[1];
 3978607        Span<ulong> numerator = stackalloc ulong[MaxCoulombComparisonWords / 2];
 3978608        SetMagnitude(inverseMass, inverseMassMagnitude);
 3978609        fixedScale.Clear();
 3978610        fixedScale[0] = (ulong)FixedMath.ONE_L;
 3978611        if (rational)
 612        {
 3753613            if (combinedSign == 0)
 614            {
 2794615                result = Fixed64.Zero;
 2794616                return true;
 617            }
 618
 959619            Span<ulong> denominator = stackalloc ulong[MaxCoulombComparisonWords / 2];
 959620            Multiply3(commonDenominator, fixedScale, fixedScale, denominator);
 959621            WideArithmetic.MultiplyMagnitudes(
 959622                combined,
 959623                inverseMassMagnitude,
 959624                numerator);
 959625            return Fixed64.TryGetSignedRawRatio(
 959626                numerator,
 959627                denominator,
 959628                combinedSign < 0,
 959629                out result);
 630        }
 631
 225632        Span<ulong> scaledNumerator =
 225633            stackalloc ulong[MaxCoulombComparisonWords / 2];
 225634        Span<ulong> cachedCombined = stackalloc ulong[state?.HasTangents == true ? 64 : 3];
 225635        Span<ulong> cachedNumerator = stackalloc ulong[state?.HasTangents == true ? 65 : 4];
 225636        Span<ulong> cachedDenominator = stackalloc ulong[2];
 225637        WideArithmetic.MultiplyMagnitudes(
 225638            combined,
 225639            inverseMassMagnitude,
 225640            numerator);
 225641        WideArithmetic.MultiplyMagnitudes(
 225642            numerator,
 225643            radialNumerator,
 225644            scaledNumerator);
 225645        GetWeightedFixedSum(
 225646            primaryAxis,
 225647            accumulatedPrimaryTangentImpulse,
 225648            secondaryAxis,
 225649            accumulatedSecondaryTangentImpulse,
 225650            cachedCombined,
 225651            out int cachedSign, state);
 225652        WideArithmetic.MultiplyMagnitudes(
 225653            cachedCombined,
 225654            inverseMassMagnitude,
 225655            cachedNumerator);
 225656        WideArithmetic.MultiplyMagnitudes(
 225657            fixedScale,
 225658            fixedScale,
 225659            cachedDenominator);
 225660        return TryGetSignedRadicalAndRationalSum(
 225661            scaledNumerator,
 225662            radialDenominator,
 225663            radicand,
 225664            combinedSign,
 225665            cachedNumerator,
 225666            cachedDenominator,
 225667            -cachedSign,
 225668            out result);
 669    }
 670
 671    private static bool TryGetDiskAngularVelocityDelta(
 672        in ExactLever3D lever,
 673        Vector3d primaryAxis,
 674        Vector3d secondaryAxis,
 675        Fixed3x3 inverseInertia,
 676        ReadOnlySpan<ulong> primaryNumerator,
 677        int primarySign,
 678        ReadOnlySpan<ulong> secondaryNumerator,
 679        int secondarySign,
 680        ReadOnlySpan<ulong> commonDenominator,
 681        bool rational,
 682        ReadOnlySpan<ulong> radialNumerator,
 683        ReadOnlySpan<ulong> radialDenominator,
 684        ReadOnlySpan<ulong> radicand,
 685        Fixed64 accumulatedPrimaryTangentImpulse,
 686        Fixed64 accumulatedSecondaryTangentImpulse,
 687        out Vector3d result,
 688        ExactContactImpulseState? state = null)
 689    {
 2140690        ExactLever3D.GetTransformedCrossProduct(
 2140691            lever,
 2140692            primaryAxis,
 2140693            inverseInertia,
 2140694            out Signed832 primaryX,
 2140695            out Signed832 primaryY,
 2140696            out Signed832 primaryZ);
 2140697        ExactLever3D.GetTransformedCrossProduct(
 2140698            lever,
 2140699            secondaryAxis,
 2140700            inverseInertia,
 2140701            out Signed832 secondaryX,
 2140702            out Signed832 secondaryY,
 2140703            out Signed832 secondaryZ);
 2140704        Signed320 fixedScaleSquared = WideArithmetic.MultiplySigned192(
 2140705            Signed192.One,
 2140706            Signed192.One);
 2140707        Signed704 transformedDenominator =
 2140708            WideArithmetic.MultiplySigned576ToSigned704(
 2140709                lever.Denominator,
 2140710                fixedScaleSquared);
 711
 2140712        bool xResolved = TryGetDiskAngularComponent(
 2140713            primaryX,
 2140714            secondaryX,
 2140715            transformedDenominator,
 2140716            primaryNumerator,
 2140717            primarySign,
 2140718            secondaryNumerator,
 2140719            secondarySign,
 2140720            commonDenominator,
 2140721            rational,
 2140722            radialNumerator,
 2140723            radialDenominator,
 2140724            radicand,
 2140725            accumulatedPrimaryTangentImpulse,
 2140726            accumulatedSecondaryTangentImpulse,
 2140727            out Fixed64 x, state);
 2140728        bool yResolved = TryGetDiskAngularComponent(
 2140729            primaryY,
 2140730            secondaryY,
 2140731            transformedDenominator,
 2140732            primaryNumerator,
 2140733            primarySign,
 2140734            secondaryNumerator,
 2140735            secondarySign,
 2140736            commonDenominator,
 2140737            rational,
 2140738            radialNumerator,
 2140739            radialDenominator,
 2140740            radicand,
 2140741            accumulatedPrimaryTangentImpulse,
 2140742            accumulatedSecondaryTangentImpulse,
 2140743            out Fixed64 y, state);
 2140744        bool zResolved = TryGetDiskAngularComponent(
 2140745            primaryZ,
 2140746            secondaryZ,
 2140747            transformedDenominator,
 2140748            primaryNumerator,
 2140749            primarySign,
 2140750            secondaryNumerator,
 2140751            secondarySign,
 2140752            commonDenominator,
 2140753            rational,
 2140754            radialNumerator,
 2140755            radialDenominator,
 2140756            radicand,
 2140757            accumulatedPrimaryTangentImpulse,
 2140758            accumulatedSecondaryTangentImpulse,
 2140759            out Fixed64 z, state);
 2140760        result = xResolved & yResolved & zResolved
 2140761            ? new Vector3d(x, y, z)
 2140762            : default;
 2140763        return xResolved & yResolved & zResolved;
 764    }
 765
 766    private static bool TryGetDiskAngularComponent(
 767        Signed832 primary,
 768        Signed832 secondary,
 769        Signed704 transformedDenominator,
 770        ReadOnlySpan<ulong> primaryNumerator,
 771        int primarySign,
 772        ReadOnlySpan<ulong> secondaryNumerator,
 773        int secondarySign,
 774        ReadOnlySpan<ulong> commonDenominator,
 775        bool rational,
 776        ReadOnlySpan<ulong> radialNumerator,
 777        ReadOnlySpan<ulong> radialDenominator,
 778        ReadOnlySpan<ulong> radicand,
 779        Fixed64 accumulatedPrimaryTangentImpulse,
 780        Fixed64 accumulatedSecondaryTangentImpulse,
 781        out Fixed64 result,
 782        ExactContactImpulseState? state = null)
 783    {
 6420784        Span<ulong> primaryMagnitude = stackalloc ulong[13];
 6420785        Span<ulong> secondaryMagnitude = stackalloc ulong[13];
 6420786        SetMagnitude(primary, primaryMagnitude);
 6420787        SetMagnitude(secondary, secondaryMagnitude);
 6420788        Span<ulong> primaryProduct =
 6420789            stackalloc ulong[MaxCoulombComparisonWords / 2];
 6420790        Span<ulong> secondaryProduct =
 6420791            stackalloc ulong[MaxCoulombComparisonWords / 2];
 6420792        WideArithmetic.MultiplyMagnitudes(
 6420793            primaryMagnitude,
 6420794            primaryNumerator,
 6420795            primaryProduct);
 6420796        WideArithmetic.MultiplyMagnitudes(
 6420797            secondaryMagnitude,
 6420798            secondaryNumerator,
 6420799            secondaryProduct);
 6420800        Span<ulong> combined =
 6420801            stackalloc ulong[MaxCoulombComparisonWords / 2];
 6420802        AddSignedMagnitudes(
 6420803            primaryProduct,
 6420804            primary.Sign * primarySign,
 6420805            secondaryProduct,
 6420806            secondary.Sign * secondarySign,
 6420807            combined,
 6420808            out int combinedSign);
 809
 6420810        Span<ulong> transformedDenominatorMagnitude =
 6420811            stackalloc ulong[11];
 6420812        Span<ulong> fixedScale = stackalloc ulong[1];
 6420813        SetMagnitude(transformedDenominator, transformedDenominatorMagnitude);
 6420814        fixedScale.Clear();
 6420815        fixedScale[0] = (ulong)FixedMath.ONE_L;
 6420816        if (rational)
 817        {
 6084818            if (combinedSign == 0)
 819            {
 5341820                result = Fixed64.Zero;
 5341821                return true;
 822            }
 823
 743824            Span<ulong> denominator = stackalloc ulong[MaxCoulombComparisonWords / 2];
 743825            Multiply3(transformedDenominatorMagnitude, commonDenominator, fixedScale, denominator);
 743826            return Fixed64.TryGetSignedRawRatio(
 743827                combined,
 743828                denominator,
 743829                combinedSign < 0,
 743830                out result);
 831        }
 832
 336833        Span<ulong> scaledNumerator =
 336834            stackalloc ulong[MaxCoulombComparisonWords / 2];
 336835        Span<ulong> cachedCombined = stackalloc ulong[state?.HasTangents == true ? 64 : 15];
 336836        Span<ulong> cachedDenominator = stackalloc ulong[12];
 336837        WideArithmetic.MultiplyMagnitudes(
 336838            combined,
 336839            radialNumerator,
 336840            scaledNumerator);
 336841        GetWeightedSignedWideSum(
 336842            primary,
 336843            accumulatedPrimaryTangentImpulse,
 336844            secondary,
 336845            accumulatedSecondaryTangentImpulse,
 336846            cachedCombined,
 336847            out int cachedSign, state);
 336848        WideArithmetic.MultiplyMagnitudes(
 336849            transformedDenominatorMagnitude,
 336850            fixedScale,
 336851            cachedDenominator);
 336852        return TryGetSignedRadicalAndRationalSum(
 336853            scaledNumerator,
 336854            radialDenominator,
 336855            radicand,
 336856            combinedSign,
 336857            cachedCombined,
 336858            cachedDenominator,
 336859            -cachedSign,
 336860            out result);
 861    }
 862
 863    private static void GetWeightedSignedSum(
 864        Fixed64 firstFactor,
 865        ReadOnlySpan<ulong> firstMagnitude,
 866        int firstSign,
 867        Fixed64 secondFactor,
 868        ReadOnlySpan<ulong> secondMagnitude,
 869        int secondSign,
 870        Span<ulong> result,
 871        out int resultSign)
 872    {
 21010873        Span<ulong> factorMagnitude = stackalloc ulong[3];
 21010874        Span<ulong> first = stackalloc ulong[MaxCoulombWords + 2];
 21010875        Span<ulong> second = stackalloc ulong[MaxCoulombWords + 2];
 21010876        SetMagnitude(firstFactor, factorMagnitude);
 21010877        WideArithmetic.MultiplyMagnitudes(
 21010878            factorMagnitude,
 21010879            firstMagnitude,
 21010880            first);
 21010881        SetMagnitude(secondFactor, factorMagnitude);
 21010882        WideArithmetic.MultiplyMagnitudes(
 21010883            factorMagnitude,
 21010884            secondMagnitude,
 21010885            second);
 21010886        AddSignedMagnitudes(
 21010887            first,
 21010888            firstFactor == Fixed64.Zero
 21010889                ? 0
 21010890                : firstFactor < Fixed64.Zero ? -firstSign : firstSign,
 21010891            second,
 21010892            secondFactor == Fixed64.Zero
 21010893                ? 0
 21010894                : secondFactor < Fixed64.Zero ? -secondSign : secondSign,
 21010895            result,
 21010896            out resultSign);
 21010897    }
 898
 899}

/home/runner/work/Gravitas/Gravitas/src/Gravitas/CollisionHandling/Response/Exact/ExactContactResponseKernel.Normal.cs

#LineLine coverage
 1//=======================================================================
 2// ExactContactResponseKernel.Normal.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 System.Runtime.CompilerServices;
 10
 11using FixedMathSharp;
 12using FixedMathSharp.Geometry;
 13
 14namespace Gravitas.CollisionHandling;
 15
 16/// <content>
 17/// Owns unilateral exact normal-response policy and materialization.
 18/// </content>
 19internal static partial class ExactContactResponseKernel
 20{
 21    [MethodImpl(MethodImplOptions.AggressiveInlining)]
 22    internal static bool TryGetNormalResponse(
 23        in ExactContactResponseOperand3D first,
 24        in ExactContactResponseOperand3D second,
 25        Vector3d normal,
 26        Fixed64 restitution,
 27        Fixed64 restitutionVelocityThreshold,
 28        out ExactNormalResponse3D response)
 29    {
 13430        ExactContactImpulseState? unused = null;
 13431        return TryGetNormalResponseCore(first, second, normal, restitution,
 13432            restitutionVelocityThreshold, Fixed64.Zero, Fixed64.One, Fixed64.One,
 13433            includeAccumulator: false, default, out response, ref unused, retainOversized: false);
 34    }
 35
 36    [MethodImpl(MethodImplOptions.AggressiveInlining)]
 37    internal static bool TryGetAccumulatedNormalResponse(
 38        in ExactContactResponseOperand3D first,
 39        in ExactContactResponseOperand3D second,
 40        Vector3d normal,
 41        Fixed64 restitution,
 42        Fixed64 restitutionVelocityThreshold,
 43        Fixed64 accumulatedImpulse,
 44        Fixed64 positiveImpulseScale,
 45        Fixed64 negativeImpulseScale,
 46        out ExactNormalResponse3D response,
 47        in ContactResponseSnapshot impact = default)
 48    {
 1193949        ExactContactImpulseState? unused = null;
 1193950        return TryGetNormalResponseCore(first, second, normal, restitution,
 1193951            restitutionVelocityThreshold, accumulatedImpulse, positiveImpulseScale,
 1193952            negativeImpulseScale, includeAccumulator: true, impact, out response,
 1193953            ref unused, retainOversized: false);
 54    }
 55
 56    [MethodImpl(MethodImplOptions.AggressiveInlining)]
 57    internal static bool TryGetAccumulatedNormalResponse(
 58        in ExactContactResponseOperand3D first,
 59        in ExactContactResponseOperand3D second,
 60        Vector3d normal,
 61        Fixed64 restitution,
 62        Fixed64 restitutionVelocityThreshold,
 63        Fixed64 accumulatedImpulse,
 64        Fixed64 positiveImpulseScale,
 65        Fixed64 negativeImpulseScale,
 66        out ExactNormalResponse3D response,
 67        in ContactResponseSnapshot impact,
 68        ref ExactContactImpulseState? exactImpulse) =>
 308669        TryGetNormalResponseCore(first, second, normal, restitution,
 308670            restitutionVelocityThreshold, accumulatedImpulse, positiveImpulseScale,
 308671            negativeImpulseScale, includeAccumulator: true, impact, out response,
 308672            ref exactImpulse, retainOversized: true);
 73
 74    [MethodImpl(MethodImplOptions.NoInlining)]
 75    private static bool TryGetNormalResponseCore(
 76        in ExactContactResponseOperand3D first,
 77        in ExactContactResponseOperand3D second,
 78        Vector3d normal,
 79        Fixed64 restitution,
 80        Fixed64 restitutionVelocityThreshold,
 81        Fixed64 accumulatedImpulse,
 82        Fixed64 positiveImpulseScale,
 83        Fixed64 negativeImpulseScale,
 84        bool includeAccumulator,
 85        in ContactResponseSnapshot impact,
 86        out ExactNormalResponse3D response,
 87        ref ExactContactImpulseState? exactImpulse,
 88        bool retainOversized)
 89    {
 1515990        response = default;
 1515991        if (first.Lever.Denominator.Sign == 0
 1515992            || second.Lever.Denominator.Sign == 0
 1515993            || restitution < Fixed64.Zero
 1515994            || restitutionVelocityThreshold < Fixed64.Zero
 1515995            || accumulatedImpulse < Fixed64.Zero
 1515996            || positiveImpulseScale < Fixed64.Zero
 1515997            || negativeImpulseScale < Fixed64.Zero
 1515998            || first.InverseMass < Fixed64.Zero
 1515999            || second.InverseMass < Fixed64.Zero
 15159100            || !normal.IsNormalized())
 101        {
 13102            return false;
 103        }
 104
 15146105        ExactLever3D.GetRelativePointVelocityRatio(
 15146106            first.LinearVelocity,
 15146107            first.AngularVelocity,
 15146108            first.Lever,
 15146109            second.LinearVelocity,
 15146110            second.AngularVelocity,
 15146111            second.Lever,
 15146112            normal,
 15146113            out Signed832 velocityNumerator,
 15146114            out Signed832 velocityDenominator, first.PlanarAngularVelocity, second.PlanarAngularVelocity);
 15146115        int velocitySign =
 15146116            velocityNumerator.Sign * velocityDenominator.Sign;
 15146117        bool hasNormalVelocity = Fixed64.TryGetSignedRawRatio(
 15146118            velocityNumerator,
 15146119            velocityDenominator,
 15146120            0,
 15146121            out Fixed64 normalVelocity);
 15146122        Span<ulong> effectiveNumerator =
 15146123            stackalloc ulong[MaxResponseWords];
 15146124        Span<ulong> effectiveDenominator =
 15146125            stackalloc ulong[MaxResponseWords];
 15146126        GetEffectiveMassRatio(
 15146127            first,
 15146128            second,
 15146129            normal,
 15146130            effectiveNumerator,
 15146131            effectiveDenominator);
 132
 15146133        bool denominatorTooSmall =
 15146134            WideArithmetic.IsZeroMagnitude(effectiveNumerator);
 15146135        if (denominatorTooSmall)
 136        {
 21137            if (velocitySign < 0 && !includeAccumulator)
 14138                return false;
 139
 7140            response = CreateZeroResponse(
 7141                velocitySign < 0,
 7142                hasNormalVelocity,
 7143                normalVelocity,
 7144                includeAccumulator,
 7145                accumulatedImpulse, exactImpulse);
 7146            return true;
 147        }
 148
 15125149        if (velocitySign == 0 && !impact.IsValid)
 150        {
 83151            response = CreateZeroResponse(
 83152                isClosing: false,
 83153                hasNormalVelocity,
 83154                normalVelocity,
 83155                includeAccumulator,
 83156                accumulatedImpulse, exactImpulse);
 83157            return true;
 158        }
 159
 15042160        bool applyRestitution = velocitySign < 0
 15042161            && (!hasNormalVelocity
 15042162                || normalVelocity < -restitutionVelocityThreshold);
 15042163        Fixed64 impulseScale = velocitySign < 0
 15042164            ? positiveImpulseScale
 15042165            : negativeImpulseScale;
 15042166        Span<ulong> impulseNumerator =
 15042167            stackalloc ulong[MaxResponseWords];
 15042168        Span<ulong> impulseDenominator =
 15042169            stackalloc ulong[MaxResponseWords];
 170        int impulseSign;
 15042171        if (impact.IsValid)
 172        {
 14580173            impulseSign = BuildImpactImpulseRatio(first, second, normal, impact,
 14580174                restitution, restitutionVelocityThreshold, velocityNumerator,
 14580175                velocityDenominator, effectiveNumerator, effectiveDenominator,
 14580176                impulseNumerator, impulseDenominator);
 177        }
 178        else
 179        {
 462180            BuildImpulseRatio(velocityNumerator, velocityDenominator,
 462181                effectiveNumerator, effectiveDenominator,
 462182                applyRestitution ? restitution : Fixed64.Zero, impulseScale,
 462183                impulseNumerator, impulseDenominator);
 462184            impulseSign = -velocitySign;
 185        }
 186
 15042187        Span<ulong> appliedNumerator =
 15042188            stackalloc ulong[MaxResponseWords];
 15042189        Span<ulong> appliedDenominator =
 15042190            stackalloc ulong[MaxResponseWords];
 15042191        appliedNumerator.Clear();
 15042192        appliedDenominator.Clear();
 15042193        Span<ulong> completedAccumulator = stackalloc ulong[MaxResponseWords];
 15042194        completedAccumulator.Clear();
 15042195        ResolveUnilateralImpulse(
 15042196            impulseNumerator,
 15042197            impulseDenominator,
 15042198            impulseSign,
 15042199            accumulatedImpulse,
 15042200            includeAccumulator,
 15042201            appliedNumerator,
 15042202            appliedDenominator,
 15042203            out int appliedSign,
 15042204            out bool useImpulseRatio,
 15042205            out bool hasAccumulatedProjection,
 15042206            out Fixed64 accumulatedProjection, completedAccumulator, exactImpulse);
 207
 15042208        ReadOnlySpan<ulong> resolvedImpulseNumerator =
 15042209            useImpulseRatio ? impulseNumerator : appliedNumerator;
 15042210        ReadOnlySpan<ulong> resolvedImpulseDenominator =
 15042211            useImpulseRatio ? impulseDenominator : appliedDenominator;
 15042212        bool hasAppliedImpulse =
 15042213            !WideArithmetic.IsZeroMagnitude(resolvedImpulseNumerator);
 15042214        bool hasAppliedProjection = Fixed64.TryGetSignedRawRatio(
 15042215            resolvedImpulseNumerator,
 15042216            resolvedImpulseDenominator,
 15042217            appliedSign < 0,
 15042218            out Fixed64 appliedImpulse);
 15042219        bool resolved = TryGetLinearVelocityDelta(
 15042220            first.LinearImpulseAxis,
 15042221            first.InverseMass,
 15042222            resolvedImpulseNumerator,
 15042223            resolvedImpulseDenominator,
 15042224            appliedSign,
 15042225            out Vector3d firstLinear);
 15042226        resolved &= TryGetAngularVelocityDelta(
 15042227            first.Lever,
 15042228            -normal,
 15042229            first.InverseInertia,
 15042230            resolvedImpulseNumerator,
 15042231            resolvedImpulseDenominator,
 15042232            appliedSign,
 15042233            out Vector3d firstAngular);
 15042234        resolved &= TryGetLinearVelocityDelta(
 15042235            second.LinearImpulseAxis,
 15042236            second.InverseMass,
 15042237            resolvedImpulseNumerator,
 15042238            resolvedImpulseDenominator,
 15042239            appliedSign,
 15042240            out Vector3d secondLinear);
 15042241        resolved &= TryGetAngularVelocityDelta(
 15042242            second.Lever,
 15042243            normal,
 15042244            second.InverseInertia,
 15042245            resolvedImpulseNumerator,
 15042246            resolvedImpulseDenominator,
 15042247            appliedSign,
 15042248            out Vector3d secondAngular);
 15042249        resolved &= first.CanRepresentAngularDelta(firstAngular);
 15042250        resolved &= second.CanRepresentAngularDelta(secondAngular);
 15042251        if (!resolved)
 24252            return false;
 15018253        if (retainOversized && (!hasAccumulatedProjection || exactImpulse?.HasNormal == true))
 254        {
 255            // Retained state must describe an applied delta. The oversized lane
 256            // verifies final motion before committing its accumulator.
 148257            if (!Vector3d.TryAdd(first.LinearVelocity, firstLinear, out _)
 148258                || !first.CanApplyAngularDelta(firstAngular)
 148259                || !Vector3d.TryAdd(second.LinearVelocity, secondLinear, out _)
 152260                || !second.CanApplyAngularDelta(secondAngular)) return false;
 144261            (exactImpulse ??= new ExactContactImpulseState()).SetNormal(completedAccumulator, impulseDenominator);
 262        }
 263
 15014264        response = new ExactNormalResponse3D(
 15014265            velocitySign < 0,
 15014266            hasAppliedImpulse,
 15014267            firstLinear,
 15014268            firstAngular,
 15014269            secondLinear,
 15014270            secondAngular,
 15014271            hasNormalVelocity,
 15014272            normalVelocity,
 15014273            hasAppliedProjection,
 15014274            appliedImpulse,
 15014275            includeAccumulator && hasAccumulatedProjection,
 15014276            accumulatedProjection);
 15014277        return true;
 278    }
 279
 280    private static ExactNormalResponse3D CreateZeroResponse(
 281        bool isClosing,
 282        bool hasNormalVelocity,
 283        Fixed64 normalVelocity,
 284        bool includeAccumulator,
 285        Fixed64 accumulatedImpulse,
 286        ExactContactImpulseState? exactImpulse = null)
 287    {
 90288        bool hasAccumulator = includeAccumulator;
 90289        if (includeAccumulator && exactImpulse?.HasNormal == true)
 2290            hasAccumulator = Fixed64.TryGetSignedRawRatio(exactImpulse.NormalNumerator,
 2291                exactImpulse.NormalDenominator, negative: false, out accumulatedImpulse);
 90292        return new(
 90293            isClosing,
 90294            hasAppliedImpulse: false,
 90295            Vector3d.Zero,
 90296            Vector3d.Zero,
 90297            Vector3d.Zero,
 90298            Vector3d.Zero,
 90299            hasNormalVelocity,
 90300            normalVelocity,
 90301            hasAppliedImpulseProjection: true,
 90302            Fixed64.Zero,
 90303            hasAccumulator,
 90304            accumulatedImpulse);
 305    }
 306
 307    private static void GetEffectiveMassRatio(
 308        in ExactContactResponseOperand3D first,
 309        in ExactContactResponseOperand3D second,
 310        Vector3d normal,
 311        Span<ulong> resultNumerator,
 312        Span<ulong> resultDenominator)
 313    {
 17516314        Signed192 normalSquared = GetRawDot(normal, normal);
 315
 17516316        Signed192 firstAllowed =
 17516317            GetRawDot(first.LinearImpulseAxis, -normal);
 17516318        Signed192 secondAllowed =
 17516319            GetRawDot(second.LinearImpulseAxis, normal);
 17516320        Signed320 linearNumerator = default;
 17516321        if (firstAllowed.Sign > 0
 17516322            && first.InverseMass > Fixed64.Zero)
 323        {
 6252324            linearNumerator = WideArithmetic.MultiplySigned192(
 6252325                firstAllowed,
 6252326                Signed192.Raw(first.InverseMass));
 327        }
 17516328        if (secondAllowed.Sign > 0
 17516329            && second.InverseMass > Fixed64.Zero)
 330        {
 13775331            linearNumerator = WideArithmetic.AddSigned320(
 13775332                linearNumerator,
 13775333                WideArithmetic.MultiplySigned192(
 13775334                    secondAllowed,
 13775335                    Signed192.Raw(second.InverseMass)));
 336        }
 337
 17516338        ExactLever3D.GetCrossProductQuadraticFormRatio(
 17516339            first.Lever,
 17516340            normal,
 17516341            first.InverseInertia,
 17516342            out Signed832 firstAngularNumerator,
 17516343            out Signed832 firstAngularDenominator);
 17516344        ExactLever3D.GetCrossProductQuadraticFormRatio(
 17516345            second.Lever,
 17516346            normal,
 17516347            second.InverseInertia,
 17516348            out Signed832 secondAngularNumerator,
 17516349            out Signed832 secondAngularDenominator);
 350
 17516351        Span<ulong> linearNumeratorMagnitude =
 17516352            stackalloc ulong[MaxResponseWords];
 17516353        Span<ulong> linearDenominatorMagnitude =
 17516354            stackalloc ulong[MaxResponseWords];
 17516355        Span<ulong> firstNumeratorMagnitude =
 17516356            stackalloc ulong[MaxResponseWords];
 17516357        Span<ulong> firstDenominatorMagnitude =
 17516358            stackalloc ulong[MaxResponseWords];
 17516359        Span<ulong> secondNumeratorMagnitude =
 17516360            stackalloc ulong[MaxResponseWords];
 17516361        Span<ulong> secondDenominatorMagnitude =
 17516362            stackalloc ulong[MaxResponseWords];
 17516363        SetMagnitude(linearNumerator, linearNumeratorMagnitude);
 17516364        SetMagnitude(normalSquared, linearDenominatorMagnitude);
 17516365        SetNonNegativeRatio(
 17516366            firstAngularNumerator,
 17516367            firstAngularDenominator,
 17516368            firstNumeratorMagnitude,
 17516369            firstDenominatorMagnitude);
 17516370        SetNonNegativeRatio(
 17516371            secondAngularNumerator,
 17516372            secondAngularDenominator,
 17516373            secondNumeratorMagnitude,
 17516374            secondDenominatorMagnitude);
 375
 376        // Add the angular terms first, then combine with the linear ratio.
 17516377        Span<ulong> angularDenominator = stackalloc ulong[MaxResponseWords];
 17516378        Span<ulong> angularNumerator = stackalloc ulong[MaxResponseWords];
 17516379        Span<ulong> temporary = stackalloc ulong[MaxResponseWords];
 17516380        WideArithmetic.MultiplyMagnitudes(firstDenominatorMagnitude, secondDenominatorMagnitude, angularDenominator);
 17516381        WideArithmetic.MultiplyMagnitudes(firstNumeratorMagnitude, secondDenominatorMagnitude, angularNumerator);
 17516382        WideArithmetic.MultiplyMagnitudes(secondNumeratorMagnitude, firstDenominatorMagnitude, temporary);
 17516383        WideArithmetic.AddMagnitudeInto(temporary, angularNumerator);
 17516384        WideArithmetic.MultiplyMagnitudes(linearDenominatorMagnitude, angularDenominator, resultDenominator);
 17516385        WideArithmetic.MultiplyMagnitudes(linearNumeratorMagnitude, angularDenominator, resultNumerator);
 17516386        WideArithmetic.MultiplyMagnitudes(linearDenominatorMagnitude, angularNumerator, temporary);
 17516387        WideArithmetic.AddMagnitudeInto(temporary, resultNumerator);
 17516388    }
 389
 390    private static int BuildImpactImpulseRatio(
 391        in ExactContactResponseOperand3D first, in ExactContactResponseOperand3D second,
 392        Vector3d normal, in ContactResponseSnapshot impact, Fixed64 restitution,
 393        Fixed64 threshold, Signed832 velocityNumerator, Signed832 velocityDenominator,
 394        ReadOnlySpan<ulong> effectiveNumerator, ReadOnlySpan<ulong> effectiveDenominator,
 395        Span<ulong> numerator, Span<ulong> denominator)
 396    {
 14584397        Signed832 incoming = default;
 14584398        Fixed64 bounce = Fixed64.Zero;
 14584399        if (restitution != Fixed64.Zero)
 400        {
 2320401            ExactLever3D.GetRelativePointVelocityRatio(impact.LinearA, impact.AngularA,
 2320402                first.Lever, impact.LinearB, impact.AngularB, second.Lever, normal,
 2320403                out incoming, out Signed832 incomingDenominator,
 2320404                firstPlanarAngularVelocity: first.PlanarAngularVelocity.HasValue ? impact.PlanarAngularA : null,
 2320405                secondPlanarAngularVelocity: second.PlanarAngularVelocity.HasValue ? impact.PlanarAngularB : null);
 2320406            bool representable = Fixed64.TryGetSignedRawRatio(incoming, incomingDenominator,
 2320407                0, out Fixed64 incomingVelocity);
 2320408            if (incoming.Sign * incomingDenominator.Sign < 0
 2320409                && (!representable || incomingVelocity < -threshold))
 609410                bounce = restitution;
 411        }
 14584412        Span<ulong> currentMagnitude = stackalloc ulong[MaxResponseWords];
 14584413        Span<ulong> incomingMagnitude = stackalloc ulong[MaxResponseWords];
 14584414        Span<ulong> error = stackalloc ulong[MaxCoulombWords + 2];
 14584415        Span<ulong> velocityDenominatorMagnitude = stackalloc ulong[MaxResponseWords];
 14584416        SetMagnitude(velocityNumerator, currentMagnitude);
 14584417        SetMagnitude(incoming, incomingMagnitude);
 418        // Both velocities use the same frozen levers, so their exact denominators
 419        // are identical. Fuse v_current + e*v_incoming before effective-mass division.
 420        // Its width is bounded by the existing restitution product plus one carry.
 14584421        GetWeightedSignedSum(Fixed64.One, currentMagnitude, velocityNumerator.Sign,
 14584422            bounce, incomingMagnitude, incoming.Sign, error, out int errorSign);
 14584423        SetMagnitude(velocityDenominator, velocityDenominatorMagnitude);
 14584424        WideArithmetic.MultiplyMagnitudes(error[..MaxResponseWords], effectiveDenominator, numerator);
 14584425        WideArithmetic.MultiplyMagnitudes(velocityDenominatorMagnitude, effectiveNumerator, denominator);
 14584426        return -errorSign * velocityDenominator.Sign;
 427    }
 428
 429    private static void BuildImpulseRatio(
 430        Signed832 velocityNumerator,
 431        Signed832 velocityDenominator,
 432        ReadOnlySpan<ulong> effectiveNumerator,
 433        ReadOnlySpan<ulong> effectiveDenominator,
 434        Fixed64 restitution,
 435        Fixed64 impulseScale,
 436        Span<ulong> numerator,
 437        Span<ulong> denominator)
 438    {
 1507439        Span<ulong> velocityNumeratorMagnitude =
 1507440            stackalloc ulong[MaxResponseWords];
 1507441        Span<ulong> velocityDenominatorMagnitude =
 1507442            stackalloc ulong[MaxResponseWords];
 1507443        Span<ulong> restitutionMagnitude =
 1507444            stackalloc ulong[MaxResponseWords];
 1507445        Span<ulong> scaleMagnitude =
 1507446            stackalloc ulong[MaxResponseWords];
 1507447        Span<ulong> fixedScale =
 1507448            stackalloc ulong[MaxResponseWords];
 1507449        SetMagnitude(velocityNumerator, velocityNumeratorMagnitude);
 1507450        SetMagnitude(velocityDenominator, velocityDenominatorMagnitude);
 1507451        SetUnsignedSum(
 1507452            (ulong)FixedMath.ONE_L,
 1507453            (ulong)restitution.m_rawValue,
 1507454            restitutionMagnitude);
 1507455        SetMagnitude(impulseScale, scaleMagnitude);
 1507456        fixedScale.Clear();
 1507457        fixedScale[0] = (ulong)FixedMath.ONE_L;
 458
 1507459        Multiply4(
 1507460            velocityNumeratorMagnitude,
 1507461            effectiveDenominator,
 1507462            restitutionMagnitude,
 1507463            scaleMagnitude,
 1507464            numerator);
 1507465        Multiply3(
 1507466            velocityDenominatorMagnitude,
 1507467            effectiveNumerator,
 1507468            fixedScale,
 1507469            denominator);
 1507470    }
 471
 472    private static void ResolveUnilateralImpulse(
 473        ReadOnlySpan<ulong> impulseNumerator,
 474        ReadOnlySpan<ulong> impulseDenominator,
 475        int impulseSign,
 476        Fixed64 accumulatedImpulse,
 477        bool includeAccumulator,
 478        Span<ulong> appliedNumerator,
 479        Span<ulong> appliedDenominator,
 480        out int appliedSign,
 481        out bool useImpulseRatio,
 482        out bool hasAccumulatedProjection,
 483        out Fixed64 accumulatedProjection,
 484        Span<ulong> completedAccumulator,
 485        ExactContactImpulseState? exactImpulse = null)
 486    {
 15042487        appliedNumerator.Clear();
 15042488        appliedDenominator.Clear();
 15042489        useImpulseRatio = false;
 15042490        hasAccumulatedProjection = false;
 15042491        accumulatedProjection = default;
 15042492        if (!includeAccumulator)
 493        {
 109494            if (impulseSign <= 0)
 495            {
 2496                appliedDenominator[0] = 1UL;
 2497                appliedSign = 0;
 2498                return;
 499            }
 500
 107501            appliedSign = 1;
 107502            useImpulseRatio = true;
 107503            return;
 504        }
 505
 14933506        Span<ulong> accumulatorMagnitude =
 14933507            stackalloc ulong[MaxResponseWords];
 14933508        Span<ulong> accumulatedAtImpulseDenominator =
 14933509            stackalloc ulong[MaxResponseWords];
 14933510        SetMagnitude(accumulatedImpulse, accumulatorMagnitude);
 14933511        if (exactImpulse?.HasNormal == true)
 512        {
 513            // Frozen anchors, inertia and axes give every iteration the same
 514            // denominator. Adding numerators therefore adds only carry bits,
 515            // rather than multiplying denominators on every solver iteration.
 516#if DEBUG
 517            System.Diagnostics.Debug.Assert(WideArithmetic.CompareMagnitudeEqualLength(
 518                exactImpulse.NormalDenominator, impulseDenominator) == 0);
 519#endif
 97520            exactImpulse.NormalNumerator.CopyTo(accumulatedAtImpulseDenominator);
 521        }
 522        else
 523        {
 14836524            WideArithmetic.MultiplyMagnitudes(
 14836525                accumulatorMagnitude,
 14836526                impulseDenominator,
 14836527                accumulatedAtImpulseDenominator);
 528        }
 14933529        if (impulseSign < 0
 14933530            && WideArithmetic.CompareMagnitudeEqualLength(
 14933531                impulseNumerator,
 14933532                accumulatedAtImpulseDenominator) >= 0)
 533        {
 1018534            if (exactImpulse?.HasNormal == true)
 535            {
 2536                accumulatedAtImpulseDenominator.CopyTo(appliedNumerator);
 2537                impulseDenominator.CopyTo(appliedDenominator);
 538            }
 539            else
 540            {
 1016541                accumulatorMagnitude.CopyTo(appliedNumerator);
 1016542                appliedDenominator[0] = 1UL;
 543            }
 1018544            appliedSign = WideArithmetic.IsZeroMagnitude(
 1018545                appliedNumerator)
 1018546                ? 0
 1018547                : -1;
 1018548            hasAccumulatedProjection = true;
 1018549            accumulatedProjection = Fixed64.Zero;
 1018550            return;
 551        }
 552
 13915553        if (impulseSign > 0)
 554        {
 8839555            accumulatedAtImpulseDenominator.CopyTo(completedAccumulator);
 8839556            WideArithmetic.AddMagnitudeInto(
 8839557                impulseNumerator,
 8839558                completedAccumulator);
 559        }
 560        else
 561        {
 5076562            WideArithmetic.SubtractEqualMagnitudes(
 5076563                accumulatedAtImpulseDenominator,
 5076564                impulseNumerator,
 5076565                completedAccumulator);
 566        }
 567
 13915568        hasAccumulatedProjection = Fixed64.TryGetSignedRawRatio(
 13915569            completedAccumulator,
 13915570            impulseDenominator,
 13915571            negative: false,
 13915572            out accumulatedProjection);
 13915573        appliedSign = impulseSign;
 13915574        useImpulseRatio = true;
 13915575    }
 576
 577    private static bool TryGetLinearVelocityDelta(
 578        Vector3d impulseAxis,
 579        Fixed64 inverseMass,
 580        ReadOnlySpan<ulong> impulseNumerator,
 581        ReadOnlySpan<ulong> impulseDenominator,
 582        int impulseSign,
 583        out Vector3d result)
 584    {
 30258585        if (inverseMass == Fixed64.Zero
 30258586            || impulseAxis == Vector3d.Zero
 30258587            || WideArithmetic.IsZeroMagnitude(impulseNumerator))
 588        {
 15048589            result = Vector3d.Zero;
 15048590            return true;
 591        }
 592
 593        // Share the mass product and Q32.32 scale factors across all axes.
 15210594        Span<ulong> inverseMassMagnitude = stackalloc ulong[3];
 15210595        Span<ulong> scaledNumerator = stackalloc ulong[MaxResponseWords];
 15210596        Span<ulong> denominator = stackalloc ulong[MaxResponseWords];
 15210597        SetMagnitude(inverseMass, inverseMassMagnitude);
 15210598        WideArithmetic.MultiplyMagnitudes(impulseNumerator, inverseMassMagnitude, scaledNumerator);
 15210599        ReadOnlySpan<ulong> fixedScaleSquared = stackalloc ulong[] { 0UL, 1UL };
 15210600        WideArithmetic.MultiplyMagnitudes(impulseDenominator, fixedScaleSquared, denominator);
 15210601        bool xResolved = TryGetLinearComponent(
 15210602            impulseAxis.X,
 15210603            scaledNumerator,
 15210604            denominator,
 15210605            impulseSign,
 15210606            out Fixed64 x);
 15210607        bool yResolved = TryGetLinearComponent(
 15210608            impulseAxis.Y,
 15210609            scaledNumerator,
 15210610            denominator,
 15210611            impulseSign,
 15210612            out Fixed64 y);
 15210613        bool zResolved = TryGetLinearComponent(
 15210614            impulseAxis.Z,
 15210615            scaledNumerator,
 15210616            denominator,
 15210617            impulseSign,
 15210618            out Fixed64 z);
 15210619        result = xResolved & yResolved & zResolved
 15210620            ? new Vector3d(x, y, z)
 15210621            : default;
 15210622        return xResolved & yResolved & zResolved;
 623    }
 624
 625    private static bool TryGetLinearComponent(
 626        Fixed64 axis,
 627        ReadOnlySpan<ulong> scaledNumerator,
 628        ReadOnlySpan<ulong> denominator,
 629        int impulseSign,
 630        out Fixed64 result)
 631    {
 45630632        if (axis == Fixed64.Zero)
 633        {
 26332634            result = Fixed64.Zero;
 26332635            return true;
 636        }
 637
 19298638        Span<ulong> axisMagnitude = stackalloc ulong[3];
 19298639        Span<ulong> numerator = stackalloc ulong[MaxResponseWords];
 19298640        SetMagnitude(axis, axisMagnitude);
 19298641        WideArithmetic.MultiplyMagnitudes(axisMagnitude, scaledNumerator, numerator);
 19298642        return Fixed64.TryGetSignedRawRatio(
 19298643            numerator,
 19298644            denominator,
 19298645            (axis < Fixed64.Zero) != (impulseSign < 0),
 19298646            out result);
 647    }
 648
 649    private static bool TryGetAngularVelocityDelta(
 650        in ExactLever3D lever,
 651        Vector3d impulseAxis,
 652        Fixed3x3 inverseInertia,
 653        ReadOnlySpan<ulong> impulseNumerator,
 654        ReadOnlySpan<ulong> impulseDenominator,
 655        int impulseSign,
 656        out Vector3d result)
 657    {
 30258658        if (inverseInertia == Fixed3x3.Zero || WideArithmetic.IsZeroMagnitude(impulseNumerator))
 659        {
 15443660            result = Vector3d.Zero;
 15443661            return true;
 662        }
 663
 14815664        ExactLever3D.GetTransformedCrossProduct(
 14815665            lever,
 14815666            impulseAxis,
 14815667            inverseInertia,
 14815668            out Signed832 transformedX,
 14815669            out Signed832 transformedY,
 14815670            out Signed832 transformedZ);
 14815671        Signed320 fixedScaleSquared = WideArithmetic.MultiplySigned192(
 14815672            Signed192.One,
 14815673            Signed192.One);
 14815674        Signed704 transformedDenominator =
 14815675            WideArithmetic.MultiplySigned576ToSigned704(
 14815676                lever.Denominator,
 14815677                fixedScaleSquared);
 14815678        Span<ulong> transformedDenominatorMagnitude = stackalloc ulong[11];
 14815679        Span<ulong> denominator = stackalloc ulong[MaxResponseWords];
 14815680        SetMagnitude(transformedDenominator, transformedDenominatorMagnitude);
 14815681        ReadOnlySpan<ulong> fixedScale = stackalloc ulong[] { (ulong)FixedMath.ONE_L };
 14815682        Multiply3(transformedDenominatorMagnitude, impulseDenominator, fixedScale, denominator);
 14815683        bool xResolved = TryGetAngularComponent(
 14815684            transformedX,
 14815685            impulseNumerator,
 14815686            denominator,
 14815687            impulseSign,
 14815688            out Fixed64 x);
 14815689        bool yResolved = TryGetAngularComponent(
 14815690            transformedY,
 14815691            impulseNumerator,
 14815692            denominator,
 14815693            impulseSign,
 14815694            out Fixed64 y);
 14815695        bool zResolved = TryGetAngularComponent(
 14815696            transformedZ,
 14815697            impulseNumerator,
 14815698            denominator,
 14815699            impulseSign,
 14815700            out Fixed64 z);
 14815701        result = xResolved & yResolved & zResolved
 14815702            ? new Vector3d(x, y, z)
 14815703            : default;
 14815704        return xResolved & yResolved & zResolved;
 705    }
 706
 707    private static bool TryGetAngularComponent(
 708        Signed832 transformed,
 709        ReadOnlySpan<ulong> impulseNumerator,
 710        ReadOnlySpan<ulong> denominator,
 711        int impulseSign,
 712        out Fixed64 result)
 713    {
 44445714        if (transformed.Sign == 0)
 715        {
 6442716            result = Fixed64.Zero;
 6442717            return true;
 718        }
 719
 38003720        Span<ulong> transformedMagnitude = stackalloc ulong[13];
 38003721        Span<ulong> numerator = stackalloc ulong[MaxResponseWords];
 38003722        SetMagnitude(transformed, transformedMagnitude);
 38003723        WideArithmetic.MultiplyMagnitudes(transformedMagnitude, impulseNumerator, numerator);
 38003724        return Fixed64.TryGetSignedRawRatio(
 38003725            numerator,
 38003726            denominator,
 38003727            (transformed.Sign < 0) != (impulseSign < 0),
 38003728            out result);
 729    }
 730
 731    private static Signed192 GetRawDot(Vector3d left, Vector3d right) =>
 52548732        WideGeometry.GetDifferenceDotProduct3D(
 52548733            left.X,
 52548734            Fixed64.Zero,
 52548735            left.Y,
 52548736            Fixed64.Zero,
 52548737            left.Z,
 52548738            Fixed64.Zero,
 52548739            right.X,
 52548740            Fixed64.Zero,
 52548741            right.Y,
 52548742            Fixed64.Zero,
 52548743            right.Z,
 52548744            Fixed64.Zero);
 745
 746    private static void SetNonNegativeRatio(
 747        Signed832 numerator,
 748        Signed832 denominator,
 749        Span<ulong> numeratorMagnitude,
 750        Span<ulong> denominatorMagnitude)
 751    {
 35032752        numeratorMagnitude.Clear();
 35032753        denominatorMagnitude.Clear();
 35032754        if (numerator.Sign <= 0)
 755        {
 17122756            denominatorMagnitude[0] = 1UL;
 17122757            return;
 758        }
 759
 17910760        SetMagnitude(numerator, numeratorMagnitude);
 17910761        SetMagnitude(denominator, denominatorMagnitude);
 17910762    }
 763
 764    private static void SetUnsignedSum(
 765        ulong first,
 766        ulong second,
 767        Span<ulong> result)
 768    {
 1507769        result.Clear();
 1507770        result[0] = unchecked(first + second);
 1507771    }
 772
 773    private static void SetMagnitude(
 774        Fixed64 value,
 775        Span<ulong> result)
 776    {
 105165777        result.Clear();
 105165778        WideArithmetic.GetMagnitude(
 105165779            Signed192.Raw(value),
 105165780            out result[2],
 105165781            out result[1],
 105165782            out result[0]);
 105165783    }
 784
 785    private static void SetMagnitude(
 786        Signed192 value,
 787        Span<ulong> result)
 788    {
 17516789        result.Clear();
 17516790        WideArithmetic.GetMagnitude(
 17516791            value,
 17516792            out result[2],
 17516793            out result[1],
 17516794            out result[0]);
 17516795    }
 796
 797    private static void SetMagnitude(
 798        Signed320 value,
 799        Span<ulong> result)
 800    {
 17516801        result.Clear();
 17516802        WideArithmetic.GetMagnitude(
 17516803            value,
 17516804            out result[4],
 17516805            out result[3],
 17516806            out result[2],
 17516807            out result[1],
 17516808            out result[0]);
 17516809    }
 810
 811    private static void SetMagnitude(
 812        Signed704 value,
 813        Span<ulong> result)
 814    {
 21235815        result.Clear();
 21235816        WideArithmetic.GetMagnitude(value, result);
 21235817    }
 818
 819    private static void SetMagnitude(
 820        Signed832 value,
 821        Span<ulong> result)
 822    {
 134101823        result.Clear();
 134101824        WideArithmetic.GetMagnitude(value, result);
 134101825    }
 826
 827    private static void Multiply3(
 828        ReadOnlySpan<ulong> first,
 829        ReadOnlySpan<ulong> second,
 830        ReadOnlySpan<ulong> third,
 831        Span<ulong> result)
 832    {
 18739833        Span<ulong> temporary =
 18739834            stackalloc ulong[result.Length];
 18739835        WideArithmetic.MultiplyMagnitudes(first, second, temporary);
 18739836        WideArithmetic.MultiplyMagnitudes(temporary, third, result);
 18739837    }
 838
 839    private static void Multiply4(
 840        ReadOnlySpan<ulong> first,
 841        ReadOnlySpan<ulong> second,
 842        ReadOnlySpan<ulong> third,
 843        ReadOnlySpan<ulong> fourth,
 844        Span<ulong> result)
 845    {
 1507846        Span<ulong> firstProduct =
 1507847            stackalloc ulong[MaxResponseWords];
 1507848        Span<ulong> secondProduct =
 1507849            stackalloc ulong[MaxResponseWords];
 1507850        WideArithmetic.MultiplyMagnitudes(first, second, firstProduct);
 1507851        WideArithmetic.MultiplyMagnitudes(third, fourth, secondProduct);
 1507852        WideArithmetic.MultiplyMagnitudes(
 1507853            firstProduct,
 1507854            secondProduct,
 1507855            result);
 1507856    }
 857
 858}

Methods/Properties

TryGetCoulombLineResponse(Gravitas.CollisionHandling.ExactNormalConstraint3D&,Gravitas.CollisionHandling.ExactContactResponseOperand3D&,Gravitas.CollisionHandling.ExactContactResponseOperand3D&,FixedMathSharp.Vector3d,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,Gravitas.CollisionHandling.ExactCoulombResponse3D&,Gravitas.CollisionHandling.ContactResponseSnapshot&)
TryGetCoulombDiskResponse(FixedMathSharp.Vector3d,FixedMathSharp.Fixed64,Gravitas.CollisionHandling.ExactContactResponseOperand3D&,Gravitas.CollisionHandling.ExactContactResponseOperand3D&,FixedMathSharp.Vector3d,FixedMathSharp.Fixed64,Gravitas.CollisionHandling.ExactContactResponseOperand3D&,Gravitas.CollisionHandling.ExactContactResponseOperand3D&,FixedMathSharp.Vector3d,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,Gravitas.CollisionHandling.ExactCoulombResponse3D&)
TryGetCoulombDiskResponse(FixedMathSharp.Vector3d,FixedMathSharp.Fixed64,Gravitas.CollisionHandling.ExactContactResponseOperand3D&,Gravitas.CollisionHandling.ExactContactResponseOperand3D&,FixedMathSharp.Vector3d,FixedMathSharp.Fixed64,Gravitas.CollisionHandling.ExactContactResponseOperand3D&,Gravitas.CollisionHandling.ExactContactResponseOperand3D&,FixedMathSharp.Vector3d,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,Gravitas.CollisionHandling.ExactCoulombResponse3D&,Gravitas.CollisionHandling.ExactContactImpulseState&)
TryGetCoulombDiskResponseRetained(FixedMathSharp.Vector3d,FixedMathSharp.Fixed64,Gravitas.CollisionHandling.ExactContactResponseOperand3D&,Gravitas.CollisionHandling.ExactContactResponseOperand3D&,FixedMathSharp.Vector3d,FixedMathSharp.Fixed64,Gravitas.CollisionHandling.ExactContactResponseOperand3D&,Gravitas.CollisionHandling.ExactContactResponseOperand3D&,FixedMathSharp.Vector3d,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,Gravitas.CollisionHandling.ExactCoulombResponse3D&,Gravitas.CollisionHandling.ExactContactImpulseState&,System.Boolean)
TryGetCoulombDiskResponse(Gravitas.CollisionHandling.ExactNormalConstraint3D&,Gravitas.CollisionHandling.ExactContactResponseOperand3D&,Gravitas.CollisionHandling.ExactContactResponseOperand3D&,FixedMathSharp.Vector3d,FixedMathSharp.Fixed64,Gravitas.CollisionHandling.ExactContactResponseOperand3D&,Gravitas.CollisionHandling.ExactContactResponseOperand3D&,FixedMathSharp.Vector3d,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,Gravitas.CollisionHandling.ExactCoulombResponse3D&)
TryGetCoulombDiskResponseCore(System.ReadOnlySpan`1<System.UInt64>,System.ReadOnlySpan`1<System.UInt64>,Gravitas.CollisionHandling.ExactContactResponseOperand3D&,Gravitas.CollisionHandling.ExactContactResponseOperand3D&,FixedMathSharp.Vector3d,FixedMathSharp.Fixed64,Gravitas.CollisionHandling.ExactContactResponseOperand3D&,Gravitas.CollisionHandling.ExactContactResponseOperand3D&,FixedMathSharp.Vector3d,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,Gravitas.CollisionHandling.ExactCoulombResponse3D&,Gravitas.CollisionHandling.ExactContactImpulseState&,System.Boolean)
AreCoulombDiskInputsValid(FixedMathSharp.Vector3d,Gravitas.CollisionHandling.ExactContactResponseOperand3D&,Gravitas.CollisionHandling.ExactContactResponseOperand3D&,FixedMathSharp.Vector3d,Gravitas.CollisionHandling.ExactContactResponseOperand3D&,Gravitas.CollisionHandling.ExactContactResponseOperand3D&,FixedMathSharp.Vector3d,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64)
TryGetCompletedNormalAccumulatorRatio(Gravitas.CollisionHandling.ExactNormalConstraint3D&,System.Span`1<System.UInt64>,System.Span`1<System.UInt64>,Gravitas.CollisionHandling.ContactResponseSnapshot&)
GetBilateralImpulseRatio(Gravitas.CollisionHandling.ExactContactResponseOperand3D&,Gravitas.CollisionHandling.ExactContactResponseOperand3D&,FixedMathSharp.Vector3d,FixedMathSharp.Fixed64,System.Span`1<System.UInt64>,System.Span`1<System.UInt64>,System.Int32&)
GetFrictionLimit(System.ReadOnlySpan`1<System.UInt64>,System.ReadOnlySpan`1<System.UInt64>,FixedMathSharp.Fixed64,System.Span`1<System.UInt64>,System.Span`1<System.UInt64>)
AddFixedToRatio(System.ReadOnlySpan`1<System.UInt64>,System.ReadOnlySpan`1<System.UInt64>,System.Int32,FixedMathSharp.Fixed64,System.Span`1<System.UInt64>,System.Int32&)
SubtractFixedFromRatio(System.ReadOnlySpan`1<System.UInt64>,System.ReadOnlySpan`1<System.UInt64>,System.Int32,FixedMathSharp.Fixed64,System.Span`1<System.UInt64>,System.Span`1<System.UInt64>,System.Int32&)
CompareRatios(System.ReadOnlySpan`1<System.UInt64>,System.ReadOnlySpan`1<System.UInt64>,System.ReadOnlySpan`1<System.UInt64>,System.ReadOnlySpan`1<System.UInt64>)
IsVectorWithinLimit(System.ReadOnlySpan`1<System.UInt64>,System.ReadOnlySpan`1<System.UInt64>,System.ReadOnlySpan`1<System.UInt64>,System.ReadOnlySpan`1<System.UInt64>)
AddSignedMagnitudes(System.ReadOnlySpan`1<System.UInt64>,System.Int32,System.ReadOnlySpan`1<System.UInt64>,System.Int32,System.Span`1<System.UInt64>,System.Int32&)
TryGetSignedRatioOverSquareRoot(System.ReadOnlySpan`1<System.UInt64>,System.ReadOnlySpan`1<System.UInt64>,System.ReadOnlySpan`1<System.UInt64>,System.Boolean,FixedMathSharp.Fixed64&)
CompareRatioOverSquareRoot(System.ReadOnlySpan`1<System.UInt64>,System.ReadOnlySpan`1<System.UInt64>,System.ReadOnlySpan`1<System.UInt64>,System.UInt64)
CompareRatioOverSquareRoot(System.ReadOnlySpan`1<System.UInt64>,System.ReadOnlySpan`1<System.UInt64>,System.ReadOnlySpan`1<System.UInt64>,System.ReadOnlySpan`1<System.UInt64>,System.UInt64)
HaveMatchingParticipants(Gravitas.CollisionHandling.ExactContactResponseOperand3D&,Gravitas.CollisionHandling.ExactContactResponseOperand3D&)
AddCachedTangentToRatio(System.ReadOnlySpan`1<System.UInt64>,System.ReadOnlySpan`1<System.UInt64>,System.Int32,FixedMathSharp.Fixed64,Gravitas.CollisionHandling.ExactContactImpulseState,System.Boolean,System.Span`1<System.UInt64>,System.Int32&)
SubtractCachedTangentFromRatio(System.ReadOnlySpan`1<System.UInt64>,System.ReadOnlySpan`1<System.UInt64>,System.Int32,FixedMathSharp.Fixed64,Gravitas.CollisionHandling.ExactContactImpulseState,System.Boolean,System.Span`1<System.UInt64>,System.Span`1<System.UInt64>,System.Int32&)
RoundCachedRatio(System.ReadOnlySpan`1<System.UInt64>,System.ReadOnlySpan`1<System.UInt64>,System.Span`1<System.UInt64>)
RoundCachedRadialRatio(System.ReadOnlySpan`1<System.UInt64>,System.ReadOnlySpan`1<System.UInt64>,System.ReadOnlySpan`1<System.UInt64>,System.Span`1<System.UInt64>)
IsRadialProjectionEqualToFixed(System.ReadOnlySpan`1<System.UInt64>,System.Int32,System.ReadOnlySpan`1<System.UInt64>,System.ReadOnlySpan`1<System.UInt64>,System.ReadOnlySpan`1<System.UInt64>,FixedMathSharp.Fixed64,Gravitas.CollisionHandling.ExactContactImpulseState,System.Boolean)
TryGetSignedRadicalAndRationalSum(System.ReadOnlySpan`1<System.UInt64>,System.ReadOnlySpan`1<System.UInt64>,System.ReadOnlySpan`1<System.UInt64>,System.Int32,System.ReadOnlySpan`1<System.UInt64>,System.ReadOnlySpan`1<System.UInt64>,System.Int32,FixedMathSharp.Fixed64&)
TryGetSignedSumIntegerParts(System.ReadOnlySpan`1<System.UInt64>,System.ReadOnlySpan`1<System.UInt64>,System.ReadOnlySpan`1<System.UInt64>,System.Int32,System.ReadOnlySpan`1<System.UInt64>,System.ReadOnlySpan`1<System.UInt64>,System.Int32,System.Int64&)
GetRadialQuotientBitBound(System.ReadOnlySpan`1<System.UInt64>,System.ReadOnlySpan`1<System.UInt64>,System.ReadOnlySpan`1<System.UInt64>)
ComparePreparedSignedSum(System.ReadOnlySpan`1<System.UInt64>,System.Int32,System.ReadOnlySpan`1<System.UInt64>,System.Int32,System.ReadOnlySpan`1<System.UInt64>,System.ReadOnlySpan`1<System.UInt64>,System.UInt64,System.Boolean)
GetWeightedFixedSum(FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,System.Span`1<System.UInt64>,System.Int32&,Gravitas.CollisionHandling.ExactContactImpulseState)
GetWeightedSignedWideSum(FixedMathSharp.Signed832,FixedMathSharp.Fixed64,FixedMathSharp.Signed832,FixedMathSharp.Fixed64,System.Span`1<System.UInt64>,System.Int32&,Gravitas.CollisionHandling.ExactContactImpulseState)
GetProductSign(FixedMathSharp.Fixed64,FixedMathSharp.Fixed64)
TrimMagnitude(System.ReadOnlySpan`1<System.UInt64>)
TryGetDiskVelocityDeltas(Gravitas.CollisionHandling.ExactContactResponseOperand3D&,Gravitas.CollisionHandling.ExactContactResponseOperand3D&,Gravitas.CollisionHandling.ExactContactResponseOperand3D&,Gravitas.CollisionHandling.ExactContactResponseOperand3D&,FixedMathSharp.Vector3d,FixedMathSharp.Vector3d,System.ReadOnlySpan`1<System.UInt64>,System.Int32,System.ReadOnlySpan`1<System.UInt64>,System.Int32,System.ReadOnlySpan`1<System.UInt64>,System.Boolean,System.ReadOnlySpan`1<System.UInt64>,System.ReadOnlySpan`1<System.UInt64>,System.ReadOnlySpan`1<System.UInt64>,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,FixedMathSharp.Vector3d&,FixedMathSharp.Vector3d&,FixedMathSharp.Vector3d&,FixedMathSharp.Vector3d&,Gravitas.CollisionHandling.ExactContactImpulseState)
TryGetDiskLinearVelocityDelta(FixedMathSharp.Vector3d,FixedMathSharp.Vector3d,FixedMathSharp.Fixed64,System.ReadOnlySpan`1<System.UInt64>,System.Int32,System.ReadOnlySpan`1<System.UInt64>,System.Int32,System.ReadOnlySpan`1<System.UInt64>,System.Boolean,System.ReadOnlySpan`1<System.UInt64>,System.ReadOnlySpan`1<System.UInt64>,System.ReadOnlySpan`1<System.UInt64>,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,FixedMathSharp.Vector3d&,Gravitas.CollisionHandling.ExactContactImpulseState)
TryGetDiskLinearComponent(FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,System.ReadOnlySpan`1<System.UInt64>,System.Int32,System.ReadOnlySpan`1<System.UInt64>,System.Int32,System.ReadOnlySpan`1<System.UInt64>,System.Boolean,System.ReadOnlySpan`1<System.UInt64>,System.ReadOnlySpan`1<System.UInt64>,System.ReadOnlySpan`1<System.UInt64>,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64&,Gravitas.CollisionHandling.ExactContactImpulseState)
TryGetDiskAngularVelocityDelta(Gravitas.CollisionHandling.ExactLever3D&,FixedMathSharp.Vector3d,FixedMathSharp.Vector3d,FixedMathSharp.Fixed3x3,System.ReadOnlySpan`1<System.UInt64>,System.Int32,System.ReadOnlySpan`1<System.UInt64>,System.Int32,System.ReadOnlySpan`1<System.UInt64>,System.Boolean,System.ReadOnlySpan`1<System.UInt64>,System.ReadOnlySpan`1<System.UInt64>,System.ReadOnlySpan`1<System.UInt64>,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,FixedMathSharp.Vector3d&,Gravitas.CollisionHandling.ExactContactImpulseState)
TryGetDiskAngularComponent(FixedMathSharp.Signed832,FixedMathSharp.Signed832,FixedMathSharp.Signed704,System.ReadOnlySpan`1<System.UInt64>,System.Int32,System.ReadOnlySpan`1<System.UInt64>,System.Int32,System.ReadOnlySpan`1<System.UInt64>,System.Boolean,System.ReadOnlySpan`1<System.UInt64>,System.ReadOnlySpan`1<System.UInt64>,System.ReadOnlySpan`1<System.UInt64>,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64&,Gravitas.CollisionHandling.ExactContactImpulseState)
GetWeightedSignedSum(FixedMathSharp.Fixed64,System.ReadOnlySpan`1<System.UInt64>,System.Int32,FixedMathSharp.Fixed64,System.ReadOnlySpan`1<System.UInt64>,System.Int32,System.Span`1<System.UInt64>,System.Int32&)
TryGetNormalResponse(Gravitas.CollisionHandling.ExactContactResponseOperand3D&,Gravitas.CollisionHandling.ExactContactResponseOperand3D&,FixedMathSharp.Vector3d,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,Gravitas.CollisionHandling.ExactNormalResponse3D&)
TryGetAccumulatedNormalResponse(Gravitas.CollisionHandling.ExactContactResponseOperand3D&,Gravitas.CollisionHandling.ExactContactResponseOperand3D&,FixedMathSharp.Vector3d,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,Gravitas.CollisionHandling.ExactNormalResponse3D&,Gravitas.CollisionHandling.ContactResponseSnapshot&)
TryGetAccumulatedNormalResponse(Gravitas.CollisionHandling.ExactContactResponseOperand3D&,Gravitas.CollisionHandling.ExactContactResponseOperand3D&,FixedMathSharp.Vector3d,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,Gravitas.CollisionHandling.ExactNormalResponse3D&,Gravitas.CollisionHandling.ContactResponseSnapshot&,Gravitas.CollisionHandling.ExactContactImpulseState&)
TryGetNormalResponseCore(Gravitas.CollisionHandling.ExactContactResponseOperand3D&,Gravitas.CollisionHandling.ExactContactResponseOperand3D&,FixedMathSharp.Vector3d,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,System.Boolean,Gravitas.CollisionHandling.ContactResponseSnapshot&,Gravitas.CollisionHandling.ExactNormalResponse3D&,Gravitas.CollisionHandling.ExactContactImpulseState&,System.Boolean)
CreateZeroResponse(System.Boolean,System.Boolean,FixedMathSharp.Fixed64,System.Boolean,FixedMathSharp.Fixed64,Gravitas.CollisionHandling.ExactContactImpulseState)
GetEffectiveMassRatio(Gravitas.CollisionHandling.ExactContactResponseOperand3D&,Gravitas.CollisionHandling.ExactContactResponseOperand3D&,FixedMathSharp.Vector3d,System.Span`1<System.UInt64>,System.Span`1<System.UInt64>)
BuildImpactImpulseRatio(Gravitas.CollisionHandling.ExactContactResponseOperand3D&,Gravitas.CollisionHandling.ExactContactResponseOperand3D&,FixedMathSharp.Vector3d,Gravitas.CollisionHandling.ContactResponseSnapshot&,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,FixedMathSharp.Signed832,FixedMathSharp.Signed832,System.ReadOnlySpan`1<System.UInt64>,System.ReadOnlySpan`1<System.UInt64>,System.Span`1<System.UInt64>,System.Span`1<System.UInt64>)
BuildImpulseRatio(FixedMathSharp.Signed832,FixedMathSharp.Signed832,System.ReadOnlySpan`1<System.UInt64>,System.ReadOnlySpan`1<System.UInt64>,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,System.Span`1<System.UInt64>,System.Span`1<System.UInt64>)
ResolveUnilateralImpulse(System.ReadOnlySpan`1<System.UInt64>,System.ReadOnlySpan`1<System.UInt64>,System.Int32,FixedMathSharp.Fixed64,System.Boolean,System.Span`1<System.UInt64>,System.Span`1<System.UInt64>,System.Int32&,System.Boolean&,System.Boolean&,FixedMathSharp.Fixed64&,System.Span`1<System.UInt64>,Gravitas.CollisionHandling.ExactContactImpulseState)
TryGetLinearVelocityDelta(FixedMathSharp.Vector3d,FixedMathSharp.Fixed64,System.ReadOnlySpan`1<System.UInt64>,System.ReadOnlySpan`1<System.UInt64>,System.Int32,FixedMathSharp.Vector3d&)
TryGetLinearComponent(FixedMathSharp.Fixed64,System.ReadOnlySpan`1<System.UInt64>,System.ReadOnlySpan`1<System.UInt64>,System.Int32,FixedMathSharp.Fixed64&)
TryGetAngularVelocityDelta(Gravitas.CollisionHandling.ExactLever3D&,FixedMathSharp.Vector3d,FixedMathSharp.Fixed3x3,System.ReadOnlySpan`1<System.UInt64>,System.ReadOnlySpan`1<System.UInt64>,System.Int32,FixedMathSharp.Vector3d&)
TryGetAngularComponent(FixedMathSharp.Signed832,System.ReadOnlySpan`1<System.UInt64>,System.ReadOnlySpan`1<System.UInt64>,System.Int32,FixedMathSharp.Fixed64&)
GetRawDot(FixedMathSharp.Vector3d,FixedMathSharp.Vector3d)
SetNonNegativeRatio(FixedMathSharp.Signed832,FixedMathSharp.Signed832,System.Span`1<System.UInt64>,System.Span`1<System.UInt64>)
SetUnsignedSum(System.UInt64,System.UInt64,System.Span`1<System.UInt64>)
SetMagnitude(FixedMathSharp.Fixed64,System.Span`1<System.UInt64>)
SetMagnitude(FixedMathSharp.Signed192,System.Span`1<System.UInt64>)
SetMagnitude(FixedMathSharp.Signed320,System.Span`1<System.UInt64>)
SetMagnitude(FixedMathSharp.Signed704,System.Span`1<System.UInt64>)
SetMagnitude(FixedMathSharp.Signed832,System.Span`1<System.UInt64>)
Multiply3(System.ReadOnlySpan`1<System.UInt64>,System.ReadOnlySpan`1<System.UInt64>,System.ReadOnlySpan`1<System.UInt64>,System.Span`1<System.UInt64>)
Multiply4(System.ReadOnlySpan`1<System.UInt64>,System.ReadOnlySpan`1<System.UInt64>,System.ReadOnlySpan`1<System.UInt64>,System.ReadOnlySpan`1<System.UInt64>,System.Span`1<System.UInt64>)