private DrawingImage DrawImage(ColorImageFrame colorFrame, WagSkeleton[] skeletons)
{
DrawingGroup dgColorImageAndSkeleton = new DrawingGroup();
DrawingImage drawingImage = new DrawingImage(dgColorImageAndSkeleton);
skeletonDrawer = new SkeletonDrawer(kinectSensor);
using (DrawingContext drawingContext = dgColorImageAndSkeleton.Open())
{
InitializeDrawingImage(colorFrame, drawingContext);
if (skeletons != null && skeletons.Count() > 0)
{
foreach (WagSkeleton skeleton in skeletons)
{
skeletonDrawer.DrawUpperSkeleton(skeleton, drawingContext);
Joint head = skeleton.Joints.SingleOrDefault(temp => temp.JointType == JointType.Head);
System.Windows.Point headP = skeletonDrawer.SkeletonPointToScreen(head.Position);
System.Windows.Point socialDataP = headP;
socialDataP.Y = headP.Y - 50;
System.Windows.Point simpleDataP = headP;
simpleDataP.Y = headP.Y - 90;
//FormattedText
drawingContext.DrawText(
new FormattedText(skeleton.AttentionSocial.ToString(), CultureInfo.GetCultureInfo("en-us"),
FlowDirection.LeftToRight, new Typeface("Verdana"), 20, System.Windows.Media.Brushes.Green),
socialDataP);
drawingContext.DrawText(
new FormattedText(skeleton.AttentionSimple.ToString(), CultureInfo.GetCultureInfo("en-us"),
FlowDirection.LeftToRight, new Typeface("Verdana"), 20, System.Windows.Media.Brushes.Green),
simpleDataP);
}
}
}
//Make sure the image remains within the defined width and height
dgColorImageAndSkeleton.ClipGeometry = new RectangleGeometry(new System.Windows.Rect(0.0, 0.0, RENDER_WIDTH, RENDER_HEIGHT));
return drawingImage;
}
private Point3D CalculateHeadLocation(WagSkeleton skeleton)
{
Point3D headLocation = new Point3D(0, 0, 0);
Joint head = skeleton.TransformedJoints[JointType.Head];
if (head != null && head.TrackingState == JointTrackingState.Tracked)
headLocation = new Point3D(head.Position.X, head.Position.Y, head.Position.Z);
return headLocation;
}
private Vector3D CalculateHeadOrientation(WagSkeleton skeleton)
{
Vector3D headOrientation = new Vector3D(0, 0, -1);
FaceTrackFrame face = skeleton.FaceFrame;
var FacePoints = face.Get3DShape();
Vector3DF eyeLeft = FacePoints[LEFT_EYE];
Vector3DF eyeRight = FacePoints[RIGHT_EYE];
Vector3DF faceTop = FacePoints[FACE_TOP];
Vector3DF faceBottom = FacePoints[FACE_BOTTOM];
Vector3D faceVectorHorizontal = new Vector3D(eyeLeft.X - eyeRight.X, eyeLeft.Y - eyeRight.Y, eyeLeft.Z - eyeRight.Z);
Vector3D faceVectorVertical = new Vector3D(faceTop.X - faceBottom.X, faceTop.Y - faceBottom.Y, faceTop.Z - faceBottom.Z);
headOrientation = Vector3D.CrossProduct(faceVectorHorizontal, faceVectorVertical);
headOrientation = originTransform.Transform(headOrientation);
headOrientation.Normalize();
Matrix3D headPointsPointUpMatrix = new Matrix3D();
headPointsPointUpMatrix.RotateAt(new Quaternion(new Vector3D(1, 0, 0), -20), skeleton.TransformedJoints[JointType.Head].Position.ToPoint3D());
Vector3D lowered = headPointsPointUpMatrix.Transform(headOrientation);
if (headOrientation.Z > 0)
throw new Exception("Right hand rule violation");
return lowered;
}
private void ApplyTransformations(WagSkeleton skeleton)
{
skeleton.TransformedPosition = originTransform.Transform(new Point3D()
{
X = skeleton.Position.X,
Y = skeleton.Position.Y,
Z = skeleton.Position.Z
});
foreach (JointType type in Enum.GetValues(typeof(JointType)))
{
Point3D transformpoint = originTransform.Transform(new Point3D()
{
X = skeleton.Joints[type].Position.X,
Y = skeleton.Joints[type].Position.Y,
Z = skeleton.Joints[type].Position.Z
});
SkeletonPoint jointPosition = new SkeletonPoint()
{
X = (float)transformpoint.X,
Y = (float)transformpoint.Y,
Z = (float)transformpoint.Z
};
skeleton.TransformedJoints[type] = new Joint()
{
TrackingState = skeleton.TransformedJoints[type].TrackingState,
Position = jointPosition
};
}
}
