Stuck a essential matrix decomposition

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];

LinearAlgebra/Quaternion.cs

@@ -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) { }
+    }
+}

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<int> { }
+    public class Vector3Int : Vector3Of<int> {
-    public class Vector3Float : Vector3Of<float> { }
+        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) { }
+    }
 }