reference/reference.cs

// Copyright PS-Tech B.V. All Rights Reserved.
 
using System;
using System.Linq;
using System.Runtime.InteropServices;
using System.Threading;
using PSTech.Pstsdk;
 
namespace PsTech.Reference
{
    public class Reference
    {
        //Registering control handler function.
        [DllImport("Kernel32")]
        private static extern bool SetConsoleCtrlHandler(ConsoleEventHandler handler, bool add);
 
        private delegate bool ConsoleEventHandler(CtrlType sig);
        static ConsoleEventHandler ConsoleCtrlEventHandler;
 
        public enum CtrlType
        {
            CtrlCEvent = 0,
            CtrlBreakEvent = 1,
            CtrlCloseEvent = 2,
            CtrlLogOffEvent = 5,
            CtrlShutdownEvent = 6,
        }

        private static bool ConsoleHandler(CtrlType sig)
        {
            Exithandler((int)sig);
            return true;
        }

        public static void PrintMatrix(float[] matrix)
        {
            for (int i = 0; i < 4; i++)
            {
                for (int j = 0; j < 4; j++)
                {
                    Console.Write(matrix[j + i * 4] + "\t");
                }
                Console.WriteLine();
            }
        }
 
        // Control variable for main loop.
        static bool Running = true;
 
        // Number of data points to grab before application termination.
        static int NumberOfSamplesToGrab = 1000;
        static int SamplesGrabbed = 0;

        class MyTrackerListener : TrackerListener
        {
            public override void OnTrackerData(TrackerData data, ErrorStatus status)
            {
                if (SamplesGrabbed++ >= NumberOfSamplesToGrab)
                {
                    Running = false;
                }
 
                // Do something with the received data.
            }
        }

        static void Exithandler(int sig)
        {
            Tracker.Shutdown(); // Cleans up native resources only; does NOT remove listeners
            Running = false;
        }
        static int Main(string[] args)
        {
            // Register the exit handler with the application
            ConsoleCtrlEventHandler += new ConsoleEventHandler(ConsoleHandler);
            SetConsoleCtrlHandler(ConsoleCtrlEventHandler, true);
 
            // Implement error handling of PSTech.TrackerException exceptions to prevent
            // improper PST Tracker shutdown on errors.
            try
            {
                // Create an instance of the Tracker object using the default configuration path and file names.
                // Wrap the Tracker in a using statement to ensure Dispose() is called automatically
                using (Tracker tracker = new Tracker())
                {
                    // Print version number of the tracker server being used.
                    Console.Write("Running PST Server version " + tracker.GetVersionInfo() + "\n");
 
                    //Create tracker listener with callback functions for data and/or mode updates.
                    TrackerListener listener = new MyTrackerListener();
 
                    // Register the TrackerListener object to the tracker server.
                    tracker.AddTrackerListener(listener);
 
                    // Start the tracker server.
                    tracker.StartTracker();
 
                    // Perform a system check to see if the tracker server is running OK and print the result.
                    Console.Write("System check: " + tracker.Systemcheck() + "\n\n");
 
                    // Set the frame rate to 30 Hz.
                    tracker.SetFramerate(30);
 
                    // Print the new frame rate to see if it was set correctly. Note that for PST HD and Pico
                    // trackers the frame rate actually being set can differ from the value provided to SetFramerate().
                    Console.Write("Frame rate set to " + tracker.GetFramerate() + "\n\n");
                    Console.Write("*******************\n\n");
 
                    // Get the transformation matrix for the current reference system.
                    Console.Write("Current reference system transformation matrix:\n");
                    PrintMatrix(tracker.GetReference());
 
                    Console.Write("\n\n*******************\n\n");
 
                    // Define new reference system transformation matrix rotating the reference system by
                    // 90 degrees around the X-axis and 180 degrees around the Y-axis. Then translate the
                    // origin of the reference system by 0.1 m in the X direction, -0.5 m in the Y direction
                    // and 0.5 m in the Z direction.
                    float[] reference = {  -1.0f,  0.0f,  0.0f,  0.1f,
                                        0.0f,  0.0f,  1.0f, -0.5f,
                                        0.0f,  1.0f,  0.0f,  0.5f,
                                        0.0f,  0.0f,  0.0f,  1.0f };
                    tracker.SetReference(reference);
                    Console.Write("New reference system transformation matrix:\n");
                    PrintMatrix(tracker.GetReference());
 
                    // Check if the reference system was set according to the input.
                    if (tracker.GetReference().SequenceEqual(reference))
                    {
                        Console.Write("Reference set correctly!\n");
                    }
                    else
                    {
                        Console.Write("Reference not set correctly!\n");
                    }
                    Console.Write("\n\n*******************\n\n");
 
                    // Trying to set the reference using a non-orthonormal transformation matrix (this should fail).
                    float[] nonOrthoNormalReference = { -1.0f,  1.0f,  0.0f,  0.1f,
                                                    0.0f,  0.0f, -1.0f, -0.5f,
                                                    0.0f, -1.0f,  0.0f,  0.5f,
                                                    0.0f,  0.0f,  0.0f,  1.0f };
 
                    try
                    {
                        tracker.SetReference(nonOrthoNormalReference);
                        Console.Write("Reference input incorrectly applied!");
                    }
                    catch (PSTech.TrackerException e)
                    {
                        // This should fail because the supplied matrix is non-orthonormal
                        Console.Write("Reference input correctly ignored: " + e.Message + "\n");
                    }
                    Console.Write("New reference system after applying non-orthonormal transformation:\n");
                    PrintMatrix(tracker.GetReference());
 
                    Console.Write("\n\n*******************\n\n");
 
                    // Adjust the reference system by applying a relative transformation. The new reference system will have
                    // the X-axis pointing outward from the tracker and the Y-axis parallel to the tracker front.
                    float[] relativeReference = {   0.0f, -1.0f,  0.0f,  0.5f,
                                                1.0f,  0.0f,  0.0f,  0.4f,
                                                0.0f,  0.0f,  1.0f,  0.0f,
                                                0.0f,  0.0f,  0.0f,  1.0f };
                    tracker.SetReference(relativeReference, true);
                    Console.Write("New reference system after applying relative transformation:\n");
                    PrintMatrix(tracker.GetReference());
 
                    Console.Write("\n\n*******************\n\n");
 
                    // Reset reference system to default (origin at 1 m form center of the PST Tracker,
                    // Z-axis pointing outward from the PST Tracker).
                    tracker.SetDefaultReference();
                    Console.Write("Reset default reference system:\n");
                    PrintMatrix(tracker.GetReference());
 
                    // Main loop, wait for auto-termination.
                    while (Running)
                    {
                        Thread.Sleep(100);
                    }
                }
            }
            catch (Exception e)
            {
                // Catch PSTech.TrackerException exceptions and print error messages.
                Console.Write(e + "\n");
            }
 
            // Pause command line to see results.
            Console.Write("Press enter to continue...\n");
            Console.Read();
 
            return 0;
        }
    }
}