sim.android.hardware.SensorManager Maven / Gradle / Ivy
The newest version!
package sim.android.hardware;
/*
* Copyright (C) 2008 The Android Open Source Project
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*/
import android.hardware.Sensor;
import android.os.Handler;
import android.os.Looper;
import android.os.Message;
import android.util.Log;
import java.lang.reflect.Constructor;
import java.lang.reflect.Field;
import java.lang.reflect.InvocationTargetException;
import java.lang.reflect.Method;
import java.util.ArrayList;
import java.util.List;
import sim.android.hardware.service.impl.SensorEventService;
public class SensorManager {
private static final String TAG = "SensorManager";
private static final float[] mTempMatrix = new float[16];
@Deprecated
public static final int SENSOR_ORIENTATION = 1 << 0;
@Deprecated
public static final int SENSOR_ACCELEROMETER = 1 << 1;
@Deprecated
public static final int SENSOR_TEMPERATURE = 1 << 2;
@Deprecated
public static final int SENSOR_MAGNETIC_FIELD = 1 << 3;
@Deprecated
public static final int SENSOR_LIGHT = 1 << 4;
@Deprecated
public static final int SENSOR_PROXIMITY = 1 << 5;
@Deprecated
public static final int SENSOR_TRICORDER = 1 << 6;
@Deprecated
public static final int SENSOR_ORIENTATION_RAW = 1 << 7;
@Deprecated
public static final int SENSOR_ALL = 0x7F;
@Deprecated
public static final int SENSOR_MIN = SENSOR_ORIENTATION;
/**
* Largest sensor ID
*
* @deprecated use {@link android.hardware.Sensor Sensor} instead.
*/
@Deprecated
public static final int SENSOR_MAX = ((SENSOR_ALL + 1) >> 1);
/**
* Index of the X value in the array returned by
* {@link android.hardware.SensorListener#onSensorChanged}
*
* @deprecated use {@link android.hardware.Sensor Sensor} instead.
*/
@Deprecated
public static final int DATA_X = 0;
/**
* Index of the Y value in the array returned by
* {@link android.hardware.SensorListener#onSensorChanged}
*
* @deprecated use {@link android.hardware.Sensor Sensor} instead.
*/
@Deprecated
public static final int DATA_Y = 1;
@Deprecated
public static final int DATA_Z = 2;
@Deprecated
public static final int RAW_DATA_INDEX = 3;
@Deprecated
public static final int RAW_DATA_X = 3;
@Deprecated
public static final int RAW_DATA_Y = 4;
@Deprecated
public static final int RAW_DATA_Z = 5;
/**
* Standard gravity (g) on Earth. This value is equivalent to 1G
*/
public static final float STANDARD_GRAVITY = 9.80665f;
/**
* Sun's gravity in SI units (m/s^2)
*/
public static final float GRAVITY_SUN = 275.0f;
/**
* Mercury's gravity in SI units (m/s^2)
*/
public static final float GRAVITY_MERCURY = 3.70f;
/**
* Venus' gravity in SI units (m/s^2)
*/
public static final float GRAVITY_VENUS = 8.87f;
/**
* Earth's gravity in SI units (m/s^2)
*/
public static final float GRAVITY_EARTH = 9.80665f;
/**
* The Moon's gravity in SI units (m/s^2)
*/
public static final float GRAVITY_MOON = 1.6f;
/**
* Mars' gravity in SI units (m/s^2)
*/
public static final float GRAVITY_MARS = 3.71f;
/**
* Jupiter's gravity in SI units (m/s^2)
*/
public static final float GRAVITY_JUPITER = 23.12f;
/**
* Saturn's gravity in SI units (m/s^2)
*/
public static final float GRAVITY_SATURN = 8.96f;
/**
* Uranus' gravity in SI units (m/s^2)
*/
public static final float GRAVITY_URANUS = 8.69f;
/**
* Neptune's gravity in SI units (m/s^2)
*/
public static final float GRAVITY_NEPTUNE = 11.0f;
/**
* Pluto's gravity in SI units (m/s^2)
*/
public static final float GRAVITY_PLUTO = 0.6f;
/**
* Gravity (estimate) on the first Death Star in Empire units (m/s^2)
*/
public static final float GRAVITY_DEATH_STAR_I = 0.000000353036145f;
/**
* Gravity on the island
*/
public static final float GRAVITY_THE_ISLAND = 4.815162342f;
/**
* Maximum magnetic field on Earth's surface
*/
public static final float MAGNETIC_FIELD_EARTH_MAX = 60.0f;
/**
* Minimum magnetic field on Earth's surface
*/
public static final float MAGNETIC_FIELD_EARTH_MIN = 30.0f;
/**
* Standard atmosphere, or average sea-level pressure in hPa (millibar)
*/
public static final float PRESSURE_STANDARD_ATMOSPHERE = 1013.25f;
/**
* Maximum luminance of sunlight in lux
*/
public static final float LIGHT_SUNLIGHT_MAX = 120000.0f;
/**
* luminance of sunlight in lux
*/
public static final float LIGHT_SUNLIGHT = 110000.0f;
/**
* luminance in shade in lux
*/
public static final float LIGHT_SHADE = 20000.0f;
/**
* luminance under an overcast sky in lux
*/
public static final float LIGHT_OVERCAST = 10000.0f;
/**
* luminance at sunrise in lux
*/
public static final float LIGHT_SUNRISE = 400.0f;
/**
* luminance under a cloudy sky in lux
*/
public static final float LIGHT_CLOUDY = 100.0f;
/**
* luminance at night with full moon in lux
*/
public static final float LIGHT_FULLMOON = 0.25f;
/**
* luminance at night with no moon in lux
*/
public static final float LIGHT_NO_MOON = 0.001f;
/**
* get sensor data as fast as possible
*/
public static final int SENSOR_DELAY_FASTEST = 0;
/**
* rate suitable for games
*/
public static final int SENSOR_DELAY_GAME = 1;
/**
* rate suitable for the user interface
*/
public static final int SENSOR_DELAY_UI = 2;
/**
* rate (default) suitable for screen orientation changes
*/
public static final int SENSOR_DELAY_NORMAL = 3;
/**
* The values returned by this sensor cannot be trusted, calibration is
* needed or the environment doesn't allow readings
*/
public static final int SENSOR_STATUS_UNRELIABLE = 0;
/**
* This sensor is reporting data with low accuracy, calibration with the
* environment is needed
*/
public static final int SENSOR_STATUS_ACCURACY_LOW = 1;
public static final int SENSOR_STATUS_ACCURACY_MEDIUM = 2;
public static final int SENSOR_STATUS_ACCURACY_HIGH = 3;
public static final int AXIS_X = 1;
public static final int AXIS_Y = 2;
public static final int AXIS_Z = 3;
public static final int AXIS_MINUS_X = AXIS_X | 0x80;
public static final int AXIS_MINUS_Y = AXIS_Y | 0x80;
public static final int AXIS_MINUS_Z = AXIS_Z | 0x80;
/*-----------------------------------------------------------------------*/
private static boolean sSensorModuleInitialized = false;
private SensorEventService sensorEventService;
private EventHandler mEventHandler;
// Used within this module from outside SensorManager, don't make private
static ArrayList mSensors = new ArrayList();
static final ArrayList mListeners = new ArrayList();
class FullListener {
android.hardware.SensorEventListener sensorEventListener;
int rate;
Handler handler;
android.hardware.Sensor sensor;
}
/**
*
*/
public SensorManager() {
if (!sSensorModuleInitialized) {
Looper looper;
if ((looper = Looper.myLooper()) != null) {
mEventHandler = new EventHandler(looper);
} else if ((looper = Looper.getMainLooper()) != null) {
mEventHandler = new EventHandler(looper);
} else {
mEventHandler = null;
}
sSensorModuleInitialized = true;
// initialize the sensor list
mSensors = new ArrayList();
addAccelerometerSensor();
addMagneticFieldSensor();
addOrientationSensor();
sensorEventService = new SensorEventService(this, mEventHandler);
sensorEventService.start();
}
}
public List getListeners() {
return mListeners;
}
private void addAccelerometerSensor() {
synchronized (mSensors) {
mSensors.add(createSensor(Sensor.TYPE_ACCELEROMETER));
}
}
private android.hardware.Sensor createSensor(int type){
try {
Constructor sensorCons = android.hardware.Sensor.class.getDeclaredConstructors()[0];
sensorCons.setAccessible(true);
android.hardware.Sensor sensor =(android.hardware.Sensor) sensorCons.newInstance(new Object[]{});
Field mtype= android.hardware.Sensor.class.getDeclaredField("mType");
mtype.setAccessible(true);
mtype.set(sensor, type);
return sensor;
} catch (IllegalArgumentException e) {
// TODO Auto-generated catch block
e.printStackTrace();
} catch (IllegalAccessException e) {
// TODO Auto-generated catch block
e.printStackTrace();
} catch (InstantiationException e) {
// TODO Auto-generated catch block
e.printStackTrace();
} catch (InvocationTargetException e) {
// TODO Auto-generated catch block
e.printStackTrace();
} catch (NoSuchFieldException e) {
// TODO Auto-generated catch block
e.printStackTrace();
} catch (SecurityException e) {
// TODO Auto-generated catch block
e.printStackTrace();
}
return null;
}
private void addMagneticFieldSensor() {
synchronized (mSensors) {
mSensors.add(createSensor(Sensor.TYPE_MAGNETIC_FIELD));
}
}
private void addOrientationSensor() {
android.hardware.Sensor sensor = createSensor(Sensor.TYPE_ORIENTATION);
Method setRange;
try {
setRange = android.hardware.Sensor.class.getDeclaredMethod("setRange",Float.TYPE,Float.TYPE);
setRange.setAccessible(true);
setRange.invoke(sensor, 360.0f,1.0f);
} catch (NoSuchMethodException e) {
// TODO Auto-generated catch block
e.printStackTrace();
} catch (SecurityException e) {
// TODO Auto-generated catch block
e.printStackTrace();
} catch (IllegalAccessException e) {
// TODO Auto-generated catch block
e.printStackTrace();
} catch (IllegalArgumentException e) {
// TODO Auto-generated catch block
e.printStackTrace();
} catch (InvocationTargetException e) {
// TODO Auto-generated catch block
e.printStackTrace();
}
synchronized (mSensors) {
mSensors.add(sensor);
}
}
public class EventHandler extends Handler {
public static final int NEW_VALUE = 0;
public EventHandler(Looper looper) {
super(looper);
}
@Override
public void handleMessage(Message msg) {
switch (msg.what) {
case NEW_VALUE:
notifySensorChangedToListener((android.hardware.SensorEvent) msg.obj);
return;
default:
Log.e(TAG, "Unknown message type " + msg.what);
return;
}
}
}
public void notifySensorChangedToListener(android.hardware.SensorEvent se) {
System.out.println("notifySensorChangedToListener!!");
synchronized (mListeners) {
for (FullListener fl : mListeners) {
if (fl.sensor.getType() == se.sensor.getType()) {
fl.sensorEventListener.onSensorChanged(se);
}
}
}
}
@Deprecated
public int getSensors() {
synchronized (mSensors) {
int result = 0;
final ArrayList fullList = mSensors;
for (android.hardware.Sensor i : fullList) {
switch (i.getType()) {
case Sensor.TYPE_ACCELEROMETER:
result |= SensorManager.SENSOR_ACCELEROMETER;
break;
case Sensor.TYPE_MAGNETIC_FIELD:
result |= SensorManager.SENSOR_MAGNETIC_FIELD;
break;
case Sensor.TYPE_ORIENTATION:
result |= SensorManager.SENSOR_ORIENTATION
| SensorManager.SENSOR_ORIENTATION_RAW;
break;
}
}
return result;
}
}
public List getSensorList(int type) {
// cache the returned lists the first time
List list = new ArrayList();
synchronized (mSensors) {
if (type == Sensor.TYPE_ALL) {
list.addAll(mSensors);
} else {
for (android.hardware.Sensor s : mSensors) {
if (s.getType() == type) {
list.add(s);
}
}
}
}
return list;
}
public android.hardware.Sensor getDefaultSensor(int type) {
// TODO: need to be smarter, for now, just return the 1st sensor
List l = getSensorList(type);
return l.isEmpty() ? null : l.get(0);
}
@Deprecated
public boolean registerListener(android.hardware.SensorListener listener, int sensors) {
return registerListener(listener, sensors, SENSOR_DELAY_NORMAL);
}
@Deprecated
public boolean registerListener(android.hardware.SensorListener listener, int sensors,
int rate) {
if (listener == null) {
return false;
}
boolean result = false;
result = registerLegacyListener(SENSOR_ACCELEROMETER,
Sensor.TYPE_ACCELEROMETER, listener, sensors, rate) || result;
result = registerLegacyListener(SENSOR_MAGNETIC_FIELD,
Sensor.TYPE_MAGNETIC_FIELD, listener, sensors, rate) || result;
result = registerLegacyListener(SENSOR_ORIENTATION_RAW,
Sensor.TYPE_ORIENTATION, listener, sensors, rate) || result;
result = registerLegacyListener(SENSOR_ORIENTATION,
Sensor.TYPE_ORIENTATION, listener, sensors, rate) || result;
result = registerLegacyListener(SENSOR_TEMPERATURE,
Sensor.TYPE_TEMPERATURE, listener, sensors, rate) || result;
return result;
}
@SuppressWarnings("deprecation")
private boolean registerLegacyListener(int legacyType, int type,
android.hardware.SensorListener listener, int sensors, int rate) {
return true;
}
@Deprecated
public void unregisterListener(android.hardware.SensorListener listener, int sensors) {
unregisterLegacyListener(SENSOR_ACCELEROMETER,
Sensor.TYPE_ACCELEROMETER, listener, sensors);
unregisterLegacyListener(SENSOR_MAGNETIC_FIELD,
Sensor.TYPE_MAGNETIC_FIELD, listener, sensors);
unregisterLegacyListener(SENSOR_ORIENTATION_RAW,
Sensor.TYPE_ORIENTATION, listener, sensors);
unregisterLegacyListener(SENSOR_ORIENTATION, Sensor.TYPE_ORIENTATION,
listener, sensors);
unregisterLegacyListener(SENSOR_TEMPERATURE, Sensor.TYPE_TEMPERATURE,
listener, sensors);
}
@SuppressWarnings("deprecation")
private void unregisterLegacyListener(int legacyType, int type,
android.hardware.SensorListener listener, int sensors) {
}
@Deprecated
public void unregisterListener(android.hardware.SensorListener listener) {
unregisterListener(listener, SENSOR_ALL | SENSOR_ORIENTATION_RAW);
}
public void unregisterListener(android.hardware.SensorEventListener listener, android.hardware.Sensor sensor) {
unregisterListener((Object) listener, sensor);
}
public void unregisterListener(android.hardware.SensorEventListener listener) {
unregisterListener((Object) listener);
}
public boolean registerListener(android.hardware.SensorEventListener listener,
android.hardware.Sensor sensor, int rate) {
return registerListener(listener, sensor, rate, null);
}
public boolean registerListener(android.hardware.SensorEventListener listener,
android.hardware.Sensor sensor, int rate, Handler handler) {
if (listener == null || sensor == null) {
return false;
}
boolean result = true;
int delay = -1;
switch (rate) {
case SENSOR_DELAY_FASTEST:
delay = 0;
break;
case SENSOR_DELAY_GAME:
delay = 20000;
break;
case SENSOR_DELAY_UI:
delay = 60000;
break;
case SENSOR_DELAY_NORMAL:
delay = 200000;
break;
default:
delay = rate;
break;
}
synchronized (mListeners) {
for (FullListener fl : mListeners) {
if (fl.sensorEventListener == listener && fl.sensor == sensor) {
return true;
}
}
FullListener fl = new FullListener();
fl.handler = handler;
fl.rate = delay;
fl.sensorEventListener = listener;
fl.sensor = sensor;
mListeners.add(fl);
result = true;
}
return result;
}
private void unregisterListener(Object listener, android.hardware.Sensor sensor) {
if (listener == null || sensor == null) {
return;
}
synchronized (mListeners) {
final int size = mListeners.size();
for (int i = 0; i < size; i++) {
FullListener fl = mListeners.get(i);
if (fl.sensorEventListener == listener && fl.sensor == sensor) {
mListeners.remove(i);
break;
}
}
}
}
private void unregisterListener(Object listener) {
if (listener == null) {
return;
}
synchronized (mListeners) {
int size = mListeners.size();
for (int i = 0; i < size; i++) {
FullListener fl = mListeners.get(i);
if (fl.sensorEventListener == listener) {
mListeners.remove(i);
size = mListeners.size();
i = 0;
}
}
if(mListeners.isEmpty()) {
sensorEventService.stop();
sSensorModuleInitialized = false;
}
}
}
public static boolean getRotationMatrix(float[] R, float[] I,
float[] gravity, float[] geomagnetic) {
// TODO: move this to native code for efficiency
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) {
// device is close to free fall (or in space?), or close to
// magnetic north pole. Typical values are > 100.
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) {
// compute the inclination matrix by projecting the geomagnetic
// vector onto the Z (gravity) and X (horizontal component
// of geomagnetic vector) axes.
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;
}
public static float getInclination(float[] I) {
if (I.length == 9) {
return (float) Math.atan2(I[5], I[4]);
} else {
return (float) Math.atan2(I[6], I[5]);
}
}
public static boolean remapCoordinateSystem(float[] inR, int X, int Y,
float[] outR) {
if (inR == outR) {
final float[] temp = mTempMatrix;
synchronized (temp) {
// we don't expect to have a lot of contention
if (remapCoordinateSystemImpl(inR, X, Y, temp)) {
final int size = outR.length;
for (int i = 0; i < size; i++) {
outR[i] = temp[i];
}
return true;
}
}
}
return remapCoordinateSystemImpl(inR, X, Y, outR);
}
private static boolean remapCoordinateSystemImpl(float[] inR, int X, int Y,
float[] outR) {
final int length = outR.length;
if (inR.length != length) {
return false; // invalid parameter
}
if ((X & 0x7C) != 0 || (Y & 0x7C) != 0) {
return false; // invalid parameter
}
if (((X & 0x3) == 0) || ((Y & 0x3) == 0)) {
return false; // no axis specified
}
if ((X & 0x3) == (Y & 0x3)) {
return false; // same axis specified
}
// Z is "the other" axis, its sign is either +/- sign(X)*sign(Y)
// this can be calculated by exclusive-or'ing X and Y; except for
// the sign inversion (+/-) which is calculated below.
int Z = X ^ Y;
// extract the axis (remove the sign), offset in the range 0 to 2.
final int x = (X & 0x3) - 1;
final int y = (Y & 0x3) - 1;
final int z = (Z & 0x3) - 1;
// compute the sign of Z (whether it needs to be inverted)
final int axis_y = (z + 1) % 3;
final int axis_z = (z + 2) % 3;
if (((x ^ axis_y) | (y ^ axis_z)) != 0) {
Z ^= 0x80;
}
final boolean sx = (X >= 0x80);
final boolean sy = (Y >= 0x80);
final boolean sz = (Z >= 0x80);
// Perform R * r, in avoiding actual muls and adds.
final int rowLength = ((length == 16) ? 4 : 3);
for (int j = 0; j < 3; j++) {
final int offset = j * rowLength;
for (int i = 0; i < 3; i++) {
if (x == i) {
outR[offset + i] = sx ? -inR[offset + 0] : inR[offset + 0];
}
if (y == i) {
outR[offset + i] = sy ? -inR[offset + 1] : inR[offset + 1];
}
if (z == i) {
outR[offset + i] = sz ? -inR[offset + 2] : inR[offset + 2];
}
}
}
if (length == 16) {
outR[3] = outR[7] = outR[11] = outR[12] = outR[13] = outR[14] = 0;
outR[15] = 1;
}
return true;
}
public static float[] getOrientation(float[] R, float values[]) {
/*
* 4x4 (length=16) case: / 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 /
*
* 3x3 (length=9) case: / R[ 0] R[ 1] R[ 2] \ | R[ 3] R[ 4] R[ 5] | \ R[
* 6] R[ 7] R[ 8] /
*/
if (R.length == 9) {
values[0] = (float) Math.atan2(R[1], R[4]);
values[1] = (float) Math.asin(-R[7]);
values[2] = (float) Math.atan2(-R[6], R[8]);
} else {
values[0] = (float) Math.atan2(R[1], R[5]);
values[1] = (float) Math.asin(-R[9]);
values[2] = (float) Math.atan2(-R[8], R[10]);
}
return values;
}
public static float getAltitude(float p0, float p) {
final float coef = 1.0f / 5.255f;
return 44330.0f * (1.0f - (float) Math.pow(p / p0, coef));
}
class LmsFilter {
private static final int SENSORS_RATE_MS = 20;
private static final int COUNT = 12;
private static final float PREDICTION_RATIO = 1.0f / 3.0f;
private static final float PREDICTION_TIME = (SENSORS_RATE_MS * COUNT / 1000.0f)
* PREDICTION_RATIO;
private float mV[] = new float[COUNT * 2];
private float mT[] = new float[COUNT * 2];
private int mIndex;
public LmsFilter() {
mIndex = COUNT;
}
public float filter(long time, float in) {
float v = in;
final float ns = 1.0f / 1000000000.0f;
final float t = time * ns;
float v1 = mV[mIndex];
if ((v - v1) > 180) {
v -= 360;
} else if ((v1 - v) > 180) {
v += 360;
}
/*
* Manage the circular buffer, we write the data twice spaced by
* COUNT values, so that we don't have to copy the array when it's
* full
*/
mIndex++;
if (mIndex >= COUNT * 2) {
mIndex = COUNT;
}
mV[mIndex] = v;
mT[mIndex] = t;
mV[mIndex - COUNT] = v;
mT[mIndex - COUNT] = t;
float A, B, C, D, E;
float a, b;
int i;
A = B = C = D = E = 0;
for (i = 0; i < COUNT - 1; i++) {
final int j = mIndex - 1 - i;
final float Z = mV[j];
final float T = 0.5f * (mT[j] + mT[j + 1]) - t;
float dT = mT[j] - mT[j + 1];
dT *= dT;
A += Z * dT;
B += T * (T * dT);
C += (T * dT);
D += Z * (T * dT);
E += dT;
}
b = (A * B + C * D) / (E * B + C * C);
a = (E * b - A) / C;
float f = b + PREDICTION_TIME * a;
// Normalize
f *= (1.0f / 360.0f);
if (((f >= 0) ? f : -f) >= 0.5f) {
f = f - (float) Math.ceil(f + 0.5f) + 1.0f;
}
if (f < 0) {
f += 1.0f;
}
f *= 360.0f;
return f;
}
}
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 = prevR[0];
pri1 = prevR[1];
pri2 = prevR[2];
pri3 = prevR[3];
pri4 = prevR[4];
pri5 = prevR[5];
pri6 = prevR[6];
pri7 = prevR[7];
pri8 = prevR[8];
} else if (prevR.length == 16) {
pri0 = prevR[0];
pri1 = prevR[1];
pri2 = prevR[2];
pri3 = prevR[4];
pri4 = prevR[5];
pri5 = prevR[6];
pri6 = prevR[8];
pri7 = prevR[9];
pri8 = prevR[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);
}
public static void getRotationMatrixFromVector(float[] R,
float[] rotationVector) {
float q0;
float q1 = rotationVector[0];
float q2 = rotationVector[1];
float q3 = rotationVector[2];
if (rotationVector.length == 4) {
q0 = rotationVector[3];
} else {
q0 = 1 - q1 * q1 - q2 * q2 - q3 * q3;
q0 = (q0 > 0) ? (float) Math.sqrt(q0) : 0;
}
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;
}
}
public static void getQuaternionFromVector(float[] Q, float[] rv) {
if (rv.length == 4) {
Q[0] = rv[3];
} else {
Q[0] = 1 - rv[0] * rv[0] - rv[1] * rv[1] - rv[2] * rv[2];
Q[0] = (Q[0] > 0) ? (float) Math.sqrt(Q[0]) : 0;
}
Q[1] = rv[0];
Q[2] = rv[1];
Q[3] = rv[2];
}
}