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.squaredetect.NyARSquare;\r
  38 import jp.nyatla.nyartoolkit.core.types.*;\r
  39 /**\r
  40  * 24ビットカラーのマーカーを保持するために使うクラスです。 このクラスは、ARToolkitのパターンと、ラスタから取得したパターンを保持します。\r
  41  * 演算順序を含む最適化をしたもの\r
  42  * \r
  43  */\r
  44 public class NyARColorPatt_O3 implements INyARColorPatt\r
  45 {\r
  46         private static final int AR_PATT_SAMPLE_NUM = 64;\r
  47 \r
  48         private int[] _patdata;\r
  49         private NyARBufferReader _buf_reader;\r
  50         private NyARIntSize _size;\r
  51         private NyARRgbPixelReader_INT1D_X8R8G8B8_32 _pixelreader;\r
  52         \r
  53         public NyARColorPatt_O3(int i_width, int i_height)\r
  54         {\r
  55                 this._size=new NyARIntSize(i_width,i_height);\r
  56                 this._patdata = new int[i_height*i_width];\r
  57                 this._buf_reader=new NyARBufferReader(this._patdata,NyARBufferReader.BUFFERFORMAT_INT1D_X8R8G8B8_32);\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 INyARBufferReader getBufferReader()\r
  73         {\r
  74                 return this._buf_reader;\r
  75         }\r
  76         public INyARRgbPixelReader getRgbPixelReader()\r
  77         {\r
  78                 return this._pixelreader;\r
  79         }\r
  80         private final NyARMat wk_get_cpara_a = new NyARMat(8, 8);\r
  81         private final NyARMat wk_get_cpara_b = new NyARMat(8, 1);\r
  82         private final NyARMat wk_pickFromRaster_cpara = new NyARMat(8, 1);\r
  83 \r
  84         /**\r
  85          * @param world\r
  86          * @param vertex\r
  87          * @param o_para\r
  88          * @throws NyARException\r
  89          */\r
  90         private boolean get_cpara(final NyARIntPoint2d[] i_vertex, NyARMat o_para)throws NyARException\r
  91         {\r
  92                 int[][] world = wk_pickFromRaster_world;\r
  93                 NyARMat a = wk_get_cpara_a;// 次処理で値を設定するので、初期化不要// new NyARMat( 8, 8 );\r
  94                 double[][] a_array = a.getArray();\r
  95                 NyARMat b = wk_get_cpara_b;// 次処理で値を設定するので、初期化不要// new NyARMat( 8, 1 );\r
  96                 double[][] b_array = b.getArray();\r
  97                 double[] a_pt0, a_pt1;\r
  98                 int[] world_pti;\r
  99 \r
 100                 for (int i = 0; i < 4; i++) {\r
 101                         a_pt0 = a_array[i * 2];\r
 102                         a_pt1 = a_array[i * 2 + 1];\r
 103                         world_pti = world[i];\r
 104 \r
 105                         a_pt0[0] = (double) world_pti[0];// a->m[i*16+0] = world[i][0];\r
 106                         a_pt0[1] = (double) world_pti[1];// a->m[i*16+1] = world[i][1];\r
 107                         a_pt0[2] = 1.0;// a->m[i*16+2] = 1.0;\r
 108                         a_pt0[3] = 0.0;// a->m[i*16+3] = 0.0;\r
 109                         a_pt0[4] = 0.0;// a->m[i*16+4] = 0.0;\r
 110                         a_pt0[5] = 0.0;// a->m[i*16+5] = 0.0;\r
 111                         a_pt0[6] = (double) (-world_pti[0] * i_vertex[i].x);// a->m[i*16+6]= -world[i][0]*vertex[i][0];\r
 112                         a_pt0[7] = (double) (-world_pti[1] * i_vertex[i].x);// a->m[i*16+7]=-world[i][1]*vertex[i][0];\r
 113                         a_pt1[0] = 0.0;// a->m[i*16+8] = 0.0;\r
 114                         a_pt1[1] = 0.0;// a->m[i*16+9] = 0.0;\r
 115                         a_pt1[2] = 0.0;// a->m[i*16+10] = 0.0;\r
 116                         a_pt1[3] = (double) world_pti[0];// a->m[i*16+11] = world[i][0];\r
 117                         a_pt1[4] = (double) world_pti[1];// a->m[i*16+12] = world[i][1];\r
 118                         a_pt1[5] = 1.0;// a->m[i*16+13] = 1.0;\r
 119                         a_pt1[6] = (double) (-world_pti[0] * i_vertex[i].y);// a->m[i*16+14]=-world[i][0]*vertex[i][1];\r
 120                         a_pt1[7] = (double) (-world_pti[1] * i_vertex[i].y);// a->m[i*16+15]=-world[i][1]*vertex[i][1];\r
 121                         b_array[i * 2 + 0][0] = (double) i_vertex[i].x;// b->m[i*2+0] =vertex[i][0];\r
 122                         b_array[i * 2 + 1][0] = (double) i_vertex[i].y;// b->m[i*2+1] =vertex[i][1];\r
 123                 }\r
 124                 if (!a.matrixSelfInv()) {\r
 125                         return false;\r
 126                 }\r
 127 \r
 128                 o_para.matrixMul(a, b);\r
 129                 return true;\r
 130         }\r
 131 \r
 132         // private final double[] wk_pickFromRaster_para=new double[9];//[3][3];\r
 133         private final static int[][] wk_pickFromRaster_world = {// double world[4][2];\r
 134         { 100, 100 }, { 100 + 10, 100 }, { 100 + 10, 100 + 10 }, { 100, 100 + 10 } };\r
 135 \r
 136 \r
 137 \r
 138         /**\r
 139          * imageから、i_markerの位置にあるパターンを切り出して、保持します。 Optimize:STEP[769->750]\r
 140          * \r
 141          * @param image\r
 142          * @param i_marker\r
 143          * @throws Exception\r
 144          */\r
 145         public boolean pickFromRaster(INyARRgbRaster image, NyARSquare i_square)throws NyARException\r
 146         {\r
 147                 NyARMat cpara = this.wk_pickFromRaster_cpara;\r
 148                 NyARIntPoint2d[] local = i_square.imvertex;\r
 149                 // xdiv2,ydiv2の計算\r
 150                 int xdiv2, ydiv2;\r
 151                 int l1, l2;\r
 152                 int w1, w2;\r
 153                 // x計算\r
 154                 w1 = local[0].x - local[1].x;\r
 155                 w2 = local[0].y - local[1].y;\r
 156                 l1 = (w1 * w1 + w2 * w2);\r
 157                 w1 = local[2].x - local[3].x;\r
 158                 w2 = local[2].y - local[3].y;\r
 159                 l2 = (w1 * w1 + w2 * w2);\r
 160                 if (l2 > l1) {\r
 161                         l1 = l2;\r
 162                 }\r
 163                 l1 = l1 / 4;\r
 164                 xdiv2 = this._size.w;\r
 165                 while (xdiv2 * xdiv2 < l1) {\r
 166                         xdiv2 *= 2;\r
 167                 }\r
 168                 if (xdiv2 > AR_PATT_SAMPLE_NUM) {\r
 169                         xdiv2 = AR_PATT_SAMPLE_NUM;\r
 170                 }\r
 171 \r
 172                 // y計算\r
 173                 w1 = local[1].x - local[2].x;\r
 174                 w2 = local[1].y - local[2].y;\r
 175                 l1 = (w1 * w1 + w2 * w2);\r
 176                 w1 = local[3].x - local[0].x;\r
 177                 w2 = local[3].y - local[0].y;\r
 178                 l2 = (w1 * w1 + w2 * w2);\r
 179                 if (l2 > l1) {\r
 180                         l1 = l2;\r
 181                 }\r
 182                 ydiv2 = this._size.h;\r
 183                 l1 = l1 / 4;\r
 184                 while (ydiv2 * ydiv2 < l1) {\r
 185                         ydiv2 *= 2;\r
 186                 }\r
 187                 if (ydiv2 > AR_PATT_SAMPLE_NUM) {\r
 188                         ydiv2 = AR_PATT_SAMPLE_NUM;\r
 189                 }\r
 190 \r
 191                 // cparaの計算\r
 192                 if (!get_cpara(local, cpara)) {\r
 193                         return false;\r
 194                 }\r
 195                 updateExtpat(image, cpara, xdiv2, ydiv2);\r
 196 \r
 197                 return true;\r
 198         }\r
 199         private int[] __updateExtpat_rgbset;\r
 200         private int[] __updateExtpat_xc;\r
 201         private int[] __updateExtpat_yc;\r
 202         private double[] __updateExtpat_xw;\r
 203         private double[] __updateExtpat_yw;\r
 204         private int _last_pix_resolution_x=0;\r
 205         private int _last_pix_resolution_y=0;\r
 206         private void reservWorkBuffers(int i_xdiv,int i_ydiv)\r
 207         {\r
 208                 if(this._last_pix_resolution_x<i_xdiv || this._last_pix_resolution_y<i_ydiv){\r
 209                         this.__updateExtpat_xc=new int[i_xdiv*i_ydiv];\r
 210                         this.__updateExtpat_yc=new int[i_xdiv*i_ydiv];\r
 211                         this.__updateExtpat_xw=new double[i_xdiv];\r
 212                         this.__updateExtpat_yw=new double[i_ydiv];\r
 213                         this.__updateExtpat_rgbset=new int[i_xdiv*i_ydiv*3];\r
 214                         this._last_pix_resolution_x=i_xdiv;\r
 215                         this._last_pix_resolution_y=i_ydiv;\r
 216                 }\r
 217                 return;\r
 218         }\r
 219         private static double LT_POS=102.5;\r
 220         private static double SQ_SIZE=5.0;\r
 221         \r
 222         //分割数16未満になると少し遅くなるかも。\r
 223         private void updateExtpat(INyARRgbRaster image, NyARMat i_cpara, int i_xdiv2,int i_ydiv2) throws NyARException\r
 224         {\r
 225 \r
 226                 int i,j;\r
 227                 int r,g,b;\r
 228                 //ピクセルリーダーを取得\r
 229                 final int pat_size_w=this._size.w;\r
 230                 final int xdiv = i_xdiv2 / pat_size_w;// xdiv = xdiv2/Config.AR_PATT_SIZE_X;\r
 231                 final int ydiv = i_ydiv2 / this._size.h;// ydiv = ydiv2/Config.AR_PATT_SIZE_Y;\r
 232                 final int xdiv_x_ydiv = xdiv * ydiv;\r
 233                 double reciprocal;\r
 234                 final double[][] para=i_cpara.getArray();\r
 235                 final double para00=para[0*3+0][0];\r
 236                 final double para01=para[0*3+1][0];\r
 237                 final double para02=para[0*3+2][0];\r
 238                 final double para10=para[1*3+0][0];\r
 239                 final double para11=para[1*3+1][0];\r
 240                 final double para12=para[1*3+2][0];\r
 241                 final double para20=para[2*3+0][0];\r
 242                 final double para21=para[2*3+1][0];\r
 243 \r
 244                 INyARRgbPixelReader reader=image.getRgbPixelReader();\r
 245                 final int img_width=image.getWidth();\r
 246                 final int img_height=image.getHeight();\r
 247 \r
 248                 //ワークバッファの準備\r
 249                 reservWorkBuffers(xdiv,ydiv);\r
 250                 final double[] xw=this.__updateExtpat_xw;\r
 251                 final double[] yw=this.__updateExtpat_yw;\r
 252                 final int[] xc=this.__updateExtpat_xc;\r
 253                 final int[] yc=this.__updateExtpat_yc;\r
 254                 int[] rgb_set = this.__updateExtpat_rgbset;\r
 255 \r
 256                 \r
 257                 for(int iy=this._size.h-1;iy>=0;iy--){\r
 258                         for(int ix=pat_size_w-1;ix>=0;ix--){\r
 259                                 //xw,ywマップを作成\r
 260                                 reciprocal= 1.0 / i_xdiv2;\r
 261                                 for(i=xdiv-1;i>=0;i--){\r
 262                                         xw[i]=LT_POS + SQ_SIZE * (ix*xdiv+i + 0.5) * reciprocal;\r
 263                                 }\r
 264                                 reciprocal= 1.0 / i_ydiv2;\r
 265                                 for(i=ydiv-1;i>=0;i--){\r
 266                                         yw[i]=LT_POS + SQ_SIZE * (iy*ydiv+i + 0.5) * reciprocal;\r
 267                                 }\r
 268                                 //1ピクセルを構成するピクセル座標の集合をxc,yc配列に取得\r
 269                                 int number_of_pix=0;\r
 270                                 for(i=ydiv-1;i>=0;i--)\r
 271                                 {\r
 272                                         final double para01_x_yw_para02=para01 * yw[i] + para02;\r
 273                                         final double para11_x_yw_para12=para11 * yw[i] + para12;\r
 274                                         final double para12_x_yw_para22=para21 * yw[i]+ 1.0;\r
 275                                         for(j=xdiv-1;j>=0;j--){\r
 276                                                         \r
 277                                                 final double d = para20 * xw[j] + para12_x_yw_para22;\r
 278                                                 if (d == 0) {\r
 279                                                         throw new NyARException();\r
 280                                                 }\r
 281                                                 final int xcw= (int) ((para00 * xw[j] + para01_x_yw_para02) / d);\r
 282                                                 final int ycw= (int) ((para10 * xw[j] + para11_x_yw_para12) / d);\r
 283                                                 if(xcw<0 || xcw>=img_width || ycw<0 ||ycw>=img_height){\r
 284                                                         continue;\r
 285                                                 }\r
 286                                                 xc[number_of_pix] =xcw;\r
 287                                                 yc[number_of_pix] =ycw;\r
 288                                                 number_of_pix++;\r
 289                                         }\r
 290                                 }\r
 291                                 //1ピクセル分の配列を取得\r
 292                                 reader.getPixelSet(xc,yc,number_of_pix, rgb_set);\r
 293                                 r=g=b=0;\r
 294                                 for(i=number_of_pix*3-1;i>=0;i-=3){\r
 295                                         r += rgb_set[i-2];// R\r
 296                                         g += rgb_set[i-1];// G\r
 297                                         b += rgb_set[i];// B\r
 298                                 }\r
 299                                 //1ピクセル確定\r
 300                                 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
 301                         }\r
 302                 }\r
 303                 return;\r
 304         }\r
 305 }