ceph/src/boost/boost/qvm/quat_operations.hpp

   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 *
  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
  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
  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
 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
 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
 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
 217                 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
 237             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
 247             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 &
 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
 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_ &
 311                 operator=( T const & x )
 312                     {
 313                     assign(*this,x);
 314                     return *this;
 315                     }
 316
 317                 template <class R>
 318                 BOOST_QVM_INLINE_TRIVIAL
 319                 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
 342             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
 352             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 &
 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
 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
 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
 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
 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
 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
 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
 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
 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
 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
 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
 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
 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
 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
 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
 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
 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
 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
 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
 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
 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_ &
 750                 operator=( R const & x )
 751                     {
 752                     assign(*this,x);
 753                     return *this;
 754                     }
 755
 756                 template <class R>
 757                 BOOST_QVM_INLINE_TRIVIAL
 758                 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
 781             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 &
 792             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
 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
 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
 844                 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
 864             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
 874             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 &
 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
 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
 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
 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
 958             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
 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
 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
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
1015                 rotx_quat_()
1016                     {
1017                     }
1018
1019                 template <class R>
1020                 BOOST_QVM_INLINE_TRIVIAL
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
1043                 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
1057                 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
1071                 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
1089             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 &
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
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
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
1149                 roty_quat_()
1150                     {
1151                     }
1152
1153                 template <class R>
1154                 BOOST_QVM_INLINE_TRIVIAL
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
1177                 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
1191                 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
1205                 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
1223             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 &
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
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
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
1283                 rotz_quat_()
1284                     {
1285                     }
1286
1287                 template <class R>
1288                 BOOST_QVM_INLINE_TRIVIAL
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
1311                 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
1325                 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
1339                 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
1357             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 &
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
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
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
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