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;
}
public static Body CreateBodyData(BodyFrameData.Body input)
{
return new Body()
{
TrackingId = input.TrackingId,
Bones = BoneDef.Bones.Select(def => CreateBoneData(input, def)).ToArray(),
};
}
public static MotionBodyData CreateData(BodyFrameData bodyFrame)
{
return new MotionBodyData()
{
Bodies = (from b in bodyFrame.Bodies
where b.IsTracked
select CreateBodyData(b)).ToArray(),
};
}
public void Update(BodyFrameData frame)
{
// TODO what if multiple body is tracked
var trackedBodyIndex = Array.FindIndex(frame.Bodies, b => b.IsTracked);
if (trackedBodyIndex >= 0)
{
DrawBodies(frame.Bodies);
}
}
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 void Update(BodyFrameData frame)
{
foreach (var data in frame.Bodies)
{
if (data.IsTracked)
{
var body = bodies.Find(b => b.TrackingId == data.TrackingId);
if (body == null)
{
body = new Body(viewport, MotionBodyData.CreateBodyData(data));
bodies.Add(body);
}
body.Update(data);
}
}
}
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(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;
}
public BodyFrameUpdatedEventArgs(BodyFrameData frame)
{
Frame = frame;
}
private static double GetBoneLength(BodyFrameData.Body body, JointType from, JointType to)
{
var fromPos = body.Joints[from].Position3D;
var toPos = body.Joints[to].Position3D;
return (fromPos - toPos).Length;
}
public void AppendFrame(BodyFrameData.Body body)
{
var position = body.Joints[JointType.SpineBase].Position3D;
var rotation = body.Joints[JointType.SpineBase].Rotation;
//var rotation = Quaternion.Identity;
frames.Add(new BvhFrame(position, rotation));
foreach (var bone in Bones)
{
bone.AppendFrame(body);
}
}
private static double GetBoneLength(BodyFrameData.Body body, JointType from1, JointType to1, JointType from2, JointType to2)
{
var length1 = GetBoneLength(body, from1, to1);
var length2 = GetBoneLength(body, from2, to2);
return (length1 + length2) / 2;
}
private void DrawBodies(BodyFrameData.Body[] bodies)
{
using (var context = drawingGroup.Open())
{
// Draw a transparent background to set the render size
context.DrawRectangle(Brushes.Black, null, new Rect(0, 0, width, height));
int penIndex = 0;
foreach (var body in bodies)
{
var pen = bodyPens[penIndex++];
if (body.IsTracked)
{
DrawClippedEdges(context, body);
DrawBody(context, pen, body.Joints);
DrawHand(context, body.HandLeft.State, body.Joints[JointType.HandLeft]);
DrawHand(context, body.HandRight.State, body.Joints[JointType.HandRight]);
}
}
// prevent drawing outside of our render area
drawingGroup.ClipGeometry = new RectangleGeometry(new Rect(0.0, 0.0, width, height));
}
}
void DrawHand(DrawingContext context, HandState state, BodyFrameData.Joint joint)
{
Brush brush;
switch (state)
{
case HandState.Closed:
brush = handClosedBrush;
break;
case HandState.Open:
brush = handOpenBrush;
break;
case HandState.Lasso:
brush = handLassoBrush;
break;
default:
brush = null;
break;
}
if (brush != null)
{
context.DrawEllipse(brush, null,
joint.Position2D,
Settings.Default.HandSize,
Settings.Default.HandSize);
}
}
private void DrawClippedEdges(DrawingContext context, BodyFrameData.Body body)
{
var thickness = Settings.Default.ClipBoundsThickness;
var clippedEdges = body.ClippedEdges;
if (clippedEdges.HasFlag(FrameEdges.Bottom))
{
context.DrawRectangle(
Brushes.Red,
null,
new Rect(0, height - thickness, width, thickness));
}
if (clippedEdges.HasFlag(FrameEdges.Top))
{
context.DrawRectangle(
Brushes.Red,
null,
new Rect(0, 0, width, thickness));
}
if (clippedEdges.HasFlag(FrameEdges.Left))
{
context.DrawRectangle(
Brushes.Red,
null,
new Rect(0, 0, thickness, height));
}
if (clippedEdges.HasFlag(FrameEdges.Right))
{
context.DrawRectangle(
Brushes.Red,
null,
new Rect(width - thickness, 0, thickness, height));
}
}
public void AppendFrame(BodyFrameData.Body body)
{
var position = body.Joints[name].Position3D;
var parentName = GetParentName(this);
var parentPosition = body.Joints[parentName].Position3D;
Quaternion rotation;
if (parent != null)
{
var ancestorName = GetParentName(parent);
var ancestorPosition = body.Joints[ancestorName].Position3D;
var parentDirection = parentPosition - ancestorPosition;
var direction = position - parentPosition;
rotation = QuaternionHelper.LookRotation(parentDirection, parent.tPoseDirection);
rotation.Invert();
rotation = rotation * QuaternionHelper.LookRotation(direction, tPoseDirection);
logger.Trace("{0} ({1}) -> {2} ({3}) -> {4} ({5}) : {6} -> {7} : {8} / {9}"
, ancestorName, ancestorPosition
, parentName, parentPosition
, name, position
, parentDirection, direction
, rotation, QuaternionHelper.ToEulerAngles(rotation));
}
else
{
var direction = position - parentPosition;
rotation = QuaternionHelper.LookRotation(direction, tPoseDirection);
logger.Trace("{0} ({1}) -> {2} ({3}) : {4} : {5} / {6}"
, parentName, parentPosition
, name, position
, direction
, rotation, QuaternionHelper.ToEulerAngles(rotation));
}
frames.Add(new BvhFrame(rotation));
}
public Skeleton(BodyFrameData.Body body)
{
initialPosition = body.Joints[JointType.SpineBase].Position3D;
initialRotation = Quaternion.Identity; // TODO: Is rotation of SpineBase necessary
// directions
var up = new Vector3D(0, 1, 0);
var down = new Vector3D(0, -1, 0);
var left = new Vector3D(-1, 0, 0);
var right = new Vector3D(1, 0, 0);
var forward = new Vector3D(0, 0, 1);
var back = new Vector3D(0, 0, -1);
// Body
var spineMidLength = GetBoneLength(body, JointType.SpineMid, JointType.SpineBase);
var spineMid = CreateBone(JointType.SpineMid, null, spineMidLength, up);
var spineShoulderLength = GetBoneLength(body, JointType.SpineMid, JointType.SpineShoulder);
var spineShoulder = CreateBone(JointType.SpineShoulder, spineMid, spineShoulderLength, up);
var neckLength = GetBoneLength(body, JointType.SpineShoulder, JointType.Neck);
var neck = CreateBone(JointType.Neck, spineShoulder, neckLength, up);
var headLength = GetBoneLength(body, JointType.Neck, JointType.Head);
var head = CreateBone(JointType.Head, neck, headLength, up);
// Arm
var shoulderLength = GetBoneLength(body, JointType.SpineShoulder, JointType.ShoulderLeft, JointType.SpineShoulder, JointType.ShoulderRight);
var shoulderLeft = CreateBone(JointType.ShoulderLeft, spineShoulder, shoulderLength, left);
var shoulderRight = CreateBone(JointType.ShoulderRight, spineShoulder, shoulderLength, right);
var elbowLength = GetBoneLength(body, JointType.ShoulderLeft, JointType.ElbowLeft, JointType.ShoulderRight, JointType.ElbowRight);
var elbowLeft = CreateBone(JointType.ElbowLeft, shoulderLeft, elbowLength, left);
var elbowRight = CreateBone(JointType.ElbowRight, shoulderRight, elbowLength, right);
var wristLength = GetBoneLength(body, JointType.ElbowLeft, JointType.WristLeft, JointType.ElbowRight, JointType.WristRight);
var wristLeft = CreateBone(JointType.WristLeft, elbowLeft, wristLength, left);
var wristRight = CreateBone(JointType.WristRight, elbowRight, wristLength, right);
var handLength = GetBoneLength(body, JointType.WristLeft, JointType.HandLeft, JointType.WristRight, JointType.HandRight);
var handLeft = CreateBone(JointType.HandLeft, wristLeft, handLength, left);
var handRight = CreateBone(JointType.HandRight, wristRight, handLength, right);
var handTipLength = GetBoneLength(body, JointType.HandLeft, JointType.HandTipLeft, JointType.HandRight, JointType.HandTipRight);
var handTipLeft = CreateBone(JointType.HandTipLeft, handLeft, handTipLength, left);
var handTipRight = CreateBone(JointType.HandTipRight, handRight, handTipLength, right);
var thumbLength = GetBoneLength(body, JointType.HandLeft, JointType.ThumbLeft, JointType.HandRight, JointType.ThumbRight);
var thumbLeft = CreateBone(JointType.ThumbLeft, handLeft, thumbLength, forward);
var thumbRight = CreateBone(JointType.ThumbRight, handRight, thumbLength, forward);
// Leg
var hipLength = GetBoneLength(body, JointType.SpineBase, JointType.HipLeft, JointType.SpineBase, JointType.HipRight);
var hipLeft = CreateBone(JointType.HipLeft, null, hipLength, left);
var hipRight = CreateBone(JointType.HipRight, null, hipLength, right);
var kneeLength = GetBoneLength(body, JointType.HipLeft, JointType.KneeLeft, JointType.HipRight, JointType.KneeRight);
var kneeLeft = CreateBone(JointType.KneeLeft, hipLeft, kneeLength, down);
var kneeRight = CreateBone(JointType.KneeRight, hipRight, kneeLength, down);
var ankleLength = GetBoneLength(body, JointType.KneeLeft, JointType.AnkleLeft, JointType.KneeRight, JointType.AnkleRight);
var ankleLeft = CreateBone(JointType.AnkleLeft, kneeLeft, ankleLength, down);
var ankleRight = CreateBone(JointType.AnkleRight, kneeRight, ankleLength, down);
var footLength = GetBoneLength(body, JointType.AnkleLeft, JointType.FootLeft, JointType.AnkleRight, JointType.FootRight);
var footLeft = CreateBone(JointType.FootLeft, ankleLeft, footLength, forward);
var footRight = CreateBone(JointType.FootRight, ankleRight, footLength, forward);
}
public void Update(BodyFrameData.Body data)
{
var motion = MotionFrameData.CreateBodyData(data, this.bodyData);
foreach (var bone in motion.Bones)
{
var boneDef = BoneDef.Find(bone.Type);
var boneLength = this.bodyData.FindBone(bone.Type).Length;
if (boneDef.ParentType == BoneType.Root)
{
joints[boneDef.HeadJointType].Transform = new TranslateTransform3D(bone.HeadPosition);
}
joints[boneDef.TailJointType].Transform = new TranslateTransform3D(bone.TailPosition);
var rotation = new AxisAngleRotation3D(bone.Rotation.Axis, bone.Rotation.Angle);
var transforms = new Transform3DGroup();
transforms.Children.Add(new ScaleTransform3D(new Vector3D(boneLength, boneLength, boneLength)));
transforms.Children.Add(new TranslateTransform3D(bone.HeadPosition));
transforms.Children.Add(new RotateTransform3D(rotation, bone.HeadPosition.X, bone.HeadPosition.Y, bone.HeadPosition.Z));
bones[bone.Type].Transform = transforms;
}
}
