[Backup]NyARToolkit for Java

[nyartoolkit-and/nyartoolkit-and.git] / src / jp / nyatla / nyartoolkit / core / types / matrix / NyARDoubleMatrix44.java

diff --git a/src/jp/nyatla/nyartoolkit/core/types/matrix/NyARDoubleMatrix44.java b/src/jp/nyatla/nyartoolkit/core/types/matrix/NyARDoubleMatrix44.java
index 6a1ca49..45f2da4 100644 (file)
--- a/src/jp/nyatla/nyartoolkit/core/types/matrix/NyARDoubleMatrix44.java
+++ b/src/jp/nyatla/nyartoolkit/core/types/matrix/NyARDoubleMatrix44.java
@@ -1,12 +1,6 @@
 /* \r
- * PROJECT: NyARToolkit\r
+ * PROJECT: NyARToolkit(Extension)\r
  * --------------------------------------------------------------------------------\r
- * This work is based on the original ARToolKit developed by\r
- *   Hirokazu Kato\r
- *   Mark Billinghurst\r
- *   HITLab, University of Washington, Seattle\r
- * http://www.hitl.washington.edu/artoolkit/\r
- *\r
  * The NyARToolkit is Java version ARToolkit class library.\r
  * Copyright (C)2008 R.Iizuka\r
  *\r
@@ -105,4 +99,90 @@ public class NyARDoubleMatrix44 implements INyARDoubleMatrix
                o_value[15]=this.m33;\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