Example usage for org.apache.commons.math3.complex Quaternion Quaternion

List of usage examples for org.apache.commons.math3.complex Quaternion Quaternion

  1. HOME
  2. Java
  3. org.apache.commons
  4. org.apache.commons.math3.complex.*
  5. Quaternion
  6. Quaternion

Introduction

In this page you can find the example usage for org.apache.commons.math3.complex Quaternion Quaternion.

Prototype

public Quaternion(final double scalar, final double[] v) throws DimensionMismatchException

Source Link

Document

Builds a quaternion from scalar and vector parts.

Usage

From source file:com.luthfihm.virtualtour.ar.GyroscopeOrientation.java

private void getRotationVectorFromGyro() {
    // Calculate the angular speed of the sample
    float magnitude = (float) Math
            .sqrt(Math.pow(vGyroscope[0], 2) + Math.pow(vGyroscope[1], 2) + Math.pow(vGyroscope[2], 2));

    // Normalize the rotation vector if it's big enough to get the axis
    if (magnitude > EPSILON) {
        vGyroscope[0] /= magnitude;//  w  ww. j a va  2  s  . c o  m
        vGyroscope[1] /= magnitude;
        vGyroscope[2] /= magnitude;
    }

    // Integrate around this axis with the angular speed by the timestep
    // in order to get a delta rotation from this sample over the timestep
    // We will convert this axis-angle representation of the delta rotation
    // into a quaternion before turning it into the rotation matrix.
    float thetaOverTwo = magnitude * dT / 2.0f;
    float sinThetaOverTwo = (float) Math.sin(thetaOverTwo);
    float cosThetaOverTwo = (float) Math.cos(thetaOverTwo);

    deltaVGyroscope[0] = sinThetaOverTwo * vGyroscope[0];
    deltaVGyroscope[1] = sinThetaOverTwo * vGyroscope[1];
    deltaVGyroscope[2] = sinThetaOverTwo * vGyroscope[2];
    deltaVGyroscope[3] = cosThetaOverTwo;

    // Create a new quaternion object base on the latest rotation
    // measurements...
    qGyroscopeDelta = new Quaternion(deltaVGyroscope[3], Arrays.copyOfRange(deltaVGyroscope, 0, 3));

    // Since it is a unit quaternion, we can just multiply the old rotation
    // by the new rotation delta to integrate the rotation.
    qGyroscope = qGyroscope.multiply(qGyroscopeDelta);
}

From source file:com.kircherelectronics.accelerationexplorer.filter.ImuLaCfQuaternion.java

/**
 * Calculates orientation angles from accelerometer and magnetometer output.
 */// w  ww  .j av a 2s .  c om
private void calculateOrientation() {
    // To get the orientation vector from the acceleration and magnetic
    // sensors, we let Android do the heavy lifting. This call will
    // automatically compensate for the tilt of the compass and fail if the
    // magnitude of the acceleration is not close to 9.82m/sec^2. You could
    // perform these steps yourself, but in my opinion, this is the best way
    // to do it.
    if (SensorManager.getRotationMatrix(rotationMatrix, null, acceleration, magnetic)) {
        SensorManager.getOrientation(rotationMatrix, baseOrientation);

        getRotationVectorFromAccelMag(baseOrientation);

        if (!hasOrientation) {
            quatGyro = new Quaternion(quatAccelMag.getScalarPart(), quatAccelMag.getVectorPart());
        }

        hasOrientation = true;
    }
}

From source file:com.bolatu.gezkoncsvlogger.GyroOrientation.GyroscopeOrientation.java

/**
 * Calculates a rotation vector from the gyroscope angular speed values.
 * /*from w w w . j a va  2  s  .co m*/
 * @param gyroValues
 * @param deltaRotationVector
 * @param timeFactor
 * @see http://developer.android
 *      .com/reference/android/hardware/SensorEvent.html#values
 */
private void getRotationVectorFromGyro() {
    // Calculate the angular speed of the sample
    float magnitude = (float) Math
            .sqrt(Math.pow(vGyroscope[0], 2) + Math.pow(vGyroscope[1], 2) + Math.pow(vGyroscope[2], 2));

    // Normalize the rotation vector if it's big enough to get the axis
    if (magnitude > EPSILON) {
        vGyroscope[0] /= magnitude;
        vGyroscope[1] /= magnitude;
        vGyroscope[2] /= magnitude;
    }

    // Integrate around this axis with the angular speed by the timestep
    // in order to get a delta rotation from this sample over the timestep
    // We will convert this axis-angle representation of the delta rotation
    // into a quaternion before turning it into the rotation matrix.
    float thetaOverTwo = magnitude * dT / 2.0f;
    float sinThetaOverTwo = (float) Math.sin(thetaOverTwo);
    float cosThetaOverTwo = (float) Math.cos(thetaOverTwo);

    deltaVGyroscope[0] = sinThetaOverTwo * vGyroscope[0];
    deltaVGyroscope[1] = sinThetaOverTwo * vGyroscope[1];
    deltaVGyroscope[2] = sinThetaOverTwo * vGyroscope[2];
    deltaVGyroscope[3] = cosThetaOverTwo;

    // Create a new quaternion object base on the latest rotation
    // measurements...
    qGyroscopeDelta = new Quaternion(deltaVGyroscope[3], Arrays.copyOfRange(deltaVGyroscope, 0, 3));

    // Since it is a unit quaternion, we can just multiply the old rotation
    // by the new rotation delta to integrate the rotation.
    qGyroscope = qGyroscope.multiply(qGyroscopeDelta);
}

From source file:com.bolatu.gezkoncsvlogger.GyroOrientation.ImuOCfQuaternion.java

protected void calculateOrientationAccelMag() {
    super.calculateOrientationAccelMag();

    getRotationVectorFromAccelMag(vOrientationAccelMag);

    // Get an initial orientation vector from the acceleration and magnetic
    // sensors./*  www .  jav a 2s.  co m*/
    if (isOrientationValidAccelMag && !isInitialOrientationValid) {
        quatGyro = new Quaternion(quatAccelMag.getScalarPart(), quatAccelMag.getVectorPart());

        isInitialOrientationValid = true;
    }
}

From source file:com.bolatu.gezkoncsvlogger.GyroOrientation.ImuOKfQuaternion.java

/**
 * Calculate the fused orientation.//from w w w. j a  v  a 2 s .c o  m
 */
private void calculateFusedOrientation() {
    qvOrientationGyroscope[0] = (float) quatGyro.getVectorPart()[0];
    qvOrientationGyroscope[1] = (float) quatGyro.getVectorPart()[1];
    qvOrientationGyroscope[2] = (float) quatGyro.getVectorPart()[2];
    qvOrientationGyroscope[3] = (float) quatGyro.getScalarPart();

    // Apply the Kalman filter... Note that the prediction and correction
    // inputs could be swapped, but the filter is much more stable in this
    // configuration.
    kalmanFilter.predict(qvOrientationGyroscope);
    kalmanFilter.correct(qvOrientationAccelMag);

    // Apply the new gyroscope delta rotation to the new Kalman filter
    // rotation estimation.
    quatGyro = new Quaternion(kalmanFilter.getStateEstimation()[3],
            Arrays.copyOfRange(kalmanFilter.getStateEstimation(), 0, 3));

    // Now we get a structure we can pass to get a rotation matrix, and then
    // an orientation vector from Android.
    qvFusedOrientation[0] = (float) kalmanFilter.getStateEstimation()[0];
    qvFusedOrientation[1] = (float) kalmanFilter.getStateEstimation()[1];
    qvFusedOrientation[2] = (float) kalmanFilter.getStateEstimation()[2];
    qvFusedOrientation[3] = (float) kalmanFilter.getStateEstimation()[3];

    // We need a rotation matrix so we can get the orientation vector...
    // Getting Euler
    // angles from a quaternion is not trivial, so this is the easiest way,
    // but perhaps
    // not the fastest way of doing this.
    SensorManager.getRotationMatrixFromVector(rmFusedOrientation, qvFusedOrientation);

    // Get the fused orienatation
    SensorManager.getOrientation(rmFusedOrientation, vFusedOrientation);
}

From source file:com.bolatu.gezkoncsvlogger.GyroOrientation.ImuOCfQuaternion.java

/**
 * Calculates a rotation vector from the gyroscope angular speed values.
 * //from  w ww.j  av a  2s .co  m
 * @param gyroValues
 * @param deltaRotationVector
 * @param timeFactor
 * @see http://developer.android
 *      .com/reference/android/hardware/SensorEvent.html#values
 */
private void getRotationVectorFromGyro() {
    // Calculate the angular speed of the sample
    float magnitude = (float) Math
            .sqrt(Math.pow(vGyroscope[0], 2) + Math.pow(vGyroscope[1], 2) + Math.pow(vGyroscope[2], 2));

    // Normalize the rotation vector if it's big enough to get the axis
    if (magnitude > EPSILON) {
        vGyroscope[0] /= magnitude;
        vGyroscope[1] /= magnitude;
        vGyroscope[2] /= magnitude;
    }

    // Integrate around this axis with the angular speed by the timestep
    // in order to get a delta rotation from this sample over the timestep
    // We will convert this axis-angle representation of the delta rotation
    // into a quaternion before turning it into the rotation matrix.
    float thetaOverTwo = magnitude * dT / 2.0f;
    float sinThetaOverTwo = (float) Math.sin(thetaOverTwo);
    float cosThetaOverTwo = (float) Math.cos(thetaOverTwo);

    vDeltaGyroscope[0] = sinThetaOverTwo * vGyroscope[0];
    vDeltaGyroscope[1] = sinThetaOverTwo * vGyroscope[1];
    vDeltaGyroscope[2] = sinThetaOverTwo * vGyroscope[2];
    vDeltaGyroscope[3] = cosThetaOverTwo;

    // Create a new quaternion object base on the latest rotation
    // measurements...
    quatGyroDelta = new Quaternion(vDeltaGyroscope[3], Arrays.copyOfRange(vDeltaGyroscope, 0, 3));

    // Since it is a unit quaternion, we can just multiply the old rotation
    // by the new rotation delta to integrate the rotation.
    quatGyro = quatGyro.multiply(quatGyroDelta);
}

From source file:com.kircherelectronics.accelerationexplorer.filter.ImuLaCfQuaternion.java

/**
 * Calculates a rotation vector from the gyroscope angular speed values.
 * //from   w  w  w  . ja va  2  s. c o  m
 * @param gyroValues
 * @param deltaRotationVector
 * @param timeFactor
 * @see http://developer.android
 *      .com/reference/android/hardware/SensorEvent.html#values
 */
private void getRotationVectorFromGyro(float timeFactor) {
    // Calculate the angular speed of the sample
    float magnitude = (float) Math
            .sqrt(Math.pow(gyroscope[0], 2) + Math.pow(gyroscope[1], 2) + Math.pow(gyroscope[2], 2));

    // Normalize the rotation vector if it's big enough to get the axis
    if (magnitude > EPSILON) {
        gyroscope[0] /= magnitude;
        gyroscope[1] /= magnitude;
        gyroscope[2] /= magnitude;
    }

    // Integrate around this axis with the angular speed by the timestep
    // in order to get a delta rotation from this sample over the timestep
    // We will convert this axis-angle representation of the delta rotation
    // into a quaternion before turning it into the rotation matrix.
    float thetaOverTwo = magnitude * timeFactor / 2.0f;
    float sinThetaOverTwo = (float) Math.sin(thetaOverTwo);
    float cosThetaOverTwo = (float) Math.cos(thetaOverTwo);

    deltaVectorGyro[0] = sinThetaOverTwo * gyroscope[0];
    deltaVectorGyro[1] = sinThetaOverTwo * gyroscope[1];
    deltaVectorGyro[2] = sinThetaOverTwo * gyroscope[2];
    deltaVectorGyro[3] = cosThetaOverTwo;

    // Create a new quaternion object base on the latest rotation
    // measurements...
    quatGyroDelta = new Quaternion(deltaVectorGyro[3], Arrays.copyOfRange(deltaVectorGyro, 0, 3));

    // Since it is a unit quaternion, we can just multiply the old rotation
    // by the new rotation delta to integrate the rotation.
    quatGyro = quatGyro.multiply(quatGyroDelta);
}

From source file:com.kircherelectronics.accelerationexplorer.filter.ImuLaKfQuaternion.java

/**
 * Calculate the fused orientation.//from ww w  . ja  v  a2 s. c o m
 */
private void calculateFusedOrientation() {
    vectorGyro[0] = (float) quatGyro.getVectorPart()[0];
    vectorGyro[1] = (float) quatGyro.getVectorPart()[1];
    vectorGyro[2] = (float) quatGyro.getVectorPart()[2];
    vectorGyro[3] = (float) quatGyro.getScalarPart();

    // Apply the Kalman filter... Note that the prediction and correction
    // inputs could be swapped, but the filter is much more stable in this
    // configuration.
    kalmanFilter.predict(vectorGyro);
    kalmanFilter.correct(vectorAccelMag);

    // Apply the new gyroscope delta rotation to the new Kalman filter
    // rotation estimation.
    quatGyro = new Quaternion(kalmanFilter.getStateEstimation()[3],
            Arrays.copyOfRange(kalmanFilter.getStateEstimation(), 0, 3));

    // Now we get a structure we can pass to get a rotation matrix, and then
    // an orientation vector from Android.
    fusedVector[0] = (float) kalmanFilter.getStateEstimation()[0];
    fusedVector[1] = (float) kalmanFilter.getStateEstimation()[1];
    fusedVector[2] = (float) kalmanFilter.getStateEstimation()[2];
    fusedVector[3] = (float) kalmanFilter.getStateEstimation()[3];

    // We need a rotation matrix so we can get the orientation vector...
    // Getting Euler
    // angles from a quaternion is not trivial, so this is the easiest way,
    // but perhaps
    // not the fastest way of doing this.
    SensorManager.getRotationMatrixFromVector(fusedMatrix, fusedVector);

    // Get the fused orienatation
    SensorManager.getOrientation(fusedMatrix, fusedOrientation);
}