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

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