trunk/src/jp/nyatla/nyartoolkit/core/NyARColorPatt_O2.java

   1 package jp.nyatla.nyartoolkit.core;\r
   2 \r
   3 import jp.nyatla.nyartoolkit.NyARException;\r
   4 import jp.nyatla.nyartoolkit.core.raster.NyARRaster;\r
   5 \r
   6 /**\r
   7  * 24ビットカラーのマーカーを保持するために使うクラスです。\r
   8  * このクラスは、ARToolkitのパターンと、ラスタから取得したパターンを保持します。\r
   9  * 演算順序を含む最適化をしたもの\r
  10  *\r
  11  */\r
  12 public class NyARColorPatt_O2 implements NyARColorPatt\r
  13 {\r
  14     private static final int AR_PATT_SAMPLE_NUM=64;//#define   AR_PATT_SAMPLE_NUM   64\r
  15     private int extpat[][][];\r
  16     private int width;\r
  17     private int height;\r
  18     public NyARColorPatt_O2(int i_width,int i_height)\r
  19     {\r
  20         this.width=i_width;\r
  21         this.height=i_height;\r
  22         this.extpat=new int[i_height][i_width][3];\r
  23         this.wk_pickFromRaster_ext_pat2=new int[i_height][i_width][3];\r
  24     }\r
  25     public int[][][] getPatArray()\r
  26     {\r
  27         return extpat;\r
  28     }\r
  29     public int getWidth()\r
  30     {\r
  31         return width;\r
  32     }\r
  33     public int getHeight()\r
  34     {\r
  35         return height;\r
  36     }\r
  37     private final NyARMat wk_get_cpara_a=new NyARMat(8,8);\r
  38     private final NyARMat wk_get_cpara_b=new NyARMat(8,1);\r
  39 //    private final NyARMat wk_get_cpara_c=new NyARMat(8,1);\r
  40     /**\r
  41      * \r
  42      * @param world\r
  43      * @param vertex\r
  44      * @param para\r
  45      * [3x3]\r
  46      * @throws NyARException\r
  47      */\r
  48     /**\r
  49      * @param world\r
  50      * @param vertex\r
  51      * @param o_para\r
  52      * @throws NyARException\r
  53      */\r
  54     private void get_cpara(double vertex_0[], double vertex_1[],NyARMat o_para) throws NyARException\r
  55     {\r
  56         double world[][]=this.wk_pickFromRaster_world;\r
  57         NyARMat a =wk_get_cpara_a;//次処理で値を設定するので、初期化不要// new NyARMat( 8, 8 );\r
  58         double[][] a_array=a.getArray();\r
  59         NyARMat b =wk_get_cpara_b;//次処理で値を設定するので、初期化不要// new NyARMat( 8, 1 );\r
  60         double[][] b_array=b.getArray();\r
  61         double[] a_pt0,a_pt1,world_pti;\r
  62             \r
  63         for(int i = 0; i < 4; i++ ) {\r
  64             a_pt0=a_array[i*2];\r
  65             a_pt1=a_array[i*2+1];\r
  66             world_pti=world[i];\r
  67             \r
  68             a_pt0[0]=world_pti[0];//a->m[i*16+0]  = world[i][0];\r
  69             a_pt0[1]=world_pti[1];//a->m[i*16+1]  = world[i][1];\r
  70             a_pt0[2]=1.0;//a->m[i*16+2]  = 1.0;\r
  71             a_pt0[3]=0.0;//a->m[i*16+3]  = 0.0;\r
  72             a_pt0[4]=0.0;//a->m[i*16+4]  = 0.0;\r
  73             a_pt0[5]=0.0;//a->m[i*16+5]  = 0.0;\r
  74             a_pt0[6]=-world_pti[0] * vertex_0[i];//a->m[i*16+6]  = -world[i][0] * vertex[i][0];\r
  75             a_pt0[7]=-world_pti[1] * vertex_0[i];//a->m[i*16+7]  = -world[i][1] * vertex[i][0];\r
  76             a_pt1[0]=0.0;//a->m[i*16+8]  = 0.0;\r
  77             a_pt1[1]=0.0;//a->m[i*16+9]  = 0.0;\r
  78             a_pt1[2]=0.0;//a->m[i*16+10] = 0.0;\r
  79             a_pt1[3]=world_pti[0];//a->m[i*16+11] = world[i][0];\r
  80             a_pt1[4]=world_pti[1];//a->m[i*16+12] = world[i][1];\r
  81             a_pt1[5]=1.0;//a->m[i*16+13] = 1.0;\r
  82             a_pt1[6]=-world_pti[0] * vertex_1[i];//a->m[i*16+14] = -world[i][0] * vertex[i][1];\r
  83             a_pt1[7]=-world_pti[1] * vertex_1[i];//a->m[i*16+15] = -world[i][1] * vertex[i][1];\r
  84             b_array[i*2+0][0]=vertex_0[i];//b->m[i*2+0] = vertex[i][0];\r
  85             b_array[i*2+1][0]=vertex_1[i];//b->m[i*2+1] = vertex[i][1];\r
  86         }\r
  87 //          JartkException.trap("未チェックのパス");\r
  88         a.matrixSelfInv();\r
  89             \r
  90 //          JartkException.trap("未チェックのパス");\r
  91 //        NyARMat c = wk_get_cpara_c;//次処理で結果を受け取るので、初期化不要//new NyARMat( 8, 1 );\r
  92 //        double[][] c_array=c.getArray();\r
  93 \r
  94         o_para.matrixMul(a, b);\r
  95 //        para[0*3+0] = c_array[0*3+0][0];//para[i][0] = c->m[i*3+0];\r
  96 //        para[0*3+1] = c_array[0*3+1][0];//para[i][1] = c->m[i*3+1];\r
  97 //        para[0*3+2] = c_array[0*3+2][0];//para[i][2] = c->m[i*3+2];\r
  98 //        para[1*3+0] = c_array[1*3+0][0];//para[i][0] = c->m[i*3+0];\r
  99 //        para[1*3+1] = c_array[1*3+1][0];//para[i][1] = c->m[i*3+1];\r
 100 //        para[i*3+2] = c_array[1*3+2][0];//para[i][2] = c->m[i*3+2];\r
 101 //        para[2*3+0] = c_array[2*3+0][0];//para[2][0] = c->m[2*3+0];\r
 102 //        para[2*3+1] = c_array[2*3+1][0];//para[2][1] = c->m[2*3+1];\r
 103 //        para[2*3+2] = 1.0;//para[2][2] = 1.0;\r
 104         \r
 105 \r
 106     }\r
 107 \r
 108   //   private final double[] wk_pickFromRaster_para=new double[9];//[3][3];\r
 109     private int[][][] wk_pickFromRaster_ext_pat2=null;//コンストラクタでint[height][width][3]を作る\r
 110     private final double[][] wk_pickFromRaster_world={//double    world[4][2];\r
 111             {100.0,     100.0},\r
 112             {100.0+10.0,100.0},\r
 113             {100.0+10.0,100.0 + 10.0},\r
 114             {100.0,     100.0 + 10.0}\r
 115     };\r
 116     /**\r
 117      * pickFromRaster関数から使う変数です。\r
 118      *\r
 119      */\r
 120     private static void initValue_wk_pickFromRaster_ext_pat2(int[][][] i_ext_pat2,int i_width,int i_height)\r
 121     {\r
 122         int i,i2;\r
 123         int[][] pt2;\r
 124         int[]   pt1;\r
 125         for(i=i_height-1;i>=0;i--){\r
 126             pt2=i_ext_pat2[i];\r
 127             for(i2=i_width-1;i2>=0;i2--){\r
 128                 pt1=pt2[i2];\r
 129                 pt1[0]=0;\r
 130                 pt1[1]=0;\r
 131                 pt1[2]=0;\r
 132             }\r
 133         }\r
 134     }\r
 135     private final double[][] wk_pickFromRaster_local=new double[2][4];\r
 136     private final int[] wk_pickFromRaster_rgb_tmp=new int[3];\r
 137     private final NyARMat wk_pickFromRaster_cpara=new NyARMat(8,1);\r
 138     /**\r
 139      * imageから、i_markerの位置にあるパターンを切り出して、保持します。\r
 140      * Optimize:STEP[769->750]\r
 141      * @param image\r
 142      * @param i_marker\r
 143      * @throws Exception\r
 144      */\r
 145     public void pickFromRaster(NyARRaster image, NyARMarker i_marker) throws NyARException\r
 146     {\r
 147         NyARMat cpara=this.wk_pickFromRaster_cpara;\r
 148         //localの計算\r
 149         int[] x_coord=i_marker.x_coord;\r
 150         int[] y_coord=i_marker.y_coord;\r
 151         int[] vertex=i_marker.mkvertex;\r
 152         double[] local_0=wk_pickFromRaster_local[0];//double    local[4][2];    \r
 153         double[] local_1=wk_pickFromRaster_local[1];//double    local[4][2];    \r
 154         for(int i = 0; i < 4; i++ ) {\r
 155             local_0[i] = x_coord[vertex[i]];\r
 156             local_1[i] = y_coord[vertex[i]];\r
 157         }\r
 158         //xdiv2,ydiv2の計算\r
 159         int xdiv2, ydiv2;\r
 160         int l1,l2;\r
 161         double w1,w2;\r
 162 \r
 163         //x計算\r
 164         w1=local_0[0] - local_0[1];\r
 165         w2=local_1[0] - local_1[1];\r
 166         l1 = (int)(w1*w1+w2*w2);\r
 167         w1=local_0[2] - local_0[3];\r
 168         w2=local_1[2] - local_1[3];\r
 169         l2 = (int)(w1*w1+w2*w2);\r
 170         if( l2 > l1 ){\r
 171             l1 = l2;\r
 172         }\r
 173         l1=l1/4;\r
 174         xdiv2 =this.width;\r
 175         while( xdiv2*xdiv2 < l1 ){\r
 176             xdiv2*=2;\r
 177         }\r
 178         if( xdiv2 > AR_PATT_SAMPLE_NUM)\r
 179         {\r
 180             xdiv2 =AR_PATT_SAMPLE_NUM;\r
 181         }\r
 182         \r
 183         //y計算\r
 184         w1=local_0[1] - local_0[2];\r
 185         w2=local_1[1] - local_1[2];\r
 186         l1 = (int)(w1*w1+ w2*w2);\r
 187         w1=local_0[3] - local_0[0];\r
 188         w2=local_1[3] - local_1[0];\r
 189         l2 = (int)(w1*w1+ w2*w2);\r
 190         if( l2 > l1 ){\r
 191             l1 = l2;\r
 192         }\r
 193         ydiv2 =this.height;\r
 194         l1=l1/4;\r
 195         while( ydiv2*ydiv2 < l1 ){\r
 196             ydiv2*=2;\r
 197         }\r
 198         if( ydiv2 >AR_PATT_SAMPLE_NUM)\r
 199         {\r
 200             ydiv2 = AR_PATT_SAMPLE_NUM;\r
 201         }       \r
 202         \r
 203         //cparaの計算\r
 204         get_cpara(local_0,local_1,cpara);\r
 205 \r
 206         int img_x=image.getWidth();\r
 207         int img_y=image.getHeight();\r
 208 \r
 209         /*wk_pickFromRaster_ext_pat2ワーク変数を初期化する。*/\r
 210         int[][][] ext_pat2=wk_pickFromRaster_ext_pat2;//ARUint32  ext_pat2[AR_PATT_SIZE_Y][AR_PATT_SIZE_X][3];\r
 211         int extpat_j[][],extpat_j_i[];\r
 212         int ext_pat2_j[][],ext_pat2_j_i[];\r
 213 \r
 214         initValue_wk_pickFromRaster_ext_pat2(ext_pat2,this.width,this.height);\r
 215 \r
 216         double[][] cpara_array=cpara.getArray();\r
 217         double para21_x_yw,para01_x_yw,para11_x_yw;\r
 218         double para00,para01,para02,para10,para11,para12,para20,para21;\r
 219         para00 = cpara_array[0*3+0][0];//para[i][0] = c->m[i*3+0];\r
 220         para01 = cpara_array[0*3+1][0];//para[i][1] = c->m[i*3+1];\r
 221         para02 = cpara_array[0*3+2][0];//para[i][2] = c->m[i*3+2];\r
 222         para10 = cpara_array[1*3+0][0];//para[i][0] = c->m[i*3+0];\r
 223         para11 = cpara_array[1*3+1][0];//para[i][1] = c->m[i*3+1];\r
 224         para12 = cpara_array[1*3+2][0];//para[i][2] = c->m[i*3+2];\r
 225         para20 = cpara_array[2*3+0][0];//para[2][0] = c->m[2*3+0];\r
 226         para21 = cpara_array[2*3+1][0];//para[2][1] = c->m[2*3+1];\r
 227         //para22 = 1.0;//para[2][2] = 1.0;\r
 228 \r
 229         \r
 230         double          d, xw, yw;\r
 231         int             xc, yc;\r
 232         int i,j;\r
 233         int[] rgb_tmp=wk_pickFromRaster_rgb_tmp;\r
 234         //      arGetCode_put_zero(ext_pat2);//put_zero( (ARUint8 *)ext_pat2, AR_PATT_SIZE_Y*AR_PATT_SIZE_X*3*sizeof(ARUint32) );\r
 235         int xdiv = xdiv2/width;//xdiv = xdiv2/Config.AR_PATT_SIZE_X;\r
 236         int ydiv = ydiv2/height;//ydiv = ydiv2/Config.AR_PATT_SIZE_Y;\r
 237         double xdiv2_reciprocal = 1.0 / xdiv2;\r
 238         double ydiv2_reciprocal = 1.0 / ydiv2;\r
 239 \r
 240         for(j = 0; j < ydiv2; j++ ) {\r
 241             yw = 102.5 + 5.0 * ((double)j+0.5) * ydiv2_reciprocal;\r
 242             para21_x_yw=para21*yw+1.0;\r
 243             para11_x_yw=para11*yw+para12;\r
 244             para01_x_yw=para01*yw+para02;\r
 245             ext_pat2_j=ext_pat2[j/ydiv];\r
 246             for(i = 0; i < xdiv2; i++ ) {\r
 247                 xw = 102.5 + 5.0 * ((double)i+0.5) * xdiv2_reciprocal;\r
 248                 d = para20*xw + para21_x_yw;\r
 249                 if( d == 0 ){\r
 250                     throw new NyARException();\r
 251                 }\r
 252                 xc = (int)((para00*xw + para01_x_yw)/d);\r
 253                 yc = (int)((para10*xw + para11_x_yw)/d);\r
 254 \r
 255 \r
 256                 if( xc >= 0 && xc < img_x && yc >= 0 && yc < img_y ) {\r
 257                     image.getPixel(xc, yc, rgb_tmp);\r
 258                     ext_pat2_j_i=ext_pat2_j[i/xdiv];\r
 259 \r
 260                     ext_pat2_j_i[0] += rgb_tmp[0];//R\r
 261                     ext_pat2_j_i[1] += rgb_tmp[1];//G\r
 262                     ext_pat2_j_i[2] += rgb_tmp[2];//B\r
 263                 }\r
 264             }\r
 265         }\r
 266         /*<Optimize>*/\r
 267         int xdiv_x_ydiv=xdiv*ydiv;\r
 268         for(j =this.height-1; j>=0; j--){\r
 269             extpat_j=extpat[j];\r
 270             ext_pat2_j=ext_pat2[j];\r
 271             for(i = this.width-1; i>=0; i--){                           // PRL 2006-06-08.\r
 272                 ext_pat2_j_i=ext_pat2_j[i];\r
 273                 extpat_j_i=extpat_j[i];\r
 274                 extpat_j_i[0]=(ext_pat2_j_i[0] / xdiv_x_ydiv);//ext_pat[j][i][0] = (byte)(ext_pat2[j][i][0] / (xdiv*ydiv));\r
 275                 extpat_j_i[1]=(ext_pat2_j_i[1] / xdiv_x_ydiv);//ext_pat[j][i][1] = (byte)(ext_pat2[j][i][1] / (xdiv*ydiv));\r
 276                 extpat_j_i[2]=(ext_pat2_j_i[2] / xdiv_x_ydiv);//ext_pat[j][i][2] = (byte)(ext_pat2[j][i][2] / (xdiv*ydiv));\r
 277             }\r
 278         }\r
 279         return;\r
 280     }\r
 281 }