260 lines
11 KiB
C#
260 lines
11 KiB
C#
using System;
|
|
using System.Collections.Generic;
|
|
using System.Collections.ObjectModel;
|
|
using MultiWheelC.TrajectoryPlanning.CoarsePath;
|
|
|
|
namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
|
|
|
|
/// <summary>Resamples a compatible published trajectory onto the current ST knots as soft longitudinal references.</summary>
|
|
public sealed class LongitudinalPreviousTrajectorySeed
|
|
{
|
|
private static readonly LongitudinalPreviousTrajectorySeed empty = new LongitudinalPreviousTrajectorySeed(
|
|
Array.Empty<double>(), Array.Empty<double>());
|
|
|
|
public LongitudinalPreviousTrajectorySeed(IReadOnlyList<double> pathS,
|
|
IReadOnlyList<double> progressSpeedMetersPerSecond)
|
|
{
|
|
if (pathS == null)
|
|
throw new ArgumentNullException(nameof(pathS));
|
|
if (progressSpeedMetersPerSecond == null)
|
|
throw new ArgumentNullException(nameof(progressSpeedMetersPerSecond));
|
|
if (pathS.Count != progressSpeedMetersPerSecond.Count)
|
|
throw new ArgumentException("Previous path-S and progress-speed samples must have matching counts.");
|
|
|
|
var copiedPathS = new List<double>(pathS.Count);
|
|
var copiedSpeed = new List<double>(progressSpeedMetersPerSecond.Count);
|
|
for (int index = 0; index < pathS.Count; index++)
|
|
{
|
|
if (!IsFinite(pathS[index]) || pathS[index] < 0d ||
|
|
!IsFinite(progressSpeedMetersPerSecond[index]) || progressSpeedMetersPerSecond[index] < 0d)
|
|
{
|
|
throw new ArgumentOutOfRangeException(nameof(pathS));
|
|
}
|
|
copiedPathS.Add(pathS[index]);
|
|
copiedSpeed.Add(progressSpeedMetersPerSecond[index]);
|
|
}
|
|
|
|
PathS = new ReadOnlyCollection<double>(copiedPathS);
|
|
ProgressSpeedMetersPerSecond = new ReadOnlyCollection<double>(copiedSpeed);
|
|
}
|
|
|
|
public IReadOnlyList<double> PathS { get; }
|
|
|
|
public IReadOnlyList<double> ProgressSpeedMetersPerSecond { get; }
|
|
|
|
public static LongitudinalPreviousTrajectorySeed Empty { get { return empty; } }
|
|
|
|
private static bool IsFinite(double value)
|
|
{
|
|
return !double.IsNaN(value) && !double.IsInfinity(value);
|
|
}
|
|
}
|
|
|
|
/// <summary>Builds monotone PathS and progress-speed soft references from a prior published trajectory.</summary>
|
|
public sealed class LongitudinalPreviousTrajectorySeedBuilder
|
|
{
|
|
private const double ProjectionTolerance = 1e-10d;
|
|
|
|
public LongitudinalPreviousTrajectorySeed Build(EmTrajectory previous, LateralPath currentPath,
|
|
DateTimeOffset newEffectiveAtUtc, LongitudinalKnotSchedule knotSchedule, int segmentIndex,
|
|
TravelDirection direction)
|
|
{
|
|
if (knotSchedule == null)
|
|
return LongitudinalPreviousTrajectorySeed.Empty;
|
|
return Build(previous, currentPath, newEffectiveAtUtc, knotSchedule.KnotTimes, segmentIndex, direction);
|
|
}
|
|
|
|
public LongitudinalPreviousTrajectorySeed Build(EmTrajectory previous, LateralPath currentPath,
|
|
DateTimeOffset newEffectiveAtUtc, IReadOnlyList<double> newKnotTimes, int segmentIndex,
|
|
TravelDirection direction)
|
|
{
|
|
try
|
|
{
|
|
if (!IsCompatible(previous, currentPath, newKnotTimes, segmentIndex, direction))
|
|
return LongitudinalPreviousTrajectorySeed.Empty;
|
|
|
|
var pathS = new List<double>(newKnotTimes.Count);
|
|
var progressSpeed = new List<double>(newKnotTimes.Count);
|
|
double previousProjectedPathS = double.NegativeInfinity;
|
|
for (int index = 0; index < newKnotTimes.Count; index++)
|
|
{
|
|
DateTimeOffset sampleUtc = newEffectiveAtUtc.AddSeconds(newKnotTimes[index]);
|
|
double previousTimeSeconds = (sampleUtc - previous.Metadata.EffectiveAtUtc).TotalSeconds;
|
|
if (!TryInterpolate(previous.Points, previousTimeSeconds, out InterpolatedPreviousSample sample) ||
|
|
!TryProjectMonotonically(currentPath, sample.X, sample.Y, previousProjectedPathS,
|
|
out double projectedPathS))
|
|
{
|
|
return LongitudinalPreviousTrajectorySeed.Empty;
|
|
}
|
|
|
|
pathS.Add(projectedPathS);
|
|
progressSpeed.Add(Math.Abs(sample.SignedSpeedMetersPerSecond));
|
|
previousProjectedPathS = projectedPathS;
|
|
}
|
|
|
|
return new LongitudinalPreviousTrajectorySeed(pathS, progressSpeed);
|
|
}
|
|
catch
|
|
{
|
|
return LongitudinalPreviousTrajectorySeed.Empty;
|
|
}
|
|
}
|
|
|
|
private static bool IsCompatible(EmTrajectory previous, LateralPath currentPath,
|
|
IReadOnlyList<double> newKnotTimes, int segmentIndex, TravelDirection direction)
|
|
{
|
|
if (previous == null || currentPath == null || newKnotTimes == null || segmentIndex < 0 ||
|
|
!Enum.IsDefined(typeof(TravelDirection), direction) || !currentPath.IsIndependentlyValidated ||
|
|
currentPath.Points.Count < 2 || previous.Metadata == null || previous.Points.Count < 2 ||
|
|
previous.Metadata.SegmentIndex != segmentIndex || previous.Metadata.Direction != direction)
|
|
{
|
|
return false;
|
|
}
|
|
|
|
double previousKnotTime = double.NegativeInfinity;
|
|
for (int index = 0; index < newKnotTimes.Count; index++)
|
|
{
|
|
if (!IsFinite(newKnotTimes[index]) || newKnotTimes[index] < 0d ||
|
|
newKnotTimes[index] <= previousKnotTime)
|
|
{
|
|
return false;
|
|
}
|
|
previousKnotTime = newKnotTimes[index];
|
|
}
|
|
if (newKnotTimes.Count == 0)
|
|
return false;
|
|
|
|
double previousTime = double.NegativeInfinity;
|
|
for (int index = 0; index < previous.Points.Count; index++)
|
|
{
|
|
EmTrajectoryPoint point = previous.Points[index];
|
|
if (point == null || point.Direction != direction || !IsFinite(point.TimeFromStart) ||
|
|
!IsFinite(point.X) || !IsFinite(point.Y) || !IsFinite(point.SignedLongitudinalVelocity) ||
|
|
point.TimeFromStart <= previousTime)
|
|
{
|
|
return false;
|
|
}
|
|
previousTime = point.TimeFromStart;
|
|
}
|
|
|
|
double previousPathS = double.NegativeInfinity;
|
|
for (int index = 0; index < currentPath.Points.Count; index++)
|
|
{
|
|
LateralPathPoint point = currentPath.Points[index];
|
|
if (point == null || !IsFinite(point.PathS) || !IsFinite(point.X) || !IsFinite(point.Y) ||
|
|
point.PathS <= previousPathS)
|
|
{
|
|
return false;
|
|
}
|
|
previousPathS = point.PathS;
|
|
}
|
|
return true;
|
|
}
|
|
|
|
private static bool TryInterpolate(IReadOnlyList<EmTrajectoryPoint> points, double sampleTimeSeconds,
|
|
out InterpolatedPreviousSample sample)
|
|
{
|
|
sample = default;
|
|
if (!IsFinite(sampleTimeSeconds) || sampleTimeSeconds < points[0].TimeFromStart - ProjectionTolerance ||
|
|
sampleTimeSeconds > points[points.Count - 1].TimeFromStart + ProjectionTolerance)
|
|
{
|
|
return false;
|
|
}
|
|
|
|
if (sampleTimeSeconds <= points[0].TimeFromStart + ProjectionTolerance)
|
|
{
|
|
sample = InterpolatedPreviousSample.From(points[0]);
|
|
return true;
|
|
}
|
|
|
|
for (int index = 1; index < points.Count; index++)
|
|
{
|
|
EmTrajectoryPoint right = points[index];
|
|
if (sampleTimeSeconds <= right.TimeFromStart + ProjectionTolerance)
|
|
{
|
|
EmTrajectoryPoint left = points[index - 1];
|
|
double ratio = (sampleTimeSeconds - left.TimeFromStart) /
|
|
(right.TimeFromStart - left.TimeFromStart);
|
|
ratio = Math.Max(0d, Math.Min(1d, ratio));
|
|
sample = new InterpolatedPreviousSample(
|
|
Linear(left.X, right.X, ratio),
|
|
Linear(left.Y, right.Y, ratio),
|
|
Linear(left.SignedLongitudinalVelocity, right.SignedLongitudinalVelocity, ratio));
|
|
return true;
|
|
}
|
|
}
|
|
|
|
return false;
|
|
}
|
|
|
|
private static bool TryProjectMonotonically(LateralPath path, double x, double y, double minimumPathS,
|
|
out double projectedPathS)
|
|
{
|
|
projectedPathS = 0d;
|
|
double bestDistanceSquared = double.PositiveInfinity;
|
|
bool found = false;
|
|
for (int index = 1; index < path.Points.Count; index++)
|
|
{
|
|
LateralPathPoint left = path.Points[index - 1];
|
|
LateralPathPoint right = path.Points[index];
|
|
double dx = right.X - left.X;
|
|
double dy = right.Y - left.Y;
|
|
double lengthSquared = dx * dx + dy * dy;
|
|
if (!IsFinite(lengthSquared) || lengthSquared <= ProjectionTolerance)
|
|
continue;
|
|
|
|
double ratio = ((x - left.X) * dx + (y - left.Y) * dy) / lengthSquared;
|
|
ratio = Math.Max(0d, Math.Min(1d, ratio));
|
|
double candidatePathS = Linear(left.PathS, right.PathS, ratio);
|
|
if (candidatePathS + ProjectionTolerance < minimumPathS)
|
|
continue;
|
|
|
|
double projectedX = Linear(left.X, right.X, ratio);
|
|
double projectedY = Linear(left.Y, right.Y, ratio);
|
|
double distanceSquared = (x - projectedX) * (x - projectedX) + (y - projectedY) * (y - projectedY);
|
|
if (!found || distanceSquared < bestDistanceSquared - ProjectionTolerance ||
|
|
(Math.Abs(distanceSquared - bestDistanceSquared) <= ProjectionTolerance && candidatePathS < projectedPathS))
|
|
{
|
|
projectedPathS = candidatePathS;
|
|
bestDistanceSquared = distanceSquared;
|
|
found = true;
|
|
}
|
|
}
|
|
|
|
if (!found)
|
|
return false;
|
|
if (minimumPathS > double.NegativeInfinity)
|
|
projectedPathS = Math.Max(minimumPathS, projectedPathS);
|
|
return true;
|
|
}
|
|
|
|
private static double Linear(double left, double right, double ratio)
|
|
{
|
|
return left + (right - left) * ratio;
|
|
}
|
|
|
|
private static bool IsFinite(double value)
|
|
{
|
|
return !double.IsNaN(value) && !double.IsInfinity(value);
|
|
}
|
|
|
|
private readonly struct InterpolatedPreviousSample
|
|
{
|
|
public InterpolatedPreviousSample(double x, double y, double signedSpeedMetersPerSecond)
|
|
{
|
|
X = x;
|
|
Y = y;
|
|
SignedSpeedMetersPerSecond = signedSpeedMetersPerSecond;
|
|
}
|
|
|
|
public double X { get; }
|
|
public double Y { get; }
|
|
public double SignedSpeedMetersPerSecond { get; }
|
|
|
|
public static InterpolatedPreviousSample From(EmTrajectoryPoint point)
|
|
{
|
|
return new InterpolatedPreviousSample(point.X, point.Y, point.SignedLongitudinalVelocity);
|
|
}
|
|
}
|
|
}
|