From 077584ec491b38a099b40c59f52ca88e041d726b Mon Sep 17 00:00:00 2001
From: Gawaboumga <yourihubaut@homail.com>
Date: Fri, 21 Aug 2015 11:53:16 +0200
Subject: [PATCH] EulerAngles simplification

Former-commit-id: 775b07a51077e6f17b9fe05773dc5c277c49919d
---
 include/Nazara/Math/EulerAngles.inl | 15 ++++--
 tests/Nazara/Math/EulerAngles.cpp   | 83 +++++++++++++++++++++++++++++
 2 files changed, 94 insertions(+), 4 deletions(-)
 create mode 100644 tests/Nazara/Math/EulerAngles.cpp

diff --git a/include/Nazara/Math/EulerAngles.inl b/include/Nazara/Math/EulerAngles.inl
index 8e29a40d9..b02e5f6ed 100644
--- a/include/Nazara/Math/EulerAngles.inl
+++ b/include/Nazara/Math/EulerAngles.inl
@@ -91,11 +91,18 @@ void NzEulerAngles<T>::Set(const NzEulerAngles<U>& angles)
 template<typename T>
 NzQuaternion<T> NzEulerAngles<T>::ToQuaternion() const
 {
-	NzQuaternion<T> rotX(pitch, NzVector3<T>::UnitX());
-	NzQuaternion<T> rotY(yaw, NzVector3<T>::UnitY());
-	NzQuaternion<T> rotZ(roll, NzVector3<T>::UnitZ());
+	T c1 = std::cos(NzToRadians(yaw) / F(2.0));
+	T c2 = std::cos(NzToRadians(roll) / F(2.0));
+	T c3 = std::cos(NzToRadians(pitch) / F(2.0));
 
-	return rotY * rotX * rotZ;
+	T s1 = std::sin(NzToRadians(yaw) / F(2.0));
+	T s2 = std::sin(NzToRadians(roll) / F(2.0));
+	T s3 = std::sin(NzToRadians(pitch) / F(2.0));
+
+	return NzQuaternion<T>(c1 * c2 * c3 - s1 * s2 * s3,
+	                       s1 * s2 * c3 + c1 * c2 * s3,
+	                       s1 * c2 * c3 + c1 * s2 * s3,
+	                       c1 * s2 * c3 - s1 * c2 * s3);
 }
 
 template<typename T>
diff --git a/tests/Nazara/Math/EulerAngles.cpp b/tests/Nazara/Math/EulerAngles.cpp
new file mode 100644
index 000000000..6a4508179
--- /dev/null
+++ b/tests/Nazara/Math/EulerAngles.cpp
@@ -0,0 +1,83 @@
+#include <Nazara/Math/EulerAngles.hpp>
+#include <catch.hpp>
+
+SCENARIO("EulerAngles", "[MATH][EULERANGLES]")
+{
+	GIVEN("Two zero euler angles")
+	{
+		NzEulerAnglesf firstZero(0.f, 0.f, 0.f);
+		NzEulerAnglesf secondZero(NzEulerAngles<int>::Zero());
+
+		THEN("They should be equal")
+		{
+			REQUIRE(firstZero == secondZero);
+		}
+
+		WHEN("We do some operations")
+		{
+			NzEulerAnglesf euler90(90.f, 90.f, 90.f);
+			NzEulerAnglesf euler270(270.f, 270.f, 270.f);
+
+			NzEulerAnglesf euler360 = euler90 + euler270;
+			euler360.Normalize();
+			NzEulerAnglesf euler0 = euler270 - euler90;
+			euler0 -= euler90;
+			euler0 -= euler90;
+
+			THEN("They should still be equal")
+			{
+				REQUIRE(euler360 == firstZero);
+				REQUIRE(euler0 == secondZero);
+			}
+		}
+
+		WHEN("We ask for conversion to quaternion")
+		{
+			THEN("They are the same")
+			{
+				REQUIRE(firstZero.ToQuaternion() == secondZero.ToQuaternion());
+				REQUIRE(firstZero.ToQuaternion() == NzEulerAnglesf(NzQuaternionf(1.f, 0.f, 0.f, 0.f)));
+				REQUIRE(secondZero.ToQuaternion() == NzEulerAnglesf(NzQuaternionf(1.f, 0.f, 0.f, 0.f)));
+			}
+		}
+	}
+
+	GIVEN("Euler angles with rotation 45 on each axis")
+	{
+		WHEN("We convert to quaternion")
+		{
+			THEN("These results are expected")
+			{
+				REQUIRE(NzEulerAnglesf(NzFromDegrees(45.f), 0.f, 0.f) == NzQuaternionf(0.923879504204f, 0.382683455944f, 0.f, 0.f).ToEulerAngles());
+				REQUIRE(NzEulerAnglesf(0.f, NzFromDegrees(45.f), 0.f) == NzQuaternionf(0.923879504204f, 0.f, 0.382683455944f, 0.f).ToEulerAngles());
+				//REQUIRE(NzEulerAnglesf(0.f, 0.f, NzFromDegrees(45.f)) == NzQuaternionf(0.923879504204f, 0.f, 0.f, 0.382683455944f).ToEulerAngles());
+			}
+		}
+	}
+
+	GIVEN("Three euler angles: (0, 22.5, 22.5), (90, 90, 0) and (30, 0, 30)")
+	{
+		NzEulerAnglesf euler45(NzFromDegrees(0.f), NzFromDegrees(22.5f), NzFromDegrees(22.5f));
+		NzEulerAnglesf euler90(NzFromDegrees(90.f), NzFromDegrees(90.f), NzFromDegrees(0.f));
+		NzEulerAnglesf euler30(NzFromDegrees(30.f), NzFromDegrees(0.f), NzFromDegrees(30.f));
+
+		WHEN("We convert them to quaternion")
+		{
+			THEN("And then convert to euler angles, we have identity")
+			{
+				NzEulerAnglesf tmp = NzQuaternionf(euler45.ToQuaternion()).ToEulerAngles();
+				REQUIRE(tmp.pitch == Approx(0.f));
+				REQUIRE(tmp.yaw == Approx(22.5f));
+				REQUIRE(tmp.roll == Approx(22.5f));
+				tmp = NzQuaternionf(euler90.ToQuaternion()).ToEulerAngles();
+				REQUIRE(tmp.pitch == Approx(90.f));
+				REQUIRE(tmp.yaw == Approx(90.f));
+				REQUIRE(tmp.roll == Approx(0.f));
+				tmp = NzQuaternionf(euler30.ToQuaternion()).ToEulerAngles();
+				REQUIRE(tmp.pitch == Approx(30.f));
+				REQUIRE(tmp.yaw == Approx(0.f));
+				REQUIRE(tmp.roll == Approx(30.f));
+			}
+		}
+	}
+}