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();
}
}
