| | | 1 | | using FixedMathSharp; |
| | | 2 | | using GridForge; |
| | | 3 | | using GridForge.Grids; |
| | | 4 | | using GridForge.Spatial; |
| | | 5 | | using GridForge.Utility; |
| | | 6 | | using System.Diagnostics.CodeAnalysis; |
| | | 7 | | |
| | | 8 | | namespace Trailblazer.Pathing; |
| | | 9 | | |
| | | 10 | | internal interface IVoxelEndpointResolutionPolicy |
| | | 11 | | { |
| | | 12 | | bool CanResolve(); |
| | | 13 | | |
| | | 14 | | bool TryAcceptDirectVoxel( |
| | | 15 | | Voxel voxel, |
| | | 16 | | Fixed64 unitSize, |
| | | 17 | | bool allowUnwalkableEndpoints); |
| | | 18 | | |
| | | 19 | | bool RequiresSizeFallback(Voxel voxel, Fixed64 unitSize); |
| | | 20 | | |
| | | 21 | | bool IsTraversable(Voxel voxel, Fixed64 unitSize); |
| | | 22 | | |
| | | 23 | | bool TryGetFinalFallbackVoxel( |
| | | 24 | | Vector3d position, |
| | | 25 | | Voxel directVoxel, |
| | | 26 | | Fixed64 unitSize, |
| | | 27 | | [MaybeNullWhen(false)] out Voxel voxel); |
| | | 28 | | } |
| | | 29 | | |
| | | 30 | | internal static class EndpointVoxelResolver |
| | | 31 | | { |
| | | 32 | | public static bool TryGetEndpointVoxel<TPolicy>( |
| | | 33 | | TrailblazerWorldContext context, |
| | | 34 | | Vector3d position, |
| | | 35 | | Vector3d traceToward, |
| | | 36 | | [MaybeNullWhen(false)] out Voxel voxel, |
| | | 37 | | bool allowUnwalkableEndpoints, |
| | | 38 | | Fixed64 unitSize, |
| | | 39 | | TPolicy policy) |
| | | 40 | | where TPolicy : struct, IVoxelEndpointResolutionPolicy |
| | | 41 | | { |
| | 2794 | 42 | | PathRequestContextResolver.ThrowIfUnusable(context); |
| | 2794 | 43 | | GridWorld world = context.World; |
| | 2794 | 44 | | if (!policy.CanResolve()) |
| | | 45 | | { |
| | 8 | 46 | | voxel = null; |
| | 8 | 47 | | return false; |
| | | 48 | | } |
| | | 49 | | |
| | 2786 | 50 | | voxel = null; |
| | 2786 | 51 | | bool shouldRelaxEndpoint = allowUnwalkableEndpoints; |
| | | 52 | | |
| | 2786 | 53 | | if (world.TryGetVoxel(position, out Voxel? directVoxel) |
| | 2786 | 54 | | && directVoxel != null) |
| | | 55 | | { |
| | 2764 | 56 | | if (policy.TryAcceptDirectVoxel(directVoxel, unitSize, allowUnwalkableEndpoints)) |
| | | 57 | | { |
| | 2669 | 58 | | voxel = directVoxel; |
| | 2669 | 59 | | return true; |
| | | 60 | | } |
| | | 61 | | |
| | 95 | 62 | | shouldRelaxEndpoint = shouldRelaxEndpoint || policy.RequiresSizeFallback(directVoxel, unitSize); |
| | 95 | 63 | | if (shouldRelaxEndpoint |
| | 95 | 64 | | && TryGetClosestTraversableVoxel(context, directVoxel, out Voxel? closestNeighbor, unitSize, policy) |
| | 95 | 65 | | && closestNeighbor != null) |
| | | 66 | | { |
| | 53 | 67 | | voxel = closestNeighbor; |
| | 53 | 68 | | return true; |
| | | 69 | | } |
| | | 70 | | |
| | 42 | 71 | | if (!shouldRelaxEndpoint) |
| | 33 | 72 | | return false; |
| | | 73 | | |
| | 9 | 74 | | if (TryTraceToClosestTraversableVoxel(context, position, traceToward, unitSize, out voxel, policy)) |
| | 3 | 75 | | return true; |
| | | 76 | | |
| | 6 | 77 | | return policy.TryGetFinalFallbackVoxel(position, directVoxel, unitSize, out voxel); |
| | | 78 | | } |
| | | 79 | | |
| | 22 | 80 | | if (!shouldRelaxEndpoint) |
| | 20 | 81 | | return false; |
| | | 82 | | |
| | 2 | 83 | | if (TryTraceToClosestTraversableVoxel(context, position, traceToward, unitSize, out voxel, policy)) |
| | 2 | 84 | | return true; |
| | | 85 | | |
| | 0 | 86 | | voxel = null; |
| | 0 | 87 | | return false; |
| | | 88 | | } |
| | | 89 | | |
| | | 90 | | public static bool TryGetClosestTraversableVoxel<TPolicy>( |
| | | 91 | | TrailblazerWorldContext context, |
| | | 92 | | Voxel voxel, |
| | | 93 | | [MaybeNullWhen(false)] out Voxel closestNeighbor, |
| | | 94 | | Fixed64 unitSize, |
| | | 95 | | TPolicy policy) |
| | | 96 | | where TPolicy : struct, IVoxelEndpointResolutionPolicy |
| | | 97 | | { |
| | 70 | 98 | | PathRequestContextResolver.ThrowIfUnusable(context); |
| | 70 | 99 | | closestNeighbor = null; |
| | 70 | 100 | | if (voxel == null || !context.World.TryGetGrid(voxel.WorldIndex.GridIndex, out VoxelGrid? grid)) |
| | 0 | 101 | | return false; |
| | | 102 | | |
| | 568 | 103 | | foreach (SpatialDirection dir in SpatialAwareness.PerpendicularDirections) |
| | | 104 | | { |
| | 238 | 105 | | if (!voxel.TryGetNeighborFromDirection(grid!, dir, out Voxel? candidate) |
| | 238 | 106 | | || candidate == null |
| | 238 | 107 | | || !policy.IsTraversable(candidate, unitSize)) |
| | | 108 | | { |
| | | 109 | | continue; |
| | | 110 | | } |
| | | 111 | | |
| | 48 | 112 | | closestNeighbor = candidate; |
| | 48 | 113 | | return true; |
| | | 114 | | } |
| | | 115 | | |
| | 644 | 116 | | foreach (SpatialDirection dir in SpatialAwareness.DiagonalDirections) |
| | | 117 | | { |
| | 304 | 118 | | if (!voxel.TryGetNeighborFromDirection(grid!, dir, out Voxel? candidate) |
| | 304 | 119 | | || candidate == null |
| | 304 | 120 | | || !policy.IsTraversable(candidate, unitSize)) |
| | | 121 | | { |
| | | 122 | | continue; |
| | | 123 | | } |
| | | 124 | | |
| | 8 | 125 | | closestNeighbor = candidate; |
| | 8 | 126 | | return true; |
| | | 127 | | } |
| | | 128 | | |
| | 14 | 129 | | return false; |
| | | 130 | | } |
| | | 131 | | |
| | | 132 | | private static bool TryTraceToClosestTraversableVoxel<TPolicy>( |
| | | 133 | | TrailblazerWorldContext context, |
| | | 134 | | Vector3d position, |
| | | 135 | | Vector3d traceToward, |
| | | 136 | | Fixed64 unitSize, |
| | | 137 | | [MaybeNullWhen(false)] out Voxel voxel, |
| | | 138 | | TPolicy policy) |
| | | 139 | | where TPolicy : struct, IVoxelEndpointResolutionPolicy |
| | | 140 | | { |
| | 39 | 141 | | foreach (GridVoxelSet gridVoxelSet in GridTracer.TraceLine(context.World, position, traceToward)) |
| | | 142 | | { |
| | 127 | 143 | | foreach (Voxel current in gridVoxelSet.Voxels) |
| | | 144 | | { |
| | 55 | 145 | | if (!policy.IsTraversable(current, unitSize)) |
| | | 146 | | continue; |
| | | 147 | | |
| | 5 | 148 | | voxel = current; |
| | 5 | 149 | | return true; |
| | | 150 | | } |
| | | 151 | | } |
| | | 152 | | |
| | 6 | 153 | | voxel = null; |
| | 6 | 154 | | return false; |
| | 5 | 155 | | } |
| | | 156 | | } |