Merge changes from Yue

[~andy/iBeaconNav] / src / edu / ucla / iBeaconNav / Matrix.java

diff --git a/src/edu/ucla/iBeaconNav/Matrix.java b/src/edu/ucla/iBeaconNav/Matrix.java
new file mode 100644 (file)
index 0000000..bd0378e
--- /dev/null
+++ b/src/edu/ucla/iBeaconNav/Matrix.java
@@ -0,0 +1,78 @@
+package edu.ucla.iBeaconNav;
+
+class Matrix {
+       public float[] mValue = new float[9];
+       
+       public Matrix(){
+               for (int i=0; i<9; i++){
+                       mValue[i] = 0;
+               }
+       }
+       
+       public Matrix(float[] m){
+               for (int i=0; i<9; i++){
+                       mValue[i] = m[i];
+               }
+       }
+       
+       public Matrix rotateX(float thetaX){
+               float[] rtX = {1,                       0,                       0,
+                                          0, (float)Math.cos(thetaX),(float)-Math.sin(thetaX),
+                                          0, (float)Math.sin(thetaX),(float) Math.cos(thetaX)}; 
+               return new Matrix(rtX);
+       }
+       
+       public Matrix rotateY(float thetaY){
+               float[] rtY = {(float) Math.cos(thetaY), 0, (float)Math.sin(thetaY),
+                                                             0, 1,                       0,
+                                      (float)-Math.sin(thetaY), 0, (float)Math.cos(thetaY)};
+               return new Matrix(rtY);
+       }
+       
+       public Matrix rotateZ(float thetaZ){
+               float[] rtZ = {(float)Math.cos(thetaZ),(float)-Math.sin(thetaZ), 0,
+                                      (float)Math.sin(thetaZ),(float) Math.cos(thetaZ), 0,
+                                                            0,                       0, 1};
+               return new Matrix(rtZ);
+       }
+       
+       public Matrix multiple(Matrix m){
+               float[] result = new float[9];
+               for (int k1=0; k1<9; k1++){
+                       int i = (int) (k1/3);
+                       int j = k1%3;
+                       result[k1] = 0;
+                       for (int k2=0; k2<3; k2++){
+                               result[k1]+=mValue[i*3+k2]*m.mValue[k2*3+j];
+                       }
+                       
+               }
+               return new Matrix(result);
+       }
+       
+       public float[] multipleV(float[] v){
+               float[] result = new float[3];
+               for(int i=0; i<3; i++){
+                       result[i] = 0;
+                       for(int j=0; j<3; j++){
+                               result[i]+=mValue[3*i+j]*v[j];
+                       }
+               }
+               return result;
+       }
+       
+       public Matrix transpose(){
+               float[] result = new float[9];
+               result[0] = mValue[0];
+               result[1] = mValue[3];
+               result[2] = mValue[6];
+               result[4] = mValue[4];
+               result[3] = mValue[1];
+               result[5] = mValue[7];
+               result[6] = mValue[2];
+               result[7] = mValue[5];
+               result[8] = mValue[8];
+               return new Matrix(result);
+       }
+       
+}