Quaternion.hpp

Go to the documentation of this file.

#pragma once
#include <NDEVR/Vector.h>
#include <NDEVR/Angle.h>
#include <NDEVR/Matrix.h>
namespace NDEVR
{
    /**--------------------------------------------------------------------------------------------------
    Class: AxisAngle
 
    \brief An angle about a particular, defined 3D axis
    **/
    template<class t_angle_type, class t_axis_type>
    struct AxisAngle
    {
        Angle<t_angle_type> angle;
        Vector<3, t_axis_type> axis;
    };
    /**--------------------------------------------------------------------------------------------------
    \brief https://www.3dgep.com/understanding-quaternions/
    **/
    template<class t_type>
    class Quaternion : public Vector<4, t_type>
    {
    public:
        constexpr Quaternion()
            : Vector<4, t_type>(t_type(1), t_type(0), t_type(0), t_type(0))
        {}
        constexpr Quaternion(const Vector<4, t_type>& vec)
            : Vector<4, t_type>(vec)
        {}
        constexpr Quaternion(t_type x, t_type y, t_type z, t_type w)
            : Vector<4, t_type>(x, y, z, w)
        {}
        constexpr explicit Quaternion(t_type x)
            : Vector<4, t_type>(x, x, x, x)
        {}
        template<class t_angle_type, class t_axis_type>
        constexpr Quaternion(Angle<t_angle_type> angle, Vector<3, t_axis_type> axis)
        {
            // Formula from http://www.euclideanspace.com/maths/geometry/rotations/conversions/angleToQuaternion/
            Vector<4, t_type>::m_values[W] = cos(angle / 2.0);
            t_axis_type c = cast<t_axis_type>(sin(angle / 2.0));
            for (uint01 i = 0; i < 3; i++)
                Vector<4, t_type>::m_values[i] = cast<t_type>(c * axis[i]);
        }
 
        template<class t_angle_type>
        AxisAngle<t_angle_type, t_type> axisAngle() const
        {
            AxisAngle<t_angle_type, t_type> axis_angle;
            axis_angle.angle = 2.0 * Angle<t_angle_type>::acos(Vector<4, t_type>::m_values[W]);
            t_type divider = sqrt(cast<t_type>(1) - Vector<4, t_type>::m_values[W] * Vector<4, t_type>::m_values[W]);
 
            if (divider != 0.0)
            {
                // Calculate the axis
                axis_angle.axis = Vector<4, t_type>::template as<3, t_type>() / divider;
            }
            else
            {
                // Arbitrary normalized axis
                axis_angle.axis[X] = cast<t_type>(1);
                axis_angle.axis[Y] = cast<t_type>(0);
                axis_angle.axis[Z] = cast<t_type>(0);
            }
            return axis_angle;
        }
 
        template<class t_angle_type>
        void setFromEuler(Vector<3, Angle<t_angle_type>> euler)
        {
            double cy = cos(euler[ROLL] * 0.5);
            double sy = sin(euler[ROLL] * 0.5);
            double cr = cos(euler[YAW] * 0.5);
            double sr = sin(euler[YAW] * 0.5);
            double cp = cos(euler[PITCH] * 0.5);
            double sp = sin(euler[PITCH] * 0.5);
 
            Vector<4, t_type>::m_values[W] = cy * cr * cp + sy * sr * sp;
            Vector<4, t_type>::m_values[X] = cy * sr * cp - sy * cr * sp;
            Vector<4, t_type>::m_values[Y] = cy * cr * sp + sy * sr * cp;
            Vector<4, t_type>::m_values[Z] = sy * cr * cp - cy * sr * sp;
        }
 
        template<class t_angle_type>
        Vector<3, Angle<t_angle_type>> euler() const
        {
            Vector<3, Angle<t_angle_type>> output;
 
            // Roll (x-axis rotation)
            t_type sinr_cosp = cast<t_type>(2) * (Vector<4, t_type>::m_values[W] * Vector<4, t_type>::m_values[X] + Vector<4, t_type>::m_values[Y] * Vector<4, t_type>::m_values[Z]);
            t_type cosr_cosp = cast<t_type>(1) - cast<t_type>(2) * (Vector<4, t_type>::m_values[X] * Vector<4, t_type>::m_values[X] + Vector<4, t_type>::m_values[Y] * Vector<4, t_type>::m_values[Y]);
            output[0] = Angle<t_angle_type>::atan2(sinr_cosp, cosr_cosp);
 
            // Pitch (y-axis rotation)
            t_type sinp = cast<t_type>(2) * (Vector<4, t_type>::m_values[W] * Vector<4, t_type>::m_values[1] - Vector<4, t_type>::m_values[2] * Vector<4, t_type>::m_values[0]);
            if (abs(sinp) >= 1)
                output[1] = sinp > 0 ? Angle<t_angle_type>(DEGREES, 90.0) : Angle<t_angle_type>(DEGREES, -90.0); // use 90 degrees if out of range
            else
                output[1] = Angle<t_angle_type>::asin(sinp);
 
            // Yaw (z-axis rotation)
            t_type siny_cosp = cast<t_type>(2) * (Vector<4, t_type>::m_values[W] * Vector<4, t_type>::m_values[2] + Vector<4, t_type>::m_values[0] * Vector<4, t_type>::m_values[1]);
            t_type cosy_cosp = cast<t_type>(1) - cast<t_type>(2) * (Vector<4, t_type>::m_values[1] * Vector<4, t_type>::m_values[1] + Vector<4, t_type>::m_values[2] * Vector<4, t_type>::m_values[2]);
            output[2] = Angle<t_angle_type>::atan2(siny_cosp, cosy_cosp);
        }
 
        Quaternion conjugate() const
        {
            Quaternion conj;
            conj[W] = Vector<4, t_type>::m_values[W];
            for (uint01 i = 0; i < 3; i++)
                conj[i] = -Vector<4, t_type>::m_values[i];
            return conj;
        }
        Quaternion invert() const
        {
            Quaternion q = conjugate();
            return q / dot((*this), (*this));
        }
        const Quaternion operator/(t_type s) const
        {
            return Quaternion((*this)[X] / s, (*this)[Y] / s, (*this)[Z] / s, (*this)[W] / s);
        }
        Quaternion operator*(const Quaternion& q2) const
        {
            Quaternion result;
 
            /*
            Formula from http://www.euclideanspace.com/maths/algebra/realNormedAlgebra/quaternions/arithmetic/index.htm
                     a*e - b*f - c*g - d*h
                + i (b*e + a*f + c*h- d*g)
                + j (a*g - b*h + c*e + d*f)
                + k (a*h + b*g - c*f + d*e)
            */
            result[W] = Vector<4, t_type>::m_values[W] * q2[W] - Vector<4, t_type>::m_values[0] * q2[0] - Vector<4, t_type>::m_values[1] * q2[1] - Vector<4, t_type>::m_values[2] * q2[2];
            result[X] = Vector<4, t_type>::m_values[0] * q2[W] + Vector<4, t_type>::m_values[W] * q2[0] + Vector<4, t_type>::m_values[1] * q2[2] - Vector<4, t_type>::m_values[2] * q2[1];
            result[Y] = Vector<4, t_type>::m_values[W] * q2[1] - Vector<4, t_type>::m_values[0] * q2[2] + Vector<4, t_type>::m_values[1] * q2[W] + Vector<4, t_type>::m_values[2] * q2[0];
            result[Z] = Vector<4, t_type>::m_values[W] * q2[2] + Vector<4, t_type>::m_values[0] * q2[1] - Vector<4, t_type>::m_values[1] * q2[0] + Vector<4, t_type>::m_values[2] * q2[W];
 
            return result;
        }
 
        template<class t_vec_type>
        Vector<3, t_vec_type> operator*(const Vector<3, t_vec_type>& v) const
        {
            Vector<3, t_vec_type> result;
 
            t_type ww = Vector<4, t_type>::m_values[W] * Vector<4, t_type>::m_values[W];
            t_type xx = Vector<4, t_type>::m_values[0] * Vector<4, t_type>::m_values[0];
            t_type yy = Vector<4, t_type>::m_values[1] * Vector<4, t_type>::m_values[1];
            t_type zz = Vector<4, t_type>::m_values[2] * Vector<4, t_type>::m_values[2];
            t_type wx = Vector<4, t_type>::m_values[W] * Vector<4, t_type>::m_values[0];
            t_type wy = Vector<4, t_type>::m_values[W] * Vector<4, t_type>::m_values[1];
            t_type wz = Vector<4, t_type>::m_values[W] * Vector<4, t_type>::m_values[2];
            t_type xy = Vector<4, t_type>::m_values[0] * Vector<4, t_type>::m_values[1];
            t_type xz = Vector<4, t_type>::m_values[0] * Vector<4, t_type>::m_values[2];
            t_type yz = Vector<4, t_type>::m_values[1] * Vector<4, t_type>::m_values[2];
 
            // Formula from http://www.euclideanspace.com/maths/algebra/realNormedAlgebra/quaternions/transforms/index.htm
            // p2.x = w*w*p1.x + 2*y*w*p1.z - 2*z*w*p1.y + x*x*p1.x + 2*y*x*p1.y + 2*z*x*p1.z - z*z*p1.x - y*y*p1.x;
            // p2.y = 2*x*y*p1.x + y*y*p1.y + 2*z*y*p1.z + 2*w*z*p1.x - z*z*p1.y + w*w*p1.y - 2*x*w*p1.z - x*x*p1.y;
            // p2.z = 2*x*z*p1.x + 2*y*z*p1.y + z*z*p1.z - 2*w*y*p1.x - y*y*p1.z + 2*w*x*p1.y - x*x*p1.z + w*w*p1.z;
 
            result[0] = ww * v[0] + 2 * wy * v[2] - 2 * wz * v[1] +
                xx * v[0] + 2 * xy * v[1] + 2 * xz * v[2] -
                zz * v[0] - yy * v[0];
            result[1] = 2 * xy * v[0] + yy * v[1] + 2 * yz * v[2] +
                2 * wz * v[0] - zz * v[1] + ww * v[1] -
                2 * wx * v[2] - xx * v[1];
            result[2] = 2 * xz * v[0] + 2 * yz * v[1] + zz * v[2] -
                2 * wy * v[0] - yy * v[2] + 2 * wx * v[1] -
                xx * v[2] + ww * v[2];
            return result;
        }
        /*
        void Quaternion_slerp(Quaternion* q1, Quaternion* q2, double t, Quaternion* output)
        {
            Quaternion result;
 
            // Based on http://www.euclideanspace.com/maths/algebra/realNormedAlgebra/quaternions/slerp/index.htm
            double cosHalfTheta = q1->w * q2->w + q1->v[0] * q2->v[0] + q1->v[1] * q2->v[1] + q1->v[2] * q2->v[2];
 
            // if q1=q2 or qa=-q2 then theta = 0 and we can return qa
            if (fabs(cosHalfTheta) >= 1.0) {
                Quaternion_copy(q1, output);
                return;
            }
 
            double halfTheta = acos(cosHalfTheta);
            double sinHalfTheta = sqrt(1.0 - cosHalfTheta * cosHalfTheta);
            // If theta = 180 degrees then result is not fully defined
            // We could rotate around any axis normal to q1 or q2
            if (fabs(sinHalfTheta) < QUATERNION_EPS) {
                result.w = (q1->w * 0.5 + q2->w * 0.5);
                result.v[0] = (q1->v[0] * 0.5 + q2->v[0] * 0.5);
                result.v[1] = (q1->v[1] * 0.5 + q2->v[1] * 0.5);
                result.v[2] = (q1->v[2] * 0.5 + q2->v[2] * 0.5);
            }
            else {
                // Default quaternion calculation
                double ratioA = sin((1 - t) * halfTheta) / sinHalfTheta;
                double ratioB = sin(t * halfTheta) / sinHalfTheta;
                result.w = (q1->w * ratioA + q2->w * ratioB);
                result.v[0] = (q1->v[0] * ratioA + q2->v[0] * ratioB);
                result.v[1] = (q1->v[1] * ratioA + q2->v[1] * ratioB);
                result.v[2] = (q1->v[2] * ratioA + q2->v[2] * ratioB);
            }
            *output = result;
        }*/
 
 
        inline Matrix<t_type> toMatrix() const
        {
            // NOTE: assume the quaternion is unit length
            // compute common values
            t_type x2 = (*this)[X] + (*this)[X];
            t_type y2 = (*this)[Y] + (*this)[Y];
            t_type z2 = (*this)[Z] + (*this)[Z];
            t_type xx2 = (*this)[X] * x2;
            t_type xy2 = (*this)[X] * y2;
            t_type xz2 = (*this)[X] * z2;
            t_type yy2 = (*this)[Y] * y2;
            t_type yz2 = (*this)[Y] * z2;
            t_type zz2 = (*this)[Z] * z2;
            t_type sx2 = (*this)[W] * x2;
            t_type sy2 = (*this)[W] * y2;
            t_type sz2 = (*this)[W] * z2;
            
            // build 4x4 matrix (column-major) and return
            return Matrix<t_type>(
                1 - (yy2 + zz2), xy2 + sz2, xz2 - sy2, 0,
                xy2 - sz2, 1 - (xx2 + zz2), yz2 + sx2, 0,
                xz2 + sy2, yz2 - sx2, 1 - (xx2 + yy2), 0,
                0, 0, 0, 1);//
        }
    };
    static Quaternion<fltp08> difference(const Quaternion<fltp08>& q1, const Quaternion<fltp08>& q2)
    {
        return q1.invert() * q2;
    }
    static Quaternion<fltp04> difference(const Quaternion<fltp04>& q1, const Quaternion<fltp04>& q2)
    {
        return q1.invert() * q2;
    }
    /**--------------------------------------------------------------------------------------------------
    Struct: Constant<Vertex<t_dims,t_type,t_vector_type>>
 
    A constant.
 
    Author: Tyler Parke
 
    Date: 2017-11-19
    **/
    template<class t_type>
    struct Constant<Quaternion<t_type>>
    {
        constexpr const static Quaternion<t_type> Invalid{ Constant<t_type>::Invalid };
        constexpr const static Quaternion<t_type> Min{ 0 };
        constexpr const static Quaternion<t_type> Max{ Constant<t_type>::Max };
    };
 
    /**--------------------------------------------------------------------------------------------------
        Query if 'value' is Invalid.
 
    Author: Tyler Parke
 
    Date: 2017-11-19
 
    Typeparams:
    t_dims -         Type of the dims.
    t_type -         Type of the type.
    t_vector_type -  Type of the vector type.
    Parameters:
    value -  The value.
 
    \returns True if nan, false if not.
    **/
 
    template<class t_type>
    static constexpr bool IsInvalid(const Quaternion<t_type>& value)
    {
        for (uint01 dim = 0; dim < 4; ++dim)
        {
            if (IsInvalid(value[dim]))
                return true;
        }
        return false;
    }
}