From 982bb5ce5d3515f51f72c86aaaca8e0778537ce7 Mon Sep 17 00:00:00 2001
From: Christophe Riccio <mail@g-truc.net>
Date: Fri, 22 Feb 2013 01:08:33 +0100
Subject: [PATCH] Added rotation function, issue #22

---
 glm/gtx/norm.hpp            |  6 -----
 glm/gtx/norm.inl            |  9 -------
 glm/gtx/quaternion.hpp      | 18 ++++++++++++++
 glm/gtx/quaternion.inl      | 48 +++++++++++++++++++++++++++++++++++++
 readme.txt                  |  1 +
 test/gtx/gtx_quaternion.cpp | 19 ++++++++++++++-
 6 files changed, 85 insertions(+), 16 deletions(-)

diff --git a/glm/gtx/norm.hpp b/glm/gtx/norm.hpp
index 2b065f4c..bbf4088b 100644
--- a/glm/gtx/norm.hpp
+++ b/glm/gtx/norm.hpp
@@ -64,12 +64,6 @@ namespace glm
 	typename genType::value_type length2(
 		genType const & x);
 		
-	//! Returns the squared length of x.
-	//! From GLM_GTX_norm extension.
-	template <typename T>
-	T length2(
-		detail::tquat<T> const & q);
-
 	//! Returns the squared distance between p0 and p1, i.e., length(p0 - p1).
 	//! From GLM_GTX_norm extension.
 	template <typename T>
diff --git a/glm/gtx/norm.inl b/glm/gtx/norm.inl
index 427a5a16..480cb501 100644
--- a/glm/gtx/norm.inl
+++ b/glm/gtx/norm.inl
@@ -45,15 +45,6 @@ namespace glm
 		return dot(x, x);
 	}
 
-	template <typename T> 
-	GLM_FUNC_QUALIFIER T length2
-	(
-		detail::tquat<T> const & q
-	)
-	{
-		return q.x * q.x + q.y * q.y + q.z * q.z + q.w * q.w;
-	}
-
 	template <typename T> 
 	GLM_FUNC_QUALIFIER T distance2
 	(
diff --git a/glm/gtx/quaternion.hpp b/glm/gtx/quaternion.hpp
index 3c9e8b04..ac536887 100644
--- a/glm/gtx/quaternion.hpp
+++ b/glm/gtx/quaternion.hpp
@@ -41,7 +41,9 @@
 
 // Dependency:
 #include "../glm.hpp"
+#include "../gtc/constants.hpp"
 #include "../gtc/quaternion.hpp"
+#include "../gtx/norm.hpp"
 
 #if(defined(GLM_MESSAGES) && !defined(glm_ext))
 #	pragma message("GLM: GLM_GTX_quaternion extension included")
@@ -187,6 +189,22 @@ namespace glm
 		detail::tquat<T> const & y, 
 		T const & a);
 
+	/// Compute the rotation between two vectors. 
+	/// param orig vector, needs to be normalized
+	/// param dest vector, needs to be normalized
+	///
+	/// @see gtx_quaternion
+	template <typename T>
+	detail::tquat<T> rotation(
+		detail::tvec3<T> const & orig, 
+		detail::tvec3<T> const & dest);
+
+	/// Returns the squared length of x.
+	/// 
+	/// @see gtx_quaternion
+	template <typename T>
+	T length2(detail::tquat<T> const & q);
+
 	/// @}
 }//namespace glm
 
diff --git a/glm/gtx/quaternion.inl b/glm/gtx/quaternion.inl
index 32155ce5..401ff308 100644
--- a/glm/gtx/quaternion.inl
+++ b/glm/gtx/quaternion.inl
@@ -153,6 +153,15 @@ namespace glm
 			return -sqrt(w);
 	}
 
+	template <typename T> 
+	GLM_FUNC_QUALIFIER T length2
+	(
+		detail::tquat<T> const & q
+	)
+	{
+		return q.x * q.x + q.y * q.y + q.z * q.z + q.w * q.w;
+	}
+
 	template <typename T>
 	GLM_FUNC_QUALIFIER detail::tquat<T> shortMix
 	(
@@ -205,4 +214,43 @@ namespace glm
 	{
 		return glm::normalize(x * (T(1) - a) + (y * a));
 	}
+
+	template <typename T>
+	GLM_FUNC_QUALIFIER detail::tquat<T> rotation
+	(
+		detail::tvec3<T> const & orig, 
+		detail::tvec3<T> const & dest
+	)
+	{
+		T cosTheta = dot(orig, dest);
+		detail::tvec3<T> rotationAxis;
+
+		if(cosTheta < T(-1) + epsilon<T>())
+		{
+			// special case when vectors in opposite directions :
+			// there is no "ideal" rotation axis
+			// So guess one; any will do as long as it's perpendicular to start
+			// This implementation favors a rotation around the Up axis (Y),
+			// since it's often what you want to do.
+			rotationAxis = cross(detail::tvec3<T>(0, 0, 1), orig);
+			if(length2(rotationAxis) < epsilon<T>()) // bad luck, they were parallel, try again!
+				rotationAxis = cross(detail::tvec3<T>(1, 0, 0), orig);
+
+			rotationAxis = normalize(rotationAxis);
+			return angleAxis(pi<T>(), rotationAxis);
+		}
+
+		// Implementation from Stan Melax's Game Programming Gems 1 article
+		rotationAxis = cross(orig, dest);
+
+		T s = sqrt((T(1) + cosTheta) * T(2));
+		T invs = T(1) / s;
+
+		return detail::tquat<T>(
+			s * T(0.5f), 
+			rotationAxis.x * invs,
+			rotationAxis.y * invs,
+			rotationAxis.z * invs);
+	}
+
 }//namespace glm
diff --git a/readme.txt b/readme.txt
index b7a38cc8..5da96053 100644
--- a/readme.txt
+++ b/readme.txt
@@ -43,6 +43,7 @@ GLM 0.9.5.0: 2013-XX-XX
 - Added bitfieldInterleave and _mm_bit_interleave_si128 functions
 - Added GTX_scalar_relational
 - Added GTX_dual_quaternion
+- Added rotation function to GTX_quaternion (#22)
 
 ================================================================================
 GLM 0.9.4.3: 2013-0X-XX
diff --git a/test/gtx/gtx_quaternion.cpp b/test/gtx/gtx_quaternion.cpp
index 16e3be08..7fe3fb2d 100644
--- a/test/gtx/gtx_quaternion.cpp
+++ b/test/gtx/gtx_quaternion.cpp
@@ -67,10 +67,27 @@ int test_orientation()
 	return Error;
 }
 
+int test_rotation()
+{
+	int Error(0);
+
+	glm::vec3 v(1, 0, 0);
+	glm::vec3 u(0, 1, 0);
+
+	glm::quat Rotation = glm::rotation(v, u);
+
+	float Angle = glm::angle(Rotation);
+
+	Error += glm::abs(Angle - 90.0f) < glm::epsilon<float>() ? 0 : 1;
+
+	return Error;
+}
+
 int main()
 {
-	int Error = 0;
+	int Error(0);
 
+	Error += test_rotation();
 	Error += test_quat_fastMix();
 	Error += test_quat_shortMix();