div class="twikiTopBar">

Chair for Computer Aided Medical Procedures & Augmented Reality
Lehrstuhl für Informatikanwendungen in der Medizin & Augmented Reality

dwarfutil.h

Go to the documentation of this file.
00001 /**
00002  * @file dwarfutil.h
00003  *
00004  * Utility classes and functions to simplify working with the 
00005  * DWARF framework under C++. Mainly conversion routines between different
00006  * representations of rotations (axis/angle, quaternions, rotation matrices,
00007  * euler angles)
00008  *
00009  * @author Martin Wagner
00010  */
00011 
00012 /*
00013  * Distributed Wearable Augmented Reality Framework - www.augmentedreality.de
00014  * $Id: dwarfutil.h,v 1.25 2004/12/09 00:53:15 siggelko Exp $
00015  * $Revision: 1.25 $
00016  */
00017 
00018 #ifndef __DWARFUTIL_H_
00019 #define __DWARFUTIL_H_
00020 
00021 #include <DWARF/DwarfCommon.h>
00022 
00023 
00024 namespace DWARF {
00025   namespace Util {
00026 
00027                 /**
00028                  * @brief Get the current system time as a DWARF::Time struct
00029                  *
00030                  * @return: the current system time
00031                  */
00032                 Time currentTime();
00033 
00034                 /**
00035                  * @brief Get the difference to the current time
00036                  *
00037                  * @param compareTime the time to be be compared
00038                  */
00039                 Time getTimeToNow(Time &compareTime);
00040 
00041                 /*
00042                  * Some constants and methods for accessing quaternion components
00043                  */
00044                 //! @brief Use only this constant for accessing a quaternion's X component
00045                 const unsigned char QX = 0;
00046                 //! @brief Use only this constant for accessing a quaternion's Y component
00047                 const unsigned char QY = 1;
00048                 //! @brief Use only this constant for accessing a quaternion's Z component
00049                 const unsigned char QZ = 2;
00050                 //! @brief Use only this constant for accessing a quaternion's scalar component
00051                 const unsigned char QW = 3;
00052                 //! @brief Convenience method for accessing a quaternion's X component
00053                 inline double q_x( double *q) { return q[QX]; }
00054                 //! @brief Convenience method for accessing a quaternion's y component
00055                 inline double q_y( double *q) { return q[QY]; }
00056                 //! @brief Convenience method for accessing a quaternion's z component
00057                 inline double q_z( double *q) { return q[QZ]; }
00058                 //! @brief Convenience method for accessing a quaternion's scalar component
00059                 inline double q_w( double *q) { return q[QW]; }
00060 
00061                 /**
00062                  * @brief Construct rotation matrix from unit quaternion.
00063                  *
00064                  * Assumes matrix is used to multiply column vector on the left:
00065                  * vnew = mat vold. Works correctly for right-handed coordinate system
00066                  * and right-handed rotations. 
00067                  *
00068                  * @param quat 4-dim unit quaternion as input
00069                  * @param matrix 4x4-dim double homogeneous rotation matrix
00070                  *                as return value, organized as follows (row major)<br>
00071                  *                <b>NOTE: OpenGL has another order, column major!</b><br>
00072                  * <CODE>           ( 0  1  2  3 )<br>
00073                  *                  ( 4  5  6  7 )<br>
00074                  *                  ( 8  9 10 11 )<br>
00075                  *                  (12 13 14 15 )
00076                  * </CODE>
00077                  * @return pointer to matrix
00078                  */
00079                 double *quaternionToMatrix( const double *quat, double *matrix);
00080 
00081                 /**
00082                  * @brief Construct a unit quaternion from a rotation matrix. 
00083                  * 
00084                  * Assumes matrix is 
00085                  * used to multiply column vector on the left: vnew = mat vold. Works 
00086                  * correctly for right-handed coordinate system and right-handed rotations. 
00087                  * Translation components are ignored. Matrix is assumed to be orthonormal.
00088                  *
00089                  * @param matrix 4x4-dim double homogeneous matrix
00090                  *                as input, organized as follows (row major)<br>
00091                  *                <b>NOTE: OpenGL has another order, column major!</b><br>
00092                  * <CODE>           ( 0  1  2  3 )<br>
00093                  *                  ( 4  5  6  7 )<br>
00094                  *                  ( 8  9 10 11 )<br>
00095                  *                  (12 13 14 15 )
00096                  * </CODE>
00097                  * @param quat 4-dim normalized quaternion as return value
00098                  * @return pointer to quat
00099                  */ 
00100                 double *matrixToQuaternion( const double *matrix, double *quat );
00101 
00102 
00103                 /**
00104                  * @brief Converts axis/angle representation (as used in VRML) to a unit quaternion
00105                  * 
00106                  * @param axisAngle 4-dim double axis/angle rotation (x,y,z,alpha) as input
00107                  * @param quat 4-dim unit quaternion as return value
00108                  * @return a pointer to quat
00109                  */
00110                 double* axisAngleToQuaternion( double* axisAngle, double* quat );
00111                 inline double* vrmlToQuaternion(double* vrml, double* q) {
00112                         return axisAngleToQuaternion(vrml, q);
00113                 }
00114 
00115                 /**
00116                  * @brief Converts a unit quaternion to axis/angle representation (as used in VRML)
00117                  *
00118                  * @param quat 4-dim unit quaternion as input
00119                  * @param axisAngle 4-dim double axis/angle rotation (x,y,z,alpha)
00120                  *              as return value
00121                  * @return a pointer to axisAngle
00122                  */
00123                 double* quaternionToAxisAngle( double* quat, double* axisAngle );
00124                 inline double* quaternionToVrml(double* q, double* vrml) {
00125                         return quaternionToAxisAngle(q, vrml);
00126                 }
00127 
00128                 /**
00129                  * @brief Inverts a quaternion.
00130                  *
00131                  * The resulting quaternion is the inverse of the given quaternion. This
00132                  * corresponds to translating an orthonormal rotation matrix.
00133                  * Note: To invert a unit quaternion, conjugating it suffices.
00134                  *
00135                  * @param quat 4-dim quaternion as input and return value
00136                  * @return a pointer to quat
00137                  */
00138                 double* invertQuaternion( double *quat );
00139 
00140                 /**
00141                  * @brief Conjugates a quaternion (i.e. negates its vector part)
00142                  *
00143                  * Note: Conjugating a unit quaternion results in its inverse.
00144                  *
00145                  * @param quat 4-dim quaternion as input and return value
00146                  * @return a pointer to quat
00147                  */
00148                 double* conjugateQuaternion( double *quat );
00149 
00150                 /**
00151                  * @brief Normalizes a quaternion (i.e. makes a unit quaternion)
00152                  *
00153                  * @param quat 4-dim quaternion as input and return value
00154                  * @return a pointer to quat
00155                  */
00156                 double* normalizeQuaternion( double *quat );
00157 
00158                 /**
00159                  * @brief Normalizes a vector
00160                  *
00161                  * @param vec n-dim vector as input and return value
00162                  * @param length dimension of vector
00163                  * @return a pointer to vec
00164                  */
00165                 double* normalizeVector( double *vec, int length );
00166 
00167                 /**
00168                  * multiplyQuaternion
00169                  * Calculates <CODE> quat := quat1 * quat2 </CODE>
00170                  *
00171                  * @param quat 4-dim Quaternion as return value
00172                  * @param quat1 4-dim Quaternion as input
00173                  * @param quat2 4-dim Quaternion as input
00174                  * @return a pointer to quat
00175                  */
00176                 double* multiplyQuaternion( double *quat, const double *quat1, const double *quat2 );
00177 
00178                 /**
00179                  * @brief Rotates a 3-dim point by a unit quaternion
00180                  *
00181                  * Calculates <CODE> rotPoint = quat * (point,0) * quat^-1 </CODE>
00182                  *
00183                  * @param quat 4-dim unit quaternion representing the rotation
00184                  * @param point 3-dim point to be rotated as input
00185                  * @param rotPoint memory for return value (at least 3*double)
00186                  * @return a pointer to rotPoint
00187                  */
00188                 double* rotateQuaternion( double *rotPoint, const double *quat, const double *point );
00189 
00190                 /**
00191                  * @brief Transforms the coordinate reference frame of a 6D pose.
00192                  *
00193                  * Use this method to transform the reference frame of a 6DOF pose.
00194                  * If given the transformation pose P' and an input pose P, it computes
00195                  * P'^-1 * P * P'
00196                  *
00197                  * @param transRot rotational part of transformation P', 4-double unit quaternion
00198                  * @param transPos positional part of transformation P', 3-double position
00199                  * @param rot rotational part of input pose P, is modified, 4-double unit quaternion
00200                  * @param pos positional part of input pose P, is modified, 3-double position
00201                  */
00202                 void transformCoordinateFrame( const double *transRot, const double *transPos,
00203                         double *rot, double *pos);
00204 
00205                 /**
00206                  * @brief Rotates a 3-dim point by a orthonormal rotation matrix
00207                  *
00208                  * Calculates <CODE> rotPoint = matrix * (point,1) </CODE>
00209                  *
00210                  * @param quat 3x4-dim row major orthonormal matrix representing the rotation, will not be changed
00211                  * @param point 3-dim point to be rotated as input
00212                  * @param rotPoint memory for return value (at least 3*double)
00213                  * @return a pointer to rotPoint
00214                  */
00215                 double* rotateMatrix( double *rotPoint, const double *matrix, const double *point );
00216 
00217                 /**
00218                  * @brief (self) inverts a matrix
00219                  *
00220                  * Calculates <CODE> matrix = matrix^-1 </CODE>
00221                  *
00222                  * @param matrix the matix that should be inverted
00223                  * @param col count of the colums in the matix
00224                  * @param row count of the rows in the matix
00225                  * @return a pointer to the inverted matix
00226                  */
00227                 double* invertMatrix( double *matrix, int col, int row );
00228 
00229 
00230                 /**
00231                  * @brief Compute cross product p = a x b of two 3D-vectors
00232                  *
00233                  * @param p 3-dim pointer to return value memory
00234                  * @param p 3-dim vector a
00235                  * @param p 3-dim vector b
00236                  *
00237                  * @return a pointer to p = a x b
00238                  */
00239                 double* crossProduct3D( double *p, double *a, double *b );
00240 
00241                 /**
00242                  * @brief Compute scalar product p = a . b of two 3D-vectors
00243                  *
00244                  * @param a 3-dim vector a
00245                  * @param b 3-dim vector b
00246                  *
00247                  * @return the scalar value p = a . b
00248                  */
00249                 double scalarProduct3D(  double *a, double *b );
00250                 
00251                 /**
00252                  * @brief calculates the angle between two 3D-vectors
00253                  *
00254                  * @param a 3-dim vector a
00255                  * @param b 3-dim vector b
00256                  *
00257                  * @return the angle
00258                  */
00259                 double angleBetweenVectors3D(  double *a, double *b );
00260 
00261                 /**
00262                  * @brief Inverts a pose transformation.
00263                  *
00264                  * Identical to inverting a homogeneous 4x4 matrix, computes
00265                  * rot_new = rot_old^-1; t_new = - rot_old^-1 * t_old
00266                  *
00267                  * This function computes the right inverse, i.e. H^-1 such that
00268                  * H* H^-1 = I
00269                  *
00270                  * @param newQuat 4-dim unit quaternion, new rotation (return value)
00271                  * @param newTrans 3-dim pont, new translation (return value)
00272                  * @param oldQuat 4-dim unit quaternion, old rotation
00273                  * @param oldTrans 3-dim point, old translation
00274                  */
00275                 void poseRightInverse( double* newQuat, double* newTrans,
00276                         const double* oldQuat, const double* oldTrans );
00277                 inline void invertQuaternionPose( double* newQuat, double* newTrans,
00278                         const double* oldQuat, const double* oldTrans ) {
00279                         return poseRightInverse(newQuat, newTrans, oldQuat, oldTrans);
00280                 }
00281 
00282                 /**
00283                  * @brief Inverts a pose transformation.
00284                  *
00285                  * Identical to inverting a homogeneous 4x4 matrix, computes
00286                  * rot_new = rot_old^-1; t_new = - rot_old * t_old
00287                  *
00288                  * This function computes the left inverse, i.e. H^-1 such that
00289                  * H^-1 * H = I
00290                  *
00291                  * @param newQuat 4-dim unit quaternion, new rotation (return value)
00292                  * @param newTrans 3-dim pont, new translation (return value)
00293                  * @param oldQuat 4-dim unit quaternion, old rotation
00294                  * @param oldTrans 3-dim point, old translation
00295                  */
00296                 void poseLeftInverse( double* newQuat, double* newTrans,
00297                         const double* oldQuat, const double* oldTrans );
00298 
00299                 /**
00300                  * @brief Contructs Euler rotation representation out of matrix
00301                  *
00302                  * This code was partially taken from OpenTracker, TU Vienna
00303                  * http://studierstube.org/opentracker/
00304                  * originally, it seems to be from Gary Bishop
00305                  *
00306                  * FIXME: This code is supposed to take a column matrix. Ask
00307                  *        Gerhard Reitmayr whether that is the same as the
00308                  *        representation below or not. Adjust comment if
00309                  *        necessary. DO NOT change the code in here, it is
00310                  *        for the ARTkPoseReconstruction service.
00311                  *
00312                  * @param colMatrix: 4x4-dim double homogeneous matrix (row x column)
00313                  *                   as input, organized as follows<br>
00314                  * <CODE>           ( 0  1  2  3 )<br>
00315                  *                  ( 4  5  6  7 )<br>
00316                  *                  ( 8  9 10 11 )<br>
00317                  *                  (12 13 14 15 )
00318                  * </CODE>
00319                  * @param angles 3-dim angles (roll, pitch, yaw) as return value
00320                  * @return pointer to angles
00321                  */
00322                 double* colMatrixToEuler( double* colMatrix, double* angles );
00323 
00324                 /**
00325                  * @brief Constructs quaternion from Euler representation
00326                  *
00327                  * This code was partially taken from OpenTracker, TU Vienna
00328                  * http://studierstube.org/opentracker/
00329                  *
00330                  * @param angles 3-dim angles (roll, pitch, yaw)
00331                  * @param quat 4-dim normalized quaternion as return value
00332                  * @return pointer to quat
00333                  */
00334                 double* eulerToQuaternion( double* angles, double* quat );
00335 
00336                 /**
00337                  * @brief Interpolates quaternions using the SLERP algorithm
00338                  *
00339                  * @param quat destination quaternion
00340                  * @param quat1 first source quaternion
00341                  * @param quat2 second source quaternion
00342                  * @param h interpolation factor. 0.0=quat1, 1.0=quat2. May exceed [0,1] for extrapolation.
00343                  * @return pointer to quat
00344                  */
00345                 double* quaternionSlerp( double* quat, const double* quat1, const double* quat2, double h );
00346 
00347                 /**
00348                  * @brief Negates a quaternion if the result is closer to a given reference.
00349                  *
00350                  * Every rotation can be represented by two different quaternions, which
00351                  * can cause great trouble when analyzing rotation sequences. One can
00352                  * switch between the two representation by negating all four quaternion
00353                  * entries. This routine negates the quaternion if the result is closer to
00354                  * a given reference quaternion, e.g. a preceding quaternion in a sequence.
00355                  *
00356                  * @param quat Quaternion to be negated
00357                  * @param otherquat Reference quaternion
00358                  * @return pointer to quat
00359                  */
00360                 double* quaternionNegateIfCloser( double* quat, const double* otherquat );
00361 
00362                 /**
00363                  * @brief Negates a quaternion.
00364                  *
00365                  * Sets x = -x; y=-y; z=-z; w=-w
00366                  * The resulting quaternion still represents the same rotation.
00367                  * @param quat Quaternion to be negated
00368                  @return pointer to quat
00369                  */
00370                 double* negateQuaternion( double *quat );
00371   };
00372 };
00373 
00374 
00375 
00376 #endif

dwarfutil.h Source File | generated on Sun Apr 29 02:00:58 2007 by Doxygen 1.4.1 for DWARF