How to use

multiply

method

in

us.ihmc.euclid.tuple4D.Quaternion

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);

Qip.set(0,.707106781186548,0,-.707106781186548);
Qip2.set(0,0,1,0);
Qip.multiply(Qip2);
Qsi.multiply(Qip);
qs.set(Qsi.getS());
qx.set(Qsi.getX());

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);