Kinect-Project/Sources/Lib/ColorAndBodyImageStream.cs

using Microsoft.Kinect;
using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;
using System.Windows.Controls;
using System.Windows.Media.Imaging;
using System.Windows.Media;
using System.Windows.Shapes;
using System.Windows;

namespace Lib
{
    public class ColorAndBodyImageStream : KinectStream
    {
        private BodyFrameReader reader;
        public BodyFrameReader Reader
        {
            get { return reader; }
            private set { reader = value; }
        }

        private ColorFrameReader _colorReader;
        public ColorFrameReader ColorReader { 
            get { return _colorReader; }
            private set { _colorReader = value; }
        }

        private Body[] bodies;
        public Body[] Bodies
        {
            get { return bodies; }
            private set { bodies = value; }
        }
        public override void Stop()
        {
            if (Reader != null)
            {
                Reader.Dispose();
                Reader = null;
            }
            if(ColorReader != null)
            {
                ColorReader.Dispose();
                ColorReader = null;
            }

        }

        public ColorAndBodyImageStream(KinectManager kinectmanager, Canvas skeletonCanvas) : base(kinectmanager)
        {
            var framedescription = kinectmanager.Sensor.ColorFrameSource.CreateFrameDescription(ColorImageFormat.Bgra);
            Bitmap = new WriteableBitmap(framedescription.Width, framedescription.Height, 96.0, 96.0, PixelFormats.Bgr32, null);
            reader = kinectmanager.Sensor.BodyFrameSource.OpenReader();
            reader.FrameArrived += Reader_BodyFrameArrived;
            ColorReader = KinectManager.Sensor.ColorFrameSource.OpenReader();
            ColorReader.FrameArrived += Reader_ColorFrameArrived;
            // initialiser le tableau des corps
            this.bodies = new Body[kinectmanager.Sensor.BodyFrameSource.BodyCount];
            Canvas = skeletonCanvas;
        }

        private void drawbone(Body body, JointType JointType0, JointType JointType1)
        {
            Joint joint0 = body.Joints[JointType0];
            Joint joint1 = body.Joints[JointType1];

            // ne dessinez que si les deux joints sont suivis
            if (joint0.TrackingState == TrackingState.Tracked && joint1.TrackingState == TrackingState.Tracked)
            {
                Line bone = new Line
                {
                    Stroke = new SolidColorBrush(Colors.LightBlue),
                    StrokeThickness = 4,
                    X1 = MapJointToScreen(joint0).X,
                    Y1 = MapJointToScreen(joint0).Y,
                    X2 = MapJointToScreen(joint1).X,
                    Y2 = MapJointToScreen(joint1).Y
                };

                Canvas.Children.Add(bone);
            }
        }

        private Point MapJointToScreen(Joint joint)
        {
            ColorSpacePoint colorPoint = this.KinectManager.Sensor.CoordinateMapper.MapCameraPointToColorSpace(joint.Position);

            // Gestion des coordonnées infinies
            float x = float.IsInfinity(colorPoint.X) ? 0 : colorPoint.X;
            float y = float.IsInfinity(colorPoint.Y) ? 0 : colorPoint.Y;

            return new Point(x, y);
        }

        private void drawskeleton(Body body)
        {
            // tête et cou
            drawbone(body, JointType.Head, JointType.Neck);
            drawbone(body, JointType.Neck, JointType.SpineShoulder);

            // torse
            drawbone(body, JointType.SpineShoulder, JointType.SpineMid);
            drawbone(body, JointType.SpineMid, JointType.SpineBase);
            drawbone(body, JointType.SpineShoulder, JointType.ShoulderRight);
            drawbone(body, JointType.SpineShoulder, JointType.ShoulderLeft);
            drawbone(body, JointType.SpineBase, JointType.HipRight);
            drawbone(body, JointType.SpineBase, JointType.HipLeft);

            // bras droit
            drawbone(body, JointType.ShoulderRight, JointType.ElbowRight);
            drawbone(body, JointType.ElbowRight, JointType.WristRight);
            drawbone(body, JointType.WristRight, JointType.HandRight);
            drawbone(body, JointType.HandRight, JointType.HandTipRight);
            drawbone(body, JointType.WristRight, JointType.ThumbRight);

            // bras gauche
            drawbone(body, JointType.ShoulderLeft, JointType.ElbowLeft);
            drawbone(body, JointType.ElbowLeft, JointType.WristLeft);
            drawbone(body, JointType.WristLeft, JointType.HandLeft);
            drawbone(body, JointType.HandLeft, JointType.HandTipLeft);
            drawbone(body, JointType.WristLeft, JointType.ThumbLeft);

            // jambe droite
            drawbone(body, JointType.HipRight, JointType.KneeRight);
            drawbone(body, JointType.KneeRight, JointType.AnkleRight);
            drawbone(body, JointType.AnkleRight, JointType.FootRight);

            // jambe gauche
            drawbone(body, JointType.HipLeft, JointType.KneeLeft);
            drawbone(body, JointType.KneeLeft, JointType.AnkleLeft);
            drawbone(body, JointType.AnkleLeft, JointType.FootLeft);

            // dessinez les joints
            foreach (JointType JointType in body.Joints.Keys)
            {
                Joint joint = body.Joints[JointType];
                if (joint.TrackingState == TrackingState.Tracked)
                {
                    DrawJoint(MapJointToScreen(joint));
                }
            }
        }

        private void DrawJoint(Point point)
        {
            Ellipse ellipse = new Ellipse
            {
                Width = 10,
                Height = 10,
                Fill = new SolidColorBrush(Colors.Red)
            };

            Canvas.SetLeft(ellipse, point.X - ellipse.Width / 2);
            Canvas.SetTop(ellipse, point.Y - ellipse.Height / 2);

            Canvas.Children.Add(ellipse);
        }
        private void Reader_BodyFrameArrived(object sender, BodyFrameArrivedEventArgs e)
        {
            using (var bodyframe = e.FrameReference.AcquireFrame())
            {
                if (bodyframe != null)
                {
                    bodyframe.GetAndRefreshBodyData(this.bodies);

                    Canvas.Children.Clear(); // nettoyer le Canvas avant de dessiner

                    foreach (var body in this.bodies)
                    {
                        if (body.IsTracked)
                        {
                            // dessiner le squelette
                            drawskeleton(body);
                        }
                    }
                }
            }
        }
        private void Reader_ColorFrameArrived(object sender, ColorFrameArrivedEventArgs e)
        {
            using (ColorFrame colorFrame = e.FrameReference.AcquireFrame())
            {
                if (colorFrame != null)
                {
                    // ... Logique existante pour traiter la frame
                    //Debug.WriteLine("Traitement de la frame de couleur.");
                    FrameDescription colorFrameDescription = colorFrame.FrameDescription;

                    using (KinectBuffer colorBuffer = colorFrame.LockRawImageBuffer())
                    {
                        this.Bitmap.Lock();

                        // Vérifier si la taille de l'image a changé
                        if ((colorFrameDescription.Width == this.Bitmap.PixelWidth) && (colorFrameDescription.Height == this.Bitmap.PixelHeight))
                        {
                            colorFrame.CopyConvertedFrameDataToIntPtr(
                                this.Bitmap.BackBuffer,
                                (uint)(colorFrameDescription.Width * colorFrameDescription.Height * 4),
                                ColorImageFormat.Bgra);

                            this.Bitmap.AddDirtyRect(new Int32Rect(0, 0, this.Bitmap.PixelWidth, this.Bitmap.PixelHeight));
                        }

                        this.Bitmap.Unlock();
                    }
                    //Debug.WriteLine("Frame de couleur traitée.");
                }
            }
        }
    }
}