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

   1 package edu.berkeley.qfat.geom;
   2
   3 // FEATURE: precompute/cache determinant?
   4
   5 /** an affine matrix; immutable */
   6 public class Matrix {
   7
   8     /**
   9      *  <pre>
  10      *  [ a b c d ]   [ x ]
  11      *  [ e f g h ]   [ y ]
  12      *  [ i j k l ]   [ z ]
  13      *  [ m n o p ]   [ 1 ]
  14      *  </pre>
  15      */
  16     public final float a, b, c, d, e, f, g, h, i, j, k, l, m, n, o, p;
  17
  18     /** the zero matrix */
  19     public static final Matrix ZERO          = new Matrix(0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0);
  20
  21     /** the identity matrix */
  22     public static final Matrix ONE           = new Matrix(1,0,0,0,0,1,0,0,0,0,1,0,0,0,0,1);
  23
  24     /** the identity matrix */
  25     public static final Matrix NEGATIVE_ONE  = new Matrix(-1,0,0,0,0,-1,0,0,0,0,-1,0,0,0,0,1);
  26
  27     public Matrix(float a, float b, float c, float d, float e, float f, float g,
  28                   float h, float i, float j, float k, float l, float m, float n, float o, float p) {
  29         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;
  30         this.j = j; this.k = k; this.l = l; this.m = m; this.n = n; this.o = o; this.p = p;
  31     }
  32
  33     /** a scaling matrix (uniform in all dimensions) */
  34     public static Matrix scale(float scale) { return scale(scale, scale, scale); }
  35
  36     /** a scaling matrix */
  37     public static Matrix scale(float scalex, float scaley, float scalez) {
  38         return new Matrix(scalex, 0, 0, 0, 0, scaley, 0, 0, 0, 0, scalez, 0, 0, 0, 0, 1); }
  39
  40     /** a translation matrix */
  41     public static Matrix translate(Vec translate) {
  42         return new Matrix(1, 0, 0, translate.x,
  43                           0, 1, 0, translate.y,
  44                           0, 0, 1, translate.z,
  45                           0, 0, 0, 1);
  46     }
  47
  48     /** a rotation matrix, <tt>angle</tt> degrees around <tt>axis</tt> */
  49     public static Matrix rotate(Vec axis, float angle) {
  50         float q = (float)Math.cos(angle);
  51         float s = (float)Math.sin(angle);
  52         float t = 1 - q;
  53         float a = (float)(q + axis.x*axis.x*t);
  54         float f = (float)(q + axis.y*axis.y*t);
  55         float k = (float)(q + axis.z*axis.z*t);
  56         float tmp1 = axis.x*axis.y*t;
  57         float tmp2 = axis.z*s;
  58         float e = (float)(tmp1 + tmp2);
  59         float b = (float)(tmp1 - tmp2);
  60         tmp1 = axis.x*axis.z*t;
  61         tmp2 = axis.y*s;
  62         float i = (float)(tmp1 - tmp2);
  63         float c = (float)(tmp1 + tmp2);
  64         tmp1 = axis.y*axis.z*t;
  65         tmp2 = axis.x*s;
  66         float j = (float)(tmp1 + tmp2);
  67         float g = (float)(tmp1 - tmp2);
  68         return new Matrix(a, b, c, 0,
  69                           e, f, g, 0,
  70                           i, j, k, 0,
  71                           0, 0, 0, 1);
  72     }
  73
  74     public Matrix plus(Matrix x) {
  75         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);
  76     }
  77     public Matrix minus(Matrix x) {
  78         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);
  79     }
  80     public Matrix times(float x) {
  81         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);
  82     }
  83
  84     /** computes (v^T)(this)(v) */
  85     public float preAndPostMultiply(Point point) {
  86         float ret =
  87             ((a*point.x + b*point.y + c*point.z + d) * point.x) +
  88             ((e*point.x + f*point.y + g*point.z + h) * point.y) +
  89             ((i*point.x + j*point.y + k*point.z + l) * point.z) +
  90             ((m*point.x + n*point.y + o*point.z + p) * 1);
  91         return ret;
  92     }
  93
  94     /** discards bottom row */
  95     public Point times(Point p) {
  96         // discards bottom row
  97         return new Point(a*p.x + b*p.y + c*p.z + d,
  98                          e*p.x + f*p.y + g*p.z + h,
  99                          i*p.x + j*p.y + k*p.z + l);
 100     }
 101
 102     /** discards bottom row */
 103     public Vec times(Vec p) {
 104         return new Vec(a*p.x + b*p.y + c*p.z + d,
 105                        e*p.x + f*p.y + g*p.z + h,
 106                        i*p.x + j*p.y + k*p.z + l);
 107     }
 108
 109     /** multiply by another matrix */
 110     public Matrix times(Matrix z) {
 111         float t00 = a;
 112         float t01 = b;
 113         float t02 = c;
 114         float t03 = d;
 115         float t10 = e;
 116         float t11 = f;
 117         float t12 = g;
 118         float t13 = h;
 119         float t20 = i;
 120         float t21 = j;
 121         float t22 = k;
 122         float t23 = l;
 123         float t30 = m;
 124         float t31 = n;
 125         float t32 = o;
 126         float t33 = p;
 127         float m00 = z.a;
 128         float m01 = z.b;
 129         float m02 = z.c;
 130         float m03 = z.d;
 131         float m10 = z.e;
 132         float m11 = z.f;
 133         float m12 = z.g;
 134         float m13 = z.h;
 135         float m20 = z.i;
 136         float m21 = z.j;
 137         float m22 = z.k;
 138         float m23 = z.l;
 139         float m30 = z.m;
 140         float m31 = z.n;
 141         float m32 = z.o;
 142         float m33 = z.p;
 143         return new Matrix(t00*m00 + t01*m10 + t02*m20 + t03*m30,
 144                           t00*m01 + t01*m11 + t02*m21 + t03*m31,
 145                           t00*m02 + t01*m12 + t02*m22 + t03*m32,
 146                           t00*m03 + t01*m13 + t02*m23 + t03*m33,
 147                           t10*m00 + t11*m10 + t12*m20 + t13*m30,
 148                           t10*m01 + t11*m11 + t12*m21 + t13*m31,
 149                           t10*m02 + t11*m12 + t12*m22 + t13*m32,
 150                           t10*m03 + t11*m13 + t12*m23 + t13*m33,
 151                           t20*m00 + t21*m10 + t22*m20 + t23*m30,
 152                           t20*m01 + t21*m11 + t22*m21 + t23*m31,
 153                           t20*m02 + t21*m12 + t22*m22 + t23*m32,
 154                           t20*m03 + t21*m13 + t22*m23 + t23*m33,
 155                           t30*m00 + t31*m10 + t32*m20 + t33*m30,
 156                           t30*m01 + t31*m11 + t32*m21 + t33*m31,
 157                           t30*m02 + t31*m12 + t32*m22 + t33*m32,
 158                           t30*m03 + t31*m13 + t32*m23 + t33*m33);
 159     }
 160
 161     /** compute the determinant of the matrix */
 162     public float determinant() {
 163         float m00 = a;
 164         float m01 = b;
 165         float m02 = c;
 166         float m03 = d;
 167         float m10 = e;
 168         float m11 = f;
 169         float m12 = g;
 170         float m13 = h;
 171         float m20 = i;
 172         float m21 = j;
 173         float m22 = k;
 174         float m23 = l;
 175         float m30 = m;
 176         float m31 = n;
 177         float m32 = o;
 178         float m33 = p;
 179         return
 180             m03 * m12 * m21 * m30-m02 * m13 * m21 * m30-m03 * m11 * m22 * m30+m01 * m13    * m22 * m30+
 181             m02 * m11 * m23 * m30-m01 * m12 * m23 * m30-m03 * m12 * m20 * m31+m02 * m13    * m20 * m31+
 182             m03 * m10 * m22 * m31-m00 * m13 * m22 * m31-m02 * m10 * m23 * m31+m00 * m12    * m23 * m31+
 183             m03 * m11 * m20 * m32-m01 * m13 * m20 * m32-m03 * m10 * m21 * m32+m00 * m13    * m21 * m32+
 184             m01 * m10 * m23 * m32-m00 * m11 * m23 * m32-m02 * m11 * m20 * m33+m01 * m12    * m20 * m33+
 185             m02 * m10 * m21 * m33-m00 * m12 * m21 * m33-m01 * m10 * m22 * m33+m00 * m11    * m22 * m33;
 186     }
 187
 188     /** compute the inverse of the matrix, returns null if not invertible */
 189     public Matrix inverse() {
 190         float determinant = determinant();
 191         if (determinant==0) return null;
 192         float m00 = a;
 193         float m01 = b;
 194         float m02 = c;
 195         float m03 = d;
 196         float m10 = e;
 197         float m11 = f;
 198         float m12 = g;
 199         float m13 = h;
 200         float m20 = i;
 201         float m21 = j;
 202         float m22 = k;
 203         float m23 = l;
 204         float m30 = m;
 205         float m31 = n;
 206         float m32 = o;
 207         float m33 = p;
 208         return
 209             new Matrix(m12*m23*m31 - m13*m22*m31 + m13*m21*m32 - m11*m23*m32 - m12*m21*m33 + m11*m22*m33,
 210                        m03*m22*m31 - m02*m23*m31 - m03*m21*m32 + m01*m23*m32 + m02*m21*m33 - m01*m22*m33,
 211                        m02*m13*m31 - m03*m12*m31 + m03*m11*m32 - m01*m13*m32 - m02*m11*m33 + m01*m12*m33,
 212                        m03*m12*m21 - m02*m13*m21 - m03*m11*m22 + m01*m13*m22 + m02*m11*m23 - m01*m12*m23,
 213                        m13*m22*m30 - m12*m23*m30 - m13*m20*m32 + m10*m23*m32 + m12*m20*m33 - m10*m22*m33,
 214                        m02*m23*m30 - m03*m22*m30 + m03*m20*m32 - m00*m23*m32 - m02*m20*m33 + m00*m22*m33,
 215                        m03*m12*m30 - m02*m13*m30 - m03*m10*m32 + m00*m13*m32 + m02*m10*m33 - m00*m12*m33,
 216                        m02*m13*m20 - m03*m12*m20 + m03*m10*m22 - m00*m13*m22 - m02*m10*m23 + m00*m12*m23,
 217                        m11*m23*m30 - m13*m21*m30 + m13*m20*m31 - m10*m23*m31 - m11*m20*m33 + m10*m21*m33,
 218                        m03*m21*m30 - m01*m23*m30 - m03*m20*m31 + m00*m23*m31 + m01*m20*m33 - m00*m21*m33,
 219                        m01*m13*m30 - m03*m11*m30 + m03*m10*m31 - m00*m13*m31 - m01*m10*m33 + m00*m11*m33,
 220                        m03*m11*m20 - m01*m13*m20 - m03*m10*m21 + m00*m13*m21 + m01*m10*m23 - m00*m11*m23,
 221                        m12*m21*m30 - m11*m22*m30 - m12*m20*m31 + m10*m22*m31 + m11*m20*m32 - m10*m21*m32,
 222                        m01*m22*m30 - m02*m21*m30 + m02*m20*m31 - m00*m22*m31 - m01*m20*m32 + m00*m21*m32,
 223                        m02*m11*m30 - m01*m12*m30 - m02*m10*m31 + m00*m12*m31 + m01*m10*m32 - m00*m11*m32,
 224                        m01*m12*m20 - m02*m11*m20 + m02*m10*m21 - m00*m12*m21 - m01*m10*m22 + m00*m11*m22)
 225             .times(1/determinant);
 226     }
 227
 228     public String toString() {
 229         return
 230             "\n [ " + a + "\t" + b + "\t" + c + "\t" + d + "\t" + "]" +
 231             "\n [ " + e + "\t" + f + "\t" + g + "\t" + h + "\t" + "]" +
 232             "\n [ " + i + "\t" + j + "\t" + k + "\t" + l + "\t" + "]" +
 233             "\n [ " + m + "\t" + n + "\t" + o + "\t" + p + "\t" + "]\n";
 234     }
 235
 236     public boolean equals(Object oo) {
 237         if (oo==null) return false;
 238         if (!(oo instanceof Matrix)) return false;
 239         Matrix z = (Matrix)oo;
 240         return
 241             near(a,z.a) &&
 242             near(b,z.b) &&
 243             near(c,z.c) &&
 244             near(d,z.d) &&
 245             near(e,z.e) &&
 246             near(f,z.f) &&
 247             near(g,z.g) &&
 248             near(h,z.h) &&
 249             near(i,z.i) &&
 250             near(j,z.j) &&
 251             near(k,z.k) &&
 252             near(l,z.l) &&
 253             near(m,z.m) &&
 254             near(n,z.n) &&
 255             near(o,z.o) &&
 256             near(p,z.p);
 257     }
 258     private static final float EPSILON = 0.001f;
 259     private static boolean near(float a, float b) { return a==b; }
 260
 261     public int hashCode() {
 262         return
 263             Float.floatToIntBits(a) ^
 264             Float.floatToIntBits(b) ^
 265             Float.floatToIntBits(c) ^
 266             Float.floatToIntBits(d) ^
 267             Float.floatToIntBits(e) ^
 268             Float.floatToIntBits(f) ^
 269             Float.floatToIntBits(g) ^
 270             Float.floatToIntBits(h) ^
 271             Float.floatToIntBits(i) ^
 272             Float.floatToIntBits(j) ^
 273             Float.floatToIntBits(k) ^
 274             Float.floatToIntBits(l) ^
 275             Float.floatToIntBits(m) ^
 276             Float.floatToIntBits(n) ^
 277             Float.floatToIntBits(o) ^
 278             Float.floatToIntBits(p);
 279     }
 280 }