40 lines
1.5 KiB
C#
40 lines
1.5 KiB
C#
using System;
|
|
using System.Collections.Generic;
|
|
|
|
namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
|
|
|
|
public static class LongitudinalTerminalSchedule
|
|
{
|
|
public static int GetStabilizationStartIndex(IReadOnlyList<double> knotTimes,
|
|
double minimumStabilizationDurationSeconds)
|
|
{
|
|
if (knotTimes == null || knotTimes.Count < 3 ||
|
|
double.IsNaN(minimumStabilizationDurationSeconds) ||
|
|
double.IsInfinity(minimumStabilizationDurationSeconds) ||
|
|
minimumStabilizationDurationSeconds <= 0d)
|
|
{
|
|
throw new ArgumentException("A positive stabilization tail and at least three knots are required.");
|
|
}
|
|
|
|
double previous = double.NegativeInfinity;
|
|
for (int index = 0; index < knotTimes.Count; index++)
|
|
{
|
|
if (double.IsNaN(knotTimes[index]) || double.IsInfinity(knotTimes[index]) ||
|
|
knotTimes[index] <= previous)
|
|
{
|
|
throw new ArgumentException("Terminal-schedule knots must be finite and strictly increasing.");
|
|
}
|
|
previous = knotTimes[index];
|
|
}
|
|
|
|
double finalTime = knotTimes[knotTimes.Count - 1];
|
|
for (int index = knotTimes.Count - 2; index >= 1; index--)
|
|
{
|
|
if (finalTime - knotTimes[index] >= minimumStabilizationDurationSeconds - 1e-12d)
|
|
return index;
|
|
}
|
|
|
|
throw new ArgumentException("The time horizon cannot contain a full terminal stabilization interval.");
|
|
}
|
|
}
|