62 lines
2.2 KiB
C#
62 lines
2.2 KiB
C#
using System;
|
|
|
|
namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
|
|
|
|
/// <summary>Immutable world-space sample reconstructed from one lateral candidate.</summary>
|
|
public sealed class LateralPathPoint
|
|
{
|
|
public LateralPathPoint(double referenceS, double pathS, double l, double dl, double ddl, double dddl,
|
|
double x, double y, double vehicleYaw, double geometricCurvature, double vehicleCurvature,
|
|
double vehicleCurvatureDerivative)
|
|
{
|
|
RequireFinite(referenceS, nameof(referenceS));
|
|
RequireFinite(pathS, nameof(pathS));
|
|
RequireFinite(l, nameof(l));
|
|
RequireFinite(dl, nameof(dl));
|
|
RequireFinite(ddl, nameof(ddl));
|
|
RequireFinite(dddl, nameof(dddl));
|
|
RequireFinite(x, nameof(x));
|
|
RequireFinite(y, nameof(y));
|
|
RequireFinite(vehicleYaw, nameof(vehicleYaw));
|
|
RequireFinite(geometricCurvature, nameof(geometricCurvature));
|
|
RequireFinite(vehicleCurvature, nameof(vehicleCurvature));
|
|
RequireFinite(vehicleCurvatureDerivative, nameof(vehicleCurvatureDerivative));
|
|
if (referenceS < 0d)
|
|
throw new ArgumentOutOfRangeException(nameof(referenceS));
|
|
if (pathS < 0d)
|
|
throw new ArgumentOutOfRangeException(nameof(pathS));
|
|
|
|
ReferenceS = referenceS;
|
|
PathS = pathS;
|
|
L = l;
|
|
DL = dl;
|
|
DDL = ddl;
|
|
DDDL = dddl;
|
|
X = x;
|
|
Y = y;
|
|
VehicleYaw = vehicleYaw;
|
|
GeometricCurvature = geometricCurvature;
|
|
VehicleCurvature = vehicleCurvature;
|
|
VehicleCurvatureDerivative = vehicleCurvatureDerivative;
|
|
}
|
|
|
|
public double ReferenceS { get; }
|
|
public double PathS { get; }
|
|
public double L { get; }
|
|
public double DL { get; }
|
|
public double DDL { get; }
|
|
public double DDDL { get; }
|
|
public double X { get; }
|
|
public double Y { get; }
|
|
public double VehicleYaw { get; }
|
|
public double GeometricCurvature { get; }
|
|
public double VehicleCurvature { get; }
|
|
public double VehicleCurvatureDerivative { get; }
|
|
|
|
private static void RequireFinite(double value, string parameterName)
|
|
{
|
|
if (double.IsNaN(value) || double.IsInfinity(value))
|
|
throw new ArgumentOutOfRangeException(parameterName);
|
|
}
|
|
}
|