From 408d51e46bc0cab1f357b67cf456b4d8ed58c3c0 Mon Sep 17 00:00:00 2001
From: Dander7BD <dander.1981@gmail.com>
Date: Tue, 21 Jan 2014 11:15:33 +0100
Subject: [PATCH] SnapAxisYToNormal_UsingNlerp implemented

---
 Code/OysterMath/LinearMath.h | 20 +++++++++++++++++---
 1 file changed, 17 insertions(+), 3 deletions(-)

diff --git a/Code/OysterMath/LinearMath.h b/Code/OysterMath/LinearMath.h
index 0fac61e6..e7e99c91 100644
--- a/Code/OysterMath/LinearMath.h
+++ b/Code/OysterMath/LinearMath.h
@@ -728,13 +728,27 @@ namespace LinearAlgebra3D
 	{ return normalizedAxis * ( vector.Dot(normalizedAxis) ); }
 
 	template<typename ScalarType>
-	::LinearAlgebra::Matrix4x4<ScalarType> & SnapAxisYToAxis_Nlerp( ::LinearAlgebra::Matrix4x4<ScalarType> &rotation, const ::LinearAlgebra::Vector4<ScalarType> &normalizedAxis )
+	::LinearAlgebra::Matrix4x4<ScalarType> & SnapAxisYToNormal_UsingNlerp( ::LinearAlgebra::Matrix4x4<ScalarType> &rotation, const ::LinearAlgebra::Vector4<ScalarType> &normalizedAxis )
 	{
-
+		ScalarType projectedMagnitude = rotation.v[0].Dot( normalizedAxis );
+		if( projectedMagnitude == 1 )
+		{ // infinite possible solutions -> roadtrip!
+			::LinearAlgebra::Vector4<ScalarType> interpolated = ::LinearAlgebra::Nlerp( rotation.v[1], normalizedAxis, t );
+			
+			// interpolated.Dot( interpolated ) == 0 should be impossible at this point
+			projectedMagnitude = rotation.v[0].Dot( interpolated );
+			rotation.v[0] -= projectedMagnitude * interpolated;
+			rotation.v[0].Normalize();
+			projectedMagnitude = rotation.v[0].Dot( normalizedAxis );
+		}
+		rotation.v[0] -= projectedMagnitude * normalizedAxis;
+		rotation.v[0].Normalize();
+		rotation.v[1] = normalizedAxis;
+		rotation.v[2] = rotation.v[0].Cross( rotation.v[1] );
 	}
 
 	template<typename ScalarType>
-	::LinearAlgebra::Matrix4x4<ScalarType> & InterpolateAxisYToAxis_Nlerp( ::LinearAlgebra::Matrix4x4<ScalarType> &rotation, const ::LinearAlgebra::Vector4<ScalarType> &normalizedAxis, ScalarType t )
+	::LinearAlgebra::Matrix4x4<ScalarType> & InterpolateAxisYToNormal_UsingNlerp( ::LinearAlgebra::Matrix4x4<ScalarType> &rotation, const ::LinearAlgebra::Vector4<ScalarType> &normalizedAxis, ScalarType t )
 	{
 		::LinearAlgebra::Vector4<ScalarType> interpolated = ::LinearAlgebra::Nlerp( rotation.v[1], normalizedAxis, t );
 		if( interpolated.Dot(interpolated) == 0 )