feat: assemble lateral LS quadratic programs

2026-08-04 00:58:09 +08:00
parent c225b17d37
commit f703d418ab
4 changed files with 766 additions and 8 deletions
@@ -0,0 +1,39 @@
+using System;
+using System.Collections.Generic;
+using System.Collections.ObjectModel;
+using System.Threading;
+using MultiWheelC.TrajectoryPlanning.EMPlanner;
+
+namespace EMPlannerVerificationHost;
+
+internal sealed class FakeQpSolver : IQpSolver
+{
+    private readonly QpSolveResult _result;
+
+    public FakeQpSolver(QpSolveResult result)
+    {
+        _result = result ?? throw new ArgumentNullException(nameof(result));
+        LastWarmStart = Array.Empty<double>();
+    }
+
+    public QuadraticProgram? LastProblem { get; private set; }
+
+    public QpSolverSettings? LastSettings { get; private set; }
+
+    public IReadOnlyList<double> LastWarmStart { get; private set; }
+
+    public QpSolveResult Solve(QuadraticProgram problem, QpSolverSettings settings, IReadOnlyList<double> warmStart,
+        CancellationToken cancellationToken)
+    {
+        LastProblem = problem ?? throw new ArgumentNullException(nameof(problem));
+        LastSettings = settings ?? throw new ArgumentNullException(nameof(settings));
+        var copy = new List<double>(warmStart == null ? 0 : warmStart.Count);
+        if (warmStart != null)
+        {
+            for (int index = 0; index < warmStart.Count; index++)
+                copy.Add(warmStart[index]);
+        }
+        LastWarmStart = new ReadOnlyCollection<double>(copy);
+        return _result;
+    }
+}