< Summary

Information
Class: Gravitas.CollisionHandling.GjkSimplexPolicy
Assembly: Gravitas
File(s): /home/runner/work/Gravitas/Gravitas/src/Gravitas/CollisionHandling/Detection/3D/Convex/GjkSimplexPolicy.cs
Line coverage
100%
Covered lines: 156
Uncovered lines: 0
Coverable lines: 156
Total lines: 273
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
AddPoint(...)100%44100%
Update(...)100%44100%
UpdateLine(...)100%66100%
UpdateTriangle(...)100%2222100%
UpdateTetrahedron(...)100%66100%
SameDirection(...)100%11100%
TripleCross(...)100%11100%
IsOutsideFace(...)100%1010100%
HasSeparatingSupport(...)100%44100%
RequiresExactProducts(...)100%44100%
HasBoundedComponents(...)100%1010100%
IsOutsideEdge(...)100%11100%
GetFaceDirection(...)100%11100%
Perpendicular(...)100%44100%

File(s)

/home/runner/work/Gravitas/Gravitas/src/Gravitas/CollisionHandling/Detection/3D/Convex/GjkSimplexPolicy.cs

#LineLine coverage
 1//=======================================================================
 2// GjkSimplexPolicy.cs
 3//=======================================================================
 4// MIT License, Copyright (c) 2026-present David Oravsky (mrdav30)
 5// See LICENSE file in the project root for full license information.
 6//=======================================================================
 7
 8using System;
 9using System.Runtime.CompilerServices;
 10using FixedMathSharp;
 11using FixedMathSharp.Geometry;
 12
 13namespace Gravitas.CollisionHandling;
 14
 15/// <summary>
 16/// Updates the GJK simplex and search direction, using exact predicates
 17/// when fixed-point products are unsafe.
 18/// </summary>
 19internal static class GjkSimplexPolicy
 20{
 21    // Each vertex component <= 256 in magnitude bounds cubic crosses by 536,870,912; side dots stay exact.
 22    private const long ProductSafeVertexRaw = 256L << FixedMath.SHIFT_AMOUNT_I;
 23
 24    public static void AddPoint(Span<Vector3d> simplex, ref int count, Vector3d point)
 25    {
 1690826        for (int i = Math.Min(count, 3); i > 0; i--)
 548427            simplex[i] = simplex[i - 1];
 28
 297029        simplex[0] = point;
 297030        if (count < 4)
 296931            count++;
 297032    }
 33
 34    public static bool Update(Span<Vector3d> simplex, ref int count, ref Vector3d direction)
 35    {
 296936        return count switch
 296937        {
 130138            2 => UpdateLine(simplex, ref count, ref direction),
 82439            3 => UpdateTriangle(simplex, ref count, ref direction),
 84440            _ => UpdateTetrahedron(simplex, ref count, ref direction)
 296941        };
 42    }
 43
 44    public static bool UpdateLine(Span<Vector3d> simplex, ref int count, ref Vector3d direction)
 45    {
 132746        Vector3d a = simplex[0];
 132747        Vector3d b = simplex[1];
 132748        Vector3d ab = b - a;
 132749        Vector3d ao = -a;
 132750        bool exact = RequiresExactProducts(simplex, 2);
 51
 132752        if (SameDirection(ab, ao))
 53        {
 132454            direction = exact
 132455                ? Vector3d.GetNormalizedProjectionOnPlane(ao, ab)
 132456                : TripleCross(ab, ao, ab);
 132457            if (direction.MagnitudeSquared <= Fixed64.Epsilon)
 22658                direction = Perpendicular(ab);
 132459            return false;
 60        }
 61
 362        simplex[0] = a;
 363        count = 1;
 364        direction = ao;
 365        return false;
 66    }
 67
 68    public static bool UpdateTriangle(Span<Vector3d> simplex, ref int count, ref Vector3d direction)
 69    {
 83770        Vector3d a = simplex[0];
 83771        Vector3d b = simplex[1];
 83772        Vector3d c = simplex[2];
 83773        Vector3d ab = b - a;
 83774        Vector3d ac = c - a;
 83775        Vector3d ao = -a;
 83776        bool exact = RequiresExactProducts(simplex, 3);
 83777        Vector3d abc = exact ? default : Vector3d.Cross(ab, ac);
 78
 83779        if (exact ? IsOutsideEdge(ab, ac, ao) : SameDirection(Vector3d.Cross(abc, ac), ao))
 80        {
 4281            if (SameDirection(ac, ao))
 82            {
 4083                simplex[1] = c;
 4084                count = 2;
 4085                direction = exact
 4086                    ? Vector3d.GetNormalizedProjectionOnPlane(ao, ac)
 4087                    : TripleCross(ac, ao, ac);
 4088                if (direction.MagnitudeSquared <= Fixed64.Epsilon)
 189                    direction = Perpendicular(ac);
 4090                return false;
 91            }
 92
 293            simplex[1] = b;
 294            count = 2;
 295            return UpdateLine(simplex, ref count, ref direction);
 96        }
 97
 79598        if (exact ? IsOutsideEdge(ac, ab, ao) : SameDirection(Vector3d.Cross(ab, abc), ao))
 99        {
 20100            simplex[1] = b;
 20101            count = 2;
 20102            return UpdateLine(simplex, ref count, ref direction);
 103        }
 104
 775105        if (exact)
 193106            abc = GetFaceDirection(ab, ac);
 775107        if (exact ? WideGeometry.GetTripleProductSign(
 775108                ao.X, ao.Y, ao.Z, ab.X, ab.Y, ab.Z, ac.X, ac.Y, ac.Z) > 0
 775109            : SameDirection(abc, ao))
 110        {
 103111            direction = abc;
 103112            return false;
 113        }
 114
 672115        simplex[1] = c;
 672116        simplex[2] = b;
 672117        direction = -abc;
 672118        return false;
 119    }
 120
 121    public static bool UpdateTetrahedron(Span<Vector3d> simplex, ref int count, ref Vector3d direction)
 122    {
 854123        Vector3d a = simplex[0];
 854124        Vector3d b = simplex[1];
 854125        Vector3d c = simplex[2];
 854126        Vector3d d = simplex[3];
 854127        bool exact = RequiresExactProducts(simplex, 4);
 128
 854129        if (IsOutsideFace(a, b, c, d, exact, out Vector3d abc))
 130        {
 63131            simplex[0] = a;
 63132            simplex[1] = b;
 63133            simplex[2] = c;
 63134            count = 3;
 63135            direction = abc;
 63136            return false;
 137        }
 138
 791139        if (IsOutsideFace(a, c, d, b, exact, out Vector3d acd))
 140        {
 34141            simplex[0] = a;
 34142            simplex[1] = c;
 34143            simplex[2] = d;
 34144            count = 3;
 34145            direction = acd;
 34146            return false;
 147        }
 148
 757149        if (IsOutsideFace(a, d, b, c, exact, out Vector3d adb))
 150        {
 36151            simplex[0] = a;
 36152            simplex[1] = d;
 36153            simplex[2] = b;
 36154            count = 3;
 36155            direction = adb;
 36156            return false;
 157        }
 158
 721159        return true;
 160    }
 161
 162    [MethodImpl(MethodImplOptions.AggressiveInlining)]
 163    private static bool SameDirection(Vector3d first, Vector3d second) =>
 6877164        WideGeometry.GetDifferenceDotProduct3D(
 6877165            first.X, Fixed64.Zero, first.Y, Fixed64.Zero, first.Z, Fixed64.Zero,
 6877166            second.X, Fixed64.Zero, second.Y, Fixed64.Zero, second.Z, Fixed64.Zero).Sign > 0;
 167
 168    [MethodImpl(MethodImplOptions.AggressiveInlining)]
 169    private static Vector3d TripleCross(Vector3d first, Vector3d second, Vector3d third) =>
 789170        Vector3d.Cross(Vector3d.Cross(first, second), third);
 171
 172    private static bool IsOutsideFace(
 173        Vector3d a, Vector3d b, Vector3d c, Vector3d opposite,
 174        bool exact, out Vector3d normal)
 175    {
 2402176        Vector3d ab = b - a;
 2402177        Vector3d ac = c - a;
 2402178        if (!exact)
 179        {
 1866180            normal = Vector3d.Cross(ab, ac);
 1866181            if (SameDirection(normal, opposite - a))
 8182                normal = -normal;
 1866183            return SameDirection(normal, -a);
 184        }
 185
 536186        Vector3d oppositeOffset = opposite - a;
 536187        Vector3d ao = -a;
 536188        bool reversed = WideGeometry.GetTripleProductSign(
 536189            oppositeOffset.X, oppositeOffset.Y, oppositeOffset.Z,
 536190            ab.X, ab.Y, ab.Z, ac.X, ac.Y, ac.Z) > 0;
 536191        int side = WideGeometry.GetTripleProductSign(
 536192            ao.X, ao.Y, ao.Z, ab.X, ab.Y, ab.Z, ac.X, ac.Y, ac.Z);
 536193        if ((reversed ? -side : side) <= 0)
 194        {
 522195            normal = default;
 522196            return false;
 197        }
 14198        normal = GetFaceDirection(ab, ac);
 14199        if (reversed)
 2200            normal = -normal;
 14201        return true;
 202    }
 203
 204    public static bool HasSeparatingSupport(Vector3d point, Vector3d direction)
 205    {
 206        // This gate precedes simplex reduction: a new support can exceed its old bound.
 207        // 3 * 64 * 2^23 < 2^31 bounds the complete scalar dot, including partial sums.
 3597208        if (HasBoundedComponents(point, 64L << FixedMath.SHIFT_AMOUNT_I)
 3597209            && HasBoundedComponents(direction, (1L << 23) << FixedMath.SHIFT_AMOUNT_I))
 210        {
 2272211            return Vector3d.Dot(point, direction) < -Fixed64.Epsilon;
 212        }
 1325213        Signed192 dot = WideGeometry.GetDifferenceDotProduct3D(
 1325214            point.X, Fixed64.Zero, point.Y, Fixed64.Zero, point.Z, Fixed64.Zero,
 1325215            direction.X, Fixed64.Zero, direction.Y, Fixed64.Zero, direction.Z, Fixed64.Zero);
 1325216        return WideArithmetic.AddSigned192(
 1325217            dot, Fixed64.GetExactRawProduct(Fixed64.Epsilon, Fixed64.One)).Sign < 0;
 218    }
 219
 220    private static bool RequiresExactProducts(Span<Vector3d> simplex, int count)
 221    {
 18146222        for (int i = 0; i < count; i++)
 7021223            if (!HasBoundedComponents(simplex[i], ProductSafeVertexRaw))
 966224                return true;
 2052225        return false;
 226    }
 227
 228    [MethodImpl(MethodImplOptions.AggressiveInlining)]
 229    private static bool HasBoundedComponents(Vector3d value, long limit) =>
 12903230        value.X.m_rawValue >= -limit && value.X.m_rawValue <= limit
 12903231        && value.Y.m_rawValue >= -limit && value.Y.m_rawValue <= limit
 12903232        && value.Z.m_rawValue >= -limit && value.Z.m_rawValue <= limit;
 233
 234    private static bool IsOutsideEdge(Vector3d firstEdge, Vector3d secondEdge, Vector3d ao)
 235    {
 236        // Both outward edge signs negate the plane-projection numerator. Test its dot directly:
 237        // |second|^2*(first.ao) - (first.second)*(second.ao); raw quartics fit Signed320.
 438238        Signed192 q = WideGeometry.GetDifferenceDotProduct3D(
 438239            secondEdge.X, Fixed64.Zero, secondEdge.Y, Fixed64.Zero, secondEdge.Z, Fixed64.Zero,
 438240            secondEdge.X, Fixed64.Zero, secondEdge.Y, Fixed64.Zero, secondEdge.Z, Fixed64.Zero);
 438241        Signed192 firstTowardOrigin = WideGeometry.GetDifferenceDotProduct3D(
 438242            firstEdge.X, Fixed64.Zero, firstEdge.Y, Fixed64.Zero, firstEdge.Z, Fixed64.Zero,
 438243            ao.X, Fixed64.Zero, ao.Y, Fixed64.Zero, ao.Z, Fixed64.Zero);
 438244        Signed192 projection = WideGeometry.GetDifferenceDotProduct3D(
 438245            firstEdge.X, Fixed64.Zero, firstEdge.Y, Fixed64.Zero, firstEdge.Z, Fixed64.Zero,
 438246            secondEdge.X, Fixed64.Zero, secondEdge.Y, Fixed64.Zero, secondEdge.Z, Fixed64.Zero);
 438247        Signed192 secondTowardOrigin = WideGeometry.GetDifferenceDotProduct3D(
 438248            secondEdge.X, Fixed64.Zero, secondEdge.Y, Fixed64.Zero, secondEdge.Z, Fixed64.Zero,
 438249            ao.X, Fixed64.Zero, ao.Y, Fixed64.Zero, ao.Z, Fixed64.Zero);
 438250        return WideArithmetic.SubtractSigned320(
 438251            WideArithmetic.MultiplySigned192(q, firstTowardOrigin),
 438252            WideArithmetic.MultiplySigned192(projection, secondTowardOrigin)).Sign < 0;
 253    }
 254
 255    private static Vector3d GetFaceDirection(Vector3d ab, Vector3d ac)
 256    {
 207257        WideGeometry.GetDifferenceCrossProduct3D(
 207258            ab.X, Fixed64.Zero, ab.Y, Fixed64.Zero, ab.Z, Fixed64.Zero,
 207259            ac.X, Fixed64.Zero, ac.Y, Fixed64.Zero, ac.Z, Fixed64.Zero,
 207260            out Signed192 x, out Signed192 y, out Signed192 z);
 207261        return WideNormalization.GetNormalized(x, y, z);
 262    }
 263
 264    private static Vector3d Perpendicular(Vector3d vector)
 265    {
 227266        Vector3d candidate = Vector3d.Cross(vector, Vector3d.Up);
 227267        if (candidate.MagnitudeSquared > Fixed64.Epsilon)
 224268            return candidate;
 269
 3270        candidate = Vector3d.Cross(vector, Vector3d.Right);
 3271        return candidate.MagnitudeSquared > Fixed64.Epsilon ? candidate : Vector3d.Forward;
 272    }
 273}