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