src/jp/nyatla/nyartoolkit/core/NyARTransMat.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  * This class calculates ARMatrix from square information and holds it.\r
  41  * --\r
  42  * 変換行列を計算して、結果を保持するクラス。\r
  43  *\r
  44  */\r
  45 public class NyARTransMat{\r
  46     private final static int AR_FITTING_TO_IDEAL=0;//#define  AR_FITTING_TO_IDEAL          0\r
  47     private final static int AR_FITTING_TO_INPUT=1;//#define  AR_FITTING_TO_INPUT          1\r
  48     private final static int    arFittingMode   =AR_FITTING_TO_INPUT;\r
  49 \r
  50     private final static int AR_GET_TRANS_MAT_MAX_LOOP_COUNT=5;//#define   AR_GET_TRANS_MAT_MAX_LOOP_COUNT         5\r
  51     private final static double AR_GET_TRANS_MAT_MAX_FIT_ERROR=1.0;//#define   AR_GET_TRANS_MAT_MAX_FIT_ERROR          1.0\r
  52     private final static int P_MAX=10;//頂点の数(4で十分だけどなんとなく10)//#define P_MAX       500\r
  53     private final static int NUMBER_OF_VERTEX=4;//処理対象の頂点数\r
  54     private final NyARTransRot transrot;\r
  55     private final double[] center={0.0,0.0};\r
  56     private final NyARParam param;\r
  57     private final NyARMat result_mat=new NyARMat(3,4);\r
  58     public NyARTransMat(NyARParam i_param)throws NyARException\r
  59     {\r
  60         param=i_param;\r
  61         transrot=new NyARTransRot_O3(i_param,NUMBER_OF_VERTEX);\r
  62 \r
  63     }\r
  64     public void setCenter(double i_x,double i_y)\r
  65     {\r
  66         center[0]=i_x;\r
  67         center[1]=i_x;\r
  68     }\r
  69     public NyARMat getTransformationMatrix()\r
  70     {\r
  71         return result_mat;\r
  72     }\r
  73 \r
  74     private final double[] wk_transMat_pos3d=new double[P_MAX*3];//pos3d[P_MAX][3];\r
  75     private final double[][] wk_transMat_ppos2d=new double[4][2];\r
  76     private final double[][] wk_transMat_ppos3d=new double[4][2];\r
  77     private final double[] wk_transMat_off=new double[3];\r
  78     /**\r
  79      * double arGetTransMat( ARMarkerInfo *marker_info,double center[2], double width, double conv[3][4] )\r
  80      * 関数の置き換え。\r
  81      * 保持している変換行列を更新する。\r
  82      * @param square\r
  83      * 計算対象のNyARSquareオブジェクト\r
  84      * @param i_direction\r
  85      * @param width\r
  86      * @return\r
  87      * @throws NyARException\r
  88      */\r
  89     public double transMat( NyARSquare square,int i_direction, double width)throws NyARException\r
  90     {\r
  91         double[][]  ppos2d=wk_transMat_ppos2d;\r
  92         double[][]  ppos3d=wk_transMat_ppos3d;\r
  93         double[] off=wk_transMat_off;\r
  94         double[] pos3d=wk_transMat_pos3d;\r
  95         int     dir;\r
  96         double  err=-1;\r
  97 \r
  98 \r
  99 \r
 100         transrot.initRot(square,i_direction);\r
 101 \r
 102         dir = i_direction;\r
 103         ppos2d[0][0] = square.sqvertex[(4-dir)%4][0];\r
 104         ppos2d[0][1] = square.sqvertex[(4-dir)%4][1];\r
 105         ppos2d[1][0] = square.sqvertex[(5-dir)%4][0];\r
 106         ppos2d[1][1] = square.sqvertex[(5-dir)%4][1];\r
 107         ppos2d[2][0] = square.sqvertex[(6-dir)%4][0];\r
 108         ppos2d[2][1] = square.sqvertex[(6-dir)%4][1];\r
 109         ppos2d[3][0] = square.sqvertex[(7-dir)%4][0];\r
 110         ppos2d[3][1] = square.sqvertex[(7-dir)%4][1];\r
 111         ppos3d[0][0] = center[0] - width/2.0;\r
 112         ppos3d[0][1] = center[1] + width/2.0;\r
 113         ppos3d[1][0] = center[0] + width/2.0;\r
 114         ppos3d[1][1] = center[1] + width/2.0;\r
 115         ppos3d[2][0] = center[0] + width/2.0;\r
 116         ppos3d[2][1] = center[1] - width/2.0;\r
 117         ppos3d[3][0] = center[0] - width/2.0;\r
 118         ppos3d[3][1] = center[1] - width/2.0;\r
 119 \r
 120         //arGetTransMat3の前段処理(pos3dとoffを初期化)\r
 121         arGetTransMat3_initPos3d(ppos3d,pos3d,off);\r
 122 \r
 123 \r
 124 \r
 125         for(int i=0;i<AR_GET_TRANS_MAT_MAX_LOOP_COUNT; i++ ) {\r
 126             err = arGetTransMat3(ppos2d, pos3d,off);\r
 127             if( err < AR_GET_TRANS_MAT_MAX_FIT_ERROR ){\r
 128                 break;\r
 129             }\r
 130         }\r
 131         return err;\r
 132     }\r
 133     private final double[] wk_arGetTransMat3_initPos3d_pmax=new double[3];\r
 134     private final double[] wk_arGetTransMat3_initPos3d_pmin=new double[3];\r
 135     /**\r
 136      * arGetTransMat3関数の前処理部分。i_ppos3dから、o_pos3dとoffを計算する。\r
 137      * 計算結果から再帰的に変更される可能性が無いので、切り離し。\r
 138      * @param i_ppos3d\r
 139      * 入力配列[num][3]\r
 140      * @param o_pos3d\r
 141      * 出力配列[P_MAX][3]\r
 142      * @param o_off\r
 143      * [3]\r
 144      * @throws NyARException\r
 145      */\r
 146     private final void arGetTransMat3_initPos3d(double i_ppos3d[][],double[] o_pos3d,double[] o_off)throws NyARException\r
 147     {\r
 148         double[] pmax=wk_arGetTransMat3_initPos3d_pmax;//new double[3];\r
 149         double[] pmin=wk_arGetTransMat3_initPos3d_pmin;//new double[3];//double  off[3], pmax[3], pmin[3];\r
 150         pmax[0]=pmax[1]=pmax[2] = -10000000000.0;\r
 151         pmin[0]=pmin[1]=pmin[2] =  10000000000.0;\r
 152         for(int i = 0; i < NUMBER_OF_VERTEX; i++ ) {\r
 153             if( i_ppos3d[i][0] > pmax[0] ){\r
 154                 pmax[0] = i_ppos3d[i][0];\r
 155             }\r
 156             if( i_ppos3d[i][0] < pmin[0] ){\r
 157                 pmin[0] = i_ppos3d[i][0];\r
 158             }\r
 159             if( i_ppos3d[i][1] > pmax[1] ){\r
 160                 pmax[1] = i_ppos3d[i][1];\r
 161             }\r
 162             if( i_ppos3d[i][1] < pmin[1] ){\r
 163                 pmin[1] = i_ppos3d[i][1];\r
 164             }\r
 165         }\r
 166         o_off[0] = -(pmax[0] + pmin[0]) / 2.0;\r
 167         o_off[1] = -(pmax[1] + pmin[1]) / 2.0;\r
 168         o_off[2] = -(pmax[2] + pmin[2]) / 2.0;\r
 169         for(int i = 0; i < NUMBER_OF_VERTEX; i++ ) {\r
 170             o_pos3d[i*3+0] = i_ppos3d[i][0] + o_off[0];\r
 171             o_pos3d[i*3+1] = i_ppos3d[i][1] + o_off[1];\r
 172             o_pos3d[i*3+2] = 0.0;\r
 173         }\r
 174     }    \r
 175     \r
 176      \r
 177     /**\r
 178      * double arGetTransMat3( double rot[3][3], double ppos2d[][2],double ppos3d[][2], int num, double conv[3][4],double *dist_factor, double cpara[3][4] )\r
 179      * STEP 414->\r
 180      * @param ppos2d\r
 181      * @param i_pos3d\r
 182      * @param i_off\r
 183      * @param num\r
 184      * @return\r
 185      * @throws NyARException\r
 186      */\r
 187     private final double arGetTransMat3(\r
 188             double ppos2d[][],\r
 189             final double i_pos3d[],\r
 190             final double i_off[])throws NyARException{\r
 191         \r
 192         double  ret;\r
 193         ret = arGetTransMatSub(ppos2d, i_pos3d);\r
 194         double[][] conv=result_mat.getArray();\r
 195         conv[0][3] = conv[0][0]*i_off[0] + conv[0][1]*i_off[1] + conv[0][2]*i_off[2] + conv[0][3];\r
 196         conv[1][3] = conv[1][0]*i_off[0] + conv[1][1]*i_off[1] + conv[1][2]*i_off[2] + conv[1][3];\r
 197         conv[2][3] = conv[2][0]*i_off[0] + conv[2][1]*i_off[1] + conv[2][2]*i_off[2] + conv[2][3];\r
 198         return ret;\r
 199     }\r
 200     private final NyARMat wk_arGetTransMatSub_mat_a=new NyARMat(NUMBER_OF_VERTEX*2,3);\r
 201     private final NyARMat wk_arGetTransMatSub_mat_b=new NyARMat(3,NUMBER_OF_VERTEX*2);\r
 202     private final NyARMat wk_arGetTransMatSub_mat_c=new NyARMat(NUMBER_OF_VERTEX*2,1);\r
 203     private final NyARMat wk_arGetTransMatSub_mat_d=new NyARMat( 3, 3 );\r
 204     private final NyARMat wk_arGetTransMatSub_mat_e=new NyARMat( 3, 1 );\r
 205     private final NyARMat wk_arGetTransMatSub_mat_f=new NyARMat( 3, 1 );\r
 206     private final double[] wk_arGetTransMatSub_trans=new double[3];\r
 207     private final double[] wk_arGetTransMatSub_pos2d=new double[P_MAX*2];//pos2d[P_MAX][2];\r
 208     private final DoubleValue wk_arGetTransMatSub_a1=new DoubleValue();\r
 209     private final DoubleValue wk_arGetTransMatSub_a2=new DoubleValue();\r
 210     \r
 211 \r
 212     /**\r
 213      * 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
 214      * Optimize:2008.04.20:STEP[1033→1004]\r
 215      * @param rot\r
 216      * @param ppos2d\r
 217      * @param pos3d\r
 218      * @param num\r
 219      * @return\r
 220      * @throws NyARException\r
 221      */\r
 222     private final double arGetTransMatSub(double i_ppos2d[][],double i_pos3d[]) throws NyARException\r
 223     {\r
 224         double[] pos2d=wk_arGetTransMatSub_pos2d;\r
 225         double cpara[]=param.get34Array();\r
 226         NyARMat mat_a,mat_b,mat_c,mat_d,mat_e,mat_f;//ARMat   *mat_a, *mat_b, *mat_c, *mat_d, *mat_e, *mat_f;\r
 227 \r
 228         double  wx, wy, wz;\r
 229         double  ret;\r
 230         int     i, j;\r
 231 \r
 232         mat_a =this.wk_arGetTransMatSub_mat_a;\r
 233 //      mat_a.realloc(NUMBER_OF_VERTEX*2,3);\r
 234         double[][] a_array=mat_a.getArray();\r
 235 \r
 236         mat_b =this.wk_arGetTransMatSub_mat_b;\r
 237 //      mat_b.realloc(3,NUMBER_OF_VERTEX*2);\r
 238         double[][] b_array=mat_b.getArray();\r
 239 \r
 240 \r
 241         DoubleValue a1=wk_arGetTransMatSub_a1;\r
 242         DoubleValue a2=wk_arGetTransMatSub_a2;\r
 243         if(arFittingMode == AR_FITTING_TO_INPUT ){\r
 244             for( i = 0; i < NUMBER_OF_VERTEX; i++ ) {\r
 245                 param.ideal2Observ(i_ppos2d[i][0], i_ppos2d[i][1],a1,a2);//arParamIdeal2Observ(dist_factor, ppos2d[i][0], ppos2d[i][1],&pos2d[i][0], &pos2d[i][1]);\r
 246                 pos2d[i*2+0]=a1.value;\r
 247                 pos2d[i*2+1]=a2.value;\r
 248             }\r
 249         }else{\r
 250             for( i = 0; i < NUMBER_OF_VERTEX; i++ ) {\r
 251                 pos2d[i*2+0] = i_ppos2d[i][0];\r
 252                 pos2d[i*2+1] = i_ppos2d[i][1];\r
 253             }\r
 254         }\r
 255         mat_c =this.wk_arGetTransMatSub_mat_c;//次処理で値をもらうので、初期化の必要は無い。\r
 256 //      mat_c.realloc(NUMBER_OF_VERTEX*2,1);\r
 257         double[][] c_array=mat_c.getArray();\r
 258         double[] rot=transrot.getArray();\r
 259         for( j = 0; j < NUMBER_OF_VERTEX; j++ ) {\r
 260             int x2=j*2;\r
 261             //<Optimize>    \r
 262             //wx = rot[0][0] * pos3d[j][0]+ rot[0][1] * pos3d[j][1]+ rot[0][2] * pos3d[j][2];\r
 263             //wy = rot[1][0] * pos3d[j][0]+ rot[1][1] * pos3d[j][1]+ rot[1][2] * pos3d[j][2];\r
 264             //wz = rot[2][0] * pos3d[j][0]+ rot[2][1] * pos3d[j][1]+ rot[2][2] * pos3d[j][2];\r
 265             wx = rot[0] * i_pos3d[j*3+0]+ rot[1] * i_pos3d[j*3+1]+ rot[2] * i_pos3d[j*3+2];\r
 266             wy = rot[3] * i_pos3d[j*3+0]+ rot[4] * i_pos3d[j*3+1]+ rot[5] * i_pos3d[j*3+2];\r
 267             wz = rot[6] * i_pos3d[j*3+0]+ rot[7] * i_pos3d[j*3+1]+ rot[8] * i_pos3d[j*3+2];\r
 268             //</Optimize>\r
 269             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
 270             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
 271             a_array[x2  ][2]=b_array[2][x2]=cpara[0*4+2] - pos2d[j*2+0];//mat_a->m[j*6+2] = mat_b->m[num*4+j*2] = cpara[0][2] - pos2d[j][0];\r
 272             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
 273             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
 274             a_array[x2+1][2]=b_array[2][x2+1]=cpara[1*4+2] - pos2d[j*2+1];//mat_a->m[j*6+5] = mat_b->m[num*4+j*2+1] = cpara[1][2] - pos2d[j][1];\r
 275             c_array[x2][0]  =wz * pos2d[j*2+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
 276             c_array[x2+1][0]=wz * pos2d[j*2+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
 277         }\r
 278         mat_d = this.wk_arGetTransMatSub_mat_d;//次処理で値をもらうので、初期化の必要は無い。\r
 279         mat_e = this.wk_arGetTransMatSub_mat_e;//次処理で値をもらうので、初期化の必要は無い。\r
 280         mat_f = this.wk_arGetTransMatSub_mat_f;//次処理で値をもらうので、初期化の必要は無い。\r
 281         double[][] f_array=mat_f.getArray();\r
 282 \r
 283         mat_d.matrixMul(mat_b, mat_a );\r
 284         mat_e.matrixMul(mat_b, mat_c );\r
 285         mat_d.matrixSelfInv();\r
 286         mat_f.matrixMul(mat_d, mat_e );\r
 287 \r
 288         double[] trans=wk_arGetTransMatSub_trans;//double  trans[3];    \r
 289         trans[0] = f_array[0][0];//trans[0] = mat_f->m[0];\r
 290         trans[1] = f_array[1][0];\r
 291         trans[2] = f_array[2][0];//trans[2] = mat_f->m[2];\r
 292 \r
 293         ret =transrot.modifyMatrix(trans, i_pos3d, pos2d);\r
 294         for( j = 0; j < NUMBER_OF_VERTEX; j++ ) {\r
 295             int x2=j*2;\r
 296             //<Optimize>    \r
 297             //wx = rot[0][0] * pos3d[j][0]+ rot[0][1] * pos3d[j][1]+ rot[0][2] * pos3d[j][2];\r
 298             //wy = rot[1][0] * pos3d[j][0]+ rot[1][1] * pos3d[j][1]+ rot[1][2] * pos3d[j][2];\r
 299             //wz = rot[2][0] * pos3d[j][0]+ rot[2][1] * pos3d[j][1]+ rot[2][2] * pos3d[j][2];\r
 300             wx = rot[0] * i_pos3d[j*3+0]+ rot[1] * i_pos3d[j*3+1]+ rot[2] * i_pos3d[j*3+2];\r
 301             wy = rot[3] * i_pos3d[j*3+0]+ rot[4] * i_pos3d[j*3+1]+ rot[5] * i_pos3d[j*3+2];\r
 302             wz = rot[6] * i_pos3d[j*3+0]+ rot[7] * i_pos3d[j*3+1]+ rot[8] * i_pos3d[j*3+2];\r
 303             //</Optimize>\r
 304             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
 305             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
 306             a_array[x2  ][2]=b_array[2][x2]=cpara[0*4+2] - pos2d[j*2+0];//mat_a->m[j*6+2] = mat_b->m[num*4+j*2] = cpara[0][2] - pos2d[j][0];\r
 307             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
 308             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
 309             a_array[x2+1][2]=b_array[2][x2+1]=cpara[1*4+2] - pos2d[j*2+1];//mat_a->m[j*6+5] = mat_b->m[num*4+j*2+1] = cpara[1][2] - pos2d[j][1];\r
 310             c_array[x2][0]  =wz * pos2d[j*2+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
 311             c_array[x2+1][0]=wz * pos2d[j*2+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
 312         }\r
 313 //      JartkException.trap("未チェックのパス");{\r
 314         mat_d.matrixMul(mat_b, mat_a );\r
 315         mat_e.matrixMul(mat_b, mat_c );\r
 316         mat_d.matrixSelfInv();\r
 317         mat_f.matrixMul(mat_d, mat_e );\r
 318 //      }\r
 319         trans[0] = f_array[0][0];//trans[0] = mat_f->m[0];\r
 320         trans[1] = f_array[1][0];\r
 321         trans[2] = f_array[2][0];//trans[2] = mat_f->m[2];\r
 322 \r
 323 \r
 324         ret = transrot.modifyMatrix(trans, i_pos3d, pos2d);\r
 325         double[][] conv=result_mat.getArray();\r
 326         for( j = 2; j >=0; j-- ) {//<Optimize/>for( j = 0; j < 3; j++ ) {\r
 327             //<Optimize>\r
 328             //for( i = 0; i < 3; i++ ){\r
 329             //  conv[j][i] = rot[j][i];\r
 330             //}\r
 331             conv[j][0] = rot[j*3+0];\r
 332             conv[j][1] = rot[j*3+1];\r
 333             conv[j][2] = rot[j*3+2];\r
 334             //</Optimize>\r
 335             conv[j][3] = trans[j];\r
 336         }\r
 337         return ret;\r
 338     }\r
 339 }\r