frame.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 "frame.h"
#include <math.h>

using namespace qglviewer;
using namespace std;


Frame::Frame()
    : constraint_(NULL), referenceFrame_(NULL)
{}

Frame::Frame(const Vec& position, const Quaternion& orientation)
    : t_(position), q_(orientation), constraint_(NULL), referenceFrame_(NULL)
{}

Frame& Frame::operator=(const Frame& frame)
{
    // Automatic compiler generated version would not emit the modified() signals as is done in
    // setTranslationAndRotation.
    setTranslationAndRotation(frame.translation(), frame.rotation());
    setConstraint(frame.constraint());
    setReferenceFrame(frame.referenceFrame());
    return *this;
}

Frame::Frame(const Frame& frame)
    : QObject()
{
    (*this) = frame;
}


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

void Frame::getMatrix(GLdouble m[4][4]) const
{
    q_.getMatrix(m);

    m[3][0] = t_[0];
    m[3][1] = t_[1];
    m[3][2] = t_[2];
}

void Frame::getMatrix(GLdouble m[16]) const
{
    q_.getMatrix(m);

    m[12] = t_[0];
    m[13] = t_[1];
    m[14] = t_[2];
}

Frame Frame::inverse() const
{
    Frame fr(-(q_.inverseRotate(t_)), q_.inverse());
    fr.setReferenceFrame(referenceFrame());
    return fr;
}

const GLdouble* Frame::worldMatrix() const
{
    // This test is done for efficiency reasons (creates lots of temp objects otherwise).
    if (referenceFrame())
    {
        static Frame fr;
        fr.setTranslation(position());
        fr.setRotation(orientation());
        return fr.matrix();
    }
    else
        return matrix();
}

void Frame::getWorldMatrix(GLdouble m[4][4]) const
{
    const GLdouble* mat = worldMatrix();
    for (int i=0; i<4; ++i)
        for (int j=0; j<4; ++j)
            m[i][j] = mat[i*4+j];
}

void Frame::getWorldMatrix(GLdouble m[16]) const
{
    const GLdouble* mat = worldMatrix();
    for (int i=0; i<16; ++i)
        m[i] = mat[i];
}

void Frame::setFromMatrix(const GLdouble m[4][4])
{
    if (fabs(m[3][3]) < 1E-8)
    {
        qWarning("Frame::setFromMatrix: Null homogeneous coefficient");
        return;
    }

    double rot[3][3];
    for (int i=0; i<3; ++i)
    {
        t_[i] = m[3][i] / m[3][3];
        for (int j=0; j<3; ++j)
            // Beware of the transposition (OpenGL to European math)
            rot[i][j] = m[j][i] / m[3][3];
    }
    q_.setFromRotationMatrix(rot);
    Q_EMIT modified();
}

void Frame::setFromMatrix(const GLdouble m[16])
{
    GLdouble mat[4][4];
    for (int i=0; i<4; ++i)
        for (int j=0; j<4; ++j)
            mat[i][j] = m[i*4+j];
    setFromMatrix(mat);
}



void Frame::setTranslation(float x, float y, float z)
{
    setTranslation(Vec(x, y, z));
}

void Frame::getTranslation(float& x, float& y, float& z) const
{
    const Vec t = translation();
    x = t[0];
    y = t[1];
    z = t[2];
}

void Frame::setRotation(double q0, double q1, double q2, double q3)
{
    setRotation(Quaternion(q0, q1, q2, q3));
}

void Frame::getRotation(double& q0, double& q1, double& q2, double& q3) const
{
    const Quaternion q = rotation();
    q0 = q[0];
    q1 = q[1];
    q2 = q[2];
    q3 = q[3];
}


void Frame::translate(const Vec& t)
{
    Vec tbis = t;
    translate(tbis);
}

void Frame::translate(Vec& t)
{
    if (constraint())
        constraint()->constrainTranslation(t, this);
    t_ += t;
    Q_EMIT modified();
}

void Frame::translate(float x, float y, float z)
{
    Vec t(x,y,z);
    translate(t);
}

void Frame::translate(float& x, float& y, float& z)
{
    Vec t(x,y,z);
    translate(t);
    x = t[0];
    y = t[1];
    z = t[2];
}

void Frame::rotate(const Quaternion& q)
{
    Quaternion qbis = q;
    rotate(qbis);
}

void Frame::rotate(Quaternion& q)
{
    if (constraint())
        constraint()->constrainRotation(q, this);
    q_ *= q;
    q_.normalize(); // Prevents numerical drift
    Q_EMIT modified();
}

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

void Frame::rotate(double q0, double q1, double q2, double q3)
{
    Quaternion q(q0,q1,q2,q3);
    rotate(q);
}

void Frame::rotateAroundPoint(Quaternion& rotation, const Vec& point)
{
    if (constraint())
        constraint()->constrainRotation(rotation, this);
    q_ *= rotation;
    q_.normalize(); // Prevents numerical drift
    Vec trans = point + Quaternion(inverseTransformOf(rotation.axis()), rotation.angle()).rotate(position()-point) - t_;
    if (constraint())
        constraint()->constrainTranslation(trans, this);
    t_ += trans;
    Q_EMIT modified();
}

void Frame::rotateAroundPoint(const Quaternion& rotation, const Vec& point)
{
    Quaternion rot = rotation;
    rotateAroundPoint(rot, point);
}


void Frame::setPosition(const Vec& position)
{
    if (referenceFrame())
        setTranslation(referenceFrame()->coordinatesOf(position));
    else
        setTranslation(position);
}

void Frame::setPosition(float x, float y, float z)
{
    setPosition(Vec(x, y, z));
}

void Frame::setPositionAndOrientation(const Vec& position, const Quaternion& orientation)
{
    if (referenceFrame())
    {
        t_ = referenceFrame()->coordinatesOf(position);
        q_ = referenceFrame()->orientation().inverse() * orientation;
    }
    else
    {
        t_ = position;
        q_ = orientation;
    }
    Q_EMIT modified();
}


void Frame::setTranslationAndRotation(const Vec& translation, const Quaternion& rotation)
{
    t_ = translation;
    q_ = rotation;
    Q_EMIT modified();
}


void Frame::getPosition(float& x, float& y, float& z) const
{
    Vec p = position();
    x = p.x;
    y = p.y;
    z = p.z;
}

void Frame::setOrientation(const Quaternion& orientation)
{
    if (referenceFrame())
        setRotation(referenceFrame()->orientation().inverse() * orientation);
    else
        setRotation(orientation);
}

void Frame::setOrientation(double q0, double q1, double q2, double q3)
{
    setOrientation(Quaternion(q0, q1, q2, q3));
}

void Frame::getOrientation(double& q0, double& q1, double& q2, double& q3) const
{
    Quaternion o = orientation();
    q0 = o[0];
    q1 = o[1];
    q2 = o[2];
    q3 = o[3];
}

Vec Frame::position() const {
    if (referenceFrame_)
        return inverseCoordinatesOf(Vec(0.0,0.0,0.0));
    else
        return t_;
}

Quaternion Frame::orientation() const
{
    Quaternion res = rotation();
    const Frame* fr = referenceFrame();
    while (fr != NULL)
    {
        res = fr->rotation() * res;
        fr  = fr->referenceFrame();
    }
    return res;
}



void Frame::setTranslationWithConstraint(Vec& translation)
{
    Vec deltaT = translation - this->translation();
    if (constraint())
        constraint()->constrainTranslation(deltaT, this);

    setTranslation(this->translation() + deltaT);
    translation = this->translation();
}

void Frame::setRotationWithConstraint(Quaternion& rotation)
{
    Quaternion deltaQ = this->rotation().inverse() * rotation;
    if (constraint())
        constraint()->constrainRotation(deltaQ, this);

    // Prevent numerical drift
    deltaQ.normalize();

    setRotation(this->rotation() * deltaQ);
    q_.normalize();
    rotation = this->rotation();
}

void Frame::setTranslationAndRotationWithConstraint(Vec& translation, Quaternion& rotation)
{
    Vec deltaT = translation - this->translation();
    Quaternion deltaQ = this->rotation().inverse() * rotation;

    if (constraint())
    {
        constraint()->constrainTranslation(deltaT, this);
        constraint()->constrainRotation(deltaQ, this);
    }

    // Prevent numerical drift
    deltaQ.normalize();

    t_ += deltaT;
    q_ *= deltaQ;
    q_.normalize();

    translation = this->translation();
    rotation = this->rotation();

    Q_EMIT modified();
}

void Frame::setPositionWithConstraint(Vec& position)
{
    if (referenceFrame())
        position = referenceFrame()->coordinatesOf(position);

    setTranslationWithConstraint(position);
}

void Frame::setOrientationWithConstraint(Quaternion& orientation)
{
    if (referenceFrame())
        orientation = referenceFrame()->orientation().inverse() * orientation;

    setRotationWithConstraint(orientation);
}

void Frame::setPositionAndOrientationWithConstraint(Vec& position, Quaternion& orientation)
{
    if (referenceFrame())
    {
        position = referenceFrame()->coordinatesOf(position);
        orientation = referenceFrame()->orientation().inverse() * orientation;
    }
    setTranslationAndRotationWithConstraint(position, orientation);
}



void Frame::setReferenceFrame(const Frame* const refFrame)
{
    if (settingAsReferenceFrameWillCreateALoop(refFrame))
        qWarning("Frame::setReferenceFrame would create a loop in Frame hierarchy");
    else
    {
        bool identical = (referenceFrame_ == refFrame);
        referenceFrame_ = refFrame;
        if (!identical)
            Q_EMIT modified();
    }
}

bool Frame::settingAsReferenceFrameWillCreateALoop(const Frame* const frame)
{
    const Frame* f = frame;
    while (f != NULL)
    {
        if (f == this)
            return true;
        f = f->referenceFrame();
    }
    return false;
}


Vec Frame::coordinatesOf(const Vec& src) const
{
    if (referenceFrame())
        return localCoordinatesOf(referenceFrame()->coordinatesOf(src));
    else
        return localCoordinatesOf(src);
}

Vec Frame::inverseCoordinatesOf(const Vec& src) const
{
    const Frame* fr = this;
    Vec res = src;
    while (fr != NULL)
    {
        res = fr->localInverseCoordinatesOf(res);
        fr  = fr->referenceFrame();
    }
    return res;
}

Vec Frame::localCoordinatesOf(const Vec& src) const
{
    return rotation().inverseRotate(src - translation());
}

Vec Frame::localInverseCoordinatesOf(const Vec& src) const
{
    return rotation().rotate(src) + translation();
}

Vec Frame::coordinatesOfFrom(const Vec& src, const Frame* const from) const
{
    if (this == from)
        return src;
    else
        if (referenceFrame())
            return localCoordinatesOf(referenceFrame()->coordinatesOfFrom(src, from));
        else
            return localCoordinatesOf(from->inverseCoordinatesOf(src));
}

Vec Frame::coordinatesOfIn(const Vec& src, const Frame* const in) const
{
    const Frame* fr = this;
    Vec res = src;
    while ((fr != NULL) && (fr != in))
    {
        res = fr->localInverseCoordinatesOf(res);
        fr  = fr->referenceFrame();
    }

    if (fr != in)
        // in was not found in the branch of this, res is now expressed in the world
        // coordinate system. Simply convert to in coordinate system.
        res = in->coordinatesOf(res);

    return res;
}


void Frame::getCoordinatesOf(const float src[3], float res[3]) const
{
    const Vec r = coordinatesOf(Vec(src));
    for (int i=0; i<3 ; ++i)
        res[i] = r[i];
}

void Frame::getInverseCoordinatesOf(const float src[3], float res[3]) const
{
    const Vec r = inverseCoordinatesOf(Vec(src));
    for (int i=0; i<3 ; ++i)
        res[i] = r[i];
}

void Frame::getLocalCoordinatesOf(const float src[3], float res[3]) const
{
    const Vec r = localCoordinatesOf(Vec(src));
    for (int i=0; i<3 ; ++i)
        res[i] = r[i];
}

void Frame::getLocalInverseCoordinatesOf(const float src[3], float res[3]) const
{
    const Vec r = localInverseCoordinatesOf(Vec(src));
    for (int i=0; i<3 ; ++i)
        res[i] = r[i];
}

void Frame::getCoordinatesOfIn(const float src[3], float res[3], const Frame* const in) const
{
    const Vec r = coordinatesOfIn(Vec(src), in);
    for (int i=0; i<3 ; ++i)
        res[i] = r[i];
}

void Frame::getCoordinatesOfFrom(const float src[3], float res[3], const Frame* const from) const
{
    const Vec r = coordinatesOfFrom(Vec(src), from);
    for (int i=0; i<3 ; ++i)
        res[i] = r[i];
}



Vec Frame::transformOf(const Vec& src) const
{
    if (referenceFrame())
        return localTransformOf(referenceFrame()->transformOf(src));
    else
        return localTransformOf(src);
}

Vec Frame::inverseTransformOf(const Vec& src) const
{
    const Frame* fr = this;
    Vec res = src;
    while (fr != NULL)
    {
        res = fr->localInverseTransformOf(res);
        fr  = fr->referenceFrame();
    }
    return res;
}

Vec Frame::localTransformOf(const Vec& src) const
{
    return rotation().inverseRotate(src);
}

Vec Frame::localInverseTransformOf(const Vec& src) const
{
    return rotation().rotate(src);
}

Vec Frame::transformOfFrom(const Vec& src, const Frame* const from) const
{
    if (this == from)
        return src;
    else
        if (referenceFrame())
            return localTransformOf(referenceFrame()->transformOfFrom(src, from));
        else
            return localTransformOf(from->inverseTransformOf(src));
}

Vec Frame::transformOfIn(const Vec& src, const Frame* const in) const
{
    const Frame* fr = this;
    Vec    res = src;
    while ((fr != NULL) && (fr != in))
    {
        res = fr->localInverseTransformOf(res);
        fr  = fr->referenceFrame();
    }

    if (fr != in)
        // in was not found in the branch of this, res is now expressed in the world
        // coordinate system. Simply convert to in coordinate system.
        res = in->transformOf(res);

    return res;
}


void Frame::getTransformOf(const float src[3], float res[3]) const
{
    Vec r = transformOf(Vec(src));
    for (int i=0; i<3 ; ++i)
        res[i] = r[i];
}

void Frame::getInverseTransformOf(const float src[3], float res[3]) const
{
    Vec r = inverseTransformOf(Vec(src));
    for (int i=0; i<3 ; ++i)
        res[i] = r[i];
}

void Frame::getLocalTransformOf(const float src[3], float res[3]) const
{
    Vec r = localTransformOf(Vec(src));
    for (int i=0; i<3 ; ++i)
        res[i] = r[i];
}

void Frame::getLocalInverseTransformOf(const float src[3], float res[3]) const
{
    Vec r = localInverseTransformOf(Vec(src));
    for (int i=0; i<3 ; ++i)
        res[i] = r[i];
}

void Frame::getTransformOfIn(const float src[3], float res[3], const Frame* const in) const
{
    Vec r = transformOfIn(Vec(src), in);
    for (int i=0; i<3 ; ++i)
        res[i] = r[i];
}

void Frame::getTransformOfFrom(const float src[3], float res[3], const Frame* const from) const
{
    Vec r = transformOfFrom(Vec(src), from);
    for (int i=0; i<3 ; ++i)
        res[i] = r[i];
}


QDomElement Frame::domElement(const QString& name, QDomDocument& document) const
{
    // TODO: use translation and rotation instead when referenceFrame is coded...
    QDomElement e = document.createElement(name);
    e.appendChild(position().domElement("position", document));
    e.appendChild(orientation().domElement("orientation", document));
    return e;
}

void Frame::initFromDOMElement(const QDomElement& element)
{
    // TODO: use translation and rotation instead when referenceFrame is coded...

    // Reset default values. Attention: destroys constraint.
    // *this = Frame();
    // This instead ? Better : what is not set is not changed.
    // setPositionAndOrientation(Vec(), Quaternion());

    QDomElement child=element.firstChild().toElement();
    while (!child.isNull())
    {
        if (child.tagName() == "position")
            setPosition(Vec(child));
        if (child.tagName() == "orientation")
            setOrientation(Quaternion(child).normalized());

        child = child.nextSibling().toElement();
    }
}


void Frame::alignWithFrame(const Frame* const frame, bool move, float threshold)
{
    Vec directions[2][3];
    for (int d=0; d<3; ++d)
    {
        Vec dir((d==0)? 1.0 : 0.0, (d==1)? 1.0 : 0.0, (d==2)? 1.0 : 0.0);
        if (frame)
            directions[0][d] = frame->inverseTransformOf(dir);
        else
            directions[0][d] = dir;
        directions[1][d] = inverseTransformOf(dir);
    }

    float maxProj = 0.0f;
    float proj;
    unsigned short index[2];
    index[0] = index[1] = 0;
    for (int i=0; i<3; ++i)
        for (int j=0; j<3; ++j)
            if ( (proj=fabs(directions[0][i]*directions[1][j])) >= maxProj )
            {
                index[0] = i;
                index[1] = j;
                maxProj  = proj;
            }

    Frame old;
    old=*this;

    float coef = directions[0][index[0]] * directions[1][index[1]];
    if (fabs(coef) >= threshold)
    {
        const Vec axis = cross(directions[0][index[0]], directions[1][index[1]]);
        float angle = asin(axis.norm());
        if (coef >= 0.0)
            angle = -angle;
        rotate(rotation().inverse() * Quaternion(axis, angle) * orientation());

        // Try to align an other axis direction
        unsigned short d = (index[1]+1) % 3;
        Vec dir((d==0)? 1.0 : 0.0, (d==1)? 1.0 : 0.0, (d==2)? 1.0 : 0.0);
        dir = inverseTransformOf(dir);

        float max = 0.0f;
        for (int i=0; i<3; ++i)
        {
            float proj = fabs(directions[0][i]*dir);
            if (proj > max)
            {
                index[0] = i;
                max = proj;
            }
        }

        if (max >= threshold)
        {
            const Vec axis = cross(directions[0][index[0]], dir);
            float angle = asin(axis.norm());
            if (directions[0][index[0]] * dir >= 0.0)
                angle = -angle;
            rotate(rotation().inverse() * Quaternion(axis, angle) * orientation());
        }
    }

    if (move)
    {
        Vec center;
        if (frame)
            center = frame->position();

        translate(center - orientation().rotate(old.coordinatesOf(center)) - translation());
    }
}

void Frame::projectOnLine(const Vec& origin, const Vec& direction)
{
    // If you are trying to find a bug here, because of memory problems, you waste your time.
    // This is a bug in the gcc 3.3 compiler. Compile the library in debug mode and test.
    // Uncommenting this line also seems to solve the problem. Horrible.
    // cout << "position = " << position() << endl;
    // If you found a problem or are using a different compiler, please let me know.
    const Vec shift = origin - position();
    Vec proj = shift;
    proj.projectOnAxis(direction);
    translate(shift-proj);
}