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
+     * <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
+        double[] rotationAngles = new double[3];
+        getOrientationMobileDevice(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]),

[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() {
         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));
             }
         }
     }
 
 }