quat.h

/********************************************************************
 * Copyright (C) 2015 Liangliang Nan <liangliang.nan@gmail.com>
 * https://3d.bk.tudelft.nl/liangliang/
 *
 * This file is part of Easy3D. If it is useful in your research/work,
 * I would be grateful if you show your appreciation by citing it:
 * ------------------------------------------------------------------
 *      Liangliang Nan.
 *      Easy3D: a lightweight, easy-to-use, and efficient C++ library
 *      for processing and rendering 3D data.
 *      Journal of Open Source Software, 6(64), 3255, 2021.
 * ------------------------------------------------------------------
 *
 * Easy3D is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License Version 3
 * as published by the Free Software Foundation.
 *
 * Easy3D is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program. If not, see <http://www.gnu.org/licenses/>.
 ********************************************************************/
 
#ifndef EASY3D_CORE_QUATERNION_H
#define EASY3D_CORE_QUATERNION_H
 
#include <easy3d/core/constant.h>
#include <easy3d/core/vec.h>
#include <easy3d/core/mat.h>
 
 
namespace easy3d {
 
    template <typename FT>
    class Quat
    {
    public:
        typedef Vec<3, FT>  Vec3;
        typedef Quat<FT>    thisclass;
 
    public:
        Quat()
        {
            _q[0] = _q[1] = _q[2] = FT(0);  _q[3] = FT(1);
        }
 
        explicit Quat(const Mat3<FT>& m)
        {
            set_from_rotation_matrix(m);
        }
 
        Quat(const Vec3& axis, FT angle)
        {
            set_axis_angle(axis, angle);
        }
 
        Quat(const Vec3& from, const Vec3& to);
 
        Quat(FT q0, FT q1, FT q2, FT q3)
        { _q[0]=q0;    _q[1]=q1;    _q[2]=q2;    _q[3]=q3; }
 
        Quat(const thisclass& Q)
        { for (int i=0; i<4; ++i) _q[i] = Q._q[i]; }
 
        Quat& operator=(const thisclass& Q) {
            for (int i=0; i<4; ++i)
                _q[i] = Q._q[i];
            return (*this);
        }
 
        void set_axis_angle(const Vec3& axis, FT angle);
 
        void set_value(FT q0, FT q1, FT q2, FT q3)
        { _q[0]=q0;    _q[1]=q1;    _q[2]=q2;    _q[3]=q3; }
 
        void set_from_rotation_matrix(const Mat3<FT>& m);
 
        void set_from_rotated_basis(const Vec3& X, const Vec3& Y, const Vec3& Z);
 
        Vec3 axis() const;
 
        FT angle() const;
 
        void get_axis_angle(Vec3& axis, FT& angle) const;
 
        FT operator[](int i) const { return _q[i]; }
 
        FT& operator[](int i) { return _q[i]; }
 
        /* Rotation computations */
 
        friend Quat operator*(const Quat& a, const Quat& b)
        {
            return Quat(
                a._q[3]*b._q[0] + b._q[3]*a._q[0] + a._q[1]*b._q[2] - a._q[2]*b._q[1],
                a._q[3]*b._q[1] + b._q[3]*a._q[1] + a._q[2]*b._q[0] - a._q[0]*b._q[2],
                a._q[3]*b._q[2] + b._q[3]*a._q[2] + a._q[0]*b._q[1] - a._q[1]*b._q[0],
                a._q[3]*b._q[3] - b._q[0]*a._q[0] - a._q[1]*b._q[1] - a._q[2]*b._q[2]
                );
        }
 
        Quat& operator*=(const Quat &q) {
            *this = (*this)*q;
            return *this;
        }
 
        friend Vec3 operator*(const Quat& q, const Vec3& v) { return q.rotate(v); }
 
        Vec3 rotate(const Vec3& v) const;
 
        Vec3 inverse_rotate(const Vec3& v) const {
            return inverse().rotate(v);
        }
 
        Quat inverse() const { return Quat(-_q[0], -_q[1], -_q[2], _q[3]); }
 
        void invert() { _q[0] = -_q[0]; _q[1] = -_q[1]; _q[2] = -_q[2]; }
 
        void negate() { invert(); _q[3] = -_q[3]; }
 
        FT length() const {
            return std::sqrt(_q[0] * _q[0] + _q[1] * _q[1] + _q[2] * _q[2] + _q[3] * _q[3]);
        }
 
        FT normalize() {
            const FT norm = std::sqrt(_q[0] * _q[0] + _q[1] * _q[1] + _q[2] * _q[2] + _q[3] * _q[3]);
            for (int i=0; i<4; ++i)
                _q[i] /= norm;
            return norm;
        }
 
        Quat normalized() const {
            FT Q[4];
            const FT norm = std::sqrt(_q[0] * _q[0] + _q[1] * _q[1] + _q[2] * _q[2] + _q[3] * _q[3]);
            for (int i=0; i<4; ++i)
                Q[i] = _q[i] / norm;
            return Quat(Q[0], Q[1], Q[2], Q[3]);
        }
 
        Mat4<FT> matrix() const;
 
        Mat4<FT> inverse_matrix() const;
 
        static Quat slerp(const Quat<FT>& a, const Quat<FT>& b, FT t, bool allowFlip = true);
 
        static Quat squad(const Quat<FT>& a, const Quat<FT>& tgA, const Quat<FT>& tgB, const Quat<FT>& b, FT t);
 
        static FT dot(const Quat<FT>& a, const Quat<FT>& b) { return a[0] * b[0] + a[1] * b[1] + a[2] * b[2] + a[3] * b[3]; }
 
        Quat log();
        Quat exp();
 
        static Quat ln_dif(const Quat<FT>& a, const Quat<FT>& b);
        static Quat squad_tangent(const Quat<FT>& before, const Quat<FT>& center, const Quat<FT>& after);
 
        static thisclass random_quat();
 
        //data intentionally left public to allow q.x ...
    public:
        /* The internal data representation is private, use operator[] to access values. */
        union {
            struct { FT x; FT y; FT z; FT w; };
            FT _q[4];
        };
    };
 
    template <typename FT>  std::ostream& operator<<(std::ostream& os, const Quat<FT>& q);
    template <typename FT>  std::istream& operator>>(std::istream& is, Quat<FT>& q);
 
 
    //-------------- implementation -----------------
 
 
    template <typename FT>
    Quat<FT>::Quat(const Vec3& from, const Vec3& to)
    {
        const FT epsilon = FT(1E-10);
 
        const FT fromSqNorm = from.length2();
        const FT toSqNorm = to.length2();
        // Identity Quaternion when one vector is null
        if ((fromSqNorm < epsilon) || (toSqNorm < epsilon))
        {
            _q[0] = _q[1] = _q[2] = 0.0;
            _q[3] = 1.0;
        }
        else
        {
            Vec3 axis = cross(from, to);
            const FT axisSqNorm = axis.length2();
 
            // Aligned vectors, pick any axis, not aligned with from or to
            if (axisSqNorm < epsilon)
                axis = orthogonal(from);
 
            FT angle = std::asin(std::sqrt(axisSqNorm / (fromSqNorm * toSqNorm)));
 
            if (easy3d::dot(from, to) < 0.0)
                angle = FT(M_PI - angle);
 
            set_axis_angle(axis, angle);
        }
    }
 
    template <typename FT>
    void Quat<FT>::set_axis_angle(const Vec3& axis, FT angle)
    {
        const FT norm = axis.length();
        if (norm < 1E-8)
        {
            // Null rotation
            _q[0] = FT(0);      _q[1] = FT(0);       _q[2] = FT(0);      _q[3] = FT(1);
        }
        else
        {
            const FT sin_half_angle = std::sin(angle / 2.0f);
            _q[0] = sin_half_angle*axis[0] / norm;
            _q[1] = sin_half_angle*axis[1] / norm;
            _q[2] = sin_half_angle*axis[2] / norm;
            _q[3] = (FT)std::cos(angle / 2.0f);
        }
    }
 
 
    template <typename FT>
    typename Quat<FT>::Vec3 Quat<FT>::rotate(const Vec3& v) const
    {
        const FT q00 = FT(2.0l * _q[0] * _q[0]);
        const FT q11 = FT(2.0l * _q[1] * _q[1]);
        const FT q22 = FT(2.0l * _q[2] * _q[2]);
 
        const FT q01 = FT(2.0l * _q[0] * _q[1]);
        const FT q02 = FT(2.0l * _q[0] * _q[2]);
        const FT q03 = FT(2.0l * _q[0] * _q[3]);
 
        const FT q12 = FT(2.0l * _q[1] * _q[2]);
        const FT q13 = FT(2.0l * _q[1] * _q[3]);
 
        const FT q23 = FT(2.0l * _q[2] * _q[3]);
 
        return Vec3(
            FT((1.0 - q11 - q22)*v[0] + (q01 - q23)*v[1] + (q02 + q13)*v[2]),
            FT((q01 + q23)*v[0] + (1.0 - q22 - q00)*v[1] + (q12 - q03)*v[2]),
            FT((q02 - q13)*v[0] + (q12 + q03)*v[1] + (1.0 - q11 - q00)*v[2]));
    }
 
    template <typename FT>
    void Quat<FT>::set_from_rotation_matrix(const Mat3<FT>& m)
    {
        //  see http://www.euclideanspace.com/maths/geometry/rotations/conversions/matrixToQuaternion/index.htm
        // Compute one plus the trace of the matrix
        const FT onePlusTrace = FT(1.0) + m(0, 0) + m(1, 1) + m(2, 2);
 
        if (onePlusTrace > 1E-5) {
            // Direct computation
            const FT s = std::sqrt(onePlusTrace) * FT(2.0);
            _q[0] = (m(2, 1) - m(1, 2)) / s;
            _q[1] = (m(0, 2) - m(2, 0)) / s;
            _q[2] = (m(1, 0) - m(0, 1)) / s;
            _q[3] = FT(0.25) * s;
        }
        else
        {
            // Computation depends on major diagonal term
            if ((m(0, 0) > m(1, 1)) && (m(0, 0) > m(2, 2))) {
                const FT s = std::sqrt(FT(1.0) + m(0, 0) - m(1, 1) - m(2, 2)) * FT(2.0);
                _q[0] = FT(0.25) * s;
                _q[1] = (m(0, 1) + m(1, 0)) / s;
                _q[2] = (m(0, 2) + m(2, 0)) / s;
                _q[3] = (m(1, 2) - m(2, 1)) / s;
            }
            else if (m(1, 1) > m(2, 2)) {
                const FT s = std::sqrt(FT(1.0) + m(1, 1) - m(0, 0) - m(2, 2)) * FT(2.0);
                _q[0] = (m(0, 1) + m(1, 0)) / s;
                _q[1] = FT(0.25) * s;
                _q[2] = (m(1, 2) + m(2, 1)) / s;
                _q[3] = (m(0, 2) - m(2, 0)) / s;
            }
            else {
                const FT s = std::sqrt(FT(1.0) + m(2, 2) - m(0, 0) - m(1, 1)) * FT(2.0);
                _q[0] = (m(0, 2) + m(2, 0)) / s;
                _q[1] = (m(1, 2) + m(2, 1)) / s;
                _q[2] = FT(0.25) * s;
                _q[3] = (m(0, 1) - m(1, 0)) / s;
            }
        }
        normalize();
    }
 
    template <typename FT>
    void Quat<FT>::set_from_rotated_basis(const Vec3& X, const Vec3& Y, const Vec3& Z)
    {
        Mat3<FT> m;
        FT normX = X.length();
        FT normY = Y.length();
        FT normZ = Z.length();
 
        for (int i = 0; i < 3; ++i) {
            m(i, 0) = X[i] / normX;
            m(i, 1) = Y[i] / normY;
            m(i, 2) = Z[i] / normZ;
        }
 
        set_from_rotation_matrix(m);
    }
 
    template <typename FT>
    void Quat<FT>::get_axis_angle(Vec3& axis, FT& angle) const
    {
        // The normalize() is here to prevent failure introduced by numerical error.
        // We call std::acos(_q[3]), but _q[3] equaling to 1 can actually be e.g., 1.00000012.
        if (_q[3] > FT(1)) {
            const_cast<Quat*>(this)->normalize();
        }
        angle = FT(2.0) * std::acos(_q[3]);
        axis = Vec3(_q[0], _q[1], _q[2]);
        const FT sinus = axis.length();
        if (sinus > 1E-8)
            axis = axis / sinus;
 
        if (angle > M_PI) {
            angle = 2.0*M_PI - angle;
            axis = -axis;
        }
    }
 
    template <typename FT>
    typename Quat<FT>::Vec3 Quat<FT>::axis() const
    {
        Vec3 res(_q[0], _q[1], _q[2]);
        const FT sinus = res.length();
        if (sinus > 1E-8)
            res = res / sinus;
 
        // The normalize() is here to prevent failure introduced by numerical error.
        // We call std::acos(_q[3]), but _q[3] equaling to 1 can actually be e.g., 1.00000012.
        if (_q[3] > FT(1)) {
            const_cast<Quat*>(this)->normalize();
        }
        return (std::acos(_q[3]) <= M_PI / 2.0) ? res : -res;
    }
 
    template <typename FT>
    FT Quat<FT>::angle() const
    {
        // The normalize() is here to prevent failure introduced by numerical error.
        // We call std::acos(_q[3]), but _q[3] equaling to 1 can actually be e.g., 1.00000012.
        if (_q[3] > FT(1)) {
            const_cast<Quat*>(this)->normalize();
        }
        const FT angle = FT(2.0) * std::acos(_q[3]);
        return (angle <= M_PI) ? angle : FT(2.0*M_PI - angle);
    }
 
 
    //Mat4 quat_to_matrix(const Quat& q)
    //{
    //  Mat4 M;
    //
    //  const double x = q.vector()[0];
    //  const double y = q.vector()[1];
    //  const double z = q.vector()[2];
    //  const double w = q.scalar();
    //  const double s = 2 / norm(q);
    //
    //  M(0, 0) = 1 - s*(y*y + z*z); M(0, 1) = s*(x*y - w*z);   M(0, 2) = s*(x*z + w*y);   M(0, 3) = 0;
    //  M(1, 0) = s*(x*y + w*z);   M(1, 1) = 1 - s*(x*x + z*z); M(1, 2) = s*(y*z - w*x);   M(1, 3) = 0;
    //  M(2, 0) = s*(x*z - w*y);   M(2, 1) = s*(y*z + w*x);   M(2, 2) = 1 - s*(x*x + y*y); M(2, 3) = 0;
    //  M(3, 0) = 0;             M(3, 1) = 0;             M(3, 2) = 0;             M(3, 3) = 1;
    //
    //  return M;
    //}
    //
    //Mat4 unit_quat_to_matrix(const Quat& q)
    //{
    //  Mat4 M;
    //
    //  const double x = q.vector()[0];
    //  const double y = q.vector()[1];
    //  const double z = q.vector()[2];
    //  const double w = q.scalar();
    //
    //  M(0, 0) = 1 - 2 * (y*y + z*z); M(0, 1) = 2 * (x*y - w*z);   M(0, 2) = 2 * (x*z + w*y);   M(0, 3) = 0;
    //  M(1, 0) = 2 * (x*y + w*z);   M(1, 1) = 1 - 2 * (x*x + z*z); M(1, 2) = 2 * (y*z - w*x);   M(1, 3) = 0;
    //  M(2, 0) = 2 * (x*z - w*y);   M(2, 1) = 2 * (y*z + w*x);   M(2, 2) = 1 - 2 * (x*x + y*y); M(2, 3) = 0;
    //  M(3, 0) = 0;             M(3, 1) = 0;             M(3, 2) = 0;             M(3, 3) = 1;
    //
    //  return M;
    //}
 
 
    template <typename FT>
    Mat4<FT> Quat<FT>::matrix() const
    {
        const FT q00 = FT(2.0l * _q[0] * _q[0]);
        const FT q11 = FT(2.0l * _q[1] * _q[1]);
        const FT q22 = FT(2.0l * _q[2] * _q[2]);
 
        const FT q01 = FT(2.0l * _q[0] * _q[1]);
        const FT q02 = FT(2.0l * _q[0] * _q[2]);
        const FT q03 = FT(2.0l * _q[0] * _q[3]);
 
        const FT q12 = FT(2.0l * _q[1] * _q[2]);
        const FT q13 = FT(2.0l * _q[1] * _q[3]);
 
        const FT q23 = FT(2.0l * _q[2] * _q[3]);
 
        Mat4<FT> m;
        m(0, 0) = FT(1.0) - q11 - q22;
        m(0, 1) = q01 - q23;
        m(0, 2) = q02 + q13;
 
        m(1, 0) = q01 + q23;
        m(1, 1) = FT(1.0) - q22 - q00;
        m(1, 2) = q12 - q03;
 
        m(2, 0) = q02 - q13;
        m(2, 1) = q12 + q03;
        m(2, 2) = FT(1.0) - q11 - q00;
 
        m(3, 0) = FT(0.0);
        m(3, 1) = FT(0.0);
        m(3, 2) = FT(0.0);
 
        m(0, 3) = FT(0.0);
        m(1, 3) = FT(0.0);
        m(2, 3) = FT(0.0);
        m(3, 3) = FT(1.0);
 
        return m;
    }
 
    template <typename FT>
    Mat4<FT> Quat<FT>::inverse_matrix() const
    {
        return inverse().matrix();
    }
 
    template <typename FT>
    Quat<FT> Quat<FT>::slerp(const Quat<FT>& a, const Quat<FT>& b, FT t, bool allowFlip)
    {
        FT cosAngle = Quat<FT>::dot(a, b);
 
        FT c1, c2;
        // Linear interpolation for close orientations
        if ((1.0 - std::fabs(cosAngle)) < 0.01)
        {
            c1 = FT(1.0) - t;
            c2 = t;
        }
        else
        {
            // Spherical interpolation
            FT angle = std::acos(std::fabs(cosAngle));
            FT sinAngle = std::sin(angle);
            c1 = std::sin(angle * (1.0 - t)) / sinAngle;
            c2 = std::sin(angle * t) / sinAngle;
        }
 
        // Use the shortest path
        if (allowFlip && (cosAngle < 0.0))
            c1 = -c1;
 
        return Quat(c1*a[0] + c2*b[0], c1*a[1] + c2*b[1], c1*a[2] + c2*b[2], c1*a[3] + c2*b[3]);
    }
 
    template <typename FT>
    Quat<FT> Quat<FT>::squad(const Quat<FT>& a, const Quat<FT>& tgA, const Quat<FT>& tgB, const Quat<FT>& b, FT t)
    {
        Quat<FT> ab = Quat<FT>::slerp(a, b, t);
        Quat<FT> tg = Quat<FT>::slerp(tgA, tgB, t, false);
        return Quat<FT>::slerp(ab, tg, 2.0*t*(1.0 - t), false);
    }
 
    template <typename FT>
    Quat<FT> Quat<FT>::log()
    {
        FT len = std::sqrt(_q[0] * _q[0] + _q[1] * _q[1] + _q[2] * _q[2]);
 
        if (len < 1E-6)
            return Quat(_q[0], _q[1], _q[2], 0.0);
        else
        {
            // The normalize() is here to prevent failure introduced by numerical error.
            // We call std::acos(_q[3]), but _q[3] equaling to 1 can actually be e.g., 1.00000012.
            if (_q[3] > FT(1)) {
                const_cast<Quat*>(this)->normalize();
            }       FT coef = std::acos(_q[3]) / len;
            return Quat(_q[0] * coef, _q[1] * coef, _q[2] * coef, 0.0);
        }
    }
 
    template <typename FT>
    Quat<FT> Quat<FT>::exp()
    {
        FT theta = std::sqrt(_q[0] * _q[0] + _q[1] * _q[1] + _q[2] * _q[2]);
 
        if (theta < 1E-6)
            return Quat(_q[0], _q[1], _q[2], std::cos(theta));
        else
        {
            FT coef = std::sin(theta) / theta;
            return Quat(_q[0] * coef, _q[1] * coef, _q[2] * coef, std::cos(theta));
        }
    }
 
    template <typename FT>
    Quat<FT> Quat<FT>::ln_dif(const Quat& a, const Quat& b)
    {
        Quat<FT> dif = a.inverse()*b;
        dif.normalize();
        return dif.log();
    }
 
    template <typename FT>
    Quat<FT> Quat<FT>::squad_tangent(const Quat<FT>& before, const Quat<FT>& center, const Quat<FT>& after)
    {
        Quat<FT> l1 = Quat<FT>::ln_dif(center, before);
        Quat<FT> l2 = Quat<FT>::ln_dif(center, after);
        Quat<FT> e;
        for (int i = 0; i < 4; ++i)
            e._q[i] = -0.25 * (l1._q[i] + l2._q[i]);
        e = center*(e.exp());
 
        // if (Quat<FT>::dot(e,b) < 0.0)
        // e.negate();
 
        return e;
    }
 
    template <typename FT>
    Quat<FT> Quat<FT>::random_quat()
    {
        // The rand() function is not very portable and may not be available on your system.
        // Add the appropriate include or replace it by another random function in case of problem.
        FT seed = rand() / (FT)RAND_MAX;
        FT r1 = std::sqrt(1.0f - seed);
        FT r2 = std::sqrt(seed);
        FT t1 = 2.0f * M_PI * (rand() / (FT)RAND_MAX);
        FT t2 = 2.0f * M_PI * (rand() / (FT)RAND_MAX);
        return Quat(std::sin(t1)*r1, std::cos(t1)*r1, std::sin(t2)*r2, std::cos(t2)*r2);
    }
 
    template <typename FT> inline
    std::ostream& operator<<(std::ostream& os, const Quat<FT>& Q)
    {
        return os << Q[0] << ' ' << Q[1] << ' ' << Q[2] << ' ' << Q[3];
    }
 
    template <typename FT> inline
    std::istream& operator>>(std::istream& is, Quat<FT>& Q) {
        return is >> Q[0] >> Q[1] >> Q[2] >> Q[3];
    }
 
    template <class FT> inline
    bool has_nan(const Quat<FT>& Q) {
        for (int i=0; i<4; ++i) {
            if (std::isnan(Q[i]) || std::isinf(Q[i]))
                return true;
        }
        return false;
    }
 
}
 
 
#endif // EASY3D_CORE_QUATERNION_H