Android: fix the computation of device orientation angles in accordance with spec.

content/public/android/java/src/org/chromium/content/browser/DeviceMotionAndOrientation.java

 // Copyright (c) 2013 The Chromium Authors. All rights reserved.
 // Use of this source code is governed by a BSD-style license that can be
 // found in the LICENSE file.
 
 package org.chromium.content.browser;
 
 import android.content.Context;
 import android.hardware.Sensor;
 import android.hardware.SensorEvent;
 import android.hardware.SensorEventListener;
@@ skipping 191 matching lines @@
                 break;
             case Sensor.TYPE_GYROSCOPE:
                 if (mDeviceMotionIsActive) {
                     gotRotationRate(values[0], values[1], values[2]);
                 }
                 break;
             case Sensor.TYPE_MAGNETIC_FIELD:
                 if (mMagneticFieldVector == null) {
                     mMagneticFieldVector = new float[3];
                 }
-                System.arraycopy(values, 0, mMagneticFieldVector, 0,
-                        mMagneticFieldVector.length);
+                System.arraycopy(values, 0, mMagneticFieldVector, 0, mMagneticFieldVector.length);
                 if (mDeviceOrientationIsActive) {
                     getOrientationUsingGetRotationMatrix();
                 }
                 break;
             default:
                 // Unexpected
                 return;
         }
     }
 
+    /**
+     * Returns orientation angles from a rotation matrix, such that the angles are according
+     * to spec {@link http://dev.w3.org/geo/api/spec-source-orientation.html}.
+     * <p>
+     * It is assumed the rotation matrix transforms a 3D column vector from device coordinate system
+     * to the world's coordinate system, as e.g. computed by {@see SensorManager.getRotationMatrix}.
+     * <p>
+     * In particular we compute the decomposition of a given rotation matrix R such that <br>
+     * R = Rz(alpha) * Rx(beta) * Ry(gamma), <br>
+     * where Rz, Rx and Ry are rotation matrices around Z, X and Y axes in the world coordinate
+     * reference frame respectively. The reference frame consists of three orthogonal axes X, Y, Z
+     * where X points East, Y points north and Z points upwards perpendicular to the ground plane.
+     * The computed angles alpha, beta and gamma are in radians and clockwise-positive when viewed
+     * along the positive direction of the corresponding axis. Except for the special case when the
+     * beta angle is +-pi/2 these angles uniquely define the orientation of a mobile device in 3D
+     * space. The alpha-beta-gamma representation resembles the yaw-pitch-roll convention used in
+     * vehicle dynamics, however it does not exactly match it. One of the differences is that the
+     * 'pitch' angle beta is allowed to be within [-pi, pi). A mobile device with pitch angle
+     * greater than pi/2 could correspond to a user lying down and looking upward at the screen.
+     *
+     * <p>
+     * Upon return the array values is filled with the result,
+     * <ul>
+     * <li>values[0]: rotation around the Z axis, alpha in [0, 2*pi)</li>
+     * <li>values[1]: rotation around the X axis, beta in [-pi, pi)</li>
+     * <li>values[2]: rotation around the Y axis, gamma in [-pi/2, pi/2)</li>
+     * </ul>
+     * <p>
+     *
+     * @param R
+     *        a 3x3 rotation matrix {@see SensorManager.getRotationMatrix}.
+     *
+     * @param values
+     *        an array of 3 doubles to hold the result.
+     *
+     * @return the array values passed as argument.
+     */
+    @VisibleForTesting
+    public static double[] computeDeviceOrientationFromRotationMatrix(float[] R, double[] values) {
+        /*
+         * 3x3 (length=9) case:
+         *   /  R[ 0]   R[ 1]   R[ 2]  \
+         *   |  R[ 3]   R[ 4]   R[ 5]  |
+         *   \  R[ 6]   R[ 7]   R[ 8]  /
+         *
+         */
+        if (R.length != 9)
+            return values;
+
+        if (R[8] > 0) {  // cos(beta) > 0
+            values[0] = Math.atan2(-R[1], R[4]);
+            values[1] = Math.asin(R[7]);           // beta (-pi/2, pi/2)
+            values[2] = Math.atan2(-R[6], R[8]);   // gamma (-pi/2, pi/2)
+        } else if (R[8] < 0) {  // cos(beta) < 0
+            values[0] = Math.atan2(R[1], -R[4]);
+            values[1] = -Math.asin(R[7]);
+            values[1] += (values[1] >= 0) ? -Math.PI : Math.PI; // beta [-pi,-pi/2) U (pi/2,pi)
+            values[2] = Math.atan2(R[6], -R[8]);   // gamma (-pi/2, pi/2)
+        } else { // R[8] == 0
+            if (R[6] > 0) {  // cos(gamma) == 0, cos(beta) > 0
+                values[0] = Math.atan2(-R[1], R[4]);
+                values[1] = Math.asin(R[7]);       // beta [-pi/2, pi/2]
+                values[2] = -Math.PI / 2;          // gamma = -pi/2
+            } else if (R[6] < 0) { // cos(gamma) == 0, cos(beta) < 0
+                values[0] = Math.atan2(R[1], -R[4]);
+                values[1] = -Math.asin(R[7]);
+                values[1] += (values[1] >= 0) ? -Math.PI : Math.PI; // beta [-pi,-pi/2) U (pi/2,pi)
+                values[2] = -Math.PI / 2;          // gamma = -pi/2
+            } else { // R[6] == 0, cos(beta) == 0
+                // gimbal lock discontinuity
+                values[0] = Math.atan2(R[3], R[0]);
+                values[1] = (R[7] > 0) ? Math.PI / 2 : -Math.PI / 2;  // beta = +-pi/2
+                values[2] = 0;                                        // gamma = 0
+            }
+        }
+
+        // alpha is in [-pi, pi], make sure it is in [0, 2*pi).
+        if (values[0] < 0)
+            values[0] += 2 * Math.PI; // alpha [0, 2*pi)
+
+        return values;
+    }
+
     private void getOrientationUsingGetRotationMatrix() {
         if (mAccelerationIncludingGravityVector == null || mMagneticFieldVector == null) {
             return;
         }
 
-        // Get the rotation matrix.
-        // The rotation matrix that transforms from the body frame to the earth
-        // frame.
         float[] deviceRotationMatrix = new float[9];
         if (!SensorManager.getRotationMatrix(deviceRotationMatrix, null,
                 mAccelerationIncludingGravityVector, mMagneticFieldVector)) {
             return;
         }
 
-        // Convert rotation matrix to rotation angles.
-        // Assuming that the rotations are appied in the order listed at
-        // http://developer.android.com/reference/android/hardware/SensorEvent.html#values
-        // the rotations are applied about the same axes and in the same order as required by the
-        // API. The only conversions are sign changes as follows.  The angles are in radians
+        double[] rotationAngles = new double[3];
+        computeDeviceOrientationFromRotationMatrix(deviceRotationMatrix, rotationAngles);
 
-        float[] rotationAngles = new float[3];
-        SensorManager.getOrientation(deviceRotationMatrix, rotationAngles);
-
-        double alpha = Math.toDegrees(-rotationAngles[0]);
-        while (alpha < 0.0) {
-            alpha += 360.0; // [0, 360)
-        }
-
-        double beta = Math.toDegrees(-rotationAngles[1]);
-        while (beta < -180.0) {
-            beta += 360.0; // [-180, 180)
-        }
-
-        double gamma = Math.toDegrees(rotationAngles[2]);
-        while (gamma < -90.0) {
-            gamma += 360.0; // [-90, 90)
-        }
-
-        gotOrientation(alpha, beta, gamma);
+        gotOrientation(Math.toDegrees(rotationAngles[0]),
+                       Math.toDegrees(rotationAngles[1]),
+                       Math.toDegrees(rotationAngles[2]));
     }
 
     private SensorManagerProxy getSensorManagerProxy() {
         if (mSensorManagerProxy != null) {
             return mSensorManagerProxy;
         }
         SensorManager sensorManager = (SensorManager)WeakContext.getSystemService(
                 Context.SENSOR_SERVICE);
         if (sensorManager != null) {
             mSensorManagerProxy = new SensorManagerProxyImpl(sensorManager);
@@ skipping 181 matching lines @@
         @Override
         public void unregisterListener(SensorEventListener listener, int sensorType) {
             List<Sensor> sensors = mSensorManager.getSensorList(sensorType);
             if (!sensors.isEmpty()) {
                 mSensorManager.unregisterListener(listener, sensors.get(0));
             }
         }
     }
 
 }