Sitting posture recognition with Kinect sensor

using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using Microsoft.Research.Kinect.Nui;

namespace RecognitionPose
{
    public static class Recogniton
    {

        /// <summary>
        /// Recognize current pose
        /// </summary>
        /// <param name="joints">Skelet positions</param>
        /// <param name="confidenceAngle">Maximum possible angle between left and right part body angles </param>
        /// <param name="standPoseFactor">Factor for recognition standing and sitting poses</param>
        /// <param name="isAutomaticChoiceAngle"> Chice flag of calculation angle between vectors HipShoulder and KneeShoulder
        /// if true nereast to sensor part body choiced else angle calculate as average between  left and right part body angles
        /// </param>
        /// <param name="angleShift">angle shift</param>
        /// <param name="recognitionHeadDistance">First time recognition distance of skelet head</param>
        /// <param name="leftAngle">left body part angle between vectors HipShoulder and KneeShoulder</param>
        /// <param name="rightAngle">left body part angle between vectors HipShoulder and KneeShoulder</param>
        /// <param name="currentHeadDistance">current Head Distance</param>
        /// <returns></returns>
        public static string RecognizePose(Microsoft.Research.Kinect.Nui.JointsCollection joints, double confidenceAngle, double standPoseFactor,
            bool isAutomaticChoiceAngle, double  angleShift,
            ref double recognitionHeadDistance, out double leftAngle, out double rightAngle, out double currentHeadDistance)
        {

            string result;

            rightAngle = CalculateAngle(joints[JointID.ShoulderLeft], joints[JointID.HipRight], joints[JointID.KneeRight]);
            leftAngle = CalculateAngle(joints[JointID.ShoulderRight], joints[JointID.HipLeft], joints[JointID.KneeLeft]);

            if (recognitionHeadDistance == 0)
            {

                recognitionHeadDistance = vectorNorm(joints[JointID.Head].Position.X, joints[JointID.Head].Position.Y,
                    joints[JointID.Head].Position.Z);
            }

            currentHeadDistance = vectorNorm(joints[JointID.Head].Position.X, joints[JointID.Head].Position.Y,
                    joints[JointID.Head].Position.Z);
         


            if (!(recognitionHeadDistance < standPoseFactor * currentHeadDistance))
                if (joints[JointID.HandLeft].Position.Y > joints[JointID.Head].Position.Y || joints[JointID.HandRight].Position.Y > joints[JointID.Head].Position.Y)
                {
                    if (joints[JointID.HandLeft].Position.Y > joints[JointID.Head].Position.Y)
                        result = "Raising left hand";
                    else
                        result = "Raising right hand";

                }
                else
                {
                    if (Math.Abs(rightAngle - leftAngle) < confidenceAngle)
                    {
                        double angle;
                        if (!isAutomaticChoiceAngle)
                            angle = (rightAngle + leftAngle) / 2;
                        else
                        {
                            double shLD = vectorNorm(joints[JointID.ShoulderLeft].Position.X, joints[JointID.ShoulderLeft].Position.Y,
                                joints[JointID.ShoulderLeft].Position.Z);
                            double shRD = vectorNorm(joints[JointID.ShoulderRight].Position.X, joints[JointID.ShoulderRight].Position.Y,
                               joints[JointID.ShoulderRight].Position.Z);
                            if (shLD > shRD)
                                angle = rightAngle;
                            else
                                angle = leftAngle;

                        }
                        angle = angle - angleShift;
                        leftAngle -= angleShift;
                        rightAngle -= angleShift;
                        result = RecognizeSittingPoseByAngle(angle);
                    }
                    else
                        result = "Undefined sitting pose";

                }
            else
            {
                if (joints[JointID.WristLeft].Position.Y > joints[JointID.Head].Position.Y || joints[JointID.WristRight].Position.Y > joints[JointID.Head].Position.Y)
                    result = "Raising hand";
                else
                    result = "Standing pose";
            }


            return result;

        }
        /// <summary>
        /// Calculate angle between vectors HipShoulder and KneeShoulder
        /// </summary>
        /// <param name="shoulder">shoulder position</param>
        /// <param name="hip">hip position</param>
        /// <param name="knee">knee position</param>
        /// <returns></returns>
        public static double CalculateAngle(Joint shoulder, Joint hip, Joint knee)
        {
            double angle = 0;
            double shrhX = shoulder.Position.X - hip.Position.X;
            double shrhY = shoulder.Position.Y - hip.Position.Y;
            double shrhZ = shoulder.Position.Z - hip.Position.Z;
            double hsl = vectorNorm(shrhX, shrhY, shrhZ);
            double unrhX = knee.Position.X - hip.Position.X;
            double unrhY = knee.Position.Y - hip.Position.Y;
            double unrhZ = knee.Position.Z - hip.Position.Z;
            double hul = vectorNorm(unrhX, unrhY, unrhZ);
            double mhshu = shrhX * unrhX + 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;
        }

        // RecognizeSittingPoseByAngle 
        /// <summary>
        ///  RecognizeSittingPoseByAngle
        ///  0 ~ 40	down	sleeping
        ///  40~80	down	non-concentrating
        ///  80~100	down	concentrating
        ///  	up	raising hand
        /// </summary>
        /// <param name="angle">angle between vectors HipShoulder and KneeShoulder</param>
        /// <returns></returns>
        public static string RecognizeSittingPoseByAngle(double angle)
        {
            string result = null;
            if (angle < 40)
                result = "Sleeping";
            if (40 < angle && angle < 80 || 100 < angle && angle < 180)
                result = "Non concentrating";
            if (80 <= angle && angle <= 100)
                result = "Concentrating";
            return result;
        }
        /// <summary>
        /// Euclidean norm of 3-component Vector
        /// </summary>
        /// <param name="x"></param>
        /// <param name="y"></param>
        /// <param name="z"></param>
        /// <returns></returns>
        public static double vectorNorm(double x, double y, double z)
        {

            return Math.Sqrt(Math.Pow(x, 2) + Math.Pow(y, 2) + Math.Pow(z, 2));

        }
    }
}