MathUtils.h

Go to the documentation of this file.

/* ========================================================================
 * Copyright (c) 2006,
 * Institute for Computer Graphics and Vision
 * Graz University of Technology
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are
 * met:
 *
 * Redistributions of source code must retain the above copyright notice,
 * this list of conditions and the following disclaimer.
 *
 * Redistributions in binary form must reproduce the above copyright
 * notice, this list of conditions and the following disclaimer in the
 * documentation and/or other materials provided with the distribution.
 *
 * Neither the name of the Graz University of Technology nor the names of
 * its contributors may be used to endorse or promote products derived from
 * this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
 * IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
 * TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
 * PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER
 * OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
 * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
 * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
 * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
 * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
 * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
 * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 * ========================================================================
 * PROJECT: OpenTracker
 * ======================================================================== */
/* ======================================================================= */

#ifndef _MATHUTILS_H
#define _MATHUTILS_H

#include <vector>

#include "../dllinclude.h"

/* for accessing the elements of Quaternion and Vector3 */
#define Q_X    0
#define Q_Y    1
#define Q_Z    2
#define Q_W    3

namespace ot {

    class OPENTRACKER_API MathUtils
    {
    public:
        typedef double Matrix3x3[3][3];
        typedef double Matrix4x4[4][4];
        typedef double Vector3[3];
        typedef double Quaternion[4];

        enum EULER {
            // the encoding is as follows :
            // the two lsb bits encode the last axis
            // the next two the middle axis
            // and the next two the first axis
            // X = 0, Y = 1, Z = 2
            // the enums are ordered by integer value
            XYX =  0 + 4 + 0,
            XYZ =  0 + 4 + 2,
            XZX =  0 + 8 + 0,
            XZY =  0 + 8 + 1,
            YXY = 16 + 0 + 1,
            YXZ = 16 + 0 + 2,
            YZX = 16 + 8 + 0,
            YZY = 16 + 8 + 1,
            ZXY = 32 + 0 + 1,
            ZXZ = 32 + 0 + 2,
            ZYX = 32 + 4 + 0,
            ZYZ = 32 + 4 + 2
        };

        static float* axisAngleToQuaternion(const float* axisa, float* qResult);
        static std::vector<float>& axisAngleToQuaternion(const std::vector<float> &axisa, std::vector<float> &qResult);
        static double* axisAngleToQuaternion(const double* axisa, Quaternion qResult);
        static float* eulerToQuaternion(const float roll, const float pitch, const float yaw, float* qResult);
        static std::vector<float>& eulerToQuaternion(const float roll, const float pitch, const float yaw, std::vector<float> &qResult);
        static float * eulerToQuaternion( const double alpha, const double beta,
                                          const double gamma, const enum MathUtils::EULER sequence,
                                          float * qResult );
        static float* invertQuaternion(const float* q, float* qResult);
        static std::vector<float>& invertQuaternion(const std::vector<float> &q, std::vector<float> &qResult);
        static float* matrixToQuaternion(const float matrix[3][3], float* qResult);
        static std::vector<float>& matrixToQuaternion(const float matrix[3][3], std::vector<float> &qResult);
        static float* multiplyQuaternion(const float* q1, const float* q2, float* qResult);
        static std::vector<float>& multiplyQuaternion(const std::vector<float> &q1, const std::vector<float> &q2, std::vector<float> &qResult);
        static double* multiplyQuaternion(const Quaternion q1, const Quaternion q2, Quaternion qResult);
        static float* normalizeQuaternion(float* q);
        static std::vector<float>& normalizeQuaternion(std::vector<float> &q);
        static double* normalizeQuaternion(Quaternion q);
        static float* rotateVector(const float* q, const float* v, float* vResult);
        static std::vector<float>& rotateVector(const std::vector<float> &q, const std::vector<float> &v, std::vector<float> &vResult);

        static float determinant( const float matrix[3][3] );

        static float* quaternionToAxisAngle(const float *q, float *axisa);

        static void  quaternionToAxisAngle(std::vector<float> &q, std::vector<float> &axisa);


        void translationQuaternionTo4x4Matrix(std::vector<float>& pos, std::vector<float>& quat, MathUtils::Matrix4x4& mat);

        static double angle( const float * v1, const float * v2, const int dim );
        static double angle( const std::vector<float> &v1, const std::vector<float> & v2, const int dim );
        static double dot( const float * v1, const float * v2, const int dim );
        static double dot( const std::vector<float> &v1, const std::vector<float> &v2, const int dim );
        static float * slerp( const float * q1, const float *q2, const float t, float * qResult );
        static std::vector<float>& slerp( const std::vector<float> &q1, const std::vector<float> &q2, const float t, std::vector<float> &qResult );

        static void quaternionToMatrix( Quaternion &q, Matrix4x4 &m);
        static void matrixToQuaternion( Matrix4x4& matrix, Quaternion& qResult);
        
        static void matrixMultiply(const Matrix4x4 m1, const Matrix4x4 m2, Matrix4x4 &m);
        static void matrixMultiply(const Matrix3x3 m1, const Matrix3x3 m2, Matrix3x3 &m);

        static const Matrix4x4 matrix4x4_flipY;
        static const Matrix4x4 matrix4x4_identity;

        /*****************************************************************************
         * MatrixToEuler - convert a column matrix to euler angles
         *
         *  input:
         *      - vector to hold euler angles
         *  - src column matrix
         *  Vector3     angles    :      Holds outgoing roll, pitch, yaw
         *  Matrix4x4   colMatrix :      Holds incoming rotation
         *
         *  output:
         *  - euler angles in radians in the range -pi to pi;
         *  vec[0] = yaw, vec[1] = pitch, vec[2] = roll
         *  yaw is rotation about Z axis, pitch is about Y, roll -> X rot.
         *
         * notes:
         *      - originally written by Gary Bishop
         *
         *****************************************************************************/
        static void MatrixToEuler(Vector3 angles, const Matrix4x4 colMatrix);

        static const double Pi;

        static const double E;

        static const double GradToRad;
    };

} // namespace ot

#endif

/* 
 * ------------------------------------------------------------
 *   End of MathUtils.h
 * ------------------------------------------------------------
 *   Automatic Emacs configuration follows.
 *   Local Variables:
 *   mode:c++
 *   c-basic-offset: 4
 *   eval: (c-set-offset 'substatement-open 0)
 *   eval: (c-set-offset 'case-label '+)
 *   eval: (c-set-offset 'statement 'c-lineup-runin-statements)
 *   eval: (setq indent-tabs-mode nil)
 *   End:
 * ------------------------------------------------------------ 
 */

---