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

   1 package edu.berkeley.qfat.geom;
   2
   3 /** an affine matrix; immutable */
   4 public class Matrix {
   5
   6     /**
   7      *  <pre>
   8      *  [ a b c d ]   [ x ]
   9      *  [ e f g h ]   [ y ]
  10      *  [ i j k l ]   [ z ]
  11      *  [ m n o p ]   [ 1 ]
  12      *  </pre>
  13      */
  14     public final float a, b, c, d, e, f, g, h, i, j, k, l, m, n, o, p;
  15
  16     public static final Matrix ZERO = new Matrix(0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0);
  17     public static final Matrix ONE  = new Matrix(1,0,0,0,0,1,0,0,0,0,1,0,0,0,0,1);
  18
  19     public static Matrix scale(float scale) { return new Matrix(scale, 0, 0, 0, 0, scale, 0, 0, 0, 0, scale, 0, 0, 0, 0, 1); }
  20
  21     public Matrix(float scalex, float scaley, float scalez) {
  22         a = scalex;
  23         f = scaley;
  24         k = scalez;
  25         l = h = d = e = b = i = c = j = g = 0;
  26         m = n = o = 0;
  27         p = 1;
  28     }
  29     public Matrix(Vec translate) {
  30         d = translate.x; h = translate.y; l = translate.z;
  31         a = f = k = 1;
  32         b = c = e = g = i = j = 0;
  33         m = n = o = 0;
  34         p = 1;
  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     public Matrix plus(Matrix x) {
  42         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);
  43     }
  44     public Matrix minus(Matrix x) {
  45         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);
  46     }
  47     public Matrix times(float x) {
  48         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);
  49     }
  50
  51     /** computes (v^T)(this)(v) */
  52     public float preAndPostMultiply(Point point) {
  53         float ret =
  54             ((a*point.x + b*point.y + c*point.z + d) * point.x) +
  55             ((e*point.x + f*point.y + g*point.z + h) * point.y) +
  56             ((i*point.x + j*point.y + k*point.z + l) * point.z) +
  57             ((m*point.x + n*point.y + o*point.z + p) * 1);
  58         return ret;
  59     }
  60
  61     public Matrix(Vec axis, float angle) {
  62         double q = Math.cos(angle);
  63         double s = Math.sin(angle);
  64         double t = 1.0 - q;
  65         a = (float)(q + axis.x*axis.x*t);
  66         f = (float)(q + axis.y*axis.y*t);
  67         k = (float)(q + axis.z*axis.z*t);
  68         double tmp1 = axis.x*axis.y*t;
  69         double tmp2 = axis.z*s;
  70         e = (float)(tmp1 + tmp2);
  71         b = (float)(tmp1 - tmp2);
  72         tmp1 = axis.x*axis.z*t;
  73         tmp2 = axis.y*s;
  74         i = (float)(tmp1 - tmp2);
  75         c = (float)(tmp1 + tmp2);
  76         tmp1 = axis.y*axis.z*t;
  77         tmp2 = axis.x*s;
  78         j = (float)(tmp1 + tmp2);
  79         g = (float)(tmp1 - tmp2);
  80         d = h = l = 0;
  81         m = n = o = 0;
  82         p = 1;
  83     }
  84     public Point times(Point p) {
  85         // discards bottom row
  86         return new Point(a*p.x + b*p.y + c*p.z + d,
  87                          e*p.x + f*p.y + g*p.z + h,
  88                          i*p.x + j*p.y + k*p.z + l);
  89     }
  90     public Vec times(Vec p) {
  91         // discards bottom row
  92         return new Vec(a*p.x + b*p.y + c*p.z + d,
  93                        e*p.x + f*p.y + g*p.z + h,
  94                        i*p.x + j*p.y + k*p.z + l);
  95     }
  96     public Point apply(Point p) { return p; }
  97     public Vec apply(Vec v) { return v; }
  98     public Matrix invert() { return this; }
  99     public Matrix times(Matrix m) { return this; }
 100
 101
 102     public float determinant() {
 103         float m00 = a;
 104         float m01 = b;
 105         float m02 = c;
 106         float m03 = d;
 107         float m10 = e;
 108         float m11 = f;
 109         float m12 = g;
 110         float m13 = h;
 111         float m20 = i;
 112         float m21 = j;
 113         float m22 = k;
 114         float m23 = l;
 115         float m30 = m;
 116         float m31 = n;
 117         float m32 = o;
 118         float m33 = p;
 119         return
 120             m03 * m12 * m21 * m30-m02 * m13 * m21 * m30-m03 * m11 * m22 * m30+m01 * m13    * m22 * m30+
 121             m02 * m11 * m23 * m30-m01 * m12 * m23 * m30-m03 * m12 * m20 * m31+m02 * m13    * m20 * m31+
 122             m03 * m10 * m22 * m31-m00 * m13 * m22 * m31-m02 * m10 * m23 * m31+m00 * m12    * m23 * m31+
 123             m03 * m11 * m20 * m32-m01 * m13 * m20 * m32-m03 * m10 * m21 * m32+m00 * m13    * m21 * m32+
 124             m01 * m10 * m23 * m32-m00 * m11 * m23 * m32-m02 * m11 * m20 * m33+m01 * m12    * m20 * m33+
 125             m02 * m10 * m21 * m33-m00 * m12 * m21 * m33-m01 * m10 * m22 * m33+m00 * m11    * m22 * m33;
 126     }
 127
 128     public Matrix inverse() {
 129         float m00 = a;
 130         float m01 = b;
 131         float m02 = c;
 132         float m03 = d;
 133         float m10 = e;
 134         float m11 = f;
 135         float m12 = g;
 136         float m13 = h;
 137         float m20 = i;
 138         float m21 = j;
 139         float m22 = k;
 140         float m23 = l;
 141         float m30 = m;
 142         float m31 = n;
 143         float m32 = o;
 144         float m33 = p;
 145         return
 146             new Matrix(m12*m23*m31 - m13*m22*m31 + m13*m21*m32 - m11*m23*m32 - m12*m21*m33 + m11*m22*m33,
 147                        m03*m22*m31 - m02*m23*m31 - m03*m21*m32 + m01*m23*m32 + m02*m21*m33 - m01*m22*m33,
 148                        m02*m13*m31 - m03*m12*m31 + m03*m11*m32 - m01*m13*m32 - m02*m11*m33 + m01*m12*m33,
 149                        m03*m12*m21 - m02*m13*m21 - m03*m11*m22 + m01*m13*m22 + m02*m11*m23 - m01*m12*m23,
 150                        m13*m22*m30 - m12*m23*m30 - m13*m20*m32 + m10*m23*m32 + m12*m20*m33 - m10*m22*m33,
 151                        m02*m23*m30 - m03*m22*m30 + m03*m20*m32 - m00*m23*m32 - m02*m20*m33 + m00*m22*m33,
 152                        m03*m12*m30 - m02*m13*m30 - m03*m10*m32 + m00*m13*m32 + m02*m10*m33 - m00*m12*m33,
 153                        m02*m13*m20 - m03*m12*m20 + m03*m10*m22 - m00*m13*m22 - m02*m10*m23 + m00*m12*m23,
 154                        m11*m23*m30 - m13*m21*m30 + m13*m20*m31 - m10*m23*m31 - m11*m20*m33 + m10*m21*m33,
 155                        m03*m21*m30 - m01*m23*m30 - m03*m20*m31 + m00*m23*m31 + m01*m20*m33 - m00*m21*m33,
 156                        m01*m13*m30 - m03*m11*m30 + m03*m10*m31 - m00*m13*m31 - m01*m10*m33 + m00*m11*m33,
 157                        m03*m11*m20 - m01*m13*m20 - m03*m10*m21 + m00*m13*m21 + m01*m10*m23 - m00*m11*m23,
 158                        m12*m21*m30 - m11*m22*m30 - m12*m20*m31 + m10*m22*m31 + m11*m20*m32 - m10*m21*m32,
 159                        m01*m22*m30 - m02*m21*m30 + m02*m20*m31 - m00*m22*m31 - m01*m20*m32 + m00*m21*m32,
 160                        m02*m11*m30 - m01*m12*m30 - m02*m10*m31 + m00*m12*m31 + m01*m10*m32 - m00*m11*m32,
 161                        m01*m12*m20 - m02*m11*m20 + m02*m10*m21 - m00*m12*m21 - m01*m10*m22 + m00*m11*m22)
 162             .times(1/determinant());
 163     }
 164 }