115 lines
4.6 KiB
C#
115 lines
4.6 KiB
C#
using System;
|
|
using System.Collections.Generic;
|
|
using System.Collections.ObjectModel;
|
|
|
|
namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
|
|
|
|
/// <summary>One discrete lateral iterate expressed against exact reference-S stations.</summary>
|
|
public sealed class LateralCandidate
|
|
{
|
|
public LateralCandidate(IReadOnlyList<double> referenceStations, IReadOnlyList<double> l, IReadOnlyList<double> dl,
|
|
IReadOnlyList<double> ddl, IReadOnlyList<double> dddl)
|
|
{
|
|
ReferenceStations = CopyStations(referenceStations);
|
|
int stationCount = ReferenceStations.Count;
|
|
L = CopyValues(l, stationCount, nameof(l));
|
|
DL = CopyValues(dl, stationCount, nameof(dl));
|
|
DDL = CopyValues(ddl, stationCount, nameof(ddl));
|
|
DDDL = CopyValues(dddl, stationCount - 1, nameof(dddl));
|
|
}
|
|
|
|
public IReadOnlyList<double> ReferenceStations { get; }
|
|
|
|
public IReadOnlyList<double> L { get; }
|
|
|
|
public IReadOnlyList<double> DL { get; }
|
|
|
|
public IReadOnlyList<double> DDL { get; }
|
|
|
|
public IReadOnlyList<double> DDDL { get; }
|
|
|
|
public static LateralCandidate Integrate(IReadOnlyList<double> referenceStations, double initialL, double initialDL,
|
|
double initialDDL, IReadOnlyList<double> dddl)
|
|
{
|
|
IReadOnlyList<double> stations = CopyStations(referenceStations);
|
|
if (!IsFinite(initialL) || !IsFinite(initialDL) || !IsFinite(initialDDL))
|
|
throw new ArgumentOutOfRangeException(nameof(initialL));
|
|
IReadOnlyList<double> copiedJerk = CopyValues(dddl, stations.Count - 1, nameof(dddl));
|
|
|
|
var l = new double[stations.Count];
|
|
var dl = new double[stations.Count];
|
|
var ddl = new double[stations.Count];
|
|
l[0] = initialL;
|
|
dl[0] = initialDL;
|
|
ddl[0] = initialDDL;
|
|
for (int index = 0; index < copiedJerk.Count; index++)
|
|
{
|
|
double ds = stations[index + 1] - stations[index];
|
|
double jerk = copiedJerk[index];
|
|
ddl[index + 1] = ddl[index] + ds * jerk;
|
|
dl[index + 1] = dl[index] + ds * ddl[index] + 0.5d * ds * ds * jerk;
|
|
l[index + 1] = l[index] + ds * dl[index] + 0.5d * ds * ds * ddl[index] +
|
|
ds * ds * ds * jerk / 6d;
|
|
}
|
|
return new LateralCandidate(stations, l, dl, ddl, copiedJerk);
|
|
}
|
|
|
|
public bool SatisfiesExactDiscreteDynamics(double tolerance)
|
|
{
|
|
if (!IsFinite(tolerance) || tolerance < 0d)
|
|
throw new ArgumentOutOfRangeException(nameof(tolerance));
|
|
|
|
for (int index = 0; index < DDDL.Count; index++)
|
|
{
|
|
double ds = ReferenceStations[index + 1] - ReferenceStations[index];
|
|
double jerk = DDDL[index];
|
|
if (Math.Abs(DDL[index + 1] - (DDL[index] + ds * jerk)) > tolerance ||
|
|
Math.Abs(DL[index + 1] - (DL[index] + ds * DDL[index] + 0.5d * ds * ds * jerk)) > tolerance ||
|
|
Math.Abs(L[index + 1] - (L[index] + ds * DL[index] + 0.5d * ds * ds * DDL[index] +
|
|
ds * ds * ds * jerk / 6d)) > tolerance)
|
|
{
|
|
return false;
|
|
}
|
|
}
|
|
return true;
|
|
}
|
|
|
|
private static IReadOnlyList<double> CopyStations(IReadOnlyList<double> values)
|
|
{
|
|
if (values == null || values.Count < 2)
|
|
throw new ArgumentException("At least two reference-S stations are required.", nameof(values));
|
|
|
|
var copy = new List<double>(values.Count);
|
|
double previous = double.NegativeInfinity;
|
|
for (int index = 0; index < values.Count; index++)
|
|
{
|
|
double value = values[index];
|
|
if (!IsFinite(value) || value <= previous)
|
|
throw new ArgumentException("Reference-S stations must be finite and strictly increasing.", nameof(values));
|
|
copy.Add(value);
|
|
previous = value;
|
|
}
|
|
return new ReadOnlyCollection<double>(copy);
|
|
}
|
|
|
|
private static IReadOnlyList<double> CopyValues(IReadOnlyList<double> values, int expectedCount, string parameterName)
|
|
{
|
|
if (values == null || values.Count != expectedCount)
|
|
throw new ArgumentException("Lateral value count does not match the station layout.", parameterName);
|
|
|
|
var copy = new List<double>(values.Count);
|
|
for (int index = 0; index < values.Count; index++)
|
|
{
|
|
if (!IsFinite(values[index]))
|
|
throw new ArgumentOutOfRangeException(parameterName);
|
|
copy.Add(values[index]);
|
|
}
|
|
return new ReadOnlyCollection<double>(copy);
|
|
}
|
|
|
|
private static bool IsFinite(double value)
|
|
{
|
|
return !double.IsNaN(value) && !double.IsInfinity(value);
|
|
}
|
|
}
|