|
| 1 | +package org.thoughtcrime.securesms.components.sensors; |
| 2 | + |
| 3 | +public final class SensorUtil { |
| 4 | + |
| 5 | + private SensorUtil() { } |
| 6 | + |
| 7 | + public static void getRotationMatrixWithoutMagneticSensorData(float[] rotationMatrix, float[] accelerometerReading) { |
| 8 | + double gx, gy, gz; |
| 9 | + gx = accelerometerReading[0] / 9.81f; |
| 10 | + gy = accelerometerReading[1] / 9.81f; |
| 11 | + gz = accelerometerReading[2] / 9.81f; |
| 12 | + |
| 13 | + float pitch = (float) -Math.atan(gy / Math.sqrt(gx * gx + gz * gz)); |
| 14 | + float roll = (float) -Math.atan(gx / Math.sqrt(gy * gy + gz * gz)); |
| 15 | + float azimuth = 0; |
| 16 | + |
| 17 | + float[] fakeMagnetometerReading = { azimuth, pitch, roll }; |
| 18 | + |
| 19 | + System.arraycopy(getRotationMatrixForOrientation(fakeMagnetometerReading), 0, rotationMatrix, 0, rotationMatrix.length); |
| 20 | + } |
| 21 | + |
| 22 | + private static float[] getRotationMatrixForOrientation(float[] o) { |
| 23 | + float[] xM = new float[9]; |
| 24 | + float[] yM = new float[9]; |
| 25 | + float[] zM = new float[9]; |
| 26 | + |
| 27 | + float sinX = (float) Math.sin(o[1]); |
| 28 | + float cosX = (float) Math.cos(o[1]); |
| 29 | + float sinY = (float) Math.sin(o[2]); |
| 30 | + float cosY = (float) Math.cos(o[2]); |
| 31 | + float sinZ = (float) Math.sin(o[0]); |
| 32 | + float cosZ = (float) Math.cos(o[0]); |
| 33 | + |
| 34 | + xM[0] = 1.0f; |
| 35 | + xM[1] = 0.0f; |
| 36 | + xM[2] = 0.0f; |
| 37 | + |
| 38 | + xM[3] = 0.0f; |
| 39 | + xM[4] = cosX; |
| 40 | + xM[5] = sinX; |
| 41 | + |
| 42 | + xM[6] = 0.0f; |
| 43 | + xM[7] = -sinX; |
| 44 | + xM[8] = cosX; |
| 45 | + |
| 46 | + yM[0] = cosY; |
| 47 | + yM[1] = 0.0f; |
| 48 | + yM[2] = sinY; |
| 49 | + |
| 50 | + yM[3] = 0.0f; |
| 51 | + yM[4] = 1.0f; |
| 52 | + yM[5] = 0.0f; |
| 53 | + |
| 54 | + yM[6] = -sinY; |
| 55 | + yM[7] = 0.0f; |
| 56 | + yM[8] = cosY; |
| 57 | + |
| 58 | + zM[0] = cosZ; |
| 59 | + zM[1] = sinZ; |
| 60 | + zM[2] = 0.0f; |
| 61 | + |
| 62 | + zM[3] = -sinZ; |
| 63 | + zM[4] = cosZ; |
| 64 | + zM[5] = 0.0f; |
| 65 | + |
| 66 | + zM[6] = 0.0f; |
| 67 | + zM[7] = 0.0f; |
| 68 | + zM[8] = 1.0f; |
| 69 | + |
| 70 | + float[] resultMatrix = matrixMultiplication(xM, yM); |
| 71 | + resultMatrix = matrixMultiplication(zM, resultMatrix); |
| 72 | + return resultMatrix; |
| 73 | + } |
| 74 | + |
| 75 | + private static float[] matrixMultiplication(float[] A, float[] B) { |
| 76 | + float[] result = new float[9]; |
| 77 | + |
| 78 | + result[0] = A[0] * B[0] + A[1] * B[3] + A[2] * B[6]; |
| 79 | + result[1] = A[0] * B[1] + A[1] * B[4] + A[2] * B[7]; |
| 80 | + result[2] = A[0] * B[2] + A[1] * B[5] + A[2] * B[8]; |
| 81 | + |
| 82 | + result[3] = A[3] * B[0] + A[4] * B[3] + A[5] * B[6]; |
| 83 | + result[4] = A[3] * B[1] + A[4] * B[4] + A[5] * B[7]; |
| 84 | + result[5] = A[3] * B[2] + A[4] * B[5] + A[5] * B[8]; |
| 85 | + |
| 86 | + result[6] = A[6] * B[0] + A[7] * B[3] + A[8] * B[6]; |
| 87 | + result[7] = A[6] * B[1] + A[7] * B[4] + A[8] * B[7]; |
| 88 | + result[8] = A[6] * B[2] + A[7] * B[5] + A[8] * B[8]; |
| 89 | + |
| 90 | + return result; |
| 91 | + } |
| 92 | +} |
0 commit comments