91 lines
3.8 KiB
C#
91 lines
3.8 KiB
C#
using System;
|
|
|
|
namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
|
|
|
|
/// <summary>仅在同一方向段、同一方向和同一边界类型的轨迹区间内插值不可变轨迹。</summary>
|
|
public sealed class TrajectorySampler
|
|
{
|
|
private const double TimeEpsilonSeconds = 1e-9d;
|
|
|
|
/// <summary>按轨迹起点后的时间采样一个同质区间内的轨迹点。</summary>
|
|
/// <param name="trajectory">完整不可变轨迹；其点序列必须按 <c>TimeFromStart</c> 严格递增。</param>
|
|
/// <param name="timeFromStart">相对轨迹起点的采样时刻，单位 s。</param>
|
|
/// <param name="sampledPoint">成功时为原始点或插值新点；失败时为 <see langword="null"/>。</param>
|
|
/// <returns>时刻位于轨迹范围内且不需跨方向段/方向/边界插值时为 <see langword="true"/>；区间外或跨边界时为 <see langword="false"/>。</returns>
|
|
public bool TrySample(EmTrajectory trajectory, double timeFromStart, out EmTrajectoryPoint sampledPoint)
|
|
{
|
|
sampledPoint = null;
|
|
if (trajectory == null || double.IsNaN(timeFromStart) || double.IsInfinity(timeFromStart) ||
|
|
trajectory.Points.Count == 0)
|
|
{
|
|
return false;
|
|
}
|
|
|
|
EmTrajectoryPoint first = trajectory.Points[0];
|
|
EmTrajectoryPoint last = trajectory.Points[trajectory.Points.Count - 1];
|
|
if (timeFromStart < first.TimeFromStart - TimeEpsilonSeconds ||
|
|
timeFromStart > last.TimeFromStart + TimeEpsilonSeconds)
|
|
{
|
|
return false;
|
|
}
|
|
|
|
int lower = 0;
|
|
int upper = trajectory.Points.Count - 1;
|
|
while (upper - lower > 1)
|
|
{
|
|
int middle = lower + (upper - lower) / 2;
|
|
EmTrajectoryPoint point = trajectory.Points[middle];
|
|
if (Math.Abs(point.TimeFromStart - timeFromStart) <= TimeEpsilonSeconds)
|
|
{
|
|
sampledPoint = point;
|
|
return true;
|
|
}
|
|
|
|
if (point.TimeFromStart < timeFromStart)
|
|
lower = middle;
|
|
else
|
|
upper = middle;
|
|
}
|
|
|
|
EmTrajectoryPoint left = trajectory.Points[lower];
|
|
EmTrajectoryPoint right = trajectory.Points[upper];
|
|
if (Math.Abs(left.TimeFromStart - timeFromStart) <= TimeEpsilonSeconds)
|
|
{
|
|
sampledPoint = left;
|
|
return true;
|
|
}
|
|
if (Math.Abs(right.TimeFromStart - timeFromStart) <= TimeEpsilonSeconds)
|
|
{
|
|
sampledPoint = right;
|
|
return true;
|
|
}
|
|
if (left.SegmentIndex != right.SegmentIndex || left.Direction != right.Direction ||
|
|
left.BoundaryType != right.BoundaryType)
|
|
{
|
|
return false;
|
|
}
|
|
|
|
double fraction = (timeFromStart - left.TimeFromStart) / (right.TimeFromStart - left.TimeFromStart);
|
|
sampledPoint = new EmTrajectoryPoint(
|
|
Interpolate(left.X, right.X, fraction),
|
|
Interpolate(left.Y, right.Y, fraction),
|
|
Interpolate(left.Yaw, right.Yaw, fraction),
|
|
Interpolate(left.SignedLongitudinalVelocity, right.SignedLongitudinalVelocity, fraction),
|
|
timeFromStart,
|
|
Interpolate(left.VehicleCurvature, right.VehicleCurvature, fraction),
|
|
left.SegmentIndex,
|
|
Interpolate(left.SegmentLocalS, right.SegmentLocalS, fraction),
|
|
Interpolate(left.PathS, right.PathS, fraction),
|
|
left.Direction,
|
|
left.BoundaryType,
|
|
Interpolate(left.LongitudinalAcceleration, right.LongitudinalAcceleration, fraction),
|
|
Interpolate(left.LongitudinalJerk, right.LongitudinalJerk, fraction));
|
|
return true;
|
|
}
|
|
|
|
private static double Interpolate(double left, double right, double fraction)
|
|
{
|
|
return left + (right - left) * fraction;
|
|
}
|
|
}
|