branches/1.2.0-last/src/jp/nyatla/nyartoolkit/core/NyARTransRot.java

   1 package jp.nyatla.nyartoolkit.core;\r
   2 \r
   3 import jp.nyatla.nyartoolkit.NyARException;\r
   4 \r
   5 interface NyARTransRot\r
   6 {\r
   7     public double[] getArray();\r
   8     /**\r
   9      * \r
  10      * @param trans\r
  11      * @param vertex\r
  12      * @param pos2d\r
  13      * [n*2]配列\r
  14      * @return\r
  15      * @throws NyARException\r
  16      */\r
  17     public double modifyMatrix(double trans[],double vertex[][], double pos2d[][]) throws NyARException;\r
  18     public void initRot(NyARSquare marker_info,int i_direction) throws NyARException;\r
  19     public void initRotByPrevResult(NyARTransMatResult i_prev_result);\r
  20 }\r
  21 \r
  22 /**\r
  23  * NyARTransRot派生クラスで共通に使いそうな関数類をまとめたもの。\r
  24  *\r
  25  */\r
  26 abstract class NyARTransRot_OptimizeCommon implements NyARTransRot\r
  27 {\r
  28     protected final int number_of_vertex;\r
  29     protected final double[] array=new double[9];\r
  30     protected final NyARParam cparam;\r
  31     public final void initRotByPrevResult(NyARTransMatResult i_prev_result)\r
  32     {\r
  33         double[][] prev_array=i_prev_result.getArray();\r
  34         double[] pt;\r
  35         final double[] L_rot=this.array;\r
  36         pt=prev_array[0];\r
  37         L_rot[0*3+0]=pt[0];\r
  38         L_rot[0*3+1]=pt[1];\r
  39         L_rot[0*3+2]=pt[2];\r
  40         pt=prev_array[1];\r
  41         L_rot[1*3+0]=pt[0];\r
  42         L_rot[1*3+1]=pt[1];\r
  43         L_rot[1*3+2]=pt[2];\r
  44         pt=prev_array[2];\r
  45         L_rot[2*3+0]=pt[0];\r
  46         L_rot[2*3+1]=pt[1];\r
  47         L_rot[2*3+2]=pt[2];\r
  48     }\r
  49     public final double[] getArray()\r
  50     {\r
  51         return this.array;\r
  52     }\r
  53     /**\r
  54      * インスタンスを準備します。\r
  55      * @param i_param\r
  56      * nullを指定した場合、一部の関数が使用不能になります。\r
  57      */\r
  58     public NyARTransRot_OptimizeCommon(NyARParam i_param,int i_number_of_vertex) throws NyARException\r
  59     {\r
  60         number_of_vertex=i_number_of_vertex;\r
  61         cparam=i_param;\r
  62     }\r
  63 \r
  64     private final double[] wk_check_dir_world=new double[6];\r
  65     private final double[] wk_check_dir_camera=new double[4];\r
  66     private final NyARMat wk_check_dir_NyARMat=new NyARMat( 3, 3 );\r
  67     /**\r
  68      * static int check_dir( double dir[3], double st[2], double ed[2],double cpara[3][4] )\r
  69      * Optimize:STEP[526->468]\r
  70      * @param dir\r
  71      * @param st\r
  72      * @param ed\r
  73      * @param cpara\r
  74      * \r
  75      * @throws NyARException\r
  76      */\r
  77     protected final void check_dir( double dir[], double st[], double ed[],double cpara[]) throws NyARException\r
  78     {\r
  79         double    h;\r
  80         int       i, j;\r
  81 \r
  82         NyARMat mat_a = this.wk_check_dir_NyARMat;//ここ、事前に初期化できそう\r
  83         double[][] a_array=mat_a.getArray();\r
  84         for(j=0;j<3;j++){\r
  85             for(i=0;i<3;i++){\r
  86                 a_array[j][i]=cpara[j*4+i];//m[j*3+i] = cpara[j][i];\r
  87             }\r
  88             \r
  89         }\r
  90         //      JartkException.trap("未チェックのパス");\r
  91         mat_a.matrixSelfInv();\r
  92         double[] world=wk_check_dir_world;//[2][3];\r
  93         //<Optimize>\r
  94         //world[0][0] = a_array[0][0]*st[0]*10.0+ a_array[0][1]*st[1]*10.0+ a_array[0][2]*10.0;//mat_a->m[0]*st[0]*10.0+ mat_a->m[1]*st[1]*10.0+ mat_a->m[2]*10.0;\r
  95         //world[0][1] = a_array[1][0]*st[0]*10.0+ a_array[1][1]*st[1]*10.0+ a_array[1][2]*10.0;//mat_a->m[3]*st[0]*10.0+ mat_a->m[4]*st[1]*10.0+ mat_a->m[5]*10.0;\r
  96         //world[0][2] = a_array[2][0]*st[0]*10.0+ a_array[2][1]*st[1]*10.0+ a_array[2][2]*10.0;//mat_a->m[6]*st[0]*10.0+ mat_a->m[7]*st[1]*10.0+ mat_a->m[8]*10.0;\r
  97         //world[1][0] = world[0][0] + dir[0];\r
  98         //world[1][1] = world[0][1] + dir[1];\r
  99         //world[1][2] = world[0][2] + dir[2];\r
 100         world[0] = a_array[0][0]*st[0]*10.0+ a_array[0][1]*st[1]*10.0+ a_array[0][2]*10.0;//mat_a->m[0]*st[0]*10.0+ mat_a->m[1]*st[1]*10.0+ mat_a->m[2]*10.0;\r
 101         world[1] = a_array[1][0]*st[0]*10.0+ a_array[1][1]*st[1]*10.0+ a_array[1][2]*10.0;//mat_a->m[3]*st[0]*10.0+ mat_a->m[4]*st[1]*10.0+ mat_a->m[5]*10.0;\r
 102         world[2] = a_array[2][0]*st[0]*10.0+ a_array[2][1]*st[1]*10.0+ a_array[2][2]*10.0;//mat_a->m[6]*st[0]*10.0+ mat_a->m[7]*st[1]*10.0+ mat_a->m[8]*10.0;\r
 103         world[3] = world[0] + dir[0];\r
 104         world[4] = world[1] + dir[1];\r
 105         world[5] = world[2] + dir[2];\r
 106         //</Optimize>\r
 107 \r
 108         double[] camera=wk_check_dir_camera;//[2][2];\r
 109         for( i = 0; i < 2; i++ ) {\r
 110             h = cpara[2*4+0] * world[i*3+0]+ cpara[2*4+1] * world[i*3+1]+ cpara[2*4+2] * world[i*3+2];\r
 111             if( h == 0.0 ){\r
 112                 throw new NyARException();\r
 113             }\r
 114             camera[i*2+0] = (cpara[0*4+0] * world[i*3+0]+ cpara[0*4+1] * world[i*3+1]+ cpara[0*4+2] * world[i*3+2]) / h;\r
 115             camera[i*2+1] = (cpara[1*4+0] * world[i*3+0]+ cpara[1*4+1] * world[i*3+1]+ cpara[1*4+2] * world[i*3+2]) / h;\r
 116         }\r
 117         //<Optimize>\r
 118         //v[0][0] = ed[0] - st[0];\r
 119         //v[0][1] = ed[1] - st[1];\r
 120         //v[1][0] = camera[1][0] - camera[0][0];\r
 121         //v[1][1] = camera[1][1] - camera[0][1];\r
 122         double v=(ed[0]-st[0])*(camera[2]-camera[0])+(ed[1]-st[1])*(camera[3]-camera[1]);\r
 123         //</Optimize>\r
 124         if(v<0) {//if( v[0][0]*v[1][0] + v[0][1]*v[1][1] < 0 ) {\r
 125             dir[0] = -dir[0];\r
 126             dir[1] = -dir[1];\r
 127             dir[2] = -dir[2];\r
 128         }\r
 129     }\r
 130     /*int check_rotation( double rot[2][3] )*/\r
 131     protected final static void check_rotation( double rot[][] ) throws NyARException\r
 132     {\r
 133         double[]  v1=new double[3], v2=new double[3], v3=new double[3];\r
 134         double  ca, cb, k1, k2, k3, k4;\r
 135         double  a, b, c, d;\r
 136         double  p1, q1, r1;\r
 137         double  p2, q2, r2;\r
 138         double  p3, q3, r3;\r
 139         double  p4, q4, r4;\r
 140         double  w;\r
 141         double  e1, e2, e3, e4;\r
 142         int     f;\r
 143 \r
 144         v1[0] = rot[0][0];\r
 145         v1[1] = rot[0][1];\r
 146         v1[2] = rot[0][2];\r
 147         v2[0] = rot[1][0];\r
 148         v2[1] = rot[1][1];\r
 149         v2[2] = rot[1][2];\r
 150         v3[0] = v1[1]*v2[2] - v1[2]*v2[1];\r
 151         v3[1] = v1[2]*v2[0] - v1[0]*v2[2];\r
 152         v3[2] = v1[0]*v2[1] - v1[1]*v2[0];\r
 153         w = Math.sqrt( v3[0]*v3[0]+v3[1]*v3[1]+v3[2]*v3[2] );\r
 154         if( w == 0.0 ){\r
 155             throw new NyARException();\r
 156         }\r
 157         v3[0] /= w;\r
 158         v3[1] /= w;\r
 159         v3[2] /= w;\r
 160 \r
 161         cb = v1[0]*v2[0] + v1[1]*v2[1] + v1[2]*v2[2];\r
 162         if( cb < 0 ) cb *= -1.0;\r
 163         ca = (Math.sqrt(cb+1.0) + Math.sqrt(1.0-cb)) * 0.5;\r
 164 \r
 165         if( v3[1]*v1[0] - v1[1]*v3[0] != 0.0 ) {\r
 166             f = 0;\r
 167         }\r
 168         else {\r
 169             if( v3[2]*v1[0] - v1[2]*v3[0] != 0.0 ) {\r
 170                 w = v1[1]; v1[1] = v1[2]; v1[2] = w;\r
 171                 w = v3[1]; v3[1] = v3[2]; v3[2] = w;\r
 172                 f = 1;\r
 173             }\r
 174             else {\r
 175                 w = v1[0]; v1[0] = v1[2]; v1[2] = w;\r
 176                 w = v3[0]; v3[0] = v3[2]; v3[2] = w;\r
 177                 f = 2;\r
 178             }\r
 179         }\r
 180         if( v3[1]*v1[0] - v1[1]*v3[0] == 0.0 ){\r
 181             throw new NyARException();\r
 182         }\r
 183         k1 = (v1[1]*v3[2] - v3[1]*v1[2]) / (v3[1]*v1[0] - v1[1]*v3[0]);\r
 184         k2 = (v3[1] * ca) / (v3[1]*v1[0] - v1[1]*v3[0]);\r
 185         k3 = (v1[0]*v3[2] - v3[0]*v1[2]) / (v3[0]*v1[1] - v1[0]*v3[1]);\r
 186         k4 = (v3[0] * ca) / (v3[0]*v1[1] - v1[0]*v3[1]);\r
 187 \r
 188         a = k1*k1 + k3*k3 + 1;\r
 189         b = k1*k2 + k3*k4;\r
 190         c = k2*k2 + k4*k4 - 1;\r
 191 \r
 192         d = b*b - a*c;\r
 193         if( d < 0 ){\r
 194             throw new NyARException();\r
 195         }\r
 196         r1 = (-b + Math.sqrt(d))/a;\r
 197         p1 = k1*r1 + k2;\r
 198         q1 = k3*r1 + k4;\r
 199         r2 = (-b - Math.sqrt(d))/a;\r
 200         p2 = k1*r2 + k2;\r
 201         q2 = k3*r2 + k4;\r
 202         if( f == 1 ) {\r
 203             w = q1; q1 = r1; r1 = w;\r
 204             w = q2; q2 = r2; r2 = w;\r
 205             w = v1[1]; v1[1] = v1[2]; v1[2] = w;\r
 206             w = v3[1]; v3[1] = v3[2]; v3[2] = w;\r
 207             f = 0;\r
 208         }\r
 209         if( f == 2 ) {\r
 210             w = p1; p1 = r1; r1 = w;\r
 211             w = p2; p2 = r2; r2 = w;\r
 212             w = v1[0]; v1[0] = v1[2]; v1[2] = w;\r
 213             w = v3[0]; v3[0] = v3[2]; v3[2] = w;\r
 214             f = 0;\r
 215         }\r
 216 \r
 217         if( v3[1]*v2[0] - v2[1]*v3[0] != 0.0 ) {\r
 218             f = 0;\r
 219         }else {\r
 220             if( v3[2]*v2[0] - v2[2]*v3[0] != 0.0 ) {\r
 221                 w = v2[1]; v2[1] = v2[2]; v2[2] = w;\r
 222                 w = v3[1]; v3[1] = v3[2]; v3[2] = w;\r
 223                 f = 1;\r
 224             }\r
 225             else {\r
 226                 w = v2[0]; v2[0] = v2[2]; v2[2] = w;\r
 227                 w = v3[0]; v3[0] = v3[2]; v3[2] = w;\r
 228                 f = 2;\r
 229             }\r
 230         }\r
 231         if( v3[1]*v2[0] - v2[1]*v3[0] == 0.0 ){\r
 232             throw new NyARException();\r
 233         }\r
 234         k1 = (v2[1]*v3[2] - v3[1]*v2[2]) / (v3[1]*v2[0] - v2[1]*v3[0]);\r
 235         k2 = (v3[1] * ca) / (v3[1]*v2[0] - v2[1]*v3[0]);\r
 236         k3 = (v2[0]*v3[2] - v3[0]*v2[2]) / (v3[0]*v2[1] - v2[0]*v3[1]);\r
 237         k4 = (v3[0] * ca) / (v3[0]*v2[1] - v2[0]*v3[1]);\r
 238 \r
 239         a = k1*k1 + k3*k3 + 1;\r
 240         b = k1*k2 + k3*k4;\r
 241         c = k2*k2 + k4*k4 - 1;\r
 242 \r
 243         d = b*b - a*c;\r
 244         if( d < 0 ){\r
 245             throw new NyARException();\r
 246         }\r
 247         r3 = (-b + Math.sqrt(d))/a;\r
 248         p3 = k1*r3 + k2;\r
 249         q3 = k3*r3 + k4;\r
 250         r4 = (-b - Math.sqrt(d))/a;\r
 251         p4 = k1*r4 + k2;\r
 252         q4 = k3*r4 + k4;\r
 253         if( f == 1 ) {\r
 254             w = q3; q3 = r3; r3 = w;\r
 255             w = q4; q4 = r4; r4 = w;\r
 256             w = v2[1]; v2[1] = v2[2]; v2[2] = w;\r
 257             w = v3[1]; v3[1] = v3[2]; v3[2] = w;\r
 258             f = 0;\r
 259         }\r
 260         if( f == 2 ) {\r
 261             w = p3; p3 = r3; r3 = w;\r
 262             w = p4; p4 = r4; r4 = w;\r
 263             w = v2[0]; v2[0] = v2[2]; v2[2] = w;\r
 264             w = v3[0]; v3[0] = v3[2]; v3[2] = w;\r
 265             f = 0;\r
 266         }\r
 267 \r
 268         e1 = p1*p3+q1*q3+r1*r3;\r
 269         if( e1 < 0 ){\r
 270             e1 = -e1;\r
 271         }\r
 272         e2 = p1*p4+q1*q4+r1*r4;\r
 273         if( e2 < 0 ){\r
 274             e2 = -e2;\r
 275         }\r
 276         e3 = p2*p3+q2*q3+r2*r3;\r
 277         if( e3 < 0 ){\r
 278             e3 = -e3;\r
 279         }\r
 280         e4 = p2*p4+q2*q4+r2*r4;\r
 281         if( e4 < 0 ){\r
 282             e4 = -e4;\r
 283         }\r
 284         if( e1 < e2 ) {\r
 285             if( e1 < e3 ) {\r
 286                 if( e1 < e4 ) {\r
 287                     rot[0][0] = p1;\r
 288                     rot[0][1] = q1;\r
 289                     rot[0][2] = r1;\r
 290                     rot[1][0] = p3;\r
 291                     rot[1][1] = q3;\r
 292                     rot[1][2] = r3;\r
 293                 }\r
 294                 else {\r
 295                     rot[0][0] = p2;\r
 296                     rot[0][1] = q2;\r
 297                     rot[0][2] = r2;\r
 298                     rot[1][0] = p4;\r
 299                     rot[1][1] = q4;\r
 300                     rot[1][2] = r4;\r
 301                 }\r
 302             }\r
 303             else {\r
 304                 if( e3 < e4 ) {\r
 305                     rot[0][0] = p2;\r
 306                     rot[0][1] = q2;\r
 307                     rot[0][2] = r2;\r
 308                     rot[1][0] = p3;\r
 309                     rot[1][1] = q3;\r
 310                     rot[1][2] = r3;\r
 311                 }\r
 312                 else {\r
 313                     rot[0][0] = p2;\r
 314                     rot[0][1] = q2;\r
 315                     rot[0][2] = r2;\r
 316                     rot[1][0] = p4;\r
 317                     rot[1][1] = q4;\r
 318                     rot[1][2] = r4;\r
 319                 }\r
 320             }\r
 321         }\r
 322         else {\r
 323             if( e2 < e3 ) {\r
 324                 if( e2 < e4 ) {\r
 325                     rot[0][0] = p1;\r
 326                     rot[0][1] = q1;\r
 327                     rot[0][2] = r1;\r
 328                     rot[1][0] = p4;\r
 329                     rot[1][1] = q4;\r
 330                     rot[1][2] = r4;\r
 331                 }\r
 332                 else {\r
 333                     rot[0][0] = p2;\r
 334                     rot[0][1] = q2;\r
 335                     rot[0][2] = r2;\r
 336                     rot[1][0] = p4;\r
 337                     rot[1][1] = q4;\r
 338                     rot[1][2] = r4;\r
 339                 }\r
 340             }\r
 341             else {\r
 342                 if( e3 < e4 ) {\r
 343                     rot[0][0] = p2;\r
 344                     rot[0][1] = q2;\r
 345                     rot[0][2] = r2;\r
 346                     rot[1][0] = p3;\r
 347                     rot[1][1] = q3;\r
 348                     rot[1][2] = r3;\r
 349                 }\r
 350                 else {\r
 351                     rot[0][0] = p2;\r
 352                     rot[0][1] = q2;\r
 353                     rot[0][2] = r2;\r
 354                     rot[1][0] = p4;\r
 355                     rot[1][1] = q4;\r
 356                     rot[1][2] = r4;\r
 357                 }\r
 358             }\r
 359         }\r
 360     }  \r
 361     /**\r
 362      * パラメタa,b,cからrotを計算してインスタンスに保存する。\r
 363      * rotを1次元配列に変更\r
 364      * Optimize:2008.04.20:STEP[253→186]\r
 365      * @param a\r
 366      * @param b\r
 367      * @param c\r
 368      * @param o_rot\r
 369      */\r
 370     protected final static void arGetRot( double a, double b, double c,double[] o_rot)\r
 371     {\r
 372         double   sina, sinb, sinc;\r
 373         double   cosa, cosb, cosc;\r
 374 \r
 375         sina = Math.sin(a);\r
 376         cosa = Math.cos(a);\r
 377         sinb = Math.sin(b);\r
 378         cosb = Math.cos(b);\r
 379         sinc = Math.sin(c);\r
 380         cosc = Math.cos(c);\r
 381         //Optimize\r
 382         double CACA,SASA,SACA,SASB,CASB,SACACB;\r
 383         CACA  =cosa*cosa;\r
 384         SASA  =sina*sina;\r
 385         SACA  =sina*cosa;\r
 386         SASB  =sina*sinb;\r
 387         CASB  =cosa*sinb;\r
 388         SACACB=SACA*cosb;\r
 389         \r
 390 \r
 391         o_rot[0] = CACA*cosb*cosc+SASA*cosc+SACACB*sinc-SACA*sinc;\r
 392         o_rot[1] = -CACA*cosb*sinc-SASA*sinc+SACACB*cosc-SACA*cosc;\r
 393         o_rot[2] = CASB;\r
 394         o_rot[3] = SACACB*cosc-SACA*cosc+SASA*cosb*sinc+CACA*sinc;\r
 395         o_rot[4] = -SACACB*sinc+SACA*sinc+SASA*cosb*cosc+CACA*cosc;\r
 396         o_rot[5] = SASB;\r
 397         o_rot[6] = -CASB*cosc-SASB*sinc;\r
 398         o_rot[7] = CASB*sinc-SASB*cosc;\r
 399         o_rot[8] = cosb;\r
 400     }\r
 401     /**\r
 402      * int arGetAngle( double rot[3][3], double *wa, double *wb, double *wc )\r
 403      * Optimize:2008.04.20:STEP[481→433]\r
 404      * @param rot\r
 405      * 2次元配列を1次元化してあります。\r
 406      * @param o_abc\r
 407      * @return\r
 408      */\r
 409     protected final int arGetAngle(double[] o_abc)\r
 410     {\r
 411         double      a, b, c,tmp;\r
 412         double      sina, cosa, sinb, cosb, sinc, cosc;\r
 413         double[] rot=array;\r
 414         if( rot[8] > 1.0 ) {//<Optimize/>if( rot[2][2] > 1.0 ) {\r
 415             rot[8] = 1.0;//<Optimize/>rot[2][2] = 1.0;\r
 416         }else if( rot[8] < -1.0 ) {//<Optimize/>}else if( rot[2][2] < -1.0 ) {\r
 417             rot[8] = -1.0;//<Optimize/>rot[2][2] = -1.0;\r
 418         }\r
 419         cosb = rot[8];//<Optimize/>cosb = rot[2][2];\r
 420         b = Math.acos( cosb );\r
 421         sinb = Math.sin( b );\r
 422         if( b >= 0.000001 || b <= -0.000001) {\r
 423             cosa = rot[2] / sinb;//<Optimize/>cosa = rot[0][2] / sinb;\r
 424             sina = rot[5] / sinb;//<Optimize/>sina = rot[1][2] / sinb;\r
 425             if( cosa > 1.0 ) {\r
 426                 /* printf("cos(alph) = %f\n", cosa); */\r
 427                 cosa = 1.0;\r
 428                 sina = 0.0;\r
 429             }\r
 430             if( cosa < -1.0 ) {\r
 431                 /* printf("cos(alph) = %f\n", cosa); */\r
 432                 cosa = -1.0;\r
 433                 sina =  0.0;\r
 434             }\r
 435             if( sina > 1.0 ) {\r
 436                 /* printf("sin(alph) = %f\n", sina); */\r
 437                 sina = 1.0;\r
 438                 cosa = 0.0;\r
 439             }\r
 440             if( sina < -1.0 ) {\r
 441                 /* printf("sin(alph) = %f\n", sina); */\r
 442                 sina = -1.0;\r
 443                 cosa =  0.0;\r
 444             }\r
 445             a = Math.acos( cosa );\r
 446             if( sina < 0 ){\r
 447                 a = -a;\r
 448             }\r
 449             //<Optimize>\r
 450             //sinc =  (rot[2][1]*rot[0][2]-rot[2][0]*rot[1][2])/ (rot[0][2]*rot[0][2]+rot[1][2]*rot[1][2]);\r
 451             //cosc =  -(rot[0][2]*rot[2][0]+rot[1][2]*rot[2][1])/ (rot[0][2]*rot[0][2]+rot[1][2]*rot[1][2]);\r
 452             tmp = (rot[2]*rot[2]+rot[5]*rot[5]);\r
 453             sinc =  (rot[7]*rot[2]-rot[6]*rot[5])/ tmp;\r
 454             cosc =  -(rot[2]*rot[6]+rot[5]*rot[7])/ tmp;\r
 455             //</Optimize>\r
 456 \r
 457             if( cosc > 1.0 ) {\r
 458                 /* printf("cos(r) = %f\n", cosc); */\r
 459                 cosc = 1.0;\r
 460                 sinc = 0.0;\r
 461             }\r
 462             if( cosc < -1.0 ) {\r
 463                 /* printf("cos(r) = %f\n", cosc); */\r
 464                 cosc = -1.0;\r
 465                 sinc =  0.0;\r
 466             }\r
 467             if( sinc > 1.0 ) {\r
 468                 /* printf("sin(r) = %f\n", sinc); */\r
 469                 sinc = 1.0;\r
 470                 cosc = 0.0;\r
 471             }\r
 472             if( sinc < -1.0 ) {\r
 473                 /* printf("sin(r) = %f\n", sinc); */\r
 474                 sinc = -1.0;\r
 475                 cosc =  0.0;\r
 476             }\r
 477             c = Math.acos( cosc );\r
 478             if( sinc < 0 ){\r
 479                 c = -c;\r
 480             }\r
 481         }else {\r
 482             a = b = 0.0;\r
 483             cosa = cosb = 1.0;\r
 484             sina = sinb = 0.0;\r
 485             cosc = rot[0];//<Optimize/>cosc = rot[0][0];\r
 486             sinc = rot[1];//<Optimize/>sinc = rot[1][0];\r
 487             if( cosc > 1.0 ) {\r
 488                 /* printf("cos(r) = %f\n", cosc); */\r
 489                 cosc = 1.0;\r
 490                 sinc = 0.0;\r
 491             }\r
 492             if( cosc < -1.0 ) {\r
 493                 /* printf("cos(r) = %f\n", cosc); */\r
 494                 cosc = -1.0;\r
 495                 sinc =  0.0;\r
 496             }\r
 497             if( sinc > 1.0 ) {\r
 498                 /* printf("sin(r) = %f\n", sinc); */\r
 499                 sinc = 1.0;\r
 500                 cosc = 0.0;\r
 501             }\r
 502             if( sinc < -1.0 ) {\r
 503                 /* printf("sin(r) = %f\n", sinc); */\r
 504                 sinc = -1.0;\r
 505                 cosc =  0.0;\r
 506             }\r
 507             c = Math.acos( cosc );\r
 508             if( sinc < 0 ){\r
 509                 c = -c;\r
 510             }\r
 511         }\r
 512         o_abc[0]=a;//wa.value=a;//*wa = a;\r
 513         o_abc[1]=b;//wb.value=b;//*wb = b;\r
 514         o_abc[2]=c;//wc.value=c;//*wc = c;\r
 515         return 0;\r
 516     }    \r
 517 }\r
 518 \r
 519 /**\r
 520  * NyARModifyMatrixの最適化バージョン1\r
 521  * 配列の１次元化、計算ステップの圧縮等の最適化をしてみた。\r
 522  *\r
 523  */\r
 524 class NyARTransRot_O1 extends NyARTransRot_OptimizeCommon\r
 525 {\r
 526     public NyARTransRot_O1(NyARParam i_param,int i_number_of_vertex) throws NyARException\r
 527     {\r
 528         super(i_param,i_number_of_vertex);\r
 529     }\r
 530     /**\r
 531      * int arGetInitRot( ARMarkerInfo *marker_info, double cpara[3][4], double rot[3][3] )\r
 532      * Optimize:2008.04.20:STEP[716→698]\r
 533      * @param marker_info\r
 534      * @param i_direction\r
 535      * @param i_param\r
 536      * @throws NyARException\r
 537      */\r
 538     public final void initRot(NyARSquare marker_info,int i_direction) throws NyARException\r
 539     {\r
 540         double cpara[]= cparam.get34Array();\r
 541         double[][]  wdir=new double[3][3];\r
 542         double  w, w1, w2, w3;\r
 543         int     dir;\r
 544         int     j;\r
 545 \r
 546         dir = i_direction;\r
 547 \r
 548         for( j = 0; j < 2; j++ ) {\r
 549             w1 = marker_info.line[(4-dir+j)%4][0] * marker_info.line[(6-dir+j)%4][1]- marker_info.line[(6-dir+j)%4][0] * marker_info.line[(4-dir+j)%4][1];\r
 550             w2 = marker_info.line[(4-dir+j)%4][1] * marker_info.line[(6-dir+j)%4][2]- marker_info.line[(6-dir+j)%4][1] * marker_info.line[(4-dir+j)%4][2];\r
 551             w3 = marker_info.line[(4-dir+j)%4][2] * marker_info.line[(6-dir+j)%4][0]- marker_info.line[(6-dir+j)%4][2] * marker_info.line[(4-dir+j)%4][0];\r
 552 \r
 553             wdir[j][0] =  w1*(cpara[0*4+1]*cpara[1*4+2]-cpara[0*4+2]*cpara[1*4+1])+  w2*cpara[1*4+1]-  w3*cpara[0*4+1];\r
 554             wdir[j][1] = -w1*cpara[0*4+0]*cpara[1*4+2]+  w3*cpara[0*4+0];\r
 555             wdir[j][2] =  w1*cpara[0*4+0]*cpara[1*4+1];\r
 556             w = Math.sqrt( wdir[j][0]*wdir[j][0]+ wdir[j][1]*wdir[j][1]+ wdir[j][2]*wdir[j][2] );\r
 557             wdir[j][0] /= w;\r
 558             wdir[j][1] /= w;\r
 559             wdir[j][2] /= w;\r
 560         }\r
 561 \r
 562         //以下3ケースは、計算エラーのときは例外が発生する。\r
 563         check_dir(wdir[0], marker_info.sqvertex[(4-dir)%4],marker_info.sqvertex[(5-dir)%4], cpara);\r
 564 \r
 565         check_dir(wdir[1], marker_info.sqvertex[(7-dir)%4],marker_info.sqvertex[(4-dir)%4], cpara);\r
 566 \r
 567         check_rotation(wdir);\r
 568 \r
 569 \r
 570         wdir[2][0] = wdir[0][1]*wdir[1][2] - wdir[0][2]*wdir[1][1];\r
 571         wdir[2][1] = wdir[0][2]*wdir[1][0] - wdir[0][0]*wdir[1][2];\r
 572         wdir[2][2] = wdir[0][0]*wdir[1][1] - wdir[0][1]*wdir[1][0];\r
 573         w = Math.sqrt( wdir[2][0]*wdir[2][0]+ wdir[2][1]*wdir[2][1]+ wdir[2][2]*wdir[2][2] );\r
 574         wdir[2][0] /= w;\r
 575         wdir[2][1] /= w;\r
 576         wdir[2][2] /= w;\r
 577         /*\r
 578         if( wdir[2][2] < 0 ) {\r
 579             wdir[2][0] /= -w;\r
 580             wdir[2][1] /= -w;\r
 581             wdir[2][2] /= -w;\r
 582         }\r
 583         else {\r
 584             wdir[2][0] /= w;\r
 585             wdir[2][1] /= w;\r
 586             wdir[2][2] /= w;\r
 587         }\r
 588          */\r
 589         //<Optimize>\r
 590         //rot[0][0] = wdir[0][0];\r
 591         //rot[1][0] = wdir[0][1];\r
 592         //rot[2][0] = wdir[0][2];\r
 593         //rot[0][1] = wdir[1][0];\r
 594         //rot[1][1] = wdir[1][1];\r
 595         //rot[2][1] = wdir[1][2];\r
 596         //rot[0][2] = wdir[2][0];\r
 597         //rot[1][2] = wdir[2][1];\r
 598         //rot[2][2] = wdir[2][2];\r
 599         double[] rot=this.array;\r
 600         rot[0] = wdir[0][0];\r
 601         rot[3] = wdir[0][1];\r
 602         rot[6] = wdir[0][2];\r
 603         rot[1] = wdir[1][0];\r
 604         rot[4] = wdir[1][1];\r
 605         rot[7] = wdir[1][2];\r
 606         rot[2] = wdir[2][0];\r
 607         rot[5] = wdir[2][1];\r
 608         rot[8] = wdir[2][2];\r
 609         //</Optimize>    \r
 610     }\r
 611     private final double[] wk_arModifyMatrix_combo=new double[12];//[3][4];\r
 612     private final double[] wk_arModifyMatrix_abc=new double[3];\r
 613     private final double[] wk_arModifyMatrix_rot=new double[9];\r
 614     /**\r
 615      * Optimize:2008.04.20:STEP[456→-]\r
 616      * @param rot\r
 617      * [3x3]配列\r
 618      * @param trans\r
 619      * @param vertex\r
 620      * @param pos2d\r
 621      * @param num\r
 622      * @return\r
 623      */\r
 624     public final double modifyMatrix(double trans[],double vertex[][], double pos2d[][]) throws NyARException\r
 625     {\r
 626         int num=this.number_of_vertex;\r
 627         double    factor;\r
 628         double    a1, b1, c1;\r
 629         double    a2, b2, c2;\r
 630         double    ma = 0.0, mb = 0.0, mc = 0.0;\r
 631         double    hx, hy, h, x, y;\r
 632         double    err, minerr=0;\r
 633         int       t1, t2, t3;\r
 634         int       s1 = 0, s2 = 0, s3 = 0;\r
 635         int       i, j;\r
 636         double[] combo=this.wk_arModifyMatrix_combo;//arGetNewMatrixで初期化されるので初期化不要//new double[3][4];\r
 637         double[] abc=wk_arModifyMatrix_abc;\r
 638         double[] rot=wk_arModifyMatrix_rot;\r
 639 \r
 640         arGetAngle(abc);//arGetAngle( rot, &a, &b, &c );\r
 641         a2 = abc[0];\r
 642         b2 = abc[1];\r
 643         c2 = abc[2];\r
 644         factor = 10.0*Math.PI/180.0;\r
 645         for( j = 0; j < 10; j++ ) {\r
 646             minerr = 1000000000.0;\r
 647             for(t1=-1;t1<=1;t1++) {\r
 648                 for(t2=-1;t2<=1;t2++) {\r
 649                     for(t3=-1;t3<=1;t3++) {\r
 650                         a1 = a2 + factor*t1;\r
 651                         b1 = b2 + factor*t2;\r
 652                         c1 = c2 + factor*t3;\r
 653                         arGetRot( a1, b1, c1,rot);\r
 654                         arGetNewMatrix(rot,trans, null, combo );\r
 655                         err = 0.0;\r
 656                         for( i = 0; i < num; i++ ) {\r
 657                             hx = combo[0] * vertex[i][0]+ combo[1] * vertex[i][1]+ combo[2] * vertex[i][2]+ combo[3];\r
 658                             hy = combo[4] * vertex[i][0]+ combo[5] * vertex[i][1]+ combo[6] * vertex[i][2]+ combo[7];\r
 659                             h  = combo[8] * vertex[i][0]+ combo[9] * vertex[i][1]+ combo[10] * vertex[i][2]+ combo[11];\r
 660                             x = hx / h;\r
 661                             y = hy / h;\r
 662                             err += (pos2d[i][0] - x) * (pos2d[i][0] - x)+ (pos2d[i][1] - y) * (pos2d[i][1] - y);\r
 663                         }\r
 664                         if( err < minerr ) {\r
 665                             minerr = err;\r
 666                             ma = a1;\r
 667                             mb = b1;\r
 668                             mc = c1;\r
 669                             s1 = t1;\r
 670                             s2 = t2;\r
 671                             s3 = t3;\r
 672                         }\r
 673                     }\r
 674                 }\r
 675             }\r
 676             if( s1 == 0 && s2 == 0 && s3 == 0 ){\r
 677                 factor *= 0.5;\r
 678             }\r
 679             a2 = ma;\r
 680             b2 = mb;\r
 681             c2 = mc;\r
 682         }\r
 683         arGetRot(ma, mb, mc,this.array);\r
 684         /*  printf("factor = %10.5f\n", factor*180.0/MD_PI); */\r
 685         return minerr/num;\r
 686     }\r
 687     private final double[] wk_cpara2_arGetNewMatrix=new double[12];//[3][4];\r
 688     /**\r
 689      * Optimize:2008.04.20:STEP[569->432]\r
 690      * @param i_rot\r
 691      * [9]\r
 692      * @param trans\r
 693      * @param trans2\r
 694      * @param ret\r
 695      * double[3x4]配列\r
 696      * @return\r
 697      */\r
 698     private final int arGetNewMatrix(double[] i_rot,double trans[], double trans2[][], double ret[]) throws NyARException\r
 699     {\r
 700         final double cpara[]=cparam.get34Array();\r
 701         final double[] cpara2;  //この関数で初期化される。\r
 702         int j,j_idx;\r
 703 //      double[] cpara_pt;\r
 704         //cparaの2次元配列→1次元に変換して計算\r
 705         if( trans2 != null ) {\r
 706             cpara2=wk_cpara2_arGetNewMatrix;    //この関数で初期化される。\r
 707 \r
 708             for( j = 0; j < 3; j++ ) {\r
 709 //              Optimize(使わないから最適化してない)\r
 710                 NyARException.trap("未チェックのパス");\r
 711                 cpara2[j*4+0] = cpara[j*4+0] * trans2[0][0]+ cpara[j*4+1] * trans2[1][0]+ cpara[j*4+2] * trans2[2][0];\r
 712                 cpara2[j*4+1] = cpara[j*4+0] * trans2[0][1]+ cpara[j*4+1] * trans2[1][1]+ cpara[j*4+2] * trans2[2][1];\r
 713                 cpara2[j*4+2] = cpara[j*4+0] * trans2[0][2]+ cpara[j*4+1] * trans2[1][2]+ cpara[j*4+2] * trans2[2][2];\r
 714                 cpara2[j*4+3] = cpara[j*4+0] * trans2[0][3]+ cpara[j*4+1] * trans2[1][3]+ cpara[j*4+2] * trans2[2][3];\r
 715             }\r
 716         }else{\r
 717             cpara2=cpara;//cparaの値をそのまま使う\r
 718         }\r
 719         for( j = 0; j < 3; j++ ) {\r
 720             //cpara2_pt=cpara2[j];\r
 721             j_idx=j*4;\r
 722             //<Optimize>\r
 723             //ret[j][0] = cpara2_pt[0] * rot[0][0]+ cpara2_pt[1] * rot[1][0]+ cpara2_pt[2] * rot[2][0];\r
 724             //ret[j][1] = cpara2_pt[0] * rot[0][1]+ cpara2_pt[1] * rot[1][1]+ cpara2_pt[2] * rot[2][1];\r
 725             //ret[j][2] = cpara2_pt[0] * rot[0][2]+ cpara2_pt[1] * rot[1][2]+ cpara2_pt[2] * rot[2][2];\r
 726             //ret[j][3] = cpara2_pt[0] * trans[0]+ cpara2_pt[1] * trans[1]+ cpara2_pt[2] * trans[2]+ cpara2_pt[3];\r
 727             ret[j_idx+0] = cpara2[j_idx+0] * i_rot[0]+ cpara2[j_idx+1] * i_rot[3]+ cpara2[j_idx+2] * i_rot[6];\r
 728             ret[j_idx+1] = cpara2[j_idx+0] * i_rot[1]+ cpara2[j_idx+1] * i_rot[4]+ cpara2[j_idx+2] * i_rot[7];\r
 729             ret[j_idx+2] = cpara2[j_idx+0] * i_rot[2]+ cpara2[j_idx+1] * i_rot[5]+ cpara2[j_idx+2] * i_rot[8];\r
 730             ret[j_idx+3] = cpara2[j_idx+0] * trans[0]+ cpara2[j_idx+1] * trans[1]+ cpara2[j_idx+2] * trans[2]+ cpara2[j_idx+3];\r
 731             //</Optimize>\r
 732         }\r
 733         return(0);\r
 734     }    \r
 735 }\r
 736 \r
 737 /**\r
 738  * NyARModifyMatrixの最適化バージョン2\r
 739  * 計算手順の変更、構造変更など含む最適化をしたもの\r
 740  *\r
 741  */\r
 742 class NyARTransRot_O2 extends NyARTransRot_OptimizeCommon\r
 743 {\r
 744     public NyARTransRot_O2(NyARParam i_param,int i_number_of_vertex) throws NyARException\r
 745     {\r
 746         super(i_param,i_number_of_vertex);\r
 747     }  \r
 748     \r
 749     //private double CACA,SASA,SACA,CA,SA;    \r
 750     private double CACA,SASA,SACA,CA,SA;\r
 751     final public void arGetRotA( double a)\r
 752     {\r
 753         double   sina,cosa;\r
 754         sina = Math.sin(a);\r
 755         cosa = Math.cos(a);\r
 756         //Optimize\r
 757         CACA=cosa*cosa;\r
 758         SASA=sina*sina;\r
 759         SACA=sina*cosa;\r
 760         CA=cosa;\r
 761         SA=sina;\r
 762     }\r
 763     private double CACACB,SACACB,SASACB,CASB,SASB;\r
 764     final public void arGetRotB(double b,double[] o_rot)\r
 765     {\r
 766         double   sinb,cosb;\r
 767         sinb = Math.sin(b);\r
 768         cosb = Math.cos(b);\r
 769         CACACB=CACA*cosb;\r
 770         SACACB=SACA*cosb;\r
 771         SASACB=SASA*cosb;\r
 772         CASB=CA*sinb;\r
 773         SASB=SA*sinb;\r
 774         o_rot[2] = CASB;\r
 775         o_rot[5] = SASB;\r
 776         o_rot[8] = cosb;\r
 777     }\r
 778     /**\r
 779      * 分割arGetRot\r
 780      * @param c\r
 781      */\r
 782     public final void arGetRotC(double c,double[] o_rot)\r
 783     {\r
 784         double   sinc,cosc;\r
 785         sinc = Math.sin(c);\r
 786         cosc = Math.cos(c);\r
 787         double SACASC,SACACBSC,SACACBCC,SACACC;\r
 788         SACASC=SACA*sinc;\r
 789         SACACC=SACA*cosc;\r
 790         SACACBSC=SACACB*sinc;\r
 791         SACACBCC=SACACB*cosc;\r
 792         o_rot[0] = CACACB*cosc+SASA*cosc+SACACBSC-SACASC;\r
 793         o_rot[1] = -CACACB*sinc-SASA*sinc+SACACBCC-SACACC;\r
 794         o_rot[3] = SACACBCC-SACACC+SASACB*sinc+CACA*sinc;\r
 795         o_rot[4] = -SACACBSC+SACASC+SASACB*cosc+CACA*cosc;\r
 796         o_rot[6] = -CASB*cosc-SASB*sinc;\r
 797         o_rot[7] = CASB*sinc-SASB*cosc;\r
 798     }\r
 799     private final double[][] wk_initRot_wdir=new double[3][3];\r
 800     /**\r
 801      * int arGetInitRot( ARMarkerInfo *marker_info, double cpara[3][4], double rot[3][3] )\r
 802      * Optimize:2008.04.20:STEP[716→698]\r
 803      * @param marker_info\r
 804      * @param i_direction\r
 805      * @param i_param\r
 806      * @throws NyARException\r
 807      */\r
 808     public void initRot(NyARSquare marker_info,int i_direction) throws NyARException\r
 809     {\r
 810         double cpara[]= cparam.get34Array();\r
 811         double[][]  wdir=wk_initRot_wdir;//この関数で初期化される\r
 812         double  w, w1, w2, w3;\r
 813         int     dir;\r
 814         int     j;\r
 815 \r
 816         dir = i_direction;\r
 817 \r
 818         for( j = 0; j < 2; j++ ) {\r
 819             w1 = marker_info.line[(4-dir+j)%4][0] * marker_info.line[(6-dir+j)%4][1]- marker_info.line[(6-dir+j)%4][0] * marker_info.line[(4-dir+j)%4][1];\r
 820             w2 = marker_info.line[(4-dir+j)%4][1] * marker_info.line[(6-dir+j)%4][2]- marker_info.line[(6-dir+j)%4][1] * marker_info.line[(4-dir+j)%4][2];\r
 821             w3 = marker_info.line[(4-dir+j)%4][2] * marker_info.line[(6-dir+j)%4][0]- marker_info.line[(6-dir+j)%4][2] * marker_info.line[(4-dir+j)%4][0];\r
 822 \r
 823             wdir[j][0] =  w1*(cpara[0*4+1]*cpara[1*4+2]-cpara[0*4+2]*cpara[1*4+1])+  w2*cpara[1*4+1]-  w3*cpara[0*4+1];\r
 824             wdir[j][1] = -w1*cpara[0*4+0]*cpara[1*4+2]+  w3*cpara[0*4+0];\r
 825             wdir[j][2] =  w1*cpara[0*4+0]*cpara[1*4+1];\r
 826             w = Math.sqrt( wdir[j][0]*wdir[j][0]+ wdir[j][1]*wdir[j][1]+ wdir[j][2]*wdir[j][2] );\r
 827             wdir[j][0] /= w;\r
 828             wdir[j][1] /= w;\r
 829             wdir[j][2] /= w;\r
 830         }\r
 831 \r
 832         //以下3ケースは、計算エラーのときは例外が発生する。\r
 833         check_dir(wdir[0], marker_info.sqvertex[(4-dir)%4],marker_info.sqvertex[(5-dir)%4], cpara);\r
 834 \r
 835         check_dir(wdir[1], marker_info.sqvertex[(7-dir)%4],marker_info.sqvertex[(4-dir)%4], cpara);\r
 836 \r
 837         check_rotation(wdir);\r
 838 \r
 839 \r
 840         wdir[2][0] = wdir[0][1]*wdir[1][2] - wdir[0][2]*wdir[1][1];\r
 841         wdir[2][1] = wdir[0][2]*wdir[1][0] - wdir[0][0]*wdir[1][2];\r
 842         wdir[2][2] = wdir[0][0]*wdir[1][1] - wdir[0][1]*wdir[1][0];\r
 843         w = Math.sqrt( wdir[2][0]*wdir[2][0]+ wdir[2][1]*wdir[2][1]+ wdir[2][2]*wdir[2][2] );\r
 844         wdir[2][0] /= w;\r
 845         wdir[2][1] /= w;\r
 846         wdir[2][2] /= w;\r
 847         //<Optimize>\r
 848         //rot[0][0] = wdir[0][0];\r
 849         //rot[1][0] = wdir[0][1];\r
 850         //rot[2][0] = wdir[0][2];\r
 851         //rot[0][1] = wdir[1][0];\r
 852         //rot[1][1] = wdir[1][1];\r
 853         //rot[2][1] = wdir[1][2];\r
 854         //rot[0][2] = wdir[2][0];\r
 855         //rot[1][2] = wdir[2][1];\r
 856         //rot[2][2] = wdir[2][2];\r
 857         double[] rot=this.array;\r
 858         rot[0] = wdir[0][0];\r
 859         rot[3] = wdir[0][1];\r
 860         rot[6] = wdir[0][2];\r
 861         rot[1] = wdir[1][0];\r
 862         rot[4] = wdir[1][1];\r
 863         rot[7] = wdir[1][2];\r
 864         rot[2] = wdir[2][0];\r
 865         rot[5] = wdir[2][1];\r
 866         rot[8] = wdir[2][2];\r
 867         //</Optimize>    \r
 868     }\r
 869     private final double[] wk_arModifyMatrix_combo=new double[12];//[3][4];\r
 870     private final double[] wk_arModifyMatrix_abc=new double[3];\r
 871     private final double[] wk_arModifyMatrix_rot=new double[9];    \r
 872     /**\r
 873      * arGetRot計算を階層化したModifyMatrix\r
 874      * @param nyrot\r
 875      * @param trans\r
 876      * @param vertex\r
 877      * @param pos2d\r
 878      * @param num\r
 879      * @return\r
 880      * @throws NyARException\r
 881      */\r
 882     public double modifyMatrix(double trans[],double vertex[][], double pos2d[][]) throws NyARException\r
 883     {\r
 884         int num=this.number_of_vertex;\r
 885         double    factor;\r
 886         double    a1, b1, c1;\r
 887         double    a2, b2, c2;\r
 888         double    ma = 0.0, mb = 0.0, mc = 0.0;\r
 889         double    hx, hy, h, x, y;\r
 890         double    err, minerr=0;\r
 891         int       t1, t2, t3;\r
 892         int       s1 = 0, s2 = 0, s3 = 0;\r
 893         int       i, j;\r
 894         final double[] combo=this.wk_arModifyMatrix_combo;//arGetNewMatrixで初期化されるので初期化不要//new double[3][4];\r
 895         final double[] abc=wk_arModifyMatrix_abc;\r
 896         double[] rot=wk_arModifyMatrix_rot;\r
 897 \r
 898         arGetAngle(abc);//arGetAngle( rot, &a, &b, &c );\r
 899         a2 = abc[0];\r
 900         b2 = abc[1];\r
 901         c2 = abc[2];\r
 902         factor = 10.0*Math.PI/180.0;\r
 903 \r
 904         nyatla_arGetNewMatrix_row3(trans,combo);//comboの3行目を先に計算\r
 905         for( j = 0; j < 10; j++ ) {\r
 906             minerr = 1000000000.0;\r
 907             for(t1=-1;t1<=1;t1++) {\r
 908                 a1 = a2 + factor*t1;\r
 909                 arGetRotA(a1);\r
 910                 for(t2=-1;t2<=1;t2++) {\r
 911                     b1 = b2 + factor*t2;\r
 912                     arGetRotB(b1,rot);\r
 913                     for(t3=-1;t3<=1;t3++) {\r
 914                         c1 = c2 + factor*t3;\r
 915                         arGetRotC(c1,rot);\r
 916                         //comboの0-2行目を計算\r
 917                         nyatla_arGetNewMatrix_row012(rot,trans,combo);//第二パラメタは常にnull//arGetNewMatrix(trans, null, combo );\r
 918                         err = 0.0;\r
 919                         for( i = 0; i < num; i++ ) {\r
 920                             hx = combo[0] * vertex[i][0]+ combo[1] * vertex[i][1]+ combo[2] * vertex[i][2]+ combo[3];\r
 921                             hy = combo[4] * vertex[i][0]+ combo[5] * vertex[i][1]+ combo[6] * vertex[i][2]+ combo[7];\r
 922                             h  = combo[8] * vertex[i][0]+ combo[9] * vertex[i][1]+ combo[10] * vertex[i][2]+ combo[11];\r
 923                             x = hx / h;\r
 924                             y = hy / h;\r
 925                             err += (pos2d[i][0] - x) * (pos2d[i][0] - x)+ (pos2d[i][1] - y) * (pos2d[i][1] - y);\r
 926                         }\r
 927                         if( err < minerr ) {\r
 928                             minerr = err;\r
 929                             ma = a1;\r
 930                             mb = b1;\r
 931                             mc = c1;\r
 932                             s1 = t1;\r
 933                             s2 = t2;\r
 934                             s3 = t3;\r
 935                         }\r
 936                     }\r
 937                 }\r
 938             }\r
 939             if( s1 == 0 && s2 == 0 && s3 == 0 ){\r
 940                 factor *= 0.5;\r
 941             }\r
 942             a2 = ma;\r
 943             b2 = mb;\r
 944             c2 = mc;\r
 945         }\r
 946         arGetRot(ma,mb,mc,this.array);\r
 947         /*  printf("factor = %10.5f\n", factor*180.0/MD_PI); */\r
 948         return minerr/num;\r
 949     }\r
 950     /**\r
 951      * arGetNewMatrixの0-2行目を初期化する関数\r
 952      * Optimize:2008.04.20:STEP[569->144]\r
 953      * @param i_rot\r
 954      * @param trans\r
 955      * @param trans2\r
 956      * @param ret\r
 957      * double[3x4]配列\r
 958      * @return\r
 959      */\r
 960     private final void nyatla_arGetNewMatrix_row012(double i_rot[],double trans[],double ret[]) throws NyARException\r
 961     {\r
 962         int j;\r
 963         double c0,c1,c2;\r
 964         final double cpara2[]=cparam.get34Array();\r
 965         for( j = 0; j < 3; j++ ) {\r
 966             //cpara2_pt=cpara2[j];\r
 967             c0=cpara2[j*4+0];\r
 968             c1=cpara2[j*4+1];\r
 969             c2=cpara2[j*4+2];\r
 970             ret[j*4+0] = c0 * i_rot[0]+ c1 * i_rot[3]+ c2 * i_rot[6];\r
 971             ret[j*4+1] = c0 * i_rot[1]+ c1 * i_rot[4]+ c2 * i_rot[7];\r
 972             ret[j*4+2] = c0 * i_rot[2]+ c1 * i_rot[5]+ c2 * i_rot[8];\r
 973             //</Optimize>\r
 974         }\r
 975         return;\r
 976     }\r
 977     /**\r
 978      * arGetNewMatrixの3行目を初期化する関数\r
 979      * @param trans\r
 980      * @param ret\r
 981      * @throws NyARException\r
 982      */\r
 983     private final void nyatla_arGetNewMatrix_row3(double trans[],double ret[]) throws NyARException\r
 984     {\r
 985         final double cpara2[]=cparam.get34Array();\r
 986         int j,j_idx;\r
 987         for( j = 0; j < 3; j++ ) {\r
 988             j_idx=j*4;\r
 989             ret[j_idx+3] = cpara2[j_idx+0] * trans[0]+ cpara2[j_idx+1] * trans[1]+ cpara2[j_idx+2] * trans[2]+ cpara2[j_idx+3];\r
 990         }\r
 991         return;\r
 992     }          \r
 993 }\r
 994 \r
 995 \r
 996 /**\r
 997  * NyARModifyMatrixの最適化バージョン3\r
 998  * O3版の演算テーブル版\r
 999  * 計算速度のみを追求する\r
1000  *\r
1001  */\r
1002 class NyARTransRot_O3 extends NyARTransRot_OptimizeCommon\r
1003 {\r
1004     public NyARTransRot_O3(NyARParam i_param,int i_number_of_vertex) throws NyARException\r
1005     {\r
1006         super(i_param,i_number_of_vertex);\r
1007         if(i_number_of_vertex!=4){\r
1008             //4以外の頂点数は処理しない\r
1009             throw new NyARException();\r
1010         }\r
1011     }  \r
1012     \r
1013     //private double CACA,SASA,SACA,CA,SA;    \r
1014     private final double[][] wk_initRot_wdir=new double[3][3];\r
1015     /**\r
1016      * int arGetInitRot( ARMarkerInfo *marker_info, double cpara[3][4], double rot[3][3] )\r
1017      * Optimize:2008.04.20:STEP[716→698]\r
1018      * @param marker_info\r
1019      * @param i_direction\r
1020      * @param i_param\r
1021      * @throws NyARException\r
1022      */\r
1023     public void initRot(NyARSquare marker_info,int i_direction) throws NyARException\r
1024     {\r
1025         double cpara[]= cparam.get34Array();\r
1026         double[][]  wdir=wk_initRot_wdir;//この関数で初期化される\r
1027         double  w, w1, w2, w3;\r
1028         int     dir;\r
1029         int     j;\r
1030 \r
1031         dir = i_direction;\r
1032 \r
1033         for( j = 0; j < 2; j++ ) {\r
1034             w1 = marker_info.line[(4-dir+j)%4][0] * marker_info.line[(6-dir+j)%4][1]- marker_info.line[(6-dir+j)%4][0] * marker_info.line[(4-dir+j)%4][1];\r
1035             w2 = marker_info.line[(4-dir+j)%4][1] * marker_info.line[(6-dir+j)%4][2]- marker_info.line[(6-dir+j)%4][1] * marker_info.line[(4-dir+j)%4][2];\r
1036             w3 = marker_info.line[(4-dir+j)%4][2] * marker_info.line[(6-dir+j)%4][0]- marker_info.line[(6-dir+j)%4][2] * marker_info.line[(4-dir+j)%4][0];\r
1037 \r
1038             wdir[j][0] =  w1*(cpara[0*4+1]*cpara[1*4+2]-cpara[0*4+2]*cpara[1*4+1])+  w2*cpara[1*4+1]-  w3*cpara[0*4+1];\r
1039             wdir[j][1] = -w1*cpara[0*4+0]*cpara[1*4+2]+  w3*cpara[0*4+0];\r
1040             wdir[j][2] =  w1*cpara[0*4+0]*cpara[1*4+1];\r
1041             w = Math.sqrt( wdir[j][0]*wdir[j][0]+ wdir[j][1]*wdir[j][1]+ wdir[j][2]*wdir[j][2] );\r
1042             wdir[j][0] /= w;\r
1043             wdir[j][1] /= w;\r
1044             wdir[j][2] /= w;\r
1045         }\r
1046 \r
1047         //以下3ケースは、計算エラーのときは例外が発生する。\r
1048         check_dir(wdir[0], marker_info.sqvertex[(4-dir)%4],marker_info.sqvertex[(5-dir)%4], cpara);\r
1049 \r
1050         check_dir(wdir[1], marker_info.sqvertex[(7-dir)%4],marker_info.sqvertex[(4-dir)%4], cpara);\r
1051 \r
1052         check_rotation(wdir);\r
1053 \r
1054 \r
1055         wdir[2][0] = wdir[0][1]*wdir[1][2] - wdir[0][2]*wdir[1][1];\r
1056         wdir[2][1] = wdir[0][2]*wdir[1][0] - wdir[0][0]*wdir[1][2];\r
1057         wdir[2][2] = wdir[0][0]*wdir[1][1] - wdir[0][1]*wdir[1][0];\r
1058         w = Math.sqrt( wdir[2][0]*wdir[2][0]+ wdir[2][1]*wdir[2][1]+ wdir[2][2]*wdir[2][2] );\r
1059         wdir[2][0] /= w;\r
1060         wdir[2][1] /= w;\r
1061         wdir[2][2] /= w;\r
1062         double[] rot=this.array;\r
1063         rot[0] = wdir[0][0];\r
1064         rot[3] = wdir[0][1];\r
1065         rot[6] = wdir[0][2];\r
1066         rot[1] = wdir[1][0];\r
1067         rot[4] = wdir[1][1];\r
1068         rot[7] = wdir[1][2];\r
1069         rot[2] = wdir[2][0];\r
1070         rot[5] = wdir[2][1];\r
1071         rot[8] = wdir[2][2];\r
1072         //</Optimize>    \r
1073     }\r
1074     private final double[][] wk_arModifyMatrix_double1D=new double[8][3];\r
1075     /**\r
1076      * arGetRot計算を階層化したModifyMatrix\r
1077      * 896\r
1078      * @param nyrot\r
1079      * @param trans\r
1080      * @param vertex\r
1081      * [m][3]\r
1082      * @param pos2d\r
1083      * [n][2]\r
1084      * @return\r
1085      * @throws NyARException\r
1086      */\r
1087     public double modifyMatrix(double trans[],double vertex[][], double pos2d[][]) throws NyARException\r
1088     {\r
1089         double    factor;\r
1090         double    a2, b2, c2;\r
1091         double    ma = 0.0, mb = 0.0, mc = 0.0;\r
1092         double    h, x, y;\r
1093         double    err, minerr=0;\r
1094         int       t1, t2, t3;\r
1095         int       s1 = 0, s2 = 0, s3 = 0;\r
1096 \r
1097         factor = 10.0*Math.PI/180.0;\r
1098         double rot0,rot1,rot3,rot4,rot6,rot7;\r
1099         double combo00,combo01,combo02,combo03,combo10,combo11,combo12,combo13,combo20,combo21,combo22,combo23;\r
1100         double combo02_2,combo02_5,combo02_8,combo02_11;\r
1101         double combo22_2,combo22_5,combo22_8,combo22_11;\r
1102         double combo12_2,combo12_5,combo12_8,combo12_11;\r
1103         //vertex展開\r
1104         final double VX00,VX01,VX02,VX10,VX11,VX12,VX20,VX21,VX22,VX30,VX31,VX32;\r
1105         double[] d_pt;\r
1106         d_pt=vertex[0];VX00=d_pt[0];VX01=d_pt[1];VX02=d_pt[2];\r
1107         d_pt=vertex[1];VX10=d_pt[0];VX11=d_pt[1];VX12=d_pt[2];\r
1108         d_pt=vertex[2];VX20=d_pt[0];VX21=d_pt[1];VX22=d_pt[2];\r
1109         d_pt=vertex[3];VX30=d_pt[0];VX31=d_pt[1];VX32=d_pt[2];\r
1110         final double P2D00,P2D01,P2D10,P2D11,P2D20,P2D21,P2D30,P2D31;\r
1111         d_pt=pos2d[0];P2D00=d_pt[0];P2D01=d_pt[1];\r
1112         d_pt=pos2d[1];P2D10=d_pt[0];P2D11=d_pt[1];\r
1113         d_pt=pos2d[2];P2D20=d_pt[0];P2D21=d_pt[1];\r
1114         d_pt=pos2d[3];P2D30=d_pt[0];P2D31=d_pt[1];\r
1115         final double cpara[]=cparam.get34Array();\r
1116         final double CP0,CP1,CP2,CP3,CP4,CP5,CP6,CP7,CP8,CP9,CP10;\r
1117         CP0=cpara[0];CP1=cpara[1];CP2=cpara[2];CP3=cpara[3];\r
1118         CP4=cpara[4];CP5=cpara[5];CP6=cpara[6];CP7=cpara[7];\r
1119         CP8=cpara[8];CP9=cpara[9];CP10=cpara[10];\r
1120         combo03 = CP0 * trans[0]+ CP1 * trans[1]+ CP2 * trans[2]+ CP3;\r
1121         combo13 = CP4 * trans[0]+ CP5 * trans[1]+ CP6 * trans[2]+ CP7;\r
1122         combo23 = CP8 * trans[0]+ CP9 * trans[1]+ CP10 * trans[2]+ cpara[11];\r
1123         double CACA,SASA,SACA,CA,SA;\r
1124         double CACACB,SACACB,SASACB,CASB,SASB;\r
1125         double SACASC,SACACBSC,SACACBCC,SACACC;        \r
1126         final double[][] double1D=this.wk_arModifyMatrix_double1D;\r
1127 \r
1128         final double[] abc     =double1D[0];\r
1129         final double[] a_factor=double1D[1];\r
1130         final double[] sinb    =double1D[2];\r
1131         final double[] cosb    =double1D[3];\r
1132         final double[] b_factor=double1D[4];\r
1133         final double[] sinc    =double1D[5];\r
1134         final double[] cosc    =double1D[6];\r
1135         final double[] c_factor=double1D[7];\r
1136         double w,w2;\r
1137         double wsin,wcos;\r
1138 \r
1139         arGetAngle(abc);//arGetAngle( rot, &a, &b, &c );\r
1140         a2 = abc[0];\r
1141         b2 = abc[1];\r
1142         c2 = abc[2];\r
1143         \r
1144         //comboの3行目を先に計算\r
1145         for(int i = 0; i < 10; i++ ) {\r
1146             minerr = 1000000000.0;\r
1147             //sin-cosテーブルを計算(これが外に出せるとは…。)\r
1148             for(int j=0;j<3;j++){\r
1149                 w2=factor*(j-1);\r
1150                 w= a2 + w2;\r
1151                 a_factor[j]=w;\r
1152                 w= b2 + w2;\r
1153                 b_factor[j]=w;\r
1154                 sinb[j]=Math.sin(w);\r
1155                 cosb[j]=Math.cos(w);\r
1156                 w= c2 + w2;\r
1157                 c_factor[j]=w;\r
1158                 sinc[j]=Math.sin(w);\r
1159                 cosc[j]=Math.cos(w);\r
1160             }\r
1161             //\r
1162             for(t1=0;t1<3;t1++) {\r
1163                 SA = Math.sin(a_factor[t1]);\r
1164                 CA = Math.cos(a_factor[t1]);\r
1165                 //Optimize\r
1166                 CACA=CA*CA;\r
1167                 SASA=SA*SA;\r
1168                 SACA=SA*CA;\r
1169                 for(t2=0;t2<3;t2++) {\r
1170                     wsin=sinb[t2];\r
1171                     wcos=cosb[t2];\r
1172                     CACACB=CACA*wcos;\r
1173                     SACACB=SACA*wcos;\r
1174                     SASACB=SASA*wcos;\r
1175                     CASB=CA*wsin;\r
1176                     SASB=SA*wsin;\r
1177                     //comboの計算1\r
1178                     combo02 = CP0 * CASB+ CP1 * SASB+ CP2 * wcos;\r
1179                     combo12 = CP4 * CASB+ CP5 * SASB+ CP6 * wcos;\r
1180                     combo22 = CP8 * CASB+ CP9 * SASB+ CP10 * wcos;\r
1181 \r
1182                     combo02_2 =combo02 * VX02 + combo03;\r
1183                     combo02_5 =combo02 * VX12 + combo03;\r
1184                     combo02_8 =combo02 * VX22 + combo03;\r
1185                     combo02_11=combo02 * VX32 + combo03;\r
1186                     combo12_2 =combo12 * VX02 + combo13;\r
1187                     combo12_5 =combo12 * VX12 + combo13;\r
1188                     combo12_8 =combo12 * VX22 + combo13;\r
1189                     combo12_11=combo12 * VX32 + combo13;\r
1190                     combo22_2 =combo22 * VX02 + combo23;\r
1191                     combo22_5 =combo22 * VX12 + combo23;\r
1192                     combo22_8 =combo22 * VX22 + combo23;\r
1193                     combo22_11=combo22 * VX32 + combo23;            \r
1194                     for(t3=0;t3<3;t3++){\r
1195                         wsin=sinc[t3];\r
1196                         wcos=cosc[t3];                  \r
1197                         SACASC=SACA*wsin;\r
1198                         SACACC=SACA*wcos;\r
1199                         SACACBSC=SACACB*wsin;\r
1200                         SACACBCC=SACACB*wcos;\r
1201 \r
1202                         rot0 = CACACB*wcos+SASA*wcos+SACACBSC-SACASC;\r
1203                         rot3 = SACACBCC-SACACC+SASACB*wsin+CACA*wsin;\r
1204                         rot6 = -CASB*wcos-SASB*wsin;\r
1205 \r
1206                         combo00 = CP0 * rot0+ CP1 * rot3+ CP2 * rot6;\r
1207                         combo10 = CP4 * rot0+ CP5 * rot3+ CP6 * rot6;\r
1208                         combo20 = CP8 * rot0+ CP9 * rot3+ CP10 * rot6;\r
1209 \r
1210                         rot1 = -CACACB*wsin-SASA*wsin+SACACBCC-SACACC;\r
1211                         rot4 = -SACACBSC+SACASC+SASACB*wcos+CACA*wcos;\r
1212                         rot7 = CASB*wsin-SASB*wcos;\r
1213                         combo01 = CP0 * rot1+ CP1 * rot4+ CP2 * rot7;\r
1214                         combo11 = CP4 * rot1+ CP5 * rot4+ CP6 * rot7;\r
1215                         combo21 = CP8 * rot1+ CP9 * rot4+ CP10 * rot7;\r
1216                         //\r
1217                         err = 0.0;\r
1218                         h  = combo20 * VX00+ combo21 * VX01+ combo22_2;\r
1219                         x = P2D00 - (combo00 * VX00+ combo01 * VX01+ combo02_2) / h;\r
1220                         y = P2D01 - (combo10 * VX00+ combo11 * VX01+ combo12_2) / h;\r
1221                         err += x*x+y*y;\r
1222                         h  = combo20 * VX10+ combo21 * VX11+ combo22_5;\r
1223                         x = P2D10 - (combo00 * VX10+ combo01 * VX11+ combo02_5) / h;\r
1224                         y = P2D11 - (combo10 * VX10+ combo11 * VX11+ combo12_5) / h;\r
1225                         err += x*x+y*y;\r
1226                         h  = combo20 * VX20+ combo21 * VX21+ combo22_8;\r
1227                         x = P2D20 - (combo00 * VX20+ combo01 * VX21+ combo02_8) / h;\r
1228                         y = P2D21 - (combo10 * VX20+ combo11 * VX21+ combo12_8) / h;\r
1229                         err += x*x+y*y;\r
1230                         h  = combo20 * VX30+ combo21 * VX31+ combo22_11;\r
1231                         x = P2D30 - (combo00 * VX30+ combo01 * VX31+ combo02_11) / h;\r
1232                         y = P2D31 - (combo10 * VX30+ combo11 * VX31+ combo12_11) / h;\r
1233                         err += x*x+y*y;\r
1234                         if( err < minerr ) {\r
1235                             minerr = err;\r
1236                             ma = a_factor[t1];\r
1237                             mb = b_factor[t2];\r
1238                             mc = c_factor[t3];\r
1239                             s1 = t1-1;\r
1240                             s2 = t2-1;\r
1241                             s3 = t3-1;\r
1242                         }\r
1243                     }\r
1244                 }\r
1245             }\r
1246             if( s1 == 0 && s2 == 0 && s3 == 0 ){\r
1247                 factor *= 0.5;\r
1248             }\r
1249             a2 = ma;\r
1250             b2 = mb;\r
1251             c2 = mc;\r
1252         }\r
1253         arGetRot(ma,mb,mc,this.array);\r
1254         /*  printf("factor = %10.5f\n", factor*180.0/MD_PI); */\r
1255         return minerr/4;\r
1256     }                       \r
1257 }\r
1258 \r