X-Git-Url: http://git.megacz.com/?p=anneal.git;a=blobdiff_plain;f=src%2Fedu%2Fberkeley%2Fqfat%2Fgeom%2FMatrix.java;h=c2419d6fa7aa9f3179da3304522ed724a95548dd;hp=751dfa10193dfe32a7b559151a0752a6d811c7be;hb=553823b9fbe373bbc8b5409c423857253f18c4aa;hpb=965caa65c5048f85c4335fd4d0f962f8c5ae7012

diff --git a/src/edu/berkeley/qfat/geom/Matrix.java b/src/edu/berkeley/qfat/geom/Matrix.java
index 751dfa1..c2419d6 100644
--- a/src/edu/berkeley/qfat/geom/Matrix.java
+++ b/src/edu/berkeley/qfat/geom/Matrix.java
@@ -1,65 +1,388 @@
 package edu.berkeley.qfat.geom;
+import javax.media.opengl.*;
 
-/** affine matrix; immutable */
+// FEATURE: precompute/cache determinant?
+
+/** an affine matrix; immutable */
 public class Matrix {
-    //
-    //  [ a b c d ]   [ x ]
-    //  [ e f g h ]   [ y ]
-    //  [ i j k l ]   [ z ]
-    //  [ 0 0 0 1 ]   [ 1 ]
-    //
-    public final float a, b, c, d, e, f, g, h, i, j, k, l;
-    public Matrix() { this(1); }
-    public Matrix(float scale) {
-        a = f = k = scale;
-        l = h = d = e = b = i = c = j = g = 0;            
-    }
-    public Matrix(float scalex, float scaley, float scalez) {
-        a = scalex;
-        f = scaley;
-        k = scalez;
-        l = h = d = e = b = i = c = j = g = 0;            
-    }
-    public Matrix(Vec translate) {
-        d = translate.x; h = translate.y; l = translate.z;
-        a = f = k = 1;
-        b = c = e = g = i = j = 0;
-    }
-    public Matrix(float a, float b, float c, float d, float e, float f, float g, float h, float i, float j, float k, float l) {
+
+    /**
+     *  <pre>
+     *  [ a b c d ]   [ x ]
+     *  [ e f g h ]   [ y ]
+     *  [ i j k l ]   [ z ]
+     *  [ m n o p ]   [ 1 ]
+     *  </pre>
+     */
+    public final float a, b, c, d, e, f, g, h, i, j, k, l, m, n, o, p;
+
+    /** the zero matrix */
+    public static final Matrix ZERO          = new Matrix(0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0);
+
+    /** the identity matrix */
+    public static final Matrix ONE           = new Matrix(1,0,0,0,0,1,0,0,0,0,1,0,0,0,0,1);
+
+    /** the identity matrix */
+    public static final Matrix NEGATIVE_ONE  = new Matrix(-1,0,0,0,0,-1,0,0,0,0,-1,0,0,0,0,1);
+
+    public Matrix(double a, double b, double c, double d, double e, double f, double g,
+                  double h, double i, double j, double k, double l, double m, double n, double o, double p) {
+        this((float)a, (float)b, (float)c, (float)d,
+             (float)e, (float)f, (float)g, (float)h,
+             (float)i, (float)j, (float)k, (float)l,
+             (float)m, (float)n, (float)o, (float)p);
+    }
+
+    public Matrix(float a, float b, float c, float d, float e, float f, float g,
+                  float h, float i, float j, float k, float l, float m, float n, float o, float p) {
         this.a = a; this.b = b; this.c = c; this.d = d; this.e = e; this.f = f; this.g = g; this.h = h; this.i = i;
-        this.j = j; this.k = k; this.l = l;
+        this.j = j; this.k = k; this.l = l; this.m = m; this.n = n; this.o = o; this.p = p;
     }
-    public Matrix times(float x) {
-        return new Matrix(a*x, b*x, c*x, d*x, e*x, f*x, g*x, h*x, i*x, j*x, k*x, l*x);
-    }
-    public Matrix(Vec axis, float angle) {
-        double q = Math.cos(angle);
-        double s = Math.sin(angle);
-        double t = 1.0 - q;
-        a = (float)(q + axis.x*axis.x*t);
-        f = (float)(q + axis.y*axis.y*t);
-        k = (float)(q + axis.z*axis.z*t);
-        double tmp1 = axis.x*axis.y*t;
-        double tmp2 = axis.z*s;
-        e = (float)(tmp1 + tmp2);
-        b = (float)(tmp1 - tmp2);
+
+    /** a scaling matrix (uniform in all dimensions) */
+    public static Matrix scale(float scale) { return scale(scale, scale, scale); }
+
+    /** a scaling matrix */
+    public static Matrix scale(float scalex, float scaley, float scalez) {
+        return new Matrix(scalex, 0, 0, 0, 0, scaley, 0, 0, 0, 0, scalez, 0, 0, 0, 0, 1); }
+
+    /** a translation matrix */
+    public static Matrix translate(Vec translate) {
+        return new Matrix(1, 0, 0, translate.x,
+                          0, 1, 0, translate.y,
+                          0, 0, 1, translate.z,
+                          0, 0, 0, 1);
+    }
+
+    /** a rotation matrix, <tt>angle</tt> degrees around <tt>axis</tt> */
+    public static Matrix rotate(Vec axis, float angle) {
+        float q = (float)Math.cos(angle);
+        float s = (float)Math.sin(angle);
+        float t = 1 - q;
+        float a = (float)(q + axis.x*axis.x*t);
+        float f = (float)(q + axis.y*axis.y*t);
+        float k = (float)(q + axis.z*axis.z*t);
+        float tmp1 = axis.x*axis.y*t;
+        float tmp2 = axis.z*s;
+        float e = (float)(tmp1 + tmp2);
+        float b = (float)(tmp1 - tmp2);
         tmp1 = axis.x*axis.z*t;
         tmp2 = axis.y*s;
-        i = (float)(tmp1 - tmp2);
-        c = (float)(tmp1 + tmp2);
+        float i = (float)(tmp1 - tmp2);
+        float c = (float)(tmp1 + tmp2);
         tmp1 = axis.y*axis.z*t;
         tmp2 = axis.x*s;
-        j = (float)(tmp1 + tmp2);
-        g = (float)(tmp1 - tmp2);
-        d = h = l = 0;
+        float j = (float)(tmp1 + tmp2);
+        float g = (float)(tmp1 - tmp2);
+        return new Matrix(a, b, c, 0,
+                          e, f, g, 0,
+                          i, j, k, 0,
+                          0, 0, 0, 1);
+    }
+
+    /** a reflection matrix across the plane passing through the origin with the specified normal */
+    public static Matrix reflect(Vec v) {
+        Vec reflectionPlaneNormal = v.norm();
+        float a = reflectionPlaneNormal.x;
+        float b = reflectionPlaneNormal.y;
+        float c = reflectionPlaneNormal.z;
+        return
+                new Matrix( 1-2*a*a,  -2*a*b,  -2*a*c, 0,
+                            -2*a*b,  1-2*b*b,  -2*b*c, 0,
+                            -2*a*c,   -2*b*c, 1-2*c*c, 0,
+                            0,       0,       0,       1);
+    }
+
+    /** returns the translational component of this matrix */
+    public Vec getTranslationalComponent() { return this.times(Point.ZERO).minus(Point.ZERO); }
+
+    public Matrix plus(Matrix x) {
+        return new Matrix(a+x.a, b+x.b, c+x.c, d+x.d, e+x.e, f+x.f, g+x.g, h+x.h, i+x.i, j+x.j, k+x.k, l+x.l, m+x.m, n+x.n, o+x.o, p+x.p);
+    }
+    public Matrix minus(Matrix x) {
+        return new Matrix(a-x.a, b-x.b, c-x.c, d-x.d, e-x.e, f-x.f, g-x.g, h-x.h, i-x.i, j-x.j, k-x.k, l-x.l, m-x.m, n-x.n, o-x.o, p-x.p);
+    }
+    public Matrix times(float x) {
+        return new Matrix(a*x, b*x, c*x, d*x, e*x, f*x, g*x, h*x, i*x, j*x, k*x, l*x, m*x, n*x, o*x, p*x);
+    }
+
+    /** computes (v^T)(this)(v) */
+    public float preAndPostMultiply(Point point) {
+        float ret =
+            ((a*point.x + b*point.y + c*point.z + d) * point.x) +
+            ((e*point.x + f*point.y + g*point.z + h) * point.y) +
+            ((i*point.x + j*point.y + k*point.z + l) * point.z) + 
+            ((m*point.x + n*point.y + o*point.z + p) * 1);
+        return ret;
     }
+
     public Point times(Point p) {
-        return new Point(a*p.x + b*p.y + c*p.z + d,
-                         e*p.x + f*p.y + g*p.z + h,
-                         i*p.x + j*p.y + k*p.z + l);
-    }
-    public Point apply(Point p) { return p; }
-    public Vec apply(Vec v) { return v; }
-    public Matrix invert() { return this; }
-    public Matrix times(Matrix m) { return this; }
+        double x = a*p.x + b*p.y + c*p.z + d;
+        double y = e*p.x + f*p.y + g*p.z + h;
+        double z = i*p.x + j*p.y + k*p.z + l;
+        double q = m*p.x + n*p.y + o*p.z + this.p;
+        return new Point(x/q, y/q, z/q);
+    }
+
+    public Vec times(Vec p) {
+        return this.times(Matrix.translate(p)).getTranslationalComponent();
+    }
+
+    /** multiply by another matrix */
+    public Matrix times(Matrix z) {
+        float t00 = a;
+        float t01 = b;
+        float t02 = c;
+        float t03 = d;
+        float t10 = e;
+        float t11 = f;
+        float t12 = g;
+        float t13 = h;
+        float t20 = i;
+        float t21 = j;
+        float t22 = k;
+        float t23 = l;
+        float t30 = m;
+        float t31 = n;
+        float t32 = o;
+        float t33 = p;
+        float m00 = z.a;
+        float m01 = z.b;
+        float m02 = z.c;
+        float m03 = z.d;
+        float m10 = z.e;
+        float m11 = z.f;
+        float m12 = z.g;
+        float m13 = z.h;
+        float m20 = z.i;
+        float m21 = z.j;
+        float m22 = z.k;
+        float m23 = z.l;
+        float m30 = z.m;
+        float m31 = z.n;
+        float m32 = z.o;
+        float m33 = z.p;
+        return new Matrix(t00*m00 + t01*m10 + t02*m20 + t03*m30,
+                          t00*m01 + t01*m11 + t02*m21 + t03*m31,
+                          t00*m02 + t01*m12 + t02*m22 + t03*m32,
+                          t00*m03 + t01*m13 + t02*m23 + t03*m33,
+                          t10*m00 + t11*m10 + t12*m20 + t13*m30,
+                          t10*m01 + t11*m11 + t12*m21 + t13*m31,
+                          t10*m02 + t11*m12 + t12*m22 + t13*m32,
+                          t10*m03 + t11*m13 + t12*m23 + t13*m33,
+                          t20*m00 + t21*m10 + t22*m20 + t23*m30,
+                          t20*m01 + t21*m11 + t22*m21 + t23*m31,
+                          t20*m02 + t21*m12 + t22*m22 + t23*m32,
+                          t20*m03 + t21*m13 + t22*m23 + t23*m33,
+                          t30*m00 + t31*m10 + t32*m20 + t33*m30,
+                          t30*m01 + t31*m11 + t32*m21 + t33*m31,
+                          t30*m02 + t31*m12 + t32*m22 + t33*m32,
+                          t30*m03 + t31*m13 + t32*m23 + t33*m33);
+    }
+
+    /** compute the determinant of the matrix */
+    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;
+    }
+
+    /** compute the inverse of the matrix, returns null if not invertible */
+    public Matrix inverse() {
+        float determinant = determinant();
+        if (determinant==0) return null;
+        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);
+    }
+
+    public String toString() {
+        return
+            "\n [ " + a + "\t" + b + "\t" + c + "\t" + d + "\t" + "]" + 
+            "\n [ " + e + "\t" + f + "\t" + g + "\t" + h + "\t" + "]" + 
+            "\n [ " + i + "\t" + j + "\t" + k + "\t" + l + "\t" + "]" + 
+            "\n [ " + m + "\t" + n + "\t" + o + "\t" + p + "\t" + "]\n";
+    }
+
+    public boolean equals(Object oo) {
+        if (oo==null) return false;
+        if (!(oo instanceof Matrix)) return false;
+        Matrix z = (Matrix)oo;
+        return
+            (a==z.a) && 
+            (b==z.b) && 
+            (c==z.c) && 
+            (d==z.d) && 
+            (e==z.e) && 
+            (f==z.f) && 
+            (g==z.g) && 
+            (h==z.h) && 
+            (i==z.i) && 
+            (j==z.j) && 
+            (k==z.k) && 
+            (l==z.l) && 
+            (m==z.m) && 
+            (n==z.n) && 
+            (o==z.o) && 
+            (p==z.p);
+    }
+
+    public boolean equalsModuloEpsilon(Matrix z, float epsilon) {
+        return
+            Math.abs(a-z.a)<epsilon && 
+            Math.abs(b-z.b)<epsilon && 
+            Math.abs(c-z.c)<epsilon && 
+            Math.abs(d-z.d)<epsilon && 
+            Math.abs(e-z.e)<epsilon && 
+            Math.abs(f-z.f)<epsilon && 
+            Math.abs(g-z.g)<epsilon && 
+            Math.abs(h-z.h)<epsilon && 
+            Math.abs(i-z.i)<epsilon && 
+            Math.abs(j-z.j)<epsilon && 
+            Math.abs(k-z.k)<epsilon && 
+            Math.abs(l-z.l)<epsilon && 
+            Math.abs(m-z.m)<epsilon && 
+            Math.abs(n-z.n)<epsilon && 
+            Math.abs(o-z.o)<epsilon && 
+            Math.abs(p-z.p)<epsilon;
+    }
+
+    public int hashCode() {
+        return
+            Float.floatToIntBits(a) ^
+            Float.floatToIntBits(b) ^
+            Float.floatToIntBits(c) ^
+            Float.floatToIntBits(d) ^
+            Float.floatToIntBits(e) ^
+            Float.floatToIntBits(f) ^
+            Float.floatToIntBits(g) ^
+            Float.floatToIntBits(h) ^
+            Float.floatToIntBits(i) ^
+            Float.floatToIntBits(j) ^
+            Float.floatToIntBits(k) ^
+            Float.floatToIntBits(l) ^
+            Float.floatToIntBits(m) ^
+            Float.floatToIntBits(n) ^
+            Float.floatToIntBits(o) ^
+            Float.floatToIntBits(p);
+    }
+
+    /** retrieves the current objectspace-to-windowspace projection matrix from the GL context */
+    public static Matrix getProjectionMatrix(GL gl) {
+        int view[] = new int[4];
+        double mvmatrix[] = new double[16];
+        double projmatrix[] = new double[16];
+        gl.glGetIntegerv(GL.GL_VIEWPORT, view, 0);
+        gl.glGetDoublev(GL.GL_MODELVIEW_MATRIX, mvmatrix, 0);
+        gl.glGetDoublev(GL.GL_PROJECTION_MATRIX, projmatrix, 0);
+        Matrix m = new Matrix(mvmatrix[0],
+                              mvmatrix[4],
+                              mvmatrix[8],
+                              mvmatrix[12],
+                              mvmatrix[1],
+                              mvmatrix[5],
+                              mvmatrix[9],
+                              mvmatrix[13],
+                              mvmatrix[2],
+                              mvmatrix[6],
+                              mvmatrix[10],
+                              mvmatrix[14],
+                              mvmatrix[3],
+                              mvmatrix[7],
+                              mvmatrix[11],
+                              mvmatrix[15]);
+        Matrix p = new Matrix(projmatrix[0],
+                              projmatrix[4],
+                              projmatrix[8],
+                              projmatrix[12],
+                              projmatrix[1],
+                              projmatrix[5],
+                              projmatrix[9],
+                              projmatrix[13],
+                              projmatrix[2],
+                              projmatrix[6],
+                              projmatrix[10],
+                              projmatrix[14],
+                              projmatrix[3],
+                              projmatrix[7],
+                              projmatrix[11],
+                              projmatrix[15]);
+        Matrix z =
+            new Matrix(0.5*view[2], 0,           0,   view[0]+view[2]*0.5,
+                       0,           0.5*view[3], 0,   view[1]+view[3]*0.5,
+                       0,           0,           0.5, 0.5,
+                       0,           0,           0,   1);
+        return z.times(p).times(m);
+    }
+
+    /** returns the constraint-conjunction "(forall v)Mv=v" */
+    public AffineConstraint getAffineConstraint(float epsilon) {
+        AffineConstraint c1 = getAffineConstraint(a-1, b,   c,   d  , epsilon);
+        AffineConstraint c2 = getAffineConstraint(e,   f-1, g,   h  , epsilon);
+        AffineConstraint c3 = getAffineConstraint(i,   j,   k-1, l  , epsilon);
+        // FIXME: bottom row constraint?
+        return c1.intersect(c2.intersect(c3, epsilon), epsilon);
+    }
+
+    /** the AffineConstraint representing ax+by+cz+d=0 */
+    private static AffineConstraint getAffineConstraint(float a, float b, float c, float d, float epsilon) {
+        a = Math.abs(a) <= epsilon ? 0 : a;
+        b = Math.abs(b) <= epsilon ? 0 : b;
+        c = Math.abs(c) <= epsilon ? 0 : c;
+        d = Math.abs(d) <= epsilon ? 0 : d;
+        if (a!=0 || b!=0 || c!=0) return new Plane(a, b, c, d);
+        if (d==0) return new AffineConstraint.All();
+        return new AffineConstraint.Nothing();
+    }
 }