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; } }
void GetAveragePosQuat() { int num = MaxReports; VRPNTrackerPosReports(TrackerName, repsPtr, ref num); float[] repsSum = new float[7]; int repsCount = 0; TrackerReport[] reps = new TrackerReport[num]; 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++; } } 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; if (trackerPos != Vector3.zero || trackerQuat != Quaternion.identity) { _lastReportTime = Time.unscaledTime; } } else { trackerPos = lastPos; trackerQuat = lastQuat; } }