| | | 1 | | //======================================================================= |
| | | 2 | | // NavSteering.Simulation.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 System.Runtime.CompilerServices; |
| | | 10 | | using FixedMathSharp; |
| | | 11 | | using Trailblazer.Navigation.MovementGroups; |
| | | 12 | | using Trailblazer.Pathing; |
| | | 13 | | |
| | | 14 | | namespace Trailblazer.Navigation.Steering; |
| | | 15 | | |
| | | 16 | | public partial class NavSteering |
| | | 17 | | { |
| | | 18 | | #region Simulation Lifecycle |
| | | 19 | | |
| | | 20 | | /// <summary> |
| | | 21 | | /// Initializes the object by setting up its defaults, events, traversal state, and movement controller. |
| | | 22 | | /// </summary> |
| | | 23 | | protected virtual void OnInitialize() |
| | | 24 | | { |
| | 365 | 25 | | LeaveMovementGroup(); |
| | | 26 | | |
| | 365 | 27 | | _stoppedFrameCount = 0; |
| | 365 | 28 | | _autoStopFrameCount = 0; |
| | | 29 | | |
| | 365 | 30 | | _shouldRequestPathThisFrame = false; |
| | 365 | 31 | | _hasLineOfSightPath = false; |
| | 365 | 32 | | _shouldMove = false; |
| | | 33 | | |
| | 365 | 34 | | _isStuck = false; |
| | 365 | 35 | | _stuckFrameCount = 0; |
| | 365 | 36 | | _repathTries = 0; |
| | | 37 | | |
| | 365 | 38 | | _isAtDestination = false; |
| | | 39 | | |
| | 365 | 40 | | _currentQuery = null; |
| | 365 | 41 | | _pendingCommittedAreaPolicy = null; |
| | 365 | 42 | | _navigationGuideLease = null; |
| | 365 | 43 | | _navigationFlowFieldLease = null; |
| | 365 | 44 | | _requestedDestination = Vector3d.Zero; |
| | 365 | 45 | | _movementGroupSession.Reset(); |
| | 365 | 46 | | _movementGroupMode = MovementGroupTravelMode.None; |
| | 365 | 47 | | } |
| | | 48 | | |
| | | 49 | | internal void Reset() |
| | | 50 | | { |
| | 23 | 51 | | ReleaseNavigationGuidance(); |
| | 23 | 52 | | OnInitialize(); |
| | 23 | 53 | | } |
| | | 54 | | |
| | | 55 | | private TrailblazerWorldContext ResolveContext() |
| | | 56 | | { |
| | 9078 | 57 | | TrailblazerWorldContext context = _context!; |
| | 9078 | 58 | | TrailblazerWorldContext.ThrowIfUnusable(context); |
| | 9078 | 59 | | return context; |
| | | 60 | | } |
| | | 61 | | |
| | 8027 | 62 | | private MovementGroupCoordinatorState MovementGroups => ResolveContext().Navigation.MovementGroups; |
| | | 63 | | |
| | | 64 | | private void RemoveMovementGroupSession() |
| | | 65 | | { |
| | 589 | 66 | | ResolveContext().Navigation.MovementGroups.Remove(_movementGroupSession); |
| | 589 | 67 | | } |
| | | 68 | | |
| | 39 | 69 | | private int StuckFrameThresholdForContext => ResolveFrameRate() / 4; |
| | | 70 | | |
| | 3 | 71 | | private int AutoPauseStopTimeForContext => ResolveFrameRate() / 8; |
| | | 72 | | |
| | | 73 | | private int ResolveFrameRate() |
| | | 74 | | { |
| | 42 | 75 | | return ResolveContext().FrameRate; |
| | | 76 | | } |
| | | 77 | | |
| | | 78 | | /// <summary> |
| | | 79 | | /// Called every simulation step to handle agent steering and movement logic. |
| | | 80 | | /// </summary> |
| | | 81 | | public virtual Vector3d GetHeading( |
| | | 82 | | ISteer vessel, |
| | | 83 | | out NavigationTransitionInstruction? pendingTransition) |
| | | 84 | | { |
| | 151 | 85 | | pendingTransition = null; |
| | 151 | 86 | | CacheOwner(vessel); |
| | | 87 | | |
| | 151 | 88 | | if (!CanMove) |
| | 1 | 89 | | return Vector3d.Zero; |
| | | 90 | | |
| | 150 | 91 | | if (!ShouldMove || IsAtDestination) |
| | 17 | 92 | | return FinalizeIdleHeading(vessel.Speed); |
| | | 93 | | |
| | 133 | 94 | | if (!TryEnsureCurrentRequest(out Vector3d heading)) |
| | 1 | 95 | | return heading; |
| | | 96 | | |
| | 132 | 97 | | Vector3d pathPosition = vessel.Position |
| | 132 | 98 | | + Vector3d.Down * vessel.BodyShape.RootToFootOffsetY; |
| | 132 | 99 | | UpdateMovementGroupState(vessel.Position, vessel.Radius); |
| | | 100 | | |
| | 132 | 101 | | if (!TryPrepareMovementPathForHeading(pathPosition)) |
| | 4 | 102 | | return Vector3d.Zero; |
| | | 103 | | |
| | 128 | 104 | | UpdateTargetDirection( |
| | 128 | 105 | | vessel, |
| | 128 | 106 | | pathPosition, |
| | 128 | 107 | | out pendingTransition, |
| | 128 | 108 | | out bool isAStarTransitionApproach); |
| | 128 | 109 | | if (!ShouldMove || IsAtDestination) |
| | 4 | 110 | | return Vector3d.Zero; |
| | 124 | 111 | | if (_currentQuery.HasValue && _shouldRequestPathThisFrame) |
| | 7 | 112 | | return FinalizeHeadingFrame(); |
| | | 113 | | |
| | 117 | 114 | | if (ShouldArriveWithoutNavigationGuidance()) |
| | | 115 | | { |
| | 4 | 116 | | Arrive(); |
| | 4 | 117 | | return Vector3d.Zero; |
| | | 118 | | } |
| | | 119 | | |
| | 113 | 120 | | if (!CheckStuckStatus(vessel.Position, vessel.Speed, vessel.StuckThresholdSpeed)) |
| | | 121 | | { |
| | 2 | 122 | | TrailblazerLogger.DebugChannel.Info($"Stuck agent arriving!"); |
| | 2 | 123 | | Arrive(); |
| | 2 | 124 | | return Vector3d.Zero; |
| | | 125 | | } |
| | | 126 | | |
| | 111 | 127 | | if (UpdateNavigationGuidanceProgress( |
| | 111 | 128 | | vessel.Acceleration, |
| | 111 | 129 | | vessel.Speed, |
| | 111 | 130 | | isAStarTransitionApproach)) |
| | | 131 | | { |
| | 1 | 132 | | Arrive(); |
| | 1 | 133 | | return Vector3d.Zero; |
| | | 134 | | } |
| | 110 | 135 | | return FinalizeHeadingFrame(); |
| | | 136 | | } |
| | | 137 | | |
| | | 138 | | /// <summary> |
| | | 139 | | /// Periodically called to initiate a pathfinding query based on the current position and destination. |
| | | 140 | | /// Note: This runs once on the next simulation after a guided trek request is applied. |
| | | 141 | | /// </summary> |
| | | 142 | | protected virtual bool ValidateMovementPath(Vector3d origin) |
| | 102 | 143 | | => ValidateGraphMovementPath(origin); |
| | | 144 | | |
| | | 145 | | private bool ValidateGraphMovementPath(Vector3d origin) |
| | | 146 | | { |
| | 102 | 147 | | if (_hasLineOfSightPath) |
| | 2 | 148 | | return true; |
| | 100 | 149 | | if (_currentQuery!.Value.Algorithm == PathAlgorithm.FlowField) |
| | 34 | 150 | | return ValidateGraphFlowMovementPath(origin); |
| | | 151 | | |
| | 66 | 152 | | if (_navigationGuideLease?.Status is NavigationGuideStatus.Stale |
| | 66 | 153 | | or NavigationGuideStatus.CapacityExceeded) |
| | 2 | 154 | | PreparePathRetry(); |
| | | 155 | | |
| | 66 | 156 | | if (!_shouldRequestPathThisFrame) |
| | 18 | 157 | | return true; |
| | | 158 | | |
| | 48 | 159 | | _shouldRequestPathThisFrame = false; |
| | 48 | 160 | | PathQuery currentQuery = _currentQuery!.Value; |
| | 48 | 161 | | PathQuery query = currentQuery.WithStartState( |
| | 48 | 162 | | origin, |
| | 48 | 163 | | currentQuery.Traversal.StartMedium); |
| | 48 | 164 | | _currentQuery = query; |
| | 48 | 165 | | _requestedDestination = query.End.Position; |
| | 48 | 166 | | _destination = _requestedDestination; |
| | 48 | 167 | | if (TryHandleInitialGraphDirectTravel(query, origin)) |
| | 6 | 168 | | return true; |
| | 42 | 169 | | ReleaseNavigationGuidance(); |
| | 42 | 170 | | _pathCheckCooldown = PathRecheckCooldownFrames; |
| | | 171 | | |
| | 42 | 172 | | NavigationGuideStatus status = ResolveContext().Guides.RequestGuide( |
| | 42 | 173 | | query, |
| | 42 | 174 | | out NavigationGuideLease? lease); |
| | 42 | 175 | | if (status != NavigationGuideStatus.Success) |
| | | 176 | | { |
| | 4 | 177 | | if (status is NavigationGuideStatus.Stale |
| | 4 | 178 | | or NavigationGuideStatus.CapacityExceeded) |
| | | 179 | | { |
| | 3 | 180 | | PreparePathRetry(); |
| | 3 | 181 | | return true; |
| | | 182 | | } |
| | | 183 | | |
| | 1 | 184 | | return false; |
| | | 185 | | } |
| | | 186 | | |
| | 38 | 187 | | _navigationGuideLease = lease!.Value; |
| | 38 | 188 | | return true; |
| | | 189 | | } |
| | | 190 | | |
| | | 191 | | private bool ValidateGraphFlowMovementPath(Vector3d origin) |
| | | 192 | | { |
| | 34 | 193 | | PathQuery currentQuery = _currentQuery!.Value; |
| | 34 | 194 | | NavigationGuideStatus currentStatus = _navigationFlowFieldLease?.Status |
| | 34 | 195 | | ?? NavigationGuideStatus.Stale; |
| | 34 | 196 | | if (_navigationFlowFieldLease != null |
| | 34 | 197 | | && currentStatus is NavigationGuideStatus.Stale |
| | 34 | 198 | | or NavigationGuideStatus.CapacityExceeded) |
| | | 199 | | { |
| | 1 | 200 | | PreparePathRetry(); |
| | | 201 | | } |
| | | 202 | | |
| | 34 | 203 | | if (!_shouldRequestPathThisFrame) |
| | 9 | 204 | | return currentStatus == NavigationGuideStatus.Success; |
| | | 205 | | |
| | 25 | 206 | | _shouldRequestPathThisFrame = false; |
| | 25 | 207 | | PathQuery query = currentQuery.WithStartState( |
| | 25 | 208 | | origin, |
| | 25 | 209 | | currentQuery.Traversal.StartMedium); |
| | 25 | 210 | | _currentQuery = query; |
| | 25 | 211 | | _requestedDestination = query.End.Position; |
| | 25 | 212 | | _destination = _requestedDestination; |
| | 25 | 213 | | if (TryHandleInitialGraphDirectTravel(query, origin)) |
| | 1 | 214 | | return true; |
| | 24 | 215 | | ReleaseNavigationGuidance(); |
| | 24 | 216 | | _pathCheckCooldown = PathRecheckCooldownFrames; |
| | | 217 | | |
| | 24 | 218 | | NavigationGuideStatus status = ResolveContext().Guides.RequestFlowField( |
| | 24 | 219 | | query, |
| | 24 | 220 | | out NavigationFlowFieldLease? lease); |
| | 24 | 221 | | if (status != NavigationGuideStatus.Success) |
| | | 222 | | { |
| | 3 | 223 | | if (status is NavigationGuideStatus.Stale |
| | 3 | 224 | | or NavigationGuideStatus.CapacityExceeded) |
| | | 225 | | { |
| | 2 | 226 | | PreparePathRetry(); |
| | 2 | 227 | | return true; |
| | | 228 | | } |
| | | 229 | | |
| | 1 | 230 | | return false; |
| | | 231 | | } |
| | | 232 | | |
| | 21 | 233 | | _navigationFlowFieldLease = lease!.Value; |
| | 21 | 234 | | return true; |
| | | 235 | | } |
| | | 236 | | |
| | | 237 | | private bool TryHandleInitialGraphDirectTravel(PathQuery query, Vector3d origin) |
| | | 238 | | { |
| | 73 | 239 | | if ((query.Traversal.TargetMedia |
| | 73 | 240 | | & NavigationCell.ToMedia(query.Traversal.StartMedium)) == 0) |
| | | 241 | | { |
| | 19 | 242 | | _hasLineOfSightPath = false; |
| | 19 | 243 | | return false; |
| | | 244 | | } |
| | 54 | 245 | | NavigationRayStatus status = ResolveContext().Guides.TryGetDirectHeading( |
| | 54 | 246 | | query, |
| | 54 | 247 | | origin, |
| | 54 | 248 | | out _); |
| | 54 | 249 | | if (status == NavigationRayStatus.Success) |
| | | 250 | | { |
| | 6 | 251 | | _hasLineOfSightPath = true; |
| | 6 | 252 | | ReleaseNavigationGuidance(); |
| | 6 | 253 | | _pathCheckCooldown = PathRecheckCooldownFrames; |
| | 6 | 254 | | return true; |
| | | 255 | | } |
| | | 256 | | |
| | 48 | 257 | | _hasLineOfSightPath = false; |
| | 48 | 258 | | if (status != NavigationRayStatus.Stale) |
| | 47 | 259 | | return false; |
| | 1 | 260 | | PreparePathRetry(); |
| | 1 | 261 | | return true; |
| | | 262 | | } |
| | | 263 | | |
| | | 264 | | /// <summary> |
| | | 265 | | /// Computes the steering direction toward the destination or along the path. |
| | | 266 | | /// </summary> |
| | | 267 | | protected virtual Vector3d FindTargetDirection( |
| | | 268 | | Vector3d position, |
| | | 269 | | out NavigationTransitionInstruction? pendingTransition) |
| | | 270 | | { |
| | 104 | 271 | | pendingTransition = null; |
| | 104 | 272 | | Vector3d targetDirection = Vector3d.Zero; |
| | 104 | 273 | | if (HasLineOfSightPath) |
| | 11 | 274 | | targetDirection = Destination - position; |
| | 93 | 275 | | else if (_navigationFlowFieldLease is NavigationFlowFieldLease flowGuide) |
| | | 276 | | { |
| | 30 | 277 | | NavigationGuideStatus status = flowGuide.TrySample( |
| | 30 | 278 | | position, |
| | 30 | 279 | | ResolveContext().Settings.GuideSampleBudget, |
| | 30 | 280 | | out NavigationFlowSample sample); |
| | 30 | 281 | | targetDirection = sample.Heading; |
| | 30 | 282 | | if (status is NavigationGuideStatus.Stale |
| | 30 | 283 | | or NavigationGuideStatus.CapacityExceeded) |
| | | 284 | | { |
| | 1 | 285 | | PreparePathRetry(); |
| | 1 | 286 | | return Vector3d.Zero; |
| | | 287 | | } |
| | 29 | 288 | | if (status == NavigationGuideStatus.BudgetExceeded) |
| | 2 | 289 | | return Vector3d.Zero; |
| | 27 | 290 | | if (status == NavigationGuideStatus.LocalRecoveryRequired) |
| | 3 | 291 | | return Vector3d.Zero; |
| | 24 | 292 | | if (sample.HasTransition) |
| | | 293 | | { |
| | 3 | 294 | | pendingTransition = sample.Transition; |
| | 3 | 295 | | return Vector3d.Zero; |
| | | 296 | | } |
| | 21 | 297 | | if (targetDirection == Vector3d.Zero) |
| | | 298 | | { |
| | 3 | 299 | | Arrive(); |
| | 3 | 300 | | return Vector3d.Zero; |
| | | 301 | | } |
| | | 302 | | } |
| | 63 | 303 | | else if (_navigationGuideLease != null) |
| | | 304 | | { |
| | 55 | 305 | | NavigationGuideLease guide = _navigationGuideLease.Value; |
| | 55 | 306 | | guide.TryGetCurrentStep(out NavigationGuideStep step); |
| | | 307 | | |
| | 55 | 308 | | if (step.HasTransition) |
| | | 309 | | { |
| | 6 | 310 | | targetDirection = step.Position - position; |
| | 6 | 311 | | if (targetDirection == Vector3d.Zero) |
| | | 312 | | { |
| | 4 | 313 | | pendingTransition = step.Transition; |
| | 4 | 314 | | return Vector3d.Zero; |
| | | 315 | | } |
| | | 316 | | } |
| | | 317 | | else |
| | | 318 | | { |
| | 49 | 319 | | targetDirection = step.Position - position; |
| | | 320 | | } |
| | 51 | 321 | | if (targetDirection == Vector3d.Zero) |
| | | 322 | | { |
| | 43 | 323 | | if (IsAtFinalWaypoint(guide)) |
| | | 324 | | { |
| | 3 | 325 | | ReleaseNavigationGuidance(); |
| | 3 | 326 | | return Vector3d.Zero; |
| | | 327 | | } |
| | | 328 | | |
| | 40 | 329 | | guide.TryAdvanceStep(); |
| | 40 | 330 | | guide.TryGetCurrentStep(out step); |
| | | 331 | | |
| | 40 | 332 | | if (step.HasTransition) |
| | | 333 | | { |
| | 16 | 334 | | targetDirection = step.Position - position; |
| | 16 | 335 | | if (targetDirection == Vector3d.Zero) |
| | | 336 | | { |
| | 14 | 337 | | pendingTransition = step.Transition; |
| | 14 | 338 | | return Vector3d.Zero; |
| | | 339 | | } |
| | | 340 | | } |
| | | 341 | | else |
| | | 342 | | { |
| | 24 | 343 | | targetDirection = step.Position - position; |
| | | 344 | | } |
| | | 345 | | } |
| | | 346 | | } |
| | 71 | 347 | | if (targetDirection == Vector3d.Zero) |
| | | 348 | | { |
| | 9 | 349 | | TrailblazerLogger.DebugChannel.Info($"No viable movement direction found."); |
| | 9 | 350 | | return Vector3d.Zero; |
| | | 351 | | } |
| | | 352 | | |
| | | 353 | | // This is now the direction we want to be travelling in |
| | 62 | 354 | | return targetDirection.NormalizeInPlace(out _distanceToTarget); |
| | | 355 | | } |
| | | 356 | | |
| | | 357 | | /// <summary> |
| | | 358 | | /// Returns true if the agent passed a step within its arrival radius, |
| | | 359 | | /// or entered the explicit ordinary-step tolerance. |
| | | 360 | | /// </summary> |
| | | 361 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 362 | | public bool ShouldAdvanceToNextWaypoint() |
| | | 363 | | { |
| | 33 | 364 | | Fixed64 arrivalRadius = _currentQuery?.Agent.ArrivalRadius ?? Fixed64.Zero; |
| | 33 | 365 | | return (_distanceToTarget <= arrivalRadius |
| | 33 | 366 | | && Vector3d.Dot(TargetDirection, LastTargetDirection) < Fixed64.Zero) |
| | 33 | 367 | | || _distanceToTarget <= WaypointTolerance; |
| | | 368 | | } |
| | | 369 | | |
| | | 370 | | /// <summary> |
| | | 371 | | /// Evaluates the agent's current movement direction and velocity, updating stuck and arrival state. |
| | | 372 | | /// </summary> |
| | | 373 | | protected virtual bool CheckStuckStatus( |
| | | 374 | | Vector3d position, |
| | | 375 | | Fixed64 speed, |
| | | 376 | | Fixed64 stuckThreshold) |
| | | 377 | | { |
| | 158 | 378 | | if (!CanAutoStop) |
| | 32 | 379 | | return true; |
| | | 380 | | |
| | 126 | 381 | | if (stuckThreshold <= Fixed64.Zero || speed >= stuckThreshold) |
| | 87 | 382 | | return ResetStuckStatus(); |
| | | 383 | | |
| | 39 | 384 | | _stuckFrameCount++; |
| | 39 | 385 | | if (_stuckFrameCount <= StuckFrameThresholdForContext) |
| | 24 | 386 | | return true; |
| | | 387 | | |
| | 15 | 388 | | return _repathTries < StuckRepathTries |
| | 15 | 389 | | ? TryRecoverFromStuck(position) |
| | 15 | 390 | | : DeclareHardStuck(); |
| | | 391 | | } |
| | | 392 | | |
| | | 393 | | private Vector3d FinalizeIdleHeading(Fixed64 speed) |
| | | 394 | | { |
| | 17 | 395 | | _targetDirection = Vector3d.Zero; |
| | 17 | 396 | | if (speed <= Fixed64.Epsilon) |
| | 16 | 397 | | _stoppedFrameCount++; |
| | | 398 | | |
| | 17 | 399 | | return FinalizeHeadingFrame(); |
| | | 400 | | } |
| | | 401 | | |
| | | 402 | | private bool TryEnsureCurrentRequest(out Vector3d heading) |
| | | 403 | | { |
| | 133 | 404 | | if (_currentQuery.HasValue) |
| | | 405 | | { |
| | 132 | 406 | | heading = Vector3d.Zero; |
| | 132 | 407 | | return true; |
| | | 408 | | } |
| | | 409 | | |
| | 1 | 410 | | Arrive(); |
| | 1 | 411 | | heading = TargetDirection; |
| | 1 | 412 | | return false; |
| | | 413 | | } |
| | | 414 | | |
| | | 415 | | private bool TryPrepareMovementPathForHeading(Vector3d position) |
| | | 416 | | { |
| | 132 | 417 | | if (CanPathfind && !ValidateMovementPath(position)) |
| | | 418 | | { |
| | 4 | 419 | | HandleInvalidPath("Invalid path detected!"); |
| | 4 | 420 | | return false; |
| | | 421 | | } |
| | | 422 | | |
| | 128 | 423 | | RefreshLineOfSightState(position); |
| | 128 | 424 | | return true; |
| | | 425 | | } |
| | | 426 | | |
| | | 427 | | private void RefreshLineOfSightState(Vector3d position) |
| | | 428 | | { |
| | 128 | 429 | | if (_pathCheckCooldown > 0) |
| | 123 | 430 | | return; |
| | | 431 | | |
| | 5 | 432 | | if (_currentQuery is not PathQuery currentQuery) |
| | 1 | 433 | | return; |
| | 4 | 434 | | bool wasDirect = _hasLineOfSightPath; |
| | 4 | 435 | | PathQuery query = currentQuery.WithStartState( |
| | 4 | 436 | | position, |
| | 4 | 437 | | currentQuery.Traversal.StartMedium); |
| | 4 | 438 | | if ((query.Traversal.TargetMedia |
| | 4 | 439 | | & NavigationCell.ToMedia(query.Traversal.StartMedium)) == 0) |
| | | 440 | | { |
| | 1 | 441 | | _hasLineOfSightPath = false; |
| | 1 | 442 | | _pathCheckCooldown = PathRecheckCooldownFrames; |
| | 1 | 443 | | return; |
| | | 444 | | } |
| | 3 | 445 | | NavigationRayStatus status = ResolveContext().Guides.TryGetDirectHeading( |
| | 3 | 446 | | query, |
| | 3 | 447 | | position, |
| | 3 | 448 | | out _); |
| | 3 | 449 | | if (status == NavigationRayStatus.Success) |
| | | 450 | | { |
| | 1 | 451 | | _currentQuery = query; |
| | 1 | 452 | | _hasLineOfSightPath = true; |
| | 1 | 453 | | ReleaseNavigationGuidance(); |
| | | 454 | | } |
| | 2 | 455 | | else if (status == NavigationRayStatus.Stale || wasDirect) |
| | | 456 | | { |
| | 1 | 457 | | _hasLineOfSightPath = false; |
| | 1 | 458 | | PreparePathRetry(); |
| | | 459 | | } |
| | 3 | 460 | | _pathCheckCooldown = PathRecheckCooldownFrames; |
| | 3 | 461 | | } |
| | | 462 | | |
| | | 463 | | private void HandleInvalidPath(string debugMessage) |
| | | 464 | | { |
| | 4 | 465 | | TrailblazerLogger.DebugChannel.Info($"{debugMessage}"); |
| | 4 | 466 | | Events.OnInvalidPath?.Invoke(); |
| | 4 | 467 | | Arrive(); |
| | 4 | 468 | | } |
| | | 469 | | |
| | | 470 | | private void UpdateTargetDirection( |
| | | 471 | | ISteer vessel, |
| | | 472 | | Vector3d pathPosition, |
| | | 473 | | out NavigationTransitionInstruction? pendingTransition, |
| | | 474 | | out bool isAStarTransitionApproach) |
| | | 475 | | { |
| | 128 | 476 | | _lastTargetDirection = _targetDirection; |
| | 128 | 477 | | _targetDirection = FindTargetDirection( |
| | 128 | 478 | | pathPosition, |
| | 128 | 479 | | out pendingTransition); |
| | 128 | 480 | | isAStarTransitionApproach = pendingTransition == null |
| | 128 | 481 | | && IsCurrentAStarTransitionStep(); |
| | 128 | 482 | | if (_targetDirection == Vector3d.Zero) |
| | 41 | 483 | | return; |
| | 87 | 484 | | _targetDirection += ComputeCombinedSteering( |
| | 87 | 485 | | vessel.Position, |
| | 87 | 486 | | vessel.Velocity, |
| | 87 | 487 | | vessel.Speed, |
| | 87 | 488 | | vessel.Radius, |
| | 87 | 489 | | vessel.GlobalId); |
| | 87 | 490 | | } |
| | | 491 | | |
| | | 492 | | private bool IsCurrentAStarTransitionStep() |
| | | 493 | | { |
| | 107 | 494 | | if (_navigationGuideLease is not NavigationGuideLease guide) |
| | 73 | 495 | | return false; |
| | | 496 | | |
| | 34 | 497 | | guide.TryGetCurrentStep(out NavigationGuideStep step); |
| | 34 | 498 | | return step.HasTransition; |
| | | 499 | | } |
| | | 500 | | |
| | | 501 | | private bool ShouldArriveWithoutNavigationGuidance() |
| | | 502 | | { |
| | 117 | 503 | | if (HasNavigationGuidance) |
| | 78 | 504 | | return false; |
| | | 505 | | |
| | 39 | 506 | | Fixed64 moveAmount = FixedMath.Clamp01(TargetDirection.Magnitude); |
| | 39 | 507 | | bool reachedTarget = _distanceToTarget <= _currentQuery!.Value.Agent.ArrivalRadius; |
| | 39 | 508 | | bool noInput = moveAmount == Fixed64.Zero; |
| | 39 | 509 | | return reachedTarget || (!IsStuck && noInput); |
| | | 510 | | } |
| | | 511 | | |
| | | 512 | | private bool UpdateNavigationGuidanceProgress( |
| | | 513 | | Vector3d acceleration, |
| | | 514 | | Fixed64 speed, |
| | | 515 | | bool isAStarTransitionApproach) |
| | | 516 | | { |
| | 111 | 517 | | if (TargetDirection == Vector3d.Zero) |
| | 34 | 518 | | return false; |
| | | 519 | | |
| | 77 | 520 | | if (!isAStarTransitionApproach && _navigationGuideLease is NavigationGuideLease currentGuide) |
| | | 521 | | { |
| | 30 | 522 | | if (IsAtFinalWaypoint(currentGuide)) |
| | | 523 | | { |
| | 2 | 524 | | if (_distanceToTarget <= _currentQuery!.Value.Agent.ArrivalRadius) |
| | | 525 | | { |
| | 1 | 526 | | ReleaseNavigationGuidance(); |
| | 1 | 527 | | return true; |
| | | 528 | | } |
| | | 529 | | } |
| | 28 | 530 | | else if (ShouldAdvanceToNextWaypoint()) |
| | | 531 | | { |
| | 27 | 532 | | currentGuide.TryAdvanceStep(); |
| | | 533 | | } |
| | | 534 | | } |
| | | 535 | | |
| | 76 | 536 | | if (HasNavigationGuidance) |
| | 51 | 537 | | SetDeceleration(acceleration, speed); |
| | | 538 | | |
| | 76 | 539 | | return false; |
| | | 540 | | } |
| | | 541 | | |
| | | 542 | | private static bool IsAtFinalWaypoint(NavigationGuideLease guide) => |
| | 73 | 543 | | guide.CurrentStepIndex == guide.StepCount - 1; |
| | | 544 | | |
| | | 545 | | internal NavigationGuideStatus CompletePendingTransition( |
| | | 546 | | in NavigationTransitionInstruction instruction) |
| | | 547 | | { |
| | 9 | 548 | | PathQuery query = _currentQuery!.Value; |
| | 9 | 549 | | NavigationGuideStatus status = query.Algorithm == PathAlgorithm.AStar |
| | 9 | 550 | | ? _navigationGuideLease!.Value.CompletePendingTransition(instruction) |
| | 9 | 551 | | : _navigationFlowFieldLease!.Value.CompletePendingTransition(instruction); |
| | 9 | 552 | | if (status == NavigationGuideStatus.Success) |
| | | 553 | | { |
| | 5 | 554 | | _currentQuery = query.WithStartState( |
| | 5 | 555 | | query.Start.Position, |
| | 5 | 556 | | instruction.DestinationMedium); |
| | | 557 | | } |
| | 9 | 558 | | return status; |
| | | 559 | | } |
| | | 560 | | |
| | | 561 | | private Vector3d FinalizeHeadingFrame() |
| | | 562 | | { |
| | 134 | 563 | | _autoStopFrameCount--; |
| | 134 | 564 | | _pathCheckCooldown--; |
| | | 565 | | |
| | 134 | 566 | | Events.OnStartTraversal?.Invoke(TargetDirection); |
| | 134 | 567 | | return TargetDirection; |
| | | 568 | | } |
| | | 569 | | |
| | | 570 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 571 | | private bool ResetStuckStatus() |
| | | 572 | | { |
| | 87 | 573 | | _isStuck = false; |
| | 87 | 574 | | _stuckFrameCount = 0; |
| | 87 | 575 | | _repathTries = 0; |
| | 87 | 576 | | return true; |
| | | 577 | | } |
| | | 578 | | |
| | | 579 | | private bool TryRecoverFromStuck(Vector3d position) |
| | | 580 | | { |
| | 12 | 581 | | _hasLineOfSightPath = false; |
| | | 582 | | |
| | 12 | 583 | | if (IsInGroup) |
| | 1 | 584 | | LeaveMovementGroup(); |
| | | 585 | | |
| | 12 | 586 | | PreparePathRetry(); |
| | 12 | 587 | | _repathTries++; |
| | 12 | 588 | | return true; |
| | | 589 | | } |
| | | 590 | | |
| | | 591 | | private void PreparePathRetry() |
| | | 592 | | { |
| | 23 | 593 | | _targetDirection = Vector3d.Zero; |
| | 23 | 594 | | _shouldRequestPathThisFrame = true; |
| | 23 | 595 | | DisposeCurrentNavigationGuidance(); |
| | 23 | 596 | | } |
| | | 597 | | |
| | | 598 | | private bool DeclareHardStuck() |
| | | 599 | | { |
| | 3 | 600 | | _isStuck = true; |
| | 3 | 601 | | DisposeCurrentNavigationGuidance(); |
| | 3 | 602 | | Events.OnIsStuck?.Invoke(); |
| | 3 | 603 | | return false; |
| | | 604 | | } |
| | | 605 | | |
| | | 606 | | private void DisposeCurrentNavigationGuidance() |
| | | 607 | | { |
| | 26 | 608 | | ReleaseNavigationGuidance(); |
| | 26 | 609 | | } |
| | | 610 | | |
| | | 611 | | /// <summary> |
| | | 612 | | /// Adjusts the target direction to decelerate the object as it approaches its destination based on the specified |
| | | 613 | | /// acceleration and current speed. |
| | | 614 | | /// </summary> |
| | | 615 | | /// <remarks> |
| | | 616 | | /// This method is intended to be overridden in derived classes to customize deceleration behavior. |
| | | 617 | | /// It modulates the target direction to ensure smooth slowing as the object nears its target. |
| | | 618 | | /// </remarks> |
| | | 619 | | /// <param name="acceleration"> |
| | | 620 | | /// The acceleration vector used to determine the deceleration rate. |
| | | 621 | | /// If the vector is zero, a default braking power is used. |
| | | 622 | | /// </param> |
| | | 623 | | /// <param name="speed">The current speed of the object, used to calculate the distance required to slow down.</para |
| | | 624 | | protected virtual void SetDeceleration(Vector3d acceleration, Fixed64 speed) |
| | | 625 | | { |
| | | 626 | | // Scaling direction before passing to the motor lets us |
| | | 627 | | // modulate movement before acceleration is applied |
| | 56 | 628 | | Fixed64 deceleration = acceleration != Vector3d.Zero |
| | 56 | 629 | | ? acceleration.Magnitude |
| | 56 | 630 | | : BrakingPower; |
| | 56 | 631 | | Fixed64 slowDistance = speed / deceleration; |
| | 56 | 632 | | if (DistanceToTarget > Fixed64.Epsilon && DistanceToTarget <= slowDistance) |
| | | 633 | | { |
| | 3 | 634 | | Fixed64 closingSpeed = DistanceToTarget / slowDistance; |
| | 3 | 635 | | _targetDirection *= closingSpeed; // reduce magnitude = slow down |
| | | 636 | | } |
| | 56 | 637 | | } |
| | | 638 | | |
| | | 639 | | /// <summary> |
| | | 640 | | /// Triggers the arrival event and resets internal movement tracking. |
| | | 641 | | /// </summary> |
| | | 642 | | public void Arrive() |
| | | 643 | | { |
| | 22 | 644 | | NavigationAreaPolicyKey? areaPolicy = _currentQuery?.AreaPolicy; |
| | 22 | 645 | | StopMove(); |
| | 22 | 646 | | _pendingCommittedAreaPolicy = areaPolicy; |
| | | 647 | | |
| | 22 | 648 | | _distanceToTarget = Fixed64.Zero; |
| | 22 | 649 | | _isAtDestination = true; |
| | 22 | 650 | | _targetDirection = Vector3d.Zero; |
| | | 651 | | |
| | 22 | 652 | | Events.OnArrive?.Invoke(); |
| | 6 | 653 | | } |
| | | 654 | | |
| | | 655 | | /// <summary> |
| | | 656 | | /// Resets the movement and pathfinding logic, halting the agent. |
| | | 657 | | /// </summary> |
| | | 658 | | public virtual void StopMove() |
| | | 659 | | { |
| | 53 | 660 | | bool wasMoving = _shouldMove; |
| | 53 | 661 | | _pendingCommittedAreaPolicy = null; |
| | 53 | 662 | | ReleaseNavigationGuidance(); |
| | 53 | 663 | | _pendingTransitionOwner?.NotifySteeringSessionEnded(); |
| | 53 | 664 | | _currentQuery = null; |
| | 53 | 665 | | _requestedDestination = Vector3d.Zero; |
| | 53 | 666 | | _destination = Vector3d.Zero; |
| | 53 | 667 | | _shouldMove = false; |
| | 53 | 668 | | _shouldRequestPathThisFrame = false; |
| | 53 | 669 | | _hasLineOfSightPath = false; |
| | 53 | 670 | | LeaveMovementGroup(); |
| | 53 | 671 | | if (!wasMoving) |
| | 17 | 672 | | return; |
| | | 673 | | |
| | 36 | 674 | | _autoStopFrameCount = 0; |
| | 36 | 675 | | _stuckFrameCount = 0; |
| | 36 | 676 | | _stoppedFrameCount = 0; |
| | | 677 | | |
| | 36 | 678 | | Events.OnStopMove?.Invoke(); |
| | 2 | 679 | | } |
| | | 680 | | |
| | | 681 | | #endregion |
| | | 682 | | } |