lib/src/jp/nyatla/nyartoolkit/core/types/NyARLinear.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.types;\r
  32 \r
  33 \r
  34 /**\r
  35  * このクラスは、0=a*x+b*y+cのパラメータを格納します。\r
  36  * x,yの増加方向は、x=L→R,y=B→Tです。 y軸が反転しているので注意してください。\r
  37  *\r
  38  */\r
  39 public class NyARLinear\r
  40 {\r
  41         /** 直線式の係数 b*/\r
  42         public double b;\r
  43         /** 直線式の係数 a*/\r
  44         public double a;\r
  45         /** 直線式の係数 c*/\r
  46         public double c;\r
  47         \r
  48         /**\r
  49          * この関数は、指定サイズのオブジェクト配列を作ります。\r
  50          * @param i_number\r
  51          * 作成する配列の長さ\r
  52          * @return\r
  53          * 新しい配列。\r
  54          */     \r
  55         public static NyARLinear[] createArray(int i_number)\r
  56         {\r
  57                 NyARLinear[] ret=new NyARLinear[i_number];\r
  58                 for(int i=0;i<i_number;i++)\r
  59                 {\r
  60                         ret[i]=new NyARLinear();\r
  61                 }\r
  62                 return ret;\r
  63         }\r
  64         /**\r
  65          * この関数は、引数値からパラメータをインスタンスへコピーします。\r
  66          * @param i_source\r
  67          * コピー元のオブジェクト\r
  68          */\r
  69         public final void copyFrom(NyARLinear i_source)\r
  70         {\r
  71                 this.b=i_source.b;\r
  72                 this.a=i_source.a;\r
  73                 this.c=i_source.c;\r
  74                 return;\r
  75         }\r
  76         /**\r
  77          * この関数は、直線の交点を計算します。\r
  78          * @param l_line_2\r
  79          * 交点を計算する直線式\r
  80          * @param o_point\r
  81          * 交点座標を格納するオブジェクト\r
  82          * @return\r
  83          * 交点が求まればtrue\r
  84          */\r
  85         public final boolean crossPos(NyARLinear l_line_2,NyARDoublePoint2d o_point)\r
  86         {\r
  87                 final double w1 = this.a * l_line_2.b - l_line_2.a * this.b;\r
  88                 if (w1 == 0.0) {\r
  89                         return false;\r
  90                 }\r
  91                 o_point.x = (this.b * l_line_2.c - l_line_2.b * this.c) / w1;\r
  92                 o_point.y = (l_line_2.a * this.c - this.a * l_line_2.c) / w1;\r
  93                 return true;\r
  94         }\r
  95         /**\r
  96          * この関数は、直線の交点を計算します。\r
  97          * @param i_a\r
  98          * 交点を求める直線式の係数a\r
  99          * @param i_b\r
 100          * 交点を求める直線式の係数b\r
 101          * @param i_c\r
 102          * 交点を求める直線式の係数c\r
 103          * @param o_point\r
 104          * 交点座標を格納するオブジェクト\r
 105          * @return\r
 106          * 交点が求まればtrue\r
 107          */\r
 108         public final boolean crossPos(double i_a,double i_b,double i_c,NyARDoublePoint2d o_point)\r
 109         {\r
 110                 final double w1 = this.a * i_b - i_a * this.b;\r
 111                 if (w1 == 0.0) {\r
 112                         return false;\r
 113                 }\r
 114                 o_point.x = (this.b * i_c - i_b * this.c) / w1;\r
 115                 o_point.y = (i_a * this.c - this.a * i_c) / w1;\r
 116                 return true;\r
 117         }\r
 118         /**\r
 119          * この関数は、直線の交点を計算します。\r
 120          * @param i_a\r
 121          * 交点を求める直線式の係数a\r
 122          * @param i_b\r
 123          * 交点を求める直線式の係数b\r
 124          * @param i_c\r
 125          * 交点を求める直線式の係数c\r
 126          * @param o_point\r
 127          * 交点座標を格納するオブジェクト\r
 128          * @return\r
 129          * 交点が求まればtrue\r
 130          */\r
 131         public final boolean crossPos(double i_a,double i_b,double i_c,NyARIntPoint2d o_point)\r
 132         {\r
 133                 final double w1 = this.a * i_b - i_a * this.b;\r
 134                 if (w1 == 0.0) {\r
 135                         return false;\r
 136                 }\r
 137                 o_point.x = (int)((this.b * i_c - i_b * this.c) / w1);\r
 138                 o_point.y = (int)((i_a * this.c - this.a * i_c) / w1);\r
 139                 return true;\r
 140         }\r
 141         /**\r
 142          * この関数は、2直線が交差しているかを返します。\r
 143          * @param l_line_2\r
 144          * 交差しているか確認するオブジェクト\r
 145          * @return\r
 146          * 交差していればtrue\r
 147          */\r
 148         public final boolean isCross(NyARLinear l_line_2)\r
 149         {\r
 150                 final double w1 = this.a * l_line_2.b - l_line_2.a * this.b;\r
 151                 return (w1 == 0.0)?false:true;\r
 152         }\r
 153         \r
 154         /**\r
 155          * この関数は、2点を結ぶ直線式を計算して、インスタンスに格納します。\r
 156          * 式の係数値は、正規化されます。\r
 157          * @param i_point1\r
 158          * 点１\r
 159          * @param i_point2\r
 160          * 点２\r
 161          * @return\r
 162          * 直線式が求まれば、true\r
 163          */\r
 164         public final boolean makeLinearWithNormalize(NyARIntPoint2d i_point1,NyARIntPoint2d i_point2)\r
 165         {\r
 166                 return makeLinearWithNormalize(i_point1.x,i_point1.y,i_point2.x,i_point2.y);\r
 167         }\r
 168         /**\r
 169          * この関数は、2点を結ぶ直線式を計算して、インスタンスに格納します。\r
 170          * 式の係数値は、正規化されます。\r
 171          * @param i_point1\r
 172          * 点１\r
 173          * @param i_point2\r
 174          * 点２\r
 175          * @return\r
 176          * 直線式が求るとtrueを返します。\r
 177          */\r
 178         public final boolean makeLinearWithNormalize(NyARDoublePoint2d i_point1,NyARDoublePoint2d i_point2)\r
 179         {\r
 180                 return makeLinearWithNormalize(i_point1.x,i_point1.y,i_point2.x,i_point2.y);\r
 181         }\r
 182         /**\r
 183          * この関数は、2点を結ぶ直線式を計算して、インスタンスに格納します。\r
 184          * 式の係数値は、正規化されます。\r
 185          * @param x1\r
 186          * 点1の座標(X)\r
 187          * @param y1\r
 188          * 点1の座標(Y)\r
 189          * @param x2\r
 190          * 点2の座標(X)\r
 191          * @param y2\r
 192          * 点2の座標(Y)\r
 193          * @return\r
 194          * 直線式が求るとtrueを返します。\r
 195          */\r
 196         public final boolean makeLinearWithNormalize(double x1,double y1,double x2,double y2)\r
 197         {\r
 198                 double dx=y2-y1;\r
 199                 double dy=x1-x2;\r
 200                 double sq=Math.sqrt(dx*dx+dy*dy);\r
 201                 if(sq==0){\r
 202                         return false;\r
 203                 }\r
 204                 sq=1/sq;\r
 205                 this.a=dx*sq;\r
 206                 this.b=dy*sq;\r
 207                 this.c=(x1*(y1-y2)+y1*(x2-x1))*sq;\r
 208                 return true;\r
 209         }\r
 210         /**\r
 211          * この関数は、傾きと通過点から直線式を計算して、インスタンスへセットします。\r
 212          * @param i_dx\r
 213          * Xの傾き\r
 214          * @param i_dy\r
 215          * Yの傾き\r
 216          * @param i_x\r
 217          * 通過点の座標X\r
 218          * @param i_y\r
 219          * 通過点の座標Y\r
 220          */\r
 221         public final void setVector(double i_dx,double i_dy,double i_x,double i_y)\r
 222         {\r
 223                 this.a= i_dy;\r
 224                 this.b=-i_dx;\r
 225                 this.c=(i_dx*i_y-i_dy*i_x);\r
 226                 return;\r
 227         }\r
 228         /**\r
 229          * この関数は、{@link NyARVecLinear2d}を直線式に変換して、インスタンスへセットします。\r
 230          * @param i_vector\r
 231          * セットするオブジェクト\r
 232          */\r
 233         public final void setVector(NyARVecLinear2d i_vector)\r
 234         {\r
 235                 this.a= i_vector.dy;\r
 236                 this.b=-i_vector.dx;\r
 237                 this.c=(i_vector.dx*i_vector.y-i_vector.dy*i_vector.x);\r
 238                 return;         \r
 239         }\r
 240         /**\r
 241          * この関数は、{@link NyARVecLinear2d}を正規化された直線式に変換して、インスタンスへセットします。\r
 242          * @param i_vector\r
 243          * セットするオブジェクト\r
 244          */\r
 245         public final boolean setVectorWithNormalize(NyARVecLinear2d i_vector)\r
 246         {\r
 247                 double dx=i_vector.dx;\r
 248                 double dy=i_vector.dy;\r
 249                 double sq=Math.sqrt(dx*dx+dy*dy);\r
 250                 if(sq==0){\r
 251                         return false;\r
 252                 }\r
 253                 sq=1/sq;\r
 254                 this.a= dy*sq;\r
 255                 this.b=-dx*sq;\r
 256                 this.c=-(this.a*i_vector.x+this.b*i_vector.y);          \r
 257                 return true;\r
 258         }\r
 259         /**\r
 260          * この関数は、i_x,i_yを通過する、i_linearの法線を計算して、インスタンスへ格納します。\r
 261          * @param i_x\r
 262          * 通過点X\r
 263          * @param i_y\r
 264          * 通過点Y\r
 265          * @param i_linear\r
 266          * 法線を計算する直線式(この引数にはthisを指定できます。)\r
 267          */\r
 268         public final void normalLine(double i_x,double i_y,NyARLinear i_linear)\r
 269         {\r
 270                 double la=i_linear.a;\r
 271                 double lb=i_linear.b;\r
 272                 this.a=lb;\r
 273                 this.b=-la;\r
 274                 this.c=-(lb*i_x-la*i_y);\r
 275         }\r
 276         /**\r
 277          * この関数は、i_x,i_yを通るこの直線の法線と、i_linearが交わる点を返します。\r
 278          * @param i_x\r
 279          * 法線が通過する点X\r
 280          * @param i_y\r
 281          * 法線が通過する点Y\r
 282          * @param i_linear\r
 283          * 交点を計算する直線式\r
 284          * @param o_point\r
 285          * 交点を返却するオブジェクト\r
 286          * @return\r
 287          * 交点が求まれば、trueを返します。\r
 288          */\r
 289         public final boolean normalLineCrossPos(double i_x,double i_y,NyARLinear i_linear,NyARDoublePoint2d o_point)\r
 290         {\r
 291                 //thisを法線に変換\r
 292                 double la=this.b;\r
 293                 double lb=-this.a;\r
 294                 double lc=-(la*i_x+lb*i_y);\r
 295                 //交点を計算\r
 296                 final double w1 = i_linear.a * lb - la * i_linear.b;\r
 297                 if (w1 == 0.0) {\r
 298                         return false;\r
 299                 }\r
 300                 o_point.x = ((i_linear.b * lc - lb * i_linear.c) / w1);\r
 301                 o_point.y = ((la * i_linear.c - i_linear.a * lc) / w1);\r
 302                 return true;\r
 303         }\r
 304 //      /**\r
 305 //       * i_x,i_yを通るこの直線の法線上での、この直線とi_linearの距離の二乗値を返します。\r
 306 //       * i_x,i_yに直線上の点を指定すると、この直線の垂線上での、もう一方の直線との距離の二乗値が得られます。\r
 307 //       * @param i_linear\r
 308 //       * @param i_x\r
 309 //       * @param i_y\r
 310 //       * @param o_point\r
 311 //       * @return\r
 312 //       * 交点が無い場合、無限大を返します。\r
 313 //       */\r
 314 //      public final double sqDistWithLinear(NyARLinear i_linear, double i_x,double i_y)\r
 315 //      {\r
 316 //              //thisを法線に変換\r
 317 //              double la=this.b;\r
 318 //              double lb=-this.a;\r
 319 //              double lc=-(la*i_x+lb*i_y);\r
 320 //              //交点を計算\r
 321 //              final double w1 = i_linear.a * lb - la * i_linear.b;\r
 322 //              if (w1 == 0.0) {\r
 323 //                      return Double.POSITIVE_INFINITY;\r
 324 //              }\r
 325 //              double x=i_x-((i_linear.b * lc - lb * i_linear.c) / w1);\r
 326 //              double y=i_y-((la * i_linear.c - i_linear.a * lc) / w1);\r
 327 //              return x*x+y*y;\r
 328 //      }\r
 329 \r
 330         /**\r
 331          * この関数は、直線を0,0基点(左上)の矩形でクリッピングしたときの、端点を計算します。\r
 332          * @param i_width\r
 333          * 矩形の幅\r
 334          * @param i_height\r
 335          * 矩形の高さ\r
 336          * @param o_point\r
 337          * 端点を返すオブジェクト配列。2要素である必要があります。\r
 338          * @return\r
 339          * 端点が求まればtrue\r
 340          */\r
 341         public final boolean makeSegmentLine(int i_width,int i_height,NyARIntPoint2d[] o_point)\r
 342         {       \r
 343                 int idx=0;\r
 344                 NyARIntPoint2d ptr=o_point[0];\r
 345                 if(this.crossPos(0,-1,0,ptr) && ptr.x>=0 && ptr.x<i_width)\r
 346                 {\r
 347                         //y=rect.yの線\r
 348                         idx++;\r
 349                         ptr=o_point[idx];\r
 350                 }\r
 351                 if(this.crossPos(0,-1,i_height-1,ptr) && ptr.x>=0 && ptr.x<i_width)\r
 352                 {\r
 353                         //y=(rect.y+rect.h-1)の線\r
 354                         idx++;\r
 355                         if(idx==2){\r
 356                                 return true;\r
 357                         }\r
 358                         ptr=o_point[idx];\r
 359                 }\r
 360                 if(this.crossPos(-1,0,0,ptr) && ptr.y>=0 && ptr.y<i_height)\r
 361                 {\r
 362                         //x=i_leftの線\r
 363                         idx++;\r
 364                         if(idx==2){\r
 365                                 return true;\r
 366                         }\r
 367                         ptr=o_point[idx];\r
 368                 }\r
 369                 if(this.crossPos(-1,0,i_width-1, ptr) && ptr.y>=0 && ptr.y<i_height)\r
 370                 {\r
 371                         //x=i_right-1の線\r
 372                         idx++;\r
 373                         if(idx==2){\r
 374                                 return true;\r
 375                         }\r
 376                 }\r
 377                 return false;\r
 378         }\r
 379         /**\r
 380          * この関数は、直線を矩形でクリッピングしたときの、端点を計算します。\r
 381          * @param i_left\r
 382          * 矩形の左上座標(X)\r
 383          * @param i_top\r
 384          * 矩形の左上座標(Y)\r
 385          * @param i_width\r
 386          * 矩形の幅\r
 387          * @param i_height\r
 388          * 矩形の高さ\r
 389          * @param o_point\r
 390          * 端点を返すオブジェクト配列。2要素である必要があります。\r
 391          * @return\r
 392          * 端点が求まればtrue\r
 393          */\r
 394         public final boolean makeSegmentLine(int i_left,int i_top,int i_width,int i_height,NyARIntPoint2d[] o_point)\r
 395         {       \r
 396                 int bottom=i_top+i_height;\r
 397                 int right=i_left+i_width;\r
 398                 int idx=0;\r
 399                 NyARIntPoint2d ptr=o_point[0];\r
 400                 if(this.crossPos(0,-1,i_top,ptr) && ptr.x>=i_left && ptr.x<right)\r
 401                 {\r
 402                         //y=rect.yの線\r
 403                         idx++;\r
 404                         ptr=o_point[idx];\r
 405                 }\r
 406                 if(this.crossPos(0,-1,bottom-1,ptr) && ptr.x>=i_left && ptr.x<right)\r
 407                 {\r
 408                         //y=(rect.y+rect.h-1)の線\r
 409                         idx++;\r
 410                         if(idx==2){\r
 411                                 return true;\r
 412                         }\r
 413                         ptr=o_point[idx];\r
 414                 }\r
 415                 if(this.crossPos(-1,0,i_left,ptr) && ptr.y>=i_top && ptr.y<bottom)\r
 416                 {\r
 417                         //x=i_leftの線\r
 418                         idx++;\r
 419                         if(idx==2){\r
 420                                 return true;\r
 421                         }\r
 422                         ptr=o_point[idx];\r
 423                 }\r
 424                 if(this.crossPos(-1,0,right-1, ptr) && ptr.y>=i_top && ptr.y<bottom)\r
 425                 {\r
 426                         //x=i_right-1の線\r
 427                         idx++;\r
 428                         if(idx==2){\r
 429                                 return true;\r
 430                         }\r
 431                 }\r
 432                 return false;\r
 433         }\r
 434         /**\r
 435          * この関数は、この直線と、i_sp1とi_sp2の作る線分との、二乗距離値の合計を返します。\r
 436          * 計算方法は、線分の端点を通過する直線の法線上での、端点と直線の距離の合計です。\r
 437          * 線分と直線の類似度を判定する数値になります。\r
 438          * @param i_sp1\r
 439          * 線分の端点1\r
 440          * @param i_sp2\r
 441          * 線分の端点2\r
 442          * @return\r
 443          * 二乗距離値の合計。距離が取れないときは無限大です。\r
 444          */\r
 445         public final double sqDistBySegmentLineEdge(NyARDoublePoint2d i_sp1,NyARDoublePoint2d i_sp2)\r
 446         {\r
 447                 double la,lb,lc;\r
 448                 double x,y,w1;\r
 449                 //thisを法線に変換\r
 450                 la=this.b;\r
 451                 lb=-this.a;\r
 452 \r
 453                 //交点を計算\r
 454                 w1 = this.a * lb - la * this.b;\r
 455                 if (w1 == 0.0) {\r
 456                         return Double.POSITIVE_INFINITY;\r
 457                 }\r
 458                 //i_sp1と、i_linerの交点\r
 459                 lc=-(la*i_sp1.x+lb*i_sp1.y);\r
 460                 x = ((this.b * lc - lb * this.c) / w1)-i_sp1.x;\r
 461                 y = ((la * this.c - this.a * lc) / w1)-i_sp1.y;\r
 462                 double sqdist=x*x+y*y;\r
 463 \r
 464                 lc=-(la*i_sp2.x+lb*i_sp2.y);\r
 465                 x = ((this.b * lc - lb * this.c) / w1)-i_sp2.x;\r
 466                 y = ((la * this.c - this.a * lc) / w1)-i_sp2.y;\r
 467 \r
 468                 return sqdist+x*x+y*y;\r
 469         }\r
 470         /**\r
 471          * この関数は、頂点群から最小二乗法を使用して直線を計算します。\r
 472          * @param i_points\r
 473          * 頂点群を格納した配列。\r
 474          * @param i_number_of_data\r
 475          * 計算対象の頂点群の数\r
 476          * @return\r
 477          * 計算に成功すると、trueを返します。\r
 478          */\r
 479         public boolean leastSquares(NyARDoublePoint2d[] i_points,int i_number_of_data)\r
 480         {\r
 481                 assert(i_number_of_data>1);\r
 482                 int i;\r
 483                 double sum_xy = 0, sum_x = 0, sum_y = 0, sum_x2 = 0;\r
 484                 for (i=0; i<i_number_of_data; i++){\r
 485                         NyARDoublePoint2d ptr=i_points[i];\r
 486                         double xw=ptr.x;\r
 487                         sum_xy += xw * ptr.y;\r
 488                         sum_x += xw;\r
 489                         sum_y += ptr.y;\r
 490                         sum_x2 += xw*xw;\r
 491                 }\r
 492                 this.b =-(i_number_of_data * sum_x2 - sum_x*sum_x);\r
 493                 this.a = (i_number_of_data * sum_xy - sum_x * sum_y);\r
 494                 this.c = (sum_x2 * sum_y - sum_xy * sum_x);\r
 495                 return true;\r
 496         }\r
 497 }\r