public Body(HelixViewport3D viewport, MotionBodyData.Body bodyData)
{
this.viewport = viewport;
this.bodyData = bodyData;
CreateJoints();
CreateBones(bodyData);
}
public static MotionFrameData CreateData(BodyFrameData bodyFrame, MotionBodyData bodyDefs)
{
return new MotionFrameData()
{
RelativeTime = bodyFrame.RelativeTime,
Bodies = bodyFrame.Bodies
.Where(b => b.IsTracked)
.Select(b =>
{
var bodyDef = Array.Find(bodyDefs.Bodies, bd => bd.TrackingId == b.TrackingId);
return CreateBodyData(b, bodyDef);
}).ToArray(),
};
}
public static Body CreateBodyData(BodyFrameData.Body input, MotionBodyData.Body bodyDef)
{
var body = new Body()
{
TrackingId = input.TrackingId,
Position = input.Joints[Schemas.RecorderMessages.JointType.SpineBase].Position3D,
Rotation = input.Joints[Schemas.RecorderMessages.JointType.SpineBase].Rotation,
Bones = new Bone[BoneDef.BoneCount],
};
foreach (var boneDef in BoneDef.BonesByHierarchy)
{
body.Bones[(int)boneDef.Type] = CreateBoneData(body, boneDef, input, bodyDef);
}
return body;
}
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));
}
private void CreateBones(MotionBodyData.Body data)
{
foreach (var bone in data.Bones)
{
var model = new ModelVisual3D() { Content = BoneModel };
viewport.Children.Add(model);
bones.Add(bone.Type, model);
}
}