Back to home page

EIC code displayed by LXR
 
 

    

File indexing completed on 2025-09-15 08:51:38 

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