tags/2.5.0/src/jp/nyatla/nyartoolkit/core/pickup/NyARColorPatt_O3.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 edition ARToolKit class library.\r
  11  * Copyright (C)2008-2009 Ryo Iizuka\r
  12  *\r
  13  * This program is free software: you can redistribute it and/or modify\r
  14  * it under the terms of the GNU General Public License as published by\r
  15  * the Free Software Foundation, either version 3 of the License, or\r
  16  * (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 program.  If not, see <http://www.gnu.org/licenses/>.\r
  25  * \r
  26  * For further information please contact.\r
  27  *      http://nyatla.jp/nyatoolkit/\r
  28  *      <airmail(at)ebony.plala.or.jp> or <nyatla(at)nyatla.jp>\r
  29  * \r
  30  */\r
  31 package jp.nyatla.nyartoolkit.core.pickup;\r
  32 \r
  33 import jp.nyatla.nyartoolkit.NyARException;\r
  34 import jp.nyatla.nyartoolkit.core.NyARMat;\r
  35 import jp.nyatla.nyartoolkit.core.raster.rgb.*;\r
  36 import jp.nyatla.nyartoolkit.core.rasterreader.*;\r
  37 import jp.nyatla.nyartoolkit.core.types.*;\r
  38 import jp.nyatla.nyartoolkit.core.raster.*;\r
  39 \r
  40 /**\r
  41  * 24ビットカラーのマーカーを保持するために使うクラスです。 このクラスは、ARToolkitのパターンと、ラスタから取得したパターンを保持します。\r
  42  * 演算順序を含む最適化をしたもの\r
  43  * \r
  44  */\r
  45 public class NyARColorPatt_O3 implements INyARColorPatt\r
  46 {\r
  47         private static final int AR_PATT_SAMPLE_NUM = 64;\r
  48         private static final int BUFFER_FORMAT=NyARBufferType.INT1D_X8R8G8B8_32;\r
  49 \r
  50         private int[] _patdata;\r
  51         private NyARIntSize _size;\r
  52         private NyARRgbPixelReader_INT1D_X8R8G8B8_32 _pixelreader;\r
  53         \r
  54         public NyARColorPatt_O3(int i_width, int i_height)\r
  55         {\r
  56                 this._size=new NyARIntSize(i_width,i_height);\r
  57                 this._patdata = new int[i_height*i_width];\r
  58                 this._pixelreader=new NyARRgbPixelReader_INT1D_X8R8G8B8_32(this._patdata,this._size);\r
  59         }\r
  60         public int getWidth()\r
  61         {\r
  62                 return this._size.w;\r
  63         }\r
  64         public int getHeight()\r
  65         {\r
  66                 return this._size.h;\r
  67         }\r
  68         public NyARIntSize getSize()\r
  69         {\r
  70                 return  this._size;\r
  71         }\r
  72         public INyARRgbPixelReader getRgbPixelReader()\r
  73         {\r
  74                 return this._pixelreader;\r
  75         }\r
  76         public Object getBuffer()\r
  77         {\r
  78                 return this._patdata;\r
  79         }\r
  80         public boolean hasBuffer()\r
  81         {\r
  82                 return this._patdata!=null;\r
  83         }\r
  84         public void wrapBuffer(Object i_ref_buf) throws NyARException\r
  85         {\r
  86                 NyARException.notImplement();\r
  87         }\r
  88         final public int getBufferType()\r
  89         {\r
  90                 return BUFFER_FORMAT;\r
  91         }\r
  92         final public boolean isEqualBufferType(int i_type_value)\r
  93         {\r
  94                 return BUFFER_FORMAT==i_type_value;\r
  95         }\r
  96         private final NyARMat wk_get_cpara_a = new NyARMat(8, 8);\r
  97         private final NyARMat wk_get_cpara_b = new NyARMat(8, 1);\r
  98         private final NyARMat wk_pickFromRaster_cpara = new NyARMat(8, 1);\r
  99 \r
 100         /**\r
 101          * @param world\r
 102          * @param vertex\r
 103          * @param o_para\r
 104          * @throws NyARException\r
 105          */\r
 106         private boolean get_cpara(final NyARIntPoint2d[] i_vertex, NyARMat o_para)throws NyARException\r
 107         {\r
 108                 int[][] world = wk_pickFromRaster_world;\r
 109                 NyARMat a = wk_get_cpara_a;// 次処理で値を設定するので、初期化不要// new NyARMat( 8, 8 );\r
 110                 double[][] a_array = a.getArray();\r
 111                 NyARMat b = wk_get_cpara_b;// 次処理で値を設定するので、初期化不要// new NyARMat( 8, 1 );\r
 112                 double[][] b_array = b.getArray();\r
 113                 double[] a_pt0, a_pt1;\r
 114                 int[] world_pti;\r
 115 \r
 116                 for (int i = 0; i < 4; i++) {\r
 117                         a_pt0 = a_array[i * 2];\r
 118                         a_pt1 = a_array[i * 2 + 1];\r
 119                         world_pti = world[i];\r
 120 \r
 121                         a_pt0[0] = (double) world_pti[0];// a->m[i*16+0] = world[i][0];\r
 122                         a_pt0[1] = (double) world_pti[1];// a->m[i*16+1] = world[i][1];\r
 123                         a_pt0[2] = 1.0;// a->m[i*16+2] = 1.0;\r
 124                         a_pt0[3] = 0.0;// a->m[i*16+3] = 0.0;\r
 125                         a_pt0[4] = 0.0;// a->m[i*16+4] = 0.0;\r
 126                         a_pt0[5] = 0.0;// a->m[i*16+5] = 0.0;\r
 127                         a_pt0[6] = (double) (-world_pti[0] * i_vertex[i].x);// a->m[i*16+6]= -world[i][0]*vertex[i][0];\r
 128                         a_pt0[7] = (double) (-world_pti[1] * i_vertex[i].x);// a->m[i*16+7]=-world[i][1]*vertex[i][0];\r
 129                         a_pt1[0] = 0.0;// a->m[i*16+8] = 0.0;\r
 130                         a_pt1[1] = 0.0;// a->m[i*16+9] = 0.0;\r
 131                         a_pt1[2] = 0.0;// a->m[i*16+10] = 0.0;\r
 132                         a_pt1[3] = (double) world_pti[0];// a->m[i*16+11] = world[i][0];\r
 133                         a_pt1[4] = (double) world_pti[1];// a->m[i*16+12] = world[i][1];\r
 134                         a_pt1[5] = 1.0;// a->m[i*16+13] = 1.0;\r
 135                         a_pt1[6] = (double) (-world_pti[0] * i_vertex[i].y);// a->m[i*16+14]=-world[i][0]*vertex[i][1];\r
 136                         a_pt1[7] = (double) (-world_pti[1] * i_vertex[i].y);// a->m[i*16+15]=-world[i][1]*vertex[i][1];\r
 137                         b_array[i * 2 + 0][0] = (double) i_vertex[i].x;// b->m[i*2+0] =vertex[i][0];\r
 138                         b_array[i * 2 + 1][0] = (double) i_vertex[i].y;// b->m[i*2+1] =vertex[i][1];\r
 139                 }\r
 140                 if (!a.matrixSelfInv()) {\r
 141                         return false;\r
 142                 }\r
 143 \r
 144                 o_para.matrixMul(a, b);\r
 145                 return true;\r
 146         }\r
 147 \r
 148         // private final double[] wk_pickFromRaster_para=new double[9];//[3][3];\r
 149         private final static int[][] wk_pickFromRaster_world = {// double world[4][2];\r
 150         { 100, 100 }, { 100 + 10, 100 }, { 100 + 10, 100 + 10 }, { 100, 100 + 10 } };\r
 151 \r
 152 \r
 153 \r
 154         /**\r
 155          * @see INyARColorPatt#pickFromRaster\r
 156          */\r
 157         public boolean pickFromRaster(INyARRgbRaster image,NyARIntPoint2d[] i_vertexs)throws NyARException\r
 158         {\r
 159                 NyARMat cpara = this.wk_pickFromRaster_cpara;\r
 160                 // xdiv2,ydiv2の計算\r
 161                 int xdiv2, ydiv2;\r
 162                 int l1, l2;\r
 163                 int w1, w2;\r
 164                 // x計算\r
 165                 w1 = i_vertexs[0].x - i_vertexs[1].x;\r
 166                 w2 = i_vertexs[0].y - i_vertexs[1].y;\r
 167                 l1 = (w1 * w1 + w2 * w2);\r
 168                 w1 = i_vertexs[2].x - i_vertexs[3].x;\r
 169                 w2 = i_vertexs[2].y - i_vertexs[3].y;\r
 170                 l2 = (w1 * w1 + w2 * w2);\r
 171                 if (l2 > l1) {\r
 172                         l1 = l2;\r
 173                 }\r
 174                 l1 = l1 / 4;\r
 175                 xdiv2 = this._size.w;\r
 176                 while (xdiv2 * xdiv2 < l1) {\r
 177                         xdiv2 *= 2;\r
 178                 }\r
 179                 if (xdiv2 > AR_PATT_SAMPLE_NUM) {\r
 180                         xdiv2 = AR_PATT_SAMPLE_NUM;\r
 181                 }\r
 182 \r
 183                 // y計算\r
 184                 w1 = i_vertexs[1].x - i_vertexs[2].x;\r
 185                 w2 = i_vertexs[1].y - i_vertexs[2].y;\r
 186                 l1 = (w1 * w1 + w2 * w2);\r
 187                 w1 = i_vertexs[3].x - i_vertexs[0].x;\r
 188                 w2 = i_vertexs[3].y - i_vertexs[0].y;\r
 189                 l2 = (w1 * w1 + w2 * w2);\r
 190                 if (l2 > l1) {\r
 191                         l1 = l2;\r
 192                 }\r
 193                 ydiv2 = this._size.h;\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                         ydiv2 = AR_PATT_SAMPLE_NUM;\r
 200                 }\r
 201 \r
 202                 // cparaの計算\r
 203                 if (!get_cpara(i_vertexs, cpara)) {\r
 204                         return false;\r
 205                 }\r
 206                 updateExtpat(image, cpara, xdiv2, ydiv2);\r
 207 \r
 208                 return true;\r
 209         }\r
 210         private int[] __updateExtpat_rgbset;\r
 211         private int[] __updateExtpat_xc;\r
 212         private int[] __updateExtpat_yc;\r
 213         private double[] __updateExtpat_xw;\r
 214         private double[] __updateExtpat_yw;\r
 215         private int _last_pix_resolution_x=0;\r
 216         private int _last_pix_resolution_y=0;\r
 217         private void reservWorkBuffers(int i_xdiv,int i_ydiv)\r
 218         {\r
 219                 if(this._last_pix_resolution_x<i_xdiv || this._last_pix_resolution_y<i_ydiv){\r
 220                         this.__updateExtpat_xc=new int[i_xdiv*i_ydiv];\r
 221                         this.__updateExtpat_yc=new int[i_xdiv*i_ydiv];\r
 222                         this.__updateExtpat_xw=new double[i_xdiv];\r
 223                         this.__updateExtpat_yw=new double[i_ydiv];\r
 224                         this.__updateExtpat_rgbset=new int[i_xdiv*i_ydiv*3];\r
 225                         this._last_pix_resolution_x=i_xdiv;\r
 226                         this._last_pix_resolution_y=i_ydiv;\r
 227                 }\r
 228                 return;\r
 229         }\r
 230         private static double LT_POS=102.5;\r
 231         private static double SQ_SIZE=5.0;\r
 232         \r
 233         //分割数16未満になると少し遅くなるかも。\r
 234         private void updateExtpat(INyARRgbRaster image, NyARMat i_cpara, int i_xdiv2,int i_ydiv2) throws NyARException\r
 235         {\r
 236 \r
 237                 int i,j;\r
 238                 int r,g,b;\r
 239                 //ピクセルリーダーを取得\r
 240                 final int pat_size_w=this._size.w;\r
 241                 final int xdiv = i_xdiv2 / pat_size_w;// xdiv = xdiv2/Config.AR_PATT_SIZE_X;\r
 242                 final int ydiv = i_ydiv2 / this._size.h;// ydiv = ydiv2/Config.AR_PATT_SIZE_Y;\r
 243                 final int xdiv_x_ydiv = xdiv * ydiv;\r
 244                 double reciprocal;\r
 245                 final double[][] para=i_cpara.getArray();\r
 246                 final double para00=para[0*3+0][0];\r
 247                 final double para01=para[0*3+1][0];\r
 248                 final double para02=para[0*3+2][0];\r
 249                 final double para10=para[1*3+0][0];\r
 250                 final double para11=para[1*3+1][0];\r
 251                 final double para12=para[1*3+2][0];\r
 252                 final double para20=para[2*3+0][0];\r
 253                 final double para21=para[2*3+1][0];\r
 254 \r
 255                 INyARRgbPixelReader reader=image.getRgbPixelReader();\r
 256                 final int img_width=image.getWidth();\r
 257                 final int img_height=image.getHeight();\r
 258 \r
 259                 //ワークバッファの準備\r
 260                 reservWorkBuffers(xdiv,ydiv);\r
 261                 final double[] xw=this.__updateExtpat_xw;\r
 262                 final double[] yw=this.__updateExtpat_yw;\r
 263                 final int[] xc=this.__updateExtpat_xc;\r
 264                 final int[] yc=this.__updateExtpat_yc;\r
 265                 int[] rgb_set = this.__updateExtpat_rgbset;\r
 266 \r
 267                 \r
 268                 for(int iy=this._size.h-1;iy>=0;iy--){\r
 269                         for(int ix=pat_size_w-1;ix>=0;ix--){\r
 270                                 //xw,ywマップを作成\r
 271                                 reciprocal= 1.0 / i_xdiv2;\r
 272                                 for(i=xdiv-1;i>=0;i--){\r
 273                                         xw[i]=LT_POS + SQ_SIZE * (ix*xdiv+i + 0.5) * reciprocal;\r
 274                                 }\r
 275                                 reciprocal= 1.0 / i_ydiv2;\r
 276                                 for(i=ydiv-1;i>=0;i--){\r
 277                                         yw[i]=LT_POS + SQ_SIZE * (iy*ydiv+i + 0.5) * reciprocal;\r
 278                                 }\r
 279                                 //1ピクセルを構成するピクセル座標の集合をxc,yc配列に取得\r
 280                                 int number_of_pix=0;\r
 281                                 for(i=ydiv-1;i>=0;i--)\r
 282                                 {\r
 283                                         final double para01_x_yw_para02=para01 * yw[i] + para02;\r
 284                                         final double para11_x_yw_para12=para11 * yw[i] + para12;\r
 285                                         final double para12_x_yw_para22=para21 * yw[i]+ 1.0;\r
 286                                         for(j=xdiv-1;j>=0;j--){\r
 287                                                         \r
 288                                                 final double d = para20 * xw[j] + para12_x_yw_para22;\r
 289                                                 if (d == 0) {\r
 290                                                         throw new NyARException();\r
 291                                                 }\r
 292                                                 final int xcw= (int) ((para00 * xw[j] + para01_x_yw_para02) / d);\r
 293                                                 final int ycw= (int) ((para10 * xw[j] + para11_x_yw_para12) / d);\r
 294                                                 if(xcw<0 || xcw>=img_width || ycw<0 ||ycw>=img_height){\r
 295                                                         continue;\r
 296                                                 }\r
 297                                                 xc[number_of_pix] =xcw;\r
 298                                                 yc[number_of_pix] =ycw;\r
 299                                                 number_of_pix++;\r
 300                                         }\r
 301                                 }\r
 302                                 //1ピクセル分の配列を取得\r
 303                                 reader.getPixelSet(xc,yc,number_of_pix, rgb_set);\r
 304                                 r=g=b=0;\r
 305                                 for(i=number_of_pix*3-1;i>=0;i-=3){\r
 306                                         r += rgb_set[i-2];// R\r
 307                                         g += rgb_set[i-1];// G\r
 308                                         b += rgb_set[i];// B\r
 309                                 }\r
 310                                 //1ピクセル確定\r
 311                                 this._patdata[iy*pat_size_w+ix]=(((r / xdiv_x_ydiv)&0xff)<<16)|(((g / xdiv_x_ydiv)&0xff)<<8)|(((b / xdiv_x_ydiv)&0xff));\r
 312                         }\r
 313                 }\r
 314                 return;\r
 315         }\r
 316 }