private static Bone CreateBoneData(Body body, BoneDef boneDef, BodyFrameData.Body input, MotionBodyData.Body bodyDef)
{
var bone = new Bone(body, boneDef.Type);
// Head position
if (boneDef.ParentType == BoneType.Root)
{
bone.HeadPosition = input.Joints[boneDef.HeadJointType].Position3D;
}
else
{
bone.HeadPosition = body.FindBone(boneDef.ParentType).TailPosition;
}
// Tail position
var boneLength = bodyDef.FindBone(boneDef.Type).Length;
var rawHeadPosition = input.Joints[boneDef.HeadJointType].Position3D;
var rawTailPosition = input.Joints[boneDef.TailJointType].Position3D;
var direction = rawTailPosition - rawHeadPosition;
direction.Normalize();
bone.TailPosition = bone.HeadPosition + direction * boneLength;
// Rotation
if (boneDef.IsEnd)
{
var upward = new Vector3D(0, 1, 0);
bone.Rotation = QuaternionHelper.LookRotation(rawTailPosition - rawHeadPosition, upward);
}
else
{
bone.Rotation = input.Joints[boneDef.TailJointType].Rotation;
}
return bone;
}
