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