< Summary

Information
Class: FixedMathSharp.FixedQuaternion
Assembly: FixedMathSharp
File(s): File 1: /home/runner/work/FixedMathSharp/FixedMathSharp/src/FixedMathSharp/Numerics/Rotations/FixedQuaternion.Conversions.cs
File 2: /home/runner/work/FixedMathSharp/FixedMathSharp/src/FixedMathSharp/Numerics/Rotations/FixedQuaternion.cs
File 3: /home/runner/work/FixedMathSharp/FixedMathSharp/src/FixedMathSharp/Numerics/Rotations/FixedQuaternion.Equality.cs
File 4: /home/runner/work/FixedMathSharp/FixedMathSharp/src/FixedMathSharp/Numerics/Rotations/FixedQuaternion.Operators.cs
File 5: /home/runner/work/FixedMathSharp/FixedMathSharp/src/FixedMathSharp/Numerics/Rotations/FixedQuaternion.ScaledTransform.cs
File 6: /home/runner/work/FixedMathSharp/FixedMathSharp/src/FixedMathSharp/Numerics/Rotations/FixedQuaternion.Statics.cs
Line coverage
100%
Covered lines: 426
Uncovered lines: 0
Coverable lines: 426
Total lines: 1398
Line coverage: 100%
Branch coverage
100%
Covered branches: 128
Total branches: 128
Branch coverage: 100%
Method coverage

Feature is only available for sponsors

Upgrade to PRO version

Metrics

MethodBranch coverage Crap Score Cyclomatic complexity Line coverage
File 1: ToEulerAngles()100%22100%
File 1: ToDirection()100%11100%
File 1: ToMatrix3x3()100%66100%
File 1: Deconstruct(...)100%11100%
File 1: Deconstruct(...)100%11100%
File 1: Deconstruct(...)100%11100%
File 1: Deconstruct(...)100%11100%
File 2: get_Identity()100%11100%
File 2: get_Zero()100%11100%
File 2: .ctor(...)100%11100%
File 2: get_Normalized()100%11100%
File 2: get_Magnitude()100%11100%
File 2: get_MagnitudeSquared()100%11100%
File 2: get_EulerAngles()100%11100%
File 2: set_EulerAngles(...)100%11100%
File 2: get_Item(...)100%55100%
File 2: set_Item(...)100%55100%
File 2: Set(...)100%11100%
File 2: NormalizeInPlace()100%11100%
File 2: Conjugate()100%11100%
File 2: Inverse()100%44100%
File 2: Rotate(...)100%11100%
File 2: TryRotate(...)100%22100%
File 2: TryTransformPoint(...)100%44100%
File 2: TryTransformPoint(...)100%22100%
File 2: TryGetRelativeOffset(...)100%44100%
File 2: Rotated(...)100%22100%
File 3: Equals(...)100%22100%
File 3: Equals(...)100%66100%
File 3: GetHashCode()100%11100%
File 3: ToString()100%11100%
File 3: ToString(...)100%11100%
File 3: TryFormat(...)100%1818100%
File 4: op_Multiply(...)100%11100%
File 4: op_Multiply(...)100%11100%
File 4: op_Multiply(...)100%11100%
File 4: op_Division(...)100%11100%
File 4: op_Addition(...)100%11100%
File 4: op_Subtraction(...)100%11100%
File 4: op_UnaryNegation(...)100%11100%
File 4: op_Equality(...)100%11100%
File 4: op_Inequality(...)100%11100%
File 5: TryTransformScaledPoint(...)100%11100%
File 5: TryTransformScaledPoint(...)100%22100%
File 5: TryInverseTransformScaledPoint(...)100%88100%
File 6: IsNormalized()100%22100%
File 6: GetMagnitude(...)100%11100%
File 6: GetNormalizationSquaredMagnitude(...)100%11100%
File 6: GetNormalizationMagnitude(...)100%88100%
File 6: GetNormalized(...)100%1212100%
File 6: GetScaleNormalized(...)100%11100%
File 6: Divide(...)100%22100%
File 6: LookRotation(...)100%22100%
File 6: FromMatrix(...)100%88100%
File 6: FromMatrix(...)100%11100%
File 6: FromDirection(...)100%1010100%
File 6: FromAxisAngle(...)100%44100%
File 6: FromEulerAnglesInDegrees(...)100%11100%
File 6: FromEulerAngles(...)100%11100%
File 6: QuaternionLog(...)100%22100%
File 6: ToAngularVelocity(...)100%11100%
File 6: Lerp(...)100%22100%
File 6: Slerp(...)100%44100%
File 6: Angle(...)100%11100%
File 6: AngleAxis(...)100%11100%
File 6: Dot(...)100%11100%

File(s)

/home/runner/work/FixedMathSharp/FixedMathSharp/src/FixedMathSharp/Numerics/Rotations/FixedQuaternion.Conversions.cs

#LineLine coverage
 1//=======================================================================
 2// FixedQuaternion.Conversions.cs
 3//=======================================================================
 4// MIT License, Copyright (c) 2024–present David Oravsky (mrdav30)
 5// See LICENSE file in the project root for full license information.
 6//=======================================================================
 7
 8using System.Runtime.CompilerServices;
 9
 10namespace FixedMathSharp;
 11
 12/// <content>
 13/// Conversion methods for <see cref="FixedQuaternion"/>, including Euler angles,
 14/// direction vectors, 3x3 rotation matrices, and component deconstruction.
 15/// </content>
 16public partial struct FixedQuaternion
 17{
 18    #region Conversion
 19
 20    /// <summary>
 21    /// Converts this quaternion to Euler angles in degrees.
 22    /// Returns angles as (pitch, yaw, roll), where:
 23    /// pitch = rotation around X
 24    /// yaw   = rotation around Y
 25    /// roll  = rotation around Z
 26    ///
 27    /// The extraction matches FromEulerAngles(), which composes rotations in YXZ order:
 28    /// q = qy * qx * qz
 29    /// </summary>
 30    public Vector3d ToEulerAngles()
 31    {
 432        Fixed3x3 m = ToMatrix3x3();
 33
 34        Fixed64 pitch;
 35        Fixed64 yaw;
 36        Fixed64 roll;
 37
 38        // For YXZ:
 39        // m32 = -sin(pitch)
 40        // m31 =  sin(yaw) * cos(pitch)
 41        // m33 =  cos(yaw) * cos(pitch)
 42        // m12 =  sin(roll) * cos(pitch)
 43        // m22 =  cos(roll) * cos(pitch)
 44
 445        Fixed64 sinPitch = -m.M32;
 46
 447        if (sinPitch.Abs() >= Fixed64.One)
 48        {
 49            // Gimbal lock: pitch is ±90°, yaw/roll are coupled.
 350            pitch = FixedMath.CopySign(Fixed64.HalfPi, sinPitch);
 51
 52            // Choose roll = 0 and solve remaining yaw from matrix.
 353            roll = Fixed64.Zero;
 354            yaw = FixedMath.Atan2(-m.M13, m.M11);
 55        }
 56        else
 57        {
 158            pitch = FixedMath.Asin(sinPitch);
 159            yaw = FixedMath.Atan2(m.M31, m.M33);
 160            roll = FixedMath.Atan2(m.M12, m.M22);
 61        }
 62
 463        return new Vector3d(
 464            FixedMath.RadToDeg(pitch),
 465            FixedMath.RadToDeg(yaw),
 466            FixedMath.RadToDeg(roll));
 67    }
 68
 69    /// <summary>
 70    /// Converts this FixedQuaternion to the rotated canonical forward direction.
 71    /// </summary>
 72    /// <remarks>
 73    /// The identity quaternion returns <see cref="Vector3d.Forward"/> because FixedMathSharp's
 74    /// canonical 3D forward direction is <c>+Z</c>.
 75    /// </remarks>
 76    /// <returns>A Vector3d representing the rotated canonical forward direction.</returns>
 77    [MethodImpl(MethodImplOptions.AggressiveInlining)]
 78    public Vector3d ToDirection() =>
 179        new(2 * (X * Z - W * Y),
 180            2 * (Y * Z + W * X),
 181            Fixed64.One - 2 * (X * X + Y * Y));
 82
 83
 84    /// <summary>
 85    /// Converts the quaternion into a 3x3 rotation matrix.
 86    /// </summary>
 87    /// <remarks>
 88    /// Every nonzero scalar multiple represents the same rotation. The zero
 89    /// quaternion converts to <see cref="Fixed3x3.Identity"/>.
 90    /// </remarks>
 91    /// <returns>A FixedMatrix3x3 representing the same rotation as the quaternion.</returns>
 92    public Fixed3x3 ToMatrix3x3()
 93    {
 3752794        Fixed64 componentScale = FixedMath.Max(
 3752795            FixedMath.Max(X.Abs(), Y.Abs()),
 3752796            FixedMath.Max(Z.Abs(), W.Abs()));
 3752797        if (componentScale == Fixed64.Zero)
 298            return Fixed3x3.Identity;
 99
 37525100        Fixed64 x = X;
 37525101        Fixed64 y = Y;
 37525102        Fixed64 z = Z;
 37525103        Fixed64 w = W;
 104
 105        // Ordinary rotation inputs can use their original coordinates without
 106        // square underflow, sum saturation, or material factor quantization.
 37525107        if (componentScale < Fixed64.Half
 37525108            || componentScale > Fixed64.Two)
 109        {
 15110            x /= componentScale;
 15111            y /= componentScale;
 15112            z /= componentScale;
 15113            w /= componentScale;
 114        }
 115
 37525116        Fixed64 x2 = x * x;
 37525117        Fixed64 y2 = y * y;
 37525118        Fixed64 z2 = z * z;
 37525119        Fixed64 xy = x * y;
 37525120        Fixed64 xz = x * z;
 37525121        Fixed64 yz = y * z;
 37525122        Fixed64 xw = x * w;
 37525123        Fixed64 yw = y * w;
 37525124        Fixed64 zw = z * w;
 125
 37525126        Fixed3x3 result = new();
 37525127        Fixed64 factor = Fixed64.Two / (x2 + y2 + z2 + (w * w));
 128
 37525129        result.M11 = Fixed64.One - factor * (y2 + z2);
 37525130        result.M12 = factor * (xy + zw);
 37525131        result.M13 = factor * (xz - yw);
 132
 37525133        result.M21 = factor * (xy - zw);
 37525134        result.M22 = Fixed64.One - factor * (x2 + z2);
 37525135        result.M23 = factor * (yz + xw);
 136
 37525137        result.M31 = factor * (xz + yw);
 37525138        result.M32 = factor * (yz - xw);
 37525139        result.M33 = Fixed64.One - factor * (x2 + y2);
 140
 37525141        return result;
 142    }
 143
 144    /// <summary>
 145    /// Deconstructs the quaternion into its four Fixed64 components.
 146    /// </summary>
 147    [MethodImpl(MethodImplOptions.AggressiveInlining)]
 148    public void Deconstruct(out Fixed64 x, out Fixed64 y, out Fixed64 z, out Fixed64 w)
 149    {
 1150        x = X;
 1151        y = Y;
 1152        z = Z;
 1153        w = W;
 1154    }
 155
 156    /// <summary>
 157    /// Deconstructs the quaternion into its four int components.
 158    /// </summary>
 159    [MethodImpl(MethodImplOptions.AggressiveInlining)]
 160    public void Deconstruct(out int x, out int y, out int z, out int w)
 161    {
 1162        x = X.RoundToInt();
 1163        y = Y.RoundToInt();
 1164        z = Z.RoundToInt();
 1165        w = W.RoundToInt();
 1166    }
 167
 168    /// <summary>
 169    /// Deconstructs the quaternion into its four long components.
 170    /// </summary>
 171    [MethodImpl(MethodImplOptions.AggressiveInlining)]
 172    public void Deconstruct(out long x, out long y, out long z, out long w)
 173    {
 1174        x = X.m_rawValue;
 1175        y = Y.m_rawValue;
 1176        z = Z.m_rawValue;
 1177        w = W.m_rawValue;
 1178    }
 179
 180    /// <summary>
 181    /// Deconstructs the quaternion into its four double components.
 182    /// </summary>
 183    [MethodImpl(MethodImplOptions.AggressiveInlining)]
 184    public void Deconstruct(out double x, out double y, out double z, out double w)
 185    {
 1186        x = (double)X;
 1187        y = (double)Y;
 1188        z = (double)Z;
 1189        w = (double)W;
 1190    }
 191
 192    #endregion
 193}

/home/runner/work/FixedMathSharp/FixedMathSharp/src/FixedMathSharp/Numerics/Rotations/FixedQuaternion.cs

#LineLine coverage
 1//=======================================================================
 2// FixedQuaternion.cs
 3//=======================================================================
 4// MIT License, Copyright (c) 2024–present David Oravsky (mrdav30)
 5// See LICENSE file in the project root for full license information.
 6//=======================================================================
 7
 8using System;
 9using System.Runtime.CompilerServices;
 10using System.Text.Json.Serialization;
 11using FixedMathSharp.Geometry;
 12using MemoryPack;
 13
 14namespace FixedMathSharp;
 15
 16/// <summary>
 17/// Represents a quaternion (x, y, z, w) with fixed-point numbers.
 18/// Quaternions are useful for representing rotations and can be used to perform smooth rotations and avoid gimbal lock.
 19/// </summary>
 20/// <remarks>
 21/// Direction-oriented quaternion APIs use FixedMathSharp's canonical 3D convention: <c>+X</c>
 22/// right, <c>+Y</c> up, and <c>+Z</c> forward. Convert external engine or tool directions
 23/// before calling these APIs when their semantic basis differs.
 24/// </remarks>
 25[Serializable]
 26[MemoryPackable]
 27public partial struct FixedQuaternion : IEquatable<FixedQuaternion>, IFormattable
 28#if NET8_0_OR_GREATER
 29    , ISpanFormattable
 30#endif
 31{
 32    #region Static Readonly Fields
 33
 34    /// <summary>
 35    /// Identity quaternion (0, 0, 0, 1).
 36    /// </summary>
 2453437    public static FixedQuaternion Identity => new(Fixed64.Zero, Fixed64.Zero, Fixed64.Zero, Fixed64.One);
 38
 39    /// <summary>
 40    /// Empty quaternion (0, 0, 0, 0).
 41    /// </summary>
 300842    public static FixedQuaternion Zero => new(Fixed64.Zero, Fixed64.Zero, Fixed64.Zero, Fixed64.Zero);
 43
 44    #endregion
 45    #region Fields and Constants
 46
 47    private const long NearOppositeDirectionDotRaw = -4290672328L;
 48
 49    // Approximately 9.536743e-7 in Q32.32.
 50    private const long QuaternionLogVectorThresholdRaw = 4_096L;
 51
 52    /// <summary>
 53    /// Represents the X component of the vector as a fixed-point value.
 54    /// </summary>
 55    [JsonInclude]
 56    [MemoryPackOrder(0)]
 57    public Fixed64 X;
 58
 59    /// <summary>
 60    /// Represents the Y component of the vector as a fixed-point value.
 61    /// </summary>
 62    [JsonInclude]
 63    [MemoryPackOrder(1)]
 64    public Fixed64 Y;
 65
 66    /// <summary>
 67    /// Represents the Z component of the vector as a fixed-point value.
 68    /// </summary>
 69    [JsonInclude]
 70    [MemoryPackOrder(2)]
 71    public Fixed64 Z;
 72
 73    /// <summary>
 74    /// Represents the W component of the vector as a fixed-point value.
 75    /// </summary>
 76    [JsonInclude]
 77    [MemoryPackOrder(3)]
 78    public Fixed64 W;
 79
 80    #endregion
 81    #region Constructors
 82
 83    /// <summary>
 84    /// Creates a new FixedQuaternion with the specified components.
 85    /// </summary>
 86    public FixedQuaternion(Fixed64 x, Fixed64 y, Fixed64 z, Fixed64 w)
 87    {
 7471088        X = x;
 7471089        Y = y;
 7471090        Z = z;
 7471091        W = w;
 7471092    }
 93
 94    #endregion
 95    #region Properties
 96
 97    /// <summary>
 98    /// Normalized version of this quaternion.
 99    /// </summary>
 100    [JsonIgnore]
 101    [MemoryPackIgnore]
 102    public FixedQuaternion Normalized
 103    {
 104        [MethodImpl(MethodImplOptions.AggressiveInlining)]
 15104105        get => GetNormalized(this);
 106    }
 107
 108    /// <summary>
 109    /// Gets the magnitude of this quaternion.
 110    /// </summary>
 111    [JsonIgnore]
 112    [MemoryPackIgnore]
 113    public Fixed64 Magnitude
 114    {
 115        [MethodImpl(MethodImplOptions.AggressiveInlining)]
 13116        get => GetMagnitude(this);
 117    }
 118
 119    /// <summary>
 120    /// Gets the squared magnitude of this quaternion.
 121    /// </summary>
 122    [JsonIgnore]
 123    [MemoryPackIgnore]
 124    public Fixed64 MagnitudeSquared
 125    {
 126        [MethodImpl(MethodImplOptions.AggressiveInlining)]
 1135127        get => X * X + Y * Y + Z * Z + W * W;
 128    }
 129
 130    /// <summary>
 131    /// Returns the Euler angles (in degrees) of this quaternion.
 132    /// </summary>
 133    [JsonIgnore]
 134    [MemoryPackIgnore]
 135    public Vector3d EulerAngles
 136    {
 137        [MethodImpl(MethodImplOptions.AggressiveInlining)]
 2138        get => ToEulerAngles();
 139        [MethodImpl(MethodImplOptions.AggressiveInlining)]
 1140        set => this = FromEulerAnglesInDegrees(value.X, value.Y, value.Z);
 141    }
 142
 143    /// <summary>
 144    /// Gets or sets the component value at the specified index.
 145    /// </summary>
 146    /// <remarks>Index 0 corresponds to the x component, 1 to y, 2 to z, and 3 to w.</remarks>
 147    /// <param name="index">The zero-based index of the component to access. Valid values are 0 (x), 1 (y), 2 (z), and 3
 148    /// <returns>The value of the component at the specified index.</returns>
 149    /// <exception cref="IndexOutOfRangeException">Thrown when the specified index is less than 0 or greater than 3.</ex
 150    [JsonIgnore]
 151    [MemoryPackIgnore]
 152    public Fixed64 this[int index]
 153    {
 154        [MethodImpl(MethodImplOptions.AggressiveInlining)]
 155        get
 156        {
 5157            return index switch
 5158            {
 1159                0 => X,
 1160                1 => Y,
 1161                2 => Z,
 1162                3 => W,
 1163                _ => throw new IndexOutOfRangeException("Invalid FixedQuaternion index!"),
 5164            };
 165        }
 166        [MethodImpl(MethodImplOptions.AggressiveInlining)]
 167        set
 168        {
 169            switch (index)
 170            {
 171                case 0:
 1172                    X = value;
 1173                    break;
 174                case 1:
 1175                    Y = value;
 1176                    break;
 177                case 2:
 1178                    Z = value;
 1179                    break;
 180                case 3:
 1181                    W = value;
 1182                    break;
 183                default:
 1184                    throw new IndexOutOfRangeException("Invalid FixedQuaternion index!");
 185            }
 186        }
 187    }
 188
 189    #endregion
 190    #region Methods (Instance)
 191
 192    /// <summary>
 193    /// Set x, y, z and w components of an existing Quaternion.
 194    /// </summary>
 195    /// <param name="newX"></param>
 196    /// <param name="newY"></param>
 197    /// <param name="newZ"></param>
 198    /// <param name="newW"></param>
 199    [MethodImpl(MethodImplOptions.AggressiveInlining)]
 200    public void Set(Fixed64 newX, Fixed64 newY, Fixed64 newZ, Fixed64 newW)
 201    {
 1202        X = newX;
 1203        Y = newY;
 1204        Z = newZ;
 1205        W = newW;
 1206    }
 207
 208    /// <summary>
 209    /// Normalizes this quaternion in place.
 210    /// </summary>
 211    [MethodImpl(MethodImplOptions.AggressiveInlining)]
 18212    public FixedQuaternion NormalizeInPlace() => this = GetNormalized(this);
 213
 214    /// <summary>
 215    /// Returns the conjugate of this quaternion (inverses the rotational effect).
 216    /// </summary>
 217    [MethodImpl(MethodImplOptions.AggressiveInlining)]
 4907218    public FixedQuaternion Conjugate() => new(-X, -Y, -Z, W);
 219
 220    /// <summary>
 221    /// Returns the inverse of this quaternion.
 222    /// </summary>
 223    public FixedQuaternion Inverse()
 224    {
 8596225        if (this == Identity) return Identity;
 1132226        Fixed64 norm = MagnitudeSquared;
 1133227        if (norm == Fixed64.Zero) return this; // Handle division by zero by returning the same quaternion
 228
 1131229        Fixed64 invNorm = Fixed64.One / norm;
 1131230        return new FixedQuaternion(X * -invNorm, Y * -invNorm, Z * -invNorm, W * invNorm);
 231    }
 232
 233    /// <summary>
 234    /// Rotates a vector by this quaternion.
 235    /// </summary>
 236    public Vector3d Rotate(Vector3d v)
 237    {
 4901238        FixedQuaternion normalizedQuat = Normalized;
 4901239        FixedQuaternion vQuat = new(v.X, v.Y, v.Z, Fixed64.Zero);
 4901240        FixedQuaternion invQuat = normalizedQuat.Conjugate();
 4901241        FixedQuaternion rotatedVQuat = (normalizedQuat * vQuat) * invQuat;
 4901242        return new Vector3d(rotatedVQuat.X, rotatedVQuat.Y, rotatedVQuat.Z);
 243    }
 244
 245    /// <summary>
 246    /// Attempts to rotate a vector using the quaternion's exact
 247    /// scale-invariant rational basis.
 248    /// </summary>
 249    /// <remarks>
 250    /// Each result component is rounded once after the complete linear
 251    /// combination. The zero quaternion preserves the legacy
 252    /// <see cref="Rotate(Vector3d)"/> result of zero.
 253    /// </remarks>
 254    /// <returns>
 255    /// <see langword="true"/> when every final component is representable;
 256    /// otherwise, <see langword="false"/> and <paramref name="result"/> is
 257    /// <see langword="default"/>.
 258    /// </returns>
 259    public bool TryRotate(Vector3d vector, out Vector3d result)
 260    {
 111261        if (this == Zero)
 262        {
 1263            result = Vector3d.Zero;
 1264            return true;
 265        }
 266
 110267        return WideOrientedBox.TryTransformLocalOffset(
 110268            this,
 110269            vector,
 110270            out result);
 271    }
 272
 273    /// <summary>
 274    /// Attempts to transform a local point by this rotation and a world origin
 275    /// with one final round-half-to-even conversion per component.
 276    /// </summary>
 277    /// <remarks>
 278    /// The zero quaternion preserves the legacy rotation contract and returns
 279    /// <paramref name="origin"/>.
 280    /// </remarks>
 281    public bool TryTransformPoint(
 282        Vector3d origin,
 283        Vector3d localPoint,
 284        out Vector3d result)
 285    {
 76286        if (this == Zero)
 287        {
 1288            result = origin;
 1289            return true;
 290        }
 75291        if (this == Identity)
 35292            return Vector3d.TryAdd(origin, localPoint, out result);
 293
 40294        return WideOrientedBox.TryMaterializeLocalPoint(
 40295            origin,
 40296            this,
 40297            localPoint,
 40298            out result);
 299    }
 300
 301    /// <summary>
 302    /// Attempts to transform a local point by this rotation and add two world
 303    /// origins with one final round-half-to-even conversion per component.
 304    /// </summary>
 305    /// <remarks>
 306    /// The zero quaternion preserves the legacy rotation contract and returns
 307    /// the exact sum of the two origins.
 308    /// </remarks>
 309    public bool TryTransformPoint(
 310        Vector3d firstOrigin,
 311        Vector3d secondOrigin,
 312        Vector3d localPoint,
 313        out Vector3d result)
 314    {
 103315        if (this == Zero)
 316        {
 34317            return Vector3d.TrySubtractSums(
 34318                firstOrigin,
 34319                secondOrigin,
 34320                Vector3d.Zero,
 34321                Vector3d.Zero,
 34322                out result);
 323        }
 324
 69325        return WideOrientedBox.TryMaterializeLocalPoint(
 69326            firstOrigin,
 69327            secondOrigin,
 69328            this,
 69329            localPoint,
 69330            out result);
 331    }
 332
 333    /// <summary>
 334    /// Attempts to obtain the exact relative offset
 335    /// <c>firstOrigin + firstOffset - secondOrigin - Rotate(secondLocalPoint)</c>.
 336    /// </summary>
 337    /// <remarks>
 338    /// No rotated point or intermediate sum is narrowed independently. The
 339    /// zero quaternion preserves the legacy zero-rotation-result contract.
 340    /// </remarks>
 341    public bool TryGetRelativeOffset(
 342        Vector3d firstOrigin,
 343        Vector3d firstOffset,
 344        Vector3d secondOrigin,
 345        Vector3d secondLocalPoint,
 346        out Vector3d result)
 347    {
 71348        if (this == Zero)
 349        {
 1350            return Vector3d.TrySubtractSums(
 1351                firstOrigin,
 1352                firstOffset,
 1353                secondOrigin,
 1354                Vector3d.Zero,
 1355                out result);
 356        }
 70357        if (this == Identity)
 358        {
 35359            return Vector3d.TrySubtractSums(
 35360                firstOrigin,
 35361                firstOffset,
 35362                secondOrigin,
 35363                secondLocalPoint,
 35364                out result);
 365        }
 366
 35367        return WideOrientedBox.TryGetRelativeOffset(
 35368            this,
 35369            firstOrigin,
 35370            firstOffset,
 35371            secondOrigin,
 35372            secondLocalPoint,
 35373            out result);
 374    }
 375
 376    /// <summary>
 377    /// Rotates this quaternion by a given angle around a specified axis (default: Y-axis).
 378    /// </summary>
 379    /// <param name="sin">Sine of the rotation angle.</param>
 380    /// <param name="cos">Cosine of the rotation angle.</param>
 381    /// <param name="axis">The axis to rotate around (default: Vector3d.Up).</param>
 382    /// <returns>A new quaternion representing the rotated result.</returns>
 383    [MethodImpl(MethodImplOptions.AggressiveInlining)]
 384    public FixedQuaternion Rotated(Fixed64 sin, Fixed64 cos, Vector3d? axis = null)
 385    {
 4386        Vector3d rotateAxis = axis ?? Vector3d.Up;
 387
 388        // The rotation angle is the arc tangent of sin and cos
 4389        Fixed64 angle = FixedMath.Atan2(sin, cos);
 390
 391        // Construct a quaternion representing a rotation around the axis (default is y aka Vector3d.up)
 4392        FixedQuaternion rotationQuat = FromAxisAngle(rotateAxis, angle);
 393
 394        // Apply the rotation and return the result
 4395        return rotationQuat * this;
 396    }
 397
 398    #endregion
 399}

/home/runner/work/FixedMathSharp/FixedMathSharp/src/FixedMathSharp/Numerics/Rotations/FixedQuaternion.Equality.cs

#LineLine coverage
 1//=======================================================================
 2// FixedQuaternion.Equality.cs
 3//=======================================================================
 4// MIT License, Copyright (c) 2024–present David Oravsky (mrdav30)
 5// See LICENSE file in the project root for full license information.
 6//=======================================================================
 7
 8using System;
 9using System.Globalization;
 10using System.Runtime.CompilerServices;
 11
 12namespace FixedMathSharp;
 13
 14/// <content>
 15/// Equality, hashing, and string formatting for <see cref="FixedQuaternion"/>.
 16/// </content>
 17public partial struct FixedQuaternion
 18{
 19    #region Equality and HashCode Overrides
 20
 21    /// <inheritdoc/>
 22    [MethodImpl(MethodImplOptions.AggressiveInlining)]
 723    public override bool Equals(object? obj) => obj is FixedQuaternion other && Equals(other);
 24
 25    /// <inheritdoc/>
 26    [MethodImpl(MethodImplOptions.AggressiveInlining)]
 27    public bool Equals(FixedQuaternion other) =>
 1126028        X == other.X && Y == other.Y && Z == other.Z && W == other.W;
 29
 30    /// <inheritdoc/>
 31    [MethodImpl(MethodImplOptions.AggressiveInlining)]
 32    public override int GetHashCode() =>
 1033        X.GetHashCode() ^ Y.GetHashCode() << 2 ^ Z.GetHashCode() >> 2 ^ W.GetHashCode();
 34
 35    /// <summary>
 36    /// Returns a string that represents the current object in the format "(x, y, z, w)".
 37    /// </summary>
 38    /// <returns>A string containing the values of the object formatted as a tuple.</returns>
 39    [MethodImpl(MethodImplOptions.AggressiveInlining)]
 485540    public override string ToString() => ToString(null, CultureInfo.InvariantCulture);
 41
 42    /// <summary>
 43    /// Returns a string that represents the current object in the format "(x, y, z, w)".
 44    /// </summary>
 45    [MethodImpl(MethodImplOptions.AggressiveInlining)]
 46    public string ToString(string? format, IFormatProvider? formatProvider)
 47    {
 487148        FixedQuaternion value = this;
 487149        return FixedDiagnosticsFormatter.ToString((Span<char> destination, out int charsWritten) =>
 487150            value.TryFormat(destination, out charsWritten, format.AsSpan(), formatProvider));
 51    }
 52
 53    /// <summary>
 54    /// Formats this quaternion into the provided destination buffer.
 55    /// </summary>
 56    public bool TryFormat(
 57        Span<char> destination,
 58        out int charsWritten,
 59        ReadOnlySpan<char> format,
 60        IFormatProvider? provider)
 61    {
 509962        int written = 0;
 509963        if (!FixedDiagnosticsFormatter.Append('(', destination, ref written) ||
 509964            !FixedDiagnosticsFormatter.Append(X, destination, ref written, format, provider) ||
 509965            !FixedDiagnosticsFormatter.Append(", ", destination, ref written) ||
 509966            !FixedDiagnosticsFormatter.Append(Y, destination, ref written, format, provider) ||
 509967            !FixedDiagnosticsFormatter.Append(", ", destination, ref written) ||
 509968            !FixedDiagnosticsFormatter.Append(Z, destination, ref written, format, provider) ||
 509969            !FixedDiagnosticsFormatter.Append(", ", destination, ref written) ||
 509970            !FixedDiagnosticsFormatter.Append(W, destination, ref written, format, provider) ||
 509971            !FixedDiagnosticsFormatter.Append(')', destination, ref written))
 72        {
 2673            charsWritten = 0;
 2674            return false;
 75        }
 76
 507377        charsWritten = written;
 507378        return true;
 79    }
 80
 81    #endregion
 82}

/home/runner/work/FixedMathSharp/FixedMathSharp/src/FixedMathSharp/Numerics/Rotations/FixedQuaternion.Operators.cs

#LineLine coverage
 1//=======================================================================
 2// FixedQuaternion.Operators.cs
 3//=======================================================================
 4// MIT License, Copyright (c) 2024–present David Oravsky (mrdav30)
 5// See LICENSE file in the project root for full license information.
 6//=======================================================================
 7
 8using System.Runtime.CompilerServices;
 9
 10namespace FixedMathSharp;
 11
 12/// <content>
 13/// Operator overloads for <see cref="FixedQuaternion"/>, including multiplication, scaling, addition,
 14/// subtraction, negation, and equality comparisons.
 15/// </content>
 16public partial struct FixedQuaternion
 17{
 18    #region Operators
 19
 20    /// <summary>
 21    /// Multiplies two quaternions, combining their rotations into a single quaternion.
 22    /// </summary>
 23    /// <remarks>
 24    /// Quaternion multiplication is not commutative; the order of operands affects the result.
 25    /// This operation is commonly used to concatenate rotations.
 26    /// </remarks>
 27    /// <param name="a">The first quaternion to multiply.</param>
 28    /// <param name="b">The second quaternion to multiply.</param>
 29    /// <returns>A new FixedQuaternion representing the combined rotation of the two input quaternions.</returns>
 30    [MethodImpl(MethodImplOptions.AggressiveInlining)]
 31    public static FixedQuaternion operator *(FixedQuaternion a, FixedQuaternion b) =>
 1058132        new((a.W * b.X) + (a.X * b.W) + (a.Y * b.Z) - (a.Z * b.Y),
 1058133            (a.W * b.Y) - (a.X * b.Z) + (a.Y * b.W) + (a.Z * b.X),
 1058134            (a.W * b.Z) + (a.X * b.Y) - (a.Y * b.X) + (a.Z * b.W),
 1058135            (a.W * b.W) - (a.X * b.X) - (a.Y * b.Y) - (a.Z * b.Z));
 36
 37    /// <summary>
 38    /// Multiplies each component of the specified quaternion by the given scalar value.
 39    /// </summary>
 40    /// <param name="q">The quaternion whose components are to be multiplied.</param>
 41    /// <param name="scalar">The scalar value by which to multiply each component of the quaternion.</param>
 42    /// <returns>A new FixedQuaternion whose components are the result of multiplying the corresponding components of th
 43    /// quaternion by the scalar value.</returns>
 44    [MethodImpl(MethodImplOptions.AggressiveInlining)]
 45    public static FixedQuaternion operator *(FixedQuaternion q, Fixed64 scalar) =>
 3246        new(q.X * scalar, q.Y * scalar, q.Z * scalar, q.W * scalar);
 47
 48    /// <inheritdoc cref="operator *(FixedQuaternion, Fixed64)"/>
 49    [MethodImpl(MethodImplOptions.AggressiveInlining)]
 50    public static FixedQuaternion operator *(Fixed64 scalar, FixedQuaternion q) =>
 151        new(q.X * scalar, q.Y * scalar, q.Z * scalar, q.W * scalar);
 52
 53    /// <summary>
 54    /// Divides each component of the specified quaternion by the given scalar value.
 55    /// </summary>
 56    /// <remarks>Division by zero will result in an exception or undefined behavior.</remarks>
 57    /// <param name="q">The quaternion whose components are to be divided.</param>
 58    /// <param name="scalar">The scalar value by which to divide each component of the quaternion.</param>
 59    /// <returns>A new FixedQuaternion whose components are the result of dividing the corresponding components of the i
 60    /// quaternion by the scalar value.</returns>
 61    [MethodImpl(MethodImplOptions.AggressiveInlining)]
 62    public static FixedQuaternion operator /(FixedQuaternion q, Fixed64 scalar) =>
 563        new(q.X / scalar, q.Y / scalar, q.Z / scalar, q.W / scalar);
 64
 65    /// <summary>
 66    /// Adds two quaternions component-wise and returns the resulting quaternion.
 67    /// </summary>
 68    /// <remarks>
 69    /// This operation performs a simple component-wise addition.
 70    /// </remarks>
 71    /// <param name="q1">The first quaternion to add.</param>
 72    /// <param name="q2">The second quaternion to add.</param>
 73    /// <returns>A new FixedQuaternion whose components are the sums of the corresponding components of q1 and q2.</retu
 74    [MethodImpl(MethodImplOptions.AggressiveInlining)]
 75    public static FixedQuaternion operator +(FixedQuaternion q1, FixedQuaternion q2) =>
 176        new(q1.X + q2.X, q1.Y + q2.Y, q1.Z + q2.Z, q1.W + q2.W);
 77
 78    /// <summary>
 79    /// Subtracts two quaternions component-wise and returns the resulting quaternion.
 80    /// </summary>
 81    /// <remarks>
 82    /// This operation performs component-wise subtraction.
 83    /// </remarks>
 84    [MethodImpl(MethodImplOptions.AggressiveInlining)]
 85    public static FixedQuaternion operator -(FixedQuaternion q1, FixedQuaternion q2) =>
 186        new(q1.X - q2.X, q1.Y - q2.Y, q1.Z - q2.Z, q1.W - q2.W);
 87
 88    /// <summary>
 89    /// Negates each component of the specified quaternion.
 90    /// </summary>
 91    [MethodImpl(MethodImplOptions.AggressiveInlining)]
 92    public static FixedQuaternion operator -(FixedQuaternion q) =>
 693        new(-q.X, -q.Y, -q.Z, -q.W);
 94
 95    /// <summary>
 96    /// Determines whether two FixedQuaternion instances are equal.
 97    /// </summary>
 98    [MethodImpl(MethodImplOptions.AggressiveInlining)]
 943699    public static bool operator ==(FixedQuaternion left, FixedQuaternion right) => left.Equals(right);
 100
 101    /// <summary>
 102    /// Determines whether two FixedQuaternion instances are not equal.
 103    /// </summary>
 104    [MethodImpl(MethodImplOptions.AggressiveInlining)]
 1049105    public static bool operator !=(FixedQuaternion left, FixedQuaternion right) => !left.Equals(right);
 106
 107    #endregion
 108}

/home/runner/work/FixedMathSharp/FixedMathSharp/src/FixedMathSharp/Numerics/Rotations/FixedQuaternion.ScaledTransform.cs

#LineLine coverage
 1//=======================================================================
 2// FixedQuaternion.ScaledTransform.cs
 3//=======================================================================
 4// MIT License, Copyright (c) 2024–present David Oravsky (mrdav30)
 5// See LICENSE file in the project root for full license information.
 6//=======================================================================
 7
 8namespace FixedMathSharp;
 9
 10/// <content>
 11/// Provides scaled-point transformation helpers for combining rotation, scale,
 12/// and displacement into a single deterministic world-space result.
 13/// </content>
 14public partial struct FixedQuaternion
 15{
 16    /// <summary>
 17    /// Attempts to transform a component-scaled local point by this rotation
 18    /// and a world origin with one final round-half-to-even conversion per
 19    /// component.
 20    /// </summary>
 21    public bool TryTransformScaledPoint(
 22        Vector3d origin,
 23        Vector3d localPoint,
 24        Vector3d scale,
 25        out Vector3d result) =>
 426        TryTransformScaledPoint(
 427            origin,
 428            localPoint,
 429            scale,
 430            Vector3d.Zero,
 431            out result);
 32
 33    /// <summary>
 34    /// Attempts to transform a component-scaled local point plus an unscaled
 35    /// local displacement by this rotation and a world origin with one final
 36    /// round-half-to-even conversion per component.
 37    /// </summary>
 38    /// <remarks>
 39    /// Computes
 40    /// <c>origin + Rotate(scale * localPoint + localDisplacement)</c> without
 41    /// narrowing the scaled point, local sum, or rotated offset independently.
 42    /// The zero quaternion preserves the legacy rotation contract and returns
 43    /// <paramref name="origin"/>.
 44    /// </remarks>
 45    public bool TryTransformScaledPoint(
 46        Vector3d origin,
 47        Vector3d localPoint,
 48        Vector3d scale,
 49        Vector3d localDisplacement,
 50        out Vector3d result)
 51    {
 7152        if (this == Zero)
 53        {
 154            result = origin;
 155            return true;
 56        }
 57
 7058        return WideVector3dTransform.TryTransformScaledPoint(
 7059            origin,
 7060            this,
 7061            localPoint,
 7062            scale,
 7063            localDisplacement,
 7064            out result);
 65    }
 66
 67    /// <summary>
 68    /// Attempts to inverse-transform a world point by this rotation, a world
 69    /// origin, and a component scale with one final round-half-to-even
 70    /// conversion per local component.
 71    /// </summary>
 72    /// <remarks>
 73    /// Computes <c>InverseRotate(worldPoint - origin) / scale</c> without
 74    /// narrowing the world offset or rotated point independently. A zero
 75    /// quaternion or any zero scale component returns <see langword="false"/>.
 76    /// </remarks>
 77    /// <param name="origin">The world-space origin of the scaled local frame.</param>
 78    /// <param name="worldPoint">The world-space point to inverse-transform.</param>
 79    /// <param name="scale">The component scale of the local frame.</param>
 80    /// <param name="result">
 81    /// The local-space point on success; otherwise zero.
 82    /// </param>
 83    /// <returns>
 84    /// <see langword="true"/> when this quaternion is nonzero, every scale
 85    /// component is nonzero, and every final local coordinate is representable;
 86    /// otherwise <see langword="false"/>.
 87    /// </returns>
 88    public bool TryInverseTransformScaledPoint(
 89        Vector3d origin,
 90        Vector3d worldPoint,
 91        Vector3d scale,
 92        out Vector3d result)
 93    {
 10894        if (this == Zero
 10895            || scale.X == Fixed64.Zero
 10896            || scale.Y == Fixed64.Zero
 10897            || scale.Z == Fixed64.Zero)
 98        {
 499            result = default;
 4100            return false;
 101        }
 102
 104103        return WideVector3dTransform.TryInverseTransformScaledPoint(
 104104            origin,
 104105            this,
 104106            worldPoint,
 104107            scale,
 104108            out result);
 109    }
 110}

/home/runner/work/FixedMathSharp/FixedMathSharp/src/FixedMathSharp/Numerics/Rotations/FixedQuaternion.Statics.cs

#LineLine coverage
 1//=======================================================================
 2// FixedQuaternion.Statics.cs
 3//=======================================================================
 4// MIT License, Copyright (c) 2024–present David Oravsky (mrdav30)
 5// See LICENSE file in the project root for full license information.
 6//=======================================================================
 7
 8using System;
 9using System.Runtime.CompilerServices;
 10
 11namespace FixedMathSharp;
 12
 13/// <content>
 14/// Static helper methods and operations for <see cref="FixedQuaternion"/>, including
 15/// normalization, magnitude calculations, and related utility functions.
 16/// </content>
 17public partial struct FixedQuaternion
 18{
 19    #region Quaternion Operations
 20
 21    /// <summary>
 22    /// Checks whether this nonzero quaternion's squared magnitude is within
 23    /// epsilon of one.
 24    /// </summary>
 25    public bool IsNormalized()
 26    {
 3184027        Fixed64 squaredMagnitude = GetNormalizationSquaredMagnitude(this);
 3184028        return squaredMagnitude != Fixed64.Zero
 3184029            && FixedMath.Abs(squaredMagnitude - Fixed64.One) <= Fixed64.Epsilon;
 30    }
 31
 32    /// <summary>
 33    /// Calculates the magnitude (or length) of the specified quaternion.
 34    /// </summary>
 35    /// <remarks>
 36    /// Component squares are accumulated exactly and the integer square root is
 37    /// rounded once. Only an unrepresentable rounded result saturates.
 38    /// </remarks>
 39    /// <param name="q">The quaternion for which to compute the magnitude.</param>
 40    /// <returns>The magnitude of the quaternion as a Fixed64 value. Returns 0 if the quaternion is the zero quaternion.
 41    public static Fixed64 GetMagnitude(FixedQuaternion q)
 2002842        => Fixed64.GetRoundedMagnitude(q.X, q.Y, q.Z, q.W);
 43
 44    private static Fixed64 GetNormalizationSquaredMagnitude(FixedQuaternion q) =>
 4715045        (q.X * q.X) + (q.Y * q.Y) + (q.Z * q.Z) + (q.W * q.W);
 46
 47    private static Fixed64 GetNormalizationMagnitude(
 48        FixedQuaternion q,
 49        out bool isNormalized)
 50    {
 1531051        Fixed64 mag = GetNormalizationSquaredMagnitude(q);
 1531052        isNormalized = mag != Fixed64.Zero
 1531053            && FixedMath.Abs(mag - Fixed64.One) <= Fixed64.Epsilon;
 54
 1531055        if (mag == Fixed64.MaxValue || mag <= FixedMath.ScaleSafeMagnitudeSquaredThreshold)
 1356            return FixedMath.GetScaledMagnitude(q.X, q.Y, q.Z, q.W);
 57
 1529758        if (isNormalized)
 1522659            return Fixed64.One;
 60
 7161        return FixedMath.Sqrt(mag);
 62    }
 63
 64    /// <summary>
 65    /// Normalizes the quaternion to a unit quaternion.
 66    /// </summary>
 67    [MethodImpl(MethodImplOptions.AggressiveInlining)]
 68    public static FixedQuaternion GetNormalized(FixedQuaternion q)
 69    {
 1531070        Fixed64 mag = GetNormalizationMagnitude(q, out bool isNormalized);
 71
 72        // If magnitude is zero, return identity quaternion (to avoid divide by zero)
 1531073        if (mag == Fixed64.Zero)
 474            return Identity;
 75
 1530676        if (isNormalized)
 1522677            return q;
 78
 8079        if (mag == Fixed64.MaxValue || mag == Fixed64.One)
 680            return WideNormalization.GetNormalized(q);
 81
 7482        if (mag <= FixedMath.ScaleSafeMagnitudeThreshold)
 283            return GetScaleNormalized(q);
 84
 7285        var normalized = new FixedQuaternion(
 7286            q.X / mag,
 7287            q.Y / mag,
 7288            q.Z / mag,
 7289            q.W / mag);
 7290        return normalized.IsNormalized()
 7291            ? normalized
 7292            : WideNormalization.GetNormalized(q);
 93    }
 94
 95    private static FixedQuaternion GetScaleNormalized(FixedQuaternion q)
 96    {
 297        Fixed64 scale = FixedMath.Max(
 298            FixedMath.Max(q.X.Abs(), q.Y.Abs()),
 299            FixedMath.Max(q.Z.Abs(), q.W.Abs()));
 2100        FixedQuaternion scaled = q / scale;
 2101        Fixed64 scaledMagnitude = FixedMath.GetScaledMagnitude(
 2102            scaled.X,
 2103            scaled.Y,
 2104            scaled.Z,
 2105            scaled.W);
 2106        return scaled / scaledMagnitude;
 107    }
 108
 109    /// <summary>
 110    /// Divides one quaternion by another using inverse quaternion multiplication.
 111    /// </summary>
 112    /// <remarks>
 113    /// This is equivalent to <c>dividend * Inverse(divisor)</c>.
 114    /// </remarks>
 115    /// <exception cref="InvalidOperationException">
 116    /// Thrown when <paramref name="divisor"/> is not invertible.
 117    /// </exception>
 118    [MethodImpl(MethodImplOptions.AggressiveInlining)]
 119    public static FixedQuaternion Divide(FixedQuaternion dividend, FixedQuaternion divisor)
 120    {
 2121        Fixed64 divisorMagnitudeSquared = divisor.MagnitudeSquared;
 122
 2123        if (divisorMagnitudeSquared == Fixed64.Zero)
 1124            throw new InvalidOperationException("Quaternion divisor is not invertible.");
 125
 1126        Fixed64 invNorm = Fixed64.One / divisorMagnitudeSquared;
 1127        FixedQuaternion inverseDivisor = new(
 1128            -divisor.X * invNorm,
 1129            -divisor.Y * invNorm,
 1130            -divisor.Z * invNorm,
 1131            divisor.W * invNorm);
 132
 1133        return dividend * inverseDivisor;
 134    }
 135
 136    /// <summary>
 137    /// Creates a quaternion whose canonical forward direction aligns with the specified direction.
 138    /// </summary>
 139    /// <remarks>
 140    /// The <paramref name="forward"/> and <paramref name="upwards"/> vectors are expressed in
 141    /// FixedMathSharp's canonical basis: <c>+X</c> right, <c>+Y</c> up, and <c>+Z</c> forward.
 142    /// Use <see cref="CoordinateConvention3d"/> or adapter-specific basis conversion before
 143    /// calling this method when external APIs use different semantic axes.
 144    /// </remarks>
 145    /// <param name="forward">The forward direction vector.</param>
 146    /// <param name="upwards">The upwards direction vector (optional, default: Vector3d.Up).</param>
 147    /// <returns>A quaternion representing the rotation from one direction to another.</returns>
 148    public static FixedQuaternion LookRotation(Vector3d forward, Vector3d? upwards = null)
 149    {
 3150        Vector3d up = upwards ?? Vector3d.Up;
 151
 3152        Vector3d forwardNormalized = forward.Normalized;
 3153        Vector3d right = Vector3d.Cross(up.Normalized, forwardNormalized);
 3154        up = Vector3d.Cross(forwardNormalized, right);
 155
 3156        return FromMatrix(new Fixed3x3(
 3157            right.X, right.Y, right.Z,
 3158            up.X, up.Y, up.Z,
 3159            forwardNormalized.X, forwardNormalized.Y, forwardNormalized.Z));
 160    }
 161
 162    /// <summary>
 163    /// Converts a rotation matrix into a quaternion representation.
 164    /// </summary>
 165    /// <param name="matrix">The rotation matrix to convert.</param>
 166    /// <returns>A quaternion representing the same rotation as the matrix.</returns>
 167    public static FixedQuaternion FromMatrix(Fixed3x3 matrix)
 168    {
 50169        Fixed64 trace = matrix.M11 + matrix.M22 + matrix.M33;
 170
 171        Fixed64 w, x, y, z;
 172
 50173        if (trace > Fixed64.Zero)
 174        {
 42175            Fixed64 s = FixedMath.Sqrt(trace + Fixed64.One);
 42176            w = s * Fixed64.Half;
 42177            s = Fixed64.Half / s;
 42178            x = (matrix.M23 - matrix.M32) * s;
 42179            y = (matrix.M31 - matrix.M13) * s;
 42180            z = (matrix.M12 - matrix.M21) * s;
 181        }
 8182        else if (matrix.M11 > matrix.M22 && matrix.M11 > matrix.M33)
 183        {
 2184            Fixed64 s = FixedMath.Sqrt(Fixed64.One + matrix.M11 - matrix.M22 - matrix.M33);
 2185            x = s * Fixed64.Half;
 2186            s = Fixed64.Half / s;
 2187            y = (matrix.M21 + matrix.M12) * s;
 2188            z = (matrix.M13 + matrix.M31) * s;
 2189            w = (matrix.M23 - matrix.M32) * s;
 190        }
 6191        else if (matrix.M22 > matrix.M33)
 192        {
 2193            Fixed64 s = FixedMath.Sqrt(Fixed64.One + matrix.M22 - matrix.M11 - matrix.M33);
 2194            y = s * Fixed64.Half;
 2195            s = Fixed64.Half / s;
 2196            z = (matrix.M32 + matrix.M23) * s;
 2197            x = (matrix.M21 + matrix.M12) * s;
 2198            w = (matrix.M31 - matrix.M13) * s;
 199        }
 200        else
 201        {
 4202            Fixed64 s = FixedMath.Sqrt(Fixed64.One + matrix.M33 - matrix.M11 - matrix.M22);
 4203            z = s * Fixed64.Half;
 4204            s = Fixed64.Half / s;
 4205            x = (matrix.M13 + matrix.M31) * s;
 4206            y = (matrix.M32 + matrix.M23) * s;
 4207            w = (matrix.M12 - matrix.M21) * s;
 208        }
 209
 50210        return new FixedQuaternion(x, y, z, w);
 211    }
 212
 213    /// <summary>
 214    /// Converts a rotation matrix (upper-left 3x3 part of a 4x4 matrix) into a quaternion representation.
 215    /// </summary>
 216    /// <param name="matrix">The 4x4 matrix containing the rotation component.</param>
 217    /// <remarks>Extracts the upper-left 3x3 rotation part of the 4x4</remarks>
 218    /// <returns>A quaternion representing the same rotation as the matrix.</returns>
 219    public static FixedQuaternion FromMatrix(Fixed4x4 matrix)
 220    {
 12221        Fixed3x3 rotationMatrix = new(
 12222            matrix.M11, matrix.M12, matrix.M13,
 12223            matrix.M21, matrix.M22, matrix.M23,
 12224            matrix.M31, matrix.M32, matrix.M33
 12225        );
 226
 12227        return FromMatrix(rotationMatrix);
 228    }
 229
 230    /// <summary>
 231    /// Creates a quaternion representing the rotation needed to align canonical <c>+Z</c> forward with the given direct
 232    /// </summary>
 233    /// <remarks>
 234    /// <see cref="Vector3d.Forward"/> returns <see cref="FixedQuaternion.Identity"/>. If an external API names
 235    /// <c>-Z</c> as forward, convert that direction into the canonical convention before calling
 236    /// this method.
 237    /// </remarks>
 238    /// <param name="direction">The target direction vector.</param>
 239    /// <returns>A quaternion representing the rotation to align with the direction.</returns>
 240    public static FixedQuaternion FromDirection(Vector3d direction)
 241    {
 12242        if (direction == Vector3d.Zero)
 1243            return Identity;
 244
 11245        if (!direction.IsNormalized())
 2246            direction = direction.Normalized;
 247
 11248        Fixed64 dot = direction.Z;
 11249        if (dot <= -Fixed64.One + Fixed64.Epsilon)
 1250            return FromAxisAngle(Vector3d.Up, Fixed64.Pi);
 251
 10252        if (dot >= Fixed64.One - Fixed64.Epsilon)
 2253            return Identity;
 254
 8255        if (dot < Fixed64.FromRaw(NearOppositeDirectionDotRaw))
 256        {
 1257            Vector3d axis = new Vector3d(-direction.Y, direction.X, Fixed64.Zero).Normalized;
 258
 1259            return FromAxisAngle(axis, FixedMath.Acos(dot));
 260        }
 261
 7262        Fixed64 scale = FixedMath.Sqrt((Fixed64.One + dot) * Fixed64.Two);
 7263        return new FixedQuaternion(
 7264            -direction.Y / scale,
 7265            direction.X / scale,
 7266            Fixed64.Zero,
 7267            scale * Fixed64.Half);
 268    }
 269
 270    /// <summary>
 271    /// Creates a quaternion representing a rotation around a specified axis by a given angle.
 272    /// </summary>
 273    /// <param name="axis">The axis to rotate around. Nonzero inputs are normalized; zero returns identity.</param>
 274    /// <param name="angle">The rotation angle in radians.</param>
 275    /// <returns>A quaternion representing the rotation.</returns>
 276    public static FixedQuaternion FromAxisAngle(Vector3d axis, Fixed64 angle)
 277    {
 697278        if (axis == Vector3d.Zero)
 2279            return Identity;
 280
 695281        if (!axis.IsNormalized())
 18282            axis = axis.Normalized;
 283
 695284        Fixed64 halfAngle = angle / Fixed64.Two;  // Half-angle formula
 695285        Fixed64 sinHalfAngle = FixedMath.Sin(halfAngle);
 695286        Fixed64 cosHalfAngle = FixedMath.Cos(halfAngle);
 287
 695288        return new FixedQuaternion(
 695289            axis.X * sinHalfAngle,
 695290            axis.Y * sinHalfAngle,
 695291            axis.Z * sinHalfAngle,
 695292            cosHalfAngle);
 293    }
 294
 295    /// <summary>
 296    /// Assume the input angles are in degrees and converts them to radians before calling <see cref="FromEulerAngles"/>
 297    /// </summary>
 298    /// <param name="pitch"></param>
 299    /// <param name="yaw"></param>
 300    /// <param name="roll"></param>
 301    /// <returns></returns>
 302    public static FixedQuaternion FromEulerAnglesInDegrees(Fixed64 pitch, Fixed64 yaw, Fixed64 roll)
 303    {
 304        // Convert input angles from degrees to radians
 129305        pitch = FixedMath.DegToRad(pitch);
 129306        yaw = FixedMath.DegToRad(yaw);
 129307        roll = FixedMath.DegToRad(roll);
 308
 309        // Call the original method that expects angles in radians
 129310        return FromEulerAngles(pitch, yaw, roll);
 311    }
 312
 313    /// <summary>
 314    /// Converts Euler angles (pitch, yaw, roll) to a quaternion and normalizes the result afterwards.
 315    /// Assumes the input angles are in radians.
 316    /// </summary>
 317    /// <remarks>
 318    /// The order of operations is YXZ or yaw-pitch-roll
 319    /// </remarks>
 320    public static FixedQuaternion FromEulerAngles(Fixed64 pitch, Fixed64 yaw, Fixed64 roll)
 321    {
 149322        Fixed64 halfPitch = pitch / Fixed64.Two;
 149323        Fixed64 halfYaw = yaw / Fixed64.Two;
 149324        Fixed64 halfRoll = roll / Fixed64.Two;
 325
 149326        Fixed64 sx = FixedMath.Sin(halfPitch);
 149327        Fixed64 cx = FixedMath.Cos(halfPitch);
 149328        Fixed64 sy = FixedMath.Sin(halfYaw);
 149329        Fixed64 cy = FixedMath.Cos(halfYaw);
 149330        Fixed64 sz = FixedMath.Sin(halfRoll);
 149331        Fixed64 cz = FixedMath.Cos(halfRoll);
 332
 333        // q = qy * qx * qz
 149334        Fixed64 x = (cx * sy * sz) + (cy * cz * sx);
 149335        Fixed64 y = (cx * cz * sy) - (cy * sx * sz);
 149336        Fixed64 z = (cx * cy * sz) - (cz * sx * sy);
 149337        Fixed64 w = (cx * cy * cz) + (sx * sy * sz);
 338
 149339        return GetNormalized(new FixedQuaternion(x, y, z, w));
 340    }
 341
 342    /// <summary>
 343    /// Computes the logarithm of a quaternion, which represents the rotational displacement.
 344    /// This is useful for interpolation and angular velocity calculations.
 345    /// </summary>
 346    /// <param name="q">The quaternion to compute the logarithm of.</param>
 347    /// <returns>A Vector3d representing the logarithm of the quaternion (axis-angle representation).</returns>
 348    /// <remarks>
 349    /// The logarithm of a unit quaternion is given by:
 350    /// log(q) = (θ * v̀‚), where:
 351    /// - Î¸ = 2 * acos(w) is the rotation angle.
 352    /// - v̀‚ = (x, y, z) / ||(x, y, z)|| is the unit vector representing the axis of rotation.
 353    /// If the quaternion is close to identity, the function returns a zero vector to avoid numerical instability.
 354    /// </remarks>
 355    public static Vector3d QuaternionLog(FixedQuaternion q)
 356    {
 357        // Ensure the quaternion is normalized
 23358        q = GetNormalized(q);
 359
 360        // Extract vector part
 23361        Vector3d v = new(q.X, q.Y, q.Z);
 23362        Fixed64 vLength = v.Magnitude;
 363
 364        // If rotation is very small, avoid division by zero
 23365        if (vLength < Fixed64.FromRaw(QuaternionLogVectorThresholdRaw))
 5366            return Vector3d.Zero;
 367
 368        // Compute angle (theta = 2 * acos(w))
 18369        Fixed64 normalizedW = FixedMath.Clamp(q.W, -Fixed64.One, Fixed64.One);
 18370        Fixed64 theta = Fixed64.Two * FixedMath.Acos(normalizedW);
 371
 372        // Convert to angular velocity
 18373        return (v / vLength) * theta;
 374    }
 375
 376    /// <summary>
 377    /// Computes the angular velocity required to move from `previousRotation` to `currentRotation` over a given time st
 378    /// </summary>
 379    /// <param name="currentRotation">The current orientation as a quaternion.</param>
 380    /// <param name="previousRotation">The previous orientation as a quaternion.</param>
 381    /// <param name="deltaTime">The time step over which the rotation occurs.</param>
 382    /// <returns>A Vector3d representing the angular velocity (in radians per second).</returns>
 383    /// <remarks>
 384    /// This function calculates the change in rotation over `deltaTime` and converts it into angular velocity.
 385    /// - First, it computes the relative rotation: `rotationDelta = currentRotation * previousRotation.Inverse()`.
 386    /// - Then, it applies `QuaternionLog(rotationDelta)` to extract the axis-angle representation.
 387    /// - Finally, it divides by `deltaTime` to compute the angular velocity.
 388    /// </remarks>
 389    public static Vector3d ToAngularVelocity(
 390        FixedQuaternion currentRotation,
 391        FixedQuaternion previousRotation,
 392        Fixed64 deltaTime)
 393    {
 4394        FixedQuaternion rotationDelta = currentRotation * previousRotation.Inverse();
 4395        Vector3d angularDisplacement = QuaternionLog(rotationDelta);
 396
 4397        return angularDisplacement / deltaTime; // Convert to angular velocity
 398    }
 399
 400    /// <summary>
 401    /// Performs a simple linear interpolation between the components of the input quaternions
 402    /// </summary>
 403    public static FixedQuaternion Lerp(FixedQuaternion a, FixedQuaternion b, Fixed64 t)
 404    {
 5405        t = FixedMath.Clamp01(t);
 406
 5407        if (Dot(a, b) < Fixed64.Zero)
 1408            b = -b;
 409
 410        FixedQuaternion result;
 5411        Fixed64 oneMinusT = Fixed64.One - t;
 5412        result.X = a.X * oneMinusT + b.X * t;
 5413        result.Y = a.Y * oneMinusT + b.Y * t;
 5414        result.Z = a.Z * oneMinusT + b.Z * t;
 5415        result.W = a.W * oneMinusT + b.W * t;
 416
 5417        result.NormalizeInPlace();
 418
 5419        return result;
 420    }
 421
 422    /// <summary>
 423    ///  Calculates the spherical linear interpolation, which results in a smoother and more accurate rotation interpola
 424    /// </summary>
 425    public static FixedQuaternion Slerp(FixedQuaternion a, FixedQuaternion b, Fixed64 t)
 426    {
 4427        t = FixedMath.Clamp01(t);
 428
 4429        Fixed64 cosOmega = a.X * b.X + a.Y * b.Y + a.Z * b.Z + a.W * b.W;
 430
 431        // If the dot product is negative, negate one of the input quaternions.
 432        // This ensures that the interpolation takes the shortest path around the sphere.
 4433        if (cosOmega < Fixed64.Zero)
 434        {
 1435            b.X = -b.X;
 1436            b.Y = -b.Y;
 1437            b.Z = -b.Z;
 1438            b.W = -b.W;
 1439            cosOmega = -cosOmega;
 440        }
 441
 442        Fixed64 k0, k1;
 443
 444        // If the quaternions are close, use linear interpolation
 4445        if (cosOmega > Fixed64.One - Fixed64.Epsilon)
 446        {
 1447            k0 = Fixed64.One - t;
 1448            k1 = t;
 449        }
 450        else
 451        {
 452            // Otherwise, use spherical linear interpolation
 3453            Fixed64 sinOmega = FixedMath.Sqrt(Fixed64.One - cosOmega * cosOmega);
 3454            Fixed64 omega = FixedMath.Atan2(sinOmega, cosOmega);
 455
 3456            k0 = FixedMath.Sin((Fixed64.One - t) * omega) / sinOmega;
 3457            k1 = FixedMath.Sin(t * omega) / sinOmega;
 458        }
 459
 460        FixedQuaternion result;
 4461        result.X = a.X * k0 + b.X * k1;
 4462        result.Y = a.Y * k0 + b.Y * k1;
 4463        result.Z = a.Z * k0 + b.Z * k1;
 4464        result.W = a.W * k0 + b.W * k1;
 465
 4466        return result;
 467    }
 468
 469    /// <summary>
 470    /// Returns the angle in degrees between two rotations a and b.
 471    /// </summary>
 472    /// <param name="a">The first rotation.</param>
 473    /// <param name="b">The second rotation.</param>
 474    /// <returns>The angle in degrees between the two rotations.</returns>
 475    public static Fixed64 Angle(FixedQuaternion a, FixedQuaternion b)
 476    {
 5477        FixedQuaternion normalizedA = a.Normalized;
 5478        FixedQuaternion normalizedB = b.Normalized;
 5479        FixedQuaternion relative = normalizedA.Conjugate() * normalizedB;
 5480        _ = Vector3d.TryGetMagnitude(
 5481            new Vector3d(relative.X, relative.Y, relative.Z),
 5482            out Fixed64 vectorMagnitude);
 5483        Fixed64 halfAngle = FixedMath.Atan2(vectorMagnitude, relative.W.Abs());
 5484        return FixedMath.RadToDeg(halfAngle * Fixed64.Two);
 485    }
 486
 487    /// <summary>
 488    /// Creates a quaternion from an angle and axis.
 489    /// </summary>
 490    /// <param name="angle">The angle in degrees.</param>
 491    /// <param name="axis">The axis to rotate around. Nonzero inputs are normalized; zero returns identity.</param>
 492    /// <returns>A quaternion representing the rotation.</returns>
 493    public static FixedQuaternion AngleAxis(Fixed64 angle, Vector3d axis) =>
 11494        FromAxisAngle(axis, FixedMath.DegToRad(angle));
 495
 496    /// <summary>
 497    /// Calculates the dot product of two quaternions.
 498    /// </summary>
 499    /// <param name="a">The first quaternion.</param>
 500    /// <param name="b">The second quaternion.</param>
 501    /// <returns>The dot product of the two quaternions.</returns>
 502    [MethodImpl(MethodImplOptions.AggressiveInlining)]
 12503    public static Fixed64 Dot(FixedQuaternion a, FixedQuaternion b) => a.W * b.W + a.X * b.X + a.Y * b.Y + a.Z * b.Z;
 504
 505    #endregion
 506}
Generated by: ReportGenerator 5.5.11.0
08/04/2026 - 20:34:44
GitHub | reportgenerator.io

Methods/Properties

ToEulerAngles()
ToDirection()
ToMatrix3x3()
Deconstruct(FixedMathSharp.Fixed64&,FixedMathSharp.Fixed64&,FixedMathSharp.Fixed64&,FixedMathSharp.Fixed64&)
Deconstruct(System.Int32&,System.Int32&,System.Int32&,System.Int32&)
Deconstruct(System.Int64&,System.Int64&,System.Int64&,System.Int64&)
Deconstruct(System.Double&,System.Double&,System.Double&,System.Double&)
get_Identity()
get_Zero()
.ctor(FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64)
get_Normalized()
get_Magnitude()
get_MagnitudeSquared()
get_EulerAngles()
set_EulerAngles(FixedMathSharp.Vector3d)
get_Item(System.Int32)
set_Item(System.Int32,FixedMathSharp.Fixed64)
Set(FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64)
NormalizeInPlace()
Conjugate()
Inverse()
Rotate(FixedMathSharp.Vector3d)
TryRotate(FixedMathSharp.Vector3d,FixedMathSharp.Vector3d&)
TryTransformPoint(FixedMathSharp.Vector3d,FixedMathSharp.Vector3d,FixedMathSharp.Vector3d&)
TryTransformPoint(FixedMathSharp.Vector3d,FixedMathSharp.Vector3d,FixedMathSharp.Vector3d,FixedMathSharp.Vector3d&)
TryGetRelativeOffset(FixedMathSharp.Vector3d,FixedMathSharp.Vector3d,FixedMathSharp.Vector3d,FixedMathSharp.Vector3d,FixedMathSharp.Vector3d&)
Rotated(FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,System.Nullable`1<FixedMathSharp.Vector3d>)
Equals(System.Object)
Equals(FixedMathSharp.FixedQuaternion)
GetHashCode()
ToString()
ToString(System.String,System.IFormatProvider)
TryFormat(System.Span`1<System.Char>,System.Int32&,System.ReadOnlySpan`1<System.Char>,System.IFormatProvider)
op_Multiply(FixedMathSharp.FixedQuaternion,FixedMathSharp.FixedQuaternion)
op_Multiply(FixedMathSharp.FixedQuaternion,FixedMathSharp.Fixed64)
op_Multiply(FixedMathSharp.Fixed64,FixedMathSharp.FixedQuaternion)
op_Division(FixedMathSharp.FixedQuaternion,FixedMathSharp.Fixed64)
op_Addition(FixedMathSharp.FixedQuaternion,FixedMathSharp.FixedQuaternion)
op_Subtraction(FixedMathSharp.FixedQuaternion,FixedMathSharp.FixedQuaternion)
op_UnaryNegation(FixedMathSharp.FixedQuaternion)
op_Equality(FixedMathSharp.FixedQuaternion,FixedMathSharp.FixedQuaternion)
op_Inequality(FixedMathSharp.FixedQuaternion,FixedMathSharp.FixedQuaternion)
TryTransformScaledPoint(FixedMathSharp.Vector3d,FixedMathSharp.Vector3d,FixedMathSharp.Vector3d,FixedMathSharp.Vector3d&)
TryTransformScaledPoint(FixedMathSharp.Vector3d,FixedMathSharp.Vector3d,FixedMathSharp.Vector3d,FixedMathSharp.Vector3d,FixedMathSharp.Vector3d&)
TryInverseTransformScaledPoint(FixedMathSharp.Vector3d,FixedMathSharp.Vector3d,FixedMathSharp.Vector3d,FixedMathSharp.Vector3d&)
IsNormalized()
GetMagnitude(FixedMathSharp.FixedQuaternion)
GetNormalizationSquaredMagnitude(FixedMathSharp.FixedQuaternion)
GetNormalizationMagnitude(FixedMathSharp.FixedQuaternion,System.Boolean&)
GetNormalized(FixedMathSharp.FixedQuaternion)
GetScaleNormalized(FixedMathSharp.FixedQuaternion)
Divide(FixedMathSharp.FixedQuaternion,FixedMathSharp.FixedQuaternion)
LookRotation(FixedMathSharp.Vector3d,System.Nullable`1<FixedMathSharp.Vector3d>)
FromMatrix(FixedMathSharp.Fixed3x3)
FromMatrix(FixedMathSharp.Fixed4x4)
FromDirection(FixedMathSharp.Vector3d)
FromAxisAngle(FixedMathSharp.Vector3d,FixedMathSharp.Fixed64)
FromEulerAnglesInDegrees(FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64)
FromEulerAngles(FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64)
QuaternionLog(FixedMathSharp.FixedQuaternion)
ToAngularVelocity(FixedMathSharp.FixedQuaternion,FixedMathSharp.FixedQuaternion,FixedMathSharp.Fixed64)
Lerp(FixedMathSharp.FixedQuaternion,FixedMathSharp.FixedQuaternion,FixedMathSharp.Fixed64)
Slerp(FixedMathSharp.FixedQuaternion,FixedMathSharp.FixedQuaternion,FixedMathSharp.Fixed64)
Angle(FixedMathSharp.FixedQuaternion,FixedMathSharp.FixedQuaternion)
AngleAxis(FixedMathSharp.Fixed64,FixedMathSharp.Vector3d)
Dot(FixedMathSharp.FixedQuaternion,FixedMathSharp.FixedQuaternion)