How to use

distance

method

in

us.ihmc.euclid.tuple4D.Quaternion

@Test
public void testDistance() throws Exception
{
 Random random = new Random(5321);
 for (int i = 0; i < ITERATIONS; i++)
 {
   YawPitchRoll firstYPR = EuclidCoreRandomTools.nextYawPitchRoll(random);
   YawPitchRoll secondYPR = EuclidCoreRandomTools.nextYawPitchRoll(random);
   Quaternion firstQ = new Quaternion(firstYPR);
   Quaternion secondQ = new Quaternion(secondYPR);
   assertEquals(firstQ.distance(secondQ), YawPitchRollTools.distance(firstYPR, secondYPR), EPSILON);
   assertEquals(firstQ.distance(secondQ), YawPitchRollTools.distance(firstYPR.getYaw(), firstYPR.getPitch(), firstYPR.getRoll(), secondYPR.getYaw(),
                                    secondYPR.getPitch(), secondYPR.getRoll()),
         EPSILON);
 }
}

@Test
public void testDistance() throws Exception
{
 Random random = new Random(32434L);
 for (int i = 0; i < ITERATIONS; i++)
 {
   AxisAngleReadOnly aa1 = createRandomAxisAngle(random);
   AxisAngleReadOnly aa2 = createRandomAxisAngle(random);
   Quaternion q1 = new Quaternion(aa1);
   Quaternion q2 = new Quaternion(aa2);
   double actualDistance = aa1.distance(aa2);
   double expectedDistance = q1.distance(q2);
   assertEquals(expectedDistance, actualDistance, getEpsilon());
   assertEquals(0.0, aa1.distance(aa1), getEpsilon());
 }
}

public static double getDistance(RigidBodyTransform from, RigidBodyTransform to, double positionWeight, double orientationWeight)
{
 Point3D pointFrom = new Point3D(from.getTranslationVector());
 Quaternion orientationFrom = new Quaternion(from.getRotationMatrix());
 Point3D pointTo = new Point3D(to.getTranslationVector());
 Quaternion orientationTo = new Quaternion(to.getRotationMatrix());
 double positionDistance = positionWeight * pointFrom.distance(pointTo);
 double orientationDistance = orientationWeight * orientationFrom.distance(orientationTo);
 double distance = positionDistance + orientationDistance;
 return distance;
}

double expectedDistance = q1.distance(q2);
assertEquals(expectedDistance, actualDistance, EPSILON);
assertEquals(0.0, aa1.distance(aa1), EPSILON);
double expectedDistance = q1.distance(q2);
assertEquals(expectedDistance, actualDistance, EPSILON);
assertEquals(0.0, aa1.distance(aa1), EPSILON);

double expectedDistance = Math.abs(EuclidCoreTools.trimAngleMinusPiToPi(q1.distance(q2)));
assertEquals(expectedDistance, actualDistance, EPS);
assertEquals(0.0, RotationMatrixTools.distance(m1, m1), EPS);