| | | 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 FixedQuaternion(Fixed64.Zero, Fixed64.Zero, Fixed64.Zero, Fixe |
| | | 22 | | |
| | | 23 | | /// <summary> |
| | | 24 | | /// Empty quaternion (0, 0, 0, 0). |
| | | 25 | | /// </summary> |
| | | 26 | | public static readonly FixedQuaternion Zero = new FixedQuaternion(Fixed64.Zero, Fixed64.Zero, Fixed64.Zero, Fixed64. |
| | | 27 | | |
| | | 28 | | #endregion |
| | | 29 | | |
| | | 30 | | #region Fields and Constants |
| | | 31 | | |
| | | 32 | | [JsonInclude] |
| | | 33 | | [MemoryPackOrder(0)] |
| | | 34 | | public Fixed64 x; |
| | | 35 | | |
| | | 36 | | [JsonInclude] |
| | | 37 | | [MemoryPackOrder(1)] |
| | | 38 | | public Fixed64 y; |
| | | 39 | | |
| | | 40 | | [JsonInclude] |
| | | 41 | | [MemoryPackOrder(2)] |
| | | 42 | | public Fixed64 z; |
| | | 43 | | |
| | | 44 | | [JsonInclude] |
| | | 45 | | [MemoryPackOrder(3)] |
| | | 46 | | public Fixed64 w; |
| | | 47 | | |
| | | 48 | | #endregion |
| | | 49 | | |
| | | 50 | | #region Constructors |
| | | 51 | | |
| | | 52 | | /// <summary> |
| | | 53 | | /// Creates a new FixedQuaternion with the specified components. |
| | | 54 | | /// </summary> |
| | | 55 | | public FixedQuaternion(Fixed64 x, Fixed64 y, Fixed64 z, Fixed64 w) |
| | 164 | 56 | | { |
| | 164 | 57 | | this.x = x; |
| | 164 | 58 | | this.y = y; |
| | 164 | 59 | | this.z = z; |
| | 164 | 60 | | this.w = w; |
| | 164 | 61 | | } |
| | | 62 | | |
| | | 63 | | #endregion |
| | | 64 | | |
| | | 65 | | #region Properties |
| | | 66 | | |
| | | 67 | | /// <summary> |
| | | 68 | | /// Normalized version of this quaternion. |
| | | 69 | | /// </summary> |
| | | 70 | | [JsonIgnore] |
| | | 71 | | [MemoryPackIgnore] |
| | | 72 | | public FixedQuaternion Normal |
| | | 73 | | { |
| | | 74 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | 11 | 75 | | get => GetNormalized(this); |
| | | 76 | | } |
| | | 77 | | |
| | | 78 | | /// <summary> |
| | | 79 | | /// Returns the Euler angles (in degrees) of this quaternion. |
| | | 80 | | /// </summary> |
| | | 81 | | [JsonIgnore] |
| | | 82 | | [MemoryPackIgnore] |
| | | 83 | | public Vector3d EulerAngles |
| | | 84 | | { |
| | | 85 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | 1 | 86 | | get => ToEulerAngles(); |
| | | 87 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | 1 | 88 | | set => this = FromEulerAnglesInDegrees(value.x, value.y, value.z); |
| | | 89 | | } |
| | | 90 | | |
| | | 91 | | [JsonIgnore] |
| | | 92 | | [MemoryPackIgnore] |
| | | 93 | | public Fixed64 this[int index] |
| | | 94 | | { |
| | | 95 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 96 | | get |
| | 5 | 97 | | { |
| | 5 | 98 | | return index switch |
| | 5 | 99 | | { |
| | 1 | 100 | | 0 => x, |
| | 1 | 101 | | 1 => y, |
| | 1 | 102 | | 2 => z, |
| | 1 | 103 | | 3 => w, |
| | 1 | 104 | | _ => throw new IndexOutOfRangeException("Invalid FixedQuaternion index!"), |
| | 5 | 105 | | }; |
| | 4 | 106 | | } |
| | | 107 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 108 | | set |
| | 5 | 109 | | { |
| | 5 | 110 | | switch (index) |
| | | 111 | | { |
| | | 112 | | case 0: |
| | 1 | 113 | | x = value; |
| | 1 | 114 | | break; |
| | | 115 | | case 1: |
| | 1 | 116 | | y = value; |
| | 1 | 117 | | break; |
| | | 118 | | case 2: |
| | 1 | 119 | | z = value; |
| | 1 | 120 | | break; |
| | | 121 | | case 3: |
| | 1 | 122 | | w = value; |
| | 1 | 123 | | break; |
| | | 124 | | default: |
| | 1 | 125 | | throw new IndexOutOfRangeException("Invalid FixedQuaternion index!"); |
| | | 126 | | } |
| | 4 | 127 | | } |
| | | 128 | | } |
| | | 129 | | |
| | | 130 | | #endregion |
| | | 131 | | |
| | | 132 | | #region Methods (Instance) |
| | | 133 | | |
| | | 134 | | /// <summary> |
| | | 135 | | /// Set x, y, z and w components of an existing Quaternion. |
| | | 136 | | /// </summary> |
| | | 137 | | /// <param name="newX"></param> |
| | | 138 | | /// <param name="newY"></param> |
| | | 139 | | /// <param name="newZ"></param> |
| | | 140 | | /// <param name="newW"></param> |
| | | 141 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 142 | | public void Set(Fixed64 newX, Fixed64 newY, Fixed64 newZ, Fixed64 newW) |
| | 1 | 143 | | { |
| | 1 | 144 | | x = newX; |
| | 1 | 145 | | y = newY; |
| | 1 | 146 | | z = newZ; |
| | 1 | 147 | | w = newW; |
| | 1 | 148 | | } |
| | | 149 | | |
| | | 150 | | /// <summary> |
| | | 151 | | /// Normalizes this quaternion in place. |
| | | 152 | | /// </summary> |
| | | 153 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 154 | | public FixedQuaternion Normalize() |
| | 31 | 155 | | { |
| | 31 | 156 | | return this = GetNormalized(this); |
| | 31 | 157 | | } |
| | | 158 | | |
| | | 159 | | /// <summary> |
| | | 160 | | /// Returns the conjugate of this quaternion (inverses the rotational effect). |
| | | 161 | | /// </summary> |
| | | 162 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 163 | | public FixedQuaternion Conjugate() |
| | 1 | 164 | | { |
| | 1 | 165 | | return new FixedQuaternion(-x, -y, -z, w); |
| | 1 | 166 | | } |
| | | 167 | | |
| | | 168 | | /// <summary> |
| | | 169 | | /// Returns the inverse of this quaternion. |
| | | 170 | | /// </summary> |
| | | 171 | | public FixedQuaternion Inverse() |
| | 9 | 172 | | { |
| | 14 | 173 | | if (this == Identity) return Identity; |
| | 4 | 174 | | Fixed64 norm = x * x + y * y + z * z + w * w; |
| | 5 | 175 | | if (norm == Fixed64.Zero) return this; // Handle division by zero by returning the same quaternion |
| | | 176 | | |
| | 3 | 177 | | Fixed64 invNorm = Fixed64.One / norm; |
| | 3 | 178 | | return new FixedQuaternion(x * -invNorm, y * -invNorm, z * -invNorm, w * invNorm); |
| | 9 | 179 | | } |
| | | 180 | | |
| | | 181 | | /// <summary> |
| | | 182 | | /// Rotates a vector by this quaternion. |
| | | 183 | | /// </summary> |
| | | 184 | | public Vector3d Rotate(Vector3d v) |
| | 1 | 185 | | { |
| | 1 | 186 | | FixedQuaternion normalizedQuat = Normal; |
| | 1 | 187 | | FixedQuaternion vQuat = new FixedQuaternion(v.x, v.y, v.z, Fixed64.Zero); |
| | 1 | 188 | | FixedQuaternion invQuat = normalizedQuat.Inverse(); |
| | 1 | 189 | | FixedQuaternion rotatedVQuat = normalizedQuat * vQuat * invQuat; |
| | 1 | 190 | | return new Vector3d(rotatedVQuat.x, rotatedVQuat.y, rotatedVQuat.z).Normalize(); |
| | 1 | 191 | | } |
| | | 192 | | |
| | | 193 | | /// <summary> |
| | | 194 | | /// Rotates this quaternion by a given angle around a specified axis (default: Y-axis). |
| | | 195 | | /// </summary> |
| | | 196 | | /// <param name="sin">Sine of the rotation angle.</param> |
| | | 197 | | /// <param name="cos">Cosine of the rotation angle.</param> |
| | | 198 | | /// <param name="axis">The axis to rotate around (default: Vector3d.Up).</param> |
| | | 199 | | /// <returns>A new quaternion representing the rotated result.</returns> |
| | | 200 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 201 | | public FixedQuaternion Rotated(Fixed64 sin, Fixed64 cos, Vector3d? axis = null) |
| | 4 | 202 | | { |
| | 4 | 203 | | Vector3d rotateAxis = axis ?? Vector3d.Up; |
| | | 204 | | |
| | | 205 | | // The rotation angle is the arc tangent of sin and cos |
| | 4 | 206 | | Fixed64 angle = FixedMath.Atan2(sin, cos); |
| | | 207 | | |
| | | 208 | | // Construct a quaternion representing a rotation around the axis (default is y aka Vector3d.up) |
| | 4 | 209 | | FixedQuaternion rotationQuat = FromAxisAngle(rotateAxis, angle); |
| | | 210 | | |
| | | 211 | | // Apply the rotation and return the result |
| | 4 | 212 | | return rotationQuat * this; |
| | 4 | 213 | | } |
| | | 214 | | |
| | | 215 | | #endregion |
| | | 216 | | |
| | | 217 | | #region Quaternion Operations |
| | | 218 | | |
| | | 219 | | /// <summary> |
| | | 220 | | /// Checks if this vector has been normalized by checking if the magnitude is close to 1. |
| | | 221 | | /// </summary> |
| | | 222 | | public bool IsNormalized() |
| | 3 | 223 | | { |
| | 3 | 224 | | Fixed64 mag = GetMagnitude(this); |
| | 3 | 225 | | return FixedMath.Abs(mag - Fixed64.One) <= Fixed64.Epsilon; |
| | 3 | 226 | | } |
| | | 227 | | |
| | | 228 | | public static Fixed64 GetMagnitude(FixedQuaternion q) |
| | 87 | 229 | | { |
| | 87 | 230 | | Fixed64 mag = (q.x * q.x) + (q.y * q.y) + (q.z * q.z) + (q.w * q.w); |
| | | 231 | | // If rounding error caused the final magnitude to be slightly above 1, clamp it |
| | 87 | 232 | | if (mag > Fixed64.One && mag <= Fixed64.One + Fixed64.Epsilon) |
| | 4 | 233 | | return Fixed64.One; |
| | | 234 | | |
| | 83 | 235 | | return mag != Fixed64.Zero ? FixedMath.Sqrt(mag) : Fixed64.Zero; |
| | 87 | 236 | | } |
| | | 237 | | |
| | | 238 | | /// <summary> |
| | | 239 | | /// Normalizes the quaternion to a unit quaternion. |
| | | 240 | | /// </summary> |
| | | 241 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 242 | | public static FixedQuaternion GetNormalized(FixedQuaternion q) |
| | 81 | 243 | | { |
| | 81 | 244 | | Fixed64 mag = GetMagnitude(q); |
| | | 245 | | |
| | | 246 | | // If magnitude is zero, return identity quaternion (to avoid divide by zero) |
| | 81 | 247 | | if (mag == Fixed64.Zero) |
| | 1 | 248 | | return new FixedQuaternion(Fixed64.Zero, Fixed64.Zero, Fixed64.Zero, Fixed64.One); |
| | | 249 | | |
| | | 250 | | // If already normalized, return as-is |
| | 80 | 251 | | if (mag == Fixed64.One) |
| | 62 | 252 | | return q; |
| | | 253 | | |
| | | 254 | | // Normalize it exactly |
| | 18 | 255 | | return new FixedQuaternion( |
| | 18 | 256 | | q.x / mag, |
| | 18 | 257 | | q.y / mag, |
| | 18 | 258 | | q.z / mag, |
| | 18 | 259 | | q.w / mag |
| | 18 | 260 | | ); |
| | 81 | 261 | | } |
| | | 262 | | |
| | | 263 | | /// <summary> |
| | | 264 | | /// Creates a quaternion that rotates one vector to align with another. |
| | | 265 | | /// </summary> |
| | | 266 | | /// <param name="forward">The forward direction vector.</param> |
| | | 267 | | /// <param name="upwards">The upwards direction vector (optional, default: Vector3d.Up).</param> |
| | | 268 | | /// <returns>A quaternion representing the rotation from one direction to another.</returns> |
| | | 269 | | public static FixedQuaternion LookRotation(Vector3d forward, Vector3d? upwards = null) |
| | 2 | 270 | | { |
| | 2 | 271 | | Vector3d up = upwards ?? Vector3d.Up; |
| | | 272 | | |
| | 2 | 273 | | Vector3d forwardNormalized = forward.Normal; |
| | 2 | 274 | | Vector3d right = Vector3d.Cross(up.Normal, forwardNormalized); |
| | 2 | 275 | | up = Vector3d.Cross(forwardNormalized, right); |
| | | 276 | | |
| | 2 | 277 | | return FromMatrix(new Fixed3x3(right.x, up.x, forwardNormalized.x, |
| | 2 | 278 | | right.y, up.y, forwardNormalized.y, |
| | 2 | 279 | | right.z, up.z, forwardNormalized.z)); |
| | 2 | 280 | | } |
| | | 281 | | |
| | | 282 | | /// <summary> |
| | | 283 | | /// Converts a rotation matrix into a quaternion representation. |
| | | 284 | | /// </summary> |
| | | 285 | | /// <param name="matrix">The rotation matrix to convert.</param> |
| | | 286 | | /// <returns>A quaternion representing the same rotation as the matrix.</returns> |
| | | 287 | | public static FixedQuaternion FromMatrix(Fixed3x3 matrix) |
| | 29 | 288 | | { |
| | 29 | 289 | | Fixed64 trace = matrix.m00 + matrix.m11 + matrix.m22; |
| | | 290 | | |
| | | 291 | | Fixed64 w, x, y, z; |
| | | 292 | | |
| | 29 | 293 | | if (trace > Fixed64.Zero) |
| | 24 | 294 | | { |
| | 24 | 295 | | Fixed64 s = FixedMath.Sqrt(trace + Fixed64.One); |
| | 24 | 296 | | w = s * Fixed64.Half; |
| | 24 | 297 | | s = Fixed64.Half / s; |
| | 24 | 298 | | x = (matrix.m21 - matrix.m12) * s; |
| | 24 | 299 | | y = (matrix.m02 - matrix.m20) * s; |
| | 24 | 300 | | z = (matrix.m10 - matrix.m01) * s; |
| | 24 | 301 | | } |
| | 5 | 302 | | else if (matrix.m00 > matrix.m11 && matrix.m00 > matrix.m22) |
| | 1 | 303 | | { |
| | 1 | 304 | | Fixed64 s = FixedMath.Sqrt(Fixed64.One + matrix.m00 - matrix.m11 - matrix.m22); |
| | 1 | 305 | | x = s * Fixed64.Half; |
| | 1 | 306 | | s = Fixed64.Half / s; |
| | 1 | 307 | | y = (matrix.m10 + matrix.m01) * s; |
| | 1 | 308 | | z = (matrix.m02 + matrix.m20) * s; |
| | 1 | 309 | | w = (matrix.m21 - matrix.m12) * s; |
| | 1 | 310 | | } |
| | 4 | 311 | | else if (matrix.m11 > matrix.m22) |
| | 1 | 312 | | { |
| | 1 | 313 | | Fixed64 s = FixedMath.Sqrt(Fixed64.One + matrix.m11 - matrix.m00 - matrix.m22); |
| | 1 | 314 | | y = s * Fixed64.Half; |
| | 1 | 315 | | s = Fixed64.Half / s; |
| | 1 | 316 | | z = (matrix.m21 + matrix.m12) * s; |
| | 1 | 317 | | x = (matrix.m10 + matrix.m01) * s; |
| | 1 | 318 | | w = (matrix.m02 - matrix.m20) * s; |
| | 1 | 319 | | } |
| | | 320 | | else |
| | 3 | 321 | | { |
| | 3 | 322 | | Fixed64 s = FixedMath.Sqrt(Fixed64.One + matrix.m22 - matrix.m00 - matrix.m11); |
| | 3 | 323 | | z = s * Fixed64.Half; |
| | 3 | 324 | | s = Fixed64.Half / s; |
| | 3 | 325 | | x = (matrix.m02 + matrix.m20) * s; |
| | 3 | 326 | | y = (matrix.m21 + matrix.m12) * s; |
| | 3 | 327 | | w = (matrix.m10 - matrix.m01) * s; |
| | 3 | 328 | | } |
| | | 329 | | |
| | 29 | 330 | | return new FixedQuaternion(x, y, z, w); |
| | 29 | 331 | | } |
| | | 332 | | |
| | | 333 | | /// <summary> |
| | | 334 | | /// Converts a rotation matrix (upper-left 3x3 part of a 4x4 matrix) into a quaternion representation. |
| | | 335 | | /// </summary> |
| | | 336 | | /// <param name="matrix">The 4x4 matrix containing the rotation component.</param> |
| | | 337 | | /// <remarks>Extracts the upper-left 3x3 rotation part of the 4x4</remarks> |
| | | 338 | | /// <returns>A quaternion representing the same rotation as the matrix.</returns> |
| | | 339 | | public static FixedQuaternion FromMatrix(Fixed4x4 matrix) |
| | 23 | 340 | | { |
| | | 341 | | |
| | 23 | 342 | | var rotationMatrix = new Fixed3x3( |
| | 23 | 343 | | matrix.m00, matrix.m01, matrix.m02, |
| | 23 | 344 | | matrix.m10, matrix.m11, matrix.m12, |
| | 23 | 345 | | matrix.m20, matrix.m21, matrix.m22 |
| | 23 | 346 | | ); |
| | | 347 | | |
| | 23 | 348 | | return FromMatrix(rotationMatrix); |
| | 23 | 349 | | } |
| | | 350 | | |
| | | 351 | | /// <summary> |
| | | 352 | | /// Creates a quaternion representing the rotation needed to align the forward vector with the given direction. |
| | | 353 | | /// </summary> |
| | | 354 | | /// <param name="direction">The target direction vector.</param> |
| | | 355 | | /// <returns>A quaternion representing the rotation to align with the direction.</returns> |
| | | 356 | | public static FixedQuaternion FromDirection(Vector3d direction) |
| | 2 | 357 | | { |
| | | 358 | | // Compute the rotation axis as the cross product of the standard forward vector and the desired direction |
| | 2 | 359 | | Vector3d axis = Vector3d.Cross(Vector3d.Forward, direction); |
| | 2 | 360 | | Fixed64 axisLength = axis.Magnitude; |
| | | 361 | | |
| | | 362 | | // If the axis length is very close to zero, it means that the desired direction is almost equal to the standard |
| | 2 | 363 | | if (axisLength.Abs() == Fixed64.Zero) |
| | 1 | 364 | | return Identity; // Return the identity quaternion if no rotation is needed |
| | | 365 | | |
| | | 366 | | // Normalize the rotation axis |
| | 1 | 367 | | axis = (axis / axisLength).Normal; |
| | | 368 | | |
| | | 369 | | // Compute the angle between the standard forward vector and the desired direction |
| | 1 | 370 | | Fixed64 angle = FixedMath.Acos(Vector3d.Dot(Vector3d.Forward, direction)); |
| | | 371 | | |
| | | 372 | | // Compute the rotation quaternion from the axis and angle |
| | 1 | 373 | | return FromAxisAngle(axis, angle); |
| | 2 | 374 | | } |
| | | 375 | | |
| | | 376 | | /// <summary> |
| | | 377 | | /// Creates a quaternion representing a rotation around a specified axis by a given angle. |
| | | 378 | | /// </summary> |
| | | 379 | | /// <param name="axis">The axis to rotate around (must be normalized).</param> |
| | | 380 | | /// <param name="angle">The rotation angle in radians.</param> |
| | | 381 | | /// <returns>A quaternion representing the rotation.</returns> |
| | | 382 | | public static FixedQuaternion FromAxisAngle(Vector3d axis, Fixed64 angle) |
| | 36 | 383 | | { |
| | | 384 | | // Check if the axis is a unit vector |
| | 36 | 385 | | if (!axis.IsNormalized()) |
| | 1 | 386 | | axis = axis.Normalize(); |
| | | 387 | | |
| | | 388 | | // Check if the angle is in a valid range (-pi, pi) |
| | 36 | 389 | | if (angle < -FixedMath.PI || angle > FixedMath.PI) |
| | 1 | 390 | | throw new ArgumentOutOfRangeException(nameof(angle), angle, $"Angle must be in the range ({-FixedMath.PI}, { |
| | | 391 | | |
| | 35 | 392 | | Fixed64 halfAngle = angle / Fixed64.Two; // Half-angle formula |
| | 35 | 393 | | Fixed64 sinHalfAngle = FixedMath.Sin(halfAngle); |
| | 35 | 394 | | Fixed64 cosHalfAngle = FixedMath.Cos(halfAngle); |
| | | 395 | | |
| | 35 | 396 | | return new FixedQuaternion(axis.x * sinHalfAngle, axis.y * sinHalfAngle, axis.z * sinHalfAngle, cosHalfAngle); |
| | 35 | 397 | | } |
| | | 398 | | |
| | | 399 | | /// <summary> |
| | | 400 | | /// Assume the input angles are in degrees and converts them to radians before calling <see cref="FromEulerAngles"/> |
| | | 401 | | /// </summary> |
| | | 402 | | /// <param name="pitch"></param> |
| | | 403 | | /// <param name="yaw"></param> |
| | | 404 | | /// <param name="roll"></param> |
| | | 405 | | /// <returns></returns> |
| | | 406 | | public static FixedQuaternion FromEulerAnglesInDegrees(Fixed64 pitch, Fixed64 yaw, Fixed64 roll) |
| | 27 | 407 | | { |
| | | 408 | | // Convert input angles from degrees to radians |
| | 27 | 409 | | pitch = FixedMath.DegToRad(pitch); |
| | 27 | 410 | | yaw = FixedMath.DegToRad(yaw); |
| | 27 | 411 | | roll = FixedMath.DegToRad(roll); |
| | | 412 | | |
| | | 413 | | // Call the original method that expects angles in radians |
| | 27 | 414 | | return FromEulerAngles(pitch, yaw, roll).Normalize(); |
| | 27 | 415 | | } |
| | | 416 | | |
| | | 417 | | /// <summary> |
| | | 418 | | /// Converts Euler angles (pitch, yaw, roll) to a quaternion and normalizes the result afterwards. Assumes the input |
| | | 419 | | /// </summary> |
| | | 420 | | /// <remarks> |
| | | 421 | | /// The order of operations is YZX or yaw-roll-pitch, commonly used in applications such as robotics. |
| | | 422 | | /// </remarks> |
| | | 423 | | public static FixedQuaternion FromEulerAngles(Fixed64 pitch, Fixed64 yaw, Fixed64 roll) |
| | 32 | 424 | | { |
| | | 425 | | // Check if the angles are in a valid range (-pi, pi) |
| | 32 | 426 | | if (pitch < -FixedMath.PI || pitch > FixedMath.PI) |
| | 1 | 427 | | throw new ArgumentOutOfRangeException(nameof(pitch), pitch, $"Pitch must be in the range ({-FixedMath.PI}, { |
| | 31 | 428 | | if (yaw < -FixedMath.PI || yaw > FixedMath.PI) |
| | 1 | 429 | | throw new ArgumentOutOfRangeException(nameof(yaw), yaw, $"Yaw must be in the range ({-FixedMath.PI}, {FixedM |
| | 30 | 430 | | if (roll < -FixedMath.PI || roll > FixedMath.PI) |
| | 1 | 431 | | throw new ArgumentOutOfRangeException(nameof(roll), roll, $"Roll must be in the range ({-FixedMath.PI}, {Fix |
| | | 432 | | |
| | 29 | 433 | | Fixed64 c1 = FixedMath.Cos(yaw / Fixed64.Two); |
| | 29 | 434 | | Fixed64 s1 = FixedMath.Sin(yaw / Fixed64.Two); |
| | 29 | 435 | | Fixed64 c2 = FixedMath.Cos(roll / Fixed64.Two); |
| | 29 | 436 | | Fixed64 s2 = FixedMath.Sin(roll / Fixed64.Two); |
| | 29 | 437 | | Fixed64 c3 = FixedMath.Cos(pitch / Fixed64.Two); |
| | 29 | 438 | | Fixed64 s3 = FixedMath.Sin(pitch / Fixed64.Two); |
| | | 439 | | |
| | 29 | 440 | | Fixed64 c1c2 = c1 * c2; |
| | 29 | 441 | | Fixed64 s1s2 = s1 * s2; |
| | | 442 | | |
| | 29 | 443 | | Fixed64 w = c1c2 * c3 - s1s2 * s3; |
| | 29 | 444 | | Fixed64 x = c1c2 * s3 + s1s2 * c3; |
| | 29 | 445 | | Fixed64 y = s1 * c2 * c3 + c1 * s2 * s3; |
| | 29 | 446 | | Fixed64 z = c1 * s2 * c3 - s1 * c2 * s3; |
| | | 447 | | |
| | 29 | 448 | | return GetNormalized(new FixedQuaternion(x, y, z, w)); |
| | 29 | 449 | | } |
| | | 450 | | |
| | | 451 | | /// <summary> |
| | | 452 | | /// Computes the logarithm of a quaternion, which represents the rotational displacement. |
| | | 453 | | /// This is useful for interpolation and angular velocity calculations. |
| | | 454 | | /// </summary> |
| | | 455 | | /// <param name="q">The quaternion to compute the logarithm of.</param> |
| | | 456 | | /// <returns>A Vector3d representing the logarithm of the quaternion (axis-angle representation).</returns> |
| | | 457 | | /// <remarks> |
| | | 458 | | /// The logarithm of a unit quaternion is given by: |
| | | 459 | | /// log(q) = (θ * v̂), where: |
| | | 460 | | /// - θ = 2 * acos(w) is the rotation angle. |
| | | 461 | | /// - v̂ = (x, y, z) / ||(x, y, z)|| is the unit vector representing the axis of rotation. |
| | | 462 | | /// If the quaternion is close to identity, the function returns a zero vector to avoid numerical instability. |
| | | 463 | | /// </remarks> |
| | | 464 | | public static Vector3d QuaternionLog(FixedQuaternion q) |
| | 7 | 465 | | { |
| | | 466 | | // Ensure the quaternion is normalized |
| | 7 | 467 | | q = q.Normal; |
| | | 468 | | |
| | | 469 | | // Extract vector part |
| | 7 | 470 | | Vector3d v = new Vector3d(q.x, q.y, q.z); |
| | 7 | 471 | | Fixed64 vLength = v.Magnitude; |
| | | 472 | | |
| | | 473 | | // If rotation is very small, avoid division by zero |
| | 7 | 474 | | if (vLength < Fixed64.FromRaw(0x00001000L)) // Small epsilon |
| | 3 | 475 | | return Vector3d.Zero; |
| | | 476 | | |
| | | 477 | | // Compute angle (theta = 2 * acos(w)) |
| | 4 | 478 | | Fixed64 theta = Fixed64.Two * FixedMath.Acos(q.w); |
| | | 479 | | |
| | | 480 | | // Convert to angular velocity |
| | 4 | 481 | | return (v / vLength) * theta; |
| | 7 | 482 | | } |
| | | 483 | | |
| | | 484 | | /// <summary> |
| | | 485 | | /// Computes the angular velocity required to move from `previousRotation` to `currentRotation` over a given time st |
| | | 486 | | /// </summary> |
| | | 487 | | /// <param name="currentRotation">The current orientation as a quaternion.</param> |
| | | 488 | | /// <param name="previousRotation">The previous orientation as a quaternion.</param> |
| | | 489 | | /// <param name="deltaTime">The time step over which the rotation occurs.</param> |
| | | 490 | | /// <returns>A Vector3d representing the angular velocity (in radians per second).</returns> |
| | | 491 | | /// <remarks> |
| | | 492 | | /// This function calculates the change in rotation over `deltaTime` and converts it into angular velocity. |
| | | 493 | | /// - First, it computes the relative rotation: `rotationDelta = currentRotation * previousRotation.Inverse()`. |
| | | 494 | | /// - Then, it applies `QuaternionLog(rotationDelta)` to extract the axis-angle representation. |
| | | 495 | | /// - Finally, it divides by `deltaTime` to compute the angular velocity. |
| | | 496 | | /// </remarks> |
| | | 497 | | public static Vector3d ToAngularVelocity( |
| | | 498 | | FixedQuaternion currentRotation, |
| | | 499 | | FixedQuaternion previousRotation, |
| | | 500 | | Fixed64 deltaTime) |
| | 4 | 501 | | { |
| | 4 | 502 | | FixedQuaternion rotationDelta = currentRotation * previousRotation.Inverse(); |
| | 4 | 503 | | Vector3d angularDisplacement = QuaternionLog(rotationDelta); |
| | | 504 | | |
| | 4 | 505 | | return angularDisplacement / deltaTime; // Convert to angular velocity |
| | 4 | 506 | | } |
| | | 507 | | |
| | | 508 | | /// <summary> |
| | | 509 | | /// Performs a simple linear interpolation between the components of the input quaternions |
| | | 510 | | /// </summary> |
| | | 511 | | public static FixedQuaternion Lerp(FixedQuaternion a, FixedQuaternion b, Fixed64 t) |
| | 2 | 512 | | { |
| | 2 | 513 | | t = FixedMath.Clamp01(t); |
| | | 514 | | |
| | | 515 | | FixedQuaternion result; |
| | 2 | 516 | | Fixed64 oneMinusT = Fixed64.One - t; |
| | 2 | 517 | | result.x = a.x * oneMinusT + b.x * t; |
| | 2 | 518 | | result.y = a.y * oneMinusT + b.y * t; |
| | 2 | 519 | | result.z = a.z * oneMinusT + b.z * t; |
| | 2 | 520 | | result.w = a.w * oneMinusT + b.w * t; |
| | | 521 | | |
| | 2 | 522 | | result.Normalize(); |
| | | 523 | | |
| | 2 | 524 | | return result; |
| | 2 | 525 | | } |
| | | 526 | | |
| | | 527 | | /// <summary> |
| | | 528 | | /// Calculates the spherical linear interpolation, which results in a smoother and more accurate rotation interpola |
| | | 529 | | /// </summary> |
| | | 530 | | public static FixedQuaternion Slerp(FixedQuaternion a, FixedQuaternion b, Fixed64 t) |
| | 2 | 531 | | { |
| | 2 | 532 | | t = FixedMath.Clamp01(t); |
| | | 533 | | |
| | 2 | 534 | | Fixed64 cosOmega = a.x * b.x + a.y * b.y + a.z * b.z + a.w * b.w; |
| | | 535 | | |
| | | 536 | | // If the dot product is negative, negate one of the input quaternions. |
| | | 537 | | // This ensures that the interpolation takes the shortest path around the sphere. |
| | 2 | 538 | | if (cosOmega < Fixed64.Zero) |
| | 1 | 539 | | { |
| | 1 | 540 | | b.x = -b.x; |
| | 1 | 541 | | b.y = -b.y; |
| | 1 | 542 | | b.z = -b.z; |
| | 1 | 543 | | b.w = -b.w; |
| | 1 | 544 | | cosOmega = -cosOmega; |
| | 1 | 545 | | } |
| | | 546 | | |
| | | 547 | | Fixed64 k0, k1; |
| | | 548 | | |
| | | 549 | | // If the quaternions are close, use linear interpolation |
| | 2 | 550 | | if (cosOmega > Fixed64.One - Fixed64.Precision) |
| | 1 | 551 | | { |
| | 1 | 552 | | k0 = Fixed64.One - t; |
| | 1 | 553 | | k1 = t; |
| | 1 | 554 | | } |
| | | 555 | | else |
| | 1 | 556 | | { |
| | | 557 | | // Otherwise, use spherical linear interpolation |
| | 1 | 558 | | Fixed64 sinOmega = FixedMath.Sqrt(Fixed64.One - cosOmega * cosOmega); |
| | 1 | 559 | | Fixed64 omega = FixedMath.Atan2(sinOmega, cosOmega); |
| | | 560 | | |
| | 1 | 561 | | k0 = FixedMath.Sin((Fixed64.One - t) * omega) / sinOmega; |
| | 1 | 562 | | k1 = FixedMath.Sin(t * omega) / sinOmega; |
| | 1 | 563 | | } |
| | | 564 | | |
| | | 565 | | FixedQuaternion result; |
| | 2 | 566 | | result.x = a.x * k0 + b.x * k1; |
| | 2 | 567 | | result.y = a.y * k0 + b.y * k1; |
| | 2 | 568 | | result.z = a.z * k0 + b.z * k1; |
| | 2 | 569 | | result.w = a.w * k0 + b.w * k1; |
| | | 570 | | |
| | 2 | 571 | | return result; |
| | 2 | 572 | | } |
| | | 573 | | |
| | | 574 | | /// <summary> |
| | | 575 | | /// Returns the angle in degrees between two rotations a and b. |
| | | 576 | | /// </summary> |
| | | 577 | | /// <param name="a">The first rotation.</param> |
| | | 578 | | /// <param name="b">The second rotation.</param> |
| | | 579 | | /// <returns>The angle in degrees between the two rotations.</returns> |
| | | 580 | | public static Fixed64 Angle(FixedQuaternion a, FixedQuaternion b) |
| | 1 | 581 | | { |
| | | 582 | | // Calculate the dot product of the two quaternions |
| | 1 | 583 | | Fixed64 dot = Dot(a, b); |
| | | 584 | | |
| | | 585 | | // Ensure the dot product is in the range of [-1, 1] to avoid floating-point inaccuracies |
| | 1 | 586 | | dot = FixedMath.Clamp(dot, -Fixed64.One, Fixed64.One); |
| | | 587 | | |
| | | 588 | | // Calculate the angle between the two quaternions using the inverse cosine (arccos) |
| | | 589 | | // arccos(dot(a, b)) gives us the angle in radians, so we convert it to degrees |
| | 1 | 590 | | Fixed64 angleInRadians = FixedMath.Acos(dot); |
| | | 591 | | |
| | | 592 | | // Convert the angle from radians to degrees |
| | 1 | 593 | | Fixed64 angleInDegrees = FixedMath.RadToDeg(angleInRadians); |
| | | 594 | | |
| | 1 | 595 | | return angleInDegrees; |
| | 1 | 596 | | } |
| | | 597 | | |
| | | 598 | | /// <summary> |
| | | 599 | | /// Creates a quaternion from an angle and axis. |
| | | 600 | | /// </summary> |
| | | 601 | | /// <param name="angle">The angle in degrees.</param> |
| | | 602 | | /// <param name="axis">The axis to rotate around (must be normalized).</param> |
| | | 603 | | /// <returns>A quaternion representing the rotation.</returns> |
| | | 604 | | public static FixedQuaternion AngleAxis(Fixed64 angle, Vector3d axis) |
| | 1 | 605 | | { |
| | | 606 | | // Convert the angle to radians |
| | 1 | 607 | | angle = angle.ToRadians(); |
| | | 608 | | |
| | | 609 | | // Normalize the axis |
| | 1 | 610 | | axis = axis.Normal; |
| | | 611 | | |
| | | 612 | | // Use the half-angle formula (sin(theta / 2), cos(theta / 2)) |
| | 1 | 613 | | Fixed64 halfAngle = angle / Fixed64.Two; |
| | 1 | 614 | | Fixed64 sinHalfAngle = FixedMath.Sin(halfAngle); |
| | 1 | 615 | | Fixed64 cosHalfAngle = FixedMath.Cos(halfAngle); |
| | | 616 | | |
| | 1 | 617 | | return new FixedQuaternion( |
| | 1 | 618 | | axis.x * sinHalfAngle, |
| | 1 | 619 | | axis.y * sinHalfAngle, |
| | 1 | 620 | | axis.z * sinHalfAngle, |
| | 1 | 621 | | cosHalfAngle |
| | 1 | 622 | | ); |
| | 1 | 623 | | } |
| | | 624 | | |
| | | 625 | | /// <summary> |
| | | 626 | | /// Calculates the dot product of two quaternions. |
| | | 627 | | /// </summary> |
| | | 628 | | /// <param name="a">The first quaternion.</param> |
| | | 629 | | /// <param name="b">The second quaternion.</param> |
| | | 630 | | /// <returns>The dot product of the two quaternions.</returns> |
| | | 631 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 632 | | public static Fixed64 Dot(FixedQuaternion a, FixedQuaternion b) |
| | 6 | 633 | | { |
| | 6 | 634 | | return a.w * b.w + a.x * b.x + a.y * b.y + a.z * b.z; |
| | 6 | 635 | | } |
| | | 636 | | |
| | | 637 | | #endregion |
| | | 638 | | |
| | | 639 | | #region Operators |
| | | 640 | | |
| | | 641 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 642 | | public static FixedQuaternion operator *(FixedQuaternion a, FixedQuaternion b) |
| | 12 | 643 | | { |
| | 12 | 644 | | return new FixedQuaternion( |
| | 12 | 645 | | a.w * b.x + a.x * b.w + a.y * b.z - a.z * b.y, |
| | 12 | 646 | | a.w * b.y - a.x * b.z + a.y * b.w + a.z * b.x, |
| | 12 | 647 | | a.w * b.z + a.x * b.y - a.y * b.x + a.z * b.w, |
| | 12 | 648 | | a.w * b.w - a.x * b.x - a.y * b.y - a.z * b.z |
| | 12 | 649 | | ); |
| | 12 | 650 | | } |
| | | 651 | | |
| | | 652 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 653 | | public static FixedQuaternion operator *(FixedQuaternion q, Fixed64 scalar) |
| | 4 | 654 | | { |
| | 4 | 655 | | return new FixedQuaternion(q.x * scalar, q.y * scalar, q.z * scalar, q.w * scalar); |
| | 4 | 656 | | } |
| | | 657 | | |
| | | 658 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 659 | | public static FixedQuaternion operator *(Fixed64 scalar, FixedQuaternion q) |
| | 1 | 660 | | { |
| | 1 | 661 | | return new FixedQuaternion(q.x * scalar, q.y * scalar, q.z * scalar, q.w * scalar); |
| | 1 | 662 | | } |
| | | 663 | | |
| | | 664 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 665 | | public static FixedQuaternion operator /(FixedQuaternion q, Fixed64 scalar) |
| | 1 | 666 | | { |
| | 1 | 667 | | return new FixedQuaternion(q.x / scalar, q.y / scalar, q.z / scalar, q.w / scalar); |
| | 1 | 668 | | } |
| | | 669 | | |
| | | 670 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 671 | | public static FixedQuaternion operator /(Fixed64 scalar, FixedQuaternion q) |
| | 1 | 672 | | { |
| | 1 | 673 | | return new FixedQuaternion(q.x / scalar, q.y / scalar, q.z / scalar, q.w / scalar); |
| | 1 | 674 | | } |
| | | 675 | | |
| | | 676 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 677 | | public static FixedQuaternion operator +(FixedQuaternion q1, FixedQuaternion q2) |
| | 1 | 678 | | { |
| | 1 | 679 | | return new FixedQuaternion(q1.x + q2.x, q1.y + q2.y, q1.z + q2.z, q1.w + q2.w); |
| | 1 | 680 | | } |
| | | 681 | | |
| | | 682 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 683 | | public static bool operator ==(FixedQuaternion left, FixedQuaternion right) |
| | 10 | 684 | | { |
| | 10 | 685 | | return left.Equals(right); |
| | 10 | 686 | | } |
| | | 687 | | |
| | | 688 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 689 | | public static bool operator !=(FixedQuaternion left, FixedQuaternion right) |
| | 1 | 690 | | { |
| | 1 | 691 | | return !left.Equals(right); |
| | 1 | 692 | | } |
| | | 693 | | |
| | | 694 | | #endregion |
| | | 695 | | |
| | | 696 | | #region Conversion |
| | | 697 | | |
| | | 698 | | /// <summary> |
| | | 699 | | /// Converts this FixedQuaternion to Euler angles (pitch, yaw, roll). |
| | | 700 | | /// </summary> |
| | | 701 | | /// <remarks> |
| | | 702 | | /// Handles the case where the pitch angle (asin of sinp) would be out of the range -π/2 to π/2. |
| | | 703 | | /// This is known as the gimbal lock situation, where the pitch angle reaches ±90 degrees and we lose one degree of |
| | | 704 | | /// In this case, we simply set the pitch to ±90 degrees depending on the sign of sinp. |
| | | 705 | | /// </remarks> |
| | | 706 | | /// <returns>A Vector3d representing the Euler angles (in degrees) equivalent to this FixedQuaternion in YZX order ( |
| | | 707 | | public Vector3d ToEulerAngles() |
| | 3 | 708 | | { |
| | | 709 | | // roll (x-axis rotation) |
| | 3 | 710 | | Fixed64 sinr_cosp = 2 * (w * x + y * z); |
| | 3 | 711 | | Fixed64 cosr_cosp = Fixed64.One - 2 * (x * x + y * y); |
| | 3 | 712 | | Fixed64 roll = FixedMath.Atan2(sinr_cosp, cosr_cosp); |
| | | 713 | | |
| | | 714 | | // pitch (y-axis rotation) |
| | 3 | 715 | | Fixed64 sinp = 2 * (w * y - z * x); |
| | | 716 | | Fixed64 pitch; |
| | 3 | 717 | | if (sinp.Abs() >= Fixed64.One) |
| | 1 | 718 | | pitch = FixedMath.CopySign(FixedMath.PiOver2, sinp); // use 90 degrees if out of range |
| | | 719 | | else |
| | 2 | 720 | | pitch = FixedMath.Asin(sinp); |
| | | 721 | | |
| | | 722 | | // yaw (z-axis rotation) |
| | 3 | 723 | | Fixed64 siny_cosp = 2 * (w * z + x * y); |
| | 3 | 724 | | Fixed64 cosy_cosp = Fixed64.One - 2 * (y * y + z * z); |
| | 3 | 725 | | Fixed64 yaw = FixedMath.Atan2(siny_cosp, cosy_cosp); |
| | | 726 | | |
| | | 727 | | // Convert radians to degrees |
| | 3 | 728 | | roll = FixedMath.RadToDeg(roll); |
| | 3 | 729 | | pitch = FixedMath.RadToDeg(pitch); |
| | 3 | 730 | | yaw = FixedMath.RadToDeg(yaw); |
| | | 731 | | |
| | 3 | 732 | | return new Vector3d(roll, pitch, yaw); |
| | 3 | 733 | | } |
| | | 734 | | |
| | | 735 | | /// <summary> |
| | | 736 | | /// Converts this FixedQuaternion to a direction vector. |
| | | 737 | | /// </summary> |
| | | 738 | | /// <returns>A Vector3d representing the direction equivalent to this FixedQuaternion.</returns> |
| | | 739 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 740 | | public Vector3d ToDirection() |
| | 1 | 741 | | { |
| | 1 | 742 | | return new Vector3d( |
| | 1 | 743 | | 2 * (x * z - w * y), |
| | 1 | 744 | | 2 * (y * z + w * x), |
| | 1 | 745 | | Fixed64.One - 2 * (x * x + y * y) |
| | 1 | 746 | | ); |
| | 1 | 747 | | } |
| | | 748 | | |
| | | 749 | | /// <summary> |
| | | 750 | | /// Converts the quaternion into a 3x3 rotation matrix. |
| | | 751 | | /// </summary> |
| | | 752 | | /// <returns>A FixedMatrix3x3 representing the same rotation as the quaternion.</returns> |
| | | 753 | | public Fixed3x3 ToMatrix3x3() |
| | 27 | 754 | | { |
| | 27 | 755 | | Fixed64 x2 = x * x; |
| | 27 | 756 | | Fixed64 y2 = y * y; |
| | 27 | 757 | | Fixed64 z2 = z * z; |
| | 27 | 758 | | Fixed64 xy = x * y; |
| | 27 | 759 | | Fixed64 xz = x * z; |
| | 27 | 760 | | Fixed64 yz = y * z; |
| | 27 | 761 | | Fixed64 xw = x * w; |
| | 27 | 762 | | Fixed64 yw = y * w; |
| | 27 | 763 | | Fixed64 zw = z * w; |
| | | 764 | | |
| | 27 | 765 | | Fixed3x3 result = new Fixed3x3(); |
| | 27 | 766 | | Fixed64 scale = Fixed64.One * 2; |
| | | 767 | | |
| | 27 | 768 | | result.m00 = Fixed64.One - scale * (y2 + z2); |
| | 27 | 769 | | result.m01 = scale * (xy - zw); |
| | 27 | 770 | | result.m02 = scale * (xz + yw); |
| | | 771 | | |
| | 27 | 772 | | result.m10 = scale * (xy + zw); |
| | 27 | 773 | | result.m11 = Fixed64.One - scale * (x2 + z2); |
| | 27 | 774 | | result.m12 = scale * (yz - xw); |
| | | 775 | | |
| | 27 | 776 | | result.m20 = scale * (xz - yw); |
| | 27 | 777 | | result.m21 = scale * (yz + xw); |
| | 27 | 778 | | result.m22 = Fixed64.One - scale * (x2 + y2); |
| | | 779 | | |
| | 27 | 780 | | return result; |
| | 27 | 781 | | } |
| | | 782 | | |
| | | 783 | | #endregion |
| | | 784 | | |
| | | 785 | | #region Equality and HashCode Overrides |
| | | 786 | | |
| | | 787 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 788 | | public override bool Equals(object? obj) |
| | 7 | 789 | | { |
| | 7 | 790 | | return obj is FixedQuaternion other && Equals(other); |
| | 7 | 791 | | } |
| | | 792 | | |
| | | 793 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 794 | | public bool Equals(FixedQuaternion other) |
| | 31 | 795 | | { |
| | 31 | 796 | | return other.x == x && other.y == y && other.z == z && other.w == w; |
| | 31 | 797 | | } |
| | | 798 | | |
| | | 799 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 800 | | public override int GetHashCode() |
| | 2 | 801 | | { |
| | 2 | 802 | | return x.GetHashCode() ^ y.GetHashCode() << 2 ^ z.GetHashCode() >> 2 ^ w.GetHashCode(); |
| | 2 | 803 | | } |
| | | 804 | | |
| | | 805 | | /// <summary> |
| | | 806 | | /// Returns a formatted string for this quaternion. |
| | | 807 | | /// </summary> |
| | | 808 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 809 | | public override string ToString() |
| | 78 | 810 | | { |
| | 78 | 811 | | return $"({x}, {y}, {z}, {w})"; |
| | 78 | 812 | | } |
| | | 813 | | |
| | | 814 | | #endregion |
| | | 815 | | } |