Initial commit

srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/SLAM/Cartographer/Geometry/Matrix3x3.cs

@@ -0,0 +1,118 @@
+namespace RobotNet10.RobotApp.SLAM.Cartographer.Geometry;
+
+/// <summary>
+/// 3x3 matrix for representing covariance matrices
+/// </summary>
+public readonly struct Matrix3x3
+{
+    private readonly double[,] _data;
+
+    /// <summary>
+    /// Creates a new 3x3 matrix with all zeros
+    /// </summary>
+    public Matrix3x3()
+    {
+        _data = new double[3, 3];
+    }
+
+    /// <summary>
+    /// Creates a new 3x3 matrix from a 2D array
+    /// </summary>
+    /// <param name="data">3x3 array of values</param>
+    /// <exception cref="ArgumentException">Thrown if array is not 3x3</exception>
+    public Matrix3x3(double[,] data)
+    {
+        if (data.GetLength(0) != 3 || data.GetLength(1) != 3)
+        {
+            throw new ArgumentException("Matrix must be 3x3", nameof(data));
+        }
+        _data = new double[3, 3];
+        Array.Copy(data, _data, 9);
+    }
+
+    /// <summary>
+    /// Gets or sets the value at the specified row and column
+    /// </summary>
+    public double this[int row, int col]
+    {
+        get => _data[row, col];
+        set => _data[row, col] = value;
+    }
+
+    /// <summary>
+    /// Gets the determinant of the matrix
+    /// </summary>
+    public double Determinant
+    {
+        get
+        {
+            return _data[0, 0] * (_data[1, 1] * _data[2, 2] - _data[1, 2] * _data[2, 1])
+                 - _data[0, 1] * (_data[1, 0] * _data[2, 2] - _data[1, 2] * _data[2, 0])
+                 + _data[0, 2] * (_data[1, 0] * _data[2, 1] - _data[1, 1] * _data[2, 0]);
+        }
+    }
+
+    /// <summary>
+    /// Creates an identity matrix
+    /// </summary>
+    public static Matrix3x3 Identity()
+    {
+        var matrix = new Matrix3x3();
+        matrix[0, 0] = 1.0;
+        matrix[1, 1] = 1.0;
+        matrix[2, 2] = 1.0;
+        return matrix;
+    }
+
+    /// <summary>
+    /// Creates a covariance matrix for 2D pose (x, y, theta)
+    /// </summary>
+    /// <param name="xx">Variance in x</param>
+    /// <param name="yy">Variance in y</param>
+    /// <param name="tt">Variance in theta</param>
+    /// <param name="xy">Covariance between x and y</param>
+    /// <param name="xt">Covariance between x and theta</param>
+    /// <param name="yt">Covariance between y and theta</param>
+    public static Matrix3x3 Covariance(double xx, double yy, double tt, double xy = 0.0, double xt = 0.0, double yt = 0.0)
+    {
+        var matrix = new Matrix3x3();
+        matrix[0, 0] = xx; // Var(x)
+        matrix[1, 1] = yy; // Var(y)
+        matrix[2, 2] = tt; // Var(theta)
+        matrix[0, 1] = matrix[1, 0] = xy; // Cov(x,y)
+        matrix[0, 2] = matrix[2, 0] = xt; // Cov(x,theta)
+        matrix[1, 2] = matrix[2, 1] = yt; // Cov(y,theta)
+        return matrix;
+    }
+
+    /// <summary>
+    /// Gets all values as a 2D array
+    /// </summary>
+    public double[,] ToArray()
+    {
+        var result = new double[3, 3];
+        Array.Copy(_data, result, 9);
+        return result;
+    }
+
+    /// <summary>
+    /// Gets the matrix as a flattened array (row-major order)
+    /// </summary>
+    public double[] ToFlatArray()
+    {
+        return
+        [
+            _data[0, 0], _data[0, 1], _data[0, 2],
+            _data[1, 0], _data[1, 1], _data[1, 2],
+            _data[2, 0], _data[2, 1], _data[2, 2]
+        ];
+    }
+
+    public override string ToString()
+    {
+        return $"[{_data[0, 0]:F3}, {_data[0, 1]:F3}, {_data[0, 2]:F3}]\n" +
+               $"[{_data[1, 0]:F3}, {_data[1, 1]:F3}, {_data[1, 2]:F3}]\n" +
+               $"[{_data[2, 0]:F3}, {_data[2, 1]:F3}, {_data[2, 2]:F3}]";
+    }
+}
+