fix: unify path smoothing geometry metrics

2026-07-30 16:39:08 +08:00
parent d7ceb761b7
commit d6b34f88ce
16 changed files with 259 additions and 115 deletions
@@ -129,7 +129,8 @@ public sealed class SmoothedPathValidator
            SmoothedPathPoint point = candidatePath[index];
            output.Add(new SmoothedPathPoint(
                point.X, point.Y, point.Heading, point.UnwrappedHeading, point.ArcLength, point.Direction,
-                point.GeometricCurvature, point.VehicleCurvature, checkedClearances[index], point.IsGearSwitchPoint, point.Source));
+                point.GeometricCurvature, point.VehicleCurvature, point.VehicleCurvatureDerivative,
+                checkedClearances[index], point.IsGearSwitchPoint, point.Source));
        }

        pathWithClearance = new ReadOnlyCollection<SmoothedPathPoint>(output);
@@ -206,6 +207,7 @@ public sealed class SmoothedPathValidator
            NumericGuard.IsFinite(point.Heading) && NumericGuard.IsFinite(point.UnwrappedHeading) &&
            NumericGuard.IsFinite(point.ArcLength) && point.ArcLength >= 0d &&
            NumericGuard.IsFinite(point.GeometricCurvature) && NumericGuard.IsFinite(point.VehicleCurvature) &&
+            NumericGuard.IsFinite(point.VehicleCurvatureDerivative) &&
            IsDirection(point.Direction) && Enum.IsDefined(typeof(SmoothedPathPointSource), point.Source) &&
            Math.Abs(AngleMath.ShortestSignedDifference(point.Heading, AngleMath.NormalizeRadians(point.Heading))) <= Tolerance;
    }