From 07b2f5342183a3a396ff7de09aacb183058c6e70 Mon Sep 17 00:00:00 2001
From: Zeerek Ahmad <zeerekahmad@hotmail.com>
Date: Sun, 28 Jun 2026 22:55:11 -0700
Subject: [PATCH] If linear is 0, no matter what we want to return 0

---
 src/articulated_model.cpp | 7 ++++---
 1 file changed, 4 insertions(+), 3 deletions(-)

diff --git a/src/articulated_model.cpp b/src/articulated_model.cpp
index 0b04a59..9d2fd97 100644
--- a/src/articulated_model.cpp
+++ b/src/articulated_model.cpp
@@ -24,10 +24,11 @@ namespace polymath::kinematics
 ArticulatedVehicleState ArticulatedModel::bodyVelocityToVehicleState(
   double linear_velocity_m_s, double angular_velocity_rad_s)
 {
-  // Guard: fully stationary — sqrt denominator collapses to 0, producing 0/0 = NaN
+  // Guard: If fully stationary — sqrt denominator collapses to 0, producing 0/0 = NaN
+  // Guard: If linear velocity is 0 and angular velocity is > 0, atan2 -> pi which can cause jumps. Handle in calling system.
   if (
-    std::abs(linear_velocity_m_s) < ZERO_VELOCITY_THRESHOLD &&
-    std::abs(angular_velocity_rad_s) < ZERO_VELOCITY_THRESHOLD)
+    std::abs(linear_velocity_m_s) < ZERO_VELOCITY_THRESHOLD 
+  )
   {
     return ArticulatedVehicleState{
       0.0, 0.0, 0.0, 0.0, 0.0, 0.0, std::numeric_limits<double>::infinity(), std::numeric_limits<double>::infinity()};