BQP/srcs/RobotNet10/Shared/RobotNet10.Shared/Numbers/Vector3.cs

using System.Runtime.CompilerServices;

namespace RobotNet10.Shared.Numbers;

/// <summary>
/// Custom 3D vector struct that supports JSON serialization.
/// Replacement for System.Numerics.Vector3 which doesn't serialize properly.
/// </summary>
public struct Vector3 : IEquatable<Vector3>
{
    /// <summary>
    /// X component
    /// </summary>
    public double X { get; set; }

    /// <summary>
    /// Y component
    /// </summary>
    public double Y { get; set; }

    /// <summary>
    /// Z component
    /// </summary>
    public double Z { get; set; }

    /// <summary>
    /// Creates a new Vector3
    /// </summary>
    public Vector3(double x, double y, double z)
    {
        X = x;
        Y = y;
        Z = z;
    }

    /// <summary>
    /// Creates a Vector3 with all components set to the same value
    /// </summary>
    public Vector3(double value)
    {
        X = Y = Z = value;
    }

    #region Static Properties

    /// <summary>
    /// Returns a Vector3 with all components set to zero
    /// </summary>
    public static Vector3 Zero => new(0, 0, 0);

    /// <summary>
    /// Returns a Vector3 with all components set to one
    /// </summary>
    public static Vector3 One => new(1, 1, 1);

    /// <summary>
    /// Returns the unit vector for the X axis (1, 0, 0)
    /// </summary>
    public static Vector3 UnitX => new(1, 0, 0);

    /// <summary>
    /// Returns the unit vector for the Y axis (0, 1, 0)
    /// </summary>
    public static Vector3 UnitY => new(0, 1, 0);

    /// <summary>
    /// Returns the unit vector for the Z axis (0, 0, 1)
    /// </summary>
    public static Vector3 UnitZ => new(0, 0, 1);

    #endregion

    #region Properties

    /// <summary>
    /// Returns the length (magnitude) of the vector
    /// </summary>
    public readonly double Length()
    {
        return Math.Sqrt(X * X + Y * Y + Z * Z);
    }

    /// <summary>
    /// Returns the squared length of the vector (faster than Length)
    /// </summary>
    public readonly double LengthSquared()
    {
        return X * X + Y * Y + Z * Z;
    }

    #endregion

    #region Methods

    /// <summary>
    /// Returns a normalized copy of this vector (unit length)
    /// </summary>
    public readonly Vector3 Normalize()
    {
        double length = Length();
        if (length < double.Epsilon)
            return Zero;

        double invLength = 1.0 / length;
        return new Vector3(X * invLength, Y * invLength, Z * invLength);
    }

    /// <summary>
    /// Normalizes this vector in place
    /// </summary>
    public void NormalizeInPlace()
    {
        double length = Length();
        if (length < double.Epsilon)
        {
            X = Y = Z = 0;
            return;
        }

        double invLength = 1.0 / length;
        X *= invLength;
        Y *= invLength;
        Z *= invLength;
    }

    #endregion

    #region Static Methods

    /// <summary>
    /// Calculates the dot product of two vectors
    /// </summary>
    [MethodImpl(MethodImplOptions.AggressiveInlining)]
    public static double Dot(Vector3 left, Vector3 right)
    {
        return left.X * right.X + left.Y * right.Y + left.Z * right.Z;
    }

    /// <summary>
    /// Calculates the cross product of two vectors
    /// </summary>
    [MethodImpl(MethodImplOptions.AggressiveInlining)]
    public static Vector3 Cross(Vector3 left, Vector3 right)
    {
        return new Vector3(
            left.Y * right.Z - left.Z * right.Y,
            left.Z * right.X - left.X * right.Z,
            left.X * right.Y - left.Y * right.X
        );
    }

    /// <summary>
    /// Returns a normalized copy of a vector (static version)
    /// </summary>
    [MethodImpl(MethodImplOptions.AggressiveInlining)]
    public static Vector3 Normalize(Vector3 value)
    {
        return value.Normalize();
    }

    /// <summary>
    /// Returns the distance between two vectors
    /// </summary>
    [MethodImpl(MethodImplOptions.AggressiveInlining)]
    public static double Distance(Vector3 value1, Vector3 value2)
    {
        double dx = value1.X - value2.X;
        double dy = value1.Y - value2.Y;
        double dz = value1.Z - value2.Z;
        return Math.Sqrt(dx * dx + dy * dy + dz * dz);
    }

    /// <summary>
    /// Returns the squared distance between two vectors (faster than Distance)
    /// </summary>
    [MethodImpl(MethodImplOptions.AggressiveInlining)]
    public static double DistanceSquared(Vector3 value1, Vector3 value2)
    {
        double dx = value1.X - value2.X;
        double dy = value1.Y - value2.Y;
        double dz = value1.Z - value2.Z;
        return dx * dx + dy * dy + dz * dz;
    }

    /// <summary>
    /// Performs linear interpolation between two vectors
    /// </summary>
    [MethodImpl(MethodImplOptions.AggressiveInlining)]
    public static Vector3 Lerp(Vector3 value1, Vector3 value2, double amount)
    {
        return new Vector3(
            value1.X + (value2.X - value1.X) * amount,
            value1.Y + (value2.Y - value1.Y) * amount,
            value1.Z + (value2.Z - value1.Z) * amount
        );
    }

    /// <summary>
    /// Returns a vector with the minimum components of two vectors
    /// </summary>
    [MethodImpl(MethodImplOptions.AggressiveInlining)]
    public static Vector3 Min(Vector3 value1, Vector3 value2)
    {
        return new Vector3(
            Math.Min(value1.X, value2.X),
            Math.Min(value1.Y, value2.Y),
            Math.Min(value1.Z, value2.Z)
        );
    }

    /// <summary>
    /// Returns a vector with the maximum components of two vectors
    /// </summary>
    [MethodImpl(MethodImplOptions.AggressiveInlining)]
    public static Vector3 Max(Vector3 value1, Vector3 value2)
    {
        return new Vector3(
            Math.Max(value1.X, value2.X),
            Math.Max(value1.Y, value2.Y),
            Math.Max(value1.Z, value2.Z)
        );
    }

    /// <summary>
    /// Returns a vector whose components are the absolute values of the input vector
    /// </summary>
    [MethodImpl(MethodImplOptions.AggressiveInlining)]
    public static Vector3 Abs(Vector3 value)
    {
        return new Vector3(
            Math.Abs(value.X),
            Math.Abs(value.Y),
            Math.Abs(value.Z)
        );
    }

    /// <summary>
    /// Clamps a vector to the specified minimum and maximum values
    /// </summary>
    [MethodImpl(MethodImplOptions.AggressiveInlining)]
    public static Vector3 Clamp(Vector3 value, Vector3 min, Vector3 max)
    {
        return new Vector3(
            Math.Clamp(value.X, min.X, max.X),
            Math.Clamp(value.Y, min.Y, max.Y),
            Math.Clamp(value.Z, min.Z, max.Z)
        );
    }

    /// <summary>
    /// Reflects a vector off a surface with the specified normal
    /// </summary>
    [MethodImpl(MethodImplOptions.AggressiveInlining)]
    public static Vector3 Reflect(Vector3 vector, Vector3 normal)
    {
        double dot = Dot(vector, normal);
        return vector - 2.0 * dot * normal;
    }

    /// <summary>
    /// Transforms a Vector3 by a Quaternion rotation
    /// </summary>
    [MethodImpl(MethodImplOptions.AggressiveInlining)]
    public static Vector3 Transform(Vector3 value, Quaternion rotation)
    {
        // This is the formula: q * v * q^-1 where v is treated as a quaternion with w=0
        // Optimized version without creating intermediate quaternions

        double x2 = rotation.X + rotation.X;
        double y2 = rotation.Y + rotation.Y;
        double z2 = rotation.Z + rotation.Z;

        double wx2 = rotation.W * x2;
        double wy2 = rotation.W * y2;
        double wz2 = rotation.W * z2;
        double xx2 = rotation.X * x2;
        double xy2 = rotation.X * y2;
        double xz2 = rotation.X * z2;
        double yy2 = rotation.Y * y2;
        double yz2 = rotation.Y * z2;
        double zz2 = rotation.Z * z2;

        return new Vector3(
            value.X * (1.0 - yy2 - zz2) + value.Y * (xy2 - wz2) + value.Z * (xz2 + wy2),
            value.X * (xy2 + wz2) + value.Y * (1.0 - xx2 - zz2) + value.Z * (yz2 - wx2),
            value.X * (xz2 - wy2) + value.Y * (yz2 + wx2) + value.Z * (1.0 - xx2 - yy2)
        );
    }

    #endregion

    #region Operators

    /// <summary>
    /// Adds two vectors
    /// </summary>
    [MethodImpl(MethodImplOptions.AggressiveInlining)]
    public static Vector3 operator +(Vector3 left, Vector3 right)
    {
        return new Vector3(left.X + right.X, left.Y + right.Y, left.Z + right.Z);
    }

    /// <summary>
    /// Subtracts two vectors
    /// </summary>
    [MethodImpl(MethodImplOptions.AggressiveInlining)]
    public static Vector3 operator -(Vector3 left, Vector3 right)
    {
        return new Vector3(left.X - right.X, left.Y - right.Y, left.Z - right.Z);
    }

    /// <summary>
    /// Negates a vector
    /// </summary>
    [MethodImpl(MethodImplOptions.AggressiveInlining)]
    public static Vector3 operator -(Vector3 value)
    {
        return new Vector3(-value.X, -value.Y, -value.Z);
    }

    /// <summary>
    /// Multiplies two vectors component-wise
    /// </summary>
    [MethodImpl(MethodImplOptions.AggressiveInlining)]
    public static Vector3 operator *(Vector3 left, Vector3 right)
    {
        return new Vector3(left.X * right.X, left.Y * right.Y, left.Z * right.Z);
    }

    /// <summary>
    /// Multiplies a vector by a scalar
    /// </summary>
    [MethodImpl(MethodImplOptions.AggressiveInlining)]
    public static Vector3 operator *(Vector3 left, double right)
    {
        return new Vector3(left.X * right, left.Y * right, left.Z * right);
    }

    /// <summary>
    /// Multiplies a scalar by a vector
    /// </summary>
    [MethodImpl(MethodImplOptions.AggressiveInlining)]
    public static Vector3 operator *(double left, Vector3 right)
    {
        return new Vector3(left * right.X, left * right.Y, left * right.Z);
    }

    /// <summary>
    /// Divides two vectors component-wise
    /// </summary>
    [MethodImpl(MethodImplOptions.AggressiveInlining)]
    public static Vector3 operator /(Vector3 left, Vector3 right)
    {
        return new Vector3(left.X / right.X, left.Y / right.Y, left.Z / right.Z);
    }

    /// <summary>
    /// Divides a vector by a scalar
    /// </summary>
    [MethodImpl(MethodImplOptions.AggressiveInlining)]
    public static Vector3 operator /(Vector3 left, double right)
    {
        double invRight = 1.0 / right;
        return new Vector3(left.X * invRight, left.Y * invRight, left.Z * invRight);
    }

    /// <summary>
    /// Checks if two vectors are equal
    /// </summary>
    [MethodImpl(MethodImplOptions.AggressiveInlining)]
    public static bool operator ==(Vector3 left, Vector3 right)
    {
        return left.X == right.X && left.Y == right.Y && left.Z == right.Z;
    }

    /// <summary>
    /// Checks if two vectors are not equal
    /// </summary>
    [MethodImpl(MethodImplOptions.AggressiveInlining)]
    public static bool operator !=(Vector3 left, Vector3 right)
    {
        return left.X != right.X || left.Y != right.Y || left.Z != right.Z;
    }

    /// <summary>
    /// Implicit conversion from System.Numerics.Vector3
    /// </summary>
    [MethodImpl(MethodImplOptions.AggressiveInlining)]
    public static implicit operator Vector3(System.Numerics.Vector3 value)
    {
        return new Vector3(value.X, value.Y, value.Z);
    }

    /// <summary>
    /// Implicit conversion to System.Numerics.Vector3
    /// </summary>
    [MethodImpl(MethodImplOptions.AggressiveInlining)]
    public static implicit operator System.Numerics.Vector3(Vector3 value)
    {
        return new Vector3((float)value.X, (float)value.Y, (float)value.Z);
    }

    #endregion

    #region Equality

    /// <summary>
    /// Checks if this vector equals another vector
    /// </summary>
    public readonly bool Equals(Vector3 other)
    {
        return X == other.X && Y == other.Y && Z == other.Z;
    }

    /// <summary>
    /// Checks if this vector equals an object
    /// </summary>
    public override readonly bool Equals(object? obj)
    {
        return obj is Vector3 vector && Equals(vector);
    }

    /// <summary>
    /// Gets the hash code for this vector
    /// </summary>
    public override readonly int GetHashCode()
    {
        return HashCode.Combine(X, Y, Z);
    }

    #endregion

    #region String

    /// <summary>
    /// Returns a string representation of this vector
    /// </summary>
    public override readonly string ToString()
    {
        return $"<{X}, {Y}, {Z}>";
    }

    /// <summary>
    /// Returns a formatted string representation of this vector
    /// </summary>
    public readonly string ToString(string format)
    {
        return $"<{X.ToString(format)}, {Y.ToString(format)}, {Z.ToString(format)}>";
    }

    #endregion
}