List <Microsoft.Research.Kinect.Nui.Vector> convertRealDepth()
        {
            List <Microsoft.Research.Kinect.Nui.Vector> ret = new List <Microsoft.Research.Kinect.Nui.Vector>();

            Console.WriteLine("Pts: {0}", depthPixels.Length);
            for (int i = 0; i < depthPixels.Length; i += 2)
            {
                int depthPixel = depthPixels[i / 2];

                // The x and y positions can be calculated using modulus
                // division from the array index

                int x = (i / 2) % depthWidth;
                int y = (i / 2) / depthWidth;

                // The x and y we pass into DepthImageToSkeleton() need to
                // be normalised (between 0 and 1), so we divide by the
                // width and height of the depth image, respectively

                // As we're using UseDepth (not UseDepthAndPlayerIndex) in
                // the depth sensor settings, we also need to shift the
                // depth pixel by 3 bits

                Microsoft.Research.Kinect.Nui.Vector v = _kinectRuntime.SkeletonEngine.DepthImageToSkeleton(
                    ((float)x) / ((float)depthWidth),
                    ((float)y) / ((float)depthHeight),
                    (short)(depthPixel << 3)
                    );
                if (v.Z > 0)
                {
                    ret.Add(v);
                }
            }
            return(ret);
        }
示例#2
0
        public virtual void Add(Vector position, SkeletonEngine engine)
        {
            Entry newEntry = new Entry {
                Position = position.ToVector3(), Time = DateTime.Now
            };

            Entries.Add(newEntry);

            if (displayCanvas != null)
            {
                newEntry.DisplayEllipse = new Ellipse
                {
                    Width  = 4,
                    Height = 4,
                    HorizontalAlignment = HorizontalAlignment.Left,
                    VerticalAlignment   = VerticalAlignment.Top,
                    StrokeThickness     = 2.0,
                    Stroke         = new SolidColorBrush(displayColor),
                    StrokeLineJoin = PenLineJoin.Round
                };


                float x, y;

                engine.SkeletonToDepthImage(position, out x, out y);

                x = (float)(x * displayCanvas.ActualWidth);
                y = (float)(y * displayCanvas.ActualHeight);

                Canvas.SetLeft(newEntry.DisplayEllipse, x - newEntry.DisplayEllipse.Width / 2);
                Canvas.SetTop(newEntry.DisplayEllipse, y - newEntry.DisplayEllipse.Height / 2);

                displayCanvas.Children.Add(newEntry.DisplayEllipse);
            }

            if (Entries.Count > WindowSize)
            {
                Entry entryToRemove = Entries[0];

                if (displayCanvas != null)
                {
                    displayCanvas.Children.Remove(entryToRemove.DisplayEllipse);
                }

                Entries.Remove(entryToRemove);
            }

            LookForGesture();
        }
        //This method is used to position the ellipses on the canvas
        //according to correct movements of the tracked joints.

        //IMPORTANT NOTE: Code for vector scaling was imported from the Coding4Fun Kinect Toolkit
        //available here: http://c4fkinect.codeplex.com/
        //I only used this part to avoid adding an extra reference.
        private void SetEllipsePosition(Ellipse ellipse, Joint joint)
        {
            Microsoft.Research.Kinect.Nui.Vector vector = new Microsoft.Research.Kinect.Nui.Vector();
            vector.X = ScaleVector(640, joint.Position.X);
            vector.Y = ScaleVector(480, -joint.Position.Y);
            vector.Z = joint.Position.Z;

            Joint updatedJoint = new Joint();

            updatedJoint.ID            = joint.ID;
            updatedJoint.TrackingState = JointTrackingState.Tracked;
            updatedJoint.Position      = vector;

            Canvas.SetLeft(ellipse, updatedJoint.Position.X);
            Canvas.SetTop(ellipse, updatedJoint.Position.Y);
        }
        private Point getDisplayPosition(Microsoft.Research.Kinect.Nui.Vector mainPoint)
        {
            float depthX, depthY;

            nui.SkeletonEngine.SkeletonToDepthImage(mainPoint, out depthX, out depthY);
            // Convert to 320, 240 space
            depthX = depthX * 320; // MICHI: WATCH OUT HERE!!!! this are the depth X/Y coordinates first in the 0-1 interval
            depthY = depthY * 240;
            int           colorX, colorY;
            ImageViewArea iv = new ImageViewArea();

            // Only ImageResolution.Resolution640x480 is supported at this point
            nui.NuiCamera.GetColorPixelCoordinatesFromDepthPixel(ImageResolution.Resolution640x480, iv, (int)depthX, (int)depthY, (short)0, out colorX, out colorY);

            // Map back to skeletonCanvas.Width & skeletonCanvas.Height
            return(new Point((int)(skeletonCanvas.Width * colorX / 640.0), (int)(skeletonCanvas.Height * colorY / 480)));
        }
        public virtual void Add(Vector position, SkeletonEngine engine)
        {
            Entry newEntry = new Entry {Position = position.ToVector3(), Time = DateTime.Now};
            Entries.Add(newEntry);

            if (displayCanvas != null)
            {
                newEntry.DisplayEllipse = new Ellipse
                {
                    Width = 4,
                    Height = 4,
                    HorizontalAlignment = HorizontalAlignment.Left,
                    VerticalAlignment = VerticalAlignment.Top,
                    StrokeThickness = 2.0,
                    Stroke = new SolidColorBrush(displayColor),
                    StrokeLineJoin = PenLineJoin.Round
                };


                float x, y;

                engine.SkeletonToDepthImage(position, out x, out y);

                x = (float)(x * displayCanvas.ActualWidth);
                y = (float)(y * displayCanvas.ActualHeight);

                Canvas.SetLeft(newEntry.DisplayEllipse, x - newEntry.DisplayEllipse.Width / 2);
                Canvas.SetTop(newEntry.DisplayEllipse, y - newEntry.DisplayEllipse.Height / 2);

                displayCanvas.Children.Add(newEntry.DisplayEllipse);
            }

            if (Entries.Count > WindowSize)
            {
                Entry entryToRemove = Entries[0];
                
                if (displayCanvas != null)
                {
                    displayCanvas.Children.Remove(entryToRemove.DisplayEllipse);
                }

                Entries.Remove(entryToRemove);
            }

            LookForGesture();
        }
示例#6
0
        public static Gesture guessGuesture(Microsoft.Research.Kinect.Nui.Vector v, Joint source, float xThreshold, float yThreshold, float zThreshold)
        {
            //double max = Math.Max(Math.Abs(v.X), Math.Max(Math.Abs(v.Y), Math.Abs(v.Z)));
            //if (max < 0.17)
            //{
            //    return new Gesture(DateTime.Now, 0, GestureID.Invalid, source);
            //}

            //if (max == Math.Abs(v.X) && max > xThreshold
            //   && ((previousGuesture == GestureID.SwipeLeft || previousGuesture == GestureID.SwipeRight || previousGuesture == GestureID.Invalid)
            //    || DateTime.Now.Subtract(timer).TotalSeconds > 0.1))
            //{
            //    timer = DateTime.Now;
            //    return (v.X > 0.0) ? new Gesture(DateTime.Now, max, GestureID.SwipeRight, source) :
            //          new Gesture(DateTime.Now, max, GestureID.SwipeLeft, source);
            //}
            //if (DateTime.Now.Subtract(timer).TotalSeconds > 0.1)
            //{
            //    if (max == Math.Abs(v.Y) && max > yThreshold)
            //    {
            //        timer = DateTime.Now;
            //        return (v.Y > 0.0) ? new Gesture(DateTime.Now, max, GestureID.SwipeUp, source) :
            //             new Gesture(DateTime.Now, max, GestureID.SwipeDown, source);
            //    }
            //    else if (max == Math.Abs(v.Z) && max > zThreshold)
            //    {
            //        timer = DateTime.Now;
            //        return (v.Z > 0.0) ? new Gesture(DateTime.Now, max, GestureID.Pull, source) :
            //             new Gesture(DateTime.Now, max, GestureID.Push, source);
            //    }

            //}
            if (DateTime.Now.Subtract(timer).TotalSeconds > 0.1)
            {
                if (Math.Abs(v.Z) > zThreshold)
                {
                    timer = DateTime.Now;
                    return(new Gesture(DateTime.Now, Math.Abs(v.Z), v.Z > 0 ? GestureID.Pull : GestureID.Push, source));
                }
            }

            return(new Gesture(DateTime.Now, 0, GestureID.Invalid, source));
        }
示例#7
0
        //這個方法會傳入代表某個點的控制項,還有該點相對於Kinect擷取到的座標
        private void SetFrameworkElementPosition(FrameworkElement frameworkElement, Joint joint)
        {
            Microsoft.Research.Kinect.Nui.Vector vector = new Microsoft.Research.Kinect.Nui.Vector();
            //下面的兩行為透過呼叫ScaleVector方法,將Kinect的座標系換算回螢幕座標系
            vector.X = ScaleVector(640, joint.Position.X);
            //別忘了,Y座標軸往上是負的,所以這邊要變負數才行
            vector.Y = ScaleVector(480, -joint.Position.Y);
            vector.Z = joint.Position.Z;

            //宣告一個新的Joint物件來存放重新計算過的座標
            Joint updatedJoint = new Joint();

            updatedJoint.ID            = joint.ID;
            updatedJoint.TrackingState = JointTrackingState.Tracked;
            updatedJoint.Position      = vector;

            //重新設定控制項的座標位置,後面我自己加上了加回控制項大小的程式碼,以減少誤差
            Canvas.SetLeft(frameworkElement, updatedJoint.Position.X + frameworkElement.ActualWidth);
            Canvas.SetTop(frameworkElement, updatedJoint.Position.Y + frameworkElement.ActualHeight);
        }
示例#8
0
        public static Gesture guessGuesture(Microsoft.Research.Kinect.Nui.Vector v, Joint source, float xThreshold, float yThreshold, float zThreshold)
        {
            double magnitude = Math.Sqrt(v.X * v.X + v.Y * v.Y + v.Z * v.Z);
            double max       = Math.Max(Math.Abs(v.X), Math.Max(Math.Abs(v.Y), Math.Abs(v.Z)));

            if (max < 0.17f)
            {
                return(new Gesture(DateTime.Now, 0, GestureID.Invalid, source));
            }

            if (Math.Abs(v.Z) * 2 >= magnitude && v.Z > zThreshold)
            {
                timer = DateTime.Now;
                return(new Gesture(DateTime.Now, Math.Abs(v.Z), v.Z > 0 ? GestureID.Pull : GestureID.Push, source));
            }
            if (max == Math.Abs(v.X) && max > xThreshold &&
                ((previousGuesture == GestureID.SwipeLeft || previousGuesture == GestureID.SwipeRight || previousGuesture == GestureID.Invalid) ||
                 DateTime.Now.Subtract(timer).TotalSeconds > 0.1))
            {
                timer = DateTime.Now;
                return((v.X > 0.0) ? new Gesture(DateTime.Now, max, GestureID.SwipeRight, source) :
                       new Gesture(DateTime.Now, max, GestureID.SwipeLeft, source));
            }
            if (DateTime.Now.Subtract(timer).TotalSeconds > 0.1)
            {
                if (max == Math.Abs(v.Y) && max > yThreshold && Math.Abs(v.Y) * 1.5 >= magnitude)
                {
                    timer = DateTime.Now;
                    return((v.Y > 0.0) ? new Gesture(DateTime.Now, max, GestureID.SwipeUp, source) :
                           new Gesture(DateTime.Now, max, GestureID.SwipeDown, source));
                }
                else if (max == Math.Abs(v.Z) && max > zThreshold)
                {
                    timer = DateTime.Now;
                    return((v.Z > 0.0) ? new Gesture(DateTime.Now, max, GestureID.Pull, source) :
                           new Gesture(DateTime.Now, max, GestureID.Push, source));
                }
            }

            return(new Gesture(DateTime.Now, 0, GestureID.Invalid, source));
        }
示例#9
0
 private void SmoothenBody(Body currentBody)
 {
     if (_smoothBody == null)
     {
         _smoothBody = currentBody;
     }
     else
     {
         Vector head = _smoothBody.Head;
         head.X           = (float)(_smoothBody.Head.X + _dampeningFactor * (currentBody.Head.X - _smoothBody.Head.X));
         head.Y           = (float)(_smoothBody.Head.Y + _dampeningFactor * (currentBody.Head.Y - _smoothBody.Head.Y));
         _smoothBody.Head = head;
         Vector left = _smoothBody.LeftHand;
         left.X = (float)(_smoothBody.LeftHand.X + _dampeningFactor * (currentBody.LeftHand.X - _smoothBody.LeftHand.X));
         left.Y = (float)(_smoothBody.LeftHand.Y + _dampeningFactor * (currentBody.LeftHand.Y - _smoothBody.LeftHand.Y));
         _smoothBody.LeftHand = left;
         Vector right = _smoothBody.RightHand;
         right.X = (float)(_smoothBody.RightHand.X + _dampeningFactor * (currentBody.RightHand.X - _smoothBody.RightHand.X));
         right.Y = (float)(_smoothBody.RightHand.Y + _dampeningFactor * (currentBody.RightHand.Y - _smoothBody.RightHand.Y));
         _smoothBody.RightHand = right;
     }
 }
示例#10
0
        public static Gesture guessGuesture(Microsoft.Research.Kinect.Nui.Vector v, Joint source, float xThreshold, float yThreshold, float zThreshold)
        {
            double max = Math.Max(Math.Abs(v.X), Math.Max(Math.Abs(v.Y), Math.Abs(v.Z)));

            if (max < 0.0)
            {
                return(new Gesture(DateTime.Now, 0, GestureID.Invalid, source));
            }

            else if (Math.Sqrt(v.X * v.X + v.Y * v.Y) <= 0.031)// Math.Abs(v.Z - ((v.X + v.Y) )) <= 0.1 )
            {
                return(new Gesture(DateTime.Now, max, GestureID.Still, source));
            }

            else if ((Math.Abs(v.Z) > 2 * Math.Abs(v.X)) && (Math.Abs(v.Z) > 2 * Math.Abs(v.Y)))
            {
                return(new Gesture(DateTime.Now, max, (v.Z > 0) ? GestureID.Pull : GestureID.Push, source));
            }

            else
            {
                return(new Gesture(DateTime.Now, max, GestureID.Planar, source));
            }
        }
示例#11
0
        public static Vector2 ToVector2(Microsoft.Research.Kinect.Nui.Vector p, Vector2 scale)
        {
            var half = scale / 2.0f;

            return((new Vector2(p.X, p.Y) * half * new Vector2(1, -1)) + half);
        }
示例#12
0
        static void RuntimeSkeletonFrameReady(object sender, SkeletonFrameReadyEventArgs e)
        {
            var skeleton = e.SkeletonFrame.Skeletons
                .Where(s => s.TrackingState == SkeletonTrackingState.Tracked)
                .FirstOrDefault();

            if (skeleton != null)
            {
                Status = KinectStatus.TrackingUser;
                leftHandJoint = skeleton.Joints[JointID.HandLeft];
                LeftHand = leftHandJoint.Position;
                rightHandJoint = skeleton.Joints[JointID.HandRight];
                RightHand = rightHandJoint.Position;
                Torso = skeleton.Joints[JointID.Spine].Position;
            }
            else
            {
                Status = KinectStatus.WaitingToAcquire;
            }
        }
        private void Window_Loaded(object sender, EventArgs e)
        {
            nui = new Runtime();

            try
            {
                nui.Initialize(RuntimeOptions.UseDepthAndPlayerIndex | RuntimeOptions.UseSkeletalTracking | RuntimeOptions.UseColor);
            }
            catch (InvalidOperationException)
            {
                System.Windows.MessageBox.Show("Runtime initialization failed. Please make sure Kinect device is plugged in.");
                return;
            }

            try
            {
                nui.VideoStream.Open(ImageStreamType.Video, 2, ImageResolution.Resolution640x480, ImageType.Color);
                nui.DepthStream.Open(ImageStreamType.Depth, 2, ImageResolution.Resolution320x240, ImageType.DepthAndPlayerIndex);
            }
            catch (InvalidOperationException)
            {
                System.Windows.MessageBox.Show("Failed to open stream. Please make sure to specify a supported image type and resolution.");
                return;
            }

            lastTime = DateTime.Now;

            nui.SkeletonFrameReady += new EventHandler<SkeletonFrameReadyEventArgs>(nui_SkeletonFrameReady);
            nui.VideoFrameReady += new EventHandler<ImageFrameReadyEventArgs>(nui_ColorFrameReady);

            SkeletonDataAlternative.JointPositionConverter = new JointPositionConverter(delegate(Joint joint)
            {
                float depthX, depthY;
                nui.SkeletonEngine.SkeletonToDepthImage(joint.Position, out depthX, out depthY);
                ImageViewArea iv = new ImageViewArea();
                int colorX, colorY;
                nui.NuiCamera.GetColorPixelCoordinatesFromDepthPixel(ImageResolution.Resolution640x480, iv, (int)(depthX * 320), (int)(depthY * 240), (short)0, out colorX, out colorY);
                Microsoft.Research.Kinect.Nui.Vector converted = new Microsoft.Research.Kinect.Nui.Vector();
                converted.X = (float)colorX / 640.0f;
                converted.Y = (float)colorY / 480.0f;
                converted.Z = joint.Position.Z;
                converted.W = joint.Position.W;
                joint.Position = converted;
                return joint;
            });
        }
示例#14
0
        void nui_SkeletonFrameReady(object sender, SkeletonFrameReadyEventArgs e)
        {
            SkeletonFrame allSkeletons = e.SkeletonFrame;

            //get the first tracked skeleton
            SkeletonData skeleton = (from s in allSkeletons.Skeletons
                                     where s.TrackingState == SkeletonTrackingState.Tracked
                                     select s).FirstOrDefault();
            skeletonReady = true;
            playerCenter = skeleton.Position;

            if (Math.Abs(playerCenter.X - skellyX) > 0.05)
            {
                string dataString = playerCenter.X.ToString();
                Byte[] sendBytes = Encoding.ASCII.GetBytes(dataString);
                myClient.Send(sendBytes, sendBytes.Length); //virtual ball pos sent to server
                skellyX = playerCenter.X;
            }
            //textBlock2.Text ="W: " + e.SkeletonFrame.FloorClipPlane.W.ToString() + "\tX: " + e.SkeletonFrame.FloorClipPlane.X.ToString() + "\tY: " + e.SkeletonFrame.FloorClipPlane.X.ToString() + "\tZ: " + e.SkeletonFrame.FloorClipPlane.X.ToString();
            //textBlock2.Text = "X: " + skeleton.Joints[JointID.HandLeft].Position.X.ToString() + "\tY: " + skeleton.Joints[JointID.HandLeft].Position.Y.ToString() + "\tZ: " + skeleton.Joints[JointID.HandLeft].Position.Z.ToString();
        }
示例#15
0
        // skeleton handler
        void nui_SkeletonFrameReady(object sender, SkeletonFrameReadyEventArgs e)
        {
            SkeletonFrame skeletonFrame = e.SkeletonFrame;

            lastSkeletonTime = DateTime.Now;

            uint absoluteTime = (uint)(lastSkeletonTime.Ticks / TimeSpan.TicksPerMillisecond); // 32-bit uint

            log.Text = "";

            skeleton.Children.Clear();

            byte skeletonNum = 0;

            using (MemoryStream skeletonStream = new MemoryStream())
            {
                using (BinaryWriter skeletonWriter = new BinaryWriter(skeletonStream))
                {
                    foreach (SkeletonData data in skeletonFrame.Skeletons)
                    {
                        if (SkeletonTrackingState.Tracked == data.TrackingState)
                        {
                            // skeleton id write to buffer

                            skeletonWriter.Write(KinectConst.MS_DATA);
                            skeletonWriter.Write(KinectConst.OUT_SKELETON);
                            skeletonWriter.Write(KinectConst.SKELETON_BUFFER_LENGTH);
                            skeletonWriter.Write(data.TrackingID);

                            // Draw bones
                            Brush brush = brushes[skeletonNum % brushes.Length];
                            skeleton.Children.Add(getBodySegment(data.Joints, brush, JointID.HipCenter, JointID.Spine, JointID.ShoulderCenter, JointID.Head));
                            skeleton.Children.Add(getBodySegment(data.Joints, brush, JointID.ShoulderCenter, JointID.ShoulderLeft, JointID.ElbowLeft, JointID.WristLeft, JointID.HandLeft));
                            skeleton.Children.Add(getBodySegment(data.Joints, brush, JointID.ShoulderCenter, JointID.ShoulderRight, JointID.ElbowRight, JointID.WristRight, JointID.HandRight));
                            skeleton.Children.Add(getBodySegment(data.Joints, brush, JointID.HipCenter, JointID.HipLeft, JointID.KneeLeft, JointID.AnkleLeft, JointID.FootLeft));
                            skeleton.Children.Add(getBodySegment(data.Joints, brush, JointID.HipCenter, JointID.HipRight, JointID.KneeRight, JointID.AnkleRight, JointID.FootRight));

                            // Draw and write to buffer joints

                            foreach (Joint joint in data.Joints)
                            {
                                Point   jointPos   = getDisplayPosition(joint);
                                Ellipse jointPoint = new Ellipse();
                                jointPoint.Width  = 6;
                                jointPoint.Height = 6;
                                jointPoint.HorizontalAlignment = HorizontalAlignment.Center;
                                jointPoint.VerticalAlignment   = VerticalAlignment.Center;
                                jointPoint.Fill   = jointColors[joint.ID];
                                jointPoint.Margin = new Thickness(jointPos.X - 3, jointPos.Y - 3, 0, 0);
                                skeleton.Children.Add(jointPoint);

                                // write to buffer

                                skeletonWriter.Write(joint.Position.X);
                                skeletonWriter.Write(joint.Position.Y);
                                skeletonWriter.Write(joint.Position.Z);
                            }

                            skeletonWriter.Write(absoluteTime);

                            skeletonNum++;
                            Microsoft.Research.Kinect.Nui.Vector position = data.Joints[JointID.ShoulderCenter].Position;
                            log.AppendText("skeleton " + data.TrackingID + "\t(" + position.X.ToString("000.00") + "\t" + position.Y.ToString("000.00") + "\t" + position.Z.ToString("000.00") + ")\n");
                            position = data.Joints[JointID.HandLeft].Position;
                            log.AppendText("leftHand\t\t(" + position.X.ToString("000.00") + "\t" + position.Y.ToString("000.00") + "\t" + position.Z.ToString("000.00") + ")\n");
                            position = data.Joints[JointID.HandRight].Position;
                            log.AppendText("rightHand\t(" + position.X.ToString("000.00") + "\t" + position.Y.ToString("000.00") + "\t" + position.Z.ToString("000.00") + ")\n");
                            if (sre == null)
                            {
                                log.AppendText("Speech Recognition Platform x86 is not instalted!\n");
                            }
                        }
                    } // for each skeleton

                    log.AppendText("skeletons : " + skeletonNum + "\n");
                    if (lastRecognizedCommand != "")
                    {
                        log.AppendText("cmd : " + lastRecognizedCommand + "\t" + lastRecognizedAngle + "\t" + lastRecognizedConfidence + "\n");
                    }
                    log.AppendText("clients : " + ClientConnection.Count() + "\n");

                    if (skeletonNum > 0 && server != null)
                    {
                        server.Send(skeletonStream.ToArray());
                    }
                }
            }
        }
示例#16
0
        void nui_DepthFrameReady(object sender, ImageFrameReadyEventArgs e)
        {
            int i16=0,x,y;
            long dx = 0, dy = 0;
            double bx=0, by=0, bz=0;
            short depth;
            int count = 0;
            Microsoft.Research.Kinect.Nui.Vector point;
            DateTime cur = DateTime.Now;
            frameSpan = cur.Subtract(lastTime);
            //textBox1.Text += cur.Subtract(lastTime).ToString() + '\n';

            //work on virtual ball
            if (virtualBall)
            {
                xDisplace = xDisplace + (xVel * frameSpan.Milliseconds)/1000;
                yDisplace = yDisplace + (yVel * frameSpan.Milliseconds)/1000;
                zDisplace = zDisplace + (zVel * frameSpan.Milliseconds)/1000;
                //send to server here!!
                string dataString = (xDisplace+0.3f).ToString() + " " + (yDisplace-0.1f).ToString() + " " + zDisplace.ToString();
                Byte[] sendBytes = Encoding.ASCII.GetBytes(dataString);
                myClient.Send(sendBytes, sendBytes.Length); //virtual ball pos sent to server
                virtualBallsSent++;
                textBlock10.Text = "Virtual balls sent: " + virtualBallsSent.ToString();

                textBlock9.Text = xDisplace.ToString() + " " + yDisplace.ToString() + " " + zDisplace.ToString();
                if(yDisplace < -1.5 || Math.Abs(xDisplace) > 2 || zDisplace > 8) //out of play bounds
                    virtualBall = false;
                return;
            }

            //process current depth frame
            PlanarImage depthData = e.ImageFrame.Image;
            for (y=0;y<depthData.Height;y++)
            {
                for(x=0;x<depthData.Width;x++)
                {
                    if ((depthData.Bits[i16] & 0x07) != 0)
                    {
                        depthData.Bits[i16 + 1] = (byte)0;
                        depthData.Bits[i16] = (byte)0;
                        i16 += 2;
                        continue;
                    }
                    depth = (short)((depthData.Bits[i16+1]<<8) | depthData.Bits[i16]);
                    point = nui.SkeletonEngine.DepthImageToSkeleton(((float)x) / ((float)depthData.Width), ((float)y) / ((float)depthData.Height),depth);
                    if (point.Z < 1 || point.Z > 3.8 || point.Y < -1 || point.Y > 1.2)
                    {
                        //not ball
                        depthData.Bits[i16 + 1] = (byte)0;
                        depthData.Bits[i16] = (byte)0;
                    }
                    else
                    {
                        //ball
                        count++;
                        dx += x;
                        dy += y;
                        bx += point.X;
                        by += point.Y;
                        bz += point.Z;
                        depthData.Bits[i16 + 1] = (byte)60;
                        depthData.Bits[i16] = (byte)60;
                    }
                    i16 += 2;
                }
            }
            if (count > 80 && count < 4000)
            {
                //ball detected in frame
                ballframes++;
                dx /= count;
                dy /= count;
                bx /= count;
                by /= count;
                bz /= count;
                i16 = (int)dy * depthData.Width * depthData.BytesPerPixel + (int)dx * depthData.BytesPerPixel;
                depthData.Bits[i16] = (byte)255;
                depthData.Bits[i16 + 1] = (byte)255;
                textBlock1.Text = "X: " + bx.ToString();
                textBlock2.Text = "Y: " + by.ToString();
                textBlock3.Text = "Z: " + bz.ToString();
                textBlock4.Text = "FrameNumber: " + e.ImageFrame.FrameNumber.ToString();
                textBlock5.Text = "Ball Frames: " + ballframes.ToString();
                //textBox1.Text += "FrameNumber: " + e.ImageFrame.FrameNumber.ToString() + " X: " + bx.ToString() + " Y: " + by.ToString() + " Z: " + bz.ToString() + " T: " + frameSpan.ToString() + '\n';
                lastTime = cur;
                Microsoft.Research.Kinect.Nui.Vector tempPoint = new Microsoft.Research.Kinect.Nui.Vector();
                tempPoint.X = (float)bx;
                tempPoint.Y = (float)by;
                tempPoint.Z = (float)bz;
                if (listSize == 0) //first in sequence
                {
                    pointList.Add(tempPoint);
                    numList.Add(e.ImageFrame.FrameNumber);
                    timeList.Add(frameSpan);
                    listSize++;
                }
                else //not first
                {
                    if ((e.ImageFrame.FrameNumber != (numList[listSize - 1] + 2)) || ((tempPoint.Z - 0.05f) < pointList[listSize - 1].Z)) //end of sequence
                    {
                        if (listSize > 2)
                        {
                            textBox1.Text += "Last " + listSize.ToString() + " records are forward movement\n";
                            forwardCount++;
                            textBlock7.Text = forwardCount.ToString();

                            Microsoft.Research.Kinect.Nui.Vector sPoint = pointList[listSize - 3];
                            Microsoft.Research.Kinect.Nui.Vector ePoint = pointList[listSize - 1];
                            //do tracking schtuff here
                            xDisplace = ePoint.X - sPoint.X;
                            yDisplace = ePoint.Y - sPoint.Y;
                            zDisplace = ePoint.Z - sPoint.Z;
                            TimeSpan displaceTime = timeList[listSize - 1] + timeList[listSize - 2];
                            xVel = (xDisplace / displaceTime.Milliseconds) * 1000;
                            yVel = (yDisplace / displaceTime.Milliseconds) * 1000;
                            zVel = (zDisplace / displaceTime.Milliseconds) * 1000;

                            virtualBall = true;
                        }
                        pointList.Clear();
                        numList.Clear();
                        timeList.Clear();
                        listSize = 0;
                    }
                    else //still moving forward!
                    {
                        //float[] fltArr = {tempPoint.X,tempPoint.Y,tempPoint.Z};
                        //Byte[] sendBytes = ToByteArray(fltArr);
                        string dataString = (xDisplace + 0.3f).ToString() + " " + (yDisplace - 0.1f).ToString() + " " + zDisplace.ToString();
                        Byte[] sendBytes = Encoding.ASCII.GetBytes(dataString);
                        myClient.Send(sendBytes, sendBytes.Length); //real ball pos sent to server

                        realBallsSent++;
                        textBlock8.Text = "Real Balls Sent: " + realBallsSent.ToString();
                        pointList.Add(tempPoint);
                        numList.Add(e.ImageFrame.FrameNumber);
                        timeList.Add(frameSpan);
                        listSize++;
                    }
                }
            }
            else //not a ball detected in this frame
            {
                if (listSize == 0) //first in sequence
                {

                }
                else //not first
                {
                    if (listSize > 2)
                    {
                        textBox1.Text += "Last " + listSize.ToString() + " records are forward movement\n";
                        forwardCount++;
                        textBlock7.Text = forwardCount.ToString();

                        Microsoft.Research.Kinect.Nui.Vector sPoint = pointList[listSize - 3];
                        Microsoft.Research.Kinect.Nui.Vector ePoint = pointList[listSize - 1];
                        //do tracking schtuff here
                        xDisplace = ePoint.X - sPoint.X;
                        yDisplace = ePoint.Y - sPoint.Y;
                        zDisplace = ePoint.Z - sPoint.Z;
                        TimeSpan displaceTime = timeList[listSize - 1] + timeList[listSize - 2];
                        xVel = (xDisplace / displaceTime.Milliseconds)*1000;
                        yVel = (yDisplace / displaceTime.Milliseconds)*1000;
                        zVel = (zDisplace / displaceTime.Milliseconds)*1000;
                        virtualBall = true;
                    }
                    pointList.Clear();
                    numList.Clear();
                    timeList.Clear();
                    listSize = 0;
                }
            }
            ++totalFrames;
            //timeList.Add(cur.Subtract(lastTime));
            /*
            if (cur.Subtract(lastTime) > TimeSpan.FromSeconds(1))
            {
                int frameDiff = totalFrames - lastFrames;
                lastFrames = totalFrames;
                lastTime = cur;
                textBlock6.Text = "FPS: " + frameDiff.ToString();
            }*/
            //Use Coding4Fun extension method on ImageFrame class for Depth
            image2.Source = BitmapSource.Create(depthData.Width, depthData.Height, 96, 96, PixelFormats.Gray16, null, depthData.Bits, depthData.Width * depthData.BytesPerPixel);
        }
示例#17
0
 public virtual void Add(Vector position, SkeletonEngine engine)
 {
     currentPosition = new InputData(position.ToVector3(), 0);
     LookForGesture();
 }
        void nui_SkeletonFrameReady(object sender, SkeletonFrameReadyEventArgs e)
        {
            SkeletonFrame skeletonFrame = e.SkeletonFrame;
            int           iSkeleton     = 0;

            Brush[] brushes = new Brush[6];
            brushes[0] = new SolidColorBrush(Color.FromRgb(255, 0, 0));
            brushes[1] = new SolidColorBrush(Color.FromRgb(0, 255, 0));
            brushes[2] = new SolidColorBrush(Color.FromRgb(64, 255, 255));
            brushes[3] = new SolidColorBrush(Color.FromRgb(255, 255, 64));
            brushes[4] = new SolidColorBrush(Color.FromRgb(255, 64, 255));
            brushes[5] = new SolidColorBrush(Color.FromRgb(128, 128, 255));

            skeleton.Children.Clear();
            foreach (SkeletonData data in skeletonFrame.Skeletons)
            {
                if (SkeletonTrackingState.Tracked == data.TrackingState)
                {
                    // Draw bones
                    Brush brush = brushes[iSkeleton % brushes.Length];
                    skeleton.Children.Add(getBodySegment(data.Joints, brush, JointID.HipCenter, JointID.Spine, JointID.ShoulderCenter, JointID.Head));
                    skeleton.Children.Add(getBodySegment(data.Joints, brush, JointID.ShoulderCenter, JointID.ShoulderLeft, JointID.ElbowLeft, JointID.WristLeft, JointID.HandLeft));
                    skeleton.Children.Add(getBodySegment(data.Joints, brush, JointID.ShoulderCenter, JointID.ShoulderRight, JointID.ElbowRight, JointID.WristRight, JointID.HandRight));
                    skeleton.Children.Add(getBodySegment(data.Joints, brush, JointID.HipCenter, JointID.HipLeft, JointID.KneeLeft, JointID.AnkleLeft, JointID.FootLeft));
                    skeleton.Children.Add(getBodySegment(data.Joints, brush, JointID.HipCenter, JointID.HipRight, JointID.KneeRight, JointID.AnkleRight, JointID.FootRight));

                    JointsCollection jc = data.Joints;

                    StringBuilder sb = new StringBuilder();
                    StringBuilder up = new StringBuilder();
                    StringBuilder lo = new StringBuilder();
                    int           k  = 0;
                    foreach (JointID jid in Enum.GetValues(typeof(JointID)))
                    {
                        if (jid != JointID.Count)
                        {
                            k++;
                            Microsoft.Research.Kinect.Nui.Vector v = jc[jid].Position;
                            if (k <= 4)
                            {
                                sb.Append(string.Format("{0} = {1}, {2}, {3}\n", jid, v.X, v.Y, v.Z));
                            }
                            else if (k <= 12)
                            {
                                up.Append(string.Format("{0} = {1}, {2}, {3}\n", jid, v.X, v.Y, v.Z));

                                if (k == 8)//HandLeft
                                {
                                    HandLY = v.Y;
                                    up.Append(string.Format("\n"));
                                }
                                if (k == 9)//ShoulderRight
                                {
                                    ShouRX = v.X;
                                    ShouRY = v.Y;
                                    ShouRZ = v.Z;
                                }
                                if (k == 10)//ElbowRight
                                {
                                    ElRX = v.X;
                                    ElRY = v.Y;
                                    ElRZ = v.Z;
                                }
                                if (k == 11)//WristRight
                                {
                                    WrRX = v.X;
                                    WrRY = v.Y;
                                    WrRZ = v.Z;
                                }
                                if (k == 12)//HandRight
                                {
                                    HandRX = v.X;
                                    HandRY = v.Y;
                                    HandRZ = v.Z;
                                }
                            }
                            else if (k <= 20)
                            {
                                lo.Append(string.Format("{0} = {1}, {2}, {3}\n", jid, v.X, v.Y, v.Z));
                                if (k == 13)//HipLeft
                                {
                                    HipLY = v.Y;
                                }
                                if (k == 16)
                                {
                                    lo.Append(string.Format("\n"));
                                }
                            }
                        }
                    }

                    middle.Text = sb.ToString();
                    upper.Text  = up.ToString();
                    lower.Text  = lo.ToString();

                    //Arduino Control 1st Rotation Angle
                    double angROT1 = Math.Atan((HandRX - ShouRX) / (HandRY - ShouRY));
                    if (angROT1 < 0)
                    {
                        angROT1 = 0.5 * Math.PI - angROT1;
                    }
                    arduino.analogWrite(3, Convert.ToInt32(Math.Floor(angROT1 * 180 / Math.PI)));

                    //Arduino Control 2nd Rotation Angle (Shoulder)
                    double angROT2 = Math.Atan((ShouRZ - ElRZ) / Math.Sqrt(Math.Pow((ElRY - ShouRY), 2) + Math.Pow((ElRX - ShouRX), 2)));
                    if (angROT2 < 0)
                    {
                        angROT2 += Math.PI;
                    }
                    if (ElRX < ShouRX)
                    {
                        angROT2 = Math.PI - angROT2;
                    }
                    int degRot2 = Convert.ToInt32(Math.Floor(angROT2 * 180 / Math.PI));
                    //Constrained by mechanism
                    if (degRot2 < 25)
                    {
                        degRot2 = 25;
                    }
                    else if (degRot2 > 155)
                    {
                        degRot2 = 155;
                    }
                    arduino.analogWrite(5, degRot2);

                    //Arduino Control 3rd Rotation Angle (Elbo)
                    double ax      = HandRX - ElRX;
                    double ay      = HandRY - ElRY;
                    double az      = HandRZ - ElRZ;
                    double bx      = ElRX - ShouRX;
                    double by      = ElRY - ShouRY;
                    double bz      = ElRZ - ShouRZ;
                    double angROT3 = Math.Acos((ax * bx + ay * by + az * bz) / Math.Sqrt(ax * ax + ay * ay + az * az) / Math.Sqrt(bx * bx + by * by + bz * bz));
                    arduino.analogWrite(10, Convert.ToInt32(Math.Floor(angROT3 * 180 / Math.PI)));

                    //Arduino Control 4th Rotation Angle (Grab)
                    if (HandLY > HipLY)
                    {
                        arduino.analogWrite(9, 30);
                    }
                    else
                    {
                        arduino.analogWrite(9, 110);
                    }

                    // Draw joints
                    foreach (Joint joint in data.Joints)
                    {
                        Point jointPos  = getDisplayPosition(joint);
                        Line  jointLine = new Line();
                        jointLine.X1              = jointPos.X - 3;
                        jointLine.X2              = jointLine.X1 + 6;
                        jointLine.Y1              = jointLine.Y2 = jointPos.Y;
                        jointLine.Stroke          = jointColors[joint.ID];
                        jointLine.StrokeThickness = 6;
                        skeleton.Children.Add(jointLine);
                    }
                }
                iSkeleton++;
            } // for each skeleton
        }
        /// <summary>
        /// This method is used to position the ellipses on the 
        /// canvas according to correct movements of the tracked 
        /// joints.
        /// IMPORTANT NOTE: Code for vector scaling was imported from 
        /// the Coding4Fun Kinect Toolkit available here: 
        /// http://c4fkinect.codeplex.com/
        /// I only used this part to avoid adding an extra reference.
        /// </summary>
        /// <param name="ellipse"></param>
        /// <param name="joint"></param>
        private void SetEllipsePosition(Ellipse ellipse, Joint joint)
        {
            Microsoft.Research.Kinect.Nui.Vector vector = new Microsoft.Research.Kinect.Nui.Vector();
            vector.X = ScaleVector(640, joint.Position.X);
            vector.Y = ScaleVector(480, -joint.Position.Y);
            vector.Z = joint.Position.Z;

            Joint updatedJoint = new Joint();
            updatedJoint.ID = joint.ID;
            updatedJoint.TrackingState = JointTrackingState.Tracked;
            updatedJoint.Position = vector;

            Canvas.SetLeft(ellipse, updatedJoint.Position.X);
            Canvas.SetTop(ellipse, updatedJoint.Position.Y);
        }
示例#20
0
        public static Vector3 ToVector3(Microsoft.Research.Kinect.Nui.Vector p, Vector3 scale)
        {
            var half = scale / 2.0f;

            return((new Vector3(p.X, p.Y, p.Z) * half * new Vector3(1, -1, 1)) + half);
        }
示例#21
0
        public Gesture track(SkeletonData trackSkeleton, Joint trackJoint, double elevationAngle)
        {
            //Guesture guesture = null;
            double theta = elevationAngle * Math.PI / 180;

            if (positions.Count < trackSkeleton.Joints.Count)
            {
                foreach (Joint j in trackSkeleton.Joints)
                {
                    if (!positions.ContainsKey(j))
                    {
                        Microsoft.Research.Kinect.Nui.Vector newPosition
                                      = new Microsoft.Research.Kinect.Nui.Vector();
                        newPosition.W = j.Position.W;
                        newPosition.X = j.Position.X;
                        newPosition.Y = (float)(j.Position.Y * Math.Cos(theta) + j.Position.Z * Math.Sin(theta));
                        newPosition.Z = (float)(j.Position.Z * Math.Cos(theta) - j.Position.Y * Math.Sin(theta));

                        RotationalArray <Microsoft.Research.Kinect.Nui.Vector> jointPositions =
                            new RotationalArray <Microsoft.Research.Kinect.Nui.Vector>(interval, 0);
                        jointPositions.Add(newPosition);

                        positions.Add(j, jointPositions);
                    }
                }
            }
            else
            {
                foreach (Joint j in trackSkeleton.Joints)
                {
                    for (int i = 0; i < positions.Keys.Count; i++)
                    {
                        Joint k = positions.Keys.ElementAt(i);
                        if (j.ID == k.ID)
                        {
                            Microsoft.Research.Kinect.Nui.Vector newPosition
                                          = new Microsoft.Research.Kinect.Nui.Vector();
                            newPosition.W = j.Position.W;
                            newPosition.X = j.Position.X;
                            newPosition.Y = (float)(j.Position.Y * Math.Cos(theta) + j.Position.Z * Math.Sin(theta));
                            newPosition.Z = (float)(j.Position.Z * Math.Cos(theta) - j.Position.Y * Math.Sin(theta));

                            positions[k].Add(newPosition);

                            if (k.ID == trackJoint.ID && positions[k].GetFirst().X != 0)
                            {
                                Microsoft.Research.Kinect.Nui.Vector velocityVector = new Microsoft.Research.Kinect.Nui.Vector();
                                velocityVector.X = positions[k].GetLast().X - positions[k].GetFirst().X;
                                velocityVector.Y = positions[k].GetLast().Y - positions[k].GetFirst().Y;
                                velocityVector.Z = positions[k].GetLast().Z - positions[k].GetFirst().Z;
                                velocityVector.W = positions[k].GetLast().W - positions[k].GetFirst().W;

                                guesture = guessGuesture(velocityVector, trackJoint, xThreshold, yThreshold, zThreshold);
                                if (guesture.id == previousGuesture)
                                {
                                    lockout = true;
                                }
                                else
                                {
                                    lockout          = false;
                                    previousGuesture = guesture.id;
                                }
                            }

                            break;
                        }
                    }
                }
            }

            if (!lockout)
            {
                return(guesture);
            }
            else
            {
                return(new Gesture(DateTime.Now, 0, GestureID.Invalid, trackJoint));
            }
        }
 public bool IsActive(Microsoft.Research.Kinect.Nui.Vector CurrentCoords, float ReferencePlaneZ)
 {
     Position = CurrentCoords;
     if ((NearEnd < (ReferencePlaneZ - CurrentCoords.Z)) && ((ReferencePlaneZ - CurrentCoords.Z) < FarEnd))
     {
         if (ControlDisabled)
             return false;
         if (!ActiveFlag)
             Init = CurrentCoords;
         ActiveFlag = true;
         return true;
     }
     else
     {
         if (!ControlDisabled && ActiveFlag)
             CompleteGesture();
         ActiveFlag = false;
         ControlDisabled = false;
         return false;
     }
 }