public void actualizarPostura(Skeleton esqueleto)
 {
     manoDerAct = esqueleto.Joints[JointType.WristRight];
     manoIzqAct = esqueleto.Joints[JointType.WristLeft];
     codoDerAct = esqueleto.Joints[JointType.ElbowRight];
     codoIzqAct = esqueleto.Joints[JointType.ElbowLeft];
 }

        /// <summary>
        /// Draw a line between two joints in a canvas
        /// </summary>
        /// <param name="canvas">the canvas</param>
        /// <param name="jstart">the joint where to start the line</param>
        /// <param name="jend">the joint where to end the line</param>
        /// <param name="vskeleton">the visual skeleton</param>
        private static void DrawLineBetween(Canvas canvas, Joint jstart, Joint jend, VisualSkeleton vskeleton)
        {
            if (vskeleton.positionMap.ContainsKey(jstart) && vskeleton.positionMap.ContainsKey(jend))
            {
                Line l = new Line();

                l.X1 = vskeleton.positionMap[jstart].X;
                l.Y1 = vskeleton.positionMap[jstart].Y;
                l.X2 = vskeleton.positionMap[jend].X;
                l.Y2 = vskeleton.positionMap[jend].Y;

                if (jstart.TrackingState == JointTrackingState.Inferred &&
                    jend.TrackingState == JointTrackingState.Inferred)
                {
                    l.Stroke = Brushes.Yellow;
                    l.StrokeThickness = 3;
                }
                else if (jstart.TrackingState == JointTrackingState.Tracked &&
                    jend.TrackingState == JointTrackingState.Tracked)
                {
                    l.Stroke = Brushes.Green;
                    l.StrokeThickness = 3;
                }
                else if (jstart.TrackingState == JointTrackingState.NotTracked ||
                    jend.TrackingState == JointTrackingState.NotTracked)
                {
                    l.Stroke = Brushes.Transparent;
                    l.StrokeThickness = 0;
                }

                canvas.Children.Add(l);
            }
        }

        public void JointTrackingStateIsSerialised()
        {
            var j = new Joint {TrackingState = JointTrackingState.Tracked};

            var json = j.ToJson();
            Assert.IsTrue(json.Contains("Tracked"));
        }

        private void DrawBone(Joint jointFrom, Joint jointTo, Pen aPen, DrawingContext aContext)
        {
            if (jointFrom.TrackingState == JointTrackingState.NotTracked ||
            jointTo.TrackingState == JointTrackingState.NotTracked)
            {
                return;
            }

            if (jointFrom.TrackingState == JointTrackingState.Inferred ||
            jointTo.TrackingState == JointTrackingState.Inferred)
            {
                ColorImagePoint p1 = mySensor.CoordinateMapper.MapSkeletonPointToColorPoint(jointFrom.Position, ColorImageFormat.RgbResolution640x480Fps30);
                ColorImagePoint p2 = mySensor.CoordinateMapper.MapSkeletonPointToColorPoint(jointTo.Position, ColorImageFormat.RgbResolution640x480Fps30);
                //Thin line
                aPen.DashStyle = DashStyles.Dash;
                aContext.DrawLine(aPen, new Point(p1.X, p1.Y), new Point(p2.X, p2.Y));

            }
            if (jointFrom.TrackingState == JointTrackingState.Tracked ||
            jointTo.TrackingState == JointTrackingState.Tracked)
            {
                ColorImagePoint p1 = mySensor.CoordinateMapper.MapSkeletonPointToColorPoint(jointFrom.Position, ColorImageFormat.RgbResolution640x480Fps30);
                ColorImagePoint p2 = mySensor.CoordinateMapper.MapSkeletonPointToColorPoint(jointTo.Position, ColorImageFormat.RgbResolution640x480Fps30);
                //Thick line
                aPen.DashStyle = DashStyles.Solid;
                aContext.DrawLine(aPen, new Point(p1.X, p1.Y), new Point(p2.X, p2.Y));
            }
        }

        /// <summary>
        /// ジョイント間の線を引く
        /// </summary>
        /// <param name="skeleton"></param>
        /// <param name="joint"></param>
        /// <returns></returns>
        private void DrawJointLine( Skeleton skeleton, Joint joint )
        {
            // ジョイントと関連づいている一方のジョイントを取得する
            var start = skeleton.BoneOrientations[joint.JointType].StartJoint;
            var end = skeleton.BoneOrientations[joint.JointType].EndJoint;
            var startJoint = skeleton.Joints[start];
            var endJoint = skeleton.Joints[end];

            // どちらかが追跡状態でない場合には描画しない
            if ( (startJoint.TrackingState == JointTrackingState.NotTracked) || 
                 (endJoint.TrackingState == JointTrackingState.NotTracked) ) {
                return;
            }

            // 3次元座標を距離カメラの2次元座標に変換する
            var startPoint = SkeletonToPiont( skeleton.Joints[start].Position );
            var endPoint = SkeletonToPiont( skeleton.Joints[end].Position );
            if ( !IsDrawablePoint( startPoint ) || !IsDrawablePoint( endPoint ) ) {
                return;
            }


            // ジョイント間の線を引く
            canvas.Children.Add( new Line()
            {
                X1 = startPoint.X,
                Y1 = startPoint.Y,
                X2 = endPoint.X,
                Y2 = endPoint.Y,
                Stroke = new SolidColorBrush( Colors.Red ),
            } );
        }

 /// <summary>
 /// Gets the joint distance.
 /// </summary>
 /// <param name="firstJoint">The first joint.</param>
 /// <param name="secondJoint">The second joint.</param>
 /// <returns>retunr the distance</returns>
 public static float GetJointDistance(Joint firstJoint, Joint secondJoint)
 {
     float distanceX = firstJoint.Position.X - secondJoint.Position.X;
     float distanceY = firstJoint.Position.Y - secondJoint.Position.Y;
     float distanceZ = firstJoint.Position.Z - secondJoint.Position.Z;
     return (float)Math.Sqrt(Math.Pow(distanceX, 2) + Math.Pow(distanceY, 2) + Math.Pow(distanceZ, 2));
 }

        public void SetPosition(Joint joint)
        {
            if (!menuOpened)
            {
                if (Global.initPos)
                {

                    //in Global check if z moving forward by some amount then increase 0.3 and 0.4 so hand moves less
                    Joint scaledJoint = joint.ScaleTo((int)Global.windowWidth, (int)Global.windowHeight, (float)0.25, (float)0.25);
                    cursorX = cursorX + 519;
                    cursorX = (double)Math.Max(0, Math.Min(scaledJoint.Position.X, Global.windowWidth - hand.Width));
                    cursorY = (double)Math.Max(0, Math.Min(scaledJoint.Position.Y, Global.windowHeight - hand.Height));
                    Canvas.SetLeft(this, cursorX);
                    Canvas.SetTop(this, cursorY);
                }
                else
                {

                    Canvas.SetLeft(this, Global.windowWidth - hand.Width);
                    Canvas.SetTop(this, Global.windowHeight - hand.Height);
                }
            }
            else
            {
                radialMenu.setCursorPosition(this, joint);
                Canvas.SetLeft(this, cursorX);
                Canvas.SetTop(this, cursorY);
            }
        }

        double CalculateAngle2D(Joint shoulder, Joint hip, Joint knee)
        {
            double angle = 0;
            double value = CalcDiff();
            value = 0.3 * value;
            double shrhY = shoulder.Position.Y - (hip.Position.Y-value);
            double shrhZ = shoulder.Position.Z - hip.Position.Z;
            double hsl = vectorNorm( shrhY, shrhZ);
            double unrhY = knee.Position.Y - (hip.Position.Y-value);
            double unrhZ = knee.Position.Z - hip.Position.Z;
            double hul = vectorNorm( unrhY, unrhZ);
            double mhshu = shrhY * unrhY + shrhZ * unrhZ;

            double x = mhshu / (hul * hsl);
            if (x != Double.NaN)
            {
                if (-1 <= x && x <= 1)
                {
                    double angleRad = Math.Acos(x);
                    angle = angleRad * (180.0 / 3.1416);
                }
                else
                    angle = 0;
            }
            else
                angle = 0;

            return angle;
        }

        /// <summary>
        /// Calculates the angle between the provided joints in the YZ plane.
        /// </summary>
        /// <param name="joint1">The first joint.</param>
        /// <param name="joint2">The second joint.</param>
        /// <returns>The angle (in degrees) between the joints.</returns>
        public static double CalculateAngleYZ(Joint joint1, Joint joint2)
        {
            double deltaY = joint2.Position.Y - joint1.Position.Y;
            double deltaZ = joint2.Position.Z - joint1.Position.Z;

            return Units.RadiansToDegrees(Math.Atan2(deltaZ, deltaY));
        }

        public static Joint alignJoints(Joint aligningJoint, Joint startingJoint, Joint endingJoint)
        {
            Vector3 pointToBeTranslated = new Vector3(startingJoint.Position.X, startingJoint.Position.Y, startingJoint.Position.Z);
            Vector3 origin = new Vector3(aligningJoint.Position.X, aligningJoint.Position.Y, aligningJoint.Position.Z);

            Vector3 differentialVector = pointToBeTranslated - origin;

            //get the distance between the alingning joint and starting joint. We need to maintain this distance when we rotate the joint
            double distance = Math.Sqrt(startingJoint.Position.X * aligningJoint.Position.X + startingJoint.Position.Y * aligningJoint.Position.Y + startingJoint.Position.Z * aligningJoint.Position.Z);
            //get current vectors
            Vector3 startingVector = new Vector3(startingJoint.Position.X - aligningJoint.Position.X, startingJoint.Position.Y - aligningJoint.Position.Y, startingJoint.Position.Z - aligningJoint.Position.Z);
            Vector3 expectedVector = new Vector3(endingJoint.Position.X - aligningJoint.Position.X, endingJoint.Position.Y - aligningJoint.Position.Y, endingJoint.Position.Z - aligningJoint.Position.Z);

            // Normalize vectors so we can use Dot product correctly
            startingVector.Normalize();
            expectedVector.Normalize();
            float dotAngle = (float)Math.Acos(Vector3.Dot(startingVector, expectedVector));

            if (float.IsNaN(dotAngle))
            {
                dotAngle = 0;
            }

            Vector3 rotatedVect = Vector3.Transform(differentialVector, Matrix.CreateFromAxisAngle(startingVector, dotAngle));

            rotatedVect += origin;

            SkeletonPoint newSkeletonPoint = new SkeletonPoint();
            newSkeletonPoint.X = rotatedVect.X;
            newSkeletonPoint.Y = rotatedVect.Y;
            newSkeletonPoint.Z = rotatedVect.Z;
            startingJoint.Position = newSkeletonPoint;
            return startingJoint;
        }

        public void SetPosition(Joint joint)
        {
            Joint scaledJoint = joint.ScaleTo(1270 - 88, 750 - 38, 0.5f, 0.5f);

            Canvas.SetLeft(this, scaledJoint.Position.X);
            Canvas.SetTop(this, scaledJoint.Position.Y);
        }

 /// <summary>
 /// Returns the length of the segment defined by the specified joints.
 /// </summary>
 /// <param name="p1">The first joint (start of the segment).</param>
 /// <param name="p2">The second joint (end of the segment).</param>
 /// <returns>The length of the segment in meters.</returns>
 public static double Distance(Joint p1, Joint p2)
 {
     return Math.Sqrt(
         Math.Pow(p1.Position.X - p2.Position.X, 2) +
         Math.Pow(p1.Position.Y - p2.Position.Y, 2) +
         Math.Pow(p1.Position.Z - p2.Position.Z, 2));
 }

 private ColorImagePoint ConverterCoordenadasArticulacao(Joint articulacao, double larguraCanvas, double alturaCanvas)
 {
     ColorImagePoint posicaoArticulacao = Kinect.CoordinateMapper.MapSkeletonPointToColorPoint(articulacao.Position, Kinect.ColorStream.Format);
     posicaoArticulacao.X = (int)(posicaoArticulacao.X * larguraCanvas) / Kinect.ColorStream.FrameWidth;
     posicaoArticulacao.Y = (int)(posicaoArticulacao.Y * alturaCanvas) / Kinect.ColorStream.FrameHeight;
     return posicaoArticulacao;
 }

        double findAngle(Joint hombroIzquierdo, Joint hombroDerecho)
        {
            if (hombroDerecho.TrackingState == JointTrackingState.NotTracked && hombroIzquierdo.TrackingState == JointTrackingState.NotTracked)
            {
                return 0;
            }

            if (hombroDerecho.TrackingState == JointTrackingState.NotTracked ||
                hombroDerecho.TrackingState == JointTrackingState.Inferred)
            {
                return 90;
            }

            else if (hombroIzquierdo.TrackingState == JointTrackingState.NotTracked ||
                hombroIzquierdo.TrackingState == JointTrackingState.Inferred)
            {
                return -90;
            }
            else
            {
                return Math.Atan2(
                    hombroDerecho.Position.Z - hombroIzquierdo.Position.Z,
                    hombroDerecho.Position.X - hombroIzquierdo.Position.X) * 180.0 / Math.PI;
            }
        }

        /// <summary>
        /// Draws the bone.
        /// </summary>
        /// <param name="trackedJoint1">The tracked joint1.</param>
        /// <param name="trackedJoint2">The tracked joint2.</param>
        /// 
        void drawBone(Joint trackedJoint1, Joint trackedJoint2)
        {
            Line bone = new Line();
            bone.Stroke = Brushes.Red;
            bone.StrokeThickness = 3;
            Point joint1 = this.ScalePosition(trackedJoint1.Position);
            bone.X1 = joint1.X;
            bone.Y1 = joint1.Y;

            Point mappedPoint1 = this.ScalePosition(trackedJoint1.Position);
            Rectangle r = new Rectangle(); r.Height = 10; r.Width = 10;
            r.Fill = Brushes.Red;
            Canvas.SetLeft(r, mappedPoint1.X - 2);
            Canvas.SetTop(r, mappedPoint1.Y - 2);
            this.caller.myCanvas.Children.Add(r);

            Point joint2 = this.ScalePosition(trackedJoint2.Position);
            bone.X2 = joint2.X;
            bone.Y2 = joint2.Y;

            Point mappedPoint2 = this.ScalePosition(trackedJoint2.Position);

            if (LeafJoint(trackedJoint2))
            {
                Rectangle r1 = new Rectangle(); r1.Height = 10; r1.Width = 10;
                r1.Fill = Brushes.Red;
                Canvas.SetLeft(r1, mappedPoint2.X - 2);
                Canvas.SetTop(r1, mappedPoint2.Y - 2);
                this.caller.myCanvas.Children.Add(r1);
                Point mappedPoint = this.ScalePosition(trackedJoint2.Position);

            }

            this.caller.myCanvas.Children.Add(bone);
        }

        /// <summary>
        /// Calculates the angle between the provided joints in the XY plane.
        /// </summary>
        /// <param name="joint1">The first joint.</param>
        /// <param name="joint2">The second joint.</param>
        /// <returns>The angle (in degrees) between the joints.</returns>
        public static double CalculateAngleXY(Joint joint1, Joint joint2)
        {
            double deltaX = joint2.Position.X - joint1.Position.X;
            double deltaY = joint2.Position.Y - joint1.Position.Y;

            return Units.RadiansToDegrees(Math.Atan2(deltaY, deltaX));
        }

 /// <summary>
 /// Get most matched action with given joint queue.
 /// Calculating match by multiplying similarity and count of points checked.
 /// </summary>
 /// <param name="joint">Array of joint positions, smaller index means former position</param>
 /// <param name="leaseCnt">Least amount of positions to be checked</param>
 /// <param name="similarity">Similarity of matched action</param>
 /// <returns>Most matched action type</returns>
 public static ActionType MatchedAction(Joint[] joint, int leaseCnt, out float similarity)
 {
     float maxSimi = 0.0f;
     ActionType maxType = ActionType.NONE;
     //for (int i = leaseCnt; i < joint.Length; ++i)
     //{
         for (int j = 0; j < 8; ++j)
         {
             ActionType type = (ActionType)j;
             float simi = Similarity(joint, leaseCnt, type);
             if (simi > maxSimi)
             {
                 maxSimi = simi;
                 maxType = type;
             }
         }
     //}
     similarity = maxSimi;
     if (maxSimi > 0.25f)
     {
         return maxType;
     }
     else
     {
         return ActionType.NONE;
     }
 }

        const float POSITIVEFARDISTANCE = 0.35f; //Distancia extra que debe alejarse la mano para considerar que esta en la zona positiva lejana

        #endregion Fields

        #region Methods

        /*
         * Función: Get1AxleValue
         * Descripción: Función que devuelve el valor de movimiento en 1 eje
         * Autor: Christian Vargas
         * Fecha de creación: 19/08/15
         * Fecha de modificación: --/--/--
         * Entradas: mainHand (Joint, nodo de la mano principal del usuario), secondaryHand (Joint, nodo de la mano secundaria del usuario), mainShoulder (Joint, nodo del hombro principal del usuario), head (Joint, nodo de la cabeza el usuario)
         * Salidas: (Tuple<bool, int>, valor que indica si el movimiento continua y el valor del eje)
         */
        public static Tuple<bool, int> Get1AxleValue(Joint mainHand, Joint secondaryHand, Joint mainShoulder, Joint head)
        {
            int xMovement = 0;
            bool moving = true;
            if (secondaryHand.Position.Y >= head.Position.Y)
            {
                moving = false;
            }
            else
            {
                if (mainHand.Position.X >= mainShoulder.Position.X + POSITIVEDISTANCE + POSITIVEFARDISTANCE)
                {
                    xMovement = 2;
                }
                else if (mainHand.Position.X >= mainShoulder.Position.X + POSITIVEDISTANCE)
                {
                    xMovement = 1;
                }
                else if (mainHand.Position.X + NEGATIVEDISTANCE + NEGATIVEFARDISTANCE < mainShoulder.Position.X)
                {
                    xMovement = -2;
                }
                else if (mainHand.Position.X + NEGATIVEDISTANCE < mainShoulder.Position.X)
                {
                    xMovement = -1;
                }
            }
            return Tuple.Create(moving, xMovement);
        }

 private Node AddChildNode(Node parentNode, Joint childJoint)
 {
     Node newChildNode = new Node();
     newChildNode.Name = GetNodeIdName(childJoint.JointType);
     parentNode.Children.Add(newChildNode);
     return (newChildNode);
 }

        public void Test90DegreeAngle()
        {
            Joint vertex = new Joint();
            Joint[] otherJoints = new Joint[2];

            SkeletonPoint sp = new SkeletonPoint();
            sp.X = 0.0F;
            sp.Y = 0.0F;
            sp.Z = 0.0F;
            vertex.Position = sp;

            Joint joint1 = new Joint();
            SkeletonPoint point = new SkeletonPoint();
            point.X = 0.0F;
            point.Y = 1.0F;
            point.Z = 0.0F;
            joint1.Position = point;

            Joint joint2 = new Joint();
            SkeletonPoint point2 = new SkeletonPoint();
            point2.X = 1.0F;
            point2.Y = 0.0F;
            point2.Z = 0.0F;
            joint2.Position = point2;

            otherJoints[0] = joint1;
            otherJoints[1] = joint2;

            Assert.AreEqual(90, JointAnalyzer.findAngle(vertex,otherJoints), "Angle should be 90 degrees");
        }