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     public static Matrix reflect(Vec v) {
  84         Vec reflectionPlaneNormal = v.norm();
  85         float a = reflectionPlaneNormal.x;
  86         float b = reflectionPlaneNormal.y;
  87         float c = reflectionPlaneNormal.z;
  88         return
  89                 new Matrix( 1-2*a*a,  -2*a*b,  -2*a*c, 0,
  90                             -2*a*b,  1-2*b*b,  -2*b*c, 0,
  91                             -2*a*c,   -2*b*c, 1-2*c*c, 0,
  92                             0,       0,       0,       1);
  93     }
  94
  95     public Vec getTranslationalComponent() {
  96         return new Vec(d, h, l);
  97     }
  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     /** discards bottom row */
 120     public Point times(Point p) {
 121         // discards bottom row
 122         double x = a*p.x + b*p.y + c*p.z + d;
 123         double y = e*p.x + f*p.y + g*p.z + h;
 124         double z = i*p.x + j*p.y + k*p.z + l;
 125         double q = m*p.x + n*p.y + o*p.z + this.p;
 126         return new Point(x/q, y/q, z/q);
 127     }
 128
 129     /** discards bottom row */
 130     public Vec times(Vec p) {
 131         return new Vec(a*p.x + b*p.y + c*p.z + d,
 132                        e*p.x + f*p.y + g*p.z + h,
 133                        i*p.x + j*p.y + k*p.z + l);
 134     }
 135
 136     public Matrix preMultiplyTranslationalComponentBy(Matrix mm) {
 137         Vec v = mm.times(getTranslationalComponent());
 138         return new Matrix(a, b, c, v.x,
 139                           e, f, g, v.y,
 140                           i, j, k, v.z,
 141                           m, n, o, 1);
 142     }
 143
 144     /** multiply by another matrix */
 145     public Matrix times(Matrix z) {
 146         float t00 = a;
 147         float t01 = b;
 148         float t02 = c;
 149         float t03 = d;
 150         float t10 = e;
 151         float t11 = f;
 152         float t12 = g;
 153         float t13 = h;
 154         float t20 = i;
 155         float t21 = j;
 156         float t22 = k;
 157         float t23 = l;
 158         float t30 = m;
 159         float t31 = n;
 160         float t32 = o;
 161         float t33 = p;
 162         float m00 = z.a;
 163         float m01 = z.b;
 164         float m02 = z.c;
 165         float m03 = z.d;
 166         float m10 = z.e;
 167         float m11 = z.f;
 168         float m12 = z.g;
 169         float m13 = z.h;
 170         float m20 = z.i;
 171         float m21 = z.j;
 172         float m22 = z.k;
 173         float m23 = z.l;
 174         float m30 = z.m;
 175         float m31 = z.n;
 176         float m32 = z.o;
 177         float m33 = z.p;
 178         return new Matrix(t00*m00 + t01*m10 + t02*m20 + t03*m30,
 179                           t00*m01 + t01*m11 + t02*m21 + t03*m31,
 180                           t00*m02 + t01*m12 + t02*m22 + t03*m32,
 181                           t00*m03 + t01*m13 + t02*m23 + t03*m33,
 182                           t10*m00 + t11*m10 + t12*m20 + t13*m30,
 183                           t10*m01 + t11*m11 + t12*m21 + t13*m31,
 184                           t10*m02 + t11*m12 + t12*m22 + t13*m32,
 185                           t10*m03 + t11*m13 + t12*m23 + t13*m33,
 186                           t20*m00 + t21*m10 + t22*m20 + t23*m30,
 187                           t20*m01 + t21*m11 + t22*m21 + t23*m31,
 188                           t20*m02 + t21*m12 + t22*m22 + t23*m32,
 189                           t20*m03 + t21*m13 + t22*m23 + t23*m33,
 190                           t30*m00 + t31*m10 + t32*m20 + t33*m30,
 191                           t30*m01 + t31*m11 + t32*m21 + t33*m31,
 192                           t30*m02 + t31*m12 + t32*m22 + t33*m32,
 193                           t30*m03 + t31*m13 + t32*m23 + t33*m33);
 194     }
 195
 196     /** compute the determinant of the matrix */
 197     public float determinant() {
 198         float m00 = a;
 199         float m01 = b;
 200         float m02 = c;
 201         float m03 = d;
 202         float m10 = e;
 203         float m11 = f;
 204         float m12 = g;
 205         float m13 = h;
 206         float m20 = i;
 207         float m21 = j;
 208         float m22 = k;
 209         float m23 = l;
 210         float m30 = m;
 211         float m31 = n;
 212         float m32 = o;
 213         float m33 = p;
 214         return
 215             m03 * m12 * m21 * m30-m02 * m13 * m21 * m30-m03 * m11 * m22 * m30+m01 * m13    * m22 * m30+
 216             m02 * m11 * m23 * m30-m01 * m12 * m23 * m30-m03 * m12 * m20 * m31+m02 * m13    * m20 * m31+
 217             m03 * m10 * m22 * m31-m00 * m13 * m22 * m31-m02 * m10 * m23 * m31+m00 * m12    * m23 * m31+
 218             m03 * m11 * m20 * m32-m01 * m13 * m20 * m32-m03 * m10 * m21 * m32+m00 * m13    * m21 * m32+
 219             m01 * m10 * m23 * m32-m00 * m11 * m23 * m32-m02 * m11 * m20 * m33+m01 * m12    * m20 * m33+
 220             m02 * m10 * m21 * m33-m00 * m12 * m21 * m33-m01 * m10 * m22 * m33+m00 * m11    * m22 * m33;
 221     }
 222
 223     /** compute the inverse of the matrix, returns null if not invertible */
 224     public Matrix inverse() {
 225         float determinant = determinant();
 226         if (determinant==0) return null;
 227         float m00 = a;
 228         float m01 = b;
 229         float m02 = c;
 230         float m03 = d;
 231         float m10 = e;
 232         float m11 = f;
 233         float m12 = g;
 234         float m13 = h;
 235         float m20 = i;
 236         float m21 = j;
 237         float m22 = k;
 238         float m23 = l;
 239         float m30 = m;
 240         float m31 = n;
 241         float m32 = o;
 242         float m33 = p;
 243         return
 244             new Matrix(m12*m23*m31 - m13*m22*m31 + m13*m21*m32 - m11*m23*m32 - m12*m21*m33 + m11*m22*m33,
 245                        m03*m22*m31 - m02*m23*m31 - m03*m21*m32 + m01*m23*m32 + m02*m21*m33 - m01*m22*m33,
 246                        m02*m13*m31 - m03*m12*m31 + m03*m11*m32 - m01*m13*m32 - m02*m11*m33 + m01*m12*m33,
 247                        m03*m12*m21 - m02*m13*m21 - m03*m11*m22 + m01*m13*m22 + m02*m11*m23 - m01*m12*m23,
 248                        m13*m22*m30 - m12*m23*m30 - m13*m20*m32 + m10*m23*m32 + m12*m20*m33 - m10*m22*m33,
 249                        m02*m23*m30 - m03*m22*m30 + m03*m20*m32 - m00*m23*m32 - m02*m20*m33 + m00*m22*m33,
 250                        m03*m12*m30 - m02*m13*m30 - m03*m10*m32 + m00*m13*m32 + m02*m10*m33 - m00*m12*m33,
 251                        m02*m13*m20 - m03*m12*m20 + m03*m10*m22 - m00*m13*m22 - m02*m10*m23 + m00*m12*m23,
 252                        m11*m23*m30 - m13*m21*m30 + m13*m20*m31 - m10*m23*m31 - m11*m20*m33 + m10*m21*m33,
 253                        m03*m21*m30 - m01*m23*m30 - m03*m20*m31 + m00*m23*m31 + m01*m20*m33 - m00*m21*m33,
 254                        m01*m13*m30 - m03*m11*m30 + m03*m10*m31 - m00*m13*m31 - m01*m10*m33 + m00*m11*m33,
 255                        m03*m11*m20 - m01*m13*m20 - m03*m10*m21 + m00*m13*m21 + m01*m10*m23 - m00*m11*m23,
 256                        m12*m21*m30 - m11*m22*m30 - m12*m20*m31 + m10*m22*m31 + m11*m20*m32 - m10*m21*m32,
 257                        m01*m22*m30 - m02*m21*m30 + m02*m20*m31 - m00*m22*m31 - m01*m20*m32 + m00*m21*m32,
 258                        m02*m11*m30 - m01*m12*m30 - m02*m10*m31 + m00*m12*m31 + m01*m10*m32 - m00*m11*m32,
 259                        m01*m12*m20 - m02*m11*m20 + m02*m10*m21 - m00*m12*m21 - m01*m10*m22 + m00*m11*m22)
 260             .times(1/determinant);
 261     }
 262
 263     public String toString() {
 264         return
 265             "\n [ " + a + "\t" + b + "\t" + c + "\t" + d + "\t" + "]" +
 266             "\n [ " + e + "\t" + f + "\t" + g + "\t" + h + "\t" + "]" +
 267             "\n [ " + i + "\t" + j + "\t" + k + "\t" + l + "\t" + "]" +
 268             "\n [ " + m + "\t" + n + "\t" + o + "\t" + p + "\t" + "]\n";
 269     }
 270
 271     public boolean equals(Object oo) {
 272         if (oo==null) return false;
 273         if (!(oo instanceof Matrix)) return false;
 274         Matrix z = (Matrix)oo;
 275         return
 276             near(a,z.a) &&
 277             near(b,z.b) &&
 278             near(c,z.c) &&
 279             near(d,z.d) &&
 280             near(e,z.e) &&
 281             near(f,z.f) &&
 282             near(g,z.g) &&
 283             near(h,z.h) &&
 284             near(i,z.i) &&
 285             near(j,z.j) &&
 286             near(k,z.k) &&
 287             near(l,z.l) &&
 288             near(m,z.m) &&
 289             near(n,z.n) &&
 290             near(o,z.o) &&
 291             near(p,z.p);
 292     }
 293     private static final float EPSILON = 0.001f;
 294     private static boolean near(float a, float b) { return Math.abs(a-b)<EPSILON; }
 295
 296     public int hashCode() {
 297         return
 298             Float.floatToIntBits(a) ^
 299             Float.floatToIntBits(b) ^
 300             Float.floatToIntBits(c) ^
 301             Float.floatToIntBits(d) ^
 302             Float.floatToIntBits(e) ^
 303             Float.floatToIntBits(f) ^
 304             Float.floatToIntBits(g) ^
 305             Float.floatToIntBits(h) ^
 306             Float.floatToIntBits(i) ^
 307             Float.floatToIntBits(j) ^
 308             Float.floatToIntBits(k) ^
 309             Float.floatToIntBits(l) ^
 310             Float.floatToIntBits(m) ^
 311             Float.floatToIntBits(n) ^
 312             Float.floatToIntBits(o) ^
 313             Float.floatToIntBits(p);
 314     }
 315
 316     public static Matrix getProjectionMatrix(GL gl) {
 317         int view[] = new int[4];
 318         double mvmatrix[] = new double[16];
 319         double projmatrix[] = new double[16];
 320         gl.glGetIntegerv(GL.GL_VIEWPORT, view, 0);
 321         gl.glGetDoublev(GL.GL_MODELVIEW_MATRIX, mvmatrix, 0);
 322         gl.glGetDoublev(GL.GL_PROJECTION_MATRIX, projmatrix, 0);
 323         Matrix m = new Matrix(mvmatrix[0],
 324                               mvmatrix[4],
 325                               mvmatrix[8],
 326                               mvmatrix[12],
 327                               mvmatrix[1],
 328                               mvmatrix[5],
 329                               mvmatrix[9],
 330                               mvmatrix[13],
 331                               mvmatrix[2],
 332                               mvmatrix[6],
 333                               mvmatrix[10],
 334                               mvmatrix[14],
 335                               mvmatrix[3],
 336                               mvmatrix[7],
 337                               mvmatrix[11],
 338                               mvmatrix[15]);
 339         Matrix p = new Matrix(projmatrix[0],
 340                               projmatrix[4],
 341                               projmatrix[8],
 342                               projmatrix[12],
 343                               projmatrix[1],
 344                               projmatrix[5],
 345                               projmatrix[9],
 346                               projmatrix[13],
 347                               projmatrix[2],
 348                               projmatrix[6],
 349                               projmatrix[10],
 350                               projmatrix[14],
 351                               projmatrix[3],
 352                               projmatrix[7],
 353                               projmatrix[11],
 354                               projmatrix[15]);
 355         Matrix z =
 356             new Matrix(0.5*view[2], 0,           0,   view[0]+view[2]*0.5,
 357                        0,           0.5*view[3], 0,   view[1]+view[3]*0.5,
 358                        0,           0,           0.5, 0.5,
 359                        0,           0,           0,   1);
 360         return z.times(p).times(m);
 361     }
 362
 363 }