Beispiel #1
0
    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;
        }
    }
Beispiel #2
0
 //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);
     }
 }