| | | 1 | | //======================================================================= |
| | | 2 | | // NavigationIncomingSurfaceEdgeEnumerator.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 | | |
| | | 8 | | namespace Trailblazer.Pathing; |
| | | 9 | | |
| | | 10 | | /// <summary>Enumerates original forward edges into one destination in canonical order.</summary> |
| | | 11 | | internal struct NavigationIncomingSurfaceEdgeEnumerator |
| | | 12 | | { |
| | | 13 | | private readonly NavigationWorldGraph? _graph; |
| | | 14 | | private readonly NavigationNodeRef _destination; |
| | | 15 | | private readonly NavigationCellAddress _destinationAddress; |
| | | 16 | | private readonly bool _structural; |
| | | 17 | | private NavigationSurfaceEdgeEnumerator _incomingCandidates; |
| | | 18 | | private NavigationSurfaceEdgeEnumerator _outgoingEdges; |
| | | 19 | | private NavigationGraphEdge _incomingCandidate; |
| | | 20 | | private NavigationNodeRef _predecessor; |
| | | 21 | | private bool _hasIncomingCandidate; |
| | | 22 | | |
| | | 23 | | internal NavigationIncomingSurfaceEdgeEnumerator( |
| | | 24 | | NavigationWorldGraph graph, |
| | | 25 | | NavigationNodeRef destination, |
| | | 26 | | bool structural = false) |
| | | 27 | | { |
| | 111167 | 28 | | _destination = destination; |
| | 111167 | 29 | | _outgoingEdges = default; |
| | 111167 | 30 | | _incomingCandidate = default; |
| | 111167 | 31 | | _predecessor = default; |
| | 111167 | 32 | | _hasIncomingCandidate = false; |
| | 111167 | 33 | | _structural = structural; |
| | 111167 | 34 | | Current = default; |
| | 111167 | 35 | | bool found = graph.TryGetNodeAddress(destination, out _destinationAddress); |
| | | 36 | | System.Diagnostics.Debug.Assert(found, |
| | | 37 | | "incoming enumeration receives a destination owned by this immutable graph"); |
| | 111167 | 38 | | if (structural) |
| | | 39 | | { |
| | | 40 | | System.Diagnostics.Debug.Assert(graph.HasEffectiveCell(_destinationAddress), |
| | | 41 | | "structural enumeration receives an effective graph-owned destination"); |
| | | 42 | | } |
| | | 43 | | else |
| | | 44 | | { |
| | 23477 | 45 | | bool hasState = graph.TryGetNodeState( |
| | 23477 | 46 | | destination, |
| | 23477 | 47 | | out NavigationNodeState state); |
| | | 48 | | System.Diagnostics.Debug.Assert(hasState && state.IsPresent, |
| | | 49 | | "runtime incoming enumeration receives a present graph-owned destination"); |
| | | 50 | | } |
| | | 51 | | |
| | 111167 | 52 | | _graph = graph; |
| | 111167 | 53 | | _incomingCandidates = new NavigationSurfaceEdgeEnumerator( |
| | 111167 | 54 | | graph, |
| | 111167 | 55 | | destination, |
| | 111167 | 56 | | incoming: true, |
| | 111167 | 57 | | includeNative: true, |
| | 111167 | 58 | | includeAutomaticSeams: true, |
| | 111167 | 59 | | structural: structural); |
| | 111167 | 60 | | } |
| | | 61 | | |
| | | 62 | | internal NavigationIncomingSurfaceEdge Current { get; private set; } |
| | | 63 | | |
| | | 64 | | internal bool MoveNext() |
| | | 65 | | { |
| | 120020 | 66 | | int unbounded = int.MaxValue; |
| | | 67 | | while (true) |
| | | 68 | | { |
| | 220038 | 69 | | NavigationSurfaceEdgeAdvanceStatus status = AdvanceOne( |
| | 220038 | 70 | | (NavigationWorkMeter?)null, |
| | 220038 | 71 | | ref unbounded); |
| | 220038 | 72 | | if (status == NavigationSurfaceEdgeAdvanceStatus.Edge) |
| | 100016 | 73 | | return true; |
| | 120022 | 74 | | if (status == NavigationSurfaceEdgeAdvanceStatus.Complete) |
| | 20004 | 75 | | return false; |
| | | 76 | | } |
| | | 77 | | } |
| | | 78 | | |
| | | 79 | | internal NavigationSurfaceEdgeAdvanceStatus AdvanceOne( |
| | | 80 | | NavigationWorkMeter? meter, |
| | | 81 | | ref int edgeStepRemaining) |
| | 238862 | 82 | | => AdvanceOneCore(meter, null, ref edgeStepRemaining); |
| | | 83 | | |
| | | 84 | | internal NavigationSurfaceEdgeAdvanceStatus AdvanceOne( |
| | | 85 | | MaintenanceWorkMeter meter, |
| | | 86 | | ref int edgeStepRemaining) |
| | 971735 | 87 | | => AdvanceOneCore(null, meter, ref edgeStepRemaining); |
| | | 88 | | |
| | | 89 | | private NavigationSurfaceEdgeAdvanceStatus AdvanceOneCore( |
| | | 90 | | NavigationWorkMeter? queryMeter, |
| | | 91 | | MaintenanceWorkMeter? maintenanceMeter, |
| | | 92 | | ref int edgeStepRemaining) |
| | | 93 | | { |
| | | 94 | | System.Diagnostics.Debug.Assert(_graph != null, |
| | | 95 | | "incoming enumeration advances only after graph-owned initialization"); |
| | | 96 | | |
| | | 97 | | while (true) |
| | | 98 | | { |
| | 1427447 | 99 | | if (!_hasIncomingCandidate) |
| | | 100 | | { |
| | 724266 | 101 | | NavigationSurfaceEdgeAdvanceStatus incomingStatus = |
| | 724266 | 102 | | queryMeter != null |
| | 724266 | 103 | | ? _incomingCandidates.AdvanceOne(queryMeter, ref edgeStepRemaining) |
| | 724266 | 104 | | : maintenanceMeter != null |
| | 724266 | 105 | | ? _incomingCandidates.AdvanceOne( |
| | 724266 | 106 | | maintenanceMeter, |
| | 724266 | 107 | | ref edgeStepRemaining) |
| | 724266 | 108 | | : _incomingCandidates.AdvanceOne( |
| | 724266 | 109 | | (NavigationWorkMeter?)null, |
| | 724266 | 110 | | ref edgeStepRemaining); |
| | 724266 | 111 | | if (incomingStatus == NavigationSurfaceEdgeAdvanceStatus.Complete) |
| | 111151 | 112 | | Current = default; |
| | 724266 | 113 | | if (incomingStatus != NavigationSurfaceEdgeAdvanceStatus.Edge) |
| | 435527 | 114 | | return incomingStatus; |
| | | 115 | | |
| | 288739 | 116 | | _incomingCandidate = _incomingCandidates.Current; |
| | 288739 | 117 | | _predecessor = _incomingCandidate.Target; |
| | 288739 | 118 | | _outgoingEdges = _structural |
| | 288739 | 119 | | ? _graph.EnumerateStructuralSurfaceEdges(_predecessor) |
| | 288739 | 120 | | : _graph.EnumerateSurfaceEdges(_predecessor); |
| | 288739 | 121 | | _hasIncomingCandidate = true; |
| | | 122 | | } |
| | | 123 | | |
| | 991920 | 124 | | NavigationSurfaceEdgeAdvanceStatus outgoingStatus = |
| | 991920 | 125 | | queryMeter != null |
| | 991920 | 126 | | ? _outgoingEdges.AdvanceOne(queryMeter, ref edgeStepRemaining) |
| | 991920 | 127 | | : maintenanceMeter != null |
| | 991920 | 128 | | ? _outgoingEdges.AdvanceOne( |
| | 991920 | 129 | | maintenanceMeter, |
| | 991920 | 130 | | ref edgeStepRemaining) |
| | 991920 | 131 | | : _outgoingEdges.AdvanceOne( |
| | 991920 | 132 | | (NavigationWorkMeter?)null, |
| | 991920 | 133 | | ref edgeStepRemaining); |
| | 991920 | 134 | | if (outgoingStatus != NavigationSurfaceEdgeAdvanceStatus.Edge) |
| | | 135 | | { |
| | | 136 | | System.Diagnostics.Debug.Assert( |
| | | 137 | | outgoingStatus != NavigationSurfaceEdgeAdvanceStatus.Complete); |
| | 486333 | 138 | | return outgoingStatus; |
| | | 139 | | } |
| | | 140 | | |
| | 505587 | 141 | | NavigationGraphEdge forwardEdge = _outgoingEdges.Current; |
| | 505587 | 142 | | if (!MatchesIncomingCandidate(forwardEdge)) |
| | | 143 | | continue; |
| | | 144 | | |
| | 288737 | 145 | | Current = new NavigationIncomingSurfaceEdge( |
| | 288737 | 146 | | _predecessor, |
| | 288737 | 147 | | forwardEdge, |
| | 288737 | 148 | | new NavigationSelectedEdgeRef( |
| | 288737 | 149 | | _destinationAddress, |
| | 288737 | 150 | | TraversalMedium.Solid, |
| | 288737 | 151 | | _outgoingEdges.CurrentOrdinal)); |
| | 288737 | 152 | | ClearIncomingCandidate(); |
| | 288737 | 153 | | return NavigationSurfaceEdgeAdvanceStatus.Edge; |
| | | 154 | | } |
| | | 155 | | } |
| | | 156 | | |
| | | 157 | | private bool MatchesIncomingCandidate(in NavigationGraphEdge forwardEdge) |
| | | 158 | | { |
| | 505587 | 159 | | if (!forwardEdge.Target.Equals(_destination) |
| | 505587 | 160 | | || forwardEdge.Kind != _incomingCandidate.Kind) |
| | | 161 | | { |
| | 212794 | 162 | | return false; |
| | | 163 | | } |
| | | 164 | | |
| | 292793 | 165 | | return forwardEdge.Kind switch |
| | 292793 | 166 | | { |
| | 32716 | 167 | | NavigationGraphEdgeKind.Explicit => ReferenceEquals( |
| | 32716 | 168 | | forwardEdge.ExplicitConnection, |
| | 32716 | 169 | | _incomingCandidate.ExplicitConnection), |
| | 60788 | 170 | | NavigationGraphEdgeKind.Seam => ReferenceEquals( |
| | 60788 | 171 | | forwardEdge.AutomaticSeam.Pair, |
| | 60788 | 172 | | _incomingCandidate.AutomaticSeam.Pair), |
| | 199289 | 173 | | _ => true |
| | 292793 | 174 | | }; |
| | | 175 | | } |
| | | 176 | | |
| | | 177 | | private void ClearIncomingCandidate() |
| | | 178 | | { |
| | 288737 | 179 | | _outgoingEdges = default; |
| | 288737 | 180 | | _incomingCandidate = default; |
| | 288737 | 181 | | _predecessor = default; |
| | 288737 | 182 | | _hasIncomingCandidate = false; |
| | 288737 | 183 | | } |
| | | 184 | | } |