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