53 lines
1.5 KiB
C#
53 lines
1.5 KiB
C#
using System;
|
|
|
|
namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
|
|
|
|
/// <summary>Deterministic contiguous variable ranges for one time-domain longitudinal QP.</summary>
|
|
public sealed class LongitudinalVariableLayout
|
|
{
|
|
public LongitudinalVariableLayout(int knotCount)
|
|
{
|
|
if (knotCount < 2)
|
|
throw new ArgumentOutOfRangeException(nameof(knotCount), "At least two time knots are required.");
|
|
|
|
KnotCount = knotCount;
|
|
SStart = 0;
|
|
UStart = knotCount;
|
|
AStart = 2 * knotCount;
|
|
JStart = 3 * knotCount;
|
|
VariableCount = 4 * knotCount - 1;
|
|
}
|
|
|
|
public int KnotCount { get; }
|
|
|
|
public int SStart { get; }
|
|
|
|
public int UStart { get; }
|
|
|
|
public int AStart { get; }
|
|
|
|
public int JStart { get; }
|
|
|
|
public int VariableCount { get; }
|
|
|
|
public int S(int knotIndex) { return RequireKnotIndex(knotIndex, SStart); }
|
|
|
|
public int U(int knotIndex) { return RequireKnotIndex(knotIndex, UStart); }
|
|
|
|
public int A(int knotIndex) { return RequireKnotIndex(knotIndex, AStart); }
|
|
|
|
public int J(int intervalIndex)
|
|
{
|
|
if (intervalIndex < 0 || intervalIndex >= KnotCount - 1)
|
|
throw new ArgumentOutOfRangeException(nameof(intervalIndex));
|
|
return JStart + intervalIndex;
|
|
}
|
|
|
|
private int RequireKnotIndex(int knotIndex, int start)
|
|
{
|
|
if (knotIndex < 0 || knotIndex >= KnotCount)
|
|
throw new ArgumentOutOfRangeException(nameof(knotIndex));
|
|
return start + knotIndex;
|
|
}
|
|
}
|