< Summary

Information
Class: Trailblazer.Pathing.NavigationSelectedEdgeProgressWork
Assembly: Trailblazer
File(s): /home/runner/work/Trailblazer/Trailblazer/src/Trailblazer/Pathing/Search/Flow/NavigationSelectedEdgeProgressWork.cs
Line coverage
100%
Covered lines: 988
Uncovered lines: 0
Coverable lines: 988
Total lines: 1519
Line coverage: 100%
Branch coverage
100%
Covered branches: 305
Total branches: 305
Branch coverage: 100%
Method coverage

Feature is only available for sponsors

Upgrade to PRO version

Metrics

MethodBranch coverage Crap Score Cyclomatic complexity Line coverage
MapNodeLookupStatus(...)100%66100%
MapStructuralEdgeStatus(...)100%55100%
TryGetRejoinTarget(...)100%66100%
TrySample(...)100%6868100%
TryResolveSelectedEdge(...)100%66100%
TrySampleVolume(...)100%2222100%
IsWithinArrivalRadius(...)100%11100%
TrySampleTransitionApproach(...)100%66100%
RunRay(...)100%44100%
TryGetNode(...)100%1010100%
TryAdvanceRecovery(...)100%44100%
TryRejoin(...)100%1818100%
TryResolvePortal(...)100%88100%
TrySampleDirectedLeg(...)100%66100%
TrySampleDirectedLegUnchecked(...)100%88100%
FailDirectedLeg(...)100%11100%
TrySampleExplicit(...)100%5858100%
TryReadExplicitPortal(...)100%22100%
TrySetHeading(...)100%44100%
TrySetHeadingUnchecked(...)100%44100%
HasReachedOrPassed(...)100%88100%
TryRebase(...)100%2424100%
TryResolveRebaseCursorStatus(...)100%66100%
ConsiderCandidate(...)100%11100%
ConsiderCandidateAddress(...)100%1414100%
SelectNearestCandidate(...)100%88100%

File(s)

/home/runner/work/Trailblazer/Trailblazer/src/Trailblazer/Pathing/Search/Flow/NavigationSelectedEdgeProgressWork.cs

#LineLine coverage
 1//=======================================================================
 2// NavigationSelectedEdgeProgressWork.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;
 11using GridForge.Grids.Topology;
 12
 13namespace Trailblazer.Pathing;
 14
 15internal readonly struct NavigationFlowRejoinTarget
 16{
 17    internal NavigationFlowRejoinTarget(
 18        Vector3d position,
 19        NavigationRayChainConstraint constraint)
 20    {
 21        Position = position;
 22        Constraint = constraint;
 23    }
 24
 25    internal Vector3d Position { get; }
 26
 27    internal NavigationRayChainConstraint Constraint { get; }
 28}
 29
 30/// <summary>Samples only the canonical selected edge stored by one flow node.</summary>
 31internal enum NavigationFlowNodeLookupStatus : byte
 32{
 33    Success,
 34    NotFound,
 35    BudgetExceeded
 36}
 37
 38internal static class NavigationSelectedEdgeProgressWork
 39{
 40    internal static NavigationGuideStatus MapNodeLookupStatus(
 41        NavigationFlowNodeLookupStatus status,
 1942        bool required) => status switch
 1943    {
 244        NavigationFlowNodeLookupStatus.Success => NavigationGuideStatus.Success,
 1945        NavigationFlowNodeLookupStatus.BudgetExceeded =>
 1446            NavigationGuideStatus.BudgetExceeded,
 347        _ => required ? NavigationGuideStatus.Stale : NavigationGuideStatus.Success
 1948    };
 49
 50    internal static NavigationGuideStatus MapStructuralEdgeStatus(
 252851        NavigationSurfaceEdgeAdvanceStatus status) => status switch
 252852    {
 124953        NavigationSurfaceEdgeAdvanceStatus.Pending => NavigationGuideStatus.Success,
 127554        NavigationSurfaceEdgeAdvanceStatus.Edge => NavigationGuideStatus.Success,
 355        NavigationSurfaceEdgeAdvanceStatus.Blocked => NavigationGuideStatus.BudgetExceeded,
 156        _ => NavigationGuideStatus.Stale
 252857    };
 58
 59    internal static bool TryGetRejoinTarget(
 60        NavigationCellAddress sourceAddress,
 61        NavigationNodeState sourceState,
 62        NavigationNodeState targetState,
 63        NavigationSelectedEdgeRef selected,
 64        Vector3d selectedExitTarget,
 65        int targetOrdinal,
 66        out NavigationFlowRejoinTarget target)
 67    {
 31568        SwiftThrowHelper.ThrowIfNegative(targetOrdinal, nameof(targetOrdinal));
 31569        target = default;
 31570        if (targetOrdinal == 0)
 71        {
 29272            target = new NavigationFlowRejoinTarget(
 29273                sourceState.FootAnchor,
 29274                NavigationRayChainConstraint.SourceOnly(sourceAddress));
 29275            return true;
 76        }
 77
 2378        NavigationRayChainConstraint constraint =
 2379            NavigationRayChainConstraint.SelectedEdge(
 2380                sourceAddress,
 2381                selected.Target,
 2382                selected.CanonicalOutgoingOrdinal);
 2383        if (targetOrdinal == 1)
 84        {
 1185            target = new NavigationFlowRejoinTarget(selectedExitTarget, constraint);
 1186            return true;
 87        }
 1288        if (targetOrdinal == 2)
 89        {
 690            target = new NavigationFlowRejoinTarget(targetState.FootAnchor, constraint);
 691            return true;
 92        }
 693        return false;
 94    }
 95
 96    internal static NavigationGuideStatus TrySample(
 97        bool dependencyCurrent,
 98        GridWorld world,
 99        NavigationWorldGraphStore store,
 100        NavigationWorldGraph graph,
 101        NavigationFlowFieldPayload payload,
 102        NavigationCellAddress sourceAddress,
 103        TraversalMedium medium,
 104        Vector3d actualFootPosition,
 105        ref GuideSampleWorkMeter meter,
 106        GridCoveredAddressCursor coveredAddressCursor,
 107        GridCoveredAddressGeneration[] coveredAddressGenerations,
 108        GridCoveredAddress[] coveredAddressOutput,
 109        NavigationImmediateRayWorkspace immediateRayWorkspace,
 110        out NavigationFlowFieldNode currentNode,
 111        out NavigationCellAddress nextSourceAddress,
 112        out Vector3d target,
 113        out Vector3d heading)
 114    {
 1357115        currentNode = default;
 1357116        nextSourceAddress = sourceAddress;
 1357117        target = default;
 1357118        heading = Vector3d.Zero;
 1357119        if (!dependencyCurrent)
 1120            return NavigationGuideStatus.Stale;
 1356121        if (medium is TraversalMedium.Gas or TraversalMedium.Liquid)
 122        {
 102123            return TrySampleVolume(
 102124                world,
 102125                store,
 102126                graph,
 102127                payload,
 102128                sourceAddress,
 102129                medium,
 102130                actualFootPosition,
 102131                ref meter,
 102132                immediateRayWorkspace,
 102133                out currentNode,
 102134                out nextSourceAddress,
 102135                out target,
 102136                out heading);
 137        }
 1254138        NavigationCellAddress currentSource = sourceAddress;
 1254139        bool allowRecovery = true;
 4140        while (true)
 141        {
 1813142            NavigationFlowNodeLookupStatus lookup = TryGetNode(
 1813143                payload,
 1813144                currentSource,
 1813145                medium,
 1813146                ref meter,
 1813147                out NavigationFlowFieldNode node);
 1813148            if (lookup != NavigationFlowNodeLookupStatus.Success)
 149            {
 11150                return MapNodeLookupStatus(lookup, required: true);
 151            }
 1802152            currentNode = node;
 1802153            if (node.TransitionInstructionOrdinal >= 0)
 154            {
 15155                nextSourceAddress = currentSource;
 15156                return NavigationGuideStatus.Success;
 157            }
 1787158            graph.TryGetNodeRef(currentSource, out NavigationNodeRef sourceRef);
 1787159            graph.TryGetNodeState(
 1787160                sourceRef,
 1787161                medium,
 1787162                out NavigationNodeState sourceState);
 1787163            graph.TryGetSeamPrism(currentSource, out GridCellPrism sourcePrism);
 1787164            if (!meter.TryConsumePrismChecks(1))
 2165                return NavigationGuideStatus.BudgetExceeded;
 1785166            bool sourceContains = sourcePrism.Contains(actualFootPosition);
 1785167            target = sourceState.FootAnchor;
 1785168            if (currentSource == payload.Key.DestinationAddress)
 169            {
 561170                if (sourceContains
 561171                    && GridCellGeometry.IsNavigationBodyAnchorValid(
 561172                        sourcePrism,
 561173                        actualFootPosition,
 561174                        payload.Key.Agent.Shape.Radius,
 561175                        payload.Key.Agent.Shape.Height,
 561176                        default))
 177                {
 545178                    nextSourceAddress = currentSource;
 545179                    if (IsWithinArrivalRadius(
 545180                            actualFootPosition,
 545181                            sourceState.FootAnchor,
 545182                            payload.Key.Agent.ArrivalRadius))
 183                    {
 542184                        heading = Vector3d.Zero;
 542185                        return NavigationGuideStatus.Success;
 186                    }
 3187                    return TrySetHeading(
 3188                        actualFootPosition,
 3189                        sourceState.FootAnchor,
 3190                        ref meter,
 3191                        out heading);
 192                }
 16193                NavigationGuideStatus destinationRecovery = TryAdvanceRecovery(
 16194                    world,
 16195                    store,
 16196                    graph,
 16197                    payload,
 16198                    medium,
 16199                    actualFootPosition,
 16200                    ref meter,
 16201                    coveredAddressCursor,
 16202                    coveredAddressGenerations,
 16203                    coveredAddressOutput,
 16204                    immediateRayWorkspace,
 16205                    currentSource,
 16206                    sourceState,
 16207                    default,
 16208                    Vector3d.Zero,
 16209                    default,
 16210                    hasSelectedEdge: false,
 16211                    allowRecovery,
 16212                    out currentSource,
 16213                    out Vector3d destinationRejoinHeading,
 16214                    out bool destinationRejoined);
 16215                if (destinationRecovery != NavigationGuideStatus.Success)
 8216                    return destinationRecovery;
 8217                if (destinationRejoined)
 218                {
 1219                    nextSourceAddress = sourceAddress;
 1220                    heading = destinationRejoinHeading;
 1221                    return NavigationGuideStatus.Success;
 222                }
 7223                allowRecovery = false;
 7224                continue;
 225            }
 226
 1224227            NavigationGuideStatus edgeStatus = TryResolveSelectedEdge(
 1224228                graph,
 1224229                sourceRef,
 1224230                node.SelectedEdge,
 1224231                ref meter,
 1224232                out NavigationGraphEdge edge);
 1224233            if (edgeStatus != NavigationGuideStatus.Success)
 2234                return edgeStatus;
 1222235            graph.TryGetNodeState(
 1222236                edge.Target,
 1222237                medium,
 1222238                out NavigationNodeState targetState);
 1222239            graph.TryGetSeamPrism(
 1222240                node.SelectedEdge.Target,
 1222241                out GridCellPrism targetPrism);
 1222242            if (!meter.TryConsumePrismChecks(1))
 2243                return NavigationGuideStatus.BudgetExceeded;
 1220244            bool targetContains = targetPrism.Contains(actualFootPosition);
 1220245            target = targetState.FootAnchor;
 246
 1220247            if (edge.Kind == NavigationGraphEdgeKind.Explicit)
 248            {
 30249                NavigationGuideStatus explicitStatus = TrySampleExplicit(
 30250                    graph,
 30251                    edge.ExplicitConnection,
 30252                    currentSource,
 30253                    sourceState,
 30254                    sourcePrism,
 30255                    targetState,
 30256                    targetPrism,
 30257                    sourceContains,
 30258                    targetContains,
 30259                    actualFootPosition,
 30260                    payload.Key.Agent.Shape,
 30261                    node.SelectedEdge.Target == payload.Key.DestinationAddress,
 30262                    payload.Key.Agent.ArrivalRadius,
 30263                    ref meter,
 30264                    out NavigationCellAddress explicitSource,
 30265                    out heading);
 30266                if (explicitStatus == NavigationGuideStatus.LocalRecoveryRequired)
 267                {
 6268                    NavigationGuideStatus recovery = TryAdvanceRecovery(
 6269                        world,
 6270                        store,
 6271                        graph,
 6272                        payload,
 6273                        medium,
 6274                        actualFootPosition,
 6275                        ref meter,
 6276                        coveredAddressCursor,
 6277                        coveredAddressGenerations,
 6278                        coveredAddressOutput,
 6279                        immediateRayWorkspace,
 6280                        currentSource,
 6281                        sourceState,
 6282                        node.SelectedEdge,
 6283                        edge.ExplicitConnection.Definition.ExitAnchor,
 6284                        targetState,
 6285                        hasSelectedEdge: true,
 6286                        allowRecovery,
 6287                        out currentSource,
 6288                        out Vector3d rejoinHeading,
 6289                        out bool rejoined);
 6290                    if (recovery != NavigationGuideStatus.Success)
 2291                        return recovery;
 4292                    if (rejoined)
 293                    {
 2294                        nextSourceAddress = sourceAddress;
 2295                        heading = rejoinHeading;
 2296                        return NavigationGuideStatus.Success;
 297                    }
 2298                    allowRecovery = false;
 2299                    continue;
 300                }
 24301                if (explicitStatus != NavigationGuideStatus.Success
 24302                    || explicitSource == currentSource)
 303                {
 22304                    nextSourceAddress = currentSource;
 22305                    return explicitStatus;
 306                }
 2307                currentSource = explicitSource;
 2308                continue;
 309            }
 310
 1190311            NavigationGuideStatus portalStatus = TryResolvePortal(
 1190312                edge,
 1190313                sourceState,
 1190314                payload.Key.Agent.Shape,
 1190315                ref meter,
 1190316                out GridNavigationPortal selectedPortal,
 1190317                out Vector3d sourcePortal,
 1190318                out Vector3d targetPortal);
 1190319            if (portalStatus != NavigationGuideStatus.Success)
 11320                return portalStatus;
 1179321            bool sourceBodyValid = sourceContains
 1179322                && GridCellGeometry.IsNavigationBodyAnchorValid(
 1179323                    sourcePrism,
 1179324                    actualFootPosition,
 1179325                    payload.Key.Agent.Shape.Radius,
 1179326                    payload.Key.Agent.Shape.Height,
 1179327                    selectedPortal);
 1179328            bool targetBodyValid = targetContains
 1179329                && GridCellGeometry.IsNavigationBodyAnchorValid(
 1179330                    targetPrism,
 1179331                    actualFootPosition,
 1179332                    payload.Key.Agent.Shape.Radius,
 1179333                    payload.Key.Agent.Shape.Height,
 1179334                    selectedPortal);
 1179335            if (!sourceBodyValid && !targetBodyValid)
 336            {
 840337                NavigationGuideStatus recovery = TryAdvanceRecovery(
 840338                    world,
 840339                    store,
 840340                    graph,
 840341                    payload,
 840342                    medium,
 840343                    actualFootPosition,
 840344                    ref meter,
 840345                    coveredAddressCursor,
 840346                    coveredAddressGenerations,
 840347                    coveredAddressOutput,
 840348                    immediateRayWorkspace,
 840349                    currentSource,
 840350                    sourceState,
 840351                    node.SelectedEdge,
 840352                    targetPortal,
 840353                    targetState,
 840354                    hasSelectedEdge: true,
 840355                    allowRecovery,
 840356                    out currentSource,
 840357                    out Vector3d rejoinHeading,
 840358                    out bool rejoined);
 840359                if (recovery != NavigationGuideStatus.Success)
 14360                    return recovery;
 826361                if (rejoined)
 362                {
 282363                    nextSourceAddress = sourceAddress;
 282364                    heading = rejoinHeading;
 282365                    return NavigationGuideStatus.Success;
 366                }
 544367                allowRecovery = false;
 544368                continue;
 369            }
 339370            if (targetBodyValid
 339371                && node.SelectedEdge.Target == payload.Key.DestinationAddress
 339372                && IsWithinArrivalRadius(
 339373                    actualFootPosition,
 339374                    targetState.FootAnchor,
 339375                    payload.Key.Agent.ArrivalRadius))
 376            {
 2377                nextSourceAddress = currentSource;
 2378                heading = Vector3d.Zero;
 2379                return NavigationGuideStatus.Success;
 380            }
 337381            if (sourceBodyValid)
 382            {
 328383                nextSourceAddress = currentSource;
 328384                return TrySampleDirectedLeg(
 328385                    sourceState.FootAnchor,
 328386                    sourcePortal,
 328387                    targetPortal,
 328388                    targetState.FootAnchor,
 328389                    actualFootPosition,
 328390                    ref meter,
 328391                    out heading);
 392            }
 393
 9394            NavigationGuideStatus targetProgress = HasReachedOrPassed(
 9395                targetPortal,
 9396                targetState.FootAnchor,
 9397                actualFootPosition,
 9398                out bool passedTarget);
 9399            if (targetProgress != NavigationGuideStatus.Success)
 1400                return targetProgress;
 8401            if (!passedTarget)
 402            {
 2403                nextSourceAddress = currentSource;
 2404                return TrySetHeading(
 2405                    actualFootPosition,
 2406                    targetState.FootAnchor,
 2407                    ref meter,
 2408                    out heading);
 409            }
 6410            if (!meter.TryConsumeCursorRebases(1))
 2411                return NavigationGuideStatus.BudgetExceeded;
 4412            currentSource = node.SelectedEdge.Target;
 413        }
 414    }
 415
 416    private static NavigationGuideStatus TryResolveSelectedEdge(
 417        NavigationWorldGraph graph,
 418        NavigationNodeRef source,
 419        NavigationSelectedEdgeRef selected,
 420        ref GuideSampleWorkMeter meter,
 421        out NavigationGraphEdge edge)
 422    {
 1224423        NavigationSurfaceEdgeEnumerator edges = graph.EnumerateStructuralSurfaceEdges(source);
 1224424        int edgeStepRemaining = int.MaxValue;
 425        while (true)
 426        {
 2524427            NavigationSurfaceEdgeAdvanceStatus status = edges.AdvanceOne(
 2524428                ref meter,
 2524429                ref edgeStepRemaining);
 2524430            NavigationGuideStatus mappedStatus = MapStructuralEdgeStatus(status);
 2524431            if (mappedStatus != NavigationGuideStatus.Success)
 432            {
 2433                edge = default;
 2434                return mappedStatus;
 435            }
 2522436            if (status == NavigationSurfaceEdgeAdvanceStatus.Pending)
 437                continue;
 1274438            if (edges.CurrentOrdinal != selected.CanonicalOutgoingOrdinal)
 439                continue;
 1222440            edge = edges.Current;
 1222441            return NavigationGuideStatus.Success;
 442        }
 443    }
 444
 445    private static NavigationGuideStatus TrySampleVolume(
 446        GridWorld world,
 447        NavigationWorldGraphStore store,
 448        NavigationWorldGraph graph,
 449        NavigationFlowFieldPayload payload,
 450        NavigationCellAddress sourceAddress,
 451        TraversalMedium medium,
 452        Vector3d actualFootPosition,
 453        ref GuideSampleWorkMeter meter,
 454        NavigationImmediateRayWorkspace immediateRayWorkspace,
 455        out NavigationFlowFieldNode currentNode,
 456        out NavigationCellAddress nextSourceAddress,
 457        out Vector3d target,
 458        out Vector3d heading)
 459    {
 102460        currentNode = default;
 102461        nextSourceAddress = sourceAddress;
 102462        target = default;
 102463        heading = Vector3d.Zero;
 102464        NavigationCellAddress currentSource = sourceAddress;
 102465        graph.AreaCatalog.TryGet(
 102466            payload.Key.AreaPolicy,
 102467            out NavigationAreaPolicy areaPolicy);
 468        while (true)
 469        {
 107470            NavigationFlowNodeLookupStatus lookup = TryGetNode(
 107471                payload,
 107472                currentSource,
 107473                medium,
 107474                ref meter,
 107475                out NavigationFlowFieldNode node);
 107476            if (lookup != NavigationFlowNodeLookupStatus.Success)
 477            {
 1478                return MapNodeLookupStatus(lookup, required: true);
 479            }
 106480            currentNode = node;
 106481            if (node.TransitionInstructionOrdinal >= 0)
 482            {
 7483                nextSourceAddress = currentSource;
 7484                return NavigationGuideStatus.Success;
 485            }
 99486            graph.TryGetNodeRef(currentSource, out NavigationNodeRef sourceRef);
 99487            graph.TryGetNodeState(
 99488                sourceRef,
 99489                medium,
 99490                out NavigationNodeState sourceState);
 99491            sourceState.TryGetCenteredVolumeFootAnchor(
 99492                payload.Key.Agent.Shape.Height,
 99493                out Vector3d sourceAnchor);
 99494            target = sourceAnchor;
 99495            if (!node.SelectedEdge.IsValid)
 496            {
 6497                nextSourceAddress = currentSource;
 6498                NavigationGuideStatus destination = RunRay(
 6499                    world,
 6500                    store,
 6501                    graph,
 6502                    payload,
 6503                    areaPolicy,
 6504                    medium,
 6505                    actualFootPosition,
 6506                    sourceAnchor,
 6507                    NavigationRayChainConstraint.SourceOnly(currentSource),
 6508                    ref meter,
 6509                    immediateRayWorkspace);
 6510                if (destination != NavigationGuideStatus.Success)
 1511                    return destination;
 5512                if (IsWithinArrivalRadius(
 5513                        actualFootPosition,
 5514                        sourceAnchor,
 5515                        payload.Key.Agent.ArrivalRadius))
 516                {
 4517                    heading = Vector3d.Zero;
 4518                    return NavigationGuideStatus.Success;
 519                }
 1520                return TrySetHeadingUnchecked(actualFootPosition, sourceAnchor, out heading);
 521            }
 93522            graph.TryGetNodeRef(
 93523                node.SelectedEdge.Target,
 93524                out NavigationNodeRef targetRef);
 93525            graph.TryGetNodeState(
 93526                targetRef,
 93527                medium,
 93528                out NavigationNodeState targetState);
 93529            targetState.TryGetCenteredVolumeFootAnchor(
 93530                payload.Key.Agent.Shape.Height,
 93531                out Vector3d targetAnchor);
 93532            target = targetAnchor;
 93533            if (actualFootPosition == targetAnchor)
 534            {
 8535                NavigationGuideStatus arrived = RunRay(
 8536                    world,
 8537                    store,
 8538                    graph,
 8539                    payload,
 8540                    areaPolicy,
 8541                    medium,
 8542                    actualFootPosition,
 8543                    actualFootPosition,
 8544                    NavigationRayChainConstraint.SourceOnly(node.SelectedEdge.Target),
 8545                    ref meter,
 8546                    immediateRayWorkspace);
 8547                if (arrived != NavigationGuideStatus.Success)
 1548                    return arrived;
 7549                if (!meter.TryConsumeCursorRebases(1))
 2550                    return NavigationGuideStatus.BudgetExceeded;
 5551                currentSource = node.SelectedEdge.Target;
 5552                nextSourceAddress = currentSource;
 5553                continue;
 554            }
 555
 85556            NavigationGuideStatus status = RunRay(
 85557                world,
 85558                store,
 85559                graph,
 85560                payload,
 85561                areaPolicy,
 85562                medium,
 85563                actualFootPosition,
 85564                targetAnchor,
 85565                NavigationRayChainConstraint.SelectedEdge(
 85566                    currentSource,
 85567                    node.SelectedEdge.Target,
 85568                    node.SelectedEdge.CanonicalOutgoingOrdinal),
 85569                ref meter,
 85570                immediateRayWorkspace);
 85571            if (status != NavigationGuideStatus.Success)
 2572                return status;
 83573            nextSourceAddress = currentSource;
 83574            if (node.SelectedEdge.Target == payload.Key.DestinationAddress
 83575                && IsWithinArrivalRadius(
 83576                    actualFootPosition,
 83577                    targetAnchor,
 83578                    payload.Key.Agent.ArrivalRadius))
 579            {
 1580                heading = Vector3d.Zero;
 1581                return NavigationGuideStatus.Success;
 582            }
 82583            return TrySetHeadingUnchecked(actualFootPosition, targetAnchor, out heading);
 584        }
 585    }
 586
 587    private static bool IsWithinArrivalRadius(
 588        Vector3d actualFootPosition,
 589        Vector3d target,
 590        Fixed64 arrivalRadius) =>
 651591        Vector3d.CompareDistanceSquared(
 651592            actualFootPosition,
 651593            target,
 651594            Vector3d.Zero,
 651595            new Vector3d(arrivalRadius, Fixed64.Zero, Fixed64.Zero)) <= 0;
 596
 597    internal static NavigationGuideStatus TrySampleTransitionApproach(
 598        GridWorld world,
 599        NavigationWorldGraphStore store,
 600        NavigationWorldGraph graph,
 601        NavigationFlowFieldPayload payload,
 602        NavigationCellAddress source,
 603        TraversalMedium medium,
 604        Vector3d actualFootPosition,
 605        Vector3d actionPosition,
 606        ref GuideSampleWorkMeter meter,
 607        NavigationImmediateRayWorkspace immediateRayWorkspace,
 608        out Vector3d heading)
 609    {
 21610        heading = Vector3d.Zero;
 21611        bool alreadyAtAction = actualFootPosition == actionPosition;
 21612        graph.AreaCatalog.TryGet(
 21613            payload.Key.AreaPolicy,
 21614            out NavigationAreaPolicy areaPolicy);
 21615        NavigationGuideStatus status = RunRay(
 21616            world,
 21617            store,
 21618            graph,
 21619            payload,
 21620            areaPolicy,
 21621            medium,
 21622            actualFootPosition,
 21623            actionPosition,
 21624            NavigationRayChainConstraint.SourceOnly(source),
 21625            ref meter,
 21626            immediateRayWorkspace);
 21627        if (alreadyAtAction && status == NavigationGuideStatus.LocalRecoveryRequired)
 4628            return NavigationGuideStatus.Success;
 17629        return status == NavigationGuideStatus.Success
 17630            ? TrySetHeadingUnchecked(actualFootPosition, actionPosition, out heading)
 17631            : status;
 632    }
 633
 634    private static NavigationGuideStatus RunRay(
 635        GridWorld world,
 636        NavigationWorldGraphStore store,
 637        NavigationWorldGraph graph,
 638        NavigationFlowFieldPayload payload,
 639        NavigationAreaPolicy areaPolicy,
 640        TraversalMedium medium,
 641        Vector3d start,
 642        Vector3d end,
 643        NavigationRayChainConstraint constraint,
 644        ref GuideSampleWorkMeter meter,
 645        NavigationImmediateRayWorkspace immediateRayWorkspace)
 646    {
 120647        lock (immediateRayWorkspace.SyncRoot)
 648        {
 120649            NavigationWorkMeter rayMeter = immediateRayWorkspace.WorkMeter;
 120650            rayMeter.ResetForGuideSample(
 120651                meter.GetCurrentNodeLookupAllowance(),
 120652                meter.GetCursorLegScanAllowance(),
 120653                meter.GetPortalCheckAllowance(),
 120654                meter.GetPrismCheckAllowance(),
 120655                meter.GetTraceIntervalAllowance());
 120656            NavigationRayWork ray = immediateRayWorkspace.RayWork;
 120657            ray.Begin(new NavigationRayRequest(
 120658                world,
 120659                store,
 120660                graph,
 120661                payload.Key.Agent,
 120662                areaPolicy,
 120663                medium,
 120664                start,
 120665                end,
 120666                NavigationRayEndpointAllowance.StartPrefix,
 120667                constraint));
 668            NavigationRayStatus status;
 669            try
 670            {
 671                do
 672                {
 120673                    status = ray.Advance(rayMeter);
 674                }
 120675                while (status == NavigationRayStatus.Pending);
 120676            }
 677            finally
 678            {
 120679                ray.Reset();
 120680            }
 120681            meter.TryConsumeCurrentNodeLookupProbes(
 120682                checked(rayMeter.LookupProbes + rayMeter.CoveredVoxelIntervals));
 120683            meter.TryConsumeCursorLegScans(
 120684                checked(rayMeter.EvaluatedEdges + rayMeter.ConnectionLegs));
 120685            meter.TryConsumePortalChecks(rayMeter.GuidePortalChecks);
 120686            meter.TryConsumePrismChecks(rayMeter.GuidePrismChecks);
 120687            meter.TryConsumeTraceIntervals(rayMeter.TraceIntervals);
 120688            return status == NavigationRayStatus.Success
 120689                ? NavigationGuideStatus.Success
 120690                : NavigationGuideStatusMapper.ToPublic(status);
 691        }
 120692    }
 693
 694    private static NavigationFlowNodeLookupStatus TryGetNode(
 695        NavigationFlowFieldPayload payload,
 696        NavigationCellAddress address,
 697        TraversalMedium medium,
 698        ref GuideSampleWorkMeter meter,
 699        out NavigationFlowFieldNode node)
 700    {
 2475701        int low = 0;
 2475702        int high = payload.AddressLookupOrdinals.Length - 1;
 6051703        while (low <= high)
 704        {
 6050705            if (!meter.TryConsumeCurrentNodeLookupProbes(1))
 706            {
 12707                node = default;
 12708                return NavigationFlowNodeLookupStatus.BudgetExceeded;
 709            }
 6038710            int middle = low + ((high - low) >> 1);
 6038711            NavigationFlowFieldNode candidate =
 6038712                payload.Nodes[payload.AddressLookupOrdinals[middle]];
 6038713            int comparison = candidate.Address.CompareTo(address);
 6038714            if (comparison == 0)
 2462715                comparison = ((int)candidate.Medium).CompareTo((int)medium);
 6038716            if (comparison == 0)
 717            {
 2462718                node = candidate;
 2462719                return NavigationFlowNodeLookupStatus.Success;
 720            }
 3576721            if (comparison < 0)
 2344722                low = middle + 1;
 723            else
 1232724                high = middle - 1;
 725        }
 1726        node = default;
 1727        return NavigationFlowNodeLookupStatus.NotFound;
 728    }
 729
 730    private static NavigationGuideStatus TryAdvanceRecovery(
 731        GridWorld world,
 732        NavigationWorldGraphStore store,
 733        NavigationWorldGraph graph,
 734        NavigationFlowFieldPayload payload,
 735        TraversalMedium medium,
 736        Vector3d actualFootPosition,
 737        ref GuideSampleWorkMeter meter,
 738        GridCoveredAddressCursor coveredAddressCursor,
 739        GridCoveredAddressGeneration[] coveredAddressGenerations,
 740        GridCoveredAddress[] coveredAddressOutput,
 741        NavigationImmediateRayWorkspace immediateRayWorkspace,
 742        NavigationCellAddress sourceAddress,
 743        NavigationNodeState sourceState,
 744        NavigationSelectedEdgeRef selected,
 745        Vector3d selectedExitTarget,
 746        NavigationNodeState targetState,
 747        bool hasSelectedEdge,
 748        bool allowRecovery,
 749        out NavigationCellAddress rebased,
 750        out Vector3d heading,
 751        out bool rejoined)
 752    {
 862753        rebased = default;
 862754        heading = Vector3d.Zero;
 862755        rejoined = false;
 862756        if (!allowRecovery)
 5757            return NavigationGuideStatus.LocalRecoveryRequired;
 857758        NavigationGuideStatus rebase = TryRebase(
 857759            world,
 857760            graph,
 857761            payload,
 857762            medium,
 857763            actualFootPosition,
 857764            ref meter,
 857765            coveredAddressCursor,
 857766            coveredAddressGenerations,
 857767            coveredAddressOutput,
 857768            out rebased);
 857769        if (rebase != NavigationGuideStatus.LocalRecoveryRequired)
 566770            return rebase;
 291771        NavigationGuideStatus status = TryRejoin(
 291772            world,
 291773            store,
 291774            graph,
 291775            payload,
 291776            medium,
 291777            sourceAddress,
 291778            sourceState,
 291779            selected,
 291780            selectedExitTarget,
 291781            targetState,
 291782            hasSelectedEdge,
 291783            actualFootPosition,
 291784            ref meter,
 291785            immediateRayWorkspace,
 291786            out heading);
 291787        rejoined = status == NavigationGuideStatus.Success;
 291788        return status;
 789    }
 790
 791    private static NavigationGuideStatus TryRejoin(
 792        GridWorld world,
 793        NavigationWorldGraphStore store,
 794        NavigationWorldGraph graph,
 795        NavigationFlowFieldPayload payload,
 796        TraversalMedium medium,
 797        NavigationCellAddress sourceAddress,
 798        NavigationNodeState sourceState,
 799        NavigationSelectedEdgeRef selected,
 800        Vector3d selectedExitTarget,
 801        NavigationNodeState targetState,
 802        bool hasSelectedEdge,
 803        Vector3d actualFootPosition,
 804        ref GuideSampleWorkMeter meter,
 805        NavigationImmediateRayWorkspace immediateRayWorkspace,
 806        out Vector3d heading)
 807    {
 291808        heading = Vector3d.Zero;
 291809        graph.AreaCatalog.TryGet(
 291810            payload.Key.AreaPolicy,
 291811            out NavigationAreaPolicy areaPolicy);
 291812        lock (immediateRayWorkspace.SyncRoot)
 813        {
 291814            NavigationRayWork ray = immediateRayWorkspace.RayWork;
 305815            for (int targetOrdinal = 0; ; targetOrdinal++)
 816            {
 305817                if (targetOrdinal > 0 && !hasSelectedEdge)
 1818                    return NavigationGuideStatus.LocalRecoveryRequired;
 304819                if (!TryGetRejoinTarget(
 304820                    sourceAddress,
 304821                    sourceState,
 304822                    targetState,
 304823                    selected,
 304824                    selectedExitTarget,
 304825                    targetOrdinal,
 304826                    out NavigationFlowRejoinTarget target))
 827                {
 4828                    return NavigationGuideStatus.LocalRecoveryRequired;
 829                }
 830
 300831                ray.Begin(new NavigationRayRequest(
 300832                    world,
 300833                    store,
 300834                    graph,
 300835                    payload.Key.Agent,
 300836                    areaPolicy,
 300837                    medium,
 300838                    actualFootPosition,
 300839                    target.Position,
 300840                    NavigationRayEndpointAllowance.StartPrefix,
 300841                    target.Constraint));
 842                NavigationRayStatus status;
 843                NavigationRayResult result;
 844                try
 845                {
 846                    do
 847                    {
 300848                        status = ray.Advance(ref meter);
 849                    }
 300850                    while (status == NavigationRayStatus.Pending);
 300851                    result = ray.Result;
 300852                }
 853                finally
 854                {
 300855                    ray.Reset();
 300856                }
 300857                if (status == NavigationRayStatus.Blocked
 300858                    || (status == NavigationRayStatus.Success
 300859                        && !result.IsSemanticCostNeutral
 300860                        && target.Constraint.Kind
 300861                            != NavigationRayChainConstraintKind.SelectedEdge))
 862                {
 863                    continue;
 864                }
 286865                if (status != NavigationRayStatus.Success)
 1866                    return NavigationGuideStatusMapper.ToPublic(status);
 285867                return TrySetHeadingUnchecked(
 285868                    actualFootPosition,
 285869                    target.Position,
 285870                    out heading);
 871            }
 872        }
 291873    }
 874
 875    private static NavigationGuideStatus TryResolvePortal(
 876        NavigationGraphEdge edge,
 877        NavigationNodeState sourceState,
 878        KinematicBodyShape shape,
 879        ref GuideSampleWorkMeter meter,
 880        out GridNavigationPortal resolvedPortal,
 881        out Vector3d sourcePortal,
 882        out Vector3d targetPortal)
 883    {
 1190884        resolvedPortal = default;
 1190885        sourcePortal = default;
 1190886        targetPortal = default;
 1190887        if (!meter.TryConsumePortalChecks(1))
 11888            return NavigationGuideStatus.BudgetExceeded;
 1179889        bool reverse = false;
 1179890        if (edge.Kind == NavigationGraphEdgeKind.Native)
 891        {
 1175892            edge.NativePortal.TryTranslate(sourceState.Center, out resolvedPortal);
 893        }
 894        else
 895        {
 4896            NavigationAutomaticSeamRef seam = edge.AutomaticSeam;
 4897            resolvedPortal = seam.Portal;
 4898            reverse = seam.IsReverse;
 899        }
 1179900        resolvedPortal.TryResolveProfile(
 1179901            shape.Radius,
 1179902            shape.Height,
 1179903            out Vector3d first,
 1179904            out Vector3d second);
 1179905        sourcePortal = reverse ? second : first;
 1179906        targetPortal = reverse ? first : second;
 1179907        return NavigationGuideStatus.Success;
 908    }
 909
 910    private static NavigationGuideStatus TrySampleDirectedLeg(
 911        Vector3d sourceFootAnchor,
 912        Vector3d sourcePortal,
 913        Vector3d targetPortal,
 914        Vector3d targetFootAnchor,
 915        Vector3d actualFootPosition,
 916        ref GuideSampleWorkMeter meter,
 917        out Vector3d heading)
 918    {
 336919        int traceIntervals = sourcePortal == targetPortal ? 2 : 3;
 336920        if (!meter.TryConsumeCursorLegScans(1)
 336921            || !meter.TryConsumeTraceIntervals(traceIntervals))
 922        {
 3923            heading = Vector3d.Zero;
 3924            return NavigationGuideStatus.BudgetExceeded;
 925        }
 333926        return TrySampleDirectedLegUnchecked(
 333927            sourceFootAnchor,
 333928            sourcePortal,
 333929            targetPortal,
 333930            targetFootAnchor,
 333931            actualFootPosition,
 333932            out heading);
 933    }
 934
 935    private static NavigationGuideStatus TrySampleDirectedLegUnchecked(
 936        Vector3d sourceFootAnchor,
 937        Vector3d sourcePortal,
 938        Vector3d targetPortal,
 939        Vector3d targetFootAnchor,
 940        Vector3d actualFootPosition,
 941        out Vector3d heading)
 942    {
 333943        NavigationGuideStatus status = HasReachedOrPassed(
 333944            sourceFootAnchor,
 333945            sourcePortal,
 333946            actualFootPosition,
 333947            out bool passedSourcePortal);
 333948        bool passedTargetPortal = false;
 333949        if (status == NavigationGuideStatus.Success && passedSourcePortal)
 950        {
 2951            status = HasReachedOrPassed(
 2952                sourcePortal,
 2953                targetPortal,
 2954                actualFootPosition,
 2955                out passedTargetPortal);
 956        }
 333957        if (status != NavigationGuideStatus.Success)
 1958            return FailDirectedLeg(status, out heading);
 332959        if (!passedSourcePortal)
 960        {
 330961            return TrySetHeadingUnchecked(actualFootPosition, sourcePortal, out heading);
 962        }
 2963        return TrySetHeadingUnchecked(
 2964            actualFootPosition,
 2965            passedTargetPortal ? targetFootAnchor : targetPortal,
 2966            out heading);
 967    }
 968
 969    private static NavigationGuideStatus FailDirectedLeg(
 970        NavigationGuideStatus status,
 971        out Vector3d heading)
 972    {
 1973        heading = Vector3d.Zero;
 1974        return status;
 975    }
 976
 977    private static NavigationGuideStatus TrySampleExplicit(
 978        NavigationWorldGraph graph,
 979        NavigationExplicitConnectionRecord record,
 980        NavigationCellAddress sourceAddress,
 981        NavigationNodeState sourceState,
 982        GridCellPrism sourcePrism,
 983        NavigationNodeState targetState,
 984        GridCellPrism targetPrism,
 985        bool sourceContains,
 986        bool targetContains,
 987        Vector3d actualFootPosition,
 988        KinematicBodyShape shape,
 989        bool targetIsDestination,
 990        Fixed64 arrivalRadius,
 991        ref GuideSampleWorkMeter meter,
 992        out NavigationCellAddress nextSourceAddress,
 993        out Vector3d heading)
 994    {
 30995        nextSourceAddress = sourceAddress;
 30996        heading = Vector3d.Zero;
 30997        NavigationConnection connection = record.Definition;
 30998        NavigationPagedSequence<GridNavigationPortal>.Enumerator portals =
 30999            record.NavigationPortals.GetEnumerator();
 301000        NavigationGuideStatus firstPortalStatus = TryReadExplicitPortal(
 301001            ref portals,
 301002            shape,
 301003            ref meter,
 301004            out GridNavigationPortal incomingPortal,
 301005            out Vector3d incomingSourceAnchor,
 301006            out Vector3d incomingTargetAnchor);
 301007        if (firstPortalStatus != NavigationGuideStatus.Success)
 11008            return firstPortalStatus;
 1009
 291010        bool zeroWitnessTargetBodyValid = connection.Witnesses.Count == 0
 291011            && targetContains
 291012            && GridCellGeometry.IsNavigationBodyAnchorValid(
 291013                targetPrism,
 291014                actualFootPosition,
 291015                shape.Radius,
 291016                shape.Height,
 291017                incomingPortal);
 291018        if (targetIsDestination
 291019            && zeroWitnessTargetBodyValid
 291020            && IsWithinArrivalRadius(actualFootPosition, targetState.FootAnchor, arrivalRadius))
 1021        {
 11022            return NavigationGuideStatus.Success;
 1023        }
 1024
 281025        bool sourceBodyValid = sourceContains
 281026            && GridCellGeometry.IsNavigationBodyAnchorValid(
 281027                sourcePrism,
 281028                actualFootPosition,
 281029                shape.Radius,
 281030                shape.Height,
 281031                incomingPortal);
 281032        if (sourceBodyValid && !zeroWitnessTargetBodyValid)
 1033        {
 61034            NavigationGuideStatus progressStatus = HasReachedOrPassed(
 61035                sourceState.FootAnchor,
 61036                connection.EntryAnchor,
 61037                actualFootPosition,
 61038                out bool passedEntry);
 61039            if (progressStatus != NavigationGuideStatus.Success)
 11040                return progressStatus;
 51041            if (!passedEntry)
 1042            {
 21043                return TrySetHeading(
 21044                    actualFootPosition,
 21045                    connection.EntryAnchor,
 21046                    ref meter,
 21047                    out heading);
 1048            }
 31049            if (connection.Witnesses.Count == 0)
 1050            {
 21051                return TrySampleDirectedLeg(
 21052                    connection.EntryAnchor,
 21053                    incomingSourceAnchor,
 21054                    incomingTargetAnchor,
 21055                    connection.ExitAnchor,
 21056                    actualFootPosition,
 21057                    ref meter,
 21058                    out heading);
 1059            }
 11060            NavigationCellAddress firstAddress = connection.Witnesses[0];
 11061            graph.TryGetNodeRef(firstAddress, out NavigationNodeRef firstRef);
 11062            graph.TryGetNodeState(firstRef, out NavigationNodeState firstState);
 11063            return TrySampleDirectedLeg(
 11064                connection.EntryAnchor,
 11065                incomingSourceAnchor,
 11066                incomingTargetAnchor,
 11067                firstState.FootAnchor,
 11068                actualFootPosition,
 11069                ref meter,
 11070                out heading);
 1071        }
 1072
 821073        for (int i = 0; i < connection.Witnesses.Count; i++)
 1074        {
 261075            NavigationGuideStatus outgoingPortalStatus = TryReadExplicitPortal(
 261076                ref portals,
 261077                shape,
 261078                ref meter,
 261079                out GridNavigationPortal outgoingPortal,
 261080                out Vector3d outgoingSourceAnchor,
 261081                out Vector3d outgoingTargetAnchor);
 261082            if (outgoingPortalStatus != NavigationGuideStatus.Success)
 11083                return outgoingPortalStatus;
 251084            NavigationCellAddress witnessAddress = connection.Witnesses[i];
 251085            graph.TryGetSeamPrism(witnessAddress, out GridCellPrism witnessPrism);
 251086            if (!meter.TryConsumePrismChecks(1))
 11087                return NavigationGuideStatus.BudgetExceeded;
 241088            if (!witnessPrism.Contains(actualFootPosition))
 1089            {
 181090                incomingPortal = outgoingPortal;
 181091                incomingTargetAnchor = outgoingTargetAnchor;
 181092                continue;
 1093            }
 61094            bool witnessBodyValid = GridCellGeometry.IsNavigationBodyAnchorValid(
 61095                    witnessPrism,
 61096                    actualFootPosition,
 61097                    shape.Radius,
 61098                    shape.Height,
 61099                    outgoingPortal)
 61100                || GridCellGeometry.IsNavigationBodyAnchorValid(
 61101                    witnessPrism,
 61102                    actualFootPosition,
 61103                    shape.Radius,
 61104                    shape.Height,
 61105                    incomingPortal);
 61106            if (!witnessBodyValid)
 1107            {
 11108                incomingPortal = outgoingPortal;
 11109                incomingTargetAnchor = outgoingTargetAnchor;
 11110                continue;
 1111            }
 51112            NavigationCellAddress nextAddress = i + 1 < connection.Witnesses.Count
 51113                ? connection.Witnesses[i + 1]
 51114                : record.Destination;
 1115            NavigationNodeState nextState;
 51116            if (nextAddress == record.Destination)
 1117            {
 11118                nextState = targetState;
 1119            }
 1120            else
 1121            {
 41122                graph.TryGetNodeRef(nextAddress, out NavigationNodeRef nextRef);
 41123                graph.TryGetNodeState(nextRef, out nextState);
 1124            }
 51125            return TrySampleDirectedLeg(
 51126                incomingTargetAnchor,
 51127                outgoingSourceAnchor,
 51128                outgoingTargetAnchor,
 51129                nextAddress == record.Destination
 51130                    ? connection.ExitAnchor
 51131                    : nextState.FootAnchor,
 51132                actualFootPosition,
 51133                ref meter,
 51134                out heading);
 1135        }
 1136
 151137        bool targetBodyValid = zeroWitnessTargetBodyValid
 151138            || (connection.Witnesses.Count > 0
 151139                && targetContains
 151140                && GridCellGeometry.IsNavigationBodyAnchorValid(
 151141                    targetPrism,
 151142                    actualFootPosition,
 151143                    shape.Radius,
 151144                    shape.Height,
 151145                    incomingPortal));
 151146        if (targetBodyValid)
 1147        {
 91148            if (targetIsDestination
 91149                && IsWithinArrivalRadius(actualFootPosition, targetState.FootAnchor, arrivalRadius))
 1150            {
 11151                return NavigationGuideStatus.Success;
 1152            }
 81153            NavigationGuideStatus progressStatus = HasReachedOrPassed(
 81154                incomingTargetAnchor,
 81155                connection.ExitAnchor,
 81156                actualFootPosition,
 81157                out bool passedExit);
 81158            if (progressStatus != NavigationGuideStatus.Success)
 11159                return progressStatus;
 71160            if (!passedExit)
 1161            {
 21162                return TrySetHeading(
 21163                    actualFootPosition,
 21164                    connection.ExitAnchor,
 21165                    ref meter,
 21166                    out heading);
 1167            }
 51168            progressStatus = HasReachedOrPassed(
 51169                connection.ExitAnchor,
 51170                targetState.FootAnchor,
 51171                actualFootPosition,
 51172                out bool passedTarget);
 51173            if (progressStatus != NavigationGuideStatus.Success)
 11174                return progressStatus;
 41175            if (!passedTarget)
 1176            {
 11177                return TrySetHeading(
 11178                    actualFootPosition,
 11179                    targetState.FootAnchor,
 11180                    ref meter,
 11181                    out heading);
 1182            }
 31183            if (!meter.TryConsumeCursorRebases(1))
 11184                return NavigationGuideStatus.BudgetExceeded;
 21185            nextSourceAddress = record.Destination;
 21186            return NavigationGuideStatus.Success;
 1187        }
 61188        return NavigationGuideStatus.LocalRecoveryRequired;
 1189    }
 1190
 1191    private static NavigationGuideStatus TryReadExplicitPortal(
 1192        ref NavigationPagedSequence<GridNavigationPortal>.Enumerator portals,
 1193        KinematicBodyShape shape,
 1194        ref GuideSampleWorkMeter meter,
 1195        out GridNavigationPortal portal,
 1196        out Vector3d sourcePortalAnchor,
 1197        out Vector3d targetPortalAnchor)
 1198    {
 561199        portal = default;
 561200        sourcePortalAnchor = default;
 561201        targetPortalAnchor = default;
 561202        if (!meter.TryConsumePortalChecks(1))
 21203            return NavigationGuideStatus.BudgetExceeded;
 541204        portals.MoveNext();
 541205        portal = portals.Current;
 541206        portal.TryResolveProfile(
 541207            shape.Radius,
 541208            shape.Height,
 541209            out sourcePortalAnchor,
 541210            out targetPortalAnchor);
 541211        return NavigationGuideStatus.Success;
 1212    }
 1213
 1214    internal static NavigationGuideStatus TrySetHeading(
 1215        Vector3d actualFootPosition,
 1216        Vector3d target,
 1217        ref GuideSampleWorkMeter meter,
 1218        out Vector3d heading)
 1219    {
 141220        if (!meter.TryConsumeCursorLegScans(1)
 141221            || !meter.TryConsumeTraceIntervals(1))
 1222        {
 21223            heading = Vector3d.Zero;
 21224            return NavigationGuideStatus.BudgetExceeded;
 1225        }
 121226        return TrySetHeadingUnchecked(actualFootPosition, target, out heading);
 1227    }
 1228
 1229    private static NavigationGuideStatus TrySetHeadingUnchecked(
 1230        Vector3d actualFootPosition,
 1231        Vector3d target,
 1232        out Vector3d heading)
 1233    {
 7241234        heading = Vector3d.Zero;
 7241235        if (!Vector3d.TrySubtract(target, actualFootPosition, out Vector3d delta))
 11236            return NavigationGuideStatus.CostOverflow;
 7231237        if (delta != Vector3d.Zero)
 7141238            heading = delta.Normalized;
 7231239        return NavigationGuideStatus.Success;
 1240    }
 1241
 1242    internal static NavigationGuideStatus HasReachedOrPassed(
 1243        Vector3d start,
 1244        Vector3d end,
 1245        Vector3d actual,
 1246        out bool passed)
 1247    {
 3691248        passed = false;
 3691249        if (start == end)
 1250        {
 41251            passed = true;
 41252            return NavigationGuideStatus.Success;
 1253        }
 3651254        if (!Vector3d.TrySubtract(end, start, out Vector3d direction)
 3651255            || !Vector3d.TrySubtract(actual, end, out Vector3d beyond)
 3651256            || !Vector3d.TryDot(beyond, direction, out Fixed64 projection))
 1257        {
 81258            return NavigationGuideStatus.CostOverflow;
 1259        }
 3571260        passed = projection >= Fixed64.Zero;
 3571261        return NavigationGuideStatus.Success;
 1262    }
 1263
 1264    private static NavigationGuideStatus TryRebase(
 1265        GridWorld world,
 1266        NavigationWorldGraph graph,
 1267        NavigationFlowFieldPayload payload,
 1268        TraversalMedium medium,
 1269        Vector3d actualFootPosition,
 1270        ref GuideSampleWorkMeter meter,
 1271        GridCoveredAddressCursor cursor,
 1272        GridCoveredAddressGeneration[] generations,
 1273        GridCoveredAddress[] output,
 1274        out NavigationCellAddress rebased)
 1275    {
 8571276        rebased = default;
 8571277        if (!meter.TryConsumeLocalRecoveryAttempts(1)
 8571278            || !meter.TryConsumeCursorRebases(1))
 1279        {
 71280            return NavigationGuideStatus.BudgetExceeded;
 1281        }
 33981282        for (int i = 0; i < graph.MapCount; i++)
 1283        {
 8501284            if (!meter.TryConsumeCurrentNodeLookupProbes(1))
 11285                return NavigationGuideStatus.BudgetExceeded;
 8491286            bool foundGeneration = graph.TryGetCoveredAddressGeneration(
 8491287                i,
 8491288                out _,
 8491289                out generations[i]);
 1290            System.Diagnostics.Debug.Assert(
 1291                foundGeneration,
 1292                "published graph instances are materialized and indexed one-to-one");
 1293        }
 8491294        bool began = world.TryBeginCoveredAddresses(
 8491295            cursor,
 8491296            actualFootPosition,
 8491297            actualFootPosition,
 8491298            graph.MapCount);
 1299        System.Diagnostics.Debug.Assert(began);
 1300
 8491301        int inputOrdinal = 0;
 8491302        bool hasCandidate = false;
 8491303        Fixed64 bestDistance = Fixed64.Zero;
 8491304        NavigationCellAddress best = default;
 1305        while (true)
 1306        {
 39941307            if (inputOrdinal < graph.MapCount)
 1308            {
 8491309                int available = meter.GetCurrentNodeLookupAllowance();
 8491310                if (available == 0)
 11311                    return NavigationGuideStatus.BudgetExceeded;
 8481312                GridCoveredAddressCursorStatus bindStatus = world.AdvanceCoveredAddresses(
 8481313                    cursor,
 8481314                    generations.AsSpan(inputOrdinal, graph.MapCount - inputOrdinal),
 8481315                    output,
 8481316                    lookupProbeLimit: available,
 8481317                    addressProbeLimit: 0,
 8481318                    outputLimit: 0,
 8481319                    out int lookupProbes,
 8481320                    out int addressProbes,
 8481321                    out int inputsConsumed,
 8481322                    out int outputCount);
 8481323                meter.TryConsumeCurrentNodeLookupProbes(
 8481324                    checked(lookupProbes + addressProbes + outputCount));
 8481325                inputOrdinal += inputsConsumed;
 8481326                if (bindStatus == GridCoveredAddressCursorStatus.Stale)
 11327                    return NavigationGuideStatus.Stale;
 1328                continue;
 1329            }
 1330
 31451331            int outputBudget = meter.GetCurrentNodeLookupAllowance() > 0 ? 1 : 0;
 31451332            GridCoveredAddressCursorStatus flushStatus = world.AdvanceCoveredAddresses(
 31451333                cursor,
 31451334                ReadOnlySpan<GridCoveredAddressGeneration>.Empty,
 31451335                output,
 31451336                lookupProbeLimit: 0,
 31451337                addressProbeLimit: 0,
 31451338                outputLimit: outputBudget,
 31451339                out int flushLookups,
 31451340                out int flushAddresses,
 31451341                out _,
 31451342                out int flushed);
 31451343            meter.TryConsumeCurrentNodeLookupProbes(
 31451344                checked(flushLookups + flushAddresses + flushed));
 31451345            if (flushed != 0)
 1346            {
 1347                System.Diagnostics.Debug.Assert(
 1348                    flushStatus != GridCoveredAddressCursorStatus.Stale,
 1349                    "a stale covered-address cursor cannot publish output");
 11501350                NavigationGuideStatus candidateStatus = ConsiderCandidate(
 11501351                    graph,
 11501352                    payload,
 11501353                    medium,
 11501354                    actualFootPosition,
 11501355                    output[0],
 11501356                    ref meter,
 11501357                    ref hasCandidate,
 11501358                    ref bestDistance,
 11501359                    ref best);
 11501360                if (candidateStatus != NavigationGuideStatus.Success)
 21361                    return candidateStatus;
 1362                continue;
 1363            }
 19951364            if (TryResolveRebaseCursorStatus(
 19951365                    flushStatus,
 19951366                    hasCandidate,
 19951367                    best,
 19951368                    out NavigationGuideStatus flushResult,
 19951369                    out rebased))
 1370            {
 8421371                return flushResult;
 1372            }
 1373
 11531374            int addressBudget = meter.GetCurrentNodeLookupAllowance();
 11531375            if (addressBudget == 0)
 11376                return NavigationGuideStatus.BudgetExceeded;
 11521377            GridCoveredAddressCursorStatus addressStatus = world.AdvanceCoveredAddresses(
 11521378                cursor,
 11521379                ReadOnlySpan<GridCoveredAddressGeneration>.Empty,
 11521380                output,
 11521381                lookupProbeLimit: 0,
 11521382                addressProbeLimit: addressBudget,
 11521383                outputLimit: 0,
 11521384                out int addressLookups,
 11521385                out int enumeratedAddresses,
 11521386                out _,
 11521387                out int addressOutputs);
 11521388            meter.TryConsumeCurrentNodeLookupProbes(
 11521389                checked(addressLookups + enumeratedAddresses + addressOutputs));
 11521390            if (TryResolveRebaseCursorStatus(
 11521391                    addressStatus,
 11521392                    hasCandidate,
 11521393                    best,
 11521394                    out NavigationGuideStatus addressResult,
 11521395                    out rebased))
 1396            {
 21397                return addressResult;
 1398            }
 1399        }
 1400    }
 1401
 1402    internal static bool TryResolveRebaseCursorStatus(
 1403        GridCoveredAddressCursorStatus cursorStatus,
 1404        bool hasCandidate,
 1405        NavigationCellAddress best,
 1406        out NavigationGuideStatus status,
 1407        out NavigationCellAddress rebased)
 1408    {
 31511409        if (cursorStatus == GridCoveredAddressCursorStatus.Stale)
 1410        {
 11411            status = NavigationGuideStatus.Stale;
 11412            rebased = default;
 11413            return true;
 1414        }
 31501415        if (cursorStatus == GridCoveredAddressCursorStatus.Complete)
 1416        {
 8461417            status = hasCandidate
 8461418                ? NavigationGuideStatus.Success
 8461419                : NavigationGuideStatus.LocalRecoveryRequired;
 8461420            rebased = best;
 8461421            return true;
 1422        }
 23041423        status = NavigationGuideStatus.Success;
 23041424        rebased = default;
 23041425        return false;
 1426    }
 1427
 1428    private static NavigationGuideStatus ConsiderCandidate(
 1429        NavigationWorldGraph graph,
 1430        NavigationFlowFieldPayload payload,
 1431        TraversalMedium medium,
 1432        Vector3d actualFootPosition,
 1433        GridCoveredAddress candidate,
 1434        ref GuideSampleWorkMeter meter,
 1435        ref bool hasCandidate,
 1436        ref Fixed64 bestDistance,
 1437        ref NavigationCellAddress best)
 1438    {
 1439        // Covered generations originate from this graph's configuration index.
 11501440        graph.TryGetMapId(candidate.ConfigurationKey, out string mapId);
 11501441        var address = new NavigationCellAddress(mapId, candidate.VoxelIndex);
 11501442        return ConsiderCandidateAddress(
 11501443            graph,
 11501444            payload,
 11501445            medium,
 11501446            actualFootPosition,
 11501447            address,
 11501448            ref meter,
 11501449            ref hasCandidate,
 11501450            ref bestDistance,
 11501451            ref best);
 1452    }
 1453
 1454    internal static NavigationGuideStatus ConsiderCandidateAddress(
 1455        NavigationWorldGraph graph,
 1456        NavigationFlowFieldPayload payload,
 1457        TraversalMedium medium,
 1458        Vector3d actualFootPosition,
 1459        NavigationCellAddress address,
 1460        ref GuideSampleWorkMeter meter,
 1461        ref bool hasCandidate,
 1462        ref Fixed64 bestDistance,
 1463        ref NavigationCellAddress best)
 1464    {
 11511465        if (!meter.TryConsumePrismChecks(1))
 11466            return NavigationGuideStatus.BudgetExceeded;
 11501467        bool foundPrism = graph.TryGetSeamPrism(address, out GridCellPrism prism);
 1468        System.Diagnostics.Debug.Assert(foundPrism,
 1469            "covered-address candidates retain their graph-owned configuration prism");
 11501470        if (!prism.Contains(actualFootPosition)
 11501471            || !graph.TryGetNodeRef(address, out NavigationNodeRef node)
 11501472            || !graph.TryGetNodeState(node, medium, out NavigationNodeState state)
 11501473            || !state.IsPresent)
 1474        {
 5951475            return NavigationGuideStatus.Success;
 1476        }
 5551477        NavigationFlowNodeLookupStatus lookup = TryGetNode(
 5551478            payload,
 5551479            address,
 5551480            medium,
 5551481            ref meter,
 5551482            out _);
 5551483        if (lookup != NavigationFlowNodeLookupStatus.Success)
 1484        {
 11485            return MapNodeLookupStatus(lookup, required: false);
 1486        }
 5541487        if (!Vector3d.TryGetDistance(
 5541488                actualFootPosition,
 5541489                state.FootAnchor,
 5541490                out Fixed64 distance))
 1491        {
 11492            return NavigationGuideStatus.CostOverflow;
 1493        }
 5531494        SelectNearestCandidate(
 5531495            distance,
 5531496            address,
 5531497            ref hasCandidate,
 5531498            ref bestDistance,
 5531499            ref best);
 5531500        return NavigationGuideStatus.Success;
 1501    }
 1502
 1503    internal static void SelectNearestCandidate(
 1504        Fixed64 candidateDistance,
 1505        NavigationCellAddress candidate,
 1506        ref bool hasCandidate,
 1507        ref Fixed64 bestDistance,
 1508        ref NavigationCellAddress best)
 1509    {
 5591510        if (!hasCandidate
 5591511            || candidateDistance < bestDistance
 5591512            || (candidateDistance == bestDistance && candidate.CompareTo(best) < 0))
 1513        {
 5561514            hasCandidate = true;
 5561515            bestDistance = candidateDistance;
 5561516            best = candidate;
 1517        }
 5591518    }
 1519}
Generated by: ReportGenerator 5.5.11.0
09/02/2026 - 04:28:06
GitHub | reportgenerator.io

Methods/Properties

MapNodeLookupStatus(Trailblazer.Pathing.NavigationFlowNodeLookupStatus,System.Boolean)
MapStructuralEdgeStatus(Trailblazer.Pathing.NavigationSurfaceEdgeAdvanceStatus)
TryGetRejoinTarget(Trailblazer.Pathing.NavigationCellAddress,Trailblazer.Pathing.NavigationNodeState,Trailblazer.Pathing.NavigationNodeState,Trailblazer.Pathing.NavigationSelectedEdgeRef,FixedMathSharp.Vector3d,System.Int32,Trailblazer.Pathing.NavigationFlowRejoinTarget&)
TrySample(System.Boolean,GridForge.Grids.GridWorld,Trailblazer.Pathing.NavigationWorldGraphStore,Trailblazer.Pathing.NavigationWorldGraph,Trailblazer.Pathing.NavigationFlowFieldPayload,Trailblazer.Pathing.NavigationCellAddress,Trailblazer.TraversalMedium,FixedMathSharp.Vector3d,Trailblazer.Pathing.GuideSampleWorkMeter&,GridForge.Grids.Topology.GridCoveredAddressCursor,GridForge.Grids.Topology.GridCoveredAddressGeneration[],GridForge.Grids.Topology.GridCoveredAddress[],Trailblazer.Pathing.NavigationImmediateRayWorkspace,Trailblazer.Pathing.NavigationFlowFieldNode&,Trailblazer.Pathing.NavigationCellAddress&,FixedMathSharp.Vector3d&,FixedMathSharp.Vector3d&)
TryResolveSelectedEdge(Trailblazer.Pathing.NavigationWorldGraph,Trailblazer.Pathing.NavigationNodeRef,Trailblazer.Pathing.NavigationSelectedEdgeRef,Trailblazer.Pathing.GuideSampleWorkMeter&,Trailblazer.Pathing.NavigationGraphEdge&)
TrySampleVolume(GridForge.Grids.GridWorld,Trailblazer.Pathing.NavigationWorldGraphStore,Trailblazer.Pathing.NavigationWorldGraph,Trailblazer.Pathing.NavigationFlowFieldPayload,Trailblazer.Pathing.NavigationCellAddress,Trailblazer.TraversalMedium,FixedMathSharp.Vector3d,Trailblazer.Pathing.GuideSampleWorkMeter&,Trailblazer.Pathing.NavigationImmediateRayWorkspace,Trailblazer.Pathing.NavigationFlowFieldNode&,Trailblazer.Pathing.NavigationCellAddress&,FixedMathSharp.Vector3d&,FixedMathSharp.Vector3d&)
IsWithinArrivalRadius(FixedMathSharp.Vector3d,FixedMathSharp.Vector3d,FixedMathSharp.Fixed64)
TrySampleTransitionApproach(GridForge.Grids.GridWorld,Trailblazer.Pathing.NavigationWorldGraphStore,Trailblazer.Pathing.NavigationWorldGraph,Trailblazer.Pathing.NavigationFlowFieldPayload,Trailblazer.Pathing.NavigationCellAddress,Trailblazer.TraversalMedium,FixedMathSharp.Vector3d,FixedMathSharp.Vector3d,Trailblazer.Pathing.GuideSampleWorkMeter&,Trailblazer.Pathing.NavigationImmediateRayWorkspace,FixedMathSharp.Vector3d&)
RunRay(GridForge.Grids.GridWorld,Trailblazer.Pathing.NavigationWorldGraphStore,Trailblazer.Pathing.NavigationWorldGraph,Trailblazer.Pathing.NavigationFlowFieldPayload,Trailblazer.Pathing.NavigationAreaPolicy,Trailblazer.TraversalMedium,FixedMathSharp.Vector3d,FixedMathSharp.Vector3d,Trailblazer.Pathing.NavigationRayChainConstraint,Trailblazer.Pathing.GuideSampleWorkMeter&,Trailblazer.Pathing.NavigationImmediateRayWorkspace)
TryGetNode(Trailblazer.Pathing.NavigationFlowFieldPayload,Trailblazer.Pathing.NavigationCellAddress,Trailblazer.TraversalMedium,Trailblazer.Pathing.GuideSampleWorkMeter&,Trailblazer.Pathing.NavigationFlowFieldNode&)
TryAdvanceRecovery(GridForge.Grids.GridWorld,Trailblazer.Pathing.NavigationWorldGraphStore,Trailblazer.Pathing.NavigationWorldGraph,Trailblazer.Pathing.NavigationFlowFieldPayload,Trailblazer.TraversalMedium,FixedMathSharp.Vector3d,Trailblazer.Pathing.GuideSampleWorkMeter&,GridForge.Grids.Topology.GridCoveredAddressCursor,GridForge.Grids.Topology.GridCoveredAddressGeneration[],GridForge.Grids.Topology.GridCoveredAddress[],Trailblazer.Pathing.NavigationImmediateRayWorkspace,Trailblazer.Pathing.NavigationCellAddress,Trailblazer.Pathing.NavigationNodeState,Trailblazer.Pathing.NavigationSelectedEdgeRef,FixedMathSharp.Vector3d,Trailblazer.Pathing.NavigationNodeState,System.Boolean,System.Boolean,Trailblazer.Pathing.NavigationCellAddress&,FixedMathSharp.Vector3d&,System.Boolean&)
TryRejoin(GridForge.Grids.GridWorld,Trailblazer.Pathing.NavigationWorldGraphStore,Trailblazer.Pathing.NavigationWorldGraph,Trailblazer.Pathing.NavigationFlowFieldPayload,Trailblazer.TraversalMedium,Trailblazer.Pathing.NavigationCellAddress,Trailblazer.Pathing.NavigationNodeState,Trailblazer.Pathing.NavigationSelectedEdgeRef,FixedMathSharp.Vector3d,Trailblazer.Pathing.NavigationNodeState,System.Boolean,FixedMathSharp.Vector3d,Trailblazer.Pathing.GuideSampleWorkMeter&,Trailblazer.Pathing.NavigationImmediateRayWorkspace,FixedMathSharp.Vector3d&)
TryResolvePortal(Trailblazer.Pathing.NavigationGraphEdge,Trailblazer.Pathing.NavigationNodeState,Trailblazer.Pathing.KinematicBodyShape,Trailblazer.Pathing.GuideSampleWorkMeter&,GridForge.Grids.Topology.GridNavigationPortal&,FixedMathSharp.Vector3d&,FixedMathSharp.Vector3d&)
TrySampleDirectedLeg(FixedMathSharp.Vector3d,FixedMathSharp.Vector3d,FixedMathSharp.Vector3d,FixedMathSharp.Vector3d,FixedMathSharp.Vector3d,Trailblazer.Pathing.GuideSampleWorkMeter&,FixedMathSharp.Vector3d&)
TrySampleDirectedLegUnchecked(FixedMathSharp.Vector3d,FixedMathSharp.Vector3d,FixedMathSharp.Vector3d,FixedMathSharp.Vector3d,FixedMathSharp.Vector3d,FixedMathSharp.Vector3d&)
FailDirectedLeg(Trailblazer.Pathing.NavigationGuideStatus,FixedMathSharp.Vector3d&)
TrySampleExplicit(Trailblazer.Pathing.NavigationWorldGraph,Trailblazer.Pathing.NavigationExplicitConnectionRecord,Trailblazer.Pathing.NavigationCellAddress,Trailblazer.Pathing.NavigationNodeState,GridForge.Grids.Topology.GridCellPrism,Trailblazer.Pathing.NavigationNodeState,GridForge.Grids.Topology.GridCellPrism,System.Boolean,System.Boolean,FixedMathSharp.Vector3d,Trailblazer.Pathing.KinematicBodyShape,System.Boolean,FixedMathSharp.Fixed64,Trailblazer.Pathing.GuideSampleWorkMeter&,Trailblazer.Pathing.NavigationCellAddress&,FixedMathSharp.Vector3d&)
TryReadExplicitPortal(Trailblazer.Pathing.NavigationPagedSequence`1/Enumerator<GridForge.Grids.Topology.GridNavigationPortal>&,Trailblazer.Pathing.KinematicBodyShape,Trailblazer.Pathing.GuideSampleWorkMeter&,GridForge.Grids.Topology.GridNavigationPortal&,FixedMathSharp.Vector3d&,FixedMathSharp.Vector3d&)
TrySetHeading(FixedMathSharp.Vector3d,FixedMathSharp.Vector3d,Trailblazer.Pathing.GuideSampleWorkMeter&,FixedMathSharp.Vector3d&)
TrySetHeadingUnchecked(FixedMathSharp.Vector3d,FixedMathSharp.Vector3d,FixedMathSharp.Vector3d&)
HasReachedOrPassed(FixedMathSharp.Vector3d,FixedMathSharp.Vector3d,FixedMathSharp.Vector3d,System.Boolean&)
TryRebase(GridForge.Grids.GridWorld,Trailblazer.Pathing.NavigationWorldGraph,Trailblazer.Pathing.NavigationFlowFieldPayload,Trailblazer.TraversalMedium,FixedMathSharp.Vector3d,Trailblazer.Pathing.GuideSampleWorkMeter&,GridForge.Grids.Topology.GridCoveredAddressCursor,GridForge.Grids.Topology.GridCoveredAddressGeneration[],GridForge.Grids.Topology.GridCoveredAddress[],Trailblazer.Pathing.NavigationCellAddress&)
TryResolveRebaseCursorStatus(GridForge.Grids.Topology.GridCoveredAddressCursorStatus,System.Boolean,Trailblazer.Pathing.NavigationCellAddress,Trailblazer.Pathing.NavigationGuideStatus&,Trailblazer.Pathing.NavigationCellAddress&)
ConsiderCandidate(Trailblazer.Pathing.NavigationWorldGraph,Trailblazer.Pathing.NavigationFlowFieldPayload,Trailblazer.TraversalMedium,FixedMathSharp.Vector3d,GridForge.Grids.Topology.GridCoveredAddress,Trailblazer.Pathing.GuideSampleWorkMeter&,System.Boolean&,FixedMathSharp.Fixed64&,Trailblazer.Pathing.NavigationCellAddress&)
ConsiderCandidateAddress(Trailblazer.Pathing.NavigationWorldGraph,Trailblazer.Pathing.NavigationFlowFieldPayload,Trailblazer.TraversalMedium,FixedMathSharp.Vector3d,Trailblazer.Pathing.NavigationCellAddress,Trailblazer.Pathing.GuideSampleWorkMeter&,System.Boolean&,FixedMathSharp.Fixed64&,Trailblazer.Pathing.NavigationCellAddress&)
SelectNearestCandidate(FixedMathSharp.Fixed64,Trailblazer.Pathing.NavigationCellAddress,System.Boolean&,FixedMathSharp.Fixed64&,Trailblazer.Pathing.NavigationCellAddress&)