diff --git a/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Lateral/LateralGeometryEvaluator.cs b/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Lateral/LateralGeometryEvaluator.cs
new file mode 100644
index 0000000..2482794
--- /dev/null
+++ b/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Lateral/LateralGeometryEvaluator.cs
@@ -0,0 +1,201 @@
+using System;
+using System.Collections.Generic;
+using MultiWheelC.TrajectoryPlanning.CoarsePath;
+using MultiWheelC.TrajectoryPlanning.Utils;
+
+namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
+
+/// Reconstructs world geometry and actual path arc length from a lateral candidate.
+public sealed class LateralGeometryEvaluator
+{
+ public bool TryEvaluate(LateralPlanningInput input, LateralCandidate candidate, out LateralPath path,
+ out string failureReason)
+ {
+ path = null;
+ failureReason = string.Empty;
+ if (!HasMatchingStations(input, candidate, out failureReason))
+ return false;
+
+ try
+ {
+ List samples = Reconstruct(input, candidate, out failureReason);
+ if (samples == null)
+ return false;
+ CalculateActualPathSAndCurvatureDerivative(samples, out failureReason);
+ if (failureReason.Length != 0)
+ return false;
+
+ var points = new List(samples.Count);
+ for (int index = 0; index < samples.Count; index++)
+ {
+ GeometrySample sample = samples[index];
+ double dddl = candidate.DDDL[Math.Min(index, candidate.DDDL.Count - 1)];
+ points.Add(new LateralPathPoint(sample.ReferenceS, sample.PathS, sample.L, sample.DL, sample.DDL, dddl,
+ sample.X, sample.Y, sample.VehicleYaw, sample.GeometricCurvature, sample.VehicleCurvature,
+ sample.VehicleCurvatureDerivative));
+ }
+ path = new LateralPath(points, false);
+ return true;
+ }
+ catch (ArgumentException exception)
+ {
+ failureReason = exception.Message;
+ return false;
+ }
+ }
+
+ private static List Reconstruct(LateralPlanningInput input, LateralCandidate candidate,
+ out string failureReason)
+ {
+ failureReason = string.Empty;
+ double minimumDenominator = input.Configuration.Frenet.MinimumFrenetDenominator;
+ if (!IsFinite(minimumDenominator) || minimumDenominator <= 0d)
+ {
+ failureReason = "The minimum Frenet denominator is invalid.";
+ return null;
+ }
+
+ double directionSign = input.ReferenceSegment.Direction == TravelDirection.Forward ? 1d : -1d;
+ var samples = new List(candidate.ReferenceStations.Count);
+ for (int index = 0; index < candidate.ReferenceStations.Count; index++)
+ {
+ FrenetReferencePoint reference = ReferencePathInterpolator.Interpolate(input.ReferenceSegment,
+ candidate.ReferenceStations[index]);
+ double l = candidate.L[index];
+ double dl = candidate.DL[index];
+ double ddl = candidate.DDL[index];
+ double denominator = 1d - reference.GeometricCurvature * l;
+ if (!IsFinite(denominator) || denominator < minimumDenominator)
+ {
+ failureReason = "Frenet denominator is below the hard minimum at station " + index + ".";
+ return null;
+ }
+
+ double travelYaw = reference.TravelYaw + Math.Atan2(dl, denominator);
+ double vehicleYaw = input.ReferenceSegment.Direction == TravelDirection.Forward
+ ? AngleMath.NormalizeRadians(travelYaw)
+ : AngleMath.NormalizeRadians(travelYaw + Math.PI);
+ double x = reference.X - l * Math.Sin(reference.TravelYaw);
+ double y = reference.Y + l * Math.Cos(reference.TravelYaw);
+ double geometricCurvature = CalculateGeometricCurvature(reference, l, dl, ddl,
+ directionSign * reference.VehicleCurvatureDerivative);
+ double vehicleCurvature = directionSign * geometricCurvature;
+ if (!IsFinite(travelYaw) || !IsFinite(vehicleYaw) || !IsFinite(x) || !IsFinite(y) ||
+ !IsFinite(geometricCurvature) || !IsFinite(vehicleCurvature))
+ {
+ failureReason = "Reconstructed lateral geometry is non-finite at station " + index + ".";
+ return null;
+ }
+ samples.Add(new GeometrySample(candidate.ReferenceStations[index], l, dl, ddl, x, y, travelYaw, vehicleYaw,
+ geometricCurvature, vehicleCurvature));
+ }
+ return samples;
+ }
+
+ private static void CalculateActualPathSAndCurvatureDerivative(IReadOnlyList samples,
+ out string failureReason)
+ {
+ failureReason = string.Empty;
+ samples[0].PathS = 0d;
+ for (int index = 1; index < samples.Count; index++)
+ {
+ double dx = samples[index].X - samples[index - 1].X;
+ double dy = samples[index].Y - samples[index - 1].Y;
+ double chord = Math.Sqrt(dx * dx + dy * dy);
+ if (!IsFinite(chord) || chord <= 0d)
+ {
+ failureReason = "Reconstructed path S is not strictly increasing at station " + index + ".";
+ return;
+ }
+ samples[index].PathS = samples[index - 1].PathS + chord;
+ }
+ for (int index = 0; index < samples.Count; index++)
+ {
+ int lower = index == 0 ? 0 : index - 1;
+ int upper = index == samples.Count - 1 ? samples.Count - 1 : index + 1;
+ double span = samples[upper].PathS - samples[lower].PathS;
+ if (!IsFinite(span) || span <= 0d)
+ {
+ failureReason = "Path-S curvature derivative span is invalid at station " + index + ".";
+ return;
+ }
+ double derivative = (samples[upper].VehicleCurvature - samples[lower].VehicleCurvature) / span;
+ if (!IsFinite(derivative))
+ {
+ failureReason = "Vehicle curvature derivative is non-finite at station " + index + ".";
+ return;
+ }
+ samples[index].VehicleCurvatureDerivative = derivative;
+ }
+ }
+
+ private static bool HasMatchingStations(LateralPlanningInput input, LateralCandidate candidate, out string failureReason)
+ {
+ failureReason = string.Empty;
+ if (input == null || candidate == null)
+ {
+ failureReason = "Lateral input and candidate are required.";
+ return false;
+ }
+ if (candidate.ReferenceStations.Count != input.ReferenceStations.Count)
+ {
+ failureReason = "Candidate station count does not match the lateral input.";
+ return false;
+ }
+ for (int index = 0; index < input.ReferenceStations.Count; index++)
+ {
+ if (Math.Abs(candidate.ReferenceStations[index] - input.ReferenceStations[index]) > 1e-12d)
+ {
+ failureReason = "Candidate stations do not match the lateral input.";
+ return false;
+ }
+ }
+ return true;
+ }
+
+ internal static double CalculateGeometricCurvature(FrenetReferencePoint reference, double l, double dl, double ddl,
+ double referenceCurvatureDerivative)
+ {
+ double a = 1d - reference.GeometricCurvature * l;
+ double denominatorSquared = a * a + dl * dl;
+ double numerator = a * a * reference.GeometricCurvature + a * ddl +
+ referenceCurvatureDerivative * l * dl + 2d * reference.GeometricCurvature * dl * dl;
+ return numerator / (denominatorSquared * Math.Sqrt(denominatorSquared));
+ }
+
+ private static bool IsFinite(double value)
+ {
+ return !double.IsNaN(value) && !double.IsInfinity(value);
+ }
+
+ private sealed class GeometrySample
+ {
+ public GeometrySample(double referenceS, double l, double dl, double ddl, double x, double y, double travelYaw,
+ double vehicleYaw, double geometricCurvature, double vehicleCurvature)
+ {
+ ReferenceS = referenceS;
+ L = l;
+ DL = dl;
+ DDL = ddl;
+ X = x;
+ Y = y;
+ TravelYaw = travelYaw;
+ VehicleYaw = vehicleYaw;
+ GeometricCurvature = geometricCurvature;
+ VehicleCurvature = vehicleCurvature;
+ }
+
+ public double ReferenceS { get; }
+ public double L { get; }
+ public double DL { get; }
+ public double DDL { get; }
+ public double X { get; }
+ public double Y { get; }
+ public double TravelYaw { get; }
+ public double VehicleYaw { get; }
+ public double GeometricCurvature { get; }
+ public double VehicleCurvature { get; }
+ public double PathS { get; set; }
+ public double VehicleCurvatureDerivative { get; set; }
+ }
+}
diff --git a/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Lateral/LateralSolutionValidator.cs b/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Lateral/LateralSolutionValidator.cs
new file mode 100644
index 0000000..3de7692
--- /dev/null
+++ b/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Lateral/LateralSolutionValidator.cs
@@ -0,0 +1,318 @@
+using System;
+using System.Collections.Generic;
+using MultiWheelC.TrajectoryPlanning.CoarsePath;
+using MultiWheelC.TrajectoryPlanning.Utils;
+
+namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
+
+/// Independently recomputes and verifies lateral world geometry before a path may be published.
+public sealed class LateralSolutionValidator
+{
+ public bool TryValidate(LateralPlanningInput input, LateralCandidate candidate, LateralPath path,
+ out LateralPath validatedPath, out string failureReason)
+ {
+ validatedPath = null;
+ failureReason = string.Empty;
+ if (input == null || candidate == null || path == null)
+ {
+ failureReason = "Lateral input, candidate, and reconstructed path are required.";
+ return false;
+ }
+ if (path.Points.Count != input.ReferenceStations.Count || candidate.ReferenceStations.Count != path.Points.Count)
+ {
+ failureReason = "Lateral path point count does not match the candidate stations.";
+ return false;
+ }
+
+ try
+ {
+ double spatialTolerance = RequireNonnegative(input.Configuration.Validation.SpatialToleranceMeters,
+ "spatial tolerance");
+ double kinematicTolerance = RequireNonnegative(input.Configuration.Validation.KinematicTolerance,
+ "kinematic tolerance");
+ double residualTolerance = RequireNonnegative(input.Configuration.Solver.StrictResidualTolerance,
+ "strict residual tolerance");
+ if (!ValidateCandidateConstraints(input, candidate, spatialTolerance, kinematicTolerance, residualTolerance,
+ out failureReason))
+ {
+ return false;
+ }
+
+ List expected = ReconstructIndependently(input, candidate, out failureReason);
+ if (expected == null)
+ return false;
+ if (!ComparePath(path, candidate, input.ReferenceStations, expected, spatialTolerance, kinematicTolerance,
+ out failureReason))
+ {
+ return false;
+ }
+
+ validatedPath = new LateralPath(path.Points, true);
+ return true;
+ }
+ catch (ArgumentException exception)
+ {
+ failureReason = exception.Message;
+ return false;
+ }
+ }
+
+ private static bool ValidateCandidateConstraints(LateralPlanningInput input, LateralCandidate candidate,
+ double spatialTolerance, double kinematicTolerance, double residualTolerance, out string failureReason)
+ {
+ failureReason = string.Empty;
+ if (candidate.ReferenceStations.Count != input.ReferenceStations.Count)
+ {
+ failureReason = "Candidate station count does not match the input.";
+ return false;
+ }
+ if (!candidate.SatisfiesExactDiscreteDynamics(residualTolerance))
+ {
+ failureReason = "Candidate violates exact lateral dynamics.";
+ return false;
+ }
+
+ LateralConfiguration lateral = input.Configuration.Lateral;
+ double maximumCurvature = GetMaximumVehicleCurvature(input.Vehicle);
+ for (int index = 0; index < input.ReferenceStations.Count; index++)
+ {
+ if (Math.Abs(candidate.ReferenceStations[index] - input.ReferenceStations[index]) > spatialTolerance)
+ {
+ failureReason = "Candidate reference-S does not match the input at station " + index + ".";
+ return false;
+ }
+ LateralInterval corridor = input.Corridor.Stations[index];
+ double l = candidate.L[index];
+ double dl = candidate.DL[index];
+ double ddl = candidate.DDL[index];
+ if (!IsFinite(l) || !IsFinite(dl) || !IsFinite(ddl) || l < corridor.MinimumL - spatialTolerance ||
+ l > corridor.MaximumL + spatialTolerance || Math.Abs(l) > input.Configuration.Corridor.MaximumLateralOffsetMeters + spatialTolerance ||
+ Math.Abs(dl) > lateral.MaximumLateralSlope + kinematicTolerance ||
+ Math.Abs(ddl) > lateral.MaximumLateralSecondDerivativePerMeter + kinematicTolerance)
+ {
+ failureReason = "Candidate violates lateral corridor or derivative limits at station " + index + ".";
+ return false;
+ }
+ FrenetReferencePoint reference = ReferencePathInterpolator.Interpolate(input.ReferenceSegment,
+ input.ReferenceStations[index]);
+ double denominator = 1d - reference.GeometricCurvature * l;
+ if (!IsFinite(denominator) || denominator < input.Configuration.Frenet.MinimumFrenetDenominator - kinematicTolerance)
+ {
+ failureReason = "Candidate violates the Frenet denominator at station " + index + ".";
+ return false;
+ }
+ double geometricCurvature = CalculateGeometricCurvature(reference, l, dl, ddl,
+ (input.ReferenceSegment.Direction == TravelDirection.Forward ? 1d : -1d) * reference.VehicleCurvatureDerivative);
+ double vehicleCurvature = (input.ReferenceSegment.Direction == TravelDirection.Forward ? 1d : -1d) *
+ geometricCurvature;
+ if (!IsFinite(geometricCurvature) || !IsFinite(vehicleCurvature) ||
+ Math.Abs(vehicleCurvature) > maximumCurvature + kinematicTolerance)
+ {
+ failureReason = "Candidate violates vehicle curvature at station " + index + ".";
+ return false;
+ }
+ }
+ for (int index = 0; index < candidate.DDDL.Count; index++)
+ {
+ if (!IsFinite(candidate.DDDL[index]) ||
+ Math.Abs(candidate.DDDL[index]) > lateral.MaximumLateralThirdDerivativePerSquareMeter + kinematicTolerance)
+ {
+ failureReason = "Candidate violates third-derivative limits at interval " + index + ".";
+ return false;
+ }
+ }
+
+ double startDenominator = 1d - input.StartProjection.ReferencePoint.GeometricCurvature *
+ input.StartProjection.LateralOffset;
+ double expectedStartDL = startDenominator * Math.Tan(input.StartProjection.HeadingError);
+ if (!IsFinite(expectedStartDL) || Math.Abs(candidate.L[0] - input.StartProjection.LateralOffset) > spatialTolerance ||
+ Math.Abs(candidate.DL[0] - expectedStartDL) > kinematicTolerance)
+ {
+ failureReason = "Candidate violates the lateral start state.";
+ return false;
+ }
+ if (input.TerminalType != EmTerminalType.RollingSafetyStop &&
+ (Math.Abs(candidate.L[candidate.L.Count - 1]) > spatialTolerance ||
+ Math.Abs(candidate.DL[candidate.DL.Count - 1]) > kinematicTolerance))
+ {
+ failureReason = "Candidate violates the exact terminal lateral state.";
+ return false;
+ }
+ return true;
+ }
+
+ private static List ReconstructIndependently(LateralPlanningInput input, LateralCandidate candidate,
+ out string failureReason)
+ {
+ failureReason = string.Empty;
+ double minimumDenominator = input.Configuration.Frenet.MinimumFrenetDenominator;
+ double directionSign = input.ReferenceSegment.Direction == TravelDirection.Forward ? 1d : -1d;
+ var samples = new List(candidate.ReferenceStations.Count);
+ for (int index = 0; index < candidate.ReferenceStations.Count; index++)
+ {
+ FrenetReferencePoint reference = ReferencePathInterpolator.Interpolate(input.ReferenceSegment,
+ candidate.ReferenceStations[index]);
+ double l = candidate.L[index];
+ double dl = candidate.DL[index];
+ double ddl = candidate.DDL[index];
+ double denominator = 1d - reference.GeometricCurvature * l;
+ if (!IsFinite(denominator) || denominator < minimumDenominator)
+ {
+ failureReason = "Independent reconstruction found a Frenet denominator violation at station " + index + ".";
+ return null;
+ }
+ double travelYaw = reference.TravelYaw + Math.Atan2(dl, denominator);
+ double vehicleYaw = input.ReferenceSegment.Direction == TravelDirection.Forward
+ ? AngleMath.NormalizeRadians(travelYaw)
+ : AngleMath.NormalizeRadians(travelYaw + Math.PI);
+ double geometricCurvature = CalculateGeometricCurvature(reference, l, dl, ddl,
+ directionSign * reference.VehicleCurvatureDerivative);
+ double vehicleCurvature = directionSign * geometricCurvature;
+ double x = reference.X - l * Math.Sin(reference.TravelYaw);
+ double y = reference.Y + l * Math.Cos(reference.TravelYaw);
+ if (!IsFinite(x) || !IsFinite(y) || !IsFinite(travelYaw) || !IsFinite(vehicleYaw) ||
+ !IsFinite(geometricCurvature) || !IsFinite(vehicleCurvature))
+ {
+ failureReason = "Independent reconstruction produced non-finite geometry at station " + index + ".";
+ return null;
+ }
+ samples.Add(new ExpectedSample(candidate.ReferenceStations[index], x, y, vehicleYaw, geometricCurvature,
+ vehicleCurvature));
+ }
+
+ samples[0].PathS = 0d;
+ for (int index = 1; index < samples.Count; index++)
+ {
+ double dx = samples[index].X - samples[index - 1].X;
+ double dy = samples[index].Y - samples[index - 1].Y;
+ double chord = Math.Sqrt(dx * dx + dy * dy);
+ if (!IsFinite(chord) || chord <= 0d)
+ {
+ failureReason = "Independent reconstruction found non-increasing PathS at station " + index + ".";
+ return null;
+ }
+ samples[index].PathS = samples[index - 1].PathS + chord;
+ }
+ for (int index = 0; index < samples.Count; index++)
+ {
+ int lower = index == 0 ? 0 : index - 1;
+ int upper = index == samples.Count - 1 ? samples.Count - 1 : index + 1;
+ double span = samples[upper].PathS - samples[lower].PathS;
+ if (!IsFinite(span) || span <= 0d)
+ {
+ failureReason = "Independent curvature derivative span is invalid at station " + index + ".";
+ return null;
+ }
+ samples[index].VehicleCurvatureDerivative =
+ (samples[upper].VehicleCurvature - samples[lower].VehicleCurvature) / span;
+ }
+ return samples;
+ }
+
+ private static bool ComparePath(LateralPath path, LateralCandidate candidate, IReadOnlyList stations,
+ IReadOnlyList expected, double spatialTolerance, double kinematicTolerance,
+ out string failureReason)
+ {
+ failureReason = string.Empty;
+ for (int index = 0; index < path.Points.Count; index++)
+ {
+ LateralPathPoint actual = path.Points[index];
+ ExpectedSample sample = expected[index];
+ double dddl = candidate.DDDL[Math.Min(index, candidate.DDDL.Count - 1)];
+ if (!AreClose(actual.ReferenceS, stations[index], spatialTolerance) ||
+ !AreClose(actual.PathS, sample.PathS, spatialTolerance) ||
+ !AreClose(actual.L, candidate.L[index], spatialTolerance) ||
+ !AreClose(actual.DL, candidate.DL[index], kinematicTolerance) ||
+ !AreClose(actual.DDL, candidate.DDL[index], kinematicTolerance) ||
+ !AreClose(actual.DDDL, dddl, kinematicTolerance) ||
+ !AreClose(actual.X, sample.X, spatialTolerance) || !AreClose(actual.Y, sample.Y, spatialTolerance) ||
+ Math.Abs(AngleMath.NormalizeRadians(actual.VehicleYaw - sample.VehicleYaw)) > kinematicTolerance ||
+ !AreClose(actual.GeometricCurvature, sample.GeometricCurvature, kinematicTolerance) ||
+ !AreClose(actual.VehicleCurvature, sample.VehicleCurvature, kinematicTolerance) ||
+ !AreClose(actual.VehicleCurvatureDerivative, sample.VehicleCurvatureDerivative, kinematicTolerance))
+ {
+ failureReason = "Independent lateral geometry validation failed at station " + index + ".";
+ return false;
+ }
+ if (index == 0 && Math.Abs(actual.PathS) > spatialTolerance)
+ {
+ failureReason = "PathS must start at zero.";
+ return false;
+ }
+ if (index > 0 && actual.PathS <= path.Points[index - 1].PathS + spatialTolerance)
+ {
+ failureReason = "PathS must be strictly increasing.";
+ return false;
+ }
+ }
+ return true;
+ }
+
+ private static double CalculateGeometricCurvature(FrenetReferencePoint reference, double l, double dl, double ddl,
+ double referenceCurvatureDerivative)
+ {
+ double a = 1d - reference.GeometricCurvature * l;
+ double denominatorSquared = a * a + dl * dl;
+ double numerator = a * a * reference.GeometricCurvature + a * ddl +
+ referenceCurvatureDerivative * l * dl + 2d * reference.GeometricCurvature * dl * dl;
+ return numerator / (denominatorSquared * Math.Sqrt(denominatorSquared));
+ }
+
+ private static double GetMaximumVehicleCurvature(VehicleParameters vehicle)
+ {
+ if (vehicle == null)
+ throw new ArgumentNullException(nameof(vehicle));
+ if (vehicle.MaximumCurvaturePerMeter.HasValue)
+ return RequirePositive(vehicle.MaximumCurvaturePerMeter.Value, "vehicle maximum curvature");
+ if (vehicle.MinimumTurningRadiusMeters.HasValue)
+ return 1d / RequirePositive(vehicle.MinimumTurningRadiusMeters.Value, "vehicle minimum turning radius");
+ throw new ArgumentException("Vehicle maximum curvature is required.", nameof(vehicle));
+ }
+
+ private static bool AreClose(double actual, double expected, double tolerance)
+ {
+ return IsFinite(actual) && IsFinite(expected) && Math.Abs(actual - expected) <= tolerance;
+ }
+
+ private static double RequirePositive(double value, string name)
+ {
+ if (!IsFinite(value) || value <= 0d)
+ throw new ArgumentOutOfRangeException(name);
+ return value;
+ }
+
+ private static double RequireNonnegative(double value, string name)
+ {
+ if (!IsFinite(value) || value < 0d)
+ throw new ArgumentOutOfRangeException(name);
+ return value;
+ }
+
+ private static bool IsFinite(double value)
+ {
+ return !double.IsNaN(value) && !double.IsInfinity(value);
+ }
+
+ private sealed class ExpectedSample
+ {
+ public ExpectedSample(double referenceS, double x, double y, double vehicleYaw, double geometricCurvature,
+ double vehicleCurvature)
+ {
+ ReferenceS = referenceS;
+ X = x;
+ Y = y;
+ VehicleYaw = vehicleYaw;
+ GeometricCurvature = geometricCurvature;
+ VehicleCurvature = vehicleCurvature;
+ }
+
+ public double ReferenceS { get; }
+ public double X { get; }
+ public double Y { get; }
+ public double VehicleYaw { get; }
+ public double GeometricCurvature { get; }
+ public double VehicleCurvature { get; }
+ public double PathS { get; set; }
+ public double VehicleCurvatureDerivative { get; set; }
+ }
+}
diff --git a/ClumsyPilot/tests/EMPlannerVerificationHost/LateralModelChecks.cs b/ClumsyPilot/tests/EMPlannerVerificationHost/LateralModelChecks.cs
index ea8b4fa..5539d41 100644
--- a/ClumsyPilot/tests/EMPlannerVerificationHost/LateralModelChecks.cs
+++ b/ClumsyPilot/tests/EMPlannerVerificationHost/LateralModelChecks.cs
@@ -5,6 +5,7 @@ using EMPlannerVerificationHost;
using MultiWheelC.TrajectoryPlanning.CoarsePath;
using MultiWheelC.TrajectoryPlanning.CoarsePath.Vehicle;
using MultiWheelC.TrajectoryPlanning.PathSmoothing;
+using MultiWheelC.TrajectoryPlanning.Utils;
namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
@@ -20,6 +21,8 @@ internal static class LateralModelChecks
VerifiesAllNamedCostScales();
VerifiesEmptyHardBoundIntersectionFailsBeforeSolve();
VerifiesFakeSolverCapturesTheNeutralQpBoundary();
+ VerifiesNonlinearGeometryInBothDirections();
+ VerifiesIndependentGeometryValidationRejectsUnsafeOrTamperedPaths();
}
private static void VerifiesDeterministicVariableLayout()
@@ -275,6 +278,114 @@ internal static class LateralModelChecks
Verification.NearlyEqual(2d, solver.LastWarmStart[1], "fake solver records a defensive warm-start copy");
}
+ private static void VerifiesNonlinearGeometryInBothDirections()
+ {
+ double[] stations = { 0d, 0.7d, 2d };
+ LateralCandidate candidate = LateralCandidate.Integrate(stations, 0.1d, 0.1d, 0.05d,
+ new[] { 0.02d, -0.03d });
+ LateralGeometryEvaluator evaluator = CreateGeometryEvaluator();
+ LateralSolutionValidator validator = CreateGeometryValidator();
+
+ VerifyGeometryForDirection(TravelDirection.Forward, 0d, candidate, evaluator, validator);
+ VerifyGeometryForDirection(TravelDirection.Reverse, 0d, candidate, evaluator, validator);
+ VerifyGeometryForDirection(TravelDirection.Forward, 0.2d, candidate, evaluator, validator);
+ VerifyGeometryForDirection(TravelDirection.Reverse, 0.2d, candidate, evaluator, validator);
+ }
+
+ private static void VerifiesIndependentGeometryValidationRejectsUnsafeOrTamperedPaths()
+ {
+ LateralGeometryEvaluator evaluator = CreateGeometryEvaluator();
+ LateralSolutionValidator validator = CreateGeometryValidator();
+ double[] stations = { 0d, 1d, 2d };
+
+ LateralPlanningInput singularInput = CreateGeometryInput(TravelDirection.Forward, 1d, stations, 0.81d, 0d, 10d);
+ LateralCandidate singular = LateralCandidate.Integrate(stations, 0.81d, 0d, 0d, new[] { 0d, 0d });
+ Verification.True(!evaluator.TryEvaluate(singularInput, singular, out LateralPath singularPath, out string singularReason),
+ "Frenet denominator below 0.20 is rejected by reconstruction");
+ Verification.True(singularPath == null && singularReason.Length > 0, "singular reconstruction has no path");
+
+ LateralPlanningInput curvatureInput = CreateGeometryInput(TravelDirection.Forward, 0d, stations, 0d, 0d, 1d);
+ LateralCandidate excessiveCurvature = LateralCandidate.Integrate(stations, 0d, 0d, 2d, new[] { 0d, 0d });
+ Verification.True(evaluator.TryEvaluate(curvatureInput, excessiveCurvature, out LateralPath excessivePath,
+ out string excessiveReason), "curvature reconstruction remains geometric: " + excessiveReason);
+ Verification.True(!validator.TryValidate(curvatureInput, excessiveCurvature, excessivePath,
+ out LateralPath rejectedCurvature, out string curvatureReason), "curvature above vehicle limit is rejected");
+ Verification.True(rejectedCurvature == null && curvatureReason.Length > 0, "curvature failure has no validated path");
+
+ LateralCandidate valid = LateralCandidate.Integrate(stations, 0d, 0d, 0d, new[] { 0d, 0d });
+ Verification.True(evaluator.TryEvaluate(curvatureInput, valid, out LateralPath rawPath, out string rawReason),
+ "valid path reconstructs: " + rawReason);
+ var tamperedPoints = new List(rawPath.Points);
+ LateralPathPoint original = tamperedPoints[1];
+ tamperedPoints[1] = new LateralPathPoint(original.ReferenceS, original.PathS, original.L, original.DL,
+ original.DDL, original.DDDL, original.X + 0.01d, original.Y, original.VehicleYaw,
+ original.GeometricCurvature, original.VehicleCurvature, original.VehicleCurvatureDerivative);
+ Verification.True(!validator.TryValidate(curvatureInput, valid, new LateralPath(tamperedPoints, false),
+ out LateralPath rejectedTampered, out string tamperedReason),
+ "validator independently rejects a world-coordinate mismatch");
+ Verification.True(rejectedTampered == null && tamperedReason.Length > 0, "tampered path has no validated copy");
+
+ ExpectArgumentException(() => new LateralCandidate(stations, new[] { double.NaN, 0d, 0d },
+ new[] { 0d, 0d, 0d }, new[] { 0d, 0d, 0d }, new[] { 0d, 0d }), "non-finite lateral values are rejected");
+ ExpectArgumentException(() => new LateralPathPoint(0d, 0d, 0d, 0d, 0d, 0d, double.NaN, 0d, 0d, 0d, 0d, 0d),
+ "non-finite world geometry is rejected");
+ }
+
+ private static void VerifyGeometryForDirection(TravelDirection direction, double referenceCurvature,
+ LateralCandidate candidate,
+ LateralGeometryEvaluator evaluator, LateralSolutionValidator validator)
+ {
+ LateralPlanningInput input = CreateGeometryInput(direction, referenceCurvature, candidate.ReferenceStations,
+ candidate.L[0], candidate.DL[0], 10d);
+ Verification.True(evaluator.TryEvaluate(input, candidate, out LateralPath rawPath, out string evaluationReason),
+ direction + " geometry reconstructs: " + evaluationReason);
+ Verification.True(!rawPath.IsIndependentlyValidated, direction + " evaluator does not self-validate");
+ Verification.True(validator.TryValidate(input, candidate, rawPath, out LateralPath validatedPath,
+ out string validationReason), direction + " geometry validates: " + validationReason);
+ Verification.True(validatedPath.IsIndependentlyValidated, direction + " validation creates a marked immutable path");
+
+ double directionSign = direction == TravelDirection.Forward ? 1d : -1d;
+ Verification.NearlyEqual(0d, rawPath.Points[0].PathS, direction + " PathS starts at zero");
+ for (int index = 0; index < rawPath.Points.Count; index++)
+ {
+ LateralPathPoint point = rawPath.Points[index];
+ FrenetReferencePoint reference = ReferencePathInterpolator.Interpolate(input.ReferenceSegment, point.ReferenceS);
+ double denominator = 1d - reference.GeometricCurvature * point.L;
+ double expectedX = reference.X - point.L * Math.Sin(reference.TravelYaw);
+ double expectedY = reference.Y + point.L * Math.Cos(reference.TravelYaw);
+ double expectedTravelYaw = reference.TravelYaw + Math.Atan2(point.DL, denominator);
+ double expectedVehicleYaw = direction == TravelDirection.Forward
+ ? AngleMath.NormalizeRadians(expectedTravelYaw)
+ : AngleMath.NormalizeRadians(expectedTravelYaw + Math.PI);
+ double expectedGeometricCurvature = CalculateGeometricCurvature(reference, point.L, point.DL, point.DDL,
+ directionSign * reference.VehicleCurvatureDerivative);
+ Verification.NearlyEqual(expectedX, point.X, direction + " world x " + index);
+ Verification.NearlyEqual(expectedY, point.Y, direction + " world y " + index);
+ Verification.NearlyEqual(expectedVehicleYaw, point.VehicleYaw, direction + " vehicle yaw " + index);
+ Verification.NearlyEqual(expectedGeometricCurvature, point.GeometricCurvature,
+ direction + " full Frenet curvature " + index);
+ Verification.NearlyEqual(directionSign * point.GeometricCurvature, point.VehicleCurvature,
+ direction + " vehicle curvature sign " + index);
+ if (index > 0)
+ {
+ LateralPathPoint previous = rawPath.Points[index - 1];
+ double chord = Math.Sqrt((point.X - previous.X) * (point.X - previous.X) +
+ (point.Y - previous.Y) * (point.Y - previous.Y));
+ Verification.NearlyEqual(previous.PathS + chord, point.PathS, direction + " actual chord PathS " + index);
+ Verification.True(point.PathS > previous.PathS, direction + " PathS strictly increases " + index);
+ }
+ }
+
+ LateralPathPoint check = rawPath.Points[1];
+ double signedSpeed = directionSign * 0.3d;
+ var trajectoryPoint = new EmTrajectoryPoint(check.X, check.Y, check.VehicleYaw, signedSpeed, 0d,
+ check.VehicleCurvature, 0, check.ReferenceS, check.PathS, direction, EmBoundaryType.None, 0d, 0d);
+ Verification.NearlyEqual(signedSpeed * check.VehicleCurvature, trajectoryPoint.YawRate,
+ direction + " yaw-rate identity");
+ Verification.NearlyEqual(0.3d * check.GeometricCurvature, trajectoryPoint.YawRate,
+ direction + " travel curvature yaw-rate identity");
+ }
+
private static DirectionSegmentView CreateStraightSegment(double referenceCurvatureDerivative = 0d,
double referenceCurvature = 0d)
{
@@ -327,6 +438,57 @@ internal static class LateralModelChecks
CreateVehicle(maximumVehicleCurvature), configuration, seed);
}
+ private static LateralPlanningInput CreateGeometryInput(TravelDirection direction, double referenceCurvature,
+ IReadOnlyList stations, double startL, double startDL, double maximumVehicleCurvature)
+ {
+ var points = new List(stations.Count);
+ for (int index = 0; index < stations.Count; index++)
+ {
+ double s = stations[index];
+ double travelYaw = referenceCurvature * s;
+ double x = Math.Abs(referenceCurvature) <= 1e-12d ? s : Math.Sin(travelYaw) / referenceCurvature;
+ double y = Math.Abs(referenceCurvature) <= 1e-12d ? 0d : (1d - Math.Cos(travelYaw)) / referenceCurvature;
+ double vehicleYaw = direction == TravelDirection.Forward ? travelYaw : travelYaw - Math.PI;
+ points.Add(new SmoothedPathPoint(x, y, AngleMath.NormalizeRadians(vehicleYaw), vehicleYaw, s, direction,
+ referenceCurvature, direction == TravelDirection.Forward ? referenceCurvature : -referenceCurvature,
+ 0d, 1d, false, SmoothedPathPointSource.Anchor));
+ }
+ double end = stations[stations.Count - 1];
+ var segment = new DirectionSegmentView(0, direction, points,
+ new ReferenceBoundary(0, 0d, EmBoundaryType.None, 0d),
+ new ReferenceBoundary(0, end, EmBoundaryType.RollingSafetyStop, end), 0d);
+ var corridorStations = new List(stations.Count);
+ for (int index = 0; index < stations.Count; index++)
+ corridorStations.Add(new LateralInterval(stations[index], -1d, 1d, startL));
+ EmPlannerConfiguration configuration = CreateUnitScaleConfiguration();
+ configuration.Lateral.MaximumLateralSlope = 5d;
+ configuration.Lateral.MaximumLateralSecondDerivativePerMeter = 5d;
+ configuration.Lateral.MaximumLateralThirdDerivativePerSquareMeter = 5d;
+ double startDenominator = 1d - referenceCurvature * startL;
+ return new LateralPlanningInput(segment, new StaticCorridor(corridorStations),
+ new FrenetProjection(ReferencePathInterpolator.Interpolate(segment, stations[0]), startL,
+ Math.Atan2(startDL, startDenominator), 0d), EmTerminalType.RollingSafetyStop,
+ CreateVehicle(maximumVehicleCurvature), configuration, Array.Empty());
+ }
+
+ private static double CalculateGeometricCurvature(FrenetReferencePoint reference, double l, double dl, double ddl,
+ double referenceCurvatureDerivative)
+ {
+ double a = 1d - reference.GeometricCurvature * l;
+ return (a * a * reference.GeometricCurvature + a * ddl + referenceCurvatureDerivative * l * dl +
+ 2d * reference.GeometricCurvature * dl * dl) / Math.Pow(a * a + dl * dl, 1.5d);
+ }
+
+ private static LateralGeometryEvaluator CreateGeometryEvaluator()
+ {
+ return new LateralGeometryEvaluator();
+ }
+
+ private static LateralSolutionValidator CreateGeometryValidator()
+ {
+ return new LateralSolutionValidator();
+ }
+
private static LateralObjectiveBuilder CreateObjectiveBuilder()
{
return new LateralObjectiveBuilder();