branches/nyatla/src/jp/nyatla/nyartoolkit/core/transmat/NyARTransMat_O1.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.transmat;\r
  33 \r
  34 import jp.nyatla.nyartoolkit.NyARException;\r
  35 import jp.nyatla.nyartoolkit.core.NyARMat;\r
  36 import jp.nyatla.nyartoolkit.core.NyARParam;\r
  37 import jp.nyatla.nyartoolkit.core.NyARSquare;\r
  38 import jp.nyatla.util.DoubleValue;\r
  39 \r
  40 public class NyARTransMat_O1 implements INyARTransMat{\r
  41     private final static int AR_FITTING_TO_IDEAL=0;//#define  AR_FITTING_TO_IDEAL          0\r
  42     private final static int AR_FITTING_TO_INPUT=1;//#define  AR_FITTING_TO_INPUT          1\r
  43     private final static int    arFittingMode   =AR_FITTING_TO_INPUT;\r
  44 \r
  45     private final static int AR_GET_TRANS_MAT_MAX_LOOP_COUNT=5;//#define   AR_GET_TRANS_MAT_MAX_LOOP_COUNT         5\r
  46     private final static double AR_GET_TRANS_MAT_MAX_FIT_ERROR=1.0;//#define   AR_GET_TRANS_MAT_MAX_FIT_ERROR          1.0\r
  47     private final static double AR_GET_TRANS_CONT_MAT_MAX_FIT_ERROR=1.0;\r
  48     private final static int P_MAX=10;//頂点の数(4で十分だけどなんとなく10)//#define P_MAX       500\r
  49     private final static int NUMBER_OF_VERTEX=4;//処理対象の頂点数\r
  50     private final NyARTransRot transrot;\r
  51     private final double[] center={0.0,0.0};\r
  52     private final NyARParam param;\r
  53 //    private final NyARMat result_mat=new NyARMat(3,4);\r
  54     public NyARTransMat_O1(NyARParam i_param)throws NyARException\r
  55     {\r
  56         param=i_param;\r
  57         transrot=new NyARTransRot_O3(i_param,NUMBER_OF_VERTEX);\r
  58 \r
  59     }\r
  60     public void setCenter(double i_x,double i_y)\r
  61     {\r
  62         center[0]=i_x;\r
  63         center[1]=i_x;\r
  64     }\r
  65 //    public NyARMat getTransformationMatrix()\r
  66 //    {\r
  67 //      return result_mat;\r
  68 //    }\r
  69 \r
  70     private final double[][] wk_transMat_pos3d=new double[P_MAX][3];//pos3d[P_MAX][3];\r
  71     private final double[][] wk_transMat_ppos2d=new double[4][2];\r
  72     private final double[][] wk_transMat_ppos3d=new double[4][2];\r
  73     private final double[] wk_transMat_off=new double[3];\r
  74     /**\r
  75      * double arGetTransMat( ARMarkerInfo *marker_info,double center[2], double width, double conv[3][4] )\r
  76      * 関数の置き換え。\r
  77      * 保持している変換行列を更新する。\r
  78      * @param square\r
  79      * 計算対象のNyARSquareオブジェクト\r
  80      * @param i_direction\r
  81      * マーカーの方向\r
  82      * @param i_width\r
  83      * マーカーのサイズ(mm)\r
  84      * @param o_result_conv\r
  85      * 変換行列を受け取るオブジェクトを指定します。\r
  86      * @return\r
  87      * @throws NyARException\r
  88      */\r
  89     public double transMat(NyARSquare square,int i_direction,double i_width,NyARTransMatResult o_result_conv)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] - i_width/2.0;\r
 112         ppos3d[0][1] = center[1] + i_width/2.0;\r
 113         ppos3d[1][0] = center[0] + i_width/2.0;\r
 114         ppos3d[1][1] = center[1] + i_width/2.0;\r
 115         ppos3d[2][0] = center[0] + i_width/2.0;\r
 116         ppos3d[2][1] = center[1] - i_width/2.0;\r
 117         ppos3d[3][0] = center[0] - i_width/2.0;\r
 118         ppos3d[3][1] = center[1] - i_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,o_result_conv);\r
 127             if( err < AR_GET_TRANS_MAT_MAX_FIT_ERROR ){\r
 128                 break;\r
 129             }\r
 130         }\r
 131 \r
 132         return err;\r
 133     }\r
 134     /**\r
 135      * transMatContinue用のワーク\r
 136      */\r
 137     private final NyARTransMatResult wk_transMatContinue_result=new NyARTransMatResult();\r
 138     /**\r
 139      * double arGetTransMatCont( ARMarkerInfo *marker_info, double prev_conv[3][4],double center[2], double width, double conv[3][4] )\r
 140      *     \r
 141      * @param i_square\r
 142      * @param i_direction\r
 143      * マーカーの方位を指定する。\r
 144      * @param i_width\r
 145      * @param io_result_conv\r
 146      * 計算履歴を持つNyARTransMatResultオブジェクトを指定する。\r
 147      * 履歴を持たない場合は、transMatと同じ処理を行う。\r
 148      * @return\r
 149      * @throws NyARException\r
 150      */\r
 151     public double transMatContinue(NyARSquare i_square,int i_direction, double i_width,NyARTransMatResult io_result_conv)throws NyARException\r
 152     {\r
 153         //io_result_convが初期値なら、transMatで計算する。\r
 154         if(!io_result_conv.hasValue()){\r
 155             return this.transMat(i_square, i_direction, i_width, io_result_conv);\r
 156         }\r
 157         \r
 158         \r
 159         \r
 160         double  err1,err2;\r
 161         int     i,dir;\r
 162         double[][]  ppos2d=wk_transMat_ppos2d;\r
 163         double[][]  ppos3d=wk_transMat_ppos3d;\r
 164         double[]    off   =wk_transMat_off;\r
 165         double[][]  pos3d=wk_transMat_pos3d;\r
 166 \r
 167         //arGetTransMatContSub計算部分\r
 168         transrot.initRotByPrevResult(io_result_conv);\r
 169 \r
 170         dir = i_direction;\r
 171         ppos2d[0][0] = i_square.sqvertex[(4-dir)%4][0];\r
 172         ppos2d[0][1] = i_square.sqvertex[(4-dir)%4][1];\r
 173         ppos2d[1][0] = i_square.sqvertex[(5-dir)%4][0];\r
 174         ppos2d[1][1] = i_square.sqvertex[(5-dir)%4][1];\r
 175         ppos2d[2][0] = i_square.sqvertex[(6-dir)%4][0];\r
 176         ppos2d[2][1] = i_square.sqvertex[(6-dir)%4][1];\r
 177         ppos2d[3][0] = i_square.sqvertex[(7-dir)%4][0];\r
 178         ppos2d[3][1] = i_square.sqvertex[(7-dir)%4][1];\r
 179         ppos3d[0][0] = center[0] - i_width/2.0;\r
 180         ppos3d[0][1] = center[1] + i_width/2.0;\r
 181         ppos3d[1][0] = center[0] + i_width/2.0;\r
 182         ppos3d[1][1] = center[1] + i_width/2.0;\r
 183         ppos3d[2][0] = center[0] + i_width/2.0;\r
 184         ppos3d[2][1] = center[1] - i_width/2.0;\r
 185         ppos3d[3][0] = center[0] - i_width/2.0;\r
 186         ppos3d[3][1] = center[1] - i_width/2.0;\r
 187 \r
 188         //arGetTransMat3の前段処理(pos3dとoffを初期化)\r
 189         arGetTransMat3_initPos3d(ppos3d,pos3d,off);\r
 190 \r
 191         err1=err2=-1;\r
 192         for( i = 0; i < AR_GET_TRANS_MAT_MAX_LOOP_COUNT; i++ ) {\r
 193             err1 = arGetTransMat3(ppos2d, pos3d,off,io_result_conv);\r
 194             if( err1 < AR_GET_TRANS_MAT_MAX_FIT_ERROR ){\r
 195                 //十分な精度を達成できたらブレーク\r
 196                 break;\r
 197             }\r
 198         }\r
 199 \r
 200         //エラー値が許容範囲でなければTransMatをやり直し\r
 201         if(err1>AR_GET_TRANS_CONT_MAT_MAX_FIT_ERROR ) {\r
 202             NyARTransMatResult result2=this.wk_transMatContinue_result;\r
 203             //transMatを実行\r
 204             transrot.initRot(i_square,i_direction);\r
 205             err2 = transMat(i_square,i_direction,i_width,result2);\r
 206             //transmMatここまで\r
 207             if(err2<err1){\r
 208                 io_result_conv.copyFrom(result2);\r
 209                 err1 = err2;\r
 210             }\r
 211         }\r
 212         return err1;\r
 213     }\r
 214 \r
 215 \r
 216  \r
 217 \r
 218     private final double[] wk_arGetTransMat3_initPos3d_pmax=new double[3];\r
 219     private final double[] wk_arGetTransMat3_initPos3d_pmin=new double[3];\r
 220     /**\r
 221      * arGetTransMat3関数の前処理部分。i_ppos3dから、o_pos3dとoffを計算する。\r
 222      * 計算結果から再帰的に変更される可能性が無いので、切り離し。\r
 223      * @param i_ppos3d\r
 224      * 入力配列[num][3]\r
 225      * @param o_pos3d\r
 226      * 出力配列[P_MAX][3]\r
 227      * @param o_off\r
 228      * [3]\r
 229      * @throws NyARException\r
 230      */\r
 231     private final void arGetTransMat3_initPos3d(double i_ppos3d[][],double[][] o_pos3d,double[] o_off)throws NyARException\r
 232     {\r
 233         double[] pmax=wk_arGetTransMat3_initPos3d_pmax;//new double[3];\r
 234         double[] pmin=wk_arGetTransMat3_initPos3d_pmin;//new double[3];//double  off[3], pmax[3], pmin[3];\r
 235         int i;\r
 236         pmax[0]=pmax[1]=pmax[2] = -10000000000.0;\r
 237         pmin[0]=pmin[1]=pmin[2] =  10000000000.0;\r
 238         for(i = 0; i < NUMBER_OF_VERTEX; i++ ) {\r
 239             if( i_ppos3d[i][0] > pmax[0] ){\r
 240                 pmax[0] = i_ppos3d[i][0];\r
 241             }\r
 242             if( i_ppos3d[i][0] < pmin[0] ){\r
 243                 pmin[0] = i_ppos3d[i][0];\r
 244             }\r
 245             if( i_ppos3d[i][1] > pmax[1] ){\r
 246                 pmax[1] = i_ppos3d[i][1];\r
 247             }\r
 248             if( i_ppos3d[i][1] < pmin[1] ){\r
 249                 pmin[1] = i_ppos3d[i][1];\r
 250             }\r
 251             /*  オリジナルでもコメントアウト\r
 252                 if( ppos3d[i][2] > pmax[2] ) pmax[2] = ppos3d[i][2];\r
 253                 if( ppos3d[i][2] < pmin[2] ) pmin[2] = ppos3d[i][2];\r
 254              */     \r
 255         }\r
 256         o_off[0] = -(pmax[0] + pmin[0]) / 2.0;\r
 257         o_off[1] = -(pmax[1] + pmin[1]) / 2.0;\r
 258         o_off[2] = -(pmax[2] + pmin[2]) / 2.0;\r
 259 \r
 260         \r
 261         double[] o_pos3d_pt;\r
 262         double[] i_pos_pd_pt;   \r
 263         for(i = 0; i < NUMBER_OF_VERTEX; i++ ) {\r
 264             o_pos3d_pt =o_pos3d[i];\r
 265             i_pos_pd_pt=i_ppos3d[i];\r
 266             o_pos3d_pt[0] = i_pos_pd_pt[0] + o_off[0];\r
 267             o_pos3d_pt[1] = i_pos_pd_pt[1] + o_off[1];\r
 268             o_pos3d_pt[2] = 0.0;\r
 269         }\r
 270     }    \r
 271     \r
 272      \r
 273     /**\r
 274      * 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
 275      * STEP 414->\r
 276      * @param ppos2d\r
 277      * @param i_pos3d\r
 278      * @param i_off\r
 279      * @param num\r
 280      * @return\r
 281      * @throws NyARException\r
 282      */\r
 283     private final double arGetTransMat3(\r
 284             double ppos2d[][],\r
 285             final double i_pos3d[][],\r
 286             final double i_off[],\r
 287             NyARTransMatResult o_result_conv)throws NyARException{\r
 288 \r
 289         double  ret;\r
 290         ret = arGetTransMatSub(ppos2d,i_pos3d,i_off,o_result_conv);\r
 291         \r
 292 //        double[][] conv=o_result_conv.getArray();\r
 293 //        double[] rot=transrot.getArray();\r
 294 //        for(int i=0;i<3;i++){\r
 295 //            conv[i][0] = rot[i*3+0];\r
 296 //            conv[i][1] = rot[i*3+1];\r
 297 //            conv[i][2] = rot[i*3+2];\r
 298 //        }\r
 299 //      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
 300 //      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
 301 //      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
 302         return ret;\r
 303     }\r
 304     private final NyARMat wk_arGetTransMatSub_mat_a=new NyARMat(NUMBER_OF_VERTEX*2,3);\r
 305     private final NyARMat wk_arGetTransMatSub_mat_b=new NyARMat(3,NUMBER_OF_VERTEX*2);\r
 306     private final NyARMat wk_arGetTransMatSub_mat_c=new NyARMat(NUMBER_OF_VERTEX*2,1);\r
 307     private final NyARMat wk_arGetTransMatSub_mat_d=new NyARMat( 3, 3 );\r
 308     private final NyARMat wk_arGetTransMatSub_mat_e=new NyARMat( 3, 1 );\r
 309     private final NyARMat wk_arGetTransMatSub_mat_f=new NyARMat( 3, 1 );\r
 310     private final double[] wk_arGetTransMatSub_trans=new double[3];\r
 311     private final double[][] wk_arGetTransMatSub_pos2d=new double[P_MAX][2];//pos2d[P_MAX][2];\r
 312     private final DoubleValue wk_arGetTransMatSub_a1=new DoubleValue();\r
 313     private final DoubleValue wk_arGetTransMatSub_a2=new DoubleValue();\r
 314     \r
 315 \r
 316     /**\r
 317      * 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
 318      * Optimize:2008.04.20:STEP[1033→1004]\r
 319      * @param i_ppos2d\r
 320      * @param i_pos3d\r
 321      * @return\r
 322      * @throws NyARException\r
 323      */\r
 324     private final double arGetTransMatSub(double i_ppos2d[][],double i_pos3d[][],double[] i_off,NyARTransMatResult o_result_conv) throws NyARException\r
 325     {\r
 326         double[][] pos2d=wk_arGetTransMatSub_pos2d;\r
 327         double cpara[]=param.get34Array();\r
 328         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
 329 \r
 330         double  wx, wy, wz;\r
 331         double  ret;\r
 332         int     i;\r
 333         double[] po2d_pt;\r
 334 \r
 335         mat_a =this.wk_arGetTransMatSub_mat_a;\r
 336 //      mat_a.realloc(NUMBER_OF_VERTEX*2,3);\r
 337         double[][] a_array=mat_a.getArray();\r
 338 \r
 339         mat_b =this.wk_arGetTransMatSub_mat_b;\r
 340 //      mat_b.realloc(3,NUMBER_OF_VERTEX*2);\r
 341         double[][] b_array=mat_b.getArray();\r
 342 \r
 343 \r
 344         DoubleValue a1=wk_arGetTransMatSub_a1;\r
 345         DoubleValue a2=wk_arGetTransMatSub_a2;\r
 346         if(arFittingMode == AR_FITTING_TO_INPUT ){\r
 347             for( i = 0; i < NUMBER_OF_VERTEX; i++ ) {\r
 348                 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
 349                 po2d_pt=pos2d[i];\r
 350                 po2d_pt[0]=a1.value;\r
 351                 po2d_pt[1]=a2.value;\r
 352             }\r
 353         }else{\r
 354             for( i = 0; i < NUMBER_OF_VERTEX; i++ ){\r
 355                 po2d_pt=pos2d[i];\r
 356                 pos2d[i][0] = i_ppos2d[i][0];\r
 357                 pos2d[i][1] = i_ppos2d[i][1];\r
 358             }\r
 359         }\r
 360         mat_c =this.wk_arGetTransMatSub_mat_c;//次処理で値をもらうので、初期化の必要は無い。\r
 361 //      mat_c.realloc(NUMBER_OF_VERTEX*2,1);\r
 362         double[][] c_array=mat_c.getArray();\r
 363         double[] rot=transrot.getArray();\r
 364         double[] i_pos3d_pt;\r
 365         int x2;\r
 366         for( i = 0; i < NUMBER_OF_VERTEX; i++ ) {\r
 367             x2=i*2;\r
 368             i_pos3d_pt=i_pos3d[i];\r
 369             po2d_pt=pos2d[i];\r
 370             wx = rot[0] * i_pos3d_pt[0]+ rot[1] * i_pos3d_pt[1]+ rot[2] * i_pos3d_pt[2];\r
 371             wy = rot[3] * i_pos3d_pt[0]+ rot[4] * i_pos3d_pt[1]+ rot[5] * i_pos3d_pt[2];\r
 372             wz = rot[6] * i_pos3d_pt[0]+ rot[7] * i_pos3d_pt[1]+ rot[8] * i_pos3d_pt[2];\r
 373             //</Optimize>\r
 374             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
 375             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
 376             a_array[x2  ][2]=b_array[2][x2]=cpara[0*4+2] - po2d_pt[0];//mat_a->m[j*6+2] = mat_b->m[num*4+j*2] = cpara[0][2] - pos2d[j][0];\r
 377             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
 378             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
 379             a_array[x2+1][2]=b_array[2][x2+1]=cpara[1*4+2] - po2d_pt[1];//mat_a->m[j*6+5] = mat_b->m[num*4+j*2+1] = cpara[1][2] - pos2d[j][1];\r
 380             c_array[x2][0]  =wz * po2d_pt[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
 381             c_array[x2+1][0]=wz * po2d_pt[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
 382         }\r
 383         mat_d = this.wk_arGetTransMatSub_mat_d;//次処理で値をもらうので、初期化の必要は無い。\r
 384         mat_e = this.wk_arGetTransMatSub_mat_e;//次処理で値をもらうので、初期化の必要は無い。\r
 385         mat_f = this.wk_arGetTransMatSub_mat_f;//次処理で値をもらうので、初期化の必要は無い。\r
 386         double[][] f_array=mat_f.getArray();\r
 387 \r
 388         mat_d.matrixMul(mat_b, mat_a );\r
 389         mat_e.matrixMul(mat_b, mat_c );\r
 390         mat_d.matrixSelfInv();\r
 391         mat_f.matrixMul(mat_d, mat_e );\r
 392 \r
 393         double[] trans=wk_arGetTransMatSub_trans;//double  trans[3];    \r
 394         trans[0] = f_array[0][0];//trans[0] = mat_f->m[0];\r
 395         trans[1] = f_array[1][0];\r
 396         trans[2] = f_array[2][0];//trans[2] = mat_f->m[2];\r
 397 \r
 398         ret =transrot.modifyMatrix(trans, i_pos3d, pos2d);\r
 399         for( i = 0; i < NUMBER_OF_VERTEX; i++ ) {\r
 400             x2=i*2;\r
 401             i_pos3d_pt=i_pos3d[i];\r
 402             po2d_pt=pos2d[i];\r
 403             //<Optimize>    \r
 404             //wx = rot[0][0] * pos3d[j][0]+ rot[0][1] * pos3d[j][1]+ rot[0][2] * pos3d[j][2];\r
 405             //wy = rot[1][0] * pos3d[j][0]+ rot[1][1] * pos3d[j][1]+ rot[1][2] * pos3d[j][2];\r
 406             //wz = rot[2][0] * pos3d[j][0]+ rot[2][1] * pos3d[j][1]+ rot[2][2] * pos3d[j][2];\r
 407             wx = rot[0] * i_pos3d_pt[0]+ rot[1] * i_pos3d_pt[1]+ rot[2] * i_pos3d_pt[2];\r
 408             wy = rot[3] * i_pos3d_pt[0]+ rot[4] * i_pos3d_pt[1]+ rot[5] * i_pos3d_pt[2];\r
 409             wz = rot[6] * i_pos3d_pt[0]+ rot[7] * i_pos3d_pt[1]+ rot[8] * i_pos3d_pt[2];\r
 410             //</Optimize>\r
 411             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
 412             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
 413             a_array[x2  ][2]=b_array[2][x2]=cpara[0*4+2] - po2d_pt[0];//mat_a->m[j*6+2] = mat_b->m[num*4+j*2] = cpara[0][2] - pos2d[j][0];\r
 414             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
 415             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
 416             a_array[x2+1][2]=b_array[2][x2+1]=cpara[1*4+2] - po2d_pt[1];//mat_a->m[j*6+5] = mat_b->m[num*4+j*2+1] = cpara[1][2] - pos2d[j][1];\r
 417             c_array[x2][0]  =wz * po2d_pt[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
 418             c_array[x2+1][0]=wz * po2d_pt[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
 419         }\r
 420 \r
 421         mat_d.matrixMul(mat_b, mat_a );\r
 422         mat_e.matrixMul(mat_b, mat_c );\r
 423         mat_d.matrixSelfInv();\r
 424         mat_f.matrixMul(mat_d, mat_e );\r
 425 \r
 426         trans[0] = f_array[0][0];//trans[0] = mat_f->m[0];\r
 427         trans[1] = f_array[1][0];\r
 428         trans[2] = f_array[2][0];//trans[2] = mat_f->m[2];\r
 429 \r
 430 \r
 431         ret = transrot.modifyMatrix(trans, i_pos3d, pos2d);\r
 432         //変換行列を保存\r
 433         o_result_conv.updateMatrixValue(this.transrot,i_off,trans);\r
 434         \r
 435 //      double[][] conv=o_result_conv.getArray();\r
 436 //      for( i = 2; i >=0; i-- ) {//<Optimize/>for( j = 0; j < 3; j++ ) {\r
 437 //          //<Optimize>\r
 438 //          //for( i = 0; i < 3; i++ ){\r
 439 //          //  conv[j][i] = rot[j][i];\r
 440 //          //}\r
 441 //          conv[i][0] = rot[i*3+0];\r
 442 //          conv[i][1] = rot[i*3+1];\r
 443 //          conv[i][2] = rot[i*3+2];\r
 444 //          //</Optimize>\r
 445 //          conv[i][3] = trans[i];\r
 446 //      }\r
 447         return ret;\r
 448     }\r
 449 }\r