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

diff --git a/src/edu/berkeley/qfat/geom/Matrix.java b/src/edu/berkeley/qfat/geom/Matrix.java
index f617376..c2419d6 100644
--- a/src/edu/berkeley/qfat/geom/Matrix.java
+++ b/src/edu/berkeley/qfat/geom/Matrix.java
@@ -1,75 +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 ]
-    //
+
+    /**
+     *  <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;
-    public Matrix() { this(1); }
-    public Matrix(float scale) {
-        a = f = k = scale;
-        l = h = d = e = b = i = c = j = g = 0;            
-        m = n = o = 0;
-        p = 1;
-    }
-    public Matrix(float scalex, float scaley, float scalez) {
-        a = scalex;
-        f = scaley;
-        k = scalez;
-        l = h = d = e = b = i = c = j = g = 0;            
-        m = n = o = 0;
-        p = 1;
-    }
-    public Matrix(Vec translate) {
-        d = translate.x; h = translate.y; l = translate.z;
-        a = f = k = 1;
-        b = c = e = g = i = j = 0;
-        m = n = o = 0;
-        p = 1;
+
+    /** 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.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, m*x, n*x, o*x, p*x);
+
+    /** 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);
     }
-    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 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;
-        m = n = o = 0;
-        p = 1;
+        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) {
-        // discards bottom row
-        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();
+    }
 }