146 lines
6.8 KiB
C#
146 lines
6.8 KiB
C#
using System;
|
||
using System.Collections.Generic;
|
||
using System.Collections.ObjectModel;
|
||
using MultiWheelC.TrajectoryPlanning.PathSmoothing;
|
||
using MultiWheelC.TrajectoryPlanning.Utils;
|
||
|
||
namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
|
||
|
||
/// <summary>
|
||
/// 当前方向段内供一次 EM 优化使用的参考路径切片，保留全局参考站与切片局部弧长的对应关系。
|
||
/// </summary>
|
||
public sealed class ReferenceHorizonSlice
|
||
{
|
||
/// <summary>
|
||
/// 创建一个以当前段起点为基准的参考地平线切片。
|
||
/// 参数：points 为局部 S（m）有序的参考点，terminalBoundary 必须属于 segment；返回：points 的防御性只读副本，空输入或边界不匹配会被拒绝。
|
||
/// </summary>
|
||
public ReferenceHorizonSlice(DirectionSegmentView segment, IReadOnlyList<SmoothedPathPoint> points,
|
||
ReferenceBoundary terminalBoundary)
|
||
{
|
||
if (segment == null)
|
||
throw new ArgumentNullException(nameof(segment));
|
||
if (points == null || points.Count == 0)
|
||
throw new ArgumentException("A horizon slice requires points.", nameof(points));
|
||
if (terminalBoundary == null || terminalBoundary.SegmentIndex != segment.SegmentIndex)
|
||
throw new ArgumentException("A matching terminal boundary is required.", nameof(terminalBoundary));
|
||
|
||
var copy = new List<SmoothedPathPoint>(points.Count);
|
||
for (int index = 0; index < points.Count; index++) copy.Add(points[index]);
|
||
Segment = segment;
|
||
Points = new ReadOnlyCollection<SmoothedPathPoint>(copy);
|
||
TerminalBoundary = terminalBoundary;
|
||
}
|
||
|
||
/// <summary>
|
||
/// 切片所属且不会跨越的单一方向段。
|
||
/// </summary>
|
||
public DirectionSegmentView Segment { get; }
|
||
|
||
/// <summary>
|
||
/// 从段起点至精确终端点的参考点只读副本，局部 ArcLength 单位为 m。
|
||
/// </summary>
|
||
public IReadOnlyList<SmoothedPathPoint> Points { get; }
|
||
|
||
/// <summary>
|
||
/// 切片末端的精确参考边界；中途截断时具有滚动安全停车语义。
|
||
/// </summary>
|
||
public ReferenceBoundary TerminalBoundary { get; }
|
||
}
|
||
|
||
/// <summary>
|
||
/// 按局部参考距离截取单一方向段，并在请求终端精确插值参考点。
|
||
/// 切片只包含本段坐标；S、原始弧长与 X/Y 使用 m，航向使用 rad，终端附近以 1e-12 m 判定相等。
|
||
/// </summary>
|
||
public static class ReferenceHorizonSlicer
|
||
{
|
||
/// <summary>
|
||
/// 端点复制、精确节点匹配和原段终点判定使用的局部 S 容差，单位 m。
|
||
/// </summary>
|
||
private const double Epsilon = 1e-12d;
|
||
|
||
/// <summary>
|
||
/// 将方向段截取到请求的局部终端 S，并追加该终端的精确点。
|
||
/// 参数：requestedEndSegmentLocalS 为非负有限 m 制局部 S；超过段长的请求夹紧到段末。
|
||
/// 返回：段末保留原边界，中途截断创建 RollingSafetyStop 边界；空段或非法 S 会引发异常。
|
||
/// </summary>
|
||
public static ReferenceHorizonSlice Slice(DirectionSegmentView segment, double requestedEndSegmentLocalS)
|
||
{
|
||
if (segment == null)
|
||
throw new ArgumentNullException(nameof(segment));
|
||
if (double.IsNaN(requestedEndSegmentLocalS) || double.IsInfinity(requestedEndSegmentLocalS) || requestedEndSegmentLocalS < 0d)
|
||
throw new ArgumentOutOfRangeException(nameof(requestedEndSegmentLocalS));
|
||
|
||
double terminalS = Math.Min(requestedEndSegmentLocalS, segment.LengthMeters);
|
||
var points = new List<SmoothedPathPoint>();
|
||
for (int index = 0; index < segment.Points.Count; index++)
|
||
{
|
||
SmoothedPathPoint point = segment.Points[index];
|
||
if (point.ArcLength < terminalS - Epsilon)
|
||
points.Add(point);
|
||
}
|
||
|
||
points.Add(GetExactTerminalPoint(segment, terminalS));
|
||
ReferenceBoundary terminal = terminalS >= segment.LengthMeters - Epsilon
|
||
? segment.EndBoundary
|
||
: new ReferenceBoundary(segment.SegmentIndex, terminalS, EmBoundaryType.RollingSafetyStop,
|
||
segment.SourceStartArcLength + terminalS);
|
||
return new ReferenceHorizonSlice(segment, points, terminal);
|
||
}
|
||
|
||
/// <summary>
|
||
/// 取得给定局部 S 的精确已有点或其相邻点之间的插值点。
|
||
/// 参数：terminalS 为已夹紧的 m 制局部 S；返回：与节点相差不超过 1e-12 m 时复用节点，否则在正跨度内线性插值。
|
||
/// </summary>
|
||
private static SmoothedPathPoint GetExactTerminalPoint(DirectionSegmentView segment, double terminalS)
|
||
{
|
||
for (int index = 0; index < segment.Points.Count; index++)
|
||
{
|
||
SmoothedPathPoint point = segment.Points[index];
|
||
if (Math.Abs(point.ArcLength - terminalS) <= Epsilon)
|
||
return point;
|
||
if (point.ArcLength > terminalS)
|
||
{
|
||
SmoothedPathPoint previous = segment.Points[index - 1];
|
||
return Interpolate(previous, point, terminalS);
|
||
}
|
||
}
|
||
return segment.Points[segment.Points.Count - 1];
|
||
}
|
||
|
||
/// <summary>
|
||
/// 在线性弧长区间内插值一个终端平滑路径点。
|
||
/// 参数：lower/upper 的 ArcLength 为 m，localS 为其间的 m 制位置；返回：位置、展开航向、曲率和净空线性插值，标记为 Interpolated。
|
||
/// </summary>
|
||
private static SmoothedPathPoint Interpolate(SmoothedPathPoint lower, SmoothedPathPoint upper, double localS)
|
||
{
|
||
double interval = upper.ArcLength - lower.ArcLength;
|
||
if (interval <= 0d)
|
||
throw new ArgumentException("Reference points do not bracket a positive interval.");
|
||
double fraction = (localS - lower.ArcLength) / interval;
|
||
double unwrappedHeading = lower.UnwrappedHeading + (upper.UnwrappedHeading - lower.UnwrappedHeading) * fraction;
|
||
return new SmoothedPathPoint(
|
||
Interpolate(lower.X, upper.X, fraction),
|
||
Interpolate(lower.Y, upper.Y, fraction),
|
||
AngleMath.NormalizeRadians(unwrappedHeading),
|
||
unwrappedHeading,
|
||
localS,
|
||
lower.Direction,
|
||
Interpolate(lower.GeometricCurvature, upper.GeometricCurvature, fraction),
|
||
Interpolate(lower.VehicleCurvature, upper.VehicleCurvature, fraction),
|
||
Interpolate(lower.VehicleCurvatureDerivative, upper.VehicleCurvatureDerivative, fraction),
|
||
Interpolate(lower.BodyClearance, upper.BodyClearance, fraction),
|
||
false,
|
||
SmoothedPathPointSource.Interpolated);
|
||
}
|
||
|
||
/// <summary>
|
||
/// 计算同单位标量的线性插值。
|
||
/// 参数：lower、upper 为端点，fraction 为无单位比例；返回：未夹紧的线性结果。
|
||
/// </summary>
|
||
private static double Interpolate(double lower, double upper, double fraction)
|
||
{
|
||
return lower + (upper - lower) * fraction;
|
||
}
|
||
}
|