64 lines
1.6 KiB
C#
64 lines
1.6 KiB
C#
using System;
|
|
|
|
namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
|
|
|
|
/// <summary>Deterministic variable ranges for one lateral QP discretized on N stations.</summary>
|
|
public sealed class LateralVariableLayout
|
|
{
|
|
public LateralVariableLayout(int stationCount)
|
|
{
|
|
if (stationCount < 2)
|
|
throw new ArgumentOutOfRangeException(nameof(stationCount), "At least two reference-S stations are required.");
|
|
|
|
StationCount = stationCount;
|
|
LStart = 0;
|
|
DLStart = stationCount;
|
|
DDLStart = 2 * stationCount;
|
|
DDDLStart = 3 * stationCount;
|
|
VariableCount = 4 * stationCount - 1;
|
|
}
|
|
|
|
public int StationCount { get; }
|
|
|
|
public int LStart { get; }
|
|
|
|
public int DLStart { get; }
|
|
|
|
public int DDLStart { get; }
|
|
|
|
public int DDDLStart { get; }
|
|
|
|
public int VariableCount { get; }
|
|
|
|
public int L(int stationIndex)
|
|
{
|
|
RequireStationIndex(stationIndex);
|
|
return LStart + stationIndex;
|
|
}
|
|
|
|
public int DL(int stationIndex)
|
|
{
|
|
RequireStationIndex(stationIndex);
|
|
return DLStart + stationIndex;
|
|
}
|
|
|
|
public int DDL(int stationIndex)
|
|
{
|
|
RequireStationIndex(stationIndex);
|
|
return DDLStart + stationIndex;
|
|
}
|
|
|
|
public int DDDL(int intervalIndex)
|
|
{
|
|
if (intervalIndex < 0 || intervalIndex >= StationCount - 1)
|
|
throw new ArgumentOutOfRangeException(nameof(intervalIndex));
|
|
return DDDLStart + intervalIndex;
|
|
}
|
|
|
|
private void RequireStationIndex(int stationIndex)
|
|
{
|
|
if (stationIndex < 0 || stationIndex >= StationCount)
|
|
throw new ArgumentOutOfRangeException(nameof(stationIndex));
|
|
}
|
|
}
|