X-Git-Url: https://git.dogcows.com/gitweb?p=chaz%2Fyoink;a=blobdiff_plain;f=src%2FMoof%2Fcml%2Fquaternion%2Fquaternion.h;fp=src%2FMoof%2Fcml%2Fquaternion%2Fquaternion.h;h=771f34ad0424098a84e895dcb4aa161f5c7a25b2;hp=0000000000000000000000000000000000000000;hb=c2321281bf12a7efaedde930422c7ddbc92080d4;hpb=87bc17e55b0c1dc73ecc66df856d3f08fd7a7724 diff --git a/src/Moof/cml/quaternion/quaternion.h b/src/Moof/cml/quaternion/quaternion.h new file mode 100644 index 0000000..771f34a --- /dev/null +++ b/src/Moof/cml/quaternion/quaternion.h @@ -0,0 +1,519 @@ +/* -*- C++ -*- ------------------------------------------------------------ + +Copyright (c) 2007 Jesse Anders and Demian Nave http://cmldev.net/ + +The Configurable Math Library (CML) is distributed under the terms of the +Boost Software License, v1.0 (see cml/LICENSE for details). + + *-----------------------------------------------------------------------*/ +/** @file + * @brief + * + * @todo Return a VectorXpr adaptor from the imaginary() method of + * quaternion and the expression node types. + * + * @todo swap multiplication order based upon template param + * + * @todo change element order based upon template param + */ + +#ifndef quaternion_h +#define quaternion_h + +#include +#include +#include +#include + +/* This is used below to create a more meaningful compile-time error when + * the quaternion class is not created with a fixed-size 4-vector: + */ +struct quaternion_requires_fixed_size_array_type_error; + +namespace cml { + +/** A configurable quaternion type. + * + * @note Quaternions with two different orders cannot be used in the same + * expression. + */ +template< + typename Element, + class ArrayType, + class Order, + class Cross +> +class quaternion +{ + /* The ArrayType must be fixed<> or external<>: */ + CML_STATIC_REQUIRE_M( + (same_type< ArrayType, fixed<> >::is_true + || same_type< ArrayType, external<> >::is_true), + quaternion_requires_fixed_size_array_type_error); + + public: + + /* Shorthand for the array type generator: */ + typedef ArrayType storage_type; + typedef typename ArrayType::template rebind<4>::other generator_type; + + /* Vector representing the quaternion. Use the rebinding template to + * set the vector size: + */ + typedef vector vector_type; + + /* Vector temporary type: */ + typedef typename vector_type::temporary_type vector_temporary; + + /* Quaternion order: */ + typedef Order order_type; + + /* Quaternion multiplication order: */ + typedef Cross cross_type; + + /* Scalar type representing the scalar part: */ + typedef typename vector_type::value_type value_type; + typedef typename vector_type::reference reference; + typedef typename vector_type::const_reference const_reference; + /* XXX Need to verify that this is a true scalar type. */ + + /* The quaternion type: */ + typedef quaternion + quaternion_type; + + /* For integration into the expression template code: */ + typedef quaternion_type expr_type; + + /* For integration into the expression template code: */ + typedef quaternion< + Element, typename vector_temporary::storage_type, + order_type, cross_type> temporary_type; + + /* For integration into the expression templates code: */ + typedef quaternion_type& expr_reference; + typedef const quaternion_type& expr_const_reference; + + /* For matching by storage type: */ + typedef typename vector_type::memory_tag memory_tag; + + /* For matching by size type: */ + typedef typename vector_type::size_tag size_tag; + + /* Get the imaginary part type: */ + typedef typename vector_temporary::subvector_type imaginary_type; + + /* For matching by result-type: */ + typedef cml::et::quaternion_result_tag result_tag; + + /* For matching by assignability: */ + typedef cml::et::assignable_tag assignable_tag; + + + public: + + /** Record result size as an enum. */ + enum { array_size = 4 }; + + /** Localize the ordering as an enum. */ + enum { + W = order_type::W, + X = order_type::X, + Y = order_type::Y, + Z = order_type::Z + }; + + + public: + + /** Return the scalar part. */ + value_type real() const { return m_q[W]; } + + /** Return the imaginary vector. */ + imaginary_type imaginary() const { + /* + imaginary_type v; + v[0] = m_q[X]; v[1] = m_q[Y]; v[2] = m_q[Z]; + return v; + */ + return imaginary_type(m_q[X], m_q[Y], m_q[Z]); + } + + /** Return the vector representing the quaternion. */ + const vector_type& as_vector() const { + return m_q; + } + + /** Return the Cayley norm of the quaternion. */ + value_type norm() const { + return length_squared(); + } + + /** Return square of the quaternion length. */ + value_type length_squared() const { + return cml::dot(*this,*this); + } + + /** Return the quaternion length. */ + value_type length() const { + return std::sqrt(length_squared()); + } + + /** Normalize this quaternion (divide by its length). + * + * @todo Make this return a QuaternionXpr. + */ + quaternion_type& normalize() { + return (*this /= length()); + } + + /** Set this quaternion to the conjugate. */ + quaternion_type& conjugate() { + return (*this) = cml::conjugate(*this); + } + + /** Set this quaternion to the inverse. */ + quaternion_type& inverse() { + return (*this) = cml::inverse(*this); + } + + /** Set this quaternion to the multiplicative identity. */ + quaternion_type& identity() { + m_q[W] = value_type(1); + m_q[X] = value_type(0); + m_q[Y] = value_type(0); + m_q[Z] = value_type(0); + return *this; + } + + /** Return the log of this quaternion. */ + temporary_type log( + value_type tolerance = epsilon::placeholder()) const + { + value_type a = acos_safe(real()); + value_type s = std::sin(a); + + if (s > tolerance) { + return temporary_type(value_type(0), imaginary() * (a / s)); + } else { + return temporary_type(value_type(0), imaginary()); + } + } + + /** + * Return the result of the exponential function as applied to + * this quaternion. + */ + temporary_type exp( + value_type tolerance = epsilon::placeholder()) const + { + imaginary_type v = imaginary(); + value_type a = cml::length(v); + + if (a > tolerance) { + return temporary_type(std::cos(a), v * (std::sin(a) / a)); + } else { + return temporary_type(std::cos(a), v); + } + } + + + /** Const access to the quaternion as a vector. */ + const_reference operator[](size_t i) const { return m_q[i]; } + + /** Mutable access to the quaternion as a vector. */ + reference operator[](size_t i) { return m_q[i]; } + + /** Fill quaternion with random elements. + * + * @warning This does not generate uniformly random rotations. + */ + void random(value_type min, value_type max) { + for (size_t i = 0; i < 4; ++i) { + m_q[i] = random_real(min,max); + } + } + + public: + + /** Default initializer. + * + * @note The default constructor cannot be used with an external<> + * array type. + */ + quaternion() {} + + /** Initializer for an external<> vector type. */ + quaternion(Element* const array) : m_q(array) {} + + /** Copy construct from the same type of quaternion. */ + quaternion(const quaternion_type& q) : m_q(q.m_q) {} + + /** Construct from a quaternion having a different array type. */ + template quaternion( + const quaternion& q) + : m_q(q.as_vector()) {} + + /** Copy construct from a QuaternionXpr. */ + template quaternion(QUATXPR_ARG_TYPE e) { + typedef typename XprT::order_type arg_order; + m_q[W] = e[arg_order::W]; + m_q[X] = e[arg_order::X]; + m_q[Y] = e[arg_order::Y]; + m_q[Z] = e[arg_order::Z]; + } + + + + /** Initialize from a 4-vector. + * + * If Order is scalar_first, then v[0] is the real part. Otherwise, + * v[3] is the real part. + */ + quaternion(const vector_type& v) : m_q(v) {} + + /** Initialize from an array of scalars. + * + * If Order is scalar_first, then v[0] is the real part. Otherwise, + * v[3] is the real part. + * + * @note The target vector must have CML_VEC_COPY_FROM_ARRAY + * implemented, so this cannot be used with external<> vectors. + */ + quaternion(const value_type v[4]) : m_q(v) {} + + /** Initialize from 4 scalars. + * + * If Order is scalar_first, then a is the real part, and (b,c,d) is + * the imaginary part. Otherwise, (a,b,c) is the imaginary part, and d + * is the real part. + */ + quaternion( + const value_type& a, const value_type& b, + const value_type& c, const value_type& d) + { + /* Call the overloaded assignment function: */ + assign(a, b, c, d, Order()); + } + + /** Initialize both the real and imaginary parts. + * + * The imaginary part is given by a 3-vector. Although the imaginary + * part is specified first, the proper coefficient order (vector or + * scalar first) is maintained. + */ + quaternion(const value_type& s, const imaginary_type& v) { + m_q[W] = s; m_q[X] = v[0]; m_q[Y] = v[1]; m_q[Z] = v[2]; + } + + /** Initialize both the real and imaginary parts. + * + * The imaginary part is given by a 3-vector. Although the imaginary + * part is specified second, the proper coefficient order (vector or + * scalar first) is maintained. + */ + quaternion(const imaginary_type& v, const value_type& s) { + m_q[W] = s; m_q[X] = v[0]; m_q[Y] = v[1]; m_q[Z] = v[2]; + } + + /** Initialize both the real and imaginary parts. + * + * The imaginary part is given by an array of scalars. Although the + * imaginary part is specified first, the proper coefficient order + * (vector or scalar first) is maintained. + */ + quaternion(const value_type v[3], const value_type& s) { + m_q[W] = s; m_q[X] = v[0]; m_q[Y] = v[1]; m_q[Z] = v[2]; + } + + /** Initialize both the real and imaginary parts. + * + * The imaginary part is given by an array of scalars. Although the + * imaginary part is specified second, the proper coefficient order + * (vector or scalar first) is maintained. + */ + quaternion(const value_type& s, const value_type v[3]) { + m_q[W] = s; m_q[X] = v[0]; m_q[Y] = v[1]; m_q[Z] = v[2]; + } + + + + /** Initialize from a VectorXpr. */ + template + quaternion(VECXPR_ARG_TYPE e) : m_q(e) {} + + /** Initialize both the real and imaginary parts. + * + * The imaginary part is initialized with a VectorXpr. + */ + template + quaternion(const value_type& s, VECXPR_ARG_TYPE e) { + m_q[W] = s; m_q[X] = e[0]; m_q[Y] = e[1]; m_q[Z] = e[2]; + } + + // @todo: Are we missing: + + // quaternion(VECXPR_ARG_TYPE e, const value_type& s) {} + + // Or is that covered elsewhere? + + /** In-place op from a quaternion. + * + * This assumes that _op_ is defined for both the quaternion's vector + * type and its scalar type. + */ +#define CML_QUAT_ASSIGN_FROM_QUAT(_op_) \ + template const quaternion_type& \ + operator _op_ (const quaternion& q) { \ + m_q[W] _op_ q[W]; \ + m_q[X] _op_ q[X]; \ + m_q[Y] _op_ q[Y]; \ + m_q[Z] _op_ q[Z]; \ + return *this; \ + } + + /** In-place op from a QuaternionXpr. + * + * This assumes that _op_ is defined for the quaternion's scalar type. + */ +#define CML_QUAT_ASSIGN_FROM_QUATXPR(_op_) \ + template quaternion_type& \ + operator _op_ (QUATXPR_ARG_TYPE e) { \ + typedef typename XprT::order_type arg_order; \ + m_q[W] _op_ e[arg_order::W]; \ + m_q[X] _op_ e[arg_order::X]; \ + m_q[Y] _op_ e[arg_order::Y]; \ + m_q[Z] _op_ e[arg_order::Z]; \ + return *this; \ + } + + /** In-place op from a scalar type. + * + * This assumes that _op_ is defined for the quaternion's scalar type. + */ +#define CML_QUAT_ASSIGN_FROM_SCALAR(_op_,_op_name_) \ + quaternion_type& operator _op_ (const value_type& s) { \ + typedef _op_name_ OpT; \ + OpT().apply(m_q[W],s); \ + OpT().apply(m_q[X],s); \ + OpT().apply(m_q[Y],s); \ + OpT().apply(m_q[Z],s); \ + return *this; \ + } + + CML_QUAT_ASSIGN_FROM_QUAT(=) + CML_QUAT_ASSIGN_FROM_QUAT(+=) + CML_QUAT_ASSIGN_FROM_QUAT(-=) + + CML_QUAT_ASSIGN_FROM_QUATXPR(=) + CML_QUAT_ASSIGN_FROM_QUATXPR(+=) + CML_QUAT_ASSIGN_FROM_QUATXPR(-=) + + CML_QUAT_ASSIGN_FROM_SCALAR(*=, cml::et::OpMulAssign) + CML_QUAT_ASSIGN_FROM_SCALAR(/=, cml::et::OpDivAssign) + +#undef CML_QUAT_ASSIGN_FROM_QUAT +#undef CML_QUAT_ASSIGN_FROM_QUATXPR +#undef CML_QUAT_ASSIGN_FROM_SCALAR + + /** Accumulated multiplication with a quaternion. + * + * Compute p = p * q for two quaternions p and q. + * + * @internal Using operator* here is okay, as long as cml/quaternion.h + * is included before using this method (the only supported case for + * end-user code). This is because modern compilers won't instantiate a + * method in a template class until it is used, and including the main + * header ensures all definitions are available before any possible use + * of this method. + */ + quaternion_type& operator*=(const quaternion_type& q) { + return (*this = *this * q); + } + + /** Accumulated multiplication with a quaternion expression. + * + * Compute p = p * e for a quaternion p and a quaternion expression e. + * + * @internal Using operator* here is okay, as long as cml/quaternion.h + * is included before using this method (the only supported case for + * end-user code). This is because modern compilers won't instantiate a + * method in a template class until it is used, and including the main + * header ensures all definitions are available before any possible use + * of this method. + */ + template quaternion_type& operator*=(QUATXPR_ARG_TYPE e) { + return (*this = *this * e); + } + + /* NOTE: Quaternion division no longer supported, but I'm leaving the + code here for reference (Jesse) */ + + #if 0 + /** Accumulated division with a quaternion. + * + * Compute p = p * inverse(q). + * + * @note Because quaternion multiplication is non-commutative, division + * is ambiguous. This method assumes a multiplication order consistent + * with the notational order; i.e. p = q / r means p = q*inverse(r). + * + * @internal Using operator* and cml::inverse here is okay, as long as + * cml/quaternion.h is included before using this method (the only + * supported case for end-user code). This is because modern compilers + * won't instantiate a method in a template class until it is used, and + * including the main header ensures all definitions are available + * before any possible use of this method. + */ + quaternion_type& operator/=(const quaternion_type& q) { + return (*this = *this * cml::inverse(q)); + } + + /** Accumulated division with a quaternion expression. + * + * Compute p = p * inverse(q). + * + * @note Because quaternion multiplication is non-commutative, division + * is ambiguous. This method assumes a multiplication order consistent + * with the notational order; i.e. p = q / r means p = q*inverse(r). + * + * @internal Using operator* and cml::inverse here is okay, as long as + * cml/quaternion.h is included before using this method (the only + * supported case for end-user code). This is because modern compilers + * won't instantiate a method in a template class until it is used, and + * including the main header ensures all definitions are available + * before any possible use of this method. + */ + template quaternion_type& operator/=(QUATXPR_ARG_TYPE e) { + return (*this = *this * cml::inverse(e)); + } + #endif + + + protected: + + /** Overloaded function to assign the quaternion from 4 scalars. */ + void assign(const value_type& a, const value_type& b, + const value_type& c, const value_type& d, scalar_first) + { + m_q[W] = a; m_q[X] = b; m_q[Y] = c; m_q[Z] = d; + } + + /** Overloaded function to assign the quaternion from 4 scalars. */ + void assign(const value_type& a, const value_type& b, + const value_type& c, const value_type& d, vector_first) + { + m_q[X] = a; m_q[Y] = b; m_q[Z] = c; m_q[W] = d; + } + + + protected: + + vector_type m_q; +}; + +} // namespace cml + +#endif + +// ------------------------------------------------------------------------- +// vim:ft=cpp