Develop Reference

Android Gyroscope for tilting

Thanks in advance for your help first.
I have found so many examples using Gyroscope. But I couldn't find adequate one for me.
I'd like to make a simple quiz game that do actions when I tilt VM to 90 degrees forward and backward. Many examples said I might use "pitch" value of Gyroscope. Could you give some advices for me??

I have done a similar thing where i need draw a rectangle with includes nearby places and must point it to the place and show details.
public void onSensorChanged(SensorEvent event) {
final Handler handler = new Handler();
switch (event.sensor.getType()) {
case Sensor.TYPE_ACCELEROMETER:
mAcceleromterReading =
SensorUtilities.filterSensors(event.values, mAcceleromterReading);
break;
case Sensor.TYPE_MAGNETIC_FIELD:
mMagnetometerReading =
SensorUtilities.filterSensors(event.values, mMagnetometerReading);
break;
float[] orientation =
SensorUtilities.computeDeviceOrientation(mAcceleromterReading, mMagnetometerReading);
if (orientation != null) {
float azimuth = (float) Math.toDegrees(orientation[0]);
if (azimuth < 0) {
azimuth += 360f;
}
// Convert pitch and roll from radians to degrees
float pitch = (float) Math.toDegrees(orientation[1]);
float roll = (float) Math.toDegrees(orientation[2]);
if (abs(pitch - pitchPrev) > PITCH_THRESHOLD && abs(roll - rollPrev) > ROLL_THRESHOLD
&& abs(azimuth - azimuthPrev) > AZIMUTH_THRESHOLD) { // && abs(roll - rollPrev) > rollThreshold
if (DashplexManager.getInstance().mlocation != null) {
mOverlayDisplayView.setHorizontalFOV(mPreview.getHorizontalFOV());
mOverlayDisplayView.setVerticalFOV(mPreview.getVerticalFOV());
mOverlayDisplayView.setAzimuth(azimuth);
mOverlayDisplayView.setPitch(pitch);
mOverlayDisplayView.setRoll(roll);
// Update the OverlayDisplayView to red raw when sensor dataLogin changes,
// redrawing only when the camera is not pointing straight up or down
if (pitch <= 75 && pitch >= -75) {
//Log.d("issueAR", "invalidate: ");
mOverlayDisplayView.invalidate();
}
}
pitchPrev = pitch;
rollPrev = roll;
azimuthPrev = azimuth;
}
}
computeDeviceOrientation method
public static float[] computeDeviceOrientation(float[] accelerometerReading, float[] magnetometerReading) {
if (accelerometerReading == null || magnetometerReading == null) {
return null;
}
final float[] rotationMatrix = new float[9];
SensorManager.getRotationMatrix(rotationMatrix, null, accelerometerReading, magnetometerReading);
// Remap the coordinates with the camera pointing along the Y axis.
// This way, portrait and landscape orientation return the same azimuth to magnetic north.
final float cameraRotationMatrix[] = new float[9];
SensorManager.remapCoordinateSystem(rotationMatrix, SensorManager.AXIS_X,
SensorManager.AXIS_Z, cameraRotationMatrix);
final float[] orientationAngles = new float[3];
SensorManager.getOrientation(cameraRotationMatrix, orientationAngles);
// Return a float array containing [azimuth, pitch, roll]
return orientationAngles;
}
onDraw method
#SuppressLint("DrawAllocation")
#Override
protected void onDraw(Canvas canvas) {
super.onDraw(canvas);
// Log.d("issueAR", "onDraw: ");
// Log.d("issueAR", "mVerticalFOV: "+mVerticalFOV+" "+"mHorizontalFOV"+mHorizontalFOV);
// Get the viewports only once
if (!mGotViewports && mVerticalFOV > 0 && mHorizontalFOV > 0) {
mViewportHeight = canvas.getHeight() / mVerticalFOV;
mViewportWidth = canvas.getWidth() / mHorizontalFOV;
mGotViewports = true;
//Log.d("onDraw", "mViewportHeight: " + mViewportHeight);
}
if (!mGotViewports) {
return;
}
// Set the paints that remain constant only once
if (!mSetPaints) {
mTextPaint.setTextAlign(Paint.Align.LEFT);
mTextPaint.setTextSize(getResources().getDimensionPixelSize(R.dimen.canvas_text_size));
mTextPaint.setColor(Color.WHITE);
mOutlinePaint.setStyle(Paint.Style.STROKE);
mOutlinePaint.setStrokeWidth(mOutline);
mBubblePaint.setStyle(Paint.Style.FILL);
mSetPaints = true;
}
// Center of view
float x = canvas.getWidth() / 2;
float y = canvas.getHeight() / 2;
/*
* Uncomment line below to allow rotation of display around the center point
* based on the roll. However, this "feature" is not very intuitive, and requires
* locking device orientation to portrait or changes the sensor rotation matrix
* on device rotation. It's really quite a nightmare.
*/
//canvas.rotate((0.0f - mRoll), x, y);
float dy = mPitch * mViewportHeight;
if (mNearbyPlaces != null) {
//Log.d("OverlayDisplayView", "mNearbyPlaces: "+mNearbyPlaces.size());
// Iterate backwards to draw more distant places first
for (int i = mNearbyPlaces.size() - 1; i >= 0; i--) {
NearbyPlace nearbyPlace = mNearbyPlaces.get(i);
float xDegreesToTarget = mAzimuth - nearbyPlace.getBearingToPlace();
float dx = mViewportWidth * xDegreesToTarget;
float iconX = x - dx;
float iconY = y - dy;
if (isOverlapping(iconX, iconX).isOverlapped()) {
PointF point = calculateNewXY(new PointF(iconX, iconY + mViewportHeight));
iconX = point.x;
iconY = point.y;
}
nearbyPlace.setIconX(iconX);
nearbyPlace.setIconY(iconY);
Bitmap icon = getIcon(nearbyPlace.getIcon_id());
float width = icon.getWidth() + mTextPaint.measureText(nearbyPlace.getName()) + mMargin;
RectF recf=new RectF(iconX, iconY, width, icon.getHeight());
nearbyPlace.setRect(recf);
float angleToTarget = xDegreesToTarget;
if (xDegreesToTarget < 0) {
angleToTarget = 360 + xDegreesToTarget;
}
if (angleToTarget >= 0 && angleToTarget < 90) {
nearbyPlace.setQuadrant(1);
mQuad1Places.add(nearbyPlace);
} else if (angleToTarget >= 90 && angleToTarget < 180) {
nearbyPlace.setQuadrant(2);
mQuad2Places.add(nearbyPlace);
} else if (angleToTarget >= 180 && angleToTarget < 270) {
nearbyPlace.setQuadrant(3);
mQuad3Places.add(nearbyPlace);
} else {
nearbyPlace.setQuadrant(4);
mQuad4Places.add(nearbyPlace);
}
//Log.d("TAG", " - X: " + iconX + " y: " + iconY + " angle: " + angleToTarget + " display: " + nearbyPlace.getIcon_id());
}
drawQuadrant(mQuad1Places, canvas);
drawQuadrant(mQuad2Places, canvas);
drawQuadrant(mQuad3Places, canvas);
drawQuadrant(mQuad4Places, canvas);
}
}
It doesnot contain full code, but you may understand how pitch and azimuth with roll is used.. Best of luck

How to get smooth orientation data in android

I have an app which uses orientation data which works very well using the pre API-8 method of using a Sensor.TYPE_ORIENTAITON. Smoothing that data was relatively easy.
I am trying to update the code to avoid using this deprecated approach. The new standard approach is to replace the single Sensor.TYPE_ORIENTATION with a Sensor.TYPE_ACCELEROMETER and Sensor.TYPE_MAGENTIC_FIELD combination. As that data is received, it is sent (via SensorManager.getRotationMatrix()) to SensorManager.getOrientation(). This (theoretically) returns the same information as Sensor.TYPE_ORIENTATION did (apart from different units and axis orientation).
However, this approach seems to generate data which is much more jittery (ie noisy) than the deprecated method (which still works). So, if you compare the same information on the same device, the deprecated method provides much less noisy data than the current method.
How do I get the actual same (less noisy) data that the deprecated method used to provide?
To make my question a little clearer: I have read various answers on this subject, and I have tried all sorts of filter: simple KF / IIR low pass as you suggest; median filter between 5 and 19 points, but so far I have yet to get anywhere close to the smoothness of the data the phone supplies via TYPE_ORIENTATION.

Apply a low-pass filter to your sensor output.
This is my low-pass filter method:
private static final float ALPHA = 0.5f;
//lower alpha should equal smoother movement
...
private float[] applyLowPassFilter(float[] input, float[] output) {
if ( output == null ) return input;
for ( int i=0; i<input.length; i++ ) {
output[i] = output[i] + ALPHA * (input[i] - output[i]);
}
return output;
}
Apply it like so:
float[] mGravity;
float[] mGeomagnetic;
#Override
public void onSensorChanged(SensorEvent event) {
if (event.sensor.getType() == Sensor.TYPE_ACCELEROMETER)
mGravity = applyLowPassFilter(event.values.clone(), mGravity);
if (event.sensor.getType() == Sensor.TYPE_MAGNETIC_FIELD)
mGeomagnetic = applyLowPassFilter(event.values.clone(), mGeomagnetic);
if (mGravity != null && mGeomagnetic != null) {
float R[] = new float[9];
float I[] = new float[9];
boolean success = SensorManager.getRotationMatrix(R, I, mGravity, mGeomagnetic);
if (success) {
float orientation[] = new float[3];
SensorManager.getOrientation(R, orientation);
azimuth = -orientation[0];
invalidate();
}
}
}
This is obviously code for a compass, remove what you don't need.
Also, take a look at this SE question How to implement low pass filter using java

It turns out that there is another, not particularly documented, way to get orientation data. Hidden in the list of sensor types is TYPE_ROTATION_VECTOR. So, set one up:
Sensor mRotationVectorSensor = sensorManager.getDefaultSensor(Sensor.TYPE_ROTATION_VECTOR);
sensorManager.registerListener(this, mRotationVectorSensor, SensorManager.SENSOR_DELAY_GAME);
Then:
#Override
public void onSensorChanged(SensorEvent event) {
final int eventType = event.sensor.getType();
if (eventType != Sensor.TYPE_ROTATION_VECTOR) return;
long timeNow = System.nanoTime();
float mOrientationData[] = new float[3];
calcOrientation(mOrientationData, event.values.clone());
// Do what you want with mOrientationData
}
The key mechanism is going from the incoming rotation data to an orientation vector via a rotation matrix. The slightly frustrating thing is the orientation vector comes from quaternion data in the first place, but I can't see how to get the quaternion delivered direct. (If you ever wondered how quaternions relate to orientatin and rotation information, and why they are used, see here.)
private void calcOrientation(float[] orientation, float[] incomingValues) {
// Get the quaternion
float[] quatF = new float[4];
SensorManager.getQuaternionFromVector(quatF, incomingValues);
// Get the rotation matrix
//
// This is a variant on the code presented in
// http://www.euclideanspace.com/maths/geometry/rotations/conversions/quaternionToMatrix/
// which has been altered for scaling and (I think) a different axis arrangement. It
// tells you the rotation required to get from the between the phone's axis
// system and the earth's.
//
// Phone axis system:
// https://developer.android.com/guide/topics/sensors/sensors_overview.html#sensors-coords
//
// Earth axis system:
// https://developer.android.com/reference/android/hardware/SensorManager.html#getRotationMatrix(float[], float[], float[], float[])
//
// Background information:
// https://en.wikipedia.org/wiki/Rotation_matrix
//
float[][] rotMatF = new float[3][3];
rotMatF[0][0] = quatF[1]*quatF[1] + quatF[0]*quatF[0] - 0.5f;
rotMatF[0][1] = quatF[1]*quatF[2] - quatF[3]*quatF[0];
rotMatF[0][2] = quatF[1]*quatF[3] + quatF[2]*quatF[0];
rotMatF[1][0] = quatF[1]*quatF[2] + quatF[3]*quatF[0];
rotMatF[1][1] = quatF[2]*quatF[2] + quatF[0]*quatF[0] - 0.5f;
rotMatF[1][2] = quatF[2]*quatF[3] - quatF[1]*quatF[0];
rotMatF[2][0] = quatF[1]*quatF[3] - quatF[2]*quatF[0];
rotMatF[2][1] = quatF[2]*quatF[3] + quatF[1]*quatF[0];
rotMatF[2][2] = quatF[3]*quatF[3] + quatF[0]*quatF[0] - 0.5f;
// Get the orientation of the phone from the rotation matrix
//
// There is some discussion of this at
// http://stackoverflow.com/questions/30279065/how-to-get-the-euler-angles-from-the-rotation-vector-sensor-type-rotation-vecto
// in particular equation 451.
//
final float rad2deg = (float)(180.0 / PI);
orientation[0] = (float)Math.atan2(-rotMatF[1][0], rotMatF[0][0]) * rad2deg;
orientation[1] = (float)Math.atan2(-rotMatF[2][1], rotMatF[2][2]) * rad2deg;
orientation[2] = (float)Math.asin ( rotMatF[2][0]) * rad2deg;
if (orientation[0] < 0) orientation[0] += 360;
}
This seems to give data very similar in feel (I haven't run numeric tests) to the old TYPE_ORIENTATION data: it was usable for motion control of the device with marginal filtering.
There is also helpful information here, and a possible alternative solution here.

Here's what worked out for me using SensorManager.SENSOR_DELAY_GAME for a fast update i.e.
#Override
protected void onResume() {
super.onResume();
sensor_manager.registerListener(this, sensor_manager.getDefaultSensor(Sensor.TYPE_ACCELEROMETER), SensorManager.SENSOR_DELAY_GAME);
sensor_manager.registerListener(this, sensor_manager.getDefaultSensor(Sensor.TYPE_MAGNETIC_FIELD), SensorManager.SENSOR_DELAY_GAME);
}
MOVING AVERAGE
(less efficient)
private float[] gravity;
private float[] geomagnetic;
private float azimuth;
private float pitch;
private float roll;
#Override
public void onSensorChanged(SensorEvent event) {
if (event.sensor.getType() == Sensor.TYPE_ACCELEROMETER)
gravity = moving_average_gravity(event.values);
if (event.sensor.getType() == Sensor.TYPE_MAGNETIC_FIELD)
geomagnetic = moving_average_geomagnetic(event.values);
if (gravity != null && geomagnetic != null) {
float R[] = new float[9];
float I[] = new float[9];
boolean success = SensorManager.getRotationMatrix(R, I, gravity, geomagnetic);
if (success) {
float orientation[] = new float[3];
SensorManager.getOrientation(R, orientation);
azimuth = (float) Math.toDegrees(orientation[0]);
pitch = (float) Math.toDegrees(orientation[1]);
roll = (float) Math.toDegrees(orientation[2]);
//if(roll>-46F && roll<46F)view.setTranslationX((roll/45F)*max_translation); //tilt from -45° to 45° to x-translate a view positioned centrally in a layout, from 0 - max_translation
Log.i("TAG","azimuth: "+azimuth+" | pitch: "+pitch+" | roll: "+roll);
}
}
}
private ArrayList<Float[]> moving_gravity;
private ArrayList<Float[]> moving_geomagnetic;
private static final float moving_average_size=12;//change
private float[] moving_average_gravity(float[] gravity) {
if(moving_gravity ==null){
moving_gravity =new ArrayList<>();
for (int i = 0; i < moving_average_size; i++) {
moving_gravity.add(new Float[]{0F,0F,0F});
}return new float[]{0F,0F,0F};
}
moving_gravity.remove(0);
moving_gravity.add(new Float[]{gravity[0],gravity[1],gravity[2]});
return moving_average(moving_gravity);
}
private float[] moving_average_geomagnetic(float[] geomagnetic) {
if(moving_geomagnetic ==null){
this.moving_geomagnetic =new ArrayList<>();
for (int i = 0; i < moving_average_size; i++) {
moving_geomagnetic.add(new Float[]{0F,0F,0F});
}return new float[]{0F,0F,0F};
}
moving_geomagnetic.remove(0);
moving_geomagnetic.add(new Float[]{geomagnetic[0],geomagnetic[1],geomagnetic[2]});
return moving_average(moving_geomagnetic);
}
private float[] moving_average(ArrayList<Float[]> moving_values){
float[] moving_average =new float[]{0F,0F,0F};
for (int i = 0; i < moving_average_size; i++) {
moving_average[0]+= moving_values.get(i)[0];
moving_average[1]+= moving_values.get(i)[1];
moving_average[2]+= moving_values.get(i)[2];
}
moving_average[0]= moving_average[0]/moving_average_size;
moving_average[1]= moving_average[1]/moving_average_size;
moving_average[2]= moving_average[2]/moving_average_size;
return moving_average;
}
LOW PASS FILTER
(more efficient)
private float[] gravity;
private float[] geomagnetic;
private float azimuth;
private float pitch;
private float roll;
#Override
public void onSensorChanged(SensorEvent event) {
if (event.sensor.getType() == Sensor.TYPE_ACCELEROMETER)
gravity = LPF(event.values.clone(), gravity);
if (event.sensor.getType() == Sensor.TYPE_MAGNETIC_FIELD)
geomagnetic = LPF(event.values.clone(), geomagnetic);
if (gravity != null && geomagnetic != null) {
float R[] = new float[9];
float I[] = new float[9];
boolean success = SensorManager.getRotationMatrix(R, I, gravity, geomagnetic);
if (success) {
float orientation[] = new float[3];
SensorManager.getOrientation(R, orientation);
azimuth = (float) Math.toDegrees(orientation[0]);
pitch = (float) Math.toDegrees(orientation[1]);
roll = (float) Math.toDegrees(orientation[2]);
//if(roll>-46F && roll<46F)view.setTranslationX((roll/45F)*max_translation); //tilt from -45° to 45° to x-translate a view positioned centrally in a layout, from 0 - max_translation
Log.i("TAG","azimuth: "+azimuth+" | pitch: "+pitch+" | roll: "+roll);
}
}
}
private static final float ALPHA = 1/16F;//adjust sensitivity
private float[] LPF(float[] input, float[] output) {
if ( output == null ) return input;
for ( int i=0; i<input.length; i++ ) {
output[i] = output[i] + ALPHA * (input[i] - output[i]);
}return output;
}
N.B
moving average of 12 values instead as per here
low pass filter of ALPHA = 0.0625 instead as per here

Measure Distance Between Camera And Object in front of camera

As this question has been asked already, i didnt get any clear idea in it. So i am asking this question again.I am struggling with it for two days.
I want to calculate the distance between camera of my devices with that of the object that are infront of camera.
I tried to use sensor managers as it was suggested in the following link but i am getting wrong results in it.
I have used two sensors accelerometer and magnetic field.
I calculate the distance based on orientation of the device as suggested in the above link.
onSensorChanged Callback in Sensor listener
#Override
public void onSensorChanged(SensorEvent event) {
if (event.sensor.getType() == Sensor.TYPE_ACCELEROMETER)
gravity = event.values.clone();
if (event.sensor.getType() == Sensor.TYPE_MAGNETIC_FIELD)
geoMagnetic = event.values.clone();
if (gravity != null && geoMagnetic != null) {
float R[] = new float[9];
float I[] = new float[9];
boolean success = SensorManager.getRotationMatrix(R, I, gravity,
geoMagnetic);
if (success) {
float orientation[] = new float[3];
SensorManager.getOrientation(R, orientation);
azimuth = 57.29578F * orientation[0];
pitch = 57.29578F * orientation[1];
roll = 57.29578F * orientation[2];
}
}
}
Portrait:
double dist = Math.abs((float) (1.4f * Math.tan(roll * Math.PI / 180)));
Landscape:
double dist = Math.abs((float) (1.4f * Math.tan(pitch* Math.PI / 180)));
Please provide information about how to calculate the distance. Any ideas and suggestion are appreciable,
Thanks in advance.

Get orientation in degrees

I am trying to create a simple compass app (for learning purposes) and I am using the rotation matrix using the magnetic sensor and accelerometer. This is my code:
#Override
public void onSensorChanged(SensorEvent event) {
if (event.sensor.getType() == Sensor.TYPE_MAGNETIC_FIELD) {
geomagnetic[0] = event.values[0];
geomagnetic[1] = event.values[1];
geomagnetic[2] = event.values[2];
havemag = true;
}
if (event.sensor.getType() == Sensor.TYPE_ACCELEROMETER) {
gravity[0] = event.values[0];
gravity[1] = event.values[1];
gravity[2] = event.values[2];
haveacc = true;
}
if (haveacc && havemag) {
if (SensorManager.getRotationMatrix(Rm, I, gravity, geomagnetic)) {
float[] result = new float[3];
SensorManager.getOrientation(Rm, result);
azimuth = result[0];
pitch = result[1];
roll = result[2];
}
}
}
When I look at these values while moving my phone though (outputing using Log.d), I get incorrect values. I use this code to make degrees out of them:
int deg = (int)(azimuth * (float)57.295);
But this value never reaches zero (which is supposed to be on the exact opposite side of 180/-180). Instead, the opposite of where 180/-180 is, I get -85 (approx).

You should use the proper JavaMath function to convert your radian angle to degree. Furthermore converting the angle (in degree) to a [0 - 360] degree interval is very useful.
// angle in degree [-180 - 0 - 180] degree
azimuth = Math.toDegrees( result[0] )
// angle in degree [0 - 360] degree
azimuth = ( Math.toDegrees( result[0] ) + 360 ) % 360;

Android getOrientation Azimuth gets polluted when phone is tilted

I'm having a really annoying problem with a AR view acting like a compass. So when I hold the phone in portrait (so that the screen is pointing to my face), then I call the remapCoordinateSystem that the pitch is 0 when holding it portrait. Then the azimuth (compass functionality) is perfect, but as soon as I tilt the phone the azimuth gets ruined, if I bend forward the azimuth increases and if I bend backwards it decreases.
I use 2 sensors to get the readings, Sensor.TYPE_MAGNETIC_FIELD and Sensor.TYPE_GRAVITY.
I use a lowpassfilter which is pretty basic, it's implemented with an alpha constant and is used directly on the read values from the sensors.
Here is my code:
float[] rotationMatrix = new float[9];
SensorManager.getRotationMatrix(rotationMatrix, null, gravitymeterValues,
magnetometerValues);
float[] remappedRotationMatrix = new float[9];
SensorManager.remapCoordinateSystem(rotationMatrix, SensorManager.AXIS_X,
SensorManager.AXIS_Z, remappedRotationMatrix);
float results[] = new float[3];
SensorManager.getOrientation(remappedRotationMatrix, results);
float azimuth = (float) (results[0] * 180 / Math.PI);
if (azimuth < 0) {
azimuth += 360;
}
float pitch = (float) (results[1] * 180 / Math.PI);
float roll = (float) (results[2] * 180 / Math.PI);
As you see there is no magic here. I call this piece of code when the gravitymeterValues and the magnetometerValues are ready to be used.
My question is how do I stop the azimuth from going crazy when I tilt the phone?
I checked a free app on the Google Play Store, Compass and it hasn't solved this problem, but I hope there is a solution.
I have 2 solutions in mind:
Make the AR view only work in very constrainted pitch angles, right now I have something like pitch >= -5 && pitch <= 30. If this isn't fullfilled the user is shown a screen that asks him/her to rotate the phone to portrait.
Somehow use the pitch to suppress the azimuth, this seems like a pretty device-specific solution though, but of course I'm open for suggestions.
I can also add that I've been searching for a couple of hours for a decent solution and I haven't found any that has given me any better solutions than 2) here.
Thanks in advance!

For complete code see https://github.com/hoananguyen/dsensor
Keep a history and average out, I do not know the correct interpretation of pitch and roll so the following code is for azimuth only.
Class members
private List<float[]> mRotHist = new ArrayList<float[]>();
private int mRotHistIndex;
// Change the value so that the azimuth is stable and fit your requirement
private int mHistoryMaxLength = 40;
float[] mGravity;
float[] mMagnetic;
float[] mRotationMatrix = new float[9];
// the direction of the back camera, only valid if the device is tilted up by
// at least 25 degrees.
private float mFacing = Float.NAN;
public static final float TWENTY_FIVE_DEGREE_IN_RADIAN = 0.436332313f;
public static final float ONE_FIFTY_FIVE_DEGREE_IN_RADIAN = 2.7052603f;
onSensorChanged
#Override
public void onSensorChanged(SensorEvent event)
{
if (event.sensor.getType() == Sensor.TYPE_GRAVITY)
{
mGravity = event.values.clone();
}
else
{
mMagnetic = event.values.clone();
}
if (mGravity != null && mMagnetic != null)
{
if (SensorManager.getRotationMatrix(mRotationMatrix, null, mGravity, mMagnetic))
{
// inclination is the degree of tilt by the device independent of orientation (portrait or landscape)
// if less than 25 or more than 155 degrees the device is considered lying flat
float inclination = (float) Math.acos(mRotationMatrix[8]);
if (inclination < TWENTY_FIVE_DEGREE_IN_RADIAN
|| inclination > ONE_FIFTY_FIVE_DEGREE_IN_RADIAN)
{
// mFacing is undefined, so we need to clear the history
clearRotHist();
mFacing = Float.NaN;
}
else
{
setRotHist();
// mFacing = azimuth is in radian
mFacing = findFacing();
}
}
}
}
private void clearRotHist()
{
if (DEBUG) {Log.d(TAG, "clearRotHist()");}
mRotHist.clear();
mRotHistIndex = 0;
}
private void setRotHist()
{
if (DEBUG) {Log.d(TAG, "setRotHist()");}
float[] hist = mRotationMatrix.clone();
if (mRotHist.size() == mHistoryMaxLength)
{
mRotHist.remove(mRotHistIndex);
}
mRotHist.add(mRotHistIndex++, hist);
mRotHistIndex %= mHistoryMaxLength;
}
private float findFacing()
{
if (DEBUG) {Log.d(TAG, "findFacing()");}
float[] averageRotHist = average(mRotHist);
return (float) Math.atan2(-averageRotHist[2], -averageRotHist[5]);
}
public float[] average(List<float[]> values)
{
float[] result = new float[9];
for (float[] value : values)
{
for (int i = 0; i < 9; i++)
{
result[i] += value[i];
}
}
for (int i = 0; i < 9; i++)
{
result[i] = result[i] / values.size();
}
return result;
}