private static double CalculateBoneLength(BodyFrameData.Body body, BoneDef boneDef)
{
var headPosition = body.Joints[boneDef.HeadJointType].Position3D;
var tailPosition = body.Joints[boneDef.TailJointType].Position3D;
return (headPosition - tailPosition).Length;
}
private static Bone CreateBoneData(BodyFrameData.Body body, BoneDef boneDef)
{
var length = CalculateBoneLength(body, boneDef);
if (boneDef.MirrorType != boneDef.Type)
{
var mirrorDef = BoneDef.Find(boneDef.MirrorType);
var mirrorLength = CalculateBoneLength(body, mirrorDef);
length = (length + mirrorLength) / 2;
}
return new Bone()
{
Type = boneDef.Type,
Length = length,
};
}
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;
}
private void WriteBone(MotionBodyData.Body body, BoneDef boneDef, int indent)
{
var bone = body.FindBone(boneDef.Type);
var indentString = CreateIndent(indent);
var offset = boneDef.ParentType == BoneType.Root
? new Vector3D(0, 0, 0)
: BoneDef.Find(boneDef.ParentType).TPoseDirection3D * body.FindBone(boneDef.ParentType).Length;
var jointStartString = string.Format(JOINT_START
, boneDef.TailJointType
, offset.X
, offset.Y
, offset.Z
, indentString);
writer.WriteLine(jointStartString);
if (boneDef.IsEnd)
{
offset = boneDef.TPoseDirection3D * body.FindBone(boneDef.Type).Length;
var endString = string.Format(END
, offset.X
, offset.Y
, offset.Z
, CreateIndent(indent + 1));
writer.WriteLine(endString);
}
else
{
foreach (var childBoneDef in BoneDef.FindChildren(boneDef.Type))
{
WriteBone(body, childBoneDef, indent + 1);
}
}
writer.WriteLine(string.Format(JOINT_END, indentString));
}
