From b4eb3eb25d87a8479ee457e0eeb5ef75adc26eaf Mon Sep 17 00:00:00 2001
From: Dander7BD <dander.1981@gmail.com>
Date: Fri, 24 Jan 2014 17:39:09 +0100
Subject: [PATCH] Slerp

Quaternion Slerp
---
 Code/OysterMath/LinearMath.h | 25 +++++++++++++++++++++++++
 Code/OysterMath/OysterMath.h |  6 ++++++
 2 files changed, 31 insertions(+)

diff --git a/Code/OysterMath/LinearMath.h b/Code/OysterMath/LinearMath.h
index 51feacda..ae299ed9 100644
--- a/Code/OysterMath/LinearMath.h
+++ b/Code/OysterMath/LinearMath.h
@@ -204,6 +204,19 @@ namespace LinearAlgebra
 		}		
 		return output; // error: returning nullvector
 	}
+
+	/********************************************************************
+	 * Spherical Linear Interpolation on Quaternions
+	 ********************************************************************/
+	template<typename ScalarType>
+	inline Quaternion<ScalarType> Slerp( const Quaternion<ScalarType> &start, const Quaternion<ScalarType> &end,  const ScalarType &t )
+	{
+		ScalarType angle = (ScalarType)::std::acos( Vector4(start.imaginary, start.real).Dot(Vector4(end.imaginary, end.real)) );
+		Quaternion<ScalarType> result = start * (ScalarType)::std::sin( angle * (1 - t) );
+		result += end * (ScalarType)::std::sin( angle * t );
+		result *= (ScalarType)1.0f / (ScalarType)::std::sin( angle );
+		return result;
+	}
 }
 
 namespace LinearAlgebra2D
@@ -766,6 +779,18 @@ namespace LinearAlgebra3D
 		targetMem.v[3] = ::LinearAlgebra::Lerp( start.v[3], end.v[3], t );
 		return targetMem;
 	}
+
+	template<typename ScalarType>
+	::LinearAlgebra::Matrix4x4<ScalarType> & InterpolateOrientation_UsingNonRigidNlerp( const ::LinearAlgebra::Quaternion<ScalarType> &startR, const ::LinearAlgebra::Vector3<ScalarType> &startT, const ::LinearAlgebra::Quaternion<ScalarType> &endR, const ::LinearAlgebra::Vector3<ScalarType> &endT, ScalarType t, ::LinearAlgebra::Matrix4x4<ScalarType> &targetMem )
+	{
+		return InterpolateOrientation_UsingNonRigidNlerp( OrientationMatrix(startR, startT), OrientationMatrix(endR, endT), t, targetMem );
+	}
+
+	template<typename ScalarType>
+	::LinearAlgebra::Matrix4x4<ScalarType> & InterpolateOrientation_UsingSlerp( const ::LinearAlgebra::Quaternion<ScalarType> &startR, const ::LinearAlgebra::Vector3<ScalarType> &startT, const ::LinearAlgebra::Quaternion<ScalarType> &endR, const ::LinearAlgebra::Vector3<ScalarType> &endT, ScalarType t, ::LinearAlgebra::Matrix4x4<ScalarType> &targetMem )
+	{
+		return OrientationMatrix( ::LinearAlgebra::Slerp(startR, endR, t), ::LinearAlgebra::Lerp(::LinearAlgebra::Vector4<ScalarType>(startT, (ScalarType)1.0f), ::LinearAlgebra::Vector4<ScalarType>(endT, (ScalarType)1.0f), t).xyz, targetMem );
+	}	
 }
 
 #include "Utilities.h"
diff --git a/Code/OysterMath/OysterMath.h b/Code/OysterMath/OysterMath.h
index 225705ca..559ba0d3 100644
--- a/Code/OysterMath/OysterMath.h
+++ b/Code/OysterMath/OysterMath.h
@@ -57,6 +57,11 @@ namespace Oyster { namespace Math //! Oyster's native math library
 	 * @return nullvector if Lerp( start, end, t ) is nullvector.
 	 ********************************************************************/
 	using ::LinearAlgebra::Nlerp;
+
+	/********************************************************************
+	 * Spherical Linear Interpolation on Quaternions
+	 ********************************************************************/
+	using ::LinearAlgebra::Slerp;
 } }
 
 inline ::Oyster::Math::Float2 & operator *= ( ::Oyster::Math::Float2 &left, const ::Oyster::Math::Float2 &right )
@@ -344,6 +349,7 @@ namespace Oyster { namespace Math3D //! Oyster's native math library specialized
 	using ::LinearAlgebra3D::SnapAxisYToNormal_UsingNlerp;
 	using ::LinearAlgebra3D::InterpolateAxisYToNormal_UsingNlerp;
 	using ::LinearAlgebra3D::InterpolateOrientation_UsingNonRigidNlerp;
+	using ::LinearAlgebra3D::InterpolateOrientation_UsingSlerp;
 } }
 
 #endif
\ No newline at end of file