void GetAveragePosQuat()
{
int num = MaxReports;
VRPNTrackerPosReports(TrackerName.ToString(), repsPtr, ref num);
float[] repsSum = new float[7];
int repsCount = 0;
TrackerReport[] reps = new TrackerReport[num];
Dictionary <int, VRPNTracker.TrackerReport> lastReports = new Dictionary <int, VRPNTracker.TrackerReport>();
for (int i = 0; i < num; i++)
{
reps[i] = (TrackerReport)Marshal.PtrToStructure(repsPtr[i], typeof(TrackerReport));
if (reps[i].sensor == SensorNumber && VRPNManager.TimeValGreater(ref reps[i].msg_time, ref LastReport))
{
repsSum[0] += (float)reps[i].pos[0];
repsSum[1] += (float)reps[i].pos[1];
repsSum[2] += (float)reps[i].pos[2];
repsSum[3] += (float)reps[i].quat[2];
repsSum[4] += -(float)reps[i].quat[1];
repsSum[5] += (float)reps[i].quat[0];
repsSum[6] += (float)reps[i].quat[3];
LastReport.tv_sec = reps[i].msg_time.tv_sec;
LastReport.tv_usec = reps[i].msg_time.tv_usec;
repsCount++;
}
VRPNTracker.TrackerReport test;
if (lastReports.TryGetValue(reps[i].sensor, out test))
{
lastReports[reps[i].sensor] = reps[i];
}
else
{
lastReports.Add(reps[i].sensor, reps[i]);
}
}
foreach (KeyValuePair <int, VRPNTracker.TrackerReport> pair in lastReports)
{
//Trigger tracker event in event manager
VRPNEventManager.TriggerEventTracker(TrackerType.ToString(), TrackerName.ToString(), pair.Value);
}
if (repsCount > 0)
{
trackerPos.x = repsSum[0] / (float)repsCount;
trackerPos.y = repsSum[1] / (float)repsCount;
trackerPos.z = repsSum[2] / (float)repsCount;
trackerQuat.x = repsSum[3] / (float)repsCount;
trackerQuat.y = repsSum[4] / (float)repsCount;
trackerQuat.z = repsSum[5] / (float)repsCount;
trackerQuat.w = repsSum[6] / (float)repsCount;
}
else
{
trackerPos = lastPos;
trackerQuat = lastQuat;
}
}
//Auxiliar method that sends last frame report for each sensor
private void sendingReports(Dictionary <int, VRPNTracker.TrackerReportNew> lastReports)
{
foreach (KeyValuePair <int, VRPNTracker.TrackerReportNew> pair in lastReports)
{
VRPNTracker.TrackerReport newReport = new VRPNTracker.TrackerReport();
VRPNManager.TimeVal newMsgTime = new VRPNManager.TimeVal();
newMsgTime.tv_sec = (UInt32)pair.Value.msg_time.tv_sec;
newMsgTime.tv_usec = (UInt32)pair.Value.msg_time.tv_usec;
newReport.msg_time = newMsgTime;
newReport.pos = pair.Value.pos;
newReport.quat = pair.Value.quat;
newReport.sensor = pair.Value.sensor;
VRPNEventManager.TriggerEventTracker(data.deviceType, data.deviceName, newReport);
}
}
