public bool InitializeRigidBody()
{
_nRigidBodies = 0;
for (int i = 0; i < RigidBodyMaxLength; i++) {
_rigidBody [i] = new OptiTrackRigidBody ();
}
return true;
}
public bool InitializeRigidBody()
{
_nRigidBodies = 0;
for (int i = 0; i < RigidBodyMaxLength; i++)
{
_rigidBody [i] = new OptiTrackRigidBody();
}
return(true);
}
// Unpack RigidBody data
private static void ReadRigidBody(Byte[] b, ref int offset, OptiTrackRigidBody rb)
{
try {
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]; // 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;
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 += 4;
// add bug fix 20160201
Buffer.BlockCopy(b, offset, iData, 0, 2);
offset += 2;
} catch (Exception e) {
Debug.LogError(e.ToString());
}
}
// Unpack RigidBody data
private static void ReadRigidBody(Byte[] b, ref int offset, OptiTrackRigidBody rb, long frameIndex)
{
try
{
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]; // already have it from data descriptions
// RB pos
float[] pos = new float[3];
Buffer.BlockCopy(b, offset, pos, 0, 4 * 3); offset += 4 * 3;
/********************** EDIT START ***********************/
rb.position = new TimePointF(-pos[0], -pos[2], frameIndex);
//Console.WriteLine(rb.frameIndex.ToString());
/********************** EDIT END ***********************/
// 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;
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 += 4;
} catch (Exception e)
{
Console.WriteLine(e.ToString());
}
}
// Unpack RigidBody data
private static void ReadRigidBody(Byte[] b, ref int offset, OptiTrackRigidBody rb)
{
try
{
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]; // 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;
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 += 4;
} catch (Exception e)
{
Debug.LogError(e.ToString());
}
}
