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