88 lines
4.1 KiB
C#
88 lines
4.1 KiB
C#
using System;
|
||
using MultiWheelC.TrajectoryPlanning.PathSmoothing;
|
||
using MultiWheelC.TrajectoryPlanning.Utils;
|
||
|
||
namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
|
||
|
||
/// <summary>
|
||
/// 在恰好一个方向段内按局部参考弧长插值参考几何。
|
||
/// 输入和输出 S、X/Y 与净空均为 m，航向为 rad，曲率为 1/m；插值不会越过方向段边界。
|
||
/// </summary>
|
||
public static class ReferencePathInterpolator
|
||
{
|
||
/// <summary>
|
||
/// 接受端点轻微 S 超差并判断精确节点的数值容差，单位 m。
|
||
/// </summary>
|
||
private const double Epsilon = 1e-12d;
|
||
|
||
/// <summary>
|
||
/// 为给定局部 S 生成方向一致的 Frenet 参考样本。
|
||
/// 参数：segment 不可为空，referenceS 为 m；[-1e-12, Length+1e-12] 内的端点超差会夹紧到边界。
|
||
/// 返回：精确点直接转换，区间内对位置、展开航向和几何量线性插值；超界或退化跨度会引发异常。
|
||
/// </summary>
|
||
public static FrenetReferencePoint Interpolate(DirectionSegmentView segment, double referenceS)
|
||
{
|
||
if (segment == null)
|
||
throw new ArgumentNullException(nameof(segment));
|
||
if (!IsFinite(referenceS) || referenceS < -Epsilon || referenceS > segment.LengthMeters + Epsilon)
|
||
throw new ArgumentOutOfRangeException(nameof(referenceS));
|
||
|
||
double clampedS = Math.Max(0d, Math.Min(segment.LengthMeters, referenceS));
|
||
for (int index = 0; index < segment.Points.Count; index++)
|
||
{
|
||
SmoothedPathPoint upper = segment.Points[index];
|
||
if (Math.Abs(upper.ArcLength - clampedS) <= Epsilon)
|
||
return FromPoint(upper);
|
||
if (upper.ArcLength > clampedS)
|
||
{
|
||
SmoothedPathPoint lower = segment.Points[index - 1];
|
||
double span = upper.ArcLength - lower.ArcLength;
|
||
if (span <= Epsilon)
|
||
throw new ArgumentException("Reference points must have positive interpolation spans.", nameof(segment));
|
||
double fraction = (clampedS - lower.ArcLength) / span;
|
||
return new FrenetReferencePoint(
|
||
clampedS,
|
||
Lerp(lower.X, upper.X, fraction),
|
||
Lerp(lower.Y, upper.Y, fraction),
|
||
AngleMath.NormalizeRadians(Lerp(lower.UnwrappedHeading, upper.UnwrappedHeading, fraction)),
|
||
Lerp(lower.UnwrappedHeading, upper.UnwrappedHeading, fraction),
|
||
segment.Direction,
|
||
Lerp(lower.GeometricCurvature, upper.GeometricCurvature, fraction),
|
||
Lerp(lower.VehicleCurvature, upper.VehicleCurvature, fraction),
|
||
Lerp(lower.VehicleCurvatureDerivative, upper.VehicleCurvatureDerivative, fraction),
|
||
Lerp(lower.BodyClearance, upper.BodyClearance, fraction));
|
||
}
|
||
}
|
||
return FromPoint(segment.Points[segment.Points.Count - 1]);
|
||
}
|
||
|
||
/// <summary>
|
||
/// 将原始平滑路径点转换为同一局部 S 的 Frenet 参考样本。
|
||
/// 参数：point 已携带世界 X/Y（m）、航向（rad）和曲率量；返回：保持其方向及所有几何量的不可变副本。
|
||
/// </summary>
|
||
private static FrenetReferencePoint FromPoint(SmoothedPathPoint point)
|
||
{
|
||
return new FrenetReferencePoint(point.ArcLength, point.X, point.Y, point.Heading, point.UnwrappedHeading,
|
||
point.Direction, point.GeometricCurvature, point.VehicleCurvature, point.VehicleCurvatureDerivative,
|
||
point.BodyClearance);
|
||
}
|
||
|
||
/// <summary>
|
||
/// 在线性标量区间内计算插值值。
|
||
/// 参数：lower、upper 为同单位端点，fraction 为无单位比例；返回：同单位的未夹紧线性结果。
|
||
/// </summary>
|
||
private static double Lerp(double lower, double upper, double fraction)
|
||
{
|
||
return lower + (upper - lower) * fraction;
|
||
}
|
||
|
||
/// <summary>
|
||
/// 判定插值输入是否为有限实数。
|
||
/// 参数：value 为任意标量；返回：NaN 或无穷时为 false。
|
||
/// </summary>
|
||
private static bool IsFinite(double value)
|
||
{
|
||
return !double.IsNaN(value) && !double.IsInfinity(value);
|
||
}
|
||
}
|