///////////////////////////////////////////////////////////////////// // Collection of Linear Math Stuff // © Dan Andersson 2013 ///////////////////////////////////////////////////////////////////// #ifndef LINEARMATH_H #define LINEARMATH_H #include "Vector.h" #include "Matrix.h" #include "Quaternion.h" #include namespace std { template inline ::LinearAlgebra::Vector2 asin( const ::LinearAlgebra::Vector2 &vec ) { return ::LinearAlgebra::Vector2( asin(vec.x), asin(vec.y) ); } template inline ::LinearAlgebra::Vector3 asin( const ::LinearAlgebra::Vector3 &vec ) { return ::LinearAlgebra::Vector3( asin(vec.x), asin(vec.y), asin(vec.z) ); } template inline ::LinearAlgebra::Vector4 asin( const ::LinearAlgebra::Vector4 &vec ) { return ::LinearAlgebra::Vector4( asin(vec.x), asin(vec.y), asin(vec.z), asin(vec.w) ); } } // x2 template ::LinearAlgebra::Matrix2x2 operator * ( const ::LinearAlgebra::Matrix2x2 &left, const ::LinearAlgebra::Matrix2x2 &right ) { return ::LinearAlgebra::Matrix2x2( (left.m11 * right.m11) + (left.m12 * right.m21), (left.m11 * right.m12) + (left.m12 * right.m22), (left.m21 * right.m11) + (left.m22 * right.m21), (left.m21 * right.m12) + (left.m22 * right.m22) ); } template ::LinearAlgebra::Vector2 operator * ( const ::LinearAlgebra::Matrix2x2 &matrix, const ::LinearAlgebra::Vector2 &vector ) { return ::LinearAlgebra::Vector2( (matrix.m11 * vector.x) + (matrix.m12 * vector.y), (matrix.m21 * vector.x) + (matrix.m22 * vector.y) ); } template ::LinearAlgebra::Vector2 operator * ( const ::LinearAlgebra::Vector2 &vector, const ::LinearAlgebra::Matrix2x2 &left ) { return ::LinearAlgebra::Vector2( (vector.x * matrix.m11) + (vector.y * matrix.m21), (vector.x * matrix.m12) + (vector.y * matrix.m22) ); } // x3 template ::LinearAlgebra::Matrix3x3 operator * ( const ::LinearAlgebra::Matrix3x3 &left, const ::LinearAlgebra::Matrix3x3 &right ) { return ::LinearAlgebra::Matrix3x3( (left.m11 * right.m11) + (left.m12 * right.m21) + (left.m13 * right.m31), (left.m11 * right.m12) + (left.m12 * right.m22) + (left.m13 * right.m32), (left.m11 * right.m13) + (left.m12 * right.m23) + (left.m13 * right.m33), (left.m21 * right.m11) + (left.m22 * right.m21) + (left.m23 * right.m31), (left.m21 * right.m12) + (left.m22 * right.m22) + (left.m23 * right.m32), (left.m21 * right.m13) + (left.m22 * right.m23) + (left.m23 * right.m33), (left.m31 * right.m11) + (left.m32 * right.m21) + (left.m33 * right.m31), (left.m31 * right.m12) + (left.m32 * right.m22) + (left.m33 * right.m32), (left.m31 * right.m13) + (left.m32 * right.m23) + (left.m33 * right.m33) ); } template ::LinearAlgebra::Vector3 operator * ( const ::LinearAlgebra::Matrix3x3 &matrix, const ::LinearAlgebra::Vector3 &vector ) { return ::LinearAlgebra::Vector3( (matrix.m11 * vector.x) + (matrix.m12 * vector.y) + (matrix.m13 * vector.z), (matrix.m21 * vector.x) + (matrix.m22 * vector.y) + (matrix.m23 * vector.z), (matrix.m31 * vector.x) + (matrix.m32 * vector.y) + (matrix.m33 * vector.z) ); } template ::LinearAlgebra::Vector3 operator * ( const ::LinearAlgebra::Vector3 &vector, const ::LinearAlgebra::Matrix3x3 &left ) { return ::LinearAlgebra::Vector3( (vector.x * matrix.m11) + (vector.y * matrix.m21) + (vector.z * matrix.m31), (vector.x * matrix.m12) + (vector.y * matrix.m22) + (vector.z * matrix.m32), (vector.x * matrix.m13) + (vector.y * matrix.m23) + (vector.z * matrix.m33) ); } // x4 template ::LinearAlgebra::Matrix4x4 operator * ( const ::LinearAlgebra::Matrix4x4 &left, const ::LinearAlgebra::Matrix4x4 &right ) { return ::LinearAlgebra::Matrix4x4( (left.m11 * right.m11) + (left.m12 * right.m21) + (left.m13 * right.m31) + (left.m14 * right.m41), (left.m11 * right.m12) + (left.m12 * right.m22) + (left.m13 * right.m32) + (left.m14 * right.m42), (left.m11 * right.m13) + (left.m12 * right.m23) + (left.m13 * right.m33) + (left.m14 * right.m43), (left.m11 * right.m14) + (left.m12 * right.m24) + (left.m13 * right.m34) + (left.m14 * right.m44), (left.m21 * right.m11) + (left.m22 * right.m21) + (left.m23 * right.m31) + (left.m24 * right.m41), (left.m21 * right.m12) + (left.m22 * right.m22) + (left.m23 * right.m32) + (left.m24 * right.m42), (left.m21 * right.m13) + (left.m22 * right.m23) + (left.m23 * right.m33) + (left.m24 * right.m43), (left.m21 * right.m14) + (left.m22 * right.m24) + (left.m23 * right.m34) + (left.m24 * right.m44), (left.m31 * right.m11) + (left.m32 * right.m21) + (left.m33 * right.m31) + (left.m34 * right.m41), (left.m31 * right.m12) + (left.m32 * right.m22) + (left.m33 * right.m32) + (left.m34 * right.m42), (left.m31 * right.m13) + (left.m32 * right.m23) + (left.m33 * right.m33) + (left.m34 * right.m43), (left.m31 * right.m14) + (left.m32 * right.m24) + (left.m33 * right.m34) + (left.m34 * right.m44), (left.m41 * right.m11) + (left.m42 * right.m21) + (left.m43 * right.m31) + (left.m44 * right.m41), (left.m41 * right.m12) + (left.m42 * right.m22) + (left.m43 * right.m32) + (left.m44 * right.m42), (left.m41 * right.m13) + (left.m42 * right.m23) + (left.m43 * right.m33) + (left.m44 * right.m43), (left.m41 * right.m14) + (left.m42 * right.m24) + (left.m43 * right.m34) + (left.m44 * right.m44) ); } template ::LinearAlgebra::Vector4 operator * ( const ::LinearAlgebra::Matrix4x4 &matrix, const ::LinearAlgebra::Vector4 &vector ) { return ::LinearAlgebra::Vector4( (matrix.m11 * vector.x) + (matrix.m12 * vector.y) + (matrix.m13 * vector.z) + (matrix.m14 * vector.w), (matrix.m21 * vector.x) + (matrix.m22 * vector.y) + (matrix.m23 * vector.z) + (matrix.m24 * vector.w), (matrix.m23 * vector.x) + (matrix.m32 * vector.y) + (matrix.m33 * vector.z) + (matrix.m34 * vector.w), (matrix.m41 * vector.x) + (matrix.m42 * vector.y) + (matrix.m43 * vector.z) + (matrix.m44 * vector.w) ); } template ::LinearAlgebra::Vector4 operator * ( const ::LinearAlgebra::Vector4 &vector, const ::LinearAlgebra::Matrix4x4 &matrix ) { return ::LinearAlgebra::Vector4( (vector.x * matrix.m11) + (vector.y * matrix.m21) + (vector.z * matrix.m31) + (vector.w * matrix.m41), (vector.x * matrix.m12) + (vector.y * matrix.m22) + (vector.z * matrix.m32) + (vector.w * matrix.m42), (vector.x * matrix.m13) + (vector.y * matrix.m23) + (vector.z * matrix.m33) + (vector.w * matrix.m43), (vector.x * matrix.m14) + (vector.y * matrix.m24) + (vector.z * matrix.m34) + (vector.w * matrix.m44) ); } namespace LinearAlgebra { // Creates a solution matrix for 'out´= 'targetMem' * 'in'. // Returns false if there is no explicit solution. template bool SuperpositionMatrix( const Matrix2x2 &in, const Matrix2x2 &out, Matrix2x2 &targetMem ) { ScalarType d = in.GetDeterminant(); if( d == 0 ) return false; targetMem = out * (in.GetAdjoint() /= d); return true; } // Creates a solution matrix for 'out´= 'targetMem' * 'in'. // Returns false if there is no explicit solution. template bool SuperpositionMatrix( const Matrix3x3 &in, const Matrix3x3 &out, Matrix3x3 &targetMem ) { ScalarType d = in.GetDeterminant(); if( d == 0 ) return false; targetMem = out * (in.GetAdjoint() /= d); return true; } // Creates a solution matrix for 'out´= 'targetMem' * 'in'. // Returns false if there is no explicit solution. template bool SuperpositionMatrix( const Matrix4x4 &in, const Matrix4x4 &out, Matrix4x4 &targetMem ) { ScalarType d = in.GetDeterminant(); if( d == 0 ) return false; targetMem = out * (in.GetAdjoint() /= d); return true; } /******************************************************************** * Linear Interpolation * @return start * (1-t) + end * t ********************************************************************/ template inline PointType Lerp( const PointType &start, const PointType &end, const ScalarType &t ) { return end * t + start * ( 1 - t ); } /******************************************************************** * Normalized Linear Interpolation * @return nullvector if Lerp( start, end, t ) is nullvector. ********************************************************************/ template inline Vector2 Nlerp( const Vector2 &start, const Vector2 &end, const ScalarType &t ) { Vector2 output = Lerp( start, end, t ); ScalarType magnitudeSquared = output.Dot( output ); if( magnitudeSquared != 0 ) { return output /= (ScalarType)::std::sqrt( magnitudeSquared ); } return output; // error: returning nullvector } /******************************************************************** * Normalized Linear Interpolation * @return nullvector if Lerp( start, end, t ) is nullvector. ********************************************************************/ template inline Vector3 Nlerp( const Vector3 &start, const Vector3 &end, const ScalarType &t ) { Vector3 output = Lerp( start, end, t ); ScalarType magnitudeSquared = output.Dot( output ); if( magnitudeSquared != 0 ) { return output /= (ScalarType)::std::sqrt( magnitudeSquared ); } return output; // error: returning nullvector } /******************************************************************** * Normalized Linear Interpolation * @return nullvector if Lerp( start, end, t ) is nullvector. ********************************************************************/ template inline Vector4 Nlerp( const Vector4 &start, const Vector4 &end, const ScalarType &t ) { Vector4 output = Lerp( start, end, t ); ScalarType magnitudeSquared = output.Dot( output ); if( magnitudeSquared != 0 ) { return output /= (ScalarType)::std::sqrt( magnitudeSquared ); } return output; // error: returning nullvector } /******************************************************************** * Spherical Linear Interpolation on Quaternions ********************************************************************/ template inline Quaternion Slerp( const Quaternion &start, const Quaternion &end, const ScalarType &t ) { ScalarType angle = (ScalarType)::std::acos( Vector4(start.imaginary, start.real).Dot(Vector4(end.imaginary, end.real)) ); Quaternion 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 { template inline ::LinearAlgebra::Vector2 X_AxisTo( const ::LinearAlgebra::Vector2 &yAxis ) { return ::LinearAlgebra::Vector2( yAxis.y, -yAxis.x ); } template inline ::LinearAlgebra::Vector2 Y_AxisTo( const ::LinearAlgebra::Vector2 &xAxis ) { return ::LinearAlgebra::Vector2( -xAxis.y, xAxis.x ); } template inline ::LinearAlgebra::Matrix3x3 & TranslationMatrix( const ::LinearAlgebra::Vector2 &position, ::LinearAlgebra::Matrix3x3 &targetMem = ::LinearAlgebra::Matrix3x3() ) { return targetMem = ::LinearAlgebra::Matrix3x3( 1, 0, position.x, 0, 1, position.y, 0, 0, 1 ); } template inline ::LinearAlgebra::Matrix2x2 & RotationMatrix( const ScalarType &radian, ::LinearAlgebra::Matrix2x2 &targetMem = ::LinearAlgebra::Matrix2x2() ) { ScalarType s = ::std::sin( radian ), c = ::std::cos( radian ); return targetMem = ::LinearAlgebra::Matrix2x2( c, -s, s, c ); } template inline ::LinearAlgebra::Matrix3x3 & RotationMatrix( const ScalarType &radian, ::LinearAlgebra::Matrix3x3 &targetMem = ::LinearAlgebra::Matrix3x3() ) { ScalarType s = ::std::sin( radian ), c = ::std::cos( radian ); return targetMem = ::LinearAlgebra::Matrix3x3( c,-s, 0, s, c, 0, 0, 0, 1 ); } template inline ::LinearAlgebra::Matrix2x2 & InverseRotationMatrix( const ::LinearAlgebra::Matrix2x2 &rotationMatrix ) { return rotationMatrix.GetTranspose(); } template inline ::LinearAlgebra::Matrix3x3 & InverseRotationMatrix( const ::LinearAlgebra::Matrix3x3 &rotationMatrix ) { return rotationMatrix.GetTranspose(); } template inline ::LinearAlgebra::Matrix3x3 OrientationMatrix( const ::LinearAlgebra::Matrix2x2 &rotation, const ::LinearAlgebra::Vector2 &translation ) { return ::LinearAlgebra::Matrix3x3( ::LinearAlgebra::Vector3(rotation.v[0], 0), ::LinearAlgebra::Vector3(rotation.v[1], 0), ::LinearAlgebra::Vector3(translation, 1) ); } template inline ::LinearAlgebra::Matrix3x3 OrientationMatrix( const ::LinearAlgebra::Matrix3x3 &rotation, const ::LinearAlgebra::Vector2 &translation ) { return ::LinearAlgebra::Matrix3x3( rotation.v[0], rotation.v[1], ::LinearAlgebra::Vector3(translation, 1) ); } template inline ::LinearAlgebra::Matrix3x3 & OrientationMatrix( const ScalarType &radian, const ::LinearAlgebra::Vector2 &position, ::LinearAlgebra::Matrix3x3 &targetMem = ::LinearAlgebra::Matrix3x3() ) { ScalarType s = ::std::sin( radian ), c = ::std::cos( radian ); return targetMem = ::LinearAlgebra::Matrix3x3( c,-s, position.x, s, c, position.y, 0, 0, 1 ); } template inline ::LinearAlgebra::Matrix3x3 & OrientationMatrix( const ::LinearAlgebra::Vector2 &lookAt, const ::LinearAlgebra::Vector2 &position, ::LinearAlgebra::Matrix3x3 &targetMem = ::LinearAlgebra::Matrix3x3() ) { ::LinearAlgebra::Vector2 direction = ( lookAt - position ).GetNormalized(); return targetMem = ::LinearAlgebra::Matrix3x3( ::LinearAlgebra::Vector3( X_AxisTo(direction), 0.0f ), ::LinearAlgebra::Vector3( direction, 0.0f ), ::LinearAlgebra::Vector3( position, 1.0f ) ); } template inline ::LinearAlgebra::Matrix3x3 & OrientationMatrix( const ScalarType &radian, const ::LinearAlgebra::Vector2 &position, const ::LinearAlgebra::Vector2 ¢erOfRotation, ::LinearAlgebra::Matrix3x3 &targetMem = ::LinearAlgebra::Matrix3x3() ) { // TODO: not tested RotationMatrix( radian, targetMem ); targetMem.v[2].xy = position + centerOfRotation; targetMem.v[2].x -= ::LinearAlgebra::Vector2( targetMem.v[0].x, targetMem.v[1].x ).Dot( centerOfRotation ); targetMem.v[2].y -= ::LinearAlgebra::Vector2( targetMem.v[0].y, targetMem.v[1].y ).Dot( centerOfRotation ); return targetMem; } template inline ::LinearAlgebra::Matrix3x3 & InverseOrientationMatrix( const ::LinearAlgebra::Matrix3x3 &orientationMatrix, ::LinearAlgebra::Matrix3x3 &targetMem = ::LinearAlgebra::Matrix3x3() ) { return targetMem = ::LinearAlgebra::Matrix3x3( orientationMatrix.m11, orientationMatrix.m21, -orientationMatrix.v[0].xy.Dot(orientationMatrix.v[2].xy), orientationMatrix.m12, orientationMatrix.m22, -orientationMatrix.v[1].xy.Dot(orientationMatrix.v[2].xy), 0, 0, 1 ); } template inline ::LinearAlgebra::Matrix3x3 ExtractRotationMatrix( const ::LinearAlgebra::Matrix3x3 &orientationMatrix ) { return ::LinearAlgebra::Matrix3x3( orientationMatrix.v[0], orientationMatrix.v[1], ::LinearAlgebra::Vector3::standard_unit_z ); } } namespace LinearAlgebra3D { template inline ::LinearAlgebra::Vector4 AngularAxis( const ::LinearAlgebra::Matrix3x3 &rotationMatrix ) { return ::std::asin( ::LinearAlgebra::Vector4(rotationMatrix.v[1].z, rotationMatrix.v[2].x, rotationMatrix.v[0].y, 1) ); } template inline ::LinearAlgebra::Vector4 AngularAxis( const ::LinearAlgebra::Matrix4x4 &rotationMatrix ) { return ::std::asin( ::LinearAlgebra::Vector4(rotationMatrix.v[1].z, rotationMatrix.v[2].x, rotationMatrix.v[0].y, 1) ); } template inline ::LinearAlgebra::Vector4 ExtractAngularAxis( const ::LinearAlgebra::Matrix4x4 &orientationMatrix ) { return ::std::asin( ::LinearAlgebra::Vector4(orientationMatrix.v[1].z, orientationMatrix.v[2].x, orientationMatrix.v[0].y, 0) ); } template inline ::LinearAlgebra::Matrix4x4 & TranslationMatrix( const ::LinearAlgebra::Vector3 &position, ::LinearAlgebra::Matrix4x4 &targetMem = ::LinearAlgebra::Matrix4x4() ) { return targetMem = ::LinearAlgebra::Matrix4x4( 1, 0, 0, position.x, 0, 1, 0, position.y, 0, 0, 1, position.z, 0, 0, 0, 1 ); } template inline ::LinearAlgebra::Quaternion Rotation( const ScalarType &radian, const ::LinearAlgebra::Vector3 &normalizedAxis ) { ScalarType r = radian * 0.5f, s = std::sin( r ), c = std::cos( r ); return ::LinearAlgebra::Quaternion( normalizedAxis * s, c ); } template inline ::LinearAlgebra::Quaternion Rotation( const ScalarType &radian, const ::LinearAlgebra::Vector4 &normalizedAxis ) { ScalarType r = radian * 0.5f, s = std::sin( r ), c = std::cos( r ); return ::LinearAlgebra::Quaternion( (normalizedAxis * s).xyz, c ); } template inline ::LinearAlgebra::Quaternion Rotation( const ::LinearAlgebra::Vector3 &angularAxis ) { ScalarType radius = angularAxis.Dot( angularAxis ); if( radius != 0 ) { radius = (ScalarType)::std::sqrt( radius ); return Rotation( radius, angularAxis / radius ); } else { return ::LinearAlgebra::Quaternion::identity; } } template inline ::LinearAlgebra::Quaternion Rotation( const ::LinearAlgebra::Vector4 &angularAxis ) { ScalarType radius = angularAxis.Dot( angularAxis ); if( radius != 0 ) { radius = (ScalarType)::std::sqrt( radius ); return Rotation( radius, angularAxis / radius ); } else { return ::LinearAlgebra::Quaternion::identity; } } template inline ::LinearAlgebra::Matrix4x4 & RotationMatrix( const ::LinearAlgebra::Quaternion &rotationQuaternion, ::LinearAlgebra::Matrix4x4 &targetMem = ::LinearAlgebra::Matrix4x4() ) { ::LinearAlgebra::Quaternion conjugate = rotationQuaternion.GetConjugate(); targetMem.v[0] = ::LinearAlgebra::Vector4( (rotationQuaternion*::LinearAlgebra::Vector3(1,0,0)*conjugate).imaginary, 0 ); targetMem.v[1] = ::LinearAlgebra::Vector4( (rotationQuaternion*::LinearAlgebra::Vector3(0,1,0)*conjugate).imaginary, 0 ); targetMem.v[2] = ::LinearAlgebra::Vector4( (rotationQuaternion*::LinearAlgebra::Vector3(0,0,1)*conjugate).imaginary, 0 ); targetMem.v[3] = ::LinearAlgebra::Vector4::standard_unit_w; return targetMem; } template inline ::LinearAlgebra::Matrix4x4 & OrientationMatrix( const ::LinearAlgebra::Quaternion &rotationQuaternion, const ::LinearAlgebra::Vector3 &translation, ::LinearAlgebra::Matrix4x4 &targetMem = ::LinearAlgebra::Matrix4x4() ) { ::LinearAlgebra::Quaternion conjugate = rotationQuaternion.GetConjugate(); targetMem.v[0] = ::LinearAlgebra::Vector4( (rotationQuaternion*::LinearAlgebra::Vector3(1,0,0)*conjugate).imaginary, 0 ); targetMem.v[1] = ::LinearAlgebra::Vector4( (rotationQuaternion*::LinearAlgebra::Vector3(0,1,0)*conjugate).imaginary, 0 ); targetMem.v[2] = ::LinearAlgebra::Vector4( (rotationQuaternion*::LinearAlgebra::Vector3(0,0,1)*conjugate).imaginary, 0 ); targetMem.v[3] = ::LinearAlgebra::Vector4( translation, 1 ); return targetMem; } template inline ::LinearAlgebra::Matrix4x4 & OrientationMatrix( const ::LinearAlgebra::Quaternion &rotationQuaternion, const ::LinearAlgebra::Vector4 &translation, ::LinearAlgebra::Matrix4x4 &targetMem = ::LinearAlgebra::Matrix4x4() ) { ::LinearAlgebra::Quaternion conjugate = rotationQuaternion.GetConjugate(); targetMem.v[0] = ::LinearAlgebra::Vector4( (rotationQuaternion*::LinearAlgebra::Vector3(1,0,0)*conjugate).imaginary, 0 ); targetMem.v[1] = ::LinearAlgebra::Vector4( (rotationQuaternion*::LinearAlgebra::Vector3(0,1,0)*conjugate).imaginary, 0 ); targetMem.v[2] = ::LinearAlgebra::Vector4( (rotationQuaternion*::LinearAlgebra::Vector3(0,0,1)*conjugate).imaginary, 0 ); targetMem.v[3] = translation; return targetMem; } template inline ::LinearAlgebra::Matrix4x4 & ViewMatrix( const ::LinearAlgebra::Quaternion &rotationQuaternion, const ::LinearAlgebra::Vector3 &translation, ::LinearAlgebra::Matrix4x4 &targetMem = ::LinearAlgebra::Matrix4x4() ) { OrientationMatrix( rotationQuaternion, translation, targetMem ); return InverseOrientationMatrix( targetMem, targetMem ); } template inline ::LinearAlgebra::Matrix4x4 & ViewMatrix( const ::LinearAlgebra::Quaternion &rotationQuaternion, const ::LinearAlgebra::Vector4 &translation, ::LinearAlgebra::Matrix4x4 &targetMem = ::LinearAlgebra::Matrix4x4() ) { OrientationMatrix( rotationQuaternion, translation, targetMem ); return InverseOrientationMatrix( targetMem, targetMem ); } template inline ::LinearAlgebra::Matrix3x3 & RotationMatrix_AxisX( const ScalarType &radian, ::LinearAlgebra::Matrix3x3 &targetMem = ::LinearAlgebra::Matrix3x3() ) { ScalarType s = std::sin( radian ), c = std::cos( radian ); return targetMem = ::LinearAlgebra::Matrix3x3( 1, 0, 0, 0, c,-s, 0, s, c ); } template inline ::LinearAlgebra::Matrix4x4 & RotationMatrix_AxisX( const ScalarType &radian, ::LinearAlgebra::Matrix4x4 &targetMem = ::LinearAlgebra::Matrix4x4() ) { ScalarType s = std::sin( radian ), c = std::cos( radian ); return targetMem = ::LinearAlgebra::Matrix4x4( 1, 0, 0, 0, 0, c,-s, 0, 0, s, c, 0, 0, 0, 0, 1 ); } template inline ::LinearAlgebra::Matrix3x3 & RotationMatrix_AxisY( const ScalarType &radian, ::LinearAlgebra::Matrix3x3 &targetMem = ::LinearAlgebra::Matrix3x3() ) { ScalarType s = std::sin( radian ), c = std::cos( radian ); return targetMem = ::LinearAlgebra::Matrix3x3( c, 0, s, 0, 1, 0, -s, 0, c ); } template inline ::LinearAlgebra::Matrix4x4 & RotationMatrix_AxisY( const ScalarType &radian, ::LinearAlgebra::Matrix4x4 &targetMem = ::LinearAlgebra::Matrix4x4() ) { ScalarType s = std::sin( radian ), c = std::cos( radian ); return targetMem = ::LinearAlgebra::Matrix4x4( c, 0, s, 0, 0, 1, 0, 0, -s, 0, c, 0, 0, 0, 0, 1 ); } template inline ::LinearAlgebra::Matrix3x3 & RotationMatrix_AxisZ( const ScalarType &radian, ::LinearAlgebra::Matrix3x3 &targetMem = ::LinearAlgebra::Matrix3x3() ) { return ::LinearAlgebra2D::RotationMatrix( radian, targetMem ); } template inline ::LinearAlgebra::Matrix4x4 & RotationMatrix_AxisZ( const ScalarType &radian, ::LinearAlgebra::Matrix4x4 &targetMem = ::LinearAlgebra::Matrix4x4() ) { ScalarType s = std::sin( radian ), c = std::cos( radian ); return targetMem = ::LinearAlgebra::Matrix4x4( c,-s, 0, 0, s, c, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1 ); } template inline ::LinearAlgebra::Matrix4x4 RotationMatrix( const ::LinearAlgebra::Vector3 &angularAxis ) { ScalarType radian = angularAxis.GetMagnitude(); if( radian != 0 ) { return RotationMatrix( angularAxis / radian, radian ); } else { return ::LinearAlgebra::Matrix4x4::identity; } } template ::LinearAlgebra::Matrix4x4 & RotationMatrix( const ::LinearAlgebra::Vector3 &normalizedAxis, const ScalarType &radian, ::LinearAlgebra::Matrix4x4 &targetMem = ::LinearAlgebra::Matrix4x4() ) { /// TODO: not verified ScalarType r = radian * 0.5f, s = std::sin( r ), c = std::cos( r ); ::LinearAlgebra::Quaternion q( normalizedAxis * s, c ), qConj = q.GetConjugate(); targetMem.v[0] = ::LinearAlgebra::Vector4( (q*::LinearAlgebra::Vector3(1,0,0)*qConj).imaginary, 0 ); targetMem.v[1] = ::LinearAlgebra::Vector4( (q*::LinearAlgebra::Vector3(0,1,0)*qConj).imaginary, 0 ); targetMem.v[2] = ::LinearAlgebra::Vector4( (q*::LinearAlgebra::Vector3(0,0,1)*qConj).imaginary, 0 ); targetMem.v[3] = ::LinearAlgebra::Vector4::standard_unit_w; return targetMem; } template inline ::LinearAlgebra::Matrix3x3 & InverseRotationMatrix( const ::LinearAlgebra::Matrix3x3 &rotationMatrix ) { return rotationMatrix.GetTranspose(); } template inline ::LinearAlgebra::Matrix4x4 & InverseRotationMatrix( const ::LinearAlgebra::Matrix4x4 &rotationMatrix ) { return rotationMatrix.GetTranspose(); } template inline ::LinearAlgebra::Matrix4x4 OrientationMatrix( const ::LinearAlgebra::Matrix3x3 &rotation, const ::LinearAlgebra::Vector3 &translation ) { return ::LinearAlgebra::Matrix4x4( ::LinearAlgebra::Vector4(rotation.v[0], 0), ::LinearAlgebra::Vector4(rotation.v[1], 0), ::LinearAlgebra::Vector4(rotation.v[2], 0), ::LinearAlgebra::Vector4(translation, 1) ); } template inline ::LinearAlgebra::Matrix4x4 OrientationMatrix( const ::LinearAlgebra::Matrix4x4 &rotation, const ::LinearAlgebra::Vector3 &translation ) { return ::LinearAlgebra::Matrix4x4( rotation.v[0], rotation.v[1], rotation.v[2], ::LinearAlgebra::Vector4(translation, 1) ); } template ::LinearAlgebra::Matrix4x4 & OrientationMatrix( const ::LinearAlgebra::Vector3 &normalizedAxis, const ScalarType &deltaRadian, const ::LinearAlgebra::Vector3 &sumTranslation, ::LinearAlgebra::Matrix4x4 &targetMem = ::LinearAlgebra::Matrix4x4() ) { /** @todo TODO: not tested */ RotationMatrix( normalizedAxis, deltaRadian, targetMem ); targetMem.v[3].xyz = sumTranslation; return targetMem; } template ::LinearAlgebra::Matrix4x4 & OrientationMatrix( const ::LinearAlgebra::Vector3 &angularAxis, const ::LinearAlgebra::Vector3 &translation, ::LinearAlgebra::Matrix4x4 &targetMem = ::LinearAlgebra::Matrix4x4() ) { ScalarType radian = angularAxis.Dot( angularAxis ); if( radian > 0 ) { radian = ::std::sqrt( radian ); return OrientationMatrix( angularAxis / radian, radian, translation, targetMem ); } else { targetMem = ::LinearAlgebra::Matrix4x4::identity; targetMem.v[3].xyz = translation; return targetMem; } } template ::LinearAlgebra::Matrix4x4 & ViewMatrix( const ::LinearAlgebra::Vector3 &angularAxis, const ::LinearAlgebra::Vector3 &translation, ::LinearAlgebra::Matrix4x4 &targetMem = ::LinearAlgebra::Matrix4x4() ) { ScalarType radian = angularAxis.Dot( angularAxis ); if( radian > 0 ) { radian = ::std::sqrt( radian ); return InverseOrientationMatrix( OrientationMatrix(angularAxis / radian, radian, translation, targetMem) ); } else { targetMem = ::LinearAlgebra::Matrix4x4::identity; targetMem.v[3].xyz = -translation; return targetMem; } } template ::LinearAlgebra::Matrix4x4 & OrientationMatrix( const ::LinearAlgebra::Vector3 &sumDeltaAngularAxis, const ::LinearAlgebra::Vector3 &sumTranslation, const ::LinearAlgebra::Vector3 ¢erOfRotation, ::LinearAlgebra::Matrix4x4 &targetMem = ::LinearAlgebra::Matrix4x4() ) { /** @todo TODO: not tested */ ScalarType radian = sumDeltaAngularAxis.Dot( sumDeltaAngularAxis ); if( radian > 0 ) { radian = ::std::sqrt( radian ); RotationMatrix( sumDeltaAngularAxis / radian, radian, targetMem ); } else targetMem = ::LinearAlgebra::Matrix4x4::identity; targetMem.v[3].xyz = sumTranslation + centerOfRotation; targetMem.v[3].x -= ::LinearAlgebra::Vector3( targetMem.v[0].x, targetMem.v[1].x, targetMem.v[2].x ).Dot( centerOfRotation ); targetMem.v[3].y -= ::LinearAlgebra::Vector3( targetMem.v[0].y, targetMem.v[1].y, targetMem.v[2].y ).Dot( centerOfRotation ); targetMem.v[3].z -= ::LinearAlgebra::Vector3( targetMem.v[0].z, targetMem.v[1].z, targetMem.v[2].z ).Dot( centerOfRotation ); return targetMem; } template inline ::LinearAlgebra::Matrix4x4 OrientationMatrix_LookAtDirection( const ::LinearAlgebra::Vector3 &normalizedDirection, const ::LinearAlgebra::Vector3 &normalizedUpVector, const ::LinearAlgebra::Vector3 &worldPos ) { // Righthanded system! Forward is considered to be along negative z axis. Up is considered along positive y axis. ::LinearAlgebra::Vector3 right = normalizedDirection.Cross( normalizedUpVector ).GetNormalized(); return ::LinearAlgebra::Matrix4x4( ::LinearAlgebra::Vector4( right, 0.0f ), ::LinearAlgebra::Vector4( right.Cross( normalizedDirection ), 0.0f ), ::LinearAlgebra::Vector4( normalizedDirection, 0.0f ), ::LinearAlgebra::Vector4( worldPos, 1.0f ) ); } template inline ::LinearAlgebra::Matrix4x4 OrientationMatrix_LookAtPos( const ::LinearAlgebra::Vector3 &worldLookAt, const ::LinearAlgebra::Vector3 &normalizedUpVector, const ::LinearAlgebra::Vector3 &worldPos ) { // Righthanded system! Forward is considered to be along negative z axis. Up is considered along positive y axis. ::LinearAlgebra::Vector3 direction = ( worldLookAt - worldPos ).GetNormalized(); return OrientationMatrix_LookAtDirection( direction, normalizedUpVector, worldPos ); } template inline ::LinearAlgebra::Matrix4x4 & InverseOrientationMatrix( const ::LinearAlgebra::Matrix4x4 &orientationMatrix, ::LinearAlgebra::Matrix4x4 &targetMem = ::LinearAlgebra::Matrix4x4() ) { return targetMem = ::LinearAlgebra::Matrix4x4( orientationMatrix.m11, orientationMatrix.m21, orientationMatrix.m31, -orientationMatrix.v[0].xyz.Dot(orientationMatrix.v[3].xyz), orientationMatrix.m12, orientationMatrix.m22, orientationMatrix.m32, -orientationMatrix.v[1].xyz.Dot(orientationMatrix.v[3].xyz), orientationMatrix.m13, orientationMatrix.m23, orientationMatrix.m33, -orientationMatrix.v[2].xyz.Dot(orientationMatrix.v[3].xyz), 0, 0, 0, 1 ); } // O0 = T0 * R0 // O1 = T1 * T0 * R1 * R0 template ::LinearAlgebra::Matrix4x4 & UpdateOrientationMatrix( const ::LinearAlgebra::Vector3 &deltaPosition, const ::LinearAlgebra::Matrix4x4 &deltaRotationMatrix, ::LinearAlgebra::Matrix4x4 &orientationMatrix ) { ::LinearAlgebra::Vector3 position = deltaPosition + orientationMatrix.v[3].xyz; orientationMatrix.v[3].xyz = ::LinearAlgebra::Vector3::null; orientationMatrix = deltaRotationMatrix * orientationMatrix; orientationMatrix.v[3].xyz = position; return orientationMatrix; } template inline ::LinearAlgebra::Matrix4x4 ExtractRotationMatrix( const ::LinearAlgebra::Matrix4x4 &orientationMatrix ) { return ::LinearAlgebra::Matrix4x4( orientationMatrix.v[0], orientationMatrix.v[1], orientationMatrix.v[2], ::LinearAlgebra::Vector4::standard_unit_w ); } /* Creates an orthographic projection matrix designed for DirectX enviroment. width; of the projection sample volume. height; of the projection sample volume. nearClip: Distance to the nearClippingPlane. farClip: Distance to the farClippingPlane targetMem; is set to an orthographic projection matrix and then returned. */ template ::LinearAlgebra::Matrix4x4 & ProjectionMatrix_Orthographic( const ScalarType &width, const ScalarType &height, const ScalarType &nearClip, const ScalarType &farClip, ::LinearAlgebra::Matrix4x4 &targetMem = ::LinearAlgebra::Matrix4x4() ) { /** @todo TODO: not tested */ ScalarType c = 1 / (nearClip - farClip); return targetMem = ::LinearAlgebra::Matrix4x4( 2/width, 0, 0, 0, 0, 2/height, 0, 0, 0, 0, -c, nearClip*c, 0, 0, 0, 1 ); } /******************************************************************* * Creates a perspective projection matrix designed for DirectX enviroment. * @param vertFoV; is the vertical field of vision in radians. (lookup FoV Hor+ ) * @param aspect; is the screenratio width/height (example 16/9 or 16/10 ) * @param nearClip: Distance to the nearClippingPlane * @param farClip: Distance to the farClippingPlane * @param targetMem; is set to a perspective transform matrix and then returned. * @return targetMem * @test Compare with transposed D3D counterpart *******************************************************************/ template ::LinearAlgebra::Matrix4x4 & ProjectionMatrix_Perspective( const ScalarType &vertFoV, const ScalarType &aspect, const ScalarType &nearClip, const ScalarType &farClip, ::LinearAlgebra::Matrix4x4 &targetMem = ::LinearAlgebra::Matrix4x4() ) { ScalarType fov = 1 / ::std::tan( vertFoV * 0.5f ), dDepth = farClip / (farClip - nearClip); return targetMem = ::LinearAlgebra::Matrix4x4( fov / aspect, 0, 0, 0, 0, fov, 0, 0, 0, 0, dDepth, -(dDepth * nearClip), 0, 0, 1, 0 ); } template