Ported LinearTriangulate

LinearAlgebra/Matrix.cs

@@ -1,3 +1,5 @@
+using Vector3 = UnityEngine.Vector3;
+
 public class Matrix {
     private readonly uint rows = 0;
     private readonly uint cols = 0;
@@ -114,6 +116,28 @@ public class Matrix {
         return result;
     }
 
+    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];
@@ -123,6 +147,21 @@ public class Matrix {
         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];
@@ -134,4 +173,5 @@ public class Matrix {
         for (int i = 0; i < size; i++) I[i, i] = 1.0f;
         return I;
     }
+
 }