public bool InitializeRigidBody()
{
_nRigidBodies = 0;
for (int i = 0; i < 200; i++)
{
_rigidBodies[i] = new OptiTrackRigidBody();
}
return(true);
}
// Unpack RigidBody data
private void ReadRigidBody(Byte[] b, ref int offset, OptiTrackRigidBody rb)
{
try
{
rb.used = true;
int[] iData = new int[100];
float[] fData = new float[100];
// RB ID
Buffer.BlockCopy(b, offset, iData, 0, 4); offset += 4;
//int iSkelID = iData[0] >> 16; // hi 16 bits = ID of bone's parent skeleton
//int iBoneID = iData[0] & 0xffff; // lo 16 bits = ID of bone
rb.ID = iData[0] - 1; // already have it from data descriptions
// RB pos
float[] pos = new float[3];
Buffer.BlockCopy(b, offset, pos, 0, 4 * 3); offset += 4 * 3;
rb.position.x = pos[0]; 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.orientation.x = ori[0]; rb.orientation.y = ori[1]; rb.orientation.z = ori[2]; rb.orientation.w = ori[3];
Buffer.BlockCopy(b, offset, iData, 0, 4); offset += 4;
int nMarkers = iData[0];
Buffer.BlockCopy(b, offset, fData, 0, 4 * 3 * nMarkers); offset += 4 * 3 * nMarkers;
/*
* for (int i = 0; i < nMarkers;i++)
* {
* Buffer.BlockCopy(b, offset, fData, 0, 4 * 3); offset += 4 * 3;
* _dataStream.marker[_dataStream.markerIndex].pos = new Vector3(fData[0], fData[1], fData[2]);
* _dataStream.markerIndex++;
* }
*/
Buffer.BlockCopy(b, offset, iData, 0, 4 * nMarkers); offset += 4 * nMarkers;
Buffer.BlockCopy(b, offset, fData, 0, 4 * nMarkers); offset += 4 * nMarkers;
Buffer.BlockCopy(b, offset, fData, 0, 4); offset += 6;
} catch (Exception e)
{
Debug.LogError(e.ToString());
}
}