// 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; }