trunk/src/jp/nyatla/nyartoolkit/core/types/matrix/NyARDoubleMatrix33.java

   1 /* \r
   2  * PROJECT: NyARToolkit(Extension)\r
   3  * --------------------------------------------------------------------------------\r
   4  * The NyARToolkit is Java version ARToolkit class library.\r
   5  * Copyright (C)2008 R.Iizuka\r
   6  *\r
   7  * This program is free software; you can redistribute it and/or\r
   8  * modify it under the terms of the GNU General Public License\r
   9  * as published by the Free Software Foundation; either version 2\r
  10  * of the License, or (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 framework; if not, write to the Free Software\r
  19  * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA\r
  20  * \r
  21  * For further information please contact.\r
  22  *      http://nyatla.jp/nyatoolkit/\r
  23  *      <airmail(at)ebony.plala.or.jp>\r
  24  * \r
  25  */\r
  26 package jp.nyatla.nyartoolkit.core.types.matrix;\r
  27 \r
  28 import jp.nyatla.nyartoolkit.*;\r
  29 import jp.nyatla.nyartoolkit.core.types.NyARDoublePoint3d;\r
  30 \r
  31 public class NyARDoubleMatrix33 implements INyARDoubleMatrix\r
  32 {\r
  33         public double m00;\r
  34         public double m01;\r
  35         public double m02;\r
  36         public double m10;\r
  37         public double m11;\r
  38         public double m12;\r
  39         public double m20;\r
  40         public double m21;\r
  41         public double m22;\r
  42         public static NyARDoubleMatrix33[] createArray(int i_number)\r
  43         {\r
  44                 NyARDoubleMatrix33[] ret=new NyARDoubleMatrix33[i_number];\r
  45                 for(int i=0;i<i_number;i++)\r
  46                 {\r
  47                         ret[i]=new NyARDoubleMatrix33();\r
  48                 }\r
  49                 return ret;\r
  50         }\r
  51         /**\r
  52          * 遅いからあんまり使わないでね。\r
  53          */\r
  54         public void setValue(double[] i_value)\r
  55         {\r
  56                 this.m00=i_value[0];\r
  57                 this.m01=i_value[1];\r
  58                 this.m02=i_value[2];\r
  59                 this.m10=i_value[3];\r
  60                 this.m11=i_value[4];\r
  61                 this.m12=i_value[5];\r
  62                 this.m20=i_value[6];\r
  63                 this.m21=i_value[7];\r
  64                 this.m22=i_value[8];\r
  65                 return;\r
  66         }\r
  67         public void setValue(NyARDoubleMatrix33 i_value)\r
  68         {\r
  69                 this.m00=i_value.m00;\r
  70                 this.m01=i_value.m01;\r
  71                 this.m02=i_value.m02;\r
  72                 this.m10=i_value.m10;\r
  73                 this.m11=i_value.m11;\r
  74                 this.m12=i_value.m12;\r
  75                 this.m20=i_value.m20;\r
  76                 this.m21=i_value.m21;\r
  77                 this.m22=i_value.m22;\r
  78                 return;\r
  79         }       \r
  80         /**\r
  81          * 遅いからあんまり使わないでね。\r
  82          */\r
  83         public void getValue(double[] o_value)\r
  84         {\r
  85                 o_value[0]=this.m00;\r
  86                 o_value[1]=this.m01;\r
  87                 o_value[2]=this.m02;\r
  88                 o_value[3]=this.m10;\r
  89                 o_value[4]=this.m11;\r
  90                 o_value[5]=this.m12;\r
  91                 o_value[6]=this.m20;\r
  92                 o_value[7]=this.m21;\r
  93                 o_value[8]=this.m22;\r
  94                 return;\r
  95         }\r
  96         public boolean inverse(NyARDoubleMatrix33 i_src)\r
  97         {\r
  98                 final double a11,a12,a13,a21,a22,a23,a31,a32,a33;\r
  99                 final double b11,b12,b13,b21,b22,b23,b31,b32,b33;       \r
 100                 a11=i_src.m00;a12=i_src.m01;a13=i_src.m02;\r
 101                 a21=i_src.m10;a22=i_src.m11;a23=i_src.m12;\r
 102                 a31=i_src.m20;a32=i_src.m21;a33=i_src.m22;\r
 103                 \r
 104                 b11=a22*a33-a23*a32;\r
 105                 b12=a32*a13-a33*a12;\r
 106                 b13=a12*a23-a13*a22;\r
 107                 \r
 108                 b21=a13*a31-a21*a33;\r
 109                 b22=a33*a11-a31*a13;\r
 110                 b23=a13*a21-a11*a23;\r
 111                 \r
 112                 b31=a21*a32-a22*a31;\r
 113                 b32=a31*a12-a32*a11;\r
 114                 b33=a11*a22-a12*a21;\r
 115                                 \r
 116                 double det_1=a11*b11+a21*b12+a31*b13;\r
 117                 if(det_1==0){\r
 118                         return false;\r
 119                 }\r
 120                 det_1=1/det_1;\r
 121 \r
 122                 this.m00=b11*det_1;\r
 123                 this.m01=b12*det_1;\r
 124                 this.m02=b13*det_1;\r
 125                 \r
 126                 this.m10=b21*det_1;\r
 127                 this.m11=b22*det_1;\r
 128                 this.m12=b23*det_1;\r
 129                 \r
 130                 this.m20=b31*det_1;\r
 131                 this.m21=b32*det_1;\r
 132                 this.m22=b33*det_1;\r
 133                 \r
 134                 return true;\r
 135         }\r
 136         /**\r
 137          * この関数は、0-PIの間で値を返します。\r
 138          * @param o_out\r
 139          */\r
 140         public final void getZXYAngle(NyARDoublePoint3d o_out)\r
 141         {\r
 142                 double sina = this.m21;\r
 143                 if (sina >= 1.0) {\r
 144                         o_out.x = Math.PI / 2;\r
 145                         o_out.y = 0;\r
 146                         o_out.z = Math.atan2(-this.m10, this.m00);\r
 147                 } else if (sina <= -1.0) {\r
 148                         o_out.x = -Math.PI / 2;\r
 149                         o_out.y = 0;\r
 150                         o_out.z = Math.atan2(-this.m10, this.m00);\r
 151                 } else {\r
 152                         o_out.x = Math.asin(sina);\r
 153                         o_out.z = Math.atan2(-this.m01, this.m11);\r
 154                         o_out.y = Math.atan2(-this.m20, this.m22);\r
 155                 }\r
 156         }\r
 157         public final void setZXYAngle(final double i_x, final double i_y, final double i_z)\r
 158         {\r
 159                 final double sina = Math.sin(i_x);\r
 160                 final double cosa = Math.cos(i_x);\r
 161                 final double sinb = Math.sin(i_y);\r
 162                 final double cosb = Math.cos(i_y);\r
 163                 final double sinc = Math.sin(i_z);\r
 164                 final double cosc = Math.cos(i_z);\r
 165                 this.m00 = cosc * cosb - sinc * sina * sinb;\r
 166                 this.m01 = -sinc * cosa;\r
 167                 this.m02 = cosc * sinb + sinc * sina * cosb;\r
 168                 this.m10 = sinc * cosb + cosc * sina * sinb;\r
 169                 this.m11 = cosc * cosa;\r
 170                 this.m12 = sinc * sinb - cosc * sina * cosb;\r
 171                 this.m20 = -cosa * sinb;\r
 172                 this.m21 = sina;\r
 173                 this.m22 = cosb * cosa;\r
 174         }       \r
 175 }\r