コード例 #1
0
  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);
  }
コード例 #2
0
  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;
  }