Python numpy.quaternion() Examples

The following are 30 code examples for showing how to use numpy.quaternion(). These examples are extracted from open source projects. You can vote up the ones you like or vote down the ones you don't like, and go to the original project or source file by following the links above each example.

You may check out the related API usage on the sidebar.

You may also want to check out all available functions/classes of the module numpy , or try the search function .

Example 1
Project: quaternion   Author: moble   File: quaternion_time_series.py    License: MIT License 7 votes vote down vote up
def slerp(R1, R2, t1, t2, t_out):
    """Spherical linear interpolation of rotors

    This function uses a simpler interface than the more fundamental
    `slerp_evaluate` and `slerp_vectorized` functions.  The latter
    are fast, being implemented at the C level, but take input `tau`
    instead of time.  This function adjusts the time accordingly.

    Parameters
    ----------
    R1: quaternion
        Quaternion at beginning of interpolation
    R2: quaternion
        Quaternion at end of interpolation
    t1: float
        Time corresponding to R1
    t2: float
        Time corresponding to R2
    t_out: float or array of floats
        Times to which the rotors should be interpolated


    """
    tau = (t_out-t1)/(t2-t1)
    return np.slerp_vectorized(R1, R2, tau) 
Example 2
Project: quaternion   Author: moble   File: __init__.py    License: MIT License 6 votes vote down vote up
def as_rotation_vector(q):
    """Convert input quaternion to the axis-angle representation

    Note that if any of the input quaternions has norm zero, no error is
    raised, but NaNs will appear in the output.

    Parameters
    ----------
    q: quaternion or array of quaternions
        The quaternion(s) need not be normalized, but must all be nonzero

    Returns
    -------
    rot: float array
        Output shape is q.shape+(3,).  Each vector represents the axis of
        the rotation, with norm proportional to the angle of the rotation in
        radians.

    """
    return as_float_array(2*np.log(np.normalized(q)))[..., 1:] 
Example 3
Project: quaternion   Author: moble   File: __init__.py    License: MIT License 6 votes vote down vote up
def from_rotation_vector(rot):
    """Convert input 3-vector in axis-angle representation to unit quaternion

    Parameters
    ----------
    rot: (Nx3) float array
        Each vector represents the axis of the rotation, with norm
        proportional to the angle of the rotation in radians.

    Returns
    -------
    q: array of quaternions
        Unit quaternions resulting in rotations corresponding to input
        rotations.  Output shape is rot.shape[:-1].

    """
    rot = np.array(rot, copy=False)
    quats = np.zeros(rot.shape[:-1]+(4,))
    quats[..., 1:] = rot[...]/2
    quats = as_quat_array(quats)
    return np.exp(quats) 
Example 4
Project: quaternion   Author: moble   File: means.py    License: MIT License 6 votes vote down vote up
def mean_rotor_in_chordal_metric(R, t=None):
    """Return rotor that is closest to all R in the least-squares sense

    This can be done (quasi-)analytically because of the simplicity of
    the chordal metric function.  It is assumed that the input R values
    all are normalized (or at least have the same norm).

    Note that the `t` argument is optional.  If it is present, the
    times are used to weight the corresponding integral.  If it is not
    present, a simple sum is used instead (which may be slightly
    faster).  However, because a spline is used to do this integral,
    the number of input points must be at least 4 (one more than the
    degree of the spline).

    """
    import numpy as np
    from . import as_float_array
    from .calculus import definite_integral
    if t is None:
        return np.sum(R).normalized()
    if len(t) < 4 or len(R) < 4:
        raise ValueError('Input arguments must have length greater than 3; their lengths are {0} and {1}.'.format(len(R), len(t)))
    mean = definite_integral(as_float_array(R), t)
    return np.quaternion(*mean).normalized() 
Example 5
Project: quaternion   Author: moble   File: test_quaternion.py    License: MIT License 6 votes vote down vote up
def test_isclose():
    from quaternion import x, y

    assert np.array_equal(quaternion.isclose([1e10*x, 1e-7*y], [1.00001e10*x, 1e-8*y], rtol=1.e-5, atol=2.e-8),
                          np.array([True, False]))
    assert np.array_equal(quaternion.isclose([1e10*x, 1e-8*y], [1.00001e10*x, 1e-9*y], rtol=1.e-5, atol=2.e-8),
                          np.array([True, True]))
    assert np.array_equal(quaternion.isclose([1e10*x, 1e-8*y], [1.0001e10*x, 1e-9*y], rtol=1.e-5, atol=2.e-8),
                          np.array([False, True]))
    assert np.array_equal(quaternion.isclose([x, np.nan*y], [x, np.nan*y]),
                          np.array([True, False]))
    assert np.array_equal(quaternion.isclose([x, np.nan*y], [x, np.nan*y], equal_nan=True),
                          np.array([True, True]))

    np.random.seed(1234)
    a = quaternion.as_quat_array(np.random.random((3, 5, 4)))
    assert quaternion.allclose(1e10 * a, 1.00001e10 * a, rtol=1.e-5, atol=2.e-8, verbose=True) == True
    assert quaternion.allclose(1e-7 * a, 1e-8 * a, rtol=1.e-5, atol=2.e-8) == False
    assert quaternion.allclose(1e10 * a, 1.00001e10 * a, rtol=1.e-5, atol=2.e-8, verbose=True) == True
    assert quaternion.allclose(1e-8 * a, 1e-9 * a, rtol=1.e-5, atol=2.e-8, verbose=True) == True
    assert quaternion.allclose(1e10 * a, 1.0001e10 * a, rtol=1.e-5, atol=2.e-8) == False
    assert quaternion.allclose(1e-8 * a, 1e-9 * a, rtol=1.e-5, atol=2.e-8, verbose=True) == True
    assert quaternion.allclose(np.nan * a, np.nan * a) == False
    assert quaternion.allclose(np.nan * a, np.nan * a, equal_nan=True, verbose=True) == True 
Example 6
Project: quaternion   Author: moble   File: test_quaternion.py    License: MIT License 6 votes vote down vote up
def test_from_spherical_coords():
    np.random.seed(1843)
    random_angles = [[np.random.uniform(-np.pi, np.pi), np.random.uniform(-np.pi, np.pi)]
                     for i in range(5000)]
    for vartheta, varphi in random_angles:
        q = quaternion.from_spherical_coords(vartheta, varphi)
        assert abs((np.quaternion(0, 0, 0, varphi / 2.).exp() * np.quaternion(0, 0, vartheta / 2., 0).exp())
                   - q) < 1.e-15
        xprime = q * quaternion.x * q.inverse()
        yprime = q * quaternion.y * q.inverse()
        zprime = q * quaternion.z * q.inverse()
        nhat = np.quaternion(0.0, math.sin(vartheta)*math.cos(varphi), math.sin(vartheta)*math.sin(varphi),
                             math.cos(vartheta))
        thetahat = np.quaternion(0.0, math.cos(vartheta)*math.cos(varphi), math.cos(vartheta)*math.sin(varphi),
                                 -math.sin(vartheta))
        phihat = np.quaternion(0.0, -math.sin(varphi), math.cos(varphi), 0.0)
        assert abs(xprime - thetahat) < 1.e-15
        assert abs(yprime - phihat) < 1.e-15
        assert abs(zprime - nhat) < 1.e-15
    assert np.max(np.abs(quaternion.from_spherical_coords(random_angles)
                         - np.array([quaternion.from_spherical_coords(vartheta, varphi)
                                     for vartheta, varphi in random_angles]))) < 1.e-15 
Example 7
Project: quaternion   Author: moble   File: test_quaternion.py    License: MIT License 6 votes vote down vote up
def test_as_euler_angles():
    np.random.seed(1843)
    random_angles = [[np.random.uniform(-np.pi, np.pi),
                      np.random.uniform(-np.pi, np.pi),
                      np.random.uniform(-np.pi, np.pi)]
                     for i in range(5000)]
    for alpha, beta, gamma in random_angles:
        R1 = quaternion.from_euler_angles(alpha, beta, gamma)
        R2 = quaternion.from_euler_angles(*list(quaternion.as_euler_angles(R1)))
        d = quaternion.rotation_intrinsic_distance(R1, R2)
        assert d < 6e3*eps, ((alpha, beta, gamma), R1, R2, d)  # Can't use allclose here; we don't care about rotor sign
    q0 = quaternion.quaternion(0, 0.6, 0.8, 0)
    assert q0.norm() == 1.0
    assert abs(q0 - quaternion.from_euler_angles(*list(quaternion.as_euler_angles(q0)))) < 1.e-15


# Unary bool returners 
Example 8
Project: quaternion   Author: moble   File: test_quaternion.py    License: MIT License 6 votes vote down vote up
def test_quaternion_parity_conjugates(Qs):
    for q in Qs[Qs_finite]:
        assert q.x_parity_conjugate() == np.quaternion(q.w, q.x, -q.y, -q.z)
        assert q.y_parity_conjugate() == np.quaternion(q.w, -q.x, q.y, -q.z)
        assert q.z_parity_conjugate() == np.quaternion(q.w, -q.x, -q.y, q.z)
        assert q.parity_conjugate() == np.quaternion(q.w, q.x, q.y, q.z)
    assert np.array_equal(np.x_parity_conjugate(Qs[Qs_finite]),
                          np.array([q.x_parity_conjugate() for q in Qs[Qs_finite]]))
    assert np.array_equal(np.y_parity_conjugate(Qs[Qs_finite]),
                          np.array([q.y_parity_conjugate() for q in Qs[Qs_finite]]))
    assert np.array_equal(np.z_parity_conjugate(Qs[Qs_finite]),
                          np.array([q.z_parity_conjugate() for q in Qs[Qs_finite]]))
    assert np.array_equal(np.parity_conjugate(Qs[Qs_finite]), np.array([q.parity_conjugate() for q in Qs[Qs_finite]]))


# Quaternion-quaternion binary quaternion returners 
Example 9
Project: quaternion   Author: moble   File: test_quaternion.py    License: MIT License 6 votes vote down vote up
def test_quaternion_multiply_ufunc(Qs):
    ufunc_binary_utility(np.array([quaternion.one]), Qs[Qs_finite], operator.mul)
    ufunc_binary_utility(Qs[Qs_finite], np.array([quaternion.one]), operator.mul)
    ufunc_binary_utility(np.array([1.0]), Qs[Qs_finite], operator.mul)
    ufunc_binary_utility(Qs[Qs_finite], np.array([1.0]), operator.mul)
    ufunc_binary_utility(np.array([1]), Qs[Qs_finite], operator.mul)
    ufunc_binary_utility(Qs[Qs_finite], np.array([1]), operator.mul)
    ufunc_binary_utility(np.array([0.0]), Qs[Qs_finite], operator.mul)
    ufunc_binary_utility(Qs[Qs_finite], np.array([0.0]), operator.mul)
    ufunc_binary_utility(np.array([0]), Qs[Qs_finite], operator.mul)
    ufunc_binary_utility(Qs[Qs_finite], np.array([0]), operator.mul)

    ufunc_binary_utility(np.array([-3, -2.3, -1.2, -1.0, 0.0, 0, 1.0, 1, 1.2, 2.3, 3]),
                         Qs[Qs_finite], operator.mul)
    ufunc_binary_utility(Qs[Qs_finite],
                         np.array([-3, -2.3, -1.2, -1.0, 0.0, 0, 1.0, 1, 1.2, 2.3, 3]), operator.mul)

    ufunc_binary_utility(Qs[Qs_finite], Qs[Qs_finite], operator.mul) 
Example 10
Project: quaternion   Author: moble   File: test_quaternion.py    License: MIT License 6 votes vote down vote up
def test_quaternion_divide_ufunc(Qs):
    ufunc_binary_utility(np.array([quaternion.one]), Qs[Qs_finitenonzero], operator.truediv)
    ufunc_binary_utility(Qs[Qs_finite], np.array([quaternion.one]), operator.truediv)
    ufunc_binary_utility(np.array([1.0]), Qs[Qs_finitenonzero], operator.truediv)
    ufunc_binary_utility(Qs[Qs_finite], np.array([1.0]), operator.truediv)
    ufunc_binary_utility(np.array([1]), Qs[Qs_finitenonzero], operator.truediv)
    ufunc_binary_utility(Qs[Qs_finite], np.array([1]), operator.truediv)
    ufunc_binary_utility(np.array([0.0]), Qs[Qs_finitenonzero], operator.truediv)
    ufunc_binary_utility(np.array([0]), Qs[Qs_finitenonzero], operator.truediv)

    ufunc_binary_utility(np.array([-3, -2.3, -1.2, -1.0, 0.0, 0, 1.0, 1, 1.2, 2.3, 3]),
                         Qs[Qs_finitenonzero], operator.truediv)
    ufunc_binary_utility(Qs[Qs_finitenonzero],
                         np.array([-3, -2.3, -1.2, -1.0, 1.0, 1, 1.2, 2.3, 3]), operator.truediv)

    ufunc_binary_utility(Qs[Qs_finite], Qs[Qs_finitenonzero], operator.truediv)
    ufunc_binary_utility(Qs[Qs_finite], Qs[Qs_finitenonzero], operator.floordiv)

    ufunc_binary_utility(Qs[Qs_finitenonzero], Qs[Qs_finitenonzero], operator.truediv)
    ufunc_binary_utility(Qs[Qs_finitenonzero], Qs[Qs_finitenonzero], operator.floordiv) 
Example 11
Project: AlignNet-3D   Author: grossjohannes   File: pointcloud.py    License: BSD 3-Clause "New" or "Revised" License 6 votes vote down vote up
def clean_color(color):
    if color == 'red':
        color = [255, 0, 0]
    elif color == 'green':
        color = [0, 255, 0]
    elif color == 'blue':
        color = [0, 0, 255]
    elif color == 'yellow':
        color = [255, 255, 0]
    elif color == 'pink':
        color = [255, 0, 255]
    elif color == 'cyan':
        color = [0, 255, 255]
    elif color == 'white':
        color = [255, 255, 255]
    return color


####### Dataset generation


# https://stackoverflow.com/questions/31600717/how-to-generate-a-random-quaternion-quickly
# http://planning.cs.uiuc.edu/node198.html 
Example 12
Project: spherical_functions   Author: moble   File: test_WignerD.py    License: MIT License 6 votes vote down vote up
def test_Wigner_D_element_values(special_angles, ell_max):
    LMpM = sf.LMpM_range_half_integer(0, ell_max // 2)
    # Compare with more explicit forms given in Euler angles
    print("")
    for alpha in special_angles:
        print("\talpha={0}".format(alpha))  # Need to show some progress to Travis
        for beta in special_angles:
            print("\t\tbeta={0}".format(beta))
            for gamma in special_angles:
                a = np.conjugate(np.array([slow_Wigner_D_element(alpha, beta, gamma, ell, mp, m) for ell,mp,m in LMpM]))
                b = sf.Wigner_D_element(quaternion.from_euler_angles(alpha, beta, gamma), LMpM)
                # if not np.allclose(a, b,
                #     atol=ell_max ** 6 * precision_Wigner_D_element,
                #     rtol=ell_max ** 6 * precision_Wigner_D_element):
                #     for i in range(min(a.shape[0], 100)):
                #         print(LMpM[i], "\t", abs(a[i]-b[i]), "\t\t", a[i], "\t", b[i])
                assert np.allclose(a, b,
                    atol=ell_max ** 6 * precision_Wigner_D_element,
                    rtol=ell_max ** 6 * precision_Wigner_D_element) 
Example 13
Project: spherical_functions   Author: moble   File: test_SWSHs.py    License: MIT License 6 votes vote down vote up
def test_SWSH_spin_behavior(Rs, special_angles, ell_max):
    # We expect that the SWSHs behave according to
    # sYlm( R * exp(gamma*z/2) ) = sYlm(R) * exp(-1j*s*gamma)
    # See http://moble.github.io/spherical_functions/SWSHs.html#fn:2
    # for a more detailed explanation
    # print("")
    for i, R in enumerate(Rs):
        # print("\t{0} of {1}: R = {2}".format(i, len(Rs), R))
        for gamma in special_angles:
            for ell in range(ell_max + 1):
                for s in range(-ell, ell + 1):
                    LM = np.array([[ell, m] for m in range(-ell, ell + 1)])
                    Rgamma = R * np.quaternion(math.cos(gamma / 2.), 0, 0, math.sin(gamma / 2.))
                    sYlm1 = sf.SWSH(Rgamma, s, LM)
                    sYlm2 = sf.SWSH(R, s, LM) * cmath.exp(-1j * s * gamma)
                    # print(R, gamma, ell, s, np.max(np.abs(sYlm1-sYlm2)))
                    assert np.allclose(sYlm1, sYlm2, atol=ell ** 6 * precision_SWSH, rtol=ell ** 6 * precision_SWSH) 
Example 14
Project: spherical_functions   Author: moble   File: test_SWSHs.py    License: MIT License 6 votes vote down vote up
def test_SWSH_grid(special_angles, ell_max):
    LM = sf.LM_range(0, ell_max)

    # Test flat array arrangement
    R_grid = np.array([quaternion.from_euler_angles(alpha, beta, gamma).normalized()
                       for alpha in special_angles
                       for beta in special_angles
                       for gamma in special_angles])
    for s in range(-ell_max + 1, ell_max):
        values_explicit = np.array([sf.SWSH(R, s, LM) for R in R_grid])
        values_grid = sf.SWSH_grid(R_grid, s, ell_max)
        assert np.array_equal(values_explicit, values_grid)

    # Test nested array arrangement
    R_grid = np.array([[[quaternion.from_euler_angles(alpha, beta, gamma)
                         for alpha in special_angles]
                        for beta in special_angles]
                       for gamma in special_angles])
    for s in range(-ell_max + 1, ell_max):
        values_explicit = np.array([[[sf.SWSH(R, s, LM) for R in R1] for R1 in R2] for R2 in R_grid])
        values_grid = sf.SWSH_grid(R_grid, s, ell_max)
        assert np.array_equal(values_explicit, values_grid) 
Example 15
Project: habitat-api   Author: facebookresearch   File: geometry_utils.py    License: MIT License 6 votes vote down vote up
def quaternion_from_two_vectors(v0: np.array, v1: np.array) -> np.quaternion:
    r"""Computes the quaternion representation of v1 using v0 as the origin.
    """
    v0 = v0 / np.linalg.norm(v0)
    v1 = v1 / np.linalg.norm(v1)
    c = v0.dot(v1)
    # Epsilon prevents issues at poles.
    if c < (-1 + EPSILON):
        c = max(c, -1)
        m = np.stack([v0, v1], 0)
        _, _, vh = np.linalg.svd(m, full_matrices=True)
        axis = vh.T[:, 2]
        w2 = (1 + c) * 0.5
        w = np.sqrt(w2)
        axis = axis * np.sqrt(1 - w2)
        return np.quaternion(w, *axis)

    axis = np.cross(v0, v1)
    s = np.sqrt((1 + c) * 2)
    return np.quaternion(s * 0.5, *(axis / s)) 
Example 16
Project: quaternion   Author: moble   File: quaternion_time_series.py    License: MIT License 5 votes vote down vote up
def minimal_rotation(R, t, iterations=2):
    """Adjust frame so that there is no rotation about z' axis

    The output of this function is a frame that rotates the z axis onto the same z' axis as the
    input frame, but with minimal rotation about that axis.  This is done by pre-composing the input
    rotation with a rotation about the z axis through an angle gamma, where

        dgamma/dt = 2*(dR/dt * z * R.conjugate()).w

    This ensures that the angular velocity has no component along the z' axis.

    Note that this condition becomes easier to impose the closer the input rotation is to a
    minimally rotating frame, which means that repeated application of this function improves its
    accuracy.  By default, this function is iterated twice, though a few more iterations may be
    called for.

    Parameters
    ==========
    R: quaternion array
        Time series describing rotation
    t: float array
        Corresponding times at which R is measured
    iterations: int [defaults to 2]
        Repeat the minimization to refine the result

    """
    from scipy.interpolate import InterpolatedUnivariateSpline as spline
    if iterations == 0:
        return R
    R = quaternion.as_float_array(R)
    Rdot = np.empty_like(R)
    for i in range(4):
        Rdot[:, i] = spline(t, R[:, i]).derivative()(t)
    R = quaternion.from_float_array(R)
    Rdot = quaternion.from_float_array(Rdot)
    halfgammadot = quaternion.as_float_array(Rdot * quaternion.z * np.conjugate(R))[:, 0]
    halfgamma = spline(t, halfgammadot).antiderivative()(t)
    Rgamma = np.exp(quaternion.z * halfgamma)
    return minimal_rotation(R * Rgamma, t, iterations=iterations-1) 
Example 17
Project: quaternion   Author: moble   File: quaternion_time_series.py    License: MIT License 5 votes vote down vote up
def angular_velocity(R, t):
    from scipy.interpolate import InterpolatedUnivariateSpline as spline
    R = quaternion.as_float_array(R)
    Rdot = np.empty_like(R)
    for i in range(4):
        Rdot[:, i] = spline(t, R[:, i]).derivative()(t)
    R = quaternion.from_float_array(R)
    Rdot = quaternion.from_float_array(Rdot)
    return quaternion.as_float_array(2*Rdot/R)[:, 1:] 
Example 18
Project: quaternion   Author: moble   File: __init__.py    License: MIT License 5 votes vote down vote up
def as_float_array(a):
    """View the quaternion array as an array of floats

    This function is fast (of order 1 microsecond) because no data is
    copied; the returned quantity is just a "view" of the original.

    The output view has one more dimension (of size 4) than the input
    array, but is otherwise the same shape.

    """
    return np.asarray(a, dtype=np.quaternion).view((np.double, 4)) 
Example 19
Project: quaternion   Author: moble   File: test_quaternion.py    License: MIT License 5 votes vote down vote up
def allclose(*args, **kwargs):
    kwargs.update({'verbose': True})
    return quaternion.allclose(*args, **kwargs) 
Example 20
Project: quaternion   Author: moble   File: test_quaternion.py    License: MIT License 5 votes vote down vote up
def Rs():
    ones = [0, -1., 1.]
    rs = [np.quaternion(w, x, y, z).normalized() for w in ones for x in ones for y in ones for z in ones][1:]
    np.random.seed(1842)
    rs = rs + [r.normalized() for r in [np.quaternion(np.random.uniform(-1, 1), np.random.uniform(-1, 1),
                                                      np.random.uniform(-1, 1), np.random.uniform(-1, 1)) for i in range(20)]]
    return np.array(rs) 
Example 21
Project: quaternion   Author: moble   File: test_quaternion.py    License: MIT License 5 votes vote down vote up
def test_quaternion_members():
    Q = quaternion.quaternion(1.1, 2.2, 3.3, 4.4)
    assert Q.real == 1.1
    assert Q.w == 1.1
    assert Q.x == 2.2
    assert Q.y == 3.3
    assert Q.z == 4.4 
Example 22
Project: quaternion   Author: moble   File: test_quaternion.py    License: MIT License 5 votes vote down vote up
def test_quaternion_constructors():
    Q = quaternion.quaternion(2.2, 3.3, 4.4)
    assert Q.real == 0.0
    assert Q.w == 0.0
    assert Q.x == 2.2
    assert Q.y == 3.3
    assert Q.z == 4.4
    
    P = quaternion.quaternion(1.1, 2.2, 3.3, 4.4)
    Q = quaternion.quaternion(P)
    assert Q.real == 1.1
    assert Q.w == 1.1
    assert Q.x == 2.2
    assert Q.y == 3.3
    assert Q.z == 4.4

    Q = quaternion.quaternion(1.1)
    assert Q.real == 1.1
    assert Q.w == 1.1
    assert Q.x == 0.0
    assert Q.y == 0.0
    assert Q.z == 0.0

    Q = quaternion.quaternion(0.0)
    assert Q.real == 0.0
    assert Q.w == 0.0
    assert Q.x == 0.0
    assert Q.y == 0.0
    assert Q.z == 0.0

    with pytest.raises(TypeError):
        quaternion.quaternion(1.2, 3.4)

    with pytest.raises(TypeError):
        quaternion.quaternion(1.2, 3.4, 5.6, 7.8, 9.0) 
Example 23
Project: quaternion   Author: moble   File: test_quaternion.py    License: MIT License 5 votes vote down vote up
def test_constants():
    assert quaternion.one == np.quaternion(1.0, 0.0, 0.0, 0.0)
    assert quaternion.x == np.quaternion(0.0, 1.0, 0.0, 0.0)
    assert quaternion.y == np.quaternion(0.0, 0.0, 1.0, 0.0)
    assert quaternion.z == np.quaternion(0.0, 0.0, 0.0, 1.0) 
Example 24
Project: quaternion   Author: moble   File: test_quaternion.py    License: MIT License 5 votes vote down vote up
def test_as_rotation_matrix(Rs):
    def quat_mat(quat):
        return np.array([(quat * v * quat.inverse()).vec for v in [quaternion.x, quaternion.y, quaternion.z]]).T

    def quat_mat_vec(quats):
        mat_vec = np.array([quaternion.as_float_array(quats * v * np.reciprocal(quats))[..., 1:]
                            for v in [quaternion.x, quaternion.y, quaternion.z]])
        return np.transpose(mat_vec, tuple(range(mat_vec.ndim))[1:-1]+(-1, 0))

    with pytest.raises(ZeroDivisionError):
        quaternion.as_rotation_matrix(quaternion.zero)

    for R in Rs:
        # Test correctly normalized rotors:
        assert allclose(quat_mat(R), quaternion.as_rotation_matrix(R), atol=2*eps)
        # Test incorrectly normalized rotors:
        assert allclose(quat_mat(R), quaternion.as_rotation_matrix(1.1*R), atol=2*eps)

    Rs0 = Rs.copy()
    Rs0[Rs.shape[0]//2] = quaternion.zero
    with pytest.raises(ZeroDivisionError):
        quaternion.as_rotation_matrix(Rs0)

    # Test correctly normalized rotors:
    assert allclose(quat_mat_vec(Rs), quaternion.as_rotation_matrix(Rs), atol=2*eps)
    # Test incorrectly normalized rotors:
    assert allclose(quat_mat_vec(Rs), quaternion.as_rotation_matrix(1.1*Rs), atol=2*eps)

    # Simply test that this function succeeds and returns the right shape
    assert quaternion.as_rotation_matrix(Rs.reshape((2, 5, 10))).shape == (2, 5, 10, 3, 3) 
Example 25
Project: quaternion   Author: moble   File: test_quaternion.py    License: MIT License 5 votes vote down vote up
def test_from_rotation_matrix(Rs):
    try:
        from scipy import linalg
        have_linalg = True
    except ImportError:
        have_linalg = False

    for nonorthogonal in [True, False]:
        if nonorthogonal and have_linalg:
            rot_mat_eps = 10*eps
        else:
            rot_mat_eps = 5*eps
        for i, R1 in enumerate(Rs):
            R2 = quaternion.from_rotation_matrix(quaternion.as_rotation_matrix(R1), nonorthogonal=nonorthogonal)
            d = quaternion.rotation_intrinsic_distance(R1, R2)
            assert d < rot_mat_eps, (i, R1, R2, d)  # Can't use allclose here; we don't care about rotor sign

        Rs2 = quaternion.from_rotation_matrix(quaternion.as_rotation_matrix(Rs), nonorthogonal=nonorthogonal)
        for R1, R2 in zip(Rs, Rs2):
            d = quaternion.rotation_intrinsic_distance(R1, R2)
            assert d < rot_mat_eps, (R1, R2, d)  # Can't use allclose here; we don't care about rotor sign

        Rs3 = Rs.reshape((2, 5, 10))
        Rs4 = quaternion.from_rotation_matrix(quaternion.as_rotation_matrix(Rs3))
        for R3, R4 in zip(Rs3.flatten(), Rs4.flatten()):
            d = quaternion.rotation_intrinsic_distance(R3, R4)
            assert d < rot_mat_eps, (R3, R4, d)  # Can't use allclose here; we don't care about rotor sign 
Example 26
Project: quaternion   Author: moble   File: test_quaternion.py    License: MIT License 5 votes vote down vote up
def test_as_rotation_vector():
    np.random.seed(1234)
    n_tests = 1000
    vecs = np.random.uniform(high=math.pi/math.sqrt(3), size=n_tests*3).reshape((n_tests, 3))
    quats = np.zeros(vecs.shape[:-1]+(4,))
    quats[..., 1:] = vecs[...]
    quats = quaternion.as_quat_array(quats)
    quats = np.exp(quats/2)
    quat_vecs = quaternion.as_rotation_vector(quats)
    assert allclose(quat_vecs, vecs) 
Example 27
Project: quaternion   Author: moble   File: test_quaternion.py    License: MIT License 5 votes vote down vote up
def test_from_rotation_vector():
    np.random.seed(1234)
    n_tests = 1000
    vecs = np.random.uniform(high=math.pi/math.sqrt(3), size=n_tests*3).reshape((n_tests, 3))
    quats = np.zeros(vecs.shape[:-1]+(4,))
    quats[..., 1:] = vecs[...]
    quats = quaternion.as_quat_array(quats)
    quats = np.exp(quats/2)
    quat_vecs = quaternion.as_rotation_vector(quats)
    quats2 = quaternion.from_rotation_vector(quat_vecs)
    assert allclose(quats, quats2) 
Example 28
Project: quaternion   Author: moble   File: test_quaternion.py    License: MIT License 5 votes vote down vote up
def test_as_spherical_coords(Rs):
    np.random.seed(1843)
    # First test on rotors that are precisely spherical-coordinate rotors
    random_angles = [[np.random.uniform(0, np.pi), np.random.uniform(0, 2*np.pi)]
                     for i in range(5000)]
    for vartheta, varphi in random_angles:
        vartheta2, varphi2 = quaternion.as_spherical_coords(quaternion.from_spherical_coords(vartheta, varphi))
        varphi2 = (varphi2 + 2*np.pi) if varphi2 < 0 else varphi2
        assert abs(vartheta - vartheta2) < 1e-12, ((vartheta, varphi), (vartheta2, varphi2))
        assert abs(varphi - varphi2) < 1e-12, ((vartheta, varphi), (vartheta2, varphi2))
    # Now test that arbitrary rotors rotate z to the appropriate location
    for R in Rs:
        vartheta, varphi = quaternion.as_spherical_coords(R)
        R2 = quaternion.from_spherical_coords(vartheta, varphi)
        assert (R*quaternion.z*R.inverse() - R2*quaternion.z*R2.inverse()).abs() < 4e-15, (R, R2, (vartheta, varphi)) 
Example 29
Project: quaternion   Author: moble   File: test_quaternion.py    License: MIT License 5 votes vote down vote up
def test_from_euler_angles():
    np.random.seed(1843)
    random_angles = [[np.random.uniform(-np.pi, np.pi),
                      np.random.uniform(-np.pi, np.pi),
                      np.random.uniform(-np.pi, np.pi)]
                     for i in range(5000)]
    for alpha, beta, gamma in random_angles:
        assert abs((np.quaternion(0, 0, 0, alpha / 2.).exp()
                    * np.quaternion(0, 0, beta / 2., 0).exp()
                    * np.quaternion(0, 0, 0, gamma / 2.).exp()
                   )
                   - quaternion.from_euler_angles(alpha, beta, gamma)) < 1.e-15
    assert np.max(np.abs(quaternion.from_euler_angles(random_angles)
                         - np.array([quaternion.from_euler_angles(alpha, beta, gamma)
                                     for alpha, beta, gamma in random_angles]))) < 1.e-15 
Example 30
Project: quaternion   Author: moble   File: test_quaternion.py    License: MIT License 5 votes vote down vote up
def test_quaternion_norm(Qs):
    for q in Qs[Qs_nan]:
        assert np.isnan(q.norm())
    for q in Qs[Qs_inf]:
        if on_windows:
            assert np.isinf(q.norm()) or np.isnan(q.norm())
        else:
            assert np.isinf(q.norm())
    for q, a in [(Qs[q_0], 0.0), (Qs[q_1], 1.0), (Qs[x], 1.0), (Qs[y], 1.0), (Qs[z], 1.0),
                 (Qs[Q], Qs[Q].w ** 2 + Qs[Q].x ** 2 + Qs[Q].y ** 2 + Qs[Q].z ** 2),
                 (Qs[Qbar], Qs[Q].w ** 2 + Qs[Q].x ** 2 + Qs[Q].y ** 2 + Qs[Q].z ** 2)]:
        assert np.allclose(q.norm(), a)


# Unary quaternion returners