| | | 1 | | using FixedMathSharp; |
| | | 2 | | using GridForge.Grids; |
| | | 3 | | using System.Diagnostics.CodeAnalysis; |
| | | 4 | | |
| | | 5 | | namespace Trailblazer.Pathing; |
| | | 6 | | |
| | | 7 | | /// <summary> |
| | | 8 | | /// Performs a bounded same-layer fallback scan around a query position and returns |
| | | 9 | | /// the first unblocked voxel found in deterministic star/ring order. |
| | | 10 | | /// </summary> |
| | | 11 | | public class AlternativeVoxelFinder |
| | | 12 | | { |
| | | 13 | | private Vector3d _worldPos; |
| | | 14 | | |
| | | 15 | | private Vector3d _anchorVoxelPosition; |
| | | 16 | | |
| | | 17 | | private GridWorld? _world; |
| | | 18 | | |
| | | 19 | | private Fixed64 _voxelSize; |
| | | 20 | | |
| | | 21 | | private int _maxTestDistance; |
| | | 22 | | |
| | | 23 | | private (int x, int z) _direction; |
| | | 24 | | |
| | | 25 | | private int _layer; |
| | | 26 | | |
| | | 27 | | /// <summary> |
| | | 28 | | /// Configures the fallback search around the given world-space query point for one explicit context. |
| | | 29 | | /// </summary> |
| | | 30 | | public void SetQuery( |
| | | 31 | | TrailblazerWorldContext context, |
| | | 32 | | Vector3d worldPos, |
| | | 33 | | Voxel anchorVoxel, |
| | | 34 | | int maxTestDistance) |
| | | 35 | | { |
| | 11 | 36 | | PathRequestContextResolver.ThrowIfUnusable(context); |
| | 11 | 37 | | _world = context.World; |
| | 11 | 38 | | _voxelSize = context.VoxelSize; |
| | 11 | 39 | | _worldPos = worldPos; |
| | 11 | 40 | | _anchorVoxelPosition = anchorVoxel.WorldPosition; |
| | 11 | 41 | | _maxTestDistance = maxTestDistance; |
| | 11 | 42 | | _layer = 1; |
| | 11 | 43 | | } |
| | | 44 | | |
| | | 45 | | /// <summary> |
| | | 46 | | /// Attempts to find the first unblocked voxel in deterministic star/ring order on the query layer. |
| | | 47 | | /// </summary> |
| | | 48 | | public bool GetVoxel([MaybeNullWhen(false)] out Voxel nextVoxel) |
| | | 49 | | { |
| | 11 | 50 | | nextVoxel = null; |
| | 11 | 51 | | InitializeDirection(); |
| | | 52 | | |
| | 11 | 53 | | int layerStartX = _direction.x; |
| | 11 | 54 | | int layerStartZ = _direction.z; |
| | | 55 | | |
| | 11 | 56 | | int iterations = 0; // <- this is for debugging |
| | 62 | 57 | | for (_layer = 1; _layer <= _maxTestDistance;) |
| | | 58 | | { |
| | 50 | 59 | | Vector3d checkPosition = new( |
| | 50 | 60 | | _worldPos.x + _direction.x, |
| | 50 | 61 | | _worldPos.y, |
| | 50 | 62 | | _worldPos.z + _direction.z); |
| | 50 | 63 | | if (_world != null |
| | 50 | 64 | | && _world.TryGetVoxel(checkPosition, out Voxel? checkVoxel) |
| | 50 | 65 | | && checkVoxel != null |
| | 50 | 66 | | && IsSearchCandidate(checkVoxel)) |
| | | 67 | | { |
| | 10 | 68 | | nextVoxel = checkVoxel; |
| | 10 | 69 | | return true; |
| | | 70 | | } |
| | | 71 | | |
| | 40 | 72 | | AdvanceRotation(); |
| | | 73 | | // If we make a full loop |
| | 40 | 74 | | if (layerStartX == _direction.x && layerStartZ == _direction.z) |
| | | 75 | | { |
| | 5 | 76 | | _layer++; |
| | | 77 | | // Advance a layer instead of rotation |
| | 5 | 78 | | if (_direction.x > 0) |
| | 1 | 79 | | _direction.x = _layer; |
| | 4 | 80 | | else if (_direction.x < 0) |
| | 2 | 81 | | _direction.x = -_layer; |
| | | 82 | | |
| | 5 | 83 | | if (_direction.z > 0) |
| | 1 | 84 | | _direction.z = _layer; |
| | 4 | 85 | | else if (_direction.z < 0) |
| | 1 | 86 | | _direction.z = -_layer; |
| | | 87 | | |
| | 5 | 88 | | layerStartX = _direction.x; |
| | 5 | 89 | | layerStartZ = _direction.z; |
| | | 90 | | } |
| | | 91 | | |
| | 40 | 92 | | iterations++; |
| | 40 | 93 | | if (iterations > 500) |
| | | 94 | | { |
| | 0 | 95 | | TrailblazerLogger.Channel.Error( |
| | 0 | 96 | | $"Alternative voxel search exceeded the iteration safety cap for query {_worldPos} at layer {_layer} |
| | 0 | 97 | | break; |
| | | 98 | | } |
| | | 99 | | } |
| | | 100 | | |
| | 1 | 101 | | return false; |
| | | 102 | | } |
| | | 103 | | |
| | | 104 | | /// <summary> |
| | | 105 | | /// Advances the rotation clockwise |
| | | 106 | | /// </summary> |
| | | 107 | | private void AdvanceRotation() |
| | | 108 | | { |
| | | 109 | | // sides |
| | 40 | 110 | | if (_direction.x == 0) |
| | | 111 | | { |
| | 10 | 112 | | if (_direction.z == 1) // up |
| | 5 | 113 | | _direction.x = _layer; |
| | | 114 | | else // down |
| | 5 | 115 | | _direction.x = -_layer; |
| | | 116 | | |
| | 5 | 117 | | return; |
| | | 118 | | } |
| | | 119 | | |
| | 30 | 120 | | if (_direction.z == 0) |
| | | 121 | | { |
| | | 122 | | |
| | 10 | 123 | | if (_direction.x == 1) // right |
| | 5 | 124 | | _direction.z = -_layer; |
| | | 125 | | else // left |
| | 5 | 126 | | _direction.z = _layer; |
| | | 127 | | |
| | 5 | 128 | | return; |
| | | 129 | | } |
| | | 130 | | |
| | | 131 | | // corners |
| | 20 | 132 | | if (_direction.x > 0) |
| | | 133 | | { |
| | | 134 | | |
| | 10 | 135 | | if (_direction.z > 0) // top-right |
| | 5 | 136 | | _direction.z = 0; |
| | | 137 | | else |
| | 5 | 138 | | _direction.x = 0; // bottom-right |
| | | 139 | | |
| | 5 | 140 | | return; |
| | | 141 | | } |
| | | 142 | | |
| | 10 | 143 | | if (_direction.z > 0) // top-left |
| | 5 | 144 | | _direction.x = 0; |
| | | 145 | | else |
| | 5 | 146 | | _direction.z = 0; // bottom-left |
| | 5 | 147 | | } |
| | | 148 | | |
| | | 149 | | private static bool IsSearchCandidate(Voxel voxel) => |
| | 50 | 150 | | voxel != null |
| | 50 | 151 | | && !voxel.IsBlocked; |
| | | 152 | | |
| | | 153 | | private void InitializeDirection() |
| | | 154 | | { |
| | 11 | 155 | | Fixed64 halfVoxel = _voxelSize * Fixed64.Half; |
| | 11 | 156 | | Fixed64 xOffsetFromCenter = _worldPos.x - (_anchorVoxelPosition.x + halfVoxel); |
| | 11 | 157 | | Fixed64 zOffsetFromCenter = _worldPos.z - (_anchorVoxelPosition.z + halfVoxel); |
| | | 158 | | |
| | 11 | 159 | | if (xOffsetFromCenter.Abs() >= zOffsetFromCenter.Abs()) |
| | | 160 | | { |
| | 9 | 161 | | _direction.x = xOffsetFromCenter < Fixed64.Zero ? -1 : 1; |
| | 9 | 162 | | _direction.z = 0; |
| | 9 | 163 | | return; |
| | | 164 | | } |
| | | 165 | | |
| | 2 | 166 | | _direction.x = 0; |
| | 2 | 167 | | _direction.z = zOffsetFromCenter < Fixed64.Zero ? -1 : 1; |
| | 2 | 168 | | } |
| | | 169 | | } |