test: reproduce EM PathS resampling regression

ClumsyPilot/tests/EMPlannerVerificationHost/TrajectoryChecks.cs

@@ -20,6 +20,8 @@ internal static class TrajectoryChecks
         VerifiesWorldSpacePublicationMutationsAreRejected();
         VerifiesTerminalPoseAndSampleLimitPublicationGates();
         VerifiesReverseSpeedLimitUsesReverseConfiguration();
+        VerifiesFullScopeNormalizesToleranceSizedPathSResiduals();
+        VerifiesFullScopeRejectsMaterialPathSRegressionWithDiagnostics();
     }
 
     private static void VerifiesForwardFieldsExactTerminalAndHold()
@@ -479,11 +481,71 @@ internal static class TrajectoryChecks
             new LongitudinalCandidate(times, pathS, speed, acceleration, jerk), string.Empty);
     }
 
+    private static void VerifiesFullScopeNormalizesToleranceSizedPathSResiduals()
+    {
+        EmPlannerConfiguration configuration = EmPlannerConfiguration.CreateDefault();
+        configuration.Scheduling.OutputTimeStepSeconds = 0.1d;
+        var candidate = new LongitudinalCandidate(
+            new[] { 0d, 0.1d, 0.2d, 0.3d },
+            new[] { 0d, 0.05d, 0.05d - 0.5d * configuration.Validation.KinematicTolerance, 0.10d },
+            new[] { 0.5d, 0d, 0d, 0d },
+            new[] { 0d, 0d, 0d, 0d },
+            new[] { 0d, 0d, 0d });
+
+        EmTrajectory trajectory = new EmTrajectoryAssembler(configuration).Assemble(
+            CreatePath(TravelDirection.Forward, 0d, 0d),
+            new LongitudinalPlanningResult(EmPlanningStatus.Success, candidate, string.Empty),
+            CreateMetadata(TravelDirection.Forward, EmTerminalType.RollingSafetyStop,
+                EmLongitudinalMode.RollingContinuation, EmPlanningScope.FullDirectionSegment));
+
+        for (int index = 1; index < trajectory.Points.Count; index++)
+        {
+            Verification.True(trajectory.Points[index].PathS >= trajectory.Points[index - 1].PathS,
+                "full-scope numerical residual is normalized at sample " + index);
+        }
+        Verification.True(trajectory.Points[trajectory.Points.Count - 1].PathS <= candidate.S[candidate.S.Count - 1],
+            "full-scope publication remains within terminal PathS");
+    }
+
+    private static void VerifiesFullScopeRejectsMaterialPathSRegressionWithDiagnostics()
+    {
+        EmPlannerConfiguration configuration = EmPlannerConfiguration.CreateDefault();
+        configuration.Scheduling.OutputTimeStepSeconds = 0.1d;
+        var candidate = new LongitudinalCandidate(
+            new[] { 0d, 0.1d, 0.2d, 0.3d },
+            new[] { 0d, 0.05d, 0.04d, 0.10d },
+            new[] { 0.5d, 0d, 0d, 0d },
+            new[] { 0d, 0d, 0d, 0d },
+            new[] { 0d, 0d, 0d });
+
+        try
+        {
+            new EmTrajectoryAssembler(configuration).Assemble(
+                CreatePath(TravelDirection.Forward, 0d, 0d),
+                new LongitudinalPlanningResult(EmPlanningStatus.Success, candidate, string.Empty),
+                CreateMetadata(TravelDirection.Forward, EmTerminalType.RollingSafetyStop,
+                    EmLongitudinalMode.RollingContinuation, EmPlanningScope.FullDirectionSegment));
+            throw new InvalidOperationException("Material PathS regression was accepted.");
+        }
+        catch (ArgumentException exception)
+        {
+            Verification.True(exception.Message.Contains("sampleIndex=2"),
+                "PathS diagnostic contains sample index");
+            Verification.True(exception.Message.Contains("previousPathS="),
+                "PathS diagnostic contains previous value");
+            Verification.True(exception.Message.Contains("candidatePathS="),
+                "PathS diagnostic contains candidate value");
+            Verification.True(exception.Message.Contains("difference="),
+                "PathS diagnostic contains difference");
+        }
+    }
+
     private static EmTrajectoryMetadata CreateMetadata(TravelDirection direction, EmTerminalType terminalType,
-        EmLongitudinalMode longitudinalMode = EmLongitudinalMode.ExactStopAtBoundary)
+        EmLongitudinalMode longitudinalMode = EmLongitudinalMode.ExactStopAtBoundary,
+        EmPlanningScope planningScope = EmPlanningScope.RollingHorizon)
     {
         return new EmTrajectoryMetadata("trajectory", DateTimeOffset.UnixEpoch, DateTimeOffset.UnixEpoch, 3L,
             "reference", 4L, string.Empty, 2, direction, terminalType, longitudinalMode,
-            EmPlanningScope.RollingHorizon);
+            planningScope);
     }
 }