RoboidControl/RoboidControl-csharp
RoboidControl/RoboidControl-csharp
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Residual & Jacobian progress

Browse Source
This commit is contained in:
Pascal Serrarens 2025-02-14 11:07:30 +01:00
parent 8ea28beb42
commit dd23355554
1 changed files with 15 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

23
LinearAlgebra/Matrix.cs
View File

@ -5,14 +5,21 @@ using Vector3 = UnityEngine.Vector3;
public class Matrix2 { public class Matrix2 {
public float[,] data { get; } public float[,] data { get; }
public int nRows => data.GetLength(0); public uint nRows => (uint)data.GetLength(0);
public int nCols => data.GetLength(1); public uint nCols => (uint)data.GetLength(1);
public Matrix2(uint nRows, uint nCols) {
this.data = new float[nRows, nCols];
}
public Matrix2(float[,] data) { public Matrix2(float[,] data) {
this.data = data; this.data = data;
} }
public static Matrix2 Identity(int size) { public static Matrix2 Zero(uint nRows, uint nCols) {
return new Matrix2(nRows, nCols);
}
public static Matrix2 Identity(uint size) {
return Diagonal(1, size); return Diagonal(1, size);
// float[,] resultData = new float[size, size]; // float[,] resultData = new float[size, size];
// for (int i = 0; i < size; i++) // for (int i = 0; i < size; i++)
@ -26,7 +33,7 @@ public class Matrix2 {
resultData[ix, ix] = v.data[ix]; resultData[ix, ix] = v.data[ix];
return new Matrix2(resultData); return new Matrix2(resultData);
} }
public static Matrix2 Diagonal(float f, int size) { public static Matrix2 Diagonal(float f, uint size) {
float[,] resultData = new float[size, size]; float[,] resultData = new float[size, size];
for (int ix = 0; ix < size; ix++) for (int ix = 0; ix < size; ix++)
resultData[ix, ix] = f; resultData[ix, ix] = f;
@ -137,7 +144,7 @@ public class Matrix2 {
public Matrix2 Inverse() { public Matrix2 Inverse() {
Matrix2 A = this; Matrix2 A = this;
// unchecked // unchecked
int n = A.nRows; uint n = A.nRows;
// Create an identity matrix of the same size as the original matrix // Create an identity matrix of the same size as the original matrix
float[,] augmentedMatrix = new float[n, 2 * n]; float[,] augmentedMatrix = new float[n, 2 * n];
@ -168,9 +175,9 @@ public class Matrix2 {
} }
// Back substitution // Back substitution
for (int i = n - 1; i >= 0; i--) { for (uint i = n - 1; i >= 0; i--) {
// Eliminate the column above the pivot // Eliminate the column above the pivot
for (int j = i - 1; j >= 0; j--) { for (uint j = i - 1; j >= 0; j--) {
float factor = augmentedMatrix[j, i]; float factor = augmentedMatrix[j, i];
for (int k = 0; k < 2 * n; k++) for (int k = 0; k < 2 * n; k++)
augmentedMatrix[j, k] -= factor * augmentedMatrix[i, k]; augmentedMatrix[j, k] -= factor * augmentedMatrix[i, k];
@ -191,7 +198,7 @@ public class Matrix2 {
public class Matrix1 { public class Matrix1 {
public float[] data { get; } public float[] data { get; }
public int magnitude => data.GetLength(0); public uint magnitude => (uint)data.GetLength(0);
public Matrix1(uint magnitude) { public Matrix1(uint magnitude) {
this.data = new float[magnitude]; this.data = new float[magnitude];