127 lines
5.6 KiB
C#
127 lines
5.6 KiB
C#
using MultiWheelC.TrajectoryPlanning.CoarsePath;
|
|
|
|
namespace MultiWheelC.TrajectoryPlanning.PathSmoothing;
|
|
|
|
/// <summary>平滑空间路径上的不可变采样点；位置和长度单位为 m，航向为 rad，曲率为 1/m。</summary>
|
|
public sealed class SmoothedPathPoint
|
|
{
|
|
/// <summary>
|
|
/// 创建不显式提供曲率导数的不可变平滑路径点；曲率对弧长导数按 0 处理。
|
|
/// </summary>
|
|
/// <param name="xMeters">世界 X 坐标，单位 m。</param>
|
|
/// <param name="yMeters">世界 Y 坐标，单位 m。</param>
|
|
/// <param name="headingRadians">规范化车辆航向，单位 rad。</param>
|
|
/// <param name="unwrappedHeadingRadians">跨越 ±π 后仍连续的车辆航向，单位 rad。</param>
|
|
/// <param name="arcLengthMeters">从完整路径起点累计的弧长，单位 m。</param>
|
|
/// <param name="direction">该点所属方向段的实际行驶方向。</param>
|
|
/// <param name="geometricCurvaturePerMeter">几何曲线曲率，单位 1/m。</param>
|
|
/// <param name="vehicleCurvaturePerMeter">车辆模型使用的有符号曲率，单位 1/m。</param>
|
|
/// <param name="bodyClearanceMeters">扩大车体后的保守净空下界，单位 m。</param>
|
|
/// <param name="isGearSwitchPoint">为 <see langword="true"/> 时表示新方向段开始的精确换向点。</param>
|
|
/// <param name="source">该点在平滑流程中的来源枚举。</param>
|
|
public SmoothedPathPoint(
|
|
double xMeters,
|
|
double yMeters,
|
|
double headingRadians,
|
|
double unwrappedHeadingRadians,
|
|
double arcLengthMeters,
|
|
TravelDirection direction,
|
|
double geometricCurvaturePerMeter,
|
|
double vehicleCurvaturePerMeter,
|
|
double bodyClearanceMeters,
|
|
bool isGearSwitchPoint,
|
|
SmoothedPathPointSource source)
|
|
: this(
|
|
xMeters,
|
|
yMeters,
|
|
headingRadians,
|
|
unwrappedHeadingRadians,
|
|
arcLengthMeters,
|
|
direction,
|
|
geometricCurvaturePerMeter,
|
|
vehicleCurvaturePerMeter,
|
|
0d,
|
|
bodyClearanceMeters,
|
|
isGearSwitchPoint,
|
|
source)
|
|
{
|
|
}
|
|
|
|
/// <summary>创建带有车辆曲率对弧长导数的不可变平滑路径点。</summary>
|
|
/// <param name="xMeters">世界 X 坐标，单位 m。</param>
|
|
/// <param name="yMeters">世界 Y 坐标，单位 m。</param>
|
|
/// <param name="headingRadians">规范化车辆航向，单位 rad。</param>
|
|
/// <param name="unwrappedHeadingRadians">连续展开的车辆航向，单位 rad。</param>
|
|
/// <param name="arcLengthMeters">从完整路径起点累计的弧长，单位 m。</param>
|
|
/// <param name="direction">该点所属的前进或倒车方向。</param>
|
|
/// <param name="geometricCurvaturePerMeter">几何曲率，单位 1/m。</param>
|
|
/// <param name="vehicleCurvaturePerMeter">车辆曲率，单位 1/m。</param>
|
|
/// <param name="vehicleCurvatureDerivativePerSquareMeter">车辆曲率对弧长的导数 dκ/ds，单位 1/m²。</param>
|
|
/// <param name="bodyClearanceMeters">扩大车体后的保守净空下界，单位 m。</param>
|
|
/// <param name="isGearSwitchPoint">是否为新方向段开始的换向点。</param>
|
|
/// <param name="source">该点的平滑来源。</param>
|
|
public SmoothedPathPoint(
|
|
double xMeters,
|
|
double yMeters,
|
|
double headingRadians,
|
|
double unwrappedHeadingRadians,
|
|
double arcLengthMeters,
|
|
TravelDirection direction,
|
|
double geometricCurvaturePerMeter,
|
|
double vehicleCurvaturePerMeter,
|
|
double vehicleCurvatureDerivativePerSquareMeter,
|
|
double bodyClearanceMeters,
|
|
bool isGearSwitchPoint,
|
|
SmoothedPathPointSource source)
|
|
{
|
|
X = xMeters;
|
|
Y = yMeters;
|
|
Heading = headingRadians;
|
|
UnwrappedHeading = unwrappedHeadingRadians;
|
|
ArcLength = arcLengthMeters;
|
|
Direction = direction;
|
|
GeometricCurvature = geometricCurvaturePerMeter;
|
|
VehicleCurvature = vehicleCurvaturePerMeter;
|
|
VehicleCurvatureDerivative = vehicleCurvatureDerivativePerSquareMeter;
|
|
BodyClearance = bodyClearanceMeters;
|
|
IsGearSwitchPoint = isGearSwitchPoint;
|
|
Source = source;
|
|
}
|
|
|
|
/// <summary>世界 X 坐标，单位 m。</summary>
|
|
public double X { get; }
|
|
|
|
/// <summary>世界 Y 坐标，单位 m。</summary>
|
|
public double Y { get; }
|
|
|
|
/// <summary>归一化的车辆航向，单位 rad。</summary>
|
|
public double Heading { get; }
|
|
|
|
/// <summary>连续展开的车辆航向，单位 rad。</summary>
|
|
public double UnwrappedHeading { get; }
|
|
|
|
/// <summary>从完整路径起点累计的弧长，单位 m。</summary>
|
|
public double ArcLength { get; }
|
|
|
|
/// <summary>该点所在连续方向段的行驶方向。</summary>
|
|
public TravelDirection Direction { get; }
|
|
|
|
/// <summary>按几何弧长计算的有符号路径曲率，单位 1/m。</summary>
|
|
public double GeometricCurvature { get; }
|
|
|
|
/// <summary>车辆模型使用的有符号曲率，单位 1/m。</summary>
|
|
public double VehicleCurvature { get; }
|
|
|
|
/// <summary>车辆曲率对弧长的导数 dκ/ds，单位 1/m²。</summary>
|
|
public double VehicleCurvatureDerivative { get; }
|
|
|
|
/// <summary>扩大车体后的保守净空下界，单位 m。</summary>
|
|
public double BodyClearance { get; }
|
|
|
|
/// <summary>该点是否为新方向段开始的换向点。</summary>
|
|
public bool IsGearSwitchPoint { get; }
|
|
|
|
/// <summary>该点在平滑流程中的来源。</summary>
|
|
public SmoothedPathPointSource Source { get; }
|
|
}
|