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,113 @@
+using System;
+using System.Collections.Generic;
+using System.Collections.ObjectModel;
+using MultiWheelC.TrajectoryPlanning.Utils;
+
+namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
+
+public sealed class QuadraticProgram
+{
+    public const double MaximumFiniteBound = 1e30d;
+
+    public QuadraticProgram(
+        SparseCscMatrix upperTriangularP,
+        IReadOnlyList<double> q,
+        SparseCscMatrix a,
+        IReadOnlyList<double> lowerBounds,
+        IReadOnlyList<double> upperBounds)
+    {
+        if (upperTriangularP == null)
+            throw new ArgumentNullException(nameof(upperTriangularP));
+        if (q == null)
+            throw new ArgumentNullException(nameof(q));
+        if (a == null)
+            throw new ArgumentNullException(nameof(a));
+        if (lowerBounds == null)
+            throw new ArgumentNullException(nameof(lowerBounds));
+        if (upperBounds == null)
+            throw new ArgumentNullException(nameof(upperBounds));
+        if (upperTriangularP.RowCount != upperTriangularP.ColumnCount)
+            throw new ArgumentOutOfRangeException(nameof(upperTriangularP), "The quadratic Hessian must be square.");
+        if (q.Count != upperTriangularP.ColumnCount)
+            throw new ArgumentOutOfRangeException(nameof(q), "The linear cost length must match the variable count.");
+        if (a.ColumnCount != upperTriangularP.ColumnCount)
+            throw new ArgumentOutOfRangeException(nameof(a), "Constraint columns must match the variable count.");
+        if (lowerBounds.Count != a.RowCount || upperBounds.Count != a.RowCount)
+            throw new ArgumentOutOfRangeException(nameof(lowerBounds), "Constraint bounds must match the constraint count.");
+
+        ValidateUpperTriangle(upperTriangularP);
+        ValidateFinite(q, nameof(q));
+        ValidateBounds(lowerBounds, upperBounds);
+
+        UpperTriangularP = CopyMatrix(upperTriangularP);
+        LinearCost = Copy(q);
+        ConstraintMatrix = CopyMatrix(a);
+        LowerBounds = Copy(lowerBounds);
+        UpperBounds = Copy(upperBounds);
+        VariableCount = UpperTriangularP.ColumnCount;
+        ConstraintCount = ConstraintMatrix.RowCount;
+    }
+
+    public SparseCscMatrix UpperTriangularP { get; }
+
+    public IReadOnlyList<double> LinearCost { get; }
+
+    public SparseCscMatrix ConstraintMatrix { get; }
+
+    public IReadOnlyList<double> LowerBounds { get; }
+
+    public IReadOnlyList<double> UpperBounds { get; }
+
+    public int VariableCount { get; }
+
+    public int ConstraintCount { get; }
+
+    private static void ValidateUpperTriangle(SparseCscMatrix matrix)
+    {
+        for (int column = 0; column < matrix.ColumnCount; column++)
+        {
+            for (int index = matrix.ColumnPointers[column]; index < matrix.ColumnPointers[column + 1]; index++)
+            {
+                if (matrix.RowIndices[index] > column)
+                    throw new ArgumentOutOfRangeException(nameof(matrix), "The quadratic Hessian must store only its upper triangle.");
+            }
+        }
+    }
+
+    private static void ValidateFinite(IReadOnlyList<double> values, string parameterName)
+    {
+        for (int index = 0; index < values.Count; index++)
+        {
+            if (!NumericGuard.IsFinite(values[index]))
+                throw new ArgumentOutOfRangeException(parameterName, "Values must be finite.");
+        }
+    }
+
+    private static void ValidateBounds(IReadOnlyList<double> lowerBounds, IReadOnlyList<double> upperBounds)
+    {
+        for (int index = 0; index < lowerBounds.Count; index++)
+        {
+            double lower = lowerBounds[index];
+            double upper = upperBounds[index];
+            if (!NumericGuard.IsFinite(lower) || !NumericGuard.IsFinite(upper) ||
+                lower < -MaximumFiniteBound || upper > MaximumFiniteBound || lower > upper)
+            {
+                throw new ArgumentOutOfRangeException(nameof(lowerBounds),
+                    "Constraint bounds must be finite, within the supported range, and ordered.");
+            }
+        }
+    }
+
+    private static SparseCscMatrix CopyMatrix(SparseCscMatrix source)
+    {
+        return new SparseCscMatrix(source.RowCount, source.ColumnCount, source.Values, source.RowIndices, source.ColumnPointers);
+    }
+
+    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);
+    }
+}