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
@@ -196,6 +196,15 @@ $straightEnd = $straightAnalysis.Path[$straightAnalysis.Path.Count - 1]
 Assert-Near 1.0 $straightEnd.X 0.0 'Resampling must retain the exact final X coordinate.'
 Assert-Near 0.0 $straightEnd.Y 0.0 'Resampling must retain the exact final Y coordinate.'

+# Raw coarse anchors carry the vehicle pose, which may differ slightly from the chord tangent of a finite integration step.
+$poseAnchoredCurve = New-DirectionSegment 0 $forward @(
+    (New-GeometryPoint 0.0 0.0 0.0 0.0 0.0),
+    (New-GeometryPoint 1.0 0.2 1.0 0.4 0.4))
+$poseAnchoredAnalysis = Invoke-Analysis @($poseAnchoredCurve)
+Assert-Near 0.0 $poseAnchoredAnalysis.Path[0].Heading 0.000000000001 'Geometry analysis must retain the first coarse-anchor heading rather than replace it with a chord tangent.'
+$poseAnchoredEnd = $poseAnchoredAnalysis.Path[$poseAnchoredAnalysis.Path.Count - 1]
+Assert-Near 0.4 $poseAnchoredEnd.Heading 0.000000000001 'Geometry analysis must retain the final coarse-anchor heading rather than replace it with a chord tangent.'
+
 # A forward R=2 quarter circle has positive +0.5 1/m vehicle curvature.
 $forwardArcPoints = New-Object System.Collections.Generic.List[object]
 for ($index = 0; $index -le 32; $index++) {