completed ComputeResidualAndJacobian

LinearAlgebra/Matrix.cs

@@ -1,6 +1,16 @@
 using System;
 using System.Diagnostics;
 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) {
+        this.start = start;
+        this.stop = stop;
+    }
+}
 
 public class Matrix2 {
     public float[,] data { get; }
@@ -40,6 +50,15 @@ public class Matrix2 {
         return new Matrix2(resultData);
     }
 
+    public static Matrix2 SkewMatrix(Vector3 v) {
+        float[,] result = new float[3, 3] {
+            {0, -v.z, v.y},
+            {v.z, 0, -v.x},
+            {-v.y, v.x, 0}
+        };
+        return new Matrix2(result);
+    }
+
     public Matrix2 Transpose() {
         float[,] resultData = new float[this.nCols, this.nRows];
         for (uint rowIx = 0; rowIx < this.nRows; rowIx++) {
@@ -50,6 +69,16 @@ public class Matrix2 {
         // double checked code
     }
 
+    public static Matrix2 operator -(Matrix2 m) {
+        float[,] result = new float[m.nRows, m.nCols];
+
+        for (int i = 0; i < m.nRows; i++) {
+            for (int j = 0; j < m.nCols; j++)
+                result[i, j] = -m.data[i, j];
+        }
+        return new Matrix2(result);
+    }
+
     public static Matrix2 operator -(Matrix2 A, Matrix2 B) {
         if (A.nRows != B.nRows || A.nCols != B.nCols)
             throw new System.ArgumentException("Size of A must match size of B.");
@@ -97,8 +126,6 @@ public class Matrix2 {
     }
 
     public static Matrix1 operator *(Matrix2 A, Matrix1 v) {
-        // int rows = A.GetLength(0);
-        // int cols = A.GetLength(1);
         float[] result = new float[A.nRows];
 
         for (int i = 0; i < A.nRows; i++) {
@@ -110,6 +137,14 @@ public class Matrix2 {
         return new Matrix1(result);
     }
 
+    public static Vector3 operator *(Matrix2 A, Vector3 v) {
+        return new Vector3() {
+            x = A.data[0, 0] * v.x + A.data[0, 1] * v.y + A.data[0, 2] * v.z,
+            y = A.data[1, 0] * v.x + A.data[1, 1] * v.y + A.data[1, 2] * v.z,
+            z = A.data[2, 0] * v.x + A.data[2, 1] * v.y + A.data[2, 2] * v.z
+        };
+    }
+
 
     public static Matrix2 operator *(Matrix2 A, float s) {
         float[,] result = new float[A.nRows, A.nCols];
@@ -125,6 +160,9 @@ public class Matrix2 {
         return A * scalar;
     }
 
+    public Matrix2 Slice(Slice slice) {
+        return Slice(slice.start, slice.stop);
+    }
     public Matrix2 Slice(int from, int to) {
         if (from < 0 || to >= this.nRows)
             throw new System.ArgumentException("Slice index out of range.");
@@ -140,6 +178,19 @@ public class Matrix2 {
 
         return new Matrix2(result);
     }
+    public void UpdateSlice(Slice slice, Matrix2 m) {
+        int mRowIx = 0;
+        for (int 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++)
+                this.data[i, j] = m.data[i - rowRange.start, j - colRange.stop];
+        }
+    }
 
     public Matrix2 Inverse() {
         Matrix2 A = this;
@@ -212,6 +263,28 @@ public class Matrix1 {
         return new Matrix1(magnitude);
     }
 
+    public static Matrix1 FromVector2(Vector2 v) {
+        float[] result = new float[2];
+        result[0] = v.x;
+        result[1] = v.y;
+        return new Matrix1(result);
+    }
+
+    public static Matrix1 FromVector3(Vector3 v) {
+        float[] result = new float[3];
+        result[0] = v.x;
+        result[1] = v.y;
+        result[2] = v.z;
+        return new Matrix1(result);
+    }
+
+    public Vector2 vector2 {
+        get {
+            if (this.magnitude != 2)
+                throw new System.ArgumentException("Matrix1 must be of size 2");
+            return new Vector2(this.data[0], this.data[1]);
+        }
+    }
     public Vector3 vector3 {
         get {
             if (this.magnitude != 3)
@@ -251,6 +324,9 @@ public class Matrix1 {
         return A * f;
     }
 
+    public Matrix1 Slice(Slice range) {
+        return Slice(range.start, range.stop);
+    }
     public Matrix1 Slice(int from, int to) {
         if (from < 0 || to >= this.magnitude)
             throw new System.ArgumentException("Slice index out of range.");
@@ -262,6 +338,11 @@ public class Matrix1 {
 
         return new Matrix1(result);
     }
+    public void UpdateSlice(Slice slice, Matrix1 v) {
+        int vIx = 0;
+        for (int ix = slice.start; ix < slice.stop; ix++, vIx++)
+            this.data[ix] = v.data[vIx];
+    }
 }
 
 public class Matrix {