| | | 1 | | //======================================================================= |
| | | 2 | | // NavigationFlowFieldWork.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; |
| | | 9 | | using FixedMathSharp; |
| | | 10 | | using GridForge.Grids; |
| | | 11 | | |
| | | 12 | | namespace Trailblazer.Pathing; |
| | | 13 | | |
| | | 14 | | /// <summary>Builds one bounded weighted destination-centric flow-field prefix.</summary> |
| | | 15 | | internal sealed class NavigationFlowFieldWork : IDisposable |
| | | 16 | | { |
| | | 17 | | private enum Stage : byte |
| | | 18 | | { |
| | | 19 | | Search = 0, |
| | | 20 | | CountTransitions = 1, |
| | | 21 | | ReplayTransitions = 2, |
| | | 22 | | SortDependencies = 3, |
| | | 23 | | CaptureDependencies = 4, |
| | | 24 | | BuildPayload = 5, |
| | | 25 | | SortPayloadNodes = 6, |
| | | 26 | | SortPayloadLookup = 7 |
| | | 27 | | } |
| | | 28 | | |
| | | 29 | | private NavigationResolvedPathQuery? _query; |
| | | 30 | | private GridWorld? _world; |
| | | 31 | | private NavigationWorldGraph? _graph; |
| | | 32 | | private readonly NavigationFlowFieldWorkspace _workspace; |
| | | 33 | | private readonly NavigationWorkMeter _meter; |
| | | 34 | | private readonly long _maximumPayloadBytes; |
| | | 35 | | private NavigationFlowFieldOpenHeap _heap; |
| | | 36 | | private NavigationIncomingTraversalEdgeEnumerator _incoming; |
| | | 37 | | private NavigationTraversalEdgeEnumerator _replayEdges; |
| | | 38 | | private NavigationDependencySortWork _dependencySort; |
| | | 39 | | private NavigationFlowPayloadSortWork _payloadSort; |
| | | 40 | | private NavigationDependencyStampWork? _dependencyStamp; |
| | | 41 | | private NavigationFlowFieldNode[]? _payloadNodes; |
| | | 42 | | private NavigationTransitionInstruction[]? _payloadTransitionInstructions; |
| | | 43 | | private int[]? _payloadLookup; |
| | | 44 | | private NavigationFlowFieldStatus _resultStatus; |
| | | 45 | | private Stage _stage; |
| | | 46 | | private int _currentSlot; |
| | | 47 | | private int _postNodeOrdinal; |
| | | 48 | | private int _transitionOrdinal; |
| | | 49 | | private readonly ulong _worldChangeSequence; |
| | | 50 | | private bool _hasCurrent; |
| | | 51 | | private bool _replayActive; |
| | | 52 | | private bool _originSettled; |
| | | 53 | | private bool _isComplete; |
| | | 54 | | private bool _requiresWorldStamp; |
| | | 55 | | private Fixed64 _coverageThreshold; |
| | | 56 | | |
| | | 57 | | internal NavigationFlowFieldWork( |
| | | 58 | | GridWorld world, |
| | | 59 | | NavigationResolvedPathQuery query, |
| | | 60 | | NavigationFlowFieldWorkspace workspace, |
| | | 61 | | long maximumPayloadBytes = long.MaxValue) |
| | | 62 | | { |
| | | 63 | | SwiftThrowHelper.ThrowIfNull(world, nameof(world)); |
| | | 64 | | SwiftThrowHelper.ThrowIfNull(query, nameof(query)); |
| | | 65 | | SwiftThrowHelper.ThrowIfNull(workspace, nameof(workspace)); |
| | | 66 | | |
| | | 67 | | _query = query; |
| | | 68 | | _world = world; |
| | | 69 | | _graph = query.Graph; |
| | | 70 | | _workspace = workspace; |
| | | 71 | | _workspace.ResetSearch(); |
| | | 72 | | _meter = query.Meter; |
| | | 73 | | _maximumPayloadBytes = maximumPayloadBytes; |
| | | 74 | | _worldChangeSequence = query.WorldChangeSequence; |
| | | 75 | | _requiresWorldStamp = query.RequiresWorldStamp |
| | | 76 | | || query.StartMedium == TraversalMedium.Gas |
| | | 77 | | || query.StartMedium == TraversalMedium.Liquid; |
| | | 78 | | _heap = new NavigationFlowFieldOpenHeap(workspace); |
| | | 79 | | _resultStatus = NavigationFlowFieldStatus.Success; |
| | | 80 | | |
| | | 81 | | if (!TryRecordPage(query.Start.Node) |
| | | 82 | | || !TryRecordPage(query.End.Node)) |
| | | 83 | | { |
| | | 84 | | Finish(NavigationFlowFieldStatus.CapacityExceeded); |
| | | 85 | | return; |
| | | 86 | | } |
| | | 87 | | if (!query.Query.AllowTransitions |
| | | 88 | | && !_graph.AreInSameSurfaceComponent( |
| | | 89 | | query.Start.Address, |
| | | 90 | | query.StartMedium, |
| | | 91 | | query.End.Address, |
| | | 92 | | query.StartMedium)) |
| | | 93 | | { |
| | | 94 | | Finish(NavigationFlowFieldStatus.NoPath); |
| | | 95 | | return; |
| | | 96 | | } |
| | | 97 | | |
| | | 98 | | if (!TrySeedTargetMedium(TraversalMedium.Solid) |
| | | 99 | | || !TrySeedTargetMedium(TraversalMedium.Gas) |
| | | 100 | | || !TrySeedTargetMedium(TraversalMedium.Liquid)) |
| | | 101 | | { |
| | | 102 | | Finish(NavigationFlowFieldStatus.CapacityExceeded); |
| | | 103 | | return; |
| | | 104 | | } |
| | | 105 | | } |
| | | 106 | | |
| | | 107 | | internal NavigationFlowFieldStatus Status { get; private set; } |
| | | 108 | | |
| | | 109 | | internal NavigationFlowFieldPayload? Result { get; private set; } |
| | | 110 | | |
| | | 111 | | internal NavigationFlowFieldStatus Advance( |
| | | 112 | | int lookupStepLimit, |
| | | 113 | | int nodeStepLimit, |
| | | 114 | | int edgeStepLimit, |
| | | 115 | | int connectionStepLimit) |
| | | 116 | | { |
| | | 117 | | SwiftThrowHelper.ThrowIfNegative(lookupStepLimit, nameof(lookupStepLimit)); |
| | | 118 | | SwiftThrowHelper.ThrowIfNegative(nodeStepLimit, nameof(nodeStepLimit)); |
| | | 119 | | SwiftThrowHelper.ThrowIfNegative(edgeStepLimit, nameof(edgeStepLimit)); |
| | | 120 | | SwiftThrowHelper.ThrowIfNegative(connectionStepLimit, nameof(connectionStepLimit)); |
| | | 121 | | if (Status != NavigationFlowFieldStatus.Pending) |
| | | 122 | | return Status; |
| | | 123 | | int lookupRemaining = lookupStepLimit; |
| | | 124 | | int nodeRemaining = nodeStepLimit; |
| | | 125 | | int edgeRemaining = edgeStepLimit; |
| | | 126 | | int connectionRemaining = connectionStepLimit; |
| | | 127 | | |
| | | 128 | | while (true) |
| | | 129 | | { |
| | | 130 | | if (!IsWorldCurrent()) |
| | | 131 | | return Finish(NavigationFlowFieldStatus.Stale); |
| | | 132 | | if (_stage == Stage.Search) |
| | | 133 | | { |
| | | 134 | | if (!_hasCurrent) |
| | | 135 | | { |
| | | 136 | | if (!TryBeginCurrent(ref nodeRemaining)) |
| | | 137 | | return Status; |
| | | 138 | | if (_stage != Stage.Search) |
| | | 139 | | continue; |
| | | 140 | | } |
| | | 141 | | NavigationTraversalEdgeAdvanceStatus edgeStatus = |
| | | 142 | | _incoming.AdvanceOne( |
| | | 143 | | _meter, |
| | | 144 | | _workspace.EndpointWorkspace.Dependencies, |
| | | 145 | | ref edgeRemaining, |
| | | 146 | | ref connectionRemaining); |
| | | 147 | | CaptureWorldDependency(_incoming.RequiresWorldStamp); |
| | | 148 | | edgeStatus = ResolvePostEnumerationStatus( |
| | | 149 | | IsWorldCurrent(), |
| | | 150 | | edgeStatus); |
| | | 151 | | if (edgeStatus == NavigationTraversalEdgeAdvanceStatus.Pending) |
| | | 152 | | continue; |
| | | 153 | | if (edgeStatus == NavigationTraversalEdgeAdvanceStatus.Complete) |
| | | 154 | | { |
| | | 155 | | _hasCurrent = false; |
| | | 156 | | continue; |
| | | 157 | | } |
| | | 158 | | if (edgeStatus == NavigationTraversalEdgeAdvanceStatus.Blocked) |
| | | 159 | | return Status; |
| | | 160 | | if (edgeStatus != NavigationTraversalEdgeAdvanceStatus.Edge) |
| | | 161 | | return Finish(NavigationGuideStatusMapper.ToFlowField(edgeStatus)); |
| | | 162 | | NavigationFlowFieldStatus applied = ApplyIncoming(); |
| | | 163 | | if (applied != NavigationFlowFieldStatus.Pending) |
| | | 164 | | return applied; |
| | | 165 | | continue; |
| | | 166 | | } |
| | | 167 | | |
| | | 168 | | if (_stage == Stage.CountTransitions) |
| | | 169 | | { |
| | | 170 | | while (_postNodeOrdinal < _workspace.SettledCount |
| | | 171 | | && nodeRemaining > 0) |
| | | 172 | | { |
| | | 173 | | int slot = _workspace.SettledSlots[_postNodeOrdinal++]; |
| | | 174 | | if (_workspace.GetRecord(slot).SelectedIsTransition) |
| | | 175 | | _transitionOrdinal++; |
| | | 176 | | nodeRemaining--; |
| | | 177 | | } |
| | | 178 | | if (_postNodeOrdinal < _workspace.SettledCount) |
| | | 179 | | return Status; |
| | | 180 | | _payloadTransitionInstructions = _transitionOrdinal == 0 |
| | | 181 | | ? Array.Empty<NavigationTransitionInstruction>() |
| | | 182 | | : new NavigationTransitionInstruction[_transitionOrdinal]; |
| | | 183 | | _postNodeOrdinal = 0; |
| | | 184 | | _transitionOrdinal = 0; |
| | | 185 | | _stage = Stage.ReplayTransitions; |
| | | 186 | | continue; |
| | | 187 | | } |
| | | 188 | | |
| | | 189 | | if (_stage == Stage.ReplayTransitions) |
| | | 190 | | { |
| | | 191 | | NavigationFlowFieldStatus replay = AdvanceTransitionReplay( |
| | | 192 | | ref nodeRemaining, |
| | | 193 | | ref edgeRemaining, |
| | | 194 | | ref connectionRemaining); |
| | | 195 | | if (replay != NavigationFlowFieldStatus.Pending) |
| | | 196 | | return replay; |
| | | 197 | | if (_stage == Stage.ReplayTransitions) |
| | | 198 | | return Status; |
| | | 199 | | continue; |
| | | 200 | | } |
| | | 201 | | |
| | | 202 | | if (_stage == Stage.SortDependencies) |
| | | 203 | | { |
| | | 204 | | int lookupBefore = _meter.LookupProbes; |
| | | 205 | | bool complete = _dependencySort.Advance(_meter, lookupRemaining); |
| | | 206 | | lookupRemaining -= _meter.LookupProbes - lookupBefore; |
| | | 207 | | if (!complete) |
| | | 208 | | { |
| | | 209 | | return _meter.RemainingLookupProbes == 0 |
| | | 210 | | ? Finish(NavigationFlowFieldStatus.BudgetExceeded) |
| | | 211 | | : Status; |
| | | 212 | | } |
| | | 213 | | _stage = Stage.CaptureDependencies; |
| | | 214 | | continue; |
| | | 215 | | } |
| | | 216 | | |
| | | 217 | | if (_stage == Stage.CaptureDependencies) |
| | | 218 | | { |
| | | 219 | | _dependencyStamp ??= new NavigationDependencyStampWork( |
| | | 220 | | _graph!, |
| | | 221 | | _query!.AreaPolicy, |
| | | 222 | | _workspace.DependencyComponents, |
| | | 223 | | _workspace.DependencyComponentCount, |
| | | 224 | | _workspace.DependencyPages, |
| | | 225 | | _workspace.DependencyPageCount, |
| | | 226 | | _workspace.EndpointWorkspace.Dependencies |
| | | 227 | | .HasTransitionDependency); |
| | | 228 | | int lookupBefore = _meter.LookupProbes; |
| | | 229 | | bool complete = _dependencyStamp.Advance(_meter, lookupRemaining); |
| | | 230 | | lookupRemaining -= _meter.LookupProbes - lookupBefore; |
| | | 231 | | if (!complete) |
| | | 232 | | { |
| | | 233 | | return _meter.RemainingLookupProbes == 0 |
| | | 234 | | ? Finish(NavigationFlowFieldStatus.BudgetExceeded) |
| | | 235 | | : Status; |
| | | 236 | | } |
| | | 237 | | _payloadNodes = new NavigationFlowFieldNode[_workspace.SettledCount]; |
| | | 238 | | _payloadLookup = new int[_workspace.SettledCount]; |
| | | 239 | | _stage = Stage.BuildPayload; |
| | | 240 | | continue; |
| | | 241 | | } |
| | | 242 | | |
| | | 243 | | if (_stage == Stage.BuildPayload) |
| | | 244 | | { |
| | | 245 | | while (_postNodeOrdinal < _workspace.SettledCount && nodeRemaining > 0) |
| | | 246 | | { |
| | | 247 | | int slot = _workspace.SettledSlots[_postNodeOrdinal]; |
| | | 248 | | ref NavigationFlowFieldSearchNode record = |
| | | 249 | | ref _workspace.GetRecord(slot); |
| | | 250 | | _payloadNodes![_postNodeOrdinal] = new NavigationFlowFieldNode( |
| | | 251 | | record.Address, |
| | | 252 | | _workspace.GetNode(slot).Medium, |
| | | 253 | | record.IntegrationCost, |
| | | 254 | | record.SelectedEdge, |
| | | 255 | | record.SelectedIsTransition |
| | | 256 | | ? _transitionOrdinal++ |
| | | 257 | | : -1); |
| | | 258 | | _payloadLookup![_postNodeOrdinal] = _postNodeOrdinal; |
| | | 259 | | _postNodeOrdinal++; |
| | | 260 | | nodeRemaining--; |
| | | 261 | | } |
| | | 262 | | if (_postNodeOrdinal < _workspace.SettledCount) |
| | | 263 | | return Status; |
| | | 264 | | _payloadSort = new NavigationFlowPayloadSortWork( |
| | | 265 | | _payloadNodes!, |
| | | 266 | | _payloadLookup!, |
| | | 267 | | sortNodes: true); |
| | | 268 | | _stage = Stage.SortPayloadNodes; |
| | | 269 | | continue; |
| | | 270 | | } |
| | | 271 | | |
| | | 272 | | if (_stage == Stage.SortPayloadNodes) |
| | | 273 | | { |
| | | 274 | | int lookupBefore = _meter.LookupProbes; |
| | | 275 | | bool complete = _payloadSort.Advance(_meter, lookupRemaining); |
| | | 276 | | lookupRemaining -= _meter.LookupProbes - lookupBefore; |
| | | 277 | | if (!complete) |
| | | 278 | | { |
| | | 279 | | return _meter.RemainingLookupProbes == 0 |
| | | 280 | | ? Finish(NavigationFlowFieldStatus.BudgetExceeded) |
| | | 281 | | : Status; |
| | | 282 | | } |
| | | 283 | | _payloadSort = new NavigationFlowPayloadSortWork( |
| | | 284 | | _payloadNodes!, |
| | | 285 | | _payloadLookup!, |
| | | 286 | | sortNodes: false); |
| | | 287 | | _stage = Stage.SortPayloadLookup; |
| | | 288 | | continue; |
| | | 289 | | } |
| | | 290 | | |
| | | 291 | | int finalLookupBefore = _meter.LookupProbes; |
| | | 292 | | bool lookupComplete = _payloadSort.Advance(_meter, lookupRemaining); |
| | | 293 | | lookupRemaining -= _meter.LookupProbes - finalLookupBefore; |
| | | 294 | | if (!lookupComplete) |
| | | 295 | | { |
| | | 296 | | return _meter.RemainingLookupProbes == 0 |
| | | 297 | | ? Finish(NavigationFlowFieldStatus.BudgetExceeded) |
| | | 298 | | : Status; |
| | | 299 | | } |
| | | 300 | | NavigationResolvedPathQuery resolved = _query!; |
| | | 301 | | Result = new NavigationFlowFieldPayload( |
| | | 302 | | new NavigationFlowFieldPayloadKey( |
| | | 303 | | resolved.Query, |
| | | 304 | | resolved.End.Address, |
| | | 305 | | resolved.StartMedium, |
| | | 306 | | resolved.TargetMedia), |
| | | 307 | | _payloadNodes!, |
| | | 308 | | _payloadLookup!, |
| | | 309 | | _payloadTransitionInstructions!, |
| | | 310 | | _dependencyStamp!.Result, |
| | | 311 | | _isComplete, |
| | | 312 | | _requiresWorldStamp ? _worldChangeSequence : null); |
| | | 313 | | NavigationFlowFieldPayload? result = Result; |
| | | 314 | | NavigationFlowFieldStatus resultStatus = ResolveFinalPublication( |
| | | 315 | | IsWorldCurrent(), |
| | | 316 | | _resultStatus, |
| | | 317 | | ref result); |
| | | 318 | | Result = result; |
| | | 319 | | return Finish(resultStatus); |
| | | 320 | | } |
| | | 321 | | } |
| | | 322 | | |
| | | 323 | | public void Dispose() |
| | | 324 | | { |
| | | 325 | | NavigationResolvedPathQuery? query = _query; |
| | | 326 | | _query = null; |
| | | 327 | | query?.Dispose(); |
| | | 328 | | ReleaseRuntimeState(); |
| | | 329 | | } |
| | | 330 | | |
| | | 331 | | private void ReleaseRuntimeState() |
| | | 332 | | { |
| | | 333 | | _graph = null; |
| | | 334 | | _world = null; |
| | | 335 | | _incoming = default; |
| | | 336 | | _replayEdges = default; |
| | | 337 | | _dependencySort = default; |
| | | 338 | | _dependencyStamp = null; |
| | | 339 | | _payloadNodes = null; |
| | | 340 | | _payloadTransitionInstructions = null; |
| | | 341 | | _payloadLookup = null; |
| | | 342 | | _payloadSort = default; |
| | | 343 | | } |
| | | 344 | | |
| | | 345 | | private bool TryBeginCurrent(ref int nodeRemaining) |
| | | 346 | | { |
| | | 347 | | if (!_heap.TryPeek(out int nextSlot)) |
| | | 348 | | { |
| | | 349 | | _isComplete = true; |
| | | 350 | | _resultStatus = _originSettled |
| | | 351 | | ? NavigationFlowFieldStatus.Success |
| | | 352 | | : NavigationFlowFieldStatus.NoPath; |
| | | 353 | | return TryBeginTransitionCount(); |
| | | 354 | | } |
| | | 355 | | if (_originSettled |
| | | 356 | | && _workspace.GetRecord(nextSlot).IntegrationCost > _coverageThreshold) |
| | | 357 | | { |
| | | 358 | | _isComplete = false; |
| | | 359 | | _resultStatus = NavigationFlowFieldStatus.Success; |
| | | 360 | | return TryBeginTransitionCount(); |
| | | 361 | | } |
| | | 362 | | if (nodeRemaining == 0) |
| | | 363 | | { |
| | | 364 | | if (_meter.RemainingExpandedNodes == 0) |
| | | 365 | | Finish(NavigationFlowFieldStatus.BudgetExceeded); |
| | | 366 | | return false; |
| | | 367 | | } |
| | | 368 | | if (!_meter.TryConsumeExpandedNodes(1)) |
| | | 369 | | { |
| | | 370 | | Finish(NavigationFlowFieldStatus.BudgetExceeded); |
| | | 371 | | return false; |
| | | 372 | | } |
| | | 373 | | nodeRemaining--; |
| | | 374 | | _currentSlot = _heap.Pop(); |
| | | 375 | | ref NavigationFlowFieldSearchNode current = |
| | | 376 | | ref _workspace.GetRecord(_currentSlot); |
| | | 377 | | current.Closed = true; |
| | | 378 | | _workspace.SettledSlots[_workspace.SettledCount++] = _currentSlot; |
| | | 379 | | NavigationMediumStateRef currentState = _workspace.GetNode(_currentSlot); |
| | | 380 | | if (currentState.Equals(new NavigationMediumStateRef( |
| | | 381 | | _query!.Start.Node, |
| | | 382 | | _query.StartMedium))) |
| | | 383 | | { |
| | | 384 | | if (!Fixed64.TryAdd( |
| | | 385 | | current.IntegrationCost, |
| | | 386 | | _query.Query.FlowField.ExtraIntegrationCost, |
| | | 387 | | out _coverageThreshold)) |
| | | 388 | | { |
| | | 389 | | Finish(NavigationFlowFieldStatus.CostOverflow); |
| | | 390 | | return false; |
| | | 391 | | } |
| | | 392 | | _originSettled = true; |
| | | 393 | | } |
| | | 394 | | _incoming = new NavigationIncomingTraversalEdgeEnumerator( |
| | | 395 | | _world!, |
| | | 396 | | _graph!, |
| | | 397 | | currentState, |
| | | 398 | | _query.Query.Agent, |
| | | 399 | | _query.AreaPolicy, |
| | | 400 | | _workspace.RayWorkspace, |
| | | 401 | | _query.Query.AllowTransitions); |
| | | 402 | | _hasCurrent = true; |
| | | 403 | | return true; |
| | | 404 | | } |
| | | 405 | | |
| | | 406 | | private NavigationFlowFieldStatus ApplyIncoming() |
| | | 407 | | { |
| | | 408 | | NavigationMediumStateRef predecessorState = _incoming.CurrentPredecessor; |
| | | 409 | | if (!TryRecordStateDependencies(predecessorState)) |
| | | 410 | | return Finish(NavigationFlowFieldStatus.CapacityExceeded); |
| | | 411 | | ref NavigationFlowFieldSearchNode current = |
| | | 412 | | ref _workspace.GetRecord(_currentSlot); |
| | | 413 | | if (!Fixed64.TryAdd( |
| | | 414 | | current.IntegrationCost, |
| | | 415 | | _incoming.CurrentCost, |
| | | 416 | | out Fixed64 candidate)) |
| | | 417 | | { |
| | | 418 | | return Finish(NavigationFlowFieldStatus.CostOverflow); |
| | | 419 | | } |
| | | 420 | | if (!_workspace.TryGetOrAdd( |
| | | 421 | | predecessorState, |
| | | 422 | | out int predecessorSlot, |
| | | 423 | | out bool added)) |
| | | 424 | | { |
| | | 425 | | return Finish(NavigationFlowFieldStatus.CapacityExceeded); |
| | | 426 | | } |
| | | 427 | | ref NavigationFlowFieldSearchNode predecessor = |
| | | 428 | | ref _workspace.GetRecord(predecessorSlot); |
| | | 429 | | if (added) |
| | | 430 | | { |
| | | 431 | | _graph!.TryGetNodeAddress(predecessorState.Node, out predecessor.Address); |
| | | 432 | | predecessor.IntegrationCost = candidate; |
| | | 433 | | SetSelectedEdge(ref predecessor, current.Address, currentState: _workspace.GetNode(_currentSlot)); |
| | | 434 | | predecessor.HeapIndex = -1; |
| | | 435 | | _heap.Push(predecessorSlot); |
| | | 436 | | return Status; |
| | | 437 | | } |
| | | 438 | | if (predecessor.Closed) |
| | | 439 | | return Status; |
| | | 440 | | if (candidate > predecessor.IntegrationCost) |
| | | 441 | | return Status; |
| | | 442 | | if (candidate == predecessor.IntegrationCost) |
| | | 443 | | { |
| | | 444 | | if (predecessor.SelectedEdge.IsValid |
| | | 445 | | && CompareSelectedEdge( |
| | | 446 | | CreateSelectedEdge(current.Address, _workspace.GetNode(_currentSlot)), |
| | | 447 | | predecessor.SelectedEdge) < 0) |
| | | 448 | | { |
| | | 449 | | SetSelectedEdge(ref predecessor, current.Address, _workspace.GetNode(_currentSlot)); |
| | | 450 | | } |
| | | 451 | | return Status; |
| | | 452 | | } |
| | | 453 | | predecessor.IntegrationCost = candidate; |
| | | 454 | | SetSelectedEdge(ref predecessor, current.Address, _workspace.GetNode(_currentSlot)); |
| | | 455 | | _heap.DecreaseKey(predecessorSlot); |
| | | 456 | | return Status; |
| | | 457 | | } |
| | | 458 | | |
| | | 459 | | private NavigationFlowFieldStatus AdvanceTransitionReplay( |
| | | 460 | | ref int nodeRemaining, |
| | | 461 | | ref int edgeRemaining, |
| | | 462 | | ref int connectionRemaining) |
| | | 463 | | { |
| | | 464 | | while (_postNodeOrdinal < _workspace.SettledCount) |
| | | 465 | | { |
| | | 466 | | int slot = _workspace.SettledSlots[_postNodeOrdinal]; |
| | | 467 | | ref NavigationFlowFieldSearchNode record = ref _workspace.GetRecord(slot); |
| | | 468 | | if (!record.SelectedIsTransition) |
| | | 469 | | { |
| | | 470 | | if (nodeRemaining == 0) |
| | | 471 | | return Status; |
| | | 472 | | nodeRemaining--; |
| | | 473 | | _postNodeOrdinal++; |
| | | 474 | | continue; |
| | | 475 | | } |
| | | 476 | | NavigationMediumStateRef source = _workspace.GetNode(slot); |
| | | 477 | | if (!_replayActive) |
| | | 478 | | { |
| | | 479 | | if (nodeRemaining == 0) |
| | | 480 | | return Status; |
| | | 481 | | nodeRemaining--; |
| | | 482 | | _replayEdges = new NavigationTraversalEdgeEnumerator( |
| | | 483 | | _world!, |
| | | 484 | | _graph!, |
| | | 485 | | source, |
| | | 486 | | _query!.Query.Agent, |
| | | 487 | | _query.AreaPolicy, |
| | | 488 | | _workspace.RayWorkspace, |
| | | 489 | | allowTransitions: true, |
| | | 490 | | emittedSurfaceOrdinal: -1); |
| | | 491 | | _replayActive = true; |
| | | 492 | | } |
| | | 493 | | NavigationTraversalEdgeAdvanceStatus status = _replayEdges.AdvanceOne( |
| | | 494 | | _meter, |
| | | 495 | | _workspace.EndpointWorkspace.Dependencies, |
| | | 496 | | ref edgeRemaining, |
| | | 497 | | ref connectionRemaining); |
| | | 498 | | CaptureWorldDependency(_replayEdges.RequiresWorldStamp); |
| | | 499 | | status = ResolvePostEnumerationStatus(IsWorldCurrent(), status); |
| | | 500 | | if (status == NavigationTraversalEdgeAdvanceStatus.Pending) |
| | | 501 | | continue; |
| | | 502 | | if (status == NavigationTraversalEdgeAdvanceStatus.Blocked) |
| | | 503 | | return Status; |
| | | 504 | | if (status != NavigationTraversalEdgeAdvanceStatus.Edge) |
| | | 505 | | return Finish(NavigationGuideStatusMapper.ToFlowField(status)); |
| | | 506 | | int selectedOrdinal = record.SelectedEdge.CanonicalOutgoingOrdinal; |
| | | 507 | | if (_replayEdges.CurrentOrdinal < selectedOrdinal) |
| | | 508 | | continue; |
| | | 509 | | NavigationMediumStateRef target = _replayEdges.CurrentTarget; |
| | | 510 | | _payloadTransitionInstructions![_transitionOrdinal++] = |
| | | 511 | | new NavigationTransitionInstruction( |
| | | 512 | | _replayEdges.CurrentTransitionIdentityKind, |
| | | 513 | | _replayEdges.CurrentTransitionOwnerMapId, |
| | | 514 | | _replayEdges.CurrentTransitionId, |
| | | 515 | | _replayEdges.CurrentTransitionType, |
| | | 516 | | record.Address, |
| | | 517 | | record.SelectedEdge.Target, |
| | | 518 | | source.Medium, |
| | | 519 | | target.Medium, |
| | | 520 | | _replayEdges.CurrentTransitionSourceAction, |
| | | 521 | | _replayEdges.CurrentTransitionDestinationAction, |
| | | 522 | | _replayEdges.CurrentTransitionHints); |
| | | 523 | | _replayEdges = default; |
| | | 524 | | _replayActive = false; |
| | | 525 | | _postNodeOrdinal++; |
| | | 526 | | } |
| | | 527 | | _postNodeOrdinal = 0; |
| | | 528 | | _transitionOrdinal = 0; |
| | | 529 | | TryBeginDependencySort(); |
| | | 530 | | return Status; |
| | | 531 | | } |
| | | 532 | | |
| | | 533 | | internal static NavigationTraversalEdgeAdvanceStatus ResolvePostEnumerationStatus( |
| | | 534 | | bool worldCurrent, |
| | | 535 | | NavigationTraversalEdgeAdvanceStatus status) => worldCurrent |
| | | 536 | | ? status |
| | | 537 | | : NavigationTraversalEdgeAdvanceStatus.Stale; |
| | | 538 | | |
| | | 539 | | internal static NavigationFlowFieldStatus ResolveFinalPublication( |
| | | 540 | | bool worldCurrent, |
| | | 541 | | NavigationFlowFieldStatus status, |
| | | 542 | | ref NavigationFlowFieldPayload? payload) |
| | | 543 | | { |
| | | 544 | | if (worldCurrent) |
| | | 545 | | return status; |
| | | 546 | | payload = null; |
| | | 547 | | return NavigationFlowFieldStatus.Stale; |
| | | 548 | | } |
| | | 549 | | |
| | | 550 | | private bool TryRecordPage(NavigationNodeRef node) |
| | | 551 | | { |
| | | 552 | | _graph!.TryGetNodeAddress(node, out NavigationCellAddress address); |
| | | 553 | | return _workspace.TryRecordPage( |
| | | 554 | | address.MapId, |
| | | 555 | | node.CellSlot / NavigationSemanticPage.SlotCount); |
| | | 556 | | } |
| | | 557 | | |
| | | 558 | | private bool TryRecordStateDependencies(NavigationMediumStateRef state) |
| | | 559 | | { |
| | | 560 | | _graph!.TryGetNodeAddress(state.Node, out NavigationCellAddress address); |
| | | 561 | | if (!_workspace.TryRecordPage( |
| | | 562 | | address.MapId, |
| | | 563 | | state.Node.CellSlot / NavigationSemanticPage.SlotCount)) |
| | | 564 | | { |
| | | 565 | | return false; |
| | | 566 | | } |
| | | 567 | | _graph.TryGetSurfaceComponent( |
| | | 568 | | address, |
| | | 569 | | state.Medium, |
| | | 570 | | out NavigationSurfaceComponentKey component, |
| | | 571 | | out _); |
| | | 572 | | return _workspace.TryRecordComponent(component); |
| | | 573 | | } |
| | | 574 | | |
| | | 575 | | private NavigationFlowFieldStatus Finish(NavigationFlowFieldStatus status) |
| | | 576 | | { |
| | | 577 | | Status = status; |
| | | 578 | | _query!.ReleaseLease(); |
| | | 579 | | ReleaseRuntimeState(); |
| | | 580 | | return Status; |
| | | 581 | | } |
| | | 582 | | |
| | | 583 | | private bool TryBeginTransitionCount() |
| | | 584 | | { |
| | | 585 | | _stage = Stage.CountTransitions; |
| | | 586 | | return true; |
| | | 587 | | } |
| | | 588 | | |
| | | 589 | | private bool TryBeginDependencySort() |
| | | 590 | | { |
| | | 591 | | long maximumRetainedBytes = |
| | | 592 | | NavigationFlowFieldPayload.GetMaximumRetainedBytes( |
| | | 593 | | _workspace.SettledCount, |
| | | 594 | | _payloadTransitionInstructions!.Length, |
| | | 595 | | _workspace.DependencyComponentCount, |
| | | 596 | | _workspace.DependencyPageCount); |
| | | 597 | | if (maximumRetainedBytes > _maximumPayloadBytes) |
| | | 598 | | { |
| | | 599 | | Finish(NavigationFlowFieldStatus.CapacityExceeded); |
| | | 600 | | return false; |
| | | 601 | | } |
| | | 602 | | _dependencySort = new NavigationDependencySortWork( |
| | | 603 | | _workspace.DependencyComponents, |
| | | 604 | | _workspace.DependencyComponentCount, |
| | | 605 | | _workspace.DependencyPages, |
| | | 606 | | _workspace.DependencyPageCount); |
| | | 607 | | _stage = Stage.SortDependencies; |
| | | 608 | | return true; |
| | | 609 | | } |
| | | 610 | | |
| | | 611 | | private static int CompareSelectedEdge( |
| | | 612 | | NavigationSelectedEdgeRef left, |
| | | 613 | | NavigationSelectedEdgeRef right) => |
| | | 614 | | left.CanonicalOutgoingOrdinal.CompareTo(right.CanonicalOutgoingOrdinal); |
| | | 615 | | |
| | | 616 | | private NavigationSelectedEdgeRef CreateSelectedEdge( |
| | | 617 | | NavigationCellAddress target, |
| | | 618 | | NavigationMediumStateRef targetState) => new( |
| | | 619 | | target, |
| | | 620 | | targetState.Medium, |
| | | 621 | | _incoming.CurrentOrdinal); |
| | | 622 | | |
| | | 623 | | private void SetSelectedEdge( |
| | | 624 | | ref NavigationFlowFieldSearchNode record, |
| | | 625 | | NavigationCellAddress target, |
| | | 626 | | NavigationMediumStateRef currentState) |
| | | 627 | | { |
| | | 628 | | record.SelectedEdge = CreateSelectedEdge(target, currentState); |
| | | 629 | | record.SelectedIsTransition = |
| | | 630 | | _incoming.CurrentKind == NavigationTraversalEdgeKind.Transition; |
| | | 631 | | } |
| | | 632 | | |
| | | 633 | | private bool TrySeedTargetMedium(TraversalMedium medium) |
| | | 634 | | { |
| | | 635 | | if (medium is TraversalMedium.Gas or TraversalMedium.Liquid) |
| | | 636 | | { |
| | | 637 | | if ((_query!.TargetMedia & NavigationCell.ToMedia(medium)) != 0) |
| | | 638 | | _requiresWorldStamp = true; |
| | | 639 | | } |
| | | 640 | | if ((_query!.End.Media & NavigationCell.ToMedia(medium)) == 0) |
| | | 641 | | return true; |
| | | 642 | | _graph!.TryGetSurfaceComponent( |
| | | 643 | | _query.End.Address, |
| | | 644 | | medium, |
| | | 645 | | out NavigationSurfaceComponentKey component, |
| | | 646 | | out _); |
| | | 647 | | var state = new NavigationMediumStateRef(_query.End.Node, medium); |
| | | 648 | | if (!_workspace.TryRecordComponent(component) |
| | | 649 | | || !_workspace.TryGetOrAdd(state, out int slot, out _)) |
| | | 650 | | { |
| | | 651 | | return false; |
| | | 652 | | } |
| | | 653 | | ref NavigationFlowFieldSearchNode destination = |
| | | 654 | | ref _workspace.GetRecord(slot); |
| | | 655 | | destination.Address = _query.End.Address; |
| | | 656 | | destination.IntegrationCost = Fixed64.Zero; |
| | | 657 | | destination.HeapIndex = -1; |
| | | 658 | | _heap.Push(slot); |
| | | 659 | | return true; |
| | | 660 | | } |
| | | 661 | | |
| | | 662 | | private void CaptureWorldDependency(bool requiresWorldStamp) |
| | | 663 | | { |
| | | 664 | | if (requiresWorldStamp) |
| | | 665 | | _requiresWorldStamp = true; |
| | | 666 | | } |
| | | 667 | | |
| | | 668 | | private bool IsWorldCurrent() => |
| | | 669 | | !_requiresWorldStamp |
| | | 670 | | || _world!.ChangeSequence == _worldChangeSequence; |
| | | 671 | | |
| | | 672 | | } |
| | | 673 | | |
| | | 674 | | /// <summary>Canonically heap-sorts flow nodes or lookup ordinals with bounded comparisons.</summary> |
| | | 675 | | internal struct NavigationFlowPayloadSortWork |
| | | 676 | | { |
| | | 677 | | private enum SiftStage : byte |
| | | 678 | | { |
| | | 679 | | None = 0, |
| | | 680 | | ChooseChild = 1, |
| | | 681 | | CompareRoot = 2 |
| | | 682 | | } |
| | | 683 | | |
| | | 684 | | private readonly NavigationFlowFieldNode[] _nodes; |
| | | 685 | | private readonly int[] _lookup; |
| | | 686 | | private readonly bool _sortNodes; |
| | | 687 | | private SiftStage _siftStage; |
| | | 688 | | private int _heapSize; |
| | | 689 | | private int _buildIndex; |
| | | 690 | | private int _sortEnd; |
| | | 691 | | private int _siftRoot; |
| | | 692 | | private int _siftCandidate; |
| | | 693 | | private bool _building; |
| | | 694 | | |
| | | 695 | | internal NavigationFlowPayloadSortWork( |
| | | 696 | | NavigationFlowFieldNode[] nodes, |
| | | 697 | | int[] lookup, |
| | | 698 | | bool sortNodes) |
| | | 699 | | { |
| | 1404 | 700 | | _nodes = nodes; |
| | 1404 | 701 | | _lookup = lookup; |
| | 1404 | 702 | | _sortNodes = sortNodes; |
| | 1404 | 703 | | _siftStage = SiftStage.None; |
| | 1404 | 704 | | _heapSize = nodes.Length; |
| | 1404 | 705 | | _buildIndex = (nodes.Length / 2) - 1; |
| | 1404 | 706 | | _sortEnd = nodes.Length - 1; |
| | 1404 | 707 | | _siftRoot = 0; |
| | 1404 | 708 | | _siftCandidate = 0; |
| | 1404 | 709 | | _building = true; |
| | 1404 | 710 | | } |
| | | 711 | | |
| | | 712 | | internal bool Advance(NavigationWorkMeter meter, int lookupStepLimit) |
| | | 713 | | { |
| | 1446 | 714 | | int remaining = Math.Min(lookupStepLimit, meter.RemainingLookupProbes); |
| | 7947 | 715 | | while (true) |
| | | 716 | | { |
| | 33509 | 717 | | if (_siftStage == SiftStage.None) |
| | | 718 | | { |
| | 9748 | 719 | | if (_building) |
| | | 720 | | { |
| | 3918 | 721 | | if (_buildIndex < 0) |
| | | 722 | | { |
| | 1402 | 723 | | _building = false; |
| | 1402 | 724 | | continue; |
| | | 725 | | } |
| | 2516 | 726 | | BeginSift(_buildIndex--); |
| | 2516 | 727 | | continue; |
| | | 728 | | } |
| | 5830 | 729 | | if (_sortEnd <= 0) |
| | 1401 | 730 | | return true; |
| | 4429 | 731 | | Swap(0, _sortEnd); |
| | 4429 | 732 | | _heapSize = _sortEnd--; |
| | 4429 | 733 | | BeginSift(0); |
| | 4429 | 734 | | continue; |
| | | 735 | | } |
| | 23761 | 736 | | if (_siftStage == SiftStage.ChooseChild) |
| | | 737 | | { |
| | 14910 | 738 | | int left = checked((_siftRoot * 2) + 1); |
| | 14910 | 739 | | if (left >= _heapSize) |
| | | 740 | | { |
| | 6065 | 741 | | _siftStage = SiftStage.None; |
| | 6065 | 742 | | continue; |
| | | 743 | | } |
| | 8845 | 744 | | _siftCandidate = left; |
| | 8845 | 745 | | int right = left + 1; |
| | 8845 | 746 | | if (right < _heapSize) |
| | | 747 | | { |
| | 6265 | 748 | | if (!TryConsumeComparison(meter, ref remaining)) |
| | 18 | 749 | | return false; |
| | 6247 | 750 | | if (Compare(left, right) < 0) |
| | 2837 | 751 | | _siftCandidate = right; |
| | | 752 | | } |
| | 8827 | 753 | | _siftStage = SiftStage.CompareRoot; |
| | 8827 | 754 | | continue; |
| | | 755 | | } |
| | 8851 | 756 | | if (!TryConsumeComparison(meter, ref remaining)) |
| | 27 | 757 | | return false; |
| | 8824 | 758 | | if (Compare(_siftRoot, _siftCandidate) >= 0) |
| | | 759 | | { |
| | 877 | 760 | | _siftStage = SiftStage.None; |
| | 877 | 761 | | continue; |
| | | 762 | | } |
| | 7947 | 763 | | Swap(_siftRoot, _siftCandidate); |
| | 7947 | 764 | | _siftRoot = _siftCandidate; |
| | 7947 | 765 | | _siftStage = SiftStage.ChooseChild; |
| | | 766 | | } |
| | | 767 | | } |
| | | 768 | | |
| | | 769 | | private void BeginSift(int root) |
| | | 770 | | { |
| | 6945 | 771 | | _siftRoot = root; |
| | 6945 | 772 | | _siftStage = SiftStage.ChooseChild; |
| | 6945 | 773 | | } |
| | | 774 | | |
| | | 775 | | private int Compare(int left, int right) |
| | | 776 | | { |
| | 15071 | 777 | | if (!_sortNodes) |
| | | 778 | | { |
| | 7362 | 779 | | NavigationFlowFieldNode lookupLeft = _nodes[_lookup[left]]; |
| | 7362 | 780 | | NavigationFlowFieldNode lookupRight = _nodes[_lookup[right]]; |
| | 7362 | 781 | | int lookupComparison = lookupLeft.Address.CompareTo(lookupRight.Address); |
| | 7362 | 782 | | return lookupComparison != 0 |
| | 7362 | 783 | | ? lookupComparison |
| | 7362 | 784 | | : ((int)lookupLeft.Medium).CompareTo((int)lookupRight.Medium); |
| | | 785 | | } |
| | 7709 | 786 | | NavigationFlowFieldNode leftNode = _nodes[left]; |
| | 7709 | 787 | | NavigationFlowFieldNode rightNode = _nodes[right]; |
| | 7709 | 788 | | int comparison = leftNode.IntegrationCost.CompareTo( |
| | 7709 | 789 | | rightNode.IntegrationCost); |
| | 7709 | 790 | | if (comparison != 0) |
| | 7684 | 791 | | return comparison; |
| | 25 | 792 | | comparison = leftNode.Address.CompareTo(rightNode.Address); |
| | 25 | 793 | | return comparison != 0 |
| | 25 | 794 | | ? comparison |
| | 25 | 795 | | : ((int)leftNode.Medium).CompareTo((int)rightNode.Medium); |
| | | 796 | | } |
| | | 797 | | |
| | | 798 | | private void Swap(int left, int right) |
| | | 799 | | { |
| | 12376 | 800 | | if (_sortNodes) |
| | | 801 | | { |
| | 6405 | 802 | | (_nodes[left], _nodes[right]) = (_nodes[right], _nodes[left]); |
| | 6405 | 803 | | return; |
| | | 804 | | } |
| | 5971 | 805 | | (_lookup[left], _lookup[right]) = (_lookup[right], _lookup[left]); |
| | 5971 | 806 | | } |
| | | 807 | | |
| | | 808 | | private static bool TryConsumeComparison( |
| | | 809 | | NavigationWorkMeter meter, |
| | | 810 | | ref int remaining) |
| | | 811 | | { |
| | 15116 | 812 | | if (remaining == 0 || !meter.TryConsumeLookupProbes(1)) |
| | 45 | 813 | | return false; |
| | 15071 | 814 | | remaining--; |
| | 15071 | 815 | | return true; |
| | | 816 | | } |
| | | 817 | | } |