Set up auto opted out controller discovery (vive wands) (#75)

* Set up opted out controller discovery

* Printout if no controllers found

* Discovery now works with invalid role controllers

* Fix loop stucture

* New DiscoverController using priority_queue

* Fix right hand pop, now working with vive controllers

* Force generic controllers/trackers to the corect hand

* Clean up structure a bit

* Cleaned up discoverController further

include/ControllerPose.h

@@ -20,4 +20,6 @@ class ControllerPose {
 
 	std::string m_thisDeviceManufacturer;
 
+	bool ControllerPose::IsOtherRole(int32_t test);
+
 };

src/ControllerPose.cpp

@@ -1,7 +1,7 @@
 #include "ControllerPose.h"
 
 #include <utility>
-
+#include <queue>
 #include "DriverLog.h"
 #include "Quaternion.h"
 
@@ -73,30 +73,80 @@ vr::DriverPose_t ControllerPose::UpdatePose() {
     return newPose;
 }
 
+//Checks if a role is the opposite hand from the current hand
+bool ControllerPose::IsOtherRole(int32_t test) {
+    if (m_shadowDeviceOfRole == vr::ETrackedControllerRole::TrackedControllerRole_RightHand)
+        return test == vr::ETrackedControllerRole::TrackedControllerRole_LeftHand;
+    else
+        return test == vr::ETrackedControllerRole::TrackedControllerRole_RightHand;
+}
+
 void ControllerPose::DiscoverController() {
+    //if there's an override, default to that
+    if (m_poseConfiguration.controllerOverrideEnabled) {
+        DriverLog("Controller ID override set to id: %i", m_poseConfiguration.controllerIdOverride);
+        m_shadowControllerId = m_poseConfiguration.controllerIdOverride;
+        return;
+    }
+
+    std::priority_queue<std::pair<int, int>> backupDevices;
+    bool otherTaken = false;
+
     //omit id 0, as this is always the headset pose
-    if (!m_poseConfiguration.controllerOverrideEnabled) {
-        for (int i = 1; i < vr::k_unMaxTrackedDeviceCount; i++) {
-            vr::ETrackedPropertyError err;
-
-            vr::PropertyContainerHandle_t container = vr::VRProperties()->TrackedDeviceToPropertyContainer(i);
-
-            std::string foundDeviceManufacturer = vr::VRProperties()->GetStringProperty(container,
-                                                                                        vr::Prop_ManufacturerName_String,
-                                                                                        &err);
-            int32_t deviceControllerRole = vr::VRProperties()->GetInt32Property(container,
-                                                                                vr::ETrackedDeviceProperty::Prop_ControllerRoleHint_Int32,
-                                                                                &err);
-
-            //We have a device which identifies itself as a tracked device that we want to be searching for, and that device is not this one.
-            if (deviceControllerRole == m_shadowDeviceOfRole && foundDeviceManufacturer != m_thisDeviceManufacturer) {
-                DebugDriverLog("Discovered a controller! Id: %i, Manufacturer: %s", i, foundDeviceManufacturer.c_str());
-                m_shadowControllerId = i;
-                break;
-            }
+    for (int i = 1; i < vr::k_unMaxTrackedDeviceCount; i++) {
+        vr::ETrackedPropertyError err;
+        vr::PropertyContainerHandle_t container = vr::VRProperties()->TrackedDeviceToPropertyContainer(i);
+        std::string foundDeviceManufacturer = vr::VRProperties()->GetStringProperty(container,
+                                                                                    vr::Prop_ManufacturerName_String,
+                                                                                    &err);
+        const int32_t foundDeviceClass = vr::VRProperties()->GetInt32Property(container, vr::ETrackedDeviceProperty::Prop_DeviceClass_Int32, &err);
+
+        int32_t foundControllerRole = vr::VRProperties()->GetInt32Property(container,
+                                                                vr::ETrackedDeviceProperty::Prop_ControllerRoleHint_Int32,
+                                                                &err);
+
+        //make sure we're not trying to find our own controllers
+        if (m_thisDeviceManufacturer == foundDeviceManufacturer) continue;
+
+        //We have a device which identifies itself as the correct role, and that device is not this one.
+        if (foundControllerRole == m_shadowDeviceOfRole) {
+            DriverLog("Discovered a %s handed controller! Id: %i, Manufacturer: %s",
+                        m_shadowDeviceOfRole == vr::ETrackedControllerRole::TrackedControllerRole_RightHand ? "right" : "left",
+                        i, foundDeviceManufacturer.c_str());
+            m_shadowControllerId = i;
+            return;
         }
-    } else {
-        DebugDriverLog("Controller ID override set to id: %i", m_poseConfiguration.controllerIdOverride);
-        m_shadowControllerId = m_poseConfiguration.controllerIdOverride;
+        //If this belongs to the other hand, then skip it
+        else if (IsOtherRole(foundControllerRole)) {
+            otherTaken = true;
+            continue;
+        }
+        //otherwise fill backup
+        switch (foundDeviceClass) {
+        case vr::ETrackedDeviceClass::TrackedDeviceClass_Controller:
+            backupDevices.push(std::make_pair(2, i)); //this is a generic controller like a vive wand
+            break;
+        case vr::ETrackedDeviceClass::TrackedDeviceClass_GenericTracker:
+            backupDevices.push(std::make_pair(1, i)); //this is a generic tracker
+            break;
+        /*default:
+            backupDevices.push(std::make_pair(0, i)); //currently not considering other devices
+        */
+        }
+    }
+
+    //If we haven't already picked something, use the backup
+    if (!backupDevices.empty()) {
+        m_shadowControllerId = backupDevices.top().second;
+        DriverLog("Selected a controller/tracker from backup. Id: %i, Priority: %i", m_shadowControllerId, backupDevices.top().first);
+        vr::ETrackedPropertyError err;
+        vr::PropertyContainerHandle_t container = vr::VRProperties()->TrackedDeviceToPropertyContainer(m_shadowControllerId);
+        /*
+        * There really needs to be a better way to ensure we're attached to the right generic controller, 
+        * but unfortunately we have not found a practical solution for this yet which works with vive wands. :(
+        */
+        vr::VRProperties()->SetInt32Property(container, vr::ETrackedDeviceProperty::Prop_ControllerRoleHint_Int32, m_shadowDeviceOfRole);
     }
+    else
+        DebugDriverLog("No suitable devices to attach to!");
 }