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,13 @@
+using System.Collections.Generic;
+using System.Threading;
+
+namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
+
+public interface IQpSolver
+{
+    QpSolveResult Solve(
+        QuadraticProgram problem,
+        QpSolverSettings settings,
+        IReadOnlyList<double> warmStart,
+        CancellationToken cancellationToken);
+}
@@ -0,0 +1,68 @@
+using System;
+using System.Collections.Generic;
+using System.Collections.ObjectModel;
+using MultiWheelC.TrajectoryPlanning.Utils;
+
+namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
+
+public sealed class QpSolveResult
+{
+    public QpSolveResult(
+        QpSolveStatus status,
+        IReadOnlyList<double> primal,
+        double objective,
+        double primalResidual,
+        double dualResidual,
+        int iterations,
+        TimeSpan solveTime,
+        string nativeStatus,
+        string diagnostic)
+    {
+        if (!Enum.IsDefined(typeof(QpSolveStatus), status))
+            throw new ArgumentOutOfRangeException(nameof(status));
+        if (primal == null)
+            throw new ArgumentNullException(nameof(primal));
+        if (iterations < 0)
+            throw new ArgumentOutOfRangeException(nameof(iterations));
+        if (solveTime < TimeSpan.Zero)
+            throw new ArgumentOutOfRangeException(nameof(solveTime));
+        if (!NumericGuard.IsFinite(objective) || !NumericGuard.IsFinite(primalResidual) || !NumericGuard.IsFinite(dualResidual))
+            throw new ArgumentOutOfRangeException(nameof(objective), "Solver metrics must be finite.");
+
+        var copiedPrimal = new List<double>(primal.Count);
+        for (int index = 0; index < primal.Count; index++)
+        {
+            if (!NumericGuard.IsFinite(primal[index]))
+                throw new ArgumentOutOfRangeException(nameof(primal), "Primal values must be finite.");
+            copiedPrimal.Add(primal[index]);
+        }
+
+        Status = status;
+        Primal = new ReadOnlyCollection<double>(copiedPrimal);
+        Objective = objective;
+        PrimalResidual = primalResidual;
+        DualResidual = dualResidual;
+        Iterations = iterations;
+        SolveTime = solveTime;
+        NativeStatus = nativeStatus ?? string.Empty;
+        Diagnostic = diagnostic ?? string.Empty;
+    }
+
+    public QpSolveStatus Status { get; }
+
+    public IReadOnlyList<double> Primal { get; }
+
+    public double Objective { get; }
+
+    public double PrimalResidual { get; }
+
+    public double DualResidual { get; }
+
+    public int Iterations { get; }
+
+    public TimeSpan SolveTime { get; }
+
+    public string NativeStatus { get; }
+
+    public string Diagnostic { get; }
+}
@@ -0,0 +1,15 @@
+namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
+
+public enum QpSolveStatus
+{
+    Solved,
+    SolvedInaccurate,
+    PrimalInfeasible,
+    DualInfeasible,
+    MaximumIterations,
+    TimeLimit,
+    Cancelled,
+    SolverUnavailable,
+    InvalidProblem,
+    NativeError,
+}
@@ -0,0 +1,48 @@
+using System;
+using MultiWheelC.TrajectoryPlanning.Utils;
+
+namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
+
+public sealed class QpSolverSettings
+{
+    public QpSolverSettings(
+        int maximumIterations,
+        double absoluteTolerance,
+        double relativeTolerance,
+        TimeSpan timeLimit,
+        bool enableWarmStart,
+        bool enablePolishing,
+        bool enableNativeVerboseOutput)
+    {
+        if (maximumIterations <= 0)
+            throw new ArgumentOutOfRangeException(nameof(maximumIterations));
+        if (!NumericGuard.IsPositiveFinite(absoluteTolerance))
+            throw new ArgumentOutOfRangeException(nameof(absoluteTolerance));
+        if (!NumericGuard.IsPositiveFinite(relativeTolerance))
+            throw new ArgumentOutOfRangeException(nameof(relativeTolerance));
+        if (timeLimit <= TimeSpan.Zero)
+            throw new ArgumentOutOfRangeException(nameof(timeLimit));
+
+        MaximumIterations = maximumIterations;
+        AbsoluteTolerance = absoluteTolerance;
+        RelativeTolerance = relativeTolerance;
+        TimeLimit = timeLimit;
+        EnableWarmStart = enableWarmStart;
+        EnablePolishing = enablePolishing;
+        EnableNativeVerboseOutput = enableNativeVerboseOutput;
+    }
+
+    public int MaximumIterations { get; }
+
+    public double AbsoluteTolerance { get; }
+
+    public double RelativeTolerance { get; }
+
+    public TimeSpan TimeLimit { get; }
+
+    public bool EnableWarmStart { get; }
+
+    public bool EnablePolishing { get; }
+
+    public bool EnableNativeVerboseOutput { get; }
+}
@@ -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);
+    }
+}
@@ -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);
+    }
+}
@@ -0,0 +1,104 @@
+using System;
+using System.Collections.Generic;
+using MultiWheelC.TrajectoryPlanning.Utils;
+
+namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
+
+public sealed class SparseTripletBuilder
+{
+    private readonly int rowCount;
+    private readonly int columnCount;
+    private readonly bool upperTriangleOnly;
+    private readonly List<Triplet> triplets = new List<Triplet>();
+
+    public SparseTripletBuilder(int rowCount, int columnCount)
+        : this(rowCount, columnCount, false)
+    {
+    }
+
+    public SparseTripletBuilder(int rowCount, int columnCount, bool upperTriangleOnly)
+    {
+        if (rowCount < 0)
+            throw new ArgumentOutOfRangeException(nameof(rowCount));
+        if (columnCount < 0)
+            throw new ArgumentOutOfRangeException(nameof(columnCount));
+
+        this.rowCount = rowCount;
+        this.columnCount = columnCount;
+        this.upperTriangleOnly = upperTriangleOnly;
+    }
+
+    public void Add(int row, int column, double value)
+    {
+        if (row < 0 || row >= rowCount)
+            throw new ArgumentOutOfRangeException(nameof(row));
+        if (column < 0 || column >= columnCount)
+            throw new ArgumentOutOfRangeException(nameof(column));
+        if (!NumericGuard.IsFinite(value))
+            throw new ArgumentOutOfRangeException(nameof(value), "Sparse triplet values must be finite.");
+        if (upperTriangleOnly && row > column)
+            throw new ArgumentOutOfRangeException(nameof(row), "Upper-triangular storage does not accept lower-triangular entries.");
+
+        triplets.Add(new Triplet(row, column, value));
+    }
+
+    public SparseCscMatrix Build()
+    {
+        var ordered = new List<Triplet>(triplets);
+        ordered.Sort(CompareTriplets);
+
+        var values = new List<double>();
+        var rows = new List<int>();
+        var pointers = new List<int>(columnCount + 1) { 0 };
+        int nextTriplet = 0;
+
+        for (int column = 0; column < columnCount; column++)
+        {
+            while (nextTriplet < ordered.Count && ordered[nextTriplet].Column == column)
+            {
+                int row = ordered[nextTriplet].Row;
+                double sum = 0d;
+                do
+                {
+                    sum += ordered[nextTriplet].Value;
+                    nextTriplet++;
+                }
+                while (nextTriplet < ordered.Count && ordered[nextTriplet].Column == column && ordered[nextTriplet].Row == row);
+
+                if (!NumericGuard.IsFinite(sum))
+                    throw new ArgumentOutOfRangeException(nameof(triplets), "Duplicate sparse triplets must sum to a finite value.");
+                if (sum != 0d)
+                {
+                    rows.Add(row);
+                    values.Add(sum);
+                }
+            }
+
+            pointers.Add(values.Count);
+        }
+
+        return new SparseCscMatrix(rowCount, columnCount, values, rows, pointers);
+    }
+
+    private static int CompareTriplets(Triplet left, Triplet right)
+    {
+        int columnComparison = left.Column.CompareTo(right.Column);
+        return columnComparison != 0 ? columnComparison : left.Row.CompareTo(right.Row);
+    }
+
+    private sealed class Triplet
+    {
+        public Triplet(int row, int column, double value)
+        {
+            Row = row;
+            Column = column;
+            Value = value;
+        }
+
+        public int Row { get; }
+
+        public int Column { get; }
+
+        public double Value { get; }
+    }
+}