Back to home page

EIC code displayed by LXR
 
 

    

File indexing completed on 2025-01-18 09:51:08 

0001 #ifndef BOOST_QVM_QUAT_OPERATIONS
0002 #define BOOST_QVM_QUAT_OPERATIONS
0003 
0004 // Copyright 2008-2022 Emil Dotchevski and Reverge Studios, Inc.
0005 
0006 // Distributed under the Boost Software License, Version 1.0. (See accompanying
0007 // file LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)
0008 
0009 #include <boost/qvm/detail/quat_assign.hpp>
0010 #include <boost/qvm/deduce_quat.hpp>
0011 #include <boost/qvm/mat_traits.hpp>
0012 #include <boost/qvm/scalar_traits.hpp>
0013 #include <boost/qvm/math.hpp>
0014 #include <boost/qvm/assert.hpp>
0015 #include <boost/qvm/error.hpp>
0016 #include <boost/qvm/throw_exception.hpp>
0017 #include <boost/qvm/to_string.hpp>
0018 
0019 namespace boost { namespace qvm {
0020 
0021 namespace
0022 qvm_detail
0023     {
0024     BOOST_QVM_INLINE_CRITICAL
0025     void const *
0026     get_valid_ptr_quat_operations()
0027         {
0028         static int const obj=0;
0029         return &obj;
0030         }
0031     }
0032 
0033 ////////////////////////////////////////////////
0034 
0035 namespace
0036 msvc_parse_bug_workaround
0037     {
0038     template <class A,class B>
0039     struct
0040     quats
0041         {
0042         static bool const value=is_quat<A>::value && is_quat<B>::value;
0043         };
0044     }
0045 
0046 
0047 template <class A>
0048 inline
0049 typename enable_if_c<
0050     is_quat<A>::value,
0051     std::string>::type
0052 to_string( A const & a )
0053     {
0054     using namespace qvm_to_string_detail;
0055     return '('+
0056         to_string(quat_traits<A>::template read_element<0>(a))+','+
0057         to_string(quat_traits<A>::template read_element<1>(a))+','+
0058         to_string(quat_traits<A>::template read_element<2>(a))+','+
0059         to_string(quat_traits<A>::template read_element<3>(a))+')';
0060     }
0061 
0062 ////////////////////////////////////////////////
0063 
0064 template <class A,class B,class Cmp>
0065 BOOST_QVM_CONSTEXPR BOOST_QVM_INLINE_OPERATIONS
0066 typename enable_if_c<
0067     is_quat<A>::value && is_quat<B>::value,
0068     bool>::type
0069 cmp( A const & a, B const & b, Cmp f )
0070     {
0071     typedef typename quat_traits<A>::scalar_type T;
0072     typedef typename quat_traits<B>::scalar_type U;
0073     T q1[4] =
0074         {
0075         quat_traits<A>::template read_element<0>(a),
0076         quat_traits<A>::template read_element<1>(a),
0077         quat_traits<A>::template read_element<2>(a),
0078         quat_traits<A>::template read_element<3>(a)
0079         };
0080     U q2[4] =
0081         {
0082         quat_traits<B>::template read_element<0>(b),
0083         quat_traits<B>::template read_element<1>(b),
0084         quat_traits<B>::template read_element<2>(b),
0085         quat_traits<B>::template read_element<3>(b)
0086         };
0087     int i=0;
0088     for( ; i!=4; ++i )
0089         if( !f(q1[i],q2[i]) )
0090             break;
0091     if( i==4 )
0092         return true;
0093     for( i=0; i!=4; ++i )
0094         if( !f(q1[i],-q2[i]) )
0095             return false;
0096     return true;
0097     }
0098 
0099 ////////////////////////////////////////////////
0100 
0101 template <class R,class A>
0102 BOOST_QVM_CONSTEXPR BOOST_QVM_INLINE_TRIVIAL
0103 typename enable_if_c<
0104     is_quat<R>::value && is_quat<A>::value,
0105     R>::type
0106 convert_to( A const & a )
0107     {
0108     R r;
0109     write_quat_element<0>(r,quat_traits<A>::template read_element<0>(a));
0110     write_quat_element<1>(r,quat_traits<A>::template read_element<1>(a));
0111     write_quat_element<2>(r,quat_traits<A>::template read_element<2>(a));
0112     write_quat_element<3>(r,quat_traits<A>::template read_element<3>(a));
0113     return r;
0114     }
0115 
0116 template <class R,class A>
0117 BOOST_QVM_CONSTEXPR BOOST_QVM_INLINE_OPERATIONS
0118 typename enable_if_c<
0119     is_quat<R>::value && is_mat<A>::value &&
0120     mat_traits<A>::rows==3 && mat_traits<A>::cols==3,
0121     R>::type
0122 convert_to( A const & a )
0123     {
0124     typedef typename mat_traits<A>::scalar_type T;
0125     T const mat[3][3] =
0126         {
0127             { 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) },
0128             { 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) },
0129             { 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) }
0130         };
0131     R r;
0132     if( mat[0][0]+mat[1][1]+mat[2][2] > scalar_traits<T>::value(0) )
0133         {
0134         T t = mat[0][0] + mat[1][1] + mat[2][2] + scalar_traits<T>::value(1);
0135         T s = (scalar_traits<T>::value(1)/sqrt(t))/2;
0136         write_quat_element<0>(r,s*t);
0137         write_quat_element<1>(r,(mat[2][1]-mat[1][2])*s);
0138         write_quat_element<2>(r,(mat[0][2]-mat[2][0])*s);
0139         write_quat_element<3>(r,(mat[1][0]-mat[0][1])*s);
0140         }
0141     else if( mat[0][0]>mat[1][1] && mat[0][0]>mat[2][2] )
0142         {
0143         T t = mat[0][0] - mat[1][1] - mat[2][2] + scalar_traits<T>::value(1);
0144         T s = (scalar_traits<T>::value(1)/sqrt(t))/2;
0145         write_quat_element<0>(r,(mat[2][1]-mat[1][2])*s);
0146         write_quat_element<1>(r,s*t);
0147         write_quat_element<2>(r,(mat[1][0]+mat[0][1])*s);
0148         write_quat_element<3>(r,(mat[0][2]+mat[2][0])*s);
0149         }
0150     else if( mat[1][1]>mat[2][2] )
0151         {
0152         T t = - mat[0][0] + mat[1][1] - mat[2][2] + scalar_traits<T>::value(1);
0153         T s = (scalar_traits<T>::value(1)/sqrt(t))/2;
0154         write_quat_element<0>(r,(mat[0][2]-mat[2][0])*s);
0155         write_quat_element<1>(r,(mat[1][0]+mat[0][1])*s);
0156         write_quat_element<2>(r,s*t);
0157         write_quat_element<3>(r,(mat[2][1]+mat[1][2])*s);
0158         }
0159     else
0160         {
0161         T t = - mat[0][0] - mat[1][1] + mat[2][2] + scalar_traits<T>::value(1);
0162         T s = (scalar_traits<T>::value(1)/sqrt(t))/2;
0163         write_quat_element<0>(r,(mat[1][0]-mat[0][1])*s);
0164         write_quat_element<1>(r,(mat[0][2]+mat[2][0])*s);
0165         write_quat_element<2>(r,(mat[2][1]+mat[1][2])*s);
0166         write_quat_element<3>(r,s*t);
0167         }
0168     return r;
0169     }
0170 
0171 ////////////////////////////////////////////////
0172 
0173 template <class A>
0174 BOOST_QVM_CONSTEXPR BOOST_QVM_INLINE_OPERATIONS
0175 typename lazy_enable_if_c<
0176     is_quat<A>::value,
0177     deduce_quat<A> >::type
0178 conjugate( A const & a )
0179     {
0180     typedef typename deduce_quat<A>::type R;
0181     R r;
0182     write_quat_element<0>(r,quat_traits<A>::template read_element<0>(a));
0183     write_quat_element<1>(r,-quat_traits<A>::template read_element<1>(a));
0184     write_quat_element<2>(r,-quat_traits<A>::template read_element<2>(a));
0185     write_quat_element<3>(r,-quat_traits<A>::template read_element<3>(a));
0186     return r;
0187     }
0188 
0189 ////////////////////////////////////////////////
0190 
0191 namespace
0192 qvm_detail
0193     {
0194     template <class T>
0195     class
0196     identity_quat_
0197         {
0198         identity_quat_( identity_quat_ const & );
0199         identity_quat_ & operator=( identity_quat_ const & );
0200         ~identity_quat_();
0201 
0202         public:
0203 
0204         template <class R
0205 #if __cplusplus >= 201103L
0206             , class = typename enable_if<is_quat<R> >::type
0207 #endif
0208         >
0209         BOOST_QVM_CONSTEXPR BOOST_QVM_INLINE_TRIVIAL
0210         operator R() const
0211             {
0212             R r;
0213             assign(r,*this);
0214             return r;
0215             }
0216         };
0217     }
0218 
0219 template <class T>
0220 struct
0221 quat_traits< qvm_detail::identity_quat_<T> >
0222     {
0223     typedef qvm_detail::identity_quat_<T> this_quaternion;
0224     typedef T scalar_type;
0225 
0226     template <int I>
0227     static
0228     BOOST_QVM_CONSTEXPR BOOST_QVM_INLINE_CRITICAL
0229     scalar_type
0230     read_element( this_quaternion const & )
0231         {
0232         BOOST_QVM_STATIC_ASSERT(I>=0);
0233         BOOST_QVM_STATIC_ASSERT(I<4);
0234         return scalar_traits<T>::value(I==0);
0235         }
0236 
0237     static
0238     BOOST_QVM_CONSTEXPR BOOST_QVM_INLINE_CRITICAL
0239     scalar_type
0240     read_element_idx( int i, this_quaternion const & )
0241         {
0242         BOOST_QVM_ASSERT(i>=0);
0243         BOOST_QVM_ASSERT(i<4);
0244         return scalar_traits<T>::value(i==0);
0245         }
0246     };
0247 
0248 template <class T>
0249 struct
0250 deduce_quat< qvm_detail::identity_quat_<T> >
0251     {
0252     typedef quat<T> type;
0253     };
0254 
0255 template <class T>
0256 struct
0257 deduce_quat2< qvm_detail::identity_quat_<T>, qvm_detail::identity_quat_<T> >
0258     {
0259     typedef quat<T> type;
0260     };
0261 
0262 template <class T>
0263 BOOST_QVM_CONSTEXPR BOOST_QVM_INLINE_TRIVIAL
0264 qvm_detail::identity_quat_<T> const &
0265 identity_quat()
0266     {
0267     return *(qvm_detail::identity_quat_<T> const *)qvm_detail::get_valid_ptr_quat_operations();
0268     }
0269 
0270 template <class A>
0271 BOOST_QVM_CONSTEXPR BOOST_QVM_INLINE_OPERATIONS
0272 typename enable_if_c<
0273     is_quat<A>::value,
0274     void>::type
0275 set_identity( A & a )
0276     {
0277     typedef typename quat_traits<A>::scalar_type T;
0278     T const zero=scalar_traits<T>::value(0);
0279     T const one=scalar_traits<T>::value(1);
0280     write_quat_element<0>(a,one);
0281     write_quat_element<1>(a,zero);
0282     write_quat_element<2>(a,zero);
0283     write_quat_element<3>(a,zero);
0284     }
0285 
0286 ////////////////////////////////////////////////
0287 
0288 namespace
0289 qvm_detail
0290     {
0291     template <class OriginalType,class Scalar>
0292     class
0293     quaternion_scalar_cast_
0294         {
0295         quaternion_scalar_cast_( quaternion_scalar_cast_ const & );
0296         quaternion_scalar_cast_ & operator=( quaternion_scalar_cast_ const & );
0297         ~quaternion_scalar_cast_();
0298 
0299         public:
0300 
0301         template <class T>
0302         BOOST_QVM_CONSTEXPR BOOST_QVM_INLINE_TRIVIAL
0303         quaternion_scalar_cast_ &
0304         operator=( T const & x )
0305             {
0306             assign(*this,x);
0307             return *this;
0308             }
0309 
0310         template <class R
0311 #if __cplusplus >= 201103L
0312             , class = typename enable_if<is_quat<R> >::type
0313 #endif
0314         >
0315         BOOST_QVM_CONSTEXPR BOOST_QVM_INLINE_TRIVIAL
0316         operator R() const
0317             {
0318             R r;
0319             assign(r,*this);
0320             return r;
0321             }
0322         };
0323 
0324     template <bool> struct scalar_cast_quaternion_filter { };
0325     template <> struct scalar_cast_quaternion_filter<true> { typedef int type; };
0326     }
0327 
0328 template <class OriginalType,class Scalar>
0329 struct
0330 quat_traits< qvm_detail::quaternion_scalar_cast_<OriginalType,Scalar> >
0331     {
0332     typedef Scalar scalar_type;
0333     typedef qvm_detail::quaternion_scalar_cast_<OriginalType,Scalar> this_quaternion;
0334 
0335     template <int I>
0336     static
0337     BOOST_QVM_CONSTEXPR BOOST_QVM_INLINE_CRITICAL
0338     scalar_type
0339     read_element( this_quaternion const & x )
0340         {
0341         BOOST_QVM_STATIC_ASSERT(I>=0);
0342         BOOST_QVM_STATIC_ASSERT(I<4);
0343         return scalar_type(quat_traits<OriginalType>::template read_element<I>(reinterpret_cast<OriginalType const &>(x)));
0344         }
0345 
0346     static
0347     BOOST_QVM_CONSTEXPR BOOST_QVM_INLINE_CRITICAL
0348     scalar_type
0349     read_element_idx( int i, this_quaternion const & x )
0350         {
0351         BOOST_QVM_ASSERT(i>=0);
0352         BOOST_QVM_ASSERT(i<4);
0353         return scalar_type(quat_traits<OriginalType>::read_element_idx(i,reinterpret_cast<OriginalType const &>(x)));
0354         }
0355     };
0356 
0357 template <class Scalar,class T>
0358 BOOST_QVM_CONSTEXPR BOOST_QVM_INLINE_TRIVIAL
0359 qvm_detail::quaternion_scalar_cast_<T,Scalar> const &
0360 scalar_cast( T const & x, typename qvm_detail::scalar_cast_quaternion_filter<is_quat<T>::value>::type=0 )
0361     {
0362     return reinterpret_cast<qvm_detail::quaternion_scalar_cast_<T,Scalar> const &>(x);
0363     }
0364 
0365 ////////////////////////////////////////////////
0366 
0367 template <class A,class B>
0368 BOOST_QVM_CONSTEXPR BOOST_QVM_INLINE_OPERATIONS
0369 typename enable_if_c<
0370     is_quat<A>::value && is_scalar<B>::value,
0371     A &>::type
0372 operator/=( A & a, B b )
0373     {
0374     write_quat_element<0>(a,quat_traits<A>::template read_element<0>(a)/b);
0375     write_quat_element<1>(a,quat_traits<A>::template read_element<1>(a)/b);
0376     write_quat_element<2>(a,quat_traits<A>::template read_element<2>(a)/b);
0377     write_quat_element<3>(a,quat_traits<A>::template read_element<3>(a)/b);
0378     return a;
0379     }
0380 
0381 template <class A,class B>
0382 BOOST_QVM_CONSTEXPR BOOST_QVM_INLINE_OPERATIONS
0383 typename lazy_enable_if_c<
0384     is_quat<A>::value && is_scalar<B>::value,
0385     deduce_quat2<A,B> >::type
0386 operator/( A const & a, B b )
0387     {
0388     typedef typename deduce_quat2<A,B>::type R;
0389     R r;
0390     write_quat_element<0>(r,quat_traits<A>::template read_element<0>(a)/b);
0391     write_quat_element<1>(r,quat_traits<A>::template read_element<1>(a)/b);
0392     write_quat_element<2>(r,quat_traits<A>::template read_element<2>(a)/b);
0393     write_quat_element<3>(r,quat_traits<A>::template read_element<3>(a)/b);
0394     return r;
0395     }
0396 
0397 template <class A,class B>
0398 BOOST_QVM_CONSTEXPR BOOST_QVM_INLINE_OPERATIONS
0399 typename lazy_enable_if_c<
0400     is_quat<A>::value && is_quat<B>::value,
0401     deduce_scalar<typename quat_traits<A>::scalar_type,typename quat_traits<B>::scalar_type> >::type
0402 dot( A const & a, B const & b )
0403     {
0404     typedef typename quat_traits<A>::scalar_type Ta;
0405     typedef typename quat_traits<B>::scalar_type Tb;
0406     typedef typename deduce_scalar<Ta,Tb>::type Tr;
0407     Ta const a0=quat_traits<A>::template read_element<0>(a);
0408     Ta const a1=quat_traits<A>::template read_element<1>(a);
0409     Ta const a2=quat_traits<A>::template read_element<2>(a);
0410     Ta const a3=quat_traits<A>::template read_element<3>(a);
0411     Tb const b0=quat_traits<B>::template read_element<0>(b);
0412     Tb const b1=quat_traits<B>::template read_element<1>(b);
0413     Tb const b2=quat_traits<B>::template read_element<2>(b);
0414     Tb const b3=quat_traits<B>::template read_element<3>(b);
0415     Tr const dp=a0*b0+a1*b1+a2*b2+a3*b3;
0416     return dp;
0417     }
0418 
0419 template <class A,class B>
0420 BOOST_QVM_CONSTEXPR BOOST_QVM_INLINE_OPERATIONS
0421 typename enable_if_c<
0422     is_quat<A>::value && is_quat<B>::value,
0423     bool>::type
0424 operator==( A const & a, B const & b )
0425     {
0426     return
0427         quat_traits<A>::template read_element<0>(a)==quat_traits<B>::template read_element<0>(b) &&
0428         quat_traits<A>::template read_element<1>(a)==quat_traits<B>::template read_element<1>(b) &&
0429         quat_traits<A>::template read_element<2>(a)==quat_traits<B>::template read_element<2>(b) &&
0430         quat_traits<A>::template read_element<3>(a)==quat_traits<B>::template read_element<3>(b);
0431     }
0432 
0433 template <class A>
0434 BOOST_QVM_CONSTEXPR BOOST_QVM_INLINE_OPERATIONS
0435 typename lazy_enable_if_c<
0436     is_quat<A>::value,
0437     deduce_quat<A> >::type
0438 inverse( A const & a )
0439     {
0440     typedef typename deduce_quat<A>::type R;
0441     typedef typename quat_traits<A>::scalar_type TA;
0442     TA aa = quat_traits<A>::template read_element<0>(a);
0443     TA ab = quat_traits<A>::template read_element<1>(a);
0444     TA ac = quat_traits<A>::template read_element<2>(a);
0445     TA ad = quat_traits<A>::template read_element<3>(a);
0446     TA m2 = ab*ab + ac*ac + ad*ad + aa*aa;
0447     if( m2==scalar_traits<TA>::value(0) )
0448         BOOST_QVM_THROW_EXCEPTION(zero_magnitude_error());
0449     TA rm=scalar_traits<TA>::value(1)/m2;
0450     R r;
0451     write_quat_element<0>(r,aa*rm);
0452     write_quat_element<1>(r,-ab*rm);
0453     write_quat_element<2>(r,-ac*rm);
0454     write_quat_element<3>(r,-ad*rm);
0455     return r;
0456     }
0457 
0458 template <class A>
0459 BOOST_QVM_CONSTEXPR BOOST_QVM_INLINE_OPERATIONS
0460 typename enable_if_c<
0461     is_quat<A>::value,
0462     typename quat_traits<A>::scalar_type>::type
0463 mag_sqr( A const & a )
0464     {
0465     typedef typename quat_traits<A>::scalar_type T;
0466     T x=quat_traits<A>::template read_element<0>(a);
0467     T y=quat_traits<A>::template read_element<1>(a);
0468     T z=quat_traits<A>::template read_element<2>(a);
0469     T w=quat_traits<A>::template read_element<3>(a);
0470     return x*x+y*y+z*z+w*w;
0471     }
0472 
0473 template <class A>
0474 BOOST_QVM_CONSTEXPR BOOST_QVM_INLINE_OPERATIONS
0475 typename enable_if_c<
0476     is_quat<A>::value,
0477     typename quat_traits<A>::scalar_type>::type
0478 mag( A const & a )
0479     {
0480     typedef typename quat_traits<A>::scalar_type T;
0481     T x=quat_traits<A>::template read_element<0>(a);
0482     T y=quat_traits<A>::template read_element<1>(a);
0483     T z=quat_traits<A>::template read_element<2>(a);
0484     T w=quat_traits<A>::template read_element<3>(a);
0485     return sqrt(x*x+y*y+z*z+w*w);
0486     }
0487 
0488 template <class A,class B>
0489 BOOST_QVM_CONSTEXPR BOOST_QVM_INLINE_OPERATIONS
0490 typename enable_if<
0491     msvc_parse_bug_workaround::quats<A,B>,
0492     A &>::type
0493 operator-=( A & a, B const & b )
0494     {
0495     write_quat_element<0>(a,quat_traits<A>::template read_element<0>(a)-quat_traits<B>::template read_element<0>(b));
0496     write_quat_element<1>(a,quat_traits<A>::template read_element<1>(a)-quat_traits<B>::template read_element<1>(b));
0497     write_quat_element<2>(a,quat_traits<A>::template read_element<2>(a)-quat_traits<B>::template read_element<2>(b));
0498     write_quat_element<3>(a,quat_traits<A>::template read_element<3>(a)-quat_traits<B>::template read_element<3>(b));
0499     return a;
0500     }
0501 
0502 template <class A,class B>
0503 BOOST_QVM_CONSTEXPR BOOST_QVM_INLINE_OPERATIONS
0504 typename lazy_enable_if_c<
0505     is_quat<A>::value && is_quat<B>::value,
0506     deduce_quat2<A,B> >::type
0507 operator-( A const & a, B const & b )
0508     {
0509     typedef typename deduce_quat2<A,B>::type R;
0510     R r;
0511     write_quat_element<0>(r,quat_traits<A>::template read_element<0>(a)-quat_traits<B>::template read_element<0>(b));
0512     write_quat_element<1>(r,quat_traits<A>::template read_element<1>(a)-quat_traits<B>::template read_element<1>(b));
0513     write_quat_element<2>(r,quat_traits<A>::template read_element<2>(a)-quat_traits<B>::template read_element<2>(b));
0514     write_quat_element<3>(r,quat_traits<A>::template read_element<3>(a)-quat_traits<B>::template read_element<3>(b));
0515     return r;
0516     }
0517 
0518 template <class A>
0519 BOOST_QVM_CONSTEXPR BOOST_QVM_INLINE_OPERATIONS
0520 typename lazy_enable_if_c<
0521     is_quat<A>::value,
0522     deduce_quat<A> >::type
0523 operator-( A const & a )
0524     {
0525     typedef typename deduce_quat<A>::type R;
0526     R r;
0527     write_quat_element<0>(r,-quat_traits<A>::template read_element<0>(a));
0528     write_quat_element<1>(r,-quat_traits<A>::template read_element<1>(a));
0529     write_quat_element<2>(r,-quat_traits<A>::template read_element<2>(a));
0530     write_quat_element<3>(r,-quat_traits<A>::template read_element<3>(a));
0531     return r;
0532     }
0533 
0534 template <class A,class B>
0535 BOOST_QVM_CONSTEXPR BOOST_QVM_INLINE_OPERATIONS
0536 typename enable_if<
0537     msvc_parse_bug_workaround::quats<A,B>,
0538     A &>::type
0539 operator*=( A & a, B const & b )
0540     {
0541     typedef typename quat_traits<A>::scalar_type TA;
0542     typedef typename quat_traits<B>::scalar_type TB;
0543     TA const aa=quat_traits<A>::template read_element<0>(a);
0544     TA const ab=quat_traits<A>::template read_element<1>(a);
0545     TA const ac=quat_traits<A>::template read_element<2>(a);
0546     TA const ad=quat_traits<A>::template read_element<3>(a);
0547     TB const ba=quat_traits<B>::template read_element<0>(b);
0548     TB const bb=quat_traits<B>::template read_element<1>(b);
0549     TB const bc=quat_traits<B>::template read_element<2>(b);
0550     TB const bd=quat_traits<B>::template read_element<3>(b);
0551     write_quat_element<0>(a,aa*ba - ab*bb - ac*bc - ad*bd);
0552     write_quat_element<1>(a,aa*bb + ab*ba + ac*bd - ad*bc);
0553     write_quat_element<2>(a,aa*bc + ac*ba + ad*bb - ab*bd);
0554     write_quat_element<3>(a,aa*bd + ad*ba + ab*bc - ac*bb);
0555     return a;
0556     }
0557 
0558 template <class A,class B>
0559 BOOST_QVM_CONSTEXPR BOOST_QVM_INLINE_OPERATIONS
0560 typename enable_if_c<
0561     is_quat<A>::value && is_scalar<B>::value,
0562     A &>::type
0563 operator*=( A & a, B b )
0564     {
0565     write_quat_element<0>(a, quat_traits<A>::template read_element<0>(a)*b);
0566     write_quat_element<1>(a, quat_traits<A>::template read_element<1>(a)*b);
0567     write_quat_element<2>(a, quat_traits<A>::template read_element<2>(a)*b);
0568     write_quat_element<3>(a, quat_traits<A>::template read_element<3>(a)*b);
0569     return a;
0570     }
0571 
0572 template <class A,class B>
0573 BOOST_QVM_CONSTEXPR BOOST_QVM_INLINE_OPERATIONS
0574 typename lazy_enable_if_c<
0575     is_quat<A>::value && is_quat<B>::value,
0576     deduce_quat2<A,B> >::type
0577 operator*( A const & a, B const & b )
0578     {
0579     typedef typename deduce_quat2<A,B>::type R;
0580     typedef typename quat_traits<A>::scalar_type TA;
0581     typedef typename quat_traits<B>::scalar_type TB;
0582     TA const aa=quat_traits<A>::template read_element<0>(a);
0583     TA const ab=quat_traits<A>::template read_element<1>(a);
0584     TA const ac=quat_traits<A>::template read_element<2>(a);
0585     TA const ad=quat_traits<A>::template read_element<3>(a);
0586     TB const ba=quat_traits<B>::template read_element<0>(b);
0587     TB const bb=quat_traits<B>::template read_element<1>(b);
0588     TB const bc=quat_traits<B>::template read_element<2>(b);
0589     TB const bd=quat_traits<B>::template read_element<3>(b);
0590     R r;
0591     write_quat_element<0>(r,aa*ba - ab*bb - ac*bc - ad*bd);
0592     write_quat_element<1>(r,aa*bb + ab*ba + ac*bd - ad*bc);
0593     write_quat_element<2>(r,aa*bc + ac*ba + ad*bb - ab*bd);
0594     write_quat_element<3>(r,aa*bd + ad*ba + ab*bc - ac*bb);
0595     return r;
0596     }
0597 
0598 template <class A,class B>
0599 BOOST_QVM_CONSTEXPR BOOST_QVM_INLINE_OPERATIONS
0600 typename lazy_enable_if_c2<
0601     is_quat<A>::value && is_scalar<B>::value,
0602     deduce_quat2<A,B> >::type
0603 operator*( A const & a, B b )
0604     {
0605     typedef typename deduce_quat2<A,B>::type R;
0606     R r;
0607     write_quat_element<0>(r,quat_traits<A>::template read_element<0>(a)*b);
0608     write_quat_element<1>(r,quat_traits<A>::template read_element<1>(a)*b);
0609     write_quat_element<2>(r,quat_traits<A>::template read_element<2>(a)*b);
0610     write_quat_element<3>(r,quat_traits<A>::template read_element<3>(a)*b);
0611     return r;
0612     }
0613 
0614 template <class A,class B>
0615 BOOST_QVM_CONSTEXPR BOOST_QVM_INLINE_OPERATIONS
0616 typename enable_if_c<
0617     is_quat<A>::value && is_quat<B>::value,
0618     bool>::type
0619 operator!=( A const & a, B const & b )
0620     {
0621     return
0622         quat_traits<A>::template read_element<0>(a)!=quat_traits<B>::template read_element<0>(b) ||
0623         quat_traits<A>::template read_element<1>(a)!=quat_traits<B>::template read_element<1>(b) ||
0624         quat_traits<A>::template read_element<2>(a)!=quat_traits<B>::template read_element<2>(b) ||
0625         quat_traits<A>::template read_element<3>(a)!=quat_traits<B>::template read_element<3>(b);
0626     }
0627 
0628 template <class A>
0629 BOOST_QVM_CONSTEXPR BOOST_QVM_INLINE_OPERATIONS
0630 typename lazy_enable_if_c<
0631     is_quat<A>::value,
0632     deduce_quat<A> >::type
0633 normalized( A const & a )
0634     {
0635     typedef typename quat_traits<A>::scalar_type T;
0636     T const a0=quat_traits<A>::template read_element<0>(a);
0637     T const a1=quat_traits<A>::template read_element<1>(a);
0638     T const a2=quat_traits<A>::template read_element<2>(a);
0639     T const a3=quat_traits<A>::template read_element<3>(a);
0640     T const m2=a0*a0+a1*a1+a2*a2+a3*a3;
0641     if( m2==scalar_traits<typename quat_traits<A>::scalar_type>::value(0) )
0642         BOOST_QVM_THROW_EXCEPTION(zero_magnitude_error());
0643     T const rm=scalar_traits<T>::value(1)/sqrt(m2);
0644     typedef typename deduce_quat<A>::type R;
0645     R r;
0646     write_quat_element<0>(r,a0*rm);
0647     write_quat_element<1>(r,a1*rm);
0648     write_quat_element<2>(r,a2*rm);
0649     write_quat_element<3>(r,a3*rm);
0650     return r;
0651     }
0652 
0653 template <class A>
0654 BOOST_QVM_CONSTEXPR BOOST_QVM_INLINE_OPERATIONS
0655 typename enable_if_c<
0656     is_quat<A>::value,
0657     void>::type
0658 normalize( A & a )
0659     {
0660     typedef typename quat_traits<A>::scalar_type T;
0661     T const a0=quat_traits<A>::template read_element<0>(a);
0662     T const a1=quat_traits<A>::template read_element<1>(a);
0663     T const a2=quat_traits<A>::template read_element<2>(a);
0664     T const a3=quat_traits<A>::template read_element<3>(a);
0665     T const m2=a0*a0+a1*a1+a2*a2+a3*a3;
0666     if( m2==scalar_traits<typename quat_traits<A>::scalar_type>::value(0) )
0667         BOOST_QVM_THROW_EXCEPTION(zero_magnitude_error());
0668     T const rm=scalar_traits<T>::value(1)/sqrt(m2);
0669     write_quat_element<0>(a,quat_traits<A>::template read_element<0>(a)*rm);
0670     write_quat_element<1>(a,quat_traits<A>::template read_element<1>(a)*rm);
0671     write_quat_element<2>(a,quat_traits<A>::template read_element<2>(a)*rm);
0672     write_quat_element<3>(a,quat_traits<A>::template read_element<3>(a)*rm);
0673     }
0674 
0675 template <class A,class B>
0676 BOOST_QVM_CONSTEXPR BOOST_QVM_INLINE_OPERATIONS
0677 typename enable_if<
0678     msvc_parse_bug_workaround::quats<A,B>,
0679     A &>::type
0680 operator+=( A & a, B const & b )
0681     {
0682     write_quat_element<0>(a,quat_traits<A>::template read_element<0>(a)+quat_traits<B>::template read_element<0>(b));
0683     write_quat_element<1>(a,quat_traits<A>::template read_element<1>(a)+quat_traits<B>::template read_element<1>(b));
0684     write_quat_element<2>(a,quat_traits<A>::template read_element<2>(a)+quat_traits<B>::template read_element<2>(b));
0685     write_quat_element<3>(a,quat_traits<A>::template read_element<3>(a)+quat_traits<B>::template read_element<3>(b));
0686     return a;
0687     }
0688 
0689 template <class A,class B>
0690 BOOST_QVM_CONSTEXPR BOOST_QVM_INLINE_OPERATIONS
0691 typename lazy_enable_if_c<
0692     is_quat<A>::value && is_quat<B>::value,
0693     deduce_quat2<A,B> >::type
0694 operator+( A const & a, B const & b )
0695     {
0696     typedef typename deduce_quat2<A,B>::type R;
0697     R r;
0698     write_quat_element<0>(r,quat_traits<A>::template read_element<0>(a)+quat_traits<B>::template read_element<0>(b));
0699     write_quat_element<1>(r,quat_traits<A>::template read_element<1>(a)+quat_traits<B>::template read_element<1>(b));
0700     write_quat_element<2>(r,quat_traits<A>::template read_element<2>(a)+quat_traits<B>::template read_element<2>(b));
0701     write_quat_element<3>(r,quat_traits<A>::template read_element<3>(a)+quat_traits<B>::template read_element<3>(b));
0702     return r;
0703     }
0704 
0705 template <class A,class B,class C>
0706 BOOST_QVM_CONSTEXPR BOOST_QVM_INLINE_OPERATIONS
0707 typename lazy_enable_if_c<
0708     is_quat<A>::value && is_quat<B>::value && is_scalar<C>::value,
0709     deduce_quat2<A,B> >::type
0710 slerp360( A const & a, B const & b, C t )
0711     {
0712     typedef typename deduce_quat2<A,B>::type R;
0713     typedef typename quat_traits<R>::scalar_type TR;
0714     TR const one = scalar_traits<TR>::value(1);
0715     TR const threshold = one - one / scalar_traits<TR>::value(2000); //0.9995
0716     TR const dp = dot(a,b);
0717     TR const abs_dp = abs(dp);
0718     if( abs_dp > threshold )
0719         return a*(one-t) + b*t;
0720     TR const th = acos(dp);
0721     TR const invsinth = one / sin(th);
0722     return a * (sin(th * (one-t)) * invsinth) + b * (sin(th * t) * invsinth);
0723     }
0724 
0725 template <class A,class B,class C>
0726 BOOST_QVM_CONSTEXPR BOOST_QVM_INLINE_OPERATIONS
0727 typename lazy_enable_if_c<
0728     is_quat<A>::value && is_quat<B>::value && is_scalar<C>::value,
0729     deduce_quat2<A,B> >::type
0730 slerp180( A const & a, B const & b, C t )
0731     {
0732     typedef typename deduce_quat2<A,B>::type R;
0733     typedef typename quat_traits<R>::scalar_type TR;
0734     TR const one = scalar_traits<TR>::value(1);
0735     TR const threshold = one - one / scalar_traits<TR>::value(2000); //0.9995
0736     TR const dp = dot(a,b);
0737     TR const abs_dp = abs(dp);
0738     if( abs_dp > threshold )
0739         return a*(one-t)*sign(dp) + b*t;
0740     TR const th = acos(abs_dp);
0741     TR const invsinth = one / sin(th);
0742     return a * (sin(th * (one-t)) * invsinth * sign(dp)) + b * (sin(th * t) * invsinth);
0743     }
0744 
0745 template <class A,class B,class C>
0746 BOOST_QVM_DEPRECATED("please use slerp180 or slerp360")
0747 BOOST_QVM_CONSTEXPR BOOST_QVM_INLINE_OPERATIONS
0748 typename lazy_enable_if_c<
0749     is_quat<A>::value && is_quat<B>::value && is_scalar<C>::value,
0750     deduce_quat2<A,B> >::type
0751 slerp( A const & a, B const & b, C t )
0752     {
0753     return slerp360(a, b, t);
0754     }
0755 
0756 ////////////////////////////////////////////////
0757 
0758 namespace
0759 qvm_detail
0760     {
0761     template <class T>
0762     class
0763     qref_
0764         {
0765         qref_( qref_ const & );
0766         qref_ & operator=( qref_ const & );
0767         ~qref_();
0768 
0769         public:
0770 
0771         template <class R>
0772         BOOST_QVM_CONSTEXPR BOOST_QVM_INLINE_TRIVIAL
0773         qref_ &
0774         operator=( R const & x )
0775             {
0776             assign(*this,x);
0777             return *this;
0778             }
0779 
0780         template <class R
0781 #if __cplusplus >= 201103L
0782             , class = typename enable_if<is_quat<R> >::type
0783 #endif
0784         >
0785         BOOST_QVM_CONSTEXPR BOOST_QVM_INLINE_TRIVIAL
0786         operator R() const
0787             {
0788             R r;
0789             assign(r,*this);
0790             return r;
0791             }
0792         };
0793 
0794     template <class Q,bool WriteElementRef=quat_write_element_ref<Q>::value>
0795     struct qref_write_traits;
0796 
0797     template <class Q>
0798     struct
0799     qref_write_traits<Q,true>
0800         {
0801         typedef typename quat_traits<Q>::scalar_type scalar_type;
0802         typedef qvm_detail::qref_<Q> this_quaternion;
0803 
0804         template <int I>
0805         static
0806         BOOST_QVM_CONSTEXPR BOOST_QVM_INLINE_CRITICAL
0807         scalar_type &
0808         write_element( this_quaternion & x )
0809             {
0810             BOOST_QVM_STATIC_ASSERT(I>=0);
0811             BOOST_QVM_STATIC_ASSERT(I<4);
0812             return quat_traits<Q>::template write_element<I>(reinterpret_cast<Q &>(x));
0813             }
0814         };
0815 
0816     template <class Q>
0817     struct
0818     qref_write_traits<Q,false>
0819         {
0820         typedef typename quat_traits<Q>::scalar_type scalar_type;
0821         typedef qvm_detail::qref_<Q> this_quaternion;
0822 
0823         template <int I>
0824         static
0825         BOOST_QVM_CONSTEXPR BOOST_QVM_INLINE_CRITICAL
0826         void
0827         write_element( this_quaternion & x, scalar_type s )
0828             {
0829             BOOST_QVM_STATIC_ASSERT(I>=0);
0830             BOOST_QVM_STATIC_ASSERT(I<4);
0831             quat_traits<Q>::template write_element<I>(reinterpret_cast<Q &>(x), s);
0832             }
0833         };
0834     }
0835 
0836 template <class Q>
0837 struct quat_traits;
0838 
0839 template <class Q>
0840 struct
0841 quat_traits< qvm_detail::qref_<Q> >:
0842     qvm_detail::qref_write_traits<Q>
0843     {
0844     typedef typename quat_traits<Q>::scalar_type scalar_type;
0845     typedef qvm_detail::qref_<Q> this_quaternion;
0846 
0847     template <int I>
0848     static
0849     BOOST_QVM_CONSTEXPR BOOST_QVM_INLINE_CRITICAL
0850     scalar_type
0851     read_element( this_quaternion const & x )
0852         {
0853         BOOST_QVM_STATIC_ASSERT(I>=0);
0854         BOOST_QVM_STATIC_ASSERT(I<4);
0855         return quat_traits<Q>::template read_element<I>(reinterpret_cast<Q const &>(x));
0856         }
0857     };
0858 
0859 template <class Q>
0860 struct
0861 deduce_quat< qvm_detail::qref_<Q> >
0862     {
0863     typedef quat<typename quat_traits<Q>::scalar_type> type;
0864     };
0865 
0866 template <class Q>
0867 BOOST_QVM_CONSTEXPR BOOST_QVM_INLINE_TRIVIAL
0868 typename enable_if_c<
0869     is_quat<Q>::value,
0870     qvm_detail::qref_<Q> const &>::type
0871 qref( Q const & a )
0872     {
0873     return reinterpret_cast<qvm_detail::qref_<Q> const &>(a);
0874     }
0875 
0876 template <class Q>
0877 BOOST_QVM_CONSTEXPR BOOST_QVM_INLINE_TRIVIAL
0878 typename enable_if_c<
0879     is_quat<Q>::value,
0880     qvm_detail::qref_<Q> &>::type
0881 qref( Q & a )
0882     {
0883     return reinterpret_cast<qvm_detail::qref_<Q> &>(a);
0884     }
0885 
0886 ////////////////////////////////////////////////
0887 
0888 namespace
0889 qvm_detail
0890     {
0891     template <class T>
0892     class
0893     zero_q_
0894         {
0895         zero_q_( zero_q_ const & );
0896         zero_q_ & operator=( zero_q_ const & );
0897         ~zero_q_();
0898 
0899         public:
0900 
0901         template <class R
0902 #if __cplusplus >= 201103L
0903             , class = typename enable_if<is_quat<R> >::type
0904 #endif
0905         >
0906         BOOST_QVM_CONSTEXPR BOOST_QVM_INLINE_TRIVIAL
0907         operator R() const
0908             {
0909             R r;
0910             assign(r,*this);
0911             return r;
0912             }
0913         };
0914     }
0915 
0916 template <class T>
0917 struct
0918 quat_traits< qvm_detail::zero_q_<T> >
0919     {
0920     typedef qvm_detail::zero_q_<T> this_quaternion;
0921     typedef T scalar_type;
0922 
0923     template <int I>
0924     static
0925     BOOST_QVM_CONSTEXPR BOOST_QVM_INLINE_CRITICAL
0926     scalar_type
0927     read_element( this_quaternion const & )
0928         {
0929         BOOST_QVM_STATIC_ASSERT(I>=0);
0930         BOOST_QVM_STATIC_ASSERT(I<4);
0931         return scalar_traits<scalar_type>::value(0);
0932         }
0933 
0934     static
0935     BOOST_QVM_CONSTEXPR BOOST_QVM_INLINE_CRITICAL
0936     scalar_type
0937     read_element_idx( int i, this_quaternion const & )
0938         {
0939         BOOST_QVM_ASSERT(i>=0);
0940         BOOST_QVM_ASSERT(i<4);
0941         return scalar_traits<scalar_type>::value(0);
0942         }
0943     };
0944 
0945 template <class T>
0946 BOOST_QVM_CONSTEXPR BOOST_QVM_INLINE_TRIVIAL
0947 qvm_detail::zero_q_<T> const &
0948 zero_quat()
0949     {
0950     return *(qvm_detail::zero_q_<T> const *)qvm_detail::get_valid_ptr_quat_operations();
0951     }
0952 
0953 template <class A>
0954 BOOST_QVM_CONSTEXPR BOOST_QVM_INLINE_OPERATIONS
0955 typename enable_if_c<
0956     is_quat<A>::value,
0957     void>::type
0958 set_zero( A & a )
0959     {
0960     typedef typename quat_traits<A>::scalar_type T;
0961     T const zero=scalar_traits<T>::value(0);
0962     write_quat_element<0>(a,zero);
0963     write_quat_element<1>(a,zero);
0964     write_quat_element<2>(a,zero);
0965     write_quat_element<3>(a,zero);
0966     }
0967 
0968 ////////////////////////////////////////////////
0969 
0970 namespace
0971 qvm_detail
0972     {
0973     template <class V>
0974     struct
0975     rot_quat_
0976         {
0977         typedef typename vec_traits<V>::scalar_type scalar_type;
0978         scalar_type a[4];
0979 
0980         template <class Angle>
0981         BOOST_QVM_CONSTEXPR BOOST_QVM_INLINE
0982         rot_quat_( V const & axis, Angle angle )
0983             {
0984             scalar_type const x=vec_traits<V>::template read_element<0>(axis);
0985             scalar_type const y=vec_traits<V>::template read_element<1>(axis);
0986             scalar_type const z=vec_traits<V>::template read_element<2>(axis);
0987             scalar_type const m2=x*x+y*y+z*z;
0988             if( m2==scalar_traits<scalar_type>::value(0) )
0989                 BOOST_QVM_THROW_EXCEPTION(zero_magnitude_error());
0990             scalar_type const rm=scalar_traits<scalar_type>::value(1)/sqrt(m2);
0991             angle/=2;
0992             scalar_type const s=sin(angle);
0993             a[0] = cos(angle);
0994             a[1] = rm*x*s;
0995             a[2] = rm*y*s;
0996             a[3] = rm*z*s;
0997             }
0998 
0999         template <class R
1000 #if __cplusplus >= 201103L
1001             , class = typename enable_if<is_quat<R> >::type
1002 #endif
1003         >
1004         BOOST_QVM_CONSTEXPR BOOST_QVM_INLINE_TRIVIAL
1005         operator R() const
1006             {
1007             R r;
1008             assign(r,*this);
1009             return r;
1010             }
1011         };
1012     }
1013 
1014 template <class V>
1015 struct
1016 quat_traits< qvm_detail::rot_quat_<V> >
1017     {
1018     typedef qvm_detail::rot_quat_<V> this_quaternion;
1019     typedef typename this_quaternion::scalar_type scalar_type;
1020 
1021     template <int I>
1022     static
1023     BOOST_QVM_CONSTEXPR BOOST_QVM_INLINE_CRITICAL
1024     scalar_type
1025     read_element( this_quaternion const & x )
1026         {
1027         BOOST_QVM_STATIC_ASSERT(I>=0);
1028         BOOST_QVM_STATIC_ASSERT(I<4);
1029         return x.a[I];
1030         }
1031     };
1032 
1033 template <class V>
1034 struct
1035 deduce_quat< qvm_detail::rot_quat_<V> >
1036     {
1037     typedef quat<typename vec_traits<V>::scalar_type> type;
1038     };
1039 
1040 template <class A,class Angle>
1041 BOOST_QVM_CONSTEXPR BOOST_QVM_INLINE
1042 typename enable_if_c<
1043     is_vec<A>::value && vec_traits<A>::dim==3,
1044     qvm_detail::rot_quat_<A> >::type
1045 rot_quat( A const & axis, Angle angle )
1046     {
1047     return qvm_detail::rot_quat_<A>(axis,angle);
1048     }
1049 
1050 template <class A,class B,class Angle>
1051 BOOST_QVM_CONSTEXPR BOOST_QVM_INLINE_OPERATIONS
1052 typename enable_if_c<
1053     is_quat<A>::value &&
1054     is_vec<B>::value && vec_traits<B>::dim==3,
1055     void>::type
1056 set_rot( A & a, B const & axis, Angle angle )
1057     {
1058     assign(a,rot_quat(axis,angle));
1059     }
1060 
1061 template <class A,class B,class Angle>
1062 BOOST_QVM_CONSTEXPR BOOST_QVM_INLINE_OPERATIONS
1063 typename enable_if_c<
1064     is_quat<A>::value &&
1065     is_vec<B>::value && vec_traits<B>::dim==3,
1066     void>::type
1067 rotate( A & a, B const & axis, Angle angle )
1068     {
1069     a *= rot_quat(axis,angle);
1070     }
1071 
1072 ////////////////////////////////////////////////
1073 
1074 namespace
1075 qvm_detail
1076     {
1077     template <class T>
1078     struct
1079     rotx_quat_
1080         {
1081         BOOST_QVM_CONSTEXPR BOOST_QVM_INLINE_TRIVIAL
1082         rotx_quat_()
1083             {
1084             }
1085 
1086         template <class R
1087 #if __cplusplus >= 201103L
1088             , class = typename enable_if<is_quat<R> >::type
1089 #endif
1090         >
1091         BOOST_QVM_CONSTEXPR BOOST_QVM_INLINE_TRIVIAL
1092         operator R() const
1093             {
1094             R r;
1095             assign(r,*this);
1096             return r;
1097             }
1098 
1099         private:
1100 
1101         rotx_quat_( rotx_quat_ const & );
1102         rotx_quat_ & operator=( rotx_quat_ const & );
1103         ~rotx_quat_();
1104         };
1105 
1106     template <int I>
1107     struct
1108     rotx_q_get
1109         {
1110         template <class T>
1111         static
1112         BOOST_QVM_CONSTEXPR BOOST_QVM_INLINE_CRITICAL
1113         T
1114         get( T const & )
1115             {
1116             return scalar_traits<T>::value(0);
1117             }
1118         };
1119 
1120     template <>
1121     struct
1122     rotx_q_get<1>
1123         {
1124         template <class T>
1125         static
1126         BOOST_QVM_CONSTEXPR BOOST_QVM_INLINE_CRITICAL
1127         T
1128         get( T const & angle )
1129             {
1130             return sin(angle/2);
1131             }
1132         };
1133 
1134     template <>
1135     struct
1136     rotx_q_get<0>
1137         {
1138         template <class T>
1139         static
1140         BOOST_QVM_CONSTEXPR BOOST_QVM_INLINE_CRITICAL
1141         T
1142         get( T const & angle )
1143             {
1144             return cos(angle/2);
1145             }
1146         };
1147     }
1148 
1149 template <class Angle>
1150 struct
1151 quat_traits< qvm_detail::rotx_quat_<Angle> >
1152     {
1153     typedef qvm_detail::rotx_quat_<Angle> this_quaternion;
1154     typedef Angle scalar_type;
1155 
1156     template <int I>
1157     static
1158     BOOST_QVM_CONSTEXPR BOOST_QVM_INLINE_CRITICAL
1159     scalar_type
1160     read_element( this_quaternion const & x )
1161         {
1162         BOOST_QVM_STATIC_ASSERT(I>=0);
1163         BOOST_QVM_STATIC_ASSERT(I<4);
1164         return qvm_detail::rotx_q_get<I>::get(reinterpret_cast<Angle const &>(x));
1165         }
1166     };
1167 
1168 template <class Angle>
1169 struct
1170 deduce_quat< qvm_detail::rotx_quat_<Angle> >
1171     {
1172     typedef quat<Angle> type;
1173     };
1174 
1175 template <class Angle>
1176 struct
1177 deduce_quat2< qvm_detail::rotx_quat_<Angle>, qvm_detail::rotx_quat_<Angle> >
1178     {
1179     typedef quat<Angle> type;
1180     };
1181 
1182 template <class Angle>
1183 BOOST_QVM_CONSTEXPR BOOST_QVM_INLINE_TRIVIAL
1184 qvm_detail::rotx_quat_<Angle> const &
1185 rotx_quat( Angle const & angle )
1186     {
1187     return reinterpret_cast<qvm_detail::rotx_quat_<Angle> const &>(angle);
1188     }
1189 
1190 template <class A,class Angle>
1191 BOOST_QVM_CONSTEXPR BOOST_QVM_INLINE_OPERATIONS
1192 typename enable_if_c<
1193     is_quat<A>::value,
1194     void>::type
1195 set_rotx( A & a, Angle angle )
1196     {
1197     assign(a,rotx_quat(angle));
1198     }
1199 
1200 template <class A,class Angle>
1201 BOOST_QVM_CONSTEXPR BOOST_QVM_INLINE_OPERATIONS
1202 typename enable_if_c<
1203     is_quat<A>::value,
1204     void>::type
1205 rotate_x( A & a, Angle angle )
1206     {
1207     a *= rotx_quat(angle);
1208     }
1209 
1210 ////////////////////////////////////////////////
1211 
1212 namespace
1213 qvm_detail
1214     {
1215     template <class T>
1216     struct
1217     roty_quat_
1218         {
1219         BOOST_QVM_CONSTEXPR BOOST_QVM_INLINE_TRIVIAL
1220         roty_quat_()
1221             {
1222             }
1223 
1224         template <class R
1225 #if __cplusplus >= 201103L
1226             , class = typename enable_if<is_quat<R> >::type
1227 #endif
1228         >
1229         BOOST_QVM_CONSTEXPR BOOST_QVM_INLINE_TRIVIAL
1230         operator R() const
1231             {
1232             R r;
1233             assign(r,*this);
1234             return r;
1235             }
1236 
1237         private:
1238 
1239         roty_quat_( roty_quat_ const & );
1240         roty_quat_ & operator=( roty_quat_ const & );
1241         ~roty_quat_();
1242         };
1243 
1244     template <int I>
1245     struct
1246     roty_q_get
1247         {
1248         template <class T>
1249         static
1250         BOOST_QVM_CONSTEXPR BOOST_QVM_INLINE_CRITICAL
1251         T
1252         get( T const & )
1253             {
1254             return scalar_traits<T>::value(0);
1255             }
1256         };
1257 
1258     template <>
1259     struct
1260     roty_q_get<2>
1261         {
1262         template <class T>
1263         static
1264         BOOST_QVM_CONSTEXPR BOOST_QVM_INLINE_CRITICAL
1265         T
1266         get( T const & angle )
1267             {
1268             return sin(angle/2);
1269             }
1270         };
1271 
1272     template <>
1273     struct
1274     roty_q_get<0>
1275         {
1276         template <class T>
1277         static
1278         BOOST_QVM_CONSTEXPR BOOST_QVM_INLINE_CRITICAL
1279         T
1280         get( T const & angle )
1281             {
1282             return cos(angle/2);
1283             }
1284         };
1285     }
1286 
1287 template <class Angle>
1288 struct
1289 quat_traits< qvm_detail::roty_quat_<Angle> >
1290     {
1291     typedef qvm_detail::roty_quat_<Angle> this_quaternion;
1292     typedef Angle scalar_type;
1293 
1294     template <int I>
1295     static
1296     BOOST_QVM_CONSTEXPR BOOST_QVM_INLINE_CRITICAL
1297     scalar_type
1298     read_element( this_quaternion const & x )
1299         {
1300         BOOST_QVM_STATIC_ASSERT(I>=0);
1301         BOOST_QVM_STATIC_ASSERT(I<4);
1302         return qvm_detail::roty_q_get<I>::get(reinterpret_cast<Angle const &>(x));
1303         }
1304     };
1305 
1306 template <class Angle>
1307 struct
1308 deduce_quat< qvm_detail::roty_quat_<Angle> >
1309     {
1310     typedef quat<Angle> type;
1311     };
1312 
1313 template <class Angle>
1314 struct
1315 deduce_quat2< qvm_detail::roty_quat_<Angle>, qvm_detail::roty_quat_<Angle> >
1316     {
1317     typedef quat<Angle> type;
1318     };
1319 
1320 template <class Angle>
1321 BOOST_QVM_CONSTEXPR BOOST_QVM_INLINE_TRIVIAL
1322 qvm_detail::roty_quat_<Angle> const &
1323 roty_quat( Angle const & angle )
1324     {
1325     return reinterpret_cast<qvm_detail::roty_quat_<Angle> const &>(angle);
1326     }
1327 
1328 template <class A,class Angle>
1329 BOOST_QVM_CONSTEXPR BOOST_QVM_INLINE_OPERATIONS
1330 typename enable_if_c<
1331     is_quat<A>::value,
1332     void>::type
1333 set_roty( A & a, Angle angle )
1334     {
1335     assign(a,roty_quat(angle));
1336     }
1337 
1338 template <class A,class Angle>
1339 BOOST_QVM_CONSTEXPR BOOST_QVM_INLINE_OPERATIONS
1340 typename enable_if_c<
1341     is_quat<A>::value,
1342     void>::type
1343 rotate_y( A & a, Angle angle )
1344     {
1345     a *= roty_quat(angle);
1346     }
1347 
1348 ////////////////////////////////////////////////
1349 
1350 namespace
1351 qvm_detail
1352     {
1353     template <class T>
1354     struct
1355     rotz_quat_
1356         {
1357         BOOST_QVM_CONSTEXPR BOOST_QVM_INLINE_TRIVIAL
1358         rotz_quat_()
1359             {
1360             }
1361 
1362         template <class R
1363 #if __cplusplus >= 201103L
1364             , class = typename enable_if<is_quat<R> >::type
1365 #endif
1366         >
1367         BOOST_QVM_CONSTEXPR BOOST_QVM_INLINE_TRIVIAL
1368         operator R() const
1369             {
1370             R r;
1371             assign(r,*this);
1372             return r;
1373             }
1374 
1375         private:
1376 
1377         rotz_quat_( rotz_quat_ const & );
1378         rotz_quat_ & operator=( rotz_quat_ const & );
1379         ~rotz_quat_();
1380         };
1381 
1382     template <int I>
1383     struct
1384     rotz_q_get
1385         {
1386         template <class T>
1387         static
1388         BOOST_QVM_CONSTEXPR BOOST_QVM_INLINE_CRITICAL
1389         T
1390         get( T const & )
1391             {
1392             return scalar_traits<T>::value(0);
1393             }
1394         };
1395 
1396     template <>
1397     struct
1398     rotz_q_get<3>
1399         {
1400         template <class T>
1401         static
1402         BOOST_QVM_CONSTEXPR BOOST_QVM_INLINE_CRITICAL
1403         T
1404         get( T const & angle )
1405             {
1406             return sin(angle/2);
1407             }
1408         };
1409 
1410     template <>
1411     struct
1412     rotz_q_get<0>
1413         {
1414         template <class T>
1415         static
1416         BOOST_QVM_CONSTEXPR BOOST_QVM_INLINE_CRITICAL
1417         T
1418         get( T const & angle )
1419             {
1420             return cos(angle/2);
1421             }
1422         };
1423     }
1424 
1425 template <class Angle>
1426 struct
1427 quat_traits< qvm_detail::rotz_quat_<Angle> >
1428     {
1429     typedef qvm_detail::rotz_quat_<Angle> this_quaternion;
1430     typedef Angle scalar_type;
1431 
1432     template <int I>
1433     static
1434     BOOST_QVM_CONSTEXPR BOOST_QVM_INLINE_CRITICAL
1435     scalar_type
1436     read_element( this_quaternion const & x )
1437         {
1438         BOOST_QVM_STATIC_ASSERT(I>=0);
1439         BOOST_QVM_STATIC_ASSERT(I<4);
1440         return qvm_detail::rotz_q_get<I>::get(reinterpret_cast<Angle const &>(x));
1441         }
1442     };
1443 
1444 template <class Angle>
1445 struct
1446 deduce_quat< qvm_detail::rotz_quat_<Angle> >
1447     {
1448     typedef quat<Angle> type;
1449     };
1450 
1451 template <class Angle>
1452 struct
1453 deduce_quat2< qvm_detail::rotz_quat_<Angle>, qvm_detail::rotz_quat_<Angle> >
1454     {
1455     typedef quat<Angle> type;
1456     };
1457 
1458 template <class Angle>
1459 BOOST_QVM_CONSTEXPR BOOST_QVM_INLINE_TRIVIAL
1460 qvm_detail::rotz_quat_<Angle> const &
1461 rotz_quat( Angle const & angle )
1462     {
1463     return reinterpret_cast<qvm_detail::rotz_quat_<Angle> const &>(angle);
1464     }
1465 
1466 template <class A,class Angle>
1467 BOOST_QVM_CONSTEXPR BOOST_QVM_INLINE_OPERATIONS
1468 typename enable_if_c<
1469     is_quat<A>::value,
1470     void>::type
1471 set_rotz( A & a, Angle angle )
1472     {
1473     assign(a,rotz_quat(angle));
1474     }
1475 
1476 template <class A,class Angle>
1477 BOOST_QVM_CONSTEXPR BOOST_QVM_INLINE_OPERATIONS
1478 typename enable_if_c<
1479     is_quat<A>::value,
1480     void>::type
1481 rotate_z( A & a, Angle angle )
1482     {
1483     a *= rotz_quat(angle);
1484     }
1485 
1486 template <class A,class B>
1487 BOOST_QVM_CONSTEXPR BOOST_QVM_INLINE_OPERATIONS
1488 typename enable_if_c<
1489     is_quat<A>::value && is_vec<B>::value && vec_traits<B>::dim==3,
1490     typename quat_traits<A>::scalar_type>::type
1491 axis_angle( A const & a, B & b )
1492     {
1493     typedef typename quat_traits<A>::scalar_type T;
1494     T a0=quat_traits<A>::template read_element<0>(a);
1495     T a1=quat_traits<A>::template read_element<1>(a);
1496     T a2=quat_traits<A>::template read_element<2>(a);
1497     T a3=quat_traits<A>::template read_element<3>(a);
1498     if( a0>1 )
1499         {
1500         T const m2=a0*a0+a1*a1+a2*a2+a3*a3;
1501         if( m2==scalar_traits<T>::value(0) )
1502             BOOST_QVM_THROW_EXCEPTION(zero_magnitude_error());
1503         T const s=sqrt(m2);
1504         a0/=s;
1505         a1/=s;
1506         a2/=s;
1507         a3/=s;
1508         }
1509     if( T s=sqrt(1-a0*a0) )
1510         {
1511         write_vec_element<0>(b, a1/s);
1512         write_vec_element<1>(b, a2/s);
1513         write_vec_element<2>(b, a3/s);
1514         }
1515     else
1516         {
1517         typedef typename vec_traits<B>::scalar_type U;
1518         write_vec_element<0>(b, scalar_traits<U>::value(1));
1519         write_vec_element<1>(b, scalar_traits<U>::value(0));
1520         write_vec_element<2>(b, scalar_traits<U>::value(0));
1521         }
1522     return scalar_traits<T>::value(2) * qvm::acos(a0);
1523     }
1524 
1525 ////////////////////////////////////////////////
1526 
1527 namespace
1528 sfinae
1529     {
1530     using ::boost::qvm::assign;
1531     using ::boost::qvm::cmp;
1532     using ::boost::qvm::convert_to;
1533     using ::boost::qvm::conjugate;
1534     using ::boost::qvm::set_identity;
1535     using ::boost::qvm::set_zero;
1536     using ::boost::qvm::scalar_cast;
1537     using ::boost::qvm::operator/=;
1538     using ::boost::qvm::operator/;
1539     using ::boost::qvm::dot;
1540     using ::boost::qvm::operator==;
1541     using ::boost::qvm::inverse;
1542     using ::boost::qvm::mag_sqr;
1543     using ::boost::qvm::mag;
1544     using ::boost::qvm::slerp360;
1545     using ::boost::qvm::slerp180;
1546     using ::boost::qvm::slerp;
1547     using ::boost::qvm::operator-=;
1548     using ::boost::qvm::operator-;
1549     using ::boost::qvm::operator*=;
1550     using ::boost::qvm::operator*;
1551     using ::boost::qvm::operator!=;
1552     using ::boost::qvm::normalized;
1553     using ::boost::qvm::normalize;
1554     using ::boost::qvm::operator+=;
1555     using ::boost::qvm::operator+;
1556     using ::boost::qvm::qref;
1557     using ::boost::qvm::rot_quat;
1558     using ::boost::qvm::set_rot;
1559     using ::boost::qvm::rotate;
1560     using ::boost::qvm::rotx_quat;
1561     using ::boost::qvm::set_rotx;
1562     using ::boost::qvm::rotate_x;
1563     using ::boost::qvm::roty_quat;
1564     using ::boost::qvm::set_roty;
1565     using ::boost::qvm::rotate_y;
1566     using ::boost::qvm::rotz_quat;
1567     using ::boost::qvm::set_rotz;
1568     using ::boost::qvm::rotate_z;
1569     }
1570 
1571 } }
1572 
1573 #endif
[ Source navigation ] [ Diff markup ] [ Identifier search ] [ general search ]

This page was automatically generated by the 2.3.7 LXR engine.

The LXR team
Valid CSS 2.1! Valid HTML 4.01!