| | | 1 | | //======================================================================= |
| | | 2 | | // NavigationOverlayFoldWork.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 GridForge.Grids.Topology; |
| | | 10 | | |
| | | 11 | | namespace Trailblazer.Pathing; |
| | | 12 | | |
| | | 13 | | /// <summary>Folds one atomic overlay transaction into persistent map roots across frames.</summary> |
| | | 14 | | internal sealed class NavigationOverlayFoldWork |
| | | 15 | | { |
| | | 16 | | private readonly NavigationOperationCandidate _source; |
| | | 17 | | private readonly NavigationOperationCandidate _working; |
| | | 18 | | private readonly NavigationOverlayTransaction _transaction; |
| | | 19 | | private readonly long _operationSequence; |
| | | 20 | | private readonly NavigationOperationLimits _limits; |
| | | 21 | | private readonly GridCellPrism[] _corridorPrisms; |
| | | 22 | | private readonly FixedMathSharp.Vector3d[] _corridorWaypoints; |
| | | 23 | | private readonly NavigationCellAddress[] _corridorAddresses; |
| | | 24 | | private readonly NavigationAddressStampSet _corridorAddressSet; |
| | | 25 | | private readonly NavigationOperationCandidate.MapState[] _priorStates; |
| | | 26 | | private readonly NavigationOperationCandidate.MapState[] _nextStates; |
| | | 27 | | private readonly string[] _changedMapIds; |
| | | 28 | | private int _mapIndex; |
| | | 29 | | private int _cellIndex; |
| | | 30 | | private int _connectionIndex; |
| | | 31 | | private int _transitionIndex; |
| | | 32 | | private NavigationOperationCandidate.MapState? _current; |
| | | 33 | | private NavigationMapOverlayState? _overlay; |
| | | 34 | | private PersistentVoxelIndexMap<byte>? _dynamicAddresses; |
| | | 35 | | private int _dependencyMapIndex; |
| | | 36 | | private int _dependencyStage; |
| | | 37 | | private int _dependencyIndex; |
| | 309 | 38 | | private bool _removeDependencies = true; |
| | | 39 | | private int _validationMapIndex; |
| | 309 | 40 | | private int _validationSourceIndex = -1; |
| | | 41 | | private int _validationStage; |
| | | 42 | | private int _validationEdgeIndex; |
| | | 43 | | private Stage _stage; |
| | | 44 | | private NavigationOperationCandidate.ExplicitConnectionRefreshWork? _explicitRefresh; |
| | | 45 | | private long _displacedMapStatePayloadBytes; |
| | | 46 | | private int _displacedMapStatePayloadPages; |
| | | 47 | | |
| | 309 | 48 | | internal NavigationOverlayFoldWork( |
| | 309 | 49 | | NavigationOperationCandidate source, |
| | 309 | 50 | | NavigationOverlayTransaction transaction, |
| | 309 | 51 | | long operationSequence, |
| | 309 | 52 | | NavigationOperationLimits limits, |
| | 309 | 53 | | GridCellPrism[] corridorPrisms, |
| | 309 | 54 | | FixedMathSharp.Vector3d[] corridorWaypoints, |
| | 309 | 55 | | NavigationCellAddress[] corridorAddresses, |
| | 309 | 56 | | NavigationAddressStampSet corridorAddressSet) |
| | | 57 | | { |
| | 309 | 58 | | _source = source; |
| | 309 | 59 | | _working = source.Clone(); |
| | 309 | 60 | | _transaction = transaction; |
| | 309 | 61 | | _operationSequence = operationSequence; |
| | 309 | 62 | | _limits = limits; |
| | 309 | 63 | | _corridorPrisms = corridorPrisms; |
| | 309 | 64 | | _corridorWaypoints = corridorWaypoints; |
| | 309 | 65 | | _corridorAddresses = corridorAddresses; |
| | 309 | 66 | | _corridorAddressSet = corridorAddressSet; |
| | 309 | 67 | | int mapCount = transaction.MapSpan.Length; |
| | 309 | 68 | | _priorStates = new NavigationOperationCandidate.MapState[mapCount]; |
| | 309 | 69 | | _nextStates = new NavigationOperationCandidate.MapState[mapCount]; |
| | 309 | 70 | | _changedMapIds = new string[mapCount]; |
| | 309 | 71 | | } |
| | | 72 | | |
| | 5882 | 73 | | internal NavigationOperationCandidate Candidate => _working; |
| | | 74 | | |
| | | 75 | | internal long DisplacedExplicitPayloadBytes => |
| | 873 | 76 | | _explicitRefresh?.DisplacedSourcePayloadBytes ?? 0L; |
| | | 77 | | |
| | | 78 | | internal int DisplacedExplicitPayloadPages => |
| | 873 | 79 | | _explicitRefresh?.DisplacedSourcePayloadPages ?? 0; |
| | | 80 | | |
| | 870 | 81 | | internal long DisplacedMapStatePayloadBytes => _displacedMapStatePayloadBytes; |
| | | 82 | | |
| | 870 | 83 | | internal int DisplacedMapStatePayloadPages => _displacedMapStatePayloadPages; |
| | | 84 | | |
| | 870 | 85 | | internal long RetainedBytes => checked( |
| | 870 | 86 | | 160L |
| | 870 | 87 | | + _transaction.EstimatedDescriptorBytes |
| | 870 | 88 | | + ((long)(_priorStates.Length + _nextStates.Length + _changedMapIds.Length) |
| | 870 | 89 | | * IntPtr.Size) |
| | 870 | 90 | | + _working.WorkCopiedPersistentBytes |
| | 870 | 91 | | + (_explicitRefresh?.RetainedBytes ?? 0) |
| | 870 | 92 | | + GetPendingMapStatePayloadBytes()); |
| | | 93 | | |
| | 870 | 94 | | internal int PersistentPageCount => checked( |
| | 870 | 95 | | 1 |
| | 870 | 96 | | + _working.WorkCopiedPersistentPages |
| | 870 | 97 | | + (_explicitRefresh?.PersistentPageCount ?? 0) |
| | 870 | 98 | | + GetPendingMapStatePayloadPages()); |
| | | 99 | | |
| | | 100 | | internal bool Advance( |
| | | 101 | | MaintenanceWorkMeter meter, |
| | | 102 | | out NavigationOperationRejection rejection) |
| | | 103 | | { |
| | 494 | 104 | | if (_stage == Stage.Fold) |
| | | 105 | | { |
| | 384 | 106 | | ReadOnlySpan<NavigationMapOverlayDelta> maps = _transaction.MapSpan; |
| | 728 | 107 | | while (_mapIndex < maps.Length) |
| | | 108 | | { |
| | 432 | 109 | | NavigationMapOverlayDelta delta = maps[_mapIndex]; |
| | 432 | 110 | | if (_current == null) |
| | | 111 | | { |
| | 357 | 112 | | if (!_source.TryGetState(delta.MapId, out _current) || _current == null) |
| | | 113 | | { |
| | 1 | 114 | | rejection = NavigationOperationRejection.MissingMap; |
| | 1 | 115 | | return true; |
| | | 116 | | } |
| | 356 | 117 | | _overlay = _current.Overlay; |
| | 356 | 118 | | _dynamicAddresses = _current.DynamicAddresses; |
| | | 119 | | } |
| | 431 | 120 | | ReadOnlySpan<NavigationCellOverlayOperation> cells = delta.CellSpan; |
| | 17638 | 121 | | while (_cellIndex < cells.Length) |
| | | 122 | | { |
| | 17287 | 123 | | if (!meter.TryConsumeOverlaySlots(1)) |
| | | 124 | | { |
| | 76 | 125 | | rejection = NavigationOperationRejection.None; |
| | 76 | 126 | | return false; |
| | | 127 | | } |
| | 17211 | 128 | | NavigationCellOverlayOperation change = cells[_cellIndex++]; |
| | 17211 | 129 | | if (!_current.Map.GridBinding.IsValidIndex(change.Index) |
| | 17211 | 130 | | || (change.Kind == NavigationCellOverlayOperationKind.Set |
| | 17211 | 131 | | && change.Cell.Area.Value >= _working.NavigationAreaCount)) |
| | | 132 | | { |
| | 4 | 133 | | rejection = NavigationOperationRejection.ValidationFailed; |
| | 4 | 134 | | return true; |
| | | 135 | | } |
| | 17207 | 136 | | _overlay = _overlay!.Apply(change, _operationSequence); |
| | 17207 | 137 | | _working.RecordPersistentCopies(_overlay.LastApplyCopiedNodeCount); |
| | 17207 | 138 | | if (!_current.Map.ContainsCell(change.Index) |
| | 17207 | 139 | | && (change.Kind == NavigationCellOverlayOperationKind.Set |
| | 17207 | 140 | | || (change.Kind == NavigationCellOverlayOperationKind.Suppress |
| | 17207 | 141 | | && _current.Map.DefaultCell.HasValue))) |
| | | 142 | | { |
| | 17038 | 143 | | _dynamicAddresses = _dynamicAddresses!.Set( |
| | 17038 | 144 | | change.Index, |
| | 17038 | 145 | | 0, |
| | 17038 | 146 | | out int copiedNodes); |
| | 17038 | 147 | | _working.RecordPersistentCopies(copiedNodes); |
| | | 148 | | } |
| | | 149 | | } |
| | 351 | 150 | | ReadOnlySpan<NavigationConnectionOverlayOperation> connections = delta.ConnectionSpan; |
| | 4509 | 151 | | while (_connectionIndex < connections.Length) |
| | | 152 | | { |
| | 4160 | 153 | | if (!meter.TryConsumeOverlaySlots(1)) |
| | | 154 | | { |
| | 1 | 155 | | rejection = NavigationOperationRejection.None; |
| | 1 | 156 | | return false; |
| | | 157 | | } |
| | 4159 | 158 | | NavigationConnectionOverlayOperation change = connections[_connectionIndex++]; |
| | 4159 | 159 | | if (change.Kind == NavigationConnectionOverlayOperationKind.Upsert |
| | 4159 | 160 | | && change.Connection!.Witnesses.Count > _limits.MaxCorridorCells - 2) |
| | | 161 | | { |
| | 1 | 162 | | rejection = NavigationOperationRejection.CapacityExceeded; |
| | 1 | 163 | | return true; |
| | | 164 | | } |
| | 4158 | 165 | | _overlay = _overlay!.Apply(change, _operationSequence); |
| | 4158 | 166 | | _working.RecordPersistentCopies(_overlay.LastApplyCopiedNodeCount); |
| | | 167 | | } |
| | 349 | 168 | | ReadOnlySpan<TraversalTransitionOverlayOperation> transitions = delta.TransitionSpan; |
| | 4466 | 169 | | while (_transitionIndex < transitions.Length) |
| | | 170 | | { |
| | 4118 | 171 | | if (!meter.TryConsumeOverlaySlots(1)) |
| | | 172 | | { |
| | 1 | 173 | | rejection = NavigationOperationRejection.None; |
| | 1 | 174 | | return false; |
| | | 175 | | } |
| | 4117 | 176 | | _overlay = _overlay!.Apply(transitions[_transitionIndex++], _operationSequence); |
| | 4117 | 177 | | _working.RecordPersistentCopies(_overlay.LastApplyCopiedNodeCount); |
| | | 178 | | } |
| | | 179 | | |
| | 348 | 180 | | NavigationOperationCandidate.MapState next = new( |
| | 348 | 181 | | _current.Map, |
| | 348 | 182 | | _current.BakeVersion, |
| | 348 | 183 | | _current.PreparedMapRetainedBytes, |
| | 348 | 184 | | _overlay!, |
| | 348 | 185 | | _current.DynamicSlotGeneration, |
| | 348 | 186 | | _dynamicAddresses!, |
| | 348 | 187 | | _current.BakedCellLookup); |
| | 348 | 188 | | _working.RecordMapStateOwnership( |
| | 348 | 189 | | delta.MapId, |
| | 348 | 190 | | next, |
| | 348 | 191 | | _source, |
| | 348 | 192 | | ref _displacedMapStatePayloadBytes, |
| | 348 | 193 | | ref _displacedMapStatePayloadPages); |
| | 348 | 194 | | rejection = _working.ReplaceOverlayState( |
| | 348 | 195 | | _current, |
| | 348 | 196 | | next, |
| | 348 | 197 | | _limits); |
| | 348 | 198 | | if (rejection != NavigationOperationRejection.None) |
| | 4 | 199 | | return true; |
| | 344 | 200 | | _priorStates[_mapIndex] = _current; |
| | 344 | 201 | | _nextStates[_mapIndex] = next; |
| | 344 | 202 | | _changedMapIds[_mapIndex] = delta.MapId; |
| | 344 | 203 | | _mapIndex++; |
| | 344 | 204 | | _cellIndex = 0; |
| | 344 | 205 | | _connectionIndex = 0; |
| | 344 | 206 | | _transitionIndex = 0; |
| | 344 | 207 | | _current = null; |
| | 344 | 208 | | _overlay = null; |
| | 344 | 209 | | _dynamicAddresses = null; |
| | | 210 | | } |
| | 296 | 211 | | _stage = Stage.Dependencies; |
| | | 212 | | } |
| | | 213 | | |
| | 406 | 214 | | if (_stage == Stage.Dependencies) |
| | | 215 | | { |
| | 983 | 216 | | while (_dependencyMapIndex < _priorStates.Length) |
| | | 217 | | { |
| | 687 | 218 | | NavigationOperationCandidate.MapState state = _removeDependencies |
| | 687 | 219 | | ? _priorStates[_dependencyMapIndex] |
| | 687 | 220 | | : _nextStates[_dependencyMapIndex]; |
| | 687 | 221 | | if (!AdvanceDependencies(state, _removeDependencies, meter)) |
| | | 222 | | { |
| | 1 | 223 | | rejection = NavigationOperationRejection.None; |
| | 1 | 224 | | return false; |
| | | 225 | | } |
| | 686 | 226 | | if (_removeDependencies) |
| | | 227 | | { |
| | 343 | 228 | | _removeDependencies = false; |
| | 343 | 229 | | ResetDependencyCursor(); |
| | 343 | 230 | | continue; |
| | | 231 | | } |
| | 343 | 232 | | _removeDependencies = true; |
| | 343 | 233 | | _dependencyMapIndex++; |
| | 343 | 234 | | ResetDependencyCursor(); |
| | | 235 | | } |
| | 296 | 236 | | _stage = Stage.Validation; |
| | | 237 | | } |
| | | 238 | | |
| | 1097 | 239 | | while (_validationMapIndex < _nextStates.Length) |
| | | 240 | | { |
| | | 241 | | NavigationOperationCandidate.MapState state; |
| | 699 | 242 | | if (_validationSourceIndex < 0) |
| | 346 | 243 | | state = _nextStates[_validationMapIndex]; |
| | | 244 | | else |
| | | 245 | | { |
| | 353 | 246 | | string target = _changedMapIds[_validationMapIndex]; |
| | 353 | 247 | | int sourceCount = _working.GetIncomingSourceCount(target); |
| | 353 | 248 | | if (_validationSourceIndex >= sourceCount) |
| | | 249 | | { |
| | 339 | 250 | | _validationMapIndex++; |
| | 339 | 251 | | _validationSourceIndex = -1; |
| | 339 | 252 | | continue; |
| | | 253 | | } |
| | 14 | 254 | | string sourceId = _working.GetIncomingSource(target, _validationSourceIndex); |
| | 14 | 255 | | bool hasSource = _working.TryGetState( |
| | 14 | 256 | | sourceId, |
| | 14 | 257 | | out NavigationOperationCandidate.MapState? source); |
| | | 258 | | System.Diagnostics.Debug.Assert(hasSource && source != null); |
| | 14 | 259 | | state = source!; |
| | | 260 | | } |
| | 360 | 261 | | if (!AdvanceValidation(state, meter, out bool valid)) |
| | | 262 | | { |
| | 3 | 263 | | rejection = NavigationOperationRejection.None; |
| | 3 | 264 | | return false; |
| | | 265 | | } |
| | 357 | 266 | | if (!valid) |
| | | 267 | | { |
| | 4 | 268 | | rejection = NavigationOperationRejection.ValidationFailed; |
| | 4 | 269 | | return true; |
| | | 270 | | } |
| | 353 | 271 | | if (_validationSourceIndex < 0) |
| | 339 | 272 | | _validationSourceIndex = 0; |
| | | 273 | | else |
| | 14 | 274 | | _validationSourceIndex++; |
| | | 275 | | } |
| | 398 | 276 | | _explicitRefresh ??= _working.BeginExplicitConnectionRefresh( |
| | 398 | 277 | | _transaction, |
| | 398 | 278 | | _source.ExplicitConnections, |
| | 398 | 279 | | _corridorPrisms, |
| | 398 | 280 | | _corridorWaypoints, |
| | 398 | 281 | | _corridorAddresses, |
| | 398 | 282 | | _corridorAddressSet); |
| | 398 | 283 | | if (!_explicitRefresh.Advance(meter)) |
| | | 284 | | { |
| | 107 | 285 | | rejection = NavigationOperationRejection.None; |
| | 107 | 286 | | return false; |
| | | 287 | | } |
| | 291 | 288 | | if (!_explicitRefresh.IsValid) |
| | | 289 | | { |
| | 3 | 290 | | rejection = NavigationOperationRejection.ValidationFailed; |
| | 3 | 291 | | return true; |
| | | 292 | | } |
| | 288 | 293 | | _stage = Stage.Complete; |
| | 288 | 294 | | rejection = NavigationOperationRejection.None; |
| | 288 | 295 | | return true; |
| | | 296 | | } |
| | | 297 | | |
| | | 298 | | private long GetPendingMapStatePayloadBytes() |
| | | 299 | | { |
| | 870 | 300 | | long bytes = 0; |
| | 870 | 301 | | if (_current == null) |
| | 615 | 302 | | return bytes; |
| | 255 | 303 | | if (!ReferenceEquals(_overlay, _current.Overlay)) |
| | 85 | 304 | | bytes = checked(bytes + _overlay!.RetainedBytes); |
| | 255 | 305 | | if (!ReferenceEquals(_dynamicAddresses, _current.DynamicAddresses)) |
| | | 306 | | { |
| | 16 | 307 | | bytes = checked(bytes + _dynamicAddresses!.RetainedBytes); |
| | | 308 | | } |
| | 255 | 309 | | return bytes; |
| | | 310 | | } |
| | | 311 | | |
| | | 312 | | private int GetPendingMapStatePayloadPages() |
| | | 313 | | { |
| | 870 | 314 | | int pages = 0; |
| | 870 | 315 | | if (_current == null) |
| | 615 | 316 | | return pages; |
| | 255 | 317 | | if (!ReferenceEquals(_overlay, _current.Overlay)) |
| | 85 | 318 | | pages = checked(pages + _overlay!.PersistentNodeCount); |
| | 255 | 319 | | if (!ReferenceEquals(_dynamicAddresses, _current.DynamicAddresses)) |
| | | 320 | | { |
| | 16 | 321 | | pages = checked(pages + _dynamicAddresses!.PersistentNodeCount); |
| | | 322 | | } |
| | 255 | 323 | | return pages; |
| | | 324 | | } |
| | | 325 | | |
| | | 326 | | private bool AdvanceDependencies( |
| | | 327 | | NavigationOperationCandidate.MapState state, |
| | | 328 | | bool remove, |
| | | 329 | | MaintenanceWorkMeter meter) |
| | | 330 | | { |
| | 2059 | 331 | | while (_dependencyStage < 2) |
| | | 332 | | { |
| | 1373 | 333 | | int count = _dependencyStage == 0 |
| | 1373 | 334 | | ? state.Map.TransitionSpan.Length |
| | 1373 | 335 | | : state.Overlay.TransitionCount; |
| | 5526 | 336 | | while (_dependencyIndex < count) |
| | | 337 | | { |
| | 4154 | 338 | | TraversalTransitionOverlayOperation overlay = _dependencyStage == 1 |
| | 4154 | 339 | | ? state.Overlay.GetTransitionAt(_dependencyIndex) |
| | 4154 | 340 | | : default; |
| | 4154 | 341 | | if (_dependencyStage == 1 |
| | 4154 | 342 | | && overlay.Kind != TraversalTransitionOverlayOperationKind.Upsert) |
| | | 343 | | { |
| | 24 | 344 | | _dependencyIndex++; |
| | 24 | 345 | | continue; |
| | | 346 | | } |
| | 4130 | 347 | | if (!meter.TryConsumeDependencyEntries(1)) |
| | 1 | 348 | | return false; |
| | 4129 | 349 | | string destination = _dependencyStage == 0 |
| | 4129 | 350 | | ? state.Map.TransitionSpan[_dependencyIndex].Destination.MapId |
| | 4129 | 351 | | : overlay.Transition.Destination.MapId; |
| | 4129 | 352 | | _working.UpdateIncomingSourceForWork(destination, state.Map.MapId, remove); |
| | 4129 | 353 | | _dependencyIndex++; |
| | | 354 | | } |
| | 1372 | 355 | | _dependencyStage++; |
| | 1372 | 356 | | _dependencyIndex = 0; |
| | | 357 | | } |
| | 686 | 358 | | return true; |
| | | 359 | | } |
| | | 360 | | |
| | | 361 | | private bool AdvanceValidation( |
| | | 362 | | NavigationOperationCandidate.MapState state, |
| | | 363 | | MaintenanceWorkMeter meter, |
| | | 364 | | out bool valid) |
| | | 365 | | { |
| | 1070 | 366 | | while (_validationStage < 2) |
| | | 367 | | { |
| | 717 | 368 | | int count = _validationStage == 0 |
| | 717 | 369 | | ? state.Map.TransitionSpan.Length |
| | 717 | 370 | | : state.Overlay.TransitionCount; |
| | 8954 | 371 | | while (_validationEdgeIndex < count) |
| | | 372 | | { |
| | 8244 | 373 | | if (!meter.TryConsumeExplicitEdges(1)) |
| | | 374 | | { |
| | 3 | 375 | | valid = true; |
| | 3 | 376 | | return false; |
| | | 377 | | } |
| | 8241 | 378 | | if (!_working.ValidateTransitionForWork( |
| | 8241 | 379 | | state, |
| | 8241 | 380 | | overlay: _validationStage == 1, |
| | 8241 | 381 | | _validationEdgeIndex++, |
| | 8241 | 382 | | _changedMapIds, |
| | 8241 | 383 | | _nextStates)) |
| | | 384 | | { |
| | 4 | 385 | | valid = false; |
| | 4 | 386 | | return true; |
| | | 387 | | } |
| | | 388 | | } |
| | 710 | 389 | | _validationStage++; |
| | 710 | 390 | | _validationEdgeIndex = 0; |
| | | 391 | | } |
| | 353 | 392 | | _validationStage = 0; |
| | 353 | 393 | | valid = true; |
| | 353 | 394 | | return true; |
| | | 395 | | } |
| | | 396 | | |
| | | 397 | | private void ResetDependencyCursor() |
| | | 398 | | { |
| | 686 | 399 | | _dependencyStage = 0; |
| | 686 | 400 | | _dependencyIndex = 0; |
| | 686 | 401 | | } |
| | | 402 | | |
| | | 403 | | private enum Stage |
| | | 404 | | { |
| | | 405 | | Fold, |
| | | 406 | | Dependencies, |
| | | 407 | | Validation, |
| | | 408 | | Complete |
| | | 409 | | } |
| | | 410 | | } |