libcruft-util/quaternion.hpp

/*
 * This Source Code Form is subject to the terms of the Mozilla Public
 * License, v. 2.0. If a copy of the MPL was not distributed with this
 * file, You can obtain one at http://mozilla.org/MPL/2.0/.
 *
 * Copyright 2011-2018 Danny Robson <danny@nerdcruft.net>
 */

#ifndef __UTIL_QUATERNION_HPP
#define __UTIL_QUATERNION_HPP

#include "coord/traits.hpp"

#include "maths.hpp"
#include "vector.hpp"
#include "matrix.hpp"

#include <iosfwd>


namespace cruft {
    /// Represents a quaternion value.
    ///
    /// Quaternion's are _just_ different enough to other coord types that we
    /// special case as a distinct POD type and provide many of the same
    /// functions as distinct declarations.
    ///
    /// Considerations include:
    /// * strictly 4 dimensions
    /// * scalar operations sometimes don't make sense on the w component
    /// * objects must be normalised to make sense
    template <typename T>
    struct quaternion {
        T w, x, y, z;

        static constexpr std::size_t size (void) { return 4; }

        /// Construct a quaternion that represents a rotation of `radians`
        /// around the unit-vector `axis`.
        static quaternion angle_axis (T radians, vector<3,T> axis);

        /// Construct a quaternion that represents a sequence of euler angle
        /// rotations in radians. Equivalent to the concatentation of x, y,
        /// and then z rotations.
        static quaternion from_euler (vector<3,T>);

        /// Constructs a quaternion that represents the rotation of a unit
        /// vector `a` to the unit vector `b`.
        static quaternion from_to (vector<3,T> a, vector<3,T> b);

        /// Constructs a quaternion that represents the rotation needed to
        /// look along the unit vector `fwd` with the unit vector `up`
        /// denoting the upward direction.
        static quaternion look (vector<3,T> fwd, vector<3,T> up);

        /// Converts a rotation quaternion into an equivalent matrix form.
        matrix4<T> as_matrix (void) const;

        /// Constructs a identity rotation quaternion.
        static constexpr
        quaternion<T>
        identity (void)
        {
            return { 1, 0, 0, 0 };
        }
    };


    //-------------------------------------------------------------------------
    template <typename T>
    struct arity<quaternion<T>,void>
    :std::integral_constant<std::size_t, 4>
    { };


    ///////////////////////////////////////////////////////////////////////////
    /// Returns the result of rotating the unit vector `dir` using the
    /// quaternion `q`.
    template <typename T>
    vector3<T>
    rotate (vector3<T> dir, quaternion<T> q);


    ///------------------------------------------------------------------------
    /// Returns the result of rotating the unit vector `v` by the quaternion `q`.
    template <typename T>
    vector3<T>
    operator* (quaternion<T> const q, vector3<T> const v)
    {
        return rotate (v, q);
    }


    ///////////////////////////////////////////////////////////////////////////
    /// Returns the square of the L2 norm of the quaternion `q`.
    template <typename T>
    constexpr
    T
    norm2 (quaternion<T> q)
    {
        return q.w * q.w +
               q.x * q.x +
               q.y * q.y +
               q.z * q.z;
    }


    ///------------------------------------------------------------------------
    /// Returns the L2 norm of the quaternion `q`.
    template <typename T>
    constexpr
    T
    norm (quaternion<T> q)
    {
        return std::sqrt (norm2 (q));
    }


    ///------------------------------------------------------------------------
    /// Returns true if the quaternion has a unit L2 norm.
    template <typename T>
    constexpr
    bool
    is_normalised (quaternion<T> q)
    {
        return almost_equal (T{1}, norm2 (q));
    }


    ///------------------------------------------------------------------------
    /// Returns the normalised form of quaternion `q`.
    template <typename T>
    constexpr
    quaternion<T>
    normalised (quaternion<T> q)
    {
        return q / norm (q);
    }


    ///////////////////////////////////////////////////////////////////////////
    template <typename T>
    quaternion<T>
    conjugate (quaternion<T>);


    ///////////////////////////////////////////////////////////////////////////
    template <typename T>
    quaternion<T>
    operator* (quaternion<T>, quaternion<T>);


    //-------------------------------------------------------------------------
    template <typename T>
    quaternion<T>&
    operator*= (quaternion<T>&, quaternion<T>);


    //-------------------------------------------------------------------------
    template <typename T>
    quaternion<T>
    operator/ (quaternion<T>, quaternion<T>);


    //-------------------------------------------------------------------------
    template <typename T>
    constexpr
    quaternion<T>
    operator/ (quaternion<T> q, T t)
    {
        return { q.w / t, q.x / t, q.y / t, q.z / t };
    }


    ///////////////////////////////////////////////////////////////////////////
    template <typename T>
    matrix<4,4,T>
    operator* (quaternion<T> q, matrix<4,4,T> m)
    {
        return q.as_matrix () * m;
    }


    //-------------------------------------------------------------------------
    template <typename T>
    matrix<4,4,T>
    operator* (matrix<4,4,T> m, quaternion<T> q)
    {
        return m * q.as_matrix ();
    }


    ///////////////////////////////////////////////////////////////////////////
    template <typename T>
    constexpr
    bool operator== (quaternion<T> a, quaternion<T> b)
    {
        return exactly_equal (a.w, b.w) &&
               exactly_equal (a.x, b.x) &&
               exactly_equal (a.y, b.y) &&
               exactly_equal (a.z, b.z);
    }


    //-------------------------------------------------------------------------
    template <typename T>
    bool almost_equal (quaternion<T>, quaternion<T>);


    ///////////////////////////////////////////////////////////////////////////
    typedef quaternion<float>  quaternionf;
    typedef quaternion<double> quaterniond;


    ///////////////////////////////////////////////////////////////////////////
    template <typename T>
    std::ostream&
    operator<< (std::ostream&, quaternion<T>);
}

#endif