checkpoint

[anneal.git] / src / edu / berkeley / qfat / geom / Matrix.java

diff --git a/src/edu/berkeley/qfat/geom/Matrix.java b/src/edu/berkeley/qfat/geom/Matrix.java
index 7d14a71..346f53e 100644 (file)
--- a/src/edu/berkeley/qfat/geom/Matrix.java
+++ b/src/edu/berkeley/qfat/geom/Matrix.java
@@ -2,6 +2,9 @@ package edu.berkeley.qfat.geom;
 
 /** affine matrix; immutable */
 public class Matrix {
+
+    public static final Matrix ZERO = new Matrix(0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0);
+
     //
     //  [ a b c d ]   [ x ]
     //  [ e f g h ]   [ y ]
@@ -95,4 +98,68 @@ public class Matrix {
     public Vec apply(Vec v) { return v; }
     public Matrix invert() { return this; }
     public Matrix times(Matrix m) { return this; }
+
+
+    public float determinant() {
+        float m00 = a;
+        float m01 = b;
+        float m02 = c;
+        float m03 = d;
+        float m10 = e;
+        float m11 = f;
+        float m12 = g;
+        float m13 = h;
+        float m20 = i;
+        float m21 = j;
+        float m22 = k;
+        float m23 = l;
+        float m30 = m;
+        float m31 = n;
+        float m32 = o;
+        float m33 = p;
+        return
+            m03 * m12 * m21 * m30-m02 * m13 * m21 * m30-m03 * m11 * m22 * m30+m01 * m13    * m22 * m30+
+            m02 * m11 * m23 * m30-m01 * m12 * m23 * m30-m03 * m12 * m20 * m31+m02 * m13    * m20 * m31+
+            m03 * m10 * m22 * m31-m00 * m13 * m22 * m31-m02 * m10 * m23 * m31+m00 * m12    * m23 * m31+
+            m03 * m11 * m20 * m32-m01 * m13 * m20 * m32-m03 * m10 * m21 * m32+m00 * m13    * m21 * m32+
+            m01 * m10 * m23 * m32-m00 * m11 * m23 * m32-m02 * m11 * m20 * m33+m01 * m12    * m20 * m33+
+            m02 * m10 * m21 * m33-m00 * m12 * m21 * m33-m01 * m10 * m22 * m33+m00 * m11    * m22 * m33;
+    }
+
+    public Matrix inverse() {
+        float m00 = a;
+        float m01 = b;
+        float m02 = c;
+        float m03 = d;
+        float m10 = e;
+        float m11 = f;
+        float m12 = g;
+        float m13 = h;
+        float m20 = i;
+        float m21 = j;
+        float m22 = k;
+        float m23 = l;
+        float m30 = m;
+        float m31 = n;
+        float m32 = o;
+        float m33 = p;
+        return
+            new Matrix(m12*m23*m31 - m13*m22*m31 + m13*m21*m32 - m11*m23*m32 - m12*m21*m33 + m11*m22*m33,
+                       m03*m22*m31 - m02*m23*m31 - m03*m21*m32 + m01*m23*m32 + m02*m21*m33 - m01*m22*m33,
+                       m02*m13*m31 - m03*m12*m31 + m03*m11*m32 - m01*m13*m32 - m02*m11*m33 + m01*m12*m33,
+                       m03*m12*m21 - m02*m13*m21 - m03*m11*m22 + m01*m13*m22 + m02*m11*m23 - m01*m12*m23,
+                       m13*m22*m30 - m12*m23*m30 - m13*m20*m32 + m10*m23*m32 + m12*m20*m33 - m10*m22*m33,
+                       m02*m23*m30 - m03*m22*m30 + m03*m20*m32 - m00*m23*m32 - m02*m20*m33 + m00*m22*m33,
+                       m03*m12*m30 - m02*m13*m30 - m03*m10*m32 + m00*m13*m32 + m02*m10*m33 - m00*m12*m33,
+                       m02*m13*m20 - m03*m12*m20 + m03*m10*m22 - m00*m13*m22 - m02*m10*m23 + m00*m12*m23,
+                       m11*m23*m30 - m13*m21*m30 + m13*m20*m31 - m10*m23*m31 - m11*m20*m33 + m10*m21*m33,
+                       m03*m21*m30 - m01*m23*m30 - m03*m20*m31 + m00*m23*m31 + m01*m20*m33 - m00*m21*m33,
+                       m01*m13*m30 - m03*m11*m30 + m03*m10*m31 - m00*m13*m31 - m01*m10*m33 + m00*m11*m33,
+                       m03*m11*m20 - m01*m13*m20 - m03*m10*m21 + m00*m13*m21 + m01*m10*m23 - m00*m11*m23,
+                       m12*m21*m30 - m11*m22*m30 - m12*m20*m31 + m10*m22*m31 + m11*m20*m32 - m10*m21*m32,
+                       m01*m22*m30 - m02*m21*m30 + m02*m20*m31 - m00*m22*m31 - m01*m20*m32 + m00*m21*m32,
+                       m02*m11*m30 - m01*m12*m30 - m02*m10*m31 + m00*m12*m31 + m01*m10*m32 - m00*m11*m32,
+                       m01*m12*m20 - m02*m11*m20 + m02*m10*m21 - m00*m12*m21 - m01*m10*m22 + m00*m11*m22)
+            .times(1/determinant());
+    }
 }