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