< Summary

Information
Class: Gravitas.CollisionHandling.ContactNormalImpulseMixed
Assembly: Gravitas
File(s): /home/runner/work/Gravitas/Gravitas/src/Gravitas/CollisionHandling/Response/Mixed/ContactNormalImpulseMixed.cs
Line coverage
100%
Covered lines: 367
Uncovered lines: 0
Coverable lines: 367
Total lines: 688
Line coverage: 100%
Branch coverage
100%
Covered branches: 74
Total branches: 74
Branch coverage: 100%
Method coverage

Feature is only available for sponsors

Upgrade to PRO version

Metrics

MethodBranch coverage Crap Score Cyclomatic complexity Line coverage
CanUseCompactResponse(...)100%66100%
TryCalculateVelocityDeltas(...)100%2020100%
TryCalculateAccumulatedDelta(...)100%2424100%
TryCalculateVelocityDeltasExact(...)100%22100%
TryCalculateAccumulatedDeltaExact(...)100%44100%
TryComputeNormalVelocity(...)100%11100%
TryComputeDenominator(...)100%22100%
GetConstrainedInverseMass3D(...)100%22100%
GetConstrainedInverseMass2D(...)100%44100%
TryResolveAngularVelocityDelta3D(...)100%44100%
TryResolveAngularVelocityDelta2D(...)100%66100%
ZeroImpulse(...)100%11100%
ZeroVelocityDelta(...)100%11100%

File(s)

/home/runner/work/Gravitas/Gravitas/src/Gravitas/CollisionHandling/Response/Mixed/ContactNormalImpulseMixed.cs

#LineLine coverage
 1//=======================================================================
 2// ContactNormalImpulseMixed.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 FixedMathSharp.Geometry;
 10using System.Runtime.CompilerServices;
 11
 12namespace Gravitas.CollisionHandling;
 13
 14/// <summary>
 15/// Side-effect-free mixed 3D/2D contact-normal impulse result.
 16/// </summary>
 17internal readonly struct ContactNormalImpulseResultMixed
 18{
 19    public ContactNormalImpulseResultMixed(
 20        Fixed64 normalVelocity,
 21        Fixed64 impulseScalar,
 22        Vector3d linearVelocityDelta3D,
 23        Vector3d angularVelocityDelta3D,
 24        Vector2d linearVelocityDelta2D,
 25        Fixed64 angularVelocityDelta2D)
 26        : this(
 27            normalVelocity,
 28            impulseScalar,
 29            impulseScalar,
 30            linearVelocityDelta3D,
 31            angularVelocityDelta3D,
 32            linearVelocityDelta2D,
 33            angularVelocityDelta2D)
 34    {
 35    }
 36
 37    public ContactNormalImpulseResultMixed(
 38        Fixed64 normalVelocity,
 39        Fixed64 impulseScalar,
 40        Fixed64 appliedImpulseScalar,
 41        Vector3d linearVelocityDelta3D,
 42        Vector3d angularVelocityDelta3D,
 43        Vector2d linearVelocityDelta2D,
 44        Fixed64 angularVelocityDelta2D,
 45        bool hasRepresentableNormalVelocity = true,
 46        bool hasRepresentableAppliedImpulse = true)
 47    {
 48        NormalVelocity = normalVelocity;
 49        ImpulseScalar = impulseScalar;
 50        AppliedImpulseScalar = appliedImpulseScalar;
 51        LinearVelocityDelta3D = linearVelocityDelta3D;
 52        AngularVelocityDelta3D = angularVelocityDelta3D;
 53        LinearVelocityDelta2D = linearVelocityDelta2D;
 54        AngularVelocityDelta2D = angularVelocityDelta2D;
 55        HasRepresentableNormalVelocity = hasRepresentableNormalVelocity;
 56        HasRepresentableAppliedImpulse = hasRepresentableAppliedImpulse;
 57    }
 58
 59    public Fixed64 NormalVelocity { get; }
 60
 61    public Fixed64 ImpulseScalar { get; }
 62
 63    public Fixed64 AppliedImpulseScalar { get; }
 64
 65    public bool HasRepresentableNormalVelocity { get; }
 66
 67    public bool HasRepresentableAppliedImpulse { get; }
 68
 69    public Vector3d LinearVelocityDelta3D { get; }
 70
 71    public Vector3d AngularVelocityDelta3D { get; }
 72
 73    public Vector2d LinearVelocityDelta2D { get; }
 74
 75    public Fixed64 AngularVelocityDelta2D { get; }
 76}
 77
 78/// <summary>
 79/// Side-effect-free mixed velocity deltas for a normal response whose impulse
 80/// scalar does not need to be representable.
 81/// </summary>
 82internal readonly struct ContactNormalVelocityDeltaResultMixed
 83{
 84    public ContactNormalVelocityDeltaResultMixed(
 85        Fixed64 normalVelocity,
 86        Vector3d linearVelocityDelta3D,
 87        Vector3d angularVelocityDelta3D,
 88        Vector2d linearVelocityDelta2D,
 89        Fixed64 angularVelocityDelta2D)
 90        : this(
 91            normalVelocity,
 92            linearVelocityDelta3D,
 93            angularVelocityDelta3D,
 94            linearVelocityDelta2D,
 95            angularVelocityDelta2D,
 96            normalVelocity < Fixed64.Zero,
 97            hasRepresentableNormalVelocity: true)
 98    {
 99    }
 100
 101    public ContactNormalVelocityDeltaResultMixed(
 102        Fixed64 normalVelocity,
 103        Vector3d linearVelocityDelta3D,
 104        Vector3d angularVelocityDelta3D,
 105        Vector2d linearVelocityDelta2D,
 106        Fixed64 angularVelocityDelta2D,
 107        bool isClosing,
 108        bool hasRepresentableNormalVelocity)
 109    {
 110        NormalVelocity = normalVelocity;
 111        LinearVelocityDelta3D = linearVelocityDelta3D;
 112        AngularVelocityDelta3D = angularVelocityDelta3D;
 113        LinearVelocityDelta2D = linearVelocityDelta2D;
 114        AngularVelocityDelta2D = angularVelocityDelta2D;
 115        IsClosing = isClosing;
 116        HasRepresentableNormalVelocity = hasRepresentableNormalVelocity;
 117    }
 118
 119    public Fixed64 NormalVelocity { get; }
 120
 121    public bool IsClosing { get; }
 122
 123    public bool HasRepresentableNormalVelocity { get; }
 124
 125    public Vector3d LinearVelocityDelta3D { get; }
 126
 127    public Vector3d AngularVelocityDelta3D { get; }
 128
 129    public Vector2d LinearVelocityDelta2D { get; }
 130
 131    public Fixed64 AngularVelocityDelta2D { get; }
 132}
 133
 134/// <summary>
 135/// Calculates allocation-free mixed 3D/2D contact-point normal response
 136/// without mutating either participant.
 137/// </summary>
 138internal static class ContactNormalImpulseMixed
 139{
 140    internal static bool CanUseCompactResponse(
 141        SolidBody? body3D,
 142        Vector3d linearVelocity3D,
 143        Vector3d angularVelocity3D,
 144        Vector3d relativeContactPoint3D,
 145        SolidBody2D? body2D,
 146        Vector2d linearVelocity2D,
 147        Fixed64 angularVelocity2D,
 148        Vector2d relativeContactPoint2D,
 149        Vector3d axis)
 150    {
 384151        Vector3d planarLever =
 384152            ExactContactLever2D.ToSpatial(relativeContactPoint2D);
 384153        return ContactResponseArithmetic3D.CanUseFastPointVelocity(
 384154                linearVelocity3D,
 384155                angularVelocity3D,
 384156                relativeContactPoint3D,
 384157                ExactContactLever2D.ToSpatial(linearVelocity2D),
 384158                new Vector3d(
 384159                    Fixed64.Zero,
 384160                    -angularVelocity2D,
 384161                    Fixed64.Zero),
 384162                planarLever,
 384163                axis)
 384164            && ContactResponseArithmetic3D.CanUseFastAngularResponse(
 384165                relativeContactPoint3D,
 384166                axis,
 384167                body3D?.GetConstrainedInverseInertiaTensor()
 384168                    ?? Fixed3x3.Zero)
 384169            && ContactResponseArithmetic3D.CanUseFastAngularResponse(
 384170                planarLever,
 384171                axis,
 384172                ExactContactLever2D.CreateInverseInertia(body2D));
 173    }
 174
 175    internal static bool TryCalculateVelocityDeltas(
 176        SolidBody? body3D,
 177        Vector3d linearVelocity3D,
 178        Vector3d angularVelocity3D,
 179        Vector3d relativeContactPoint3D,
 180        SolidBody2D? body2D,
 181        Vector2d linearVelocity2D,
 182        Fixed64 angularVelocity2D,
 183        Vector2d relativeContactPoint2D,
 184        Vector3d normal,
 185        Fixed64 restitution,
 186        Fixed64 restitutionVelocityThreshold,
 187        out ContactNormalVelocityDeltaResultMixed result)
 188    {
 37189        result = default;
 37190        if (!TryComputeNormalVelocity(
 37191                linearVelocity3D,
 37192                angularVelocity3D,
 37193                relativeContactPoint3D,
 37194                linearVelocity2D,
 37195                angularVelocity2D,
 37196                relativeContactPoint2D,
 37197                normal,
 37198                out Fixed64 normalVelocity))
 199        {
 1200            return false;
 201        }
 36202        if (normalVelocity >= Fixed64.Zero)
 203        {
 1204            result = ZeroVelocityDelta(normalVelocity);
 1205            return true;
 206        }
 207
 35208        if (!TryComputeDenominator(
 35209                body3D,
 35210                relativeContactPoint3D,
 35211                body2D,
 35212                relativeContactPoint2D,
 35213                normal,
 35214                out Fixed64 denominator))
 215        {
 1216            return false;
 217        }
 34218        if (denominator <= Fixed64.Zero)
 3219            return false;
 220
 31221        Fixed64 appliedRestitution = normalVelocity < -restitutionVelocityThreshold
 31222            ? restitution
 31223            : Fixed64.Zero;
 31224        Fixed64 responseFactor = -(Fixed64.One + appliedRestitution);
 31225        Vector2d planarNormal = normal.ToVector2d();
 31226        bool linear3DResolved = ContinuousCollisionImpulsePolicy.TryResolveVelocityDelta(
 31227                body3D?.ProjectLinearMotion(-normal) ?? Vector3d.Zero,
 31228                normalVelocity,
 31229                responseFactor,
 31230                body3D?.EffectiveInverseMass ?? Fixed64.Zero,
 31231                denominator,
 31232                out Vector3d linearVelocityDelta3D);
 31233        bool angular3DResolved = TryResolveAngularVelocityDelta3D(
 31234                body3D,
 31235                relativeContactPoint3D,
 31236                -normal,
 31237                normalVelocity,
 31238                responseFactor,
 31239                denominator,
 31240                out Vector3d angularVelocityDelta3D);
 31241        bool linear2DResolved = ContinuousCollisionImpulsePolicy.TryResolveVelocityDelta(
 31242                body2D?.ProjectLinearMotion(planarNormal) ?? Vector2d.Zero,
 31243                normalVelocity,
 31244                responseFactor,
 31245                body2D?.EffectiveInverseMass ?? Fixed64.Zero,
 31246                denominator,
 31247                out Vector2d linearVelocityDelta2D);
 31248        bool angular2DResolved = TryResolveAngularVelocityDelta2D(
 31249                body2D,
 31250                relativeContactPoint2D,
 31251                planarNormal,
 31252                normalVelocity,
 31253                responseFactor,
 31254                denominator,
 31255                out Fixed64 angularVelocityDelta2D);
 31256        if (!(linear3DResolved
 31257                & angular3DResolved
 31258                & linear2DResolved
 31259                & angular2DResolved))
 260        {
 1261            return false;
 262        }
 263
 30264        result = new ContactNormalVelocityDeltaResultMixed(
 30265            normalVelocity,
 30266            linearVelocityDelta3D,
 30267            angularVelocityDelta3D,
 30268            linearVelocityDelta2D,
 30269            angularVelocityDelta2D);
 30270        return true;
 271    }
 272
 273    internal static bool TryCalculateAccumulatedDelta(
 274        SolidBody? body3D,
 275        Vector3d linearVelocity3D,
 276        Vector3d angularVelocity3D,
 277        Vector3d relativeContactPoint3D,
 278        SolidBody2D? body2D,
 279        Vector2d linearVelocity2D,
 280        Fixed64 angularVelocity2D,
 281        Vector2d relativeContactPoint2D,
 282        Vector3d normal,
 283        Fixed64 restitution,
 284        Fixed64 restitutionVelocityThreshold,
 285        Fixed64 accumulatedImpulse,
 286        Fixed64 positiveImpulseScale,
 287        Fixed64 negativeImpulseScale,
 288        out ContactNormalImpulseResultMixed result)
 289    {
 285290        result = default;
 285291        bool inputsValid =
 285292            accumulatedImpulse >= Fixed64.Zero
 285293            & positiveImpulseScale >= Fixed64.Zero
 285294            & negativeImpulseScale >= Fixed64.Zero;
 285295        if (!inputsValid)
 3296            return false;
 282297        if (!TryComputeNormalVelocity(
 282298                linearVelocity3D,
 282299                angularVelocity3D,
 282300                relativeContactPoint3D,
 282301                linearVelocity2D,
 282302                angularVelocity2D,
 282303                relativeContactPoint2D,
 282304                normal,
 282305                out Fixed64 normalVelocity)
 282306            || !TryComputeDenominator(
 282307                body3D,
 282308                relativeContactPoint3D,
 282309                body2D,
 282310                relativeContactPoint2D,
 282311                normal,
 282312                out Fixed64 denominator))
 313        {
 3314            return false;
 315        }
 279316        if (denominator <= Fixed64.Zero)
 317        {
 3318            result = ZeroImpulse(normalVelocity);
 3319            return true;
 320        }
 321
 276322        Fixed64 appliedRestitution = normalVelocity < -restitutionVelocityThreshold
 276323            ? restitution
 276324            : Fixed64.Zero;
 276325        Fixed64 responseFactor = -(Fixed64.One + appliedRestitution);
 276326        Fixed64 impulseScale = normalVelocity < Fixed64.Zero
 276327            ? positiveImpulseScale
 276328            : negativeImpulseScale;
 329        Fixed64 impulseScalar;
 276330        if (!Fixed64.TryMultiplyDivide(
 276331                normalVelocity,
 276332                responseFactor,
 276333                impulseScale,
 276334                denominator,
 276335                out Fixed64 scaledImpulse))
 336        {
 4337            if (normalVelocity < Fixed64.Zero)
 2338                return false;
 2339            impulseScalar = -accumulatedImpulse;
 340        }
 272341        else if (!Fixed64.TryAdd(
 272342                    accumulatedImpulse,
 272343                    scaledImpulse,
 272344                    out Fixed64 accumulated)
 272345                || !Fixed64.TrySubtract(
 272346                    FixedMath.Max(Fixed64.Zero, accumulated),
 272347                    accumulatedImpulse,
 272348                    out impulseScalar))
 349        {
 1350            return false;
 351        }
 273352        if (impulseScalar == Fixed64.Zero)
 353        {
 183354            result = ZeroImpulse(normalVelocity);
 183355            return true;
 356        }
 357
 90358        Vector2d planarNormal = normal.ToVector2d();
 90359        bool impulse3DResolved = ContactResponseArithmetic3D.TryScale(
 90360            -normal,
 90361            impulseScalar,
 90362            out Vector3d impulse3D);
 90363        bool linear3DResolved =
 90364            ContactNormalImpulse3D.TryComputeLinearVelocityDelta(
 90365                body3D,
 90366                impulse3D,
 90367                out Vector3d linearVelocityDelta3D);
 90368        bool angular3DResolved =
 90369            ContactNormalImpulse3D.TryComputeAngularVelocityDelta(
 90370                body3D,
 90371                relativeContactPoint3D,
 90372                impulse3D,
 90373                out Vector3d angularVelocityDelta3D);
 90374        bool linear2DResolved =
 90375            ContactNormalImpulse2D.TryComputeLinearVelocityDelta(
 90376                body2D,
 90377                planarNormal,
 90378                impulseScalar,
 90379                out Vector2d linearVelocityDelta2D);
 90380        bool angular2DResolved =
 90381            ContactNormalImpulse2D.TryComputeAngularVelocityDelta(
 90382                body2D,
 90383                relativeContactPoint2D,
 90384                planarNormal,
 90385                impulseScalar,
 90386                out Fixed64 angularVelocityDelta2D);
 90387        if (!(impulse3DResolved
 90388            & linear3DResolved
 90389            & angular3DResolved
 90390            & linear2DResolved
 90391            & angular2DResolved))
 392        {
 1393            return false;
 394        }
 395
 89396        result = new ContactNormalImpulseResultMixed(
 89397            normalVelocity,
 89398            impulseScalar,
 89399            linearVelocityDelta3D,
 89400            angularVelocityDelta3D,
 89401            linearVelocityDelta2D,
 89402            angularVelocityDelta2D);
 89403        return true;
 404    }
 405
 406    internal static bool TryCalculateVelocityDeltasExact(
 407        SolidBody? body3D,
 408        Vector3d linearVelocity3D,
 409        Vector3d angularVelocity3D,
 410        in ExactLever3D relativeContactPoint3D,
 411        SolidBody2D? body2D,
 412        Vector2d linearVelocity2D,
 413        Fixed64 angularVelocity2D,
 414        in ExactLever3D relativeContactPoint2D,
 415        Vector3d normal,
 416        Fixed64 restitution,
 417        Fixed64 restitutionVelocityThreshold,
 418        out ContactNormalVelocityDeltaResultMixed result)
 419    {
 92420        result = default;
 92421        ExactContactResponseOperand3D first =
 92422            ExactContactLever3D.CreateResponseOperand(
 92423                body3D,
 92424                linearVelocity3D,
 92425                angularVelocity3D,
 92426                relativeContactPoint3D,
 92427                -normal);
 92428        ExactContactResponseOperand3D second =
 92429            ExactContactLever2D.CreateResponseOperand(
 92430                body2D,
 92431                linearVelocity2D,
 92432                angularVelocity2D,
 92433                relativeContactPoint2D,
 92434                normal);
 92435        if (!ExactContactResponseKernel.TryGetNormalResponse(
 92436                first,
 92437                second,
 92438                normal,
 92439                restitution,
 92440                restitutionVelocityThreshold,
 92441                out ExactNormalResponse3D response))
 442        {
 2443            return false;
 444        }
 445
 90446        bool hasNormalVelocity =
 90447            response.TryGetNormalVelocity(out Fixed64 normalVelocity);
 90448        result = new ContactNormalVelocityDeltaResultMixed(
 90449            normalVelocity,
 90450            response.FirstLinearVelocityDelta,
 90451            response.FirstAngularVelocityDelta,
 90452            ExactContactLever2D.ToPlanar(response.SecondLinearVelocityDelta),
 90453            ExactContactLever2D.ToPlanarAngular(response.SecondAngularVelocityDelta),
 90454            response.IsClosing,
 90455            hasNormalVelocity);
 90456        return true;
 457    }
 458
 459    internal static bool TryCalculateAccumulatedDeltaExact(
 460        SolidBody? body3D,
 461        Vector3d linearVelocity3D,
 462        Vector3d angularVelocity3D,
 463        in ExactLever3D relativeContactPoint3D,
 464        SolidBody2D? body2D,
 465        Vector2d linearVelocity2D,
 466        Fixed64 angularVelocity2D,
 467        in ExactLever3D relativeContactPoint2D,
 468        Vector3d normal,
 469        Fixed64 restitution,
 470        Fixed64 restitutionVelocityThreshold,
 471        Fixed64 accumulatedImpulse,
 472        Fixed64 positiveImpulseScale,
 473        Fixed64 negativeImpulseScale,
 474        out ContactNormalImpulseResultMixed result)
 475    {
 32476        result = default;
 32477        ExactContactResponseOperand3D first =
 32478            ExactContactLever3D.CreateResponseOperand(
 32479                body3D,
 32480                linearVelocity3D,
 32481                angularVelocity3D,
 32482                relativeContactPoint3D,
 32483                -normal);
 32484        ExactContactResponseOperand3D second =
 32485            ExactContactLever2D.CreateResponseOperand(
 32486                body2D,
 32487                linearVelocity2D,
 32488                angularVelocity2D,
 32489                relativeContactPoint2D,
 32490                normal);
 32491        if (!ExactContactResponseKernel.TryGetAccumulatedNormalResponse(
 32492                first,
 32493                second,
 32494                normal,
 32495                restitution,
 32496                restitutionVelocityThreshold,
 32497                accumulatedImpulse,
 32498                positiveImpulseScale,
 32499                negativeImpulseScale,
 32500                out ExactNormalResponse3D response))
 501        {
 3502            return false;
 503        }
 504
 29505        bool hasNormalVelocity =
 29506            response.TryGetNormalVelocity(out Fixed64 normalVelocity);
 29507        bool hasAppliedImpulse =
 29508            response.TryGetAppliedImpulse(out Fixed64 appliedImpulse);
 29509        Fixed64 impulseScalar = response.TryGetAccumulatedImpulse(
 29510                out Fixed64 newAccumulatedImpulse)
 29511            ? newAccumulatedImpulse - accumulatedImpulse
 29512            : -accumulatedImpulse;
 29513        result = new ContactNormalImpulseResultMixed(
 29514            normalVelocity,
 29515            impulseScalar,
 29516            appliedImpulse,
 29517            response.FirstLinearVelocityDelta,
 29518            response.FirstAngularVelocityDelta,
 29519            ExactContactLever2D.ToPlanar(response.SecondLinearVelocityDelta),
 29520            ExactContactLever2D.ToPlanarAngular(response.SecondAngularVelocityDelta),
 29521            hasNormalVelocity,
 29522            hasAppliedImpulse);
 29523        return true;
 524    }
 525
 526    [MethodImpl(MethodImplOptions.AggressiveInlining)]
 527    private static bool TryComputeNormalVelocity(
 528        Vector3d linearVelocity3D,
 529        Vector3d angularVelocity3D,
 530        Vector3d relativeContactPoint3D,
 531        Vector2d linearVelocity2D,
 532        Fixed64 angularVelocity2D,
 533        Vector2d relativeContactPoint2D,
 534        Vector3d normal,
 535        out Fixed64 normalVelocity) =>
 319536        ContactNormalImpulse3D.TryComputeNormalVelocity(
 319537            linearVelocity3D,
 319538            angularVelocity3D,
 319539            relativeContactPoint3D,
 319540            ExactContactLever2D.ToSpatial(linearVelocity2D),
 319541            new Vector3d(Fixed64.Zero, -angularVelocity2D, Fixed64.Zero),
 319542            ExactContactLever2D.ToSpatial(relativeContactPoint2D),
 319543            normal,
 319544            out normalVelocity);
 545
 546    [MethodImpl(MethodImplOptions.AggressiveInlining)]
 547    private static bool TryComputeDenominator(
 548        SolidBody? body3D,
 549        Vector3d relativeContactPoint3D,
 550        SolidBody2D? body2D,
 551        Vector2d relativeContactPoint2D,
 552        Vector3d normal,
 553        out Fixed64 denominator)
 554    {
 316555        Vector2d planarNormal = normal.ToVector2d();
 316556        bool angular3DResolved =
 316557            ContactNormalImpulse3D.TryComputeAngularDenominator(
 316558                body3D,
 316559                relativeContactPoint3D,
 316560                normal,
 316561                out Fixed64 angular3D);
 316562        bool angular2DResolved =
 316563            ContactNormalImpulse2D.TryComputeAngularDenominator(
 316564                body2D,
 316565                relativeContactPoint2D,
 316566                planarNormal,
 316567                out Fixed64 angular2D);
 316568        bool sumResolved = Fixed64.TryAdd(
 316569                GetConstrainedInverseMass3D(body3D, normal),
 316570                GetConstrainedInverseMass2D(body2D, normal),
 316571                out Fixed64 linear)
 316572            & Fixed64.TryAdd(linear, angular3D, out Fixed64 first)
 316573            & Fixed64.TryAdd(first, angular2D, out denominator);
 316574        if (!(angular3DResolved & angular2DResolved & sumResolved))
 575        {
 3576            denominator = default;
 3577            return false;
 578        }
 579
 313580        return true;
 581    }
 582
 583    [MethodImpl(MethodImplOptions.AggressiveInlining)]
 584    private static Fixed64 GetConstrainedInverseMass3D(SolidBody? body, Vector3d axis) =>
 316585        body?.GetConstrainedInverseMass(axis) ?? Fixed64.Zero;
 586
 587    [MethodImpl(MethodImplOptions.AggressiveInlining)]
 588    private static Fixed64 GetConstrainedInverseMass2D(SolidBody2D? body, Vector3d axis)
 589    {
 316590        if (body == null)
 41591            return Fixed64.Zero;
 592
 275593        Vector2d planarAxis = axis.ToVector2d();
 275594        return planarAxis == Vector2d.Zero
 275595            ? Fixed64.Zero
 275596            : body.GetConstrainedInverseMass(planarAxis) * planarAxis.MagnitudeSquared;
 597    }
 598
 599    private static bool TryResolveAngularVelocityDelta3D(
 600        SolidBody? body,
 601        Vector3d relativeContactPoint,
 602        Vector3d signedNormal,
 603        Fixed64 normalVelocity,
 604        Fixed64 responseFactor,
 605        Fixed64 denominator,
 606        out Vector3d velocityDelta)
 607    {
 31608        velocityDelta = Vector3d.Zero;
 31609        if (body?.CanRotate != true)
 8610            return true;
 611
 23612        Vector3d response = body.ApplyConstrainedInverseInertia(
 23613            Vector3d.Cross(relativeContactPoint, signedNormal));
 23614        bool xResolved = Fixed64.TryMultiplyDivide(
 23615            response.X,
 23616            normalVelocity,
 23617            responseFactor,
 23618            denominator,
 23619            out Fixed64 x);
 23620        bool yResolved = Fixed64.TryMultiplyDivide(
 23621            response.Y,
 23622            normalVelocity,
 23623            responseFactor,
 23624            denominator,
 23625            out Fixed64 y);
 23626        bool zResolved = Fixed64.TryMultiplyDivide(
 23627            response.Z,
 23628            normalVelocity,
 23629            responseFactor,
 23630            denominator,
 23631            out Fixed64 z);
 23632        velocityDelta = new Vector3d(x, y, z);
 23633        return xResolved & yResolved & zResolved;
 634    }
 635
 636    [MethodImpl(MethodImplOptions.AggressiveInlining)]
 637    private static bool TryResolveAngularVelocityDelta2D(
 638        SolidBody2D? body,
 639        Vector2d relativeContactPoint,
 640        Vector2d signedNormal,
 641        Fixed64 normalVelocity,
 642        Fixed64 responseFactor,
 643        Fixed64 denominator,
 644        out Fixed64 velocityDelta)
 645    {
 31646        velocityDelta = Fixed64.Zero;
 31647        if (body?.CanRotate != true)
 9648            return true;
 649
 22650        Fixed64 torqueScale = Vector2d.CrossProduct(relativeContactPoint, signedNormal);
 22651        if (torqueScale == Fixed64.Zero)
 14652            return true;
 653
 8654        bool angularScaleResolved = Fixed64.TryMultiplyDivide(
 8655            normalVelocity,
 8656            responseFactor,
 8657            body.EffectiveInverseMomentOfInertia,
 8658            denominator,
 8659            out Fixed64 angularScale);
 8660        bool velocityDeltaResolved = Fixed64.TryMultiplyDivide(
 8661            torqueScale,
 8662            angularScale,
 8663            Fixed64.One,
 8664            out velocityDelta);
 8665        return angularScaleResolved
 8666            & (angularScale != Fixed64.Zero | torqueScale.Abs() <= Fixed64.One)
 8667            & velocityDeltaResolved;
 668    }
 669
 670    [MethodImpl(MethodImplOptions.AggressiveInlining)]
 671    private static ContactNormalImpulseResultMixed ZeroImpulse(Fixed64 normalVelocity) =>
 186672        new(
 186673            normalVelocity,
 186674            Fixed64.Zero,
 186675            Vector3d.Zero,
 186676            Vector3d.Zero,
 186677            Vector2d.Zero,
 186678            Fixed64.Zero);
 679
 680    [MethodImpl(MethodImplOptions.AggressiveInlining)]
 681    private static ContactNormalVelocityDeltaResultMixed ZeroVelocityDelta(Fixed64 normalVelocity) =>
 1682        new(
 1683            normalVelocity,
 1684            Vector3d.Zero,
 1685            Vector3d.Zero,
 1686            Vector2d.Zero,
 1687            Fixed64.Zero);
 688}
Generated by: ReportGenerator 5.5.11.0
08/05/2026 - 20:00:44
GitHub | reportgenerator.io

Methods/Properties

CanUseCompactResponse(Gravitas.SolidBody,FixedMathSharp.Vector3d,FixedMathSharp.Vector3d,FixedMathSharp.Vector3d,Gravitas.SolidBody2D,FixedMathSharp.Vector2d,FixedMathSharp.Fixed64,FixedMathSharp.Vector2d,FixedMathSharp.Vector3d)
TryCalculateVelocityDeltas(Gravitas.SolidBody,FixedMathSharp.Vector3d,FixedMathSharp.Vector3d,FixedMathSharp.Vector3d,Gravitas.SolidBody2D,FixedMathSharp.Vector2d,FixedMathSharp.Fixed64,FixedMathSharp.Vector2d,FixedMathSharp.Vector3d,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,Gravitas.CollisionHandling.ContactNormalVelocityDeltaResultMixed&)
TryCalculateAccumulatedDelta(Gravitas.SolidBody,FixedMathSharp.Vector3d,FixedMathSharp.Vector3d,FixedMathSharp.Vector3d,Gravitas.SolidBody2D,FixedMathSharp.Vector2d,FixedMathSharp.Fixed64,FixedMathSharp.Vector2d,FixedMathSharp.Vector3d,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,Gravitas.CollisionHandling.ContactNormalImpulseResultMixed&)
TryCalculateVelocityDeltasExact(Gravitas.SolidBody,FixedMathSharp.Vector3d,FixedMathSharp.Vector3d,Gravitas.CollisionHandling.ExactLever3D&,Gravitas.SolidBody2D,FixedMathSharp.Vector2d,FixedMathSharp.Fixed64,Gravitas.CollisionHandling.ExactLever3D&,FixedMathSharp.Vector3d,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,Gravitas.CollisionHandling.ContactNormalVelocityDeltaResultMixed&)
TryCalculateAccumulatedDeltaExact(Gravitas.SolidBody,FixedMathSharp.Vector3d,FixedMathSharp.Vector3d,Gravitas.CollisionHandling.ExactLever3D&,Gravitas.SolidBody2D,FixedMathSharp.Vector2d,FixedMathSharp.Fixed64,Gravitas.CollisionHandling.ExactLever3D&,FixedMathSharp.Vector3d,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,Gravitas.CollisionHandling.ContactNormalImpulseResultMixed&)
TryComputeNormalVelocity(FixedMathSharp.Vector3d,FixedMathSharp.Vector3d,FixedMathSharp.Vector3d,FixedMathSharp.Vector2d,FixedMathSharp.Fixed64,FixedMathSharp.Vector2d,FixedMathSharp.Vector3d,FixedMathSharp.Fixed64&)
TryComputeDenominator(Gravitas.SolidBody,FixedMathSharp.Vector3d,Gravitas.SolidBody2D,FixedMathSharp.Vector2d,FixedMathSharp.Vector3d,FixedMathSharp.Fixed64&)
GetConstrainedInverseMass3D(Gravitas.SolidBody,FixedMathSharp.Vector3d)
GetConstrainedInverseMass2D(Gravitas.SolidBody2D,FixedMathSharp.Vector3d)
TryResolveAngularVelocityDelta3D(Gravitas.SolidBody,FixedMathSharp.Vector3d,FixedMathSharp.Vector3d,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,FixedMathSharp.Vector3d&)
TryResolveAngularVelocityDelta2D(Gravitas.SolidBody2D,FixedMathSharp.Vector2d,FixedMathSharp.Vector2d,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64&)
ZeroImpulse(FixedMathSharp.Fixed64)
ZeroVelocityDelta(FixedMathSharp.Fixed64)