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 - ::LinearAlgebra::Matrix4x4 & SnapAxisYToAxis_Nlerp( ::LinearAlgebra::Matrix4x4 &rotation, const ::LinearAlgebra::Vector4 &normalizedAxis ) + ::LinearAlgebra::Matrix4x4 & SnapAxisYToNormal_UsingNlerp( ::LinearAlgebra::Matrix4x4 &rotation, const ::LinearAlgebra::Vector4 &normalizedAxis ) { - + ScalarType projectedMagnitude = rotation.v[0].Dot( normalizedAxis ); + if( projectedMagnitude == 1 ) + { // infinite possible solutions -> roadtrip! + ::LinearAlgebra::Vector4 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 - ::LinearAlgebra::Matrix4x4 & InterpolateAxisYToAxis_Nlerp( ::LinearAlgebra::Matrix4x4 &rotation, const ::LinearAlgebra::Vector4 &normalizedAxis, ScalarType t ) + ::LinearAlgebra::Matrix4x4 & InterpolateAxisYToNormal_UsingNlerp( ::LinearAlgebra::Matrix4x4 &rotation, const ::LinearAlgebra::Vector4 &normalizedAxis, ScalarType t ) { ::LinearAlgebra::Vector4 interpolated = ::LinearAlgebra::Nlerp( rotation.v[1], normalizedAxis, t ); if( interpolated.Dot(interpolated) == 0 )