X-Git-Url: http://pileus.org/git/?p=~andy%2FiBeaconNav;a=blobdiff_plain;f=src%2Fedu%2Fucla%2FiBeaconNav%2FMatrix.java;fp=src%2Fedu%2Fucla%2FiBeaconNav%2FMatrix.java;h=bd0378e90e309668504d0f129db3be270c55cdb5;hp=0000000000000000000000000000000000000000;hb=1ac485ec0eff95b11ba80a3c98b8ddfd9c5be56f;hpb=1423589b8378f8176a0ad31571ab0c62141f3ec6

diff --git a/src/edu/ucla/iBeaconNav/Matrix.java b/src/edu/ucla/iBeaconNav/Matrix.java
new file mode 100644
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);
+	}
+	
+}