91 lines
4.2 KiB
C#
91 lines
4.2 KiB
C#
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;
|
|
}
|
|
}
|