private void WriteFrame(FrameOfMocapData data, TelemetryData telemetry)
{
String str = "";
bool recordMarkerData = true;
//bool recordForcerData = false;
//bool recordRBData = false;
str += data.fTimestamp.ToString("F3") + "\t";
str += telemetry.TransmitLatency.ToString("F3") + "\t";
str += telemetry.TotalLatency.ToString("F3") + "\t";
str += telemetry.DroppedFrames.ToString() + "\t";
// 'all' markerset data
if (recordMarkerData)
{
for (int i = 0; i < m_FrameOfData.nMarkerSets; i++)
{
NatNetML.MarkerSetData ms = m_FrameOfData.MarkerSets[i];
if (ms.MarkerSetName == "all")
{
for (int j = 0; j < ms.nMarkers; j++)
{
str += ms.Markers[j].x.ToString("F3") + "\t";
str += ms.Markers[j].y.ToString("F3") + "\t";
str += ms.Markers[j].z.ToString("F3") + "\t";
}
}
}
}
mWriter.WriteLine(str);
}
/// <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;
}
}
}
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];
// testing - tools inverts qz for packetizing/sending, so invert it back
float qx, qy, qw, qz;
qx = rb.qx;
qy = rb.qy;// *-1.0f;
qz = -rb.qz;
qw = rb.qw;
// quats coming from Tools are already normalized
//QuaternionNormalise(ref qx, ref qy, ref qw, ref qz);
//qy = qy * 0.5f;
int key = rb.ID.GetHashCode();
if (htRigidBodies.ContainsKey(key))
{
int rowIndex = (int)htRigidBodies[key];
if (rowIndex >= 0)
{
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;
double y, z, x;
QuaternionToEuler(qx, qy, qz, qw, out y, out z, out x);
//y *= -1.0f;
z *= -1.0f;
y = RadiansToDegrees(y);
z = RadiansToDegrees(z);
x = RadiansToDegrees(x);
dataGridView1.Rows[rowIndex].Cells[4].Value = y;
dataGridView1.Rows[rowIndex].Cells[5].Value = z;
dataGridView1.Rows[rowIndex].Cells[6].Value = x;
// Marker data associated with this rigid body
for (int j = 0; j < rb.nMarkers; j++)
{
}
}
}
}
// 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++)
{
NatNetML.RigidBodyData rb = sk.RigidBodies[j];
int key = rb.ID.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;
double y, z, x;
QuaternionToEuler(rb.qx, rb.qy, rb.qz, rb.qw, out y, out z, out x);
y = RadiansToDegrees(y);
z = RadiansToDegrees(z);
x = RadiansToDegrees(x);
dataGridView1.Rows[rowIndex].Cells[4].Value = y;
dataGridView1.Rows[rowIndex].Cells[5].Value = z;
dataGridView1.Rows[rowIndex].Cells[6].Value = x;
// Marker data associated with this rigid body
for (int k = 0; k < rb.nMarkers; k++)
{
}
}
}
}
} // end skeleton update
}
public double[] get_last_frame(string track_type, int object_num)
{
/*
* when asking for data set, default to the first marker set
*/
//grab a frame
FrameOfMocapData data = mNatNet.GetLastFrameOfData();
//
switch (track_type)
{
default:
Console.WriteLine("Unrecognized track object type {0}", track_type);
return(null);
case "markers": //defailt to return first marker of the firsr marker set
NatNetML.MarkerSetData ms = data.MarkerSets[0];
if (object_num < ms.nMarkers)
{
return(new double[3] {
ms.Markers[object_num].x * m_ServerToMillimeters,
ms.Markers[object_num].x * m_ServerToMillimeters,
ms.Markers[object_num].z * m_ServerToMillimeters
});
}
else
{
Console.WriteLine("the specified object number( {0} ) is greater than the total markers ({1})", object_num, ms.nMarkers);
return(null);
}
case "rb":
if (debug)
{
Console.WriteLine("asked for rigid body number {0}", object_num);
Console.WriteLine("number of rigid body: {0}", data.nRigidBodies);
}
if (object_num < data.nRigidBodies)
{
NatNetML.RigidBodyData rb = data.RigidBodies[object_num];
//get the quotenions
float[] quat = new float[4] {
rb.qx, rb.qy, rb.qz, rb.qw
};
float[] eulers = new float[3];
eulers = NatNetClientML.QuatToEuler(quat, NATEulerOrder.NAT_XYZr);
double x = RadiansToDegrees(eulers[0]); // convert to degrees
double y = RadiansToDegrees(eulers[1]);
double z = RadiansToDegrees(eulers[2]);
if (debug)
{
Console.WriteLine("received x y z positions");
Console.WriteLine("{0}, {1}, {2}", x, y, z);
stopwatch.Stop();
Console.WriteLine("Reading the quene takes {0} ms", stopwatch.ElapsedMilliseconds);
}
return(new double[7] {
rb.x *m_ServerToMillimeters, //mm for obvious reaons
rb.y *m_ServerToMillimeters,
rb.z *m_ServerToMillimeters,
x, y, z, data.iFrame
}); //angles in degrees
}
else
{
return(null);
}
}
}
private void UpdateDataGrid()
{
broadcast();
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();
float[] quat2 = new float[4] {
rb.qx, rb.qy, rb.qz, rb.qw
};
float[] euler2 = new float[3];
euler2 = m_NatNet.QuatToEuler(quat2, (int)NATEulerOrder.NAT_XYZr);
/*double x = RadiansToDegrees(eulers[0]);
* double y = RadiansToDegrees(eulers[1]);
* double z = RadiansToDegrees(eulers[2]);*/
DateTime dt = DateTime.Now;
int t = ((dt.Hour * 60 + dt.Minute) * 60 + dt.Second) * 1000 + dt.Millisecond;
// 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)
{
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]);
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;
}
}
}
}
}
}
}
}
