List of usage examples for org.apache.commons.math3.complex Quaternion getQ0
public double getQ0()
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 ww w . j ava 2 s .c o m return inputQ; } }
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 ww w .jav a2 s .com*/ 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 Rot weightedAverageRotation(ArrayList<Rot> rotList, ArrayList<Double> rotWeight) { double addedSoFar = 0; double totalWeight = 0d; for (Double rt : rotWeight) totalWeight += rt;//from w w w. j a v a 2s. com 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:com.diozero.sandpit.imu.invensense.MPU9150DriverTest.java
public void run() { try (MPU9150Driver mpu = new MPU9150Driver(I2CConstants.BUS_1, I2CConstants.ADDR_SIZE_7, MPU9150Constants.I2C_CLOCK_FREQUENCY_FAST)) { mpuInit(mpu);/*from w ww. j a v a 2 s .c om*/ System.err.println("Ready."); do { ImuData imu_data = update(mpu); System.out.print("Got IMU data: compass=[" + imu_data.getCompass() + "], temp=" + imu_data.getTemperature() + ", gyro=[" + imu_data.getGyro() + "], accel=[" + imu_data.getAccel() + "], quat=[" + imu_data.getQuaternion().getQ0() + ", " + imu_data.getQuaternion().getQ1() + ", " + imu_data.getQuaternion().getQ2() + ", " + imu_data.getQuaternion().getQ3() + "], timestamp=" + imu_data.getTimestamp() + ", "); Quaternion q = imu_data.getQuaternion(); //double[] ypr = q.toEuler(); //double[] ypr = quat.getYawPitchRoll(); Rotation r = new Rotation(q.getQ0(), q.getQ1(), q.getQ2(), q.getQ3(), true); double[] ypr = null; try { ypr = r.getAngles(RotationOrder.XYZ, RotationConvention.VECTOR_OPERATOR); } catch (CardanEulerSingularityException e) { ypr = new double[] { 2 * Math.atan2(q.getQ1(), q.getQ0()), Math.PI / 2, 0 }; System.out.print("Singularity detected, "); } Logger.info("ypr=[{}, {}, {}]", Double.valueOf(ypr[0]), Double.valueOf(ypr[1]), Double.valueOf(ypr[2])); } while (true); } catch (RuntimeIOException ioe) { Logger.error(ioe, "Error: {}", ioe); } }
From source file:com.diozero.sandpit.imu.invensense.MPU9150DriverMqttPublisher.java
public void run() { try (MPU9150Driver mpu = new MPU9150Driver(I2CConstants.BUS_1, I2CConstants.ADDR_SIZE_7, MPU9150Constants.I2C_CLOCK_FREQUENCY_FAST)) { mpuInit(mpu);//from w w w. j a v a2 s.c o m mqttInit(); System.err.println("Ready."); do { ImuData imu_data = update(mpu); System.out.print("Got IMU data: compass=[" + imu_data.getCompass() + "], temp=" + imu_data.getTemperature() + ", gyro=[" + imu_data.getGyro() + "], accel=[" + imu_data.getAccel() + "], quat=[" + imu_data.getQuaternion().getQ0() + ", " + imu_data.getQuaternion().getQ1() + ", " + imu_data.getQuaternion().getQ2() + ", " + imu_data.getQuaternion().getQ3() + "], timestamp=" + imu_data.getTimestamp() + ", "); Quaternion q = imu_data.getQuaternion(); //double[] ypr = q.toEuler(); //double[] ypr = quat.getYawPitchRoll(); Rotation r = new Rotation(q.getQ0(), q.getQ1(), q.getQ2(), q.getQ3(), true); double[] ypr = null; try { ypr = r.getAngles(RotationOrder.XYZ, RotationConvention.VECTOR_OPERATOR); } catch (CardanEulerSingularityException e) { ypr = new double[] { 2 * Math.atan2(q.getQ1(), q.getQ0()), Math.PI / 2, 0 }; System.out.print("Singularity detected, "); } Logger.info("ypr=[{}, {}, {}]", Double.valueOf(ypr[0]), Double.valueOf(ypr[1]), Double.valueOf(ypr[2])); mqttPublish(imu_data, ypr); } while (true); } catch (RuntimeIOException ioe) { Logger.error(ioe, "Error: {}", ioe); } catch (MqttException me) { Logger.error(me, "Error: {}", me); } finally { try { mqttClient.disconnect(); } catch (Exception e) { } } }
From source file:IK.AbstractArmature.java
private void applyAverageWeightedRotations(StrandedArmature collection) { for (AbstractBone b : collection.allBonesInStrandCollection) { //double totalDist = 0d; ArrayList<Strand> strandList = collection.boneStrandMap.get(b); /*for(Strand s : strandList) { s.updateDistToTarget();/*w ww.j ava 2 s.c om*/ totalDist += s.distToTarget; }*/ double totalCount = 0; double wT = 0; double xT = 0; double yT = 0; double zT = 0; //totalDist = collection.totalPinDist; Rot ir = new Rot(new DVector(1, 1, 1), 0);//strandList.get(0).rotationsMap.get(b); Quaternion initialQ = new Quaternion(ir.rotation.getQ0(), ir.rotation.getQ1(), ir.rotation.getQ2(), ir.rotation.getQ3()); for (Strand s : strandList) { double distance = s.distToTarget; Rot r = s.rotationsMap.get(b); //r = new Rot(r.getAxis(), r.getAngle()*(distance/totalDist)); Quaternion current = G.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(); totalCount++; //totalDist += distance; } Rot avg = new Rot(wT / totalCount, xT / totalCount, yT / totalCount, zT / totalCount, true); b.rotateBy(avg.rotation); //TODO DEBUG: TEST WHAT HAPPENS IF I ENABLE SNAPPING TO CONSTRAINTS HERE //b.snapToConstraints(); collection.setDeltaMeasureForBone(b, avg.getAngle()); } }
From source file:org.gearvrf.GVRInternalSensorListener.java
@Override public void onSensorChanged(SensorEvent event) { float w;// www. j a v a 2s . co m float x = event.values[0]; float y = event.values[1]; float z = event.values[2]; if (Build.VERSION.SDK_INT < 18) { w = getQuaternionW(event.values[0], event.values[1], event.values[2]); } else { w = event.values[3]; } Quaternion sensorQuaternion = new Quaternion(w, x, y, z); Quaternion quaternion = COORDINATE_QUATERNION.getInverse().multiply(OFFSET_QUATERNION) .multiply(sensorQuaternion).multiply(COORDINATE_QUATERNION); mSensor.onInternalRotationSensor(GVRTime.getCurrentTime(), (float) quaternion.getQ0(), (float) quaternion.getQ1(), (float) quaternion.getQ2(), (float) quaternion.getQ3(), 0.0f, 0.0f, 0.0f); }
From source file:org.ndm.photogrammetry.QuaternionUtil.java
/** * Converts a quaternion into a rotation matrix. * @param q - a quaternion//from www .ja va 2 s . c o m * @return - 3x3 rotation matrix */ public static RealMatrix rotationMatrixFromQuaternion2(Quaternion q) { double[][] m = new double[3][3]; double sqw = q.getQ0() * q.getQ0(); double sqx = q.getQ1() * q.getQ1(); double sqy = q.getQ2() * q.getQ2(); double sqz = q.getQ3() * q.getQ3(); // normalize as we go... double invs = 1 / (sqx + sqy + sqz + sqw); m[0][0] = (sqx - sqy - sqz + sqw) * invs; // since sqw + sqx + sqy + sqz =1/invs*invs m[1][1] = (-sqx + sqy - sqz + sqw) * invs; m[2][2] = (-sqx - sqy + sqz + sqw) * invs; double tmp1 = q.getQ1() * q.getQ2(); double tmp2 = q.getQ3() * q.getQ0(); m[1][0] = 2.0 * (tmp1 + tmp2) * invs; m[0][1] = 2.0 * (tmp1 - tmp2) * invs; tmp1 = q.getQ1() * q.getQ3(); tmp2 = q.getQ2() * q.getQ0(); m[2][0] = 2.0 * (tmp1 - tmp2) * invs; m[0][2] = 2.0 * (tmp1 + tmp2) * invs; tmp1 = q.getQ2() * q.getQ3(); tmp2 = q.getQ1() * q.getQ0(); m[2][1] = 2.0 * (tmp1 + tmp2) * invs; m[1][2] = 2.0 * (tmp1 - tmp2) * invs; Array2DRowRealMatrix rotationMatrix = new Array2DRowRealMatrix(m); return rotationMatrix; }
From source file:sceneGraph.Rot.java
public Rot(double amount, Rot v1, Rot v2) { Quaternion q = slerp(amount, new Quaternion(v1.rotation.getQ0(), v1.rotation.getQ1(), v1.rotation.getQ2(), v1.rotation.getQ3()), new Quaternion(v2.rotation.getQ0(), v2.rotation.getQ1(), v2.rotation.getQ2(), v2.rotation.getQ3())); rotation = new Rotation(q.getQ0(), q.getQ1(), q.getQ2(), q.getQ3(), false); }
From source file:sceneGraph.Rot.java
/** Get the swing rotation and twist rotation for the specified axis. The twist rotation represents the rotation around the * specified axis. The swing rotation represents the rotation of the specified axis itself, which is the rotation around an * axis perpendicular to the specified axis. </p> The swing and twist rotation can be used to reconstruct the original * quaternion: this = swing * twist//from www . j a va 2 s . co m * * @param axisX the X component of the normalized axis for which to get the swing and twist rotation * @param axisY the Y component of the normalized axis for which to get the swing and twist rotation * @param axisZ the Z component of the normalized axis for which to get the swing and twist rotation * @return an Array of Rot objects. With the first element representing the swing, and the second representing the twist * @see <a href="http://www.euclideanspace.com/maths/geometry/rotations/for/decomposition">calculation</a> */ public Rot[] getSwingTwist(DVector axis) { Quaternion thisQ = new Quaternion(rotation.getQ0(), rotation.getQ1(), rotation.getQ2(), rotation.getQ3()); double d = new DVector(thisQ.getQ1(), thisQ.getQ2(), thisQ.getQ3()).dot(axis); Quaternion twist = new Quaternion(rotation.getQ0(), axis.x * d, axis.y * d, axis.z * d).normalize(); if (d < 0) twist.multiply(-1f); Quaternion swing = twist.getConjugate(); swing = Quaternion.multiply(thisQ, swing); Rot[] result = new Rot[2]; result[0] = new Rot(new Rotation(swing.getQ0(), swing.getQ1(), swing.getQ2(), swing.getQ3(), true)); result[1] = new Rot(new Rotation(twist.getQ0(), twist.getQ1(), twist.getQ2(), twist.getQ3(), true)); return result; }