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

   1 /* \r
   2  * PROJECT: NyARToolkit\r
   3  * --------------------------------------------------------------------------------\r
   4  * This work is based on the original ARToolKit developed by\r
   5  *   Hirokazu Kato\r
   6  *   Mark Billinghurst\r
   7  *   HITLab, University of Washington, Seattle\r
   8  * http://www.hitl.washington.edu/artoolkit/\r
   9  *\r
  10  * The NyARToolkit is Java version ARToolkit class library.\r
  11  * Copyright (C)2008 R.Iizuka\r
  12  *\r
  13  * This program is free software; you can redistribute it and/or\r
  14  * modify it under the terms of the GNU General Public License\r
  15  * as published by the Free Software Foundation; either version 2\r
  16  * of the License, or (at your option) any later version.\r
  17  * \r
  18  * This program is distributed in the hope that it will be useful,\r
  19  * but WITHOUT ANY WARRANTY; without even the implied warranty of\r
  20  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the\r
  21  * GNU General Public License for more details.\r
  22  * \r
  23  * You should have received a copy of the GNU General Public License\r
  24  * along with this framework; if not, write to the Free Software\r
  25  * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA\r
  26  * \r
  27  * For further information please contact.\r
  28  *      http://nyatla.jp/nyatoolkit/\r
  29  *      <airmail(at)ebony.plala.or.jp>\r
  30  * \r
  31  */\r
  32 package jp.nyatla.nyartoolkit.core;\r
  33 \r
  34 import jp.nyatla.nyartoolkit.NyARException;\r
  35 import jp.nyatla.nyartoolkit.core.raster.INyARRaster;\r
  36 \r
  37 /**\r
  38  * 24ビットカラーのマーカーを保持するために使うクラスです。\r
  39  * このクラスは、ARToolkitのパターンと、ラスタから取得したパターンを保持します。\r
  40  * 演算順序を含む最適化をしたもの\r
  41  *\r
  42  */\r
  43 public class NyARColorPatt_O2 implements NyARColorPatt\r
  44 {\r
  45     private static final int AR_PATT_SAMPLE_NUM=64;//#define   AR_PATT_SAMPLE_NUM   64\r
  46     private int extpat[][][];\r
  47     private int width;\r
  48     private int height;\r
  49     public NyARColorPatt_O2(int i_width,int i_height)\r
  50     {\r
  51         this.width=i_width;\r
  52         this.height=i_height;\r
  53         this.extpat=new int[i_height][i_width][3];\r
  54         this.wk_pickFromRaster_ext_pat2=new int[i_height][i_width][3];\r
  55     }\r
  56 //    public void setSize(int i_new_width,int i_new_height)\r
  57 //    {\r
  58 //      int array_w=this.extpat[0].length;\r
  59 //      int array_h=this.extpat.length;\r
  60 //      //十分なサイズのバッファがあるか確認\r
  61 //      if(array_w>=i_new_width && array_h>=i_new_height){\r
  62 //          //OK 十分だ→サイズ調整のみ\r
  63 //      }else{\r
  64 //          //足りないよ→取り直し\r
  65 //          this.wk_pickFromRaster_ext_pat2=new int[i_new_height][i_new_width][3];\r
  66 //          this.extpat=new int[i_new_height][i_new_width][3];\r
  67 //      }\r
  68 //        this.width =i_new_width;\r
  69 //        this.height=i_new_height;\r
  70 //        return;\r
  71 //    }\r
  72     public int[][][] getPatArray()\r
  73     {\r
  74         return extpat;\r
  75     }\r
  76     public int getWidth()\r
  77     {\r
  78         return width;\r
  79     }\r
  80     public int getHeight()\r
  81     {\r
  82         return height;\r
  83     }\r
  84     private final NyARMat wk_get_cpara_a=new NyARMat(8,8);\r
  85     private final NyARMat wk_get_cpara_b=new NyARMat(8,1);\r
  86 //    private final NyARMat wk_get_cpara_c=new NyARMat(8,1);\r
  87     /**\r
  88      * @param world\r
  89      * @param vertex\r
  90      * @param o_para\r
  91      * @throws NyARException\r
  92      */\r
  93     private boolean get_cpara(double vertex_0[], double vertex_1[],NyARMat o_para) throws NyARException\r
  94     {\r
  95         double world[][]=this.wk_pickFromRaster_world;\r
  96         NyARMat a =wk_get_cpara_a;//次処理で値を設定するので、初期化不要// new NyARMat( 8, 8 );\r
  97         double[][] a_array=a.getArray();\r
  98         NyARMat b =wk_get_cpara_b;//次処理で値を設定するので、初期化不要// new NyARMat( 8, 1 );\r
  99         double[][] b_array=b.getArray();\r
 100         double[] a_pt0,a_pt1,world_pti;\r
 101 \r
 102         for(int i = 0; i < 4; i++ ) {\r
 103             a_pt0=a_array[i*2];\r
 104             a_pt1=a_array[i*2+1];\r
 105             world_pti=world[i];\r
 106 \r
 107             a_pt0[0]=world_pti[0];//a->m[i*16+0]  = world[i][0];\r
 108             a_pt0[1]=world_pti[1];//a->m[i*16+1]  = world[i][1];\r
 109             a_pt0[2]=1.0;//a->m[i*16+2]  = 1.0;\r
 110             a_pt0[3]=0.0;//a->m[i*16+3]  = 0.0;\r
 111             a_pt0[4]=0.0;//a->m[i*16+4]  = 0.0;\r
 112             a_pt0[5]=0.0;//a->m[i*16+5]  = 0.0;\r
 113             a_pt0[6]=-world_pti[0] * vertex_0[i];//a->m[i*16+6]  = -world[i][0] * vertex[i][0];\r
 114             a_pt0[7]=-world_pti[1] * vertex_0[i];//a->m[i*16+7]  = -world[i][1] * vertex[i][0];\r
 115             a_pt1[0]=0.0;//a->m[i*16+8]  = 0.0;\r
 116             a_pt1[1]=0.0;//a->m[i*16+9]  = 0.0;\r
 117             a_pt1[2]=0.0;//a->m[i*16+10] = 0.0;\r
 118             a_pt1[3]=world_pti[0];//a->m[i*16+11] = world[i][0];\r
 119             a_pt1[4]=world_pti[1];//a->m[i*16+12] = world[i][1];\r
 120             a_pt1[5]=1.0;//a->m[i*16+13] = 1.0;\r
 121             a_pt1[6]=-world_pti[0] * vertex_1[i];//a->m[i*16+14] = -world[i][0] * vertex[i][1];\r
 122             a_pt1[7]=-world_pti[1] * vertex_1[i];//a->m[i*16+15] = -world[i][1] * vertex[i][1];\r
 123             b_array[i*2+0][0]=vertex_0[i];//b->m[i*2+0] = vertex[i][0];\r
 124             b_array[i*2+1][0]=vertex_1[i];//b->m[i*2+1] = vertex[i][1];\r
 125         }\r
 126 //      JartkException.trap("未チェックのパス");\r
 127         if(!a.matrixSelfInv()){\r
 128             return false;\r
 129         }\r
 130 \r
 131 //      JartkException.trap("未チェックのパス");\r
 132 //      NyARMat c = wk_get_cpara_c;//次処理で結果を受け取るので、初期化不要//new NyARMat( 8, 1 );\r
 133 //      double[][] c_array=c.getArray();\r
 134 \r
 135         o_para.matrixMul(a, b);\r
 136 //      para[0*3+0] = c_array[0*3+0][0];//para[i][0] = c->m[i*3+0];\r
 137 //      para[0*3+1] = c_array[0*3+1][0];//para[i][1] = c->m[i*3+1];\r
 138 //      para[0*3+2] = c_array[0*3+2][0];//para[i][2] = c->m[i*3+2];\r
 139 //      para[1*3+0] = c_array[1*3+0][0];//para[i][0] = c->m[i*3+0];\r
 140 //      para[1*3+1] = c_array[1*3+1][0];//para[i][1] = c->m[i*3+1];\r
 141 //      para[i*3+2] = c_array[1*3+2][0];//para[i][2] = c->m[i*3+2];\r
 142 //      para[2*3+0] = c_array[2*3+0][0];//para[2][0] = c->m[2*3+0];\r
 143 //      para[2*3+1] = c_array[2*3+1][0];//para[2][1] = c->m[2*3+1];\r
 144 //      para[2*3+2] = 1.0;//para[2][2] = 1.0;\r
 145         return true;\r
 146     }\r
 147 \r
 148   //   private final double[] wk_pickFromRaster_para=new double[9];//[3][3];\r
 149     private int[][][] wk_pickFromRaster_ext_pat2=null;//コンストラクタでint[height][width][3]を作る\r
 150     private final double[][] wk_pickFromRaster_world={//double    world[4][2];\r
 151             {100.0,     100.0},\r
 152             {100.0+10.0,100.0},\r
 153             {100.0+10.0,100.0 + 10.0},\r
 154             {100.0,     100.0 + 10.0}\r
 155     };\r
 156     /**\r
 157      * pickFromRaster関数から使う変数です。\r
 158      *\r
 159      */\r
 160     private static void initValue_wk_pickFromRaster_ext_pat2(int[][][] i_ext_pat2,int i_width,int i_height)\r
 161     {\r
 162         int i,i2;\r
 163         int[][] pt2;\r
 164         int[]   pt1;\r
 165         for(i=i_height-1;i>=0;i--){\r
 166             pt2=i_ext_pat2[i];\r
 167             for(i2=i_width-1;i2>=0;i2--){\r
 168                 pt1=pt2[i2];\r
 169                 pt1[0]=0;\r
 170                 pt1[1]=0;\r
 171                 pt1[2]=0;\r
 172             }\r
 173         }\r
 174     }\r
 175     private final double[][] wk_pickFromRaster_local=new double[2][4];\r
 176     private final int[] wk_pickFromRaster_rgb_tmp=new int[3];\r
 177     private final NyARMat wk_pickFromRaster_cpara=new NyARMat(8,1);\r
 178     /**\r
 179      * imageから、i_markerの位置にあるパターンを切り出して、保持します。\r
 180      * Optimize:STEP[769->750]\r
 181      * @param image\r
 182      * @param i_marker\r
 183      * @throws Exception\r
 184      */\r
 185     public boolean pickFromRaster(INyARRaster image, NyARSquare i_square) throws NyARException\r
 186     {\r
 187         NyARMat cpara=this.wk_pickFromRaster_cpara;\r
 188         //localの計算        \r
 189 //      int[] x_coord=i_marker.x_coord;\r
 190 //      int[] y_coord=i_marker.y_coord;\r
 191 //      int[] vertex=i_marker.mkvertex;\r
 192         double[] local_0=wk_pickFromRaster_local[0];//double    local[4][2];    \r
 193         double[] local_1=wk_pickFromRaster_local[1];//double    local[4][2];    \r
 194         //\r
 195         for(int i = 0; i < 4; i++ ) {\r
 196             local_0[i] = i_square.imvertex[i][0];\r
 197             local_1[i] = i_square.imvertex[i][1];\r
 198         }       \r
 199         \r
 200         //xdiv2,ydiv2の計算\r
 201         int xdiv2, ydiv2;\r
 202         int l1,l2;\r
 203         double w1,w2;\r
 204 \r
 205         //x計算\r
 206         w1=local_0[0] - local_0[1];\r
 207         w2=local_1[0] - local_1[1];\r
 208         l1 = (int)(w1*w1+w2*w2);\r
 209         w1=local_0[2] - local_0[3];\r
 210         w2=local_1[2] - local_1[3];\r
 211         l2 = (int)(w1*w1+w2*w2);\r
 212         if( l2 > l1 ){\r
 213             l1 = l2;\r
 214         }\r
 215         l1=l1/4;\r
 216         xdiv2 =this.width;\r
 217         while( xdiv2*xdiv2 < l1 ){\r
 218             xdiv2*=2;\r
 219         }\r
 220         if( xdiv2 > AR_PATT_SAMPLE_NUM)\r
 221         {\r
 222             xdiv2 =AR_PATT_SAMPLE_NUM;\r
 223         }\r
 224         \r
 225         //y計算\r
 226         w1=local_0[1] - local_0[2];\r
 227         w2=local_1[1] - local_1[2];\r
 228         l1 = (int)(w1*w1+ w2*w2);\r
 229         w1=local_0[3] - local_0[0];\r
 230         w2=local_1[3] - local_1[0];\r
 231         l2 = (int)(w1*w1+ w2*w2);\r
 232         if( l2 > l1 ){\r
 233             l1 = l2;\r
 234         }\r
 235         ydiv2 =this.height;\r
 236         l1=l1/4;\r
 237         while( ydiv2*ydiv2 < l1 ){\r
 238             ydiv2*=2;\r
 239         }\r
 240         if( ydiv2 >AR_PATT_SAMPLE_NUM)\r
 241         {\r
 242             ydiv2 = AR_PATT_SAMPLE_NUM;\r
 243         }       \r
 244         \r
 245         //cparaの計算\r
 246         if(!get_cpara(local_0,local_1,cpara)){\r
 247             return false;\r
 248         }\r
 249 \r
 250         int img_x=image.getWidth();\r
 251         int img_y=image.getHeight();\r
 252 \r
 253         /*wk_pickFromRaster_ext_pat2ワーク変数を初期化する。*/\r
 254         int[][][] ext_pat2=wk_pickFromRaster_ext_pat2;//ARUint32  ext_pat2[AR_PATT_SIZE_Y][AR_PATT_SIZE_X][3];\r
 255         int extpat_j[][],extpat_j_i[];\r
 256         int ext_pat2_j[][],ext_pat2_j_i[];\r
 257 \r
 258         initValue_wk_pickFromRaster_ext_pat2(ext_pat2,this.width,this.height);\r
 259 \r
 260         double[][] cpara_array=cpara.getArray();\r
 261         double para21_x_yw,para01_x_yw,para11_x_yw;\r
 262         double para00,para01,para02,para10,para11,para12,para20,para21;\r
 263         para00 = cpara_array[0*3+0][0];//para[i][0] = c->m[i*3+0];\r
 264         para01 = cpara_array[0*3+1][0];//para[i][1] = c->m[i*3+1];\r
 265         para02 = cpara_array[0*3+2][0];//para[i][2] = c->m[i*3+2];\r
 266         para10 = cpara_array[1*3+0][0];//para[i][0] = c->m[i*3+0];\r
 267         para11 = cpara_array[1*3+1][0];//para[i][1] = c->m[i*3+1];\r
 268         para12 = cpara_array[1*3+2][0];//para[i][2] = c->m[i*3+2];\r
 269         para20 = cpara_array[2*3+0][0];//para[2][0] = c->m[2*3+0];\r
 270         para21 = cpara_array[2*3+1][0];//para[2][1] = c->m[2*3+1];\r
 271         //para22 = 1.0;//para[2][2] = 1.0;\r
 272 \r
 273         \r
 274         double          d, xw, yw;\r
 275         int             xc, yc;\r
 276         int i,j;\r
 277         int[] rgb_tmp=wk_pickFromRaster_rgb_tmp;\r
 278         //      arGetCode_put_zero(ext_pat2);//put_zero( (ARUint8 *)ext_pat2, AR_PATT_SIZE_Y*AR_PATT_SIZE_X*3*sizeof(ARUint32) );\r
 279         int xdiv = xdiv2/width;//xdiv = xdiv2/Config.AR_PATT_SIZE_X;\r
 280         int ydiv = ydiv2/height;//ydiv = ydiv2/Config.AR_PATT_SIZE_Y;\r
 281         double xdiv2_reciprocal = 1.0 / xdiv2;\r
 282         double ydiv2_reciprocal = 1.0 / ydiv2;\r
 283 \r
 284         for(j = 0; j < ydiv2; j++ ) {\r
 285             yw = 102.5 + 5.0 * ((double)j+0.5) * ydiv2_reciprocal;\r
 286             para21_x_yw=para21*yw+1.0;\r
 287             para11_x_yw=para11*yw+para12;\r
 288             para01_x_yw=para01*yw+para02;\r
 289             ext_pat2_j=ext_pat2[j/ydiv];\r
 290             for(i = 0; i < xdiv2; i++ ) {\r
 291                 xw = 102.5 + 5.0 * ((double)i+0.5) * xdiv2_reciprocal;\r
 292                 d = para20*xw + para21_x_yw;\r
 293                 if( d == 0 ){\r
 294                     throw new NyARException();\r
 295                 }\r
 296                 xc = (int)((para00*xw + para01_x_yw)/d);\r
 297                 yc = (int)((para10*xw + para11_x_yw)/d);\r
 298 \r
 299 \r
 300                 if( xc >= 0 && xc < img_x && yc >= 0 && yc < img_y ) {\r
 301                     image.getPixel(xc, yc, rgb_tmp);\r
 302                     ext_pat2_j_i=ext_pat2_j[i/xdiv];\r
 303 \r
 304                     ext_pat2_j_i[0] += rgb_tmp[0];//R\r
 305                     ext_pat2_j_i[1] += rgb_tmp[1];//G\r
 306                     ext_pat2_j_i[2] += rgb_tmp[2];//B\r
 307                 }\r
 308             }\r
 309         }\r
 310         /*<Optimize>*/\r
 311         int xdiv_x_ydiv=xdiv*ydiv;\r
 312         for(j =this.height-1; j>=0; j--){\r
 313             extpat_j=extpat[j];\r
 314             ext_pat2_j=ext_pat2[j];\r
 315             for(i = this.width-1; i>=0; i--){                           // PRL 2006-06-08.\r
 316                 ext_pat2_j_i=ext_pat2_j[i];\r
 317                 extpat_j_i=extpat_j[i];\r
 318                 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
 319                 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
 320                 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
 321             }\r
 322         }\r
 323         return true;\r
 324     }\r
 325 }