private void processGyroscopeEvent(SensorEvent event) {
float[] values = event.values;
long timestamp = event.timestamp;
if (mLastGyroTimeStamp == 0) {
mLastGyroTimeStamp = timestamp; // throw away the first gyro event
System.arraycopy(values, 0, lastValues, 0, 3);
return;
}
double deltaT = (double) (timestamp - mLastGyroTimeStamp) / NANO;
// Log.d(TAG, "values = (" + values[0] + ", " + values[1] + ", "
// + values[2] + "), deltaT = " + deltaT);
// This timestep's delta rotation to be multiplied by the current
// rotation after computing it from the gyro sample data.
if (timestamp != 0) {
double deltaRotationVector[] = new double[3];
final double TO_RADIANS = Math.PI / 180;
deltaRotationVector[0] = -(values[0] + lastValues[0]) / 2 * deltaT * TO_RADIANS;
deltaRotationVector[1] = -(values[1] + lastValues[1]) / 2 * deltaT * TO_RADIANS;
deltaRotationVector[2] = -(values[2] + lastValues[2]) / 2 * deltaT * TO_RADIANS;
double[] rotationMatrix = Transform.getRotationMatrixSORA(deltaRotationVector);
// double x = -(values[0]) * deltaT * SLOW_DOWN;
// double y = (values[1]) * deltaT * SLOW_DOWN;
// double z = (values[2]) * deltaT * SLOW_DOWN;
// double[] rotationMatrix2 = Transform.getRotationMatrixSORA(x, y, z);
// System.out.println("rotationMatrix:\n" + rotationMatrix[0]+" " +
// rotationMatrix[1] + " " + rotationMatrix[2] + "\n"
// + rotationMatrix[3] + " " + rotationMatrix[4] + " " +
// rotationMatrix[5]+ "\n"
// + rotationMatrix[6] + " " + rotationMatrix[7] + " " +
// rotationMatrix[8]);
worldBase.transform(new Matrix(3, 3, rotationMatrix));
// mCube.rotatePoints(rotationMatrix);
// mCube2.rotatePoints(rotationMatrix2);
// mCube3.rotatePoints(rotationMatrix3);
// rotatePoints(deltaRotationVector[0], deltaRotationVector[1],
// deltaRotationVector[2]);
}
mLastGyroTimeStamp = timestamp;
System.arraycopy(values, 0, lastValues, 0, 3);
}
private void processAccelerometerEvent(SensorEvent event) {
for (int i = 0; i < 3; i++) {
mAccMovingAverage[i].push(event.timestamp, event.values[i]);
}
long timestamp = event.timestamp;
if (mLastAccTimeStamp == 0) {
mLastAccTimeStamp = timestamp; // throw away the first event if
// timestamp not set
return;
}
boolean realign = realignCondition();
long now = System.nanoTime();
if (realign && now - lastRealignTimestamp > NANO) {
lastRealignTimestamp = now;
mTouched = false;
// worldBase.realign(new Matrix(1, 3, new double[] {} ), 3);
Vector3d axisZ = worldBase.getAxis(2); // z axis
Vector3d acc =
new Vector3d(
mAccMovingAverage[0].getAverage(),
mAccMovingAverage[1].getAverage(),
mAccMovingAverage[2].getAverage());
Vector3d rotationVector = axisZ.normalize().crossProduct(acc.normalize());
double[] rotationMatrix = Transform.getRotationMatrixSORA(rotationVector.getValues());
worldBase.transform(new Matrix(3, 3, rotationMatrix));
mAccModel.reset();
}
// double deltaT = (double) (timestamp - mLastAccTimeStamp) / NANO;
// Log.d(TAG, "values = (" + values[0] + ", " + values[1] + ", " +
// values[2] + "), deltaT = " + deltaT);
if (timestamp != 0) {
Matrix accelerationInWorld =
worldBase.to(
new Matrix(1, 3, new double[] {event.values[0], event.values[1], event.values[2]}));
mAccModel.processAccelerometerEvent(
timestamp,
(float) accelerationInWorld.getValue(0, 0),
(float) accelerationInWorld.getValue(1, 0),
(float) accelerationInWorld.getValue(2, 0));
mAccelerationVector.setValues(
accelerationInWorld.getValue(0, 0),
accelerationInWorld.getValue(1, 0),
accelerationInWorld.getValue(2, 0));
}
mLastAccTimeStamp = timestamp;
}