tracker/steamvr: support choosing device by its serial number

Since the vr handle is accessed on the GUI and pipeline threads
now, had to add implicit locking. This sadly reorganizes most of
the file.

Sadly this refactor likely broke things.

cf. https://github.com/opentrack/opentrack/issues/352#issuecomment-290252520

1 files changed, 203 insertions, 78 deletions

diff --git a/tracker-steamvr/steamvr.cpp b/tracker-steamvr/steamvr.cpp
index 2d92c5a4..b60d4896 100644
--- a/tracker-steamvr/steamvr.cpp
+++ b/tracker-steamvr/steamvr.cpp
@@ -16,45 +16,161 @@
  */
 
 #include "steamvr.hpp"
+
 #include "api/plugin-api.hpp"
 #include "compat/util.hpp"
 
 #include <cstdlib>
 #include <cmath>
+#include <type_traits>
+#include <algorithm>
 
 #include <QMessageBox>
 #include <QDebug>
 
-void steamvr::vr_deleter()
+QMutex device_list::mtx(QMutex::Recursive);
+
+template<typename F>
+static auto with_vr_lock(F&& fun) -> decltype(fun(vr_t(), error_t()))
 {
-    static std::atomic_flag atexit_done = ATOMIC_FLAG_INIT;
+    QMutexLocker l(&device_list::mtx);
+    error_t e; vr_t v;
+    std::tie(v, e) = device_list::vr_init();
+    return fun(v, e);
+}
 
-    if (atexit_done.test_and_set(std::memory_order_seq_cst))
+void device_list::fill_device_specs(QList<device_spec>& list)
+{
+    with_vr_lock([&](vr_t v, error_t)
     {
-        std::atexit(vr::VR_Shutdown);
-    }
+        list.clear();
+        list.reserve(max_devices);
+
+        pose_t device_states[max_devices];
+
+        if (v)
+        {
+            v->GetDeviceToAbsoluteTrackingPose(origin::TrackingUniverseSeated, 0,
+                                               device_states, vr::k_unMaxTrackedDeviceCount);
+
+            static constexpr unsigned bufsiz = vr::k_unTrackingStringSize;
+            char str[bufsiz] {};
+
+            for (unsigned k = 0; k < vr::k_unMaxTrackedDeviceCount; k++)
+            {
+               if (v->GetTrackedDeviceClass(k) == vr::ETrackedDeviceClass::TrackedDeviceClass_Invalid ||
+                   v->GetTrackedDeviceClass(k) == vr::ETrackedDeviceClass::TrackedDeviceClass_TrackingReference)
+                   continue;
+
+               if (!device_states[k].bDeviceIsConnected)
+                   continue;
+
+               unsigned len;
+
+               len = v->GetStringTrackedDeviceProperty(k, vr::ETrackedDeviceProperty::Prop_SerialNumber_String, str, bufsiz);
+               if (!len)
+               {
+                   qDebug() << "steamvr: getting serial number failed for" << k;
+                   continue;
+               }
+
+               device_spec dev;
+
+               dev.serial = str;
+
+               len = v->GetStringTrackedDeviceProperty(k, vr::ETrackedDeviceProperty::Prop_ModelNumber_String, str, bufsiz);
+               if (!len)
+               {
+                   qDebug() << "steamvr: getting model number failed for" << k;
+                   continue;
+               }
+
+               dev.model = str;
+               dev.pose = device_states[k];
+               dev.k = k;
+
+               list.push_back(dev);
+            }
+        }
+    });
+}
+
+device_list::device_list()
+{
+    refresh_device_list();
+}
+
+void device_list::refresh_device_list()
+{
+    fill_device_specs(device_specs);
+}
+
+device_list::maybe_pose device_list::get_pose(int k)
+{
+    if (k < 0 || !(k < max_devices))
+        return maybe_pose(false, pose_t{});
+
+    return with_vr_lock([k](vr_t v, error_t)
+    {
+        static pose_t poses[max_devices] {}; // vr_lock removes reentrancy
+
+        v->GetDeviceToAbsoluteTrackingPose(origin::TrackingUniverseSeated, 0,
+                                           poses, max_devices);
+
+        const pose_t& pose = poses[k];
+
+        if (pose.bPoseIsValid && pose.bDeviceIsConnected)
+            return maybe_pose(true, poses[k]);
+        else
+            once_only(qDebug() << "steamvr:"
+                               << "no valid pose from device" << k
+                               << "valid" << pose.bPoseIsValid
+                               << "connected" << pose.bDeviceIsConnected);
+
+        return maybe_pose(false, pose_t{});
+    });
+}
+
+bool device_list::get_all_poses(pose_t* poses)
+{
+    with_vr_lock([poses](vr_t v, error_t)
+    {
+        if (v)
+        {
+            v->GetDeviceToAbsoluteTrackingPose(origin::TrackingUniverseSeated, 0,
+                                               poses, max_devices);
+        }
+        return v != nullptr;
+    });
+
 }
 
-steamvr::vr_t steamvr::vr_init(error_t& error)
+tt device_list::vr_init()
 {
-    error = error_t::VRInitError_Unknown;
+    static tt t = vr_init_();
+    return t;
+}
 
-    // the background type would surely confuse users
-    vr_t vr = vr::VR_Init(&error, vr::EVRApplicationType::VRApplication_Other);
+tt device_list::vr_init_()
+{
+    error_t error = error_t::VRInitError_Unknown;
+    vr_t v = vr::VR_Init(&error, vr::EVRApplicationType::VRApplication_Other);
 
-    if (vr)
-        vr_deleter();
+    if (v)
+        std::atexit(vr::VR_Shutdown);
+    else
+        once_only(qDebug() << "steamvr: init failure" << error << strerror(error));
 
-    return vr;
+    return tt(v, error);
 }
 
-QString steamvr::strerror(error_t error)
+QString device_list::strerror(error_t err)
 {
-    const char* str(vr::VR_GetVRInitErrorAsSymbol(error));
+    const char* str(vr::VR_GetVRInitErrorAsSymbol(err));
     return QString(str ? str : "No description");
 }
 
-steamvr::steamvr() : vr(nullptr)
+steamvr::steamvr() : device_index(-1)
 {
 }
 
@@ -64,97 +180,106 @@ steamvr::~steamvr()
 
 void steamvr::start_tracker(QFrame*)
 {
-    error_t e(error_t::VRInitError_Unknown);
-    vr = vr_init(e);
-
-    if (!vr)
+    with_vr_lock([this](vr_t v, error_t e)
     {
-        QMessageBox::warning(nullptr,
-                             tr("Valve SteamVR init error"), strerror(e),
-                             QMessageBox::Close, QMessageBox::NoButton);
-        return;
-    }
+        if (!v)
+        {
+            QMessageBox::warning(nullptr,
+                                 tr("Valve SteamVR init error"), strerror(e),
+                                 QMessageBox::Close, QMessageBox::NoButton);
+        }
 
-    bool ok = false;
+        const QString serial = s.device_serial;
+        device_list d;
+        const QList<device_spec>& specs = d.devices();
+        const int sz = specs.count();
 
-    for (unsigned k = 0; k < vr::k_unMaxTrackedDeviceCount; k++)
-        if (vr->GetTrackedDeviceClass(k) == vr::ETrackedDeviceClass::TrackedDeviceClass_HMD)
+        if (sz == 0)
         {
-            ok = true;
-            break;
+            QMessageBox::warning(nullptr,
+                                 tr("Valve SteamVR init error"),
+                                 tr("No HMD connected"),
+                                 QMessageBox::Close, QMessageBox::NoButton);
         }
 
-    if (!ok)
-    {
-        QMessageBox::warning(nullptr,
-                             tr("Valve SteamVR init warning"),
-                             tr("No HMD connected"),
-                             QMessageBox::Close, QMessageBox::NoButton);
-        return;
-    }
+        for (const device_spec& spec : specs)
+        {
+            if (serial == "" || serial == spec.serial)
+            {
+                device_index = int(spec.k);
+                break;
+            }
+        }
+    });
 }
 
 void steamvr::data(double* data)
 {
-    if (vr)
+    if (device_index != -1)
     {
-        vr::TrackedDevicePose_t devices[vr::k_unMaxTrackedDeviceCount] = {};
-
-        vr->GetDeviceToAbsoluteTrackingPose(vr::ETrackingUniverseOrigin::TrackingUniverseSeated, 0,
-                                            devices, vr::k_unMaxTrackedDeviceCount);
-
-        for (unsigned k = 0; k < vr::k_unMaxTrackedDeviceCount; k++)
+        pose_t pose; bool ok;
+        std::tie(ok, pose) = device_list::get_pose(device_index);
+        if (ok)
         {
-            if (!devices[k].bPoseIsValid)
-                continue;
-
-            if (vr->GetTrackedDeviceClass(k) != vr::ETrackedDeviceClass::TrackedDeviceClass_HMD)
-                continue;
-
-            const vr::HmdMatrix34_t& result = devices[k].mDeviceToAbsoluteTracking;
+            static constexpr int c = 10;
 
-            int c = 10;
+            const auto& result = pose.mDeviceToAbsoluteTracking;
 
             data[TX] = -result.m[0][3] * c;
             data[TY] = result.m[1][3] * c;
             data[TZ] = result.m[2][3] * c;
 
-            using std::atan2;
-            using std::asin;
-
-            // transformation matrix to euler angles
-            if (result.m[0][0] == 1.0f || result.m[0][0] == -1.0f)
-            {
-                data[Yaw] = -atan2(result.m[0][2], result.m[2][3]);
-                data[Pitch] = 0;
-                data[Roll] = 0;
-            }
-            else
-            {
-                data[Yaw] = -atan2(-result.m[2][0], result.m[0][0]);
-                data[Pitch] = atan2(-result.m[1][2], result.m[1][1]);
-                data[Roll] = atan2(result.m[1][1], result.m[0][1]);
-            }
+            matrix_to_euler(data[Yaw], data[Pitch], data[Roll], result);
 
             static constexpr double r2d = 180 / M_PI;
-
-            data[Yaw] *= r2d;
-            data[Pitch] *= r2d;
-            data[Roll] *= r2d;
-
-            break;
+            data[Yaw] *= r2d; data[Pitch] *= r2d; data[Roll] *= r2d;
         }
     }
 }
 
 bool steamvr::center()
 {
-    if (vr)
-        vr->ResetSeatedZeroPose();
+    with_vr_lock([&](vr_t v, error_t)
+    {
+        if (v)
+            v->ResetSeatedZeroPose();
+    });
     return false;
 }
 
-void steamvr_dialog::register_tracker(ITracker*) {}
-void steamvr_dialog::unregister_tracker() {}
+void steamvr::matrix_to_euler(double& yaw, double& pitch, double& roll, const vr::HmdMatrix34_t& result)
+{
+    yaw = atan2(result.m[2][0], result.m[0][0]);
+    pitch = atan2(result.m[1][1], result.m[1][2]);
+    roll = atan2(result.m[1][1], result.m[0][1]);
+}
+
+steamvr_dialog::steamvr_dialog()
+{
+    ui.setupUi(this);
+
+    connect(ui.buttonBox, SIGNAL(accepted()), this, SLOT(doOK()));
+    connect(ui.buttonBox, SIGNAL(rejected()), this, SLOT(doCancel()));
+
+    ui.device->clear();
+    ui.device->addItem("");
+
+    device_list list;
+    for (const device_spec& spec : list.devices())
+        ui.device->addItem(spec.serial);
+
+    tie_setting(s.device_serial, ui.device);
+}
+
+void steamvr_dialog::doOK()
+{
+    s.b->save();
+    close();
+}
+
+void steamvr_dialog::doCancel()
+{
+    close();
+}
 
 OPENTRACK_DECLARE_TRACKER(steamvr, steamvr_dialog, steamvr_metadata)