frame.cpp

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

 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.

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

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

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;
      // setOrientation(Quaternion(axis, angle) * orientation());
      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;
      // setOrientation(Quaternion(axis, angle) * orientation());
      rotate(rotation().inverse() * Quaternion(axis, angle) * orientation());
    }
    }

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

      // setPosition(center - orientation().rotate(old.coordinatesOf(center)));
      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);
}