OpenTracker

An Open Architecture for Reconfigurable Tracking based on XML | Contact

MathUtils.cxx

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 #include "MathUtils.h"
00045 #include "Event.h"
00046 #include <math.h>
00047 #include <stdio.h>
00048 #include <assert.h>
00049 
00050 /* tolerance for quaternion operations */
00051 const double Q_EPSILON = (1e-10);
00052 
00063 namespace ot {
00064 
00065     template <typename T> class MathStuff {
00066 
00067     public:
00068         static T sin_over_x( T x ) {
00069             if ((T)1. + x*x == (T)1.)
00070                 return (T)1.;
00071             else
00072                 return sin(x)/x;
00073         };
00074     };
00075 
00076     typedef MathStuff<double> MD;
00077     typedef MathStuff<float> MF;
00078 
00079     const double MathUtils::Pi = 3.1415926535897932385;
00080 
00081     const double MathUtils::E = 2.7182818284590452354;
00082 
00083     const double MathUtils::GradToRad = MathUtils::Pi / 180.0;
00084 
00085     const MathUtils::Matrix4x4 MathUtils::matrix4x4_flipY = {{1, 0, 0, 0},
00086                                                              {0,-1, 0, 0},
00087                                                              {0, 0, 1, 0},
00088                                                              {0, 0, 0, 1}};
00089 
00090     const MathUtils::Matrix4x4 MathUtils::matrix4x4_identity = {{1, 0, 0, 0},
00091                                                                 {0, 1, 0, 0},
00092                                                                 {0, 0, 1, 0},
00093                                                                 {0, 0, 0, 1}};
00094 
00095     // converts an axis angle representation into a quaternion
00096 
00097     float* MathUtils::axisAngleToQuaternion(const float* axisa, float* qResult)
00098     {
00099         double s = sin( axisa[3]/2 );
00100         qResult[3] = (float)cos( axisa[3]/2 );
00101         qResult[0] = (float)(axisa[0]*s);
00102         qResult[1] = (float)(axisa[1]*s);
00103         qResult[2] = (float)(axisa[2]*s);
00104         return qResult;
00105     }
00106 
00107     std::vector<float>& MathUtils::axisAngleToQuaternion(const std::vector<float> &axisa, std::vector<float> &qResult)
00108     {
00109         float axisaA[4];
00110         float qResultA[4];
00111 
00112         copyV2A(axisa, axisaA);
00113         axisAngleToQuaternion(axisaA, qResultA);
00114         copyA2V(qResultA, 4, qResult);
00115 
00116         return qResult;
00117     }
00118 
00119     double* MathUtils::axisAngleToQuaternion(const double* axisa, Quaternion qResult)
00120     {
00121         double s = sin( axisa[3]/2 );
00122         qResult[3] = cos( axisa[3]/2 );
00123         qResult[0] = (axisa[0]*s);
00124         qResult[1] = (axisa[1]*s);
00125         qResult[2] = (axisa[2]*s);
00126         return qResult;
00127     }
00128 
00129     // computes a quaternion from euler angles representing a rotation.
00130 
00131     float* MathUtils::eulerToQuaternion(const float roll, const float pitch, const float yaw,
00132                                         float* qResult)
00133     {
00134         double cr, cp, cy, sr, sp, sy, cpcy, spsy;
00135 
00136         // calculate trig identities
00137         cr = cos(roll/2);
00138         cp = cos(pitch/2);
00139         cy = cos(yaw/2);
00140 
00141         sr = sin(roll/2);
00142         sp = sin(pitch/2);
00143         sy = sin(yaw/2);
00144 
00145         cpcy = cp * cy;
00146         spsy = sp * sy;
00147 
00148         qResult[0] = (float)(sr * cpcy - cr * spsy);
00149         qResult[1] = (float)(cr * sp * cy + sr * cp * sy);
00150         qResult[2] = (float)(cr * cp * sy - sr * sp * cy);
00151         qResult[3] = (float)(cr * cpcy + sr * spsy);
00152         return qResult;
00153     }
00154 
00155     std::vector<float>& MathUtils::eulerToQuaternion(const float roll, const float pitch, const float yaw, std::vector<float> &qResult)
00156     {
00157         float qResultA[4];
00158         eulerToQuaternion(roll, pitch, yaw, qResultA);
00159         copyA2V(qResultA, 4, qResult);
00160         return qResult;
00161     }
00162 
00163     // computes a quaternion from euler angles representing a rotation. new version
00164 
00165     float * MathUtils::eulerToQuaternion( const double alpha, const double beta,
00166                                           const double gamma, const enum MathUtils::EULER sequence,
00167                                           float * qResult )
00168     {
00169         float q1[4] = {0,0,0,0}, q2[4] = {0,0,0,0}, q3[4] = {0,0,0,0};
00170 
00171         int axis1 = sequence >> 4;
00172         assert( 0 <= axis1 && axis1 <= 2 );
00173         int axis2 = (sequence >> 2) & 3;
00174         assert( 0 <= axis2 && axis2 <= 2 );
00175         int axis3 = sequence & 3;
00176         assert( 0 <= axis3 && axis3 <= 2 );
00177 
00178         q1[axis1] = (float)sin(alpha/2);
00179         q1[3] = (float)cos(alpha/2);
00180 
00181         q2[axis2] = (float)sin(beta/2);
00182         q2[3] = (float)cos(beta/2);
00183 
00184         q3[axis3] = (float)sin(gamma/2);
00185         q3[3] = (float)cos(gamma/2);
00186 
00187         float temp[4];
00188 
00189         MathUtils::multiplyQuaternion(q1, q2, temp);
00190         MathUtils::multiplyQuaternion(temp, q3, qResult);
00191         return qResult;
00192     }
00193 
00194     // Inverts a Quaternion. Returns result in the second parameter.
00195 
00196     float* MathUtils::invertQuaternion(const float* q, float* qResult)
00197     {
00198         double mod = sqrt(q[0]*q[0]+q[1]*q[1]+q[2]*q[2]+q[3]*q[3]);
00199         qResult[0] = (float)( -q[0] / mod );
00200         qResult[1] = (float)( -q[1] / mod );
00201         qResult[2] = (float)( -q[2] / mod );
00202         qResult[3] = (float)( q[3] / mod );
00203         return qResult;
00204     }
00205 
00206     std::vector<float>& MathUtils::invertQuaternion(const std::vector<float> &q, std::vector<float> &qResult)
00207     {
00208         float qA[4];
00209         float qResultA[4];
00210 
00211         copyV2A(q, qA);
00212         invertQuaternion(qA, qResultA);
00213         copyA2V(qResultA, 4, qResult);
00214 
00215         return qResult;
00216     }
00217 
00218     // computes a quaternion from a passed rotation matrix.
00219 
00220     float* MathUtils::matrixToQuaternion(const float matrix[3][3], float* qResult)
00221     {
00222         double tr, s;
00223         int i, j, k;
00224         int nxt[3] = {1, 2, 0};
00225 
00226         tr = matrix[0][0] + matrix[1][1] + matrix[2][2];
00227 
00228         // check the diagonal
00229         if (tr > 0.0)
00230         {
00231             s = sqrt (tr + 1.0);
00232             qResult[3] =(float)( s / 2.0 );
00233             s = 0.5 / s;
00234             qResult[0] = (float)((matrix[2][1] - matrix[1][2]) * s);
00235             qResult[1] = (float)((matrix[0][2] - matrix[2][0]) * s);
00236             qResult[2] = (float)((matrix[1][0] - matrix[0][1]) * s);
00237         }
00238         else
00239         {
00240             // diagonal is negative
00241             i = 0;
00242             if (matrix[1][1] > matrix[0][0]) i = 1;
00243             if (matrix[2][2] > matrix[i][i]) i = 2;
00244             j = nxt[i];
00245             k = nxt[j];
00246 
00247             s = sqrt((matrix[i][i] - (matrix[j][j] + matrix[k][k])) + 1.0);
00248 
00249             qResult[i] = (float)( s * 0.5 );
00250 
00251             if (s != 0.0) s = 0.5 / s;
00252 
00253             qResult[3] =(float)((matrix[k][j] - matrix[j][k]) * s );
00254             qResult[j] =(float)( (matrix[j][i] + matrix[i][j]) * s );
00255             qResult[k] =(float)( (matrix[k][i] + matrix[i][k]) * s );
00256         }
00257         return qResult;
00258     }
00259 
00260     std::vector<float>& MathUtils::matrixToQuaternion(const float matrix[3][3], std::vector<float> &qResult)
00261     {
00262         float qResultA[4];
00263         matrixToQuaternion(matrix, qResultA);
00264         copyA2V(qResultA, 4, qResult);
00265         return qResult;
00266     }
00267 
00268     // multiplies two quaternions and returns result in a third.
00269 
00270     float* MathUtils::multiplyQuaternion(const float* q1,const float* q2, float* qResult)
00271     {
00272         qResult[0] = q1[3]*q2[0]+q1[0]*q2[3]+q1[1]*q2[2]-q1[2]*q2[1];
00273         qResult[1] = q1[3]*q2[1]-q1[0]*q2[2]+q1[1]*q2[3]+q1[2]*q2[0];
00274         qResult[2] = q1[3]*q2[2]+q1[0]*q2[1]-q1[1]*q2[0]+q1[2]*q2[3];
00275         qResult[3] = q1[3]*q2[3]-q1[0]*q2[0]-q1[1]*q2[1]-q1[2]*q2[2];
00276         return qResult;
00277     }
00278 
00279     std::vector<float>& MathUtils::multiplyQuaternion(const std::vector<float> &q1, const std::vector<float> &q2, std::vector<float> &qResult)
00280     {
00281         float q1A[4];
00282         float q2A[4];
00283         float qResultA[4];
00284 
00285         copyV2A(q1, q1A);
00286         copyV2A(q2, q2A);
00287         multiplyQuaternion(q1A, q2A, qResultA);
00288         copyA2V(qResultA, 4, qResult);
00289 
00290         return qResult;
00291     }
00292 
00293     double* MathUtils::multiplyQuaternion(const ot::MathUtils::Quaternion q1,
00294                                           const ot::MathUtils::Quaternion q2,
00295                                           ot::MathUtils::Quaternion qResult)
00296     {
00297         qResult[0] = q1[3]*q2[0]+q1[0]*q2[3]+q1[1]*q2[2]-q1[2]*q2[1];
00298         qResult[1] = q1[3]*q2[1]-q1[0]*q2[2]+q1[1]*q2[3]+q1[2]*q2[0];
00299         qResult[2] = q1[3]*q2[2]+q1[0]*q2[1]-q1[1]*q2[0]+q1[2]*q2[3];
00300         qResult[3] = q1[3]*q2[3]-q1[0]*q2[0]-q1[1]*q2[1]-q1[2]*q2[2];
00301         return qResult;
00302     }
00303 
00304     // normalizes a quaternion to a unit quaternion.
00305 
00306     float* MathUtils::normalizeQuaternion(float* q)
00307     {
00308         double mod = sqrt(q[0]*q[0]+q[1]*q[1]+q[2]*q[2]+q[3]*q[3]);
00309         q[0] = (float)( q[0] / mod );
00310         q[1] = (float)( q[1] / mod );
00311         q[2] = (float)( q[2] / mod );
00312         q[3] = (float)( q[3] / mod );
00313         return q;
00314     }
00315 
00316     // normalizes a quaternion to a unit quaternion.
00317 
00318     std::vector<float>& MathUtils::normalizeQuaternion(std::vector<float> &q)
00319     {
00320         float qA[4];
00321 
00322         copyV2A(q, qA);
00323         normalizeQuaternion(qA);
00324         copyA2V(qA, 4, q);
00325 
00326         return q;
00327     }
00328 
00329     // normalizes a quaternion to a unit quaternion.
00330 
00331     double* MathUtils::normalizeQuaternion(Quaternion q)
00332     {
00333         double mod = sqrt(q[0]*q[0]+q[1]*q[1]+q[2]*q[2]+q[3]*q[3]);
00334         q[0] = q[0] / mod;
00335         q[1] = q[1] / mod;
00336         q[2] = q[2] / mod;
00337         q[3] = q[3] / mod;
00338         return q;
00339     }
00340 
00341     // rotates a vector by a given quaternion.
00342 
00343     float* MathUtils::rotateVector(const float* q, const float* v, float* vResult)
00344     {
00345         float t1[4], t2[4], t3[4];
00346         // vector in t2
00347         t2[0] = v[0];
00348         t2[1] = v[1];
00349         t2[2] = v[2];
00350         t2[3] = 0;
00351         // inverse in t1
00352         invertQuaternion( q, t1 );
00353         // v * t1 -> t3
00354         multiplyQuaternion( t2, t1, t3 );
00355         // q * t3 -> t1
00356         multiplyQuaternion( q, t3, t1 );
00357         // t1 result -> vResult
00358         vResult[0] = t1[0];
00359         vResult[1] = t1[1];
00360         vResult[2] = t1[2];
00361         return vResult;
00362     }
00363 
00364     std::vector<float>& MathUtils::rotateVector(const std::vector<float> &q, const std::vector<float> &v, std::vector<float> &vResult)
00365     {
00366         float qA[4];
00367         float vA[3];
00368         float vResultA[3];
00369 
00370         copyV2A(q, qA);
00371         copyV2A(v, vA);
00372         rotateVector(qA, vA, vResultA);
00373         copyA2V(vResultA, 3, vResult);
00374 
00375         return vResult;
00376     }
00377 
00378     // computes determinant of a matrix
00379 
00380     float MathUtils::determinant( const float matrix[3][3] )
00381     {
00382         double res = 0;
00383         res += matrix[0][0]*matrix[1][1]*matrix[2][2];
00384         res += matrix[0][1]*matrix[1][2]*matrix[2][0];
00385         res += matrix[0][2]*matrix[1][0]*matrix[2][1];
00386         res -= matrix[0][0]*matrix[1][2]*matrix[2][1];
00387         res -= matrix[0][1]*matrix[1][0]*matrix[2][2];
00388         res -= matrix[0][2]*matrix[1][1]*matrix[2][0];
00389         return (float)res;
00390     }
00391 
00392     // compute angle in a more robust fashion
00393 
00394     double MathUtils::angle( const float * v1, const float * v2, const int dim )
00395     {
00396         int i;
00397         double dot = MathUtils::dot( v1, v2, dim );
00398         /* Numerically stable implementation:
00399          *  if (dot(u,v) < 0.)
00400          *      return M_PI - 2*asin(|v+u)|/2)
00401          *  else
00402          *      return 2*asin(|v-u|/2)
00403          */
00404         if(dot < 0.0)
00405         {
00406             dot = 0;
00407             for( i = 0; i < dim; i++)
00408                 dot += (v2[i]+v1[i])*(v2[i]+v1[i]);
00409             dot = sqrt(dot) / 2.0;
00410             return MathUtils::Pi - 2*asin( dot );
00411         }
00412         else
00413         {
00414             dot = 0;
00415             for( i = 0; i < dim; i++)
00416                 dot += (v2[i]-v1[i])*(v2[i]-v1[i]);
00417             dot = sqrt( dot ) / 2.0;
00418             return 2*asin(dot);
00419         }
00420         return 0;
00421     }
00422 
00423     double MathUtils::angle( const std::vector<float> &v1, const std::vector<float> & v2, const int dim )
00424     {
00425         float *v1A = (float*)malloc(dim * sizeof(float));
00426         float *v2A = (float*)malloc(dim * sizeof(float));
00427         double retValue;
00428 
00429         copyV2A(v1, v1A);
00430         copyV2A(v2, v2A);
00431         retValue = angle(v1A, v2A, dim);
00432         free(v1A);
00433         free(v2A);
00434         return retValue;
00435     }
00436 
00437     // computes slerp
00438 
00439     float * MathUtils::slerp( const float * q1, const float *q2, const float t, float * qResult )
00440     {
00441 
00442         const float*    r1q = q2;
00443 
00444         float           rot1q[4];
00445         double          omega, cosom, sinom;
00446         double          scalerot0, scalerot1;
00447         int             i;
00448 
00449         // Calculate the cosine
00450         cosom = q1[0]*q2[0] + q1[1]*q2[1]
00451             + q1[2]*q2[2] + q1[3]*q2[3];
00452 
00453         // adjust signs if necessary
00454         if ( cosom < 0.0 ) {
00455             cosom = -cosom;
00456             for ( int j = 0; j < 4; j++ )
00457                 rot1q[j] = -r1q[j];
00458         } else  {
00459             for ( int j = 0; j < 4; j++ )
00460                 rot1q[j] = r1q[j];
00461         }
00462 
00463         // calculate interpolating coeffs
00464         if ( (1.0 - cosom) > 0.00001 ) {
00465             // standard case
00466             omega = acos(cosom);
00467             sinom = sin(omega);
00468             scalerot0 = sin((1.0 - t) * omega) / sinom;
00469             scalerot1 = sin(t * omega) / sinom;
00470         } else {
00471             // rot0 and rot1 very close - just do linear interp.
00472             scalerot0 = 1.0 - t;
00473             scalerot1 = t;
00474         }
00475 
00476         // build the new quarternion
00477         for (i = 0; i <4; i++)
00478             qResult[i] = (float)(scalerot0 * q1[i] + scalerot1 * rot1q[i]);
00479 
00480         MathUtils::normalizeQuaternion( qResult );
00481 
00482         return qResult;
00483 
00484         /*
00485           double angle = MathUtils::angle( q1, q2, 4);
00486           float temp[4];
00487           if( angle < 0)
00488           {
00489           temp[0] = -q2[0];
00490           temp[1] = -q2[1];
00491           temp[2] = -q2[2];
00492           temp[3] = -q2[3];
00493           angle = MathUtils::angle( q1, temp, 4);
00494           }
00495           else
00496           {
00497           temp[0] = q2[0];
00498           temp[1] = q2[1];
00499           temp[2] = q2[2];
00500           temp[3] = q2[3];
00501           }
00502           double c1 = MD::sin_over_x((1-t)*angle) * (t-1) / MD::sin_over_x(angle);
00503           double c2 = MD::sin_over_x(t*angle) * t / MD::sin_over_x(angle);
00504           int i;
00505           for( i = 0; i < 4; i++)
00506           {
00507           qResult[i] = (float)(q1[i] * c1 + temp[i]*c2);
00508           }
00509         */
00510 
00511         return qResult;
00512     }
00513 
00514     std::vector<float>& MathUtils::slerp( const std::vector<float> &q1, const std::vector<float> &q2, const float t, std::vector<float> &qResult )
00515     {
00516         float q1A[4];
00517         float q2A[4];
00518         float qResultA[4];
00519 
00520         copyV2A(q1, q1A);
00521         copyV2A(q2, q2A);
00522         slerp(q1A, q2A, t, qResultA);
00523         copyA2V(qResultA, 4, qResult);
00524 
00525         return qResult;
00526     }
00527 
00528     // ----------------------------------------------------------------------------------------
00529     void MathUtils::matrixMultiply(const Matrix4x4 m1, const Matrix4x4 m2, Matrix4x4 &m)
00530     {
00531         m[0][0] = m1[0][0]*m2[0][0] + m1[0][1]*m2[1][0] + m1[0][2]*m2[2][0] + m1[0][3]*m2[3][0];
00532         m[0][1] = m1[0][0]*m2[0][1] + m1[0][1]*m2[1][1] + m1[0][2]*m2[2][1] + m1[0][3]*m2[3][1];
00533         m[0][2] = m1[0][0]*m2[0][2] + m1[0][1]*m2[1][2] + m1[0][2]*m2[2][2] + m1[0][3]*m2[3][2];
00534         m[0][3] = m1[0][0]*m2[0][3] + m1[0][1]*m2[1][3] + m1[0][2]*m2[2][3] + m1[0][3]*m2[3][3];
00535         m[1][0] = m1[1][0]*m2[0][0] + m1[1][1]*m2[1][0] + m1[1][2]*m2[2][0] + m1[1][3]*m2[3][0];
00536         m[1][1] = m1[1][0]*m2[0][1] + m1[1][1]*m2[1][1] + m1[1][2]*m2[2][1] + m1[1][3]*m2[3][1];
00537         m[1][2] = m1[1][0]*m2[0][2] + m1[1][1]*m2[1][2] + m1[1][2]*m2[2][2] + m1[1][3]*m2[3][2];
00538         m[1][3] = m1[1][0]*m2[0][3] + m1[1][1]*m2[1][3] + m1[1][2]*m2[2][3] + m1[1][3]*m2[3][3];
00539         m[2][0] = m1[2][0]*m2[0][0] + m1[2][1]*m2[1][0] + m1[2][2]*m2[2][0] + m1[2][3]*m2[3][0];
00540         m[2][1] = m1[2][0]*m2[0][1] + m1[2][1]*m2[1][1] + m1[2][2]*m2[2][1] + m1[2][3]*m2[3][1];
00541         m[2][2] = m1[2][0]*m2[0][2] + m1[2][1]*m2[1][2] + m1[2][2]*m2[2][2] + m1[2][3]*m2[3][2];
00542         m[2][3] = m1[2][0]*m2[0][3] + m1[2][1]*m2[1][3] + m1[2][2]*m2[2][3] + m1[2][3]*m2[3][3];
00543         m[3][0] = m1[3][0]*m2[0][0] + m1[3][1]*m2[1][0] + m1[3][2]*m2[2][0] + m1[3][3]*m2[3][0];
00544         m[3][1] = m1[3][0]*m2[0][1] + m1[3][1]*m2[1][1] + m1[3][2]*m2[2][1] + m1[3][3]*m2[3][1];
00545         m[3][2] = m1[3][0]*m2[0][2] + m1[3][1]*m2[1][2] + m1[3][2]*m2[2][2] + m1[3][3]*m2[3][2];
00546         m[3][3] = m1[3][0]*m2[0][3] + m1[3][1]*m2[1][3] + m1[3][2]*m2[2][3] + m1[3][3]*m2[3][3];
00547     }
00548 
00549     // ----------------------------------------------------------------------------------------
00550     void MathUtils::matrixMultiply(const Matrix3x3 m1, const Matrix3x3 m2, Matrix3x3 &m)
00551     {
00552         m[0][0] = m1[0][0]*m2[0][0] + m1[0][1]*m2[1][0] + m1[0][2]*m2[2][0];
00553         m[0][1] = m1[0][0]*m2[0][1] + m1[0][1]*m2[1][1] + m1[0][2]*m2[2][1];
00554         m[0][2] = m1[0][0]*m2[0][2] + m1[0][1]*m2[1][2] + m1[0][2]*m2[2][2];
00555         m[1][0] = m1[1][0]*m2[0][0] + m1[1][1]*m2[1][0] + m1[1][2]*m2[2][0];
00556         m[1][1] = m1[1][0]*m2[0][1] + m1[1][1]*m2[1][1] + m1[1][2]*m2[2][1];
00557         m[1][2] = m1[1][0]*m2[0][2] + m1[1][1]*m2[1][2] + m1[1][2]*m2[2][2];
00558         m[2][0] = m1[2][0]*m2[0][0] + m1[2][1]*m2[1][0] + m1[2][2]*m2[2][0];
00559         m[2][1] = m1[2][0]*m2[0][1] + m1[2][1]*m2[1][1] + m1[2][2]*m2[2][1];
00560         m[2][2] = m1[2][0]*m2[0][2] + m1[2][1]*m2[1][2] + m1[2][2]*m2[2][2];
00561     }
00562 
00563     // ----------------------------------------------------------------------------------------
00564 
00565     void MathUtils::MatrixToEuler(Vector3 angles, const Matrix4x4 colMatrix)
00566     {
00567 
00568         double sinPitch, cosPitch, sinRoll, cosRoll, sinYaw, cosYaw;
00569 
00570 
00571         sinPitch = -colMatrix[2][0];
00572         cosPitch = sqrt(1 - sinPitch*sinPitch);
00573 
00574         if ( fabs(cosPitch) > Q_EPSILON )
00575         {
00576             sinRoll = colMatrix[2][1] / cosPitch;
00577             cosRoll = colMatrix[2][2] / cosPitch;
00578             sinYaw = colMatrix[1][0] / cosPitch;
00579             cosYaw = colMatrix[0][0] / cosPitch;
00580         }
00581         else
00582         {
00583             sinRoll = -colMatrix[1][2];
00584             cosRoll = colMatrix[1][1];
00585             sinYaw = 0;
00586             cosYaw = 1;
00587         }
00588 
00589         /* yaw */
00590         angles[0] = atan2(sinYaw, cosYaw);
00591 
00592         /* pitch */
00593         angles[1] = atan2(sinPitch, cosPitch);
00594 
00595         /* roll */
00596         angles[2] = atan2(sinRoll, cosRoll);
00597 
00598     } /* MatrixToEuler */
00599 
00601     //
00602     //   Heavily based on code by Ken Shoemake and code by Gavin Bell
00603     float *MathUtils::quaternionToAxisAngle(const float q[4], float axisa[4])
00604     {
00605         double  len;
00606 
00607         len = sqrt(q[0]*q[0] + q[1]*q[1] + q[2]*q[2]);
00608         // check quaternion
00609         if (len  > Q_EPSILON) {
00610             // if valid compute vec and angle
00611             // normalize vec
00612             float invLen = (float)(1.0/ len);
00613             axisa[0]    = q[0] * invLen;
00614             axisa[1]    = q[1] * invLen;
00615             axisa[2]    = q[2] * invLen;
00616 
00617             axisa[3]    =(float)( 2.0f * (float)acos(q[3]));
00618         }
00619 
00620         else {
00621             // return identity quaternion
00622             axisa[0] = 0.0;
00623             axisa[1] = 0.0;
00624             axisa[2] = 1.0;
00625             axisa[3] = 0.0;
00626         }
00627         return axisa;
00628     }
00629 
00630     double MathUtils::dot( const float * v1, const float * v2, const int dim )
00631     {
00632         double result = 0;
00633         int i;
00634         for( i = 0; i < dim; i++ )
00635         {
00636             result += v1[i] * v2[i];
00637         }
00638         return result;
00639     }
00640 
00641     double MathUtils::dot( const std::vector<float> &v1, const std::vector<float> &v2, const int dim )
00642     {
00643         float *v1A = (float*)malloc(dim * sizeof(float));
00644         float *v2A = (float*)malloc(dim * sizeof(float));
00645         double retValue;
00646 
00647         copyV2A(v1, v1A);
00648         copyV2A(v2, v2A);
00649         retValue = dot(v1A, v2A, dim);
00650         free(v1A);
00651         free(v2A);
00652         return retValue;
00653     }
00654 
00655 
00656 } // namespace ot
00657 
00658 /* 
00659  * ------------------------------------------------------------
00660  *   End of MathUtils.cxx
00661  * ------------------------------------------------------------
00662  *   Automatic Emacs configuration follows.
00663  *   Local Variables:
00664  *   mode:c++
00665  *   c-basic-offset: 4
00666  *   eval: (c-set-offset 'substatement-open 0)
00667  *   eval: (c-set-offset 'case-label '+)
00668  *   eval: (c-set-offset 'statement 'c-lineup-runin-statements)
00669  *   eval: (setq indent-tabs-mode nil)
00670  *   End:
00671  * ------------------------------------------------------------ 
00672  */

copyright (c) 2006 Graz University of Technology