65 lines
2.9 KiB
C#
65 lines
2.9 KiB
C#
using System;
|
|
using MultiWheelC.TrajectoryPlanning.PathSmoothing;
|
|
using MultiWheelC.TrajectoryPlanning.Utils;
|
|
|
|
namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
|
|
|
|
/// <summary>Interpolates reference geometry inside exactly one direction segment.</summary>
|
|
public static class ReferencePathInterpolator
|
|
{
|
|
private const double Epsilon = 1e-12d;
|
|
|
|
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]);
|
|
}
|
|
|
|
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);
|
|
}
|
|
|
|
private static double Lerp(double lower, double upper, double fraction)
|
|
{
|
|
return lower + (upper - lower) * fraction;
|
|
}
|
|
|
|
private static bool IsFinite(double value)
|
|
{
|
|
return !double.IsNaN(value) && !double.IsInfinity(value);
|
|
}
|
|
}
|