CustomOperatorSamples/CHOP/OneEuroCHOP/OneEuroImpl.cpp at main · TouchDesigner/CustomOperatorSamples · GitHub

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
#include "OneEuroImpl.h"
#include <cmath>

static constexpr double PI = 3.141592653589793238463;

class LowPassFilter
{
public:
    LowPassFilter() :
        myFirstFiltering{ true }, myHatXPrev{}, myHatX{}
    {

    }

    virtual ~LowPassFilter()
    {

    }

    double
    filter(double value, double alpha)
    {
        if (myFirstFiltering)
        {
            myFirstFiltering = false;
            myHatXPrev = value;
        }
        myHatX = alpha * value + (1.0 - alpha) * myHatXPrev;
        myHatXPrev = myHatX;
        return myHatX;
    }

    double
    getHatXPrev()
    {
        return myHatXPrev;
    }

private:
    bool    myFirstFiltering;

    double  myHatXPrev;
    double  myHatX;
};

OneEuroImpl::OneEuroImpl(
        double rate, double minCutOff, double beta, double dCutOff
    ) :
    myFirstFiltering{true}, myRate{rate}, myMinCutOff{minCutOff},
    myBeta{beta}, myXFilt{new LowPassFilter()}, myDCutOff{dCutOff},
    myDxFilt{new LowPassFilter()}
{

}

OneEuroImpl::~OneEuroImpl()
{
    delete myXFilt;
    delete myDxFilt;
}

void
OneEuroImpl::changeInput(double rate, double minCutOff, double beta, double dCutOff)
{
    myRate = rate;
    myMinCutOff = minCutOff;
    myBeta = beta;
    myDCutOff = dCutOff;
}

double
OneEuroImpl::filter(double x)
{
    double  dx = myFirstFiltering ? 0.0 : (x - myXFilt->getHatXPrev()) * myRate;
    myFirstFiltering = false;
    double  edx = myDxFilt->filter(dx, alpha(myDCutOff));
    double cutOff = myMinCutOff + myBeta * std::abs(edx);
    return myXFilt->filter(x, alpha(cutOff));
}

double
OneEuroImpl::alpha(double cutoff)
{
    double  tau = 1.0 / (2 * PI * cutoff);
    double  te = 1.0 / myRate;
    return 1.0 / (1.0 + tau/te);
}