diff --git a/LinearAlgebra/Matrix.cs b/LinearAlgebra/Matrix.cs index c2e6334..b5f7e9d 100644 --- a/LinearAlgebra/Matrix.cs +++ b/LinearAlgebra/Matrix.cs @@ -4,8 +4,8 @@ using Vector3 = UnityEngine.Vector3; using Vector2 = UnityEngine.Vector2; public readonly struct Slice { - public int start {get;} - public int stop {get;} + public int start { get; } + public int stop { get; } public Slice(int start, int stop) { this.start = start; this.stop = stop; @@ -25,6 +25,15 @@ public class Matrix2 { this.data = data; } + public Matrix2 Clone() { + float[,] data = new float[this.nRows, nCols]; + for (int rowIx = 0; rowIx < this.nRows; rowIx++) { + for (int colIx = 0; colIx < this.nCols; colIx++) + data[rowIx, colIx] = this.data[rowIx, colIx]; + } + return new Matrix2(data); + } + public static Matrix2 Zero(uint nRows, uint nCols) { return new Matrix2(nRows, nCols); } @@ -59,6 +68,30 @@ public class Matrix2 { return new Matrix2(result); } + public Vector3 GetRow3(int rowIx) { + uint cols = this.nCols; + Vector3 row = new() { + x = this.data[rowIx, 0], + y = this.data[rowIx, 1], + z = this.data[rowIx, 2] + }; + return row; + } + public void SetRow3(int rowIx, Vector3 v) { + this.data[rowIx, 0] = v.x; + this.data[rowIx, 1] = v.y; + this.data[rowIx, 2] = v.z; + } + + public Matrix1 GetColumn(int colIx) { + float[] column = new float[this.nRows]; + for (int i = 0; i < this.nRows; i++) { + column[i] = this.data[i, colIx]; + } + return new Matrix1(column); + } + + public Matrix2 Transpose() { float[,] resultData = new float[this.nCols, this.nRows]; for (uint rowIx = 0; rowIx < this.nRows; rowIx++) { @@ -145,7 +178,6 @@ public class Matrix2 { }; } - public static Matrix2 operator *(Matrix2 A, float s) { float[,] result = new float[A.nRows, A.nCols]; @@ -244,6 +276,48 @@ public class Matrix2 { return new Matrix2(inverse); } + + public float Determinant() { + uint n = this.nRows; + if (n != this.nCols) + throw new System.ArgumentException("Matrix must be square."); + + if (n == 1) + return this.data[0, 0]; // Base case for 1x1 matrix + + if (n == 2) // Base case for 2x2 matrix + return this.data[0, 0] * this.data[1, 1] - this.data[0, 1] * this.data[1, 0]; + + float det = 0; + for (int col = 0; col < n; col++) + det += (col % 2 == 0 ? 1 : -1) * this.data[0, col] * this.Minor(0, col).Determinant(); + + return det; + } + + // Helper function to compute the minor of a matrix + private Matrix2 Minor(int rowToRemove, int colToRemove) { + uint n = this.nRows; + 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] = this.data[i, j]; + c++; + } + r++; + } + + return new Matrix2(minor); + } + + } public class Matrix1 { @@ -343,180 +417,4 @@ public class Matrix1 { for (int ix = slice.start; ix < slice.stop; ix++, vIx++) this.data[ix] = v.data[vIx]; } -} - -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; - } - } \ No newline at end of file diff --git a/LinearAlgebra/Quaternion.cs b/LinearAlgebra/Quaternion.cs index 55ad014..9a9fed7 100644 --- a/LinearAlgebra/Quaternion.cs +++ b/LinearAlgebra/Quaternion.cs @@ -13,15 +13,16 @@ namespace Passer.LinearAlgebra { this.w = w; } - public static float[,] ToRotationMatrix(Quaternion q) { + public static Matrix2 ToRotationMatrix(Quaternion q) { float w = q.x, x = q.y, y = q.z, z = q.w; - return new float[,] + float[,] result = new float[,] { { 1 - 2 * (y * y + z * z), 2 * (x * y - w * z), 2 * (x * z + w * y) }, { 2 * (x * y + w * z), 1 - 2 * (x * x + z * z), 2 * (y * z - w * x) }, { 2 * (x * z - w * y), 2 * (y * z + w * x), 1 - 2 * (x * x + y * y) } }; + return new Matrix2(result); } }