public Body Select(BodyFrame frame)
        {
            if (frame == null || frame.BodyCount == 0)
            {
                this.trackingId = 0;
                return null;
            }

            var bodies = new Body[frame.BodyCount];
            frame.GetAndRefreshBodyData(bodies);

            var trackedBody = bodies.FirstOrDefault(b =>
                b.TrackingId == this.trackingId &&
                b.Joints[JointType.HandLeft].Position.Y > b.Joints[JointType.HipLeft].Position.Y + 0.05);

            if (trackedBody != null)
            {
                return trackedBody;
            }

            var activeBody = bodies.FirstOrDefault(b =>
                b.IsTracked &&
                b.HandLeftState == HandState.Open &&
                b.Joints[JointType.HandLeft].Position.Y > b.Joints[JointType.ShoulderLeft].Position.Y);

            if (activeBody != null)
            {
                this.trackingId = activeBody.TrackingId;
                return activeBody;
            }

            this.trackingId = 0;
            return null;
        }
        private void RegisterGesture(BodyFrame bodyFrame)
        {
            bool dataReceived = false;
            Body[] bodies = null;

            if (bodyFrame != null)
            {
                if (bodies == null)
                {
                    // Creates an array of 6 bodies, which is the max number of bodies that Kinect can track simultaneously
                    bodies = new Body[bodyFrame.BodyCount];
                }

                // The first time GetAndRefreshBodyData is called, Kinect will allocate each Body in the array.
                // As long as those body objects are not disposed and not set to null in the array,
                // those body objects will be re-used.
                bodyFrame.GetAndRefreshBodyData(bodies);
                dataReceived = true;
            }

            if (dataReceived)
            {
                // We may have lost/acquired bodies, so update the corresponding gesture detectors
                if (bodies != null)
                {
                    // Loop through all bodies to see if any of the gesture detectors need to be updated
                    for (int i = 0; i < bodyFrame.BodyCount; ++i)
                    {
                        Body body = bodies[i];
                        ulong trackingId = body.TrackingId;

                        // If the current body TrackingId changed, update the corresponding gesture detector with the new value
                        if (trackingId != this.gestureDetectorList[i].TrackingId)
                        {
                            this.gestureDetectorList[i].TrackingId = trackingId;

                            // If the current body is tracked, unpause its detector to get VisualGestureBuilderFrameArrived events
                            // If the current body is not tracked, pause its detector so we don't waste resources trying to get invalid gesture results
                            this.gestureDetectorList[i].IsPaused = trackingId == 0;
                        }
                    }
                }
            }
        }
		private static Body FindClosestBody(BodyFrame bodyFrame)	// Finds the closest body from the sensor if any
		{
			Body result = null;
			double closestBodyDistance = double.MaxValue;
			Body[] bodies = new Body[bodyFrame.BodyCount];
			bodyFrame.GetAndRefreshBodyData(bodies);
				
			foreach (var body in bodies)
			{
				if (body.IsTracked)
				{
					var currentLocation = body.Joints[JointType.SpineBase].Position;
			    		var currentDistance = VectorLength(currentLocation);
			    		if (result == null || currentDistance < closestBodyDistance)
		    			{
			        		result = body;
			        		closestBodyDistance = currentDistance;
			    		}
				}
			}
			return result;
	        }
        private void Reader_MultiSourceFrameArrived(MultiSourceFrameReader sender, MultiSourceFrameArrivedEventArgs e)
        {
            MultiSourceFrame multiSourceFrame = e.FrameReference.AcquireFrame();

            // If the Frame has expired by the time we process this event, return.
            if (multiSourceFrame == null)
            {
                return;
            }

            DepthFrame     depthFrame     = null;
            ColorFrame     colorFrame     = null;
            InfraredFrame  infraredFrame  = null;
            BodyIndexFrame bodyIndexFrame = null;
            BodyFrame      bodyFrame      = null;
            //windows.storage.streams
            IBuffer           depthFrameDataBuffer = null;
            IBuffer           bodyIndexFrameData   = null;
            IBufferByteAccess bodyIndexByteAccess  = null; // Com interface for unsafe byte manipulation

            switch (currentDisplayFrameType)
            {
            case DisplayFrameType.Infrared:
                using (infraredFrame = multiSourceFrame.InfraredFrameReference.AcquireFrame()) {
                    ShowInfraredFrame(infraredFrame);
                }
                break;

            case DisplayFrameType.Color:
                using (colorFrame = multiSourceFrame.ColorFrameReference.AcquireFrame()) {
                    ShowColorFrame(colorFrame);
                }
                break;

            case DisplayFrameType.Depth:
                using (depthFrame = multiSourceFrame.DepthFrameReference.AcquireFrame()) {
                    ShowDepthFrame(depthFrame);
                }
                break;

            case DisplayFrameType.BodyMask:
                // Put in a try catch to utilise finally() and clean up frames
                try {
                    depthFrame     = multiSourceFrame.DepthFrameReference.AcquireFrame();
                    bodyIndexFrame = multiSourceFrame.BodyIndexFrameReference.AcquireFrame();
                    colorFrame     = multiSourceFrame.ColorFrameReference.AcquireFrame();
                    if ((depthFrame == null) || (colorFrame == null) || (bodyIndexFrame == null))
                    {
                        return;
                    }

                    // Access the depth frame data directly via LockImageBuffer to avoid making a copy
                    depthFrameDataBuffer = depthFrame.LockImageBuffer();
                    this.coordinateMapper.MapColorFrameToDepthSpaceUsingIBuffer(depthFrameDataBuffer, this.colorMappedToDepthPoints);
                    // Process Color
                    colorFrame.CopyConvertedFrameDataToBuffer(this.bitmap.PixelBuffer, ColorImageFormat.Bgra);
                    // Access the body index frame data directly via LockImageBuffer to avoid making a copy
                    bodyIndexFrameData = bodyIndexFrame.LockImageBuffer();
                    ShowMappedBodyFrame(depthFrame.FrameDescription.Width, depthFrame.FrameDescription.Height, bodyIndexFrameData, bodyIndexByteAccess);
                } finally {
                    if (depthFrame != null)
                    {
                        depthFrame.Dispose();
                    }
                    if (colorFrame != null)
                    {
                        colorFrame.Dispose();
                    }
                    if (bodyIndexFrame != null)
                    {
                        bodyIndexFrame.Dispose();
                    }

                    if (depthFrameDataBuffer != null)
                    {
                        // We must force a release of the IBuffer in order toensure that we have dropped all references to it.
                        System.Runtime.InteropServices.Marshal.ReleaseComObject(depthFrameDataBuffer);
                    }
                    if (bodyIndexFrameData != null)
                    {
                        System.Runtime.InteropServices.Marshal.ReleaseComObject(bodyIndexFrameData);
                    }
                    if (bodyIndexByteAccess != null)
                    {
                        System.Runtime.InteropServices.Marshal.ReleaseComObject(bodyIndexByteAccess);
                    }
                }
                break;

            case DisplayFrameType.BodyJoints:
                using (bodyFrame = multiSourceFrame.BodyFrameReference.AcquireFrame()) {
                    ShowBodyJoints(bodyFrame);
                }
                break;

            default:
                break;
            }
        }
Ejemplo n.º 5
0
        private void ShowBody( BodyFrame bodyFrame )
        {
            frameCount++;
            if ( sw.ElapsedMilliseconds >= 1000 ) {
                scratch.AddSensorValue( "FrameCount", frameCount.ToString() );

                sw.Restart();
                frameCount = 0;
            }

            var mapper = kinect.CoordinateMapper;

            CanvasBody.Children.Clear();

            bodyFrame.GetAndRefreshBodyData( bodies );
            var trackedBody = bodies.FirstOrDefault( b => b.IsTracked );
            if ( trackedBody != null ) {
                var body = trackedBody;
                foreach ( var jointType in body.Joints.Keys ) {
                    var joint = body.Joints[jointType];
                    if ( joint.TrackingState != TrackingState.NotTracked ) {
                        ShowJoint( mapper, joint );

                        int scale = 200;
                        AddSensorValue( jointType, "X", (int)(joint.Position.X * scale) );
                        AddSensorValue( jointType, "Y", (int)(joint.Position.Y * scale) );
                    }
                }

                if ( body.HandLeftConfidence == TrackingConfidence.High ) {
                    AddSensorValue( "HandLeftState", body.HandLeftState.ToString() );
                }
                else {
                    AddSensorValue( "HandLeftState", HandState.Unknown.ToString() );
                }

                if ( body.HandRightConfidence == TrackingConfidence.High ) {
                    AddSensorValue( "HandRightState", body.HandRightState.ToString() );
                }
                else {
                    AddSensorValue( "HandRightState", HandState.Unknown.ToString() );
                }
            }
            else {
                foreach ( JointType jointType in Enum.GetValues(typeof(JointType) ) ) {
                    AddSensorValue( jointType, "X", 0 );
                    AddSensorValue( jointType, "Y", 0 );
                }

                AddSensorValue( "HandLeftState", HandState.Unknown.ToString() );
                AddSensorValue( "HandRightState", HandState.Unknown.ToString() );
            }

            scratch.UpdateSensor();
        }
Ejemplo n.º 6
0
        /// <summary>
        /// Find if there is a body tracked with the given trackingId
        /// </summary>
        /// <param name="bodyFrame">A body frame</param>
        /// <param name="trackingId">The tracking Id</param>
        /// <returns>The body object, null of none</returns>
        private static Body FindBodyWithTrackingId(BodyFrame bodyFrame, ulong trackingId)
        {
            Body result = null;

            Body[] bodies = new Body[bodyFrame.BodyCount];
            bodyFrame.GetAndRefreshBodyData(bodies);

            foreach (var body in bodies)
            {
                if (body.IsTracked)
                {
                    if (body.TrackingId == trackingId)
                    {
                        result = body;
                        break;
                    }
                }
            }

            return result;
        }
        private void ProcessFrames(DepthFrame depthFrame, ColorFrame colorFrame, BodyIndexFrame bodyIndexFrame, BodyFrame bodyFrame)
        {



            FrameDescription depthFrameDescription = depthFrame.FrameDescription;
            FrameDescription colorFrameDescription = colorFrame.FrameDescription;
            FrameDescription bodyIndexFrameDescription = bodyIndexFrame.FrameDescription;




            int bodyIndexWidth = bodyIndexFrameDescription.Width;
            int bodyIndexHeight = bodyIndexFrameDescription.Height;


            // The ImageModel object is used to transfer Kinect data into the DataFlow rotunies. 
            ImageModel imageModel = new ImageModel()
            {
                DepthWidth = depthFrameDescription.Width,
                DepthHeight = depthFrameDescription.Height,
                ColorWidth = colorFrameDescription.Width,
                ColorHeight = colorFrameDescription.Height,
                ShowTrails = _vm.LeaveTrails,
                PersonFill = _vm.PersonFill,
                MaxDistance = _vm.BackgroundDistance
            };
            imageModel.ColorFrameData = new byte[imageModel.ColorWidth * imageModel.ColorHeight * this.bytesPerPixel];

            imageModel.DisplayPixels = new byte[_PreviousFrameDisplayPixels.Length];
            imageModel.BodyIndexFrameData = new byte[imageModel.DepthWidth * imageModel.DepthHeight];
            imageModel.ColorPoints = new ColorSpacePoint[imageModel.DepthWidth * imageModel.DepthHeight];
            imageModel.BytesPerPixel = bytesPerPixel;
            imageModel.Bodies = new Body[this.kinectSensor.BodyFrameSource.BodyCount];
            bodyFrame.GetAndRefreshBodyData(imageModel.Bodies);
            imageModel.DepthData = new ushort[imageModel.DepthWidth * imageModel.DepthHeight];
            
            depthFrame.CopyFrameDataToArray(imageModel.DepthData);
            depthFrame.CopyFrameDataToArray(this.DepthFrameData);
            
            if (colorFrame.RawColorImageFormat == ColorImageFormat.Bgra)
            {
                colorFrame.CopyRawFrameDataToArray(imageModel.ColorFrameData);
            }
            else
            {
                colorFrame.CopyConvertedFrameDataToArray(imageModel.ColorFrameData, ColorImageFormat.Bgra);
            }
            imageModel.PixelFormat = PixelFormats.Bgra32;



            _ColorBitmap.WritePixels(new Int32Rect(0, 0, imageModel.ColorWidth, imageModel.ColorHeight),
                                          imageModel.ColorFrameData,
                                          imageModel.ColorWidth * imageModel.BytesPerPixel,
                                          0);


            //RenderTargetBitmap renderBitmap = new RenderTargetBitmap((int)CompositeImage.ActualWidth, (int)CompositeImage.ActualHeight, 96.0, 96.0, PixelFormats.Pbgra32);
            //DrawingVisual dv = new DrawingVisual();
            //VisualBrush brush = new VisualBrush(CompositeImage);

            //foreach(Body body in _bodies)
            //{
            //    if (body.IsTracked)
            //    {
            //        Joint joint = body.Joints[JointType.HandRight];
            //        using (DrawingContext dc = dv.RenderOpen())
            //        {

            //            dc.DrawRectangle(brush, null, new Rect(new Point(), new Size(CompositeImage.ActualWidth, CompositeImage.ActualHeight)));
            //            ImageBrush brush2 = new ImageBrush(_pointerBitmap);
            //            brush2.Opacity = 1.0;
            //            dc.DrawRectangle(brush2, null, new Rect(new Point(0, CompositeImage.ActualHeight - _Overlay.Height), new Size(_pointerBitmap.Width, _pointerBitmap.Height)));
            //        }
            //    }
            //}

            //ConvertIRDataToByte();






            ImagePreview.Source = _ColorBitmap;


            bodyIndexFrame.CopyFrameDataToArray(imageModel.BodyIndexFrameData);

            this.coordinateMapper.MapDepthFrameToColorSpace(DepthFrameData, imageModel.ColorPoints);

            if (_vm.LeaveTrails)
            {
                Array.Copy(this._PreviousFrameDisplayPixels, imageModel.DisplayPixels, this._PreviousFrameDisplayPixels.Length);
            }


            try
            {
                //Send the imageModel to the DataFlow transformer
                _ImageTransformer.Post(imageModel);
            }
            catch (Exception ex)
            {
#if DEBUG
                Console.WriteLine(ex);
#endif
            }


        }
        private void Reader_MultiSourceFrameArrived(object sender, MultiSourceFrameArrivedEventArgs e)
        {
            var reference = e.FrameReference;

            MultiSourceFrame multiSourceFrame = null;
            ColorFrame       colorFrame       = null;
            DepthFrame       depthFrame       = null;
            BodyFrame        bodyFrame        = null;
            BodyIndexFrame   bodyIndexFrame   = null;

            try
            {
                using (_frameCounter.Increment())
                {
                    multiSourceFrame = reference.AcquireFrame();
                    if (multiSourceFrame == null)
                    {
                        return;
                    }

                    colorFrame     = multiSourceFrame.ColorFrameReference.AcquireFrame();
                    depthFrame     = multiSourceFrame.DepthFrameReference.AcquireFrame();
                    bodyFrame      = multiSourceFrame.BodyFrameReference.AcquireFrame();
                    bodyIndexFrame = multiSourceFrame.BodyIndexFrameReference.AcquireFrame();
                    if (colorFrame == null | depthFrame == null | bodyFrame == null | bodyIndexFrame == null)
                    {
                        return;
                    }


                    var colorDesc   = colorFrame.FrameDescription;
                    int colorWidth  = colorDesc.Width;
                    int colorHeight = colorDesc.Height;
                    if (_colorFrameData == null)
                    {
                        int size = colorDesc.Width * colorDesc.Height;
                        _colorFrameData = new byte[size * bytesPerPixel];
                        _displayFrame   = new byte[size * bytesPerPixel];
                    }

                    var  depthDesc = depthFrame.FrameDescription;
                    uint depthSize = depthDesc.LengthInPixels;
                    _depthFrameData   = new ushort[depthSize];
                    _colorSpacePoints = new ColorSpacePoint[depthSize];

                    FrameDescription bodyIndexFrameDescription = bodyIndexFrame.FrameDescription;
                    int bodyIndexWidth  = bodyIndexFrameDescription.Width;
                    int bodyIndexHeight = bodyIndexFrameDescription.Height;
                    if ((bodyIndexWidth * bodyIndexHeight) == bodyIndexFrameData.Length)
                    {
                        bodyIndexFrame.CopyFrameDataToArray(bodyIndexFrameData);
                    }

                    Array.Clear(_displayFrame, 0, _displayFrame.Length);


                    colorFrame.CopyConvertedFrameDataToArray(_colorFrameData, ColorImageFormat.Bgra);
                    depthFrame.CopyFrameDataToArray(_depthFrameData);
                    kinectSensor.CoordinateMapper.MapDepthFrameToColorSpace(_depthFrameData, _colorSpacePoints);
                    kinectSensor.CoordinateMapper.MapDepthFrameToCameraSpace(_depthFrameData, _cameraPoints);

                    for (int depthIndex = 0; depthIndex < _depthFrameData.Length; ++depthIndex)
                    {
                        byte player = bodyIndexFrameData[depthIndex];
                        bool?c      = OnlyPlayersMenuItem.IsChecked;
                        bool val    = c != null ? (bool)c : false;
                        if (!val || player != 0xff)
                        {
                            ColorSpacePoint  point = _colorSpacePoints[depthIndex];
                            CameraSpacePoint p     = this._cameraPoints[depthIndex];

                            int colorX          = (int)Math.Floor(point.X + 0.5);
                            int colorY          = (int)Math.Floor(point.Y + 0.5);
                            int colorImageIndex = ((colorWidth * colorY) + colorX) * bytesPerPixel;

                            if ((colorX >= 0) && (colorX < colorWidth) && (colorY >= 0) && (colorY < colorHeight))
                            {
                                if (p.Z > 0)
                                {
                                    _displayFrame[colorImageIndex]     = _colorFrameData[colorImageIndex];     // b
                                    _displayFrame[colorImageIndex + 1] = _colorFrameData[colorImageIndex + 1]; // g
                                    _displayFrame[colorImageIndex + 2] = _colorFrameData[colorImageIndex + 2]; // r
                                    _displayFrame[colorImageIndex + 3] = _colorFrameData[colorImageIndex + 3]; // a
                                }
                            }
                        }
                    }

                    colorBitmap.WritePixels(
                        new Int32Rect(0, 0, colorDesc.Width, colorDesc.Height),
                        _displayFrame,
                        //_colorFrameData,
                        colorDesc.Width * bytesPerPixel,
                        0);

                    if (calibratingSurface)
                    {
                        if (_pointsToDepth.Count > 0)
                        {
                            foreach (Point p in _pointsToDepth)
                            {
                                int depthIndex = Convert.ToInt32(p.Y) * depthDesc.Width + Convert.ToInt32(p.X);
                                try
                                {
                                    CameraSpacePoint cameraPoint = _cameraPoints[depthIndex];
                                    if (!(Double.IsInfinity(cameraPoint.X)) && !(Double.IsInfinity(cameraPoint.Y)) && !(Double.IsInfinity(cameraPoint.Z) && cameraPoint.Z > 0))
                                    {
                                        Console.WriteLine("" + p.X + " " + p.Y + "  ---> " + cameraPoint.X + " " + cameraPoint.Y + " " + cameraPoint.Z);

                                        _calibrationPoints.Add(cameraPoint);
                                        drawEllipse(p.X, p.Y);
                                    }
                                }
                                catch { }
                            }
                            _pointsToDepth = new List <Point>();
                        }

                        if (false && _calibrationPoints.Count == 3)
                        {
                            canvas.Children.Clear();


                            CameraSpacePoint a  = VectorTools.subPoint(_calibrationPoints[0], _calibrationPoints[1]);
                            CameraSpacePoint b  = VectorTools.subPoint(_calibrationPoints[2], _calibrationPoints[1]);
                            CameraSpacePoint up = VectorTools.cross(a, b);
                            CameraSpacePoint c1 = VectorTools.cross(b, up);
                            CameraSpacePoint c2 = VectorTools.mult(c1, -1f);
                            CameraSpacePoint c;

                            if (VectorTools.distance(_calibrationPoints[2], VectorTools.addPoint(_calibrationPoints[1], c1)) < VectorTools.distance(_calibrationPoints[2], VectorTools.addPoint(_calibrationPoints[1], c2)))
                            {
                                c = VectorTools.mult(VectorTools.normalize(c1), 9.0f / 16.0f * VectorTools.norm(a) /*norm(b)*/);
                            }
                            else
                            {
                                c = VectorTools.mult(VectorTools.normalize(c2), 9.0f / 16.0f * VectorTools.norm(a) /*norm(b)*/);
                            }


                            CameraSpacePoint BL = _calibrationPoints[0];
                            CameraSpacePoint BR = _calibrationPoints[1];
                            CameraSpacePoint TR = VectorTools.addPoint(BR, c);
                            CameraSpacePoint TL = VectorTools.addPoint(BL, c);

                            VectorTools.DebugPoint(BL);
                            VectorTools.DebugPoint(BR);
                            VectorTools.DebugPoint(TR);
                            VectorTools.DebugPoint(TL);

                            //_drawSurface(coordinateMapper.MapCameraPointToColorSpace(BL),
                            //    coordinateMapper.MapCameraPointToColorSpace(BR),
                            //    coordinateMapper.MapCameraPointToColorSpace(TR),
                            //    coordinateMapper.MapCameraPointToColorSpace(TL));

                            _calibrationPoints.Clear();
                            calibratingSurface = false;
                        }
                    }
                }
            }
            catch (Exception ex)
            {
                Console.WriteLine(ex.Message);
                Console.WriteLine(ex.StackTrace);
                Console.WriteLine(ex.Source);
            }
            finally
            {
                if (colorFrame != null)
                {
                    colorFrame.Dispose();
                }
                if (depthFrame != null)
                {
                    depthFrame.Dispose();
                }
                if (bodyFrame != null)
                {
                    bodyFrame.Dispose();
                }
                if (bodyIndexFrame != null)
                {
                    bodyIndexFrame.Dispose();
                }
            }
        }
Ejemplo n.º 9
0
        /// <summary>
        /// Handles the body frame data arriving from the sensor
        /// </summary>
        /// <param name="sender">object sending the event</param>
        /// <param name="e">event arguments</param>
        private void Reader_FrameArrived(object sender, BodyFrameArrivedEventArgs e)
        {
            bool dataReceived = false;

            using (BodyFrame bodyFrame = e.FrameReference.AcquireFrame())
            {
                if (bodyFrame != null)
                {
                    if (this.bodies == null)
                    {
                        this.bodies = new Body[bodyFrame.BodyCount];
                    }

                    // The first time GetAndRefreshBodyData is called, Kinect will allocate each Body in the array.
                    // As long as those body objects are not disposed and not set to null in the array,
                    // those body objects will be re-used.
                    bodyFrame.GetAndRefreshBodyData(this.bodies);
                    dataReceived = true;
                }
            }
            if (kinect_start != 0)
            {
                foreach (Body body in bodies)
                {
                    if (body.IsTracked == true)
                    {
                        DateTime datenow     = DateTime.Now;
                        int      hour        = datenow.Hour;
                        int      minute      = datenow.Minute;
                        int      second      = datenow.Second;
                        int      millisecond = datenow.Millisecond;
                        int      timestamp   = hour * 3600 * 1000 + minute * 60 * 1000 + second * 1000 + millisecond;

                        // sw1.WriteLine(skelet.Joints[JointType.ShoulderRight].Position.X + " " + skelet.Joints[JointType.ShoulderRight].Position.Y + " " + skelet.Joints[JointType.ShoulderRight].Position.Z + " " + skelet.Joints[JointType.ShoulderRight].TrackingState + " " + timestamp);
                        // sw1.WriteLine(skelet.Joints[JointType.ElbowRight].Position.X + " " + skelet.Joints[JointType.ElbowRight].Position.Y + " " + skelet.Joints[JointType.ElbowRight].Position.Z + " " + skelet.Joints[JointType.ElbowRight].TrackingState + " " + timestamp);
                        sw1.WriteLine(body.Joints[JointType.WristRight].Position.X + " " + body.Joints[JointType.WristRight].Position.Y + " " + body.Joints[JointType.WristRight].Position.Z + " " + body.Joints[JointType.WristRight].TrackingState + " " + timestamp);
                        // sw1.WriteLine(skelet.Joints[JointType.ShoulderLeft].Position.X + " " + skelet.Joints[JointType.ShoulderLeft].Position.Y + " " + skelet.Joints[JointType.ShoulderLeft].Position.Z + " " + skelet.Joints[JointType.ShoulderLeft].TrackingState + " " + timestamp);
                        // sw1.WriteLine(skelet.Joints[JointType.ElbowLeft].Position.X + " " + skelet.Joints[JointType.ElbowLeft].Position.Y + " " + skelet.Joints[JointType.ElbowLeft].Position.Z + " " + skelet.Joints[JointType.ElbowLeft].TrackingState + " " + timestamp);
                        sw1.WriteLine(body.Joints[JointType.WristLeft].Position.X + " " + body.Joints[JointType.WristLeft].Position.Y + " " + body.Joints[JointType.WristLeft].Position.Z + " " + body.Joints[JointType.WristLeft].TrackingState + " " + timestamp);
                        // sw1.WriteLine(skelet.Joints[JointType.HipRight].Position.X + " " + skelet.Joints[JointType.HipRight].Position.Y + " " + skelet.Joints[JointType.HipRight].Position.Z + " " + skelet.Joints[JointType.HipRight].TrackingState + " " + timestamp);
                        // sw1.WriteLine(skelet.Joints[JointType.KneeRight].Position.X + " " + skelet.Joints[JointType.KneeRight].Position.Y + " " + skelet.Joints[JointType.KneeRight].Position.Z + " " + skelet.Joints[JointType.KneeRight].TrackingState + " " + timestamp);
                        //sw1.WriteLine(skelet.Joints[JointType.FootRight].Position.X + " " + skelet.Joints[JointType.FootRight].Position.Y + " " + skelet.Joints[JointType.FootRight].Position.Z + " " + skelet.Joints[JointType.FootRight].TrackingState + " " + timestamp);
                        //sw1.WriteLine(skelet.Joints[JointType.HipLeft].Position.X + " " + skelet.Joints[JointType.HipLeft].Position.Y + " " + skelet.Joints[JointType.HipLeft].Position.Z + " " + skelet.Joints[JointType.HipLeft].TrackingState + " " + timestamp);
                        // sw1.WriteLine(skelet.Joints[JointType.KneeLeft].Position.X + " " + skelet.Joints[JointType.KneeLeft].Position.Y + " " + skelet.Joints[JointType.KneeLeft].Position.Z + " " + skelet.Joints[JointType.KneeLeft].TrackingState + " " + timestamp);
                        //sw1.WriteLine(skelet.Joints[JointType.FootLeft].Position.X + " " + skelet.Joints[JointType.FootLeft].Position.Y + " " + skelet.Joints[JointType.FootLeft].Position.Z + " " + skelet.Joints[JointType.FootLeft].TrackingState + " " + timestamp);
                    }
                }
            }
            if (dataReceived)
            {
                using (DrawingContext dc = this.drawingGroup.Open())
                {
                    // Draw a transparent background to set the render size
                    dc.DrawRectangle(Brushes.Black, null, new Rect(0.0, 0.0, this.displayWidth, this.displayHeight));

                    int penIndex = 0;
                    foreach (Body body in this.bodies)
                    {
                        Pen drawPen = this.bodyColors[penIndex++];

                        if (body.IsTracked)
                        {
                            this.DrawClippedEdges(body, dc);

                            IReadOnlyDictionary <JointType, Joint> joints = body.Joints;

                            // convert the joint points to depth (display) space
                            Dictionary <JointType, Point> jointPoints = new Dictionary <JointType, Point>();

                            foreach (JointType jointType in joints.Keys)
                            {
                                // sometimes the depth(Z) of an inferred joint may show as negative
                                // clamp down to 0.1f to prevent coordinatemapper from returning (-Infinity, -Infinity)
                                CameraSpacePoint position = joints[jointType].Position;
                                if (position.Z < 0)
                                {
                                    position.Z = InferredZPositionClamp;
                                }

                                DepthSpacePoint depthSpacePoint = this.coordinateMapper.MapCameraPointToDepthSpace(position);
                                jointPoints[jointType] = new Point(depthSpacePoint.X, depthSpacePoint.Y);
                            }

                            this.DrawBody(joints, jointPoints, dc, drawPen);

                            this.DrawHand(body.HandLeftState, jointPoints[JointType.HandLeft], dc);
                            this.DrawHand(body.HandRightState, jointPoints[JointType.HandRight], dc);
                        }
                    }

                    // prevent drawing outside of our render area
                    this.drawingGroup.ClipGeometry = new RectangleGeometry(new Rect(0.0, 0.0, this.displayWidth, this.displayHeight));
                }
            }
        }
Ejemplo n.º 10
0
    public static PhysicsJoint CreateJoint(GameObject parentBody, GameObject childBody, BasicJointInfo.BasicJointType jointType)
    {
        var bodyPBounds = parentBody.GetComponent <MeshRenderer>().bounds;
        var bodyCBounds = childBody.GetComponent <MeshRenderer>().bounds;

        var pointConPWorld = bodyPBounds.ClosestPoint(bodyCBounds.center);
        var pointPonCWorld = bodyCBounds.ClosestPoint(bodyPBounds.center);

        var bodyPTransform = new RigidTransform(parentBody.transform.rotation, parentBody.transform.position); // was torso
        var bodyCTransform = new RigidTransform(childBody.transform.rotation, childBody.transform.position);   // was head

        PhysicsJoint jointData = default;

        switch (jointType)
        {
        case BasicJointInfo.BasicJointType.BallAndSocket:
        {
            var pivotP = math.transform(math.inverse(bodyPTransform), pointConPWorld);
            var pivotC = math.transform(math.inverse(bodyCTransform), pointConPWorld);
            jointData = PhysicsJoint.CreateBallAndSocket(pivotP, pivotC);
        }
        break;

        case BasicJointInfo.BasicJointType.Distance:
        {
            var pivotP = math.transform(math.inverse(bodyPTransform), pointConPWorld);
            var pivotC = math.transform(math.inverse(bodyCTransform), pointPonCWorld);
            var range  = new FloatRange(0, math.distance(pointConPWorld, pointPonCWorld));
            jointData = PhysicsJoint.CreateLimitedDistance(pivotP, pivotC, range);
        }
        break;

        case BasicJointInfo.BasicJointType.Hinge:
        {
            var commonPivotPointWorld = math.lerp(pointConPWorld, pointPonCWorld, 0.5f);

            // assume a vertical hinge joint
            var axisP = math.rotate(math.inverse(bodyPTransform.rot), math.up());
            var axisC = math.rotate(math.inverse(bodyCTransform.rot), math.up());

            float3 perpendicularAxisA, perpendicularAxisB;
            Math.CalculatePerpendicularNormalized(axisP, out perpendicularAxisA, out _);
            Math.CalculatePerpendicularNormalized(axisC, out perpendicularAxisB, out _);

            var pivotP      = math.transform(math.inverse(bodyPTransform), commonPivotPointWorld);
            var pivotC      = math.transform(math.inverse(bodyCTransform), commonPivotPointWorld);
            var jointFrameP = new BodyFrame {
                Axis = axisP, PerpendicularAxis = perpendicularAxisA, Position = pivotP
            };
            var jointFrameC = new BodyFrame {
                Axis = axisC, PerpendicularAxis = perpendicularAxisB, Position = pivotC
            };
            var range = new FloatRange(math.radians(-90), math.radians(90.0f));
            jointData = PhysicsJoint.CreateLimitedHinge(jointFrameP, jointFrameC, range);
        }
        break;

        default:
            break;
        }
        return(jointData);
    }
Ejemplo n.º 11
0
        private void Process(BodyFrame bodyFrame)
        {
            double currentTime = bodyFrame.RelativeTime.TotalMilliseconds;

            // Selects the first body that is tracked and use that for our calculations
            Body body = System.Linq.Enumerable.FirstOrDefault(this.bodies, bod => bod.IsTracked);

            if (body != null && body.IsTracked)
            {
                currentBody = body;

                drawer.Draw();

                // add all joint positions to the records
                jointRecords.AddRecordForEachJoint(currentTime);

                // Main sound mapping logic
                if (!timeSignature.isEstablished)
                {
                    // First, establish the time signature (with specific gestures)
                    timeSignature.CheckForBeats();
                }
                else
                {
                    // Then, do general sound-movement mappings
                    SoundCreator.Create();
                }

                // update last hand state
                lastHandLeftState = currentBody.HandLeftState;
                lastHandRightState = currentBody.HandRightState;
            }

        }
 public void OnNewFrame(BodyFrame frame)
 {
     throw new NotImplementedException();
 }
Ejemplo n.º 13
0
        void DoThingsWithBodyFrame(BodyFrame frame)
        {
            canvas.Children.Clear();

            bodies = new Body[frame.BodyFrameSource.BodyCount];

            frame.GetAndRefreshBodyData(bodies);

            foreach (Body body in bodies)
            {
                if (body != null)
                {
                    //Console.WriteLine("Body not null");
                    //canvas.DrawBody(body);

                    if (body.IsTracked)
                    {
                        Joint[] joints = {
                                             body.Joints[JointType.ThumbLeft],
                                             body.Joints[JointType.ThumbRight],
                                             body.Joints[JointType.HandLeft],
                                             body.Joints[JointType.HandRight]
                                         };
                        foreach (Joint joint in joints)
                        {
                            CameraSpacePoint jointPosition = joint.Position;

                            Point point = new Point();

                            if (joint.TrackingState == TrackingState.Tracked)
                            {
                                ColorSpacePoint colorPoint = sensor.CoordinateMapper.MapCameraPointToColorSpace(jointPosition);
                                point.X = float.IsInfinity(colorPoint.X) ? 0 : colorPoint.X;
                                point.Y = float.IsInfinity(colorPoint.Y) ? 0 : colorPoint.Y;
                            }

                            Ellipse ellipse = new Ellipse
                            {
                                Fill = Brushes.IndianRed,
                                Width = 15,
                                Height = 15
                            };

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

                            canvas.Children.Add(ellipse);
                        }
                    }
                    #region oldTest
                    //if (body.IsTracked)
                    //{

                    //    Joint handRight = body.Joints[JointType.HandRight];
                    //    CameraSpacePoint camHandRight = handRight.Position;
                    //    ColorSpacePoint hrPoint = sensor.CoordinateMapper.MapCameraPointToColorSpace(camHandRight);

                    //    Joint thumbRight = body.Joints[JointType.ThumbRight];
                    //    CameraSpacePoint camThumbRight = thumbRight.Position;
                    //    ColorSpacePoint trPoint = sensor.CoordinateMapper.MapCameraPointToColorSpace(camThumbRight);

                    //    Joint handLeft = body.Joints[JointType.HandLeft];
                    //    CameraSpacePoint camHandLeft = handLeft.Position;
                    //    ColorSpacePoint hlPoint = sensor.CoordinateMapper.MapCameraPointToColorSpace(camHandLeft);

                    //    Joint thumbLeft = body.Joints[JointType.ThumbLeft];
                    //    CameraSpacePoint camThumbLeft = thumbLeft.Position;
                    //    ColorSpacePoint tlPoint = sensor.CoordinateMapper.MapCameraPointToColorSpace(camThumbLeft);
                    //    if (handLeft.TrackingState != TrackingState.Tracked) return;
                    //    if (handRight.TrackingState != TrackingState.Tracked) return;
                    //    if (thumbLeft.TrackingState != TrackingState.Tracked) return;
                    //    if (thumbRight.TrackingState != TrackingState.Tracked) return;
                    //    canvas.DrawPoint(hrPoint, 0);
                    //    canvas.DrawPoint(trPoint, 1);
                    //    canvas.DrawPoint(hlPoint, 0);
                    //    canvas.DrawPoint(tlPoint, 1);

                    //}
                    #endregion
                }
            }
        }
    // Polls for new skeleton data
    public static bool PollSkeleton(ref BodyFrame bodyFrame, Int64 lastFrameTime)
    {
        bool newSkeleton = false;

        int hr = KinectWrapper.GetBodyFrameData(ref bodyFrame, true, true);
        if(hr == 0 && (bodyFrame.liRelativeTime > lastFrameTime))
        {
            newSkeleton = true;
        }

        return newSkeleton;
    }
 public static extern int GetBodyFrameData(ref BodyFrame pBodyFrame, bool bGetOrientations, bool bGetHandStates);
Ejemplo n.º 16
0
        /// <summary>
        /// Update to get a new frame.
        /// This code is similar to the code in the Kinect SDK samples.
        /// </summary>
        private static void Update()
        {
            if (!isConnected)
            {
                return;
            }

            dataAvailable.WaitOne();

            MultiSourceFrame multiSourceFrame = null;
            DepthFrame       depthFrame       = null;
            InfraredFrame    irFrame          = null;
            BodyFrame        bodyFrame        = null;

            lock (updateLock)
            {
                try
                {
                    if (frameReference != null)
                    {
                        multiSourceFrame = frameReference.AcquireFrame();

                        if (multiSourceFrame != null)
                        {
                            DepthFrameReference    depthFrameReference = multiSourceFrame.DepthFrameReference;
                            InfraredFrameReference irFrameReference    = multiSourceFrame.InfraredFrameReference;
                            BodyFrameReference     bodyFrameReference  = multiSourceFrame.BodyFrameReference;

                            depthFrame = depthFrameReference.AcquireFrame();
                            irFrame    = irFrameReference.AcquireFrame();

                            if ((depthFrame != null) && (irFrame != null))
                            {
                                FrameDescription depthFrameDescription = depthFrame.FrameDescription;
                                FrameDescription irFrameDescription    = irFrame.FrameDescription;

                                int depthWidth  = depthFrameDescription.Width;
                                int depthHeight = depthFrameDescription.Height;
                                int irWidth     = irFrameDescription.Width;
                                int irHeight    = irFrameDescription.Height;

                                // verify data and write the new registered frame data to the display bitmap
                                if (((depthWidth * depthHeight) == depthFrameData.Length) &&
                                    ((irWidth * irHeight) == irFrameData.Length))
                                {
                                    depthFrame.CopyFrameDataToArray(depthFrameData);
                                    irFrame.CopyFrameDataToArray(irFrameData);
                                }

                                if (bodyFrameReference != null)
                                {
                                    bodyFrame = bodyFrameReference.AcquireFrame();

                                    if (bodyFrame != null)
                                    {
                                        if (bodies == null || bodies.Length < bodyFrame.BodyCount)
                                        {
                                            bodies = new Body[bodyFrame.BodyCount];
                                        }
                                        using (bodyFrame)
                                        {
                                            bodyFrame.GetAndRefreshBodyData(bodies);
                                        }
                                    }
                                }
                            }
                        }
                    }
                }
                catch (Exception)
                {
                    // ignore if the frame is no longer available
                }
                finally
                {
                    if (depthFrame != null)
                    {
                        depthFrame.Dispose();
                        depthFrame = null;
                    }

                    if (irFrame != null)
                    {
                        irFrame.Dispose();
                        irFrame = null;
                    }
                    if (bodyFrame != null)
                    {
                        bodyFrame.Dispose();
                        bodyFrame = null;
                    }
                    if (multiSourceFrame != null)
                    {
                        multiSourceFrame = null;
                    }
                }
            }
        }
Ejemplo n.º 17
0
        /// <summary>
        /// Kinect数据帧事件处理函数
        /// </summary>
        /// <param name="sender"></param>
        /// <param name="e"></param>
        private void Reader_FrameArrived(object sender, BodyFrameArrivedEventArgs e)
        {
            if (IsOpen == true)
            {
                if (conn.IsConnect == true)                               //待socket连接完成再进行数据处理
                {
                    using (BodyFrame b = e.FrameReference.AcquireFrame()) //获取body数据帧,using会在括号结束后自动销毁申请的数据结构
                    {
                        try
                        {
                            body = new Body[b.BodyCount];   //新建body数组
                            b.GetAndRefreshBodyData(body);  //从body数据帧里获取body对象放入body数组
                        }
                        catch (Exception)
                        {
                            //Console.WriteLine(ie.Message);在此计数,累加到一定次数后初始化传感器
                            if (FrameNullCounter++ == 10)
                            {
                                FrameNullCounter = 0;
                                sensor.Close();
                                sensor.Open();
                            }
                            return;
                        }
                    }
                    foreach (var item in body)                                            //遍历每个body
                    {
                        if (item.IsTracked)                                               //body处于被追踪状态时处理
                        {
                            if (item.JointOrientations[JointType.Head].Orientation.Z < 2) //Z轴距离大于2不处理
                            {
                                int[] position = new int[Constants.POSITION_LENTH];       //新建PWM数组

                                if (TrackID == 9)                                         //第一次进入时trackID = 9,其他时候都不等于9
                                {
                                    TrackID = item.TrackingId;
                                }

                                if (TrackID == item.TrackingId)   //只有当TrackID和当前ID一致时进行处理
                                {
                                    #region 关节角度计算
                                    Stopwatch sw = new Stopwatch(); sw.Start();
                                    int       c  = 0;
                                    foreach (var j in item.Joints)             //遍历所有关节结构体
                                    {
                                        joints[c++] = ToAngle.filter(j.Value); //过滤
                                    }
                                    //Console.WriteLine("State:{0}    x:{1} y:{2}", item.LeanTrackingState, item.Lean.X, item.Lean.Y);
                                    position[(int)angle.servos.ShoulderRight] = ToAngle.ToPWMshoulder(joints[(int)JointType.ShoulderRight], joints[(int)JointType.ElbowRight], joints[(int)JointType.HandRight]);
                                    position[(int)angle.servos.ShoulderLeft]  = ToAngle.ToPWMshoulder(joints[(int)JointType.ShoulderLeft], joints[(int)JointType.ElbowLeft], joints[(int)JointType.HandLeft]);
                                    position[(int)angle.servos.ElbowRight]    = ToAngle.ToPWM_elbow(joints[(int)JointType.ShoulderRight], joints[(int)JointType.ElbowRight]);
                                    position[(int)angle.servos.ElbowLeft]     = ToAngle.ToPWM_elbow(joints[(int)JointType.ShoulderLeft], joints[(int)JointType.ElbowLeft]);
                                    position[(int)angle.servos.HandRight]     = ToAngle.ToPWM_hand(joints[(int)JointType.ShoulderRight], joints[(int)JointType.ElbowRight], joints[(int)JointType.HandRight]);
                                    position[(int)angle.servos.HandLeft]      = ToAngle.ToPWM_hand(joints[(int)JointType.ShoulderLeft], joints[(int)JointType.ElbowLeft], joints[(int)JointType.HandLeft]);
                                    position[(int)angle.servos.HipRight]      = ToAngle.ToPWM_hip(joints[(int)JointType.HipRight], joints[(int)JointType.KneeRight]);
                                    position[(int)angle.servos.HipLeft]       = ToAngle.ToPWM_hip(joints[(int)JointType.HipLeft], joints[(int)JointType.KneeLeft]);
                                    position[(int)angle.servos.ThighRight]    = ToAngle.ToPWM_thigh(joints[(int)JointType.HipRight], joints[(int)JointType.KneeRight]);
                                    position[(int)angle.servos.ThighLeft]     = ToAngle.ToPWM_thigh(joints[(int)JointType.HipLeft], joints[(int)JointType.KneeLeft]);
                                    //position[(int)angle.servos.KneeRight] =     ToAngle.ToPWM_knee(   joints[(int)JointType.HipRight],      joints[(int)JointType.KneeRight], joints[(int)JointType.AnkleRight]);
                                    //position[(int)angle.servos.KneeLeft] =      ToAngle.ToPWM_knee(   joints[(int)JointType.HipLeft],       joints[(int)JointType.KneeLeft],  joints[(int)JointType.AnkleLeft]);
                                    //Console.WriteLine("{0}: {1}",angle.servos.ThighLeft.ToString() ,position[(int)angle.servos.ThighLeft]);
                                    //position[(int)angle.servos.AnkleRight] =    ToAngle.ToPWM_ankle(  joints[(int)JointType.AnkleRight]);
                                    //position[(int)angle.servos.AnkleLeft] =     ToAngle.ToPWM_ankle(  joints[(int)JointType.AnkleLeft]);
                                    #endregion

                                    #region 舵机PMW异常为0,数据帧丢弃
                                    //Console.WriteLine("舵机PMW异常为0,数据帧丢弃");
                                    for (int i = 0; i < Constants.POSITION_LENTH; i++)
                                    {
                                        switch (position[i])
                                        {
                                        case 0:
                                            if (i != (int)angle.servos.FootLeft && i != (int)angle.servos.FootRight && i != (int)angle.servos.Head && i != Constants.POSITION_LENTH - 1 && i != (int)angle.servos.AnkleRight && i != (int)angle.servos.AnkleLeft && i != (int)angle.servos.KneeLeft && i != (int)angle.servos.KneeRight)
                                            {
                                                return;    //非踝部脚部和头部舵机  值为0,计算异常,数据帧丢弃
                                            }
                                            break;

                                        case Constants.INVALID_JOINT_VALUE:
                                            return;    //有舵机值异常,数据帧丢弃

                                        default:
                                            break;
                                        }
                                    }
                                    #endregion

                                    //Console.WriteLine("算术平均滤波");
                                    #region 算术平均滤波
                                    //开始执行PWM算术平均滤波
                                    if (speed++ < Constants.frame_count) //先判断,再自增
                                    {
                                        positions.Add(position);         //把当前舵机值添加至列表
                                        return;                          //返回
                                    }
                                    else//采集处理了超过frame_count帧数据
                                    {
                                        speed = 0;                             //复位计数器
                                        positions.Add(position);               //添加当前帧至列表
                                        position = filter_position(positions); //执行算术平均滤波
                                        positions.Clear();                     //清除列表
                                    }
                                    #endregion
                                    difference = true;
                                    int diff = 0;
                                    #region 若计算的PWM值变化小于10,返回不发送
                                    //Console.WriteLine("若计算的PWM值变化小于10,返回不发送");
                                    if (IsFirst)//如果是第一次进入
                                    {
                                        //Console.WriteLine("first in");
                                        IsFirst    = false;//设置第一次进入标志位false
                                        difference = false;
                                    }
                                    else
                                    {
                                        for (int i = 0; i < position.Length; i++)
                                        {
                                            switch (i)
                                            {
                                            case (int)angle.servos.HandLeft:    //只对手部及大腿舵机进行变化值过滤
                                            case (int)angle.servos.HandRight:
                                            case (int)angle.servos.ElbowLeft:
                                            case (int)angle.servos.ElbowRight:
                                            case (int)angle.servos.ShoulderLeft:
                                            case (int)angle.servos.ShoulderRight:
                                            case (int)angle.servos.ThighLeft:
                                            case (int)angle.servos.ThighRight:
                                                diff = Math.Abs(position_pre[i] - position[i]); //计算本帧舵机PWM值和上帧的差值
                                                if (diff > 5)                                   //若有舵机变化值大于5,则置位difference标志
                                                {
                                                    difference = false;
                                                }
                                                //Console.WriteLine("diff:" + diff);
                                                else if (diff > 100)    //变化值大于40说明计算异常,数据帧丢弃
                                                {
                                                    //ToAngle.PrintPosition(position_pre);
                                                    Console.WriteLine(position_pre[i] + "  " + position[i]);
                                                    Console.WriteLine("here is >" + ((angle.servos)i).ToString());
                                                    return;
                                                }
                                                break;

                                            default:    //非手部及大腿舵机不进行过滤
                                                break;
                                            }
                                        }
                                    }

                                    ToAngle.CopyArray(ref position_pre, ref position);
                                    if (difference == true)//无变化
                                    {
                                        //Console.WriteLine("here is <");
                                        return;//返回
                                    }
                                    #endregion

                                    #region 姿态检测
                                    //Console.WriteLine("姿态检测");
                                    IsOpen = false;
                                    ToAngle.PoseDetect(position, item.HandRightState);
                                    #endregion


                                    #region PWM数据帧发送和显示
                                    //Console.WriteLine("PWM数据帧发送和显示");
                                    if (ToAngle.IsSquat == true)             //如果是下蹲
                                    {
                                        ToAngle.InitBottomLeg(ref position); //则初始化至下蹲
                                    }
                                    else
                                    {
                                        ToAngle.InitLeg(ref position);//否则初始化至站立
                                    }
                                    string PWM = ToAngle.toPWM(position);
                                    if (PWM == null)
                                    {
                                        return;
                                    }
                                    try
                                    {
                                        if (TimeOut)
                                        {
                                            conn.Send("sb" + PWM + "\r\n");
                                        }

                                        //Console.WriteLine("PWM:" + PWM.Length);
                                    }
                                    catch (Exception ex)
                                    {
                                        MessageBox.Show(ex.ToString());
                                    }
                                    this.Dispatcher.Invoke(new Action(() => //在与 Dispatcher 关联的线程上同步执行指定的委托
                                    {
                                        /*  在 WPF 中,只有创建 DispatcherObject 的线程才能访问该对象。例如,一个从主 UI 线程派生的后台线程不能更新在该 UI 线程上创建的 Button 的内容。为了使该后台线程能够访问 Button 的 Content 属性,该后台线程必须将此工作委托给与该 UI 线程关联的 Dispatcher。它使用Invoke 或 BeginInvoke完成。Invoke是同步,BeginInvoke 是异步。该操作将按指定的 DispatcherPriority 添加到 Dispatcher 的事件队列中。
                                         * Invoke 是同步操作;因此,直到回调返回之后才会将控制权返回给调用对象。              */

                                        //String p;
                                        //TextBox_data.AppendText(PWM.Substring(0, 2));
                                        //p = System.Text.RegularExpressions.Regex.Replace(PWM.Substring(2, 51), @".{3}","$0 ");
                                        //TextBox_data.AppendText(p + "\n");
                                        TextBox_data.AppendText(PWM + "\n"); //textbox追加PWM
                                        scroller_dataShow.ScrollToEnd();     //滚动至尾部
                                    }));

                                    #endregion
                                    //Console.WriteLine(sw.Elapsed.TotalMilliseconds);
                                }
                            }
                        }
                    }
                    //IsOpen = true;
                }
            }
        }
        public void processBodyFrame(BodyFrame frame)
        {
            frame.GetAndRefreshBodyData(bodies);

            foreach (var body in Bodies)
            {
                if (!eManager.users.ContainsKey(body.TrackingId))
                {
                    eManager.users[body.TrackingId] = new MyHuman(body);
                    eManager.holdTime[body.TrackingId] = 0;
                }

                // Multithreading maybe
                drawer.currentCanvasName = "body";
                drawer.drawSkeleton(body);
            }
        }
Ejemplo n.º 19
0
        /// <summary>
        /// Handles the body frame data arriving from the sensor
        /// </summary>
        /// <param name="sender">object sending the event</param>
        /// <param name="e">event arguments</param>
        private void Reader_FrameArrived(object sender, BodyFrameArrivedEventArgs e)
        {
            bool dataReceived = false;

            using (BodyFrame bodyFrame = e.FrameReference.AcquireFrame())
            {
                if (bodyFrame != null)
                {
                    if (this.bodies == null)
                    {
                        this.bodies = new Body[bodyFrame.BodyCount];
                    }

                    // The first time GetAndRefreshBodyData is called, Kinect will allocate each Body in the array.
                    // As long as those body objects are not disposed and not set to null in the array,
                    // those body objects will be re-used.
                    bodyFrame.GetAndRefreshBodyData(this.bodies);
                    dataReceived = true;
                }
            }

            if (dataReceived)
            {
                using (DrawingContext dc = this.drawingGroup.Open())
                {
                    // Draw a transparent background to set the render size
                    dc.DrawRectangle(Brushes.Black, null, new Rect(0.0, 0.0, this.displayWidth, this.displayHeight));

                    int penIndex = 0;

                    int       bodyIndex = 0;
                    OscBundle oscBundle = new OscBundle(DateTime.Now);

                    foreach (Body body in this.bodies)
                    {
                        Pen drawPen = this.bodyColors[penIndex++];

                        if (body.IsTracked)
                        {
                            bodyIndex++;

                            this.DrawClippedEdges(body, dc);

                            IReadOnlyDictionary <JointType, Joint> joints = body.Joints;

                            // convert the joint points to depth (display) space
                            Dictionary <JointType, Point> jointPoints = new Dictionary <JointType, Point>();

                            foreach (JointType jointType in joints.Keys)
                            {
                                // sometimes the depth(Z) of an inferred joint may show as negative
                                // clamp down to 0.1f to prevent coordinatemapper from returning (-Infinity, -Infinity)
                                CameraSpacePoint position = joints[jointType].Position;
                                if (position.Z < 0)
                                {
                                    position.Z = InferredZPositionClamp;
                                }

                                // add all joints data to osc bundle
                                Object[] args = new Object[10];
                                args[0] = bodyIndex;
                                args[1] = (int)jointType;
                                args[2] = position.X;
                                args[3] = position.Y;
                                args[4] = position.Z;
                                args[5] = body.JointOrientations[jointType].Orientation.X;
                                args[6] = body.JointOrientations[jointType].Orientation.Y;
                                args[7] = body.JointOrientations[jointType].Orientation.Z;
                                args[8] = body.JointOrientations[jointType].Orientation.W;
                                args[9] = (int)joints[jointType].TrackingState;
                                // JointType: https://msdn.microsoft.com/en-us/library/microsoft.kinect.jointtype.aspx

                                foreach (OscConnection oscConnection in oscConnectionList)
                                {
                                    if (oscConnection.OscUdpWriter != null)
                                    {
                                        string oscAddress = "/" + kinectSensor.UniqueKinectId;
                                        if (oscConnection.Identifier != null)
                                        {
                                            if (oscConnection.Identifier != "")
                                            {
                                                oscAddress = "/" + oscConnection.Identifier;
                                            }
                                        }
                                        OscElement oscElement = new OscElement(oscAddress, args);
                                        oscConnection.OscUdpWriter.Send(oscElement);
                                        oscBundle.AddElement(oscElement);
                                    }
                                }

                                DepthSpacePoint depthSpacePoint = this.coordinateMapper.MapCameraPointToDepthSpace(position);
                                jointPoints[jointType] = new Point(depthSpacePoint.X, depthSpacePoint.Y);
                            }

                            this.DrawBody(joints, jointPoints, dc, drawPen);

                            this.DrawHand(body.HandLeftState, jointPoints[JointType.HandLeft], dc);
                            this.DrawHand(body.HandRightState, jointPoints[JointType.HandRight], dc);
                        }
                    }

                    // send osc bundle
                    // if (oscUdpWriter != null)
                    // oscUdpWriter.SendBundle(oscBundle);

                    // prevent drawing outside of our render area
                    this.drawingGroup.ClipGeometry = new RectangleGeometry(new Rect(0.0, 0.0, this.displayWidth, this.displayHeight));
                }
            }
        }
Ejemplo n.º 20
0
        private void ShowInfraredFrame(InfraredFrame infraredFrame, BodyFrame bodyFrame)
        {
            bool infraredFrameProcessed = false;

            if (infraredFrame != null)
            {
                FrameDescription infraredFrameDescription = infraredFrame.FrameDescription;

                // verify data and write the new infrared frame data to the display bitmap
                if (((infraredFrameDescription.Width * infraredFrameDescription.Height)
                    == this.infraredFrameData.Length) &&
                    (infraredFrameDescription.Width == this.bitmap.PixelWidth) &&
                    (infraredFrameDescription.Height == this.bitmap.PixelHeight))
                {

                    //Debug.WriteLine("Width is " + infraredFrameDescription.Width);
                    //Debug.WriteLine("Height is " + infraredFrameDescription.Height);

                    infraredWidth = infraredFrameDescription.Width;
                    infraredHeight = infraredFrameDescription.Height;

                    // Copy the pixel data from the image to a temporary array
                    infraredFrame.CopyFrameDataToArray(this.infraredFrameData);

                    infraredFrameProcessed = true;
                }
            }

         
            if (bodyFrame != null && bodyFrame.BodyCount > 0)
            {
                myBodies = new Body[this.kinectSensor.BodyFrameSource.BodyCount];
                bodyFrame.GetAndRefreshBodyData(myBodies);
            }

            // we got a frame, convert and render
            if (infraredFrameProcessed)
            {
                this.ConvertInfraredDataToPixels(myBodies);
                this.RenderPixelArray(this.infraredPixels);
            }
        }
Ejemplo n.º 21
0
        void msfr_MultiSourceFrameArrived(object sender, MultiSourceFrameArrivedEventArgs e)
        {
            try
            {
                MultiSourceFrame msf = e.FrameReference.AcquireFrame();

                if (msf != null)
                {
                    using (BodyFrame bodyFrame = msf.BodyFrameReference.AcquireFrame())
                        using (ColorFrame colorFrame = msf.ColorFrameReference.AcquireFrame())
                            using (DepthFrame irFrame = msf.DepthFrameReference.AcquireFrame())
                            {
                                if (bodyFrame != null && colorFrame != null && irFrame != null)
                                {
                                    bodies = new Body[bodyFrame.BodyFrameSource.BodyCount];
                                    FrameDescription framedesc = colorFrame.FrameDescription;
                                    using (KinectBuffer colorBuffer = colorFrame.LockRawImageBuffer())
                                    {
                                        colorBitmap.Lock();
                                        if ((framedesc.Width == colorBitmap.PixelWidth) && (framedesc.Height == colorBitmap.PixelHeight))
                                        {
                                            colorFrame.CopyConvertedFrameDataToIntPtr(colorBitmap.BackBuffer, (uint)(framedesc.Width * framedesc.Height * 4), ColorImageFormat.Bgra);
                                            colorBitmap.AddDirtyRect(new Int32Rect(0, 0, colorBitmap.PixelWidth, colorBitmap.PixelHeight));
                                        }
                                        colorBitmap.Unlock();

                                        bodyFrame.GetAndRefreshBodyData(bodies);
                                        canvas.Children.Clear();

                                        foreach (var body in bodies)
                                        {
                                            if (body != null)
                                            {
                                                if (body.IsTracked)
                                                {
                                                    //DrawJoint(body);

                                                    foreach (var item in body.Joints)
                                                    {
                                                        JointType jointType = item.Key;
                                                        Joint     joint     = item.Value;

                                                        if (flag)
                                                        {
                                                            if (joint.TrackingState == TrackingState.Tracked)
                                                            {
                                                                CameraSpacePoint camersp = joint.Position;
                                                                ColorSpacePoint  csp     = this.kinectRegion.KinectSensor.CoordinateMapper.MapCameraPointToColorSpace(camersp);

                                                                //this part draws a line between the waistline
                                                                if (joint.JointType == JointType.HipLeft)
                                                                {
                                                                    Point point1 = new Point(csp.X - 140, csp.Y);
                                                                    foreach (Joint joint2 in body.Joints.Values)
                                                                    {
                                                                        if (joint2.JointType == JointType.HipRight)
                                                                        {
                                                                            CameraSpacePoint camers = joint2.Position;
                                                                            ColorSpacePoint  cs     = this.kinectRegion.KinectSensor.CoordinateMapper.MapCameraPointToColorSpace(camers);
                                                                            Point            point2 = new Point(cs.X + 140, cs.Y);
                                                                            DrawLine(point1, point2);
                                                                            DrawTextWidth("Your waist is large about " + BodyWidth.Width(body) + " centimeters", cs, 410, 70);
                                                                            DrawPoint(point1, point2);
                                                                        }
                                                                    }
                                                                }

                                                                //this part draws a line between the head and the foot for the height
                                                                if (joint.JointType == JointType.Head)
                                                                {
                                                                    Point point3 = new Point(csp.X + 500, csp.Y);
                                                                    foreach (Joint joint3 in body.Joints.Values)
                                                                    {
                                                                        if (joint3.JointType == JointType.FootLeft)
                                                                        {
                                                                            if ((joint3.TrackingState != TrackingState.Inferred))
                                                                            {
                                                                                CameraSpacePoint camer  = joint3.Position;
                                                                                ColorSpacePoint  c      = this.kinectRegion.KinectSensor.CoordinateMapper.MapCameraPointToColorSpace(camer);
                                                                                Point            point4 = new Point(csp.X + 500, c.Y);
                                                                                if (IsPointForDrawing(point3))
                                                                                {
                                                                                    if (IsPointForDrawing(point4))
                                                                                    {
                                                                                        DrawLine(point3, point4);
                                                                                        DrawTextHeight("Your height is about " + BodyHeight.Height(body) + " meters", csp, 320, 70);
                                                                                        DrawPoint(point3, point4);
                                                                                    }
                                                                                }
                                                                            }
                                                                        }
                                                                    }
                                                                }
                                                            }
                                                        }
                                                        //this part puts some buttons on screen
                                                        if (joint.JointType == JointType.SpineMid)
                                                        {
                                                            SetTables();

                                                            DepthSpacePoint space2 = this.kinectRegion.KinectSensor.CoordinateMapper.MapCameraPointToDepthSpace(joint.Position);
                                                            ColorSpacePoint space  = this.kinectRegion.KinectSensor.CoordinateMapper.MapCameraPointToColorSpace(joint.Position);
                                                            Point           center = new Point(space.X - 75, space.Y);

                                                            foreach (Joint joints in body.Joints.Values)
                                                            {
                                                                if (joints.JointType == JointType.ShoulderLeft)
                                                                {
                                                                    DepthSpacePoint depth_point = this.kinectRegion.KinectSensor.CoordinateMapper.MapCameraPointToDepthSpace(joints.Position);
                                                                    ColorSpacePoint color_point = this.kinectRegion.KinectSensor.CoordinateMapper.MapCameraPointToColorSpace(joints.Position);

                                                                    float distance = (float)Math.Round((joints.Position.Z), 2);

                                                                    ShowMessageAboutDistance(distance, color_point);

                                                                    Point[] verticies = new Point[MAX];

                                                                    //DrawText("Your distance from the sensor is " + distance, color_point);

                                                                    //TODO probabilmente è da cambiare la variabile all'interno
                                                                    for (int i = 0; i < bodyFrame.BodyCount; i++)
                                                                    {
                                                                        if (distance < 1.5)
                                                                        {
                                                                            verticies = Polygon.CalculateVertices(buttons_array[i].Count, 300, 18, center);
                                                                            ReplaceButtons(100, 100, center, buttons_array[i], verticies);
                                                                        }
                                                                        if ((distance < 2.5) && (distance > 1.5))
                                                                        {
                                                                            verticies = Polygon.CalculateVertices(buttons_array[i].Count, 250, 18, center);
                                                                            ReplaceButtons(80, 85, center, buttons_array[i], verticies);
                                                                        }

                                                                        if ((distance < 3.5) && (distance > 2.5))
                                                                        {
                                                                            verticies = Polygon.CalculateVertices(buttons_array[i].Count, 200, 18, center);
                                                                            ReplaceButtons(60, 70, center, buttons_array[i], verticies);
                                                                        }
                                                                    }
                                                                }
                                                            }
                                                        }
                                                    }
                                                }
                                            }
                                        }
                                    }
                                }
                            }
                }
            }
            finally
            {
                //if (msfr != null) msfr.Dispose();
            }
        }
Ejemplo n.º 22
0
        private void Reader_MultiSourceFrameArrived(object sender, MultiSourceFrameArrivedEventArgs e)
        {
            if (!(KinectStreamerConfig.ProvideBodyData || KinectStreamerConfig.ProvideColorData || KinectStreamerConfig.ProvideDepthData))
            {
                return;
            }

            depthFrame = null;
            colorFrame = null;
            bodyFrame = null;

            multiSourceFrame = e.FrameReference.AcquireFrame();

            // If the Frame has expired by the time we process this event, return.
            if (multiSourceFrame == null)
            {
                return;
            }

            // We use a try/finally to ensure that we clean up before we exit the function.
            // This includes calling Dispose on any Frame objects that we may have and unlocking the bitmap back buffer.
            try
            {
                depthFrame = multiSourceFrame.DepthFrameReference.AcquireFrame();
                colorFrame = multiSourceFrame.ColorFrameReference.AcquireFrame();
                bodyFrame = multiSourceFrame.BodyFrameReference.AcquireFrame();

                // If any frame has expired by the time we process this event, return.
                // The "finally" statement will Dispose any that are not null.
                if ((depthFrame == null) || (colorFrame == null) || (bodyFrame == null))
                {
                    return;
                }

                // Process color stream if needed

                if (KinectStreamerConfig.ProvideColorData)
                {
                    ProcessColorData();
                }

                // Process depth frame if needed

                if (KinectStreamerConfig.ProvideDepthData)
                {
                    ProcessDepthData();
                }

                // Process body data if needed
                if (KinectStreamerConfig.ProvideBodyData)
                {
                    ProcessBodyData();
                }

            }
            finally
            {
                if (depthFrame != null)
                {
                    depthFrame.Dispose();
                }
                if (colorFrame != null)
                {
                    colorFrame.Dispose();
                }
                if (bodyFrame != null)
                {
                    bodyFrame.Dispose();
                }
            }
        }
Ejemplo n.º 23
0
        public void RecordFrame(BodyFrame frame)
        {
            if (!_isStarted)
                throw new InvalidOperationException("Cannot record frames unless the KinectRecorder is started.");

            if (frame != null)
            {
                var replayFrame = new RPBodyFrame(frame);
                if (MapDepthPositions)
                {
                    replayFrame.MapDepthPositions();
                }
                if (MapColorPositions)
                {
                    replayFrame.MapColorPositions();
                }
               // _recordQueue.Enqueue(replayFrame);
                System.Diagnostics.Debug.WriteLine("+++ Enqueued Body Frame ({0})", _recordQueue.Count);
            }
            else
            {
                System.Diagnostics.Debug.WriteLine("!!! FRAME SKIPPED (Body in KinectRecorder)");
            }
        }
Ejemplo n.º 24
0
    private void BodyFrameReader_FrameArrived(object sender, BodyFrameArrivedEventArgs e)
    {
        // effector start at (-81.7, -0.6, -19.3)
        // Shoulder->Elbow->Wrist->Hand->HandTip
        // Wrist->Thumb

        bool dataReceived = false;

        using (BodyFrame bodyFrame = e.FrameReference.AcquireFrame())
        {
            if (bodyFrame != null)
            {
                if (bodies == null)
                {
                    bodies = new Body[bodyFrame.BodyCount];
                }
                bodyFrame.GetAndRefreshBodyData(bodies);
                dataReceived = true;
            }
        }

        if (dataReceived)
        {
            var body = bodies.Where(b => b.IsTracked).FirstOrDefault();
            if (body != null)
            {
                var joints   = body.Joints;
                var shoulder = joints[JointType.ShoulderRight];
                var elbow    = joints[JointType.ElbowRight];
                var wrist    = joints[JointType.WristRight];
                var hand     = joints[JointType.HandRight];
                var handTip  = joints[JointType.HandTipRight];
                var thumb    = joints[JointType.ThumbRight];

                shoulderPosition = new Vector3(shoulder.Position.X, shoulder.Position.Y, shoulder.Position.Z);
                elbowPositon     = new Vector3(elbow.Position.X, elbow.Position.Y, elbow.Position.Z);
                handPosition     = new Vector3(hand.Position.X, hand.Position.Y, hand.Position.Z);

                var   pe     = ProjectPointOnLine(shoulderPosition, handPosition, elbowPositon);
                float dist   = Vector3.Distance(pe, elbowPositon);
                float length = Vector3.Distance(shoulderPosition, handPosition);
                armCalibrationRate = dist / length;

                if (!armCalibrated && dist / length < 0.01)
                {
                    armCalibrated = true;
                    armLength     = length;
                }

                if (armCalibrated)
                {
                    var diff = handPosition - shoulderPosition;
                    var tar  = diff / armLength * ur5length;
                    var t    = tar.z;
                    tar.z = tar.x;
                    tar.x = t;
                    target.transform.position = tar;
                }
            }
        }
    }
        private void RenderBodyFrame(BodyFrame bodyFrame)
        {
            bool dataReceived = false;

            Body[] bodies = null;
            if (bodyFrame != null)
            {
                if (bodies == null)
                {
                    bodies = new Body[bodyFrame.BodyCount];
                }

                // The first time GetAndRefreshBodyData is called, Kinect will allocate each Body in the array.
                // As long as those body objects are not disposed and not set to null in the array,
                // those body objects will be re-used.
                bodyFrame.GetAndRefreshBodyData(bodies);
                dataReceived = true;
            }

            if (dataReceived)
            {

                using (DrawingContext dc = this.drawingGroup.Open())
                {
                    // Draw a transparent background to set the render size
                    dc.DrawRectangle(Brushes.Black, null, new Rect(0.0, 0.0, Instance._displayWidth, Instance._displayHeight));

                    int penIndex = 0;
                    foreach (Body body in bodies)
                    {
                        Pen drawPen = this.bodyColors[penIndex++];

                        if (body.IsTracked)
                        {

                            IReadOnlyDictionary<JointType, Joint> joints = body.Joints;

                            // convert the joint points to depth (display) space
                            Dictionary<JointType, Point> jointPoints = new Dictionary<JointType, Point>();

                            foreach (JointType jointType in joints.Keys)
                            {
                                // sometimes the depth(Z) of an inferred joint may show as negative
                                // clamp down to 0.1f to prevent coordinatemapper from returning (-Infinity, -Infinity)
                                CameraSpacePoint position = joints[jointType].Position;
                                if (position.Z < 0)
                                {
                                    position.Z = InferredZPositionClamp;
                                }

                                DepthSpacePoint depthSpacePoint = this.coordinateMapper.MapCameraPointToDepthSpace(position);
                                jointPoints[jointType] = new Point(depthSpacePoint.X, depthSpacePoint.Y);
                            }

                            this.DrawBody(joints, jointPoints, dc, drawPen, body.JointOrientations);
                            if (Instance.DrawOrientationVectors == true)
                            {
                                this.DrawLocalCoordinates(body.JointOrientations, joints, jointPoints, dc);
                            }
                        }
                    }

                    // prevent drawing outside of our render area
                    this.drawingGroup.ClipGeometry = new RectangleGeometry(new Rect(0.0, 0.0, Instance._displayWidth, Instance._displayHeight));
                }

            }
        }
Ejemplo n.º 26
0
        /// <summary>
        /// 相当于主循环
        /// Handles the body frame data arriving from the sensor
        /// </summary>
        /// <param name="sender">object sending the event</param>
        /// <param name="e">event arguments</param>
        private void Reader_FrameArrived(object sender, BodyFrameArrivedEventArgs e)
        {
            bool dataReceived = false;

            using (BodyFrame bodyFrame = e.FrameReference.AcquireFrame())
            {
                if (bodyFrame != null)
                {
                    if (this.bodies == null)
                    {
                        this.bodies = new Body[bodyFrame.BodyCount];
                    }

                    // The first time GetAndRefreshBodyData is called, Kinect will allocate each Body in the array.
                    // As long as those body objects are not disposed and not set to null in the array,
                    // those body objects will be re-used.
                    bodyFrame.GetAndRefreshBodyData(this.bodies);
                    dataReceived = true;
                }
            }

            if (dataReceived)
            {
                using (DrawingContext dc = this.drawingGroup.Open())
                {
                    // 画一个透明背景设置渲染的大小(绘制矩形,黑色背景)
                    dc.DrawRectangle(Brushes.DarkGray, null, new Rect(0.0, 0.0, this.displayWidth, this.displayHeight));
                    //虚拟按钮绘制
                    //dc.DrawRectangle(null, new Pen(Brushes.Red, 4), new Rect(50, 120, 50, 100));//左
                    //dc.DrawRectangle(null, new Pen(Brushes.Red, 4), new Rect(240, 120, 50, 100));//右
                    //dc.DrawRectangle(null, new Pen(Brushes.Red, 4), new Rect(120, 50, 100, 50));//前
                    //dc.DrawRectangle(null, new Pen(Brushes.Red, 4), new Rect(120, 240, 100, 50));//后
                    //dc.DrawRectangle(null, new Pen(Brushes.Red, 4), new Rect(120, 120, 100, 40));//上
                    //dc.DrawRectangle(null, new Pen(Brushes.Red, 4), new Rect(120, 180, 100, 40));//下

                    int penIndex = 0;
                    foreach (Body body in this.bodies)
                    {
                        Pen drawPen = this.bodyColors[penIndex++];

                        if (body.IsTracked)
                        {
                            //this.DrawClippedEdges(body, dc);

                            IReadOnlyDictionary <JointType, Joint> joints = body.Joints;

                            // 关节点转换为深度(显示)空间:X Y坐标
                            Dictionary <JointType, Point> jointPoints = new Dictionary <JointType, Point>();

                            foreach (JointType jointType in joints.Keys)
                            {
                                // sometimes the depth(Z) of an inferred joint may show as negative
                                // clamp down to 0.1f to prevent coordinatemapper from returning (-Infinity, -Infinity)
                                CameraSpacePoint position = joints[jointType].Position;
                                if (position.Z < 0)
                                {
                                    position.Z = InferredZPositionClamp;
                                }

                                DepthSpacePoint depthSpacePoint = this.coordinateMapper.MapCameraPointToDepthSpace(position);
                                jointPoints[jointType] = new Point(depthSpacePoint.X, depthSpacePoint.Y);
                            }

                            //this.DrawBody(joints, jointPoints, dc, drawPen);

                            //this.DrawHand(body.HandLeftState, jointPoints[JointType.HandLeft], dc);
                            //this.DrawHand(body.HandRightState, jointPoints[JointType.HandRight], dc);

                            //////////user//////////////
                            //显示坐标
                            Lhand_PostionX.Content = (jointPoints[JointType.HandLeft].X).ToString("#0.00");
                            Lhand_PostionY.Content = (jointPoints[JointType.HandLeft].Y).ToString("#0.00");
                            Rhand_PostionX.Content = (jointPoints[JointType.HandRight].X).ToString("#0.00");
                            Rhand_PostionY.Content = (jointPoints[JointType.HandRight].Y).ToString("#0.00");

                            if (StartControl && joints[JointType.HandLeft].TrackingState == TrackingState.Tracked)
                            {
                                Canvas.SetTop(_controlHand, jointPoints[JointType.HandLeft].Y - 30);
                                Canvas.SetLeft(_controlHand, jointPoints[JointType.HandLeft].X - 30);

                                double deltaY = 25;

                                if (body.HandLeftState == HandState.Lasso && body.HandRightState == HandState.Lasso)
                                {
                                    ControlMode = false;
                                }
                                if (body.HandLeftState == HandState.Closed && body.HandRightState == HandState.Closed && Math.Abs(jointPoints[JointType.HandLeft].Y - jointPoints[JointType.HandRight].Y) < deltaY)
                                {
                                    ControlMode = true;
                                }

                                if (ControlMode)
                                {
                                    if (AppStateIsDrawing == true)
                                    {
                                        AppStateIsDrawing = false;
                                        AppStateIsButton  = true;
                                        BeginStoryboard((Storyboard)FindResource("Area_Show"));
                                    }
                                    this.PositonControl(joints, jointPoints);//进入按键控制
                                }
                                else
                                {
                                    if (AppStateIsButton == true)
                                    {
                                        AppStateIsButton  = false;
                                        AppStateIsDrawing = true;
                                        BeginStoryboard((Storyboard)FindResource("Area_Dispear"));//按钮消失
                                        drawPoints.Clear();
                                    }
                                    this.DrawControl(body, joints, jointPoints, dc);//进入画笔控制
                                }
                            }
                            else if (joints[JointType.HandLeft].TrackingState != TrackingState.Tracked)  //
                            {
                                _controlHand.Opacity = 0;
                                //mainTimer.Stop();
                            }

                            //huatu
                            if (!ControlMode)
                            {
                                Pen tempPen = new Pen(Brushes.Red, 2);
                                foreach (List <System.Drawing.Point> lp in drawPoints)
                                {
                                    for (int i = 0; i < lp.Count - 1; i++)
                                    {
                                        Point tpt  = new Point(lp[i].X, lp[i].Y);
                                        Point tpt2 = new Point(lp[i + 1].X, lp[i + 1].Y);
                                        dc.DrawLine(tempPen, tpt, tpt2);
                                    }
                                }
                            }
                        }
                    }

                    // 防止图像出界
                    this.drawingGroup.ClipGeometry = new RectangleGeometry(new Rect(0.0, 0.0, this.displayWidth, this.displayHeight));
                }
            }
        }
Ejemplo n.º 27
0
    private void RefreshBodyData(BodyFrame frame){
      cache.Clear();
      foreach (var body in (IList<Body>) frame.RawData) {
        if (!body.IsTracked) { continue; }

        NBody nbody = frame.Find(body.TrackingId);
        if (nbody == null) {
          nbody = new NBody(body.TrackingId, frame.Width, frame.Height);
        }

        cache.Add(nbody);
        RefreshBodyData(body, nbody);
      }

      lock (frame.Bodies) {
        frame.Bodies.Clear();
        frame.Bodies.AddRange(cache);
      }
    }
Ejemplo n.º 28
0
        /// <summary>
        /// Handles the body frame data arriving from the sensor
        /// </summary>
        /// <param name="sender">object sending the event</param>
        /// <param name="e">event arguments</param>
        ///
        private void Reader_FrameArrived(object sender, BodyFrameArrivedEventArgs e)
        {
            bool dataReceived = false;

            using (BodyFrame bodyFrame = e.FrameReference.AcquireFrame())
            {
                if (bodyFrame != null)
                {
                    if (this.bodies == null)
                    {
                        this.bodies = new Body[bodyFrame.BodyCount];
                    }

                    // The first time GetAndRefreshBodyData is called, Kinect will allocate each Body in the array.
                    // As long as those body objects are not disposed and not set to null in the array,
                    // those body objects will be re-used.
                    bodyFrame.GetAndRefreshBodyData(this.bodies);
                    dataReceived = true;
                }
            }

            if (dataReceived)
            {
                txt_kinect_info.Content = "Tracking: " + this.scanType.ToString() + "\n";

                if (this.drawingGroup != null && this.bodyColors != null)
                {
                    using (DrawingContext dc = this.drawingGroup.Open())
                    {
                        // Draw a transparent background to set the render size
                        dc.DrawRectangle(Brushes.Transparent, null, new Rect(0.0, 0.0, this.displayWidth, this.displayHeight));
                        int penIndex = 0;
                        foreach (Body body in this.bodies)
                        {
                            Pen drawPen = this.bodyColors[penIndex++];

                            if (body.IsTracked)//body.istrack
                            {
                                this.DrawClippedEdges(body, dc);
                                IReadOnlyDictionary <JointType, Joint> joints      = body.Joints;
                                Dictionary <JointType, Point>          jointPoints = new Dictionary <JointType, Point>();

                                CameraSpacePoint r_hip      = new CameraSpacePoint();
                                CameraSpacePoint r_shoulder = new CameraSpacePoint();
                                CameraSpacePoint r_elbow    = new CameraSpacePoint();
                                CameraSpacePoint r_wrist    = new CameraSpacePoint();
                                CameraSpacePoint l_hip      = new CameraSpacePoint();
                                CameraSpacePoint l_shoulder = new CameraSpacePoint();
                                CameraSpacePoint l_elbow    = new CameraSpacePoint();
                                CameraSpacePoint l_wrist    = new CameraSpacePoint();
                                CameraSpacePoint neck       = new CameraSpacePoint();
                                CameraSpacePoint up         = new CameraSpacePoint();
                                CameraSpacePoint spine_base = new CameraSpacePoint();
                                CameraSpacePoint spine_mid  = new CameraSpacePoint();
                                CameraSpacePoint head       = new CameraSpacePoint();

                                foreach (JointType jointType in joints.Keys)
                                {
                                    CameraSpacePoint position = joints[jointType].Position;
                                    float            x        = position.X;
                                    switch (jointType)
                                    {
                                    case JointType.ShoulderRight:
                                        r_shoulder.X = position.X;
                                        r_shoulder.Y = position.Y;
                                        break;

                                    case JointType.HipRight:
                                        r_hip.X = position.X;
                                        r_hip.Y = position.Y;
                                        //this is for trunk
                                        up.X = position.X;
                                        up.Y = position.Y + 0.1f;
                                        break;

                                    case JointType.ElbowRight:
                                        r_elbow.X = position.X;
                                        r_elbow.Y = position.Y;
                                        break;

                                    case JointType.WristRight:
                                        r_wrist.X = position.X;
                                        r_wrist.Y = position.Y;
                                        break;

                                    case JointType.ShoulderLeft:
                                        l_shoulder.X = position.X;
                                        l_shoulder.Y = position.Y;
                                        break;

                                    case JointType.HipLeft:
                                        l_hip.X = position.X;
                                        l_hip.Y = position.Y;
                                        //this is for trunk
                                        up.X = position.X;
                                        up.Y = position.Y + 0.1f;
                                        break;

                                    case JointType.ElbowLeft:
                                        l_elbow.X = position.X;
                                        l_elbow.Y = position.Y;
                                        break;

                                    case JointType.WristLeft:
                                        l_wrist.X = position.X;
                                        l_wrist.Y = position.Y;
                                        break;


                                    case JointType.Neck:
                                        neck.X = position.X;
                                        neck.Y = position.Y;
                                        break;

                                    case JointType.SpineBase:
                                        spine_base.X = position.X;
                                        spine_base.Y = position.Y;
                                        break;

                                    case JointType.Head:
                                        head.X = position.X;
                                        head.Y = position.Y;
                                        break;

                                    case JointType.SpineMid:
                                        spine_mid.X = position.X;
                                        spine_mid.Y = position.Y;
                                        break;
                                    }
                                    if (position.Z < 0)
                                    {
                                        position.Z = InferredZPositionClamp;
                                    }
                                    var depthSpacePoint = this.coordinateMapper.MapCameraPointToColorSpace(position);
                                    jointPoints[jointType] = new Point(depthSpacePoint.X, depthSpacePoint.Y);
                                    JointType parent = Helpers.GetOrigintJoint(jointType);

                                    if (this.typeOfEvaluation == "right" && parent != JointType.Head && r_hip != null && r_shoulder != null && r_elbow != null && r_wrist != null && neck != null)
                                    {
                                        #region Right Rula
                                        double r_hip_shoulder   = Helpers.Angle(r_shoulder, r_hip);
                                        double r_shoulder_elbow = Helpers.Angle(r_shoulder, r_elbow, 90);
                                        double r_elbow_wrist    = Helpers.Angle(r_elbow, r_wrist, 90);
                                        double upper_arm_angle  = Helpers.Inverse(r_shoulder_elbow - 180);
                                        double lower_arm_angle  = Helpers.Inverse((r_elbow_wrist - 180)) + upper_arm_angle;
                                        double up_spinebase     = Helpers.Angle(spine_mid, up);
                                        double neck_spine_base  = Helpers.Angle(neck, spine_mid);
                                        //double trunk_angle = (Angle(up, r_hip) + Angle(r_hip, neck)) - 270;
                                        double trunk_angle = neck_spine_base - 90;
                                        double neck_head   = Helpers.Angle(head, neck);
                                        double neck_angle  = (neck_head - trunk_angle) - 90 - 20;

                                        #region Scores
                                        score_upper_armk = Helpers.GetRulaUpperArmScore(upper_arm_angle);
                                        score_lower_armk = Helpers.GetRulaLowerArmScore(lower_arm_angle);
                                        score_trunk      = Helpers.GetRulaTrunkScore(trunk_angle);
                                        score_neck       = Helpers.GetRulaNeckScore(neck_angle);
                                        #endregion


                                        //txt_kinect_info.Content += "UPPER ARM:" + upper_arm_angle.ToString("00.0") + "\n";
                                        //txt_kinect_info.Content += "UPPER ARM SCORE:" + score_upper_armk.ToString("0") + "\n";

                                        //txt_kinect_info.Content += "LOWER ARM:" + lower_arm_angle.ToString("00.0") + "\n";
                                        //txt_kinect_info.Content += "LOWER ARM SCORE:" + score_lower_armk.ToString("0") + "\n";

                                        //txt_kinect_info.Content += "Trunk Angle:" + trunk_angle.ToString("00.0") + "\n";
                                        //txt_kinect_info.Content += "Trunk Score:" + score_trunk.ToString("00.0") + "\n";

                                        //txt_kinect_info.Content += "Neck Angle:" + neck_angle.ToString("00.0") + "\n";
                                        //txt_kinect_info.Content += "Neck Score:" + score_neck.ToString("00.0") + "\n";
                                        #endregion
                                    }
                                    else if (this.typeOfEvaluation == "left" && parent != JointType.Head && l_hip != null && l_shoulder != null && l_elbow != null && l_wrist != null && neck != null)
                                    {
                                        double l_hip_shoulder   = Helpers.Angle(l_shoulder, l_hip);
                                        double l_shoulder_elbow = Helpers.Angle(l_shoulder, l_elbow, 90);
                                        double l_elbow_wrist    = Helpers.Angle(l_elbow, l_wrist, 90);
                                        double upper_arm_angle  = Helpers.Inverse(l_shoulder_elbow - 180);
                                        double lower_arm_angle  = Helpers.Inverse((l_elbow_wrist - 180)) + upper_arm_angle;
                                        double up_spinebase     = Helpers.Angle(spine_mid, up);
                                        double neck_spine_base  = Helpers.Angle(neck, spine_mid);
                                        //double trunk_angle = (Angle(up, r_hip) + Angle(r_hip, neck)) - 270;
                                        double trunk_angle = neck_spine_base - 90;
                                        double neck_head   = Helpers.Angle(head, neck);
                                        double neck_angle  = (neck_head - trunk_angle) - 90 - 20;

                                        #region Scores
                                        score_upper_armk = Helpers.GetRulaUpperArmScore(upper_arm_angle);
                                        score_lower_armk = Helpers.GetRulaLowerArmScore(lower_arm_angle);
                                        score_trunk      = Helpers.GetRulaTrunkScore(trunk_angle);
                                        score_neck       = Helpers.GetRulaNeckScore(neck_angle);
                                        #endregion
                                        //txt_kinect_info.Content += "UPPER ARM:" + upper_arm_angle.ToString("00.0") + "\n";
                                        //txt_kinect_info.Content += "UPPER ARM SCORE:" + score_upper_armk.ToString("0") + "\n";

                                        //txt_kinect_info.Content += "LOWER ARM:" + lower_arm_angle.ToString("00.0") + "\n";
                                        //txt_kinect_info.Content += "LOWER ARM SCORE:" + score_lower_armk.ToString("0") + "\n";

                                        //txt_kinect_info.Content += "Trunk Angle:" + trunk_angle.ToString("00.0") + "\n";
                                        //txt_kinect_info.Content += "Trunk Score:" + score_trunk.ToString("00.0") + "\n";

                                        //txt_kinect_info.Content += "Neck Angle:" + neck_angle.ToString("00.0") + "\n";
                                        //txt_kinect_info.Content += "Neck Score:" + score_neck.ToString("00.0") + "\n";
                                    }
                                }
                                this.DrawBody(joints, jointPoints, dc, drawPen);
                                //this.DrawHand(body.HandLeftState, jointPoints[JointType.HandLeft], dc);
                                //this.DrawHand(body.HandRightState, jointPoints[JointType.HandRight], dc);
                            }
                        }
                        // prevent drawing outside of our render area
                        this.drawingGroup.ClipGeometry = new RectangleGeometry(new Rect(0.0, 0.0, this.displayWidth, this.displayHeight));
                    }
                }
            }
        }
        private void ProcessFrames(ColorFrame colorFrame, DepthFrame depthFrame, BodyIndexFrame bodyIndexFrame, BodyFrame bodyFrame, byte [] psBytes0, byte [] psBytes1)
        {            
            // create multiframe to process
            long ticksCopyData = DateTime.Now.Ticks;

            MultiFrame multiFrame = new MultiFrame();
            multiFrame.FrameNb = Interlocked.Increment(ref frameNb);

            // color
            long ticksCreateColorData = DateTime.Now.Ticks;
            byte[] colorData = new byte[colorByteSize];
            Utils.UpdateTimer("CreateColorData", ticksCreateColorData);

            long ticksCopyColorData = DateTime.Now.Ticks;
            colorFrame.CopyConvertedFrameDataToArray(colorData, ColorImageFormat.Bgra);
            Utils.UpdateTimer("CopyColorData", ticksCopyColorData);

            // depth
            long ticksCreateDepthData = DateTime.Now.Ticks;
            ushort[] depthData = new ushort[depthPixelSize];
            depthFrame.CopyFrameDataToArray(depthData);            
            Utils.UpdateTimer("CreateDepthData", ticksCreateDepthData);

            // body index
            long ticksCreateBodyIndexData = DateTime.Now.Ticks;
            byte[] bodyIndexData = new byte[depthPixelSize];
            bodyIndexFrame.CopyFrameDataToArray(bodyIndexData);
            Utils.UpdateTimer("CreateBodyIndexData", ticksCreateBodyIndexData);

            // bodies
            long ticksCreateBodiesData = DateTime.Now.Ticks;
            Body[] bodies = new Body[bodyFrame.BodyCount];
            bodyFrame.GetAndRefreshBodyData(bodies);
            Utils.UpdateTimer("CreateBodiesData", ticksCreateBodiesData);

            // ps3eye
            byte[] psBytes = null;
            if (psBytes0 != null && psBytes1 != null)
            {
                long ticksCreatePS3EyeData = DateTime.Now.Ticks;
                psBytes = new byte[psByteSize * 2];
                Utils.UpdateTimer("CreatePS3EyeData", ticksCreatePS3EyeData);

                long ticksCopyPS3EyeData = DateTime.Now.Ticks;
                CopyPS3EyeDataMirror(psBytes, psBytes0, psBytes1);
                Utils.UpdateTimer("CopyPS3EyeData", ticksCopyPS3EyeData);
            }

            // multiFrame
            long ticksMultiFrame = DateTime.Now.Ticks;
            multiFrame.DepthData = depthData;
            multiFrame.ColorData = colorData;
            multiFrame.BodyIndexData = bodyIndexData;
            multiFrame.Bodies = bodies;
            multiFrame.PS3EyeData = psBytes;
            multiFrame.HasKinectData = true;
            multiFrame.HasPS3EyeData = psBytes != null ? true : false;
            Utils.UpdateTimer("MultiFrame", ticksMultiFrame);

            long ticksEnqueue = DateTime.Now.Ticks;
            ProcessingManager.Instance.EnqueueMultiFrame(multiFrame);
            Utils.UpdateTimer("Enqueue", ticksEnqueue);

            Utils.UpdateTimer("CopyFramesData", ticksCopyData);

            // display timers & queues
            Context.GUI.DisplayPerformance();
        }
        private void ProcessBodyFrame(BodyFrame bodyFrame)
        {
            if (null != bodyFrame)
            {
                if (this.m_currentTrackedBody != null)
                {
                    this.m_currentTrackedBody = FindBodyWithTrackingId(bodyFrame, this.m_CurrentTrackingId);

                    if (this.m_currentTrackedBody != null)
                    {
                        return;
                    }
                }

                Body selectedBody = FindClosestBody(bodyFrame);

                if (selectedBody == null)
                {
                    return;
                }

                this.m_currentTrackedBody = selectedBody;
                this.m_CurrentTrackingId = selectedBody.TrackingId;

                //SetupFace
                InitializeFace();
            }
        }
Ejemplo n.º 31
0
        private void bfReader_FrameArrived(object sender, BodyFrameArrivedEventArgs e)
        {
            using (BodyFrame bdframe = e.FrameReference.AcquireFrame())
            {
                if (null != bdframe)
                {
                    Bodies = new Body[bdframe.BodyCount];
                    bdframe.GetAndRefreshBodyData(Bodies);

                    Body bodyselected = selectTracker(Bodies);
                    if (bodyselected != null)
                    {
                        bdframe.BodyFrameSource.OverrideHandTracking(bodyselected.TrackingId);
                        n++;
                        if (bodyselected.HandRightConfidence == TrackingConfidence.High && bodyselected.HandRightState == HandState.Closed)
                        {
                            startpos     = bodyselected.Joints[JointType.HandRight].Position;
                            priorpos     = startpos;
                            ControlState = 1;
                        }

                        if (ControlState == 1)
                        {
                            Currentpos = bodyselected.Joints[JointType.HandRight].Position;
                            float dirflag  = Currentpos.Y - startpos.Y;//正反转方向
                            float addOrsub = Currentpos.X - priorpos.X;

                            if (Math.Abs(addOrsub) > CtlWidth)
                            {
                                if (addOrsub > CtlWidth)
                                {
                                    if (dirflag > 0)
                                    {
                                        addone = true;
                                        subone = false;
                                    }
                                    else
                                    {
                                        addone = false;
                                        subone = false;
                                    }
                                }
                                else
                                {
                                    if (dirflag > 0)
                                    {
                                        addone = false;
                                        subone = true;
                                    }
                                    else
                                    {
                                        addone = true;
                                        subone = false;
                                    }
                                }
                            }
                        }
                    }
                }
            }
        }
Ejemplo n.º 32
0
        /// <summary>
        /// Handles the body frame data arriving from the sensor
        /// </summary>
        /// <param name="sender">object sending the event</param>
        /// <param name="e">event arguments</param>
        private void Reader_FrameArrived(object sender, BodyFrameArrivedEventArgs e)
        {
            bool dataReceived = false;

            using (BodyFrame bodyFrame = e.FrameReference.AcquireFrame())
            {
                if (bodyFrame != null)
                {
                    if (this.bodies == null)
                    {
                        this.bodies = new Body[bodyFrame.BodyCount];
                    }

                    // The first time GetAndRefreshBodyData is called, Kinect will allocate each Body in the array.
                    // As long as those body objects are not disposed and not set to null in the array,
                    // those body objects will be re-used.
                    bodyFrame.GetAndRefreshBodyData(this.bodies);
                    dataReceived = true;
                }
            }

            if (dataReceived)
            {
                if (!File.Exists("Data.csv"))
                {
                    using (StreamWriter writer = new StreamWriter("Data.csv", true, Encoding.UTF8))
                    {
                        writer.Write(string.Join(",", CsvData.Fields()) + "\n");
                    }
                }

                using (DrawingContext dc = this.drawingGroup.Open())
                {
                    var names = new List <string>();
                    this.timeText   = DateTime.Now.ToString("yyyy-MM-dd HH:mm:ss.fff");
                    this.StatusText = (this.kinectSensor.IsAvailable ? Properties.Resources.RunningStatusText
                                                                     : Properties.Resources.SensorNotAvailableStatusText) + " " + this.timeText;
                    names.Add(this.timeText);

                    // Draw a transparent background to set the render size
                    dc.DrawImage(this.colorBitmap, new Rect(0.0, 0.0, this.displayWidth, this.displayHeight));

                    int penIndex = 0;
                    foreach (Body body in this.bodies)
                    {
                        Pen drawPen = this.bodyColors[penIndex++];

                        if (body.IsTracked)
                        {
                            this.DrawClippedEdges(body, dc);

                            IReadOnlyDictionary <JointType, Joint> joints = body.Joints;

                            // convert the joint points to depth (display) space
                            Dictionary <JointType, Point> jointPoints = new Dictionary <JointType, Point>();

                            foreach (JointType jointType in joints.Keys)
                            {
                                // sometimes the depth(Z) of an inferred joint may show as negative
                                // clamp down to 0.1f to prevent coordinatemapper from returning (-Infinity, -Infinity)
                                CameraSpacePoint position = joints[jointType].Position;
                                names.Add(position.X + "," + position.Y + "," + position.Z);

                                if (position.Z < 0)
                                {
                                    position.Z = InferredZPositionClamp;
                                }

                                ColorSpacePoint depthSpacePoint = this.coordinateMapper.MapCameraPointToColorSpace(position);
                                jointPoints[jointType] = new Point(depthSpacePoint.X, depthSpacePoint.Y);
                                names.Add(depthSpacePoint.X + "," + depthSpacePoint.Y);
                            }

                            this.DrawBody(joints, jointPoints, dc, drawPen);

                            this.DrawHand(body.HandLeftState, jointPoints[JointType.HandLeft], dc);
                            this.DrawHand(body.HandRightState, jointPoints[JointType.HandRight], dc);
                        }
                    }

                    // prevent drawing outside of our render area
                    this.drawingGroup.ClipGeometry = new RectangleGeometry(new Rect(0.0, 0.0, this.displayWidth, this.displayHeight));

                    if (names.Count > 1)
                    {
                        using (StreamWriter writer = new StreamWriter("Data.csv", true, Encoding.UTF8))
                        {
                            writer.Write(string.Join(",", names) + "\n");
                        }
                    }
                }
            }
        }
Ejemplo n.º 33
0
        /// <summary>
        /// Handles the body frame data arriving from the sensor
        /// </summary>
        /// <param name="sender">object sending the event</param>
        /// <param name="e">event arguments</param>
        private void Reader_FrameArrived(object sender, BodyFrameArrivedEventArgs e)
        {
            bool dataReceived = false;

            using (BodyFrame bodyFrame = e.FrameReference.AcquireFrame())
            {
                if (bodyFrame != null)
                {
                    if (this.bodies == null)
                    {
                        this.bodies = new Body[bodyFrame.BodyCount];
                    }

                    // The first time GetAndRefreshBodyData is called, Kinect will allocate each Body in the array.
                    // As long as those body objects are not disposed and not set to null in the array,
                    // those body objects will be re-used.
                    bodyFrame.GetAndRefreshBodyData(this.bodies);
                    dataReceived = true;
                }
            }

            if (dataReceived)
            {
                using (DrawingContext dc = this.drawingGroup.Open())
                {
                    // Draw a transparent background to set the render size
                    dc.DrawRectangle(Brushes.Black, null, new Rect(0.0, 0.0, this.displayWidth, this.displayHeight));

                    int penIndex = 0;
                    foreach (Body body in this.bodies)
                    {
                        Pen drawPen = this.bodyColors[penIndex++];

                        if (body.IsTracked)
                        {
                            this.DrawClippedEdges(body, dc);

                            IReadOnlyDictionary <JointType, Joint> joints = body.Joints;

                            // convert the joint points to depth (display) space
                            Dictionary <JointType, Point> jointPoints = new Dictionary <JointType, Point>();

                            foreach (JointType jointType in joints.Keys)
                            {
                                // sometimes the depth(Z) of an inferred joint may show as negative
                                // clamp down to 0.1f to prevent coordinatemapper from returning (-Infinity, -Infinity)
                                CameraSpacePoint position = joints[jointType].Position;
                                if (position.Z < 0)
                                {
                                    position.Z = InferredZPositionClamp;
                                }

                                DepthSpacePoint depthSpacePoint = this.coordinateMapper.MapCameraPointToDepthSpace(position);
                                jointPoints[jointType] = new Point(depthSpacePoint.X, depthSpacePoint.Y);
                            }

                            using (BodyFrame bodyFrame = e.FrameReference.AcquireFrame())
                            {
                                // System.Console.WriteLine(bodyFrame.FloorClipPlane.W + " " + bodyFrame.FloorClipPlane.X + " " + bodyFrame.FloorClipPlane.Y + " " + bodyFrame.FloorClipPlane.Z);
                                CameraSpacePoint position = joints[JointType.SpineBase].Position;
                                //JUSTTHERE
                                // System.Console.WriteLine(position.X);
                                // System.Console.WriteLine(position.Z);
                                if (position.Z != 0)
                                {
                                    if (position.X / position.Z > 0.6)
                                    {
                                        if (position.Z >= 1.15)
                                        {
                                            flag = 1;
                                            System.Console.WriteLine("head and turn right" + position.X / position.Z);
                                        }
                                        if (position.Z >= 0.85 && position.Z < 1.15)
                                        {
                                            flag = 2;
                                            System.Console.WriteLine("stop" + position.X / position.Z);
                                        }
                                        if (position.Z < 0.85)
                                        {
                                            flag = 3;
                                            System.Console.WriteLine("back" + position.X / position.Z);
                                        }
                                    }

                                    if ((position.X / position.Z > 0.5) && (position.X / position.Z <= 0.6))
                                    {
                                        if (position.Z >= 1.15)
                                        {
                                            flag = 4;
                                            System.Console.WriteLine("head and turn right" + position.X / position.Z);
                                        }
                                        if (position.Z >= 0.85 && position.Z < 1.15)
                                        {
                                            flag = 2;
                                            System.Console.WriteLine("stop" + position.X / position.Z);
                                        }
                                        if (position.Z < 0.85)
                                        {
                                            flag = 3;
                                            System.Console.WriteLine("back" + position.X / position.Z);
                                        }
                                    }

                                    if ((position.X / position.Z > 0.4) && (position.X / position.Z <= 0.5))
                                    {
                                        if (position.Z >= 1.15)
                                        {
                                            flag = 5;
                                            System.Console.WriteLine("head and turn right" + position.X / position.Z);
                                        }
                                        if (position.Z >= 0.85 && position.Z < 1.15)
                                        {
                                            flag = 2;
                                            System.Console.WriteLine("stop" + position.X / position.Z);
                                        }
                                        if (position.Z < 0.85)
                                        {
                                            flag = 3;
                                            System.Console.WriteLine("back" + position.X / position.Z);
                                        }
                                    }

                                    if ((position.X / position.Z > 0.3) && (position.X / position.Z <= 0.4))
                                    {
                                        if (position.Z >= 1.15)
                                        {
                                            flag = 6;
                                            System.Console.WriteLine("ahead and turn right" + position.X / position.Z);
                                        }
                                        if (position.Z >= 0.85 && position.Z < 1.15)
                                        {
                                            flag = 2;
                                            System.Console.WriteLine("stop" + position.X / position.Z);
                                        }
                                        if (position.Z < 0.85)
                                        {
                                            flag = 3;
                                            System.Console.WriteLine("back" + position.X / position.Z);
                                        }
                                    }

                                    if ((position.X / position.Z > 0.2) && (position.X / position.Z <= 0.3))
                                    {
                                        if (position.Z >= 1.15)
                                        {
                                            flag = 7;
                                            System.Console.WriteLine("ahead and turn right" + position.X / position.Z);
                                        }
                                        if (position.Z >= 0.85 && position.Z < 1.15)
                                        {
                                            flag = 2;
                                            System.Console.WriteLine("stop" + position.X / position.Z);
                                        }
                                        if (position.Z < 0.85)
                                        {
                                            flag = 3;
                                            System.Console.WriteLine("back" + position.X / position.Z);
                                        }
                                    }

                                    if ((position.X / position.Z > -0.2) && (position.X / position.Z <= 0.2))
                                    {
                                        if (position.Z >= 1.15)
                                        {
                                            flag = 8;
                                            System.Console.WriteLine("ahead" + position.X / position.Z);
                                        }
                                        if (position.Z >= 0.85 && position.Z < 1.15)
                                        {
                                            flag = 2;
                                            System.Console.WriteLine("stop");
                                        }
                                        if (position.Z < 0.85)
                                        {
                                            flag = 3;
                                            System.Console.WriteLine("back");
                                        }
                                    }

                                    if ((position.X / position.Z > -0.3) && (position.X / position.Z <= -0.2))
                                    {
                                        if (position.Z >= 1.15)
                                        {
                                            flag = 9;
                                            System.Console.WriteLine("ahead and turn left" + position.X / position.Z);
                                        }
                                        if (position.Z >= 0.85 && position.Z < 1.15)
                                        {
                                            flag = 2;
                                            System.Console.WriteLine("stop");
                                        }
                                        if (position.Z < 0.85)
                                        {
                                            flag = 3;
                                            System.Console.WriteLine("back");
                                        }
                                    }

                                    if ((position.X / position.Z >= -0.4) && (position.X / position.Z <= -0.3))
                                    {
                                        if (position.Z >= 1.15)
                                        {
                                            flag = 10;
                                            System.Console.WriteLine("ahead and turn left" + position.X / position.Z);
                                        }
                                        if (position.Z >= 0.85 && position.Z < 1.15)
                                        {
                                            flag = 2;
                                            System.Console.WriteLine("stop");
                                        }
                                        if (position.Z < 0.85)
                                        {
                                            flag = 3;
                                            System.Console.WriteLine("back");
                                        }
                                    }

                                    if ((position.X / position.Z >= -0.5) && (position.X / position.Z <= -0.4))
                                    {
                                        if (position.Z >= 1.15)
                                        {
                                            flag = 11;
                                            System.Console.WriteLine("ahead and turn left" + position.X / position.Z);
                                        }
                                        if (position.Z >= 0.85 && position.Z < 1.15)
                                        {
                                            flag = 2;
                                            System.Console.WriteLine("stop");
                                        }
                                        if (position.Z < 0.85)
                                        {
                                            flag = 3;
                                            System.Console.WriteLine("back");
                                        }
                                    }

                                    if ((position.X / position.Z >= -0.6) && (position.X / position.Z <= -0.5))
                                    {
                                        if (position.Z >= 1.15)
                                        {
                                            flag = 12;
                                            System.Console.WriteLine("ahead" + position.X / position.Z);
                                        }
                                        if (position.Z >= 0.85 && position.Z < 1.15)
                                        {
                                            flag = 2;
                                            System.Console.WriteLine("stop");
                                        }
                                        if (position.Z < 0.85)
                                        {
                                            flag = 3;
                                            System.Console.WriteLine("back");
                                        }
                                    }

                                    if (position.X / position.Z < -0.6)
                                    {
                                        if (position.Z >= 1.15)
                                        {
                                            flag = 13;
                                            System.Console.WriteLine("ahead and turn left" + position.X / position.Z);
                                        }
                                        if (position.Z >= 0.85 && position.Z < 1.15)
                                        {
                                            flag = 2;
                                            System.Console.WriteLine("stop");
                                        }
                                        if (position.Z < 0.85)
                                        {
                                            flag = 3;
                                            System.Console.WriteLine("back");
                                        }
                                    }
                                }
                            }
                            this.DrawBody(joints, jointPoints, dc, drawPen);

                            this.DrawHand(body.HandLeftState, jointPoints[JointType.HandLeft], dc);
                            this.DrawHand(body.HandRightState, jointPoints[JointType.HandRight], dc);
                        }
                    }

                    // prevent drawing outside of our render area
                    this.drawingGroup.ClipGeometry = new RectangleGeometry(new Rect(0.0, 0.0, this.displayWidth, this.displayHeight));
                }
            }
        }
Ejemplo n.º 34
0
        // BodyFrameReader event handler
        void bodyFrameReader_FrameArrived(object sender, BodyFrameArrivedEventArgs e)
        {
            using (BodyFrame bodyFrame = e.FrameReference.AcquireFrame())
            {
                if (bodyFrame == null)
                {
                    return;
                }
                if (bodies == null)
                {
                    bodies = new Body[bodyFrame.BodyCount];
                }

                // Updates body data
                bodyFrame.GetAndRefreshBodyData(bodies);

                foreach (var body in bodies)
                {
                    if (body.IsTracked)
                    {
                        var joints = body.Joints;
                        // Sets up tracking for left and right hands
                        var handRight = joints[JointType.HandRight];
                        var handLeft  = joints[JointType.HandLeft];

                        if (handRight.TrackingState == TrackingState.Tracked && handLeft.TrackingState == TrackingState.Tracked)
                        {
                            // Updates the onscreen labels with current data
                            handLx.Content = handLeft.Position.X;
                            handLy.Content = handLeft.Position.Y;
                            handLz.Content = handLeft.Position.Z;
                            handRx.Content = handRight.Position.X;
                            handRy.Content = handRight.Position.Y;
                            handRz.Content = handRight.Position.Z;

                            //pitch increases as right hand moves up, volume increases as left hand moves up
                            if (handRight.Position.Y <= 1.5)
                            {
                                currentPitch = handLeft.Position.Y / (1 / 6) * -1 + pitchReference;
                            }
                            else if (handRight.Position.Y > 1.5)
                            {
                                currentPitch = handLeft.Position.Y / (1 / 6) + pitchReference;
                            }

                            currentPitch  = pitchReference + handRight.Position.Y;
                            osc.Frequency = freqReference * Math.Pow(freqRatio, currentPitch - pitchReference);
                            osc.Amplitude = handLeft.Position.Y / .05 * 2716;

                            //frequency output on screen in label
                            freqLabel.Content = osc.Frequency;

                            //plays note only if kinect has sensed a body
                            if (waveOut != null)
                            {
                                waveOut.Play();
                            }

                            using (var canvas = bodyHighlight.Open())
                            {
                                canvas.DrawRectangle(Brushes.Transparent, null, new Rect(0, 0, Width, Height));

                                // Place a circle around the user's left hand
                                canvas.DrawEllipse(Brushes.Red, null, new Point(handLeft.Position.X, handLeft.Position.Y), 20, 20);

                                // Place a circle around the user's right hand
                                canvas.DrawEllipse(Brushes.Blue, null, new Point(handRight.Position.X, handRight.Position.Y), 20, 20);

                                // Pushes bodyHighlight to the window
                                BodyOverlay.Source = new DrawingImage(bodyHighlight);
                            }
                        }
                    }
                }
            }
        }
Ejemplo n.º 35
0
        /// <summary>
        /// Kinect が複数種類のフレームを取得したとき実行されるメソッド(イベントハンドラ)。
        /// </summary>
        /// <param name="sender">
        /// イベントを通知したオブジェクト。ここでは Kinect になる。
        /// </param>
        /// <param name="e">
        /// イベントの発生時に渡されるデータ。
        /// </param>
        void MultiSourceFrameReader_MultiSourceFrameArrived
            (object sender, MultiSourceFrameArrivedEventArgs e)
        {
            MultiSourceFrame frames = this.multiSourceFrameReader.AcquireLatestFrame();

            if (frames == null)
            {
                return;
            }

            ColorFrame colorFrame = frames.ColorFrameReference.AcquireFrame();

            if (colorFrame == null)
            {
                return;
            }

            BodyFrame bodyFrame = frames.BodyFrameReference.AcquireFrame();

            if (bodyFrame == null)
            {
                colorFrame.Dispose();
                return;
            }

            //キャンバスをクリアして更新する。
            this.canvas.Background
                = new ImageBrush(GetBitmapSource(colorFrame, colorFrameDescription));

            this.canvas.Children.Clear();


            Body[] bodies = new Body[bodyFrame.BodyCount];

            bodyFrame.GetAndRefreshBodyData(bodies);

            foreach (Body body in bodies)
            {
                if (body.IsTracked == false)
                {
                    continue;
                }


                //描く関節を可視化する。
                IReadOnlyDictionary <JointType, Joint> joints = body.Joints;

                foreach (KeyValuePair <JointType, Joint> joint in joints)
                //foreach (var joint in joints) // こちらでも可。
                {
                    if (joint.Value.TrackingState == TrackingState.Tracked)
                    {
                        DrawJointEllipseInColorSpace(joint.Value, 10, Colors.Aqua);
                    }
                    else if (joint.Value.TrackingState == TrackingState.Inferred)
                    {
                        DrawJointEllipseInColorSpace(joint.Value, 10, Colors.Yellow);
                    }
                }

                //左手の状態を可視化する。
                switch (body.HandLeftState)
                {
                //閉じてる(グー)。
                case HandState.Closed:
                {
                    DrawJointEllipseInColorSpace(joints[JointType.HandLeft], 20, Colors.Blue);
                    break;
                }

                //チョキ(実際には精度の都合上、指一本でも反応するが)
                case HandState.Lasso:
                {
                    DrawJointEllipseInColorSpace(joints[JointType.HandLeft], 20, Colors.Green);
                    break;
                }

                //開いている(パー)。
                case HandState.Open:
                {
                    DrawJointEllipseInColorSpace(joints[JointType.HandLeft], 20, Colors.Red);
                    break;
                }
                }
            }

            colorFrame.Dispose();
            bodyFrame.Dispose();
        }
Ejemplo n.º 36
0
 public void Dispose()
 {
     ColorFrame?.Dispose();
     BodyFrame?.Dispose();
     DepthFrame?.Dispose();
 }
Ejemplo n.º 37
0
        /// <summary>
        /// Handles the body frame data arriving from the sensor
        /// </summary>
        /// <param name="sender">object sending the event</param>
        /// <param name="e">event arguments</param>
        private void Reader_FrameArrived(object sender, BodyFrameArrivedEventArgs e)
        {
            bool dataReceived = false;

            using (BodyFrame bodyFrame = e.FrameReference.AcquireFrame())
            {
                if (bodyFrame != null)
                {
                    if (this.bodies == null)
                    {
                        this.bodies = new Body[bodyFrame.BodyCount];
                    }

                    // The first time GetAndRefreshBodyData is called, Kinect will allocate each Body in the array.
                    // As long as those body objects are not disposed and not set to null in the array,
                    // those body objects will be re-used.
                    bodyFrame.GetAndRefreshBodyData(this.bodies);
                    dataReceived = true;
                }
            }

            if (dataReceived)
            {
                using (DrawingContext dc = this.drawingGroup.Open())
                {
                    // Draw a transparent background to set the render size
                    dc.DrawRectangle(Brushes.Black, null, new Rect(0.0, 0.0, this.displayWidth, this.displayHeight));

                    int penIndex = 0;
                    foreach (Body body in this.bodies)
                    {
                        Pen drawPen = this.bodyColors[penIndex++];

                        if (body.IsTracked)
                        {
                            this.DrawClippedEdges(body, dc);

                            IReadOnlyDictionary <JointType, Joint> joints = body.Joints;

                            // convert the joint points to depth (display) space
                            Dictionary <JointType, Point> jointPoints = new Dictionary <JointType, Point>();

                            foreach (JointType jointType in joints.Keys)
                            {
                                // sometimes the depth(Z) of an inferred joint may show as negative
                                // clamp down to 0.1f to prevent coordinatemapper from returning (-Infinity, -Infinity)
                                CameraSpacePoint position = joints[jointType].Position;
                                if (position.Z < 0)
                                {
                                    position.Z = InferredZPositionClamp;
                                }

                                DepthSpacePoint depthSpacePoint = this.coordinateMapper.MapCameraPointToDepthSpace(position);
                                jointPoints[jointType] = new Point(depthSpacePoint.X, depthSpacePoint.Y);
                            }

                            this.DrawBody(joints, jointPoints, dc, drawPen);

                            this.DrawHand(body.HandLeftState, jointPoints[JointType.HandLeft], dc);
                            this.DrawHand(body.HandRightState, jointPoints[JointType.HandRight], dc);
                            soundState.addPoint(JointType.HandLeft, jointPoints[JointType.HandLeft]);
                            soundState.addPoint(JointType.HandRight, jointPoints[JointType.HandRight]);
                            //System.Console.WriteLine("left hand x:{0} right hand x: {1}, diff: {2}",
                            //    jointPoints[JointType.HandLeft].X,
                            //    jointPoints[JointType.HandRight].X,
                            //    jointPoints[JointType.HandRight].X - jointPoints[JointType.HandLeft].X);



                            // play sound dongdong
                            if (jointPoints[JointType.HandRight].X - jointPoints[JointType.HandLeft].X <SOUND_TRIGGER_DISTANCE
                                                                                                        &&
                                                                                                        jointPoints[JointType.HandRight].Y> jointPoints[JointType.Head].Y
                                &&
                                jointPoints[JointType.HandLeft].Y > jointPoints[JointType.Head].Y
                                &&
                                soundState.is_applauding()
                                )
                            {
                                Play_Sound(soundPlayer_1);
                                //if (Play_Sound_DongDongCi() == true)
                                //{
                                //    System.Console.WriteLine("Play Sound true");
                                //}
                            }
                            if (jointPoints[JointType.HandRight].X - jointPoints[JointType.HandLeft].X < SOUND_TRIGGER_DISTANCE
                                &&
                                jointPoints[JointType.HandRight].Y < jointPoints[JointType.Head].Y
                                &&
                                jointPoints[JointType.HandLeft].Y < jointPoints[JointType.Head].Y
                                &&
                                soundState.is_applauding()
                                )
                            {
                                Play_Sound(soundPlayer_2);
                            }
                        }
                    }

                    // prevent drawing outside of our render area
                    this.drawingGroup.ClipGeometry = new RectangleGeometry(new Rect(0.0, 0.0, this.displayWidth, this.displayHeight));
                }
            }
        }
Ejemplo n.º 38
0
        /// <summary>
        /// Handles the body frame data arriving from the sensor
        /// </summary>
        /// <param name="sender">object sending the event</param>
        /// <param name="e">event arguments</param>
        private void Reader_FrameArrived(object sender, BodyFrameArrivedEventArgs e)
        {
            bool dataReceived = false;

            using (BodyFrame bodyFrame = e.FrameReference.AcquireFrame())
            {
                if (bodyFrame != null)
                {
                    if (this.bodies == null)
                    {
                        this.bodies = new Body[bodyFrame.BodyCount];
                    }

                    // The first time GetAndRefreshBodyData is called, Kinect will allocate each Body in the array.
                    // As long as those body objects are not disposed and not set to null in the array,
                    // those body objects will be re-used.
                    bodyFrame.GetAndRefreshBodyData(this.bodies);
                    dataReceived = true;
                }
            }
//======================================================================================================================================
            if (dataReceived)
            {
                using (DrawingContext dc = this.drawingGroup.Open())
                {
                    // Draw a transparent background to set the render size
                    dc.DrawRectangle(Brushes.Black, null, new Rect(0.0, 0.0, this.displayWidth, this.displayHeight));

                    int penIndex = 0;
                    foreach (Body body in this.bodies)
                    {
                        Pen drawPen = this.bodyColors[penIndex++];

                        if (body.IsTracked)
                        {
                            this.DrawClippedEdges(body, dc);

                            IReadOnlyDictionary <JointType, Joint> joints = body.Joints;

                            // convert the joint points to depth (display) space
                            Dictionary <JointType, Point> jointPoints = new Dictionary <JointType, Point>();

                            foreach (JointType jointType in joints.Keys)
                            {
                                CameraSpacePoint position = joints[jointType].Position;

                                CameraSpacePoint head       = joints[JointType.Head].Position;
                                CameraSpacePoint HandLeft   = joints[JointType.HandLeft].Position;
                                CameraSpacePoint HandRight  = joints[JointType.HandRight].Position;
                                CameraSpacePoint SpineBase  = joints[JointType.SpineBase].Position;
                                CameraSpacePoint SpineMid   = joints[JointType.SpineMid].Position;
                                CameraSpacePoint Shoulder   = joints[JointType.SpineShoulder].Position;
                                CameraSpacePoint ElbowLeft  = joints[JointType.ElbowLeft].Position;
                                CameraSpacePoint ElbowRight = joints[JointType.ElbowRight].Position;
                                CameraSpacePoint KneeLeft   = joints[JointType.KneeLeft].Position;
                                CameraSpacePoint KneeRight  = joints[JointType.KneeRight].Position;
                                CameraSpacePoint FootLeft   = joints[JointType.FootLeft].Position;

                                // sometimes the depth(Z) of an inferred joint may show as negative
                                // clamp down to 0.1f to prevent coordinatemapper from returning (-Infinity, -Infinity)
                                if (position.Z < 0)
                                {
                                    position.Z = InferredZPositionClamp;
                                }

                                DepthSpacePoint depthSpacePoint = this.coordinateMapper.MapCameraPointToDepthSpace(position);
                                jointPoints[jointType] = new Point(depthSpacePoint.X, depthSpacePoint.Y);


                                p_head       = Math.Round(head.Y, 2);
                                p_lhand      = Math.Round(HandLeft.Y, 2);
                                p_rhand      = Math.Round(HandLeft.Y, 2);
                                p_base_spine = Math.Round(SpineBase.Y, 2);
                                p_mid_base   = Math.Round(SpineMid.Y, 2);
                                p_shoulder   = Math.Round(Shoulder.Y);
                                p_lelbow     = Math.Round(ElbowLeft.Y, 2);
                                p_relbow     = Math.Round(ElbowRight.Y, 2);
                                p_lknee      = Math.Round(KneeLeft.Y, 2);
                                p_rknee      = Math.Round(KneeRight.Y, 2);
                                p_lfoot      = Math.Round(FootLeft.Y, 2);

                                x_head  = Math.Round(head.X, 2);
                                x_lknee = Math.Round(head.X, 2);
                                x_lfoot = Math.Round(KneeLeft.X, 2);
                                x_rfoot = Math.Round(FootLeft.X, 2);

                                start_lhand = p_lhand;
                                start_rhand = p_rhand;
                            }

                            this.DrawBody(joints, jointPoints, dc, drawPen);

                            this.DrawHand(body.HandLeftState, jointPoints[JointType.HandLeft], dc);
                            this.DrawHand(body.HandRightState, jointPoints[JointType.HandRight], dc);

                            distance_root_knee = p_base_spine - p_lknee;   //dystans miedzy kolanem a poczatkiem kregoslopa
                            double distance_root_hand = p_lhand - p_lknee; //dystand lewe kolano lewa reka

                            while (jumping_jack_done && distance_root_hand < 0.2)
                            {
                                counter_jumping_jack++;
                                jumping_jack_done      = false;
                                JumpinJacksOutput.Text = counter_jumping_jack.ToString();
                                break;
                            }
                            while (distance_root_hand < 0.2 && !under_root)
                            {
                                under_root        = true;
                                under_return_root = false;
                            }
                            while (p_lhand > p_shoulder && p_rhand > p_shoulder && under_root && !shoulder_height)
                            {
                                shoulder_height = true;
                                System.Console.WriteLine("Rece ponad barkami" + distance_root_hand);
                            }
                            while (p_lhand > p_head && p_rhand > p_head && shoulder_height && !pac)
                            {
                                pac = true;
                                System.Console.WriteLine("Rece nad glowa");
                            }
                            while (p_head < p_rhand && p_head < p_lhand && pac && !under_return_root)
                            {
                                under_return_root = true;
                                System.Console.WriteLine("Rece pod rootem -> returns");
                            }

                            while (under_return_root && !jumping_jack_done)
                            {
                                under_root        = false;
                                shoulder_height   = false;
                                pac               = false;
                                under_return_root = false;
                                jumping_jack_done = true;
                            }

                            //skretosklony================================================================================

                            distance_foot = x_rfoot - x_lfoot;


                            double rece_ramiona = p_lhand - p_shoulder;
                            while (skret_done && p_lhand < p_mid_base && p_lhand > p_lknee)
                            {
                                counter_skret_done++;
                                skret_done    = false;
                                skretOut.Text = counter_skret_done.ToString();
                                break;
                                System.Console.WriteLine("skretosklon");
                            }
                            while (rece_ramiona < 0.2 && !position_skret)
                            {
                                position_skret        = true;
                                return_position_skret = false;
                                System.Console.WriteLine("aaaaaaaa");
                            }
                            while ((p_lhand < p_lknee && !half_skret) || (p_rhand < p_lknee && !half_skret) && position_skret)
                            {
                                half_skret = true;
                                System.Console.WriteLine("bbbbbb2");
                            }

                            while ((p_lhand > p_head || p_rhand > p_head) && !hand_on_ground)
                            {
                                hand_on_ground = true;
                                System.Console.WriteLine("ccccccccc");
                            }
                            rece_ramiona = p_lhand - p_shoulder;
                            while (p_head > p_mid_base && hand_on_ground && !return_position_skret && rece_ramiona < 0.2)
                            {
                                return_position_skret = true;
                                position_skret        = false;
                                System.Console.WriteLine("ddddddd");
                            }

                            while (!position_skret && return_position_skret)
                            {
                                position_skret        = false;
                                return_position_skret = false;

                                hand_on_ground = false;
                                half_skret     = false;
                                System.Console.WriteLine("eeeee");
                                skret_done = true;
                            }


                            //=======================================================================================================
                            //////////////////////////////////////
                            //150 na zapisywanie
                            // sklony ====================================================================

                            distance_foot = x_rfoot - x_lfoot;

                            while (slope_done && p_head < p_lhand)
                            {
                                counter_slope_done += 1;
                                sklonyOut.Text      = counter_slope_done.ToString();
                                slope_done          = false;
                                System.Console.WriteLine("skretosklon");

                                break;
                            }

                            while (p_lhand - p_shoulder < 0.2 && !position_slope)
                            {
                                position_slope        = true;
                                return_position_slope = false;
                                System.Console.WriteLine("111111111111111");
                            }
                            while (p_lhand < p_lknee && !half_slope)
                            {
                                half_slope = true;
                                System.Console.WriteLine("222222222");
                            }

                            while (p_rhand - p_lfoot < 0.2 && p_lhand - p_lfoot < 0.2 && !hands_on_ground && half_slope)
                            {
                                hands_on_ground = true;
                                System.Console.WriteLine("333333");
                            }
                            while (p_head > p_mid_base && hands_on_ground && !return_position_slope)
                            {
                                return_position_slope = true;
                                position_slope        = false;
                                System.Console.WriteLine("44444444");
                            }

                            while (!position_slope && return_position_slope)
                            {
                                position_slope        = false;
                                return_position_slope = false;

                                hands_on_ground = false;
                                half_slope      = false;
                                System.Console.WriteLine("5555555");
                                slope_done = true;
                            }

                            ////////////////////
                        }
                    }

                    double root_move = p_base_spine - p_lknee;// dystans miedzy poczatkiem kregoslupa a kolanem


                    while (p_lelbow > p_mid_base && sit_up_done)
                    {
                        counter_sit_up++;
                        SitUps.Text = counter_sit_up.ToString();
                        sit_up_done = false;
                    }

                    if (p_lhand == p_head && p_rhand == p_head && root_move > 0.2 && root_goes_down == false)
                    {
                        hands_on_head = true;
                    }

                    if (root_move < 0.1)
                    {
                        root_goes_down = true;//przykucykuc
                    }

                    if (root_move > 0.2 && root_goes_down == true)
                    {
                        sit_up_done    = true;
                        root_goes_down = false;
                    }

                    this.drawingGroup.ClipGeometry = new RectangleGeometry(new Rect(0.0, 0.0, this.displayWidth, this.displayHeight));


                    //===========================sklonyOut,brzuszkiOut====================================================
                }
            }
        }
Ejemplo n.º 39
0
        public void Read_BodyFrameArrived(object sender, BodyFrameArrivedEventArgs e)
        {
            bool dataReceived = false;

            using (BodyFrame bodyFrame = e.FrameReference.AcquireFrame())
            {
                if (bodyFrame != null)
                {
                    if (this.bodies == null)
                    {
                        this.bodies = new Body[bodyFrame.BodyCount];
                    }

                    bodyFrame.GetAndRefreshBodyData(this.bodies);
                    dataReceived = true;
                }
            }

            if (dataReceived)
            {
                using (DrawingContext dc = this.drawingGroup.Open())
                {
                    dc.DrawRectangle(Brushes.Transparent, null, new Rect(0.0, 0.0, this.displayWidth, this.displayHeight));
                    //Transparent

                    int penIndex = 0;
                    foreach (Body body in this.bodies)
                    {
                        Pen drawPen = this.bodyColors[penIndex++];

                        if (body.IsTracked)
                        {
                            this.DrawClippedEdges(body, dc);

                            IReadOnlyDictionary <JointType, Joint> joints = body.Joints;

                            Dictionary <JointType, Point> jointPoints = new Dictionary <JointType, Point>();

                            foreach (JointType jointType in joints.Keys)
                            {
                                CameraSpacePoint position = joints[jointType].Position;
                                if (position.Z < 0)
                                {
                                    position.Z = InferredZPositionClamp;
                                }

                                DepthSpacePoint depthSpacePoint = this.coordinateMapper.MapCameraPointToDepthSpace(position);
                                jointPoints[jointType] = new Point(depthSpacePoint.X, depthSpacePoint.Y);
                            }

                            this.DrawBody(joints, jointPoints, dc, drawPen);

                            HandMotion_Tracking.DrawHand_Right(body.HandRightState, jointPoints[JointType.HandRight], dc);
                            //HandMotion_Tracking.DrawHand_left(body.HandLeftState, jointPoints[JointType.HandLeft], dc);
                        }
                    }

                    this.drawingGroup.ClipGeometry = new RectangleGeometry(new Rect(0.0, 0.0, this.displayWidth, this.displayHeight));
                }
            }
        }
        /// <summary>
        /// Handles the body frame data arriving from the sensor and updates the associated gesture detector object for each body
        /// </summary>
        /// <param name="sender">object sending the event</param>
        /// <param name="e">event arguments</param>
        private void Reader_BodyFrameArrived(object sender, BodyFrameArrivedEventArgs e)
        {
            bool dataReceived = false;

            using (BodyFrame bodyFrame = e.FrameReference.AcquireFrame())
            {
                if (bodyFrame != null)
                {
                    if (this.bodies == null)
                    {
                        // creates an array of 6 bodies, which is the max number of bodies that Kinect can track simultaneously
                        this.bodies = new Body[bodyFrame.BodyCount];
                    }

                    // The first time GetAndRefreshBodyData is called, Kinect will allocate each Body in the array.
                    // As long as those body objects are not disposed and not set to null in the array,
                    // those body objects will be re-used.
                    bodyFrame.GetAndRefreshBodyData(this.bodies);
                    if (!this.bodies[this.activeBodyIndex].IsTracked)
                    {
                        // we lost tracking of the active body, so update to the first tracked body in the array
                        int bodyIndex = this.GetActiveBodyIndex();

                        if (bodyIndex >= 0)
                        {
                            this.activeBodyIndex = bodyIndex;
                        }
                    }
                    dataReceived = true;
                }
            }

            if (dataReceived)
            {
                // visualize the new body data
                this.kinectBodyView.UpdateBodyFrame(this.bodies);
                sagittalAngle.Text = kinectBodyView.kinectFeedback.sagittalAngleTxt;
                flexAngle.Text     = kinectBodyView.kinectFeedback.flexAngleTxt;
                zeroBool.Text      = kinectBodyView.kinectFeedback.isZero;
                thirtyBool.Text    = kinectBodyView.kinectFeedback.isThirty;
                isFullFlex.Text    = kinectBodyView.kinectFeedback.isFlex;

                // we may have lost/acquired bodies, so update the corresponding gesture detectors
                if (this.bodies != null)
                {
                    // loop through all bodies to see if any of the gesture detectors need to be updated
                    //int maxBodies = this.kinectSensor.BodyFrameSource.BodyCount;
                    int maxBodies = 1;
                    for (int i = 0; i < maxBodies; ++i)
                    {
                        Body  activeBody = this.bodies[this.activeBodyIndex];
                        ulong trackingId = activeBody.TrackingId;

                        // if the current body TrackingId changed, update the corresponding gesture detector with the new value
                        if (activeBody.TrackingId != this.gestureDetectorList[i].TrackingId)
                        {
                            this.gestureDetectorList[i].TrackingId = trackingId;

                            // if the current body is tracked, unpause its detector to get VisualGestureBuilderFrameArrived events
                            // if the current body is not tracked, pause its detector so we don't waste resources trying to get invalid gesture results
                            this.gestureDetectorList[i].IsPaused = trackingId == 0;
                        }
                    }
                }
            }
        }
        private void Reader_MultiSourceFrameArrived(object sender, MultiSourceFrameArrivedEventArgs e)
        {
            int depthWidth  = 0;
            int depthHeight = 0;

            DepthFrame     depthFrame     = null;
            ColorFrame     colorFrame     = null;
            BodyIndexFrame bodyIndexFrame = null;
            BodyFrame      bodyFrame      = null;
            bool           isBitmapLocked = false;

            try
            {
                MultiSourceFrame multiSourceFrame = e.FrameReference.AcquireFrame();

                depthFrame     = multiSourceFrame.DepthFrameReference.AcquireFrame();
                colorFrame     = multiSourceFrame.ColorFrameReference.AcquireFrame();
                bodyIndexFrame = multiSourceFrame.BodyIndexFrameReference.AcquireFrame();
                bodyFrame      = multiSourceFrame.BodyFrameReference.AcquireFrame();

                if ((depthFrame == null) || (colorFrame == null) || (bodyIndexFrame == null) || (bodyFrame == null))
                {
                    return;
                }

                if (this.bodies == null)
                {
                    this.bodies = new Body[bodyFrame.BodyCount];
                }

                bodyFrame.GetAndRefreshBodyData(this.bodies);
                bodyFrame.Dispose();
                bodyFrame = null;

                bool isBodyTracked = false;
                foreach (Body body in this.bodies)
                {
                    if (body.IsTracked)
                    {
                        isBodyTracked = true;
                        continue;
                    }
                }
                hasTrackedBodies = isBodyTracked;
                if (hasTrackedBodies && !isCapturing)
                {
                    BeginCountdown();
                }

                FrameDescription depthFrameDescription = depthFrame.FrameDescription;

                depthWidth  = depthFrameDescription.Width;
                depthHeight = depthFrameDescription.Height;

                using (KinectBuffer depthFrameData = depthFrame.LockImageBuffer())
                {
                    this.coordinateMapper.MapColorFrameToDepthSpaceUsingIntPtr(
                        depthFrameData.UnderlyingBuffer,
                        depthFrameData.Size,
                        this.colorMappedToDepthPoints);
                }

                depthFrame.Dispose();
                depthFrame = null;

                this.liveBitmap.Lock();
                isBitmapLocked = true;

                colorFrame.CopyConvertedFrameDataToIntPtr(this.liveBitmap.BackBuffer, this.bitmapBackBufferSize, ColorImageFormat.Bgra);

                colorFrame.Dispose();
                colorFrame = null;

                using (KinectBuffer bodyIndexData = bodyIndexFrame.LockImageBuffer())
                {
                    unsafe
                    {
                        byte *bodyIndexDataPointer = (byte *)bodyIndexData.UnderlyingBuffer;

                        int colorMappedToDepthPointCount = this.colorMappedToDepthPoints.Length;

                        fixed(DepthSpacePoint *colorMappedToDepthPointsPointer = this.colorMappedToDepthPoints)
                        {
                            uint *bitmapPixelsPointer = (uint *)this.liveBitmap.BackBuffer;

                            for (int colorIndex = 0; colorIndex < colorMappedToDepthPointCount; ++colorIndex)
                            {
                                float colorMappedToDepthX = colorMappedToDepthPointsPointer[colorIndex].X;
                                float colorMappedToDepthY = colorMappedToDepthPointsPointer[colorIndex].Y;

                                if (!float.IsNegativeInfinity(colorMappedToDepthX) &&
                                    !float.IsNegativeInfinity(colorMappedToDepthY))
                                {
                                    int depthX = (int)(colorMappedToDepthX + 0.5f);
                                    int depthY = (int)(colorMappedToDepthY + 0.5f);

                                    if ((depthX >= 0) && (depthX < depthWidth) && (depthY >= 0) && (depthY < depthHeight))
                                    {
                                        int depthIndex = (depthY * depthWidth) + depthX;
                                        if (bodyIndexDataPointer[depthIndex] != 0xff)
                                        {
                                            continue;
                                        }
                                    }
                                }

                                bitmapPixelsPointer[colorIndex] = 0;
                            }
                        }

                        this.liveBitmap.AddDirtyRect(new Int32Rect(0, 0, this.liveBitmap.PixelWidth, this.liveBitmap.PixelHeight));
                    }
                }
            }
            finally
            {
                if (isBitmapLocked)
                {
                    this.liveBitmap.Unlock();
                }
                if (depthFrame != null)
                {
                    depthFrame.Dispose();
                }
                if (colorFrame != null)
                {
                    colorFrame.Dispose();
                }
                if (bodyIndexFrame != null)
                {
                    bodyIndexFrame.Dispose();
                }
                if (bodyFrame != null)
                {
                    bodyFrame.Dispose();
                }
            }
        }
        /// <summary>
        /// Handles the body frame data arriving from the sensor and updates the associated gesture detector object for each body
        /// </summary>
        /// <param name="sender">object sending the event</param>
        /// <param name="e">event arguments</param>
        private void Reader_BodyFrameArrived(object sender, BodyFrameArrivedEventArgs e)
        {
            bool dataReceived = false;

            using (BodyFrame bodyFrame = e.FrameReference.AcquireFrame())
            {
                if (bodyFrame != null)
                {
                    if (this.bodies == null)
                    {
                        // creates an array of 6 bodies, which is the max number of bodies that Kinect can track simultaneously
                        this.bodies = new Body[bodyFrame.BodyCount];
                    }

                    // The first time GetAndRefreshBodyData is called, Kinect will allocate each Body in the array.
                    // As long as those body objects are not disposed and not set to null in the array,
                    // those body objects will be re-used.
                    bodyFrame.GetAndRefreshBodyData(this.bodies);

                    foreach (var body in bodies)
                    {
                        if (body != null)
                        {
                            if (body.IsTracked)
                            {
                                Joint handRight = body.Joints[JointType.HandRight];

                                CameraSpacePoint skeletonPoint = handRight.Position;

                                DepthSpacePoint depthPoint = this.kinectSensor.CoordinateMapper.MapCameraPointToDepthSpace(skeletonPoint);

                                /*
                                 * this.Dispatcher.Invoke(() =>
                                 * {
                                 *  cursor.Flip(handRight);
                                 *  cursor.Update(depthPoint);
                                 * });*/
                            }
                        }
                    }

                    dataReceived = true;
                }
                else
                {
                    Console.WriteLine("bodyFrame is null");
                }
            }

            if (dataReceived)
            {
                // visualize the new body data
                this.kinectBodyView.UpdateBodyFrame(this.bodies);

                // we may have lost/acquired bodies, so update the corresponding gesture detectors
                if (this.bodies != null)
                {
                    // loop through all bodies to see if any of the gesture detectors need to be updated
                    int maxBodies = 1; // this.kinectSensor.BodyFrameSource.BodyCount;
                    for (int i = 0; i < maxBodies; ++i)
                    {
                        Body  body       = this.bodies[i];
                        ulong trackingId = body.TrackingId;

                        // if the current body TrackingId changed, update the corresponding gesture detector with the new value
                        if (trackingId != this.gestureDetectorList[i].TrackingId)
                        {
                            this.gestureDetectorList[i].TrackingId = trackingId;

                            // if the current body is tracked, unpause its detector to get VisualGestureBuilderFrameArrived events
                            // if the current body is not tracked, pause its detector so we don't waste resources trying to get invalid gesture results
                            this.gestureDetectorList[i].IsPaused = trackingId == 0;
                        }
                    }
                }
            }
        }
Ejemplo n.º 43
0
        private void OnFrameArrived(object sender, MultiSourceFrameArrivedEventArgs e)
        {
            double           utcTime          = (DateTime.UtcNow - new DateTime(1970, 1, 1)).TotalSeconds;
            MultiSourceFrame multiSourceFrame = e.FrameReference.AcquireFrame();

            using (ColorFrame colorFrame = multiSourceFrame.ColorFrameReference.AcquireFrame())
            {
                if (colorFrame != null)
                {
                    colorFrame.CopyConvertedFrameDataToArray(this.colorArray, ColorImageFormat.Bgra);
                    System.Buffer.BlockCopy(this.colorArray, 0, this.byteColorArray, 0, (this.kinect.ColorFrameSource.FrameDescription.Height * this.kinect.ColorFrameSource.FrameDescription.Width * BYTES_PER_COLOR_PIXEL));
                    System.Buffer.BlockCopy(BitConverter.GetBytes(utcTime), 0, this.byteColorArray, (this.kinect.ColorFrameSource.FrameDescription.Height * this.kinect.ColorFrameSource.FrameDescription.Width * BYTES_PER_COLOR_PIXEL), sizeof(double));
                    this.colorConnector.Broadcast(this.byteColorArray);
                }
            }

            using (DepthFrame depthFrame = multiSourceFrame.DepthFrameReference.AcquireFrame())
            {
                if (depthFrame != null)
                {
                    depthFrame.CopyFrameDataToArray(this.depthArray);
                    System.Buffer.BlockCopy(this.depthArray, 0, this.byteDepthArray, 0, this.kinect.DepthFrameSource.FrameDescription.Height * this.kinect.DepthFrameSource.FrameDescription.Width * BYTES_PER_DEPTH_PIXEL);
                    System.Buffer.BlockCopy(BitConverter.GetBytes(utcTime), 0, this.byteDepthArray, this.kinect.DepthFrameSource.FrameDescription.Height * this.kinect.DepthFrameSource.FrameDescription.Width * BYTES_PER_DEPTH_PIXEL, sizeof(double));
                    this.depthConnector.Broadcast(this.byteDepthArray);
                }
            }

            using (InfraredFrame irFrame = multiSourceFrame.InfraredFrameReference.AcquireFrame())
            {
                if (irFrame != null)
                {
                    irFrame.CopyFrameDataToArray(this.irArray);
                    System.Buffer.BlockCopy(this.irArray, 0, this.byteIRArray, 0, this.kinect.InfraredFrameSource.FrameDescription.Height * this.kinect.InfraredFrameSource.FrameDescription.Width * BYTES_PER_IR_PIXEL);
                    System.Buffer.BlockCopy(BitConverter.GetBytes(utcTime), 0, this.byteIRArray, this.kinect.InfraredFrameSource.FrameDescription.Height * this.kinect.InfraredFrameSource.FrameDescription.Width * BYTES_PER_IR_PIXEL, sizeof(double));
                    this.irConnector.Broadcast(this.byteIRArray);
                }
            }

            using (BodyFrame bodyFrame = multiSourceFrame.BodyFrameReference.AcquireFrame())
            {
                if (bodyFrame != null)
                {
                    bodyFrame.GetAndRefreshBodyData(this.bodyArray);

                    /*//Console.WriteLine("New bodies");
                     * foreach (Body b in this.bodyArray)
                     * {
                     *  //Console.WriteLine(b.TrackingId);
                     *  //Console.WriteLine(b.IsTracked);
                     * }*/
                    for (int i = 0; i < this.bodyArray.Length; i++)
                    {
                        this.filter.UpdateFilter(this.bodyArray[i]);
                    }
                    string jsonString = JsonConvert.SerializeObject(this.bodyArray);
                    int    diff       = 40000 - jsonString.Length;

                    /*for (int i = 0; i < diff;i++ )
                     * {
                     *  jsonString += " ";
                     * }*/

                    byte[] bodyByteArray = new byte[jsonString.Length * sizeof(char) + sizeof(double) + 4]; //modification start
                    int    sizeofString  = jsonString.Length * sizeof(char) + sizeof(double);
                    byte[] sizeByte      = BitConverter.GetBytes(sizeofString);
                    System.Buffer.BlockCopy(sizeByte, 0, bodyByteArray, 0, 4);
                    System.Buffer.BlockCopy(jsonString.ToCharArray(), 0, bodyByteArray, 4, jsonString.Length * sizeof(char));
                    System.Buffer.BlockCopy(BitConverter.GetBytes(utcTime), 0, bodyByteArray, 4 + jsonString.Length * sizeof(char), sizeof(double));
                    this.bodyConnector.Broadcast(bodyByteArray);
                }
            }
        }
Ejemplo n.º 44
0
 public static extern int GetBodyFrameData(ref BodyFrame pBodyFrame, bool bGetOrientations, bool bGetHandStates);
        /// <summary>
        /// Handles the body frame data arriving from the sensor
        /// </summary>
        /// <param name="sender">object sending the event</param>
        /// <param name="e">event arguments</param>
        private void Reader_FrameArrived(object sender, BodyFrameArrivedEventArgs e)
        {
            bool dataReceived = false;

            using (BodyFrame bodyFrame = e.FrameReference.AcquireFrame())
            {
                if (bodyFrame != null)
                {
                    if (this.bodies == null)
                    {
                        this.bodies = new Body[bodyFrame.BodyCount];
                    }

                    // The first time GetAndRefreshBodyData is called, Kinect will allocate each Body in the array.
                    // As long as those body objects are not disposed and not set to null in the array,
                    // those body objects will be re-used.
                    bodyFrame.GetAndRefreshBodyData(this.bodies);
                    dataReceived = true;
                }
            }

            if (dataReceived)
            {
                using (DrawingContext dc = this.drawingGroup.Open())
                {
                    // Draw a transparent background to set the render size
                    dc.DrawRectangle(Brushes.Black, null, new Rect(0.0, 0.0, this.displayWidth, this.displayHeight));



                    int penIndex = 0;
                    foreach (Body body in this.bodies)
                    {
                        Pen drawPen = this.bodyColors[penIndex++];

                        if (body.IsTracked)
                        {
                            //The following function is commented out to reduce load on the machine
                            //this.DrawClippedEdges(body, dc);

                            IReadOnlyDictionary <JointType, Joint> joints = body.Joints;

                            // convert the joint points to depth (display) space
                            Dictionary <JointType, Point> jointPoints = new Dictionary <JointType, Point>();



                            foreach (JointType jointType in joints.Keys)
                            {
                                // sometimes the depth(Z) of an inferred joint may show as negative
                                // clamp down to 0.1f to prevent coordinatemapper from returning (-Infinity, -Infinity)
                                CameraSpacePoint position = joints[jointType].Position;
                                if (position.Z < 0)
                                {
                                    position.Z = InferredZPositionClamp;
                                }

                                DepthSpacePoint depthSpacePoint = this.coordinateMapper.MapCameraPointToDepthSpace(position);
                                jointPoints[jointType] = new Point(depthSpacePoint.X, depthSpacePoint.Y);
                            }

                            // Added parameter: StreamWriter sw
                            this.DrawBody(joints, jointPoints, dc, drawPen);

                            ///this.DrawHand(body.HandLeftState, jointPoints[JointType.HandLeft], dc);
                            ///this.DrawHand(body.HandRightState, jointPoints[JointType.HandRight], dc);
                        }
                    }

                    // prevent drawing outside of our render area
                    this.drawingGroup.ClipGeometry = new RectangleGeometry(new Rect(0.0, 0.0, this.displayWidth, this.displayHeight));
                }
            }
        }
Ejemplo n.º 46
0
        /// <summary>
        /// Handles the body frame data arriving from the sensor
        /// </summary>
        /// <param name="sender">object sending the event</param>
        /// <param name="e">event arguments</param>
        private void Reader_FrameArrived(object sender, BodyFrameArrivedEventArgs e)
        {
            bool dataReceived = false;

            using (BodyFrame bodyFrame = e.FrameReference.AcquireFrame())
            {
                if (bodyFrame != null)
                {
                    if (this.bodies == null)
                    {
                        this.bodies = new Body[bodyFrame.BodyCount];
                    }
                    // The first time GetAndRefreshBodyData is called, Kinect will allocate each Body in the array.
                    // As long as those body objects are not disposed and not set to null in the array,
                    // those body objects will be re-used.
                    bodyFrame.GetAndRefreshBodyData(this.bodies);
                    dataReceived = true;
                }
            }

            if (dataReceived)
            {
                try
                {
                    using (DrawingContext dc = this.drawingGroup.Open())
                    {
                        // Draw a transparent background to set the render size
                        dc.DrawRectangle(Brushes.Transparent, null, new Rect(0.0, 0.0, this.displayWidth, this.displayHeight));

                        int penIndex = 0;
                        var body     = bodies[0];
                        foreach (Body thebody in this.bodies)
                        {
                            Pen drawPen = this.bodyColors[penIndex++];
                            if (thebody.IsTracked)
                            {
                                this.DrawBody(thebody, dc, drawPen);
                                if (body.Joints[JointType.SpineBase].Position.Z == 0)
                                {
                                    body = thebody;
                                }
                                else if (body.Joints[JointType.SpineBase].Position.Z > thebody.Joints[JointType.SpineBase].Position.Z)
                                {
                                    body = thebody;
                                }
                            }
                        }

                        Pen drawPenInside  = new Pen(Brushes.LightBlue, 11);
                        Pen drawPenOutside = new Pen(Brushes.DarkBlue, 11);
                        this.DrawClippedEdges(body, dc);
                        if (outside)
                        {
                            this.DrawBody(body, dc, drawPenOutside);
                        }
                        else
                        {
                            this.DrawBody(body, dc, drawPenInside);
                            #region PPT control
                            CameraSpacePoint head      = getCameraSpacePoint(body, JointType.Head);
                            CameraSpacePoint handL     = getCameraSpacePoint(body, JointType.HandLeft);
                            CameraSpacePoint handR     = getCameraSpacePoint(body, JointType.HandRight);
                            CameraSpacePoint spineBase = getCameraSpacePoint(body, JointType.SpineBase);
                            CameraSpacePoint shoulderR = getCameraSpacePoint(body, JointType.ShoulderRight);
                            CameraSpacePoint shoulderL = getCameraSpacePoint(body, JointType.ShoulderLeft);

                            if (hiding)
                            {
                                state.Content    = "";
                                state.Background = Brushes.Transparent;
                            }
                            else
                            {
                                state.Content    = mode[State];
                                state.Background = Brushes.Black;
                            }
                            if (handL.Y < head.Y)
                            {
                                State = LOCKED;
                            }
                            if (State == LOCKED)
                            {
                            }
                            else if (spineBase.Z - handL.Z > LIFT && spineBase.Z - handR.Z < LIFT && handR.X - spineBase.X > 150)
                            {
                                exit();
                            }
                            else if (spineBase.Z - handL.Z > LIFT && spineBase.Z - handR.Z > LIFT)
                            {
                                if (body.HandRightState == HandState.Open && body.HandLeftState == HandState.Open)
                                {
                                    State = TOESC;
                                }
                                if (body.HandRightState == HandState.Open && body.HandLeftState == HandState.Closed)
                                {
                                    State = SELECTING; itab = 0;
                                }
                                if (body.HandRightState == HandState.Closed && body.HandLeftState == HandState.Open)
                                {
                                    State = TOPLAY;
                                }
                                if (body.HandRightState == HandState.Closed && body.HandLeftState == HandState.Closed)
                                {
                                    State = TOSTART;
                                }
                                if (body.HandRightState == HandState.Open && body.HandLeftState == HandState.Lasso)
                                {
                                    State = TOHIDE;
                                }
                            }
                            switch (State)
                            {
                            case LOCKED:
                                if (handR.Y < head.Y)
                                {
                                    State = CONTROLLING;
                                }
                                break;

                            case TOHIDE:
                                if (spineBase.Z - handL.Z < LIFT && spineBase.Z - handR.Z < LIFT)
                                {
                                    hiding = !hiding;
                                    State  = CONTROLLING;
                                }
                                break;

                            case TOSTART:
                                if (spineBase.Z - handL.Z < LIFT && spineBase.Z - handR.Z < LIFT)
                                {
                                    SendKeys.SendWait("{F5}");
                                    State = CONTROLLING;
                                }
                                break;

                            case TOPLAY:
                                if (spineBase.Z - handL.Z < LIFT && spineBase.Z - handR.Z < LIFT)
                                {
                                    SendKeys.SendWait("{Tab}");
                                    SendKeys.SendWait("{Enter}");
                                    State = LOCKED;
                                }
                                break;

                            case TOESC:
                                if (spineBase.Z - handL.Z < LIFT && spineBase.Z - handR.Z < LIFT)
                                {
                                    SendKeys.SendWait("{Esc}");
                                    State = LOCKED;
                                }
                                break;

                            case CONTROLLING:
                                if (spineBase.Z - handL.Z > LIFT && spineBase.Z - handR.Z < LIFT)     // if left hand lift forward only
                                {
                                    if (handL.X < shoulderL.X)
                                    {
                                        prevpage = true;
                                    }
                                    else
                                    {
                                        if (prevpage)
                                        {
                                            SendKeys.SendWait("{Left}");
                                            prevpage = false;
                                        }
                                    }
                                }
                                else if (spineBase.Z - handR.Z > LIFT && spineBase.Z - handL.Z < LIFT)     // if right hand lift forward only
                                {
                                    if (handR.X > shoulderR.X)
                                    {
                                        nextpage = true;
                                    }
                                    else
                                    {
                                        if (nextpage)
                                        {
                                            SendKeys.SendWait("{Right}");
                                            nextpage = false;
                                        }
                                    }
                                }
                                break;

                            case SELECTING:
                                if (itab != 0)
                                {
                                    state.Content += "(" + itab.ToString() + ")";
                                }
                                if (spineBase.Z - handL.Z < LIFT && spineBase.Z - handR.Z < LIFT)
                                {
                                    SendKeys.SendWait("{Enter}");
                                    State = CONTROLLING;
                                }
                                else if (spineBase.Z - handL.Z > LIFT && spineBase.Z - handR.Z < LIFT)
                                {
                                    if (handL.X < shoulderL.X)
                                    {
                                        prevpage = true;
                                    }
                                    else
                                    {
                                        if (prevpage)
                                        {
                                            SendKeys.SendWait("+{Tab}");
                                            itab--;
                                            prevpage = false;
                                        }
                                    }
                                }
                                else if (spineBase.Z - handR.Z > LIFT && spineBase.Z - handL.Z < LIFT)
                                {
                                    if (handR.X > shoulderR.X)
                                    {
                                        nextpage = true;
                                    }
                                    else
                                    {
                                        if (nextpage)
                                        {
                                            SendKeys.SendWait("{Tab}");
                                            itab++;
                                            nextpage = false;
                                        }
                                    }
                                }
                                break;

                            default: break;
                            }
                            #endregion
                        }

                        // prevent drawing outside of our render area
                        this.drawingGroup.ClipGeometry = new RectangleGeometry(new Rect(0.0, 0.0, this.displayWidth, this.displayHeight));
                    }
                }
                catch { return; }
            }
        }
Ejemplo n.º 47
0
 // Reads in the bodyFrame (if it contains bodies)
 private void GetBodyJoints(BodyFrame bodyFrame)
 {
     //Makes sure the bodyFrame contains bodies, and if so, puts the data in bodies
     if(bodyFrame != null)
     {
         if (this.bodies == null)
         {
             bodies = new Body[bodyFrame.BodyCount];
         }
         // The first time GetAndRefreshBodyData is called, Kinect will allocate each Body in the array.
         // As long as those body objects are not disposed and not set to null in the array,
         // those body objects will be re-used.
         bodyFrame.GetAndRefreshBodyData(this.bodies);
         dataReceived = true;
     }
 }
Ejemplo n.º 48
0
        /*  Analisi dei dati provenienti dal sensore, identificazione utente e calibrazione area di gioco
         *  area di gioco identificata a partire dalla posizione iniziale dell'utente
         */
        void reader_MultiSourceFrameArrived(object sender, MultiSourceFrameArrivedEventArgs e)
        {
            MultiSourceFrame frame_reference = e.FrameReference.AcquireFrame();

            /*Prelevo frame RGB*/
            using (ColorFrame frame = frame_reference.ColorFrameReference.AcquireFrame())
            {
                if (frame != null)
                {
                    this._currentCFrame = frame.ToBitmap();
                }
            }

            /*Elaborazione informazioni body Frame*/
            using (BodyFrame bodyFrame = frame_reference.BodyFrameReference.AcquireFrame())
            {
                if (bodyFrame != null)
                {
                    if (bodies == null)
                    {
                        /* Alloco un vettore di dimensioni pari al numero di persone identificate
                         * all'avvio del sensore
                         */
                        this.bodies = new Body[bodyFrame.BodyCount];
                    }

                    /* Aggiorno le informazioni sui corpi identificati dalla kinect */
                    bodyFrame.GetAndRefreshBodyData(this.bodies);

                    #region Player Lost

                    /* Caso di Player Perso
                     * Potrebbe capitare che il player esca dall'area tracciata dalla kinect, in tal caso va notificato all'utente
                     * di rientrare nell'area di gioco ed alzare il braccio destro o sinistro per essere reintrodotto nel sistema.
                     * Il caso di player perso è identificato dalla variabile _playerLost==True e _playerIsRecognizer==True;
                     * */

                    //Verifica se tra i corpi tracciati dal sensore è presente il player, verifica effettuata tramite TrackingID
                    if (this._playerIsRecognized)
                    {
                        this._playerLost = true;
                        foreach (Body body in this.bodies)
                        {
                            if (body.IsTracked && body.TrackingId == this._playerIdentifier)
                            {
                                this._playerLost = false;
                                break;
                            }
                        }
                    }

                    //se l'utente è uscito dal campo visivo della kinect
                    if (this._playerLost)
                    {
                        //Se è stato già lanciato l'evento effettuo analisi per identificare di nuovo il player
                        if (this._playerLost_Launched)
                        {
                            foreach (Body body in this.bodies)
                            {
                                if (body.IsTracked)
                                {
                                    //Se utente alza una delle due mani sopra la spalla viene identificato come Player
                                    if (body.Joints[JointType.HandRight].Position.Y > body.Joints[JointType.Head].Position.Y ||
                                        body.Joints[JointType.HandLeft].Position.Y > body.Joints[JointType.Head].Position.Y)
                                    {
                                        this._playerIdentifier    = body.TrackingId;
                                        this._playerLost_Launched = false;
                                        this.OnPlayerLocked();
                                    }
                                }
                            }
                        }
                        //Se l'evento di player perso non è stato lanciato, viene lanciato e termina funzione
                        else
                        {
                            this.OnPlayerLost();
                        }
                        //Terminate le operazioni di Player Perso termino notifico aggiornamento dati e ritorno
                        this.OnDataUpdated();
                        return;
                    }


                    #endregion

                    //Player non identificato
                    if (!this._playerIsRecognized)
                    {
                        #region PlayerIdentification
                        foreach (Body body in this.bodies)
                        {
                            if (body.IsTracked)
                            {
                                //Se utente alza entrambe le mani all'altezza delle spalle
                                if (body.Joints[JointType.HandRight].Position.Y > body.Joints[JointType.ElbowRight].Position.Y &&
                                    body.Joints[JointType.HandLeft].Position.Y > body.Joints[JointType.ElbowLeft].Position.Y)
                                {
                                    this._currentPlayerPosition.X = body.Joints[_trackedJoint].Position.X;
                                    this._currentPlayerPosition.Y = body.Joints[_trackedJoint].Position.Y;
                                    this._currentPlayerPosition.Z = body.Joints[_trackedJoint].Position.Z;
                                    //Salvo la posizione iniziale dell'utente, utile in fase di calibrazione
                                    this._startingPlayerPosition = this._currentPlayerPosition;
                                    //Converto coordinate sensore in coordinate RGB
                                    this._playerIdColorPoint     = kinect.CoordinateMapper.MapCameraPointToColorSpace(body.Joints[_trackedJoint].Position);
                                    this._playerIdColorPoint3D.X = _playerIdColorPoint.X;
                                    this._playerIdColorPoint3D.Y = _playerIdColorPoint.Y;
                                    //Il body è identificato come player, salvo il trackingID
                                    this._playerIdentifier   = body.TrackingId;
                                    this._playerIsRecognized = true;
                                    //Notifico cambio di stato del player
                                    this.OnPlayerLocked();
                                    //Esco dal ciclo
                                    break;
                                }
                            }
                        }
                        #endregion PlayerIdentification
                    }
                    //Player identificato
                    else
                    {
                        #region Movement Analysis
                        foreach (Body body in bodies)
                        {
                            //Se il corpo è tracciato ed è quello del player allora memorizzo la posizione
                            if (body.IsTracked && body.TrackingId == this._playerIdentifier)
                            {
                                this._currentPlayerPosition.X = body.Joints[_trackedJoint].Position.X;
                                this._currentPlayerPosition.Y = body.Joints[_trackedJoint].Position.Y;
                                this._currentPlayerPosition.Z = body.Joints[_trackedJoint].Position.Z;
                                this._playerIdColorPoint      = kinect.CoordinateMapper.MapCameraPointToColorSpace(body.Joints[_trackedJoint].Position);
                                this._playerIdColorPoint3D.X  = _playerIdColorPoint.X;
                                this._playerIdColorPoint3D.Y  = _playerIdColorPoint.Y;

                                //Conversione coordinata X
                                this._convertedPlayerPosition.X = this._currentPlayerPosition.X - this._startingPlayerPosition.X;
                                this._convertedPlayerPosition.Z = this._currentPlayerPosition.Z - this._startingPlayerPosition.Z;


                                /*
                                 * Normalizzo l'altezza del player a 2 metri, l'abbassamento è proporzionale
                                 * questo scalamento risolve il problema dell'altezza del sensore kinect, che influenza l'altezza
                                 * del personaggio nel mondo virtuale.
                                 */
                                //this._convertedPlayerPosition.Y = (float)Math.Round(((this._currentPlayerPosition.Y * 1.8) / this._startingPlayerPosition.Y), 2, MidpointRounding.AwayFromZero);
                                this._convertedPlayerPosition.Y = 2f + this._currentPlayerPosition.Y - this._startingPlayerPosition.Y;

                                //PUNTO PER INSERIRE EVENTUALI CONTROLLI DA PARTE DELL'UTENTE

                                //Esco dal ciclo
                                break;
                            }
                        }
                        #endregion
                    }

                    //Lancio Evento dati aggiornati
                    this.OnDataUpdated();
                }
            }
        }
Ejemplo n.º 49
0
    private void InitFrames(DepthImageFrame depthFrame, ColorImageFrame colorFrame, SkeletonFrame skeletonFrame) {
      if (init) { return; } init = true;

      // Color Frame
      Color = new ColorFrame();
      Color.Width = colorFrame.Width;
      Color.Height = colorFrame.Height;
      Color.Pixels = new byte[colorFrame.PixelDataLength];
      Color.Stamp = new Timestamp();
      Color.Fps = FPS;
      AddOnManager.GetInstance().InitFrame(Name, Color);
      Log(Color.ToString());
      ColorFormat = colorFrame.Format;

      // Depth Frame
      Depth = new DepthFrame();
      Depth.Width = depthFrame.Width;
      Depth.Height = depthFrame.Height;
      Depth.Pixelss = new short[depthFrame.PixelDataLength];
      Depth.Stamp = new Timestamp();
      AddOnManager.GetInstance().InitFrame(Name, Depth);
      Log(Depth.ToString());

      var dueTime = TimeSpan.FromMilliseconds(200);
      var interval = TimeSpan.FromMilliseconds(ConfigManager.GetInstance().Find("kinect_v1.motion.ms", 100));
      Task = new MotionTask(dueTime, interval);
      Task.Device = "";
      Task.AddFrame(Depth);
      Task.Start();


      // Skeleton Frame
      Skeletons = new BodyFrame();
      Skeletons.Width  = colorFrame.Width;
      Skeletons.Height = colorFrame.Height;
      Skeletons.RawData = new Skeleton[6];
      Skeletons.Bodies  = new List<NBody>(6);
      Skeletons.Stamp = new Timestamp();
      AddOnManager.GetInstance().InitFrame(Name, Skeletons);
      Log(Skeletons.ToString());

    }
Ejemplo n.º 50
0
    private void RefreshBodyData(BodyFrame frame) {
      cache.Clear();
      foreach (var skeleton in (IList<Skeleton>)frame.RawData) {
        if (skeleton.TrackingState != SkeletonTrackingState.Tracked) { continue; }

        NBody nbody = frame.Find(Convert.ToUInt64(skeleton.TrackingId));
        if (nbody == null) {
          nbody = new NBody(Convert.ToUInt64(skeleton.TrackingId), frame.Width, frame.Height);
        }

        cache.Add(nbody);
        RefreshBodyData(skeleton, nbody);
      }

      lock (frame.Bodies) {
        frame.Bodies.Clear();
        frame.Bodies.AddRange(cache);
      }
    }
Ejemplo n.º 51
0
        private void updateBodies(BodyFrame frame)
        {
            if (this.bodies == null)
            {
                this.bodies = new Body[frame.BodyCount];
            }

            // The first time GetAndRefreshBodyData is called, Kinect will allocate each Body in the array.
            // As long as those body objects are not disposed and not set to null in the array,
            // those body objects will be re-used.
            frame.GetAndRefreshBodyData(this.bodies);
        }
        private void processBodyFrame(BodyFrame bodyFrame, bool showSkeleton, bool showHandStates)
        {
            if (this.bodies == null)
            {
                this.bodies = new Body[bodyFrame.BodyCount];
            }

            // The first time GetAndRefreshBodyData is called, Kinect will allocate each Body in the array.
            // As long as those body objects are not disposed and not set to null in the array,
            // those body objects will be re-used.
            bodyFrame.GetAndRefreshBodyData(this.bodies);

            players[0] = null;
            players[1] = null;
            Body[] tempBodies = new Body[2];
            int playerCount = 0;
            foreach (Body body in this.bodies)
            {
                if (body.IsTracked && playerCount < 2)
                {
                    tempBodies[playerCount++] = body;
                }
            }

            if (playerCount == 2)
            {
                CameraSpacePoint player0Pos = tempBodies[0].Joints[JointType.SpineMid].Position;
                CameraSpacePoint player1Pos = tempBodies[1].Joints[JointType.SpineMid].Position;

                if (player0Pos.X < player1Pos.X)
                {
                    players[0] = new Player(tempBodies[0]);
                    players[1] = new Player(tempBodies[1]);
                }
                else
                {
                    players[1] = new Player(tempBodies[0]);
                    players[0] = new Player(tempBodies[1]);
                }
            }

            using (DrawingContext dc = this.drawingGroup.Open())
            {
                // Draw a transparent background to set the render size
                dc.DrawRectangle(Brushes.Transparent, null, new Rect(0.0, 0.0, this.displayWidth, this.displayHeight));

                int penIndex = 0;
                foreach (Player player in players)
                {
                    if (player == null) continue;
                    Body body = player.body;

                    Pen drawPen = this.bodyColors[penIndex++];

                    if (body.IsTracked)
                    {
                        //this.DrawClippedEdges(body, dc);

                        IReadOnlyDictionary<JointType, Joint> joints = body.Joints;

                        // convert the joint points to depth (display) space
                        Dictionary<JointType, Point> jointPoints = new Dictionary<JointType, Point>();
                        Dictionary<JointType, float> jointPointDepths = new Dictionary<JointType, float>();

                        foreach (JointType jointType in joints.Keys)
                        {
                            // sometimes the depth(Z) of an inferred joint may show as negative
                            // clamp down to 0.1f to prevent coordinatemapper from returning (-Infinity, -Infinity)
                            CameraSpacePoint position = joints[jointType].Position;
                            if (position.Z < 0)
                            {
                                position.Z = InferredZPositionClamp;
                            }

                            ColorSpacePoint colorSpacePoint = this.coordinateMapper.MapCameraPointToColorSpace(position);
                            jointPoints[jointType] = new Point(colorSpacePoint.X, colorSpacePoint.Y);
                            jointPointDepths[jointType] = position.Z;
                        }

                        // Maps the actual joint points to the player
                        player.jointPoints = jointPoints;
                        player.jointPointDepths = jointPointDepths;

                        if (showSkeleton)
                        {
                            this.DrawBody(joints, jointPoints, dc, drawPen);
                        }

                        if (showHandStates)
                        {
                            this.DrawHand(body.HandLeftState, jointPoints[JointType.HandLeft], dc);
                            this.DrawHand(body.HandRightState, jointPoints[JointType.HandRight], dc);
                        }

                        // THIS IS WHERE THE UPDATES GO. CAN'T MOVE IT BECAUSE OF THE GC OF DRAWING CONTEXT
                        /*
                        this.targetCircleL.update(
                            body.HandLeftState,
                            body.HandRightState,
                            jointPoints[JointType.HandLeft],
                            jointPoints[JointType.HandRight],
                            jointPointDepths[JointType.HandLeft],
                            jointPointDepths[JointType.HandLeft]
                        );
                        this.targetCircleL.draw(dc);
                         */
                        //basketballManager.update(player[0], player[1]);
                        //basketballManager.draw(dc);
                        //processBasketballManager(players[0], players[1], dc);
                    }
                }
                if (players[0] != null && players[1] != null)
                    processBasketballManager(players[0], players[1], dc);

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

            bodyFrame.Dispose();
        }
Ejemplo n.º 53
0
    private void StartBodyStream() {

      // Init infr buffer
      BodyFrame frame = Body = new BodyFrame();
      frame.Width   = Color.Width;
      frame.Height  = Color.Height;
      frame.RawData = new Body[6]; // FIXME
      frame.Bodies  = new List<NBody>(6);
      frame.Stamp   = new Timestamp();

      AddOnManager.GetInstance().InitFrame(Name, frame);
      Log(frame.ToString());

      // Start Watch
      BodyWatch = new StopwatchAvg();
    }
Ejemplo n.º 54
0
        private void Reader_MultiSourceFrameArrived(object sender, MultiSourceFrameArrivedEventArgs e)
        {
            WriteableBitmap depthWBitmap = null;
            WriteableBitmap colorWBitmap = null;
            Bitmap          colorBitmap  = null;
            Bitmap          depthBitmap  = null;

            MultiSourceFrame frame = e.FrameReference.AcquireFrame();

            if (frame != null)
            {
                using (DepthFrame depthFrame = frame.DepthFrameReference.AcquireFrame())
                {
                    if (depthFrame != null)
                    {
                        _frameParser.ParseToBitmaps(depthFrame, out depthBitmap, out depthWBitmap);

                        if (_selectedCamera == ECameraType.Depth)
                        {
                            if (_selectedCamera == ECameraType.Depth)
                            {
                                bodyViewer.UpdateFrameCounter();
                                bodyViewer.KinectImage = depthWBitmap;
                            }
                        }
                    }
                }

                if (_selectedCamera == ECameraType.Color)
                {
                    using (ColorFrame colorFrame = frame.ColorFrameReference.AcquireFrame())
                    {
                        if (colorFrame != null)
                        {
                            _frameParser.ParseToBitmaps(colorFrame, out colorBitmap, out colorWBitmap);

                            if (_selectedCamera == ECameraType.Color)
                            {
                                bodyViewer.UpdateFrameCounter();
                                bodyViewer.KinectImage = colorWBitmap;
                            }
                        }
                    }
                }

                using (BodyFrame bodyFrame = frame.BodyFrameReference.AcquireFrame())
                {
                    if (bodyFrame != null)
                    {
                        BodyFrameWrapper bodyFrameWrapper = new BodyFrameWrapper(bodyFrame);

                        if (bodyFrameWrapper.TrackedBodies.Any())
                        {
                            bodyViewer.RenderBodies(bodyFrameWrapper.TrackedBodies, _selectedCamera);

                            if (_recordingStarted)
                            {
                                _recordedBodyFrames.Add(bodyFrameWrapper);
                                if (depthWBitmap != null)
                                {
                                    _depthVideoWriter.WriteVideoFrame(depthBitmap);

                                    //writing colorframes does work but should be deactivated by default
                                    //_colorVideoWriter.WriteVideoFrame(colorBitmap);
                                }
                                else
                                {
                                    Logger.Instance.LogMessage("DepthVideoFrame missing at: " + _recordedBodyFrames.Count.ToString());
                                }
                            }
                        }
                    }
                    else
                    {
                        bodyViewer.DeleteUntrackedBodies(null);
                    }
                }
            }
        }
Ejemplo n.º 55
0
        void BodyFrameReady(BodyFrame frame)
        {
            frame.GetAndRefreshBodyData(m_bodies);

            // now update the player-index-to-body mapping
            for (int i = 0; i < PlayerCount; i++) {
                UpdatePlayerMapping(i);
            }

            for (int i = 0; i < PlayerCount; i++) {
                if (m_eventSinkArray[i] != null)
                {
                    // Update the bodies with the latest pose state.  This may fire events and state machine actions.
                    m_holofunkBodies[i].Update(this,
                        m_eventSinkArray[i].OnLeftHand,
                        m_eventSinkArray[i].OnLeftArm,
                        m_eventSinkArray[i].OnRightHand,
                        m_eventSinkArray[i].OnRightArm);
                }
            }

            if (m_bodyFrameUpdateAction != null) {
                m_bodyFrameUpdateAction(this);
            }
        }
Ejemplo n.º 56
0
    // Update is called once per frame
    void Update()
    {
        // process bodies
        bool newBodyData = false;

        using (BodyFrame bodyFrame = this.bodyFrameReader.AcquireLatestFrame())
        {
            if (bodyFrame != null)
            {
                bodyFrame.GetAndRefreshBodyData(this.bodies);
                newBodyData = true;
            }
        }

        if (newBodyData)
        {
            // update gesture detectors with the correct tracking id
            minposition_z = 8;

            for (int bodyIndex = 0; bodyIndex < this.bodyCount; bodyIndex++)
            {
                var body = this.bodies[bodyIndex];

                if (body != null)
                {
                    //var trackingId = this.bodies[choosen].TrackingId;
                    var trackingId = body.TrackingId;
                    //print("trackingID is " + trackingId);
                    if (body.IsTracked)
                    {
                        //print("bodyindex is " + bodyIndex);
                        //print("trackingidold! " + head_Zindex[bodyIndex, 1] + " BODYINDEX " + bodyIndex);
                    }
                    //print("min" + minposition_z);
                    //print("true or false : " + this.gestureDetectorList[bodyIndex].IsPaused);

                    if (trackingId != this.gestureDetectorList[bodyIndex].TrackingId)
                    {
                        print("different!!! so please change");
                        this.gestureDetectorList[bodyIndex].TrackingId = trackingId;

                        // if the current body is tracked, unpause its detector to get VisualGestureBuilderFrameArrived events
                        // if the current body is not tracked, pause its detector so we don't waste resources trying to get invalid gesture results
                        this.gestureDetectorList[bodyIndex].IsPaused = (trackingId == 0);
                        //this.gestureDetectorList[bodyIndex].OnGestureDetected += CreateOnGestureHandler(bodyIndex);
                    }


                    // if the current body TrackingId changed, update the corresponding gesture detector with the new value
                    if (trackingId == 0)
                    {
                        // this bodyIndex isn't tracked
                        head_Zindex[bodyIndex, 0] = bodyIndex;
                        head_Zindex[bodyIndex, 1] = 0;

                        bodyindex_id[bodyIndex, 0] = bodyIndex;
                        bodyindex_id[bodyIndex, 1] = 0;
                    }
                    else
                    //already tracked!!
                    {
                        print("tracked " + bodyIndex);
                        CameraSpacePoint position_head = body.Joints[JointType.Head].Position;
                        head_Zindex[bodyIndex, 0] = bodyIndex;
                        head_Zindex[bodyIndex, 1] = position_head.Z;

                        bodyindex_id[bodyIndex, 0] = bodyIndex;
                        bodyindex_id[bodyIndex, 1] = (float)trackingId;
                    }
                }
            }

            for (int bodyIndex = 0; bodyIndex < this.bodyCount; bodyIndex++)
            {
                if (minposition_z > head_Zindex[bodyIndex, 1] && head_Zindex[bodyIndex, 1] != 0)
                {
                    choosen       = bodyIndex;
                    minposition_z = head_Zindex[bodyIndex, 1];
                }
            }

            for (int bodyIndex = 0; bodyIndex < this.bodyCount; bodyIndex++)
            {
                if (bodyIndex != choosen)
                {
                    this.gestureDetectorList[bodyIndex].IsPaused = true;
                }

                else
                {
                    this.gestureDetectorList[bodyIndex].IsPaused = false;
                }
            }

            //print("choosen us + " + choosen);
            if (this.bodies[choosen].IsTracked)
            {
                CameraSpacePoint right_hand = this.bodies[choosen].Joints[JointType.HandRight].Position;
                CameraSpacePoint left_hand  = this.bodies[choosen].Joints[JointType.HandLeft].Position;

                double x1 = Math.Round(right_hand.X, 3);
                double y1 = Math.Round(right_hand.Y, 3);
                double z1 = Math.Round(right_hand.Z, 3);


                //print("choosen is " + choosen);
                //print("right hand position: X" + x1 + " y" + y1);

                G2scripts.positionX = x1;
                G2scripts.positionY = y1;
                G2scripts.positionZ = z1;
                if (z1 <= 1.8 && z1 > 1.6)
                {
                    if (y1 <= -0.65 && y1 > -0.83)
                    {
                        if (x1 <= -0.9 && x1 > -1.2)
                        {
                            print("A");
                            G2scripts.ChoosenBall = 'A';
                            //G2scriptsscript.ChoosenBall = 'C';
                            // G2scripts.positionX = x1;
                            G2scripts.A = true;
                            G2scripts.B = false;
                            G2scripts.C = false;
                            G2scripts.D = false;
                            G2scripts.E = false;
                        }

                        else if (x1 <= -0.43 && x1 > -0.56)
                        {
                            print("B");
                            G2scripts.ChoosenBall = 'B';
                            //G2scriptsscript.ChoosenBall = 'C';
                            // G2scripts.positionX = x1;
                            G2scripts.A = false;
                            G2scripts.B = true;
                            G2scripts.C = false;
                            G2scripts.D = false;
                            G2scripts.E = false;
                        }

                        else if (x1 <= 0.17 && x1 > -0.1)
                        {
                            print("C");
                            G2scripts.ChoosenBall = 'C';
                            //G2scriptsscript.ChoosenBall = 'C';
                            // G2scripts.positionX = x1;
                            G2scripts.A = false;
                            G2scripts.B = false;
                            G2scripts.C = true;
                            G2scripts.D = false;
                            G2scripts.E = false;
                        }


                        else if (x1 > 0.53 && x1 <= 0.75)
                        {
                            print("D");
                            G2scripts.ChoosenBall = 'D';
                            // G2scripts.positionX = x1;
                            G2scripts.A = false;
                            G2scripts.B = false;
                            G2scripts.C = false;
                            G2scripts.D = true;
                            G2scripts.E = false;
                        }

                        else if (x1 <= 1.25 && x1 > 1.10)
                        {
                            print("E");
                            G2scripts.ChoosenBall = 'E';
                            //G2scripts.positionX = x1;
                            G2scripts.A = false;
                            G2scripts.B = false;
                            G2scripts.C = false;
                            G2scripts.D = false;
                            G2scripts.E = true;
                        }
                        else
                        {
                            G2scripts.A = false;
                            G2scripts.B = false;
                            G2scripts.C = false;
                            G2scripts.D = false;
                            G2scripts.E = false;
                            print("NON");
                        }
                    }
                }
                else
                {
                    G2scripts.A = false;
                    G2scripts.B = false;
                    G2scripts.C = false;
                    G2scripts.D = false;
                    G2scripts.E = false;
                    print("NON");
                }
            }
        }

        /*
         *   if (trackingId != this.gestureDetectorList[bodyIndex].TrackingId)
         *      {
         *          GestureTextGameObject.text = "none";
         *          //this.bodyText[bodyIndex] = "none";
         *          this.gestureDetectorList[bodyIndex].TrackingId = trackingId;
         *
         *          // if the current body is tracked, unpause its detector to get VisualGestureBuilderFrameArrived events
         *          // if the current body is not tracked, pause its detector so we don't waste resources trying to get invalid gesture results
         *          this.gestureDetectorList[bodyIndex].IsPaused = (trackingId == 0);
         *          this.gestureDetectorList[bodyIndex].OnGestureDetected += CreateOnGestureHandler(bodyIndex);
         *      }
         *  }
         */
        /*
         *
         * if(body.IsTracked)
         * {
         *  CameraSpacePoint position_head = body.Joints[JointType.Head].Position;
         *  print("tracked" + bodyIndex);
         *  //print("gesturedetector.body is " + gestureDetectorList[bodyIndex].TrackingId);
         *  //print("position_head" + position_head.Z);
         * }
         *
         */
    }
Ejemplo n.º 57
0
        private void ShowBodyJoints(BodyFrame bodyFrame)
        {
            Body[] bodies = new Body[this.kinectSensor.BodyFrameSource.BodyCount];
            bool dataReceived = false;
            if (bodyFrame != null)
            {
                bodyFrame.GetAndRefreshBodyData(bodies);
                dataReceived = true;
            }

            if (dataReceived)
            {
                this.bodiesManager.UpdateBodiesAndEdges(bodies);
            }
        }
Ejemplo n.º 58
0
        private void RegisterGesture(BodyFrame bodyFrame)
        {
            bool dataReceived = false;
            Body[] bodies = null;
            if (bodyFrame != null)
            {
                if (bodies == null)
                {
                    bodies = new Body[bodyFrame.BodyCount];
                }
                bodyFrame.GetAndRefreshBodyData(bodies);
                dataReceived = true;
            }
            if (dataReceived)
            {
                if (bodies != null)
                {
                    for (int i = 0; i < bodyFrame.BodyCount; i++)
                    {
                        Body body = bodies[i];
                        ulong trackingId = body.TrackingId;
                        if (trackingId != gestureDetectorList[i].TrackingId)
                        {
                            gestureDetectorList[i].TrackingId = trackingId;
                            gestureDetectorList[i].IsPaused = trackingId == 0;
                        }
                    }
                }
            }

        }