RoboidControl-csharp/LinearAlgebra/Matrix.cs

using System.Diagnostics;
using Vector3 = UnityEngine.Vector3;

public class Matrix2 {
    public float[,] data { get; }

    public int nRows => data.GetLength(0);
    public int nCols => data.GetLength(1);

    public Matrix2(float[,] data) {
        this.data = data;
    }

    public static Matrix2 Identity(int size) {
        float[,] I = new float[size, size];
        for (int i = 0; i < size; i++) I[i, i] = 1.0f;
        return new Matrix2(I);
    }

    public Matrix2 Transpose() {
        float[,] r = new float[this.nCols, this.nRows];
        for (uint rowIx = 0; rowIx < this.nRows; rowIx++) {
            for (uint colIx = 0; colIx < this.nCols; colIx++)
                r[colIx, rowIx] = this.data[rowIx, colIx];
        }
        return new Matrix2(r);
        // double checked code
    }

    public static Matrix2 operator +(Matrix2 A, Matrix2 B) {
        if (A.nRows != B.nRows || A.nCols != B.nCols)
            throw new System.ArgumentException("Size of A must match size of B.");

        float[,] result = new float[A.nRows, B.nCols];

        for (int i = 0; i < A.nRows; i++) {
            for (int j = 0; j < A.nCols; j++) {
                result[i, j] = A.data[i, j] + B.data[i, j];
            }
        }
        return new Matrix2(result);
    }

    public static Matrix2 operator *(Matrix2 A, Matrix2 B) {
        if (A.nCols != B.nRows)
            throw new System.ArgumentException("Number of columns in A must match number of rows in B.");

        float[,] result = new float[A.nRows, B.nCols];

        for (int i = 0; i < A.nRows; i++) {
            for (int j = 0; j < B.nCols; j++) {
                float sum = 0.0f;
                for (int k = 0; k < A.nCols; k++)
                    sum += A.data[i, k] * B.data[k, j];

                result[i, j] = sum;
            }
        }

        return new Matrix2(result);
        // double checked code
    }

    public static Matrix1 operator *(Matrix2 A, Matrix1 v) {
        // int rows = A.GetLength(0);
        // int cols = A.GetLength(1);
        float[] result = new float[A.nRows];

        for (int i = 0; i < A.nRows; i++) {
            for (int j = 0; j < A.nCols; j++) {
                result[i] += A.data[i, j] * v.data[j];
            }
        }

        return new Matrix1(result);
    }


    public static Matrix2 operator *(Matrix2 A, float s) {
        float[,] result = new float[A.nRows, A.nCols];

        for (int i = 0; i < A.nRows; i++) {
            for (int j = 0; j < A.nCols; j++)
                result[i, j] += A.data[i, j] * s;
        }

        return new Matrix2(result);
    }
    public static Matrix2 operator *(float scalar, Matrix2 A) {
        return A * scalar;
    }

}

public class Matrix1 {
    public float[] data { get; }

    public int size => data.GetLength(0);

    public Matrix1(float[] data) {
        this.data = data;
    }

    public Matrix2 Transpose() {
        float[,] r = new float[1, this.size];
        for (uint colIx = 0; colIx < this.size; colIx++)
            r[1, colIx] = this.data[colIx];

        return new Matrix2(r);
    }

    public static float Dot(Matrix1 a, Matrix1 b) {
        if (a.size != b.size)
            throw new System.ArgumentException("Vectors must be of the same length.");

        float result = 0.0f;
        for (int i = 0; i < a.size; i++) {
            result += a.data[i] * b.data[i];
        }
        return result;
    }

}

public class Matrix {
    private readonly uint rows = 0;
    private readonly uint cols = 0;
    private float[] data;

    public Matrix(uint rows, uint cols) {
        this.rows = rows;
        this.cols = cols;
    }

    public static float[,] Diagonal(float[] v) {
        float[,] r = new float[v.Length, v.Length];
        for (int i = 0; i < v.Length; i++) {
            r[i, i] = v[i];
        }
        return r;
    }

    public static float[,] Transpose(float[,] m) {
        int rows = m.GetLength(0);
        int cols = m.GetLength(1);
        float[,] r = new float[cols, rows];
        for (uint rowIx = 0; rowIx < rows; rowIx++) {
            for (uint colIx = 0; colIx < cols; colIx++)
                r[colIx, rowIx] = m[rowIx, colIx];
        }
        return r;
        // double checked code
    }

    public static void NegateColumn(float[,] m, uint colIx) {
        for (uint rowIx = 0; rowIx < m.GetLength(0); rowIx++) {
            m[rowIx, colIx] = -m[rowIx, colIx];
        }
    }

    public static float Determinant(float[,] matrix) {
        int n = matrix.GetLength(0);
        if (n != matrix.GetLength(1))
            throw new System.ArgumentException("Matrix must be square.");

        if (n == 1)
            return matrix[0, 0]; // Base case for 1x1 matrix

        if (n == 2) // Base case for 2x2 matrix
            return matrix[0, 0] * matrix[1, 1] - matrix[0, 1] * matrix[1, 0];

        float det = 0;
        for (int col = 0; col < n; col++)
            det += (col % 2 == 0 ? 1 : -1) * matrix[0, col] * Determinant(Minor(matrix, 0, col));

        return det;
    }

    // Helper function to compute the minor of a matrix
    private static float[,] Minor(float[,] matrix, int rowToRemove, int colToRemove) {
        int n = matrix.GetLength(0);
        float[,] minor = new float[n - 1, n - 1];

        int r = 0, c = 0;
        for (int i = 0; i < n; i++) {
            if (i == rowToRemove) continue;

            c = 0;
            for (int j = 0; j < n; j++) {
                if (j == colToRemove) continue;

                minor[r, c] = matrix[i, j];
                c++;
            }
            r++;
        }

        return minor;
    }

    public static float[,] MultiplyMatrices(float[,] A, float[,] B) {
        int rowsA = A.GetLength(0);
        int colsA = A.GetLength(1);
        int rowsB = B.GetLength(0);
        int colsB = B.GetLength(1);

        if (colsA != rowsB)
            throw new System.ArgumentException("Number of columns in A must match number of rows in B.");

        float[,] result = new float[rowsA, colsB];

        for (int i = 0; i < rowsA; i++) {
            for (int j = 0; j < colsB; j++) {
                float sum = 0.0f;
                for (int k = 0; k < colsA; k++)
                    sum += A[i, k] * B[k, j];

                result[i, j] = sum;
            }
        }

        return result;
        // double checked code
    }

    public static float[] MultiplyMatrixVector(float[,] A, float[] v) {
        int rows = A.GetLength(0);
        int cols = A.GetLength(1);
        float[] result = new float[rows];

        for (int i = 0; i < rows; i++) {
            for (int j = 0; j < cols; j++) {
                result[i] += A[i, j] * v[j];
            }
        }

        return result;
    }

    // Vector-matrix multiplication
    public static Vector3 MultiplyMatrixVector3(float[,] A, Vector3 v) {
        return new Vector3() {
            x = A[0, 0] * v.x + A[0, 1] * v.y + A[0, 2] * v.z,
            y = A[1, 0] * v.x + A[1, 1] * v.y + A[1, 2] * v.z,
            z = A[2, 0] * v.x + A[2, 1] * v.y + A[2, 2] * v.z
        };
    }

    public static float[,] MultiplyMatrixScalar(float[,] A, float s) {
        int rows = A.GetLength(0);
        int cols = A.GetLength(1);
        float[,] result = new float[rows, cols];

        for (int i = 0; i < rows; i++) {
            for (int j = 0; j < cols; j++) {
                result[i, j] += A[i, j] * s;
            }
        }

        return result;
    }

    public static float[] GetColumn(float[,] M, int col) {
        int rows = M.GetLength(0);
        float[] column = new float[rows];
        for (int i = 0; i < rows; i++) {
            column[i] = M[i, col];
        }
        return column;
    }

    public static Vector3 GetRow3(float[,] M, int rowIx) {
        int cols = M.GetLength(1);
        Vector3 row = new();
        row.x = M[rowIx, 0];
        row.y = M[rowIx, 1];
        row.z = M[rowIx, 2];
        return row;
    }

    public static void SetRow3(float[,] M, int rowIx, Vector3 v) {
        M[rowIx, 0] = v.x;
        M[rowIx, 1] = v.y;
        M[rowIx, 2] = v.z;
    }

    public static float Dot(float[] a, float[] b) {
        float sum = 0;
        for (int i = 0; i < a.Length; i++) sum += a[i] * b[i];
        return sum;
    }

    public static float[,] IdentityMatrix(int size) {
        float[,] I = new float[size, size];
        for (int i = 0; i < size; i++) I[i, i] = 1.0f;
        return I;
    }

}