lib/src/jp/nyatla/nyartoolkit/core/types/matrix/NyARDoubleMatrix44.java

   1 /* \r
   2  * PROJECT: NyARToolkit(Extension)\r
   3  * --------------------------------------------------------------------------------\r
   4  * The NyARToolkit is Java edition ARToolKit class library.\r
   5  * Copyright (C)2008-2009 Ryo Iizuka\r
   6  *\r
   7  * This program is free software: you can redistribute it and/or modify\r
   8  * it under the terms of the GNU General Public License as published by\r
   9  * the Free Software Foundation, either version 3 of the License, or\r
  10  * (at your option) any later version.\r
  11  * \r
  12  * This program is distributed in the hope that it will be useful,\r
  13  * but WITHOUT ANY WARRANTY; without even the implied warranty of\r
  14  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the\r
  15  * GNU General Public License for more details.\r
  16  *\r
  17  * You should have received a copy of the GNU General Public License\r
  18  * along with this program.  If not, see <http://www.gnu.org/licenses/>.\r
  19  * \r
  20  * For further information please contact.\r
  21  *      http://nyatla.jp/nyatoolkit/\r
  22  *      <airmail(at)ebony.plala.or.jp> or <nyatla(at)nyatla.jp>\r
  23  * \r
  24  */\r
  25 package jp.nyatla.nyartoolkit.core.types.matrix;\r
  26 \r
  27 import jp.nyatla.nyartoolkit.core.types.NyARDoublePoint3d;\r
  28 \r
  29 \r
  30 \r
  31 public class NyARDoubleMatrix44 implements INyARDoubleMatrix\r
  32 {\r
  33         /** 行列の要素値です。*/\r
  34         public double m00;\r
  35         /** 行列の要素値です。*/\r
  36         public double m01;\r
  37         /** 行列の要素値です。*/\r
  38         public double m02;\r
  39         /** 行列の要素値です。*/\r
  40         public double m03;\r
  41         /** 行列の要素値です。*/\r
  42         public double m10;\r
  43         /** 行列の要素値です。*/\r
  44         public double m11;\r
  45         /** 行列の要素値です。*/\r
  46         public double m12;\r
  47         /** 行列の要素値です。*/\r
  48         public double m13;\r
  49         /** 行列の要素値です。*/\r
  50         public double m20;\r
  51         /** 行列の要素値です。*/\r
  52         public double m21;\r
  53         /** 行列の要素値です。*/\r
  54         public double m22;\r
  55         /** 行列の要素値です。*/\r
  56         public double m23;\r
  57         /** 行列の要素値です。*/\r
  58         public double m30;\r
  59         /** 行列の要素値です。*/\r
  60         public double m31;\r
  61         /** 行列の要素値です。*/\r
  62         public double m32;\r
  63         /** 行列の要素値です。*/\r
  64         public double m33;\r
  65         /**\r
  66          * この関数は、オブジェクトの配列を生成して返します。\r
  67          * @param i_number\r
  68          * 配列の長さ\r
  69          * @return\r
  70          * 新しいオブジェクト配列\r
  71          */     \r
  72         public static NyARDoubleMatrix44[] createArray(int i_number)\r
  73         {\r
  74                 NyARDoubleMatrix44[] ret=new NyARDoubleMatrix44[i_number];\r
  75                 for(int i=0;i<i_number;i++)\r
  76                 {\r
  77                         ret[i]=new NyARDoubleMatrix44();\r
  78                 }\r
  79                 return ret;\r
  80         }\r
  81         /**\r
  82          * この関数は、要素数16の配列を、行列にセットします。\r
  83          * 00,01,02,03,10...の順です。\r
  84          */\r
  85         public void setValue(double[] i_value)\r
  86         {\r
  87                 this.m00=i_value[ 0];\r
  88                 this.m01=i_value[ 1];\r
  89                 this.m02=i_value[ 2];\r
  90                 this.m03=i_value[ 3];\r
  91                 this.m10=i_value[ 4];\r
  92                 this.m11=i_value[ 5];\r
  93                 this.m12=i_value[ 6];\r
  94                 this.m13=i_value[ 7];\r
  95                 this.m20=i_value[ 8];\r
  96                 this.m21=i_value[ 9];\r
  97                 this.m22=i_value[10];\r
  98                 this.m23=i_value[11];\r
  99                 this.m30=i_value[12];\r
 100                 this.m31=i_value[13];\r
 101                 this.m32=i_value[14];\r
 102                 this.m33=i_value[15];\r
 103                 return;\r
 104         }\r
 105         /**\r
 106          * この関数は、オブジェクトの内容をインスタンスにコピーします。\r
 107          * @param i_value\r
 108          * コピー元のオブジェクト\r
 109          */\r
 110         public void setValue(NyARDoubleMatrix44 i_value)\r
 111         {\r
 112                 this.m00=i_value.m00;\r
 113                 this.m01=i_value.m01;\r
 114                 this.m02=i_value.m02;\r
 115                 this.m03=i_value.m03;\r
 116                 this.m10=i_value.m10;\r
 117                 this.m11=i_value.m11;\r
 118                 this.m12=i_value.m12;\r
 119                 this.m13=i_value.m13;\r
 120                 this.m20=i_value.m20;\r
 121                 this.m21=i_value.m21;\r
 122                 this.m22=i_value.m22;\r
 123                 this.m23=i_value.m23;\r
 124                 this.m30=i_value.m30;\r
 125                 this.m31=i_value.m31;\r
 126                 this.m32=i_value.m32;\r
 127                 this.m33=i_value.m33;\r
 128                 return;\r
 129         }\r
 130         /**\r
 131          * この関数は、要素数16の配列に、行列の内容をコピーします。\r
 132          * 順番は、00,01,02,03,10...の順です。\r
 133          */     \r
 134         public void getValue(double[] o_value)\r
 135         {\r
 136                 o_value[ 0]=this.m00;\r
 137                 o_value[ 1]=this.m01;\r
 138                 o_value[ 2]=this.m02;\r
 139                 o_value[ 3]=this.m03;\r
 140                 o_value[ 4]=this.m10;\r
 141                 o_value[ 5]=this.m11;\r
 142                 o_value[ 6]=this.m12;\r
 143                 o_value[ 7]=this.m13;\r
 144                 o_value[ 8]=this.m20;\r
 145                 o_value[ 9]=this.m21;\r
 146                 o_value[10]=this.m22;\r
 147                 o_value[11]=this.m23;\r
 148                 o_value[12]=this.m30;\r
 149                 o_value[13]=this.m31;\r
 150                 o_value[14]=this.m32;\r
 151                 o_value[15]=this.m33;\r
 152                 return;\r
 153         }\r
 154         /**\r
 155          * この関数は、要素数16の配列に、行列の内容を転置してからコピーします。\r
 156          * 順番は、00,10,20,30,01...の順です。\r
 157          * @param o_value\r
 158          * 値を受け取る配列\r
 159          */     \r
 160         public void getValueT(double[] o_value)\r
 161         {\r
 162                 o_value[ 0]=this.m00;\r
 163                 o_value[ 1]=this.m10;\r
 164                 o_value[ 2]=this.m20;\r
 165                 o_value[ 3]=this.m30;\r
 166                 o_value[ 4]=this.m01;\r
 167                 o_value[ 5]=this.m11;\r
 168                 o_value[ 6]=this.m21;\r
 169                 o_value[ 7]=this.m31;\r
 170                 o_value[ 8]=this.m02;\r
 171                 o_value[ 9]=this.m12;\r
 172                 o_value[10]=this.m22;\r
 173                 o_value[11]=this.m32;\r
 174                 o_value[12]=this.m03;\r
 175                 o_value[13]=this.m13;\r
 176                 o_value[14]=this.m23;\r
 177                 o_value[15]=this.m33;\r
 178                 return;\r
 179         }\r
 180         /**\r
 181          * この関数は、逆行列を計算して、インスタンスにセットします。\r
 182          * @param i_src\r
 183          * 逆行列を計算するオブジェクト。thisを指定できます。\r
 184          * @return\r
 185          * 逆行列を得られると、trueを返します。\r
 186          */\r
 187         public boolean inverse(NyARDoubleMatrix44 i_src)\r
 188         {\r
 189                 final double a11,a12,a13,a14,a21,a22,a23,a24,a31,a32,a33,a34,a41,a42,a43,a44;\r
 190                 final double b11,b12,b13,b14,b21,b22,b23,b24,b31,b32,b33,b34,b41,b42,b43,b44;   \r
 191                 double t1,t2,t3,t4,t5,t6;\r
 192                 a11=i_src.m00;a12=i_src.m01;a13=i_src.m02;a14=i_src.m03;\r
 193                 a21=i_src.m10;a22=i_src.m11;a23=i_src.m12;a24=i_src.m13;\r
 194                 a31=i_src.m20;a32=i_src.m21;a33=i_src.m22;a34=i_src.m23;\r
 195                 a41=i_src.m30;a42=i_src.m31;a43=i_src.m32;a44=i_src.m33;\r
 196                 \r
 197                 t1=a33*a44-a34*a43;\r
 198                 t2=a34*a42-a32*a44;\r
 199                 t3=a32*a43-a33*a42;\r
 200                 t4=a34*a41-a31*a44;\r
 201                 t5=a31*a43-a33*a41;\r
 202                 t6=a31*a42-a32*a41;\r
 203                 \r
 204                 b11=a22*t1+a23*t2+a24*t3;\r
 205                 b21=-(a23*t4+a24*t5+a21*t1);\r
 206                 b31=a24*t6-a21*t2+a22*t4;\r
 207                 b41=-(a21*t3-a22*t5+a23*t6);\r
 208                 \r
 209                 t1=a43*a14-a44*a13;\r
 210                 t2=a44*a12-a42*a14;\r
 211                 t3=a42*a13-a43*a12;\r
 212                 t4=a44*a11-a41*a14;\r
 213                 t5=a41*a13-a43*a11;\r
 214                 t6=a41*a12-a42*a11;\r
 215 \r
 216                 b12=-(a32*t1+a33*t2+a34*t3);\r
 217                 b22=a33*t4+a34*t5+a31*t1;\r
 218                 b32=-(a34*t6-a31*t2+a32*t4);\r
 219                 b42=a31*t3-a32*t5+a33*t6;\r
 220                 \r
 221                 t1=a13*a24-a14*a23;\r
 222                 t2=a14*a22-a12*a24;\r
 223                 t3=a12*a23-a13*a22;\r
 224                 t4=a14*a21-a11*a24;\r
 225                 t5=a11*a23-a13*a21;\r
 226                 t6=a11*a22-a12*a21;\r
 227 \r
 228                 b13=a42*t1+a43*t2+a44*t3;\r
 229                 b23=-(a43*t4+a44*t5+a41*t1);\r
 230                 b33=a44*t6-a41*t2+a42*t4;\r
 231                 b43=-(a41*t3-a42*t5+a43*t6);\r
 232 \r
 233                 t1=a23*a34-a24*a33;\r
 234                 t2=a24*a32-a22*a34;\r
 235                 t3=a22*a33-a23*a32;\r
 236                 t4=a24*a31-a21*a34;             \r
 237                 t5=a21*a33-a23*a31;\r
 238                 t6=a21*a32-a22*a31;\r
 239 \r
 240                 b14=-(a12*t1+a13*t2+a14*t3);\r
 241                 b24=a13*t4+a14*t5+a11*t1;\r
 242                 b34=-(a14*t6-a11*t2+a12*t4);\r
 243                 b44=a11*t3-a12*t5+a13*t6;\r
 244                 \r
 245                 double det_1=(a11*b11+a21*b12+a31*b13+a41*b14);\r
 246                 if(det_1==0){\r
 247                         return false;\r
 248                 }\r
 249                 det_1=1/det_1;\r
 250 \r
 251                 this.m00=b11*det_1;\r
 252                 this.m01=b12*det_1;\r
 253                 this.m02=b13*det_1;\r
 254                 this.m03=b14*det_1;\r
 255                 \r
 256                 this.m10=b21*det_1;\r
 257                 this.m11=b22*det_1;\r
 258                 this.m12=b23*det_1;\r
 259                 this.m13=b24*det_1;\r
 260                 \r
 261                 this.m20=b31*det_1;\r
 262                 this.m21=b32*det_1;\r
 263                 this.m22=b33*det_1;\r
 264                 this.m23=b34*det_1;\r
 265                 \r
 266                 this.m30=b41*det_1;\r
 267                 this.m31=b42*det_1;\r
 268                 this.m32=b43*det_1;\r
 269                 this.m33=b44*det_1;\r
 270                 \r
 271                 return true;\r
 272         }\r
 273         /**\r
 274          * この関数は、3次元座標を座標変換します。\r
 275          * 4列目は1と仮定します。\r
 276          * @param i_x\r
 277          * 変換する三次元座標(X)\r
 278          * @param i_y\r
 279          * 変換する三次元座標(Y)\r
 280          * @param i_z\r
 281          * 変換する三次元座標(Z)\r
 282          * @param o_out\r
 283          * 変換後の座標を受け取るオブジェクト\r
 284          */\r
 285         public final void transform3d(double i_x,double i_y,double i_z,NyARDoublePoint3d o_out)\r
 286         {\r
 287                 o_out.x=this.m00*i_x+this.m01*i_y+this.m02*i_z+this.m03;\r
 288                 o_out.y=this.m10*i_x+this.m11*i_y+this.m12*i_z+this.m13;\r
 289                 o_out.z=this.m20*i_x+this.m21*i_y+this.m22*i_z+this.m23;\r
 290                 return;\r
 291         }\r
 292         /**\r
 293          * この関数は、3次元座標を座標変換します。\r
 294          * 4列目は1と仮定します。\r
 295          * @param i_in\r
 296          * 返還前する座標値\r
 297          * @param o_out\r
 298          * 変換後の座標を受け取るオブジェクト\r
 299          */     \r
 300         public final void transform3d(NyARDoublePoint3d i_in,NyARDoublePoint3d o_out)\r
 301         {\r
 302                 transform3d(i_in.x,i_in.y,i_in.z,o_out);\r
 303         }\r
 304         /**\r
 305          * この関数は、行列の回転成分から、ZXY系の角度値を計算します。\r
 306          * @param o_out\r
 307          *　角度値を受け取るオブジェクトです。\r
 308          * 角度値の範囲は、0-PIです。\r
 309          */\r
 310         public final void getZXYAngle(NyARDoublePoint3d o_out)\r
 311         {\r
 312                 double sina = this.m21;\r
 313                 if (sina >= 1.0) {\r
 314                         o_out.x = Math.PI / 2;\r
 315                         o_out.y = 0;\r
 316                         o_out.z = Math.atan2(-this.m10, this.m00);\r
 317                 } else if (sina <= -1.0) {\r
 318                         o_out.x = -Math.PI / 2;\r
 319                         o_out.y = 0;\r
 320                         o_out.z = Math.atan2(-this.m10, this.m00);\r
 321                 } else {\r
 322                         o_out.x = Math.asin(sina);\r
 323                         o_out.z = Math.atan2(-this.m01, this.m11);\r
 324                         o_out.y = Math.atan2(-this.m20, this.m22);\r
 325                 }\r
 326         }\r
 327         /**\r
 328          * この関数は、行列同士の掛け算をして、インスタンスに格納します。\r
 329          *　i_mat_lとi_mat_rには、thisを指定しないでください。\r
 330          * @param i_mat_l\r
 331          * 左成分の行列\r
 332          * @param i_mat_r\r
 333          * 右成分の行列\r
 334          */\r
 335         public final void mul(NyARDoubleMatrix44 i_mat_l,NyARDoubleMatrix44 i_mat_r)\r
 336         {\r
 337                 assert(this!=i_mat_l);\r
 338                 assert(this!=i_mat_r);\r
 339                 this.m00=i_mat_l.m00*i_mat_r.m00 + i_mat_l.m01*i_mat_r.m10 + i_mat_l.m02*i_mat_r.m20 + i_mat_l.m03*i_mat_r.m30;\r
 340                 this.m01=i_mat_l.m00*i_mat_r.m01 + i_mat_l.m01*i_mat_r.m11 + i_mat_l.m02*i_mat_r.m21 + i_mat_l.m03*i_mat_r.m31;\r
 341                 this.m02=i_mat_l.m00*i_mat_r.m02 + i_mat_l.m01*i_mat_r.m12 + i_mat_l.m02*i_mat_r.m22 + i_mat_l.m03*i_mat_r.m32;\r
 342                 this.m03=i_mat_l.m00*i_mat_r.m03 + i_mat_l.m01*i_mat_r.m13 + i_mat_l.m02*i_mat_r.m23 + i_mat_l.m03*i_mat_r.m33;\r
 343 \r
 344                 this.m10=i_mat_l.m10*i_mat_r.m00 + i_mat_l.m11*i_mat_r.m10 + i_mat_l.m12*i_mat_r.m20 + i_mat_l.m13*i_mat_r.m30;\r
 345                 this.m11=i_mat_l.m10*i_mat_r.m01 + i_mat_l.m11*i_mat_r.m11 + i_mat_l.m12*i_mat_r.m21 + i_mat_l.m13*i_mat_r.m31;\r
 346                 this.m12=i_mat_l.m10*i_mat_r.m02 + i_mat_l.m11*i_mat_r.m12 + i_mat_l.m12*i_mat_r.m22 + i_mat_l.m13*i_mat_r.m32;\r
 347                 this.m13=i_mat_l.m10*i_mat_r.m03 + i_mat_l.m11*i_mat_r.m13 + i_mat_l.m12*i_mat_r.m23 + i_mat_l.m13*i_mat_r.m33;\r
 348 \r
 349                 this.m20=i_mat_l.m20*i_mat_r.m00 + i_mat_l.m21*i_mat_r.m10 + i_mat_l.m22*i_mat_r.m20 + i_mat_l.m23*i_mat_r.m30;\r
 350                 this.m21=i_mat_l.m20*i_mat_r.m01 + i_mat_l.m21*i_mat_r.m11 + i_mat_l.m22*i_mat_r.m21 + i_mat_l.m23*i_mat_r.m31;\r
 351                 this.m22=i_mat_l.m20*i_mat_r.m02 + i_mat_l.m21*i_mat_r.m12 + i_mat_l.m22*i_mat_r.m22 + i_mat_l.m23*i_mat_r.m32;\r
 352                 this.m23=i_mat_l.m20*i_mat_r.m03 + i_mat_l.m21*i_mat_r.m13 + i_mat_l.m22*i_mat_r.m23 + i_mat_l.m23*i_mat_r.m33;\r
 353 \r
 354                 this.m30=i_mat_l.m30*i_mat_r.m00 + i_mat_l.m31*i_mat_r.m10 + i_mat_l.m32*i_mat_r.m20 + i_mat_l.m33*i_mat_r.m30;\r
 355                 this.m31=i_mat_l.m30*i_mat_r.m01 + i_mat_l.m31*i_mat_r.m11 + i_mat_l.m32*i_mat_r.m21 + i_mat_l.m33*i_mat_r.m31;\r
 356                 this.m32=i_mat_l.m30*i_mat_r.m02 + i_mat_l.m31*i_mat_r.m12 + i_mat_l.m32*i_mat_r.m22 + i_mat_l.m33*i_mat_r.m32;\r
 357                 this.m33=i_mat_l.m30*i_mat_r.m03 + i_mat_l.m31*i_mat_r.m13 + i_mat_l.m32*i_mat_r.m23 + i_mat_l.m33*i_mat_r.m33; \r
 358                 return;\r
 359         }\r
 360         /**\r
 361          * この関数は、行列を単位行列にします。\r
 362          */\r
 363         public final void identity()\r
 364         {\r
 365                 this.m00=this.m11=this.m22=this.m33=1;\r
 366                 this.m01=this.m02=this.m03=this.m10=this.m12=this.m13=this.m20=this.m21=this.m23=this.m30=this.m31=this.m32=0;\r
 367                 return;\r
 368         }\r
 369         /**\r
 370          * この関数は、行列に右手系のX軸回転を設定します。\r
 371          * @param i_radian\r
 372          * 設定するX回転角(radian)\r
 373          */\r
 374         public final void setRotateX(double i_radian)\r
 375         {\r
 376                 double s=Math.sin(i_radian);\r
 377                 double c=Math.cos(i_radian);\r
 378                 this.identity();\r
 379                 this.m11=c;\r
 380                 this.m12=-s;\r
 381                 this.m21=s;\r
 382                 this.m22=c;\r
 383                 return;\r
 384         }\r
 385         /**\r
 386          * この関数は、行列に右手系のY軸回転を設定します。\r
 387          * @param i_radian\r
 388          * 設定するY回転角(radian)\r
 389          */\r
 390         public final void setRotateY(double i_radian)\r
 391         {\r
 392                 double s=Math.sin(i_radian);\r
 393                 double c=Math.cos(i_radian);\r
 394                 this.identity();\r
 395                 this.m00=c;\r
 396                 this.m02=s;\r
 397                 this.m20=-s;\r
 398                 this.m22=c;\r
 399         }       \r
 400         /**\r
 401          * この関数は、行列に右手系のZ軸回転を設定します。\r
 402          * @param i_radian\r
 403          * 設定するZ回転角(radian)\r
 404          */\r
 405         public final void setRotateZ(double i_radian)\r
 406         {\r
 407                 double s=Math.sin(i_radian);\r
 408                 double c=Math.cos(i_radian);\r
 409                 this.identity();\r
 410                 this.m00=c;\r
 411                 this.m01=-s;\r
 412                 this.m10=s;\r
 413                 this.m11=c;\r
 414                 \r
 415         }\r
 416         /**\r
 417          * この関数は、行列に右手系の平行移動量を設定します。\r
 418          * @param i_x\r
 419          * x軸方向の平行移動量\r
 420          * @param i_y\r
 421          * y軸方向の平行移動量\r
 422          * @param i_z\r
 423          * z軸方向の平行移動量\r
 424          */\r
 425         public final void setTranslate(double i_x,double i_y,double i_z)\r
 426         {\r
 427                 this.identity();\r
 428                 this.m03=i_x;\r
 429                 this.m13=i_y;\r
 430                 this.m23=i_z;\r
 431         }\r
 432         /**\r
 433          * この関数は、現在の行列をX軸で回転します。\r
 434          * @param i_radian\r
 435          * 回転量(radian)\r
 436          */\r
 437         public final void rotateX(double i_radian)\r
 438         {\r
 439                 double s=Math.sin(i_radian);\r
 440                 double c=Math.cos(i_radian);\r
 441                 \r
 442                 double t1,t2;\r
 443                 t1=this.m01;t2=this.m02;\r
 444                 this.m01=t1*c + t2*s;\r
 445                 this.m02=t1*(-s) + t2*c;\r
 446                 t1=this.m11;t2=this.m12;\r
 447                 this.m11=t1*c + t2*s;\r
 448                 this.m12=t1*(-s) + t2*c;\r
 449                 t1=this.m21;t2=this.m22;\r
 450                 this.m21=t1*c + t2*s;\r
 451                 this.m22=t1*(-s) + t2*c;                \r
 452                 t1=this.m31;t2=this.m32;\r
 453                 this.m31=t1*c + t2*s;\r
 454                 this.m32=t1*(-s) + t2*c;\r
 455         }\r
 456         /**\r
 457          * この関数は、現在の行列をY軸で回転します。\r
 458          * @param i_radian\r
 459          * 回転量(radian)\r
 460          */\r
 461         public final void rotateY(double i_radian)\r
 462         {\r
 463                 double s=Math.sin(i_radian);\r
 464                 double c=Math.cos(i_radian);\r
 465 \r
 466                 double t1,t2;\r
 467                 t1=this.m00;t2=this.m02;\r
 468                 this.m00=t1*c + t2*(-s);\r
 469                 this.m02=t1*s + t2*c;\r
 470                 t1=this.m10;t2=this.m12;\r
 471                 this.m10=t1*c + t2*(-s);\r
 472                 this.m12=t1*s + t2*c;\r
 473                 t1=this.m20;t2=this.m22;\r
 474                 this.m20=t1*c + t2*(-s);\r
 475                 this.m22=t1*s + t2*c;\r
 476                 t1=this.m30;t2=this.m32;\r
 477                 this.m30=t1*c + t2*(-s);\r
 478                 this.m32=t1*s + t2*c;\r
 479         }\r
 480         /**\r
 481          * この関数は、現在の行列をZ軸で回転します。\r
 482          * @param i_radian\r
 483          * 回転量(radian)\r
 484          */\r
 485         public final void rotateZ(double i_radian)\r
 486         {\r
 487                 double s=Math.sin(i_radian);\r
 488                 double c=Math.cos(i_radian);\r
 489                 \r
 490                 double t1,t2;\r
 491                 t1=this.m00;t2=this.m01;\r
 492                 this.m00=t1*c + t2*s;\r
 493                 this.m01=t1*(-s) + t2*c;\r
 494 \r
 495                 t1=this.m10;t2=this.m11;\r
 496                 this.m10=t1*c + t2*s;\r
 497                 this.m11=t1*(-s) + t2*c;\r
 498 \r
 499                 t1=this.m20;t2=this.m21;\r
 500                 this.m20=t1*c + t2*s;\r
 501                 this.m21=t1*(-s) + t2*c;\r
 502 \r
 503                 t1=this.m30;t2=this.m31;\r
 504                 this.m30=t1*c + t2*s;\r
 505                 this.m31=t1*(-s) + t2*c;\r
 506                 return;\r
 507         }\r
 508         /**\r
 509          * この関数は、現在の行列を平行移動します。\r
 510          * @param i_x\r
 511          * X方向の平行移動量\r
 512          * @param i_y\r
 513          * Y方向の平行移動量\r
 514          * @param i_z\r
 515          * Z方向の平行移動量\r
 516          */\r
 517         public final void translate(double i_x,double i_y,double i_z)\r
 518         {\r
 519                 this.m03=this.m00*i_x + this.m01*i_y + this.m02*i_z + this.m03;\r
 520                 this.m13=this.m10*i_x + this.m11*i_y + this.m12*i_z + this.m13;\r
 521                 this.m23=this.m20*i_x + this.m21*i_y + this.m22*i_z + this.m23;\r
 522                 this.m33=this.m30*i_x + this.m31*i_y + this.m32*i_z + this.m33;\r
 523                 return;\r
 524         }\r
 525         /**\r
 526          * テストプログラム\r
 527          * @param args\r
 528          */\r
 529         public static void main(String[] args)\r
 530         {\r
 531 \r
 532                 try {\r
 533                         NyARDoubleMatrix44 t1 = new NyARDoubleMatrix44();\r
 534                         NyARDoubleMatrix44 t2 = new NyARDoubleMatrix44();\r
 535                         NyARDoubleMatrix44 t3 = new NyARDoubleMatrix44();\r
 536                         NyARDoubleMatrix44 t4 = new NyARDoubleMatrix44();\r
 537                         t1.m00=Math.random()*100;\r
 538                         t1.m01=Math.random()*100;\r
 539                         t1.m02=Math.random()*100;\r
 540                         t1.m03=Math.random()*100;\r
 541                         t1.m10=Math.random()*100;\r
 542                         t1.m11=Math.random()*100;\r
 543                         t1.m12=Math.random()*100;\r
 544                         t1.m13=Math.random()*100;\r
 545                         t1.m20=Math.random()*100;\r
 546                         t1.m21=Math.random()*100;\r
 547                         t1.m22=Math.random()*100;\r
 548                         t1.m23=Math.random()*100;\r
 549                         t1.m30=Math.random()*100;\r
 550                         t1.m31=Math.random()*100;\r
 551                         t1.m32=Math.random()*100;\r
 552                         t1.m33=Math.random()*100;\r
 553                         t2.setValue(t1);\r
 554                         t1.rotateZ(Math.PI*10/180);\r
 555                         t4.setRotateZ(Math.PI*10/180);\r
 556                         t3.mul(t2,t4);\r
 557                         System.out.print("");\r
 558                 } catch (Exception e) {\r
 559                         e.printStackTrace();\r
 560                 }\r
 561         }       \r
 562         \r
 563 }\r