Kinect.KinectManager.ProcessBodyFrameData C# (CSharp) Method

ProcessBodyFrameData() private method

private ProcessBodyFrameData ( ) : void
return void
        private void ProcessBodyFrameData()
        {
            List<Int64> addedUsers = new List<Int64>();
            List<int> addedIndexes = new List<int>();

            List<Int64> lostUsers = new List<Int64>();
            lostUsers.AddRange(alUserIds);

            if ((autoHeightAngle == AutoHeightAngle.ShowInfoOnly || autoHeightAngle == AutoHeightAngle.AutoUpdateAndShowInfo) &&
               (sensorData.sensorHgtDetected != 0f || sensorData.sensorRotDetected.eulerAngles.x != 0f) &&
               calibrationText != null)
            {
                float angle = sensorData.sensorRotDetected.eulerAngles.x;
                angle = angle > 180f ? (angle - 360f) : angle;

                calibrationText.GetComponent<GUIText>().text = string.Format("Sensor Height: {0:F1} m, Angle: {1:F0} deg", sensorData.sensorHgtDetected, -angle);
            }

            if ((autoHeightAngle == AutoHeightAngle.AutoUpdate || autoHeightAngle == AutoHeightAngle.AutoUpdateAndShowInfo) &&
               (sensorData.sensorHgtDetected != 0f || sensorData.sensorRotDetected.eulerAngles.x != 0f))
            {
                float angle = sensorData.sensorRotDetected.eulerAngles.x;
                angle = angle > 180f ? (angle - 360f) : angle;
                sensorAngle = -angle;

                float height = sensorData.sensorHgtDetected > 0f ? sensorData.sensorHgtDetected : sensorHeight;
                sensorHeight = height;

                // update the kinect to world matrix
                Quaternion quatTiltAngle = Quaternion.Euler(-sensorAngle, 0.0f, 0.0f);
                kinectToWorld.SetTRS(new Vector3(0.0f, sensorHeight, 0.0f), quatTiltAngle, Vector3.one);
            }

            int trackedUsers = 0;

            for (int i = 0; i < sensorData.bodyCount; i++)
            {
                KinectInterop.BodyData bodyData = bodyFrame.bodyData[i];
                Int64 userId = bodyData.liTrackingID;

                if (bodyData.bIsTracked != 0 && Mathf.Abs(bodyData.position.z) >= minUserDistance &&
                   (maxUserDistance <= 0f || Mathf.Abs(bodyData.position.z) <= maxUserDistance) &&
                   (maxTrackedUsers < 0 || trackedUsers < maxTrackedUsers))
                {
                    // get the body position
                    Vector3 bodyPos = bodyData.position;

                    if (liPrimaryUserId == 0)
                    {
                        // check if this is the closest user
                        bool bClosestUser = true;
                        int iClosestUserIndex = i;

                        if (detectClosestUser)
                        {
                            for (int j = 0; j < sensorData.bodyCount; j++)
                            {
                                if (j != i)
                                {
                                    KinectInterop.BodyData bodyDataOther = bodyFrame.bodyData[j];

                                    if ((bodyDataOther.bIsTracked != 0) &&
                                        (Mathf.Abs(bodyDataOther.position.z) < Mathf.Abs(bodyPos.z)))
                                    {
                                        bClosestUser = false;
                                        iClosestUserIndex = j;
                                        break;
                                    }
                                }
                            }
                        }

                        if (bClosestUser)
                        {
                            // add the first or closest userId to the list of new users
                            if (!addedUsers.Contains(userId))
                            {
                                addedUsers.Add(userId);
                                addedIndexes.Add(iClosestUserIndex);
                                trackedUsers++;
                            }

                        }
                    }

                    // add userId to the list of new users
                    if (!addedUsers.Contains(userId))
                    {
                        addedUsers.Add(userId);
                        addedIndexes.Add(i);
                        trackedUsers++;
                    }

                    // convert Kinect positions to world positions
                    bodyFrame.bodyData[i].position = bodyPos;
                    //string debugText = String.Empty;

                    // process special cases
                    ProcessBodySpecialData(ref bodyData);

                    ////// 		turnaround mode start
                    // determine if the user is turned around
                    //float bodyTurnAngle = 0f;
                    //float neckTiltAngle = 0f;

                    if (allowTurnArounds && // sensorData.sensorInterface.IsFaceTrackingActive() &&
                       bodyData.joint[(int)KinectInterop.JointType.Neck].trackingState != KinectInterop.TrackingState.NotTracked)
                    {
                        //bodyTurnAngle = bodyData.bodyTurnAngle > 180f ? bodyData.bodyTurnAngle - 360f : bodyData.bodyTurnAngle;
                        //neckTiltAngle = Vector3.Angle(Vector3.up, bodyData.joint[(int)KinectInterop.JointType.Neck].direction.normalized);

                        //if(neckTiltAngle < 20f)
                        {
                            bool bTurnedAround = sensorData.sensorInterface.IsBodyTurned(ref bodyData);

                            if (bTurnedAround && bodyData.turnAroundFactor < 1f)
                            {
                                bodyData.turnAroundFactor += 5f * Time.deltaTime;
                                if (bodyData.turnAroundFactor > 1f)
                                    bodyData.turnAroundFactor = 1f;
                            }
                            else if (!bTurnedAround && bodyData.turnAroundFactor > 0f)
                            {
                                bodyData.turnAroundFactor -= 5f * Time.deltaTime;
                                if (bodyData.turnAroundFactor < 0f)
                                    bodyData.turnAroundFactor = 0f;
                            }

                            bodyData.isTurnedAround = (bodyData.turnAroundFactor >= 1f) ? true : (bodyData.turnAroundFactor <= 0f ? false : bodyData.isTurnedAround);
                            //bodyData.isTurnedAround = bTurnedAround;  // false;

                            //						RaiseHandListener handListener = RaiseHandListener.Instance;
                            //						if(handListener != null)
                            //						{
                            //							if(handListener.IsRaiseRightHand())
                            //							{
                            //								bodyData.isTurnedAround = true;
                            //							}
                            //							if(handListener.IsRaiseLeftHand())
                            //							{
                            //								bodyData.isTurnedAround = false;
                            //							}
                            //						}

                            if (bodyData.isTurnedAround)
                            {
                                // switch left and right joints
                                SwitchJointsData(ref bodyData, (int)KinectInterop.JointType.ShoulderLeft, (int)KinectInterop.JointType.ShoulderRight);
                                SwitchJointsData(ref bodyData, (int)KinectInterop.JointType.ElbowLeft, (int)KinectInterop.JointType.ElbowRight);
                                SwitchJointsData(ref bodyData, (int)KinectInterop.JointType.WristLeft, (int)KinectInterop.JointType.WristRight);
                                SwitchJointsData(ref bodyData, (int)KinectInterop.JointType.HandLeft, (int)KinectInterop.JointType.HandRight);
                                SwitchJointsData(ref bodyData, (int)KinectInterop.JointType.ThumbLeft, (int)KinectInterop.JointType.ThumbRight);
                                SwitchJointsData(ref bodyData, (int)KinectInterop.JointType.HandTipLeft, (int)KinectInterop.JointType.HandTipRight);

                                SwitchJointsData(ref bodyData, (int)KinectInterop.JointType.HipLeft, (int)KinectInterop.JointType.HipRight);
                                SwitchJointsData(ref bodyData, (int)KinectInterop.JointType.KneeLeft, (int)KinectInterop.JointType.KneeRight);
                                SwitchJointsData(ref bodyData, (int)KinectInterop.JointType.AnkleLeft, (int)KinectInterop.JointType.AnkleRight);
                                SwitchJointsData(ref bodyData, (int)KinectInterop.JointType.FootLeft, (int)KinectInterop.JointType.FootRight);

                                // recalculate the bone dirs and special data
                                KinectInterop.RecalcBoneDirs(sensorData, ref bodyData);
                                //ProcessBodySpecialData(ref bodyData);
                            }
                        }
                    }

                    //				if(allowTurnArounds && calibrationText)
                    //				{
                    //					calibrationText.GetComponent<GUIText>().text = string.Format("{0} - BodyAngle: {1:000}", 
                    //					    (!bodyData.isTurnedAround ? "FACE" : "BACK"), bodyData.bodyTurnAngle);
                    //				}

                    ////// 		turnaround mode end

                    // calculate world orientations of the body joints
                    CalculateJointOrients(ref bodyData);

                    if (sensorData != null && sensorData.sensorInterface != null)
                    {
                        // do sensor-specific fixes of joint positions and orientations
                        sensorData.sensorInterface.FixJointOrientations(sensorData, ref bodyData);
                    }

                    // filter orientation constraints
                    if (useBoneOrientationConstraints && boneConstraintsFilter != null)
                    {
                        boneConstraintsFilter.Constrain(ref bodyData);
                    }

                    lostUsers.Remove(userId);
                    bodyFrame.bodyData[i] = bodyData;
                }
                else
                {
                    // consider body as not tracked
                    bodyFrame.bodyData[i].bIsTracked = 0;
                }
            }

            // remove the lost users if any
            if (lostUsers.Count > 0)
            {
                foreach (Int64 userId in lostUsers)
                {
                    RemoveUser(userId);
                }

                lostUsers.Clear();
            }

            // calibrate newly detected users
            if (addedUsers.Count > 0)
            {
                for (int i = 0; i < addedUsers.Count; i++)
                {
                    Int64 userId = addedUsers[i];
                    int userIndex = addedIndexes[i];

                    CalibrateUser(userId, userIndex);
                }

                addedUsers.Clear();
                addedIndexes.Clear();
            }
        }
KinectManager