// Unpack RigidBody data
private static void ReadRigidBody(Byte[] b, ref int offset, NatNetSkeleton sk)
{
try {
// RB ID
int[] _id = new int[1];
Buffer.BlockCopy(b, offset, _id, 0, 4); offset += 4;
int id = _id[0];
int iSkelID = _id[0] >> 16; // hi 16 bits = ID of bone's parent skeleton
int iBoneID = _id[0] & 0xffff; // lo 16 bits = ID of bone
NatNetRigidBody rb = null;
if (sk == null)
{
if (!streamData.rigidBodies.ContainsKey(id))
{
streamData.rigidBodies[id] = new NatNetRigidBody(id);
}
rb = streamData.rigidBodies[id];
}
else if (sk.ID == iSkelID)
{
if (!sk.bones.ContainsKey(iBoneID))
{
sk.bones[iBoneID] = new NatNetRigidBody(iBoneID);
}
rb = sk.bones[iBoneID];
}
// RB pos
float[] pos = new float[3];
Buffer.BlockCopy(b, offset, pos, 0, 4 * 3); offset += 4 * 3;
rb.position.x = pos[0] * -1; rb.position.y = pos[1]; rb.position.z = pos[2];
// RB ori
float[] ori = new float[4];
Buffer.BlockCopy(b, offset, ori, 0, 4 * 4); offset += 4 * 4;
rb.rotation.x = -ori[0]; rb.rotation.y = ori[1]; rb.rotation.z = ori[2]; rb.rotation.w = -ori[3];
// nMarkers
int[] _nMarkers = new int[1];
Buffer.BlockCopy(b, offset, _nMarkers, 0, 4); offset += 4;
int nMarkers = _nMarkers[0];
// Marker positions
float[] _markerData = new float[3 * nMarkers];
Buffer.BlockCopy(b, offset, _markerData, 0, 4 * 3 * nMarkers); offset += 4 * 3 * nMarkers;
// Marker IDs
int[] _markerIDs = new int[nMarkers];
Buffer.BlockCopy(b, offset, _markerIDs, 0, 4 * nMarkers); offset += 4 * nMarkers;
// Marker Sizes
float[] _markerSizes = new float[nMarkers];
Buffer.BlockCopy(b, offset, _markerSizes, 0, 4 * nMarkers); offset += 4 * nMarkers;
// Marker error
float[] _error = new float[1];
Buffer.BlockCopy(b, offset, _error, 0, 4); offset += 4;
// Tracking Flags
short[] _params = new short[1];
Buffer.BlockCopy(b, offset, _params, 0, 2); offset += 2;
rb.tracked = (_params[0] & 0x01) > 0;
// 0x01 : rigid body was successfully tracked in this frame
} catch (Exception e) {
Debug.LogError(e.ToString());
}
}
private static void ReadPacket(Byte[] b)
{
int offset = 0;
int nBytes = 0;
Buffer.BlockCopy(b, offset, iData, 0, 2); offset += 2;
int messageID = iData[0];
Buffer.BlockCopy(b, offset, iData, 0, 2); offset += 2;
nBytes = iData[0];
if (messageID == 5)
{
Debug.Log("DirectParseClient: Data descriptions");
Buffer.BlockCopy(b, offset, iData, 0, 4); offset += 4;
int nDatasets = iData[0];
for (int d = 0; d < nDatasets; d++)
{
int[] _type = new int[1]; Buffer.BlockCopy(b, offset, _type, 0, 4); offset += 4;
if (_type[0] == 0) // Markerset
{
string name = "";
int namelen = 0;
while (b[offset + namelen] != 0x00)
{
name += (char)b[offset + namelen];
namelen++;
}
offset = offset + namelen + 1;
int[] _nMarkers = new int[1]; Buffer.BlockCopy(b, offset, _nMarkers, 0, 4); offset += 4;
int nMarkers = _nMarkers[0];
//Debug.Log("Markerset: "+name+" ("+nMarkers+")");
for (int j = 0; j < nMarkers; j++)
{
string marker_name = "";
int marker_namelen = 0;
while (b[offset + marker_namelen] != 0x00)
{
marker_name += (char)b[offset + marker_namelen];
marker_namelen++;
}
offset = offset + marker_namelen + 1;
//Debug.Log("....with marker: "+marker_name);
}
//Debug.Log("Done");
}
else if (_type[0] == 1) // Rigidbody
{
string name = "";
int namelen = 0;
while (b[offset + namelen] != 0x00)
{
name += (char)b[offset + namelen];
namelen++;
}
offset = offset + namelen + 1;
int[] _id = new int[1]; Buffer.BlockCopy(b, offset, _id, 0, 4); offset += 4;
int[] _parentID = new int[1]; Buffer.BlockCopy(b, offset, _parentID, 0, 4); offset += 4;
int id = _id[0];
int parentID = _parentID[0];
float[] _offsets = new float[3]; Buffer.BlockCopy(b, offset, _offsets, 0, 4 * 3); offset += 4 * 3;
Vector3 offsets = new Vector3(_offsets[0] * -1, _offsets[1], _offsets[2]);
NatNetRigidBody rb = null;
if (!streamData.rigidBodies.ContainsKey(id))
{
streamData.rigidBodies[id] = new NatNetRigidBody(id);
}
rb = streamData.rigidBodies[id];
rb.name = name;
rb.parentID = parentID;
rb.offset = offsets;
Debug.Log(name + ": (" + _id[0] + "," + _parentID[0] + ") [" + offsets.x + "," + offsets.y + "," + offsets.z + "]");
}
else if (_type[0] == 2) // Skeleton
{
string name = "";
while (b[offset] != 0x00)
{
name += (char)b[offset];
offset++;
}
offset++;
//int[] _id = new int[1]; Buffer.BlockCopy(b, offset, _id, 0, 4); offset += 4;
int _skeletonID = BitConverter.ToInt32(b, offset); offset += 4; // Buffer.BlockCopy(b, offset, _idx, 0, 4); offset += 4;
int _nBones = BitConverter.ToInt32(b, offset); offset += 4;
NatNetSkeleton sk = null;
if (!streamData.skeletons.ContainsKey(_skeletonID))
{
streamData.skeletons[_skeletonID] = new NatNetSkeleton(_skeletonID);
}
sk = streamData.skeletons[_skeletonID];
sk.name = name;
//Debug.Log("Skeleton (ID: " + sk.ID + ", Name: "+ sk.name + ") with " + _nBones + " bones");
for (int i = 0; i < _nBones; i++)
{
string boneName = "";
int boneNameLen = 0;
while (b[offset + boneNameLen] != 0x00)
{
boneName += (char)b[offset + boneNameLen];
boneNameLen++;
}
offset = offset + boneNameLen + 1;
int boneID = BitConverter.ToInt32(b, offset); offset += 4;
int boneParentID = BitConverter.ToInt32(b, offset); offset += 4;
float[] _boneOffsets = new float[3]; Buffer.BlockCopy(b, offset, _boneOffsets, 0, 4 * 3); offset += 4 * 3;
Vector3 boneOffsets = new Vector3(_boneOffsets[0] * -1, _boneOffsets[1], _boneOffsets[2]);
NatNetRigidBody rb = null;
if (!sk.bones.ContainsKey(boneID))
{
sk.bones[boneID] = new NatNetRigidBody(boneID);
}
rb = sk.bones[boneID];
rb.name = boneName;
rb.parentID = boneParentID;
rb.offset = boneOffsets;
//Debug.Log("\t" + boneName + ": [ID=" + boneID + ", ParentID=" + boneParentID + ", Offset=[" + rb.offset.x + "," + rb.offset.y + "," + rb.offset.z + "])");
}
sk.receivedDescription = true;
}
}
receivedDataDescription = true;
Debug.Log("End data descriptions.");
}
else if (receivedDataDescription && messageID == 7)
{
frameLog = String.Format("DirectParseClient: [UDPClient] Read FrameOfMocapData: {0}\n", nBytes);
Buffer.BlockCopy(b, offset, iData, 0, 4); offset += 4;
frameLog += String.Format("Frame # : {0}\n", iData[0]);
//number of data sets (markersets, rigidbodies, etc)
Buffer.BlockCopy(b, offset, iData, 0, 4); offset += 4;
int nMarkerSets = iData[0];
frameLog += String.Format("MarkerSets # : {0}\n", iData[0]);
for (int i = 0; i < nMarkerSets; i++)
{
String strName = "";
int nChars = 0;
while (b[offset + nChars] != '\0')
{
nChars++;
}
strName = System.Text.Encoding.ASCII.GetString(b, offset, nChars);
offset += nChars + 1;
Buffer.BlockCopy(b, offset, iData, 0, 4); offset += 4;
frameLog += String.Format("{0}:" + strName + ": marker count : {1}\n", i, iData[0]);
nBytes = iData[0] * 3 * 4;
Buffer.BlockCopy(b, offset, fData, 0, nBytes); offset += nBytes;
}
// Other Markers - All 3D points that were triangulated but not labeled for the given frame.
Buffer.BlockCopy(b, offset, iData, 0, 4); offset += 4;
int nOtherMarkers = iData[0];
frameLog += String.Format("Other Markers : {0}\n", iData[0]);
nBytes = nOtherMarkers * 3 * 4;
Buffer.BlockCopy(b, offset, fData, 0, nBytes); offset += nBytes;
// Rigid Bodies
Buffer.BlockCopy(b, offset, iData, 0, 4); offset += 4;
int expectedRigidBodies = iData[0];
frameLog += String.Format("Rigid Bodies : {0}\n", iData[0]);
for (int i = 0; i < expectedRigidBodies; i++)
{
ReadRigidBody(b, ref offset, null);
}
// Skeletons
Buffer.BlockCopy(b, offset, iData, 0, 4); offset += 4;
int expectedSkeletons = iData[0];
frameLog += String.Format("Rigid Bodies : {0}\n", iData[0]);
for (int i = 0; i < expectedSkeletons; i++)
{
ReadSkeleton(b, ref offset);
}
// Labeled Markers
// Force Plate data
// latency
// timecode
// timestamp
// frame params
// end of data tag
//Debug.Log(_strFrameLog);
}
else
{
Debug.Log("Unhandled messageID: " + messageID);
}
if (!receivedDataDescription)
{
SendDataDescriptionRequest();
}
}
