//called when data for any output pin is requested
public void Evaluate(int SpreadMax)
{
if (FFrame.SliceCount > 0)
{
// There are avaliable frames
lock (syncLock) {
//FLogger.Log(LogType.Debug, "" + m_FrameQueue.Count);
mFrameOfData = FFrame[0];
FTracked.SliceCount = mFrameOfData.nRigidBodies;
FTransform.SliceCount = mFrameOfData.nRigidBodies;
FLogger.Log(LogType.Debug, "There are " + mFrameOfData.nRigidBodies + " rigid bodies detected");
for (int i = 0; i < mFrameOfData.nRigidBodies; ++i)
{
NatNetML.RigidBodyData rb = mFrameOfData.RigidBodies[i];
// Is tracked
FTracked[i] = rb.Tracked;
// Positions
Vector3D newPos = new Vector3D(rb.x, rb.y, rb.z);
// Orientations (quaternions)
float qx, qy, qw, qz;
qx = rb.qx;
qy = rb.qy;
qz = rb.qz;
qw = rb.qw;
QuaternionNormalise(ref qx, ref qy, ref qz, ref qw);
Vector4D newQuat = new Vector4D(rb.qx, rb.qy, rb.qz, rb.qw);
//Orientations (Euler)
Vector3D euler = VMath.QuaternionToEulerYawPitchRoll(newQuat);
//Matrix transform
FTransform[i] = ToVVVVMatrix(newPos, euler);
// Marker positions
FMarkerPosition.SliceCount = rb.nMarkers;
for (int j = 0; j < FMarkerPosition.SliceCount; ++j)
{
Marker marker = rb.Markers[j];
FMarkerPosition[j] = new Vector3D(-marker.x, marker.y, marker.z);
}
}
}
}
}
private NatNetPoseData ProcessPoseData(NatNetML.FrameOfMocapData data)
{
NatNetPoseData poseData = new NatNetPoseData();
/* Parsing Simple Marker Data */
for (int i = 0; i < data.nMarkers; i++)
{
Marker marker = data.LabeledMarkers[i];
int mID = marker.ID;
// Console.WriteLine("\tMarker ({0}): \t\tpos ({0:N3}, {1:N3}, {2:N3})", mID, marker.x, marker.y, marker.z);
poseData.mPos = new Vector3(marker.x, marker.y, marker.z);
}
/* Parsing Rigid Body Frame Data */
for (int i = 0; i < mRigidBodies.Count; i++)
{
int rbID = mRigidBodies[i].ID; // Fetching rigid body IDs from the saved descriptions
for (int j = 0; j < data.nRigidBodies; j++)
{
if (rbID == data.RigidBodies[j].ID) // When rigid body ID of the descriptions matches rigid body ID of the frame data.
{
NatNetML.RigidBodyData rbData = data.RigidBodies[j]; // Received rigid body descriptions
if (rbData.Tracked == true)
{
poseData.rbPos = new Vector3(rbData.x, rbData.y, rbData.z);
poseData.rbRot = new Quaternion(rbData.qx, rbData.qy, rbData.qz, rbData.qw);
}
else
{
NatNetML.RigidBody rb = mRigidBodies[i]; // Saved rigid body descriptions
Console.WriteLine("\t{0} is not tracked in current frame", rb.Name);
}
}
}
}
return(poseData);
}
/// <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;
}
}
}
static void processFrameData(NatNetML.FrameOfMocapData data)
{
/* Parsing Rigid Body Frame Data */
for (int i = 0; i < mRigidBodies.Count; i++)
{
int rbID = mRigidBodies[i].ID; // Fetching rigid body IDs from the saved descriptions
for (int j = 0; j < data.nRigidBodies; j++)
{
if (rbID == data.RigidBodies[j].ID) // When rigid body ID of the descriptions matches rigid body ID of the frame data.
{
NatNetML.RigidBody rb = mRigidBodies[i]; // Saved rigid body descriptions
NatNetML.RigidBodyData rbData = data.RigidBodies[j]; // Received rigid body descriptions
if (rbData.Tracked == true)
{
Console.WriteLine("\tRigidBody ({0}):", rb.Name);
Console.WriteLine("\t\tpos ({0:N3}, {1:N3}, {2:N3})", rbData.x, rbData.y, rbData.z);
// Rigid Body Euler Orientation
float[] quat = new float[4] {
rbData.qx, rbData.qy, rbData.qz, rbData.qw
};
float[] eulers = new float[3];
eulers = NatNetClientML.QuatToEuler(quat, NATEulerOrder.NAT_XYZr); //Converting quat orientation into XYZ Euler representation.
double xrot = RadiansToDegrees(eulers[0]);
double yrot = RadiansToDegrees(eulers[1]);
double zrot = RadiansToDegrees(eulers[2]);
Console.WriteLine("\t\tori ({0:N3}, {1:N3}, {2:N3})", xrot, yrot, zrot);
}
else
{
Console.WriteLine("\t{0} is not tracked in current frame", rb.Name);
}
}
}
}
/* Parsing Skeleton Frame Data */
for (int i = 0; i < mSkeletons.Count; i++) // Fetching skeleton IDs from the saved descriptions
{
int sklID = mSkeletons[i].ID;
for (int j = 0; j < data.nSkeletons; j++)
{
if (sklID == data.Skeletons[j].ID) // When skeleton ID of the description matches skeleton ID of the frame data.
{
NatNetML.Skeleton skl = mSkeletons[i]; // Saved skeleton descriptions
NatNetML.SkeletonData sklData = data.Skeletons[j]; // Received skeleton frame data
Console.WriteLine("\tSkeleton ({0}):", skl.Name);
Console.WriteLine("\t\tSegment count: {0}", sklData.nRigidBodies);
/* Now, for each of the skeleton segments */
for (int k = 0; k < sklData.nRigidBodies; k++)
{
NatNetML.RigidBodyData boneData = sklData.RigidBodies[k];
/* Decoding skeleton bone ID */
int skeletonID = HighWord(boneData.ID);
int rigidBodyID = LowWord(boneData.ID);
int uniqueID = skeletonID * 1000 + rigidBodyID;
int key = uniqueID.GetHashCode();
NatNetML.RigidBody bone = (RigidBody)mHtSkelRBs[key]; //Fetching saved skeleton bone descriptions
//Outputting only the hip segment data for the purpose of this sample.
if (k == 0)
{
Console.WriteLine("\t\t{0:N3}: pos({1:N3}, {2:N3}, {3:N3})", bone.Name, boneData.x, boneData.y, boneData.z);
}
}
}
}
}
/* Parsing Force Plate Frame Data */
for (int i = 0; i < mForcePlates.Count; i++)
{
int fpID = mForcePlates[i].ID; // Fetching force plate IDs from the saved descriptions
for (int j = 0; j < data.nForcePlates; j++)
{
if (fpID == data.ForcePlates[j].ID) // When force plate ID of the descriptions matches force plate ID of the frame data.
{
NatNetML.ForcePlate fp = mForcePlates[i]; // Saved force plate descriptions
NatNetML.ForcePlateData fpData = data.ForcePlates[i]; // Received forceplate frame data
Console.WriteLine("\tForce Plate ({0}):", fp.Serial);
// Here we will be printing out only the first force plate "subsample" (index 0) that was collected with the mocap frame.
for (int k = 0; k < fpData.nChannels; k++)
{
Console.WriteLine("\t\tChannel {0}: {1}", fp.ChannelNames[k], fpData.ChannelData[k].Values[0]);
}
}
}
}
Console.WriteLine("\n");
}
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()
{
// 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
}
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;
}
}
}
}
}
}
}
}
/// <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 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] != null) && (rb.Markers[0].ID != -1))
{
string name;
RigidBody rbDef = FindRB(rb.ID);
if (rbDef != null)
{
name = rbDef.Name;
}
else
{
name = rb.ID.ToString();
}
int rowIndex = dataGridView1.Rows.Add("RigidBody: " + 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 = 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.");
}
//重点 这里是更新位置信息 而且Y和Z得对调一下
WX = Convert.ToDouble(rb.x * m_ServerToMillimeters / 1000);
WY = Convert.ToDouble(rb.z * m_ServerToMillimeters / 1000);
WZ = Convert.ToDouble(rb.y * m_ServerToMillimeters / 1000);
dataGridView1.Rows[rowIndex].Cells[1].Value = WX;
dataGridView1.Rows[rowIndex].Cells[2].Value = WY;
dataGridView1.Rows[rowIndex].Cells[3].Value = WZ;
//数据进入队列缓存
Queue(WX, WY, WZ);
// update Marker data associated with this rigid body
// 更新点的位置数据
for (int j = 0; j < rb.nMarkers; j++)
{
if (rb.Markers[j].ID != -1)
{
string name;
RigidBody rbDef = FindRB(rb.ID);
if (rbDef != null)
{
name = rbDef.Name;
}
else
{
name = rb.ID.ToString();
}
String strUniqueName = 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.z;
dataGridView1.Rows[rowIndexMarker].Cells[3].Value = m.y;
}
}
}
}
}
}
}
static int processFrameData(NatNetML.FrameOfMocapData data, int index)
{
int index2 = 1;
string startupPath = Environment.CurrentDirectory;
//Console.WriteLine(startupPath);
string pathCmd = string.Format("cd {0}\\..\\..", startupPath);
//Console.WriteLine(pathCmd);
// Create the MATLAB instance
MLApp.MLApp matlab = new MLApp.MLApp();
// Change to the directory where the function is located
matlab.Execute(pathCmd);
Console.WriteLine("labeled markers: " + data.nMarkers);
/* Parsing Rigid Body Frame Data */
for (int i = 0; i < mRigidBodies.Count; i++)
{
int rbID = mRigidBodies[i].ID; // Fetching rigid body IDs from the saved descriptions
for (int j = 0; j < data.nRigidBodies; j++)
{
if (rbID == data.RigidBodies[j].ID) // When rigid body ID of the descriptions matches rigid body ID of the frame data.
{
NatNetML.RigidBody rb = mRigidBodies[i]; // Saved rigid body descriptions
NatNetML.RigidBodyData rbData = data.RigidBodies[j]; // Received rigid body descriptions
if (rbData.Tracked == true && HLdata.Count > 0 && index > 0)
{
NatNetML.Marker marker = data.LabeledMarkers[i];
int mID = marker.ID;
//Console.WriteLine("\tMarker ({0}):", mID);
//Console.WriteLine("\t\tpos ({0:N3}, {1:N3}, {2:N3})", marker.x, marker.y, marker.z);
//Console.WriteLine("\tRigidBody ({0}):", rb.Name);
//Console.WriteLine("\t\tpos ({0:N3}, {1:N3}, {2:N3})", rbData.x, rbData.y, rbData.z);
// Rigid Body Euler Orientation
double[] rbquat = new double[4] {
rbData.qx, rbData.qy, rbData.qz, rbData.qw
};
double[] rbpos = new double[3] {
rbData.x, rbData.y, rbData.z
};
double[] mkpos = new double[3] {
marker.x, marker.y, marker.z
};
int idxHl = HLdata.Count - 1;
double[] kbP = new double[3] {
(double)HLdata[idxHl][0], (double)HLdata[idxHl][1], (double)HLdata[idxHl][2]
};
double[] kbQ = new double[4] {
(double)HLdata[idxHl][3], (double)HLdata[idxHl][4], (double)HLdata[idxHl][5], (double)HLdata[idxHl][6]
};
double[] hlPs = new double[3] {
(double)HLdata[idxHl][7], (double)HLdata[idxHl][8], (double)HLdata[idxHl][9]
};
double[] hlQs = new double[4] {
(double)HLdata[idxHl][10], (double)HLdata[idxHl][11], (double)HLdata[idxHl][12], (double)HLdata[idxHl][13]
};
bool mlEval = true;
if (mlEval)
{
object newmarkerpos = null;
// object result = null;
//float[] A = { 1, 2 };
//float[] B = { 2, 2 };
// matlab.Feval("add", 1, out result, A, B);
//object[] res2 = result as object[];
//Console.WriteLine(Convert.ToSingle(res2[0]));
//Console.WriteLine(res2[1]);
matlab.Feval("trans", 3, out newmarkerpos, mkpos, rbpos, rbquat, kbP, kbQ, hlPs, hlQs);
object[] res = newmarkerpos as object[];
double xpos = Convert.ToDouble(res[0]);
double ypos = Convert.ToDouble(res[1]);
double zpos = Convert.ToDouble(res[2]);
double[] newmarkerpos1 = new double[3] {
xpos, ypos, zpos
};
object result1 = null;
//Console.WriteLine(index);
matlab.Feval("recognition", 2, out result1, newmarkerpos1, index);
object[] res2 = result1 as object[];
Console.WriteLine(res2[0]);
Console.WriteLine(res2[1]);
index2 = Convert.ToInt32(res2[0]);
}
}
else
{
if (rbData.Tracked)
{
Console.WriteLine("\t HLData {0}, index {1}", HLdata.Count, index);
}
else
{
Console.WriteLine("\t{0} is not tracked in current frame", rb.Name);
}
}
}
}
}
//Console.WriteLine(index2);
return(index2);
}
//static uint GPS_LEAPSECONDS_MILLIS = 18000;
static void processFrameData(NatNetML.FrameOfMocapData data)
{
bool data_gogo = true;
/* Parsing Rigid Body Frame Data */
for (int i = 0; i < mRigidBodies.Count; i++)
{
int rbID = mRigidBodies[i].ID; // Fetching rigid body IDs from the saved descriptions
for (int j = 0; j < data.nRigidBodies; j++)
{
if (rbID == data.RigidBodies[j].ID) // When rigid body ID of the descriptions matches rigid body ID of the frame data.
{
NatNetML.RigidBody rb = mRigidBodies[i]; // Saved rigid body descriptions
NatNetML.RigidBodyData rbData = data.RigidBodies[j]; // Received rigid body descriptions
if (rbData.Tracked == true)
{
#if DEBUG_MSG
Console.WriteLine("\tRigidBody ({0}):", rb.Name);
Console.WriteLine("\t\tpos ({0:N3}, {1:N3}, {2:N3})", rbData.x, rbData.y, rbData.z);
// Rigid Body Euler Orientation
float[] quat = new float[4] {
rbData.qx, rbData.qy, rbData.qz, rbData.qw
};
float[] eulers = new float[3];
eulers = NatNetClientML.QuatToEuler(quat, NATEulerOrder.NAT_XYZr); //Converting quat orientation into XYZ Euler representation.
double xrot = RadiansToDegrees(eulers[0]);
double yrot = RadiansToDegrees(eulers[1]);
double zrot = RadiansToDegrees(eulers[2]);
Console.WriteLine("\t\tori ({0:N3}, {1:N3}, {2:N3})", xrot, yrot, zrot);
#endif
if (drones.ContainsKey(rb.Name))
{
DroneData drone = drones[rb.Name];
drone.lost_count = 0;
long cur_ms = stopwatch.ElapsedMilliseconds;
if (drone.send_count > 0)
{
drone.send_count--;
}
else if (data_gogo)
{
drone.send_count = 10;
data_gogo = false;
MAVLink.mavlink_att_pos_mocap_t att_pos = new MAVLink.mavlink_att_pos_mocap_t();
att_pos.time_usec = (ulong)(cur_ms * 1000);
att_pos.x = rbData.x; //north
att_pos.y = rbData.z; //east
att_pos.z = -rbData.y; //down
att_pos.q = new float[4] {
rbData.qw, rbData.qx, rbData.qz, -rbData.qy
};
byte[] pkt = mavlinkParse.GenerateMAVLinkPacket10(MAVLink.MAVLINK_MSG_ID.ATT_POS_MOCAP, att_pos);
if (drone.lastTime >= 0)
{
MAVLink.mavlink_vision_speed_estimate_t vis_speed = new MAVLink.mavlink_vision_speed_estimate_t();
float total_s = (cur_ms - drone.lastTime) * 0.001f;
vis_speed.x = (rbData.x - drone.lastPosN) / total_s;
vis_speed.y = (rbData.z - drone.lastPosE) / total_s;
vis_speed.z = (-rbData.y - drone.lastPosD) / total_s;
vis_speed.usec = (ulong)(cur_ms * 1000);
byte[] pkt2 = mavlinkParse.GenerateMAVLinkPacket10(MAVLink.MAVLINK_MSG_ID.VISION_SPEED_ESTIMATE, vis_speed);
int total_len = pkt.Length + pkt2.Length;
byte[] uwb_data = new byte[10 + total_len + 1];
uwb_data[0] = 0x54;
uwb_data[1] = 0xf1;
uwb_data[2] = 0xff;
uwb_data[3] = 0xff;
uwb_data[4] = 0xff;
uwb_data[5] = 0xff;
uwb_data[6] = 2;
uwb_data[7] = drone.uwb_tag_id;
uwb_data[8] = (byte)(total_len & 0xff);
uwb_data[9] = (byte)(total_len >> 16);
Array.Copy(pkt, 0, uwb_data, 10, pkt.Length);
Array.Copy(pkt2, 0, uwb_data, 10 + pkt.Length, pkt2.Length);
byte chk_sum = 0;
foreach (byte uwb_data_byte in uwb_data)
{
chk_sum += uwb_data_byte;
}
uwb_data[uwb_data.Length - 1] = chk_sum;
mavSock.SendTo(uwb_data, drone.ep);
}
}
drone.lastTime = cur_ms;
drone.lastPosN = rbData.x;
drone.lastPosE = rbData.z;
drone.lastPosD = -rbData.y;
}
}
else
{
Console.WriteLine("\t{0} is not tracked in current frame", rb.Name);
/*if (drones.ContainsKey(rb.Name))
* {
* DroneData drone = drones[rb.Name];
* drone.lost_count++;
* if (drone.lost_count > 3)
* {
* drone.lost_count = 0;
* MAVLink.mavlink_gps_input_t gps_input = new MAVLink.mavlink_gps_input_t();
* gps_input.gps_id = 0;
* gps_input.fix_type = (byte)MAVLink.GPS_FIX_TYPE.NO_FIX;
* byte[] pkt = mavlinkParse.GenerateMAVLinkPacket10(MAVLink.MAVLINK_MSG_ID.GPS_INPUT, gps_input);
* mavSock.SendTo(pkt, drone.ep);
* }
* }*/
}
}
}
}
/* Parsing Skeleton Frame Data */
for (int i = 0; i < mSkeletons.Count; i++) // Fetching skeleton IDs from the saved descriptions
{
int sklID = mSkeletons[i].ID;
for (int j = 0; j < data.nSkeletons; j++)
{
if (sklID == data.Skeletons[j].ID) // When skeleton ID of the description matches skeleton ID of the frame data.
{
NatNetML.Skeleton skl = mSkeletons[i]; // Saved skeleton descriptions
NatNetML.SkeletonData sklData = data.Skeletons[j]; // Received skeleton frame data
Console.WriteLine("\tSkeleton ({0}):", skl.Name);
Console.WriteLine("\t\tSegment count: {0}", sklData.nRigidBodies);
/* Now, for each of the skeleton segments */
for (int k = 0; k < sklData.nRigidBodies; k++)
{
NatNetML.RigidBodyData boneData = sklData.RigidBodies[k];
/* Decoding skeleton bone ID */
int skeletonID = HighWord(boneData.ID);
int rigidBodyID = LowWord(boneData.ID);
int uniqueID = skeletonID * 1000 + rigidBodyID;
int key = uniqueID.GetHashCode();
NatNetML.RigidBody bone = (RigidBody)mHtSkelRBs[key]; //Fetching saved skeleton bone descriptions
//Outputting only the hip segment data for the purpose of this sample.
if (k == 0)
{
Console.WriteLine("\t\t{0:N3}: pos({1:N3}, {2:N3}, {3:N3})", bone.Name, boneData.x, boneData.y, boneData.z);
}
}
}
}
}
/* Parsing Force Plate Frame Data */
for (int i = 0; i < mForcePlates.Count; i++)
{
int fpID = mForcePlates[i].ID; // Fetching force plate IDs from the saved descriptions
for (int j = 0; j < data.nForcePlates; j++)
{
if (fpID == data.ForcePlates[j].ID) // When force plate ID of the descriptions matches force plate ID of the frame data.
{
NatNetML.ForcePlate fp = mForcePlates[i]; // Saved force plate descriptions
NatNetML.ForcePlateData fpData = data.ForcePlates[i]; // Received forceplate frame data
Console.WriteLine("\tForce Plate ({0}):", fp.Serial);
// Here we will be printing out only the first force plate "subsample" (index 0) that was collected with the mocap frame.
for (int k = 0; k < fpData.nChannels; k++)
{
Console.WriteLine("\t\tChannel {0}: {1}", fp.ChannelNames[k], fpData.ChannelData[k].Values[0]);
}
}
}
}
#if DEBUG_MSG
Console.WriteLine("\n");
#endif
}
//called when data for any output pin is requested
public void Evaluate(int SpreadMax)
{
if (FInputFrameData[0] != null)
{
int _rigidBodyCount = FInputFrameData[0].nRigidBodies;
FOutputID.SliceCount = _rigidBodyCount;
FOutputTransform.SliceCount = _rigidBodyCount;
FOutputTracked.SliceCount = _rigidBodyCount;
//FOutputMarkers.SliceCount = _rigidBodyCount;
//FOutputNMarkers.SliceCount = _rigidBodyCount;
FOutputMeanError.SliceCount = _rigidBodyCount;
for (int i = 0; i < _rigidBodyCount; i++)
{
NatNetML.RigidBodyData rb = FInputFrameData[0].RigidBodies[i];
List <NatNetML.DataDescriptor> descs = new List <NatNetML.DataDescriptor>();
;
FOutputID[i] = rb.ID;
//FOutputName[i] = rb.Name;
FOutputTracked[i] = rb.Tracked;
FOutputMeanError[i] = rb.MeanError;
Matrix4x4 _rigidBody;
double qw = rb.qw;
double qx = rb.qx;
double qy = rb.qy;
double qz = rb.qz;
double n = 1.0f / Math.Sqrt(qx * qx + qy * qy + qz * qz + qw * qw);
qx *= n;
qy *= n;
qz *= n;
qw *= n;
_rigidBody.m11 = 1.0f - 2.0f * qy * qy - 2.0f * qz * qz;
_rigidBody.m12 = 2.0f * qx * qy - 2.0f * qz * qw;
_rigidBody.m13 = 2.0f * qx * qz + 2.0f * qy * qw;
_rigidBody.m14 = 0;
_rigidBody.m21 = 2.0f * qx * qy + 2.0f * qz * qw;
_rigidBody.m22 = 1.0f - 2.0f * qx * qx - 2.0f * qz * qz;
_rigidBody.m23 = 2.0f * qy * qz - 2.0f * qx * qw;
_rigidBody.m24 = 0;
_rigidBody.m31 = 2.0f * qx * qz - 2.0f * qy * qw;
_rigidBody.m32 = 2.0f * qy * qz + 2.0f * qx * qw;
_rigidBody.m33 = 1.0f - 2.0f * qx * qx - 2.0f * qy * qy;
_rigidBody.m34 = 0;
_rigidBody.m41 = rb.x;
_rigidBody.m42 = rb.y;
_rigidBody.m43 = rb.z;
_rigidBody.m44 = 1;
FOutputTransform[i] = _rigidBody;
}
}
else
{
FOutputID.SliceCount = 0;
FOutputTransform.SliceCount = 0;
FOutputTracked.SliceCount = 0;
//FOutputMarkers.SliceCount = 0;
//FOutputNMarkers.SliceCount = 0;
FOutputMeanError.SliceCount = 0;
}
}
private NatNetPoseData ProcessPoseData(NatNetML.FrameOfMocapData data)
{
NatNetPoseData poseData = new NatNetPoseData();
/* Parsing Simple Marker Data */
for (int i = 0; i < data.nMarkers; i++)
{
Marker marker = data.LabeledMarkers[i];
int mID = marker.ID;
// Console.WriteLine("\tMarker ({0}): \t\tpos ({0:N3}, {1:N3}, {2:N3})", mID, marker.x, marker.y, marker.z);
poseData.mPos = new Vector3(marker.x, marker.y, marker.z);
}
/* Parsing Rigid Body Frame Data */
for (int i = 0; i < mRigidBodies.Count; i++)
{
int rbID = mRigidBodies[i].ID; // Fetching rigid body IDs from the saved descriptions
for (int j = 0; j < data.nRigidBodies; j++)
{
if (rbID == data.RigidBodies[j].ID) // When rigid body ID of the descriptions matches rigid body ID of the frame data.
{
NatNetML.RigidBody rb = mRigidBodies[i]; // Saved rigid body descriptions
NatNetML.RigidBodyData rbData = data.RigidBodies[j]; // Received rigid body descriptions
if (rbData.Tracked == true)
{
poseData.rbPos = new Vector3(rbData.x, rbData.y, rbData.z);
poseData.rbRot = new Quaternion(rbData.qx, rbData.qy, rbData.qz, rbData.qw);
//Console.WriteLine("\tRigidBody ({0}):", rb.Name);
//Console.WriteLine("\t\tpos ({0:N3}, {1:N3}, {2:N3})", rbData.x, rbData.y, rbData.z);
// Rigid Body Euler Orientation
float[] quat = new float[4] {
rbData.qx, rbData.qy, rbData.qz, rbData.qw
};
float[] eulers = new float[3];
eulers = NatNetClientML.QuatToEuler(quat, NATEulerOrder.NAT_XYZr); //Converting quat orientation into XYZ Euler representation.
double xrot = RadiansToDegrees(eulers[0]);
double yrot = RadiansToDegrees(eulers[1]);
double zrot = RadiansToDegrees(eulers[2]);
//Console.WriteLine("\t\tori ({0:N3}, {1:N3}, {2:N3})", xrot, yrot, zrot);
string display = string.Format("\tRigidBody ({0}):", rb.Name);
display += string.Format("\t\tpos ({0:N3}, {1:N3}, {2:N3})", rbData.x, rbData.y, rbData.z);
display += string.Format("\t\tori ({0:N3}, {1:N3}, {2:N3})", xrot, yrot, zrot);
//Console.WriteLine(display);
}
else
{
Console.WriteLine("\t{0} is not tracked in current frame", rb.Name);
}
}
}
}
return(poseData);
}