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