Initial commit

srcs/RobotNet10/Shared/RobotNet10.Shared/Numbers/Quaternion.cs

@@ -0,0 +1,605 @@
+using System.Runtime.CompilerServices;
+
+namespace RobotNet10.Shared.Numbers;
+
+/// <summary>
+/// Custom quaternion struct that supports JSON serialization.
+/// Replacement for System.Numerics.Quaternion which doesn't serialize properly.
+/// Represents rotation in 3D space using the formula: q = w + xi + yj + zk
+/// </summary>
+public struct Quaternion : IEquatable<Quaternion>
+{
+    /// <summary>
+    /// X component of the vector part
+    /// </summary>
+    public double X { get; set; }
+
+    /// <summary>
+    /// Y component of the vector part
+    /// </summary>
+    public double Y { get; set; }
+
+    /// <summary>
+    /// Z component of the vector part
+    /// </summary>
+    public double Z { get; set; }
+
+    /// <summary>
+    /// W component (scalar/real part)
+    /// </summary>
+    public double W { get; set; }
+
+    /// <summary>
+    /// Creates a new Quaternion
+    /// </summary>
+    public Quaternion(double x, double y, double z, double w)
+    {
+        X = x;
+        Y = y;
+        Z = z;
+        W = w;
+    }
+
+    /// <summary>
+    /// Creates a quaternion from a vector and scalar parts
+    /// </summary>
+    public Quaternion(Vector3 vectorPart, double scalarPart)
+    {
+        X = vectorPart.X;
+        Y = vectorPart.Y;
+        Z = vectorPart.Z;
+        W = scalarPart;
+    }
+
+    #region Static Properties
+
+    /// <summary>
+    /// Returns the identity quaternion (no rotation)
+    /// </summary>
+    public static Quaternion Identity => new(0, 0, 0, 1);
+
+    public static Quaternion FromYawRadian(double yaw)
+    {
+        var halfYaw = yaw / 2.0;
+        return new Quaternion(0, 0, Math.Sin(halfYaw), Math.Cos(halfYaw));
+    }
+
+    #endregion
+
+    #region Properties
+
+    /// <summary>
+    /// Returns the length (magnitude) of the quaternion
+    /// </summary>
+    public readonly double Length()
+    {
+        return Math.Sqrt(X * X + Y * Y + Z * Z + W * W);
+    }
+
+    /// <summary>
+    /// Returns the squared length of the quaternion (faster than Length)
+    /// </summary>
+    public readonly double LengthSquared()
+    {
+        return X * X + Y * Y + Z * Z + W * W;
+    }
+
+    /// <summary>
+    /// Returns true if this is a unit quaternion
+    /// </summary>
+    public readonly bool IsIdentity
+    {
+        get
+        {
+            return X == 0 && Y == 0 && Z == 0 && W == 1;
+        }
+    }
+
+    #endregion
+
+    #region Methods
+
+    /// <summary>
+    /// Returns a normalized copy of this quaternion (unit length)
+    /// </summary>
+    public readonly Quaternion Normalize()
+    {
+        double length = Length();
+        if (length < double.Epsilon)
+            return Identity;
+
+        double invLength = 1.0 / length;
+        return new Quaternion(X * invLength, Y * invLength, Z * invLength, W * invLength);
+    }
+
+    /// <summary>
+    /// Normalizes this quaternion in place
+    /// </summary>
+    public void NormalizeInPlace()
+    {
+        double length = Length();
+        if (length < double.Epsilon)
+        {
+            X = Y = Z = 0;
+            W = 1;
+            return;
+        }
+
+        double invLength = 1.0 / length;
+        X *= invLength;
+        Y *= invLength;
+        Z *= invLength;
+        W *= invLength;
+    }
+
+    /// <summary>
+    /// Returns the conjugate of this quaternion (negated vector part)
+    /// For unit quaternions, conjugate equals inverse
+    /// </summary>
+    public readonly Quaternion Conjugate()
+    {
+        return new Quaternion(-X, -Y, -Z, W);
+    }
+
+    /// <summary>
+    /// Returns the inverse of this quaternion
+    /// </summary>
+    public readonly Quaternion Inverse()
+    {
+        double lengthSq = LengthSquared();
+        if (lengthSq < double.Epsilon)
+            return Identity;
+
+        double invLengthSq = 1.0 / lengthSq;
+        return new Quaternion(-X * invLengthSq, -Y * invLengthSq, -Z * invLengthSq, W * invLengthSq);
+    }
+
+    #endregion
+
+    #region Static Methods
+
+    /// <summary>
+    /// Calculates the dot product of two quaternions
+    /// </summary>
+    [MethodImpl(MethodImplOptions.AggressiveInlining)]
+    public static double Dot(Quaternion quaternion1, Quaternion quaternion2)
+    {
+        return quaternion1.X * quaternion2.X +
+               quaternion1.Y * quaternion2.Y +
+               quaternion1.Z * quaternion2.Z +
+               quaternion1.W * quaternion2.W;
+    }
+
+    /// <summary>
+    /// Returns the conjugate of a quaternion (static version)
+    /// </summary>
+    [MethodImpl(MethodImplOptions.AggressiveInlining)]
+    public static Quaternion Conjugate(Quaternion value)
+    {
+        return value.Conjugate();
+    }
+
+    /// <summary>
+    /// Returns the inverse of a quaternion (static version)
+    /// </summary>
+    [MethodImpl(MethodImplOptions.AggressiveInlining)]
+    public static Quaternion Inverse(Quaternion value)
+    {
+        return value.Inverse();
+    }
+
+    /// <summary>
+    /// Returns a normalized copy of a quaternion (static version)
+    /// </summary>
+    [MethodImpl(MethodImplOptions.AggressiveInlining)]
+    public static Quaternion Normalize(Quaternion value)
+    {
+        return value.Normalize();
+    }
+
+    /// <summary>
+    /// Multiplies two quaternions (static version)
+    /// </summary>
+    [MethodImpl(MethodImplOptions.AggressiveInlining)]
+    public static Quaternion Multiply(Quaternion value1, Quaternion value2)
+    {
+        return value1 * value2;
+    }
+
+    /// <summary>
+    /// Performs spherical linear interpolation between two quaternions
+    /// </summary>
+    public static Quaternion Slerp(Quaternion quaternion1, Quaternion quaternion2, double amount)
+    {
+        double cosOmega = Dot(quaternion1, quaternion2);
+
+        bool flip = false;
+        if (cosOmega < 0.0)
+        {
+            flip = true;
+            cosOmega = -cosOmega;
+        }
+
+        double s1, s2;
+
+        if (cosOmega > (1.0 - 1e-6))
+        {
+            // Too close, do straight linear interpolation
+            s1 = 1.0 - amount;
+            s2 = flip ? -amount : amount;
+        }
+        else
+        {
+            double omega = Math.Acos(cosOmega);
+            double invSinOmega = 1.0 / Math.Sin(omega);
+
+            s1 = Math.Sin((1.0 - amount) * omega) * invSinOmega;
+            s2 = flip
+                ? -Math.Sin(amount * omega) * invSinOmega
+                : Math.Sin(amount * omega) * invSinOmega;
+        }
+
+        return new Quaternion(
+            s1 * quaternion1.X + s2 * quaternion2.X,
+            s1 * quaternion1.Y + s2 * quaternion2.Y,
+            s1 * quaternion1.Z + s2 * quaternion2.Z,
+            s1 * quaternion1.W + s2 * quaternion2.W
+        );
+    }
+
+    /// <summary>
+    /// Performs linear interpolation between two quaternions
+    /// </summary>
+    [MethodImpl(MethodImplOptions.AggressiveInlining)]
+    public static Quaternion Lerp(Quaternion quaternion1, Quaternion quaternion2, double amount)
+    {
+        double t = amount;
+        double t1 = 1.0 - t;
+
+        Quaternion result;
+
+        double dot = Dot(quaternion1, quaternion2);
+
+        if (dot >= 0.0)
+        {
+            result = new Quaternion(
+                t1 * quaternion1.X + t * quaternion2.X,
+                t1 * quaternion1.Y + t * quaternion2.Y,
+                t1 * quaternion1.Z + t * quaternion2.Z,
+                t1 * quaternion1.W + t * quaternion2.W
+            );
+        }
+        else
+        {
+            result = new Quaternion(
+                t1 * quaternion1.X - t * quaternion2.X,
+                t1 * quaternion1.Y - t * quaternion2.Y,
+                t1 * quaternion1.Z - t * quaternion2.Z,
+                t1 * quaternion1.W - t * quaternion2.W
+            );
+        }
+
+        return result.Normalize();
+    }
+
+    /// <summary>
+    /// Concatenates two quaternions (applies rotation1 followed by rotation2)
+    /// </summary>
+    [MethodImpl(MethodImplOptions.AggressiveInlining)]
+    public static Quaternion Concatenate(Quaternion value1, Quaternion value2)
+    {
+        // This is equivalent to value2 * value1
+        double q1x = value2.X;
+        double q1y = value2.Y;
+        double q1z = value2.Z;
+        double q1w = value2.W;
+
+        double q2x = value1.X;
+        double q2y = value1.Y;
+        double q2z = value1.Z;
+        double q2w = value1.W;
+
+        // Cross product
+        double cx = q1y * q2z - q1z * q2y;
+        double cy = q1z * q2x - q1x * q2z;
+        double cz = q1x * q2y - q1y * q2x;
+
+        double dot = q1x * q2x + q1y * q2y + q1z * q2z;
+
+        return new Quaternion(
+            q1x * q2w + q2x * q1w + cx,
+            q1y * q2w + q2y * q1w + cy,
+            q1z * q2w + q2z * q1w + cz,
+            q1w * q2w - dot
+        );
+    }
+
+    /// <summary>
+    /// Creates a quaternion from an axis and angle
+    /// </summary>
+    public static Quaternion CreateFromAxisAngle(Vector3 axis, double angle)
+    {
+        double halfAngle = angle * 0.5;
+        double s = Math.Sin(halfAngle);
+        double c = Math.Cos(halfAngle);
+
+        return new Quaternion(
+            axis.X * s,
+            axis.Y * s,
+            axis.Z * s,
+            c
+        );
+    }
+
+    /// <summary>
+    /// Creates a quaternion from yaw, pitch, and roll angles (in radians)
+    /// </summary>
+    public static Quaternion CreateFromYawPitchRoll(double yaw, double pitch, double roll)
+    {
+        double halfRoll = roll * 0.5;
+        double halfPitch = pitch * 0.5;
+        double halfYaw = yaw * 0.5;
+
+        double sinRoll = Math.Sin(halfRoll);
+        double cosRoll = Math.Cos(halfRoll);
+        double sinPitch = Math.Sin(halfPitch);
+        double cosPitch = Math.Cos(halfPitch);
+        double sinYaw = Math.Sin(halfYaw);
+        double cosYaw = Math.Cos(halfYaw);
+
+        return new Quaternion(
+            cosYaw * sinPitch * cosRoll + sinYaw * cosPitch * sinRoll,
+            sinYaw * cosPitch * cosRoll - cosYaw * sinPitch * sinRoll,
+            cosYaw * cosPitch * sinRoll - sinYaw * sinPitch * cosRoll,
+            cosYaw * cosPitch * cosRoll + sinYaw * sinPitch * sinRoll
+        );
+    }
+
+    /// <summary>
+    /// Creates a quaternion from a rotation matrix
+    /// </summary>
+    public static Quaternion CreateFromRotationMatrix(double[,] matrix)
+    {
+        if (matrix.GetLength(0) < 3 || matrix.GetLength(1) < 3)
+            return Identity;
+
+        double trace = matrix[0, 0] + matrix[1, 1] + matrix[2, 2];
+
+        Quaternion q = default;
+
+        if (trace > 0.0)
+        {
+            double s = Math.Sqrt(trace + 1.0);
+            q.W = s * 0.5;
+            s = 0.5 / s;
+            q.X = (matrix[2, 1] - matrix[1, 2]) * s;
+            q.Y = (matrix[0, 2] - matrix[2, 0]) * s;
+            q.Z = (matrix[1, 0] - matrix[0, 1]) * s;
+        }
+        else
+        {
+            if (matrix[0, 0] >= matrix[1, 1] && matrix[0, 0] >= matrix[2, 2])
+            {
+                double s = Math.Sqrt(1.0 + matrix[0, 0] - matrix[1, 1] - matrix[2, 2]);
+                double invS = 0.5 / s;
+                q.X = 0.5 * s;
+                q.Y = (matrix[1, 0] + matrix[0, 1]) * invS;
+                q.Z = (matrix[2, 0] + matrix[0, 2]) * invS;
+                q.W = (matrix[2, 1] - matrix[1, 2]) * invS;
+            }
+            else if (matrix[1, 1] > matrix[2, 2])
+            {
+                double s = Math.Sqrt(1.0 + matrix[1, 1] - matrix[0, 0] - matrix[2, 2]);
+                double invS = 0.5 / s;
+                q.X = (matrix[0, 1] + matrix[1, 0]) * invS;
+                q.Y = 0.5 * s;
+                q.Z = (matrix[1, 2] + matrix[2, 1]) * invS;
+                q.W = (matrix[0, 2] - matrix[2, 0]) * invS;
+            }
+            else
+            {
+                double s = Math.Sqrt(1.0 + matrix[2, 2] - matrix[0, 0] - matrix[1, 1]);
+                double invS = 0.5 / s;
+                q.X = (matrix[0, 2] + matrix[2, 0]) * invS;
+                q.Y = (matrix[1, 2] + matrix[2, 1]) * invS;
+                q.Z = 0.5 * s;
+                q.W = (matrix[1, 0] - matrix[0, 1]) * invS;
+            }
+        }
+
+        return q;
+    }
+
+    #endregion
+
+    #region Operators
+
+    /// <summary>
+    /// Adds two quaternions component-wise (rarely used - prefer multiplication for combining rotations)
+    /// </summary>
+    [MethodImpl(MethodImplOptions.AggressiveInlining)]
+    public static Quaternion operator +(Quaternion value1, Quaternion value2)
+    {
+        return new Quaternion(
+            value1.X + value2.X,
+            value1.Y + value2.Y,
+            value1.Z + value2.Z,
+            value1.W + value2.W
+        );
+    }
+
+    /// <summary>
+    /// Subtracts two quaternions component-wise (rarely used)
+    /// </summary>
+    [MethodImpl(MethodImplOptions.AggressiveInlining)]
+    public static Quaternion operator -(Quaternion value1, Quaternion value2)
+    {
+        return new Quaternion(
+            value1.X - value2.X,
+            value1.Y - value2.Y,
+            value1.Z - value2.Z,
+            value1.W - value2.W
+        );
+    }
+
+    /// <summary>
+    /// Negates a quaternion
+    /// </summary>
+    [MethodImpl(MethodImplOptions.AggressiveInlining)]
+    public static Quaternion operator -(Quaternion value)
+    {
+        return new Quaternion(-value.X, -value.Y, -value.Z, -value.W);
+    }
+
+    /// <summary>
+    /// Multiplies two quaternions (combines rotations: first apply value2, then value1)
+    /// </summary>
+    [MethodImpl(MethodImplOptions.AggressiveInlining)]
+    public static Quaternion operator *(Quaternion value1, Quaternion value2)
+    {
+        double q1x = value1.X;
+        double q1y = value1.Y;
+        double q1z = value1.Z;
+        double q1w = value1.W;
+
+        double q2x = value2.X;
+        double q2y = value2.Y;
+        double q2z = value2.Z;
+        double q2w = value2.W;
+
+        // Cross product
+        double cx = q1y * q2z - q1z * q2y;
+        double cy = q1z * q2x - q1x * q2z;
+        double cz = q1x * q2y - q1y * q2x;
+
+        double dot = q1x * q2x + q1y * q2y + q1z * q2z;
+
+        return new Quaternion(
+            q1x * q2w + q2x * q1w + cx,
+            q1y * q2w + q2y * q1w + cy,
+            q1z * q2w + q2z * q1w + cz,
+            q1w * q2w - dot
+        );
+    }
+
+    /// <summary>
+    /// Multiplies a quaternion by a scalar
+    /// </summary>
+    [MethodImpl(MethodImplOptions.AggressiveInlining)]
+    public static Quaternion operator *(Quaternion value1, double value2)
+    {
+        return new Quaternion(
+            value1.X * value2,
+            value1.Y * value2,
+            value1.Z * value2,
+            value1.W * value2
+        );
+    }
+
+    /// <summary>
+    /// Divides a quaternion by a scalar
+    /// </summary>
+    [MethodImpl(MethodImplOptions.AggressiveInlining)]
+    public static Quaternion operator /(Quaternion value1, double value2)
+    {
+        double invValue = 1.0 / value2;
+        return new Quaternion(
+            value1.X * invValue,
+            value1.Y * invValue,
+            value1.Z * invValue,
+            value1.W * invValue
+        );
+    }
+
+    /// <summary>
+    /// Checks if two quaternions are equal
+    /// </summary>
+    [MethodImpl(MethodImplOptions.AggressiveInlining)]
+    public static bool operator ==(Quaternion value1, Quaternion value2)
+    {
+        return value1.X == value2.X &&
+               value1.Y == value2.Y &&
+               value1.Z == value2.Z &&
+               value1.W == value2.W;
+    }
+
+    /// <summary>
+    /// Checks if two quaternions are not equal
+    /// </summary>
+    [MethodImpl(MethodImplOptions.AggressiveInlining)]
+    public static bool operator !=(Quaternion value1, Quaternion value2)
+    {
+        return value1.X != value2.X ||
+               value1.Y != value2.Y ||
+               value1.Z != value2.Z ||
+               value1.W != value2.W;
+    }
+
+    /// <summary>
+    /// Implicit conversion from System.Numerics.Quaternion
+    /// </summary>
+    [MethodImpl(MethodImplOptions.AggressiveInlining)]
+    public static implicit operator Quaternion(System.Numerics.Quaternion value)
+    {
+        return new Quaternion(value.X, value.Y, value.Z, value.W);
+    }
+
+    /// <summary>
+    /// Implicit conversion to System.Numerics.Quaternion
+    /// </summary>
+    [MethodImpl(MethodImplOptions.AggressiveInlining)]
+    public static implicit operator System.Numerics.Quaternion(Quaternion value)
+    {
+        return new Quaternion((float)value.X, (float)value.Y, (float)value.Z, (float)value.W);
+    }
+
+    #endregion
+
+    #region Equality
+
+    /// <summary>
+    /// Checks if this quaternion equals another quaternion
+    /// </summary>
+    public readonly bool Equals(Quaternion other)
+    {
+        return X == other.X && Y == other.Y && Z == other.Z && W == other.W;
+    }
+
+    /// <summary>
+    /// Checks if this quaternion equals an object
+    /// </summary>
+    public override readonly bool Equals(object? obj)
+    {
+        return obj is Quaternion quaternion && Equals(quaternion);
+    }
+
+    /// <summary>
+    /// Gets the hash code for this quaternion
+    /// </summary>
+    public override readonly int GetHashCode()
+    {
+        return HashCode.Combine(X, Y, Z, W);
+    }
+
+    #endregion
+
+    #region String
+
+    /// <summary>
+    /// Returns a string representation of this quaternion
+    /// </summary>
+    public override readonly string ToString()
+    {
+        return $"{{X:{X} Y:{Y} Z:{Z} W:{W}}}";
+    }
+
+    /// <summary>
+    /// Returns a formatted string representation of this quaternion
+    /// </summary>
+    public readonly string ToString(string format)
+    {
+        return $"{{X:{X.ToString(format)} Y:{Y.ToString(format)} Z:{Z.ToString(format)} W:{W.ToString(format)}}}";
+    }
+
+    #endregion
+}