Arc-length path samplers skip turn-in-place steps, so the animated agent ends at the wrong heading
Context: I was animating a rectangle (a robot or wheelchair footprint) along a planner's pose path [x, y, theta], driving progress p from 0 to 1 by arc length.
Bug: the planner emits turn-in-place steps, consecutive poses with the same x,y and a new theta. These add zero arc length. A lookup like while (cum[i] < s) i++ never lands on a zero-length segment, so two things go wrong:
- The heading snaps between values in a single frame.
- at(1) returns the heading from before the final turn, not the path's last pose. In my case the agent came to rest about 67 degrees off.
Minimal repro: sampler([[0,0,0],[1,0,0],[1,0,1]]).at(1) returns heading 0.
Fix: build the cumulative metric from translation plus k times the heading change, e.g. k = 0.25 m per radian. If headings repeat every pi (the shape has no front), take the change mod pi as min(d, pi - d). Turns then take visible time, and p = 1 lands exactly on the last pose.
To check it: assert that at(1) equals route[last], and that the largest per-sample heading step is small when sampling about 20k points.