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
@@ -0,0 +1,97 @@
+using System.Collections.Generic;
+using System.Collections.ObjectModel;
+using MultiWheelC.TrajectoryPlanning.CoarsePath;
+using MultiWheelC.TrajectoryPlanning.Mapping;
+
+namespace MultiWheelC.TrajectoryPlanning.PathSmoothing;
+
+/// <summary>路径平滑所需的原始粗路径、复核上下文和配置。</summary>
+public sealed class PathSmoothingRequest
+{
+    private readonly VehicleParameters _vehicle;
+    private readonly PathSmoothingConfiguration _configuration;
+
+    /// <summary>创建路径平滑请求，并复制粗路径和方向分段集合。</summary>
+    public PathSmoothingRequest(
+        IReadOnlyList<CoarsePathPoint> coarsePath,
+        IReadOnlyList<PathSegment> segments,
+        PlanningGridMap map,
+        VehicleParameters vehicle,
+        PathSmoothingConfiguration configuration)
+    {
+        CoarsePath = CopyReadOnly(coarsePath);
+        Segments = CopyReadOnly(segments);
+        Map = map;
+        _vehicle = CopyVehicle(vehicle);
+        _configuration = CopyConfiguration(configuration);
+    }
+
+    /// <summary>原始粗路径的不可变快照。</summary>
+    public IReadOnlyList<CoarsePathPoint> CoarsePath { get; }
+
+    /// <summary>原始粗路径方向分段的不可变快照。</summary>
+    public IReadOnlyList<PathSegment> Segments { get; }
+
+    /// <summary>用于平滑后完整车体复核的规划栅格地图。</summary>
+    public PlanningGridMap Map { get; }
+
+    /// <summary>车辆几何与最大曲率约束的不可变快照副本。</summary>
+    public VehicleParameters Vehicle => CopyVehicle(_vehicle);
+
+    /// <summary>本次平滑配置的不可变快照副本。</summary>
+    public PathSmoothingConfiguration Configuration => CopyConfiguration(_configuration);
+
+    private static IReadOnlyList<T> CopyReadOnly<T>(IReadOnlyList<T> source)
+    {
+        var copy = new List<T>(source == null ? 0 : source.Count);
+        if (source != null)
+        {
+            for (int index = 0; index < source.Count; index++)
+                copy.Add(source[index]);
+        }
+        return new ReadOnlyCollection<T>(copy);
+    }
+
+    private static VehicleParameters CopyVehicle(VehicleParameters source)
+    {
+        if (source == null) return null;
+        return new VehicleParameters
+        {
+            LengthMeters = source.LengthMeters,
+            WidthMeters = source.WidthMeters,
+            SafetyMarginMeters = source.SafetyMarginMeters,
+            MaximumCurvaturePerMeter = source.MaximumCurvaturePerMeter,
+            MinimumTurningRadiusMeters = source.MinimumTurningRadiusMeters,
+        };
+    }
+
+    private static PathSmoothingConfiguration CopyConfiguration(PathSmoothingConfiguration source)
+    {
+        if (source == null) return null;
+        var copy = new PathSmoothingConfiguration
+        {
+            Method = source.Method,
+            OutputSpacingMeters = source.OutputSpacingMeters,
+            MaximumCollisionCheckStepMeters = source.MaximumCollisionCheckStepMeters,
+            MinimumClearanceReserveMeters = source.MinimumClearanceReserveMeters,
+            SmoothingStrength = source.SmoothingStrength,
+            AllowFallbackToCoarsePath = source.AllowFallbackToCoarsePath,
+        };
+        copy.CubicBSpline.EndpointTangentScale = source.CubicBSpline.EndpointTangentScale;
+        copy.LocalCubicBezier.CornerHeadingThresholdRadians = source.LocalCubicBezier.CornerHeadingThresholdRadians;
+        copy.LocalCubicBezier.MaximumWindowLengthMeters = source.LocalCubicBezier.MaximumWindowLengthMeters;
+        copy.LocalCubicBezier.HandleLengthRatio = source.LocalCubicBezier.HandleLengthRatio;
+        copy.PiecewiseQuintic.KnotSpacingMeters = source.PiecewiseQuintic.KnotSpacingMeters;
+        copy.PiecewiseQuintic.MinimumKnotSpacingMeters = source.PiecewiseQuintic.MinimumKnotSpacingMeters;
+        copy.LocalG2Quintic.MinimumWindowLengthMeters = source.LocalG2Quintic.MinimumWindowLengthMeters;
+        copy.LocalG2Quintic.PreferredWindowLengthMeters = source.LocalG2Quintic.PreferredWindowLengthMeters;
+        copy.LocalG2Quintic.MaximumWindowLengthMeters = source.LocalG2Quintic.MaximumWindowLengthMeters;
+        copy.LocalG2Quintic.MaximumDeviationMeters = source.LocalG2Quintic.MaximumDeviationMeters;
+        copy.LocalG2Quintic.AbsoluteCurvatureJumpFloorPerMeter = source.LocalG2Quintic.AbsoluteCurvatureJumpFloorPerMeter;
+        copy.LocalG2Quintic.CurvatureJumpRatioOfMaximum = source.LocalG2Quintic.CurvatureJumpRatioOfMaximum;
+        copy.LocalG2Quintic.MinimumPeakGradientImprovementRatio = source.LocalG2Quintic.MinimumPeakGradientImprovementRatio;
+        copy.LocalG2Quintic.MaximumVariationCostRegressionRatio = source.LocalG2Quintic.MaximumVariationCostRegressionRatio;
+        copy.LocalG2Quintic.MaximumCandidatesPerRegion = source.LocalG2Quintic.MaximumCandidatesPerRegion;
+        return copy;
+    }
+}