tags/1.1.1/src/jp/nyatla/nyartoolkit/core/NyARTransMat_O2.java

   1 /* \r
   2  * PROJECT: NyARToolkit\r
   3  * --------------------------------------------------------------------------------\r
   4  * This work is based on the original ARToolKit developed by\r
   5  *   Hirokazu Kato\r
   6  *   Mark Billinghurst\r
   7  *   HITLab, University of Washington, Seattle\r
   8  * http://www.hitl.washington.edu/artoolkit/\r
   9  *\r
  10  * The NyARToolkit is Java version ARToolkit class library.\r
  11  * Copyright (C)2008 R.Iizuka\r
  12  *\r
  13  * This program is free software; you can redistribute it and/or\r
  14  * modify it under the terms of the GNU General Public License\r
  15  * as published by the Free Software Foundation; either version 2\r
  16  * of the License, or (at your option) any later version.\r
  17  * \r
  18  * This program is distributed in the hope that it will be useful,\r
  19  * but WITHOUT ANY WARRANTY; without even the implied warranty of\r
  20  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the\r
  21  * GNU General Public License for more details.\r
  22  * \r
  23  * You should have received a copy of the GNU General Public License\r
  24  * along with this framework; if not, write to the Free Software\r
  25  * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA\r
  26  * \r
  27  * For further information please contact.\r
  28  *      http://nyatla.jp/nyatoolkit/\r
  29  *      <airmail(at)ebony.plala.or.jp>\r
  30  * \r
  31  */\r
  32 package jp.nyatla.nyartoolkit.core;\r
  33 \r
  34 \r
  35 \r
  36 import jp.nyatla.nyartoolkit.NyARException;\r
  37 import jp.nyatla.util.DoubleValue;\r
  38 \r
  39 \r
  40 \r
  41 /**\r
  42  * This class calculates ARMatrix from square information and holds it.\r
  43  * --\r
  44  * 変換行列を計算して、結果を保持するクラス。\r
  45  *\r
  46  */\r
  47 public class NyARTransMat_O2 implements NyARTransMat{\r
  48     private final static int AR_FITTING_TO_IDEAL=0;//#define  AR_FITTING_TO_IDEAL          0\r
  49     private final static int AR_FITTING_TO_INPUT=1;//#define  AR_FITTING_TO_INPUT          1\r
  50     private final static int    arFittingMode   =AR_FITTING_TO_INPUT;\r
  51 \r
  52     private final static int AR_GET_TRANS_MAT_MAX_LOOP_COUNT=5;//#define   AR_GET_TRANS_MAT_MAX_LOOP_COUNT         5\r
  53     private final static double AR_GET_TRANS_MAT_MAX_FIT_ERROR=1.0;//#define   AR_GET_TRANS_MAT_MAX_FIT_ERROR          1.0\r
  54     private final static int P_MAX=10;//頂点の数(4で十分だけどなんとなく10)//#define P_MAX       500\r
  55     private final static int NUMBER_OF_VERTEX=4;//処理対象の頂点数\r
  56     private final NyARTransRot transrot;\r
  57     private final double[] center={0.0,0.0};\r
  58     private final NyARParam param;\r
  59     private final NyARMat result_mat=new NyARMat(3,4);\r
  60     public NyARTransMat_O2(NyARParam i_param)throws NyARException\r
  61     {\r
  62         param=i_param;\r
  63         transrot=new NyARTransRot_O3(i_param,NUMBER_OF_VERTEX);\r
  64 \r
  65     }\r
  66     public void setCenter(double i_x,double i_y)\r
  67     {\r
  68         center[0]=i_x;\r
  69         center[1]=i_x;\r
  70     }\r
  71     public NyARMat getTransformationMatrix()\r
  72     {\r
  73         return result_mat;\r
  74     }\r
  75 \r
  76     private final double[][] wk_transMat_pos3d=new double[P_MAX][3];//pos3d[P_MAX][3];\r
  77     private final double[][] wk_transMat_ppos2d=new double[4][2];\r
  78     private final double[][] wk_transMat_ppos3d=new double[4][2];\r
  79     private final double[] wk_transMat_off=new double[3];\r
  80     private final double[][] wk_transMat_pos2d=new double[P_MAX][2];//pos2d[P_MAX][2];\r
  81 \r
  82 \r
  83     private final DoubleValue wk_arGetTransMatSub_a1=new DoubleValue();\r
  84     private final DoubleValue wk_arGetTransMatSub_a2=new DoubleValue();\r
  85     private final NyARMat wk_transMat_mat_a=new NyARMat(NUMBER_OF_VERTEX*2,3);\r
  86     private final NyARMat wk_transMat_mat_b=new NyARMat(3,NUMBER_OF_VERTEX*2);\r
  87     private final NyARMat wk_transMat_mat_d=new NyARMat( 3, 3 );    \r
  88     private final double[] wk_transMat_mat_trans=new double[3];\r
  89 \r
  90     /**\r
  91      * double arGetTransMat( ARMarkerInfo *marker_info,double center[2], double width, double conv[3][4] )\r
  92      * 演算シーケンス最適化のため、arGetTransMat3等の関数フラグメントを含みます。\r
  93      * 保持している変換行列を更新する。\r
  94      * @param square\r
  95      * 計算対象のNyARSquareオブジェクト\r
  96      * @param i_direction\r
  97      * @param width\r
  98      * @return\r
  99      * @throws NyARException\r
 100      */\r
 101     public double transMat( NyARSquare square,int i_direction, double width)throws NyARException\r
 102     {\r
 103         double[][]  ppos2d=wk_transMat_ppos2d;\r
 104         double[][]  ppos3d=wk_transMat_ppos3d;\r
 105         double[] off=wk_transMat_off;\r
 106         double[][] pos3d=wk_transMat_pos3d;\r
 107 \r
 108 \r
 109         ppos2d[0][0] = square.sqvertex[(4-i_direction)%4][0];\r
 110         ppos2d[0][1] = square.sqvertex[(4-i_direction)%4][1];\r
 111         ppos2d[1][0] = square.sqvertex[(5-i_direction)%4][0];\r
 112         ppos2d[1][1] = square.sqvertex[(5-i_direction)%4][1];\r
 113         ppos2d[2][0] = square.sqvertex[(6-i_direction)%4][0];\r
 114         ppos2d[2][1] = square.sqvertex[(6-i_direction)%4][1];\r
 115         ppos2d[3][0] = square.sqvertex[(7-i_direction)%4][0];\r
 116         ppos2d[3][1] = square.sqvertex[(7-i_direction)%4][1];\r
 117         ppos3d[0][0] = center[0] - width/2.0;\r
 118         ppos3d[0][1] = center[1] + width/2.0;\r
 119         ppos3d[1][0] = center[0] + width/2.0;\r
 120         ppos3d[1][1] = center[1] + width/2.0;\r
 121         ppos3d[2][0] = center[0] + width/2.0;\r
 122         ppos3d[2][1] = center[1] - width/2.0;\r
 123         ppos3d[3][0] = center[0] - width/2.0;\r
 124         ppos3d[3][1] = center[1] - width/2.0;\r
 125 \r
 126         transrot.initRot(square,i_direction);\r
 127 \r
 128         //arGetTransMat3の前段処理(pos3dとoffを初期化)\r
 129         arGetTransMat3_initPos3d(ppos3d,pos3d,off);\r
 130 \r
 131 \r
 132         //arGetTransMatSubにあった処理。毎回おなじっぽい。pos2dに変換座標を格納する。\r
 133         double[][] pos2d=this.wk_transMat_pos2d;\r
 134         DoubleValue a1=this.wk_arGetTransMatSub_a1;\r
 135         DoubleValue a2=this.wk_arGetTransMatSub_a2;\r
 136         if(arFittingMode == AR_FITTING_TO_INPUT ){\r
 137             for(int i = 0; i < NUMBER_OF_VERTEX; i++ ) {\r
 138                 param.ideal2Observ(ppos2d[i][0], ppos2d[i][1],a1,a2);//arParamIdeal2Observ(dist_factor, ppos2d[i][0], ppos2d[i][1],&pos2d[i][0], &pos2d[i][1]);\r
 139                 pos2d[i][0]=a1.value;\r
 140                 pos2d[i][1]=a2.value;\r
 141             }\r
 142         }else{\r
 143             for(int i = 0; i < NUMBER_OF_VERTEX; i++ ){\r
 144                 pos2d[i][0] = ppos2d[i][0];\r
 145                 pos2d[i][1] = ppos2d[i][1];\r
 146             }\r
 147         }\r
 148 \r
 149         //変換マトリクスdとbの準備(arGetTransMatSubの一部)\r
 150         final double cpara[]=param.get34Array();\r
 151         final NyARMat mat_a =this.wk_transMat_mat_a;\r
 152         final double[][] a_array=mat_a.getArray();\r
 153 \r
 154         final NyARMat mat_b =this.wk_transMat_mat_b;\r
 155         final double[][] b_array=mat_b.getArray();\r
 156 \r
 157         int x2;\r
 158         for(int i = 0; i < NUMBER_OF_VERTEX; i++ ) {\r
 159             x2=i*2;\r
 160             //</Optimize>\r
 161             a_array[x2  ][0]=b_array[0][x2]=cpara[0*4+0];//mat_a->m[j*6+0] = mat_b->m[num*0+j*2] = cpara[0][0];\r
 162             a_array[x2  ][1]=b_array[1][x2]=cpara[0*4+1];//mat_a->m[j*6+1] = mat_b->m[num*2+j*2] = cpara[0][1];\r
 163             a_array[x2  ][2]=b_array[2][x2]=cpara[0*4+2]-pos2d[i][0];//mat_a->m[j*6+2] = mat_b->m[num*4+j*2] = cpara[0][2] - pos2d[j][0];\r
 164             a_array[x2+1][0]=b_array[0][x2+1]=0.0;//mat_a->m[j*6+3] = mat_b->m[num*0+j*2+1] = 0.0;\r
 165             a_array[x2+1][1]=b_array[1][x2+1]=cpara[1*4+1];//mat_a->m[j*6+4] = mat_b->m[num*2+j*2+1] = cpara[1][1];\r
 166             a_array[x2+1][2]=b_array[2][x2+1]=cpara[1*4+2] - pos2d[i][1];//mat_a->m[j*6+5] = mat_b->m[num*4+j*2+1] = cpara[1][2] - pos2d[j][1];\r
 167         }\r
 168         final NyARMat mat_d =this.wk_transMat_mat_d;\r
 169         mat_d.matrixMul(mat_b,mat_a);\r
 170         mat_d.matrixSelfInv();\r
 171 \r
 172         double  err=-1;\r
 173         double[] rot=transrot.getArray();\r
 174         double[][] conv=result_mat.getArray();\r
 175         double[] trans=this.wk_transMat_mat_trans;\r
 176         for(int i=0;i<AR_GET_TRANS_MAT_MAX_LOOP_COUNT; i++ ){\r
 177             //<arGetTransMat3>\r
 178             err = arGetTransMatSub(pos2d, pos3d,mat_b,mat_d,trans);\r
 179             conv[0][0] = rot[0*3+0];\r
 180             conv[0][1] = rot[0*3+1];\r
 181             conv[0][2] = rot[0*3+2];\r
 182             conv[1][0] = rot[1*3+0];\r
 183             conv[1][1] = rot[1*3+1];\r
 184             conv[1][2] = rot[1*3+2];\r
 185             conv[2][0] = rot[2*3+0];\r
 186             conv[2][1] = rot[2*3+1];\r
 187             conv[2][2] = rot[2*3+2];\r
 188             conv[0][3] = rot[0*3+0]*off[0] + rot[0*3+1]*off[1] + rot[0*3+2]*off[2] + trans[0];\r
 189             conv[1][3] = rot[1*3+0]*off[0] + rot[1*3+1]*off[1] + rot[1*3+2]*off[2] + trans[1];\r
 190             conv[2][3] = rot[2*3+0]*off[0] + rot[2*3+1]*off[1] + rot[2*3+2]*off[2] + trans[2];\r
 191 \r
 192             //</arGetTransMat3>\r
 193             if( err < AR_GET_TRANS_MAT_MAX_FIT_ERROR ){\r
 194                 break;\r
 195             }\r
 196         }\r
 197         return err;\r
 198     }\r
 199     private final double[] wk_arGetTransMat3_initPos3d_pmax=new double[3];\r
 200     private final double[] wk_arGetTransMat3_initPos3d_pmin=new double[3];\r
 201     /**\r
 202      * arGetTransMat3関数の前処理部分。i_ppos3dから、o_pos3dとoffを計算する。\r
 203      * 計算結果から再帰的に変更される可能性が無いので、切り離し。\r
 204      * @param i_ppos3d\r
 205      * 入力配列[num][3]\r
 206      * @param o_pos3d\r
 207      * 出力配列[P_MAX][3]\r
 208      * @param o_off\r
 209      * [3]\r
 210      * @throws NyARException\r
 211      */\r
 212     private final void arGetTransMat3_initPos3d(double i_ppos3d[][],double[][] o_pos3d,double[] o_off)throws NyARException\r
 213     {\r
 214         final double[] pmax=wk_arGetTransMat3_initPos3d_pmax;//new double[3];\r
 215         final double[] pmin=wk_arGetTransMat3_initPos3d_pmin;//new double[3];//double  off[3], pmax[3], pmin[3];\r
 216         int i;\r
 217         pmax[0]=pmax[1]=pmax[2] = -10000000000.0;\r
 218         pmin[0]=pmin[1]=pmin[2] =  10000000000.0;\r
 219         for(i = 0; i < NUMBER_OF_VERTEX; i++ ) {\r
 220             if( i_ppos3d[i][0] > pmax[0] ){\r
 221                 pmax[0] = i_ppos3d[i][0];\r
 222             }\r
 223             if( i_ppos3d[i][0] < pmin[0] ){\r
 224                 pmin[0] = i_ppos3d[i][0];\r
 225             }\r
 226             if( i_ppos3d[i][1] > pmax[1] ){\r
 227                 pmax[1] = i_ppos3d[i][1];\r
 228             }\r
 229             if( i_ppos3d[i][1] < pmin[1] ){\r
 230                 pmin[1] = i_ppos3d[i][1];\r
 231             }\r
 232         }\r
 233         o_off[0] = -(pmax[0] + pmin[0]) / 2.0;\r
 234         o_off[1] = -(pmax[1] + pmin[1]) / 2.0;\r
 235         o_off[2] = -(pmax[2] + pmin[2]) / 2.0;\r
 236 \r
 237 \r
 238         double[] o_pos3d_pt;\r
 239         double[] i_pos_pd_pt;   \r
 240         for(i = 0; i < NUMBER_OF_VERTEX; i++ ) {\r
 241             o_pos3d_pt =o_pos3d[i];\r
 242             i_pos_pd_pt=i_ppos3d[i];\r
 243             o_pos3d_pt[0] = i_pos_pd_pt[0] + o_off[0];\r
 244             o_pos3d_pt[1] = i_pos_pd_pt[1] + o_off[1];\r
 245             o_pos3d_pt[2] = 0.0;\r
 246         }\r
 247     }    \r
 248 \r
 249     private final NyARMat wk_arGetTransMatSub_mat_c=new NyARMat(NUMBER_OF_VERTEX*2,1);\r
 250     private final NyARMat wk_arGetTransMatSub_mat_e=new NyARMat( 3, 1 );\r
 251     private final NyARMat wk_arGetTransMatSub_mat_f=new NyARMat( 3, 1 );\r
 252 \r
 253     /**\r
 254      * static double arGetTransMatSub( double rot[3][3], double ppos2d[][2],double pos3d[][3], int num, double conv[3][4],double *dist_factor, double cpara[3][4] )\r
 255      * Optimize:2008.04.20:STEP[1033→1004]\r
 256      * @param i_ppos2d\r
 257      * @param i_pos3d\r
 258      * @param i_mat_b\r
 259      * 演算用行列b\r
 260      * @param i_mat_d\r
 261      * 演算用行列d\r
 262      * @return\r
 263      * @throws NyARException\r
 264      */\r
 265     private final double arGetTransMatSub(double i_ppos2d[][],double i_pos3d[][],NyARMat i_mat_b,NyARMat i_mat_d,double[] o_trans) throws NyARException\r
 266     {\r
 267         double cpara[]=param.get34Array();\r
 268         NyARMat mat_c,mat_e,mat_f;//ARMat   *mat_a, *mat_b, *mat_c, *mat_d, *mat_e, *mat_f;\r
 269 \r
 270         double  wx, wy, wz;\r
 271         double  ret;\r
 272         int     i;\r
 273 \r
 274         mat_c =this.wk_arGetTransMatSub_mat_c;//次処理で値をもらうので、初期化の必要は無い。\r
 275         double[][] c_array=mat_c.getArray();\r
 276         double[] rot=transrot.getArray();\r
 277         double[] i_pos3d_pt;\r
 278         int x2;\r
 279         for( i = 0; i < NUMBER_OF_VERTEX; i++ ) {\r
 280             x2=i*2;\r
 281             i_pos3d_pt=i_pos3d[i];\r
 282             wx = rot[0] * i_pos3d_pt[0]+ rot[1] * i_pos3d_pt[1]+ rot[2] * i_pos3d_pt[2];\r
 283             wy = rot[3] * i_pos3d_pt[0]+ rot[4] * i_pos3d_pt[1]+ rot[5] * i_pos3d_pt[2];\r
 284             wz = rot[6] * i_pos3d_pt[0]+ rot[7] * i_pos3d_pt[1]+ rot[8] * i_pos3d_pt[2];\r
 285             c_array[x2][0]  =wz * i_ppos2d[i][0]- cpara[0*4+0]*wx - cpara[0*4+1]*wy - cpara[0*4+2]*wz;//mat_c->m[j*2+0] = wz * pos2d[j][0]- cpara[0][0]*wx - cpara[0][1]*wy - cpara[0][2]*wz;\r
 286             c_array[x2+1][0]=wz * i_ppos2d[i][1]- cpara[1*4+1]*wy - cpara[1*4+2]*wz;//mat_c->m[j*2+1] = wz * pos2d[j][1]- cpara[1][1]*wy - cpara[1][2]*wz;\r
 287         }\r
 288         mat_e = this.wk_arGetTransMatSub_mat_e;//次処理で値をもらうので、初期化の必要は無い。\r
 289         mat_f = this.wk_arGetTransMatSub_mat_f;//次処理で値をもらうので、初期化の必要は無い。\r
 290         double[][] f_array=mat_f.getArray();\r
 291 \r
 292         mat_e.matrixMul(i_mat_b, mat_c );\r
 293         mat_f.matrixMul(i_mat_d, mat_e );\r
 294 \r
 295 //      double[] trans=wk_arGetTransMatSub_trans;//double  trans[3];    \r
 296         o_trans[0] = f_array[0][0];//trans[0] = mat_f->m[0];\r
 297         o_trans[1] = f_array[1][0];\r
 298         o_trans[2] = f_array[2][0];//trans[2] = mat_f->m[2];\r
 299         ret =transrot.modifyMatrix(o_trans, i_pos3d, i_ppos2d);\r
 300         for( i = 0; i < NUMBER_OF_VERTEX; i++ ) {\r
 301             x2=i*2;\r
 302             i_pos3d_pt=i_pos3d[i];\r
 303             wx = rot[0] * i_pos3d_pt[0]+ rot[1] * i_pos3d_pt[1]+ rot[2] * i_pos3d_pt[2];\r
 304             wy = rot[3] * i_pos3d_pt[0]+ rot[4] * i_pos3d_pt[1]+ rot[5] * i_pos3d_pt[2];\r
 305             wz = rot[6] * i_pos3d_pt[0]+ rot[7] * i_pos3d_pt[1]+ rot[8] * i_pos3d_pt[2];\r
 306             c_array[x2][0]  =wz * i_ppos2d[i][0]- cpara[0*4+0]*wx - cpara[0*4+1]*wy - cpara[0*4+2]*wz;//mat_c->m[j*2+0] = wz * pos2d[j][0]- cpara[0][0]*wx - cpara[0][1]*wy - cpara[0][2]*wz;\r
 307             c_array[x2+1][0]=wz * i_ppos2d[i][1]- cpara[1*4+1]*wy - cpara[1*4+2]*wz;//mat_c->m[j*2+1] = wz * pos2d[j][1]- cpara[1][1]*wy - cpara[1][2]*wz;\r
 308         }\r
 309 \r
 310         mat_e.matrixMul(i_mat_b, mat_c );\r
 311         mat_f.matrixMul(i_mat_d, mat_e );\r
 312         o_trans[0] = f_array[0][0];//trans[0] = mat_f->m[0];\r
 313         o_trans[1] = f_array[1][0];\r
 314         o_trans[2] = f_array[2][0];//trans[2] = mat_f->m[2];\r
 315         ret = transrot.modifyMatrix(o_trans, i_pos3d, i_ppos2d);\r
 316 \r
 317 //      double[][] conv=result_mat.getArray();\r
 318 //      for( i = 2; i >=0; i-- ) {//<Optimize/>for( j = 0; j < 3; j++ ) {\r
 319 //      //<Optimize>\r
 320 //      //for( i = 0; i < 3; i++ ){\r
 321 //      //      conv[j][i] = rot[j][i];\r
 322 //      //}\r
 323 //      conv[i][0] = rot[i*3+0];\r
 324 //      conv[i][1] = rot[i*3+1];\r
 325 //      conv[i][2] = rot[i*3+2];\r
 326 //      //</Optimize>\r
 327 //      conv[i][3] = trans[i];\r
 328 //      }\r
 329         return ret;\r
 330     }\r
 331 }\r