public void PoseInterpolate()
        {
            //Console.WriteLine("Test");

            if (eventListener.newPose == null || eventListener.TimeBetweenPoses == 0)
            {
                //Console.WriteLine("Error: Can't run interpolator...");
                return;
            }
            else if (eventListener.curPose == null || eventListener.lastPose == null)
            {
                eventListener.curPose = eventListener.newPose;
            }
            else             // Interpolate
            {
                //Console.WriteLine("Interpolating pose...");

                if (lastPose != eventListener.lastPose)
                {
                    curPoseTimePassed = 0;
                    lastPose          = eventListener.lastPose;
                }

                InterpolatorDelay = (int)Math.Floor(eventListener.TimeBetweenPoses / InterpolatorMultiplier);

                double interpolateDivider = (InterpolatorDelay > 0 ? ((eventListener.TimeBetweenPoses - curPoseTimePassed) / InterpolatorDelay) : 0);
                interpolateDivider = (interpolateDivider < 1 ? 1 : interpolateDivider);

                if (interpolateDivider >= 1 && InterpolatorDelay > 0)
                {
                    //Console.WriteLine("Interpolate Divider: " + interpolateDivider);

                    Pose newPose = eventListener.newPose;

                    if (typeof(Pose2D) == newPose.GetType())
                    {
                        List <KeyPoint2D> newCurPose = new List <KeyPoint2D>();

                        for (int i = 0; i < newPose.KeyPoints.Count; i++)
                        {
                            KeyPoint2D keyPoint = ((Pose2D)newPose).GetKeyPoint2D(i);

                            KeyPoint2D newKeyPoint = InterpolateKeyPoint2Ds(((Pose2D)lastPose).GetKeyPoint2D(keyPoint.BodyPoint), keyPoint, interpolateDivider);

                            if (newKeyPoint != null && newKeyPoint.IsValid)
                            {
                                newCurPose.Add(newKeyPoint);
                            }
                            else
                            {
                                newCurPose.Add(keyPoint);
                            }
                        }

                        Pose2D interPose = new Pose2D(newCurPose.ToArray());

                        foreach (KeyPoint2D keyPoint in newPose.KeyPoints)
                        {
                            if (interPose.GetKeyPoint2D(keyPoint.BodyPoint) == null)
                            {
                                interPose.KeyPoints.Add(keyPoint);
                            }
                        }

                        eventListener.curPose = interPose;
                    }
                    else if (typeof(Pose3D) == newPose.GetType())
                    {
                        List <KeyPoint3D> newCurPose = new List <KeyPoint3D>();

                        for (int i = 0; i < newPose.KeyPoints.Count; i++)
                        {
                            KeyPoint3D keyPoint = ((Pose3D)newPose).GetKeyPoint3D(i);

                            KeyPoint3D newKeyPoint = InterpolateKeyPoint3Ds(((Pose3D)lastPose).GetKeyPoint3D(keyPoint.BodyPoint), keyPoint, interpolateDivider);

                            if (newKeyPoint != null && newKeyPoint.IsValid)
                            {
                                newCurPose.Add(newKeyPoint);
                            }
                            else
                            {
                                newCurPose.Add(keyPoint);
                            }
                        }

                        Pose3D interPose = new Pose3D(newCurPose.ToArray());

                        foreach (KeyPoint3D keyPoint in newPose.KeyPoints)
                        {
                            if (interPose.GetKeyPoint3D(keyPoint.BodyPoint) == null)
                            {
                                interPose.KeyPoints.Add(keyPoint);
                            }
                        }

                        eventListener.curPose = interPose;
                    }
                }
            }

            //Console.WriteLine("Sending pose...");

            SendPose(eventListener.curPose);
        }
        public void SendPose(Pose pose)
        {
            try
            {
                //foreach (KeyPoint point in pose.KeyPoints)
                //{
                //	Console.WriteLine(point.ToString());
                //}

                if (pose != null && typeof(Pose2D) == pose.GetType())
                {
                    Pose2D pose2D = (Pose2D)pose;

                    KeyPoint2D nose = pose2D.GetKeyPoint2D(BodyPoint.Nose_or_Top_Head);
                    //KeyPoint2D leftEye = pose2D.GetKeyPoint2D(BodyPoint.Left_Eye);
                    //KeyPoint2D rightEye = pose2D.GetKeyPoint2D(BodyPoint.Right_Eye);

                    if (nose != null && nose.IsValid)                    //&& leftEye != null && rightEye != null && nose.IsValid && leftEye.IsValid && rightEye.IsValid)
                    {
                        //if (!HeadsetStatus)
                        //{
                        //	program.headTrackingService.ChangeStatus(true);
                        //	HeadsetStatus = true;
                        //}

                        //double height = (leftEye.Y + rightEye.Y) / 2;
                        program.headTrackingService.SendPositionOnly(nose.X, nose.Y, 0);
                    }
                    //else
                    //{
                    //	if (HeadsetStatus)
                    //	{
                    //		program.headTrackingService.ChangeStatus(false);
                    //		HeadsetStatus = false;
                    //	}
                    //}

                    KeyPoint2D rightHand = pose2D.GetKeyPoint2D(BodyPoint.Right_Wrist);

                    if (rightHand != null && rightHand.IsValid)
                    {
                        program.controllerService2.SetControllerState(0, rightHand.X, rightHand.Y, -0.6, 0, 0, 0, 0, 0, 0, false, false, false);
                    }

                    KeyPoint2D leftHand = pose2D.GetKeyPoint2D(BodyPoint.Left_Wrist);

                    if (leftHand != null && leftHand.IsValid)
                    {
                        program.controllerService.SetControllerState(1, leftHand.X, leftHand.Y, -0.6, 0, 0, 0, 0, 0, 0, false, false, false);
                    }
                }
                else if (pose != null && typeof(Pose3D) == pose.GetType())
                {
                    Pose3D pose3D = (Pose3D)pose;

                    KeyPoint3D nose = pose3D.GetKeyPoint3D(BodyPoint.Nose_or_Top_Head);

                    if (nose != null && nose.IsValid)
                    {
                        //program.headTrackingService.SendPositionOnly(nose.X, nose.Y, 0);
                        program.headTrackingService.SendRotationAndPosition(nose.Yaw, nose.Pitch, nose.Roll, nose.X, nose.Y, nose.Z);
                        //program.headTrackingService.SendAsyncOffset(0, 0, 0);
                    }

                    KeyPoint3D rightHand = pose3D.GetKeyPoint3D(BodyPoint.Right_Wrist);

                    if (rightHand != null && rightHand.IsValid)
                    {
                        program.controllerService2.SetControllerState(0, rightHand.X, rightHand.Y, rightHand.Z, rightHand.Yaw, rightHand.Pitch, rightHand.Roll, 0, 0, 0, false, false, false);
                    }

                    KeyPoint3D leftHand = pose3D.GetKeyPoint3D(BodyPoint.Left_Wrist);

                    if (leftHand != null && leftHand.IsValid)
                    {
                        program.controllerService.SetControllerState(1, leftHand.X, leftHand.Y, leftHand.Z, leftHand.Yaw, leftHand.Pitch, leftHand.Roll, 0, 0, 0, false, false, false);
                    }

                    KeyPoint3D rightFoot = pose3D.GetKeyPoint3D(BodyPoint.Right_Heel);

                    if (rightFoot != null && rightFoot.IsValid)
                    {
                        //program.controllerService3.SetControllerState(2, rightFoot.X, rightFoot.Y, -0.6, 0, 0, 0, 0, 0, 0, false, false, false);
                    }

                    KeyPoint3D leftFoot = pose3D.GetKeyPoint3D(BodyPoint.Left_Heel);

                    if (leftFoot != null && leftFoot.IsValid)
                    {
                        //program.controllerService4.SetControllerState(3, leftFoot.X, leftFoot.Y, -0.6, 0, 0, 0, 0, 0, 0, false, false, false);
                    }
                }
            }
            catch (Exception)
            {
                program.ConnectOrReconnect();
            }
        }