1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55
|
public void onSensorChanged(SensorEvent event) {
//Compute azimuth pitch roll
if (event.sensor.getType() == Sensor.TYPE_ACCELEROMETER) {
acceleromterVector = event.values.clone();
} else if (event.sensor.getType() == Sensor.TYPE_MAGNETIC_FIELD) {
magneticVector = event.values.clone();
}else if (event.sensor.getType() == Sensor.TYPE_ORIENTATION) {
yaw = event.values[0] * Deg2Rad;
}
SensorManager.getRotationMatrix(resultMatrix, null, acceleromterVector, magneticVector);
SensorManager.getOrientation(resultMatrix, values);
pitch = Math.floor(values[1] * 100.0) / 100.0;
roll = values[2];
center.rotate((float)yaw, (float)pitch, 0);
}
public void rotate(float yaw, float pitch, float roll){
float A = (float) Math.cos(pitch); // angle x
float B = (float) Math.sin(pitch);
float C = (float) Math.cos(yaw); // angle y
float D = (float) Math.sin(yaw);
float E = (float) Math.cos(roll); // angle z
float F = (float) Math.sin(roll);
float AD = A * D;
float BD = B * D;
float mat[] = new float[16];
mat[0] = C * E;
mat[1] = -C * F;
mat[2] = -D;
mat[4] = -BD * E + A * F;
mat[5] = BD * F + A * E;
mat[6] = -B * C;
mat[8] = AD * E + B * F;
mat[9] = -AD * F + B * E;
mat[10] = A * C;
mat[3] = mat[7] = mat[11] = mat[12] = mat[13] = mat[14] = 0;
mat[15] = 1;
multMatrix(mat);
}
public void multMatrix(float m[]){
float tmpX = m[0] * x + m[1] * y + m[2] * z;
float tmpY = m[4] * x + m[5] * y + m[6] * z;
z = m[8] * x + m[9] * y + m[10] * z;
x = tmpX;
y = tmpY;
} |
Partager