tags/2.3.1/src/jp/nyatla/nyartoolkit/core/transmat/rotmatrix/NyARRotVector.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 version ARToolkit class library.\r
  11  * Copyright (C)2008 R.Iizuka\r
  12  *\r
  13  * This program is free software; you can redistribute it and/or\r
  14  * modify it under the terms of the GNU General Public License\r
  15  * as published by the Free Software Foundation; either version 2\r
  16  * of the License, or (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 framework; if not, write to the Free Software\r
  25  * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA\r
  26  * \r
  27  * For further information please contact.\r
  28  *      http://nyatla.jp/nyatoolkit/\r
  29  *      <airmail(at)ebony.plala.or.jp>\r
  30  * \r
  31  */\r
  32 package jp.nyatla.nyartoolkit.core.transmat.rotmatrix;\r
  33 \r
  34 import jp.nyatla.nyartoolkit.NyARException;\r
  35 import jp.nyatla.nyartoolkit.core.NyARMat;\r
  36 import jp.nyatla.nyartoolkit.core.types.NyARDoublePoint2d;\r
  37 import jp.nyatla.nyartoolkit.core.types.NyARLinear;\r
  38 import jp.nyatla.nyartoolkit.core.param.*;\r
  39 \r
  40 public class NyARRotVector\r
  41 {\r
  42 \r
  43         //publicメンバ達\r
  44         public double v1;\r
  45 \r
  46         public double v2;\r
  47 \r
  48         public double v3;\r
  49 \r
  50         //privateメンバ達\r
  51         \r
  52         private NyARPerspectiveProjectionMatrix _projection_mat_ref;\r
  53 \r
  54         private double[][] _inv_cpara_array_ref;\r
  55 \r
  56         public NyARRotVector(NyARPerspectiveProjectionMatrix i_cmat) throws NyARException\r
  57         {\r
  58                 NyARMat mat_a = new NyARMat(3, 3);\r
  59                 double[][] a_array = mat_a.getArray();\r
  60                 \r
  61                 a_array[0][0] =i_cmat.m00;\r
  62                 a_array[0][1] =i_cmat.m01;\r
  63                 a_array[0][2] =i_cmat.m02;\r
  64                 a_array[1][0] =i_cmat.m10;\r
  65                 a_array[1][1] =i_cmat.m11;\r
  66                 a_array[1][2] =i_cmat.m12;\r
  67                 a_array[2][0] =i_cmat.m20;\r
  68                 a_array[2][1] =i_cmat.m21;\r
  69                 a_array[2][2] =i_cmat.m22;\r
  70                 \r
  71                 mat_a.matrixSelfInv();\r
  72                 this._projection_mat_ref = i_cmat;\r
  73                 this._inv_cpara_array_ref = mat_a.getArray();\r
  74                 //GCない言語のときは、ここで配列の所有権委譲してね！\r
  75         }\r
  76 \r
  77         /**\r
  78          * ２直線に直交するベクトルを計算する・・・だと思う。\r
  79          * @param i_linear1\r
  80          * @param i_linear2\r
  81          */\r
  82         public void exteriorProductFromLinear(NyARLinear i_linear1, NyARLinear i_linear2)\r
  83         {\r
  84                 //1行目\r
  85                 final NyARPerspectiveProjectionMatrix cmat= this._projection_mat_ref;\r
  86                 final double w1 = i_linear1.run * i_linear2.rise - i_linear2.run * i_linear1.rise;\r
  87                 final double w2 = i_linear1.rise * i_linear2.intercept - i_linear2.rise * i_linear1.intercept;\r
  88                 final double w3 = i_linear1.intercept * i_linear2.run - i_linear2.intercept * i_linear1.run;\r
  89 \r
  90                 final double m0 = w1 * (cmat.m01 * cmat.m12 - cmat.m02 * cmat.m11) + w2 * cmat.m11 - w3 * cmat.m01;//w1 * (cpara[0 * 4 + 1] * cpara[1 * 4 + 2] - cpara[0 * 4 + 2] * cpara[1 * 4 + 1]) + w2 * cpara[1 * 4 + 1] - w3 * cpara[0 * 4 + 1];\r
  91                 final double m1 = -w1 * cmat.m00 * cmat.m12 + w3 * cmat.m00;//-w1 * cpara[0 * 4 + 0] * cpara[1 * 4 + 2] + w3 * cpara[0 * 4 + 0];\r
  92                 final double m2 = w1 * cmat.m00 * cmat.m11;//w1 * cpara[0 * 4 + 0] * cpara[1 * 4 + 1];\r
  93                 final double w = Math.sqrt(m0 * m0 + m1 * m1 + m2 * m2);\r
  94                 this.v1 = m0 / w;\r
  95                 this.v2 = m1 / w;\r
  96                 this.v3 = m2 / w;\r
  97                 return;\r
  98         }\r
  99 \r
 100         /**\r
 101          * static int check_dir( double dir[3], double st[2], double ed[2],double cpara[3][4] ) Optimize:STEP[526->468]\r
 102          * ベクトルの開始/終了座標を指定して、ベクトルの方向を調整する。\r
 103          * @param i_start_vertex\r
 104          * @param i_end_vertex\r
 105          * @param cpara\r
 106          */\r
 107         public void checkVectorByVertex(final NyARDoublePoint2d i_start_vertex, final NyARDoublePoint2d i_end_vertex) throws NyARException\r
 108         {\r
 109                 double h;\r
 110                 final double[][] inv_cpara = this._inv_cpara_array_ref;\r
 111                 //final double[] world = __checkVectorByVertex_world;// [2][3];\r
 112                 final double world0 = inv_cpara[0][0] * i_start_vertex.x * 10.0 + inv_cpara[0][1] * i_start_vertex.y * 10.0 + inv_cpara[0][2] * 10.0;// mat_a->m[0]*st[0]*10.0+\r
 113                 final double world1 = inv_cpara[1][0] * i_start_vertex.x * 10.0 + inv_cpara[1][1] * i_start_vertex.y * 10.0 + inv_cpara[1][2] * 10.0;// mat_a->m[3]*st[0]*10.0+\r
 114                 final double world2 = inv_cpara[2][0] * i_start_vertex.x * 10.0 + inv_cpara[2][1] * i_start_vertex.y * 10.0 + inv_cpara[2][2] * 10.0;// mat_a->m[6]*st[0]*10.0+\r
 115                 final double world3 = world0 + this.v1;\r
 116                 final double world4 = world1 + this.v2;\r
 117                 final double world5 = world2 + this.v3;\r
 118                 // </Optimize>\r
 119 \r
 120                 //final double[] camera = __checkVectorByVertex_camera;// [2][2];\r
 121                 final NyARPerspectiveProjectionMatrix cmat= this._projection_mat_ref;\r
 122                 //h = cpara[2 * 4 + 0] * world0 + cpara[2 * 4 + 1] * world1 + cpara[2 * 4 + 2] * world2;\r
 123                 h = cmat.m20 * world0 + cmat.m21 * world1 + cmat.m22 * world2;\r
 124                 if (h == 0.0) {\r
 125                         throw new NyARException();\r
 126                 }\r
 127                 //final double camera0 = (cpara[0 * 4 + 0] * world0 + cpara[0 * 4 + 1] * world1 + cpara[0 * 4 + 2] * world2) / h;\r
 128                 //final double camera1 = (cpara[1 * 4 + 0] * world0 + cpara[1 * 4 + 1] * world1 + cpara[1 * 4 + 2] * world2) / h;\r
 129                 final double camera0 = (cmat.m00 * world0 + cmat.m01 * world1 + cmat.m02 * world2) / h;\r
 130                 final double camera1 = (cmat.m10 * world0 + cmat.m11 * world1 + cmat.m12 * world2) / h;\r
 131 \r
 132                 //h = cpara[2 * 4 + 0] * world3 + cpara[2 * 4 + 1] * world4 + cpara[2 * 4 + 2] * world5;\r
 133                 h = cmat.m20 * world3 + cmat.m21 * world4 + cmat.m22 * world5;\r
 134                 if (h == 0.0) {\r
 135                         throw new NyARException();\r
 136                 }\r
 137                 //final double camera2 = (cpara[0 * 4 + 0] * world3 + cpara[0 * 4 + 1] * world4 + cpara[0 * 4 + 2] * world5) / h;\r
 138                 //final double camera3 = (cpara[1 * 4 + 0] * world3 + cpara[1 * 4 + 1] * world4 + cpara[1 * 4 + 2] * world5) / h;\r
 139                 final double camera2 = (cmat.m00 * world3 + cmat.m01 * world4 + cmat.m02 * world5) / h;\r
 140                 final double camera3 = (cmat.m10 * world3 + cmat.m11 * world4 + cmat.m12 * world5) / h;\r
 141 \r
 142                 final double v = (i_end_vertex.x - i_start_vertex.x) * (camera2 - camera0) + (i_end_vertex.y - i_start_vertex.y) * (camera3 - camera1);\r
 143                 if (v < 0) {\r
 144                         this.v1 = -this.v1;\r
 145                         this.v2 = -this.v2;\r
 146                         this.v3 = -this.v3;\r
 147                 }\r
 148         }\r
 149         /**\r
 150          * int check_rotation( double rot[2][3] )\r
 151          * 2つのベクトル引数の調整をする？\r
 152          * @param i_r\r
 153          * @throws NyARException\r
 154          */\r
 155 \r
 156         public final static void checkRotation(NyARRotVector io_vec1, NyARRotVector io_vec2) throws NyARException\r
 157         {\r
 158                 double w;\r
 159                 int f;\r
 160 \r
 161                 double vec10 = io_vec1.v1;\r
 162                 double vec11 = io_vec1.v2;\r
 163                 double vec12 = io_vec1.v3;\r
 164                 double vec20 = io_vec2.v1;\r
 165                 double vec21 = io_vec2.v2;\r
 166                 double vec22 = io_vec2.v3;\r
 167                 \r
 168                 double vec30 = vec11 * vec22 - vec12 * vec21;\r
 169                 double vec31 = vec12 * vec20 - vec10 * vec22;\r
 170                 double vec32 = vec10 * vec21 - vec11 * vec20;\r
 171                 w = Math.sqrt(vec30 * vec30 + vec31 * vec31 + vec32 * vec32);\r
 172                 if (w == 0.0) {\r
 173                         throw new NyARException();\r
 174                 }\r
 175                 vec30 /= w;\r
 176                 vec31 /= w;\r
 177                 vec32 /= w;\r
 178 \r
 179                 double cb = vec10 * vec20 + vec11 * vec21 + vec12 * vec22;\r
 180                 if (cb < 0){\r
 181                         cb=-cb;//cb *= -1.0;                    \r
 182                 }\r
 183                 final double ca = (Math.sqrt(cb + 1.0) + Math.sqrt(1.0 - cb)) * 0.5;\r
 184 \r
 185                 if (vec31 * vec10 - vec11 * vec30 != 0.0) {\r
 186                         f = 0;\r
 187                 } else {\r
 188                         if (vec32 * vec10 - vec12 * vec30 != 0.0) {\r
 189                                 w = vec11;vec11 = vec12;vec12 = w;\r
 190                                 w = vec31;vec31 = vec32;vec32 = w;\r
 191                                 f = 1;\r
 192                         } else {\r
 193                                 w = vec10;vec10 = vec12;vec12 = w;\r
 194                                 w = vec30;vec30 = vec32;vec32 = w;\r
 195                                 f = 2;\r
 196                         }\r
 197                 }\r
 198                 if (vec31 * vec10 - vec11 * vec30 == 0.0) {\r
 199                         throw new NyARException();\r
 200                 }\r
 201                 \r
 202                 double k1,k2,k3,k4;\r
 203                 double a, b, c, d;\r
 204                 double p1, q1, r1;\r
 205                 double p2, q2, r2;\r
 206                 double p3, q3, r3;\r
 207                 double p4, q4, r4;              \r
 208                 \r
 209                 \r
 210                 k1 = (vec11 * vec32 - vec31 * vec12) / (vec31 * vec10 - vec11 * vec30);\r
 211                 k2 = (vec31 * ca) / (vec31 * vec10 - vec11 * vec30);\r
 212                 k3 = (vec10 * vec32 - vec30 * vec12) / (vec30 * vec11 - vec10 * vec31);\r
 213                 k4 = (vec30 * ca) / (vec30 * vec11 - vec10 * vec31);\r
 214 \r
 215                 a = k1 * k1 + k3 * k3 + 1;\r
 216                 b = k1 * k2 + k3 * k4;\r
 217                 c = k2 * k2 + k4 * k4 - 1;\r
 218 \r
 219                 d = b * b - a * c;\r
 220                 if (d < 0) {\r
 221                         throw new NyARException();\r
 222                 }\r
 223                 r1 = (-b + Math.sqrt(d)) / a;\r
 224                 p1 = k1 * r1 + k2;\r
 225                 q1 = k3 * r1 + k4;\r
 226                 r2 = (-b - Math.sqrt(d)) / a;\r
 227                 p2 = k1 * r2 + k2;\r
 228                 q2 = k3 * r2 + k4;\r
 229                 if (f == 1) {\r
 230                         w = q1;q1 = r1;r1 = w;\r
 231                         w = q2;q2 = r2;r2 = w;\r
 232                         w = vec11;vec11 = vec12;vec12 = w;\r
 233                         w = vec31;vec31 = vec32;vec32 = w;\r
 234                         f = 0;\r
 235                 }\r
 236                 if (f == 2) {\r
 237                         w = p1;p1 = r1;r1 = w;\r
 238                         w = p2;p2 = r2;r2 = w;\r
 239                         w = vec10;vec10 = vec12;vec12 = w;\r
 240                         w = vec30;vec30 = vec32;vec32 = w;\r
 241                         f = 0;\r
 242                 }\r
 243 \r
 244                 if (vec31 * vec20 - vec21 * vec30 != 0.0) {\r
 245                         f = 0;\r
 246                 } else {\r
 247                         if (vec32 * vec20 - vec22 * vec30 != 0.0) {\r
 248                                 w = vec21;vec21 = vec22;vec22 = w;\r
 249                                 w = vec31;vec31 = vec32;vec32 = w;\r
 250                                 f = 1;\r
 251                         } else {\r
 252                                 w = vec20;vec20 = vec22;vec22 = w;\r
 253                                 w = vec30;vec30 = vec32;vec32 = w;\r
 254                                 f = 2;\r
 255                         }\r
 256                 }\r
 257                 if (vec31 * vec20 - vec21 * vec30 == 0.0) {\r
 258                         throw new NyARException();\r
 259                 }\r
 260                 k1 = (vec21 * vec32 - vec31 * vec22) / (vec31 * vec20 - vec21 * vec30);\r
 261                 k2 = (vec31 * ca) / (vec31 * vec20 - vec21 * vec30);\r
 262                 k3 = (vec20 * vec32 - vec30 * vec22) / (vec30 * vec21 - vec20 * vec31);\r
 263                 k4 = (vec30 * ca) / (vec30 * vec21 - vec20 * vec31);\r
 264 \r
 265                 a = k1 * k1 + k3 * k3 + 1;\r
 266                 b = k1 * k2 + k3 * k4;\r
 267                 c = k2 * k2 + k4 * k4 - 1;\r
 268 \r
 269                 d = b * b - a * c;\r
 270                 if (d < 0) {\r
 271                         throw new NyARException();\r
 272                 }\r
 273                 r3 = (-b + Math.sqrt(d)) / a;\r
 274                 p3 = k1 * r3 + k2;\r
 275                 q3 = k3 * r3 + k4;\r
 276                 r4 = (-b - Math.sqrt(d)) / a;\r
 277                 p4 = k1 * r4 + k2;\r
 278                 q4 = k3 * r4 + k4;\r
 279                 if (f == 1) {\r
 280                         w = q3;q3 = r3;r3 = w;\r
 281                         w = q4;q4 = r4;r4 = w;\r
 282                         w = vec21;vec21 = vec22;vec22 = w;\r
 283                         w = vec31;vec31 = vec32;vec32 = w;\r
 284                         f = 0;\r
 285                 }\r
 286                 if (f == 2) {\r
 287                         w = p3;p3 = r3;r3 = w;\r
 288                         w = p4;p4 = r4;r4 = w;\r
 289                         w = vec20;vec20 = vec22;vec22 = w;\r
 290                         w = vec30;vec30 = vec32;vec32 = w;\r
 291                         f = 0;\r
 292                 }\r
 293 \r
 294                 double e1 = p1 * p3 + q1 * q3 + r1 * r3;\r
 295                 if (e1 < 0) {\r
 296                         e1 = -e1;\r
 297                 }\r
 298                 double e2 = p1 * p4 + q1 * q4 + r1 * r4;\r
 299                 if (e2 < 0) {\r
 300                         e2 = -e2;\r
 301                 }\r
 302                 double e3 = p2 * p3 + q2 * q3 + r2 * r3;\r
 303                 if (e3 < 0) {\r
 304                         e3 = -e3;\r
 305                 }\r
 306                 double e4 = p2 * p4 + q2 * q4 + r2 * r4;\r
 307                 if (e4 < 0) {\r
 308                         e4 = -e4;\r
 309                 }\r
 310                 if (e1 < e2) {\r
 311                         if (e1 < e3) {\r
 312                                 if (e1 < e4) {\r
 313                                         io_vec1.v1 = p1;\r
 314                                         io_vec1.v2 = q1;\r
 315                                         io_vec1.v3 = r1;\r
 316                                         io_vec2.v1 = p3;\r
 317                                         io_vec2.v2 = q3;\r
 318                                         io_vec2.v3 = r3;\r
 319                                 } else {\r
 320                                         io_vec1.v1 = p2;\r
 321                                         io_vec1.v2 = q2;\r
 322                                         io_vec1.v3 = r2;\r
 323                                         io_vec2.v1 = p4;\r
 324                                         io_vec2.v2 = q4;\r
 325                                         io_vec2.v3 = r4;\r
 326                                 }\r
 327                         } else {\r
 328                                 if (e3 < e4) {\r
 329                                         io_vec1.v1 = p2;\r
 330                                         io_vec1.v2 = q2;\r
 331                                         io_vec1.v3 = r2;\r
 332                                         io_vec2.v1 = p3;\r
 333                                         io_vec2.v2 = q3;\r
 334                                         io_vec2.v3 = r3;\r
 335                                 } else {\r
 336                                         io_vec1.v1 = p2;\r
 337                                         io_vec1.v2 = q2;\r
 338                                         io_vec1.v3 = r2;\r
 339                                         io_vec2.v1 = p4;\r
 340                                         io_vec2.v2 = q4;\r
 341                                         io_vec2.v3 = r4;\r
 342                                 }\r
 343                         }\r
 344                 } else {\r
 345                         if (e2 < e3) {\r
 346                                 if (e2 < e4) {\r
 347                                         io_vec1.v1 = p1;\r
 348                                         io_vec1.v2 = q1;\r
 349                                         io_vec1.v3 = r1;\r
 350                                         io_vec2.v1 = p4;\r
 351                                         io_vec2.v2 = q4;\r
 352                                         io_vec2.v3 = r4;\r
 353                                 } else {\r
 354                                         io_vec1.v1 = p2;\r
 355                                         io_vec1.v2 = q2;\r
 356                                         io_vec1.v3 = r2;\r
 357                                         io_vec2.v1 = p4;\r
 358                                         io_vec2.v2 = q4;\r
 359                                         io_vec2.v3 = r4;\r
 360                                 }\r
 361                         } else {\r
 362                                 if (e3 < e4) {\r
 363                                         io_vec1.v1 = p2;\r
 364                                         io_vec1.v2 = q2;\r
 365                                         io_vec1.v3 = r2;\r
 366                                         io_vec2.v1 = p3;\r
 367                                         io_vec2.v2 = q3;\r
 368                                         io_vec2.v3 = r3;\r
 369                                 } else {\r
 370                                         io_vec1.v1 = p2;\r
 371                                         io_vec1.v2 = q2;\r
 372                                         io_vec1.v3 = r2;\r
 373                                         io_vec2.v1 = p4;\r
 374                                         io_vec2.v2 = q4;\r
 375                                         io_vec2.v3 = r4;\r
 376                                 }\r
 377                         }\r
 378                 }\r
 379                 return;\r
 380         }       \r
 381 }\r