| | | 1 | | using FixedMathSharp; |
| | | 2 | | using GridForge.Spatial; |
| | | 3 | | using SwiftCollections; |
| | | 4 | | using System; |
| | | 5 | | using System.Runtime.CompilerServices; |
| | | 6 | | |
| | | 7 | | namespace Trailblazer.Pathing; |
| | | 8 | | |
| | | 9 | | /// <summary> |
| | | 10 | | /// Stores the grid-space transform required to sample a flow-field result without resolving voxels through the world ma |
| | | 11 | | /// </summary> |
| | | 12 | | internal sealed class FlowFieldSamplingGrid |
| | | 13 | | { |
| | | 14 | | private const int DenseSparsityFactor = 4; |
| | | 15 | | |
| | | 16 | | private readonly int _worldSpawnToken; |
| | | 17 | | private readonly ushort _gridIndex; |
| | | 18 | | private readonly int _gridSpawnToken; |
| | | 19 | | private readonly Vector3d _originWorldPosition; |
| | | 20 | | private readonly Fixed64 _voxelSize; |
| | | 21 | | private readonly int _minX; |
| | | 22 | | private readonly int _minY; |
| | | 23 | | private readonly int _minZ; |
| | | 24 | | private readonly int _sizeX; |
| | | 25 | | private readonly int _sizeY; |
| | | 26 | | private readonly int _sizeZ; |
| | | 27 | | private readonly Vector3d[]? _denseDirections; |
| | | 28 | | private readonly bool[]? _denseOccupied; |
| | | 29 | | private readonly SwiftDictionary<FlowFieldLocalIndex, Vector3d>? _sparseDirections; |
| | | 30 | | |
| | 124 | 31 | | public FlowFieldSamplingGrid( |
| | 124 | 32 | | WorldVoxelIndex sampleIndex, |
| | 124 | 33 | | Vector3d originWorldPosition, |
| | 124 | 34 | | Fixed64 voxelSize, |
| | 124 | 35 | | int minX, |
| | 124 | 36 | | int minY, |
| | 124 | 37 | | int minZ, |
| | 124 | 38 | | int maxX, |
| | 124 | 39 | | int maxY, |
| | 124 | 40 | | int maxZ, |
| | 124 | 41 | | int fieldCount) |
| | | 42 | | { |
| | 124 | 43 | | _worldSpawnToken = sampleIndex.WorldSpawnToken; |
| | 124 | 44 | | _gridIndex = sampleIndex.GridIndex; |
| | 124 | 45 | | _gridSpawnToken = sampleIndex.GridSpawnToken; |
| | 124 | 46 | | _originWorldPosition = originWorldPosition; |
| | 124 | 47 | | _voxelSize = voxelSize; |
| | 124 | 48 | | _minX = minX; |
| | 124 | 49 | | _minY = minY; |
| | 124 | 50 | | _minZ = minZ; |
| | 124 | 51 | | _sizeX = maxX - minX + 1; |
| | 124 | 52 | | _sizeY = maxY - minY + 1; |
| | 124 | 53 | | _sizeZ = maxZ - minZ + 1; |
| | | 54 | | |
| | 124 | 55 | | long denseLength = (long)_sizeX * _sizeY * _sizeZ; |
| | 124 | 56 | | long denseLimit = Math.Max((long)fieldCount + 32L, (long)fieldCount * DenseSparsityFactor); |
| | 124 | 57 | | if (denseLength > 0 && denseLength <= denseLimit && denseLength <= int.MaxValue) |
| | | 58 | | { |
| | 123 | 59 | | _denseDirections = new Vector3d[(int)denseLength]; |
| | 123 | 60 | | _denseOccupied = new bool[(int)denseLength]; |
| | | 61 | | } |
| | | 62 | | else |
| | | 63 | | { |
| | 1 | 64 | | _sparseDirections = fieldCount > 0 |
| | 1 | 65 | | ? new SwiftDictionary<FlowFieldLocalIndex, Vector3d>(fieldCount) |
| | 1 | 66 | | : new SwiftDictionary<FlowFieldLocalIndex, Vector3d>(); |
| | | 67 | | } |
| | 1 | 68 | | } |
| | | 69 | | |
| | | 70 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 71 | | public bool MatchesGrid(WorldVoxelIndex index) |
| | | 72 | | { |
| | 1818 | 73 | | return index.WorldSpawnToken == _worldSpawnToken |
| | 1818 | 74 | | && index.GridIndex == _gridIndex |
| | 1818 | 75 | | && index.GridSpawnToken == _gridSpawnToken; |
| | | 76 | | } |
| | | 77 | | |
| | | 78 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 79 | | public void AddDirection(WorldVoxelIndex index, Vector3d direction) |
| | | 80 | | { |
| | 1818 | 81 | | FlowFieldLocalIndex localIndex = FlowFieldLocalIndex.FromVoxelIndex(index.VoxelIndex); |
| | 1818 | 82 | | if (_denseDirections != null && _denseOccupied != null) |
| | | 83 | | { |
| | 1817 | 84 | | int denseIndex = GetDenseIndex(localIndex.X, localIndex.Y, localIndex.Z); |
| | 1817 | 85 | | _denseDirections[denseIndex] = direction; |
| | 1817 | 86 | | _denseOccupied[denseIndex] = true; |
| | 1817 | 87 | | return; |
| | | 88 | | } |
| | | 89 | | |
| | 1 | 90 | | _sparseDirections!.Add(localIndex, direction); |
| | 1 | 91 | | } |
| | | 92 | | |
| | | 93 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 94 | | public bool TryGetDirection(Vector3d worldPosition, out Vector3d direction) |
| | | 95 | | { |
| | 47 | 96 | | Fixed64 localX = (worldPosition.x - _originWorldPosition.x) / _voxelSize; |
| | 47 | 97 | | Fixed64 localY = (worldPosition.y - _originWorldPosition.y) / _voxelSize; |
| | 47 | 98 | | Fixed64 localZ = (worldPosition.z - _originWorldPosition.z) / _voxelSize; |
| | | 99 | | |
| | 47 | 100 | | int x = localX.FloorToInt(); |
| | 47 | 101 | | int y = localY.FloorToInt(); |
| | 47 | 102 | | int z = localZ.FloorToInt(); |
| | | 103 | | |
| | 47 | 104 | | if (_denseDirections != null && _denseOccupied != null) |
| | | 105 | | { |
| | 45 | 106 | | if (x < _minX |
| | 45 | 107 | | || x >= _minX + _sizeX |
| | 45 | 108 | | || y < _minY |
| | 45 | 109 | | || y >= _minY + _sizeY |
| | 45 | 110 | | || z < _minZ |
| | 45 | 111 | | || z >= _minZ + _sizeZ) |
| | | 112 | | { |
| | 4 | 113 | | direction = Vector3d.Zero; |
| | 4 | 114 | | return false; |
| | | 115 | | } |
| | | 116 | | |
| | 41 | 117 | | int denseIndex = GetDenseIndex(x, y, z); |
| | 41 | 118 | | if (!_denseOccupied[denseIndex]) |
| | | 119 | | { |
| | 1 | 120 | | direction = Vector3d.Zero; |
| | 1 | 121 | | return false; |
| | | 122 | | } |
| | | 123 | | |
| | 40 | 124 | | direction = _denseDirections[denseIndex]; |
| | 40 | 125 | | return true; |
| | | 126 | | } |
| | | 127 | | |
| | 2 | 128 | | return _sparseDirections!.TryGetValue(new FlowFieldLocalIndex(x, y, z), out direction); |
| | | 129 | | } |
| | | 130 | | |
| | | 131 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 132 | | private int GetDenseIndex(int x, int y, int z) |
| | | 133 | | { |
| | 1858 | 134 | | return (((y - _minY) * _sizeZ) + (z - _minZ)) * _sizeX + (x - _minX); |
| | | 135 | | } |
| | | 136 | | } |