libdspl-2.0/dspl/src/goertzel.c

110 wiersze
2.6 KiB
C
Czysty Zwykły widok Historia

/*
* Copyright (c) 2015-2018 Sergey Bakhurin
* Digital Signal Processing Library [http://dsplib.org]
*
* This file is part of libdspl-2.0.
2018-10-24 17:39:51 +00:00
*
* is free software: you can redistribute it and/or modify
* it under the terms of the GNU Lesser General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* DSPL is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU Lesser General Public License
* along with Foobar. If not, see <http://www.gnu.org/licenses/>.
*/
#include <stdlib.h>
#include <string.h>
#include "dspl.h"
2018-10-24 17:39:51 +00:00
/******************************************************************************
Goertzel algorithm for real vector
2018-10-24 17:39:51 +00:00
*******************************************************************************/
int DSPL_API goertzel(double *x, int n, int *ind, int k, complex_t *y)
{
2018-10-24 17:39:51 +00:00
int m, p;
double wR, wI;
double alpha;
double v[3];
if(!x || !y || !ind)
return ERROR_PTR;
if(n < 1 || k < 1)
return ERROR_SIZE;
for(p = 0; p < k; p++)
{
wR = cos(M_2PI * (double)ind[p] / (double)n);
wI = sin(M_2PI * (double)ind[p] / (double)n);
alpha = 2.0 * wR;
v[0] = v[1] = v[2] = 0.0;
for(m = 0; m < n; m++)
{
v[2] = v[1];
v[1] = v[0];
v[0] = x[m]+alpha*v[1] - v[2];
}
RE(y[p]) = wR * v[0] - v[1];
IM(y[p]) = wI * v[0];
}
return RES_OK;
}
2018-10-24 17:39:51 +00:00
/******************************************************************************
Goertzel algorithm for complex vector
2018-10-24 17:39:51 +00:00
*******************************************************************************/
int DSPL_API goertzel_cmplx(complex_t *x, int n, int *ind, int k, complex_t *y)
{
2018-10-24 17:39:51 +00:00
int m, p;
complex_t w;
double alpha;
complex_t v[3];
if(!x || !y || !ind)
return ERROR_PTR;
if(n < 1 || k < 1)
return ERROR_SIZE;
for(p = 0; p < k; p++)
{
RE(w) = cos(M_2PI * (double)ind[p] / (double)n);
IM(w) = sin(M_2PI * (double)ind[p] / (double)n);
alpha = 2.0 * RE(w);
memset(v, 0, 3*sizeof(complex_t));
for(m = 0; m < n; m++)
{
RE(v[2]) = RE(v[1]);
RE(v[1]) = RE(v[0]);
RE(v[0]) = RE(x[m]) + alpha * RE(v[1]) - RE(v[2]);
IM(v[2]) = IM(v[1]);
IM(v[1]) = IM(v[0]);
IM(v[0]) = IM(x[m]) + alpha * IM(v[1]) - IM(v[2]);
}
RE(y[p]) = CMRE(w, v[0]) - RE(v[1]);
IM(y[p]) = CMIM(w, v[0]) - IM(v[1]);
}
return RES_OK;
}
2018-10-24 17:39:51 +00:00