diff options
Diffstat (limited to 'tags/Version Original/ARToolkit/lib/SRC/AR/arGetTransMat2.c')
| -rwxr-xr-x | tags/Version Original/ARToolkit/lib/SRC/AR/arGetTransMat2.c | 187 |
1 files changed, 0 insertions, 187 deletions
diff --git a/tags/Version Original/ARToolkit/lib/SRC/AR/arGetTransMat2.c b/tags/Version Original/ARToolkit/lib/SRC/AR/arGetTransMat2.c deleted file mode 100755 index 90bcaee..0000000 --- a/tags/Version Original/ARToolkit/lib/SRC/AR/arGetTransMat2.c +++ /dev/null @@ -1,187 +0,0 @@ -/******************************************************* - * - * Author: Hirokazu Kato - * - * kato@sys.im.hiroshima-cu.ac.jp - * - * Revision: 3.1 - * Date: 01/12/07 - * -*******************************************************/ -#include <stdlib.h> -#include <math.h> -#include <AR/ar.h> -#include <AR/matrix.h> - -#define MD_PI 3.14159265358979323846 - -double arModifyMatrix( double rot[3][3], double trans[3], double cpara[3][4], - double vertex[][3], double pos2d[][2], int num ) -{ - double factor; - double a, b, c; - double a1, b1, c1; - double a2, b2, c2; - double ma = 0.0, mb = 0.0, mc = 0.0; - double combo[3][4]; - double hx, hy, h, x, y; - double err, minerr; - int t1, t2, t3; - int s1 = 0, s2 = 0, s3 = 0; - int i, j; - - arGetAngle( rot, &a, &b, &c ); - - a2 = a; - b2 = b; - c2 = c; - factor = 10.0*MD_PI/180.0; - for( j = 0; j < 10; j++ ) { - minerr = 1000000000.0; - for(t1=-1;t1<=1;t1++) { - for(t2=-1;t2<=1;t2++) { - for(t3=-1;t3<=1;t3++) { - a1 = a2 + factor*t1; - b1 = b2 + factor*t2; - c1 = c2 + factor*t3; - arGetNewMatrix( a1, b1, c1, trans, NULL, cpara, combo ); - - err = 0.0; - for( i = 0; i < num; i++ ) { - hx = combo[0][0] * vertex[i][0] - + combo[0][1] * vertex[i][1] - + combo[0][2] * vertex[i][2] - + combo[0][3]; - hy = combo[1][0] * vertex[i][0] - + combo[1][1] * vertex[i][1] - + combo[1][2] * vertex[i][2] - + combo[1][3]; - h = combo[2][0] * vertex[i][0] - + combo[2][1] * vertex[i][1] - + combo[2][2] * vertex[i][2] - + combo[2][3]; - x = hx / h; - y = hy / h; - - err += (pos2d[i][0] - x) * (pos2d[i][0] - x) - + (pos2d[i][1] - y) * (pos2d[i][1] - y); - } - - if( err < minerr ) { - minerr = err; - ma = a1; - mb = b1; - mc = c1; - s1 = t1; s2 = t2; s3 = t3; - } - } - } - } - - if( s1 == 0 && s2 == 0 && s3 == 0 ) factor *= 0.5; - a2 = ma; - b2 = mb; - c2 = mc; - } - - arGetRot( ma, mb, mc, rot ); - -/* printf("factor = %10.5f\n", factor*180.0/MD_PI); */ - - return minerr/num; -} - -double arsModifyMatrix( double rot[3][3], double trans[3], ARSParam *arsParam, - double pos3dL[][3], double pos2dL[][2], int numL, - double pos3dR[][3], double pos2dR[][2], int numR ) -{ - double factor; - double a, b, c; - double a1, b1, c1; - double a2, b2, c2; - double ma = 0.0, mb = 0.0, mc = 0.0; - double combo[3][4]; - double hx, hy, h, x, y; - double err, minerr; - int t1, t2, t3; - int s1 = 0, s2 = 0, s3 = 0; - int i, j; - - arGetAngle( rot, &a, &b, &c ); - - a2 = a; - b2 = b; - c2 = c; - factor = 10.0*MD_PI/180.0; - for( j = 0; j < 10; j++ ) { - minerr = 1000000000.0; - for(t1=-1;t1<=1;t1++) { - for(t2=-1;t2<=1;t2++) { - for(t3=-1;t3<=1;t3++) { - a1 = a2 + factor*t1; - b1 = b2 + factor*t2; - c1 = c2 + factor*t3; - err = 0.0; - - arGetNewMatrix( a1, b1, c1, trans, NULL, arsParam->matL, combo ); - for( i = 0; i < numL; i++ ) { - hx = combo[0][0] * pos3dL[i][0] - + combo[0][1] * pos3dL[i][1] - + combo[0][2] * pos3dL[i][2] - + combo[0][3]; - hy = combo[1][0] * pos3dL[i][0] - + combo[1][1] * pos3dL[i][1] - + combo[1][2] * pos3dL[i][2] - + combo[1][3]; - h = combo[2][0] * pos3dL[i][0] - + combo[2][1] * pos3dL[i][1] - + combo[2][2] * pos3dL[i][2] - + combo[2][3]; - x = hx / h; - y = hy / h; - err += (pos2dL[i][0] - x) * (pos2dL[i][0] - x) - + (pos2dL[i][1] - y) * (pos2dL[i][1] - y); - } - - arGetNewMatrix( a1, b1, c1, trans, arsParam->matL2R, arsParam->matR, combo ); - for( i = 0; i < numR; i++ ) { - hx = combo[0][0] * pos3dR[i][0] - + combo[0][1] * pos3dR[i][1] - + combo[0][2] * pos3dR[i][2] - + combo[0][3]; - hy = combo[1][0] * pos3dR[i][0] - + combo[1][1] * pos3dR[i][1] - + combo[1][2] * pos3dR[i][2] - + combo[1][3]; - h = combo[2][0] * pos3dR[i][0] - + combo[2][1] * pos3dR[i][1] - + combo[2][2] * pos3dR[i][2] - + combo[2][3]; - x = hx / h; - y = hy / h; - - err += (pos2dR[i][0] - x) * (pos2dR[i][0] - x) - + (pos2dR[i][1] - y) * (pos2dR[i][1] - y); - } - - if( err < minerr ) { - minerr = err; - ma = a1; - mb = b1; - mc = c1; - s1 = t1; s2 = t2; s3 = t3; - } - } - } - } - - if( s1 == 0 && s2 == 0 && s3 == 0 ) factor *= 0.5; - a2 = ma; - b2 = mb; - c2 = mc; - } - - arGetRot( ma, mb, mc, rot ); - - return minerr / (numL+numR); -} |