chore: save current workspace progress

2026-08-09 22:13:18 +08:00
parent 650c2ab0e3
commit 2f4fd15e52
449 changed files with 76593 additions and 971 deletions
@@ -81,7 +81,7 @@ public sealed class EmTrajectoryValidator
    public EmTrajectoryValidationResult Validate(EmTrajectory trajectory, PlanningGridMap map, VehicleParameters vehicle,
        EmPlannerConfiguration configuration, int segmentIndex, double pathUpperBoundS, EmBoundaryType terminalBoundary)
    {
-        return ValidateCore(trajectory, map, vehicle, configuration, segmentIndex, pathUpperBoundS, null,
+        return ValidateCore(trajectory, map, vehicle, configuration, segmentIndex, double.PositiveInfinity, pathUpperBoundS, null,
            terminalBoundary, false);
    }

@@ -89,12 +89,34 @@ public sealed class EmTrajectoryValidator
        EmPlannerConfiguration configuration, int segmentIndex, double pathUpperBoundS, Pose2D terminalPose,
        EmBoundaryType terminalBoundary)
    {
-        return ValidateCore(trajectory, map, vehicle, configuration, segmentIndex, pathUpperBoundS, terminalPose,
+        return ValidateCore(trajectory, map, vehicle, configuration, segmentIndex, double.PositiveInfinity, pathUpperBoundS, terminalPose,
            terminalBoundary, true);
    }

+    public EmTrajectoryValidationResult Validate(EmTrajectory trajectory, PlanningGridMap map, VehicleParameters vehicle,
+        EmPlannerConfiguration configuration, int segmentIndex, double segmentUpperBoundS, double pathUpperBoundS,
+        EmBoundaryType terminalBoundary)
+    {
+        if (!IsFinite(segmentUpperBoundS) || segmentUpperBoundS < 0d)
+            return EmTrajectoryValidationResult.Reject(EmTrajectoryValidationFailure.InvalidInput, -1,
+                "Segment-local trajectory bounds are invalid.");
+        return ValidateCore(trajectory, map, vehicle, configuration, segmentIndex, segmentUpperBoundS, pathUpperBoundS,
+            null, terminalBoundary, false);
+    }
+
+    public EmTrajectoryValidationResult Validate(EmTrajectory trajectory, PlanningGridMap map, VehicleParameters vehicle,
+        EmPlannerConfiguration configuration, int segmentIndex, double segmentUpperBoundS, double pathUpperBoundS,
+        Pose2D terminalPose, EmBoundaryType terminalBoundary)
+    {
+        if (!IsFinite(segmentUpperBoundS) || segmentUpperBoundS < 0d)
+            return EmTrajectoryValidationResult.Reject(EmTrajectoryValidationFailure.InvalidInput, -1,
+                "Segment-local trajectory bounds are invalid.");
+        return ValidateCore(trajectory, map, vehicle, configuration, segmentIndex, segmentUpperBoundS, pathUpperBoundS,
+            terminalPose, terminalBoundary, true);
+    }
+
    private EmTrajectoryValidationResult ValidateCore(EmTrajectory trajectory, PlanningGridMap map, VehicleParameters vehicle,
-        EmPlannerConfiguration configuration, int segmentIndex, double pathUpperBoundS, Pose2D terminalPose,
+        EmPlannerConfiguration configuration, int segmentIndex, double segmentUpperBoundS, double pathUpperBoundS, Pose2D terminalPose,
        EmBoundaryType terminalBoundary, bool requiresTerminalPose)
    {
        if (trajectory == null || map == null || vehicle == null || configuration == null || configuration.Validation == null ||
@@ -111,7 +133,7 @@ public sealed class EmTrajectoryValidator
            EmTrajectoryPoint point = trajectory.Points[index];
            if (point == null || !HasOnlyFiniteValues(point))
                return Reject(EmTrajectoryValidationFailure.NonFinite, index, "Trajectory contains a non-finite point.");
-            if (point.SegmentIndex != segmentIndex || point.SegmentLocalS > pathUpperBoundS + limits.SpatialTolerance ||
+            if (point.SegmentIndex != segmentIndex || point.SegmentLocalS > segmentUpperBoundS + limits.SpatialTolerance ||
                point.PathS > pathUpperBoundS + limits.SpatialTolerance)
            {
                return Reject(EmTrajectoryValidationFailure.SegmentBoundaryExceeded, index,