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