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  * 定点と傾きのパラメータで、直線を表現します。\r
  30  *\r
  31  */\r
  32 public class NyARVecLinear2d\r
  33 {\r
  34         public double x;\r
  35         public double y;\r
  36         public double dx;\r
  37         public double dy;\r
  38         public static NyARVecLinear2d[] createArray(int i_length)\r
  39         {\r
  40                 NyARVecLinear2d[] r=new NyARVecLinear2d[i_length];\r
  41                 for(int i=0;i<i_length;i++){\r
  42                         r[i]=new NyARVecLinear2d();\r
  43                 }\r
  44                 return r;\r
  45         }\r
  46         /**\r
  47          * 法線ベクトルを計算します。\r
  48          * @param i_src\r
  49          * 元のベクトルを指定します。この値には、thisを指定できます。\r
  50          */\r
  51         public final void normalVec(NyARVecLinear2d i_src)\r
  52         {\r
  53                 double w=this.dx;\r
  54                 this.dx=i_src.dy;\r
  55                 this.dy=-w;\r
  56         }\r
  57         public final void setValue(NyARVecLinear2d i_value)\r
  58         {\r
  59                 this.dx=i_value.dx;\r
  60                 this.dy=i_value.dy;\r
  61                 this.x=i_value.x;\r
  62                 this.y=i_value.y;\r
  63         }\r
  64         /**\r
  65          * このベクトルと指定した直線が作るCos値を返します。\r
  66          * @param i_v1\r
  67          * @return\r
  68          */\r
  69         public final double getVecCos(NyARVecLinear2d i_v1)\r
  70         {\r
  71                 double x1=i_v1.dx;\r
  72                 double y1=i_v1.dy;\r
  73                 double x2=this.dx;\r
  74                 double y2=this.dy;\r
  75                 double d=(x1*x2+y1*y2)/Math.sqrt((x1*x1+y1*y1)*(x2*x2+y2*y2));\r
  76                 return d;\r
  77         }\r
  78         public final double getAbsVecCos(NyARVecLinear2d i_v1)\r
  79         {\r
  80                 double x1=i_v1.dx;\r
  81                 double y1=i_v1.dy;\r
  82                 double x2=this.dx;\r
  83                 double y2=this.dy;\r
  84                 double d=(x1*x2+y1*y2)/Math.sqrt((x1*x1+y1*y1)*(x2*x2+y2*y2));\r
  85                 return d>=0?d:-d;\r
  86         }\r
  87         /**\r
  88          * このベクトルと指定したベクトルが作るCos値を返します。\r
  89          * @param i_dx\r
  90          * @param i_dy\r
  91          * @return\r
  92          */\r
  93         public final double getVecCos(double i_dx,double i_dy)\r
  94         {\r
  95                 double x1=this.dx;\r
  96                 double y1=this.dy;\r
  97                 double d=(x1*i_dx+y1*i_dy)/Math.sqrt((x1*x1+y1*y1)*(i_dx*i_dx+i_dy*i_dy));\r
  98                 return d;\r
  99         }\r
 100         public final double getVecCos(NyARDoublePoint2d i_pos1,NyARDoublePoint2d i_pos2)\r
 101         {\r
 102                 double d=getAbsVecCos(i_pos2.x-i_pos1.x,i_pos2.y-i_pos1.y);\r
 103                 return d>=0?d:-d;\r
 104         }       \r
 105         public final double getAbsVecCos(double i_v2_x,double i_v2_y)\r
 106         {\r
 107                 double x1=this.dx;\r
 108                 double y1=this.dy;\r
 109                 double d=(x1*i_v2_x+y1*i_v2_y)/Math.sqrt((x1*x1+y1*y1)*(i_v2_x*i_v2_x+i_v2_y*i_v2_y));\r
 110                 return d>=0?d:-d;\r
 111         }\r
 112         /**\r
 113          * このベクトルと、i_pos1-&lt;i_pos2を結ぶ線分が作るcos値の絶対値を返します。\r
 114          * @param i_pos1\r
 115          * @param i_pos2\r
 116          * @return\r
 117          */\r
 118         public final double getAbsVecCos(NyARDoublePoint2d i_pos1,NyARDoublePoint2d i_pos2)\r
 119         {\r
 120                 double d=getAbsVecCos(i_pos2.x-i_pos1.x,i_pos2.y-i_pos1.y);\r
 121                 return d>=0?d:-d;\r
 122         }\r
 123         \r
 124         /**\r
 125          * 交点を求めます。\r
 126          * @param i_vector1\r
 127          * @param i_vector2\r
 128          * @param o_point\r
 129          * @return\r
 130          */\r
 131         public final boolean crossPos(NyARVecLinear2d i_vector1,NyARDoublePoint2d o_point)\r
 132         {\r
 133                 double a1= i_vector1.dy;\r
 134                 double b1=-i_vector1.dx;\r
 135                 double c1=(i_vector1.dx*i_vector1.y-i_vector1.dy*i_vector1.x);\r
 136                 double a2= this.dy;\r
 137                 double b2=-this.dx;\r
 138                 double c2=(this.dx*this.y-this.dy*this.x);\r
 139                 final double w1 = a1 * b2 - a2 * b1;\r
 140                 if (w1 == 0.0) {\r
 141                         return false;\r
 142                 }\r
 143                 o_point.x = (b1 * c2 - b2 * c1) / w1;\r
 144                 o_point.y = (a2 * c1 - a1 * c2) / w1;\r
 145                 return true;\r
 146         }\r
 147         /**\r
 148          * 直線と、i_sp1とi_sp2の作る線分との二乗距離値の合計を返します。\r
 149          * 線分と直線の類似度を\r
 150          * @param i_sp1\r
 151          * @param i_sp2\r
 152          * @param o_point\r
 153          * @return\r
 154          * 距離が取れないときは無限大です。\r
 155          */\r
 156         public final double sqDistBySegmentLineEdge(NyARDoublePoint2d i_sp1,NyARDoublePoint2d i_sp2)\r
 157         {\r
 158                 double sa,sb,sc;\r
 159                 sa= this.dy;\r
 160                 sb=-this.dx;\r
 161                 sc=(this.dx*this.y-this.dy*this.x);\r
 162                 \r
 163 \r
 164                 double lc;\r
 165                 double x,y,w1;\r
 166                 //thisを法線に変換\r
 167 \r
 168                 //交点を計算\r
 169                 w1 = sa * (-sa) - sb * sb;\r
 170                 if (w1 == 0.0) {\r
 171                         return Double.POSITIVE_INFINITY;\r
 172                 }\r
 173                 //i_sp1と、i_linerの交点\r
 174                 lc=-(sb*i_sp1.x-sa*i_sp1.y);\r
 175                 x = ((sb * lc +sa * sc) / w1)-i_sp1.x;\r
 176                 y = ((sb * sc - sa * lc) / w1)-i_sp1.y;\r
 177                 double sqdist=x*x+y*y;\r
 178 \r
 179                 lc=-(sb*i_sp2.x-sa*i_sp2.y);\r
 180                 x = ((sb * lc + sa * sc) / w1)-i_sp2.x;\r
 181                 y = ((sb * sc - sa * lc) / w1)-i_sp2.y;\r
 182 \r
 183                 return sqdist+x*x+y*y;\r
 184         }\r
 185 \r
 186         /**\r
 187          * i_lineの直線をセットします。x,yの値は、(i_x,i_y)を通過するi_lineの法線とi_lineの交点をセットします。\r
 188          * @param i_line\r
 189          * @param i_x\r
 190          * @param i_y\r
 191          */\r
 192         public boolean setLinear(NyARLinear i_line,double i_x,double i_y)\r
 193         {\r
 194                 double la=i_line.b;\r
 195                 double lb=-i_line.a;\r
 196                 double lc=-(la*i_x+lb*i_y);\r
 197                 //交点を計算\r
 198                 final double w1 = -lb * lb - la * la;\r
 199                 if (w1 == 0.0) {\r
 200                         return false;\r
 201                 }               \r
 202                 this.x=((la * lc - lb * i_line.c) / w1);\r
 203                 this.y= ((la * i_line.c +lb * lc) / w1);\r
 204                 this.dy=-lb;\r
 205                 this.dx=-la;\r
 206                 return true;\r
 207         }\r
 208         /**\r
 209          * 点群から最小二乗法で直線を計算してセットします。\r
 210          * 通過点x,yは、点群の中央値を通過する、算出された直線の法線との交点です。\r
 211          * @param i_points\r
 212          * @param i_number_of_data\r
 213          * @return\r
 214          */\r
 215         public final boolean leastSquares(NyARDoublePoint2d[] i_points,int i_number_of_data)\r
 216         {\r
 217                 int i;\r
 218                 double sum_xy = 0, sum_x = 0, sum_y = 0, sum_x2 = 0;\r
 219                 for (i=0; i<i_number_of_data; i++){\r
 220                         NyARDoublePoint2d ptr=i_points[i];\r
 221                         double xw=ptr.x;\r
 222                         sum_xy += xw * ptr.y;\r
 223                         sum_x += xw;\r
 224                         sum_y += ptr.y;\r
 225                         sum_x2 += xw*xw;\r
 226                 }\r
 227                 double la=-(i_number_of_data * sum_x2 - sum_x*sum_x);\r
 228                 double lb=-(i_number_of_data * sum_xy - sum_x * sum_y);\r
 229                 double cc=(sum_x2 * sum_y - sum_xy * sum_x);\r
 230                 double lc=-(la*sum_x+lb*sum_y)/i_number_of_data;\r
 231                 //交点を計算\r
 232                 final double w1 = -lb * lb - la * la;\r
 233                 if (w1 == 0.0) {\r
 234                         return false;\r
 235                 }               \r
 236                 this.x=((la * lc - lb * cc) / w1);\r
 237                 this.y= ((la * cc +lb * lc) / w1);\r
 238                 this.dy=-lb;\r
 239                 this.dx=-la;\r
 240                 return true;\r
 241         }\r
 242         /**\r
 243          * 正規化したベクトルを出力する{@link #leastSquares}です。\r
 244          * @param i_points\r
 245          * @param i_number_of_data\r
 246          * @return\r
 247          */\r
 248         public final boolean leastSquaresWithNormalize(NyARDoublePoint2d[] i_points,int i_number_of_data)\r
 249         {\r
 250                 boolean ret=this.leastSquares(i_points, i_number_of_data);\r
 251                 double sq=1/Math.sqrt(this.dx*this.dx+this.dy*this.dy);\r
 252                 this.dx*=sq;\r
 253                 this.dy*=sq;\r
 254                 return ret;\r
 255         }\r
 256 \r
 257 }\r