| | | 1 | | //======================================================================= |
| | | 2 | | // NavTurning.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 Chronicler; |
| | | 11 | | using FixedMathSharp; |
| | | 12 | | |
| | | 13 | | namespace Trailblazer.Navigation.Turning; |
| | | 14 | | |
| | | 15 | | /// <summary> |
| | | 16 | | /// The Turn class manages the character's rotation and turning functionality. |
| | | 17 | | /// </summary> |
| | | 18 | | public partial class NavTurning : IRecordable |
| | | 19 | | { |
| | | 20 | | #region Constants |
| | | 21 | | |
| | | 22 | | /// <summary> |
| | | 23 | | /// The minimum physical angular difference (in degrees) required to initiate a turn. |
| | | 24 | | /// </summary> |
| | 1 | 25 | | private static readonly Fixed64 _minTurnRequiredAngle = |
| | 1 | 26 | | Fixed64.FromRaw(0x9520000L); // 0.036407470703125f * 2^32; |
| | | 27 | | |
| | | 28 | | /// <summary> |
| | | 29 | | /// The physical angular threshold (in degrees) below which a turn is considered complete. |
| | | 30 | | /// </summary> |
| | 1 | 31 | | private static readonly Fixed64 _arriveThresholdAngle = |
| | 1 | 32 | | Fixed64.FromRaw(0x68DB9L); // 0.0001M; |
| | | 33 | | |
| | | 34 | | /// <summary> |
| | | 35 | | /// Represents the default turn rate value used for rotation calculations. |
| | | 36 | | /// </summary> |
| | | 37 | | /// <remarks> |
| | | 38 | | /// This value is typically used as a standard or baseline turn rate in movement or rotation logic. |
| | | 39 | | /// The specific value is one eighth of a full rotation, which may correspond to 45 degrees if a full rotation is co |
| | | 40 | | /// </remarks> |
| | 1 | 41 | | public static readonly Fixed64 DefaultTurnRate = Fixed64.One / 8; |
| | | 42 | | |
| | | 43 | | #endregion |
| | | 44 | | |
| | | 45 | | #region Fields |
| | | 46 | | |
| | | 47 | | /// <summary> |
| | | 48 | | /// Whether this turning controller is currently allowed to perform turns. |
| | | 49 | | /// </summary> |
| | 307 | 50 | | public bool CanTurn = true; |
| | | 51 | | |
| | | 52 | | /// <summary> |
| | | 53 | | /// The base turn rate, controlling how much rotation is applied per simulation step. |
| | | 54 | | /// </summary> |
| | 307 | 55 | | public Fixed64 TurnRate = DefaultTurnRate; |
| | | 56 | | |
| | | 57 | | /// <summary> |
| | | 58 | | /// Buffered target rotation quaternion to apply on the next simulation frame. |
| | | 59 | | /// </summary> |
| | | 60 | | private FixedQuaternion? _pendingTarget; |
| | | 61 | | |
| | | 62 | | /// <summary> |
| | | 63 | | /// Custom interpolation factor to override the default TurnRate for the next turn operation. |
| | | 64 | | /// </summary> |
| | | 65 | | private Fixed64 _pendingInterpolation; |
| | | 66 | | |
| | | 67 | | /// <summary> |
| | | 68 | | /// Navigator radius cached so collision auto-turn thresholds can track frame-rate changes. |
| | | 69 | | /// </summary> |
| | | 70 | | private Fixed64 _radius; |
| | | 71 | | |
| | | 72 | | private TrailblazerWorldContext? _context; |
| | | 73 | | |
| | | 74 | | /// <summary> |
| | | 75 | | /// Flag indicating that a collision has occurred and an auto-turn should be considered. |
| | | 76 | | /// </summary> |
| | | 77 | | private bool _isColliding; |
| | | 78 | | |
| | | 79 | | #endregion |
| | | 80 | | |
| | | 81 | | #region Properties |
| | | 82 | | |
| | | 83 | | /// <summary> |
| | | 84 | | /// Indicates whether the current turn operation has completed (i.e., the target rotation has been reached). |
| | | 85 | | /// </summary> |
| | | 86 | | public bool TargetReached { get; private set; } |
| | | 87 | | |
| | | 88 | | /// <summary> |
| | | 89 | | /// The desired target rotation quaternion that the object is turning toward. |
| | | 90 | | /// </summary> |
| | | 91 | | public FixedQuaternion TargetRotation { get; private set; } |
| | | 92 | | |
| | | 93 | | #endregion |
| | | 94 | | |
| | | 95 | | #region Actions and Functions |
| | | 96 | | |
| | | 97 | | /// <summary> |
| | | 98 | | /// Optional predicate that determines whether an auto-turn is permitted after a collision. |
| | | 99 | | /// </summary> |
| | | 100 | | public Func<bool>? CanTurnOnCollision { get; set; } = null; |
| | | 101 | | |
| | | 102 | | #endregion |
| | | 103 | | |
| | | 104 | | /// <summary> |
| | | 105 | | /// Constructs and immediately initializes a context-bound <see cref="NavTurning"/>. |
| | | 106 | | /// </summary> |
| | 307 | 107 | | public NavTurning(TrailblazerWorldContext context, Fixed64 radius) |
| | | 108 | | { |
| | 307 | 109 | | BindContext(context); |
| | 304 | 110 | | OnInitialize(radius); |
| | 304 | 111 | | } |
| | | 112 | | |
| | | 113 | | /// <summary> |
| | | 114 | | /// Binds this turning controller to a world context. |
| | | 115 | | /// </summary> |
| | | 116 | | public void BindContext(TrailblazerWorldContext context) |
| | | 117 | | { |
| | 338 | 118 | | TrailblazerWorldContext.ThrowIfUnusable(context); |
| | 335 | 119 | | _context = context; |
| | 335 | 120 | | } |
| | | 121 | | |
| | 19 | 122 | | private TrailblazerWorldContext RequireContext() => _context!; |
| | | 123 | | |
| | 9 | 124 | | private int FrameRate => RequireContext().FrameRate; |
| | | 125 | | |
| | 10 | 126 | | private Fixed64 DeltaTime => RequireContext().DeltaTime; |
| | | 127 | | |
| | | 128 | | /// <summary> |
| | | 129 | | /// Configures internal thresholds based on the object’s radius and resets turn state. |
| | | 130 | | /// </summary> |
| | | 131 | | internal void OnInitialize(Fixed64 radius) |
| | | 132 | | { |
| | 334 | 133 | | _radius = radius; |
| | 334 | 134 | | TargetReached = true; |
| | 334 | 135 | | TargetRotation = FixedQuaternion.Identity; |
| | | 136 | | |
| | 334 | 137 | | _pendingTarget = null; |
| | 334 | 138 | | _pendingInterpolation = Fixed64.Zero; |
| | 334 | 139 | | _isColliding = false; |
| | 334 | 140 | | } |
| | | 141 | | |
| | | 142 | | /// <summary> |
| | | 143 | | ///Advances the object’s rotation toward the <see cref="TargetRotation"/>, handling both buffered and auto-turn logi |
| | | 144 | | /// </summary> |
| | | 145 | | public bool TrySimulateTurn( |
| | | 146 | | Vector3d position, |
| | | 147 | | Vector3d lastPosition, |
| | | 148 | | Vector3d forward, |
| | | 149 | | FixedQuaternion rotation, |
| | | 150 | | out FixedQuaternion appliedRotation) |
| | | 151 | | { |
| | 54 | 152 | | appliedRotation = FixedQuaternion.Identity; |
| | | 153 | | // 1) Preconditions |
| | 55 | 154 | | if (!CanTurn) return false; |
| | | 155 | | |
| | | 156 | | // 2) If we’re idle (finished last turn): |
| | 53 | 157 | | if (TargetReached) |
| | | 158 | | { |
| | | 159 | | // 2a) Phase-2: consume a buffered turn (if any) *and continue* to rotation |
| | 52 | 160 | | if (_pendingTarget.HasValue) |
| | | 161 | | { |
| | 12 | 162 | | TargetRotation = _pendingTarget.Value; |
| | 12 | 163 | | _pendingTarget = null; |
| | 12 | 164 | | TargetReached = false; |
| | | 165 | | // **no return** here — we want to immediately start turning |
| | | 166 | | } |
| | | 167 | | // 2b) Phase-1: no buffer yet, check for collision and buffer, then bail |
| | | 168 | | else |
| | | 169 | | { |
| | 40 | 170 | | CheckAutoTurn( |
| | 40 | 171 | | position, |
| | 40 | 172 | | lastPosition, |
| | 40 | 173 | | forward); |
| | 40 | 174 | | return false; // only here do we exit early |
| | | 175 | | } |
| | | 176 | | } |
| | | 177 | | |
| | | 178 | | // 3) Mid-turn (or just consumed buffer): do the Slerp |
| | 13 | 179 | | var t = _pendingInterpolation > Fixed64.Zero |
| | 13 | 180 | | ? _pendingInterpolation |
| | 13 | 181 | | : TurnRate * DeltaTime; |
| | 13 | 182 | | t = FixedMath.Clamp(t, Fixed64.Zero, Fixed64.One); |
| | | 183 | | |
| | 13 | 184 | | var next = FixedQuaternion.Slerp(rotation, TargetRotation, t); |
| | | 185 | | |
| | 13 | 186 | | if (FixedQuaternion.Angle(next, TargetRotation) <= _arriveThresholdAngle) |
| | | 187 | | { |
| | | 188 | | // we’ve arrived |
| | 1 | 189 | | appliedRotation = TargetRotation; |
| | 1 | 190 | | StopTurn(); |
| | | 191 | | } |
| | | 192 | | else |
| | 12 | 193 | | appliedRotation = next; |
| | | 194 | | |
| | 13 | 195 | | return true; |
| | | 196 | | } |
| | | 197 | | |
| | | 198 | | /// <summary> |
| | | 199 | | /// Checks if a recent collision and sufficient movement warrant buffering an auto-turn, and buffers it if so. |
| | | 200 | | /// </summary> |
| | | 201 | | private void CheckAutoTurn( |
| | | 202 | | Vector3d position, |
| | | 203 | | Vector3d lastPosition, |
| | | 204 | | Vector3d curDirection) |
| | | 205 | | { |
| | 71 | 206 | | if (!_isColliding) return; |
| | | 207 | | |
| | | 208 | | // 1) compute delta first |
| | 9 | 209 | | Vector3d delta = position - lastPosition; |
| | 9 | 210 | | if (delta.MagnitudeSquared < GetCollisionTurnThreshold()) |
| | | 211 | | { |
| | | 212 | | // keep _isColliding true so we retry next frame |
| | 1 | 213 | | return; |
| | | 214 | | } |
| | | 215 | | |
| | 8 | 216 | | Func<bool>? canTurnOnCollision = CanTurnOnCollision; |
| | 8 | 217 | | if (canTurnOnCollision != null && !canTurnOnCollision()) |
| | 2 | 218 | | return; |
| | | 219 | | |
| | | 220 | | // 2) now we know we’ll actually turn |
| | 6 | 221 | | _isColliding = false; |
| | 6 | 222 | | delta.NormalizeInPlace(); |
| | 6 | 223 | | RequestTurnDirection(curDirection, delta); |
| | 6 | 224 | | } |
| | | 225 | | |
| | | 226 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 227 | | private Fixed64 GetCollisionTurnThreshold() |
| | | 228 | | { |
| | 9 | 229 | | Fixed64 threshold = _radius / FrameRate * Fixed64.Half; |
| | 9 | 230 | | return threshold * threshold; |
| | | 231 | | } |
| | | 232 | | |
| | | 233 | | /// <summary> |
| | | 234 | | /// Buffers a new turn request toward the given target direction if the required angle exceeds the minimum threshold |
| | | 235 | | /// </summary> |
| | | 236 | | public void RequestTurnDirection( |
| | | 237 | | Vector3d curDirection, |
| | | 238 | | Vector3d targetDirection, |
| | | 239 | | Fixed64? interpolation = null) |
| | | 240 | | { |
| | 20 | 241 | | if (!NeedsTurn(curDirection, targetDirection)) |
| | 6 | 242 | | return; |
| | | 243 | | |
| | 14 | 244 | | _pendingInterpolation = interpolation ?? Fixed64.Zero; |
| | 14 | 245 | | _pendingTarget = FixedQuaternion.FromDirection(targetDirection); |
| | 14 | 246 | | } |
| | | 247 | | |
| | | 248 | | /// <summary> |
| | | 249 | | /// Determines whether the angular difference between the current forward and a desired target direction exceeds the |
| | | 250 | | /// </summary> |
| | | 251 | | public static bool NeedsTurn( |
| | | 252 | | Vector3d currentForward, |
| | | 253 | | Vector3d targetDirection, |
| | | 254 | | Fixed64? minAngle = null) |
| | | 255 | | { |
| | 23 | 256 | | FixedQuaternion currentRotation = FixedQuaternion.FromDirection(currentForward); |
| | 23 | 257 | | FixedQuaternion targetRotation = FixedQuaternion.FromDirection(targetDirection); |
| | | 258 | | |
| | 23 | 259 | | Fixed64 angle = FixedQuaternion.Angle(currentRotation, targetRotation); |
| | 23 | 260 | | bool withinTurn = angle <= (minAngle ?? _minTurnRequiredAngle); |
| | | 261 | | |
| | 23 | 262 | | return !withinTurn; |
| | | 263 | | } |
| | | 264 | | |
| | | 265 | | /// <summary> |
| | | 266 | | /// Marks the current turn operation as complete, allowing new turns to be requested. |
| | | 267 | | /// </summary> |
| | 2 | 268 | | public void StopTurn() => TargetReached = true; |
| | | 269 | | |
| | | 270 | | /// <summary> |
| | | 271 | | /// Signals that a collision has occurred and an auto-turn should be evaluated on the next call to <see cref="TrySim |
| | | 272 | | /// </summary> |
| | | 273 | | public void NotifyCollision() |
| | | 274 | | { |
| | 8 | 275 | | _isColliding = true; |
| | 8 | 276 | | } |
| | | 277 | | |
| | | 278 | | /// <inheritdoc /> |
| | | 279 | | public void RecordData(IChronicler chronicler) |
| | | 280 | | { |
| | 139 | 281 | | RecordValues.Look(chronicler, ref CanTurn, "CanTurn", true); |
| | 139 | 282 | | RecordValues.Look(chronicler, ref TurnRate, "TurnRate", DefaultTurnRate); |
| | 139 | 283 | | } |
| | | 284 | | } |