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