Some math work

src/wpimath/Matrix.cs

@@ -0,0 +1,70 @@
+using System;
+using System.Diagnostics;
+using System.Numerics;
+using System.Numerics.Tensors;
+using System.Runtime.InteropServices;
+using CommunityToolkit.Diagnostics;
+using CommunityToolkit.HighPerformance;
+using MathNet.Numerics;
+using MathNet.Numerics.LinearAlgebra.Double;
+using WPIMath.Numbers;
+
+namespace WPIMath;
+
+public readonly struct Matrix<R, C> where R : class, Nat where C : class, Nat
+{
+    private readonly DenseMatrix m_storage;
+
+    public Matrix()
+    {
+        int rows = R.Num;
+        int cols = C.Num;
+        m_storage = new DenseMatrix(rows, cols);
+    }
+
+    private Matrix(DenseMatrix storage)
+    {
+        Debug.Assert(storage.RowCount == R.Num);
+        Debug.Assert(storage.ColumnCount == C.Num);
+        m_storage = storage;
+    }
+
+    public readonly DenseMatrix Storage => m_storage;
+
+    public readonly MatrixSpan<R, C> Span => new(this);
+
+    public readonly int NumRows => R.Num;
+    public readonly int NumColumns => C.Num;
+
+    public readonly double this[int row, int col]
+    {
+        get
+        {
+            return m_storage[row, col];
+        }
+        set
+        {
+            m_storage[row, col] = value;
+        }
+    }
+
+    public void SetRow(MatrixSpan<N1, C> val)
+    {
+        throw new NotImplementedException();
+    }
+
+    public void SetColumn(MatrixSpan<R, N1> val)
+    {
+        throw new NotImplementedException();
+    }
+
+    public void Fill(double value)
+    {
+        m_storage.Values.AsSpan().Fill(value);
+    }
+
+    public Matrix<R, C> Diag()
+    {
+        return new(DenseMatrix.OfDiagonalVector(m_storage.Diagonal()));
+    }
+}

src/wpimath/MatrixSpan.cs

@@ -0,0 +1,38 @@
+using CommunityToolkit.HighPerformance;
+
+namespace WPIMath;
+
+public ref struct MatrixSpan<R, C> where R : class, Nat where C : class, Nat
+{
+    private Span2D<double> m_storage;
+
+    public MatrixSpan()
+    {
+        int rows = R.Num;
+        int cols = C.Num;
+        int length = rows * cols;
+        m_storage = new Memory2D<double>(new double[length], rows, cols).Span;
+    }
+
+    public MatrixSpan(Matrix<R, C> storage)
+    {
+        m_storage = storage.Storage.Span;
+    }
+
+    public readonly Span2D<double> Storage => m_storage;
+
+    public readonly int NumRows => R.Num;
+    public readonly int NumColumns => C.Num;
+
+    public double this[int row, int col]
+    {
+        get
+        {
+            return m_storage[row, col];
+        }
+        set
+        {
+            m_storage[row, col] = value;
+        }
+    }
+}

src/wpimath/Nat.cs

@@ -0,0 +1,6 @@
+namespace WPIMath;
+
+public interface Nat
+{
+    public static abstract int Num {get;}
+}

src/wpimath/Numbers/N1.cs

@@ -0,0 +1,8 @@
+namespace WPIMath.Numbers;
+
+public sealed class N1 : Nat
+{
+    private N1() { }
+
+    public static int Num => 1;
+}

src/wpimath/Numbers/N2.cs

@@ -0,0 +1,8 @@
+namespace WPIMath.Numbers;
+
+public sealed class N2 : Nat
+{
+    private N2() { }
+
+    public static int Num => 2;
+}

src/wpimath/Numbers/N3.cs

@@ -0,0 +1,8 @@
+namespace WPIMath.Numbers;
+
+public sealed class N3 : Nat
+{
+    private N3() { }
+
+    public static int Num => 3;
+}

src/wpimath/wpimath.csproj

@@ -13,4 +13,10 @@
     <ProjectReference Include="..\wpiutil\wpiutil.csproj" />
   </ItemGroup>
 
+  <ItemGroup>
+    <PackageReference Include="CommunityToolkit.HighPerformance" Version="8.2.2" />
+    <PackageReference Include="MathNet.Numerics" Version="5.0.0" />
+    <PackageReference Include="System.Numerics.Tensors" Version="8.0.0" />
+  </ItemGroup>
+
 </Project>