From 85182ac2b82504e419f3bbd83a8da08abf2e81ba Mon Sep 17 00:00:00 2001
From: Oleg Kalachev <okalachev@gmail.com>
Date: Tue, 20 Feb 2024 04:47:13 +0300
Subject: [PATCH] =?UTF-8?q?Use=20more=20correct=20implementation=20of=20to?=
 =?UTF-8?q?EulerZYX=20fixing=20some=20yaw=20issues=20We=20actually=20need?=
 =?UTF-8?q?=20to=20use=20Tait=E2=80=93Bryan=20Z-Y-X=20angles,=20not=20clas?=
 =?UTF-8?q?sic=20Euler's?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 flix/quaternion.h | 26 ++++++++++++++++++++++----
 1 file changed, 22 insertions(+), 4 deletions(-)

diff --git a/flix/quaternion.h b/flix/quaternion.h
index 8aee072..c535d63 100644
--- a/flix/quaternion.h
+++ b/flix/quaternion.h
@@ -68,10 +68,28 @@ public:
 	}
 
 	Vector toEulerZYX() const {
-		return Vector(
-			atan2(2 * (w * x + y * z), 1 - 2 * (x * x + y * y)),
-			asin(2 * (w * y - z * x)),
-			atan2(2 * (w * z + x * y), 1 - 2 * (y * y + z * z)));
+		// https://github.com/ros/geometry2/blob/589caf083cae9d8fae7effdb910454b4681b9ec1/tf2/include/tf2/impl/utils.h#L87
+		Vector euler;
+		float sqx = x * x;
+		float sqy = y * y;
+		float sqz = z * z;
+		float sqw = w * w;
+		// Cases derived from https://orbitalstation.wordpress.com/tag/quaternion/
+		float sarg = -2 * (x * z - w * y) / (sqx + sqy + sqz + sqw); /* normalization added from urdfom_headers */
+		if (sarg <= -0.99999) {
+			euler.x = 0;
+			euler.y = -0.5 * PI;
+			euler.z = -2 * atan2(y, x);
+		} else if (sarg >= 0.99999) {
+			euler.x = 0;
+			euler.y = 0.5 * PI;
+			euler.z = 2 * atan2(y, x);
+		} else {
+			euler.x = atan2(2 * (y * z + w * x), sqw - sqx - sqy + sqz);
+			euler.y = asin(sarg);
+			euler.z = atan2(2 * (x * y + w * z), sqw + sqx - sqy - sqz);
+		}
+		return euler;
 	}
 
 	float getYaw() const {