86 lines
3.2 KiB
C#
86 lines
3.2 KiB
C#
using System;
|
|
|
|
namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
|
|
|
|
/// <summary>Interpolates an immutable trajectory only inside one homogeneous trajectory interval.</summary>
|
|
public sealed class TrajectorySampler
|
|
{
|
|
private const double TimeEpsilonSeconds = 1e-9d;
|
|
|
|
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;
|
|
}
|
|
}
|