< Summary

Information
Class: GridForge.Grids.Topology.GridCellGeometry
Assembly: GridForge
File(s): File 1: /home/runner/work/GridForge/GridForge/src/GridForge/Grids/Topology/GridCellGeometry.cs
File 2: /home/runner/work/GridForge/GridForge/src/GridForge/Grids/Topology/GridCellGeometry.NavigationBodyAnchor.cs
File 3: /home/runner/work/GridForge/GridForge/src/GridForge/Grids/Topology/GridCellGeometry.NavigationBodySegment.cs
File 4: /home/runner/work/GridForge/GridForge/src/GridForge/Grids/Topology/GridCellGeometry.NavigationCorridor.cs
File 5: /home/runner/work/GridForge/GridForge/src/GridForge/Grids/Topology/GridCellGeometry.NavigationPortal.cs
Line coverage
100%
Covered lines: 1029
Uncovered lines: 0
Coverable lines: 1029
Total lines: 1903
Line coverage: 100%
Branch coverage
100%
Covered branches: 414
Total branches: 414
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%2222100%
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%2020100%
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        SwiftThrowHelper.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    {
 766762        if (!GridTopologyMetrics.IsValid(topologyKind, topologyMetrics))
 63        {
 264            prism = default;
 265            return false;
 66        }
 67
 766568        GridTopologyMetrics metrics = GridTopologyMetrics.Normalize(topologyKind, topologyMetrics);
 766569        if (!TryGetExactHalf(metrics.LayerHeight, out Fixed64 halfHeight))
 70        {
 671            prism = default;
 672            return false;
 73        }
 74
 765975        if (!TryGetSymmetricBounds(center.Y, halfHeight, out Fixed64 verticalMin, out Fixed64 verticalMax))
 76        {
 277            prism = default;
 278            return false;
 79        }
 80
 765781        Span<Vector2d> footprint = stackalloc Vector2d[6];
 82        int vertexCount;
 83        Fixed64 planarInradius;
 84
 765785        if (topologyKind == GridTopologyKind.RectangularPrism)
 86        {
 695587            if (!TryGetExactHalf(metrics.CellWidth, out Fixed64 halfWidth)
 695588                || !TryGetExactHalf(metrics.CellLength, out Fixed64 halfLength))
 89            {
 890                prism = default;
 891                return false;
 92            }
 93
 694794            bool hasExactBounds = TryGetSymmetricBounds(
 694795                center.X,
 694796                halfWidth,
 694797                out Fixed64 minX,
 694798                out Fixed64 maxX);
 694799            hasExactBounds &= TryGetSymmetricBounds(
 6947100                center.Z,
 6947101                halfLength,
 6947102                out Fixed64 minZ,
 6947103                out Fixed64 maxZ);
 6947104            if (!hasExactBounds)
 105            {
 8106                prism = default;
 8107                return false;
 108            }
 6939109            footprint[0] = new Vector2d(minX, minZ);
 6939110            footprint[1] = new Vector2d(maxX, minZ);
 6939111            footprint[2] = new Vector2d(maxX, maxZ);
 6939112            footprint[3] = new Vector2d(minX, maxZ);
 6939113            vertexCount = 4;
 6939114            planarInradius = FixedMath.Min(halfWidth, halfLength);
 115        }
 116        else
 117        {
 702118            Fixed64 radius = metrics.CellRadius;
 702119            if (!Fixed64.TryMultiplyAdd(
 702120                    radius,
 702121                    HexCoordinateUtility.Sqrt3,
 702122                    Fixed64.Zero,
 702123                    out Fixed64 fullWidth))
 124            {
 1125                prism = default;
 1126                return false;
 127            }
 128
 701129            if (!TryGetExactHalf(radius, out Fixed64 halfRadius)
 701130                || !TryGetExactHalf(fullWidth, out Fixed64 apothem))
 131            {
 2132                prism = default;
 2133                return false;
 134            }
 699135            if (metrics.HexOrientation == HexOrientation.FlatTop)
 136            {
 280137                bool hasExactBounds = TryGetSymmetricBounds(
 280138                    center.X,
 280139                    radius,
 280140                    out Fixed64 minRadiusX,
 280141                    out Fixed64 maxRadiusX);
 280142                hasExactBounds &= TryGetSymmetricBounds(
 280143                    center.X,
 280144                    halfRadius,
 280145                    out Fixed64 minHalfX,
 280146                    out Fixed64 maxHalfX);
 280147                hasExactBounds &= TryGetSymmetricBounds(
 280148                    center.Z,
 280149                    apothem,
 280150                    out Fixed64 minApothemZ,
 280151                    out Fixed64 maxApothemZ);
 280152                if (!hasExactBounds)
 153                {
 1154                    prism = default;
 1155                    return false;
 156                }
 157
 279158                footprint[0] = new Vector2d(minRadiusX, center.Z);
 279159                footprint[1] = new Vector2d(minHalfX, minApothemZ);
 279160                footprint[2] = new Vector2d(maxHalfX, minApothemZ);
 279161                footprint[3] = new Vector2d(maxRadiusX, center.Z);
 279162                footprint[4] = new Vector2d(maxHalfX, maxApothemZ);
 279163                footprint[5] = new Vector2d(minHalfX, maxApothemZ);
 164            }
 165            else
 166            {
 419167                bool hasExactBounds = TryGetSymmetricBounds(
 419168                    center.Z,
 419169                    radius,
 419170                    out Fixed64 minRadiusZ,
 419171                    out Fixed64 maxRadiusZ);
 419172                hasExactBounds &= TryGetSymmetricBounds(
 419173                    center.Z,
 419174                    halfRadius,
 419175                    out Fixed64 minHalfZ,
 419176                    out Fixed64 maxHalfZ);
 419177                hasExactBounds &= TryGetSymmetricBounds(
 419178                    center.X,
 419179                    apothem,
 419180                    out Fixed64 minApothemX,
 419181                    out Fixed64 maxApothemX);
 419182                if (!hasExactBounds)
 183                {
 3184                    prism = default;
 3185                    return false;
 186                }
 187
 416188                footprint[0] = new Vector2d(center.X, minRadiusZ);
 416189                footprint[1] = new Vector2d(maxApothemX, minHalfZ);
 416190                footprint[2] = new Vector2d(maxApothemX, maxHalfZ);
 416191                footprint[3] = new Vector2d(center.X, maxRadiusZ);
 416192                footprint[4] = new Vector2d(minApothemX, maxHalfZ);
 416193                footprint[5] = new Vector2d(minApothemX, minHalfZ);
 194            }
 195
 695196            vertexCount = 6;
 695197            planarInradius = apothem;
 198        }
 199
 7634200        ReadOnlySpan<Vector2d> resolvedFootprint = footprint[..vertexCount];
 7634201        prism = new GridCellPrism(
 7634202            cell,
 7634203            topologyKind,
 7634204            center,
 7634205            verticalMin,
 7634206            verticalMax,
 7634207            planarInradius,
 7634208            resolvedFootprint);
 7634209        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    {
 2645219        Vector3d sourceToTarget = target.Center - source.Center;
 2645220        Fixed64 verticalMin = FixedMath.Max(source.VerticalMin, target.VerticalMin);
 2645221        Fixed64 verticalMax = FixedMath.Min(source.VerticalMax, target.VerticalMax);
 2645222        if (verticalMin > verticalMax)
 1223            return CreateSeparated(source.Cell, target.Cell, sourceToTarget);
 224
 2644225        Span<Vector2d> intersection = stackalloc Vector2d[GridConvexPolygon2d.MaxVertexCount];
 2644226        int intersectionCount = BuildFootprintIntersection(source, target, intersection);
 2644227        if (intersectionCount == 0)
 12228            return CreateSeparated(source.Cell, target.Cell, sourceToTarget);
 229
 2632230        bool hasVerticalSpan = verticalMax > verticalMin;
 2632231        if (intersectionCount >= 3
 2632232            && FixedConvex2dRelations.IsStrictlyConvex(intersection[..intersectionCount])
 2632233            && FixedConvex2dRelations.TryGetAreaAndCentroid(
 2632234                intersection[..intersectionCount],
 2632235                out Fixed64 overlapArea,
 2632236                out _))
 237        {
 238            // A strictly convex hull proves positive exact area even when the
 239            // narrowed Fixed64 area underflows to zero.
 52240            bool areaRepresentable = overlapArea > Fixed64.Zero
 52241                && overlapArea != Fixed64.MaxValue;
 52242            GridConvexPolygon2d polygon = new GridConvexPolygon2d(intersection[..intersectionCount]);
 52243            return new VoxelContactManifold(
 52244                source.Cell,
 52245                target.Cell,
 52246                sourceToTarget,
 52247                hasVerticalSpan ? VoxelContactKind.VolumeOverlap : VoxelContactKind.Face,
 52248                hasVerticalSpan ? VoxelContactFaceKind.None : VoxelContactFaceKind.Horizontal,
 52249                verticalMin,
 52250                verticalMax,
 52251                default,
 52252                default,
 52253                polygon,
 52254                overlapArea,
 52255                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        SwiftThrowHelper.ThrowIfNull(sourceGrid, nameof(sourceGrid));
 14344        SwiftThrowHelper.ThrowIfNull(targetGrid, nameof(targetGrid));
 14345        SwiftThrowHelper.ThrowIfNull(results, nameof(results));
 14346        SwiftThrowHelper.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    {
 2644416        Span<Vector2d> sourceVertices = stackalloc Vector2d[6];
 2644417        Span<Vector2d> targetVertices = stackalloc Vector2d[6];
 2644418        source.CopyFootprintTo(sourceVertices);
 2644419        target.CopyFootprintTo(targetVertices);
 2644420        ReadOnlySpan<Vector2d> sourceFootprint = sourceVertices[..source.FootprintVertexCount];
 2644421        ReadOnlySpan<Vector2d> targetFootprint = targetVertices[..target.FootprintVertexCount];
 2644422        Span<Vector2d> sourceOffsets = stackalloc Vector2d[6];
 2644423        Span<Vector2d> targetOffsets = stackalloc Vector2d[6];
 2644424        Vector2d sourceOrigin = new Vector2d(source.Center.X, source.Center.Z);
 2644425        Vector2d targetOrigin = new Vector2d(target.Center.X, target.Center.Z);
 26716426        for (int i = 0; i < sourceFootprint.Length; i++)
 10714427            sourceOffsets[i] = sourceFootprint[i] - sourceOrigin;
 26732428        for (int i = 0; i < targetFootprint.Length; i++)
 10722429            targetOffsets[i] = targetFootprint[i] - targetOrigin;
 2644430        ReadOnlySpan<Vector2d> resolvedSourceOffsets = sourceOffsets[..sourceFootprint.Length];
 2644431        ReadOnlySpan<Vector2d> resolvedTargetOffsets = targetOffsets[..targetFootprint.Length];
 2644432        Span<Vector2d> candidates = stackalloc Vector2d[MaximumIntersectionCandidateCount];
 2644433        int candidateCount = 0;
 434
 26716435        for (int i = 0; i < sourceFootprint.Length; i++)
 436        {
 10714437            Vector2d vertex = sourceFootprint[i];
 10714438            if (FixedConvex2dRelations.ContainsPoint(vertex, targetOrigin, resolvedTargetOffsets))
 5231439                AddUnique(candidates, ref candidateCount, vertex);
 440        }
 441
 26732442        for (int i = 0; i < targetFootprint.Length; i++)
 443        {
 10722444            Vector2d vertex = targetFootprint[i];
 10722445            if (FixedConvex2dRelations.ContainsPoint(vertex, sourceOrigin, resolvedSourceOffsets))
 5255446                AddUnique(candidates, ref candidateCount, vertex);
 447        }
 448
 26716449        for (int sourceEdge = 0; sourceEdge < sourceFootprint.Length; sourceEdge++)
 450        {
 10714451            FixedSegment2d sourceSegment = new FixedSegment2d(
 10714452                sourceFootprint[sourceEdge],
 10714453                sourceFootprint[(sourceEdge + 1) % sourceFootprint.Length]);
 108828454            for (int targetEdge = 0; targetEdge < targetFootprint.Length; targetEdge++)
 455            {
 43700456                FixedSegment2d targetSegment = new FixedSegment2d(
 43700457                    targetFootprint[targetEdge],
 43700458                    targetFootprint[(targetEdge + 1) % targetFootprint.Length]);
 43700459                if (sourceSegment.TryGetUniqueIntersection(targetSegment, out Fixed64 parameter))
 460                {
 15560461                    AddUnique(
 15560462                        candidates,
 15560463                        ref candidateCount,
 15560464                        Vector2d.Lerp(sourceSegment.Start, sourceSegment.End, parameter));
 465                }
 466            }
 467        }
 468
 2644469        if (candidateCount == 0)
 12470            return 0;
 2632471        if (candidateCount == 1)
 472        {
 68473            intersection[0] = candidates[0];
 68474            return 1;
 475        }
 476
 2564477        return BuildConvexHull(candidates[..candidateCount], intersection);
 478    }
 479
 480    private static int BuildConvexHull(Span<Vector2d> candidates, Span<Vector2d> destination)
 481    {
 10510482        for (int i = 1; i < candidates.Length; i++)
 483        {
 2691484            Vector2d candidate = candidates[i];
 2691485            int insertion = i - 1;
 2939486            while (insertion >= 0 && CompareCoordinates(candidates[insertion], candidate) > 0)
 487            {
 248488                candidates[insertion + 1] = candidates[insertion];
 248489                insertion--;
 490            }
 491
 2691492            candidates[insertion + 1] = candidate;
 493        }
 494
 2564495        Span<Vector2d> hull = stackalloc Vector2d[MaximumIntersectionCandidateCount * 2];
 2564496        int count = 0;
 15638497        for (int i = 0; i < candidates.Length; i++)
 498        {
 5320499            while (count >= 2
 5320500                && Vector2d.OrientationSign(hull[count - 2], hull[count - 1], candidates[i]) <= 0)
 65501                count--;
 502
 5255503            hull[count++] = candidates[i];
 504        }
 505
 2564506        int lowerCount = count;
 10510507        for (int i = candidates.Length - 2; i >= 0; i--)
 508        {
 2760509            while (count > lowerCount
 2760510                && Vector2d.OrientationSign(hull[count - 2], hull[count - 1], candidates[i]) <= 0)
 511            {
 69512                count--;
 513            }
 514
 2691515            hull[count++] = candidates[i];
 516        }
 517
 2564518        count--;
 2564519        hull[..count].CopyTo(destination);
 2564520        return count;
 521    }
 522
 523    [MethodImpl(MethodImplOptions.AggressiveInlining)]
 524    private static bool TryGetExactHalf(Fixed64 value, out Fixed64 half)
 525    {
 22968526        if ((value.m_rawValue & 1L) != 0L)
 527        {
 16528            half = default;
 16529            return false;
 530        }
 531
 22952532        half = Fixed64.FromRaw(value.m_rawValue >> 1);
 22952533        return true;
 534    }
 535
 536    [MethodImpl(MethodImplOptions.AggressiveInlining)]
 537    private static bool TryGetSymmetricBounds(
 538        Fixed64 center,
 539        Fixed64 extent,
 540        out Fixed64 minimum,
 541        out Fixed64 maximum)
 542    {
 23650543        maximum = default;
 23650544        return Fixed64.TrySubtract(center, extent, out minimum)
 23650545            && Fixed64.TryAdd(center, extent, out maximum);
 546    }
 547
 548    [MethodImpl(MethodImplOptions.AggressiveInlining)]
 549    private static void AddUnique(Span<Vector2d> candidates, ref int count, Vector2d candidate)
 550    {
 79504551        for (int i = 0; i < count; i++)
 552        {
 34429553            if (candidates[i] == candidate)
 20723554                return;
 555        }
 556
 5323557        candidates[count++] = candidate;
 5323558    }
 559
 560    private static void GetSegmentExtents(
 561        ReadOnlySpan<Vector2d> points,
 562        out Vector2d start,
 563        out Vector2d end)
 564    {
 2580565        start = points[0];
 2580566        end = points[^1];
 2580567    }
 568
 569    [MethodImpl(MethodImplOptions.AggressiveInlining)]
 570    private static int CompareCoordinates(Vector2d first, Vector2d second)
 571    {
 2849572        int xComparison = first.X.CompareTo(second.X);
 2849573        return xComparison != 0 ? xComparison : first.Y.CompareTo(second.Y);
 574    }
 575
 576    [MethodImpl(MethodImplOptions.AggressiveInlining)]
 577    private static bool IsPrimaryOffset(
 578        GridTopologyKind topologyKind,
 579        VoxelIndex source,
 580        VoxelIndex target)
 581    {
 22582        int x = target.x - source.x;
 22583        int y = target.y - source.y;
 22584        int z = target.z - source.z;
 22585        if (topologyKind == GridTopologyKind.RectangularPrism)
 586        {
 4587            RectangularDirection direction = RectangularDirectionUtility.GetDirectionFromOffset((x, y, z));
 4588            return RectangularDirectionUtility.IsPerpendicularNeighbor(direction);
 589        }
 590
 18591        VoxelIndex offset = new(x, y, z);
 18592        ReadOnlySpan<HexDirection> directions = HexDirectionUtility.Primary;
 180593        for (int i = 0; i < directions.Length; i++)
 594        {
 88595            if (HexDirectionUtility.GetOffset(directions[i]) == offset)
 16596                return true;
 597        }
 598
 2599        return false;
 600    }
 601
 602    [MethodImpl(MethodImplOptions.AggressiveInlining)]
 603    private static VoxelContactManifold CreateSeparated(
 604        WorldVoxelIndex source,
 605        WorldVoxelIndex target,
 606        Vector3d sourceToTarget) =>
 13607        new VoxelContactManifold(
 13608            source,
 13609            target,
 13610            sourceToTarget,
 13611            VoxelContactKind.Separated,
 13612            VoxelContactFaceKind.None,
 13613            default,
 13614            default,
 13615            default,
 13616            default,
 13617            default,
 13618            Fixed64.Zero,
 13619            true);
 620
 621    private static void SortByVoxelIndex(SwiftList<Voxel> voxels)
 622    {
 267623        Voxel[] items = voxels.InnerArray;
 267624        int count = voxels.Count;
 625
 626        // Array.Sort allocates a comparer-backed sorting helper on this hot path.
 627        // Heap sort keeps ordering deterministic without allocating or degrading
 628        // to quadratic behavior for large grid boundaries.
 1042629        for (int root = (count >> 1) - 1; root >= 0; root--)
 254630            SiftDown(items, root, count);
 631
 1538632        for (int end = count - 1; end > 0; end--)
 633        {
 502634            (items[0], items[end]) = (items[end], items[0]);
 502635            SiftDown(items, 0, end);
 636        }
 267637    }
 638
 639    private static void SiftDown(Voxel[] items, int root, int count)
 640    {
 2846641        while (true)
 642        {
 3602643            int child = (root << 1) + 1;
 3602644            if (child >= count)
 740645                return;
 646
 2862647            int right = child + 1;
 2862648            if (right < count && items[child].Index.CompareTo(items[right].Index) < 0)
 1472649                child = right;
 650
 2862651            if (items[root].Index.CompareTo(items[child].Index) >= 0)
 16652                return;
 653
 2846654            (items[root], items[child]) = (items[child], items[root]);
 2846655            root = child;
 656        }
 657    }
 658
 659    private static void CollectPotentialSourceVoxels(
 660        VoxelGrid sourceGrid,
 661        VoxelGrid targetGrid,
 662        GridContactQueryScratch scratch)
 663    {
 10664        if (targetGrid.Configuration.StorageKind == GridStorageKind.Sparse)
 665        {
 3666            AllocatedVoxelCollector collector = new AllocatedVoxelCollector(scratch.CandidateVoxels);
 3667            targetGrid.VisitVoxels(ref collector);
 16668            for (int i = 0; i < scratch.CandidateVoxels.Count; i++)
 669            {
 5670                Voxel targetVoxel = scratch.CandidateVoxels[i];
 5671                if (!TryGetPrism(targetGrid, targetVoxel.Index, out GridCellPrism targetPrism))
 672                    continue;
 673
 4674                AddSourceCandidates(
 4675                    sourceGrid,
 4676                    targetPrism.GetAabb().Expand(sourceGrid.Topology.MaxCellEdge),
 4677                    scratch);
 678            }
 679        }
 680        else
 681        {
 7682            Fixed64 expansion = targetGrid.Topology.MaxCellEdge + sourceGrid.Topology.MaxCellEdge;
 7683            AddSourceCandidates(
 7684                sourceGrid,
 7685                new TopologyVoxelAabb(targetGrid.BoundsMin, targetGrid.BoundsMax).Expand(expansion),
 7686                scratch);
 687        }
 688
 10689        scratch.CandidateVoxels.Clear();
 10690        scratch.ProcessedVoxels.Clear();
 10691        SortByVoxelIndex(scratch.SourceVoxels);
 10692    }
 693
 694    private static void AddSourceCandidates(
 695        VoxelGrid sourceGrid,
 696        TopologyVoxelAabb targetBounds,
 697        GridContactQueryScratch scratch)
 698    {
 11699        if (!TopologyVoxelRangeUtility.TryGetCandidateRange(
 11700            sourceGrid,
 11701            targetBounds,
 11702            out VoxelIndex minIndex,
 11703            out VoxelIndex maxIndex))
 704        {
 1705            return;
 706        }
 707
 10708        sourceGrid.AddVoxelsInIndexRange(
 10709            minIndex,
 10710            maxIndex,
 10711            scratch.SourceVoxels,
 10712            scratch.ProcessedVoxels);
 10713    }
 714
 715    private readonly struct AllocatedVoxelCollector : IVoxelStorageVisitor
 716    {
 717        private readonly SwiftList<Voxel> _results;
 718
 719        public AllocatedVoxelCollector(SwiftList<Voxel> results)
 720        {
 3721            _results = results;
 3722        }
 723
 724        public bool Visit(Voxel voxel)
 725        {
 5726            _results.Add(voxel);
 5727            return true;
 728        }
 729    }
 730}

/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    {
 108030        return IsNavigationBodySegmentValid(
 108031            prism,
 108032            foot,
 108033            foot,
 108034            horizontalRadius,
 108035            bodyHeight,
 108036            selectedPortal,
 108037            default,
 108038            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        SwiftThrowHelper.ThrowIfArgument(
 28736            horizontalRadius < Fixed64.Zero,
 28737            nameof(horizontalRadius),
 28738            "Horizontal radius must be nonnegative.");
 28739        SwiftThrowHelper.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    {
 4446179        SwiftThrowHelper.ThrowIfArgument(
 4446180            horizontalRadius < Fixed64.Zero,
 4446181            nameof(horizontalRadius),
 4446182            "Horizontal radius must be nonnegative.");
 4446183        SwiftThrowHelper.ThrowIfArgument(
 4446184            bodyHeight <= Fixed64.Zero,
 4446185            nameof(bodyHeight),
 4446186            "Body height must be positive.");
 187
 4446188        if (endpointAllowance != GridNavigationBodySegmentEndpointAllowance.None
 4446189            && endpointAllowance != GridNavigationBodySegmentEndpointAllowance.StartFootprintEdge
 4446190            && endpointAllowance != GridNavigationBodySegmentEndpointAllowance.EndFootprintEdge)
 191        {
 1192            throw new System.ArgumentOutOfRangeException(nameof(endpointAllowance));
 193        }
 194
 4445195        if (!IsNavigationPrismValid(prism))
 1196            return false;
 197
 4444198        int allowedEdgeIndex = -1;
 4444199        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        }
 4437214        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
 4437230        return IsNavigationBodySegmentValidCore(
 4437231            prism,
 4437232            footStart,
 4437233            footEnd,
 4437234            horizontalRadius,
 4437235            bodyHeight,
 4437236            incomingPortal,
 4437237            outgoingPortal,
 4437238            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    {
 4437251        if (!prism.Contains(footStart)
 4437252            || !prism.Contains(footEnd)
 4437253            || !Fixed64.TryAdd(footStart.Y, bodyHeight, out Fixed64 startTop)
 4437254            || !Fixed64.TryAdd(footEnd.Y, bodyHeight, out Fixed64 endTop)
 4437255            || startTop > prism.VerticalMax
 4437256            || endTop > prism.VerticalMax)
 257        {
 9258            return false;
 259        }
 260
 4428261        FixedSegment2d path = new(
 4428262            new Vector2d(footStart.X, footStart.Z),
 4428263            new Vector2d(footEnd.X, footEnd.Z));
 44272264        for (int edgeIndex = 0; edgeIndex < prism.FootprintVertexCount; edgeIndex++)
 265        {
 17739266            if (edgeIndex == allowedEdgeIndex)
 267                continue;
 17736268            Vector2d edgeStart = prism.GetFootprintVertex(edgeIndex);
 17736269            Vector2d edgeEnd = prism.GetFootprintVertex(
 17736270                (edgeIndex + 1) % prism.FootprintVertexCount);
 17736271            FixedSegment2d edge = new(edgeStart, edgeEnd);
 17736272            if (path.IsDistanceAtLeast(edge, horizontalRadius))
 273                continue;
 274
 5353275            bool hasFirst = TryGetActiveOpening(
 5353276                edge,
 5353277                path,
 5353278                footStart.Y,
 5353279                footEnd.Y,
 5353280                horizontalRadius,
 5353281                bodyHeight,
 5353282                incomingPortal,
 5353283                edgeStart,
 5353284                out Vector2d firstStart,
 5353285                out Vector2d firstEnd);
 5353286            bool hasSecond = TryGetActiveOpening(
 5353287                edge,
 5353288                path,
 5353289                footStart.Y,
 5353290                footEnd.Y,
 5353291                horizontalRadius,
 5353292                bodyHeight,
 5353293                outgoingPortal,
 5353294                edgeStart,
 5353295                out Vector2d secondStart,
 5353296                out Vector2d secondEnd);
 5353297            if (!hasFirst && !hasSecond)
 21298                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
 4397341        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    {
 10706426        openingStart = default;
 10706427        openingEnd = default;
 10706428        if (!portal.IsValid
 10706429            || portal.FaceKind != VoxelContactFaceKind.Vertical
 10706430            || horizontalRadius > portal.MaximumHorizontalRadius
 10706431            || bodyHeight > portal.MaximumBodyHeight
 10706432            || !IsPortalCertifiedOnEdge(edge, portal))
 433        {
 5359434            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    {
 3231638        Fixed64 prismHalfThickness = prism.VerticalMax - prism.Center.Y;
 639
 3231640        Span<Vector2d> offsets = stackalloc Vector2d[6];
 3231641        Vector2d planarOrigin = new(prism.Center.X, prism.Center.Z);
 32890642        for (int i = 0; i < prism.FootprintVertexCount; i++)
 13214643            offsets[i] = prism.GetFootprintVertex(i) - planarOrigin;
 644
 3231645        return FixedConvexPrismRelations.IntersectsSweptUprightCylinderStrict(
 3231646            footStart,
 3231647            footEnd,
 3231648            horizontalRadius,
 3231649            bodyHeight,
 3231650            prism.Center,
 3231651            Fixed64.Zero,
 3231652            offsets[..prism.FootprintVertexCount],
 3231653            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    {
 471663        Vector2d origin = new(prism.Center.X, prism.Center.Z);
 471664        Span<Vector2d> vertices = stackalloc Vector2d[6];
 471665        Span<Vector2d> offsets = stackalloc Vector2d[6];
 471666        prism.CopyFootprintTo(vertices);
 5414667        for (int i = 0; i < prism.FootprintVertexCount; i++)
 2236668            offsets[i] = vertices[i] - origin;
 471669        ReadOnlySpan<Vector2d> footprint = offsets[..prism.FootprintVertexCount];
 670
 471671        bool startContained = FixedConvex2dRelations.ContainsPoint(start, origin, footprint);
 471672        if (start == end)
 673        {
 60674            overlapEnter = Fixed64.Zero;
 60675            overlapExit = Fixed64.One;
 60676            return startContained;
 677        }
 678
 411679        Span<Fixed64> parameters = stackalloc Fixed64[16];
 411680        int count = 0;
 411681        if (startContained)
 89682            parameters[count++] = Fixed64.Zero;
 411683        if (FixedConvex2dRelations.ContainsPoint(end, origin, footprint))
 92684            parameters[count++] = Fixed64.One;
 685
 411686        FixedSegment2d path = new(start, end);
 4790687        for (int i = 0; i < prism.FootprintVertexCount; i++)
 688        {
 1984689            FixedSegment2d edge = new(
 1984690                vertices[i],
 1984691                vertices[(i + 1) % prism.FootprintVertexCount]);
 1984692            if (path.TryGetUniqueIntersection(edge, out Fixed64 parameter))
 449693                AddNavigationBodyParameter(parameters, ref count, parameter);
 1984694            if (Vector2d.OrientationSign(start, end, edge.Start) == 0
 1984695                && Vector2d.OrientationSign(start, end, edge.End) == 0)
 696            {
 45697                AddNavigationBodyParameter(parameters, ref count, GetNavigationBodyParameter(path, edge.Start));
 45698                AddNavigationBodyParameter(parameters, ref count, GetNavigationBodyParameter(path, edge.End));
 699            }
 700        }
 701
 411702        if (count == 0)
 703        {
 156704            overlapEnter = default;
 156705            overlapExit = default;
 156706            return false;
 707        }
 708
 255709        overlapEnter = parameters[0];
 255710        overlapExit = parameters[0];
 936711        for (int i = 1; i < count; i++)
 712        {
 213713            overlapEnter = FixedMath.Min(overlapEnter, parameters[i]);
 213714            overlapExit = FixedMath.Max(overlapExit, parameters[i]);
 715        }
 255716        return true;
 717    }
 718
 719    private static void AddNavigationBodyParameter(
 720        Span<Fixed64> parameters,
 721        ref int count,
 722        Fixed64 parameter)
 723    {
 539724        if ((ulong)parameter.m_rawValue > (ulong)Fixed64.One.m_rawValue)
 6725            return;
 1530726        for (int i = 0; i < count; i++)
 727        {
 478728            if (parameters[i] == parameter)
 246729                return;
 730        }
 287731        parameters[count++] = parameter;
 287732    }
 733
 734    private static Fixed64 GetNavigationBodyParameter(FixedSegment2d path, Vector2d point)
 735    {
 90736        Vector2d delta = path.Delta;
 90737        return FixedMath.Abs(delta.X) >= FixedMath.Abs(delta.Y)
 90738            ? (point.X - path.Start.X) / delta.X
 90739            : (point.Y - path.Start.Y) / delta.Y;
 740    }
 741
 742}

/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)
 9310102        => prism.FootprintVertexCount is 4 or 6;
 103
 104    private static bool IsPlanarPointContained(in GridCellPrism prism, Vector2d point) =>
 42105        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}
Generated by: ReportGenerator 5.5.11.0
08/28/2026 - 03:09:44
GitHub | reportgenerator.io

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&)
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)