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 \r
  28 public class NyARDoubleMatrix44 implements INyARDoubleMatrix\r
  29 {\r
  30         public double m00;\r
  31         public double m01;\r
  32         public double m02;\r
  33         public double m03;\r
  34         public double m10;\r
  35         public double m11;\r
  36         public double m12;\r
  37         public double m13;\r
  38         public double m20;\r
  39         public double m21;\r
  40         public double m22;\r
  41         public double m23;\r
  42         public double m30;\r
  43         public double m31;\r
  44         public double m32;\r
  45         public double m33;\r
  46         public static NyARDoubleMatrix44[] createArray(int i_number)\r
  47         {\r
  48                 NyARDoubleMatrix44[] ret=new NyARDoubleMatrix44[i_number];\r
  49                 for(int i=0;i<i_number;i++)\r
  50                 {\r
  51                         ret[i]=new NyARDoubleMatrix44();\r
  52                 }\r
  53                 return ret;\r
  54         }\r
  55         /**\r
  56          * 遅いからあんまり使わないでね。\r
  57          */\r
  58         public void setValue(double[] i_value)\r
  59         {\r
  60                 this.m00=i_value[ 0];\r
  61                 this.m01=i_value[ 1];\r
  62                 this.m02=i_value[ 2];\r
  63                 this.m03=i_value[ 3];\r
  64                 this.m10=i_value[ 4];\r
  65                 this.m11=i_value[ 5];\r
  66                 this.m12=i_value[ 6];\r
  67                 this.m13=i_value[ 7];\r
  68                 this.m20=i_value[ 8];\r
  69                 this.m21=i_value[ 9];\r
  70                 this.m22=i_value[10];\r
  71                 this.m23=i_value[11];\r
  72                 this.m30=i_value[12];\r
  73                 this.m31=i_value[13];\r
  74                 this.m32=i_value[14];\r
  75                 this.m33=i_value[15];\r
  76                 return;\r
  77         }\r
  78         /**\r
  79          * 遅いからあんまり使わないでね。\r
  80          */\r
  81         public void getValue(double[] o_value)\r
  82         {\r
  83                 o_value[ 0]=this.m00;\r
  84                 o_value[ 1]=this.m01;\r
  85                 o_value[ 2]=this.m02;\r
  86                 o_value[ 3]=this.m03;\r
  87                 o_value[ 4]=this.m10;\r
  88                 o_value[ 5]=this.m11;\r
  89                 o_value[ 6]=this.m12;\r
  90                 o_value[ 7]=this.m13;\r
  91                 o_value[ 8]=this.m20;\r
  92                 o_value[ 9]=this.m21;\r
  93                 o_value[10]=this.m22;\r
  94                 o_value[11]=this.m23;\r
  95                 o_value[12]=this.m30;\r
  96                 o_value[13]=this.m31;\r
  97                 o_value[14]=this.m32;\r
  98                 o_value[15]=this.m33;\r
  99                 return;\r
 100         }\r
 101         public boolean inverse(NyARDoubleMatrix44 i_src)\r
 102         {\r
 103                 final double a11,a12,a13,a14,a21,a22,a23,a24,a31,a32,a33,a34,a41,a42,a43,a44;\r
 104                 final double b11,b12,b13,b14,b21,b22,b23,b24,b31,b32,b33,b34,b41,b42,b43,b44;   \r
 105                 double t1,t2,t3,t4,t5,t6;\r
 106                 a11=i_src.m00;a12=i_src.m01;a13=i_src.m02;a14=i_src.m03;\r
 107                 a21=i_src.m10;a22=i_src.m11;a23=i_src.m12;a24=i_src.m13;\r
 108                 a31=i_src.m20;a32=i_src.m21;a33=i_src.m22;a34=i_src.m23;\r
 109                 a41=i_src.m30;a42=i_src.m31;a43=i_src.m32;a44=i_src.m33;\r
 110                 \r
 111                 t1=a33*a44-a34*a43;\r
 112                 t2=a34*a42-a32*a44;\r
 113                 t3=a32*a43-a33*a42;\r
 114                 t4=a34*a41-a31*a44;\r
 115                 t5=a31*a43-a33*a41;\r
 116                 t6=a31*a42-a32*a41;\r
 117                 \r
 118                 b11=a22*t1+a23*t2+a24*t3;\r
 119                 b21=-(a23*t4+a24*t5+a21*t1);\r
 120                 b31=a24*t6-a21*t2+a22*t4;\r
 121                 b41=-(a21*t3-a22*t5+a23*t6);\r
 122                 \r
 123                 t1=a43*a14-a44*a13;\r
 124                 t2=a44*a12-a42*a14;\r
 125                 t3=a42*a13-a43*a12;\r
 126                 t4=a44*a11-a41*a14;\r
 127                 t5=a41*a13-a43*a11;\r
 128                 t6=a41*a12-a42*a11;\r
 129 \r
 130                 b12=-(a32*t1+a33*t2+a34*t3);\r
 131                 b22=a33*t4+a34*t5+a31*t1;\r
 132                 b32=-(a34*t6-a31*t2+a32*t4);\r
 133                 b42=a31*t3-a32*t5+a33*t6;\r
 134                 \r
 135                 t1=a13*a24-a14*a23;\r
 136                 t2=a14*a22-a12*a24;\r
 137                 t3=a12*a23-a13*a22;\r
 138                 t4=a14*a21-a11*a24;\r
 139                 t5=a11*a23-a13*a21;\r
 140                 t6=a11*a22-a12*a21;\r
 141 \r
 142                 b13=a42*t1+a43*t2+a44*t3;\r
 143                 b23=-(a43*t4+a44*t5+a41*t1);\r
 144                 b33=a44*t6-a41*t2+a42*t4;\r
 145                 b43=-(a41*t3-a42*t5+a43*t6);\r
 146 \r
 147                 t1=a23*a34-a24*a33;\r
 148                 t2=a24*a32-a22*a34;\r
 149                 t3=a22*a33-a23*a32;\r
 150                 t4=a24*a31-a21*a34;             \r
 151                 t5=a21*a33-a23*a31;\r
 152                 t6=a21*a32-a22*a31;\r
 153 \r
 154                 b14=-(a12*t1+a13*t2+a14*t3);\r
 155                 b24=a13*t4+a14*t5+a11*t1;\r
 156                 b34=-(a14*t6-a11*t2+a12*t4);\r
 157                 b44=a11*t3-a12*t5+a13*t6;\r
 158                 \r
 159                 double det_1=(a11*b11+a21*b12+a31*b13+a41*b14);\r
 160                 if(det_1==0){\r
 161                         return false;\r
 162                 }\r
 163                 det_1=1/det_1;\r
 164 \r
 165                 this.m00=b11*det_1;\r
 166                 this.m01=b12*det_1;\r
 167                 this.m02=b13*det_1;\r
 168                 this.m03=b14*det_1;\r
 169                 \r
 170                 this.m10=b21*det_1;\r
 171                 this.m11=b22*det_1;\r
 172                 this.m12=b23*det_1;\r
 173                 this.m13=b24*det_1;\r
 174                 \r
 175                 this.m20=b31*det_1;\r
 176                 this.m21=b32*det_1;\r
 177                 this.m22=b33*det_1;\r
 178                 this.m23=b34*det_1;\r
 179                 \r
 180                 this.m30=b41*det_1;\r
 181                 this.m31=b42*det_1;\r
 182                 this.m32=b43*det_1;\r
 183                 this.m33=b44*det_1;\r
 184                 \r
 185                 return true;\r
 186         }       \r
 187 }\r