| | | 1 | | //======================================================================= |
| | | 2 | | // NavigationSelectedEdgeProgressWork.cs |
| | | 3 | | //======================================================================= |
| | | 4 | | // MIT License, Copyright (c) 2024-present David Oravsky (mrdav30) |
| | | 5 | | // See LICENSE file in the project root for full license information. |
| | | 6 | | //======================================================================= |
| | | 7 | | |
| | | 8 | | using System; |
| | | 9 | | using FixedMathSharp; |
| | | 10 | | using GridForge.Grids; |
| | | 11 | | using GridForge.Grids.Topology; |
| | | 12 | | |
| | | 13 | | namespace Trailblazer.Pathing; |
| | | 14 | | |
| | | 15 | | internal readonly struct NavigationFlowRejoinTarget |
| | | 16 | | { |
| | | 17 | | internal NavigationFlowRejoinTarget( |
| | | 18 | | Vector3d position, |
| | | 19 | | NavigationRayChainConstraint constraint) |
| | | 20 | | { |
| | 309 | 21 | | Position = position; |
| | 309 | 22 | | Constraint = constraint; |
| | 309 | 23 | | } |
| | | 24 | | |
| | | 25 | | internal Vector3d Position { get; } |
| | | 26 | | |
| | | 27 | | internal NavigationRayChainConstraint Constraint { get; } |
| | | 28 | | } |
| | | 29 | | |
| | | 30 | | /// <summary>Samples only the canonical selected edge stored by one flow node.</summary> |
| | | 31 | | internal enum NavigationFlowNodeLookupStatus : byte |
| | | 32 | | { |
| | | 33 | | Success, |
| | | 34 | | NotFound, |
| | | 35 | | BudgetExceeded |
| | | 36 | | } |
| | | 37 | | |
| | | 38 | | internal static class NavigationSelectedEdgeProgressWork |
| | | 39 | | { |
| | | 40 | | internal static NavigationGuideStatus MapNodeLookupStatus( |
| | | 41 | | NavigationFlowNodeLookupStatus status, |
| | | 42 | | bool required) => status switch |
| | | 43 | | { |
| | | 44 | | NavigationFlowNodeLookupStatus.Success => NavigationGuideStatus.Success, |
| | | 45 | | NavigationFlowNodeLookupStatus.BudgetExceeded => |
| | | 46 | | NavigationGuideStatus.BudgetExceeded, |
| | | 47 | | _ => required ? NavigationGuideStatus.Stale : NavigationGuideStatus.Success |
| | | 48 | | }; |
| | | 49 | | |
| | | 50 | | internal static NavigationGuideStatus MapStructuralEdgeStatus( |
| | | 51 | | NavigationSurfaceEdgeAdvanceStatus status) => status switch |
| | | 52 | | { |
| | | 53 | | NavigationSurfaceEdgeAdvanceStatus.Pending => NavigationGuideStatus.Success, |
| | | 54 | | NavigationSurfaceEdgeAdvanceStatus.Edge => NavigationGuideStatus.Success, |
| | | 55 | | NavigationSurfaceEdgeAdvanceStatus.Blocked => NavigationGuideStatus.BudgetExceeded, |
| | | 56 | | _ => NavigationGuideStatus.Stale |
| | | 57 | | }; |
| | | 58 | | |
| | | 59 | | internal static bool TryGetRejoinTarget( |
| | | 60 | | NavigationCellAddress sourceAddress, |
| | | 61 | | NavigationNodeState sourceState, |
| | | 62 | | NavigationNodeState targetState, |
| | | 63 | | NavigationSelectedEdgeRef selected, |
| | | 64 | | Vector3d selectedExitTarget, |
| | | 65 | | int targetOrdinal, |
| | | 66 | | out NavigationFlowRejoinTarget target) |
| | | 67 | | { |
| | | 68 | | SwiftThrowHelper.ThrowIfNegative(targetOrdinal, nameof(targetOrdinal)); |
| | | 69 | | target = default; |
| | | 70 | | if (targetOrdinal == 0) |
| | | 71 | | { |
| | | 72 | | target = new NavigationFlowRejoinTarget( |
| | | 73 | | sourceState.FootAnchor, |
| | | 74 | | NavigationRayChainConstraint.SourceOnly(sourceAddress)); |
| | | 75 | | return true; |
| | | 76 | | } |
| | | 77 | | |
| | | 78 | | NavigationRayChainConstraint constraint = |
| | | 79 | | NavigationRayChainConstraint.SelectedEdge( |
| | | 80 | | sourceAddress, |
| | | 81 | | selected.Target, |
| | | 82 | | selected.CanonicalOutgoingOrdinal); |
| | | 83 | | if (targetOrdinal == 1) |
| | | 84 | | { |
| | | 85 | | target = new NavigationFlowRejoinTarget(selectedExitTarget, constraint); |
| | | 86 | | return true; |
| | | 87 | | } |
| | | 88 | | if (targetOrdinal == 2) |
| | | 89 | | { |
| | | 90 | | target = new NavigationFlowRejoinTarget(targetState.FootAnchor, constraint); |
| | | 91 | | return true; |
| | | 92 | | } |
| | | 93 | | return false; |
| | | 94 | | } |
| | | 95 | | |
| | | 96 | | internal static NavigationGuideStatus TrySample( |
| | | 97 | | bool dependencyCurrent, |
| | | 98 | | GridWorld world, |
| | | 99 | | NavigationWorldGraphStore store, |
| | | 100 | | NavigationWorldGraph graph, |
| | | 101 | | NavigationFlowFieldPayload payload, |
| | | 102 | | NavigationCellAddress sourceAddress, |
| | | 103 | | TraversalMedium medium, |
| | | 104 | | Vector3d actualFootPosition, |
| | | 105 | | ref GuideSampleWorkMeter meter, |
| | | 106 | | GridCoveredAddressCursor coveredAddressCursor, |
| | | 107 | | GridCoveredAddressGeneration[] coveredAddressGenerations, |
| | | 108 | | GridCoveredAddress[] coveredAddressOutput, |
| | | 109 | | NavigationImmediateRayWorkspace immediateRayWorkspace, |
| | | 110 | | out NavigationFlowFieldNode currentNode, |
| | | 111 | | out NavigationCellAddress nextSourceAddress, |
| | | 112 | | out Vector3d target, |
| | | 113 | | out Vector3d heading) |
| | | 114 | | { |
| | | 115 | | currentNode = default; |
| | | 116 | | nextSourceAddress = sourceAddress; |
| | | 117 | | target = default; |
| | | 118 | | heading = Vector3d.Zero; |
| | | 119 | | if (!dependencyCurrent) |
| | | 120 | | return NavigationGuideStatus.Stale; |
| | | 121 | | if (medium is TraversalMedium.Gas or TraversalMedium.Liquid) |
| | | 122 | | { |
| | | 123 | | return TrySampleVolume( |
| | | 124 | | world, |
| | | 125 | | store, |
| | | 126 | | graph, |
| | | 127 | | payload, |
| | | 128 | | sourceAddress, |
| | | 129 | | medium, |
| | | 130 | | actualFootPosition, |
| | | 131 | | ref meter, |
| | | 132 | | immediateRayWorkspace, |
| | | 133 | | out currentNode, |
| | | 134 | | out nextSourceAddress, |
| | | 135 | | out target, |
| | | 136 | | out heading); |
| | | 137 | | } |
| | | 138 | | NavigationCellAddress currentSource = sourceAddress; |
| | | 139 | | bool allowRecovery = true; |
| | | 140 | | while (true) |
| | | 141 | | { |
| | | 142 | | NavigationFlowNodeLookupStatus lookup = TryGetNode( |
| | | 143 | | payload, |
| | | 144 | | currentSource, |
| | | 145 | | medium, |
| | | 146 | | ref meter, |
| | | 147 | | out NavigationFlowFieldNode node); |
| | | 148 | | if (lookup != NavigationFlowNodeLookupStatus.Success) |
| | | 149 | | { |
| | | 150 | | return MapNodeLookupStatus(lookup, required: true); |
| | | 151 | | } |
| | | 152 | | currentNode = node; |
| | | 153 | | if (node.TransitionInstructionOrdinal >= 0) |
| | | 154 | | { |
| | | 155 | | nextSourceAddress = currentSource; |
| | | 156 | | return NavigationGuideStatus.Success; |
| | | 157 | | } |
| | | 158 | | graph.TryGetNodeRef(currentSource, out NavigationNodeRef sourceRef); |
| | | 159 | | graph.TryGetNodeState( |
| | | 160 | | sourceRef, |
| | | 161 | | medium, |
| | | 162 | | out NavigationNodeState sourceState); |
| | | 163 | | graph.TryGetSeamPrism(currentSource, out GridCellPrism sourcePrism); |
| | | 164 | | if (!meter.TryConsumePrismChecks(1)) |
| | | 165 | | return NavigationGuideStatus.BudgetExceeded; |
| | | 166 | | bool sourceContains = sourcePrism.Contains(actualFootPosition); |
| | | 167 | | target = sourceState.FootAnchor; |
| | | 168 | | if (currentSource == payload.Key.DestinationAddress) |
| | | 169 | | { |
| | | 170 | | if (sourceContains |
| | | 171 | | && GridCellGeometry.IsNavigationBodyAnchorValid( |
| | | 172 | | sourcePrism, |
| | | 173 | | actualFootPosition, |
| | | 174 | | payload.Key.Agent.Shape.Radius, |
| | | 175 | | payload.Key.Agent.Shape.Height, |
| | | 176 | | default)) |
| | | 177 | | { |
| | | 178 | | nextSourceAddress = currentSource; |
| | | 179 | | if (IsWithinArrivalRadius( |
| | | 180 | | actualFootPosition, |
| | | 181 | | sourceState.FootAnchor, |
| | | 182 | | payload.Key.Agent.ArrivalRadius)) |
| | | 183 | | { |
| | | 184 | | heading = Vector3d.Zero; |
| | | 185 | | return NavigationGuideStatus.Success; |
| | | 186 | | } |
| | | 187 | | return TrySetHeading( |
| | | 188 | | actualFootPosition, |
| | | 189 | | sourceState.FootAnchor, |
| | | 190 | | ref meter, |
| | | 191 | | out heading); |
| | | 192 | | } |
| | | 193 | | NavigationGuideStatus destinationRecovery = TryAdvanceRecovery( |
| | | 194 | | world, |
| | | 195 | | store, |
| | | 196 | | graph, |
| | | 197 | | payload, |
| | | 198 | | medium, |
| | | 199 | | actualFootPosition, |
| | | 200 | | ref meter, |
| | | 201 | | coveredAddressCursor, |
| | | 202 | | coveredAddressGenerations, |
| | | 203 | | coveredAddressOutput, |
| | | 204 | | immediateRayWorkspace, |
| | | 205 | | currentSource, |
| | | 206 | | sourceState, |
| | | 207 | | default, |
| | | 208 | | Vector3d.Zero, |
| | | 209 | | default, |
| | | 210 | | hasSelectedEdge: false, |
| | | 211 | | allowRecovery, |
| | | 212 | | out currentSource, |
| | | 213 | | out Vector3d destinationRejoinHeading, |
| | | 214 | | out bool destinationRejoined); |
| | | 215 | | if (destinationRecovery != NavigationGuideStatus.Success) |
| | | 216 | | return destinationRecovery; |
| | | 217 | | if (destinationRejoined) |
| | | 218 | | { |
| | | 219 | | nextSourceAddress = sourceAddress; |
| | | 220 | | heading = destinationRejoinHeading; |
| | | 221 | | return NavigationGuideStatus.Success; |
| | | 222 | | } |
| | | 223 | | allowRecovery = false; |
| | | 224 | | continue; |
| | | 225 | | } |
| | | 226 | | |
| | | 227 | | NavigationGuideStatus edgeStatus = TryResolveSelectedEdge( |
| | | 228 | | graph, |
| | | 229 | | sourceRef, |
| | | 230 | | node.SelectedEdge, |
| | | 231 | | ref meter, |
| | | 232 | | out NavigationGraphEdge edge); |
| | | 233 | | if (edgeStatus != NavigationGuideStatus.Success) |
| | | 234 | | return edgeStatus; |
| | | 235 | | graph.TryGetNodeState( |
| | | 236 | | edge.Target, |
| | | 237 | | medium, |
| | | 238 | | out NavigationNodeState targetState); |
| | | 239 | | graph.TryGetSeamPrism( |
| | | 240 | | node.SelectedEdge.Target, |
| | | 241 | | out GridCellPrism targetPrism); |
| | | 242 | | if (!meter.TryConsumePrismChecks(1)) |
| | | 243 | | return NavigationGuideStatus.BudgetExceeded; |
| | | 244 | | bool targetContains = targetPrism.Contains(actualFootPosition); |
| | | 245 | | target = targetState.FootAnchor; |
| | | 246 | | |
| | | 247 | | if (edge.Kind == NavigationGraphEdgeKind.Explicit) |
| | | 248 | | { |
| | | 249 | | NavigationGuideStatus explicitStatus = TrySampleExplicit( |
| | | 250 | | graph, |
| | | 251 | | edge.ExplicitConnection, |
| | | 252 | | currentSource, |
| | | 253 | | sourceState, |
| | | 254 | | sourcePrism, |
| | | 255 | | targetState, |
| | | 256 | | targetPrism, |
| | | 257 | | sourceContains, |
| | | 258 | | targetContains, |
| | | 259 | | actualFootPosition, |
| | | 260 | | payload.Key.Agent.Shape, |
| | | 261 | | node.SelectedEdge.Target == payload.Key.DestinationAddress, |
| | | 262 | | payload.Key.Agent.ArrivalRadius, |
| | | 263 | | ref meter, |
| | | 264 | | out NavigationCellAddress explicitSource, |
| | | 265 | | out heading); |
| | | 266 | | if (explicitStatus == NavigationGuideStatus.LocalRecoveryRequired) |
| | | 267 | | { |
| | | 268 | | NavigationGuideStatus recovery = TryAdvanceRecovery( |
| | | 269 | | world, |
| | | 270 | | store, |
| | | 271 | | graph, |
| | | 272 | | payload, |
| | | 273 | | medium, |
| | | 274 | | actualFootPosition, |
| | | 275 | | ref meter, |
| | | 276 | | coveredAddressCursor, |
| | | 277 | | coveredAddressGenerations, |
| | | 278 | | coveredAddressOutput, |
| | | 279 | | immediateRayWorkspace, |
| | | 280 | | currentSource, |
| | | 281 | | sourceState, |
| | | 282 | | node.SelectedEdge, |
| | | 283 | | edge.ExplicitConnection.Definition.ExitAnchor, |
| | | 284 | | targetState, |
| | | 285 | | hasSelectedEdge: true, |
| | | 286 | | allowRecovery, |
| | | 287 | | out currentSource, |
| | | 288 | | out Vector3d rejoinHeading, |
| | | 289 | | out bool rejoined); |
| | | 290 | | if (recovery != NavigationGuideStatus.Success) |
| | | 291 | | return recovery; |
| | | 292 | | if (rejoined) |
| | | 293 | | { |
| | | 294 | | nextSourceAddress = sourceAddress; |
| | | 295 | | heading = rejoinHeading; |
| | | 296 | | return NavigationGuideStatus.Success; |
| | | 297 | | } |
| | | 298 | | allowRecovery = false; |
| | | 299 | | continue; |
| | | 300 | | } |
| | | 301 | | if (explicitStatus != NavigationGuideStatus.Success |
| | | 302 | | || explicitSource == currentSource) |
| | | 303 | | { |
| | | 304 | | nextSourceAddress = currentSource; |
| | | 305 | | return explicitStatus; |
| | | 306 | | } |
| | | 307 | | currentSource = explicitSource; |
| | | 308 | | continue; |
| | | 309 | | } |
| | | 310 | | |
| | | 311 | | NavigationGuideStatus portalStatus = TryResolvePortal( |
| | | 312 | | edge, |
| | | 313 | | sourceState, |
| | | 314 | | payload.Key.Agent.Shape, |
| | | 315 | | ref meter, |
| | | 316 | | out GridNavigationPortal selectedPortal, |
| | | 317 | | out Vector3d sourcePortal, |
| | | 318 | | out Vector3d targetPortal); |
| | | 319 | | if (portalStatus != NavigationGuideStatus.Success) |
| | | 320 | | return portalStatus; |
| | | 321 | | bool sourceBodyValid = sourceContains |
| | | 322 | | && GridCellGeometry.IsNavigationBodyAnchorValid( |
| | | 323 | | sourcePrism, |
| | | 324 | | actualFootPosition, |
| | | 325 | | payload.Key.Agent.Shape.Radius, |
| | | 326 | | payload.Key.Agent.Shape.Height, |
| | | 327 | | selectedPortal); |
| | | 328 | | bool targetBodyValid = targetContains |
| | | 329 | | && GridCellGeometry.IsNavigationBodyAnchorValid( |
| | | 330 | | targetPrism, |
| | | 331 | | actualFootPosition, |
| | | 332 | | payload.Key.Agent.Shape.Radius, |
| | | 333 | | payload.Key.Agent.Shape.Height, |
| | | 334 | | selectedPortal); |
| | | 335 | | if (!sourceBodyValid && !targetBodyValid) |
| | | 336 | | { |
| | | 337 | | NavigationGuideStatus recovery = TryAdvanceRecovery( |
| | | 338 | | world, |
| | | 339 | | store, |
| | | 340 | | graph, |
| | | 341 | | payload, |
| | | 342 | | medium, |
| | | 343 | | actualFootPosition, |
| | | 344 | | ref meter, |
| | | 345 | | coveredAddressCursor, |
| | | 346 | | coveredAddressGenerations, |
| | | 347 | | coveredAddressOutput, |
| | | 348 | | immediateRayWorkspace, |
| | | 349 | | currentSource, |
| | | 350 | | sourceState, |
| | | 351 | | node.SelectedEdge, |
| | | 352 | | targetPortal, |
| | | 353 | | targetState, |
| | | 354 | | hasSelectedEdge: true, |
| | | 355 | | allowRecovery, |
| | | 356 | | out currentSource, |
| | | 357 | | out Vector3d rejoinHeading, |
| | | 358 | | out bool rejoined); |
| | | 359 | | if (recovery != NavigationGuideStatus.Success) |
| | | 360 | | return recovery; |
| | | 361 | | if (rejoined) |
| | | 362 | | { |
| | | 363 | | nextSourceAddress = sourceAddress; |
| | | 364 | | heading = rejoinHeading; |
| | | 365 | | return NavigationGuideStatus.Success; |
| | | 366 | | } |
| | | 367 | | allowRecovery = false; |
| | | 368 | | continue; |
| | | 369 | | } |
| | | 370 | | if (targetBodyValid |
| | | 371 | | && node.SelectedEdge.Target == payload.Key.DestinationAddress |
| | | 372 | | && IsWithinArrivalRadius( |
| | | 373 | | actualFootPosition, |
| | | 374 | | targetState.FootAnchor, |
| | | 375 | | payload.Key.Agent.ArrivalRadius)) |
| | | 376 | | { |
| | | 377 | | nextSourceAddress = currentSource; |
| | | 378 | | heading = Vector3d.Zero; |
| | | 379 | | return NavigationGuideStatus.Success; |
| | | 380 | | } |
| | | 381 | | if (sourceBodyValid) |
| | | 382 | | { |
| | | 383 | | nextSourceAddress = currentSource; |
| | | 384 | | return TrySampleDirectedLeg( |
| | | 385 | | sourceState.FootAnchor, |
| | | 386 | | sourcePortal, |
| | | 387 | | targetPortal, |
| | | 388 | | targetState.FootAnchor, |
| | | 389 | | actualFootPosition, |
| | | 390 | | ref meter, |
| | | 391 | | out heading); |
| | | 392 | | } |
| | | 393 | | |
| | | 394 | | NavigationGuideStatus targetProgress = HasReachedOrPassed( |
| | | 395 | | targetPortal, |
| | | 396 | | targetState.FootAnchor, |
| | | 397 | | actualFootPosition, |
| | | 398 | | out bool passedTarget); |
| | | 399 | | if (targetProgress != NavigationGuideStatus.Success) |
| | | 400 | | return targetProgress; |
| | | 401 | | if (!passedTarget) |
| | | 402 | | { |
| | | 403 | | nextSourceAddress = currentSource; |
| | | 404 | | return TrySetHeading( |
| | | 405 | | actualFootPosition, |
| | | 406 | | targetState.FootAnchor, |
| | | 407 | | ref meter, |
| | | 408 | | out heading); |
| | | 409 | | } |
| | | 410 | | if (!meter.TryConsumeCursorRebases(1)) |
| | | 411 | | return NavigationGuideStatus.BudgetExceeded; |
| | | 412 | | currentSource = node.SelectedEdge.Target; |
| | | 413 | | } |
| | | 414 | | } |
| | | 415 | | |
| | | 416 | | private static NavigationGuideStatus TryResolveSelectedEdge( |
| | | 417 | | NavigationWorldGraph graph, |
| | | 418 | | NavigationNodeRef source, |
| | | 419 | | NavigationSelectedEdgeRef selected, |
| | | 420 | | ref GuideSampleWorkMeter meter, |
| | | 421 | | out NavigationGraphEdge edge) |
| | | 422 | | { |
| | | 423 | | NavigationSurfaceEdgeEnumerator edges = graph.EnumerateStructuralSurfaceEdges(source); |
| | | 424 | | int edgeStepRemaining = int.MaxValue; |
| | | 425 | | while (true) |
| | | 426 | | { |
| | | 427 | | NavigationSurfaceEdgeAdvanceStatus status = edges.AdvanceOne( |
| | | 428 | | ref meter, |
| | | 429 | | ref edgeStepRemaining); |
| | | 430 | | NavigationGuideStatus mappedStatus = MapStructuralEdgeStatus(status); |
| | | 431 | | if (mappedStatus != NavigationGuideStatus.Success) |
| | | 432 | | { |
| | | 433 | | edge = default; |
| | | 434 | | return mappedStatus; |
| | | 435 | | } |
| | | 436 | | if (status == NavigationSurfaceEdgeAdvanceStatus.Pending) |
| | | 437 | | continue; |
| | | 438 | | if (edges.CurrentOrdinal != selected.CanonicalOutgoingOrdinal) |
| | | 439 | | continue; |
| | | 440 | | edge = edges.Current; |
| | | 441 | | return NavigationGuideStatus.Success; |
| | | 442 | | } |
| | | 443 | | } |
| | | 444 | | |
| | | 445 | | private static NavigationGuideStatus TrySampleVolume( |
| | | 446 | | GridWorld world, |
| | | 447 | | NavigationWorldGraphStore store, |
| | | 448 | | NavigationWorldGraph graph, |
| | | 449 | | NavigationFlowFieldPayload payload, |
| | | 450 | | NavigationCellAddress sourceAddress, |
| | | 451 | | TraversalMedium medium, |
| | | 452 | | Vector3d actualFootPosition, |
| | | 453 | | ref GuideSampleWorkMeter meter, |
| | | 454 | | NavigationImmediateRayWorkspace immediateRayWorkspace, |
| | | 455 | | out NavigationFlowFieldNode currentNode, |
| | | 456 | | out NavigationCellAddress nextSourceAddress, |
| | | 457 | | out Vector3d target, |
| | | 458 | | out Vector3d heading) |
| | | 459 | | { |
| | | 460 | | currentNode = default; |
| | | 461 | | nextSourceAddress = sourceAddress; |
| | | 462 | | target = default; |
| | | 463 | | heading = Vector3d.Zero; |
| | | 464 | | NavigationCellAddress currentSource = sourceAddress; |
| | | 465 | | graph.AreaCatalog.TryGet( |
| | | 466 | | payload.Key.AreaPolicy, |
| | | 467 | | out NavigationAreaPolicy areaPolicy); |
| | | 468 | | while (true) |
| | | 469 | | { |
| | | 470 | | NavigationFlowNodeLookupStatus lookup = TryGetNode( |
| | | 471 | | payload, |
| | | 472 | | currentSource, |
| | | 473 | | medium, |
| | | 474 | | ref meter, |
| | | 475 | | out NavigationFlowFieldNode node); |
| | | 476 | | if (lookup != NavigationFlowNodeLookupStatus.Success) |
| | | 477 | | { |
| | | 478 | | return MapNodeLookupStatus(lookup, required: true); |
| | | 479 | | } |
| | | 480 | | currentNode = node; |
| | | 481 | | if (node.TransitionInstructionOrdinal >= 0) |
| | | 482 | | { |
| | | 483 | | nextSourceAddress = currentSource; |
| | | 484 | | return NavigationGuideStatus.Success; |
| | | 485 | | } |
| | | 486 | | graph.TryGetNodeRef(currentSource, out NavigationNodeRef sourceRef); |
| | | 487 | | graph.TryGetNodeState( |
| | | 488 | | sourceRef, |
| | | 489 | | medium, |
| | | 490 | | out NavigationNodeState sourceState); |
| | | 491 | | sourceState.TryGetCenteredVolumeFootAnchor( |
| | | 492 | | payload.Key.Agent.Shape.Height, |
| | | 493 | | out Vector3d sourceAnchor); |
| | | 494 | | target = sourceAnchor; |
| | | 495 | | if (!node.SelectedEdge.IsValid) |
| | | 496 | | { |
| | | 497 | | nextSourceAddress = currentSource; |
| | | 498 | | NavigationGuideStatus destination = RunRay( |
| | | 499 | | world, |
| | | 500 | | store, |
| | | 501 | | graph, |
| | | 502 | | payload, |
| | | 503 | | areaPolicy, |
| | | 504 | | medium, |
| | | 505 | | actualFootPosition, |
| | | 506 | | sourceAnchor, |
| | | 507 | | NavigationRayChainConstraint.SourceOnly(currentSource), |
| | | 508 | | ref meter, |
| | | 509 | | immediateRayWorkspace); |
| | | 510 | | if (destination != NavigationGuideStatus.Success) |
| | | 511 | | return destination; |
| | | 512 | | if (IsWithinArrivalRadius( |
| | | 513 | | actualFootPosition, |
| | | 514 | | sourceAnchor, |
| | | 515 | | payload.Key.Agent.ArrivalRadius)) |
| | | 516 | | { |
| | | 517 | | heading = Vector3d.Zero; |
| | | 518 | | return NavigationGuideStatus.Success; |
| | | 519 | | } |
| | | 520 | | return TrySetHeadingUnchecked(actualFootPosition, sourceAnchor, out heading); |
| | | 521 | | } |
| | | 522 | | graph.TryGetNodeRef( |
| | | 523 | | node.SelectedEdge.Target, |
| | | 524 | | out NavigationNodeRef targetRef); |
| | | 525 | | graph.TryGetNodeState( |
| | | 526 | | targetRef, |
| | | 527 | | medium, |
| | | 528 | | out NavigationNodeState targetState); |
| | | 529 | | targetState.TryGetCenteredVolumeFootAnchor( |
| | | 530 | | payload.Key.Agent.Shape.Height, |
| | | 531 | | out Vector3d targetAnchor); |
| | | 532 | | target = targetAnchor; |
| | | 533 | | if (actualFootPosition == targetAnchor) |
| | | 534 | | { |
| | | 535 | | NavigationGuideStatus arrived = RunRay( |
| | | 536 | | world, |
| | | 537 | | store, |
| | | 538 | | graph, |
| | | 539 | | payload, |
| | | 540 | | areaPolicy, |
| | | 541 | | medium, |
| | | 542 | | actualFootPosition, |
| | | 543 | | actualFootPosition, |
| | | 544 | | NavigationRayChainConstraint.SourceOnly(node.SelectedEdge.Target), |
| | | 545 | | ref meter, |
| | | 546 | | immediateRayWorkspace); |
| | | 547 | | if (arrived != NavigationGuideStatus.Success) |
| | | 548 | | return arrived; |
| | | 549 | | if (!meter.TryConsumeCursorRebases(1)) |
| | | 550 | | return NavigationGuideStatus.BudgetExceeded; |
| | | 551 | | currentSource = node.SelectedEdge.Target; |
| | | 552 | | nextSourceAddress = currentSource; |
| | | 553 | | continue; |
| | | 554 | | } |
| | | 555 | | |
| | | 556 | | NavigationGuideStatus status = RunRay( |
| | | 557 | | world, |
| | | 558 | | store, |
| | | 559 | | graph, |
| | | 560 | | payload, |
| | | 561 | | areaPolicy, |
| | | 562 | | medium, |
| | | 563 | | actualFootPosition, |
| | | 564 | | targetAnchor, |
| | | 565 | | NavigationRayChainConstraint.SelectedEdge( |
| | | 566 | | currentSource, |
| | | 567 | | node.SelectedEdge.Target, |
| | | 568 | | node.SelectedEdge.CanonicalOutgoingOrdinal), |
| | | 569 | | ref meter, |
| | | 570 | | immediateRayWorkspace); |
| | | 571 | | if (status != NavigationGuideStatus.Success) |
| | | 572 | | return status; |
| | | 573 | | nextSourceAddress = currentSource; |
| | | 574 | | if (node.SelectedEdge.Target == payload.Key.DestinationAddress |
| | | 575 | | && IsWithinArrivalRadius( |
| | | 576 | | actualFootPosition, |
| | | 577 | | targetAnchor, |
| | | 578 | | payload.Key.Agent.ArrivalRadius)) |
| | | 579 | | { |
| | | 580 | | heading = Vector3d.Zero; |
| | | 581 | | return NavigationGuideStatus.Success; |
| | | 582 | | } |
| | | 583 | | return TrySetHeadingUnchecked(actualFootPosition, targetAnchor, out heading); |
| | | 584 | | } |
| | | 585 | | } |
| | | 586 | | |
| | | 587 | | private static bool IsWithinArrivalRadius( |
| | | 588 | | Vector3d actualFootPosition, |
| | | 589 | | Vector3d target, |
| | | 590 | | Fixed64 arrivalRadius) => |
| | | 591 | | Vector3d.CompareDistanceSquared( |
| | | 592 | | actualFootPosition, |
| | | 593 | | target, |
| | | 594 | | Vector3d.Zero, |
| | | 595 | | new Vector3d(arrivalRadius, Fixed64.Zero, Fixed64.Zero)) <= 0; |
| | | 596 | | |
| | | 597 | | internal static NavigationGuideStatus TrySampleTransitionApproach( |
| | | 598 | | GridWorld world, |
| | | 599 | | NavigationWorldGraphStore store, |
| | | 600 | | NavigationWorldGraph graph, |
| | | 601 | | NavigationFlowFieldPayload payload, |
| | | 602 | | NavigationCellAddress source, |
| | | 603 | | TraversalMedium medium, |
| | | 604 | | Vector3d actualFootPosition, |
| | | 605 | | Vector3d actionPosition, |
| | | 606 | | ref GuideSampleWorkMeter meter, |
| | | 607 | | NavigationImmediateRayWorkspace immediateRayWorkspace, |
| | | 608 | | out Vector3d heading) |
| | | 609 | | { |
| | | 610 | | heading = Vector3d.Zero; |
| | | 611 | | bool alreadyAtAction = actualFootPosition == actionPosition; |
| | | 612 | | graph.AreaCatalog.TryGet( |
| | | 613 | | payload.Key.AreaPolicy, |
| | | 614 | | out NavigationAreaPolicy areaPolicy); |
| | | 615 | | NavigationGuideStatus status = RunRay( |
| | | 616 | | world, |
| | | 617 | | store, |
| | | 618 | | graph, |
| | | 619 | | payload, |
| | | 620 | | areaPolicy, |
| | | 621 | | medium, |
| | | 622 | | actualFootPosition, |
| | | 623 | | actionPosition, |
| | | 624 | | NavigationRayChainConstraint.SourceOnly(source), |
| | | 625 | | ref meter, |
| | | 626 | | immediateRayWorkspace); |
| | | 627 | | if (alreadyAtAction && status == NavigationGuideStatus.LocalRecoveryRequired) |
| | | 628 | | return NavigationGuideStatus.Success; |
| | | 629 | | return status == NavigationGuideStatus.Success |
| | | 630 | | ? TrySetHeadingUnchecked(actualFootPosition, actionPosition, out heading) |
| | | 631 | | : status; |
| | | 632 | | } |
| | | 633 | | |
| | | 634 | | private static NavigationGuideStatus RunRay( |
| | | 635 | | GridWorld world, |
| | | 636 | | NavigationWorldGraphStore store, |
| | | 637 | | NavigationWorldGraph graph, |
| | | 638 | | NavigationFlowFieldPayload payload, |
| | | 639 | | NavigationAreaPolicy areaPolicy, |
| | | 640 | | TraversalMedium medium, |
| | | 641 | | Vector3d start, |
| | | 642 | | Vector3d end, |
| | | 643 | | NavigationRayChainConstraint constraint, |
| | | 644 | | ref GuideSampleWorkMeter meter, |
| | | 645 | | NavigationImmediateRayWorkspace immediateRayWorkspace) |
| | | 646 | | { |
| | | 647 | | lock (immediateRayWorkspace.SyncRoot) |
| | | 648 | | { |
| | | 649 | | NavigationWorkMeter rayMeter = immediateRayWorkspace.WorkMeter; |
| | | 650 | | rayMeter.ResetForGuideSample( |
| | | 651 | | meter.GetCurrentNodeLookupAllowance(), |
| | | 652 | | meter.GetCursorLegScanAllowance(), |
| | | 653 | | meter.GetPortalCheckAllowance(), |
| | | 654 | | meter.GetPrismCheckAllowance(), |
| | | 655 | | meter.GetTraceIntervalAllowance()); |
| | | 656 | | NavigationRayWork ray = immediateRayWorkspace.RayWork; |
| | | 657 | | ray.Begin(new NavigationRayRequest( |
| | | 658 | | world, |
| | | 659 | | store, |
| | | 660 | | graph, |
| | | 661 | | payload.Key.Agent, |
| | | 662 | | areaPolicy, |
| | | 663 | | medium, |
| | | 664 | | start, |
| | | 665 | | end, |
| | | 666 | | NavigationRayEndpointAllowance.StartPrefix, |
| | | 667 | | constraint)); |
| | | 668 | | NavigationRayStatus status; |
| | | 669 | | try |
| | | 670 | | { |
| | | 671 | | do |
| | | 672 | | { |
| | | 673 | | status = ray.Advance(rayMeter); |
| | | 674 | | } |
| | | 675 | | while (status == NavigationRayStatus.Pending); |
| | | 676 | | } |
| | | 677 | | finally |
| | | 678 | | { |
| | | 679 | | ray.Reset(); |
| | | 680 | | } |
| | | 681 | | meter.TryConsumeCurrentNodeLookupProbes( |
| | | 682 | | checked(rayMeter.LookupProbes + rayMeter.CoveredVoxelIntervals)); |
| | | 683 | | meter.TryConsumeCursorLegScans( |
| | | 684 | | checked(rayMeter.EvaluatedEdges + rayMeter.ConnectionLegs)); |
| | | 685 | | meter.TryConsumePortalChecks(rayMeter.GuidePortalChecks); |
| | | 686 | | meter.TryConsumePrismChecks(rayMeter.GuidePrismChecks); |
| | | 687 | | meter.TryConsumeTraceIntervals(rayMeter.TraceIntervals); |
| | | 688 | | return status == NavigationRayStatus.Success |
| | | 689 | | ? NavigationGuideStatus.Success |
| | | 690 | | : NavigationGuideStatusMapper.ToPublic(status); |
| | | 691 | | } |
| | | 692 | | } |
| | | 693 | | |
| | | 694 | | private static NavigationFlowNodeLookupStatus TryGetNode( |
| | | 695 | | NavigationFlowFieldPayload payload, |
| | | 696 | | NavigationCellAddress address, |
| | | 697 | | TraversalMedium medium, |
| | | 698 | | ref GuideSampleWorkMeter meter, |
| | | 699 | | out NavigationFlowFieldNode node) |
| | | 700 | | { |
| | | 701 | | int low = 0; |
| | | 702 | | int high = payload.AddressLookupOrdinals.Length - 1; |
| | | 703 | | while (low <= high) |
| | | 704 | | { |
| | | 705 | | if (!meter.TryConsumeCurrentNodeLookupProbes(1)) |
| | | 706 | | { |
| | | 707 | | node = default; |
| | | 708 | | return NavigationFlowNodeLookupStatus.BudgetExceeded; |
| | | 709 | | } |
| | | 710 | | int middle = low + ((high - low) >> 1); |
| | | 711 | | NavigationFlowFieldNode candidate = |
| | | 712 | | payload.Nodes[payload.AddressLookupOrdinals[middle]]; |
| | | 713 | | int comparison = candidate.Address.CompareTo(address); |
| | | 714 | | if (comparison == 0) |
| | | 715 | | comparison = ((int)candidate.Medium).CompareTo((int)medium); |
| | | 716 | | if (comparison == 0) |
| | | 717 | | { |
| | | 718 | | node = candidate; |
| | | 719 | | return NavigationFlowNodeLookupStatus.Success; |
| | | 720 | | } |
| | | 721 | | if (comparison < 0) |
| | | 722 | | low = middle + 1; |
| | | 723 | | else |
| | | 724 | | high = middle - 1; |
| | | 725 | | } |
| | | 726 | | node = default; |
| | | 727 | | return NavigationFlowNodeLookupStatus.NotFound; |
| | | 728 | | } |
| | | 729 | | |
| | | 730 | | private static NavigationGuideStatus TryAdvanceRecovery( |
| | | 731 | | GridWorld world, |
| | | 732 | | NavigationWorldGraphStore store, |
| | | 733 | | NavigationWorldGraph graph, |
| | | 734 | | NavigationFlowFieldPayload payload, |
| | | 735 | | TraversalMedium medium, |
| | | 736 | | Vector3d actualFootPosition, |
| | | 737 | | ref GuideSampleWorkMeter meter, |
| | | 738 | | GridCoveredAddressCursor coveredAddressCursor, |
| | | 739 | | GridCoveredAddressGeneration[] coveredAddressGenerations, |
| | | 740 | | GridCoveredAddress[] coveredAddressOutput, |
| | | 741 | | NavigationImmediateRayWorkspace immediateRayWorkspace, |
| | | 742 | | NavigationCellAddress sourceAddress, |
| | | 743 | | NavigationNodeState sourceState, |
| | | 744 | | NavigationSelectedEdgeRef selected, |
| | | 745 | | Vector3d selectedExitTarget, |
| | | 746 | | NavigationNodeState targetState, |
| | | 747 | | bool hasSelectedEdge, |
| | | 748 | | bool allowRecovery, |
| | | 749 | | out NavigationCellAddress rebased, |
| | | 750 | | out Vector3d heading, |
| | | 751 | | out bool rejoined) |
| | | 752 | | { |
| | | 753 | | rebased = default; |
| | | 754 | | heading = Vector3d.Zero; |
| | | 755 | | rejoined = false; |
| | | 756 | | if (!allowRecovery) |
| | | 757 | | return NavigationGuideStatus.LocalRecoveryRequired; |
| | | 758 | | NavigationGuideStatus rebase = TryRebase( |
| | | 759 | | world, |
| | | 760 | | graph, |
| | | 761 | | payload, |
| | | 762 | | medium, |
| | | 763 | | actualFootPosition, |
| | | 764 | | ref meter, |
| | | 765 | | coveredAddressCursor, |
| | | 766 | | coveredAddressGenerations, |
| | | 767 | | coveredAddressOutput, |
| | | 768 | | out rebased); |
| | | 769 | | if (rebase != NavigationGuideStatus.LocalRecoveryRequired) |
| | | 770 | | return rebase; |
| | | 771 | | NavigationGuideStatus status = TryRejoin( |
| | | 772 | | world, |
| | | 773 | | store, |
| | | 774 | | graph, |
| | | 775 | | payload, |
| | | 776 | | medium, |
| | | 777 | | sourceAddress, |
| | | 778 | | sourceState, |
| | | 779 | | selected, |
| | | 780 | | selectedExitTarget, |
| | | 781 | | targetState, |
| | | 782 | | hasSelectedEdge, |
| | | 783 | | actualFootPosition, |
| | | 784 | | ref meter, |
| | | 785 | | immediateRayWorkspace, |
| | | 786 | | out heading); |
| | | 787 | | rejoined = status == NavigationGuideStatus.Success; |
| | | 788 | | return status; |
| | | 789 | | } |
| | | 790 | | |
| | | 791 | | private static NavigationGuideStatus TryRejoin( |
| | | 792 | | GridWorld world, |
| | | 793 | | NavigationWorldGraphStore store, |
| | | 794 | | NavigationWorldGraph graph, |
| | | 795 | | NavigationFlowFieldPayload payload, |
| | | 796 | | TraversalMedium medium, |
| | | 797 | | NavigationCellAddress sourceAddress, |
| | | 798 | | NavigationNodeState sourceState, |
| | | 799 | | NavigationSelectedEdgeRef selected, |
| | | 800 | | Vector3d selectedExitTarget, |
| | | 801 | | NavigationNodeState targetState, |
| | | 802 | | bool hasSelectedEdge, |
| | | 803 | | Vector3d actualFootPosition, |
| | | 804 | | ref GuideSampleWorkMeter meter, |
| | | 805 | | NavigationImmediateRayWorkspace immediateRayWorkspace, |
| | | 806 | | out Vector3d heading) |
| | | 807 | | { |
| | | 808 | | heading = Vector3d.Zero; |
| | | 809 | | graph.AreaCatalog.TryGet( |
| | | 810 | | payload.Key.AreaPolicy, |
| | | 811 | | out NavigationAreaPolicy areaPolicy); |
| | | 812 | | lock (immediateRayWorkspace.SyncRoot) |
| | | 813 | | { |
| | | 814 | | NavigationRayWork ray = immediateRayWorkspace.RayWork; |
| | | 815 | | for (int targetOrdinal = 0; ; targetOrdinal++) |
| | | 816 | | { |
| | | 817 | | if (targetOrdinal > 0 && !hasSelectedEdge) |
| | | 818 | | return NavigationGuideStatus.LocalRecoveryRequired; |
| | | 819 | | if (!TryGetRejoinTarget( |
| | | 820 | | sourceAddress, |
| | | 821 | | sourceState, |
| | | 822 | | targetState, |
| | | 823 | | selected, |
| | | 824 | | selectedExitTarget, |
| | | 825 | | targetOrdinal, |
| | | 826 | | out NavigationFlowRejoinTarget target)) |
| | | 827 | | { |
| | | 828 | | return NavigationGuideStatus.LocalRecoveryRequired; |
| | | 829 | | } |
| | | 830 | | |
| | | 831 | | ray.Begin(new NavigationRayRequest( |
| | | 832 | | world, |
| | | 833 | | store, |
| | | 834 | | graph, |
| | | 835 | | payload.Key.Agent, |
| | | 836 | | areaPolicy, |
| | | 837 | | medium, |
| | | 838 | | actualFootPosition, |
| | | 839 | | target.Position, |
| | | 840 | | NavigationRayEndpointAllowance.StartPrefix, |
| | | 841 | | target.Constraint)); |
| | | 842 | | NavigationRayStatus status; |
| | | 843 | | NavigationRayResult result; |
| | | 844 | | try |
| | | 845 | | { |
| | | 846 | | do |
| | | 847 | | { |
| | | 848 | | status = ray.Advance(ref meter); |
| | | 849 | | } |
| | | 850 | | while (status == NavigationRayStatus.Pending); |
| | | 851 | | result = ray.Result; |
| | | 852 | | } |
| | | 853 | | finally |
| | | 854 | | { |
| | | 855 | | ray.Reset(); |
| | | 856 | | } |
| | | 857 | | if (status == NavigationRayStatus.Blocked |
| | | 858 | | || (status == NavigationRayStatus.Success |
| | | 859 | | && !result.IsSemanticCostNeutral |
| | | 860 | | && target.Constraint.Kind |
| | | 861 | | != NavigationRayChainConstraintKind.SelectedEdge)) |
| | | 862 | | { |
| | | 863 | | continue; |
| | | 864 | | } |
| | | 865 | | if (status != NavigationRayStatus.Success) |
| | | 866 | | return NavigationGuideStatusMapper.ToPublic(status); |
| | | 867 | | return TrySetHeadingUnchecked( |
| | | 868 | | actualFootPosition, |
| | | 869 | | target.Position, |
| | | 870 | | out heading); |
| | | 871 | | } |
| | | 872 | | } |
| | | 873 | | } |
| | | 874 | | |
| | | 875 | | private static NavigationGuideStatus TryResolvePortal( |
| | | 876 | | NavigationGraphEdge edge, |
| | | 877 | | NavigationNodeState sourceState, |
| | | 878 | | KinematicBodyShape shape, |
| | | 879 | | ref GuideSampleWorkMeter meter, |
| | | 880 | | out GridNavigationPortal resolvedPortal, |
| | | 881 | | out Vector3d sourcePortal, |
| | | 882 | | out Vector3d targetPortal) |
| | | 883 | | { |
| | | 884 | | resolvedPortal = default; |
| | | 885 | | sourcePortal = default; |
| | | 886 | | targetPortal = default; |
| | | 887 | | if (!meter.TryConsumePortalChecks(1)) |
| | | 888 | | return NavigationGuideStatus.BudgetExceeded; |
| | | 889 | | bool reverse = false; |
| | | 890 | | if (edge.Kind == NavigationGraphEdgeKind.Native) |
| | | 891 | | { |
| | | 892 | | edge.NativePortal.TryTranslate(sourceState.Center, out resolvedPortal); |
| | | 893 | | } |
| | | 894 | | else |
| | | 895 | | { |
| | | 896 | | NavigationAutomaticSeamRef seam = edge.AutomaticSeam; |
| | | 897 | | resolvedPortal = seam.Portal; |
| | | 898 | | reverse = seam.IsReverse; |
| | | 899 | | } |
| | | 900 | | resolvedPortal.TryResolveProfile( |
| | | 901 | | shape.Radius, |
| | | 902 | | shape.Height, |
| | | 903 | | out Vector3d first, |
| | | 904 | | out Vector3d second); |
| | | 905 | | sourcePortal = reverse ? second : first; |
| | | 906 | | targetPortal = reverse ? first : second; |
| | | 907 | | return NavigationGuideStatus.Success; |
| | | 908 | | } |
| | | 909 | | |
| | | 910 | | private static NavigationGuideStatus TrySampleDirectedLeg( |
| | | 911 | | Vector3d sourceFootAnchor, |
| | | 912 | | Vector3d sourcePortal, |
| | | 913 | | Vector3d targetPortal, |
| | | 914 | | Vector3d targetFootAnchor, |
| | | 915 | | Vector3d actualFootPosition, |
| | | 916 | | ref GuideSampleWorkMeter meter, |
| | | 917 | | out Vector3d heading) |
| | | 918 | | { |
| | | 919 | | int traceIntervals = sourcePortal == targetPortal ? 2 : 3; |
| | | 920 | | if (!meter.TryConsumeCursorLegScans(1) |
| | | 921 | | || !meter.TryConsumeTraceIntervals(traceIntervals)) |
| | | 922 | | { |
| | | 923 | | heading = Vector3d.Zero; |
| | | 924 | | return NavigationGuideStatus.BudgetExceeded; |
| | | 925 | | } |
| | | 926 | | return TrySampleDirectedLegUnchecked( |
| | | 927 | | sourceFootAnchor, |
| | | 928 | | sourcePortal, |
| | | 929 | | targetPortal, |
| | | 930 | | targetFootAnchor, |
| | | 931 | | actualFootPosition, |
| | | 932 | | out heading); |
| | | 933 | | } |
| | | 934 | | |
| | | 935 | | private static NavigationGuideStatus TrySampleDirectedLegUnchecked( |
| | | 936 | | Vector3d sourceFootAnchor, |
| | | 937 | | Vector3d sourcePortal, |
| | | 938 | | Vector3d targetPortal, |
| | | 939 | | Vector3d targetFootAnchor, |
| | | 940 | | Vector3d actualFootPosition, |
| | | 941 | | out Vector3d heading) |
| | | 942 | | { |
| | | 943 | | NavigationGuideStatus status = HasReachedOrPassed( |
| | | 944 | | sourceFootAnchor, |
| | | 945 | | sourcePortal, |
| | | 946 | | actualFootPosition, |
| | | 947 | | out bool passedSourcePortal); |
| | | 948 | | bool passedTargetPortal = false; |
| | | 949 | | if (status == NavigationGuideStatus.Success && passedSourcePortal) |
| | | 950 | | { |
| | | 951 | | status = HasReachedOrPassed( |
| | | 952 | | sourcePortal, |
| | | 953 | | targetPortal, |
| | | 954 | | actualFootPosition, |
| | | 955 | | out passedTargetPortal); |
| | | 956 | | } |
| | | 957 | | if (status != NavigationGuideStatus.Success) |
| | | 958 | | return FailDirectedLeg(status, out heading); |
| | | 959 | | if (!passedSourcePortal) |
| | | 960 | | { |
| | | 961 | | return TrySetHeadingUnchecked(actualFootPosition, sourcePortal, out heading); |
| | | 962 | | } |
| | | 963 | | return TrySetHeadingUnchecked( |
| | | 964 | | actualFootPosition, |
| | | 965 | | passedTargetPortal ? targetFootAnchor : targetPortal, |
| | | 966 | | out heading); |
| | | 967 | | } |
| | | 968 | | |
| | | 969 | | private static NavigationGuideStatus FailDirectedLeg( |
| | | 970 | | NavigationGuideStatus status, |
| | | 971 | | out Vector3d heading) |
| | | 972 | | { |
| | | 973 | | heading = Vector3d.Zero; |
| | | 974 | | return status; |
| | | 975 | | } |
| | | 976 | | |
| | | 977 | | private static NavigationGuideStatus TrySampleExplicit( |
| | | 978 | | NavigationWorldGraph graph, |
| | | 979 | | NavigationExplicitConnectionRecord record, |
| | | 980 | | NavigationCellAddress sourceAddress, |
| | | 981 | | NavigationNodeState sourceState, |
| | | 982 | | GridCellPrism sourcePrism, |
| | | 983 | | NavigationNodeState targetState, |
| | | 984 | | GridCellPrism targetPrism, |
| | | 985 | | bool sourceContains, |
| | | 986 | | bool targetContains, |
| | | 987 | | Vector3d actualFootPosition, |
| | | 988 | | KinematicBodyShape shape, |
| | | 989 | | bool targetIsDestination, |
| | | 990 | | Fixed64 arrivalRadius, |
| | | 991 | | ref GuideSampleWorkMeter meter, |
| | | 992 | | out NavigationCellAddress nextSourceAddress, |
| | | 993 | | out Vector3d heading) |
| | | 994 | | { |
| | | 995 | | nextSourceAddress = sourceAddress; |
| | | 996 | | heading = Vector3d.Zero; |
| | | 997 | | NavigationConnection connection = record.Definition; |
| | | 998 | | NavigationPagedSequence<GridNavigationPortal>.Enumerator portals = |
| | | 999 | | record.NavigationPortals.GetEnumerator(); |
| | | 1000 | | NavigationGuideStatus firstPortalStatus = TryReadExplicitPortal( |
| | | 1001 | | ref portals, |
| | | 1002 | | shape, |
| | | 1003 | | ref meter, |
| | | 1004 | | out GridNavigationPortal incomingPortal, |
| | | 1005 | | out Vector3d incomingSourceAnchor, |
| | | 1006 | | out Vector3d incomingTargetAnchor); |
| | | 1007 | | if (firstPortalStatus != NavigationGuideStatus.Success) |
| | | 1008 | | return firstPortalStatus; |
| | | 1009 | | |
| | | 1010 | | bool zeroWitnessTargetBodyValid = connection.Witnesses.Count == 0 |
| | | 1011 | | && targetContains |
| | | 1012 | | && GridCellGeometry.IsNavigationBodyAnchorValid( |
| | | 1013 | | targetPrism, |
| | | 1014 | | actualFootPosition, |
| | | 1015 | | shape.Radius, |
| | | 1016 | | shape.Height, |
| | | 1017 | | incomingPortal); |
| | | 1018 | | if (targetIsDestination |
| | | 1019 | | && zeroWitnessTargetBodyValid |
| | | 1020 | | && IsWithinArrivalRadius(actualFootPosition, targetState.FootAnchor, arrivalRadius)) |
| | | 1021 | | { |
| | | 1022 | | return NavigationGuideStatus.Success; |
| | | 1023 | | } |
| | | 1024 | | |
| | | 1025 | | bool sourceBodyValid = sourceContains |
| | | 1026 | | && GridCellGeometry.IsNavigationBodyAnchorValid( |
| | | 1027 | | sourcePrism, |
| | | 1028 | | actualFootPosition, |
| | | 1029 | | shape.Radius, |
| | | 1030 | | shape.Height, |
| | | 1031 | | incomingPortal); |
| | | 1032 | | if (sourceBodyValid && !zeroWitnessTargetBodyValid) |
| | | 1033 | | { |
| | | 1034 | | NavigationGuideStatus progressStatus = HasReachedOrPassed( |
| | | 1035 | | sourceState.FootAnchor, |
| | | 1036 | | connection.EntryAnchor, |
| | | 1037 | | actualFootPosition, |
| | | 1038 | | out bool passedEntry); |
| | | 1039 | | if (progressStatus != NavigationGuideStatus.Success) |
| | | 1040 | | return progressStatus; |
| | | 1041 | | if (!passedEntry) |
| | | 1042 | | { |
| | | 1043 | | return TrySetHeading( |
| | | 1044 | | actualFootPosition, |
| | | 1045 | | connection.EntryAnchor, |
| | | 1046 | | ref meter, |
| | | 1047 | | out heading); |
| | | 1048 | | } |
| | | 1049 | | if (connection.Witnesses.Count == 0) |
| | | 1050 | | { |
| | | 1051 | | return TrySampleDirectedLeg( |
| | | 1052 | | connection.EntryAnchor, |
| | | 1053 | | incomingSourceAnchor, |
| | | 1054 | | incomingTargetAnchor, |
| | | 1055 | | connection.ExitAnchor, |
| | | 1056 | | actualFootPosition, |
| | | 1057 | | ref meter, |
| | | 1058 | | out heading); |
| | | 1059 | | } |
| | | 1060 | | NavigationCellAddress firstAddress = connection.Witnesses[0]; |
| | | 1061 | | graph.TryGetNodeRef(firstAddress, out NavigationNodeRef firstRef); |
| | | 1062 | | graph.TryGetNodeState(firstRef, out NavigationNodeState firstState); |
| | | 1063 | | return TrySampleDirectedLeg( |
| | | 1064 | | connection.EntryAnchor, |
| | | 1065 | | incomingSourceAnchor, |
| | | 1066 | | incomingTargetAnchor, |
| | | 1067 | | firstState.FootAnchor, |
| | | 1068 | | actualFootPosition, |
| | | 1069 | | ref meter, |
| | | 1070 | | out heading); |
| | | 1071 | | } |
| | | 1072 | | |
| | | 1073 | | for (int i = 0; i < connection.Witnesses.Count; i++) |
| | | 1074 | | { |
| | | 1075 | | NavigationGuideStatus outgoingPortalStatus = TryReadExplicitPortal( |
| | | 1076 | | ref portals, |
| | | 1077 | | shape, |
| | | 1078 | | ref meter, |
| | | 1079 | | out GridNavigationPortal outgoingPortal, |
| | | 1080 | | out Vector3d outgoingSourceAnchor, |
| | | 1081 | | out Vector3d outgoingTargetAnchor); |
| | | 1082 | | if (outgoingPortalStatus != NavigationGuideStatus.Success) |
| | | 1083 | | return outgoingPortalStatus; |
| | | 1084 | | NavigationCellAddress witnessAddress = connection.Witnesses[i]; |
| | | 1085 | | graph.TryGetSeamPrism(witnessAddress, out GridCellPrism witnessPrism); |
| | | 1086 | | if (!meter.TryConsumePrismChecks(1)) |
| | | 1087 | | return NavigationGuideStatus.BudgetExceeded; |
| | | 1088 | | if (!witnessPrism.Contains(actualFootPosition)) |
| | | 1089 | | { |
| | | 1090 | | incomingPortal = outgoingPortal; |
| | | 1091 | | incomingTargetAnchor = outgoingTargetAnchor; |
| | | 1092 | | continue; |
| | | 1093 | | } |
| | | 1094 | | bool witnessBodyValid = GridCellGeometry.IsNavigationBodyAnchorValid( |
| | | 1095 | | witnessPrism, |
| | | 1096 | | actualFootPosition, |
| | | 1097 | | shape.Radius, |
| | | 1098 | | shape.Height, |
| | | 1099 | | outgoingPortal) |
| | | 1100 | | || GridCellGeometry.IsNavigationBodyAnchorValid( |
| | | 1101 | | witnessPrism, |
| | | 1102 | | actualFootPosition, |
| | | 1103 | | shape.Radius, |
| | | 1104 | | shape.Height, |
| | | 1105 | | incomingPortal); |
| | | 1106 | | if (!witnessBodyValid) |
| | | 1107 | | { |
| | | 1108 | | incomingPortal = outgoingPortal; |
| | | 1109 | | incomingTargetAnchor = outgoingTargetAnchor; |
| | | 1110 | | continue; |
| | | 1111 | | } |
| | | 1112 | | NavigationCellAddress nextAddress = i + 1 < connection.Witnesses.Count |
| | | 1113 | | ? connection.Witnesses[i + 1] |
| | | 1114 | | : record.Destination; |
| | | 1115 | | NavigationNodeState nextState; |
| | | 1116 | | if (nextAddress == record.Destination) |
| | | 1117 | | { |
| | | 1118 | | nextState = targetState; |
| | | 1119 | | } |
| | | 1120 | | else |
| | | 1121 | | { |
| | | 1122 | | graph.TryGetNodeRef(nextAddress, out NavigationNodeRef nextRef); |
| | | 1123 | | graph.TryGetNodeState(nextRef, out nextState); |
| | | 1124 | | } |
| | | 1125 | | return TrySampleDirectedLeg( |
| | | 1126 | | incomingTargetAnchor, |
| | | 1127 | | outgoingSourceAnchor, |
| | | 1128 | | outgoingTargetAnchor, |
| | | 1129 | | nextAddress == record.Destination |
| | | 1130 | | ? connection.ExitAnchor |
| | | 1131 | | : nextState.FootAnchor, |
| | | 1132 | | actualFootPosition, |
| | | 1133 | | ref meter, |
| | | 1134 | | out heading); |
| | | 1135 | | } |
| | | 1136 | | |
| | | 1137 | | bool targetBodyValid = zeroWitnessTargetBodyValid |
| | | 1138 | | || (connection.Witnesses.Count > 0 |
| | | 1139 | | && targetContains |
| | | 1140 | | && GridCellGeometry.IsNavigationBodyAnchorValid( |
| | | 1141 | | targetPrism, |
| | | 1142 | | actualFootPosition, |
| | | 1143 | | shape.Radius, |
| | | 1144 | | shape.Height, |
| | | 1145 | | incomingPortal)); |
| | | 1146 | | if (targetBodyValid) |
| | | 1147 | | { |
| | | 1148 | | if (targetIsDestination |
| | | 1149 | | && IsWithinArrivalRadius(actualFootPosition, targetState.FootAnchor, arrivalRadius)) |
| | | 1150 | | { |
| | | 1151 | | return NavigationGuideStatus.Success; |
| | | 1152 | | } |
| | | 1153 | | NavigationGuideStatus progressStatus = HasReachedOrPassed( |
| | | 1154 | | incomingTargetAnchor, |
| | | 1155 | | connection.ExitAnchor, |
| | | 1156 | | actualFootPosition, |
| | | 1157 | | out bool passedExit); |
| | | 1158 | | if (progressStatus != NavigationGuideStatus.Success) |
| | | 1159 | | return progressStatus; |
| | | 1160 | | if (!passedExit) |
| | | 1161 | | { |
| | | 1162 | | return TrySetHeading( |
| | | 1163 | | actualFootPosition, |
| | | 1164 | | connection.ExitAnchor, |
| | | 1165 | | ref meter, |
| | | 1166 | | out heading); |
| | | 1167 | | } |
| | | 1168 | | progressStatus = HasReachedOrPassed( |
| | | 1169 | | connection.ExitAnchor, |
| | | 1170 | | targetState.FootAnchor, |
| | | 1171 | | actualFootPosition, |
| | | 1172 | | out bool passedTarget); |
| | | 1173 | | if (progressStatus != NavigationGuideStatus.Success) |
| | | 1174 | | return progressStatus; |
| | | 1175 | | if (!passedTarget) |
| | | 1176 | | { |
| | | 1177 | | return TrySetHeading( |
| | | 1178 | | actualFootPosition, |
| | | 1179 | | targetState.FootAnchor, |
| | | 1180 | | ref meter, |
| | | 1181 | | out heading); |
| | | 1182 | | } |
| | | 1183 | | if (!meter.TryConsumeCursorRebases(1)) |
| | | 1184 | | return NavigationGuideStatus.BudgetExceeded; |
| | | 1185 | | nextSourceAddress = record.Destination; |
| | | 1186 | | return NavigationGuideStatus.Success; |
| | | 1187 | | } |
| | | 1188 | | return NavigationGuideStatus.LocalRecoveryRequired; |
| | | 1189 | | } |
| | | 1190 | | |
| | | 1191 | | private static NavigationGuideStatus TryReadExplicitPortal( |
| | | 1192 | | ref NavigationPagedSequence<GridNavigationPortal>.Enumerator portals, |
| | | 1193 | | KinematicBodyShape shape, |
| | | 1194 | | ref GuideSampleWorkMeter meter, |
| | | 1195 | | out GridNavigationPortal portal, |
| | | 1196 | | out Vector3d sourcePortalAnchor, |
| | | 1197 | | out Vector3d targetPortalAnchor) |
| | | 1198 | | { |
| | | 1199 | | portal = default; |
| | | 1200 | | sourcePortalAnchor = default; |
| | | 1201 | | targetPortalAnchor = default; |
| | | 1202 | | if (!meter.TryConsumePortalChecks(1)) |
| | | 1203 | | return NavigationGuideStatus.BudgetExceeded; |
| | | 1204 | | portals.MoveNext(); |
| | | 1205 | | portal = portals.Current; |
| | | 1206 | | portal.TryResolveProfile( |
| | | 1207 | | shape.Radius, |
| | | 1208 | | shape.Height, |
| | | 1209 | | out sourcePortalAnchor, |
| | | 1210 | | out targetPortalAnchor); |
| | | 1211 | | return NavigationGuideStatus.Success; |
| | | 1212 | | } |
| | | 1213 | | |
| | | 1214 | | internal static NavigationGuideStatus TrySetHeading( |
| | | 1215 | | Vector3d actualFootPosition, |
| | | 1216 | | Vector3d target, |
| | | 1217 | | ref GuideSampleWorkMeter meter, |
| | | 1218 | | out Vector3d heading) |
| | | 1219 | | { |
| | | 1220 | | if (!meter.TryConsumeCursorLegScans(1) |
| | | 1221 | | || !meter.TryConsumeTraceIntervals(1)) |
| | | 1222 | | { |
| | | 1223 | | heading = Vector3d.Zero; |
| | | 1224 | | return NavigationGuideStatus.BudgetExceeded; |
| | | 1225 | | } |
| | | 1226 | | return TrySetHeadingUnchecked(actualFootPosition, target, out heading); |
| | | 1227 | | } |
| | | 1228 | | |
| | | 1229 | | private static NavigationGuideStatus TrySetHeadingUnchecked( |
| | | 1230 | | Vector3d actualFootPosition, |
| | | 1231 | | Vector3d target, |
| | | 1232 | | out Vector3d heading) |
| | | 1233 | | { |
| | | 1234 | | heading = Vector3d.Zero; |
| | | 1235 | | if (!Vector3d.TrySubtract(target, actualFootPosition, out Vector3d delta)) |
| | | 1236 | | return NavigationGuideStatus.CostOverflow; |
| | | 1237 | | if (delta != Vector3d.Zero) |
| | | 1238 | | heading = delta.Normalized; |
| | | 1239 | | return NavigationGuideStatus.Success; |
| | | 1240 | | } |
| | | 1241 | | |
| | | 1242 | | internal static NavigationGuideStatus HasReachedOrPassed( |
| | | 1243 | | Vector3d start, |
| | | 1244 | | Vector3d end, |
| | | 1245 | | Vector3d actual, |
| | | 1246 | | out bool passed) |
| | | 1247 | | { |
| | | 1248 | | passed = false; |
| | | 1249 | | if (start == end) |
| | | 1250 | | { |
| | | 1251 | | passed = true; |
| | | 1252 | | return NavigationGuideStatus.Success; |
| | | 1253 | | } |
| | | 1254 | | if (!Vector3d.TrySubtract(end, start, out Vector3d direction) |
| | | 1255 | | || !Vector3d.TrySubtract(actual, end, out Vector3d beyond) |
| | | 1256 | | || !Vector3d.TryDot(beyond, direction, out Fixed64 projection)) |
| | | 1257 | | { |
| | | 1258 | | return NavigationGuideStatus.CostOverflow; |
| | | 1259 | | } |
| | | 1260 | | passed = projection >= Fixed64.Zero; |
| | | 1261 | | return NavigationGuideStatus.Success; |
| | | 1262 | | } |
| | | 1263 | | |
| | | 1264 | | private static NavigationGuideStatus TryRebase( |
| | | 1265 | | GridWorld world, |
| | | 1266 | | NavigationWorldGraph graph, |
| | | 1267 | | NavigationFlowFieldPayload payload, |
| | | 1268 | | TraversalMedium medium, |
| | | 1269 | | Vector3d actualFootPosition, |
| | | 1270 | | ref GuideSampleWorkMeter meter, |
| | | 1271 | | GridCoveredAddressCursor cursor, |
| | | 1272 | | GridCoveredAddressGeneration[] generations, |
| | | 1273 | | GridCoveredAddress[] output, |
| | | 1274 | | out NavigationCellAddress rebased) |
| | | 1275 | | { |
| | | 1276 | | rebased = default; |
| | | 1277 | | if (!meter.TryConsumeLocalRecoveryAttempts(1) |
| | | 1278 | | || !meter.TryConsumeCursorRebases(1)) |
| | | 1279 | | { |
| | | 1280 | | return NavigationGuideStatus.BudgetExceeded; |
| | | 1281 | | } |
| | | 1282 | | for (int i = 0; i < graph.MapCount; i++) |
| | | 1283 | | { |
| | | 1284 | | if (!meter.TryConsumeCurrentNodeLookupProbes(1)) |
| | | 1285 | | return NavigationGuideStatus.BudgetExceeded; |
| | | 1286 | | bool foundGeneration = graph.TryGetCoveredAddressGeneration( |
| | | 1287 | | i, |
| | | 1288 | | out _, |
| | | 1289 | | out generations[i]); |
| | | 1290 | | System.Diagnostics.Debug.Assert( |
| | | 1291 | | foundGeneration, |
| | | 1292 | | "published graph instances are materialized and indexed one-to-one"); |
| | | 1293 | | } |
| | | 1294 | | bool began = world.TryBeginCoveredAddresses( |
| | | 1295 | | cursor, |
| | | 1296 | | actualFootPosition, |
| | | 1297 | | actualFootPosition, |
| | | 1298 | | graph.MapCount); |
| | | 1299 | | System.Diagnostics.Debug.Assert(began); |
| | | 1300 | | |
| | | 1301 | | int inputOrdinal = 0; |
| | | 1302 | | bool hasCandidate = false; |
| | | 1303 | | Fixed64 bestDistance = Fixed64.Zero; |
| | | 1304 | | NavigationCellAddress best = default; |
| | | 1305 | | while (true) |
| | | 1306 | | { |
| | | 1307 | | if (inputOrdinal < graph.MapCount) |
| | | 1308 | | { |
| | | 1309 | | int available = meter.GetCurrentNodeLookupAllowance(); |
| | | 1310 | | if (available == 0) |
| | | 1311 | | return NavigationGuideStatus.BudgetExceeded; |
| | | 1312 | | GridCoveredAddressCursorStatus bindStatus = world.AdvanceCoveredAddresses( |
| | | 1313 | | cursor, |
| | | 1314 | | generations.AsSpan(inputOrdinal, graph.MapCount - inputOrdinal), |
| | | 1315 | | output, |
| | | 1316 | | lookupProbeLimit: available, |
| | | 1317 | | addressProbeLimit: 0, |
| | | 1318 | | outputLimit: 0, |
| | | 1319 | | out int lookupProbes, |
| | | 1320 | | out int addressProbes, |
| | | 1321 | | out int inputsConsumed, |
| | | 1322 | | out int outputCount); |
| | | 1323 | | meter.TryConsumeCurrentNodeLookupProbes( |
| | | 1324 | | checked(lookupProbes + addressProbes + outputCount)); |
| | | 1325 | | inputOrdinal += inputsConsumed; |
| | | 1326 | | if (bindStatus == GridCoveredAddressCursorStatus.Stale) |
| | | 1327 | | return NavigationGuideStatus.Stale; |
| | | 1328 | | continue; |
| | | 1329 | | } |
| | | 1330 | | |
| | | 1331 | | int outputBudget = meter.GetCurrentNodeLookupAllowance() > 0 ? 1 : 0; |
| | | 1332 | | GridCoveredAddressCursorStatus flushStatus = world.AdvanceCoveredAddresses( |
| | | 1333 | | cursor, |
| | | 1334 | | ReadOnlySpan<GridCoveredAddressGeneration>.Empty, |
| | | 1335 | | output, |
| | | 1336 | | lookupProbeLimit: 0, |
| | | 1337 | | addressProbeLimit: 0, |
| | | 1338 | | outputLimit: outputBudget, |
| | | 1339 | | out int flushLookups, |
| | | 1340 | | out int flushAddresses, |
| | | 1341 | | out _, |
| | | 1342 | | out int flushed); |
| | | 1343 | | meter.TryConsumeCurrentNodeLookupProbes( |
| | | 1344 | | checked(flushLookups + flushAddresses + flushed)); |
| | | 1345 | | if (flushed != 0) |
| | | 1346 | | { |
| | | 1347 | | System.Diagnostics.Debug.Assert( |
| | | 1348 | | flushStatus != GridCoveredAddressCursorStatus.Stale, |
| | | 1349 | | "a stale covered-address cursor cannot publish output"); |
| | | 1350 | | NavigationGuideStatus candidateStatus = ConsiderCandidate( |
| | | 1351 | | graph, |
| | | 1352 | | payload, |
| | | 1353 | | medium, |
| | | 1354 | | actualFootPosition, |
| | | 1355 | | output[0], |
| | | 1356 | | ref meter, |
| | | 1357 | | ref hasCandidate, |
| | | 1358 | | ref bestDistance, |
| | | 1359 | | ref best); |
| | | 1360 | | if (candidateStatus != NavigationGuideStatus.Success) |
| | | 1361 | | return candidateStatus; |
| | | 1362 | | continue; |
| | | 1363 | | } |
| | | 1364 | | if (TryResolveRebaseCursorStatus( |
| | | 1365 | | flushStatus, |
| | | 1366 | | hasCandidate, |
| | | 1367 | | best, |
| | | 1368 | | out NavigationGuideStatus flushResult, |
| | | 1369 | | out rebased)) |
| | | 1370 | | { |
| | | 1371 | | return flushResult; |
| | | 1372 | | } |
| | | 1373 | | |
| | | 1374 | | int addressBudget = meter.GetCurrentNodeLookupAllowance(); |
| | | 1375 | | if (addressBudget == 0) |
| | | 1376 | | return NavigationGuideStatus.BudgetExceeded; |
| | | 1377 | | GridCoveredAddressCursorStatus addressStatus = world.AdvanceCoveredAddresses( |
| | | 1378 | | cursor, |
| | | 1379 | | ReadOnlySpan<GridCoveredAddressGeneration>.Empty, |
| | | 1380 | | output, |
| | | 1381 | | lookupProbeLimit: 0, |
| | | 1382 | | addressProbeLimit: addressBudget, |
| | | 1383 | | outputLimit: 0, |
| | | 1384 | | out int addressLookups, |
| | | 1385 | | out int enumeratedAddresses, |
| | | 1386 | | out _, |
| | | 1387 | | out int addressOutputs); |
| | | 1388 | | meter.TryConsumeCurrentNodeLookupProbes( |
| | | 1389 | | checked(addressLookups + enumeratedAddresses + addressOutputs)); |
| | | 1390 | | if (TryResolveRebaseCursorStatus( |
| | | 1391 | | addressStatus, |
| | | 1392 | | hasCandidate, |
| | | 1393 | | best, |
| | | 1394 | | out NavigationGuideStatus addressResult, |
| | | 1395 | | out rebased)) |
| | | 1396 | | { |
| | | 1397 | | return addressResult; |
| | | 1398 | | } |
| | | 1399 | | } |
| | | 1400 | | } |
| | | 1401 | | |
| | | 1402 | | internal static bool TryResolveRebaseCursorStatus( |
| | | 1403 | | GridCoveredAddressCursorStatus cursorStatus, |
| | | 1404 | | bool hasCandidate, |
| | | 1405 | | NavigationCellAddress best, |
| | | 1406 | | out NavigationGuideStatus status, |
| | | 1407 | | out NavigationCellAddress rebased) |
| | | 1408 | | { |
| | | 1409 | | if (cursorStatus == GridCoveredAddressCursorStatus.Stale) |
| | | 1410 | | { |
| | | 1411 | | status = NavigationGuideStatus.Stale; |
| | | 1412 | | rebased = default; |
| | | 1413 | | return true; |
| | | 1414 | | } |
| | | 1415 | | if (cursorStatus == GridCoveredAddressCursorStatus.Complete) |
| | | 1416 | | { |
| | | 1417 | | status = hasCandidate |
| | | 1418 | | ? NavigationGuideStatus.Success |
| | | 1419 | | : NavigationGuideStatus.LocalRecoveryRequired; |
| | | 1420 | | rebased = best; |
| | | 1421 | | return true; |
| | | 1422 | | } |
| | | 1423 | | status = NavigationGuideStatus.Success; |
| | | 1424 | | rebased = default; |
| | | 1425 | | return false; |
| | | 1426 | | } |
| | | 1427 | | |
| | | 1428 | | private static NavigationGuideStatus ConsiderCandidate( |
| | | 1429 | | NavigationWorldGraph graph, |
| | | 1430 | | NavigationFlowFieldPayload payload, |
| | | 1431 | | TraversalMedium medium, |
| | | 1432 | | Vector3d actualFootPosition, |
| | | 1433 | | GridCoveredAddress candidate, |
| | | 1434 | | ref GuideSampleWorkMeter meter, |
| | | 1435 | | ref bool hasCandidate, |
| | | 1436 | | ref Fixed64 bestDistance, |
| | | 1437 | | ref NavigationCellAddress best) |
| | | 1438 | | { |
| | | 1439 | | // Covered generations originate from this graph's configuration index. |
| | | 1440 | | graph.TryGetMapId(candidate.ConfigurationKey, out string mapId); |
| | | 1441 | | var address = new NavigationCellAddress(mapId, candidate.VoxelIndex); |
| | | 1442 | | return ConsiderCandidateAddress( |
| | | 1443 | | graph, |
| | | 1444 | | payload, |
| | | 1445 | | medium, |
| | | 1446 | | actualFootPosition, |
| | | 1447 | | address, |
| | | 1448 | | ref meter, |
| | | 1449 | | ref hasCandidate, |
| | | 1450 | | ref bestDistance, |
| | | 1451 | | ref best); |
| | | 1452 | | } |
| | | 1453 | | |
| | | 1454 | | internal static NavigationGuideStatus ConsiderCandidateAddress( |
| | | 1455 | | NavigationWorldGraph graph, |
| | | 1456 | | NavigationFlowFieldPayload payload, |
| | | 1457 | | TraversalMedium medium, |
| | | 1458 | | Vector3d actualFootPosition, |
| | | 1459 | | NavigationCellAddress address, |
| | | 1460 | | ref GuideSampleWorkMeter meter, |
| | | 1461 | | ref bool hasCandidate, |
| | | 1462 | | ref Fixed64 bestDistance, |
| | | 1463 | | ref NavigationCellAddress best) |
| | | 1464 | | { |
| | | 1465 | | if (!meter.TryConsumePrismChecks(1)) |
| | | 1466 | | return NavigationGuideStatus.BudgetExceeded; |
| | | 1467 | | bool foundPrism = graph.TryGetSeamPrism(address, out GridCellPrism prism); |
| | | 1468 | | System.Diagnostics.Debug.Assert(foundPrism, |
| | | 1469 | | "covered-address candidates retain their graph-owned configuration prism"); |
| | | 1470 | | if (!prism.Contains(actualFootPosition) |
| | | 1471 | | || !graph.TryGetNodeRef(address, out NavigationNodeRef node) |
| | | 1472 | | || !graph.TryGetNodeState(node, medium, out NavigationNodeState state) |
| | | 1473 | | || !state.IsPresent) |
| | | 1474 | | { |
| | | 1475 | | return NavigationGuideStatus.Success; |
| | | 1476 | | } |
| | | 1477 | | NavigationFlowNodeLookupStatus lookup = TryGetNode( |
| | | 1478 | | payload, |
| | | 1479 | | address, |
| | | 1480 | | medium, |
| | | 1481 | | ref meter, |
| | | 1482 | | out _); |
| | | 1483 | | if (lookup != NavigationFlowNodeLookupStatus.Success) |
| | | 1484 | | { |
| | | 1485 | | return MapNodeLookupStatus(lookup, required: false); |
| | | 1486 | | } |
| | | 1487 | | if (!Vector3d.TryGetDistance( |
| | | 1488 | | actualFootPosition, |
| | | 1489 | | state.FootAnchor, |
| | | 1490 | | out Fixed64 distance)) |
| | | 1491 | | { |
| | | 1492 | | return NavigationGuideStatus.CostOverflow; |
| | | 1493 | | } |
| | | 1494 | | SelectNearestCandidate( |
| | | 1495 | | distance, |
| | | 1496 | | address, |
| | | 1497 | | ref hasCandidate, |
| | | 1498 | | ref bestDistance, |
| | | 1499 | | ref best); |
| | | 1500 | | return NavigationGuideStatus.Success; |
| | | 1501 | | } |
| | | 1502 | | |
| | | 1503 | | internal static void SelectNearestCandidate( |
| | | 1504 | | Fixed64 candidateDistance, |
| | | 1505 | | NavigationCellAddress candidate, |
| | | 1506 | | ref bool hasCandidate, |
| | | 1507 | | ref Fixed64 bestDistance, |
| | | 1508 | | ref NavigationCellAddress best) |
| | | 1509 | | { |
| | | 1510 | | if (!hasCandidate |
| | | 1511 | | || candidateDistance < bestDistance |
| | | 1512 | | || (candidateDistance == bestDistance && candidate.CompareTo(best) < 0)) |
| | | 1513 | | { |
| | | 1514 | | hasCandidate = true; |
| | | 1515 | | bestDistance = candidateDistance; |
| | | 1516 | | best = candidate; |
| | | 1517 | | } |
| | | 1518 | | } |
| | | 1519 | | } |