chore: save current workspace progress

ClumsyPilot/ParkrobTrajplanner/EMPlanner/Lateral/SequentialConvexOptimizer.cs

@@ -68,6 +68,8 @@ public sealed class SequentialConvexOptimizer
                     remainingBudget, settings.EnableWarmStart, settings.EnablePolishing, settings.EnableNativeVerboseOutput),
                 warmStart, cancellationToken);
 
+            if (cancellationToken.IsCancellationRequested)
+                return Failed(EmPlanningStatus.Cancelled, "Lateral SQP was cancelled after the QP solve.");
             if (solved == null)
                 return FallbackOrFailure(lastValidatedPath, EmPlanningStatus.Failed, "The lateral QP solver returned no result.");
 
@@ -100,16 +102,27 @@ public sealed class SequentialConvexOptimizer
                 continue;
             if (!_geometryEvaluator.TryEvaluate(input, candidate, out LateralPath evaluatedPath, out _))
                 continue;
+
+            // A geometrically evaluable QP solution that remains inside the static
+            // corridor is the next SQP linearization point, even when the independent
+            // validator rejects it.  Otherwise the next outer pass rebuilds the identical
+            // convex subproblem and cannot correct that result.  An out-of-corridor vector
+            // must not become an iterate: it can make the following trust region infeasible.
+            LateralCandidate previousIterate = iterate;
+            if (RespectsStaticCorridor(input, candidate))
+            {
+                iterate = candidate;
+                warmStart = CopyValues(solved.Primal);
+            }
+
             if (!_solutionValidator.TryValidate(input, candidate, evaluatedPath, out LateralPath validatedPath, out _))
                 continue;
 
-            double maximumLateralChange = MaximumLateralChange(iterate, candidate);
+            double maximumLateralChange = MaximumLateralChange(previousIterate, candidate);
             double relativeObjectiveImprovement = hasPreviousObjective
                 ? RelativeObjectiveImprovement(previousObjective, solved.Objective)
                 : double.PositiveInfinity;
             lastValidatedPath = CopyPath(validatedPath);
-            iterate = candidate;
-            warmStart = CopyValues(solved.Primal);
             previousObjective = solved.Objective;
             hasPreviousObjective = true;
 
@@ -119,7 +132,10 @@ public sealed class SequentialConvexOptimizer
 
         return lastValidatedPath == null
             ? Failed(EmPlanningStatus.LateralInfeasible, "No independently validated lateral candidate was found.")
-            : new LateralPlanningResult(EmPlanningStatus.Success, lastValidatedPath, string.Empty);
+            : new LateralPlanningResult(
+                EmPlanningStatus.SuccessWithFallback,
+                lastValidatedPath,
+                "Lateral SQP reached its outer-iteration limit before strict convergence.");
     }
 
     private static bool TryCreateSettings(LateralPlanningInput input, out QpSolverSettings settings, out TimeSpan totalBudget,
@@ -226,6 +242,17 @@ public sealed class SequentialConvexOptimizer
         return maximum;
     }
 
+    private static bool RespectsStaticCorridor(LateralPlanningInput input, LateralCandidate candidate)
+    {
+        for (int index = 0; index < candidate.L.Count; index++)
+        {
+            LateralInterval interval = input.Corridor.Stations[index];
+            if (candidate.L[index] < interval.MinimumL || candidate.L[index] > interval.MaximumL)
+                return false;
+        }
+        return true;
+    }
+
     private static double RelativeObjectiveImprovement(double previous, double current)
     {
         return Math.Abs(previous - current) / Math.Max(1d, Math.Abs(previous));
@@ -233,7 +260,7 @@ public sealed class SequentialConvexOptimizer
 
     private static LateralPlanningResult FallbackOrFailure(LateralPath path, EmPlanningStatus failureStatus, string failureReason)
     {
-        return path == null
+        return failureStatus == EmPlanningStatus.Cancelled || path == null
             ? Failed(failureStatus, failureReason)
             : new LateralPlanningResult(EmPlanningStatus.SuccessWithFallback, path, failureReason);
     }