Source

neglect / neglect / cpplib / include / neglect / quaternion.hpp

Full commit
/* platform independence utilities */
#ifndef _INC_NEGLECT_QUATERNION_HPP_
#define _INC_NEGLECT_QUATERNION_HPP_

#include <neglect/boot.hpp>
#include <neglect/math.hpp>
#include <neglect/matrix.hpp>
#include <neglect/vector.hpp>


namespace neglect {

    /* implements quaternions.  we represent quaternions as a scalar w
       plus a three dimensional vector x, y, z. */
    template <typename T>
    class quaternion {
    public:
        T w;
        vector<T, 3> vec;

        quaternion()
        {
        }

        quaternion(T w, T x, T y, T z) : w(w), vec(x, y, z)
        {
        }

        quaternion(T w, const vector<T, 3> &vec) : w(w), vec(vec)
        {
        }

        static quaternion<T> zero()
        {
            quaternion<T> rv;
            rv.set_zero();
            return rv;
        }

        // pointer access

        T *ptr()
        {
            return &w;
        }

        const T *ptr() const
        {
            return &w;
        }

        // element access

        T &operator[](size_t idx)
        {
            return *(&w + i);
        }

        T operator[](size_t idx) const
        {
            return *(&w + i);
        }

        void set_zero()
        {
            w = vec.x = vec.y = vec.z = T();
        }

        quaternion<T> operator-() const
        {
            return quaternion<T>(-w, -vec);
        }
    };


    /* multiply two quaternions */
    template <typename T>
    quaternion<T> operator*(const quaternion<T> &lhs,
                            const quaternion<T> &rhs)
    {
        return quaternion<T>(lhs.w * rhs.w - dot(lhs.vec, rhs.vec),
                             (lhs.w * rhs.vec) + (rhs.w * lhs.vec)
                             + cross(lhs.vec, rhs.vec));
    }

    template <typename T>
    quaternion<T> &operator*=(quaternion<T> &lhs,
                              const quaternion<T> &rhs)
    {
        lhs = lhs * rhs;
        return lhs;
    }

    /* compare two quaternions */
    template <typename T1, typename T2>
    bool operator==(const quaternion<T1> &q1,
                    const quaternion<T2> &q2)
    {
        return q1.w == q2.w && q1.vec == q2.vec;
    }

    template <typename T1, typename T2>
    bool operator!=(const quaternion<T1> &q1,
                    const quaternion<T2> &q2)
    {
        return !(q1 == q2);
    }

    template <typename T1, typename T2>
    bool almost_equal(const quaternion<T1> &q1,
                      const quaternion<T2> &q2)
    {
        return almost_equal(q1.w, q2.w) &&
               almost_equal(q1.vec, q2.vec);
    }

    /* calculate the inverse of a quaternion */
    template <typename T>
    quaternion<T> inverse(const quaternion<T> &q)
    {
        return quaternion<T>(q.w, -q.vec);
    }

    /* calculate the dot product */
    template <typename T>
    T dot(const quaternion<T> &lhs,
          const quaternion<T> &rhs)
    {
        return lhs.w * rhs.w + dot(lhs.vec, rhs.vec);
    }

    typedef quaternion<float> quat;
    typedef quaternion<int> quati;
}

#endif