From 6d4c7db767da06b44fb3a238b2015af7da8674f1 Mon Sep 17 00:00:00 2001
From: Narcha <42248344+Narcha@users.noreply.github.com>
Date: Sat, 5 Oct 2024 16:29:29 +0200
Subject: [PATCH 1/2] This looked wrong

---
 crates/control/src/motion/step_planner.rs | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/crates/control/src/motion/step_planner.rs b/crates/control/src/motion/step_planner.rs
index 98f5d72d48..4aed0c0b6c 100644
--- a/crates/control/src/motion/step_planner.rs
+++ b/crates/control/src/motion/step_planner.rs
@@ -87,7 +87,7 @@ impl StepPlanner {
 
         let target_pose = match segment {
             PathSegment::LineSegment(line_segment) => {
-                let direction = line_segment.1;
+                let direction = line_segment.1 - line_segment.0;
                 let rotation = if direction.coords().norm_squared() < f32::EPSILON {
                     Orientation2::identity()
                 } else {

From dd430bf221627880449d4fd731fa302883cc710e Mon Sep 17 00:00:00 2001
From: Narcha <42248344+Narcha@users.noreply.github.com>
Date: Sat, 5 Oct 2024 16:29:48 +0200
Subject: [PATCH 2/2] Simplify

---
 crates/control/src/motion/step_planner.rs | 10 +---------
 1 file changed, 1 insertion(+), 9 deletions(-)

diff --git a/crates/control/src/motion/step_planner.rs b/crates/control/src/motion/step_planner.rs
index 4aed0c0b6c..94af8af9c0 100644
--- a/crates/control/src/motion/step_planner.rs
+++ b/crates/control/src/motion/step_planner.rs
@@ -88,15 +88,7 @@ impl StepPlanner {
         let target_pose = match segment {
             PathSegment::LineSegment(line_segment) => {
                 let direction = line_segment.1 - line_segment.0;
-                let rotation = if direction.coords().norm_squared() < f32::EPSILON {
-                    Orientation2::identity()
-                } else {
-                    let normalized_direction = direction.coords().normalize();
-                    Orientation2::from_cos_sin_unchecked(
-                        normalized_direction.x(),
-                        normalized_direction.y(),
-                    )
-                };
+                let rotation = Orientation2::from_vector(direction);
                 Pose2::from_parts(line_segment.1, rotation)
             }
             PathSegment::Arc(arc, orientation) => {