< Summary

Information
Class: Gravitas.Constraints.JointSolver2D
Assembly: Gravitas
File(s): /home/runner/work/Gravitas/Gravitas/src/Gravitas/Constraints/2D/Joints/JointSolver2D.cs
Line coverage
100%
Covered lines: 357
Uncovered lines: 0
Coverable lines: 357
Total lines: 594
Line coverage: 100%
Branch coverage
100%
Covered branches: 97
Total branches: 97
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%2424100%
TryPrepareBiasVelocities(...)100%88100%
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/Joints/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 System;
 9using System.Runtime.CompilerServices;
 10using FixedMathSharp;
 11using Gravitas.CollisionHandling;
 12
 13namespace Gravitas.Constraints;
 14
 15/// <summary>
 16/// Builds and solves planar joint rows, applying cached impulses and recording solve metrics.
 17/// </summary>
 18internal static class JointSolver2D
 19{
 20    internal const int MaxRowsPerJoint = 8;
 21
 22    private const int CacheLinearX = 0;
 23    private const int CacheLinearY = 1;
 24    private const int CacheAngular = 2;
 25    private const int CacheLimit = 3;
 26    private const int CacheMotor = 4;
 27
 28    // Fraction of structural error targeted by the next fixed step.
 129    private static readonly Fixed64 BiasFactor = Fixed64.FromFraction(2, 5);
 130    private static readonly Fixed64 RowEpsilon = Fixed64.Epsilon;
 31
 32    internal static void Solve(Joint2D joint, bool applyCachedImpulse)
 33    {
 174234        Span<JointConstraintRow2D> rows = stackalloc JointConstraintRow2D[MaxRowsPerJoint];
 174235        int rowCount = BuildRows(
 174236            joint,
 174237            rows,
 174238            out Fixed64 linearAnchorErrorMagnitude,
 174239            out Fixed64 angularErrorMagnitude,
 174240            out Fixed64 limitErrorMagnitude,
 174241            out Fixed64 motorErrorMagnitude);
 174242        if (rowCount == 0 || !TryPrepareBiasVelocities(joint, rows[..rowCount]))
 43        {
 4444            joint.LastSolvedRowCount = 0;
 4445            joint.LastSolveMetrics = default;
 4446            return;
 47        }
 48
 169849        Fixed64 incrementalImpulseMagnitude = Fixed64.Zero;
 169850        Fixed64 motorImpulseMagnitude = Fixed64.Zero;
 169851        int clampedRowCount = 0;
 1052452        for (int i = 0; i < rowCount; i++)
 53        {
 356454            JointConstraintRow2D row = rows[i];
 356455            if (applyCachedImpulse)
 56            {
 44757                row.AccumulatedImpulse = joint.GetCachedImpulse(row.CacheIndex);
 44758                ApplyImpulse(joint.BodyA, joint.BodyB, row, row.AccumulatedImpulse);
 59            }
 60
 356461            Fixed64 impulse = SolveRow(joint.BodyA, joint.BodyB, row, out bool clamped);
 356462            row.AccumulatedImpulse += impulse;
 356463            rows[i] = row;
 356464            joint.SetCachedImpulse(row.CacheIndex, row.AccumulatedImpulse);
 356465            Fixed64 rowImpulseMagnitude = impulse.Abs();
 356466            incrementalImpulseMagnitude += rowImpulseMagnitude;
 356467            if (row.Kind == JointConstraintRowKind2D.Motor)
 8068                motorImpulseMagnitude += rowImpulseMagnitude;
 356469            if (clamped)
 9970                clampedRowCount++;
 71        }
 72
 169873        Fixed64 impulseMagnitude = Fixed64.Zero;
 1052474        for (int i = 0; i < rowCount; i++)
 356475            impulseMagnitude += rows[i].AccumulatedImpulse.Abs();
 76
 3056477        for (int i = 0; i < MaxRowsPerJoint; i++)
 78        {
 1358479            bool rowPrepared = false;
 7246880            for (int rowIndex = 0; rowIndex < rowCount; rowIndex++)
 81            {
 2621482                if (rows[rowIndex].CacheIndex == i)
 83                {
 356484                    rowPrepared = true;
 356485                    break;
 86                }
 87            }
 88
 1358489            if (!rowPrepared)
 1002090                joint.SetCachedImpulse(i, Fixed64.Zero);
 91        }
 92
 169893        joint.LastSolvedRowCount = rowCount;
 169894        joint.AccumulatedImpulseMagnitude += incrementalImpulseMagnitude;
 169895        joint.LastSolveMetrics = new JointSolveMetrics2D(
 169896            rowCount,
 169897            linearAnchorErrorMagnitude,
 169898            angularErrorMagnitude,
 169899            limitErrorMagnitude,
 1698100            impulseMagnitude,
 1698101            incrementalImpulseMagnitude,
 1698102            motorImpulseMagnitude,
 1698103            motorErrorMagnitude,
 1698104            clampedRowCount);
 1698105        if (incrementalImpulseMagnitude > Fixed64.Zero)
 608106            joint.Context.Diagnostics.EmitJointImpulse(joint, joint.LastSolveMetrics);
 1698107    }
 108
 109    private static bool TryPrepareBiasVelocities(Joint2D joint, Span<JointConstraintRow2D> rows)
 110    {
 10554111        foreach (ref JointConstraintRow2D row in rows)
 112        {
 113            // Structural error is corrected per fixed step. Motor gains already target velocity.
 3576114            if (row.Kind != JointConstraintRowKind2D.Motor && !Fixed64.TryMultiplyDivide(
 3576115                    row.BiasVelocity, joint.Context.InvDeltaTime, Fixed64.One, out row.BiasVelocity))
 116            {
 12117                if (ComputeDenominator(joint.BodyA, joint.BodyB, row) <= RowEpsilon)
 118                {
 6119                    row.BiasVelocity = Fixed64.Zero;
 6120                    continue;
 121                }
 6122                GravitasLogger.Channel.Write(DiagnosticLevel.Error,
 6123                    "A 2D joint's correction velocity cannot be represented.", nameof(JointSolver2D));
 6124                return false;
 125            }
 126        }
 1698127        return true;
 128    }
 129
 130    private static int BuildRows(
 131        Joint2D joint,
 132        Span<JointConstraintRow2D> rows,
 133        out Fixed64 linearAnchorErrorMagnitude,
 134        out Fixed64 angularErrorMagnitude,
 135        out Fixed64 limitErrorMagnitude,
 136        out Fixed64 motorErrorMagnitude)
 137    {
 1742138        int count = 0;
 1742139        limitErrorMagnitude = Fixed64.Zero;
 1742140        motorErrorMagnitude = Fixed64.Zero;
 141
 1742142        SolidBody2D bodyA = joint.BodyA;
 1742143        SolidBody2D bodyB = joint.BodyB;
 1742144        Fixed64 frameAngleA = bodyA.Rotation + joint.LocalFrameA.Angle;
 1742145        Fixed64 frameAngleB = bodyB.Rotation + joint.LocalFrameB.Angle;
 1742146        if (!Vector2d.TryRotate(
 1742147                joint.LocalFrameA.Anchor,
 1742148                bodyA.Rotation,
 1742149                out Vector2d anchorOffsetA)
 1742150            || !Vector2d.TryRotate(
 1742151                joint.LocalFrameB.Anchor,
 1742152                bodyB.Rotation,
 1742153                out Vector2d anchorOffsetB)
 1742154            || !bodyA.TryGetOffsetFromCenterOfMass(
 1742155                new ContactAnchor2D(bodyA.Position, anchorOffsetA),
 1742156                out Vector2d relativeAnchorA)
 1742157            || !bodyB.TryGetOffsetFromCenterOfMass(
 1742158                new ContactAnchor2D(bodyB.Position, anchorOffsetB),
 1742159                out Vector2d relativeAnchorB)
 1742160            || !Vector2d.TrySubtractSums(
 1742161                bodyB.Position,
 1742162                anchorOffsetB,
 1742163                bodyA.Position,
 1742164                anchorOffsetA,
 1742165                out Vector2d anchorError))
 166        {
 30167            linearAnchorErrorMagnitude = Fixed64.Zero;
 30168            angularErrorMagnitude = Fixed64.Zero;
 30169            GravitasLogger.Channel.Write(
 30170                DiagnosticLevel.Error,
 30171                "A 2D joint's anchors cannot be represented in response space.",
 30172                nameof(JointSolver2D));
 30173            return 0;
 174        }
 175
 1712176        linearAnchorErrorMagnitude = anchorError.Magnitude;
 1712177        angularErrorMagnitude = NormalizeAngle(frameAngleB - frameAngleA).Abs();
 178
 1712179        switch (joint.Type)
 180        {
 181            case JointType2D.Distance:
 272182                AddDistanceRow(joint, rows, ref count, anchorError, bodyB.Position - bodyA.Position, frameAngleA, relati
 272183                break;
 184            case JointType2D.Pin:
 1072185                AddPlanarAnchorRows(rows, ref count, anchorError, relativeAnchorA, relativeAnchorB);
 1072186                AddAngularLimitRow(joint, rows, ref count, frameAngleA, frameAngleB, ref limitErrorMagnitude);
 1072187                break;
 188            case JointType2D.Weld:
 192189                AddPlanarAnchorRows(rows, ref count, anchorError, relativeAnchorA, relativeAnchorB);
 192190                AddAngularErrorRow(rows, ref count, NormalizeAngle(frameAngleB - frameAngleA), Fixed64.Zero, Fixed64.Max
 192191                AddAngularLimitRow(joint, rows, ref count, frameAngleA, frameAngleB, ref limitErrorMagnitude);
 192192                break;
 193            case JointType2D.Prismatic:
 176194                AddPrismaticRows(joint, rows, ref count, anchorError, relativeAnchorA, relativeAnchorB, frameAngleA, fra
 195                break;
 196        }
 197
 1712198        AddMotorRow(joint, rows, ref count, anchorError, frameAngleA, frameAngleB, relativeAnchorA, relativeAnchorB, out
 1712199        return count;
 200    }
 201
 202    private static void AddDistanceRow(
 203        Joint2D joint,
 204        Span<JointConstraintRow2D> rows,
 205        ref int count,
 206        Vector2d anchorError,
 207        Vector2d bodyDelta,
 208        Fixed64 frameAngleA,
 209        Vector2d relativeAnchorA,
 210        Vector2d relativeAnchorB)
 211    {
 272212        Fixed64 magnitudeSquared = anchorError.MagnitudeSquared;
 272213        Fixed64 targetDistance = joint.Limits.TargetDistance;
 272214        if (magnitudeSquared <= RowEpsilon && targetDistance <= RowEpsilon)
 8215            return;
 216
 264217        Fixed64 distance = magnitudeSquared > RowEpsilon ? FixedMath.Sqrt(magnitudeSquared) : Fixed64.Zero;
 218        Vector2d axis;
 264219        if (distance > RowEpsilon)
 220        {
 248221            axis = anchorError / distance;
 222        }
 16223        else if (bodyDelta.MagnitudeSquared > RowEpsilon)
 224        {
 8225            axis = bodyDelta.Normalized;
 226        }
 227        else
 228        {
 8229            axis = Vector2d.Rotate(Vector2d.Right, frameAngleA).Normalized;
 230        }
 231
 264232        AddLinearRow(
 264233            rows,
 264234            ref count,
 264235            axis,
 264236            relativeAnchorA,
 264237            relativeAnchorB,
 264238            (distance - targetDistance) * BiasFactor,
 264239            Fixed64.Zero,
 264240            Fixed64.MinValue,
 264241            Fixed64.MaxValue,
 264242            CacheLinearX);
 264243    }
 244
 245    private static void AddPlanarAnchorRows(
 246        Span<JointConstraintRow2D> rows,
 247        ref int count,
 248        Vector2d anchorError,
 249        Vector2d relativeAnchorA,
 250        Vector2d relativeAnchorB)
 251    {
 1264252        AddLinearRow(
 1264253            rows,
 1264254            ref count,
 1264255            Vector2d.Right,
 1264256            relativeAnchorA,
 1264257            relativeAnchorB,
 1264258            Vector2d.Dot(anchorError, Vector2d.Right) * BiasFactor,
 1264259            Fixed64.Zero,
 1264260            Fixed64.MinValue,
 1264261            Fixed64.MaxValue,
 1264262            CacheLinearX);
 1264263        AddLinearRow(
 1264264            rows,
 1264265            ref count,
 1264266            Vector2d.Forward,
 1264267            relativeAnchorA,
 1264268            relativeAnchorB,
 1264269            Vector2d.Dot(anchorError, Vector2d.Forward) * BiasFactor,
 1264270            Fixed64.Zero,
 1264271            Fixed64.MinValue,
 1264272            Fixed64.MaxValue,
 1264273            CacheLinearY);
 1264274    }
 275
 276    private static void AddPrismaticRows(
 277        Joint2D joint,
 278        Span<JointConstraintRow2D> rows,
 279        ref int count,
 280        Vector2d anchorError,
 281        Vector2d relativeAnchorA,
 282        Vector2d relativeAnchorB,
 283        Fixed64 frameAngleA,
 284        Fixed64 frameAngleB,
 285        ref Fixed64 limitErrorMagnitude)
 286    {
 176287        Vector2d axis = Vector2d.Rotate(Vector2d.Right, frameAngleA).Normalized;
 176288        Vector2d normal = Perpendicular(axis);
 176289        AddLinearRow(
 176290            rows,
 176291            ref count,
 176292            normal,
 176293            relativeAnchorA,
 176294            relativeAnchorB,
 176295            Vector2d.Dot(anchorError, normal) * BiasFactor,
 176296            Fixed64.Zero,
 176297            Fixed64.MinValue,
 176298            Fixed64.MaxValue,
 176299            CacheLinearX);
 300
 176301        AddAngularErrorRow(rows, ref count, NormalizeAngle(frameAngleB - frameAngleA), Fixed64.Zero, Fixed64.MaxValue, C
 302
 176303        if (joint.Limits.Kind != JointLimitKind2D.Slider)
 48304            return;
 305
 128306        Fixed64 translation = Vector2d.Dot(anchorError, axis);
 128307        if (translation < joint.Limits.MinTranslation)
 308        {
 40309            Fixed64 error = translation - joint.Limits.MinTranslation;
 40310            limitErrorMagnitude += error.Abs();
 40311            AddLinearRow(
 40312                rows,
 40313                ref count,
 40314                axis,
 40315                relativeAnchorA,
 40316                relativeAnchorB,
 40317                error * BiasFactor,
 40318                Fixed64.Zero,
 40319                Fixed64.Zero,
 40320                Fixed64.MaxValue,
 40321                CacheLimit);
 40322            joint.Context.Diagnostics.EmitJointLimitReached(joint, error);
 323        }
 88324        else if (translation > joint.Limits.MaxTranslation)
 325        {
 24326            Fixed64 error = translation - joint.Limits.MaxTranslation;
 24327            limitErrorMagnitude += error.Abs();
 24328            AddLinearRow(
 24329                rows,
 24330                ref count,
 24331                axis,
 24332                relativeAnchorA,
 24333                relativeAnchorB,
 24334                error * BiasFactor,
 24335                Fixed64.Zero,
 24336                Fixed64.MinValue,
 24337                Fixed64.Zero,
 24338                CacheLimit);
 24339            joint.Context.Diagnostics.EmitJointLimitReached(joint, error);
 340        }
 88341    }
 342
 343    private static void AddAngularLimitRow(
 344        Joint2D joint,
 345        Span<JointConstraintRow2D> rows,
 346        ref int count,
 347        Fixed64 frameAngleA,
 348        Fixed64 frameAngleB,
 349        ref Fixed64 limitErrorMagnitude)
 350    {
 1264351        if (joint.Limits.Kind != JointLimitKind2D.Angular)
 1152352            return;
 353
 112354        Fixed64 angle = NormalizeAngle(frameAngleB - frameAngleA);
 112355        if (angle < joint.Limits.MinAngle)
 356        {
 32357            Fixed64 error = angle - joint.Limits.MinAngle;
 32358            limitErrorMagnitude += error.Abs();
 32359            AddAngularRow(
 32360                rows,
 32361                ref count,
 32362                error * BiasFactor,
 32363                Fixed64.Zero,
 32364                Fixed64.Zero,
 32365                Fixed64.MaxValue,
 32366                CacheLimit);
 32367            joint.Context.Diagnostics.EmitJointLimitReached(joint, error);
 368        }
 80369        else if (angle > joint.Limits.MaxAngle)
 370        {
 64371            Fixed64 error = angle - joint.Limits.MaxAngle;
 64372            limitErrorMagnitude += error.Abs();
 64373            AddAngularRow(
 64374                rows,
 64375                ref count,
 64376                error * BiasFactor,
 64377                Fixed64.Zero,
 64378                Fixed64.MinValue,
 64379                Fixed64.Zero,
 64380                CacheLimit);
 64381            joint.Context.Diagnostics.EmitJointLimitReached(joint, error);
 382        }
 80383    }
 384
 385    private static void AddMotorRow(
 386        Joint2D joint,
 387        Span<JointConstraintRow2D> rows,
 388        ref int count,
 389        Vector2d anchorError,
 390        Fixed64 frameAngleA,
 391        Fixed64 frameAngleB,
 392        Vector2d relativeAnchorA,
 393        Vector2d relativeAnchorB,
 394        out Fixed64 motorErrorMagnitude)
 395    {
 1712396        motorErrorMagnitude = Fixed64.Zero;
 1712397        JointMotor2D motor = joint.Motor;
 1712398        if (!motor.IsEnabled)
 1632399            return;
 400
 80401        if (motor.Kind == JointMotorKind2D.Angular)
 402        {
 32403            Fixed64 error = NormalizeAngle(frameAngleB - frameAngleA - motor.Target);
 32404            motorErrorMagnitude = error.Abs();
 32405            AddAngularRow(
 32406                rows,
 32407                ref count,
 32408                error * motor.DriveStrength,
 32409                motor.Damping,
 32410                -motor.MaximumMotorImpulse,
 32411                motor.MaximumMotorImpulse,
 32412                CacheMotor,
 32413                JointConstraintRowKind2D.Motor);
 32414            return;
 415        }
 416
 48417        Vector2d axis = Vector2d.Rotate(Vector2d.Right, frameAngleA).Normalized;
 48418        Fixed64 errorTranslation = Vector2d.Dot(anchorError, axis) - motor.Target;
 48419        motorErrorMagnitude = errorTranslation.Abs();
 48420        AddLinearRow(
 48421            rows,
 48422            ref count,
 48423            axis,
 48424            relativeAnchorA,
 48425            relativeAnchorB,
 48426            errorTranslation * motor.DriveStrength,
 48427            motor.Damping,
 48428            -motor.MaximumMotorImpulse,
 48429            motor.MaximumMotorImpulse,
 48430            CacheMotor,
 48431            JointConstraintRowKind2D.Motor);
 48432    }
 433
 434    private static void AddAngularErrorRow(
 435        Span<JointConstraintRow2D> rows,
 436        ref int count,
 437        Fixed64 error,
 438        Fixed64 damping,
 439        Fixed64 maxImpulse,
 440        int cacheIndex)
 441    {
 368442        AddAngularRow(
 368443            rows,
 368444            ref count,
 368445            error * BiasFactor,
 368446            damping,
 368447            -maxImpulse,
 368448            maxImpulse,
 368449            cacheIndex);
 368450    }
 451
 452    private static void AddLinearRow(
 453        Span<JointConstraintRow2D> rows,
 454        ref int count,
 455        Vector2d axis,
 456        Vector2d relativeAnchorA,
 457        Vector2d relativeAnchorB,
 458        Fixed64 biasVelocity,
 459        Fixed64 damping,
 460        Fixed64 lowerImpulse,
 461        Fixed64 upperImpulse,
 462        int cacheIndex,
 463        JointConstraintRowKind2D kind = JointConstraintRowKind2D.Linear)
 464    {
 3080465        Vector2d normalizedAxis = axis.MagnitudeSquared == Fixed64.One
 3080466            ? axis
 3080467            : axis.Normalized;
 3080468        rows[count] = new JointConstraintRow2D(
 3080469            kind,
 3080470            normalizedAxis,
 3080471            relativeAnchorA,
 3080472            relativeAnchorB,
 3080473            biasVelocity,
 3080474            damping,
 3080475            lowerImpulse,
 3080476            upperImpulse,
 3080477            cacheIndex);
 3080478        count++;
 3080479    }
 480
 481    private static void AddAngularRow(
 482        Span<JointConstraintRow2D> rows,
 483        ref int count,
 484        Fixed64 biasVelocity,
 485        Fixed64 damping,
 486        Fixed64 lowerImpulse,
 487        Fixed64 upperImpulse,
 488        int cacheIndex,
 489        JointConstraintRowKind2D kind = JointConstraintRowKind2D.Angular)
 490    {
 496491        rows[count] = new JointConstraintRow2D(
 496492            kind,
 496493            Vector2d.Zero,
 496494            Vector2d.Zero,
 496495            Vector2d.Zero,
 496496            biasVelocity,
 496497            damping,
 496498            lowerImpulse,
 496499            upperImpulse,
 496500            cacheIndex);
 496501        count++;
 496502    }
 503
 504    private static Fixed64 SolveRow(
 505        SolidBody2D bodyA,
 506        SolidBody2D bodyB,
 507        JointConstraintRow2D row,
 508        out bool clampedToBounds)
 509    {
 3564510        clampedToBounds = false;
 3564511        Fixed64 denominator = ComputeDenominator(bodyA, bodyB, row);
 3564512        if (denominator <= Fixed64.Epsilon)
 44513            return Fixed64.Zero;
 514
 3520515        Fixed64 velocity = ComputeRelativeVelocity(bodyA, bodyB, row);
 3520516        Fixed64 lambda = -(velocity + row.BiasVelocity + velocity * row.Damping) / denominator;
 3520517        Fixed64 unclamped = row.AccumulatedImpulse + lambda;
 3520518        Fixed64 clamped = FixedMath.Clamp(unclamped, row.LowerImpulse, row.UpperImpulse);
 3520519        clampedToBounds = clamped != unclamped;
 3520520        lambda = clamped - row.AccumulatedImpulse;
 3520521        if (lambda == Fixed64.Zero)
 2882522            return Fixed64.Zero;
 523
 638524        ApplyImpulse(bodyA, bodyB, row, lambda);
 638525        return lambda;
 526    }
 527
 528    private static Fixed64 ComputeRelativeVelocity(SolidBody2D bodyA, SolidBody2D bodyB, JointConstraintRow2D row)
 529    {
 3520530        if (row.Kind == JointConstraintRowKind2D.Linear || row.Axis != Vector2d.Zero)
 531        {
 3056532            Vector2d velocityA = bodyA.LinearVelocity + AngularVelocityAtPoint(bodyA.AngularVelocity, row.RelativeAnchor
 3056533            Vector2d velocityB = bodyB.LinearVelocity + AngularVelocityAtPoint(bodyB.AngularVelocity, row.RelativeAnchor
 3056534            return Vector2d.Dot(velocityB - velocityA, row.Axis);
 535        }
 536
 464537        return bodyB.AngularVelocity - bodyA.AngularVelocity;
 538    }
 539
 540    private static Fixed64 ComputeDenominator(SolidBody2D bodyA, SolidBody2D bodyB, JointConstraintRow2D row)
 541    {
 3576542        if (row.Kind != JointConstraintRowKind2D.Linear && row.Axis == Vector2d.Zero)
 496543            return bodyA.EffectiveInverseMomentOfInertia + bodyB.EffectiveInverseMomentOfInertia;
 544
 3080545        Fixed64 inverseMass = bodyA.GetConstrainedInverseMass(row.Axis) + bodyB.GetConstrainedInverseMass(row.Axis);
 3080546        Fixed64 torqueA = Cross(row.RelativeAnchorA, row.Axis);
 3080547        Fixed64 torqueB = Cross(row.RelativeAnchorB, row.Axis);
 3080548        return inverseMass
 3080549            + torqueA * torqueA * bodyA.EffectiveInverseMomentOfInertia
 3080550            + torqueB * torqueB * bodyB.EffectiveInverseMomentOfInertia;
 551    }
 552
 553    private static void ApplyImpulse(SolidBody2D bodyA, SolidBody2D bodyB, JointConstraintRow2D row, Fixed64 lambda)
 554    {
 1085555        if (lambda == Fixed64.Zero)
 396556            return;
 557
 689558        if (row.Kind != JointConstraintRowKind2D.Linear && row.Axis == Vector2d.Zero)
 559        {
 61560            bodyA.ApplyCollisionAngularVelocityDelta(-lambda * bodyA.EffectiveInverseMomentOfInertia);
 61561            bodyB.ApplyCollisionAngularVelocityDelta(lambda * bodyB.EffectiveInverseMomentOfInertia);
 61562            return;
 563        }
 564
 628565        Vector2d impulse = row.Axis * lambda;
 628566        bodyA.ApplyCollisionLinearVelocityDelta(-impulse * bodyA.GetConstrainedInverseMass(row.Axis));
 628567        bodyB.ApplyCollisionLinearVelocityDelta(impulse * bodyB.GetConstrainedInverseMass(row.Axis));
 568
 628569        Fixed64 angularA = Cross(row.RelativeAnchorA, -impulse) * bodyA.EffectiveInverseMomentOfInertia;
 628570        Fixed64 angularB = Cross(row.RelativeAnchorB, impulse) * bodyB.EffectiveInverseMomentOfInertia;
 628571        bodyA.ApplyCollisionAngularVelocityDelta(angularA);
 628572        bodyB.ApplyCollisionAngularVelocityDelta(angularB);
 628573    }
 574
 575    [MethodImpl(MethodImplOptions.AggressiveInlining)]
 576    private static Vector2d AngularVelocityAtPoint(Fixed64 angularVelocity, Vector2d relativeAnchor) =>
 6112577        new(-angularVelocity * relativeAnchor.Y, angularVelocity * relativeAnchor.X);
 578
 579    [MethodImpl(MethodImplOptions.AggressiveInlining)]
 176580    private static Vector2d Perpendicular(Vector2d vector) => new(-vector.Y, vector.X);
 581
 582    [MethodImpl(MethodImplOptions.AggressiveInlining)]
 7416583    private static Fixed64 Cross(Vector2d left, Vector2d right) => left.X * right.Y - left.Y * right.X;
 584
 585    private static Fixed64 NormalizeAngle(Fixed64 angle)
 586    {
 2224587        angle %= Fixed64.TwoPi;
 2224588        if (angle < -Fixed64.Pi)
 32589            angle += Fixed64.TwoPi;
 2192590        else if (angle > Fixed64.Pi)
 32591            angle -= Fixed64.TwoPi;
 2224592        return angle;
 593    }
 594}

Methods/Properties

.cctor()
Solve(Gravitas.Constraints.Joint2D,System.Boolean)
TryPrepareBiasVelocities(Gravitas.Constraints.Joint2D,System.Span`1<Gravitas.Constraints.JointConstraintRow2D>)
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)