tags/2.4.2/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 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 modify\r
  14  * it under the terms of the GNU General Public License as published by\r
  15  * the Free Software Foundation, either version 3 of the License, or\r
  16  * (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 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.types.*;\r
  35 import jp.nyatla.nyartoolkit.core.param.*;\r
  36 /**\r
  37  * 回転行列計算用の、3x3行列\r
  38  *\r
  39  */\r
  40 public class NyARRotMatrix_ARToolKit extends NyARRotMatrix\r
  41 {       \r
  42         /**\r
  43          * インスタンスを準備します。\r
  44          * \r
  45          * @param i_param\r
  46          */\r
  47         public NyARRotMatrix_ARToolKit(NyARPerspectiveProjectionMatrix i_matrix) throws NyARException\r
  48         {\r
  49                 super(i_matrix);\r
  50                 this._angle=new NyARDoublePoint3d();\r
  51                 return;\r
  52         }\r
  53         final protected NyARDoublePoint3d _angle;\r
  54         \r
  55 \r
  56         \r
  57         public final void initRotBySquare(final NyARLinear[] i_linear,final NyARDoublePoint2d[] i_sqvertex) throws NyARException\r
  58         {\r
  59                 super.initRotBySquare(i_linear,i_sqvertex);\r
  60                 //Matrixからangleをロード\r
  61                 this.updateAngleFromMatrix();\r
  62                 return;\r
  63         }\r
  64         public final NyARDoublePoint3d refAngle()\r
  65         {\r
  66                 return this._angle;\r
  67         }\r
  68         /**\r
  69          * 回転角から回転行列を計算してセットします。\r
  70          * @param i_x\r
  71          * @param i_y\r
  72          * @param i_z\r
  73          */\r
  74         public void setAngle(final double i_x, final double i_y, final double i_z)\r
  75         {\r
  76                 final double sina = Math.sin(i_x);\r
  77                 final double cosa = Math.cos(i_x);\r
  78                 final double sinb = Math.sin(i_y);\r
  79                 final double cosb = Math.cos(i_y);\r
  80                 final double sinc = Math.sin(i_z);\r
  81                 final double cosc = Math.cos(i_z);\r
  82                 // Optimize\r
  83                 final double CACA = cosa * cosa;\r
  84                 final double SASA = sina * sina;\r
  85                 final double SACA = sina * cosa;\r
  86                 final double SASB = sina * sinb;\r
  87                 final double CASB = cosa * sinb;\r
  88                 final double SACACB = SACA * cosb;\r
  89 \r
  90                 this.m00 = CACA * cosb * cosc + SASA * cosc + SACACB * sinc - SACA * sinc;\r
  91                 this.m01 = -CACA * cosb * sinc - SASA * sinc + SACACB * cosc - SACA * cosc;\r
  92                 this.m02 = CASB;\r
  93                 this.m10 = SACACB * cosc - SACA * cosc + SASA * cosb * sinc + CACA * sinc;\r
  94                 this.m11 = -SACACB * sinc + SACA * sinc + SASA * cosb * cosc + CACA * cosc;\r
  95                 this.m12 = SASB;\r
  96                 this.m20 = -CASB * cosc - SASB * sinc;\r
  97                 this.m21 = CASB * sinc - SASB * cosc;\r
  98                 this.m22 = cosb;\r
  99                 updateAngleFromMatrix();\r
 100                 return;\r
 101         }\r
 102         /**\r
 103          * 現在のMatrixからangkeを復元する。\r
 104          * @param o_angle\r
 105          */\r
 106         private final void updateAngleFromMatrix()\r
 107         {\r
 108                 double a,b,c;\r
 109                 double sina, cosa, sinb,cosb,sinc, cosc;\r
 110                 \r
 111                 if (this.m22 > 1.0) {// <Optimize/>if( rot[2][2] > 1.0 ) {\r
 112                         cosb = 1.0;// <Optimize/>rot[2][2] = 1.0;\r
 113                 } else if (this.m22 < -1.0) {// <Optimize/>}else if( rot[2][2] < -1.0 ) {\r
 114                         cosb = -1.0;// <Optimize/>rot[2][2] = -1.0;\r
 115                 }else{\r
 116                         cosb =this.m22;// <Optimize/>cosb = rot[2][2];\r
 117                 }\r
 118                 b = Math.acos(cosb);\r
 119                 sinb =Math.sin(b);\r
 120                 final double rot02=this.m02;\r
 121                 final double rot12=this.m12;\r
 122                 if (b >= 0.000001 || b <= -0.000001) {\r
 123                         cosa = rot02 / sinb;// <Optimize/>cosa = rot[0][2] / sinb;\r
 124                         sina = rot12 / sinb;// <Optimize/>sina = rot[1][2] / sinb;\r
 125                         if (cosa > 1.0) {\r
 126                                 cosa = 1.0;\r
 127                                 sina = 0.0;\r
 128                         }\r
 129                         if (cosa < -1.0) {\r
 130                                 cosa = -1.0;\r
 131                                 sina = 0.0;\r
 132                         }\r
 133                         if (sina > 1.0) {\r
 134                                 sina = 1.0;\r
 135                                 cosa = 0.0;\r
 136                         }\r
 137                         if (sina < -1.0) {\r
 138                                 sina = -1.0;\r
 139                                 cosa = 0.0;\r
 140                         }\r
 141                         a = Math.acos(cosa);\r
 142                         if (sina < 0) {\r
 143                                 a = -a;\r
 144                         }\r
 145                         final double tmp = (rot02 * rot02 + rot12 * rot12);\r
 146                         sinc = (this.m21 * rot02 - this.m20 * rot12) / tmp;\r
 147                         cosc = -(rot02 * this.m20 + rot12 * this.m21) / tmp;\r
 148 \r
 149                         if (cosc > 1.0) {\r
 150                                 cosc = 1.0;\r
 151                                 sinc = 0.0;\r
 152                         }\r
 153                         if (cosc < -1.0) {\r
 154                                 cosc = -1.0;\r
 155                                 sinc = 0.0;\r
 156                         }\r
 157                         if (sinc > 1.0) {\r
 158                                 sinc = 1.0;\r
 159                                 cosc = 0.0;\r
 160                         }\r
 161                         if (sinc < -1.0) {\r
 162                                 sinc = -1.0;\r
 163                                 cosc = 0.0;\r
 164                         }\r
 165                         c = Math.acos(cosc);\r
 166                         if (sinc < 0) {\r
 167                                 c = -c;\r
 168                         }\r
 169                 } else {\r
 170                         a = b = 0.0;\r
 171                         cosa = cosb = 1.0;\r
 172                         sina = sinb = 0.0;\r
 173                         cosc=this.m00;//cosc = rot[0];// <Optimize/>cosc = rot[0][0];\r
 174                         sinc=this.m01;//sinc = rot[1];// <Optimize/>sinc = rot[1][0];\r
 175                         if (cosc > 1.0) {\r
 176                                 cosc = 1.0;\r
 177                                 sinc = 0.0;\r
 178                         }\r
 179                         if (cosc < -1.0) {\r
 180                                 cosc = -1.0;\r
 181                                 sinc = 0.0;\r
 182                         }\r
 183                         if (sinc > 1.0) {\r
 184                                 sinc = 1.0;\r
 185                                 cosc = 0.0;\r
 186                         }\r
 187                         if (sinc < -1.0) {\r
 188                                 sinc = -1.0;\r
 189                                 cosc = 0.0;\r
 190                         }\r
 191                         c = Math.acos(cosc);\r
 192                         if (sinc < 0) {\r
 193                                 c = -c;\r
 194                         }\r
 195                 }\r
 196                 //angleの更新\r
 197                 this._angle.x = a;// wa.value=a;//*wa = a;\r
 198                 this._angle.y = b;// wb.value=b;//*wb = b;\r
 199                 this._angle.z = c;// wc.value=c;//*wc = c;\r
 200                 return;\r
 201         }       \r
 202 }\r