# Path: sensbiotk/transforms3d/quaternions.py
# MAX_FLOAT = np.maximum_sctype(np.float)
# FLOAT_EPS = np.finfo(np.float).eps
#     X = x*s
#     Y = y*s
#     Z = z*s
#     K = np.array([
#         [Qxx - Qyy - Qzz, 0,               0,               0              ],
#         [Qyx + Qxy,       Qyy - Qxx - Qzz, 0,               0              ],
#         [Qzx + Qxz,       Qzy + Qyz,       Qzz - Qxx - Qyy, 0              ],
#         [Qyz - Qzy,       Qzx - Qxz,       Qxy - Qyx,       Qxx + Qyy + Qzz]]
#         ) / 3.0
#     c = math.cos(theta); s = math.sin(theta); C = 1-c
# def fillpositive(xyz, w2_thresh=None):
# def quat2mat(q):
# def mat2quat(M):
# def mult(q1, q2):
# def conjugate(q):
# def norm(q):
# def norm2(q):
# def normalize(q):
# def isunit(q):
# def inverse(q):
# def eye():
# def rotate_vector(v, q):
# def nearly_equivalent(q1, q2, rtol=1e-5, atol=1e-8):
# def angle_axis2quat(theta, vector, is_normalized=False):
# def angle_axis2mat(theta, vector, is_normalized=False):
# def quat2angle_axis(quat, identity_thresh=None):
# 
# Path: sensbiotk/transforms3d/eulerangles.py
# _FLOAT_EPS_4 = np.finfo(float).eps * 4.0
#     M = np.asarray(M)
#     M = nq.angle_axis2mat(theta, vector, is_normalized)
# def euler2mat(z=0, y=0, x=0):
# def mat2euler(M, cy_thresh=None):
# def euler2quat(z=0, y=0, x=0):
# def euler2quat2(yaw=0, pitch=0, roll=0):
# def quat2euler(q):
# def quat2euler2(q):
# def quat2euler3(q):
# def quat2euler4(q):
# def quat2euler5(q):
# def euler2angle_axis(z=0, y=0, x=0):
# def angle_axis2euler(theta, vector, is_normalized=False):


import math
import numpy as np
from numpy import pi
from sensbiotk.transforms3d import quaternions as nq
from sensbiotk.transforms3d import eulerangles as nea
from nose.tools import assert_true, assert_false, assert_equal
from numpy.testing import assert_array_equal, assert_array_almost_equal
def crude_mat2euler(M):
    ''' The simplest possible - ignoring atan2 instability '''
    r11, r12, r13, r21, r22, r23, r31, r32, r33 = M.flat
    return math.atan2(-r12, r11), math.asin(r13), math.atan2(-r23, r33)


def test_euler_instability():
    # Test for numerical errors in mat2euler
    # problems arise for cos(y) near 0
    po2 = pi / 2
    zyx = po2, po2, po2
    M = nea.euler2mat(*zyx)
    # Round trip
    M_back = nea.euler2mat(*nea.mat2euler(M))
    yield assert_true, np.allclose(M, M_back)
    # disturb matrix slightly
    M_e = M - FLOAT_EPS
    # round trip to test - OK
    M_e_back = nea.euler2mat(*nea.mat2euler(M_e))
    yield assert_true, np.allclose(M_e, M_e_back)
    # not so with crude routine
    M_e_back = nea.euler2mat(*crude_mat2euler(M_e))
    yield assert_false, np.allclose(M_e, M_e_back)


def test_quats():
    for x, y, z in eg_rots:
        M1 = nea.euler2mat(z, y, x)
        quatM = nq.mat2quat(M1)
        quat = nea.euler2quat(z, y, x)

# Path: sensbiotk/transforms3d/quaternions.py
# MAX_FLOAT = np.maximum_sctype(np.float)
# FLOAT_EPS = np.finfo(np.float).eps
#     X = x*s
#     Y = y*s
#     Z = z*s
#     K = np.array([
#         [Qxx - Qyy - Qzz, 0,               0,               0              ],
#         [Qyx + Qxy,       Qyy - Qxx - Qzz, 0,               0              ],
#         [Qzx + Qxz,       Qzy + Qyz,       Qzz - Qxx - Qyy, 0              ],
#         [Qyz - Qzy,       Qzx - Qxz,       Qxy - Qyx,       Qxx + Qyy + Qzz]]
#         ) / 3.0
#     c = math.cos(theta); s = math.sin(theta); C = 1-c
# def fillpositive(xyz, w2_thresh=None):
# def quat2mat(q):
# def mat2quat(M):
# def mult(q1, q2):
# def conjugate(q):
# def norm(q):
# def norm2(q):
# def normalize(q):
# def isunit(q):
# def inverse(q):
# def eye():
# def rotate_vector(v, q):
# def nearly_equivalent(q1, q2, rtol=1e-5, atol=1e-8):
# def angle_axis2quat(theta, vector, is_normalized=False):
# def angle_axis2mat(theta, vector, is_normalized=False):
# def quat2angle_axis(quat, identity_thresh=None):
# 
# Path: sensbiotk/transforms3d/eulerangles.py
# _FLOAT_EPS_4 = np.finfo(float).eps * 4.0
#     M = np.asarray(M)
#     M = nq.angle_axis2mat(theta, vector, is_normalized)
# def euler2mat(z=0, y=0, x=0):
# def mat2euler(M, cy_thresh=None):
# def euler2quat(z=0, y=0, x=0):
# def euler2quat2(yaw=0, pitch=0, roll=0):
# def quat2euler(q):
# def quat2euler2(q):
# def quat2euler3(q):
# def quat2euler4(q):
# def quat2euler5(q):
# def euler2angle_axis(z=0, y=0, x=0):
# def angle_axis2euler(theta, vector, is_normalized=False):


# Path: sensbiotk/tests/test_euler.py
import math
import numpy as np
from numpy import pi
from sensbiotk.transforms3d import quaternions as nq
from sensbiotk.transforms3d import eulerangles as nea
from nose.tools import assert_true, assert_false, assert_equal
from numpy.testing import assert_array_equal, assert_array_almost_equal

def crude_mat2euler(M):
    ''' The simplest possible - ignoring atan2 instability '''
    r11, r12, r13, r21, r22, r23, r31, r32, r33 = M.flat
    return math.atan2(-r12, r11), math.asin(r13), math.atan2(-r23, r33)


def test_euler_instability():
    # Test for numerical errors in mat2euler
    # problems arise for cos(y) near 0
    po2 = pi / 2
    zyx = po2, po2, po2
    M = nea.euler2mat(*zyx)
    # Round trip
    M_back = nea.euler2mat(*nea.mat2euler(M))
    yield assert_true, np.allclose(M, M_back)
    # disturb matrix slightly
    M_e = M - FLOAT_EPS
    # round trip to test - OK
    M_e_back = nea.euler2mat(*nea.mat2euler(M_e))
    yield assert_true, np.allclose(M_e, M_e_back)
    # not so with crude routine
    M_e_back = nea.euler2mat(*crude_mat2euler(M_e))
    yield assert_false, np.allclose(M_e, M_e_back)


def test_quats():
    for x, y, z in eg_rots:
        M1 = nea.euler2mat(z, y, x)
        quatM = nq.mat2quat(M1)
        quat = nea.euler2quat(z, y, x)

        yield nq.nearly_equivalent, quatM, quat