| | | 1 | | using FixedMathSharp; |
| | | 2 | | using GridForge.Grids; |
| | | 3 | | using System.Diagnostics.CodeAnalysis; |
| | | 4 | | using Trailblazer.Pathing; |
| | | 5 | | |
| | | 6 | | namespace Trailblazer.Navigation; |
| | | 7 | | |
| | | 8 | | /// <summary> |
| | | 9 | | /// Creates built-in path requests for navigators from host-facing guided travel commands. |
| | | 10 | | /// </summary> |
| | | 11 | | public static class NavigatorPathRequestFactory |
| | | 12 | | { |
| | | 13 | | internal static bool TryCreate( |
| | | 14 | | TrailblazerWorldContext context, |
| | | 15 | | Vector3d origin, |
| | | 16 | | Vector3d targetPosition, |
| | | 17 | | Fixed64 unitSize, |
| | | 18 | | SolidPathAlgorithm pathMode, |
| | | 19 | | bool allowUnwalkableEndpoints, |
| | | 20 | | bool allowTraversalTransitions, |
| | | 21 | | Fixed64 maxClimbHeight, |
| | | 22 | | TraversalMedium traversalMedium, |
| | | 23 | | HeuristicMethod aStarHeuristic, |
| | | 24 | | int flowFieldExtraFloodRange, |
| | | 25 | | [NotNullWhen(true)] out IPathRequest? request, |
| | | 26 | | out GuidedVolumeExitHandoff? handoff) |
| | | 27 | | { |
| | 80 | 28 | | PathRequestContextResolver.ThrowIfUnusable(context); |
| | 80 | 29 | | handoff = null; |
| | | 30 | | |
| | | 31 | | // For gas and liquid traversal, we only support volume path requests, |
| | | 32 | | // so we bypass the path mode switch and go straight to trying to create a volume request. |
| | | 33 | | // If that fails and traversal transitions are allowed, |
| | | 34 | | // we attempt to create a guided volume exit handoff request which will plan a |
| | | 35 | | // volume path to an exit if needed before transitioning to a chart-based path for |
| | | 36 | | // the remainder of the journey. |
| | 80 | 37 | | if (traversalMedium == TraversalMedium.Gas || traversalMedium == TraversalMedium.Liquid) |
| | | 38 | | { |
| | 44 | 39 | | VolumePathRequest? volume = VolumePathRequest.Create( |
| | 44 | 40 | | context, |
| | 44 | 41 | | origin, |
| | 44 | 42 | | targetPosition, |
| | 44 | 43 | | unitSize, |
| | 44 | 44 | | aStarHeuristic, |
| | 44 | 45 | | allowUnwalkableEndpoints, |
| | 44 | 46 | | traversalMedium); |
| | 44 | 47 | | if (volume == null) |
| | 19 | 48 | | return TryCreateVolumeExitHandoff( |
| | 19 | 49 | | context, |
| | 19 | 50 | | origin, |
| | 19 | 51 | | targetPosition, |
| | 19 | 52 | | unitSize, |
| | 19 | 53 | | traversalMedium, |
| | 19 | 54 | | pathMode, |
| | 19 | 55 | | allowUnwalkableEndpoints, |
| | 19 | 56 | | allowTraversalTransitions, |
| | 19 | 57 | | maxClimbHeight, |
| | 19 | 58 | | aStarHeuristic, |
| | 19 | 59 | | flowFieldExtraFloodRange, |
| | 19 | 60 | | out request, |
| | 19 | 61 | | out handoff, |
| | 19 | 62 | | out _); |
| | | 63 | | |
| | 25 | 64 | | if (TryCreateVolumeExitHandoffIfNeeded( |
| | 25 | 65 | | context, |
| | 25 | 66 | | targetPosition, |
| | 25 | 67 | | traversalMedium, |
| | 25 | 68 | | volume, |
| | 25 | 69 | | pathMode, |
| | 25 | 70 | | allowUnwalkableEndpoints, |
| | 25 | 71 | | allowTraversalTransitions, |
| | 25 | 72 | | maxClimbHeight, |
| | 25 | 73 | | aStarHeuristic, |
| | 25 | 74 | | flowFieldExtraFloodRange, |
| | 25 | 75 | | out request, |
| | 25 | 76 | | out handoff)) |
| | | 77 | | { |
| | 4 | 78 | | return true; |
| | | 79 | | } |
| | | 80 | | |
| | 21 | 81 | | request = volume; |
| | 21 | 82 | | return true; |
| | | 83 | | } |
| | | 84 | | |
| | | 85 | | switch (pathMode) |
| | | 86 | | { |
| | | 87 | | case SolidPathAlgorithm.AStar: |
| | 27 | 88 | | return TryCreateAStarRequest( |
| | 27 | 89 | | context, |
| | 27 | 90 | | origin, targetPosition, unitSize, |
| | 27 | 91 | | aStarHeuristic, allowUnwalkableEndpoints, allowTraversalTransitions, |
| | 27 | 92 | | maxClimbHeight, out request); |
| | | 93 | | |
| | | 94 | | case SolidPathAlgorithm.FlowField: |
| | 8 | 95 | | return TryCreateFlowFieldRequest( |
| | 8 | 96 | | context, |
| | 8 | 97 | | origin, targetPosition, unitSize, |
| | 8 | 98 | | allowUnwalkableEndpoints, allowTraversalTransitions, |
| | 8 | 99 | | maxClimbHeight, flowFieldExtraFloodRange, out request); |
| | | 100 | | |
| | | 101 | | default: |
| | 1 | 102 | | request = null; |
| | 1 | 103 | | return false; |
| | | 104 | | } |
| | | 105 | | } |
| | | 106 | | |
| | | 107 | | private static bool TryCreateVolumeExitHandoff( |
| | | 108 | | TrailblazerWorldContext context, |
| | | 109 | | Vector3d origin, |
| | | 110 | | Vector3d targetPosition, |
| | | 111 | | Fixed64 unitSize, |
| | | 112 | | TraversalMedium medium, |
| | | 113 | | SolidPathAlgorithm chartPathMode, |
| | | 114 | | bool allowUnwalkableEndpoints, |
| | | 115 | | bool allowTraversalTransitions, |
| | | 116 | | Fixed64 maxClimbHeight, |
| | | 117 | | HeuristicMethod aStarHeuristic, |
| | | 118 | | int flowFieldExtraFloodRange, |
| | | 119 | | [NotNullWhen(true)] out IPathRequest? request, |
| | | 120 | | out GuidedVolumeExitHandoff? handoff, |
| | | 121 | | out int totalPathCost) |
| | | 122 | | { |
| | 26 | 123 | | request = null; |
| | 26 | 124 | | handoff = null; |
| | 26 | 125 | | totalPathCost = 0; |
| | | 126 | | |
| | 26 | 127 | | if (!allowTraversalTransitions) |
| | 4 | 128 | | return false; |
| | | 129 | | |
| | 22 | 130 | | return GuidedVolumeExitPlanner.TryPlan( |
| | 22 | 131 | | context, |
| | 22 | 132 | | origin, |
| | 22 | 133 | | targetPosition, |
| | 22 | 134 | | unitSize, |
| | 22 | 135 | | medium, |
| | 22 | 136 | | chartPathMode, |
| | 22 | 137 | | allowUnwalkableEndpoints, |
| | 22 | 138 | | allowTraversalTransitions, |
| | 22 | 139 | | maxClimbHeight, |
| | 22 | 140 | | aStarHeuristic, |
| | 22 | 141 | | flowFieldExtraFloodRange, |
| | 22 | 142 | | out VolumePathRequest? volumeRequest, |
| | 22 | 143 | | out handoff, |
| | 22 | 144 | | out totalPathCost) |
| | 22 | 145 | | && volumeRequest != null |
| | 22 | 146 | | && (request = volumeRequest) != null; |
| | | 147 | | } |
| | | 148 | | |
| | | 149 | | private static bool TryCreateVolumeExitHandoffIfNeeded( |
| | | 150 | | TrailblazerWorldContext context, |
| | | 151 | | Vector3d targetPosition, |
| | | 152 | | TraversalMedium medium, |
| | | 153 | | VolumePathRequest directRequest, |
| | | 154 | | SolidPathAlgorithm chartPathMode, |
| | | 155 | | bool allowUnwalkableEndpoints, |
| | | 156 | | bool allowTraversalTransitions, |
| | | 157 | | Fixed64 maxClimbHeight, |
| | | 158 | | HeuristicMethod aStarHeuristic, |
| | | 159 | | int flowFieldExtraFloodRange, |
| | | 160 | | [NotNullWhen(true)] out IPathRequest? request, |
| | | 161 | | out GuidedVolumeExitHandoff? handoff) |
| | | 162 | | { |
| | 25 | 163 | | request = null; |
| | 25 | 164 | | handoff = null; |
| | | 165 | | |
| | 25 | 166 | | if (directRequest == null |
| | 25 | 167 | | || !allowTraversalTransitions |
| | 25 | 168 | | || !TryGetChartBackedTargetState( |
| | 25 | 169 | | context, |
| | 25 | 170 | | targetPosition, |
| | 25 | 171 | | medium, |
| | 25 | 172 | | out bool targetRequiresConstrainedExitHandoff)) |
| | | 173 | | { |
| | 16 | 174 | | return false; |
| | | 175 | | } |
| | | 176 | | |
| | 9 | 177 | | if (!targetRequiresConstrainedExitHandoff |
| | 9 | 178 | | && !TryCreateGasLandingHandoff( |
| | 9 | 179 | | directRequest, |
| | 9 | 180 | | context, |
| | 9 | 181 | | targetPosition, |
| | 9 | 182 | | medium, |
| | 9 | 183 | | chartPathMode, |
| | 9 | 184 | | allowUnwalkableEndpoints, |
| | 9 | 185 | | allowTraversalTransitions, |
| | 9 | 186 | | maxClimbHeight, |
| | 9 | 187 | | aStarHeuristic, |
| | 9 | 188 | | flowFieldExtraFloodRange, |
| | 9 | 189 | | out request, |
| | 9 | 190 | | out handoff)) |
| | | 191 | | { |
| | 5 | 192 | | return false; |
| | | 193 | | } |
| | | 194 | | |
| | 4 | 195 | | if (request != null) |
| | 2 | 196 | | return true; |
| | | 197 | | |
| | 2 | 198 | | return TryCreateVolumeExitHandoff( |
| | 2 | 199 | | context, |
| | 2 | 200 | | directRequest.Origin, |
| | 2 | 201 | | targetPosition, |
| | 2 | 202 | | directRequest.UnitSize, |
| | 2 | 203 | | medium, |
| | 2 | 204 | | chartPathMode, |
| | 2 | 205 | | allowUnwalkableEndpoints, |
| | 2 | 206 | | allowTraversalTransitions, |
| | 2 | 207 | | maxClimbHeight, |
| | 2 | 208 | | aStarHeuristic, |
| | 2 | 209 | | flowFieldExtraFloodRange, |
| | 2 | 210 | | out request, |
| | 2 | 211 | | out handoff, |
| | 2 | 212 | | out _); |
| | | 213 | | } |
| | | 214 | | |
| | | 215 | | private static bool TryGetChartBackedTargetState( |
| | | 216 | | TrailblazerWorldContext context, |
| | | 217 | | Vector3d targetPosition, |
| | | 218 | | TraversalMedium medium, |
| | | 219 | | out bool targetRequiresConstrainedExitHandoff) |
| | | 220 | | { |
| | 17 | 221 | | targetRequiresConstrainedExitHandoff = false; |
| | | 222 | | |
| | 17 | 223 | | if (!context.World.TryGetVoxel(targetPosition, out Voxel? targetVoxel) |
| | 17 | 224 | | || targetVoxel == null) |
| | 1 | 225 | | return false; |
| | | 226 | | |
| | 16 | 227 | | if (targetVoxel.TryGetPartition(out SolidChartPartition? _) != true) |
| | 7 | 228 | | return false; |
| | | 229 | | |
| | 9 | 230 | | targetRequiresConstrainedExitHandoff = !VolumeMediumRules.Matches(context.Pathing.State, targetVoxel, medium); |
| | 9 | 231 | | return true; |
| | | 232 | | } |
| | | 233 | | |
| | | 234 | | private static bool TryCreateGasLandingHandoff( |
| | | 235 | | VolumePathRequest directRequest, |
| | | 236 | | TrailblazerWorldContext context, |
| | | 237 | | Vector3d targetPosition, |
| | | 238 | | TraversalMedium medium, |
| | | 239 | | SolidPathAlgorithm chartPathMode, |
| | | 240 | | bool allowUnwalkableEndpoints, |
| | | 241 | | bool allowTraversalTransitions, |
| | | 242 | | Fixed64 maxClimbHeight, |
| | | 243 | | HeuristicMethod aStarHeuristic, |
| | | 244 | | int flowFieldExtraFloodRange, |
| | | 245 | | [NotNullWhen(true)] out IPathRequest? request, |
| | | 246 | | out GuidedVolumeExitHandoff? handoff) |
| | | 247 | | { |
| | 7 | 248 | | request = null; |
| | 7 | 249 | | handoff = null; |
| | | 250 | | |
| | 7 | 251 | | if (medium != TraversalMedium.Gas) |
| | 2 | 252 | | return false; |
| | | 253 | | |
| | 5 | 254 | | if (!TryCreateVolumeExitHandoff( |
| | 5 | 255 | | context, |
| | 5 | 256 | | directRequest.Origin, |
| | 5 | 257 | | targetPosition, |
| | 5 | 258 | | directRequest.UnitSize, |
| | 5 | 259 | | medium, |
| | 5 | 260 | | chartPathMode, |
| | 5 | 261 | | allowUnwalkableEndpoints, |
| | 5 | 262 | | allowTraversalTransitions, |
| | 5 | 263 | | maxClimbHeight, |
| | 5 | 264 | | aStarHeuristic, |
| | 5 | 265 | | flowFieldExtraFloodRange, |
| | 5 | 266 | | out IPathRequest? plannedRequest, |
| | 5 | 267 | | out GuidedVolumeExitHandoff? plannedHandoff, |
| | 5 | 268 | | out int handoffPathCost)) |
| | | 269 | | { |
| | 1 | 270 | | return false; |
| | | 271 | | } |
| | | 272 | | |
| | 4 | 273 | | if (plannedRequest == null || plannedHandoff == null || directRequest.EndNode == null) |
| | 0 | 274 | | return false; |
| | | 275 | | |
| | 4 | 276 | | if (directRequest.EndNode.WorldPosition != targetPosition) |
| | | 277 | | { |
| | 1 | 278 | | request = plannedRequest; |
| | 1 | 279 | | handoff = plannedHandoff; |
| | 1 | 280 | | return true; |
| | | 281 | | } |
| | | 282 | | |
| | 3 | 283 | | if (handoffPathCost < GetDirectVolumePathCost(directRequest)) |
| | | 284 | | { |
| | 1 | 285 | | request = plannedRequest; |
| | 1 | 286 | | handoff = plannedHandoff; |
| | 1 | 287 | | return true; |
| | | 288 | | } |
| | | 289 | | |
| | 2 | 290 | | return false; |
| | | 291 | | } |
| | | 292 | | |
| | | 293 | | private static bool TryCreateAStarRequest( |
| | | 294 | | TrailblazerWorldContext context, |
| | | 295 | | Vector3d origin, |
| | | 296 | | Vector3d targetPosition, |
| | | 297 | | Fixed64 unitSize, |
| | | 298 | | HeuristicMethod heuristic, |
| | | 299 | | bool allowUnwalkableEndpoints, |
| | | 300 | | bool allowTraversalTransitions, |
| | | 301 | | Fixed64 maxClimbHeight, |
| | | 302 | | [NotNullWhen(true)] out IPathRequest? request) |
| | | 303 | | { |
| | 27 | 304 | | AStarPathRequest? aStar = AStarPathRequest.Create( |
| | 27 | 305 | | context, |
| | 27 | 306 | | origin, |
| | 27 | 307 | | targetPosition, |
| | 27 | 308 | | unitSize, |
| | 27 | 309 | | heuristic, |
| | 27 | 310 | | allowUnwalkableEndpoints, |
| | 27 | 311 | | allowTraversalTransitions); |
| | 27 | 312 | | if (aStar == null) |
| | | 313 | | { |
| | 3 | 314 | | request = null; |
| | 3 | 315 | | return false; |
| | | 316 | | } |
| | | 317 | | |
| | 24 | 318 | | aStar.MaxClimbHeight = maxClimbHeight; |
| | 24 | 319 | | request = aStar; |
| | 24 | 320 | | return true; |
| | | 321 | | } |
| | | 322 | | |
| | | 323 | | private static bool TryCreateFlowFieldRequest( |
| | | 324 | | TrailblazerWorldContext context, |
| | | 325 | | Vector3d origin, |
| | | 326 | | Vector3d targetPosition, |
| | | 327 | | Fixed64 unitSize, |
| | | 328 | | bool allowUnwalkableEndpoints, |
| | | 329 | | bool allowTraversalTransitions, |
| | | 330 | | Fixed64 maxClimbHeight, |
| | | 331 | | int flowFieldExtraFloodRange, |
| | | 332 | | [NotNullWhen(true)] out IPathRequest? request) |
| | | 333 | | { |
| | 8 | 334 | | FlowFieldPathRequest? flowField = FlowFieldPathRequest.Create( |
| | 8 | 335 | | context, |
| | 8 | 336 | | origin, |
| | 8 | 337 | | targetPosition, |
| | 8 | 338 | | unitSize, |
| | 8 | 339 | | allowUnwalkableEndpoints, |
| | 8 | 340 | | allowTraversalTransitions); |
| | 8 | 341 | | if (flowField == null) |
| | | 342 | | { |
| | 2 | 343 | | request = null; |
| | 2 | 344 | | return false; |
| | | 345 | | } |
| | | 346 | | |
| | 6 | 347 | | flowField.MaxClimbHeight = maxClimbHeight; |
| | 6 | 348 | | flowField.ExtraFloodRange = flowFieldExtraFloodRange; |
| | 6 | 349 | | request = flowField; |
| | 6 | 350 | | return true; |
| | | 351 | | } |
| | | 352 | | |
| | | 353 | | private static int GetDirectVolumePathCost(VolumePathRequest request) |
| | | 354 | | { |
| | 3 | 355 | | if (request.HasZeroDisplacement) |
| | 1 | 356 | | return 0; |
| | | 357 | | |
| | 2 | 358 | | VolumeSurveyResult result = request.Context.Pathing.State.GuideState.VolumeSurveyor.FindPath(request); |
| | 2 | 359 | | return result.HasPath && result.Waypoints != null && result.Waypoints.Length > 0 |
| | 2 | 360 | | ? result.Waypoints[^1].PathCost |
| | 2 | 361 | | : int.MaxValue; |
| | | 362 | | } |
| | | 363 | | } |