123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195 |
- using System;
- using System.Runtime.InteropServices;
- using System.Threading;
- namespace OptiTrack
- {
- public class OptiTrackConnector
- {
- private bool _stop = true;
- private Thread _tracker;
- public delegate void OnFrameReady(Frame frame);
- public bool Init(String path)
- {
- int a = OptiTrackNativeWrapper.TT_Initialize();
- int b = OptiTrackNativeWrapper.TT_LoadProject(path);
- Console.WriteLine("a:" + a + ", b:" + b);
- if (a + b != 0)
- {
- // breakpoint
- }
- return a + b == 0;
- }
- public void StartTracking(OnFrameReady frameReadyDelegate)
- {
- _stop = false;
- _tracker = new Thread(delegate ()
- {
- HighPerformanceTimer hpt = new HighPerformanceTimer();
- int i = 0;
- hpt.Start();
- while (!_stop)
- {
- if (i++ == 200)
- {
- //Console.WriteLine("Tracker FPS: " + 200.0 / HPT.Stop());
- i = 0;
- hpt.Start();
- }
- OptiTrackNativeWrapper.TT_Update();
- frameReadyDelegate(BuildFrame());
- Thread.Sleep(15);
- }
- });
- _tracker.Start();
- }
- Frame _lastFrame;
- private Frame BuildFrame()
- {
- Frame frame = new Frame(OptiTrackNativeWrapper.TT_FrameMarkerCount(), OptiTrackNativeWrapper.TT_TrackableCount());
- for (int i = 0; i < OptiTrackNativeWrapper.TT_FrameMarkerCount(); i++)
- {
- frame.Markers[i] = new Marker(
- OptiTrackNativeWrapper.TT_FrameMarkerX(i),
- OptiTrackNativeWrapper.TT_FrameMarkerY(i),
- OptiTrackNativeWrapper.TT_FrameMarkerZ(i))
- {
- BoundToTrackable = null
- };
- }
- for (int i = 0; i < OptiTrackNativeWrapper.TT_TrackableCount(); i++)
- {
- Trackable t = new Trackable
- {
- IsTracked = OptiTrackNativeWrapper.TT_IsTrackableTracked(i),
- Id = OptiTrackNativeWrapper.TT_TrackableID(i)
- };
- if (t.IsTracked)
- {
- OptiTrackNativeWrapper.TT_TrackableLocation(i, out t.X, out t.Y, out t.Z, out t.Qx, out t.Qy, out t.Qz, out t.Qw,
- out t.Pitch, out t.Yaw, out t.Roll);
- if(_lastFrame != null)
- {
- if (_lastFrame.Trackables[i].IsAlmostSameCoordinates(t))
- t.IsTracked = false;
- }
- /*for (int j = 0; j < OptiTrackNativeWrapper.TT_TrackableMarkerCount(i); j++)
- {
- float markerRadius = trackableMarker(i, j);
- frame.markers[boundTrackable(frame, markerRadius,t)].boundToTrackable = t;
- }*/
- }
- frame.Trackables[i] = t;
- }
- _lastFrame = frame;
- return frame;
- }
- /*private float trackableMarker(int trackableId, int markerId)
- {
- Marker tempMarker = new Marker(0, 0, 0);
- OptiTrackNativeWrapper.TT_TrackableMarker(trackableId, markerId, out tempMarker.x, out tempMarker.y, out tempMarker.z);
- float markerRadius = (float)Math.Pow(((float)Math.Pow(tempMarker.x, 2.00) + (float)Math.Pow(tempMarker.y, 2.00) + (float)Math.Pow(tempMarker.z, 2.00)),0.50);
- markerRadius += (float)0.01;
- return markerRadius;
- }
- private int boundTrackable(Frame frame, float markerRadius, Trackable trackable)
- {
- List<float> distanceValues = new List<float>();
- for (int k = 0; k < frame.markers.Count(); k++)
- distanceValues.Add((float)Math.Pow(((float)Math.Pow(trackable.x - frame.markers[k].x, 2.00) + (float)Math.Pow(trackable.y - frame.markers[k].y, 2.00) + (float)Math.Pow(trackable.z - frame.markers[k].z, 2.00)),0.50));
- float minimumDistance = markerRadius;
-
- int boundMarkerId = 0;
- for (int i = 0; i < distanceValues.Count; ++i)
- {
-
- if (distanceValues[i] <= minimumDistance)
- {
- minimumDistance = distanceValues[i];
- boundMarkerId = i;
- }
- }
- return boundMarkerId;
- }*/
- public void StopTracking()
- {
- _stop = true;
- OptiTrackNativeWrapper.TT_Shutdown();
- }
- }
-
- class OptiTrackNativeWrapper
- {
- [DllImport("NPTrackingTools.dll", CallingConvention = CallingConvention.Cdecl)]
- public static extern int TT_Initialize();
- [DllImport("NPTrackingTools.dll", CallingConvention = CallingConvention.Cdecl)]
- public static extern int TT_Shutdown();
- [DllImport("NPTrackingTools.dll", CharSet = CharSet.Auto, CallingConvention = CallingConvention.Cdecl)]
- public static extern int TT_LoadProject([MarshalAs(UnmanagedType.LPStr)]string file);
- [DllImport("NPTrackingTools.dll", CallingConvention = CallingConvention.Cdecl)]
- public static extern int TT_Update();
- [DllImport("NPTrackingTools.dll", CallingConvention = CallingConvention.Cdecl)]
- public static extern int TT_FrameMarkerCount();
- [DllImport("NPTrackingTools.dll", CallingConvention = CallingConvention.Cdecl)]
- public static extern int TT_TrackableCount();
- [DllImport("NPTrackingTools.dll", CallingConvention = CallingConvention.Cdecl)]
- public static extern int TT_TrackableLocation([In]int index, [Out]out float x, [Out]out float y, [Out]out float z, [Out]out float qx, [Out]out float qy, [Out]out float qz, [Out]out float qw, [Out]out float yaw, [Out]out float pitch, [Out]out float roll);
- [DllImport("NPTrackingTools.dll", CallingConvention = CallingConvention.Cdecl)]
- public static extern void TT_TrackableMarker([In]int rigidIndex, [In]int markerIndex, [Out]out float x, [Out]out float y, [Out]out float z);
- [DllImport("NPTrackingTools.dll", CallingConvention = CallingConvention.Cdecl)]
- public static extern float TT_FrameMarkerX([In]int index);
- [DllImport("NPTrackingTools.dll", CallingConvention = CallingConvention.Cdecl)]
- public static extern float TT_FrameMarkerY([In]int index);
- [DllImport("NPTrackingTools.dll", CallingConvention = CallingConvention.Cdecl)]
- public static extern float TT_FrameMarkerZ([In]int index);
- [DllImport("NPTrackingTools.dll", CallingConvention = CallingConvention.Cdecl)]
- public static extern bool TT_IsTrackableTracked([In]int index);
- [DllImport("NPTrackingTools.dll", CallingConvention = CallingConvention.Cdecl)]
- public static extern int TT_TrackableID(int index);
- [DllImport("NPTrackingTools.dll", CallingConvention = CallingConvention.Cdecl)]
- public static extern int TT_TrackableMarkerCount(int index);
- }
- }
|