1549 lines
44 KiB
C++
1549 lines
44 KiB
C++
#ifndef BOOST_QVM_QUAT_OPERATIONS
|
|
#define BOOST_QVM_QUAT_OPERATIONS
|
|
|
|
// Copyright 2008-2022 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)
|
|
|
|
#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>
|
|
#include <cmath>
|
|
|
|
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 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_CONSTEXPR 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=0;
|
|
for( ; 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_CONSTEXPR 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;
|
|
write_quat_element<0>(r,quat_traits<A>::template read_element<0>(a));
|
|
write_quat_element<1>(r,quat_traits<A>::template read_element<1>(a));
|
|
write_quat_element<2>(r,quat_traits<A>::template read_element<2>(a));
|
|
write_quat_element<3>(r,quat_traits<A>::template read_element<3>(a));
|
|
return r;
|
|
}
|
|
|
|
template <class R,class A>
|
|
BOOST_QVM_CONSTEXPR 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))/2;
|
|
write_quat_element<0>(r,s*t);
|
|
write_quat_element<1>(r,(mat[2][1]-mat[1][2])*s);
|
|
write_quat_element<2>(r,(mat[0][2]-mat[2][0])*s);
|
|
write_quat_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))/2;
|
|
write_quat_element<0>(r,(mat[2][1]-mat[1][2])*s);
|
|
write_quat_element<1>(r,s*t);
|
|
write_quat_element<2>(r,(mat[1][0]+mat[0][1])*s);
|
|
write_quat_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))/2;
|
|
write_quat_element<0>(r,(mat[0][2]-mat[2][0])*s);
|
|
write_quat_element<1>(r,(mat[1][0]+mat[0][1])*s);
|
|
write_quat_element<2>(r,s*t);
|
|
write_quat_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))/2;
|
|
write_quat_element<0>(r,(mat[1][0]-mat[0][1])*s);
|
|
write_quat_element<1>(r,(mat[0][2]+mat[2][0])*s);
|
|
write_quat_element<2>(r,(mat[2][1]+mat[1][2])*s);
|
|
write_quat_element<3>(r,s*t);
|
|
}
|
|
return r;
|
|
}
|
|
|
|
////////////////////////////////////////////////
|
|
|
|
template <class A>
|
|
BOOST_QVM_CONSTEXPR 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;
|
|
write_quat_element<0>(r,quat_traits<A>::template read_element<0>(a));
|
|
write_quat_element<1>(r,-quat_traits<A>::template read_element<1>(a));
|
|
write_quat_element<2>(r,-quat_traits<A>::template read_element<2>(a));
|
|
write_quat_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
|
|
#if __cplusplus >= 201103L
|
|
, class = typename enable_if<is_quat<R> >::type
|
|
#endif
|
|
>
|
|
BOOST_QVM_CONSTEXPR 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_CONSTEXPR 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_CONSTEXPR 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_CONSTEXPR 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_CONSTEXPR 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);
|
|
write_quat_element<0>(a,one);
|
|
write_quat_element<1>(a,zero);
|
|
write_quat_element<2>(a,zero);
|
|
write_quat_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_CONSTEXPR BOOST_QVM_INLINE_TRIVIAL
|
|
quaternion_scalar_cast_ &
|
|
operator=( T const & x )
|
|
{
|
|
assign(*this,x);
|
|
return *this;
|
|
}
|
|
|
|
template <class R
|
|
#if __cplusplus >= 201103L
|
|
, class = typename enable_if<is_quat<R> >::type
|
|
#endif
|
|
>
|
|
BOOST_QVM_CONSTEXPR 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_CONSTEXPR 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_CONSTEXPR 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_CONSTEXPR 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_CONSTEXPR BOOST_QVM_INLINE_OPERATIONS
|
|
typename enable_if_c<
|
|
is_quat<A>::value && is_scalar<B>::value,
|
|
A &>::type
|
|
operator/=( A & a, B b )
|
|
{
|
|
write_quat_element<0>(a,quat_traits<A>::template read_element<0>(a)/b);
|
|
write_quat_element<1>(a,quat_traits<A>::template read_element<1>(a)/b);
|
|
write_quat_element<2>(a,quat_traits<A>::template read_element<2>(a)/b);
|
|
write_quat_element<3>(a,quat_traits<A>::template read_element<3>(a)/b);
|
|
return a;
|
|
}
|
|
|
|
template <class A,class B>
|
|
BOOST_QVM_CONSTEXPR BOOST_QVM_INLINE_OPERATIONS
|
|
typename lazy_enable_if_c<
|
|
is_quat<A>::value && is_scalar<B>::value,
|
|
deduce_quat2<A,B> >::type
|
|
operator/( A const & a, B b )
|
|
{
|
|
typedef typename deduce_quat2<A,B>::type R;
|
|
R r;
|
|
write_quat_element<0>(r,quat_traits<A>::template read_element<0>(a)/b);
|
|
write_quat_element<1>(r,quat_traits<A>::template read_element<1>(a)/b);
|
|
write_quat_element<2>(r,quat_traits<A>::template read_element<2>(a)/b);
|
|
write_quat_element<3>(r,quat_traits<A>::template read_element<3>(a)/b);
|
|
return r;
|
|
}
|
|
|
|
template <class A,class B>
|
|
BOOST_QVM_CONSTEXPR 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_CONSTEXPR 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_CONSTEXPR 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;
|
|
write_quat_element<0>(r,aa*rm);
|
|
write_quat_element<1>(r,-ab*rm);
|
|
write_quat_element<2>(r,-ac*rm);
|
|
write_quat_element<3>(r,-ad*rm);
|
|
return r;
|
|
}
|
|
|
|
template <class A>
|
|
BOOST_QVM_CONSTEXPR 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_CONSTEXPR 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(x*x+y*y+z*z+w*w);
|
|
}
|
|
|
|
template <class A,class B>
|
|
BOOST_QVM_CONSTEXPR BOOST_QVM_INLINE_OPERATIONS
|
|
typename enable_if<
|
|
msvc_parse_bug_workaround::quats<A,B>,
|
|
A &>::type
|
|
operator-=( A & a, B const & b )
|
|
{
|
|
write_quat_element<0>(a,quat_traits<A>::template read_element<0>(a)-quat_traits<B>::template read_element<0>(b));
|
|
write_quat_element<1>(a,quat_traits<A>::template read_element<1>(a)-quat_traits<B>::template read_element<1>(b));
|
|
write_quat_element<2>(a,quat_traits<A>::template read_element<2>(a)-quat_traits<B>::template read_element<2>(b));
|
|
write_quat_element<3>(a,quat_traits<A>::template read_element<3>(a)-quat_traits<B>::template read_element<3>(b));
|
|
return a;
|
|
}
|
|
|
|
template <class A,class B>
|
|
BOOST_QVM_CONSTEXPR 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;
|
|
write_quat_element<0>(r,quat_traits<A>::template read_element<0>(a)-quat_traits<B>::template read_element<0>(b));
|
|
write_quat_element<1>(r,quat_traits<A>::template read_element<1>(a)-quat_traits<B>::template read_element<1>(b));
|
|
write_quat_element<2>(r,quat_traits<A>::template read_element<2>(a)-quat_traits<B>::template read_element<2>(b));
|
|
write_quat_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_CONSTEXPR 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;
|
|
write_quat_element<0>(r,-quat_traits<A>::template read_element<0>(a));
|
|
write_quat_element<1>(r,-quat_traits<A>::template read_element<1>(a));
|
|
write_quat_element<2>(r,-quat_traits<A>::template read_element<2>(a));
|
|
write_quat_element<3>(r,-quat_traits<A>::template read_element<3>(a));
|
|
return r;
|
|
}
|
|
|
|
template <class A,class B>
|
|
BOOST_QVM_CONSTEXPR 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);
|
|
write_quat_element<0>(a,aa*ba - ab*bb - ac*bc - ad*bd);
|
|
write_quat_element<1>(a,aa*bb + ab*ba + ac*bd - ad*bc);
|
|
write_quat_element<2>(a,aa*bc + ac*ba + ad*bb - ab*bd);
|
|
write_quat_element<3>(a,aa*bd + ad*ba + ab*bc - ac*bb);
|
|
return a;
|
|
}
|
|
|
|
template <class A,class B>
|
|
BOOST_QVM_CONSTEXPR BOOST_QVM_INLINE_OPERATIONS
|
|
typename enable_if_c<
|
|
is_quat<A>::value && is_scalar<B>::value,
|
|
A &>::type
|
|
operator*=( A & a, B b )
|
|
{
|
|
write_quat_element<0>(a, quat_traits<A>::template read_element<0>(a)*b);
|
|
write_quat_element<1>(a, quat_traits<A>::template read_element<1>(a)*b);
|
|
write_quat_element<2>(a, quat_traits<A>::template read_element<2>(a)*b);
|
|
write_quat_element<3>(a, quat_traits<A>::template read_element<3>(a)*b);
|
|
return a;
|
|
}
|
|
|
|
template <class A,class B>
|
|
BOOST_QVM_CONSTEXPR 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;
|
|
write_quat_element<0>(r,aa*ba - ab*bb - ac*bc - ad*bd);
|
|
write_quat_element<1>(r,aa*bb + ab*ba + ac*bd - ad*bc);
|
|
write_quat_element<2>(r,aa*bc + ac*ba + ad*bb - ab*bd);
|
|
write_quat_element<3>(r,aa*bd + ad*ba + ab*bc - ac*bb);
|
|
return r;
|
|
}
|
|
|
|
template <class A,class B>
|
|
BOOST_QVM_CONSTEXPR BOOST_QVM_INLINE_OPERATIONS
|
|
typename lazy_enable_if_c2<
|
|
is_quat<A>::value && is_scalar<B>::value,
|
|
deduce_quat2<A,B> >::type
|
|
operator*( A const & a, B b )
|
|
{
|
|
typedef typename deduce_quat2<A,B>::type R;
|
|
R r;
|
|
write_quat_element<0>(r,quat_traits<A>::template read_element<0>(a)*b);
|
|
write_quat_element<1>(r,quat_traits<A>::template read_element<1>(a)*b);
|
|
write_quat_element<2>(r,quat_traits<A>::template read_element<2>(a)*b);
|
|
write_quat_element<3>(r,quat_traits<A>::template read_element<3>(a)*b);
|
|
return r;
|
|
}
|
|
|
|
template <class A,class B>
|
|
BOOST_QVM_CONSTEXPR 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_CONSTEXPR 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(m2);
|
|
typedef typename deduce_quat<A>::type R;
|
|
R r;
|
|
write_quat_element<0>(r,a0*rm);
|
|
write_quat_element<1>(r,a1*rm);
|
|
write_quat_element<2>(r,a2*rm);
|
|
write_quat_element<3>(r,a3*rm);
|
|
return r;
|
|
}
|
|
|
|
template <class A>
|
|
BOOST_QVM_CONSTEXPR 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(m2);
|
|
write_quat_element<0>(a,quat_traits<A>::template read_element<0>(a)*rm);
|
|
write_quat_element<1>(a,quat_traits<A>::template read_element<1>(a)*rm);
|
|
write_quat_element<2>(a,quat_traits<A>::template read_element<2>(a)*rm);
|
|
write_quat_element<3>(a,quat_traits<A>::template read_element<3>(a)*rm);
|
|
}
|
|
|
|
template <class A,class B>
|
|
BOOST_QVM_CONSTEXPR BOOST_QVM_INLINE_OPERATIONS
|
|
typename enable_if<
|
|
msvc_parse_bug_workaround::quats<A,B>,
|
|
A &>::type
|
|
operator+=( A & a, B const & b )
|
|
{
|
|
write_quat_element<0>(a,quat_traits<A>::template read_element<0>(a)+quat_traits<B>::template read_element<0>(b));
|
|
write_quat_element<1>(a,quat_traits<A>::template read_element<1>(a)+quat_traits<B>::template read_element<1>(b));
|
|
write_quat_element<2>(a,quat_traits<A>::template read_element<2>(a)+quat_traits<B>::template read_element<2>(b));
|
|
write_quat_element<3>(a,quat_traits<A>::template read_element<3>(a)+quat_traits<B>::template read_element<3>(b));
|
|
return a;
|
|
}
|
|
|
|
template <class A,class B>
|
|
BOOST_QVM_CONSTEXPR 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;
|
|
write_quat_element<0>(r,quat_traits<A>::template read_element<0>(a)+quat_traits<B>::template read_element<0>(b));
|
|
write_quat_element<1>(r,quat_traits<A>::template read_element<1>(a)+quat_traits<B>::template read_element<1>(b));
|
|
write_quat_element<2>(r,quat_traits<A>::template read_element<2>(a)+quat_traits<B>::template read_element<2>(b));
|
|
write_quat_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_CONSTEXPR 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 const threshold = one - one / scalar_traits<TR>::value(2000); //0.9995
|
|
TR dp = dot(a,b);
|
|
if( dp > threshold )
|
|
return normalized(a+(b-a)*t);
|
|
if( dp < -one )
|
|
dp = -one;
|
|
double theta_0 = acos(dp);
|
|
double theta = theta_0*t;
|
|
return a*cos(theta) + normalized(b-a*dp)*sin(theta);
|
|
}
|
|
|
|
////////////////////////////////////////////////
|
|
|
|
namespace
|
|
qvm_detail
|
|
{
|
|
template <class T>
|
|
class
|
|
qref_
|
|
{
|
|
qref_( qref_ const & );
|
|
qref_ & operator=( qref_ const & );
|
|
~qref_();
|
|
|
|
public:
|
|
|
|
template <class R>
|
|
BOOST_QVM_CONSTEXPR BOOST_QVM_INLINE_TRIVIAL
|
|
qref_ &
|
|
operator=( R const & x )
|
|
{
|
|
assign(*this,x);
|
|
return *this;
|
|
}
|
|
|
|
template <class R
|
|
#if __cplusplus >= 201103L
|
|
, class = typename enable_if<is_quat<R> >::type
|
|
#endif
|
|
>
|
|
BOOST_QVM_CONSTEXPR BOOST_QVM_INLINE_TRIVIAL
|
|
operator R() const
|
|
{
|
|
R r;
|
|
assign(r,*this);
|
|
return r;
|
|
}
|
|
};
|
|
|
|
template <class Q,bool WriteElementRef=quat_write_element_ref<Q>::value>
|
|
struct qref_write_traits;
|
|
|
|
template <class Q>
|
|
struct
|
|
qref_write_traits<Q,true>
|
|
{
|
|
typedef typename quat_traits<Q>::scalar_type scalar_type;
|
|
typedef qvm_detail::qref_<Q> this_quaternion;
|
|
|
|
template <int I>
|
|
static
|
|
BOOST_QVM_CONSTEXPR 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
|
|
qref_write_traits<Q,false>
|
|
{
|
|
typedef typename quat_traits<Q>::scalar_type scalar_type;
|
|
typedef qvm_detail::qref_<Q> this_quaternion;
|
|
|
|
template <int I>
|
|
static
|
|
BOOST_QVM_CONSTEXPR BOOST_QVM_INLINE_CRITICAL
|
|
void
|
|
write_element( this_quaternion & x, scalar_type s )
|
|
{
|
|
BOOST_QVM_STATIC_ASSERT(I>=0);
|
|
BOOST_QVM_STATIC_ASSERT(I<4);
|
|
quat_traits<Q>::template write_element<I>(reinterpret_cast<Q &>(x), s);
|
|
}
|
|
};
|
|
}
|
|
|
|
template <class Q>
|
|
struct quat_traits;
|
|
|
|
template <class Q>
|
|
struct
|
|
quat_traits< qvm_detail::qref_<Q> >:
|
|
qvm_detail::qref_write_traits<Q>
|
|
{
|
|
typedef typename quat_traits<Q>::scalar_type scalar_type;
|
|
typedef qvm_detail::qref_<Q> this_quaternion;
|
|
|
|
template <int I>
|
|
static
|
|
BOOST_QVM_CONSTEXPR 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 <class Q>
|
|
struct
|
|
deduce_quat< qvm_detail::qref_<Q> >
|
|
{
|
|
typedef quat<typename quat_traits<Q>::scalar_type> type;
|
|
};
|
|
|
|
template <class Q>
|
|
BOOST_QVM_CONSTEXPR 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_CONSTEXPR 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
|
|
#if __cplusplus >= 201103L
|
|
, class = typename enable_if<is_quat<R> >::type
|
|
#endif
|
|
>
|
|
BOOST_QVM_CONSTEXPR 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_CONSTEXPR 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_CONSTEXPR 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_CONSTEXPR 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_CONSTEXPR 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);
|
|
write_quat_element<0>(a,zero);
|
|
write_quat_element<1>(a,zero);
|
|
write_quat_element<2>(a,zero);
|
|
write_quat_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_CONSTEXPR 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(m2);
|
|
angle/=2;
|
|
scalar_type const s=sin(angle);
|
|
a[0] = cos(angle);
|
|
a[1] = rm*x*s;
|
|
a[2] = rm*y*s;
|
|
a[3] = rm*z*s;
|
|
}
|
|
|
|
template <class R
|
|
#if __cplusplus >= 201103L
|
|
, class = typename enable_if<is_quat<R> >::type
|
|
#endif
|
|
>
|
|
BOOST_QVM_CONSTEXPR 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_CONSTEXPR 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_CONSTEXPR 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_CONSTEXPR 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_CONSTEXPR 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_CONSTEXPR BOOST_QVM_INLINE_TRIVIAL
|
|
rotx_quat_()
|
|
{
|
|
}
|
|
|
|
template <class R
|
|
#if __cplusplus >= 201103L
|
|
, class = typename enable_if<is_quat<R> >::type
|
|
#endif
|
|
>
|
|
BOOST_QVM_CONSTEXPR 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_CONSTEXPR 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_CONSTEXPR BOOST_QVM_INLINE_CRITICAL
|
|
T
|
|
get( T const & angle )
|
|
{
|
|
return sin(angle/2);
|
|
}
|
|
};
|
|
|
|
template <>
|
|
struct
|
|
rotx_q_get<0>
|
|
{
|
|
template <class T>
|
|
static
|
|
BOOST_QVM_CONSTEXPR BOOST_QVM_INLINE_CRITICAL
|
|
T
|
|
get( T const & angle )
|
|
{
|
|
return cos(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_CONSTEXPR 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_CONSTEXPR 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_CONSTEXPR 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_CONSTEXPR 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_CONSTEXPR BOOST_QVM_INLINE_TRIVIAL
|
|
roty_quat_()
|
|
{
|
|
}
|
|
|
|
template <class R
|
|
#if __cplusplus >= 201103L
|
|
, class = typename enable_if<is_quat<R> >::type
|
|
#endif
|
|
>
|
|
BOOST_QVM_CONSTEXPR 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_CONSTEXPR 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_CONSTEXPR BOOST_QVM_INLINE_CRITICAL
|
|
T
|
|
get( T const & angle )
|
|
{
|
|
return sin(angle/2);
|
|
}
|
|
};
|
|
|
|
template <>
|
|
struct
|
|
roty_q_get<0>
|
|
{
|
|
template <class T>
|
|
static
|
|
BOOST_QVM_CONSTEXPR BOOST_QVM_INLINE_CRITICAL
|
|
T
|
|
get( T const & angle )
|
|
{
|
|
return cos(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_CONSTEXPR 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_CONSTEXPR 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_CONSTEXPR 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_CONSTEXPR 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_CONSTEXPR BOOST_QVM_INLINE_TRIVIAL
|
|
rotz_quat_()
|
|
{
|
|
}
|
|
|
|
template <class R
|
|
#if __cplusplus >= 201103L
|
|
, class = typename enable_if<is_quat<R> >::type
|
|
#endif
|
|
>
|
|
BOOST_QVM_CONSTEXPR 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_CONSTEXPR 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_CONSTEXPR BOOST_QVM_INLINE_CRITICAL
|
|
T
|
|
get( T const & angle )
|
|
{
|
|
return sin(angle/2);
|
|
}
|
|
};
|
|
|
|
template <>
|
|
struct
|
|
rotz_q_get<0>
|
|
{
|
|
template <class T>
|
|
static
|
|
BOOST_QVM_CONSTEXPR BOOST_QVM_INLINE_CRITICAL
|
|
T
|
|
get( T const & angle )
|
|
{
|
|
return cos(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_CONSTEXPR 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_CONSTEXPR 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_CONSTEXPR 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_CONSTEXPR 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_CONSTEXPR 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(m2);
|
|
a0/=s;
|
|
a1/=s;
|
|
a2/=s;
|
|
a3/=s;
|
|
}
|
|
if( T s=sqrt(1-a0*a0) )
|
|
{
|
|
write_vec_element<0>(b, a1/s);
|
|
write_vec_element<1>(b, a2/s);
|
|
write_vec_element<2>(b, a3/s);
|
|
}
|
|
else
|
|
{
|
|
typedef typename vec_traits<B>::scalar_type U;
|
|
write_vec_element<0>(b, scalar_traits<U>::value(1));
|
|
write_vec_element<1>(b, scalar_traits<U>::value(0));
|
|
write_vec_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
|