| | | 1 | | //======================================================================= |
| | | 2 | | // NavigationAStarGuideLease.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 | | using System.Diagnostics; |
| | | 9 | | using FixedMathSharp; |
| | | 10 | | |
| | | 11 | | namespace Trailblazer.Pathing; |
| | | 12 | | |
| | | 13 | | /// <summary>Owns one immutable A* payload and a guide-local waypoint cursor.</summary> |
| | | 14 | | internal sealed class NavigationAStarGuideLease |
| | | 15 | | { |
| | | 16 | | private readonly NavigationAStarPayloadCache _owner; |
| | 74 | 17 | | private readonly object _sync = new(); |
| | | 18 | | private NavigationWorldGraphStore? _store; |
| | | 19 | | private NavigationAStarPayloadLease? _payloadLease; |
| | | 20 | | private NavigationAStarQueryStatus _status; |
| | | 21 | | private int _currentWaypointOrdinal; |
| | | 22 | | private TraversalMedium _currentMedium; |
| | | 23 | | private long _generation; |
| | | 24 | | |
| | 74 | 25 | | internal NavigationAStarGuideLease(NavigationAStarPayloadCache owner) |
| | | 26 | | { |
| | 74 | 27 | | _owner = owner; |
| | 74 | 28 | | } |
| | | 29 | | |
| | | 30 | | internal long Generation |
| | | 31 | | { |
| | 1893 | 32 | | get { lock (_sync) return _generation; } |
| | | 33 | | } |
| | | 34 | | |
| | | 35 | | internal NavigationAStarGuideLease? NextPooled { get; set; } |
| | | 36 | | |
| | | 37 | | internal void Bind( |
| | | 38 | | NavigationWorldGraphStore store, |
| | | 39 | | NavigationAStarPayloadLease payloadLease) |
| | | 40 | | { |
| | 628 | 41 | | lock (_sync) |
| | | 42 | | { |
| | | 43 | | // The cache rents only detached guides and does not return one to its pool |
| | | 44 | | // until TryDetach has cleared this complete ownership tuple. |
| | | 45 | | Debug.Assert(_store == null && _payloadLease == null); |
| | 628 | 46 | | _generation = NavigationGenerationCounter.Advance( |
| | 628 | 47 | | _generation, |
| | 628 | 48 | | "The A* guide lease generation is exhausted."); |
| | 628 | 49 | | _store = store; |
| | 628 | 50 | | _payloadLease = payloadLease; |
| | 628 | 51 | | _currentWaypointOrdinal = 0; |
| | 628 | 52 | | _currentMedium = payloadLease.Payload.Key.StartMedium; |
| | 628 | 53 | | _status = NavigationAStarQueryStatus.Success; |
| | 628 | 54 | | } |
| | 628 | 55 | | } |
| | | 56 | | |
| | | 57 | | internal NavigationAStarQueryStatus GetStatus(long generation) |
| | | 58 | | { |
| | 56 | 59 | | lock (_sync) |
| | | 60 | | { |
| | 56 | 61 | | if (!IsGenerationActiveUnderLock(generation)) |
| | 2 | 62 | | return NavigationAStarQueryStatus.Stale; |
| | 54 | 63 | | return _status == NavigationAStarQueryStatus.Success |
| | 54 | 64 | | ? ValidateCurrentPayloadUnderLock(out _) |
| | 54 | 65 | | : _status; |
| | | 66 | | } |
| | 56 | 67 | | } |
| | | 68 | | |
| | | 69 | | internal int GetCurrentWaypointOrdinal(long generation) |
| | | 70 | | { |
| | 92 | 71 | | lock (_sync) |
| | 92 | 72 | | return IsGenerationActiveUnderLock(generation) ? _currentWaypointOrdinal : -1; |
| | 92 | 73 | | } |
| | | 74 | | |
| | | 75 | | internal int GetWaypointCount(long generation) |
| | | 76 | | { |
| | 259 | 77 | | lock (_sync) |
| | | 78 | | { |
| | 259 | 79 | | return IsGenerationActiveUnderLock(generation) |
| | 259 | 80 | | ? _payloadLease!.Payload.GuidePoints.Length |
| | 259 | 81 | | : 0; |
| | | 82 | | } |
| | 259 | 83 | | } |
| | | 84 | | |
| | | 85 | | internal Fixed64 GetTotalCost(long generation) |
| | | 86 | | { |
| | 37 | 87 | | lock (_sync) |
| | | 88 | | { |
| | 37 | 89 | | return IsGenerationActiveUnderLock(generation) |
| | 37 | 90 | | ? _payloadLease!.Payload.Cost |
| | 37 | 91 | | : Fixed64.Zero; |
| | | 92 | | } |
| | 37 | 93 | | } |
| | | 94 | | |
| | | 95 | | internal NavigationAStarQueryStatus TryAdvanceWaypoint(long generation) |
| | | 96 | | { |
| | 277 | 97 | | lock (_sync) |
| | | 98 | | { |
| | 277 | 99 | | return IsGenerationActiveUnderLock(generation) |
| | 277 | 100 | | ? TryAdvanceWaypointUnderLock() |
| | 277 | 101 | | : NavigationAStarQueryStatus.Stale; |
| | | 102 | | } |
| | 277 | 103 | | } |
| | | 104 | | |
| | | 105 | | internal NavigationAStarQueryStatus TryGetCurrentStep( |
| | | 106 | | long generation, |
| | | 107 | | out NavigationGuideStep step) |
| | | 108 | | { |
| | 3541 | 109 | | lock (_sync) |
| | | 110 | | { |
| | 3541 | 111 | | if (!IsGenerationActiveUnderLock(generation)) |
| | | 112 | | { |
| | 3 | 113 | | step = default; |
| | 3 | 114 | | return NavigationAStarQueryStatus.Stale; |
| | | 115 | | } |
| | 3538 | 116 | | return TryGetCurrentStepUnderLock(out step); |
| | | 117 | | } |
| | 3541 | 118 | | } |
| | | 119 | | |
| | | 120 | | internal NavigationAStarQueryStatus CompletePendingTransition( |
| | | 121 | | long generation, |
| | | 122 | | in NavigationTransitionInstruction instruction) |
| | | 123 | | { |
| | 38 | 124 | | lock (_sync) |
| | | 125 | | { |
| | 38 | 126 | | if (!IsGenerationActiveUnderLock(generation)) |
| | 1 | 127 | | return NavigationAStarQueryStatus.Stale; |
| | 37 | 128 | | NavigationAStarQueryStatus status = ValidateCurrentPayloadUnderLock( |
| | 37 | 129 | | out NavigationAStarPayload payload); |
| | 37 | 130 | | if (status != NavigationAStarQueryStatus.Success) |
| | 5 | 131 | | return status; |
| | 32 | 132 | | NavigationAStarGuidePoint current = |
| | 32 | 133 | | payload.GuidePoints[_currentWaypointOrdinal]; |
| | 32 | 134 | | if (!current.HasTransition |
| | 32 | 135 | | || !instruction.MatchesCompletion( |
| | 32 | 136 | | this, |
| | 32 | 137 | | unchecked((ulong)generation), |
| | 32 | 138 | | _currentWaypointOrdinal)) |
| | | 139 | | { |
| | 5 | 140 | | return NavigationAStarQueryStatus.Stale; |
| | | 141 | | } |
| | 27 | 142 | | _currentMedium = payload |
| | 27 | 143 | | .TransitionInstructions[current.TransitionOrdinal] |
| | 27 | 144 | | .DestinationMedium; |
| | 27 | 145 | | if (_currentWaypointOrdinal + 1 < payload.GuidePoints.Length) |
| | 27 | 146 | | _currentWaypointOrdinal++; |
| | 27 | 147 | | return NavigationAStarQueryStatus.Success; |
| | | 148 | | } |
| | 38 | 149 | | } |
| | | 150 | | |
| | | 151 | | internal bool TryDetach( |
| | | 152 | | long generation, |
| | | 153 | | out NavigationAStarPayloadLease payloadLease) |
| | | 154 | | { |
| | 613 | 155 | | lock (_sync) |
| | | 156 | | { |
| | 613 | 157 | | payloadLease = null!; |
| | 613 | 158 | | if (!IsGenerationActiveUnderLock(generation)) |
| | 5 | 159 | | return false; |
| | 608 | 160 | | _store = null; |
| | 608 | 161 | | payloadLease = _payloadLease!; |
| | 608 | 162 | | _payloadLease = null; |
| | 608 | 163 | | _currentWaypointOrdinal = 0; |
| | 608 | 164 | | _currentMedium = TraversalMedium.Unknown; |
| | 608 | 165 | | _status = NavigationAStarQueryStatus.Pending; |
| | 608 | 166 | | return true; |
| | | 167 | | } |
| | 613 | 168 | | } |
| | | 169 | | |
| | 613 | 170 | | internal void Dispose(long generation) => _owner.ReturnGuide(this, generation); |
| | | 171 | | |
| | | 172 | | private bool IsGenerationActiveUnderLock(long generation) => |
| | 4913 | 173 | | generation > 0 |
| | 4913 | 174 | | && generation == _generation |
| | 4913 | 175 | | && _store != null |
| | 4913 | 176 | | && _payloadLease != null; |
| | | 177 | | |
| | | 178 | | private NavigationAStarQueryStatus TryGetCurrentStepUnderLock( |
| | | 179 | | out NavigationGuideStep step) |
| | | 180 | | { |
| | 3538 | 181 | | step = default; |
| | 3538 | 182 | | NavigationAStarQueryStatus status = ValidateCurrentPayloadUnderLock( |
| | 3538 | 183 | | out NavigationAStarPayload payload); |
| | 3538 | 184 | | if (status != NavigationAStarQueryStatus.Success) |
| | 3 | 185 | | return status; |
| | 3535 | 186 | | NavigationAStarGuidePoint current = payload.GuidePoints[_currentWaypointOrdinal]; |
| | 3535 | 187 | | if (!current.HasTransition) |
| | | 188 | | { |
| | 480 | 189 | | step = new NavigationGuideStep( |
| | 480 | 190 | | current.Address, |
| | 480 | 191 | | current.Position, |
| | 480 | 192 | | current.Medium, |
| | 480 | 193 | | default, |
| | 480 | 194 | | hasTransition: false); |
| | 480 | 195 | | return NavigationAStarQueryStatus.Success; |
| | | 196 | | } |
| | 3055 | 197 | | NavigationTransitionInstruction stamped = payload |
| | 3055 | 198 | | .TransitionInstructions[current.TransitionOrdinal] |
| | 3055 | 199 | | .WithCompletionStamp(this, unchecked((ulong)_generation), _currentWaypointOrdinal); |
| | 3055 | 200 | | step = new NavigationGuideStep( |
| | 3055 | 201 | | current.Address, |
| | 3055 | 202 | | current.Position, |
| | 3055 | 203 | | current.Medium, |
| | 3055 | 204 | | stamped, |
| | 3055 | 205 | | hasTransition: true); |
| | 3055 | 206 | | return NavigationAStarQueryStatus.Success; |
| | | 207 | | } |
| | | 208 | | |
| | | 209 | | private NavigationAStarQueryStatus TryAdvanceWaypointUnderLock() |
| | | 210 | | { |
| | 276 | 211 | | NavigationAStarQueryStatus status = ValidateCurrentPayloadUnderLock( |
| | 276 | 212 | | out NavigationAStarPayload payload); |
| | 276 | 213 | | if (status != NavigationAStarQueryStatus.Success) |
| | 1 | 214 | | return status; |
| | 275 | 215 | | NavigationAStarGuidePoint current = payload.GuidePoints[_currentWaypointOrdinal]; |
| | 275 | 216 | | if (current.HasTransition) |
| | 3 | 217 | | return NavigationAStarQueryStatus.Pending; |
| | 272 | 218 | | if (_currentWaypointOrdinal + 1 < payload.GuidePoints.Length) |
| | 265 | 219 | | _currentWaypointOrdinal++; |
| | 272 | 220 | | return NavigationAStarQueryStatus.Success; |
| | | 221 | | } |
| | | 222 | | |
| | | 223 | | private NavigationAStarQueryStatus ValidateCurrentPayloadUnderLock( |
| | | 224 | | out NavigationAStarPayload payload) |
| | | 225 | | { |
| | 3902 | 226 | | payload = null!; |
| | 3902 | 227 | | if (_status != NavigationAStarQueryStatus.Success) |
| | 3 | 228 | | return _status; |
| | 3899 | 229 | | NavigationWorldGraphStore store = _store!; |
| | 3899 | 230 | | NavigationAStarPayloadLease payloadLease = _payloadLease!; |
| | 3899 | 231 | | NavigationWorldGraphLease? graphLease = store.TryAcquire(); |
| | 3899 | 232 | | if (graphLease == null) |
| | 4 | 233 | | return NavigationAStarQueryStatus.CapacityExceeded; |
| | 3895 | 234 | | using (graphLease) |
| | | 235 | | { |
| | 3895 | 236 | | payload = payloadLease.Payload; |
| | 3895 | 237 | | if (!graphLease.Graph.IsDependencyCurrent(payload.Dependencies) |
| | 3895 | 238 | | || !_owner.IsWorldCurrent(payload) |
| | 3895 | 239 | | || !store.Current.IsDependencyCurrent(payload.Dependencies) |
| | 3895 | 240 | | || !_owner.IsWorldCurrent(payload)) |
| | | 241 | | { |
| | 15 | 242 | | payload = null!; |
| | 15 | 243 | | return MarkStaleUnderLock(); |
| | | 244 | | } |
| | 3880 | 245 | | return NavigationAStarQueryStatus.Success; |
| | | 246 | | } |
| | 3895 | 247 | | } |
| | | 248 | | |
| | | 249 | | private NavigationAStarQueryStatus MarkStaleUnderLock() |
| | | 250 | | { |
| | 15 | 251 | | _status = NavigationAStarQueryStatus.Stale; |
| | 15 | 252 | | return _status; |
| | | 253 | | } |
| | | 254 | | } |