59 lines
2.3 KiB
C#
59 lines
2.3 KiB
C#
using System;
|
|
using MultiWheelC.TrajectoryPlanning.CoarsePath;
|
|
using MultiWheelC.TrajectoryPlanning.Utils;
|
|
|
|
namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
|
|
|
|
/// <summary>Immutable interpolated reference sample in a single direction segment.</summary>
|
|
public sealed class FrenetReferencePoint
|
|
{
|
|
public FrenetReferencePoint(double referenceS, double x, double y, double vehicleYaw, double unwrappedVehicleYaw,
|
|
TravelDirection direction, double geometricCurvature, double vehicleCurvature,
|
|
double vehicleCurvatureDerivative, double bodyClearance)
|
|
{
|
|
RequireFinite(referenceS, nameof(referenceS));
|
|
RequireFinite(x, nameof(x));
|
|
RequireFinite(y, nameof(y));
|
|
RequireFinite(vehicleYaw, nameof(vehicleYaw));
|
|
RequireFinite(unwrappedVehicleYaw, nameof(unwrappedVehicleYaw));
|
|
RequireFinite(geometricCurvature, nameof(geometricCurvature));
|
|
RequireFinite(vehicleCurvature, nameof(vehicleCurvature));
|
|
RequireFinite(vehicleCurvatureDerivative, nameof(vehicleCurvatureDerivative));
|
|
RequireFinite(bodyClearance, nameof(bodyClearance));
|
|
|
|
ReferenceS = referenceS;
|
|
X = x;
|
|
Y = y;
|
|
VehicleYaw = AngleMath.NormalizeRadians(vehicleYaw);
|
|
UnwrappedVehicleYaw = unwrappedVehicleYaw;
|
|
Direction = direction;
|
|
GeometricCurvature = geometricCurvature;
|
|
VehicleCurvature = vehicleCurvature;
|
|
VehicleCurvatureDerivative = vehicleCurvatureDerivative;
|
|
BodyClearance = bodyClearance;
|
|
}
|
|
|
|
public double ReferenceS { get; }
|
|
public double X { get; }
|
|
public double Y { get; }
|
|
public double VehicleYaw { get; }
|
|
public double UnwrappedVehicleYaw { get; }
|
|
public TravelDirection Direction { get; }
|
|
public double GeometricCurvature { get; }
|
|
public double VehicleCurvature { get; }
|
|
public double VehicleCurvatureDerivative { get; }
|
|
public double BodyClearance { get; }
|
|
|
|
/// <summary>Unwrapped direction of travel, used internally for geometry.</summary>
|
|
public double TravelYaw
|
|
{
|
|
get { return Direction == TravelDirection.Forward ? UnwrappedVehicleYaw : UnwrappedVehicleYaw + Math.PI; }
|
|
}
|
|
|
|
private static void RequireFinite(double value, string name)
|
|
{
|
|
if (double.IsNaN(value) || double.IsInfinity(value))
|
|
throw new ArgumentOutOfRangeException(name);
|
|
}
|
|
}
|