| | | 1 | | //======================================================================= |
| | | 2 | | // NavigationFlowFieldOpenHeap.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 | | namespace Trailblazer.Pathing; |
| | | 9 | | |
| | | 10 | | /// <summary>Orders flow nodes by exact integration cost and stable address.</summary> |
| | | 11 | | internal struct NavigationFlowFieldOpenHeap |
| | | 12 | | { |
| | | 13 | | private readonly NavigationFlowFieldWorkspace _workspace; |
| | | 14 | | |
| | | 15 | | internal NavigationFlowFieldOpenHeap(NavigationFlowFieldWorkspace workspace) => |
| | 736 | 16 | | _workspace = workspace; |
| | | 17 | | |
| | | 18 | | internal bool TryPeek(out int slot) |
| | | 19 | | { |
| | 4626 | 20 | | if (_workspace.HeapCount == 0) |
| | | 21 | | { |
| | 372 | 22 | | slot = -1; |
| | 372 | 23 | | return false; |
| | | 24 | | } |
| | 4254 | 25 | | slot = _workspace.HeapSlots[0]; |
| | 4254 | 26 | | return true; |
| | | 27 | | } |
| | | 28 | | |
| | | 29 | | internal void Push(int slot) |
| | | 30 | | { |
| | 3914 | 31 | | int index = _workspace.HeapCount++; |
| | 3914 | 32 | | _workspace.HeapSlots[index] = slot; |
| | 3914 | 33 | | _workspace.GetRecord(slot).HeapIndex = index; |
| | 3914 | 34 | | SortUp(index); |
| | 3914 | 35 | | } |
| | | 36 | | |
| | | 37 | | internal int Pop() |
| | | 38 | | { |
| | 3528 | 39 | | int result = _workspace.HeapSlots[0]; |
| | 3528 | 40 | | _workspace.GetRecord(result).HeapIndex = -1; |
| | 3528 | 41 | | int last = --_workspace.HeapCount; |
| | 3528 | 42 | | if (last > 0) |
| | | 43 | | { |
| | 69 | 44 | | int moved = _workspace.HeapSlots[last]; |
| | 69 | 45 | | _workspace.HeapSlots[0] = moved; |
| | 69 | 46 | | _workspace.GetRecord(moved).HeapIndex = 0; |
| | 69 | 47 | | SortDown(0); |
| | | 48 | | } |
| | 3528 | 49 | | return result; |
| | | 50 | | } |
| | | 51 | | |
| | | 52 | | internal void DecreaseKey(int slot) => |
| | 10 | 53 | | SortUp(_workspace.GetRecord(slot).HeapIndex); |
| | | 54 | | |
| | | 55 | | private void SortUp(int index) |
| | | 56 | | { |
| | 3949 | 57 | | while (index > 0) |
| | | 58 | | { |
| | 119 | 59 | | int parent = (index - 1) / 2; |
| | 119 | 60 | | if (Compare(index, parent) >= 0) |
| | 94 | 61 | | return; |
| | 25 | 62 | | Swap(index, parent); |
| | 25 | 63 | | index = parent; |
| | | 64 | | } |
| | 3830 | 65 | | } |
| | | 66 | | |
| | | 67 | | private void SortDown(int index) |
| | | 68 | | { |
| | 66 | 69 | | while (true) |
| | | 70 | | { |
| | 135 | 71 | | int left = checked((index * 2) + 1); |
| | 135 | 72 | | if (left >= _workspace.HeapCount) |
| | 49 | 73 | | return; |
| | 86 | 74 | | int right = left + 1; |
| | 86 | 75 | | int best = right < _workspace.HeapCount && Compare(right, left) < 0 |
| | 86 | 76 | | ? right |
| | 86 | 77 | | : left; |
| | 86 | 78 | | if (Compare(best, index) >= 0) |
| | 20 | 79 | | return; |
| | 66 | 80 | | Swap(index, best); |
| | 66 | 81 | | index = best; |
| | | 82 | | } |
| | | 83 | | } |
| | | 84 | | |
| | | 85 | | private int Compare(int leftIndex, int rightIndex) |
| | | 86 | | { |
| | 264 | 87 | | ref NavigationFlowFieldSearchNode left = ref _workspace.GetRecord( |
| | 264 | 88 | | _workspace.HeapSlots[leftIndex]); |
| | 264 | 89 | | ref NavigationFlowFieldSearchNode right = ref _workspace.GetRecord( |
| | 264 | 90 | | _workspace.HeapSlots[rightIndex]); |
| | 264 | 91 | | int comparison = left.IntegrationCost.CompareTo(right.IntegrationCost); |
| | 264 | 92 | | if (comparison != 0) |
| | 237 | 93 | | return comparison; |
| | 27 | 94 | | comparison = left.Address.CompareTo(right.Address); |
| | 27 | 95 | | return comparison != 0 |
| | 27 | 96 | | ? comparison |
| | 27 | 97 | | : _workspace.GetNode(_workspace.HeapSlots[leftIndex]).Medium.CompareTo( |
| | 27 | 98 | | _workspace.GetNode(_workspace.HeapSlots[rightIndex]).Medium); |
| | | 99 | | } |
| | | 100 | | |
| | | 101 | | private void Swap(int left, int right) |
| | | 102 | | { |
| | 91 | 103 | | int leftSlot = _workspace.HeapSlots[left]; |
| | 91 | 104 | | int rightSlot = _workspace.HeapSlots[right]; |
| | 91 | 105 | | _workspace.HeapSlots[left] = rightSlot; |
| | 91 | 106 | | _workspace.HeapSlots[right] = leftSlot; |
| | 91 | 107 | | _workspace.GetRecord(rightSlot).HeapIndex = left; |
| | 91 | 108 | | _workspace.GetRecord(leftSlot).HeapIndex = right; |
| | 91 | 109 | | } |
| | | 110 | | } |