diff options
Diffstat (limited to 'source/blender/freestyle/intern/app_blender/frame.cpp')
| -rw-r--r-- | source/blender/freestyle/intern/app_blender/frame.cpp | 1070 |
1 files changed, 1070 insertions, 0 deletions
diff --git a/source/blender/freestyle/intern/app_blender/frame.cpp b/source/blender/freestyle/intern/app_blender/frame.cpp new file mode 100644 index 00000000000..26c2ee5d2b4 --- /dev/null +++ b/source/blender/freestyle/intern/app_blender/frame.cpp @@ -0,0 +1,1070 @@ +/**************************************************************************** + + Copyright (C) 2002-2007 Gilles Debunne (Gilles.Debunne@imag.fr) + + This file is part of the QGLViewer library. + Version 2.2.6-3, released on August 28, 2007. + + http://artis.imag.fr/Members/Gilles.Debunne/QGLViewer + + libQGLViewer is free software; you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation; either version 2 of the License, or + (at your option) any later version. + + libQGLViewer is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with libQGLViewer; if not, write to the Free Software + Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + +*****************************************************************************/ + +#include "frame.h" +#include <math.h> + +//using namespace qglviewer; +using namespace std; + + +/*! Creates a default Frame. + + Its position() is (0,0,0) and it has an identity orientation() Quaternion. The referenceFrame() + and the constraint() are \c NULL. */ +Frame::Frame() + : constraint_(NULL), referenceFrame_(NULL) +{} + +/*! Creates a Frame with a position() and an orientation(). + + See the Vec and Quaternion documentations for convenient constructors and methods. + + The Frame is defined in the world coordinate system (its referenceFrame() is \c NULL). It + has a \c NULL associated constraint(). */ +Frame::Frame(const Vec& position, const Quaternion& orientation) + : t_(position), q_(orientation), constraint_(NULL), referenceFrame_(NULL) +{} + +/*! Equal operator. + + The referenceFrame() and constraint() pointers are copied. + + \attention Signal and slot connections are not copied. */ +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; +} + +/*! Copy constructor. + + The translation() and rotation() as well as constraint() and referenceFrame() pointers are + copied. */ +Frame::Frame(const Frame& frame) +{ + (*this) = frame; +} + +/////////////////////////////// MATRICES ////////////////////////////////////// + +/*! Returns the 4x4 OpenGL transformation matrix represented by the Frame. + + This method should be used in conjunction with \c glMultMatrixd() to modify the OpenGL modelview + matrix from a Frame hierarchy. With this Frame hierarchy: + \code + Frame* body = new Frame(); + Frame* leftArm = new Frame(); + Frame* rightArm = new Frame(); + leftArm->setReferenceFrame(body); + rightArm->setReferenceFrame(body); + \endcode + + The associated OpenGL drawing code should look like: + \code + void Viewer::draw() + { + glPushMatrix(); + glMultMatrixd(body->matrix()); + drawBody(); + + glPushMatrix(); + glMultMatrixd(leftArm->matrix()); + drawArm(); + glPopMatrix(); + + glPushMatrix(); + glMultMatrixd(rightArm->matrix()); + drawArm(); + glPopMatrix(); + + glPopMatrix(); + } + \endcode + Note the use of nested \c glPushMatrix() and \c glPopMatrix() blocks to represent the frame hierarchy: \c + leftArm and \c rightArm are both correctly drawn with respect to the \c body coordinate system. + + This matrix only represents the local Frame transformation (i.e. with respect to the + referenceFrame()). Use worldMatrix() to get the full Frame transformation matrix (i.e. from the + world to the Frame coordinate system). These two match when the referenceFrame() is \c NULL. + + The result is only valid until the next call to matrix(), getMatrix(), worldMatrix() or + getWorldMatrix(). Use it immediately (as above) or use getMatrix() instead. + + \attention The OpenGL format of the result is the transpose of the actual mathematical European + representation (translation is on the last \e line instead of the last \e column). + + \note The scaling factor of the 4x4 matrix is 1.0. */ +const GLdouble* Frame::matrix() const +{ + static GLdouble m[4][4]; + getMatrix(m); + return (const GLdouble*)(m); +} + +/*! \c GLdouble[4][4] version of matrix(). See also getWorldMatrix() and matrix(). */ +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]; +} + +/*! \c GLdouble[16] version of matrix(). See also getWorldMatrix() and matrix(). */ +void Frame::getMatrix(GLdouble m[16]) const +{ + q_.getMatrix(m); + + m[12] = t_[0]; + m[13] = t_[1]; + m[14] = t_[2]; +} + +/*! Returns a Frame representing the inverse of the Frame space transformation. + + The rotation() of the new Frame is the Quaternion::inverse() of the original rotation. + Its translation() is the negated inverse rotated image of the original translation. + + If a Frame is considered as a space rigid transformation (translation and rotation), the inverse() + Frame performs the inverse transformation. + + Only the local Frame transformation (i.e. defined with respect to the referenceFrame()) is inverted. + Use worldInverse() for a global inverse. + + The resulting Frame has the same referenceFrame() as the Frame and a \c NULL constraint(). + + \note The scaling factor of the 4x4 matrix is 1.0. */ +Frame Frame::inverse() const +{ + Frame fr(-(q_.inverseRotate(t_)), q_.inverse()); + fr.setReferenceFrame(referenceFrame()); + return fr; +} + +/*! Returns the 4x4 OpenGL transformation matrix represented by the Frame. + + This method should be used in conjunction with \c glMultMatrixd() to modify + the OpenGL modelview matrix from a Frame: + \code + // The modelview here corresponds to the world coordinate system. + Frame fr(pos, Quaternion(from, to)); + glPushMatrix(); + glMultMatrixd(fr.worldMatrix()); + // draw object in the fr coordinate system. + glPopMatrix(); + \endcode + + This matrix represents the global Frame transformation: the entire referenceFrame() hierarchy is + taken into account to define the Frame transformation from the world coordinate system. Use + matrix() to get the local Frame transformation matrix (i.e. defined with respect to the + referenceFrame()). These two match when the referenceFrame() is \c NULL. + + The OpenGL format of the result is the transpose of the actual mathematical European + representation (translation is on the last \e line instead of the last \e column). + + \attention The result is only valid until the next call to matrix(), getMatrix(), worldMatrix() or + getWorldMatrix(). Use it immediately (as above) or use getWorldMatrix() instead. + + \note The scaling factor of the 4x4 matrix is 1.0. */ +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(); +} + +/*! float[4][4] parameter version of worldMatrix(). See also getMatrix() and 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]; +} + +/*! float[16] parameter version of worldMatrix(). See also getMatrix() and matrix(). */ +void Frame::getWorldMatrix(GLdouble m[16]) const +{ + const GLdouble* mat = worldMatrix(); + for (int i=0; i<16; ++i) + m[i] = mat[i]; +} + +/*! This is an overloaded method provided for convenience. Same as setFromMatrix(). */ +void Frame::setFromMatrix(const GLdouble m[4][4]) +{ + if (fabs(m[3][3]) < 1E-8) + { + cout << "Frame::setFromMatrix: Null homogeneous coefficient" << endl; + 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); +} + +/*! Sets the Frame from an OpenGL matrix representation (rotation in the upper left 3x3 matrix and + translation on the last line). + + Hence, if a code fragment looks like: + \code + GLdouble m[16]={...}; + glMultMatrixd(m); + \endcode + It is equivalent to write: + \code + Frame fr; + fr.setFromMatrix(m); + glMultMatrixd(fr.matrix()); + \endcode + + Using this conversion, you can benefit from the powerful Frame transformation methods to translate + points and vectors to and from the Frame coordinate system to any other Frame coordinate system + (including the world coordinate system). See coordinatesOf() and transformOf(). + + Emits the modified() signal. See also matrix(), getMatrix() and + Quaternion::setFromRotationMatrix(). + + \attention A Frame does not contain a scale factor. The possible scaling in \p m will not be + converted into the Frame by this method. */ +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); +} + +//////////////////// SET AND GET LOCAL TRANSLATION AND ROTATION /////////////////////////////// + + +/*! Same as setTranslation(), but with \p float parameters. */ +void Frame::setTranslation(float x, float y, float z) +{ + setTranslation(Vec(x, y, z)); +} + +/*! Fill \c x, \c y and \c z with the translation() of the Frame. */ +void Frame::getTranslation(float& x, float& y, float& z) const +{ + const Vec t = translation(); + x = t[0]; + y = t[1]; + z = t[2]; +} + +/*! Same as setRotation() but with \c float Quaternion parameters. */ +void Frame::setRotation(double q0, double q1, double q2, double q3) +{ + setRotation(Quaternion(q0, q1, q2, q3)); +} + +/*! The \p q are set to the rotation() of the Frame. + +See Quaternion::Quaternion(double, double, double, double) for details on \c q. */ +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]; +} + +//////////////////////////////////////////////////////////////////////////////// + +/*! Translates the Frame of \p t (defined in the Frame coordinate system). + + The translation actually applied to the Frame may differ from \p t since it can be filtered by the + constraint(). Use translate(Vec&) or setTranslationWithConstraint() to retrieve the filtered + translation value. Use setTranslation() to directly translate the Frame without taking the + constraint() into account. + + See also rotate(const Quaternion&). Emits the modified() signal. */ +void Frame::translate(const Vec& t) +{ + Vec tbis = t; + translate(tbis); +} + +/*! Same as translate(const Vec&) but \p t may be modified to satisfy the translation constraint(). + Its new value corresponds to the translation that has actually been applied to the Frame. */ +void Frame::translate(Vec& t) +{ + if (constraint()) + constraint()->constrainTranslation(t, this); + t_ += t; +} + +/*! Same as translate(const Vec&) but with \c float parameters. */ +void Frame::translate(float x, float y, float z) +{ + Vec t(x,y,z); + translate(t); +} + +/*! Same as translate(Vec&) but with \c float parameters. */ +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]; +} + +/*! Rotates the Frame by \p q (defined in the Frame coordinate system): R = R*q. + + The rotation actually applied to the Frame may differ from \p q since it can be filtered by the + constraint(). Use rotate(Quaternion&) or setRotationWithConstraint() to retrieve the filtered + rotation value. Use setRotation() to directly rotate the Frame without taking the constraint() + into account. + + See also translate(const Vec&). Emits the modified() signal. */ +void Frame::rotate(const Quaternion& q) +{ + Quaternion qbis = q; + rotate(qbis); +} + +/*! Same as rotate(const Quaternion&) but \p q may be modified to satisfy the rotation constraint(). + Its new value corresponds to the rotation that has actually been applied to the Frame. */ +void Frame::rotate(Quaternion& q) +{ + if (constraint()) + constraint()->constrainRotation(q, this); + q_ *= q; + q_.normalize(); // Prevents numerical drift +} + +/*! Same as rotate(Quaternion&) but with \c float Quaternion parameters. */ +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]; +} + +/*! Same as rotate(const Quaternion&) but with \c float Quaternion parameters. */ +void Frame::rotate(double q0, double q1, double q2, double q3) +{ + Quaternion q(q0,q1,q2,q3); + rotate(q); +} + +/*! Makes the Frame rotate() by \p rotation around \p point. + + \p point is defined in the world coordinate system, while the \p rotation axis is defined in the + Frame coordinate system. + + If the Frame has a constraint(), \p rotation is first constrained using + Constraint::constrainRotation(). The translation which results from the filtered rotation around + \p point is then computed and filtered using Constraint::constrainTranslation(). The new \p + rotation value corresponds to the rotation that has actually been applied to the Frame. + + Emits the modified() signal. */ +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; +} + +/*! Same as rotateAroundPoint(), but with a \c const \p rotation Quaternion. Note that the actual + rotation may differ since it can be filtered by the constraint(). */ +void Frame::rotateAroundPoint(const Quaternion& rotation, const Vec& point) +{ + Quaternion rot = rotation; + rotateAroundPoint(rot, point); +} + +//////////////////// SET AND GET WORLD POSITION AND ORIENTATION /////////////////////////////// + +/*! Sets the position() of the Frame, defined in the world coordinate system. Emits the modified() + signal. + +Use setTranslation() to define the \e local frame translation (with respect to the +referenceFrame()). The potential constraint() of the Frame is not taken into account, use +setPositionWithConstraint() instead. */ +void Frame::setPosition(const Vec& position) +{ + if (referenceFrame()) + setTranslation(referenceFrame()->coordinatesOf(position)); + else + setTranslation(position); +} + +/*! Same as setPosition(), but with \c float parameters. */ +void Frame::setPosition(float x, float y, float z) +{ + setPosition(Vec(x, y, z)); +} + +/*! Same as successive calls to setPosition() and then setOrientation(). + +Only one modified() signal is emitted, which is convenient if this signal is connected to a +QGLViewer::updateGL() slot. See also setTranslationAndRotation() and +setPositionAndOrientationWithConstraint(). */ +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; + } +} + + +/*! Same as successive calls to setTranslation() and then setRotation(). + +Only one modified() signal is emitted, which is convenient if this signal is connected to a +QGLViewer::updateGL() slot. See also setPositionAndOrientation() and +setTranslationAndRotationWithConstraint(). */ +void Frame::setTranslationAndRotation(const Vec& translation, const Quaternion& rotation) +{ + t_ = translation; + q_ = rotation; +} + + +/*! \p x, \p y and \p z are set to the position() of the Frame. */ +void Frame::getPosition(float& x, float& y, float& z) const +{ + Vec p = position(); + x = p.x; + y = p.y; + z = p.z; +} + +/*! Sets the orientation() of the Frame, defined in the world coordinate system. Emits the modified() signal. + +Use setRotation() to define the \e local frame rotation (with respect to the referenceFrame()). The +potential constraint() of the Frame is not taken into account, use setOrientationWithConstraint() +instead. */ +void Frame::setOrientation(const Quaternion& orientation) +{ + if (referenceFrame()) + setRotation(referenceFrame()->orientation().inverse() * orientation); + else + setRotation(orientation); +} + +/*! Same as setOrientation(), but with \c float parameters. */ +void Frame::setOrientation(double q0, double q1, double q2, double q3) +{ + setOrientation(Quaternion(q0, q1, q2, q3)); +} + +/*! Get the current orientation of the frame (same as orientation()). + Parameters are the orientation Quaternion values. + See also setOrientation(). */ + +/*! The \p q are set to the orientation() of the Frame. + +See Quaternion::Quaternion(double, double, double, double) for details on \c q. */ +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]; +} + +/*! Returns the orientation of the Frame, defined in the world coordinate system. See also + position(), setOrientation() and rotation(). */ +Quaternion Frame::orientation() const +{ + Quaternion res = rotation(); + const Frame* fr = referenceFrame(); + while (fr != NULL) + { + res = fr->rotation() * res; + fr = fr->referenceFrame(); + } + return res; +} + + +////////////////////// C o n s t r a i n t V e r s i o n s ////////////////////////// + +/*! Same as setTranslation(), but \p translation is modified so that the potential constraint() of the + Frame is satisfied. + + Emits the modified() signal. See also setRotationWithConstraint() and setPositionWithConstraint(). */ +void Frame::setTranslationWithConstraint(Vec& translation) +{ + Vec deltaT = translation - this->translation(); + if (constraint()) + constraint()->constrainTranslation(deltaT, this); + + setTranslation(this->translation() + deltaT); + translation = this->translation(); +} + +/*! Same as setRotation(), but \p rotation is modified so that the potential constraint() of the + Frame is satisfied. + + Emits the modified() signal. See also setTranslationWithConstraint() and setOrientationWithConstraint(). */ +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(); +} + +/*! Same as setTranslationAndRotation(), but \p translation and \p orientation are modified to + satisfy the constraint(). Emits the modified() signal. */ +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(); + +} + +/*! Same as setPosition(), but \p position is modified so that the potential constraint() of the + Frame is satisfied. See also setOrientationWithConstraint() and setTranslationWithConstraint(). */ +void Frame::setPositionWithConstraint(Vec& position) +{ + if (referenceFrame()) + position = referenceFrame()->coordinatesOf(position); + + setTranslationWithConstraint(position); +} + +/*! Same as setOrientation(), but \p orientation is modified so that the potential constraint() of the Frame + is satisfied. See also setPositionWithConstraint() and setRotationWithConstraint(). */ +void Frame::setOrientationWithConstraint(Quaternion& orientation) +{ + if (referenceFrame()) + orientation = referenceFrame()->orientation().inverse() * orientation; + + setRotationWithConstraint(orientation); +} + +/*! Same as setPositionAndOrientation() but \p position and \p orientation are modified to satisfy +the constraint. Emits the modified() signal. */ +void Frame::setPositionAndOrientationWithConstraint(Vec& position, Quaternion& orientation) +{ + if (referenceFrame()) + { + position = referenceFrame()->coordinatesOf(position); + orientation = referenceFrame()->orientation().inverse() * orientation; + } + setTranslationAndRotationWithConstraint(position, orientation); +} + + +///////////////////////////// REFERENCE FRAMES /////////////////////////////////////// + +/*! Sets the referenceFrame() of the Frame. + +The Frame translation() and rotation() are then defined in the referenceFrame() coordinate system. +Use position() and orientation() to express these in the world coordinate system. + +Emits the modified() signal if \p refFrame differs from the current referenceFrame(). + +Using this method, you can create a hierarchy of Frames. This hierarchy needs to be a tree, which +root is the world coordinate system (i.e. a \c NULL referenceFrame()). A warning is printed and no +action is performed if setting \p refFrame as the referenceFrame() would create a loop in the Frame +hierarchy (see settingAsReferenceFrameWillCreateALoop()). */ +void Frame::setReferenceFrame(const Frame* const refFrame) +{ + if (settingAsReferenceFrameWillCreateALoop(refFrame)) + cout << "Frame::setReferenceFrame would create a loop in Frame hierarchy" << endl; + else + { + bool identical = (referenceFrame_ == refFrame); + referenceFrame_ = refFrame; + } +} + +/*! Returns \c true if setting \p frame as the Frame's referenceFrame() would create a loop in the + Frame hierarchy. */ +bool Frame::settingAsReferenceFrameWillCreateALoop(const Frame* const frame) +{ + const Frame* f = frame; + while (f != NULL) + { + if (f == this) + return true; + f = f->referenceFrame(); + } + return false; +} + +///////////////////////// FRAME TRANSFORMATIONS OF 3D POINTS ////////////////////////////// + +/*! Returns the Frame coordinates of a point \p src defined in the world coordinate system (converts + from world to Frame). + + inverseCoordinatesOf() performs the inverse convertion. transformOf() converts 3D vectors instead + of 3D coordinates. + + See the <a href="../examples/frameTransform.html">frameTransform example</a> for an + illustration. */ +Vec Frame::coordinatesOf(const Vec& src) const +{ + if (referenceFrame()) + return localCoordinatesOf(referenceFrame()->coordinatesOf(src)); + else + return localCoordinatesOf(src); +} + +/*! Returns the world coordinates of the point whose position in the Frame coordinate system is \p + src (converts from Frame to world). + + coordinatesOf() performs the inverse convertion. Use inverseTransformOf() to transform 3D vectors + instead of 3D coordinates. */ +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; +} + +/*! Returns the Frame coordinates of a point \p src defined in the referenceFrame() coordinate + system (converts from referenceFrame() to Frame). + + localInverseCoordinatesOf() performs the inverse convertion. See also localTransformOf(). */ +Vec Frame::localCoordinatesOf(const Vec& src) const +{ + return rotation().inverseRotate(src - translation()); +} + +/*! Returns the referenceFrame() coordinates of a point \p src defined in the Frame coordinate + system (converts from Frame to referenceFrame()). + + localCoordinatesOf() performs the inverse convertion. See also localInverseTransformOf(). */ +Vec Frame::localInverseCoordinatesOf(const Vec& src) const +{ + return rotation().rotate(src) + translation(); +} + +/*! Returns the Frame coordinates of the point whose position in the \p from coordinate system is \p + src (converts from \p from to Frame). + + coordinatesOfIn() performs the inverse transformation. */ +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)); +} + +/*! Returns the \p in coordinates of the point whose position in the Frame coordinate system is \p + src (converts from Frame to \p in). + + coordinatesOfFrom() performs the inverse transformation. */ +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; +} + +////// float[3] versions + +/*! Same as coordinatesOf(), but with \c float parameters. */ +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]; +} + +/*! Same as inverseCoordinatesOf(), but with \c float parameters. */ +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]; +} + +/*! Same as localCoordinatesOf(), but with \c float parameters. */ +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]; +} + + /*! Same as localInverseCoordinatesOf(), but with \c float parameters. */ +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]; +} + +/*! Same as coordinatesOfIn(), but with \c float parameters. */ +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]; +} + +/*! Same as coordinatesOfFrom(), but with \c float parameters. */ +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]; +} + + +///////////////////////// FRAME TRANSFORMATIONS OF VECTORS ////////////////////////////// + +/*! Returns the Frame transform of a vector \p src defined in the world coordinate system (converts + vectors from world to Frame). + + inverseTransformOf() performs the inverse transformation. coordinatesOf() converts 3D coordinates + instead of 3D vectors (here only the rotational part of the transformation is taken into account). + + See the <a href="../examples/frameTransform.html">frameTransform example</a> for an + illustration. */ +Vec Frame::transformOf(const Vec& src) const +{ + if (referenceFrame()) + return localTransformOf(referenceFrame()->transformOf(src)); + else + return localTransformOf(src); +} + +/*! Returns the world transform of the vector whose coordinates in the Frame coordinate + system is \p src (converts vectors from Frame to world). + + transformOf() performs the inverse transformation. Use inverseCoordinatesOf() to transform 3D + coordinates instead of 3D vectors. */ +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; +} + +/*! Returns the Frame transform of a vector \p src defined in the referenceFrame() coordinate system + (converts vectors from referenceFrame() to Frame). + + localInverseTransformOf() performs the inverse transformation. See also localCoordinatesOf(). */ +Vec Frame::localTransformOf(const Vec& src) const +{ + return rotation().inverseRotate(src); +} + +/*! Returns the referenceFrame() transform of a vector \p src defined in the Frame coordinate + system (converts vectors from Frame to referenceFrame()). + + localTransformOf() performs the inverse transformation. See also localInverseCoordinatesOf(). */ +Vec Frame::localInverseTransformOf(const Vec& src) const +{ + return rotation().rotate(src); +} + +/*! Returns the Frame transform of the vector whose coordinates in the \p from coordinate system is \p + src (converts vectors from \p from to Frame). + + transformOfIn() performs the inverse transformation. */ +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)); +} + +/*! Returns the \p in transform of the vector whose coordinates in the Frame coordinate system is \p + src (converts vectors from Frame to \p in). + + transformOfFrom() performs the inverse transformation. */ +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; +} + +///////////////// float[3] versions ////////////////////// + +/*! Same as transformOf(), but with \c float parameters. */ +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]; +} + +/*! Same as inverseTransformOf(), but with \c float parameters. */ +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]; +} + +/*! Same as localTransformOf(), but with \c float parameters. */ +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]; +} + +/*! Same as localInverseTransformOf(), but with \c float parameters. */ +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]; +} + +/*! Same as transformOfIn(), but with \c float parameters. */ +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]; +} + +/*! Same as transformOfFrom(), but with \c float parameters. */ +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]; +} + +///////////////////////////////// ALIGN ///////////////////////////////// + +/*! Aligns the Frame with \p frame, so that two of their axis are parallel. + +If one of the X, Y and Z axis of the Frame is almost parallel to any of the X, Y, or Z axis of \p +frame, the Frame is rotated so that these two axis actually become parallel. + +If, after this first rotation, two other axis are also almost parallel, a second alignment is +performed. The two frames then have identical orientations, up to 90 degrees rotations. + +\p threshold measures how close two axis must be to be considered parallel. It is compared with the +absolute values of the dot product of the normalized axis. + +When \p move is set to \c true, the Frame position() is also affected by the alignment. The new +Frame position() is such that the \p frame position (computed with coordinatesOf(), in the Frame +coordinates system) does not change. + +\p frame may be \c NULL and then represents the world coordinate system (same convention than for +the referenceFrame()). + +The rotation (and translation when \p move is \c true) applied to the Frame are filtered by the +possible constraint(). */ +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()); + } +} + +/*! Translates the Frame so that its position() lies on the line defined by \p origin and \p + direction (defined in the world coordinate system). + +Simply uses an orthogonal projection. \p direction does not need to be normalized. */ +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); +} |