100 lines
3.4 KiB
C#
100 lines
3.4 KiB
C#
using System;
|
|
|
|
namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
|
|
|
|
internal sealed class LateralPathInterpolator
|
|
{
|
|
private const double BoundaryTolerance = 1e-10d;
|
|
private readonly LateralPath path;
|
|
|
|
public LateralPathInterpolator(LateralPath path)
|
|
{
|
|
if (path == null || !path.IsIndependentlyValidated || path.Points.Count < 2)
|
|
throw new ArgumentException("Trajectory assembly requires an independently validated lateral path.", nameof(path));
|
|
|
|
double previousPathS = double.NegativeInfinity;
|
|
for (int index = 0; index < path.Points.Count; index++)
|
|
{
|
|
LateralPathPoint point = path.Points[index];
|
|
if (point == null || point.PathS <= previousPathS)
|
|
throw new ArgumentException("Lateral path points must have strictly increasing PathS.", nameof(path));
|
|
previousPathS = point.PathS;
|
|
}
|
|
|
|
this.path = path;
|
|
}
|
|
|
|
public InterpolatedLateralPathPoint Interpolate(double pathS)
|
|
{
|
|
if (!IsFinite(pathS))
|
|
throw new ArgumentOutOfRangeException(nameof(pathS));
|
|
|
|
LateralPathPoint first = path.Points[0];
|
|
LateralPathPoint last = path.Points[path.Points.Count - 1];
|
|
if (pathS < first.PathS - BoundaryTolerance || pathS > last.PathS + BoundaryTolerance)
|
|
throw new ArgumentOutOfRangeException(nameof(pathS));
|
|
if (pathS <= first.PathS + BoundaryTolerance)
|
|
return From(first);
|
|
if (pathS >= last.PathS - BoundaryTolerance)
|
|
return From(last);
|
|
|
|
for (int index = 1; index < path.Points.Count; index++)
|
|
{
|
|
LateralPathPoint right = path.Points[index];
|
|
if (pathS <= right.PathS)
|
|
{
|
|
LateralPathPoint left = path.Points[index - 1];
|
|
double ratio = (pathS - left.PathS) / (right.PathS - left.PathS);
|
|
return new InterpolatedLateralPathPoint(
|
|
Linear(left.X, right.X, ratio),
|
|
Linear(left.Y, right.Y, ratio),
|
|
NormalizeYaw(left.VehicleYaw + ratio * NormalizeYaw(right.VehicleYaw - left.VehicleYaw)),
|
|
Linear(left.VehicleCurvature, right.VehicleCurvature, ratio));
|
|
}
|
|
}
|
|
|
|
throw new InvalidOperationException("A PathS value inside the lateral path was not bracketed.");
|
|
}
|
|
|
|
private static InterpolatedLateralPathPoint From(LateralPathPoint point)
|
|
{
|
|
return new InterpolatedLateralPathPoint(point.X, point.Y, NormalizeYaw(point.VehicleYaw), point.VehicleCurvature);
|
|
}
|
|
|
|
private static double Linear(double left, double right, double ratio)
|
|
{
|
|
return left + ratio * (right - left);
|
|
}
|
|
|
|
internal static double NormalizeYaw(double yaw)
|
|
{
|
|
double normalized = yaw % (2d * Math.PI);
|
|
if (normalized >= Math.PI)
|
|
normalized -= 2d * Math.PI;
|
|
if (normalized < -Math.PI)
|
|
normalized += 2d * Math.PI;
|
|
return normalized;
|
|
}
|
|
|
|
private static bool IsFinite(double value)
|
|
{
|
|
return !double.IsNaN(value) && !double.IsInfinity(value);
|
|
}
|
|
}
|
|
|
|
internal sealed class InterpolatedLateralPathPoint
|
|
{
|
|
public InterpolatedLateralPathPoint(double x, double y, double yaw, double vehicleCurvature)
|
|
{
|
|
X = x;
|
|
Y = y;
|
|
Yaw = yaw;
|
|
VehicleCurvature = vehicleCurvature;
|
|
}
|
|
|
|
public double X { get; }
|
|
public double Y { get; }
|
|
public double Yaw { get; }
|
|
public double VehicleCurvature { get; }
|
|
}
|