quaternion.cpp

Go to the documentation of this file.

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

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

 This file is part of the QGLViewer library version 2.6.3.

 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.

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

#include "domUtils.h"
#include "quaternion.h"
#include <stdlib.h> // RAND_MAX

// All the methods are declared inline in Quaternion.h
using namespace qglviewer;
using namespace std;

Quaternion::Quaternion(const Vec& from, const Vec& to)
{
        const qreal epsilon = 1E-10;

        const qreal fromSqNorm = from.squaredNorm();
        const qreal toSqNorm   = to.squaredNorm();
        // 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
        {
                Vec axis = cross(from, to);
                const qreal axisSqNorm = axis.squaredNorm();

                // Aligned vectors, pick any axis, not aligned with from or to
                if (axisSqNorm < epsilon)
                        axis = from.orthogonalVec();

                qreal angle = asin(sqrt(axisSqNorm / (fromSqNorm * toSqNorm)));

                if (from*to < 0.0)
                        angle = M_PI-angle;

                setAxisAngle(axis, angle);
        }
}

Vec Quaternion::inverseRotate(const Vec& v) const
{
        return inverse().rotate(v);
}

Vec Quaternion::rotate(const Vec& v) const
{
        const qreal q00 = 2.0 * q[0] * q[0];
        const qreal q11 = 2.0 * q[1] * q[1];
        const qreal q22 = 2.0 * q[2] * q[2];

        const qreal q01 = 2.0 * q[0] * q[1];
        const qreal q02 = 2.0 * q[0] * q[2];
        const qreal q03 = 2.0 * q[0] * q[3];

        const qreal q12 = 2.0 * q[1] * q[2];
        const qreal q13 = 2.0 * q[1] * q[3];

        const qreal q23 = 2.0 * q[2] * q[3];

        return Vec((1.0 - q11 - q22)*v[0] + (      q01 - q23)*v[1] + (      q02 + q13)*v[2],
                        (      q01 + q23)*v[0] + (1.0 - q22 - q00)*v[1] + (      q12 - q03)*v[2],
                        (      q02 - q13)*v[0] + (      q12 + q03)*v[1] + (1.0 - q11 - q00)*v[2] );
}

void Quaternion::setFromRotationMatrix(const qreal m[3][3])
{
        // Compute one plus the trace of the matrix
        const qreal onePlusTrace = 1.0 + m[0][0] + m[1][1] + m[2][2];

        if (onePlusTrace > 1E-5)
        {
                // Direct computation
                const qreal s = sqrt(onePlusTrace) * 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] = 0.25 * s;
        }
        else
        {
                // Computation depends on major diagonal term
                if ((m[0][0] > m[1][1])&(m[0][0] > m[2][2]))
                {
                        const qreal s = sqrt(1.0 + m[0][0] - m[1][1] - m[2][2]) * 2.0;
                        q[0] = 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 qreal s = sqrt(1.0 + m[1][1] - m[0][0] - m[2][2]) * 2.0;
                                q[0] = (m[0][1] + m[1][0]) / s;
                                q[1] = 0.25 * s;
                                q[2] = (m[1][2] + m[2][1]) / s;
                                q[3] = (m[0][2] - m[2][0]) / s;
                        }
                        else
                        {
                                const qreal s = sqrt(1.0 + m[2][2] - m[0][0] - m[1][1]) * 2.0;
                                q[0] = (m[0][2] + m[2][0]) / s;
                                q[1] = (m[1][2] + m[2][1]) / s;
                                q[2] = 0.25 * s;
                                q[3] = (m[0][1] - m[1][0]) / s;
                        }
        }
        normalize();
}

#ifndef DOXYGEN
void Quaternion::setFromRotationMatrix(const float m[3][3])
{
        qWarning("setFromRotationMatrix now expects a double[3][3] parameter");

        qreal mat[3][3];
        for (int i=0; i<3; ++i)
                for (int j=0; j<3; ++j)
                        mat[i][j] = qreal(m[i][j]);

        setFromRotationMatrix(mat);
}

void Quaternion::setFromRotatedBase(const Vec& X, const Vec& Y, const Vec& Z)
{
        qWarning("setFromRotatedBase is deprecated, use setFromRotatedBasis instead");
        setFromRotatedBasis(X,Y,Z);
}
#endif

void Quaternion::setFromRotatedBasis(const Vec& X, const Vec& Y, const Vec& Z)
{
        qreal m[3][3];
        qreal normX = X.norm();
        qreal normY = Y.norm();
        qreal normZ = Z.norm();

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

        setFromRotationMatrix(m);
}

void Quaternion::getAxisAngle(Vec& axis, qreal& angle) const
{
        angle = 2.0 * acos(q[3]);
        axis = Vec(q[0], q[1], q[2]);
        const qreal sinus = axis.norm();
        if (sinus > 1E-8)
                axis /= sinus;

        if (angle > M_PI)
        {
                angle = 2.0 * qreal(M_PI) - angle;
                axis = -axis;
        }
}

Vec Quaternion::axis() const
{
        Vec res = Vec(q[0], q[1], q[2]);
        const qreal sinus = res.norm();
        if (sinus > 1E-8)
                res /= sinus;
        return (acos(q[3]) <= M_PI/2.0) ? res : -res;
}

qreal Quaternion::angle() const
{
        const qreal angle = 2.0 * acos(q[3]);
        return (angle <= M_PI) ? angle : 2.0*M_PI - angle;
}

QDomElement Quaternion::domElement(const QString& name, QDomDocument& document) const
{
        QDomElement de = document.createElement(name);
        de.setAttribute("q0", QString::number(q[0]));
        de.setAttribute("q1", QString::number(q[1]));
        de.setAttribute("q2", QString::number(q[2]));
        de.setAttribute("q3", QString::number(q[3]));
        return de;
}

void Quaternion::initFromDOMElement(const QDomElement& element)
{
        Quaternion q(element);
        *this = q;
}

Quaternion::Quaternion(const QDomElement& element)
{
        QStringList attribute;
        attribute << "q0" << "q1" << "q2" << "q3";
        for (int i=0; i<attribute.size(); ++i)
                q[i] = DomUtils::qrealFromDom(element, attribute[i], ((i<3)?0.0:1.0));
}

const GLdouble* Quaternion::matrix() const
{
        static GLdouble m[4][4];
        getMatrix(m);
        return (const GLdouble*)(m);
}

void Quaternion::getMatrix(GLdouble m[4][4]) const
{
        const qreal q00 = 2.0 * q[0] * q[0];
        const qreal q11 = 2.0 * q[1] * q[1];
        const qreal q22 = 2.0 * q[2] * q[2];

        const qreal q01 = 2.0 * q[0] * q[1];
        const qreal q02 = 2.0 * q[0] * q[2];
        const qreal q03 = 2.0 * q[0] * q[3];

        const qreal q12 = 2.0 * q[1] * q[2];
        const qreal q13 = 2.0 * q[1] * q[3];

        const qreal q23 = 2.0 * q[2] * q[3];

        m[0][0] = 1.0 - q11 - q22;
        m[1][0] =       q01 - q23;
        m[2][0] =       q02 + q13;

        m[0][1] =       q01 + q23;
        m[1][1] = 1.0 - q22 - q00;
        m[2][1] =       q12 - q03;

        m[0][2] =       q02 - q13;
        m[1][2] =       q12 + q03;
        m[2][2] = 1.0 - q11 - q00;

        m[0][3] = 0.0;
        m[1][3] = 0.0;
        m[2][3] = 0.0;

        m[3][0] = 0.0;
        m[3][1] = 0.0;
        m[3][2] = 0.0;
        m[3][3] = 1.0;
}

void Quaternion::getMatrix(GLdouble m[16]) const
{
        static GLdouble mat[4][4];
        getMatrix(mat);
        int count = 0;
        for (int i=0; i<4; ++i)
                for (int j=0; j<4; ++j)
                        m[count++] = mat[i][j];
}

void Quaternion::getRotationMatrix(qreal m[3][3]) const
{
        static GLdouble mat[4][4];
        getMatrix(mat);
        for (int i=0; i<3; ++i)
                for (int j=0; j<3; ++j)
                        // Beware of transposition
                        m[i][j] = qreal(mat[j][i]);
}

const GLdouble* Quaternion::inverseMatrix() const
{
        static GLdouble m[4][4];
        getInverseMatrix(m);
        return (const GLdouble*)(m);
}

void Quaternion::getInverseMatrix(GLdouble m[4][4]) const
{
        inverse().getMatrix(m);
}

void Quaternion::getInverseMatrix(GLdouble m[16]) const
{
        inverse().getMatrix(m);
}

void Quaternion::getInverseRotationMatrix(qreal m[3][3]) const
{
        static GLdouble mat[4][4];
        getInverseMatrix(mat);
        for (int i=0; i<3; ++i)
                for (int j=0; j<3; ++j)
                        // Beware of transposition
                        m[i][j] = qreal(mat[j][i]);
}


Quaternion Quaternion::slerp(const Quaternion& a, const Quaternion& b, qreal t, bool allowFlip)
{
        qreal cosAngle = Quaternion::dot(a, b);

        qreal c1, c2;
        // Linear interpolation for close orientations
        if ((1.0 - fabs(cosAngle)) < 0.01)
        {
                c1 = 1.0 - t;
                c2 = t;
        }
        else
        {
                // Spherical interpolation
                qreal angle    = acos(fabs(cosAngle));
                qreal sinAngle = sin(angle);
                c1 = sin(angle * (1.0 - t)) / sinAngle;
                c2 = sin(angle * t) / sinAngle;
        }

        // Use the shortest path
        if (allowFlip && (cosAngle < 0.0))
                c1 = -c1;

        return Quaternion(c1*a[0] + c2*b[0], c1*a[1] + c2*b[1], c1*a[2] + c2*b[2], c1*a[3] + c2*b[3]);
}

Quaternion Quaternion::squad(const Quaternion& a, const Quaternion& tgA, const Quaternion& tgB, const Quaternion& b, qreal t)
{
        Quaternion ab = Quaternion::slerp(a, b, t);
        Quaternion tg = Quaternion::slerp(tgA, tgB, t, false);
        return Quaternion::slerp(ab, tg, 2.0*t*(1.0-t), false);
}

Quaternion Quaternion::log()
{
        qreal len = sqrt(q[0]*q[0] + q[1]*q[1] + q[2]*q[2]);

        if (len < 1E-6)
                return Quaternion(q[0], q[1], q[2], 0.0);
        else
        {
                qreal coef = acos(q[3]) / len;
                return Quaternion(q[0]*coef, q[1]*coef, q[2]*coef, 0.0);
        }
}

Quaternion Quaternion::exp()
{
        qreal theta = sqrt(q[0]*q[0] + q[1]*q[1] + q[2]*q[2]);

        if (theta < 1E-6)
                return Quaternion(q[0], q[1], q[2], cos(theta));
        else
        {
                qreal coef = sin(theta) / theta;
                return Quaternion(q[0]*coef, q[1]*coef, q[2]*coef, cos(theta));
        }
}

Quaternion Quaternion::lnDif(const Quaternion& a, const Quaternion& b)
{
        Quaternion dif = a.inverse()*b;
        dif.normalize();
        return dif.log();
}

Quaternion Quaternion::squadTangent(const Quaternion& before, const Quaternion& center, const Quaternion& after)
{
        Quaternion l1 = Quaternion::lnDif(center,before);
        Quaternion l2 = Quaternion::lnDif(center,after);
        Quaternion e;
        for (int i=0; i<4; ++i)
                e.q[i] = -0.25 * (l1.q[i] + l2.q[i]);
        e = center*(e.exp());

        // if (Quaternion::dot(e,b) < 0.0)
        // e.negate();

        return e;
}

ostream& operator<<(ostream& o, const Quaternion& Q)
{
        return o << Q[0] << '\t' << Q[1] << '\t' << Q[2] << '\t' << Q[3];
}

Quaternion Quaternion::randomQuaternion()
{
        // The rand() function is not very portable and may not be available on your system.
        // Add the appropriate include or replace by an other random function in case of problem.
        qreal seed = rand()/(qreal)RAND_MAX;
        qreal r1 = sqrt(1.0 - seed);
        qreal r2 = sqrt(seed);
        qreal t1 = 2.0 * M_PI * (rand()/(qreal)RAND_MAX);
        qreal t2 = 2.0 * M_PI * (rand()/(qreal)RAND_MAX);
        return Quaternion(sin(t1)*r1, cos(t1)*r1, sin(t2)*r2, cos(t2)*r2);
}