OTS/OTSCPP/Tools/boost/qvm/quat_operations.hpp

//Copyright (c) 2008-2016 Emil Dotchevski and Reverge Studios, Inc.

//Distributed under the Boost Software License, Version 1.0. (See accompanying
//file LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)

#ifndef BOOST_QVM_E6519754D19211DFB8405F74DFD72085
#define BOOST_QVM_E6519754D19211DFB8405F74DFD72085

#include <boost/qvm/detail/quat_assign.hpp>
#include <boost/qvm/deduce_quat.hpp>
#include <boost/qvm/mat_traits.hpp>
#include <boost/qvm/scalar_traits.hpp>
#include <boost/qvm/math.hpp>
#include <boost/qvm/assert.hpp>
#include <boost/qvm/error.hpp>
#include <boost/qvm/throw_exception.hpp>
#include <string>

namespace
boost
    {
    namespace
    qvm
        {
        namespace
        qvm_detail
            {
            BOOST_QVM_INLINE_CRITICAL
            void const *
            get_valid_ptr_quat_operations()
                {
                static int const obj=0;
                return &obj;
                }
            }

        ////////////////////////////////////////////////

        namespace
        msvc_parse_bug_workaround
            {
            template <class A,class B>
            struct
            quats
                {
                static bool const value=is_quat<A>::value && is_quat<B>::value;
                };
            }

        namespace
        qvm_to_string_detail
            {
            template <class T>
            std::string to_string( T const & x );
            }

        template <class A>
        inline
        typename boost::enable_if_c<
            is_quat<A>::value,
            std::string>::type
        to_string( A const & a )
            {
            using namespace qvm_to_string_detail;
            return '('+
                to_string(quat_traits<A>::template read_element<0>(a))+','+
                to_string(quat_traits<A>::template read_element<1>(a))+','+
                to_string(quat_traits<A>::template read_element<2>(a))+','+
                to_string(quat_traits<A>::template read_element<3>(a))+')';
            }

        ////////////////////////////////////////////////

        template <class A,class B,class Cmp>
        BOOST_QVM_INLINE_OPERATIONS
        typename enable_if_c<
            is_quat<A>::value && is_quat<B>::value,
            bool>::type
        cmp( A const & a, B const & b, Cmp f )
            {
            typedef typename quat_traits<A>::scalar_type T;
            typedef typename quat_traits<B>::scalar_type U;
            T q1[4] =
                {
                quat_traits<A>::template read_element<0>(a),
                quat_traits<A>::template read_element<1>(a),
                quat_traits<A>::template read_element<2>(a),
                quat_traits<A>::template read_element<3>(a)
                };
            U q2[4] =
                {
                quat_traits<B>::template read_element<0>(b),
                quat_traits<B>::template read_element<1>(b),
                quat_traits<B>::template read_element<2>(b),
                quat_traits<B>::template read_element<3>(b)
                };
            int i;
            for( i=0; i!=4; ++i )
                if( !f(q1[i],q2[i]) )
                    break;
            if( i==4 )
                return true;
            for( i=0; i!=4; ++i )
                if( !f(q1[i],-q2[i]) )
                    return false;
            return true;
            }

        ////////////////////////////////////////////////

        template <class R,class A>
        BOOST_QVM_INLINE_TRIVIAL
        typename enable_if_c<
            is_quat<R>::value && is_quat<A>::value,
            R>::type
        convert_to( A const & a )
            {
            R r;
            quat_traits<R>::template write_element<0>(r) = quat_traits<A>::template read_element<0>(a);
            quat_traits<R>::template write_element<1>(r) = quat_traits<A>::template read_element<1>(a);
            quat_traits<R>::template write_element<2>(r) = quat_traits<A>::template read_element<2>(a);
            quat_traits<R>::template write_element<3>(r) = quat_traits<A>::template read_element<3>(a);
            return r;
            }

        template <class R,class A>
        BOOST_QVM_INLINE_OPERATIONS
        typename enable_if_c<
            is_quat<R>::value && is_mat<A>::value &&
            mat_traits<A>::rows==3 && mat_traits<A>::cols==3,
            R>::type
        convert_to( A const & a )
            {
            typedef typename mat_traits<A>::scalar_type T;
            T const mat[3][3] =
                {
                    { 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) },
                    { 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) },
                    { 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) }
                };
            R r;
            if( mat[0][0]+mat[1][1]+mat[2][2] > scalar_traits<T>::value(0) )
                {
                T t = mat[0][0] + mat[1][1] + mat[2][2] + scalar_traits<T>::value(1);
                T s = (scalar_traits<T>::value(1)/sqrt<T>(t))/2;
                quat_traits<R>::template write_element<0>(r)=s*t;
                quat_traits<R>::template write_element<1>(r)=(mat[2][1]-mat[1][2])*s;
                quat_traits<R>::template write_element<2>(r)=(mat[0][2]-mat[2][0])*s;
                quat_traits<R>::template write_element<3>(r)=(mat[1][0]-mat[0][1])*s;
                }
            else if( mat[0][0]>mat[1][1] && mat[0][0]>mat[2][2] )
                {
                T t = mat[0][0] - mat[1][1] - mat[2][2] + scalar_traits<T>::value(1);
                T s = (scalar_traits<T>::value(1)/sqrt<T>(t))/2;
                quat_traits<R>::template write_element<0>(r)=(mat[2][1]-mat[1][2])*s;
                quat_traits<R>::template write_element<1>(r)=s*t;
                quat_traits<R>::template write_element<2>(r)=(mat[1][0]+mat[0][1])*s;
                quat_traits<R>::template write_element<3>(r)=(mat[0][2]+mat[2][0])*s;
                }
            else if( mat[1][1]>mat[2][2] )
                {
                T t = - mat[0][0] + mat[1][1] - mat[2][2] + scalar_traits<T>::value(1);
                T s = (scalar_traits<T>::value(1)/sqrt<T>(t))/2;
                quat_traits<R>::template write_element<0>(r)=(mat[0][2]-mat[2][0])*s;
                quat_traits<R>::template write_element<1>(r)=(mat[1][0]+mat[0][1])*s;
                quat_traits<R>::template write_element<2>(r)=s*t;
                quat_traits<R>::template write_element<3>(r)=(mat[2][1]+mat[1][2])*s;
                }
            else
                {
                T t = - mat[0][0] - mat[1][1] + mat[2][2] + scalar_traits<T>::value(1);
                T s = (scalar_traits<T>::value(1)/sqrt<T>(t))/2;
                quat_traits<R>::template write_element<0>(r)=(mat[1][0]-mat[0][1])*s;
                quat_traits<R>::template write_element<1>(r)=(mat[0][2]+mat[2][0])*s;
                quat_traits<R>::template write_element<2>(r)=(mat[2][1]+mat[1][2])*s;
                quat_traits<R>::template write_element<3>(r)=s*t;
                }
            return r;
            }

        ////////////////////////////////////////////////

        template <class A>
        BOOST_QVM_INLINE_OPERATIONS
        typename lazy_enable_if_c<
            is_quat<A>::value,
            deduce_quat<A> >::type
        conjugate( A const & a )
            {
            typedef typename deduce_quat<A>::type R;
            R r;
            quat_traits<R>::template write_element<0>(r)=quat_traits<A>::template read_element<0>(a);
            quat_traits<R>::template write_element<1>(r)=-quat_traits<A>::template read_element<1>(a);
            quat_traits<R>::template write_element<2>(r)=-quat_traits<A>::template read_element<2>(a);
            quat_traits<R>::template write_element<3>(r)=-quat_traits<A>::template read_element<3>(a);
            return r;
            }

        ////////////////////////////////////////////////

        namespace
        qvm_detail
            {
            template <class T>
            class
            identity_quat_
                {
                identity_quat_( identity_quat_ const & );
                identity_quat_ & operator=( identity_quat_ const & );
                ~identity_quat_();

                public:

                template <class R>
                BOOST_QVM_INLINE_TRIVIAL
                operator R() const
                    {
                    R r;
                    assign(r,*this);
                    return r;
                    }
                };
            }

        template <class T>
        struct
        quat_traits< qvm_detail::identity_quat_<T> >
            {
            typedef qvm_detail::identity_quat_<T> this_quaternion;
            typedef T scalar_type;

            template <int I>
            static
            BOOST_QVM_INLINE_CRITICAL
            scalar_type
            read_element( this_quaternion const & x )
                {
                BOOST_QVM_STATIC_ASSERT(I>=0);
                BOOST_QVM_STATIC_ASSERT(I<4);
                return scalar_traits<T>::value(I==0);
                }

            static
            BOOST_QVM_INLINE_CRITICAL
            scalar_type
            read_element_idx( int i, this_quaternion const & x )
                {
                BOOST_QVM_ASSERT(i>=0);
                BOOST_QVM_ASSERT(i<4);
                return scalar_traits<T>::value(i==0);
                }
            };

        template <class T>
        struct
        deduce_quat< qvm_detail::identity_quat_<T> >
            {
            typedef quat<T> type;
            };

        template <class T>
        struct
        deduce_quat2< qvm_detail::identity_quat_<T>, qvm_detail::identity_quat_<T> >
            {
            typedef quat<T> type;
            };

        template <class T>
        BOOST_QVM_INLINE_TRIVIAL
        qvm_detail::identity_quat_<T> const &
        identity_quat()
            {
            return *(qvm_detail::identity_quat_<T> const *)qvm_detail::get_valid_ptr_quat_operations();
            }

        template <class A>
        BOOST_QVM_INLINE_OPERATIONS
        typename enable_if_c<
            is_quat<A>::value,
            void>::type
        set_identity( A & a )
            {
            typedef typename quat_traits<A>::scalar_type T;
            T const zero=scalar_traits<T>::value(0);
            T const one=scalar_traits<T>::value(1);
            quat_traits<A>::template write_element<0>(a) = one;
            quat_traits<A>::template write_element<1>(a) = zero;
            quat_traits<A>::template write_element<2>(a) = zero;
            quat_traits<A>::template write_element<3>(a) = zero;
            }

        ////////////////////////////////////////////////

        namespace
        qvm_detail
            {
            template <class OriginalType,class Scalar>
            class
            quaternion_scalar_cast_
                {
                quaternion_scalar_cast_( quaternion_scalar_cast_ const & );
                quaternion_scalar_cast_ & operator=( quaternion_scalar_cast_ const & );
                ~quaternion_scalar_cast_();

                public:

                template <class T>
                BOOST_QVM_INLINE_TRIVIAL
                quaternion_scalar_cast_ &
                operator=( T const & x )
                    {
                    assign(*this,x);
                    return *this;
                    }

                template <class R>
                BOOST_QVM_INLINE_TRIVIAL
                operator R() const
                    {
                    R r;
                    assign(r,*this);
                    return r;
                    }
                };

            template <bool> struct scalar_cast_quaternion_filter { };
            template <> struct scalar_cast_quaternion_filter<true> { typedef int type; };
            }

        template <class OriginalType,class Scalar>
        struct
        quat_traits< qvm_detail::quaternion_scalar_cast_<OriginalType,Scalar> >
            {
            typedef Scalar scalar_type;
            typedef qvm_detail::quaternion_scalar_cast_<OriginalType,Scalar> this_quaternion;

            template <int I>
            static
            BOOST_QVM_INLINE_CRITICAL
            scalar_type
            read_element( this_quaternion const & x )
                {
                BOOST_QVM_STATIC_ASSERT(I>=0);
                BOOST_QVM_STATIC_ASSERT(I<4);
                return scalar_type(quat_traits<OriginalType>::template read_element<I>(reinterpret_cast<OriginalType const &>(x)));
                }

            static
            BOOST_QVM_INLINE_CRITICAL
            scalar_type
            read_element_idx( int i, this_quaternion const & x )
                {
                BOOST_QVM_ASSERT(i>=0);
                BOOST_QVM_ASSERT(i<4);
                return scalar_type(quat_traits<OriginalType>::read_element_idx(i,reinterpret_cast<OriginalType const &>(x)));
                }
            };

        template <class Scalar,class T>
        BOOST_QVM_INLINE_TRIVIAL
        qvm_detail::quaternion_scalar_cast_<T,Scalar> const &
        scalar_cast( T const & x, typename qvm_detail::scalar_cast_quaternion_filter<is_quat<T>::value>::type=0 )
            {
            return reinterpret_cast<qvm_detail::quaternion_scalar_cast_<T,Scalar> const &>(x);
            }

        ////////////////////////////////////////////////

        template <class A,class B>
        BOOST_QVM_INLINE_OPERATIONS
        typename enable_if_c<
            is_quat<A>::value && is_scalar<B>::value,
            A &>::type
        operator/=( A & a, B b )
            {
            quat_traits<A>::template write_element<0>(a)/=b;
            quat_traits<A>::template write_element<1>(a)/=b;
            quat_traits<A>::template write_element<2>(a)/=b;
            quat_traits<A>::template write_element<3>(a)/=b;
            return a;
            }

        template <class A,class B>
        BOOST_QVM_INLINE_OPERATIONS
        typename lazy_enable_if_c<
            is_quat<A>::value && is_scalar<B>::value,
            deduce_quat<A> >::type
        operator/( A const & a, B b )
            {
            typedef typename deduce_quat<A>::type R;
            R r;
            quat_traits<R>::template write_element<0>(r) = quat_traits<A>::template read_element<0>(a)/b;
            quat_traits<R>::template write_element<1>(r) = quat_traits<A>::template read_element<1>(a)/b;
            quat_traits<R>::template write_element<2>(r) = quat_traits<A>::template read_element<2>(a)/b;
            quat_traits<R>::template write_element<3>(r) = quat_traits<A>::template read_element<3>(a)/b;
            return r;
            }

        template <class A,class B>
        BOOST_QVM_INLINE_OPERATIONS
        typename lazy_enable_if_c<
            is_quat<A>::value && is_quat<B>::value,
            deduce_scalar<typename quat_traits<A>::scalar_type,typename quat_traits<B>::scalar_type> >::type
        dot( A const & a, B const & b )
            {
            typedef typename quat_traits<A>::scalar_type Ta;
            typedef typename quat_traits<B>::scalar_type Tb;
            typedef typename deduce_scalar<Ta,Tb>::type Tr;
            Ta const a0=quat_traits<A>::template read_element<0>(a);
            Ta const a1=quat_traits<A>::template read_element<1>(a);
            Ta const a2=quat_traits<A>::template read_element<2>(a);
            Ta const a3=quat_traits<A>::template read_element<3>(a);
            Tb const b0=quat_traits<B>::template read_element<0>(b);
            Tb const b1=quat_traits<B>::template read_element<1>(b);
            Tb const b2=quat_traits<B>::template read_element<2>(b);
            Tb const b3=quat_traits<B>::template read_element<3>(b);
            Tr const dp=a0*b0+a1*b1+a2*b2+a3*b3;
            return dp;
            }

        template <class A,class B>
        BOOST_QVM_INLINE_OPERATIONS
        typename enable_if_c<
            is_quat<A>::value && is_quat<B>::value,
            bool>::type
        operator==( A const & a, B const & b )
            {
            return
                quat_traits<A>::template read_element<0>(a)==quat_traits<B>::template read_element<0>(b) &&
                quat_traits<A>::template read_element<1>(a)==quat_traits<B>::template read_element<1>(b) &&
                quat_traits<A>::template read_element<2>(a)==quat_traits<B>::template read_element<2>(b) &&
                quat_traits<A>::template read_element<3>(a)==quat_traits<B>::template read_element<3>(b);
            }

        template <class A>
        BOOST_QVM_INLINE_OPERATIONS
        typename lazy_enable_if_c<
            is_quat<A>::value,
            deduce_quat<A> >::type
        inverse( A const & a )
            {
            typedef typename deduce_quat<A>::type R;
            typedef typename quat_traits<A>::scalar_type TA;
            TA aa = quat_traits<A>::template read_element<0>(a);
            TA ab = quat_traits<A>::template read_element<1>(a);
            TA ac = quat_traits<A>::template read_element<2>(a);
            TA ad = quat_traits<A>::template read_element<3>(a);
            TA m2 = ab*ab + ac*ac + ad*ad + aa*aa;
            if( m2==scalar_traits<TA>::value(0) )
                BOOST_QVM_THROW_EXCEPTION(zero_magnitude_error());
            TA rm=scalar_traits<TA>::value(1)/m2;
            R r;
            quat_traits<R>::template write_element<0>(r) = aa*rm;
            quat_traits<R>::template write_element<1>(r) = -ab*rm;
            quat_traits<R>::template write_element<2>(r) = -ac*rm;
            quat_traits<R>::template write_element<3>(r) = -ad*rm;
            return r;
            }

        template <class A>
        BOOST_QVM_INLINE_OPERATIONS
        typename enable_if_c<
            is_quat<A>::value,
            typename quat_traits<A>::scalar_type>::type
        mag_sqr( A const & a )
            {
            typedef typename quat_traits<A>::scalar_type T;
            T x=quat_traits<A>::template read_element<0>(a);
            T y=quat_traits<A>::template read_element<1>(a);
            T z=quat_traits<A>::template read_element<2>(a);
            T w=quat_traits<A>::template read_element<3>(a);
            return x*x+y*y+z*z+w*w;
            }

        template <class A>
        BOOST_QVM_INLINE_OPERATIONS
        typename enable_if_c<
            is_quat<A>::value,
            typename quat_traits<A>::scalar_type>::type
        mag( A const & a )
            {
            typedef typename quat_traits<A>::scalar_type T;
            T x=quat_traits<A>::template read_element<0>(a);
            T y=quat_traits<A>::template read_element<1>(a);
            T z=quat_traits<A>::template read_element<2>(a);
            T w=quat_traits<A>::template read_element<3>(a);
            return sqrt<T>(x*x+y*y+z*z+w*w);
            }

        template <class A,class B>
        BOOST_QVM_INLINE_OPERATIONS
        typename enable_if<
            msvc_parse_bug_workaround::quats<A,B>,
            A &>::type
        operator-=( A & a, B const & b )
            {
            quat_traits<A>::template write_element<0>(a)-=quat_traits<B>::template read_element<0>(b);
            quat_traits<A>::template write_element<1>(a)-=quat_traits<B>::template read_element<1>(b);
            quat_traits<A>::template write_element<2>(a)-=quat_traits<B>::template read_element<2>(b);
            quat_traits<A>::template write_element<3>(a)-=quat_traits<B>::template read_element<3>(b);
            return a;
            }

        template <class A,class B>
        BOOST_QVM_INLINE_OPERATIONS
        typename lazy_enable_if_c<
            is_quat<A>::value && is_quat<B>::value,
            deduce_quat2<A,B> >::type
        operator-( A const & a, B const & b )
            {
            typedef typename deduce_quat2<A,B>::type R;
            R r;
            quat_traits<R>::template write_element<0>(r)=quat_traits<A>::template read_element<0>(a)-quat_traits<B>::template read_element<0>(b);
            quat_traits<R>::template write_element<1>(r)=quat_traits<A>::template read_element<1>(a)-quat_traits<B>::template read_element<1>(b);
            quat_traits<R>::template write_element<2>(r)=quat_traits<A>::template read_element<2>(a)-quat_traits<B>::template read_element<2>(b);
            quat_traits<R>::template write_element<3>(r)=quat_traits<A>::template read_element<3>(a)-quat_traits<B>::template read_element<3>(b);
            return r;
            }

        template <class A>
        BOOST_QVM_INLINE_OPERATIONS
        typename lazy_enable_if_c<
            is_quat<A>::value,
            deduce_quat<A> >::type
        operator-( A const & a )
            {
            typedef typename deduce_quat<A>::type R;
            R r;
            quat_traits<R>::template write_element<0>(r)=-quat_traits<A>::template read_element<0>(a);
            quat_traits<R>::template write_element<1>(r)=-quat_traits<A>::template read_element<1>(a);
            quat_traits<R>::template write_element<2>(r)=-quat_traits<A>::template read_element<2>(a);
            quat_traits<R>::template write_element<3>(r)=-quat_traits<A>::template read_element<3>(a);
            return r;
            }

        template <class A,class B>
        BOOST_QVM_INLINE_OPERATIONS
        typename enable_if<
            msvc_parse_bug_workaround::quats<A,B>,
            A &>::type
        operator*=( A & a, B const & b )
            {
            typedef typename quat_traits<A>::scalar_type TA;
            typedef typename quat_traits<B>::scalar_type TB;
            TA const aa=quat_traits<A>::template read_element<0>(a);
            TA const ab=quat_traits<A>::template read_element<1>(a);
            TA const ac=quat_traits<A>::template read_element<2>(a);
            TA const ad=quat_traits<A>::template read_element<3>(a);
            TB const ba=quat_traits<B>::template read_element<0>(b);
            TB const bb=quat_traits<B>::template read_element<1>(b);
            TB const bc=quat_traits<B>::template read_element<2>(b);
            TB const bd=quat_traits<B>::template read_element<3>(b);
            quat_traits<A>::template write_element<0>(a) = aa*ba - ab*bb - ac*bc - ad*bd;
            quat_traits<A>::template write_element<1>(a) = aa*bb + ab*ba + ac*bd - ad*bc;
            quat_traits<A>::template write_element<2>(a) = aa*bc + ac*ba + ad*bb - ab*bd;
            quat_traits<A>::template write_element<3>(a) = aa*bd + ad*ba + ab*bc - ac*bb;
            return a;
            }

        template <class A,class B>
        BOOST_QVM_INLINE_OPERATIONS
        typename enable_if_c<
            is_quat<A>::value && is_scalar<B>::value,
            A &>::type
        operator*=( A & a, B b )
            {
            quat_traits<A>::template write_element<0>(a)*=b;
            quat_traits<A>::template write_element<1>(a)*=b;
            quat_traits<A>::template write_element<2>(a)*=b;
            quat_traits<A>::template write_element<3>(a)*=b;
            return a;
            }

        template <class A,class B>
        BOOST_QVM_INLINE_OPERATIONS
        typename lazy_enable_if_c<
            is_quat<A>::value && is_quat<B>::value,
            deduce_quat2<A,B> >::type
        operator*( A const & a, B const & b )
            {
            typedef typename deduce_quat2<A,B>::type R;
            typedef typename quat_traits<A>::scalar_type TA;
            typedef typename quat_traits<B>::scalar_type TB;
            TA const aa=quat_traits<A>::template read_element<0>(a);
            TA const ab=quat_traits<A>::template read_element<1>(a);
            TA const ac=quat_traits<A>::template read_element<2>(a);
            TA const ad=quat_traits<A>::template read_element<3>(a);
            TB const ba=quat_traits<B>::template read_element<0>(b);
            TB const bb=quat_traits<B>::template read_element<1>(b);
            TB const bc=quat_traits<B>::template read_element<2>(b);
            TB const bd=quat_traits<B>::template read_element<3>(b);
            R r;
            quat_traits<R>::template write_element<0>(r) = aa*ba - ab*bb - ac*bc - ad*bd;
            quat_traits<R>::template write_element<1>(r) = aa*bb + ab*ba + ac*bd - ad*bc;
            quat_traits<R>::template write_element<2>(r) = aa*bc + ac*ba + ad*bb - ab*bd;
            quat_traits<R>::template write_element<3>(r) = aa*bd + ad*ba + ab*bc - ac*bb;
            return r;
            }

        template <class A,class B>
        BOOST_QVM_INLINE_OPERATIONS
        typename lazy_enable_if_c<
            is_quat<A>::value && is_scalar<B>::value,
            deduce_quat<A> >::type
        operator*( A const & a, B b )
            {
            typedef typename deduce_quat<A>::type R;
            R r;
            quat_traits<R>::template write_element<0>(r)=quat_traits<A>::template read_element<0>(a)*b;
            quat_traits<R>::template write_element<1>(r)=quat_traits<A>::template read_element<1>(a)*b;
            quat_traits<R>::template write_element<2>(r)=quat_traits<A>::template read_element<2>(a)*b;
            quat_traits<R>::template write_element<3>(r)=quat_traits<A>::template read_element<3>(a)*b;
            return r;
            }

        template <class A,class B>
        BOOST_QVM_INLINE_OPERATIONS
        typename enable_if_c<
            is_quat<A>::value && is_quat<B>::value,
            bool>::type
        operator!=( A const & a, B const & b )
            {
            return
                quat_traits<A>::template read_element<0>(a)!=quat_traits<B>::template read_element<0>(b) ||
                quat_traits<A>::template read_element<1>(a)!=quat_traits<B>::template read_element<1>(b) ||
                quat_traits<A>::template read_element<2>(a)!=quat_traits<B>::template read_element<2>(b) ||
                quat_traits<A>::template read_element<3>(a)!=quat_traits<B>::template read_element<3>(b);
            }

        template <class A>
        BOOST_QVM_INLINE_OPERATIONS
        typename lazy_enable_if_c<
            is_quat<A>::value,
            deduce_quat<A> >::type
        normalized( A const & a )
            {
            typedef typename quat_traits<A>::scalar_type T;
            T const a0=quat_traits<A>::template read_element<0>(a);
            T const a1=quat_traits<A>::template read_element<1>(a);
            T const a2=quat_traits<A>::template read_element<2>(a);
            T const a3=quat_traits<A>::template read_element<3>(a);
            T const m2=a0*a0+a1*a1+a2*a2+a3*a3;
            if( m2==scalar_traits<typename quat_traits<A>::scalar_type>::value(0) )
                BOOST_QVM_THROW_EXCEPTION(zero_magnitude_error());
            T const rm=scalar_traits<T>::value(1)/sqrt<T>(m2);
            typedef typename deduce_quat<A>::type R;
            R r;
            quat_traits<R>::template write_element<0>(r)=a0*rm;
            quat_traits<R>::template write_element<1>(r)=a1*rm;
            quat_traits<R>::template write_element<2>(r)=a2*rm;
            quat_traits<R>::template write_element<3>(r)=a3*rm;
            return r;
            }

        template <class A>
        BOOST_QVM_INLINE_OPERATIONS
        typename enable_if_c<
            is_quat<A>::value,
            void>::type
        normalize( A & a )
            {
            typedef typename quat_traits<A>::scalar_type T;
            T const a0=quat_traits<A>::template read_element<0>(a);
            T const a1=quat_traits<A>::template read_element<1>(a);
            T const a2=quat_traits<A>::template read_element<2>(a);
            T const a3=quat_traits<A>::template read_element<3>(a);
            T const m2=a0*a0+a1*a1+a2*a2+a3*a3;
            if( m2==scalar_traits<typename quat_traits<A>::scalar_type>::value(0) )
                BOOST_QVM_THROW_EXCEPTION(zero_magnitude_error());
            T const rm=scalar_traits<T>::value(1)/sqrt<T>(m2);
            quat_traits<A>::template write_element<0>(a)*=rm;
            quat_traits<A>::template write_element<1>(a)*=rm;
            quat_traits<A>::template write_element<2>(a)*=rm;
            quat_traits<A>::template write_element<3>(a)*=rm;
            }

        template <class A,class B>
        BOOST_QVM_INLINE_OPERATIONS
        typename enable_if<
            msvc_parse_bug_workaround::quats<A,B>,
            A &>::type
        operator+=( A & a, B const & b )
            {
            quat_traits<A>::template write_element<0>(a)+=quat_traits<B>::template read_element<0>(b);
            quat_traits<A>::template write_element<1>(a)+=quat_traits<B>::template read_element<1>(b);
            quat_traits<A>::template write_element<2>(a)+=quat_traits<B>::template read_element<2>(b);
            quat_traits<A>::template write_element<3>(a)+=quat_traits<B>::template read_element<3>(b);
            return a;
            }

        template <class A,class B>
        BOOST_QVM_INLINE_OPERATIONS
        typename lazy_enable_if_c<
            is_quat<A>::value && is_quat<B>::value,
            deduce_quat2<A,B> >::type
        operator+( A const & a, B const & b )
            {
            typedef typename deduce_quat2<A,B>::type R;
            R r;
            quat_traits<R>::template write_element<0>(r)=quat_traits<A>::template read_element<0>(a)+quat_traits<B>::template read_element<0>(b);
            quat_traits<R>::template write_element<1>(r)=quat_traits<A>::template read_element<1>(a)+quat_traits<B>::template read_element<1>(b);
            quat_traits<R>::template write_element<2>(r)=quat_traits<A>::template read_element<2>(a)+quat_traits<B>::template read_element<2>(b);
            quat_traits<R>::template write_element<3>(r)=quat_traits<A>::template read_element<3>(a)+quat_traits<B>::template read_element<3>(b);
            return r;
            }

        template <class A,class B,class C>
        BOOST_QVM_INLINE_OPERATIONS
        typename lazy_enable_if_c<
            is_quat<A>::value && is_quat<B>::value && is_scalar<C>::value,
            deduce_quat2<A,B> >::type
        slerp( A const & a, B const & b, C t )
            {
            typedef typename deduce_quat2<A,B>::type R;
            typedef typename quat_traits<R>::scalar_type TR;
            TR const one = scalar_traits<TR>::value(1);
            TR dp = dot(a,b);
            TR sc=one;
            if( dp < one )
                {
                TR const theta = acos<TR>(dp);
                TR const invsintheta = one/sin<TR>(theta);
                TR const scale = sin<TR>(theta*(one-t)) * invsintheta;
                TR const invscale = sin<TR>(theta*t) * invsintheta * sc;
                return a*scale + b*invscale;
                }
            else
                return normalized(a+(b-a)*t);
            }

        ////////////////////////////////////////////////

        namespace
        qvm_detail
            {
            template <class T>
            class
            qref_
                {
                qref_( qref_ const & );
                qref_ & operator=( qref_ const & );
                ~qref_();

                public:

                template <class R>
                BOOST_QVM_INLINE_TRIVIAL
                qref_ &
                operator=( R const & x )
                    {
                    assign(*this,x);
                    return *this;
                    }

                template <class R>
                BOOST_QVM_INLINE_TRIVIAL
                operator R() const
                    {
                    R r;
                    assign(r,*this);
                    return r;
                    }
                };
            }

        template <class Q>
        struct quat_traits;

        template <class Q>
        struct
        quat_traits< qvm_detail::qref_<Q> >
            {
            typedef typename quat_traits<Q>::scalar_type scalar_type;
            typedef qvm_detail::qref_<Q> this_quaternion;

            template <int I>
            static
            BOOST_QVM_INLINE_CRITICAL
            scalar_type
            read_element( this_quaternion const & x )
                {
                BOOST_QVM_STATIC_ASSERT(I>=0);
                BOOST_QVM_STATIC_ASSERT(I<4);
                return quat_traits<Q>::template read_element<I>(reinterpret_cast<Q const &>(x));
                }

            template <int I>
            static
            BOOST_QVM_INLINE_CRITICAL
            scalar_type &
            write_element( this_quaternion & x )
                {
                BOOST_QVM_STATIC_ASSERT(I>=0);
                BOOST_QVM_STATIC_ASSERT(I<4);
                return quat_traits<Q>::template write_element<I>(reinterpret_cast<Q &>(x));
                }
            };

        template <class Q>
        struct
        deduce_quat< qvm_detail::qref_<Q> >
            {
            typedef quat<typename quat_traits<Q>::scalar_type> type;
            };

        template <class Q>
        BOOST_QVM_INLINE_TRIVIAL
        typename enable_if_c<
            is_quat<Q>::value,
            qvm_detail::qref_<Q> const &>::type
        qref( Q const & a )
            {
            return reinterpret_cast<qvm_detail::qref_<Q> const &>(a);
            }

        template <class Q>
        BOOST_QVM_INLINE_TRIVIAL
        typename enable_if_c<
            is_quat<Q>::value,
            qvm_detail::qref_<Q> &>::type
        qref( Q & a )
            {
            return reinterpret_cast<qvm_detail::qref_<Q> &>(a);
            }

        ////////////////////////////////////////////////

        namespace
        qvm_detail
            {
            template <class T>
            class
            zero_q_
                {
                zero_q_( zero_q_ const & );
                zero_q_ & operator=( zero_q_ const & );
                ~zero_q_();

                public:

                template <class R>
                BOOST_QVM_INLINE_TRIVIAL
                operator R() const
                    {
                    R r;
                    assign(r,*this);
                    return r;
                    }
                };
            }

        template <class T>
        struct
        quat_traits< qvm_detail::zero_q_<T> >
            {
            typedef qvm_detail::zero_q_<T> this_quaternion;
            typedef T scalar_type;

            template <int I>
            static
            BOOST_QVM_INLINE_CRITICAL
            scalar_type
            read_element( this_quaternion const & x )
                {
                BOOST_QVM_STATIC_ASSERT(I>=0);
                BOOST_QVM_STATIC_ASSERT(I<4);
                return scalar_traits<scalar_type>::value(0);
                }

            static
            BOOST_QVM_INLINE_CRITICAL
            scalar_type
            read_element_idx( int i, this_quaternion const & x )
                {
                BOOST_QVM_ASSERT(i>=0);
                BOOST_QVM_ASSERT(i<4);
                return scalar_traits<scalar_type>::value(0);
                }
            };

        template <class T>
        BOOST_QVM_INLINE_TRIVIAL
        qvm_detail::zero_q_<T> const &
        zero_quat()
            {
            return *(qvm_detail::zero_q_<T> const *)qvm_detail::get_valid_ptr_quat_operations();
            }

        template <class A>
        BOOST_QVM_INLINE_OPERATIONS
        typename enable_if_c<
            is_quat<A>::value,
            void>::type
        set_zero( A & a )
            {
            typedef typename quat_traits<A>::scalar_type T;
            T const zero=scalar_traits<T>::value(0);
            quat_traits<A>::template write_element<0>(a) = zero;
            quat_traits<A>::template write_element<1>(a) = zero;
            quat_traits<A>::template write_element<2>(a) = zero;
            quat_traits<A>::template write_element<3>(a) = zero;
            }

        ////////////////////////////////////////////////

        namespace
        qvm_detail
            {
            template <class V>
            struct
            rot_quat_
                {
                typedef typename vec_traits<V>::scalar_type scalar_type;
                scalar_type a[4];

                template <class Angle>
                BOOST_QVM_INLINE
                rot_quat_( V const & axis, Angle angle )
                    {
                    scalar_type const x=vec_traits<V>::template read_element<0>(axis);
                    scalar_type const y=vec_traits<V>::template read_element<1>(axis);
                    scalar_type const z=vec_traits<V>::template read_element<2>(axis);
                    scalar_type const m2=x*x+y*y+z*z;
                    if( m2==scalar_traits<scalar_type>::value(0) )
                        BOOST_QVM_THROW_EXCEPTION(zero_magnitude_error());
                    scalar_type const rm=scalar_traits<scalar_type>::value(1)/sqrt<scalar_type>(m2);
                    angle/=2;
                    scalar_type const s=sin<Angle>(angle);
                    a[0] = cos<Angle>(angle);
                    a[1] = rm*x*s;
                    a[2] = rm*y*s;
                    a[3] = rm*z*s;
                    }

                template <class R>
                BOOST_QVM_INLINE_TRIVIAL
                operator R() const
                    {
                    R r;
                    assign(r,*this);
                    return r;
                    }
                };
            }

        template <class V>
        struct
        quat_traits< qvm_detail::rot_quat_<V> >
            {
            typedef qvm_detail::rot_quat_<V> this_quaternion;
            typedef typename this_quaternion::scalar_type scalar_type;

            template <int I>
            static
            BOOST_QVM_INLINE_CRITICAL
            scalar_type
            read_element( this_quaternion const & x )
                {
                BOOST_QVM_STATIC_ASSERT(I>=0);
                BOOST_QVM_STATIC_ASSERT(I<4);
                return x.a[I];
                }
            };

        template <class V>
        struct
        deduce_quat< qvm_detail::rot_quat_<V> >
            {
            typedef quat<typename vec_traits<V>::scalar_type> type;
            };

        template <class A,class Angle>
        BOOST_QVM_INLINE
        typename enable_if_c<
            is_vec<A>::value && vec_traits<A>::dim==3,
            qvm_detail::rot_quat_<A> >::type
        rot_quat( A const & axis, Angle angle )
            {
            return qvm_detail::rot_quat_<A>(axis,angle);
            }

        template <class A,class B,class Angle>
        BOOST_QVM_INLINE_OPERATIONS
        typename enable_if_c<
            is_quat<A>::value &&
            is_vec<B>::value && vec_traits<B>::dim==3,
            void>::type
        set_rot( A & a, B const & axis, Angle angle )
            {
            assign(a,rot_quat(axis,angle));
            }

        template <class A,class B,class Angle>
        BOOST_QVM_INLINE_OPERATIONS
        typename enable_if_c<
            is_quat<A>::value &&
            is_vec<B>::value && vec_traits<B>::dim==3,
            void>::type
        rotate( A & a, B const & axis, Angle angle )
            {
            a *= rot_quat(axis,angle);
            }

        ////////////////////////////////////////////////

        namespace
        qvm_detail
            {
            template <class T>
            struct
            rotx_quat_
                {
                BOOST_QVM_INLINE_TRIVIAL
                rotx_quat_()
                    {
                    }

                template <class R>
                BOOST_QVM_INLINE_TRIVIAL
                operator R() const
                    {
                    R r;
                    assign(r,*this);
                    return r;
                    }

                private:

                rotx_quat_( rotx_quat_ const & );
                rotx_quat_ & operator=( rotx_quat_ const & );
                ~rotx_quat_();
                };

            template <int I>
            struct
            rotx_q_get
                {
                template <class T>
                static
                BOOST_QVM_INLINE_CRITICAL
                T
                get( T const & )
                    {
                    return scalar_traits<T>::value(0);
                    }
                };

            template <>
            struct
            rotx_q_get<1>
                {
                template <class T>
                static
                BOOST_QVM_INLINE_CRITICAL
                T
                get( T const & angle )
                    {
                    return sin<T>(angle/2);
                    }
                };

            template <>
            struct
            rotx_q_get<0>
                {
                template <class T>
                static
                BOOST_QVM_INLINE_CRITICAL
                T
                get( T const & angle )
                    {
                    return cos<T>(angle/2);
                    }
                };
            }

        template <class Angle>
        struct
        quat_traits< qvm_detail::rotx_quat_<Angle> >
            {
            typedef qvm_detail::rotx_quat_<Angle> this_quaternion;
            typedef Angle scalar_type;

            template <int I>
            static
            BOOST_QVM_INLINE_CRITICAL
            scalar_type
            read_element( this_quaternion const & x )
                {
                BOOST_QVM_STATIC_ASSERT(I>=0);
                BOOST_QVM_STATIC_ASSERT(I<4);
                return qvm_detail::rotx_q_get<I>::get(reinterpret_cast<Angle const &>(x));
                }
            };

        template <class Angle>
        struct
        deduce_quat< qvm_detail::rotx_quat_<Angle> >
            {
            typedef quat<Angle> type;
            };

        template <class Angle>
        struct
        deduce_quat2< qvm_detail::rotx_quat_<Angle>, qvm_detail::rotx_quat_<Angle> >
            {
            typedef quat<Angle> type;
            };

        template <class Angle>
        BOOST_QVM_INLINE_TRIVIAL
        qvm_detail::rotx_quat_<Angle> const &
        rotx_quat( Angle const & angle )
            {
            return reinterpret_cast<qvm_detail::rotx_quat_<Angle> const &>(angle);
            }

        template <class A,class Angle>
        BOOST_QVM_INLINE_OPERATIONS
        typename enable_if_c<
            is_quat<A>::value,
            void>::type
        set_rotx( A & a, Angle angle )
            {
            assign(a,rotx_quat(angle));
            }

        template <class A,class Angle>
        BOOST_QVM_INLINE_OPERATIONS
        typename enable_if_c<
            is_quat<A>::value,
            void>::type
        rotate_x( A & a, Angle angle )
            {
            a *= rotx_quat(angle);
            }

        ////////////////////////////////////////////////

        namespace
        qvm_detail
            {
            template <class T>
            struct
            roty_quat_
                {
                BOOST_QVM_INLINE_TRIVIAL
                roty_quat_()
                    {
                    }

                template <class R>
                BOOST_QVM_INLINE_TRIVIAL
                operator R() const
                    {
                    R r;
                    assign(r,*this);
                    return r;
                    }

                private:

                roty_quat_( roty_quat_ const & );
                roty_quat_ & operator=( roty_quat_ const & );
                ~roty_quat_();
                };

            template <int I>
            struct
            roty_q_get
                {
                template <class T>
                static
                BOOST_QVM_INLINE_CRITICAL
                T
                get( T const & )
                    {
                    return scalar_traits<T>::value(0);
                    }
                };

            template <>
            struct
            roty_q_get<2>
                {
                template <class T>
                static
                BOOST_QVM_INLINE_CRITICAL
                T
                get( T const & angle )
                    {
                    return sin<T>(angle/2);
                    }
                };

            template <>
            struct
            roty_q_get<0>
                {
                template <class T>
                static
                BOOST_QVM_INLINE_CRITICAL
                T
                get( T const & angle )
                    {
                    return cos<T>(angle/2);
                    }
                };
            }

        template <class Angle>
        struct
        quat_traits< qvm_detail::roty_quat_<Angle> >
            {
            typedef qvm_detail::roty_quat_<Angle> this_quaternion;
            typedef Angle scalar_type;

            template <int I>
            static
            BOOST_QVM_INLINE_CRITICAL
            scalar_type
            read_element( this_quaternion const & x )
                {
                BOOST_QVM_STATIC_ASSERT(I>=0);
                BOOST_QVM_STATIC_ASSERT(I<4);
                return qvm_detail::roty_q_get<I>::get(reinterpret_cast<Angle const &>(x));
                }
            };

        template <class Angle>
        struct
        deduce_quat< qvm_detail::roty_quat_<Angle> >
            {
            typedef quat<Angle> type;
            };

        template <class Angle>
        struct
        deduce_quat2< qvm_detail::roty_quat_<Angle>, qvm_detail::roty_quat_<Angle> >
            {
            typedef quat<Angle> type;
            };

        template <class Angle>
        BOOST_QVM_INLINE_TRIVIAL
        qvm_detail::roty_quat_<Angle> const &
        roty_quat( Angle const & angle )
            {
            return reinterpret_cast<qvm_detail::roty_quat_<Angle> const &>(angle);
            }

        template <class A,class Angle>
        BOOST_QVM_INLINE_OPERATIONS
        typename enable_if_c<
            is_quat<A>::value,
            void>::type
        set_roty( A & a, Angle angle )
            {
            assign(a,roty_quat(angle));
            }

        template <class A,class Angle>
        BOOST_QVM_INLINE_OPERATIONS
        typename enable_if_c<
            is_quat<A>::value,
            void>::type
        rotate_y( A & a, Angle angle )
            {
            a *= roty_quat(angle);
            }

        ////////////////////////////////////////////////

        namespace
        qvm_detail
            {
            template <class T>
            struct
            rotz_quat_
                {
                BOOST_QVM_INLINE_TRIVIAL
                rotz_quat_()
                    {
                    }

                template <class R>
                BOOST_QVM_INLINE_TRIVIAL
                operator R() const
                    {
                    R r;
                    assign(r,*this);
                    return r;
                    }

                private:

                rotz_quat_( rotz_quat_ const & );
                rotz_quat_ & operator=( rotz_quat_ const & );
                ~rotz_quat_();
                };

            template <int I>
            struct
            rotz_q_get
                {
                template <class T>
                static
                BOOST_QVM_INLINE_CRITICAL
                T
                get( T const & )
                    {
                    return scalar_traits<T>::value(0);
                    }
                };

            template <>
            struct
            rotz_q_get<3>
                {
                template <class T>
                static
                BOOST_QVM_INLINE_CRITICAL
                T
                get( T const & angle )
                    {
                    return sin<T>(angle/2);
                    }
                };

            template <>
            struct
            rotz_q_get<0>
                {
                template <class T>
                static
                BOOST_QVM_INLINE_CRITICAL
                T
                get( T const & angle )
                    {
                    return cos<T>(angle/2);
                    }
                };
            }

        template <class Angle>
        struct
        quat_traits< qvm_detail::rotz_quat_<Angle> >
            {
            typedef qvm_detail::rotz_quat_<Angle> this_quaternion;
            typedef Angle scalar_type;

            template <int I>
            static
            BOOST_QVM_INLINE_CRITICAL
            scalar_type
            read_element( this_quaternion const & x )
                {
                BOOST_QVM_STATIC_ASSERT(I>=0);
                BOOST_QVM_STATIC_ASSERT(I<4);
                return qvm_detail::rotz_q_get<I>::get(reinterpret_cast<Angle const &>(x));
                }
            };

        template <class Angle>
        struct
        deduce_quat< qvm_detail::rotz_quat_<Angle> >
            {
            typedef quat<Angle> type;
            };

        template <class Angle>
        struct
        deduce_quat2< qvm_detail::rotz_quat_<Angle>, qvm_detail::rotz_quat_<Angle> >
            {
            typedef quat<Angle> type;
            };

        template <class Angle>
        BOOST_QVM_INLINE_TRIVIAL
        qvm_detail::rotz_quat_<Angle> const &
        rotz_quat( Angle const & angle )
            {
            return reinterpret_cast<qvm_detail::rotz_quat_<Angle> const &>(angle);
            }

        template <class A,class Angle>
        BOOST_QVM_INLINE_OPERATIONS
        typename enable_if_c<
            is_quat<A>::value,
            void>::type
        set_rotz( A & a, Angle angle )
            {
            assign(a,rotz_quat(angle));
            }

        template <class A,class Angle>
        BOOST_QVM_INLINE_OPERATIONS
        typename enable_if_c<
            is_quat<A>::value,
            void>::type
        rotate_z( A & a, Angle angle )
            {
            a *= rotz_quat(angle);
            }

        template <class A,class B>
        BOOST_QVM_INLINE_OPERATIONS
        typename enable_if_c<
            is_quat<A>::value && is_vec<B>::value && vec_traits<B>::dim==3,
            typename quat_traits<A>::scalar_type>::type
        axis_angle( A const & a, B & b )
            {
            typedef typename quat_traits<A>::scalar_type T;
            T a0=quat_traits<A>::template read_element<0>(a);
            T a1=quat_traits<A>::template read_element<1>(a);
            T a2=quat_traits<A>::template read_element<2>(a);
            T a3=quat_traits<A>::template read_element<3>(a);
            if( a0>1 )
                {
                T const m2=a0*a0+a1*a1+a2*a2+a3*a3;
                if( m2==scalar_traits<T>::value(0) )
                    BOOST_QVM_THROW_EXCEPTION(zero_magnitude_error());
                T const s=sqrt<T>(m2);
                a0/=s;
                a1/=s;
                a2/=s;
                a3/=s;
                }
            if( T s=sqrt<T>(1-a0*a0) )
                {
                vec_traits<B>::template write_element<0>(b) = a1/s;
                vec_traits<B>::template write_element<1>(b) = a2/s;
                vec_traits<B>::template write_element<2>(b) = a3/s;
                }
            else
                {
                typedef typename vec_traits<B>::scalar_type U;
                vec_traits<B>::template write_element<0>(b) = scalar_traits<U>::value(1);
                vec_traits<B>::template write_element<1>(b) = vec_traits<B>::template write_element<2>(b) = scalar_traits<U>::value(0);
                }
            return scalar_traits<T>::value(2) * qvm::acos(a0);
            }

        ////////////////////////////////////////////////

        namespace
        sfinae
            {
            using ::boost::qvm::assign;
            using ::boost::qvm::cmp;
            using ::boost::qvm::convert_to;
            using ::boost::qvm::conjugate;
            using ::boost::qvm::set_identity;
            using ::boost::qvm::set_zero;
            using ::boost::qvm::scalar_cast;
            using ::boost::qvm::operator/=;
            using ::boost::qvm::operator/;
            using ::boost::qvm::dot;
            using ::boost::qvm::operator==;
            using ::boost::qvm::inverse;
            using ::boost::qvm::mag_sqr;
            using ::boost::qvm::mag;
            using ::boost::qvm::slerp;
            using ::boost::qvm::operator-=;
            using ::boost::qvm::operator-;
            using ::boost::qvm::operator*=;
            using ::boost::qvm::operator*;
            using ::boost::qvm::operator!=;
            using ::boost::qvm::normalized;
            using ::boost::qvm::normalize;
            using ::boost::qvm::operator+=;
            using ::boost::qvm::operator+;
            using ::boost::qvm::qref;
            using ::boost::qvm::rot_quat;
            using ::boost::qvm::set_rot;
            using ::boost::qvm::rotate;
            using ::boost::qvm::rotx_quat;
            using ::boost::qvm::set_rotx;
            using ::boost::qvm::rotate_x;
            using ::boost::qvm::roty_quat;
            using ::boost::qvm::set_roty;
            using ::boost::qvm::rotate_y;
            using ::boost::qvm::rotz_quat;
            using ::boost::qvm::set_rotz;
            using ::boost::qvm::rotate_z;
            }

        ////////////////////////////////////////////////
        }
    }

#endif