# Path: imusim/maths/transforms.py
# class AffineTransform(object):
# class UnscentedTransform(object):
#     def __init__(self, transform=None, rz=0, ry=0, rx=0, scale=1,
#             translation=vector(0,0,0)):
#     def apply(self,v):
#     def reverse(self,v):
#     def __init__(self, function):
#     def __call__(self, mean, covariance, *args, **kwargs):
#     def sigmaPoints(mean, covariance):
# def cartesianToPolar(x,y):
# def polarToCartesian(r,theta):
# def convertCGtoNED(param):
# def convertNEDtoCG(param):
#         N = len(mean)
# 
# Path: imusim/maths/transforms.py
# class AffineTransform(object):
#     """
#     Affine transform composed of a linear transform matrix and translation.
#     """
#     def __init__(self, transform=None, rz=0, ry=0, rx=0, scale=1,
#             translation=vector(0,0,0)):
#         """
#         Construct an affine transform.
# 
#         A generic transformation matrix can be supplied, or a transform
#         composed of rotation and scaling can be built.
# 
#         Generating rotations using supplied Euler angles is performed from the
#         perspective of a fixed camera viewing a rotating object, i.e. points
#         rotate within a fixed reference frame. Rotations are applied in
#         aerospace (ZYX) order.
# 
#         @param transform: 3x3 linear transformation matrix.
#         @param rx: X rotation angle in radians.
#         @param ry: Y rotation angle in radians.
#         @param rz: Z rotation angle in radians.
#         @param scale: Scaling factor
#         @param translation: 3x1 translation vector.
#         """
#         if transform is not None:
#             self._transform = transform
#         else:
#             self._transform = scale * matrixFromEuler((rz,ry,rx),'zyx',False)
# 
#         self._inverseTransform = np.linalg.inv(self._transform)
#         self._translation = translation
# 
#     def apply(self,v):
#         """
#         Apply transform to an array of column vectors.
#         """
#         return np.tensordot(self._transform, v, axes=([1],[0])) \
#                 + self._translation
# 
#     def reverse(self,v):
#         """
#         Apply the inverse of this transform to an array of column vectors.
#         """
#         return np.tensordot(self._inverseTransform,
#                 v-self._translation,axes=([1],[0]))


import numpy as np
from numpy import random
from scipy import stats
from numpy import testing
from imusim.maths import transforms
from imusim.maths.vectors import vector
from imusim.maths.quaternions import Quaternion
from imusim.maths.transforms import AffineTransform
from math import sqrt

def checkCartesianToPolar(cartesian, polar):
    assert_almost_equal(transforms.cartesianToPolar(*cartesian), polar)

def testCartesianPolarConversion():
    for testParams in CARTESIAN_POLAR_TEST_VALUES:
        cartesian, polar = testParams
        yield checkPolarToCartesian, cartesian, polar
        yield checkCartesianToPolar, cartesian, polar

# each element of the list is a tuple of vectors in NED and CG co-ords
NED_CG_TEST_VALUES = [
        (vector(1,0,0), vector(0,0,1)),
        (vector(0,1,0), vector(-1,0,0)),
        (vector(0,0,1), vector(0,-1,0))
        ]

def checkNED_to_CG(ned, cg):
    assert_almost_equal(transforms.convertNEDtoCG(ned), cg)

def checkCG_to_NED(ned, cg):
    assert_almost_equal(transforms.convertCGtoNED(cg), ned)

def testCoordinateChange():
    for ned, cg in NED_CG_TEST_VALUES:
        yield checkNED_to_CG, ned, cg
        yield checkCG_to_NED, ned, cg

def testCG_to_NED_Quat():
    testing.assert_equal(

# Path: imusim/maths/transforms.py
# class AffineTransform(object):
# class UnscentedTransform(object):
#     def __init__(self, transform=None, rz=0, ry=0, rx=0, scale=1,
#             translation=vector(0,0,0)):
#     def apply(self,v):
#     def reverse(self,v):
#     def __init__(self, function):
#     def __call__(self, mean, covariance, *args, **kwargs):
#     def sigmaPoints(mean, covariance):
# def cartesianToPolar(x,y):
# def polarToCartesian(r,theta):
# def convertCGtoNED(param):
# def convertNEDtoCG(param):
#         N = len(mean)
# 
# Path: imusim/maths/transforms.py
# class AffineTransform(object):
#     """
#     Affine transform composed of a linear transform matrix and translation.
#     """
#     def __init__(self, transform=None, rz=0, ry=0, rx=0, scale=1,
#             translation=vector(0,0,0)):
#         """
#         Construct an affine transform.
# 
#         A generic transformation matrix can be supplied, or a transform
#         composed of rotation and scaling can be built.
# 
#         Generating rotations using supplied Euler angles is performed from the
#         perspective of a fixed camera viewing a rotating object, i.e. points
#         rotate within a fixed reference frame. Rotations are applied in
#         aerospace (ZYX) order.
# 
#         @param transform: 3x3 linear transformation matrix.
#         @param rx: X rotation angle in radians.
#         @param ry: Y rotation angle in radians.
#         @param rz: Z rotation angle in radians.
#         @param scale: Scaling factor
#         @param translation: 3x1 translation vector.
#         """
#         if transform is not None:
#             self._transform = transform
#         else:
#             self._transform = scale * matrixFromEuler((rz,ry,rx),'zyx',False)
# 
#         self._inverseTransform = np.linalg.inv(self._transform)
#         self._translation = translation
# 
#     def apply(self,v):
#         """
#         Apply transform to an array of column vectors.
#         """
#         return np.tensordot(self._transform, v, axes=([1],[0])) \
#                 + self._translation
# 
#     def reverse(self,v):
#         """
#         Apply the inverse of this transform to an array of column vectors.
#         """
#         return np.tensordot(self._inverseTransform,
#                 v-self._translation,axes=([1],[0]))


# Path: imusim/tests/maths/transforms_test.py
import numpy as np
from numpy import random
from scipy import stats
from numpy import testing
from imusim.maths import transforms
from imusim.maths.vectors import vector
from imusim.maths.quaternions import Quaternion
from imusim.maths.transforms import AffineTransform
from math import sqrt


def checkCartesianToPolar(cartesian, polar):
    assert_almost_equal(transforms.cartesianToPolar(*cartesian), polar)

def testCartesianPolarConversion():
    for testParams in CARTESIAN_POLAR_TEST_VALUES:
        cartesian, polar = testParams
        yield checkPolarToCartesian, cartesian, polar
        yield checkCartesianToPolar, cartesian, polar

# each element of the list is a tuple of vectors in NED and CG co-ords
NED_CG_TEST_VALUES = [
        (vector(1,0,0), vector(0,0,1)),
        (vector(0,1,0), vector(-1,0,0)),
        (vector(0,0,1), vector(0,-1,0))
        ]

def checkNED_to_CG(ned, cg):
    assert_almost_equal(transforms.convertNEDtoCG(ned), cg)

def checkCG_to_NED(ned, cg):
    assert_almost_equal(transforms.convertCGtoNED(cg), ned)

def testCoordinateChange():
    for ned, cg in NED_CG_TEST_VALUES:
        yield checkNED_to_CG, ned, cg
        yield checkCG_to_NED, ned, cg

def testCG_to_NED_Quat():
    testing.assert_equal(

            transforms.convertCGtoNED(Quaternion()).components,