| | | 1 | | using MemoryPack; |
| | | 2 | | using System; |
| | | 3 | | using System.Runtime.CompilerServices; |
| | | 4 | | using System.Text.Json.Serialization; |
| | | 5 | | |
| | | 6 | | namespace FixedMathSharp; |
| | | 7 | | |
| | | 8 | | /// <summary> |
| | | 9 | | /// Represents a quaternion (x, y, z, w) with fixed-point numbers. |
| | | 10 | | /// Quaternions are useful for representing rotations and can be used to perform smooth rotations and avoid gimbal lock. |
| | | 11 | | /// </summary> |
| | | 12 | | [Serializable] |
| | | 13 | | [MemoryPackable] |
| | | 14 | | public partial struct FixedQuaternion : IEquatable<FixedQuaternion> |
| | | 15 | | { |
| | | 16 | | #region Static Readonly Fields |
| | | 17 | | |
| | | 18 | | /// <summary> |
| | | 19 | | /// Identity quaternion (0, 0, 0, 1). |
| | | 20 | | /// </summary> |
| | | 21 | | public static readonly FixedQuaternion Identity = new(Fixed64.Zero, Fixed64.Zero, Fixed64.Zero, Fixed64.One); |
| | | 22 | | |
| | | 23 | | /// <summary> |
| | | 24 | | /// Empty quaternion (0, 0, 0, 0). |
| | | 25 | | /// </summary> |
| | | 26 | | public static readonly FixedQuaternion Zero = new(Fixed64.Zero, Fixed64.Zero, Fixed64.Zero, Fixed64.Zero); |
| | | 27 | | |
| | | 28 | | #endregion |
| | | 29 | | |
| | | 30 | | #region Fields and Constants |
| | | 31 | | |
| | | 32 | | /// <summary> |
| | | 33 | | /// Represents the X component of the vector as a fixed-point value. |
| | | 34 | | /// </summary> |
| | | 35 | | [JsonInclude] |
| | | 36 | | [MemoryPackOrder(0)] |
| | | 37 | | public Fixed64 x; |
| | | 38 | | |
| | | 39 | | /// <summary> |
| | | 40 | | /// Represents the Y component of the vector as a fixed-point value. |
| | | 41 | | /// </summary> |
| | | 42 | | [JsonInclude] |
| | | 43 | | [MemoryPackOrder(1)] |
| | | 44 | | public Fixed64 y; |
| | | 45 | | |
| | | 46 | | /// <summary> |
| | | 47 | | /// Represents the Z component of the vector as a fixed-point value. |
| | | 48 | | /// </summary> |
| | | 49 | | [JsonInclude] |
| | | 50 | | [MemoryPackOrder(2)] |
| | | 51 | | public Fixed64 z; |
| | | 52 | | |
| | | 53 | | /// <summary> |
| | | 54 | | /// Represents the W component of the vector as a fixed-point value. |
| | | 55 | | /// </summary> |
| | | 56 | | [JsonInclude] |
| | | 57 | | [MemoryPackOrder(3)] |
| | | 58 | | public Fixed64 w; |
| | | 59 | | |
| | | 60 | | #endregion |
| | | 61 | | |
| | | 62 | | #region Constructors |
| | | 63 | | |
| | | 64 | | /// <summary> |
| | | 65 | | /// Creates a new FixedQuaternion with the specified components. |
| | | 66 | | /// </summary> |
| | | 67 | | public FixedQuaternion(Fixed64 x, Fixed64 y, Fixed64 z, Fixed64 w) |
| | 237 | 68 | | { |
| | 237 | 69 | | this.x = x; |
| | 237 | 70 | | this.y = y; |
| | 237 | 71 | | this.z = z; |
| | 237 | 72 | | this.w = w; |
| | 237 | 73 | | } |
| | | 74 | | |
| | | 75 | | #endregion |
| | | 76 | | |
| | | 77 | | #region Properties |
| | | 78 | | |
| | | 79 | | /// <summary> |
| | | 80 | | /// Normalized version of this quaternion. |
| | | 81 | | /// </summary> |
| | | 82 | | [JsonIgnore] |
| | | 83 | | [MemoryPackIgnore] |
| | | 84 | | public FixedQuaternion Normal |
| | | 85 | | { |
| | | 86 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | 11 | 87 | | get => GetNormalized(this); |
| | | 88 | | } |
| | | 89 | | |
| | | 90 | | /// <summary> |
| | | 91 | | /// Gets the magnitude of this quaternion. |
| | | 92 | | /// </summary> |
| | | 93 | | [JsonIgnore] |
| | | 94 | | [MemoryPackIgnore] |
| | | 95 | | public Fixed64 Magnitude |
| | | 96 | | { |
| | | 97 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | 2 | 98 | | get => GetMagnitude(this); |
| | | 99 | | } |
| | | 100 | | |
| | | 101 | | /// <summary> |
| | | 102 | | /// Gets the squared magnitude of this quaternion. |
| | | 103 | | /// </summary> |
| | | 104 | | [JsonIgnore] |
| | | 105 | | [MemoryPackIgnore] |
| | | 106 | | public Fixed64 SqrMagnitude |
| | | 107 | | { |
| | | 108 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | 6 | 109 | | get => x * x + y * y + z * z + w * w; |
| | | 110 | | } |
| | | 111 | | |
| | | 112 | | /// <summary> |
| | | 113 | | /// Returns the Euler angles (in degrees) of this quaternion. |
| | | 114 | | /// </summary> |
| | | 115 | | [JsonIgnore] |
| | | 116 | | [MemoryPackIgnore] |
| | | 117 | | public Vector3d EulerAngles |
| | | 118 | | { |
| | | 119 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | 1 | 120 | | get => ToEulerAngles(); |
| | | 121 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | 1 | 122 | | set => this = FromEulerAnglesInDegrees(value.x, value.y, value.z); |
| | | 123 | | } |
| | | 124 | | |
| | | 125 | | /// <summary> |
| | | 126 | | /// Gets or sets the component value at the specified index. |
| | | 127 | | /// </summary> |
| | | 128 | | /// <remarks>Index 0 corresponds to the x component, 1 to y, 2 to z, and 3 to w.</remarks> |
| | | 129 | | /// <param name="index">The zero-based index of the component to access. Valid values are 0 (x), 1 (y), 2 (z), and 3 |
| | | 130 | | /// <returns>The value of the component at the specified index.</returns> |
| | | 131 | | /// <exception cref="IndexOutOfRangeException">Thrown when the specified index is less than 0 or greater than 3.</ex |
| | | 132 | | [JsonIgnore] |
| | | 133 | | [MemoryPackIgnore] |
| | | 134 | | public Fixed64 this[int index] |
| | | 135 | | { |
| | | 136 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 137 | | get |
| | 5 | 138 | | { |
| | 5 | 139 | | return index switch |
| | 5 | 140 | | { |
| | 1 | 141 | | 0 => x, |
| | 1 | 142 | | 1 => y, |
| | 1 | 143 | | 2 => z, |
| | 1 | 144 | | 3 => w, |
| | 1 | 145 | | _ => throw new IndexOutOfRangeException("Invalid FixedQuaternion index!"), |
| | 5 | 146 | | }; |
| | 4 | 147 | | } |
| | | 148 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 149 | | set |
| | 5 | 150 | | { |
| | 5 | 151 | | switch (index) |
| | | 152 | | { |
| | | 153 | | case 0: |
| | 1 | 154 | | x = value; |
| | 1 | 155 | | break; |
| | | 156 | | case 1: |
| | 1 | 157 | | y = value; |
| | 1 | 158 | | break; |
| | | 159 | | case 2: |
| | 1 | 160 | | z = value; |
| | 1 | 161 | | break; |
| | | 162 | | case 3: |
| | 1 | 163 | | w = value; |
| | 1 | 164 | | break; |
| | | 165 | | default: |
| | 1 | 166 | | throw new IndexOutOfRangeException("Invalid FixedQuaternion index!"); |
| | | 167 | | } |
| | 4 | 168 | | } |
| | | 169 | | } |
| | | 170 | | |
| | | 171 | | #endregion |
| | | 172 | | |
| | | 173 | | #region Methods (Instance) |
| | | 174 | | |
| | | 175 | | /// <summary> |
| | | 176 | | /// Set x, y, z and w components of an existing Quaternion. |
| | | 177 | | /// </summary> |
| | | 178 | | /// <param name="newX"></param> |
| | | 179 | | /// <param name="newY"></param> |
| | | 180 | | /// <param name="newZ"></param> |
| | | 181 | | /// <param name="newW"></param> |
| | | 182 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 183 | | public void Set(Fixed64 newX, Fixed64 newY, Fixed64 newZ, Fixed64 newW) |
| | 1 | 184 | | { |
| | 1 | 185 | | x = newX; |
| | 1 | 186 | | y = newY; |
| | 1 | 187 | | z = newZ; |
| | 1 | 188 | | w = newW; |
| | 1 | 189 | | } |
| | | 190 | | |
| | | 191 | | /// <summary> |
| | | 192 | | /// Normalizes this quaternion in place. |
| | | 193 | | /// </summary> |
| | | 194 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 195 | | public FixedQuaternion Normalize() |
| | 5 | 196 | | { |
| | 5 | 197 | | return this = GetNormalized(this); |
| | 5 | 198 | | } |
| | | 199 | | |
| | | 200 | | /// <summary> |
| | | 201 | | /// Returns the conjugate of this quaternion (inverses the rotational effect). |
| | | 202 | | /// </summary> |
| | | 203 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 204 | | public FixedQuaternion Conjugate() |
| | 2 | 205 | | { |
| | 2 | 206 | | return new FixedQuaternion(-x, -y, -z, w); |
| | 2 | 207 | | } |
| | | 208 | | |
| | | 209 | | /// <summary> |
| | | 210 | | /// Returns the inverse of this quaternion. |
| | | 211 | | /// </summary> |
| | | 212 | | public FixedQuaternion Inverse() |
| | 8 | 213 | | { |
| | 13 | 214 | | if (this == Identity) return Identity; |
| | 3 | 215 | | Fixed64 norm = SqrMagnitude; |
| | 4 | 216 | | if (norm == Fixed64.Zero) return this; // Handle division by zero by returning the same quaternion |
| | | 217 | | |
| | 2 | 218 | | Fixed64 invNorm = Fixed64.One / norm; |
| | 2 | 219 | | return new FixedQuaternion(x * -invNorm, y * -invNorm, z * -invNorm, w * invNorm); |
| | 8 | 220 | | } |
| | | 221 | | |
| | | 222 | | /// <summary> |
| | | 223 | | /// Rotates a vector by this quaternion. |
| | | 224 | | /// </summary> |
| | | 225 | | public Vector3d Rotate(Vector3d v) |
| | 1 | 226 | | { |
| | 1 | 227 | | FixedQuaternion normalizedQuat = Normal; |
| | 1 | 228 | | FixedQuaternion vQuat = new(v.x, v.y, v.z, Fixed64.Zero); |
| | 1 | 229 | | FixedQuaternion invQuat = normalizedQuat.Conjugate(); |
| | 1 | 230 | | FixedQuaternion rotatedVQuat = (normalizedQuat * vQuat) * invQuat; |
| | 1 | 231 | | return new Vector3d(rotatedVQuat.x, rotatedVQuat.y, rotatedVQuat.z); |
| | 1 | 232 | | } |
| | | 233 | | |
| | | 234 | | /// <summary> |
| | | 235 | | /// Rotates this quaternion by a given angle around a specified axis (default: Y-axis). |
| | | 236 | | /// </summary> |
| | | 237 | | /// <param name="sin">Sine of the rotation angle.</param> |
| | | 238 | | /// <param name="cos">Cosine of the rotation angle.</param> |
| | | 239 | | /// <param name="axis">The axis to rotate around (default: Vector3d.Up).</param> |
| | | 240 | | /// <returns>A new quaternion representing the rotated result.</returns> |
| | | 241 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 242 | | public FixedQuaternion Rotated(Fixed64 sin, Fixed64 cos, Vector3d? axis = null) |
| | 4 | 243 | | { |
| | 4 | 244 | | Vector3d rotateAxis = axis ?? Vector3d.Up; |
| | | 245 | | |
| | | 246 | | // The rotation angle is the arc tangent of sin and cos |
| | 4 | 247 | | Fixed64 angle = FixedMath.Atan2(sin, cos); |
| | | 248 | | |
| | | 249 | | // Construct a quaternion representing a rotation around the axis (default is y aka Vector3d.up) |
| | 4 | 250 | | FixedQuaternion rotationQuat = FromAxisAngle(rotateAxis, angle); |
| | | 251 | | |
| | | 252 | | // Apply the rotation and return the result |
| | 4 | 253 | | return rotationQuat * this; |
| | 4 | 254 | | } |
| | | 255 | | |
| | | 256 | | #endregion |
| | | 257 | | |
| | | 258 | | #region Quaternion Operations |
| | | 259 | | |
| | | 260 | | /// <summary> |
| | | 261 | | /// Checks if this vector has been normalized by checking if the magnitude is close to 1. |
| | | 262 | | /// </summary> |
| | | 263 | | public bool IsNormalized() |
| | 3 | 264 | | { |
| | 3 | 265 | | Fixed64 mag = GetMagnitude(this); |
| | 3 | 266 | | return FixedMath.Abs(mag - Fixed64.One) <= Fixed64.Epsilon; |
| | 3 | 267 | | } |
| | | 268 | | |
| | | 269 | | /// <summary> |
| | | 270 | | /// Calculates the magnitude (or length) of the specified quaternion. |
| | | 271 | | /// </summary> |
| | | 272 | | /// <remarks> |
| | | 273 | | /// If rounding errors cause the computed magnitude to be slightly greater than 1 but within epsilon, the result is |
| | | 274 | | /// This helps maintain numerical stability when working with normalized quaternions.</remarks> |
| | | 275 | | /// <param name="q">The quaternion for which to compute the magnitude.</param> |
| | | 276 | | /// <returns>The magnitude of the quaternion as a Fixed64 value. Returns 0 if the quaternion is the zero quaternion. |
| | | 277 | | public static Fixed64 GetMagnitude(FixedQuaternion q) |
| | 109 | 278 | | { |
| | 109 | 279 | | Fixed64 mag = (q.x * q.x) + (q.y * q.y) + (q.z * q.z) + (q.w * q.w); |
| | | 280 | | // If rounding error caused the final magnitude to be slightly above 1, clamp it |
| | 109 | 281 | | if (mag > Fixed64.One && mag <= Fixed64.One + Fixed64.Epsilon) |
| | 56 | 282 | | return Fixed64.One; |
| | | 283 | | |
| | 53 | 284 | | return mag != Fixed64.Zero ? FixedMath.Sqrt(mag) : Fixed64.Zero; |
| | 109 | 285 | | } |
| | | 286 | | |
| | | 287 | | /// <summary> |
| | | 288 | | /// Normalizes the quaternion to a unit quaternion. |
| | | 289 | | /// </summary> |
| | | 290 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 291 | | public static FixedQuaternion GetNormalized(FixedQuaternion q) |
| | 100 | 292 | | { |
| | 100 | 293 | | Fixed64 mag = GetMagnitude(q); |
| | | 294 | | |
| | | 295 | | // If magnitude is zero, return identity quaternion (to avoid divide by zero) |
| | 100 | 296 | | if (mag == Fixed64.Zero) |
| | 1 | 297 | | return new FixedQuaternion(Fixed64.Zero, Fixed64.Zero, Fixed64.Zero, Fixed64.One); |
| | | 298 | | |
| | | 299 | | // If already normalized, return as-is |
| | 99 | 300 | | if (FixedMath.Abs(mag - Fixed64.One) <= Fixed64.Epsilon) |
| | 73 | 301 | | return q; |
| | | 302 | | |
| | | 303 | | // Normalize it exactly |
| | 26 | 304 | | return new FixedQuaternion( |
| | 26 | 305 | | q.x / mag, |
| | 26 | 306 | | q.y / mag, |
| | 26 | 307 | | q.z / mag, |
| | 26 | 308 | | q.w / mag |
| | 26 | 309 | | ); |
| | 100 | 310 | | } |
| | | 311 | | |
| | | 312 | | /// <summary> |
| | | 313 | | /// Divides one quaternion by another using inverse quaternion multiplication. |
| | | 314 | | /// </summary> |
| | | 315 | | /// <remarks> |
| | | 316 | | /// This is equivalent to <c>dividend * Inverse(divisor)</c>. |
| | | 317 | | /// </remarks> |
| | | 318 | | /// <exception cref="InvalidOperationException"> |
| | | 319 | | /// Thrown when <paramref name="divisor"/> is not invertible. |
| | | 320 | | /// </exception> |
| | | 321 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 322 | | public static FixedQuaternion Divide(FixedQuaternion dividend, FixedQuaternion divisor) |
| | 2 | 323 | | { |
| | 2 | 324 | | Fixed64 divisorSqrMagnitude = divisor.SqrMagnitude; |
| | 2 | 325 | | if (divisorSqrMagnitude == Fixed64.Zero) |
| | 1 | 326 | | throw new InvalidOperationException("Quaternion divisor is not invertible."); |
| | | 327 | | |
| | 1 | 328 | | Fixed64 invNorm = Fixed64.One / divisorSqrMagnitude; |
| | 1 | 329 | | FixedQuaternion inverseDivisor = new( |
| | 1 | 330 | | -divisor.x * invNorm, |
| | 1 | 331 | | -divisor.y * invNorm, |
| | 1 | 332 | | -divisor.z * invNorm, |
| | 1 | 333 | | divisor.w * invNorm); |
| | | 334 | | |
| | 1 | 335 | | return dividend * inverseDivisor; |
| | 1 | 336 | | } |
| | | 337 | | |
| | | 338 | | /// <summary> |
| | | 339 | | /// Creates a quaternion that rotates one vector to align with another. |
| | | 340 | | /// </summary> |
| | | 341 | | /// <param name="forward">The forward direction vector.</param> |
| | | 342 | | /// <param name="upwards">The upwards direction vector (optional, default: Vector3d.Up).</param> |
| | | 343 | | /// <returns>A quaternion representing the rotation from one direction to another.</returns> |
| | | 344 | | public static FixedQuaternion LookRotation(Vector3d forward, Vector3d? upwards = null) |
| | 2 | 345 | | { |
| | 2 | 346 | | Vector3d up = upwards ?? Vector3d.Up; |
| | | 347 | | |
| | 2 | 348 | | Vector3d forwardNormalized = forward.Normal; |
| | 2 | 349 | | Vector3d right = Vector3d.Cross(up.Normal, forwardNormalized); |
| | 2 | 350 | | up = Vector3d.Cross(forwardNormalized, right); |
| | | 351 | | |
| | 2 | 352 | | return FromMatrix(new Fixed3x3(right.x, up.x, forwardNormalized.x, |
| | 2 | 353 | | right.y, up.y, forwardNormalized.y, |
| | 2 | 354 | | right.z, up.z, forwardNormalized.z)); |
| | 2 | 355 | | } |
| | | 356 | | |
| | | 357 | | /// <summary> |
| | | 358 | | /// Converts a rotation matrix into a quaternion representation. |
| | | 359 | | /// </summary> |
| | | 360 | | /// <param name="matrix">The rotation matrix to convert.</param> |
| | | 361 | | /// <returns>A quaternion representing the same rotation as the matrix.</returns> |
| | | 362 | | public static FixedQuaternion FromMatrix(Fixed3x3 matrix) |
| | 63 | 363 | | { |
| | 63 | 364 | | Fixed64 trace = matrix.m00 + matrix.m11 + matrix.m22; |
| | | 365 | | |
| | | 366 | | Fixed64 w, x, y, z; |
| | | 367 | | |
| | 63 | 368 | | if (trace > Fixed64.Zero) |
| | 58 | 369 | | { |
| | 58 | 370 | | Fixed64 s = FixedMath.Sqrt(trace + Fixed64.One); |
| | 58 | 371 | | w = s * Fixed64.Half; |
| | 58 | 372 | | s = Fixed64.Half / s; |
| | 58 | 373 | | x = (matrix.m21 - matrix.m12) * s; |
| | 58 | 374 | | y = (matrix.m02 - matrix.m20) * s; |
| | 58 | 375 | | z = (matrix.m10 - matrix.m01) * s; |
| | 58 | 376 | | } |
| | 5 | 377 | | else if (matrix.m00 > matrix.m11 && matrix.m00 > matrix.m22) |
| | 1 | 378 | | { |
| | 1 | 379 | | Fixed64 s = FixedMath.Sqrt(Fixed64.One + matrix.m00 - matrix.m11 - matrix.m22); |
| | 1 | 380 | | x = s * Fixed64.Half; |
| | 1 | 381 | | s = Fixed64.Half / s; |
| | 1 | 382 | | y = (matrix.m10 + matrix.m01) * s; |
| | 1 | 383 | | z = (matrix.m02 + matrix.m20) * s; |
| | 1 | 384 | | w = (matrix.m21 - matrix.m12) * s; |
| | 1 | 385 | | } |
| | 4 | 386 | | else if (matrix.m11 > matrix.m22) |
| | 1 | 387 | | { |
| | 1 | 388 | | Fixed64 s = FixedMath.Sqrt(Fixed64.One + matrix.m11 - matrix.m00 - matrix.m22); |
| | 1 | 389 | | y = s * Fixed64.Half; |
| | 1 | 390 | | s = Fixed64.Half / s; |
| | 1 | 391 | | z = (matrix.m21 + matrix.m12) * s; |
| | 1 | 392 | | x = (matrix.m10 + matrix.m01) * s; |
| | 1 | 393 | | w = (matrix.m02 - matrix.m20) * s; |
| | 1 | 394 | | } |
| | | 395 | | else |
| | 3 | 396 | | { |
| | 3 | 397 | | Fixed64 s = FixedMath.Sqrt(Fixed64.One + matrix.m22 - matrix.m00 - matrix.m11); |
| | 3 | 398 | | z = s * Fixed64.Half; |
| | 3 | 399 | | s = Fixed64.Half / s; |
| | 3 | 400 | | x = (matrix.m02 + matrix.m20) * s; |
| | 3 | 401 | | y = (matrix.m21 + matrix.m12) * s; |
| | 3 | 402 | | w = (matrix.m10 - matrix.m01) * s; |
| | 3 | 403 | | } |
| | | 404 | | |
| | 63 | 405 | | return new FixedQuaternion(x, y, z, w); |
| | 63 | 406 | | } |
| | | 407 | | |
| | | 408 | | /// <summary> |
| | | 409 | | /// Converts a rotation matrix (upper-left 3x3 part of a 4x4 matrix) into a quaternion representation. |
| | | 410 | | /// </summary> |
| | | 411 | | /// <param name="matrix">The 4x4 matrix containing the rotation component.</param> |
| | | 412 | | /// <remarks>Extracts the upper-left 3x3 rotation part of the 4x4</remarks> |
| | | 413 | | /// <returns>A quaternion representing the same rotation as the matrix.</returns> |
| | | 414 | | public static FixedQuaternion FromMatrix(Fixed4x4 matrix) |
| | 57 | 415 | | { |
| | | 416 | | |
| | 57 | 417 | | var rotationMatrix = new Fixed3x3( |
| | 57 | 418 | | matrix.m00, matrix.m01, matrix.m02, |
| | 57 | 419 | | matrix.m10, matrix.m11, matrix.m12, |
| | 57 | 420 | | matrix.m20, matrix.m21, matrix.m22 |
| | 57 | 421 | | ); |
| | | 422 | | |
| | 57 | 423 | | return FromMatrix(rotationMatrix); |
| | 57 | 424 | | } |
| | | 425 | | |
| | | 426 | | /// <summary> |
| | | 427 | | /// Creates a quaternion representing the rotation needed to align the forward vector with the given direction. |
| | | 428 | | /// </summary> |
| | | 429 | | /// <param name="direction">The target direction vector.</param> |
| | | 430 | | /// <returns>A quaternion representing the rotation to align with the direction.</returns> |
| | | 431 | | public static FixedQuaternion FromDirection(Vector3d direction) |
| | 2 | 432 | | { |
| | | 433 | | // Compute the rotation axis as the cross product of the standard forward vector and the desired direction |
| | 2 | 434 | | Vector3d axis = Vector3d.Cross(Vector3d.Forward, direction); |
| | 2 | 435 | | Fixed64 axisLength = axis.Magnitude; |
| | | 436 | | |
| | | 437 | | // If the axis length is very close to zero, it means that the desired direction is almost equal to the standard |
| | 2 | 438 | | if (axisLength.Abs() == Fixed64.Zero) |
| | 1 | 439 | | return Identity; // Return the identity quaternion if no rotation is needed |
| | | 440 | | |
| | | 441 | | // Normalize the rotation axis |
| | 1 | 442 | | axis = (axis / axisLength).Normal; |
| | | 443 | | |
| | | 444 | | // Compute the angle between the standard forward vector and the desired direction |
| | 1 | 445 | | Fixed64 angle = FixedMath.Acos(Vector3d.Dot(Vector3d.Forward, direction)); |
| | | 446 | | |
| | | 447 | | // Compute the rotation quaternion from the axis and angle |
| | 1 | 448 | | return FromAxisAngle(axis, angle); |
| | 2 | 449 | | } |
| | | 450 | | |
| | | 451 | | /// <summary> |
| | | 452 | | /// Creates a quaternion representing a rotation around a specified axis by a given angle. |
| | | 453 | | /// </summary> |
| | | 454 | | /// <param name="axis">The axis to rotate around (must be normalized).</param> |
| | | 455 | | /// <param name="angle">The rotation angle in radians.</param> |
| | | 456 | | /// <returns>A quaternion representing the rotation.</returns> |
| | | 457 | | public static FixedQuaternion FromAxisAngle(Vector3d axis, Fixed64 angle) |
| | 45 | 458 | | { |
| | | 459 | | // Check if the axis is a unit vector |
| | 45 | 460 | | if (!axis.IsNormalized()) |
| | 1 | 461 | | axis = axis.Normalize(); |
| | | 462 | | |
| | | 463 | | // Check if the angle is in a valid range (-pi, pi) |
| | 45 | 464 | | if (angle < -FixedMath.PI || angle > FixedMath.PI) |
| | 3 | 465 | | throw new ArgumentOutOfRangeException(nameof(angle), angle, $"Angle must be in the range ({-FixedMath.PI}, { |
| | | 466 | | |
| | 42 | 467 | | Fixed64 halfAngle = angle / Fixed64.Two; // Half-angle formula |
| | 42 | 468 | | Fixed64 sinHalfAngle = FixedMath.Sin(halfAngle); |
| | 42 | 469 | | Fixed64 cosHalfAngle = FixedMath.Cos(halfAngle); |
| | | 470 | | |
| | 42 | 471 | | return GetNormalized(new FixedQuaternion( |
| | 42 | 472 | | axis.x * sinHalfAngle, |
| | 42 | 473 | | axis.y * sinHalfAngle, |
| | 42 | 474 | | axis.z * sinHalfAngle, |
| | 42 | 475 | | cosHalfAngle)); |
| | 42 | 476 | | } |
| | | 477 | | |
| | | 478 | | /// <summary> |
| | | 479 | | /// Assume the input angles are in degrees and converts them to radians before calling <see cref="FromEulerAngles"/> |
| | | 480 | | /// </summary> |
| | | 481 | | /// <param name="pitch"></param> |
| | | 482 | | /// <param name="yaw"></param> |
| | | 483 | | /// <param name="roll"></param> |
| | | 484 | | /// <returns></returns> |
| | | 485 | | public static FixedQuaternion FromEulerAnglesInDegrees(Fixed64 pitch, Fixed64 yaw, Fixed64 roll) |
| | 28 | 486 | | { |
| | | 487 | | // Convert input angles from degrees to radians |
| | 28 | 488 | | pitch = FixedMath.DegToRad(pitch); |
| | 28 | 489 | | yaw = FixedMath.DegToRad(yaw); |
| | 28 | 490 | | roll = FixedMath.DegToRad(roll); |
| | | 491 | | |
| | | 492 | | // Call the original method that expects angles in radians |
| | 28 | 493 | | return FromEulerAngles(pitch, yaw, roll); |
| | 28 | 494 | | } |
| | | 495 | | |
| | | 496 | | /// <summary> |
| | | 497 | | /// Converts Euler angles (pitch, yaw, roll) to a quaternion and normalizes the result afterwards. |
| | | 498 | | /// Assumes the input angles are in radians. |
| | | 499 | | /// </summary> |
| | | 500 | | /// <remarks> |
| | | 501 | | /// The order of operations is YXZ or yaw-pitch-roll |
| | | 502 | | /// </remarks> |
| | | 503 | | public static FixedQuaternion FromEulerAngles(Fixed64 pitch, Fixed64 yaw, Fixed64 roll) |
| | 41 | 504 | | { |
| | | 505 | | // Check if the angles are in a valid range (-pi, pi) |
| | 41 | 506 | | if (pitch < -FixedMath.PI || pitch > FixedMath.PI) |
| | 3 | 507 | | throw new ArgumentOutOfRangeException(nameof(pitch), pitch, $"Pitch must be in the range ({-FixedMath.PI}, { |
| | 38 | 508 | | if (yaw < -FixedMath.PI || yaw > FixedMath.PI) |
| | 3 | 509 | | throw new ArgumentOutOfRangeException(nameof(yaw), yaw, $"Yaw must be in the range ({-FixedMath.PI}, {FixedM |
| | 35 | 510 | | if (roll < -FixedMath.PI || roll > FixedMath.PI) |
| | 3 | 511 | | throw new ArgumentOutOfRangeException(nameof(roll), roll, $"Roll must be in the range ({-FixedMath.PI}, {Fix |
| | | 512 | | |
| | 32 | 513 | | Fixed64 halfPitch = pitch / Fixed64.Two; |
| | 32 | 514 | | Fixed64 halfYaw = yaw / Fixed64.Two; |
| | 32 | 515 | | Fixed64 halfRoll = roll / Fixed64.Two; |
| | | 516 | | |
| | 32 | 517 | | Fixed64 sx = FixedMath.Sin(halfPitch); |
| | 32 | 518 | | Fixed64 cx = FixedMath.Cos(halfPitch); |
| | 32 | 519 | | Fixed64 sy = FixedMath.Sin(halfYaw); |
| | 32 | 520 | | Fixed64 cy = FixedMath.Cos(halfYaw); |
| | 32 | 521 | | Fixed64 sz = FixedMath.Sin(halfRoll); |
| | 32 | 522 | | Fixed64 cz = FixedMath.Cos(halfRoll); |
| | | 523 | | |
| | | 524 | | // q = qy * qx * qz |
| | 32 | 525 | | Fixed64 x = (cx * sy * sz) + (cy * cz * sx); |
| | 32 | 526 | | Fixed64 y = (cx * cz * sy) - (cy * sx * sz); |
| | 32 | 527 | | Fixed64 z = (cx * cy * sz) - (cz * sx * sy); |
| | 32 | 528 | | Fixed64 w = (cx * cy * cz) + (sx * sy * sz); |
| | | 529 | | |
| | 32 | 530 | | return GetNormalized(new FixedQuaternion(x, y, z, w)); |
| | 32 | 531 | | } |
| | | 532 | | |
| | | 533 | | /// <summary> |
| | | 534 | | /// Computes the logarithm of a quaternion, which represents the rotational displacement. |
| | | 535 | | /// This is useful for interpolation and angular velocity calculations. |
| | | 536 | | /// </summary> |
| | | 537 | | /// <param name="q">The quaternion to compute the logarithm of.</param> |
| | | 538 | | /// <returns>A Vector3d representing the logarithm of the quaternion (axis-angle representation).</returns> |
| | | 539 | | /// <remarks> |
| | | 540 | | /// The logarithm of a unit quaternion is given by: |
| | | 541 | | /// log(q) = (θ * v̂), where: |
| | | 542 | | /// - θ = 2 * acos(w) is the rotation angle. |
| | | 543 | | /// - v̂ = (x, y, z) / ||(x, y, z)|| is the unit vector representing the axis of rotation. |
| | | 544 | | /// If the quaternion is close to identity, the function returns a zero vector to avoid numerical instability. |
| | | 545 | | /// </remarks> |
| | | 546 | | public static Vector3d QuaternionLog(FixedQuaternion q) |
| | 7 | 547 | | { |
| | | 548 | | // Ensure the quaternion is normalized |
| | 7 | 549 | | q = q.Normal; |
| | | 550 | | |
| | | 551 | | // Extract vector part |
| | 7 | 552 | | Vector3d v = new(q.x, q.y, q.z); |
| | 7 | 553 | | Fixed64 vLength = v.Magnitude; |
| | | 554 | | |
| | | 555 | | // If rotation is very small, avoid division by zero |
| | 7 | 556 | | if (vLength < Fixed64.FromRaw(0x00001000L)) // Small epsilon |
| | 3 | 557 | | return Vector3d.Zero; |
| | | 558 | | |
| | | 559 | | // Compute angle (theta = 2 * acos(w)) |
| | 4 | 560 | | Fixed64 theta = Fixed64.Two * FixedMath.Acos(q.w); |
| | | 561 | | |
| | | 562 | | // Convert to angular velocity |
| | 4 | 563 | | return (v / vLength) * theta; |
| | 7 | 564 | | } |
| | | 565 | | |
| | | 566 | | /// <summary> |
| | | 567 | | /// Computes the angular velocity required to move from `previousRotation` to `currentRotation` over a given time st |
| | | 568 | | /// </summary> |
| | | 569 | | /// <param name="currentRotation">The current orientation as a quaternion.</param> |
| | | 570 | | /// <param name="previousRotation">The previous orientation as a quaternion.</param> |
| | | 571 | | /// <param name="deltaTime">The time step over which the rotation occurs.</param> |
| | | 572 | | /// <returns>A Vector3d representing the angular velocity (in radians per second).</returns> |
| | | 573 | | /// <remarks> |
| | | 574 | | /// This function calculates the change in rotation over `deltaTime` and converts it into angular velocity. |
| | | 575 | | /// - First, it computes the relative rotation: `rotationDelta = currentRotation * previousRotation.Inverse()`. |
| | | 576 | | /// - Then, it applies `QuaternionLog(rotationDelta)` to extract the axis-angle representation. |
| | | 577 | | /// - Finally, it divides by `deltaTime` to compute the angular velocity. |
| | | 578 | | /// </remarks> |
| | | 579 | | public static Vector3d ToAngularVelocity( |
| | | 580 | | FixedQuaternion currentRotation, |
| | | 581 | | FixedQuaternion previousRotation, |
| | | 582 | | Fixed64 deltaTime) |
| | 4 | 583 | | { |
| | 4 | 584 | | FixedQuaternion rotationDelta = currentRotation * previousRotation.Inverse(); |
| | 4 | 585 | | Vector3d angularDisplacement = QuaternionLog(rotationDelta); |
| | | 586 | | |
| | 4 | 587 | | return angularDisplacement / deltaTime; // Convert to angular velocity |
| | 4 | 588 | | } |
| | | 589 | | |
| | | 590 | | /// <summary> |
| | | 591 | | /// Performs a simple linear interpolation between the components of the input quaternions |
| | | 592 | | /// </summary> |
| | | 593 | | public static FixedQuaternion Lerp(FixedQuaternion a, FixedQuaternion b, Fixed64 t) |
| | 3 | 594 | | { |
| | 3 | 595 | | t = FixedMath.Clamp01(t); |
| | | 596 | | |
| | 3 | 597 | | if (Dot(a, b) < Fixed64.Zero) |
| | 1 | 598 | | b = -b; |
| | | 599 | | |
| | | 600 | | FixedQuaternion result; |
| | 3 | 601 | | Fixed64 oneMinusT = Fixed64.One - t; |
| | 3 | 602 | | result.x = a.x * oneMinusT + b.x * t; |
| | 3 | 603 | | result.y = a.y * oneMinusT + b.y * t; |
| | 3 | 604 | | result.z = a.z * oneMinusT + b.z * t; |
| | 3 | 605 | | result.w = a.w * oneMinusT + b.w * t; |
| | | 606 | | |
| | 3 | 607 | | result.Normalize(); |
| | | 608 | | |
| | 3 | 609 | | return result; |
| | 3 | 610 | | } |
| | | 611 | | |
| | | 612 | | /// <summary> |
| | | 613 | | /// Calculates the spherical linear interpolation, which results in a smoother and more accurate rotation interpola |
| | | 614 | | /// </summary> |
| | | 615 | | public static FixedQuaternion Slerp(FixedQuaternion a, FixedQuaternion b, Fixed64 t) |
| | 2 | 616 | | { |
| | 2 | 617 | | t = FixedMath.Clamp01(t); |
| | | 618 | | |
| | 2 | 619 | | Fixed64 cosOmega = a.x * b.x + a.y * b.y + a.z * b.z + a.w * b.w; |
| | | 620 | | |
| | | 621 | | // If the dot product is negative, negate one of the input quaternions. |
| | | 622 | | // This ensures that the interpolation takes the shortest path around the sphere. |
| | 2 | 623 | | if (cosOmega < Fixed64.Zero) |
| | 1 | 624 | | { |
| | 1 | 625 | | b.x = -b.x; |
| | 1 | 626 | | b.y = -b.y; |
| | 1 | 627 | | b.z = -b.z; |
| | 1 | 628 | | b.w = -b.w; |
| | 1 | 629 | | cosOmega = -cosOmega; |
| | 1 | 630 | | } |
| | | 631 | | |
| | | 632 | | Fixed64 k0, k1; |
| | | 633 | | |
| | | 634 | | // If the quaternions are close, use linear interpolation |
| | 2 | 635 | | if (cosOmega > Fixed64.One - Fixed64.Epsilon) |
| | 1 | 636 | | { |
| | 1 | 637 | | k0 = Fixed64.One - t; |
| | 1 | 638 | | k1 = t; |
| | 1 | 639 | | } |
| | | 640 | | else |
| | 1 | 641 | | { |
| | | 642 | | // Otherwise, use spherical linear interpolation |
| | 1 | 643 | | Fixed64 sinOmega = FixedMath.Sqrt(Fixed64.One - cosOmega * cosOmega); |
| | 1 | 644 | | Fixed64 omega = FixedMath.Atan2(sinOmega, cosOmega); |
| | | 645 | | |
| | 1 | 646 | | k0 = FixedMath.Sin((Fixed64.One - t) * omega) / sinOmega; |
| | 1 | 647 | | k1 = FixedMath.Sin(t * omega) / sinOmega; |
| | 1 | 648 | | } |
| | | 649 | | |
| | | 650 | | FixedQuaternion result; |
| | 2 | 651 | | result.x = a.x * k0 + b.x * k1; |
| | 2 | 652 | | result.y = a.y * k0 + b.y * k1; |
| | 2 | 653 | | result.z = a.z * k0 + b.z * k1; |
| | 2 | 654 | | result.w = a.w * k0 + b.w * k1; |
| | | 655 | | |
| | 2 | 656 | | return result; |
| | 2 | 657 | | } |
| | | 658 | | |
| | | 659 | | /// <summary> |
| | | 660 | | /// Returns the angle in degrees between two rotations a and b. |
| | | 661 | | /// </summary> |
| | | 662 | | /// <param name="a">The first rotation.</param> |
| | | 663 | | /// <param name="b">The second rotation.</param> |
| | | 664 | | /// <returns>The angle in degrees between the two rotations.</returns> |
| | | 665 | | public static Fixed64 Angle(FixedQuaternion a, FixedQuaternion b) |
| | 1 | 666 | | { |
| | | 667 | | // Calculate the dot product of the two quaternions |
| | 1 | 668 | | Fixed64 dot = Dot(a, b); |
| | | 669 | | |
| | | 670 | | // Ensure the dot product is in the range of [-1, 1] to avoid floating-point inaccuracies |
| | 1 | 671 | | dot = FixedMath.Clamp(dot, -Fixed64.One, Fixed64.One); |
| | | 672 | | |
| | | 673 | | // Calculate the angle between the two quaternions using the inverse cosine (arccos) |
| | | 674 | | // arccos(dot(a, b)) gives us the angle in radians, so we convert it to degrees |
| | 1 | 675 | | Fixed64 angleInRadians = FixedMath.Acos(dot); |
| | | 676 | | |
| | | 677 | | // Convert the angle from radians to degrees |
| | 1 | 678 | | Fixed64 angleInDegrees = FixedMath.RadToDeg(angleInRadians); |
| | | 679 | | |
| | 1 | 680 | | return angleInDegrees; |
| | 1 | 681 | | } |
| | | 682 | | |
| | | 683 | | /// <summary> |
| | | 684 | | /// Creates a quaternion from an angle and axis. |
| | | 685 | | /// </summary> |
| | | 686 | | /// <param name="angle">The angle in degrees.</param> |
| | | 687 | | /// <param name="axis">The axis to rotate around (must be normalized).</param> |
| | | 688 | | /// <returns>A quaternion representing the rotation.</returns> |
| | | 689 | | public static FixedQuaternion AngleAxis(Fixed64 angle, Vector3d axis) |
| | 1 | 690 | | { |
| | | 691 | | // Convert the angle to radians |
| | 1 | 692 | | angle = angle.ToRadians(); |
| | | 693 | | |
| | | 694 | | // Normalize the axis |
| | 1 | 695 | | axis = axis.Normal; |
| | | 696 | | |
| | | 697 | | // Use the half-angle formula (sin(theta / 2), cos(theta / 2)) |
| | 1 | 698 | | Fixed64 halfAngle = angle / Fixed64.Two; |
| | 1 | 699 | | Fixed64 sinHalfAngle = FixedMath.Sin(halfAngle); |
| | 1 | 700 | | Fixed64 cosHalfAngle = FixedMath.Cos(halfAngle); |
| | | 701 | | |
| | 1 | 702 | | return new FixedQuaternion( |
| | 1 | 703 | | axis.x * sinHalfAngle, |
| | 1 | 704 | | axis.y * sinHalfAngle, |
| | 1 | 705 | | axis.z * sinHalfAngle, |
| | 1 | 706 | | cosHalfAngle |
| | 1 | 707 | | ); |
| | 1 | 708 | | } |
| | | 709 | | |
| | | 710 | | /// <summary> |
| | | 711 | | /// Calculates the dot product of two quaternions. |
| | | 712 | | /// </summary> |
| | | 713 | | /// <param name="a">The first quaternion.</param> |
| | | 714 | | /// <param name="b">The second quaternion.</param> |
| | | 715 | | /// <returns>The dot product of the two quaternions.</returns> |
| | | 716 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 717 | | public static Fixed64 Dot(FixedQuaternion a, FixedQuaternion b) |
| | 9 | 718 | | { |
| | 9 | 719 | | return a.w * b.w + a.x * b.x + a.y * b.y + a.z * b.z; |
| | 9 | 720 | | } |
| | | 721 | | |
| | | 722 | | #endregion |
| | | 723 | | |
| | | 724 | | #region Operators |
| | | 725 | | |
| | | 726 | | /// <summary> |
| | | 727 | | /// Multiplies two quaternions, combining their rotations into a single quaternion. |
| | | 728 | | /// </summary> |
| | | 729 | | /// <remarks> |
| | | 730 | | /// Quaternion multiplication is not commutative; the order of operands affects the result. |
| | | 731 | | /// This operation is commonly used to concatenate rotations. |
| | | 732 | | /// </remarks> |
| | | 733 | | /// <param name="a">The first quaternion to multiply.</param> |
| | | 734 | | /// <param name="b">The second quaternion to multiply.</param> |
| | | 735 | | /// <returns>A new FixedQuaternion representing the combined rotation of the two input quaternions.</returns> |
| | | 736 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 737 | | public static FixedQuaternion operator *(FixedQuaternion a, FixedQuaternion b) |
| | 13 | 738 | | { |
| | 13 | 739 | | return new FixedQuaternion( |
| | 13 | 740 | | (a.w * b.x) + (a.x * b.w) + (a.y * b.z) - (a.z * b.y), |
| | 13 | 741 | | (a.w * b.y) - (a.x * b.z) + (a.y * b.w) + (a.z * b.x), |
| | 13 | 742 | | (a.w * b.z) + (a.x * b.y) - (a.y * b.x) + (a.z * b.w), |
| | 13 | 743 | | (a.w * b.w) - (a.x * b.x) - (a.y * b.y) - (a.z * b.z) |
| | 13 | 744 | | ); |
| | 13 | 745 | | } |
| | | 746 | | |
| | | 747 | | /// <summary> |
| | | 748 | | /// Multiplies each component of the specified quaternion by the given scalar value. |
| | | 749 | | /// </summary> |
| | | 750 | | /// <param name="q">The quaternion whose components are to be multiplied.</param> |
| | | 751 | | /// <param name="scalar">The scalar value by which to multiply each component of the quaternion.</param> |
| | | 752 | | /// <returns>A new FixedQuaternion whose components are the result of multiplying the corresponding components of th |
| | | 753 | | /// quaternion by the scalar value.</returns> |
| | | 754 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 755 | | public static FixedQuaternion operator *(FixedQuaternion q, Fixed64 scalar) |
| | 5 | 756 | | { |
| | 5 | 757 | | return new FixedQuaternion(q.x * scalar, q.y * scalar, q.z * scalar, q.w * scalar); |
| | 5 | 758 | | } |
| | | 759 | | |
| | | 760 | | /// <inheritdoc cref="operator *(FixedQuaternion, Fixed64)"/> |
| | | 761 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 762 | | public static FixedQuaternion operator *(Fixed64 scalar, FixedQuaternion q) |
| | 1 | 763 | | { |
| | 1 | 764 | | return new FixedQuaternion(q.x * scalar, q.y * scalar, q.z * scalar, q.w * scalar); |
| | 1 | 765 | | } |
| | | 766 | | |
| | | 767 | | /// <summary> |
| | | 768 | | /// Divides each component of the specified quaternion by the given scalar value. |
| | | 769 | | /// </summary> |
| | | 770 | | /// <remarks>Division by zero will result in an exception or undefined behavior.</remarks> |
| | | 771 | | /// <param name="q">The quaternion whose components are to be divided.</param> |
| | | 772 | | /// <param name="scalar">The scalar value by which to divide each component of the quaternion.</param> |
| | | 773 | | /// <returns>A new FixedQuaternion whose components are the result of dividing the corresponding components of the i |
| | | 774 | | /// quaternion by the scalar value.</returns> |
| | | 775 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 776 | | public static FixedQuaternion operator /(FixedQuaternion q, Fixed64 scalar) |
| | 1 | 777 | | { |
| | 1 | 778 | | return new FixedQuaternion(q.x / scalar, q.y / scalar, q.z / scalar, q.w / scalar); |
| | 1 | 779 | | } |
| | | 780 | | |
| | | 781 | | /// <summary> |
| | | 782 | | /// Adds two quaternions component-wise and returns the resulting quaternion. |
| | | 783 | | /// </summary> |
| | | 784 | | /// <remarks> |
| | | 785 | | /// This operation performs a simple component-wise addition. |
| | | 786 | | /// </remarks> |
| | | 787 | | /// <param name="q1">The first quaternion to add.</param> |
| | | 788 | | /// <param name="q2">The second quaternion to add.</param> |
| | | 789 | | /// <returns>A new FixedQuaternion whose components are the sums of the corresponding components of q1 and q2.</retu |
| | | 790 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 791 | | public static FixedQuaternion operator +(FixedQuaternion q1, FixedQuaternion q2) |
| | 1 | 792 | | { |
| | 1 | 793 | | return new FixedQuaternion(q1.x + q2.x, q1.y + q2.y, q1.z + q2.z, q1.w + q2.w); |
| | 1 | 794 | | } |
| | | 795 | | |
| | | 796 | | /// <summary> |
| | | 797 | | /// Subtracts two quaternions component-wise and returns the resulting quaternion. |
| | | 798 | | /// </summary> |
| | | 799 | | /// <remarks> |
| | | 800 | | /// This operation performs component-wise subtraction. |
| | | 801 | | /// </remarks> |
| | | 802 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 803 | | public static FixedQuaternion operator -(FixedQuaternion q1, FixedQuaternion q2) |
| | 1 | 804 | | { |
| | 1 | 805 | | return new FixedQuaternion(q1.x - q2.x, q1.y - q2.y, q1.z - q2.z, q1.w - q2.w); |
| | 1 | 806 | | } |
| | | 807 | | |
| | | 808 | | /// <summary> |
| | | 809 | | /// Negates each component of the specified quaternion. |
| | | 810 | | /// </summary> |
| | | 811 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 812 | | public static FixedQuaternion operator -(FixedQuaternion q) |
| | 2 | 813 | | { |
| | 2 | 814 | | return new FixedQuaternion(-q.x, -q.y, -q.z, -q.w); |
| | 2 | 815 | | } |
| | | 816 | | |
| | | 817 | | /// <summary> |
| | | 818 | | /// Determines whether two FixedQuaternion instances are equal. |
| | | 819 | | /// </summary> |
| | | 820 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 821 | | public static bool operator ==(FixedQuaternion left, FixedQuaternion right) |
| | 9 | 822 | | { |
| | 9 | 823 | | return left.Equals(right); |
| | 9 | 824 | | } |
| | | 825 | | |
| | | 826 | | /// <summary> |
| | | 827 | | /// Determines whether two FixedQuaternion instances are not equal. |
| | | 828 | | /// </summary> |
| | | 829 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 830 | | public static bool operator !=(FixedQuaternion left, FixedQuaternion right) |
| | 1 | 831 | | { |
| | 1 | 832 | | return !left.Equals(right); |
| | 1 | 833 | | } |
| | | 834 | | |
| | | 835 | | #endregion |
| | | 836 | | |
| | | 837 | | #region Conversion |
| | | 838 | | |
| | | 839 | | /// <summary> |
| | | 840 | | /// Converts this quaternion to Euler angles in degrees. |
| | | 841 | | /// Returns angles as (pitch, yaw, roll), where: |
| | | 842 | | /// pitch = rotation around X |
| | | 843 | | /// yaw = rotation around Y |
| | | 844 | | /// roll = rotation around Z |
| | | 845 | | /// |
| | | 846 | | /// The extraction matches FromEulerAngles(), which composes rotations in YXZ order: |
| | | 847 | | /// q = qy * qx * qz |
| | | 848 | | /// </summary> |
| | | 849 | | public Vector3d ToEulerAngles() |
| | 3 | 850 | | { |
| | 3 | 851 | | Fixed3x3 m = ToMatrix3x3(); |
| | | 852 | | |
| | | 853 | | Fixed64 pitch; |
| | | 854 | | Fixed64 yaw; |
| | | 855 | | Fixed64 roll; |
| | | 856 | | |
| | | 857 | | // For YXZ: |
| | | 858 | | // m12 = -sin(pitch) |
| | | 859 | | // m02 = sin(yaw) * cos(pitch) |
| | | 860 | | // m22 = cos(yaw) * cos(pitch) |
| | | 861 | | // m10 = sin(roll) * cos(pitch) |
| | | 862 | | // m11 = cos(roll) * cos(pitch) |
| | | 863 | | |
| | 3 | 864 | | Fixed64 sinPitch = -m.m12; |
| | | 865 | | |
| | 3 | 866 | | if (sinPitch.Abs() >= Fixed64.One) |
| | 2 | 867 | | { |
| | | 868 | | // Gimbal lock: pitch is ±90°, yaw/roll are coupled. |
| | 2 | 869 | | pitch = FixedMath.CopySign(FixedMath.PiOver2, sinPitch); |
| | | 870 | | |
| | | 871 | | // Choose roll = 0 and solve remaining yaw from matrix. |
| | 2 | 872 | | roll = Fixed64.Zero; |
| | 2 | 873 | | yaw = FixedMath.Atan2(-m.m20, m.m00); |
| | 2 | 874 | | } |
| | | 875 | | else |
| | 1 | 876 | | { |
| | 1 | 877 | | pitch = FixedMath.Asin(sinPitch); |
| | 1 | 878 | | yaw = FixedMath.Atan2(m.m02, m.m22); |
| | 1 | 879 | | roll = FixedMath.Atan2(m.m10, m.m11); |
| | 1 | 880 | | } |
| | | 881 | | |
| | 3 | 882 | | return new Vector3d( |
| | 3 | 883 | | FixedMath.RadToDeg(pitch), |
| | 3 | 884 | | FixedMath.RadToDeg(yaw), |
| | 3 | 885 | | FixedMath.RadToDeg(roll)); |
| | 3 | 886 | | } |
| | | 887 | | |
| | | 888 | | /// <summary> |
| | | 889 | | /// Converts this FixedQuaternion to a direction vector. |
| | | 890 | | /// </summary> |
| | | 891 | | /// <returns>A Vector3d representing the direction equivalent to this FixedQuaternion.</returns> |
| | | 892 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 893 | | public Vector3d ToDirection() |
| | 1 | 894 | | { |
| | 1 | 895 | | return new Vector3d( |
| | 1 | 896 | | 2 * (x * z - w * y), |
| | 1 | 897 | | 2 * (y * z + w * x), |
| | 1 | 898 | | Fixed64.One - 2 * (x * x + y * y) |
| | 1 | 899 | | ); |
| | 1 | 900 | | } |
| | | 901 | | |
| | | 902 | | /// <summary> |
| | | 903 | | /// Converts the quaternion into a 3x3 rotation matrix. |
| | | 904 | | /// </summary> |
| | | 905 | | /// <returns>A FixedMatrix3x3 representing the same rotation as the quaternion.</returns> |
| | | 906 | | public Fixed3x3 ToMatrix3x3() |
| | 35 | 907 | | { |
| | 35 | 908 | | Fixed64 x2 = x * x; |
| | 35 | 909 | | Fixed64 y2 = y * y; |
| | 35 | 910 | | Fixed64 z2 = z * z; |
| | 35 | 911 | | Fixed64 xy = x * y; |
| | 35 | 912 | | Fixed64 xz = x * z; |
| | 35 | 913 | | Fixed64 yz = y * z; |
| | 35 | 914 | | Fixed64 xw = x * w; |
| | 35 | 915 | | Fixed64 yw = y * w; |
| | 35 | 916 | | Fixed64 zw = z * w; |
| | | 917 | | |
| | 35 | 918 | | Fixed3x3 result = new(); |
| | 35 | 919 | | Fixed64 scale = Fixed64.One * 2; |
| | | 920 | | |
| | 35 | 921 | | result.m00 = Fixed64.One - scale * (y2 + z2); |
| | 35 | 922 | | result.m01 = scale * (xy - zw); |
| | 35 | 923 | | result.m02 = scale * (xz + yw); |
| | | 924 | | |
| | 35 | 925 | | result.m10 = scale * (xy + zw); |
| | 35 | 926 | | result.m11 = Fixed64.One - scale * (x2 + z2); |
| | 35 | 927 | | result.m12 = scale * (yz - xw); |
| | | 928 | | |
| | 35 | 929 | | result.m20 = scale * (xz - yw); |
| | 35 | 930 | | result.m21 = scale * (yz + xw); |
| | 35 | 931 | | result.m22 = Fixed64.One - scale * (x2 + y2); |
| | | 932 | | |
| | 35 | 933 | | return result; |
| | 35 | 934 | | } |
| | | 935 | | |
| | | 936 | | /// <summary> |
| | | 937 | | /// Deconstructs the quaternion into its four components. |
| | | 938 | | /// </summary> |
| | | 939 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 940 | | public void Deconstruct(out Fixed64 x, out Fixed64 y, out Fixed64 z, out Fixed64 w) |
| | 1 | 941 | | { |
| | 1 | 942 | | x = this.x; |
| | 1 | 943 | | y = this.y; |
| | 1 | 944 | | z = this.z; |
| | 1 | 945 | | w = this.w; |
| | 1 | 946 | | } |
| | | 947 | | |
| | | 948 | | #endregion |
| | | 949 | | |
| | | 950 | | #region Equality and HashCode Overrides |
| | | 951 | | |
| | | 952 | | /// <inheritdoc/> |
| | | 953 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 954 | | public override bool Equals(object? obj) |
| | 7 | 955 | | { |
| | 7 | 956 | | return obj is FixedQuaternion other && Equals(other); |
| | 7 | 957 | | } |
| | | 958 | | |
| | | 959 | | /// <inheritdoc/> |
| | | 960 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 961 | | public bool Equals(FixedQuaternion other) |
| | 32 | 962 | | { |
| | 32 | 963 | | return x == other.x && y == other.y && z == other.z && w == other.w; |
| | 32 | 964 | | } |
| | | 965 | | |
| | | 966 | | /// <inheritdoc/> |
| | | 967 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 968 | | public override int GetHashCode() |
| | 2 | 969 | | { |
| | 2 | 970 | | return x.GetHashCode() ^ y.GetHashCode() << 2 ^ z.GetHashCode() >> 2 ^ w.GetHashCode(); |
| | 2 | 971 | | } |
| | | 972 | | |
| | | 973 | | /// <summary> |
| | | 974 | | /// Returns a string that represents the current object in the format "(x, y, z, w)". |
| | | 975 | | /// </summary> |
| | | 976 | | /// <returns>A string containing the values of the object formatted as a tuple.</returns> |
| | | 977 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 978 | | public override string ToString() |
| | 166 | 979 | | { |
| | 166 | 980 | | return $"({x}, {y}, {z}, {w})"; |
| | 166 | 981 | | } |
| | | 982 | | |
| | | 983 | | #endregion |
| | | 984 | | } |