rpp_vecmat.cpp
Go to the documentation of this file.
00001 /* ========================================================================
00002  * PROJECT: ARToolKitPlus
00003  * ========================================================================
00004  * This work is based on the original ARToolKit developed by
00005  *   Hirokazu Kato
00006  *   Mark Billinghurst
00007  *   HITLab, University of Washington, Seattle
00008  * http://www.hitl.washington.edu/artoolkit/
00009  *
00010  * Copyright of the derived and new portions of this work
00011  *     (C) 2006 Graz University of Technology
00012  *
00013  * This framework is free software; you can redistribute it and/or modify
00014  * it under the terms of the GNU General Public License as published by
00015  * the Free Software Foundation; either version 2 of the License, or
00016  * (at your option) any later version.
00017  *
00018  * This framework is distributed in the hope that it will be useful,
00019  * but WITHOUT ANY WARRANTY; without even the implied warranty of
00020  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00021  * GNU General Public License for more details.
00022  *
00023  * You should have received a copy of the GNU General Public License
00024  * along with this framework; if not, write to the Free Software
00025  * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
00026  *
00027  * For further information please contact 
00028  *   Dieter Schmalstieg
00029  *   <schmalstieg@icg.tu-graz.ac.at>
00030  *   Graz University of Technology, 
00031  *   Institut for Computer Graphics and Vision,
00032  *   Inffeldgasse 16a, 8010 Graz, Austria.
00033  * ========================================================================
00034  ** @author   Thomas Pintaric
00035  *
00036  * $Id: rpp_vecmat.cpp 162 2006-04-19 21:28:10Z grabner $
00037  * @file
00038  * ======================================================================== */
00039 
00040 
00041 #ifndef _NO_LIBRPP_
00042 
00043 
00044 #include "rpp/rpp_vecmat.h"
00045 #include "math.h"
00046 #include "assert.h"
00047 #include <stdlib.h>
00048 #include <stdio.h>
00049 
00050 
00051 namespace rpp {
00052 
00053 
00054 int svdcmp( double **a, int m,int n, double *w, double **v);
00055 int quintic(double [], double [], double [], int*, double);
00056 int quartic(double[], double[], double[], int* );
00057 int cubic(double[], double[], int*);
00058 
00059 
00060 
00061 #ifdef _WIN32_WCE
00062         real_t _sin(real_t a) { return(real_t(::sin(a))); }
00063         real_t _cos(real_t a) { return(real_t(::cos(a))); }
00064         real_t _atan2(real_t a, real_t b) { return(real_t(::atan2(a,b))); }
00065         real_t _abs(real_t a) { return((a>0?a:-a)); }
00066         real_t _acos(real_t a) { return(real_t(::acos(a))); }
00067         real_t _sqrt(real_t a) { return(real_t(::sqrt(a))); }
00068         real_t _pow(real_t a, real_t b) { return(real_t(::pow(a,b))); }
00069 #else
00070         real_t _sin(real_t a) { return(::sin(a)); }
00071         real_t _cos(real_t a) { return(::cos(a)); }
00072         real_t _atan2(real_t a, real_t b) { return(::atan2(a,b)); }
00073         real_t _abs(real_t a) { return((a>0?a:-a)); }
00074         real_t _acos(real_t a) { return(::acos(a)); }
00075         real_t _sqrt(real_t a) { return(::sqrt(a)); }
00076         real_t _pow(real_t a, real_t b) { return(::pow(a,b)); }
00077         /*
00078         real_t _sin(real_t a) { return(::sinf(a)); }
00079         real_t _cos(real_t a) { return(::cosf(a)); }
00080         real_t _atan2(real_t a, real_t b) { return(::atan2f(a,b)); }
00081         real_t _abs(real_t a) { return((a>0?a:-a)); }
00082         real_t _acos(real_t a) { return(::acosf(a)); }
00083         real_t _sqrt(real_t a) { return(::sqrtf(a)); }
00084         real_t _pow(real_t a, real_t b) { return(::powf(a,b)); }
00085         */
00086 #endif
00087 
00088 // ---------------------------------------------------------------------------
00089 
00090 void mat33_from_double_pptr(mat33_t &mat, double** m_ptr)
00091 {
00092         for(int m=0; m<3; m++)
00093                 for(int n=0; n<3; n++)
00094                         mat.m[m][n] = (real_t) m_ptr[m][n];
00095 }
00096 
00097 
00098 void mat33_from_float_pptr(mat33_t &mat, float** m_ptr)
00099 {
00100         for(int m=0; m<3; m++)
00101                 for(int n=0; n<3; n++)
00102                         mat.m[m][n] = (real_t) m_ptr[m][n];
00103 }
00104 
00105 
00106 double ** mat33_to_double_pptr(const mat33_t &mat)
00107 {
00108         double **M = (double**) malloc(3*sizeof(double*));
00109         for(int i=0; i<3; i++) M[i] = (double*) malloc(3*sizeof(double));
00110         for(int m=0; m<3; m++)
00111                 for(int n=0; n<3; n++)
00112                         M[m][n] = (double) mat.m[m][n];
00113         return(M);
00114 }
00115 
00116 
00117 float ** mat33_to_float_pptr(const mat33_t &mat)
00118 {
00119         float **M = (float**) malloc(3*sizeof(float*));
00120         for(int i=0; i<3; i++) M[i] = (float*) malloc(3*sizeof(float));
00121         for(int m=0; m<3; m++)
00122                 for(int n=0; n<3; n++)
00123                         M[m][n] = (float) mat.m[m][n];
00124         return(M);
00125 }
00126 
00127 
00128 void free_double_pptr(double*** m_ptr)
00129 {
00130         for(int i=0; i<3; i++) free((*m_ptr)[i]);
00131         free(*m_ptr);
00132 }
00133 
00134 void free_float_pptr(float*** m_ptr)
00135 {
00136         for(int i=0; i<3; i++) free((*m_ptr)[i]);
00137         free(*m_ptr);
00138 }
00139 
00140 void vec3_from_double_ptr(vec3_t &vec, double* v_ptr)
00141 {
00142         vec.v[0] = (real_t)v_ptr[0];
00143         vec.v[1] = (real_t)v_ptr[1];
00144         vec.v[2] = (real_t)v_ptr[2];
00145 }
00146 
00147 void vec3_from_float_ptr(vec3_t &vec, float* v_ptr)
00148 {
00149         vec.v[0] = (real_t)v_ptr[0];
00150         vec.v[1] = (real_t)v_ptr[1];
00151         vec.v[2] = (real_t)v_ptr[2];
00152 }
00153 
00154 double* vec3_to_double_ptr(const vec3_t &vec)
00155 {
00156         double* v = (double*) malloc(3* sizeof(double));
00157         v[0] = (double) vec.v[0];
00158         v[1] = (double) vec.v[1];
00159         v[2] = (double) vec.v[2];
00160         return(v);
00161 }
00162 
00163 float* vec3_to_float_ptr(const vec3_t &vec)
00164 {
00165         float* v = (float*) malloc(3* sizeof(float));
00166         v[0] = (float) vec.v[0];
00167         v[1] = (float) vec.v[1];
00168         v[2] = (float) vec.v[2];
00169         return(v);
00170 }
00171 
00172 
00173 void free_double_ptr(double** v_ptr)
00174 {
00175         free(*v_ptr);
00176 }
00177 
00178 void free_float_ptr(float** v_ptr)
00179 {
00180         free(*v_ptr);
00181 }
00182 
00183 
00184 
00185 void mat33_assign(mat33_t &m,
00186                                   const real_t m00, const real_t m01, const real_t m02,
00187                                   const real_t m10, const real_t m11, const real_t m12,
00188                                   const real_t m20, const real_t m21, const real_t m22)
00189 {
00190         m.m[0][0] = m00;        m.m[0][1] = m01;        m.m[0][2] = m02;
00191         m.m[1][0] = m10;        m.m[1][1] = m11;        m.m[1][2] = m12;
00192         m.m[2][0] = m20;        m.m[2][1] = m21;        m.m[2][2] = m22;
00193 }
00194 
00195 
00196 void _dbg_quat_print(const quat_t &q, char* name)
00197 {
00198         printf("%s:  s = [ %.4f ]  v = [ ",name,q.s);
00199         for(unsigned int c=0; c<3; c++)
00200         {
00201                 // double d = (double)(q.v.v[c]);
00202                 printf("%.4f ",q.v.v[c]);
00203         }
00204         printf("]\n");
00205 }
00206 
00207 void _dbg_mat33_print(const mat33_t &m, char* name)
00208 {
00209         printf("%s:\n",name);
00210         for(unsigned int r=0; r<3; r++)
00211         {
00212                 printf("[ ");
00213                 for(unsigned int c=0; c<3; c++)
00214                 {
00215                         double d = (double)(m.m[r][c]);
00216                         printf("%.4f ",d);
00217                 }
00218                 printf("]\n");
00219         }
00220 }
00221 
00222 void _dbg_mat33_array_print(const mat33_array &m, char* name)
00223 {
00224         for(unsigned int i=0; i<m.size(); i++)
00225         {
00226                 printf("%s.at(%i):\n",name,i);
00227                 for(unsigned int r=0; r<3; r++)
00228                 {
00229                         printf("[ ");
00230                         for(unsigned int c=0; c<3; c++)
00231                         {
00232                                 double d = (double)(m.at(i).m[r][c]);
00233                                 printf("%.4f ",d);
00234                         }
00235                         printf("]\n");
00236                 }
00237         }
00238 }
00239 
00240 void _dbg_vec3_print(const vec3_t &v, char* name)
00241 {
00242         printf("%s:  [ ",name);
00243         for(unsigned int c=0; c<3; c++)
00244         {
00245                 double d = (double)(v.v[c]);
00246                 printf("%.4f ",d);
00247         }
00248         printf("]\n");
00249 }
00250 
00251 void _dbg_vec3_array_print(const vec3_array &v, char* name)
00252 {
00253         for(unsigned int i=0; i<v.size(); i++)
00254         {
00255                 printf("%s.at(%i):  [ ",name,i);
00256                 for(unsigned int c=0; c<3; c++)
00257                 {
00258                         double d = (double)(v.at(i).v[c]);
00259                         printf("%.4f ",d);
00260                 }
00261                 printf("]\n");
00262         }
00263 }
00264 
00265 
00266 bool _dbg_load_vec3_array(vec3_array &va, char* filename)
00267 {
00268         FILE *fp = fopen( filename, "r" );
00269         if( fp == NULL ) return(false);
00270 
00271         int n;
00272         int lineno = 0;
00273         va.clear();
00274 
00275         while(!feof(fp))
00276         {
00277                 ++lineno;
00278                 double vx,vy,vz;
00279                 vec3_t v;
00280                 n = fscanf(fp, "%lf%lf%lf\n", &vx,&vy,&vz);
00281                 if((n!=3) || ferror(fp))
00282                 {
00283                         printf("file i/o error: %s (line %i)",filename,lineno);
00284                         fclose(fp);
00285                         return(lineno > 1);
00286                 }
00287                 v.v[0] = (real_t)vx;
00288                 v.v[1] = (real_t)vy;
00289                 v.v[2] = (real_t)vz;
00290                 va.push_back(v);
00291         }
00292 
00293         fclose(fp);
00294         return(true);
00295 }
00296 
00297 
00298 void vec3_assign(vec3_t &v, const real_t x, const real_t y, const real_t z)
00299 {
00300         v.v[0] = x;
00301         v.v[1] = y;
00302         v.v[2] = z;
00303 }
00304 
00305 void vec3_clear(vec3_t &v)
00306 {
00307         v.v[0] = 0;
00308         v.v[1] = 0;
00309         v.v[2] = 0;
00310 }
00311 
00312 void vec3_copy(vec3_t &a, const vec3_t &b)
00313 {
00314         a.v[0] = b.v[0];
00315         a.v[1] = b.v[1];
00316         a.v[2] = b.v[2];
00317 }
00318 
00319 
00320 void vec3_array_sum(vec3_t &v_sum2, const vec3_array &va)
00321 {
00322         vec3_clear(v_sum2);
00323         for(vec3_array_const_iter iter = va.begin();
00324                 iter != va.end(); iter++)
00325         {
00326                 v_sum2.v[0] += (*iter).v[0];
00327                 v_sum2.v[1] += (*iter).v[1];
00328                 v_sum2.v[2] += (*iter).v[2];
00329         }
00330 }
00331 
00332 void vec3_array_sum(scalar_array &v_sum1, const vec3_array &va)
00333 {
00334         v_sum1.clear();
00335         v_sum1.resize(va.size());
00336 
00337         for(unsigned int i=0; i<va.size(); i++)
00338                 v_sum1.at(i) = (va[i].v[0] + va[i].v[1] + va[i].v[2]);
00339 }
00340 
00341 void vec3_array_pow2(vec3_array &va)
00342 {
00343         for(vec3_array_iter iter = va.begin();
00344                 iter != va.end(); iter++)
00345         {
00346                 (*iter).v[0] *= (*iter).v[0];
00347                 (*iter).v[1] *= (*iter).v[1];
00348                 (*iter).v[2] *= (*iter).v[2];
00349         }
00350 }
00351 
00352 
00353 
00354 
00355 void vec3_div(vec3_t &va, const real_t n)
00356 {
00357         va.v[0] /= n;
00358         va.v[1] /= n;
00359         va.v[2] /= n;
00360 }
00361 
00362 void vec3_div(vec3_t &va, const vec3_t &vb)
00363 {
00364         va.v[0] /= vb.v[0];
00365         va.v[1] /= vb.v[1];
00366         va.v[2] /= vb.v[2];
00367 }
00368 
00369 void vec3_mult(vec3_t &va, const real_t n)
00370 {
00371         va.v[0] *= n;
00372         va.v[1] *= n;
00373         va.v[2] *= n;
00374 }
00375 
00376 void vec3_mult(vec3_t &va, const vec3_t &vb)
00377 {
00378         va.v[0] *= vb.v[0];
00379         va.v[1] *= vb.v[1];
00380         va.v[2] *= vb.v[2];
00381 }
00382 
00383 void vec3_add(vec3_t &va, const real_t f)
00384 {
00385         va.v[0] += f;
00386         va.v[1] += f;
00387         va.v[2] += f;
00388 }
00389 
00390 void vec3_add(vec3_t &va, const vec3_t &vb)
00391 {
00392         va.v[0] += vb.v[0];
00393         va.v[1] += vb.v[1];
00394         va.v[2] += vb.v[2];
00395 }
00396 
00397 void vec3_add(vec3_t &va, const vec3_t &vb, const vec3_t &vc)
00398 {
00399         va.v[0] = vb.v[0] + vc.v[0];
00400         va.v[1] = vb.v[1] + vc.v[1];
00401         va.v[2] = vb.v[2] + vc.v[2];
00402 }
00403 
00404 void vec3_sub(vec3_t &va, const real_t f)
00405 {
00406         va.v[0] -= f;
00407         va.v[1] -= f;
00408         va.v[2] -= f;
00409 }
00410 
00411 void vec3_sub(vec3_t &va, const vec3_t &vb)
00412 {
00413         va.v[0] -= vb.v[0];
00414         va.v[1] -= vb.v[1];
00415         va.v[2] -= vb.v[2];
00416 }
00417 
00418 void vec3_sub(vec3_t &va, const vec3_t &vb, const vec3_t &vc)
00419 {
00420         va.v[0] = vb.v[0] - vc.v[0];
00421         va.v[1] = vb.v[1] - vc.v[1];
00422         va.v[2] = vb.v[2] - vc.v[2];
00423 }
00424 
00425 real_t vec3_dot(const vec3_t &va, const vec3_t &vb)
00426 {
00427         return(va.v[0]*vb.v[0]+va.v[1]*vb.v[1]+va.v[2]*vb.v[2]);
00428 }
00429 
00430 void vec3_cross(vec3_t &va, const vec3_t &vb, const vec3_t &vc)
00431 {
00432         va.v[0] = (vb.v[1] * vc.v[2] - vc.v[1] * vb.v[2]);
00433         va.v[1] = (vb.v[2] * vc.v[0] - vc.v[2] * vb.v[0]);
00434         va.v[2] = (vb.v[0] * vc.v[1] - vc.v[0] * vb.v[1]);
00435 }
00436 
00437 real_t vec3_norm(const vec3_t &v)
00438 {
00439         return(_sqrt(v.v[0]*v.v[0] + v.v[1]*v.v[1] + v.v[2]*v.v[2]));
00440 }
00441 
00442 real_t vec3_sum(const vec3_t &v)
00443 {
00444         return(v.v[0] + v.v[1] + v.v[2]);
00445 }
00446 
00447 void vec3_array_add(vec3_array &va, const vec3_t &a)
00448 {
00449         for(vec3_array_iter iter = va.begin();
00450                 iter != va.end(); iter++)
00451         {
00452                 (*iter).v[0] += a.v[0];
00453                 (*iter).v[1] += a.v[1];
00454                 (*iter).v[2] += a.v[2];
00455         }
00456 }
00457 
00458 void vec3_array_sub(vec3_array &va, const vec3_t &a) 
00459 {
00460         for(vec3_array_iter iter = va.begin();
00461                 iter != va.end(); iter++)
00462         {
00463                 (*iter).v[0] -= a.v[0];
00464                 (*iter).v[1] -= a.v[1];
00465                 (*iter).v[2] -= a.v[2];
00466         }
00467 }
00468 
00469 void vec3_array_set(vec3_array &va, const vec3_t &a, const bool mask[3])
00470 {
00471         for(vec3_array_iter iter = va.begin();
00472                 iter != va.end(); iter++)
00473         {
00474                 if(mask[0]) (*iter).v[0] = a.v[0];
00475                 if(mask[1]) (*iter).v[1] = a.v[1];
00476                 if(mask[2]) (*iter).v[2] = a.v[2];
00477         }
00478 }
00479 
00480 void vec3_array_mult(vec3_array &va, const scalar_array &c)
00481 {
00482         assert(va.size() == c.size());
00483         for(unsigned int i=0; i<va.size(); i++)
00484         {
00485                 va[i].v[0] *= c[i];
00486                 va[i].v[1] *= c[i];
00487                 va[i].v[2] *= c[i];
00488         }
00489 }
00490 
00491 void vec3_array_mean(vec3_t &v_mean, const vec3_array &va)
00492 {
00493         vec3_array_sum(v_mean, va);
00494         real_t l = (real_t) va.size();
00495         vec3_div(v_mean, l);
00496 }
00497 
00498 
00499 void vec3_mul_vec3trans(mat33_t &m, const vec3_t &va, const vec3_t &vb)
00500 {
00501         m.m[0][0] = va.v[0] * vb.v[0];
00502         m.m[0][1] = va.v[0] * vb.v[1];
00503         m.m[0][2] = va.v[0] * vb.v[2];
00504         m.m[1][0] = va.v[1] * vb.v[0];
00505         m.m[1][1] = va.v[1] * vb.v[1];
00506         m.m[1][2] = va.v[1] * vb.v[2];
00507         m.m[2][0] = va.v[2] * vb.v[0];
00508         m.m[2][1] = va.v[2] * vb.v[1];
00509         m.m[2][2] = va.v[2] * vb.v[2];
00510 }
00511 
00512 real_t vec3trans_mul_vec3(const vec3_t &va, const vec3_t &vb)
00513 {
00514         return(va.v[0] * vb.v[0] + va.v[1] * vb.v[1] + va.v[2] * vb.v[2]);
00515 }
00516 
00517 
00518 void mat33_clear(mat33_t &m)
00519 {
00520         for(unsigned int r=0; r<3; r++)
00521                 for(unsigned int c=0; c<3; c++)
00522                 {
00523                         m.m[r][c] = 0;
00524                 }
00525 }
00526 
00527 void mat33_copy(mat33_t &md, const mat33_t &ms)
00528 {
00529         for(unsigned int r=0; r<3; r++)
00530                 for(unsigned int c=0; c<3; c++)
00531                 {
00532                         md.m[r][c] = ms.m[r][c];
00533                 }
00534 }
00535 
00536 
00537 void mat33_to_col_vec3(vec3_t &c0, vec3_t &c1, vec3_t &c2, const mat33_t &m)
00538 {
00539         for(unsigned int r=0; r<3; r++)
00540         {
00541                 c0.v[r] = m.m[r][0];
00542                 c1.v[r] = m.m[r][1];
00543                 c2.v[r] = m.m[r][2];
00544         }
00545 }
00546 
00547 
00548 void mat33_div(mat33_t &m, const real_t f)
00549 {
00550         m.m[0][0] /= f;
00551         m.m[0][1] /= f;
00552         m.m[0][2] /= f;
00553         m.m[1][0] /= f;
00554         m.m[1][1] /= f;
00555         m.m[1][2] /= f;
00556         m.m[2][0] /= f;
00557         m.m[2][1] /= f;
00558         m.m[2][2] /= f;
00559 }
00560 
00561 void mat33_eye(mat33_t &m)
00562 {
00563         m.m[0][0] = 1;
00564         m.m[0][1] = 0;
00565         m.m[0][2] = 0;
00566         m.m[1][0] = 0;
00567         m.m[1][1] = 1;
00568         m.m[1][2] = 0;
00569         m.m[2][0] = 0;
00570         m.m[2][1] = 0;
00571         m.m[2][2] = 1;
00572 }
00573 
00574 real_t mat33_sum(const mat33_t &m)
00575 {
00576         real_t sum(0.0f);
00577         for(unsigned int r=0; r<3; r++)
00578                 for(unsigned int c=0; c<3; c++)
00579                 {
00580                         sum += m.m[r][c];
00581                 }
00582                 return(sum);
00583 }
00584 
00585 
00586 bool mat33_all_zeros(const mat33_t &m)
00587 {
00588         for(unsigned int r=0; r<3; r++)
00589                 for(unsigned int c=0; c<3; c++)
00590                 {
00591                         if(m.m[r][c] != 0) return(false);
00592                 }
00593                 return(true);
00594 }
00595 
00596 void mat33_set_all_zeros(mat33_t &m)
00597 {
00598         for(unsigned int r=0; r<3; r++)
00599                 for(unsigned int c=0; c<3; c++)
00600                 {
00601                         m.m[r][c] = 0;
00602                 }
00603 }
00604 
00605 
00606 void mat33_array_sum(mat33_t &s, const mat33_array &ma)
00607 {
00608         mat33_clear(s);
00609         for(mat33_array_const_iter iter = ma.begin();
00610                 iter != ma.end(); iter++)
00611         {
00612                 for(unsigned int c=0; c<3; c++)
00613                 {
00614                         s.m[0][c] += (*iter).m[0][c];
00615                         s.m[1][c] += (*iter).m[1][c];
00616                         s.m[2][c] += (*iter).m[2][c];
00617                 }
00618         }
00619 }
00620 
00621 
00622 void mat33_sub(mat33_t &mr, const mat33_t &ma, const mat33_t &mb)
00623 {
00624         for(unsigned int r=0; r<3; r++)
00625                 for(unsigned int c=0; c<3; c++)
00626                 {
00627                         mr.m[r][c] = ma.m[r][c]-mb.m[r][c];
00628                 }
00629 }
00630 
00631 void mat33_sub(mat33_t &ma, const mat33_t &mb)
00632 {
00633         for(unsigned int r=0; r<3; r++)
00634                 for(unsigned int c=0; c<3; c++)
00635                 {
00636                         ma.m[r][c] -= mb.m[r][c];
00637                 }
00638 }
00639 
00640 void mat33_add(mat33_t &mr, const mat33_t &ma, const mat33_t &mb)
00641 {
00642         for(unsigned int r=0; r<3; r++)
00643                 for(unsigned int c=0; c<3; c++)
00644                 {
00645                         mr.m[r][c] = ma.m[r][c]+mb.m[r][c];
00646                 }
00647 }
00648 
00649 void mat33_add(mat33_t &ma, const mat33_t &mb)
00650 {
00651         for(unsigned int r=0; r<3; r++)
00652                 for(unsigned int c=0; c<3; c++)
00653                 {
00654                         ma.m[r][c] += mb.m[r][c];
00655                 }
00656 }
00657 
00658 
00659 real_t mat33_det(const mat33_t &a)
00660 {
00661         real_t determinant = a.m[0][0]*a.m[1][1]*a.m[2][2] + a.m[0][1]*a.m[1][2]*a.m[2][0] +
00662                 a.m[0][2]*a.m[1][0]*a.m[2][1] - a.m[2][0]*a.m[1][1]*a.m[0][2] - 
00663                 a.m[2][1]*a.m[1][2]*a.m[0][0] - a.m[2][2]*a.m[1][0]*a.m[0][1];
00664         return(determinant);
00665 }
00666 
00667 void mat33_inv(mat33_t &mi, const mat33_t &ma)
00668 {
00669         real_t determinant = mat33_det(ma);
00670         mi.m[0][0] = (ma.m[1][1]*ma.m[2][2] - ma.m[1][2]*ma.m[2][1])/determinant;
00671         mi.m[0][1] = (ma.m[0][2]*ma.m[2][1] - ma.m[0][1]*ma.m[2][2])/determinant;
00672         mi.m[0][2] = (ma.m[0][1]*ma.m[1][2] - ma.m[0][2]*ma.m[1][1])/determinant;
00673 
00674         mi.m[1][0] = (ma.m[1][2]*ma.m[2][0] - ma.m[1][0]*ma.m[2][2])/determinant;
00675         mi.m[1][1] = (ma.m[0][0]*ma.m[2][2] - ma.m[0][2]*ma.m[2][0])/determinant;
00676         mi.m[1][2] = (ma.m[0][2]*ma.m[1][0] - ma.m[0][0]*ma.m[1][2])/determinant;
00677 
00678         mi.m[2][0] = (ma.m[1][0]*ma.m[2][1] - ma.m[1][1]*ma.m[2][0])/determinant;
00679         mi.m[2][1] = (ma.m[0][1]*ma.m[2][0] - ma.m[0][0]*ma.m[2][1])/determinant;
00680         mi.m[2][2] = (ma.m[0][0]*ma.m[1][1] - ma.m[0][1]*ma.m[1][0])/determinant;
00681 }
00682 
00683 void mat33_mult(mat33_t &m0, const mat33_t &m1, const mat33_t &m2)
00684 {
00685         m0.m[0][0] = m1.m[0][0]*m2.m[0][0] + m1.m[0][1]*m2.m[1][0] + m1.m[0][2]*m2.m[2][0];
00686         m0.m[0][1] = m1.m[0][0]*m2.m[0][1] + m1.m[0][1]*m2.m[1][1] + m1.m[0][2]*m2.m[2][1];
00687         m0.m[0][2] = m1.m[0][0]*m2.m[0][2] + m1.m[0][1]*m2.m[1][2] + m1.m[0][2]*m2.m[2][2];
00688         m0.m[1][0] = m1.m[1][0]*m2.m[0][0] + m1.m[1][1]*m2.m[1][0] + m1.m[1][2]*m2.m[2][0];
00689         m0.m[1][1] = m1.m[1][0]*m2.m[0][1] + m1.m[1][1]*m2.m[1][1] + m1.m[1][2]*m2.m[2][1];
00690         m0.m[1][2] = m1.m[1][0]*m2.m[0][2] + m1.m[1][1]*m2.m[1][2] + m1.m[1][2]*m2.m[2][2];
00691         m0.m[2][0] = m1.m[2][0]*m2.m[0][0] + m1.m[2][1]*m2.m[1][0] + m1.m[2][2]*m2.m[2][0];
00692         m0.m[2][1] = m1.m[2][0]*m2.m[0][1] + m1.m[2][1]*m2.m[1][1] + m1.m[2][2]*m2.m[2][1];
00693         m0.m[2][2] = m1.m[2][0]*m2.m[0][2] + m1.m[2][1]*m2.m[1][2] + m1.m[2][2]*m2.m[2][2];
00694 }
00695 
00696 void mat33_mult(mat33_t &mr, const real_t n)
00697 {
00698 
00699         for(unsigned int r=0; r<3; r++)
00700                 for(unsigned int c=0; c<3; c++)
00701                 {
00702                         mr.m[r][c] *= n;
00703                 }
00704 }
00705 
00706 void mat33_transpose(mat33_t &t, const mat33_t m)
00707 {
00708         for(unsigned int r=0; r<3; r++)
00709                 for(unsigned int c=0; c<3; c++)
00710                 {
00711                         t.m[r][c] = m.m[c][r];
00712                 }
00713 }
00714 
00715 void vec3_mult(vec3_t &v0, const mat33_t &m1, const vec3_t &v2)
00716 {
00717         v0.v[0] = m1.m[0][0]*v2.v[0] + m1.m[0][1]*v2.v[1] + m1.m[0][2]*v2.v[2];
00718         v0.v[1] = m1.m[1][0]*v2.v[0] + m1.m[1][1]*v2.v[1] + m1.m[1][2]*v2.v[2];
00719         v0.v[2] = m1.m[2][0]*v2.v[0] + m1.m[2][1]*v2.v[1] + m1.m[2][2]*v2.v[2];
00720 }
00721 
00722 void vec3_array_mult(vec3_array &va, const mat33_t &m, const vec3_array &vb)
00723 {
00724         va.clear();
00725         va.resize(vb.size());
00726         for(unsigned int i=0; i<vb.size(); i++)
00727         {
00728                 vec3_mult(va.at(i),m,vb.at(i));
00729         }
00730 }
00731 
00732 void mat33_svd2(mat33_t &u, mat33_t &s, mat33_t &v, const mat33_t &m)
00733 {
00734         mat33_clear(u);
00735         mat33_clear(v);
00736 
00737         double** m_ptr = mat33_to_double_pptr(m);
00738         double** v_ptr = mat33_to_double_pptr(v);
00739 
00740         vec3_t q;
00741         vec3_clear(q);
00742         double*  q_ptr = vec3_to_double_ptr(q);
00743 
00744         /*int ret =*/ svdcmp(m_ptr, 3, 3, q_ptr, v_ptr);
00745 
00746         mat33_from_double_pptr(u,m_ptr);
00747         mat33_from_double_pptr(v,v_ptr);
00748         vec3_from_double_ptr(q,q_ptr);
00749 
00750         mat33_clear(s);
00751         s.m[0][0] = (real_t)q_ptr[0];
00752         s.m[1][1] = (real_t)q_ptr[1];
00753         s.m[2][2] = (real_t)q_ptr[2];
00754 
00755         free_double_pptr(&m_ptr);
00756         free_double_pptr(&v_ptr);
00757         free_double_ptr(&q_ptr);
00758 }
00759 
00760 void quat_mult(quat_t &q, const real_t s)
00761 {
00762         vec3_mult(q.v,s);
00763         q.s *= s;
00764 }
00765 
00766 real_t quat_norm(const quat_t &q)
00767 {
00768         const real_t f_vn = vec3_norm(q.v);
00769         return(_sqrt((f_vn*f_vn) + (q.s*q.s)));
00770 
00771 }
00772 
00773 void mat33_from_quat(mat33_t &m, const quat_t &q)
00774 {
00775         const real_t a = q.s;
00776         const real_t b = q.v.v[0];
00777         const real_t c = q.v.v[1];
00778         const real_t d = q.v.v[2];
00779 
00780         m.m[0][0] = (a*a)+(b*b)-(c*c)-(d*d);
00781         m.m[0][1] = real_t(2.0f)*(b*c-a*d);
00782         m.m[0][2] = real_t(2.0f)*(b*d+a*c);
00783 
00784         m.m[1][0] = real_t(2.0f)*(b*c+a*d);
00785         m.m[1][1] = (a*a)+(c*c)-(b*b)-(d*d);
00786         m.m[1][2] = real_t(2.0f)*(c*d-a*b);
00787 
00788         m.m[2][0] = real_t(2.0f)*(b*d-a*c);
00789         m.m[2][1] = real_t(2.0f)*(c*d+a*b);
00790         m.m[2][2] = (a*a)+(d*d)-(b*b)-(c*c);
00791 }
00792 
00793 // ===========================================================================================
00794 void normRv(vec3_t &n, const vec3_t &v)
00795 {
00796         vec3_t _v1;
00797         vec3_copy(_v1,v);
00798         vec3_mult(_v1,_v1);
00799         real_t l = 1.0f / _sqrt(_v1.v[0] + _v1.v[1] + _v1.v[2]);
00800         vec3_copy(n,v);
00801         vec3_mult(n,l);
00802 }
00803 
00804 // ===========================================================================================
00805 void normRv(vec3_array &normR_v, const vec3_array &v)
00806 {
00807         normR_v.assign(v.begin(),v.end());
00808         vec3_array_pow2(normR_v);
00809         scalar_array _l;
00810         vec3_array_sum(_l,normR_v);
00811 
00812         for(scalar_array::iterator iter = _l.begin();
00813                 iter != _l.end(); iter++)
00814         {
00815                 (*iter) = 1.0f / _sqrt(*iter);
00816         }
00817 
00818         normR_v.assign(v.begin(),v.end());
00819         vec3_array_mult(normR_v,_l);
00820 }
00821 
00822 // ===========================================================================================
00823 int solve_polynomial(scalar_array &r_sol, const scalar_array &coefficients)
00824 {
00825         if(coefficients.size() != 5) return(0);
00826         r_sol.clear();
00827 
00828         double dd[5] = {(double)coefficients[0],
00829                 (double)coefficients[1],
00830                 (double)coefficients[2],
00831                 (double)coefficients[3],
00832                 (double)coefficients[4] };
00833 
00834         double sol[4] = {0,0,0,0};
00835         double soli[4] = {0,0,0,0};
00836         int n_sol = 0;
00837         quartic(dd, sol, soli, &n_sol);
00838 
00839         if(n_sol <= 0) return(0); // assert(false);
00840 
00841         r_sol.resize(n_sol);
00842         for(int i=0; i<n_sol; i++) r_sol[i] = (real_t)sol[i];
00843         return(n_sol);
00844 }
00845 // ===========================================================================================
00846 
00847 void scalar_array_pow(scalar_array &sa, const real_t f)
00848 {
00849         for(unsigned int i=0; i<sa.size(); i++) sa.at(i) = _pow(sa[i],f);
00850 }
00851 
00852 void scalar_array_negate(scalar_array &sa)
00853 {
00854         for(unsigned int i=0; i<sa.size(); i++) sa.at(i) = - sa.at(i);
00855 }
00856 
00857 void scalar_array_assign(scalar_array &sa,
00858                                                  const  real_t f,
00859                                                  const unsigned int sz)
00860 {
00861         sa.clear();
00862         sa.resize(sz);
00863         for(unsigned int i=0; i<sz; i++) sa.at(i) = f;
00864 }
00865 
00866 
00867 void scalar_array_add(scalar_array &sa, const scalar_array &sb)
00868 {
00869         assert(sa.size() == sb.size());
00870         for(unsigned int i=0; i<(unsigned int)sa.size(); i++)
00871                 sa.at(i) = sa.at(i) + sb.at(i);
00872 }
00873 
00874 void scalar_array_clear(scalar_array &sa)
00875 {
00876         for(unsigned int i=0; i<(unsigned int)sa.size(); i++) sa.at(i) = 0.;
00877 }
00878 
00879 void scalar_array_atan2(scalar_array &sa, 
00880                                                 const scalar_array &sb,
00881                                                 const scalar_array &sc)
00882 {
00883         assert(sb.size() == sc.size());
00884         sa.clear();
00885         sa.resize(sb.size());
00886         for(unsigned int i=0; i<(unsigned int)sb.size(); i++)
00887                 sa[i] = _atan2(sb[i],sc[i]);
00888 }
00889 
00890 void _dbg_scalar_array_print(const scalar_array &sa, char* name)
00891 {
00892         for(unsigned int i=0; i<sa.size(); i++)
00893                 printf("%s.at(%i):  [ %e ]\n",name,i,sa[i]);
00894 }
00895 
00896 void scalar_array_div(scalar_array &sa, real_t f)
00897 {
00898         for(unsigned int i=0; i<(unsigned int)sa.size(); i++)
00899         {
00900                 sa.at(i) /= f;
00901         }
00902 }
00903 
00904 void scalar_array_div(scalar_array &sa, const scalar_array &sb)
00905 {
00906         assert(sa.size() == sb.size());
00907         for(unsigned int i=0; i<(unsigned int)sa.size(); i++)
00908                 sa[i] /= sb[i];
00909 }
00910 
00911 void scalar_array_mult(scalar_array &sa, real_t f)
00912 {
00913         for(unsigned int i=0; i<(unsigned int)sa.size(); i++)
00914         {
00915                 sa.at(i) *= f;
00916         }
00917 }
00918 
00919 void scalar_array_add(scalar_array &sa, real_t f)
00920 {
00921         for(unsigned int i=0; i<(unsigned int)sa.size(); i++)
00922         {
00923                 sa.at(i) += f;
00924         }
00925 }
00926 
00927 void scalar_array_sub(scalar_array &sa, real_t f)
00928 {
00929         for(unsigned int i=0; i<(unsigned int)sa.size(); i++)
00930         {
00931                 sa.at(i) -= f;
00932         }
00933 }
00934 
00935 void mat33_pow2(mat33_t &m)
00936 {
00937         for(unsigned int r=0; r<3; r++)
00938                 for(unsigned int c=0; c<3; c++)
00939                 {
00940                         m.m[r][c] *= m.m[r][c];
00941                 }
00942 }
00943 
00944 
00945 void _dbg_vec3_fprint(void* fp, const vec3_t &v, char* name)
00946 {
00947         fprintf((FILE*)fp,"%s:  [ ",name);
00948         for(unsigned int c=0; c<3; c++)
00949         {
00950                 fprintf((FILE*)fp,"%.4f ",v.v[c]);
00951         }
00952         fprintf((FILE*)fp,"]\n");
00953 }
00954 
00955 void _dbg_mat33_fprint(void* fp, const mat33_t &m, char* name)
00956 {
00957         fprintf((FILE*)fp,"%s:\n",name);
00958         for(unsigned int r=0; r<3; r++)
00959         {
00960                 fprintf((FILE*)fp,"[ ");
00961                 for(unsigned int c=0; c<3; c++)
00962                 {
00963                         fprintf((FILE*)fp,"%.4f ",m.m[r][c]);
00964                 }
00965                 fprintf((FILE*)fp,"]\n");
00966         }
00967 }
00968 
00969 
00970 } // namespace rpp
00971 
00972 
00973 #endif //_NO_LIBRPP_


aruco_pose
Author(s): Julian Brunner
autogenerated on Mon Oct 6 2014 08:32:34