chore: save current workspace progress

2026-08-09 22:13:18 +08:00
parent 650c2ab0e3
commit 2f4fd15e52
449 changed files with 76593 additions and 971 deletions
@@ -0,0 +1,34 @@
+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;
+    }
+}