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