chore: save current workspace progress

ClumsyPilot/tests/EMPlannerVerificationHost/LateralModelChecks.cs

@@ -18,6 +18,7 @@ internal static class LateralModelChecks
         VerifiesPlanningInputBoundariesAndDefensiveCopies();
         VerifiesLateralResultPublicationContract();
         VerifiesNormalizedObjectiveAndHardConstraints();
+        VerifiesVehicleCurvatureIsAHardQpConstraint();
         VerifiesAllNamedCostScales();
         VerifiesEmptyHardBoundIntersectionFailsBeforeSolve();
         VerifiesFakeSolverCapturesTheNeutralQpBoundary();
@@ -245,6 +246,29 @@ internal static class LateralModelChecks
             "Frenet denominator is intersected as a finite hard lateral bound");
     }
 
+    private static void VerifiesVehicleCurvatureIsAHardQpConstraint()
+    {
+        EmPlannerConfiguration configuration = CreateUnitScaleConfiguration();
+        const double maximumVehicleCurvature = 0.25d;
+        LateralPlanningInput input = CreateModelInput(EmTerminalType.RollingSafetyStop, configuration,
+            Array.Empty<double>(), maximumVehicleCurvature: maximumVehicleCurvature);
+        LateralCandidate linearization = LateralCandidate.Integrate(input.ReferenceStations, 0d, 0d, 0d,
+            new[] { 0d, 0d });
+
+        Verification.True(CreateConstraintBuilder().TryBuild(input, linearization, out QuadraticProgram problem,
+                out string failureReason), "curvature-constrained QP builds: " + failureReason);
+
+        var layout = new LateralVariableLayout(input.ReferenceStations.Count);
+        Verification.Equal(8 * layout.StationCount - 2, problem.ConstraintCount,
+            "each lateral station adds one curvature hard-constraint row");
+        for (int station = 0; station < layout.StationCount; station++)
+        {
+            Verification.True(HasBound(problem, new Dictionary<int, double> { { layout.DDL(station), 1d } },
+                    -maximumVehicleCurvature, maximumVehicleCurvature),
+                "straight-reference curvature is a hard DDL bound at station " + station);
+        }
+    }
+
     private static void VerifiesEmptyHardBoundIntersectionFailsBeforeSolve()
     {
         EmPlannerConfiguration configuration = CreateUnitScaleConfiguration();
@@ -550,6 +574,21 @@ internal static class LateralModelChecks
         return false;
     }
 
+    private static bool HasBound(QuadraticProgram problem, IReadOnlyDictionary<int, double> expected,
+        double lower, double upper)
+    {
+        for (int row = 0; row < problem.ConstraintCount; row++)
+        {
+            if (RowMatches(problem.ConstraintMatrix, row, expected) &&
+                Math.Abs(problem.LowerBounds[row] - lower) <= 1e-12d &&
+                Math.Abs(problem.UpperBounds[row] - upper) <= 1e-12d)
+            {
+                return true;
+            }
+        }
+        return false;
+    }
+
     private static bool RowMatches(SparseCscMatrix matrix, int targetRow, IReadOnlyDictionary<int, double> expected)
     {
         var actual = new Dictionary<int, double>();