branches/nyatla/src/jp/nyatla/nyartoolkit/core/pickup/NyARColorPatt_O1.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 /**\r
  40  * 24ビットカラーのマーカーを保持するために使うクラスです。 このクラスは、ARToolkitのパターンと、ラスタから取得したパターンを保持します。\r
  41  * 演算順序以外の最適化をしたもの\r
  42  * \r
  43  */\r
  44 public class NyARColorPatt_O1 implements INyARColorPatt\r
  45 {\r
  46         private static final int AR_PATT_SAMPLE_NUM = 64;// #define\r
  47                                                                                                                 // AR_PATT_SAMPLE_NUM 64\r
  48 \r
  49         private int extpat[][][];\r
  50 \r
  51         private int width;\r
  52 \r
  53         private int height;\r
  54 \r
  55         public NyARColorPatt_O1(int i_width, int i_height)\r
  56         {\r
  57                 this.width = i_width;\r
  58                 this.height = i_height;\r
  59                 this.extpat = new int[i_height][i_width][3];\r
  60                 this.wk_pickFromRaster_ext_pat2 = new int[i_height][i_width][3];\r
  61         }\r
  62 \r
  63         // public void setSize(int i_new_width,int i_new_height)\r
  64         // {\r
  65         // int array_w=this.extpat[0].length;\r
  66         // int array_h=this.extpat.length;\r
  67         // //十分なサイズのバッファがあるか確認\r
  68         // if(array_w>=i_new_width && array_h>=i_new_height){\r
  69         // //OK 十分だ→サイズ調整のみ\r
  70         // }else{\r
  71         // //足りないよ→取り直し\r
  72         // this.wk_pickFromRaster_ext_pat2=new int[i_new_height][i_new_width][3];\r
  73         // this.extpat=new int[i_new_height][i_new_width][3];\r
  74         // }\r
  75         // this.width =i_new_width;\r
  76         // this.height=i_new_height;\r
  77         // return;\r
  78         // }\r
  79 \r
  80         public int[][][] getPatArray()\r
  81         {\r
  82                 return extpat;\r
  83         }\r
  84 \r
  85         public int getWidth()\r
  86         {\r
  87                 return width;\r
  88         }\r
  89 \r
  90         public int getHeight()\r
  91         {\r
  92                 return height;\r
  93         }\r
  94 \r
  95         private final NyARMat wk_get_cpara_a = new NyARMat(8, 8);\r
  96 \r
  97         private final NyARMat wk_get_cpara_b = new NyARMat(8, 1);\r
  98 \r
  99         private final NyARMat wk_get_cpara_c = new NyARMat(8, 1);\r
 100 \r
 101         /**\r
 102          * \r
 103          * @param world\r
 104          * @param vertex\r
 105          * @param para\r
 106          *            [3x3]\r
 107          * @throws NyARException\r
 108          */\r
 109         private boolean get_cpara(double world[][], double vertex[][], double[] para)\r
 110                         throws NyARException\r
 111         {\r
 112                 NyARMat a = wk_get_cpara_a;// 次処理で値を設定するので、初期化不要// new NyARMat( 8, 8 );\r
 113                 double[][] a_array = a.getArray();\r
 114                 NyARMat b = wk_get_cpara_b;// 次処理で値を設定するので、初期化不要// new NyARMat( 8, 1 );\r
 115                 double[][] b_array = b.getArray();\r
 116                 double[] a_pt0, a_pt1, world_pti;\r
 117 \r
 118                 for (int i = 0; i < 4; i++) {\r
 119                         a_pt0 = a_array[i * 2];\r
 120                         a_pt1 = a_array[i * 2 + 1];\r
 121                         world_pti = world[i];\r
 122 \r
 123                         a_pt0[0] = world_pti[0];// a->m[i*16+0] = world[i][0];\r
 124                         a_pt0[1] = world_pti[1];// a->m[i*16+1] = world[i][1];\r
 125                         a_pt0[2] = 1.0;// a->m[i*16+2] = 1.0;\r
 126                         a_pt0[3] = 0.0;// a->m[i*16+3] = 0.0;\r
 127                         a_pt0[4] = 0.0;// a->m[i*16+4] = 0.0;\r
 128                         a_pt0[5] = 0.0;// a->m[i*16+5] = 0.0;\r
 129                         a_pt0[6] = -world_pti[0] * vertex[i][0];// a->m[i*16+6] =\r
 130                                                                                                         // -world[i][0] *\r
 131                                                                                                         // vertex[i][0];\r
 132                         a_pt0[7] = -world_pti[1] * vertex[i][0];// a->m[i*16+7] =\r
 133                                                                                                         // -world[i][1] *\r
 134                                                                                                         // vertex[i][0];\r
 135                         a_pt1[0] = 0.0;// a->m[i*16+8] = 0.0;\r
 136                         a_pt1[1] = 0.0;// a->m[i*16+9] = 0.0;\r
 137                         a_pt1[2] = 0.0;// a->m[i*16+10] = 0.0;\r
 138                         a_pt1[3] = world_pti[0];// a->m[i*16+11] = world[i][0];\r
 139                         a_pt1[4] = world_pti[1];// a->m[i*16+12] = world[i][1];\r
 140                         a_pt1[5] = 1.0;// a->m[i*16+13] = 1.0;\r
 141                         a_pt1[6] = -world_pti[0] * vertex[i][1];// a->m[i*16+14] =\r
 142                                                                                                         // -world[i][0] *\r
 143                                                                                                         // vertex[i][1];\r
 144                         a_pt1[7] = -world_pti[1] * vertex[i][1];// a->m[i*16+15] =\r
 145                                                                                                         // -world[i][1] *\r
 146                                                                                                         // vertex[i][1];\r
 147                         b_array[i * 2 + 0][0] = vertex[i][0];// b->m[i*2+0] =\r
 148                                                                                                         // vertex[i][0];\r
 149                         b_array[i * 2 + 1][0] = vertex[i][1];// b->m[i*2+1] =\r
 150                                                                                                         // vertex[i][1];\r
 151                 }\r
 152                 // JartkException.trap("未チェックのパス");\r
 153                 if (!a.matrixSelfInv()) {\r
 154                         return false;// 逆行列を求められないので失敗\r
 155                 }\r
 156 \r
 157                 // JartkException.trap("未チェックのパス");\r
 158                 NyARMat c = wk_get_cpara_c;// 次処理で結果を受け取るので、初期化不要//new NyARMat( 8, 1 );\r
 159                 double[][] c_array = c.getArray();\r
 160 \r
 161                 c.matrixMul(a, b);\r
 162                 for (int i = 0; i < 2; i++) {\r
 163                         para[i * 3 + 0] = c_array[i * 3 + 0][0];// para[i][0] = c->m[i*3+0];\r
 164                         para[i * 3 + 1] = c_array[i * 3 + 1][0];// para[i][1] = c->m[i*3+1];\r
 165                         para[i * 3 + 2] = c_array[i * 3 + 2][0];// para[i][2] = c->m[i*3+2];\r
 166                 }\r
 167                 para[2 * 3 + 0] = c_array[2 * 3 + 0][0];// para[2][0] = c->m[2*3+0];\r
 168                 para[2 * 3 + 1] = c_array[2 * 3 + 1][0];// para[2][1] = c->m[2*3+1];\r
 169                 para[2 * 3 + 2] = 1.0;// para[2][2] = 1.0;\r
 170                 return true;\r
 171         }\r
 172 \r
 173         private final double[][] wk_pickFromRaster_local = new double[4][2];\r
 174 \r
 175         private final double[] wk_pickFromRaster_para = new double[9];// [3][3];\r
 176 \r
 177         private int[][][] wk_pickFromRaster_ext_pat2 = null;// コンストラクタでint[height][width][3]を作る\r
 178 \r
 179         private final double[][] wk_pickFromRaster_world = {// double world[4][2];\r
 180         { 100.0, 100.0 }, { 100.0 + 10.0, 100.0 }, { 100.0 + 10.0, 100.0 + 10.0 },\r
 181                         { 100.0, 100.0 + 10.0 } };\r
 182 \r
 183         /**\r
 184          * pickFromRaster関数から使う変数です。\r
 185          * \r
 186          */\r
 187         private static void initValue_wk_pickFromRaster_ext_pat2(\r
 188                         int[][][] i_ext_pat2, int i_width, int i_height)\r
 189         {\r
 190                 int i, i2;\r
 191                 int[][] pt2;\r
 192                 int[] pt1;\r
 193                 for (i = i_height - 1; i >= 0; i--) {\r
 194                         pt2 = i_ext_pat2[i];\r
 195                         for (i2 = i_width - 1; i2 >= 0; i2--) {\r
 196                                 pt1 = pt2[i2];\r
 197                                 pt1[0] = 0;\r
 198                                 pt1[1] = 0;\r
 199                                 pt1[2] = 0;\r
 200                         }\r
 201                 }\r
 202         }\r
 203 \r
 204         private final int[] wk_pickFromRaster_rgb_tmp = new int[3];\r
 205 \r
 206         /**\r
 207          * imageから、i_markerの位置にあるパターンを切り出して、保持します。 Optimize:STEP[769->]\r
 208          * \r
 209          * @param image\r
 210          * @param i_marker\r
 211          * @return 切り出しに失敗した\r
 212          * @throws Exception\r
 213          */\r
 214         public boolean pickFromRaster(INyARRgbRaster image, NyARSquare i_square)throws NyARException\r
 215         {\r
 216                 double d, xw, yw;\r
 217                 int xc, yc;\r
 218                 int xdiv, ydiv;\r
 219                 int xdiv2, ydiv2;\r
 220                 int lx1, lx2, ly1, ly2;\r
 221 \r
 222                 int img_x = image.getWidth();\r
 223                 int img_y = image.getHeight();\r
 224 \r
 225                 double xdiv2_reciprocal; // [tp]\r
 226                 double ydiv2_reciprocal; // [tp]\r
 227 \r
 228                 // int[] x_coord=i_marker.x_coord;\r
 229                 // int[] y_coord=i_marker.y_coord;\r
 230                 // int[] vertex=i_marker.mkvertex;\r
 231                 double[][] local = wk_pickFromRaster_local;// double local[4][2];\r
 232                 //\r
 233                 for (int i = 0; i < 4; i++) {\r
 234                         local[i][0] = i_square.imvertex[i].x;\r
 235                         local[i][1] = i_square.imvertex[i].y;\r
 236                 }\r
 237 \r
 238                 double[][] world = wk_pickFromRaster_world;\r
 239                 /*\r
 240                  * world[0][0] = 100.0; world[0][1] = 100.0; world[1][0] = 100.0 + 10.0;\r
 241                  * world[1][1] = 100.0; world[2][0] = 100.0 + 10.0; world[2][1] = 100.0 +\r
 242                  * 10.0; world[3][0] = 100.0; world[3][1] = 100.0 + 10.0;\r
 243                  */\r
 244                 double[] para = wk_pickFromRaster_para; // double para[3][3];\r
 245                 // パターンの切り出しに失敗することもある。\r
 246                 if (!get_cpara(world, local, para)) {\r
 247                         return false;\r
 248                 }\r
 249                 lx1 = (int) ((local[0][0] - local[1][0]) * (local[0][0] - local[1][0]) + (local[0][1] - local[1][1])\r
 250                                 * (local[0][1] - local[1][1]));\r
 251                 lx2 = (int) ((local[2][0] - local[3][0]) * (local[2][0] - local[3][0]) + (local[2][1] - local[3][1])\r
 252                                 * (local[2][1] - local[3][1]));\r
 253                 ly1 = (int) ((local[1][0] - local[2][0]) * (local[1][0] - local[2][0]) + (local[1][1] - local[2][1])\r
 254                                 * (local[1][1] - local[2][1]));\r
 255                 ly2 = (int) ((local[3][0] - local[0][0]) * (local[3][0] - local[0][0]) + (local[3][1] - local[0][1])\r
 256                                 * (local[3][1] - local[0][1]));\r
 257                 if (lx2 > lx1) {\r
 258                         lx1 = lx2;\r
 259                 }\r
 260                 if (ly2 > ly1) {\r
 261                         ly1 = ly2;\r
 262                 }\r
 263                 xdiv2 = this.width;\r
 264                 ydiv2 = this.height;\r
 265 \r
 266                 while (xdiv2 * xdiv2 < lx1 / 4) {\r
 267                         xdiv2 *= 2;\r
 268                 }\r
 269                 while (ydiv2 * ydiv2 < ly1 / 4) {\r
 270                         ydiv2 *= 2;\r
 271                 }\r
 272 \r
 273                 if (xdiv2 > AR_PATT_SAMPLE_NUM) {\r
 274                         xdiv2 = AR_PATT_SAMPLE_NUM;\r
 275                 }\r
 276                 if (ydiv2 > AR_PATT_SAMPLE_NUM) {\r
 277                         ydiv2 = AR_PATT_SAMPLE_NUM;\r
 278                 }\r
 279 \r
 280                 xdiv = xdiv2 / width;// xdiv = xdiv2/Config.AR_PATT_SIZE_X;\r
 281                 ydiv = ydiv2 / height;// ydiv = ydiv2/Config.AR_PATT_SIZE_Y;\r
 282 \r
 283                 /* wk_pickFromRaster_ext_pat2ワーク変数を初期化する。 */\r
 284                 int[][][] ext_pat2 = wk_pickFromRaster_ext_pat2;// ARUint32 ext_pat2[AR_PATT_SIZE_Y][AR_PATT_SIZE_X][3];\r
 285                 int extpat_j[][], extpat_j_i[];\r
 286                 int ext_pat2_j[][], ext_pat2_j_i[];\r
 287 \r
 288                 initValue_wk_pickFromRaster_ext_pat2(ext_pat2, this.width, this.height);\r
 289 \r
 290                 xdiv2_reciprocal = 1.0 / xdiv2;\r
 291                 ydiv2_reciprocal = 1.0 / ydiv2;\r
 292                 int i, j;\r
 293                 int[] rgb_tmp = wk_pickFromRaster_rgb_tmp;\r
 294 \r
 295                 //ピクセルリーダーを取得\r
 296                 INyARRgbPixelReader reader=image.getRgbPixelReader();\r
 297                 \r
 298                 // arGetCode_put_zero(ext_pat2);//put_zero( (ARUint8 *)ext_pat2,\r
 299                 // AR_PATT_SIZE_Y*AR_PATT_SIZE_X*3*sizeof(ARUint32) );\r
 300                 for (j = 0; j < ydiv2; j++) {\r
 301                         yw = 102.5 + 5.0 * (j + 0.5) * ydiv2_reciprocal;\r
 302                         for (i = 0; i < xdiv2; i++) {\r
 303                                 xw = 102.5 + 5.0 * (i + 0.5) * xdiv2_reciprocal;\r
 304                                 d = para[2 * 3 + 0] * xw + para[2 * 3 + 1] * yw+ para[2 * 3 + 2];\r
 305                                 if (d == 0) {\r
 306                                         throw new NyARException();\r
 307                                 }\r
 308                                 xc = (int) ((para[0 * 3 + 0] * xw + para[0 * 3 + 1] * yw + para[0 * 3 + 2]) / d);\r
 309                                 yc = (int) ((para[1 * 3 + 0] * xw + para[1 * 3 + 1] * yw + para[1 * 3 + 2]) / d);\r
 310 \r
 311                                 if (xc >= 0 && xc < img_x && yc >= 0 && yc < img_y) {\r
 312                                         reader.getPixel(xc, yc, rgb_tmp);\r
 313                                         ext_pat2_j_i = ext_pat2[j / ydiv][i / xdiv];\r
 314 \r
 315                                         ext_pat2_j_i[0] += rgb_tmp[0];// R\r
 316                                         ext_pat2_j_i[1] += rgb_tmp[1];// G\r
 317                                         ext_pat2_j_i[2] += rgb_tmp[2];// B\r
 318                                         // System.out.println(xc+":"+yc+":"+rgb_tmp[0]+":"+rgb_tmp[1]+":"+rgb_tmp[2]);\r
 319                                 }\r
 320                         }\r
 321                 }\r
 322                 // short[][][] ext_pat=new short[Config.AR_PATT_SIZE_Y][Config.AR_PATT_SIZE_X][3];//ARUint32\r
 323                 // ext_pat2[AR_PATT_SIZE_Y][AR_PATT_SIZE_X][3];\r
 324                 /* <Optimize> */\r
 325                 int xdiv_x_ydiv = xdiv * ydiv;\r
 326                 for (j = this.height - 1; j >= 0; j--) {\r
 327                         extpat_j = extpat[j];\r
 328                         ext_pat2_j = ext_pat2[j];\r
 329                         for (i = this.width - 1; i >= 0; i--) { // PRL 2006-06-08.\r
 330                                 ext_pat2_j_i = ext_pat2_j[i];\r
 331                                 extpat_j_i = extpat_j[i];\r
 332                                 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
 333                                 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
 334                                 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
 335                         }\r
 336                 }\r
 337                 return true;\r
 338         }\r
 339 }