diff --git a/LinearAlgebra/Matrix.cs b/LinearAlgebra/Matrix.cs index d51825e..c0abac8 100644 --- a/LinearAlgebra/Matrix.cs +++ b/LinearAlgebra/Matrix.cs @@ -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]; diff --git a/LinearAlgebra/Quaternion.cs b/LinearAlgebra/Quaternion.cs new file mode 100644 index 0000000..55ad014 --- /dev/null +++ b/LinearAlgebra/Quaternion.cs @@ -0,0 +1,31 @@ +namespace Passer.LinearAlgebra { + + public class QuaternionOf { + 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 { + public Quaternion(float x, float y, float z, float w) : base(x, y, z, w) { } + } +} \ No newline at end of file diff --git a/LinearAlgebra/Vector3.cs b/LinearAlgebra/Vector3.cs index 56bb9d8..c0bc6dd 100644 --- a/LinearAlgebra/Vector3.cs +++ b/LinearAlgebra/Vector3.cs @@ -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 { } - public class Vector3Float : Vector3Of { } + public class Vector3Int : Vector3Of { + public Vector3Int(int x, int y, int z) : base(x, y, z) { } + } + public class Vector3Float : Vector3Of { + public Vector3Float(float x, float y, float z) : base(x, y, z) { } + } } \ No newline at end of file