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; }