Each input matrix is either as a 3x3 or 4x4 row-major matrix + * depending on the length of the passed array: + *
If the array length is 9, then the array elements represent this matrix + *
+ * / R[ 0] R[ 1] R[ 2] \ + * | R[ 3] R[ 4] R[ 5] | + * \ R[ 6] R[ 7] R[ 8] / + *+ *
If the array length is 16, then the array elements represent this matrix + *
+ * / 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] / + *+ * @param R current 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: + *
+ * / R[ 0] R[ 1] R[ 2] \ + * | R[ 3] R[ 4] R[ 5] | + * \ R[ 6] R[ 7] R[ 8] / + *+ * If R.length == 16, the following matrix is returned: + *
+ * / 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 / + *+ * @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();