//THE MOST IMPORTANT FUNCTION: to define the UniForm of broadcasting optitrack data.
/**
* Modified by daaqing & zsyzgu from Optitrack Standard Program.
*
*
*
*
*
**/
private void broadcast()
{
server.Send("begin");
FrameOfMocapData frame = m_FrameOfData;
data += "Frame\n";
data += frame.fTimestamp + "\n";
data += DateTime.Now.TimeOfDay.ToString(); // 20:33:50.7187500
for (int i = 0; i < frame.nRigidBodies; ++i)
{
RigidBodyData rb = frame.RigidBodies[i];
float[] quat = new float[4] {
rb.qx, rb.qy, rb.qz, rb.qw
};
float[] eulers = m_NatNet.QuatToEuler(quat, (int)NATEulerOrder.NAT_XYZr);
int id = rb.ID;
double x = -rb.x * 0.5;
double y = rb.y * 0.5;
double z = rb.z * 0.5;
double rx = RadiansToDegrees(eulers[0]);
double ry = RadiansToDegrees(eulers[1]);
double rz = -RadiansToDegrees(eulers[2]);
server.Send("rb " + id + " " + x + " " + y + " " + z + " " + rx + " " + ry + " " + rz);
data += "rb " + id + " " + rb.Tracked + " " + x + " " + y + " " + z + " " + rx + " " + ry + " " + rz + "\n";
}
for (int i = 0; i < frame.nOtherMarkers; i++)
{
Marker m = frame.OtherMarkers[i];
data += "x " + m.x + " y " + m.y + " z " + m.z + "\n";
}
data += "\n";
server.Send("end");
}
/// <summary>
/// Update the spreadsheet.
/// Note: This refresh is quite slow and provided here only as a complete example.
/// In a production setting this would be optimized.
/// </summary>
private void UpdateDataGrid()
{
// update MarkerSet data
for (int i = 0; i < m_FrameOfData.nMarkerSets; i++)
{
NatNetML.MarkerSetData ms = m_FrameOfData.MarkerSets[i];
for (int j = 0; j < ms.nMarkers; j++)
{
string strUniqueName = ms.MarkerSetName + j.ToString();
int key = strUniqueName.GetHashCode();
if (htMarkers.Contains(key))
{
int rowIndex = (int)htMarkers[key];
if (rowIndex >= 0)
{
dataGridView1.Rows[rowIndex].Cells[1].Value = ms.Markers[j].x;
dataGridView1.Rows[rowIndex].Cells[2].Value = ms.Markers[j].y;
dataGridView1.Rows[rowIndex].Cells[3].Value = ms.Markers[j].z;
}
}
}
}
// update RigidBody data
for (int i = 0; i < m_FrameOfData.nRigidBodies; i++)
{
NatNetML.RigidBodyData rb = m_FrameOfData.RigidBodies[i];
int key = rb.ID.GetHashCode();
// note : must add rb definitions here one time instead of on get data descriptions because we don't know the marker list yet.
if (!htRigidBodies.ContainsKey(key))
{
// Add RigidBody def to the grid
if (rb.Markers[0].ID != -1)
{
RigidBody rbDef = FindRB(rb.ID);
if (rbDef != null)
{
int rowIndex = dataGridView1.Rows.Add("RigidBody: " + rbDef.Name);
key = rb.ID.GetHashCode();
htRigidBodies.Add(key, rowIndex);
// Add Markers associated with this rigid body to the grid
for (int j = 0; j < rb.nMarkers; j++)
{
String strUniqueName = rbDef.Name + "-" + rb.Markers[j].ID.ToString();
int keyMarker = strUniqueName.GetHashCode();
int newRowIndexMarker = dataGridView1.Rows.Add(strUniqueName);
htMarkers.Add(keyMarker, newRowIndexMarker);
}
}
}
}
else
{
// update RigidBody data
int rowIndex = (int)htRigidBodies[key];
if (rowIndex >= 0)
{
bool tracked = rb.Tracked;
if (!tracked)
{
OutputMessage("RigidBody not tracked in this frame.");
}
dataGridView1.Rows[rowIndex].Cells[1].Value = rb.x * m_ServerToMillimeters;
dataGridView1.Rows[rowIndex].Cells[2].Value = rb.y * m_ServerToMillimeters;
dataGridView1.Rows[rowIndex].Cells[3].Value = rb.z * m_ServerToMillimeters;
// Convert quaternion to eulers. Motive coordinate conventions: X(Pitch), Y(Yaw), Z(Roll), Relative, RHS
float[] quat = new float[4] {
rb.qx, rb.qy, rb.qz, rb.qw
};
float[] eulers = new float[3];
eulers = m_NatNet.QuatToEuler(quat, (int)NATEulerOrder.NAT_XYZr);
double x = RadiansToDegrees(eulers[0]); // convert to degrees
double y = RadiansToDegrees(eulers[1]);
double z = RadiansToDegrees(eulers[2]);
/*
* if (m_UpAxis == 2)
* {
* double yOriginal = y;
* y = -z;
* z = yOriginal;
* }
*/
dataGridView1.Rows[rowIndex].Cells[4].Value = x;
dataGridView1.Rows[rowIndex].Cells[5].Value = y;
dataGridView1.Rows[rowIndex].Cells[6].Value = z;
// update Marker data associated with this rigid body
for (int j = 0; j < rb.nMarkers; j++)
{
if (rb.Markers[j].ID != -1)
{
RigidBody rbDef = FindRB(rb.ID);
if (rbDef != null)
{
String strUniqueName = rbDef.Name + "-" + rb.Markers[j].ID.ToString();
int keyMarker = strUniqueName.GetHashCode();
if (htMarkers.ContainsKey(keyMarker))
{
int rowIndexMarker = (int)htMarkers[keyMarker];
NatNetML.Marker m = rb.Markers[j];
dataGridView1.Rows[rowIndexMarker].Cells[1].Value = m.x;
dataGridView1.Rows[rowIndexMarker].Cells[2].Value = m.y;
dataGridView1.Rows[rowIndexMarker].Cells[3].Value = m.z;
}
}
}
}
}
}
}
// update Skeleton data
for (int i = 0; i < m_FrameOfData.nSkeletons; i++)
{
NatNetML.SkeletonData sk = m_FrameOfData.Skeletons[i];
for (int j = 0; j < sk.nRigidBodies; j++)
{
// note : skeleton rigid body ids are of the form:
// parent skeleton ID : high word (upper 16 bits of int)
// rigid body id : low word (lower 16 bits of int)
NatNetML.RigidBodyData rb = sk.RigidBodies[j];
int skeletonID = HighWord(rb.ID);
int rigidBodyID = LowWord(rb.ID);
int uniqueID = skeletonID * 1000 + rigidBodyID;
int key = uniqueID.GetHashCode();
if (htRigidBodies.ContainsKey(key))
{
int rowIndex = (int)htRigidBodies[key];
if (rowIndex >= 0)
{
dataGridView1.Rows[rowIndex].Cells[1].Value = rb.x;
dataGridView1.Rows[rowIndex].Cells[2].Value = rb.y;
dataGridView1.Rows[rowIndex].Cells[3].Value = rb.z;
// Convert quaternion to eulers. Motive coordinate conventions: X(Pitch), Y(Yaw), Z(Roll), Relative, RHS
float[] quat = new float[4] {
rb.qx, rb.qy, rb.qz, rb.qw
};
float[] eulers = new float[3];
eulers = m_NatNet.QuatToEuler(quat, (int)NATEulerOrder.NAT_XYZr);
double x = RadiansToDegrees(eulers[0]); // convert to degrees
double y = RadiansToDegrees(eulers[1]);
double z = RadiansToDegrees(eulers[2]);
dataGridView1.Rows[rowIndex].Cells[4].Value = x;
dataGridView1.Rows[rowIndex].Cells[5].Value = y;
dataGridView1.Rows[rowIndex].Cells[6].Value = z;
// Marker data associated with this rigid body
for (int k = 0; k < rb.nMarkers; k++)
{
}
}
}
}
} // end skeleton update
// update labeled markers data
// remove previous dynamic marker list
// for testing only - this simple approach to grid updating too slow for large marker count use
if (false)
{
int nRows = htMarkers.Count + htRigidBodies.Count;
int nTotalRows = dataGridView1.Rows.Count;
for (int i = nRows; i < nTotalRows; i++)
{
dataGridView1.Rows.RemoveAt(nRows);
}
for (int i = 0; i < m_FrameOfData.nMarkers; i++)
{
NatNetML.Marker m = m_FrameOfData.LabeledMarkers[i];
int modelID, markerID;
m_NatNet.DecodeID(m.ID, out modelID, out markerID);
int rowIndex = dataGridView1.Rows.Add("Labeled Marker (ModelID: " + modelID + " MarkerID: " + markerID + ")");
dataGridView1.Rows[rowIndex].Cells[1].Value = m.x;
dataGridView1.Rows[rowIndex].Cells[2].Value = m.y;
dataGridView1.Rows[rowIndex].Cells[3].Value = m.z;
}
}
}