From 52c24e76dbf2dcd1169c8a41e9bf59fe7b2573ea Mon Sep 17 00:00:00 2001
From: Lynix <lynix680@gmail.com>
Date: Tue, 2 Jun 2020 17:06:01 +0200
Subject: [PATCH] SDK/Physics2D: Fix entity rotation not being taken into
 account for static physics

---
 SDK/src/NDK/Components/PhysicsComponent2D.cpp |  8 +++++++-
 SDK/src/NDK/Systems/PhysicsSystem2D.cpp       | 17 +++++++----------
 include/Nazara/Math/Quaternion.hpp            |  1 +
 include/Nazara/Math/Quaternion.inl            | 16 +++++++++++++++-
 4 files changed, 30 insertions(+), 12 deletions(-)

diff --git a/SDK/src/NDK/Components/PhysicsComponent2D.cpp b/SDK/src/NDK/Components/PhysicsComponent2D.cpp
index 871fa762e..3ad5616e6 100644
--- a/SDK/src/NDK/Components/PhysicsComponent2D.cpp
+++ b/SDK/src/NDK/Components/PhysicsComponent2D.cpp
@@ -51,9 +51,15 @@ namespace Ndk
 
 		m_object = std::make_unique<Nz::RigidBody2D>(&world, 1.f, geom);
 		m_object->SetPositionOffset(positionOffset);
-		m_object->SetPosition(Nz::Vector2f(matrix.GetTranslation()));
 		m_object->SetUserdata(reinterpret_cast<void*>(static_cast<std::ptrdiff_t>(m_entity->GetId())));
 
+		if (m_entity->HasComponent<NodeComponent>())
+		{
+			auto& entityNode = m_entity->GetComponent<NodeComponent>();
+			m_object->SetPosition(Nz::Vector2f(entityNode.GetPosition()));
+			m_object->SetRotation(entityNode.GetRotation().To2DAngle());
+		}
+
 		if (m_pendingStates.valid)
 			ApplyPhysicsState(*m_object);
 	}
diff --git a/SDK/src/NDK/Systems/PhysicsSystem2D.cpp b/SDK/src/NDK/Systems/PhysicsSystem2D.cpp
index 4b6386c74..53d2ec0d4 100644
--- a/SDK/src/NDK/Systems/PhysicsSystem2D.cpp
+++ b/SDK/src/NDK/Systems/PhysicsSystem2D.cpp
@@ -249,19 +249,16 @@ namespace Ndk
 			else
 				body->SetVelocity(Nz::Vector2f::Zero());
 
-			/*if (newRotation != oldRotation)
-			{
-				Nz::Quaternionf transition = newRotation * oldRotation.GetConjugate();
-				Nz::EulerAnglesf angles = transition.ToEulerAngles();
-				Nz::Vector3f angularVelocity(Nz::ToRadians(angles.pitch * invElapsedTime),
-				                             Nz::ToRadians(angles.yaw * invElapsedTime),
-				                             Nz::ToRadians(angles.roll * invElapsedTime));
+			Nz::RadianAnglef oldRotation = body->GetRotation();
+			Nz::RadianAnglef newRotation = node.GetRotation(Nz::CoordSys_Global).To2DAngle();
 
-				physObj->SetRotation(oldRotation);
-				physObj->SetAngularVelocity(angularVelocity);
+			if (newRotation != oldRotation)
+			{
+				body->SetRotation(oldRotation);
+				body->SetAngularVelocity((newRotation - oldRotation) * invElapsedTime);
 			}
 			else
-				physObj->SetAngularVelocity(Nz::Vector3f::Zero());*/
+				body->SetAngularVelocity(Nz::RadianAnglef::Zero());
 		}
 	}
 
diff --git a/include/Nazara/Math/Quaternion.hpp b/include/Nazara/Math/Quaternion.hpp
index 991b23b94..e26149573 100644
--- a/include/Nazara/Math/Quaternion.hpp
+++ b/include/Nazara/Math/Quaternion.hpp
@@ -60,6 +60,7 @@ namespace Nz
 
 			T SquaredMagnitude() const;
 
+			RadianAngle<T> To2DAngle() const;
 			EulerAngles<T> ToEulerAngles() const;
 			//Matrix3<T> ToRotationMatrix() const;
 			String ToString() const;
diff --git a/include/Nazara/Math/Quaternion.inl b/include/Nazara/Math/Quaternion.inl
index 6d8937566..68c088eec 100644
--- a/include/Nazara/Math/Quaternion.inl
+++ b/include/Nazara/Math/Quaternion.inl
@@ -462,13 +462,27 @@ namespace Nz
 		return w * w + x * x + y * y + z * z;
 	}
 
+	/*!
+	* \brief Returns the "roll angle" of this quaternion
+	* \return Roll rotation
+	*
+	* \remark This function only has sense when quaternion only represents a "roll rotation"
+	*/
+	template<typename T>
+	RadianAngle<T> Quaternion<T>::To2DAngle() const
+	{
+		T siny_cosp = T(2.0) * (w * z + x * y);
+		T cosy_cosp = T(1.0) - T(2.0) * (y * y + z * z);
+
+		return std::atan2(siny_cosp, cosy_cosp);
+	}
+
 	/*!
 	* \brief Converts this quaternion to Euler angles representation
 	* \return EulerAngles which is the representation of this rotation
 	*
 	* \remark Rotation are "left-handed"
 	*/
-
 	template<typename T>
 	EulerAngles<T> Quaternion<T>::ToEulerAngles() const
 	{