def test_dual_quaternion_applied_to_point():
    random_state = np.random.RandomState(1000)
    for _ in range(5):
        p_A = random_vector(random_state, 3)
        A2B = random_transform(random_state)
        dq = dual_quaternion_from_transform(A2B)
        p_B = dq_prod_vector(dq, p_A)
        assert_array_almost_equal(p_B,
                                  transform(A2B, vector_to_point(p_A))[:3])
def test_vectors_to_points():
    """Test conversion from vectors to homogenous coordinates."""
    V = np.array([[1, 2, 3], [2, 3, 4]])
    PA = vectors_to_points(V)
    assert_array_almost_equal(PA, [[1, 2, 3, 1], [2, 3, 4, 1]])

    random_state = np.random.RandomState(0)
    V = random_state.randn(10, 3)
    for i, p in enumerate(vectors_to_points(V)):
        assert_array_almost_equal(p, vector_to_point(V[i]))
def test_vector_to_point():
    """Test conversion from vector to homogenous coordinates."""
    v = np.array([1, 2, 3])
    pA = vector_to_point(v)
    assert_array_almost_equal(pA, [1, 2, 3, 1])

    random_state = np.random.RandomState(0)
    R = matrix_from(a=random_axis_angle(random_state))
    p = random_vector(random_state)
    A2B = transform_from(R, p)
    assert_transform(A2B)
    _ = transform(A2B, pA)
def plot_screw(figure,
               q=np.zeros(3),
               s_axis=np.array([1.0, 0.0, 0.0]),
               h=1.0,
               theta=1.0,
               A2B=None,
               s=1.0):
    """Plot transformation about and along screw axis.

    Parameters
    ----------
    figure : Figure
        Interface to Open3D's visualizer

    q : array-like, shape (3,), optional (default: [0, 0, 0])
        Vector to a point on the screw axis

    s_axis : array-like, shape (3,), optional (default: [1, 0, 0])
        Direction vector of the screw axis

    h : float, optional (default: 1)
        Pitch of the screw. The pitch is the ratio of translation and rotation
        of the screw axis. Infinite pitch indicates pure translation.

    theta : float, optional (default: 1)
        Rotation angle. h * theta is the translation.

    A2B : array-like, shape (4, 4), optional (default: I)
        Origin of the screw

    s : float, optional (default: 1)
        Scaling of the axis and angle that will be drawn
    """
    from pytransform3d.rotations import (vector_projection,
                                         angle_between_vectors,
                                         perpendicular_to_vectors,
                                         slerp_weights)
    from pytransform3d.transformations import (check_screw_parameters,
                                               transform, translate_transform,
                                               vector_to_point,
                                               vector_to_direction,
                                               vectors_to_points)

    if A2B is None:
        A2B = np.eye(4)

    q, s_axis, h = check_screw_parameters(q, s_axis, h)

    origin_projected_on_screw_axis = q + vector_projection(-q, s_axis)

    pure_translation = np.isinf(h)

    if not pure_translation:
        screw_axis_to_old_frame = -origin_projected_on_screw_axis
        screw_axis_to_rotated_frame = perpendicular_to_vectors(
            s_axis, screw_axis_to_old_frame)
        screw_axis_to_translated_frame = h * s_axis

        arc = np.empty((100, 3))
        angle = angle_between_vectors(screw_axis_to_old_frame,
                                      screw_axis_to_rotated_frame)
        for i, t in enumerate(
                zip(np.linspace(0, 2 * theta / np.pi, len(arc)),
                    np.linspace(0.0, 1.0, len(arc)))):
            t1, t2 = t
            w1, w2 = slerp_weights(angle, t1)
            arc[i] = (origin_projected_on_screw_axis +
                      w1 * screw_axis_to_old_frame +
                      w2 * screw_axis_to_rotated_frame +
                      screw_axis_to_translated_frame * t2 * theta)

    q = transform(A2B, vector_to_point(q))[:3]
    s_axis = transform(A2B, vector_to_direction(s_axis))[:3]
    if not pure_translation:
        arc = transform(A2B, vectors_to_points(arc))[:, :3]
        origin_projected_on_screw_axis = transform(
            A2B, vector_to_point(origin_projected_on_screw_axis))[:3]

    # Screw axis
    Q = translate_transform(np.eye(4), q)
    fig.plot_sphere(radius=s * 0.02, A2B=Q, c=[1, 0, 0])
    if pure_translation:
        s_axis *= theta
        Q_plus_S_axis = translate_transform(np.eye(4), q + s_axis)
        fig.plot_sphere(radius=s * 0.02, A2B=Q_plus_S_axis, c=[0, 1, 0])
    P = np.array(
        [[q[0] - s * s_axis[0], q[1] - s * s_axis[1], q[2] - s * s_axis[2]],
         [
             q[0] + (1 + s) * s_axis[0], q[1] + (1 + s) * s_axis[1],
             q[2] + (1 + s) * s_axis[2]
         ]])
    figure.plot(P=P, c=[0, 0, 0])

    if not pure_translation:
        # Transformation
        figure.plot(arc, c=[0, 0, 0])

        for i, c in zip([0, -1], [[1, 0, 0], [0, 1, 0]]):
            arc_bound = np.vstack((origin_projected_on_screw_axis, arc[i]))
            figure.plot(arc_bound, c=c)