00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011 #define CHECK_CALC 0
00012
00013 #include <stdlib.h>
00014 #include <math.h>
00015 #include <AR/ar.h>
00016 #include <AR/matrix.h>
00017
00018 #define MD_PI 3.14159265358979323846
00019
00020 static int check_rotation( double rot[2][3] );
00021 static int check_dir( double dir[3], double st[2], double ed[2],
00022 double cpara[3][4] );
00023
00024 int arGetAngle( double rot[3][3], double *wa, double *wb, double *wc )
00025 {
00026 double a, b, c;
00027 double sina, cosa, sinb, cosb, sinc, cosc;
00028 #if CHECK_CALC
00029 double w[3];
00030 int i;
00031 for(i=0;i<3;i++) w[i] = rot[i][0];
00032 for(i=0;i<3;i++) rot[i][0] = rot[i][1];
00033 for(i=0;i<3;i++) rot[i][1] = rot[i][2];
00034 for(i=0;i<3;i++) rot[i][2] = w[i];
00035 #endif
00036
00037 if( rot[2][2] > 1.0 ) {
00038
00039 rot[2][2] = 1.0;
00040 }
00041 else if( rot[2][2] < -1.0 ) {
00042
00043 rot[2][2] = -1.0;
00044 }
00045 cosb = rot[2][2];
00046 b = acos( cosb );
00047 sinb = sin( b );
00048 if( b >= 0.000001 || b <= -0.000001) {
00049 cosa = rot[0][2] / sinb;
00050 sina = rot[1][2] / sinb;
00051 if( cosa > 1.0 ) {
00052
00053 cosa = 1.0;
00054 sina = 0.0;
00055 }
00056 if( cosa < -1.0 ) {
00057
00058 cosa = -1.0;
00059 sina = 0.0;
00060 }
00061 if( sina > 1.0 ) {
00062
00063 sina = 1.0;
00064 cosa = 0.0;
00065 }
00066 if( sina < -1.0 ) {
00067
00068 sina = -1.0;
00069 cosa = 0.0;
00070 }
00071 a = acos( cosa );
00072 if( sina < 0 ) a = -a;
00073
00074 sinc = (rot[2][1]*rot[0][2]-rot[2][0]*rot[1][2])
00075 / (rot[0][2]*rot[0][2]+rot[1][2]*rot[1][2]);
00076 cosc = -(rot[0][2]*rot[2][0]+rot[1][2]*rot[2][1])
00077 / (rot[0][2]*rot[0][2]+rot[1][2]*rot[1][2]);
00078 if( cosc > 1.0 ) {
00079
00080 cosc = 1.0;
00081 sinc = 0.0;
00082 }
00083 if( cosc < -1.0 ) {
00084
00085 cosc = -1.0;
00086 sinc = 0.0;
00087 }
00088 if( sinc > 1.0 ) {
00089
00090 sinc = 1.0;
00091 cosc = 0.0;
00092 }
00093 if( sinc < -1.0 ) {
00094
00095 sinc = -1.0;
00096 cosc = 0.0;
00097 }
00098 c = acos( cosc );
00099 if( sinc < 0 ) c = -c;
00100 }
00101 else {
00102 a = b = 0.0;
00103 cosa = cosb = 1.0;
00104 sina = sinb = 0.0;
00105 cosc = rot[0][0];
00106 sinc = rot[1][0];
00107 if( cosc > 1.0 ) {
00108
00109 cosc = 1.0;
00110 sinc = 0.0;
00111 }
00112 if( cosc < -1.0 ) {
00113
00114 cosc = -1.0;
00115 sinc = 0.0;
00116 }
00117 if( sinc > 1.0 ) {
00118
00119 sinc = 1.0;
00120 cosc = 0.0;
00121 }
00122 if( sinc < -1.0 ) {
00123
00124 sinc = -1.0;
00125 cosc = 0.0;
00126 }
00127 c = acos( cosc );
00128 if( sinc < 0 ) c = -c;
00129 }
00130
00131 *wa = a;
00132 *wb = b;
00133 *wc = c;
00134
00135 return 0;
00136 }
00137
00138 int arGetRot( double a, double b, double c, double rot[3][3] )
00139 {
00140 double sina, sinb, sinc;
00141 double cosa, cosb, cosc;
00142 #if CHECK_CALC
00143 double w[3];
00144 int i;
00145 #endif
00146
00147 sina = sin(a); cosa = cos(a);
00148 sinb = sin(b); cosb = cos(b);
00149 sinc = sin(c); cosc = cos(c);
00150 rot[0][0] = cosa*cosa*cosb*cosc+sina*sina*cosc+sina*cosa*cosb*sinc-sina*cosa*sinc;
00151 rot[0][1] = -cosa*cosa*cosb*sinc-sina*sina*sinc+sina*cosa*cosb*cosc-sina*cosa*cosc;
00152 rot[0][2] = cosa*sinb;
00153 rot[1][0] = sina*cosa*cosb*cosc-sina*cosa*cosc+sina*sina*cosb*sinc+cosa*cosa*sinc;
00154 rot[1][1] = -sina*cosa*cosb*sinc+sina*cosa*sinc+sina*sina*cosb*cosc+cosa*cosa*cosc;
00155 rot[1][2] = sina*sinb;
00156 rot[2][0] = -cosa*sinb*cosc-sina*sinb*sinc;
00157 rot[2][1] = cosa*sinb*sinc-sina*sinb*cosc;
00158 rot[2][2] = cosb;
00159
00160 #if CHECK_CALC
00161 for(i=0;i<3;i++) w[i] = rot[i][2];
00162 for(i=0;i<3;i++) rot[i][2] = rot[i][1];
00163 for(i=0;i<3;i++) rot[i][1] = rot[i][0];
00164 for(i=0;i<3;i++) rot[i][0] = w[i];
00165 #endif
00166
00167 return 0;
00168 }
00169
00170 int arGetNewMatrix( double a, double b, double c,
00171 double trans[3], double trans2[3][4],
00172 double cpara[3][4], double ret[3][4] )
00173 {
00174 double cpara2[3][4];
00175 double rot[3][3];
00176 int i, j;
00177
00178 arGetRot( a, b, c, rot );
00179
00180 if( trans2 != NULL ) {
00181 for( j = 0; j < 3; j++ ) {
00182 for( i = 0; i < 4; i++ ) {
00183 cpara2[j][i] = cpara[j][0] * trans2[0][i]
00184 + cpara[j][1] * trans2[1][i]
00185 + cpara[j][2] * trans2[2][i];
00186 }
00187 }
00188 }
00189 else {
00190 for( j = 0; j < 3; j++ ) {
00191 for( i = 0; i < 4; i++ ) {
00192 cpara2[j][i] = cpara[j][i];
00193 }
00194 }
00195 }
00196
00197 for( j = 0; j < 3; j++ ) {
00198 for( i = 0; i < 3; i++ ) {
00199 ret[j][i] = cpara2[j][0] * rot[0][i]
00200 + cpara2[j][1] * rot[1][i]
00201 + cpara2[j][2] * rot[2][i];
00202 }
00203 ret[j][3] = cpara2[j][0] * trans[0]
00204 + cpara2[j][1] * trans[1]
00205 + cpara2[j][2] * trans[2]
00206 + cpara2[j][3];
00207 }
00208
00209 return(0);
00210 }
00211
00212 int arGetInitRot( ARMarkerInfo *marker_info, double cpara[3][4], double rot[3][3] )
00213 {
00214 double wdir[3][3];
00215 double w, w1, w2, w3;
00216 int dir;
00217 int j;
00218
00219 dir = marker_info->dir;
00220
00221 for( j = 0; j < 2; j++ ) {
00222 w1 = marker_info->line[(4-dir+j)%4][0] * marker_info->line[(6-dir+j)%4][1]
00223 - marker_info->line[(6-dir+j)%4][0] * marker_info->line[(4-dir+j)%4][1];
00224 w2 = marker_info->line[(4-dir+j)%4][1] * marker_info->line[(6-dir+j)%4][2]
00225 - marker_info->line[(6-dir+j)%4][1] * marker_info->line[(4-dir+j)%4][2];
00226 w3 = marker_info->line[(4-dir+j)%4][2] * marker_info->line[(6-dir+j)%4][0]
00227 - marker_info->line[(6-dir+j)%4][2] * marker_info->line[(4-dir+j)%4][0];
00228
00229 wdir[j][0] = w1*(cpara[0][1]*cpara[1][2]-cpara[0][2]*cpara[1][1])
00230 + w2*cpara[1][1]
00231 - w3*cpara[0][1];
00232 wdir[j][1] = -w1*cpara[0][0]*cpara[1][2]
00233 + w3*cpara[0][0];
00234 wdir[j][2] = w1*cpara[0][0]*cpara[1][1];
00235 w = sqrt( wdir[j][0]*wdir[j][0]
00236 + wdir[j][1]*wdir[j][1]
00237 + wdir[j][2]*wdir[j][2] );
00238 wdir[j][0] /= w;
00239 wdir[j][1] /= w;
00240 wdir[j][2] /= w;
00241 }
00242
00243 if( check_dir(wdir[0], marker_info->vertex[(4-dir)%4],
00244 marker_info->vertex[(5-dir)%4], cpara) < 0 ) return -1;
00245 if( check_dir(wdir[1], marker_info->vertex[(7-dir)%4],
00246 marker_info->vertex[(4-dir)%4], cpara) < 0 ) return -1;
00247 if( check_rotation(wdir) < 0 ) return -1;
00248
00249 wdir[2][0] = wdir[0][1]*wdir[1][2] - wdir[0][2]*wdir[1][1];
00250 wdir[2][1] = wdir[0][2]*wdir[1][0] - wdir[0][0]*wdir[1][2];
00251 wdir[2][2] = wdir[0][0]*wdir[1][1] - wdir[0][1]*wdir[1][0];
00252 w = sqrt( wdir[2][0]*wdir[2][0]
00253 + wdir[2][1]*wdir[2][1]
00254 + wdir[2][2]*wdir[2][2] );
00255 wdir[2][0] /= w;
00256 wdir[2][1] /= w;
00257 wdir[2][2] /= w;
00258
00259
00260
00261
00262
00263
00264
00265
00266
00267
00268
00269
00270
00271 rot[0][0] = wdir[0][0];
00272 rot[1][0] = wdir[0][1];
00273 rot[2][0] = wdir[0][2];
00274 rot[0][1] = wdir[1][0];
00275 rot[1][1] = wdir[1][1];
00276 rot[2][1] = wdir[1][2];
00277 rot[0][2] = wdir[2][0];
00278 rot[1][2] = wdir[2][1];
00279 rot[2][2] = wdir[2][2];
00280
00281 return 0;
00282 }
00283
00284 static int check_dir( double dir[3], double st[2], double ed[2],
00285 double cpara[3][4] )
00286 {
00287 ARMat *mat_a;
00288 double world[2][3];
00289 double camera[2][2];
00290 double v[2][2];
00291 double h;
00292 int i, j;
00293
00294 mat_a = arMatrixAlloc( 3, 3 );
00295 for(j=0;j<3;j++) for(i=0;i<3;i++) mat_a->m[j*3+i] = cpara[j][i];
00296 arMatrixSelfInv( mat_a );
00297 world[0][0] = mat_a->m[0]*st[0]*10.0
00298 + mat_a->m[1]*st[1]*10.0
00299 + mat_a->m[2]*10.0;
00300 world[0][1] = mat_a->m[3]*st[0]*10.0
00301 + mat_a->m[4]*st[1]*10.0
00302 + mat_a->m[5]*10.0;
00303 world[0][2] = mat_a->m[6]*st[0]*10.0
00304 + mat_a->m[7]*st[1]*10.0
00305 + mat_a->m[8]*10.0;
00306 arMatrixFree( mat_a );
00307 world[1][0] = world[0][0] + dir[0];
00308 world[1][1] = world[0][1] + dir[1];
00309 world[1][2] = world[0][2] + dir[2];
00310
00311 for( i = 0; i < 2; i++ ) {
00312 h = cpara[2][0] * world[i][0]
00313 + cpara[2][1] * world[i][1]
00314 + cpara[2][2] * world[i][2];
00315 if( h == 0.0 ) return -1;
00316 camera[i][0] = (cpara[0][0] * world[i][0]
00317 + cpara[0][1] * world[i][1]
00318 + cpara[0][2] * world[i][2]) / h;
00319 camera[i][1] = (cpara[1][0] * world[i][0]
00320 + cpara[1][1] * world[i][1]
00321 + cpara[1][2] * world[i][2]) / h;
00322 }
00323
00324 v[0][0] = ed[0] - st[0];
00325 v[0][1] = ed[1] - st[1];
00326 v[1][0] = camera[1][0] - camera[0][0];
00327 v[1][1] = camera[1][1] - camera[0][1];
00328
00329 if( v[0][0]*v[1][0] + v[0][1]*v[1][1] < 0 ) {
00330 dir[0] = -dir[0];
00331 dir[1] = -dir[1];
00332 dir[2] = -dir[2];
00333 }
00334
00335 return 0;
00336 }
00337
00338 static int check_rotation( double rot[2][3] )
00339 {
00340 double v1[3], v2[3], v3[3];
00341 double ca, cb, k1, k2, k3, k4;
00342 double a, b, c, d;
00343 double p1, q1, r1;
00344 double p2, q2, r2;
00345 double p3, q3, r3;
00346 double p4, q4, r4;
00347 double w;
00348 double e1, e2, e3, e4;
00349 int f;
00350
00351 v1[0] = rot[0][0];
00352 v1[1] = rot[0][1];
00353 v1[2] = rot[0][2];
00354 v2[0] = rot[1][0];
00355 v2[1] = rot[1][1];
00356 v2[2] = rot[1][2];
00357 v3[0] = v1[1]*v2[2] - v1[2]*v2[1];
00358 v3[1] = v1[2]*v2[0] - v1[0]*v2[2];
00359 v3[2] = v1[0]*v2[1] - v1[1]*v2[0];
00360 w = sqrt( v3[0]*v3[0]+v3[1]*v3[1]+v3[2]*v3[2] );
00361 if( w == 0.0 ) return -1;
00362 v3[0] /= w;
00363 v3[1] /= w;
00364 v3[2] /= w;
00365
00366 cb = v1[0]*v2[0] + v1[1]*v2[1] + v1[2]*v2[2];
00367 if( cb < 0 ) cb *= -1.0;
00368 ca = (sqrt(cb+1.0) + sqrt(1.0-cb)) * 0.5;
00369
00370 if( v3[1]*v1[0] - v1[1]*v3[0] != 0.0 ) {
00371 f = 0;
00372 }
00373 else {
00374 if( v3[2]*v1[0] - v1[2]*v3[0] != 0.0 ) {
00375 w = v1[1]; v1[1] = v1[2]; v1[2] = w;
00376 w = v3[1]; v3[1] = v3[2]; v3[2] = w;
00377 f = 1;
00378 }
00379 else {
00380 w = v1[0]; v1[0] = v1[2]; v1[2] = w;
00381 w = v3[0]; v3[0] = v3[2]; v3[2] = w;
00382 f = 2;
00383 }
00384 }
00385 if( v3[1]*v1[0] - v1[1]*v3[0] == 0.0 ) return -1;
00386 k1 = (v1[1]*v3[2] - v3[1]*v1[2]) / (v3[1]*v1[0] - v1[1]*v3[0]);
00387 k2 = (v3[1] * ca) / (v3[1]*v1[0] - v1[1]*v3[0]);
00388 k3 = (v1[0]*v3[2] - v3[0]*v1[2]) / (v3[0]*v1[1] - v1[0]*v3[1]);
00389 k4 = (v3[0] * ca) / (v3[0]*v1[1] - v1[0]*v3[1]);
00390
00391 a = k1*k1 + k3*k3 + 1;
00392 b = k1*k2 + k3*k4;
00393 c = k2*k2 + k4*k4 - 1;
00394
00395 d = b*b - a*c;
00396 if( d < 0 ) return -1;
00397 r1 = (-b + sqrt(d))/a;
00398 p1 = k1*r1 + k2;
00399 q1 = k3*r1 + k4;
00400 r2 = (-b - sqrt(d))/a;
00401 p2 = k1*r2 + k2;
00402 q2 = k3*r2 + k4;
00403 if( f == 1 ) {
00404 w = q1; q1 = r1; r1 = w;
00405 w = q2; q2 = r2; r2 = w;
00406 w = v1[1]; v1[1] = v1[2]; v1[2] = w;
00407 w = v3[1]; v3[1] = v3[2]; v3[2] = w;
00408 f = 0;
00409 }
00410 if( f == 2 ) {
00411 w = p1; p1 = r1; r1 = w;
00412 w = p2; p2 = r2; r2 = w;
00413 w = v1[0]; v1[0] = v1[2]; v1[2] = w;
00414 w = v3[0]; v3[0] = v3[2]; v3[2] = w;
00415 f = 0;
00416 }
00417
00418 if( v3[1]*v2[0] - v2[1]*v3[0] != 0.0 ) {
00419 f = 0;
00420 }
00421 else {
00422 if( v3[2]*v2[0] - v2[2]*v3[0] != 0.0 ) {
00423 w = v2[1]; v2[1] = v2[2]; v2[2] = w;
00424 w = v3[1]; v3[1] = v3[2]; v3[2] = w;
00425 f = 1;
00426 }
00427 else {
00428 w = v2[0]; v2[0] = v2[2]; v2[2] = w;
00429 w = v3[0]; v3[0] = v3[2]; v3[2] = w;
00430 f = 2;
00431 }
00432 }
00433 if( v3[1]*v2[0] - v2[1]*v3[0] == 0.0 ) return -1;
00434 k1 = (v2[1]*v3[2] - v3[1]*v2[2]) / (v3[1]*v2[0] - v2[1]*v3[0]);
00435 k2 = (v3[1] * ca) / (v3[1]*v2[0] - v2[1]*v3[0]);
00436 k3 = (v2[0]*v3[2] - v3[0]*v2[2]) / (v3[0]*v2[1] - v2[0]*v3[1]);
00437 k4 = (v3[0] * ca) / (v3[0]*v2[1] - v2[0]*v3[1]);
00438
00439 a = k1*k1 + k3*k3 + 1;
00440 b = k1*k2 + k3*k4;
00441 c = k2*k2 + k4*k4 - 1;
00442
00443 d = b*b - a*c;
00444 if( d < 0 ) return -1;
00445 r3 = (-b + sqrt(d))/a;
00446 p3 = k1*r3 + k2;
00447 q3 = k3*r3 + k4;
00448 r4 = (-b - sqrt(d))/a;
00449 p4 = k1*r4 + k2;
00450 q4 = k3*r4 + k4;
00451 if( f == 1 ) {
00452 w = q3; q3 = r3; r3 = w;
00453 w = q4; q4 = r4; r4 = w;
00454 w = v2[1]; v2[1] = v2[2]; v2[2] = w;
00455 w = v3[1]; v3[1] = v3[2]; v3[2] = w;
00456 f = 0;
00457 }
00458 if( f == 2 ) {
00459 w = p3; p3 = r3; r3 = w;
00460 w = p4; p4 = r4; r4 = w;
00461 w = v2[0]; v2[0] = v2[2]; v2[2] = w;
00462 w = v3[0]; v3[0] = v3[2]; v3[2] = w;
00463 f = 0;
00464 }
00465
00466 e1 = p1*p3+q1*q3+r1*r3; if( e1 < 0 ) e1 = -e1;
00467 e2 = p1*p4+q1*q4+r1*r4; if( e2 < 0 ) e2 = -e2;
00468 e3 = p2*p3+q2*q3+r2*r3; if( e3 < 0 ) e3 = -e3;
00469 e4 = p2*p4+q2*q4+r2*r4; if( e4 < 0 ) e4 = -e4;
00470 if( e1 < e2 ) {
00471 if( e1 < e3 ) {
00472 if( e1 < e4 ) {
00473 rot[0][0] = p1;
00474 rot[0][1] = q1;
00475 rot[0][2] = r1;
00476 rot[1][0] = p3;
00477 rot[1][1] = q3;
00478 rot[1][2] = r3;
00479 }
00480 else {
00481 rot[0][0] = p2;
00482 rot[0][1] = q2;
00483 rot[0][2] = r2;
00484 rot[1][0] = p4;
00485 rot[1][1] = q4;
00486 rot[1][2] = r4;
00487 }
00488 }
00489 else {
00490 if( e3 < e4 ) {
00491 rot[0][0] = p2;
00492 rot[0][1] = q2;
00493 rot[0][2] = r2;
00494 rot[1][0] = p3;
00495 rot[1][1] = q3;
00496 rot[1][2] = r3;
00497 }
00498 else {
00499 rot[0][0] = p2;
00500 rot[0][1] = q2;
00501 rot[0][2] = r2;
00502 rot[1][0] = p4;
00503 rot[1][1] = q4;
00504 rot[1][2] = r4;
00505 }
00506 }
00507 }
00508 else {
00509 if( e2 < e3 ) {
00510 if( e2 < e4 ) {
00511 rot[0][0] = p1;
00512 rot[0][1] = q1;
00513 rot[0][2] = r1;
00514 rot[1][0] = p4;
00515 rot[1][1] = q4;
00516 rot[1][2] = r4;
00517 }
00518 else {
00519 rot[0][0] = p2;
00520 rot[0][1] = q2;
00521 rot[0][2] = r2;
00522 rot[1][0] = p4;
00523 rot[1][1] = q4;
00524 rot[1][2] = r4;
00525 }
00526 }
00527 else {
00528 if( e3 < e4 ) {
00529 rot[0][0] = p2;
00530 rot[0][1] = q2;
00531 rot[0][2] = r2;
00532 rot[1][0] = p3;
00533 rot[1][1] = q3;
00534 rot[1][2] = r3;
00535 }
00536 else {
00537 rot[0][0] = p2;
00538 rot[0][1] = q2;
00539 rot[0][2] = r2;
00540 rot[1][0] = p4;
00541 rot[1][1] = q4;
00542 rot[1][2] = r4;
00543 }
00544 }
00545 }
00546
00547 return 0;
00548 }
