From b10929619d91811454abccfef356c8aef87da753 Mon Sep 17 00:00:00 2001
From: Pascal Serrarens <pascal@passervr.com>
Date: Mon, 17 Feb 2025 11:53:32 +0100
Subject: [PATCH] Added initial imu processing

---
 LinearAlgebra/Matrix.cs     | 44 +++++++++++++++++++++++++++----------
 LinearAlgebra/Quaternion.cs |  7 +++---
 2 files changed, 37 insertions(+), 14 deletions(-)

diff --git a/LinearAlgebra/Matrix.cs b/LinearAlgebra/Matrix.cs
index b5f7e9d..158c1d6 100644
--- a/LinearAlgebra/Matrix.cs
+++ b/LinearAlgebra/Matrix.cs
@@ -4,9 +4,9 @@ using Vector3 = UnityEngine.Vector3;
 using Vector2 = UnityEngine.Vector2;
 
 public readonly struct Slice {
-    public int start { get; }
-    public int stop { get; }
-    public Slice(int start, int stop) {
+    public uint start { get; }
+    public uint stop { get; }
+    public Slice(uint start, uint stop) {
         this.start = start;
         this.stop = stop;
     }
@@ -101,6 +101,9 @@ public class Matrix2 {
         return new Matrix2(resultData);
         // double checked code
     }
+    public Matrix2 transposed {
+        get => Transpose();
+    }
 
     public static Matrix2 operator -(Matrix2 m) {
         float[,] result = new float[m.nRows, m.nCols];
@@ -195,13 +198,13 @@ public class Matrix2 {
     public Matrix2 Slice(Slice slice) {
         return Slice(slice.start, slice.stop);
     }
-    public Matrix2 Slice(int from, int to) {
+    public Matrix2 Slice(uint from, uint to) {
         if (from < 0 || to >= this.nRows)
             throw new System.ArgumentException("Slice index out of range.");
 
         float[,] result = new float[to - from, this.nCols];
         int resultRowIx = 0;
-        for (int rowIx = from; rowIx < to; rowIx++) {
+        for (uint rowIx = from; rowIx < to; rowIx++) {
             for (int colIx = 0; colIx < this.nCols; colIx++) {
                 result[resultRowIx, colIx] = this.data[rowIx, colIx];
             }
@@ -210,16 +213,35 @@ public class Matrix2 {
 
         return new Matrix2(result);
     }
+    public Matrix2 Slice(Slice rowRange, Slice colRange) {
+        return Slice((rowRange.start, rowRange.stop), (colRange.start, colRange.stop));
+    }
+    
+    public Matrix2 Slice((uint start, uint stop) rowRange, (uint start, uint stop) colRange) {
+        float[,] result = new float[rowRange.stop - rowRange.start, colRange.stop - colRange.start];
+
+        uint resultRowIx = 0;
+        uint resultColIx = 0;
+        for (uint i = rowRange.start; i < rowRange.stop; i++) {
+            for (uint j = colRange.start; j < colRange.stop; j++)
+                result[resultRowIx, resultColIx] = this.data[i, j];
+        }
+        return new Matrix2(result);
+    }
+
     public void UpdateSlice(Slice slice, Matrix2 m) {
         int mRowIx = 0;
-        for (int rowIx = slice.start; rowIx < slice.stop; rowIx++) {
+        for (uint rowIx = slice.start; rowIx < slice.stop; rowIx++) {
             for (int colIx = 0; colIx < this.nCols; colIx++)
                 this.data[rowIx, colIx] = m.data[mRowIx, colIx];
         }
     }
     public void UpdateSlice(Slice rowRange, Slice colRange, Matrix2 m) {
-        for (int i = rowRange.start; i < rowRange.stop; i++) {
-            for (int j = colRange.start; j < colRange.stop; j++)
+        UpdateSlice((rowRange.start, rowRange.stop), (colRange.start, colRange.stop), m);
+    }
+    public void UpdateSlice((uint start, uint stop) rowRange, (uint start, uint stop) colRange, Matrix2 m) {
+        for (uint i = rowRange.start; i < rowRange.stop; i++) {
+            for (uint j = colRange.start; j < colRange.stop; j++)
                 this.data[i, j] = m.data[i - rowRange.start, j - colRange.stop];
         }
     }
@@ -401,20 +423,20 @@ public class Matrix1 {
     public Matrix1 Slice(Slice range) {
         return Slice(range.start, range.stop);
     }
-    public Matrix1 Slice(int from, int to) {
+    public Matrix1 Slice(uint from, uint to) {
         if (from < 0 || to >= this.magnitude)
             throw new System.ArgumentException("Slice index out of range.");
 
         float[] result = new float[to - from];
         int resultIx = 0;
-        for (int ix = from; ix < to; ix++)
+        for (uint ix = from; ix < to; ix++)
             result[resultIx++] = this.data[ix];
 
         return new Matrix1(result);
     }
     public void UpdateSlice(Slice slice, Matrix1 v) {
         int vIx = 0;
-        for (int ix = slice.start; ix < slice.stop; ix++, vIx++)
+        for (uint ix = slice.start; ix < slice.stop; ix++, vIx++)
             this.data[ix] = v.data[vIx];
     }
 }
\ No newline at end of file
diff --git a/LinearAlgebra/Quaternion.cs b/LinearAlgebra/Quaternion.cs
index 9a9fed7..742cd97 100644
--- a/LinearAlgebra/Quaternion.cs
+++ b/LinearAlgebra/Quaternion.cs
@@ -1,3 +1,4 @@
+using Quaternion = UnityEngine.Quaternion;
 namespace Passer.LinearAlgebra {
 
     public class QuaternionOf<T> {
@@ -26,7 +27,7 @@ namespace Passer.LinearAlgebra {
         }
     }
 
-    public class Quaternion : QuaternionOf<float> {
-        public Quaternion(float x, float y, float z, float w) : base(x, y, z, w) { }
-    }
+    // public class Quaternion : QuaternionOf<float> {
+    //     public Quaternion(float x, float y, float z, float w) : base(x, y, z, w) { }
+    // }
 }
\ No newline at end of file