#ifndef BOOST_QVM_MAT_OPERATIONS_HPP_INCLUDED #define BOOST_QVM_MAT_OPERATIONS_HPP_INCLUDED /// Copyright (c) 2008-2021 Emil Dotchevski and Reverge Studios, Inc. /// Copyright (c) 2019 agate-pris /// Distributed under the Boost Software License, Version 1.0. (See accompanying /// file LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt) #include #include #include #include #include #include #include #include #include #include namespace boost { namespace qvm { namespace qvm_detail { BOOST_QVM_INLINE_CRITICAL void const * get_valid_ptr_mat_operations() { static int const obj=0; return &obj; } } //////////////////////////////////////////////// namespace qvm_to_string_detail { template std::string to_string( T const & x ); } namespace qvm_detail { template struct to_string_m_defined { static bool const value=false; }; template struct to_string_matrix_elements { template static std::string f( A const & a ) { using namespace qvm_to_string_detail; return ( (I%mat_traits::cols)==0 ? '(' : ',' ) + to_string(mat_traits::template read_element::cols,I%mat_traits::cols>(a)) + ( (I%mat_traits::cols)==mat_traits::cols-1 ? ")" : "" ) + to_string_matrix_elements::f(a); } }; template struct to_string_matrix_elements { template static std::string f( A const & a ) { using namespace qvm_to_string_detail; return ( (SizeMinusOne%mat_traits::cols)==0 ? '(' : ',' ) + to_string(mat_traits::template read_element::cols,SizeMinusOne%mat_traits::cols>(a)) + ')'; } }; } template inline typename enable_if_c< is_mat::value && !qvm_detail::to_string_m_defined::rows,mat_traits::cols>::value, std::string>::type to_string( A const & a ) { return "("+qvm_detail::to_string_matrix_elements<0,mat_traits::rows*mat_traits::cols-1>::f(a)+')'; } //////////////////////////////////////////////// template BOOST_QVM_INLINE_OPERATIONS typename enable_if_c< is_mat::value && is_mat::value && mat_traits::rows==mat_traits::rows && mat_traits::cols==mat_traits::cols, bool>::type cmp( A const & a, B const & b, Cmp f ) { for( int i=0; i!=mat_traits::rows; ++i ) for( int j=0; j!=mat_traits::cols; ++j ) if( !f( mat_traits::read_element_idx(i, j, a), mat_traits::read_element_idx(i, j, b)) ) return false; return true; } //////////////////////////////////////////////// namespace qvm_detail { template struct convert_to_m_defined { static bool const value=false; }; } template BOOST_QVM_INLINE_TRIVIAL typename enable_if_c< is_mat::value && is_mat::value && mat_traits::rows==mat_traits::rows && mat_traits::cols==mat_traits::cols && !qvm_detail::convert_to_m_defined::rows,mat_traits::cols>::value, R>::type convert_to( A const & a ) { R r; assign(r,a); return r; } //////////////////////////////////////////////// namespace qvm_detail { template struct determinant_defined { static bool const value=false; }; } template BOOST_QVM_INLINE_TRIVIAL typename enable_if_c< is_mat::value && mat_traits::rows==mat_traits::cols && !qvm_detail::determinant_defined::rows>::value, typename mat_traits::scalar_type>::type determinant( A const & a ) { return qvm_detail::determinant_impl(a); } //////////////////////////////////////////////// namespace qvm_detail { template class identity_mat_ { identity_mat_( identity_mat_ const & ); identity_mat_ & operator=( identity_mat_ const & ); ~identity_mat_(); public: template BOOST_QVM_INLINE_TRIVIAL operator R() const { R r; assign(r,*this); return r; } }; } template struct mat_traits< qvm_detail::identity_mat_ > { typedef qvm_detail::identity_mat_ this_matrix; typedef T scalar_type; static int const rows=Dim; static int const cols=Dim; template static BOOST_QVM_INLINE_CRITICAL scalar_type read_element( this_matrix const & /*x*/ ) { BOOST_QVM_STATIC_ASSERT(Row>=0); BOOST_QVM_STATIC_ASSERT(Row=0); BOOST_QVM_STATIC_ASSERT(Col::value(Row==Col); } static BOOST_QVM_INLINE_CRITICAL scalar_type read_element_idx( int row, int col, this_matrix const & /*x*/ ) { BOOST_QVM_ASSERT(row>=0); BOOST_QVM_ASSERT(row=0); BOOST_QVM_ASSERT(col::value(row==col); } }; template BOOST_QVM_INLINE_TRIVIAL qvm_detail::identity_mat_ const & identity_mat() { return *(qvm_detail::identity_mat_ const *)qvm_detail::get_valid_ptr_mat_operations(); } template BOOST_QVM_INLINE_OPERATIONS typename enable_if_c< is_mat::value && mat_traits::rows==mat_traits::cols, void>::type set_identity( A & a ) { assign(a,identity_mat::scalar_type,mat_traits::rows>()); } //////////////////////////////////////////////// namespace qvm_detail { template struct projection_ { T const _00; T const _11; T const _22; T const _23; T const _32; BOOST_QVM_INLINE_TRIVIAL projection_( T _00, T _11, T _22, T _23, T _32 ): _00(_00), _11(_11), _22(_22), _23(_23), _32(_32) { } template BOOST_QVM_INLINE_TRIVIAL operator R() const { R r; assign(r,*this); return r; } }; template struct projection_get { template static BOOST_QVM_INLINE_CRITICAL T get( projection_ const & ) { return scalar_traits::value(0); } }; template <> struct projection_get<0,0> { template static BOOST_QVM_INLINE_CRITICAL T get( projection_ const & m ) { return m._00; } }; template <> struct projection_get<1,1> { template static BOOST_QVM_INLINE_CRITICAL T get( projection_ const & m ) { return m._11; } }; template <> struct projection_get<2,2> { template static BOOST_QVM_INLINE_CRITICAL T get( projection_ const & m ) { return m._22; } }; template <> struct projection_get<2,3> { template static BOOST_QVM_INLINE_CRITICAL T get( projection_ const & m ) { return m._23; } }; template <> struct projection_get<3,2> { template static BOOST_QVM_INLINE_CRITICAL T get( projection_ const & m ) { return m._32; } }; } template struct mat_traits< qvm_detail::projection_ > { typedef qvm_detail::projection_ this_matrix; typedef T scalar_type; static int const rows=4; static int const cols=4; template static BOOST_QVM_INLINE_CRITICAL scalar_type read_element( this_matrix const & x ) { BOOST_QVM_STATIC_ASSERT(Row>=0); BOOST_QVM_STATIC_ASSERT(Row=0); BOOST_QVM_STATIC_ASSERT(Col::get(x); } }; template qvm_detail::projection_ BOOST_QVM_INLINE_OPERATIONS perspective_lh( T fov_y, T aspect_ratio, T z_near, T z_far ) { T const one = scalar_traits::value(1); T const ys = one/tan(fov_y/scalar_traits::value(2)); T const xs = ys/aspect_ratio; T const zd = z_far-z_near; T const z1 = z_far/zd; T const z2 = -z_near*z1; return qvm_detail::projection_(xs,ys,z1,z2,one); } template qvm_detail::projection_ BOOST_QVM_INLINE_OPERATIONS perspective_rh( T fov_y, T aspect_ratio, T z_near, T z_far ) { T const one = scalar_traits::value(1); T const ys = one/tan(fov_y/scalar_traits::value(2)); T const xs = ys/aspect_ratio; T const zd = z_near-z_far; T const z1 = z_far/zd; T const z2 = z_near*z1; return qvm_detail::projection_(xs,ys,z1,z2,-one); } //////////////////////////////////////////////// namespace qvm_detail { template class matrix_scalar_cast_ { matrix_scalar_cast_( matrix_scalar_cast_ const & ); matrix_scalar_cast_ & operator=( matrix_scalar_cast_ const & ); ~matrix_scalar_cast_(); public: template BOOST_QVM_INLINE_TRIVIAL matrix_scalar_cast_ & operator=( T const & x ) { assign(*this,x); return *this; } template BOOST_QVM_INLINE_TRIVIAL operator R() const { R r; assign(r,*this); return r; } }; template struct scalar_cast_matrix_filter { }; template <> struct scalar_cast_matrix_filter { typedef int type; }; } template struct mat_traits< qvm_detail::matrix_scalar_cast_ > { typedef Scalar scalar_type; typedef qvm_detail::matrix_scalar_cast_ this_matrix; static int const rows=mat_traits::rows; static int const cols=mat_traits::cols; template static BOOST_QVM_INLINE_CRITICAL scalar_type read_element( this_matrix const & x ) { BOOST_QVM_STATIC_ASSERT(Row>=0); BOOST_QVM_STATIC_ASSERT(Row=0); BOOST_QVM_STATIC_ASSERT(Col::template read_element(reinterpret_cast(x))); } static BOOST_QVM_INLINE_CRITICAL scalar_type read_element_idx( int row, int col, this_matrix const & x ) { BOOST_QVM_ASSERT(row>=0); BOOST_QVM_ASSERT(row=0); BOOST_QVM_ASSERT(col::read_element_idx(col,row,reinterpret_cast(x))); } }; template struct deduce_mat,R,C> { typedef mat type; }; template BOOST_QVM_INLINE_TRIVIAL qvm_detail::matrix_scalar_cast_ const & scalar_cast( T const & x, typename qvm_detail::scalar_cast_matrix_filter::value>::type=0 ) { return reinterpret_cast const &>(x); } //////////////////////////////////////////////// namespace qvm_detail { template struct div_eq_ms_defined { static bool const value=false; }; } template BOOST_QVM_INLINE_OPERATIONS typename enable_if_c< is_mat::value && is_scalar::value && !qvm_detail::div_eq_ms_defined::rows,mat_traits::cols>::value, A &>::type operator/=( A & a, B b ) { for( int i=0; i!=mat_traits::rows; ++i ) for( int j=0; j!=mat_traits::cols; ++j ) mat_traits::write_element_idx(i,j,a)/=b; return a; } //////////////////////////////////////////////// namespace qvm_detail { template struct div_ms_defined { static bool const value=false; }; } template BOOST_QVM_INLINE_OPERATIONS typename lazy_enable_if_c< is_mat::value && is_scalar::value && !qvm_detail::div_ms_defined::rows,mat_traits::cols>::value, deduce_mat2::rows,mat_traits::cols> >::type operator/( A const & a, B b ) { typedef typename deduce_mat2::rows,mat_traits::cols>::type R; R r; for( int i=0; i!=mat_traits::rows; ++i ) for( int j=0; j!=mat_traits::cols; ++j ) mat_traits::write_element_idx(i,j,r)=mat_traits::read_element_idx(i,j,a)/b; return r; } //////////////////////////////////////////////// namespace qvm_detail { template struct eq_mm_defined { static bool const value=false; }; } template BOOST_QVM_INLINE_OPERATIONS typename enable_if_c< is_mat::value && is_mat::value && mat_traits::rows==mat_traits::rows && mat_traits::cols==mat_traits::cols && !qvm_detail::eq_mm_defined::rows,mat_traits::cols>::value, bool>::type operator==( A const & a, B const & b ) { for( int i=0; i!=mat_traits::rows; ++i ) for( int j=0; j!=mat_traits::cols; ++j ) if( mat_traits::read_element_idx(i,j,a)!=mat_traits::read_element_idx(i,j,b) ) return false; return true; } //////////////////////////////////////////////// namespace qvm_detail { template struct minus_eq_mm_defined { static bool const value=false; }; } template BOOST_QVM_INLINE_OPERATIONS typename enable_if_c< is_mat::value && is_mat::value && mat_traits::rows==mat_traits::rows && mat_traits::cols==mat_traits::cols && !qvm_detail::minus_eq_mm_defined::rows,mat_traits::cols>::value, A &>::type operator-=( A & a, B const & b ) { for( int i=0; i!=mat_traits::rows; ++i ) for( int j=0; j!=mat_traits::cols; ++j ) mat_traits::write_element_idx(i,j,a)-=mat_traits::read_element_idx(i,j,b); return a; } //////////////////////////////////////////////// namespace qvm_detail { template struct minus_m_defined { static bool const value=false; }; } template BOOST_QVM_INLINE_OPERATIONS typename lazy_enable_if_c< is_mat::value && !qvm_detail::minus_m_defined::rows,mat_traits::cols>::value, deduce_mat >::type operator-( A const & a ) { typedef typename deduce_mat::type R; R r; for( int i=0; i!=mat_traits::rows; ++i ) for( int j=0; j!=mat_traits::cols; ++j ) mat_traits::write_element_idx(i,j,r)=-mat_traits::read_element_idx(i,j,a); return r; } //////////////////////////////////////////////// namespace qvm_detail { template struct minus_mm_defined { static bool const value=false; }; } template BOOST_QVM_INLINE_OPERATIONS typename lazy_enable_if_c< is_mat::value && is_mat::value && mat_traits::rows==mat_traits::rows && mat_traits::cols==mat_traits::cols && !qvm_detail::minus_mm_defined::rows,mat_traits::cols>::value, deduce_mat2::rows,mat_traits::cols> >::type operator-( A const & a, B const & b ) { typedef typename deduce_mat2::rows,mat_traits::cols>::type R; R r; for( int i=0; i!=mat_traits::rows; ++i ) for( int j=0; j!=mat_traits::cols; ++j ) mat_traits::write_element_idx(i,j,r)=mat_traits::read_element_idx(i,j,a)-mat_traits::read_element_idx(i,j,b); return r; } //////////////////////////////////////////////// namespace qvm_detail { template struct mul_eq_mm_defined { static bool const value=false; }; } template BOOST_QVM_INLINE_OPERATIONS typename enable_if_c< is_mat::value && is_mat::value && mat_traits::rows==mat_traits::cols && mat_traits::rows==mat_traits::rows && mat_traits::cols==mat_traits::cols && !qvm_detail::mul_eq_mm_defined::rows>::value, A &>::type operator*=( A & r, B const & b ) { typedef typename mat_traits::scalar_type Ta; Ta a[mat_traits::rows][mat_traits::cols]; for( int i=0; i::rows; ++i ) for( int j=0; j::cols; ++j ) a[i][j]=mat_traits::read_element_idx(i,j,r); for( int i=0; i::rows; ++i ) for( int j=0; j::cols; ++j ) { Ta x(scalar_traits::value(0)); for( int k=0; k::cols; ++k ) x += a[i][k]*mat_traits::read_element_idx(k,j,b); mat_traits::write_element_idx(i,j,r) = x; } return r; } //////////////////////////////////////////////// namespace qvm_detail { template struct mul_eq_ms_defined { static bool const value=false; }; } template BOOST_QVM_INLINE_OPERATIONS typename enable_if_c< is_mat::value && is_scalar::value && !qvm_detail::mul_eq_ms_defined::rows,mat_traits::cols>::value, A &>::type operator*=( A & a, B b ) { for( int i=0; i!=mat_traits::rows; ++i ) for( int j=0; j!=mat_traits::cols; ++j ) mat_traits::write_element_idx(i,j,a)*=b; return a; } //////////////////////////////////////////////// namespace qvm_detail { template struct mul_mm_defined { static bool const value=false; }; } template BOOST_QVM_INLINE_OPERATIONS typename lazy_enable_if_c< is_mat::value && is_mat::value && mat_traits::cols==mat_traits::rows && !qvm_detail::mul_mm_defined::rows,mat_traits::cols,mat_traits::cols>::value, deduce_mat2::rows,mat_traits::cols> >::type operator*( A const & a, B const & b ) { typedef typename deduce_mat2::rows,mat_traits::cols>::type R; R r; for( int i=0; i::rows; ++i ) for( int j=0; j::cols; ++j ) { typedef typename mat_traits::scalar_type Ta; Ta x(scalar_traits::value(0)); for( int k=0; k::cols; ++k ) x += mat_traits::read_element_idx(i,k,a)*mat_traits::read_element_idx(k,j,b); mat_traits::write_element_idx(i,j,r) = x; } return r; } //////////////////////////////////////////////// namespace qvm_detail { template struct mul_ms_defined { static bool const value=false; }; } template BOOST_QVM_INLINE_OPERATIONS typename lazy_enable_if_c< is_mat::value && is_scalar::value && !qvm_detail::mul_ms_defined::rows,mat_traits::cols>::value, deduce_mat2::rows,mat_traits::cols> >::type operator*( A const & a, B b ) { typedef typename deduce_mat2::rows,mat_traits::cols>::type R; R r; for( int i=0; i!=mat_traits::rows; ++i ) for( int j=0; j!=mat_traits::cols; ++j ) mat_traits::write_element_idx(i,j,r)=mat_traits::read_element_idx(i,j,a)*b; return r; } //////////////////////////////////////////////// namespace qvm_detail { template struct mul_sm_defined { static bool const value=false; }; } template BOOST_QVM_INLINE_OPERATIONS typename lazy_enable_if_c< is_scalar::value && is_mat::value && !qvm_detail::mul_sm_defined::rows,mat_traits::cols>::value, deduce_mat2::rows,mat_traits::cols> >::type operator*( A a, B const & b ) { typedef typename deduce_mat2::rows,mat_traits::cols>::type R; R r; for( int i=0; i!=mat_traits::rows; ++i ) for( int j=0; j!=mat_traits::cols; ++j ) mat_traits::write_element_idx(i,j,r)=a*mat_traits::read_element_idx(i,j,b); return r; } //////////////////////////////////////////////// namespace qvm_detail { template struct neq_mm_defined { static bool const value=false; }; } template BOOST_QVM_INLINE_OPERATIONS typename enable_if_c< is_mat::value && is_mat::value && mat_traits::rows==mat_traits::rows && mat_traits::cols==mat_traits::cols && !qvm_detail::neq_mm_defined::rows,mat_traits::cols>::value, bool>::type operator!=( A const & a, B const & b ) { for( int i=0; i!=mat_traits::rows; ++i ) for( int j=0; j!=mat_traits::cols; ++j ) if( mat_traits::read_element_idx(i,j,a)!=mat_traits::read_element_idx(i,j,b) ) return true; return false; } //////////////////////////////////////////////// namespace qvm_detail { template struct plus_eq_mm_defined { static bool const value=false; }; } template BOOST_QVM_INLINE_OPERATIONS typename enable_if_c< is_mat::value && is_mat::value && mat_traits::rows==mat_traits::rows && mat_traits::cols==mat_traits::cols && !qvm_detail::plus_eq_mm_defined::rows,mat_traits::cols>::value, A &>::type operator+=( A & a, B const & b ) { for( int i=0; i!=mat_traits::rows; ++i ) for( int j=0; j!=mat_traits::cols; ++j ) mat_traits::write_element_idx(i,j,a)+=mat_traits::read_element_idx(i,j,b); return a; } //////////////////////////////////////////////// namespace qvm_detail { template struct plus_mm_defined { static bool const value=false; }; } template BOOST_QVM_INLINE_OPERATIONS typename lazy_enable_if_c< is_mat::value && is_mat::value && mat_traits::rows==mat_traits::rows && mat_traits::cols==mat_traits::cols && !qvm_detail::plus_mm_defined::rows,mat_traits::cols>::value, deduce_mat2::rows,mat_traits::cols> >::type operator+( A const & a, B const & b ) { typedef typename deduce_mat2::rows,mat_traits::cols>::type R; R r; for( int i=0; i!=mat_traits::rows; ++i ) for( int j=0; j!=mat_traits::cols; ++j ) mat_traits::write_element_idx(i,j,r)=mat_traits::read_element_idx(i,j,a)+mat_traits::read_element_idx(i,j,b); return r; } //////////////////////////////////////////////// namespace qvm_detail { template class mref_ { mref_( mref_ const & ); mref_ & operator=( mref_ const & ); ~mref_(); public: template BOOST_QVM_INLINE_TRIVIAL mref_ & operator=( R const & x ) { assign(*this,x); return *this; } template BOOST_QVM_INLINE_TRIVIAL operator R() const { R r; assign(r,*this); return r; } }; } template struct mat_traits< qvm_detail::mref_ > { typedef typename mat_traits::scalar_type scalar_type; typedef qvm_detail::mref_ this_matrix; static int const rows=mat_traits::rows; static int const cols=mat_traits::cols; template static BOOST_QVM_INLINE_CRITICAL scalar_type read_element( this_matrix const & x ) { BOOST_QVM_STATIC_ASSERT(Row>=0); BOOST_QVM_STATIC_ASSERT(Row=0); BOOST_QVM_STATIC_ASSERT(Col::template read_element(reinterpret_cast(x)); } template static BOOST_QVM_INLINE_CRITICAL scalar_type & write_element( this_matrix & x ) { BOOST_QVM_STATIC_ASSERT(Row>=0); BOOST_QVM_STATIC_ASSERT(Row=0); BOOST_QVM_STATIC_ASSERT(Col::template write_element(reinterpret_cast(x)); } static BOOST_QVM_INLINE_CRITICAL scalar_type read_element_idx( int row, int col, this_matrix const & x ) { BOOST_QVM_ASSERT(row>=0); BOOST_QVM_ASSERT(row=0); BOOST_QVM_ASSERT(col::read_element_idx(row,col,reinterpret_cast(x)); } static BOOST_QVM_INLINE_CRITICAL scalar_type & write_element_idx( int row, int col, this_matrix & x ) { BOOST_QVM_ASSERT(row>=0); BOOST_QVM_ASSERT(row=0); BOOST_QVM_ASSERT(col::write_element_idx(row,col,reinterpret_cast(x)); } }; template struct deduce_mat,R,C> { typedef mat::scalar_type,R,C> type; }; template BOOST_QVM_INLINE_TRIVIAL typename enable_if_c< is_mat::value, qvm_detail::mref_ const &>::type mref( M const & a ) { return reinterpret_cast const &>(a); } template BOOST_QVM_INLINE_TRIVIAL typename enable_if_c< is_mat::value, qvm_detail::mref_ &>::type mref( M & a ) { return reinterpret_cast &>(a); } //////////////////////////////////////////////// namespace qvm_detail { template class zero_mat_ { zero_mat_( zero_mat_ const & ); zero_mat_ & operator=( zero_mat_ const & ); ~zero_mat_(); public: template BOOST_QVM_INLINE_TRIVIAL operator R() const { R r; assign(r,*this); return r; } }; } template struct mat_traits< qvm_detail::zero_mat_ > { typedef qvm_detail::zero_mat_ this_matrix; typedef T scalar_type; static int const rows=Rows; static int const cols=Cols; template static BOOST_QVM_INLINE_CRITICAL scalar_type read_element( this_matrix const & ) { BOOST_QVM_STATIC_ASSERT(Row>=0); BOOST_QVM_STATIC_ASSERT(Row=0); BOOST_QVM_STATIC_ASSERT(Col::value(0); } static BOOST_QVM_INLINE_CRITICAL scalar_type read_element_idx( int row, int col, this_matrix const & ) { BOOST_QVM_ASSERT(row>=0); BOOST_QVM_ASSERT(row=0); BOOST_QVM_ASSERT(col::value(0); } }; template struct deduce_mat,R,C> { typedef mat type; }; template BOOST_QVM_INLINE_TRIVIAL qvm_detail::zero_mat_ const & zero_mat() { return *(qvm_detail::zero_mat_ const *)qvm_detail::get_valid_ptr_mat_operations(); } template BOOST_QVM_INLINE_TRIVIAL qvm_detail::zero_mat_ const & zero_mat() { return *(qvm_detail::zero_mat_ const *)qvm_detail::get_valid_ptr_mat_operations(); } template BOOST_QVM_INLINE_OPERATIONS typename enable_if_c< is_mat::value, void>::type set_zero( A & a ) { assign(a,zero_mat::scalar_type,mat_traits::rows,mat_traits::cols>()); } //////////////////////////////////////////////// namespace qvm_detail { template struct rot_mat_ { typedef S scalar_type; scalar_type a[3][3]; BOOST_QVM_INLINE rot_mat_( scalar_type a00, scalar_type a01, scalar_type a02, scalar_type a10, scalar_type a11, scalar_type a12, scalar_type a20, scalar_type a21, scalar_type a22 ) { a[0][0] = a00; a[0][1] = a01; a[0][2] = a02; a[1][0] = a10; a[1][1] = a11; a[1][2] = a12; a[2][0] = a20; a[2][1] = a21; a[2][2] = a22; } template BOOST_QVM_INLINE_TRIVIAL operator R() const { R r; assign(r,*this); return r; } }; template struct rot_m_get { template static BOOST_QVM_INLINE_CRITICAL T get( T const (&)[3][3] ) { return scalar_traits::value(Row==Col); } }; template <> struct rot_m_get<0,0> { template static BOOST_QVM_INLINE_CRITICAL T get( T const (&a)[3][3] ) { return a[0][0]; } }; template <> struct rot_m_get<0,1> { template static BOOST_QVM_INLINE_CRITICAL T get( T const (&a)[3][3] ) { return a[0][1]; } }; template <> struct rot_m_get<0,2> { template static BOOST_QVM_INLINE_CRITICAL T get( T const (&a)[3][3] ) { return a[0][2]; } }; template <> struct rot_m_get<1,0> { template static BOOST_QVM_INLINE_CRITICAL T get( T const (&a)[3][3] ) { return a[1][0]; } }; template <> struct rot_m_get<1,1> { template static BOOST_QVM_INLINE_CRITICAL T get( T const (&a)[3][3] ) { return a[1][1]; } }; template <> struct rot_m_get<1,2> { template static BOOST_QVM_INLINE_CRITICAL T get( T const (&a)[3][3] ) { return a[1][2]; } }; template <> struct rot_m_get<2,0> { template static BOOST_QVM_INLINE_CRITICAL T get( T const (&a)[3][3] ) { return a[2][0]; } }; template <> struct rot_m_get<2,1> { template static BOOST_QVM_INLINE_CRITICAL T get( T const (&a)[3][3] ) { return a[2][1]; } }; template <> struct rot_m_get<2,2> { template static BOOST_QVM_INLINE_CRITICAL T get( T const (&a)[3][3] ) { return a[2][2]; } }; } template struct mat_traits; template struct mat_traits< qvm_detail::rot_mat_ > { typedef qvm_detail::rot_mat_ this_matrix; typedef typename this_matrix::scalar_type scalar_type; static int const rows=D; static int const cols=D; template static BOOST_QVM_INLINE_CRITICAL scalar_type read_element( this_matrix const & x ) { BOOST_QVM_STATIC_ASSERT(Row>=0); BOOST_QVM_STATIC_ASSERT(Row=0); BOOST_QVM_STATIC_ASSERT(Col::get(x.a); } static BOOST_QVM_INLINE_CRITICAL scalar_type read_element_idx( int row, int col, this_matrix const & x ) { BOOST_QVM_ASSERT(row>=0); BOOST_QVM_ASSERT(row=0); BOOST_QVM_ASSERT(col::value(row==col); } }; template BOOST_QVM_INLINE typename enable_if_c< is_vec::value && vec_traits::dim==3, qvm_detail::rot_mat_ >::type rot_mat( V const & axis, Angle angle ) { typedef Angle scalar_type; scalar_type const x=vec_traits::template read_element<0>(axis); scalar_type const y=vec_traits::template read_element<1>(axis); scalar_type const z=vec_traits::template read_element<2>(axis); scalar_type const m2=x*x+y*y+z*z; if( m2==scalar_traits::value(0) ) BOOST_QVM_THROW_EXCEPTION(zero_magnitude_error()); scalar_type const s = sin(angle); scalar_type const c = cos(angle); scalar_type const x2 = x*x; scalar_type const y2 = y*y; scalar_type const z2 = z*z; scalar_type const xy = x*y; scalar_type const xz = x*z; scalar_type const yz = y*z; scalar_type const xs = x*s; scalar_type const ys = y*s; scalar_type const zs = z*s; scalar_type const one = scalar_traits::value(1); scalar_type const c1 = one-c; return qvm_detail::rot_mat_( x2+(one-x2)*c, xy*c1-zs, xz*(one-c)+ys, xy*c1+zs, y2+(one-y2)*c, yz*c1-xs, xz*c1-ys, yz*c1+xs, z2+(one-z2)*c ); } template BOOST_QVM_INLINE_OPERATIONS typename enable_if_c< is_mat::value && mat_traits::rows==mat_traits::cols && mat_traits::rows>=3 && is_vec::value && vec_traits::dim==3, void>::type set_rot( A & a, B const & axis, Angle angle ) { assign(a,rot_mat::rows>(axis,angle)); } template BOOST_QVM_INLINE_OPERATIONS typename enable_if_c< is_mat::value && mat_traits::rows==mat_traits::cols && mat_traits::rows>=3 && is_vec::value && vec_traits::dim==3, void>::type rotate( A & a, B const & axis, Angle angle ) { a *= rot_mat::rows>(axis,angle); } //////////////////////////////////////////////// template BOOST_QVM_INLINE qvm_detail::rot_mat_ rot_mat_xzy( Angle x1, Angle z2, Angle y3 ) { typedef Angle scalar_type; scalar_type const c1 = cos(x1); scalar_type const s1 = sin(x1); scalar_type const c2 = cos(z2); scalar_type const s2 = sin(z2); scalar_type const c3 = cos(y3); scalar_type const s3 = sin(y3); return qvm_detail::rot_mat_( c2*c3, -s2, c2*s3, s1*s3 + c1*c3*s2, c1*c2, c1*s2*s3 - c3*s1, c3*s1*s2 - c1*s3, c2*s1, c1*c3 + s1*s2*s3 ); } template BOOST_QVM_INLINE_OPERATIONS typename enable_if_c< is_mat::value && mat_traits::rows==mat_traits::cols && mat_traits::rows>=3, void>::type set_rot_xzy( A & a, Angle x1, Angle z2, Angle y3 ) { assign(a,rot_mat_xzy::rows>(x1,z2,y3)); } template BOOST_QVM_INLINE_OPERATIONS typename enable_if_c< is_mat::value && mat_traits::rows==mat_traits::cols && mat_traits::rows>=3, void>::type rotate_xzy( A & a, Angle x1, Angle z2, Angle y3 ) { a *= rot_mat_xzy::rows>(x1,z2,y3); } //////////////////////////////////////////////// template BOOST_QVM_INLINE qvm_detail::rot_mat_ rot_mat_xyz( Angle x1, Angle y2, Angle z3 ) { typedef Angle scalar_type; scalar_type const c1 = cos(x1); scalar_type const s1 = sin(x1); scalar_type const c2 = cos(y2); scalar_type const s2 = sin(y2); scalar_type const c3 = cos(z3); scalar_type const s3 = sin(z3); return qvm_detail::rot_mat_( c2*c3, -c2*s3, s2, c1*s3 + c3*s1*s2, c1*c3 - s1*s2*s3, -c2*s1, s1*s3 - c1*c3*s2, c3*s1 + c1*s2*s3, c1*c2 ); } template BOOST_QVM_INLINE_OPERATIONS typename enable_if_c< is_mat::value && mat_traits::rows==mat_traits::cols && mat_traits::rows>=3, void>::type set_rot_xyz( A & a, Angle x1, Angle y2, Angle z3 ) { assign(a,rot_mat_xyz::rows>(x1,y2,z3)); } template BOOST_QVM_INLINE_OPERATIONS typename enable_if_c< is_mat::value && mat_traits::rows==mat_traits::cols && mat_traits::rows>=3, void>::type rotate_xyz( A & a, Angle x1, Angle y2, Angle z3 ) { a *= rot_mat_xyz::rows>(x1,y2,z3); } //////////////////////////////////////////////// template BOOST_QVM_INLINE qvm_detail::rot_mat_ rot_mat_yxz( Angle y1, Angle x2, Angle z3 ) { typedef Angle scalar_type; scalar_type const c1 = cos(y1); scalar_type const s1 = sin(y1); scalar_type const c2 = cos(x2); scalar_type const s2 = sin(x2); scalar_type const c3 = cos(z3); scalar_type const s3 = sin(z3); return qvm_detail::rot_mat_( c1*c3 + s1*s2*s3, c3*s1*s2 - c1*s3, c2*s1, c2*s3, c2*c3, -s2, c1*s2*s3 - c3*s1, c1*c3*s2 + s1*s3, c1*c2 ); } template BOOST_QVM_INLINE_OPERATIONS typename enable_if_c< is_mat::value && mat_traits::rows==mat_traits::cols && mat_traits::rows>=3, void>::type set_rot_yxz( A & a, Angle y1, Angle x2, Angle z3 ) { assign(a,rot_mat_yxz::rows>(y1,x2,z3)); } template BOOST_QVM_INLINE_OPERATIONS typename enable_if_c< is_mat::value && mat_traits::rows==mat_traits::cols && mat_traits::rows>=3, void>::type rotate_yxz( A & a, Angle y1, Angle x2, Angle z3 ) { a *= rot_mat_yxz::rows>(y1,x2,z3); } //////////////////////////////////////////////// template BOOST_QVM_INLINE qvm_detail::rot_mat_ rot_mat_yzx( Angle y1, Angle z2, Angle x3 ) { typedef Angle scalar_type; scalar_type const c1 = cos(y1); scalar_type const s1 = sin(y1); scalar_type const c2 = cos(z2); scalar_type const s2 = sin(z2); scalar_type const c3 = cos(x3); scalar_type const s3 = sin(x3); return qvm_detail::rot_mat_( c1*c2, s1*s3 - c1*c3*s2, c3*s1 + c1*s2*s3, s2, c2*c3, -c2*s3, -c2*s1, c1*s3 + c3*s1*s2, c1*c3 - s1*s2*s3 ); } template BOOST_QVM_INLINE_OPERATIONS typename enable_if_c< is_mat::value && mat_traits::rows==mat_traits::cols && mat_traits::rows>=3, void>::type set_rot_yzx( A & a, Angle y1, Angle z2, Angle x3 ) { assign(a,rot_mat_yzx::rows>(y1,z2,x3)); } template BOOST_QVM_INLINE_OPERATIONS typename enable_if_c< is_mat::value && mat_traits::rows==mat_traits::cols && mat_traits::rows>=3, void>::type rotate_yzx( A & a, Angle y1, Angle z2, Angle x3 ) { a *= rot_mat_yzx::rows>(y1,z2,x3); } //////////////////////////////////////////////// template BOOST_QVM_INLINE qvm_detail::rot_mat_ rot_mat_zyx( Angle z1, Angle y2, Angle x3 ) { typedef Angle scalar_type; scalar_type const c1 = cos(z1); scalar_type const s1 = sin(z1); scalar_type const c2 = cos(y2); scalar_type const s2 = sin(y2); scalar_type const c3 = cos(x3); scalar_type const s3 = sin(x3); return qvm_detail::rot_mat_( c1*c2, c1*s2*s3 - c3*s1, s1*s3 + c1*c3*s2, c2*s1, c1*c3 + s1*s2*s3, c3*s1*s2 - c1*s3, -s2, c2*s3, c2*c3 ); } template BOOST_QVM_INLINE_OPERATIONS typename enable_if_c< is_mat::value && mat_traits::rows==mat_traits::cols && mat_traits::rows>=3, void>::type set_rot_zyx( A & a, Angle z1, Angle y2, Angle x3 ) { assign(a,rot_mat_zyx::rows>(z1,y2,x3)); } template BOOST_QVM_INLINE_OPERATIONS typename enable_if_c< is_mat::value && mat_traits::rows==mat_traits::cols && mat_traits::rows>=3, void>::type rotate_zyx( A & a, Angle z1, Angle y2, Angle x3 ) { a *= rot_mat_zyx::rows>(z1,y2,x3); } //////////////////////////////////////////////// template BOOST_QVM_INLINE qvm_detail::rot_mat_ rot_mat_zxy( Angle z1, Angle x2, Angle y3 ) { typedef Angle scalar_type; scalar_type const c1 = cos(z1); scalar_type const s1 = sin(z1); scalar_type const c2 = cos(x2); scalar_type const s2 = sin(x2); scalar_type const c3 = cos(y3); scalar_type const s3 = sin(y3); return qvm_detail::rot_mat_( c1*c3 - s1*s2*s3, -c2*s1, c1*s3 + c3*s1*s2, c3*s1 + c1*s2*s3, c1*c2, s1*s3 - c1*c3*s2, -c2*s3, s2, c2*c3 ); } template BOOST_QVM_INLINE_OPERATIONS typename enable_if_c< is_mat::value && mat_traits::rows==mat_traits::cols && mat_traits::rows>=3, void>::type set_rot_zxy( A & a, Angle z1, Angle x2, Angle y3 ) { assign(a,rot_mat_zxy::rows>(z1,x2,y3)); } template BOOST_QVM_INLINE_OPERATIONS typename enable_if_c< is_mat::value && mat_traits::rows==mat_traits::cols && mat_traits::rows>=3, void>::type rotate_zxy( A & a, Angle z1, Angle x2, Angle y3 ) { a *= rot_mat_zxy::rows>(z1,x2,y3); } //////////////////////////////////////////////// template BOOST_QVM_INLINE qvm_detail::rot_mat_ rot_mat_xzx( Angle x1, Angle z2, Angle x3 ) { typedef Angle scalar_type; scalar_type const c1 = cos(x1); scalar_type const s1 = sin(x1); scalar_type const c2 = cos(z2); scalar_type const s2 = sin(z2); scalar_type const c3 = cos(x3); scalar_type const s3 = sin(x3); return qvm_detail::rot_mat_( c2, -c3*s2, s2*s3, c1*s2, c1*c2*c3 - s1*s3, -c3*s1 - c1*c2*s3, s1*s2, c1*s3 + c2*c3*s1, c1*c3 - c2*s1*s3 ); } template BOOST_QVM_INLINE_OPERATIONS typename enable_if_c< is_mat::value && mat_traits::rows==mat_traits::cols && mat_traits::rows>=3, void>::type set_rot_xzx( A & a, Angle x1, Angle z2, Angle x3 ) { assign(a,rot_mat_xzx::rows>(x1,z2,x3)); } template BOOST_QVM_INLINE_OPERATIONS typename enable_if_c< is_mat::value && mat_traits::rows==mat_traits::cols && mat_traits::rows>=3, void>::type rotate_xzx( A & a, Angle x1, Angle z2, Angle x3 ) { a *= rot_mat_xzx::rows>(x1,z2,x3); } //////////////////////////////////////////////// template BOOST_QVM_INLINE qvm_detail::rot_mat_ rot_mat_xyx( Angle x1, Angle y2, Angle x3 ) { typedef Angle scalar_type; scalar_type const c1 = cos(x1); scalar_type const s1 = sin(x1); scalar_type const c2 = cos(y2); scalar_type const s2 = sin(y2); scalar_type const c3 = cos(x3); scalar_type const s3 = sin(x3); return qvm_detail::rot_mat_( c2, s2*s3, c3*s2, s1*s2, c1*c3 - c2*s1*s3, -c1*s3 - c2*c3*s1, -c1*s2, c3*s1 + c1*c2*s3, c1*c2*c3 - s1*s3 ); } template BOOST_QVM_INLINE_OPERATIONS typename enable_if_c< is_mat::value && mat_traits::rows==mat_traits::cols && mat_traits::rows>=3, void>::type set_rot_xyx( A & a, Angle x1, Angle y2, Angle x3 ) { assign(a,rot_mat_xyx::rows>(x1,y2,x3)); } template BOOST_QVM_INLINE_OPERATIONS typename enable_if_c< is_mat::value && mat_traits::rows==mat_traits::cols && mat_traits::rows>=3, void>::type rotate_xyx( A & a, Angle x1, Angle y2, Angle x3 ) { a *= rot_mat_xyx::rows>(x1,y2,x3); } //////////////////////////////////////////////// template BOOST_QVM_INLINE qvm_detail::rot_mat_ rot_mat_yxy( Angle y1, Angle x2, Angle y3 ) { typedef Angle scalar_type; scalar_type const c1 = cos(y1); scalar_type const s1 = sin(y1); scalar_type const c2 = cos(x2); scalar_type const s2 = sin(x2); scalar_type const c3 = cos(y3); scalar_type const s3 = sin(y3); return qvm_detail::rot_mat_( c1*c3 - c2*s1*s3, s1*s2, c1*s3 + c2*c3*s1, s2*s3, c2, -c3*s2, -c3*s1 - c1*c2*s3, c1*s2, c1*c2*c3 - s1*s3 ); } template BOOST_QVM_INLINE_OPERATIONS typename enable_if_c< is_mat::value && mat_traits::rows==mat_traits::cols && mat_traits::rows>=3, void>::type set_rot_yxy( A & a, Angle y1, Angle x2, Angle y3 ) { assign(a,rot_mat_yxy::rows>(y1,x2,y3)); } template BOOST_QVM_INLINE_OPERATIONS typename enable_if_c< is_mat::value && mat_traits::rows==mat_traits::cols && mat_traits::rows>=3, void>::type rotate_yxy( A & a, Angle y1, Angle x2, Angle y3 ) { a *= rot_mat_yxy::rows>(y1,x2,y3); } //////////////////////////////////////////////// template BOOST_QVM_INLINE qvm_detail::rot_mat_ rot_mat_yzy( Angle y1, Angle z2, Angle y3 ) { typedef Angle scalar_type; scalar_type const c1 = cos(y1); scalar_type const s1 = sin(y1); scalar_type const c2 = cos(z2); scalar_type const s2 = sin(z2); scalar_type const c3 = cos(y3); scalar_type const s3 = sin(y3); return qvm_detail::rot_mat_( c1*c2*c3 - s1*s3, -c1*s2, c3*s1 + c1*c2*s3, c3*s2, c2, s2*s3, -c1*s3 - c2*c3*s1, s1*s2, c1*c3 - c2*s1*s3 ); } template BOOST_QVM_INLINE_OPERATIONS typename enable_if_c< is_mat::value && mat_traits::rows==mat_traits::cols && mat_traits::rows>=3, void>::type set_rot_yzy( A & a, Angle y1, Angle z2, Angle y3 ) { assign(a,rot_mat_yzy::rows>(y1,z2,y3)); } template BOOST_QVM_INLINE_OPERATIONS typename enable_if_c< is_mat::value && mat_traits::rows==mat_traits::cols && mat_traits::rows>=3, void>::type rotate_yzy( A & a, Angle y1, Angle z2, Angle y3 ) { a *= rot_mat_yzy::rows>(y1,z2,y3); } //////////////////////////////////////////////// template BOOST_QVM_INLINE qvm_detail::rot_mat_ rot_mat_zyz( Angle z1, Angle y2, Angle z3 ) { typedef Angle scalar_type; scalar_type const c1 = cos(z1); scalar_type const s1 = sin(z1); scalar_type const c2 = cos(y2); scalar_type const s2 = sin(y2); scalar_type const c3 = cos(z3); scalar_type const s3 = sin(z3); return qvm_detail::rot_mat_( c1*c2*c3 - s1*s3, -c3*s1 - c1*c2*s3, c1*s2, c1*s3 + c2*c3*s1, c1*c3 - c2*s1*s3, s1*s2, -c3*s2, s2*s3, c2 ); } template BOOST_QVM_INLINE_OPERATIONS typename enable_if_c< is_mat::value && mat_traits::rows==mat_traits::cols && mat_traits::rows>=3, void>::type set_rot_zyz( A & a, Angle z1, Angle y2, Angle z3 ) { assign(a,rot_mat_zyz::rows>(z1,y2,z3)); } template BOOST_QVM_INLINE_OPERATIONS typename enable_if_c< is_mat::value && mat_traits::rows==mat_traits::cols && mat_traits::rows>=3, void>::type rotate_zyz( A & a, Angle z1, Angle y2, Angle z3 ) { a *= rot_mat_zyz::rows>(z1,y2,z3); } //////////////////////////////////////////////// template BOOST_QVM_INLINE qvm_detail::rot_mat_ rot_mat_zxz( Angle z1, Angle x2, Angle z3 ) { typedef Angle scalar_type; scalar_type const c1 = cos(z1); scalar_type const s1 = sin(z1); scalar_type const c2 = cos(x2); scalar_type const s2 = sin(x2); scalar_type const c3 = cos(z3); scalar_type const s3 = sin(z3); return qvm_detail::rot_mat_( c1*c3 - c2*s1*s3, -c1*s3 - c2*c3*s1, s1*s2, c3*s1 + c1*c2*s3, c1*c2*c3 - s1*s3, -c1*s2, s2*s3, c3*s2, c2 ); } template BOOST_QVM_INLINE_OPERATIONS typename enable_if_c< is_mat::value && mat_traits::rows==mat_traits::cols && mat_traits::rows>=3, void>::type set_rot_zxz( A & a, Angle z1, Angle x2, Angle z3 ) { assign(a,rot_mat_zxz::rows>(z1,x2,z3)); } template BOOST_QVM_INLINE_OPERATIONS typename enable_if_c< is_mat::value && mat_traits::rows==mat_traits::cols && mat_traits::rows>=3, void>::type rotate_zxz( A & a, Angle z1, Angle x2, Angle z3 ) { a *= rot_mat_zxz::rows>(z1,x2,z3); } //////////////////////////////////////////////// namespace qvm_detail { template struct rotx_mat_ { BOOST_QVM_INLINE_TRIVIAL rotx_mat_() { } template BOOST_QVM_INLINE_TRIVIAL operator R() const { R r; assign(r,*this); return r; } private: rotx_mat_( rotx_mat_ const & ); rotx_mat_ & operator=( rotx_mat_ const & ); ~rotx_mat_(); }; template struct rotx_m_get { template static BOOST_QVM_INLINE_CRITICAL T get( T const & ) { return scalar_traits::value(Row==Col); } }; template <> struct rotx_m_get<1,1> { template static BOOST_QVM_INLINE_CRITICAL T get( T const & angle ) { return cos(angle); } }; template <> struct rotx_m_get<1,2> { template static BOOST_QVM_INLINE_CRITICAL T get( T const & angle ) { return -sin(angle); } }; template <> struct rotx_m_get<2,1> { template static BOOST_QVM_INLINE_CRITICAL T get( T const & angle ) { return sin(angle); } }; template <> struct rotx_m_get<2,2> { template static BOOST_QVM_INLINE_CRITICAL T get( T const & angle ) { return cos(angle); } }; } template struct mat_traits< qvm_detail::rotx_mat_ > { typedef qvm_detail::rotx_mat_ this_matrix; typedef Angle scalar_type; static int const rows=Dim; static int const cols=Dim; template static BOOST_QVM_INLINE_CRITICAL scalar_type read_element( this_matrix const & x ) { BOOST_QVM_STATIC_ASSERT(Row>=0); BOOST_QVM_STATIC_ASSERT(Col>=0); BOOST_QVM_STATIC_ASSERT(Row::get(reinterpret_cast(x)); } static BOOST_QVM_INLINE_CRITICAL scalar_type read_element_idx( int row, int col, this_matrix const & x ) { BOOST_QVM_ASSERT(row>=0); BOOST_QVM_ASSERT(col>=0); BOOST_QVM_ASSERT(row(x); if( row==1 ) { if( col==1 ) return cos(a); if( col==2 ) return -sin(a); } if( row==2 ) { if( col==1 ) return sin(a); if( col==2 ) return cos(a); } return scalar_traits::value(row==col); } }; template struct deduce_mat,Dim,Dim> { typedef mat type; }; template struct deduce_mat2,qvm_detail::rotx_mat_,Dim,Dim> { typedef mat type; }; template BOOST_QVM_INLINE_TRIVIAL qvm_detail::rotx_mat_ const & rotx_mat( Angle const & angle ) { BOOST_QVM_STATIC_ASSERT(Dim>=3); return reinterpret_cast const &>(angle); } template BOOST_QVM_INLINE_OPERATIONS typename enable_if_c< is_mat::value && mat_traits::rows>=3 && mat_traits::rows==mat_traits::cols, void>::type set_rotx( A & a, Angle angle ) { assign(a,rotx_mat::rows>(angle)); } template BOOST_QVM_INLINE_OPERATIONS typename enable_if_c< is_mat::value && mat_traits::rows>=3 && mat_traits::rows==mat_traits::cols, void>::type rotate_x( A & a, Angle angle ) { a *= rotx_mat::rows>(angle); } //////////////////////////////////////////////// namespace qvm_detail { template struct roty_mat_ { BOOST_QVM_INLINE_TRIVIAL roty_mat_() { } template BOOST_QVM_INLINE_TRIVIAL operator R() const { R r; assign(r,*this); return r; } private: roty_mat_( roty_mat_ const & ); roty_mat_ & operator=( roty_mat_ const & ); ~roty_mat_(); }; template struct roty_m_get { template static BOOST_QVM_INLINE_CRITICAL T get( T const & ) { return scalar_traits::value(Row==Col); } }; template <> struct roty_m_get<0,0> { template static BOOST_QVM_INLINE_CRITICAL T get( T const & angle ) { return cos(angle); } }; template <> struct roty_m_get<0,2> { template static BOOST_QVM_INLINE_CRITICAL T get( T const & angle ) { return sin(angle); } }; template <> struct roty_m_get<2,0> { template static BOOST_QVM_INLINE_CRITICAL T get( T const & angle ) { return -sin(angle); } }; template <> struct roty_m_get<2,2> { template static BOOST_QVM_INLINE_CRITICAL T get( T const & angle ) { return cos(angle); } }; } template struct mat_traits< qvm_detail::roty_mat_ > { typedef qvm_detail::roty_mat_ this_matrix; typedef Angle scalar_type; static int const rows=Dim; static int const cols=Dim; template static BOOST_QVM_INLINE_CRITICAL scalar_type read_element( this_matrix const & x ) { BOOST_QVM_STATIC_ASSERT(Row>=0); BOOST_QVM_STATIC_ASSERT(Col>=0); BOOST_QVM_STATIC_ASSERT(Row::get(reinterpret_cast(x)); } static BOOST_QVM_INLINE_CRITICAL scalar_type read_element_idx( int row, int col, this_matrix const & x ) { BOOST_QVM_ASSERT(row>=0); BOOST_QVM_ASSERT(col>=0); BOOST_QVM_ASSERT(row(x); if( row==0 ) { if( col==0 ) return cos(a); if( col==2 ) return sin(a); } if( row==2 ) { if( col==0 ) return -sin(a); if( col==2 ) return cos(a); } return scalar_traits::value(row==col); } }; template struct deduce_mat,Dim,Dim> { typedef mat type; }; template struct deduce_mat2,qvm_detail::roty_mat_,Dim,Dim> { typedef mat type; }; template BOOST_QVM_INLINE_TRIVIAL qvm_detail::roty_mat_ const & roty_mat( Angle const & angle ) { BOOST_QVM_STATIC_ASSERT(Dim>=3); return reinterpret_cast const &>(angle); } template BOOST_QVM_INLINE_OPERATIONS typename enable_if_c< is_mat::value && mat_traits::rows>=2 && mat_traits::rows==mat_traits::cols, void>::type set_roty( A & a, Angle angle ) { assign(a,roty_mat::rows>(angle)); } template BOOST_QVM_INLINE_OPERATIONS typename enable_if_c< is_mat::value && mat_traits::rows>=3 && mat_traits::rows==mat_traits::cols, void>::type rotate_y( A & a, Angle angle ) { a *= roty_mat::rows>(angle); } //////////////////////////////////////////////// namespace qvm_detail { template struct rotz_mat_ { BOOST_QVM_INLINE_TRIVIAL rotz_mat_() { } template BOOST_QVM_INLINE_TRIVIAL operator R() const { R r; assign(r,*this); return r; } private: rotz_mat_( rotz_mat_ const & ); rotz_mat_ & operator=( rotz_mat_ const & ); ~rotz_mat_(); }; template struct rotz_m_get { template static BOOST_QVM_INLINE_CRITICAL T get( T const & ) { return scalar_traits::value(Row==Col); } }; template <> struct rotz_m_get<0,0> { template static BOOST_QVM_INLINE_CRITICAL T get( T const & angle ) { return cos(angle); } }; template <> struct rotz_m_get<0,1> { template static BOOST_QVM_INLINE_CRITICAL T get( T const & angle ) { return -sin(angle); } }; template <> struct rotz_m_get<1,0> { template static BOOST_QVM_INLINE_CRITICAL T get( T const & angle ) { return sin(angle); } }; template <> struct rotz_m_get<1,1> { template static BOOST_QVM_INLINE_CRITICAL T get( T const & angle ) { return cos(angle); } }; } template struct mat_traits< qvm_detail::rotz_mat_ > { typedef qvm_detail::rotz_mat_ this_matrix; typedef Angle scalar_type; static int const rows=Dim; static int const cols=Dim; template static BOOST_QVM_INLINE_CRITICAL scalar_type read_element( this_matrix const & x ) { BOOST_QVM_STATIC_ASSERT(Row>=0); BOOST_QVM_STATIC_ASSERT(Col>=0); BOOST_QVM_STATIC_ASSERT(Row::get(reinterpret_cast(x)); } static BOOST_QVM_INLINE_CRITICAL scalar_type read_element_idx( int row, int col, this_matrix const & x ) { BOOST_QVM_ASSERT(row>=0); BOOST_QVM_ASSERT(col>=0); BOOST_QVM_ASSERT(row(x); if( row==0 ) { if( col==0 ) return cos(a); if( col==1 ) return -sin(a); } if( row==1 ) { if( col==0 ) return sin(a); if( col==1 ) return cos(a); } return scalar_traits::value(row==col); } }; template struct deduce_mat,Dim,Dim> { typedef mat type; }; template struct deduce_mat2,qvm_detail::rotz_mat_,R,C> { typedef mat type; }; template BOOST_QVM_INLINE_TRIVIAL qvm_detail::rotz_mat_ const & rotz_mat( Angle const & angle ) { BOOST_QVM_STATIC_ASSERT(Dim>=2); return reinterpret_cast const &>(angle); } template BOOST_QVM_INLINE_OPERATIONS typename enable_if_c< is_mat::value && mat_traits::rows>=2 && mat_traits::rows==mat_traits::cols, void>::type set_rotz( A & a, Angle angle ) { assign(a,rotz_mat::rows>(angle)); } template BOOST_QVM_INLINE_OPERATIONS typename enable_if_c< is_mat::value && mat_traits::rows>=2 && mat_traits::rows==mat_traits::cols, void>::type rotate_z( A & a, Angle angle ) { a *= rotz_mat::rows>(angle); } //////////////////////////////////////////////// namespace qvm_detail { template struct inverse_m_defined { static bool const value=false; }; } template BOOST_QVM_INLINE_TRIVIAL typename lazy_enable_if_c< is_mat::value && is_scalar::value && mat_traits::rows==mat_traits::cols && !qvm_detail::inverse_m_defined::rows>::value, deduce_mat2::rows,mat_traits::cols> >::type inverse( A const & a, B det ) { typedef typename mat_traits::scalar_type T; BOOST_QVM_ASSERT(det!=scalar_traits::value(0)); T f=scalar_traits::value(1)/det; typedef typename deduce_mat2::rows,mat_traits::cols>::type cofactor_return_type; cofactor_return_type c=qvm_detail::cofactor_impl(a); return reinterpret_cast const &>(c) * f; } template BOOST_QVM_INLINE_TRIVIAL typename lazy_enable_if_c< is_mat::value && mat_traits::rows==mat_traits::cols && !qvm_detail::inverse_m_defined::rows>::value, deduce_mat >::type inverse( A const & a ) { typedef typename mat_traits::scalar_type T; T det=determinant(a); if( det==scalar_traits::value(0) ) BOOST_QVM_THROW_EXCEPTION(zero_determinant_error()); return inverse(a,det); } //////////////////////////////////////////////// namespace sfinae { using ::boost::qvm::to_string; using ::boost::qvm::assign; using ::boost::qvm::determinant; using ::boost::qvm::cmp; using ::boost::qvm::convert_to; using ::boost::qvm::set_identity; using ::boost::qvm::set_zero; using ::boost::qvm::scalar_cast; using ::boost::qvm::operator/=; using ::boost::qvm::operator/; using ::boost::qvm::operator==; using ::boost::qvm::operator-=; using ::boost::qvm::operator-; using ::boost::qvm::operator*=; using ::boost::qvm::operator*; using ::boost::qvm::operator!=; using ::boost::qvm::operator+=; using ::boost::qvm::operator+; using ::boost::qvm::mref; using ::boost::qvm::rot_mat; using ::boost::qvm::set_rot; using ::boost::qvm::rotate; using ::boost::qvm::set_rotx; using ::boost::qvm::rotate_x; using ::boost::qvm::set_roty; using ::boost::qvm::rotate_y; using ::boost::qvm::set_rotz; using ::boost::qvm::rotate_z; using ::boost::qvm::inverse; } } } #endif