// calculates joint velocities in a body frame private static void CalcBodyFrameJointVels(SensorData sensorData, ref BodyFrameData bodyFrame) { // calculate the inter-frame time float frameTime = (float)(bodyFrame.liRelativeTime - bodyFrame.liPreviousTime) / 100000000000f; for (int i = 0; i < sensorData.bodyCount; i++) { if (bodyFrame.bodyData [i].bIsTracked != 0) { for (int j = 0; j < sensorData.jointCount; j++) { KinectInterop.JointData jointData = bodyFrame.bodyData [i].joint [j]; int p = (int)sensorData.sensorInterface.GetParentJoint((KinectInterop.JointType)j); Vector3 parentPos = bodyFrame.bodyData [i].joint [p].position; jointData.posRel = jointData.position - parentPos; jointData.posVel = frameTime > 0f ? (jointData.position - jointData.posPrev) / frameTime : Vector3.zero; jointData.posPrev = jointData.position; bodyFrame.bodyData [i].joint [j] = jointData; } } } }
public void Start() { currentFrame = new BodyFrameData(); sender = new IPEndPoint(IPAddress.Any, 1); client = new UdpClient(); client.Client.Bind(new IPEndPoint(IPAddress.Any, port)); discoveryClient = new UdpClient(); discoveryClient.Client.Bind(new IPEndPoint(IPAddress.Any, discoveryPort)); discoveryClient.EnableBroadcast = true; BinaryWriter writer = new BinaryWriter(new MemoryStream()); string localIPStr = GetLocalIPAddress(); writer.Write((byte)0x00); writer.Write(localIPStr); writer.Write(port); discoveryPayload = (writer.BaseStream as MemoryStream).ToArray(); foreach (var ki in kinectInfo) { kinectInfoTable.Add(ki.id, ki); } }
// Polls for new skeleton data public static bool PollBodyFrame(SensorData sensorData, ref BodyFrameData bodyFrame, ref Matrix4x4 kinectToWorld, bool bIgnoreJointZ) { bool bNewFrame = false; if (sensorData.sensorInterface != null) { bNewFrame = sensorData.sensorInterface.PollBodyFrame(sensorData, ref bodyFrame, ref kinectToWorld, bIgnoreJointZ); if (bNewFrame) { if (bodyFrame.bTurnAnalisys && bodyFrame.liPreviousTime > 0) { CalcBodyFrameJointVels(sensorData, ref bodyFrame); } CalcBodyFrameBoneDirs(sensorData, ref bodyFrame); // frame is ready lock (sensorData.bodyFrameLock) { sensorData.bodyFrameReady = true; } } } return(bNewFrame); }
// Calculates all valid bone directions in a body frame private static void CalcBodyFrameBoneDirs(SensorData sensorData, ref BodyFrameData bodyFrame) { for (int i = 0; i < sensorData.bodyCount; i++) { if (bodyFrame.bodyData [i].bIsTracked != 0) { for (int j = 0; j < sensorData.jointCount; j++) { if (j == 0) { bodyFrame.bodyData [i].joint [j].direction = Vector3.zero; } else { int jParent = (int)sensorData.sensorInterface.GetParentJoint((KinectInterop.JointType)j); if (bodyFrame.bodyData [i].joint [j].trackingState != TrackingState.NotTracked && bodyFrame.bodyData [i].joint [jParent].trackingState != TrackingState.NotTracked) { bodyFrame.bodyData [i].joint [j].direction = bodyFrame.bodyData [i].joint [j].position - bodyFrame.bodyData [i].joint [jParent].position; } } } } } }
void Update() { //UpdateMatrix(); while (client.Available > 0) { byte[] arr = client.Receive(ref sender); switch (arr[0]) { //process body packet case 0xFF: if (!playingSession) { HandleBodyPacket(arr); } break; case 0xFD: //heartbeat if (!playingSession) { HandleHearbeatPacket(arr); } break; default: continue; } } while (discoveryClient.Available > 0) { HandleDiscoveryPacket(discoveryClient.Receive(ref sender)); } List <byte> blankIdUpdates = new List <byte>(); foreach (var pair in streamBlankTable) { if (Time.time > pair.Value) { if (OnBodyFrameDataReceived != null) { OnBodyFrameDataReceived(BodyFrameData.GetBlankWithID(pair.Key)); } blankIdUpdates.Add(pair.Key); } } foreach (var b in blankIdUpdates) { streamBlankTable[b] = Time.time + blankFrameDelay; } }
void HandleBodyPacket(byte[] data) { lastPacketTime = Time.time; byte kinectId = data[1]; if (currentFrames.ContainsKey(kinectId)) { currentFrame = GetCurrentFrame(kinectId); } else { currentFrame = new BodyFrameData(kinectId); currentFrames.Add(kinectId, currentFrame); } //deprecated currentFrame.Process(data); streamStateTable[currentFrame.kinectId] = currentFrame.streamState; streamAddressTable[currentFrame.kinectId] = sender.Address.ToString(); //TODO: maybe timestamp it if (kinectInfoTable.ContainsKey(currentFrame.kinectId)) { var ki = kinectInfoTable[currentFrame.kinectId]; ki.active = true; ki.lastDataTime = Time.time; if (ki.filter != null) { ki.filter.Validate(ki, currentFrame.bodyData); } } if (OnBodyFrameDataReceived != null) { OnBodyFrameDataReceived(currentFrame); } if (recordingSession) { //sessionWriter.Write(Time.realtimeSinceStartup - sessionStartTime); //sessionWriter.Write(data.Length); //sessionWriter.Write(data); } streamBlankTable[currentFrame.kinectId] = Time.time + blankFrameDelay; //nextBlankFrameTime = Time.time + blankFrameDelay; }
public static BodyFrameData Deserialize(BodyFrameDataMsg message) { var frame = new BodyFrameData() { RelativeTime = new TimeSpan(message.RelativeTime), Bodies = new Body[message.BodiesLength], }; for (int i = 0; i < message.BodiesLength; i++) { frame.Bodies[i] = Body.Deserialize(message.GetBodies(i)); } return frame; }
// Polls for new skeleton data /// <summary> /// 轮询新的骨架数据 /// </summary> /// <param name="sensorData">传感器数据</param> /// <param name="bodyFrame">身体帧</param> /// <param name="kinectToWorld">Kinect世界矩阵</param> /// <returns></returns> public static bool PollBodyFrame(SensorData sensorData, ref BodyFrameData bodyFrame, ref Matrix4x4 kinectToWorld) { bool bNewFrame = false; if (sensorData.sensorInterface != null) { //获取新的身体帧 bNewFrame = sensorData.sensorInterface.PollBodyFrame(sensorData, ref bodyFrame, ref kinectToWorld); if (bNewFrame) { for (int i = 0; i < sensorData.bodyCount; i++) { if (bodyFrame.bodyData[i].bIsTracked != 0) { // calculate joint directions //计算关节方向 for (int j = 0; j < sensorData.jointCount; j++) { if (j == 0) { bodyFrame.bodyData[i].joint[j].direction = Vector3.zero; } else { //获取关节的父节点 int jParent = (int)sensorData.sensorInterface.GetParentJoint(bodyFrame.bodyData[i].joint[j].jointType); //当前关节和父关节都不是未跟踪时,则进行处理 if (bodyFrame.bodyData[i].joint[j].trackingState != TrackingState.NotTracked && bodyFrame.bodyData[i].joint[jParent].trackingState != TrackingState.NotTracked) { //设置身体数据关节方向 bodyFrame.bodyData[i].joint[j].direction = bodyFrame.bodyData[i].joint[j].position - bodyFrame.bodyData[i].joint[jParent].position; } } } } } } } return(bNewFrame); }
// Polls for new skeleton data public static bool PollBodyFrame(SensorData sensorData, ref BodyFrameData bodyFrame, ref Matrix4x4 kinectToWorld) { bool bNewFrame = false; if(sensorData.sensorInterface != null) { bNewFrame = sensorData.sensorInterface.PollBodyFrame(sensorData, ref bodyFrame, ref kinectToWorld); if(bNewFrame) { for(int i = 0; i < sensorData.bodyCount; i++) { if(bodyFrame.bodyData[i].bIsTracked != 0) { // calculate joint directions for(int j = 0; j < sensorData.jointCount; j++) { if(j == 0) { bodyFrame.bodyData[i].joint[j].direction = Vector3.zero; } else { int jParent = (int)sensorData.sensorInterface.GetParentJoint(bodyFrame.bodyData[i].joint[j].jointType); if(bodyFrame.bodyData[i].joint[j].trackingState != TrackingState.NotTracked && bodyFrame.bodyData[i].joint[jParent].trackingState != TrackingState.NotTracked) { bodyFrame.bodyData[i].joint[j].direction = bodyFrame.bodyData[i].joint[j].position - bodyFrame.bodyData[i].joint[jParent].position; } } } } } } } return bNewFrame; }
// converts current body frame to a single csv line. returns empty string if there is no new data public static string GetBodyFrameAsCsv(SensorData sensorData, ref BodyFrameData bodyFrame, ref long liRelTime, ref float fUnityTime) { // check for invalid sensor data and if the frame is still the same if (sensorData == null) return string.Empty; if (bodyFrame.liRelativeTime == liRelTime) return string.Empty; // create the output string StringBuilder sbBuf = new StringBuilder(); const char delimiter = ','; sbBuf.Append("kb").Append(delimiter); sbBuf.Append(bodyFrame.liRelativeTime).Append(delimiter); liRelTime = bodyFrame.liRelativeTime; fUnityTime = Time.time; sbBuf.Append(sensorData.bodyCount).Append(delimiter); sbBuf.Append(sensorData.jointCount).Append(delimiter); // add information for all bodies for (int i = 0; i < sensorData.bodyCount; i++) { sbBuf.Append(bodyFrame.bodyData[i].bIsTracked).Append(delimiter); if (bodyFrame.bodyData[i].bIsTracked != 0) { // add information for the tracked body - body-id and joints sbBuf.Append(bodyFrame.bodyData[i].liTrackingID).Append(delimiter); for (int j = 0; j < sensorData.jointCount; j++) { KinectInterop.JointData jointData = bodyFrame.bodyData[i].joint[j]; sbBuf.Append((int)jointData.trackingState).Append(delimiter); if (jointData.trackingState != TrackingState.NotTracked) { sbBuf.AppendFormat("{0:F3}", jointData.kinectPos.x).Append(delimiter); sbBuf.AppendFormat("{0:F3}", jointData.kinectPos.y).Append(delimiter); sbBuf.AppendFormat("{0:F3}", jointData.kinectPos.z).Append(delimiter); } } } } // remove the last delimiter if (sbBuf.Length > 0 && sbBuf[sbBuf.Length - 1] == delimiter) { sbBuf.Remove(sbBuf.Length - 1, 1); } return sbBuf.ToString(); }
// Calculates all valid bone directions in a body frame private static void CalcBodyFrameBoneDirs(SensorData sensorData, ref BodyFrameData bodyFrame) { for (int i = 0; i < sensorData.bodyCount; i++) { if (bodyFrame.bodyData[i].bIsTracked != 0) { for (int j = 0; j < sensorData.jointCount; j++) { if (j == 0) { bodyFrame.bodyData[i].joint[j].direction = Vector3.zero; } else { int jParent = (int)sensorData.sensorInterface.GetParentJoint((KinectInterop.JointType)j); if (bodyFrame.bodyData[i].joint[j].trackingState != TrackingState.NotTracked && bodyFrame.bodyData[i].joint[jParent].trackingState != TrackingState.NotTracked) { bodyFrame.bodyData[i].joint[j].direction = bodyFrame.bodyData[i].joint[j].position - bodyFrame.bodyData[i].joint[jParent].position; } } } } } }
// sets current body frame from the given csv line. returns true on success, false otherwise public static bool SetBodyFrameFromCsv(string sCsvLine, SensorData sensorData, ref BodyFrameData bodyFrame, ref Matrix4x4 kinectToWorld) { // check for invalid sensor data and for same frame time if (sensorData == null) return false; if (sCsvLine.Length == 0) return false; // split the csv line in parts char[] delimiters = { ',' }; string[] alCsvParts = sCsvLine.Split(delimiters); if (alCsvParts.Length < 4) return false; // check the id, body count & joint count int bodyCount = 0, jointCount = 0; int.TryParse(alCsvParts[2], out bodyCount); int.TryParse(alCsvParts[3], out jointCount); long liRelTime = 0; long.TryParse(alCsvParts[1], out liRelTime); if (alCsvParts[0] != "kb" || bodyCount == 0 || jointCount == 0 || liRelTime == 0) return false; if (bodyCount != sensorData.bodyCount || jointCount != sensorData.jointCount) return false; // update body frame data bodyFrame.liPreviousTime = bodyFrame.liRelativeTime; bodyFrame.liRelativeTime = liRelTime; int iIndex = 4; for (int i = 0; i < sensorData.bodyCount; i++) { if (alCsvParts.Length < (iIndex + 1)) return false; // update the tracked-flag and body id short bIsTracked = 0; long liTrackingID = 0; short.TryParse(alCsvParts[iIndex], out bIsTracked); iIndex++; if (bIsTracked != 0 && alCsvParts.Length >= (iIndex + 1)) { long.TryParse(alCsvParts[iIndex], out liTrackingID); iIndex++; if (liTrackingID == 0) { bIsTracked = 0; } } bodyFrame.bodyData[i].bIsTracked = bIsTracked; bodyFrame.bodyData[i].liTrackingID = liTrackingID; if (bIsTracked != 0) { // update joints' data for (int j = 0; j < sensorData.jointCount; j++) { KinectInterop.JointData jointData = bodyFrame.bodyData[i].joint[j]; int iTrackingState = 0; if (alCsvParts.Length >= (iIndex + 1)) { int.TryParse(alCsvParts[iIndex], out iTrackingState); iIndex++; jointData.trackingState = (KinectInterop.TrackingState)iTrackingState; if (iTrackingState != (int)TrackingState.NotTracked && alCsvParts.Length >= (iIndex + 3)) { float x = 0f, y = 0f, z = 0f; float.TryParse(alCsvParts[iIndex], out x); float.TryParse(alCsvParts[iIndex + 1], out y); float.TryParse(alCsvParts[iIndex + 2], out z); iIndex += 3; jointData.kinectPos = new Vector3(x, y, z); } else { jointData.kinectPos = Vector3.zero; } jointData.position = kinectToWorld.MultiplyPoint3x4(jointData.kinectPos); jointData.orientation = Quaternion.identity; if (j == 0) { // set body position bodyFrame.bodyData[i].position = jointData.position; bodyFrame.bodyData[i].orientation = jointData.orientation; } } bodyFrame.bodyData[i].joint[j] = jointData; } } } // calculate bone directions CalcBodyFrameBoneDirs(sensorData, ref bodyFrame); return true; }
// sets current hand data from the given csv line. returns true on success, false otherwise public static bool SetHandsDataFromCsv(string sCsvLine, SensorData sensorData, ref BodyFrameData bodyFrame) { // check for invalid sensor data and for same frame time if(sensorData == null) return false; if(sCsvLine.Length == 0) return false; // split the csv line in parts char[] delimiters = { ',' }; string[] alCsvParts = sCsvLine.Split(delimiters); if(alCsvParts.Length < 3) return false; // check the id & body count int bodyCount = 0; int.TryParse(alCsvParts[2], out bodyCount); long liRelTime = 0; long.TryParse(alCsvParts[1], out liRelTime); if(alCsvParts[0] != "kh" || bodyCount == 0 || liRelTime == 0) return false; // check rel time if(bodyFrame.liRelativeTime > liRelTime) return false; int iIndex = 3; for(int i = 0; i < bodyCount; i++) { if(alCsvParts.Length < (iIndex + 1)) return false; // update the tracked-flag and body id short bIsTracked = 0; long liTrackingID = 0; short.TryParse(alCsvParts[iIndex], out bIsTracked); iIndex++; if(bIsTracked != 0 && alCsvParts.Length >= (iIndex + 5)) { long.TryParse(alCsvParts[iIndex], out liTrackingID); iIndex++; int lhState = 0, lhConf = 0, rhState = 0, rhConf = 0; int.TryParse(alCsvParts[iIndex++], out lhState); int.TryParse(alCsvParts[iIndex++], out lhConf); int.TryParse(alCsvParts[iIndex++], out rhState); int.TryParse(alCsvParts[iIndex++], out rhConf); if(i < sensorData.bodyCount && bodyFrame.bodyData[i].bIsTracked != 0 && bodyFrame.bodyData[i].liTrackingID == liTrackingID) { bodyFrame.bodyData[i].leftHandState = (HandState)lhState; bodyFrame.bodyData[i].leftHandConfidence = (TrackingConfidence)lhConf; bodyFrame.bodyData[i].rightHandState = (HandState)rhState; bodyFrame.bodyData[i].rightHandConfidence = (TrackingConfidence)rhConf; } } } return true; }
// Polls for new skeleton data public static bool PollBodyFrame(SensorData sensorData, ref BodyFrameData bodyFrame, ref Matrix4x4 kinectToWorld, bool bIgnoreJointZ) { bool bNewFrame = false; if(sensorData.sensorInterface != null) { // wait for buffer release while(sensorData.bodyFrameReady) { System.Threading.Thread.Sleep(1); } bNewFrame = sensorData.sensorInterface.PollBodyFrame(sensorData, ref bodyFrame, ref kinectToWorld, bIgnoreJointZ); if(bNewFrame) { CalcBodyFrameBoneDirs(sensorData, ref bodyFrame); // frame is ready lock(sensorData.bodyFrameLock) { sensorData.bodyFrameReady = true; } } } return bNewFrame; }
// converts current hand data to a single csv line. returns empty string if there is no new data public static string GetHandsDataAsCsv(SensorData sensorData, ref BodyFrameData bodyFrame, ref long liRelTime) { // check for invalid sensor data and if the frame is still the same if(sensorData == null) return string.Empty; // if(bodyFrame.liRelativeTime == liRelTime) // return string.Empty; // create the output string StringBuilder sbBuf = new StringBuilder(); const char delimiter = ','; sbBuf.Append("kh").Append(delimiter); sbBuf.Append(bodyFrame.liRelativeTime).Append(delimiter); sbBuf.Append(sensorData.bodyCount).Append(delimiter); // add information for all bodies for(int i = 0; i < sensorData.bodyCount; i++) { sbBuf.Append(bodyFrame.bodyData[i].bIsTracked).Append(delimiter); if(bodyFrame.bodyData[i].bIsTracked != 0) { // add information for the tracked body - body-id and hand states sbBuf.Append(bodyFrame.bodyData[i].liTrackingID).Append(delimiter); sbBuf.AppendFormat("{0}", (int)bodyFrame.bodyData[i].leftHandState).Append(delimiter); sbBuf.AppendFormat("{0}", (int)bodyFrame.bodyData[i].leftHandConfidence).Append(delimiter); sbBuf.AppendFormat("{0}", (int)bodyFrame.bodyData[i].rightHandState).Append(delimiter); sbBuf.AppendFormat("{0}", (int)bodyFrame.bodyData[i].rightHandConfidence).Append(delimiter); } } // remove the last delimiter if(sbBuf.Length > 0 && sbBuf[sbBuf.Length - 1] == delimiter) { sbBuf.Remove(sbBuf.Length - 1, 1); } return sbBuf.ToString(); }
// Polls for new skeleton data public static bool PollBodyFrame(SensorData sensorData, ref BodyFrameData bodyFrame, ref Matrix4x4 kinectToWorld) { bool bNewFrame = false; if (sensorData.sensorInterface != null) { bNewFrame = sensorData.sensorInterface.PollBodyFrame(sensorData, ref bodyFrame, ref kinectToWorld); if (bNewFrame) { CalcBodyFrameBoneDirs(sensorData, ref bodyFrame); } } return bNewFrame; }
// Polls for new skeleton data public static bool PollBodyData(SensorData sensorData, ref BodyFrameData bodyFrame) { bool bNewFrame = false; // BodyFrameData tempBodyFrame = new BodyFrameData(); // int hr = GetBodyFrameData(ref tempBodyFrame, true, true); // //// if((hr == 0) && ((sensorData.colorImage == null) || (tempBodyFrame.liRelativeTime >= sensorData.lastColorFrameTime)) && //// ((sensorData.depthImage == null) || (tempBodyFrame.liRelativeTime >= sensorData.lastDepthFrameTime))) // { // if(hr == 0 /**&& (bodyFrame.liRelativeTime > lastFrameTime)*/) // { // bodyFrame = tempBodyFrame; // sensorData.lastBodyFrameTime = bodyFrame.liRelativeTime; // bNewFrame = true; // } // } if (sensorData.bodyFrameReader != null) { var frame = sensorData.bodyFrameReader.AcquireLatestFrame(); if (frame != null) { frame.GetAndRefreshBodyData(sensorData.bodyData); bodyFrame.liRelativeTime = frame.RelativeTime; frame.Dispose(); frame = null; for (int i = 0; i < sensorData.bodyCount; i++) { Body body = sensorData.bodyData[i]; if (body == null) { bodyFrame.bodyData[i].bIsTracked = 0; continue; } bodyFrame.bodyData[i].bIsTracked = (short)(body.IsTracked ? 1 : 0); if (body.IsTracked) { bodyFrame.bodyData[i].liTrackingID = (long)body.TrackingId; for (int j = 0; j < Constants.JointCount; j++) { Windows.Kinect.Joint joint = body.Joints[(JointType)j]; JointData jointData = bodyFrame.bodyData[i].joint[j]; jointData.jointType = (JointType)j; jointData.trackingState = joint.TrackingState; jointData.position = new Vector3(joint.Position.X, joint.Position.Y, joint.Position.Z); jointData.kinectPos = jointData.position; //Debug.Log(jointData.jointType + " - " + joint.TrackingState + " " + jointData.kinectPos); if (j == 0) { jointData.direction = Vector3.zero; } else { int jParent = (int)GetParentJoint(jointData.jointType); if (jointData.trackingState != TrackingState.NotTracked && bodyFrame.bodyData[i].joint[jParent].trackingState != TrackingState.NotTracked) { jointData.direction = jointData.kinectPos - bodyFrame.bodyData[i].joint[jParent].kinectPos; } } jointData.orientation = Quaternion.identity; // Windows.Kinect.Vector4 vQ = body.JointOrientations[jointData.jointType].Orientation; // Quaternion orientation = new Quaternion(vQ.X, vQ.Y, vQ.Z, vQ.W); // // if(j != 0) // { // Quaternion parentOri = bodyFrame.bodyData[i].joint[jParent].orientation; // jointData.orientation = parentOri * orientation; // } // else // { // jointData.orientation = orientation; // } if (j == 0) { bodyFrame.bodyData[i].position = jointData.position; bodyFrame.bodyData[i].orientation = jointData.orientation; } bodyFrame.bodyData[i].joint[j] = jointData; } bodyFrame.bodyData[i].leftHandState = body.HandLeftState; bodyFrame.bodyData[i].leftHandConfidence = body.HandLeftConfidence; bodyFrame.bodyData[i].rightHandState = body.HandRightState; bodyFrame.bodyData[i].rightHandConfidence = body.HandRightConfidence; } } bNewFrame = true; } } return(bNewFrame); }
// Polls for new skeleton data public static bool PollBodyData(SensorData sensorData, ref BodyFrameData bodyFrame) { bool bNewFrame = false; // BodyFrameData tempBodyFrame = new BodyFrameData(); // int hr = GetBodyFrameData(ref tempBodyFrame, true, true); // //// if((hr == 0) && ((sensorData.colorImage == null) || (tempBodyFrame.liRelativeTime >= sensorData.lastColorFrameTime)) && //// ((sensorData.depthImage == null) || (tempBodyFrame.liRelativeTime >= sensorData.lastDepthFrameTime))) // { // if(hr == 0 /**&& (bodyFrame.liRelativeTime > lastFrameTime)*/) // { // bodyFrame = tempBodyFrame; // sensorData.lastBodyFrameTime = bodyFrame.liRelativeTime; // bNewFrame = true; // } // } if(sensorData.bodyFrameReader != null) { var frame = sensorData.bodyFrameReader.AcquireLatestFrame(); if(frame != null) { frame.GetAndRefreshBodyData(sensorData.bodyData); bodyFrame.liRelativeTime = frame.RelativeTime; frame.Dispose(); frame = null; for(int i = 0; i < sensorData.bodyCount; i++) { Body body = sensorData.bodyData[i]; if (body == null) { bodyFrame.bodyData[i].bIsTracked = 0; continue; } bodyFrame.bodyData[i].bIsTracked = (short)(body.IsTracked ? 1 : 0); if(body.IsTracked) { bodyFrame.bodyData[i].liTrackingID = (long)body.TrackingId; for(int j = 0; j < Constants.JointCount; j++) { Windows.Kinect.Joint joint = body.Joints[(JointType)j]; JointData jointData = bodyFrame.bodyData[i].joint[j]; jointData.jointType = (JointType)j; jointData.trackingState = joint.TrackingState; jointData.position = new Vector3(joint.Position.X, joint.Position.Y, joint.Position.Z); jointData.kinectPos = jointData.position; //Debug.Log(jointData.jointType + " - " + joint.TrackingState + " " + jointData.kinectPos); if(j == 0) { jointData.direction = Vector3.zero; } else { int jParent = (int)GetParentJoint(jointData.jointType); if(jointData.trackingState != TrackingState.NotTracked && bodyFrame.bodyData[i].joint[jParent].trackingState != TrackingState.NotTracked) { jointData.direction = jointData.kinectPos - bodyFrame.bodyData[i].joint[jParent].kinectPos; } } jointData.orientation = Quaternion.identity; // Windows.Kinect.Vector4 vQ = body.JointOrientations[jointData.jointType].Orientation; // Quaternion orientation = new Quaternion(vQ.X, vQ.Y, vQ.Z, vQ.W); // // if(j != 0) // { // Quaternion parentOri = bodyFrame.bodyData[i].joint[jParent].orientation; // jointData.orientation = parentOri * orientation; // } // else // { // jointData.orientation = orientation; // } if(j == 0) { bodyFrame.bodyData[i].position = jointData.position; bodyFrame.bodyData[i].orientation = jointData.orientation; } bodyFrame.bodyData[i].joint[j] = jointData; } bodyFrame.bodyData[i].leftHandState = body.HandLeftState; bodyFrame.bodyData[i].leftHandConfidence = body.HandLeftConfidence; bodyFrame.bodyData[i].rightHandState = body.HandRightState; bodyFrame.bodyData[i].rightHandConfidence = body.HandRightConfidence; } } bNewFrame = true; } } return bNewFrame; }