/// <summary>
/// Set the data of packet.
/// </summary>
/// <param name="data">byte data recieved from server</param>
internal void SetData(byte[] data)
{
/* === Data packet setup ===
* Packet size - 4 bytes
* packet type - 4 bytes
* timestamp - 8 bytes
* Component count - 4 bytes
* [for each component]
* Component size - 4 bytes
* Component type - 4 bytes
* Component Data - [Component size] bytes
*/
lock (packetLock)
{
ClearData();
mData = data;
SetPacketHeader();
if (mPacketType == PacketType.PacketData)
{
SetTimeStamp();
SetFrameNumber();
SetComponentCount();
int position = RTProtocol.Constants.PACKET_HEADER_SIZE + RTProtocol.Constants.DATA_PACKET_HEADER_SIZE;
for (int component = 1; component <= mComponentCount; component++)
{
ComponentType componentType = GetComponentType(position);
position += RTProtocol.Constants.COMPONENT_HEADER;
if (componentType == ComponentType.Component3d)
{
/* Marker count - 4 bytes
* 2D Drop rate - 2 bytes
* 2D Out of sync rate - 2 bytes
* [Repeated per marker]
* X - 4 bytes
* Y - 4 bytes
* Z - 4 bytes
*/
uint markerCount = BitConvert.GetUInt32(mData, ref position);
m2DDropRate = BitConvert.GetUShort(mData, ref position);
m2DOutOfSyncRate = BitConvert.GetUShort(mData, ref position);
for (int i = 0; i < markerCount; i++)
{
Q3D marker = new Q3D();
marker.Id = 0;
marker.Residual = -1;
marker.Position = BitConvert.GetPoint(mData, ref position);
m3DMarkerData.Add(marker);
}
}
else if (componentType == ComponentType.Component3dNoLabels)
{
/* Marker count - 4 bytes
* 2D Drop rate - 2 bytes
* 2D Out of sync rate - 2 bytes
* [Repeated per marker]
* X - 4 bytes
* Y - 4 bytes
* Z - 4 bytes
* ID - 4 bytes
*/
int markerCount = BitConvert.GetInt32(mData, ref position);
m2DDropRate = BitConvert.GetUShort(mData, ref position);
m2DOutOfSyncRate = BitConvert.GetUShort(mData, ref position);
for (int i = 0; i < markerCount; i++)
{
Q3D marker = new Q3D();
marker.Residual = -1;
marker.Position = BitConvert.GetPoint(mData, ref position);
marker.Id = BitConvert.GetUInt32(mData, ref position);
m3DMarkerNoLabelData.Add(marker);
}
}
else if (componentType == ComponentType.ComponentAnalog)
{
/* Analog Device count - 4 bytes
* [Repeated per device]
* Device id - 4 bytes
* Channel count - 4 bytes
* Sample count - 4 bytes
* Sample number - 4 bytes
* Analog data - 4 * channelcount * sampleCount
*/
uint deviceCount = BitConvert.GetUInt32(mData, ref position);
for (int i = 0; i < deviceCount; i++)
{
Analog analogDeviceData = new Analog();
analogDeviceData.DeviceID = BitConvert.GetUInt32(mData, ref position);
analogDeviceData.ChannelCount = BitConvert.GetUInt32(mData, ref position);
analogDeviceData.Channels = new AnalogChannelData[analogDeviceData.ChannelCount];
uint sampleCount = BitConvert.GetUInt32(mData, ref position);
if (sampleCount * analogDeviceData.ChannelCount * 4 > mData.Length)
{
}
else if (sampleCount > 0)
{
uint sampleNumber = BitConvert.GetUInt32(mData, ref position);
for (uint j = 0; j < analogDeviceData.ChannelCount; j++)
{
AnalogChannelData sample = new AnalogChannelData();
sample.Samples = new float[sampleCount];
for (uint k = 0; k < sampleCount; k++)
{
sample.SampleNumber = sampleNumber + k;
sample.Samples[k] = BitConvert.GetFloat(mData, ref position);
}
analogDeviceData.Channels[j] = sample;
}
}
mAnalogDevices.Add(analogDeviceData);
}
}
else if (componentType == ComponentType.ComponentForce)
{
/* Force plate count - 4 bytes
* [Repeated per plate]
* Force plate ID - 4 bytes
* Force count - 4 bytes
* forceNumber - 4 bytes
* Force data - 36 * force count bytes
*/
int forcePlateCount = BitConvert.GetInt32(mData, ref position);
for (int i = 0; i < forcePlateCount; i++)
{
ForcePlate plate = new ForcePlate();
plate.PlateId = BitConvert.GetInt32(mData, ref position);
plate.ForceCount = BitConvert.GetInt32(mData, ref position);
plate.ForceNumber = BitConvert.GetInt32(mData, ref position);
plate.ForceSamples = new ForceSample[plate.ForceCount];
for (int j = 0; j < plate.ForceCount; j++)
{
ForceSample sample;
sample.Force = BitConvert.GetPoint(mData, ref position);
sample.Moment = BitConvert.GetPoint(mData, ref position);
sample.ApplicationPoint = BitConvert.GetPoint(mData, ref position);
plate.ForceSamples[j] = sample;
}
mForcePlates.Add(plate);
}
}
else if (componentType == ComponentType.Component6d)
{
/* Body count - 4 bytes
* 2D Drop rate - 2 bytes
* 2D Out of sync rate - 2 bytes
* [Repeated per body]
* X - 4 bytes
* Y - 4 bytes
* Z - 4 bytes
* rotation matrix - 9*4 bytes
*/
int bodyCount = BitConvert.GetInt32(mData, ref position);
m2DDropRate = BitConvert.GetUShort(mData, ref position);
m2DOutOfSyncRate = BitConvert.GetUShort(mData, ref position);
for (int i = 0; i < bodyCount; i++)
{
Q6DOF body = new Q6DOF();
body.Position = BitConvert.GetPoint(mData, ref position);
body.Matrix = new float[9];
for (int j = 0; j < 9; j++)
{
body.Matrix[j] = BitConvert.GetFloat(mData, ref position);
}
body.Residual = -1;
m6DOFData.Add(body);
}
}
else if (componentType == ComponentType.Component6dEuler)
{
/* Body count - 4 bytes
* 2D Drop rate - 2 bytes
* 2D Out of sync rate - 2 bytes
* [Repeated per body]
* X - 4 bytes
* Y - 4 bytes
* Z - 4 bytes
* Euler Angles - 3*4 bytes
*/
int bodyCount = BitConvert.GetInt32(mData, ref position);
m2DDropRate = BitConvert.GetUShort(mData, ref position);
m2DOutOfSyncRate = BitConvert.GetUShort(mData, ref position);
for (int i = 0; i < bodyCount; i++)
{
Q6DOFEuler body = new Q6DOFEuler();
body.Position = BitConvert.GetPoint(mData, ref position);
body.Rotation = BitConvert.GetPoint(mData, ref position);
body.Residual = -1;
m6DOFEulerData.Add(body);
}
}
else if (componentType == ComponentType.Component2d || componentType == ComponentType.Component2dLinearized)
{
/* Camera Count - 4 bytes
* 2D Drop rate - 2 bytes
* 2D Out of sync rate - 2 bytes
* [Repeated per Camera]
* Marker Count - 4 bytes
* Status Flags - 1 byte
* [Repeated per Marker]
* X - 4 Bytes
* Y - 4 Bytes
* Diameter X - 4 bytes
* Diameter Y - 4 bytes
*/
uint cameraCount = BitConvert.GetUInt32(mData, ref position);
m2DDropRate = BitConvert.GetUShort(mData, ref position);
m2DOutOfSyncRate = BitConvert.GetUShort(mData, ref position);
for (int i = 0; i < cameraCount; i++)
{
Camera camera = new Camera();
camera.MarkerCount = BitConvert.GetUInt32(mData, ref position);
camera.StatusFlags = mData[position++];
camera.MarkerData2D = new Q2D[camera.MarkerCount];
for (int j = 0; j < camera.MarkerCount; j++)
{
Q2D marker = new Q2D();
marker.X = BitConvert.GetUInt32(mData, ref position);
marker.Y = BitConvert.GetUInt32(mData, ref position);
marker.DiameterX = BitConvert.GetUShort(mData, ref position);
marker.DiameterY = BitConvert.GetUShort(mData, ref position);
camera.MarkerData2D[j] = marker;
}
if (componentType == ComponentType.Component2d)
m2DMarkerData.Add(camera);
else if (componentType == ComponentType.Component2dLinearized)
m2DLinearMarkerData.Add(camera);
}
}
else if (componentType == ComponentType.Component3dResidual)
{
/* Marker count - 4 bytes
* 2D Drop rate - 2 bytes
* 2D Out of sync rate - 2 bytes
* [Repeated per marker]
* X - 4 bytes
* Y - 4 bytes
* Z - 4 bytes
* Residual - 4 bytes
*/
int markerCount = BitConvert.GetInt32(mData, ref position);
m2DDropRate = BitConvert.GetUShort(mData, ref position);
m2DOutOfSyncRate = BitConvert.GetUShort(mData, ref position);
for (int i = 0; i < markerCount; i++)
{
Q3D marker = new Q3D();
marker.Id = 0;
marker.Position = BitConvert.GetPoint(mData, ref position);
marker.Residual = BitConvert.GetFloat(mData, ref position);
m3DMarkerResidualData.Add(marker);
}
}
else if (componentType == ComponentType.Component3dNoLabelsResidual)
{
/* Marker count - 4 bytes
* 2D Drop rate - 2 bytes
* 2D Out of sync rate - 2 bytes
* [Repeated per marker]
* X - 4 bytes
* Y - 4 bytes
* Z - 4 bytes
* Residual - 4 bytes
* ID - 4 bytes
*/
int markerCount = BitConvert.GetInt32(mData, ref position);
m2DDropRate = BitConvert.GetUShort(mData, ref position);
m2DOutOfSyncRate = BitConvert.GetUShort(mData, ref position);
for (int i = 0; i < markerCount; i++)
{
Q3D marker = new Q3D();
marker.Position = BitConvert.GetPoint(mData, ref position);
marker.Residual = BitConvert.GetFloat(mData, ref position);
marker.Id = BitConvert.GetUInt32(mData, ref position);
m3DMarkerNoLabelResidualData.Add(marker);
}
}
else if (componentType == ComponentType.Component6dResidual)
{
/* Body count - 4 bytes
* 2D Drop rate - 2 bytes
* 2D Out of sync rate - 2 bytes
* [Repeated per marker]
* X - 4 bytes
* Y - 4 bytes
* Z - 4 bytes
* rotation matrix - 9*4 bytes
* residual - 9*4 bytes
*/
int bodyCount = BitConvert.GetInt32(mData, ref position);
m2DDropRate = BitConvert.GetUShort(mData, ref position);
m2DOutOfSyncRate = BitConvert.GetUShort(mData, ref position);
for (int i = 0; i < bodyCount; i++)
{
Q6DOF body = new Q6DOF();
body.Position = BitConvert.GetPoint(mData, ref position);
body.Matrix = new float[9];
for (int j = 0; j < 9; j++)
body.Matrix[j] = BitConvert.GetFloat(mData, ref position);
body.Residual = BitConvert.GetFloat(mData, ref position); ;
m6DOFResidualData.Add(body);
}
}
else if (componentType == ComponentType.Component6dEulerResidual)
{
/* Body count - 4 bytes
* 2D Drop rate - 2 bytes
* 2D Out of sync rate - 2 bytes
* [Repeated per marker]
* X - 4 bytes
* Y - 4 bytes
* Z - 4 bytes
* Euler Angles - 3*4 bytes
* residual - 9*4 bytes
*/
int bodyCount = BitConvert.GetInt32(mData, ref position);
m2DDropRate = BitConvert.GetUShort(mData, ref position);
m2DOutOfSyncRate = BitConvert.GetUShort(mData, ref position);
for (int i = 0; i < bodyCount; i++)
{
Q6DOFEuler body = new Q6DOFEuler();
body.Position = BitConvert.GetPoint(mData, ref position);
body.Rotation = BitConvert.GetPoint(mData, ref position);
body.Residual = BitConvert.GetFloat(mData, ref position);
m6DOFEulerResidualData.Add(body);
}
}
else if (componentType == ComponentType.ComponentAnalogSingle)
{
/* Analog Device count - 4 bytes
* [Repeated per device]
* Device id - 4 bytes
* Channel count - 4 bytes
* Analog data - 4 * channelcount
*/
int deviceCount = BitConvert.GetInt32(mData, ref position);
for (int i = 0; i < deviceCount; i++)
{
Analog device = new Analog();
device.DeviceID = BitConvert.GetUInt32(mData, ref position);
device.ChannelCount = BitConvert.GetUInt32(mData, ref position);
device.Channels = new AnalogChannelData[device.ChannelCount];
for (int j = 0; j < device.ChannelCount; j++)
{
AnalogChannelData sample = new AnalogChannelData();
sample.Samples = new float[1];
sample.Samples[0] = BitConvert.GetFloat(mData, ref position);
device.Channels[j] = sample;
}
mAnalogSingleSample.Add(device);
}
}
else if (componentType == ComponentType.ComponentImage)
{
/* Camera count - 4 bytes
* [Repeated per marker]
* Camera ID - 4 bytes
* Image Format - 4 bytes
* Width - 4 bytes
* Height- 4 bytes
* Left crop - 4 bytes
* Top crop - 4 bytes
* Right crop - 4 bytes
* Bottom crop - 4 bytes
* Image size- 4 bytes
* Image data - [Image size bytes]
*/
int cameraCount = BitConvert.GetInt32(mData, ref position);
for (int i = 0; i < cameraCount; i++)
{
CameraImage image = new CameraImage();
image.CameraID = BitConvert.GetUInt32(mData, ref position);
image.ImageFormat = (ImageFormat)BitConvert.GetUInt32(mData, ref position);
image.Width = BitConvert.GetUInt32(mData, ref position);
image.Height = BitConvert.GetUInt32(mData, ref position);
image.LeftCrop = BitConvert.GetFloat(mData, ref position);
image.TopCrop = BitConvert.GetFloat(mData, ref position);
image.RightCrop = BitConvert.GetFloat(mData, ref position);
image.BottomCrop = BitConvert.GetFloat(mData, ref position);
image.ImageSize = BitConvert.GetInt32(mData, ref position);
image.ImageData = new byte[image.ImageSize];
Array.Copy(mData, position, image.ImageData, 0, image.ImageSize);
position += image.ImageSize;
mImageData.Add(image);
}
}
else if (componentType == ComponentType.ComponentForceSingle)
{
/* Force plate count - 4 bytes
* [Repeated per plate]
* Force plate ID - 4 bytes
* Force data - 36 bytes
*/
int forcePlateCount = BitConvert.GetInt32(mData, ref position);
for (int i = 0; i < forcePlateCount; i++)
{
ForcePlate plate = new ForcePlate();
plate.PlateId = BitConvert.GetInt32(mData, ref position);
plate.ForceCount = 1;
plate.ForceNumber = -1;
plate.ForceSamples = new ForceSample[plate.ForceCount];
plate.ForceSamples[0].Force = BitConvert.GetPoint(mData, ref position);
plate.ForceSamples[0].Moment = BitConvert.GetPoint(mData, ref position);
plate.ForceSamples[0].ApplicationPoint = BitConvert.GetPoint(mData, ref position);
mForceSinglePlate.Add(plate);
}
}
else if (componentType == ComponentType.ComponentGazeVector)
{
/* Gaze vector count - 4 bytes
* Gaze vector sample count - 4 bytes
* Gaze vector sample number - 4 bytes (omitted if sample count is 0)
* [Repeated per gaze vector (omitted if sample count is 0)]
* Gaze vector data - 24 bytes
*/
int gazeVectorCount = BitConvert.GetInt32(mData, ref position);
for (int i = 0; i < gazeVectorCount; i++)
{
GazeVector gazeVector = new GazeVector();
uint sampleCount = BitConvert.GetUInt32(mData, ref position);
if (sampleCount > 0)
{
uint sampleNumber = BitConvert.GetUInt32(mData, ref position);
gazeVector.SampleNumber = sampleNumber;
for (var sample = 0; sample < sampleCount; sample++)
{
gazeVector.Gaze = BitConvert.GetPoint(mData, ref position);
gazeVector.Position = BitConvert.GetPoint(mData, ref position);
}
}
mGazeVector.Add(gazeVector);
}
}
}
}
}
}
