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