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