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 static Matrix reflect(Vec v) {
  75         Vec reflectionPlaneNormal = v.norm();
  76         float a = reflectionPlaneNormal.x;
  77         float b = reflectionPlaneNormal.y;
  78         float c = reflectionPlaneNormal.z;
  79         return
  80                 new Matrix( 1-2*a*a,  -2*a*b,  -2*a*c, 0,
  81                             -2*a*b,  1-2*b*b,  -2*b*c, 0,
  82                             -2*a*c,   -2*b*c, 1-2*c*c, 0,
  83                             0,       0,       0,       1);
  84     }
  85
  86     public Vec getTranslationalComponent() {
  87         return new Vec(d, h, l);
  88     }
  89
  90     public Matrix plus(Matrix x) {
  91         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);
  92     }
  93     public Matrix minus(Matrix x) {
  94         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);
  95     }
  96     public Matrix times(float x) {
  97         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);
  98     }
  99
 100     /** computes (v^T)(this)(v) */
 101     public float preAndPostMultiply(Point point) {
 102         float ret =
 103             ((a*point.x + b*point.y + c*point.z + d) * point.x) +
 104             ((e*point.x + f*point.y + g*point.z + h) * point.y) +
 105             ((i*point.x + j*point.y + k*point.z + l) * point.z) +
 106             ((m*point.x + n*point.y + o*point.z + p) * 1);
 107         return ret;
 108     }
 109
 110     /** discards bottom row */
 111     public Point times(Point p) {
 112         // discards bottom row
 113         return new Point(a*p.x + b*p.y + c*p.z + d,
 114                          e*p.x + f*p.y + g*p.z + h,
 115                          i*p.x + j*p.y + k*p.z + l);
 116     }
 117
 118     /** discards bottom row */
 119     public Vec times(Vec p) {
 120         return new Vec(a*p.x + b*p.y + c*p.z + d,
 121                        e*p.x + f*p.y + g*p.z + h,
 122                        i*p.x + j*p.y + k*p.z + l);
 123     }
 124
 125     public Matrix preMultiplyTranslationalComponentBy(Matrix mm) {
 126         Vec v = mm.times(getTranslationalComponent());
 127         return new Matrix(a, b, c, v.x,
 128                           e, f, g, v.y,
 129                           i, j, k, v.z,
 130                           m, n, o, 1);
 131     }
 132
 133     /** multiply by another matrix */
 134     public Matrix times(Matrix z) {
 135         float t00 = a;
 136         float t01 = b;
 137         float t02 = c;
 138         float t03 = d;
 139         float t10 = e;
 140         float t11 = f;
 141         float t12 = g;
 142         float t13 = h;
 143         float t20 = i;
 144         float t21 = j;
 145         float t22 = k;
 146         float t23 = l;
 147         float t30 = m;
 148         float t31 = n;
 149         float t32 = o;
 150         float t33 = p;
 151         float m00 = z.a;
 152         float m01 = z.b;
 153         float m02 = z.c;
 154         float m03 = z.d;
 155         float m10 = z.e;
 156         float m11 = z.f;
 157         float m12 = z.g;
 158         float m13 = z.h;
 159         float m20 = z.i;
 160         float m21 = z.j;
 161         float m22 = z.k;
 162         float m23 = z.l;
 163         float m30 = z.m;
 164         float m31 = z.n;
 165         float m32 = z.o;
 166         float m33 = z.p;
 167         return new Matrix(t00*m00 + t01*m10 + t02*m20 + t03*m30,
 168                           t00*m01 + t01*m11 + t02*m21 + t03*m31,
 169                           t00*m02 + t01*m12 + t02*m22 + t03*m32,
 170                           t00*m03 + t01*m13 + t02*m23 + t03*m33,
 171                           t10*m00 + t11*m10 + t12*m20 + t13*m30,
 172                           t10*m01 + t11*m11 + t12*m21 + t13*m31,
 173                           t10*m02 + t11*m12 + t12*m22 + t13*m32,
 174                           t10*m03 + t11*m13 + t12*m23 + t13*m33,
 175                           t20*m00 + t21*m10 + t22*m20 + t23*m30,
 176                           t20*m01 + t21*m11 + t22*m21 + t23*m31,
 177                           t20*m02 + t21*m12 + t22*m22 + t23*m32,
 178                           t20*m03 + t21*m13 + t22*m23 + t23*m33,
 179                           t30*m00 + t31*m10 + t32*m20 + t33*m30,
 180                           t30*m01 + t31*m11 + t32*m21 + t33*m31,
 181                           t30*m02 + t31*m12 + t32*m22 + t33*m32,
 182                           t30*m03 + t31*m13 + t32*m23 + t33*m33);
 183     }
 184
 185     /** compute the determinant of the matrix */
 186     public float determinant() {
 187         float m00 = a;
 188         float m01 = b;
 189         float m02 = c;
 190         float m03 = d;
 191         float m10 = e;
 192         float m11 = f;
 193         float m12 = g;
 194         float m13 = h;
 195         float m20 = i;
 196         float m21 = j;
 197         float m22 = k;
 198         float m23 = l;
 199         float m30 = m;
 200         float m31 = n;
 201         float m32 = o;
 202         float m33 = p;
 203         return
 204             m03 * m12 * m21 * m30-m02 * m13 * m21 * m30-m03 * m11 * m22 * m30+m01 * m13    * m22 * m30+
 205             m02 * m11 * m23 * m30-m01 * m12 * m23 * m30-m03 * m12 * m20 * m31+m02 * m13    * m20 * m31+
 206             m03 * m10 * m22 * m31-m00 * m13 * m22 * m31-m02 * m10 * m23 * m31+m00 * m12    * m23 * m31+
 207             m03 * m11 * m20 * m32-m01 * m13 * m20 * m32-m03 * m10 * m21 * m32+m00 * m13    * m21 * m32+
 208             m01 * m10 * m23 * m32-m00 * m11 * m23 * m32-m02 * m11 * m20 * m33+m01 * m12    * m20 * m33+
 209             m02 * m10 * m21 * m33-m00 * m12 * m21 * m33-m01 * m10 * m22 * m33+m00 * m11    * m22 * m33;
 210     }
 211
 212     /** compute the inverse of the matrix, returns null if not invertible */
 213     public Matrix inverse() {
 214         float determinant = determinant();
 215         if (determinant==0) return null;
 216         float m00 = a;
 217         float m01 = b;
 218         float m02 = c;
 219         float m03 = d;
 220         float m10 = e;
 221         float m11 = f;
 222         float m12 = g;
 223         float m13 = h;
 224         float m20 = i;
 225         float m21 = j;
 226         float m22 = k;
 227         float m23 = l;
 228         float m30 = m;
 229         float m31 = n;
 230         float m32 = o;
 231         float m33 = p;
 232         return
 233             new Matrix(m12*m23*m31 - m13*m22*m31 + m13*m21*m32 - m11*m23*m32 - m12*m21*m33 + m11*m22*m33,
 234                        m03*m22*m31 - m02*m23*m31 - m03*m21*m32 + m01*m23*m32 + m02*m21*m33 - m01*m22*m33,
 235                        m02*m13*m31 - m03*m12*m31 + m03*m11*m32 - m01*m13*m32 - m02*m11*m33 + m01*m12*m33,
 236                        m03*m12*m21 - m02*m13*m21 - m03*m11*m22 + m01*m13*m22 + m02*m11*m23 - m01*m12*m23,
 237                        m13*m22*m30 - m12*m23*m30 - m13*m20*m32 + m10*m23*m32 + m12*m20*m33 - m10*m22*m33,
 238                        m02*m23*m30 - m03*m22*m30 + m03*m20*m32 - m00*m23*m32 - m02*m20*m33 + m00*m22*m33,
 239                        m03*m12*m30 - m02*m13*m30 - m03*m10*m32 + m00*m13*m32 + m02*m10*m33 - m00*m12*m33,
 240                        m02*m13*m20 - m03*m12*m20 + m03*m10*m22 - m00*m13*m22 - m02*m10*m23 + m00*m12*m23,
 241                        m11*m23*m30 - m13*m21*m30 + m13*m20*m31 - m10*m23*m31 - m11*m20*m33 + m10*m21*m33,
 242                        m03*m21*m30 - m01*m23*m30 - m03*m20*m31 + m00*m23*m31 + m01*m20*m33 - m00*m21*m33,
 243                        m01*m13*m30 - m03*m11*m30 + m03*m10*m31 - m00*m13*m31 - m01*m10*m33 + m00*m11*m33,
 244                        m03*m11*m20 - m01*m13*m20 - m03*m10*m21 + m00*m13*m21 + m01*m10*m23 - m00*m11*m23,
 245                        m12*m21*m30 - m11*m22*m30 - m12*m20*m31 + m10*m22*m31 + m11*m20*m32 - m10*m21*m32,
 246                        m01*m22*m30 - m02*m21*m30 + m02*m20*m31 - m00*m22*m31 - m01*m20*m32 + m00*m21*m32,
 247                        m02*m11*m30 - m01*m12*m30 - m02*m10*m31 + m00*m12*m31 + m01*m10*m32 - m00*m11*m32,
 248                        m01*m12*m20 - m02*m11*m20 + m02*m10*m21 - m00*m12*m21 - m01*m10*m22 + m00*m11*m22)
 249             .times(1/determinant);
 250     }
 251
 252     public String toString() {
 253         return
 254             "\n [ " + a + "\t" + b + "\t" + c + "\t" + d + "\t" + "]" +
 255             "\n [ " + e + "\t" + f + "\t" + g + "\t" + h + "\t" + "]" +
 256             "\n [ " + i + "\t" + j + "\t" + k + "\t" + l + "\t" + "]" +
 257             "\n [ " + m + "\t" + n + "\t" + o + "\t" + p + "\t" + "]\n";
 258     }
 259
 260     public boolean equals(Object oo) {
 261         if (oo==null) return false;
 262         if (!(oo instanceof Matrix)) return false;
 263         Matrix z = (Matrix)oo;
 264         return
 265             near(a,z.a) &&
 266             near(b,z.b) &&
 267             near(c,z.c) &&
 268             near(d,z.d) &&
 269             near(e,z.e) &&
 270             near(f,z.f) &&
 271             near(g,z.g) &&
 272             near(h,z.h) &&
 273             near(i,z.i) &&
 274             near(j,z.j) &&
 275             near(k,z.k) &&
 276             near(l,z.l) &&
 277             near(m,z.m) &&
 278             near(n,z.n) &&
 279             near(o,z.o) &&
 280             near(p,z.p);
 281     }
 282     private static final float EPSILON = 0.001f;
 283     private static boolean near(float a, float b) { return Math.abs(a-b)<EPSILON; }
 284
 285     public int hashCode() {
 286         return
 287             Float.floatToIntBits(a) ^
 288             Float.floatToIntBits(b) ^
 289             Float.floatToIntBits(c) ^
 290             Float.floatToIntBits(d) ^
 291             Float.floatToIntBits(e) ^
 292             Float.floatToIntBits(f) ^
 293             Float.floatToIntBits(g) ^
 294             Float.floatToIntBits(h) ^
 295             Float.floatToIntBits(i) ^
 296             Float.floatToIntBits(j) ^
 297             Float.floatToIntBits(k) ^
 298             Float.floatToIntBits(l) ^
 299             Float.floatToIntBits(m) ^
 300             Float.floatToIntBits(n) ^
 301             Float.floatToIntBits(o) ^
 302             Float.floatToIntBits(p);
 303     }
 304 }