List of usage examples for org.apache.commons.math3.complex Quaternion Quaternion
public Quaternion(final double a, final double b, final double c, final double d)
From source file:com.diozero.api.imu.ImuDataFactory.java
public static Quaternion createQuaternion(int[] quat, double scale) { // This article suggests QUAT_W is [0] // https://github.com/vmayoral/bb_mpu9150/blob/master/src/linux-mpu9150/mpu9150/mpu9150.c Quaternion quaterion = new Quaternion(quat[MPU9150_QUAT_W] * scale, quat[MPU9150_QUAT_X] * scale, quat[MPU9150_QUAT_Y] * scale, quat[MPU9150_QUAT_Z] * scale); return quaterion.normalize(); }
From source file:com.luthfihm.virtualtour.ar.GyroscopeOrientation.java
private void getRotationVectorFromAccelMag() { // Assuming the angles are in radians. // getOrientation() values: // values[0]: azimuth, rotation around the Z axis. // values[1]: pitch, rotation around the X axis. // values[2]: roll, rotation around the Y axis. // Heading, Azimuth, Yaw double c1 = Math.cos(vOrientationAccelMag[0] / 2); double s1 = Math.sin(vOrientationAccelMag[0] / 2); // Pitch, Attitude // The equation assumes the pitch is pointed in the opposite direction // of the orientation vector provided by Android, so we invert it. double c2 = Math.cos(-vOrientationAccelMag[1] / 2); double s2 = Math.sin(-vOrientationAccelMag[1] / 2); // Roll, Bank double c3 = Math.cos(vOrientationAccelMag[2] / 2); double s3 = Math.sin(vOrientationAccelMag[2] / 2); double c1c2 = c1 * c2; double s1s2 = s1 * s2; double w = c1c2 * c3 - s1s2 * s3; double x = c1c2 * s3 + s1s2 * c3; double y = s1 * c2 * c3 + c1 * s2 * s3; double z = c1 * s2 * c3 - s1 * c2 * s3; // The quaternion in the equation does not share the same coordinate // system as the Android gyroscope quaternion we are using. We reorder // it here./*from w ww .j a va 2 s . c o m*/ // Android X (pitch) = Equation Z (pitch) // Android Y (roll) = Equation X (roll) // Android Z (azimuth) = Equation Y (azimuth) qGyroscope = new Quaternion(w, z, x, y); }
From source file:com.bolatu.gezkoncsvlogger.GyroOrientation.GyroscopeOrientation.java
/** * Create an angle-axis vector, in this case a unit quaternion, from the * provided Euler angle's (presumably from SensorManager.getOrientation()). * // w w w. j a v a 2s.com * Equation from * http://www.euclideanspace.com/maths/geometry/rotations/conversions * /eulerToQuaternion/ * * @param orientation */ private void getRotationVectorFromAccelMag() { // Assuming the angles are in radians. // getOrientation() values: // values[0]: azimuth, rotation around the Z axis. // values[1]: pitch, rotation around the X axis. // values[2]: roll, rotation around the Y axis. // Heading, Azimuth, Yaw double c1 = Math.cos(vOrientationAccelMag[0] / 2); double s1 = Math.sin(vOrientationAccelMag[0] / 2); // Pitch, Attitude // The equation assumes the pitch is pointed in the opposite direction // of the orientation vector provided by Android, so we invert it. double c2 = Math.cos(-vOrientationAccelMag[1] / 2); double s2 = Math.sin(-vOrientationAccelMag[1] / 2); // Roll, Bank double c3 = Math.cos(vOrientationAccelMag[2] / 2); double s3 = Math.sin(vOrientationAccelMag[2] / 2); double c1c2 = c1 * c2; double s1s2 = s1 * s2; double w = c1c2 * c3 - s1s2 * s3; double x = c1c2 * s3 + s1s2 * c3; double y = s1 * c2 * c3 + c1 * s2 * s3; double z = c1 * s2 * c3 - s1 * c2 * s3; // The quaternion in the equation does not share the same coordinate // system as the Android gyroscope quaternion we are using. We reorder // it here. // Android X (pitch) = Equation Z (pitch) // Android Y (roll) = Equation X (roll) // Android Z (azimuth) = Equation Y (azimuth) qGyroscope = new Quaternion(w, z, x, y); }
From source file:com.bolatu.gezkoncsvlogger.GyroOrientation.ImuOKfQuaternion.java
/** * Create an angle-axis vector, in this case a unit quaternion, from the * provided Euler angle's (presumably from SensorManager.getOrientation()). * //from www . j a v a 2s.c om * Equation from * http://www.euclideanspace.com/maths/geometry/rotations/conversions * /eulerToQuaternion/ * * @param orientation */ private void getRotationVectorFromAccelMag(float[] orientation) { // Assuming the angles are in radians. // getOrientation() values: // values[0]: azimuth, rotation around the Z axis. // values[1]: pitch, rotation around the X axis. // values[2]: roll, rotation around the Y axis. // Heading, Azimuth, Yaw double c1 = Math.cos(-orientation[0] / 2); double s1 = Math.sin(-orientation[0] / 2); // Pitch, Attitude // The equation assumes the pitch is pointed in the opposite direction // of the orientation vector provided by Android, so we invert it. double c2 = Math.cos(-orientation[1] / 2); double s2 = Math.sin(-orientation[1] / 2); // Roll, Bank double c3 = Math.cos(orientation[2] / 2); double s3 = Math.sin(orientation[2] / 2); double c1c2 = c1 * c2; double s1s2 = s1 * s2; double w = c1c2 * c3 - s1s2 * s3; double x = c1c2 * s3 + s1s2 * c3; double y = s1 * c2 * c3 + c1 * s2 * s3; double z = c1 * s2 * c3 - s1 * c2 * s3; // The quaternion in the equation does not share the same coordinate // system as the Android gyroscope quaternion we are using. We reorder // it here. // Android X (pitch) = Equation Z (pitch) // Android Y (roll) = Equation X (roll) // Android Z (azimuth) = Equation Y (azimuth) qvOrientationAccelMag[0] = z; qvOrientationAccelMag[1] = x; qvOrientationAccelMag[2] = y; qvOrientationAccelMag[3] = w; quatAccelMag = new Quaternion(w, z, x, y); }
From source file:com.kircherelectronics.accelerationexplorer.filter.ImuLaKfQuaternion.java
/** * Create an angle-axis vector, in this case a unit quaternion, from the * provided Euler angle's (presumably from SensorManager.getOrientation()). * /*from w ww . j a va 2s.c o m*/ * Equation from * http://www.euclideanspace.com/maths/geometry/rotations/conversions * /eulerToQuaternion/ * * @param orientation */ private void getRotationVectorFromAccelMag(float[] orientation) { // Assuming the angles are in radians. // getOrientation() values: // values[0]: azimuth, rotation around the Z axis. // values[1]: pitch, rotation around the X axis. // values[2]: roll, rotation around the Y axis. // Heading, Azimuth, Yaw double c1 = Math.cos(orientation[0] / 2); double s1 = Math.sin(orientation[0] / 2); // Pitch, Attitude // The equation assumes the pitch is pointed in the opposite direction // of the orientation vector provided by Android, so we invert it. double c2 = Math.cos(-orientation[1] / 2); double s2 = Math.sin(-orientation[1] / 2); // Roll, Bank double c3 = Math.cos(orientation[2] / 2); double s3 = Math.sin(orientation[2] / 2); double c1c2 = c1 * c2; double s1s2 = s1 * s2; double w = c1c2 * c3 - s1s2 * s3; double x = c1c2 * s3 + s1s2 * c3; double y = s1 * c2 * c3 + c1 * s2 * s3; double z = c1 * s2 * c3 - s1 * c2 * s3; // The quaternion in the equation does not share the same coordinate // system as the Android gyroscope quaternion we are using. We reorder // it here. // Android X (pitch) = Equation Z (pitch) // Android Y (roll) = Equation X (roll) // Android Z (azimuth) = Equation Y (azimuth) vectorAccelMag[0] = z; vectorAccelMag[1] = x; vectorAccelMag[2] = y; vectorAccelMag[3] = w; if (!hasOrientation) { quatAccelMag = new Quaternion(w, z, x, y); } }
From source file:com.bolatu.gezkoncsvlogger.GyroOrientation.ImuOCfQuaternion.java
/** * Create an angle-axis vector, in this case a unit quaternion, from the * provided Euler angle's (presumably from SensorManager.getOrientation()). * //from w w w. j a va2 s .c om * Equation from * http://www.euclideanspace.com/maths/geometry/rotations/conversions * /eulerToQuaternion/ * * @param orientation */ private void getRotationVectorFromAccelMag(float[] orientation) { // Assuming the angles are in radians. // getOrientation() values: // values[0]: azimuth, rotation around the Z axis. // values[1]: pitch, rotation around the X axis. // values[2]: roll, rotation around the Y axis. // Heading, Azimuth, Yaw double c1 = Math.cos(-orientation[0] / 2); double s1 = Math.sin(-orientation[0] / 2); // Pitch, Attitude // The equation assumes the pitch is pointed in the opposite direction // of the orientation vector provided by Android, so we invert it. double c2 = Math.cos(-orientation[1] / 2); double s2 = Math.sin(-orientation[1] / 2); // Roll, Bank double c3 = Math.cos(orientation[2] / 2); double s3 = Math.sin(orientation[2] / 2); double c1c2 = c1 * c2; double s1s2 = s1 * s2; double w = c1c2 * c3 - s1s2 * s3; double x = c1c2 * s3 + s1s2 * c3; double y = s1 * c2 * c3 + c1 * s2 * s3; double z = c1 * s2 * c3 - s1 * c2 * s3; // The quaternion in the equation does not share the same coordinate // system as the Android gyroscope quaternion we are using. We reorder // it here. // Android X (pitch) = Equation Z (pitch) // Android Y (roll) = Equation X (roll) // Android Z (azimuth) = Equation Y (azimuth) quatAccelMag = new Quaternion(w, z, x, y); }
From source file:com.kircherelectronics.accelerationexplorer.filter.ImuLaCfQuaternion.java
/** * Create an angle-axis vector, in this case a unit quaternion, from the * provided Euler angle's (presumably from SensorManager.getOrientation()). * /*from ww w.j a v a 2 s .c o m*/ * Equation from * http://www.euclideanspace.com/maths/geometry/rotations/conversions * /eulerToQuaternion/ * * @param orientation */ private void getRotationVectorFromAccelMag(float[] orientation) { // Assuming the angles are in radians. // getOrientation() values: // values[0]: azimuth, rotation around the Z axis. // values[1]: pitch, rotation around the X axis. // values[2]: roll, rotation around the Y axis. // Heading, Azimuth, Yaw double c1 = Math.cos(orientation[0] / 2); double s1 = Math.sin(orientation[0] / 2); // Pitch, Attitude // The equation assumes the pitch is pointed in the opposite direction // of the orientation vector provided by Android, so we invert it. double c2 = Math.cos(-orientation[1] / 2); double s2 = Math.sin(-orientation[1] / 2); // Roll, Bank double c3 = Math.cos(orientation[2] / 2); double s3 = Math.sin(orientation[2] / 2); double c1c2 = c1 * c2; double s1s2 = s1 * s2; double w = c1c2 * c3 - s1s2 * s3; double x = c1c2 * s3 + s1s2 * c3; double y = s1 * c2 * c3 + c1 * s2 * s3; double z = c1 * s2 * c3 - s1 * c2 * s3; // The quaternion in the equation does not share the same coordinate // system as the Android gyroscope quaternion we are using. We reorder // it here. // Android X (pitch) = Equation Z (pitch) // Android Y (roll) = Equation X (roll) // Android Z (azimuth) = Equation Y (azimuth) quatAccelMag = new Quaternion(w, z, x, y); }
From source file:IK.G.java
public static Rot weightedAverageRotation(ArrayList<Rot> rotList, ArrayList<Double> rotWeight) { double addedSoFar = 0; double totalWeight = 0d; for (Double rt : rotWeight) totalWeight += rt;/* w w w .j a va 2 s . c o m*/ double wT = 0; double xT = 0; double yT = 0; double zT = 0; Rot ir = rotList.get(0); Quaternion initialQ = new Quaternion(ir.rotation.getQ0(), ir.rotation.getQ1(), ir.rotation.getQ2(), ir.rotation.getQ3()); for (int i = 0; i < rotList.size(); i++) { Rot rt = rotList.get(i); Rot r = new Rot(rt.getAxis(), rt.getAngle() * (rotWeight.get(i) / totalWeight)); Quaternion current = getSingleCoveredQuaternion( new Quaternion(r.rotation.getQ0(), r.rotation.getQ1(), r.rotation.getQ2(), r.rotation.getQ3()), initialQ); wT += current.getQ0(); xT += current.getQ1(); yT += current.getQ2(); zT += current.getQ3(); addedSoFar++; } return new Rot(wT / addedSoFar, xT / addedSoFar, yT / addedSoFar, zT / addedSoFar, true); }
From source file:IK.G.java
public static Rot averageRotation(ArrayList<Rot> rotList) { double addedSoFar = 0; double wT = 0; double xT = 0; double yT = 0; double zT = 0; Rot ir = rotList.get(0);/*from www . j a va 2s. c o m*/ Quaternion initialQ = new Quaternion(ir.rotation.getQ0(), ir.rotation.getQ1(), ir.rotation.getQ2(), ir.rotation.getQ3()); for (Rot r : rotList) { Quaternion current = getSingleCoveredQuaternion( new Quaternion(r.rotation.getQ0(), r.rotation.getQ1(), r.rotation.getQ2(), r.rotation.getQ3()), initialQ); wT += current.getQ0(); xT += current.getQ1(); yT += current.getQ2(); zT += current.getQ3(); addedSoFar++; } return new Rot(wT / addedSoFar, xT / addedSoFar, yT / addedSoFar, zT / addedSoFar, true); }
From source file:IK.G.java
public static Quaternion getSingleCoveredQuaternion(Quaternion inputQ, Quaternion targetQ) { //targetQ is the Quaternion that exists on the target hemisphere if (Quaternion.dotProduct(inputQ, targetQ) < 0d) { return new Quaternion(-inputQ.getQ0(), -inputQ.getQ1(), -inputQ.getQ2(), -inputQ.getQ3()); } else {//from w w w . j av a2 s. co m return inputQ; } }