// averages the bodies in the list and returns the single merged body
private KinectInterop.BodyData GetMergedBody(List <KinectInterop.BodyData> alCloseBodies, int bodyIndex, ref List <string> lostUsers)
{
int jointCount = (int)KinectInterop.JointType.Count;
KinectInterop.BodyData mergedBody = new KinectInterop.BodyData(jointCount);
for (int j = 0; j < jointCount; j++)
{
int maxTrackingState = GetBodyJointMaxState(alCloseBodies, j);
CalcAverageBodyJoint(alCloseBodies, j, maxTrackingState, ref mergedBody);
}
mergedBody.liTrackingID = GetMergedBodyId(alCloseBodies, ref lostUsers);
mergedBody.iBodyIndex = bodyIndex;
KinectInterop.JointData pelvisData = mergedBody.joint[0];
mergedBody.bIsTracked = pelvisData.trackingState != KinectInterop.TrackingState.NotTracked;
//Debug.Log(string.Format("MBody {0} Id: {1}, pos: {2}, rot: {3}", bodyIndex, mergedBody.liTrackingID, pelvisData.position, pelvisData.normalRotation.eulerAngles));
mergedBody.kinectPos = pelvisData.kinectPos;
mergedBody.position = pelvisData.position;
mergedBody.orientation = pelvisData.orientation;
mergedBody.normalRotation = pelvisData.normalRotation;
mergedBody.mirroredRotation = pelvisData.mirroredRotation;
KinectInterop.CalcBodyJointDirs(ref mergedBody);
return(mergedBody);
}
// check if the calibration pose is complete for given user
protected virtual bool CheckForCalibrationPose(ulong UserId, int bodyIndex, GestureType calibrationGesture,
KinectGestureManager gestureManager, ref KinectInterop.BodyData[] alTrackedBodies)
{
if (calibrationGesture == GestureType.None)
{
return(true);
}
if (!gestureManager)
{
return(false);
}
KinectGestureManager.GestureData gestureData = playerCalibrationData.ContainsKey(UserId) ?
playerCalibrationData[UserId] : new KinectGestureManager.GestureData();
// init gesture data if needed
if (gestureData.userId != UserId)
{
gestureData.userId = UserId;
gestureData.gesture = calibrationGesture;
gestureData.state = 0;
gestureData.timestamp = Time.realtimeSinceStartup;
gestureData.joint = 0;
gestureData.progress = 0f;
gestureData.complete = false;
gestureData.cancelled = false;
}
// get joint positions and tracking
int iAllJointsCount = (int)KinectInterop.JointType.Count;
bool[] playerJointsTracked = new bool[iAllJointsCount];
Vector3[] playerJointsPos = new Vector3[iAllJointsCount];
int[] aiNeededJointIndexes = gestureManager.GetNeededJointIndexes();
int iNeededJointsCount = aiNeededJointIndexes.Length;
for (int i = 0; i < iNeededJointsCount; i++)
{
int joint = aiNeededJointIndexes[i];
if (joint >= 0)
{
KinectInterop.JointData jointData = alTrackedBodies[bodyIndex].joint[joint];
playerJointsTracked[joint] = jointData.trackingState != KinectInterop.TrackingState.NotTracked;
playerJointsPos[joint] = jointData.kinectPos;
if (!playerJointsTracked[joint] && (joint == (int)KinectInterop.JointType.Neck))
{
KinectInterop.JointData lShoulderData = alTrackedBodies[bodyIndex].joint[(int)KinectInterop.JointType.ShoulderLeft];
KinectInterop.JointData rShoulderData = alTrackedBodies[bodyIndex].joint[(int)KinectInterop.JointType.ShoulderRight];
if (lShoulderData.trackingState != KinectInterop.TrackingState.NotTracked && rShoulderData.trackingState != KinectInterop.TrackingState.NotTracked)
{
playerJointsTracked[joint] = true;
playerJointsPos[joint] = (lShoulderData.kinectPos + rShoulderData.kinectPos) / 2f;
}
}
}
}
// estimate the gesture progess
gestureManager.CheckForGesture(UserId, ref gestureData, Time.realtimeSinceStartup,
ref playerJointsPos, ref playerJointsTracked);
playerCalibrationData[UserId] = gestureData;
// check if gesture is complete
if (gestureData.complete)
{
gestureData.userId = 0;
playerCalibrationData[UserId] = gestureData;
return(true);
}
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;
}
