< Summary

Line coverage
100%
Covered lines: 853
Uncovered lines: 0
Coverable lines: 853
Total lines: 1269
Line coverage: 100%
Branch coverage
100%
Covered branches: 394
Total branches: 394
Branch coverage: 100%
Method coverage

Feature is only available for sponsors

Upgrade to PRO version

Metrics

MethodBranch coverage Crap Score Cyclomatic complexity Line coverage
File 1: .cctor()100%11100%
File 1: Resolve(...)100%11100%
File 1: Resolve(...)100%4444100%
File 1: ApplyPositionCorrection(...)100%1212100%
File 1: TryGetNormalImpulse(...)100%3232100%
File 1: CanApplyNormalVelocityDeltas(...)100%11100%
File 1: HasVelocityDelta(...)100%66100%
File 1: ApplyNormalVelocityDeltas(...)100%11100%
File 1: CanApplyVelocityDeltas(...)100%44100%
File 1: ApplyVelocityDeltas(...)100%44100%
File 1: TryApplyFrictionImpulse(...)100%1818100%
File 1: TryGetCompactFrictionImpulse(...)100%3434100%
File 1: TryApplyFrictionImpulseExact(...)100%2020100%
File 1: TryApplyImpulse(...)100%2222100%
File 1: TryGetCompactTangent(...)100%88100%
File 1: TryGetCompactConstrainedInverseMass(...)100%66100%
File 1: TryGetCompactConstrainedPlanarInverseMass(...)100%88100%
File 1: ResolveLinearVelocity(...)100%22100%
File 1: ResolveLinearVelocity(...)100%22100%
File 1: ResolveAngularVelocity(...)100%22100%
File 1: ResolveAngularVelocity(...)100%22100%
File 1: ResolveNormal(...)100%88100%
File 1: ResolveFallbackDirection(...)100%88100%
File 1: GetExactLevers(...)100%44100%
File 1: GetConstrainedInverseMass(...)100%22100%
File 1: GetConstrainedPlanarInverseMass(...)100%44100%
File 2: ResolveManifold(...)100%66100%
File 2: PrepareSolve(...)100%1818100%
File 2: HasPlanarResponseCoupling(...)100%44100%
File 2: ResolveManifold(...)100%1212100%
File 2: IsCorrectionOwner(...)100%1616100%
File 2: ResolveRow(...)100%3232100%
File 2: TrySolveManifoldFriction(...)100%1414100%
File 2: TrySolveManifoldVelocity(...)100%1414100%
File 2: TrySolveNormalRow(...)100%22100%
File 2: TryAddVelocityDeltas(...)100%88100%
File 2: GetExactImpulse(...)100%44100%
File 2: SetExactImpulse(...)100%66100%
File 2: ClearExactImpulses(...)100%66100%
File 2: RejectManifoldResponse()100%11100%

File(s)

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

#LineLine coverage
 1//=======================================================================
 2// CollisionResponseMixed.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.Runtime.CompilerServices;
 9using FixedMathSharp;
 10using Gravitas.Colliders;
 11using Gravitas.Materials;
 12
 13namespace Gravitas.CollisionHandling;
 14
 15/// <summary>
 16/// Solves deterministic mixed 3D/2D contact response with the 2D body constrained to its X/Z plane.
 17/// </summary>
 18/// <content>Owns single contacts and grouped mixed normal, friction and correction constraints.</content>
 19public static partial class CollisionResponseMixed
 20{
 21    /// <summary>Penetration tolerated before positional correction is applied.</summary>
 122    public static readonly Fixed64 PenetrationSlop = (Fixed64)0.01f;
 23
 24    /// <summary>Fraction of excess penetration corrected by the mixed solver.</summary>
 125    public static readonly Fixed64 PenetrationCorrectionPercent = (Fixed64)0.8f;
 26
 27    internal static bool Resolve(CollisionPairMixed pair, MixedContact contact) =>
 2728        Resolve(pair, contact, iteration: 0, iterationLimit: 1, applyPositionCorrection: true);
 29
 30    internal static bool Resolve(
 31        CollisionPairMixed pair,
 32        MixedContact contact,
 33        int iteration,
 34        int iterationLimit,
 35        bool applyPositionCorrection,
 36        bool authoritativeNormal = false)
 37    {
 550238        if (!contact.HasContact || pair.Collider3D.IsTrigger || pair.Collider2D.IsTrigger)
 339            return false;
 40
 549941        SolidBody? body3D = pair.Collider3D.Body;
 549942        SolidBody2D? body2D = pair.Collider2D.Body;
 549943        Vector3d normal = authoritativeNormal ? contact.Normal3DTo2D.Normalized : ResolveNormal(pair, contact);
 549944        bool hasPlanarResponseCoupling = normal.ToVector2d() != Vector2d.Zero;
 45
 549946        Fixed64 correctionInverseMass = GetConstrainedInverseMass(body3D, normal)
 549947            + GetConstrainedPlanarInverseMass(body2D, normal);
 48
 549949        Vector2d relative2D = default;
 549950        bool resolved3D = body3D != null
 549951            ? body3D.TryGetOffsetFromCenterOfMass(contact.Anchor3D, out Vector3d relative3D)
 549952            : contact.Anchor3D.TryGetOffsetFrom(pair.Collider3D.Center, out relative3D);
 549953        bool resolved2D = body2D == null
 549954            || contact.TryGetPlanarOffset2DFrom(
 549955                body2D.Position,
 549956                body2D.Rotation,
 549957                body2D.LocalCenterOfMassOffset,
 549958                out relative2D);
 549959        if (body2D == null)
 450960            relative2D = Vector2d.Zero;
 549961        Vector3d normalLinearVelocity3D =
 549962            body3D == null
 549963                ? Vector3d.Zero
 549964                : ResolveLinearVelocity(body3D);
 549965        Vector3d normalAngularVelocity3D =
 549966            body3D == null
 549967                ? Vector3d.Zero
 549968                : ResolveAngularVelocity(body3D);
 549969        Vector2d normalLinearVelocity2D =
 549970            body2D == null
 549971                ? Vector2d.Zero
 549972                : ResolveLinearVelocity(body2D);
 549973        Fixed64 normalAngularVelocity2D =
 549974            body2D == null
 549975                ? Fixed64.Zero
 549976                : ResolveAngularVelocity(body2D);
 77
 549978        ExactContactImpulseState? exactImpulse = null;
 549979        bool normalResolved = TryGetNormalImpulse(
 549980            pair.Collider3D, pair.Collider2D,
 549981            contact,
 549982            body3D,
 549983            body2D,
 549984            normal,
 549985            relative3D,
 549986            relative2D,
 549987            resolved3D,
 549988            resolved2D,
 549989            normalLinearVelocity3D,
 549990            normalAngularVelocity3D,
 549991            normalLinearVelocity2D,
 549992            normalAngularVelocity2D,
 549993            ref exactImpulse,
 549994            out ContactNormalImpulseResultMixed normalResult,
 549995            featureNamespace2D: authoritativeNormal ? pair.Manifold.PrimaryContact.FeatureNamespaceB : 0,
 549996            detected: authoritativeNormal);
 549997        if (!normalResolved
 549998            || !CanApplyNormalVelocityDeltas(body3D, body2D, normalResult))
 99        {
 3100            GravitasLogger.Channel.Write(
 3101                DiagnosticLevel.Error,
 3102                "Mixed contact response is outside the representable velocity domain.",
 3103                nameof(CollisionResponseMixed));
 3104            return false;
 105        }
 106
 5496107        if (applyPositionCorrection && correctionInverseMass > Fixed64.Zero)
 5421108            ApplyPositionCorrection(body3D, body2D, normal, contact.Depth);
 109
 5494110        bool appliedImpulse = HasVelocityDelta(normalResult);
 5494111        if (appliedImpulse)
 112        {
 4368113            if (normalResult.HasRepresentableAppliedImpulse
 4368114                && normalResult.HasRepresentableNormalVelocity)
 115            {
 4363116                pair.Context.Diagnostics.EmitMixedResponseImpulse(
 4363117                    pair,
 4363118                    contact,
 4363119                    normal * normalResult.AppliedImpulseScalar,
 4363120                    normalResult.NormalVelocity,
 4363121                    iteration,
 4363122                    iterationLimit);
 123            }
 4368124            ApplyNormalVelocityDeltas(body3D, body2D, normalResult);
 125        }
 5494126        PhysicsMaterial material3D = contact.HasMaterialOverride
 5494127            ? contact.Material3D
 5494128            : pair.Collider3D.Material;
 5494129        PhysicsMaterial material2D = contact.HasMaterialOverride
 5494130            ? contact.Material2D
 5494131            : pair.Collider2D.Material;
 5494132        bool frictionResolved = TryApplyFrictionImpulse(
 5494133            pair,
 5494134            contact,
 5494135            body3D,
 5494136            body2D,
 5494137            normal,
 5494138            relative3D,
 5494139            relative2D,
 5494140            material3D,
 5494141            material2D,
 5494142            normalResult,
 5494143            normalLinearVelocity3D,
 5494144            normalAngularVelocity3D,
 5494145            normalLinearVelocity2D,
 5494146            normalAngularVelocity2D,
 5494147            pair.Context.Settings.RestitutionVelocityThreshold,
 5494148            hasPlanarResponseCoupling,
 5494149            resolved3D,
 5494150            resolved2D,
 5494151            out bool frictionApplied);
 5494152        if (!frictionResolved)
 153        {
 3154            GravitasLogger.Channel.Write(
 3155                DiagnosticLevel.Error,
 3156                "Mixed contact friction is outside the representable velocity domain.",
 3157                nameof(CollisionResponseMixed));
 158        }
 159
 5494160        return appliedImpulse | frictionApplied;
 161    }
 162
 163    private static void ApplyPositionCorrection(
 164        SolidBody? body3D,
 165        SolidBody2D? body2D,
 166        Vector3d normal,
 167        Fixed64 depth)
 168    {
 13269169        Fixed64 correctionDepth = depth - PenetrationSlop;
 13269170        if (correctionDepth <= Fixed64.Zero)
 824171            return;
 172
 12445173        Fixed64 weightedDepth = correctionDepth * PenetrationCorrectionPercent;
 12445174        Fixed64 constrainedInverseMass3D = GetConstrainedInverseMass(body3D, normal);
 12445175        Fixed64 constrainedInverseMass2D = GetConstrainedPlanarInverseMass(body2D, normal);
 12445176        Vector2d planarNormal = normal.ToVector2d();
 177        Vector3d correction3D;
 178        Vector2d correction2D;
 12445179        if (CollisionPositionCorrection.TryGetCompactScale(
 12445180                weightedDepth, constrainedInverseMass3D, constrainedInverseMass2D, out Fixed64 scale))
 181        {
 12408182            correction3D = -normal * (scale * constrainedInverseMass3D);
 12408183            correction2D = planarNormal * (scale * constrainedInverseMass2D);
 184        }
 185        else
 186        {
 37187            correction3D = -normal * CollisionPositionCorrection.GetWeightedDistance(
 37188                weightedDepth, constrainedInverseMass3D, constrainedInverseMass3D, constrainedInverseMass2D);
 37189            correction2D = planarNormal * CollisionPositionCorrection.GetWeightedDistance(
 37190                weightedDepth, constrainedInverseMass2D, constrainedInverseMass3D, constrainedInverseMass2D);
 191        }
 192        // Prepare both endpoints before publishing either participant's correction.
 12445193        Vector3d position3D = body3D?.PrepareCollisionPositionCorrection(correction3D) ?? default;
 12445194        Vector2d position2D = body2D?.PrepareCollisionPositionCorrection(correction2D) ?? default;
 12443195        body3D?.ApplyCollisionPositionState(position3D);
 12443196        body2D?.ApplyCollisionPositionState(position2D);
 12443197    }
 198
 199    private static bool TryGetNormalImpulse(
 200        LSCollider collider3D,
 201        LSCollider2D collider2D,
 202        MixedContact contact,
 203        SolidBody? body3D,
 204        SolidBody2D? body2D,
 205        Vector3d normal,
 206        Vector3d relative3D,
 207        Vector2d relative2D,
 208        bool hasCompact3D,
 209        bool hasCompact2D,
 210        Vector3d linearVelocity3D,
 211        Vector3d angularVelocity3D,
 212        Vector2d linearVelocity2D,
 213        Fixed64 angularVelocity2D,
 214        ref ExactContactImpulseState? exactImpulse,
 215        out ContactNormalImpulseResultMixed result,
 216        Fixed64 accumulatedImpulse = default,
 217        in ContactResponseSnapshot impact = default,
 218        int featureNamespace2D = 0,
 219        bool detected = false)
 220    {
 184659221        ContactAnchor normalAnchor2D = contact.Anchor2D;
 184659222        ContactAnchor center2D = default;
 184659223        if (detected)
 224        {
 184579225            center2D = impact.IsValid ? impact.CenterB : body2D == null
 184579226                ? ContactAnchor.FromWorldPoint(MixedEmbedded2DGeometry.GetCenter3D(collider2D))
 184579227                : new ContactAnchor(new Vector3d(body2D.Position.X, collider2D.MixedSlabCenterY, body2D.Position.Y),
 184579228                    FixedQuaternion.FromAxisAngle(Vector3d.Up, -body2D.Rotation),
 184579229                    ExactContactLever2D.ToSpatial(body2D.LocalCenterOfMassOffset));
 184579230            normalAnchor2D = ExactContactLever2D.GetDetectedNormalAnchor(collider2D,
 184579231                featureNamespace2D, center2D, contact.Anchor2D);
 184579232            hasCompact2D = normalAnchor2D.TryGetOffsetFrom(center2D, out Vector3d normalLever2D);
 184579233            relative2D = normalLever2D.ToVector2d();
 234        }
 184659235        PhysicsMaterial material3D = contact.HasMaterialOverride
 184659236            ? contact.Material3D
 184659237            : collider3D.Material;
 184659238        PhysicsMaterial material2D = contact.HasMaterialOverride
 184659239            ? contact.Material2D
 184659240            : collider2D.Material;
 184659241        Fixed64 restitution =
 184659242            PhysicsMaterial.CombineRestitution(material3D, material2D);
 184659243        Fixed64 restitutionVelocityThreshold =
 184659244            collider3D.Context.Settings.RestitutionVelocityThreshold;
 184659245        if (hasCompact3D
 184659246            && hasCompact2D
 184659247            && exactImpulse?.HasNormal != true
 184659248            && (!impact.IsValid || restitution == Fixed64.Zero)
 184659249            && ContactNormalImpulseMixed.CanUseCompactResponse(
 184659250                body3D,
 184659251                linearVelocity3D,
 184659252                angularVelocity3D,
 184659253                relative3D,
 184659254                body2D,
 184659255                linearVelocity2D,
 184659256                angularVelocity2D,
 184659257                relative2D,
 184659258                normal)
 184659259            && ContactNormalImpulseMixed.TryCalculateAccumulatedDelta(
 184659260                body3D,
 184659261                linearVelocity3D,
 184659262                angularVelocity3D,
 184659263                relative3D,
 184659264                body2D,
 184659265                linearVelocity2D,
 184659266                angularVelocity2D,
 184659267                relative2D,
 184659268                normal,
 184659269                restitution,
 184659270                restitutionVelocityThreshold,
 184659271                accumulatedImpulse,
 184659272                Fixed64.One,
 184659273                Fixed64.One,
 184659274                out result))
 275        {
 181418276            return true;
 277        }
 278
 279        ExactLever3D exact3D, exact2D;
 3241280        if (impact.IsValid)
 281        {
 2991282            exact3D = contact.Anchor3D.GetLeverFrom(impact.CenterA);
 2991283            exact2D = normalAnchor2D.GetLeverFrom(impact.CenterB);
 284        }
 285        else
 286        {
 250287            exact3D = contact.Anchor3D.GetLeverFrom(body3D?.GetCenterOfMassAnchor()
 250288                ?? ContactAnchor.FromWorldPoint(collider3D.Center));
 250289            exact2D = detected ? normalAnchor2D.GetLeverFrom(center2D) : body2D == null
 250290                ? contact.GetPlanarXZLeverFrom(collider2D.Center, Fixed64.Zero, Vector2d.Zero)
 250291                : contact.GetPlanarXZLeverFrom(body2D.Position, body2D.Rotation, body2D.LocalCenterOfMassOffset);
 292        }
 3241293        return ContactNormalImpulseMixed.TryCalculateAccumulatedDeltaExact(
 3241294            body3D,
 3241295            linearVelocity3D,
 3241296            angularVelocity3D,
 3241297            exact3D,
 3241298            body2D,
 3241299            linearVelocity2D,
 3241300            angularVelocity2D,
 3241301            exact2D,
 3241302            normal,
 3241303            restitution,
 3241304            restitutionVelocityThreshold,
 3241305            accumulatedImpulse,
 3241306            Fixed64.One,
 3241307            Fixed64.One,
 3241308            ref exactImpulse, out result, impact);
 309    }
 310
 311    private static bool CanApplyNormalVelocityDeltas(
 312        SolidBody? body3D,
 313        SolidBody2D? body2D,
 314        ContactNormalImpulseResultMixed result) =>
 178445315        CanApplyVelocityDeltas(
 178445316            body3D,
 178445317            body2D,
 178445318            result.LinearVelocityDelta3D,
 178445319            result.AngularVelocityDelta3D,
 178445320            result.LinearVelocityDelta2D,
 178445321            result.AngularVelocityDelta2D);
 322
 323    private static bool HasVelocityDelta(
 324        ContactNormalImpulseResultMixed result) =>
 184636325        result.LinearVelocityDelta3D != Vector3d.Zero
 184636326        || result.AngularVelocityDelta3D != Vector3d.Zero
 184636327        || result.LinearVelocityDelta2D != Vector2d.Zero
 184636328        || result.AngularVelocityDelta2D != Fixed64.Zero;
 329
 330    private static void ApplyNormalVelocityDeltas(
 331        SolidBody? body3D,
 332        SolidBody2D? body2D,
 333        ContactNormalImpulseResultMixed result) =>
 177316334        ApplyVelocityDeltas(
 177316335            body3D,
 177316336            body2D,
 177316337            result.LinearVelocityDelta3D,
 177316338            result.AngularVelocityDelta3D,
 177316339            result.LinearVelocityDelta2D,
 177316340            result.AngularVelocityDelta2D);
 341
 342    private static bool CanApplyVelocityDeltas(
 343        SolidBody? body3D,
 344        SolidBody2D? body2D,
 345        Vector3d linear3D,
 346        Vector3d angular3D,
 347        Vector2d linear2D,
 348        Fixed64 angular2D)
 349    {
 178491350        bool response3DFits = body3D?.CanApplyCollisionVelocityDeltas(
 178491351                linear3D,
 178491352                angular3D)
 178491353            ?? true;
 178491354        bool response2DFits = body2D?.CanApplyCollisionVelocityDeltas(
 178491355                linear2D,
 178491356                angular2D)
 178491357            ?? true;
 178491358        return response3DFits & response2DFits;
 359    }
 360
 361    private static void ApplyVelocityDeltas(
 362        SolidBody? body3D,
 363        SolidBody2D? body2D,
 364        Vector3d linear3D,
 365        Vector3d angular3D,
 366        Vector2d linear2D,
 367        Fixed64 angular2D)
 368    {
 350305369        if (body3D != null)
 370        {
 207755371            body3D.ApplyCollisionLinearVelocityDelta(linear3D);
 207755372            body3D.ApplyCollisionAngularVelocityDelta(angular3D);
 373        }
 374
 350305375        if (body2D != null)
 376        {
 107174377            body2D.ApplyCollisionLinearVelocityDelta(linear2D);
 107174378            body2D.ApplyCollisionAngularVelocityDelta(angular2D);
 379        }
 350305380    }
 381
 382    private static bool TryApplyFrictionImpulse(
 383        CollisionPairMixed pair,
 384        MixedContact contact,
 385        SolidBody? body3D,
 386        SolidBody2D? body2D,
 387        Vector3d normal,
 388        Vector3d relative3D,
 389        Vector2d relative2D,
 390        PhysicsMaterial material3D,
 391        PhysicsMaterial material2D,
 392        ContactNormalImpulseResultMixed normalResult,
 393        Vector3d normalLinearVelocity3D,
 394        Vector3d normalAngularVelocity3D,
 395        Vector2d normalLinearVelocity2D,
 396        Fixed64 normalAngularVelocity2D,
 397        Fixed64 restitutionVelocityThreshold,
 398        bool applyTo2D,
 399        bool hasCompact3D,
 400        bool hasCompact2D,
 401        out bool applied)
 402    {
 5494403        applied = false;
 5494404        Fixed64 normalImpulse = normalResult.HasRepresentableAppliedImpulse
 5494405            ? FixedMath.Max(
 5494406                Fixed64.Zero,
 5494407                normalResult.AppliedImpulseScalar)
 5494408            : Fixed64.Zero;
 5494409        if (normalResult.HasRepresentableAppliedImpulse
 5494410            && normalImpulse <= Fixed64.Zero)
 1127411            return true;
 412
 4367413        PhysicsMaterial.CombineFriction(material3D, material2D, out Fixed64 staticFriction, out Fixed64 dynamicFriction)
 4367414        Fixed64 restitution =
 4367415            PhysicsMaterial.CombineRestitution(material3D, material2D);
 4367416        if (staticFriction <= Fixed64.Zero && dynamicFriction <= Fixed64.Zero)
 4280417            return true;
 418
 87419        if (normalResult.HasRepresentableAppliedImpulse
 87420            && hasCompact3D
 87421            && hasCompact2D
 87422            && TryGetCompactFrictionImpulse(
 87423                body3D,
 87424                body2D,
 87425                normal,
 87426                relative3D,
 87427                relative2D,
 87428                normalImpulse,
 87429                staticFriction,
 87430                dynamicFriction,
 87431                applyTo2D,
 87432                out Vector3d tangent,
 87433                out Fixed64 impulseScalar)
 87434            && (impulseScalar == Fixed64.Zero
 87435                || TryApplyImpulse(
 87436                    body3D,
 87437                    body2D,
 87438                    tangent,
 87439                    relative3D,
 87440                    relative2D,
 87441                    impulseScalar,
 87442                    applyTo2D)))
 443        {
 58444            applied = impulseScalar != Fixed64.Zero;
 58445            return true;
 446        }
 447
 29448        return TryApplyFrictionImpulseExact(
 29449            pair,
 29450            contact,
 29451            body3D,
 29452            body2D,
 29453            normal,
 29454            normalLinearVelocity3D,
 29455            normalAngularVelocity3D,
 29456            normalLinearVelocity2D,
 29457            normalAngularVelocity2D,
 29458            restitutionVelocityThreshold,
 29459            restitution,
 29460            staticFriction,
 29461            dynamicFriction,
 29462            applyTo2D,
 29463            out applied);
 464    }
 465
 466    private static bool TryGetCompactFrictionImpulse(
 467        SolidBody? body3D,
 468        SolidBody2D? body2D,
 469        Vector3d normal,
 470        Vector3d relative3D,
 471        Vector2d relative2D,
 472        Fixed64 normalImpulse,
 473        Fixed64 staticFriction,
 474        Fixed64 dynamicFriction,
 475        bool applyTo2D,
 476        out Vector3d tangent,
 477        out Fixed64 impulseScalar)
 478    {
 68479        tangent = default;
 68480        impulseScalar = Fixed64.Zero;
 68481        Vector3d linearVelocity3D = body3D == null
 68482            ? Vector3d.Zero
 68483            : ResolveLinearVelocity(body3D);
 68484        Vector3d angularVelocity3D = body3D == null
 68485            ? Vector3d.Zero
 68486            : ResolveAngularVelocity(body3D);
 68487        Vector3d linearVelocity2D = body2D == null
 68488            ? Vector3d.Zero
 68489            : ExactContactLever2D.ToSpatial(
 68490                ResolveLinearVelocity(body2D));
 68491        Vector3d angularVelocity2D = body2D == null
 68492            ? Vector3d.Zero
 68493            : new Vector3d(
 68494                Fixed64.Zero,
 68495                -ResolveAngularVelocity(body2D),
 68496                Fixed64.Zero);
 68497        if (!ContactResponseArithmetic3D.TryGetRelativePointVelocity(
 68498                linearVelocity3D,
 68499                angularVelocity3D,
 68500                relative3D,
 68501                linearVelocity2D,
 68502                angularVelocity2D,
 68503                ExactContactLever2D.ToSpatial(relative2D),
 68504                normal,
 68505                out Vector3d relativeVelocity)
 68506            || !TryGetCompactTangent(
 68507                relativeVelocity,
 68508                normal,
 68509                out tangent,
 68510                out bool hasTangentialMotion))
 511        {
 2512            return false;
 513        }
 66514        if (!hasTangentialMotion)
 37515            return true;
 516
 29517        bool denominatorResolved =
 29518            TryGetCompactConstrainedInverseMass(
 29519                body3D,
 29520                tangent,
 29521                out Fixed64 linear3D);
 29522        denominatorResolved &=
 29523            ContactNormalImpulse3D.TryComputeAngularDenominator(
 29524                body3D,
 29525                relative3D,
 29526                tangent,
 29527                out Fixed64 angular3D);
 29528        Fixed64 linear2D = Fixed64.Zero;
 29529        Fixed64 angular2D = Fixed64.Zero;
 29530        if (applyTo2D)
 531        {
 23532            denominatorResolved &=
 23533                TryGetCompactConstrainedPlanarInverseMass(
 23534                    body2D,
 23535                    tangent,
 23536                    out linear2D);
 23537            denominatorResolved &=
 23538                ContactNormalImpulse2D.TryComputeAngularDenominator(
 23539                    body2D,
 23540                    relative2D,
 23541                    tangent.ToVector2d(),
 23542                    out angular2D);
 543        }
 544
 29545        var denominatorTerms = new ContactEffectiveMassTerms3D(
 29546            linear3D,
 29547            linear2D,
 29548            angular3D,
 29549            angular2D);
 29550        if (!denominatorResolved
 29551            || !denominatorTerms.TryGetValue(out Fixed64 denominator))
 552        {
 3553            return false;
 554        }
 26555        if (denominator <= Fixed64.Zero)
 1556            return true;
 557
 25558        if (!ContactResponseArithmetic3D.TryDot(
 25559                relativeVelocity,
 25560                tangent,
 25561                out Fixed64 tangentVelocity)
 25562            || !Fixed64.TryMultiplyDivide(
 25563                -tangentVelocity,
 25564                Fixed64.One,
 25565                denominator,
 25566                out impulseScalar)
 25567            || !Fixed64.TryMultiplyDivide(
 25568                normalImpulse,
 25569                staticFriction,
 25570                Fixed64.One,
 25571                out Fixed64 staticLimit)
 25572            || !Fixed64.TryMultiplyDivide(
 25573                normalImpulse,
 25574                dynamicFriction,
 25575                Fixed64.One,
 25576                out Fixed64 dynamicLimit))
 577        {
 3578            return false;
 579        }
 580
 22581        if (impulseScalar < -staticLimit
 22582            || impulseScalar > staticLimit)
 583        {
 10584            impulseScalar = FixedMath.Clamp(
 10585                impulseScalar,
 10586                -dynamicLimit,
 10587                dynamicLimit);
 588        }
 22589        return true;
 590    }
 591
 592    [MethodImpl(MethodImplOptions.NoInlining)]
 593    private static bool TryApplyFrictionImpulseExact(
 594        CollisionPairMixed pair,
 595        MixedContact contact,
 596        SolidBody? body3D,
 597        SolidBody2D? body2D,
 598        Vector3d normal,
 599        Vector3d normalLinearVelocity3D,
 600        Vector3d normalAngularVelocity3D,
 601        Vector2d normalLinearVelocity2D,
 602        Fixed64 normalAngularVelocity2D,
 603        Fixed64 restitutionVelocityThreshold,
 604        Fixed64 restitution,
 605        Fixed64 staticFriction,
 606        Fixed64 dynamicFriction,
 607        bool applyTo2D,
 608        out bool applied)
 609    {
 29610        applied = false;
 29611        GetExactLevers(
 29612            pair,
 29613            contact,
 29614            body3D,
 29615            body2D,
 29616            out ExactLever3D exact3D,
 29617            out ExactLever3D exact2D);
 29618        Vector3d tangent = SolverContact3D.CreateTangent(normal);
 29619        Vector3d secondaryTangent =
 29620            Vector3d.Cross(normal, tangent).Normalized;
 29621        SolidBody2D? responseBody2D =
 29622            applyTo2D ? body2D : null;
 29623        Vector2d responseLinearVelocity2D =
 29624            responseBody2D == null
 29625                ? Vector2d.Zero
 29626                : normalLinearVelocity2D;
 29627        Fixed64 responseAngularVelocity2D =
 29628            responseBody2D == null
 29629                ? Fixed64.Zero
 29630                : normalAngularVelocity2D;
 29631        var normalConstraint = new ExactNormalConstraint3D(
 29632            ExactContactLever3D.CreateResponseOperand(
 29633                body3D,
 29634                normalLinearVelocity3D,
 29635                normalAngularVelocity3D,
 29636                exact3D,
 29637                -normal),
 29638            ExactContactLever2D.CreateResponseOperand(
 29639                responseBody2D,
 29640                responseLinearVelocity2D,
 29641                responseAngularVelocity2D,
 29642                exact2D,
 29643                normal),
 29644            normal,
 29645            restitution,
 29646            restitutionVelocityThreshold,
 29647            Fixed64.Zero,
 29648            Fixed64.One,
 29649            Fixed64.One);
 29650        Vector3d linearVelocity3D =
 29651            body3D == null
 29652                ? Vector3d.Zero
 29653                : ResolveLinearVelocity(body3D);
 29654        Vector3d angularVelocity3D =
 29655            body3D == null
 29656                ? Vector3d.Zero
 29657                : ResolveAngularVelocity(body3D);
 29658        Vector2d linearVelocity2D =
 29659            responseBody2D == null
 29660                ? Vector2d.Zero
 29661                : ResolveLinearVelocity(responseBody2D);
 29662        Fixed64 angularVelocity2D =
 29663            responseBody2D == null
 29664                ? Fixed64.Zero
 29665                : ResolveAngularVelocity(responseBody2D);
 29666        if (!ExactContactResponseKernel.TryGetCoulombDiskResponse(
 29667                normalConstraint,
 29668                ExactContactLever3D.CreateResponseOperand(
 29669                    body3D,
 29670                    linearVelocity3D,
 29671                    angularVelocity3D,
 29672                    exact3D,
 29673                    -tangent),
 29674                ExactContactLever2D.CreateResponseOperand(
 29675                    responseBody2D,
 29676                    linearVelocity2D,
 29677                    angularVelocity2D,
 29678                    exact2D,
 29679                    tangent),
 29680                tangent,
 29681                Fixed64.Zero,
 29682                ExactContactLever3D.CreateResponseOperand(
 29683                    body3D,
 29684                    linearVelocity3D,
 29685                    angularVelocity3D,
 29686                    exact3D,
 29687                    -secondaryTangent),
 29688                ExactContactLever2D.CreateResponseOperand(
 29689                    responseBody2D,
 29690                    linearVelocity2D,
 29691                    angularVelocity2D,
 29692                    exact2D,
 29693                    secondaryTangent),
 29694                secondaryTangent,
 29695                Fixed64.Zero,
 29696                staticFriction,
 29697                dynamicFriction,
 29698                out ExactCoulombResponse3D response))
 699        {
 2700            return false;
 701        }
 702
 27703        Vector3d linear3D =
 27704            response.FirstLinearVelocityDelta;
 27705        Vector3d angular3D =
 27706            response.FirstAngularVelocityDelta;
 27707        Vector2d linear2D = ExactContactLever2D.ToPlanar(
 27708            response.SecondLinearVelocityDelta);
 27709        Fixed64 angular2D = ExactContactLever2D.ToPlanarAngular(
 27710            response.SecondAngularVelocityDelta);
 27711        if (!CanApplyVelocityDeltas(
 27712                body3D,
 27713                body2D,
 27714                linear3D,
 27715                angular3D,
 27716                linear2D,
 27717                angular2D))
 718        {
 1719            return false;
 720        }
 721
 26722        if (!response.HasAppliedImpulse)
 1723            return true;
 724
 25725        ApplyVelocityDeltas(
 25726            body3D,
 25727            body2D,
 25728            linear3D,
 25729            angular3D,
 25730            linear2D,
 25731            angular2D);
 25732        applied = true;
 25733        return true;
 734    }
 735
 736    private static bool TryApplyImpulse(
 737        SolidBody? body3D,
 738        SolidBody2D? body2D,
 739        Vector3d axis,
 740        Vector3d relative3D,
 741        Vector2d relative2D,
 742        Fixed64 impulseScalar,
 743        bool applyTo2D)
 744    {
 21745        Vector3d linear3D = Vector3d.Zero;
 21746        Vector3d angular3D = Vector3d.Zero;
 21747        Vector2d linear2D = Vector2d.Zero;
 21748        Fixed64 angular2D = Fixed64.Zero;
 21749        Vector3d impulse3D = -axis * impulseScalar;
 21750        bool impulse3DResolved =
 21751            (impulse3D.X != Fixed64.Zero
 21752                || axis.X == Fixed64.Zero)
 21753            && (impulse3D.Y != Fixed64.Zero
 21754                || axis.Y == Fixed64.Zero)
 21755            && (impulse3D.Z != Fixed64.Zero
 21756                || axis.Z == Fixed64.Zero);
 21757        bool linear3DResolved = impulse3DResolved
 21758            && ContactNormalImpulse3D.TryComputeLinearVelocityDelta(
 21759                body3D,
 21760                impulse3D,
 21761                out linear3D);
 21762        bool angular3DResolved = impulse3DResolved
 21763            && ContactNormalImpulse3D.TryComputeAngularVelocityDelta(
 21764                body3D,
 21765                relative3D,
 21766                impulse3D,
 21767                out angular3D);
 21768        Vector2d planarAxis = axis.ToVector2d();
 21769        bool linear2DResolved = !applyTo2D
 21770            || ContactNormalImpulse2D.TryComputeLinearVelocityDelta(
 21771                body2D,
 21772                planarAxis,
 21773                impulseScalar,
 21774                out linear2D);
 21775        bool angular2DResolved = !applyTo2D
 21776            || ContactNormalImpulse2D.TryComputeAngularVelocityDelta(
 21777                body2D,
 21778                relative2D,
 21779                planarAxis,
 21780                impulseScalar,
 21781                out angular2D);
 21782        if (!(linear3DResolved
 21783            & angular3DResolved
 21784            & linear2DResolved
 21785            & angular2DResolved)
 21786            || !CanApplyVelocityDeltas(
 21787                body3D,
 21788                body2D,
 21789                linear3D,
 21790                angular3D,
 21791                linear2D,
 21792                angular2D))
 793        {
 2794            return false;
 795        }
 796
 19797        ApplyVelocityDeltas(
 19798            body3D,
 19799            body2D,
 19800            linear3D,
 19801            angular3D,
 19802            linear2D,
 19803            angular2D);
 19804        return true;
 805    }
 806
 807    private static bool TryGetCompactTangent(
 808        Vector3d relativeVelocity,
 809        Vector3d normal,
 810        out Vector3d tangent,
 811        out bool hasTangentialMotion)
 812    {
 67813        tangent = default;
 67814        hasTangentialMotion = false;
 67815        if (!ContactResponseArithmetic3D.TryDot(
 67816                relativeVelocity,
 67817                normal,
 67818                out Fixed64 normalVelocity)
 67819            || !ContactResponseArithmetic3D.TryLinearCombination(
 67820                relativeVelocity,
 67821                Fixed64.One,
 67822                normal,
 67823                -normalVelocity,
 67824                Vector3d.Zero,
 67825                Fixed64.Zero,
 67826                out Vector3d tangentVelocity))
 827        {
 1828            return false;
 829        }
 66830        if (tangentVelocity == Vector3d.Zero)
 36831            return true;
 832
 30833        Fixed64 magnitudeSquared =
 30834            tangentVelocity.MagnitudeSquared;
 30835        if (magnitudeSquared <= Fixed64.Epsilon)
 1836            return true;
 837
 29838        tangent = tangentVelocity.Normalized;
 29839        hasTangentialMotion = tangent.IsNormalized();
 29840        return hasTangentialMotion;
 841    }
 842
 843    private static bool TryGetCompactConstrainedInverseMass(
 844        SolidBody? body,
 845        Vector3d axis,
 846        out Fixed64 inverseMass)
 847    {
 29848        if (body == null)
 849        {
 3850            inverseMass = Fixed64.Zero;
 3851            return true;
 852        }
 853
 26854        inverseMass = body.GetConstrainedInverseMass(axis);
 26855        return inverseMass != Fixed64.Zero
 26856            || body.EffectiveInverseMass == Fixed64.Zero
 26857            || body.ProjectLinearMotion(axis) == Vector3d.Zero;
 858    }
 859
 860    private static bool TryGetCompactConstrainedPlanarInverseMass(
 861        SolidBody2D? body,
 862        Vector3d axis,
 863        out Fixed64 inverseMass)
 864    {
 23865        inverseMass = Fixed64.Zero;
 23866        if (body == null)
 1867            return true;
 868
 22869        Vector2d planarAxis = axis.ToVector2d();
 22870        if (!ContactNormalImpulse2D.TryGetConstrainedInverseMass(
 22871                body,
 22872                planarAxis,
 22873                out Fixed64 constrained))
 1874            return false;
 21875        if (constrained == Fixed64.Zero)
 9876            return true;
 877
 12878        Fixed64 magnitudeSquared =
 12879            planarAxis.MagnitudeSquared;
 12880        if (!Fixed64.TryMultiplyDivide(
 12881                constrained,
 12882                magnitudeSquared,
 12883                Fixed64.One,
 12884                out inverseMass))
 885        {
 1886            inverseMass = default;
 1887            return false;
 888        }
 889
 11890        return inverseMass != Fixed64.Zero;
 891    }
 892
 893    [MethodImpl(MethodImplOptions.AggressiveInlining)]
 894    private static Vector3d ResolveLinearVelocity(SolidBody body) =>
 112406895        body.ProjectLinearMotion(
 112406896            body.IsKinematic
 112406897                ? body.SampleContinuousCollisionLinearVelocity(Fixed64.One)
 112406898                : body.LinearVelocity);
 899
 900    [MethodImpl(MethodImplOptions.AggressiveInlining)]
 901    private static Vector2d ResolveLinearVelocity(SolidBody2D body) =>
 76387902        body.ProjectLinearMotion(
 76387903            body.IsKinematic
 76387904                ? body.SampleContinuousCollisionLinearVelocity(Fixed64.One)
 76387905                : body.LinearVelocity);
 906
 907    [MethodImpl(MethodImplOptions.AggressiveInlining)]
 908    private static Vector3d ResolveAngularVelocity(SolidBody body) =>
 112406909        body.ProjectAngularMotion(
 112406910            body.IsKinematic
 112406911                ? body.SampleContinuousCollisionAngularVelocity(Fixed64.One)
 112406912                : body.AngularVelocity);
 913
 914    [MethodImpl(MethodImplOptions.AggressiveInlining)]
 915    private static Fixed64 ResolveAngularVelocity(SolidBody2D body) =>
 151693916        body.IsKinematic
 151693917            ? body.SampleContinuousCollisionAngularVelocity(Fixed64.One)
 151693918            : body.AngularVelocity;
 919
 920    private static Vector3d ResolveNormal(CollisionPairMixed pair, MixedContact contact)
 921    {
 80922        Vector3d fallback = ResolveFallbackDirection(pair, contact);
 80923        if (contact.Normal3DTo2D.MagnitudeSquared > Fixed64.Epsilon)
 924        {
 78925            Vector3d normal = contact.Normal3DTo2D.Normalized;
 78926            return fallback.MagnitudeSquared > Fixed64.Epsilon && Vector3d.Dot(normal, fallback) < Fixed64.Zero
 78927                ? -normal
 78928                : normal;
 929        }
 930
 2931        return fallback == Vector3d.Zero ? Vector3d.Up : fallback;
 932    }
 933
 934    private static Vector3d ResolveFallbackDirection(CollisionPairMixed pair, MixedContact contact)
 935    {
 80936        LSCollider2D collider2D = pair.Collider2D;
 80937        Vector3d embeddedCenter = new(
 80938            collider2D.Center.X,
 80939            collider2D.MixedSlabCenterY,
 80940            collider2D.Center.Y);
 80941        Vector3d centerDirection = embeddedCenter - pair.Collider3D.Center;
 80942        if (centerDirection.MagnitudeSquared > Fixed64.Epsilon)
 37943            return centerDirection.Normalized;
 944
 43945        if (contact.TryGetPoint2D(out Vector3d point2D)
 43946            && contact.TryGetPoint3D(out Vector3d point3D))
 947        {
 25948            Vector3d pointDirection = point2D - point3D;
 25949            if (pointDirection.MagnitudeSquared > Fixed64.Epsilon)
 3950                return pointDirection.Normalized;
 951        }
 952
 40953        return Vector3d.Zero;
 954    }
 955
 956    private static void GetExactLevers(
 957        CollisionPairMixed pair,
 958        MixedContact contact,
 959        SolidBody? body3D,
 960        SolidBody2D? body2D,
 961        out ExactLever3D exact3D,
 962        out ExactLever3D exact2D)
 963    {
 29964        ContactAnchor center3D = body3D?.GetCenterOfMassAnchor()
 29965            ?? ContactAnchor.FromWorldPoint(pair.Collider3D.Center);
 29966        exact3D = contact.Anchor3D.GetLeverFrom(center3D);
 29967        exact2D = body2D == null
 29968            ? contact.GetPlanarXZLeverFrom(
 29969                pair.Collider2D.Center,
 29970                Fixed64.Zero,
 29971                Vector2d.Zero)
 29972            : contact.GetPlanarXZLeverFrom(
 29973                body2D.Position,
 29974                body2D.Rotation,
 29975                body2D.LocalCenterOfMassOffset);
 29976    }
 977
 978    [MethodImpl(MethodImplOptions.AggressiveInlining)]
 979    private static Fixed64 GetConstrainedInverseMass(SolidBody? body, Vector3d axis) =>
 27324980        body?.GetConstrainedInverseMass(axis) ?? Fixed64.Zero;
 981
 982    [MethodImpl(MethodImplOptions.AggressiveInlining)]
 983    private static Fixed64 GetConstrainedPlanarInverseMass(SolidBody2D? body, Vector3d axis)
 984    {
 27324985        if (body == null)
 20079986            return Fixed64.Zero;
 987
 7245988        Vector2d planarAxis = axis.ToVector2d();
 7245989        if (planarAxis == Vector2d.Zero)
 1706990            return Fixed64.Zero;
 991
 5539992        return body.GetConstrainedInverseMass(planarAxis) * planarAxis.MagnitudeSquared;
 993    }
 994
 995}

/home/runner/work/Gravitas/Gravitas/src/Gravitas/CollisionHandling/Response/Mixed/CollisionResponseMixed.Manifold.cs

#LineLine coverage
 1//=======================================================================
 2// CollisionResponseMixed.Manifold.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 Gravitas.Colliders;
 11using Gravitas.Materials;
 12
 13namespace Gravitas.CollisionHandling;
 14
 15/// <content>Solves retained mixed groups with independent accumulated impulse rows.</content>
 16public static partial class CollisionResponseMixed
 17{
 18    internal static void ResolveManifold(CollisionPairMixed pair)
 19    {
 1231520        if (pair.Manifold.Count == 1)
 21        {
 538622            Resolve(pair, pair.Contact, 0, 1, true, authoritativeNormal: true);
 538623            return;
 24        }
 692925        PrepareSolve(pair);
 692926        int limit = pair.Manifold.Count > 1 ? pair.Context.Settings.DiscreteSolverIterations : 1;
 9694027        for (int iteration = 0; iteration < limit; iteration++)
 4154128            ResolveManifold(pair, iteration, limit, iteration == 0);
 692929        pair.ResponseSnapshot = default;
 692930    }
 31
 32    internal static void PrepareSolve(CollisionPairMixed pair)
 33    {
 925134        pair.SolveImpulses.Clear();
 925135        ClearExactImpulses(pair.ExactSolveImpulses);
 925136        SolidBody? body3D = pair.Collider3D.Body;
 925137        SolidBody2D? body2D = pair.Collider2D.Body;
 925138        Vector3d position3D = body3D?.Position3d ?? pair.Collider3D.Center;
 925139        Vector3d position2D = body2D?.Agent.Transform.WorldPosition ?? MixedEmbedded2DGeometry.GetCenter3D(pair.Collider
 925140        ContactAnchor center2D = body2D == null
 925141            ? ContactAnchor.FromWorldPoint(position2D)
 925142            : new ContactAnchor(new Vector3d(body2D.Position.X, position2D.Y, body2D.Position.Y),
 925143                FixedQuaternion.FromAxisAngle(Vector3d.Up, -body2D.Rotation),
 925144                ExactContactLever2D.ToSpatial(body2D.LocalCenterOfMassOffset));
 45        // Every row keeps the incoming motion and lever frame, even after another row corrects position.
 925146        pair.ResponseSnapshot = new ContactResponseSnapshot(position3D, position2D,
 925147            body3D?.GetCenterOfMassAnchor() ?? ContactAnchor.FromWorldPoint(position3D), center2D,
 925148            body3D == null ? Vector3d.Zero : ResolveLinearVelocity(body3D),
 925149            body3D == null ? Vector3d.Zero : ResolveAngularVelocity(body3D),
 925150            body2D == null ? Vector3d.Zero : ExactContactLever2D.ToSpatial(ResolveLinearVelocity(body2D)),
 925151            new Vector3d(Fixed64.Zero, body2D == null ? Fixed64.Zero : -ResolveAngularVelocity(body2D), Fixed64.Zero),
 925152            body2D == null ? Fixed64.Zero : ResolveAngularVelocity(body2D));
 925153    }
 54
 55    internal static bool HasPlanarResponseCoupling(ContactManifold manifold)
 56    {
 5540557        foreach (ManifoldContact row in manifold)
 3220858            if (row.Normal.ToVector2d() != Vector2d.Zero) return true;
 188559        return false;
 1278160    }
 61
 62    internal static void ResolveManifold(CollisionPairMixed pair, int iteration, int limit, bool correctPosition)
 63    {
 5543964        if (!pair.Manifold.HasContact || pair.Collider3D.IsTrigger || pair.Collider2D.IsTrigger) return;
 23326065        for (int groupIndex = 0; groupIndex < pair.Manifold.GroupCount; groupIndex++)
 66        {
 6121967            ref ContactGroup group = ref pair.Manifold.GetGroup(groupIndex);
 46834668            for (int point = 0; point < group.Count; point++)
 17295469                ResolveRow(pair, group.Key, group[point], iteration, limit,
 17295470                    correctPosition && IsCorrectionOwner(pair.Manifold, groupIndex, point),
 17295471                    groupIndex * ContactManifold.MaxContactsPerGroup + point);
 72        }
 5541173    }
 74
 75    private static bool IsCorrectionOwner(ContactManifold manifold, int groupIndex, int pointIndex)
 76    {
 2884177        ManifoldContact row = manifold.GetGroup(groupIndex)[pointIndex];
 78        // Translation is shared by equal directions; contact levers remain independent velocity rows.
 7924279        for (int group = 0; group < manifold.GroupCount; group++)
 80        {
 3024081            ref ContactGroup otherGroup = ref manifold.GetGroup(group);
 11929082            for (int point = 0; point < otherGroup.Count; point++)
 83            {
 4886584                ManifoldContact other = otherGroup[point];
 4886585                if (other.Normal != row.Normal) continue;
 4830586                if (other.Depth > row.Depth || other.Depth == row.Depth
 6776587                    && (group < groupIndex || group == groupIndex && point < pointIndex)) return false;
 88            }
 89        }
 938190        return true;
 91    }
 92
 93    private static void ResolveRow(CollisionPairMixed pair, in ContactGroupKey key,
 94        in ManifoldContact row, int iteration, int limit, bool correctPosition, int stateIndex)
 95    {
 17295496        MixedContact contact = MixedContact.FromManifoldContact(row);
 17295497        SolidBody? first = pair.Collider3D.Body;
 17295498        SolidBody2D? second = pair.Collider2D.Body;
 17295499        Vector3d normal = row.Normal.Normalized;
 172954100        ContactResponseSnapshot impact = pair.ResponseSnapshot;
 172954101        ExactContactImpulseState? exact = GetExactImpulse(pair.ExactSolveImpulses, stateIndex);
 172954102        pair.SolveImpulses.TryGet(key, row.ContactId, out ContactWarmStartImpulse cached);
 172954103        ContactResponseSnapshot current = new(impact.PositionA, impact.PositionB, impact.CenterA, impact.CenterB,
 172954104                first == null ? Vector3d.Zero : ResolveLinearVelocity(first),
 172954105                first == null ? Vector3d.Zero : ResolveAngularVelocity(first),
 172954106                second == null ? Vector3d.Zero : ExactContactLever2D.ToSpatial(ResolveLinearVelocity(second)),
 172954107                new Vector3d(Fixed64.Zero, second == null ? Fixed64.Zero : -ResolveAngularVelocity(second), Fixed64.Zero
 172954108                second == null ? Fixed64.Zero : ResolveAngularVelocity(second));
 172954109        if (!TrySolveNormalRow(pair.Collider3D, pair.Collider2D, row, impact, current, cached,
 172954110                ref exact, out ContactNormalImpulseResultMixed result, out ContactResponseSnapshot afterNormal)
 172954111            || !CanApplyNormalVelocityDeltas(first, second, result))
 112        {
 6113            RejectManifoldResponse();
 6114            return;
 115        }
 172948116        Fixed64 normalImpulse = cached.NormalImpulse + result.ImpulseScalar;
 172948117        if (correctPosition)
 118        {
 9380119            Fixed64 mass = GetConstrainedInverseMass(first, normal) + GetConstrainedPlanarInverseMass(second, normal);
 17228120            if (mass > Fixed64.Zero) ApplyPositionCorrection(first, second, normal, row.Depth);
 121        }
 172948122        ApplyNormalVelocityDeltas(first, second, result);
 172976123        if (exact != null) SetExactImpulse(ref pair.ExactSolveImpulses, stateIndex, exact);
 124        // Publish a successful normal solve before friction, which can independently fail to materialize.
 172948125        pair.SolveImpulses.Set(key, row.ContactId, normal, normalImpulse,
 172948126            cached.TangentImpulse, cached.SecondaryTangentImpulse);
 172948127        if (result.HasRepresentableAppliedImpulse && result.HasRepresentableNormalVelocity && HasVelocityDelta(result))
 13489128            pair.Context.Diagnostics.EmitMixedResponseImpulse(pair, contact,
 13489129                normal * result.AppliedImpulseScalar, result.NormalVelocity, iteration, limit);
 172948130        if (!TrySolveManifoldFriction(pair.Collider3D, pair.Collider2D, contact, impact,
 172948131                afterNormal, normal, normalImpulse, cached, ref exact,
 172948132                out Fixed64 tangentImpulse, out Fixed64 secondaryImpulse,
 172948133                out ExactCoulombResponse3D friction))
 134        {
 3135            RejectManifoldResponse();
 3136            return;
 137        }
 172945138        ApplyVelocityDeltas(first, normal.ToVector2d() == Vector2d.Zero ? null : second,
 172945139            friction.FirstLinearVelocityDelta, friction.FirstAngularVelocityDelta,
 172945140            ExactContactLever2D.ToPlanar(friction.SecondLinearVelocityDelta),
 172945141            ExactContactLever2D.ToPlanarAngular(friction.SecondAngularVelocityDelta));
 172972142        if (exact != null) SetExactImpulse(ref pair.ExactSolveImpulses, stateIndex, exact);
 172945143        pair.SolveImpulses.Set(key, row.ContactId, normal, normalImpulse, tangentImpulse, secondaryImpulse);
 172945144    }
 145
 146    private static bool TrySolveManifoldFriction(LSCollider collider3D, LSCollider2D collider2D,
 147        MixedContact contact, in ContactResponseSnapshot impact, in ContactResponseSnapshot current,
 148        Vector3d normal, Fixed64 normalImpulse, in ContactWarmStartImpulse cached,
 149        ref ExactContactImpulseState? exact, out Fixed64 tangentImpulse, out Fixed64 secondaryImpulse,
 150        out ExactCoulombResponse3D response)
 151    {
 172948152        tangentImpulse = secondaryImpulse = Fixed64.Zero;
 172948153        response = default;
 172948154        PhysicsMaterial.CombineFriction(contact.HasMaterialOverride ? contact.Material3D : collider3D.Material,
 172948155            contact.HasMaterialOverride ? contact.Material2D : collider2D.Material,
 172948156            out Fixed64 staticFriction, out Fixed64 dynamicFriction);
 345650157        if (staticFriction <= Fixed64.Zero && dynamicFriction <= Fixed64.Zero) return true;
 246158        SolidBody? first = collider3D.Body;
 159        // Preserve the mixed embedding contract: vertical-only contact does not couple planar friction.
 246160        SolidBody2D? second = normal.ToVector2d() == Vector2d.Zero ? null : collider2D.Body;
 246161        Vector3d tangent = SolverContact3D.CreateTangent(normal);
 246162        Vector3d secondary = Vector3d.Cross(normal, tangent).Normalized;
 246163        ExactLever3D leverA = contact.Anchor3D.GetLeverFrom(impact.CenterA);
 246164        ExactLever3D leverB = contact.Anchor2D.GetLeverFrom(impact.CenterB);
 246165        Vector3d linearA = current.LinearA;
 246166        Vector3d angularA = current.AngularA;
 246167        Vector2d linearB = current.LinearB.ToVector2d();
 246168        Fixed64 angularB = current.PlanarAngularB;
 246169        if (!ExactContactResponseKernel.TryGetCoulombDiskResponse(normal, normalImpulse,
 246170                ExactContactLever3D.CreateResponseOperand(first, linearA, angularA, leverA, -tangent),
 246171                ExactContactLever2D.CreateResponseOperand(second, linearB, angularB, leverB, tangent),
 246172                tangent, cached.TangentImpulse,
 246173                ExactContactLever3D.CreateResponseOperand(first, linearA, angularA, leverA, -secondary),
 246174                ExactContactLever2D.CreateResponseOperand(second, linearB, angularB, leverB, secondary),
 246175                secondary, cached.SecondaryTangentImpulse, staticFriction, dynamicFriction,
 247176                out response, ref exact)) return false;
 245177        Vector2d deltaB = ExactContactLever2D.ToPlanar(response.SecondLinearVelocityDelta);
 245178        Fixed64 angularDeltaB = ExactContactLever2D.ToPlanarAngular(response.SecondAngularVelocityDelta);
 245179        if (!TryAddVelocityDeltas(current, response.FirstLinearVelocityDelta,
 245180                response.FirstAngularVelocityDelta, ExactContactLever2D.ToSpatial(deltaB),
 247181                angularDeltaB, out _)) return false;
 243182        response.TryGetPrimaryAccumulatedImpulse(out tangentImpulse);
 243183        response.TryGetSecondaryAccumulatedImpulse(out secondaryImpulse);
 243184        return true;
 185    }
 186
 187    // CCD retains its normal-only response policy and preflights the complete handoff before mutation.
 188    internal static bool TrySolveManifoldVelocity(LSCollider collider3D, LSCollider2D collider2D,
 189        ContactManifold manifold, in ContactResponseSnapshot impact, ref ContactWarmStartCache cache,
 190        ref ExactContactImpulseState?[]? exactImpulses, out ContactResponseSnapshot solved)
 191    {
 609192        solved = impact;
 609193        cache.Clear();
 609194        ClearExactImpulses(exactImpulses);
 610195        if (!manifold.HasContact) return false;
 608196        bool changedVelocity = false;
 608197        int iterations = manifold.Count == 1 ? 1 : collider3D.Context.Settings.DiscreteSolverIterations;
 5660198        for (int iteration = 0; iteration < iterations; iteration++)
 199        {
 10532200            for (int groupIndex = 0; groupIndex < manifold.GroupCount; groupIndex++)
 201            {
 3044202                ref ContactGroup group = ref manifold.GetGroup(groupIndex);
 18490203                for (int point = 0; point < group.Count; point++)
 204                {
 6206205                    ManifoldContact row = group[point];
 6206206                    cache.TryGet(group.Key, row.ContactId, out ContactWarmStartImpulse cached);
 6206207                    int stateIndex = groupIndex * ContactManifold.MaxContactsPerGroup + point;
 6206208                    ExactContactImpulseState? exact = GetExactImpulse(exactImpulses, stateIndex);
 6206209                    if (!TrySolveNormalRow(collider3D, collider2D, row, impact, solved, cached, ref exact,
 6206210                            out ContactNormalImpulseResultMixed response, out ContactResponseSnapshot next))
 5211                        return false;
 6201212                    solved = next;
 6201213                    changedVelocity |= HasVelocityDelta(response);
 6321214                    if (exact != null) SetExactImpulse(ref exactImpulses, stateIndex, exact);
 6201215                    cache.Set(group.Key, row.ContactId, row.Normal, cached.NormalImpulse + response.ImpulseScalar,
 6201216                        Fixed64.Zero, Fixed64.Zero);
 217                }
 218            }
 219        }
 220        // A kinematic row with no movable effective response cannot admit a CCD handoff.
 603221        return changedVelocity;
 222    }
 223
 224    private static bool TrySolveNormalRow(LSCollider collider3D, LSCollider2D collider2D,
 225        in ManifoldContact row, in ContactResponseSnapshot impact, in ContactResponseSnapshot current,
 226        in ContactWarmStartImpulse cached, ref ExactContactImpulseState? exact,
 227        out ContactNormalImpulseResultMixed result, out ContactResponseSnapshot solved)
 228    {
 179160229        solved = default;
 179160230        bool compactA = row.AnchorA.TryGetOffsetFrom(impact.CenterA, out Vector3d relativeA);
 179160231        bool compactB = row.AnchorB.TryGetOffsetFrom(impact.CenterB, out Vector3d relativeB);
 179160232        return TryGetNormalImpulse(collider3D, collider2D, MixedContact.FromManifoldContact(row),
 179160233                collider3D.Body, collider2D.Body, row.Normal.Normalized, relativeA, relativeB.ToVector2d(),
 179160234                compactA, compactB, current.LinearA, current.AngularA, current.LinearB.ToVector2d(),
 179160235                current.PlanarAngularB, ref exact, out result, cached.NormalImpulse, impact,
 179160236                row.FeatureNamespaceB, detected: true)
 179160237            && TryAddVelocityDeltas(current, result.LinearVelocityDelta3D, result.AngularVelocityDelta3D,
 179160238                ExactContactLever2D.ToSpatial(result.LinearVelocityDelta2D),
 179160239                result.AngularVelocityDelta2D, out solved);
 240    }
 241
 242    private static bool TryAddVelocityDeltas(in ContactResponseSnapshot current, Vector3d linearA,
 243        Vector3d angularA, Vector3d linearB, Fixed64 angularB, out ContactResponseSnapshot solved)
 244    {
 179396245        solved = default;
 179396246        if (!Vector3d.TryAdd(current.LinearA, linearA, out Vector3d nextLinearA)
 179396247            || !Vector3d.TryAdd(current.AngularA, angularA, out Vector3d nextAngularA)
 179396248            || !Vector3d.TryAdd(current.LinearB, linearB, out Vector3d nextLinearB)
 179400249            || !Fixed64.TryAdd(current.PlanarAngularB, angularB, out Fixed64 nextAngularB)) return false;
 179392250        solved = new ContactResponseSnapshot(current.PositionA, current.PositionB, current.CenterA,
 179392251            current.CenterB, nextLinearA, nextAngularA, nextLinearB,
 179392252            new Vector3d(Fixed64.Zero, -nextAngularB, Fixed64.Zero), nextAngularB);
 179392253        return true;
 254    }
 255
 256    private static ExactContactImpulseState? GetExactImpulse(ExactContactImpulseState?[]? states, int index) =>
 179160257        states != null && index < states.Length ? states[index] : null;
 258
 259    private static void SetExactImpulse(ref ExactContactImpulseState?[]? states, int index, ExactContactImpulseState sta
 260    {
 175261        if (states == null || index >= states.Length)
 5262            Array.Resize(ref states, Math.Max(index + 1, states?.Length * 2 ?? ContactManifold.MaxContactsPerGroup));
 175263        states![index] = state;
 175264    }
 265
 266    private static void ClearExactImpulses(ExactContactImpulseState?[]? states)
 267    {
 19683268        if (states == null) return;
 542269        foreach (ExactContactImpulseState? state in states) state?.Clear();
 37270    }
 271
 9272    private static void RejectManifoldResponse() => GravitasLogger.Channel.Write(DiagnosticLevel.Error,
 9273        "Mixed manifold response is outside the representable velocity domain.", nameof(CollisionResponseMixed));
 274}

Methods/Properties

.cctor()
Resolve(Gravitas.CollisionHandling.CollisionPairMixed,Gravitas.CollisionHandling.MixedContact)
Resolve(Gravitas.CollisionHandling.CollisionPairMixed,Gravitas.CollisionHandling.MixedContact,System.Int32,System.Int32,System.Boolean,System.Boolean)
ApplyPositionCorrection(Gravitas.SolidBody,Gravitas.SolidBody2D,FixedMathSharp.Vector3d,FixedMathSharp.Fixed64)
TryGetNormalImpulse(Gravitas.Colliders.LSCollider,Gravitas.Colliders.LSCollider2D,Gravitas.CollisionHandling.MixedContact,Gravitas.SolidBody,Gravitas.SolidBody2D,FixedMathSharp.Vector3d,FixedMathSharp.Vector3d,FixedMathSharp.Vector2d,System.Boolean,System.Boolean,FixedMathSharp.Vector3d,FixedMathSharp.Vector3d,FixedMathSharp.Vector2d,FixedMathSharp.Fixed64,Gravitas.CollisionHandling.ExactContactImpulseState&,Gravitas.CollisionHandling.ContactNormalImpulseResultMixed&,FixedMathSharp.Fixed64,Gravitas.CollisionHandling.ContactResponseSnapshot&,System.Int32,System.Boolean)
CanApplyNormalVelocityDeltas(Gravitas.SolidBody,Gravitas.SolidBody2D,Gravitas.CollisionHandling.ContactNormalImpulseResultMixed)
HasVelocityDelta(Gravitas.CollisionHandling.ContactNormalImpulseResultMixed)
ApplyNormalVelocityDeltas(Gravitas.SolidBody,Gravitas.SolidBody2D,Gravitas.CollisionHandling.ContactNormalImpulseResultMixed)
CanApplyVelocityDeltas(Gravitas.SolidBody,Gravitas.SolidBody2D,FixedMathSharp.Vector3d,FixedMathSharp.Vector3d,FixedMathSharp.Vector2d,FixedMathSharp.Fixed64)
ApplyVelocityDeltas(Gravitas.SolidBody,Gravitas.SolidBody2D,FixedMathSharp.Vector3d,FixedMathSharp.Vector3d,FixedMathSharp.Vector2d,FixedMathSharp.Fixed64)
TryApplyFrictionImpulse(Gravitas.CollisionHandling.CollisionPairMixed,Gravitas.CollisionHandling.MixedContact,Gravitas.SolidBody,Gravitas.SolidBody2D,FixedMathSharp.Vector3d,FixedMathSharp.Vector3d,FixedMathSharp.Vector2d,Gravitas.Materials.PhysicsMaterial,Gravitas.Materials.PhysicsMaterial,Gravitas.CollisionHandling.ContactNormalImpulseResultMixed,FixedMathSharp.Vector3d,FixedMathSharp.Vector3d,FixedMathSharp.Vector2d,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,System.Boolean,System.Boolean,System.Boolean,System.Boolean&)
TryGetCompactFrictionImpulse(Gravitas.SolidBody,Gravitas.SolidBody2D,FixedMathSharp.Vector3d,FixedMathSharp.Vector3d,FixedMathSharp.Vector2d,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,System.Boolean,FixedMathSharp.Vector3d&,FixedMathSharp.Fixed64&)
TryApplyFrictionImpulseExact(Gravitas.CollisionHandling.CollisionPairMixed,Gravitas.CollisionHandling.MixedContact,Gravitas.SolidBody,Gravitas.SolidBody2D,FixedMathSharp.Vector3d,FixedMathSharp.Vector3d,FixedMathSharp.Vector3d,FixedMathSharp.Vector2d,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,System.Boolean,System.Boolean&)
TryApplyImpulse(Gravitas.SolidBody,Gravitas.SolidBody2D,FixedMathSharp.Vector3d,FixedMathSharp.Vector3d,FixedMathSharp.Vector2d,FixedMathSharp.Fixed64,System.Boolean)
TryGetCompactTangent(FixedMathSharp.Vector3d,FixedMathSharp.Vector3d,FixedMathSharp.Vector3d&,System.Boolean&)
TryGetCompactConstrainedInverseMass(Gravitas.SolidBody,FixedMathSharp.Vector3d,FixedMathSharp.Fixed64&)
TryGetCompactConstrainedPlanarInverseMass(Gravitas.SolidBody2D,FixedMathSharp.Vector3d,FixedMathSharp.Fixed64&)
ResolveLinearVelocity(Gravitas.SolidBody)
ResolveLinearVelocity(Gravitas.SolidBody2D)
ResolveAngularVelocity(Gravitas.SolidBody)
ResolveAngularVelocity(Gravitas.SolidBody2D)
ResolveNormal(Gravitas.CollisionHandling.CollisionPairMixed,Gravitas.CollisionHandling.MixedContact)
ResolveFallbackDirection(Gravitas.CollisionHandling.CollisionPairMixed,Gravitas.CollisionHandling.MixedContact)
GetExactLevers(Gravitas.CollisionHandling.CollisionPairMixed,Gravitas.CollisionHandling.MixedContact,Gravitas.SolidBody,Gravitas.SolidBody2D,Gravitas.CollisionHandling.ExactLever3D&,Gravitas.CollisionHandling.ExactLever3D&)
GetConstrainedInverseMass(Gravitas.SolidBody,FixedMathSharp.Vector3d)
GetConstrainedPlanarInverseMass(Gravitas.SolidBody2D,FixedMathSharp.Vector3d)
ResolveManifold(Gravitas.CollisionHandling.CollisionPairMixed)
PrepareSolve(Gravitas.CollisionHandling.CollisionPairMixed)
HasPlanarResponseCoupling(Gravitas.CollisionHandling.ContactManifold)
ResolveManifold(Gravitas.CollisionHandling.CollisionPairMixed,System.Int32,System.Int32,System.Boolean)
IsCorrectionOwner(Gravitas.CollisionHandling.ContactManifold,System.Int32,System.Int32)
ResolveRow(Gravitas.CollisionHandling.CollisionPairMixed,Gravitas.CollisionHandling.ContactGroupKey&,Gravitas.CollisionHandling.ManifoldContact&,System.Int32,System.Int32,System.Boolean,System.Int32)
TrySolveManifoldFriction(Gravitas.Colliders.LSCollider,Gravitas.Colliders.LSCollider2D,Gravitas.CollisionHandling.MixedContact,Gravitas.CollisionHandling.ContactResponseSnapshot&,Gravitas.CollisionHandling.ContactResponseSnapshot&,FixedMathSharp.Vector3d,FixedMathSharp.Fixed64,Gravitas.CollisionHandling.ContactWarmStartImpulse&,Gravitas.CollisionHandling.ExactContactImpulseState&,FixedMathSharp.Fixed64&,FixedMathSharp.Fixed64&,Gravitas.CollisionHandling.ExactCoulombResponse3D&)
TrySolveManifoldVelocity(Gravitas.Colliders.LSCollider,Gravitas.Colliders.LSCollider2D,Gravitas.CollisionHandling.ContactManifold,Gravitas.CollisionHandling.ContactResponseSnapshot&,Gravitas.CollisionHandling.ContactWarmStartCache&,Gravitas.CollisionHandling.ExactContactImpulseState[]&,Gravitas.CollisionHandling.ContactResponseSnapshot&)
TrySolveNormalRow(Gravitas.Colliders.LSCollider,Gravitas.Colliders.LSCollider2D,Gravitas.CollisionHandling.ManifoldContact&,Gravitas.CollisionHandling.ContactResponseSnapshot&,Gravitas.CollisionHandling.ContactResponseSnapshot&,Gravitas.CollisionHandling.ContactWarmStartImpulse&,Gravitas.CollisionHandling.ExactContactImpulseState&,Gravitas.CollisionHandling.ContactNormalImpulseResultMixed&,Gravitas.CollisionHandling.ContactResponseSnapshot&)
TryAddVelocityDeltas(Gravitas.CollisionHandling.ContactResponseSnapshot&,FixedMathSharp.Vector3d,FixedMathSharp.Vector3d,FixedMathSharp.Vector3d,FixedMathSharp.Fixed64,Gravitas.CollisionHandling.ContactResponseSnapshot&)
GetExactImpulse(Gravitas.CollisionHandling.ExactContactImpulseState[],System.Int32)
SetExactImpulse(Gravitas.CollisionHandling.ExactContactImpulseState[]&,System.Int32,Gravitas.CollisionHandling.ExactContactImpulseState)
ClearExactImpulses(Gravitas.CollisionHandling.ExactContactImpulseState[])
RejectManifoldResponse()