def test_basic_2d_rotation_axis_angle():
    rotation_matrix = np.array([[0, 1],
                                [-1, 0]])
    rotation = Rotation(rotation_matrix)
    axis, angle = rotation.axis_and_angle_of_rotation()
    assert_allclose(axis, np.array([0, 0, 1]))
    assert_allclose((90 * np.pi)/180, angle)

def test_rotation_compose_before_homog():
    # can't do this inplace - so should just give transform chain
    rotation = Rotation(np.array([[1, 0],
                                  [0, 1]]))
    homog = Homogeneous(np.array([[0, 1, 0],
                                  [1, 0, 0],
                                  [0, 0, 1]]))
    res = rotation.compose_before(homog)
    assert(type(res) == Homogeneous)

def test_align_2d_rotation():
    rotation_matrix = np.array([[0, 1], [-1, 0]])
    rotation = Rotation(rotation_matrix)
    source = PointCloud(np.array([[0, 1], [1, 1], [-1, -5], [3, -5]]))
    target = rotation.apply(source)
    # estimate the transform from source and target
    estimate = AlignmentRotation(source, target)
    # check the estimates is correct
    assert_allclose(rotation.h_matrix, estimate.h_matrix, atol=1e-14)

def test_3d_rotation_as_vector():
    a = np.sqrt(3.0) / 2.0
    b = 0.5
    # this is a rotation of -30 degrees about the x axis
    rotation_matrix = np.array([[1, 0, 0],
                                [0, a, b],
                                [0, -b, a]])
    rotation = Rotation(rotation_matrix)
    assert_allclose(np.round(rotation.as_vector()[2:]), np.array([0., 0.]))

def test_align_2d_rotation_set_h_matrix_raises_notimplemented_error():
    rotation_matrix = np.array([[0, 1], [-1, 0]])
    rotation = Rotation(rotation_matrix)
    source = PointCloud(np.array([[0, 1], [1, 1], [-1, -5], [3, -5]]))
    target = rotation.apply(source)
    # estimate the transform from source and source
    estimate = AlignmentRotation(source, source)
    # and set the target
    estimate.set_h_matrix(rotation.h_matrix)

def test_3d_rotation_inverse_eye():
    a = np.sqrt(3.0)/2.0
    b = 0.5
    # this is a rotation of -30 degrees about the x axis
    rotation_matrix = np.array([[1, 0, 0],
                                [0, a, b],
                                [0, -b, a]])
    rotation = Rotation(rotation_matrix)
    transformed = rotation.compose_before(rotation.pseudoinverse())
    assert_allclose(np.eye(4), transformed.h_matrix, atol=1e-15)

def test_basic_3d_rotation():
    a = np.sqrt(3.0)/2.0
    b = 0.5
    # this is a rotation of -30 degrees about the x axis
    rotation_matrix = np.array([[1, 0, 0],
                                [0, a, b],
                                [0, -b, a]])
    rotation = Rotation(rotation_matrix)
    starting_vector = np.array([0, 1, 0])
    transformed = rotation.apply(starting_vector)
    assert_allclose(np.array([0, a, -b]), transformed)

def test_basic_3d_rotation_axis_angle():
    a = np.sqrt(3.0)/2.0
    b = 0.5
    # this is a rotation of -30 degrees about the x axis
    rotation_matrix = np.array([[1, 0, 0],
                                [0, a, b],
                                [0, -b, a]])
    rotation = Rotation(rotation_matrix)
    axis, angle = rotation.axis_and_angle_of_rotation()
    assert_allclose(axis, np.array([1, 0, 0]))
    assert_allclose((-30 * np.pi)/180, angle)

def test_transform_about_centre():
    pixels_16 = np.arange(16, dtype=np.float)
    image = Image(pixels_16.reshape(4, 4))
    transform = Rotation.init_from_2d_ccw_angle(180).compose_before(
        UniformScale(2, n_dims=2))
    # rotate 90 + scale degrees
    transformed_img = image.transform_about_centre(transform, order=0)
    expected_pixels = np.rot90(np.repeat(np.repeat(pixels_16, 2).reshape(4, -1),
                                         2, axis=0)[1:, 1:],
                               k=2)

    assert transformed_img.shape == (7, 7)
    assert_allclose(transformed_img.pixels[0], expected_pixels)

    def fit_from_camera(self, image, camera, instance=None, gt_mesh=None,
                        max_iters=50, camera_update=False,
                        focal_length_update=False, shape_prior_weight=1.,
                        texture_prior_weight=1., return_costs=False):
        # Execute multi-scale fitting
        algorithm_results = self._fit(
            image, camera, instance=instance, gt_mesh=gt_mesh,
            max_iters=max_iters, camera_update=camera_update,
            focal_length_update=focal_length_update,
            reconstruction_weight=1.,
            shape_prior_weight=shape_prior_weight,
            texture_prior_weight=texture_prior_weight,
            landmarks_prior_weight=None, landmarks=None,
            return_costs=return_costs)

        # Return multi-scale fitting result
        return self._fitter_result(
            image=image, algorithm_results=algorithm_results,
            affine_transform=Rotation.init_identity(n_dims=2),
            gt_mesh=gt_mesh)

def test_init_3d_from_quaternion():
    q = np.array([1., 0., 0.27, 0.])
    r = Rotation.init_3d_from_quaternion(q)
    axis, angle = r.axis_and_angle_of_rotation()
    assert_allclose(r.axis_and_angle_of_rotation()[0], np.array([0., 1., 0.]))
    assert np.round(angle * 180 / np.pi) == 30.

def test_3d_rotation_n_parameters():
    assert Rotation.init_identity(3).n_parameters == 4

def test_rotation2d_identity():
    assert_allclose(Rotation.init_identity(2).h_matrix, np.eye(3))

def test_basic_2d_rotation():
    rotation_matrix = np.array([[0, 1],
                                [-1, 0]])
    rotation = Rotation(rotation_matrix)
    assert_allclose(np.array([0, -1]), rotation.apply(np.array([1, 0])))

def test_rotation2d_as_vector_raises_notimplementederror():
    with raises(NotImplementedError):
        Rotation.init_identity(2).as_vector()

def test_rotation_set_h_matrix_raises_notimplementederror():
    r = Rotation(np.array([[1, 0], [0, 1]]))
    r.set_h_matrix(r.h_matrix)

def test_rotation3d_as_vector_raises_notimplementederror():
    Rotation.identity(3).as_vector()

 def init_from_image_shape_and_vector(cls, image_shape, vector):
     r = Rotation.init_identity(n_dims=3)
     t = Translation.init_identity(n_dims=3)
     p = PerspectiveProjection(focal_length=1, image_shape=image_shape)
     return cls(r, t, p).from_vector(vector)

def test_rotation2d_from_vector_raises_notimplementederror():
    Rotation.init_identity(2).from_vector(0)

def test_rotation3d_identity():
    assert_allclose(Rotation.identity(3).h_matrix, np.eye(4))