# 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
# 
# Path: imusim/trajectories/rigid_body.py
# 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)


from imusim.trajectories.rigid_body import SampledBodyModel, SampledJoint
from imusim.trajectories.rigid_body import PointTrajectory
from imusim.maths.vectors import vector
from imusim.maths.transforms import convertCGtoNED
from imusim.maths.quaternions import Quaternion
from imusim.utilities.trees import TreeNode
from pyparsing import Word, Literal, Keyword, Group, Dict, Combine
from pyparsing import ZeroOrMore, OneOrMore, Suppress, Optional, LineEnd
from pyparsing import alphanums, nums, alphas, Regex, SkipTo
                    PointTrajectory(
                            parent=imusimModel.getJoint(bone.name),
                            name=bone.name+'_end',
                            offset=bone.childoffset
                            )

        with open(amcFileName) as amcFile:
            motion = amcParser.parseFile(amcFile)
            t = 0
            for frame in motion.frames:
                for bone in frame.bones:
                    bonedata = asfModel.getBone(bone.name)
                    if bone.name == 'root':
                        data = dict((chan.lower(), v) for chan,v in
                            zip(bonedata.channels,bone.channels))
                        position = convertCGtoNED(scale * vector(data['tx'],
                            data['ty'],data['tz']))
                        imusimModel.positionKeyFrames.add(t, position)

                    axes, angles = zip(*[(chan[-1], angle) for chan, angle in
                        zip(bonedata.channels, bone.channels) if
                                chan.lower().startswith('r')])
                    rotation = (bonedata.rotationOffset.conjugate *
                            Quaternion.fromEuler(angles[::-1], axes[::-1]))
                    joint = imusimModel.getJoint(bone.name)
                    if joint.hasParent:
                        parentRot = joint.parent.rotationKeyFrames.latestValue
                        parentRotOffset = bonedata.parent.rotationOffset
                        rotation = parentRot * parentRotOffset * rotation
                    else:

# 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
# 
# Path: imusim/trajectories/rigid_body.py
# 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)


# Path: imusim/io/asf_amc.py
from imusim.trajectories.rigid_body import SampledBodyModel, SampledJoint
from imusim.trajectories.rigid_body import PointTrajectory
from imusim.maths.vectors import vector
from imusim.maths.transforms import convertCGtoNED
from imusim.maths.quaternions import Quaternion
from imusim.utilities.trees import TreeNode
from pyparsing import Word, Literal, Keyword, Group, Dict, Combine
from pyparsing import ZeroOrMore, OneOrMore, Suppress, Optional, LineEnd
from pyparsing import alphanums, nums, alphas, Regex, SkipTo

                    PointTrajectory(
                            parent=imusimModel.getJoint(bone.name),
                            name=bone.name+'_end',
                            offset=bone.childoffset
                            )

        with open(amcFileName) as amcFile:
            motion = amcParser.parseFile(amcFile)
            t = 0
            for frame in motion.frames:
                for bone in frame.bones:
                    bonedata = asfModel.getBone(bone.name)
                    if bone.name == 'root':
                        data = dict((chan.lower(), v) for chan,v in
                            zip(bonedata.channels,bone.channels))
                        position = convertCGtoNED(scale * vector(data['tx'],
                            data['ty'],data['tz']))
                        imusimModel.positionKeyFrames.add(t, position)

                    axes, angles = zip(*[(chan[-1], angle) for chan, angle in
                        zip(bonedata.channels, bone.channels) if
                                chan.lower().startswith('r')])
                    rotation = (bonedata.rotationOffset.conjugate *
                            Quaternion.fromEuler(angles[::-1], axes[::-1]))
                    joint = imusimModel.getJoint(bone.name)
                    if joint.hasParent:
                        parentRot = joint.parent.rotationKeyFrames.latestValue
                        parentRotOffset = bonedata.parent.rotationOffset
                        rotation = parentRot * parentRotOffset * rotation
                    else:

                        rotation = convertCGtoNED(rotation)