feat: preserve EM planner segment boundaries

ClumsyPilot/tests/EMPlannerVerificationHost/EmFixtureFactory.cs

@@ -0,0 +1,41 @@
+using System;
+using System.Collections.Generic;
+using MultiWheelC.TrajectoryPlanning.CoarsePath;
+using MultiWheelC.TrajectoryPlanning.PathSmoothing;
+
+namespace EMPlannerVerificationHost;
+
+internal static class EmFixtureFactory
+{
+    public static PathSmoothingResult CreateGearPairReferencePath()
+    {
+        var points = new List<SmoothedPathPoint>
+        {
+            Point(0d, 0d, TravelDirection.Forward, false, SmoothedPathPointSource.Anchor),
+            Point(1d, 1d, TravelDirection.Forward, false, SmoothedPathPointSource.Anchor),
+            Point(2d, 2d, TravelDirection.Forward, false, SmoothedPathPointSource.Anchor),
+            Point(2d, 2d, TravelDirection.Reverse, true, SmoothedPathPointSource.GearSwitch),
+            Point(1d, 3d, TravelDirection.Reverse, false, SmoothedPathPointSource.Anchor),
+            Point(0d, 4d, TravelDirection.Reverse, false, SmoothedPathPointSource.Anchor),
+        };
+        var segments = new List<SmoothedPathSegment>
+        {
+            new SmoothedPathSegment(0, TravelDirection.Forward, 0, 2, false, true),
+            new SmoothedPathSegment(1, TravelDirection.Reverse, 3, 5, true, false),
+        };
+        var metrics = new PathQualityMetrics(true, 4d, 0d, 0d, 0d, 0d, 1d, 0d, 0d, 0d, 0d, 0d);
+        return PathSmoothingResult.PublishLocalG2(PathSmoothingStatus.Complete, points, segments,
+            new PathSmoothingDiagnostics(metrics, TimeSpan.Zero), new List<PathSmoothingRegionReport>());
+    }
+
+    private static SmoothedPathPoint Point(
+        double x,
+        double arcLength,
+        TravelDirection direction,
+        bool isGearSwitchPoint,
+        SmoothedPathPointSource source)
+    {
+        return new SmoothedPathPoint(x, 0d, 0d, 0d, arcLength, direction, 0d, 0d, 0d, 1d,
+            isGearSwitchPoint, source);
+    }
+}

ClumsyPilot/tests/EMPlannerVerificationHost/Program.cs

@@ -6,16 +6,24 @@ internal static class Program
 {
     private static int Main(string[] args)
     {
-        if (args.Length != 1 || args[0] != "foundation")
+        if (args.Length != 1 || (args[0] != "foundation" && args[0] != "segmentation"))
         {
-            Console.Error.WriteLine("Usage: EMPlannerVerificationHost foundation");
+            Console.Error.WriteLine("Usage: EMPlannerVerificationHost foundation|segmentation");
             return 2;
         }
 
         try
         {
-            MultiWheelC.TrajectoryPlanning.EMPlanner.FoundationChecks.Run();
-            Console.WriteLine("PASS foundation");
+            if (args[0] == "foundation")
+            {
+                MultiWheelC.TrajectoryPlanning.EMPlanner.FoundationChecks.Run();
+                Console.WriteLine("PASS foundation");
+            }
+            else
+            {
+                MultiWheelC.TrajectoryPlanning.EMPlanner.SegmentationChecks.Run();
+                Console.WriteLine("PASS segmentation");
+            }
             return 0;
         }
         catch (Exception exception)

ClumsyPilot/tests/EMPlannerVerificationHost/SegmentationChecks.cs

@@ -0,0 +1,46 @@
+using MultiWheelC.TrajectoryPlanning.EMPlanner;
+
+namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
+
+internal static class SegmentationChecks
+{
+    public static void Run()
+    {
+        var referencePath = EMPlannerVerificationHost.EmFixtureFactory.CreateGearPairReferencePath();
+        var segments = ReferencePathSegmenter.Create(referencePath);
+
+        EMPlannerVerificationHost.Verification.Equal(2, segments.Count, "segment count");
+        EMPlannerVerificationHost.Verification.Equal(EmBoundaryType.GearSwitchApproach,
+            segments[0].EndBoundary.BoundaryType, "forward end boundary");
+        EMPlannerVerificationHost.Verification.Equal(EmBoundaryType.GearSwitchDeparture,
+            segments[1].StartBoundary.BoundaryType, "reverse start boundary");
+        EMPlannerVerificationHost.Verification.True(!segments[0].EndBoundary.Equals(segments[1].StartBoundary),
+            "gear-pair boundary identities differ");
+        EMPlannerVerificationHost.Verification.NearlyEqual(2d, referencePath.Path[2].ArcLength, "forward source arc length");
+        EMPlannerVerificationHost.Verification.NearlyEqual(2d, referencePath.Path[3].ArcLength, "reverse source arc length");
+        EMPlannerVerificationHost.Verification.NearlyEqual(0d, segments[1].Points[0].ArcLength, "rebased reverse start");
+
+        ReferenceHorizonSlice rolling = ReferenceHorizonSlicer.Slice(segments[0], 1.95d);
+        EMPlannerVerificationHost.Verification.Equal(EmBoundaryType.RollingSafetyStop,
+            rolling.TerminalBoundary.BoundaryType, "rolling terminal type");
+        EMPlannerVerificationHost.Verification.NearlyEqual(1.95d, rolling.TerminalBoundary.SegmentLocalS,
+            "rolling terminal local s");
+        EMPlannerVerificationHost.Verification.NearlyEqual(1.95d, rolling.Points[rolling.Points.Count - 1].ArcLength,
+            "rolling anchor path s");
+        EMPlannerVerificationHost.Verification.NearlyEqual(1.95d, rolling.Points[rolling.Points.Count - 1].X,
+            "rolling anchor x");
+
+        ReferenceHorizonSlice gearSwitch = ReferenceHorizonSlicer.Slice(segments[0], 2.05d);
+        EMPlannerVerificationHost.Verification.Equal(EmBoundaryType.GearSwitchApproach,
+            gearSwitch.TerminalBoundary.BoundaryType, "gear terminal type");
+        EMPlannerVerificationHost.Verification.NearlyEqual(2d, gearSwitch.TerminalBoundary.SegmentLocalS,
+            "gear terminal local s");
+        EMPlannerVerificationHost.Verification.NearlyEqual(2d, gearSwitch.Points[gearSwitch.Points.Count - 1].ArcLength,
+            "gear anchor path s");
+        for (int index = 0; index < gearSwitch.Points.Count; index++)
+        {
+            EMPlannerVerificationHost.Verification.Equal(segments[0].Direction, gearSwitch.Points[index].Direction,
+                "horizon point direction");
+        }
+    }
+}

ClumsyPilot/tests/EMPlannerVerificationHost/Verification.cs

@@ -15,4 +15,10 @@ internal static class Verification
         if (Math.Abs(expected - actual) > 1e-12d)
             throw new InvalidOperationException(name + " expected " + expected + " but was " + actual + ".");
     }
+
+    public static void True(bool condition, string name)
+    {
+        if (!condition)
+            throw new InvalidOperationException(name + " was false.");
+    }
 }