Program: 194916
Project: 194916
Buggy source:
/** 
 * <p> Computes the inclination matrix <b>I</b> as well as the rotation matrix <b>R</b> transforming a vector from the device coordinate system to the world's coordinate system which is defined as a direct orthonormal basis, where: </p> <ul> <li>X is defined as the vector product <b>Y.Z</b> (It is tangential to the ground at the device's current location and roughly points East).</li> <li>Y is tangential to the ground at the device's current location and points towards the magnetic North Pole.</li> <li>Z points towards the sky and is perpendicular to the ground.</li> </ul> <p> <center><img src="../../../images/axis_globe.png" alt="World coordinate-system diagram." border="0" /></center> </p> <p> <hr> <p> By definition: <p> [0 0 g] = <b>R</b> * <b>gravity</b> (g = magnitude of gravity) <p> [0 m 0] = <b>I</b> * <b>R</b> * <b>geomagnetic</b> (m = magnitude of geomagnetic field) <p> <b>R</b> is the identity matrix when the device is aligned with the world's coordinate system, that is, when the device's X axis points toward East, the Y axis points to the North Pole and the device is facing the sky. <p> <b>I</b> is a rotation matrix transforming the geomagnetic vector into the same coordinate space as gravity (the world's coordinate space). <b>I</b> is a simple rotation around the X axis. The inclination angle in radians can be computed with   {@link #getInclination}. <hr> <p> Each matrix is returned either as a 3x3 or 4x4 row-major matrix depending on the length of the passed array: <p> <u>If the array length is 16:</u> <pre> /  M[ 0]   M[ 1]   M[ 2]   M[ 3]  \ |  M[ 4]   M[ 5]   M[ 6]   M[ 7]  | |  M[ 8]   M[ 9]   M[10]   M[11]  | \  M[12]   M[13]   M[14]   M[15]  / </pre> This matrix is ready to be used by OpenGL ES's  {@link javax.microedition.khronos.opengles.GL10#glLoadMatrixf(float[],int) glLoadMatrixf(float[], int)}. <p> Note that because OpenGL matrices are column-major matrices you must transpose the matrix before using it. However, since the matrix is a rotation matrix, its transpose is also its inverse, conveniently, it is often the inverse of the rotation that is needed for rendering; it can therefore be used with OpenGL ES directly. <p> Also note that the returned matrices always have this form: <pre> /  M[ 0]   M[ 1]   M[ 2]   0  \ |  M[ 4]   M[ 5]   M[ 6]   0  | |  M[ 8]   M[ 9]   M[10]   0  | \      0       0       0   1  / </pre> <p> <u>If the array length is 9:</u> <pre> /  M[ 0]   M[ 1]   M[ 2]  \ |  M[ 3]   M[ 4]   M[ 5]  | \  M[ 6]   M[ 7]   M[ 8]  / </pre> <hr> <p> The inverse of each matrix can be computed easily by taking its transpose. <p> The matrices returned by this function are meaningful only when the device is not free-falling and it is not close to the magnetic north. If the device is accelerating, or placed into a strong magnetic field, the returned matrices may be inaccurate.
 * @param R is an array of 9 floats holding the rotation matrix <b>R</b> when this function returns. R can be null. <p>
 * @param I is an array of 9 floats holding the rotation matrix <b>I</b> when this function returns. I can be null. <p>
 * @param gravity is an array of 3 floats containing the gravity vector expressed in the device's coordinate. You can simply use the {@link android.hardware.SensorEvent#values values} returned by a{@link android.hardware.SensorEvent SensorEvent} of a{@link android.hardware.Sensor Sensor} of type{@link android.hardware.Sensor#TYPE_ACCELEROMETER TYPE_ACCELEROMETER}. <p>
 * @param geomagnetic is an array of 3 floats containing the geomagnetic vector expressed in the device's coordinate. You can simply use the {@link android.hardware.SensorEvent#values values} returned by a{@link android.hardware.SensorEvent SensorEvent} of a{@link android.hardware.Sensor Sensor} of type{@link android.hardware.Sensor#TYPE_MAGNETIC_FIELD TYPE_MAGNETIC_FIELD}.
 * @return <code>true</code> on success, <code>false</code> on failure (forinstance, if the device is in free fall). On failure the output matrices are not modified.
 * @see #getInclination(float[])
 * @see #getOrientation(float[],float[])
 * @see #remapCoordinateSystem(float[],int,int,float[])
 */
public static boolean getRotationMatrix(float[] R,float[] I,float[] gravity,float[] geomagnetic){
  float Ax=gravity[0];
  float Ay=gravity[1];
  float Az=gravity[2];
  final float Ex=geomagnetic[0];
  final float Ey=geomagnetic[1];
  final float Ez=geomagnetic[2];
  float Hx=Ey * Az - Ez * Ay;
  float Hy=Ez * Ax - Ex * Az;
  float Hz=Ex * Ay - Ey * Ax;
  final float normH=(float)Math.sqrt(Hx * Hx + Hy * Hy + Hz * Hz);
  if (normH < 0.1f) {
    return false;
  }
  final float invH=1.0f / normH;
  Hx*=invH;
  Hy*=invH;
  Hz*=invH;
  final float invA=1.0f / (float)Math.sqrt(Ax * Ax + Ay * Ay + Az * Az);
  Ax*=invA;
  Ay*=invA;
  Az*=invA;
  final float Mx=Ay * Hz - Az * Hy;
  final float My=Az * Hx - Ax * Hz;
  final float Mz=Ax * Hy - Ay * Hx;
  if (R != null) {
    if (R.length == 9) {
      R[0]=Hx;
      R[1]=Hy;
      R[2]=Hz;
      R[3]=Mx;
      R[4]=My;
      R[5]=Mz;
      R[6]=Ax;
      R[7]=Ay;
      R[8]=Az;
    }
 else     if (R.length == 16) {
      R[0]=Hx;
      R[1]=Hy;
      R[2]=Hz;
      R[3]=0;
      R[4]=Mx;
      R[5]=My;
      R[6]=Mz;
      R[7]=0;
      R[8]=Ax;
      R[9]=Ay;
      R[10]=Az;
      R[11]=0;
      R[12]=0;
      R[13]=0;
      R[14]=0;
      R[15]=1;
    }
  }
  if (I != null) {
    final float invE=1.0f / (float)Math.sqrt(Ex * Ex + Ey * Ey + Ez * Ez);
    final float c=(Ex * Mx + Ey * My + Ez * Mz) * invE;
    final float s=(Ex * Ax + Ey * Ay + Ez * Az) * invE;
    if (I.length == 9) {
      I[0]=1;
      I[1]=0;
      I[2]=0;
      I[3]=0;
      I[4]=c;
      I[5]=s;
      I[6]=0;
      I[7]=-s;
      I[8]=c;
    }
 else     if (I.length == 16) {
      I[0]=1;
      I[1]=0;
      I[2]=0;
      I[4]=0;
      I[5]=c;
      I[6]=s;
      I[8]=0;
      I[9]=-s;
      I[10]=c;
      I[3]=I[7]=I[11]=I[12]=I[13]=I[14]=0;
      I[15]=1;
    }
  }
  return true;
}

Fixed version (ground truth):
/** 
 * <p> Computes the inclination matrix <b>I</b> as well as the rotation matrix <b>R</b> transforming a vector from the device coordinate system to the world's coordinate system which is defined as a direct orthonormal basis, where: </p> <ul> <li>X is defined as the vector product <b>Y.Z</b> (It is tangential to the ground at the device's current location and roughly points East).</li> <li>Y is tangential to the ground at the device's current location and points towards the magnetic North Pole.</li> <li>Z points towards the sky and is perpendicular to the ground.</li> </ul> <p> <center><img src="../../../images/axis_globe.png" alt="World coordinate-system diagram." border="0" /></center> </p> <p> <hr> <p> By definition: <p> [0 0 g] = <b>R</b> * <b>gravity</b> (g = magnitude of gravity) <p> [0 m 0] = <b>I</b> * <b>R</b> * <b>geomagnetic</b> (m = magnitude of geomagnetic field) <p> <b>R</b> is the identity matrix when the device is aligned with the world's coordinate system, that is, when the device's X axis points toward East, the Y axis points to the North Pole and the device is facing the sky. <p> <b>I</b> is a rotation matrix transforming the geomagnetic vector into the same coordinate space as gravity (the world's coordinate space). <b>I</b> is a simple rotation around the X axis. The inclination angle in radians can be computed with   {@link #getInclination}. <hr> <p> Each matrix is returned either as a 3x3 or 4x4 row-major matrix depending on the length of the passed array: <p> <u>If the array length is 16:</u> <pre> /  M[ 0]   M[ 1]   M[ 2]   M[ 3]  \ |  M[ 4]   M[ 5]   M[ 6]   M[ 7]  | |  M[ 8]   M[ 9]   M[10]   M[11]  | \  M[12]   M[13]   M[14]   M[15]  / </pre> This matrix is ready to be used by OpenGL ES's  {@link javax.microedition.khronos.opengles.GL10#glLoadMatrixf(float[],int) glLoadMatrixf(float[], int)}. <p> Note that because OpenGL matrices are column-major matrices you must transpose the matrix before using it. However, since the matrix is a rotation matrix, its transpose is also its inverse, conveniently, it is often the inverse of the rotation that is needed for rendering; it can therefore be used with OpenGL ES directly. <p> Also note that the returned matrices always have this form: <pre> /  M[ 0]   M[ 1]   M[ 2]   0  \ |  M[ 4]   M[ 5]   M[ 6]   0  | |  M[ 8]   M[ 9]   M[10]   0  | \      0       0       0   1  / </pre> <p> <u>If the array length is 9:</u> <pre> /  M[ 0]   M[ 1]   M[ 2]  \ |  M[ 3]   M[ 4]   M[ 5]  | \  M[ 6]   M[ 7]   M[ 8]  / </pre> <hr> <p> The inverse of each matrix can be computed easily by taking its transpose. <p> The matrices returned by this function are meaningful only when the device is not free-falling and it is not close to the magnetic north. If the device is accelerating, or placed into a strong magnetic field, the returned matrices may be inaccurate.
 * @param R is an array of 9 floats holding the rotation matrix <b>R</b> when this function returns. R can be null. <p>
 * @param I is an array of 9 floats holding the rotation matrix <b>I</b> when this function returns. I can be null. <p>
 * @param gravity is an array of 3 floats containing the gravity vector expressed in the device's coordinate. You can simply use the {@link android.hardware.SensorEvent#values values} returned by a{@link android.hardware.SensorEvent SensorEvent} of a{@link android.hardware.Sensor Sensor} of type{@link android.hardware.Sensor#TYPE_ACCELEROMETER TYPE_ACCELEROMETER}. <p>
 * @param geomagnetic is an array of 3 floats containing the geomagnetic vector expressed in the device's coordinate. You can simply use the {@link android.hardware.SensorEvent#values values} returned by a{@link android.hardware.SensorEvent SensorEvent} of a{@link android.hardware.Sensor Sensor} of type{@link android.hardware.Sensor#TYPE_MAGNETIC_FIELD TYPE_MAGNETIC_FIELD}.
 * @return <code>true</code> on success, <code>false</code> on failure (forinstance, if the device is in free fall). Free fall is defined as condition when the magnitude of the gravity is less than 1/10 of the nominal value. On failure the output matrices are not modified.
 * @see #getInclination(float[])
 * @see #getOrientation(float[],float[])
 * @see #remapCoordinateSystem(float[],int,int,float[])
 */
public static boolean getRotationMatrix(float[] R,float[] I,float[] gravity,float[] geomagnetic){
  float Ax=gravity[0];
  float Ay=gravity[1];
  float Az=gravity[2];
  final float normsqA=(Ax * Ax + Ay * Ay + Az * Az);
  final float g=9.81f;
  final float freeFallGravitySquared=0.01f * g * g;
  if (normsqA < freeFallGravitySquared) {
    return false;
  }
  final float Ex=geomagnetic[0];
  final float Ey=geomagnetic[1];
  final float Ez=geomagnetic[2];
  float Hx=Ey * Az - Ez * Ay;
  float Hy=Ez * Ax - Ex * Az;
  float Hz=Ex * Ay - Ey * Ax;
  final float normH=(float)Math.sqrt(Hx * Hx + Hy * Hy + Hz * Hz);
  if (normH < 0.1f) {
    return false;
  }
  final float invH=1.0f / normH;
  Hx*=invH;
  Hy*=invH;
  Hz*=invH;
  final float invA=1.0f / (float)Math.sqrt(Ax * Ax + Ay * Ay + Az * Az);
  Ax*=invA;
  Ay*=invA;
  Az*=invA;
  final float Mx=Ay * Hz - Az * Hy;
  final float My=Az * Hx - Ax * Hz;
  final float Mz=Ax * Hy - Ay * Hx;
  if (R != null) {
    if (R.length == 9) {
      R[0]=Hx;
      R[1]=Hy;
      R[2]=Hz;
      R[3]=Mx;
      R[4]=My;
      R[5]=Mz;
      R[6]=Ax;
      R[7]=Ay;
      R[8]=Az;
    }
 else     if (R.length == 16) {
      R[0]=Hx;
      R[1]=Hy;
      R[2]=Hz;
      R[3]=0;
      R[4]=Mx;
      R[5]=My;
      R[6]=Mz;
      R[7]=0;
      R[8]=Ax;
      R[9]=Ay;
      R[10]=Az;
      R[11]=0;
      R[12]=0;
      R[13]=0;
      R[14]=0;
      R[15]=1;
    }
  }
  if (I != null) {
    final float invE=1.0f / (float)Math.sqrt(Ex * Ex + Ey * Ey + Ez * Ez);
    final float c=(Ex * Mx + Ey * My + Ez * Mz) * invE;
    final float s=(Ex * Ax + Ey * Ay + Ez * Az) * invE;
    if (I.length == 9) {
      I[0]=1;
      I[1]=0;
      I[2]=0;
      I[3]=0;
      I[4]=c;
      I[5]=s;
      I[6]=0;
      I[7]=-s;
      I[8]=c;
    }
 else     if (I.length == 16) {
      I[0]=1;
      I[1]=0;
      I[2]=0;
      I[4]=0;
      I[5]=c;
      I[6]=s;
      I[8]=0;
      I[9]=-s;
      I[10]=c;
      I[3]=I[7]=I[11]=I[12]=I[13]=I[14]=0;
      I[15]=1;
    }
  }
  return true;
}