@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 = Math.abs(EuclidCoreTools.trimAngleMinusPiToPi(q1.distance(q2))); assertEquals(expectedDistance, actualDistance, EPS); assertEquals(0.0, RotationMatrixTools.distance(m1, m1), EPS);