add new sensor types for handling gyro data and device orientation

more efficiently.

Change-Id: Ie19992f6599e528a79931f4ae592898dac15412e

4 files changed, 312 insertions, 3 deletions

diff --git a/api/current.xml b/api/current.xml
index baff527..eb424b7 100644
--- a/api/current.xml
+++ b/api/current.xml
@@ -74107,6 +74107,17 @@
  visibility="public"
 >
 </field>
+<field name="TYPE_GRAVITY"
+ type="int"
+ transient="false"
+ volatile="false"
+ value="9"
+ static="true"
+ final="true"
+ deprecated="not deprecated"
+ visibility="public"
+>
+</field>
 <field name="TYPE_GYROSCOPE"
  type="int"
  transient="false"
@@ -74129,6 +74140,17 @@
  visibility="public"
 >
 </field>
+<field name="TYPE_LINEAR_ACCELERATION"
+ type="int"
+ transient="false"
+ volatile="false"
+ value="10"
+ static="true"
+ final="true"
+ deprecated="not deprecated"
+ visibility="public"
+>
+</field>
 <field name="TYPE_MAGNETIC_FIELD"
  type="int"
  transient="false"
@@ -74173,6 +74195,17 @@
  visibility="public"
 >
 </field>
+<field name="TYPE_ROTATION_VECTOR"
+ type="int"
+ transient="false"
+ volatile="false"
+ value="11"
+ static="true"
+ final="true"
+ deprecated="not deprecated"
+ visibility="public"
+>
+</field>
 <field name="TYPE_TEMPERATURE"
  type="int"
  transient="false"
@@ -74317,6 +74350,23 @@
  deprecated="not deprecated"
  visibility="public"
 >
+<method name="getAngleChange"
+ return="void"
+ abstract="false"
+ native="false"
+ synchronized="false"
+ static="true"
+ final="false"
+ deprecated="not deprecated"
+ visibility="public"
+>
+<parameter name="angleChange" type="float[]">
+</parameter>
+<parameter name="R" type="float[]">
+</parameter>
+<parameter name="prevR" type="float[]">
+</parameter>
+</method>
 <method name="getDefaultSensor"
  return="android.hardware.Sensor"
  abstract="false"
@@ -74358,6 +74408,21 @@
 <parameter name="values" type="float[]">
 </parameter>
 </method>
+<method name="getQuaternionFromVector"
+ return="void"
+ abstract="false"
+ native="false"
+ synchronized="false"
+ static="true"
+ final="false"
+ deprecated="not deprecated"
+ visibility="public"
+>
+<parameter name="Q" type="float[]">
+</parameter>
+<parameter name="rv" type="float[]">
+</parameter>
+</method>
 <method name="getRotationMatrix"
  return="boolean"
  abstract="false"
@@ -74377,6 +74442,21 @@
 <parameter name="geomagnetic" type="float[]">
 </parameter>
 </method>
+<method name="getRotationMatrixFromVector"
+ return="void"
+ abstract="false"
+ native="false"
+ synchronized="false"
+ static="true"
+ final="false"
+ deprecated="not deprecated"
+ visibility="public"
+>
+<parameter name="R" type="float[]">
+</parameter>
+<parameter name="rotationVector" type="float[]">
+</parameter>
+</method>
 <method name="getSensorList"
  return="java.util.List&lt;android.hardware.Sensor&gt;"
  abstract="false"
@@ -238895,7 +238975,7 @@
 <method name="availableProcessors"
  return="int"
  abstract="false"
- native="false"
+ native="true"
  synchronized="false"
  static="false"
  final="false"
diff --git a/core/java/android/hardware/Sensor.java b/core/java/android/hardware/Sensor.java
index 317e547..710e533 100644
--- a/core/java/android/hardware/Sensor.java
+++ b/core/java/android/hardware/Sensor.java
@@ -69,7 +69,27 @@ public class Sensor {
      */
     public static final int TYPE_PROXIMITY      = 8;
 
-    
+    /**
+     * A constant describing a gravity sensor type.
+     * See {@link android.hardware.SensorEvent SensorEvent}
+     * for more details.
+     */
+    public static final int TYPE_GRAVITY = 9;
+
+    /**
+     * A constant describing a linear acceleration sensor type.
+     * See {@link android.hardware.SensorEvent SensorEvent}
+     * for more details.
+     */
+    public static final int TYPE_LINEAR_ACCELERATION = 10;
+
+    /**
+     * A constant describing a rotation vector sensor type.
+     * See {@link android.hardware.SensorEvent SensorEvent}
+     * for more details.
+     */
+    public static final int TYPE_ROTATION_VECTOR = 11;
+
     /** 
      * A constant describing all sensor types.
      */
diff --git a/core/java/android/hardware/SensorEvent.java b/core/java/android/hardware/SensorEvent.java
index 9a9f0bf..bba59a4 100644
--- a/core/java/android/hardware/SensorEvent.java
+++ b/core/java/android/hardware/SensorEvent.java
@@ -119,6 +119,16 @@ public class SensorEvent {
      *  All values are in micro-Tesla (uT) and measure the ambient magnetic
      *  field in the X, Y and Z axis.
      *
+     * <p>{@link android.hardware.Sensor#TYPE_GYROSCOPE Sensor.TYPE_GYROSCOPE}:<p>
+     *  All values are in radians/second and measure the rate of rotation
+     *  around the X, Y and Z axis. The coordinate system is the same as is
+     *  used for the acceleration sensor.  Rotation is positive in the counter-clockwise
+     *  direction.  That is, an observer looking from some positive location on the x, y.
+     *  or z axis at a device positioned on the origin would report positive rotation
+     *  if the device appeared to be rotating counter clockwise.  Note that this is the
+     *  standard mathematical definition of positive rotation and does not agree with the
+     *  definition of roll given earlier.
+     *
      * <p>{@link android.hardware.Sensor#TYPE_LIGHT Sensor.TYPE_LIGHT}:<p>
      *
      *  <p>values[0]: Ambient light level in SI lux units
@@ -130,7 +140,29 @@ public class SensorEvent {
      *  <p> Note that some proximity sensors only support a binary "close" or "far" measurement.
      *   In this case, the sensor should report its maxRange value in the "far" state and a value
      *   less than maxRange in the "near" state.
+     *
+     *  <p>{@link android.hardware.Sensor#TYPE_GRAVITY Sensor.TYPE_GRAVITY}:<p>
+     *  A three dimensional vector indicating the direction and magnitude of gravity.  Units
+     *  are m/s^2.  The coordinate system is the same as is used by the acceleration sensor.
+     *
+     *  <p>{@link android.hardware.Sensor#TYPE_LINEAR_ACCELERATION Sensor.TYPE_LINEAR_ACCELERATION}:<p>
+     *  A three dimensional vector indicating acceleration along each device axis, not including
+     *  gravity.  All values have units of m/s^2.  The coordinate system is the same as is used by the
+     * acceleration sensor.
+     *
+     *  <p>{@link android.hardware.Sensor#TYPE_ROTATION_VECTOR Sensor.TYPE_ROTATION_VECTOR}:<p>
+     *  The rotation vector represents the orientation of the device as a combination of an angle
+     *  and an axis, in which the device has rotated through an angle theta around an axis
+     *  <x, y, z>. The three elements of the rotation vector are
+     *  <x*sin(theta/2), y*sin(theta/2), z*sin(theta/2)>, such that the magnitude of the rotation
+     *  vector is equal to sin(theta/2), and the direction of the rotation vector is equal to the
+     *  direction of the axis of rotation. The three elements of the rotation vector are equal to
+     *  the last three components of a unit quaternion
+     *  <cos(theta/2), x*sin(theta/2), y*sin(theta/2), z*sin(theta/2)>.  Elements of the rotation
+     *  vector are unitless.  The x,y, and z axis are defined in the same way as the acceleration
+     *  sensor.
      */
+
     public final float[] values;
 
     /**
diff --git a/core/java/android/hardware/SensorManager.java b/core/java/android/hardware/SensorManager.java
index 98172e6..31bdc0c 100644
--- a/core/java/android/hardware/SensorManager.java
+++ b/core/java/android/hardware/SensorManager.java
@@ -1555,7 +1555,184 @@ public class SensorManager
         }
     }
 
-    
+
+    /** Helper function to compute the angle change between two rotation matrices.
+     *  Given a current rotation matrix (R) and a previous rotation matrix
+     *  (prevR) computes the rotation around the x,y, and z axes which
+     *  transforms prevR to R.
+     *  outputs a 3 element vector containing the x,y, and z angle
+     *  change at indexes 0, 1, and 2 respectively.
+     * <p> Each input matrix is either as a 3x3 or 4x4 row-major matrix
+     * depending on the length of the passed array:
+     * <p>If the array length is 9, then the array elements represent this matrix
+     * <pre>
+     *   /  R[ 0]   R[ 1]   R[ 2]   \
+     *   |  R[ 3]   R[ 4]   R[ 5]   |
+     *   \  R[ 6]   R[ 7]   R[ 8]   /
+     *</pre>
+     * <p>If the array length is 16, then the array elements represent this matrix
+     * <pre>
+     *   /  R[ 0]   R[ 1]   R[ 2]   R[ 3]  \
+     *   |  R[ 4]   R[ 5]   R[ 6]   R[ 7]  |
+     *   |  R[ 8]   R[ 9]   R[10]   R[11]  |
+     *   \  R[12]   R[13]   R[14]   R[15]  /
+     *</pre>
+     * @param R current rotation matrix see {@link android.hardware.Sensor#TYPE_ROTATION_MATRIX}.
+     * @param prevR previous rotation matrix
+     * @param angleChange an array of floats in which the angle change is stored
+     */
+
+    public static void getAngleChange( float[] angleChange, float[] R, float[] prevR) {
+        float rd1=0,rd4=0, rd6=0,rd7=0, rd8=0;
+        float ri0=0,ri1=0,ri2=0,ri3=0,ri4=0,ri5=0,ri6=0,ri7=0,ri8=0;
+        float pri0=0, pri1=0, pri2=0, pri3=0, pri4=0, pri5=0, pri6=0, pri7=0, pri8=0;
+        int i, j, k;
+
+        if(R.length == 9) {
+            ri0 = R[0];
+            ri1 = R[1];
+            ri2 = R[2];
+            ri3 = R[3];
+            ri4 = R[4];
+            ri5 = R[5];
+            ri6 = R[6];
+            ri7 = R[7];
+            ri8 = R[8];
+        } else if(R.length == 16) {
+            ri0 = R[0];
+            ri1 = R[1];
+            ri2 = R[2];
+            ri3 = R[4];
+            ri4 = R[5];
+            ri5 = R[6];
+            ri6 = R[8];
+            ri7 = R[9];
+            ri8 = R[10];
+        }
+
+        if(prevR.length == 9) {
+            pri0 = R[0];
+            pri1 = R[1];
+            pri2 = R[2];
+            pri3 = R[3];
+            pri4 = R[4];
+            pri5 = R[5];
+            pri6 = R[6];
+            pri7 = R[7];
+            pri8 = R[8];
+        } else if(prevR.length == 16) {
+            pri0 = R[0];
+            pri1 = R[1];
+            pri2 = R[2];
+            pri3 = R[4];
+            pri4 = R[5];
+            pri5 = R[6];
+            pri6 = R[8];
+            pri7 = R[9];
+            pri8 = R[10];
+        }
+
+        // calculate the parts of the rotation difference matrix we need
+        // rd[i][j] = pri[0][i] * ri[0][j] + pri[1][i] * ri[1][j] + pri[2][i] * ri[2][j];
+
+        rd1 = pri0 * ri1 + pri3 * ri4 + pri6 * ri7; //rd[0][1]
+        rd4 = pri1 * ri1 + pri4 * ri4 + pri7 * ri7; //rd[1][1]
+        rd6 = pri2 * ri0 + pri5 * ri3 + pri8 * ri6; //rd[2][0]
+        rd7 = pri2 * ri1 + pri5 * ri4 + pri8 * ri7; //rd[2][1]
+        rd8 = pri2 * ri2 + pri5 * ri5 + pri8 * ri8; //rd[2][2]
+
+        angleChange[0] = (float)Math.atan2(rd1, rd4);
+        angleChange[1] = (float)Math.asin(-rd7);
+        angleChange[2] = (float)Math.atan2(-rd6, rd8);
+
+    }
+
+    /** Helper function to convert a rotation vector to a rotation matrix.
+     *  Given a rotation vector (presumably from a ROTATION_VECTOR sensor), returns a
+     *  9  or 16 element rotation matrix in the array R.  R must have length 9 or 16.
+     *  If R.length == 9, the following matrix is returned:
+     * <pre>
+     *   /  R[ 0]   R[ 1]   R[ 2]   \
+     *   |  R[ 3]   R[ 4]   R[ 5]   |
+     *   \  R[ 6]   R[ 7]   R[ 8]   /
+     *</pre>
+     * If R.length == 16, the following matrix is returned:
+     * <pre>
+     *   /  R[ 0]   R[ 1]   R[ 2]   0  \
+     *   |  R[ 4]   R[ 5]   R[ 6]   0  |
+     *   |  R[ 8]   R[ 9]   R[10]   0  |
+     *   \  0       0       0       1  /
+     *</pre>
+     *  @param rotationVector the rotation vector to convert
+     *  @param R an array of floats in which to store the rotation matrix
+     */
+    public static void getRotationMatrixFromVector(float[] R, float[] rotationVector) {
+        float q0 = (float)Math.sqrt(1 - rotationVector[0]*rotationVector[0] -
+                                    rotationVector[1]*rotationVector[1] -
+                                    rotationVector[2]*rotationVector[2]);
+        float q1 = rotationVector[0];
+        float q2 = rotationVector[1];
+        float q3 = rotationVector[2];
+
+        float sq_q1 = 2 * q1 * q1;
+        float sq_q2 = 2 * q2 * q2;
+        float sq_q3 = 2 * q3 * q3;
+        float q1_q2 = 2 * q1 * q2;
+        float q3_q0 = 2 * q3 * q0;
+        float q1_q3 = 2 * q1 * q3;
+        float q2_q0 = 2 * q2 * q0;
+        float q2_q3 = 2 * q2 * q3;
+        float q1_q0 = 2 * q1 * q0;
+
+        if(R.length == 9) {
+            R[0] = 1 - sq_q2 - sq_q3;
+            R[1] = q1_q2 - q3_q0;
+            R[2] = q1_q3 + q2_q0;
+
+            R[3] = q1_q2 + q3_q0;
+            R[4] = 1 - sq_q1 - sq_q3;
+            R[5] = q2_q3 - q1_q0;
+
+            R[6] = q1_q3 - q2_q0;
+            R[7] = q2_q3 + q1_q0;
+            R[8] = 1 - sq_q1 - sq_q2;
+        } else if (R.length == 16) {
+            R[0] = 1 - sq_q2 - sq_q3;
+            R[1] = q1_q2 - q3_q0;
+            R[2] = q1_q3 + q2_q0;
+            R[3] = 0.0f;
+
+            R[4] = q1_q2 + q3_q0;
+            R[5] = 1 - sq_q1 - sq_q3;
+            R[6] = q2_q3 - q1_q0;
+            R[7] = 0.0f;
+
+            R[8] = q1_q3 - q2_q0;
+            R[9] = q2_q3 + q1_q0;
+            R[10] = 1 - sq_q1 - sq_q2;
+            R[11] = 0.0f;
+
+            R[12] = R[13] = R[14] = 0.0f;
+            R[15] = 1.0f;
+        }
+    }
+
+    /** Helper function to convert a rotation vector to a normalized quaternion.
+     *  Given a rotation vector (presumably from a ROTATION_VECTOR sensor), returns a normalized
+     *  quaternion in the array Q.  The quaternion is stored as [w, x, y, z]
+     *  @param rv the rotation vector to convert
+     *  @param Q an array of floats in which to store the computed quaternion
+     */
+    public static void getQuaternionFromVector(float[] Q, float[] rv) {
+        float w = (float)Math.sqrt(1 - rv[0]*rv[0] - rv[1]*rv[1] - rv[2]*rv[2]);
+        //In this case, the w component of the quaternion is known to be a positive number
+
+        Q[0] = w;
+        Q[1] = rv[0];
+        Q[2] = rv[1];
+        Q[3] = rv[2];
+    }
+
     private static native void nativeClassInit();
 
     private static native int sensors_module_init();