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 - ::LinearAlgebra::Vector4 & SnapAngularAxis( ::LinearAlgebra::Vector4 &startAngularAxis, const ::LinearAlgebra::Vector4 &localStartNormal, const ::LinearAlgebra::Vector4 &worldEndNormal, ::LinearAlgebra::Vector4 &targetMem = ::LinearAlgebra::Vector4() ) + ::LinearAlgebra::Vector4 & SnapAngularAxis( const ::LinearAlgebra::Vector4 &startAngularAxis, const ::LinearAlgebra::Vector4 &localStartNormal, const ::LinearAlgebra::Vector4 &worldEndNormal, ::LinearAlgebra::Vector4 &targetMem = ::LinearAlgebra::Vector4() ) { ::LinearAlgebra::Vector4 worldStartNormal( WorldAxisOf(Rotation(startAngularAxis.xyz), localStartNormal.xyz), (ScalarType)0 ); targetMem = ::LinearAlgebra::Vector4( worldStartNormal.xyz.Cross(worldEndNormal.xyz), (ScalarType)0); @@ -829,11 +829,12 @@ namespace LinearAlgebra3D } template - ::LinearAlgebra::Vector3 & SnapAngularAxis( ::LinearAlgebra::Vector3 &startAngularAxis, const ::LinearAlgebra::Vector3 &localStartNormal, const ::LinearAlgebra::Vector3 &worldEndNormal, ::LinearAlgebra::Vector3 &targetMem = ::LinearAlgebra::Vector3() ) + ::LinearAlgebra::Vector3 & SnapAngularAxis( const ::LinearAlgebra::Vector3 &startAngularAxis, const ::LinearAlgebra::Vector3 &localStartNormal, const ::LinearAlgebra::Vector3 &worldEndNormal, ::LinearAlgebra::Vector3 &targetMem = ::LinearAlgebra::Vector3() ) { - return targetMem = SnapAngularAxis( ::LinearAlgebra::Vector4(startAngularAxis, (ScalarType)0), - ::LinearAlgebra::Vector4(localStartNormal, (ScalarType)0), - ::LinearAlgebra::Vector4(worldEndNormal, (ScalarType)0) ).xyz; + ::LinearAlgebra::Vector4 worldStartNormal( WorldAxisOf(Rotation(startAngularAxis), localStartNormal), (ScalarType)0 ); + targetMem = ::LinearAlgebra::Vector4( worldStartNormal.xyz.Cross(worldEndNormal), (ScalarType)0); + targetMem *= (ScalarType)::std::acos( ::Utility::Value::Clamp(worldStartNormal.Dot(worldEndNormal), (ScalarType)0, (ScalarType)1) ); + return targetMem += startAngularAxis; } template