From 4261d6e6cc366884cd35daf66d9fa35c3dda6267 Mon Sep 17 00:00:00 2001
From: Khoa Nguyen <khoanguyen@3forcom.com>
Date: Tue, 26 Sep 2023 12:27:06 +0700
Subject: tracker/tobii: add tobii input
---
tracker-tobii/tobii.cpp | 121 ++++++++++++++++++++++++++++++++++++++++++++++++
1 file changed, 121 insertions(+)
create mode 100644 tracker-tobii/tobii.cpp
(limited to 'tracker-tobii/tobii.cpp')
diff --git a/tracker-tobii/tobii.cpp b/tracker-tobii/tobii.cpp
new file mode 100644
index 00000000..e25cf52a
--- /dev/null
+++ b/tracker-tobii/tobii.cpp
@@ -0,0 +1,121 @@
+/* Copyright (c) 2023, Khoa Nguyen <khoanguyen@3forcom.com>
+
+ * Permission to use, copy, modify, and/or distribute this
+ * software for any purpose with or without fee is hereby granted,
+ * provided that the above copyright notice and this permission
+ * notice appear in all copies.
+ */
+
+#include "tobii.h"
+#include "compat/math-imports.hpp"
+
+#include <QMutexLocker>
+
+static constexpr double rad_to_deg = 180.0 * M_1_PI;
+static constexpr double mm_to_cm = 0.1;
+
+static void url_receiver(char const* url, void* user_data)
+{
+ char* buffer = (char*)user_data;
+ if (*buffer != '\0')
+ return; // only keep first value
+
+ if (strlen(url) < 256)
+ strcpy(buffer, url);
+}
+
+static void head_pose_callback(tobii_head_pose_t const* head_pose, void* user_data)
+{
+ // Store the latest head pose data in the supplied storage
+ tobii_head_pose_t* head_pose_storage = (tobii_head_pose_t*)user_data;
+ *head_pose_storage = *head_pose;
+}
+
+tobii_tracker::tobii_tracker() = default;
+
+tobii_tracker::~tobii_tracker()
+{
+ QMutexLocker lck(&mtx);
+ if (device)
+ {
+ tobii_head_pose_unsubscribe(device);
+ tobii_device_destroy(device);
+ }
+ if (api)
+ {
+ tobii_api_destroy(api);
+ }
+}
+
+module_status tobii_tracker::start_tracker(QFrame*)
+{
+ QMutexLocker lck(&mtx);
+ tobii_error_t tobii_error = tobii_api_create(&api, nullptr, nullptr);
+ if (tobii_error != TOBII_ERROR_NO_ERROR)
+ {
+ return error("Failed to initialize the Tobii Stream Engine API.");
+ }
+
+ char url[256] = { 0 };
+ tobii_error = tobii_enumerate_local_device_urls(api, url_receiver, url);
+ if (tobii_error != TOBII_ERROR_NO_ERROR || url[0] == '\0')
+ {
+ tobii_api_destroy(api);
+ return error("No stream engine compatible device(s) found.");
+ }
+
+ tobii_error = tobii_device_create(api, url, TOBII_FIELD_OF_USE_INTERACTIVE, &device);
+ if (tobii_error != TOBII_ERROR_NO_ERROR)
+ {
+ tobii_api_destroy(api);
+ return error(QString("Failed to connect to %1.").arg(url));
+ }
+
+ tobii_error = tobii_head_pose_subscribe(device, head_pose_callback, &latest_head_pose);
+ if (tobii_error != TOBII_ERROR_NO_ERROR)
+ {
+ tobii_device_destroy(device);
+ tobii_api_destroy(api);
+ return error("Failed to subscribe to head pose stream.");
+ }
+
+ return status_ok();
+}
+
+void tobii_tracker::data(double* data)
+{
+ QMutexLocker lck(&mtx);
+ tobii_error_t tobii_error = tobii_device_process_callbacks(device);
+ if (tobii_error != TOBII_ERROR_NO_ERROR)
+ {
+ return;
+ }
+
+ // Tobii coordinate system is different from OpenTrack's
+ // Tobii: +x is to the right, +y is up, +z is towards the user
+ // Rotation xyz is in radians, x is pitch, y is yaw, z is roll
+
+ if (latest_head_pose.position_validity == TOBII_VALIDITY_VALID)
+ {
+ data[TX] = -latest_head_pose.position_xyz[0] * mm_to_cm;
+ data[TY] = latest_head_pose.position_xyz[1] * mm_to_cm;
+ data[TZ] = latest_head_pose.position_xyz[2] * mm_to_cm;
+ }
+
+ if (latest_head_pose.rotation_validity_xyz[0] == TOBII_VALIDITY_VALID)
+ {
+ data[Pitch] = latest_head_pose.rotation_xyz[0] * rad_to_deg;
+ }
+
+ if (latest_head_pose.rotation_validity_xyz[1] == TOBII_VALIDITY_VALID)
+ {
+ data[Yaw] = -latest_head_pose.rotation_xyz[1] * rad_to_deg;
+ }
+
+ if (latest_head_pose.rotation_validity_xyz[2] == TOBII_VALIDITY_VALID)
+ {
+ data[Roll] = latest_head_pose.rotation_xyz[2] * rad_to_deg;
+ }
+}
+
+OPENTRACK_DECLARE_TRACKER(tobii_tracker, tobii_dialog, tobii_metadata)
--
cgit v1.2.3