Pileus Git - ~andy/iBeaconNav/blob - src/edu/ucla/iBeaconNav/Matrix.java

   1 package edu.ucla.iBeaconNav;
   2
   3 class Matrix {
   4         public float[] mValue = new float[9];
   5
   6         public Matrix(){
   7                 for (int i=0; i<9; i++){
   8                         mValue[i] = 0;
   9                 }
  10         }
  11
  12         public Matrix(float[] m){
  13                 for (int i=0; i<9; i++){
  14                         mValue[i] = m[i];
  15                 }
  16         }
  17
  18         public Matrix rotateX(float thetaX){
  19                 float[] rtX = {1,                       0,                       0,
  20                                            0, (float)Math.cos(thetaX),(float)-Math.sin(thetaX),
  21                                            0, (float)Math.sin(thetaX),(float) Math.cos(thetaX)};
  22                 return new Matrix(rtX);
  23         }
  24
  25         public Matrix rotateY(float thetaY){
  26                 float[] rtY = {(float) Math.cos(thetaY), 0, (float)Math.sin(thetaY),
  27                                                               0, 1,                       0,
  28                                        (float)-Math.sin(thetaY), 0, (float)Math.cos(thetaY)};
  29                 return new Matrix(rtY);
  30         }
  31
  32         public Matrix rotateZ(float thetaZ){
  33                 float[] rtZ = {(float)Math.cos(thetaZ),(float)-Math.sin(thetaZ), 0,
  34                                        (float)Math.sin(thetaZ),(float) Math.cos(thetaZ), 0,
  35                                                              0,                       0, 1};
  36                 return new Matrix(rtZ);
  37         }
  38
  39         public Matrix multiple(Matrix m){
  40                 float[] result = new float[9];
  41                 for (int k1=0; k1<9; k1++){
  42                         int i = (int) (k1/3);
  43                         int j = k1%3;
  44                         result[k1] = 0;
  45                         for (int k2=0; k2<3; k2++){
  46                                 result[k1]+=mValue[i*3+k2]*m.mValue[k2*3+j];
  47                         }
  48
  49                 }
  50                 return new Matrix(result);
  51         }
  52
  53         public float[] multipleV(float[] v){
  54                 float[] result = new float[3];
  55                 for(int i=0; i<3; i++){
  56                         result[i] = 0;
  57                         for(int j=0; j<3; j++){
  58                                 result[i]+=mValue[3*i+j]*v[j];
  59                         }
  60                 }
  61                 return result;
  62         }
  63
  64         public Matrix transpose(){
  65                 float[] result = new float[9];
  66                 result[0] = mValue[0];
  67                 result[1] = mValue[3];
  68                 result[2] = mValue[6];
  69                 result[4] = mValue[4];
  70                 result[3] = mValue[1];
  71                 result[5] = mValue[7];
  72                 result[6] = mValue[2];
  73                 result[7] = mValue[5];
  74                 result[8] = mValue[8];
  75                 return new Matrix(result);
  76         }
  77
  78 }