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