- Add the Codota plugin to your IDE and get smart completions
private void myMethod () {Connection c =
DataSource dataSource;dataSource.getConnection()
- Smart code suggestions by Tabnine
}
private Quaternion computeQuat1Quat2Quat1Conjugate(Quaternion quaternion1, Quaternion quaternion2) { Quaternion quaternion1Inverse = new Quaternion(quaternion1); quaternion1Inverse.inverse(); Quaternion quaternion1TimesQuaternion2 = new Quaternion(); quaternion1TimesQuaternion2.multiply(quaternion1, quaternion2); Quaternion quaternion1TimesQuaternion2TimesQuaternion1Inverse = new Quaternion(); quaternion1TimesQuaternion2TimesQuaternion1Inverse.multiply(quaternion1TimesQuaternion2, quaternion1Inverse); return quaternion1TimesQuaternion2TimesQuaternion1Inverse; }
qExpected.multiplyConjugateOther(q2); else qExpected.multiply(q2);
Qsi.multiply(Qip); qs.set(Qsi.getS()); qx.set(Qsi.getX());
private void corruptOrientation(Quaternion orientation) { Vector3D axis = RandomGeometry.nextVector3D(random); double angle = RandomNumbers.nextDouble(random, -maxRotationCorruption, maxRotationCorruption); AxisAngle axisAngle4d = new AxisAngle(); axisAngle4d.set(axis, angle); Quaternion corruption = new Quaternion(); corruption.set(axisAngle4d); orientation.multiply(corruption); }
private void computeAngularVelocity(Vector3D angularVelocityToPack, Quaternion startRotationQuaternion, Quaternion endRotationQuaternion, double alphaDot) { if (startRotationQuaternion.dot(endRotationQuaternion) < 0.0) endRotationQuaternion.negate(); // compute relative orientation: orientation of interpolated frame w.r.t. start frame relativeRotationQuaternion.set(startRotationQuaternion); // R_W_S: orientation of start w.r.t. world relativeRotationQuaternion.conjugate(); // R_S_W: orientation of world w.r.t. start relativeRotationQuaternion.multiply(endRotationQuaternion); // R_S_I = R_S_W * R_W_I: orientation of interpolated w.r.t. start quaternionCalculus.log(relativeRotationQuaternion, angularVelocityToPack); angularVelocityToPack.scale(alphaDot); endRotationQuaternion.transform(angularVelocityToPack); } }
@Test public void testTransformWithQuaternion() throws Exception { Random random = new Random(34534L); RigidBodyTransform transform = EuclidCoreRandomTools.nextRigidBodyTransform(random); Quaternion quaternionOriginal = EuclidCoreRandomTools.nextQuaternion(random); Quaternion quaternionExpected = new Quaternion(); Quaternion quaternionActual = new Quaternion(); transform.getRotation(quaternionExpected); quaternionExpected.multiply(quaternionOriginal); transform.transform(quaternionOriginal, quaternionActual); EuclidCoreTestTools.assertQuaternionEquals(quaternionExpected, quaternionActual, EPS); quaternionActual.set(quaternionOriginal); transform.transform(quaternionActual); EuclidCoreTestTools.assertQuaternionEquals(quaternionExpected, quaternionActual, EPS); }
@Test public void testTransformWithQuaternion() throws Exception { Random random = new Random(34534L); AffineTransform transform = EuclidCoreRandomTools.nextAffineTransform(random); Quaternion quaternionOriginal = EuclidCoreRandomTools.nextQuaternion(random); Quaternion quaternionExpected = new Quaternion(); Quaternion quaternionActual = new Quaternion(); transform.getRotation(quaternionExpected); quaternionExpected.multiply(quaternionOriginal); transform.transform(quaternionOriginal, quaternionActual); EuclidCoreTestTools.assertQuaternionEquals(quaternionExpected, quaternionActual, EPS); quaternionActual.set(quaternionOriginal); transform.transform(quaternionActual); EuclidCoreTestTools.assertQuaternionEquals(quaternionExpected, quaternionActual, EPS); }
quaternionCalculus.interpolate(alpha, initialQuaternionDriftedToPack, finalQuaternionDriftedToPack, qInterpolated, true); quaternionCalculus.interpolate(alpha, initialDrift, finalDrift, interpolatedDrift, false); qInterpolated.multiply(interpolatedDrift, qInterpolated); initialQuaternionDriftedToPack.multiply(initialDrift, initialQuaternionDriftedToPack); finalQuaternionDriftedToPack.multiply(finalDrift, finalQuaternionDriftedToPack);
private void setYawPitchRoll() { Quaternion q = new Quaternion(); //This code has a singularity when yaw and roll line up (e.g. pitch is 90, can't rotate in one direction any more). q.setYawPitchRoll(q_yaw.getDoubleValue(), q_pitch.getDoubleValue(), q_roll.getDoubleValue()); //This code compounds the rotations so that on subsequent frames the ability to rotate in lost rotation directions is regained //This affectively uses global yaw pitch and roll each time. q.multiply(qprev); q_qs.set(q.getS()); q_qx.set(q.getX()); q_qy.set(q.getY()); q_qz.set(q.getZ()); }
@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout = 30000) public void testProjectRotationOnAxis() { Random random = new Random(9429424L); Quaternion fullRotation = new Quaternion(); Quaternion actualRotation = new Quaternion(); for (int i = 0; i < 10000; i++) { // Create random axis and a rotation around that axis. Vector3D axis = EuclidCoreRandomTools.nextVector3DWithFixedLength(random, 1.0); double angle = EuclidCoreRandomTools.nextDouble(random, -Math.PI, Math.PI); Quaternion expectedRotation = new Quaternion(new AxisAngle(axis, angle)); // Create an orthogonal rotation. Vector3D orthogonalAxis = EuclidCoreRandomTools.nextOrthogonalVector3D(random, axis, true); double orthogonalAngle = EuclidCoreRandomTools.nextDouble(random, -Math.PI, Math.PI); Quaternion orthogonalRotation = new Quaternion(new AxisAngle(orthogonalAxis, orthogonalAngle)); // From the combined rotation and the original axis back out the rotation around the original axis. fullRotation.multiply(orthogonalRotation, expectedRotation); EuclidCoreMissingTools.projectRotationOnAxis(fullRotation, axis, actualRotation); EuclidCoreTestTools.assertQuaternionGeometricallyEquals(expectedRotation, actualRotation, 1.0e-10); } } }
private static void packExtrapolatedOrienation(RotationMatrixReadOnly from, RotationMatrixReadOnly to, double ratio, RotationMatrix toPack) { Quaternion invFrom = new Quaternion(from); invFrom.inverse(); Quaternion delFromTo = new Quaternion(); delFromTo.multiply(invFrom, new Quaternion(to)); AxisAngle delFromToAxisAngle = new AxisAngle(); AxisAngleConversion.convertQuaternionToAxisAngle(delFromTo, delFromToAxisAngle); AxisAngle delFromExtraAxisAngle = new AxisAngle(delFromToAxisAngle); double extrapolatedAngle = ratio * delFromToAxisAngle.getAngle(); delFromExtraAxisAngle.setAngle(extrapolatedAngle); AxisAngle toPackAxisAngle = new AxisAngle(from); toPackAxisAngle.multiply(delFromExtraAxisAngle); AxisAngle temp = new AxisAngle(from); temp.multiply(delFromToAxisAngle); RotationMatrixConversion.convertAxisAngleToMatrix(toPackAxisAngle, toPack); }
@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout=300000) public void testMultiplication() { Random random = new Random(1776L); YoVariableRegistry registry = new YoVariableRegistry("blop"); YoFrameQuaternion yoFrameQuaternion = new YoFrameQuaternion("test", worldFrame, registry); Quaternion quat4dActual = new Quaternion(), quat4dExpected = new Quaternion(); Quaternion quat4dA, quat4dB; FrameQuaternion frameOrientation = new FrameQuaternion(worldFrame); for (int i = 0; i < 1000; i++) { quat4dA = RandomGeometry.nextQuaternion(random); quat4dB = RandomGeometry.nextQuaternion(random); quat4dExpected.multiply(quat4dA, quat4dB); yoFrameQuaternion.set(quat4dA); yoFrameQuaternion.multiply(quat4dB); quat4dActual.set(yoFrameQuaternion); assertTrue(RotationTools.quaternionEpsilonEquals(quat4dExpected, quat4dActual, EPS)); yoFrameQuaternion.set(quat4dA); frameOrientation.set(quat4dB); yoFrameQuaternion.multiply(frameOrientation); quat4dActual.set(yoFrameQuaternion); assertTrue(RotationTools.quaternionEpsilonEquals(quat4dExpected, quat4dActual, EPS)); } }
quaternionFiltered.multiply(difference); set(quaternionFiltered);
qdiff.multiply(qtest); qdiff.normalize();
perturbationQuaternion.set(perturbationAxisAngle); Quaternion perturbedQuaternion = new Quaternion(); perturbedQuaternion.multiply(nominalQuaternion, perturbationQuaternion);
tempQuaternion.multiply(exp(-TOverThree, wb)); controlRotations[2].set(log(tempQuaternion));
expected.multiply(appended); actualOrientation.append(appended);
RotationTools.integrateAngularVelocity(angularVelocityVector, dt, integratedAngularVelocity); quaternionFromIntegration.set(orientationFromIntegration); quaternionFromIntegration.multiply(integratedAngularVelocity, quaternionFromIntegration); orientationFromIntegration.set(quaternionFromIntegration);
RotationTools.integrateAngularVelocity(angularVelocityVector, dt, integratedAngularVelocity); quaternionFromIntegration.set(orientationFromIntegration); quaternionFromIntegration.multiply(integratedAngularVelocity, quaternionFromIntegration); orientationFromIntegration.set(quaternionFromIntegration);