VRPNManager.cs

Go to the documentation of this file.

using UnityEngine;
using System;
using System.Collections;
using System.Collections.Generic;
using System.Linq;
using System.Runtime.InteropServices;
using System.Reflection;
using UI;

namespace VRPN
{
    // The set of tool types.
    // An interactor is used to pull atoms during interactive molecular dynamics simulations.
    // When the interactor holds a force feedback connection, the tool magnetized by atoms.
    // When pulling an atom, the force feedback provides an insight into the applied force amplitude.
    public enum VRPNToolType {
        INTERACTOR,
        EXPLORER,
    }

    // A set of VRPN connection references for a single tool.
    struct VRPNToolConnections {
        public System.IntPtr force_feedback;
        public System.IntPtr tracker;
        public System.IntPtr buttons;

        public VRPNToolConnections(System.IntPtr force_feedback = new IntPtr(), System.IntPtr buttons = new IntPtr(), System.IntPtr tracker = new IntPtr()) {
            this.force_feedback = force_feedback;
            this.tracker = tracker;
            this.buttons = buttons;
        }
    }

    // A tool instance data, that is its type and the scene game object and a few more.
    struct VRPNToolInstance {
        public VRPNToolType type;
        // The game object of the tracker.
        public GameObject go;
        // The game object of the haptic workspace.
        public GameObject workspaceGo;
        // The last workspace position in the scene. Used for clutching.
        public Vector3 last_workspace_position;
        // The last tracker position in the scene. Used for clutching.
        public Vector3 last_tracker_position;
        // True when the clutching feature is on.
        public bool clutch;
        // The game object of the atom highlight.
        public GameObject selection_sphere;
        // A reference to the closest atom.
        public SingleAtomSelection closest_atom;
        // True activates the closest atom search. Set to false when pulling an atom.
        public bool get_closest_atom;
    }

    // Manages the VRPN connections and tools instantiation.
    // Associates a set of connections to each tool.
    // Provides the public API other systems should interface with.
    //
    // When a VRPN device is instanciated, it has to be polled one time for the connection to be effective.
    // Otherwise, functions vrpn_c_[button|tracker|...]_is_connected() return false.
    public class VRPNManager {
        
        // The function delegates used as VRPN callbacks, one for each kind of VRPN device.
        VRPN.Wrapper.TrackerCallbackDelegate interactorTrackerDelegate;
        VRPN.Wrapper.ButtonCallbackDelegate interactorButtonDelegate;
        VRPN.Wrapper.AnalogCallbackDelegate explorerAnalogDelegate;

        // The list of instanciated tools.
        List<VRPNToolInstance> tools = new List<VRPNToolInstance>();
        // The list of VRPN connection structures. Correspond one-to-one to tool instances.
        List<VRPNToolConnections> connections = new List<VRPNToolConnections>();

        // Warning: Side Effect Heavy
        // We cannot pass the tool index as a parameter due to VRPN callbacks signature.
        int current_tool_index = 0;

        // Callback function called by VRPN when receiving tracker reports for an interactor tool.
        public void interactorTrackerCallback(uint sensor, System.IntPtr pos, System.IntPtr quat)
        {
            VRPNToolInstance tool = tools[current_tool_index];
            GameObject go = tool.go;

            double[] managedArray = new double[3];
            double[] quaternionArray = new double[4];
            Marshal.Copy(pos, managedArray, 0, 3);
            Marshal.Copy(quat, quaternionArray, 0, 4);

            float factor = 20f;
            float clutchingFactor = 6f;

            // Z-axis is inverted to reflect the phantom omni position correctly.
            Vector3 scaledTrackerPositionVector = new Vector3((float)managedArray[0] * factor, (float)managedArray[1] * factor, -(float)managedArray[2] * factor);

            if (tool.clutch) {
                tool.workspaceGo.transform.localPosition = tool.last_workspace_position + (tool.last_tracker_position - scaledTrackerPositionVector) * clutchingFactor;
            }
            else {
                tool.go.transform.localPosition = scaledTrackerPositionVector;
                tool.go.transform.localRotation = new Quaternion(-(float)quaternionArray[0], -(float)quaternionArray[1], (float)quaternionArray[2], (float)quaternionArray[3]);

                if (UIData.atomtype != UIData.AtomType.optihb && UIData.atomtype != UIData.AtomType.particleball && tool.get_closest_atom) {
                    GenericAtomManager atomManager = UnityMolMain.getCurrentAtomManager() as GenericAtomManager;
                    tool.closest_atom = atomManager.getClosestAtomGameObject(go.transform.position);

                    tool.selection_sphere.transform.parent = tool.closest_atom.go.transform;
                    tool.selection_sphere.transform.localPosition = Vector3.zero;

                    tools[current_tool_index] = tool;
                }
            }
        }

        // Callback function called by VRPN when receiving button reports for an interactor tool.
        // Asserts the presence of two buttons numbered 0 and 1.
        public void interactorButtonCallback(Int32 button, Int32 state)
        {
            VRPNToolInstance tool = tools[current_tool_index];

            if (UIData.atomtype != UIData.AtomType.optihb && UIData.atomtype != UIData.AtomType.particleball && GUIMoleculeController.toggle_IMD) {
                ArtemisManager artemis = UnityMolMain.getArtemisManager();

                // The dark gray button of the phantom omni is pressed.
                // When an interactive simulation is running, this has the effect to apply a force on the closest atom.
                if (button == 0 && state == 1) {
                    tool.get_closest_atom = false;

                    // Pull the closest atom.
                    Vector3 force = tool.go.transform.position - tool.closest_atom.go.transform.position;
                    artemis.send_forces(1, new int[1] {tool.closest_atom.pdbIndex}, new float[3] {force.x, force.y, force.z});

                    tools[current_tool_index] = tool;
                }

                if (button == 0 && state == 0) {
                    // Reset the force on the closest atom.
                    artemis.send_forces(1, new int[1] {0}, new float[3] {0f, 0f, 0f});

                    tool.get_closest_atom = true;
                    tools[current_tool_index] = tool;
                }
            }

            // The light gray button of the phantom omni is pressed.
            // This allows the workspace to be moved in space relative to the camera, a kind of clutching feature.
            if (button == 1 && state == 1) {
                tool.last_workspace_position = tool.workspaceGo.transform.localPosition;
                tool.last_tracker_position = tool.go.transform.localPosition;
                tool.clutch = true;
                tools[current_tool_index] = tool;
            }

            // The light gray button of the phantom omni is released.
            // Disable the clutching. Allows the tool to move freely again.
            if (button == 1 && state == 0) {
                tool.clutch = false;
                tools[current_tool_index] = tool;
            }
        }

        public void explorerAnalogCallback(VRPN.Wrapper.VRPNAnalogReport report)
        {
            Debug.Log("----------");
            Debug.Log(report.tv_sec);
            Debug.Log(report.tv_usec);
            Debug.Log(report.num_channel);
            Debug.Log("----------Tran");
            Debug.Log(report.channel[0]);
            Debug.Log(report.channel[1]);
            Debug.Log(report.channel[2]);
            Debug.Log("----------Rot");
            Debug.Log(report.channel[3]);
            Debug.Log(report.channel[4]);
            Debug.Log(report.channel[5]);
        }

        public VRPNManager() {
            MethodInfo minfo = typeof(VRPNManager).GetMethod("interactorTrackerCallback");
            interactorTrackerDelegate = (VRPN.Wrapper.TrackerCallbackDelegate) Delegate.CreateDelegate(typeof(VRPN.Wrapper.TrackerCallbackDelegate), this, minfo, false);

            MethodInfo minfo2 = typeof(VRPNManager).GetMethod("interactorButtonCallback");
            interactorButtonDelegate = (VRPN.Wrapper.ButtonCallbackDelegate) Delegate.CreateDelegate(typeof(VRPN.Wrapper.ButtonCallbackDelegate), this, minfo2, false);

            MethodInfo minfo3 = typeof(VRPNManager).GetMethod("explorerAnalogCallback");
            explorerAnalogDelegate = (VRPN.Wrapper.AnalogCallbackDelegate) Delegate.CreateDelegate(typeof(VRPN.Wrapper.AnalogCallbackDelegate), this, minfo3, false);
        }

        /**************
         * Public API *
         **************/

        // Clear all instantiated tools.
        // This has the consequence to correctly close VRPN connections.
        public void clearTools() {
            int count = connections.Count;
            for (int i = count-1; i >= 0 ; i--) {
                RemoveTool(i);
            }
        }

        // Performs VRPN connections polling for every tool.
        public void DeviceLoop() {
            VRPNToolConnections c;

            int count = connections.Count;

            for (int i = 0; i < count; i++) {
                current_tool_index = i;

                c = connections[i];

                if ((c.tracker != System.IntPtr.Zero) && (VRPN.Wrapper.vrpn_c_tracker_is_connected(c.tracker))) {
                    VRPN.Wrapper.vrpn_c_poll_tracker(c.tracker);
                }

                if ((c.buttons != System.IntPtr.Zero) && (VRPN.Wrapper.vrpn_c_button_is_connected(c.buttons))) {
                    VRPN.Wrapper.vrpn_c_poll_button(c.buttons);
                }
            }
        }

        // Allocates a VRPN tool and returns a index related to it.
        // Returns true if the process ended as expected.
        public int InstantiateTool(VRPNToolType tool_type) {
            Debug.Log("VRPN: Adding tool of type " + tool_type.ToString());

            VRPNToolInstance instance = new VRPNToolInstance();
            instance.type = tool_type;
            instance.get_closest_atom = true;

            GenericAtomManager atomManager = UnityMolMain.getCurrentAtomManager() as GenericAtomManager;
            instance.closest_atom = atomManager.getClosestAtomGameObject(Vector3.zero);

            GameObject go = GameObject.Instantiate(Resources.Load("VRPN/VRPNPicker", typeof(GameObject)), Vector3.zero, Quaternion.identity) as GameObject;

            GameObject workspaceGo = GameObject.Instantiate(Resources.Load("VRPN/VRPN Interactor Grid") as GameObject);

            workspaceGo.transform.parent = Camera.main.transform;
            workspaceGo.transform.localPosition = Vector3.zero;
            workspaceGo.transform.localRotation = Quaternion.identity;
            workspaceGo.transform.Translate(new Vector3(0f, 0f, 30f));

            go.transform.parent = workspaceGo.transform;
            go.transform.localPosition = Vector3.zero;
            go.transform.localRotation = Quaternion.identity;

            instance.go = go;
            instance.workspaceGo = workspaceGo;

            // Instantiate a transparent selection sphere that is disabled until use.
            GameObject selection_sphere = GameObject.CreatePrimitive(PrimitiveType.Sphere);
            selection_sphere.transform.localScale = Vector3.one * 2f;

            Shader s = Shader.Find("Transparent/Diffuse");
            selection_sphere.GetComponent<Renderer>().material.shader = s;
            Color c = Color.yellow;
            c.a = 0.5f;
            selection_sphere.GetComponent<Renderer>().material.color = c;

            instance.selection_sphere = selection_sphere;

            this.tools.Add(instance);

            VRPNToolConnections connections_instance = new VRPNToolConnections();
            this.connections.Add(connections_instance);

            Debug.Log("VRPN: Tool was added");
            return this.tools.Count() - 1;
        }

        // Deallocates an instantiated tool based on its index.
        // Returns true if the process ended as expected.
        public bool RemoveTool(int tool_instance_index)
        {
            Debug.Log("VRPN: Attempting to remove tool number " + tool_instance_index);

            // Retrieve VRPN connections references and close them.
            // This avoids dangling pointers to VRPN objects.
            VRPNToolConnections c;

            try {
                c = connections[tool_instance_index];
            }
            catch (ArgumentOutOfRangeException) {
                return false;
            }

            if (c.force_feedback != System.IntPtr.Zero) {
                VRPN.Wrapper.vrpn_c_close_force_feedback(c.force_feedback);
                c.force_feedback = System.IntPtr.Zero;
            }

            if (c.tracker != System.IntPtr.Zero) {
                VRPN.Wrapper.vrpn_c_close_tracker(c.tracker);
                c.tracker = System.IntPtr.Zero;
            }

            if (c.buttons != System.IntPtr.Zero) {
                VRPN.Wrapper.vrpn_c_close_button(c.buttons);
                c.buttons = System.IntPtr.Zero;
            }

            // Reset VRPN connections pointers.
            this.connections[tool_instance_index] = c;

            // Completely delete the tool from the manager's memory.
            // Also, destroy associated game objects.
            try {
                GameObject go;
                GameObject workspaceGo;
                GameObject sphereGo;

                go = this.tools[tool_instance_index].go;
                workspaceGo = this.tools[tool_instance_index].workspaceGo;
                sphereGo = this.tools[tool_instance_index].selection_sphere;

                GameObject.Destroy(go);
                GameObject.Destroy(workspaceGo);
                GameObject.Destroy(sphereGo);

                this.tools.RemoveAt(tool_instance_index);
                this.connections.RemoveAt(tool_instance_index);
            }
            catch(ArgumentOutOfRangeException) {
                return false;
            }
            
            Debug.Log("VRPN: Removed tool correctly.");
            return true;
        }

        // Connects a tool to a VRPN Force Feedback server based on the server address.
        // Returns true if a VRPN Connection was actually created, false otherwise.
        public bool ConnectToolToForceFeedback(int tool_instance_index, string vrpn_device) {
            VRPNToolConnections connections_instance;
            try {
                connections_instance = this.connections[tool_instance_index];
            }
            catch (Exception) {
                return false;
            }

            System.IntPtr ff_connection = VRPN.Wrapper.vrpn_c_open_force_feedback(vrpn_device);

            // Wait a bit for the connection instantiation.
            System.Threading.Thread.Sleep(50);

            // Check if the target machine is effectively reached.
            // Despite this precaution, it does not mean there is effectively a VRPN server on the target machine.
            if (!VRPN.Wrapper.vrpn_c_force_feedback_doing_okay(ff_connection)) {
                Debug.LogWarning("Button instance got closed due to a mistaking server address.");
                VRPN.Wrapper.vrpn_c_close_force_feedback(ff_connection);
                return false;
            }

            VRPN.Wrapper.vrpn_c_force_feedback_set_force(ff_connection, 0f, 0f, 0f);

            connections_instance.force_feedback = ff_connection;

            this.connections[tool_instance_index] = connections_instance;

            return true;
        }

        // Connects a tool to a VRPN Tracker server based on the server address.
        // Returns true if a VRPN Connection was actually created, false otherwise.
        public bool ConnectToolToTracker(int tool_instance_index, string vrpn_device) {
            VRPNToolConnections connections_instance;
            try {
                connections_instance = this.connections[tool_instance_index];
            }
            catch (Exception) {
                return false;
            }

            System.IntPtr tracker_connection = VRPN.Wrapper.vrpn_c_open_tracker(vrpn_device, interactorTrackerDelegate);

            // Wait a bit for the connection instantiation.
            System.Threading.Thread.Sleep(50);

            // Check if the target machine is effectively reached.
            // Despite this precaution, it does not mean there is effectively a VRPN server on the target machine.
            if (!VRPN.Wrapper.vrpn_c_tracker_doing_okay(tracker_connection)) {
                Debug.LogWarning("Tracker instance got closed due to a mistaking server address.");
                VRPN.Wrapper.vrpn_c_close_tracker(tracker_connection);
                return false;
            }

            // To effectively initiate a connection, a VRPN device has to be polled at least one time.
            // When an ExecutionEngine Exception occurs, the server address is incorrect.
            VRPN.Wrapper.vrpn_c_poll_tracker(tracker_connection);

            connections_instance.tracker = tracker_connection;

            this.connections[tool_instance_index] = connections_instance;

            return true;
        }

        // Connects a tool to a VRPN Buttons server based on the server address.
        // Returns true if a VRPN Connection was actually created, false otherwise.
        public bool ConnectToolToButtons(int tool_instance_index, string vrpn_device) {
            VRPNToolConnections connections_instance;
            try {
                connections_instance = this.connections[tool_instance_index];
            }
            catch (Exception) {
                return false;
            }

            System.IntPtr button_connection = VRPN.Wrapper.vrpn_c_open_button(vrpn_device, interactorButtonDelegate);

            // Wait a bit for the connection instantiation.
            System.Threading.Thread.Sleep(50);

            // Check if the target machine is effectively reached.
            // Despite this precaution, it does not mean there is effectively a VRPN server on the target machine.
            if (!VRPN.Wrapper.vrpn_c_button_doing_okay(button_connection)) {
                Debug.LogWarning("Button instance got closed due to a mistaking server address.");
                VRPN.Wrapper.vrpn_c_close_button(button_connection);
                return false;
            }

            // To effectively initiate a connection, a VRPN device has to be polled at least one time.
            // When an ExecutionEngine Exception occurs, the server address is incorrect.
            VRPN.Wrapper.vrpn_c_poll_button(button_connection);

            connections_instance.buttons = button_connection;

            this.connections[tool_instance_index] = connections_instance;

            return true;
        }

        // Disconnects a tool from a VRPN Force Feedback server based on the tool index.
        // Returns true if the VRPN Connection was actually destroyed, false otherwise.
        public bool DisconnectToolFromForceFeedback(int tool_instance_index) {
            VRPNToolConnections connections_instance;
            try {
                connections_instance = this.connections[tool_instance_index];
            }
            catch (Exception) {
                return false;
            }

            VRPN.Wrapper.vrpn_c_force_feedback_stop(connections_instance.force_feedback);

            if (VRPN.Wrapper.vrpn_c_close_force_feedback(connections_instance.force_feedback)) {
                connections_instance.force_feedback = System.IntPtr.Zero;
                this.connections[tool_instance_index] = connections_instance;
                return true;
            }

            return false;
        }

        // Disconnects a tool from a VRPN Tracker server based on the tool index.
        // Returns true if the VRPN Connection was actually destroyed, false otherwise.
        public bool DisconnectToolFromTracker(int tool_instance_index) {
            VRPNToolConnections connections_instance;
            try {
                connections_instance = this.connections[tool_instance_index];
            }
            catch (Exception) {
                return false;
            }

            if (VRPN.Wrapper.vrpn_c_close_tracker(connections_instance.tracker)) {
                connections_instance.tracker = System.IntPtr.Zero;
                this.connections[tool_instance_index] = connections_instance;
                return true;
            }

            return false;
        }

        // Disconnects a tool from a VRPN Buttons server based on the tool index.
        // Returns true if the VRPN Connection was actually destroyed, false otherwise.
        public bool DisconnectToolFromButtons(int tool_instance_index) {
            VRPNToolConnections connections_instance;
            try {
                connections_instance = this.connections[tool_instance_index];
            }
            catch (Exception) {
                return false;
            }

            if (VRPN.Wrapper.vrpn_c_close_button(connections_instance.buttons)) {
                connections_instance.buttons = System.IntPtr.Zero;
                this.connections[tool_instance_index] = connections_instance;
                return true;
            }

            return false;
        }
    }

}