# Path: imusim/trajectories/rigid_body.py
# class SampledBodyModel(SampledPositionTrajectory, SampledJoint):
#     """A body model with sampled positions and rotations."""
# 
#     def __init__(self, name=None):
#         """
#         Initialise body model.
# 
#         @param name: The name of the root L{Joint} of the body model
#         """
#         SampledJoint.__init__(self, None, name)
#         SampledPositionTrajectory.__init__(self)
# 
#     @classmethod
#     def structureCopy(cls, ref, parent=None):
#         model = cls(ref.name)
#         for child in ref.children:
#             if child.isJoint:
#                 SampledJoint.structureCopy(child, model)
#             else:
#                 PointTrajectory(model, child.name, child.positionOffset)
#         return model
# 
# class SampledJoint(SampledRotationTrajectory, OffsetTrajectory, Joint):
#     """
#     A joint in a rigid body model with sampled rotations.
#     """
#     def __init__(self, parent, name=None, offset=None):
#         """
#         Initialise sampled joint.
# 
#         @param parent: Parent L{Joint} in the body model hierarchy.
#         @param name: Name of the joint.
#         @param offset: Offset of the joint in the parent joint
#             co-ordinate frame.
#         """
#         Joint.__init__(self, parent, name, offset)
#         OffsetTrajectory.__init__(self, parent, offset)
#         SampledRotationTrajectory.__init__(self)
# 
#     @classmethod
#     def structureCopy(cls, ref, parent=None):
#         joint = cls(parent, ref.name, ref.positionOffset)
#         for child in ref.children:
#             if child.isJoint:
#                 cls.structureCopy(child, joint)
#             else:
#                 PointTrajectory(joint, child.name, child.positionOffset)
#         return joint
# 
# class PointTrajectory(OffsetTrajectory, Point):
#     """
#     Trajectory followed by a point in a rigid body model.
#     """
#     def __init__(self, parent, name=None, offset=None):
#         """
#         Construct point trajectory.
# 
#         @param parent: Parent L{Joint} in the body model hierarchy.
#         @param name: Name of the point.
#         @param offset: Offset of the point in the parent joint
#             co-ordinate frame.
#         """
#         Point.__init__(self, parent, name, offset)
#         OffsetTrajectory.__init__(self, parent, offset)
# 
# Path: imusim/maths/transforms.py
# def convertCGtoNED(param):
#     """
#     Convert parameters specified in Computer Graphics co-ordinates to
#     North East Down co-ordinates.
# 
#     @param param: A L{Quaternion} or 3x1 vector in CG frame.
# 
#     @return:  The corresponding L{Quaternion} or vector in NED frame
#     """
#     if isinstance(param, (Quaternion,QuaternionArray)):
#         return _CGtoNED.conjugate * param * _CGtoNED
#     else:
#         return _CGtoNED.rotateFrame(param)
# 
# def convertNEDtoCG(param):
#     """
#     Convert parameters specified in North East Down co-ordinates to
#     Computer Graphics co-ordinates.
# 
#     @param param: A L{Quaternion} or 3x1 vector in NED frame.
# 
#     @return:  The corresponding L{Quaternion} or vector in CG frame
#     """
#     if isinstance(param, (Quaternion,QuaternionArray)):
#         return _NEDtoCG.conjugate * param * _NEDtoCG
#     else:
#         return _NEDtoCG.rotateFrame(param)


from imusim.trajectories.rigid_body import SampledBodyModel, SampledJoint, \
        PointTrajectory
from imusim.maths.quaternions import Quaternion
from imusim.maths.transforms import convertCGtoNED, convertNEDtoCG
import numpy as np
    def _readFrame(self):
        '''
        Read a frame of data from the BVH file.

        @return: The channel data as a list of floats.
        '''
        chanData = self._readline().split()
        if len(chanData) != self.totalChannels:
            raise SyntaxError, "Syntax error at line %d: Number of entries \
does not match the number of channels. (Found %d of %d)" %(
                self.line,len(chanData),self.totalChannels)
        return map(float,chanData)

    def _readJoint(self):
        # use jointStack[-1] to peek at the top of the jointStack
        currentJoint = self.jointStack[-1]
        name = self._token()
        # name will be 'Site' for End Site joints.
        if name == 'Site':
            name = currentJoint.parent.name+"_end"
        currentJoint.name = name

        self._checkToken("{")
        while True:
            token = self._token()
            if token == "OFFSET":
                x = self._floatToken()
                y = self._floatToken()
                z = self._floatToken()
                currentJoint.positionOffset = \

# Path: imusim/trajectories/rigid_body.py
# class SampledBodyModel(SampledPositionTrajectory, SampledJoint):
#     """A body model with sampled positions and rotations."""
# 
#     def __init__(self, name=None):
#         """
#         Initialise body model.
# 
#         @param name: The name of the root L{Joint} of the body model
#         """
#         SampledJoint.__init__(self, None, name)
#         SampledPositionTrajectory.__init__(self)
# 
#     @classmethod
#     def structureCopy(cls, ref, parent=None):
#         model = cls(ref.name)
#         for child in ref.children:
#             if child.isJoint:
#                 SampledJoint.structureCopy(child, model)
#             else:
#                 PointTrajectory(model, child.name, child.positionOffset)
#         return model
# 
# class SampledJoint(SampledRotationTrajectory, OffsetTrajectory, Joint):
#     """
#     A joint in a rigid body model with sampled rotations.
#     """
#     def __init__(self, parent, name=None, offset=None):
#         """
#         Initialise sampled joint.
# 
#         @param parent: Parent L{Joint} in the body model hierarchy.
#         @param name: Name of the joint.
#         @param offset: Offset of the joint in the parent joint
#             co-ordinate frame.
#         """
#         Joint.__init__(self, parent, name, offset)
#         OffsetTrajectory.__init__(self, parent, offset)
#         SampledRotationTrajectory.__init__(self)
# 
#     @classmethod
#     def structureCopy(cls, ref, parent=None):
#         joint = cls(parent, ref.name, ref.positionOffset)
#         for child in ref.children:
#             if child.isJoint:
#                 cls.structureCopy(child, joint)
#             else:
#                 PointTrajectory(joint, child.name, child.positionOffset)
#         return joint
# 
# class PointTrajectory(OffsetTrajectory, Point):
#     """
#     Trajectory followed by a point in a rigid body model.
#     """
#     def __init__(self, parent, name=None, offset=None):
#         """
#         Construct point trajectory.
# 
#         @param parent: Parent L{Joint} in the body model hierarchy.
#         @param name: Name of the point.
#         @param offset: Offset of the point in the parent joint
#             co-ordinate frame.
#         """
#         Point.__init__(self, parent, name, offset)
#         OffsetTrajectory.__init__(self, parent, offset)
# 
# Path: imusim/maths/transforms.py
# def convertCGtoNED(param):
#     """
#     Convert parameters specified in Computer Graphics co-ordinates to
#     North East Down co-ordinates.
# 
#     @param param: A L{Quaternion} or 3x1 vector in CG frame.
# 
#     @return:  The corresponding L{Quaternion} or vector in NED frame
#     """
#     if isinstance(param, (Quaternion,QuaternionArray)):
#         return _CGtoNED.conjugate * param * _CGtoNED
#     else:
#         return _CGtoNED.rotateFrame(param)
# 
# def convertNEDtoCG(param):
#     """
#     Convert parameters specified in North East Down co-ordinates to
#     Computer Graphics co-ordinates.
# 
#     @param param: A L{Quaternion} or 3x1 vector in NED frame.
# 
#     @return:  The corresponding L{Quaternion} or vector in CG frame
#     """
#     if isinstance(param, (Quaternion,QuaternionArray)):
#         return _NEDtoCG.conjugate * param * _NEDtoCG
#     else:
#         return _NEDtoCG.rotateFrame(param)


# Path: imusim/io/bvh.py
from imusim.trajectories.rigid_body import SampledBodyModel, SampledJoint, \
        PointTrajectory
from imusim.maths.quaternions import Quaternion
from imusim.maths.transforms import convertCGtoNED, convertNEDtoCG
import numpy as np

    def _readFrame(self):
        '''
        Read a frame of data from the BVH file.

        @return: The channel data as a list of floats.
        '''
        chanData = self._readline().split()
        if len(chanData) != self.totalChannels:
            raise SyntaxError, "Syntax error at line %d: Number of entries \
does not match the number of channels. (Found %d of %d)" %(
                self.line,len(chanData),self.totalChannels)
        return map(float,chanData)

    def _readJoint(self):
        # use jointStack[-1] to peek at the top of the jointStack
        currentJoint = self.jointStack[-1]
        name = self._token()
        # name will be 'Site' for End Site joints.
        if name == 'Site':
            name = currentJoint.parent.name+"_end"
        currentJoint.name = name

        self._checkToken("{")
        while True:
            token = self._token()
            if token == "OFFSET":
                x = self._floatToken()
                y = self._floatToken()
                z = self._floatToken()
                currentJoint.positionOffset = \

                        convertCGtoNED(np.array([[x,y,z]]).T*self.conversionFactor)