src/edu/berkeley/qfat/geom/Matrix.java

   1 package edu.berkeley.qfat.geom;
   2 import javax.media.opengl.*;
   3
   4 // FEATURE: precompute/cache determinant?
   5
   6 /** an affine matrix; immutable */
   7 public class Matrix {
   8
   9     /**
  10      *  <pre>
  11      *  [ a b c d ]   [ x ]
  12      *  [ e f g h ]   [ y ]
  13      *  [ i j k l ]   [ z ]
  14      *  [ m n o p ]   [ 1 ]
  15      *  </pre>
  16      */
  17     public final float a, b, c, d, e, f, g, h, i, j, k, l, m, n, o, p;
  18
  19     /** the zero matrix */
  20     public static final Matrix ZERO          = new Matrix(0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0);
  21
  22     /** the identity matrix */
  23     public static final Matrix ONE           = new Matrix(1,0,0,0,0,1,0,0,0,0,1,0,0,0,0,1);
  24
  25     /** the identity matrix */
  26     public static final Matrix NEGATIVE_ONE  = new Matrix(-1,0,0,0,0,-1,0,0,0,0,-1,0,0,0,0,1);
  27
  28     public Matrix(double a, double b, double c, double d, double e, double f, double g,
  29                   double h, double i, double j, double k, double l, double m, double n, double o, double p) {
  30         this((float)a, (float)b, (float)c, (float)d,
  31              (float)e, (float)f, (float)g, (float)h,
  32              (float)i, (float)j, (float)k, (float)l,
  33              (float)m, (float)n, (float)o, (float)p);
  34     }
  35
  36     public Matrix(float a, float b, float c, float d, float e, float f, float g,
  37                   float h, float i, float j, float k, float l, float m, float n, float o, float p) {
  38         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;
  39         this.j = j; this.k = k; this.l = l; this.m = m; this.n = n; this.o = o; this.p = p;
  40     }
  41
  42     /** a scaling matrix (uniform in all dimensions) */
  43     public static Matrix scale(float scale) { return scale(scale, scale, scale); }
  44
  45     /** a scaling matrix */
  46     public static Matrix scale(float scalex, float scaley, float scalez) {
  47         return new Matrix(scalex, 0, 0, 0, 0, scaley, 0, 0, 0, 0, scalez, 0, 0, 0, 0, 1); }
  48
  49     /** a translation matrix */
  50     public static Matrix translate(Vec translate) {
  51         return new Matrix(1, 0, 0, translate.x,
  52                           0, 1, 0, translate.y,
  53                           0, 0, 1, translate.z,
  54                           0, 0, 0, 1);
  55     }
  56
  57     /** a rotation matrix, <tt>angle</tt> degrees around <tt>axis</tt> */
  58     public static Matrix rotate(Vec axis, float angle) {
  59         float q = (float)Math.cos(angle);
  60         float s = (float)Math.sin(angle);
  61         float t = 1 - q;
  62         float a = (float)(q + axis.x*axis.x*t);
  63         float f = (float)(q + axis.y*axis.y*t);
  64         float k = (float)(q + axis.z*axis.z*t);
  65         float tmp1 = axis.x*axis.y*t;
  66         float tmp2 = axis.z*s;
  67         float e = (float)(tmp1 + tmp2);
  68         float b = (float)(tmp1 - tmp2);
  69         tmp1 = axis.x*axis.z*t;
  70         tmp2 = axis.y*s;
  71         float i = (float)(tmp1 - tmp2);
  72         float c = (float)(tmp1 + tmp2);
  73         tmp1 = axis.y*axis.z*t;
  74         tmp2 = axis.x*s;
  75         float j = (float)(tmp1 + tmp2);
  76         float g = (float)(tmp1 - tmp2);
  77         return new Matrix(a, b, c, 0,
  78                           e, f, g, 0,
  79                           i, j, k, 0,
  80                           0, 0, 0, 1);
  81     }
  82
  83     /** a reflection matrix across the plane passing through the origin with the specified normal */
  84     public static Matrix reflect(Vec v) {
  85         Vec reflectionPlaneNormal = v.norm();
  86         float a = reflectionPlaneNormal.x;
  87         float b = reflectionPlaneNormal.y;
  88         float c = reflectionPlaneNormal.z;
  89         return
  90                 new Matrix( 1-2*a*a,  -2*a*b,  -2*a*c, 0,
  91                             -2*a*b,  1-2*b*b,  -2*b*c, 0,
  92                             -2*a*c,   -2*b*c, 1-2*c*c, 0,
  93                             0,       0,       0,       1);
  94     }
  95
  96     /** returns the translational component of this matrix */
  97     public Vec getTranslationalComponent() { return this.times(Point.ZERO).minus(Point.ZERO); }
  98
  99     public Matrix plus(Matrix x) {
 100         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);
 101     }
 102     public Matrix minus(Matrix x) {
 103         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);
 104     }
 105     public Matrix times(float x) {
 106         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);
 107     }
 108
 109     /** computes (v^T)(this)(v) */
 110     public float preAndPostMultiply(Point point) {
 111         float ret =
 112             ((a*point.x + b*point.y + c*point.z + d) * point.x) +
 113             ((e*point.x + f*point.y + g*point.z + h) * point.y) +
 114             ((i*point.x + j*point.y + k*point.z + l) * point.z) +
 115             ((m*point.x + n*point.y + o*point.z + p) * 1);
 116         return ret;
 117     }
 118
 119     public Point times(Point p) {
 120         double x = a*p.x + b*p.y + c*p.z + d;
 121         double y = e*p.x + f*p.y + g*p.z + h;
 122         double z = i*p.x + j*p.y + k*p.z + l;
 123         double q = m*p.x + n*p.y + o*p.z + this.p;
 124         return new Point(x/q, y/q, z/q);
 125     }
 126
 127     public Vec times(Vec p) {
 128         return this.times(Matrix.translate(p)).getTranslationalComponent();
 129     }
 130
 131     /** multiply by another matrix */
 132     public Matrix times(Matrix z) {
 133         float t00 = a;
 134         float t01 = b;
 135         float t02 = c;
 136         float t03 = d;
 137         float t10 = e;
 138         float t11 = f;
 139         float t12 = g;
 140         float t13 = h;
 141         float t20 = i;
 142         float t21 = j;
 143         float t22 = k;
 144         float t23 = l;
 145         float t30 = m;
 146         float t31 = n;
 147         float t32 = o;
 148         float t33 = p;
 149         float m00 = z.a;
 150         float m01 = z.b;
 151         float m02 = z.c;
 152         float m03 = z.d;
 153         float m10 = z.e;
 154         float m11 = z.f;
 155         float m12 = z.g;
 156         float m13 = z.h;
 157         float m20 = z.i;
 158         float m21 = z.j;
 159         float m22 = z.k;
 160         float m23 = z.l;
 161         float m30 = z.m;
 162         float m31 = z.n;
 163         float m32 = z.o;
 164         float m33 = z.p;
 165         return new Matrix(t00*m00 + t01*m10 + t02*m20 + t03*m30,
 166                           t00*m01 + t01*m11 + t02*m21 + t03*m31,
 167                           t00*m02 + t01*m12 + t02*m22 + t03*m32,
 168                           t00*m03 + t01*m13 + t02*m23 + t03*m33,
 169                           t10*m00 + t11*m10 + t12*m20 + t13*m30,
 170                           t10*m01 + t11*m11 + t12*m21 + t13*m31,
 171                           t10*m02 + t11*m12 + t12*m22 + t13*m32,
 172                           t10*m03 + t11*m13 + t12*m23 + t13*m33,
 173                           t20*m00 + t21*m10 + t22*m20 + t23*m30,
 174                           t20*m01 + t21*m11 + t22*m21 + t23*m31,
 175                           t20*m02 + t21*m12 + t22*m22 + t23*m32,
 176                           t20*m03 + t21*m13 + t22*m23 + t23*m33,
 177                           t30*m00 + t31*m10 + t32*m20 + t33*m30,
 178                           t30*m01 + t31*m11 + t32*m21 + t33*m31,
 179                           t30*m02 + t31*m12 + t32*m22 + t33*m32,
 180                           t30*m03 + t31*m13 + t32*m23 + t33*m33);
 181     }
 182
 183     /** compute the determinant of the matrix */
 184     public float determinant() {
 185         float m00 = a;
 186         float m01 = b;
 187         float m02 = c;
 188         float m03 = d;
 189         float m10 = e;
 190         float m11 = f;
 191         float m12 = g;
 192         float m13 = h;
 193         float m20 = i;
 194         float m21 = j;
 195         float m22 = k;
 196         float m23 = l;
 197         float m30 = m;
 198         float m31 = n;
 199         float m32 = o;
 200         float m33 = p;
 201         return
 202             m03 * m12 * m21 * m30-m02 * m13 * m21 * m30-m03 * m11 * m22 * m30+m01 * m13    * m22 * m30+
 203             m02 * m11 * m23 * m30-m01 * m12 * m23 * m30-m03 * m12 * m20 * m31+m02 * m13    * m20 * m31+
 204             m03 * m10 * m22 * m31-m00 * m13 * m22 * m31-m02 * m10 * m23 * m31+m00 * m12    * m23 * m31+
 205             m03 * m11 * m20 * m32-m01 * m13 * m20 * m32-m03 * m10 * m21 * m32+m00 * m13    * m21 * m32+
 206             m01 * m10 * m23 * m32-m00 * m11 * m23 * m32-m02 * m11 * m20 * m33+m01 * m12    * m20 * m33+
 207             m02 * m10 * m21 * m33-m00 * m12 * m21 * m33-m01 * m10 * m22 * m33+m00 * m11    * m22 * m33;
 208     }
 209
 210     /** compute the inverse of the matrix, returns null if not invertible */
 211     public Matrix inverse() {
 212         float determinant = determinant();
 213         if (determinant==0) return null;
 214         float m00 = a;
 215         float m01 = b;
 216         float m02 = c;
 217         float m03 = d;
 218         float m10 = e;
 219         float m11 = f;
 220         float m12 = g;
 221         float m13 = h;
 222         float m20 = i;
 223         float m21 = j;
 224         float m22 = k;
 225         float m23 = l;
 226         float m30 = m;
 227         float m31 = n;
 228         float m32 = o;
 229         float m33 = p;
 230         return
 231             new Matrix(m12*m23*m31 - m13*m22*m31 + m13*m21*m32 - m11*m23*m32 - m12*m21*m33 + m11*m22*m33,
 232                        m03*m22*m31 - m02*m23*m31 - m03*m21*m32 + m01*m23*m32 + m02*m21*m33 - m01*m22*m33,
 233                        m02*m13*m31 - m03*m12*m31 + m03*m11*m32 - m01*m13*m32 - m02*m11*m33 + m01*m12*m33,
 234                        m03*m12*m21 - m02*m13*m21 - m03*m11*m22 + m01*m13*m22 + m02*m11*m23 - m01*m12*m23,
 235                        m13*m22*m30 - m12*m23*m30 - m13*m20*m32 + m10*m23*m32 + m12*m20*m33 - m10*m22*m33,
 236                        m02*m23*m30 - m03*m22*m30 + m03*m20*m32 - m00*m23*m32 - m02*m20*m33 + m00*m22*m33,
 237                        m03*m12*m30 - m02*m13*m30 - m03*m10*m32 + m00*m13*m32 + m02*m10*m33 - m00*m12*m33,
 238                        m02*m13*m20 - m03*m12*m20 + m03*m10*m22 - m00*m13*m22 - m02*m10*m23 + m00*m12*m23,
 239                        m11*m23*m30 - m13*m21*m30 + m13*m20*m31 - m10*m23*m31 - m11*m20*m33 + m10*m21*m33,
 240                        m03*m21*m30 - m01*m23*m30 - m03*m20*m31 + m00*m23*m31 + m01*m20*m33 - m00*m21*m33,
 241                        m01*m13*m30 - m03*m11*m30 + m03*m10*m31 - m00*m13*m31 - m01*m10*m33 + m00*m11*m33,
 242                        m03*m11*m20 - m01*m13*m20 - m03*m10*m21 + m00*m13*m21 + m01*m10*m23 - m00*m11*m23,
 243                        m12*m21*m30 - m11*m22*m30 - m12*m20*m31 + m10*m22*m31 + m11*m20*m32 - m10*m21*m32,
 244                        m01*m22*m30 - m02*m21*m30 + m02*m20*m31 - m00*m22*m31 - m01*m20*m32 + m00*m21*m32,
 245                        m02*m11*m30 - m01*m12*m30 - m02*m10*m31 + m00*m12*m31 + m01*m10*m32 - m00*m11*m32,
 246                        m01*m12*m20 - m02*m11*m20 + m02*m10*m21 - m00*m12*m21 - m01*m10*m22 + m00*m11*m22)
 247             .times(1/determinant);
 248     }
 249
 250     public String toString() {
 251         return
 252             "\n [ " + a + "\t" + b + "\t" + c + "\t" + d + "\t" + "]" +
 253             "\n [ " + e + "\t" + f + "\t" + g + "\t" + h + "\t" + "]" +
 254             "\n [ " + i + "\t" + j + "\t" + k + "\t" + l + "\t" + "]" +
 255             "\n [ " + m + "\t" + n + "\t" + o + "\t" + p + "\t" + "]\n";
 256     }
 257
 258     public boolean equals(Object oo) {
 259         if (oo==null) return false;
 260         if (!(oo instanceof Matrix)) return false;
 261         Matrix z = (Matrix)oo;
 262         return
 263             (a==z.a) &&
 264             (b==z.b) &&
 265             (c==z.c) &&
 266             (d==z.d) &&
 267             (e==z.e) &&
 268             (f==z.f) &&
 269             (g==z.g) &&
 270             (h==z.h) &&
 271             (i==z.i) &&
 272             (j==z.j) &&
 273             (k==z.k) &&
 274             (l==z.l) &&
 275             (m==z.m) &&
 276             (n==z.n) &&
 277             (o==z.o) &&
 278             (p==z.p);
 279     }
 280
 281     public boolean equalsModuloEpsilon(Matrix z, float epsilon) {
 282         return
 283             Math.abs(a-z.a)<epsilon &&
 284             Math.abs(b-z.b)<epsilon &&
 285             Math.abs(c-z.c)<epsilon &&
 286             Math.abs(d-z.d)<epsilon &&
 287             Math.abs(e-z.e)<epsilon &&
 288             Math.abs(f-z.f)<epsilon &&
 289             Math.abs(g-z.g)<epsilon &&
 290             Math.abs(h-z.h)<epsilon &&
 291             Math.abs(i-z.i)<epsilon &&
 292             Math.abs(j-z.j)<epsilon &&
 293             Math.abs(k-z.k)<epsilon &&
 294             Math.abs(l-z.l)<epsilon &&
 295             Math.abs(m-z.m)<epsilon &&
 296             Math.abs(n-z.n)<epsilon &&
 297             Math.abs(o-z.o)<epsilon &&
 298             Math.abs(p-z.p)<epsilon;
 299     }
 300
 301     public int hashCode() {
 302         return
 303             Float.floatToIntBits(a) ^
 304             Float.floatToIntBits(b) ^
 305             Float.floatToIntBits(c) ^
 306             Float.floatToIntBits(d) ^
 307             Float.floatToIntBits(e) ^
 308             Float.floatToIntBits(f) ^
 309             Float.floatToIntBits(g) ^
 310             Float.floatToIntBits(h) ^
 311             Float.floatToIntBits(i) ^
 312             Float.floatToIntBits(j) ^
 313             Float.floatToIntBits(k) ^
 314             Float.floatToIntBits(l) ^
 315             Float.floatToIntBits(m) ^
 316             Float.floatToIntBits(n) ^
 317             Float.floatToIntBits(o) ^
 318             Float.floatToIntBits(p);
 319     }
 320
 321     /** retrieves the current objectspace-to-windowspace projection matrix from the GL context */
 322     public static Matrix getProjectionMatrix(GL gl) {
 323         int view[] = new int[4];
 324         double mvmatrix[] = new double[16];
 325         double projmatrix[] = new double[16];
 326         gl.glGetIntegerv(GL.GL_VIEWPORT, view, 0);
 327         gl.glGetDoublev(GL.GL_MODELVIEW_MATRIX, mvmatrix, 0);
 328         gl.glGetDoublev(GL.GL_PROJECTION_MATRIX, projmatrix, 0);
 329         Matrix m = new Matrix(mvmatrix[0],
 330                               mvmatrix[4],
 331                               mvmatrix[8],
 332                               mvmatrix[12],
 333                               mvmatrix[1],
 334                               mvmatrix[5],
 335                               mvmatrix[9],
 336                               mvmatrix[13],
 337                               mvmatrix[2],
 338                               mvmatrix[6],
 339                               mvmatrix[10],
 340                               mvmatrix[14],
 341                               mvmatrix[3],
 342                               mvmatrix[7],
 343                               mvmatrix[11],
 344                               mvmatrix[15]);
 345         Matrix p = new Matrix(projmatrix[0],
 346                               projmatrix[4],
 347                               projmatrix[8],
 348                               projmatrix[12],
 349                               projmatrix[1],
 350                               projmatrix[5],
 351                               projmatrix[9],
 352                               projmatrix[13],
 353                               projmatrix[2],
 354                               projmatrix[6],
 355                               projmatrix[10],
 356                               projmatrix[14],
 357                               projmatrix[3],
 358                               projmatrix[7],
 359                               projmatrix[11],
 360                               projmatrix[15]);
 361         Matrix z =
 362             new Matrix(0.5*view[2], 0,           0,   view[0]+view[2]*0.5,
 363                        0,           0.5*view[3], 0,   view[1]+view[3]*0.5,
 364                        0,           0,           0.5, 0.5,
 365                        0,           0,           0,   1);
 366         return z.times(p).times(m);
 367     }
 368
 369     /** returns the constraint-conjunction "(forall v)Mv=v" */
 370     public AffineConstraint getConstraint(float epsilon) {
 371         AffineConstraint c1 = getConstraint(a-1, b,   c,   d  , epsilon);
 372         AffineConstraint c2 = getConstraint(e,   f-1, g,   h  , epsilon);
 373         AffineConstraint c3 = getConstraint(i,   j,   k-1, l  , epsilon);
 374         // FIXME: bottom row constraint?
 375         return c1.intersect(c2.intersect(c3, epsilon), epsilon);
 376     }
 377
 378     /** the AffineConstraint representing ax+by+cz+d=0 */
 379     private static AffineConstraint getConstraint(float a, float b, float c, float d, float epsilon) {
 380         a = Math.abs(a) <= epsilon ? 0 : a;
 381         b = Math.abs(b) <= epsilon ? 0 : b;
 382         c = Math.abs(c) <= epsilon ? 0 : c;
 383         d = Math.abs(d) <= epsilon ? 0 : d;
 384         if (a!=0 || b!=0 || c!=0) return new Plane(a, b, c, d);
 385         if (d==0) return new AffineConstraint.All();
 386         return new AffineConstraint.Nothing();
 387     }
 388 }