gistfile1.txt

private void getRotationVectorFromGyro(float timeFactor) {
        System.out.println("getRotationVectorFromGyro");

        // Calculate the angular speed of the sample
        float omegaMagnitude = (float) Math.sqrt(Math.pow(gyroscope[0], 2)
                + Math.pow(gyroscope[1], 2) + Math.pow(gyroscope[2], 2));

        // Normalize the rotation vector if it's big enough to get the axis
        if (omegaMagnitude > EPSILON) {
            gyroscope[0] /= omegaMagnitude;
            gyroscope[1] /= omegaMagnitude;
            gyroscope[2] /= omegaMagnitude;
        }

        // Integrate around this axis with the angular speed by the timestep
        // in order to get a delta rotation from this sample over the timestep
        // We will convert this axis-angle representation of the delta rotation
        // into a quaternion before turning it into the rotation matrix.
        float thetaOverTwo = omegaMagnitude * timeFactor;
        float sinThetaOverTwo = (float) Math.sin(thetaOverTwo);
        float cosThetaOverTwo = (float) Math.cos(thetaOverTwo);

        deltaRotationVector[0] = sinThetaOverTwo * gyroscope[0];
        deltaRotationVector[1] = sinThetaOverTwo * gyroscope[1];
        deltaRotationVector[2] = sinThetaOverTwo * gyroscope[2];
        deltaRotationVector[3] = cosThetaOverTwo;
    }

    private void getGravityVector(float[] deltaRotationVector) {
        System.out.println("getGravityVector");
        float q0 = deltaRotationVector[0];
        float q1 = deltaRotationVector[1];
        float q2 = deltaRotationVector[2];
        float q3 = deltaRotationVector[3];

        float gx = 2 * (q1 * q3 - q0 * q2);
        float gy = 2 * (q0 * q1 + q2 * q3);
        float gz = q0 * q0 - q1 * q1 - q2 * q2 + q3 * q3;

        gravity[0] = gx;
        gravity[1] = gy;
        gravity[2] = gz;
    }