From 30196aebc2d2d5170c089fc181d31ef5f2f1d1ec Mon Sep 17 00:00:00 2001
From: Dander7BD <dander.1981@gmail.com>
Date: Mon, 10 Feb 2014 14:30:15 +0100
Subject: [PATCH] Float3 SnapAngularAxis fix

---
 Code/OysterMath/LinearMath.h | 11 ++++++-----
 1 file changed, 6 insertions(+), 5 deletions(-)

diff --git a/Code/OysterMath/LinearMath.h b/Code/OysterMath/LinearMath.h
index 0d02616e..107ed470 100644
--- a/Code/OysterMath/LinearMath.h
+++ b/Code/OysterMath/LinearMath.h
@@ -820,7 +820,7 @@ namespace LinearAlgebra3D
 	{ return normalizedAxis * ( vector.Dot(normalizedAxis) ); }
 
 	template<typename ScalarType>
-	::LinearAlgebra::Vector4<ScalarType> & SnapAngularAxis( ::LinearAlgebra::Vector4<ScalarType> &startAngularAxis, const ::LinearAlgebra::Vector4<ScalarType> &localStartNormal, const ::LinearAlgebra::Vector4<ScalarType> &worldEndNormal, ::LinearAlgebra::Vector4<ScalarType> &targetMem = ::LinearAlgebra::Vector4<ScalarType>() )
+	::LinearAlgebra::Vector4<ScalarType> & SnapAngularAxis( const ::LinearAlgebra::Vector4<ScalarType> &startAngularAxis, const ::LinearAlgebra::Vector4<ScalarType> &localStartNormal, const ::LinearAlgebra::Vector4<ScalarType> &worldEndNormal, ::LinearAlgebra::Vector4<ScalarType> &targetMem = ::LinearAlgebra::Vector4<ScalarType>() )
 	{
 		::LinearAlgebra::Vector4<ScalarType> worldStartNormal( WorldAxisOf(Rotation(startAngularAxis.xyz), localStartNormal.xyz), (ScalarType)0 );
 		targetMem = ::LinearAlgebra::Vector4<ScalarType>( worldStartNormal.xyz.Cross(worldEndNormal.xyz), (ScalarType)0);
@@ -829,11 +829,12 @@ namespace LinearAlgebra3D
 	}
 
 	template<typename ScalarType>
-	::LinearAlgebra::Vector3<ScalarType> & SnapAngularAxis( ::LinearAlgebra::Vector3<ScalarType> &startAngularAxis, const ::LinearAlgebra::Vector3<ScalarType> &localStartNormal, const ::LinearAlgebra::Vector3<ScalarType> &worldEndNormal, ::LinearAlgebra::Vector3<ScalarType> &targetMem = ::LinearAlgebra::Vector3<ScalarType>() )
+	::LinearAlgebra::Vector3<ScalarType> & SnapAngularAxis( const ::LinearAlgebra::Vector3<ScalarType> &startAngularAxis, const ::LinearAlgebra::Vector3<ScalarType> &localStartNormal, const ::LinearAlgebra::Vector3<ScalarType> &worldEndNormal, ::LinearAlgebra::Vector3<ScalarType> &targetMem = ::LinearAlgebra::Vector3<ScalarType>() )
 	{
-		return targetMem = SnapAngularAxis( ::LinearAlgebra::Vector4<ScalarType>(startAngularAxis, (ScalarType)0),
-											::LinearAlgebra::Vector4<ScalarType>(localStartNormal, (ScalarType)0),
-											::LinearAlgebra::Vector4<ScalarType>(worldEndNormal, (ScalarType)0) ).xyz;
+		::LinearAlgebra::Vector4<ScalarType> worldStartNormal( WorldAxisOf(Rotation(startAngularAxis), localStartNormal), (ScalarType)0 );
+		targetMem = ::LinearAlgebra::Vector4<ScalarType>( worldStartNormal.xyz.Cross(worldEndNormal), (ScalarType)0);
+		targetMem *= (ScalarType)::std::acos( ::Utility::Value::Clamp(worldStartNormal.Dot(worldEndNormal), (ScalarType)0, (ScalarType)1) );
+		return targetMem += startAngularAxis;
 	}
 
 	template<typename ScalarType>