1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
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)
|
