public bool PollBodyFrame(KinectInterop.SensorData sensorData, ref KinectInterop.BodyFrameData bodyFrame, ref Matrix4x4 kinectToWorld, bool bIgnoreJointZ)
{
bool bNewFrame = _bodyFrameReady;
if (_bodyFrameReady)
{
lock (_bodyFrameLock)
{
bodyFrame.liPreviousTime = bodyFrame.liRelativeTime;
bodyFrame.liRelativeTime = _bodyFrameTime;
if (sensorData.hintHeightAngle)
{
//// get the floor plane
//Windows.Kinect.Vector4 vFloorPlane = _bodyFrame.FloorClipPlane;
//Vector3 floorPlane = new Vector3(vFloorPlane.X, vFloorPlane.Y, vFloorPlane.Z);
//sensorData.sensorRotDetected = Quaternion.FromToRotation(floorPlane, Vector3.up);
//sensorData.sensorHgtDetected = vFloorPlane.W;
}
for (int i = 0; i < sensorData.bodyCount; i++)
{
Body body = i < _bodyFrame.Bodies.Length ? _bodyFrame.Bodies[i] : null;
if (body == null)
{
bodyFrame.bodyData[i].bIsTracked = 0;
continue;
}
bodyFrame.bodyData[i].bIsTracked = (short)(body.IsTracked ? 1 : 0);
if (body.IsTracked)
{
// transfer body and joints data
byte[] entityBytes = body.EntityId.ToByteArray();
bodyFrame.bodyData[i].liTrackingID = BitConverter.ToInt64(entityBytes, 8);
// cache the body joints (following the advice of Brian Chasalow)
//Dictionary<Windows.Kinect.JointType, Windows.Kinect.Joint> bodyJoints = body.Joints;
// calculate the inter-frame time
float frameTime = 0f;
if (bodyFrame.bTurnAnalisys && bodyFrame.liPreviousTime > 0)
{
frameTime = (float)(bodyFrame.liRelativeTime - bodyFrame.liPreviousTime) / 100000000000;
}
for (int j = 0; j < sensorData.jointCount; j++)
{
if (j >= body.Joints.Count)
{
continue;
}
MultiK2.Tracking.Joint joint = body.Joints[(MultiK2.Tracking.JointType)j];
KinectInterop.JointData jointData = bodyFrame.bodyData[i].joint[j];
//jointData.jointType = (KinectInterop.JointType)j;
jointData.trackingState = (KinectInterop.TrackingState)joint.PositionTrackingState;
if ((int)joint.PositionTrackingState != (int)TrackingState.NotTracked)
{
float jPosZ = (bIgnoreJointZ && j > 0) ? bodyFrame.bodyData[i].joint[0].kinectPos.z : joint.Position.Z;
jointData.kinectPos = new Vector3(joint.Position.X, joint.Position.Y, joint.Position.Z);
jointData.position = kinectToWorld.MultiplyPoint3x4(new Vector3(joint.Position.X, joint.Position.Y, jPosZ));
}
jointData.orientation = Quaternion.identity;
if (j == 0)
{
bodyFrame.bodyData[i].position = jointData.position;
bodyFrame.bodyData[i].orientation = jointData.orientation;
}
bodyFrame.bodyData[i].joint[j] = jointData;
}
//if (bodyFrame.bTurnAnalisys && bodyFrame.liPreviousTime > 0)
//{
// for (int j = 0; j < sensorData.jointCount; j++)
// {
// KinectInterop.JointData jointData = bodyFrame.bodyData[i].joint[j];
// int p = (int)GetParentJoint((KinectInterop.JointType)j);
// Vector3 parentPos = bodyFrame.bodyData[i].joint[p].position;
// jointData.posRel = jointData.position - parentPos;
// jointData.posDrv = frameTime > 0f ? (jointData.position - jointData.posPrev) / frameTime : Vector3.zero;
// jointData.posPrev = jointData.position;
// bodyFrame.bodyData[i].joint[j] = jointData;
// }
//}
// tranfer hand states
bodyFrame.bodyData[i].leftHandState = (KinectInterop.HandState)body.HandStateLeft;
bodyFrame.bodyData[i].leftHandConfidence = (KinectInterop.TrackingConfidence)body.ConfidenceLeft;
bodyFrame.bodyData[i].rightHandState = (KinectInterop.HandState)body.HandStateRight;
bodyFrame.bodyData[i].rightHandConfidence = (KinectInterop.TrackingConfidence)body.ConfidenceRight;
}
}
_bodyFrameReady = false;
}
}
return(bNewFrame);
}
internal Bone(Joint joint1, Joint joint2)
{
Joint1 = joint1;
Joint2 = joint2;
}