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

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