calib_inp.cpp
Go to the documentation of this file.
00001 #include <math.h>
00002 #include <stdio.h>
00003 #include <malloc.h>
00004 //#include <AR/ar.h>
00005 //#include <AR/param.h>
00006 //#include <AR/matrix.h>
00007 #include <ARToolKitPlus/Tracker.h>
00008 #include "calib_camera.h"
00009 
00010 extern ARToolKitPlus::Tracker *theTracker;
00011 
00012 static int calc_inp2( CALIB_PATT_T *patt, CALIB_COORD_T *screen, ARFloat *pos2d, ARFloat *pos3d,
00013                       ARFloat dist_factor[4], ARFloat x0, ARFloat y0, ARFloat f[2], ARFloat *err );
00014 static void get_cpara( CALIB_COORD_T *world, CALIB_COORD_T *screen, int num, ARFloat para[3][3] );
00015 static int  get_fl( ARFloat *p , ARFloat *q, int num, ARFloat f[2] );
00016 static int  check_rotation( ARFloat rot[2][3] );
00017 
00018 int calc_inp( CALIB_PATT_T *patt, ARFloat dist_factor[4], int xsize, int ysize, ARFloat mat[3][4] )
00019 {
00020     CALIB_COORD_T  *screen, *sp;
00021     ARFloat         *pos2d, *pos3d, *pp;
00022     ARFloat         f[2];
00023     ARFloat         x0, y0;
00024     ARFloat         err, minerr;
00025     int            res;
00026     int            i, j, k;
00027 
00028     sp = screen = (CALIB_COORD_T *)malloc(sizeof(CALIB_COORD_T) * patt->h_num * patt->v_num * patt->loop_num);
00029     pp = pos2d = (ARFloat *)malloc(sizeof(ARFloat) * patt->h_num * patt->v_num * patt->loop_num * 2);
00030     pos3d = (ARFloat *)malloc(sizeof(ARFloat) * patt->h_num * patt->v_num * 2);
00031     for( k = 0; k < patt->loop_num; k++ ) {
00032         for( j = 0; j < patt->v_num; j++ ) {
00033             for( i = 0; i < patt->h_num; i++ ) {
00034                 ARToolKitPlus::Tracker::arParamObserv2Ideal( dist_factor, 
00035                                      patt->point[k][j*patt->h_num+i].x_coord,
00036                                      patt->point[k][j*patt->h_num+i].y_coord,
00037                                      &(sp->x_coord), &(sp->y_coord) );
00038                 *(pp++) = sp->x_coord;
00039                 *(pp++) = sp->y_coord;
00040                 sp++;
00041             }
00042         }
00043     }
00044     pp = pos3d;
00045     for( j = 0; j < patt->v_num; j++ ) {
00046         for( i = 0; i < patt->h_num; i++ ) {
00047             *(pp++) = patt->world_coord[j*patt->h_num+i].x_coord;
00048             *(pp++) = patt->world_coord[j*patt->h_num+i].y_coord;
00049         }
00050     }
00051 
00052     minerr = (ARFloat)100000000000000000000000.0;
00053     for( j = -50; j <= 50; j++ ) {
00054         printf("-- loop:%d --\n", j);
00055         y0 = dist_factor[1] + j;
00056 /*      y0 = ysize/2 + j;   */
00057         if( y0 < 0 || y0 >= ysize ) continue;
00058 
00059         for( i = -50; i <= 50; i++ ) {
00060             x0 = dist_factor[0] + i;
00061 /*          x0 = xsize/2 + i;  */
00062             if( x0 < 0 || x0 >= xsize ) continue;
00063 
00064             res = calc_inp2( patt, screen, pos2d, pos3d, dist_factor, x0, y0, f, &err );
00065             if( res < 0 ) continue;
00066             if( err < minerr ) {
00067                 printf("F = (%f,%f), Center = (%f,%f): err = %f\n", f[0], f[1], x0, y0, err);
00068                 minerr = err;
00069 
00070                 mat[0][0] = f[0];
00071                 mat[0][1] = 0.0;
00072                 mat[0][2] = x0;
00073                 mat[0][3] = 0.0;
00074                 mat[1][0] = 0.0;
00075                 mat[1][1] = f[1];
00076                 mat[1][2] = y0;
00077                 mat[1][3] = 0.0;
00078                 mat[2][0] = 0.0;
00079                 mat[2][1] = 0.0;
00080                 mat[2][2] = 1.0;
00081                 mat[2][3] = 0.0;
00082             }
00083         }
00084     }
00085 
00086     free( screen );
00087     free( pos2d );
00088     free( pos3d );
00089 
00090     if( minerr >= 100.0 ) return -1;
00091     return 0;
00092 }
00093 
00094 
00095 static int calc_inp2 ( CALIB_PATT_T *patt, CALIB_COORD_T *screen, ARFloat *pos2d, ARFloat *pos3d,
00096                       ARFloat dist_factor[4], ARFloat x0, ARFloat y0, ARFloat f[2], ARFloat *err )
00097 {
00098     ARFloat  x1, y1, x2, y2;
00099     ARFloat  p[LOOP_MAX], q[LOOP_MAX];
00100     ARFloat  para[3][3];
00101     ARFloat  rot[3][3], rot2[3][3];
00102     ARFloat  cpara[3][4], conv[3][4];
00103     ARFloat  ppos2d[4][2], ppos3d[4][2];
00104     ARFloat  d, werr, werr2;
00105     int     i, j, k, l;
00106 
00107     for( i =  0; i < patt->loop_num; i++ ) {
00108         get_cpara( patt->world_coord, &(screen[i*patt->h_num*patt->v_num]),
00109                    patt->h_num*patt->v_num, para );
00110         x1 = para[0][0] / para[2][0];
00111         y1 = para[1][0] / para[2][0];
00112         x2 = para[0][1] / para[2][1];
00113         y2 = para[1][1] / para[2][1];
00114 
00115         p[i] = (x1 - x0)*(x2 - x0);
00116         q[i] = (y1 - y0)*(y2 - y0);
00117     }
00118     if( get_fl(p, q, patt->loop_num, f) < 0 ) return -1;
00119 
00120     cpara[0][0] = f[0];
00121     cpara[0][1] = 0.0;
00122     cpara[0][2] = x0;
00123     cpara[0][3] = 0.0;
00124     cpara[1][0] = 0.0;
00125     cpara[1][1] = f[1];
00126     cpara[1][2] = y0;
00127     cpara[1][3] = 0.0;
00128     cpara[2][0] = 0.0;
00129     cpara[2][1] = 0.0;
00130     cpara[2][2] = 1.0;
00131     cpara[2][3] = 0.0;
00132 
00133     werr = 0.0;
00134     for( i =  0; i < patt->loop_num; i++ ) {
00135         get_cpara( patt->world_coord, &(screen[i*patt->h_num*patt->v_num]),
00136                    patt->h_num*patt->v_num, para );
00137         rot[0][0] = (para[0][0] - x0*para[2][0]) / f[0];
00138         rot[0][1] = (para[1][0] - y0*para[2][0]) / f[1];
00139         rot[0][2] = para[2][0];
00140         d = (ARFloat)sqrt( rot[0][0]*rot[0][0] + rot[0][1]*rot[0][1] + rot[0][2]*rot[0][2] );
00141         rot[0][0] /= d;
00142         rot[0][1] /= d;
00143         rot[0][2] /= d;
00144         rot[1][0] = (para[0][1] - x0*para[2][1]) / f[0];
00145         rot[1][1] = (para[1][1] - y0*para[2][1]) / f[1];
00146         rot[1][2] = para[2][1];
00147         d = (ARFloat)sqrt( rot[1][0]*rot[1][0] + rot[1][1]*rot[1][1] + rot[1][2]*rot[1][2] );
00148         rot[1][0] /= d;
00149         rot[1][1] /= d;
00150         rot[1][2] /= d;
00151         check_rotation( rot );
00152         rot[2][0] = rot[0][1]*rot[1][2] - rot[0][2]*rot[1][1];
00153         rot[2][1] = rot[0][2]*rot[1][0] - rot[0][0]*rot[1][2];
00154         rot[2][2] = rot[0][0]*rot[1][1] - rot[0][1]*rot[1][0];
00155         d = (ARFloat)sqrt( rot[2][0]*rot[2][0] + rot[2][1]*rot[2][1] + rot[2][2]*rot[2][2] );
00156         rot[2][0] /= d;
00157         rot[2][1] /= d;
00158         rot[2][2] /= d;
00159         rot2[0][0] = rot[0][0];
00160         rot2[1][0] = rot[0][1];
00161         rot2[2][0] = rot[0][2];
00162         rot2[0][1] = rot[1][0];
00163         rot2[1][1] = rot[1][1];
00164         rot2[2][1] = rot[1][2];
00165         rot2[0][2] = rot[2][0];
00166         rot2[1][2] = rot[2][1];
00167         rot2[2][2] = rot[2][2];
00168 
00169         ppos2d[0][0] = pos2d[i*patt->h_num*patt->v_num*2 + 0];
00170         ppos2d[0][1] = pos2d[i*patt->h_num*patt->v_num*2 + 1];
00171         ppos2d[1][0] = pos2d[i*patt->h_num*patt->v_num*2 + (patt->h_num-1)*2 + 0];
00172         ppos2d[1][1] = pos2d[i*patt->h_num*patt->v_num*2 + (patt->h_num-1)*2 + 1];
00173         ppos2d[2][0] = pos2d[i*patt->h_num*patt->v_num*2 + (patt->h_num*(patt->v_num-1))*2 + 0];
00174         ppos2d[2][1] = pos2d[i*patt->h_num*patt->v_num*2 + (patt->h_num*(patt->v_num-1))*2 + 1];
00175         ppos2d[3][0] = pos2d[i*patt->h_num*patt->v_num*2 + (patt->h_num*patt->v_num-1)*2 + 0];
00176         ppos2d[3][1] = pos2d[i*patt->h_num*patt->v_num*2 + (patt->h_num*patt->v_num-1)*2 + 1];
00177         ppos3d[0][0] = pos3d[0];
00178         ppos3d[0][1] = pos3d[1];
00179         ppos3d[1][0] = pos3d[(patt->h_num-1)*2 + 0];
00180         ppos3d[1][1] = pos3d[(patt->h_num-1)*2 + 1];
00181         ppos3d[2][0] = pos3d[(patt->h_num*(patt->v_num-1))*2 + 0];
00182         ppos3d[2][1] = pos3d[(patt->h_num*(patt->v_num-1))*2 + 1];
00183         ppos3d[3][0] = pos3d[(patt->h_num*patt->v_num-1)*2 + 0];
00184         ppos3d[3][1] = pos3d[(patt->h_num*patt->v_num-1)*2 + 1];
00185 
00186         for( j = 0; j < 5; j++ ) {
00187             theTracker->setFittingMode(AR_FITTING_TO_IDEAL);
00188             werr2 = theTracker->arGetTransMat3( rot2, ppos2d, ppos3d, 4, conv, dist_factor, cpara );
00189             for( k = 0; k < 3; k++ ) {
00190             for( l = 0; l < 3; l++ ) {
00191                 rot2[k][l] = conv[k][l];
00192             }}
00193         }
00194         werr += werr2;
00195 
00196     }
00197     *err = (ARFloat)sqrt( werr / patt->loop_num );
00198 
00199     return 0;
00200 }
00201 
00202 static void get_cpara( CALIB_COORD_T *world, CALIB_COORD_T *screen, int num, ARFloat para[3][3] )
00203 {
00204         ARToolKitPlus::ARMat   *a, *b, *c;
00205         ARToolKitPlus::ARMat   *at, *aa, res;
00206     int     i;
00207 
00208     a = ARToolKitPlus::Matrix::alloc( num*2, 8 );
00209     b = ARToolKitPlus::Matrix::alloc( num*2, 1 );
00210     c = ARToolKitPlus::Matrix::alloc( 8, num*2 );
00211     at = ARToolKitPlus::Matrix::alloc( 8, num*2 );
00212     aa = ARToolKitPlus::Matrix::alloc( 8, 8 );
00213     for( i = 0; i < num; i++ ) {
00214         a->m[i*16+0]  = world[i].x_coord;
00215         a->m[i*16+1]  = world[i].y_coord;
00216         a->m[i*16+2]  = 1.0;
00217         a->m[i*16+3]  = 0.0;
00218         a->m[i*16+4]  = 0.0;
00219         a->m[i*16+5]  = 0.0;
00220         a->m[i*16+6]  = -world[i].x_coord * screen[i].x_coord;
00221         a->m[i*16+7]  = -world[i].y_coord * screen[i].x_coord;
00222         a->m[i*16+8]  = 0.0;
00223         a->m[i*16+9]  = 0.0;
00224         a->m[i*16+10] = 0.0;
00225         a->m[i*16+11] = world[i].x_coord;
00226         a->m[i*16+12] = world[i].y_coord;
00227         a->m[i*16+13] = 1.0;
00228         a->m[i*16+14] = -world[i].x_coord * screen[i].y_coord;
00229         a->m[i*16+15] = -world[i].y_coord * screen[i].y_coord;
00230         b->m[i*2+0] = screen[i].x_coord;
00231         b->m[i*2+1] = screen[i].y_coord;
00232     }
00233     ARToolKitPlus::Matrix::trans( at, a );
00234     ARToolKitPlus::Matrix::mul( aa, at, a );
00235     ARToolKitPlus::Matrix::selfInv( aa );
00236     ARToolKitPlus::Matrix::mul( c, aa, at );
00237     res.row = 8;
00238     res.clm = 1;
00239     res.m = &(para[0][0]);
00240     ARToolKitPlus::Matrix::mul( &res, c, b );
00241     para[2][2] = 1.0;
00242 
00243     ARToolKitPlus::Matrix::free( a );
00244     ARToolKitPlus::Matrix::free( b );
00245     ARToolKitPlus::Matrix::free( c );
00246     ARToolKitPlus::Matrix::free( at );
00247     ARToolKitPlus::Matrix::free( aa );
00248 }
00249 
00250 static int get_fl( ARFloat *p , ARFloat *q, int num, ARFloat f[2] )
00251 {
00252         ARToolKitPlus::ARMat   *a, *b, *c;
00253     ARToolKitPlus::ARMat   *at, *aa, *res;
00254     int     i;
00255 
00256 #if 1
00257     a = ARToolKitPlus::Matrix::alloc( num, 2 );
00258     b = ARToolKitPlus::Matrix::alloc( num, 1 );
00259     c = ARToolKitPlus::Matrix::alloc( 2, num );
00260     at = ARToolKitPlus::Matrix::alloc( 2, num );
00261     aa = ARToolKitPlus::Matrix::alloc( 2, 2 );
00262     res = ARToolKitPlus::Matrix::alloc( 2, 1 );
00263     for( i = 0; i < num; i++ ) {
00264         a->m[i*2+0] = *(p++);
00265         a->m[i*2+1] = *(q++);
00266         b->m[i]     = -1.0;
00267     }
00268 #else
00269     a = ARToolKitPlus::Matrix::alloc( num-1, 2 );
00270     b = ARToolKitPlus::Matrix::alloc( num-1, 1 );
00271     c = ARToolKitPlus::Matrix::alloc( 2, num-1 );
00272     at = ARToolKitPlus::Matrix::alloc( 2, num-1 );
00273     aa = ARToolKitPlus::Matrix::alloc( 2, 2 );
00274     res = ARToolKitPlus::Matrix::alloc( 2, 1 );
00275     p++; q++;
00276     for( i = 0; i < num-1; i++ ) {
00277         a->m[i*2+0] = *(p++);
00278         a->m[i*2+1] = *(q++);
00279         b->m[i]     = -1.0;
00280     }
00281 #endif
00282     ARToolKitPlus::Matrix::trans( at, a );
00283     ARToolKitPlus::Matrix::mul( aa, at, a );
00284     ARToolKitPlus::Matrix::selfInv( aa );
00285     ARToolKitPlus::Matrix::mul( c, aa, at );
00286     ARToolKitPlus::Matrix::mul( res, c, b );
00287 
00288     if( res->m[0] < 0 || res->m[1] < 0 ) return -1;
00289 
00290     f[0] = (ARFloat)sqrt( 1.0 / res->m[0] );
00291     f[1] = (ARFloat)sqrt( 1.0 / res->m[1] );
00292 
00293     ARToolKitPlus::Matrix::free( a );
00294     ARToolKitPlus::Matrix::free( b );
00295     ARToolKitPlus::Matrix::free( c );
00296     ARToolKitPlus::Matrix::free( at );
00297     ARToolKitPlus::Matrix::free( aa );
00298     ARToolKitPlus::Matrix::free( res );
00299 
00300     return 0;
00301 }
00302 
00303 static int check_rotation( ARFloat rot[2][3] )
00304 {
00305     ARFloat  v1[3], v2[3], v3[3];
00306     ARFloat  ca, cb, k1, k2, k3, k4;
00307     ARFloat  a, b, c, d;
00308     ARFloat  p1, q1, r1;
00309     ARFloat  p2, q2, r2;
00310     ARFloat  p3, q3, r3;
00311     ARFloat  p4, q4, r4;
00312     ARFloat  w;
00313     ARFloat  e1, e2, e3, e4;
00314     int     f;
00315 
00316     v1[0] = rot[0][0];
00317     v1[1] = rot[0][1];
00318     v1[2] = rot[0][2];
00319     v2[0] = rot[1][0];
00320     v2[1] = rot[1][1];
00321     v2[2] = rot[1][2];
00322     v3[0] = v1[1]*v2[2] - v1[2]*v2[1];
00323     v3[1] = v1[2]*v2[0] - v1[0]*v2[2];
00324     v3[2] = v1[0]*v2[1] - v1[1]*v2[0];
00325     w = (ARFloat)sqrt( v3[0]*v3[0]+v3[1]*v3[1]+v3[2]*v3[2] );
00326     if( w == 0.0 ) return -1;
00327     v3[0] /= w;
00328     v3[1] /= w;
00329     v3[2] /= w;
00330 
00331     cb = v1[0]*v2[0] + v1[1]*v2[1] + v1[2]*v2[2];
00332     if( cb < 0 ) cb *= -1.0;
00333     ca = (ARFloat)(sqrt(cb+1.0) + sqrt(1.0-cb)) * (ARFloat)0.5;
00334 
00335     if( v3[1]*v1[0] - v1[1]*v3[0] != 0.0 ) {
00336         f = 0;
00337     }
00338     else {
00339         if( v3[2]*v1[0] - v1[2]*v3[0] != 0.0 ) {
00340             w = v1[1]; v1[1] = v1[2]; v1[2] = w;
00341             w = v3[1]; v3[1] = v3[2]; v3[2] = w;
00342             f = 1;
00343         }
00344         else {
00345             w = v1[0]; v1[0] = v1[2]; v1[2] = w;
00346             w = v3[0]; v3[0] = v3[2]; v3[2] = w;
00347             f = 2;
00348         }
00349     }
00350     if( v3[1]*v1[0] - v1[1]*v3[0] == 0.0 ) return -1;
00351     k1 = (v1[1]*v3[2] - v3[1]*v1[2]) / (v3[1]*v1[0] - v1[1]*v3[0]);
00352     k2 = (v3[1] * ca) / (v3[1]*v1[0] - v1[1]*v3[0]);
00353     k3 = (v1[0]*v3[2] - v3[0]*v1[2]) / (v3[0]*v1[1] - v1[0]*v3[1]);
00354     k4 = (v3[0] * ca) / (v3[0]*v1[1] - v1[0]*v3[1]);
00355 
00356     a = k1*k1 + k3*k3 + 1;
00357     b = k1*k2 + k3*k4;
00358     c = k2*k2 + k4*k4 - 1;
00359 
00360     d = b*b - a*c;
00361     if( d < 0 ) return -1;
00362     r1 = (-b + (ARFloat)sqrt(d))/a;
00363     p1 = k1*r1 + k2;
00364     q1 = k3*r1 + k4;
00365     r2 = (-b - (ARFloat)sqrt(d))/a;
00366     p2 = k1*r2 + k2;
00367     q2 = k3*r2 + k4;
00368     if( f == 1 ) {
00369         w = q1; q1 = r1; r1 = w;
00370         w = q2; q2 = r2; r2 = w;
00371         w = v1[1]; v1[1] = v1[2]; v1[2] = w;
00372         w = v3[1]; v3[1] = v3[2]; v3[2] = w;
00373         f = 0;
00374     }
00375     if( f == 2 ) {
00376         w = p1; p1 = r1; r1 = w;
00377         w = p2; p2 = r2; r2 = w;
00378         w = v1[0]; v1[0] = v1[2]; v1[2] = w;
00379         w = v3[0]; v3[0] = v3[2]; v3[2] = w;
00380         f = 0;
00381     }
00382 
00383     if( v3[1]*v2[0] - v2[1]*v3[0] != 0.0 ) {
00384         f = 0;
00385     }
00386     else {
00387         if( v3[2]*v2[0] - v2[2]*v3[0] != 0.0 ) {
00388             w = v2[1]; v2[1] = v2[2]; v2[2] = w;
00389             w = v3[1]; v3[1] = v3[2]; v3[2] = w;
00390             f = 1;
00391         }
00392         else {
00393             w = v2[0]; v2[0] = v2[2]; v2[2] = w;
00394             w = v3[0]; v3[0] = v3[2]; v3[2] = w;
00395             f = 2;
00396         }
00397     }
00398     if( v3[1]*v2[0] - v2[1]*v3[0] == 0.0 ) return -1;
00399     k1 = (v2[1]*v3[2] - v3[1]*v2[2]) / (v3[1]*v2[0] - v2[1]*v3[0]);
00400     k2 = (v3[1] * ca) / (v3[1]*v2[0] - v2[1]*v3[0]);
00401     k3 = (v2[0]*v3[2] - v3[0]*v2[2]) / (v3[0]*v2[1] - v2[0]*v3[1]);
00402     k4 = (v3[0] * ca) / (v3[0]*v2[1] - v2[0]*v3[1]);
00403 
00404     a = k1*k1 + k3*k3 + 1;
00405     b = k1*k2 + k3*k4;
00406     c = k2*k2 + k4*k4 - 1;
00407 
00408     d = b*b - a*c;
00409     if( d < 0 ) return -1;
00410     r3 = (-b + (ARFloat)sqrt(d))/a;
00411     p3 = k1*r3 + k2;
00412     q3 = k3*r3 + k4;
00413     r4 = (-b - (ARFloat)sqrt(d))/a;
00414     p4 = k1*r4 + k2;
00415     q4 = k3*r4 + k4;
00416     if( f == 1 ) {
00417         w = q3; q3 = r3; r3 = w;
00418         w = q4; q4 = r4; r4 = w;
00419         w = v2[1]; v2[1] = v2[2]; v2[2] = w;
00420         w = v3[1]; v3[1] = v3[2]; v3[2] = w;
00421         f = 0;
00422     }
00423     if( f == 2 ) {
00424         w = p3; p3 = r3; r3 = w;
00425         w = p4; p4 = r4; r4 = w;
00426         w = v2[0]; v2[0] = v2[2]; v2[2] = w;
00427         w = v3[0]; v3[0] = v3[2]; v3[2] = w;
00428         f = 0;
00429     }
00430 
00431     e1 = p1*p3+q1*q3+r1*r3; if( e1 < 0 ) e1 = -e1;
00432     e2 = p1*p4+q1*q4+r1*r4; if( e2 < 0 ) e2 = -e2;
00433     e3 = p2*p3+q2*q3+r2*r3; if( e3 < 0 ) e3 = -e3;
00434     e4 = p2*p4+q2*q4+r2*r4; if( e4 < 0 ) e4 = -e4;
00435     if( e1 < e2 ) {
00436         if( e1 < e3 ) {
00437             if( e1 < e4 ) {
00438                 rot[0][0] = p1;
00439                 rot[0][1] = q1;
00440                 rot[0][2] = r1;
00441                 rot[1][0] = p3;
00442                 rot[1][1] = q3;
00443                 rot[1][2] = r3;
00444             }
00445             else {
00446                 rot[0][0] = p2;
00447                 rot[0][1] = q2;
00448                 rot[0][2] = r2;
00449                 rot[1][0] = p4;
00450                 rot[1][1] = q4;
00451                 rot[1][2] = r4;
00452             }
00453         }
00454         else {
00455             if( e3 < e4 ) {
00456                 rot[0][0] = p2;
00457                 rot[0][1] = q2;
00458                 rot[0][2] = r2;
00459                 rot[1][0] = p3;
00460                 rot[1][1] = q3;
00461                 rot[1][2] = r3;
00462             }
00463             else {
00464                 rot[0][0] = p2;
00465                 rot[0][1] = q2;
00466                 rot[0][2] = r2;
00467                 rot[1][0] = p4;
00468                 rot[1][1] = q4;
00469                 rot[1][2] = r4;
00470             }
00471         }
00472     }
00473     else {
00474         if( e2 < e3 ) {
00475             if( e2 < e4 ) {
00476                 rot[0][0] = p1;
00477                 rot[0][1] = q1;
00478                 rot[0][2] = r1;
00479                 rot[1][0] = p4;
00480                 rot[1][1] = q4;
00481                 rot[1][2] = r4;
00482             }
00483             else {
00484                 rot[0][0] = p2;
00485                 rot[0][1] = q2;
00486                 rot[0][2] = r2;
00487                 rot[1][0] = p4;
00488                 rot[1][1] = q4;
00489                 rot[1][2] = r4;
00490             }
00491         }
00492         else {
00493             if( e3 < e4 ) {
00494                 rot[0][0] = p2;
00495                 rot[0][1] = q2;
00496                 rot[0][2] = r2;
00497                 rot[1][0] = p3;
00498                 rot[1][1] = q3;
00499                 rot[1][2] = r3;
00500             }
00501             else {
00502                 rot[0][0] = p2;
00503                 rot[0][1] = q2;
00504                 rot[0][2] = r2;
00505                 rot[1][0] = p4;
00506                 rot[1][1] = q4;
00507                 rot[1][2] = r4;
00508             }
00509         }
00510     }
00511 
00512     return 0;
00513 }


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