00001 #include <math.h>
00002 #include <stdio.h>
00003 #include <malloc.h>
00004
00005
00006
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
00057 if( y0 < 0 || y0 >= ysize ) continue;
00058
00059 for( i = -50; i <= 50; i++ ) {
00060 x0 = dist_factor[0] + i;
00061
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 }