FrameRotationQueue { private static void getRotationMatrixFromAngleAxis(float[] matrix, float[] angleAxis) { float x = angleAxis[0]; float y = -angleAxis[1]; float z = -angleAxis[2]; float angleRad = Matrix.length(x, y, z); if (angleRad != 0) { float angleDeg = (float) Math.toDegrees(angleRad); Matrix.setRotateM(matrix, 0, angleDeg, x / angleRad, y / angleRad, z / angleRad); } else { Matrix.setIdentityM(matrix, 0); } } FrameRotationQueue(); void setRotation(long timestampUs, float[] angleAxis); void reset(); boolean pollRotationMatrix(float[] matrix, long timestampUs); static void computeRecenterMatrix(float[] recenterMatrix, float[] rotationMatrix);  }
@Test public void testRecentering_justYaw() throws Exception { float[] actualMatrix = getRotationMatrixFromAngleAxis( (float) Math.PI, 0, 1, 0); float[] expectedMatrix = new float[16]; Matrix.setIdentityM(expectedMatrix, 0); assertEquals(actualMatrix, expectedMatrix); }
@Test public void testRecentering_yawAndPitch() throws Exception { float[] matrix = getRotationMatrixFromAngleAxis( (float) Math.PI, 1, 1, 0); assertMultiplication( 0, 0, 1, matrix, 0, 0, 1); }
@Test public void testRecentering_yawAndPitch2() throws Exception { float[] matrix = getRotationMatrixFromAngleAxis( (float) Math.PI / 2, 1, 1, 0); float sqrt2 = (float) Math.sqrt(2); assertMultiplication( sqrt2, 0, 0, matrix, 1, -1, 0); }
@Test public void testRecentering_yawAndPitchAndRoll() throws Exception { float[] matrix = getRotationMatrixFromAngleAxis( (float) Math.PI * 2 / 3, 1, 1, 1); assertMultiplication( 0, 0, 1, matrix, 0, 0, 1); }