| | | 1 | | using SwiftCollections; |
| | | 2 | | using System; |
| | | 3 | | |
| | | 4 | | namespace Trailblazer.Pathing; |
| | | 5 | | |
| | | 6 | | /// <summary> |
| | | 7 | | /// Provides deterministic directed transition snapshots for planners. |
| | | 8 | | /// </summary> |
| | | 9 | | /// <remarks> |
| | | 10 | | /// The query layer favors grid-scoped snapshots first, while still allowing callers to fall back |
| | | 11 | | /// to a full directed snapshot when they intentionally need world-wide transition discovery. |
| | | 12 | | /// </remarks> |
| | | 13 | | internal static class TraversalTransitionQuery |
| | | 14 | | { |
| | | 15 | | private enum GridMatchAxis |
| | | 16 | | { |
| | | 17 | | Source, |
| | | 18 | | Destination |
| | | 19 | | } |
| | | 20 | | |
| | 3839 | 21 | | private static TraversalTransitionQueryCache Cache => PathManager.ActiveState.TransitionQueryCache; |
| | | 22 | | |
| | 556 | 23 | | private static object _cacheLock => Cache.CacheLock; |
| | | 24 | | |
| | | 25 | | private static int _cachedRegistryVersion |
| | | 26 | | { |
| | 556 | 27 | | get => Cache.CachedRegistryVersion; |
| | 90 | 28 | | set => Cache.CachedRegistryVersion = value; |
| | | 29 | | } |
| | | 30 | | |
| | | 31 | | private static TraversalTransition[] _allDirectedTransitions |
| | | 32 | | { |
| | 173 | 33 | | get => Cache.AllDirectedTransitions; |
| | 156 | 34 | | set => Cache.AllDirectedTransitions = value; |
| | | 35 | | } |
| | | 36 | | |
| | | 37 | | private static bool _hasAllDirectedTransitions |
| | | 38 | | { |
| | 173 | 39 | | get => Cache.HasAllDirectedTransitions; |
| | 156 | 40 | | set => Cache.HasAllDirectedTransitions = value; |
| | | 41 | | } |
| | | 42 | | |
| | | 43 | | private static SwiftDictionary<TraversalTransitionType, TraversalTransition[]> _directedTransitionsByType => |
| | 126 | 44 | | Cache.DirectedTransitionsByType; |
| | | 45 | | |
| | | 46 | | private static SwiftDictionary<int, TraversalTransition[]> _directedTransitionsByMediumPair => |
| | 474 | 47 | | Cache.DirectedTransitionsByMediumPair; |
| | | 48 | | |
| | | 49 | | private static SwiftDictionary<int, TraversalTransition[]> _directedTransitionsFromSourceGrid => |
| | 288 | 50 | | Cache.DirectedTransitionsFromSourceGrid; |
| | | 51 | | |
| | | 52 | | private static SwiftDictionary<long, TraversalTransition[]> _directedTransitionsFromSourceGridByType => |
| | 130 | 53 | | Cache.DirectedTransitionsFromSourceGridByType; |
| | | 54 | | |
| | | 55 | | private static SwiftDictionary<long, TraversalTransition[]> _directedTransitionsFromSourceGridByMediumPair => |
| | 338 | 56 | | Cache.DirectedTransitionsFromSourceGridByMediumPair; |
| | | 57 | | |
| | | 58 | | private static SwiftDictionary<int, TraversalTransition[]> _directedTransitionsToDestinationGrid => |
| | 238 | 59 | | Cache.DirectedTransitionsToDestinationGrid; |
| | | 60 | | |
| | | 61 | | private static SwiftDictionary<long, TraversalTransition[]> _directedTransitionsToDestinationGridByMediumPair => |
| | 276 | 62 | | Cache.DirectedTransitionsToDestinationGridByMediumPair; |
| | | 63 | | |
| | | 64 | | private static SwiftDictionary<TraversalTransitionType, int[]> _sourceGridIndicesByType => |
| | 109 | 65 | | Cache.SourceGridIndicesByType; |
| | | 66 | | |
| | | 67 | | public static TraversalTransition[] GetDirectedTransitions() |
| | | 68 | | { |
| | 2 | 69 | | lock (_cacheLock) |
| | | 70 | | { |
| | 2 | 71 | | EnsureCacheVersion(); |
| | 2 | 72 | | return GetOrBuildAllDirectedTransitions_NoLock(); |
| | | 73 | | } |
| | 2 | 74 | | } |
| | | 75 | | |
| | | 76 | | public static TraversalTransition[] GetDirectedTransitions(TraversalTransitionType type) |
| | | 77 | | { |
| | 19 | 78 | | lock (_cacheLock) |
| | | 79 | | { |
| | 19 | 80 | | EnsureCacheVersion(); |
| | 19 | 81 | | if (_directedTransitionsByType.TryGetValue(type, out TraversalTransition[] cached)) |
| | 2 | 82 | | return cached; |
| | | 83 | | |
| | 17 | 84 | | TraversalTransition[] filtered = FilterDirectedTransitionsByType( |
| | 17 | 85 | | GetOrBuildAllDirectedTransitions_NoLock(), |
| | 17 | 86 | | type); |
| | 17 | 87 | | _directedTransitionsByType[type] = filtered; |
| | 17 | 88 | | return filtered; |
| | | 89 | | } |
| | 19 | 90 | | } |
| | | 91 | | |
| | | 92 | | public static TraversalTransition[] GetDirectedTransitions( |
| | | 93 | | TraversalMedium sourceMedium, |
| | | 94 | | TraversalMedium destinationMedium) |
| | | 95 | | { |
| | 230 | 96 | | lock (_cacheLock) |
| | | 97 | | { |
| | 230 | 98 | | EnsureCacheVersion(); |
| | 230 | 99 | | int key = MakeMediumPairKey(sourceMedium, destinationMedium); |
| | 230 | 100 | | if (_directedTransitionsByMediumPair.TryGetValue(key, out TraversalTransition[] cached)) |
| | 76 | 101 | | return cached; |
| | | 102 | | |
| | 154 | 103 | | TraversalTransition[] filtered = FilterDirectedTransitionsByMediumPair( |
| | 154 | 104 | | GetOrBuildAllDirectedTransitions_NoLock(), |
| | 154 | 105 | | sourceMedium, |
| | 154 | 106 | | destinationMedium); |
| | 154 | 107 | | _directedTransitionsByMediumPair[key] = filtered; |
| | 154 | 108 | | return filtered; |
| | | 109 | | } |
| | 230 | 110 | | } |
| | | 111 | | |
| | | 112 | | public static TraversalTransition[] GetDirectedTransitionsFromSourceGrid(int sourceGridIndex) |
| | | 113 | | { |
| | 9 | 114 | | lock (_cacheLock) |
| | | 115 | | { |
| | 9 | 116 | | EnsureCacheVersion(); |
| | 9 | 117 | | return GetOrBuildDirectedTransitionsFromSourceGrid_NoLock(sourceGridIndex); |
| | | 118 | | } |
| | 9 | 119 | | } |
| | | 120 | | |
| | | 121 | | public static TraversalTransition[] GetDirectedTransitionsFromSourceGrid( |
| | | 122 | | int sourceGridIndex, |
| | | 123 | | TraversalTransitionType type) |
| | | 124 | | { |
| | 21 | 125 | | lock (_cacheLock) |
| | | 126 | | { |
| | 21 | 127 | | EnsureCacheVersion(); |
| | 21 | 128 | | long key = MakeGridTypeKey(sourceGridIndex, type); |
| | 21 | 129 | | if (_directedTransitionsFromSourceGridByType.TryGetValue(key, out TraversalTransition[] cached)) |
| | 2 | 130 | | return cached; |
| | | 131 | | |
| | 19 | 132 | | TraversalTransition[] filtered = FilterDirectedTransitionsByType( |
| | 19 | 133 | | GetOrBuildDirectedTransitionsFromSourceGrid_NoLock(sourceGridIndex), |
| | 19 | 134 | | type); |
| | 19 | 135 | | _directedTransitionsFromSourceGridByType[key] = filtered; |
| | 19 | 136 | | return filtered; |
| | | 137 | | } |
| | 21 | 138 | | } |
| | | 139 | | |
| | | 140 | | public static TraversalTransition[] GetDirectedTransitionsFromSourceGrid( |
| | | 141 | | int sourceGridIndex, |
| | | 142 | | TraversalMedium sourceMedium, |
| | | 143 | | TraversalMedium destinationMedium) |
| | | 144 | | { |
| | 149 | 145 | | lock (_cacheLock) |
| | | 146 | | { |
| | 149 | 147 | | EnsureCacheVersion(); |
| | 149 | 148 | | long key = MakeGridMediumPairKey(sourceGridIndex, sourceMedium, destinationMedium); |
| | 149 | 149 | | if (_directedTransitionsFromSourceGridByMediumPair.TryGetValue(key, out TraversalTransition[] cached)) |
| | 50 | 150 | | return cached; |
| | | 151 | | |
| | 99 | 152 | | TraversalTransition[] filtered = FilterDirectedTransitionsByMediumPair( |
| | 99 | 153 | | GetOrBuildDirectedTransitionsFromSourceGrid_NoLock(sourceGridIndex), |
| | 99 | 154 | | sourceMedium, |
| | 99 | 155 | | destinationMedium); |
| | 99 | 156 | | _directedTransitionsFromSourceGridByMediumPair[key] = filtered; |
| | 99 | 157 | | return filtered; |
| | | 158 | | } |
| | 149 | 159 | | } |
| | | 160 | | |
| | | 161 | | public static TraversalTransition[] GetDirectedTransitionsToDestinationGrid(int destinationGridIndex) |
| | | 162 | | { |
| | 10 | 163 | | lock (_cacheLock) |
| | | 164 | | { |
| | 10 | 165 | | EnsureCacheVersion(); |
| | 10 | 166 | | return GetOrBuildDirectedTransitionsToDestinationGrid_NoLock(destinationGridIndex); |
| | | 167 | | } |
| | 10 | 168 | | } |
| | | 169 | | |
| | | 170 | | public static TraversalTransition[] GetDirectedTransitionsToDestinationGrid( |
| | | 171 | | int destinationGridIndex, |
| | | 172 | | TraversalMedium sourceMedium, |
| | | 173 | | TraversalMedium destinationMedium) |
| | | 174 | | { |
| | 106 | 175 | | lock (_cacheLock) |
| | | 176 | | { |
| | 106 | 177 | | EnsureCacheVersion(); |
| | 106 | 178 | | long key = MakeGridMediumPairKey(destinationGridIndex, sourceMedium, destinationMedium); |
| | 106 | 179 | | if (_directedTransitionsToDestinationGridByMediumPair.TryGetValue(key, out TraversalTransition[] cached)) |
| | 26 | 180 | | return cached; |
| | | 181 | | |
| | 80 | 182 | | TraversalTransition[] filtered = FilterDirectedTransitionsByMediumPair( |
| | 80 | 183 | | GetOrBuildDirectedTransitionsToDestinationGrid_NoLock(destinationGridIndex), |
| | 80 | 184 | | sourceMedium, |
| | 80 | 185 | | destinationMedium); |
| | 80 | 186 | | _directedTransitionsToDestinationGridByMediumPair[key] = filtered; |
| | 80 | 187 | | return filtered; |
| | | 188 | | } |
| | 106 | 189 | | } |
| | | 190 | | |
| | | 191 | | internal static int[] GetSourceGridIndices(TraversalTransitionType type) |
| | | 192 | | { |
| | 10 | 193 | | lock (_cacheLock) |
| | | 194 | | { |
| | 10 | 195 | | EnsureCacheVersion(); |
| | 10 | 196 | | if (_sourceGridIndicesByType.TryGetValue(type, out int[] cached)) |
| | 1 | 197 | | return cached; |
| | | 198 | | |
| | 9 | 199 | | int[] sourceGridIndices = BuildSourceGridIndices(GetDirectedTransitions(type)); |
| | 9 | 200 | | _sourceGridIndicesByType[type] = sourceGridIndices; |
| | 9 | 201 | | return sourceGridIndices; |
| | | 202 | | } |
| | 10 | 203 | | } |
| | | 204 | | |
| | | 205 | | private static void EnsureCacheVersion() |
| | | 206 | | { |
| | 556 | 207 | | int registryVersion = TraversalTransitionRegistry.RegistryVersion; |
| | 556 | 208 | | if (_cachedRegistryVersion == registryVersion) |
| | 466 | 209 | | return; |
| | | 210 | | |
| | 90 | 211 | | _allDirectedTransitions = Array.Empty<TraversalTransition>(); |
| | 90 | 212 | | _hasAllDirectedTransitions = false; |
| | 90 | 213 | | _directedTransitionsByType.Clear(); |
| | 90 | 214 | | _directedTransitionsByMediumPair.Clear(); |
| | 90 | 215 | | _directedTransitionsFromSourceGrid.Clear(); |
| | 90 | 216 | | _directedTransitionsFromSourceGridByType.Clear(); |
| | 90 | 217 | | _directedTransitionsFromSourceGridByMediumPair.Clear(); |
| | 90 | 218 | | _directedTransitionsToDestinationGrid.Clear(); |
| | 90 | 219 | | _directedTransitionsToDestinationGridByMediumPair.Clear(); |
| | 90 | 220 | | _sourceGridIndicesByType.Clear(); |
| | 90 | 221 | | _cachedRegistryVersion = registryVersion; |
| | 90 | 222 | | } |
| | | 223 | | |
| | | 224 | | private static TraversalTransition[] FilterDirectedTransitionsByType( |
| | | 225 | | TraversalTransition[] transitions, |
| | | 226 | | TraversalTransitionType type) |
| | | 227 | | { |
| | 36 | 228 | | if (transitions.Length == 0) |
| | 9 | 229 | | return Array.Empty<TraversalTransition>(); |
| | | 230 | | |
| | 27 | 231 | | SwiftList<TraversalTransition> filtered = new(); |
| | 264 | 232 | | for (int i = 0; i < transitions.Length; i++) |
| | | 233 | | { |
| | 105 | 234 | | if (transitions[i].Type == type) |
| | 94 | 235 | | filtered.Add(transitions[i]); |
| | | 236 | | } |
| | | 237 | | |
| | 27 | 238 | | return filtered.Count == 0 |
| | 27 | 239 | | ? Array.Empty<TraversalTransition>() |
| | 27 | 240 | | : filtered.ToArray(); |
| | | 241 | | } |
| | | 242 | | |
| | | 243 | | private static TraversalTransition[] FilterDirectedTransitionsByMediumPair( |
| | | 244 | | TraversalTransition[] transitions, |
| | | 245 | | TraversalMedium sourceMedium, |
| | | 246 | | TraversalMedium destinationMedium) |
| | | 247 | | { |
| | 333 | 248 | | if (transitions.Length == 0) |
| | 44 | 249 | | return Array.Empty<TraversalTransition>(); |
| | | 250 | | |
| | 289 | 251 | | SwiftList<TraversalTransition> filtered = new(); |
| | 2204 | 252 | | for (int i = 0; i < transitions.Length; i++) |
| | | 253 | | { |
| | 813 | 254 | | TraversalTransition transition = transitions[i]; |
| | 813 | 255 | | if (transition.Source.Medium == sourceMedium |
| | 813 | 256 | | && transition.Destination.Medium == destinationMedium) |
| | | 257 | | { |
| | 221 | 258 | | filtered.Add(transition); |
| | | 259 | | } |
| | | 260 | | } |
| | | 261 | | |
| | 289 | 262 | | return filtered.Count == 0 |
| | 289 | 263 | | ? Array.Empty<TraversalTransition>() |
| | 289 | 264 | | : filtered.ToArray(); |
| | | 265 | | } |
| | | 266 | | |
| | | 267 | | private static TraversalTransition[] GetOrBuildAllDirectedTransitions_NoLock() |
| | | 268 | | { |
| | 173 | 269 | | if (_hasAllDirectedTransitions) |
| | 107 | 270 | | return _allDirectedTransitions; |
| | | 271 | | |
| | 66 | 272 | | _allDirectedTransitions = BuildAllDirectedTransitions(TraversalTransitionRegistry.GetActiveTransitions()); |
| | 66 | 273 | | _hasAllDirectedTransitions = true; |
| | 66 | 274 | | return _allDirectedTransitions; |
| | | 275 | | } |
| | | 276 | | |
| | | 277 | | private static TraversalTransition[] GetOrBuildDirectedTransitionsFromSourceGrid_NoLock(int sourceGridIndex) |
| | | 278 | | { |
| | 127 | 279 | | if (_directedTransitionsFromSourceGrid.TryGetValue(sourceGridIndex, out TraversalTransition[] cached)) |
| | 56 | 280 | | return cached; |
| | | 281 | | |
| | 71 | 282 | | TraversalTransition[] directed = BuildDirectedTransitionsForGrid( |
| | 71 | 283 | | TraversalTransitionRegistry.GetActiveTransitionsTouchingGrid(sourceGridIndex), |
| | 71 | 284 | | sourceGridIndex, |
| | 71 | 285 | | GridMatchAxis.Source); |
| | 71 | 286 | | _directedTransitionsFromSourceGrid[sourceGridIndex] = directed; |
| | 71 | 287 | | return directed; |
| | | 288 | | } |
| | | 289 | | |
| | | 290 | | private static TraversalTransition[] GetOrBuildDirectedTransitionsToDestinationGrid_NoLock(int destinationGridIndex) |
| | | 291 | | { |
| | 90 | 292 | | if (_directedTransitionsToDestinationGrid.TryGetValue(destinationGridIndex, out TraversalTransition[] cached)) |
| | 32 | 293 | | return cached; |
| | | 294 | | |
| | 58 | 295 | | TraversalTransition[] directed = BuildDirectedTransitionsForGrid( |
| | 58 | 296 | | TraversalTransitionRegistry.GetActiveTransitionsTouchingGrid(destinationGridIndex), |
| | 58 | 297 | | destinationGridIndex, |
| | 58 | 298 | | GridMatchAxis.Destination); |
| | 58 | 299 | | _directedTransitionsToDestinationGrid[destinationGridIndex] = directed; |
| | 58 | 300 | | return directed; |
| | | 301 | | } |
| | | 302 | | |
| | | 303 | | private static TraversalTransition[] BuildAllDirectedTransitions( |
| | | 304 | | TraversalTransition[] transitions) |
| | | 305 | | { |
| | 66 | 306 | | SwiftList<TraversalTransition> directed = new(transitions.Length * 2); |
| | 398 | 307 | | for (int i = 0; i < transitions.Length; i++) |
| | 133 | 308 | | AddAllDirectedTransitions(directed, transitions[i]); |
| | | 309 | | |
| | 66 | 310 | | return SortTransitions(directed); |
| | | 311 | | } |
| | | 312 | | |
| | | 313 | | private static TraversalTransition[] BuildDirectedTransitionsForGrid( |
| | | 314 | | TraversalTransition[] transitions, |
| | | 315 | | int gridIndex, |
| | | 316 | | GridMatchAxis axis) |
| | | 317 | | { |
| | 129 | 318 | | SwiftList<TraversalTransition> directed = new(transitions.Length * 2); |
| | | 319 | | |
| | 770 | 320 | | for (int i = 0; i < transitions.Length; i++) |
| | | 321 | | { |
| | 256 | 322 | | TraversalTransition transition = transitions[i]; |
| | 256 | 323 | | if (MatchesGrid(transition, gridIndex, axis)) |
| | 254 | 324 | | directed.Add(transition); |
| | | 325 | | |
| | 256 | 326 | | if (transition.IsBidirectional |
| | 256 | 327 | | && MatchesGrid(transition, gridIndex, GetOppositeAxis(axis))) |
| | | 328 | | { |
| | 5 | 329 | | directed.Add(CreateReversedTransition(transition)); |
| | | 330 | | } |
| | | 331 | | } |
| | | 332 | | |
| | 129 | 333 | | return SortTransitions(directed); |
| | | 334 | | } |
| | | 335 | | |
| | | 336 | | private static int[] BuildSourceGridIndices(TraversalTransition[] transitions) |
| | | 337 | | { |
| | 9 | 338 | | if (transitions.Length == 0) |
| | 1 | 339 | | return Array.Empty<int>(); |
| | | 340 | | |
| | 8 | 341 | | SwiftHashSet<int> uniqueGridIndices = new(); |
| | 8 | 342 | | SwiftList<int> orderedGridIndices = new(); |
| | 46 | 343 | | for (int i = 0; i < transitions.Length; i++) |
| | | 344 | | { |
| | 15 | 345 | | int gridIndex = transitions[i].Source.VoxelIndex.GridIndex; |
| | 15 | 346 | | if (uniqueGridIndices.Add(gridIndex)) |
| | 9 | 347 | | orderedGridIndices.Add(gridIndex); |
| | | 348 | | } |
| | | 349 | | |
| | 8 | 350 | | int[] result = orderedGridIndices.ToArray(); |
| | 8 | 351 | | Array.Sort(result); |
| | 8 | 352 | | return result; |
| | | 353 | | } |
| | | 354 | | |
| | | 355 | | private static bool MatchesGrid( |
| | | 356 | | TraversalTransition transition, |
| | | 357 | | int gridIndex, |
| | | 358 | | GridMatchAxis axis) |
| | | 359 | | { |
| | 263 | 360 | | return axis == GridMatchAxis.Source |
| | 263 | 361 | | ? transition.Source.VoxelIndex.GridIndex == gridIndex |
| | 263 | 362 | | : transition.Destination.VoxelIndex.GridIndex == gridIndex; |
| | | 363 | | } |
| | | 364 | | |
| | | 365 | | private static void AddAllDirectedTransitions( |
| | | 366 | | SwiftList<TraversalTransition> directed, |
| | | 367 | | TraversalTransition transition) |
| | | 368 | | { |
| | 133 | 369 | | directed.Add(transition); |
| | | 370 | | |
| | 133 | 371 | | if (transition.IsBidirectional) |
| | 3 | 372 | | directed.Add(CreateReversedTransition(transition)); |
| | 133 | 373 | | } |
| | | 374 | | |
| | | 375 | | private static GridMatchAxis GetOppositeAxis(GridMatchAxis axis) |
| | | 376 | | { |
| | 7 | 377 | | return axis == GridMatchAxis.Source |
| | 7 | 378 | | ? GridMatchAxis.Destination |
| | 7 | 379 | | : GridMatchAxis.Source; |
| | | 380 | | } |
| | | 381 | | |
| | | 382 | | private static TraversalTransition CreateReversedTransition(TraversalTransition transition) |
| | | 383 | | { |
| | 8 | 384 | | return new TraversalTransition( |
| | 8 | 385 | | transition.Id, |
| | 8 | 386 | | transition.Type, |
| | 8 | 387 | | transition.Destination, |
| | 8 | 388 | | transition.Source, |
| | 8 | 389 | | transition.PathCostModifier, |
| | 8 | 390 | | transition.IsBidirectional); |
| | | 391 | | } |
| | | 392 | | |
| | | 393 | | private static TraversalTransition[] SortTransitions(SwiftList<TraversalTransition> directed) |
| | | 394 | | { |
| | 195 | 395 | | TraversalTransition[] ordered = directed.ToArray(); |
| | 195 | 396 | | TraversalTransitionOrdering.Sort(ordered); |
| | 195 | 397 | | return ordered; |
| | | 398 | | } |
| | | 399 | | |
| | | 400 | | private static int MakeMediumPairKey( |
| | | 401 | | TraversalMedium sourceMedium, |
| | | 402 | | TraversalMedium destinationMedium) |
| | | 403 | | { |
| | 485 | 404 | | return ((int)sourceMedium << 16) | (int)destinationMedium; |
| | | 405 | | } |
| | | 406 | | |
| | | 407 | | private static long MakeGridTypeKey(int gridIndex, TraversalTransitionType type) |
| | | 408 | | { |
| | 21 | 409 | | return ((long)(uint)gridIndex << 32) | (uint)type; |
| | | 410 | | } |
| | | 411 | | |
| | | 412 | | private static long MakeGridMediumPairKey( |
| | | 413 | | int gridIndex, |
| | | 414 | | TraversalMedium sourceMedium, |
| | | 415 | | TraversalMedium destinationMedium) |
| | | 416 | | { |
| | 255 | 417 | | return ((long)(uint)gridIndex << 32) | (uint)MakeMediumPairKey(sourceMedium, destinationMedium); |
| | | 418 | | } |
| | | 419 | | } |