Fast C++ sine and cosine alternatives for real-time signal processing
I know a solution that can suit you. Recall the school formula of sine and cosine for the sum of angles:
sin(a + b) = sin(a) * cos(b) + cos(a) * sin(b)
cos(a + b) = cos(a) * cos(b) - sin(a) * sin(b)
Suppose that wdt
is a small increment of the wt
angle, then we get the recursive calculation formula for the sin
and cos
for next time:
sin(wt + wdt) = sin(wt) * cos(wdt) + cos(wt) * sin(wdt)
cos(wt + wdt) = cos(wt) * cos(wdt) - sin(wt) * sin(wdt)
We need to calculate the sin(wdt)
and cos(wdt)
values only once. For other computations we need only addition and multiplication operations. Recursion can be continued from any time moment, so we can replace the values with exactly calculated time by time to avoid indefinitely error accumulation.
There is final code:
class QuadroDetect
{
const double sinwdt;
const double coswdt;
const double wdt;
double sinwt = 0;
double coswt = 1;
double wt = 0;
QuadroDetect(double w, double dt) :
sinwdt(sin(w * dt)),
coswdt(cos(w * dt)),
wdt(w * dt)
{}
inline double process(const double in)
{
double f1 = in * sinwt;
double f2 = in * coswt;
double out = sqrt(f1 * f1 + f2 * f2);
double tmp = sinwt;
sinwt = sinwt * coswdt + coswt * sinwdt;
coswt = coswt * coswdt - tmp * sinwdt;
// Recalculate sinwt and coswt to avoid indefinitely error accumulation
if (wt > 2 * M_PI)
{
wt -= 2 * M_PI;
sinwt = sin(wt);
coswt = cos(wt);
}
wt += wdt;
return out;
}
};
Please note that such recursive calculations provides less accurate results than sin(wt)
cos(wt)
, but I used it and it worked well.
If you can use std::complex the implementation becomes much simpler. Technical its the same solution as from @Dmytro Dadyka as complex numbers are working this way. If the optimiser works well it should be run the same time.
class QuadroDetect
{
public:
std::complex<double> wt;
std::complex <double> wdt;
LowFreqFilter lf1;
LowFreqFilter lf2;
QuadroDetect(const double w, const double dt)
: wt(1.0, 0.0)
, wdt(std::polar(1.0, w * dt))
{
}
inline double process(const double in)
{
auto f = in * wt;
f.imag(lf1.process(f.imag()));
f.real(lf2.process(f.real()));
wt *= wdt;
return std::abs(f);
}
};