quaternion.h

Go to the documentation of this file.

/* -*- 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).

 *-----------------------------------------------------------------------*/
#ifndef quaternion_h
#define quaternion_h

#include <cml/mathlib/epsilon.h>
#include <cml/quaternion/quaternion_expr.h>
#include <cml/quaternion/quaternion_dot.h>
#include <cml/util.h>

/* 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 {

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<Element, generator_type> 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<Element,storage_type,order_type,cross_type>
        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:

    enum { array_size = 4 };

    enum {
        W = order_type::W,
        X = order_type::X,
        Y = order_type::Y,
        Z = order_type::Z
    };


  public:

    value_type real() const { return m_q[W]; }

    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]);
    }

    const vector_type& as_vector() const {
        return m_q;
    }

    value_type norm() const {
        return length_squared();
    }

    value_type length_squared() const {
        return cml::dot(*this,*this);
    }

    value_type length() const {
        return std::sqrt(length_squared());
    }

    quaternion_type& normalize() {
        return (*this /= length());
    }

    quaternion_type& conjugate() {
        return (*this) = cml::conjugate(*this);
    }

    quaternion_type& inverse() {
        return (*this) = cml::inverse(*this);
    }

    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;
    }
    
    temporary_type log(
        value_type tolerance = epsilon<value_type>::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());
        }
    }
    
    temporary_type exp(
        value_type tolerance = epsilon<value_type>::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_reference operator[](size_t i) const { return m_q[i]; }

    reference operator[](size_t i) { return m_q[i]; }

    void random(value_type min, value_type max) {
        for (size_t i = 0; i < 4; ++i) {
            m_q[i] = random_real(min,max);
        }
    }

  public:

    quaternion() {}

    quaternion(Element* const array) : m_q(array) {}

    quaternion(const quaternion_type& q) : m_q(q.m_q) {}

    template<typename E, class AT> quaternion(
            const quaternion<E,AT,order_type,cross_type>& q)
        : m_q(q.as_vector()) {}

    template<typename XprT> 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];
    }



    quaternion(const vector_type& v) : m_q(v) {}

    quaternion(const value_type v[4]) : m_q(v) {}

    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());
    }

    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];
    }

    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];
    }

    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];
    }

    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];
    }



    template<typename XprT>
        quaternion(VECXPR_ARG_TYPE e) : m_q(e) {}

    template<typename XprT>
        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?

#define CML_QUAT_ASSIGN_FROM_QUAT(_op_)                                 \
    template<typename E, class AT> const quaternion_type&               \
    operator _op_ (const quaternion<E,AT,order_type,cross_type>& 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;                                                   \
    }

#define CML_QUAT_ASSIGN_FROM_QUATXPR(_op_)                              \
    template<typename XprT> 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;                                                   \
    }

#define CML_QUAT_ASSIGN_FROM_SCALAR(_op_,_op_name_)               \
    quaternion_type& operator _op_ (const value_type& s) {        \
        typedef _op_name_ <value_type,value_type> 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

    quaternion_type& operator*=(const quaternion_type& q) {
        return (*this = *this * q);
    }

    template<typename XprT> quaternion_type& operator*=(QUATXPR_ARG_TYPE e) {
        return (*this = *this * e);
    }

    typename vector_type::pointer data() { return m_q.data(); }

    const typename vector_type::pointer data() const { return m_q.data(); }


    /* NOTE: Quaternion division no longer supported, but I'm leaving the
       code here for reference (Jesse) */

    #if 0

    quaternion_type& operator/=(const quaternion_type& q) {
        return (*this = *this * cml::inverse(q));
    }

    template<typename XprT> quaternion_type& operator/=(QUATXPR_ARG_TYPE e) {
        return (*this = *this * cml::inverse(e));
    }
    #endif


  protected:

    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;
    }

    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