lib/src/jp/nyatla/nyartoolkit/core/types/NyARVecLinear2d.java

   1 /* \r
   2  * PROJECT: NyARToolkit(Extension)\r
   3  * --------------------------------------------------------------------------------\r
   4  * The NyARToolkit is Java edition ARToolKit class library.\r
   5  * Copyright (C)2008-2009 Ryo Iizuka\r
   6  *\r
   7  * This program is free software: you can redistribute it and/or modify\r
   8  * it under the terms of the GNU General Public License as published by\r
   9  * the Free Software Foundation, either version 3 of the License, or\r
  10  * (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 program.  If not, see <http://www.gnu.org/licenses/>.\r
  19  * \r
  20  * For further information please contact.\r
  21  *      http://nyatla.jp/nyatoolkit/\r
  22  *      <airmail(at)ebony.plala.or.jp> or <nyatla(at)nyatla.jp>\r
  23  * \r
  24  */\r
  25 package jp.nyatla.nyartoolkit.core.types;\r
  26 \r
  27 \r
  28 /**\r
  29  * このクラスは、通過点とX,Yの変化量で、直線を定義します。\r
  30  */\r
  31 public class NyARVecLinear2d\r
  32 {\r
  33         /** 直線の通過点(X)*/\r
  34         public double x;\r
  35         /** 直線の通過点(Y)*/\r
  36         public double y;\r
  37         /** x方向の直線の変化量*/\r
  38         public double dx;\r
  39         /** y方向の直線の変化量*/\r
  40         public double dy;\r
  41         \r
  42         /**\r
  43          * この関数は、指定サイズのオブジェクト配列を作ります。\r
  44          * @param i_length\r
  45          * 作成する配列の長さ\r
  46          * @return\r
  47          * 新しい配列。\r
  48          */     \r
  49         public static NyARVecLinear2d[] createArray(int i_length)\r
  50         {\r
  51                 NyARVecLinear2d[] r=new NyARVecLinear2d[i_length];\r
  52                 for(int i=0;i<i_length;i++){\r
  53                         r[i]=new NyARVecLinear2d();\r
  54                 }\r
  55                 return r;\r
  56         }\r
  57         /**\r
  58          * この関数は、法線を計算します。\r
  59          * 通過点は変更しません。\r
  60          * @param i_src\r
  61          * 元のインスタンスを指定します。この値には、thisを指定できます。\r
  62          */\r
  63         public final void normalVec(NyARVecLinear2d i_src)\r
  64         {\r
  65                 double w=this.dx;\r
  66                 this.dx=i_src.dy;\r
  67                 this.dy=-w;\r
  68         }\r
  69         /**\r
  70          * この関数は、オブジェクトの値をインスタンスにセットします。\r
  71          * @param i_value\r
  72          * コピー元のオブジェクト\r
  73          */\r
  74         public final void setValue(NyARVecLinear2d i_value)\r
  75         {\r
  76                 this.dx=i_value.dx;\r
  77                 this.dy=i_value.dy;\r
  78                 this.x=i_value.x;\r
  79                 this.y=i_value.y;\r
  80         }\r
  81         /**\r
  82          * この関数は、この直線と引数の直線とが作るCos値を返します。\r
  83          * @param i_v1\r
  84          * 直線を格納したオブジェクト\r
  85          * @return\r
  86          * ２直線のCOS値（radian）\r
  87          */\r
  88         public final double getVecCos(NyARVecLinear2d i_v1)\r
  89         {\r
  90                 double x1=i_v1.dx;\r
  91                 double y1=i_v1.dy;\r
  92                 double x2=this.dx;\r
  93                 double y2=this.dy;\r
  94                 double d=(x1*x2+y1*y2)/Math.sqrt((x1*x1+y1*y1)*(x2*x2+y2*y2));\r
  95                 return d;\r
  96         }\r
  97         /**\r
  98          * この関数は、この直線と引数の直線とが作るCos値の絶対値を返します。\r
  99          * @param i_v1\r
 100          * 直線を格納したオブジェクト\r
 101          * @return\r
 102          * ２直線のCOS値の絶対値（radian）\r
 103          */\r
 104         public final double getAbsVecCos(NyARVecLinear2d i_v1)\r
 105         {\r
 106                 double d=getVecCos(i_v1);\r
 107                 return d>=0?d:-d;\r
 108         }\r
 109         /**\r
 110          * この関数は、この直線とベクトルが作るCos値を返します。\r
 111          * @param i_dx\r
 112          * ベクトルのX成分\r
 113          * @param i_dy\r
 114          * ベクトルのY成分\r
 115          * @return\r
 116          * ２直線のCOS値（radian）\r
 117          */\r
 118         public final double getVecCos(double i_dx,double i_dy)\r
 119         {\r
 120                 double x1=this.dx;\r
 121                 double y1=this.dy;\r
 122                 return (x1*i_dx+y1*i_dy)/Math.sqrt((x1*x1+y1*y1)*(i_dx*i_dx+i_dy*i_dy));\r
 123         }\r
 124         /**\r
 125          * この関数は、この直線とベクトルが作るCos値の絶対値を返します。\r
 126          * @param i_v2_x\r
 127          * ベクトルのX成分\r
 128          * @param i_v2_y\r
 129          * ベクトルのY成分\r
 130          * @return\r
 131          * ２直線のCOS値の絶対値（radian）\r
 132          */\r
 133         public final double getAbsVecCos(double i_v2_x,double i_v2_y)\r
 134         {\r
 135                 double d=getVecCos(i_v2_x,i_v2_y);\r
 136                 return d>=0?d:-d;\r
 137         }       \r
 138         /**\r
 139          * この関数は、この直線と線分が作るCos値を返します。\r
 140          * @param i_pos1\r
 141          * 線分の端点１\r
 142          * @param i_pos2\r
 143          * 線分の端点２\r
 144          * @return\r
 145          * ２直線のCOS値（radian）\r
 146          */\r
 147         public final double getVecCos(NyARDoublePoint2d i_pos1,NyARDoublePoint2d i_pos2)\r
 148         {\r
 149                 return getVecCos(i_pos2.x-i_pos1.x,i_pos2.y-i_pos1.y);\r
 150         }       \r
 151 \r
 152         /**\r
 153          * この関数は、この直線と線分が作るCos値の絶対値を返します。\r
 154          * @param i_pos1\r
 155          * 線分の端点１\r
 156          * @param i_pos2\r
 157          * 線分の端点２\r
 158          * @return\r
 159          * ２直線のCOS値の絶対値（radian）\r
 160          */\r
 161         public final double getAbsVecCos(NyARDoublePoint2d i_pos1,NyARDoublePoint2d i_pos2)\r
 162         {\r
 163                 return getAbsVecCos(i_pos2.x-i_pos1.x,i_pos2.y-i_pos1.y);\r
 164         }\r
 165         \r
 166         /**\r
 167          * この関数は、直線との交点を求めます。\r
 168          * @param i_vector1\r
 169          * 交点を求める直線\r
 170          * @param o_point\r
 171          * 交点座標を得るオブジェクト。\r
 172          * @return\r
 173          * 交点が求まると、trueを返します。\r
 174          */\r
 175         public final boolean crossPos(NyARVecLinear2d i_vector1,NyARDoublePoint2d o_point)\r
 176         {\r
 177                 double a1= i_vector1.dy;\r
 178                 double b1=-i_vector1.dx;\r
 179                 double c1=(i_vector1.dx*i_vector1.y-i_vector1.dy*i_vector1.x);\r
 180                 double a2= this.dy;\r
 181                 double b2=-this.dx;\r
 182                 double c2=(this.dx*this.y-this.dy*this.x);\r
 183                 final double w1 = a1 * b2 - a2 * b1;\r
 184                 if (w1 == 0.0) {\r
 185                         return false;\r
 186                 }\r
 187                 o_point.x = (b1 * c2 - b2 * c1) / w1;\r
 188                 o_point.y = (a2 * c1 - a1 * c2) / w1;\r
 189                 return true;\r
 190         }\r
 191         /**\r
 192          * この関数は、この直線と、i_sp1とi_sp2の作る線分との、二乗距離値の合計を返します。\r
 193          * 計算方法は、線分の端点を通過する直線の法線上での、端点と直線の距離の合計です。\r
 194          * 線分と直線の類似度を判定する数値になります。\r
 195          * @param i_sp1\r
 196          * 線分の端点1\r
 197          * @param i_sp2\r
 198          * 線分の端点2\r
 199          * @return\r
 200          * 二乗距離値の合計。距離が取れないときは無限大です。\r
 201          */\r
 202         public final double sqDistBySegmentLineEdge(NyARDoublePoint2d i_sp1,NyARDoublePoint2d i_sp2)\r
 203         {\r
 204                 double sa,sb,sc;\r
 205                 sa= this.dy;\r
 206                 sb=-this.dx;\r
 207                 sc=(this.dx*this.y-this.dy*this.x);\r
 208                 \r
 209 \r
 210                 double lc;\r
 211                 double x,y,w1;\r
 212                 //thisを法線に変換\r
 213 \r
 214                 //交点を計算\r
 215                 w1 = sa * (-sa) - sb * sb;\r
 216                 if (w1 == 0.0) {\r
 217                         return Double.POSITIVE_INFINITY;\r
 218                 }\r
 219                 //i_sp1と、i_linerの交点\r
 220                 lc=-(sb*i_sp1.x-sa*i_sp1.y);\r
 221                 x = ((sb * lc +sa * sc) / w1)-i_sp1.x;\r
 222                 y = ((sb * sc - sa * lc) / w1)-i_sp1.y;\r
 223                 double sqdist=x*x+y*y;\r
 224 \r
 225                 lc=-(sb*i_sp2.x-sa*i_sp2.y);\r
 226                 x = ((sb * lc + sa * sc) / w1)-i_sp2.x;\r
 227                 y = ((sb * sc - sa * lc) / w1)-i_sp2.y;\r
 228 \r
 229                 return sqdist+x*x+y*y;\r
 230         }\r
 231 \r
 232         /**\r
 233          * この関数は、i_lineの直線を、インスタンスにセットします。\r
 234          * {@link #x},{@link #y}の値は、(i_x,i_y)を通過するi_lineの法線とi_lineの交点をセットします。\r
 235          * @param i_line\r
 236          * セットする直線式\r
 237          * @param i_x\r
 238          * {@link #x},{@link #y}を確定するための、法線の通過点\r
 239          * @param i_y\r
 240          * {@link #x},{@link #y}を確定するための、法線の通過点\r
 241          * @return\r
 242          * セットに成功すると、trueを返します。\r
 243          */\r
 244         public boolean setLinear(NyARLinear i_line,double i_x,double i_y)\r
 245         {\r
 246                 double la=i_line.b;\r
 247                 double lb=-i_line.a;\r
 248                 double lc=-(la*i_x+lb*i_y);\r
 249                 //交点を計算\r
 250                 final double w1 = -lb * lb - la * la;\r
 251                 if (w1 == 0.0) {\r
 252                         return false;\r
 253                 }               \r
 254                 this.x=((la * lc - lb * i_line.c) / w1);\r
 255                 this.y= ((la * i_line.c +lb * lc) / w1);\r
 256                 this.dy=-lb;\r
 257                 this.dx=-la;\r
 258                 return true;\r
 259         }\r
 260         /**\r
 261          * この関数は、頂点群から最小二乗法を使用して直線を計算します。\r
 262          * @param i_points\r
 263          * 頂点群を格納した配列。\r
 264          * @param i_number_of_data\r
 265          * 計算対象の頂点群の数\r
 266          * @return\r
 267          * 計算に成功すると、trueを返します。\r
 268          */\r
 269         public final boolean leastSquares(NyARDoublePoint2d[] i_points,int i_number_of_data)\r
 270         {\r
 271                 int i;\r
 272                 double sum_xy = 0, sum_x = 0, sum_y = 0, sum_x2 = 0;\r
 273                 for (i=0; i<i_number_of_data; i++){\r
 274                         NyARDoublePoint2d ptr=i_points[i];\r
 275                         double xw=ptr.x;\r
 276                         sum_xy += xw * ptr.y;\r
 277                         sum_x += xw;\r
 278                         sum_y += ptr.y;\r
 279                         sum_x2 += xw*xw;\r
 280                 }\r
 281                 double la=-(i_number_of_data * sum_x2 - sum_x*sum_x);\r
 282                 double lb=-(i_number_of_data * sum_xy - sum_x * sum_y);\r
 283                 double cc=(sum_x2 * sum_y - sum_xy * sum_x);\r
 284                 double lc=-(la*sum_x+lb*sum_y)/i_number_of_data;\r
 285                 //交点を計算\r
 286                 final double w1 = -lb * lb - la * la;\r
 287                 if (w1 == 0.0) {\r
 288                         return false;\r
 289                 }               \r
 290                 this.x=((la * lc - lb * cc) / w1);\r
 291                 this.y= ((la * cc +lb * lc) / w1);\r
 292                 this.dy=-lb;\r
 293                 this.dx=-la;\r
 294                 return true;\r
 295         }\r
 296         /**\r
 297          * この関数は、正規化したベクトルを出力する、{@link #leastSquares}です。\r
 298          * @param i_points\r
 299          * 頂点群を格納した配列。\r
 300          * @param i_number_of_data\r
 301          * 計算対象の頂点群の数\r
 302          * @return\r
 303          * 計算に成功すると、trueを返します。\r
 304          */\r
 305         public final boolean leastSquaresWithNormalize(NyARDoublePoint2d[] i_points,int i_number_of_data)\r
 306         {\r
 307                 boolean ret=this.leastSquares(i_points, i_number_of_data);\r
 308                 double sq=1/Math.sqrt(this.dx*this.dx+this.dy*this.dy);\r
 309                 this.dx*=sq;\r
 310                 this.dy*=sq;\r
 311                 return ret;\r
 312         }\r
 313 \r
 314 }\r