106 lines
4.5 KiB
C#
106 lines
4.5 KiB
C#
using System;
|
|
using System.Collections.Generic;
|
|
using System.Collections.ObjectModel;
|
|
using MultiWheelC.TrajectoryPlanning.PathSmoothing;
|
|
using MultiWheelC.TrajectoryPlanning.Utils;
|
|
|
|
namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
|
|
|
|
public sealed class ReferenceHorizonSlice
|
|
{
|
|
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;
|
|
}
|
|
|
|
public DirectionSegmentView Segment { get; }
|
|
|
|
public IReadOnlyList<SmoothedPathPoint> Points { get; }
|
|
|
|
public ReferenceBoundary TerminalBoundary { get; }
|
|
}
|
|
|
|
public static class ReferenceHorizonSlicer
|
|
{
|
|
private const double Epsilon = 1e-12d;
|
|
|
|
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);
|
|
}
|
|
|
|
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];
|
|
}
|
|
|
|
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);
|
|
}
|
|
|
|
private static double Interpolate(double lower, double upper, double fraction)
|
|
{
|
|
return lower + (upper - lower) * fraction;
|
|
}
|
|
}
|