Fix all matrix errors
This commit is contained in:
Pascal Serrarens
parent
8dab67f620
commit
8a709e0fd2
@ -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;
|
||||
}
|
||||
|
||||
}
|
||||
@ -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);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user