< Summary

Information
Class: Trailblazer.Pathing.NavigationSurfaceAStarWork
Assembly: Trailblazer
File(s): /home/runner/work/Trailblazer/Trailblazer/src/Trailblazer/Pathing/Search/AStar/NavigationSurfaceAStarWork.cs
Line coverage
100%
Covered lines: 809
Uncovered lines: 0
Coverable lines: 809
Total lines: 1211
Line coverage: 100%
Branch coverage
100%
Covered branches: 294
Total branches: 294
Branch coverage: 100%
Method coverage

Feature is only available for sponsors

Upgrade to PRO version

Metrics

MethodBranch coverage Crap Score Cyclomatic complexity Line coverage
.ctor(...)100%1616100%
Advance(...)100%116116100%
Dispose()100%22100%
BeginSimplification()100%22100%
FinalizeWithoutSimplification()100%11100%
AdvanceSimplification(...)100%11100%
AdvanceSimplification(...)100%44100%
AdvanceCurrentSimplification(...)100%5050100%
TryMergeRayDependencies()100%22100%
BeginRawCopy(...)100%22100%
PrepareDependencyFinalization()100%11100%
PrepareDependencyFinalization(...)100%22100%
AdvanceDependencyCapture(...)100%11100%
AdvanceDependencyCapture(...)100%1010100%
CompletePayloadBuild(...)100%88100%
IsWorldCurrent()100%22100%
IsSimplificationEpochCurrent()100%11100%
CaptureWorldDependency(...)100%22100%
ReleaseRuntimeState()100%11100%
EnumerateEdges(...)100%11100%
BeginSelectedEdgeReplay(...)100%22100%
IsTarget(...)100%22100%
GetGuideAnchor(...)100%22100%
Finish(...)100%11100%
ApplyTerminalStatus(...)100%22100%
ApplyRoute(...)100%1212100%
AppendTransitionGuidePoints(...)100%66100%
AppendGuidePoint(...)100%1414100%
TryApplyCoincidentGuidePoint(...)100%1212100%
Push(...)100%11100%
Pop()100%22100%
SortUp(...)100%44100%
SortDown(...)100%88100%
CompareHeap(...)100%66100%
SwapHeap(...)100%11100%
GetHeuristic(...)100%22100%
TryGetHeuristicFootAnchor(...)100%22100%

File(s)

/home/runner/work/Trailblazer/Trailblazer/src/Trailblazer/Pathing/Search/AStar/NavigationSurfaceAStarWork.cs

#LineLine coverage
 1//=======================================================================
 2// NavigationSurfaceAStarWork.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 GridForge.Grids;
 11
 12namespace Trailblazer.Pathing;
 13
 14internal enum NavigationSurfaceAStarStatus : byte
 15{
 16    Pending = 0,
 17    Success = 1,
 18    NoPath = 2,
 19    BudgetExceeded = 3,
 20    CostOverflow = 4,
 21    CapacityExceeded = 5,
 22    Stale = 6
 23}
 24
 25/// <summary>Runs one bounded fixed-point surface A* query over a leased graph.</summary>
 26internal sealed class NavigationSurfaceAStarWork : IDisposable
 27{
 28    private enum Stage : byte
 29    {
 30        Search = 0,
 31        Reconstruct = 1,
 32        ReversePath = 2,
 33        ExpandGuide = 3,
 34        Simplify = 4,
 35        CopyRaw = 5,
 36        SortDependencies = 6,
 37        CaptureDependencies = 7,
 38        BuildPayload = 8
 39    }
 40
 41    private NavigationResolvedPathQuery? _query;
 42    private NavigationWorldGraph? _graph;
 43    private readonly GridWorld _world;
 44    private readonly NavigationWorldGraphStore _store;
 45    private readonly NavigationRayWork _rayWork;
 46    private readonly NavigationAStarWorkspace _workspace;
 47    private readonly NavigationWorkMeter _meter;
 48    private readonly long _maximumPayloadBytes;
 49    private readonly Vector3d _targetFootAnchor;
 50    private readonly bool _useEuclideanHeuristic;
 51    private NavigationTraversalEdgeEnumerator _edges;
 52    private NavigationMediumStateRef _current;
 53    private NavigationMediumStateRef _pathCursor;
 54    private NavigationMediumStateRef _goal;
 55    private NavigationDependencySortWork _dependencySort;
 56    private NavigationDependencyStampWork? _dependencyStamp;
 57    private NavigationAStarGuidePoint[]? _payloadGuidePoints;
 58    private NavigationTransitionInstruction[]? _payloadTransitionInstructions;
 59    private NavigationSurfaceAStarStatus _resultStatus;
 60    private Stage _stage;
 61    private int _pathWrite;
 62    private int _reverseLeft;
 63    private int _reverseRight;
 64    private int _pathEdgeOrdinal;
 65    private int _payloadWrite;
 66    private int _transitionPayloadWrite;
 67    private int _simplificationSourcePathOrdinal;
 68    private int _simplificationCandidatePathOrdinal;
 69    private int _simplificationWriteOrdinal;
 70    private int _transitionBarrierScanOrdinal;
 71    private int _nextTransitionPathOrdinal;
 72    private int _rawCopyOrdinal;
 73    private int _rawCopyEndOrdinal;
 74    private int _finalizationLookupReservation;
 75    private ulong _simplificationWorldChangeSequence;
 76    private bool _requiresWorldStamp;
 77    private bool _hasCurrent;
 78    private bool _lastGuidePointIsNode;
 79    private bool _hasCompletedSimplificationProof;
 80
 25081    internal NavigationSurfaceAStarWork(
 25082        GridWorld world,
 25083        NavigationWorldGraphStore store,
 25084        NavigationResolvedPathQuery query,
 25085        NavigationAStarWorkspace workspace,
 25086        NavigationRayWork rayWork,
 25087        long maximumPayloadBytes)
 88    {
 25089        SwiftThrowHelper.ThrowIfNull(world, nameof(world));
 25090        SwiftThrowHelper.ThrowIfNull(store, nameof(store));
 25091        SwiftThrowHelper.ThrowIfNull(query, nameof(query));
 25092        SwiftThrowHelper.ThrowIfNull(workspace, nameof(workspace));
 25093        SwiftThrowHelper.ThrowIfNull(rayWork, nameof(rayWork));
 25094        if (maximumPayloadBytes < 0)
 195            throw new ArgumentOutOfRangeException(nameof(maximumPayloadBytes));
 24996        _world = world;
 24997        _store = store;
 24998        _query = query;
 24999        _graph = query.Graph;
 249100        _workspace = workspace;
 249101        _rayWork = rayWork;
 249102        _workspace.ResetSearch();
 249103        _meter = query.Meter;
 249104        _maximumPayloadBytes = maximumPayloadBytes;
 249105        _simplificationWorldChangeSequence = query.WorldChangeSequence;
 249106        _requiresWorldStamp = query.RequiresWorldStamp
 249107            || query.StartMedium == TraversalMedium.Gas
 249108            || query.StartMedium == TraversalMedium.Liquid;
 249109        _resultStatus = NavigationSurfaceAStarStatus.Success;
 249110        _useEuclideanHeuristic = !query.Query.AllowTransitions
 249111            && _graph.SurfaceComponents.TryGet(
 249112                query.Start.Address,
 249113                query.StartMedium,
 249114                out NavigationSurfaceComponent startComponent)
 249115            && startComponent.AllSurfaceEdgesEuclideanCertified;
 249116        _targetFootAnchor = query.End.FootAnchor;
 249117        if (!query.Query.AllowTransitions
 249118            && !_graph.AreInSameSurfaceComponent(
 249119                query.Start.Address,
 249120                query.StartMedium,
 249121                query.End.Address,
 249122                query.StartMedium))
 123        {
 6124            _resultStatus = NavigationSurfaceAStarStatus.NoPath;
 6125            _workspace.PathNodeCount = 0;
 6126            _dependencySort = new NavigationDependencySortWork(_workspace);
 6127            _stage = Stage.SortDependencies;
 6128            return;
 129        }
 243130        var startStateRef = new NavigationMediumStateRef(
 243131            query.Start.Node,
 243132            query.StartMedium);
 243133        if (!_workspace.NodeTable.TryGetOrAdd(
 243134                startStateRef,
 243135                out int startSlot,
 243136                out _))
 137        {
 1138            Finish(NavigationSurfaceAStarStatus.CapacityExceeded);
 1139            return;
 140        }
 242141        ref NavigationAStarNodeRecord start = ref _workspace.NodeTable.GetRecord(startSlot);
 242142        start.Cost = Fixed64.Zero;
 242143        start.Heuristic = GetHeuristic(startStateRef);
 242144        start.EstimatedTotalCost = start.Heuristic;
 242145        Push(startStateRef, startSlot);
 242146    }
 147
 148    internal NavigationSurfaceAStarStatus Status { get; private set; }
 149
 150    internal NavigationAStarPayload Result { get; private set; } = null!;
 151
 152    internal NavigationSurfaceAStarStatus Advance(
 153        int lookupStepLimit,
 154        int nodeStepLimit,
 155        int edgeStepLimit,
 156        int connectionStepLimit)
 157    {
 1234158        SwiftThrowHelper.ThrowIfNegative(lookupStepLimit, nameof(lookupStepLimit));
 1234159        SwiftThrowHelper.ThrowIfNegative(nodeStepLimit, nameof(nodeStepLimit));
 1234160        SwiftThrowHelper.ThrowIfNegative(edgeStepLimit, nameof(edgeStepLimit));
 1234161        SwiftThrowHelper.ThrowIfNegative(connectionStepLimit, nameof(connectionStepLimit));
 1234162        if (Status != NavigationSurfaceAStarStatus.Pending)
 1163            return Status;
 1233164        if (!IsWorldCurrent())
 2165            return Finish(NavigationSurfaceAStarStatus.Stale);
 1231166        int lookupRemaining = lookupStepLimit;
 1231167        int nodeRemaining = nodeStepLimit;
 1231168        int edgeRemaining = edgeStepLimit;
 1231169        int connectionRemaining = connectionStepLimit;
 170
 23487171        while (Status == NavigationSurfaceAStarStatus.Pending)
 172        {
 23487173            if (_stage == Stage.Search)
 174            {
 15285175                if (!_hasCurrent)
 176                {
 2995177                    if (_workspace.HeapCount == 0)
 178                    {
 11179                        _resultStatus = NavigationSurfaceAStarStatus.NoPath;
 11180                        _workspace.PathNodeCount = 0;
 11181                        _dependencySort = new NavigationDependencySortWork(_workspace);
 11182                        _stage = Stage.SortDependencies;
 11183                        continue;
 184                    }
 2984185                    if (nodeRemaining == 0)
 186                    {
 38187                        return _meter.RemainingExpandedNodes == 0
 38188                            ? Finish(NavigationSurfaceAStarStatus.BudgetExceeded)
 38189                            : Status;
 190                    }
 2946191                    if (!_meter.TryConsumeExpandedNodes(1))
 2192                        return Finish(NavigationSurfaceAStarStatus.BudgetExceeded);
 2944193                    nodeRemaining--;
 2944194                    _current = Pop();
 2944195                    _workspace.NodeTable.TryGetSlot(_current, out int currentSlot);
 2944196                    ref NavigationAStarNodeRecord current =
 2944197                        ref _workspace.NodeTable.GetRecord(currentSlot);
 2944198                    current.Closed = true;
 2944199                    if (IsTarget(_current))
 200                    {
 208201                        _goal = _current;
 208202                        _pathCursor = _current;
 208203                        _stage = Stage.Reconstruct;
 208204                        continue;
 205                    }
 2736206                    _edges = EnumerateEdges(_current, emittedSurfaceOrdinal: -1);
 2736207                    _hasCurrent = true;
 208                }
 209
 15026210                NavigationTraversalEdgeAdvanceStatus edgeStatus = _edges.AdvanceOne(
 15026211                    _meter,
 15026212                    _workspace.EndpointWorkspace.Dependencies,
 15026213                    ref edgeRemaining,
 15026214                    ref connectionRemaining);
 15026215                CaptureWorldDependency(_edges);
 15026216                edgeStatus = NavigationSearchFinalizationRules.ResolveTraversalEpochStatus(
 15026217                    edgeStatus,
 15026218                    IsWorldCurrent());
 15026219                if (edgeStatus == NavigationTraversalEdgeAdvanceStatus.Blocked)
 220                {
 150221                    return ApplyTerminalStatus(
 150222                        NavigationSearchFinalizationRules.ResolveBlockedTraversalStatus(
 150223                            _edges.RequiresConnectionProgress,
 150224                            _meter.RemainingConnectionLegs,
 150225                            _meter.RemainingEvaluatedEdges));
 226                }
 14876227                if (edgeStatus == NavigationTraversalEdgeAdvanceStatus.Pending)
 228                    continue;
 11949229                if (edgeStatus == NavigationTraversalEdgeAdvanceStatus.Complete)
 230                {
 2723231                    _edges = default;
 2723232                    _hasCurrent = false;
 2723233                    continue;
 234                }
 9226235                if (NavigationSearchFinalizationRules.TryResolveTraversalTerminalStatus(
 9226236                        edgeStatus,
 9226237                        out NavigationSurfaceAStarStatus terminalStatus))
 238                {
 9239                    return Finish(terminalStatus);
 240                }
 9217241                if (edgeStatus != NavigationTraversalEdgeAdvanceStatus.Edge)
 242                    continue;
 9217243                NavigationMediumStateRef target = _edges.CurrentTarget;
 9217244                if (_workspace.NodeTable.TryGetSlot(target, out int existingSlot)
 9217245                    && _workspace.NodeTable.GetRecord(existingSlot).Closed)
 246                {
 247                    continue;
 248                }
 4801249                NavigationSurfaceAStarStatus completion = ApplyRoute(
 4801250                    target,
 4801251                    _edges.CurrentCost,
 4801252                    _edges.CurrentOrdinal,
 4801253                    _edges.CurrentKind);
 4801254                if (completion != NavigationSurfaceAStarStatus.Pending)
 3255                    return completion;
 256                continue;
 257            }
 258
 8202259            if (_stage == Stage.Reconstruct)
 260            {
 940261                if (nodeRemaining == 0)
 118262                    return Status;
 822263                nodeRemaining--;
 822264                _workspace.PathNodes[_pathWrite++] = _pathCursor;
 822265                if (_pathCursor.Equals(new NavigationMediumStateRef(
 822266                        _query!.Start.Node,
 822267                        _query.StartMedium)))
 268                {
 208269                    _workspace.PathNodeCount = _pathWrite;
 208270                    _reverseLeft = 0;
 208271                    _reverseRight = _pathWrite - 1;
 208272                    _stage = Stage.ReversePath;
 208273                    continue;
 274                }
 614275                _workspace.NodeTable.TryGetSlot(_pathCursor, out int pathSlot);
 614276                NavigationAStarNodeRecord pathRecord =
 614277                    _workspace.NodeTable.GetRecord(pathSlot);
 614278                if (pathRecord.ParentEdgeKind == NavigationTraversalEdgeKind.Transition)
 48279                    _transitionPayloadWrite = checked(_transitionPayloadWrite + 1);
 614280                _pathCursor = pathRecord.Parent;
 614281                continue;
 282            }
 283
 7262284            if (_stage == Stage.ReversePath)
 285            {
 659286                if (_reverseLeft < _reverseRight)
 287                {
 426288                    if (nodeRemaining == 0)
 56289                        return Status;
 370290                    nodeRemaining--;
 370291                    (_workspace.PathNodes[_reverseLeft], _workspace.PathNodes[_reverseRight]) =
 370292                        (_workspace.PathNodes[_reverseRight], _workspace.PathNodes[_reverseLeft]);
 370293                    _reverseLeft++;
 370294                    _reverseRight--;
 370295                    continue;
 296                }
 233297                if (nodeRemaining == 0)
 25298                    return Status;
 208299                nodeRemaining--;
 208300                _payloadTransitionInstructions = _transitionPayloadWrite == 0
 208301                    ? Array.Empty<NavigationTransitionInstruction>()
 208302                    : new NavigationTransitionInstruction[_transitionPayloadWrite];
 208303                _transitionPayloadWrite = 0;
 208304                NavigationMediumStateRef startNode = _workspace.PathNodes[0];
 208305                _graph!.TryGetNodeAddress(
 208306                    startNode.Node,
 208307                    out NavigationCellAddress startAddress);
 208308                _graph.TryGetNodeState(
 208309                    startNode.Node,
 208310                    startNode.Medium,
 208311                    out NavigationNodeState startState);
 208312                if (!AppendGuidePoint(
 208313                        new NavigationAStarGuidePoint(
 208314                            startAddress,
 208315                            GetGuideAnchor(startState, startNode.Medium),
 208316                            startNode.Medium),
 208317                        pathNodeOrdinal: 0))
 318                {
 1319                    return Finish(NavigationSurfaceAStarStatus.CapacityExceeded);
 320                }
 207321                _pathEdgeOrdinal = 0;
 207322                BeginSelectedEdgeReplay(_pathEdgeOrdinal);
 207323                _stage = Stage.ExpandGuide;
 207324                continue;
 325            }
 326
 6603327            if (_stage == Stage.ExpandGuide)
 328            {
 4042329                if (_pathEdgeOrdinal + 1 >= _workspace.PathNodeCount)
 330                {
 200331                    _edges = default;
 200332                    BeginSimplification();
 200333                    continue;
 334                }
 3842335                NavigationMediumStateRef sourceNode =
 3842336                    _workspace.PathNodes[_pathEdgeOrdinal];
 3842337                NavigationMediumStateRef targetNode =
 3842338                    _workspace.PathNodes[_pathEdgeOrdinal + 1];
 3842339                _workspace.NodeTable.TryGetSlot(targetNode, out int targetSlot);
 3842340                NavigationAStarNodeRecord targetRecord = _workspace.NodeTable
 3842341                    .GetRecord(targetSlot);
 3842342                int parentEdgeOrdinal = targetRecord.ParentEdgeOrdinal;
 3842343                NavigationTraversalEdgeAdvanceStatus edgeStatus = _edges.AdvanceOne(
 3842344                    _meter,
 3842345                    _workspace.EndpointWorkspace.Dependencies,
 3842346                    ref edgeRemaining,
 3842347                    ref connectionRemaining);
 3842348                CaptureWorldDependency(_edges);
 3842349                edgeStatus = NavigationSearchFinalizationRules.ResolveTraversalEpochStatus(
 3842350                    edgeStatus,
 3842351                    IsWorldCurrent());
 3842352                if (NavigationSearchFinalizationRules.ShouldConsumeTraversalSurfacePoint(
 3842353                        _edges.HasCurrentSurfacePoint,
 3842354                        edgeStatus))
 355                {
 2089356                    if (nodeRemaining == 0)
 274357                        return Status;
 1815358                    nodeRemaining--;
 1815359                    bool isTarget = _edges.CurrentSurfacePointIsTargetFootAnchor;
 1815360                    if (!AppendGuidePoint(
 1815361                            _edges.CurrentSurfacePoint,
 1815362                            isTarget ? _pathEdgeOrdinal + 1 : -1))
 363                    {
 1364                        return Finish(NavigationSurfaceAStarStatus.CapacityExceeded);
 365                    }
 1814366                    _edges.ConsumeCurrentSurfacePoint();
 1814367                    continue;
 368                }
 1753369                if (edgeStatus == NavigationTraversalEdgeAdvanceStatus.Blocked)
 370                {
 12371                    return ApplyTerminalStatus(
 12372                        NavigationSearchFinalizationRules.ResolveBlockedTraversalStatus(
 12373                            _edges.RequiresConnectionProgress,
 12374                            _meter.RemainingConnectionLegs,
 12375                            _meter.RemainingEvaluatedEdges));
 376                }
 1741377                if (edgeStatus == NavigationTraversalEdgeAdvanceStatus.Pending)
 378                    continue;
 661379                if (NavigationSearchFinalizationRules.TryResolveTraversalTerminalStatus(
 661380                        edgeStatus,
 661381                        out NavigationSurfaceAStarStatus terminalStatus))
 382                {
 2383                    return Finish(terminalStatus);
 384                }
 385                System.Diagnostics.Debug.Assert(
 386                    edgeStatus == NavigationTraversalEdgeAdvanceStatus.Edge,
 387                    "the immutable selected edge passed the same evaluator during search");
 659388                if (_edges.CurrentOrdinal < parentEdgeOrdinal)
 389                    continue;
 610390                if (_edges.CurrentKind == NavigationTraversalEdgeKind.Transition)
 391                {
 48392                    if (!AppendTransitionGuidePoints(sourceNode, targetNode))
 2393                        return Finish(NavigationSurfaceAStarStatus.CapacityExceeded);
 394                }
 562395                else if (_edges.CurrentKind == NavigationTraversalEdgeKind.Volume)
 396                {
 20397                    _graph!.TryGetNodeAddress(
 20398                        targetNode.Node,
 20399                        out NavigationCellAddress targetAddress);
 20400                    _graph.TryGetNodeState(
 20401                        targetNode.Node,
 20402                        targetNode.Medium,
 20403                        out NavigationNodeState targetState);
 20404                    if (!AppendGuidePoint(
 20405                            new NavigationAStarGuidePoint(
 20406                                targetAddress,
 20407                                GetGuideAnchor(targetState, targetNode.Medium),
 20408                                targetNode.Medium),
 20409                            _pathEdgeOrdinal + 1))
 410                    {
 2411                        return Finish(NavigationSurfaceAStarStatus.CapacityExceeded);
 412                    }
 413                }
 606414                _pathEdgeOrdinal++;
 606415                BeginSelectedEdgeReplay(_pathEdgeOrdinal);
 606416                continue;
 417            }
 418
 2561419            if (_stage == Stage.Simplify)
 93420                return AdvanceSimplification(ref nodeRemaining);
 421
 2468422            if (_stage == Stage.CopyRaw)
 423            {
 203424                if (_rawCopyOrdinal <= _rawCopyEndOrdinal)
 425                {
 142426                    if (nodeRemaining == 0)
 1427                        return Status;
 141428                    nodeRemaining--;
 141429                    _workspace.GuidePoints[_simplificationWriteOrdinal++] =
 141430                        _workspace.GuidePoints[_rawCopyOrdinal++];
 141431                    continue;
 432                }
 61433                if (_rawCopyEndOrdinal == _workspace.GuidePointCount - 1)
 434                {
 59435                    PrepareDependencyFinalization();
 59436                    continue;
 437                }
 2438                _simplificationSourcePathOrdinal++;
 2439                if (_simplificationSourcePathOrdinal >= _nextTransitionPathOrdinal)
 440                {
 1441                    _nextTransitionPathOrdinal = -1;
 1442                    _transitionBarrierScanOrdinal =
 1443                        _simplificationSourcePathOrdinal + 1;
 444                }
 2445                _simplificationCandidatePathOrdinal = _workspace.PathNodeCount - 1;
 2446                _stage = Stage.Simplify;
 2447                continue;
 448            }
 449
 2265450            if (_stage == Stage.SortDependencies)
 451            {
 225452                int lookupBefore = _meter.LookupProbes;
 225453                bool complete = _dependencySort.Advance(_meter, lookupRemaining);
 225454                lookupRemaining -= _meter.LookupProbes - lookupBefore;
 225455                if (!complete)
 456                {
 11457                    return ApplyTerminalStatus(
 11458                        NavigationSearchFinalizationRules.ResolveIncompleteLookupStatus(
 11459                            _meter.RemainingLookupProbes));
 460                }
 214461                _dependencySort = default;
 214462                _stage = Stage.CaptureDependencies;
 214463                continue;
 464            }
 465
 2040466            if (_stage == Stage.CaptureDependencies)
 467            {
 263468                if (!AdvanceDependencyCapture(ref lookupRemaining))
 51469                    return Status;
 212470                long requiredPayloadBytes = NavigationAStarPayload.GetRetainedBytes(
 212471                    _workspace.GuidePointCount,
 212472                    _payloadTransitionInstructions?.Length ?? 0,
 212473                    _dependencyStamp!.Result);
 212474                if (requiredPayloadBytes > _maximumPayloadBytes)
 1475                    return Finish(NavigationSurfaceAStarStatus.CapacityExceeded);
 211476                _payloadGuidePoints = _workspace.GuidePointCount == 0
 211477                    ? Array.Empty<NavigationAStarGuidePoint>()
 211478                    : new NavigationAStarGuidePoint[_workspace.GuidePointCount];
 211479                _payloadTransitionInstructions ??=
 211480                    Array.Empty<NavigationTransitionInstruction>();
 211481                _stage = Stage.BuildPayload;
 211482                continue;
 483            }
 484
 1777485            if (_payloadWrite < _workspace.GuidePointCount)
 486            {
 1566487                if (nodeRemaining == 0)
 168488                    return Status;
 1398489                nodeRemaining--;
 1398490                _payloadGuidePoints![_payloadWrite] =
 1398491                    _workspace.GuidePoints[_payloadWrite];
 1398492                _payloadWrite++;
 1398493                continue;
 494            }
 211495            CompletePayloadBuild(IsWorldCurrent());
 211496            break;
 497        }
 211498        return Status;
 499    }
 500
 501    public void Dispose()
 502    {
 250503        NavigationResolvedPathQuery? query = _query;
 250504        _query = null;
 250505        query?.Dispose();
 250506        ReleaseRuntimeState();
 250507    }
 508
 509    private void BeginSimplification()
 510    {
 200511        _simplificationSourcePathOrdinal = 0;
 200512        _simplificationCandidatePathOrdinal = _workspace.PathNodeCount - 1;
 200513        _simplificationWriteOrdinal = 1;
 200514        _transitionBarrierScanOrdinal = 1;
 200515        _nextTransitionPathOrdinal = -1;
 200516        if (!NavigationSearchFinalizationRules.TryAdmitSimplification(
 200517                _workspace.PathNodeCount,
 200518                _meter.RemainingSimplificationRays,
 200519                _workspace.EndpointComponentCount,
 200520                _workspace.EndpointPageCount,
 200521                _meter,
 200522                out _finalizationLookupReservation))
 523        {
 122524            FinalizeWithoutSimplification();
 525        }
 526        else
 527        {
 78528            _stage = Stage.Simplify;
 529        }
 78530    }
 531
 532    private void FinalizeWithoutSimplification()
 533    {
 122534        _simplificationWriteOrdinal = _workspace.GuidePointCount;
 122535        PrepareDependencyFinalization();
 122536    }
 537
 538    private NavigationSurfaceAStarStatus AdvanceSimplification(
 539        ref int nodeRemaining)
 540    {
 93541        NavigationSurfaceAStarStatus result =
 93542            NavigationSearchFinalizationRules.ResolveAStarEpochStatus(
 93543                NavigationSurfaceAStarStatus.Pending,
 93544                IsWorldCurrent());
 93545        return AdvanceSimplification(result, ref nodeRemaining);
 546    }
 547
 548    internal NavigationSurfaceAStarStatus AdvanceSimplification(
 549        NavigationSurfaceAStarStatus result,
 550        ref int nodeRemaining)
 551    {
 94552        if (result == NavigationSurfaceAStarStatus.Pending)
 93553            result = AdvanceCurrentSimplification(ref nodeRemaining);
 94554        if (Status == NavigationSurfaceAStarStatus.Pending)
 92555            ApplyTerminalStatus(result);
 94556        return Status;
 557    }
 558
 559    private NavigationSurfaceAStarStatus AdvanceCurrentSimplification(
 560        ref int nodeRemaining)
 561    {
 157562        while (_nextTransitionPathOrdinal < 0
 157563            && _transitionBarrierScanOrdinal < _workspace.PathNodeCount)
 564        {
 114565            if (nodeRemaining == 0)
 4566                return Status;
 110567            nodeRemaining--;
 110568            NavigationMediumStateRef scanned =
 110569                _workspace.PathNodes[_transitionBarrierScanOrdinal];
 110570            _workspace.NodeTable.TryGetSlot(scanned, out int scannedSlot);
 110571            if (_workspace.NodeTable.GetRecord(scannedSlot).ParentEdgeKind
 110572                == NavigationTraversalEdgeKind.Transition)
 573            {
 46574                _nextTransitionPathOrdinal = _transitionBarrierScanOrdinal;
 46575                break;
 576            }
 64577            _transitionBarrierScanOrdinal++;
 578        }
 89579        if (_nextTransitionPathOrdinal < 0)
 33580            _nextTransitionPathOrdinal = int.MaxValue;
 89581        if (_simplificationCandidatePathOrdinal >= _nextTransitionPathOrdinal)
 582        {
 46583            _simplificationCandidatePathOrdinal =
 46584                _nextTransitionPathOrdinal - 1;
 585        }
 89586        if (_meter.RemainingSimplificationRays == 0)
 587        {
 4588            BeginRawCopy(copySuffix: true);
 4589            return Status;
 590        }
 85591        if (_simplificationCandidatePathOrdinal <= _simplificationSourcePathOrdinal)
 592        {
 46593            BeginRawCopy(copySuffix: false);
 46594            return Status;
 595        }
 39596        if (_simplificationCandidatePathOrdinal == _simplificationSourcePathOrdinal + 1
 39597            && _workspace.PathNodeGuidePointOrdinals[_simplificationCandidatePathOrdinal]
 39598                == _workspace.PathNodeGuidePointOrdinals[_simplificationSourcePathOrdinal] + 1)
 599        {
 6600            BeginRawCopy(copySuffix: false);
 6601            return Status;
 602        }
 33603        _meter.TryConsumeSimplificationRays(1);
 604
 33605        int sourcePathOrdinal = _simplificationSourcePathOrdinal;
 33606        int candidatePathOrdinal = _simplificationCandidatePathOrdinal;
 33607        NavigationAStarGuidePoint source = _workspace.GuidePoints[
 33608            _workspace.PathNodeGuidePointOrdinals[sourcePathOrdinal]];
 33609        NavigationAStarGuidePoint candidate = _workspace.GuidePoints[
 33610            _workspace.PathNodeGuidePointOrdinals[candidatePathOrdinal]];
 33611        _rayWork.Begin(new NavigationRayRequest(
 33612            _world,
 33613            _store,
 33614            _graph!,
 33615            _query!.Query.Agent,
 33616            _query.AreaPolicy,
 33617            _workspace.PathNodes[sourcePathOrdinal].Medium,
 33618            source.Position,
 33619            candidate.Position,
 33620            NavigationRayEndpointAllowance.None,
 33621            NavigationRayChainConstraint.SeedAt(source.Address)));
 33622        NavigationRayStatus rayStatus = _rayWork.Advance(_meter);
 33623        NavigationSurfaceAStarStatus simplificationStatus =
 33624            rayStatus == NavigationRayStatus.Stale
 33625                ? NavigationSurfaceAStarStatus.Stale
 33626                : NavigationSurfaceAStarStatus.Pending;
 33627        if (simplificationStatus == NavigationSurfaceAStarStatus.Pending
 33628            && rayStatus is not NavigationRayStatus.Success
 33629                and not NavigationRayStatus.Blocked)
 630        {
 4631            BeginRawCopy(copySuffix: true);
 4632            return Status;
 633        }
 634
 29635        bool accepted = false;
 29636        if (simplificationStatus == NavigationSurfaceAStarStatus.Pending
 29637            && rayStatus == NavigationRayStatus.Success)
 638        {
 22639            NavigationRayResult ray = _rayWork.Result;
 22640            NavigationMediumStateRef sourceNode =
 22641                _workspace.PathNodes[sourcePathOrdinal];
 22642            NavigationMediumStateRef candidateNode =
 22643                _workspace.PathNodes[candidatePathOrdinal];
 22644            _workspace.NodeTable.TryGetSlot(sourceNode, out int sourceSlot);
 22645            _workspace.NodeTable.TryGetSlot(candidateNode, out int candidateSlot);
 22646            Fixed64.TrySubtract(
 22647                _workspace.NodeTable.GetRecord(candidateSlot).Cost,
 22648                _workspace.NodeTable.GetRecord(sourceSlot).Cost,
 22649                out Fixed64 rawCost);
 22650            accepted = NavigationSearchFinalizationRules.ShouldAcceptSimplificationRay(
 22651                ray.EndAddress,
 22652                candidate.Address,
 22653                ray.TraversalCost,
 22654                rawCost);
 655        }
 656
 29657        if (simplificationStatus == NavigationSurfaceAStarStatus.Pending)
 658        {
 28659            simplificationStatus = NavigationSearchFinalizationRules
 28660                .ResolveAStarEpochStatus(
 28661                    NavigationSurfaceAStarStatus.Pending,
 28662                    IsSimplificationEpochCurrent());
 663        }
 29664        bool dependenciesMerged = simplificationStatus
 29665                == NavigationSurfaceAStarStatus.Pending
 29666            && TryMergeRayDependencies();
 29667        if (simplificationStatus == NavigationSurfaceAStarStatus.Pending
 29668            && !dependenciesMerged)
 669        {
 1670            simplificationStatus = NavigationSearchFinalizationRules
 1671                .ResolveAStarEpochStatus(
 1672                    NavigationSurfaceAStarStatus.Pending,
 1673                    IsSimplificationEpochCurrent());
 1674            if (simplificationStatus == NavigationSurfaceAStarStatus.Pending)
 675            {
 1676                BeginRawCopy(copySuffix: true);
 1677                return Status;
 678            }
 679        }
 28680        if (simplificationStatus == NavigationSurfaceAStarStatus.Pending)
 681        {
 26682            _hasCompletedSimplificationProof = true;
 26683            simplificationStatus = NavigationSearchFinalizationRules
 26684                .ResolveAStarEpochStatus(
 26685                    NavigationSurfaceAStarStatus.Pending,
 26686                    IsWorldCurrent());
 687        }
 28688        if (simplificationStatus != NavigationSurfaceAStarStatus.Pending)
 2689            return Finish(simplificationStatus);
 690
 26691        if (!accepted)
 692        {
 7693            _simplificationCandidatePathOrdinal--;
 7694            return Status;
 695        }
 19696        _workspace.GuidePoints[_simplificationWriteOrdinal++] = candidate;
 19697        _simplificationSourcePathOrdinal = candidatePathOrdinal;
 19698        _simplificationCandidatePathOrdinal = _workspace.PathNodeCount - 1;
 19699        if (_simplificationSourcePathOrdinal + 1 >= _workspace.PathNodeCount)
 17700            PrepareDependencyFinalization();
 19701        return Status;
 702    }
 703
 704    private bool TryMergeRayDependencies()
 705    {
 27706        NavigationDependencyWorkspace target = _workspace.EndpointWorkspace.Dependencies;
 27707        NavigationDependencyWorkspace source = _workspace.RayWorkspace.Dependencies;
 27708        int priorReservation = _finalizationLookupReservation;
 709        // A terminal ray and both dependency passes are one bounded atomic unit:
 710        // prove and debit the complete append pass before mutating the target.
 27711        if (!NavigationSearchFinalizationRules.TryPrepareDependencyMerge(
 27712                IsSimplificationEpochCurrent(),
 27713                target,
 27714                source,
 27715                _meter,
 27716                priorReservation,
 27717                out int enlargedReservation))
 718        {
 1719            return false;
 720        }
 26721        target.CommitMerge(source);
 26722        _meter.RecordSuccessfulDependencyMerge();
 26723        _finalizationLookupReservation = enlargedReservation;
 26724        return IsSimplificationEpochCurrent();
 725    }
 726
 727    private void BeginRawCopy(bool copySuffix)
 728    {
 61729        _rawCopyOrdinal = _workspace.PathNodeGuidePointOrdinals[
 61730            _simplificationSourcePathOrdinal] + 1;
 61731        _rawCopyEndOrdinal = copySuffix
 61732            ? _workspace.GuidePointCount - 1
 61733            : _workspace.PathNodeGuidePointOrdinals[_simplificationSourcePathOrdinal + 1];
 61734        _stage = Stage.CopyRaw;
 61735    }
 736
 737    private void PrepareDependencyFinalization()
 738    {
 198739        NavigationSurfaceAStarStatus finalizationStatus =
 198740            NavigationSearchFinalizationRules.ResolveAStarEpochStatus(
 198741                NavigationSurfaceAStarStatus.Pending,
 198742                IsWorldCurrent());
 198743        PrepareDependencyFinalization(finalizationStatus);
 198744    }
 745
 746    internal void PrepareDependencyFinalization(
 747        NavigationSurfaceAStarStatus finalizationStatus)
 748    {
 199749        if (finalizationStatus == NavigationSurfaceAStarStatus.Pending)
 750        {
 198751            _workspace.GuidePointCount = _simplificationWriteOrdinal;
 198752            _meter.ReleaseLookupReservationFloor();
 198753            _finalizationLookupReservation = 0;
 198754            _dependencySort = new NavigationDependencySortWork(_workspace);
 198755            _stage = Stage.SortDependencies;
 756        }
 199757        ApplyTerminalStatus(finalizationStatus);
 199758    }
 759
 760    private bool AdvanceDependencyCapture(ref int lookupRemaining) =>
 263761        AdvanceDependencyCapture(
 263762            NavigationSearchFinalizationRules.ResolveAStarEpochStatus(
 263763                NavigationSurfaceAStarStatus.Pending,
 263764                IsWorldCurrent()),
 263765            ref lookupRemaining);
 766
 767    internal bool AdvanceDependencyCapture(
 768        NavigationSurfaceAStarStatus captureStatus,
 769        ref int lookupRemaining)
 770    {
 264771        bool complete = false;
 264772        if (captureStatus == NavigationSurfaceAStarStatus.Pending)
 773        {
 263774            _dependencyStamp ??= new NavigationDependencyStampWork(
 263775                _graph!,
 263776                _query!.AreaPolicy,
 263777                _workspace.EndpointComponents,
 263778                _workspace.EndpointComponentCount,
 263779                _workspace.EndpointPages,
 263780                _workspace.EndpointPageCount,
 263781                _workspace.EndpointWorkspace.Dependencies.HasTransitionDependency);
 263782            int lookupBefore = _meter.LookupProbes;
 263783            complete = _dependencyStamp.Advance(_meter, lookupRemaining);
 263784            lookupRemaining -= _meter.LookupProbes - lookupBefore;
 263785            captureStatus = complete
 263786                ? NavigationSearchFinalizationRules.ResolveAStarEpochStatus(
 263787                    NavigationSurfaceAStarStatus.Pending,
 263788                    IsWorldCurrent())
 263789                : _meter.RemainingLookupProbes == 0
 263790                    ? NavigationSurfaceAStarStatus.BudgetExceeded
 263791                    : NavigationSurfaceAStarStatus.Pending;
 792        }
 264793        if (captureStatus != NavigationSurfaceAStarStatus.Pending)
 794        {
 2795            Finish(captureStatus);
 2796            return false;
 797        }
 262798        return complete;
 799    }
 800
 801    internal void CompletePayloadBuild(bool buildEpochCurrent)
 802    {
 212803        NavigationSurfaceAStarStatus finalStatus =
 212804            NavigationSearchFinalizationRules.ResolveAStarEpochStatus(
 212805                _resultStatus,
 212806                buildEpochCurrent);
 212807        if (buildEpochCurrent)
 808        {
 211809            NavigationResolvedPathQuery resolved = _query!;
 211810            Fixed64 resultCost = Fixed64.Zero;
 211811            if (_resultStatus == NavigationSurfaceAStarStatus.Success)
 812            {
 194813                _workspace.NodeTable.TryGetSlot(_goal, out int endSlot);
 194814                resultCost = _workspace.NodeTable.GetRecord(endSlot).Cost;
 815            }
 211816            Result = new NavigationAStarPayload(
 211817                new NavigationAStarPayloadKey(
 211818                    resolved.Query,
 211819                    resolved.Start.Address,
 211820                    resolved.End.Address,
 211821                    resolved.StartMedium,
 211822                    resolved.TargetMedia),
 211823                _payloadGuidePoints!,
 211824                _payloadTransitionInstructions!,
 211825                resultCost,
 211826                _dependencyStamp!.Result,
 211827                _requiresWorldStamp || _hasCompletedSimplificationProof
 211828                    ? _simplificationWorldChangeSequence
 211829                    : null,
 211830                _resultStatus);
 211831            finalStatus = NavigationSearchFinalizationRules.ResolveAStarEpochStatus(
 211832                _resultStatus,
 211833                IsWorldCurrent());
 834        }
 212835        ApplyTerminalStatus(finalStatus);
 212836    }
 837
 21315838    private bool IsWorldCurrent() => NavigationSearchFinalizationRules.IsEpochCurrent(
 21315839        _requiresWorldStamp || _hasCompletedSimplificationProof,
 21315840        _simplificationWorldChangeSequence,
 21315841        _world.ChangeSequence);
 842
 843    private bool IsSimplificationEpochCurrent() =>
 82844        NavigationSearchFinalizationRules.IsEpochCurrent(
 82845            epochRequired: true,
 82846            expectedEpoch: _simplificationWorldChangeSequence,
 82847            currentEpoch: _world.ChangeSequence);
 848
 849    private void CaptureWorldDependency(
 850        in NavigationTraversalEdgeEnumerator edges)
 851    {
 18868852        if (edges.RequiresWorldStamp)
 460853            _requiresWorldStamp = true;
 18868854    }
 855
 856    private void ReleaseRuntimeState()
 857    {
 495858        _graph = null;
 495859        _edges = default;
 495860        _meter.ReleaseLookupReservationFloor();
 495861        _finalizationLookupReservation = 0;
 495862        _rayWork.Reset();
 495863        _dependencySort = default;
 495864    }
 865
 866    private NavigationTraversalEdgeEnumerator EnumerateEdges(
 867        NavigationMediumStateRef source,
 3349868        int emittedSurfaceOrdinal) => new(
 3349869        _world,
 3349870        _graph!,
 3349871        source,
 3349872        _query!.Query.Agent,
 3349873        _query.AreaPolicy,
 3349874        _workspace.RayWorkspace,
 3349875        _query.Query.AllowTransitions,
 3349876        emittedSurfaceOrdinal);
 877
 878    private void BeginSelectedEdgeReplay(int sourcePathOrdinal)
 879    {
 813880        _edges = default;
 813881        int targetPathOrdinal = sourcePathOrdinal + 1;
 813882        if (targetPathOrdinal >= _workspace.PathNodeCount)
 200883            return;
 613884        NavigationMediumStateRef target = _workspace.PathNodes[targetPathOrdinal];
 613885        _workspace.NodeTable.TryGetSlot(target, out int targetSlot);
 613886        _edges = EnumerateEdges(
 613887            _workspace.PathNodes[sourcePathOrdinal],
 613888            _workspace.NodeTable.GetRecord(targetSlot).ParentEdgeOrdinal);
 613889    }
 890
 891    private bool IsTarget(NavigationMediumStateRef state) =>
 2944892        state.Node.Equals(_query!.End.Node)
 2944893        && (_query.TargetMedia & NavigationCell.ToMedia(state.Medium)) != 0;
 894
 895    private Vector3d GetGuideAnchor(
 896        NavigationNodeState state,
 897        TraversalMedium medium)
 898    {
 274899        if (medium == TraversalMedium.Solid)
 190900            return state.FootAnchor;
 84901        state.TryGetCenteredVolumeFootAnchor(
 84902            _query!.Query.Agent.Shape.Height,
 84903            out Vector3d anchor);
 84904        return anchor;
 905    }
 906
 907    private NavigationSurfaceAStarStatus Finish(NavigationSurfaceAStarStatus status)
 908    {
 245909        Status = status;
 910        System.Diagnostics.Debug.Assert(_query != null);
 245911        _query!.ReleaseLease();
 245912        ReleaseRuntimeState();
 245913        return Status;
 914    }
 915
 916    private NavigationSurfaceAStarStatus ApplyTerminalStatus(
 917        NavigationSurfaceAStarStatus status) =>
 676918        status == NavigationSurfaceAStarStatus.Pending
 676919            ? Status
 676920            : Finish(status);
 921
 922    private NavigationSurfaceAStarStatus ApplyRoute(
 923        NavigationMediumStateRef targetState,
 924        Fixed64 edgeCost,
 925        int edgeOrdinal,
 926        NavigationTraversalEdgeKind edgeKind)
 927    {
 4801928        if (!_workspace.NodeTable.TryGetOrAdd(
 4801929                targetState,
 4801930                out int targetSlot,
 4801931                out bool added))
 932        {
 1933            return Finish(NavigationSurfaceAStarStatus.CapacityExceeded);
 934        }
 4800935        ref NavigationAStarNodeRecord target =
 4800936            ref _workspace.NodeTable.GetRecord(targetSlot);
 4800937        _workspace.NodeTable.TryGetSlot(_current, out int sourceSlot);
 4800938        Fixed64 sourceCost = _workspace.NodeTable.GetRecord(sourceSlot).Cost;
 4800939        if (!Fixed64.TryAdd(sourceCost, edgeCost, out Fixed64 nextCost))
 1940            return Finish(NavigationSurfaceAStarStatus.CostOverflow);
 4799941        if (!added && nextCost >= target.Cost)
 1965942            return Status;
 2834943        target.Cost = nextCost;
 2834944        target.Heuristic = GetHeuristic(targetState);
 2834945        if (!Fixed64.TryAdd(
 2834946                target.Cost,
 2834947                target.Heuristic,
 2834948                out target.EstimatedTotalCost))
 949        {
 1950            return Finish(NavigationSurfaceAStarStatus.CostOverflow);
 951        }
 2833952        target.Parent = _current;
 2833953        target.ParentEdgeOrdinal = edgeOrdinal;
 2833954        target.ParentEdgeKind = edgeKind;
 2833955        target.HasParent = true;
 2833956        if (added)
 2822957            Push(targetState, targetSlot);
 958        else
 11959            SortUp(target.HeapIndex);
 2833960        return Status;
 961    }
 962
 963    private bool AppendTransitionGuidePoints(
 964        NavigationMediumStateRef source,
 965        NavigationMediumStateRef target)
 966    {
 48967        int transitionOrdinal = _transitionPayloadWrite;
 48968        _graph!.TryGetNodeAddress(source.Node, out NavigationCellAddress sourceAddress);
 48969        _graph.TryGetNodeAddress(target.Node, out NavigationCellAddress targetAddress);
 970
 48971        int guideRollback = _workspace.GuidePointCount;
 48972        _payloadTransitionInstructions![transitionOrdinal] =
 48973            new NavigationTransitionInstruction(
 48974                _edges.CurrentTransitionIdentityKind,
 48975                _edges.CurrentTransitionOwnerMapId,
 48976                _edges.CurrentTransitionId,
 48977                _edges.CurrentTransitionType,
 48978                sourceAddress,
 48979                targetAddress,
 48980                source.Medium,
 48981                target.Medium,
 48982                _edges.CurrentTransitionSourceAction,
 48983                _edges.CurrentTransitionDestinationAction,
 48984                _edges.CurrentTransitionHints);
 48985        _transitionPayloadWrite++;
 48986        _graph.TryGetNodeState(
 48987            target.Node,
 48988            target.Medium,
 48989            out NavigationNodeState targetState);
 48990        if (!AppendGuidePoint(
 48991                new NavigationAStarGuidePoint(
 48992                    sourceAddress,
 48993                    _edges.CurrentTransitionSourceAction,
 48994                    source.Medium,
 48995                    transitionOrdinal),
 48996                pathNodeOrdinal: -1)
 48997            || !AppendGuidePoint(
 48998                new NavigationAStarGuidePoint(
 48999                    targetAddress,
 481000                    _edges.CurrentTransitionDestinationAction,
 481001                    target.Medium),
 481002                pathNodeOrdinal: -1)
 481003            || !AppendGuidePoint(
 481004                new NavigationAStarGuidePoint(
 481005                    targetAddress,
 481006                    GetGuideAnchor(targetState, target.Medium),
 481007                    target.Medium),
 481008                _pathEdgeOrdinal + 1))
 1009        {
 21010            _workspace.GuidePointCount = guideRollback;
 21011            _payloadTransitionInstructions[transitionOrdinal] = default;
 21012            _transitionPayloadWrite--;
 21013            return false;
 1014        }
 461015        return true;
 1016    }
 1017
 1018    private bool AppendGuidePoint(
 1019        NavigationAStarGuidePoint point,
 1020        int pathNodeOrdinal)
 1021    {
 21841022        bool isNode = pathNodeOrdinal >= 0;
 21841023        int count = _workspace.GuidePointCount;
 21841024        if (count != 0
 21841025            && !point.HasTransition
 21841026            && !_workspace.GuidePoints[count - 1].HasTransition
 21841027            && _workspace.GuidePoints[count - 1].Medium == point.Medium
 21841028            && _workspace.GuidePoints[count - 1].Position == point.Position)
 1029        {
 7051030            int updatedCount = count;
 7051031            bool updatedLastGuidePointIsNode = _lastGuidePointIsNode;
 7051032            bool applied = TryApplyCoincidentGuidePoint(
 7051033                point,
 7051034                pathNodeOrdinal,
 7051035                _workspace.GuidePoints,
 7051036                _workspace.PathNodeGuidePointOrdinals,
 7051037                ref updatedCount,
 7051038                ref updatedLastGuidePointIsNode);
 7051039            _workspace.GuidePointCount = updatedCount;
 7051040            _lastGuidePointIsNode = updatedLastGuidePointIsNode;
 7051041            return applied;
 1042        }
 14791043        if (count >= _workspace.GuidePoints.Length)
 61044            return false;
 14731045        _workspace.GuidePoints[count] = point;
 14731046        _workspace.GuidePointCount = count + 1;
 14731047        _lastGuidePointIsNode = isNode;
 14731048        if (isNode)
 7401049            _workspace.PathNodeGuidePointOrdinals[pathNodeOrdinal] = count;
 14731050        return true;
 1051    }
 1052
 1053    internal static bool TryApplyCoincidentGuidePoint(
 1054        NavigationAStarGuidePoint point,
 1055        int pathNodeOrdinal,
 1056        NavigationAStarGuidePoint[] guidePoints,
 1057        int[] pathNodeGuidePointOrdinals,
 1058        ref int guidePointCount,
 1059        ref bool lastGuidePointIsNode)
 1060    {
 7101061        bool isNode = pathNodeOrdinal >= 0;
 7101062        int previousOrdinal = guidePointCount - 1;
 7101063        NavigationAStarGuidePoint previous = guidePoints[previousOrdinal];
 7101064        if (lastGuidePointIsNode && !isNode)
 301065            return true;
 6801066        if (!lastGuidePointIsNode || previous.Address == point.Address)
 1067        {
 6781068            guidePoints[previousOrdinal] = point;
 6781069            lastGuidePointIsNode = isNode;
 6781070            if (isNode)
 741071                pathNodeGuidePointOrdinals[pathNodeOrdinal] = previousOrdinal;
 6781072            return true;
 1073        }
 21074        if (guidePointCount >= guidePoints.Length)
 11075            return false;
 1076        System.Diagnostics.Debug.Assert(
 1077            isNode,
 1078            "only a distinct addressed node can append after coincident-point preservation and replacement are rejected"
 11079        guidePoints[guidePointCount] = point;
 11080        pathNodeGuidePointOrdinals[pathNodeOrdinal] = guidePointCount;
 11081        guidePointCount++;
 11082        lastGuidePointIsNode = isNode;
 11083        return true;
 1084    }
 1085
 1086    private void Push(NavigationMediumStateRef node, int slot)
 1087    {
 30641088        int index = _workspace.HeapCount++;
 30641089        _workspace.HeapNodes[index] = node;
 30641090        _workspace.NodeTable.GetRecord(slot).HeapIndex = index;
 30641091        SortUp(index);
 30641092    }
 1093
 1094    private NavigationMediumStateRef Pop()
 1095    {
 29441096        NavigationMediumStateRef result = _workspace.HeapNodes[0];
 29441097        int last = --_workspace.HeapCount;
 29441098        if (last > 0)
 1099        {
 21731100            _workspace.HeapNodes[0] = _workspace.HeapNodes[last];
 21731101            _workspace.NodeTable.TryGetSlot(_workspace.HeapNodes[0], out int slot);
 21731102            _workspace.NodeTable.GetRecord(slot).HeapIndex = 0;
 21731103            SortDown(0);
 1104        }
 29441105        return result;
 1106    }
 1107
 1108    private void SortUp(int index)
 1109    {
 31761110        while (index > 0)
 1111        {
 22791112            int parent = (index - 1) / 2;
 22791113            if (CompareHeap(index, parent) >= 0)
 1114                break;
 1011115            SwapHeap(index, parent);
 1011116            index = parent;
 1117        }
 30751118    }
 1119
 1120    private void SortDown(int index)
 1121    {
 70241122        while (true)
 1123        {
 91971124            int left = checked((index * 2) + 1);
 91971125            if (left >= _workspace.HeapCount)
 21461126                return;
 70511127            int right = left + 1;
 70511128            int best = right < _workspace.HeapCount && CompareHeap(right, left) < 0
 70511129                ? right
 70511130                : left;
 70511131            if (CompareHeap(best, index) >= 0)
 271132                return;
 70241133            SwapHeap(index, best);
 70241134            index = best;
 1135        }
 1136    }
 1137
 1138    private int CompareHeap(int leftIndex, int rightIndex)
 1139    {
 161621140        NavigationMediumStateRef left = _workspace.HeapNodes[leftIndex];
 161621141        NavigationMediumStateRef right = _workspace.HeapNodes[rightIndex];
 161621142        _workspace.NodeTable.TryGetSlot(left, out int leftSlot);
 161621143        _workspace.NodeTable.TryGetSlot(right, out int rightSlot);
 161621144        ref NavigationAStarNodeRecord leftRecord =
 161621145            ref _workspace.NodeTable.GetRecord(leftSlot);
 161621146        ref NavigationAStarNodeRecord rightRecord =
 161621147            ref _workspace.NodeTable.GetRecord(rightSlot);
 161621148        int comparison = leftRecord.EstimatedTotalCost.CompareTo(
 161621149            rightRecord.EstimatedTotalCost);
 161621150        if (comparison != 0)
 73151151            return comparison;
 88471152        comparison = leftRecord.Heuristic.CompareTo(rightRecord.Heuristic);
 88471153        if (comparison != 0)
 571154            return comparison;
 87901155        _graph!.TryGetNodeAddress(left.Node, out NavigationCellAddress leftAddress);
 87901156        _graph.TryGetNodeAddress(right.Node, out NavigationCellAddress rightAddress);
 87901157        comparison = leftAddress.CompareTo(rightAddress);
 87901158        return comparison != 0
 87901159            ? comparison
 87901160            : ((int)left.Medium).CompareTo((int)right.Medium);
 1161    }
 1162
 1163    private void SwapHeap(int left, int right)
 1164    {
 71251165        (_workspace.HeapNodes[left], _workspace.HeapNodes[right]) =
 71251166            (_workspace.HeapNodes[right], _workspace.HeapNodes[left]);
 71251167        _workspace.NodeTable.TryGetSlot(_workspace.HeapNodes[left], out int leftSlot);
 71251168        _workspace.NodeTable.TryGetSlot(_workspace.HeapNodes[right], out int rightSlot);
 71251169        _workspace.NodeTable.GetRecord(leftSlot).HeapIndex = left;
 71251170        _workspace.NodeTable.GetRecord(rightSlot).HeapIndex = right;
 71251171    }
 1172
 1173    private Fixed64 GetHeuristic(NavigationMediumStateRef node)
 1174    {
 30761175        Fixed64 heuristic = Fixed64.Zero;
 30761176        if (_useEuclideanHeuristic)
 1177        {
 8701178            bool hasState = _graph!.TryGetNodeState(
 8701179                node.Node,
 8701180                node.Medium,
 8701181                out NavigationNodeState state);
 1182            System.Diagnostics.Debug.Assert(hasState);
 8701183            bool hasFootAnchor = TryGetHeuristicFootAnchor(
 8701184                state,
 8701185                node.Medium,
 8701186                out Vector3d footAnchor);
 8701187            NavigationSearchFinalizationRules.TryGetEuclideanHeuristic(
 8701188                hasFootAnchor,
 8701189                footAnchor,
 8701190                _targetFootAnchor,
 8701191                out heuristic);
 1192        }
 30761193        return heuristic;
 1194    }
 1195
 1196    private bool TryGetHeuristicFootAnchor(
 1197        NavigationNodeState state,
 1198        TraversalMedium medium,
 1199        out Vector3d footAnchor)
 1200    {
 8701201        if (medium == TraversalMedium.Solid)
 1202        {
 7841203            footAnchor = state.FootAnchor;
 7841204            return true;
 1205        }
 1206
 861207        return state.TryGetCenteredVolumeFootAnchor(
 861208            _query!.Query.Agent.Shape.Height,
 861209            out footAnchor);
 1210    }
 1211}
Generated by: ReportGenerator 5.5.11.0
09/02/2026 - 04:28:06
GitHub | reportgenerator.io

Methods/Properties

.ctor(GridForge.Grids.GridWorld,Trailblazer.Pathing.NavigationWorldGraphStore,Trailblazer.Pathing.NavigationResolvedPathQuery,Trailblazer.Pathing.NavigationAStarWorkspace,Trailblazer.Pathing.NavigationRayWork,System.Int64)
Advance(System.Int32,System.Int32,System.Int32,System.Int32)
Dispose()
BeginSimplification()
FinalizeWithoutSimplification()
AdvanceSimplification(System.Int32&)
AdvanceSimplification(Trailblazer.Pathing.NavigationSurfaceAStarStatus,System.Int32&)
AdvanceCurrentSimplification(System.Int32&)
TryMergeRayDependencies()
BeginRawCopy(System.Boolean)
PrepareDependencyFinalization()
PrepareDependencyFinalization(Trailblazer.Pathing.NavigationSurfaceAStarStatus)
AdvanceDependencyCapture(System.Int32&)
AdvanceDependencyCapture(Trailblazer.Pathing.NavigationSurfaceAStarStatus,System.Int32&)
CompletePayloadBuild(System.Boolean)
IsWorldCurrent()
IsSimplificationEpochCurrent()
CaptureWorldDependency(Trailblazer.Pathing.NavigationTraversalEdgeEnumerator&)
ReleaseRuntimeState()
EnumerateEdges(Trailblazer.Pathing.NavigationMediumStateRef,System.Int32)
BeginSelectedEdgeReplay(System.Int32)
IsTarget(Trailblazer.Pathing.NavigationMediumStateRef)
GetGuideAnchor(Trailblazer.Pathing.NavigationNodeState,Trailblazer.TraversalMedium)
Finish(Trailblazer.Pathing.NavigationSurfaceAStarStatus)
ApplyTerminalStatus(Trailblazer.Pathing.NavigationSurfaceAStarStatus)
ApplyRoute(Trailblazer.Pathing.NavigationMediumStateRef,FixedMathSharp.Fixed64,System.Int32,Trailblazer.Pathing.NavigationTraversalEdgeKind)
AppendTransitionGuidePoints(Trailblazer.Pathing.NavigationMediumStateRef,Trailblazer.Pathing.NavigationMediumStateRef)
AppendGuidePoint(Trailblazer.Pathing.NavigationAStarGuidePoint,System.Int32)
TryApplyCoincidentGuidePoint(Trailblazer.Pathing.NavigationAStarGuidePoint,System.Int32,Trailblazer.Pathing.NavigationAStarGuidePoint[],System.Int32[],System.Int32&,System.Boolean&)
Push(Trailblazer.Pathing.NavigationMediumStateRef,System.Int32)
Pop()
SortUp(System.Int32)
SortDown(System.Int32)
CompareHeap(System.Int32,System.Int32)
SwapHeap(System.Int32,System.Int32)
GetHeuristic(Trailblazer.Pathing.NavigationMediumStateRef)
TryGetHeuristicFootAnchor(Trailblazer.Pathing.NavigationNodeState,Trailblazer.TraversalMedium,FixedMathSharp.Vector3d&)