-
Notifications
You must be signed in to change notification settings - Fork 34
/
HeadTracker.java
119 lines (104 loc) · 3.61 KB
/
HeadTracker.java
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
package com.google.vrtoolkit.cardboard.sensors;
import android.content.Context;
import android.hardware.Sensor;
import android.hardware.SensorEvent;
import android.hardware.SensorEventListener;
import android.hardware.SensorManager;
import android.opengl.Matrix;
import android.os.Handler;
import android.os.Looper;
import com.google.vrtoolkit.cardboard.sensors.internal.OrientationEKF;
public class HeadTracker {
private static final String TAG = "HeadTracker";
private static final double NS2S = 1.0E-9D;
private static final int[] INPUT_SENSORS = { 1, 4 };
private final Context mContext;
private final float[] mEkfToHeadTracker = new float[16];
private final float[] mTmpHeadView = new float[16];
private final float[] mTmpRotatedEvent = new float[3];
private Looper mSensorLooper;
private SensorEventListener mSensorEventListener;
private volatile boolean mTracking;
private final OrientationEKF mTracker = new OrientationEKF();
private long mLastGyroEventTimeNanos;
public HeadTracker(Context context) {
this.mContext = context;
Matrix.setRotateEulerM(this.mEkfToHeadTracker, 0, -90.0F, 0.0F, 0.0F);
}
public void startTracking() {
if (this.mTracking) {
return;
}
this.mTracker.reset();
this.mSensorEventListener = new SensorEventListener() {
public void onSensorChanged(SensorEvent event) {
HeadTracker.this.processSensorEvent(event);
}
public void onAccuracyChanged(Sensor sensor, int accuracy) {
}
};
Thread sensorThread = new Thread(new Runnable() {
public void run() {
Looper.prepare();
HeadTracker.this.mSensorLooper = Looper.myLooper();
Handler handler = new Handler();
SensorManager sensorManager = (SensorManager) HeadTracker.this.mContext
.getSystemService("sensor");
for (int sensorType : HeadTracker.INPUT_SENSORS) {
Sensor sensor = sensorManager.getDefaultSensor(sensorType);
sensorManager.registerListener(
HeadTracker.this.mSensorEventListener, sensor, 0,
handler);
}
Looper.loop();
}
});
sensorThread.start();
this.mTracking = true;
}
public void stopTracking() {
if (!this.mTracking) {
return;
}
SensorManager sensorManager = (SensorManager) this.mContext
.getSystemService("sensor");
sensorManager.unregisterListener(this.mSensorEventListener);
this.mSensorEventListener = null;
this.mSensorLooper.quit();
this.mSensorLooper = null;
this.mTracking = false;
}
public void getLastHeadView(float[] headView, int offset) {
if (offset + 16 > headView.length) {
throw new IllegalArgumentException(
"Not enough space to write the result");
}
synchronized (this.mTracker) {
double secondsSinceLastGyroEvent = (System.nanoTime() - this.mLastGyroEventTimeNanos) * 1.0E-9D;
double secondsToPredictForward = secondsSinceLastGyroEvent + 0.03333333333333333D;
double[] mat = this.mTracker
.getPredictedGLMatrix(secondsToPredictForward);
for (int i = 0; i < headView.length; i++) {
this.mTmpHeadView[i] = ((float) mat[i]);
}
}
Matrix.multiplyMM(headView, offset, this.mTmpHeadView, 0,
this.mEkfToHeadTracker, 0);
}
private void processSensorEvent(SensorEvent event) {
long timeNanos = System.nanoTime();
this.mTmpRotatedEvent[0] = (-event.values[1]);
this.mTmpRotatedEvent[1] = event.values[0];
this.mTmpRotatedEvent[2] = event.values[2];
synchronized (this.mTracker) {
if (event.sensor.getType() == 1) {
this.mTracker
.processAcc(this.mTmpRotatedEvent, event.timestamp);
} else if (event.sensor.getType() == 4) {
this.mLastGyroEventTimeNanos = timeNanos;
this.mTracker.processGyro(this.mTmpRotatedEvent,
event.timestamp);
}
}
}
}