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
|
/* Copyright (c) 2012 Stanislaw Halik
*
* 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 "ftnoir_filter_accela/ftnoir_filter_accela.h"
#include <algorithm>
#include <cmath>
#include <QDebug>
#include <QMutexLocker>
#include "facetracknoir/global-settings.h"
using namespace std;
FTNoIR_Filter::FTNoIR_Filter()
{
first_run = true;
loadSettings();
}
FTNoIR_Filter::~FTNoIR_Filter()
{
}
void FTNoIR_Filter::loadSettings() {
QSettings settings("opentrack"); // Registry settings (in HK_USER)
QString currentFile = settings.value ( "SettingsFile", QCoreApplication::applicationDirPath() + "/settings/default.ini" ).toString();
QSettings iniFile( currentFile, QSettings::IniFormat ); // Application settings (in INI-file)
iniFile.beginGroup ( "Accela" );
zoom_factor = iniFile.value("zoom-slowness", ACCELA_ZOOM_SLOWNESS).toDouble();
rotation_alpha = iniFile.value("rotation-alpha", ACCELA_SMOOTHING_ROTATION).toDouble();
translation_alpha = iniFile.value("translation-alpha", ACCELA_SMOOTHING_TRANSLATION).toDouble();
iniFile.endGroup ();
iniFile.beginGroup("Accela-Scaling");
// bigger means less filtering
static const double init_scaling[] = {
1.5, // X
1.5, // Y
1, // Z
0.8, // Yaw
0.9, // Pitch
1.25 // Roll
};
for (int i = 0; i < 6; i++)
{
scaling[i] = iniFile.value(QString("axis-%1").arg(QString::number(i)), init_scaling[i]).toDouble();
}
iniFile.endGroup();
}
void FTNoIR_Filter::receiveSettings(double rot, double trans, double zoom_fac)
{
QMutexLocker foo(&mutex);
rotation_alpha = rot;
translation_alpha = trans;
zoom_factor = zoom_fac;
}
static inline double parabola(const double a, const double x)
{
const double a1 = 1./a;
return a1 * pow(std::max<double>(x - 0.175, 1e-5), 2.2);
}
void FTNoIR_Filter::FilterHeadPoseData(const double* target_camera_position,
double *new_camera_position,
const double* last_post_filter_values)
{
if (first_run)
{
for (int i = 0; i < 6; i++)
{
new_camera_position[i] = target_camera_position[i];
current_camera_position[i] = target_camera_position[i];
}
first_run = false;
return;
}
QMutexLocker foo(&mutex);
for (int i=0;i<6;i++)
{
const double vec = target_camera_position[i] - current_camera_position[i];
const int sign = vec < 0 ? -1 : 1;
const double x = fabs(vec);
const double a = i >= 3 ? rotation_alpha : translation_alpha;
const double reduction = 1. / std::max(1., 1. + zoom_factor * -last_post_filter_values[TZ] / 1000);
const double velocity = parabola(a, x * scaling[i]) * reduction;
const double result = current_camera_position[i] + velocity * sign;
const bool done = sign > 0 ? result >= target_camera_position[i] : result <= target_camera_position[i];
new_camera_position[i] = current_camera_position[i] = done ? target_camera_position[i] : result;
}
}
extern "C" FTNOIR_FILTER_BASE_EXPORT IFilter* CALLING_CONVENTION GetConstructor()
{
return new FTNoIR_Filter;
}
|
