275 lines
11 KiB
C#
275 lines
11 KiB
C#
using System;
|
|
using System.Collections.Generic;
|
|
using System.Collections.ObjectModel;
|
|
|
|
namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
|
|
|
|
internal sealed class LongitudinalEnvelopeTrustRegion
|
|
{
|
|
internal LongitudinalEnvelopeTrustRegion(IReadOnlyList<double> minimumPathS, IReadOnlyList<double> maximumPathS,
|
|
IReadOnlyList<double> speedSlope, IReadOnlyList<double> speedIntercept,
|
|
IReadOnlyList<int> envelopeSegmentIndex, double scale)
|
|
{
|
|
MinimumPathS = Copy(minimumPathS, nameof(minimumPathS));
|
|
MaximumPathS = Copy(maximumPathS, nameof(maximumPathS));
|
|
SpeedSlope = Copy(speedSlope, nameof(speedSlope));
|
|
SpeedIntercept = Copy(speedIntercept, nameof(speedIntercept));
|
|
EnvelopeSegmentIndex = Copy(envelopeSegmentIndex, nameof(envelopeSegmentIndex));
|
|
Scale = scale;
|
|
}
|
|
|
|
internal IReadOnlyList<double> MinimumPathS { get; }
|
|
|
|
internal IReadOnlyList<double> MaximumPathS { get; }
|
|
|
|
internal IReadOnlyList<double> SpeedSlope { get; }
|
|
|
|
internal IReadOnlyList<double> SpeedIntercept { get; }
|
|
|
|
internal IReadOnlyList<int> EnvelopeSegmentIndex { get; }
|
|
|
|
internal double Scale { get; }
|
|
|
|
internal bool CanShrinkTo(double nextScale, double minimumActiveWidthMeters, out string failureReason)
|
|
{
|
|
failureReason = string.Empty;
|
|
if (!IsFinite(nextScale) || nextScale <= 0d || nextScale >= Scale)
|
|
{
|
|
failureReason = "Next trust-region scale must be finite, positive, and smaller than the current scale.";
|
|
return false;
|
|
}
|
|
if (!IsFinite(minimumActiveWidthMeters) || minimumActiveWidthMeters <= 0d)
|
|
{
|
|
failureReason = "Minimum active width must be finite and positive.";
|
|
return false;
|
|
}
|
|
double ratio = nextScale / Scale;
|
|
for (int index = 0; index < MinimumPathS.Count; index++)
|
|
{
|
|
double currentWidth = MaximumPathS[index] - MinimumPathS[index];
|
|
if (currentWidth > 0d && currentWidth * ratio < minimumActiveWidthMeters)
|
|
{
|
|
failureReason = "The next trust-region scale would fall below the minimum width at knot " + index + ".";
|
|
return false;
|
|
}
|
|
}
|
|
return true;
|
|
}
|
|
|
|
private static IReadOnlyList<double> Copy(IReadOnlyList<double> source, string parameterName)
|
|
{
|
|
if (source == null)
|
|
throw new ArgumentNullException(parameterName);
|
|
var copy = new List<double>(source.Count);
|
|
for (int index = 0; index < source.Count; index++)
|
|
copy.Add(source[index]);
|
|
return new ReadOnlyCollection<double>(copy);
|
|
}
|
|
|
|
private static IReadOnlyList<int> Copy(IReadOnlyList<int> source, string parameterName)
|
|
{
|
|
if (source == null)
|
|
throw new ArgumentNullException(parameterName);
|
|
var copy = new List<int>(source.Count);
|
|
for (int index = 0; index < source.Count; index++)
|
|
copy.Add(source[index]);
|
|
return new ReadOnlyCollection<int>(copy);
|
|
}
|
|
|
|
private static bool IsFinite(double value)
|
|
{
|
|
return !double.IsNaN(value) && !double.IsInfinity(value);
|
|
}
|
|
}
|
|
|
|
internal sealed class LongitudinalEnvelopeTrustRegionBuilder
|
|
{
|
|
private const double ScheduleProgressTolerance = 1e-12d;
|
|
private const double StationSelectionTolerance = 1e-12d;
|
|
|
|
internal bool TryBuild(PathSpeedLimit speedLimit, LongitudinalCandidate anchor,
|
|
IReadOnlyList<double> referencePathS, int terminalHoldStartIndex, double scale,
|
|
double minimumActiveWidthMeters, out LongitudinalEnvelopeTrustRegion region,
|
|
out string failureReason)
|
|
{
|
|
region = null;
|
|
failureReason = string.Empty;
|
|
if (speedLimit == null || anchor == null || referencePathS == null)
|
|
{
|
|
failureReason = "Trust region inputs must be present.";
|
|
return false;
|
|
}
|
|
if (!IsSupportedScale(scale))
|
|
{
|
|
failureReason = "Trust-region scale must be one of 1, 0.5, 0.25, or 0.125.";
|
|
return false;
|
|
}
|
|
if (!IsFinite(minimumActiveWidthMeters) || minimumActiveWidthMeters <= 0d)
|
|
{
|
|
failureReason = "Minimum active width must be finite and positive.";
|
|
return false;
|
|
}
|
|
if (referencePathS.Count != anchor.S.Count)
|
|
{
|
|
failureReason = "Reference PathS count must match the anchor knot count.";
|
|
return false;
|
|
}
|
|
if (terminalHoldStartIndex == -1)
|
|
terminalHoldStartIndex = anchor.S.Count;
|
|
if (terminalHoldStartIndex < 1 || terminalHoldStartIndex > anchor.S.Count)
|
|
{
|
|
failureReason = "Terminal-hold start index is outside the anchor knot range.";
|
|
return false;
|
|
}
|
|
if (!TryValidateAnchorAndReference(speedLimit, anchor, referencePathS, out failureReason))
|
|
return false;
|
|
|
|
int knotCount = anchor.S.Count;
|
|
var minimumPathS = new double[knotCount];
|
|
var maximumPathS = new double[knotCount];
|
|
var speedSlope = new double[knotCount];
|
|
var speedIntercept = new double[knotCount];
|
|
var segmentIndex = new int[knotCount];
|
|
for (int index = 0; index < knotCount; index++)
|
|
{
|
|
double anchorS = anchor.S[index];
|
|
bool fixedKnot = index == 0 || index >= terminalHoldStartIndex;
|
|
int segment = SelectSegment(speedLimit, anchorS, referencePathS, index, fixedKnot);
|
|
FindMaximalExactAffineRun(speedLimit, segment, out int firstSegment, out int lastSegment,
|
|
out double slope, out double intercept);
|
|
double lower = speedLimit.PathS[firstSegment];
|
|
double upper = speedLimit.PathS[lastSegment + 1];
|
|
if (anchorS < lower && lower - anchorS <= StationSelectionTolerance)
|
|
lower = anchorS;
|
|
if (anchorS > upper && anchorS - upper <= StationSelectionTolerance)
|
|
upper = anchorS;
|
|
double trustedLower = fixedKnot ? anchorS : anchorS - scale * (anchorS - lower);
|
|
double trustedUpper = fixedKnot ? anchorS : anchorS + scale * (upper - anchorS);
|
|
if (!fixedKnot && scale < 1d && trustedUpper - trustedLower < minimumActiveWidthMeters)
|
|
{
|
|
failureReason = "Active trust-region interval is narrower than the minimum width at knot " + index + ".";
|
|
return false;
|
|
}
|
|
|
|
minimumPathS[index] = trustedLower;
|
|
maximumPathS[index] = trustedUpper;
|
|
speedSlope[index] = slope;
|
|
speedIntercept[index] = intercept;
|
|
segmentIndex[index] = segment;
|
|
}
|
|
|
|
region = new LongitudinalEnvelopeTrustRegion(minimumPathS, maximumPathS, speedSlope, speedIntercept,
|
|
segmentIndex, scale);
|
|
return true;
|
|
}
|
|
|
|
private static bool TryValidateAnchorAndReference(PathSpeedLimit speedLimit, LongitudinalCandidate anchor,
|
|
IReadOnlyList<double> referencePathS, out string failureReason)
|
|
{
|
|
if (anchor.S[0] != 0d)
|
|
{
|
|
failureReason = "The anchor must begin at exact PathS zero.";
|
|
return false;
|
|
}
|
|
|
|
double minimumPathS = speedLimit.PathS[0];
|
|
double maximumPathS = speedLimit.PathS[speedLimit.PathS.Count - 1];
|
|
double previousAnchorS = double.NegativeInfinity;
|
|
for (int index = 0; index < anchor.S.Count; index++)
|
|
{
|
|
double anchorS = anchor.S[index];
|
|
double referenceS = referencePathS[index];
|
|
if (!IsFinite(anchorS) || anchorS < minimumPathS || anchorS > maximumPathS)
|
|
{
|
|
failureReason = "Anchor PathS is outside the speed-limit range at knot " + index + ".";
|
|
return false;
|
|
}
|
|
if (anchorS < previousAnchorS)
|
|
{
|
|
failureReason = "Anchor PathS must be nondecreasing.";
|
|
return false;
|
|
}
|
|
if (!IsFinite(referenceS))
|
|
{
|
|
failureReason = "Reference PathS must be finite.";
|
|
return false;
|
|
}
|
|
previousAnchorS = anchorS;
|
|
}
|
|
|
|
failureReason = string.Empty;
|
|
return true;
|
|
}
|
|
|
|
private static int SelectSegment(PathSpeedLimit speedLimit, double anchorS, IReadOnlyList<double> referencePathS,
|
|
int knotIndex, bool terminalHold)
|
|
{
|
|
int lastSegment = speedLimit.PathS.Count - 2;
|
|
if (Math.Abs(anchorS - speedLimit.PathS[0]) <= StationSelectionTolerance)
|
|
return 0;
|
|
if (Math.Abs(anchorS - speedLimit.PathS[speedLimit.PathS.Count - 1]) <= StationSelectionTolerance)
|
|
return lastSegment;
|
|
for (int index = 1; index < speedLimit.PathS.Count - 1; index++)
|
|
{
|
|
if (Math.Abs(anchorS - speedLimit.PathS[index]) <= StationSelectionTolerance)
|
|
{
|
|
if (terminalHold)
|
|
return index;
|
|
double scheduleDelta = referencePathS[knotIndex] - referencePathS[knotIndex - 1];
|
|
if (scheduleDelta > ScheduleProgressTolerance)
|
|
return index;
|
|
if (scheduleDelta < -ScheduleProgressTolerance)
|
|
return index - 1;
|
|
double leftWidth = speedLimit.PathS[index] - speedLimit.PathS[index - 1];
|
|
double rightWidth = speedLimit.PathS[index + 1] - speedLimit.PathS[index];
|
|
return rightWidth >= leftWidth ? index : index - 1;
|
|
}
|
|
if (anchorS < speedLimit.PathS[index])
|
|
return index - 1;
|
|
}
|
|
return lastSegment;
|
|
}
|
|
|
|
private static void GetAffineLine(PathSpeedLimit limit, int segment,
|
|
out double slope, out double intercept)
|
|
{
|
|
double lower = limit.PathS[segment];
|
|
double upper = limit.PathS[segment + 1];
|
|
slope = (limit.MaximumSpeedMetersPerSecond[segment + 1] -
|
|
limit.MaximumSpeedMetersPerSecond[segment]) / (upper - lower);
|
|
intercept = limit.MaximumSpeedMetersPerSecond[segment] - slope * lower;
|
|
}
|
|
|
|
private static void FindMaximalExactAffineRun(PathSpeedLimit limit, int selectedSegment,
|
|
out int firstSegment, out int lastSegment, out double slope, out double intercept)
|
|
{
|
|
GetAffineLine(limit, selectedSegment, out slope, out intercept);
|
|
firstSegment = selectedSegment;
|
|
while (firstSegment > 0)
|
|
{
|
|
GetAffineLine(limit, firstSegment - 1, out double candidateSlope, out double candidateIntercept);
|
|
if (candidateSlope != slope || candidateIntercept != intercept)
|
|
break;
|
|
firstSegment--;
|
|
}
|
|
lastSegment = selectedSegment;
|
|
while (lastSegment < limit.PathS.Count - 2)
|
|
{
|
|
GetAffineLine(limit, lastSegment + 1, out double candidateSlope, out double candidateIntercept);
|
|
if (candidateSlope != slope || candidateIntercept != intercept)
|
|
break;
|
|
lastSegment++;
|
|
}
|
|
}
|
|
|
|
private static bool IsSupportedScale(double scale)
|
|
{
|
|
return scale == 1d || scale == 0.5d || scale == 0.25d || scale == 0.125d;
|
|
}
|
|
|
|
private static bool IsFinite(double value)
|
|
{
|
|
return !double.IsNaN(value) && !double.IsInfinity(value);
|
|
}
|
|
}
|