feat: add lateral optimization model

2026-08-04 00:50:10 +08:00
parent 113d0c9d9b
commit c225b17d37
8 changed files with 625 additions and 2 deletions
@@ -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,
+        };
+    }
+}