< Summary

Line coverage
100%
Covered lines: 1083
Uncovered lines: 0
Coverable lines: 1083
Total lines: 1990
Line coverage: 100%
Branch coverage
100%
Covered branches: 464
Total branches: 464
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: TryGetPrism(...)100%44100%
File 1: TryCreatePrism(...)100%2626100%
File 1: GetContact(...)100%2020100%
File 1: TryGetPrimaryFace(...)100%88100%
File 1: GetExactBoundaryContactsInto(...)100%2222100%
File 1: BuildFootprintIntersection(...)100%3030100%
File 1: BuildConvexHull(...)100%1818100%
File 1: TryGetExactHalf(...)100%22100%
File 1: TryGetSymmetricBounds(...)100%22100%
File 1: AddUnique(...)100%44100%
File 1: GetSegmentExtents(...)100%11100%
File 1: CompareCoordinates(...)100%22100%
File 1: IsPrimaryOffset(...)100%66100%
File 1: CreateSeparated(...)100%11100%
File 1: SortByVoxelIndex(...)100%44100%
File 1: SiftDown(...)100%88100%
File 1: CollectPotentialSourceVoxels(...)100%66100%
File 1: AddSourceCandidates(...)100%22100%
File 1: .ctor(...)100%11100%
File 1: Visit(...)100%11100%
File 2: IsNavigationBodyAnchorValid(...)100%11100%
File 2: IsPortalCertifiedOnEdge(...)100%22100%
File 3: TryGetNavigationPortalTraversalParameters(...)100%66100%
File 3: TryGetCompiledNavigationPortalTraversalParameters(...)100%1818100%
File 3: IsNavigationBodySegmentValid(...)100%2020100%
File 3: IsNavigationBodySegmentValidCore(...)100%4040100%
File 3: TryClipNavigationBodySegmentEndpoint(...)100%2424100%
File 3: TryGetActiveOpening(...)100%1414100%
File 3: IsDirectedPortalCrossing(...)100%1212100%
File 3: IsPortalTraversalGapPlanarValid(...)100%1212100%
File 3: IsPortalHeightValidOverInterval(...)100%11100%
File 3: IsBodyHeightValidOverInterval(...)100%44100%
File 3: GetConservativeLerpBounds(...)100%1010100%
File 3: TryGetPointParameter(...)100%44100%
File 3: AreSamePortal(...)100%1212100%
File 3: CompareDistanceFrom(...)100%11100%
File 3: Swap(...)100%11100%
File 3: HasPositiveNavigationBodyPrismOverlap(...)100%22100%
File 3: TryGetPlanarSegmentInterval(...)100%4242100%
File 3: TryGetPlanarSlab(...)100%1212100%
File 3: IsFootprintStrictlySeparatedFromLine(...)100%88100%
File 3: AddNavigationBodyParameter(...)100%66100%
File 3: GetNavigationBodyParameter(...)100%22100%
File 4: TryValidateNavigationCorridor(...)100%22100%
File 5: TryCreateNavigationPortal(...)100%1212100%
File 5: IsNavigationPrismValid(...)100%44100%
File 5: IsPlanarPointContained(...)100%11100%
File 5: GetMinimumPolygonClearance(...)100%66100%
File 5: GetSignedDifference(...)100%22100%
File 5: GetSignedDifferenceMagnitude(...)100%44100%
File 5: Add128(...)100%11100%
File 5: Subtract128(...)100%11100%
File 5: Multiply64(...)100%11100%
File 5: MultiplyWords(...)100%44100%
File 5: AddWord(...)100%44100%
File 5: CompareWords(...)100%66100%
File 5: GetConservativeDistance(...)100%22100%
File 5: GetNearestRepresentableSegmentMidpoint(...)100%22100%
File 5: GetGreatestCommonDivisor(...)100%22100%

File(s)

/home/runner/work/GridForge/GridForge/src/GridForge/Grids/Topology/GridCellGeometry.cs

#LineLine coverage
 1//=======================================================================
 2// GridCellGeometry.cs
 3//=======================================================================
 4// MIT License, Copyright (c) 2024-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 GridForge.Grids.Storage;
 13using GridForge.Spatial;
 14using SwiftCollections;
 15
 16namespace GridForge.Grids.Topology;
 17
 18/// <summary>
 19/// Builds exact topology-owned cell prisms and contact manifolds without floating-point conversion.
 20/// </summary>
 21public static partial class GridCellGeometry
 22{
 23    private const int MaximumIntersectionCandidateCount = 48;
 24
 25    /// <summary>
 26    /// Attempts to build the exact prism for one physical voxel in an active grid.
 27    /// </summary>
 28    public static bool TryGetPrism(
 29        VoxelGrid grid,
 30        VoxelIndex index,
 31        out GridCellPrism prism)
 32    {
 96333        ChronicleGuard.ThrowIfNull(grid, nameof(grid));
 34
 96335        if (!grid.IsActive || !grid.TryGetVoxel(index, out Voxel? voxel))
 36        {
 437            prism = default;
 438            return false;
 39        }
 40
 95941        return TryCreatePrism(
 95942            grid.Configuration.TopologyKind,
 95943            grid.Configuration.TopologyMetrics,
 95944            voxel!.WorldPosition,
 95945            voxel.WorldIndex,
 95946            out prism);
 47    }
 48
 49    /// <summary>
 50    /// Attempts to build an exact prism from normalized topology metrics and a world-space cell center.
 51    /// </summary>
 52    /// <remarks>
 53    /// This overload permits offline geometry construction. The supplied identity is copied verbatim.
 54    /// </remarks>
 55    public static bool TryCreatePrism(
 56        GridTopologyKind topologyKind,
 57        GridTopologyMetrics topologyMetrics,
 58        Vector3d center,
 59        WorldVoxelIndex cell,
 60        out GridCellPrism prism)
 61    {
 2816862        if (!GridTopologyMetrics.IsValid(topologyKind, topologyMetrics))
 63        {
 264            prism = default;
 265            return false;
 66        }
 67
 2816668        GridTopologyMetrics metrics = GridTopologyMetrics.Normalize(topologyKind, topologyMetrics);
 2816669        if (!TryGetExactHalf(metrics.LayerHeight, out Fixed64 halfHeight))
 70        {
 671            prism = default;
 672            return false;
 73        }
 74
 2816075        if (!TryGetSymmetricBounds(center.Y, halfHeight, out Fixed64 verticalMin, out Fixed64 verticalMax))
 76        {
 277            prism = default;
 278            return false;
 79        }
 80
 2815881        Span<Vector2d> footprint = stackalloc Vector2d[6];
 82        int vertexCount;
 83        Fixed64 planarInradius;
 84
 2815885        if (topologyKind == GridTopologyKind.RectangularPrism)
 86        {
 2619787            if (!TryGetExactHalf(metrics.CellWidth, out Fixed64 halfWidth)
 2619788                || !TryGetExactHalf(metrics.CellLength, out Fixed64 halfLength))
 89            {
 1690                prism = default;
 1691                return false;
 92            }
 93
 2618194            bool hasExactBounds = TryGetSymmetricBounds(
 2618195                center.X,
 2618196                halfWidth,
 2618197                out Fixed64 minX,
 2618198                out Fixed64 maxX);
 2618199            hasExactBounds &= TryGetSymmetricBounds(
 26181100                center.Z,
 26181101                halfLength,
 26181102                out Fixed64 minZ,
 26181103                out Fixed64 maxZ);
 26181104            if (!hasExactBounds)
 105            {
 16106                prism = default;
 16107                return false;
 108            }
 26165109            footprint[0] = new Vector2d(minX, minZ);
 26165110            footprint[1] = new Vector2d(maxX, minZ);
 26165111            footprint[2] = new Vector2d(maxX, maxZ);
 26165112            footprint[3] = new Vector2d(minX, maxZ);
 26165113            vertexCount = 4;
 26165114            planarInradius = FixedMath.Min(halfWidth, halfLength);
 115        }
 116        else
 117        {
 1961118            Fixed64 radius = metrics.CellRadius;
 1961119            if (!Fixed64.TryMultiplyAdd(
 1961120                    radius,
 1961121                    HexCoordinateUtility.Sqrt3,
 1961122                    Fixed64.Zero,
 1961123                    out Fixed64 fullWidth))
 124            {
 1125                prism = default;
 1126                return false;
 127            }
 128
 1960129            if (!TryGetExactHalf(radius, out Fixed64 halfRadius)
 1960130                || !TryGetExactHalf(fullWidth, out Fixed64 apothem))
 131            {
 2132                prism = default;
 2133                return false;
 134            }
 1958135            if (metrics.HexOrientation == HexOrientation.FlatTop)
 136            {
 904137                bool hasExactBounds = TryGetSymmetricBounds(
 904138                    center.X,
 904139                    radius,
 904140                    out Fixed64 minRadiusX,
 904141                    out Fixed64 maxRadiusX);
 904142                hasExactBounds &= TryGetSymmetricBounds(
 904143                    center.X,
 904144                    halfRadius,
 904145                    out Fixed64 minHalfX,
 904146                    out Fixed64 maxHalfX);
 904147                hasExactBounds &= TryGetSymmetricBounds(
 904148                    center.Z,
 904149                    apothem,
 904150                    out Fixed64 minApothemZ,
 904151                    out Fixed64 maxApothemZ);
 904152                if (!hasExactBounds)
 153                {
 1154                    prism = default;
 1155                    return false;
 156                }
 157
 903158                footprint[0] = new Vector2d(minRadiusX, center.Z);
 903159                footprint[1] = new Vector2d(minHalfX, minApothemZ);
 903160                footprint[2] = new Vector2d(maxHalfX, minApothemZ);
 903161                footprint[3] = new Vector2d(maxRadiusX, center.Z);
 903162                footprint[4] = new Vector2d(maxHalfX, maxApothemZ);
 903163                footprint[5] = new Vector2d(minHalfX, maxApothemZ);
 164            }
 165            else
 166            {
 1054167                bool hasExactBounds = TryGetSymmetricBounds(
 1054168                    center.Z,
 1054169                    radius,
 1054170                    out Fixed64 minRadiusZ,
 1054171                    out Fixed64 maxRadiusZ);
 1054172                hasExactBounds &= TryGetSymmetricBounds(
 1054173                    center.Z,
 1054174                    halfRadius,
 1054175                    out Fixed64 minHalfZ,
 1054176                    out Fixed64 maxHalfZ);
 1054177                hasExactBounds &= TryGetSymmetricBounds(
 1054178                    center.X,
 1054179                    apothem,
 1054180                    out Fixed64 minApothemX,
 1054181                    out Fixed64 maxApothemX);
 1054182                if (!hasExactBounds)
 183                {
 5184                    prism = default;
 5185                    return false;
 186                }
 187
 1049188                footprint[0] = new Vector2d(center.X, minRadiusZ);
 1049189                footprint[1] = new Vector2d(maxApothemX, minHalfZ);
 1049190                footprint[2] = new Vector2d(maxApothemX, maxHalfZ);
 1049191                footprint[3] = new Vector2d(center.X, maxRadiusZ);
 1049192                footprint[4] = new Vector2d(minApothemX, maxHalfZ);
 1049193                footprint[5] = new Vector2d(minApothemX, minHalfZ);
 194            }
 195
 1952196            vertexCount = 6;
 1952197            planarInradius = apothem;
 198        }
 199
 28117200        ReadOnlySpan<Vector2d> resolvedFootprint = footprint[..vertexCount];
 28117201        prism = new GridCellPrism(
 28117202            cell,
 28117203            topologyKind,
 28117204            center,
 28117205            verticalMin,
 28117206            verticalMax,
 28117207            planarInradius,
 28117208            resolvedFootprint);
 28117209        return true;
 210    }
 211
 212    /// <summary>
 213    /// Computes the exact closed-set contact between two cell prisms.
 214    /// </summary>
 215    public static VoxelContactManifold GetContact(
 216        in GridCellPrism source,
 217        in GridCellPrism target)
 218    {
 2664219        Vector3d sourceToTarget = target.Center - source.Center;
 2664220        Fixed64 verticalMin = FixedMath.Max(source.VerticalMin, target.VerticalMin);
 2664221        Fixed64 verticalMax = FixedMath.Min(source.VerticalMax, target.VerticalMax);
 2664222        if (verticalMin > verticalMax)
 3223            return CreateSeparated(source.Cell, target.Cell, sourceToTarget);
 224
 2661225        Span<Vector2d> intersection = stackalloc Vector2d[GridConvexPolygon2d.MaxVertexCount];
 2661226        int intersectionCount = BuildFootprintIntersection(source, target, intersection);
 2659227        if (intersectionCount == 0)
 15228            return CreateSeparated(source.Cell, target.Cell, sourceToTarget);
 229
 2644230        bool hasVerticalSpan = verticalMax > verticalMin;
 2644231        if (intersectionCount >= 3
 2644232            && FixedConvex2dRelations.IsStrictlyConvex(intersection[..intersectionCount])
 2644233            && FixedConvex2dRelations.TryGetAreaAndCentroid(
 2644234                intersection[..intersectionCount],
 2644235                out Fixed64 overlapArea,
 2644236                out _))
 237        {
 238            // A strictly convex hull proves positive exact area even when the
 239            // narrowed Fixed64 area underflows to zero.
 64240            bool areaRepresentable = overlapArea > Fixed64.Zero
 64241                && overlapArea != Fixed64.MaxValue;
 64242            GridConvexPolygon2d polygon = new GridConvexPolygon2d(intersection[..intersectionCount]);
 64243            return new VoxelContactManifold(
 64244                source.Cell,
 64245                target.Cell,
 64246                sourceToTarget,
 64247                hasVerticalSpan ? VoxelContactKind.VolumeOverlap : VoxelContactKind.Face,
 64248                hasVerticalSpan ? VoxelContactFaceKind.None : VoxelContactFaceKind.Horizontal,
 64249                verticalMin,
 64250                verticalMax,
 64251                default,
 64252                default,
 64253                polygon,
 64254                overlapArea,
 64255                areaRepresentable);
 256        }
 257
 2580258        GetSegmentExtents(intersection[..intersectionCount], out Vector2d segmentStart, out Vector2d segmentEnd);
 2580259        bool hasHorizontalSpan = segmentStart != segmentEnd;
 2580260        if (hasHorizontalSpan && hasVerticalSpan)
 261        {
 2470262            Fixed64 width = Vector2d.Distance(segmentStart, segmentEnd);
 2470263            Fixed64 height = verticalMax - verticalMin;
 2470264            bool areaRepresentable = Fixed64.TryMultiplyAdd(
 2470265                width,
 2470266                height,
 2470267                Fixed64.Zero,
 2470268                out Fixed64 faceArea);
 2470269            return new VoxelContactManifold(
 2470270                source.Cell,
 2470271                target.Cell,
 2470272                sourceToTarget,
 2470273                VoxelContactKind.Face,
 2470274                VoxelContactFaceKind.Vertical,
 2470275                verticalMin,
 2470276                verticalMax,
 2470277                segmentStart,
 2470278                segmentEnd,
 2470279                default,
 2470280                faceArea,
 2470281                areaRepresentable);
 282        }
 283
 110284        VoxelContactKind kind = hasHorizontalSpan || hasVerticalSpan
 110285            ? VoxelContactKind.Edge
 110286            : VoxelContactKind.Point;
 110287        return new VoxelContactManifold(
 110288            source.Cell,
 110289            target.Cell,
 110290            sourceToTarget,
 110291            kind,
 110292            VoxelContactFaceKind.None,
 110293            verticalMin,
 110294            verticalMax,
 110295            segmentStart,
 110296            segmentEnd,
 110297            default,
 110298            Fixed64.Zero,
 110299            true);
 300    }
 301
 302    /// <summary>
 303    /// Attempts to get exact face geometry for a safe same-grid primary adjacency.
 304    /// </summary>
 305    /// <remarks>
 306    /// Rectangular diagonals and hex vertical-diagonal offsets are deliberately rejected.
 307    /// </remarks>
 308    public static bool TryGetPrimaryFace(
 309        VoxelGrid grid,
 310        VoxelIndex sourceIndex,
 311        VoxelIndex targetIndex,
 312        out VoxelContactManifold manifold)
 313    {
 23314        if (grid == null)
 1315            throw new ArgumentNullException(nameof(grid));
 316
 22317        manifold = default;
 22318        if (!IsPrimaryOffset(grid.Configuration.TopologyKind, sourceIndex, targetIndex)
 22319            || !TryGetPrism(grid, sourceIndex, out GridCellPrism source)
 22320            || !TryGetPrism(grid, targetIndex, out GridCellPrism target))
 321        {
 5322            return false;
 323        }
 324
 17325        manifold = GetContact(source, target);
 17326        return manifold.Kind == VoxelContactKind.Face;
 327    }
 328
 329    /// <summary>
 330    /// Builds exact contacts between physical voxels in a candidate grid pair into caller-owned output.
 331    /// </summary>
 332    /// <remarks>
 333    /// Source and target cells are processed in canonical local-index order. The supplied scratch retains
 334    /// broad-phase capacity so warmed calls allocate no managed memory. Separated AABB candidates are omitted.
 335    /// </remarks>
 336    /// <returns>The number of manifolds written to <paramref name="results"/>.</returns>
 337    public static int GetExactBoundaryContactsInto(
 338        VoxelGrid sourceGrid,
 339        VoxelGrid targetGrid,
 340        SwiftList<VoxelContactManifold> results,
 341        GridContactQueryScratch scratch)
 342    {
 14343        ChronicleGuard.ThrowIfNull(sourceGrid, nameof(sourceGrid));
 14344        ChronicleGuard.ThrowIfNull(targetGrid, nameof(targetGrid));
 14345        ChronicleGuard.ThrowIfNull(results, nameof(results));
 14346        ChronicleGuard.ThrowIfNull(scratch, nameof(scratch));
 347
 14348        results.Clear();
 14349        scratch.Clear();
 14350        if (ReferenceEquals(sourceGrid, targetGrid)
 14351            || !sourceGrid.IsActive
 14352            || !targetGrid.IsActive
 14353            || !ReferenceEquals(sourceGrid.World, targetGrid.World))
 354        {
 4355            return 0;
 356        }
 357
 358        try
 359        {
 10360            CollectPotentialSourceVoxels(sourceGrid, targetGrid, scratch);
 361
 536362            for (int sourceIndex = 0; sourceIndex < scratch.SourceVoxels.Count; sourceIndex++)
 363            {
 258364                Voxel sourceVoxel = scratch.SourceVoxels[sourceIndex];
 258365                if (!TryGetPrism(sourceGrid, sourceVoxel.Index, out GridCellPrism sourcePrism))
 366                    continue;
 367
 257368                TopologyVoxelAabb sourceAabb = sourcePrism.GetAabb();
 257369                TopologyVoxelAabb broadPhaseBounds = sourceAabb.Expand(targetGrid.Topology.MaxCellEdge);
 257370                if (!TopologyVoxelRangeUtility.TryGetCandidateRange(
 257371                    targetGrid,
 257372                    broadPhaseBounds,
 257373                    out VoxelIndex minIndex,
 257374                    out VoxelIndex maxIndex))
 375                {
 376                    continue;
 377                }
 378
 257379                scratch.CandidateVoxels.Clear();
 257380                scratch.ProcessedVoxels.Clear();
 257381                targetGrid.AddVoxelsInIndexRange(
 257382                    minIndex,
 257383                    maxIndex,
 257384                    scratch.CandidateVoxels,
 257385                    scratch.ProcessedVoxels);
 257386                SortByVoxelIndex(scratch.CandidateVoxels);
 387
 1532388                for (int targetIndex = 0; targetIndex < scratch.CandidateVoxels.Count; targetIndex++)
 389                {
 509390                    Voxel targetVoxel = scratch.CandidateVoxels[targetIndex];
 509391                    if (!TryGetPrism(targetGrid, targetVoxel.Index, out GridCellPrism targetPrism)
 509392                        || !sourceAabb.Overlaps(targetPrism.GetAabb(), Fixed64.Zero))
 393                    {
 394                        continue;
 395                    }
 396
 113397                    VoxelContactManifold manifold = GetContact(sourcePrism, targetPrism);
 113398                    if (manifold.Kind != VoxelContactKind.Separated)
 113399                        results.Add(manifold);
 400                }
 401            }
 402
 10403            return results.Count;
 404        }
 405        finally
 406        {
 10407            scratch.Clear();
 10408        }
 10409    }
 410
 411    private static int BuildFootprintIntersection(
 412        in GridCellPrism source,
 413        in GridCellPrism target,
 414        Span<Vector2d> intersection)
 415    {
 2661416        Span<Vector2d> sourceVertices = stackalloc Vector2d[6];
 2661417        Span<Vector2d> targetVertices = stackalloc Vector2d[6];
 2661418        source.CopyFootprintTo(sourceVertices);
 2661419        target.CopyFootprintTo(targetVertices);
 2661420        ReadOnlySpan<Vector2d> sourceFootprint = sourceVertices[..source.FootprintVertexCount];
 2661421        ReadOnlySpan<Vector2d> targetFootprint = targetVertices[..target.FootprintVertexCount];
 2661422        Span<Vector2d> sourceOffsets = stackalloc Vector2d[6];
 2661423        Span<Vector2d> targetOffsets = stackalloc Vector2d[6];
 2661424        Vector2d sourceOrigin = new Vector2d(source.Center.X, source.Center.Z);
 2661425        Vector2d targetOrigin = new Vector2d(target.Center.X, target.Center.Z);
 26870426        for (int i = 0; i < sourceFootprint.Length; i++)
 10774427            sourceOffsets[i] = sourceFootprint[i] - sourceOrigin;
 26886428        for (int i = 0; i < targetFootprint.Length; i++)
 10782429            targetOffsets[i] = targetFootprint[i] - targetOrigin;
 2661430        ReadOnlySpan<Vector2d> resolvedSourceOffsets = sourceOffsets[..sourceFootprint.Length];
 2661431        ReadOnlySpan<Vector2d> resolvedTargetOffsets = targetOffsets[..targetFootprint.Length];
 432        // Default prisms must retain the generic empty-footprint validation.
 2661433        bool sourceRectangular = source.TopologyKind == GridTopologyKind.RectangularPrism
 2661434            && source.FootprintVertexCount == 4;
 2661435        bool targetRectangular = target.TopologyKind == GridTopologyKind.RectangularPrism
 2661436            && target.FootprintVertexCount == 4;
 2661437        Span<Vector2d> candidates = stackalloc Vector2d[MaximumIntersectionCandidateCount];
 2661438        int candidateCount = 0;
 439
 26862440        for (int i = 0; i < sourceFootprint.Length; i++)
 441        {
 10771442            Vector2d vertex = sourceFootprint[i];
 10771443            if (targetRectangular
 10771444                ? IsPlanarPointContained(target, vertex)
 10771445                : FixedConvex2dRelations.ContainsPoint(vertex, targetOrigin, resolvedTargetOffsets))
 5255446                AddUnique(candidates, ref candidateCount, vertex);
 447        }
 448
 26876449        for (int i = 0; i < targetFootprint.Length; i++)
 450        {
 10779451            Vector2d vertex = targetFootprint[i];
 10779452            if (sourceRectangular
 10779453                ? IsPlanarPointContained(source, vertex)
 10779454                : FixedConvex2dRelations.ContainsPoint(vertex, sourceOrigin, resolvedSourceOffsets))
 5279455                AddUnique(candidates, ref candidateCount, vertex);
 456        }
 457
 26858458        for (int sourceEdge = 0; sourceEdge < sourceFootprint.Length; sourceEdge++)
 459        {
 10770460            FixedSegment2d sourceSegment = new FixedSegment2d(
 10770461                sourceFootprint[sourceEdge],
 10770462                sourceFootprint[(sourceEdge + 1) % sourceFootprint.Length]);
 109388463            for (int targetEdge = 0; targetEdge < targetFootprint.Length; targetEdge++)
 464            {
 43924465                FixedSegment2d targetSegment = new FixedSegment2d(
 43924466                    targetFootprint[targetEdge],
 43924467                    targetFootprint[(targetEdge + 1) % targetFootprint.Length]);
 43924468                if (sourceSegment.TryGetUniqueIntersection(targetSegment, out Fixed64 parameter))
 469                {
 15560470                    AddUnique(
 15560471                        candidates,
 15560472                        ref candidateCount,
 15560473                        Vector2d.Lerp(sourceSegment.Start, sourceSegment.End, parameter));
 474                }
 475            }
 476        }
 477
 2659478        if (candidateCount == 0)
 15479            return 0;
 2644480        if (candidateCount == 1)
 481        {
 68482            intersection[0] = candidates[0];
 68483            return 1;
 484        }
 485
 2576486        return BuildConvexHull(candidates[..candidateCount], intersection);
 487    }
 488
 489    private static int BuildConvexHull(Span<Vector2d> candidates, Span<Vector2d> destination)
 490    {
 10606491        for (int i = 1; i < candidates.Length; i++)
 492        {
 2727493            Vector2d candidate = candidates[i];
 2727494            int insertion = i - 1;
 2999495            while (insertion >= 0 && CompareCoordinates(candidates[insertion], candidate) > 0)
 496            {
 272497                candidates[insertion + 1] = candidates[insertion];
 272498                insertion--;
 499            }
 500
 2727501            candidates[insertion + 1] = candidate;
 502        }
 503
 2576504        Span<Vector2d> hull = stackalloc Vector2d[MaximumIntersectionCandidateCount * 2];
 2576505        int count = 0;
 15758506        for (int i = 0; i < candidates.Length; i++)
 507        {
 5380508            while (count >= 2
 5380509                && Vector2d.OrientationSign(hull[count - 2], hull[count - 1], candidates[i]) <= 0)
 77510                count--;
 511
 5303512            hull[count++] = candidates[i];
 513        }
 514
 2576515        int lowerCount = count;
 10606516        for (int i = candidates.Length - 2; i >= 0; i--)
 517        {
 2808518            while (count > lowerCount
 2808519                && Vector2d.OrientationSign(hull[count - 2], hull[count - 1], candidates[i]) <= 0)
 520            {
 81521                count--;
 522            }
 523
 2727524            hull[count++] = candidates[i];
 525        }
 526
 2576527        count--;
 2576528        hull[..count].CopyTo(destination);
 2576529        return count;
 530    }
 531
 532    [MethodImpl(MethodImplOptions.AggressiveInlining)]
 533    private static bool TryGetExactHalf(Fixed64 value, out Fixed64 half)
 534    {
 84463535        if ((value.m_rawValue & 1L) != 0L)
 536        {
 24537            half = default;
 24538            return false;
 539        }
 540
 84439541        half = Fixed64.FromRaw(value.m_rawValue >> 1);
 84439542        return true;
 543    }
 544
 545    [MethodImpl(MethodImplOptions.AggressiveInlining)]
 546    private static bool TryGetSymmetricBounds(
 547        Fixed64 center,
 548        Fixed64 extent,
 549        out Fixed64 minimum,
 550        out Fixed64 maximum)
 551    {
 86396552        maximum = default;
 86396553        return Fixed64.TrySubtract(center, extent, out minimum)
 86396554            && Fixed64.TryAdd(center, extent, out maximum);
 555    }
 556
 557    [MethodImpl(MethodImplOptions.AggressiveInlining)]
 558    private static void AddUnique(Span<Vector2d> candidates, ref int count, Vector2d candidate)
 559    {
 79744560        for (int i = 0; i < count; i++)
 561        {
 34501562            if (candidates[i] == candidate)
 20723563                return;
 564        }
 565
 5371566        candidates[count++] = candidate;
 5371567    }
 568
 569    private static void GetSegmentExtents(
 570        ReadOnlySpan<Vector2d> points,
 571        out Vector2d start,
 572        out Vector2d end)
 573    {
 2580574        start = points[0];
 2580575        end = points[^1];
 2580576    }
 577
 578    [MethodImpl(MethodImplOptions.AggressiveInlining)]
 579    private static int CompareCoordinates(Vector2d first, Vector2d second)
 580    {
 2909581        int xComparison = first.X.CompareTo(second.X);
 2909582        return xComparison != 0 ? xComparison : first.Y.CompareTo(second.Y);
 583    }
 584
 585    [MethodImpl(MethodImplOptions.AggressiveInlining)]
 586    private static bool IsPrimaryOffset(
 587        GridTopologyKind topologyKind,
 588        VoxelIndex source,
 589        VoxelIndex target)
 590    {
 22591        int x = target.x - source.x;
 22592        int y = target.y - source.y;
 22593        int z = target.z - source.z;
 22594        if (topologyKind == GridTopologyKind.RectangularPrism)
 595        {
 4596            RectangularDirection direction = RectangularDirectionUtility.GetDirectionFromOffset((x, y, z));
 4597            return RectangularDirectionUtility.IsPerpendicularNeighbor(direction);
 598        }
 599
 18600        VoxelIndex offset = new(x, y, z);
 18601        ReadOnlySpan<HexDirection> directions = HexDirectionUtility.Primary;
 180602        for (int i = 0; i < directions.Length; i++)
 603        {
 88604            if (HexDirectionUtility.GetOffset(directions[i]) == offset)
 16605                return true;
 606        }
 607
 2608        return false;
 609    }
 610
 611    [MethodImpl(MethodImplOptions.AggressiveInlining)]
 612    private static VoxelContactManifold CreateSeparated(
 613        WorldVoxelIndex source,
 614        WorldVoxelIndex target,
 615        Vector3d sourceToTarget) =>
 18616        new VoxelContactManifold(
 18617            source,
 18618            target,
 18619            sourceToTarget,
 18620            VoxelContactKind.Separated,
 18621            VoxelContactFaceKind.None,
 18622            default,
 18623            default,
 18624            default,
 18625            default,
 18626            default,
 18627            Fixed64.Zero,
 18628            true);
 629
 630    private static void SortByVoxelIndex(SwiftList<Voxel> voxels)
 631    {
 267632        Voxel[] items = voxels.InnerArray;
 267633        int count = voxels.Count;
 634
 635        // Array.Sort allocates a comparer-backed sorting helper on this hot path.
 636        // Heap sort keeps ordering deterministic without allocating or degrading
 637        // to quadratic behavior for large grid boundaries.
 1042638        for (int root = (count >> 1) - 1; root >= 0; root--)
 254639            SiftDown(items, root, count);
 640
 1538641        for (int end = count - 1; end > 0; end--)
 642        {
 502643            (items[0], items[end]) = (items[end], items[0]);
 502644            SiftDown(items, 0, end);
 645        }
 267646    }
 647
 648    private static void SiftDown(Voxel[] items, int root, int count)
 649    {
 2846650        while (true)
 651        {
 3602652            int child = (root << 1) + 1;
 3602653            if (child >= count)
 740654                return;
 655
 2862656            int right = child + 1;
 2862657            if (right < count && items[child].Index.CompareTo(items[right].Index) < 0)
 1472658                child = right;
 659
 2862660            if (items[root].Index.CompareTo(items[child].Index) >= 0)
 16661                return;
 662
 2846663            (items[root], items[child]) = (items[child], items[root]);
 2846664            root = child;
 665        }
 666    }
 667
 668    private static void CollectPotentialSourceVoxels(
 669        VoxelGrid sourceGrid,
 670        VoxelGrid targetGrid,
 671        GridContactQueryScratch scratch)
 672    {
 10673        if (targetGrid.Configuration.StorageKind == GridStorageKind.Sparse)
 674        {
 3675            AllocatedVoxelCollector collector = new AllocatedVoxelCollector(scratch.CandidateVoxels);
 3676            targetGrid.VisitVoxels(ref collector);
 16677            for (int i = 0; i < scratch.CandidateVoxels.Count; i++)
 678            {
 5679                Voxel targetVoxel = scratch.CandidateVoxels[i];
 5680                if (!TryGetPrism(targetGrid, targetVoxel.Index, out GridCellPrism targetPrism))
 681                    continue;
 682
 4683                AddSourceCandidates(
 4684                    sourceGrid,
 4685                    targetPrism.GetAabb().Expand(sourceGrid.Topology.MaxCellEdge),
 4686                    scratch);
 687            }
 688        }
 689        else
 690        {
 7691            Fixed64 expansion = targetGrid.Topology.MaxCellEdge + sourceGrid.Topology.MaxCellEdge;
 7692            AddSourceCandidates(
 7693                sourceGrid,
 7694                new TopologyVoxelAabb(targetGrid.BoundsMin, targetGrid.BoundsMax).Expand(expansion),
 7695                scratch);
 696        }
 697
 10698        scratch.CandidateVoxels.Clear();
 10699        scratch.ProcessedVoxels.Clear();
 10700        SortByVoxelIndex(scratch.SourceVoxels);
 10701    }
 702
 703    private static void AddSourceCandidates(
 704        VoxelGrid sourceGrid,
 705        TopologyVoxelAabb targetBounds,
 706        GridContactQueryScratch scratch)
 707    {
 11708        if (!TopologyVoxelRangeUtility.TryGetCandidateRange(
 11709            sourceGrid,
 11710            targetBounds,
 11711            out VoxelIndex minIndex,
 11712            out VoxelIndex maxIndex))
 713        {
 1714            return;
 715        }
 716
 10717        sourceGrid.AddVoxelsInIndexRange(
 10718            minIndex,
 10719            maxIndex,
 10720            scratch.SourceVoxels,
 10721            scratch.ProcessedVoxels);
 10722    }
 723
 724    private readonly struct AllocatedVoxelCollector : IVoxelStorageVisitor
 725    {
 726        private readonly SwiftList<Voxel> _results;
 727
 728        public AllocatedVoxelCollector(SwiftList<Voxel> results)
 729        {
 3730            _results = results;
 3731        }
 732
 733        public bool Visit(Voxel voxel)
 734        {
 5735            _results.Add(voxel);
 5736            return true;
 737        }
 738    }
 739}

/home/runner/work/GridForge/GridForge/src/GridForge/Grids/Topology/GridCellGeometry.NavigationBodyAnchor.cs

#LineLine coverage
 1//=======================================================================
 2// GridCellGeometry.NavigationBodyAnchor.cs
 3//=======================================================================
 4// MIT License, Copyright (c) 2024-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 GridForge.Grids.Topology;
 12
 13public static partial class GridCellGeometry
 14{
 15    /// <summary>
 16    /// Determines whether one cylindrical body anchor fits an exact cell prism, optionally through
 17    /// one selected navigation portal.
 18    /// </summary>
 19    /// <remarks>
 20    /// This is the degenerate-segment form of <see cref="IsNavigationBodySegmentValid"/>. The
 21    /// shared swept-body authority therefore owns all wall, opening, height, and equality behavior.
 22    /// </remarks>
 23    public static bool IsNavigationBodyAnchorValid(
 24        in GridCellPrism prism,
 25        Vector3d foot,
 26        Fixed64 horizontalRadius,
 27        Fixed64 bodyHeight,
 28        in GridNavigationPortal selectedPortal)
 29    {
 108630        return IsNavigationBodySegmentValid(
 108631            prism,
 108632            foot,
 108633            foot,
 108634            horizontalRadius,
 108635            bodyHeight,
 108636            selectedPortal,
 108637            default,
 108638            GridNavigationBodySegmentEndpointAllowance.None);
 39    }
 40
 41    private static bool IsPortalCertifiedOnEdge(
 42        FixedSegment2d edge,
 43        in GridNavigationPortal portal)
 44    {
 949345        FixedSegment2d segmentStart = new(
 949346            portal.VerticalFaceSegmentStart,
 949347            portal.VerticalFaceSegmentStart);
 949348        FixedSegment2d segmentEnd = new(
 949349            portal.VerticalFaceSegmentEnd,
 949350            portal.VerticalFaceSegmentEnd);
 949351        return segmentStart.TryGetUniqueIntersection(edge, out _)
 949352            && segmentEnd.TryGetUniqueIntersection(edge, out _);
 53    }
 54}

/home/runner/work/GridForge/GridForge/src/GridForge/Grids/Topology/GridCellGeometry.NavigationBodySegment.cs

#LineLine coverage
 1//=======================================================================
 2// GridCellGeometry.NavigationBodySegment.cs
 3//=======================================================================
 4// MIT License, Copyright (c) 2024-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 GridForge.Grids.Topology;
 13
 14public static partial class GridCellGeometry
 15{
 16    /// <summary>
 17    /// Attempts to certify where one straight body-foot segment traverses an exact directed portal.
 18    /// </summary>
 19    /// <remarks>
 20    /// Vertical portals return a directed source/target enclosure around the exact crossing.
 21    /// Horizontal portals return the ordered parameters of the exact profile anchors. Both forms
 22    /// certify the body against the authored source/target prism pair.
 23    /// </remarks>
 24    public static bool TryGetNavigationPortalTraversalParameters(
 25        in GridCellPrism sourcePrism,
 26        in GridCellPrism targetPrism,
 27        in GridNavigationPortal portal,
 28        Vector3d footStart,
 29        Vector3d footEnd,
 30        Fixed64 horizontalRadius,
 31        Fixed64 bodyHeight,
 32        out Fixed64 sourceParameter,
 33        out Fixed64 targetParameter)
 34    {
 28735        ChronicleGuard.ThrowIfArgument(
 28736            horizontalRadius < Fixed64.Zero,
 28737            nameof(horizontalRadius),
 28738            "Horizontal radius must be nonnegative.");
 28739        ChronicleGuard.ThrowIfArgument(
 28740            bodyHeight <= Fixed64.Zero,
 28741            nameof(bodyHeight),
 28742            "Body height must be positive.");
 43
 28744        if (!TryCreateNavigationPortal(
 28745                sourcePrism,
 28746                targetPrism,
 28747                out GridNavigationPortal expectedPortal)
 28748            || !AreSamePortal(portal, expectedPortal)
 28749            || !portal.TryResolveProfile(
 28750                horizontalRadius,
 28751                bodyHeight,
 28752                out Vector3d sourceAnchor,
 28753                out Vector3d targetAnchor))
 54        {
 955            sourceParameter = default;
 956            targetParameter = default;
 957            return false;
 58        }
 59
 27860        return TryGetCompiledNavigationPortalTraversalParameters(
 27861            sourcePrism,
 27862            targetPrism,
 27863            portal,
 27864            sourceAnchor,
 27865            targetAnchor,
 27866            footStart,
 27867            footEnd,
 27868            horizontalRadius,
 27869            bodyHeight,
 27870            out sourceParameter,
 27871            out targetParameter);
 72    }
 73
 74    internal static bool TryGetCompiledNavigationPortalTraversalParameters(
 75        in GridCellPrism sourcePrism,
 76        in GridCellPrism targetPrism,
 77        in GridNavigationPortal portal,
 78        Vector3d sourceAnchor,
 79        Vector3d targetAnchor,
 80        Vector3d footStart,
 81        Vector3d footEnd,
 82        Fixed64 horizontalRadius,
 83        Fixed64 bodyHeight,
 84        out Fixed64 sourceParameter,
 85        out Fixed64 targetParameter)
 86    {
 27887        sourceParameter = default;
 27888        targetParameter = default;
 27889        if (portal.FaceKind == VoxelContactFaceKind.Vertical)
 90        {
 27391            FixedSegment2d path = new(
 27392                new Vector2d(footStart.X, footStart.Z),
 27393                new Vector2d(footEnd.X, footEnd.Z));
 27394            FixedSegment2d opening = new(
 27395                portal.VerticalFaceSegmentStart,
 27396                portal.VerticalFaceSegmentEnd);
 27397            if (!IsDirectedPortalCrossing(sourcePrism, path, opening)
 27398                || !path.TryGetUniqueIntersectionParameterEnclosure(
 27399                    opening,
 273100                    out _,
 273101                    out sourceParameter,
 273102                    out targetParameter))
 103            {
 4104                sourceParameter = default;
 4105                targetParameter = default;
 4106                return false;
 107            }
 108
 269109            Vector3d sourcePoint = Vector3d.Lerp(footStart, footEnd, sourceParameter);
 269110            Vector3d targetPoint = Vector3d.Lerp(footStart, footEnd, targetParameter);
 269111            FixedSegment2d traversalGap = new(
 269112                new Vector2d(sourcePoint.X, sourcePoint.Z),
 269113                new Vector2d(targetPoint.X, targetPoint.Z));
 269114            path.TryGetCapsuleIntersectionParameterEnclosure(
 269115                opening,
 269116                horizontalRadius,
 269117                out Fixed64 overlapEntry,
 269118                out Fixed64 overlapExit);
 269119            if (!IsPortalTraversalGapPlanarValid(
 269120                    sourcePrism,
 269121                    traversalGap,
 269122                    horizontalRadius,
 269123                    portal)
 269124                || !IsPortalTraversalGapPlanarValid(
 269125                    targetPrism,
 269126                    traversalGap,
 269127                    horizontalRadius,
 269128                    portal)
 269129                || !IsPortalHeightValidOverInterval(
 269130                    footStart.Y,
 269131                    footEnd.Y,
 269132                    overlapEntry,
 269133                    overlapExit,
 269134                    bodyHeight,
 269135                    portal))
 136            {
 7137                sourceParameter = default;
 7138                targetParameter = default;
 7139                return false;
 140            }
 141
 262142            return true;
 143        }
 144
 5145        if (!TryGetPointParameter(footStart, footEnd, sourceAnchor, out sourceParameter)
 5146            || !TryGetPointParameter(footStart, footEnd, targetAnchor, out targetParameter)
 5147            || sourceParameter >= targetParameter)
 148        {
 3149            sourceParameter = default;
 3150            targetParameter = default;
 3151            return false;
 152        }
 153
 2154        return true;
 155    }
 156
 157    /// <summary>
 158    /// Determines whether a cylindrical body can sweep one straight foot segment through an exact
 159    /// cell prism, optionally approaching an incoming and outgoing vertical portal.
 160    /// </summary>
 161    /// <remarks>
 162    /// The horizontal capsule is compared exactly with every blocked wall span. Selected portal
 163    /// openings retain their own vertical authority and apply only while the sweep overlaps that
 164    /// opening. Each selected portal must cover its complete possible overlap; a segment that would
 165    /// need to switch height authority between two same-wall openings is rejected. A non-default
 166    /// endpoint allowance clips one exact directed footprint-edge crossing inside GridForge before
 167    /// validating the retained in-prism segment. The method retains no state and allocates nothing.
 168    /// </remarks>
 169    public static bool IsNavigationBodySegmentValid(
 170        in GridCellPrism prism,
 171        Vector3d footStart,
 172        Vector3d footEnd,
 173        Fixed64 horizontalRadius,
 174        Fixed64 bodyHeight,
 175        in GridNavigationPortal incomingPortal,
 176        in GridNavigationPortal outgoingPortal,
 177        GridNavigationBodySegmentEndpointAllowance endpointAllowance)
 178    {
 4500179        ChronicleGuard.ThrowIfArgument(
 4500180            horizontalRadius < Fixed64.Zero,
 4500181            nameof(horizontalRadius),
 4500182            "Horizontal radius must be nonnegative.");
 4500183        ChronicleGuard.ThrowIfArgument(
 4500184            bodyHeight <= Fixed64.Zero,
 4500185            nameof(bodyHeight),
 4500186            "Body height must be positive.");
 187
 4500188        if (endpointAllowance != GridNavigationBodySegmentEndpointAllowance.None
 4500189            && endpointAllowance != GridNavigationBodySegmentEndpointAllowance.StartFootprintEdge
 4500190            && endpointAllowance != GridNavigationBodySegmentEndpointAllowance.EndFootprintEdge)
 191        {
 1192            throw new System.ArgumentOutOfRangeException(nameof(endpointAllowance));
 193        }
 194
 4499195        if (!IsNavigationPrismValid(prism))
 1196            return false;
 197
 4498198        int allowedEdgeIndex = -1;
 4498199        if (endpointAllowance == GridNavigationBodySegmentEndpointAllowance.StartFootprintEdge)
 200        {
 7201            if (incomingPortal.IsValid
 7202                || !TryClipNavigationBodySegmentEndpoint(
 7203                    prism,
 7204                    footStart,
 7205                    footEnd,
 7206                    bodyHeight,
 7207                    clipStart: true,
 7208                    out footStart,
 7209                    out allowedEdgeIndex))
 210            {
 5211                return false;
 212            }
 213        }
 4491214        else if (endpointAllowance == GridNavigationBodySegmentEndpointAllowance.EndFootprintEdge)
 215        {
 3216            if (outgoingPortal.IsValid
 3217                || !TryClipNavigationBodySegmentEndpoint(
 3218                    prism,
 3219                    footStart,
 3220                    footEnd,
 3221                    bodyHeight,
 3222                    clipStart: false,
 3223                    out footEnd,
 3224                    out allowedEdgeIndex))
 225            {
 2226                return false;
 227            }
 228        }
 229
 4491230        return IsNavigationBodySegmentValidCore(
 4491231            prism,
 4491232            footStart,
 4491233            footEnd,
 4491234            horizontalRadius,
 4491235            bodyHeight,
 4491236            incomingPortal,
 4491237            outgoingPortal,
 4491238            allowedEdgeIndex);
 239    }
 240
 241    private static bool IsNavigationBodySegmentValidCore(
 242        in GridCellPrism prism,
 243        Vector3d footStart,
 244        Vector3d footEnd,
 245        Fixed64 horizontalRadius,
 246        Fixed64 bodyHeight,
 247        in GridNavigationPortal incomingPortal,
 248        in GridNavigationPortal outgoingPortal,
 249        int allowedEdgeIndex)
 250    {
 4491251        if (!prism.Contains(footStart)
 4491252            || !prism.Contains(footEnd)
 4491253            || !Fixed64.TryAdd(footStart.Y, bodyHeight, out Fixed64 startTop)
 4491254            || !Fixed64.TryAdd(footEnd.Y, bodyHeight, out Fixed64 endTop)
 4491255            || startTop > prism.VerticalMax
 4491256            || endTop > prism.VerticalMax)
 257        {
 9258            return false;
 259        }
 260
 4482261        FixedSegment2d path = new(
 4482262            new Vector2d(footStart.X, footStart.Z),
 4482263            new Vector2d(footEnd.X, footEnd.Z));
 44652264        for (int edgeIndex = 0; edgeIndex < prism.FootprintVertexCount; edgeIndex++)
 265        {
 17904266            if (edgeIndex == allowedEdgeIndex)
 267                continue;
 17901268            Vector2d edgeStart = prism.GetFootprintVertex(edgeIndex);
 17901269            Vector2d edgeEnd = prism.GetFootprintVertex(
 17901270                (edgeIndex + 1) % prism.FootprintVertexCount);
 17901271            FixedSegment2d edge = new(edgeStart, edgeEnd);
 17901272            if (path.IsDistanceAtLeast(edge, horizontalRadius))
 273                continue;
 274
 5382275            bool hasFirst = TryGetActiveOpening(
 5382276                edge,
 5382277                path,
 5382278                footStart.Y,
 5382279                footEnd.Y,
 5382280                horizontalRadius,
 5382281                bodyHeight,
 5382282                incomingPortal,
 5382283                edgeStart,
 5382284                out Vector2d firstStart,
 5382285                out Vector2d firstEnd);
 5382286            bool hasSecond = TryGetActiveOpening(
 5382287                edge,
 5382288                path,
 5382289                footStart.Y,
 5382290                footEnd.Y,
 5382291                horizontalRadius,
 5382292                bodyHeight,
 5382293                outgoingPortal,
 5382294                edgeStart,
 5382295                out Vector2d secondStart,
 5382296                out Vector2d secondEnd);
 5382297            if (!hasFirst && !hasSecond)
 50298                return false;
 5332299            if (!hasFirst)
 300            {
 2279301                firstStart = secondStart;
 2279302                firstEnd = secondEnd;
 2279303                hasSecond = false;
 304            }
 3053305            else if (hasSecond
 3053306                && CompareDistanceFrom(edgeStart, secondStart, firstStart) < 0)
 307            {
 2308                Swap(ref firstStart, ref secondStart);
 2309                Swap(ref firstEnd, ref secondEnd);
 310            }
 311
 5332312            if (!path.IsDistanceAtLeast(
 5332313                    new FixedSegment2d(edgeStart, firstStart),
 5332314                    horizontalRadius))
 315            {
 4316                return false;
 317            }
 318
 5328319            if (!hasSecond
 5328320                || CompareDistanceFrom(edgeStart, secondStart, firstEnd) <= 0)
 321            {
 5326322                if (hasSecond
 5326323                    && CompareDistanceFrom(edgeStart, secondEnd, firstEnd) > 0)
 324                {
 2325                    firstEnd = secondEnd;
 326                }
 327
 5326328                if (!path.IsDistanceAtLeast(
 5326329                        new FixedSegment2d(firstEnd, edgeEnd),
 5326330                        horizontalRadius))
 331                {
 4332                    return false;
 333                }
 334            }
 335            else
 336            {
 2337                return false;
 338            }
 339        }
 340
 4422341        return true;
 342    }
 343
 344    private static bool TryClipNavigationBodySegmentEndpoint(
 345        in GridCellPrism prism,
 346        Vector3d footStart,
 347        Vector3d footEnd,
 348        Fixed64 bodyHeight,
 349        bool clipStart,
 350        out Vector3d clippedEndpoint,
 351        out int allowedEdgeIndex)
 352    {
 8353        clippedEndpoint = default;
 8354        allowedEdgeIndex = -1;
 8355        FixedSegment2d path = new(
 8356            new Vector2d(footStart.X, footStart.Z),
 8357            new Vector2d(footEnd.X, footEnd.Z));
 8358        if (path.Start == path.End)
 1359            return false;
 360
 7361        Vector2d center = new(prism.Center.X, prism.Center.Z);
 7362        Fixed64 lowerParameter = default;
 7363        Fixed64 upperParameter = default;
 60364        for (int edgeIndex = 0; edgeIndex < prism.FootprintVertexCount; edgeIndex++)
 365        {
 25366            Vector2d edgeStart = prism.GetFootprintVertex(edgeIndex);
 25367            Vector2d edgeEnd = prism.GetFootprintVertex(
 25368                (edgeIndex + 1) % prism.FootprintVertexCount);
 25369            FixedSegment2d edge = new(edgeStart, edgeEnd);
 25370            int centerSide = Vector2d.OrientationSign(edgeStart, edgeEnd, center);
 25371            int startSide = Vector2d.OrientationSign(edgeStart, edgeEnd, path.Start);
 25372            int endSide = Vector2d.OrientationSign(edgeStart, edgeEnd, path.End);
 25373            bool directed = clipStart
 25374                ? endSide == centerSide && startSide != centerSide
 25375                : startSide == centerSide && endSide != centerSide;
 25376            if (!directed)
 377                continue;
 6378            if (!path.TryGetUniqueIntersectionParameterEnclosure(
 6379                    edge,
 6380                    out _,
 6381                    out Fixed64 candidateLower,
 6382                    out Fixed64 candidateUpper))
 383            {
 384                continue;
 385            }
 5386            if (new FixedSegment2d(edgeStart, edgeStart).TryGetUniqueIntersection(path, out _)
 5387                || new FixedSegment2d(edgeEnd, edgeEnd).TryGetUniqueIntersection(path, out _))
 388            {
 2389                return false;
 390            }
 3391            allowedEdgeIndex = edgeIndex;
 3392            lowerParameter = candidateLower;
 3393            upperParameter = candidateUpper;
 394        }
 395
 5396        if (allowedEdgeIndex < 0
 5397            || !IsBodyHeightValidOverInterval(
 5398                footStart.Y,
 5399                footEnd.Y,
 5400                lowerParameter,
 5401                upperParameter,
 5402                bodyHeight,
 5403                prism.VerticalMin,
 5404                prism.VerticalMax))
 405        {
 2406            return false;
 407        }
 408
 3409        Fixed64 containedParameter = clipStart ? upperParameter : lowerParameter;
 3410        clippedEndpoint = Vector3d.Lerp(footStart, footEnd, containedParameter);
 3411        return prism.Contains(clippedEndpoint);
 412    }
 413
 414    private static bool TryGetActiveOpening(
 415        FixedSegment2d edge,
 416        FixedSegment2d path,
 417        Fixed64 footStartY,
 418        Fixed64 footEndY,
 419        Fixed64 horizontalRadius,
 420        Fixed64 bodyHeight,
 421        in GridNavigationPortal portal,
 422        Vector2d edgeStart,
 423        out Vector2d openingStart,
 424        out Vector2d openingEnd)
 425    {
 10764426        openingStart = default;
 10764427        openingEnd = default;
 10764428        if (!portal.IsValid
 10764429            || portal.FaceKind != VoxelContactFaceKind.Vertical
 10764430            || horizontalRadius > portal.MaximumHorizontalRadius
 10764431            || bodyHeight > portal.MaximumBodyHeight
 10764432            || !IsPortalCertifiedOnEdge(edge, portal))
 433        {
 5417434            return false;
 435        }
 436
 5347437        openingStart = portal.VerticalFaceSegmentStart;
 5347438        openingEnd = portal.VerticalFaceSegmentEnd;
 5347439        if (CompareDistanceFrom(edgeStart, openingEnd, openingStart) < 0)
 2028440            Swap(ref openingStart, ref openingEnd);
 441
 5347442        FixedSegment2d opening = new(openingStart, openingEnd);
 5347443        if (!path.TryGetCapsuleIntersectionParameterEnclosure(
 5347444                opening,
 5347445                horizontalRadius,
 5347446                out Fixed64 entry,
 5347447                out Fixed64 exit))
 448        {
 2449            return false;
 450        }
 451
 5345452        return IsPortalHeightValidOverInterval(
 5345453            footStartY,
 5345454            footEndY,
 5345455            entry,
 5345456            exit,
 5345457            bodyHeight,
 5345458            portal);
 459    }
 460
 461    private static bool IsDirectedPortalCrossing(
 462        in GridCellPrism sourcePrism,
 463        FixedSegment2d path,
 464        FixedSegment2d opening)
 465    {
 273466        if (path.Start == path.End)
 1467            return false;
 468
 272469        Vector2d sourceCenter = new(sourcePrism.Center.X, sourcePrism.Center.Z);
 272470        int sourceSide = Vector2d.OrientationSign(opening.Start, opening.End, sourceCenter);
 272471        int startSide = Vector2d.OrientationSign(opening.Start, opening.End, path.Start);
 272472        int endSide = Vector2d.OrientationSign(opening.Start, opening.End, path.End);
 272473        return (startSide == 0 || startSide == sourceSide)
 272474            && (endSide == 0 || endSide == -sourceSide)
 272475            && (startSide != 0 || endSide != 0);
 476    }
 477
 478    private static bool IsPortalTraversalGapPlanarValid(
 479        in GridCellPrism prism,
 480        FixedSegment2d traversalGap,
 481        Fixed64 horizontalRadius,
 482        in GridNavigationPortal portal)
 483    {
 5324484        for (int edgeIndex = 0; edgeIndex < prism.FootprintVertexCount; edgeIndex++)
 485        {
 2131486            Vector2d edgeStart = prism.GetFootprintVertex(edgeIndex);
 2131487            Vector2d edgeEnd = prism.GetFootprintVertex(
 2131488                (edgeIndex + 1) % prism.FootprintVertexCount);
 2131489            FixedSegment2d edge = new(edgeStart, edgeEnd);
 2131490            if (!IsPortalCertifiedOnEdge(edge, portal))
 491            {
 1598492                if (!traversalGap.IsDistanceAtLeast(
 1598493                        edge,
 1598494                        horizontalRadius))
 495                {
 2496                    return false;
 497                }
 498                continue;
 499            }
 500
 533501            Vector2d openingStart = portal.VerticalFaceSegmentStart;
 533502            Vector2d openingEnd = portal.VerticalFaceSegmentEnd;
 533503            if (CompareDistanceFrom(edgeStart, openingEnd, openingStart) < 0)
 265504                Swap(ref openingStart, ref openingEnd);
 533505            if (!traversalGap.IsDistanceAtLeast(
 533506                    new FixedSegment2d(edgeStart, openingStart),
 533507                    horizontalRadius)
 533508                || !traversalGap.IsDistanceAtLeast(
 533509                    new FixedSegment2d(openingEnd, edgeEnd),
 533510                    horizontalRadius))
 511            {
 2512                return false;
 513            }
 514        }
 515
 531516        return true;
 517    }
 518
 519    private static bool IsPortalHeightValidOverInterval(
 520        Fixed64 footStartY,
 521        Fixed64 footEndY,
 522        Fixed64 entryParameter,
 523        Fixed64 exitParameter,
 524        Fixed64 bodyHeight,
 525        in GridNavigationPortal portal)
 526    {
 5610527        Fixed64 portalTop = portal.CanonicalFacePoint.Y + portal.MaximumBodyHeight;
 5610528        return IsBodyHeightValidOverInterval(
 5610529                footStartY,
 5610530                footEndY,
 5610531                entryParameter,
 5610532                exitParameter,
 5610533                bodyHeight,
 5610534                portal.CanonicalFacePoint.Y,
 5610535                portalTop);
 536    }
 537
 538    private static bool IsBodyHeightValidOverInterval(
 539        Fixed64 footStartY,
 540        Fixed64 footEndY,
 541        Fixed64 entryParameter,
 542        Fixed64 exitParameter,
 543        Fixed64 bodyHeight,
 544        Fixed64 verticalMin,
 545        Fixed64 verticalMax)
 546    {
 5613547        GetConservativeLerpBounds(
 5613548            footStartY,
 5613549            footEndY,
 5613550            entryParameter,
 5613551            out Fixed64 lowerStartY,
 5613552            out Fixed64 upperStartY);
 5613553        GetConservativeLerpBounds(
 5613554            footStartY,
 5613555            footEndY,
 5613556            exitParameter,
 5613557            out Fixed64 lowerEndY,
 5613558            out Fixed64 upperEndY);
 5613559        Fixed64 minimumFootY = FixedMath.Min(lowerStartY, lowerEndY);
 5613560        Fixed64 maximumFootY = FixedMath.Max(upperStartY, upperEndY);
 5613561        return Fixed64.TryAdd(maximumFootY, bodyHeight, out Fixed64 maximumTop)
 5613562            && minimumFootY >= verticalMin
 5613563            && maximumTop <= verticalMax;
 564    }
 565
 566    private static void GetConservativeLerpBounds(
 567        Fixed64 start,
 568        Fixed64 end,
 569        Fixed64 parameter,
 570        out Fixed64 lower,
 571        out Fixed64 upper)
 572    {
 11226573        lower = FixedMath.Lerp(start, end, parameter);
 11226574        upper = lower;
 11226575        if (start == end || parameter == Fixed64.Zero || parameter == Fixed64.One)
 11218576            return;
 8577        if (lower > Fixed64.MinValue)
 7578            lower = Fixed64.FromRaw(lower.m_rawValue - 1L);
 8579        if (upper < Fixed64.MaxValue)
 7580            upper = Fixed64.FromRaw(upper.m_rawValue + 1L);
 8581    }
 582
 583    private static bool TryGetPointParameter(
 584        Vector3d segmentStart,
 585        Vector3d segmentEnd,
 586        Vector3d point,
 587        out Fixed64 parameter)
 588    {
 9589        parameter = default;
 9590        FixedSegment segment = new(segmentStart, segmentEnd);
 9591        if (!segment.Contains(point) || segmentStart.Y == segmentEnd.Y)
 2592            return false;
 593
 7594        FixedSegment2d vertical = new(
 7595            new Vector2d(segmentStart.Y, Fixed64.Zero),
 7596            new Vector2d(segmentEnd.Y, Fixed64.Zero));
 7597        FixedSegment2d verticalPoint = new(
 7598            new Vector2d(point.Y, Fixed64.Zero),
 7599            new Vector2d(point.Y, Fixed64.Zero));
 7600        return vertical.TryGetUniqueIntersection(verticalPoint, out parameter);
 601    }
 602
 603    private static bool AreSamePortal(
 604        in GridNavigationPortal first,
 605        in GridNavigationPortal second)
 606    {
 286607        return first.FaceKind == second.FaceKind
 286608            && first.SourceToTarget == second.SourceToTarget
 286609            && first.CanonicalFacePoint == second.CanonicalFacePoint
 286610            && first.MaximumHorizontalRadius == second.MaximumHorizontalRadius
 286611            && first.MaximumBodyHeight == second.MaximumBodyHeight
 286612            && first.VerticalFaceSegmentStart == second.VerticalFaceSegmentStart
 286613            && first.VerticalFaceSegmentEnd == second.VerticalFaceSegmentEnd;
 614    }
 615
 616    private static int CompareDistanceFrom(
 617        Vector2d origin,
 618        Vector2d first,
 619        Vector2d second)
 620    {
 5893621        return Vector2d.CompareDistanceSquared(origin, first, origin, second);
 622    }
 623
 624    private static void Swap(ref Vector2d first, ref Vector2d second)
 625    {
 2297626        Vector2d value = first;
 2297627        first = second;
 2297628        second = value;
 2297629    }
 630
 631    internal static bool HasPositiveNavigationBodyPrismOverlap(
 632        in GridCellPrism prism,
 633        Vector3d footStart,
 634        Vector3d footEnd,
 635        Fixed64 horizontalRadius,
 636        Fixed64 bodyHeight)
 637    {
 2923638        Fixed64 prismHalfThickness = prism.VerticalMax - prism.Center.Y;
 639
 2923640        Span<Vector2d> offsets = stackalloc Vector2d[6];
 2923641        Vector2d planarOrigin = new(prism.Center.X, prism.Center.Z);
 29498642        for (int i = 0; i < prism.FootprintVertexCount; i++)
 11826643            offsets[i] = prism.GetFootprintVertex(i) - planarOrigin;
 644
 2923645        return FixedConvexPrismRelations.IntersectsSweptUprightCylinderStrict(
 2923646            footStart,
 2923647            footEnd,
 2923648            horizontalRadius,
 2923649            bodyHeight,
 2923650            prism.Center,
 2923651            Fixed64.Zero,
 2923652            offsets[..prism.FootprintVertexCount],
 2923653            prismHalfThickness);
 654    }
 655
 656    internal static bool TryGetPlanarSegmentInterval(
 657        in GridCellPrism prism,
 658        Vector2d start,
 659        Vector2d end,
 660        out Fixed64 overlapEnter,
 661        out Fixed64 overlapExit)
 662    {
 2746663        if (prism.TopologyKind == GridTopologyKind.RectangularPrism && prism.FootprintVertexCount == 4)
 664        {
 2402665            Vector2d minimum = prism.GetFootprintVertex(0);
 2402666            Vector2d maximum = prism.GetFootprintVertex(2);
 667            // Both nonzero axes must qualify before even a miss is handled.
 668            // Axis-aligned rays retain the generic collinear projection behavior.
 2402669            if (TryGetPlanarSlab(start.X, end.X, minimum.X, maximum.X,
 2402670                    out Fixed64 denominatorX, out Fixed64 lowerX, out Fixed64 upperX)
 2402671                && TryGetPlanarSlab(start.Y, end.Y, minimum.Y, maximum.Y,
 2402672                    out Fixed64 denominatorZ, out Fixed64 lowerZ, out Fixed64 upperZ))
 673            {
 1934674                lowerX = FixedMath.Max(Fixed64.Zero, lowerX);
 1934675                upperX = FixedMath.Min(denominatorX, upperX);
 1934676                lowerZ = FixedMath.Max(Fixed64.Zero, lowerZ);
 1934677                upperZ = FixedMath.Min(denominatorZ, upperZ);
 1934678                overlapEnter = default;
 1934679                overlapExit = default;
 680                // Compare the exact rational bounds before rounding: separated
 681                // bounds can otherwise collapse onto the same raw parameter.
 1934682                if (lowerX > upperX || lowerZ > upperZ
 1934683                    || Fixed64.CompareProducts(lowerX, denominatorZ, upperZ, denominatorX) > 0
 1934684                    || Fixed64.CompareProducts(lowerZ, denominatorX, upperX, denominatorZ) > 0)
 60685                    return false;
 686
 1874687                overlapEnter = Fixed64.CompareProducts(lowerX, denominatorZ, lowerZ, denominatorX) >= 0
 1874688                    ? lowerX / denominatorX
 1874689                    : lowerZ / denominatorZ;
 1874690                overlapExit = Fixed64.CompareProducts(upperX, denominatorZ, upperZ, denominatorX) <= 0
 1874691                    ? upperX / denominatorX
 1874692                    : upperZ / denominatorZ;
 1874693                return true;
 694            }
 695        }
 696
 812697        Span<Vector2d> vertices = stackalloc Vector2d[6];
 812698        prism.CopyFootprintTo(vertices);
 699        // A convex footprint strictly on one side cannot meet the supporting line;
 700        // zero or mixed signs retain the existing exact interval path.
 812701        if (IsFootprintStrictlySeparatedFromLine(vertices[..prism.FootprintVertexCount], start, end))
 702        {
 142703            overlapEnter = default;
 142704            overlapExit = default;
 142705            return false;
 706        }
 707
 670708        Vector2d origin = new(prism.Center.X, prism.Center.Z);
 670709        Span<Vector2d> offsets = stackalloc Vector2d[6];
 7748710        for (int i = 0; i < prism.FootprintVertexCount; i++)
 3204711            offsets[i] = vertices[i] - origin;
 670712        ReadOnlySpan<Vector2d> footprint = offsets[..prism.FootprintVertexCount];
 713
 670714        bool startContained = FixedConvex2dRelations.ContainsPoint(start, origin, footprint);
 668715        if (start == end)
 716        {
 221717            overlapEnter = Fixed64.Zero;
 221718            overlapExit = Fixed64.One;
 221719            return startContained;
 720        }
 721
 447722        Span<Fixed64> parameters = stackalloc Fixed64[16];
 447723        int count = 0;
 447724        if (startContained)
 100725            parameters[count++] = Fixed64.Zero;
 447726        if (FixedConvex2dRelations.ContainsPoint(end, origin, footprint))
 103727            parameters[count++] = Fixed64.One;
 728
 447729        FixedSegment2d path = new(start, end);
 4910730        for (int i = 0; i < prism.FootprintVertexCount; i++)
 731        {
 2008732            FixedSegment2d edge = new(
 2008733                vertices[i],
 2008734                vertices[(i + 1) % prism.FootprintVertexCount]);
 2008735            if (path.TryGetUniqueIntersection(edge, out Fixed64 parameter))
 781736                AddNavigationBodyParameter(parameters, ref count, parameter);
 2008737            if (Vector2d.OrientationSign(start, end, edge.Start) == 0
 2008738                && Vector2d.OrientationSign(start, end, edge.End) == 0)
 739            {
 55740                AddNavigationBodyParameter(parameters, ref count, GetNavigationBodyParameter(path, edge.Start));
 55741                AddNavigationBodyParameter(parameters, ref count, GetNavigationBodyParameter(path, edge.End));
 742            }
 743        }
 744
 447745        if (count == 0)
 746        {
 2747            overlapEnter = default;
 2748            overlapExit = default;
 2749            return false;
 750        }
 751
 445752        overlapEnter = parameters[0];
 445753        overlapExit = parameters[0];
 1760754        for (int i = 1; i < count; i++)
 755        {
 435756            overlapEnter = FixedMath.Min(overlapEnter, parameters[i]);
 435757            overlapExit = FixedMath.Max(overlapExit, parameters[i]);
 758        }
 445759        return true;
 760    }
 761
 762    internal static bool TryGetPlanarSlab(
 763        Fixed64 start, Fixed64 end, Fixed64 minimum, Fixed64 maximum,
 764        out Fixed64 denominator, out Fixed64 lower, out Fixed64 upper)
 765    {
 4915766        denominator = lower = upper = default;
 4915767        if (end > start)
 2949768            return Fixed64.TrySubtract(end, start, out denominator)
 2949769                && Fixed64.TrySubtract(minimum, start, out lower)
 2949770                && Fixed64.TrySubtract(maximum, start, out upper);
 771
 1966772        return end < start
 1966773            && Fixed64.TrySubtract(start, end, out denominator)
 1966774            && Fixed64.TrySubtract(start, maximum, out lower)
 1966775            && Fixed64.TrySubtract(start, minimum, out upper);
 776    }
 777
 778    private static bool IsFootprintStrictlySeparatedFromLine(
 779        ReadOnlySpan<Vector2d> vertices,
 780        Vector2d start,
 781        Vector2d end)
 782    {
 812783        if (vertices.IsEmpty)
 2784            return false;
 785
 810786        int side = Vector2d.OrientationSign(start, end, vertices[0]);
 810787        if (side == 0)
 269788            return false;
 3040789        for (int i = 1; i < vertices.Length; i++)
 790        {
 1378791            if (Vector2d.OrientationSign(start, end, vertices[i]) != side)
 399792                return false;
 793        }
 142794        return true;
 795    }
 796
 797    private static void AddNavigationBodyParameter(
 798        Span<Fixed64> parameters,
 799        ref int count,
 800        Fixed64 parameter)
 801    {
 891802        if ((ulong)parameter.m_rawValue > (ulong)Fixed64.One.m_rawValue)
 8803            return;
 2670804        for (int i = 0; i < count; i++)
 805        {
 658806            if (parameters[i] == parameter)
 206807                return;
 808        }
 677809        parameters[count++] = parameter;
 677810    }
 811
 812    private static Fixed64 GetNavigationBodyParameter(FixedSegment2d path, Vector2d point)
 813    {
 110814        Vector2d delta = path.Delta;
 110815        return FixedMath.Abs(delta.X) >= FixedMath.Abs(delta.Y)
 110816            ? (point.X - path.Start.X) / delta.X
 110817            : (point.Y - path.Start.Y) / delta.Y;
 818    }
 819
 820}

/home/runner/work/GridForge/GridForge/src/GridForge/Grids/Topology/GridCellGeometry.NavigationCorridor.cs

#LineLine coverage
 1//=======================================================================
 2// GridCellGeometry.NavigationCorridor.cs
 3//=======================================================================
 4// MIT License, Copyright (c) 2024-present David Oravsky (mrdav30)
 5// See LICENSE file in the project root for full license information.
 6//=======================================================================
 7
 8using System;
 9using FixedMathSharp;
 10
 11namespace GridForge.Grids.Topology;
 12
 13public static partial class GridCellGeometry
 14{
 15    /// <summary>
 16    /// Validates a canonical, clearance-bearing corridor through an ordered chain of exact cell prisms.
 17    /// </summary>
 18    /// <remarks>
 19    /// Consecutive cells must share a positive-area face. Vertical faces contribute one portal-center
 20    /// waypoint. Horizontal faces contribute the last point wholly contained by the source cell and the
 21    /// first point wholly contained by the target cell. The resulting polyline and checked length are
 22    /// deterministic and independent of runtime grid identity.
 23    /// </remarks>
 24    /// <param name="orderedCells">Source cell, zero or more witness cells, and destination cell.</param>
 25    /// <param name="entryAnchor">Foot position inside the source cell.</param>
 26    /// <param name="exitAnchor">Foot position inside the destination cell.</param>
 27    /// <param name="radiusClearance">Required horizontal body radius.</param>
 28    /// <param name="heightClearance">Required positive body height.</param>
 29    /// <param name="portalWaypoints">Caller-owned storage with capacity for twice the portal count.</param>
 30    /// <param name="portalWaypointCount">The number of canonical portal waypoints written.</param>
 31    /// <param name="geometricCost">The checked fixed-point length of the canonical polyline.</param>
 32    public static bool TryValidateNavigationCorridor(
 33        ReadOnlySpan<GridCellPrism> orderedCells,
 34        Vector3d entryAnchor,
 35        Vector3d exitAnchor,
 36        Fixed64 radiusClearance,
 37        Fixed64 heightClearance,
 38        Span<Vector3d> portalWaypoints,
 39        out int portalWaypointCount,
 40        out Fixed64 geometricCost)
 41    {
 1442        var cursor = new GridNavigationCorridorValidationCursor(
 1443            orderedCells.Length,
 1444            entryAnchor,
 1445            exitAnchor,
 1446            radiusClearance,
 1447            heightClearance);
 2748        while (cursor.Status == GridNavigationCorridorValidationStatus.InProgress)
 1349            cursor.Advance(orderedCells, portalWaypoints, int.MaxValue);
 50
 1451        portalWaypointCount = cursor.PortalWaypointCount;
 1452        geometricCost = cursor.GeometricCost;
 1453        return cursor.Status == GridNavigationCorridorValidationStatus.Complete;
 54    }
 55}

/home/runner/work/GridForge/GridForge/src/GridForge/Grids/Topology/GridCellGeometry.NavigationPortal.cs

#LineLine coverage
 1//=======================================================================
 2// GridCellGeometry.NavigationPortal.cs
 3//=======================================================================
 4// MIT License, Copyright (c) 2024-present David Oravsky (mrdav30)
 5// See LICENSE file in the project root for full license information.
 6//=======================================================================
 7
 8using System;
 9using System.Diagnostics;
 10using FixedMathSharp;
 11using FixedMathSharp.Geometry;
 12
 13namespace GridForge.Grids.Topology;
 14
 15public static partial class GridCellGeometry
 16{
 17    /// <summary>
 18    /// Attempts to compile one exact, directed navigation portal from two cell prisms.
 19    /// </summary>
 20    /// <remarks>
 21    /// Contact discovery and convex clipping occur only during compilation. The resulting value
 22    /// contains no live grid references and resolves body profiles in constant time.
 23    /// </remarks>
 24    public static bool TryCreateNavigationPortal(
 25        in GridCellPrism source,
 26        in GridCellPrism target,
 27        out GridNavigationPortal portal)
 28    {
 243329        portal = default;
 243330        if (!IsNavigationPrismValid(source)
 243331            || !IsNavigationPrismValid(target))
 32        {
 233            return false;
 34        }
 35
 243136        VoxelContactManifold contact = GetContact(source, target);
 243137        if (!contact.IsPositiveAreaFace)
 1238            return false;
 241939        Vector3d sourceToTarget = contact.SourceToTarget;
 40
 241941        if (contact.FaceKind == VoxelContactFaceKind.Vertical)
 42        {
 239843            Fixed64 faceHeight = contact.VerticalMax - contact.VerticalMin;
 44
 239845            Vector2d center = GetNearestRepresentableSegmentMidpoint(
 239846                contact.HorizontalSegmentStart,
 239847                contact.HorizontalSegmentEnd);
 239848            var canonicalFacePoint = new Vector3d(center.X, contact.VerticalMin, center.Y);
 239849            Fixed64 startClearance = GetConservativeDistance(
 239850                contact.HorizontalSegmentStart,
 239851                center);
 239852            Fixed64 endClearance = GetConservativeDistance(
 239853                center,
 239854                contact.HorizontalSegmentEnd);
 55
 239856            portal = new GridNavigationPortal(
 239857                VoxelContactFaceKind.Vertical,
 239858                sourceToTarget,
 239859                canonicalFacePoint,
 239860                FixedMath.Min(startClearance, endClearance),
 239861                faceHeight,
 239862                contact.HorizontalSegmentStart,
 239863                contact.HorizontalSegmentEnd);
 239864            return true;
 65        }
 66
 2167        Fixed64 sourceHeight = source.VerticalMax - source.VerticalMin;
 2168        Fixed64 targetHeight = target.VerticalMax - target.VerticalMin;
 69
 2170        Span<Vector2d> polygon = stackalloc Vector2d[GridConvexPolygon2d.MaxVertexCount];
 2171        contact.HorizontalPolygon.CopyTo(polygon);
 2172        ReadOnlySpan<Vector2d> footprint = polygon[..contact.HorizontalPolygon.VertexCount];
 2173        bool hasCentroid = FixedConvex2dRelations.TryGetAreaAndCentroid(
 2174            footprint,
 2175            out _,
 2176            out Vector2d centroid);
 77        Debug.Assert(hasCentroid);
 78
 79        Fixed64 maximumRadius;
 2180        if (IsPlanarPointContained(source, centroid) && IsPlanarPointContained(target, centroid))
 81        {
 2082            maximumRadius = GetMinimumPolygonClearance(footprint, centroid);
 83        }
 84        else
 85        {
 186            centroid = footprint[0];
 187            maximumRadius = Fixed64.Zero;
 88        }
 89
 2190        portal = new GridNavigationPortal(
 2191            VoxelContactFaceKind.Horizontal,
 2192            sourceToTarget,
 2193            new Vector3d(centroid.X, contact.VerticalMin, centroid.Y),
 2194            maximumRadius,
 2195            FixedMath.Min(sourceHeight, targetHeight),
 2196            default,
 2197            default);
 2198        return true;
 99    }
 100
 101    private static bool IsNavigationPrismValid(in GridCellPrism prism)
 9364102        => prism.FootprintVertexCount is 4 or 6;
 103
 104    private static bool IsPlanarPointContained(in GridCellPrism prism, Vector2d point) =>
 20762105        prism.Contains(new Vector3d(point.X, prism.Center.Y, point.Y));
 106
 107    private static Fixed64 GetMinimumPolygonClearance(
 108        ReadOnlySpan<Vector2d> polygon,
 109        Vector2d point)
 110    {
 20111        long minimumRaw = long.MaxValue;
 20112        Span<ulong> firstProduct = stackalloc ulong[2];
 20113        Span<ulong> secondProduct = stackalloc ulong[2];
 20114        Span<ulong> cross = stackalloc ulong[3];
 20115        Span<ulong> edgeSquared = stackalloc ulong[3];
 20116        Span<ulong> crossSquared = stackalloc ulong[6];
 20117        Span<ulong> radiusSquared = stackalloc ulong[2];
 20118        Span<ulong> scaledEdgeSquared = stackalloc ulong[6];
 204119        for (int i = 0; i < polygon.Length; i++)
 120        {
 121            // A convex polygon is the intersection of its edge half-planes. Compare
 122            // radius^2 * edgeLength^2 <= cross^2 in raw integer space so neither a
 123            // projected point nor a normalized edge direction can round outward.
 82124            Vector2d start = polygon[i];
 82125            Vector2d end = polygon[(i + 1) % polygon.Length];
 82126            GetSignedDifference(end.X.m_rawValue, start.X.m_rawValue, out bool edgeXNegative, out ulong edgeX);
 82127            GetSignedDifference(end.Y.m_rawValue, start.Y.m_rawValue, out bool edgeYNegative, out ulong edgeY);
 82128            GetSignedDifference(point.X.m_rawValue, start.X.m_rawValue, out bool pointXNegative, out ulong pointX);
 82129            GetSignedDifference(point.Y.m_rawValue, start.Y.m_rawValue, out bool pointYNegative, out ulong pointY);
 130
 82131            Multiply64(edgeX, pointY, firstProduct);
 82132            Multiply64(edgeY, pointX, secondProduct);
 82133            GetSignedDifferenceMagnitude(
 82134                firstProduct,
 82135                edgeXNegative ^ pointYNegative,
 82136                secondProduct,
 82137                !(edgeYNegative ^ pointXNegative),
 82138                cross);
 139
 82140            Multiply64(edgeX, edgeX, firstProduct);
 82141            Multiply64(edgeY, edgeY, secondProduct);
 82142            Add128(firstProduct, secondProduct, edgeSquared);
 143
 82144            MultiplyWords(cross, cross, crossSquared);
 82145            long low = 0L;
 82146            long high = minimumRaw;
 3477147            while (low < high)
 148            {
 3395149                long difference = high - low;
 3395150                long middle = low + (difference >> 1) + (difference & 1L);
 3395151                Multiply64((ulong)middle, (ulong)middle, radiusSquared);
 3395152                MultiplyWords(radiusSquared, edgeSquared, scaledEdgeSquared);
 3395153                if (CompareWords(scaledEdgeSquared, crossSquared) <= 0)
 2053154                    low = middle;
 155                else
 1342156                    high = middle - 1L;
 157            }
 158
 82159            minimumRaw = low;
 160        }
 161
 20162        return Fixed64.FromRaw(minimumRaw);
 163    }
 164
 165    private static void GetSignedDifference(
 166        long end,
 167        long start,
 168        out bool negative,
 169        out ulong magnitude)
 170    {
 2726171        negative = end < start;
 2726172        magnitude = negative
 2726173            ? unchecked((ulong)start - (ulong)end)
 2726174            : unchecked((ulong)end - (ulong)start);
 2726175    }
 176
 177    private static void GetSignedDifferenceMagnitude(
 178        ReadOnlySpan<ulong> first,
 179        bool firstNegative,
 180        ReadOnlySpan<ulong> second,
 181        bool secondNegative,
 182        Span<ulong> magnitude)
 183    {
 82184        magnitude.Clear();
 82185        if (firstNegative == secondNegative)
 186        {
 43187            Add128(first, second, magnitude);
 43188            return;
 189        }
 190
 39191        int comparison = CompareWords(first, second);
 39192        if (comparison >= 0)
 19193            Subtract128(first, second, magnitude);
 194        else
 20195            Subtract128(second, first, magnitude);
 20196    }
 197
 198    private static void Add128(
 199        ReadOnlySpan<ulong> first,
 200        ReadOnlySpan<ulong> second,
 201        Span<ulong> result)
 202    {
 125203        result.Clear();
 125204        result[0] = unchecked(first[0] + second[0]);
 125205        ulong carry = result[0] < first[0] ? 1UL : 0UL;
 125206        result[1] = unchecked(first[1] + second[1] + carry);
 125207    }
 208
 209    private static void Subtract128(
 210        ReadOnlySpan<ulong> minuend,
 211        ReadOnlySpan<ulong> subtrahend,
 212        Span<ulong> result)
 213    {
 39214        result.Clear();
 39215        result[0] = unchecked(minuend[0] - subtrahend[0]);
 39216        ulong borrow = minuend[0] < subtrahend[0] ? 1UL : 0UL;
 39217        result[1] = unchecked(minuend[1] - subtrahend[1] - borrow);
 39218    }
 219
 220    private static void Multiply64(ulong first, ulong second, Span<ulong> result)
 221    {
 24831222        ulong firstLow = (uint)first;
 24831223        ulong firstHigh = first >> 32;
 24831224        ulong secondLow = (uint)second;
 24831225        ulong secondHigh = second >> 32;
 24831226        ulong lowProduct = firstLow * secondLow;
 24831227        ulong firstCross = firstHigh * secondLow;
 24831228        ulong secondCross = firstLow * secondHigh;
 24831229        ulong carry = (lowProduct >> 32) + (uint)firstCross + (uint)secondCross;
 24831230        result[0] = (lowProduct & uint.MaxValue) | (carry << 32);
 24831231        result[1] = (firstHigh * secondHigh)
 24831232            + (firstCross >> 32)
 24831233            + (secondCross >> 32)
 24831234            + (carry >> 32);
 24831235    }
 236
 237    private static void MultiplyWords(
 238        ReadOnlySpan<ulong> first,
 239        ReadOnlySpan<ulong> second,
 240        Span<ulong> result)
 241    {
 3477242        result.Clear();
 3477243        Span<ulong> product = stackalloc ulong[2];
 21026244        for (int firstIndex = 0; firstIndex < first.Length; firstIndex++)
 245        {
 56288246            for (int secondIndex = 0; secondIndex < second.Length; secondIndex++)
 247            {
 21108248                Multiply64(first[firstIndex], second[secondIndex], product);
 21108249                AddWord(result, firstIndex + secondIndex, product[0]);
 21108250                AddWord(result, firstIndex + secondIndex + 1, product[1]);
 251            }
 252        }
 3477253    }
 254
 255    private static void AddWord(Span<ulong> value, int index, ulong addend)
 256    {
 50646257        while (addend != 0UL && index < value.Length)
 258        {
 8430259            ulong current = value[index];
 8430260            value[index] = unchecked(current + addend);
 8430261            addend = value[index] < current ? 1UL : 0UL;
 8430262            index++;
 263        }
 42216264    }
 265
 266    private static int CompareWords(ReadOnlySpan<ulong> first, ReadOnlySpan<ulong> second)
 267    {
 268        Debug.Assert(first.Length == second.Length);
 3434269        int index = first.Length - 1;
 14533270        while (index >= 0)
 271        {
 14462272            ulong firstWord = first[index];
 14462273            ulong secondWord = second[index];
 14462274            if (firstWord != secondWord)
 3363275                return firstWord < secondWord ? -1 : 1;
 11099276            index--;
 277        }
 278
 71279        return 0;
 280    }
 281
 282    private static Fixed64 GetConservativeDistance(
 283        Vector2d start,
 284        Vector2d end)
 285    {
 4796286        bool hasDistance = Vector2d.TryGetDistance(start, end, out Fixed64 distance);
 287        Debug.Assert(hasDistance);
 288
 4796289        Vector2d representedDistance = new Vector2d(distance, Fixed64.Zero);
 4796290        if (Vector2d.CompareDistanceSquared(start, end, Vector2d.Zero, representedDistance) < 0)
 2291            distance = Fixed64.FromRaw(distance.m_rawValue - 1L);
 292
 4796293        return distance;
 294    }
 295
 296    private static Vector2d GetNearestRepresentableSegmentMidpoint(Vector2d start, Vector2d end)
 297    {
 2398298        ulong xDelta = unchecked((ulong)end.X.m_rawValue - (ulong)start.X.m_rawValue);
 2398299        GetSignedDifference(end.Y.m_rawValue, start.Y.m_rawValue, out bool yNegative, out ulong yDelta);
 2398300        ulong divisor = GetGreatestCommonDivisor(xDelta, yDelta);
 2398301        ulong latticeIndex = divisor >> 1;
 2398302        ulong xOffset = (xDelta / divisor) * latticeIndex;
 2398303        ulong yOffset = (yDelta / divisor) * latticeIndex;
 2398304        long x = unchecked((long)((ulong)start.X.m_rawValue + xOffset));
 2398305        long y = unchecked((long)(yNegative
 2398306            ? (ulong)start.Y.m_rawValue - yOffset
 2398307            : (ulong)start.Y.m_rawValue + yOffset));
 2398308        return new Vector2d(Fixed64.FromRaw(x), Fixed64.FromRaw(y));
 309    }
 310
 311    private static ulong GetGreatestCommonDivisor(ulong first, ulong second)
 312    {
 5249313        while (second != 0UL)
 314        {
 2851315            ulong remainder = first % second;
 2851316            first = second;
 2851317            second = remainder;
 318        }
 319
 2398320        return first;
 321    }
 322}

Methods/Properties

TryGetPrism(GridForge.Grids.VoxelGrid,GridForge.Spatial.VoxelIndex,GridForge.Grids.Topology.GridCellPrism&)
TryCreatePrism(GridForge.Grids.Topology.GridTopologyKind,GridForge.Grids.Topology.GridTopologyMetrics,FixedMathSharp.Vector3d,GridForge.Spatial.WorldVoxelIndex,GridForge.Grids.Topology.GridCellPrism&)
GetContact(GridForge.Grids.Topology.GridCellPrism&,GridForge.Grids.Topology.GridCellPrism&)
TryGetPrimaryFace(GridForge.Grids.VoxelGrid,GridForge.Spatial.VoxelIndex,GridForge.Spatial.VoxelIndex,GridForge.Grids.Topology.VoxelContactManifold&)
GetExactBoundaryContactsInto(GridForge.Grids.VoxelGrid,GridForge.Grids.VoxelGrid,SwiftCollections.SwiftList`1<GridForge.Grids.Topology.VoxelContactManifold>,GridForge.Grids.Topology.GridContactQueryScratch)
BuildFootprintIntersection(GridForge.Grids.Topology.GridCellPrism&,GridForge.Grids.Topology.GridCellPrism&,System.Span`1<FixedMathSharp.Vector2d>)
BuildConvexHull(System.Span`1<FixedMathSharp.Vector2d>,System.Span`1<FixedMathSharp.Vector2d>)
TryGetExactHalf(FixedMathSharp.Fixed64,FixedMathSharp.Fixed64&)
TryGetSymmetricBounds(FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64&,FixedMathSharp.Fixed64&)
AddUnique(System.Span`1<FixedMathSharp.Vector2d>,System.Int32&,FixedMathSharp.Vector2d)
GetSegmentExtents(System.ReadOnlySpan`1<FixedMathSharp.Vector2d>,FixedMathSharp.Vector2d&,FixedMathSharp.Vector2d&)
CompareCoordinates(FixedMathSharp.Vector2d,FixedMathSharp.Vector2d)
IsPrimaryOffset(GridForge.Grids.Topology.GridTopologyKind,GridForge.Spatial.VoxelIndex,GridForge.Spatial.VoxelIndex)
CreateSeparated(GridForge.Spatial.WorldVoxelIndex,GridForge.Spatial.WorldVoxelIndex,FixedMathSharp.Vector3d)
SortByVoxelIndex(SwiftCollections.SwiftList`1<GridForge.Grids.Voxel>)
SiftDown(GridForge.Grids.Voxel[],System.Int32,System.Int32)
CollectPotentialSourceVoxels(GridForge.Grids.VoxelGrid,GridForge.Grids.VoxelGrid,GridForge.Grids.Topology.GridContactQueryScratch)
AddSourceCandidates(GridForge.Grids.VoxelGrid,GridForge.Grids.Topology.TopologyVoxelAabb,GridForge.Grids.Topology.GridContactQueryScratch)
.ctor(SwiftCollections.SwiftList`1<GridForge.Grids.Voxel>)
Visit(GridForge.Grids.Voxel)
IsNavigationBodyAnchorValid(GridForge.Grids.Topology.GridCellPrism&,FixedMathSharp.Vector3d,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,GridForge.Grids.Topology.GridNavigationPortal&)
IsPortalCertifiedOnEdge(FixedMathSharp.Geometry.FixedSegment2d,GridForge.Grids.Topology.GridNavigationPortal&)
TryGetNavigationPortalTraversalParameters(GridForge.Grids.Topology.GridCellPrism&,GridForge.Grids.Topology.GridCellPrism&,GridForge.Grids.Topology.GridNavigationPortal&,FixedMathSharp.Vector3d,FixedMathSharp.Vector3d,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64&,FixedMathSharp.Fixed64&)
TryGetCompiledNavigationPortalTraversalParameters(GridForge.Grids.Topology.GridCellPrism&,GridForge.Grids.Topology.GridCellPrism&,GridForge.Grids.Topology.GridNavigationPortal&,FixedMathSharp.Vector3d,FixedMathSharp.Vector3d,FixedMathSharp.Vector3d,FixedMathSharp.Vector3d,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64&,FixedMathSharp.Fixed64&)
IsNavigationBodySegmentValid(GridForge.Grids.Topology.GridCellPrism&,FixedMathSharp.Vector3d,FixedMathSharp.Vector3d,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,GridForge.Grids.Topology.GridNavigationPortal&,GridForge.Grids.Topology.GridNavigationPortal&,GridForge.Grids.Topology.GridNavigationBodySegmentEndpointAllowance)
IsNavigationBodySegmentValidCore(GridForge.Grids.Topology.GridCellPrism&,FixedMathSharp.Vector3d,FixedMathSharp.Vector3d,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,GridForge.Grids.Topology.GridNavigationPortal&,GridForge.Grids.Topology.GridNavigationPortal&,System.Int32)
TryClipNavigationBodySegmentEndpoint(GridForge.Grids.Topology.GridCellPrism&,FixedMathSharp.Vector3d,FixedMathSharp.Vector3d,FixedMathSharp.Fixed64,System.Boolean,FixedMathSharp.Vector3d&,System.Int32&)
TryGetActiveOpening(FixedMathSharp.Geometry.FixedSegment2d,FixedMathSharp.Geometry.FixedSegment2d,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,GridForge.Grids.Topology.GridNavigationPortal&,FixedMathSharp.Vector2d,FixedMathSharp.Vector2d&,FixedMathSharp.Vector2d&)
IsDirectedPortalCrossing(GridForge.Grids.Topology.GridCellPrism&,FixedMathSharp.Geometry.FixedSegment2d,FixedMathSharp.Geometry.FixedSegment2d)
IsPortalTraversalGapPlanarValid(GridForge.Grids.Topology.GridCellPrism&,FixedMathSharp.Geometry.FixedSegment2d,FixedMathSharp.Fixed64,GridForge.Grids.Topology.GridNavigationPortal&)
IsPortalHeightValidOverInterval(FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,GridForge.Grids.Topology.GridNavigationPortal&)
IsBodyHeightValidOverInterval(FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64)
GetConservativeLerpBounds(FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64&,FixedMathSharp.Fixed64&)
TryGetPointParameter(FixedMathSharp.Vector3d,FixedMathSharp.Vector3d,FixedMathSharp.Vector3d,FixedMathSharp.Fixed64&)
AreSamePortal(GridForge.Grids.Topology.GridNavigationPortal&,GridForge.Grids.Topology.GridNavigationPortal&)
CompareDistanceFrom(FixedMathSharp.Vector2d,FixedMathSharp.Vector2d,FixedMathSharp.Vector2d)
Swap(FixedMathSharp.Vector2d&,FixedMathSharp.Vector2d&)
HasPositiveNavigationBodyPrismOverlap(GridForge.Grids.Topology.GridCellPrism&,FixedMathSharp.Vector3d,FixedMathSharp.Vector3d,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64)
TryGetPlanarSegmentInterval(GridForge.Grids.Topology.GridCellPrism&,FixedMathSharp.Vector2d,FixedMathSharp.Vector2d,FixedMathSharp.Fixed64&,FixedMathSharp.Fixed64&)
TryGetPlanarSlab(FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64&,FixedMathSharp.Fixed64&,FixedMathSharp.Fixed64&)
IsFootprintStrictlySeparatedFromLine(System.ReadOnlySpan`1<FixedMathSharp.Vector2d>,FixedMathSharp.Vector2d,FixedMathSharp.Vector2d)
AddNavigationBodyParameter(System.Span`1<FixedMathSharp.Fixed64>,System.Int32&,FixedMathSharp.Fixed64)
GetNavigationBodyParameter(FixedMathSharp.Geometry.FixedSegment2d,FixedMathSharp.Vector2d)
TryValidateNavigationCorridor(System.ReadOnlySpan`1<GridForge.Grids.Topology.GridCellPrism>,FixedMathSharp.Vector3d,FixedMathSharp.Vector3d,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,System.Span`1<FixedMathSharp.Vector3d>,System.Int32&,FixedMathSharp.Fixed64&)
TryCreateNavigationPortal(GridForge.Grids.Topology.GridCellPrism&,GridForge.Grids.Topology.GridCellPrism&,GridForge.Grids.Topology.GridNavigationPortal&)
IsNavigationPrismValid(GridForge.Grids.Topology.GridCellPrism&)
IsPlanarPointContained(GridForge.Grids.Topology.GridCellPrism&,FixedMathSharp.Vector2d)
GetMinimumPolygonClearance(System.ReadOnlySpan`1<FixedMathSharp.Vector2d>,FixedMathSharp.Vector2d)
GetSignedDifference(System.Int64,System.Int64,System.Boolean&,System.UInt64&)
GetSignedDifferenceMagnitude(System.ReadOnlySpan`1<System.UInt64>,System.Boolean,System.ReadOnlySpan`1<System.UInt64>,System.Boolean,System.Span`1<System.UInt64>)
Add128(System.ReadOnlySpan`1<System.UInt64>,System.ReadOnlySpan`1<System.UInt64>,System.Span`1<System.UInt64>)
Subtract128(System.ReadOnlySpan`1<System.UInt64>,System.ReadOnlySpan`1<System.UInt64>,System.Span`1<System.UInt64>)
Multiply64(System.UInt64,System.UInt64,System.Span`1<System.UInt64>)
MultiplyWords(System.ReadOnlySpan`1<System.UInt64>,System.ReadOnlySpan`1<System.UInt64>,System.Span`1<System.UInt64>)
AddWord(System.Span`1<System.UInt64>,System.Int32,System.UInt64)
CompareWords(System.ReadOnlySpan`1<System.UInt64>,System.ReadOnlySpan`1<System.UInt64>)
GetConservativeDistance(FixedMathSharp.Vector2d,FixedMathSharp.Vector2d)
GetNearestRepresentableSegmentMidpoint(FixedMathSharp.Vector2d,FixedMathSharp.Vector2d)
GetGreatestCommonDivisor(System.UInt64,System.UInt64)