--- /dev/null
+/* \r
+ * PROJECT: NyARToolkit(Extension)\r
+ * --------------------------------------------------------------------------------\r
+ * The NyARToolkit is Java edition ARToolKit class library.\r
+ * Copyright (C)2008-2009 Ryo Iizuka\r
+ *\r
+ * This program is free software: you can redistribute it and/or modify\r
+ * it under the terms of the GNU General Public License as published by\r
+ * the Free Software Foundation, either version 3 of the License, or\r
+ * (at your option) any later version.\r
+ * \r
+ * This program is distributed in the hope that it will be useful,\r
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of\r
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the\r
+ * GNU General Public License for more details.\r
+ *\r
+ * You should have received a copy of the GNU General Public License\r
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.\r
+ * \r
+ * For further information please contact.\r
+ * http://nyatla.jp/nyatoolkit/\r
+ * <airmail(at)ebony.plala.or.jp> or <nyatla(at)nyatla.jp>\r
+ * \r
+ */\r
+package jp.nyatla.nyartoolkit.core.types.matrix;\r
+\r
+import jp.nyatla.nyartoolkit.core.types.NyARDoublePoint3d;\r
+\r
+\r
+\r
+public class NyARDoubleMatrix44 implements INyARDoubleMatrix\r
+{\r
+ public double m00;\r
+ public double m01;\r
+ public double m02;\r
+ public double m03;\r
+ public double m10;\r
+ public double m11;\r
+ public double m12;\r
+ public double m13;\r
+ public double m20;\r
+ public double m21;\r
+ public double m22;\r
+ public double m23;\r
+ public double m30;\r
+ public double m31;\r
+ public double m32;\r
+ public double m33;\r
+ public static NyARDoubleMatrix44[] createArray(int i_number)\r
+ {\r
+ NyARDoubleMatrix44[] ret=new NyARDoubleMatrix44[i_number];\r
+ for(int i=0;i<i_number;i++)\r
+ {\r
+ ret[i]=new NyARDoubleMatrix44();\r
+ }\r
+ return ret;\r
+ }\r
+ /**\r
+ * 配列の内容をセットします。順番は、00,01,02,03,10...の順です。\r
+ */\r
+ public void setValue(double[] i_value)\r
+ {\r
+ this.m00=i_value[ 0];\r
+ this.m01=i_value[ 1];\r
+ this.m02=i_value[ 2];\r
+ this.m03=i_value[ 3];\r
+ this.m10=i_value[ 4];\r
+ this.m11=i_value[ 5];\r
+ this.m12=i_value[ 6];\r
+ this.m13=i_value[ 7];\r
+ this.m20=i_value[ 8];\r
+ this.m21=i_value[ 9];\r
+ this.m22=i_value[10];\r
+ this.m23=i_value[11];\r
+ this.m30=i_value[12];\r
+ this.m31=i_value[13];\r
+ this.m32=i_value[14];\r
+ this.m33=i_value[15];\r
+ return;\r
+ }\r
+ /**\r
+ * i_valueの内容を、このインスタンスにセットします。\r
+ * @param i_value\r
+ */\r
+ public void setValue(NyARDoubleMatrix44 i_value)\r
+ {\r
+ this.m00=i_value.m00;\r
+ this.m01=i_value.m01;\r
+ this.m02=i_value.m02;\r
+ this.m03=i_value.m03;\r
+ this.m10=i_value.m10;\r
+ this.m11=i_value.m11;\r
+ this.m12=i_value.m12;\r
+ this.m13=i_value.m13;\r
+ this.m20=i_value.m20;\r
+ this.m21=i_value.m21;\r
+ this.m22=i_value.m22;\r
+ this.m23=i_value.m23;\r
+ this.m30=i_value.m30;\r
+ this.m31=i_value.m31;\r
+ this.m32=i_value.m32;\r
+ this.m33=i_value.m33;\r
+ return;\r
+ }\r
+ /**\r
+ * 行列の内容を配列に出力します。順番は、00,01,02,03,10...の順です。\r
+ */\r
+ public void getValue(double[] o_value)\r
+ {\r
+ o_value[ 0]=this.m00;\r
+ o_value[ 1]=this.m01;\r
+ o_value[ 2]=this.m02;\r
+ o_value[ 3]=this.m03;\r
+ o_value[ 4]=this.m10;\r
+ o_value[ 5]=this.m11;\r
+ o_value[ 6]=this.m12;\r
+ o_value[ 7]=this.m13;\r
+ o_value[ 8]=this.m20;\r
+ o_value[ 9]=this.m21;\r
+ o_value[10]=this.m22;\r
+ o_value[11]=this.m23;\r
+ o_value[12]=this.m30;\r
+ o_value[13]=this.m31;\r
+ o_value[14]=this.m32;\r
+ o_value[15]=this.m33;\r
+ return;\r
+ }\r
+ /**\r
+ * 行列の内容を転置してから配列に出力します。\r
+ * 順番は、00,10,20,30,01...の順です。\r
+ */ \r
+ public void getValueT(double[] o_value)\r
+ {\r
+ o_value[ 0]=this.m00;\r
+ o_value[ 1]=this.m10;\r
+ o_value[ 2]=this.m20;\r
+ o_value[ 3]=this.m30;\r
+ o_value[ 4]=this.m01;\r
+ o_value[ 5]=this.m11;\r
+ o_value[ 6]=this.m21;\r
+ o_value[ 7]=this.m31;\r
+ o_value[ 8]=this.m02;\r
+ o_value[ 9]=this.m12;\r
+ o_value[10]=this.m22;\r
+ o_value[11]=this.m32;\r
+ o_value[12]=this.m03;\r
+ o_value[13]=this.m13;\r
+ o_value[14]=this.m23;\r
+ o_value[15]=this.m33;\r
+ return;\r
+ }\r
+ /**\r
+ * 逆行列を計算して、i_srcのthisへ格納します。i_srcにはthisも指定可能です。\r
+ * @param i_src\r
+ * @return\r
+ */\r
+ public boolean inverse(NyARDoubleMatrix44 i_src)\r
+ {\r
+ final double a11,a12,a13,a14,a21,a22,a23,a24,a31,a32,a33,a34,a41,a42,a43,a44;\r
+ final double b11,b12,b13,b14,b21,b22,b23,b24,b31,b32,b33,b34,b41,b42,b43,b44; \r
+ double t1,t2,t3,t4,t5,t6;\r
+ a11=i_src.m00;a12=i_src.m01;a13=i_src.m02;a14=i_src.m03;\r
+ a21=i_src.m10;a22=i_src.m11;a23=i_src.m12;a24=i_src.m13;\r
+ a31=i_src.m20;a32=i_src.m21;a33=i_src.m22;a34=i_src.m23;\r
+ a41=i_src.m30;a42=i_src.m31;a43=i_src.m32;a44=i_src.m33;\r
+ \r
+ t1=a33*a44-a34*a43;\r
+ t2=a34*a42-a32*a44;\r
+ t3=a32*a43-a33*a42;\r
+ t4=a34*a41-a31*a44;\r
+ t5=a31*a43-a33*a41;\r
+ t6=a31*a42-a32*a41;\r
+ \r
+ b11=a22*t1+a23*t2+a24*t3;\r
+ b21=-(a23*t4+a24*t5+a21*t1);\r
+ b31=a24*t6-a21*t2+a22*t4;\r
+ b41=-(a21*t3-a22*t5+a23*t6);\r
+ \r
+ t1=a43*a14-a44*a13;\r
+ t2=a44*a12-a42*a14;\r
+ t3=a42*a13-a43*a12;\r
+ t4=a44*a11-a41*a14;\r
+ t5=a41*a13-a43*a11;\r
+ t6=a41*a12-a42*a11;\r
+\r
+ b12=-(a32*t1+a33*t2+a34*t3);\r
+ b22=a33*t4+a34*t5+a31*t1;\r
+ b32=-(a34*t6-a31*t2+a32*t4);\r
+ b42=a31*t3-a32*t5+a33*t6;\r
+ \r
+ t1=a13*a24-a14*a23;\r
+ t2=a14*a22-a12*a24;\r
+ t3=a12*a23-a13*a22;\r
+ t4=a14*a21-a11*a24;\r
+ t5=a11*a23-a13*a21;\r
+ t6=a11*a22-a12*a21;\r
+\r
+ b13=a42*t1+a43*t2+a44*t3;\r
+ b23=-(a43*t4+a44*t5+a41*t1);\r
+ b33=a44*t6-a41*t2+a42*t4;\r
+ b43=-(a41*t3-a42*t5+a43*t6);\r
+\r
+ t1=a23*a34-a24*a33;\r
+ t2=a24*a32-a22*a34;\r
+ t3=a22*a33-a23*a32;\r
+ t4=a24*a31-a21*a34; \r
+ t5=a21*a33-a23*a31;\r
+ t6=a21*a32-a22*a31;\r
+\r
+ b14=-(a12*t1+a13*t2+a14*t3);\r
+ b24=a13*t4+a14*t5+a11*t1;\r
+ b34=-(a14*t6-a11*t2+a12*t4);\r
+ b44=a11*t3-a12*t5+a13*t6;\r
+ \r
+ double det_1=(a11*b11+a21*b12+a31*b13+a41*b14);\r
+ if(det_1==0){\r
+ return false;\r
+ }\r
+ det_1=1/det_1;\r
+\r
+ this.m00=b11*det_1;\r
+ this.m01=b12*det_1;\r
+ this.m02=b13*det_1;\r
+ this.m03=b14*det_1;\r
+ \r
+ this.m10=b21*det_1;\r
+ this.m11=b22*det_1;\r
+ this.m12=b23*det_1;\r
+ this.m13=b24*det_1;\r
+ \r
+ this.m20=b31*det_1;\r
+ this.m21=b32*det_1;\r
+ this.m22=b33*det_1;\r
+ this.m23=b34*det_1;\r
+ \r
+ this.m30=b41*det_1;\r
+ this.m31=b42*det_1;\r
+ this.m32=b43*det_1;\r
+ this.m33=b44*det_1;\r
+ \r
+ return true;\r
+ }\r
+ \r
+ /**\r
+ * 行列を姿勢変換行列として、3次元座標を座標変換します。4列目は1で仮定します。\r
+ * @param i_x\r
+ * @param i_y\r
+ * @param i_z\r
+ * @param o_out\r
+ */\r
+ public final void transform3d(double i_x,double i_y,double i_z,NyARDoublePoint3d o_out)\r
+ {\r
+ o_out.x=this.m00*i_x+this.m01*i_y+this.m02*i_z+this.m03;\r
+ o_out.y=this.m10*i_x+this.m11*i_y+this.m12*i_z+this.m13;\r
+ o_out.z=this.m20*i_x+this.m21*i_y+this.m22*i_z+this.m23;\r
+ return;\r
+ }\r
+ \r
+ /**\r
+ * 行列を姿勢変換行列として、3次元座標を座標変換します。\r
+ * i_inとo_outには同一なインスタンスを指定できます。\r
+ * @param i_in\r
+ * @param o_out\r
+ */\r
+ public final void transform3d(NyARDoublePoint3d i_in,NyARDoublePoint3d o_out)\r
+ {\r
+ transform3d(i_in.x,i_in.y,i_in.z,o_out);\r
+ }\r
+ /**\r
+ * 行列を姿勢行列として、ZXY系の角度値を返します。\r
+ * この関数は、0-PIの間で値を返します。\r
+ * @param o_out\r
+ */\r
+ public final void getZXYAngle(NyARDoublePoint3d o_out)\r
+ {\r
+ double sina = this.m21;\r
+ if (sina >= 1.0) {\r
+ o_out.x = Math.PI / 2;\r
+ o_out.y = 0;\r
+ o_out.z = Math.atan2(-this.m10, this.m00);\r
+ } else if (sina <= -1.0) {\r
+ o_out.x = -Math.PI / 2;\r
+ o_out.y = 0;\r
+ o_out.z = Math.atan2(-this.m10, this.m00);\r
+ } else {\r
+ o_out.x = Math.asin(sina);\r
+ o_out.z = Math.atan2(-this.m01, this.m11);\r
+ o_out.y = Math.atan2(-this.m20, this.m22);\r
+ }\r
+ }\r
+ /**\r
+ * 行列の掛け算を実行して、結果を格納します。i_mat_lとi_mat_rには、thisを指定しないでください。\r
+ * @param i_mat_l\r
+ * @param i_mat_r\r
+ */\r
+ public final void mul(NyARDoubleMatrix44 i_mat_l,NyARDoubleMatrix44 i_mat_r)\r
+ {\r
+ assert(this!=i_mat_l);\r
+ assert(this!=i_mat_r);\r
+ 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
+ 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
+ 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
+ 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
+\r
+ 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
+ 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
+ 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
+ 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
+\r
+ 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
+ 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
+ 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
+ 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
+\r
+ 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
+ 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
+ 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
+ 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
+ return;\r
+ }\r
+ /**\r
+ * この行列を単位行列にします。\r
+ */\r
+ public final void identity()\r
+ {\r
+ this.m00=this.m11=this.m22=this.m33=1;\r
+ 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
+ return;\r
+ }\r
+ /**\r
+ * 行列に、右手系のX軸回転を設定します。\r
+ * @param i_radian\r
+ */\r
+ public final void setRotateX(double i_radian)\r
+ {\r
+ double s=Math.sin(i_radian);\r
+ double c=Math.cos(i_radian);\r
+ this.identity();\r
+ this.m11=c;\r
+ this.m12=-s;\r
+ this.m21=s;\r
+ this.m22=c;\r
+ return;\r
+ }\r
+ /**\r
+ * 行列に、右手系のY軸回転を設定します。\r
+ * @param i_radian\r
+ */\r
+ public final void setRotateY(double i_radian)\r
+ {\r
+ double s=Math.sin(i_radian);\r
+ double c=Math.cos(i_radian);\r
+ this.identity();\r
+ this.m00=c;\r
+ this.m02=s;\r
+ this.m20=-s;\r
+ this.m22=c;\r
+ } \r
+ /**\r
+ * 行列に、右手系のZ軸回転を設定します。\r
+ * @param i_radian\r
+ */\r
+ public final void setRotateZ(double i_radian)\r
+ {\r
+ double s=Math.sin(i_radian);\r
+ double c=Math.cos(i_radian);\r
+ this.identity();\r
+ this.m00=c;\r
+ this.m01=-s;\r
+ this.m10=s;\r
+ this.m11=c;\r
+ \r
+ }\r
+ /**\r
+ * 行列に、右手系の並行移動を設定します。\r
+ * @param i_radian\r
+ */\r
+ public final void setTranslate(double i_x,double i_y,double i_z)\r
+ {\r
+ this.identity();\r
+ this.m03=i_x;\r
+ this.m13=i_y;\r
+ this.m23=i_z;\r
+ }\r
+ /**\r
+ * この行列をX軸回転します。\r
+ * @param i_mat_r\r
+ */\r
+ public final void rotateX(double i_radian)\r
+ {\r
+ double s=Math.sin(i_radian);\r
+ double c=Math.cos(i_radian);\r
+ \r
+ double t1,t2;\r
+ t1=this.m01;t2=this.m02;\r
+ this.m01=t1*c + t2*s;\r
+ this.m02=t1*(-s) + t2*c;\r
+ t1=this.m11;t2=this.m12;\r
+ this.m11=t1*c + t2*s;\r
+ this.m12=t1*(-s) + t2*c;\r
+ t1=this.m21;t2=this.m22;\r
+ this.m21=t1*c + t2*s;\r
+ this.m22=t1*(-s) + t2*c; \r
+ t1=this.m31;t2=this.m32;\r
+ this.m31=t1*c + t2*s;\r
+ this.m32=t1*(-s) + t2*c;\r
+ }\r
+ /**\r
+ * この行列をY軸回転します。\r
+ * @param i_mat_r\r
+ */\r
+ public final void rotateY(double i_radian)\r
+ {\r
+ double s=Math.sin(i_radian);\r
+ double c=Math.cos(i_radian);\r
+\r
+ double t1,t2;\r
+ t1=this.m00;t2=this.m02;\r
+ this.m00=t1*c + t2*(-s);\r
+ this.m02=t1*s + t2*c;\r
+ t1=this.m10;t2=this.m12;\r
+ this.m10=t1*c + t2*(-s);\r
+ this.m12=t1*s + t2*c;\r
+ t1=this.m20;t2=this.m22;\r
+ this.m20=t1*c + t2*(-s);\r
+ this.m22=t1*s + t2*c;\r
+ t1=this.m30;t2=this.m32;\r
+ this.m30=t1*c + t2*(-s);\r
+ this.m32=t1*s + t2*c;\r
+ }\r
+ /**\r
+ * この姿勢行列をZ軸回転します。\r
+ * @i_radian\r
+ */\r
+ public final void rotateZ(double i_radian)\r
+ {\r
+ double s=Math.sin(i_radian);\r
+ double c=Math.cos(i_radian);\r
+ \r
+ double t1,t2;\r
+ t1=this.m00;t2=this.m01;\r
+ this.m00=t1*c + t2*s;\r
+ this.m01=t1*(-s) + t2*c;\r
+\r
+ t1=this.m10;t2=this.m11;\r
+ this.m10=t1*c + t2*s;\r
+ this.m11=t1*(-s) + t2*c;\r
+\r
+ t1=this.m20;t2=this.m21;\r
+ this.m20=t1*c + t2*s;\r
+ this.m21=t1*(-s) + t2*c;\r
+\r
+ t1=this.m30;t2=this.m31;\r
+ this.m30=t1*c + t2*s;\r
+ this.m31=t1*(-s) + t2*c;\r
+ return;\r
+ }\r
+ /**\r
+ * この姿勢行列を右手系で平行移動します。\r
+ * @param i_x\r
+ * @param i_y\r
+ * @param i_z\r
+ */\r
+ public final void translate(double i_x,double i_y,double i_z)\r
+ {\r
+ this.m03=this.m00*i_x + this.m01*i_y + this.m02*i_z + this.m03;\r
+ this.m13=this.m10*i_x + this.m11*i_y + this.m12*i_z + this.m13;\r
+ this.m23=this.m20*i_x + this.m21*i_y + this.m22*i_z + this.m23;\r
+ this.m33=this.m30*i_x + this.m31*i_y + this.m32*i_z + this.m33;\r
+ return;\r
+ }\r
+ public static void main(String[] args)\r
+ {\r
+\r
+ try {\r
+ NyARDoubleMatrix44 t1 = new NyARDoubleMatrix44();\r
+ NyARDoubleMatrix44 t2 = new NyARDoubleMatrix44();\r
+ NyARDoubleMatrix44 t3 = new NyARDoubleMatrix44();\r
+ NyARDoubleMatrix44 t4 = new NyARDoubleMatrix44();\r
+ t1.m00=Math.random()*100;\r
+ t1.m01=Math.random()*100;\r
+ t1.m02=Math.random()*100;\r
+ t1.m03=Math.random()*100;\r
+ t1.m10=Math.random()*100;\r
+ t1.m11=Math.random()*100;\r
+ t1.m12=Math.random()*100;\r
+ t1.m13=Math.random()*100;\r
+ t1.m20=Math.random()*100;\r
+ t1.m21=Math.random()*100;\r
+ t1.m22=Math.random()*100;\r
+ t1.m23=Math.random()*100;\r
+ t1.m30=Math.random()*100;\r
+ t1.m31=Math.random()*100;\r
+ t1.m32=Math.random()*100;\r
+ t1.m33=Math.random()*100;\r
+ t2.setValue(t1);\r
+ t1.rotateZ(Math.PI*10/180);\r
+ t4.setRotateZ(Math.PI*10/180);\r
+ t3.mul(t2,t4);\r
+ System.out.print("");\r
+ } catch (Exception e) {\r
+ e.printStackTrace();\r
+ }\r
+ } \r
+ \r
+}\r
OSDN Git Service
