< Summary

Information
Class: Gravitas.Constraints.JointSolver3D
Assembly: Gravitas
File(s): /home/runner/work/Gravitas/Gravitas/src/Gravitas/Constraints/3D/Joints/JointSolver3D.cs
Line coverage
100%
Covered lines: 303
Uncovered lines: 0
Coverable lines: 303
Total lines: 553
Line coverage: 100%
Branch coverage
100%
Covered branches: 120
Total branches: 120
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%1616100%
TryPrepareBiasVelocities(...)100%1010100%
TryGetLinearCorrectionDistance(...)100%1010100%
TryPrepareAnchors(...)100%88100%
BuildRows(...)100%22100%
AddLinearAnchorRow(...)100%11100%
AddAngularRows(...)100%44100%
AddMotorRows(...)100%22100%
AddAxisAlignmentRows(...)100%22100%
AddHingeLimitRow(...)100%66100%
AddConeTwistRows(...)100%1010100%
AddAngularErrorRows(...)100%11100%
AddMotorErrorRows(...)100%11100%
AddAngularRow(...)100%44100%
AddMotorRow(...)100%22100%
GetAngularError(...)100%11100%
GetSignedTwistAngle(...)100%44100%
NormalizeAngle(...)100%44100%
Perpendicular(...)100%22100%
SolveRow(...)100%44100%
ComputeRelativeVelocity(...)100%22100%
ComputeDenominator(...)100%22100%
ApplyImpulse(...)100%1616100%
TryPrepareImpulseBody(...)100%22100%
GetAllowedImpulseMagnitude(...)100%88100%

File(s)

/home/runner/work/Gravitas/Gravitas/src/Gravitas/Constraints/3D/Joints/JointSolver3D.cs

#LineLine coverage
 1//=======================================================================
 2// JointSolver3D.cs
 3//=======================================================================
 4// MIT License, Copyright (c) 2026-present David Oravsky (mrdav30)
 5// See LICENSE file in the project root for full license information.
 6//=======================================================================
 7
 8using System;
 9using FixedMathSharp;
 10using FixedMathSharp.Geometry;
 11using Gravitas.CollisionHandling;
 12
 13namespace Gravitas.Constraints;
 14
 15/// <summary>
 16/// Builds and solves spatial joint rows, applying cached impulses and recording solve metrics.
 17/// </summary>
 18internal static class JointSolver3D
 19{
 20    internal const int MaxRowsPerJoint = 12;
 21    // Fraction of structural error targeted by the next fixed step.
 122    private static readonly Fixed64 BiasFactor = Fixed64.FromFraction(2, 5);
 123    private static readonly Fixed64 RowEpsilon = Fixed64.Epsilon;
 24    // 4,096 raw Q32.32 units, approximately 9.536743e-7 radians.
 125    private static readonly Fixed64 QuaternionTwistVectorEpsilon = Fixed64.FromRaw(4_096);
 26
 27    internal static void Solve(Joint3D joint, bool applyCachedImpulse, Fixed64 linearCorrectionRate)
 28    {
 12539529        Span<JointConstraintRow3D> rows = stackalloc JointConstraintRow3D[MaxRowsPerJoint];
 12539530        int rowCount = BuildRows(
 12539531            joint,
 12539532            rows,
 12539533            out Fixed64 linearAnchorErrorMagnitude,
 12539534            out Fixed64 angularLimitErrorMagnitude,
 12539535            out Fixed64 motorErrorMagnitude);
 12539536        if (!TryPrepareBiasVelocities(joint, rows[..rowCount], linearCorrectionRate))
 37        {
 638            joint.LastSolvedRowCount = 0;
 639            joint.LastSolveMetrics = default;
 640            return;
 41        }
 42
 12538943        Fixed64 incrementalImpulseMagnitude = Fixed64.Zero;
 12538944        Fixed64 motorImpulseMagnitude = Fixed64.Zero;
 12538945        int clampedRowCount = 0;
 102672446        for (int i = 0; i < rowCount; i++)
 47        {
 38797348            JointConstraintRow3D row = rows[i];
 38797349            if (applyCachedImpulse)
 50            {
 3576051                row.AccumulatedImpulse = ApplyImpulse(joint.BodyA, joint.BodyB, row,
 3576052                    joint.GetCachedImpulse(row.CacheIndex));
 53            }
 54
 38797355            Fixed64 impulse = SolveRow(joint.BodyA, joint.BodyB, row, out bool clamped);
 38797356            row.AccumulatedImpulse += impulse;
 38797357            rows[i] = row;
 38797358            joint.SetCachedImpulse(row.CacheIndex, row.AccumulatedImpulse);
 38797359            Fixed64 rowImpulseMagnitude = impulse.Abs();
 38797360            incrementalImpulseMagnitude += rowImpulseMagnitude;
 38797361            if (row.Kind == JointConstraintRowKind3D.Motor)
 313662                motorImpulseMagnitude += rowImpulseMagnitude;
 38797363            if (clamped)
 123764                clampedRowCount++;
 65        }
 66
 12538967        Fixed64 impulseMagnitude = Fixed64.Zero;
 102672468        for (int i = 0; i < rowCount; i++)
 38797369            impulseMagnitude += rows[i].AccumulatedImpulse.Abs();
 70
 248416871        for (int i = rowCount; i < MaxRowsPerJoint; i++)
 111669572            joint.SetCachedImpulse(i, Fixed64.Zero);
 73
 12538974        joint.LastSolvedRowCount = rowCount;
 12538975        joint.AccumulatedImpulseMagnitude += incrementalImpulseMagnitude;
 12538976        joint.LastSolveMetrics = new JointSolveMetrics3D(
 12538977            rowCount,
 12538978            linearAnchorErrorMagnitude,
 12538979            angularLimitErrorMagnitude,
 12538980            impulseMagnitude,
 12538981            incrementalImpulseMagnitude,
 12538982            motorImpulseMagnitude,
 12538983            motorErrorMagnitude,
 12538984            clampedRowCount);
 12538985        if (incrementalImpulseMagnitude > Fixed64.Zero)
 7426586            joint.Context.Diagnostics.EmitJointImpulse(joint, joint.LastSolveMetrics);
 12538987    }
 88
 89    private static bool TryPrepareBiasVelocities(Joint3D joint, Span<JointConstraintRow3D> rows,
 90        Fixed64 linearCorrectionRate)
 91    {
 102675492        foreach (ref JointConstraintRow3D row in rows)
 93        {
 94            // Structural error is corrected per fixed step. Motor gains already target velocity.
 38798595            if (row.Kind != JointConstraintRowKind3D.Motor && !Fixed64.TryMultiplyDivide(
 38798596                    row.BiasVelocity,
 38798597                    row.Kind == JointConstraintRowKind3D.Linear ? linearCorrectionRate : joint.Context.InvDeltaTime,
 38798598                    Fixed64.One, out row.BiasVelocity))
 99            {
 12100                if (ComputeDenominator(joint.BodyA, joint.BodyB, row) <= RowEpsilon)
 101                {
 6102                    row.BiasVelocity = Fixed64.Zero;
 6103                    continue;
 104                }
 6105                GravitasLogger.Channel.Write(DiagnosticLevel.Error,
 6106                    "A 3D joint's correction velocity cannot be represented.", nameof(JointSolver3D));
 6107                return false;
 108            }
 109        }
 125389110        return true;
 111    }
 112
 113    internal static bool TryGetLinearCorrectionDistance(Joint3D joint, out Fixed64 distance)
 114    {
 11498115        distance = Fixed64.Zero;
 11498116        if (!TryPrepareAnchors(joint, out Vector3d relativeA, out Vector3d relativeB, out Vector3d error))
 6117            return false;
 11492118        var row = new JointConstraintRow3D
 11492119        {
 11492120            Kind = JointConstraintRowKind3D.Linear,
 11492121            RelativeAnchorA = relativeA,
 11492122            RelativeAnchorB = relativeB
 11492123        };
 91936124        for (int axis = 0; axis < 3; axis++)
 125        {
 34476126            row.Axis = axis == 0 ? Vector3d.Right : axis == 1 ? Vector3d.Up : Vector3d.Forward;
 127            // Frozen, unresponsive errors cannot consume the connected island's repair budget.
 34476128            if (ComputeDenominator(joint.BodyA, joint.BodyB, row) <= RowEpsilon)
 259129                error[axis] = Fixed64.Zero;
 130        }
 11492131        return Vector3d.TryGetMagnitude(error, out distance);
 132    }
 133
 134    private static bool TryPrepareAnchors(Joint3D joint, out Vector3d relativeAnchorA,
 135        out Vector3d relativeAnchorB, out Vector3d anchorError)
 136    {
 136893137        relativeAnchorA = relativeAnchorB = anchorError = default;
 273786138        SolidBody bodyA = joint.BodyA, bodyB = joint.BodyB;
 136893139        return bodyA.Rotation.TryRotate(joint.LocalFrameA.Position, out Vector3d anchorOffsetA)
 136893140            && bodyB.Rotation.TryRotate(joint.LocalFrameB.Position, out Vector3d anchorOffsetB)
 136893141            && bodyA.TryGetOffsetFromCenterOfMass(new ContactAnchor(bodyA.Position3d, anchorOffsetA), out relativeAnchor
 136893142            && bodyB.TryGetOffsetFromCenterOfMass(new ContactAnchor(bodyB.Position3d, anchorOffsetB), out relativeAnchor
 136893143            && Vector3d.TrySubtractSums(bodyB.Position3d, anchorOffsetB, bodyA.Position3d, anchorOffsetA, out anchorErro
 144    }
 145
 146    private static int BuildRows(
 147        Joint3D joint,
 148        Span<JointConstraintRow3D> rows,
 149        out Fixed64 linearAnchorErrorMagnitude,
 150        out Fixed64 angularLimitErrorMagnitude,
 151        out Fixed64 motorErrorMagnitude)
 152    {
 125395153        int count = 0;
 125395154        angularLimitErrorMagnitude = Fixed64.Zero;
 125395155        motorErrorMagnitude = Fixed64.Zero;
 125395156        SolidBody bodyA = joint.BodyA;
 125395157        SolidBody bodyB = joint.BodyB;
 158
 125395159        FixedQuaternion worldRotationA = (bodyA.Rotation * joint.LocalFrameA.Rotation).Normalized;
 125395160        FixedQuaternion worldRotationB = (bodyB.Rotation * joint.LocalFrameB.Rotation).Normalized;
 125395161        if (!TryPrepareAnchors(joint, out Vector3d relativeAnchorA,
 125395162                out Vector3d relativeAnchorB, out Vector3d anchorError))
 163        {
 36164            linearAnchorErrorMagnitude = Fixed64.Zero;
 36165            GravitasLogger.Channel.Write(
 36166                DiagnosticLevel.Error,
 36167                "A 3D joint's anchors cannot be represented in response space.",
 36168                nameof(JointSolver3D));
 36169            return 0;
 170        }
 171
 125359172        linearAnchorErrorMagnitude = anchorError.Magnitude;
 173
 125359174        AddLinearAnchorRow(rows, ref count, Vector3d.Right, anchorError, relativeAnchorA, relativeAnchorB);
 125359175        AddLinearAnchorRow(rows, ref count, Vector3d.Up, anchorError, relativeAnchorA, relativeAnchorB);
 125359176        AddLinearAnchorRow(rows, ref count, Vector3d.Forward, anchorError, relativeAnchorA, relativeAnchorB);
 177
 125359178        AddAngularRows(joint, rows, ref count, worldRotationA, worldRotationB, ref angularLimitErrorMagnitude);
 125359179        AddMotorRows(joint, rows, ref count, worldRotationA, worldRotationB, out motorErrorMagnitude);
 125359180        return count;
 181    }
 182
 183    private static void AddLinearAnchorRow(
 184        Span<JointConstraintRow3D> rows,
 185        ref int count,
 186        Vector3d axis,
 187        Vector3d anchorError,
 188        Vector3d relativeAnchorA,
 189        Vector3d relativeAnchorB)
 190    {
 376077191        Fixed64 error = Vector3d.Dot(anchorError, axis);
 376077192        rows[count] = new JointConstraintRow3D(
 376077193            JointConstraintRowKind3D.Linear,
 376077194            axis,
 376077195            relativeAnchorA,
 376077196            relativeAnchorB,
 376077197            error * BiasFactor,
 376077198            Fixed64.Zero,
 376077199            Fixed64.MinValue,
 376077200            Fixed64.MaxValue,
 376077201            count);
 376077202        count++;
 376077203    }
 204
 205    private static void AddAngularRows(
 206        Joint3D joint,
 207        Span<JointConstraintRow3D> rows,
 208        ref int count,
 209        FixedQuaternion worldRotationA,
 210        FixedQuaternion worldRotationB,
 211        ref Fixed64 angularLimitErrorMagnitude)
 212    {
 125359213        Vector3d error = GetAngularError(worldRotationA, worldRotationB);
 125359214        switch (joint.Type)
 215        {
 216            case JointType3D.Fixed:
 168217                AddAngularErrorRows(rows, ref count, error, Fixed64.Zero, Fixed64.MaxValue);
 168218                break;
 219            case JointType3D.Hinge:
 8856220                AddAxisAlignmentRows(rows, ref count, worldRotationA * Vector3d.Right, worldRotationB * Vector3d.Right);
 8856221                AddHingeLimitRow(joint, rows, ref count, error, worldRotationA * Vector3d.Right, ref angularLimitErrorMa
 8856222                break;
 223            case JointType3D.ConeTwist:
 72224                AddConeTwistRows(joint, rows, ref count, worldRotationA, worldRotationB, ref angularLimitErrorMagnitude)
 225                break;
 226        }
 72227    }
 228
 229    private static void AddMotorRows(
 230        Joint3D joint,
 231        Span<JointConstraintRow3D> rows,
 232        ref int count,
 233        FixedQuaternion worldRotationA,
 234        FixedQuaternion worldRotationB,
 235        out Fixed64 motorErrorMagnitude)
 236    {
 125359237        motorErrorMagnitude = Fixed64.Zero;
 125359238        JointMotor3D motor = joint.Motor;
 125359239        if (!motor.IsEnabled)
 121839240            return;
 241
 3520242        FixedQuaternion targetWorldB = (worldRotationA * motor.TargetLocalRotation).Normalized;
 3520243        Vector3d motorError = GetAngularError(targetWorldB, worldRotationB);
 3520244        motorErrorMagnitude = motorError.Magnitude;
 3520245        AddMotorErrorRows(rows, ref count, motorError, motor.AngularDriveStrength, motor.AngularDriveDamping, motor.Maxi
 3520246    }
 247
 248    private static void AddAxisAlignmentRows(
 249        Span<JointConstraintRow3D> rows,
 250        ref int count,
 251        Vector3d axisA,
 252        Vector3d axisB)
 253    {
 8856254        Vector3d cross = Vector3d.Cross(axisA.Normalized, axisB.Normalized);
 8856255        Fixed64 magnitude = cross.Magnitude;
 8856256        if (magnitude <= RowEpsilon)
 358257            return;
 258
 8498259        AddAngularRow(rows, ref count, cross / magnitude, magnitude, Fixed64.Zero, Fixed64.MaxValue);
 8498260    }
 261
 262    private static void AddHingeLimitRow(
 263        Joint3D joint,
 264        Span<JointConstraintRow3D> rows,
 265        ref int count,
 266        Vector3d angularError,
 267        Vector3d hingeAxis,
 268        ref Fixed64 angularLimitErrorMagnitude)
 269    {
 8856270        if (joint.Limits.Kind != JointLimitKind3D.Hinge)
 8271            return;
 272
 8848273        Vector3d axis = hingeAxis.Normalized;
 8848274        Fixed64 twist = Vector3d.Dot(angularError, axis);
 8848275        Fixed64 max = joint.Limits.MaxHingeAngle;
 8848276        if (twist.Abs() <= max)
 8784277            return;
 278
 64279        Fixed64 limitedError = twist > Fixed64.Zero ? twist - max : twist + max;
 64280        angularLimitErrorMagnitude += limitedError.Abs();
 64281        AddAngularRow(rows, ref count, axis, limitedError, Fixed64.Zero, Fixed64.MaxValue);
 64282        joint.Context.Diagnostics.EmitJointLimitReached(joint, limitedError);
 64283    }
 284
 285    private static void AddConeTwistRows(
 286        Joint3D joint,
 287        Span<JointConstraintRow3D> rows,
 288        ref int count,
 289        FixedQuaternion worldRotationA,
 290        FixedQuaternion worldRotationB,
 291        ref Fixed64 angularLimitErrorMagnitude)
 292    {
 72293        Vector3d forwardA = (worldRotationA * Vector3d.Forward).Normalized;
 72294        Vector3d forwardB = (worldRotationB * Vector3d.Forward).Normalized;
 72295        if (joint.Limits.Kind != JointLimitKind3D.ConeTwist)
 8296            return;
 297
 64298        Fixed64 dot = FixedMath.Clamp(Vector3d.Dot(forwardA, forwardB), -Fixed64.One, Fixed64.One);
 64299        Fixed64 swing = FixedMath.Acos(dot);
 64300        if (swing > joint.Limits.MaxConeAngle)
 301        {
 18302            Vector3d swingAxis = Vector3d.Cross(forwardA, forwardB);
 18303            if (swingAxis.MagnitudeSquared <= RowEpsilon)
 2304                swingAxis = Perpendicular(forwardA);
 305
 18306            Fixed64 limitedError = swing - joint.Limits.MaxConeAngle;
 18307            angularLimitErrorMagnitude += limitedError.Abs();
 18308            AddAngularRow(rows, ref count, swingAxis.Normalized, limitedError, Fixed64.Zero, Fixed64.MaxValue);
 18309            joint.Context.Diagnostics.EmitJointLimitReached(joint, limitedError);
 310        }
 311
 64312        Fixed64 twist = GetSignedTwistAngle(worldRotationA, worldRotationB, forwardA);
 64313        Fixed64 maxTwist = joint.Limits.MaxTwistAngle;
 64314        if (twist.Abs() > maxTwist)
 315        {
 38316            Fixed64 limitedError = twist > Fixed64.Zero ? twist - maxTwist : twist + maxTwist;
 38317            angularLimitErrorMagnitude += limitedError.Abs();
 38318            AddAngularRow(rows, ref count, forwardA, limitedError, Fixed64.Zero, Fixed64.MaxValue);
 38319            joint.Context.Diagnostics.EmitJointLimitReached(joint, limitedError);
 320        }
 64321    }
 322
 323    private static void AddAngularErrorRows(
 324        Span<JointConstraintRow3D> rows,
 325        ref int count,
 326        Vector3d error,
 327        Fixed64 damping,
 328        Fixed64 maxImpulse)
 329    {
 168330        AddAngularRow(rows, ref count, Vector3d.Right, Vector3d.Dot(error, Vector3d.Right), damping, maxImpulse);
 168331        AddAngularRow(rows, ref count, Vector3d.Up, Vector3d.Dot(error, Vector3d.Up), damping, maxImpulse);
 168332        AddAngularRow(rows, ref count, Vector3d.Forward, Vector3d.Dot(error, Vector3d.Forward), damping, maxImpulse);
 168333    }
 334
 335    private static void AddMotorErrorRows(
 336        Span<JointConstraintRow3D> rows,
 337        ref int count,
 338        Vector3d error,
 339        Fixed64 strength,
 340        Fixed64 damping,
 341        Fixed64 maxImpulse)
 342    {
 3520343        AddMotorRow(rows, ref count, Vector3d.Right, Vector3d.Dot(error, Vector3d.Right) * strength, damping, maxImpulse
 3520344        AddMotorRow(rows, ref count, Vector3d.Up, Vector3d.Dot(error, Vector3d.Up) * strength, damping, maxImpulse);
 3520345        AddMotorRow(rows, ref count, Vector3d.Forward, Vector3d.Dot(error, Vector3d.Forward) * strength, damping, maxImp
 3520346    }
 347
 348    private static void AddAngularRow(
 349        Span<JointConstraintRow3D> rows,
 350        ref int count,
 351        Vector3d axis,
 352        Fixed64 error,
 353        Fixed64 damping,
 354        Fixed64 maxImpulse)
 355    {
 9122356        if (error.Abs() <= RowEpsilon || axis.MagnitudeSquared <= RowEpsilon)
 344357            return;
 358
 8778359        rows[count] = new JointConstraintRow3D(
 8778360            JointConstraintRowKind3D.Angular,
 8778361            axis.Normalized,
 8778362            Vector3d.Zero,
 8778363            Vector3d.Zero,
 8778364            error * BiasFactor,
 8778365            damping,
 8778366            -maxImpulse,
 8778367            maxImpulse,
 8778368            count);
 8778369        count++;
 8778370    }
 371
 372    private static void AddMotorRow(
 373        Span<JointConstraintRow3D> rows,
 374        ref int count,
 375        Vector3d axis,
 376        Fixed64 biasVelocity,
 377        Fixed64 damping,
 378        Fixed64 maxImpulse)
 379    {
 10560380        if (biasVelocity.Abs() <= RowEpsilon)
 7424381            return;
 382
 3136383        rows[count] = new JointConstraintRow3D(
 3136384            JointConstraintRowKind3D.Motor,
 3136385            axis,
 3136386            Vector3d.Zero,
 3136387            Vector3d.Zero,
 3136388            biasVelocity,
 3136389            damping,
 3136390            -maxImpulse,
 3136391            maxImpulse,
 3136392            count);
 3136393        count++;
 3136394    }
 395
 396    private static Vector3d GetAngularError(FixedQuaternion reference, FixedQuaternion current)
 397    {
 128879398        FixedQuaternion error = (current * reference.Inverse()).Normalized;
 128879399        return FixedQuaternion.QuaternionLog(error);
 400    }
 401
 402    private static Fixed64 GetSignedTwistAngle(
 403        FixedQuaternion worldRotationA,
 404        FixedQuaternion worldRotationB,
 405        Vector3d twistAxis)
 406    {
 64407        FixedQuaternion relative = (worldRotationB * worldRotationA.Inverse()).Normalized;
 64408        Vector3d relativeVector = new(relative.X, relative.Y, relative.Z);
 64409        Fixed64 projectedTwist = Vector3d.Dot(relativeVector, twistAxis);
 64410        if (projectedTwist.Abs() < QuaternionTwistVectorEpsilon
 64411            && relative.W.Abs() < QuaternionTwistVectorEpsilon)
 412        {
 2413            return Fixed64.Zero;
 414        }
 415
 62416        return NormalizeAngle(Fixed64.Two * FixedMath.Atan2(projectedTwist, relative.W));
 417    }
 418
 419    private static Fixed64 NormalizeAngle(Fixed64 angle)
 420    {
 62421        angle %= Fixed64.TwoPi;
 62422        if (angle < -Fixed64.Pi)
 1423            angle += Fixed64.TwoPi;
 61424        else if (angle >= Fixed64.Pi)
 2425            angle -= Fixed64.TwoPi;
 62426        return angle;
 427    }
 428
 429    private static Vector3d Perpendicular(Vector3d axis)
 430    {
 2431        Vector3d candidate = Vector3d.Cross(axis, Vector3d.Up);
 2432        return candidate.MagnitudeSquared > RowEpsilon
 2433            ? candidate
 2434            : Vector3d.Cross(axis, Vector3d.Right);
 435    }
 436
 437    private static Fixed64 SolveRow(
 438        SolidBody bodyA,
 439        SolidBody bodyB,
 440        JointConstraintRow3D row,
 441        out bool clampedToBounds)
 442    {
 387973443        clampedToBounds = false;
 387973444        Fixed64 denominator = ComputeDenominator(bodyA, bodyB, row);
 387973445        if (denominator <= Fixed64.Epsilon)
 1585446            return Fixed64.Zero;
 447
 386388448        Fixed64 velocity = ComputeRelativeVelocity(bodyA, bodyB, row);
 386388449        Fixed64 lambda = -(velocity + row.BiasVelocity + velocity * row.Damping) / denominator;
 386388450        Fixed64 unclamped = row.AccumulatedImpulse + lambda;
 386388451        Fixed64 clamped = FixedMath.Clamp(unclamped, row.LowerImpulse, row.UpperImpulse);
 386388452        clampedToBounds = clamped != unclamped;
 386388453        lambda = clamped - row.AccumulatedImpulse;
 386388454        if (lambda == Fixed64.Zero)
 216688455            return Fixed64.Zero;
 456
 169700457        return ApplyImpulse(bodyA, bodyB, row, lambda);
 458    }
 459
 460    private static Fixed64 ComputeRelativeVelocity(SolidBody bodyA, SolidBody bodyB, JointConstraintRow3D row)
 461    {
 386388462        if (row.Kind == JointConstraintRowKind3D.Linear)
 463        {
 374506464            Vector3d velocityA = bodyA.LinearVelocity + Vector3d.Cross(bodyA.AngularVelocity, row.RelativeAnchorA);
 374506465            Vector3d velocityB = bodyB.LinearVelocity + Vector3d.Cross(bodyB.AngularVelocity, row.RelativeAnchorB);
 374506466            return Vector3d.Dot(velocityB - velocityA, row.Axis);
 467        }
 468
 11882469        return Vector3d.Dot(bodyB.AngularVelocity - bodyA.AngularVelocity, row.Axis);
 470    }
 471
 472    private static Fixed64 ComputeDenominator(SolidBody bodyA, SolidBody bodyB, JointConstraintRow3D row)
 473    {
 422461474        if (row.Kind != JointConstraintRowKind3D.Linear)
 475        {
 11914476            Vector3d angularA = bodyA.ApplyConstrainedInverseInertia(row.Axis);
 11914477            Vector3d angularB = bodyB.ApplyConstrainedInverseInertia(row.Axis);
 11914478            return Vector3d.Dot(row.Axis, angularA + angularB);
 479        }
 480
 410547481        Fixed64 inverseMass = bodyA.GetConstrainedInverseMass(row.Axis) + bodyB.GetConstrainedInverseMass(row.Axis);
 410547482        Vector3d torqueA = Vector3d.Cross(row.RelativeAnchorA, row.Axis);
 410547483        Vector3d torqueB = Vector3d.Cross(row.RelativeAnchorB, row.Axis);
 410547484        Vector3d linearAngularA = bodyA.ApplyConstrainedInverseInertia(torqueA);
 410547485        Vector3d linearAngularB = bodyB.ApplyConstrainedInverseInertia(torqueB);
 410547486        return inverseMass
 410547487            + Vector3d.Dot(torqueA, linearAngularA)
 410547488            + Vector3d.Dot(torqueB, linearAngularB);
 489    }
 490
 491    private static Fixed64 ApplyImpulse(SolidBody bodyA, SolidBody bodyB, JointConstraintRow3D row, Fixed64 lambda)
 492    {
 205460493        if (lambda == Fixed64.Zero)
 20119494            return Fixed64.Zero;
 185341495        bool negative = lambda < Fixed64.Zero;
 185341496        if (!Fixed64.TryMultiplyDivide(lambda, negative ? -Fixed64.One : Fixed64.One, Fixed64.One, out Fixed64 magnitude
 1497            return Fixed64.Zero;
 498
 499        // Limit the shared impulse, then prepare again from that exact scalar before publishing either body.
 371526500        for (int attempt = 0; attempt < 2; attempt++)
 501        {
 185748502            bool prepared = ContactResponseArithmetic3D.TryScale(row.Axis, lambda, out Vector3d impulse)
 185748503                & Vector3d.TrySubtract(Vector3d.Zero, impulse, out Vector3d opposite);
 185748504            prepared &= TryPrepareImpulseBody(bodyA, row, row.RelativeAnchorA, opposite, out Vector3d linearA, out Vecto
 185748505                & TryPrepareImpulseBody(bodyB, row, row.RelativeAnchorB, impulse, out Vector3d linearB, out Vector3d ang
 185748506            if (!prepared)
 1507                return Fixed64.Zero;
 508
 185747509            Fixed64 limit = bodyA.Context.Environment.MaxSpeed;
 185747510            Fixed64 allowed = FixedMath.Min(
 185747511                FixedMath.Min(GetAllowedImpulseMagnitude(bodyA.LinearVelocity, linearA, limit, magnitude),
 185747512                    GetAllowedImpulseMagnitude(bodyB.LinearVelocity, linearB, limit, magnitude)),
 185747513                FixedMath.Min(GetAllowedImpulseMagnitude(bodyA.AngularVelocity, angularA, limit, magnitude),
 185747514                    GetAllowedImpulseMagnitude(bodyB.AngularVelocity, angularB, limit, magnitude)));
 185747515            if (allowed == magnitude)
 516            {
 183079517                bodyA.ApplyCollisionVelocityState(linearA, angularA);
 183079518                bodyB.ApplyCollisionVelocityState(linearB, angularB);
 183079519                return lambda;
 520            }
 2668521            if (allowed == Fixed64.Zero)
 2245522                return Fixed64.Zero;
 423523            magnitude = allowed;
 423524            lambda = negative ? -allowed : allowed;
 525        }
 15526        return Fixed64.Zero;
 527    }
 528
 529    private static bool TryPrepareImpulseBody(SolidBody body, JointConstraintRow3D row, Vector3d relativeAnchor,
 530        Vector3d impulse, out Vector3d linear, out Vector3d angular)
 531    {
 742992532        Vector3d linearDelta = default, torque = impulse;
 371496533        bool prepared = true;
 371496534        if (row.Kind == JointConstraintRowKind3D.Linear)
 346370535            prepared = ContactResponseArithmetic3D.TryScale(impulse, body.GetConstrainedInverseMass(row.Axis), out linea
 346370536                & Vector3d.TryCross(relativeAnchor, impulse, out torque);
 371496537        prepared &= Fixed3x3.TryTransformDirection(body.GetConstrainedInverseInertiaTensor(), torque, out Vector3d angul
 371496538        return prepared & body.TryPrepareCollisionVelocityState(linearDelta, angularDelta, out linear, out angular);
 539    }
 540
 541    private static Fixed64 GetAllowedImpulseMagnitude(Vector3d current, Vector3d prepared, Fixed64 limit, Fixed64 magnit
 542    {
 543        // Match the body's rounded speed admission so publication cannot apply a different radial cap.
 742988544        if (current == prepared || prepared.Magnitude <= limit)
 740290545            return magnitude;
 2698546        var bound = new FixedBoundSphere(Vector3d.Zero, limit);
 2698547        var segment = new FixedSegment(current, prepared);
 2698548        if (!segment.TryGetSphereIntersectionDistanceInterval(bound, magnitude, out _, out Fixed64 exit))
 423549            return Fixed64.Zero;
 550        // The interval rounds half to even. Retreat one impulse LSB and recheck the materialized endpoint.
 2275551        return exit > Fixed64.Zero ? exit - Fixed64.FromRaw(1) : Fixed64.Zero;
 552    }
 553}

Methods/Properties

.cctor()
Solve(Gravitas.Constraints.Joint3D,System.Boolean,FixedMathSharp.Fixed64)
TryPrepareBiasVelocities(Gravitas.Constraints.Joint3D,System.Span`1<Gravitas.Constraints.JointConstraintRow3D>,FixedMathSharp.Fixed64)
TryGetLinearCorrectionDistance(Gravitas.Constraints.Joint3D,FixedMathSharp.Fixed64&)
TryPrepareAnchors(Gravitas.Constraints.Joint3D,FixedMathSharp.Vector3d&,FixedMathSharp.Vector3d&,FixedMathSharp.Vector3d&)
BuildRows(Gravitas.Constraints.Joint3D,System.Span`1<Gravitas.Constraints.JointConstraintRow3D>,FixedMathSharp.Fixed64&,FixedMathSharp.Fixed64&,FixedMathSharp.Fixed64&)
AddLinearAnchorRow(System.Span`1<Gravitas.Constraints.JointConstraintRow3D>,System.Int32&,FixedMathSharp.Vector3d,FixedMathSharp.Vector3d,FixedMathSharp.Vector3d,FixedMathSharp.Vector3d)
AddAngularRows(Gravitas.Constraints.Joint3D,System.Span`1<Gravitas.Constraints.JointConstraintRow3D>,System.Int32&,FixedMathSharp.FixedQuaternion,FixedMathSharp.FixedQuaternion,FixedMathSharp.Fixed64&)
AddMotorRows(Gravitas.Constraints.Joint3D,System.Span`1<Gravitas.Constraints.JointConstraintRow3D>,System.Int32&,FixedMathSharp.FixedQuaternion,FixedMathSharp.FixedQuaternion,FixedMathSharp.Fixed64&)
AddAxisAlignmentRows(System.Span`1<Gravitas.Constraints.JointConstraintRow3D>,System.Int32&,FixedMathSharp.Vector3d,FixedMathSharp.Vector3d)
AddHingeLimitRow(Gravitas.Constraints.Joint3D,System.Span`1<Gravitas.Constraints.JointConstraintRow3D>,System.Int32&,FixedMathSharp.Vector3d,FixedMathSharp.Vector3d,FixedMathSharp.Fixed64&)
AddConeTwistRows(Gravitas.Constraints.Joint3D,System.Span`1<Gravitas.Constraints.JointConstraintRow3D>,System.Int32&,FixedMathSharp.FixedQuaternion,FixedMathSharp.FixedQuaternion,FixedMathSharp.Fixed64&)
AddAngularErrorRows(System.Span`1<Gravitas.Constraints.JointConstraintRow3D>,System.Int32&,FixedMathSharp.Vector3d,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64)
AddMotorErrorRows(System.Span`1<Gravitas.Constraints.JointConstraintRow3D>,System.Int32&,FixedMathSharp.Vector3d,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64)
AddAngularRow(System.Span`1<Gravitas.Constraints.JointConstraintRow3D>,System.Int32&,FixedMathSharp.Vector3d,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64)
AddMotorRow(System.Span`1<Gravitas.Constraints.JointConstraintRow3D>,System.Int32&,FixedMathSharp.Vector3d,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64)
GetAngularError(FixedMathSharp.FixedQuaternion,FixedMathSharp.FixedQuaternion)
GetSignedTwistAngle(FixedMathSharp.FixedQuaternion,FixedMathSharp.FixedQuaternion,FixedMathSharp.Vector3d)
NormalizeAngle(FixedMathSharp.Fixed64)
Perpendicular(FixedMathSharp.Vector3d)
SolveRow(Gravitas.SolidBody,Gravitas.SolidBody,Gravitas.Constraints.JointConstraintRow3D,System.Boolean&)
ComputeRelativeVelocity(Gravitas.SolidBody,Gravitas.SolidBody,Gravitas.Constraints.JointConstraintRow3D)
ComputeDenominator(Gravitas.SolidBody,Gravitas.SolidBody,Gravitas.Constraints.JointConstraintRow3D)
ApplyImpulse(Gravitas.SolidBody,Gravitas.SolidBody,Gravitas.Constraints.JointConstraintRow3D,FixedMathSharp.Fixed64)
TryPrepareImpulseBody(Gravitas.SolidBody,Gravitas.Constraints.JointConstraintRow3D,FixedMathSharp.Vector3d,FixedMathSharp.Vector3d,FixedMathSharp.Vector3d&,FixedMathSharp.Vector3d&)
GetAllowedImpulseMagnitude(FixedMathSharp.Vector3d,FixedMathSharp.Vector3d,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64)