src/jp/nyatla/nyartoolkit/nymodel/x2/NyARRotMatrix_X2.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.nymodel.x2;\r
  33 \r
  34 import jp.nyatla.nyartoolkit.NyARException;\r
  35 import jp.nyatla.nyartoolkit.core.transmat.NyARTransMatResult;\r
  36 import jp.nyatla.nyartoolkit.core.types.*;\r
  37 import jp.nyatla.nyartoolkit.core.transmat.rotmatrix.*;\r
  38 import jp.nyatla.nyartoolkit.core.param.*;\r
  39 \r
  40 \r
  41 \r
  42 /**\r
  43  * 回転行列計算用の、3x3行列\r
  44  * \r
  45  */\r
  46 public class NyARRotMatrix_X2 extends NyARRotMatrix\r
  47 {\r
  48         private NyARSinTable _sin_table_ref;\r
  49 \r
  50         /**\r
  51          * インスタンスを準備します。\r
  52          * \r
  53          * @param i_param\r
  54          */\r
  55         public NyARRotMatrix_X2(NyARPerspectiveProjectionMatrix i_matrix,NyARSinTable i_sin_table_ref) throws NyARException\r
  56         {\r
  57                 this.__initRot_vec1 = new NyARRotVector(i_matrix);\r
  58                 this.__initRot_vec2 = new NyARRotVector(i_matrix);\r
  59                 this._sin_table_ref=i_sin_table_ref;\r
  60                 return;\r
  61         }\r
  62 \r
  63         final private NyARRotVector __initRot_vec1;\r
  64 \r
  65         final private NyARRotVector __initRot_vec2;\r
  66 \r
  67         public final void initRotByPrevResult(NyARTransMatResult i_prev_result)\r
  68         {\r
  69 \r
  70                 this.m00 = i_prev_result.m00;\r
  71                 this.m01 = i_prev_result.m01;\r
  72                 this.m02 = i_prev_result.m02;\r
  73 \r
  74                 this.m10 = i_prev_result.m10;\r
  75                 this.m11 = i_prev_result.m11;\r
  76                 this.m12 = i_prev_result.m12;\r
  77 \r
  78                 this.m20 = i_prev_result.m20;\r
  79                 this.m21 = i_prev_result.m21;\r
  80                 this.m22 = i_prev_result.m22;\r
  81                 return;\r
  82         }\r
  83 \r
  84         public final void initRotBySquare(final NyARLinear[] i_linear, final NyARDoublePoint2d[] i_sqvertex) throws NyARException\r
  85         {\r
  86                 final NyARRotVector vec1 = this.__initRot_vec1;\r
  87                 final NyARRotVector vec2 = this.__initRot_vec2;\r
  88 \r
  89                 // 向かい合った辺から、２本のベクトルを計算\r
  90 \r
  91                 // 軸１\r
  92                 vec1.exteriorProductFromLinear(i_linear[0], i_linear[2]);\r
  93                 vec1.checkVectorByVertex(i_sqvertex[0], i_sqvertex[1]);\r
  94 \r
  95                 // 軸２\r
  96                 vec2.exteriorProductFromLinear(i_linear[1], i_linear[3]);\r
  97                 vec2.checkVectorByVertex(i_sqvertex[3], i_sqvertex[0]);\r
  98 \r
  99                 // 回転の最適化？\r
 100                 NyARRotVector.checkRotation(vec1, vec2);\r
 101 \r
 102                 this.m00 = vec1.v1;\r
 103                 this.m10 = vec1.v2;\r
 104                 this.m20 = vec1.v3;\r
 105                 this.m01 = vec2.v1;\r
 106                 this.m11 = vec2.v2;\r
 107                 this.m21 = vec2.v3;\r
 108 \r
 109                 // 最後の軸を計算\r
 110                 final double w02 = vec1.v2 * vec2.v3 - vec1.v3 * vec2.v2;\r
 111                 final double w12 = vec1.v3 * vec2.v1 - vec1.v1 * vec2.v3;\r
 112                 final double w22 = vec1.v1 * vec2.v2 - vec1.v2 * vec2.v1;\r
 113                 final double w = Math.sqrt(w02 * w02 + w12 * w12 + w22 * w22);\r
 114                 this.m02 = w02 / w;\r
 115                 this.m12 = w12 / w;\r
 116                 this.m22 = w22 / w;\r
 117                 return;\r
 118         }\r
 119 \r
 120         /**\r
 121          * int arGetAngle( double rot[3][3], double *wa, double *wb, double *wc ) Optimize:2008.04.20:STEP[481→433] 3x3変換行列から、回転角を復元して返します。\r
 122          * \r
 123          * @param o_angle\r
 124          * @return\r
 125          */\r
 126         public final void getAngle(final NyARDoublePoint3d o_angle)\r
 127         {\r
 128                 double a, b, c;\r
 129                 double sina, cosa, sinb, cosb, sinc, cosc;\r
 130 \r
 131                 if (this.m22 > 1.0) {// <Optimize/>if( rot[2][2] > 1.0 ) {\r
 132                         this.m22 = 1.0;// <Optimize/>rot[2][2] = 1.0;\r
 133                 } else if (this.m22 < -1.0) {// <Optimize/>}else if( rot[2][2] < -1.0 ) {\r
 134                         this.m22 = -1.0;// <Optimize/>rot[2][2] = -1.0;\r
 135                 }\r
 136                 cosb = this.m22;// <Optimize/>cosb = rot[2][2];\r
 137                 b = Math.acos(cosb);\r
 138                 sinb = this._sin_table_ref.sin(b);\r
 139                 final double rot02 = this.m02;\r
 140                 final double rot12 = this.m12;\r
 141                 if (b >= 0.000001 || b <= -0.000001) {\r
 142                         cosa = rot02 / sinb;// <Optimize/>cosa = rot[0][2] / sinb;\r
 143                         sina = rot12 / sinb;// <Optimize/>sina = rot[1][2] / sinb;\r
 144                         if (cosa > 1.0) {\r
 145                                 /* printf("cos(alph) = %f\n", cosa); */\r
 146                                 cosa = 1.0;\r
 147                                 sina = 0.0;\r
 148                         }\r
 149                         if (cosa < -1.0) {\r
 150                                 /* printf("cos(alph) = %f\n", cosa); */\r
 151                                 cosa = -1.0;\r
 152                                 sina = 0.0;\r
 153                         }\r
 154                         if (sina > 1.0) {\r
 155                                 /* printf("sin(alph) = %f\n", sina); */\r
 156                                 sina = 1.0;\r
 157                                 cosa = 0.0;\r
 158                         }\r
 159                         if (sina < -1.0) {\r
 160                                 /* printf("sin(alph) = %f\n", sina); */\r
 161                                 sina = -1.0;\r
 162                                 cosa = 0.0;\r
 163                         }\r
 164                         a = Math.acos(cosa);\r
 165                         if (sina < 0) {\r
 166                                 a = -a;\r
 167                         }\r
 168                         // <Optimize>\r
 169                         // sinc = (rot[2][1]*rot[0][2]-rot[2][0]*rot[1][2])/(rot[0][2]*rot[0][2]+rot[1][2]*rot[1][2]);\r
 170                         // cosc = -(rot[0][2]*rot[2][0]+rot[1][2]*rot[2][1])/(rot[0][2]*rot[0][2]+rot[1][2]*rot[1][2]);\r
 171                         final double tmp = (rot02 * rot02 + rot12 * rot12);\r
 172                         sinc = (this.m21 * rot02 - this.m20 * rot12) / tmp;\r
 173                         cosc = -(rot02 * this.m20 + rot12 * this.m21) / tmp;\r
 174                         // </Optimize>\r
 175 \r
 176                         if (cosc > 1.0) {\r
 177                                 /* printf("cos(r) = %f\n", cosc); */\r
 178                                 cosc = 1.0;\r
 179                                 sinc = 0.0;\r
 180                         }\r
 181                         if (cosc < -1.0) {\r
 182                                 /* printf("cos(r) = %f\n", cosc); */\r
 183                                 cosc = -1.0;\r
 184                                 sinc = 0.0;\r
 185                         }\r
 186                         if (sinc > 1.0) {\r
 187                                 /* printf("sin(r) = %f\n", sinc); */\r
 188                                 sinc = 1.0;\r
 189                                 cosc = 0.0;\r
 190                         }\r
 191                         if (sinc < -1.0) {\r
 192                                 /* printf("sin(r) = %f\n", sinc); */\r
 193                                 sinc = -1.0;\r
 194                                 cosc = 0.0;\r
 195                         }\r
 196                         c = Math.acos(cosc);\r
 197                         if (sinc < 0) {\r
 198                                 c = -c;\r
 199                         }\r
 200                 } else {\r
 201                         a = b = 0.0;\r
 202                         cosa = cosb = 1.0;\r
 203                         sina = sinb = 0.0;\r
 204                         cosc = this.m00;// cosc = rot[0];// <Optimize/>cosc = rot[0][0];\r
 205                         sinc = this.m01;// sinc = rot[1];// <Optimize/>sinc = rot[1][0];\r
 206                         if (cosc > 1.0) {\r
 207                                 /* printf("cos(r) = %f\n", cosc); */\r
 208                                 cosc = 1.0;\r
 209                                 sinc = 0.0;\r
 210                         }\r
 211                         if (cosc < -1.0) {\r
 212                                 /* printf("cos(r) = %f\n", cosc); */\r
 213                                 cosc = -1.0;\r
 214                                 sinc = 0.0;\r
 215                         }\r
 216                         if (sinc > 1.0) {\r
 217                                 /* printf("sin(r) = %f\n", sinc); */\r
 218                                 sinc = 1.0;\r
 219                                 cosc = 0.0;\r
 220                         }\r
 221                         if (sinc < -1.0) {\r
 222                                 /* printf("sin(r) = %f\n", sinc); */\r
 223                                 sinc = -1.0;\r
 224                                 cosc = 0.0;\r
 225                         }\r
 226                         c = Math.acos(cosc);\r
 227                         if (sinc < 0) {\r
 228                                 c = -c;\r
 229                         }\r
 230                 }\r
 231                 o_angle.x = a;// wa.value=a;//*wa = a;\r
 232                 o_angle.y = b;// wb.value=b;//*wb = b;\r
 233                 o_angle.z = c;// wc.value=c;//*wc = c;\r
 234                 return;\r
 235         }\r
 236 \r
 237         /**\r
 238          * 回転角から回転行列を計算してセットします。\r
 239          * \r
 240          * @param i_x\r
 241          * @param i_y\r
 242          * @param i_z\r
 243          */\r
 244         public final void setAngle(final double i_x, final double i_y, final double i_z)\r
 245         {\r
 246                 /*\r
 247                  * |cos(a) -sin(a) 0| |cos(b) 0 sin(b)| |cos(a-c) sin(a-c) 0| rot = |sin(a) cos(a) 0| |0 1 0 | |-sin(a-c) cos(a-c) 0| |0 0 1| |-sin(b) 0 cos(b)| |0 0 1|\r
 248                  */\r
 249 \r
 250                 double Sa, Sb, Ca, Cb, Sac, Cac, CaCb, SaCb;\r
 251                 Sa = this._sin_table_ref.sin(i_x);\r
 252                 Ca = this._sin_table_ref.cos(i_x);\r
 253                 Sb = this._sin_table_ref.sin(i_y);\r
 254                 Cb = this._sin_table_ref.cos(i_y);\r
 255                 Sac = this._sin_table_ref.sin(i_x - i_z);\r
 256                 Cac = this._sin_table_ref.cos(i_x - i_z);\r
 257                 CaCb = Ca * Cb;\r
 258                 SaCb = Sa * Cb;\r
 259 \r
 260                 this.m00 = CaCb * Cac + Sa * Sac;\r
 261                 this.m01 = CaCb * Sac - Sa * Cac;\r
 262                 this.m02 = Ca * Sb;\r
 263                 this.m10 = SaCb * Cac - Ca * Sac;\r
 264                 this.m11 = SaCb * Sac + Ca * Cac;\r
 265                 this.m12 = Sa * Sb;\r
 266                 this.m20 = -Sb * Cac;\r
 267                 this.m21 = -Sb * Sac;\r
 268                 this.m22 = Cb;\r
 269 \r
 270                 return;\r
 271         }\r
 272 \r
 273         /**\r
 274          * i_in_pointを変換行列で座標変換する。\r
 275          * \r
 276          * @param i_in_point\r
 277          * @param i_out_point\r
 278          */\r
 279         public final void getPoint3d(final NyARDoublePoint3d i_in_point, final NyARDoublePoint3d i_out_point)\r
 280         {\r
 281                 final double x = i_in_point.x;\r
 282                 final double y = i_in_point.y;\r
 283                 final double z = i_in_point.z;\r
 284                 i_out_point.x = this.m00 * x + this.m01 * y + this.m02 * z;\r
 285                 i_out_point.y = this.m10 * x + this.m11 * y + this.m12 * z;\r
 286                 i_out_point.z = this.m20 * x + this.m21 * y + this.m22 * z;\r
 287                 return;\r
 288         }\r
 289 \r
 290         /**\r
 291          * 複数の頂点を一括して変換する\r
 292          * \r
 293          * @param i_in_point\r
 294          * @param i_out_point\r
 295          * @param i_number_of_vertex\r
 296          */\r
 297         public final void getPoint3dBatch(final NyARDoublePoint3d[] i_in_point, NyARDoublePoint3d[] i_out_point, int i_number_of_vertex)\r
 298         {\r
 299                 for (int i = i_number_of_vertex - 1; i >= 0; i--) {\r
 300                         final NyARDoublePoint3d out_ptr = i_out_point[i];\r
 301                         final NyARDoublePoint3d in_ptr = i_in_point[i];\r
 302                         final double x = in_ptr.x;\r
 303                         final double y = in_ptr.y;\r
 304                         final double z = in_ptr.z;\r
 305                         out_ptr.x = this.m00 * x + this.m01 * y + this.m02 * z;\r
 306                         out_ptr.y = this.m10 * x + this.m11 * y + this.m12 * z;\r
 307                         out_ptr.z = this.m20 * x + this.m21 * y + this.m22 * z;\r
 308                 }\r
 309                 return;\r
 310         }\r
 311 }\r