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


tuw_artoolkitplus
Author(s): Markus Bader
autogenerated on Sun May 29 2016 02:50:12