fix: preserve raw path smoothing baselines

2026-07-30 08:42:14 +08:00
parent 09953ab1f1
commit 20215defdd
5 changed files with 197 additions and 149 deletions
@@ -144,21 +144,14 @@ public sealed class PathGeometryAnalyzer
        for (int index = 0; index < count; index++)
        {
            double travelHeading;
-            if (count == 1)
+            if (index == 0 || index == count - 1)
            {
+                // Coarse-path endpoints encode the vehicle pose at the exact integration anchor.
+                // A chord across a finite integration step is not that pose's heading.
                travelHeading = segment.Direction == TravelDirection.Forward
                    ? samples[index].Heading
                    : samples[index].Heading - Math.PI;
            }
-            else if (index == 0)
-            {
-                travelHeading = Math.Atan2(samples[1].Y - samples[0].Y, samples[1].X - samples[0].X);
-            }
-            else if (index == count - 1)
-            {
-                travelHeading = Math.Atan2(samples[index].Y - samples[index - 1].Y,
-                    samples[index].X - samples[index - 1].X);
-            }
            else
            {
                travelHeading = Math.Atan2(samples[index + 1].Y - samples[index - 1].Y,