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 BOOST_QVM_E6519754D19211DFB8405F74DFD72085 7 #define BOOST_QVM_E6519754D19211DFB8405F74DFD72085 8 9 #include <boost/qvm/detail/quat_assign.hpp> 10 #include <boost/qvm/deduce_quat.hpp> 11 #include <boost/qvm/mat_traits.hpp> 12 #include <boost/qvm/scalar_traits.hpp> 13 #include <boost/qvm/math.hpp> 14 #include <boost/qvm/assert.hpp> 15 #include <boost/qvm/error.hpp> 16 #include <boost/qvm/throw_exception.hpp> 17 #include <string> 18 19 namespace 20 boost 21 { 22 namespace 23 qvm 24 { 25 namespace 26 qvm_detail 27 { 28 BOOST_QVM_INLINE_CRITICAL 29 void const * get_valid_ptr_quat_operations()30 get_valid_ptr_quat_operations() 31 { 32 static int const obj=0; 33 return &obj; 34 } 35 } 36 37 //////////////////////////////////////////////// 38 39 namespace 40 msvc_parse_bug_workaround 41 { 42 template <class A,class B> 43 struct 44 quats 45 { 46 static bool const value=is_quat<A>::value && is_quat<B>::value; 47 }; 48 } 49 50 namespace 51 qvm_to_string_detail 52 { 53 template <class T> 54 std::string to_string( T const & x ); 55 } 56 57 template <class A> 58 inline 59 typename boost::enable_if_c< 60 is_quat<A>::value, 61 std::string>::type to_string(A const & a)62 to_string( A const & a ) 63 { 64 using namespace qvm_to_string_detail; 65 return '('+ 66 to_string(quat_traits<A>::template read_element<0>(a))+','+ 67 to_string(quat_traits<A>::template read_element<1>(a))+','+ 68 to_string(quat_traits<A>::template read_element<2>(a))+','+ 69 to_string(quat_traits<A>::template read_element<3>(a))+')'; 70 } 71 72 //////////////////////////////////////////////// 73 74 template <class A,class B,class Cmp> 75 BOOST_QVM_INLINE_OPERATIONS 76 typename enable_if_c< 77 is_quat<A>::value && is_quat<B>::value, 78 bool>::type cmp(A const & a,B const & b,Cmp f)79 cmp( A const & a, B const & b, Cmp f ) 80 { 81 typedef typename quat_traits<A>::scalar_type T; 82 typedef typename quat_traits<B>::scalar_type U; 83 T q1[4] = 84 { 85 quat_traits<A>::template read_element<0>(a), 86 quat_traits<A>::template read_element<1>(a), 87 quat_traits<A>::template read_element<2>(a), 88 quat_traits<A>::template read_element<3>(a) 89 }; 90 U q2[4] = 91 { 92 quat_traits<B>::template read_element<0>(b), 93 quat_traits<B>::template read_element<1>(b), 94 quat_traits<B>::template read_element<2>(b), 95 quat_traits<B>::template read_element<3>(b) 96 }; 97 int i; 98 for( i=0; i!=4; ++i ) 99 if( !f(q1[i],q2[i]) ) 100 break; 101 if( i==4 ) 102 return true; 103 for( i=0; i!=4; ++i ) 104 if( !f(q1[i],-q2[i]) ) 105 return false; 106 return true; 107 } 108 109 //////////////////////////////////////////////// 110 111 template <class R,class A> 112 BOOST_QVM_INLINE_TRIVIAL 113 typename enable_if_c< 114 is_quat<R>::value && is_quat<A>::value, 115 R>::type convert_to(A const & a)116 convert_to( A const & a ) 117 { 118 R r; 119 quat_traits<R>::template write_element<0>(r) = quat_traits<A>::template read_element<0>(a); 120 quat_traits<R>::template write_element<1>(r) = quat_traits<A>::template read_element<1>(a); 121 quat_traits<R>::template write_element<2>(r) = quat_traits<A>::template read_element<2>(a); 122 quat_traits<R>::template write_element<3>(r) = quat_traits<A>::template read_element<3>(a); 123 return r; 124 } 125 126 template <class R,class A> 127 BOOST_QVM_INLINE_OPERATIONS 128 typename enable_if_c< 129 is_quat<R>::value && is_mat<A>::value && 130 mat_traits<A>::rows==3 && mat_traits<A>::cols==3, 131 R>::type convert_to(A const & a)132 convert_to( A const & a ) 133 { 134 typedef typename mat_traits<A>::scalar_type T; 135 T const mat[3][3] = 136 { 137 { mat_traits<A>::template read_element<0,0>(a), mat_traits<A>::template read_element<0,1>(a), mat_traits<A>::template read_element<0,2>(a) }, 138 { mat_traits<A>::template read_element<1,0>(a), mat_traits<A>::template read_element<1,1>(a), mat_traits<A>::template read_element<1,2>(a) }, 139 { mat_traits<A>::template read_element<2,0>(a), mat_traits<A>::template read_element<2,1>(a), mat_traits<A>::template read_element<2,2>(a) } 140 }; 141 R r; 142 if( mat[0][0]+mat[1][1]+mat[2][2] > scalar_traits<T>::value(0) ) 143 { 144 T t = mat[0][0] + mat[1][1] + mat[2][2] + scalar_traits<T>::value(1); 145 T s = (scalar_traits<T>::value(1)/sqrt<T>(t))/2; 146 quat_traits<R>::template write_element<0>(r)=s*t; 147 quat_traits<R>::template write_element<1>(r)=(mat[2][1]-mat[1][2])*s; 148 quat_traits<R>::template write_element<2>(r)=(mat[0][2]-mat[2][0])*s; 149 quat_traits<R>::template write_element<3>(r)=(mat[1][0]-mat[0][1])*s; 150 } 151 else if( mat[0][0]>mat[1][1] && mat[0][0]>mat[2][2] ) 152 { 153 T t = mat[0][0] - mat[1][1] - mat[2][2] + scalar_traits<T>::value(1); 154 T s = (scalar_traits<T>::value(1)/sqrt<T>(t))/2; 155 quat_traits<R>::template write_element<0>(r)=(mat[2][1]-mat[1][2])*s; 156 quat_traits<R>::template write_element<1>(r)=s*t; 157 quat_traits<R>::template write_element<2>(r)=(mat[1][0]+mat[0][1])*s; 158 quat_traits<R>::template write_element<3>(r)=(mat[0][2]+mat[2][0])*s; 159 } 160 else if( mat[1][1]>mat[2][2] ) 161 { 162 T t = - mat[0][0] + mat[1][1] - mat[2][2] + scalar_traits<T>::value(1); 163 T s = (scalar_traits<T>::value(1)/sqrt<T>(t))/2; 164 quat_traits<R>::template write_element<0>(r)=(mat[0][2]-mat[2][0])*s; 165 quat_traits<R>::template write_element<1>(r)=(mat[1][0]+mat[0][1])*s; 166 quat_traits<R>::template write_element<2>(r)=s*t; 167 quat_traits<R>::template write_element<3>(r)=(mat[2][1]+mat[1][2])*s; 168 } 169 else 170 { 171 T t = - mat[0][0] - mat[1][1] + mat[2][2] + scalar_traits<T>::value(1); 172 T s = (scalar_traits<T>::value(1)/sqrt<T>(t))/2; 173 quat_traits<R>::template write_element<0>(r)=(mat[1][0]-mat[0][1])*s; 174 quat_traits<R>::template write_element<1>(r)=(mat[0][2]+mat[2][0])*s; 175 quat_traits<R>::template write_element<2>(r)=(mat[2][1]+mat[1][2])*s; 176 quat_traits<R>::template write_element<3>(r)=s*t; 177 } 178 return r; 179 } 180 181 //////////////////////////////////////////////// 182 183 template <class A> 184 BOOST_QVM_INLINE_OPERATIONS 185 typename lazy_enable_if_c< 186 is_quat<A>::value, 187 deduce_quat<A> >::type conjugate(A const & a)188 conjugate( A const & a ) 189 { 190 typedef typename deduce_quat<A>::type R; 191 R r; 192 quat_traits<R>::template write_element<0>(r)=quat_traits<A>::template read_element<0>(a); 193 quat_traits<R>::template write_element<1>(r)=-quat_traits<A>::template read_element<1>(a); 194 quat_traits<R>::template write_element<2>(r)=-quat_traits<A>::template read_element<2>(a); 195 quat_traits<R>::template write_element<3>(r)=-quat_traits<A>::template read_element<3>(a); 196 return r; 197 } 198 199 //////////////////////////////////////////////// 200 201 namespace 202 qvm_detail 203 { 204 template <class T> 205 class 206 identity_quat_ 207 { 208 identity_quat_( identity_quat_ const & ); 209 identity_quat_ & operator=( identity_quat_ const & ); 210 ~identity_quat_(); 211 212 public: 213 214 template <class R> 215 BOOST_QVM_INLINE_TRIVIAL operator R() const216 operator R() const 217 { 218 R r; 219 assign(r,*this); 220 return r; 221 } 222 }; 223 } 224 225 template <class T> 226 struct 227 quat_traits< qvm_detail::identity_quat_<T> > 228 { 229 typedef qvm_detail::identity_quat_<T> this_quaternion; 230 typedef T scalar_type; 231 232 template <int I> 233 static 234 BOOST_QVM_INLINE_CRITICAL 235 scalar_type read_elementboost::qvm::quat_traits236 read_element( this_quaternion const & x ) 237 { 238 BOOST_QVM_STATIC_ASSERT(I>=0); 239 BOOST_QVM_STATIC_ASSERT(I<4); 240 return scalar_traits<T>::value(I==0); 241 } 242 243 static 244 BOOST_QVM_INLINE_CRITICAL 245 scalar_type read_element_idxboost::qvm::quat_traits246 read_element_idx( int i, this_quaternion const & x ) 247 { 248 BOOST_QVM_ASSERT(i>=0); 249 BOOST_QVM_ASSERT(i<4); 250 return scalar_traits<T>::value(i==0); 251 } 252 }; 253 254 template <class T> 255 struct 256 deduce_quat< qvm_detail::identity_quat_<T> > 257 { 258 typedef quat<T> type; 259 }; 260 261 template <class T> 262 struct 263 deduce_quat2< qvm_detail::identity_quat_<T>, qvm_detail::identity_quat_<T> > 264 { 265 typedef quat<T> type; 266 }; 267 268 template <class T> 269 BOOST_QVM_INLINE_TRIVIAL 270 qvm_detail::identity_quat_<T> const & identity_quat()271 identity_quat() 272 { 273 return *(qvm_detail::identity_quat_<T> const *)qvm_detail::get_valid_ptr_quat_operations(); 274 } 275 276 template <class A> 277 BOOST_QVM_INLINE_OPERATIONS 278 typename enable_if_c< 279 is_quat<A>::value, 280 void>::type set_identity(A & a)281 set_identity( A & a ) 282 { 283 typedef typename quat_traits<A>::scalar_type T; 284 T const zero=scalar_traits<T>::value(0); 285 T const one=scalar_traits<T>::value(1); 286 quat_traits<A>::template write_element<0>(a) = one; 287 quat_traits<A>::template write_element<1>(a) = zero; 288 quat_traits<A>::template write_element<2>(a) = zero; 289 quat_traits<A>::template write_element<3>(a) = zero; 290 } 291 292 //////////////////////////////////////////////// 293 294 namespace 295 qvm_detail 296 { 297 template <class OriginalType,class Scalar> 298 class 299 quaternion_scalar_cast_ 300 { 301 quaternion_scalar_cast_( quaternion_scalar_cast_ const & ); 302 quaternion_scalar_cast_ & operator=( quaternion_scalar_cast_ const & ); 303 ~quaternion_scalar_cast_(); 304 305 public: 306 307 template <class T> 308 BOOST_QVM_INLINE_TRIVIAL 309 quaternion_scalar_cast_ & operator =(T const & x)310 operator=( T const & x ) 311 { 312 assign(*this,x); 313 return *this; 314 } 315 316 template <class R> 317 BOOST_QVM_INLINE_TRIVIAL operator R() const318 operator R() const 319 { 320 R r; 321 assign(r,*this); 322 return r; 323 } 324 }; 325 326 template <bool> struct scalar_cast_quaternion_filter { }; 327 template <> struct scalar_cast_quaternion_filter<true> { typedef int type; }; 328 } 329 330 template <class OriginalType,class Scalar> 331 struct 332 quat_traits< qvm_detail::quaternion_scalar_cast_<OriginalType,Scalar> > 333 { 334 typedef Scalar scalar_type; 335 typedef qvm_detail::quaternion_scalar_cast_<OriginalType,Scalar> this_quaternion; 336 337 template <int I> 338 static 339 BOOST_QVM_INLINE_CRITICAL 340 scalar_type read_elementboost::qvm::quat_traits341 read_element( this_quaternion const & x ) 342 { 343 BOOST_QVM_STATIC_ASSERT(I>=0); 344 BOOST_QVM_STATIC_ASSERT(I<4); 345 return scalar_type(quat_traits<OriginalType>::template read_element<I>(reinterpret_cast<OriginalType const &>(x))); 346 } 347 348 static 349 BOOST_QVM_INLINE_CRITICAL 350 scalar_type read_element_idxboost::qvm::quat_traits351 read_element_idx( int i, this_quaternion const & x ) 352 { 353 BOOST_QVM_ASSERT(i>=0); 354 BOOST_QVM_ASSERT(i<4); 355 return scalar_type(quat_traits<OriginalType>::read_element_idx(i,reinterpret_cast<OriginalType const &>(x))); 356 } 357 }; 358 359 template <class Scalar,class T> 360 BOOST_QVM_INLINE_TRIVIAL 361 qvm_detail::quaternion_scalar_cast_<T,Scalar> const & scalar_cast(T const & x,typename qvm_detail::scalar_cast_quaternion_filter<is_quat<T>::value>::type=0)362 scalar_cast( T const & x, typename qvm_detail::scalar_cast_quaternion_filter<is_quat<T>::value>::type=0 ) 363 { 364 return reinterpret_cast<qvm_detail::quaternion_scalar_cast_<T,Scalar> const &>(x); 365 } 366 367 //////////////////////////////////////////////// 368 369 template <class A,class B> 370 BOOST_QVM_INLINE_OPERATIONS 371 typename enable_if_c< 372 is_quat<A>::value && is_scalar<B>::value, 373 A &>::type operator /=(A & a,B b)374 operator/=( A & a, B b ) 375 { 376 quat_traits<A>::template write_element<0>(a)/=b; 377 quat_traits<A>::template write_element<1>(a)/=b; 378 quat_traits<A>::template write_element<2>(a)/=b; 379 quat_traits<A>::template write_element<3>(a)/=b; 380 return a; 381 } 382 383 template <class A,class B> 384 BOOST_QVM_INLINE_OPERATIONS 385 typename lazy_enable_if_c< 386 is_quat<A>::value && is_scalar<B>::value, 387 deduce_quat<A> >::type operator /(A const & a,B b)388 operator/( A const & a, B b ) 389 { 390 typedef typename deduce_quat<A>::type R; 391 R r; 392 quat_traits<R>::template write_element<0>(r) = quat_traits<A>::template read_element<0>(a)/b; 393 quat_traits<R>::template write_element<1>(r) = quat_traits<A>::template read_element<1>(a)/b; 394 quat_traits<R>::template write_element<2>(r) = quat_traits<A>::template read_element<2>(a)/b; 395 quat_traits<R>::template write_element<3>(r) = quat_traits<A>::template read_element<3>(a)/b; 396 return r; 397 } 398 399 template <class A,class B> 400 BOOST_QVM_INLINE_OPERATIONS 401 typename lazy_enable_if_c< 402 is_quat<A>::value && is_quat<B>::value, 403 deduce_scalar<typename quat_traits<A>::scalar_type,typename quat_traits<B>::scalar_type> >::type dot(A const & a,B const & b)404 dot( A const & a, B const & b ) 405 { 406 typedef typename quat_traits<A>::scalar_type Ta; 407 typedef typename quat_traits<B>::scalar_type Tb; 408 typedef typename deduce_scalar<Ta,Tb>::type Tr; 409 Ta const a0=quat_traits<A>::template read_element<0>(a); 410 Ta const a1=quat_traits<A>::template read_element<1>(a); 411 Ta const a2=quat_traits<A>::template read_element<2>(a); 412 Ta const a3=quat_traits<A>::template read_element<3>(a); 413 Tb const b0=quat_traits<B>::template read_element<0>(b); 414 Tb const b1=quat_traits<B>::template read_element<1>(b); 415 Tb const b2=quat_traits<B>::template read_element<2>(b); 416 Tb const b3=quat_traits<B>::template read_element<3>(b); 417 Tr const dp=a0*b0+a1*b1+a2*b2+a3*b3; 418 return dp; 419 } 420 421 template <class A,class B> 422 BOOST_QVM_INLINE_OPERATIONS 423 typename enable_if_c< 424 is_quat<A>::value && is_quat<B>::value, 425 bool>::type operator ==(A const & a,B const & b)426 operator==( A const & a, B const & b ) 427 { 428 return 429 quat_traits<A>::template read_element<0>(a)==quat_traits<B>::template read_element<0>(b) && 430 quat_traits<A>::template read_element<1>(a)==quat_traits<B>::template read_element<1>(b) && 431 quat_traits<A>::template read_element<2>(a)==quat_traits<B>::template read_element<2>(b) && 432 quat_traits<A>::template read_element<3>(a)==quat_traits<B>::template read_element<3>(b); 433 } 434 435 template <class A> 436 BOOST_QVM_INLINE_OPERATIONS 437 typename lazy_enable_if_c< 438 is_quat<A>::value, 439 deduce_quat<A> >::type inverse(A const & a)440 inverse( A const & a ) 441 { 442 typedef typename deduce_quat<A>::type R; 443 typedef typename quat_traits<A>::scalar_type TA; 444 TA aa = quat_traits<A>::template read_element<0>(a); 445 TA ab = quat_traits<A>::template read_element<1>(a); 446 TA ac = quat_traits<A>::template read_element<2>(a); 447 TA ad = quat_traits<A>::template read_element<3>(a); 448 TA m2 = ab*ab + ac*ac + ad*ad + aa*aa; 449 if( m2==scalar_traits<TA>::value(0) ) 450 BOOST_QVM_THROW_EXCEPTION(zero_magnitude_error()); 451 TA rm=scalar_traits<TA>::value(1)/m2; 452 R r; 453 quat_traits<R>::template write_element<0>(r) = aa*rm; 454 quat_traits<R>::template write_element<1>(r) = -ab*rm; 455 quat_traits<R>::template write_element<2>(r) = -ac*rm; 456 quat_traits<R>::template write_element<3>(r) = -ad*rm; 457 return r; 458 } 459 460 template <class A> 461 BOOST_QVM_INLINE_OPERATIONS 462 typename enable_if_c< 463 is_quat<A>::value, 464 typename quat_traits<A>::scalar_type>::type mag_sqr(A const & a)465 mag_sqr( A const & a ) 466 { 467 typedef typename quat_traits<A>::scalar_type T; 468 T x=quat_traits<A>::template read_element<0>(a); 469 T y=quat_traits<A>::template read_element<1>(a); 470 T z=quat_traits<A>::template read_element<2>(a); 471 T w=quat_traits<A>::template read_element<3>(a); 472 return x*x+y*y+z*z+w*w; 473 } 474 475 template <class A> 476 BOOST_QVM_INLINE_OPERATIONS 477 typename enable_if_c< 478 is_quat<A>::value, 479 typename quat_traits<A>::scalar_type>::type mag(A const & a)480 mag( A const & a ) 481 { 482 typedef typename quat_traits<A>::scalar_type T; 483 T x=quat_traits<A>::template read_element<0>(a); 484 T y=quat_traits<A>::template read_element<1>(a); 485 T z=quat_traits<A>::template read_element<2>(a); 486 T w=quat_traits<A>::template read_element<3>(a); 487 return sqrt<T>(x*x+y*y+z*z+w*w); 488 } 489 490 template <class A,class B> 491 BOOST_QVM_INLINE_OPERATIONS 492 typename enable_if< 493 msvc_parse_bug_workaround::quats<A,B>, 494 A &>::type operator -=(A & a,B const & b)495 operator-=( A & a, B const & b ) 496 { 497 quat_traits<A>::template write_element<0>(a)-=quat_traits<B>::template read_element<0>(b); 498 quat_traits<A>::template write_element<1>(a)-=quat_traits<B>::template read_element<1>(b); 499 quat_traits<A>::template write_element<2>(a)-=quat_traits<B>::template read_element<2>(b); 500 quat_traits<A>::template write_element<3>(a)-=quat_traits<B>::template read_element<3>(b); 501 return a; 502 } 503 504 template <class A,class B> 505 BOOST_QVM_INLINE_OPERATIONS 506 typename lazy_enable_if_c< 507 is_quat<A>::value && is_quat<B>::value, 508 deduce_quat2<A,B> >::type operator -(A const & a,B const & b)509 operator-( A const & a, B const & b ) 510 { 511 typedef typename deduce_quat2<A,B>::type R; 512 R r; 513 quat_traits<R>::template write_element<0>(r)=quat_traits<A>::template read_element<0>(a)-quat_traits<B>::template read_element<0>(b); 514 quat_traits<R>::template write_element<1>(r)=quat_traits<A>::template read_element<1>(a)-quat_traits<B>::template read_element<1>(b); 515 quat_traits<R>::template write_element<2>(r)=quat_traits<A>::template read_element<2>(a)-quat_traits<B>::template read_element<2>(b); 516 quat_traits<R>::template write_element<3>(r)=quat_traits<A>::template read_element<3>(a)-quat_traits<B>::template read_element<3>(b); 517 return r; 518 } 519 520 template <class A> 521 BOOST_QVM_INLINE_OPERATIONS 522 typename lazy_enable_if_c< 523 is_quat<A>::value, 524 deduce_quat<A> >::type operator -(A const & a)525 operator-( A const & a ) 526 { 527 typedef typename deduce_quat<A>::type R; 528 R r; 529 quat_traits<R>::template write_element<0>(r)=-quat_traits<A>::template read_element<0>(a); 530 quat_traits<R>::template write_element<1>(r)=-quat_traits<A>::template read_element<1>(a); 531 quat_traits<R>::template write_element<2>(r)=-quat_traits<A>::template read_element<2>(a); 532 quat_traits<R>::template write_element<3>(r)=-quat_traits<A>::template read_element<3>(a); 533 return r; 534 } 535 536 template <class A,class B> 537 BOOST_QVM_INLINE_OPERATIONS 538 typename enable_if< 539 msvc_parse_bug_workaround::quats<A,B>, 540 A &>::type operator *=(A & a,B const & b)541 operator*=( A & a, B const & b ) 542 { 543 typedef typename quat_traits<A>::scalar_type TA; 544 typedef typename quat_traits<B>::scalar_type TB; 545 TA const aa=quat_traits<A>::template read_element<0>(a); 546 TA const ab=quat_traits<A>::template read_element<1>(a); 547 TA const ac=quat_traits<A>::template read_element<2>(a); 548 TA const ad=quat_traits<A>::template read_element<3>(a); 549 TB const ba=quat_traits<B>::template read_element<0>(b); 550 TB const bb=quat_traits<B>::template read_element<1>(b); 551 TB const bc=quat_traits<B>::template read_element<2>(b); 552 TB const bd=quat_traits<B>::template read_element<3>(b); 553 quat_traits<A>::template write_element<0>(a) = aa*ba - ab*bb - ac*bc - ad*bd; 554 quat_traits<A>::template write_element<1>(a) = aa*bb + ab*ba + ac*bd - ad*bc; 555 quat_traits<A>::template write_element<2>(a) = aa*bc + ac*ba + ad*bb - ab*bd; 556 quat_traits<A>::template write_element<3>(a) = aa*bd + ad*ba + ab*bc - ac*bb; 557 return a; 558 } 559 560 template <class A,class B> 561 BOOST_QVM_INLINE_OPERATIONS 562 typename enable_if_c< 563 is_quat<A>::value && is_scalar<B>::value, 564 A &>::type operator *=(A & a,B b)565 operator*=( A & a, B b ) 566 { 567 quat_traits<A>::template write_element<0>(a)*=b; 568 quat_traits<A>::template write_element<1>(a)*=b; 569 quat_traits<A>::template write_element<2>(a)*=b; 570 quat_traits<A>::template write_element<3>(a)*=b; 571 return a; 572 } 573 574 template <class A,class B> 575 BOOST_QVM_INLINE_OPERATIONS 576 typename lazy_enable_if_c< 577 is_quat<A>::value && is_quat<B>::value, 578 deduce_quat2<A,B> >::type operator *(A const & a,B const & b)579 operator*( A const & a, B const & b ) 580 { 581 typedef typename deduce_quat2<A,B>::type R; 582 typedef typename quat_traits<A>::scalar_type TA; 583 typedef typename quat_traits<B>::scalar_type TB; 584 TA const aa=quat_traits<A>::template read_element<0>(a); 585 TA const ab=quat_traits<A>::template read_element<1>(a); 586 TA const ac=quat_traits<A>::template read_element<2>(a); 587 TA const ad=quat_traits<A>::template read_element<3>(a); 588 TB const ba=quat_traits<B>::template read_element<0>(b); 589 TB const bb=quat_traits<B>::template read_element<1>(b); 590 TB const bc=quat_traits<B>::template read_element<2>(b); 591 TB const bd=quat_traits<B>::template read_element<3>(b); 592 R r; 593 quat_traits<R>::template write_element<0>(r) = aa*ba - ab*bb - ac*bc - ad*bd; 594 quat_traits<R>::template write_element<1>(r) = aa*bb + ab*ba + ac*bd - ad*bc; 595 quat_traits<R>::template write_element<2>(r) = aa*bc + ac*ba + ad*bb - ab*bd; 596 quat_traits<R>::template write_element<3>(r) = aa*bd + ad*ba + ab*bc - ac*bb; 597 return r; 598 } 599 600 template <class A,class B> 601 BOOST_QVM_INLINE_OPERATIONS 602 typename lazy_enable_if_c< 603 is_quat<A>::value && is_scalar<B>::value, 604 deduce_quat<A> >::type operator *(A const & a,B b)605 operator*( A const & a, B b ) 606 { 607 typedef typename deduce_quat<A>::type R; 608 R r; 609 quat_traits<R>::template write_element<0>(r)=quat_traits<A>::template read_element<0>(a)*b; 610 quat_traits<R>::template write_element<1>(r)=quat_traits<A>::template read_element<1>(a)*b; 611 quat_traits<R>::template write_element<2>(r)=quat_traits<A>::template read_element<2>(a)*b; 612 quat_traits<R>::template write_element<3>(r)=quat_traits<A>::template read_element<3>(a)*b; 613 return r; 614 } 615 616 template <class A,class B> 617 BOOST_QVM_INLINE_OPERATIONS 618 typename enable_if_c< 619 is_quat<A>::value && is_quat<B>::value, 620 bool>::type operator !=(A const & a,B const & b)621 operator!=( A const & a, B const & b ) 622 { 623 return 624 quat_traits<A>::template read_element<0>(a)!=quat_traits<B>::template read_element<0>(b) || 625 quat_traits<A>::template read_element<1>(a)!=quat_traits<B>::template read_element<1>(b) || 626 quat_traits<A>::template read_element<2>(a)!=quat_traits<B>::template read_element<2>(b) || 627 quat_traits<A>::template read_element<3>(a)!=quat_traits<B>::template read_element<3>(b); 628 } 629 630 template <class A> 631 BOOST_QVM_INLINE_OPERATIONS 632 typename lazy_enable_if_c< 633 is_quat<A>::value, 634 deduce_quat<A> >::type normalized(A const & a)635 normalized( A const & a ) 636 { 637 typedef typename quat_traits<A>::scalar_type T; 638 T const a0=quat_traits<A>::template read_element<0>(a); 639 T const a1=quat_traits<A>::template read_element<1>(a); 640 T const a2=quat_traits<A>::template read_element<2>(a); 641 T const a3=quat_traits<A>::template read_element<3>(a); 642 T const m2=a0*a0+a1*a1+a2*a2+a3*a3; 643 if( m2==scalar_traits<typename quat_traits<A>::scalar_type>::value(0) ) 644 BOOST_QVM_THROW_EXCEPTION(zero_magnitude_error()); 645 T const rm=scalar_traits<T>::value(1)/sqrt<T>(m2); 646 typedef typename deduce_quat<A>::type R; 647 R r; 648 quat_traits<R>::template write_element<0>(r)=a0*rm; 649 quat_traits<R>::template write_element<1>(r)=a1*rm; 650 quat_traits<R>::template write_element<2>(r)=a2*rm; 651 quat_traits<R>::template write_element<3>(r)=a3*rm; 652 return r; 653 } 654 655 template <class A> 656 BOOST_QVM_INLINE_OPERATIONS 657 typename enable_if_c< 658 is_quat<A>::value, 659 void>::type normalize(A & a)660 normalize( A & a ) 661 { 662 typedef typename quat_traits<A>::scalar_type T; 663 T const a0=quat_traits<A>::template read_element<0>(a); 664 T const a1=quat_traits<A>::template read_element<1>(a); 665 T const a2=quat_traits<A>::template read_element<2>(a); 666 T const a3=quat_traits<A>::template read_element<3>(a); 667 T const m2=a0*a0+a1*a1+a2*a2+a3*a3; 668 if( m2==scalar_traits<typename quat_traits<A>::scalar_type>::value(0) ) 669 BOOST_QVM_THROW_EXCEPTION(zero_magnitude_error()); 670 T const rm=scalar_traits<T>::value(1)/sqrt<T>(m2); 671 quat_traits<A>::template write_element<0>(a)*=rm; 672 quat_traits<A>::template write_element<1>(a)*=rm; 673 quat_traits<A>::template write_element<2>(a)*=rm; 674 quat_traits<A>::template write_element<3>(a)*=rm; 675 } 676 677 template <class A,class B> 678 BOOST_QVM_INLINE_OPERATIONS 679 typename enable_if< 680 msvc_parse_bug_workaround::quats<A,B>, 681 A &>::type operator +=(A & a,B const & b)682 operator+=( A & a, B const & b ) 683 { 684 quat_traits<A>::template write_element<0>(a)+=quat_traits<B>::template read_element<0>(b); 685 quat_traits<A>::template write_element<1>(a)+=quat_traits<B>::template read_element<1>(b); 686 quat_traits<A>::template write_element<2>(a)+=quat_traits<B>::template read_element<2>(b); 687 quat_traits<A>::template write_element<3>(a)+=quat_traits<B>::template read_element<3>(b); 688 return a; 689 } 690 691 template <class A,class B> 692 BOOST_QVM_INLINE_OPERATIONS 693 typename lazy_enable_if_c< 694 is_quat<A>::value && is_quat<B>::value, 695 deduce_quat2<A,B> >::type operator +(A const & a,B const & b)696 operator+( A const & a, B const & b ) 697 { 698 typedef typename deduce_quat2<A,B>::type R; 699 R r; 700 quat_traits<R>::template write_element<0>(r)=quat_traits<A>::template read_element<0>(a)+quat_traits<B>::template read_element<0>(b); 701 quat_traits<R>::template write_element<1>(r)=quat_traits<A>::template read_element<1>(a)+quat_traits<B>::template read_element<1>(b); 702 quat_traits<R>::template write_element<2>(r)=quat_traits<A>::template read_element<2>(a)+quat_traits<B>::template read_element<2>(b); 703 quat_traits<R>::template write_element<3>(r)=quat_traits<A>::template read_element<3>(a)+quat_traits<B>::template read_element<3>(b); 704 return r; 705 } 706 707 template <class A,class B,class C> 708 BOOST_QVM_INLINE_OPERATIONS 709 typename lazy_enable_if_c< 710 is_quat<A>::value && is_quat<B>::value && is_scalar<C>::value, 711 deduce_quat2<A,B> >::type slerp(A const & a,B const & b,C t)712 slerp( A const & a, B const & b, C t ) 713 { 714 typedef typename deduce_quat2<A,B>::type R; 715 typedef typename quat_traits<R>::scalar_type TR; 716 TR const one = scalar_traits<TR>::value(1); 717 TR dp = dot(a,b); 718 TR sc=one; 719 if( dp < one ) 720 { 721 TR const theta = acos<TR>(dp); 722 TR const invsintheta = one/sin<TR>(theta); 723 TR const scale = sin<TR>(theta*(one-t)) * invsintheta; 724 TR const invscale = sin<TR>(theta*t) * invsintheta * sc; 725 return a*scale + b*invscale; 726 } 727 else 728 return normalized(a+(b-a)*t); 729 } 730 731 //////////////////////////////////////////////// 732 733 namespace 734 qvm_detail 735 { 736 template <class T> 737 class 738 qref_ 739 { 740 qref_( qref_ const & ); 741 qref_ & operator=( qref_ const & ); 742 ~qref_(); 743 744 public: 745 746 template <class R> 747 BOOST_QVM_INLINE_TRIVIAL 748 qref_ & operator =(R const & x)749 operator=( R const & x ) 750 { 751 assign(*this,x); 752 return *this; 753 } 754 755 template <class R> 756 BOOST_QVM_INLINE_TRIVIAL operator R() const757 operator R() const 758 { 759 R r; 760 assign(r,*this); 761 return r; 762 } 763 }; 764 } 765 766 template <class Q> 767 struct quat_traits; 768 769 template <class Q> 770 struct 771 quat_traits< qvm_detail::qref_<Q> > 772 { 773 typedef typename quat_traits<Q>::scalar_type scalar_type; 774 typedef qvm_detail::qref_<Q> this_quaternion; 775 776 template <int I> 777 static 778 BOOST_QVM_INLINE_CRITICAL 779 scalar_type read_elementboost::qvm::quat_traits780 read_element( this_quaternion const & x ) 781 { 782 BOOST_QVM_STATIC_ASSERT(I>=0); 783 BOOST_QVM_STATIC_ASSERT(I<4); 784 return quat_traits<Q>::template read_element<I>(reinterpret_cast<Q const &>(x)); 785 } 786 787 template <int I> 788 static 789 BOOST_QVM_INLINE_CRITICAL 790 scalar_type & write_elementboost::qvm::quat_traits791 write_element( this_quaternion & x ) 792 { 793 BOOST_QVM_STATIC_ASSERT(I>=0); 794 BOOST_QVM_STATIC_ASSERT(I<4); 795 return quat_traits<Q>::template write_element<I>(reinterpret_cast<Q &>(x)); 796 } 797 }; 798 799 template <class Q> 800 struct 801 deduce_quat< qvm_detail::qref_<Q> > 802 { 803 typedef quat<typename quat_traits<Q>::scalar_type> type; 804 }; 805 806 template <class Q> 807 BOOST_QVM_INLINE_TRIVIAL 808 typename enable_if_c< 809 is_quat<Q>::value, 810 qvm_detail::qref_<Q> const &>::type qref(Q const & a)811 qref( Q const & a ) 812 { 813 return reinterpret_cast<qvm_detail::qref_<Q> const &>(a); 814 } 815 816 template <class Q> 817 BOOST_QVM_INLINE_TRIVIAL 818 typename enable_if_c< 819 is_quat<Q>::value, 820 qvm_detail::qref_<Q> &>::type qref(Q & a)821 qref( Q & a ) 822 { 823 return reinterpret_cast<qvm_detail::qref_<Q> &>(a); 824 } 825 826 //////////////////////////////////////////////// 827 828 namespace 829 qvm_detail 830 { 831 template <class T> 832 class 833 zero_q_ 834 { 835 zero_q_( zero_q_ const & ); 836 zero_q_ & operator=( zero_q_ const & ); 837 ~zero_q_(); 838 839 public: 840 841 template <class R> 842 BOOST_QVM_INLINE_TRIVIAL operator R() const843 operator R() const 844 { 845 R r; 846 assign(r,*this); 847 return r; 848 } 849 }; 850 } 851 852 template <class T> 853 struct 854 quat_traits< qvm_detail::zero_q_<T> > 855 { 856 typedef qvm_detail::zero_q_<T> this_quaternion; 857 typedef T scalar_type; 858 859 template <int I> 860 static 861 BOOST_QVM_INLINE_CRITICAL 862 scalar_type read_elementboost::qvm::quat_traits863 read_element( this_quaternion const & x ) 864 { 865 BOOST_QVM_STATIC_ASSERT(I>=0); 866 BOOST_QVM_STATIC_ASSERT(I<4); 867 return scalar_traits<scalar_type>::value(0); 868 } 869 870 static 871 BOOST_QVM_INLINE_CRITICAL 872 scalar_type read_element_idxboost::qvm::quat_traits873 read_element_idx( int i, this_quaternion const & x ) 874 { 875 BOOST_QVM_ASSERT(i>=0); 876 BOOST_QVM_ASSERT(i<4); 877 return scalar_traits<scalar_type>::value(0); 878 } 879 }; 880 881 template <class T> 882 BOOST_QVM_INLINE_TRIVIAL 883 qvm_detail::zero_q_<T> const & zero_quat()884 zero_quat() 885 { 886 return *(qvm_detail::zero_q_<T> const *)qvm_detail::get_valid_ptr_quat_operations(); 887 } 888 889 template <class A> 890 BOOST_QVM_INLINE_OPERATIONS 891 typename enable_if_c< 892 is_quat<A>::value, 893 void>::type set_zero(A & a)894 set_zero( A & a ) 895 { 896 typedef typename quat_traits<A>::scalar_type T; 897 T const zero=scalar_traits<T>::value(0); 898 quat_traits<A>::template write_element<0>(a) = zero; 899 quat_traits<A>::template write_element<1>(a) = zero; 900 quat_traits<A>::template write_element<2>(a) = zero; 901 quat_traits<A>::template write_element<3>(a) = zero; 902 } 903 904 //////////////////////////////////////////////// 905 906 namespace 907 qvm_detail 908 { 909 template <class V> 910 struct 911 rot_quat_ 912 { 913 typedef typename vec_traits<V>::scalar_type scalar_type; 914 scalar_type a[4]; 915 916 template <class Angle> 917 BOOST_QVM_INLINE rot_quat_boost::qvm::qvm_detail::rot_quat_918 rot_quat_( V const & axis, Angle angle ) 919 { 920 scalar_type const x=vec_traits<V>::template read_element<0>(axis); 921 scalar_type const y=vec_traits<V>::template read_element<1>(axis); 922 scalar_type const z=vec_traits<V>::template read_element<2>(axis); 923 scalar_type const m2=x*x+y*y+z*z; 924 if( m2==scalar_traits<scalar_type>::value(0) ) 925 BOOST_QVM_THROW_EXCEPTION(zero_magnitude_error()); 926 scalar_type const rm=scalar_traits<scalar_type>::value(1)/sqrt<scalar_type>(m2); 927 angle/=2; 928 scalar_type const s=sin<Angle>(angle); 929 a[0] = cos<Angle>(angle); 930 a[1] = rm*x*s; 931 a[2] = rm*y*s; 932 a[3] = rm*z*s; 933 } 934 935 template <class R> 936 BOOST_QVM_INLINE_TRIVIAL operator Rboost::qvm::qvm_detail::rot_quat_937 operator R() const 938 { 939 R r; 940 assign(r,*this); 941 return r; 942 } 943 }; 944 } 945 946 template <class V> 947 struct 948 quat_traits< qvm_detail::rot_quat_<V> > 949 { 950 typedef qvm_detail::rot_quat_<V> this_quaternion; 951 typedef typename this_quaternion::scalar_type scalar_type; 952 953 template <int I> 954 static 955 BOOST_QVM_INLINE_CRITICAL 956 scalar_type read_elementboost::qvm::quat_traits957 read_element( this_quaternion const & x ) 958 { 959 BOOST_QVM_STATIC_ASSERT(I>=0); 960 BOOST_QVM_STATIC_ASSERT(I<4); 961 return x.a[I]; 962 } 963 }; 964 965 template <class V> 966 struct 967 deduce_quat< qvm_detail::rot_quat_<V> > 968 { 969 typedef quat<typename vec_traits<V>::scalar_type> type; 970 }; 971 972 template <class A,class Angle> 973 BOOST_QVM_INLINE 974 typename enable_if_c< 975 is_vec<A>::value && vec_traits<A>::dim==3, 976 qvm_detail::rot_quat_<A> >::type rot_quat(A const & axis,Angle angle)977 rot_quat( A const & axis, Angle angle ) 978 { 979 return qvm_detail::rot_quat_<A>(axis,angle); 980 } 981 982 template <class A,class B,class Angle> 983 BOOST_QVM_INLINE_OPERATIONS 984 typename enable_if_c< 985 is_quat<A>::value && 986 is_vec<B>::value && vec_traits<B>::dim==3, 987 void>::type set_rot(A & a,B const & axis,Angle angle)988 set_rot( A & a, B const & axis, Angle angle ) 989 { 990 assign(a,rot_quat(axis,angle)); 991 } 992 993 template <class A,class B,class Angle> 994 BOOST_QVM_INLINE_OPERATIONS 995 typename enable_if_c< 996 is_quat<A>::value && 997 is_vec<B>::value && vec_traits<B>::dim==3, 998 void>::type rotate(A & a,B const & axis,Angle angle)999 rotate( A & a, B const & axis, Angle angle ) 1000 { 1001 a *= rot_quat(axis,angle); 1002 } 1003 1004 //////////////////////////////////////////////// 1005 1006 namespace 1007 qvm_detail 1008 { 1009 template <class T> 1010 struct 1011 rotx_quat_ 1012 { 1013 BOOST_QVM_INLINE_TRIVIAL rotx_quat_boost::qvm::qvm_detail::rotx_quat_1014 rotx_quat_() 1015 { 1016 } 1017 1018 template <class R> 1019 BOOST_QVM_INLINE_TRIVIAL operator Rboost::qvm::qvm_detail::rotx_quat_1020 operator R() const 1021 { 1022 R r; 1023 assign(r,*this); 1024 return r; 1025 } 1026 1027 private: 1028 1029 rotx_quat_( rotx_quat_ const & ); 1030 rotx_quat_ & operator=( rotx_quat_ const & ); 1031 ~rotx_quat_(); 1032 }; 1033 1034 template <int I> 1035 struct 1036 rotx_q_get 1037 { 1038 template <class T> 1039 static 1040 BOOST_QVM_INLINE_CRITICAL 1041 T getboost::qvm::qvm_detail::rotx_q_get1042 get( T const & ) 1043 { 1044 return scalar_traits<T>::value(0); 1045 } 1046 }; 1047 1048 template <> 1049 struct 1050 rotx_q_get<1> 1051 { 1052 template <class T> 1053 static 1054 BOOST_QVM_INLINE_CRITICAL 1055 T getboost::qvm::qvm_detail::rotx_q_get1056 get( T const & angle ) 1057 { 1058 return sin<T>(angle/2); 1059 } 1060 }; 1061 1062 template <> 1063 struct 1064 rotx_q_get<0> 1065 { 1066 template <class T> 1067 static 1068 BOOST_QVM_INLINE_CRITICAL 1069 T getboost::qvm::qvm_detail::rotx_q_get1070 get( T const & angle ) 1071 { 1072 return cos<T>(angle/2); 1073 } 1074 }; 1075 } 1076 1077 template <class Angle> 1078 struct 1079 quat_traits< qvm_detail::rotx_quat_<Angle> > 1080 { 1081 typedef qvm_detail::rotx_quat_<Angle> this_quaternion; 1082 typedef Angle scalar_type; 1083 1084 template <int I> 1085 static 1086 BOOST_QVM_INLINE_CRITICAL 1087 scalar_type read_elementboost::qvm::quat_traits1088 read_element( this_quaternion const & x ) 1089 { 1090 BOOST_QVM_STATIC_ASSERT(I>=0); 1091 BOOST_QVM_STATIC_ASSERT(I<4); 1092 return qvm_detail::rotx_q_get<I>::get(reinterpret_cast<Angle const &>(x)); 1093 } 1094 }; 1095 1096 template <class Angle> 1097 struct 1098 deduce_quat< qvm_detail::rotx_quat_<Angle> > 1099 { 1100 typedef quat<Angle> type; 1101 }; 1102 1103 template <class Angle> 1104 struct 1105 deduce_quat2< qvm_detail::rotx_quat_<Angle>, qvm_detail::rotx_quat_<Angle> > 1106 { 1107 typedef quat<Angle> type; 1108 }; 1109 1110 template <class Angle> 1111 BOOST_QVM_INLINE_TRIVIAL 1112 qvm_detail::rotx_quat_<Angle> const & rotx_quat(Angle const & angle)1113 rotx_quat( Angle const & angle ) 1114 { 1115 return reinterpret_cast<qvm_detail::rotx_quat_<Angle> const &>(angle); 1116 } 1117 1118 template <class A,class Angle> 1119 BOOST_QVM_INLINE_OPERATIONS 1120 typename enable_if_c< 1121 is_quat<A>::value, 1122 void>::type set_rotx(A & a,Angle angle)1123 set_rotx( A & a, Angle angle ) 1124 { 1125 assign(a,rotx_quat(angle)); 1126 } 1127 1128 template <class A,class Angle> 1129 BOOST_QVM_INLINE_OPERATIONS 1130 typename enable_if_c< 1131 is_quat<A>::value, 1132 void>::type rotate_x(A & a,Angle angle)1133 rotate_x( A & a, Angle angle ) 1134 { 1135 a *= rotx_quat(angle); 1136 } 1137 1138 //////////////////////////////////////////////// 1139 1140 namespace 1141 qvm_detail 1142 { 1143 template <class T> 1144 struct 1145 roty_quat_ 1146 { 1147 BOOST_QVM_INLINE_TRIVIAL roty_quat_boost::qvm::qvm_detail::roty_quat_1148 roty_quat_() 1149 { 1150 } 1151 1152 template <class R> 1153 BOOST_QVM_INLINE_TRIVIAL operator Rboost::qvm::qvm_detail::roty_quat_1154 operator R() const 1155 { 1156 R r; 1157 assign(r,*this); 1158 return r; 1159 } 1160 1161 private: 1162 1163 roty_quat_( roty_quat_ const & ); 1164 roty_quat_ & operator=( roty_quat_ const & ); 1165 ~roty_quat_(); 1166 }; 1167 1168 template <int I> 1169 struct 1170 roty_q_get 1171 { 1172 template <class T> 1173 static 1174 BOOST_QVM_INLINE_CRITICAL 1175 T getboost::qvm::qvm_detail::roty_q_get1176 get( T const & ) 1177 { 1178 return scalar_traits<T>::value(0); 1179 } 1180 }; 1181 1182 template <> 1183 struct 1184 roty_q_get<2> 1185 { 1186 template <class T> 1187 static 1188 BOOST_QVM_INLINE_CRITICAL 1189 T getboost::qvm::qvm_detail::roty_q_get1190 get( T const & angle ) 1191 { 1192 return sin<T>(angle/2); 1193 } 1194 }; 1195 1196 template <> 1197 struct 1198 roty_q_get<0> 1199 { 1200 template <class T> 1201 static 1202 BOOST_QVM_INLINE_CRITICAL 1203 T getboost::qvm::qvm_detail::roty_q_get1204 get( T const & angle ) 1205 { 1206 return cos<T>(angle/2); 1207 } 1208 }; 1209 } 1210 1211 template <class Angle> 1212 struct 1213 quat_traits< qvm_detail::roty_quat_<Angle> > 1214 { 1215 typedef qvm_detail::roty_quat_<Angle> this_quaternion; 1216 typedef Angle scalar_type; 1217 1218 template <int I> 1219 static 1220 BOOST_QVM_INLINE_CRITICAL 1221 scalar_type read_elementboost::qvm::quat_traits1222 read_element( this_quaternion const & x ) 1223 { 1224 BOOST_QVM_STATIC_ASSERT(I>=0); 1225 BOOST_QVM_STATIC_ASSERT(I<4); 1226 return qvm_detail::roty_q_get<I>::get(reinterpret_cast<Angle const &>(x)); 1227 } 1228 }; 1229 1230 template <class Angle> 1231 struct 1232 deduce_quat< qvm_detail::roty_quat_<Angle> > 1233 { 1234 typedef quat<Angle> type; 1235 }; 1236 1237 template <class Angle> 1238 struct 1239 deduce_quat2< qvm_detail::roty_quat_<Angle>, qvm_detail::roty_quat_<Angle> > 1240 { 1241 typedef quat<Angle> type; 1242 }; 1243 1244 template <class Angle> 1245 BOOST_QVM_INLINE_TRIVIAL 1246 qvm_detail::roty_quat_<Angle> const & roty_quat(Angle const & angle)1247 roty_quat( Angle const & angle ) 1248 { 1249 return reinterpret_cast<qvm_detail::roty_quat_<Angle> const &>(angle); 1250 } 1251 1252 template <class A,class Angle> 1253 BOOST_QVM_INLINE_OPERATIONS 1254 typename enable_if_c< 1255 is_quat<A>::value, 1256 void>::type set_roty(A & a,Angle angle)1257 set_roty( A & a, Angle angle ) 1258 { 1259 assign(a,roty_quat(angle)); 1260 } 1261 1262 template <class A,class Angle> 1263 BOOST_QVM_INLINE_OPERATIONS 1264 typename enable_if_c< 1265 is_quat<A>::value, 1266 void>::type rotate_y(A & a,Angle angle)1267 rotate_y( A & a, Angle angle ) 1268 { 1269 a *= roty_quat(angle); 1270 } 1271 1272 //////////////////////////////////////////////// 1273 1274 namespace 1275 qvm_detail 1276 { 1277 template <class T> 1278 struct 1279 rotz_quat_ 1280 { 1281 BOOST_QVM_INLINE_TRIVIAL rotz_quat_boost::qvm::qvm_detail::rotz_quat_1282 rotz_quat_() 1283 { 1284 } 1285 1286 template <class R> 1287 BOOST_QVM_INLINE_TRIVIAL operator Rboost::qvm::qvm_detail::rotz_quat_1288 operator R() const 1289 { 1290 R r; 1291 assign(r,*this); 1292 return r; 1293 } 1294 1295 private: 1296 1297 rotz_quat_( rotz_quat_ const & ); 1298 rotz_quat_ & operator=( rotz_quat_ const & ); 1299 ~rotz_quat_(); 1300 }; 1301 1302 template <int I> 1303 struct 1304 rotz_q_get 1305 { 1306 template <class T> 1307 static 1308 BOOST_QVM_INLINE_CRITICAL 1309 T getboost::qvm::qvm_detail::rotz_q_get1310 get( T const & ) 1311 { 1312 return scalar_traits<T>::value(0); 1313 } 1314 }; 1315 1316 template <> 1317 struct 1318 rotz_q_get<3> 1319 { 1320 template <class T> 1321 static 1322 BOOST_QVM_INLINE_CRITICAL 1323 T getboost::qvm::qvm_detail::rotz_q_get1324 get( T const & angle ) 1325 { 1326 return sin<T>(angle/2); 1327 } 1328 }; 1329 1330 template <> 1331 struct 1332 rotz_q_get<0> 1333 { 1334 template <class T> 1335 static 1336 BOOST_QVM_INLINE_CRITICAL 1337 T getboost::qvm::qvm_detail::rotz_q_get1338 get( T const & angle ) 1339 { 1340 return cos<T>(angle/2); 1341 } 1342 }; 1343 } 1344 1345 template <class Angle> 1346 struct 1347 quat_traits< qvm_detail::rotz_quat_<Angle> > 1348 { 1349 typedef qvm_detail::rotz_quat_<Angle> this_quaternion; 1350 typedef Angle scalar_type; 1351 1352 template <int I> 1353 static 1354 BOOST_QVM_INLINE_CRITICAL 1355 scalar_type read_elementboost::qvm::quat_traits1356 read_element( this_quaternion const & x ) 1357 { 1358 BOOST_QVM_STATIC_ASSERT(I>=0); 1359 BOOST_QVM_STATIC_ASSERT(I<4); 1360 return qvm_detail::rotz_q_get<I>::get(reinterpret_cast<Angle const &>(x)); 1361 } 1362 }; 1363 1364 template <class Angle> 1365 struct 1366 deduce_quat< qvm_detail::rotz_quat_<Angle> > 1367 { 1368 typedef quat<Angle> type; 1369 }; 1370 1371 template <class Angle> 1372 struct 1373 deduce_quat2< qvm_detail::rotz_quat_<Angle>, qvm_detail::rotz_quat_<Angle> > 1374 { 1375 typedef quat<Angle> type; 1376 }; 1377 1378 template <class Angle> 1379 BOOST_QVM_INLINE_TRIVIAL 1380 qvm_detail::rotz_quat_<Angle> const & rotz_quat(Angle const & angle)1381 rotz_quat( Angle const & angle ) 1382 { 1383 return reinterpret_cast<qvm_detail::rotz_quat_<Angle> const &>(angle); 1384 } 1385 1386 template <class A,class Angle> 1387 BOOST_QVM_INLINE_OPERATIONS 1388 typename enable_if_c< 1389 is_quat<A>::value, 1390 void>::type set_rotz(A & a,Angle angle)1391 set_rotz( A & a, Angle angle ) 1392 { 1393 assign(a,rotz_quat(angle)); 1394 } 1395 1396 template <class A,class Angle> 1397 BOOST_QVM_INLINE_OPERATIONS 1398 typename enable_if_c< 1399 is_quat<A>::value, 1400 void>::type rotate_z(A & a,Angle angle)1401 rotate_z( A & a, Angle angle ) 1402 { 1403 a *= rotz_quat(angle); 1404 } 1405 1406 template <class A,class B> 1407 BOOST_QVM_INLINE_OPERATIONS 1408 typename enable_if_c< 1409 is_quat<A>::value && is_vec<B>::value && vec_traits<B>::dim==3, 1410 typename quat_traits<A>::scalar_type>::type axis_angle(A const & a,B & b)1411 axis_angle( A const & a, B & b ) 1412 { 1413 typedef typename quat_traits<A>::scalar_type T; 1414 T a0=quat_traits<A>::template read_element<0>(a); 1415 T a1=quat_traits<A>::template read_element<1>(a); 1416 T a2=quat_traits<A>::template read_element<2>(a); 1417 T a3=quat_traits<A>::template read_element<3>(a); 1418 if( a0>1 ) 1419 { 1420 T const m2=a0*a0+a1*a1+a2*a2+a3*a3; 1421 if( m2==scalar_traits<T>::value(0) ) 1422 BOOST_QVM_THROW_EXCEPTION(zero_magnitude_error()); 1423 T const s=sqrt<T>(m2); 1424 a0/=s; 1425 a1/=s; 1426 a2/=s; 1427 a3/=s; 1428 } 1429 if( T s=sqrt<T>(1-a0*a0) ) 1430 { 1431 vec_traits<B>::template write_element<0>(b) = a1/s; 1432 vec_traits<B>::template write_element<1>(b) = a2/s; 1433 vec_traits<B>::template write_element<2>(b) = a3/s; 1434 } 1435 else 1436 { 1437 typedef typename vec_traits<B>::scalar_type U; 1438 vec_traits<B>::template write_element<0>(b) = scalar_traits<U>::value(1); 1439 vec_traits<B>::template write_element<1>(b) = vec_traits<B>::template write_element<2>(b) = scalar_traits<U>::value(0); 1440 } 1441 return scalar_traits<T>::value(2) * qvm::acos(a0); 1442 } 1443 1444 //////////////////////////////////////////////// 1445 1446 namespace 1447 sfinae 1448 { 1449 using ::boost::qvm::assign; 1450 using ::boost::qvm::cmp; 1451 using ::boost::qvm::convert_to; 1452 using ::boost::qvm::conjugate; 1453 using ::boost::qvm::set_identity; 1454 using ::boost::qvm::set_zero; 1455 using ::boost::qvm::scalar_cast; 1456 using ::boost::qvm::operator/=; 1457 using ::boost::qvm::operator/; 1458 using ::boost::qvm::dot; 1459 using ::boost::qvm::operator==; 1460 using ::boost::qvm::inverse; 1461 using ::boost::qvm::mag_sqr; 1462 using ::boost::qvm::mag; 1463 using ::boost::qvm::slerp; 1464 using ::boost::qvm::operator-=; 1465 using ::boost::qvm::operator-; 1466 using ::boost::qvm::operator*=; 1467 using ::boost::qvm::operator*; 1468 using ::boost::qvm::operator!=; 1469 using ::boost::qvm::normalized; 1470 using ::boost::qvm::normalize; 1471 using ::boost::qvm::operator+=; 1472 using ::boost::qvm::operator+; 1473 using ::boost::qvm::qref; 1474 using ::boost::qvm::rot_quat; 1475 using ::boost::qvm::set_rot; 1476 using ::boost::qvm::rotate; 1477 using ::boost::qvm::rotx_quat; 1478 using ::boost::qvm::set_rotx; 1479 using ::boost::qvm::rotate_x; 1480 using ::boost::qvm::roty_quat; 1481 using ::boost::qvm::set_roty; 1482 using ::boost::qvm::rotate_y; 1483 using ::boost::qvm::rotz_quat; 1484 using ::boost::qvm::set_rotz; 1485 using ::boost::qvm::rotate_z; 1486 } 1487 1488 //////////////////////////////////////////////// 1489 } 1490 } 1491 1492 #endif 1493