1 //Copyright (c) 2008-2016 Emil Dotchevski and Reverge Studios, Inc. 2 3 //Distributed under the Boost Software License, Version 1.0. (See accompanying 4 //file LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt) 5 6 #ifndef UUID_4F915D9ED30A11DF962186E3DFD72085 7 #define UUID_4F915D9ED30A11DF962186E3DFD72085 8 9 #include <boost/qvm/detail/mat_assign.hpp> 10 #include <boost/qvm/mat_operations2.hpp> 11 #include <boost/qvm/mat_operations3.hpp> 12 #include <boost/qvm/mat_operations4.hpp> 13 #include <boost/qvm/math.hpp> 14 #include <boost/qvm/detail/determinant_impl.hpp> 15 #include <boost/qvm/detail/cofactor_impl.hpp> 16 #include <boost/qvm/detail/transp_impl.hpp> 17 #include <boost/qvm/scalar_traits.hpp> 18 #include <string> 19 20 namespace 21 boost 22 { 23 namespace 24 qvm 25 { 26 namespace 27 qvm_detail 28 { 29 BOOST_QVM_INLINE_CRITICAL 30 void const * get_valid_ptr_mat_operations()31 get_valid_ptr_mat_operations() 32 { 33 static int const obj=0; 34 return &obj; 35 } 36 } 37 38 //////////////////////////////////////////////// 39 40 namespace 41 qvm_to_string_detail 42 { 43 template <class T> 44 std::string to_string( T const & x ); 45 } 46 47 namespace 48 qvm_detail 49 { 50 template <int R,int C> 51 struct 52 to_string_m_defined 53 { 54 static bool const value=false; 55 }; 56 57 template <int I,int SizeMinusOne> 58 struct 59 to_string_matrix_elements 60 { 61 template <class A> 62 static 63 std::string fboost::qvm::qvm_detail::to_string_matrix_elements64 f( A const & a ) 65 { 66 using namespace qvm_to_string_detail; 67 return 68 ( (I%mat_traits<A>::cols)==0 ? '(' : ',' ) + 69 to_string(mat_traits<A>::template read_element<I/mat_traits<A>::cols,I%mat_traits<A>::cols>(a)) + 70 ( (I%mat_traits<A>::cols)==mat_traits<A>::cols-1 ? ")" : "" ) + 71 to_string_matrix_elements<I+1,SizeMinusOne>::f(a); 72 } 73 }; 74 75 template <int SizeMinusOne> 76 struct 77 to_string_matrix_elements<SizeMinusOne,SizeMinusOne> 78 { 79 template <class A> 80 static 81 std::string fboost::qvm::qvm_detail::to_string_matrix_elements82 f( A const & a ) 83 { 84 using namespace qvm_to_string_detail; 85 return 86 ( (SizeMinusOne%mat_traits<A>::cols)==0 ? '(' : ',' ) + 87 to_string(mat_traits<A>::template read_element<SizeMinusOne/mat_traits<A>::cols,SizeMinusOne%mat_traits<A>::cols>(a)) + 88 ')'; 89 } 90 }; 91 } 92 93 template <class A> 94 inline 95 typename boost::enable_if_c< 96 is_mat<A>::value && 97 !qvm_detail::to_string_m_defined<mat_traits<A>::rows,mat_traits<A>::cols>::value, 98 std::string>::type to_string(A const & a)99 to_string( A const & a ) 100 { 101 return "("+qvm_detail::to_string_matrix_elements<0,mat_traits<A>::rows*mat_traits<A>::cols-1>::f(a)+')'; 102 } 103 104 //////////////////////////////////////////////// 105 106 template <class A,class B,class Cmp> 107 BOOST_QVM_INLINE_OPERATIONS 108 typename enable_if_c< 109 is_mat<A>::value && is_mat<B>::value && 110 mat_traits<A>::rows==mat_traits<B>::rows && 111 mat_traits<A>::cols==mat_traits<B>::cols, 112 bool>::type cmp(A const & a,B const & b,Cmp f)113 cmp( A const & a, B const & b, Cmp f ) 114 { 115 typedef typename deduce_scalar< 116 typename mat_traits<A>::scalar_type, 117 typename mat_traits<B>::scalar_type>::type T; 118 int const rows=mat_traits<A>::rows; 119 int const cols=mat_traits<A>::cols; 120 T m1[rows][cols]; assign(m1,a); 121 T m2[rows][cols]; assign(m2,b); 122 for( int i=0; i!=rows; ++i ) 123 for( int j=0; j!=cols; ++j ) 124 if( !f(m1[i][j],m2[i][j]) ) 125 return false; 126 return true; 127 } 128 129 //////////////////////////////////////////////// 130 131 namespace 132 qvm_detail 133 { 134 template <int M,int N> 135 struct 136 convert_to_m_defined 137 { 138 static bool const value=false; 139 }; 140 } 141 142 template <class R,class A> 143 BOOST_QVM_INLINE_TRIVIAL 144 typename enable_if_c< 145 is_mat<R>::value && is_mat<A>::value && 146 mat_traits<R>::rows==mat_traits<A>::rows && 147 mat_traits<R>::cols==mat_traits<A>::cols && 148 !qvm_detail::convert_to_m_defined<mat_traits<A>::rows,mat_traits<A>::cols>::value, 149 R>::type convert_to(A const & a)150 convert_to( A const & a ) 151 { 152 R r; assign(r,a); 153 return r; 154 } 155 156 //////////////////////////////////////////////// 157 158 namespace 159 qvm_detail 160 { 161 template <int D> 162 struct 163 determinant_defined 164 { 165 static bool const value=false; 166 }; 167 } 168 169 template <class A> 170 BOOST_QVM_INLINE_TRIVIAL 171 typename enable_if_c< 172 is_mat<A>::value && 173 mat_traits<A>::rows==mat_traits<A>::cols && 174 !qvm_detail::determinant_defined<mat_traits<A>::rows>::value, 175 typename mat_traits<A>::scalar_type>::type determinant(A const & a)176 determinant( A const & a ) 177 { 178 return qvm_detail::determinant_impl(a); 179 } 180 181 //////////////////////////////////////////////// 182 183 namespace 184 qvm_detail 185 { 186 template <class T,int Dim> 187 class 188 identity_mat_ 189 { 190 identity_mat_( identity_mat_ const & ); 191 identity_mat_ & operator=( identity_mat_ const & ); 192 ~identity_mat_(); 193 194 public: 195 196 template <class R> 197 BOOST_QVM_INLINE_TRIVIAL operator R() const198 operator R() const 199 { 200 R r; 201 assign(r,*this); 202 return r; 203 } 204 }; 205 } 206 207 template <class T,int Dim> 208 struct 209 mat_traits< qvm_detail::identity_mat_<T,Dim> > 210 { 211 typedef qvm_detail::identity_mat_<T,Dim> this_matrix; 212 typedef T scalar_type; 213 static int const rows=Dim; 214 static int const cols=Dim; 215 216 template <int Row,int Col> 217 static 218 BOOST_QVM_INLINE_CRITICAL 219 scalar_type read_elementboost::qvm::mat_traits220 read_element( this_matrix const & x ) 221 { 222 BOOST_QVM_STATIC_ASSERT(Row>=0); 223 BOOST_QVM_STATIC_ASSERT(Row<Dim); 224 BOOST_QVM_STATIC_ASSERT(Col>=0); 225 BOOST_QVM_STATIC_ASSERT(Col<Dim); 226 return scalar_traits<scalar_type>::value(Row==Col); 227 } 228 229 static 230 BOOST_QVM_INLINE_CRITICAL 231 scalar_type read_element_idxboost::qvm::mat_traits232 read_element_idx( int row, int col, this_matrix const & x ) 233 { 234 BOOST_QVM_ASSERT(row>=0); 235 BOOST_QVM_ASSERT(row<Dim); 236 BOOST_QVM_ASSERT(col>=0); 237 BOOST_QVM_ASSERT(col<Dim); 238 return scalar_traits<scalar_type>::value(row==col); 239 } 240 }; 241 242 template <class T,int Dim> 243 BOOST_QVM_INLINE_TRIVIAL 244 qvm_detail::identity_mat_<T,Dim> const & identity_mat()245 identity_mat() 246 { 247 return *(qvm_detail::identity_mat_<T,Dim> const *)qvm_detail::get_valid_ptr_mat_operations(); 248 } 249 250 template <class A> 251 BOOST_QVM_INLINE_OPERATIONS 252 typename enable_if_c< 253 is_mat<A>::value && 254 mat_traits<A>::rows==mat_traits<A>::cols, 255 void>::type set_identity(A & a)256 set_identity( A & a ) 257 { 258 assign(a,identity_mat<typename mat_traits<A>::scalar_type,mat_traits<A>::rows>()); 259 } 260 261 //////////////////////////////////////////////// 262 263 namespace 264 qvm_detail 265 { 266 template <class T> 267 struct 268 projection_ 269 { 270 T const _00; 271 T const _11; 272 T const _22; 273 T const _23; 274 T const _32; 275 276 BOOST_QVM_INLINE_TRIVIAL projection_boost::qvm::qvm_detail::projection_277 projection_( T _00, T _11, T _22, T _23, T _32 ): 278 _00(_00), 279 _11(_11), 280 _22(_22), 281 _23(_23), 282 _32(_32) 283 { 284 } 285 286 template <class R> 287 BOOST_QVM_INLINE_TRIVIAL operator Rboost::qvm::qvm_detail::projection_288 operator R() const 289 { 290 R r; 291 assign(r,*this); 292 return r; 293 } 294 }; 295 296 template <int Row,int Col> 297 struct 298 projection_get 299 { 300 template <class T> 301 static 302 BOOST_QVM_INLINE_CRITICAL 303 T getboost::qvm::qvm_detail::projection_get304 get( projection_<T> const & ) 305 { 306 return scalar_traits<T>::value(0); 307 } 308 }; 309 getboost::qvm::qvm_detail::projection_get310 template <> struct projection_get<0,0> { template <class T> static BOOST_QVM_INLINE_CRITICAL T get( projection_<T> const & m ) { return m._00; } }; getboost::qvm::qvm_detail::projection_get311 template <> struct projection_get<1,1> { template <class T> static BOOST_QVM_INLINE_CRITICAL T get( projection_<T> const & m ) { return m._11; } }; getboost::qvm::qvm_detail::projection_get312 template <> struct projection_get<2,2> { template <class T> static BOOST_QVM_INLINE_CRITICAL T get( projection_<T> const & m ) { return m._22; } }; getboost::qvm::qvm_detail::projection_get313 template <> struct projection_get<2,3> { template <class T> static BOOST_QVM_INLINE_CRITICAL T get( projection_<T> const & m ) { return m._23; } }; getboost::qvm::qvm_detail::projection_get314 template <> struct projection_get<3,2> { template <class T> static BOOST_QVM_INLINE_CRITICAL T get( projection_<T> const & m ) { return m._32; } }; 315 } 316 317 template <class T> 318 struct 319 mat_traits< qvm_detail::projection_<T> > 320 { 321 typedef qvm_detail::projection_<T> this_matrix; 322 typedef T scalar_type; 323 static int const rows=4; 324 static int const cols=4; 325 326 template <int Row,int Col> 327 static 328 BOOST_QVM_INLINE_CRITICAL 329 scalar_type read_elementboost::qvm::mat_traits330 read_element( this_matrix const & x ) 331 { 332 BOOST_QVM_STATIC_ASSERT(Row>=0); 333 BOOST_QVM_STATIC_ASSERT(Row<rows); 334 BOOST_QVM_STATIC_ASSERT(Col>=0); 335 BOOST_QVM_STATIC_ASSERT(Col<cols); 336 return qvm_detail::projection_get<Row,Col>::get(x); 337 } 338 }; 339 340 template <class T> 341 qvm_detail::projection_<T> 342 BOOST_QVM_INLINE_OPERATIONS perspective_lh(T fov_y,T aspect_ratio,T z_near,T z_far)343 perspective_lh( T fov_y, T aspect_ratio, T z_near, T z_far ) 344 { 345 T const one = scalar_traits<T>::value(1); 346 T const ys = one/tan<T>(fov_y/scalar_traits<T>::value(2)); 347 T const xs = ys/aspect_ratio; 348 T const zd = z_far-z_near; 349 T const z1 = z_far/zd; 350 T const z2 = -z_near*z1; 351 return qvm_detail::projection_<T>(xs,ys,z1,z2,one); 352 } 353 354 template <class T> 355 qvm_detail::projection_<T> 356 BOOST_QVM_INLINE_OPERATIONS perspective_rh(T fov_y,T aspect_ratio,T z_near,T z_far)357 perspective_rh( T fov_y, T aspect_ratio, T z_near, T z_far ) 358 { 359 T const one = scalar_traits<T>::value(1); 360 T const ys = one/tan<T>(fov_y/scalar_traits<T>::value(2)); 361 T const xs = ys/aspect_ratio; 362 T const zd = z_near-z_far; 363 T const z1 = z_far/zd; 364 T const z2 = z_near*z1; 365 return qvm_detail::projection_<T>(xs,ys,z1,z2,-one); 366 } 367 368 //////////////////////////////////////////////// 369 370 namespace 371 qvm_detail 372 { 373 template <class OriginalType,class Scalar> 374 class 375 matrix_scalar_cast_ 376 { 377 matrix_scalar_cast_( matrix_scalar_cast_ const & ); 378 matrix_scalar_cast_ & operator=( matrix_scalar_cast_ const & ); 379 ~matrix_scalar_cast_(); 380 381 public: 382 383 template <class T> 384 BOOST_QVM_INLINE_TRIVIAL 385 matrix_scalar_cast_ & operator =(T const & x)386 operator=( T const & x ) 387 { 388 assign(*this,x); 389 return *this; 390 } 391 392 template <class R> 393 BOOST_QVM_INLINE_TRIVIAL operator R() const394 operator R() const 395 { 396 R r; 397 assign(r,*this); 398 return r; 399 } 400 }; 401 402 template <bool> struct scalar_cast_matrix_filter { }; 403 template <> struct scalar_cast_matrix_filter<true> { typedef int type; }; 404 } 405 406 template <class OriginalType,class Scalar> 407 struct 408 mat_traits< qvm_detail::matrix_scalar_cast_<OriginalType,Scalar> > 409 { 410 typedef Scalar scalar_type; 411 typedef qvm_detail::matrix_scalar_cast_<OriginalType,Scalar> this_matrix; 412 static int const rows=mat_traits<OriginalType>::rows; 413 static int const cols=mat_traits<OriginalType>::cols; 414 415 template <int Row,int Col> 416 static 417 BOOST_QVM_INLINE_CRITICAL 418 scalar_type read_elementboost::qvm::mat_traits419 read_element( this_matrix const & x ) 420 { 421 BOOST_QVM_STATIC_ASSERT(Row>=0); 422 BOOST_QVM_STATIC_ASSERT(Row<rows); 423 BOOST_QVM_STATIC_ASSERT(Col>=0); 424 BOOST_QVM_STATIC_ASSERT(Col<cols); 425 return scalar_type(mat_traits<OriginalType>::template read_element<Row,Col>(reinterpret_cast<OriginalType const &>(x))); 426 } 427 428 static 429 BOOST_QVM_INLINE_CRITICAL 430 scalar_type read_element_idxboost::qvm::mat_traits431 read_element_idx( int row, int col, this_matrix const & x ) 432 { 433 BOOST_QVM_ASSERT(row>=0); 434 BOOST_QVM_ASSERT(row<rows); 435 BOOST_QVM_ASSERT(col>=0); 436 BOOST_QVM_ASSERT(col<cols); 437 return scalar_type(mat_traits<OriginalType>::read_element_idx(col,row,reinterpret_cast<OriginalType const &>(x))); 438 } 439 }; 440 441 template <class OriginalType,class Scalar,int R,int C> 442 struct 443 deduce_mat<qvm_detail::matrix_scalar_cast_<OriginalType,Scalar>,R,C> 444 { 445 typedef mat<Scalar,R,C> type; 446 }; 447 448 template <class Scalar,class T> 449 BOOST_QVM_INLINE_TRIVIAL 450 qvm_detail::matrix_scalar_cast_<T,Scalar> const & scalar_cast(T const & x,typename qvm_detail::scalar_cast_matrix_filter<is_mat<T>::value>::type=0)451 scalar_cast( T const & x, typename qvm_detail::scalar_cast_matrix_filter<is_mat<T>::value>::type=0 ) 452 { 453 return reinterpret_cast<qvm_detail::matrix_scalar_cast_<T,Scalar> const &>(x); 454 } 455 456 //////////////////////////////////////////////// 457 458 namespace 459 qvm_detail 460 { 461 template <int M,int N> 462 struct 463 div_eq_ms_defined 464 { 465 static bool const value=false; 466 }; 467 } 468 469 template <class A,class B> 470 BOOST_QVM_INLINE_OPERATIONS 471 typename enable_if_c< 472 is_mat<A>::value && is_scalar<B>::value && 473 !qvm_detail::div_eq_ms_defined<mat_traits<A>::rows,mat_traits<A>::cols>::value, 474 A &>::type operator /=(A & a,B b)475 operator/=( A & a, B b ) 476 { 477 for( int i=0; i!=mat_traits<A>::rows; ++i ) 478 for( int j=0; j!=mat_traits<A>::cols; ++j ) 479 mat_traits<A>::write_element_idx(i,j,a)/=b; 480 return a; 481 } 482 483 //////////////////////////////////////////////// 484 485 namespace 486 qvm_detail 487 { 488 template <int M,int N> 489 struct 490 div_ms_defined 491 { 492 static bool const value=false; 493 }; 494 } 495 496 template <class A,class B> 497 BOOST_QVM_INLINE_OPERATIONS 498 typename lazy_enable_if_c< 499 is_mat<A>::value && is_scalar<B>::value && 500 !qvm_detail::div_ms_defined<mat_traits<A>::rows,mat_traits<A>::cols>::value, 501 deduce_mat<A> >::type operator /(A const & a,B b)502 operator/( A const & a, B b ) 503 { 504 typedef typename deduce_mat<A>::type R; 505 R r; 506 for( int i=0; i!=mat_traits<A>::rows; ++i ) 507 for( int j=0; j!=mat_traits<A>::cols; ++j ) 508 mat_traits<R>::write_element_idx(i,j,r)=mat_traits<A>::read_element_idx(i,j,a)/b; 509 return r; 510 } 511 512 //////////////////////////////////////////////// 513 514 namespace 515 qvm_detail 516 { 517 template <int M,int N> 518 struct 519 eq_mm_defined 520 { 521 static bool const value=false; 522 }; 523 } 524 525 template <class A,class B> 526 BOOST_QVM_INLINE_OPERATIONS 527 typename enable_if_c< 528 is_mat<A>::value && is_mat<B>::value && 529 mat_traits<A>::rows==mat_traits<B>::rows && 530 mat_traits<A>::cols==mat_traits<B>::cols && 531 !qvm_detail::eq_mm_defined<mat_traits<A>::rows,mat_traits<A>::cols>::value, 532 bool>::type operator ==(A const & a,B const & b)533 operator==( A const & a, B const & b ) 534 { 535 for( int i=0; i!=mat_traits<A>::rows; ++i ) 536 for( int j=0; j!=mat_traits<A>::cols; ++j ) 537 if( mat_traits<A>::read_element_idx(i,j,a)!=mat_traits<B>::read_element_idx(i,j,b) ) 538 return false; 539 return true; 540 } 541 542 //////////////////////////////////////////////// 543 544 namespace 545 qvm_detail 546 { 547 template <int M,int N> 548 struct 549 minus_eq_mm_defined 550 { 551 static bool const value=false; 552 }; 553 } 554 555 template <class A,class B> 556 BOOST_QVM_INLINE_OPERATIONS 557 typename enable_if_c< 558 is_mat<A>::value && is_mat<B>::value && 559 mat_traits<A>::rows==mat_traits<B>::rows && 560 mat_traits<A>::cols==mat_traits<B>::cols && 561 !qvm_detail::minus_eq_mm_defined<mat_traits<A>::rows,mat_traits<A>::cols>::value, 562 A &>::type operator -=(A & a,B const & b)563 operator-=( A & a, B const & b ) 564 { 565 for( int i=0; i!=mat_traits<A>::rows; ++i ) 566 for( int j=0; j!=mat_traits<A>::cols; ++j ) 567 mat_traits<A>::write_element_idx(i,j,a)-=mat_traits<B>::read_element_idx(i,j,b); 568 return a; 569 } 570 571 //////////////////////////////////////////////// 572 573 namespace 574 qvm_detail 575 { 576 template <int M,int N> 577 struct 578 minus_m_defined 579 { 580 static bool const value=false; 581 }; 582 } 583 584 template <class A> 585 BOOST_QVM_INLINE_OPERATIONS 586 typename lazy_enable_if_c< 587 is_mat<A>::value && 588 !qvm_detail::minus_m_defined<mat_traits<A>::rows,mat_traits<A>::cols>::value, 589 deduce_mat<A> >::type operator -(A const & a)590 operator-( A const & a ) 591 { 592 typedef typename deduce_mat<A>::type R; 593 R r; 594 for( int i=0; i!=mat_traits<A>::rows; ++i ) 595 for( int j=0; j!=mat_traits<A>::cols; ++j ) 596 mat_traits<R>::write_element_idx(i,j,r)=-mat_traits<A>::read_element_idx(i,j,a); 597 return r; 598 } 599 600 //////////////////////////////////////////////// 601 602 namespace 603 qvm_detail 604 { 605 template <int M,int N> 606 struct 607 minus_mm_defined 608 { 609 static bool const value=false; 610 }; 611 } 612 613 template <class A,class B> 614 BOOST_QVM_INLINE_OPERATIONS 615 typename lazy_enable_if_c< 616 is_mat<A>::value && is_mat<B>::value && 617 mat_traits<A>::rows==mat_traits<B>::rows && 618 mat_traits<A>::cols==mat_traits<B>::cols && 619 !qvm_detail::minus_mm_defined<mat_traits<A>::rows,mat_traits<A>::cols>::value, 620 deduce_mat2<A,B,mat_traits<A>::rows,mat_traits<A>::cols> >::type operator -(A const & a,B const & b)621 operator-( A const & a, B const & b ) 622 { 623 typedef typename deduce_mat2<A,B,mat_traits<A>::rows,mat_traits<A>::cols>::type R; 624 R r; 625 for( int i=0; i!=mat_traits<A>::rows; ++i ) 626 for( int j=0; j!=mat_traits<A>::cols; ++j ) 627 mat_traits<R>::write_element_idx(i,j,r)=mat_traits<A>::read_element_idx(i,j,a)-mat_traits<B>::read_element_idx(i,j,b); 628 return r; 629 } 630 631 //////////////////////////////////////////////// 632 633 namespace 634 qvm_detail 635 { 636 template <int D> 637 struct 638 mul_eq_mm_defined 639 { 640 static bool const value=false; 641 }; 642 } 643 644 template <class A,class B> 645 BOOST_QVM_INLINE_OPERATIONS 646 typename enable_if_c< 647 is_mat<A>::value && 648 is_mat<B>::value && 649 mat_traits<A>::rows==mat_traits<A>::cols && 650 mat_traits<A>::rows==mat_traits<B>::rows && 651 mat_traits<A>::cols==mat_traits<B>::cols && 652 !qvm_detail::mul_eq_mm_defined<mat_traits<A>::rows>::value, 653 A &>::type operator *=(A & r,B const & b)654 operator*=( A & r, B const & b ) 655 { 656 typedef typename mat_traits<A>::scalar_type Ta; 657 Ta a[mat_traits<A>::rows][mat_traits<A>::cols]; 658 for( int i=0; i<mat_traits<A>::rows; ++i ) 659 for( int j=0; j<mat_traits<B>::cols; ++j ) 660 a[i][j]=mat_traits<A>::read_element_idx(i,j,r); 661 for( int i=0; i<mat_traits<A>::rows; ++i ) 662 for( int j=0; j<mat_traits<B>::cols; ++j ) 663 { 664 Ta x(scalar_traits<Ta>::value(0)); 665 for( int k=0; k<mat_traits<A>::cols; ++k ) 666 x += a[i][k]*mat_traits<B>::read_element_idx(k,j,b); 667 mat_traits<A>::write_element_idx(i,j,r) = x; 668 } 669 return r; 670 } 671 672 //////////////////////////////////////////////// 673 674 namespace 675 qvm_detail 676 { 677 template <int M,int N> 678 struct 679 mul_eq_ms_defined 680 { 681 static bool const value=false; 682 }; 683 } 684 685 template <class A,class B> 686 BOOST_QVM_INLINE_OPERATIONS 687 typename enable_if_c< 688 is_mat<A>::value && is_scalar<B>::value && 689 !qvm_detail::mul_eq_ms_defined<mat_traits<A>::rows,mat_traits<A>::cols>::value, 690 A &>::type operator *=(A & a,B b)691 operator*=( A & a, B b ) 692 { 693 for( int i=0; i!=mat_traits<A>::rows; ++i ) 694 for( int j=0; j!=mat_traits<A>::cols; ++j ) 695 mat_traits<A>::write_element_idx(i,j,a)*=b; 696 return a; 697 } 698 699 //////////////////////////////////////////////// 700 701 namespace 702 qvm_detail 703 { 704 template <int R,int CR,int C> 705 struct 706 mul_mm_defined 707 { 708 static bool const value=false; 709 }; 710 } 711 712 template <class A,class B> 713 BOOST_QVM_INLINE_OPERATIONS 714 typename lazy_enable_if_c< 715 is_mat<A>::value && is_mat<B>::value && 716 mat_traits<A>::cols==mat_traits<B>::rows && 717 !qvm_detail::mul_mm_defined<mat_traits<A>::rows,mat_traits<A>::cols,mat_traits<B>::cols>::value, 718 deduce_mat2<A,B,mat_traits<A>::rows,mat_traits<B>::cols> >::type operator *(A const & a,B const & b)719 operator*( A const & a, B const & b ) 720 { 721 typedef typename deduce_mat2<A,B,mat_traits<A>::rows,mat_traits<B>::cols>::type R; 722 R r; 723 for( int i=0; i<mat_traits<A>::rows; ++i ) 724 for( int j=0; j<mat_traits<B>::cols; ++j ) 725 { 726 typedef typename mat_traits<A>::scalar_type Ta; 727 Ta x(scalar_traits<Ta>::value(0)); 728 for( int k=0; k<mat_traits<A>::cols; ++k ) 729 x += mat_traits<A>::read_element_idx(i,k,a)*mat_traits<B>::read_element_idx(k,j,b); 730 mat_traits<R>::write_element_idx(i,j,r) = x; 731 } 732 return r; 733 } 734 735 //////////////////////////////////////////////// 736 737 namespace 738 qvm_detail 739 { 740 template <int M,int N> 741 struct 742 mul_ms_defined 743 { 744 static bool const value=false; 745 }; 746 } 747 748 template <class A,class B> 749 BOOST_QVM_INLINE_OPERATIONS 750 typename lazy_enable_if_c< 751 is_mat<A>::value && is_scalar<B>::value && 752 !qvm_detail::mul_ms_defined<mat_traits<A>::rows,mat_traits<A>::cols>::value, 753 deduce_mat<A> >::type operator *(A const & a,B b)754 operator*( A const & a, B b ) 755 { 756 typedef typename deduce_mat<A>::type R; 757 R r; 758 for( int i=0; i!=mat_traits<A>::rows; ++i ) 759 for( int j=0; j!=mat_traits<A>::cols; ++j ) 760 mat_traits<R>::write_element_idx(i,j,r)=mat_traits<A>::read_element_idx(i,j,a)*b; 761 return r; 762 } 763 764 //////////////////////////////////////////////// 765 766 namespace 767 qvm_detail 768 { 769 template <int M,int N> 770 struct 771 mul_sm_defined 772 { 773 static bool const value=false; 774 }; 775 } 776 777 template <class A,class B> 778 BOOST_QVM_INLINE_OPERATIONS 779 typename lazy_enable_if_c< 780 is_scalar<A>::value && is_mat<B>::value && 781 !qvm_detail::mul_sm_defined<mat_traits<B>::rows,mat_traits<B>::cols>::value, 782 deduce_mat<B> >::type operator *(A a,B const & b)783 operator*( A a, B const & b ) 784 { 785 typedef typename deduce_mat<B>::type R; 786 R r; 787 for( int i=0; i!=mat_traits<B>::rows; ++i ) 788 for( int j=0; j!=mat_traits<B>::cols; ++j ) 789 mat_traits<R>::write_element_idx(i,j,r)=a*mat_traits<B>::read_element_idx(i,j,b); 790 return r; 791 } 792 793 //////////////////////////////////////////////// 794 795 namespace 796 qvm_detail 797 { 798 template <int M,int N> 799 struct 800 neq_mm_defined 801 { 802 static bool const value=false; 803 }; 804 } 805 806 template <class A,class B> 807 BOOST_QVM_INLINE_OPERATIONS 808 typename enable_if_c< 809 is_mat<A>::value && is_mat<B>::value && 810 mat_traits<A>::rows==mat_traits<B>::rows && 811 mat_traits<A>::cols==mat_traits<B>::cols && 812 !qvm_detail::neq_mm_defined<mat_traits<A>::rows,mat_traits<A>::cols>::value, 813 bool>::type operator !=(A const & a,B const & b)814 operator!=( A const & a, B const & b ) 815 { 816 for( int i=0; i!=mat_traits<A>::rows; ++i ) 817 for( int j=0; j!=mat_traits<A>::cols; ++j ) 818 if( mat_traits<A>::read_element_idx(i,j,a)!=mat_traits<B>::read_element_idx(i,j,b) ) 819 return true; 820 return false; 821 } 822 823 //////////////////////////////////////////////// 824 825 namespace 826 qvm_detail 827 { 828 template <int M,int N> 829 struct 830 plus_eq_mm_defined 831 { 832 static bool const value=false; 833 }; 834 } 835 836 template <class A,class B> 837 BOOST_QVM_INLINE_OPERATIONS 838 typename enable_if_c< 839 is_mat<A>::value && is_mat<B>::value && 840 mat_traits<A>::rows==mat_traits<B>::rows && 841 mat_traits<A>::cols==mat_traits<B>::cols && 842 !qvm_detail::plus_eq_mm_defined<mat_traits<A>::rows,mat_traits<A>::cols>::value, 843 A &>::type operator +=(A & a,B const & b)844 operator+=( A & a, B const & b ) 845 { 846 for( int i=0; i!=mat_traits<A>::rows; ++i ) 847 for( int j=0; j!=mat_traits<A>::cols; ++j ) 848 mat_traits<A>::write_element_idx(i,j,a)+=mat_traits<B>::read_element_idx(i,j,b); 849 return a; 850 } 851 852 //////////////////////////////////////////////// 853 854 namespace 855 qvm_detail 856 { 857 template <int M,int N> 858 struct 859 plus_mm_defined 860 { 861 static bool const value=false; 862 }; 863 } 864 865 template <class A,class B> 866 BOOST_QVM_INLINE_OPERATIONS 867 typename lazy_enable_if_c< 868 is_mat<A>::value && is_mat<B>::value && 869 mat_traits<A>::rows==mat_traits<B>::rows && 870 mat_traits<A>::cols==mat_traits<B>::cols && 871 !qvm_detail::plus_mm_defined<mat_traits<A>::rows,mat_traits<A>::cols>::value, 872 deduce_mat2<A,B,mat_traits<A>::rows,mat_traits<A>::cols> >::type operator +(A const & a,B const & b)873 operator+( A const & a, B const & b ) 874 { 875 typedef typename deduce_mat2<A,B,mat_traits<A>::rows,mat_traits<A>::cols>::type R; 876 R r; 877 for( int i=0; i!=mat_traits<A>::rows; ++i ) 878 for( int j=0; j!=mat_traits<A>::cols; ++j ) 879 mat_traits<R>::write_element_idx(i,j,r)=mat_traits<A>::read_element_idx(i,j,a)+mat_traits<B>::read_element_idx(i,j,b); 880 return r; 881 } 882 883 //////////////////////////////////////////////// 884 885 namespace 886 qvm_detail 887 { 888 template <class T> 889 class 890 mref_ 891 { 892 mref_( mref_ const & ); 893 mref_ & operator=( mref_ const & ); 894 ~mref_(); 895 896 public: 897 898 template <class R> 899 BOOST_QVM_INLINE_TRIVIAL 900 mref_ & operator =(R const & x)901 operator=( R const & x ) 902 { 903 assign(*this,x); 904 return *this; 905 } 906 907 template <class R> 908 BOOST_QVM_INLINE_TRIVIAL operator R() const909 operator R() const 910 { 911 R r; 912 assign(r,*this); 913 return r; 914 } 915 }; 916 } 917 918 template <class M> 919 struct 920 mat_traits< qvm_detail::mref_<M> > 921 { 922 typedef typename mat_traits<M>::scalar_type scalar_type; 923 typedef qvm_detail::mref_<M> this_matrix; 924 static int const rows=mat_traits<M>::rows; 925 static int const cols=mat_traits<M>::cols; 926 927 template <int Row,int Col> 928 static 929 BOOST_QVM_INLINE_CRITICAL 930 scalar_type read_elementboost::qvm::mat_traits931 read_element( this_matrix const & x ) 932 { 933 BOOST_QVM_STATIC_ASSERT(Row>=0); 934 BOOST_QVM_STATIC_ASSERT(Row<rows); 935 BOOST_QVM_STATIC_ASSERT(Col>=0); 936 BOOST_QVM_STATIC_ASSERT(Col<cols); 937 return mat_traits<M>::template read_element<Row,Col>(reinterpret_cast<M const &>(x)); 938 } 939 940 template <int Row,int Col> 941 static 942 BOOST_QVM_INLINE_CRITICAL 943 scalar_type & write_elementboost::qvm::mat_traits944 write_element( this_matrix & x ) 945 { 946 BOOST_QVM_STATIC_ASSERT(Row>=0); 947 BOOST_QVM_STATIC_ASSERT(Row<rows); 948 BOOST_QVM_STATIC_ASSERT(Col>=0); 949 BOOST_QVM_STATIC_ASSERT(Col<cols); 950 return mat_traits<M>::template write_element<Row,Col>(reinterpret_cast<M &>(x)); 951 } 952 953 static 954 BOOST_QVM_INLINE_CRITICAL 955 scalar_type read_element_idxboost::qvm::mat_traits956 read_element_idx( int row, int col, this_matrix const & x ) 957 { 958 BOOST_QVM_ASSERT(row>=0); 959 BOOST_QVM_ASSERT(row<rows); 960 BOOST_QVM_ASSERT(col>=0); 961 BOOST_QVM_ASSERT(col<cols); 962 return mat_traits<M>::read_element_idx(row,col,reinterpret_cast<M const &>(x)); 963 } 964 965 static 966 BOOST_QVM_INLINE_CRITICAL 967 scalar_type & write_element_idxboost::qvm::mat_traits968 write_element_idx( int row, int col, this_matrix & x ) 969 { 970 BOOST_QVM_ASSERT(row>=0); 971 BOOST_QVM_ASSERT(row<rows); 972 BOOST_QVM_ASSERT(col>=0); 973 BOOST_QVM_ASSERT(col<cols); 974 return mat_traits<M>::write_element_idx(row,col,reinterpret_cast<M &>(x)); 975 } 976 }; 977 978 template <class M,int R,int C> 979 struct 980 deduce_mat<qvm_detail::mref_<M>,R,C> 981 { 982 typedef mat<typename mat_traits<M>::scalar_type,R,C> type; 983 }; 984 985 template <class M> 986 BOOST_QVM_INLINE_TRIVIAL 987 typename enable_if_c< 988 is_mat<M>::value, 989 qvm_detail::mref_<M> const &>::type mref(M const & a)990 mref( M const & a ) 991 { 992 return reinterpret_cast<qvm_detail::mref_<M> const &>(a); 993 } 994 995 template <class M> 996 BOOST_QVM_INLINE_TRIVIAL 997 typename enable_if_c< 998 is_mat<M>::value, 999 qvm_detail::mref_<M> &>::type mref(M & a)1000 mref( M & a ) 1001 { 1002 return reinterpret_cast<qvm_detail::mref_<M> &>(a); 1003 } 1004 1005 //////////////////////////////////////////////// 1006 1007 namespace 1008 qvm_detail 1009 { 1010 template <class T,int Rows,int Cols> 1011 class 1012 zero_mat_ 1013 { 1014 zero_mat_( zero_mat_ const & ); 1015 zero_mat_ & operator=( zero_mat_ const & ); 1016 ~zero_mat_(); 1017 1018 public: 1019 1020 template <class R> 1021 BOOST_QVM_INLINE_TRIVIAL operator R() const1022 operator R() const 1023 { 1024 R r; 1025 assign(r,*this); 1026 return r; 1027 } 1028 }; 1029 } 1030 1031 template <class T,int Rows,int Cols> 1032 struct 1033 mat_traits< qvm_detail::zero_mat_<T,Rows,Cols> > 1034 { 1035 typedef qvm_detail::zero_mat_<T,Rows,Cols> this_matrix; 1036 typedef T scalar_type; 1037 static int const rows=Rows; 1038 static int const cols=Cols; 1039 1040 template <int Row,int Col> 1041 static 1042 BOOST_QVM_INLINE_CRITICAL 1043 scalar_type read_elementboost::qvm::mat_traits1044 read_element( this_matrix const & ) 1045 { 1046 BOOST_QVM_STATIC_ASSERT(Row>=0); 1047 BOOST_QVM_STATIC_ASSERT(Row<Rows); 1048 BOOST_QVM_STATIC_ASSERT(Col>=0); 1049 BOOST_QVM_STATIC_ASSERT(Col<Cols); 1050 return scalar_traits<scalar_type>::value(0); 1051 } 1052 1053 static 1054 BOOST_QVM_INLINE_CRITICAL 1055 scalar_type read_element_idxboost::qvm::mat_traits1056 read_element_idx( int row, int col, this_matrix const & ) 1057 { 1058 BOOST_QVM_ASSERT(row>=0); 1059 BOOST_QVM_ASSERT(row<rows); 1060 BOOST_QVM_ASSERT(col>=0); 1061 BOOST_QVM_ASSERT(col<cols); 1062 return scalar_traits<scalar_type>::value(0); 1063 } 1064 }; 1065 1066 template <class T,int Rows,int Cols,int R,int C> 1067 struct 1068 deduce_mat<qvm_detail::zero_mat_<T,Rows,Cols>,R,C> 1069 { 1070 typedef mat<T,R,C> type; 1071 }; 1072 1073 template <class T,int Rows,int Cols> 1074 BOOST_QVM_INLINE_TRIVIAL 1075 qvm_detail::zero_mat_<T,Rows,Cols> const & zero_mat()1076 zero_mat() 1077 { 1078 return *(qvm_detail::zero_mat_<T,Rows,Cols> const *)qvm_detail::get_valid_ptr_mat_operations(); 1079 } 1080 1081 template <class T,int Dim> 1082 BOOST_QVM_INLINE_TRIVIAL 1083 qvm_detail::zero_mat_<T,Dim,Dim> const & zero_mat()1084 zero_mat() 1085 { 1086 return *(qvm_detail::zero_mat_<T,Dim,Dim> const *)qvm_detail::get_valid_ptr_mat_operations(); 1087 } 1088 1089 template <class A> 1090 BOOST_QVM_INLINE_OPERATIONS 1091 typename enable_if_c< 1092 is_mat<A>::value, 1093 void>::type set_zero(A & a)1094 set_zero( A & a ) 1095 { 1096 assign(a,zero_mat<typename mat_traits<A>::scalar_type,mat_traits<A>::rows,mat_traits<A>::cols>()); 1097 } 1098 1099 //////////////////////////////////////////////// 1100 1101 namespace 1102 qvm_detail 1103 { 1104 template <int D,class V> 1105 struct 1106 rot_mat_ 1107 { 1108 typedef typename vec_traits<V>::scalar_type scalar_type; 1109 scalar_type a[3][3]; 1110 1111 template <class Angle> 1112 BOOST_QVM_INLINE rot_mat_boost::qvm::qvm_detail::rot_mat_1113 rot_mat_( V const & axis, Angle angle ) 1114 { 1115 scalar_type const x=vec_traits<V>::template read_element<0>(axis); 1116 scalar_type const y=vec_traits<V>::template read_element<1>(axis); 1117 scalar_type const z=vec_traits<V>::template read_element<2>(axis); 1118 scalar_type const m2=x*x+y*y+z*z; 1119 if( m2==scalar_traits<scalar_type>::value(0) ) 1120 BOOST_QVM_THROW_EXCEPTION(zero_magnitude_error()); 1121 scalar_type const s = sin<Angle>(angle); 1122 scalar_type const c = cos<Angle>(angle); 1123 scalar_type const x2 = x*x; 1124 scalar_type const y2 = y*y; 1125 scalar_type const z2 = z*z; 1126 scalar_type const xy = x*y; 1127 scalar_type const xz = x*z; 1128 scalar_type const yz = y*z; 1129 scalar_type const xs = x*s; 1130 scalar_type const ys = y*s; 1131 scalar_type const zs = z*s; 1132 scalar_type const one = scalar_traits<scalar_type>::value(1); 1133 scalar_type const c1 = one-c; 1134 a[0][0] = x2+(one-x2)*c; 1135 a[0][1] = xy*c1-zs; 1136 a[0][2] = xz*(one-c)+ys; 1137 a[1][0] = xy*c1+zs; 1138 a[1][1] = y2+(one-y2)*c; 1139 a[1][2] = yz*c1-xs; 1140 a[2][0] = xz*c1-ys; 1141 a[2][1] = yz*c1+xs; 1142 a[2][2] = z2+(one-z2)*c; 1143 } 1144 1145 template <class R> 1146 BOOST_QVM_INLINE_TRIVIAL operator Rboost::qvm::qvm_detail::rot_mat_1147 operator R() const 1148 { 1149 R r; 1150 assign(r,*this); 1151 return r; 1152 } 1153 }; 1154 1155 template <int Row,int Col> 1156 struct 1157 rot_m_get 1158 { 1159 template <class T> 1160 static 1161 BOOST_QVM_INLINE_CRITICAL 1162 T getboost::qvm::qvm_detail::rot_m_get1163 get( T const (&)[3][3] ) 1164 { 1165 return scalar_traits<T>::value(Row==Col); 1166 } 1167 }; 1168 getboost::qvm::qvm_detail::rot_m_get1169 template <> struct rot_m_get<0,0> { template <class T> static BOOST_QVM_INLINE_CRITICAL T get( T const (&a)[3][3] ) { return a[0][0]; } }; getboost::qvm::qvm_detail::rot_m_get1170 template <> struct rot_m_get<0,1> { template <class T> static BOOST_QVM_INLINE_CRITICAL T get( T const (&a)[3][3] ) { return a[0][1]; } }; getboost::qvm::qvm_detail::rot_m_get1171 template <> struct rot_m_get<0,2> { template <class T> static BOOST_QVM_INLINE_CRITICAL T get( T const (&a)[3][3] ) { return a[0][2]; } }; getboost::qvm::qvm_detail::rot_m_get1172 template <> struct rot_m_get<1,0> { template <class T> static BOOST_QVM_INLINE_CRITICAL T get( T const (&a)[3][3] ) { return a[1][0]; } }; getboost::qvm::qvm_detail::rot_m_get1173 template <> struct rot_m_get<1,1> { template <class T> static BOOST_QVM_INLINE_CRITICAL T get( T const (&a)[3][3] ) { return a[1][1]; } }; getboost::qvm::qvm_detail::rot_m_get1174 template <> struct rot_m_get<1,2> { template <class T> static BOOST_QVM_INLINE_CRITICAL T get( T const (&a)[3][3] ) { return a[1][2]; } }; getboost::qvm::qvm_detail::rot_m_get1175 template <> struct rot_m_get<2,0> { template <class T> static BOOST_QVM_INLINE_CRITICAL T get( T const (&a)[3][3] ) { return a[2][0]; } }; getboost::qvm::qvm_detail::rot_m_get1176 template <> struct rot_m_get<2,1> { template <class T> static BOOST_QVM_INLINE_CRITICAL T get( T const (&a)[3][3] ) { return a[2][1]; } }; getboost::qvm::qvm_detail::rot_m_get1177 template <> struct rot_m_get<2,2> { template <class T> static BOOST_QVM_INLINE_CRITICAL T get( T const (&a)[3][3] ) { return a[2][2]; } }; 1178 } 1179 1180 template <class M> 1181 struct mat_traits; 1182 1183 template <int D,class V> 1184 struct 1185 mat_traits< qvm_detail::rot_mat_<D,V> > 1186 { 1187 typedef qvm_detail::rot_mat_<D,V> this_matrix; 1188 typedef typename this_matrix::scalar_type scalar_type; 1189 static int const rows=D; 1190 static int const cols=D; 1191 1192 template <int Row,int Col> 1193 static 1194 BOOST_QVM_INLINE_CRITICAL 1195 scalar_type read_elementboost::qvm::mat_traits1196 read_element( this_matrix const & x ) 1197 { 1198 BOOST_QVM_STATIC_ASSERT(Row>=0); 1199 BOOST_QVM_STATIC_ASSERT(Row<D); 1200 BOOST_QVM_STATIC_ASSERT(Col>=0); 1201 BOOST_QVM_STATIC_ASSERT(Col<D); 1202 return qvm_detail::rot_m_get<Row,Col>::get(x.a); 1203 } 1204 1205 static 1206 BOOST_QVM_INLINE_CRITICAL 1207 scalar_type read_element_idxboost::qvm::mat_traits1208 read_element_idx( int row, int col, this_matrix const & x ) 1209 { 1210 BOOST_QVM_ASSERT(row>=0); 1211 BOOST_QVM_ASSERT(row<D); 1212 BOOST_QVM_ASSERT(col>=0); 1213 BOOST_QVM_ASSERT(col<D); 1214 return row<3 && col<3? 1215 x.a[row][col] : 1216 scalar_traits<scalar_type>::value(row==col); 1217 } 1218 }; 1219 1220 template <int Dim,class A,class Angle> 1221 BOOST_QVM_INLINE 1222 typename enable_if_c< 1223 is_vec<A>::value && vec_traits<A>::dim==3, 1224 qvm_detail::rot_mat_<Dim,A> >::type rot_mat(A const & axis,Angle angle)1225 rot_mat( A const & axis, Angle angle ) 1226 { 1227 return qvm_detail::rot_mat_<Dim,A>(axis,angle); 1228 } 1229 1230 template <class A,class B,class Angle> 1231 BOOST_QVM_INLINE_OPERATIONS 1232 typename enable_if_c< 1233 is_mat<A>::value && 1234 mat_traits<A>::rows==mat_traits<A>::cols && 1235 mat_traits<A>::rows>=3 && 1236 is_vec<B>::value && vec_traits<B>::dim==3, 1237 void>::type 1238 set_rot( A & a, B const & axis, Angle angle ) 1239 { 1240 assign(a,rot_mat<mat_traits<A>::rows>(axis,angle)); 1241 } 1242 1243 template <class A,class B,class Angle> 1244 BOOST_QVM_INLINE_OPERATIONS 1245 typename enable_if_c< 1246 is_mat<A>::value && 1247 mat_traits<A>::rows==mat_traits<A>::cols && 1248 mat_traits<A>::rows>=3 && 1249 is_vec<B>::value && vec_traits<B>::dim==3, 1250 void>::type 1251 rotate( A & a, B const & axis, Angle angle ) 1252 { 1253 a *= rot_mat<mat_traits<A>::rows>(axis,angle); 1254 } 1255 1256 //////////////////////////////////////////////// 1257 1258 namespace 1259 qvm_detail 1260 { 1261 template <int Dim,class Angle> 1262 struct 1263 rotx_mat_ 1264 { 1265 BOOST_QVM_INLINE_TRIVIAL rotx_mat_boost::qvm::qvm_detail::rotx_mat_1266 rotx_mat_() 1267 { 1268 } 1269 1270 template <class R> 1271 BOOST_QVM_INLINE_TRIVIAL operator Rboost::qvm::qvm_detail::rotx_mat_1272 operator R() const 1273 { 1274 R r; 1275 assign(r,*this); 1276 return r; 1277 } 1278 1279 private: 1280 1281 rotx_mat_( rotx_mat_ const & ); 1282 rotx_mat_ & operator=( rotx_mat_ const & ); 1283 ~rotx_mat_(); 1284 }; 1285 1286 template <int Row,int Col> 1287 struct 1288 rotx_m_get 1289 { 1290 template <class T> 1291 static 1292 BOOST_QVM_INLINE_CRITICAL 1293 T getboost::qvm::qvm_detail::rotx_m_get1294 get( T const & ) 1295 { 1296 return scalar_traits<T>::value(Row==Col); 1297 } 1298 }; 1299 1300 template <> 1301 struct 1302 rotx_m_get<1,1> 1303 { 1304 template <class T> 1305 static 1306 BOOST_QVM_INLINE_CRITICAL 1307 T getboost::qvm::qvm_detail::rotx_m_get1308 get( T const & angle ) 1309 { 1310 return cos<T>(angle); 1311 } 1312 }; 1313 1314 template <> 1315 struct 1316 rotx_m_get<1,2> 1317 { 1318 template <class T> 1319 static 1320 BOOST_QVM_INLINE_CRITICAL 1321 T getboost::qvm::qvm_detail::rotx_m_get1322 get( T const & angle ) 1323 { 1324 return -sin<T>(angle); 1325 } 1326 }; 1327 1328 template <> 1329 struct 1330 rotx_m_get<2,1> 1331 { 1332 template <class T> 1333 static 1334 BOOST_QVM_INLINE_CRITICAL 1335 T getboost::qvm::qvm_detail::rotx_m_get1336 get( T const & angle ) 1337 { 1338 return sin<T>(angle); 1339 } 1340 }; 1341 1342 template <> 1343 struct 1344 rotx_m_get<2,2> 1345 { 1346 template <class T> 1347 static 1348 BOOST_QVM_INLINE_CRITICAL 1349 T getboost::qvm::qvm_detail::rotx_m_get1350 get( T const & angle ) 1351 { 1352 return cos<T>(angle); 1353 } 1354 }; 1355 } 1356 1357 template <int Dim,class Angle> 1358 struct 1359 mat_traits< qvm_detail::rotx_mat_<Dim,Angle> > 1360 { 1361 typedef qvm_detail::rotx_mat_<Dim,Angle> this_matrix; 1362 typedef Angle scalar_type; 1363 static int const rows=Dim; 1364 static int const cols=Dim; 1365 1366 template <int Row,int Col> 1367 static 1368 BOOST_QVM_INLINE_CRITICAL 1369 scalar_type read_elementboost::qvm::mat_traits1370 read_element( this_matrix const & x ) 1371 { 1372 BOOST_QVM_STATIC_ASSERT(Row>=0); 1373 BOOST_QVM_STATIC_ASSERT(Col>=0); 1374 BOOST_QVM_STATIC_ASSERT(Row<Dim); 1375 BOOST_QVM_STATIC_ASSERT(Col<Dim); 1376 return qvm_detail::rotx_m_get<Row,Col>::get(reinterpret_cast<Angle const &>(x)); 1377 } 1378 1379 static 1380 BOOST_QVM_INLINE_CRITICAL 1381 scalar_type read_element_idxboost::qvm::mat_traits1382 read_element_idx( int row, int col, this_matrix const & x ) 1383 { 1384 BOOST_QVM_ASSERT(row>=0); 1385 BOOST_QVM_ASSERT(col>=0); 1386 BOOST_QVM_ASSERT(row<Dim); 1387 BOOST_QVM_ASSERT(col<Dim); 1388 Angle const & a=reinterpret_cast<Angle const &>(x); 1389 if( row==1 ) 1390 { 1391 if( col==1 ) 1392 return cos<scalar_type>(a); 1393 if( col==2 ) 1394 return -sin<scalar_type>(a); 1395 } 1396 if( row==2 ) 1397 { 1398 if( col==1 ) 1399 return sin<scalar_type>(a); 1400 if( col==2 ) 1401 return cos<scalar_type>(a); 1402 } 1403 return scalar_traits<scalar_type>::value(row==col); 1404 } 1405 }; 1406 1407 template <int Dim,class Angle> 1408 struct 1409 deduce_mat<qvm_detail::rotx_mat_<Dim,Angle>,Dim,Dim> 1410 { 1411 typedef mat<Angle,Dim,Dim> type; 1412 }; 1413 1414 template <int Dim,class Angle> 1415 struct 1416 deduce_mat2<qvm_detail::rotx_mat_<Dim,Angle>,qvm_detail::rotx_mat_<Dim,Angle>,Dim,Dim> 1417 { 1418 typedef mat<Angle,Dim,Dim> type; 1419 }; 1420 1421 template <int Dim,class Angle> 1422 BOOST_QVM_INLINE_TRIVIAL 1423 qvm_detail::rotx_mat_<Dim,Angle> const & rotx_mat(Angle const & angle)1424 rotx_mat( Angle const & angle ) 1425 { 1426 BOOST_QVM_STATIC_ASSERT(Dim>=3); 1427 return reinterpret_cast<qvm_detail::rotx_mat_<Dim,Angle> const &>(angle); 1428 } 1429 1430 template <class A,class Angle> 1431 BOOST_QVM_INLINE_OPERATIONS 1432 typename enable_if_c< 1433 is_mat<A>::value && 1434 mat_traits<A>::rows>=3 && 1435 mat_traits<A>::rows==mat_traits<A>::cols, 1436 void>::type 1437 set_rotx( A & a, Angle angle ) 1438 { 1439 assign(a,rotx_mat<mat_traits<A>::rows>(angle)); 1440 } 1441 1442 template <class A,class Angle> 1443 BOOST_QVM_INLINE_OPERATIONS 1444 typename enable_if_c< 1445 is_mat<A>::value && 1446 mat_traits<A>::rows>=3 && 1447 mat_traits<A>::rows==mat_traits<A>::cols, 1448 void>::type 1449 rotate_x( A & a, Angle angle ) 1450 { 1451 a *= rotx_mat<mat_traits<A>::rows>(angle); 1452 } 1453 1454 //////////////////////////////////////////////// 1455 1456 namespace 1457 qvm_detail 1458 { 1459 template <int Dim,class Angle> 1460 struct 1461 roty_mat_ 1462 { 1463 BOOST_QVM_INLINE_TRIVIAL roty_mat_boost::qvm::qvm_detail::roty_mat_1464 roty_mat_() 1465 { 1466 } 1467 1468 template <class R> 1469 BOOST_QVM_INLINE_TRIVIAL operator Rboost::qvm::qvm_detail::roty_mat_1470 operator R() const 1471 { 1472 R r; 1473 assign(r,*this); 1474 return r; 1475 } 1476 1477 private: 1478 1479 roty_mat_( roty_mat_ const & ); 1480 roty_mat_ & operator=( roty_mat_ const & ); 1481 ~roty_mat_(); 1482 }; 1483 1484 template <int Row,int Col> 1485 struct 1486 roty_m_get 1487 { 1488 template <class T> 1489 static 1490 BOOST_QVM_INLINE_CRITICAL 1491 T getboost::qvm::qvm_detail::roty_m_get1492 get( T const & ) 1493 { 1494 return scalar_traits<T>::value(Row==Col); 1495 } 1496 }; 1497 1498 template <> 1499 struct 1500 roty_m_get<0,0> 1501 { 1502 template <class T> 1503 static 1504 BOOST_QVM_INLINE_CRITICAL 1505 T getboost::qvm::qvm_detail::roty_m_get1506 get( T const & angle ) 1507 { 1508 return cos<T>(angle); 1509 } 1510 }; 1511 1512 template <> 1513 struct 1514 roty_m_get<0,2> 1515 { 1516 template <class T> 1517 static 1518 BOOST_QVM_INLINE_CRITICAL 1519 T getboost::qvm::qvm_detail::roty_m_get1520 get( T const & angle ) 1521 { 1522 return sin<T>(angle); 1523 } 1524 }; 1525 1526 template <> 1527 struct 1528 roty_m_get<2,0> 1529 { 1530 template <class T> 1531 static 1532 BOOST_QVM_INLINE_CRITICAL 1533 T getboost::qvm::qvm_detail::roty_m_get1534 get( T const & angle ) 1535 { 1536 return -sin<T>(angle); 1537 } 1538 }; 1539 1540 template <> 1541 struct 1542 roty_m_get<2,2> 1543 { 1544 template <class T> 1545 static 1546 BOOST_QVM_INLINE_CRITICAL 1547 T getboost::qvm::qvm_detail::roty_m_get1548 get( T const & angle ) 1549 { 1550 return cos<T>(angle); 1551 } 1552 }; 1553 } 1554 1555 template <int Dim,class Angle> 1556 struct 1557 mat_traits< qvm_detail::roty_mat_<Dim,Angle> > 1558 { 1559 typedef qvm_detail::roty_mat_<Dim,Angle> this_matrix; 1560 typedef Angle scalar_type; 1561 static int const rows=Dim; 1562 static int const cols=Dim; 1563 1564 template <int Row,int Col> 1565 static 1566 BOOST_QVM_INLINE_CRITICAL 1567 scalar_type read_elementboost::qvm::mat_traits1568 read_element( this_matrix const & x ) 1569 { 1570 BOOST_QVM_STATIC_ASSERT(Row>=0); 1571 BOOST_QVM_STATIC_ASSERT(Col>=0); 1572 BOOST_QVM_STATIC_ASSERT(Row<Dim); 1573 BOOST_QVM_STATIC_ASSERT(Col<Dim); 1574 return qvm_detail::roty_m_get<Row,Col>::get(reinterpret_cast<Angle const &>(x)); 1575 } 1576 1577 static 1578 BOOST_QVM_INLINE_CRITICAL 1579 scalar_type read_element_idxboost::qvm::mat_traits1580 read_element_idx( int row, int col, this_matrix const & x ) 1581 { 1582 BOOST_QVM_ASSERT(row>=0); 1583 BOOST_QVM_ASSERT(col>=0); 1584 BOOST_QVM_ASSERT(row<Dim); 1585 BOOST_QVM_ASSERT(col<Dim); 1586 Angle const & a=reinterpret_cast<Angle const &>(x); 1587 if( row==0 ) 1588 { 1589 if( col==0 ) 1590 return cos<scalar_type>(a); 1591 if( col==2 ) 1592 return sin<scalar_type>(a); 1593 } 1594 if( row==2 ) 1595 { 1596 if( col==0 ) 1597 return -sin<scalar_type>(a); 1598 if( col==2 ) 1599 return cos<scalar_type>(a); 1600 } 1601 return scalar_traits<scalar_type>::value(row==col); 1602 } 1603 }; 1604 1605 template <int Dim,class Angle> 1606 struct 1607 deduce_mat<qvm_detail::roty_mat_<Dim,Angle>,Dim,Dim> 1608 { 1609 typedef mat<Angle,Dim,Dim> type; 1610 }; 1611 1612 template <int Dim,class Angle> 1613 struct 1614 deduce_mat2<qvm_detail::roty_mat_<Dim,Angle>,qvm_detail::roty_mat_<Dim,Angle>,Dim,Dim> 1615 { 1616 typedef mat<Angle,Dim,Dim> type; 1617 }; 1618 1619 template <int Dim,class Angle> 1620 BOOST_QVM_INLINE_TRIVIAL 1621 qvm_detail::roty_mat_<Dim,Angle> const & roty_mat(Angle const & angle)1622 roty_mat( Angle const & angle ) 1623 { 1624 BOOST_QVM_STATIC_ASSERT(Dim>=3); 1625 return reinterpret_cast<qvm_detail::roty_mat_<Dim,Angle> const &>(angle); 1626 } 1627 1628 template <class A,class Angle> 1629 BOOST_QVM_INLINE_OPERATIONS 1630 typename enable_if_c< 1631 is_mat<A>::value && 1632 mat_traits<A>::rows>=2 && 1633 mat_traits<A>::rows==mat_traits<A>::cols, 1634 void>::type 1635 set_roty( A & a, Angle angle ) 1636 { 1637 assign(a,roty_mat<mat_traits<A>::rows>(angle)); 1638 } 1639 1640 template <class A,class Angle> 1641 BOOST_QVM_INLINE_OPERATIONS 1642 typename enable_if_c< 1643 is_mat<A>::value && 1644 mat_traits<A>::rows>=3 && 1645 mat_traits<A>::rows==mat_traits<A>::cols, 1646 void>::type 1647 rotate_y( A & a, Angle angle ) 1648 { 1649 a *= roty_mat<mat_traits<A>::rows>(angle); 1650 } 1651 1652 //////////////////////////////////////////////// 1653 1654 namespace 1655 qvm_detail 1656 { 1657 template <int Dim,class Angle> 1658 struct 1659 rotz_mat_ 1660 { 1661 BOOST_QVM_INLINE_TRIVIAL rotz_mat_boost::qvm::qvm_detail::rotz_mat_1662 rotz_mat_() 1663 { 1664 } 1665 1666 template <class R> 1667 BOOST_QVM_INLINE_TRIVIAL operator Rboost::qvm::qvm_detail::rotz_mat_1668 operator R() const 1669 { 1670 R r; 1671 assign(r,*this); 1672 return r; 1673 } 1674 1675 private: 1676 1677 rotz_mat_( rotz_mat_ const & ); 1678 rotz_mat_ & operator=( rotz_mat_ const & ); 1679 ~rotz_mat_(); 1680 }; 1681 1682 template <int Row,int Col> 1683 struct 1684 rotz_m_get 1685 { 1686 template <class T> 1687 static 1688 BOOST_QVM_INLINE_CRITICAL 1689 T getboost::qvm::qvm_detail::rotz_m_get1690 get( T const & ) 1691 { 1692 return scalar_traits<T>::value(Row==Col); 1693 } 1694 }; 1695 1696 template <> 1697 struct 1698 rotz_m_get<0,0> 1699 { 1700 template <class T> 1701 static 1702 BOOST_QVM_INLINE_CRITICAL 1703 T getboost::qvm::qvm_detail::rotz_m_get1704 get( T const & angle ) 1705 { 1706 return cos<T>(angle); 1707 } 1708 }; 1709 1710 template <> 1711 struct 1712 rotz_m_get<0,1> 1713 { 1714 template <class T> 1715 static 1716 BOOST_QVM_INLINE_CRITICAL 1717 T getboost::qvm::qvm_detail::rotz_m_get1718 get( T const & angle ) 1719 { 1720 return -sin<T>(angle); 1721 } 1722 }; 1723 1724 template <> 1725 struct 1726 rotz_m_get<1,0> 1727 { 1728 template <class T> 1729 static 1730 BOOST_QVM_INLINE_CRITICAL 1731 T getboost::qvm::qvm_detail::rotz_m_get1732 get( T const & angle ) 1733 { 1734 return sin<T>(angle); 1735 } 1736 }; 1737 1738 template <> 1739 struct 1740 rotz_m_get<1,1> 1741 { 1742 template <class T> 1743 static 1744 BOOST_QVM_INLINE_CRITICAL 1745 T getboost::qvm::qvm_detail::rotz_m_get1746 get( T const & angle ) 1747 { 1748 return cos<T>(angle); 1749 } 1750 }; 1751 } 1752 1753 template <int Dim,class Angle> 1754 struct 1755 mat_traits< qvm_detail::rotz_mat_<Dim,Angle> > 1756 { 1757 typedef qvm_detail::rotz_mat_<Dim,Angle> this_matrix; 1758 typedef Angle scalar_type; 1759 static int const rows=Dim; 1760 static int const cols=Dim; 1761 1762 template <int Row,int Col> 1763 static 1764 BOOST_QVM_INLINE_CRITICAL 1765 scalar_type read_elementboost::qvm::mat_traits1766 read_element( this_matrix const & x ) 1767 { 1768 BOOST_QVM_STATIC_ASSERT(Row>=0); 1769 BOOST_QVM_STATIC_ASSERT(Col>=0); 1770 BOOST_QVM_STATIC_ASSERT(Row<Dim); 1771 BOOST_QVM_STATIC_ASSERT(Col<Dim); 1772 return qvm_detail::rotz_m_get<Row,Col>::get(reinterpret_cast<Angle const &>(x)); 1773 } 1774 1775 static 1776 BOOST_QVM_INLINE_CRITICAL 1777 scalar_type read_element_idxboost::qvm::mat_traits1778 read_element_idx( int row, int col, this_matrix const & x ) 1779 { 1780 BOOST_QVM_ASSERT(row>=0); 1781 BOOST_QVM_ASSERT(col>=0); 1782 BOOST_QVM_ASSERT(row<Dim); 1783 BOOST_QVM_ASSERT(col<Dim); 1784 Angle const & a=reinterpret_cast<Angle const &>(x); 1785 if( row==0 ) 1786 { 1787 if( col==0 ) 1788 return cos<scalar_type>(a); 1789 if( col==1 ) 1790 return -sin<scalar_type>(a); 1791 } 1792 if( row==1 ) 1793 { 1794 if( col==0 ) 1795 return sin<scalar_type>(a); 1796 if( col==1 ) 1797 return cos<scalar_type>(a); 1798 } 1799 return scalar_traits<scalar_type>::value(row==col); 1800 } 1801 }; 1802 1803 template <int Dim,class Angle> 1804 struct 1805 deduce_mat<qvm_detail::rotz_mat_<Dim,Angle>,Dim,Dim> 1806 { 1807 typedef mat<Angle,Dim,Dim> type; 1808 }; 1809 1810 template <int Dim,class Angle,int R,int C> 1811 struct 1812 deduce_mat2<qvm_detail::rotz_mat_<Dim,Angle>,qvm_detail::rotz_mat_<Dim,Angle>,R,C> 1813 { 1814 typedef mat<Angle,R,C> type; 1815 }; 1816 1817 template <int Dim,class Angle> 1818 BOOST_QVM_INLINE_TRIVIAL 1819 qvm_detail::rotz_mat_<Dim,Angle> const & rotz_mat(Angle const & angle)1820 rotz_mat( Angle const & angle ) 1821 { 1822 BOOST_QVM_STATIC_ASSERT(Dim>=2); 1823 return reinterpret_cast<qvm_detail::rotz_mat_<Dim,Angle> const &>(angle); 1824 } 1825 1826 template <class A,class Angle> 1827 BOOST_QVM_INLINE_OPERATIONS 1828 typename enable_if_c< 1829 is_mat<A>::value && 1830 mat_traits<A>::rows>=2 && 1831 mat_traits<A>::rows==mat_traits<A>::cols, 1832 void>::type 1833 set_rotz( A & a, Angle angle ) 1834 { 1835 assign(a,rotz_mat<mat_traits<A>::rows>(angle)); 1836 } 1837 1838 template <class A,class Angle> 1839 BOOST_QVM_INLINE_OPERATIONS 1840 typename enable_if_c< 1841 is_mat<A>::value && 1842 mat_traits<A>::rows>=2 && 1843 mat_traits<A>::rows==mat_traits<A>::cols, 1844 void>::type 1845 rotate_z( A & a, Angle angle ) 1846 { 1847 a *= rotz_mat<mat_traits<A>::rows>(angle); 1848 } 1849 1850 //////////////////////////////////////////////// 1851 1852 namespace 1853 qvm_detail 1854 { 1855 template <int D> 1856 struct 1857 inverse_m_defined 1858 { 1859 static bool const value=false; 1860 }; 1861 } 1862 1863 template <class A,class B> 1864 BOOST_QVM_INLINE_TRIVIAL 1865 typename lazy_enable_if_c< 1866 is_mat<A>::value && is_scalar<B>::value && 1867 mat_traits<A>::rows==mat_traits<A>::cols && 1868 !qvm_detail::inverse_m_defined<mat_traits<A>::rows>::value, 1869 deduce_mat<A> >::type inverse(A const & a,B det)1870 inverse( A const & a, B det ) 1871 { 1872 typedef typename mat_traits<A>::scalar_type T; 1873 BOOST_QVM_ASSERT(det!=scalar_traits<T>::value(0)); 1874 T f=scalar_traits<T>::value(1)/det; 1875 typedef typename deduce_mat<A>::type cofactor_return_type; 1876 cofactor_return_type c=qvm_detail::cofactor_impl(a); 1877 return reinterpret_cast<qvm_detail::transposed_<cofactor_return_type> const &>(c) * f; 1878 } 1879 1880 template <class A> 1881 BOOST_QVM_INLINE_TRIVIAL 1882 typename lazy_enable_if_c< 1883 is_mat<A>::value && 1884 mat_traits<A>::rows==mat_traits<A>::cols && 1885 !qvm_detail::inverse_m_defined<mat_traits<A>::rows>::value, 1886 deduce_mat<A> >::type inverse(A const & a)1887 inverse( A const & a ) 1888 { 1889 typedef typename mat_traits<A>::scalar_type T; 1890 T det=determinant(a); 1891 if( det==scalar_traits<T>::value(0) ) 1892 BOOST_QVM_THROW_EXCEPTION(zero_determinant_error()); 1893 return inverse(a,det); 1894 } 1895 1896 //////////////////////////////////////////////// 1897 1898 namespace 1899 sfinae 1900 { 1901 using ::boost::qvm::to_string; 1902 using ::boost::qvm::assign; 1903 using ::boost::qvm::determinant; 1904 using ::boost::qvm::cmp; 1905 using ::boost::qvm::convert_to; 1906 using ::boost::qvm::set_identity; 1907 using ::boost::qvm::set_zero; 1908 using ::boost::qvm::scalar_cast; 1909 using ::boost::qvm::operator/=; 1910 using ::boost::qvm::operator/; 1911 using ::boost::qvm::operator==; 1912 using ::boost::qvm::operator-=; 1913 using ::boost::qvm::operator-; 1914 using ::boost::qvm::operator*=; 1915 using ::boost::qvm::operator*; 1916 using ::boost::qvm::operator!=; 1917 using ::boost::qvm::operator+=; 1918 using ::boost::qvm::operator+; 1919 using ::boost::qvm::mref; 1920 using ::boost::qvm::rot_mat; 1921 using ::boost::qvm::set_rot; 1922 using ::boost::qvm::rotate; 1923 using ::boost::qvm::set_rotx; 1924 using ::boost::qvm::rotate_x; 1925 using ::boost::qvm::set_roty; 1926 using ::boost::qvm::rotate_y; 1927 using ::boost::qvm::set_rotz; 1928 using ::boost::qvm::rotate_z; 1929 using ::boost::qvm::inverse; 1930 } 1931 1932 //////////////////////////////////////////////// 1933 } 1934 } 1935 1936 #endif 1937