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

Stuck a essential matrix decomposition

Browse Source
This commit is contained in:
Pascal Serrarens 2025-01-24 17:40:12 +01:00
parent bed6d95916
commit b9d668926c
3 changed files with 58 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

15
LinearAlgebra/Matrix.cs
View File

@ -90,6 +90,21 @@ public class Matrix {
// double checked code
}
// Vector-matrix multiplication
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;
}
public static float[] GetColumn(float[,] M, int col) {
int rows = M.GetLength(0);
float[] column = new float[rows];

31
LinearAlgebra/Quaternion.cs Normal file
View File

@ -0,0 +1,31 @@
namespace Passer.LinearAlgebra {
public class QuaternionOf<T> {
public T x;
public T y;
public T z;
public T w;
public QuaternionOf(T x, T y, T z, T w) {
this.x = x;
this.y = y;
this.z = z;
this.w = w;
}
public static float[,] ToRotationMatrix(Quaternion q) {
float w = q.x, x = q.y, y = q.z, z = q.w;
return 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) }
};
}
}
public class Quaternion : QuaternionOf<float> {
public Quaternion(float x, float y, float z, float w) : base(x, y, z, w) { }
}
}

14
LinearAlgebra/Vector3.cs
View File

@ -4,8 +4,18 @@ namespace Passer.LinearAlgebra {
public T x;
public T y;
public T z;
public Vector3Of(T x, T y, T z) {
this.x = x;
this.y = y;
this.z = z;
}
}
public class Vector3Int : Vector3Of<int> { }
public class Vector3Float : Vector3Of<float> { }
public class Vector3Int : Vector3Of<int> {
public Vector3Int(int x, int y, int z) : base(x, y, z) { }
}
public class Vector3Float : Vector3Of<float> {
public Vector3Float(float x, float y, float z) : base(x, y, z) { }
}
}