public static int GetHighestValidPointIndexInRectangle(int bIndex, int cIndex)
        {
            Point pB = GetPoint(bIndex);
            Point pC = GetPoint(cIndex);
            int rectangleWidth = pB.X - pC.X;

            CameraSpacePoint highestPoint = new CameraSpacePoint()
            {
                X = float.PositiveInfinity,
                Y = float.PositiveInfinity,
                Z = float.PositiveInfinity
            };

            int highestPointIndex = -1;

            for (int i = 0; i < rectangleWidth; i++)
            {
                for (int j = 0; j < rectangleWidth; j++)
                {
                    int currentIndex = GetIndex(pC.X + j, pB.Y + i);
                    if (BoundaryCheck(currentIndex))
                    {
                        CameraSpacePoint currentPoint = GlobVar.SubtractedFilteredPointCloud[currentIndex];

                        if (currentPoint.Z < highestPoint.Z && !float.IsInfinity(currentPoint.X) && !float.IsInfinity(currentPoint.Y))
                        {
                            highestPoint = currentPoint;
                            highestPointIndex = currentIndex;
                        }
                    }
                }
            }
            return highestPointIndex;
        }

        public static float GetEuclideanDistance(CameraSpacePoint a, CameraSpacePoint b)
        {
            float distance =
                (float)Math.Sqrt((a.X - b.X) * (a.X - b.X) + (a.Y - b.Y) * (a.Y - b.Y) + (a.Z - b.Z) * (a.Z - b.Z));

            return float.IsNaN(distance) ? float.MaxValue : distance;
        }

        /// <summary>
        ///  Normalizes the depths in the input frame into a 0-255 byte gray-value array.
        /// </summary>
        public static byte[] CalculateIntensityFromCameraSpacePoints(CameraSpacePoint[] cameraSpacePoint)
        {
            var intensityMap = new byte[cameraSpacePoint.Length];

            for (int i = 0; i < cameraSpacePoint.Length; i++)
            {
                float depth = cameraSpacePoint[i].Z;
                if (depth != 0)
                {
                    float currentMax = depth - GlobVar.MinSensingDepth;
                    const float currentDepthRange = GlobVar.MaxSensingDepth - GlobVar.MinSensingDepth;

                    if (depth < GlobVar.MaxSensingDepth && depth > GlobVar.MinSensingDepth)
                    {
                        intensityMap[i] = (byte)(255 - (255 * currentMax / currentDepthRange));
                    }
                    else
                    {
                        intensityMap[i] = (byte)0;
                    }
                }
                else
                {
                    intensityMap[i] = (byte)0;
                }
            }
            return intensityMap;
        }

        private static float[] CreateIntegralImage(CameraSpacePoint[] cameraSpacePoints)
        {
            float[] integralImage = new float[GlobVar.ScaledFrameLength];
            for (var i = 0; i < GlobVar.ScaledFrameHeight; i++)
            {
                for (var j = 0; j < GlobVar.ScaledFrameWidth; j++)
                {
                    int indexCurrent = i * GlobVar.ScaledFrameWidth + j;
                    int indexUpLeft = (i - 1) * GlobVar.ScaledFrameWidth + j - 1;
                    int indexUp = (i - 1) * GlobVar.ScaledFrameWidth + j;
                    int indexLeft = i * GlobVar.ScaledFrameWidth + j - 1;

                    if (i==0 && j==0)
                    {
                        integralImage[indexCurrent] = cameraSpacePoints[indexCurrent].Z;
                    }
                    else if (i==0)
                    {
                        integralImage[indexCurrent] = cameraSpacePoints[indexCurrent].Z + integralImage[indexLeft];
                    }
                    else if (j == 0)
                    {
                        integralImage[indexCurrent] = cameraSpacePoints[indexCurrent].Z + integralImage[indexUp];
                    }
                    else
                    {
                        integralImage[indexCurrent] = cameraSpacePoints[indexCurrent].Z + integralImage[indexUp] + integralImage[indexLeft] - integralImage[indexUpLeft];
                    }
                }
            }
            return integralImage;
        }

 public void CheckActiveWorkspace(CameraSpacePoint[] handPositions)
 {
     if (handPositions.Length > 0)
     {
         CheckActiveWorkspace(Converter.CameraSpacePointsToPoint3Ds(handPositions).ToArray());
     }
 }

        public static List<Point3D> CameraSpacePointsToPoint3Ds(CameraSpacePoint[] cameraSpacePoints)
        {
            if (cameraSpacePoints == null)
            {
                return null;
            }
            if (cameraSpacePoints.Length == 0)
            {
                return null;
            }
            List<Point3D> point3Ds = new List<Point3D>();

            foreach (CameraSpacePoint point in cameraSpacePoints)
            {
                if (GeometryHelper.IsValidPoint(point))
                {
                    point3Ds.Add(new Point3D
                    {
                        X = point.X,
                        Y = point.Y,
                        Z = point.Z
                    });
                }
            }

            return point3Ds;
        }

        /// <summary>
        /// Calculates the angle between the specified points around the specified axis.
        /// </summary>
        /// <param name="center">The center of the angle.</param>
        /// <param name="start">The start of the angle.</param>
        /// <param name="end">The end of the angle.</param>
        /// <param name="axis">The axis around which the angle is calculated.</param>
        /// <returns>The angle, in degrees.</returns>
        public static double Angle(this CameraSpacePoint center, CameraSpacePoint start, CameraSpacePoint end, Axis axis)
        {
            switch (axis)
            {
                case Axis.X:
                    start.X = 0f;
                    center.X = 0f;
                    end.X = 0f;
                    break;
                case Axis.Y:
                    start.Y = 0f;
                    center.Y = 0f;
                    end.Y = 0f;
                    break;
                case Axis.Z:
                    start.Z = 0f;
                    center.Z = 0f;
                    end.Z = 0f;
                    break;
            }

            Vector3D first = start.ToVector3() - center.ToVector3();
            Vector3D second = end.ToVector3() - center.ToVector3();

            return Vector3D.AngleBetween(first, second);
        }

        public CameraSpacePoint Filter(CameraSpacePoint point)
        {
            if (this.lastTrend == null || this.lastOutput == null)
            {
                this.lastTrend = point;
                this.lastOutput = point;
                return point;
            }

            var newTrend = new CameraSpacePoint
            {
                X = gamma * (point.X - lastOutput.Value.X) + (1 - gamma) * lastTrend.Value.X,
                Y = gamma * (point.Y - lastOutput.Value.Y) + (1 - gamma) * lastTrend.Value.Y,
                Z = gamma * (point.Z - lastOutput.Value.Z) + (1 - gamma) * lastTrend.Value.Z
            };

            var newOutput = new CameraSpacePoint
            {
                X = alpha * point.X + (1 - alpha) * (lastOutput.Value.X + lastTrend.Value.X),
                Y = alpha * point.Y + (1 - alpha) * (lastOutput.Value.Y + lastTrend.Value.Y),
                Z = alpha * point.Z + (1 - alpha) * (lastOutput.Value.Z + lastTrend.Value.Z)
            };

            this.lastTrend = newTrend;
            this.lastOutput = newOutput;
            return newOutput;
        }

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

 private void SetValues(CameraSpacePoint hand)
 {
     KinectX = hand.X;
     KinectY = hand.Y;
     ScreenX = (MaxValue / 2) + (KinectX * MaxValue);
     ScreenY = (MaxValue / 2) - (KinectY * MaxValue);
 }

 /// <summary>
 /// Create a new <c>CustomJoint</c> object based on the supplied
 /// <c>JointType</c> value.
 /// </summary>
 public CustomJoint(JointType type)
 {
     _jointType = type;
     _position = new CameraSpacePoint();
     _depthPosition = new DepthSpacePoint();
     _colorPosition = new ColorSpacePoint();
     _trackingState = TrackingState.NotTracked;
 }

 private static CameraSpacePoint CopyPosition(CameraSpacePoint position)
 {
     CameraSpacePoint result = new CameraSpacePoint();
     result.X = position.X;
     result.Y = position.Y;
     result.Z = position.Z;
     return result;
 }

 public static Point3D CameraSpacePointToPoint3D(CameraSpacePoint cameraSpacePoint)
 {
     return new Point3D
     {
         X = cameraSpacePoint.X,
         Y = cameraSpacePoint.Y,
         Z = cameraSpacePoint.Z
     };
 }

        private static CameraSpacePoint SubstractPoints(CameraSpacePoint position1, CameraSpacePoint position2)
        {
            CameraSpacePoint result = new CameraSpacePoint();
            result.X = position1.X - position2.X;
            result.Y = position1.Y - position2.Y;
            result.Z = position1.Z - position2.Z;
            return result;

        }

        private static double DistanceBetweenPoints(CameraSpacePoint point1, CameraSpacePoint point2)
        {
            double dx = Math.Abs(point2.X - point1.X);
            double dy = Math.Abs(point2.Y - point1.Y);

            double dz = Math.Abs(point2.Z - point1.Z);

            return Math.Sqrt(dx * dx + dy * dy + dz * dz);
        }

        /*  Depricated  */
        public static CameraSpacePoint getEinsvektor(CameraSpacePoint point)
        {
            var e = (float)Math.Sqrt(Math.Pow(point.X, 2) + Math.Pow(point.Y, 2) + Math.Pow(point.Z, 2));
            point.X = point.X / e;
            point.Y = point.Y / e;
            point.Z = point.Z / e; 

            return point;
        }

        /// <summary>
        /// Copies camera space points from face to the gpu
        /// </summary>
        /// <param name="context"></param>
        /// <param name="points"></param>
        public void Copy(DeviceContext context, CameraSpacePoint[] points)
        {
            if (points.Length == 0)
                return;

            fixed (CameraSpacePoint* cptr = &points[0])
            {
                this.buffer.Upload(context, new IntPtr(cptr), points.Length * 12);
            }
        }

        public void AddFrame(CameraSpacePoint[] newFrame)
        {
            _temporalFrames.Add(_frameCounter,newFrame);
            _frameCounter++;

            if (_frameCounter == _maxTemporalFrames)
            {
                CalculateTemporalMedianImage();
            }
        }

        CameraSpacePoint CSVectorSubtract(CameraSpacePoint p1, CameraSpacePoint p2)
        {
            CameraSpacePoint diff = new CameraSpacePoint();

            diff.X = p1.X - p2.X;
            diff.Y = p1.Y - p2.Y;
            diff.Z = p1.Z - p2.Z;

            return diff;
        }

 public Head(int highestPointIndex)
 {
     HighestPointIndex = highestPointIndex;
     AvgCenterPoint = new CameraSpacePoint()
     {
         X = float.NaN,
         Y = float.NaN,
         Z = float.NaN
     };
 }