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,90 @@
+using System;
+using MultiWheelC.TrajectoryPlanning.Utils;
+
+namespace MultiWheelC.TrajectoryPlanning.CoarsePath.Vehicle;
+
+/// <summary>以车辆几何中心为原点的扩大旋转矩形。</summary>
+internal sealed class VehicleFootprint
+{
+    private VehicleFootprint(Pose2D pose, double halfLengthMeters, double halfWidthMeters)
+    {
+        CenterX = pose.X;
+        CenterY = pose.Y;
+        HalfLengthMeters = halfLengthMeters;
+        HalfWidthMeters = halfWidthMeters;
+        AxisLongitudinalX = Math.Cos(pose.Heading);
+        AxisLongitudinalY = Math.Sin(pose.Heading);
+        AxisLateralX = -AxisLongitudinalY;
+        AxisLateralY = AxisLongitudinalX;
+        CircumscribedRadiusMeters = Math.Sqrt(halfLengthMeters * halfLengthMeters + halfWidthMeters * halfWidthMeters);
+
+        double minX = double.PositiveInfinity;
+        double maxX = double.NegativeInfinity;
+        double minY = double.PositiveInfinity;
+        double maxY = double.NegativeInfinity;
+        for (int index = 0; index < 4; index++)
+        {
+            GetCorner(index, out double x, out double y);
+            minX = Math.Min(minX, x);
+            maxX = Math.Max(maxX, x);
+            minY = Math.Min(minY, y);
+            maxY = Math.Max(maxY, y);
+        }
+        MinX = minX;
+        MaxX = maxX;
+        MinY = minY;
+        MaxY = maxY;
+    }
+
+    public double CenterX { get; }
+    public double CenterY { get; }
+    public double HalfLengthMeters { get; }
+    public double HalfWidthMeters { get; }
+    public double AxisLongitudinalX { get; }
+    public double AxisLongitudinalY { get; }
+    public double AxisLateralX { get; }
+    public double AxisLateralY { get; }
+    public double CircumscribedRadiusMeters { get; }
+    public double MinX { get; }
+    public double MaxX { get; }
+    public double MinY { get; }
+    public double MaxY { get; }
+
+    /// <summary>创建包含车辆安全余量和临时扫掠余量的矩形。</summary>
+    public static bool TryCreate(Pose2D pose, VehicleParameters vehicle, double additionalMarginMeters, out VehicleFootprint footprint)
+    {
+        footprint = null;
+        if (pose == null || vehicle == null || !NumericGuard.IsFinite(pose.X) || !NumericGuard.IsFinite(pose.Y) ||
+            !NumericGuard.IsFinite(pose.Heading) || !NumericGuard.IsPositiveFinite(vehicle.LengthMeters) ||
+            !NumericGuard.IsPositiveFinite(vehicle.WidthMeters) || !NumericGuard.IsFinite(vehicle.SafetyMarginMeters) ||
+            vehicle.SafetyMarginMeters < 0d || !NumericGuard.IsFinite(additionalMarginMeters) || additionalMarginMeters < 0d)
+            return false;
+
+        double totalMarginMeters = vehicle.SafetyMarginMeters + additionalMarginMeters;
+        if (!NumericGuard.IsFinite(totalMarginMeters)) return false;
+        double halfLengthMeters = vehicle.LengthMeters / 2d + totalMarginMeters;
+        double halfWidthMeters = vehicle.WidthMeters / 2d + totalMarginMeters;
+        if (!NumericGuard.IsPositiveFinite(halfLengthMeters) || !NumericGuard.IsPositiveFinite(halfWidthMeters)) return false;
+
+        footprint = new VehicleFootprint(pose, halfLengthMeters, halfWidthMeters);
+        return NumericGuard.IsFinite(footprint.CircumscribedRadiusMeters) && NumericGuard.IsFinite(footprint.MinX) &&
+            NumericGuard.IsFinite(footprint.MaxX) && NumericGuard.IsFinite(footprint.MinY) && NumericGuard.IsFinite(footprint.MaxY);
+    }
+
+    /// <summary>获取指定角点。索引按逆时针顺序为 0 到 3。</summary>
+    public void GetCorner(int index, out double x, out double y)
+    {
+        double longitudinalSign;
+        double lateralSign;
+        switch (index)
+        {
+            case 0: longitudinalSign = 1d; lateralSign = 1d; break;
+            case 1: longitudinalSign = -1d; lateralSign = 1d; break;
+            case 2: longitudinalSign = -1d; lateralSign = -1d; break;
+            case 3: longitudinalSign = 1d; lateralSign = -1d; break;
+            default: throw new ArgumentOutOfRangeException(nameof(index));
+        }
+        x = CenterX + longitudinalSign * HalfLengthMeters * AxisLongitudinalX + lateralSign * HalfWidthMeters * AxisLateralX;
+        y = CenterY + longitudinalSign * HalfLengthMeters * AxisLongitudinalY + lateralSign * HalfWidthMeters * AxisLateralY;
+    }
+}