# 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 sympy_euler(z, y, x):
    # The whole matrix formula for z,y,x rotations from Sympy
    cos = math.cos
    sin = math.sin
    # the following copy / pasted from Sympy - see derivations subdirectory
    return [
        [                       cos(y)*cos(z),                       -cos(y)*sin(z),         sin(y)],
        [cos(x)*sin(z) + cos(z)*sin(x)*sin(y), cos(x)*cos(z) - sin(x)*sin(y)*sin(z), -cos(y)*sin(x)],
        [sin(x)*sin(z) - cos(x)*cos(z)*sin(y), cos(z)*sin(x) + cos(x)*sin(y)*sin(z),  cos(x)*cos(y)]
        ]


def is_valid_rotation(M):
    if not np.allclose(np.linalg.det(M), 1):
        return False
    return np.allclose(np.eye(3), np.dot(M, M.T))


def test_basic_euler():
    # some example rotations, in radians
    zr = 0.05
    yr = -0.4
    xr = 0.2
    # Rotation matrix composing the three rotations
    M = nea.euler2mat(zr, yr, xr)
    # Corresponding individual rotation matrices
    M1 = nea.euler2mat(zr)
    M2 = nea.euler2mat(0, yr)

# 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 sympy_euler(z, y, x):
    # The whole matrix formula for z,y,x rotations from Sympy
    cos = math.cos
    sin = math.sin
    # the following copy / pasted from Sympy - see derivations subdirectory
    return [
        [                       cos(y)*cos(z),                       -cos(y)*sin(z),         sin(y)],
        [cos(x)*sin(z) + cos(z)*sin(x)*sin(y), cos(x)*cos(z) - sin(x)*sin(y)*sin(z), -cos(y)*sin(x)],
        [sin(x)*sin(z) - cos(x)*cos(z)*sin(y), cos(z)*sin(x) + cos(x)*sin(y)*sin(z),  cos(x)*cos(y)]
        ]


def is_valid_rotation(M):
    if not np.allclose(np.linalg.det(M), 1):
        return False
    return np.allclose(np.eye(3), np.dot(M, M.T))


def test_basic_euler():
    # some example rotations, in radians
    zr = 0.05
    yr = -0.4
    xr = 0.2
    # Rotation matrix composing the three rotations
    M = nea.euler2mat(zr, yr, xr)
    # Corresponding individual rotation matrices
    M1 = nea.euler2mat(zr)
    M2 = nea.euler2mat(0, yr)

    M3 = nea.euler2mat(0, 0, xr)