/// <summary>
 /// Initializes a new Gesture Status.
 /// </summary>
 public GestureStatus()
 {
     this.isMakingAFast = false;
     this.startPoint = new SkeletonPoint();
     this.endPoint = new SkeletonPoint();
     this.headPoint = new SkeletonPoint();
 }

 public static bool comparePoint(SkeletonPoint a, SkeletonPoint b, float dif)
 {
     if (compareValue(a.X, b.X, dif) && compareValue(a.Y, b.Y, dif) && compareValue(a.Z, b.Z, dif))
         return true;
     else
         return false;
 }

        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");
        }

 public static SkeletonPoint vecSubt(SkeletonPoint a, SkeletonPoint b)
 {
     a.X -= b.X;
     a.Y -= b.Y;
     a.Z -= b.Z;
     return a;
 }

        public virtual void Add(SkeletonPoint position, KinectSensor sensor)
        {
            GesturePoint entity = new GesturePoint { Position = Vector.ToVector(position), Time = DateTime.Now };
            GestureEntities.Add(entity);
            if (GestureCanvas != null)
            {
                entity.DisplayEllipse = new Ellipse
                {
                    Width = 4,
                    Height = 4,
                    HorizontalAlignment = HorizontalAlignment.Left,
                    VerticalAlignment = VerticalAlignment.Top,
                    StrokeThickness = 2.0,
                    Stroke = new SolidColorBrush(DisplayColor),
                    StrokeLineJoin = PenLineJoin.Round
                };

                Point point = Tools.Convert(sensor, position);
                float x = (float)(point.X * GestureCanvas.ActualWidth);
                float y = (float)(point.Y * GestureCanvas.ActualHeight);
                Canvas.SetLeft(entity.DisplayEllipse, x - entity.DisplayEllipse.Width / 2);
                Canvas.SetTop(entity.DisplayEllipse, y - entity.DisplayEllipse.Height / 2);
                GestureCanvas.Children.Add(entity.DisplayEllipse);
            }
            if (GestureEntities.Count > WindowSize)
            {
                GesturePoint entryToRemove = GestureEntities[0];
                if (GestureCanvas != null)
                {
                    GestureCanvas.Children.Remove(entryToRemove.DisplayEllipse);
                }
                GestureEntities.Remove(entryToRemove);
            }
            LookForGesture();
        }

 public static SkeletonPoint vecMult(float coEffish, SkeletonPoint vec)
 {
     vec.X *= coEffish;
     vec.Y *= coEffish;
     vec.Z *= coEffish;
     return vec;
 }

 public static SkeletonPoint averagePoint(SkeletonPoint p1, int avgFactor)
 {
     p1.X = p1.X  / avgFactor;
     p1.Y = p1.Y  / avgFactor;
     p1.Z = p1.Z  / avgFactor;
     return p1;
 }

        public void Update(SkeletonPoint position, int updateId, long timestamp)
        {
            // Movement magnitude gets scaled by this amount in order to get the current activity metric
            const double DeltaScalingFactor = 10.0;

            // Controls how quickly new values of the metric displace old values. 1.0 means that new values
            // for metric immediately replace old values, while smaller decay amounts mean that old metric
            // values influence the metric for a longer amount of time (i.e.: decay more slowly).
            const double ActivityDecay = 0.1;

            var delta = new SkeletonPoint
            {
                X = position.X - this.LastPosition.X,
                Y = position.Y - this.LastPosition.Y,
                Z = position.Z - this.LastPosition.Z
            };

            double deltaLengthSquared = (delta.X * delta.X) + (delta.Y * delta.Y) + (delta.Z * delta.Z);
            double newMetric = DeltaScalingFactor * Math.Sqrt(deltaLengthSquared);

            this.ActivityLevel = ((1.0 - ActivityDecay) * this.ActivityLevel) + (ActivityDecay * newMetric);

            bool newIsActive = this.ActivityLevel >= ActivityMetricThreshold;

            if (newIsActive != this.IsActive)
            {
                this.IsActive = newIsActive;
                this.StateTransitionTimestamp = timestamp;
            }

            this.LastPosition = position;
            this.LastUpdateId = updateId;
        }

        public void AssignAnglesToSkeleton(Skeleton skeleton)
        {
            Hashtable angles = getAllAngles();

            foreach (JointType key in angles.Keys)
            {
                List<double> anglesInDouble = (List<double>)angles[key];

                SkeletonPoint point = new SkeletonPoint();

                if (!double.IsNaN(anglesInDouble[0]))
                {
                    point.X = (int)anglesInDouble[0];
                }
                if (!double.IsNaN(anglesInDouble[1]))
                {
                    point.Y = (int)anglesInDouble[1];
                }
                if (!double.IsNaN(anglesInDouble[2]))
                {
                    point.Z = (int)anglesInDouble[2];
                }

                var joint = skeleton.Joints[key];
                joint.Position = point;
                skeleton.Joints[key] = joint;
            }
        }

        protected void DoPan(SkeletonPoint handPoint)
        {
            var mapExtentDeltaX = (_startExtent.XMax - _startExtent.XMin);
            var mapExtentDeltaY = (_startExtent.YMax - _startExtent.YMin);

            var relativeDeltaDistance = _startHandPoint.DistanceVectorFrom(handPoint, _map.ActualWidth, _map.ActualHeight);
            var deltaX = relativeDeltaDistance.X * mapExtentDeltaX;
            var deltaY = relativeDeltaDistance.Y * mapExtentDeltaY;

            var nextExtent = new Envelope
                                 {
                                     XMin = _startExtent.XMin + deltaX,
                                     XMax = _startExtent.XMax + deltaX,
                                     YMin = _startExtent.YMin - deltaY,
                                     YMax = _startExtent.YMax - deltaY
                                 };

            _map.Extent = nextExtent;

            if (KinectPanning != null)
            {
                var handCoordinate = handPoint.ToEsriWebMercatorMapPoint(_map);
                KinectPanning(handCoordinate.ToMapCoord());
            }
        }

 public Plane(SkeletonPoint p1, SkeletonPoint p2, SkeletonPoint p3)
 {
     a = (p2.Y - p1.Y) * (p3.Z - p1.Z) - (p3.Y - p1.Y) * (p2.Z - p1.Z);
     b = (p2.Z - p1.Z) * (p3.X - p1.X) - (p3.Z - p1.Z) * (p2.X - p1.X);
     c = (p2.X - p1.X) * (p3.Y - p1.Y) - (p3.X - p1.X) * (p2.Y - p1.Y);
     d = -(a * p1.X + b * p1.Y + c * p1.Z);
 }

 /// <summary>
 /// Maps a SkeletonPoint to lie within our render space and converts to Point
 /// </summary>
 /// <param name="sensor">Sensor to proyect point</param>
 /// <param name="skelpoint">point to map</param>
 /// <returns>mapped point</returns>
 public Point SkeletonPointToScreen(KinectSensor sensor, SkeletonPoint skelpoint)
 {
     // Convert point to depth space.
     // We are not using depth directly, but we do want the points in our 640x480 output resolution.
     DepthImagePoint depthPoint = sensor.CoordinateMapper.MapSkeletonPointToDepthPoint(skelpoint, DepthImageFormat.Resolution640x480Fps30);
     return new Point(depthPoint.X, depthPoint.Y);
 }

        public bool Detect(SkeletonPoint shoulderCenter, SkeletonPoint rightHandPoint, bool rightHandTracked,
                           SkeletonPoint leftHandPoint, bool leftHandTracked)
        {
            var rightHandInFront = rightHandTracked && (shoulderCenter.Z - rightHandPoint.Z >= MinZDistanceFromBody);
            var leftHandInFront = leftHandTracked && (shoulderCenter.Z - leftHandPoint.Z >= MinZDistanceFromBody);

            // Just one hand at minimal distance from Shoulder Center
            if (rightHandInFront ^ leftHandInFront)
            {
                SkeletonPoint handPoint;
                if (rightHandInFront)
                {
                    handPoint = rightHandPoint;
                    PanningHand = Hand.Right;
                }
                else
                {
                    handPoint = leftHandPoint;
                    PanningHand = Hand.Left;
                }

                if (!IsPanning)
                {
                    StartPan(handPoint);
                }

                RunPanning(handPoint);
            }
            else
            {
                StopZooming();
            }

            return IsPanning;
        }

 public static SkeletonPoint vecAdd(SkeletonPoint a, SkeletonPoint b)
 {
     a.X += b.X;
     a.Y += b.Y;
     a.Z += b.Z;
     return a;
 }

        public bool Detect(SkeletonPoint shoulderCenter, SkeletonPoint rightHandPoint, bool rightHandTracked,
                           SkeletonPoint leftHandPoint, bool leftHandTracked)
        {
            if (rightHandTracked == false || leftHandTracked == false)
            {
                StopZooming();
            }
                // Both hands at minimal distance from Shoulder Center
            else if (shoulderCenter.Z - rightHandPoint.Z >= MinZDistanceFromBody &&
                     shoulderCenter.Z - leftHandPoint.Z >= MinZDistanceFromBody)
            {
                if (!IsZooming)
                {
                    StartZoom(rightHandPoint, leftHandPoint);
                }

                RunZooming(rightHandPoint, leftHandPoint);
            }
            else
            {
                StopZooming();
            }

            return IsZooming;
        }

        ///<summary>
        /// Receive three skeleton point (3D position of a joint).
        /// Compute two vector, return the angle between them.
        /// If something is wrong return 0
        ///</summary>
        public static float getAngle(SkeletonPoint a, SkeletonPoint b, SkeletonPoint c, bool sign)
        {
            Vecto3Float[] vector = new Vecto3Float[2];
            //vector[0] = new Vecto3Float(b.X - c.X, b.Y - c.Y, b.Z - c.Z);
            vector[0] = new Vecto3Float(c.X - b.X, c.Y - b.Y, c.Z - b.Z);
            vector[1] = new Vecto3Float(a.X - b.X, a.Y - b.Y, a.Z - b.Z);

            float v0_magnitude = vector[0].magnitude();
            float v1_magnitude = vector[1].magnitude();
            if (v0_magnitude != 0.0 && v1_magnitude != 0.0)
            {
                vector[0].normalize();
                vector[1].normalize();

                float sign_f = sign ? -1f : 1f;
                double x = (double)vector[0].cross(vector[1]).magnitude();
                double y = (double)vector[0].dot(vector[1]);

                float theta = (float)Math.Atan2(sign_f * x, sign_f * y);

                return theta;// -theta;
            }
            else
            {
                return 0.0f;
            }
        }

        public static Vector3D TransPortVector(SkeletonPoint skel) //單點做世界座標轉換
        {
            Vector3D realPoint = new Vector3D(skel.X, skel.Y, skel.Z);
            realPoint = TransPortVector(realPoint);

            return realPoint;
        }   

        public static float DistanceXYBetweenPoints(SkeletonPoint point1, SkeletonPoint point2)
        {
            double d1 = point1.X - point2.X;
            double d2 = point1.Y - point2.Y;

            return (float)Math.Sqrt(d1 * d1 + d2 * d2);
        }

 public SkeletonPoint ConvertPoint(SkeletonPoint point)
 {
     SkeletonPoint pt = new SkeletonPoint();
     pt.X = ScalePoint(point.X, bottomCenterPoint.X) + (float)ActiveRectangle.Width / 2 + (float)ActiveRectangle.X;
     pt.Y = (float)ActiveRectangle.Height - ScalePoint(point.Y, bottomCenterPoint.Y) + (float)ActiveRectangle.Y;
     pt.Z = ScalePoint(point.Z, bottomCenterPoint.Z);
     return pt;
 }

 public static Point CalculateMiddlePoint(SkeletonPoint point1, SkeletonPoint point2)
 {
     return new Point(
         GetMiddleValue(point1.X, point2.X),
         GetMiddleValue(point1.Y, point2.Y),
         GetMiddleValue(point1.Z, point2.Z)
     );
 }