34 lines
1.2 KiB
C#
34 lines
1.2 KiB
C#
using System;
|
|
|
|
namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
|
|
|
|
/// <summary>World pose projected onto one bounded direction segment.</summary>
|
|
public sealed class FrenetProjection
|
|
{
|
|
public FrenetProjection(FrenetReferencePoint referencePoint, double lateralOffset, double headingError,
|
|
double squaredDistanceMeters)
|
|
{
|
|
if (referencePoint == null)
|
|
throw new ArgumentNullException(nameof(referencePoint));
|
|
if (!IsFinite(lateralOffset) || !IsFinite(headingError) || !IsFinite(squaredDistanceMeters) || squaredDistanceMeters < 0d)
|
|
throw new ArgumentOutOfRangeException(nameof(lateralOffset));
|
|
|
|
ReferencePoint = referencePoint;
|
|
ReferenceS = referencePoint.ReferenceS;
|
|
LateralOffset = lateralOffset;
|
|
HeadingError = headingError;
|
|
SquaredDistanceMeters = squaredDistanceMeters;
|
|
}
|
|
|
|
public FrenetReferencePoint ReferencePoint { get; }
|
|
public double ReferenceS { get; }
|
|
public double LateralOffset { get; }
|
|
public double HeadingError { get; }
|
|
public double SquaredDistanceMeters { get; }
|
|
|
|
private static bool IsFinite(double value)
|
|
{
|
|
return !double.IsNaN(value) && !double.IsInfinity(value);
|
|
}
|
|
}
|