Initial commit

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

@@ -0,0 +1,452 @@
+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
+}