OpenTracker

An Open Architecture for Reconfigurable Tracking based on XML | Contact

MathUtils.h

Go to the documentation of this file.
00001 /* ========================================================================
00002  * Copyright (c) 2006,
00003  * Institute for Computer Graphics and Vision
00004  * Graz University of Technology
00005  * All rights reserved.
00006  *
00007  * Redistribution and use in source and binary forms, with or without
00008  * modification, are permitted provided that the following conditions are
00009  * met:
00010  *
00011  * Redistributions of source code must retain the above copyright notice,
00012  * this list of conditions and the following disclaimer.
00013  *
00014  * Redistributions in binary form must reproduce the above copyright
00015  * notice, this list of conditions and the following disclaimer in the
00016  * documentation and/or other materials provided with the distribution.
00017  *
00018  * Neither the name of the Graz University of Technology nor the names of
00019  * its contributors may be used to endorse or promote products derived from
00020  * this software without specific prior written permission.
00021  *
00022  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
00023  * IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
00024  * TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
00025  * PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER
00026  * OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
00027  * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
00028  * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
00029  * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
00030  * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
00031  * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
00032  * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
00033  * ========================================================================
00034  * PROJECT: OpenTracker
00035  * ======================================================================== */
00042 /* ======================================================================= */
00043 
00044 #ifndef _MATHUTILS_H
00045 #define _MATHUTILS_H
00046 
00047 #include <vector>
00048 
00049 #include "../dllinclude.h"
00050 
00051 /* for accessing the elements of Quaternion and Vector3 */
00052 #define Q_X    0
00053 #define Q_Y    1
00054 #define Q_Z    2
00055 #define Q_W    3
00056 
00067 namespace ot {
00068 
00069     class OPENTRACKER_API MathUtils
00070     {
00071     public:
00073         typedef double Matrix3x3[3][3];
00075         typedef double Matrix4x4[4][4];
00077         typedef double Vector3[3];
00079         typedef double Quaternion[4];
00080 
00082         enum EULER {
00083             // the encoding is as follows :
00084             // the two lsb bits encode the last axis
00085             // the next two the middle axis
00086             // and the next two the first axis
00087             // X = 0, Y = 1, Z = 2
00088             // the enums are ordered by integer value
00089             XYX =  0 + 4 + 0,
00090             XYZ =  0 + 4 + 2,
00091             XZX =  0 + 8 + 0,
00092             XZY =  0 + 8 + 1,
00093             YXY = 16 + 0 + 1,
00094             YXZ = 16 + 0 + 2,
00095             YZX = 16 + 8 + 0,
00096             YZY = 16 + 8 + 1,
00097             ZXY = 32 + 0 + 1,
00098             ZXZ = 32 + 0 + 2,
00099             ZYX = 32 + 4 + 0,
00100             ZYZ = 32 + 4 + 2
00101         };
00102 
00109         static float* axisAngleToQuaternion(const float* axisa, float* qResult);
00116         static std::vector<float>& axisAngleToQuaternion(const std::vector<float> &axisa, std::vector<float> &qResult);
00123         static double* axisAngleToQuaternion(const double* axisa, Quaternion qResult);
00131         static float* eulerToQuaternion(const float roll, const float pitch, const float yaw, float* qResult);
00139         static std::vector<float>& eulerToQuaternion(const float roll, const float pitch, const float yaw, std::vector<float> &qResult);
00150         static float * eulerToQuaternion( const double alpha, const double beta,
00151                                           const double gamma, const enum MathUtils::EULER sequence,
00152                                           float * qResult );
00159         static float* invertQuaternion(const float* q, float* qResult);
00166         static std::vector<float>& invertQuaternion(const std::vector<float> &q, std::vector<float> &qResult);
00171         static float* matrixToQuaternion(const float matrix[3][3], float* qResult);
00176         static std::vector<float>& matrixToQuaternion(const float matrix[3][3], std::vector<float> &qResult);
00184         static float* multiplyQuaternion(const float* q1, const float* q2, float* qResult);
00192         static std::vector<float>& multiplyQuaternion(const std::vector<float> &q1, const std::vector<float> &q2, std::vector<float> &qResult);
00200         static double* multiplyQuaternion(const Quaternion q1, const Quaternion q2, Quaternion qResult);
00205         static float* normalizeQuaternion(float* q);
00210         static std::vector<float>& normalizeQuaternion(std::vector<float> &q);
00215         static double* normalizeQuaternion(Quaternion q);
00224         static float* rotateVector(const float* q, const float* v, float* vResult);
00233         static std::vector<float>& rotateVector(const std::vector<float> &q, const std::vector<float> &v, std::vector<float> &vResult);
00234 
00238         static float determinant( const float matrix[3][3] );
00239 
00246         static float* quaternionToAxisAngle(const float *q, float *axisa);
00247 
00255         static double angle( const float * v1, const float * v2, const int dim );
00263         static double angle( const std::vector<float> &v1, const std::vector<float> & v2, const int dim );
00271         static double dot( const float * v1, const float * v2, const int dim );
00279         static double dot( const std::vector<float> &v1, const std::vector<float> &v2, const int dim );
00293         static float * slerp( const float * q1, const float *q2, const float t, float * qResult );
00307         static std::vector<float>& slerp( const std::vector<float> &q1, const std::vector<float> &q2, const float t, std::vector<float> &qResult );
00308 
00309         static void matrixMultiply(const Matrix4x4 m1, const Matrix4x4 m2, Matrix4x4 &m);
00310         static void matrixMultiply(const Matrix3x3 m1, const Matrix3x3 m2, Matrix3x3 &m);
00311 
00313         static const Matrix4x4 matrix4x4_flipY;
00315         static const Matrix4x4 matrix4x4_identity;
00316 
00317         /*****************************************************************************
00318          * MatrixToEuler - convert a column matrix to euler angles
00319          *
00320          *  input:
00321          *  - vector to hold euler angles
00322          *  - src column matrix
00323          *  Vector3     angles    :      Holds outgoing roll, pitch, yaw
00324          *  Matrix4x4   colMatrix :      Holds incoming rotation
00325          *
00326          *  output:
00327          *  - euler angles in radians in the range -pi to pi;
00328          *  vec[0] = yaw, vec[1] = pitch, vec[2] = roll
00329          *  yaw is rotation about Z axis, pitch is about Y, roll -> X rot.
00330          *
00331          * notes:
00332          *  - originally written by Gary Bishop
00333          *
00334          *****************************************************************************/
00335         static void MatrixToEuler(Vector3 angles, const Matrix4x4 colMatrix);
00336 
00338         static const double Pi;
00339 
00341         static const double E;
00342 
00344         static const double GradToRad;
00345     };
00346 
00347 } // namespace ot
00348 
00349 #endif
00350 
00351 /* 
00352  * ------------------------------------------------------------
00353  *   End of MathUtils.h
00354  * ------------------------------------------------------------
00355  *   Automatic Emacs configuration follows.
00356  *   Local Variables:
00357  *   mode:c++
00358  *   c-basic-offset: 4
00359  *   eval: (c-set-offset 'substatement-open 0)
00360  *   eval: (c-set-offset 'case-label '+)
00361  *   eval: (c-set-offset 'statement 'c-lineup-runin-statements)
00362  *   eval: (setq indent-tabs-mode nil)
00363  *   End:
00364  * ------------------------------------------------------------ 
00365  */

copyright (c) 2006 Graz University of Technology