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