worldsim/3rdParts/qglviewer/quaternion.h

/****************************************************************************

 Copyright (C) 2002-2008 Gilles Debunne. All rights reserved.

 This file is part of the QGLViewer library version 2.3.10.

 http://www.libqglviewer.com - contact@libqglviewer.com

 This file may be used under the terms of the GNU General Public License 
 versions 2.0 or 3.0 as published by the Free Software Foundation and
 appearing in the LICENSE file included in the packaging of this file.
 In addition, as a special exception, Gilles Debunne gives you certain 
 additional rights, described in the file GPL_EXCEPTION in this package.

 libQGLViewer uses dual licensing. Commercial/proprietary software must
 purchase a libQGLViewer Commercial License.

 This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
 WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.

*****************************************************************************/

#ifndef QGLVIEWER_QUATERNION_H
#define QGLVIEWER_QUATERNION_H

#include "vec.h"
#include <math.h>
#include <iostream>

namespace qglviewer {
    class QGLVIEWER_EXPORT Quaternion
    {
    public:
        Quaternion()
        { q[0]=q[1]=q[2]=0.0;  q[3]=1.0; }

        Quaternion(const Vec& axis, double angle)
        {
            setAxisAngle(axis, angle);
        }

        Quaternion(const Vec& from, const Vec& to);

        Quaternion(double q0, double q1, double q2, double q3)
        { q[0]=q0;    q[1]=q1;    q[2]=q2;    q[3]=q3; }

        Quaternion(const Quaternion& Q)
        { for (int i=0; i<4; ++i) q[i] = Q.q[i]; }

        Quaternion& operator=(const Quaternion& Q)
        {
            for (int i=0; i<4; ++i)
                q[i] = Q.q[i];
            return (*this);
        }

        void setAxisAngle(const Vec& axis, double angle)
        {
            const double norm = axis.norm();
            if (norm < 1E-8)
            {
                // Null rotation
                q[0] = 0.0;      q[1] = 0.0;      q[2] = 0.0;      q[3] = 1.0;
            }
            else
            {
                const double sin_half_angle = sin(angle / 2.0);
                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] = cos(angle / 2.0);
            }
        }

        void setValue(double q0, double q1, double q2, double q3)
        { q[0]=q0;    q[1]=q1;    q[2]=q2;    q[3]=q3; }

#ifndef DOXYGEN
        void setFromRotationMatrix(const float m[3][3]);
        void setFromRotatedBase(const Vec& X, const Vec& Y, const Vec& Z);
#endif
        void setFromRotationMatrix(const double m[3][3]);
        void setFromRotatedBasis(const Vec& X, const Vec& Y, const Vec& Z);


        Vec axis() const;
        double angle() const;
        void getAxisAngle(Vec& axis, float& angle) const;

        double operator[](int i) const { return q[i]; }

        double& operator[](int i) { return q[i]; }


        friend Quaternion operator*(const Quaternion& a, const Quaternion& b)
        {
            return Quaternion(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]);
        }

        Quaternion& operator*=(const Quaternion &q)
        {
            *this = (*this)*q;
            return *this;
        }

        friend Vec operator*(const Quaternion& q, const Vec& v)
        {
            return q.rotate(v);
        }

        Vec rotate(const Vec& v) const;
        Vec inverseRotate(const Vec& v) const;


        Quaternion inverse() const { return Quaternion(-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]; }

        double normalize()
        {
            const double norm = 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;
        }

        Quaternion normalized() const
        {
            double Q[4];
            const double norm = 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 Quaternion(Q[0], Q[1], Q[2], Q[3]);
        }


        const GLdouble* matrix() const;
        void getMatrix(GLdouble m[4][4]) const;
        void getMatrix(GLdouble m[16]) const;

        void getRotationMatrix(float m[3][3]) const;

        const GLdouble* inverseMatrix() const;
        void getInverseMatrix(GLdouble m[4][4]) const;
        void getInverseMatrix(GLdouble m[16]) const;

        void getInverseRotationMatrix(float m[3][3]) const;


        static Quaternion slerp(const Quaternion& a, const Quaternion& b, float t, bool allowFlip=true);
        static Quaternion squad(const Quaternion& a, const Quaternion& tgA, const Quaternion& tgB, const Quaternion& b, float t);
        static double dot(const Quaternion& a, const Quaternion& b) { return a[0]*b[0] + a[1]*b[1] + a[2]*b[2] + a[3]*b[3]; }

        Quaternion log();
        Quaternion exp();
        static Quaternion lnDif(const Quaternion& a, const Quaternion& b);
        static Quaternion squadTangent(const Quaternion& before, const Quaternion& center, const Quaternion& after);

        static Quaternion randomQuaternion();

        explicit Quaternion(const QDomElement& element);
        QDomElement domElement(const QString& name, QDomDocument& document) const;
        void initFromDOMElement(const QDomElement& element);

#ifdef DOXYGEN

        std::ostream& operator<<(std::ostream& o, const qglviewer::Vec&);
#endif

    private:
        double q[4];
    };

} // namespace

std::ostream& operator<<(std::ostream& o, const qglviewer::Quaternion&);

#endif // QGLVIEWER_QUATERNION_H