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