feat: add solver-neutral QP contracts

2026-08-03 23:13:56 +08:00
parent 333e047bc4
commit 104081e4a9
9 changed files with 597 additions and 2 deletions
@@ -0,0 +1,87 @@
+using System;
+using System.Collections.Generic;
+using System.Collections.ObjectModel;
+using MultiWheelC.TrajectoryPlanning.Utils;
+
+namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
+
+public sealed class SparseCscMatrix
+{
+    public SparseCscMatrix(
+        int rowCount,
+        int columnCount,
+        IReadOnlyList<double> values,
+        IReadOnlyList<int> rowIndices,
+        IReadOnlyList<int> columnPointers)
+    {
+        if (rowCount < 0)
+            throw new ArgumentOutOfRangeException(nameof(rowCount));
+        if (columnCount < 0)
+            throw new ArgumentOutOfRangeException(nameof(columnCount));
+        if (values == null)
+            throw new ArgumentNullException(nameof(values));
+        if (rowIndices == null)
+            throw new ArgumentNullException(nameof(rowIndices));
+        if (columnPointers == null)
+            throw new ArgumentNullException(nameof(columnPointers));
+        if (values.Count != rowIndices.Count)
+            throw new ArgumentException("CSC values and row indices must have the same length.", nameof(rowIndices));
+        if (columnPointers.Count != columnCount + 1)
+            throw new ArgumentException("CSC column-pointer count must equal column count plus one.", nameof(columnPointers));
+        if (columnPointers[0] != 0)
+            throw new ArgumentOutOfRangeException(nameof(columnPointers), "The first CSC column pointer must be zero.");
+        if (columnPointers[columnCount] != values.Count)
+            throw new ArgumentOutOfRangeException(nameof(columnPointers), "The final CSC column pointer must equal the nonzero count.");
+
+        for (int column = 0; column < columnCount; column++)
+        {
+            if (columnPointers[column] > columnPointers[column + 1])
+                throw new ArgumentOutOfRangeException(nameof(columnPointers), "CSC column pointers must be monotonic.");
+            if (columnPointers[column] < 0 || columnPointers[column + 1] > values.Count)
+                throw new ArgumentOutOfRangeException(nameof(columnPointers), "CSC column pointers must stay within the nonzero count.");
+        }
+
+        for (int index = 0; index < values.Count; index++)
+        {
+            if (!NumericGuard.IsFinite(values[index]))
+                throw new ArgumentOutOfRangeException(nameof(values), "CSC values must be finite.");
+            if (rowIndices[index] < 0 || rowIndices[index] >= rowCount)
+                throw new ArgumentOutOfRangeException(nameof(rowIndices), "CSC row indices must be within matrix bounds.");
+        }
+
+        for (int column = 0; column < columnCount; column++)
+        {
+            int previousRow = -1;
+            for (int index = columnPointers[column]; index < columnPointers[column + 1]; index++)
+            {
+                if (rowIndices[index] <= previousRow)
+                    throw new ArgumentException("CSC row indices must be strictly ascending in each column.", nameof(rowIndices));
+                previousRow = rowIndices[index];
+            }
+        }
+
+        RowCount = rowCount;
+        ColumnCount = columnCount;
+        Values = Copy(values);
+        RowIndices = Copy(rowIndices);
+        ColumnPointers = Copy(columnPointers);
+    }
+
+    public int RowCount { get; }
+
+    public int ColumnCount { get; }
+
+    public IReadOnlyList<double> Values { get; }
+
+    public IReadOnlyList<int> RowIndices { get; }
+
+    public IReadOnlyList<int> ColumnPointers { get; }
+
+    private static IReadOnlyList<T> Copy<T>(IReadOnlyList<T> source)
+    {
+        var copy = new List<T>(source.Count);
+        for (int index = 0; index < source.Count; index++)
+            copy.Add(source[index]);
+        return new ReadOnlyCollection<T>(copy);
+    }
+}