Я знаю решение, которое подойдет вам.Напомним школьную формулу синуса и косинуса для суммы углов:
sin(a + b) = sin(a) * cos(b) + cos(a) * sin(b)
cos(a + b) = cos(a) * cos(b) - sin(a) * sin(b)
Предположим, что wdt
- это небольшое приращение угла wt
, тогда мы получим формулу рекурсивного вычисления для sin
и cos
для следующего раза:
sin(wt + wdt) = sin(wt) * cos(wdt) + cos(wt) * sin(wdt)
cos(wt + wdt) = cos(wt) * cos(wdt) - sin(wt) * sin(wdt)
Нам нужно вычислить значения sin(wdt)
и cos(wdt)
только один раз.Для других вычислений нам нужны только операции сложения и умножения.Рекурсию можно продолжить с любого момента времени, поэтому мы можем заменить значения точно рассчитанным временем по времени, чтобы избежать неопределенного накопления ошибок.
Существует окончательный код:
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;
}
};
Обратите внимание, чтотакие рекурсивные вычисления дают менее точные результаты, чем sin(wt)
cos(wt)
, но я использовал его, и он работал хорошо.