/Gui/FMSPlayBack/Content/Kinect/SkeletonStreamRenderer.cs

using System.Windows;
using System.Windows.Controls;
using System.Windows.Media;
using Microsoft.Kinect;
using SkeletonReaderWriter;

namespace FMSPlayBack
{
    class SkeletonStreamRenderer : UserControl
    {
        /// <summary>
        /// Width of output drawing
        /// </summary>
        private const float RenderWidth = 640.0f;

        /// <summary>
        /// Height of our output drawing
        /// </summary>
        private const float RenderHeight = 480.0f;

        /// <summary>
        /// Thickness of drawn joint lines
        /// </summary>
        private const double JointThickness = 3;

        /// <summary>
        /// Thickness of body center ellipse
        /// </summary>
        private const double BodyCenterThickness = 10;

        /// <summary>
        /// Thickness of clip edge rectangles
        /// </summary>
        private const double ClipBoundsThickness = 10;

        /// <summary>
        /// Brush used to draw skeleton center point
        /// </summary>
        private readonly Brush centerPointBrush = Brushes.Blue;

        /// <summary>
        /// Brush used for drawing joints that are currently tracked
        /// </summary>
        private readonly Brush trackedJointBrush = new SolidColorBrush(Color.FromArgb(255, 68, 192, 68));

        /// <summary>
        /// Brush used for drawing joints that are currently inferred
        /// </summary>        
        private readonly Brush inferredJointBrush = Brushes.Yellow;

        /// <summary>
        /// Pen used for drawing bones that are currently tracked
        /// </summary>
        private readonly Pen trackedBonePen = new Pen(Brushes.Green, 6);

        /// <summary>
        /// Pen used for drawing bones that are currently inferred
        /// </summary>        
        private readonly Pen inferredBonePen = new Pen(Brushes.Gray, 1);

        /// <summary>
        /// Drawing group for skeleton rendering output
        /// </summary>
        private DrawingGroup drawingGroup;

        /// <summary>
        /// Drawing image that we will display
        /// </summary>
        private DrawingImage imageSource;

        /// <summary>
        /// Image control to show camera data in the user interface
        /// </summary>
        private Image image = new Image();

        /// <summary>
        /// Cached skeleton data
        /// </summary>
        private SkeletonReader.SkeletonFrameData skeleton;

        public SkeletonStreamRenderer()
        {
            // Create the drawing group we'll use for drawing
            drawingGroup = new DrawingGroup();

            // Create an image source that we can use in our image control
            imageSource = new DrawingImage(drawingGroup);

            // Display the drawing using our image control
            image.Source = imageSource;

            //set up a viewbox to handle uniform scaling
            Viewbox viewbox = new Viewbox();
            viewbox.Stretch = Stretch.Uniform;
            viewbox.HorizontalAlignment = HorizontalAlignment.Center;
            viewbox.VerticalAlignment = VerticalAlignment.Center;
            viewbox.Child = image;
            Content = viewbox;
        }

        /// <summary>
        /// Update rendered skeleton
        /// </summary>
        /// <param name="skeleton">skeleton data to process</param>
        public void ProcessSkeletonFrame(SkeletonReader.SkeletonFrameData skeleton)
        {
            this.skeleton = skeleton;
            ProcessSkeletonFrame();
        }

        /// <summary>
        /// Internal update to the rendered skeleton
        /// </summary>
        private void ProcessSkeletonFrame()
        {
            using (DrawingContext dc = drawingGroup.Open())
            {
                // Draw a background to set the render size
                dc.DrawRectangle(BackgroundBrush, null, new Rect(0.0, 0.0, RenderWidth, RenderHeight));

                if (skeleton != null)
                {
                    DrawBonesAndJoints(skeleton, dc);
                }

                // prevent drawing outside of our render area
                drawingGroup.ClipGeometry = new RectangleGeometry(new Rect(0.0, 0.0, RenderWidth, RenderHeight));
            }
        }

        /// <summary>
        /// Draws a skeleton's bones and joints
        /// </summary>
        /// <param name="skeleton">skeleton to draw</param>
        /// <param name="drawingContext">drawing context to draw to</param>
        private void DrawBonesAndJoints(SkeletonReader.SkeletonFrameData skeleton, DrawingContext drawingContext)
        {
            // Render Torso
            DrawBone(skeleton, drawingContext, JointType.Head, JointType.ShoulderCenter);
            DrawBone(skeleton, drawingContext, JointType.ShoulderCenter, JointType.ShoulderLeft);
            DrawBone(skeleton, drawingContext, JointType.ShoulderCenter, JointType.ShoulderRight);
            DrawBone(skeleton, drawingContext, JointType.ShoulderCenter, JointType.Spine);
            DrawBone(skeleton, drawingContext, JointType.Spine, JointType.HipCenter);
            DrawBone(skeleton, drawingContext, JointType.HipCenter, JointType.HipLeft);
            DrawBone(skeleton, drawingContext, JointType.HipCenter, JointType.HipRight);

            // Left Arm
            DrawBone(skeleton, drawingContext, JointType.ShoulderLeft, JointType.ElbowLeft);
            DrawBone(skeleton, drawingContext, JointType.ElbowLeft, JointType.WristLeft);
            DrawBone(skeleton, drawingContext, JointType.WristLeft, JointType.HandLeft);

            // Right Arm
            DrawBone(skeleton, drawingContext, JointType.ShoulderRight, JointType.ElbowRight);
            DrawBone(skeleton, drawingContext, JointType.ElbowRight, JointType.WristRight);
            DrawBone(skeleton, drawingContext, JointType.WristRight, JointType.HandRight);

            // Left Leg
            DrawBone(skeleton, drawingContext, JointType.HipLeft, JointType.KneeLeft);
            DrawBone(skeleton, drawingContext, JointType.KneeLeft, JointType.AnkleLeft);
            DrawBone(skeleton, drawingContext, JointType.AnkleLeft, JointType.FootLeft);

            // Right Leg
            DrawBone(skeleton, drawingContext, JointType.HipRight, JointType.KneeRight);
            DrawBone(skeleton, drawingContext, JointType.KneeRight, JointType.AnkleRight);
            DrawBone(skeleton, drawingContext, JointType.AnkleRight, JointType.FootRight);

            // Render Joints
            foreach (SkeletonReader.SkeletonFrameJoint joint in skeleton.joints)
            {
                Brush drawBrush = null;

                if (joint.trackingState == JointTrackingState.Tracked)
                {
                    drawBrush = trackedJointBrush;
                }
                else if (joint.trackingState == JointTrackingState.Inferred)
                {
                    drawBrush = inferredJointBrush;
                }

                if (drawBrush != null)
                {
                    drawingContext.DrawEllipse(drawBrush, null, SkeletonPointToScreen(joint), JointThickness, JointThickness);
                }
            }
        }

        /// <summary>
        /// Maps a 3D Point to lie within our render space and converts to Point
        /// This is done without needing to use a Kinect Sensor
        /// </summary>
        /// <param name="X">X coordinate to map to depth point</param>
        /// <param name="Y">Y coordinate to map to depth point</param>
        /// <param name="Z">Z coordinate to map to depth point</param>
        /// <returns>mapped point</returns>
        private DepthImagePoint MapSkeletonPointToDepthPoint(float X, float Y, float Z)
        {
            float pfDepthX = 0.5f + X * 285.63f / (Z * 320f);
            float pfDepthY = 0.5f - Y * 285.63f / (Z * 240f);
            float pfDepthValue = Z * 1000f;

            return new DepthImagePoint
            {
                X = (int)(pfDepthX * 640f + 0.5f),
                Y = (int)(pfDepthY * 480f + 0.5f),
                Depth = (int)(pfDepthValue + 0.5f)
            };
        }


        /// <summary>
        /// Maps a SkeletonPoint to lie within our render space and converts to Point
        /// </summary>
        /// <param name="joint">joint to get color pos from</param>
        /// <returns>mapped point</returns>
        private Point SkeletonPointToScreen(SkeletonReader.SkeletonFrameJoint joint)
        {
            if(joint.ColorPosSupported)
            {
                return new Point(joint.ColourX, joint.ColourY);
            }

            //Due to variations between Kinects, it is not valid to map to a Color Point without using the Kinect that generated the skeleton
            //As the mapping is going to be off anyway (without the Kinect) and this is a backup anyway,
            // we may as well map to depth as the skeleton is calculated off of the depth camera
            DepthImagePoint depth = MapSkeletonPointToDepthPoint(joint.X, joint.Y, joint.Z);

            return new Point(depth.X, depth.Y);
        }

        /// <summary>
        /// Draws a bone line between two joints
        /// </summary>
        /// <param name="skeleton">skeleton to draw bones from</param>
        /// <param name="drawingContext">drawing context to draw to</param>
        /// <param name="jointType0">joint to start drawing from</param>
        /// <param name="jointType1">joint to end drawing at</param>
        private void DrawBone(SkeletonReader.SkeletonFrameData skeleton, DrawingContext drawingContext, JointType jointType0, JointType jointType1)
        {
            SkeletonReader.SkeletonFrameJoint joint0 = skeleton.joints[(int)jointType0];
            SkeletonReader.SkeletonFrameJoint joint1 = skeleton.joints[(int)jointType1];

            // If we can't find either of these joints, exit
            if (joint0.trackingState == JointTrackingState.NotTracked ||
                joint1.trackingState == JointTrackingState.NotTracked)
            {
                return;
            }

            // Don't draw if both points are inferred
            if (joint0.trackingState == JointTrackingState.Inferred &&
                joint1.trackingState == JointTrackingState.Inferred)
            {
                return;
            }

            // We assume all drawn bones are inferred unless BOTH joints are tracked
            Pen drawPen = inferredBonePen;
            if (joint0.trackingState == JointTrackingState.Tracked
                && joint1.trackingState == JointTrackingState.Tracked)
            {
                drawPen = trackedBonePen;
            }

            drawingContext.DrawLine(drawPen, SkeletonPointToScreen(joint0), SkeletonPointToScreen(joint1));
        }

        /// <summary>
        /// Background to use
        /// </summary>
        public Brush BackgroundBrush
        {
            get { return (Brush)GetValue(BackgroundBrushProperty); }
            set { SetValue(BackgroundBrushProperty, value); }
        }
        public static readonly DependencyProperty BackgroundBrushProperty = DependencyProperty.Register("BackgroundBrush", typeof(Brush), typeof(SkeletonStreamRenderer), new PropertyMetadata(Brushes.Transparent, OnBackgroundBrushChanged));

        /// <summary>
        /// Handle changes to the BackgroundBrush
        /// </summary>
        /// <param name="sender">object sending the event</param>
        /// <param name="args">event arguments</param>
        private static void OnBackgroundBrushChanged(DependencyObject sender, DependencyPropertyChangedEventArgs args)
        {
            //sender should be this class
            var renderer = sender as SkeletonStreamRenderer;

            //if not, ignore it
            if (null == renderer)
            {
                return;
            }

            renderer.ProcessSkeletonFrame();
        }
    }
}