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

using System.Runtime.CompilerServices;

namespace RobotNet10.Shared.Numbers;

/// <summary>
/// Custom 3x2 matrix struct for 2D affine transformations.
/// Replacement for System.Numerics.Matrix3x2 which doesn't serialize properly.
/// </summary>
public struct Matrix3x2 : IEquatable<Matrix3x2>
{
    /// <summary>
    /// Value at row 1, column 1
    /// </summary>
    public double M11 { get; set; }

    /// <summary>
    /// Value at row 1, column 2
    /// </summary>
    public double M12 { get; set; }

    /// <summary>
    /// Value at row 2, column 1
    /// </summary>
    public double M21 { get; set; }

    /// <summary>
    /// Value at row 2, column 2
    /// </summary>
    public double M22 { get; set; }

    /// <summary>
    /// Value at row 3, column 1 (translation X)
    /// </summary>
    public double M31 { get; set; }

    /// <summary>
    /// Value at row 3, column 2 (translation Y)
    /// </summary>
    public double M32 { get; set; }

    /// <summary>
    /// Creates a new Matrix3x2
    /// </summary>
    public Matrix3x2(double m11, double m12, double m21, double m22, double m31, double m32)
    {
        M11 = m11;
        M12 = m12;
        M21 = m21;
        M22 = m22;
        M31 = m31;
        M32 = m32;
    }

    #region Static Properties

    /// <summary>
    /// Returns the identity matrix
    /// </summary>
    public static Matrix3x2 Identity => new(1, 0, 0, 1, 0, 0);

    #endregion

    #region Static Methods

    /// <summary>
    /// Creates a rotation matrix
    /// </summary>
    [MethodImpl(MethodImplOptions.AggressiveInlining)]
    public static Matrix3x2 CreateRotation(double radians)
    {
        double cos = Math.Cos(radians);
        double sin = Math.Sin(radians);
        return new Matrix3x2(cos, sin, -sin, cos, 0, 0);
    }

    /// <summary>
    /// Creates a translation matrix
    /// </summary>
    [MethodImpl(MethodImplOptions.AggressiveInlining)]
    public static Matrix3x2 CreateTranslation(double x, double y)
    {
        return new Matrix3x2(1, 0, 0, 1, x, y);
    }

    /// <summary>
    /// Creates a translation matrix from a Vector2
    /// </summary>
    [MethodImpl(MethodImplOptions.AggressiveInlining)]
    public static Matrix3x2 CreateTranslation(Vector2 position)
    {
        return new Matrix3x2(1, 0, 0, 1, position.X, position.Y);
    }

    /// <summary>
    /// Creates a scale matrix
    /// </summary>
    [MethodImpl(MethodImplOptions.AggressiveInlining)]
    public static Matrix3x2 CreateScale(double scale)
    {
        return new Matrix3x2(scale, 0, 0, scale, 0, 0);
    }

    /// <summary>
    /// Creates a scale matrix
    /// </summary>
    [MethodImpl(MethodImplOptions.AggressiveInlining)]
    public static Matrix3x2 CreateScale(double scaleX, double scaleY)
    {
        return new Matrix3x2(scaleX, 0, 0, scaleY, 0, 0);
    }

    /// <summary>
    /// Multiplies two matrices
    /// </summary>
    [MethodImpl(MethodImplOptions.AggressiveInlining)]
    public static Matrix3x2 Multiply(Matrix3x2 value1, Matrix3x2 value2)
    {
        return value1 * value2;
    }

    #endregion

    #region Operators

    /// <summary>
    /// Multiplies two matrices
    /// </summary>
    [MethodImpl(MethodImplOptions.AggressiveInlining)]
    public static Matrix3x2 operator *(Matrix3x2 value1, Matrix3x2 value2)
    {
        return new Matrix3x2(
            value1.M11 * value2.M11 + value1.M12 * value2.M21,
            value1.M11 * value2.M12 + value1.M12 * value2.M22,
            value1.M21 * value2.M11 + value1.M22 * value2.M21,
            value1.M21 * value2.M12 + value1.M22 * value2.M22,
            value1.M31 * value2.M11 + value1.M32 * value2.M21 + value2.M31,
            value1.M31 * value2.M12 + value1.M32 * value2.M22 + value2.M32
        );
    }

    /// <summary>
    /// Checks if two matrices are equal
    /// </summary>
    [MethodImpl(MethodImplOptions.AggressiveInlining)]
    public static bool operator ==(Matrix3x2 left, Matrix3x2 right)
    {
        return left.M11 == right.M11 && left.M12 == right.M12 &&
               left.M21 == right.M21 && left.M22 == right.M22 &&
               left.M31 == right.M31 && left.M32 == right.M32;
    }

    /// <summary>
    /// Checks if two matrices are not equal
    /// </summary>
    [MethodImpl(MethodImplOptions.AggressiveInlining)]
    public static bool operator !=(Matrix3x2 left, Matrix3x2 right)
    {
        return !(left == right);
    }

    #endregion

    #region Equality

    /// <summary>
    /// Checks if this matrix equals another matrix
    /// </summary>
    public readonly bool Equals(Matrix3x2 other)
    {
        return M11 == other.M11 && M12 == other.M12 &&
               M21 == other.M21 && M22 == other.M22 &&
               M31 == other.M31 && M32 == other.M32;
    }

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

    /// <summary>
    /// Gets the hash code for this matrix
    /// </summary>
    public override readonly int GetHashCode()
    {
        return HashCode.Combine(M11, M12, M21, M22, M31, M32);
    }

    #endregion

    #region String

    /// <summary>
    /// Returns a string representation of this matrix
    /// </summary>
    public override readonly string ToString()
    {
        return $"{{ {{M11:{M11} M12:{M12}}} {{M21:{M21} M22:{M22}}} {{M31:{M31} M32:{M32}}} }}";
    }

    #endregion
}