< Summary

Information
Class: Gravitas.Constraints.JointSolver2D
Assembly: Gravitas
File(s): /home/runner/work/Gravitas/Gravitas/src/Gravitas/Constraints/2D/JointSolver2D.cs
Line coverage
100%
Covered lines: 347
Uncovered lines: 0
Coverable lines: 347
Total lines: 569
Line coverage: 100%
Branch coverage
100%
Covered branches: 87
Total branches: 87
Branch coverage: 100%
Method coverage

Feature is only available for sponsors

Upgrade to PRO version

Metrics

MethodBranch coverage Crap Score Cyclomatic complexity Line coverage
.cctor()100%11100%
Solve(...)100%2222100%
BuildRows(...)100%1515100%
AddDistanceRow(...)100%1010100%
AddPlanarAnchorRows(...)100%11100%
AddPrismaticRows(...)100%66100%
AddAngularLimitRow(...)100%66100%
AddMotorRow(...)100%44100%
AddAngularErrorRow(...)100%11100%
AddLinearRow(...)100%22100%
AddAngularRow(...)100%11100%
SolveRow(...)100%44100%
ComputeRelativeVelocity(...)100%44100%
ComputeDenominator(...)100%44100%
ApplyImpulse(...)100%66100%
AngularVelocityAtPoint(...)100%11100%
Perpendicular(...)100%11100%
Cross(...)100%11100%
NormalizeAngle(...)100%44100%

File(s)

/home/runner/work/Gravitas/Gravitas/src/Gravitas/Constraints/2D/JointSolver2D.cs

#LineLine coverage
 1//=======================================================================
 2// JointSolver2D.cs
 3//=======================================================================
 4// MIT License, Copyright (c) 2026-present David Oravsky (mrdav30)
 5// See LICENSE file in the project root for full license information.
 6//=======================================================================
 7
 8using FixedMathSharp;
 9using Gravitas.CollisionHandling;
 10using System;
 11using System.Runtime.CompilerServices;
 12
 13namespace Gravitas.Constraints;
 14
 15internal static class JointSolver2D
 16{
 17    internal const int MaxRowsPerJoint = 8;
 18
 19    private const int CacheLinearX = 0;
 20    private const int CacheLinearY = 1;
 21    private const int CacheAngular = 2;
 22    private const int CacheLimit = 3;
 23    private const int CacheMotor = 4;
 24
 125    private static readonly Fixed64 BiasFactor = Fixed64.One / (Fixed64)5;
 126    private static readonly Fixed64 RowEpsilon = Fixed64.Epsilon;
 27
 28    internal static void Solve(Joint2D joint, bool applyCachedImpulse)
 29    {
 154030        Span<JointConstraintRow2D> rows = stackalloc JointConstraintRow2D[MaxRowsPerJoint];
 154031        int rowCount = BuildRows(
 154032            joint,
 154033            rows,
 154034            out Fixed64 linearAnchorErrorMagnitude,
 154035            out Fixed64 angularErrorMagnitude,
 154036            out Fixed64 limitErrorMagnitude,
 154037            out Fixed64 motorErrorMagnitude);
 154038        if (rowCount == 0)
 39        {
 3840            joint.LastSolvedRowCount = 0;
 3841            joint.LastSolveMetrics = default;
 3842            return;
 43        }
 44
 150245        Fixed64 incrementalImpulseMagnitude = Fixed64.Zero;
 150246        Fixed64 motorImpulseMagnitude = Fixed64.Zero;
 150247        int clampedRowCount = 0;
 957248        for (int i = 0; i < rowCount; i++)
 49        {
 328450            JointConstraintRow2D row = rows[i];
 328451            if (applyCachedImpulse)
 52            {
 41153                row.AccumulatedImpulse = joint.GetCachedImpulse(row.CacheIndex);
 41154                ApplyImpulse(joint.BodyA, joint.BodyB, row, row.AccumulatedImpulse);
 55            }
 56
 328457            Fixed64 impulse = SolveRow(joint.BodyA, joint.BodyB, row, out bool clamped);
 328458            row.AccumulatedImpulse += impulse;
 328459            rows[i] = row;
 328460            joint.SetCachedImpulse(row.CacheIndex, row.AccumulatedImpulse);
 328461            Fixed64 rowImpulseMagnitude = impulse.Abs();
 328462            incrementalImpulseMagnitude += rowImpulseMagnitude;
 328463            if (row.Kind == JointConstraintRowKind2D.Motor)
 8064                motorImpulseMagnitude += rowImpulseMagnitude;
 328465            if (clamped)
 13966                clampedRowCount++;
 67        }
 68
 150269        Fixed64 impulseMagnitude = Fixed64.Zero;
 957270        for (int i = 0; i < rowCount; i++)
 328471            impulseMagnitude += rows[i].AccumulatedImpulse.Abs();
 72
 2703673        for (int i = 0; i < MaxRowsPerJoint; i++)
 74        {
 1201675            bool rowPrepared = false;
 6554876            for (int rowIndex = 0; rowIndex < rowCount; rowIndex++)
 77            {
 2404278                if (rows[rowIndex].CacheIndex == i)
 79                {
 328480                    rowPrepared = true;
 328481                    break;
 82                }
 83            }
 84
 1201685            if (!rowPrepared)
 873286                joint.SetCachedImpulse(i, Fixed64.Zero);
 87        }
 88
 150289        joint.LastSolvedRowCount = rowCount;
 150290        joint.AccumulatedImpulseMagnitude += incrementalImpulseMagnitude;
 150291        joint.LastSolveMetrics = new JointSolveMetrics2D(
 150292            rowCount,
 150293            linearAnchorErrorMagnitude,
 150294            angularErrorMagnitude,
 150295            limitErrorMagnitude,
 150296            impulseMagnitude,
 150297            incrementalImpulseMagnitude,
 150298            motorImpulseMagnitude,
 150299            motorErrorMagnitude,
 1502100            clampedRowCount);
 1502101        if (incrementalImpulseMagnitude > Fixed64.Zero)
 576102            joint.Context.Diagnostics.EmitJointImpulse(joint, joint.LastSolveMetrics);
 1502103    }
 104
 105    private static int BuildRows(
 106        Joint2D joint,
 107        Span<JointConstraintRow2D> rows,
 108        out Fixed64 linearAnchorErrorMagnitude,
 109        out Fixed64 angularErrorMagnitude,
 110        out Fixed64 limitErrorMagnitude,
 111        out Fixed64 motorErrorMagnitude)
 112    {
 1540113        int count = 0;
 1540114        limitErrorMagnitude = Fixed64.Zero;
 1540115        motorErrorMagnitude = Fixed64.Zero;
 116
 1540117        SolidBody2D bodyA = joint.BodyA;
 1540118        SolidBody2D bodyB = joint.BodyB;
 1540119        Fixed64 frameAngleA = bodyA.Rotation + joint.LocalFrameA.Angle;
 1540120        Fixed64 frameAngleB = bodyB.Rotation + joint.LocalFrameB.Angle;
 1540121        if (!Vector2d.TryRotate(
 1540122                joint.LocalFrameA.Anchor,
 1540123                bodyA.Rotation,
 1540124                out Vector2d anchorOffsetA)
 1540125            || !Vector2d.TryRotate(
 1540126                joint.LocalFrameB.Anchor,
 1540127                bodyB.Rotation,
 1540128                out Vector2d anchorOffsetB)
 1540129            || !bodyA.TryGetOffsetFromCenterOfMass(
 1540130                new ContactAnchor2D(bodyA.Position, anchorOffsetA),
 1540131                out Vector2d relativeAnchorA)
 1540132            || !bodyB.TryGetOffsetFromCenterOfMass(
 1540133                new ContactAnchor2D(bodyB.Position, anchorOffsetB),
 1540134                out Vector2d relativeAnchorB)
 1540135            || !Vector2d.TrySubtractSums(
 1540136                bodyB.Position,
 1540137                anchorOffsetB,
 1540138                bodyA.Position,
 1540139                anchorOffsetA,
 1540140                out Vector2d anchorError))
 141        {
 30142            linearAnchorErrorMagnitude = Fixed64.Zero;
 30143            angularErrorMagnitude = Fixed64.Zero;
 30144            GravitasLogger.Channel.Write(
 30145                DiagnosticLevel.Error,
 30146                "A 2D joint's anchors cannot be represented in response space.",
 30147                nameof(JointSolver2D));
 30148            return 0;
 149        }
 150
 1510151        linearAnchorErrorMagnitude = anchorError.Magnitude;
 1510152        angularErrorMagnitude = NormalizeAngle(frameAngleB - frameAngleA).Abs();
 153
 1510154        switch (joint.Type)
 155        {
 156            case JointType2D.Distance:
 176157                AddDistanceRow(joint, rows, ref count, anchorError, bodyB.Position - bodyA.Position, frameAngleA, relati
 176158                break;
 159            case JointType2D.Pin:
 966160                AddPlanarAnchorRows(rows, ref count, anchorError, relativeAnchorA, relativeAnchorB);
 966161                AddAngularLimitRow(joint, rows, ref count, frameAngleA, frameAngleB, ref limitErrorMagnitude);
 966162                break;
 163            case JointType2D.Weld:
 192164                AddPlanarAnchorRows(rows, ref count, anchorError, relativeAnchorA, relativeAnchorB);
 192165                AddAngularErrorRow(rows, ref count, NormalizeAngle(frameAngleB - frameAngleA), Fixed64.Zero, Fixed64.Max
 192166                AddAngularLimitRow(joint, rows, ref count, frameAngleA, frameAngleB, ref limitErrorMagnitude);
 192167                break;
 168            case JointType2D.Prismatic:
 176169                AddPrismaticRows(joint, rows, ref count, anchorError, relativeAnchorA, relativeAnchorB, frameAngleA, fra
 170                break;
 171        }
 172
 1510173        AddMotorRow(joint, rows, ref count, anchorError, frameAngleA, frameAngleB, relativeAnchorA, relativeAnchorB, out
 1510174        return count;
 175    }
 176
 177    private static void AddDistanceRow(
 178        Joint2D joint,
 179        Span<JointConstraintRow2D> rows,
 180        ref int count,
 181        Vector2d anchorError,
 182        Vector2d bodyDelta,
 183        Fixed64 frameAngleA,
 184        Vector2d relativeAnchorA,
 185        Vector2d relativeAnchorB)
 186    {
 176187        Fixed64 magnitudeSquared = anchorError.MagnitudeSquared;
 176188        Fixed64 targetDistance = joint.Limits.TargetDistance;
 176189        if (magnitudeSquared <= RowEpsilon && targetDistance <= RowEpsilon)
 8190            return;
 191
 168192        Fixed64 distance = magnitudeSquared > RowEpsilon ? FixedMath.Sqrt(magnitudeSquared) : Fixed64.Zero;
 193        Vector2d axis;
 168194        if (distance > RowEpsilon)
 195        {
 152196            axis = anchorError / distance;
 197        }
 16198        else if (bodyDelta.MagnitudeSquared > RowEpsilon)
 199        {
 8200            axis = bodyDelta.Normalized;
 201        }
 202        else
 203        {
 8204            axis = Vector2d.Rotate(Vector2d.Right, frameAngleA).Normalized;
 205        }
 206
 168207        AddLinearRow(
 168208            rows,
 168209            ref count,
 168210            axis,
 168211            relativeAnchorA,
 168212            relativeAnchorB,
 168213            (distance - targetDistance) * BiasFactor,
 168214            Fixed64.Zero,
 168215            Fixed64.MinValue,
 168216            Fixed64.MaxValue,
 168217            CacheLinearX);
 168218    }
 219
 220    private static void AddPlanarAnchorRows(
 221        Span<JointConstraintRow2D> rows,
 222        ref int count,
 223        Vector2d anchorError,
 224        Vector2d relativeAnchorA,
 225        Vector2d relativeAnchorB)
 226    {
 1158227        AddLinearRow(
 1158228            rows,
 1158229            ref count,
 1158230            Vector2d.Right,
 1158231            relativeAnchorA,
 1158232            relativeAnchorB,
 1158233            Vector2d.Dot(anchorError, Vector2d.Right) * BiasFactor,
 1158234            Fixed64.Zero,
 1158235            Fixed64.MinValue,
 1158236            Fixed64.MaxValue,
 1158237            CacheLinearX);
 1158238        AddLinearRow(
 1158239            rows,
 1158240            ref count,
 1158241            Vector2d.Forward,
 1158242            relativeAnchorA,
 1158243            relativeAnchorB,
 1158244            Vector2d.Dot(anchorError, Vector2d.Forward) * BiasFactor,
 1158245            Fixed64.Zero,
 1158246            Fixed64.MinValue,
 1158247            Fixed64.MaxValue,
 1158248            CacheLinearY);
 1158249    }
 250
 251    private static void AddPrismaticRows(
 252        Joint2D joint,
 253        Span<JointConstraintRow2D> rows,
 254        ref int count,
 255        Vector2d anchorError,
 256        Vector2d relativeAnchorA,
 257        Vector2d relativeAnchorB,
 258        Fixed64 frameAngleA,
 259        Fixed64 frameAngleB,
 260        ref Fixed64 limitErrorMagnitude)
 261    {
 176262        Vector2d axis = Vector2d.Rotate(Vector2d.Right, frameAngleA).Normalized;
 176263        Vector2d normal = Perpendicular(axis);
 176264        AddLinearRow(
 176265            rows,
 176266            ref count,
 176267            normal,
 176268            relativeAnchorA,
 176269            relativeAnchorB,
 176270            Vector2d.Dot(anchorError, normal) * BiasFactor,
 176271            Fixed64.Zero,
 176272            Fixed64.MinValue,
 176273            Fixed64.MaxValue,
 176274            CacheLinearX);
 275
 176276        AddAngularErrorRow(rows, ref count, NormalizeAngle(frameAngleB - frameAngleA), Fixed64.Zero, Fixed64.MaxValue, C
 277
 176278        if (joint.Limits.Kind != JointLimitKind2D.Slider)
 48279            return;
 280
 128281        Fixed64 translation = Vector2d.Dot(anchorError, axis);
 128282        if (translation < joint.Limits.MinTranslation)
 283        {
 16284            Fixed64 error = translation - joint.Limits.MinTranslation;
 16285            limitErrorMagnitude += error.Abs();
 16286            AddLinearRow(
 16287                rows,
 16288                ref count,
 16289                axis,
 16290                relativeAnchorA,
 16291                relativeAnchorB,
 16292                error * BiasFactor,
 16293                Fixed64.Zero,
 16294                Fixed64.Zero,
 16295                Fixed64.MaxValue,
 16296                CacheLimit);
 16297            joint.Context.Diagnostics.EmitJointLimitReached(joint, error);
 298        }
 112299        else if (translation > joint.Limits.MaxTranslation)
 300        {
 96301            Fixed64 error = translation - joint.Limits.MaxTranslation;
 96302            limitErrorMagnitude += error.Abs();
 96303            AddLinearRow(
 96304                rows,
 96305                ref count,
 96306                axis,
 96307                relativeAnchorA,
 96308                relativeAnchorB,
 96309                error * BiasFactor,
 96310                Fixed64.Zero,
 96311                Fixed64.MinValue,
 96312                Fixed64.Zero,
 96313                CacheLimit);
 96314            joint.Context.Diagnostics.EmitJointLimitReached(joint, error);
 315        }
 112316    }
 317
 318    private static void AddAngularLimitRow(
 319        Joint2D joint,
 320        Span<JointConstraintRow2D> rows,
 321        ref int count,
 322        Fixed64 frameAngleA,
 323        Fixed64 frameAngleB,
 324        ref Fixed64 limitErrorMagnitude)
 325    {
 1158326        if (joint.Limits.Kind != JointLimitKind2D.Angular)
 1078327            return;
 328
 80329        Fixed64 angle = NormalizeAngle(frameAngleB - frameAngleA);
 80330        if (angle < joint.Limits.MinAngle)
 331        {
 32332            Fixed64 error = angle - joint.Limits.MinAngle;
 32333            limitErrorMagnitude += error.Abs();
 32334            AddAngularRow(
 32335                rows,
 32336                ref count,
 32337                error * BiasFactor,
 32338                Fixed64.Zero,
 32339                Fixed64.Zero,
 32340                Fixed64.MaxValue,
 32341                CacheLimit);
 32342            joint.Context.Diagnostics.EmitJointLimitReached(joint, error);
 343        }
 48344        else if (angle > joint.Limits.MaxAngle)
 345        {
 32346            Fixed64 error = angle - joint.Limits.MaxAngle;
 32347            limitErrorMagnitude += error.Abs();
 32348            AddAngularRow(
 32349                rows,
 32350                ref count,
 32351                error * BiasFactor,
 32352                Fixed64.Zero,
 32353                Fixed64.MinValue,
 32354                Fixed64.Zero,
 32355                CacheLimit);
 32356            joint.Context.Diagnostics.EmitJointLimitReached(joint, error);
 357        }
 48358    }
 359
 360    private static void AddMotorRow(
 361        Joint2D joint,
 362        Span<JointConstraintRow2D> rows,
 363        ref int count,
 364        Vector2d anchorError,
 365        Fixed64 frameAngleA,
 366        Fixed64 frameAngleB,
 367        Vector2d relativeAnchorA,
 368        Vector2d relativeAnchorB,
 369        out Fixed64 motorErrorMagnitude)
 370    {
 1510371        motorErrorMagnitude = Fixed64.Zero;
 1510372        JointMotor2D motor = joint.Motor;
 1510373        if (!motor.IsEnabled)
 1430374            return;
 375
 80376        if (motor.Kind == JointMotorKind2D.Angular)
 377        {
 32378            Fixed64 error = NormalizeAngle(frameAngleB - frameAngleA - motor.Target);
 32379            motorErrorMagnitude = error.Abs();
 32380            AddAngularRow(
 32381                rows,
 32382                ref count,
 32383                error * motor.DriveStrength,
 32384                motor.Damping,
 32385                -motor.MaximumMotorImpulse,
 32386                motor.MaximumMotorImpulse,
 32387                CacheMotor,
 32388                JointConstraintRowKind2D.Motor);
 32389            return;
 390        }
 391
 48392        Vector2d axis = Vector2d.Rotate(Vector2d.Right, frameAngleA).Normalized;
 48393        Fixed64 errorTranslation = Vector2d.Dot(anchorError, axis) - motor.Target;
 48394        motorErrorMagnitude = errorTranslation.Abs();
 48395        AddLinearRow(
 48396            rows,
 48397            ref count,
 48398            axis,
 48399            relativeAnchorA,
 48400            relativeAnchorB,
 48401            errorTranslation * motor.DriveStrength,
 48402            motor.Damping,
 48403            -motor.MaximumMotorImpulse,
 48404            motor.MaximumMotorImpulse,
 48405            CacheMotor,
 48406            JointConstraintRowKind2D.Motor);
 48407    }
 408
 409    private static void AddAngularErrorRow(
 410        Span<JointConstraintRow2D> rows,
 411        ref int count,
 412        Fixed64 error,
 413        Fixed64 damping,
 414        Fixed64 maxImpulse,
 415        int cacheIndex)
 416    {
 368417        AddAngularRow(
 368418            rows,
 368419            ref count,
 368420            error * BiasFactor,
 368421            damping,
 368422            -maxImpulse,
 368423            maxImpulse,
 368424            cacheIndex);
 368425    }
 426
 427    private static void AddLinearRow(
 428        Span<JointConstraintRow2D> rows,
 429        ref int count,
 430        Vector2d axis,
 431        Vector2d relativeAnchorA,
 432        Vector2d relativeAnchorB,
 433        Fixed64 biasVelocity,
 434        Fixed64 damping,
 435        Fixed64 lowerImpulse,
 436        Fixed64 upperImpulse,
 437        int cacheIndex,
 438        JointConstraintRowKind2D kind = JointConstraintRowKind2D.Linear)
 439    {
 2820440        Vector2d normalizedAxis = axis.MagnitudeSquared == Fixed64.One
 2820441            ? axis
 2820442            : axis.Normalized;
 2820443        rows[count] = new JointConstraintRow2D(
 2820444            kind,
 2820445            normalizedAxis,
 2820446            relativeAnchorA,
 2820447            relativeAnchorB,
 2820448            biasVelocity,
 2820449            damping,
 2820450            lowerImpulse,
 2820451            upperImpulse,
 2820452            cacheIndex);
 2820453        count++;
 2820454    }
 455
 456    private static void AddAngularRow(
 457        Span<JointConstraintRow2D> rows,
 458        ref int count,
 459        Fixed64 biasVelocity,
 460        Fixed64 damping,
 461        Fixed64 lowerImpulse,
 462        Fixed64 upperImpulse,
 463        int cacheIndex,
 464        JointConstraintRowKind2D kind = JointConstraintRowKind2D.Angular)
 465    {
 464466        rows[count] = new JointConstraintRow2D(
 464467            kind,
 464468            Vector2d.Zero,
 464469            Vector2d.Zero,
 464470            Vector2d.Zero,
 464471            biasVelocity,
 464472            damping,
 464473            lowerImpulse,
 464474            upperImpulse,
 464475            cacheIndex);
 464476        count++;
 464477    }
 478
 479    private static Fixed64 SolveRow(
 480        SolidBody2D bodyA,
 481        SolidBody2D bodyB,
 482        JointConstraintRow2D row,
 483        out bool clampedToBounds)
 484    {
 3284485        clampedToBounds = false;
 3284486        Fixed64 denominator = ComputeDenominator(bodyA, bodyB, row);
 3284487        if (denominator <= Fixed64.Epsilon)
 32488            return Fixed64.Zero;
 489
 3252490        Fixed64 velocity = ComputeRelativeVelocity(bodyA, bodyB, row);
 3252491        Fixed64 lambda = -(velocity + row.BiasVelocity + velocity * row.Damping) / denominator;
 3252492        Fixed64 unclamped = row.AccumulatedImpulse + lambda;
 3252493        Fixed64 clamped = FixedMath.Clamp(unclamped, row.LowerImpulse, row.UpperImpulse);
 3252494        clampedToBounds = clamped != unclamped;
 3252495        lambda = clamped - row.AccumulatedImpulse;
 3252496        if (lambda == Fixed64.Zero)
 2647497            return Fixed64.Zero;
 498
 605499        ApplyImpulse(bodyA, bodyB, row, lambda);
 605500        return lambda;
 501    }
 502
 503    private static Fixed64 ComputeRelativeVelocity(SolidBody2D bodyA, SolidBody2D bodyB, JointConstraintRow2D row)
 504    {
 3252505        if (row.Kind == JointConstraintRowKind2D.Linear || row.Axis != Vector2d.Zero)
 506        {
 2820507            Vector2d velocityA = bodyA.LinearVelocity + AngularVelocityAtPoint(bodyA.AngularVelocity, row.RelativeAnchor
 2820508            Vector2d velocityB = bodyB.LinearVelocity + AngularVelocityAtPoint(bodyB.AngularVelocity, row.RelativeAnchor
 2820509            return Vector2d.Dot(velocityB - velocityA, row.Axis);
 510        }
 511
 432512        return bodyB.AngularVelocity - bodyA.AngularVelocity;
 513    }
 514
 515    private static Fixed64 ComputeDenominator(SolidBody2D bodyA, SolidBody2D bodyB, JointConstraintRow2D row)
 516    {
 3284517        if (row.Kind != JointConstraintRowKind2D.Linear && row.Axis == Vector2d.Zero)
 464518            return bodyA.EffectiveInverseMomentOfInertia + bodyB.EffectiveInverseMomentOfInertia;
 519
 2820520        Fixed64 inverseMass = bodyA.GetConstrainedInverseMass(row.Axis) + bodyB.GetConstrainedInverseMass(row.Axis);
 2820521        Fixed64 torqueA = Cross(row.RelativeAnchorA, row.Axis);
 2820522        Fixed64 torqueB = Cross(row.RelativeAnchorB, row.Axis);
 2820523        return inverseMass
 2820524            + torqueA * torqueA * bodyA.EffectiveInverseMomentOfInertia
 2820525            + torqueB * torqueB * bodyB.EffectiveInverseMomentOfInertia;
 526    }
 527
 528    private static void ApplyImpulse(SolidBody2D bodyA, SolidBody2D bodyB, JointConstraintRow2D row, Fixed64 lambda)
 529    {
 1016530        if (lambda == Fixed64.Zero)
 360531            return;
 532
 656533        if (row.Kind != JointConstraintRowKind2D.Linear && row.Axis == Vector2d.Zero)
 534        {
 59535            bodyA.ApplyCollisionAngularVelocityDelta(-lambda * bodyA.EffectiveInverseMomentOfInertia);
 59536            bodyB.ApplyCollisionAngularVelocityDelta(lambda * bodyB.EffectiveInverseMomentOfInertia);
 59537            return;
 538        }
 539
 597540        Vector2d impulse = row.Axis * lambda;
 597541        bodyA.ApplyCollisionLinearVelocityDelta(-impulse * bodyA.GetConstrainedInverseMass(row.Axis));
 597542        bodyB.ApplyCollisionLinearVelocityDelta(impulse * bodyB.GetConstrainedInverseMass(row.Axis));
 543
 597544        Fixed64 angularA = Cross(row.RelativeAnchorA, -impulse) * bodyA.EffectiveInverseMomentOfInertia;
 597545        Fixed64 angularB = Cross(row.RelativeAnchorB, impulse) * bodyB.EffectiveInverseMomentOfInertia;
 597546        bodyA.ApplyCollisionAngularVelocityDelta(angularA);
 597547        bodyB.ApplyCollisionAngularVelocityDelta(angularB);
 597548    }
 549
 550    [MethodImpl(MethodImplOptions.AggressiveInlining)]
 551    private static Vector2d AngularVelocityAtPoint(Fixed64 angularVelocity, Vector2d relativeAnchor) =>
 5640552        new(-angularVelocity * relativeAnchor.Y, angularVelocity * relativeAnchor.X);
 553
 554    [MethodImpl(MethodImplOptions.AggressiveInlining)]
 176555    private static Vector2d Perpendicular(Vector2d vector) => new(-vector.Y, vector.X);
 556
 557    [MethodImpl(MethodImplOptions.AggressiveInlining)]
 6834558    private static Fixed64 Cross(Vector2d left, Vector2d right) => left.X * right.Y - left.Y * right.X;
 559
 560    private static Fixed64 NormalizeAngle(Fixed64 angle)
 561    {
 1990562        angle %= Fixed64.TwoPi;
 1990563        if (angle < -Fixed64.Pi)
 32564            angle += Fixed64.TwoPi;
 1958565        else if (angle > Fixed64.Pi)
 32566            angle -= Fixed64.TwoPi;
 1990567        return angle;
 568    }
 569}
Generated by: ReportGenerator 5.5.11.0
08/05/2026 - 20:00:44
GitHub | reportgenerator.io

Methods/Properties

.cctor()
Solve(Gravitas.Constraints.Joint2D,System.Boolean)
BuildRows(Gravitas.Constraints.Joint2D,System.Span`1<Gravitas.Constraints.JointConstraintRow2D>,FixedMathSharp.Fixed64&,FixedMathSharp.Fixed64&,FixedMathSharp.Fixed64&,FixedMathSharp.Fixed64&)
AddDistanceRow(Gravitas.Constraints.Joint2D,System.Span`1<Gravitas.Constraints.JointConstraintRow2D>,System.Int32&,FixedMathSharp.Vector2d,FixedMathSharp.Vector2d,FixedMathSharp.Fixed64,FixedMathSharp.Vector2d,FixedMathSharp.Vector2d)
AddPlanarAnchorRows(System.Span`1<Gravitas.Constraints.JointConstraintRow2D>,System.Int32&,FixedMathSharp.Vector2d,FixedMathSharp.Vector2d,FixedMathSharp.Vector2d)
AddPrismaticRows(Gravitas.Constraints.Joint2D,System.Span`1<Gravitas.Constraints.JointConstraintRow2D>,System.Int32&,FixedMathSharp.Vector2d,FixedMathSharp.Vector2d,FixedMathSharp.Vector2d,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64&)
AddAngularLimitRow(Gravitas.Constraints.Joint2D,System.Span`1<Gravitas.Constraints.JointConstraintRow2D>,System.Int32&,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64&)
AddMotorRow(Gravitas.Constraints.Joint2D,System.Span`1<Gravitas.Constraints.JointConstraintRow2D>,System.Int32&,FixedMathSharp.Vector2d,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,FixedMathSharp.Vector2d,FixedMathSharp.Vector2d,FixedMathSharp.Fixed64&)
AddAngularErrorRow(System.Span`1<Gravitas.Constraints.JointConstraintRow2D>,System.Int32&,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,System.Int32)
AddLinearRow(System.Span`1<Gravitas.Constraints.JointConstraintRow2D>,System.Int32&,FixedMathSharp.Vector2d,FixedMathSharp.Vector2d,FixedMathSharp.Vector2d,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,System.Int32,Gravitas.Constraints.JointConstraintRowKind2D)
AddAngularRow(System.Span`1<Gravitas.Constraints.JointConstraintRow2D>,System.Int32&,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,System.Int32,Gravitas.Constraints.JointConstraintRowKind2D)
SolveRow(Gravitas.SolidBody2D,Gravitas.SolidBody2D,Gravitas.Constraints.JointConstraintRow2D,System.Boolean&)
ComputeRelativeVelocity(Gravitas.SolidBody2D,Gravitas.SolidBody2D,Gravitas.Constraints.JointConstraintRow2D)
ComputeDenominator(Gravitas.SolidBody2D,Gravitas.SolidBody2D,Gravitas.Constraints.JointConstraintRow2D)
ApplyImpulse(Gravitas.SolidBody2D,Gravitas.SolidBody2D,Gravitas.Constraints.JointConstraintRow2D,FixedMathSharp.Fixed64)
AngularVelocityAtPoint(FixedMathSharp.Fixed64,FixedMathSharp.Vector2d)
Perpendicular(FixedMathSharp.Vector2d)
Cross(FixedMathSharp.Vector2d,FixedMathSharp.Vector2d)
NormalizeAngle(FixedMathSharp.Fixed64)