diff options
Diffstat (limited to 'ARToolKitPlus/src/core/arGetTransMat2.cxx')
| -rw-r--r-- | ARToolKitPlus/src/core/arGetTransMat2.cxx | 571 |
1 files changed, 0 insertions, 571 deletions
diff --git a/ARToolKitPlus/src/core/arGetTransMat2.cxx b/ARToolKitPlus/src/core/arGetTransMat2.cxx deleted file mode 100644 index f339dd1..0000000 --- a/ARToolKitPlus/src/core/arGetTransMat2.cxx +++ /dev/null @@ -1,571 +0,0 @@ -/* ========================================================================
- * PROJECT: ARToolKitPlus
- * ========================================================================
- * This work is based on the original ARToolKit developed by
- * Hirokazu Kato
- * Mark Billinghurst
- * HITLab, University of Washington, Seattle
- * http://www.hitl.washington.edu/artoolkit/
- *
- * Copyright of the derived and new portions of this work
- * (C) 2006 Graz University of Technology
- *
- * This framework is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2 of the License, or
- * (at your option) any later version.
- *
- * This framework is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this framework; if not, write to the Free Software
- * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
- *
- * For further information please contact
- * Dieter Schmalstieg
- * <schmalstieg@icg.tu-graz.ac.at>
- * Graz University of Technology,
- * Institut for Computer Graphics and Vision,
- * Inffeldgasse 16a, 8010 Graz, Austria.
- * ========================================================================
- ** @author Daniel Wagner
- *
- * $Id: arGetTransMat2.cxx 162 2006-04-19 21:28:10Z grabner $
- * @file
- * ======================================================================== */
-
-
-//#include <stdlib.h>
-//#include <math.h>
-
-//#include <ARToolKitPlus/Tracker.h>
-//#include <ARToolKitPlus/matrix.h>
-//#include <ARToolKitPlus/extra/Profiler.h>
-
-
-namespace ARToolKitPlus {
-
-
-#define MD_PI 3.14159265358979323846
-
-
-#ifndef _FIXEDPOINT_MATH_ACTIVATED_
-
-
-AR_TEMPL_FUNC ARFloat
-AR_TEMPL_TRACKER::arModifyMatrix(ARFloat rot[3][3], ARFloat trans[3], ARFloat cpara[3][4],
- ARFloat vertex[][3], ARFloat pos2d[][2], int num)
-{
- ARFloat factor;
- ARFloat a, b, c;
- ARFloat a1, b1, c1;
- ARFloat a2, b2, c2;
- ARFloat ma = 0, mb = 0, mc = 0;
- ARFloat combo[3][4];
- ARFloat hx, hy, h, x, y;
- ARFloat err, minerr;
- int t1, t2, t3;
- int s1 = 0, s2 = 0, s3 = 0;
- int i, j;
-
- PROFILE_BEGINSEC(profiler, MODIFYMATRIX)
-
- arGetAngle( rot, &a, &b, &c );
-
- a2 = a;
- b2 = b;
- c2 = c;
- factor = (ARFloat)(10.0*MD_PI/180.0);
- for( j = 0; j < 15; 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 /*&& ss1 == 0 && ss2 == 0 && ss3 == 0*/ ) factor *= 0.5;
- a2 = ma;
- b2 = mb;
- c2 = mc;
- }
-
- arGetRot( ma, mb, mc, rot );
-
- PROFILE_ENDSEC(profiler, MODIFYMATRIX)
-
- return minerr/num;
-}
-
-
-AR_TEMPL_FUNC ARFloat
-AR_TEMPL_TRACKER::arModifyMatrix2(ARFloat rot[3][3], ARFloat trans[3], ARFloat cpara[3][4],
- ARFloat vertex[][3], ARFloat pos2d[][2], int num)
-{
- ARFloat factor;
- ARFloat a, b, c;
- ARFloat a1, b1, c1;
- ARFloat a2, b2, c2;
- ARFloat ma, mb, mc;
- ARFloat combo[3][4];
- ARFloat hx, hy, h, x, y;
- ARFloat err, minerr;
- int t1, t2, t3, tt1,tt2,tt3;
- ARFloat tfact[5] = { 0.96f, 0.98f, 1.0f, 1.02f, 1.04f };
- ARFloat modtrans[3], mmodtrans[3];
- int s1, s2, s3, ss1,ss2,ss3;
- int i, j;
-
- arGetAngle( rot, &a, &b, &c );
-
- a2 = a;
- b2 = b;
- c2 = c;
- factor = (ARFloat)(40.0*MD_PI/180.0);
- for( j = 0; j < 15; j++ ) {
- minerr = 1000000000.0;
- for(t1=-1;t1<=1;t1++) {
- for(t2=-1;t2<=1;t2++) {
- for(t3=-1;t3<=1;t3++) {
- for(tt1=-2;tt1<=2;tt1++) {
- for(tt2=-2;tt2<=2;tt2++) {
- for(tt3=-2;tt3<=2;tt3++) {
-
- a1 = a2 + factor*t1;
- b1 = b2 + factor*t2;
- c1 = c2 + factor*t3;
- modtrans[0] = trans[0]*tfact[tt1+2];
- modtrans[1] = trans[1]*tfact[tt2+2];
- modtrans[2] = trans[2]*tfact[tt3+2];
-
- arGetNewMatrix( a1, b1, c1, modtrans, 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;
- mmodtrans[0] = modtrans[0];
- mmodtrans[1] = modtrans[1];
- mmodtrans[2] = modtrans[2];
-
- s1 = t1; s2 = t2; s3 = t3;
- ss1 = tt1; ss2 = tt2; ss3 = tt3;
- }
- }
- }
- }
- }
- }
- }
-
- if( s1 == 0 && s2 == 0 && s3 == 0 /*&& ss1 == 0 && ss2 == 0 && ss3 == 0*/ ) factor *= 0.5;
- a2 = ma;
- b2 = mb;
- c2 = mc;
- trans[0] = mmodtrans[0];
- trans[1] = mmodtrans[1];
- trans[2] = mmodtrans[2];
- }
-
- arGetRot( ma, mb, mc, rot );
-
- return minerr/num;
-}
-
-
-#else //_FIXEDPOINT_MATH_ACTIVATED_
-
-
-//////////////////////////////////////////////////////////////
-//
-// FIXEDPOINT specific code starts here
-//
-//
-
-#define BITS 8
-
-
-int arGetNewMatrix12(I32 _a, I32 _b, I32 _c, FIXED_VEC3D _trans, ARFloat trans2[3][4],
- FIXED_VEC3D _cpara[3], I32 _cpara3[3], FIXED_VEC3D ret[3], I32 _ret3[3], Profiler& nProfiler);
-
-
-
-AR_TEMPL_FUNC ARFloat
-AR_TEMPL_TRACKER::arModifyMatrix(ARFloat rot[3][3], ARFloat trans[3], ARFloat cpara[3][4],
- ARFloat vertex[][3], ARFloat pos2d[][2], int num)
-{
- //ARFloat factor;
- ARFloat a, b, c;
- //ARFloat a1, b1, c1;
- ARFloat a2, b2, c2;
- ARFloat ma, mb, mc;
- int t1, t2, t3;
- int s1, s2, s3;
- int i, j, k;
- ARFloat minerr;
-
- I32 _hx, _hy, _h;
- I32 _err, _minerr;
- U32 _ures;
- I32 _a1,_b1,_c1;
- I32 _a2,_b2,_c2;
- I32 _ma, _mb, _mc;
-
- PROFILE_BEGINSEC(profiler, MODIFYMATRIX)
-
- FIXED_VEC3D *_vertex = (FIXED_VEC3D*)malloc(num*sizeof(FIXED_VEC3D)),
- *_pos2d = (FIXED_VEC3D*)malloc(num*sizeof(FIXED_VEC3D)),
- _combo[3], _vec1, _vec2, _trans;
- I32 _combo3[3];
-
- FIXED_VEC3D _cpara[3];
- I32 _cpara3[3];
-
- _vec1.z = 0;
-
- _trans.x = FIXED_Float_To_Fixed_n(trans[0], 12);
- _trans.y = FIXED_Float_To_Fixed_n(trans[1], 12);
- _trans.z = FIXED_Float_To_Fixed_n(trans[2], 12);
-
- for(j=0; j<3; j++)
- {
- _cpara[j].x = FIXED_Float_To_Fixed_n(cpara[j][0], 12);
- _cpara[j].y = FIXED_Float_To_Fixed_n(cpara[j][1], 12);
- _cpara[j].z = FIXED_Float_To_Fixed_n(cpara[j][2], 12);
- _cpara3[j] = FIXED_Float_To_Fixed_n(cpara[j][3], 12);
- }
-
- for(j=0; j<num; j++)
- {
- _vertex[j].x = FIXED_Float_To_Fixed_n(vertex[j][0], BITS);
- _vertex[j].y = FIXED_Float_To_Fixed_n(vertex[j][1], BITS);
- _vertex[j].z = FIXED_Float_To_Fixed_n(vertex[j][2], BITS);
-
- _pos2d[j].x = FIXED_Float_To_Fixed_n(pos2d[j][0], BITS);
- _pos2d[j].y = FIXED_Float_To_Fixed_n(pos2d[j][1], BITS);
- _pos2d[j].z = 0;
- }
-
-
- arGetAngle( rot, &a, &b, &c );
-
- PROFILE_BEGINSEC(profiler, MODIFYMATRIX_LOOP)
-
- a2 = a;
- b2 = b;
- c2 = c;
- //factor = (ARFloat)(10.0*MD_PI/180.0);
-
- //I32 fix_a2, fix_b2, fix_c2;
- I32 fix_factor = FIXED_Float_To_Fixed_n((10.0*MD_PI/180.0), 12);
- I32 fix_a[3], fix_b[3], fix_c[3];
-
- _a2 = FIXED_Float_To_Fixed_n(a2, 12);
- _b2 = FIXED_Float_To_Fixed_n(b2, 12);
- _c2 = FIXED_Float_To_Fixed_n(c2, 12);
-
- for( j = 0; j < 10; j++ ) {
- _minerr = 0x40000000; // value
-
- fix_a[0] = _a2 - fix_factor; fix_a[1] = _a2; fix_a[2] = _a2 + fix_factor;
- fix_b[0] = _b2 - fix_factor; fix_b[1] = _b2; fix_b[2] = _b2 + fix_factor;
- fix_c[0] = _c2 - fix_factor; fix_c[1] = _c2; fix_c[2] = _c2 + fix_factor;
-
- 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;
-
- _a1 = fix_a[t1+1];
- _b1 = fix_b[t2+1];
- _c1 = fix_c[t3+1];
-
- PROFILE_BEGINSEC(profiler, GETNEWMATRIX)
- arGetNewMatrix12(_a1, _b1, _c1, _trans, NULL, _cpara, _cpara3, _combo, _combo3, profiler);
- PROFILE_ENDSEC(profiler, GETNEWMATRIX)
-
- for(k=0; k<3; k++)
- {
- _combo[k].x >>= 4;
- _combo[k].y >>= 4;
- _combo[k].z >>= 4;
- _combo3[k] >>= 4;
- }
-
- _err = 0;
- for( i = 0; i < num; i++ ) {
- FIXED_VEC3_DOT(_combo+0, _vertex+i, &_hx, BITS);
- _hx += _combo3[0];
-
- FIXED_VEC3_DOT(_combo+1, _vertex+i, &_hy, BITS);
- _hy += _combo3[1];
-
- FIXED_VEC3_DOT(_combo+2, _vertex+i, &_h, BITS);
- _h += _combo3[2];
-
- FIXED_DIV2(_hx, _h, _vec1.x, BITS);
- FIXED_DIV2(_hy, _h, _vec1.y, BITS);
-
- FIXED_VEC3_SUB(_pos2d+i, &_vec1, &_vec2);
- FIXED_VEC3_LENGTH_SQ(&_vec2, &_ures, BITS);
- _err += _ures;
- }
-
- if( _err < _minerr ) {
- _minerr = _err;
- //ma = a1;
- //mb = b1;
- //mc = c1;
- _ma = _a1;
- _mb = _b1;
- _mc = _c1;
- s1 = t1; s2 = t2; s3 = t3;
- }
- }
- }
- }
-
- if(s1 == 0 && s2 == 0 && s3 == 0)
- {
- //factor *= (ARFloat)0.5;
- fix_factor >>= 1;
- }
- //a2 = ma;
- //b2 = mb;
- //c2 = mc;
- _a2 = _ma;
- _b2 = _mb;
- _c2 = _mc;
- }
-
- PROFILE_ENDSEC(profiler, MODIFYMATRIX_LOOP)
-
- free(_vertex);
- free(_pos2d);
-
- ma = FIXED_Fixed_n_To_Float(_ma, 12);
- mb = FIXED_Fixed_n_To_Float(_mb, 12);
- mc = FIXED_Fixed_n_To_Float(_mc, 12);
-
- arGetRot( ma, mb, mc, rot );
-
- minerr = FIXED_Fixed_n_To_Float(_minerr, BITS);
-
- PROFILE_ENDSEC(profiler, MODIFYMATRIX)
-
- return minerr/num;
-}
-
-/*
-AR_TEMPL_FUNC ARFloat
-AR_TEMPL_TRACKER::arModifyMatrix(ARFloat rot[3][3], ARFloat trans[3], ARFloat cpara[3][4],
- ARFloat vertex[][3], ARFloat pos2d[][2], int num)
-{
- ARFloat factor;
- ARFloat a, b, c;
- ARFloat a1, b1, c1;
- ARFloat a2, b2, c2;
- ARFloat ma, mb, mc;
- int t1, t2, t3;
- int s1, s2, s3;
- int i, j, k;
- ARFloat minerr;
-
- I32 _hx, _hy, _h;
- I32 _err, _minerr;
- U32 _ures;
- I32 _a1,_b1,_c1;
-
- FIXED_VEC3D *_vertex = (FIXED_VEC3D*)malloc(num*sizeof(FIXED_VEC3D)),
- *_pos2d = (FIXED_VEC3D*)malloc(num*sizeof(FIXED_VEC3D)),
- _combo[3], _vec1, _vec2, _trans;
- I32 _combo3[3];
-
- FIXED_VEC3D _cpara[3];
- I32 _cpara3[3];
-
- _vec1.z = 0;
-
- _trans.x = FIXED_Float_To_Fixed_n(trans[0], 12);
- _trans.y = FIXED_Float_To_Fixed_n(trans[1], 12);
- _trans.z = FIXED_Float_To_Fixed_n(trans[2], 12);
-
- for(j=0; j<3; j++)
- {
- _cpara[j].x = FIXED_Float_To_Fixed_n(cpara[j][0], 12);
- _cpara[j].y = FIXED_Float_To_Fixed_n(cpara[j][1], 12);
- _cpara[j].z = FIXED_Float_To_Fixed_n(cpara[j][2], 12);
- _cpara3[j] = FIXED_Float_To_Fixed_n(cpara[j][3], 12);
- }
-
- for(j=0; j<num; j++)
- {
- _vertex[j].x = FIXED_Float_To_Fixed_n(vertex[j][0], BITS);
- _vertex[j].y = FIXED_Float_To_Fixed_n(vertex[j][1], BITS);
- _vertex[j].z = FIXED_Float_To_Fixed_n(vertex[j][2], BITS);
-
- _pos2d[j].x = FIXED_Float_To_Fixed_n(pos2d[j][0], BITS);
- _pos2d[j].y = FIXED_Float_To_Fixed_n(pos2d[j][1], BITS);
- _pos2d[j].z = 0;
- }
-
-
- arGetAngle( rot, &a, &b, &c );
-
-
- a2 = a;
- b2 = b;
- c2 = c;
- factor = (ARFloat)(10.0*MD_PI/180.0);
- for( j = 0; j < 10; j++ ) {
- _minerr = 0x40000000; // value
-
- 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;
-
- _a1 = FIXED_Float_To_Fixed_n(a1, 12);
- _b1 = FIXED_Float_To_Fixed_n(b1, 12);
- _c1 = FIXED_Float_To_Fixed_n(c1, 12);
-
- arGetNewMatrix12(_a1, _b1, _c1, _trans, NULL, _cpara, _cpara3, _combo, _combo3, profiler);
-
- for(k=0; k<3; k++)
- {
- _combo[k].x >>= 4;
- _combo[k].y >>= 4;
- _combo[k].z >>= 4;
- _combo3[k] >>= 4;
- }
-
- _err = 0;
- for( i = 0; i < num; i++ ) {
- FIXED_VEC3_DOT(_combo+0, _vertex+i, &_hx, BITS);
- _hx += _combo3[0];
-
- FIXED_VEC3_DOT(_combo+1, _vertex+i, &_hy, BITS);
- _hy += _combo3[1];
-
- FIXED_VEC3_DOT(_combo+2, _vertex+i, &_h, BITS);
- _h += _combo3[2];
-
- FIXED_DIV2(_hx, _h, _vec1.x, BITS)
- FIXED_DIV2(_hy, _h, _vec1.y, BITS)
-
- FIXED_VEC3_SUB(_pos2d+i, &_vec1, &_vec2);
- FIXED_VEC3_LENGTH_SQ(&_vec2, &_ures, BITS);
- _err += _ures;
- }
-
- 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 *= (ARFloat)0.5;
- a2 = ma;
- b2 = mb;
- c2 = mc;
- }
-
- free(_vertex);
- free(_pos2d);
-
- arGetRot( ma, mb, mc, rot );
-
- minerr = FIXED_Fixed_n_To_Float(_minerr, BITS);
-
- return minerr/num;
-}
-
-*/
-
-
-//
-//
-// FIXEDPOINT specific code ends here
-//
-//////////////////////////////////////////////////////////////
-
-
-#ifdef BITS
-#undef BITS
-#endif
-
-
-#endif //_FIXEDPOINT_MATH_ACTIVATED_
-
-
-} // namespace ARToolKitPlus
|