35 lines
1.1 KiB
C#
35 lines
1.1 KiB
C#
using System;
|
|
|
|
namespace MultiWheelC.TrajectoryPlanning.Utils;
|
|
|
|
/// <summary>Deterministic angle helpers used by planning code.</summary>
|
|
public static class AngleMath
|
|
{
|
|
public static double NormalizeRadians(double radians)
|
|
{
|
|
if (!NumericGuard.IsFinite(radians))
|
|
return double.NaN;
|
|
|
|
double normalized = radians % (2d * Math.PI);
|
|
if (normalized >= Math.PI) normalized -= 2d * Math.PI;
|
|
if (normalized < -Math.PI) normalized += 2d * Math.PI;
|
|
return normalized;
|
|
}
|
|
|
|
public static double ShortestSignedDifference(double from, double to)
|
|
{
|
|
return NormalizeRadians(to - from);
|
|
}
|
|
|
|
public static int ToHeadingIndex(double heading, double resolution, int binCount)
|
|
{
|
|
if (!NumericGuard.IsFinite(heading) || !NumericGuard.IsPositiveFinite(resolution) || binCount <= 0)
|
|
throw new ArgumentOutOfRangeException();
|
|
|
|
double normalized = NormalizeRadians(heading);
|
|
int index = (int)Math.Floor((normalized + Math.PI) / resolution);
|
|
index %= binCount;
|
|
return index < 0 ? index + binCount : index;
|
|
}
|
|
}
|