| | | 1 | | using FixedMathSharp; |
| | | 2 | | using GridForge.Grids; |
| | | 3 | | using System.Diagnostics.CodeAnalysis; |
| | | 4 | | using System.Runtime.CompilerServices; |
| | | 5 | | |
| | | 6 | | namespace Trailblazer.Pathing; |
| | | 7 | | |
| | | 8 | | /// <summary> |
| | | 9 | | /// Utility for resolving valid start and end voxels for pathfinding based on world positions, |
| | | 10 | | /// with optional size consideration and walkability fallback. |
| | | 11 | | /// </summary> |
| | | 12 | | public static class SolidVoxelFinder |
| | | 13 | | { |
| | | 14 | | /// <summary> |
| | | 15 | | /// Specifies the maximum allowable test distance. |
| | | 16 | | /// </summary> |
| | | 17 | | public const int MaxTestDistance = 3; |
| | | 18 | | |
| | | 19 | | /// <summary> |
| | | 20 | | /// Attempts to get valid start and end voxels from one explicit context. |
| | | 21 | | /// </summary> |
| | | 22 | | public static bool TryGetPathEdgeVoxels( |
| | | 23 | | TrailblazerWorldContext context, |
| | | 24 | | Vector3d origin, |
| | | 25 | | Vector3d target, |
| | | 26 | | [MaybeNullWhen(false)] out Voxel originVoxel, |
| | | 27 | | [MaybeNullWhen(false)] out Voxel targetVoxel, |
| | | 28 | | Fixed64? unitSize = null, |
| | | 29 | | bool allowUnwalkableEndpoints = false) |
| | | 30 | | { |
| | 1138 | 31 | | PathRequestContextResolver.ThrowIfUnusable(context); |
| | 1138 | 32 | | Fixed64 resolvedUnitSize = unitSize ?? context.VoxelSize; |
| | 1138 | 33 | | targetVoxel = null; |
| | 1138 | 34 | | if (!GetStartVoxel(context, origin, target, out originVoxel, allowUnwalkableEndpoints, resolvedUnitSize)) |
| | 25 | 35 | | return false; |
| | | 36 | | |
| | 1113 | 37 | | return GetEndVoxel(context, origin, target, out targetVoxel, allowUnwalkableEndpoints, resolvedUnitSize); |
| | | 38 | | } |
| | | 39 | | |
| | | 40 | | |
| | | 41 | | /// <summary> |
| | | 42 | | /// Finds a closest valid end voxel in one explicit context. |
| | | 43 | | /// </summary> |
| | | 44 | | public static bool GetEndVoxel( |
| | | 45 | | TrailblazerWorldContext context, |
| | | 46 | | Vector3d origin, |
| | | 47 | | Vector3d target, |
| | | 48 | | [MaybeNullWhen(false)] out Voxel targetVoxel, |
| | | 49 | | bool allowUnwalkableEndpoints = false, |
| | | 50 | | Fixed64? unitSize = null) |
| | | 51 | | { |
| | 1121 | 52 | | return TryGetEndpointVoxel( |
| | 1121 | 53 | | context, |
| | 1121 | 54 | | target, |
| | 1121 | 55 | | origin, |
| | 1121 | 56 | | out targetVoxel, |
| | 1121 | 57 | | allowUnwalkableEndpoints, |
| | 1121 | 58 | | unitSize ?? context.VoxelSize); |
| | | 59 | | } |
| | | 60 | | |
| | | 61 | | /// <summary> |
| | | 62 | | /// Finds a closest valid start voxel in one explicit context. |
| | | 63 | | /// </summary> |
| | | 64 | | public static bool GetStartVoxel( |
| | | 65 | | TrailblazerWorldContext context, |
| | | 66 | | Vector3d origin, |
| | | 67 | | Vector3d target, |
| | | 68 | | [MaybeNullWhen(false)] out Voxel originVoxel, |
| | | 69 | | bool allowUnwalkableEndpoints = false, |
| | | 70 | | Fixed64? unitSize = null) |
| | | 71 | | { |
| | 1217 | 72 | | return TryGetEndpointVoxel( |
| | 1217 | 73 | | context, |
| | 1217 | 74 | | origin, |
| | 1217 | 75 | | target, |
| | 1217 | 76 | | out originVoxel, |
| | 1217 | 77 | | allowUnwalkableEndpoints, |
| | 1217 | 78 | | unitSize ?? context.VoxelSize); |
| | | 79 | | } |
| | | 80 | | |
| | | 81 | | |
| | | 82 | | /// <summary> |
| | | 83 | | /// Performs a bounded same-layer star search in one explicit context. |
| | | 84 | | /// </summary> |
| | | 85 | | public static bool StarCast( |
| | | 86 | | TrailblazerWorldContext context, |
| | | 87 | | Vector3d target, |
| | | 88 | | [MaybeNullWhen(false)] out Voxel targetVoxel) => |
| | 2 | 89 | | StarCast(context, target, out targetVoxel, context.VoxelSize); |
| | | 90 | | |
| | | 91 | | /// <summary> |
| | | 92 | | /// Performs a bounded same-layer star search in one explicit context. |
| | | 93 | | /// </summary> |
| | | 94 | | public static bool StarCast( |
| | | 95 | | TrailblazerWorldContext context, |
| | | 96 | | Vector3d target, |
| | | 97 | | [MaybeNullWhen(false)] out Voxel targetVoxel, |
| | | 98 | | Fixed64 unitSize) |
| | | 99 | | { |
| | 3 | 100 | | PathRequestContextResolver.ThrowIfUnusable(context); |
| | 3 | 101 | | if (!context.World.TryGetVoxel(target, out Voxel? directVoxel) |
| | 3 | 102 | | || directVoxel == null) |
| | | 103 | | { |
| | 1 | 104 | | targetVoxel = null; |
| | 1 | 105 | | return false; |
| | | 106 | | } |
| | | 107 | | |
| | 2 | 108 | | return StarCast(context, target, directVoxel, out targetVoxel, unitSize); |
| | | 109 | | } |
| | | 110 | | |
| | | 111 | | /// <summary> |
| | | 112 | | /// Finds the closest valid neighboring solid voxel in one explicit context. |
| | | 113 | | /// </summary> |
| | | 114 | | public static bool TryGetClosestWalkableVoxel( |
| | | 115 | | TrailblazerWorldContext context, |
| | | 116 | | Voxel voxel, |
| | | 117 | | [MaybeNullWhen(false)] out Voxel closestNeighbor, |
| | | 118 | | Fixed64? unitSize = null) |
| | | 119 | | { |
| | 4 | 120 | | return EndpointVoxelResolver.TryGetClosestTraversableVoxel( |
| | 4 | 121 | | context, |
| | 4 | 122 | | voxel, |
| | 4 | 123 | | out closestNeighbor, |
| | 4 | 124 | | unitSize ?? context.VoxelSize, |
| | 4 | 125 | | new SolidEndpointPolicy(context)); |
| | | 126 | | } |
| | | 127 | | |
| | | 128 | | /// <summary> |
| | | 129 | | /// Finds the closest valid endpoint voxel for a unit size in one explicit context. |
| | | 130 | | /// </summary> |
| | | 131 | | public static bool GetClosestVoxelForSize( |
| | | 132 | | TrailblazerWorldContext context, |
| | | 133 | | Vector3d origin, |
| | | 134 | | Vector3d target, |
| | | 135 | | Fixed64 unitSize, |
| | | 136 | | [MaybeNullWhen(false)] out Voxel targetVoxel, |
| | | 137 | | bool allowUnwalkableEndpoints = false) |
| | | 138 | | { |
| | 2 | 139 | | return EndpointVoxelResolver.TryGetEndpointVoxel( |
| | 2 | 140 | | context, |
| | 2 | 141 | | target, |
| | 2 | 142 | | origin, |
| | 2 | 143 | | out targetVoxel, |
| | 2 | 144 | | allowUnwalkableEndpoints, |
| | 2 | 145 | | unitSize, |
| | 2 | 146 | | new SolidEndpointPolicy(context)); |
| | | 147 | | } |
| | | 148 | | |
| | | 149 | | private static bool TryGetEndpointVoxel( |
| | | 150 | | TrailblazerWorldContext context, |
| | | 151 | | Vector3d position, |
| | | 152 | | Vector3d traceToward, |
| | | 153 | | [MaybeNullWhen(false)] out Voxel voxel, |
| | | 154 | | bool allowUnwalkableEndpoints, |
| | | 155 | | Fixed64 unitSize) |
| | | 156 | | { |
| | 2338 | 157 | | return EndpointVoxelResolver.TryGetEndpointVoxel( |
| | 2338 | 158 | | context, |
| | 2338 | 159 | | position, |
| | 2338 | 160 | | traceToward, |
| | 2338 | 161 | | out voxel, |
| | 2338 | 162 | | allowUnwalkableEndpoints, |
| | 2338 | 163 | | unitSize, |
| | 2338 | 164 | | new SolidEndpointPolicy(context)); |
| | | 165 | | } |
| | | 166 | | |
| | | 167 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 168 | | private static bool IsChartTraversable(Voxel voxel, Fixed64 unitSize, Fixed64 voxelSize) |
| | | 169 | | { |
| | 2608 | 170 | | if (!IsBaseChartTraversable(voxel)) |
| | 234 | 171 | | return false; |
| | | 172 | | |
| | 2374 | 173 | | voxel.TryGetPartition(out SolidChartPartition? partition); |
| | 2374 | 174 | | return unitSize == voxelSize |
| | 2374 | 175 | | || (partition != null && !partition.IsImpassable(unitSize)); |
| | | 176 | | } |
| | | 177 | | |
| | | 178 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 179 | | private static bool IsBaseChartTraversable(Voxel voxel) => |
| | 2629 | 180 | | voxel != null |
| | 2629 | 181 | | && !voxel.IsBlocked |
| | 2629 | 182 | | && voxel.HasPartition<SolidChartPartition>(); |
| | | 183 | | |
| | | 184 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 185 | | private static bool RequiresSizeFallback(Voxel voxel, Fixed64 unitSize, Fixed64 voxelSize) |
| | | 186 | | { |
| | 35 | 187 | | if (unitSize == voxelSize |
| | 35 | 188 | | || !IsBaseChartTraversable(voxel) |
| | 35 | 189 | | || !voxel.TryGetPartition(out SolidChartPartition? partition) |
| | 35 | 190 | | || partition == null) |
| | | 191 | | { |
| | 14 | 192 | | return false; |
| | | 193 | | } |
| | | 194 | | |
| | 21 | 195 | | return partition.IsImpassable(unitSize); |
| | | 196 | | } |
| | | 197 | | |
| | | 198 | | private readonly struct SolidEndpointPolicy : IVoxelEndpointResolutionPolicy |
| | | 199 | | { |
| | | 200 | | private readonly TrailblazerWorldContext _context; |
| | | 201 | | private readonly Fixed64 _voxelSize; |
| | | 202 | | |
| | | 203 | | public SolidEndpointPolicy(TrailblazerWorldContext context) |
| | | 204 | | { |
| | 2344 | 205 | | _context = context; |
| | 2344 | 206 | | _voxelSize = context.VoxelSize; |
| | 2344 | 207 | | } |
| | | 208 | | |
| | | 209 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | 2340 | 210 | | public bool CanResolve() => true; |
| | | 211 | | |
| | | 212 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 213 | | public bool TryAcceptDirectVoxel( |
| | | 214 | | Voxel voxel, |
| | | 215 | | Fixed64 unitSize, |
| | | 216 | | bool allowUnwalkableEndpoints) |
| | | 217 | | { |
| | 2324 | 218 | | return IsChartTraversable(voxel, unitSize, _voxelSize); |
| | | 219 | | } |
| | | 220 | | |
| | | 221 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 222 | | public bool RequiresSizeFallback(Voxel voxel, Fixed64 unitSize) |
| | | 223 | | { |
| | 35 | 224 | | return SolidVoxelFinder.RequiresSizeFallback(voxel, unitSize, _voxelSize); |
| | | 225 | | } |
| | | 226 | | |
| | | 227 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 228 | | public bool IsTraversable(Voxel voxel, Fixed64 unitSize) |
| | | 229 | | { |
| | 280 | 230 | | return IsChartTraversable(voxel, unitSize, _voxelSize); |
| | | 231 | | } |
| | | 232 | | |
| | | 233 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 234 | | public bool TryGetFinalFallbackVoxel( |
| | | 235 | | Vector3d position, |
| | | 236 | | Voxel directVoxel, |
| | | 237 | | Fixed64 unitSize, |
| | | 238 | | [MaybeNullWhen(false)] out Voxel voxel) |
| | | 239 | | { |
| | 2 | 240 | | return StarCast(_context, position, directVoxel, out voxel, unitSize); |
| | | 241 | | } |
| | | 242 | | } |
| | | 243 | | |
| | | 244 | | private static bool StarCast( |
| | | 245 | | TrailblazerWorldContext context, |
| | | 246 | | Vector3d target, |
| | | 247 | | Voxel directVoxel, |
| | | 248 | | [MaybeNullWhen(false)] out Voxel targetVoxel, |
| | | 249 | | Fixed64 unitSize) |
| | | 250 | | { |
| | 4 | 251 | | targetVoxel = null; |
| | | 252 | | |
| | 4 | 253 | | AlternativeVoxelFinder finder = context.Pathing.State.AlternativeVoxelFinder; |
| | 4 | 254 | | finder.SetQuery(context, target, directVoxel, MaxTestDistance); |
| | | 255 | | |
| | 4 | 256 | | if (!finder.GetVoxel(out Voxel? candidateVoxel) |
| | 4 | 257 | | || candidateVoxel == null) |
| | 0 | 258 | | return false; |
| | | 259 | | |
| | 4 | 260 | | if (IsChartTraversable(candidateVoxel, unitSize, context.VoxelSize)) |
| | | 261 | | { |
| | 1 | 262 | | targetVoxel = candidateVoxel; |
| | 1 | 263 | | return true; |
| | | 264 | | } |
| | | 265 | | |
| | 3 | 266 | | return TryGetClosestWalkableVoxel(context, candidateVoxel, out targetVoxel, unitSize); |
| | | 267 | | } |
| | | 268 | | } |