| | | 1 | | //======================================================================= |
| | | 2 | | // NavigationAStarNodeTable.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 | | |
| | | 11 | | namespace Trailblazer.Pathing; |
| | | 12 | | |
| | | 13 | | internal struct NavigationAStarNodeRecord |
| | | 14 | | { |
| | | 15 | | internal Fixed64 Cost; |
| | | 16 | | internal Fixed64 Heuristic; |
| | | 17 | | // The search-only f-score adds eight bytes per workspace node, never to cached payloads. |
| | | 18 | | internal Fixed64 EstimatedTotalCost; |
| | | 19 | | internal NavigationMediumStateRef Parent; |
| | | 20 | | internal NavigationTraversalEdgeKind ParentEdgeKind; |
| | | 21 | | internal int HeapIndex; |
| | | 22 | | internal int ParentEdgeOrdinal; |
| | | 23 | | internal bool HasParent; |
| | | 24 | | internal bool Closed; |
| | | 25 | | } |
| | | 26 | | |
| | | 27 | | /// <summary>Stores fixed-capacity generation-stamped A* metadata.</summary> |
| | | 28 | | internal sealed class NavigationAStarNodeTable |
| | | 29 | | { |
| | | 30 | | private readonly NavigationMediumStateRef[] _keys; |
| | | 31 | | private readonly NavigationAStarNodeRecord[] _records; |
| | | 32 | | private readonly long[] _stamps; |
| | | 33 | | private readonly int _mask; |
| | | 34 | | private readonly int _capacity; |
| | | 35 | | private int _count; |
| | 8053 | 36 | | private long _generation = 1; |
| | | 37 | | |
| | 8053 | 38 | | internal NavigationAStarNodeTable(int capacity) |
| | | 39 | | { |
| | 8053 | 40 | | int tableSize = 1; |
| | 8053 | 41 | | int required = checked(Math.Max(1, capacity * 2)); |
| | 110527 | 42 | | while (tableSize < required) |
| | 102474 | 43 | | tableSize = checked(tableSize * 2); |
| | 8053 | 44 | | _keys = new NavigationMediumStateRef[tableSize]; |
| | 8053 | 45 | | _records = new NavigationAStarNodeRecord[tableSize]; |
| | 8053 | 46 | | _stamps = new long[tableSize]; |
| | 8053 | 47 | | _mask = tableSize - 1; |
| | 8053 | 48 | | _capacity = capacity; |
| | 8053 | 49 | | } |
| | | 50 | | |
| | | 51 | | internal void Reset() |
| | | 52 | | { |
| | | 53 | | unchecked |
| | | 54 | | { |
| | 252 | 55 | | _generation++; |
| | | 56 | | } |
| | 252 | 57 | | _count = 0; |
| | 252 | 58 | | } |
| | | 59 | | |
| | | 60 | | internal bool TryGetSlot(NavigationMediumStateRef node, out int slot) |
| | | 61 | | { |
| | 76248 | 62 | | slot = node.GetHashCode() & _mask; |
| | 76291 | 63 | | while (_stamps[slot] == _generation) |
| | | 64 | | { |
| | 70396 | 65 | | if (_keys[slot].Equals(node)) |
| | 70353 | 66 | | return true; |
| | 43 | 67 | | slot = (slot + 1) & _mask; |
| | | 68 | | } |
| | 5895 | 69 | | return false; |
| | | 70 | | } |
| | | 71 | | |
| | | 72 | | internal bool TryGetOrAdd( |
| | | 73 | | NavigationMediumStateRef node, |
| | | 74 | | out int slot, |
| | | 75 | | out bool added) |
| | | 76 | | { |
| | 5045 | 77 | | if (TryGetSlot(node, out slot)) |
| | | 78 | | { |
| | 1976 | 79 | | added = false; |
| | 1976 | 80 | | return true; |
| | | 81 | | } |
| | 3069 | 82 | | if (_count >= _capacity) |
| | | 83 | | { |
| | 2 | 84 | | added = false; |
| | 2 | 85 | | return false; |
| | | 86 | | } |
| | 3067 | 87 | | _keys[slot] = node; |
| | 3067 | 88 | | _records[slot] = default; |
| | 3067 | 89 | | _stamps[slot] = _generation; |
| | 3067 | 90 | | _count++; |
| | 3067 | 91 | | added = true; |
| | 3067 | 92 | | return true; |
| | | 93 | | } |
| | | 94 | | |
| | 76484 | 95 | | internal ref NavigationAStarNodeRecord GetRecord(int slot) => ref _records[slot]; |
| | | 96 | | } |