# 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
        Read the motion data from the BVH file.
        '''
        # update joint tree with new data.
        frame = 0
        lastRotation = {}
        for frame in range(self.frameCount):
            time = frame * self.framePeriod
            channelData = self._readFrame()
            for joint in self.model.joints:
                for chan in joint.channels:
                    chanData = channelData.pop(0)
                    if chan == "Xposition":
                        xPos = chanData
                    elif chan == "Yposition":
                        yPos = chanData
                    elif chan == "Zposition":
                        zPos = chanData
                    elif chan == "Xrotation":
                        xRot = chanData
                    elif chan == "Yrotation":
                        yRot = chanData
                    elif chan == "Zrotation":
                        zRot = chanData
                    else:
                        raise RuntimeError('Unknown channel: '+chan)

                if not joint.hasParent:
                    try:
                        p = np.array([[xPos,yPos,zPos]]).T * \
                                self.conversionFactor

# 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

        Read the motion data from the BVH file.
        '''
        # update joint tree with new data.
        frame = 0
        lastRotation = {}
        for frame in range(self.frameCount):
            time = frame * self.framePeriod
            channelData = self._readFrame()
            for joint in self.model.joints:
                for chan in joint.channels:
                    chanData = channelData.pop(0)
                    if chan == "Xposition":
                        xPos = chanData
                    elif chan == "Yposition":
                        yPos = chanData
                    elif chan == "Zposition":
                        zPos = chanData
                    elif chan == "Xrotation":
                        xRot = chanData
                    elif chan == "Yrotation":
                        yRot = chanData
                    elif chan == "Zrotation":
                        zRot = chanData
                    else:
                        raise RuntimeError('Unknown channel: '+chan)

                if not joint.hasParent:
                    try:
                        p = np.array([[xPos,yPos,zPos]]).T * \
                                self.conversionFactor

                        p = convertCGtoNED(p)