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
|
/* Copyright (c) 2012-2013 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)
{
}
static inline double parabola(const double a, const double x, const double dz, const double expt)
{
const double sign = x > 0 ? 1 : -1;
const double a1 = 1./a;
return a1 * pow(std::max<double>(fabs(x) - dz, 0), expt) * sign;
}
template<typename T>
static inline T clamp(const T min, const T max, const T value)
{
if (value < min)
return min;
if (value > max)
return max;
return value;
}
void FTNoIR_Filter::FilterHeadPoseData(const double* target_camera_position,
double *new_camera_position)
{
if (first_run)
{
for (int i = 0; i < 6; i++)
{
new_camera_position[i] = target_camera_position[i];
last_input[i] = target_camera_position[i];
for (int j = 0; j < 3; j++)
last_output[j][i] = target_camera_position[i];
}
timer.start();
frame_delta = 1;
first_run = false;
return;
}
bool new_frame = false;
for (int i = 0; i < 6; i++)
{
if (target_camera_position[i] != last_input[i])
{
new_frame = true;
break;
}
}
if (new_frame)
{
for (int i = 0; i < 6; i++)
last_input[i] = target_camera_position[i];
frame_delta = timer.start();
} else {
auto d = timer.elapsed();
double c = clamp(0.0, 1.0, d / (double) frame_delta);
for (int i = 0; i < 6; i++)
new_camera_position[i] =
last_output[1][i] + (last_output[0][i] - last_output[1][i]) * c;
return;
}
for (int i=0;i<6;i++)
{
const double vec = target_camera_position[i] - last_output[0][i];
const double vec2 = target_camera_position[i] - last_output[1][i];
const double vec3 = target_camera_position[i] - last_output[2][i];
const int sign = vec < 0 ? -1 : 1;
const double a = i >= 3 ? s.rotation_alpha : s.translation_alpha;
const double a2 = a * s.second_order_alpha;
const double a3 = a * s.third_order_alpha;
const double deadzone = i >= 3 ? s.rot_deadzone : s.trans_deadzone;
const double velocity =
parabola(a, vec, deadzone, s.expt) +
parabola(a2, vec2, deadzone, s.expt) +
parabola(a3, vec3, deadzone, s.expt);
const double result = last_output[0][i] + velocity;
const bool done = sign > 0 ? result >= target_camera_position[i] : result <= target_camera_position[i];
new_camera_position[i] = done ? target_camera_position[i] : result;
last_output[2][i] = last_output[1][i];
last_output[1][i] = last_output[0][i];
last_output[0][i] = new_camera_position[i];
}
}
extern "C" FTNOIR_FILTER_BASE_EXPORT IFilter* CALLING_CONVENTION GetConstructor()
{
return new FTNoIR_Filter;
}
|
