# 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
        from the CMU motion capture corpus this should be 2.54/100 to
        convert from inches to metres.

    @return: A {SampledBodyModel} representing the root of the rigid body
        model structure.
    """
    with open(asfFileName, 'r') as asfFile:
        data = asfParser.parseFile(asfFile)
        scale = (1.0/data.units.get('length',1)) * scaleFactor

        bones = dict((bone.name,bone) for bone in data.bones)
        asfModel = ASFRoot(data.root)

        for entry in data.hierarchy:
            parent = asfModel.getBone(entry.parent)
            for childName in entry.children:
                ASFBone(parent, bones[childName], scale)

        imusimModel = SampledBodyModel('root')
        for subtree in asfModel.children:
            for bone in subtree:
                if not bone.isDummy:
                    offset = vector(0,0,0)
                    ancestors = bone.ascendTree()
                    while True:
                        ancestor = ancestors.next().parent
                        offset += ancestor.childoffset
                        if not ancestor.isDummy:
                            break


# 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

        from the CMU motion capture corpus this should be 2.54/100 to
        convert from inches to metres.

    @return: A {SampledBodyModel} representing the root of the rigid body
        model structure.
    """
    with open(asfFileName, 'r') as asfFile:
        data = asfParser.parseFile(asfFile)
        scale = (1.0/data.units.get('length',1)) * scaleFactor

        bones = dict((bone.name,bone) for bone in data.bones)
        asfModel = ASFRoot(data.root)

        for entry in data.hierarchy:
            parent = asfModel.getBone(entry.parent)
            for childName in entry.children:
                ASFBone(parent, bones[childName], scale)

        imusimModel = SampledBodyModel('root')
        for subtree in asfModel.children:
            for bone in subtree:
                if not bone.isDummy:
                    offset = vector(0,0,0)
                    ancestors = bone.ascendTree()
                    while True:
                        ancestor = ancestors.next().parent
                        offset += ancestor.childoffset
                        if not ancestor.isDummy:
                            break


                    SampledJoint(parent=imusimModel.getJoint(ancestor.name),