| | | 1 | | using FixedMathSharp; |
| | | 2 | | using System.Runtime.CompilerServices; |
| | | 3 | | using Trailblazer.Navigation.Motor; |
| | | 4 | | using Trailblazer.Navigation.Steering; |
| | | 5 | | using Trailblazer.Pathing; |
| | | 6 | | |
| | | 7 | | namespace Trailblazer.Navigation; |
| | | 8 | | |
| | | 9 | | internal static class NavigatorGuidedTraversalState |
| | | 10 | | { |
| | | 11 | | public static bool ResolveInitialClimbIntent( |
| | | 12 | | IPathRequest pathRequest, |
| | | 13 | | GuidedVolumeExitHandoff? pendingVolumeExitHandoff, |
| | | 14 | | bool? requestedClimb, |
| | | 15 | | out GuidedClimbIntentMode intentMode) |
| | | 16 | | { |
| | 51 | 17 | | if (requestedClimb.HasValue) |
| | | 18 | | { |
| | 13 | 19 | | intentMode = GuidedClimbIntentMode.Explicit; |
| | 13 | 20 | | return requestedClimb.Value; |
| | | 21 | | } |
| | | 22 | | |
| | 38 | 23 | | intentMode = GuidedClimbIntentMode.Auto; |
| | 38 | 24 | | return GuidedClimbIntentResolver.Resolve(pathRequest, pendingVolumeExitHandoff); |
| | | 25 | | } |
| | | 26 | | |
| | | 27 | | public static void PrepareFrame( |
| | | 28 | | bool isGuided, |
| | | 29 | | ref TrekRequest frameRequest, |
| | | 30 | | NavSteering? steering, |
| | | 31 | | GuidedVolumeExitHandoff? pendingVolumeExitHandoff, |
| | | 32 | | ref bool climbIntent, |
| | | 33 | | ref GuidedClimbIntentMode climbIntentMode, |
| | | 34 | | ref int lastSeenRouteTopologyVersion) |
| | | 35 | | { |
| | 44 | 36 | | if (!isGuided) |
| | 8 | 37 | | return; |
| | | 38 | | |
| | 36 | 39 | | if (TryClearInactiveClimbIntent( |
| | 36 | 40 | | ref frameRequest, |
| | 36 | 41 | | steering, |
| | 36 | 42 | | pendingVolumeExitHandoff, |
| | 36 | 43 | | ref climbIntent, |
| | 36 | 44 | | ref climbIntentMode, |
| | 36 | 45 | | ref lastSeenRouteTopologyVersion)) |
| | | 46 | | { |
| | 4 | 47 | | return; |
| | | 48 | | } |
| | | 49 | | |
| | 32 | 50 | | frameRequest.IsRequestingClimb = climbIntent; |
| | 32 | 51 | | } |
| | | 52 | | |
| | | 53 | | public static void SyncFromSteering( |
| | | 54 | | bool isGuided, |
| | | 55 | | ref TrekRequest frameRequest, |
| | | 56 | | NavSteering? steering, |
| | | 57 | | GuidedVolumeExitHandoff? pendingVolumeExitHandoff, |
| | | 58 | | bool activatedVolumeExitHandoff, |
| | | 59 | | bool handoffRequestedClimb, |
| | | 60 | | ref bool climbIntent, |
| | | 61 | | ref GuidedClimbIntentMode climbIntentMode, |
| | | 62 | | ref int lastSeenRouteTopologyVersion) |
| | | 63 | | { |
| | 36 | 64 | | if (!isGuided) |
| | 0 | 65 | | return; |
| | | 66 | | |
| | 36 | 67 | | if (TryClearInactiveClimbIntent( |
| | 36 | 68 | | ref frameRequest, |
| | 36 | 69 | | steering, |
| | 36 | 70 | | pendingVolumeExitHandoff, |
| | 36 | 71 | | ref climbIntent, |
| | 36 | 72 | | ref climbIntentMode, |
| | 36 | 73 | | ref lastSeenRouteTopologyVersion)) |
| | | 74 | | { |
| | 4 | 75 | | return; |
| | | 76 | | } |
| | | 77 | | |
| | 32 | 78 | | if (climbIntentMode == GuidedClimbIntentMode.Auto |
| | 32 | 79 | | && steering != null |
| | 32 | 80 | | && steering.CurrentRouteTopologyVersion != lastSeenRouteTopologyVersion) |
| | | 81 | | { |
| | 25 | 82 | | bool resolvedRouteRequestsClimb = steering.CurrentRouteRequestsClimbIntent; |
| | 25 | 83 | | bool shouldDeferHandoffBootstrapClear = |
| | 25 | 84 | | activatedVolumeExitHandoff |
| | 25 | 85 | | && handoffRequestedClimb |
| | 25 | 86 | | && !resolvedRouteRequestsClimb; |
| | 25 | 87 | | if (!shouldDeferHandoffBootstrapClear) |
| | | 88 | | { |
| | 21 | 89 | | climbIntent = resolvedRouteRequestsClimb; |
| | 21 | 90 | | lastSeenRouteTopologyVersion = steering.CurrentRouteTopologyVersion; |
| | | 91 | | } |
| | | 92 | | } |
| | | 93 | | |
| | 32 | 94 | | frameRequest.IsRequestingClimb = climbIntent; |
| | 32 | 95 | | } |
| | | 96 | | |
| | | 97 | | public static bool TryActivatePendingVolumeExitHandoff( |
| | | 98 | | bool isGuided, |
| | | 99 | | TrailblazerWorldContext context, |
| | | 100 | | Vector3d position, |
| | | 101 | | Fixed64 size, |
| | | 102 | | ref TrekRequest frameRequest, |
| | | 103 | | NavSteering? steering, |
| | | 104 | | ref GuidedVolumeExitHandoff? pendingVolumeExitHandoff, |
| | | 105 | | ref bool climbIntent, |
| | | 106 | | GuidedClimbIntentMode climbIntentMode, |
| | | 107 | | ref int lastSeenRouteTopologyVersion, |
| | | 108 | | out bool handoffRequestedClimb) |
| | | 109 | | { |
| | 44 | 110 | | handoffRequestedClimb = false; |
| | 44 | 111 | | if (!isGuided |
| | 44 | 112 | | || pendingVolumeExitHandoff == null |
| | 44 | 113 | | || steering == null |
| | 44 | 114 | | || steering.ShouldMove |
| | 44 | 115 | | || steering.CurrentRequest != null) |
| | | 116 | | { |
| | 32 | 117 | | return false; |
| | | 118 | | } |
| | | 119 | | |
| | 12 | 120 | | if (!pendingVolumeExitHandoff.TryCreateFollowupRequest(context, position, size, out IPathRequest? followupReques |
| | 12 | 121 | | || followupRequest == null) |
| | | 122 | | { |
| | 1 | 123 | | return false; |
| | | 124 | | } |
| | | 125 | | |
| | 11 | 126 | | GuidedVolumeExitHandoff handoff = pendingVolumeExitHandoff; |
| | 11 | 127 | | pendingVolumeExitHandoff = null; |
| | | 128 | | |
| | 11 | 129 | | steering.ApplyPathRequest(followupRequest, handoff.MovementGroupId); |
| | 11 | 130 | | CaptureRouteTopologyVersion(steering, ref lastSeenRouteTopologyVersion); |
| | 11 | 131 | | frameRequest.IsRequestingFlight = false; |
| | 11 | 132 | | frameRequest.IsRequestingSwim = false; |
| | 11 | 133 | | handoffRequestedClimb = handoff.IsRequestingClimb; |
| | 11 | 134 | | if (climbIntentMode == GuidedClimbIntentMode.Auto) |
| | 11 | 135 | | climbIntent = handoffRequestedClimb; |
| | | 136 | | |
| | 11 | 137 | | frameRequest.IsRequestingClimb = climbIntent; |
| | 11 | 138 | | return true; |
| | | 139 | | } |
| | | 140 | | |
| | | 141 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 142 | | public static void SetClimbIntent( |
| | | 143 | | ref TrekRequest frameRequest, |
| | | 144 | | bool status, |
| | | 145 | | GuidedClimbIntentMode mode, |
| | | 146 | | ref bool climbIntent, |
| | | 147 | | ref GuidedClimbIntentMode climbIntentMode) |
| | | 148 | | { |
| | 1 | 149 | | climbIntent = status; |
| | 1 | 150 | | climbIntentMode = mode; |
| | 1 | 151 | | frameRequest.IsRequestingClimb = status; |
| | 1 | 152 | | } |
| | | 153 | | |
| | | 154 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 155 | | public static void ResetClimbIntent( |
| | | 156 | | ref bool climbIntent, |
| | | 157 | | ref GuidedClimbIntentMode climbIntentMode, |
| | | 158 | | ref int lastSeenRouteTopologyVersion) |
| | | 159 | | { |
| | 30 | 160 | | climbIntent = false; |
| | 30 | 161 | | climbIntentMode = GuidedClimbIntentMode.Auto; |
| | 30 | 162 | | lastSeenRouteTopologyVersion = 0; |
| | 30 | 163 | | } |
| | | 164 | | |
| | | 165 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 166 | | public static void CaptureRouteTopologyVersion( |
| | | 167 | | NavSteering? steering, |
| | | 168 | | ref int lastSeenRouteTopologyVersion) |
| | | 169 | | { |
| | 62 | 170 | | lastSeenRouteTopologyVersion = steering?.CurrentRouteTopologyVersion ?? 0; |
| | 62 | 171 | | } |
| | | 172 | | |
| | | 173 | | private static bool TryClearInactiveClimbIntent( |
| | | 174 | | ref TrekRequest frameRequest, |
| | | 175 | | NavSteering? steering, |
| | | 176 | | GuidedVolumeExitHandoff? pendingVolumeExitHandoff, |
| | | 177 | | ref bool climbIntent, |
| | | 178 | | ref GuidedClimbIntentMode climbIntentMode, |
| | | 179 | | ref int lastSeenRouteTopologyVersion) |
| | | 180 | | { |
| | 72 | 181 | | if (steering?.CurrentRequest != null |
| | 72 | 182 | | || pendingVolumeExitHandoff != null) |
| | | 183 | | { |
| | 64 | 184 | | return false; |
| | | 185 | | } |
| | | 186 | | |
| | 8 | 187 | | ResetClimbIntent(ref climbIntent, ref climbIntentMode, ref lastSeenRouteTopologyVersion); |
| | 8 | 188 | | frameRequest.IsRequestingClimb = false; |
| | 8 | 189 | | return true; |
| | | 190 | | } |
| | | 191 | | } |