private void OnOptiTrackFrameReady(FrameOfMocapData data, NatNetClientML client)
{
if (data != null)
{
List <Tuple <RigidBody, RigidBodyData> > rigidBodyInfo = new List <Tuple <RigidBody, RigidBodyData> >();
for (int i = 0; i < data.nRigidBodies; i++)
{
RigidBodyData rigidBodyData = data.RigidBodies[i];
int id = rigidBodyData.ID;
RigidBody rigidBody = GetRigidBody(id);
if (rigidBody != null)
{
rigidBodyInfo.Add(new Tuple <RigidBody, RigidBodyData>(rigidBody, rigidBodyData));
}
}
List <Marker> markerInfo = new List <Marker>();
for (int i = 0; i < data.nOtherMarkers; i++)
{
if (data.OtherMarkers[i].ID == -1)
{
markerInfo.Add(data.OtherMarkers[i]);
}
}
TrackingUpdated?.Invoke(this, new OptiTrackTrackingEventArgs(
rigidBodyInfo, markerInfo, m_natNetClient, data.fTimestamp));
}
}
//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");
}
private void NatNetClientOnOnFrameReady(FrameOfMocapData data, 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 ||
data.nMarkerSets != _markerSets.Count ||
data.nRigidBodies != _rigidBodies.Count ||
data.nSkeletons != _skeletons.Count))
{
Logger.Debug("\n===============================================================================\n");
Logger.Debug("Change in the list of the assets. Refetching the descriptions");
/* Clear out existing lists */
_dataDescriptor.Clear();
_markerSets.Clear();
_rigidBodies.Clear();
_skeletons.Clear();
_nameById.Clear();
/* [NatNet] Re-fetch the updated list of descriptors */
FetchDataDescriptor();
Logger.Debug("===============================================================================\n");
FireDataDescriptionChanged();
}
FireOnFrameReady(data);
}
private void ExtractRigidBodies(FrameOfMocapData data, IDictionary <string, OptiTrackBody> markerSets, ref MocapFrame handledFrame)
{
for (var i = 0; i < data.nRigidBodies; i++)
{
var rbData = data.RigidBodies[i]; // Received rigid body descriptions
if (!rbData.Tracked)
{
continue;
}
// add the rigid body with centroid and rotation
var rb = new OptiTrackBody(rbData.ID.ToString())
{
Centroid = new Vector3(rbData.x, rbData.y, rbData.z) * TranslationUnitMultiplier,
Rotation = new Quaternion(rbData.qx, rbData.qy, rbData.qz, rbData.qw),
BodyType = Body.EBodyType.RigidBody
};
// in addition add each marker as point to the body
var name = _client.NameById(rbData.ID);
if (markerSets.TryGetValue(name, out var ms))
{
rb.Name = name;
foreach (var point in ms.Points)
{
rb.Points.Add(point);
}
markerSets.Remove(name);
}
handledFrame.Bodies.Add(rb); // Add to MocapFrame list of bodies
}
}
public virtual void FireOnFrameReady(FrameOfMocapData data)
{
if (OnFrameReady != null)
{
OnFrameReady(data);
}
}
private void ExtractSkeletons(FrameOfMocapData data, ref MocapFrame handledFrame)
{
for (var i = 0; i < data.nSkeletons; i++)
{
var sklData = data.Skeletons[i]; // Received skeleton frame data
var skl = new Skeleton(sklData.ID.ToString())
{
BodyType = Body.EBodyType.Skeleton
};
/* Now, for each of the skeleton segments */
for (var j = 0; j < sklData.nRigidBodies; j++)
{
var boneData = sklData.RigidBodies[j];
var bone = new OptiTrackBody(boneData.ID.ToString())
{
Centroid = new Vector3(boneData.x, boneData.y, boneData.z) * TranslationUnitMultiplier,
Rotation = new Quaternion(boneData.qx, boneData.qy, boneData.qz, boneData.qw)
};
skl.RigidBodies.Add(bone); // Add bone to skeleton
}
handledFrame.Bodies.Add(skl);
}
}
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);
}
private IDictionary <string, OptiTrackBody> ExtractMarkerSets(FrameOfMocapData data)
{
var markerSets = new Dictionary <string, OptiTrackBody>();
for (var i = 0; i < data.nMarkerSets - 1; i++)
{
var msData = data.MarkerSets[i]; // Received marker set descriptions
var ms = new OptiTrackBody(msData.MarkerSetName)
{
BodyType = msData.nMarkers > 1
? Body.EBodyType.MarkerSet
: Body.EBodyType.Marker
};
for (var j = 0; j < msData.nMarkers; j++)
{
var markerData = msData.Markers[j];
var markerId = markerData.ID == -1 ? j : markerData.ID;
var marker = new Point(new Vector3(markerData.x, markerData.y, markerData.z) * TranslationUnitMultiplier)
{
Name = string.Join(PointIdDivider, msData.MarkerSetName, markerId),
};
ms.Points.Add(marker);
}
markerSets.Add(msData.MarkerSetName, ms);
}
return(markerSets);
}
public InputFrame(FrameOfMocapData data, double frameDeltaTime)
{
BallPosition = Vector <double> .Build.DenseOfArray(new double[] {
data.OtherMarkers[0].x * 1000.0,
data.OtherMarkers[0].y * 1000.0,
data.OtherMarkers[0].z * 1000.0
});
DeltaTime = frameDeltaTime;
}
/*
* Method that is extracting Rigidbodies and Skeletons from FrameOfMocapData
*/
public void ExtractBodies(FrameOfMocapData data, ref MocapFrame handledFrame)
{
/* save all marker sets in local dict to add all markers to
* their corresponding rigid bodies or skeletons
*/
var markerSets = ExtractMarkerSets(data);
ExtractRigidBodies(data, markerSets, ref handledFrame);
ExtractSkeletons(data, ref handledFrame);
}
public InputFrame(FrameOfMocapData data)
{
Position = Vector <double> .Build.DenseOfArray(new double[] {
data.OtherMarkers[0].x * 1000.0,
data.OtherMarkers[0].y * 1000.0,
data.OtherMarkers[0].z * 1000.0
});
FrameDeltaTime = data.fTimestamp - timestamp;
timestamp = data.fTimestamp;
}
//Method to handle frame events
public void ClientFrameReady(FrameOfMocapData data)
{
/* Write values into the handledFrame object */
var handledFrame =
new MocapFrame(_adapter.Config.Name, _adapter.Config.AdapterType)
{
Latency = ExtractLatency(data)
};
ExtractBodies(data, ref handledFrame);
_adapter.OnFrameAvailable(handledFrame);
}
public static void GetMarkersCallback(FrameOfMocapData frame, NatNetClientML client)
{
markers = new List <Point3d>();
for (int i = 0; i < frame.nOtherMarkers; ++i)
{
Point3d pt = new Point3d(frame.OtherMarkers[i].x * 1000.0, frame.OtherMarkers[i].y * 1000.0, frame.OtherMarkers[i].z * 1000.0);
pt.Transform(xform);
markers.Add(pt);
marker_sizes.Add(1.0f);
//marker_sizes.Add(frame.OtherMarkers[i].size);
}
Rhino.RhinoDoc.ActiveDoc.Views.Redraw();
}
private void ProcessFrame(FrameOfMocapData data, NatNetClientML client)
{
double frameDeltaTime = data.fTimestamp - frameTimestamp;
frameTimestamp = data.fTimestamp;
var receivedFrame = new InputFrame(data, frameDeltaTime);
var args = new FrameReceivedEventArgs {
ReceivedFrame = receivedFrame,
PrevBallPosition = ballPosition
};
ballPosition = receivedFrame.BallPosition;
FrameReceived?.Invoke(this, args);
}
void m_NatNet_OnFrameReady(NatNetML.FrameOfMocapData data, NatNetML.NatNetClientML client)
{
m_FrameQueue.Clear();
FrameOfMocapData deepCopy = new FrameOfMocapData(data);
m_FrameQueue.Enqueue(deepCopy);
FOutputFrameData.SliceCount = 1;
FOutputFrameData[0] = deepCopy;
FOutputTimestamp.SliceCount = 1;
FOutputTimestamp[0] = deepCopy.fTimestamp;
FOutputFrameCount.SliceCount = 1;
FOutputFrameCount[0] = deepCopy.iFrame;
}
private void GetLastFrameOfDataButton_Click(object sender, EventArgs e)
{
// [NatNet] GetLastFrameOfData can be used to poll for the most recent avail frame of mocap data.
// This mechanism is slower than the event handler mechanism, and in general is not recommended,
// since it must wait for a frame to become available and apply a lock to that frame while it copies
// the data to the returned value.
// get a copy of the most recent frame of data
// returns null if not available or cannot obtain a lock on it within a specified timeout
FrameOfMocapData data = m_NatNet.GetLastFrameOfData();
if (data != null)
{
// do something with the data
String frameInfo = String.Format("FrameID : {0}", data.iFrame);
OutputMessage(frameInfo);
}
}
/// <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();
}
/// <summary>
/// [NatNet] Request a description of the Active Model List from the server (e.g. Motive) and build up a new spreadsheet
/// </summary>
/// <param name="sender"></param>
/// <param name="e"></param>
//处理帧数据 //保存数据
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)
{
//OutputMessage("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)
{
//OutputMessage("Frame Drop: ( ThisFrame: " + data.iFrame.ToString() + " LastFrame: " + mLastFrame.ToString() + " )");
//mDroppedFrames++;
}
}
// recording : write packet to data file
// [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;
}
/*
* Method that is extracting the latency from FrameOfMocapData
*/
public float ExtractLatency(FrameOfMocapData data)
{
/* So far without transmission latency
* client instance is needed, can't really find the right thing in OptiTrackClient -> NatNet though*/
return(data.TransmitTimestamp - data.CameraMidExposureTimestamp);
}
void ProcessFrameOfData(ref NatNetML.FrameOfMocapData data)
{
TelemetryData telemetry = new TelemetryData();
bool bMotiveHardwareLatenciesAvailable = data.CameraMidExposureTimestamp != 0;
if (bMotiveHardwareLatenciesAvailable)
{
telemetry.TotalLatency = mNatNet.SecondsSinceHostTimestamp(data.CameraMidExposureTimestamp) * 1000.0;
telemetry.MotiveTotalLatency = (data.TransmitTimestamp - data.CameraMidExposureTimestamp) / (double)desc.HighResClockFrequency * 1000.0;
}
bool bMotiveLatenciesAvailable = data.CameraDataReceivedTimestamp != 0;
if (bMotiveLatenciesAvailable)
{
}
telemetry.TransmitLatency = mNatNet.SecondsSinceHostTimestamp(data.TransmitTimestamp) * 1000.0;
// 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 delta = thisPeriod - framePeriod;
double fudgeFactor = 0.002f; // 2 ms
if (delta > fudgeFactor)
{
//OutputMessage("Frame Drop: ( ThisTS: " + data.fTimestamp.ToString("F3") + " LastTS: " + m_fLastFrameTimestamp.ToString("F3") + " )");
double missingPeriod = delta / framePeriod;
int nMissing = (int)(missingPeriod + 0.5);
mDroppedFrames += nMissing;
telemetry.DroppedFrames = nMissing;
droppedFrameIndicator = 10; // for graphing only
}
else
{
droppedFrameIndicator = 0;
}
}
// check and report frame drop (frame id based)
if (mLastFrame != 0)
{
if ((data.iFrame - mLastFrame) != 1)
{
//OutputMessage("Frame Drop: ( ThisFrame: " + data.iFrame.ToString() + " LastFrame: " + mLastFrame.ToString() + " )");
//mDroppedFrames++;
}
}
/*
* if (data.bTrackingModelsChanged)
* mNeedTrackingListUpdate = true;
*/
// NatNet manages the incoming frame of mocap data, so if we want to keep it, we must make a copy of it
FrameOfMocapData deepCopy = new FrameOfMocapData(data);
// Add frame to a background queue for access by other threads
// Note: this lock should always succeed immediately, unless connecting/disconnecting, when the queue gets reset
lock (BackQueueLock)
{
m_BackQueue.Enqueue(deepCopy);
// limit background queue size to 10 frames
while (m_BackQueue.Count > 3)
{
m_BackQueue.Dequeue();
}
}
// Update the shared UI queue, only if the UI thread is not updating (we don't want to wait here as we're in the data update thread)
bool lockAcquired = false;
try
{
Monitor.TryEnter(FrontQueueLock, ref lockAcquired);
if (lockAcquired)
{
// [optional] clear the frame queue before adding a new frame (UI only wants most recent frame)
m_FrontQueue.Clear();
m_FrontQueue.Enqueue(deepCopy);
m_FrameTransitLatencies.Clear();
m_FrameTransitLatencies.Enqueue(telemetry.TransmitLatency);
m_TotalLatencies.Clear();
m_TotalLatencies.Enqueue(telemetry.TotalLatency);
}
else
{
mUIBusyCount++;
}
}
finally
{
if (lockAcquired)
{
Monitor.Exit(FrontQueueLock);
}
}
// recording : write packet to data file
if (mRecording)
{
WriteFrame(deepCopy, telemetry);
}
mLastFrame = data.iFrame;
m_fLastFrameTimestamp = data.fTimestamp;
}
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);
}
}
}
/// <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();
}
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;
}