# 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

            if p.isJoint:
                if not hasattr(p, "channels"):
                    if not p.hasParent:
                        p.channels = ["Xposition", "Yposition", "Zposition",
                            "Zrotation", "Xrotation", "Yrotation"]
                    else:
                        p.channels = ["Zrotation", "Xrotation", "Yrotation"]

                self.bvhFile.write("%sCHANNELS %d" %(pad*(d+1),len(p.channels)))
                for chan in p.channels:
                    self.bvhFile.write(" %s" % chan)
                self.bvhFile.write("\n")

        self.bvhFile.write("MOTION\nFrames: %d\nFrame Time: %.8f\n" %(
            self.frames,self.samplePeriod))

    def writeFrame(self, t):
        for j in self.model.joints:
            if hasattr(j,'channels') and len(j.channels) > 0:

                if not j.hasParent:
                    pos = j.position(t) * self.conversionFactor
                    pos = convertNEDtoCG(pos)

                rot = j.rotation(t)
                if j.hasParent:
                    # BVH rotations are relative to parent joint
                    # RigidBodyModel rotations are absolute
                    rot = j.parent.rotation(t).conjugate * rot

# 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


            if p.isJoint:
                if not hasattr(p, "channels"):
                    if not p.hasParent:
                        p.channels = ["Xposition", "Yposition", "Zposition",
                            "Zrotation", "Xrotation", "Yrotation"]
                    else:
                        p.channels = ["Zrotation", "Xrotation", "Yrotation"]

                self.bvhFile.write("%sCHANNELS %d" %(pad*(d+1),len(p.channels)))
                for chan in p.channels:
                    self.bvhFile.write(" %s" % chan)
                self.bvhFile.write("\n")

        self.bvhFile.write("MOTION\nFrames: %d\nFrame Time: %.8f\n" %(
            self.frames,self.samplePeriod))

    def writeFrame(self, t):
        for j in self.model.joints:
            if hasattr(j,'channels') and len(j.channels) > 0:

                if not j.hasParent:
                    pos = j.position(t) * self.conversionFactor
                    pos = convertNEDtoCG(pos)

                rot = j.rotation(t)
                if j.hasParent:
                    # BVH rotations are relative to parent joint
                    # RigidBodyModel rotations are absolute
                    rot = j.parent.rotation(t).conjugate * rot

                rot = convertNEDtoCG(rot)