Пример #1
0
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
Пример #2
0
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