int toPowerOf2(int x) {
int xx;
for(xx = 1; x > 1; x >>= 1)
xx <<= 1;
return xx;
}
void fft(double *real, int realStep, double *img, int imgStep, int count, int invert) {
if (count < 2 || !realStep || !imgStep) return;
count = toPowerOf2(count);
// check order
int flipReal = 0;
if (realStep < 0) {
flipReal = 1;
real += realStep*(count - 1);
realStep = -realStep;
}
int flipImg = 0;
if (imgStep < 0) {
flipImg = 1;
img += imgStep*(count - 1);
imgStep = -imgStep;
}
double *realEnd = real + count*realStep;
double *imgEnd = img + count*imgStep;
// flip if step was negative
if (flipReal)
for(double *i = real, *j = realEnd - realStep; i < j; i += realStep, j -= realStep)
{ double x = *i; *i = *j; *j = x; }
if (flipImg)
for(double *i = img, *j = imgEnd - imgStep; i < j; i += imgStep, j -= imgStep)
{ double x = *i; *i = *j; *j = x; }
// bit-reversal permutation
for(int i=0, j=0; i < count; ++i) {
if (j > i) {
double x, *a, *b;
a = real + i*realStep; b = real + j*realStep; x = *a; *a = *b; *b = x;
a = img + i*imgStep; b = img + j*imgStep; x = *a; *a = *b; *b = x;
}
int m = count/2;
while(m >= 1 && j >= m)
{ j -= m; m /= 2; }
j += m;
}
double k = invert ? PI : -PI;
for(int mmax = 1; mmax < count; mmax *= 2) {
// rotation coefficients
double angle = k/mmax;
double wpR = sin(0.5*angle), wpI = sin(angle);
wpR *= wpR*2;
double wR = 1, wI = 0;
int mmaxRealStep = mmax*realStep;
int mmaxRealStep2 = 2*mmaxRealStep;
int mmaxImgStep = mmax*imgStep;
int mmaxImgStep2 = 2*mmaxImgStep;
for(int m = 0; m < mmax; ++m) {
// rotate w
double wwR = wR;
wR = wR - wwR*wpR - wI*wpI,
wI = wI + wwR*wpI - wI*wpR;
// process subsequences
for( double *iR = real + m*realStep,
*iI = img + m*imgStep,
*jR = iR + mmaxRealStep,
*jI = iI + mmaxImgStep;
iR < realEnd;
iR += mmaxRealStep2,
iI += mmaxImgStep2,
jR += mmaxRealStep2,
jI += mmaxImgStep2 )
{
// radix
double tR = *jR*wR - *jI*wI;
double tI = *jR*wI + *jI*wR;
*jR = *iR - tR;
*jI = *iI - tI;
*iR += tR;
*iI += tI;
}
}
}
// reverse order
if (!flipReal)
for(double *i = real, *j = realEnd - realStep; i < j; i += realStep, j -= realStep)
{ double x = *i; *i = *j; *j = x; }
if (!flipImg)
for(double *i = img, *j = imgEnd - imgStep; i < j; i += imgStep, j -= imgStep)
{ double x = *i; *i = *j; *j = x; }
// divide by count to complete back-FFT
if (invert) {
double k = 1.0/count;
for(double *i = real; i < realEnd; i += realStep)
*i *= k;
for(double *i = img; i < imgEnd; i += imgStep)
*i *= k;
}
}
void fft2d(double *real, int realColStep, int realRowStep, double *img, int imgColStep, int imgRowStep, int rows, int cols, int invert) {
if (rows < 1 || cols < 1 || !realColStep || !imgColStep || !realRowStep || imgRowStep) return;
rows = toPowerOf2(rows);
cols = toPowerOf2(cols);
// fft rows
if (cols > 1)
for(double *r = real, *i = img, *end = real + rows*realRowStep; r < end; r += realRowStep, i += imgRowStep)
fft(r, realColStep, i, imgColStep, cols, invert);
// fft cols
if (rows > 1)
for(double *r = real, *i = img, *end = real + cols*realColStep; r < end; r += realColStep, i += imgColStep)
fft(r, realRowStep, i, imgRowStep, rows, invert);
}
double preGauss(double x, double r) {
if (fabs(r) < IMG_PRECISION)
return fabs(x) < IMG_PRECISION ? 1 : 0;
return exp(-0.5*x*x/(r*r));
}
double gauss(double x, double r) {
static const double k = 1/sqrt(2*PI);
return k*preGauss(x, r)/fabs(r);
}
void gaussBlurRows(double *row, int colStep, int rowStep, int cols, int rows, double size) {
if (!(size > IMG_PRECISION)) return;
int cnt = cols + 2*(int)ceil(size*4 - IMG_PRECISION);
int p2cnt = toPowerOf2(cnt);
if (p2cnt < cnt) p2cnt <<= 1;
cnt = p2cnt;
double *buf = (double*)calloc(1, sizeof(double)*4*cnt);
double *pat = buf + 2*cnt;
pat[0] = preGauss(0, size);
pat[cnt] = preGauss(cnt/2, size);
for(int i = 2; i < cnt; i += 2)
pat[i] = pat[cnt*2 - i] = preGauss(i, size);
fft(pat, 2, pat+1, 2, cnt, FALSE);
double *buf0 = buf + (cnt - cols);
double *buf1 = buf0 + (cols - 1)*2;
for(int i = 0; i < rows; ++i) {
for(int ch = 0; ch < 4; ++ch) {
memset(buf, 0, sizeof(double)*2*cnt);
double *chrow = row + i*rowStep + ch;
for(int j = 0; j < cols; ++j)
buf0[j*2] = chrow[j*colStep];
for(double v = *buf0, *b = buf; b < buf0; b += 2) *b = v;
for(double v = *buf1, *b = buf1 + 2; b < pat; b += 2) *b = v;
fft(buf, 2, buf+1, 2, cnt, FALSE);
for(double *b = buf, *p = pat; b < pat; b += 2, p += 2) {
double bR = b[0], bI = b[1];
b[0] = bR*p[0] - bI*p[1];
b[1] = bR*p[1] + bI*p[0];
}
fft(buf, 2, buf+1, 2, cnt, TRUE);
for(int j = 0; j < cols; ++j) {
double *b = buf0 + j*2;
chrow[j*colStep] = sqrt(b[0]*b[0] + b[1]*b[1]);
}
}
}
free(buf);
}
void filterBlurGauss(Img *img, double sx, double sy) {
if (!img->data) return;
sx = fabs(sx);
sy = fabs(sy);
if (!(sx > IMG_PRECISION) && !(sy > IMG_PRECISION)) return;
filterMulAlpha(img);
gaussBlurRows(img->data, 4, 4*img->w, img->w, img->h, sx);
gaussBlurRows(img->data, 4*img->w, 4, img->h, img->w, sy);
filterDivAlpha(img);
}