#include <float.h>
#include <limits.h>
#define _USE_MATH_DEFINES
#include <math.h>
#include <stdio.h>
#include <stdlib.h>
#define CHECK_ERROR(condition, errorcode, goto_point) \
if ((condition)) { \
errcode = (errorcode); \
goto goto_point; \
}
#define ALMOST_ZERO 0.000001
typedef struct {
unsigned int count;
unsigned int* index;
double* coeff;
} interp_filter;
typedef struct {
unsigned int factor;
unsigned int taps;
unsigned int channels;
unsigned int delay;
interp_filter* filter;
float** z;
unsigned int zi;
} interpolator_c;
interpolator_c*
interp_create_c(unsigned int taps, unsigned int factor, unsigned int channels) {
int errcode;
interpolator_c* interp;
unsigned int j;
(void) errcode;
interp = (interpolator_c*) calloc(1, sizeof(interpolator_c));
CHECK_ERROR(!interp, 0, exit);
interp->taps = taps;
interp->factor = factor;
interp->channels = channels;
interp->delay = (interp->taps + interp->factor - 1) / interp->factor;
interp->filter =
(interp_filter*) calloc(interp->factor, sizeof(*interp->filter));
CHECK_ERROR(!interp->filter, 0, free_interp);
for (j = 0; j < interp->factor; j++) {
interp->filter[j].index =
(unsigned int*) calloc(interp->delay, sizeof(unsigned int));
interp->filter[j].coeff = (double*) calloc(interp->delay, sizeof(double));
CHECK_ERROR(!interp->filter[j].index || !interp->filter[j].coeff, 0,
free_filter_index_coeff);
}
interp->z = (float**) calloc(interp->channels, sizeof(float*));
CHECK_ERROR(!interp->z, 0, free_filter_index_coeff);
for (j = 0; j < interp->channels; j++) {
interp->z[j] = (float*) calloc(interp->delay, sizeof(float));
CHECK_ERROR(!interp->z[j], 0, free_filter_z);
}
for (j = 0; j < interp->taps; j++) {
double m = (double) j - (double) (interp->taps - 1) / 2.0;
double c = 1.0;
if (fabs(m) > ALMOST_ZERO) {
c = sin(m * M_PI / interp->factor) / (m * M_PI / interp->factor);
}
c *= 0.5 * (1 - cos(2 * M_PI * j / (interp->taps - 1)));
if (fabs(c) > ALMOST_ZERO) {
unsigned int f = j % interp->factor;
unsigned int t = interp->filter[f].count++;
interp->filter[f].coeff[t] = c;
interp->filter[f].index[t] = j / interp->factor;
}
}
return interp;
free_filter_z:
for (j = 0; j < interp->channels; j++) {
free(interp->z[j]);
}
free(interp->z);
free_filter_index_coeff:
for (j = 0; j < interp->factor; j++) {
free(interp->filter[j].index);
free(interp->filter[j].coeff);
}
free(interp->filter);
free_interp:
free(interp);
exit:
return NULL;
}
void interp_destroy_c(interpolator_c* interp) {
unsigned int j = 0;
if (!interp) {
return;
}
for (j = 0; j < interp->factor; j++) {
free(interp->filter[j].index);
free(interp->filter[j].coeff);
}
free(interp->filter);
for (j = 0; j < interp->channels; j++) {
free(interp->z[j]);
}
free(interp->z);
free(interp);
}
size_t
interp_process_c(interpolator_c* interp, size_t frames, float* in, float* out) {
size_t frame = 0;
unsigned int chan = 0;
unsigned int f = 0;
unsigned int t = 0;
unsigned int out_stride = interp->channels * interp->factor;
float* outp = 0;
double acc = 0;
double c = 0;
for (frame = 0; frame < frames; frame++) {
for (chan = 0; chan < interp->channels; chan++) {
interp->z[chan][interp->zi] = *in++;
outp = out + chan;
for (f = 0; f < interp->factor; f++) {
acc = 0.0;
for (t = 0; t < interp->filter[f].count; t++) {
int i = (int) interp->zi - (int) interp->filter[f].index[t];
if (i < 0) {
i += (int) interp->delay;
}
c = interp->filter[f].coeff[t];
acc += (double) interp->z[chan][i] * c;
}
*outp = (float) acc;
outp += interp->channels;
}
}
out += out_stride;
interp->zi++;
if (interp->zi == interp->delay) {
interp->zi = 0;
}
}
return frames * interp->factor;
}