65 lines
2.4 KiB
C#
65 lines
2.4 KiB
C#
using System;
|
||
|
||
namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
|
||
|
||
/// <summary>
|
||
/// 世界车辆位姿投影到单一受限方向段后的 Frenet 描述。
|
||
/// S 与 L 单位均为 m，航向误差为 rad，距离以 m² 保存；对象不代表跨越换向边界的投影。
|
||
/// </summary>
|
||
public sealed class FrenetProjection
|
||
{
|
||
/// <summary>
|
||
/// 从参考点和相对量创建不可变投影结果。
|
||
/// 参数：lateralOffset 为沿行驶方向左法线的 L（m），headingError 为行驶航向差（rad），squaredDistanceMeters 为非负 m²。
|
||
/// 返回：携带参考 S 的投影；空参考点、非有限值或负平方距离会被拒绝。
|
||
/// </summary>
|
||
public FrenetProjection(FrenetReferencePoint referencePoint, double lateralOffset, double headingError,
|
||
double squaredDistanceMeters)
|
||
{
|
||
if (referencePoint == null)
|
||
throw new ArgumentNullException(nameof(referencePoint));
|
||
if (!IsFinite(lateralOffset) || !IsFinite(headingError) || !IsFinite(squaredDistanceMeters) || squaredDistanceMeters < 0d)
|
||
throw new ArgumentOutOfRangeException(nameof(lateralOffset));
|
||
|
||
ReferencePoint = referencePoint;
|
||
ReferenceS = referencePoint.ReferenceS;
|
||
LateralOffset = lateralOffset;
|
||
HeadingError = headingError;
|
||
SquaredDistanceMeters = squaredDistanceMeters;
|
||
}
|
||
|
||
/// <summary>
|
||
/// 投影命中的插值参考点，其坐标为世界 X/Y（m）。
|
||
/// </summary>
|
||
public FrenetReferencePoint ReferencePoint { get; }
|
||
|
||
/// <summary>
|
||
/// 命中点在当前方向段的局部参考弧长 S，单位 m。
|
||
/// </summary>
|
||
public double ReferenceS { get; }
|
||
|
||
/// <summary>
|
||
/// 世界位姿相对参考行驶方向左法线的横向偏移 L，单位 m。
|
||
/// </summary>
|
||
public double LateralOffset { get; }
|
||
|
||
/// <summary>
|
||
/// 车辆行驶航向相对参考行驶航向的归一化误差，单位 rad。
|
||
/// </summary>
|
||
public double HeadingError { get; }
|
||
|
||
/// <summary>
|
||
/// 世界位置与命中参考位置的欧氏距离平方，单位 m²。
|
||
/// </summary>
|
||
public double SquaredDistanceMeters { get; }
|
||
|
||
/// <summary>
|
||
/// 判定投影标量是否有限。
|
||
/// 参数：value 为任意实数；返回：NaN 和正负无穷均返回 false。
|
||
/// </summary>
|
||
private static bool IsFinite(double value)
|
||
{
|
||
return !double.IsNaN(value) && !double.IsInfinity(value);
|
||
}
|
||
}
|