From fdf0c8a71ff0cd08e24047b9cdd580576f5f9f28 Mon Sep 17 00:00:00 2001
From: Lynix <lynix680@gmail.com>
Date: Fri, 19 Apr 2013 13:59:57 +0200
Subject: [PATCH] Added Node::To[Global|Local]*

Fixed Node::Interpolate when used in global coord sys


Former-commit-id: 1963a6c6d3df8b2bc8a3f7a2a7a7102611ca6493
---
 include/Nazara/Utility/Node.hpp | 12 +++++-
 src/Nazara/Utility/Node.cpp     | 73 +++++++++++++++++++++++++++++++--
 src/Nazara/Utility/Skeleton.cpp |  4 +-
 3 files changed, 82 insertions(+), 7 deletions(-)

diff --git a/include/Nazara/Utility/Node.hpp b/include/Nazara/Utility/Node.hpp
index 86ce6a51a..30f588b0c 100644
--- a/include/Nazara/Utility/Node.hpp
+++ b/include/Nazara/Utility/Node.hpp
@@ -41,7 +41,7 @@ class NAZARA_API NzNode
 
 		bool HasChilds() const;
 
-		NzNode& Interpolate(const NzNode& nodeA, const NzNode& nodeB, float interpolation);
+		NzNode& Interpolate(const NzNode& nodeA, const NzNode& nodeB, float interpolation, nzCoordSys coordSys = nzCoordSys_Global);
 
 		NzNode& Move(const NzVector3f& movement, nzCoordSys coordSys = nzCoordSys_Local);
 		NzNode& Move(float movementX, float movementY, float movementZ, nzCoordSys coordSys = nzCoordSys_Local);
@@ -71,6 +71,16 @@ class NAZARA_API NzNode
 		void SetScale(float scale, nzCoordSys coordSys = nzCoordSys_Local);
 		void SetScale(float scaleX, float scaleY, float scaleZ, nzCoordSys coordSys = nzCoordSys_Local);
 
+		// Local -> global
+		NzVector3f ToGlobalPosition(const NzVector3f& localPosition) const;
+		NzQuaternionf ToGlobalRotation(const NzQuaternionf& localRotation) const;
+		NzVector3f ToGlobalScale(const NzVector3f& localScale) const;
+
+		// Global -> local
+		NzVector3f ToLocalPosition(const NzVector3f& globalPosition) const;
+		NzQuaternionf ToLocalRotation(const NzQuaternionf& globalRotation) const;
+		NzVector3f ToLocalScale(const NzVector3f& globalScale) const;
+
 		NzNode& operator=(const NzNode& node);
 
 	protected:
diff --git a/src/Nazara/Utility/Node.cpp b/src/Nazara/Utility/Node.cpp
index 6cbf57f49..926bb3fb4 100644
--- a/src/Nazara/Utility/Node.cpp
+++ b/src/Nazara/Utility/Node.cpp
@@ -177,11 +177,28 @@ bool NzNode::HasChilds() const
 	return !m_childs.empty();
 }
 
-NzNode& NzNode::Interpolate(const NzNode& nodeA, const NzNode& nodeB, float interpolation)
+NzNode& NzNode::Interpolate(const NzNode& nodeA, const NzNode& nodeB, float interpolation, nzCoordSys coordSys)
 {
-	m_position = NzVector3f::Lerp(nodeA.m_position, nodeB.m_position, interpolation);
-	m_rotation = NzQuaternionf::Slerp(nodeA.m_rotation, nodeB.m_rotation, interpolation);
-	m_scale = NzVector3f::Lerp(nodeA.m_scale, nodeB.m_scale, interpolation);
+	switch (coordSys)
+	{
+		case nzCoordSys_Global:
+			if (!nodeA.m_derivedUpdated)
+				nodeA.UpdateDerived();
+
+			if (!nodeB.m_derivedUpdated)
+				nodeB.UpdateDerived();
+
+			m_position = ToLocalPosition(NzVector3f::Lerp(nodeA.m_derivedPosition, nodeB.m_derivedPosition, interpolation));
+			m_rotation = ToLocalRotation(NzQuaternionf::Slerp(nodeA.m_derivedRotation, nodeB.m_derivedRotation, interpolation));
+			m_scale = ToLocalScale(NzVector3f::Lerp(nodeA.m_derivedScale, nodeB.m_derivedScale, interpolation));
+			break;
+
+		case nzCoordSys_Local:
+			m_position = NzVector3f::Lerp(nodeA.m_position, nodeB.m_position, interpolation);
+			m_rotation = NzQuaternionf::Slerp(nodeA.m_rotation, nodeB.m_rotation, interpolation);
+			m_scale = NzVector3f::Lerp(nodeA.m_scale, nodeB.m_scale, interpolation);
+			break;
+	}
 
 	Invalidate();
 	return *this;
@@ -490,6 +507,54 @@ void NzNode::SetScale(float scaleX, float scaleY, float scaleZ, nzCoordSys coord
 	SetScale(NzVector3f(scaleX, scaleY, scaleZ), coordSys);
 }
 
+NzVector3f NzNode::ToGlobalPosition(const NzVector3f& localPosition) const
+{
+	if (!m_derivedUpdated)
+		UpdateDerived();
+
+	return m_derivedPosition + (m_derivedScale * (m_derivedRotation * localPosition));
+}
+
+NzQuaternionf NzNode::ToGlobalRotation(const NzQuaternionf& localRotation) const
+{
+	if (!m_derivedUpdated)
+		UpdateDerived();
+
+	return m_derivedRotation * localRotation;
+}
+
+NzVector3f NzNode::ToGlobalScale(const NzVector3f& localScale) const
+{
+	if (!m_derivedUpdated)
+		UpdateDerived();
+
+	return m_derivedScale * localScale;
+}
+
+NzVector3f NzNode::ToLocalPosition(const NzVector3f& globalPosition) const
+{
+	if (!m_derivedUpdated)
+		UpdateDerived();
+
+	return (m_derivedRotation.GetConjugate()*(globalPosition - m_derivedPosition))/m_derivedScale;
+}
+
+NzQuaternionf NzNode::ToLocalRotation(const NzQuaternionf& globalRotation) const
+{
+	if (!m_derivedUpdated)
+		UpdateDerived();
+
+	return m_derivedRotation.GetConjugate() * globalRotation;
+}
+
+NzVector3f NzNode::ToLocalScale(const NzVector3f& globalScale) const
+{
+	if (!m_derivedUpdated)
+		UpdateDerived();
+
+	return globalScale / m_derivedScale;
+}
+
 NzNode& NzNode::operator=(const NzNode& node)
 {
 	SetParent(node.m_parent);
diff --git a/src/Nazara/Utility/Skeleton.cpp b/src/Nazara/Utility/Skeleton.cpp
index 611a606ed..06379c822 100644
--- a/src/Nazara/Utility/Skeleton.cpp
+++ b/src/Nazara/Utility/Skeleton.cpp
@@ -268,7 +268,7 @@ void NzSkeleton::Interpolate(const NzSkeleton& skeletonA, const NzSkeleton& skel
 	NzJoint* jointsA = &skeletonA.m_impl->joints[0];
 	NzJoint* jointsB = &skeletonB.m_impl->joints[0];
 	for (unsigned int i = 0; i < m_impl->joints.size(); ++i)
-		m_impl->joints[i].Interpolate(jointsA[i], jointsB[i], interpolation);
+		m_impl->joints[i].Interpolate(jointsA[i], jointsB[i], interpolation, nzCoordSys_Local);
 
 	m_impl->aabbUpdated = false;
 }
@@ -315,7 +315,7 @@ void NzSkeleton::Interpolate(const NzSkeleton& skeletonA, const NzSkeleton& skel
 		}
 		#endif
 
-		m_impl->joints[index].Interpolate(jointsA[index], jointsB[index], interpolation);
+		m_impl->joints[index].Interpolate(jointsA[index], jointsB[index], interpolation, nzCoordSys_Local);
 	}
 
 	m_impl->aabbUpdated = false;