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     /** multiply by another matrix */
 126     public Matrix times(Matrix z) {
 127         float t00 = a;
 128         float t01 = b;
 129         float t02 = c;
 130         float t03 = d;
 131         float t10 = e;
 132         float t11 = f;
 133         float t12 = g;
 134         float t13 = h;
 135         float t20 = i;
 136         float t21 = j;
 137         float t22 = k;
 138         float t23 = l;
 139         float t30 = m;
 140         float t31 = n;
 141         float t32 = o;
 142         float t33 = p;
 143         float m00 = z.a;
 144         float m01 = z.b;
 145         float m02 = z.c;
 146         float m03 = z.d;
 147         float m10 = z.e;
 148         float m11 = z.f;
 149         float m12 = z.g;
 150         float m13 = z.h;
 151         float m20 = z.i;
 152         float m21 = z.j;
 153         float m22 = z.k;
 154         float m23 = z.l;
 155         float m30 = z.m;
 156         float m31 = z.n;
 157         float m32 = z.o;
 158         float m33 = z.p;
 159         return new Matrix(t00*m00 + t01*m10 + t02*m20 + t03*m30,
 160                           t00*m01 + t01*m11 + t02*m21 + t03*m31,
 161                           t00*m02 + t01*m12 + t02*m22 + t03*m32,
 162                           t00*m03 + t01*m13 + t02*m23 + t03*m33,
 163                           t10*m00 + t11*m10 + t12*m20 + t13*m30,
 164                           t10*m01 + t11*m11 + t12*m21 + t13*m31,
 165                           t10*m02 + t11*m12 + t12*m22 + t13*m32,
 166                           t10*m03 + t11*m13 + t12*m23 + t13*m33,
 167                           t20*m00 + t21*m10 + t22*m20 + t23*m30,
 168                           t20*m01 + t21*m11 + t22*m21 + t23*m31,
 169                           t20*m02 + t21*m12 + t22*m22 + t23*m32,
 170                           t20*m03 + t21*m13 + t22*m23 + t23*m33,
 171                           t30*m00 + t31*m10 + t32*m20 + t33*m30,
 172                           t30*m01 + t31*m11 + t32*m21 + t33*m31,
 173                           t30*m02 + t31*m12 + t32*m22 + t33*m32,
 174                           t30*m03 + t31*m13 + t32*m23 + t33*m33);
 175     }
 176
 177     /** compute the determinant of the matrix */
 178     public float determinant() {
 179         float m00 = a;
 180         float m01 = b;
 181         float m02 = c;
 182         float m03 = d;
 183         float m10 = e;
 184         float m11 = f;
 185         float m12 = g;
 186         float m13 = h;
 187         float m20 = i;
 188         float m21 = j;
 189         float m22 = k;
 190         float m23 = l;
 191         float m30 = m;
 192         float m31 = n;
 193         float m32 = o;
 194         float m33 = p;
 195         return
 196             m03 * m12 * m21 * m30-m02 * m13 * m21 * m30-m03 * m11 * m22 * m30+m01 * m13    * m22 * m30+
 197             m02 * m11 * m23 * m30-m01 * m12 * m23 * m30-m03 * m12 * m20 * m31+m02 * m13    * m20 * m31+
 198             m03 * m10 * m22 * m31-m00 * m13 * m22 * m31-m02 * m10 * m23 * m31+m00 * m12    * m23 * m31+
 199             m03 * m11 * m20 * m32-m01 * m13 * m20 * m32-m03 * m10 * m21 * m32+m00 * m13    * m21 * m32+
 200             m01 * m10 * m23 * m32-m00 * m11 * m23 * m32-m02 * m11 * m20 * m33+m01 * m12    * m20 * m33+
 201             m02 * m10 * m21 * m33-m00 * m12 * m21 * m33-m01 * m10 * m22 * m33+m00 * m11    * m22 * m33;
 202     }
 203
 204     /** compute the inverse of the matrix, returns null if not invertible */
 205     public Matrix inverse() {
 206         float determinant = determinant();
 207         if (determinant==0) return null;
 208         float m00 = a;
 209         float m01 = b;
 210         float m02 = c;
 211         float m03 = d;
 212         float m10 = e;
 213         float m11 = f;
 214         float m12 = g;
 215         float m13 = h;
 216         float m20 = i;
 217         float m21 = j;
 218         float m22 = k;
 219         float m23 = l;
 220         float m30 = m;
 221         float m31 = n;
 222         float m32 = o;
 223         float m33 = p;
 224         return
 225             new Matrix(m12*m23*m31 - m13*m22*m31 + m13*m21*m32 - m11*m23*m32 - m12*m21*m33 + m11*m22*m33,
 226                        m03*m22*m31 - m02*m23*m31 - m03*m21*m32 + m01*m23*m32 + m02*m21*m33 - m01*m22*m33,
 227                        m02*m13*m31 - m03*m12*m31 + m03*m11*m32 - m01*m13*m32 - m02*m11*m33 + m01*m12*m33,
 228                        m03*m12*m21 - m02*m13*m21 - m03*m11*m22 + m01*m13*m22 + m02*m11*m23 - m01*m12*m23,
 229                        m13*m22*m30 - m12*m23*m30 - m13*m20*m32 + m10*m23*m32 + m12*m20*m33 - m10*m22*m33,
 230                        m02*m23*m30 - m03*m22*m30 + m03*m20*m32 - m00*m23*m32 - m02*m20*m33 + m00*m22*m33,
 231                        m03*m12*m30 - m02*m13*m30 - m03*m10*m32 + m00*m13*m32 + m02*m10*m33 - m00*m12*m33,
 232                        m02*m13*m20 - m03*m12*m20 + m03*m10*m22 - m00*m13*m22 - m02*m10*m23 + m00*m12*m23,
 233                        m11*m23*m30 - m13*m21*m30 + m13*m20*m31 - m10*m23*m31 - m11*m20*m33 + m10*m21*m33,
 234                        m03*m21*m30 - m01*m23*m30 - m03*m20*m31 + m00*m23*m31 + m01*m20*m33 - m00*m21*m33,
 235                        m01*m13*m30 - m03*m11*m30 + m03*m10*m31 - m00*m13*m31 - m01*m10*m33 + m00*m11*m33,
 236                        m03*m11*m20 - m01*m13*m20 - m03*m10*m21 + m00*m13*m21 + m01*m10*m23 - m00*m11*m23,
 237                        m12*m21*m30 - m11*m22*m30 - m12*m20*m31 + m10*m22*m31 + m11*m20*m32 - m10*m21*m32,
 238                        m01*m22*m30 - m02*m21*m30 + m02*m20*m31 - m00*m22*m31 - m01*m20*m32 + m00*m21*m32,
 239                        m02*m11*m30 - m01*m12*m30 - m02*m10*m31 + m00*m12*m31 + m01*m10*m32 - m00*m11*m32,
 240                        m01*m12*m20 - m02*m11*m20 + m02*m10*m21 - m00*m12*m21 - m01*m10*m22 + m00*m11*m22)
 241             .times(1/determinant);
 242     }
 243
 244     public String toString() {
 245         return
 246             "\n [ " + a + "\t" + b + "\t" + c + "\t" + d + "\t" + "]" +
 247             "\n [ " + e + "\t" + f + "\t" + g + "\t" + h + "\t" + "]" +
 248             "\n [ " + i + "\t" + j + "\t" + k + "\t" + l + "\t" + "]" +
 249             "\n [ " + m + "\t" + n + "\t" + o + "\t" + p + "\t" + "]\n";
 250     }
 251
 252     public boolean equals(Object oo) {
 253         if (oo==null) return false;
 254         if (!(oo instanceof Matrix)) return false;
 255         Matrix z = (Matrix)oo;
 256         return
 257             near(a,z.a) &&
 258             near(b,z.b) &&
 259             near(c,z.c) &&
 260             near(d,z.d) &&
 261             near(e,z.e) &&
 262             near(f,z.f) &&
 263             near(g,z.g) &&
 264             near(h,z.h) &&
 265             near(i,z.i) &&
 266             near(j,z.j) &&
 267             near(k,z.k) &&
 268             near(l,z.l) &&
 269             near(m,z.m) &&
 270             near(n,z.n) &&
 271             near(o,z.o) &&
 272             near(p,z.p);
 273     }
 274     private static final float EPSILON = 0.001f;
 275     private static boolean near(float a, float b) { return Math.abs(a-b)<EPSILON; }
 276
 277     public int hashCode() {
 278         return
 279             Float.floatToIntBits(a) ^
 280             Float.floatToIntBits(b) ^
 281             Float.floatToIntBits(c) ^
 282             Float.floatToIntBits(d) ^
 283             Float.floatToIntBits(e) ^
 284             Float.floatToIntBits(f) ^
 285             Float.floatToIntBits(g) ^
 286             Float.floatToIntBits(h) ^
 287             Float.floatToIntBits(i) ^
 288             Float.floatToIntBits(j) ^
 289             Float.floatToIntBits(k) ^
 290             Float.floatToIntBits(l) ^
 291             Float.floatToIntBits(m) ^
 292             Float.floatToIntBits(n) ^
 293             Float.floatToIntBits(o) ^
 294             Float.floatToIntBits(p);
 295     }
 296 }