feat: add lateral optimization model

ClumsyPilot/ParkrobTrajplanner/EMPlanner/Lateral/LateralCandidate.cs

@@ -0,0 +1,114 @@
+using System;
+using System.Collections.Generic;
+using System.Collections.ObjectModel;
+
+namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
+
+/// <summary>One discrete lateral iterate expressed against exact reference-S stations.</summary>
+public sealed class LateralCandidate
+{
+    public LateralCandidate(IReadOnlyList<double> referenceStations, IReadOnlyList<double> l, IReadOnlyList<double> dl,
+        IReadOnlyList<double> ddl, IReadOnlyList<double> dddl)
+    {
+        ReferenceStations = CopyStations(referenceStations);
+        int stationCount = ReferenceStations.Count;
+        L = CopyValues(l, stationCount, nameof(l));
+        DL = CopyValues(dl, stationCount, nameof(dl));
+        DDL = CopyValues(ddl, stationCount, nameof(ddl));
+        DDDL = CopyValues(dddl, stationCount - 1, nameof(dddl));
+    }
+
+    public IReadOnlyList<double> ReferenceStations { get; }
+
+    public IReadOnlyList<double> L { get; }
+
+    public IReadOnlyList<double> DL { get; }
+
+    public IReadOnlyList<double> DDL { get; }
+
+    public IReadOnlyList<double> DDDL { get; }
+
+    public static LateralCandidate Integrate(IReadOnlyList<double> referenceStations, double initialL, double initialDL,
+        double initialDDL, IReadOnlyList<double> dddl)
+    {
+        IReadOnlyList<double> stations = CopyStations(referenceStations);
+        if (!IsFinite(initialL) || !IsFinite(initialDL) || !IsFinite(initialDDL))
+            throw new ArgumentOutOfRangeException(nameof(initialL));
+        IReadOnlyList<double> copiedJerk = CopyValues(dddl, stations.Count - 1, nameof(dddl));
+
+        var l = new double[stations.Count];
+        var dl = new double[stations.Count];
+        var ddl = new double[stations.Count];
+        l[0] = initialL;
+        dl[0] = initialDL;
+        ddl[0] = initialDDL;
+        for (int index = 0; index < copiedJerk.Count; index++)
+        {
+            double ds = stations[index + 1] - stations[index];
+            double jerk = copiedJerk[index];
+            ddl[index + 1] = ddl[index] + ds * jerk;
+            dl[index + 1] = dl[index] + ds * ddl[index] + 0.5d * ds * ds * jerk;
+            l[index + 1] = l[index] + ds * dl[index] + 0.5d * ds * ds * ddl[index] +
+                ds * ds * ds * jerk / 6d;
+        }
+        return new LateralCandidate(stations, l, dl, ddl, copiedJerk);
+    }
+
+    public bool SatisfiesExactDiscreteDynamics(double tolerance)
+    {
+        if (!IsFinite(tolerance) || tolerance < 0d)
+            throw new ArgumentOutOfRangeException(nameof(tolerance));
+
+        for (int index = 0; index < DDDL.Count; index++)
+        {
+            double ds = ReferenceStations[index + 1] - ReferenceStations[index];
+            double jerk = DDDL[index];
+            if (Math.Abs(DDL[index + 1] - (DDL[index] + ds * jerk)) > tolerance ||
+                Math.Abs(DL[index + 1] - (DL[index] + ds * DDL[index] + 0.5d * ds * ds * jerk)) > tolerance ||
+                Math.Abs(L[index + 1] - (L[index] + ds * DL[index] + 0.5d * ds * ds * DDL[index] +
+                    ds * ds * ds * jerk / 6d)) > tolerance)
+            {
+                return false;
+            }
+        }
+        return true;
+    }
+
+    private static IReadOnlyList<double> CopyStations(IReadOnlyList<double> values)
+    {
+        if (values == null || values.Count < 2)
+            throw new ArgumentException("At least two reference-S stations are required.", nameof(values));
+
+        var copy = new List<double>(values.Count);
+        double previous = double.NegativeInfinity;
+        for (int index = 0; index < values.Count; index++)
+        {
+            double value = values[index];
+            if (!IsFinite(value) || value <= previous)
+                throw new ArgumentException("Reference-S stations must be finite and strictly increasing.", nameof(values));
+            copy.Add(value);
+            previous = value;
+        }
+        return new ReadOnlyCollection<double>(copy);
+    }
+
+    private static IReadOnlyList<double> CopyValues(IReadOnlyList<double> values, int expectedCount, string parameterName)
+    {
+        if (values == null || values.Count != expectedCount)
+            throw new ArgumentException("Lateral value count does not match the station layout.", parameterName);
+
+        var copy = new List<double>(values.Count);
+        for (int index = 0; index < values.Count; index++)
+        {
+            if (!IsFinite(values[index]))
+                throw new ArgumentOutOfRangeException(parameterName);
+            copy.Add(values[index]);
+        }
+        return new ReadOnlyCollection<double>(copy);
+    }
+
+    private static bool IsFinite(double value)
+    {
+        return !double.IsNaN(value) && !double.IsInfinity(value);
+    }
+}

ClumsyPilot/ParkrobTrajplanner/EMPlanner/Lateral/LateralPath.cs

@@ -0,0 +1,29 @@
+using System;
+using System.Collections.Generic;
+using System.Collections.ObjectModel;
+
+namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
+
+/// <summary>Immutable reconstructed lateral path, marked only after independent validation.</summary>
+public sealed class LateralPath
+{
+    public LateralPath(IReadOnlyList<LateralPathPoint> points, bool independentlyValidated)
+    {
+        if (points == null)
+            throw new ArgumentNullException(nameof(points));
+
+        var copy = new List<LateralPathPoint>(points.Count);
+        for (int index = 0; index < points.Count; index++)
+        {
+            if (points[index] == null)
+                throw new ArgumentException("Lateral path points cannot contain null values.", nameof(points));
+            copy.Add(points[index]);
+        }
+        Points = new ReadOnlyCollection<LateralPathPoint>(copy);
+        IsIndependentlyValidated = independentlyValidated;
+    }
+
+    public IReadOnlyList<LateralPathPoint> Points { get; }
+
+    public bool IsIndependentlyValidated { get; }
+}

ClumsyPilot/ParkrobTrajplanner/EMPlanner/Lateral/LateralPathPoint.cs

@@ -0,0 +1,61 @@
+using System;
+
+namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
+
+/// <summary>Immutable world-space sample reconstructed from one lateral candidate.</summary>
+public sealed class LateralPathPoint
+{
+    public LateralPathPoint(double referenceS, double pathS, double l, double dl, double ddl, double dddl,
+        double x, double y, double vehicleYaw, double geometricCurvature, double vehicleCurvature,
+        double vehicleCurvatureDerivative)
+    {
+        RequireFinite(referenceS, nameof(referenceS));
+        RequireFinite(pathS, nameof(pathS));
+        RequireFinite(l, nameof(l));
+        RequireFinite(dl, nameof(dl));
+        RequireFinite(ddl, nameof(ddl));
+        RequireFinite(dddl, nameof(dddl));
+        RequireFinite(x, nameof(x));
+        RequireFinite(y, nameof(y));
+        RequireFinite(vehicleYaw, nameof(vehicleYaw));
+        RequireFinite(geometricCurvature, nameof(geometricCurvature));
+        RequireFinite(vehicleCurvature, nameof(vehicleCurvature));
+        RequireFinite(vehicleCurvatureDerivative, nameof(vehicleCurvatureDerivative));
+        if (referenceS < 0d)
+            throw new ArgumentOutOfRangeException(nameof(referenceS));
+        if (pathS < 0d)
+            throw new ArgumentOutOfRangeException(nameof(pathS));
+
+        ReferenceS = referenceS;
+        PathS = pathS;
+        L = l;
+        DL = dl;
+        DDL = ddl;
+        DDDL = dddl;
+        X = x;
+        Y = y;
+        VehicleYaw = vehicleYaw;
+        GeometricCurvature = geometricCurvature;
+        VehicleCurvature = vehicleCurvature;
+        VehicleCurvatureDerivative = vehicleCurvatureDerivative;
+    }
+
+    public double ReferenceS { get; }
+    public double PathS { get; }
+    public double L { get; }
+    public double DL { get; }
+    public double DDL { get; }
+    public double DDDL { get; }
+    public double X { get; }
+    public double Y { get; }
+    public double VehicleYaw { get; }
+    public double GeometricCurvature { get; }
+    public double VehicleCurvature { get; }
+    public double VehicleCurvatureDerivative { get; }
+
+    private static void RequireFinite(double value, string parameterName)
+    {
+        if (double.IsNaN(value) || double.IsInfinity(value))
+            throw new ArgumentOutOfRangeException(parameterName);
+    }
+}

ClumsyPilot/ParkrobTrajplanner/EMPlanner/Lateral/LateralPlanningInput.cs

@@ -0,0 +1,140 @@
+using System;
+using System.Collections.Generic;
+using System.Collections.ObjectModel;
+using MultiWheelC.TrajectoryPlanning.CoarsePath;
+using MultiWheelC.TrajectoryPlanning.CoarsePath.Vehicle;
+
+namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
+
+/// <summary>Immutable inputs for one lateral solve over a single direction segment.</summary>
+public sealed class LateralPlanningInput
+{
+    private const double Epsilon = 1e-12d;
+
+    public LateralPlanningInput(DirectionSegmentView referenceSegment, StaticCorridor corridor,
+        FrenetProjection startProjection, EmTerminalType terminalType, VehicleParameters vehicle,
+        EmPlannerConfiguration configuration, IReadOnlyList<FrenetProjection> previousTrajectorySeed)
+    {
+        if (referenceSegment == null)
+            throw new ArgumentNullException(nameof(referenceSegment));
+        if (corridor == null)
+            throw new ArgumentNullException(nameof(corridor));
+        if (startProjection == null)
+            throw new ArgumentNullException(nameof(startProjection));
+        if (vehicle == null)
+            throw new ArgumentNullException(nameof(vehicle));
+        if (configuration == null)
+            throw new ArgumentNullException(nameof(configuration));
+        if (!Enum.IsDefined(typeof(EmTerminalType), terminalType))
+            throw new ArgumentOutOfRangeException(nameof(terminalType));
+
+        ReferenceSegment = referenceSegment;
+        Corridor = CopyCorridor(corridor);
+        ReferenceStations = CopyStationValues(Corridor.Stations);
+        ValidateCorridorMatchesInput(referenceSegment, Corridor.Stations, startProjection);
+        StartProjection = startProjection;
+        TerminalType = terminalType;
+        Vehicle = CopyVehicle(vehicle);
+        Configuration = configuration.Copy();
+        PreviousTrajectorySeed = CopySeed(previousTrajectorySeed, referenceSegment);
+    }
+
+    public DirectionSegmentView ReferenceSegment { get; }
+
+    public StaticCorridor Corridor { get; }
+
+    public IReadOnlyList<double> ReferenceStations { get; }
+
+    public FrenetProjection StartProjection { get; }
+
+    public EmTerminalType TerminalType { get; }
+
+    public VehicleParameters Vehicle { get; }
+
+    public EmPlannerConfiguration Configuration { get; }
+
+    public IReadOnlyList<FrenetProjection> PreviousTrajectorySeed { get; }
+
+    private static StaticCorridor CopyCorridor(StaticCorridor source)
+    {
+        var stations = new List<LateralInterval>(source.Stations.Count);
+        for (int index = 0; index < source.Stations.Count; index++)
+        {
+            LateralInterval station = source.Stations[index];
+            if (station == null)
+                throw new ArgumentException("Corridor stations cannot be null.", nameof(source));
+            stations.Add(new LateralInterval(station.ReferenceS, station.MinimumL, station.MaximumL, station.SeedL));
+        }
+        return new StaticCorridor(stations);
+    }
+
+    private static IReadOnlyList<double> CopyStationValues(IReadOnlyList<LateralInterval> stations)
+    {
+        if (stations.Count < 2)
+            throw new ArgumentException("A lateral planning input requires at least two corridor stations.", nameof(stations));
+
+        var copy = new List<double>(stations.Count);
+        double previous = double.NegativeInfinity;
+        for (int index = 0; index < stations.Count; index++)
+        {
+            double referenceS = stations[index].ReferenceS;
+            if (referenceS <= previous)
+                throw new ArgumentException("Corridor reference-S stations must be strictly increasing.", nameof(stations));
+            copy.Add(referenceS);
+            previous = referenceS;
+        }
+        return new ReadOnlyCollection<double>(copy);
+    }
+
+    private static void ValidateCorridorMatchesInput(DirectionSegmentView segment, IReadOnlyList<LateralInterval> stations,
+        FrenetProjection start)
+    {
+        if (start.ReferencePoint == null || start.ReferencePoint.Direction != segment.Direction ||
+            start.ReferenceS < -Epsilon || start.ReferenceS > segment.LengthMeters + Epsilon)
+        {
+            throw new ArgumentException("Start projection must belong to the selected direction segment.", nameof(start));
+        }
+        if (Math.Abs(stations[0].ReferenceS - start.ReferenceS) > Epsilon)
+            throw new ArgumentException("The first corridor station must match the start projection reference S.", nameof(stations));
+        if (start.LateralOffset < stations[0].MinimumL - Epsilon || start.LateralOffset > stations[0].MaximumL + Epsilon)
+            throw new ArgumentException("Start projection is outside the first hard corridor interval.", nameof(start));
+
+        for (int index = 0; index < stations.Count; index++)
+        {
+            if (stations[index].ReferenceS < -Epsilon || stations[index].ReferenceS > segment.LengthMeters + Epsilon)
+                throw new ArgumentException("Corridor stations must lie inside the selected direction segment.", nameof(stations));
+        }
+    }
+
+    private static IReadOnlyList<FrenetProjection> CopySeed(IReadOnlyList<FrenetProjection> seed,
+        DirectionSegmentView segment)
+    {
+        var copy = new List<FrenetProjection>(seed == null ? 0 : seed.Count);
+        if (seed != null)
+        {
+            for (int index = 0; index < seed.Count; index++)
+            {
+                FrenetProjection projection = seed[index];
+                if (projection == null || projection.ReferencePoint.Direction != segment.Direction ||
+                    projection.ReferenceS < -Epsilon || projection.ReferenceS > segment.LengthMeters + Epsilon)
+                {
+                    throw new ArgumentException("Previous lateral seed must belong to the selected direction segment.", nameof(seed));
+                }
+                copy.Add(projection);
+            }
+        }
+        return new ReadOnlyCollection<FrenetProjection>(copy);
+    }
+
+    private static VehicleParameters CopyVehicle(VehicleParameters vehicle)
+    {
+        return new VehicleParameters
+        {
+            LengthMeters = vehicle.LengthMeters,
+            WidthMeters = vehicle.WidthMeters,
+            SafetyMarginMeters = vehicle.SafetyMarginMeters,
+            MaximumCurvaturePerMeter = vehicle.MaximumCurvaturePerMeter,
+            MinimumTurningRadiusMeters = vehicle.MinimumTurningRadiusMeters,
+        };
+    }
+}

ClumsyPilot/ParkrobTrajplanner/EMPlanner/Lateral/LateralPlanningResult.cs

@@ -0,0 +1,29 @@
+using System;
+
+namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
+
+/// <summary>Result of the lateral stage; only independently validated paths may be successful.</summary>
+public sealed class LateralPlanningResult
+{
+    public LateralPlanningResult(EmPlanningStatus status, LateralPath path, string failureReason)
+    {
+        if (!Enum.IsDefined(typeof(EmPlanningStatus), status))
+            throw new ArgumentOutOfRangeException(nameof(status));
+
+        bool successful = status == EmPlanningStatus.Success || status == EmPlanningStatus.SuccessWithFallback;
+        if (successful && (path == null || path.Points.Count == 0 || !path.IsIndependentlyValidated))
+            throw new ArgumentException("Successful lateral results require a non-empty independently validated path.", nameof(path));
+        if (!successful && path != null)
+            throw new ArgumentException("Failed lateral results cannot contain a path.", nameof(path));
+
+        Status = status;
+        Path = path;
+        FailureReason = failureReason ?? string.Empty;
+    }
+
+    public EmPlanningStatus Status { get; }
+
+    public LateralPath Path { get; }
+
+    public string FailureReason { get; }
+}

ClumsyPilot/ParkrobTrajplanner/EMPlanner/Lateral/LateralVariableLayout.cs

@@ -0,0 +1,63 @@
+using System;
+
+namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
+
+/// <summary>Deterministic variable ranges for one lateral QP discretized on N stations.</summary>
+public sealed class LateralVariableLayout
+{
+    public LateralVariableLayout(int stationCount)
+    {
+        if (stationCount < 2)
+            throw new ArgumentOutOfRangeException(nameof(stationCount), "At least two reference-S stations are required.");
+
+        StationCount = stationCount;
+        LStart = 0;
+        DLStart = stationCount;
+        DDLStart = 2 * stationCount;
+        DDDLStart = 3 * stationCount;
+        VariableCount = 4 * stationCount - 1;
+    }
+
+    public int StationCount { get; }
+
+    public int LStart { get; }
+
+    public int DLStart { get; }
+
+    public int DDLStart { get; }
+
+    public int DDDLStart { get; }
+
+    public int VariableCount { get; }
+
+    public int L(int stationIndex)
+    {
+        RequireStationIndex(stationIndex);
+        return LStart + stationIndex;
+    }
+
+    public int DL(int stationIndex)
+    {
+        RequireStationIndex(stationIndex);
+        return DLStart + stationIndex;
+    }
+
+    public int DDL(int stationIndex)
+    {
+        RequireStationIndex(stationIndex);
+        return DDLStart + stationIndex;
+    }
+
+    public int DDDL(int intervalIndex)
+    {
+        if (intervalIndex < 0 || intervalIndex >= StationCount - 1)
+            throw new ArgumentOutOfRangeException(nameof(intervalIndex));
+        return DDDLStart + intervalIndex;
+    }
+
+    private void RequireStationIndex(int stationIndex)
+    {
+        if (stationIndex < 0 || stationIndex >= StationCount)
+            throw new ArgumentOutOfRangeException(nameof(stationIndex));
+    }
+}

ClumsyPilot/tests/EMPlannerVerificationHost/LateralModelChecks.cs

@@ -0,0 +1,182 @@
+using System;
+using System.Collections.Generic;
+using EMPlannerVerificationHost;
+using MultiWheelC.TrajectoryPlanning.CoarsePath;
+using MultiWheelC.TrajectoryPlanning.CoarsePath.Vehicle;
+using MultiWheelC.TrajectoryPlanning.PathSmoothing;
+
+namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
+
+internal static class LateralModelChecks
+{
+    public static void Run()
+    {
+        VerifiesDeterministicVariableLayout();
+        VerifiesExactDiscreteDynamicsForUnequalStations();
+        VerifiesPlanningInputBoundariesAndDefensiveCopies();
+        VerifiesLateralResultPublicationContract();
+    }
+
+    private static void VerifiesDeterministicVariableLayout()
+    {
+        LateralVariableLayout layout = CreateLayout(4);
+
+        Verification.Equal(15, layout.VariableCount, "layout variable count");
+        for (int index = 0; index < 4; index++)
+        {
+            Verification.Equal(index, layout.L(index), "l index " + index);
+            Verification.Equal(4 + index, layout.DL(index), "dl index " + index);
+            Verification.Equal(8 + index, layout.DDL(index), "ddl index " + index);
+        }
+        for (int index = 0; index < 3; index++)
+            Verification.Equal(12 + index, layout.DDDL(index), "dddl index " + index);
+
+        ExpectArgumentException(() => CreateLayout(1), "layout rejects fewer than two stations");
+        ExpectArgumentException(() => layout.L(4), "l index bounds check");
+        ExpectArgumentException(() => layout.DL(-1), "dl index bounds check");
+        ExpectArgumentException(() => layout.DDL(4), "ddl index bounds check");
+        ExpectArgumentException(() => layout.DDDL(3), "dddl index bounds check");
+    }
+
+    private static void VerifiesExactDiscreteDynamicsForUnequalStations()
+    {
+        double[] stations = { 0d, 0.4d, 1.25d, 2.5d };
+        double[] jerks = { 0.5d, -0.3d, 0.2d };
+        LateralCandidate candidate = LateralCandidate.Integrate(stations, 0.1d, -0.2d, 0.3d, jerks);
+
+        for (int index = 0; index < jerks.Length; index++)
+        {
+            double ds = stations[index + 1] - stations[index];
+            Verification.NearlyEqual(candidate.DDL[index] + ds * candidate.DDDL[index], candidate.DDL[index + 1],
+                "exact ddl integration " + index);
+            Verification.NearlyEqual(candidate.DL[index] + ds * candidate.DDL[index] + 0.5d * ds * ds * candidate.DDDL[index],
+                candidate.DL[index + 1], "exact dl integration " + index);
+            Verification.NearlyEqual(candidate.L[index] + ds * candidate.DL[index] + 0.5d * ds * ds * candidate.DDL[index] +
+                ds * ds * ds * candidate.DDDL[index] / 6d, candidate.L[index + 1], "exact l integration " + index);
+        }
+        Verification.True(candidate.SatisfiesExactDiscreteDynamics(1e-12d), "integrated candidate validates exact dynamics");
+
+        LateralCandidate inconsistent = new LateralCandidate(new[] { 0d, 1d }, new[] { 0d, 1d },
+            new[] { 0d, 0d }, new[] { 0d, 0d }, new[] { 0d });
+        Verification.True(!inconsistent.SatisfiesExactDiscreteDynamics(1e-12d), "candidate detects inconsistent dynamics");
+        ExpectArgumentException(() => new LateralCandidate(new[] { 0d, 0d }, new[] { 0d, 0d },
+            new[] { 0d, 0d }, new[] { 0d, 0d }, new[] { 0d }), "candidate rejects non-increasing stations");
+    }
+
+    private static void VerifiesPlanningInputBoundariesAndDefensiveCopies()
+    {
+        DirectionSegmentView segment = CreateStraightSegment();
+        var corridorStations = new[]
+        {
+            new LateralInterval(0d, -0.3d, 0.3d, 0d),
+            new LateralInterval(1d, -0.3d, 0.3d, 0d),
+            new LateralInterval(2d, -0.3d, 0.3d, 0d),
+        };
+        var seeds = new[]
+        {
+            new FrenetProjection(ReferencePathInterpolator.Interpolate(segment, 0d), 0d, 0d, 0d),
+        };
+        LateralPlanningInput input = new LateralPlanningInput(segment, new StaticCorridor(corridorStations), seeds[0],
+            EmTerminalType.Goal, CreateVehicle(), EmPlannerConfiguration.CreateDefault(), seeds);
+        corridorStations[1] = new LateralInterval(1d, -0.1d, 0.1d, 0d);
+        seeds[0] = new FrenetProjection(ReferencePathInterpolator.Interpolate(segment, 0d), 0.2d, 0d, 0d);
+
+        Verification.NearlyEqual(-0.3d, input.Corridor.Stations[1].MinimumL, "input copies corridor stations");
+        Verification.NearlyEqual(0d, input.PreviousTrajectorySeed[0].LateralOffset, "input copies seed list");
+        ExpectArgumentException(() => new LateralPlanningInput(segment,
+            new StaticCorridor(new[] { new LateralInterval(0d, -0.3d, 0.3d, 0d) }),
+            input.StartProjection, EmTerminalType.Goal, CreateVehicle(), EmPlannerConfiguration.CreateDefault(), Array.Empty<FrenetProjection>()),
+            "input rejects fewer than two stations");
+        ExpectArgumentException(() => new LateralPlanningInput(segment,
+            new StaticCorridor(new[]
+            {
+                new LateralInterval(0d, -0.3d, 0.3d, 0d),
+                new LateralInterval(0d, -0.3d, 0.3d, 0d),
+            }), input.StartProjection, EmTerminalType.Goal, CreateVehicle(), EmPlannerConfiguration.CreateDefault(), Array.Empty<FrenetProjection>()),
+            "input rejects non-increasing corridor stations");
+        ExpectArgumentException(() => new LateralPlanningInput(segment,
+            new StaticCorridor(new[]
+            {
+                new LateralInterval(0.1d, -0.3d, 0.3d, 0d),
+                new LateralInterval(2d, -0.3d, 0.3d, 0d),
+            }), input.StartProjection, EmTerminalType.Goal, CreateVehicle(), EmPlannerConfiguration.CreateDefault(), Array.Empty<FrenetProjection>()),
+            "input rejects start-corridor station mismatch");
+        ExpectArgumentException(() => new LateralPlanningInput(segment,
+            new StaticCorridor(new[]
+            {
+                new LateralInterval(0d, -0.1d, 0.1d, 0d),
+                new LateralInterval(2d, -0.1d, 0.1d, 0d),
+            }), new FrenetProjection(ReferencePathInterpolator.Interpolate(segment, 0d), 0.2d, 0d, 0d),
+            EmTerminalType.Goal, CreateVehicle(), EmPlannerConfiguration.CreateDefault(), Array.Empty<FrenetProjection>()),
+            "input rejects start projection outside first hard interval");
+    }
+
+    private static void VerifiesLateralResultPublicationContract()
+    {
+        LateralPath unvalidated = new LateralPath(new[] { CreatePathPoint(0d) }, false);
+        LateralPath validated = new LateralPath(new[] { CreatePathPoint(0d), CreatePathPoint(1d) }, true);
+
+        ExpectArgumentException(() => new LateralPlanningResult(EmPlanningStatus.Success, unvalidated, string.Empty),
+            "success requires an independently validated path");
+        ExpectArgumentException(() => new LateralPlanningResult(EmPlanningStatus.SuccessWithFallback,
+            new LateralPath(Array.Empty<LateralPathPoint>(), true), string.Empty),
+            "fallback requires a non-empty path");
+        ExpectArgumentException(() => new LateralPlanningResult(EmPlanningStatus.LateralInfeasible, validated, string.Empty),
+            "failed result has no candidate");
+        LateralPlanningResult result = new LateralPlanningResult(EmPlanningStatus.SuccessWithFallback, validated, "fallback");
+        Verification.Equal(validated, result.Path, "fallback path is preserved");
+    }
+
+    private static DirectionSegmentView CreateStraightSegment()
+    {
+        var points = new List<SmoothedPathPoint>
+        {
+            Point(0d, 0d),
+            Point(1d, 1d),
+            Point(2d, 2d),
+        };
+        return new DirectionSegmentView(0, TravelDirection.Forward, points,
+            new ReferenceBoundary(0, 0d, EmBoundaryType.None, 0d),
+            new ReferenceBoundary(0, 2d, EmBoundaryType.Goal, 2d), 0d);
+    }
+
+    private static SmoothedPathPoint Point(double x, double s)
+    {
+        return new SmoothedPathPoint(x, 0d, 0d, 0d, s, TravelDirection.Forward, 0d, 0d, 0d, 1d,
+            false, SmoothedPathPointSource.Anchor);
+    }
+
+    private static VehicleParameters CreateVehicle()
+    {
+        return new VehicleParameters
+        {
+            LengthMeters = 0.1d,
+            WidthMeters = 0.1d,
+            SafetyMarginMeters = 0d,
+            MaximumCurvaturePerMeter = 1d,
+        };
+    }
+
+    private static LateralPathPoint CreatePathPoint(double referenceS)
+    {
+        return new LateralPathPoint(referenceS, referenceS, 0d, 0d, 0d, 0d, referenceS, 0d, 0d, 0d, 0d, 0d);
+    }
+
+    private static LateralVariableLayout CreateLayout(int stationCount)
+    {
+        return new LateralVariableLayout(stationCount);
+    }
+
+    private static void ExpectArgumentException(Action action, string name)
+    {
+        try
+        {
+            action();
+        }
+        catch (ArgumentException)
+        {
+            return;
+        }
+        throw new InvalidOperationException(name + " did not throw ArgumentException.");
+    }
+}

ClumsyPilot/tests/EMPlannerVerificationHost/Program.cs

@@ -8,9 +8,9 @@ internal static class Program
     {
         if (args.Length != 1 || (args[0] != "foundation" && args[0] != "segmentation" && args[0] != "frenet" &&
             args[0] != "corridor" && args[0] != "optimization" && args[0] != "osqp" && args[0] != "osqp-loader" && args[0] != "osqp-probe" &&
-            args[0] != "all-foundation"))
+            args[0] != "all-foundation" && args[0] != "lateral-model"))
         {
-            Console.Error.WriteLine("Usage: EMPlannerVerificationHost foundation|segmentation|frenet|corridor|optimization|osqp|osqp-loader|all-foundation");
+            Console.Error.WriteLine("Usage: EMPlannerVerificationHost foundation|segmentation|frenet|corridor|optimization|osqp|osqp-loader|all-foundation|lateral-model");
             return 2;
         }
 
@@ -50,6 +50,11 @@ internal static class Program
             {
                 MultiWheelC.TrajectoryPlanning.EMPlanner.OsqpChecks.RunProbe();
             }
+            if (args[0] == "lateral-model")
+            {
+                MultiWheelC.TrajectoryPlanning.EMPlanner.LateralModelChecks.Run();
+                Console.WriteLine("PASS lateral-model");
+            }
             return 0;
         }
         catch (Exception exception)