Kinect 开发 —— 骨骼追踪 （下）

public partial class MainWindow : Window
    {
        private KinectSensor kinectDevice;
        private Skeleton[] frameSkeletons;
        private WriteableBitmap depthImage;
        private Int32Rect depthImageRect;
        private short[] depthPixelData;
        private int depthImageStride;
        private const float FeetPerMeters = 3.2808399f;

        private KinectSensor KinectDevice
        {
            get { return this.kinectDevice; }
            set
            {
                if (kinectDevice != null)
                {
                    kinectDevice = null;
                    this.kinectDevice.Stop();
                    this.kinectDevice.AllFramesReady -= kinectDevice_AllFramesReady;
                    this.kinectDevice.SkeletonStream.Disable();


                    this.DepthImage.Source = null;
                    this.depthImage = null;
                    this.depthImageStride = 0;

                    this.frameSkeletons = null;

                    this.frameSkeletons = null;
                }

                kinectDevice = value;

                if (kinectDevice != null)
                {
                    if (kinectDevice.Status == KinectStatus.Connected)
                    {
                        kinectDevice.AllFramesReady+=kinectDevice_AllFramesReady;
                        kinectDevice.SkeletonStream.Enable();
                        kinectDevice.DepthStream.Enable();

                        DepthImageStream depthStream = this.kinectDevice.DepthStream;
                        this.depthImage = new WriteableBitmap(depthStream.FrameWidth, depthStream.FrameHeight, 96, 96, PixelFormats.Bgr32, null);
                        this.depthImageRect = new Int32Rect(0,0,(int)Math.Ceiling(this.depthImage.Width),(int)Math.Ceiling(this.depthImage.Height));
                        this.depthImageStride = depthStream.FrameWidth * 4;
                        this.depthPixelData = new short[depthStream.FramePixelDataLength];
                        this.DepthImage.Source = this.depthImage;

                        this.frameSkeletons = new Skeleton[this.kinectDevice.SkeletonStream.FrameSkeletonArrayLength];
                        kinectDevice.Start();
                    }
                }
            }
        }
        public MainWindow()
        {
            InitializeComponent();

            this.Loaded += (s, e) =>
                {
                    KinectSensor.KinectSensors.StatusChanged += KinectSensors_StatusChanged;
                    this.KinectDevice = KinectSensor.KinectSensors.FirstOrDefault(x => x.Status == KinectStatus.Connected);
                };
        }

        private void KinectSensors_StatusChanged(Object sender, StatusChangedEventArgs e)
        {
            switch (e.Status)
            {
                case KinectStatus.Initializing:
                case KinectStatus.Connected:
                case KinectStatus.NotPowered:
                case KinectStatus.NotReady:
                case KinectStatus.DeviceNotGenuine:
                    this.KinectDevice = e.Sensor;
                    break;
                case KinectStatus.Disconnected:
                    //TODO: Give the user feedback to plug-in a Kinect device.                    
                    this.KinectDevice = null;
                    break;
                default:
                    //TODO: Show an error state
                    break;
            }
        }

        private void kinectDevice_AllFramesReady(Object sender, AllFramesReadyEventArgs e)
        {
            using (DepthImageFrame depthFrame = e.OpenDepthImageFrame())
            {
                if (depthFrame != null)
                {
                    using (SkeletonFrame skeletonFrame = e.OpenSkeletonFrame())
                    {
                        if (skeletonFrame != null)
                        {
                            ProcessDepthFrame(depthFrame);
                            ProcessSkeletonFrame(skeletonFrame);
                        }
                    }
                }
            }
        }

        private void ProcessDepthFrame(DepthImageFrame depthFrame)
        {
            int depth;
            int gray;
            int bytesPerPixel = 4;
            byte[] enhPixelData = new byte[depthFrame.Width*depthFrame.Height*bytesPerPixel];

            depthFrame.CopyPixelDataTo(this.depthPixelData);

            for (int i = 0, j = 0; i < this.depthPixelData.Length;i++,j+=bytesPerPixel )
            {
                depth = this.depthPixelData[i] >> DepthImageFrame.PlayerIndexBitmaskWidth;

                if (depth==0)
                {
                    gray = 0xFF;
                }
                else
                {
                    gray = (255 * depth / 0xFFF);
                }
                enhPixelData[j] = (byte)gray;
                enhPixelData[j + 1] = (byte)gray;
                enhPixelData[j + 2] = (byte)gray;
            }

            this.depthImage.WritePixels(this.depthImageRect, enhPixelData, this.depthImageStride, 0);
        }

        private void ProcessSkeletonFrame(SkeletonFrame frame)
        {
            frame.CopySkeletonDataTo(this.frameSkeletons);
            Skeleton skeleton = GetPrimarySkeleton(this.frameSkeletons);
            if (skeleton != null)
            {
                TrackHand(skeleton.Joints[JointType.HandLeft], LeftHandElement, LeftHandScaleTransform, LayoutRoot, true);
                TrackHand(skeleton.Joints[JointType.HandRight], RightHandElement, RightHandScaleTransform, LayoutRoot, false);
            }
        }

        private Skeleton GetPrimarySkeleton(Skeleton[] skeleton)
        {
            Skeleton primarySkeleton = null;
            if (skeleton != null)
            {
                for (int i = 0; i < skeleton.Length; i++)
                {
                    if (skeleton[i].TrackingState == SkeletonTrackingState.Tracked)
                    {
                        if (primarySkeleton == null)
                        {
                            primarySkeleton = skeleton[i];
                        }
                        else
                        {
                            if (primarySkeleton.Position.Z > skeleton[i].Position.Z)
                            {
                                primarySkeleton = skeleton[i];
                            }
                        }

                    }
                }
            }
            return primarySkeleton;
        }

        private void TrackHand(Joint hand, FrameworkElement cursorElement, ScaleTransform cursorScale, FrameworkElement container, bool isLeft)
        {
            if (hand.TrackingState != JointTrackingState.NotTracked)
            {
                double z = hand.Position.Z * FeetPerMeters;
                //double z = hand.Position.Z ;
                cursorElement.Visibility = System.Windows.Visibility.Visible;
                Point cursorCenter = new Point(cursorElement.ActualWidth / 2.0, cursorElement.ActualHeight / 2.0);

                Point jointPoint = GetJointPoint(this.KinectDevice, hand, container.RenderSize, cursorCenter);
                Canvas.SetLeft(cursorElement, jointPoint.X);
                Canvas.SetTop(cursorElement, jointPoint.Y);
                Canvas.SetZIndex(cursorElement, (int)(1200 - (z * 100)));

                cursorScale.ScaleX = 12 / z * ((isLeft) ? -1 : 1);
                cursorScale.ScaleY = 12 / z;
                if (hand.JointType == JointType.HandLeft)
                {
                    DebugLeftHand.Text = String.Format("Left Hand:{0:0.00} Feet", z);
                }
                else
                {
                    DebugRightHand.Text = String.Format("Right Hand:{0:0.00} Feet", z);
                }
            }
            else
            {
                DebugLeftHand.Text = String.Empty;
                DebugRightHand.Text = String.Empty;
            }
        }

        private Point GetJointPoint(KinectSensor kinectSensor, Joint hand, Size containerSize, Point cursorCenter)
        {
            DepthImagePoint point = kinectSensor.MapSkeletonPointToDepth(hand.Position, KinectDevice.DepthStream.Format);
            point.X = (int)((point.X * containerSize.Width / kinectSensor.DepthStream.FrameWidth) - cursorCenter.X);
            point.Y = (int)((point.Y * containerSize.Height / kinectSensor.DepthStream.FrameHeight) - cursorCenter.Y);
            return new Point(point.X, point.Y);
        }
    }