tags/2.5.0/test/jp/nyatla/nyartoolkit/dev/NyARColorPatt_DiagonalRatio.java

   1 /* \r
   2  * PROJECT: NyARToolkit\r
   3  * --------------------------------------------------------------------------------\r
   4  * The NyARToolkit is Java version ARToolkit class library.\r
   5  * Copyright (C)2008 R.Iizuka\r
   6  *\r
   7  * This program is free software; you can redistribute it and/or\r
   8  * modify it under the terms of the GNU General Public License\r
   9  * as published by the Free Software Foundation; either version 2\r
  10  * of the License, or (at your option) any later version.\r
  11  * \r
  12  * This program is distributed in the hope that it will be useful,\r
  13  * but WITHOUT ANY WARRANTY; without even the implied warranty of\r
  14  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the\r
  15  * GNU General Public License for more details.\r
  16  * \r
  17  * You should have received a copy of the GNU General Public License\r
  18  * along with this framework; if not, write to the Free Software\r
  19  * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA\r
  20  * \r
  21  * For further information please contact.\r
  22  *      http://nyatla.jp/nyatoolkit/\r
  23  *      <airmail(at)ebony.plala.or.jp>\r
  24  * \r
  25  */\r
  26 package jp.nyatla.nyartoolkit.dev;\r
  27 \r
  28 \r
  29 import jp.nyatla.nyartoolkit.NyARException;\r
  30 import jp.nyatla.nyartoolkit.core.raster.rgb.*;\r
  31 import jp.nyatla.nyartoolkit.core.rasterreader.*;\r
  32 import jp.nyatla.nyartoolkit.core.squaredetect.NyARSquare;\r
  33 import jp.nyatla.nyartoolkit.core.types.*;\r
  34 import jp.nyatla.nyartoolkit.core.types.matrix.*;\r
  35 import jp.nyatla.nyartoolkit.core.pickup.*;\r
  36 \r
  37 \r
  38 class NyARDoubleLine2d\r
  39 {\r
  40         double x,y;\r
  41         void set(NyARDoublePoint2d i_point_a,NyARDoublePoint2d i_point_b)\r
  42         {\r
  43                 this.x=i_point_a.x-i_point_b.x;\r
  44                 this.y=i_point_a.y-i_point_b.y;\r
  45                 return;\r
  46         }\r
  47         void add(NyARDoubleLine2d i_param,NyARDoubleLine2d o_result)\r
  48         {\r
  49                 o_result.x=this.x+i_param.x;\r
  50                 o_result.y=this.y+i_param.y;\r
  51                 return;\r
  52         }\r
  53         void sum(NyARDoubleLine2d i_param,NyARDoubleLine2d o_result)\r
  54         {\r
  55                 o_result.x=this.x-i_param.x;\r
  56                 o_result.y=this.y-i_param.y;\r
  57                 return;\r
  58         }\r
  59         /**\r
  60          * i_paramとの外積を計算する。\r
  61          * @param i_param\r
  62          * @return\r
  63          */\r
  64         double cross(NyARDoubleLine2d i_param)\r
  65         {\r
  66                 return this.x*i_param.y-this.y*i_param.x;\r
  67         }\r
  68         /**\r
  69          * i_paramとの内積を計算する\r
  70          * @param i_param\r
  71          * @return\r
  72          */\r
  73         double dot(NyARDoubleLine2d i_param)\r
  74         {\r
  75                 return this.x*i_param.x+this.y*i_param.y;\r
  76         }\r
  77         /**\r
  78          * このベクトルの絶対値を計算する\r
  79          * @param i_param\r
  80          * @return\r
  81          */\r
  82         double dist()\r
  83         {\r
  84                 return Math.sqrt(this.x*this.x+this.y*this.y);\r
  85         }\r
  86         \r
  87         \r
  88 }\r
  89 \r
  90 \r
  91 class LineParam\r
  92 {\r
  93         public double a;\r
  94         public double b;\r
  95         public static LineParam[] createArray(int i_length)\r
  96         {\r
  97                 LineParam[] result=new LineParam[i_length];\r
  98                 for(int i=result.length-1;i>=0;i--){\r
  99                         result[i]=new LineParam();\r
 100                 }\r
 101                 return result;\r
 102         }\r
 103 }\r
 104 \r
 105 class Complex\r
 106 {\r
 107         public double i;\r
 108         public double r;\r
 109         public Complex()\r
 110         {\r
 111         }\r
 112         public Complex(double i_r,double i_i)\r
 113         {\r
 114                 this.r=i_r;\r
 115                 this.i=i_i;\r
 116         }\r
 117         public void add(Complex i_v)\r
 118         {\r
 119                 this.r+=i_v.r;\r
 120                 this.i+=i_v.i;\r
 121         }\r
 122         public void sub(Complex i_v)\r
 123         {\r
 124                 this.r-=i_v.r;\r
 125                 this.i-=i_v.i;\r
 126         }\r
 127         public void sub(Complex i_v1,Complex i_v2)\r
 128         {\r
 129                 this.r=i_v1.r-i_v2.r;\r
 130                 this.i=i_v1.i-i_v2.i;\r
 131         }\r
 132         \r
 133         public void mul(Complex i_v)\r
 134         {\r
 135                 double r,i;\r
 136                 r=this.r;\r
 137                 i=this.i;\r
 138                 final double d2=Math.sqrt(r*r+i*i);\r
 139                 final double s2=Math.acos(r/d2);\r
 140                 r=i_v.r;\r
 141                 i=i_v.i;\r
 142                 final double d1=Math.sqrt(r*r+i*i);\r
 143                 final double s1=Math.acos(r/d1);\r
 144                 \r
 145                 this.r=d1*d2*Math.cos(s2+s1);\r
 146                 this.i=d1*d2*Math.sin(s2+s1);\r
 147                 return;\r
 148         }\r
 149         public void div(Complex i_v)\r
 150         {\r
 151                 double r,i;\r
 152                 r=this.r;\r
 153                 i=this.i;\r
 154                 final double d2=Math.sqrt(r*r+i*i);\r
 155                 final double s2=Math.acos(r/d2);\r
 156                 r=i_v.r;\r
 157                 i=i_v.i;\r
 158                 final double d1=Math.sqrt(r*r+i*i);\r
 159                 final double s1=Math.acos(r/d1);\r
 160                 \r
 161                 this.r=d2/d1*Math.cos(s2/s1);\r
 162                 this.i=d2/d1*Math.sin(s2/s1);\r
 163                 return;\r
 164         }\r
 165         public void pow(Complex i_v,double i_base)\r
 166         {\r
 167                 double r,i;\r
 168                 r=i_v.r;\r
 169                 i=i_v.i;\r
 170                 double d=Math.sqrt(r*r+i*i);\r
 171                 final double s=Math.acos(r/d)*i_base;\r
 172                 d=Math.pow(d,i_base);\r
 173                 this.r=d*Math.cos(s);\r
 174                 this.i=d*Math.sin(s);\r
 175         }\r
 176         public void sqrt(Complex i_v)\r
 177         {\r
 178                 double r,i;\r
 179                 r=i_v.r;\r
 180                 i=i_v.i;\r
 181                 double d=Math.sqrt(r*r+i*i);\r
 182                 final double s=Math.acos(r/d)*0.5;\r
 183                 d=Math.sqrt(d);\r
 184                 this.r=d*Math.cos(s);\r
 185                 this.i=d*Math.sin(s);\r
 186         }\r
 187         public double dist()\r
 188         {\r
 189                 return Math.sqrt(this.r*this.r+this.i*this.i);\r
 190         }\r
 191         \r
 192 }\r
 193 \r
 194 \r
 195 /**\r
 196  * 24ビットカラーのマーカーを保持するために使うクラスです。 このクラスは、ARToolkitのパターンと、ラスタから取得したパターンを保持します。\r
 197  * 演算順序を含む最適化をしたもの\r
 198  * \r
 199  */\r
 200 public class NyARColorPatt_DiagonalRatio implements INyARColorPatt\r
 201 {\r
 202         private int[] _patdata;\r
 203         private NyARBufferReader _buf_reader;\r
 204         private NyARRgbPixelReader_INT1D_X8R8G8B8_32 _pixelreader;\r
 205         private NyARIntSize _size;\r
 206                 \r
 207         public final int getWidth()\r
 208         {\r
 209                 return this._size.w;\r
 210         }\r
 211         \r
 212         public final int getHeight()\r
 213         {\r
 214                 return this._size.h;\r
 215         }\r
 216         \r
 217         public final NyARIntSize getSize()\r
 218         {\r
 219                 return  this._size;\r
 220         }\r
 221         \r
 222         public final INyARBufferReader getBufferReader()\r
 223         {\r
 224                 return this._buf_reader;\r
 225         }\r
 226         \r
 227         public final INyARRgbPixelReader getRgbPixelReader()\r
 228         {\r
 229                 return this._pixelreader;\r
 230         }\r
 231         NyARDoubleMatrix44 _invmat=new NyARDoubleMatrix44();\r
 232         /**\r
 233          * @param i_width\r
 234          * @param i_height\r
 235          */\r
 236         public NyARColorPatt_DiagonalRatio(int i_model)\r
 237         {\r
 238                 int resolution=(1<<i_model)+1;\r
 239                 this._size=new NyARIntSize(resolution,resolution);\r
 240                 this._patdata = new int[resolution*resolution];\r
 241                 this._buf_reader=new NyARBufferReader(this._patdata,NyARBufferReader.BUFFERFORMAT_INT1D_X8R8G8B8_32);\r
 242                 this._pixelreader=new NyARRgbPixelReader_INT1D_X8R8G8B8_32(this._patdata,this._size);\r
 243                 \r
 244                 this._vertex_map=NyARDoublePoint2d.create2dArray(this._size.h,this._size.w);\r
 245 \r
 246                 return;\r
 247         }       \r
 248         public NyARDoublePoint2d[][] _vertex_map;\r
 249         public boolean pickFromRaster(INyARRgbRaster image, NyARSquare i_square)throws NyARException\r
 250         {\r
 251                 NyARDoublePoint2d center=new NyARDoublePoint2d();\r
 252                 //中心を取得\r
 253                 solvCrossPoint(i_square.sqvertex[0],i_square.sqvertex[2],i_square.sqvertex[1],i_square.sqvertex[3],center);\r
 254                 \r
 255                 int[] rgb_tmp=new int[3];\r
 256                 INyARRgbPixelReader reader=image.getRgbPixelReader();\r
 257                 //頂点マトリクスの計算(2=2分割,3=4分割,4=8分割)\r
 258                 NyARDoublePoint2d[][] vertex_map=this._vertex_map;\r
 259                 solvLinePointArray(center,this._size.h-1,i_square.sqvertex,vertex_map);\r
 260                 for(int i=0;i<this._size.h;i++){\r
 261                         for(int i2=0;i2<this._size.w;i2++){\r
 262                                 if(vertex_map[i][i2].x>320||vertex_map[i][i2].y>240||vertex_map[i][i2].x<0||vertex_map[i][i2].y<0)\r
 263                                 {\r
 264                                         //System.out.println(vertex_map[i][i2].x+","+vertex_map[i][i2].y);\r
 265                                         this._patdata[i2+i*this._size.w]=0;\r
 266                                         continue;\r
 267                                 }\r
 268                                 reader.getPixel((int)vertex_map[i][i2].x,(int)vertex_map[i][i2].y,rgb_tmp);\r
 269                                 this._patdata[i2+i*this._size.w]=(rgb_tmp[0]<<16)|(rgb_tmp[1]<<8)|rgb_tmp[2];                           \r
 270                         }                       \r
 271                 }\r
 272                 return true;\r
 273         }\r
 274         /**\r
 275          * 直線をscaleで2^n分割した配列を計算する。\r
 276          * @param i_p1\r
 277          * @param i_p2\r
 278          * @param o_param\r
 279          * @return\r
 280          */\r
 281         private void solvLinePointArray4(NyARDoublePoint2d i_center,int i_div,NyARDoublePoint2d[] i_vertex,NyARDoublePoint2d[][] o_result)\r
 282         {\r
 283                 //分割直線の計算(2=2分割,3=4分割,4=8分割)\r
 284                 //中心コピー\r
 285                 o_result[0][0].setValue(i_center);\r
 286                 \r
 287                 \r
 288                 //[0]->[1]のベクトルを計算\r
 289                 NyARDoublePoint2d vec01=new NyARDoublePoint2d();\r
 290                 NyARDoublePoint2d vec12=new NyARDoublePoint2d();\r
 291                 NyARDoublePoint2d vec03=new NyARDoublePoint2d();\r
 292                 NyARDoublePoint2d vec32=new NyARDoublePoint2d();\r
 293                 vec01.vecSub(i_vertex[1],i_vertex[0]);\r
 294                 vec12.vecSub(i_vertex[2],i_vertex[1]);\r
 295                 vec03.vecSub(i_vertex[3],i_vertex[0]);\r
 296                 vec32.vecSub(i_vertex[2],i_vertex[3]);\r
 297 \r
 298                 //中心点から[0]->[1]と平行なベクトルの終点を計算\r
 299                 NyARDoublePoint2d vec01_ep=new NyARDoublePoint2d();\r
 300                 vec01_ep.vecAdd(vec01,i_center);\r
 301                 //中心点から[3]->[2]と平行なベクトルの終点を計算\r
 302                 NyARDoublePoint2d vec32_ep=new NyARDoublePoint2d();\r
 303                 vec32_ep.vecAdd(vec32,i_center);                \r
 304                 //平均値\r
 305                 NyARDoublePoint2d cx_e=new NyARDoublePoint2d();\r
 306                 cx_e.x=(vec01_ep.x+vec32_ep.x)/2;\r
 307                 cx_e.y=(vec01_ep.y+vec32_ep.y)/2;\r
 308                 \r
 309                 //ベクトル[1]->[2]との交差点を計算\r
 310                 solvCrossPoint(i_center,cx_e,i_vertex[1],i_vertex[2],o_result[1][2]);\r
 311                 //ベクトル[3]->[0]との交差点を計算\r
 312                 solvCrossPoint(i_center,cx_e,i_vertex[3],i_vertex[0],o_result[1][0]);\r
 313                 \r
 314 \r
 315                 \r
 316                 //中心点から[1]->[2]と平行なベクトルの終点を計算\r
 317                 NyARDoublePoint2d vec12_ep=new NyARDoublePoint2d();\r
 318                 vec12_ep.vecAdd(vec12,i_center);\r
 319                 //中心点から[0]->[3]と平行なベクトルの終点を計算\r
 320                 NyARDoublePoint2d vec03_ep=new NyARDoublePoint2d();\r
 321                 vec03_ep.vecAdd(vec03,i_center);                \r
 322                 //平均値\r
 323                 NyARDoublePoint2d cx_e2=new NyARDoublePoint2d();\r
 324                 cx_e2.x=(vec12_ep.x+vec03_ep.x)/2;\r
 325                 cx_e2.y=(vec12_ep.y+vec03_ep.y)/2;\r
 326                 \r
 327                 //cx_e2とベクトル[0]->[1]との交差点を計算\r
 328                 solvCrossPoint(i_center,cx_e2,i_vertex[0],i_vertex[1],o_result[0][1]);\r
 329                 //ベクトル[3]->[2]との交差点を計算\r
 330                 solvCrossPoint(i_center,cx_e2,i_vertex[3],i_vertex[2],o_result[2][1]);\r
 331                 \r
 332                 \r
 333                 \r
 334                 \r
 335                 return;\r
 336         }\r
 337         private void solvLinePointArray(NyARDoublePoint2d i_center,int i_div,NyARDoublePoint2d[] i_vertex,NyARDoublePoint2d[][] o_result)\r
 338         {\r
 339                 //中心コピー\r
 340                 o_result[0][0].setValue(i_center);\r
 341 \r
 342                 //[0]+[1]+[c]\r
 343                 NyARDoublePoint2d vt=new NyARDoublePoint2d();\r
 344                 vt.x=(i_vertex[0].x+i_vertex[1].x)/2;\r
 345                 vt.y=(i_vertex[0].y+i_vertex[1].y)/2;\r
 346                 //[2]+[3]+[c]\r
 347                 NyARDoublePoint2d vb=new NyARDoublePoint2d();\r
 348                 vb.x=(i_vertex[2].x+i_vertex[3].x)/2;\r
 349                 vb.y=(i_vertex[2].y+i_vertex[3].y)/2;\r
 350                 \r
 351                 vt.vecSub(vb);\r
 352                 vt.vecAdd(i_center);\r
 353 \r
 354                 //[0][1]->[2][3]ベクトル\r
 355                 solvCrossPoint(vt,i_center,i_vertex[0],i_vertex[1],o_result[1][2]);\r
 356                 //[0][1]->[2][3]ベクトル:v[3][0]\r
 357                 solvCrossPoint(vt,i_center,i_vertex[3],i_vertex[2],o_result[1][0]);\r
 358 \r
 359                 \r
 360                 \r
 361 /*              \r
 362                 \r
 363                 //[0]->[1]のベクトルを計算\r
 364                 NyARDoublePoint2d vec01=new NyARDoublePoint2d();\r
 365                 NyARDoublePoint2d vec12=new NyARDoublePoint2d();\r
 366                 NyARDoublePoint2d vec03=new NyARDoublePoint2d();\r
 367                 NyARDoublePoint2d vec32=new NyARDoublePoint2d();\r
 368                 vec01.vecSub(i_vertex[1],i_vertex[0]);\r
 369                 vec12.vecSub(i_vertex[2],i_vertex[1]);\r
 370                 vec03.vecSub(i_vertex[3],i_vertex[0]);\r
 371                 vec32.vecSub(i_vertex[2],i_vertex[3]);\r
 372 \r
 373                 //中心点から[0]->[1]と平行なベクトルの終点を計算\r
 374                 NyARDoublePoint2d vec01_ep=new NyARDoublePoint2d();\r
 375                 vec01_ep.vecSum(vec01,i_center);\r
 376                 //中心点から[3]->[2]と平行なベクトルの終点を計算\r
 377                 NyARDoublePoint2d vec32_ep=new NyARDoublePoint2d();\r
 378                 vec32_ep.vecSum(vec32,i_center);                \r
 379                 //平均値\r
 380                 NyARDoublePoint2d cx_e=new NyARDoublePoint2d();\r
 381                 cx_e.x=(vec01_ep.x+vec32_ep.x)/2;\r
 382                 cx_e.y=(vec01_ep.y+vec32_ep.y)/2;\r
 383                 \r
 384                 //ベクトル[1]->[2]との交差点を計算\r
 385                 solvCrossPoint(i_center,cx_e,i_vertex[1],i_vertex[2],o_result[1][2]);\r
 386                 //ベクトル[3]->[0]との交差点を計算\r
 387                 solvCrossPoint(i_center,cx_e,i_vertex[3],i_vertex[0],o_result[1][0]);\r
 388                 \r
 389 \r
 390                 \r
 391                 //中心点から[1]->[2]と平行なベクトルの終点を計算\r
 392                 NyARDoublePoint2d vec12_ep=new NyARDoublePoint2d();\r
 393                 vec12_ep.vecSum(vec12,i_center);\r
 394                 //中心点から[0]->[3]と平行なベクトルの終点を計算\r
 395                 NyARDoublePoint2d vec03_ep=new NyARDoublePoint2d();\r
 396                 vec03_ep.vecSum(vec03,i_center);                \r
 397                 //平均値\r
 398                 NyARDoublePoint2d cx_e2=new NyARDoublePoint2d();\r
 399                 cx_e2.x=(vec12_ep.x+vec03_ep.x)/2;\r
 400                 cx_e2.y=(vec12_ep.y+vec03_ep.y)/2;\r
 401                 \r
 402                 //cx_e2とベクトル[0]->[1]との交差点を計算\r
 403                 solvCrossPoint(i_center,cx_e2,i_vertex[0],i_vertex[1],o_result[0][1]);\r
 404                 //ベクトル[3]->[2]との交差点を計算\r
 405                 solvCrossPoint(i_center,cx_e2,i_vertex[3],i_vertex[2],o_result[2][1]);\r
 406                 \r
 407                 \r
 408 */              \r
 409                 \r
 410                 return;\r
 411         }\r
 412         \r
 413         \r
 414         \r
 415         private void solvLinePointArray3(NyARDoublePoint2d i_canter,int i_div,NyARDoublePoint2d[] i_vertex,NyARDoublePoint2d[][] o_result)\r
 416         {\r
 417                 NyARDoublePoint2d scale=new NyARDoublePoint2d();\r
 418                 //分割直線の計算(2=2分割,3=4分割,4=8分割)\r
 419                 int d=i_div;\r
 420                 NyARDoublePoint2d[] r=o_result[d/2];\r
 421                 //対角線Aを計算\r
 422                 r[0].x=i_vertex[0].x;\r
 423                 r[0].y=i_vertex[0].y;\r
 424                 r[d].x=i_vertex[2].x;\r
 425                 r[d].y=i_vertex[2].y;\r
 426                 scale.x=(i_canter.x-i_vertex[0].x)/(i_vertex[2].x-i_vertex[0].x);\r
 427                 scale.y=(i_canter.y-i_vertex[0].y)/(i_vertex[2].y-i_vertex[0].y);\r
 428 \r
 429                 solvLinePointArray_b(scale,0,d,r);\r
 430                 //対角線上にコピー\r
 431                 for(int i=0;i<=d;i++){\r
 432                         o_result[i][i].x=r[i].x;        \r
 433                         o_result[i][i].y=r[i].y;\r
 434                 }\r
 435                 //対角線Bを計算\r
 436                 r[0].x=i_vertex[3].x;\r
 437                 r[0].y=i_vertex[3].y;\r
 438                 r[d].x=i_vertex[1].x;\r
 439                 r[d].y=i_vertex[1].y;\r
 440                 scale.x=(i_canter.x-i_vertex[3].x)/(i_vertex[1].x-i_vertex[3].x);\r
 441                 scale.y=(i_canter.y-i_vertex[3].y)/(i_vertex[1].y-i_vertex[3].y);\r
 442                 solvLinePointArray_b(scale,0,d,r);\r
 443                 //対角線上にコピー\r
 444                 for(int i=0;i<=d;i++){\r
 445                         o_result[d-i][i].x=r[i].x;\r
 446                         o_result[d-i][i].y=r[i].y;\r
 447                 }\r
 448                 //マップ作成\r
 449                 for(int i=0;i<=d;i++){\r
 450                         final NyARDoublePoint2d y1=o_result[i][i];\r
 451                         final NyARDoublePoint2d y2=o_result[d-i][i];\r
 452                         if(i==d/2){\r
 453                                 continue;\r
 454                         }\r
 455                         for(int i2=0;i2<=d;i2++){\r
 456                                 if(i==i2){\r
 457                                         continue;\r
 458                                 }\r
 459                                 if(i==d-i2){\r
 460                                         continue;\r
 461                                 }\r
 462                                 if(i2==d/2){\r
 463                                         continue;\r
 464                                 }\r
 465                                 final NyARDoublePoint2d x1=o_result[i2][i2];\r
 466                                 final NyARDoublePoint2d x2=o_result[i2][d-i2];\r
 467                                 solvCrossPoint(y1,y2,x1,x2,o_result[i2][i]);\r
 468                         }\r
 469                 }\r
 470 \r
 471                 return;\r
 472         }       \r
 473         /**\r
 474          * 直線をscaleで2^n分割した配列を計算する。\r
 475          * @param i_p1\r
 476          * @param i_p2\r
 477          * @param o_param\r
 478          * @return\r
 479          */\r
 480         private void solvLinePointArray2(NyARDoublePoint2d i_canter,int i_div,NyARDoublePoint2d[] i_vertex,NyARDoublePoint2d[][] o_result)\r
 481         {\r
 482                 NyARDoublePoint2d scale=new NyARDoublePoint2d();\r
 483                 //分割直線の計算(2=2分割,3=4分割,4=8分割)\r
 484                 int d=i_div;\r
 485                 NyARDoublePoint2d[] r=o_result[d/2];\r
 486                 //対角線Aを計算\r
 487                 r[0].x=i_vertex[0].x;\r
 488                 r[0].y=i_vertex[0].y;\r
 489                 r[d].x=i_vertex[2].x;\r
 490                 r[d].y=i_vertex[2].y;\r
 491 //              scale.x=(i_canter.x-i_vertex[0].x)/(i_vertex[2].x-i_vertex[0].x);\r
 492 //              scale.y=(i_canter.y-i_vertex[0].y)/(i_vertex[2].y-i_vertex[0].y);\r
 493                 double sx,kx,lx,sy,ky,ly;\r
 494 \r
 495                 sx=i_vertex[0].x;\r
 496                 kx=solvK(i_canter.x-sx,i_vertex[2].x-sx);\r
 497                 lx=solvL(kx,i_canter.x-sx);\r
 498 \r
 499                 sy=i_vertex[0].y;\r
 500                 ky=solvK(i_canter.y-sy,i_vertex[2].y-sy);\r
 501                 ly=solvL(kx,i_canter.y-sy);\r
 502                 \r
 503                 solvLinePointArray_b(scale,0,d,r);\r
 504                 //対角線上にコピー\r
 505                 for(int i=0;i<=d;i++){\r
 506                         o_result[i][i].x=sx+kx*lx;\r
 507                         o_result[i][i].y=sy+ky*ly;\r
 508                         kx*=kx;\r
 509                         ky*=ky;\r
 510                 }\r
 511                 \r
 512                 sx=i_vertex[3].x;\r
 513                 kx=solvK(i_canter.x-sx,i_vertex[1].x-sx);\r
 514                 lx=solvL(kx,i_canter.x-sx);\r
 515 \r
 516                 sy=i_vertex[3].y;\r
 517                 ky=solvK(i_canter.y-sy,i_vertex[1].y-sy);\r
 518                 ly=solvL(kx,i_canter.y-sy);\r
 519                 \r
 520                 solvLinePointArray_b(scale,0,d,r);\r
 521                 //対角線上にコピー\r
 522                 for(int i=0;i<=d;i++){\r
 523                         o_result[d-i][i].x=sx+kx*lx;\r
 524                         o_result[d-i][i].y=sy+ky*ly;\r
 525                         kx*=kx;\r
 526                         ky*=ky;\r
 527                 }\r
 528                 //マップ作成\r
 529                 for(int i=0;i<=d;i++){\r
 530                         final NyARDoublePoint2d y1=o_result[i][i];\r
 531                         final NyARDoublePoint2d y2=o_result[d-i][i];\r
 532                         if(i==d/2){\r
 533                                 continue;\r
 534                         }\r
 535                         for(int i2=0;i2<=d;i2++){\r
 536                                 if(i==i2){\r
 537                                         continue;\r
 538                                 }\r
 539                                 if(i==d-i2){\r
 540                                         continue;\r
 541                                 }\r
 542                                 if(i2==d/2){\r
 543                                         continue;\r
 544                                 }\r
 545                                 final NyARDoublePoint2d x1=o_result[i2][i2];\r
 546                                 final NyARDoublePoint2d x2=o_result[i2][d-i2];\r
 547                                 solvCrossPoint(y1,y2,x1,x2,o_result[i2][i]);\r
 548                         }\r
 549                 }\r
 550 \r
 551                 return;\r
 552         }       \r
 553 \r
 554         private void solvLinePointArray_b(NyARDoublePoint2d i_scale,int i_si,int i_ei,NyARDoublePoint2d[] o_result)\r
 555         {\r
 556                 int ci=(i_ei-i_si)/2+i_si;\r
 557                 o_result[ci].x=i_scale.x*(o_result[i_ei].x-o_result[i_si].x)+o_result[i_si].x;\r
 558                 o_result[ci].y=i_scale.y*(o_result[i_ei].y-o_result[i_si].y)+o_result[i_si].y;\r
 559                 \r
 560                 if(ci-i_si==1){\r
 561                         return;\r
 562                 }\r
 563                 solvLinePointArray_b(i_scale,i_si,ci,o_result);\r
 564                 solvLinePointArray_b(i_scale,ci,i_ei,o_result);\r
 565                 return;\r
 566         }\r
 567         \r
 568         private void solvCrossPoint(NyARIntPoint2d i_p1,NyARIntPoint2d i_p2,NyARIntPoint2d i_p3,NyARIntPoint2d i_p4,NyARDoublePoint2d o_result)\r
 569         {\r
 570                 NyARDoublePoint2d va=new NyARDoublePoint2d(i_p2);\r
 571                 NyARDoublePoint2d vb=new NyARDoublePoint2d(i_p4);\r
 572                 va.vecSub(i_p1);\r
 573                 vb.vecSub(i_p3);\r
 574                 o_result.setValue(i_p3);\r
 575                 o_result.vecSub(i_p1);\r
 576                 va.vecMul(va, vb.vecCross(o_result)/vb.vecCross(va));\r
 577                 o_result.setValue(va);\r
 578                 o_result.vecAdd(i_p1);\r
 579                 return;\r
 580                 //V a=p2-p1;\r
 581                 //V b=p4-p3;\r
 582                 //return a1 + a * cross(b, b1-a1) / cross(b, a);\r
 583         }\r
 584         private void solvCrossPoint(NyARDoublePoint2d i_p1,NyARDoublePoint2d i_p2,NyARDoublePoint2d i_p3,NyARDoublePoint2d i_p4,NyARDoublePoint2d o_result)\r
 585         {\r
 586                 NyARDoublePoint2d va=new NyARDoublePoint2d(i_p2);\r
 587                 NyARDoublePoint2d vb=new NyARDoublePoint2d(i_p4);\r
 588                 va.vecSub(i_p1);\r
 589                 vb.vecSub(i_p3);\r
 590                 o_result.setValue(i_p3);\r
 591                 o_result.vecSub(i_p1);\r
 592                 va.vecMul(va, vb.vecCross(o_result)/vb.vecCross(va));\r
 593                 o_result.setValue(va);\r
 594                 o_result.vecAdd(i_p1);\r
 595                 return;\r
 596                 //V a=p2-p1;\r
 597                 //V b=p4-p3;\r
 598                 //return a1 + a * cross(b, b1-a1) / cross(b, a);\r
 599         }       \r
 600         double pow_1_3(double a)\r
 601         {\r
 602                 double x = Math.pow(a, 1./3);\r
 603                 return (2*x+a/x/x)/3; // modifier\r
 604         }\r
 605         /*\r
 606          * \r
 607          * \r
 608          * */\r
 609         //(Sqrt(((3*Z*((-Z)/(3*Z))^2-(2*Z^2)/(3*Z)+Z-1)/Z)^3/27+(Z*((-Z)/(3*Z))^2+Z*((-Z)/(3*Z))^3-((Z-1)*Z)/(3*Z)+Z-1)^2/(4*Z^2))-(Z*((-Z)/(3*Z))^2+Z*((-Z)/(3*Z))^3-((Z-1)*Z)/(3*Z)+Z-1)/(2*Z))^(1/3)+(-((Z*((-Z)/(3*Z))^2+Z*((-Z)/(3*Z))^3-((Z-1)*Z)/(3*Z)+Z-1)/(2*Z)+Sqrt(((3*Z*((-Z)/(3*Z))^2-(2*Z^2)/(3*Z)+Z-1)/Z)^3/27+(Z*((-Z)/(3*Z))^2+Z*((-Z)/(3*Z))^3-((Z-1)*Z)/(3*Z)+Z-1)^2/(4*Z^2))))^(1/3)-Z/(3*Z)\r
 610         private double solvK(double mp,double vp)\r
 611         {\r
 612                 double Z=mp/vp;\r
 613                 double a=(Z-1);\r
 614                 double b=(3.0*Z);\r
 615                 double c=(a*Z);\r
 616                 double d=(2.0*Z*Z);//(2*Z^2)\r
 617                 double e=(4.0*Z*Z);//(4*Z^2)\r
 618                 double f=((-Z)/b);\r
 619                 double g=(Z*f*f+Z*f*f*f-c/b+Z-1);//(Z*f^2+Z*f^3-c/b+Z-1)\r
 620                 double h=(3.0*Z*f*f-d/b+Z-1.0);//(3*Z*f^2-d/b+Z-1)\r
 621                 double i=(h/Z);\r
 622                 Complex j=new Complex(0,i*i*i/27.0+g*g/e);//(0,i*i*i/27.0+g*g/e);\r
 623 //              j.r=0;\r
 624 //              j.i=Math.sqrt(-i*i*i/27.0+g*g/e);\r
 625 //              j.sqrt(j);\r
 626                 Complex k=new Complex();\r
 627                 k.r=j.r-g/(2.0*Z);\r
 628                 k.i=j.i;\r
 629                 Complex l=new Complex();\r
 630                 l.r=-(g/(2.0*Z)+j.r);\r
 631                 l.i=-j.i;\r
 632                 \r
 633                 k.pow(k,1.0/3);\r
 634                 l.pow(l,1.0/3);\r
 635                 double fi=k.dist()+l.dist()-Z/b;\r
 636                 return fi;\r
 637         }\r
 638         private double solvL(double k,double mp)\r
 639         {\r
 640                 return 100/(1+k*k+k*k*k+k);\r
 641         }\r
 642         \r
 643         \r
 644         public static void main(String[] args)\r
 645         {\r
 646 \r
 647                 try {\r
 648                         NyARColorPatt_DiagonalRatio t = new NyARColorPatt_DiagonalRatio(3);\r
 649                         double k=t.solvK(60,100);\r
 650                         double l=t.solvL(k,60);\r
 651                         \r
 652                         // t.Test_arGetVersion();\r
 653                         NyARSquare s=new NyARSquare();\r
 654                         s.sqvertex[0].x=10;\r
 655                         s.sqvertex[0].y=10;\r
 656                         s.sqvertex[1].x=90;\r
 657                         s.sqvertex[1].y=0;\r
 658                         s.sqvertex[2].x=100;\r
 659                         s.sqvertex[2].y=100;\r
 660                         s.sqvertex[3].x=0;\r
 661                         s.sqvertex[3].y=100;\r
 662                         //t.getLineCrossPoint(s);\r
 663                 } catch (Exception e) {\r
 664                         e.printStackTrace();\r
 665                 }\r
 666         }\r
 667 }