From 7639201d70129b05e771ee6a7c2ca3a60cba4129 Mon Sep 17 00:00:00 2001
From: Elvin Yang <eyang@cs.utexas.edu>
Date: Tue, 1 Nov 2022 17:39:54 -0500
Subject: [PATCH] Bugfix: intermediate state for straight arcs

---
 src/navigation/constant_curvature_arcs.cc | 14 ++------------
 1 file changed, 2 insertions(+), 12 deletions(-)

diff --git a/src/navigation/constant_curvature_arcs.cc b/src/navigation/constant_curvature_arcs.cc
index b64c943..163a281 100644
--- a/src/navigation/constant_curvature_arcs.cc
+++ b/src/navigation/constant_curvature_arcs.cc
@@ -76,24 +76,14 @@ Pose2Df ConstantCurvatureArc::GetIntermediateState(float f) const {
   }
   if (fabs(curvature) < FLT_MIN) {
     // Straight-line motion
-    return Pose2Df(0, Vector2f(length, 0));
+    return Pose2Df(0, Vector2f(f * length, 0));
   }
   const float r = 1.0 / curvature;
   return Pose2Df(a, r * Vector2f(sin(a), 1.0 - cos(a)));
 }
 
 Pose2Df ConstantCurvatureArc::EndPoint() const {
-  const float a = Sign(curvature) * angular_length;
-  if (length == 0) {
-    // Pure rotational motion.
-    return Pose2Df(a, Vector2f(0, 0));
-  }
-  if (fabs(curvature) < FLT_MIN) {
-    // Straight-line motion
-    return Pose2Df(0, Vector2f(length, 0));
-  }
-  const float r = 1.0 / curvature;
-  return Pose2Df(a, r * Vector2f(sin(a), 1.0 - cos(a)));
+  return GetIntermediateState(1.0);
 }
 
 }  // namespace motion_primitives