def test_slerp(Rs): slerp_precision = 4.e-15 ones = [quaternion.one, quaternion.x, quaternion.y, quaternion.z, -quaternion.x, -quaternion.y, -quaternion.z] # Check extremes for Q1 in ones: assert quaternion.rotation_chordal_distance(quaternion.slerp(Q1, Q1, 0.0), Q1) < slerp_precision assert quaternion.rotation_chordal_distance(quaternion.slerp(Q1, Q1, 1.0), Q1) < slerp_precision assert quaternion.rotation_chordal_distance(quaternion.slerp(Q1, -Q1, 0.0), Q1) < slerp_precision assert quaternion.rotation_chordal_distance(quaternion.slerp(Q1, -Q1, 1.0), Q1) < slerp_precision for Q2 in ones: assert quaternion.rotation_chordal_distance(quaternion.slerp(Q1, Q2, 0.0), Q1) < slerp_precision assert quaternion.rotation_chordal_distance(quaternion.slerp(Q1, Q2, 1.0), Q2) < slerp_precision assert quaternion.rotation_chordal_distance(quaternion.slerp(Q1, -Q2, 0.0), Q1) < slerp_precision assert quaternion.rotation_chordal_distance(quaternion.slerp(Q1, -Q2, 1.0), -Q2) < slerp_precision assert quaternion.rotation_chordal_distance(quaternion.slerp(Q2, Q1, 0.0), Q2) < slerp_precision assert quaternion.rotation_chordal_distance(quaternion.slerp(Q2, Q1, 1.0), Q1) < slerp_precision # Test simple increases in each dimension for Q2 in ones[1:]: for t in np.linspace(0.0, 1.0, num=100, endpoint=True): assert quaternion.rotation_chordal_distance(quaternion.slerp(quaternion.one, Q2, t), (np.cos(np.pi * t / 2) * quaternion.one + np.sin( np.pi * t / 2) * Q2)) < slerp_precision # Test that (slerp of rotated rotors) is (rotated slerp of rotors) for R in Rs: for Q2 in ones[1:]: for t in np.linspace(0.0, 1.0, num=100, endpoint=True): assert quaternion.rotation_chordal_distance(R * quaternion.slerp(quaternion.one, Q2, t), quaternion.slerp(R * quaternion.one, R * Q2, t)) < slerp_precision
def test_metrics(Rs): metric_precision = 4.e-15 # Check non-negativity for R1 in Rs: for R2 in Rs: assert quaternion.rotor_chordal_distance(R1, R2) >= 0. assert quaternion.rotor_intrinsic_distance(R1, R2) >= 0. assert quaternion.rotation_chordal_distance(R1, R2) >= 0. assert quaternion.rotation_intrinsic_distance(R1, R2) >= 0. # Check discernibility for R1 in Rs: for R2 in Rs: assert bool(quaternion.rotor_chordal_distance(R1, R2) > 0.) != bool(R1 == R2) assert bool(quaternion.rotor_intrinsic_distance(R1, R2) > 0.) != bool(R1 == R2) assert bool(quaternion.rotation_chordal_distance(R1, R2) > 0.) != bool(R1 == R2 or R1 == -R2) assert bool(quaternion.rotation_intrinsic_distance(R1, R2) > 0.) != bool(R1 == R2 or R1 == -R2) # Check symmetry for R1 in Rs: for R2 in Rs: assert abs(quaternion.rotor_chordal_distance(R1, R2) - quaternion.rotor_chordal_distance(R2, R1)) < metric_precision assert abs(quaternion.rotor_intrinsic_distance(R1, R2) - quaternion.rotor_intrinsic_distance(R2, R1)) < metric_precision assert abs(quaternion.rotation_chordal_distance(R1, R2) - quaternion.rotation_chordal_distance(R2, R1)) < metric_precision assert abs(quaternion.rotation_intrinsic_distance(R1, R2) - quaternion.rotation_intrinsic_distance(R2, R1)) < metric_precision # Check triangle inequality for R1 in Rs: for R2 in Rs: for R3 in Rs: assert quaternion.rotor_chordal_distance(R1, R3) - metric_precision \ <= (quaternion.rotor_chordal_distance(R1, R2) + quaternion.rotor_chordal_distance(R2, R3)) assert quaternion.rotor_chordal_distance(R1, R3) - metric_precision \ <= (quaternion.rotor_chordal_distance(R1, R2) + quaternion.rotor_chordal_distance(R2, R3)) assert quaternion.rotation_intrinsic_distance(R1, R3) - metric_precision \ <= ( quaternion.rotation_intrinsic_distance(R1, R2) + quaternion.rotation_intrinsic_distance(R2, R3)) assert quaternion.rotation_intrinsic_distance(R1, R3) - metric_precision \ <= ( quaternion.rotation_intrinsic_distance(R1, R2) + quaternion.rotation_intrinsic_distance(R2, R3)) # Check distances from self or -self for R in Rs: # All distances from self should be 0.0 assert quaternion.rotor_chordal_distance(R, R) == 0.0 assert quaternion.rotor_intrinsic_distance(R, R) == 0.0 assert quaternion.rotation_chordal_distance(R, R) == 0.0 assert quaternion.rotation_intrinsic_distance(R, R) == 0.0 # Chordal rotor distance from -self should be 2 assert abs(quaternion.rotor_chordal_distance(R, -R) - 2.0) < metric_precision # Intrinsic rotor distance from -self should be 2pi assert abs(quaternion.rotor_intrinsic_distance(R, -R) - 2.0 * np.pi) < metric_precision # Rotation distances from -self should be 0 assert quaternion.rotation_chordal_distance(R, -R) == 0.0 assert quaternion.rotation_intrinsic_distance(R, -R) == 0.0 # We expect the chordal distance to be smaller than the intrinsic distance (or equal, if the distance is zero) for R in Rs: assert ( quaternion.rotor_chordal_distance(quaternion.one, R) < quaternion.rotor_intrinsic_distance(quaternion.one, R) or R == quaternion.one) # Check invariance under overall rotations: d(R1, R2) = d(R3*R1, R3*R2) = d(R1*R3, R2*R3) for R1 in Rs: for R2 in Rs: for R3 in Rs: assert abs(quaternion.rotor_chordal_distance(R1, R2) - quaternion.rotor_chordal_distance(R3 * R1, R3 * R2)) < metric_precision assert abs(quaternion.rotor_chordal_distance(R1, R2) - quaternion.rotor_chordal_distance(R1 * R3, R2 * R3)) < metric_precision assert abs(quaternion.rotation_intrinsic_distance(R1, R2) - quaternion.rotation_intrinsic_distance(R3 * R1, R3 * R2)) < metric_precision assert abs(quaternion.rotation_intrinsic_distance(R1, R2) - quaternion.rotation_intrinsic_distance(R1 * R3, R2 * R3)) < metric_precision