< Summary

Information
Class: Gravitas.Queries.GjkSimplexScale
Assembly: Gravitas
File(s): /home/runner/work/Gravitas/Gravitas/src/Gravitas/Queries/GjkSimplexScale.cs
Line coverage
100%
Covered lines: 111
Uncovered lines: 0
Coverable lines: 111
Total lines: 296
Line coverage: 100%
Branch coverage
100%
Covered branches: 60
Total branches: 60
Branch coverage: 100%
Method coverage

Feature is only available for sponsors

Upgrade to PRO version

Metrics

File(s)

/home/runner/work/Gravitas/Gravitas/src/Gravitas/Queries/GjkSimplexScale.cs

#LineLine coverage
 1//=======================================================================
 2// GjkSimplexScale.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;
 11
 12namespace Gravitas.Queries;
 13
 14/// <summary>
 15/// Uniformly scales GJK simplex coordinates before evaluating products whose
 16/// ratios and signs are invariant under a common scale.
 17/// </summary>
 18internal static class GjkSimplexScale
 19{
 120    private static readonly Fixed64 ProductSafeComponentLimit = (Fixed64)8;
 21
 22    /// <summary>
 23    /// Creates a two-term Minkowski difference in the shared GJK working
 24    /// coordinate. An exact arithmetic raw shift avoids round-to-even endpoint
 25    /// overshoot, so halving before subtraction covers every Fixed64 pair.
 26    /// </summary>
 27    [MethodImpl(MethodImplOptions.AggressiveInlining)]
 28    public static Vector3d CreateWorkingDifference(Vector3d first, Vector3d second) =>
 129        CreateWorkingDifference(first, second, 1);
 30
 31    [MethodImpl(MethodImplOptions.AggressiveInlining)]
 32    public static Vector3d CreateWorkingDifference(Vector3d first, Vector3d second, int shift)
 33    {
 434        ValidateShift(shift);
 235        if (Vector3d.TrySubtract(
 236            ScaleByPowerOfTwo(first, shift),
 237            ScaleByPowerOfTwo(second, shift),
 238            out Vector3d difference))
 39        {
 140            return difference;
 41        }
 42
 143        throw new InvalidOperationException("The selected GJK working shift does not preserve an exact difference.");
 44    }
 45
 46    [MethodImpl(MethodImplOptions.AggressiveInlining)]
 47    public static Vector2d CreateWorkingDifference(Vector2d first, Vector2d second) =>
 148        CreateWorkingDifference(first, second, 2);
 49
 50    [MethodImpl(MethodImplOptions.AggressiveInlining)]
 51    public static Vector2d CreateWorkingDifference(Vector2d first, Vector2d second, int shift)
 52    {
 33153        ValidateShift(shift);
 32954        if (Vector2d.TrySubtract(
 32955            ScaleByPowerOfTwo(first, shift),
 32956            ScaleByPowerOfTwo(second, shift),
 32957            out Vector2d difference))
 58        {
 32859            return difference;
 60        }
 61
 162        throw new InvalidOperationException("The selected GJK working shift does not preserve an exact difference.");
 63    }
 64
 65    /// <summary>
 66    /// Creates a three-term Minkowski difference in the shared GJK working
 67    /// coordinate without first forming a potentially saturated sum.
 68    /// </summary>
 69    [MethodImpl(MethodImplOptions.AggressiveInlining)]
 70    public static Vector2d CreateWorkingDifference(Vector2d first, Vector2d second, Vector2d third) =>
 171        CreateWorkingDifference(first, second, third, 2);
 72
 73    [MethodImpl(MethodImplOptions.AggressiveInlining)]
 74    public static Vector2d CreateWorkingDifference(
 75        Vector2d first,
 76        Vector2d second,
 77        Vector2d third,
 78        int shift)
 79    {
 183380        ValidateShift(shift);
 182981        if (Vector2d.TrySubtract(
 182982                ScaleByPowerOfTwo(first, shift),
 182983                ScaleByPowerOfTwo(second, shift),
 182984                out Vector2d difference)
 182985            && Vector2d.TrySubtract(
 182986                difference,
 182987                ScaleByPowerOfTwo(third, shift),
 182988                out Vector2d result))
 89        {
 182690            return result;
 91        }
 92
 393        throw new InvalidOperationException("The selected GJK working shift does not preserve an exact difference.");
 94    }
 95
 96    /// <summary>
 97    /// Creates a four-term Minkowski difference for owner-relative support
 98    /// without materializing the support in absolute coordinates.
 99    /// </summary>
 100    [MethodImpl(MethodImplOptions.AggressiveInlining)]
 101    public static Vector2d CreateWorkingDifference(
 102        Vector2d first,
 103        Vector2d second,
 104        Vector2d third,
 105        Vector2d fourth,
 106        int shift)
 107    {
 419108        Vector2d difference = CreateWorkingDifference(first, second, third, shift);
 416109        if (Vector2d.TrySubtract(difference, ScaleByPowerOfTwo(fourth, shift), out Vector2d result))
 415110            return result;
 111
 1112        throw new InvalidOperationException("The selected GJK working shift does not preserve an exact difference.");
 113    }
 114
 115    [MethodImpl(MethodImplOptions.AggressiveInlining)]
 116    public static Fixed64 RestoreTwoTermDistance(Fixed64 workingDistance) =>
 1117        RestoreDistance(workingDistance, 1);
 118
 119    [MethodImpl(MethodImplOptions.AggressiveInlining)]
 120    public static Fixed64 RestoreThreeTermDistance(Fixed64 workingDistance) =>
 1121        RestoreDistance(workingDistance, 2);
 122
 123    [MethodImpl(MethodImplOptions.AggressiveInlining)]
 124    public static Fixed64 RestoreDistance(Fixed64 workingDistance, int shift)
 125    {
 1296126        ValidateShift(shift);
 1294127        return shift == 0 ? workingDistance : workingDistance * (1 << shift);
 128    }
 129
 130    [MethodImpl(MethodImplOptions.AggressiveInlining)]
 131    public static Fixed64 GetCoordinateScale(int shift)
 132    {
 5242133        ValidateShift(shift);
 5240134        return shift switch
 5240135        {
 549136            0 => Fixed64.One,
 1137            1 => Fixed64.Half,
 4690138            _ => Fixed64.Quarter
 5240139        };
 140    }
 141
 142    public static int SelectTwoTermShift(
 143        Vector3d firstMin,
 144        Vector3d firstMax,
 145        Vector3d secondMin,
 146        Vector3d secondMax) =>
 2147        CanSubtractBounds(firstMin, firstMax, secondMin, secondMax) ? 0 : 1;
 148
 149    public static int SelectThreeTermShift(
 150        Vector2d point,
 151        Vector2d targetMin,
 152        Vector2d targetMax,
 153        Fixed64 expansionRadius)
 154    {
 388155        if (CanSubtractExpandedBounds(point, targetMin, targetMax, expansionRadius, 0))
 383156            return 0;
 157
 5158        return CanSubtractExpandedBounds(point, targetMin, targetMax, expansionRadius, 1) ? 1 : 2;
 159    }
 160
 161    public static int SelectFourTermShift(
 162        Vector2d point,
 163        Vector2d origin,
 164        Vector2d targetMinOffset,
 165        Vector2d targetMaxOffset,
 166        Fixed64 expansionRadius)
 167    {
 174168        if (CanSubtractExpandedBounds(point, targetMinOffset, targetMaxOffset, expansionRadius, 0, origin))
 168169            return 0;
 170
 6171        return CanSubtractExpandedBounds(point, targetMinOffset, targetMaxOffset, expansionRadius, 1, origin) ? 1 : 2;
 172    }
 173
 174    [MethodImpl(MethodImplOptions.AggressiveInlining)]
 175    private static Vector3d ScaleByPowerOfTwo(Vector3d value, int shift) =>
 4176        new(
 4177            value.X >> shift,
 4178            value.Y >> shift,
 4179            value.Z >> shift);
 180
 181    [MethodImpl(MethodImplOptions.AggressiveInlining)]
 182    private static Vector2d ScaleByPowerOfTwo(Vector2d value, int shift) =>
 8446183        new(
 8446184            value.X >> shift,
 8446185            value.Y >> shift);
 186
 187    private static bool CanSubtractBounds(
 188        Vector3d firstMin,
 189        Vector3d firstMax,
 190        Vector3d secondMin,
 191        Vector3d secondMax) =>
 2192        Vector3d.TrySubtract(firstMax, secondMin, out _)
 2193            && Vector3d.TrySubtract(firstMin, secondMax, out _);
 194
 195    private static bool CanSubtractExpandedBounds(
 196        Vector2d point,
 197        Vector2d targetMin,
 198        Vector2d targetMax,
 199        Fixed64 expansionRadius,
 200        int shift,
 201        Vector2d? origin = null)
 202    {
 573203        Vector2d scaledPoint = ScaleByPowerOfTwo(point, shift);
 573204        if (origin is Vector2d targetOrigin)
 205        {
 180206            Vector2d scaledOrigin = ScaleByPowerOfTwo(targetOrigin, shift);
 207            // The initial direction reverses this difference; both signs must fit.
 180208            if (!Vector2d.TrySubtract(scaledOrigin, scaledPoint, out _)
 180209                || !Vector2d.TrySubtract(scaledPoint, scaledOrigin, out scaledPoint))
 6210                return false;
 211        }
 212
 567213        Vector2d scaledMin = ScaleByPowerOfTwo(targetMin, shift);
 567214        Vector2d scaledMax = ScaleByPowerOfTwo(targetMax, shift);
 567215        Fixed64 scaledRadius = ScaleRadiusCeiling(expansionRadius, shift);
 567216        Vector2d radius = new(scaledRadius, scaledRadius);
 217
 567218        return Vector2d.TrySubtract(scaledPoint, scaledMin, out Vector2d positiveDifference)
 567219            && Vector2d.TryAdd(positiveDifference, radius, out _)
 567220            && Vector2d.TrySubtract(scaledPoint, scaledMax, out Vector2d negativeDifference)
 567221            && Vector2d.TrySubtract(negativeDifference, radius, out _);
 222    }
 223
 224    [MethodImpl(MethodImplOptions.AggressiveInlining)]
 225    private static Fixed64 ScaleRadiusCeiling(Fixed64 radius, int shift)
 226    {
 567227        Fixed64 scaled = radius >> shift;
 567228        if (shift == 0 || (scaled << shift) == radius)
 561229            return scaled;
 230
 6231        return scaled + Fixed64.MinIncrement;
 232    }
 233
 234    [MethodImpl(MethodImplOptions.AggressiveInlining)]
 235    private static void ValidateShift(int shift)
 236    {
 8706237        if ((uint)shift > 2U)
 12238            throw new ArgumentOutOfRangeException(nameof(shift), "GJK coordinate shifts must be between zero and two.");
 8694239    }
 240
 241    public static Fixed64 ScaleForProducts(Span<Vector3d> points)
 242    {
 24070243        Fixed64 largestComponent = Fixed64.Zero;
 183420244        for (int i = 0; i < points.Length; i++)
 245        {
 67640246            Vector3d point = points[i];
 67640247            largestComponent = FixedMath.Max(
 67640248                largestComponent,
 67640249                FixedMath.Max(point.X.Abs(), FixedMath.Max(point.Y.Abs(), point.Z.Abs())));
 250        }
 251
 24070252        Fixed64 scale = Fixed64.One;
 24904253        while (largestComponent > ProductSafeComponentLimit)
 254        {
 834255            largestComponent *= Fixed64.Half;
 834256            scale *= Fixed64.Half;
 257        }
 258
 24070259        if (scale == Fixed64.One)
 23701260            return scale;
 261
 262        // With components <= 8, 3D simplex differences are <= 16 and the
 263        // largest tetrahedron face-side product remains inside Fixed64.
 2900264        for (int i = 0; i < points.Length; i++)
 1081265            points[i] *= scale;
 266
 369267        return scale;
 268    }
 269
 270    public static Fixed64 ScaleForProducts(Span<Vector2d> points)
 271    {
 2272        Fixed64 largestComponent = Fixed64.Zero;
 12273        for (int i = 0; i < points.Length; i++)
 274        {
 4275            Vector2d point = points[i];
 4276            largestComponent = FixedMath.Max(
 4277                largestComponent,
 4278                FixedMath.Max(point.X.Abs(), point.Y.Abs()));
 279        }
 280
 2281        Fixed64 scale = Fixed64.One;
 30282        while (largestComponent > ProductSafeComponentLimit)
 283        {
 28284            largestComponent *= Fixed64.Half;
 28285            scale *= Fixed64.Half;
 286        }
 287
 2288        if (scale == Fixed64.One)
 1289            return scale;
 290
 6291        for (int i = 0; i < points.Length; i++)
 2292            points[i] *= scale;
 293
 1294        return scale;
 295    }
 296}

Methods/Properties

.cctor()
CreateWorkingDifference(FixedMathSharp.Vector3d,FixedMathSharp.Vector3d)
CreateWorkingDifference(FixedMathSharp.Vector3d,FixedMathSharp.Vector3d,System.Int32)
CreateWorkingDifference(FixedMathSharp.Vector2d,FixedMathSharp.Vector2d)
CreateWorkingDifference(FixedMathSharp.Vector2d,FixedMathSharp.Vector2d,System.Int32)
CreateWorkingDifference(FixedMathSharp.Vector2d,FixedMathSharp.Vector2d,FixedMathSharp.Vector2d)
CreateWorkingDifference(FixedMathSharp.Vector2d,FixedMathSharp.Vector2d,FixedMathSharp.Vector2d,System.Int32)
CreateWorkingDifference(FixedMathSharp.Vector2d,FixedMathSharp.Vector2d,FixedMathSharp.Vector2d,FixedMathSharp.Vector2d,System.Int32)
RestoreTwoTermDistance(FixedMathSharp.Fixed64)
RestoreThreeTermDistance(FixedMathSharp.Fixed64)
RestoreDistance(FixedMathSharp.Fixed64,System.Int32)
GetCoordinateScale(System.Int32)
SelectTwoTermShift(FixedMathSharp.Vector3d,FixedMathSharp.Vector3d,FixedMathSharp.Vector3d,FixedMathSharp.Vector3d)
SelectThreeTermShift(FixedMathSharp.Vector2d,FixedMathSharp.Vector2d,FixedMathSharp.Vector2d,FixedMathSharp.Fixed64)
SelectFourTermShift(FixedMathSharp.Vector2d,FixedMathSharp.Vector2d,FixedMathSharp.Vector2d,FixedMathSharp.Vector2d,FixedMathSharp.Fixed64)
ScaleByPowerOfTwo(FixedMathSharp.Vector3d,System.Int32)
ScaleByPowerOfTwo(FixedMathSharp.Vector2d,System.Int32)
CanSubtractBounds(FixedMathSharp.Vector3d,FixedMathSharp.Vector3d,FixedMathSharp.Vector3d,FixedMathSharp.Vector3d)
CanSubtractExpandedBounds(FixedMathSharp.Vector2d,FixedMathSharp.Vector2d,FixedMathSharp.Vector2d,FixedMathSharp.Fixed64,System.Int32,System.Nullable`1<FixedMathSharp.Vector2d>)
ScaleRadiusCeiling(FixedMathSharp.Fixed64,System.Int32)
ValidateShift(System.Int32)
ScaleForProducts(System.Span`1<FixedMathSharp.Vector3d>)
ScaleForProducts(System.Span`1<FixedMathSharp.Vector2d>)