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 = m_NatNet.QuatToEuler(quat, (int)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)htSkelRBs[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");
}
static void parseSeverDescriptor(NatNetML.ServerDescription server)
{
Console.WriteLine("Server Info:");
Console.WriteLine("\tHost: {0}", server.HostComputerName);
Console.WriteLine("\tApplication Name: {0}", server.HostApp);
Console.WriteLine("\tApplication Version: {0}.{1}.{2}.{3}", server.HostAppVersion[0], server.HostAppVersion[1], server.HostAppVersion[2], server.HostAppVersion[3]);
Console.WriteLine("\tNatNet Version: {0}.{1}.{2}.{3}\n", server.NatNetVersion[0], server.NatNetVersion[1], server.NatNetVersion[2], server.NatNetVersion[3]);
}
// [NatNet] m_NatNet_OnFrameReady will be called when a frame of Mocap
// data has is received from the server application.
//
// Note: This callback is on the network service thread, so it is
// important to return from this function quickly as possible
// to prevent incoming frames of data from buffering up on the
// network socket.
//
// Note: "data" is a reference structure to the current frame of data.
// NatNet re-uses this same instance for each incoming frame, so it should
// not be kept (the values contained in "data" will become replaced after
// this callback function has exited).
void m_NatNet_OnFrameReady(NatNetML.FrameOfMocapData data, NatNetML.NatNetClientML client)
{
Int64 currTime = timer.Value;
if(lastTime !=0)
{
// Get time elapsed in tenths of a millisecond.
Int64 timeElapsedInTicks = currTime - lastTime;
Int64 timeElapseInTenthsOfMilliseconds = (timeElapsedInTicks * 10000) / timer.Frequency;
// uncomment for timing info
//OutputMessage("Frame Delivered: (" + timeElapseInTenthsOfMilliseconds.ToString() + ") FrameTimestamp: " + data.fLatency);
}
lock(syncLock)
{
m_FrameQueue.Clear();
m_FrameQueue.Enqueue(data);
}
lastTime = currTime;
}
/// <summary>
/// [NatNet] parseFrameData will be called when a frame of Mocap
/// data has is received from the server application.
///
/// Note: This callback is on the network service thread, so it is
/// important to return from this function quickly as possible
/// to prevent incoming frames of data from buffering up on the
/// network socket.
///
/// Note: "data" is a reference structure to the current frame of data.
/// NatNet re-uses this same instance for each incoming frame, so it should
/// not be kept (the values contained in "data" will become replaced after
/// this callback function has exited).
/// </summary>
/// <param name="data">The actual frame of mocap data</param>
/// <param name="client">The NatNet client instance</param>
static void fetchFrameData(NatNetML.FrameOfMocapData data, NatNetML.NatNetClientML client)
{
/* Exception handler for cases where assets are added or removed.
Data description is re-obtained in the main function so that contents
in the frame handler is kept minimal. */
if (( data.bTrackingModelsChanged == true || data.nRigidBodies != mRigidBodies.Count || data.nSkeletons != mSkeletons.Count || data.nForcePlates != mForcePlates.Count))
{
assetChanged = true;
}
/* Processing and ouputting frame data every 200th frame.
This conditional statement is included in order to simplify the program output */
if(data.iFrame % 200 == 0)
{
if (data.bRecording == false)
Console.WriteLine("Frame #{0} Received:", data.iFrame);
else if (data.bRecording == true)
Console.WriteLine("[Recording] Frame #{0} Received:", data.iFrame);
processFrameData(data);
}
}
static void ProcessFrameOfData(ref NatNetML.FrameOfMocapData data)
{
// detect and reported any 'reported' frame drop (as reported by server)
if (m_fLastFrameTimestamp != 0.0f)
{
double framePeriod = 1.0f / m_ServerFramerate;
double thisPeriod = data.fTimestamp - m_fLastFrameTimestamp;
double fudgeFactor = 0.002f; // 2 ms
if ((thisPeriod - framePeriod) > fudgeFactor)
{
//Console.WriteLine("Frame Drop: ( ThisTS: " + data.fTimestamp.ToString("F3") + " LastTS: " + m_fLastFrameTimestamp.ToString("F3") + " )");
mDroppedFrames++;
}
}
// check and report frame drop (frame id based)
if (mLastFrame != 0)
{
if ((data.iFrame - mLastFrame) != 1)
{
//Console.WriteLine("Frame Drop: ( ThisFrame: " + data.iFrame.ToString() + " LastFrame: " + mLastFrame.ToString() + " )");
//mDroppedFrames++;
}
}
// [NatNet] Add the incoming frame of mocap data to our frame queue,
// Note: the frame queue is a shared resource with the UI thread, so lock it while writing
lock (syncLock)
{
// [optional] clear the frame queue before adding a new frame
m_FrameQueue.Clear();
FrameOfMocapData deepCopy = new FrameOfMocapData(data);
m_FrameQueue.Enqueue(deepCopy);
}
mLastFrame = data.iFrame;
m_fLastFrameTimestamp = data.fTimestamp;
}
/// <summary>
/// [NatNet] m_NatNet_OnFrameReady will be called when a frame of Mocap
/// data has is received from the server application.
///
/// Note: This callback is on the network service thread, so it is
/// important to return from this function quickly as possible
/// to prevent incoming frames of data from buffering up on the
/// network socket.
///
/// Note: "data" is a reference structure to the current frame of data.
/// NatNet re-uses this same instance for each incoming frame, so it should
/// not be kept (the values contained in "data" will become replaced after
/// this callback function has exited).
/// </summary>
/// <param name="data">The actual frame of mocap data</param>
/// <param name="client">The NatNet client instance</param>
static void m_NatNet_OnFrameReady(NatNetML.FrameOfMocapData data, NatNetML.NatNetClientML client)
{
double elapsedIntraMS = 0.0f;
QueryPerfCounter intraTimer = new QueryPerfCounter();
intraTimer.Start();
// detect and report and 'measured' frame drop (as measured by client)
m_FramePeriodTimer.Stop();
double elapsedMS = m_FramePeriodTimer.Duration();
ProcessFrameOfData(ref data);
// report if we are taking too long, which blocks packet receiving, which if long enough would result in socket buffer drop
intraTimer.Stop();
elapsedIntraMS = intraTimer.Duration();
if (elapsedIntraMS > 5.0f)
{
Console.WriteLine("Warning : Frame handler taking too long: " + elapsedIntraMS.ToString("F2"));
}
m_FramePeriodTimer.Start();
}
// [NatNet] m_NatNet_OnFrameReady will be called when a frame of Mocap
// data has is received from the server application.
//
// Note: This callback is on the network service thread, so it is
// important to return from this function quickly as possible
// to prevent incoming frames of data from buffering up on the
// network socket.
//
// Note: "data" is a reference structure to the current frame of data.
// NatNet re-uses this same instance for each incoming frame, so it should
// not be kept (the values contained in "data" will become replaced after
// this callback function has exited).
void m_NatNet_OnFrameReady(NatNetML.FrameOfMocapData data, NatNetML.NatNetClientML client)
{
lock (syncLock)
{
m_FrameQueue.Clear();
m_FrameQueue.Enqueue(data);
}
}
/// <summary>
/// [NatNet] m_NatNet_OnFrameReady will be called when a frame of Mocap
/// data has is received from the server application.
///
/// Note: This callback is on the network service thread, so it is
/// important to return from this function quickly as possible
/// to prevent incoming frames of data from buffering up on the
/// network socket.
///
/// Note: "data" is a reference structure to the current frame of data.
/// NatNet re-uses this same instance for each incoming frame, so it should
/// not be kept (the values contained in "data" will become replaced after
/// this callback function has exited).
/// </summary>
/// <param name="data">The actual frame of mocap data</param>
/// <param name="client">The NatNet client instance</param>
void m_NatNet_OnFrameReady(NatNetML.FrameOfMocapData data, NatNetML.NatNetClientML client)
{
double elapsedIntraMS = 0.0f;
QueryPerfCounter intraTimer = new QueryPerfCounter();
intraTimer.Start();
// check and report frame arrival period (time elapsed since previous frame arrived)
m_FramePeriodTimer.Stop();
double elapsedMS = m_FramePeriodTimer.Duration();
if ( (mLastFrame % 100) == 0)
{
OutputMessage("FrameID:" + data.iFrame + " Timestamp: " + data.fTimestamp + " Period:" + elapsedMS);
}
// check and report frame drop
if ((mLastFrame != 0) && ((data.iFrame - mLastFrame) != 1))
{
OutputMessage("Frame Drop: ( ThisFrame: " + data.iFrame.ToString() + " LastFrame: " + mLastFrame.ToString() + " )");
}
// [NatNet] Add the incoming frame of mocap data to our frame queue,
// Note: the frame queue is a shared resource with the UI thread, so lock it while writing
lock (syncLock)
{
// [optional] clear the frame queue before adding a new frame
m_FrameQueue.Clear();
FrameOfMocapData deepCopy = new FrameOfMocapData(data);
m_FrameQueue.Enqueue(deepCopy);
}
intraTimer.Stop();
elapsedIntraMS = intraTimer.Duration();
if (elapsedIntraMS > 5.0f)
{
OutputMessage("Warning : Frame handler taking too long: " + elapsedIntraMS.ToString("F2"));
}
mLastFrame = data.iFrame;
m_FramePeriodTimer.Start();
}
// [NatNet] m_NatNet_OnFrameReady will be called when a frame of Mocap data has is received
// from the server application.
void m_NatNet_OnFrameReady(NatNetML.FrameOfMocapData data, NatNetML.NatNetClientML client)
{
m_FrameQueue.Enqueue(data);
}
