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 {
    // 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.
    //
    // issues 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; }

        static quaternion angle_axis (T radians, vector<3,T> axis);
        static quaternion from_euler (vector<3,T>);

        /// build a quaternion that represents the rotation from a to b
        static quaternion from_to (vector<3,T> a, vector<3,T> b);

        /// build a quaternion that represents the rotation needed to look at
        /// a direction with a given up direction
        static quaternion look (vector<3,T> fwd, vector<3,T> up);

        matrix4<T> as_matrix (void) const;

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


    ///////////////////////////////////////////////////////////////////////////
    template <typename T>
    vector3<T>
    rotate (vector3<T>, quaternion<T>);


    //-------------------------------------------------------------------------
    template <typename T>
    vector3<T>
    operator* (quaternion<T> const q, vector3<T> const v)
    {
        return rotate (v, 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;
    }


    //-------------------------------------------------------------------------
    template <typename T>
    constexpr
    T
    norm (quaternion<T> q)
    {
        return std::sqrt (norm2 (q));
    }


    //-------------------------------------------------------------------------
    template <typename T>
    constexpr
    bool
    is_normalised (quaternion<T> q)
    {
        return almost_equal (T{1}, norm2 (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