< Summary

Line coverage
100%
Covered lines: 923
Uncovered lines: 0
Coverable lines: 923
Total lines: 1719
Line coverage: 100%
Branch coverage
100%
Covered branches: 284
Total branches: 284
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: TrySweepCertifiedConvexPatch(...)100%2424100%
File 2: .cctor()100%11100%
File 2: .ctor(...)100%11100%
File 2: ClearSource()100%11100%
File 2: PrepareConvexMeshSource(...)100%11100%
File 2: PrepareCompoundSource(...)100%11100%
File 2: PreparePrimitiveSource(...)100%22100%
File 2: PrepareCircleSlabSource(...)100%11100%
File 2: PrepareSphereSource(...)100%11100%
File 2: TrySweepPreparedSource(...)100%88100%
File 2: Prepare(...)100%11100%
File 2: Prepare(...)100%66100%
File 2: TrySweepCompoundSource(...)100%66100%
File 2: TrySweepSourceShape(...)100%88100%
File 2: TrySweepTargetCompound(...)100%66100%
File 2: TrySweepConcaveMeshTarget(...)100%1616100%
File 2: BuildOrderedSweepTriangleCandidates(...)100%44100%
File 2: TryComputeSweepLowerBoundNumerator(...)100%11100%
File 2: TryComputeSweepLowerBoundNumerator(...)100%44100%
File 2: IncludeAxisEntryNumerator(...)100%1616100%
File 2: RemainingSweepTrianglesCannotBeat(...)100%22100%
File 2: TrySweepConvexTarget(...)100%3030100%
File 2: TryResolveEndpointBracket(...)100%1212100%
File 2: RetainCapsuleNormal(...)100%44100%
File 2: GetChordOffset(...)100%11100%
File 2: ResolveHitAnchor(...)100%1616100%
File 2: ResolveHitNormal(...)100%1010100%
File 2: CreateColliderShape(...)100%11100%
File 2: CreateTriangleShape(...)100%11100%
File 2: ThrowIfConcaveSource(...)100%22100%
File 2: CreateSweptSourceBoundsInMeshFrame(...)100%22100%
File 2: CanSweptSourceShapeReachTarget(...)100%22100%
File 2: ComesBeforeReducerCandidate(...)100%44100%
File 2: CreateConcaveSourceException(...)100%11100%
File 3: ComputeDistance(...)100%1818100%
File 3: CreateSupportPoint(...)100%11100%
File 3: SolveClosestSimplex(...)100%66100%
File 4: TryResolveInitialMeshContact(...)100%1616100%
File 5: ReduceSegment(...)100%66100%
File 5: ReduceTriangle(...)100%11100%
File 5: ReduceTetrahedron(...)100%22100%
File 5: EvaluateFace(...)100%44100%
File 5: ReduceByWeights(...)100%22100%
File 5: AddWeightedPoint(...)100%22100%
File 5: ClosestPointOnTriangleToOrigin(...)100%22100%
File 5: ClosestPointOnDegenerateTriangleToOrigin(...)100%44100%
File 5: GetClosestSegmentWeights(...)100%66100%
File 5: IsOriginInsideTetrahedron(...)100%1212100%
File 5: ContainsSupportPoint(...)100%66100%
File 6: .ctor(...)100%11100%
File 6: .ctor(...)100%11100%
File 6: Compare(...)100%22100%
File 6: .ctor(...)100%11100%
File 6: get_Intersection()100%11100%
File 6: FromWeights(...)100%11100%
File 6: FromSpanWeights(...)100%44100%
File 6: GetWitnesses(...)100%88100%

File(s)

/home/runner/work/Gravitas/Gravitas/src/Gravitas/Queries/3D/Sweeps/ConvexSweepQueryWorker.ConvexPatch.cs

#LineLine coverage
 1//=======================================================================
 2// ConvexSweepQueryWorker.ConvexPatch.cs
 3//=======================================================================
 4// MIT License, Copyright (c) 2026-present David Oravsky (mrdav30)
 5// See LICENSE file in the project root for full license information.
 6//=======================================================================
 7
 8using System;
 9using FixedMathSharp;
 10using FixedMathSharp.Geometry;
 11using Gravitas.Colliders;
 12
 13namespace Gravitas.Queries;
 14
 15/// <content>Attempts a certified whole-mesh convex patch selected by a finite reference-center crossing.</content>
 16internal sealed partial class ConvexSweepQueryWorker
 17{
 18    private bool TrySweepCertifiedConvexPatch(in ConvexShape source, LSMeshCollider target,
 19        out Physics3DHit hit, out Fixed64 numerator)
 20    {
 113221        hit = default;
 113222        numerator = default;
 113223        PhysicsMesh mesh = target.Mesh;
 113224        if (_admitTriangle != null
 113225            || _triangleCandidates.Count != mesh.TriangleCount || _triangleCandidates.Count < 2)
 64926            return false;
 27
 48328        source.GetSourceBounds(out Vector3d min, out Vector3d max);
 48329        Vector3d padding = Vector3d.One * ContactTolerance;
 48330        if (!Vector3d.TrySubtract(min, padding, out Vector3d paddedMin)
 48331            || !Vector3d.TryAdd(max, padding, out Vector3d paddedMax)
 48332            || SweepBoundsUtility.OverlapsInclusive(paddedMin, paddedMax, target.Bounds.Min, target.Bounds.Max)
 48333            || !mesh.HasSingleConvexCoplanarPatch)
 30434            return false;
 35
 17936        ReadOnlySpan<int> corners = mesh.GetConvexCoplanarPatchCornerVertexIndices(0);
 17937        ReadOnlySpan<Vector3d> vertices = mesh.GeometryVertices;
 17938        var first = new FixedTriangle(vertices[corners[0]], vertices[corners[1]], vertices[corners[2]]);
 17939        WideDirection3d plane = WideTriangleRelations.GetScaledTriangleLocalNormal(first, mesh.GeometryFrame)
 17940            .Transform(mesh.GeometryFrame.GeometryBasis);
 17941        var motion = new WideAxis3(Signed320.ExtendValue(Signed192.Raw(_displacement.X)),
 17942            Signed320.ExtendValue(Signed192.Raw(_displacement.Y)), Signed320.ExtendValue(Signed192.Raw(_displacement.Z))
 17943        if (WideDirection3d.Dot(plane, motion).IsZero)
 144            return false;
 45
 46        // Prepare has admitted both reference-center endpoints. Their crossing
 47        // selects an attempt; GJK still tests the actual hull, not its inspection center.
 17848        _ = source.GetCenterAnchor().TryGetPoint(out Vector3d start);
 17849        _ = Vector3d.TryAdd(start, _displacement, out Vector3d end);
 17850        var segment = new FixedSegment(start, end);
 37651        for (int corner = 1; corner < corners.Length - 1; corner++)
 52        {
 18553            var triangle = new FixedTriangle(vertices[corners[0]], vertices[corners[corner]], vertices[corners[corner + 
 18554            if (!WideTriangleRelations.TryGetSegmentIntersection(triangle, mesh.GeometryFrame, segment, out _, out _))
 55                continue;
 17556            if (TrySweepConvexTarget(source, new ConvexShape(target, Vector3d.Zero, convexPatch: true),
 17557                target, out hit, out numerator) && numerator > Fixed64.Zero)
 17358                return true;
 259            hit = default;
 260            numerator = default;
 261            return false;
 62        }
 363        return false;
 64    }
 65}

/home/runner/work/Gravitas/Gravitas/src/Gravitas/Queries/3D/Sweeps/ConvexSweepQueryWorker.cs

#LineLine coverage
 1//=======================================================================
 2// ConvexSweepQueryWorker.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;
 12using Gravitas.Colliders;
 13using Gravitas.CollisionHandling;
 14using SwiftCollections;
 15using SwiftCollections.Query;
 16
 17namespace Gravitas.Queries;
 18
 19
 20
 21/// <summary>
 22/// Performs deterministic translational convex-source sweeps against 3D query
 23/// targets using support-mapped conservative advancement.
 24/// </summary>
 25/// <content>Owns source preparation, conservative advancement and ordered target reduction.</content>
 26internal sealed partial class ConvexSweepQueryWorker
 27{
 28    private const int MaxGjkIterations = 32;
 29    private const int MaxConservativeAdvancementIterations = 32;
 130    private static readonly Fixed64 DistanceTolerance =
 131        Fixed64.FromFraction(1, 1_048_576);
 132    internal static readonly Fixed64 ContactTolerance = Fixed64.FromFraction(1, 4096);
 133    private static readonly SweepTriangleCandidateComparer SweepTriangleComparer = new();
 134    private static readonly FixedPointAnchor ZeroAnchor =
 135        new(
 136            Vector3d.Zero,
 137            FixedQuaternion.Identity,
 138            Vector3d.Zero);
 39
 3736540    private readonly SupportPoint[] _simplex = new SupportPoint[4];
 3736541    private readonly SwiftList<int> _triangleCandidates = new(16);
 3736542    private readonly SwiftList<SweepTriangleCandidate> _sweepTriangleCandidates = new(16);
 43    private readonly int _maxConservativeAdvancementIterations;
 44
 45    private LSCollider? _source;
 46    private ConvexShape _sourceShape;
 47    private bool _hasSource;
 48    private bool _requireClosingInitialContact;
 49    private ContinuousSurfaceTriangleAdmission? _admitTriangle;
 50    private WideDirection3d? _admissionNormal;
 51    private Vector3d _displacement;
 52    private Vector3d _outputDirection;
 53    private Vector3d _sweptSourceBoundsMin;
 54    private Vector3d _sweptSourceBoundsMax;
 55    private FixedPointAnchor _displacementAnchor;
 56    private FixedSegment _chord;
 57    private Fixed64 _length;
 58
 59    internal int LastMeshTriangleCandidateCount { get; private set; }
 60
 3736561    internal ConvexSweepQueryWorker(
 3736562        int maxConservativeAdvancementIterations = MaxConservativeAdvancementIterations) =>
 3736563        _maxConservativeAdvancementIterations = maxConservativeAdvancementIterations;
 64
 65    internal void ClearSource()
 66    {
 2796167        _source = null;
 2796168        _sourceShape = default;
 2796169        _hasSource = false;
 2796170        _admitTriangle = null;
 2796171    }
 72
 73    public void PrepareConvexMeshSource(LSMeshCollider source, Vector3d displacement)
 74    {
 10675        ChronicleGuard.ThrowIfNull(source, nameof(source));
 10676        ThrowIfConcaveSource(source);
 10577        Prepare(source, displacement);
 10578    }
 79
 80    public void PrepareCompoundSource(LSCompoundCollider source, Vector3d displacement)
 81    {
 3082        ChronicleGuard.ThrowIfNull(source, nameof(source));
 3083        Prepare(source, displacement);
 3084    }
 85
 86    public void PreparePrimitiveSource(LSCollider source, Vector3d displacement)
 87    {
 236088        ChronicleGuard.ThrowIfNull(source, nameof(source));
 236089        if (!ConvexColliderSupport.IsSupported(source))
 190            throw new NotSupportedException(
 191                $"Convex swept queries do not support {source.GetType().Name} sources.");
 92
 235993        Prepare(source, displacement);
 235994    }
 95
 96    public void PrepareCircleSlabSource(Vector3d center, Fixed64 radius, Fixed64 halfHeight, Vector3d displacement)
 97    {
 3298        ChronicleGuard.ThrowIfArgument(radius <= Fixed64.Zero, nameof(radius), "Circle-slab sweep radius must be greater
 3199        ChronicleGuard.ThrowIfArgument(halfHeight <= Fixed64.Zero, nameof(halfHeight), "Circle-slab sweep half-height mu
 30100        _source = null;
 30101        Prepare(ConvexShape.CreateCircleSlab(center, radius, halfHeight), displacement);
 30102    }
 103
 104    internal void PrepareSphereSource(
 105        Vector3d center,
 106        Fixed64 radius,
 107        Vector3d displacement)
 108    {
 22093109        ChronicleGuard.ThrowIfArgument(
 22093110            radius < Fixed64.Zero,
 22093111            nameof(radius),
 22093112            "Sphere sweep radius cannot be negative.");
 22093113        _source = null;
 22093114        Prepare(ConvexShape.CreateSphere(center, radius), displacement);
 22093115    }
 116
 117    public bool TrySweepPreparedSource(LSCollider target, out Physics3DHit hit,
 118        bool requireClosingInitialContact = false, ContinuousSurfaceTriangleAdmission? admitTriangle = null)
 119    {
 3888120        _requireClosingInitialContact = requireClosingInitialContact;
 3888121        _admitTriangle = admitTriangle;
 3888122        LastMeshTriangleCandidateCount = 0;
 3888123        hit = default;
 3888124        if (!_hasSource
 3888125            || _length <= Fixed64.Epsilon
 3888126            || !SweepBoundsUtility.OverlapsInclusive(_sweptSourceBoundsMin, _sweptSourceBoundsMax, target.BoundsMin, tar
 127        {
 139128            return false;
 129        }
 130
 3749131        if (_source is LSCompoundCollider compound)
 24132            return TrySweepCompoundSource(compound, target, out hit);
 133
 3725134        return TrySweepSourceShape(
 3725135            _sourceShape,
 3725136            target,
 3725137            out hit,
 3725138            out _);
 139    }
 140
 141    private void Prepare(LSCollider source, Vector3d displacement)
 142    {
 2494143        _source = source;
 2494144        Prepare(CreateColliderShape(source, Vector3d.Zero), displacement);
 2494145    }
 146
 147    private void Prepare(ConvexShape sourceShape, Vector3d displacement)
 148    {
 24617149        _sourceShape = sourceShape;
 24617150        _displacement = displacement;
 24617151        _displacementAnchor = new FixedPointAnchor(
 24617152            Vector3d.Zero,
 24617153            FixedQuaternion.Identity,
 24617154            displacement);
 24617155        _chord = new FixedSegment(Vector3d.Zero, displacement);
 24617156        _hasSource =
 24617157            Vector3d.TryGetMagnitude(displacement, out _length)
 24617158            && sourceShape.CanTranslateCenter(displacement);
 24617159        if (!_hasSource)
 160        {
 5161            _outputDirection = Vector3d.Zero;
 5162            return;
 163        }
 164
 24612165        _outputDirection =
 24612166            _length <= Fixed64.Epsilon
 24612167                ? Vector3d.Zero
 24612168                : displacement.Normalized;
 24612169        sourceShape.GetSourceBounds(out Vector3d sourceMin, out Vector3d sourceMax);
 170        // Bounds are broad-phase clips, not canonical pose coordinates. A
 171        // valid center may have support beyond a scalar face.
 24612172        SweepBoundsUtility.CreateSweptBounds(
 24612173            sourceMin,
 24612174            sourceMax,
 24612175            displacement,
 24612176            ContactTolerance,
 24612177            out _sweptSourceBoundsMin,
 24612178            out _sweptSourceBoundsMax);
 24612179    }
 180
 181    private bool TrySweepCompoundSource(LSCompoundCollider source, LSCollider target, out Physics3DHit hit)
 182    {
 24183        hit = default;
 24184        bool found = false;
 24185        Fixed64 closestNumerator = Fixed64.MaxValue;
 24186        int closestPartIndex = int.MaxValue;
 187
 112188        for (int i = 0; i < source.PartCount; i++)
 189        {
 32190            LSCollider part = source.GetPartCollider(i);
 32191            if (!TrySweepSourceShape(
 32192                    CreateColliderShape(part, Vector3d.Zero),
 32193                    target,
 32194                    out Physics3DHit candidate,
 32195                    out Fixed64 candidateNumerator)
 32196                || !ComesBeforeReducerCandidate(
 32197                    candidateNumerator,
 32198                    i,
 32199                    found,
 32200                    closestNumerator,
 32201                    closestPartIndex))
 202            {
 203                continue;
 204            }
 205
 23206            hit = candidate;
 23207            closestNumerator = candidateNumerator;
 23208            closestPartIndex = i;
 23209            found = true;
 210        }
 211
 24212        return found;
 213    }
 214
 215    private bool TrySweepSourceShape(
 216        ConvexShape sourceShape,
 217        LSCollider target,
 218        out Physics3DHit hit,
 219        out Fixed64 hitNumerator)
 220    {
 4282221        hit = default;
 4282222        hitNumerator = default;
 223
 4282224        if (!CanSweptSourceShapeReachTarget(sourceShape, target))
 6225            return false;
 226
 4276227        if (target is LSCompoundCollider compound)
 228        {
 520229            return TrySweepTargetCompound(
 520230                sourceShape,
 520231                compound,
 520232                out hit,
 520233                out hitNumerator);
 234        }
 235
 3756236        if (target is LSMeshCollider mesh && mesh.Mode == MeshColliderMode.Concave)
 237        {
 1132238            return TrySweepConcaveMeshTarget(
 1132239                sourceShape,
 1132240                mesh,
 1132241                out hit,
 1132242                out hitNumerator);
 243        }
 244
 2624245        return TrySweepConvexTarget(
 2624246            sourceShape,
 2624247            CreateColliderShape(target, Vector3d.Zero),
 2624248            target,
 2624249            out hit,
 2624250            out hitNumerator);
 251    }
 252
 253    private bool TrySweepTargetCompound(
 254        ConvexShape sourceShape,
 255        LSCompoundCollider compound,
 256        out Physics3DHit hit,
 257        out Fixed64 hitNumerator)
 258    {
 520259        hit = default;
 520260        hitNumerator = default;
 520261        bool found = false;
 520262        Fixed64 closestNumerator = Fixed64.MaxValue;
 520263        int closestPartIndex = int.MaxValue;
 264
 2090265        for (int i = 0; i < compound.PartCount; i++)
 266        {
 525267            LSCollider part = compound.GetPartCollider(i);
 525268            if (!TrySweepSourceShape(
 525269                    sourceShape,
 525270                    part,
 525271                    out Physics3DHit partHit,
 525272                    out Fixed64 partNumerator)
 525273                || !ComesBeforeReducerCandidate(
 525274                    partNumerator,
 525275                    i,
 525276                    found,
 525277                    closestNumerator,
 525278                    closestPartIndex))
 279            {
 280                continue;
 281            }
 282
 487283            hit = new Physics3DHit(compound, partHit.Anchor, partHit.Normal, partHit.Distance, partHit.Direction);
 487284            hitNumerator = partNumerator;
 487285            closestNumerator = partNumerator;
 487286            closestPartIndex = i;
 487287            found = true;
 288        }
 289
 520290        return found;
 291    }
 292
 293    private bool TrySweepConcaveMeshTarget(
 294        ConvexShape sourceShape,
 295        LSMeshCollider mesh,
 296        out Physics3DHit hit,
 297        out Fixed64 hitNumerator)
 298    {
 1132299        hit = default;
 1132300        hitNumerator = default;
 1132301        bool found = false;
 1132302        Fixed64 closestNumerator = Fixed64.MaxValue;
 1132303        int closestTriangleIndex = int.MaxValue;
 304
 1132305        CreateSweptSourceBoundsInMeshFrame(
 1132306            sourceShape,
 1132307            mesh,
 1132308            out Vector3d min,
 1132309            out Vector3d max);
 310
 1132311        mesh.Mesh.GetTrianglesInGeometryBounds(
 1132312            new FixedBoundVolume(min, max),
 1132313            _triangleCandidates);
 1132314        LastMeshTriangleCandidateCount += _triangleCandidates.Count;
 1132315        if (TrySweepCertifiedConvexPatch(sourceShape, mesh, out hit, out hitNumerator))
 173316            return true;
 959317        BuildOrderedSweepTriangleCandidates(sourceShape, mesh);
 318
 4884319        for (int i = 0; i < _sweepTriangleCandidates.Count; i++)
 320        {
 1485321            SweepTriangleCandidate sweepCandidate = _sweepTriangleCandidates[i];
 1485322            if (RemainingSweepTrianglesCannotBeat(
 1485323                sweepCandidate.LowerBoundNumerator,
 1485324                found,
 1485325                closestNumerator))
 326            {
 327                break;
 328            }
 329
 1483330            int triangleIndex = sweepCandidate.TriangleIndex;
 331            // Zero is the best possible distance; skip triangles that cannot win even then.
 1483332            if (!ComesBeforeReducerCandidate(Fixed64.Zero, triangleIndex,
 1483333                    found, closestNumerator, closestTriangleIndex))
 334                continue;
 1201335            ConvexShape triangle = CreateTriangleShape(mesh, triangleIndex);
 1201336            if (!TrySweepConvexTarget(
 1201337                    sourceShape,
 1201338                    triangle,
 1201339                    mesh,
 1201340                    out Physics3DHit candidate,
 1201341                    out Fixed64 candidateNumerator)
 1201342                || _admitTriangle != null && !_admitTriangle(triangleIndex, ref candidate, _admissionNormal)
 1201343                || !ComesBeforeReducerCandidate(
 1201344                    candidateNumerator,
 1201345                    triangleIndex,
 1201346                    found,
 1201347                    closestNumerator,
 1201348                    closestTriangleIndex))
 349            {
 350                continue;
 351            }
 352
 807353            hit = candidate;
 807354            hitNumerator = candidateNumerator;
 807355            closestNumerator = candidateNumerator;
 807356            closestTriangleIndex = triangleIndex;
 807357            found = true;
 358        }
 359
 959360        return found;
 361    }
 362
 363    private void BuildOrderedSweepTriangleCandidates(ConvexShape sourceShape, LSMeshCollider mesh)
 364    {
 959365        _sweepTriangleCandidates.FastClear();
 4892366        for (int i = 0; i < _triangleCandidates.Count; i++)
 367        {
 1487368            int triangleIndex = _triangleCandidates[i];
 1487369            ConvexShape triangle = CreateTriangleShape(mesh, triangleIndex);
 1487370            if (TryComputeSweepLowerBoundNumerator(
 1487371                    sourceShape,
 1487372                    triangle,
 1487373                    out Fixed64 lowerBoundNumerator))
 374            {
 1486375                _sweepTriangleCandidates.Add(
 1486376                    new SweepTriangleCandidate(
 1486377                        triangleIndex,
 1486378                        lowerBoundNumerator));
 379            }
 380        }
 381
 959382        _sweepTriangleCandidates.SortInPlace(SweepTriangleComparer);
 959383    }
 384
 385    [MethodImpl(MethodImplOptions.AggressiveInlining)]
 386    private bool TryComputeSweepLowerBoundNumerator(
 387        ConvexShape sourceShape,
 388        ConvexShape targetShape,
 389        out Fixed64 lowerBoundNumerator)
 390    {
 1487391        sourceShape.GetSourceBounds(
 1487392            out Vector3d sourceMin,
 1487393            out Vector3d sourceMax);
 1487394        targetShape.GetBounds(
 1487395            out Vector3d targetMin,
 1487396            out Vector3d targetMax);
 397
 1487398        Vector3d padding = Vector3d.One * ContactTolerance;
 1487399        sourceMin -= padding;
 1487400        sourceMax += padding;
 1487401        return TryComputeSweepLowerBoundNumerator(
 1487402            sourceMin,
 1487403            sourceMax,
 1487404            targetMin,
 1487405            targetMax,
 1487406            _displacement,
 1487407            _length,
 1487408            out lowerBoundNumerator);
 409    }
 410
 411    [MethodImpl(MethodImplOptions.AggressiveInlining)]
 412    internal static bool TryComputeSweepLowerBoundNumerator(
 413        Vector3d sourceMin,
 414        Vector3d sourceMax,
 415        Vector3d targetMin,
 416        Vector3d targetMax,
 417        Vector3d displacement,
 418        Fixed64 length,
 419        out Fixed64 lowerBoundNumerator)
 420    {
 1492421        lowerBoundNumerator = Fixed64.Zero;
 1492422        return IncludeAxisEntryNumerator(
 1492423                sourceMin.X,
 1492424                sourceMax.X,
 1492425                targetMin.X,
 1492426                targetMax.X,
 1492427                displacement.X,
 1492428                length,
 1492429                ref lowerBoundNumerator)
 1492430            && IncludeAxisEntryNumerator(
 1492431                sourceMin.Y,
 1492432                sourceMax.Y,
 1492433                targetMin.Y,
 1492434                targetMax.Y,
 1492435                displacement.Y,
 1492436                length,
 1492437                ref lowerBoundNumerator)
 1492438            && IncludeAxisEntryNumerator(
 1492439                sourceMin.Z,
 1492440                sourceMax.Z,
 1492441                targetMin.Z,
 1492442                targetMax.Z,
 1492443                displacement.Z,
 1492444                length,
 1492445                ref lowerBoundNumerator);
 446    }
 447
 448    [MethodImpl(MethodImplOptions.AggressiveInlining)]
 449    private static bool IncludeAxisEntryNumerator(
 450        Fixed64 sourceMin,
 451        Fixed64 sourceMax,
 452        Fixed64 targetMin,
 453        Fixed64 targetMax,
 454        Fixed64 displacement,
 455        Fixed64 length,
 456        ref Fixed64 entryNumerator)
 457    {
 4472458        if (sourceMax >= targetMin
 4472459            && sourceMin <= targetMax)
 460        {
 4390461            return true;
 462        }
 463
 464        Fixed64 gap;
 465        Fixed64 span;
 82466        if (sourceMax < targetMin
 82467            && displacement > Fixed64.Zero)
 468        {
 469            // Saturation is safe here: an unrepresentable positive gap is
 470            // necessarily farther than the representable sweep span.
 73471            gap = targetMin - sourceMax;
 73472            span = displacement;
 473        }
 9474        else if (sourceMin > targetMax
 9475            && displacement < Fixed64.Zero)
 476        {
 7477            gap = sourceMin - targetMax;
 478            // Prepare admits only representable chord magnitudes, so no
 479            // component can be Fixed64.MinValue here.
 7480            span = -displacement;
 481        }
 482        else
 483        {
 2484            return false;
 485        }
 486
 80487        if (gap > span)
 1488            return false;
 489
 490        // With 0 <= gap <= span, the fused result is bounded by the already
 491        // representable chord length.
 79492        _ = Fixed64.TryMultiplyDivide(
 79493            length,
 79494            gap,
 79495            span,
 79496            out Fixed64 axisNumerator);
 497
 498        // Fused conversion keeps u = numerator / chord length exact until the
 499        // final Q32.32 rounding. One raw unit makes that rounded result a
 500        // conservative lower bound without moving an underflowed zero.
 79501        axisNumerator = FixedMath.Max(
 79502            Fixed64.Zero,
 79503            axisNumerator - Fixed64.Epsilon);
 504
 79505        if (axisNumerator > entryNumerator)
 62506            entryNumerator = axisNumerator;
 79507        return true;
 508    }
 509
 510    [MethodImpl(MethodImplOptions.AggressiveInlining)]
 511    internal static bool RemainingSweepTrianglesCannotBeat(
 512        Fixed64 candidateLowerBoundNumerator,
 513        bool found,
 514        Fixed64 closestNumerator)
 515    {
 1489516        if (!found)
 1181517            return false;
 518
 308519        return candidateLowerBoundNumerator
 308520            > closestNumerator + Fixed64.Epsilon;
 521    }
 522
 523    private bool TrySweepConvexTarget(
 524        ConvexShape sourceShape,
 525        ConvexShape targetShape,
 526        LSCollider targetCollider,
 527        out Physics3DHit hit,
 528        out Fixed64 hitNumerator)
 529    {
 4000530        hit = default;
 4000531        hitNumerator = default;
 4000532        _admissionNormal = null;
 533        // u is retained as travelNumerator / _length. Keeping the common
 534        // denominator exact preserves small chord components and physical
 535        // distance resolution without exposing wide arithmetic.
 4000536        Fixed64 travelNumerator = Fixed64.Zero;
 4000537        Vector3d normal = Vector3d.Zero;
 4000538        GjkResult result = default;
 539
 8914540        for (int i = 0; i < _maxConservativeAdvancementIterations; i++)
 541        {
 4451542            ConvexShape movedSource =
 4451543                sourceShape.WithSourceOffset(
 4451544                    GetChordOffset(travelNumerator));
 4451545            result = ComputeDistance(movedSource, targetShape);
 4451546            if (result.Intersects || result.Distance <= ContactTolerance)
 547            {
 3764548                ContactAnchor anchor = ResolveHitAnchor(
 3764549                    targetShape,
 3764550                    targetCollider,
 3764551                    movedSource,
 3764552                    result,
 3764553                    out Vector3d point,
 3764554                    out bool hasMaterializedPoint,
 3764555                    out bool hasRefinedSurfaceNormal);
 3764556                Vector3d hitNormal = ResolveHitNormal(
 3764557                    targetShape,
 3764558                    targetCollider,
 3764559                    point,
 3764560                    result.Normal,
 3764561                    normal,
 3764562                    hasRefinedSurfaceNormal,
 3764563                    hasMaterializedPoint,
 3764564                    preserveOrientation: travelNumerator == Fixed64.Zero
 3764565                        && targetCollider is LSMeshCollider);
 566
 3764567                if (travelNumerator == Fixed64.Zero)
 568                {
 3425569                    if (targetCollider is LSMeshCollider)
 570                    {
 2057571                        if (!TryResolveInitialMeshContact(sourceShape, targetShape, result,
 2057572                                ref anchor, ref hitNormal))
 1573                            return false;
 574                    }
 1368575                    else if (_requireClosingInitialContact && hasMaterializedPoint)
 5576                        hitNormal = targetCollider.GetNormalAtPoint(point);
 577
 578                }
 7187579                if (travelNumerator == Fixed64.Zero) _admissionNormal = null;
 3763580                RetainCapsuleNormal(movedSource, targetShape, anchor, ref hitNormal);
 581                // Admission precedes leaf/triangle reduction so a separating
 582                // initial feature cannot hide a later closing sibling.
 583                // A declared surface selects closing response from its full
 584                // native region; one triangle's tied exit cannot preempt it.
 3763585                if (travelNumerator == Fixed64.Zero && _requireClosingInitialContact && _admitTriangle == null
 3932586                    && Vector3d.Dot(_displacement, hitNormal) >= -Fixed64.Epsilon) return false;
 3594587                hit = new Physics3DHit(
 3594588                    targetCollider,
 3594589                    anchor,
 3594590                    hitNormal,
 3594591                    travelNumerator,
 3594592                    _outputDirection);
 3594593                hitNumerator = travelNumerator;
 3594594                return true;
 595            }
 596
 687597            normal = result.Normal;
 598            // Projecting a representable chord onto a unit normal is bounded
 599            // by the admitted chord length.
 687600            _ = _displacementAnchor.TryGetProjectedOffsetFrom(
 687601                ZeroAnchor,
 687602                -normal,
 687603                out Fixed64 closingPerFraction);
 687604            if (closingPerFraction <= Fixed64.Epsilon)
 605            {
 138606                return TryResolveEndpointBracket(
 138607                    sourceShape,
 138608                    targetShape,
 138609                    targetCollider,
 138610                    travelNumerator,
 138611                    out hit,
 138612                    out hitNumerator);
 613            }
 549614            Fixed64 boundedDistance =
 549615                FixedMath.Min(
 549616                    result.Distance,
 549617                    closingPerFraction);
 618            // Capping the advancement ratio at one preserves the endpoint
 619            // decision while keeping the fused result inside the chord length.
 549620            _ = Fixed64.TryMultiplyDivide(
 549621                boundedDistance,
 549622                _length,
 549623                closingPerFraction,
 549624                out Fixed64 stepNumerator);
 549625            Fixed64 nextTravelNumerator =
 549626                travelNumerator + stepNumerator;
 549627            if (nextTravelNumerator >= _length)
 628            {
 629                // A capped full-length step also covers every overshoot. Reuse
 630                // endpoint materialization without refining a zero-width bracket.
 92631                return TryResolveEndpointBracket(sourceShape, targetShape, targetCollider,
 92632                    _length, out hit, out hitNumerator, normal);
 633            }
 634
 457635            travelNumerator = nextTravelNumerator;
 636        }
 637
 6638        return TryResolveEndpointBracket(
 6639            sourceShape,
 6640            targetShape,
 6641            targetCollider,
 6642            travelNumerator,
 6643            out hit,
 6644            out hitNumerator);
 645    }
 646
 647    private bool TryResolveEndpointBracket(
 648        ConvexShape sourceShape,
 649        ConvexShape targetShape,
 650        LSCollider targetCollider,
 651        Fixed64 lowerNumerator,
 652        out Physics3DHit hit,
 653        out Fixed64 hitNumerator,
 654        Vector3d fallbackNormal = default)
 655    {
 236656        hit = default;
 236657        hitNumerator = default;
 236658        Fixed64 upperNumerator = _length;
 236659        ConvexShape upperSource =
 236660            sourceShape.WithSourceOffset(_displacement);
 236661        GjkResult upperResult =
 236662            ComputeDistance(upperSource, targetShape);
 236663        if (!upperResult.Intersects
 236664            && upperResult.Distance > ContactTolerance)
 665        {
 226666            return false;
 667        }
 668
 10669        for (int iteration = 0;
 12670            iteration < _maxConservativeAdvancementIterations
 12671            && upperNumerator - lowerNumerator > DistanceTolerance;
 2672            iteration++)
 673        {
 2674            Fixed64 middleNumerator =
 2675                FixedMath.Midpoint(
 2676                    lowerNumerator,
 2677                    upperNumerator);
 2678            ConvexShape middleSource =
 2679                sourceShape.WithSourceOffset(
 2680                    GetChordOffset(middleNumerator));
 2681            GjkResult middleResult =
 2682                ComputeDistance(middleSource, targetShape);
 2683            if (middleResult.Intersects
 2684                || middleResult.Distance <= ContactTolerance)
 685            {
 1686                upperNumerator = middleNumerator;
 1687                upperSource = middleSource;
 1688                upperResult = middleResult;
 689            }
 690            else
 691            {
 1692                lowerNumerator = middleNumerator;
 693            }
 694        }
 695
 10696        ContactAnchor anchor = ResolveHitAnchor(
 10697            targetShape,
 10698            targetCollider,
 10699            upperSource,
 10700            upperResult,
 10701            out Vector3d point,
 10702            out bool hasMaterializedPoint,
 10703            out bool hasRefinedSurfaceNormal);
 10704        Vector3d hitNormal = ResolveHitNormal(
 10705            targetShape,
 10706            targetCollider,
 10707            point,
 10708            upperResult.Normal,
 10709            fallbackNormal,
 10710            hasRefinedSurfaceNormal,
 10711            hasMaterializedPoint);
 10712        RetainCapsuleNormal(upperSource, targetShape, anchor, ref hitNormal);
 10713        hit = new Physics3DHit(
 10714            targetCollider,
 10715            anchor,
 10716            hitNormal,
 10717            upperNumerator,
 10718            _outputDirection);
 10719        hitNumerator = upperNumerator;
 10720        return true;
 721    }
 722
 723    private void RetainCapsuleNormal(in ConvexShape source, in ConvexShape target, in ContactAnchor anchor, ref Vector3d
 724    {
 7410725        if (_admitTriangle == null || !source.TryGetCapsuleTriangleNormalAxis(target, anchor.GeometryPoint, out WideDire
 136726        _admissionNormal = axis;
 136727        normal = axis.GetNormal(false);
 136728    }
 729
 730    [MethodImpl(MethodImplOptions.AggressiveInlining)]
 731    private Vector3d GetChordOffset(Fixed64 numerator) =>
 4453732        _chord.GetPointAtDistance(
 4453733            numerator,
 4453734            _length);
 735
 736    private ContactAnchor ResolveHitAnchor(
 737        ConvexShape targetShape,
 738        LSCollider targetCollider,
 739        ConvexShape movedSource,
 740        GjkResult result,
 741        out Vector3d point,
 742        out bool hasMaterializedPoint,
 743        out bool hasRefinedSurfaceNormal)
 744    {
 3774745        hasMaterializedPoint = true;
 3774746        hasRefinedSurfaceNormal = false;
 3774747        if (targetCollider is LSSphereCollider
 3774748            && movedSource.TryGetClosestPointOnSurface(
 3774749                targetCollider.Center,
 3774750                out Vector3d sourcePoint)
 3774751            && Vector3d.TrySubtract(
 3774752                sourcePoint,
 3774753                targetCollider.Center,
 3774754                out Vector3d centerToSource)
 3774755            && centerToSource.MagnitudeSquared > Fixed64.Epsilon)
 756        {
 757            // A sphere's closest pair is defined by its center and the closest
 758            // source feature. Refining that feature removes arbitrary support
 759            // tie bias without changing the conservative TOI.
 55760            FixedPointAnchor sphereAnchor =
 55761                ConvexColliderSupport.GetSupportAnchor(
 55762                    targetCollider,
 55763                    centerToSource,
 55764                    Vector3d.Zero);
 55765            hasMaterializedPoint = sphereAnchor.TryGetPoint(out point);
 55766            hasRefinedSurfaceNormal = hasMaterializedPoint;
 55767            return new ContactAnchor(sphereAnchor);
 768        }
 769
 3719770        FixedPointAnchor movedSourceCenter = movedSource.GetCenterAnchor();
 3719771        FixedPointAnchor targetCenter = targetShape.GetCenterAnchor();
 3719772        if (!targetShape.IsConvexPatch && movedSourceCenter.TryGetOffsetFrom(
 3719773                targetCenter,
 3719774                out Vector3d centerDifference)
 3719775            && centerDifference.MagnitudeSquared <= Fixed64.Epsilon)
 776        {
 12777            FixedPointAnchor fallbackAnchor =
 12778                targetShape.GetFallbackSurfaceAnchor(-_displacement);
 12779            hasMaterializedPoint = fallbackAnchor.TryGetPoint(out point);
 12780            return new ContactAnchor(fallbackAnchor);
 781        }
 782
 783        // GJK's target witness identifies the feature that stopped the sweep.
 784        // Center-to-center projection can select an unrelated feature for long,
 785        // offset shapes and therefore produce a non-physical response normal.
 3707786        if (!result.PointB.TryGetPoint(out point))
 787        {
 5788            point = default;
 5789            hasMaterializedPoint = false;
 5790            return new ContactAnchor(result.PointB);
 791        }
 792
 3702793        return new ContactAnchor(result.PointB);
 794    }
 795
 796    private Vector3d ResolveHitNormal(
 797        ConvexShape targetShape,
 798        LSCollider targetCollider,
 799        Vector3d point,
 800        Vector3d resultNormal,
 801        Vector3d fallbackNormal,
 802        bool hasRefinedSurfaceNormal,
 803        bool hasMaterializedPoint,
 804        bool preserveOrientation = false)
 805    {
 3774806        Vector3d planarNormal = Vector3d.Zero;
 807        // Retain the selected triangle plane before the hit policy normalizes
 808        // its already rounded inspection normal. Unknown/initial replacements
 809        // cannot authorize a continuity exclusion.
 3774810        if (_admitTriangle != null)
 811        {
 812            // The only production hook targets declared concave meshes;
 813            // its plane does not depend on a scalar world witness. Such a
 814            // witness may lie beyond a scalar face despite bounded geometry.
 815            System.Diagnostics.Debug.Assert(!hasRefinedSurfaceNormal);
 153816            targetShape.TryGetPlanarSurfaceNormal(point, out planarNormal);
 153817            WideDirection3d plane = targetShape.GetTriangleNormalAxis();
 153818            _admissionNormal = !preserveOrientation && Vector3d.CompareProjection(planarNormal, Vector3d.Zero, _displace
 819        }
 3621820        else if (hasMaterializedPoint)
 3616821            targetShape.TryGetPlanarSurfaceNormal(point, out planarNormal);
 3774822        return ConvexSweepHitPolicy.ResolveHitNormal(
 3774823            targetCollider,
 3774824            point,
 3774825            resultNormal,
 3774826            fallbackNormal,
 3774827            preserveOrientation ? Vector3d.Zero : _displacement,
 3774828            planarNormal,
 3774829            hasRefinedSurfaceNormal,
 3774830            hasMaterializedPoint);
 831    }
 832
 833    private static ConvexShape CreateColliderShape(LSCollider collider, Vector3d offset)
 834    {
 5150835        return new ConvexShape(collider, offset);
 836    }
 837
 838    private static ConvexShape CreateTriangleShape(LSMeshCollider mesh, int triangleIndex)
 839    {
 2688840        mesh.Mesh.GetGeometryTriangleVertices(
 2688841            triangleIndex,
 2688842            out Vector3d first,
 2688843            out Vector3d second,
 2688844            out Vector3d third);
 2688845        return new ConvexShape(mesh, triangleIndex, first, second, third);
 846    }
 847
 848    private static void ThrowIfConcaveSource(LSMeshCollider source)
 849    {
 106850        if (source.Mode == MeshColliderMode.Concave)
 1851            throw CreateConcaveSourceException(source);
 105852    }
 853
 854    private void CreateSweptSourceBoundsInMeshFrame(
 855        ConvexShape sourceShape,
 856        LSMeshCollider mesh,
 857        out Vector3d min,
 858        out Vector3d max)
 859    {
 1132860        if (!mesh.Mesh.GeometryFrame.IsRigid)
 861        {
 33862            _ = sourceShape.TryGetBoundsRelativeTo(mesh.Mesh.Origin, FixedQuaternion.Identity,
 33863                out Vector3d worldMin, out Vector3d worldMax);
 33864            SweepBoundsUtility.CreateSweptBounds(worldMin, worldMax, _displacement,
 33865                ContactTolerance, out worldMin, out worldMax);
 33866            bool represented = ScaledRigidFrame3d.Rigid(mesh.Mesh.Origin, FixedQuaternion.Identity)
 33867                .TryGetBoundsIn(worldMin, worldMax, mesh.Mesh.GeometryFrame, out FixedBoundBox bounds);
 868            System.Diagnostics.Debug.Assert(represented);
 66869            min = bounds.Min; max = bounds.Max;
 33870            return;
 871        }
 872        // Concave-target dispatch receives only the source's committed shape;
 873        // iterative chord offsets are introduced after broad-phase collection.
 1099874        _ = sourceShape.TryGetBoundsRelativeTo(
 1099875            mesh.Mesh.Origin,
 1099876            mesh.Mesh.Rotation,
 1099877            out Vector3d sourceMin,
 1099878            out Vector3d sourceMax);
 879
 880        // Prepare admitted the chord magnitude, so a unit rotation preserves
 881        // representability.
 1099882        _ = mesh.Mesh.Rotation.Inverse().TryRotate(
 1099883            _displacement,
 1099884            out Vector3d localDisplacement);
 1099885        SweepBoundsUtility.CreateSweptBounds(
 1099886            sourceMin,
 1099887            sourceMax,
 1099888            localDisplacement,
 1099889            ContactTolerance,
 1099890            out min,
 1099891            out max);
 1099892    }
 893
 894    private bool CanSweptSourceShapeReachTarget(ConvexShape sourceShape, LSCollider target)
 895    {
 4282896        if (!sourceShape.CanTranslateCenter(_displacement))
 1897            return false;
 898
 4281899        sourceShape.GetSourceBounds(out Vector3d sourceMin, out Vector3d sourceMax);
 4281900        SweepBoundsUtility.CreateSweptBounds(
 4281901            sourceMin,
 4281902            sourceMax,
 4281903            _displacement,
 4281904            ContactTolerance,
 4281905            out Vector3d min,
 4281906            out Vector3d max);
 4281907        return SweepBoundsUtility.OverlapsInclusive(
 4281908            min,
 4281909            max,
 4281910            target.BoundsMin,
 4281911            target.BoundsMax);
 912    }
 913
 914    [MethodImpl(MethodImplOptions.AggressiveInlining)]
 915    private static bool ComesBeforeReducerCandidate(
 916        Fixed64 hitNumerator,
 917        int candidateOrdinal,
 918        bool found,
 919        Fixed64 closestNumerator,
 920        int closestOrdinal)
 921    {
 2817922        if (!found)
 2487923            return true;
 924
 330925        int numeratorCompare =
 330926            hitNumerator.CompareTo(closestNumerator);
 330927        if (numeratorCompare != 0)
 35928            return numeratorCompare < 0;
 929
 295930        return candidateOrdinal < closestOrdinal;
 931    }
 932
 933    private static ArgumentException CreateConcaveSourceException(LSMeshCollider source) =>
 1934        new("Concave mesh sources are not supported by swept query APIs. Use an LSCompoundCollider built from authored c
 935}

/home/runner/work/Gravitas/Gravitas/src/Gravitas/Queries/3D/Sweeps/ConvexSweepQueryWorker.Gjk.cs

#LineLine coverage
 1//=======================================================================
 2// ConvexSweepQueryWorker.Gjk.cs
 3//=======================================================================
 4// MIT License, Copyright (c) 2026-present David Oravsky (mrdav30)
 5// See LICENSE file in the project root for full license information.
 6//=======================================================================
 7
 8using FixedMathSharp;
 9using FixedMathSharp.Geometry;
 10
 11namespace Gravitas.Queries;
 12
 13/// <content>Computes convex separation and witness points using GJK support queries.</content>
 14internal sealed partial class ConvexSweepQueryWorker
 15{
 16    private GjkResult ComputeDistance(ConvexShape sourceShape, ConvexShape targetShape)
 17    {
 18        const int workingShift = 2;
 19        // Quartering both endpoints bounds every two-term Minkowski component
 20        // below half the scalar domain. Any convex simplex combination then
 21        // has a representable 3D magnitude without a lossy retry.
 468922        Fixed64 workingScale = GjkSimplexScale.GetCoordinateScale(workingShift);
 468923        Fixed64 workingDistanceTolerance = DistanceTolerance * workingScale;
 468924        int simplexCount = 0;
 468925        FixedPointAnchor sourceCenter = sourceShape.GetCenterAnchor();
 468926        FixedPointAnchor targetCenter = targetShape.GetCenterAnchor();
 27        // The shift was selected from bounds containing both centers, so this
 28        // scaled difference is representable by construction.
 468929        _ = targetCenter.TryGetScaledOffsetFrom(
 468930            sourceCenter,
 468931            workingScale,
 468932            out Vector3d direction);
 468933        if (direction == Vector3d.Zero)
 34        {
 1735            if (sourceShape.ContainsCenter && targetShape.ContainsCenter)
 36            {
 937                return GjkResult.CreateIntersection(
 938                    sourceCenter,
 939                    targetCenter);
 40            }
 41
 842            direction = Vector3d.Right;
 43        }
 44
 468045        ClosestSimplexResult closest = default;
 468046        Fixed64 workingDistance = Fixed64.MaxValue;
 47
 4365648        for (int i = 0; i < MaxGjkIterations; i++)
 49        {
 2168050            SupportPoint support = CreateSupportPoint(
 2168051                sourceShape,
 2168052                targetShape,
 2168053                direction,
 2168054                workingScale);
 2168055            if (simplexCount > 0)
 56            {
 57                // A small closest-distance improvement does not certify convergence
 58                // on a thin simplex. Bound the remaining error by its support plane.
 1700059                Signed192 gap = WideGeometry.GetDifferenceDotProduct3D(
 1700060                    closest.Point.X, Fixed64.Zero, closest.Point.Y, Fixed64.Zero, closest.Point.Z, Fixed64.Zero,
 1700061                    closest.Point.X, support.Point.X, closest.Point.Y, support.Point.Y, closest.Point.Z, support.Point.Z
 1700062                if (WideArithmetic.SubtractSigned192(gap,
 1700063                        Fixed64.GetExactRawProduct(workingDistanceTolerance, workingDistance)).Sign <= 0)
 64                    break;
 65            }
 2077866            if (ContainsSupportPoint(_simplex, simplexCount, support.Point))
 67                break;
 68
 2077169            ClosestSimplexResult previousClosest = closest;
 2077170            _simplex[simplexCount++] = support;
 2077171            closest = SolveClosestSimplex(_simplex, ref simplexCount, workingScale);
 2077172            _ = Vector3d.TryGetMagnitude(closest.Point, out workingDistance);
 2077173            if (closest.Intersects)
 74            {
 75                // Tetrahedron entry follows a valid one-to-three point simplex.
 76                // Preserve that same-pose closest pair as the deterministic
 77                // surface witness for the intersection result.
 78                // Tetrahedron entry leaves the preceding three support points intact.
 98679                previousClosest.GetWitnesses(_simplex, out FixedPointAnchor previousA, out FixedPointAnchor previousB);
 98680                return GjkResult.CreateIntersection(previousA, previousB);
 81            }
 82
 1978583            if (workingDistance <= workingDistanceTolerance)
 84            {
 263785                closest.GetWitnesses(_simplex, out FixedPointAnchor pointA, out FixedPointAnchor pointB);
 263786                return GjkResult.CreateIntersection(pointA, pointB);
 87            }
 88
 1714889            direction = -closest.Point;
 90        }
 91
 105792        Fixed64 distance = GjkSimplexScale.RestoreDistance(workingDistance, workingShift);
 105793        Vector3d normal = closest.Point.Normalized;
 105794        closest.GetWitnesses(_simplex, out FixedPointAnchor finalA, out FixedPointAnchor finalB);
 105795        return new GjkResult(false, distance, finalA, finalB, normal);
 96    }
 97
 98    private static SupportPoint CreateSupportPoint(
 99        ConvexShape sourceShape,
 100        ConvexShape targetShape,
 101        Vector3d direction,
 102        Fixed64 workingScale)
 103    {
 21680104        Vector3d supportDirection = direction.Normalized;
 105        // Interior face supports avoid artificial simplex dimensions; contact
 106        // witnesses keep their established feature-selection policy.
 21680107        FixedPointAnchor pointA =
 21680108            sourceShape.GetSupportAnchor(supportDirection, preferAxialCenter: true);
 21680109        FixedPointAnchor pointB =
 21680110            targetShape.GetSupportAnchor(-supportDirection, preferAxialCenter: true);
 111        // The same bounds-selected shift covers every enclosed support pair.
 21680112        _ = pointA.TryGetScaledOffsetFrom(
 21680113            pointB,
 21680114            workingScale,
 21680115            out Vector3d difference);
 21680116        return new SupportPoint(pointA, pointB, difference);
 117    }
 118
 119    private static ClosestSimplexResult SolveClosestSimplex(
 120        SupportPoint[] simplex,
 121        ref int count,
 122        Fixed64 workingScale)
 123    {
 20771124        if (count == 1)
 4680125            return ClosestSimplexResult.FromWeights(simplex, 1, Fixed64.One, Fixed64.Zero, Fixed64.Zero, Fixed64.Zero);
 126
 16091127        if (count == 2)
 4569128            return ReduceSegment(simplex, ref count, workingScale);
 129
 11522130        if (count == 3)
 7553131            return ReduceTriangle(simplex, ref count);
 132
 3969133        return ReduceTetrahedron(simplex, ref count);
 134    }
 135}

/home/runner/work/Gravitas/Gravitas/src/Gravitas/Queries/3D/Sweeps/ConvexSweepQueryWorker.InitialContacts.cs

#LineLine coverage
 1//=======================================================================
 2// ConvexSweepQueryWorker.InitialContacts.cs
 3//=======================================================================
 4// MIT License, Copyright (c) 2026-present David Oravsky (mrdav30)
 5// See LICENSE file in the project root for full license information.
 6//=======================================================================
 7
 8using FixedMathSharp;
 9using FixedMathSharp.Geometry;
 10using Gravitas.CollisionHandling;
 11
 12namespace Gravitas.Queries;
 13
 14/// <content>Preserves geometric initial mesh contact normals before compound or triangle reduction.</content>
 15internal sealed partial class ConvexSweepQueryWorker
 16{
 17    internal static bool TryResolveInitialMeshContact(
 18        ConvexShape sourceShape, ConvexShape targetShape, GjkResult result,
 19        ref ContactAnchor anchor, ref Vector3d normal)
 20    {
 206021        if (!result.Intersects)
 22        {
 23            // An exact face or boundary contact takes precedence over a tolerant
 24            // GJK witness; a genuinely disjoint near-contact retains its gap axis.
 1425            if (targetShape.TryGetInitialMeshContactNormal(sourceShape, allowHullFaceExit: false,
 1426                    out FixedPointAnchor faceAnchor, out Vector3d faceNormal))
 27            {
 428                anchor = new ContactAnchor(faceAnchor);
 429                normal = faceNormal;
 430                return true;
 31            }
 32            // Disjoint contacts inside the query tolerance retain the actual
 33            // closest-feature separation axis, including edges and vertices.
 1034            normal = result.Normal;
 1035            return true;
 36        }
 37
 204638        if (anchor.TryGetWorldPoint(out Vector3d point)
 204639            && targetShape.TryGetPlanarSurfaceNormal(point, out Vector3d planeNormal))
 40        {
 204141            planeNormal = targetShape.GetInitialExitNormal(sourceShape, planeNormal);
 204142            FixedPointAnchor targetSupport = targetShape.GetSupportAnchor(planeNormal);
 204143            FixedPointAnchor sourceSupport = sourceShape.GetSupportAnchor(-planeNormal);
 204144            if (WidePointAnchor3d.CompareProjectedOffsets(
 204145                    targetSupport, sourceSupport, ZeroAnchor, ZeroAnchor, planeNormal) <= 0)
 46            {
 47                // A supporting plane with no positive overlap certifies
 48                // touching/separation even at an apex or triangle seam. Keep
 49                // that plane instead of an equally touching oblique edge axis
 50                // which could mistake planar tangent motion for approach.
 23451                normal = planeNormal;
 23452                return true;
 53            }
 54        }
 55
 181256        if (targetShape.TryGetInitialMeshContactNormal(sourceShape, allowHullFaceExit: true,
 181257                out FixedPointAnchor targetAnchor, out Vector3d contactNormal))
 58        {
 180659            anchor = new ContactAnchor(targetAnchor);
 180660            normal = contactNormal;
 180661            return true;
 62        }
 63
 664        Vector3d witnessNormal = WidePointAnchor3d.GetDirection(result.PointA, result.PointB);
 665        if (witnessNormal != Vector3d.Zero
 666            && WidePointAnchor3d.CompareProjectedOffsets(
 667                targetShape.GetSupportAnchor(witnessNormal),
 668                sourceShape.GetSupportAnchor(-witnessNormal),
 669                ZeroAnchor, ZeroAnchor, witnessNormal) <= 0)
 70        {
 71            // GJK's bounded working scale can collapse a one-raw gap. Its
 72            // retained anchors still provide a separating feature direction;
 73            // admit it only when the full support intervals certify that axis.
 374            normal = witnessNormal;
 375            return true;
 76        }
 77
 78        // A bounded GJK intersection alone does not establish a valid initial
 79        // normal. Do not fabricate a contact when neither complete geometry
 80        // nor the retained support witnesses can certify it.
 381        return false;
 82    }
 83}

/home/runner/work/Gravitas/Gravitas/src/Gravitas/Queries/3D/Sweeps/ConvexSweepQueryWorker.Simplex.cs

#LineLine coverage
 1//=======================================================================
 2// ConvexSweepQueryWorker.Simplex.cs
 3//=======================================================================
 4// MIT License, Copyright (c) 2026-present David Oravsky (mrdav30)
 5// See LICENSE file in the project root for full license information.
 6//=======================================================================
 7
 8using System;
 9using FixedMathSharp;
 10using FixedMathSharp.Geometry;
 11
 12namespace Gravitas.Queries;
 13
 14/// <content>Reduces GJK simplices to their closest features and computes interpolation weights.</content>
 15internal sealed partial class ConvexSweepQueryWorker
 16{
 17    private static ClosestSimplexResult ReduceSegment(
 18        SupportPoint[] simplex,
 19        ref int count,
 20        Fixed64 workingScale)
 21    {
 456922        SupportPoint a = simplex[0];
 456923        SupportPoint b = simplex[1];
 456924        Span<Vector3d> scaled = stackalloc Vector3d[2];
 456925        scaled[0] = a.Point;
 456926        scaled[1] = b.Point;
 456927        Fixed64 productScale = GjkSimplexScale.ScaleForProducts(scaled);
 456928        Vector3d scaledA = scaled[0];
 456929        Vector3d ab = scaled[1] - scaledA;
 456930        Fixed64 denominator = ab.MagnitudeSquared;
 456931        Fixed64 workingScaleSqr = workingScale * workingScale;
 456932        Fixed64 productScaleSqr = productScale * productScale;
 456933        Fixed64 denominatorTolerance = Fixed64.Epsilon * workingScaleSqr * productScaleSqr;
 456934        Fixed64 t = denominator <= denominatorTolerance
 456935            ? Fixed64.Zero
 456936            : FixedMath.Clamp(-Vector3d.Dot(scaledA, ab) / denominator, Fixed64.Zero, Fixed64.One);
 37
 456938        if (t <= Fixed64.Epsilon)
 39        {
 6240            simplex[0] = a;
 6241            count = 1;
 6242            return ClosestSimplexResult.FromWeights(simplex, count, Fixed64.One, Fixed64.Zero, Fixed64.Zero, Fixed64.Zer
 43        }
 44
 450745        if (t >= Fixed64.One - Fixed64.Epsilon)
 46        {
 5547            simplex[0] = b;
 5548            count = 1;
 5549            return ClosestSimplexResult.FromWeights(simplex, count, Fixed64.One, Fixed64.Zero, Fixed64.Zero, Fixed64.Zer
 50        }
 51
 445252        simplex[0] = a;
 445253        simplex[1] = b;
 445254        count = 2;
 445255        return ClosestSimplexResult.FromWeights(simplex, count, Fixed64.One - t, t, Fixed64.Zero, Fixed64.Zero);
 56    }
 57
 58    private static ClosestSimplexResult ReduceTriangle(SupportPoint[] simplex, ref int count)
 59    {
 755360        TriangleWeights weights = ClosestPointOnTriangleToOrigin(simplex[0].Point, simplex[1].Point, simplex[2].Point);
 755361        return ReduceByWeights(simplex, ref count, weights.A, weights.B, weights.C, Fixed64.Zero);
 62    }
 63
 64    private static ClosestSimplexResult ReduceTetrahedron(
 65        SupportPoint[] simplex,
 66        ref int count)
 67    {
 396968        if (IsOriginInsideTetrahedron(
 396969                simplex[0].Point,
 396970                simplex[1].Point,
 396971                simplex[2].Point,
 396972                simplex[3].Point))
 73        {
 98674            count = 4;
 98675            return ClosestSimplexResult.Intersection;
 76        }
 77
 298378        Vector3d bestPoint = default;
 298379        bool hasBest = false;
 298380        Span<int> bestIndices = stackalloc int[3];
 298381        Span<Fixed64> bestWeights = stackalloc Fixed64[3];
 298382        Span<Fixed64> weights = stackalloc Fixed64[3];
 83
 298384        EvaluateFace(simplex, 0, 1, 2, ref bestPoint, ref hasBest, bestIndices, bestWeights, weights);
 298385        EvaluateFace(simplex, 0, 3, 1, ref bestPoint, ref hasBest, bestIndices, bestWeights, weights);
 298386        EvaluateFace(simplex, 0, 2, 3, ref bestPoint, ref hasBest, bestIndices, bestWeights, weights);
 298387        EvaluateFace(simplex, 1, 3, 2, ref bestPoint, ref hasBest, bestIndices, bestWeights, weights);
 88
 298389        SupportPoint first = simplex[bestIndices[0]];
 298390        SupportPoint second = simplex[bestIndices[1]];
 298391        SupportPoint third = simplex[bestIndices[2]];
 298392        simplex[0] = first;
 298393        simplex[1] = second;
 298394        simplex[2] = third;
 298395        count = 3;
 298396        return ClosestSimplexResult.FromWeights(simplex, count, bestWeights[0], bestWeights[1], bestWeights[2], Fixed64.
 97    }
 98
 99    private static void EvaluateFace(
 100        SupportPoint[] simplex,
 101        int first,
 102        int second,
 103        int third,
 104        ref Vector3d bestPoint,
 105        ref bool hasBest,
 106        Span<int> bestIndices,
 107        Span<Fixed64> bestWeights,
 108        Span<Fixed64> weights)
 109    {
 11932110        TriangleWeights triangleWeights = ClosestPointOnTriangleToOrigin(
 11932111            simplex[first].Point,
 11932112            simplex[second].Point,
 11932113            simplex[third].Point);
 11932114        weights[0] = triangleWeights.A;
 11932115        weights[1] = triangleWeights.B;
 11932116        weights[2] = triangleWeights.C;
 117        // Rank only the Minkowski point; construct both witnesses once for the winner.
 11932118        Span<Vector3d> points = stackalloc Vector3d[3];
 11932119        points[0] = simplex[first].Point;
 11932120        points[1] = simplex[second].Point;
 11932121        points[2] = simplex[third].Point;
 11932122        _ = Vector3d.TryGetWeightedAverage(points, weights, out Vector3d candidatePoint);
 11932123        if (hasBest && Vector3d.CompareMagnitudeSquared(candidatePoint, bestPoint) >= 0)
 6376124            return;
 125
 5556126        bestPoint = candidatePoint;
 5556127        hasBest = true;
 5556128        bestIndices[0] = first;
 5556129        bestIndices[1] = second;
 5556130        bestIndices[2] = third;
 5556131        bestWeights[0] = weights[0];
 5556132        bestWeights[1] = weights[1];
 5556133        bestWeights[2] = weights[2];
 5556134    }
 135
 136    private static ClosestSimplexResult ReduceByWeights(
 137        SupportPoint[] simplex,
 138        ref int count,
 139        Fixed64 firstWeight,
 140        Fixed64 secondWeight,
 141        Fixed64 thirdWeight,
 142        Fixed64 fourthWeight)
 143    {
 7553144        Span<SupportPoint> reduced = stackalloc SupportPoint[4];
 7553145        Span<Fixed64> weights = stackalloc Fixed64[4];
 146        // Deferred witness state retains all four weight slots.
 7553147        weights.Clear();
 7553148        int reducedCount = 0;
 7553149        AddWeightedPoint(simplex[0], firstWeight, reduced, weights, ref reducedCount);
 7553150        AddWeightedPoint(simplex[1], secondWeight, reduced, weights, ref reducedCount);
 7553151        AddWeightedPoint(simplex[2], thirdWeight, reduced, weights, ref reducedCount);
 7553152        AddWeightedPoint(simplex[3], fourthWeight, reduced, weights, ref reducedCount);
 153
 49542154        for (int i = 0; i < reducedCount; i++)
 17218155            simplex[i] = reduced[i];
 156
 7553157        count = reducedCount;
 7553158        return ClosestSimplexResult.FromSpanWeights(reduced, weights, reducedCount);
 159    }
 160
 161    private static void AddWeightedPoint(
 162        SupportPoint point,
 163        Fixed64 weight,
 164        Span<SupportPoint> reduced,
 165        Span<Fixed64> weights,
 166        ref int count)
 167    {
 30212168        if (weight <= Fixed64.Epsilon)
 12994169            return;
 170
 17218171        reduced[count] = point;
 17218172        weights[count] = weight;
 17218173        count++;
 17218174    }
 175
 176    /// <summary>
 177    /// Returns barycentric weights for the point on a triangle closest to the origin.
 178    /// </summary>
 179    internal static TriangleWeights ClosestPointOnTriangleToOrigin(Vector3d a, Vector3d b, Vector3d c)
 180    {
 19500181        Span<Vector3d> scaled = stackalloc Vector3d[3];
 19500182        scaled[0] = a;
 19500183        scaled[1] = b;
 19500184        scaled[2] = c;
 19500185        GjkSimplexScale.ScaleForProducts(scaled);
 19500186        a = scaled[0];
 19500187        b = scaled[1];
 19500188        c = scaled[2];
 189
 19500190        var triangle = new FixedTriangle(a, b, c);
 19500191        if (!triangle.TryGetClosestPointBarycentrics(Vector3d.Zero,
 19500192                out Signed320 numeratorB, out Signed320 numeratorC,
 19500193                out Signed320 denominator, out _))
 194        {
 3095195            return ClosestPointOnDegenerateTriangleToOrigin(a, b, c);
 196        }
 197
 198        // Retain exact feature weights rather than reprojecting a rounded point.
 16405199        _ = Fixed64.TryGetUnitIntervalRatio(numeratorB, denominator, out Fixed64 weightB);
 16405200        _ = Fixed64.TryGetUnitIntervalRatio(numeratorC, denominator, out Fixed64 weightC);
 16405201        weightB = FixedMath.Clamp(
 16405202            weightB,
 16405203            Fixed64.Zero,
 16405204            Fixed64.One);
 16405205        weightC = FixedMath.Clamp(
 16405206            weightC,
 16405207            Fixed64.Zero,
 16405208            Fixed64.One - weightB);
 16405209        return new TriangleWeights(
 16405210            Fixed64.One - weightB - weightC,
 16405211            weightB,
 16405212            weightC);
 213    }
 214
 215    private static TriangleWeights ClosestPointOnDegenerateTriangleToOrigin(
 216        Vector3d first,
 217        Vector3d second,
 218        Vector3d third)
 219    {
 3095220        GetClosestSegmentWeights(
 3095221            first,
 3095222            second,
 3095223            out Fixed64 firstWeight,
 3095224            out Fixed64 secondWeight,
 3095225            out Vector3d bestPoint);
 3095226        TriangleWeights best = new(
 3095227            firstWeight,
 3095228            secondWeight,
 3095229            Fixed64.Zero);
 230
 3095231        GetClosestSegmentWeights(
 3095232            first,
 3095233            third,
 3095234            out firstWeight,
 3095235            out Fixed64 thirdWeight,
 3095236            out Vector3d candidatePoint);
 3095237        if (Vector3d.CompareMagnitudeSquared(
 3095238                candidatePoint,
 3095239                bestPoint) < 0)
 240        {
 1392241            bestPoint = candidatePoint;
 1392242            best = new TriangleWeights(
 1392243                firstWeight,
 1392244                Fixed64.Zero,
 1392245                thirdWeight);
 246        }
 247
 3095248        GetClosestSegmentWeights(
 3095249            second,
 3095250            third,
 3095251            out secondWeight,
 3095252            out thirdWeight,
 3095253            out candidatePoint);
 3095254        if (Vector3d.CompareMagnitudeSquared(
 3095255                candidatePoint,
 3095256                bestPoint) < 0)
 257        {
 935258            best = new TriangleWeights(
 935259                Fixed64.Zero,
 935260                secondWeight,
 935261                thirdWeight);
 262        }
 263
 3095264        return best;
 265    }
 266
 267    private static void GetClosestSegmentWeights(
 268        Vector3d start,
 269        Vector3d end,
 270        out Fixed64 startWeight,
 271        out Fixed64 endWeight,
 272        out Vector3d point)
 273    {
 9285274        Vector3d delta = end - start;
 9285275        Fixed64 denominator = Vector3d.Dot(delta, delta);
 9285276        Fixed64 numerator = -Vector3d.Dot(start, delta);
 9285277        if (denominator == Fixed64.Zero || numerator <= Fixed64.Zero)
 278        {
 1293279            startWeight = Fixed64.One;
 1293280            endWeight = Fixed64.Zero;
 1293281            point = start;
 1293282            return;
 283        }
 7992284        if (numerator >= denominator)
 285        {
 3170286            startWeight = Fixed64.Zero;
 3170287            endWeight = Fixed64.One;
 3170288            point = end;
 3170289            return;
 290        }
 291
 4822292        endWeight = numerator / denominator;
 4822293        startWeight = Fixed64.One - endWeight;
 4822294        point = start * startWeight + end * endWeight;
 4822295    }
 296
 297    /// <summary>Tests inclusive origin containment in a nondegenerate GJK tetrahedron.</summary>
 298    internal static bool IsOriginInsideTetrahedron(
 299        Vector3d a,
 300        Vector3d b,
 301        Vector3d c,
 302        Vector3d d)
 303    {
 304        // These consistently oriented face determinants are barycentric volume
 305        // numerators. Equal nonzero signs certify containment and nonzero volume;
 306        // rounded plane-side products can instead erase a genuine separation.
 4209307        int abc = WideGeometry.GetTripleProductSign(
 4209308            a.X, a.Y, a.Z, b.X, b.Y, b.Z, c.X, c.Y, c.Z);
 4209309        int adb = WideGeometry.GetTripleProductSign(
 4209310            a.X, a.Y, a.Z, d.X, d.Y, d.Z, b.X, b.Y, b.Z);
 4209311        int acd = WideGeometry.GetTripleProductSign(
 4209312            a.X, a.Y, a.Z, c.X, c.Y, c.Z, d.X, d.Y, d.Z);
 4209313        int bdc = WideGeometry.GetTripleProductSign(
 4209314            b.X, b.Y, b.Z, d.X, d.Y, d.Z, c.X, c.Y, c.Z);
 4209315        bool hasPositive = abc > 0 || adb > 0 || acd > 0 || bdc > 0;
 4209316        bool hasNegative = abc < 0 || adb < 0 || acd < 0 || bdc < 0;
 317        // Coplanar simplexes have conflicting signs or four zeros; retain their
 318        // closest-face reduction rather than declaring a volume intersection.
 4209319        return hasPositive != hasNegative;
 320    }
 321
 322    private static bool ContainsSupportPoint(
 323        SupportPoint[] simplex,
 324        int count,
 325        Vector3d point)
 326    {
 104728327        for (int i = 0; i < count; i++)
 328        {
 31593329            Vector3d difference = simplex[i].Point - point;
 31593330            if (Vector3d.TryGetMagnitude(difference, out Fixed64 distance)
 31593331                && distance <= Fixed64.Epsilon)
 7332                return true;
 333        }
 334
 20771335        return false;
 336    }
 337}

/home/runner/work/Gravitas/Gravitas/src/Gravitas/Queries/3D/Sweeps/ConvexSweepQueryWorker.State.cs

#LineLine coverage
 1//=======================================================================
 2// ConvexSweepQueryWorker.State.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.Collections.Generic;
 10using System.Runtime.CompilerServices;
 11using FixedMathSharp;
 12using FixedMathSharp.Geometry;
 13
 14namespace Gravitas.Queries;
 15
 16/// <content>Defines support samples, triangle candidates, and simplex results for convex sweeps.</content>
 17internal sealed partial class ConvexSweepQueryWorker
 18{
 19    private readonly struct SupportPoint
 20    {
 21        public SupportPoint(
 22            FixedPointAnchor pointA,
 23            FixedPointAnchor pointB,
 24            Vector3d point)
 25        {
 2168026            PointA = pointA;
 2168027            PointB = pointB;
 2168028            Point = point;
 2168029        }
 30
 31        public FixedPointAnchor PointA { get; }
 32
 33        public FixedPointAnchor PointB { get; }
 34
 35        public Vector3d Point { get; }
 36    }
 37
 38    private readonly struct SweepTriangleCandidate
 39    {
 40        public SweepTriangleCandidate(
 41            int triangleIndex,
 42            Fixed64 lowerBoundNumerator)
 43        {
 148644            TriangleIndex = triangleIndex;
 148645            LowerBoundNumerator = lowerBoundNumerator;
 148646        }
 47
 48        public int TriangleIndex { get; }
 49
 50        public Fixed64 LowerBoundNumerator { get; }
 51    }
 52
 53    private sealed class SweepTriangleCandidateComparer : IComparer<SweepTriangleCandidate>
 54    {
 55        [MethodImpl(MethodImplOptions.AggressiveInlining)]
 56        public int Compare(SweepTriangleCandidate left, SweepTriangleCandidate right)
 57        {
 58158            int lowerBoundCompare =
 58159                left.LowerBoundNumerator.CompareTo(
 58160                    right.LowerBoundNumerator);
 58161            return lowerBoundCompare != 0
 58162                ? lowerBoundCompare
 58163                : left.TriangleIndex.CompareTo(right.TriangleIndex);
 64        }
 65    }
 66
 67
 68
 69    private readonly struct ClosestSimplexResult
 70    {
 71        private readonly int _count;
 72        private readonly Fixed64 _first;
 73        private readonly Fixed64 _second;
 74        private readonly Fixed64 _third;
 75        private readonly Fixed64 _fourth;
 76
 77        private ClosestSimplexResult(bool intersects, Vector3d point, int count,
 78            Fixed64 first, Fixed64 second, Fixed64 third, Fixed64 fourth)
 79        {
 2077180            Intersects = intersects;
 2077181            Point = point;
 2077182            _count = count;
 2077183            _first = first;
 2077184            _second = second;
 2077185            _third = third;
 2077186            _fourth = fourth;
 2077187        }
 88
 89        public bool Intersects { get; }
 90
 91        public Vector3d Point { get; }
 92
 93        public static ClosestSimplexResult Intersection =>
 98694            new(true, Vector3d.Zero, 0, default, default, default, default);
 95
 96        public static ClosestSimplexResult FromWeights(
 97            SupportPoint[] simplex,
 98            int count,
 99            Fixed64 first,
 100            Fixed64 second,
 101            Fixed64 third,
 102            Fixed64 fourth)
 103        {
 12232104            ReadOnlySpan<Fixed64> weights = stackalloc Fixed64[4] { first, second, third, fourth };
 12232105            return FromSpanWeights(simplex, weights, count);
 106        }
 107
 108        public static ClosestSimplexResult FromSpanWeights(
 109            ReadOnlySpan<SupportPoint> simplex,
 110            ReadOnlySpan<Fixed64> weights,
 111            int count)
 112        {
 19785113            if (count == 1)
 4811114                return new ClosestSimplexResult(false, simplex[0].Point, count,
 4811115                    weights[0], weights[1], weights[2], weights[3]);
 14974116            Span<Vector3d> points = stackalloc Vector3d[4];
 100062117            for (int i = 0; i < count; i++)
 35057118                points[i] = simplex[i].Point;
 14974119            _ = Vector3d.TryGetWeightedAverage(points[..count], weights[..count], out Vector3d point);
 14974120            return new ClosestSimplexResult(false, point, count,
 14974121                weights[0], weights[1], weights[2], weights[3]);
 122        }
 123
 124        public void GetWitnesses(ReadOnlySpan<SupportPoint> simplex,
 125            out FixedPointAnchor pointA, out FixedPointAnchor pointB)
 126        {
 127            // Iterations need only the Minkowski point; materialize the selected witnesses once.
 4680128            if (_count == 1)
 129            {
 242130                FixedPointAnchor first = simplex[0].PointA;
 242131                FixedPointAnchor second = simplex[0].PointB;
 242132                pointA = new FixedPointAnchor(first.Origin, first.Rotation, first.LocalPoint, first.LocalDisplacement);
 242133                pointB = new FixedPointAnchor(second.Origin, second.Rotation, second.LocalPoint, second.LocalDisplacemen
 242134                return;
 135            }
 4438136            ReadOnlySpan<Fixed64> weights = stackalloc Fixed64[4] { _first, _second, _third, _fourth };
 4438137            Span<Vector3d> firstLocalPoints = stackalloc Vector3d[4];
 4438138            Span<Vector3d> firstLocalDisplacements = stackalloc Vector3d[4];
 4438139            Span<Vector3d> secondLocalPoints = stackalloc Vector3d[4];
 4438140            Span<Vector3d> secondLocalDisplacements = stackalloc Vector3d[4];
 4438141            int frameIndex = 0;
 30880142            for (int i = 0; i < _count; i++)
 143            {
 11002144                firstLocalPoints[i] = simplex[i].PointA.LocalPoint;
 11002145                firstLocalDisplacements[i] = simplex[i].PointA.LocalDisplacement;
 11002146                secondLocalPoints[i] = simplex[i].PointB.LocalPoint;
 11002147                secondLocalDisplacements[i] = simplex[i].PointB.LocalDisplacement;
 11002148                if (weights[frameIndex] <= Fixed64.Zero && weights[i] > Fixed64.Zero)
 59149                    frameIndex = i;
 150            }
 151
 4438152            _ = Vector3d.TryGetWeightedAverage(firstLocalPoints[.._count], weights[.._count], out Vector3d firstLocalPoi
 4438153            _ = Vector3d.TryGetWeightedAverage(firstLocalDisplacements[.._count], weights[.._count], out Vector3d firstL
 4438154            _ = Vector3d.TryGetWeightedAverage(secondLocalPoints[.._count], weights[.._count], out Vector3d secondLocalP
 4438155            _ = Vector3d.TryGetWeightedAverage(secondLocalDisplacements[.._count], weights[.._count], out Vector3d secon
 4438156            FixedPointAnchor firstFrame = simplex[frameIndex].PointA;
 4438157            FixedPointAnchor secondFrame = simplex[frameIndex].PointB;
 4438158            pointA = new FixedPointAnchor(firstFrame.Origin, firstFrame.Rotation, firstLocalPoint, firstLocalDisplacemen
 4438159            pointB = new FixedPointAnchor(secondFrame.Origin, secondFrame.Rotation, secondLocalPoint, secondLocalDisplac
 4438160        }
 161    }
 162
 163
 164}

Methods/Properties

TrySweepCertifiedConvexPatch(Gravitas.Queries.ConvexShape&,Gravitas.Colliders.LSMeshCollider,Gravitas.Queries.Physics3DHit&,FixedMathSharp.Fixed64&)
.cctor()
.ctor(System.Int32)
ClearSource()
PrepareConvexMeshSource(Gravitas.Colliders.LSMeshCollider,FixedMathSharp.Vector3d)
PrepareCompoundSource(Gravitas.Colliders.LSCompoundCollider,FixedMathSharp.Vector3d)
PreparePrimitiveSource(Gravitas.Colliders.LSCollider,FixedMathSharp.Vector3d)
PrepareCircleSlabSource(FixedMathSharp.Vector3d,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,FixedMathSharp.Vector3d)
PrepareSphereSource(FixedMathSharp.Vector3d,FixedMathSharp.Fixed64,FixedMathSharp.Vector3d)
TrySweepPreparedSource(Gravitas.Colliders.LSCollider,Gravitas.Queries.Physics3DHit&,System.Boolean,Gravitas.Queries.ContinuousSurfaceTriangleAdmission)
Prepare(Gravitas.Colliders.LSCollider,FixedMathSharp.Vector3d)
Prepare(Gravitas.Queries.ConvexShape,FixedMathSharp.Vector3d)
TrySweepCompoundSource(Gravitas.Colliders.LSCompoundCollider,Gravitas.Colliders.LSCollider,Gravitas.Queries.Physics3DHit&)
TrySweepSourceShape(Gravitas.Queries.ConvexShape,Gravitas.Colliders.LSCollider,Gravitas.Queries.Physics3DHit&,FixedMathSharp.Fixed64&)
TrySweepTargetCompound(Gravitas.Queries.ConvexShape,Gravitas.Colliders.LSCompoundCollider,Gravitas.Queries.Physics3DHit&,FixedMathSharp.Fixed64&)
TrySweepConcaveMeshTarget(Gravitas.Queries.ConvexShape,Gravitas.Colliders.LSMeshCollider,Gravitas.Queries.Physics3DHit&,FixedMathSharp.Fixed64&)
BuildOrderedSweepTriangleCandidates(Gravitas.Queries.ConvexShape,Gravitas.Colliders.LSMeshCollider)
TryComputeSweepLowerBoundNumerator(Gravitas.Queries.ConvexShape,Gravitas.Queries.ConvexShape,FixedMathSharp.Fixed64&)
TryComputeSweepLowerBoundNumerator(FixedMathSharp.Vector3d,FixedMathSharp.Vector3d,FixedMathSharp.Vector3d,FixedMathSharp.Vector3d,FixedMathSharp.Vector3d,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64&)
IncludeAxisEntryNumerator(FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64&)
RemainingSweepTrianglesCannotBeat(FixedMathSharp.Fixed64,System.Boolean,FixedMathSharp.Fixed64)
TrySweepConvexTarget(Gravitas.Queries.ConvexShape,Gravitas.Queries.ConvexShape,Gravitas.Colliders.LSCollider,Gravitas.Queries.Physics3DHit&,FixedMathSharp.Fixed64&)
TryResolveEndpointBracket(Gravitas.Queries.ConvexShape,Gravitas.Queries.ConvexShape,Gravitas.Colliders.LSCollider,FixedMathSharp.Fixed64,Gravitas.Queries.Physics3DHit&,FixedMathSharp.Fixed64&,FixedMathSharp.Vector3d)
RetainCapsuleNormal(Gravitas.Queries.ConvexShape&,Gravitas.Queries.ConvexShape&,Gravitas.CollisionHandling.ContactAnchor&,FixedMathSharp.Vector3d&)
GetChordOffset(FixedMathSharp.Fixed64)
ResolveHitAnchor(Gravitas.Queries.ConvexShape,Gravitas.Colliders.LSCollider,Gravitas.Queries.ConvexShape,Gravitas.Queries.GjkResult,FixedMathSharp.Vector3d&,System.Boolean&,System.Boolean&)
ResolveHitNormal(Gravitas.Queries.ConvexShape,Gravitas.Colliders.LSCollider,FixedMathSharp.Vector3d,FixedMathSharp.Vector3d,FixedMathSharp.Vector3d,System.Boolean,System.Boolean,System.Boolean)
CreateColliderShape(Gravitas.Colliders.LSCollider,FixedMathSharp.Vector3d)
CreateTriangleShape(Gravitas.Colliders.LSMeshCollider,System.Int32)
ThrowIfConcaveSource(Gravitas.Colliders.LSMeshCollider)
CreateSweptSourceBoundsInMeshFrame(Gravitas.Queries.ConvexShape,Gravitas.Colliders.LSMeshCollider,FixedMathSharp.Vector3d&,FixedMathSharp.Vector3d&)
CanSweptSourceShapeReachTarget(Gravitas.Queries.ConvexShape,Gravitas.Colliders.LSCollider)
ComesBeforeReducerCandidate(FixedMathSharp.Fixed64,System.Int32,System.Boolean,FixedMathSharp.Fixed64,System.Int32)
CreateConcaveSourceException(Gravitas.Colliders.LSMeshCollider)
ComputeDistance(Gravitas.Queries.ConvexShape,Gravitas.Queries.ConvexShape)
CreateSupportPoint(Gravitas.Queries.ConvexShape,Gravitas.Queries.ConvexShape,FixedMathSharp.Vector3d,FixedMathSharp.Fixed64)
SolveClosestSimplex(Gravitas.Queries.ConvexSweepQueryWorker/SupportPoint[],System.Int32&,FixedMathSharp.Fixed64)
TryResolveInitialMeshContact(Gravitas.Queries.ConvexShape,Gravitas.Queries.ConvexShape,Gravitas.Queries.GjkResult,Gravitas.CollisionHandling.ContactAnchor&,FixedMathSharp.Vector3d&)
ReduceSegment(Gravitas.Queries.ConvexSweepQueryWorker/SupportPoint[],System.Int32&,FixedMathSharp.Fixed64)
ReduceTriangle(Gravitas.Queries.ConvexSweepQueryWorker/SupportPoint[],System.Int32&)
ReduceTetrahedron(Gravitas.Queries.ConvexSweepQueryWorker/SupportPoint[],System.Int32&)
EvaluateFace(Gravitas.Queries.ConvexSweepQueryWorker/SupportPoint[],System.Int32,System.Int32,System.Int32,FixedMathSharp.Vector3d&,System.Boolean&,System.Span`1<System.Int32>,System.Span`1<FixedMathSharp.Fixed64>,System.Span`1<FixedMathSharp.Fixed64>)
ReduceByWeights(Gravitas.Queries.ConvexSweepQueryWorker/SupportPoint[],System.Int32&,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64)
AddWeightedPoint(Gravitas.Queries.ConvexSweepQueryWorker/SupportPoint,FixedMathSharp.Fixed64,System.Span`1<Gravitas.Queries.ConvexSweepQueryWorker/SupportPoint>,System.Span`1<FixedMathSharp.Fixed64>,System.Int32&)
ClosestPointOnTriangleToOrigin(FixedMathSharp.Vector3d,FixedMathSharp.Vector3d,FixedMathSharp.Vector3d)
ClosestPointOnDegenerateTriangleToOrigin(FixedMathSharp.Vector3d,FixedMathSharp.Vector3d,FixedMathSharp.Vector3d)
GetClosestSegmentWeights(FixedMathSharp.Vector3d,FixedMathSharp.Vector3d,FixedMathSharp.Fixed64&,FixedMathSharp.Fixed64&,FixedMathSharp.Vector3d&)
IsOriginInsideTetrahedron(FixedMathSharp.Vector3d,FixedMathSharp.Vector3d,FixedMathSharp.Vector3d,FixedMathSharp.Vector3d)
ContainsSupportPoint(Gravitas.Queries.ConvexSweepQueryWorker/SupportPoint[],System.Int32,FixedMathSharp.Vector3d)
.ctor(FixedMathSharp.Geometry.FixedPointAnchor,FixedMathSharp.Geometry.FixedPointAnchor,FixedMathSharp.Vector3d)
.ctor(System.Int32,FixedMathSharp.Fixed64)
Compare(Gravitas.Queries.ConvexSweepQueryWorker/SweepTriangleCandidate,Gravitas.Queries.ConvexSweepQueryWorker/SweepTriangleCandidate)
.ctor(System.Boolean,FixedMathSharp.Vector3d,System.Int32,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64)
get_Intersection()
FromWeights(Gravitas.Queries.ConvexSweepQueryWorker/SupportPoint[],System.Int32,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64)
FromSpanWeights(System.ReadOnlySpan`1<Gravitas.Queries.ConvexSweepQueryWorker/SupportPoint>,System.ReadOnlySpan`1<FixedMathSharp.Fixed64>,System.Int32)
GetWitnesses(System.ReadOnlySpan`1<Gravitas.Queries.ConvexSweepQueryWorker/SupportPoint>,FixedMathSharp.Geometry.FixedPointAnchor&,FixedMathSharp.Geometry.FixedPointAnchor&)