// gets the position and rotation of the given user
private bool GetUserPose(KinectInterop.SensorData sensorData, int uIndex, ref Vector3 userPos, ref Quaternion userRot)
{
if (sensorData != null && sensorData.trackedBodiesCount > uIndex)
{
KinectInterop.BodyData bodyData = sensorData.alTrackedBodies[uIndex];
Vector3 spaceScale = sensorData.sensorSpaceScale;
KinectInterop.TrackingState trackingState = bodyData.joint != null && bodyData.joint.Length > 0 ? bodyData.joint[0].trackingState : KinectInterop.TrackingState.NotTracked;
if (trackingState == KinectInterop.TrackingState.Tracked || trackingState == KinectInterop.TrackingState.HighConf)
{
userPos = new Vector3(bodyData.kinectPos.x * spaceScale.x, bodyData.kinectPos.y * spaceScale.y, bodyData.kinectPos.z);
userRot = initialUserRot * bodyData.mirroredRotation;
}
return(true);
}
return(false);
}
// gets the position and rotation of the given user
private bool GetUserPose(int sIndex, KinectInterop.SensorData sensorData, int uIndex, ulong lastBodyTime, ref UserBodyPose userPose)
{
if (sensorData != null && sensorData.trackedBodiesCount > uIndex && (lastBodyTime == 0 || sensorData.lastBodyFrameTime != lastBodyTime))
{
KinectInterop.BodyData bodyData = sensorData.alTrackedBodies[uIndex];
Vector3 spaceScale = sensorData.sensorSpaceScale;
userPose.bodyTime = sensorData.lastBodyFrameTime;
if (lastBodyTime != 0)
{
//Debug.Log(string.Format("S{0}, Time: {1}, Last: {2}", sIndex, sensorData.lastBodyFrameTime, lastBodyTime));
}
if (bodyData.joint != null && bodyData.joint.Length > 0)
{
for (int j = 0; j < numJoints; j++)
{
int joint = trackedBodyJoint[j];
KinectInterop.TrackingState trackingState = bodyData.joint[joint].trackingState;
userPose.jointTracked[j] = (trackingState == KinectInterop.TrackingState.Tracked) || (trackingState == KinectInterop.TrackingState.HighConf);
if (userPose.jointTracked[j])
{
Vector3 kinectPos = bodyData.joint[joint].kinectPos;
userPose.jointPos[j] = new Vector3(kinectPos.x * spaceScale.x, kinectPos.y * spaceScale.y, kinectPos.z);
userPose.jointRot[j] = initialUserRot * bodyData.joint[joint].mirroredRotation;
//Debug.Log(string.Format("S{0}: {1} - p:{2}, r:{3}", sIndex, (KinectInterop.JointType)joint, userPose.jointPos[j], userPose.jointRot[j].eulerAngles));
}
}
}
return(userPose.jointTracked[0]);
}
return(false);
}
// returns averaged position of a body joint in a list of bodies
private void CalcAverageBodyJoint(List <KinectInterop.BodyData> alBodyList, int jointIndex, int maxTrackingState,
ref KinectInterop.BodyData bodyData)
{
Vector3 avgJointPos = Vector3.zero;
Vector3 firstKinectPos = Vector3.zero;
Quaternion avgJointRot = Quaternion.identity;
Quaternion firstJointOri = Quaternion.identity;
Quaternion firstJointRot = Quaternion.identity;
float x = 0f, y = 0f, z = 0f, w = 0f;
float jointAvgCount = 0f;
int bodyCount = alBodyList.Count;
//bool bJointOnePassOnly = Array.Exists(_OnePassJoints, j => j == jointIndex);
for (int i = 0; i < bodyCount; i++)
{
////if (bJointOnePassOnly)
//if (jointIndex == (int)KinectInterop.JointType.WristLeft)
//{
// Debug.Log(string.Format("{0:F3} {1}: {2}_{3}, state: {4}, pos: {5}, rot: {6}", Time.time, (KinectInterop.JointType)jointIndex,
// alBodyList[i].sensorIndex, alBodyList[i].liTrackingID, alBodyList[i].joint[jointIndex].trackingState,
// alBodyList[i].joint[jointIndex].position, alBodyList[i].joint[jointIndex].mirroredRotation.eulerAngles));
//}
//if (SINGLE_SENSOR_INDEX >= 0 && alBodyList[i].sensorIndex != SINGLE_SENSOR_INDEX)
// continue;
KinectInterop.TrackingState jointState = alBodyList[i].joint[jointIndex].trackingState;
//if ((int)jointState == maxTrackingState)
if (jointState != KinectInterop.TrackingState.NotTracked)
{
Quaternion jointRot = alBodyList[i].joint[jointIndex].normalRotation;
if (avgJointPos == Vector3.zero)
{
firstKinectPos = alBodyList[i].joint[jointIndex].kinectPos;
firstJointOri = alBodyList[i].joint[jointIndex].orientation;
firstJointRot = jointRot;
}
if (jointIndex == 0)
{
//Debug.Log(string.Format("Body Id: {0}_{1}, pos: {2}, rot: {3}", alBodyList[i].sensorIndex, alBodyList[i].liTrackingID, alBodyList[i].joint[jointIndex].position, alBodyList[i].joint[jointIndex].normalRotation.eulerAngles));
}
float jointWeight = 1f; // jointState != KinectInterop.TrackingState.Inferred ? 1f : 0.5f;
avgJointPos += alBodyList[i].joint[jointIndex].position * jointWeight;
if (Quaternion.Dot(jointRot, firstJointRot) < 0f)
{
jointRot = new Quaternion(-jointRot.x, -jointRot.y, -jointRot.z, -jointRot.w); // inverse the sign
}
if (jointWeight < 0.9f)
{
jointRot = Quaternion.Slerp(Quaternion.identity, jointRot, jointWeight);
}
x += jointRot.x; y += jointRot.y; z += jointRot.z; w += jointRot.w;
jointAvgCount += jointWeight; // (jointState != KinectInterop.TrackingState.Inferred ? 1f : 0.5f);
//if(bJointOnePassOnly)
// break;
}
}
if (jointAvgCount > 0)
{
float addDet = 1f / jointAvgCount;
avgJointPos = avgJointPos * addDet;
x *= addDet; y *= addDet; z *= addDet; w *= addDet;
float lengthD = 1.0f / (w * w + x * x + y * y + z * z);
x *= lengthD; y *= lengthD; z *= lengthD; w *= lengthD;
avgJointRot = new Quaternion(x, y, z, w);
}
KinectInterop.JointData jointData = bodyData.joint[jointIndex];
jointData.trackingState = (KinectInterop.TrackingState)maxTrackingState;
jointData.kinectPos = firstKinectPos;
jointData.position = avgJointPos;
jointData.orientation = firstJointOri;
jointData.normalRotation = avgJointRot;
Vector3 mirroredRot = avgJointRot.eulerAngles;
mirroredRot.y = -mirroredRot.y;
mirroredRot.z = -mirroredRot.z;
jointData.mirroredRotation = Quaternion.Euler(mirroredRot);
bodyData.joint[jointIndex] = jointData;
}
