frame.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 "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;
        }

        qreal 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(qreal x, qreal y, qreal z)
{
        setTranslation(Vec(x, y, z));
}

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

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

void Frame::getRotation(qreal& q0, qreal& q1, qreal& q2, qreal& 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(qreal x, qreal y, qreal z)
{
        Vec t(x,y,z);
        translate(t);
}

void Frame::translate(qreal& x, qreal& y, qreal& 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(qreal& q0, qreal& q1, qreal& q2, qreal& q3)
{
        Quaternion q(q0,q1,q2,q3);
        rotate(q);
        q0 = q[0];
        q1 = q[1];
        q2 = q[2];
        q3 = q[3];
}

void Frame::rotate(qreal q0, qreal q1, qreal q2, qreal 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(qreal x, qreal y, qreal 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(qreal& x, qreal& y, qreal& 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(qreal q0, qreal q1, qreal q2, qreal q3)
{
        setOrientation(Quaternion(q0, q1, q2, q3));
}

void Frame::getOrientation(qreal& q0, qreal& q1, qreal& q2, qreal& 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 qreal src[3], qreal res[3]) const
{
        const Vec r = coordinatesOf(Vec(src));
        for (int i=0; i<3 ; ++i)
                res[i] = r[i];
}

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

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

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

void Frame::getCoordinatesOfIn(const qreal src[3], qreal 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 qreal src[3], qreal 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 qreal src[3], qreal res[3]) const
{
        Vec r = transformOf(Vec(src));
        for (int i=0; i<3 ; ++i)
                res[i] = r[i];
}

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

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

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

void Frame::getTransformOfIn(const qreal src[3], qreal 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 qreal src[3], qreal 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, qreal threshold)
{
        Vec directions[2][3];
        for (unsigned short 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);
        }

        qreal maxProj = 0.0;
        qreal proj;
        unsigned short index[2];
        index[0] = index[1] = 0;
        for (unsigned short i=0; i<3; ++i)
                for (unsigned short 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;

        qreal 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]]);
                qreal 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);

                qreal max = 0.0;
                for (unsigned short i=0; i<3; ++i)
                {
                        qreal 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);
                        qreal 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);
}