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

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

Index: content/public/android/java/src/org/chromium/content/browser/DeviceMotionAndOrientation.java
diff --git a/content/public/android/java/src/org/chromium/content/browser/DeviceMotionAndOrientation.java b/content/public/android/java/src/org/chromium/content/browser/DeviceMotionAndOrientation.java
index eb9cf0ec596f90bf1b888bc7ea6ba3be984ec85b..96ffe494a8e7bca57c126820b17d6777977a05cb 100644
--- a/content/public/android/java/src/org/chromium/content/browser/DeviceMotionAndOrientation.java
+++ b/content/public/android/java/src/org/chromium/content/browser/DeviceMotionAndOrientation.java
@@ -209,8 +209,7 @@ class DeviceMotionAndOrientation implements SensorEventListener {
                 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();
                 }
@@ -221,45 +220,104 @@ class DeviceMotionAndOrientation implements SensorEventListener {
         }
     }
 
+    /**
+     * 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
+     * <b>R</b> = <b>Rz</b>(alpha)*<b>Rx</b>(beta)*<b>Ry</b>(gamma),

[Michael van Ouwerkerk, 2013/10/29 16:18:06]

Perhaps all of this documentation would be easier to read without the bold and
italic tags. It seems more common to read this as plain text than as rendered
html.

[timvolodine, 2013/10/30 14:14:47]

Done.

+     * where <b>Rz</b>, <b>Rx</b> and <b>Ry</b> 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
+     * <i>radians</i> and clockwise-positive when viewed along the positive direction of the
+     * corresponding axis. Except for the special case when the beta angle is +-<i>pi</i>/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 wihtin [-<i>pi</i>, <i>pi</i>). A mobile device with pitch angle greater than
+     * <i>pi</i>/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*<i>pi</i>)</li>
+     * <li>values[1]: rotation around the X axis, beta in [-<i>pi</i>,<i>pi</i>)</li>
+     * <li>values[2]: rotation around the Y axis, gamma in [-<i>pi<i>/2,<i>pi<i>/2)</li>
+     * </ul>
+     * <p>
+     *
+     * @param R
+     *        rotation matrix {@see SensorManager.getRotationMatrix}.
+     *
+     * @param values
+     *        an array of 3 doubles to hold the result.
+     *
+     * @return the array values passed as argument.
+     */
+    public static void getOrientationMobileDevice(float[] R, double[] values) {

[Michael van Ouwerkerk, 2013/10/29 16:18:06]

Why not create the array in this method and then return it? It seems more
idiomatic for Java, and the @return documentation would make more sense.

[Michael van Ouwerkerk, 2013/10/29 16:18:06]

@VisibleForTesting

[Michael van Ouwerkerk, 2013/10/29 16:18:06]

How about naming this according to what it does e.g.
convertRotationMatrixToAngles or getAnglesFromRotationMatrix.

[timvolodine, 2013/10/30 14:14:47]

ok, I've added a return double[]. However we still need the values output
parameter for efficiency purposes, so that any code using this function can
potentially avoid using "new" for each invocation,
SensorManager.getOrientation() does this as well.

[timvolodine, 2013/10/30 14:14:47]

Done.

[timvolodine, 2013/10/30 14:14:47]

renamed to computeDeviceOrientationFromRotationMatrix()

+        /*
+         * 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;

[Michael van Ouwerkerk, 2013/10/29 16:18:06]

Worth calling out in the Javadoc the possibility for a 'zero angles' result if
the input is incomplete? Also worth logging this event? Anyway, when does this
happen? Should it throw an exception instead?

[timvolodine, 2013/10/30 14:14:47]

this should not happen, or it would be incorrect usage of the function. I've
added "a 3x3 rotation matrix" as a precondition.

+
+        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)

[bulach, 2013/10/24 17:08:44]

both in comments and in some real places like 294, spaces around "/" and "*"
(and the docstring above as well)

[timvolodine, 2013/10/30 14:14:47]

Done.
(however left some comments as is for readability)

+            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;
+            } 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;
+            } 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)
+    }
+
     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
-
-        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)
-        }
+        double[] rotationAngles = new double[3];
+        getOrientationMobileDevice(deviceRotationMatrix, rotationAngles);
 
-        gotOrientation(alpha, beta, gamma);
+        gotOrientation(Math.toDegrees(rotationAngles[0]),

[Michael van Ouwerkerk, 2013/10/29 16:18:06]

If degrees are needed, why not return those from the method that converts the
matrix?

[timvolodine, 2013/10/30 14:14:47]

yeah, but to make it more generic it would be better to use radians.
SensorManager uses radians for getOrientation() and other methods as well.

+                       Math.toDegrees(rotationAngles[1]),
+                       Math.toDegrees(rotationAngles[2]));
     }
 
     private SensorManagerProxy getSensorManagerProxy() {