public KinectProtocol_v1()
{
BinaryWritableContainer m_packetContainer = new BinaryWritableContainer();
TaggedBundle m_kinectHeader;
TaggedBundle m_skeletonExtra;
TaggedBundle m_skeletonVertices;
TaggedBundle m_kinectJoystick;
TaggedBundle m_kinectCustom;
m_kinectHeader = new TaggedBundle(19);
m_kinectHeader.Add((VersionNumber = new WritableVersionNumber("01.02.03.04")));
m_kinectHeader.Add((PlayerCount = new WritableByte(0)));
m_kinectHeader.Add((Flags = new WritableUInt(0))); // flags
m_kinectHeader.Add((FloorClipPlane = new WritableFloorClipPlane(0.1f, 0.1f, 0.1f, 0.1f)));
m_kinectHeader.Add((GravityNormal = new WritableVertex(0.0f, 0.0f, 0.0f))); // gravity normal
m_packetContainer.Add(m_kinectHeader);
byte[] trackingStates = new byte[20];
for (uint i = 0; i < trackingStates.Length; i++)
{
trackingStates[i] = (byte)Microsoft.Kinect.JointTrackingState.Tracked;
}
m_skeletonExtra = new TaggedBundle(20);
m_skeletonExtra.Add((SkeletonTrackingStates = new WritableTrackingStates(trackingStates))); // for joints
m_skeletonExtra.Add((CenterOfMass = new WritableVertex(0.1f, 0.1f, 0.1f))); // center of mass
m_skeletonExtra.Add((Quality = new WritableUInt(0))); // Quality
m_skeletonExtra.Add((SkeletonTrackState = new WritableUInt((uint)Microsoft.Kinect.SkeletonTrackingState.Tracked))); // TrackState
m_packetContainer.Add(m_skeletonExtra);
WritableVertex[] vertices = new WritableVertex[20];
for (uint i = 0; i < vertices.Length; i++)
{
vertices[i] = new WritableVertex(0.0f, 0.33f, 2f);
}
m_skeletonVertices = new TaggedBundle(21);
m_skeletonVertices.Add((SkeletonJoints = new WritableVertices(vertices)));
m_packetContainer.Add(m_skeletonVertices);
m_kinectJoystick = new TaggedBundle(24);
m_kinectJoystick.Add((Joystick1 = new WritableJoystick(new sbyte[6], 0)));
m_kinectJoystick.Add((Joystick2 = new WritableJoystick(new sbyte[6], 0)));
m_packetContainer.Add(m_kinectJoystick);
m_kinectCustom = new TaggedBundle(25);
m_kinectCustom.Add((UserBytes = new WritableByteArray(null)));
m_packetContainer.Add(m_kinectCustom);
// compute CRC
// this simply adds four 0 bytes for now
m_packet = new CRC(m_packetContainer);
}
/// <summary>
/// Updates the packet with skeleton data from the skeleton
/// </summary>
/// <param name="skeleton">Skeleton to use for update</param>
private void UpdatePacketSkeletonData(Skeleton skeleton)
{
m_version1Packet.Quality.Set((byte)skeleton.ClippedEdges);
m_version1Packet.CenterOfMass.Set(skeleton.Position.X,
skeleton.Position.Y,
skeleton.Position.Z);
m_version1Packet.SkeletonTrackState.Set((uint)skeleton.TrackingState);
// Loop through joints; get tracking states and positions
byte[] trackingStates = new byte[20];
WritableVertex[] vertices = new WritableVertex[20];
for (uint i = 0; i < skeleton.Joints.Count; i++)
{
Joint joint = skeleton.Joints[(JointType)i];
trackingStates[i] = (byte)(joint.TrackingState);
vertices[i] = new WritableVertex(joint.Position.X,
joint.Position.Y,
joint.Position.Z);
}
m_version1Packet.SkeletonTrackingStates.Set(trackingStates);
m_version1Packet.SkeletonJoints.Set(vertices);
}
/// <summary>
/// Processes the given skeleton and updates the network packet data structure, then
/// sends the resulting packet.
/// </summary>
/// <param name="frame">The Kinect skeleton to process.</param>
public void ProcessSkeleton(SkeletonFrame frame)
{
sbyte[] nullAxis = new sbyte[6];
if ((m_skeletonArray == null) || (m_skeletonArray.Length != frame.SkeletonArrayLength))
{
m_skeletonArray = new Skeleton[frame.SkeletonArrayLength];
}
frame.CopySkeletonDataTo(m_skeletonArray);
// Do stuff here
lock (m_version0Packet)
{
m_version0Packet.PlayerCount.Set((byte)KinectUtils.CountTrackedSkeletons(m_skeletonArray));
m_version0Packet.Flags.Set(0); //Flags only accessible in the C++ API??
m_version0Packet.FloorClipPlane.Set(frame.FloorClipPlane.Item1,
frame.FloorClipPlane.Item2,
frame.FloorClipPlane.Item3,
frame.FloorClipPlane.Item4);
m_version0Packet.GravityNormal.Set(0, 0, 0); //Gravity Normal removed from SDK
// Get the best skeleton
Skeleton s = KinectUtils.SelectBestSkeleton(m_skeletonArray);
m_version0Packet.Quality.Set((byte)s.ClippedEdges);
m_version0Packet.CenterOfMass.Set(s.Position.X,
s.Position.Y,
s.Position.Z);
m_version0Packet.SkeletonTrackState.Set((uint)s.TrackingState);
// Loop through joints; get tracking states and positions
byte[] trackingStates = new byte[20];
WritableVertex[] vertices = new WritableVertex[20];
for (uint i = 0; i < s.Joints.Count; i++)
{
Joint j = s.Joints[(JointType)i];
trackingStates[i] = (byte)(j.TrackingState);
vertices[i] = new WritableVertex(j.Position.X,
j.Position.Y,
j.Position.Z);
}
m_version0Packet.SkeletonTrackingStates.Set(trackingStates);
m_version0Packet.SkeletonJoints.Set(vertices);
// Update Joysticks
WritableJoystick[] sticks = new WritableJoystick[2] {
m_version0Packet.Joystick1,
m_version0Packet.Joystick2
};
if (m_version0Packet.PlayerCount.Get() != 0) //Only process and send valid data if a player is detected
{
m_gestureProcessor.ProcessGestures(sticks, s);
}
else
{
m_version0Packet.Joystick1.Set(nullAxis, 0);
m_version0Packet.Joystick2.Set(nullAxis, 0);
}
}
send();
m_heartbeatTimer.Change(HEARTBEAT_PERIOD_MS, HEARTBEAT_PERIOD_MS);
}
