| | | 1 | | //======================================================================= |
| | | 2 | | // TrailblazerGuideService.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 | | |
| | | 11 | | namespace Trailblazer.Pathing; |
| | | 12 | | |
| | | 13 | | /// <summary> |
| | | 14 | | /// Context-owned API for actionable A* and flow-field guide acquisition. |
| | | 15 | | /// </summary> |
| | | 16 | | public sealed class TrailblazerGuideService |
| | | 17 | | { |
| | | 18 | | private readonly TrailblazerWorldContext _context; |
| | 989 | 19 | | internal TrailblazerGuideService(TrailblazerWorldContext context) |
| | | 20 | | { |
| | 989 | 21 | | _context = context; |
| | 989 | 22 | | } |
| | | 23 | | |
| | | 24 | | /// <summary>Proves one cost-neutral graph-direct surface heading.</summary> |
| | | 25 | | internal NavigationRayStatus TryGetDirectHeading( |
| | | 26 | | PathQuery query, |
| | | 27 | | Vector3d actualFoot, |
| | | 28 | | out Vector3d heading) |
| | | 29 | | { |
| | 576 | 30 | | EnsureUsable(); |
| | 576 | 31 | | heading = Vector3d.Zero; |
| | 576 | 32 | | NavigationImmediateRayWorkspace immediate = _context.Pathing.ImmediateRayWorkspace; |
| | 576 | 33 | | lock (immediate.SyncRoot) |
| | | 34 | | { |
| | 576 | 35 | | NavigationWorldGraphStore store = _context.Pathing.NavigationGraphStore; |
| | 576 | 36 | | using NavigationWorldGraphLease? lease = store.TryAcquire(); |
| | 576 | 37 | | if (lease == null) |
| | 2 | 38 | | return NavigationRayStatus.CapacityExceeded; |
| | 574 | 39 | | NavigationWorldGraph graph = lease.Graph; |
| | 574 | 40 | | if (!graph.AreaCatalog.TryGet( |
| | 574 | 41 | | query.AreaPolicy, |
| | 574 | 42 | | out NavigationAreaPolicy areaPolicy)) |
| | | 43 | | { |
| | 2 | 44 | | return NavigationRayStatus.Stale; |
| | | 45 | | } |
| | | 46 | | |
| | 572 | 47 | | NavigationRayWork ray = immediate.RayWork; |
| | 572 | 48 | | ray.Begin(new NavigationRayRequest( |
| | 572 | 49 | | _context.World, |
| | 572 | 50 | | store, |
| | 572 | 51 | | graph, |
| | 572 | 52 | | query.Agent, |
| | 572 | 53 | | areaPolicy, |
| | 572 | 54 | | query.Traversal.StartMedium, |
| | 572 | 55 | | actualFoot, |
| | 572 | 56 | | query.End.Position, |
| | 572 | 57 | | NavigationRayEndpointAllowance.None)); |
| | 572 | 58 | | NavigationWorkMeter meter = immediate.WorkMeter; |
| | 572 | 59 | | meter.Reset(query.Budget); |
| | | 60 | | NavigationRayStatus status; |
| | | 61 | | NavigationRayResult result; |
| | | 62 | | try |
| | | 63 | | { |
| | | 64 | | do |
| | | 65 | | { |
| | 572 | 66 | | status = ray.Advance(meter); |
| | | 67 | | } |
| | 572 | 68 | | while (status == NavigationRayStatus.Pending); |
| | 572 | 69 | | result = ray.Result; |
| | 572 | 70 | | } |
| | | 71 | | finally |
| | | 72 | | { |
| | 572 | 73 | | ray.Reset(); |
| | 572 | 74 | | } |
| | 572 | 75 | | if (status != NavigationRayStatus.Success) |
| | 46 | 76 | | return status; |
| | 526 | 77 | | if (!result.IsSemanticCostNeutral) |
| | 2 | 78 | | return NavigationRayStatus.Blocked; |
| | 524 | 79 | | return ResolveDirectHeading(query.End.Position, actualFoot, out heading); |
| | | 80 | | } |
| | 576 | 81 | | } |
| | | 82 | | |
| | | 83 | | /// <summary> |
| | | 84 | | /// Requests one graph-backed A* guide for immutable query intent. |
| | | 85 | | /// </summary> |
| | | 86 | | public NavigationGuideStatus RequestGuide( |
| | | 87 | | PathQuery query, |
| | | 88 | | out NavigationGuideLease? result) |
| | | 89 | | { |
| | 385 | 90 | | EnsureUsable(); |
| | 383 | 91 | | result = null; |
| | | 92 | | |
| | 383 | 93 | | if (query.Algorithm != PathAlgorithm.AStar) |
| | | 94 | | { |
| | 1 | 95 | | return NavigationGuideStatus.Unsupported; |
| | | 96 | | } |
| | | 97 | | |
| | 382 | 98 | | NavigationAStarAdmissionGate gate = _context.Pathing.NavigationAStarAdmissionGate; |
| | 382 | 99 | | NavigationAStarQueryStatus beginStatus = gate.Begin(query, out NavigationAStarBatchWork work); |
| | 382 | 100 | | if (beginStatus != NavigationAStarQueryStatus.Pending) |
| | 1 | 101 | | return NavigationGuideStatusMapper.ToPublic(beginStatus); |
| | | 102 | | |
| | 381 | 103 | | using (work) |
| | | 104 | | { |
| | 845 | 105 | | while (!work.IsAdmissionComplete) |
| | | 106 | | { |
| | 464 | 107 | | work.AdvanceAdmission( |
| | 464 | 108 | | query.Budget.MaxLookupProbes, |
| | 464 | 109 | | query.Budget.MaxEndpointCandidates); |
| | | 110 | | } |
| | | 111 | | |
| | 536 | 112 | | while (!work.IsReadyToPublish(inputIndex: 0)) |
| | | 113 | | { |
| | 155 | 114 | | work.AdvanceSearch( |
| | 155 | 115 | | inputIndex: 0, |
| | 155 | 116 | | query.Budget.MaxLookupProbes, |
| | 155 | 117 | | nodeStepLimit: int.MaxValue, |
| | 155 | 118 | | query.Budget.MaxEvaluatedEdges, |
| | 155 | 119 | | query.Budget.MaxConnectionLegs); |
| | | 120 | | } |
| | | 121 | | |
| | 381 | 122 | | work.PublishReadyPrefix(maximumCount: 1); |
| | 381 | 123 | | NavigationAStarQueryStatus status = work.GetStatus(inputIndex: 0); |
| | 381 | 124 | | if (status != NavigationAStarQueryStatus.Success) |
| | 24 | 125 | | return NavigationGuideStatusMapper.ToPublic(status); |
| | | 126 | | |
| | 357 | 127 | | NavigationAStarPayloadLease payloadLease = work.TakeResult(inputIndex: 0); |
| | 357 | 128 | | return gate.PayloadCache.TryCreatePublicGuide( |
| | 357 | 129 | | _context.Pathing.NavigationGraphStore, |
| | 357 | 130 | | payloadLease, |
| | 357 | 131 | | out result); |
| | | 132 | | } |
| | 381 | 133 | | } |
| | | 134 | | |
| | | 135 | | /// <summary> |
| | | 136 | | /// Requests one graph-backed destination-centric flow field for immutable query intent. |
| | | 137 | | /// </summary> |
| | | 138 | | public NavigationGuideStatus RequestFlowField( |
| | | 139 | | PathQuery query, |
| | | 140 | | out NavigationFlowFieldLease? result) |
| | | 141 | | { |
| | 44 | 142 | | EnsureUsable(); |
| | 44 | 143 | | result = null; |
| | | 144 | | |
| | 44 | 145 | | if (!query.Agent.IsValid) |
| | 1 | 146 | | return NavigationGuideStatus.InvalidProfile; |
| | 43 | 147 | | if (query.Algorithm != PathAlgorithm.FlowField) |
| | | 148 | | { |
| | 1 | 149 | | return NavigationGuideStatus.Unsupported; |
| | | 150 | | } |
| | | 151 | | |
| | 42 | 152 | | NavigationFlowAdmissionGate gate = _context.Pathing.NavigationFlowAdmissionGate; |
| | 42 | 153 | | NavigationFlowQueryStatus beginStatus = gate.Begin(query, out NavigationFlowBatchWork work); |
| | 42 | 154 | | if (beginStatus != NavigationFlowQueryStatus.Pending) |
| | 1 | 155 | | return NavigationGuideStatusMapper.ToPublic(beginStatus); |
| | | 156 | | |
| | 41 | 157 | | using (work) |
| | | 158 | | { |
| | 109 | 159 | | while (!work.IsAdmissionComplete) |
| | | 160 | | { |
| | 68 | 161 | | work.AdvanceAdmission( |
| | 68 | 162 | | query.Budget.MaxLookupProbes, |
| | 68 | 163 | | query.Budget.MaxEndpointCandidates); |
| | | 164 | | } |
| | | 165 | | |
| | 72 | 166 | | while (!work.IsReadyToPublish(inputIndex: 0)) |
| | | 167 | | { |
| | 31 | 168 | | work.AdvanceSearch( |
| | 31 | 169 | | inputIndex: 0, |
| | 31 | 170 | | query.Budget.MaxLookupProbes, |
| | 31 | 171 | | nodeStepLimit: int.MaxValue, |
| | 31 | 172 | | query.Budget.MaxEvaluatedEdges, |
| | 31 | 173 | | query.Budget.MaxConnectionLegs); |
| | | 174 | | } |
| | | 175 | | |
| | 41 | 176 | | work.PublishReadyPrefix(maximumCount: 1); |
| | 41 | 177 | | NavigationFlowQueryStatus status = work.GetStatus(inputIndex: 0); |
| | 41 | 178 | | if (status != NavigationFlowQueryStatus.Success) |
| | 10 | 179 | | return NavigationGuideStatusMapper.ToPublic(status); |
| | | 180 | | |
| | 31 | 181 | | using NavigationFlowQueryResult flowResult = work.TakeResult(inputIndex: 0); |
| | 31 | 182 | | NavigationGuideStatus guideStatus = gate.PayloadCache.TryCreateGuide( |
| | 31 | 183 | | _context.Pathing.NavigationGraphStore, |
| | 31 | 184 | | flowResult, |
| | 31 | 185 | | out NavigationFlowFieldLease guide); |
| | 31 | 186 | | if (guideStatus != NavigationGuideStatus.Success) |
| | 1 | 187 | | return guideStatus; |
| | | 188 | | |
| | 30 | 189 | | result = guide; |
| | 30 | 190 | | return NavigationGuideStatus.Success; |
| | | 191 | | } |
| | 41 | 192 | | } |
| | | 193 | | |
| | | 194 | | private void EnsureUsable() |
| | | 195 | | { |
| | 1005 | 196 | | if (_context.IsDisposed) |
| | 1 | 197 | | throw new ObjectDisposedException(nameof(TrailblazerWorldContext)); |
| | 1004 | 198 | | SwiftThrowHelper.ThrowIfTrue( |
| | 1004 | 199 | | !_context.World.IsActive, |
| | 1004 | 200 | | message: "TrailblazerGuideService is bound to an inactive GridWorld."); |
| | 1003 | 201 | | } |
| | | 202 | | |
| | | 203 | | internal static NavigationRayStatus ResolveDirectHeading( |
| | | 204 | | Vector3d destination, |
| | | 205 | | Vector3d actualFoot, |
| | | 206 | | out Vector3d heading) |
| | | 207 | | { |
| | 525 | 208 | | heading = Vector3d.Zero; |
| | 525 | 209 | | if (!Vector3d.TrySubtract(destination, actualFoot, out Vector3d delta)) |
| | 1 | 210 | | return NavigationRayStatus.CostOverflow; |
| | 524 | 211 | | if (delta != Vector3d.Zero) |
| | 523 | 212 | | heading = delta.Normalized; |
| | 524 | 213 | | return NavigationRayStatus.Success; |
| | | 214 | | } |
| | | 215 | | |
| | | 216 | | } |