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
@@ -15,6 +15,7 @@ namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.Facade;
 public sealed class PathSmoothingService
 {
    private readonly PathSmoothingPreprocessor _preprocessor = new PathSmoothingPreprocessor();
+    private readonly PathGeometryAnalyzer _analyzer = new PathGeometryAnalyzer();
    private readonly SmoothingAlgorithmRunner _runner = new SmoothingAlgorithmRunner();
    private readonly SmoothedPathValidator _validator = new SmoothedPathValidator();
    private readonly IPathSmoother _bSpline = new CubicBSplineSmoother();
@@ -39,6 +40,8 @@ public sealed class PathSmoothingService
            if (!RawPathBaselineBuilder.TryCreate(
                    request,
                    preparedPath,
+                    _analyzer,
+                    configuration.OutputSpacingMeters,
                    _validator,
                    configuration.MaximumCollisionCheckStepMeters,
                    out _,
@@ -134,6 +137,8 @@ public sealed class PathSmoothingService
        if (!RawPathBaselineBuilder.TryCreate(
                request,
                revalidatedPath,
+                _analyzer,
+                configuration.OutputSpacingMeters,
                _validator,
                configuration.MaximumCollisionCheckStepMeters,
                out RawPathBaseline rawPath,
@@ -234,6 +239,7 @@ public sealed class PathSmoothingService
                point.Direction,
                point.GeometricCurvature,
                point.VehicleCurvature,
+                point.VehicleCurvatureDerivative,
                point.BodyClearance,
                point.IsGearSwitchPoint,
                SmoothedPathPointSource.CoarsePathFallback));