46 lines
1.9 KiB
C#
46 lines
1.9 KiB
C#
using System;
|
|
using MultiWheelC.TrajectoryPlanning.CoarsePath;
|
|
using MultiWheelC.TrajectoryPlanning.Utils;
|
|
|
|
namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
|
|
|
|
/// <summary>Coordinate conversion that keeps Frenet lateral sign relative to travel direction.</summary>
|
|
public static class FrenetTransform
|
|
{
|
|
public static bool TryReconstruct(FrenetReferencePoint referencePoint, double lateralOffset, double lateralDerivative,
|
|
double minimumFrenetDenominator, out Pose2D pose)
|
|
{
|
|
pose = null;
|
|
if (referencePoint == null || !IsFinite(lateralOffset) || !IsFinite(lateralDerivative) ||
|
|
!IsFinite(minimumFrenetDenominator) || minimumFrenetDenominator <= 0d)
|
|
return false;
|
|
|
|
double denominator = 1d - referencePoint.GeometricCurvature * lateralOffset;
|
|
if (!IsFinite(denominator) || denominator < minimumFrenetDenominator)
|
|
return false;
|
|
|
|
double travelYaw = referencePoint.TravelYaw;
|
|
double x = referencePoint.X - lateralOffset * Math.Sin(travelYaw);
|
|
double y = referencePoint.Y + lateralOffset * Math.Cos(travelYaw);
|
|
double optimizedTravelYaw = travelYaw + Math.Atan2(lateralDerivative, denominator);
|
|
double vehicleYaw = referencePoint.Direction == TravelDirection.Forward
|
|
? AngleMath.NormalizeRadians(optimizedTravelYaw)
|
|
: AngleMath.NormalizeRadians(optimizedTravelYaw + Math.PI);
|
|
if (!IsFinite(x) || !IsFinite(y) || !IsFinite(vehicleYaw))
|
|
return false;
|
|
|
|
pose = new Pose2D(x, y, vehicleYaw);
|
|
return true;
|
|
}
|
|
|
|
internal static double GetTravelYaw(double vehicleYaw, TravelDirection direction)
|
|
{
|
|
return direction == TravelDirection.Forward ? vehicleYaw : vehicleYaw + Math.PI;
|
|
}
|
|
|
|
private static bool IsFinite(double value)
|
|
{
|
|
return !double.IsNaN(value) && !double.IsInfinity(value);
|
|
}
|
|
}
|