Ejemplo n.º 1
0
        public void OnSkeletonChanged(BoneMarkers marker, ISkeleton newSkeleton, IGame game)
        {
            if (_writer == null)
            {
                return;
            }
            for (int i = 0; i < (int)SkeletonMarkers.Count - 1; i++)
            {
                Vector3 v = newSkeleton.GetPositionOf((SkeletonMarkers)i);
                _currentData[i] = new Vub.Etro.IO.Vector4(v.X, v.Y, v.Z, 0);
            }

            writeGameObjects(game, (int)labels.Length);

            //if (newSkeleton is IRichSkeleton)
            //{
            //  writeSkeletonAngles((IRichSkeleton)newSkeleton, labels.Length + game.GameObjects.Keys.Count);
            //  writeSkeletonQualities((IRichSkeleton)newSkeleton, labels.Length + game.GameObjects.Keys.Count + angleLabels.Length);
            //}
            _writer.WriteIntFrame(_currentData);
            WriteAnalogData(game);
        }
Ejemplo n.º 2
0
        public void Check(ISkeleton skeleton, ISkeletonReport report)
        {
            SkeletonCorrectiveAngle correctiveAngle = new SkeletonCorrectiveAngle();

            if (Wishness == 0)
            {
                return;
            }

            Vector3 firstBone;
            Vector3 secondBone;
            float   currentAngle;

            #region defineBonesToUse
            //we globally make the vectors point in opposite direction
            //ex : when we compare the chest and the vertical vector, the two vectors are pointing up
            //ex : when we compare the chest and the head, the head vector points down
            //ex : when we talk about chest, the arms and legs point out of the chest
            //ex : when we compare lower parts with upper parts : the upper parts point to the chest and the lower parts to wirsts and ankles
            //ex : when we compare lower/upper parts : they are pointing out of the chest
            //ex : when we talk about legs, the chest point up and, when we talk about arms, it's pointing down
            //the sternum define the angle between the shoulders vector and the hips vector -> the trunk torsion
            //this is important to correctly compare the angles
            switch (Angle)
            {
            case PlayerJoint.Spine:
                firstBone = Vector3.Subtract(skeleton.GetPositionOf(SkeletonMarkers.ShoulderCenter),
                                             skeleton.GetPositionOf(SkeletonMarkers.HipCenter));//chest
                secondBone = Vector3.Up;
                break;

            case PlayerJoint.Sternum:
                firstBone = Vector3.Subtract(skeleton.GetPositionOf(SkeletonMarkers.ShoulderRight),
                                             skeleton.GetPositionOf(SkeletonMarkers.ShoulderLeft)); //shoulders (pointing right)
                secondBone = Vector3.Subtract(skeleton.GetPositionOf(SkeletonMarkers.HipRight),
                                              skeleton.GetPositionOf(SkeletonMarkers.HipLeft));     //hips (pointing right)
                break;

            case PlayerJoint.Head:
                firstBone = Vector3.Subtract(skeleton.GetPositionOf(SkeletonMarkers.ShoulderCenter),
                                             skeleton.GetPositionOf(SkeletonMarkers.HipCenter)); //chest
                secondBone = Vector3.Subtract(skeleton.GetPositionOf(SkeletonMarkers.ShoulderCenter),
                                              skeleton.GetPositionOf(SkeletonMarkers.Head));     //head (pointing down)
                break;

            case PlayerJoint.ShoulderLeft:
                firstBone = Vector3.Subtract(skeleton.GetPositionOf(SkeletonMarkers.HipCenter),
                                             skeleton.GetPositionOf(SkeletonMarkers.ShoulderCenter)); //chest
                secondBone = Vector3.Subtract(skeleton.GetPositionOf(SkeletonMarkers.ElbowLeft),
                                              skeleton.GetPositionOf(SkeletonMarkers.ShoulderLeft));  //left upper arm
                break;

            case PlayerJoint.ElbowLeft:
                firstBone = Vector3.Subtract(skeleton.GetPositionOf(SkeletonMarkers.ShoulderLeft),
                                             skeleton.GetPositionOf(SkeletonMarkers.ElbowLeft));  //left upper arm
                secondBone = Vector3.Subtract(skeleton.GetPositionOf(SkeletonMarkers.WristLeft),
                                              skeleton.GetPositionOf(SkeletonMarkers.ElbowLeft)); //left lower arm
                break;

            case PlayerJoint.HipLeft:
                firstBone = Vector3.Subtract(skeleton.GetPositionOf(SkeletonMarkers.ShoulderCenter),
                                             skeleton.GetPositionOf(SkeletonMarkers.HipCenter)); //chest
                secondBone = Vector3.Subtract(skeleton.GetPositionOf(SkeletonMarkers.KneeLeft),
                                              skeleton.GetPositionOf(SkeletonMarkers.HipLeft));  //left upper leg
                break;

            case PlayerJoint.KneeLeft:
                firstBone = Vector3.Subtract(skeleton.GetPositionOf(SkeletonMarkers.HipLeft),
                                             skeleton.GetPositionOf(SkeletonMarkers.KneeLeft));  //left upper leg
                secondBone = Vector3.Subtract(skeleton.GetPositionOf(SkeletonMarkers.AnkleLeft),
                                              skeleton.GetPositionOf(SkeletonMarkers.KneeLeft)); //left lower leg
                break;

            case PlayerJoint.ShoulderRight:
                firstBone = Vector3.Subtract(skeleton.GetPositionOf(SkeletonMarkers.HipCenter),
                                             skeleton.GetPositionOf(SkeletonMarkers.ShoulderCenter)); //chest
                secondBone = Vector3.Subtract(skeleton.GetPositionOf(SkeletonMarkers.ElbowRight),
                                              skeleton.GetPositionOf(SkeletonMarkers.ShoulderRight)); //right upper arm
                break;

            case PlayerJoint.ShoulderCenter:
                firstBone = Vector3.Subtract(skeleton.GetPositionOf(SkeletonMarkers.ElbowRight),
                                             skeleton.GetPositionOf(SkeletonMarkers.ShoulderRight)); //right upper arm
                secondBone = Vector3.Subtract(skeleton.GetPositionOf(SkeletonMarkers.ElbowLeft),
                                              skeleton.GetPositionOf(SkeletonMarkers.ShoulderLeft)); //left upper arm
                break;

            case PlayerJoint.ElbowRight:
                firstBone = Vector3.Subtract(skeleton.GetPositionOf(SkeletonMarkers.ShoulderRight),
                                             skeleton.GetPositionOf(SkeletonMarkers.ElbowRight));  //right upper arm
                secondBone = Vector3.Subtract(skeleton.GetPositionOf(SkeletonMarkers.WristRight),
                                              skeleton.GetPositionOf(SkeletonMarkers.ElbowRight)); //right lower arm
                break;

            case PlayerJoint.HipRight:
                firstBone = Vector3.Subtract(skeleton.GetPositionOf(SkeletonMarkers.ShoulderCenter),
                                             skeleton.GetPositionOf(SkeletonMarkers.HipCenter)); //chest
                secondBone = Vector3.Subtract(skeleton.GetPositionOf(SkeletonMarkers.KneeRight),
                                              skeleton.GetPositionOf(SkeletonMarkers.HipRight)); //right upper leg
                break;

            case PlayerJoint.HipCenter:
                firstBone = Vector3.Subtract(skeleton.GetPositionOf(SkeletonMarkers.KneeLeft),
                                             skeleton.GetPositionOf(SkeletonMarkers.HipLeft));   //left upper leg
                secondBone = Vector3.Subtract(skeleton.GetPositionOf(SkeletonMarkers.KneeRight),
                                              skeleton.GetPositionOf(SkeletonMarkers.HipRight)); //right upper leg
                break;

            case PlayerJoint.KneeRight:
                firstBone = Vector3.Subtract(skeleton.GetPositionOf(SkeletonMarkers.HipRight),
                                             skeleton.GetPositionOf(SkeletonMarkers.KneeRight));  //left upper leg
                secondBone = Vector3.Subtract(skeleton.GetPositionOf(SkeletonMarkers.AnkleRight),
                                              skeleton.GetPositionOf(SkeletonMarkers.KneeRight)); //left lower leg
                break;

            default:
                firstBone  = Vector3.Zero;
                secondBone = Vector3.Zero;
                break;
            }
            #endregion

            #region check the angle
            currentAngle = MathHelper.ToDegrees(GetAngleFromVectors(firstBone, secondBone));

            if (Wishness > 0 && Math.Abs(currentAngle - WishedAngle) > Threshold)
            { //we want to reach this position and we currently don't -> wrong
                correctiveAngle = new SkeletonCorrectiveAngle(Angle, currentAngle, WishedAngle, Threshold,
                                                              Wishness, restrictionPlane, true);
            }
            else if (Wishness < 0 && Math.Abs(currentAngle - WishedAngle) < Threshold)
            { //we don't want to reach this position but we currently do -> wrong
                correctiveAngle = new SkeletonCorrectiveAngle(Angle, currentAngle, WishedAngle, Threshold,
                                                              Wishness, restrictionPlane, true);
            }
            #endregion

            #region check if in the right plane
            //the idea is, now, to check if the bones used are, more or less, in the choosen plane
            //we will mesure the angle between the normal to the plane and the normal to the two vectors which forms the angle
            //this angle will give us an information about the respect of this restriction plane

            //create the frontal plane
            Plane plane = new Plane(skeleton.GetPositionOf(SkeletonMarkers.HipRight),
                                    skeleton.GetPositionOf(SkeletonMarkers.HipLeft),
                                    skeleton.GetPositionOf(SkeletonMarkers.ShoulderCenter));

            //define the plane to use
            switch (restrictionPlane)
            {
            case physioPlanes.NONE:
                return;

            case physioPlanes.FRONTAL:
                break;

            case physioPlanes.SAGITTAL:
                //that's the plane defined by the normal to the frontal plane and the trunk vector
                //so we take the cross of these two vectors -> that define the normal to the plane
                //the distance from the origine along these normal doesn't matter for us
                plane = new Plane(Vector3.Cross(plane.Normal,
                                                Vector3.Subtract(skeleton.GetPositionOf(SkeletonMarkers.HipCenter),
                                                                 skeleton.GetPositionOf(SkeletonMarkers.ShoulderCenter))),
                                  0);
                break;

            case physioPlanes.TRANSVERSE:
                //the same principle is in application here
                plane = new Plane(Vector3.Cross(plane.Normal,
                                                Vector3.Subtract(skeleton.GetPositionOf(SkeletonMarkers.HipLeft),
                                                                 skeleton.GetPositionOf(SkeletonMarkers.HipRight))),
                                  0);
                break;

            default:
                break;
            }

            //basically, we are checking the angle with any plane parallel to the restriction plane
            float angleWithPlane = MathHelper.ToDegrees(GetAngleFromVectors(plane.Normal, Vector3.Cross(firstBone, secondBone)));
            float thresholdPlane = 15;

            if ((Math.Abs(angleWithPlane) > thresholdPlane) && (Math.Abs(angleWithPlane - 180) > thresholdPlane))
            { //we don't respect the restriction plane
                correctiveAngle = new SkeletonCorrectiveAngle(Angle, currentAngle, WishedAngle, Threshold,
                                                              Wishness, restrictionPlane, false);
            }
            #endregion

            if (correctiveAngle.Severity != 0) //so the correctiveAngle has been assigned
            {
                if (report.GeneralSeverity.CompareTo(Math.Abs(Wishness)) < 0)
                {
                    report.GeneralSeverity = Math.Abs(Wishness);
                }

                report.CorrectiveCollection.CorrectiveItems.Add(correctiveAngle);
            }
        }
Ejemplo n.º 3
0
        public void Check(ISkeleton skeleton, ISkeletonReport report)
        {
            if (Wishness == 0)
            {
                return;
            }

            //the positions we recieve from the Kinect are absolute positions ...
            //we have to use relative position if we want to apply a constraint position efficiently
            //-> use the hipcenter as (0,0,0) and convert the rest in relative positions
            //warning : the values are in meter !
            Vector3 currentRelativePosition;

            #region getCurrentRelativePosition
            switch (Joint)
            {
            case PlayerJoint.HipCenter:
                currentRelativePosition = Vector3.Zero;
                break;

            case PlayerJoint.AnkleLeft:
                currentRelativePosition = Vector3.Subtract(skeleton.GetPositionOf(SkeletonMarkers.AnkleLeft),
                                                           skeleton.GetPositionOf(SkeletonMarkers.HipCenter));
                break;

            case PlayerJoint.AnkleRight:
                currentRelativePosition = Vector3.Subtract(skeleton.GetPositionOf(SkeletonMarkers.AnkleRight),
                                                           skeleton.GetPositionOf(SkeletonMarkers.HipCenter));
                break;

            case PlayerJoint.ElbowLeft:
                currentRelativePosition = Vector3.Subtract(skeleton.GetPositionOf(SkeletonMarkers.ElbowLeft),
                                                           skeleton.GetPositionOf(SkeletonMarkers.HipCenter));
                break;

            case PlayerJoint.ElbowRight:
                currentRelativePosition = Vector3.Subtract(skeleton.GetPositionOf(SkeletonMarkers.ElbowRight),
                                                           skeleton.GetPositionOf(SkeletonMarkers.HipCenter));
                break;

            case PlayerJoint.FootLeft:
                currentRelativePosition = Vector3.Subtract(skeleton.GetPositionOf(SkeletonMarkers.FootLeft),
                                                           skeleton.GetPositionOf(SkeletonMarkers.HipCenter));
                break;

            case PlayerJoint.FootRight:
                currentRelativePosition = Vector3.Subtract(skeleton.GetPositionOf(SkeletonMarkers.FootRight),
                                                           skeleton.GetPositionOf(SkeletonMarkers.HipCenter));
                break;

            case PlayerJoint.HandLeft:
                currentRelativePosition = Vector3.Subtract(skeleton.GetPositionOf(SkeletonMarkers.HandLeft),
                                                           skeleton.GetPositionOf(SkeletonMarkers.HipCenter));
                break;

            case PlayerJoint.HandRight:
                currentRelativePosition = Vector3.Subtract(skeleton.GetPositionOf(SkeletonMarkers.HandRight),
                                                           skeleton.GetPositionOf(SkeletonMarkers.HipCenter));
                break;

            case PlayerJoint.Head:
                currentRelativePosition = Vector3.Subtract(skeleton.GetPositionOf(SkeletonMarkers.Head),
                                                           skeleton.GetPositionOf(SkeletonMarkers.HipCenter));
                break;

            case PlayerJoint.HipLeft:
                currentRelativePosition = Vector3.Subtract(skeleton.GetPositionOf(SkeletonMarkers.HipLeft),
                                                           skeleton.GetPositionOf(SkeletonMarkers.HipCenter));
                break;

            case PlayerJoint.HipRight:
                currentRelativePosition = Vector3.Subtract(skeleton.GetPositionOf(SkeletonMarkers.HipRight),
                                                           skeleton.GetPositionOf(SkeletonMarkers.HipCenter));
                break;

            case PlayerJoint.KneeLeft:
                currentRelativePosition = Vector3.Subtract(skeleton.GetPositionOf(SkeletonMarkers.KneeLeft),
                                                           skeleton.GetPositionOf(SkeletonMarkers.HipCenter));
                break;

            case PlayerJoint.KneeRight:
                currentRelativePosition = Vector3.Subtract(skeleton.GetPositionOf(SkeletonMarkers.KneeRight),
                                                           skeleton.GetPositionOf(SkeletonMarkers.HipCenter));
                break;

            case PlayerJoint.Sternum:
                currentRelativePosition = Vector3.Subtract(skeleton.GetPositionOf(SkeletonMarkers.Sternum),
                                                           skeleton.GetPositionOf(SkeletonMarkers.HipCenter));
                break;

            case PlayerJoint.ShoulderCenter:
                currentRelativePosition = Vector3.Subtract(skeleton.GetPositionOf(SkeletonMarkers.ShoulderCenter),
                                                           skeleton.GetPositionOf(SkeletonMarkers.HipCenter));
                break;

            case PlayerJoint.ShoulderLeft:
                currentRelativePosition = Vector3.Subtract(skeleton.GetPositionOf(SkeletonMarkers.ShoulderLeft),
                                                           skeleton.GetPositionOf(SkeletonMarkers.HipCenter));
                break;

            case PlayerJoint.ShoulderRight:
                currentRelativePosition = Vector3.Subtract(skeleton.GetPositionOf(SkeletonMarkers.ShoulderRight),
                                                           skeleton.GetPositionOf(SkeletonMarkers.HipCenter));
                break;

            case PlayerJoint.Spine:
                currentRelativePosition = Vector3.Subtract(skeleton.GetPositionOf(SkeletonMarkers.Spine),
                                                           skeleton.GetPositionOf(SkeletonMarkers.HipCenter));
                break;

            case PlayerJoint.WristLeft:
                currentRelativePosition = Vector3.Subtract(skeleton.GetPositionOf(SkeletonMarkers.WristLeft),
                                                           skeleton.GetPositionOf(SkeletonMarkers.HipCenter));
                break;

            case PlayerJoint.WristRight:
                currentRelativePosition = Vector3.Subtract(skeleton.GetPositionOf(SkeletonMarkers.WristRight),
                                                           skeleton.GetPositionOf(SkeletonMarkers.HipCenter));
                break;

            default:
                currentRelativePosition = Vector3.Zero;
                break;
            }
            #endregion

            //distance recieved in millimeters????
            currentRelativePosition /= 1000;

            //if the values are the same we do not test this direction
            bool xProblemAnalyze = MinPos.X != MaxPos.X;
            bool yProblemAnalyze = MinPos.Y != MaxPos.Y;
            bool zProblemAnalyze = MinPos.Z != MaxPos.Z;

            if (Wishness > 0)
            { //we want to reach this position
                if ((xProblemAnalyze &&
                     (currentRelativePosition.X < MinPos.X || currentRelativePosition.X > MaxPos.X)) ||
                    (yProblemAnalyze &&
                     (currentRelativePosition.Y < MinPos.Y || currentRelativePosition.Y > MaxPos.Y)) ||
                    (zProblemAnalyze &&
                     (currentRelativePosition.Z < MinPos.Z || currentRelativePosition.Z > MaxPos.Z)))
                {//well there's one problem but we don't really care which one here
                    if (report.GeneralSeverity.CompareTo(Math.Abs(Wishness)) < 0)
                    {
                        report.GeneralSeverity = Math.Abs(Wishness);
                    }

                    report.CorrectiveCollection.CorrectiveItems.Add(new SkeletonCorrecticePosition(
                                                                        Joint, currentRelativePosition,
                                                                        Vector3.Divide(Vector3.Add(MinPos, MaxPos), 2),
                                                                        MinPos, MaxPos, Wishness));
                }
            }
            else if (Wishness < 0)
            { //we don't want to reach this position
                bool xProblemDetected = false;
                bool yProblemDetected = false;
                bool zProblemDetected = false;

                if (xProblemAnalyze &&
                    currentRelativePosition.X > MinPos.X &&
                    currentRelativePosition.X < MaxPos.X)
                {//problem and we want to check it
                    xProblemDetected = true;
                }

                if (yProblemAnalyze &&
                    currentRelativePosition.Y > MinPos.Y &&
                    currentRelativePosition.Y < MaxPos.Y)
                {
                    yProblemDetected = true;
                }

                if (zProblemAnalyze &&
                    currentRelativePosition.Z > MinPos.Z &&
                    currentRelativePosition.Z < MaxPos.Z)
                {
                    zProblemAnalyze = true;
                }

                if ((xProblemDetected || !xProblemAnalyze) &&
                    (yProblemDetected || !yProblemAnalyze) &&
                    (zProblemDetected || !zProblemAnalyze) &&
                    (xProblemAnalyze || yProblemAnalyze || zProblemAnalyze))
                {//here we can be sure that, if we want to detect a problem on one or mutiple direction, it will be detected
                    //we are in a position which is not good !!
                    if (report.GeneralSeverity.CompareTo(Math.Abs(Wishness)) < 0)
                    {
                        report.GeneralSeverity = Math.Abs(Wishness);
                    }

                    report.CorrectiveCollection.CorrectiveItems.Add(new SkeletonCorrecticePosition(
                                                                        Joint, currentRelativePosition,
                                                                        Vector3.Divide(Vector3.Add(MinPos, MaxPos), 2),
                                                                        MinPos, MaxPos, Wishness));
                }
            }
        }