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
122
123
124
125
126
127
128
129
130
131
132
133
134
|
#include "ftnoir_tracker_freepie-udp.h"
#include "api/plugin-api.hpp"
#include "compat/math.hpp"
#include <cinttypes>
#include <algorithm>
#include <cmath>
tracker_freepie::tracker_freepie() : pose { 0,0,0, 0,0,0 }
{
}
tracker_freepie::~tracker_freepie()
{
requestInterruption();
wait();
}
void tracker_freepie::run() {
#ifdef __clang__
struct __attribute__((packed)) {
uint8_t pad1;
uint8_t flags;
float fl[12];
} data {};
#else
#pragma pack(push, 1)
struct {
uint8_t pad1;
uint8_t flags;
float fl[12];
} data {};
#pragma pack(pop)
#endif
enum F
{
flag_Raw = 1 << 0,
flag_Orient = 1 << 1,
Mask = flag_Raw | flag_Orient
};
sock.bind(QHostAddress::Any, (unsigned short) s.port, QUdpSocket::ShareAddress | QUdpSocket::ReuseAddressHint);
while (!isInterruptionRequested())
{
int order[] =
{
clamp(s.idx_x, 0, 2),
clamp(s.idx_y, 0, 2),
clamp(s.idx_z, 0, 2)
};
double orient[3] = {0, 0, 0};
bool filled = false;
while (sock.hasPendingDatagrams())
{
using t = decltype(data);
t tmp {};
(void) sock.readDatagram(reinterpret_cast<char*>(&tmp), sizeof(data));
int flags = tmp.flags & F::Mask;
switch (flags)
{
//default:
case flag_Raw:
continue;
case flag_Raw | flag_Orient:
for (int i = 0; i < 3; i++)
orient[i] = (double)tmp.fl[i+9];
break;
case flag_Orient:
for (int i = 0; i < 3; i++)
orient[i] = (double)tmp.fl[i];
break;
default: goto fail;
}
filled = true;
data = tmp;
}
if (filled)
{
constexpr int add_cbx[] =
{
0,
90,
-90,
180,
-180,
};
int add_indices[] = { s.add_yaw, s.add_pitch, s.add_roll };
QMutexLocker foo(&mtx);
constexpr double r2d = 180 / M_PI;
for (int i = 0; i < 3; i++)
{
const int axis = order[i];
const int add_idx = add_indices[i];
int add = 0;
if (add_idx >= 0 && add_idx < (int)std::size(add_cbx))
add = add_cbx[add_idx];
pose[Yaw + i] = r2d * orient[axis] + add;
}
}
fail:
usleep(4000);
}
}
module_status tracker_freepie::start_tracker(QFrame*)
{
start();
sock.moveToThread(this);
return status_ok();
}
void tracker_freepie::data(double *data)
{
QMutexLocker foo(&mtx);
data[Yaw] = pose[Yaw];
data[Pitch] = pose[Pitch];
data[Roll] = pose[Roll];
}
OPENTRACK_DECLARE_TRACKER(tracker_freepie, dialog_freepie, meta_freepie)
|
