From bed6d95916ae9478af6f36ee87a2d1224d649a6a Mon Sep 17 00:00:00 2001
From: Pascal Serrarens <pascal@passervr.com>
Date: Thu, 23 Jan 2025 15:07:24 +0100
Subject: [PATCH] Lots of improvements, but RANSAC still does not give the
 right results

---
 LinearAlgebra/Matrix.cs | 37 +++++++++++++++++++++++++++++++------
 1 file changed, 31 insertions(+), 6 deletions(-)

diff --git a/LinearAlgebra/Matrix.cs b/LinearAlgebra/Matrix.cs
index 21fe238..d51825e 100644
--- a/LinearAlgebra/Matrix.cs
+++ b/LinearAlgebra/Matrix.cs
@@ -9,12 +9,15 @@ public class Matrix {
     }
 
     public static float[,] Transpose(float[,] m) {
-        float[,] r = new float[m.GetLength(1), m.GetLength(0)];
-        for (uint rowIx = 0; rowIx < m.GetLength(0); rowIx++) {
-            for (uint colIx = 0; colIx < m.GetLength(1); colIx++)
+        int rows = m.GetLength(0);
+        int cols = m.GetLength(1);
+        float[,] r = new float[cols, rows];
+        for (uint rowIx = 0; rowIx < rows; rowIx++) {
+            for (uint colIx = 0; colIx < cols; colIx++)
                 r[colIx, rowIx] = m[rowIx, colIx];
         }
         return r;
+        // double checked code
     }
 
     public static void NegateColumn(float[,] m, uint colIx) {
@@ -75,14 +78,36 @@ public class Matrix {
 
         for (int i = 0; i < rowsA; i++) {
             for (int j = 0; j < colsB; j++) {
-                float sum = 0;
-                for (int k = 0; k < colsA; k++) {
+                float sum = 0.0f;
+                for (int k = 0; k < colsA; k++)
                     sum += A[i, k] * B[k, j];
-                }
+
                 result[i, j] = sum;
             }
         }
 
         return result;
+        // double checked code
+    }
+
+    public static float[] GetColumn(float[,] M, int col) {
+        int rows = M.GetLength(0);
+        float[] column = new float[rows];
+        for (int i = 0; i < rows; i++) {
+            column[i] = M[i, col];
+        }
+        return column;
+    }
+
+    public static float Dot(float[] a, float[] b) {
+        float sum = 0;
+        for (int i = 0; i < a.Length; i++) sum += a[i] * b[i];
+        return sum;
+    }
+
+    public static float[,] IdentityMatrix(int size) {
+        float[,] I = new float[size, size];
+        for (int i = 0; i < size; i++) I[i, i] = 1.0f;
+        return I;
     }
 }
\ No newline at end of file