quaternion.cpp

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

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

 This file is part of the QGLViewer library version 2.5.2.

 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 double epsilon = 1E-10f;

    const double fromSqNorm = from.squaredNorm();
    const double 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 double axisSqNorm = axis.squaredNorm();

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

        double 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 double q00 = 2.0l * q[0] * q[0];
    const double q11 = 2.0l * q[1] * q[1];
    const double q22 = 2.0l * q[2] * q[2];

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

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

    const double q23 = 2.0l * 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 double m[3][3])
{
    // Compute one plus the trace of the matrix
    const double onePlusTrace = 1.0 + m[0][0] + m[1][1] + m[2][2];

    if (onePlusTrace > 1E-5)
    {
        // Direct computation
        const double 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 double 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 double 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 double 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 waits for a double[3][3] parameter");

    double mat[3][3];
    for (int i=0; i<3; ++i)
        for (int j=0; j<3; ++j)
            mat[i][j] = double(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)
{
    double m[3][3];
    double normX = X.norm();
    double normY = Y.norm();
    double 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, float& angle) const
{
    angle = 2.0*acos(q[3]);
    axis = Vec(q[0], q[1], q[2]);
    const double sinus = axis.norm();
    if (sinus > 1E-8)
        axis /= sinus;

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

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

double Quaternion::angle() const
{
    const double 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::doubleFromDom(element, attribute[i], ((i<3)?0.0f:1.0f));
}

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 double q00 = 2.0l * q[0] * q[0];
    const double q11 = 2.0l * q[1] * q[1];
    const double q22 = 2.0l * q[2] * q[2];

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

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

    const double q23 = 2.0l * q[2] * q[3];

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

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

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

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

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

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(float 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] = 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(float 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] = mat[j][i];
}


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

    double c1, c2;
    // Linear interpolation for close orientations
    if ((1.0 - fabs(cosAngle)) < 0.01)
    {
        c1 = 1.0 - t;
        c2 = t;
    }
    else
    {
        // Spherical interpolation
        double angle    = acos(fabs(cosAngle));
        double 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, float 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()
{
    double 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
    {
        double coef = acos(q[3]) / len;
        return Quaternion(q[0]*coef, q[1]*coef, q[2]*coef, 0.0);
    }
}

Quaternion Quaternion::exp()
{
    double 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
    {
        double 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.
    double seed = rand()/(double)RAND_MAX;
    double r1 = sqrt(1.0 - seed);
    double r2 = sqrt(seed);
    double t1 = 2.0 * M_PI * (rand()/(double)RAND_MAX);
    double t2 = 2.0 * M_PI * (rand()/(double)RAND_MAX);
    return Quaternion(sin(t1)*r1, cos(t1)*r1, sin(t2)*r2, cos(t2)*r2);
}