/*
The noise reduction algorithms is based on optimal filters, whereas the
PDS of the noise is estimated with a minimum statistic.
We use an overlap and add method to avoid clicks caused by fast changing
optimal filters between successive blocks.
[Ref] A. Engel: "Transformationsbasierte Systeme zur einkanaligen
Stoerunterdrueckung bei Sprachsignalen", PhD Thesis, Christian-
Albrechts-Universitaet zu Kiel, 1998
*/
void CNoiseReduction::Process(CRealVector& vecrIn)
{
/* Regular block (updates the noise estimate) --------------------------- */
/* Update history of input signal */
vecrLongSignal.Merge(vecrOldSignal, vecrIn);
/* Update signal PSD estimation */
veccSigFreq = rfft(vecrLongSignal, FftPlan);
vecrSqMagSigFreq = SqMag(veccSigFreq);
/* Update minimum statistic for noise PSD estimation. This update is made
only once a regular (non-shited) block */
UpdateNoiseEst(vecrNoisePSD, vecrSqMagSigFreq, eNoiRedDegree);
/* Actual noise reducation filtering based on the noise PSD estimation and
the current squared magnitude of the input signal */
vecrFiltResult = OptimalFilter(veccSigFreq, vecrSqMagSigFreq, vecrNoisePSD);
/* Apply windowing */
vecrFiltResult *= vecrTriangWin;
/* Build output signal vector with old half and new half */
vecrOutSig1.Merge(vecrOldOutSignal, vecrFiltResult(1, iHalfBlockLen));
/* Save second half of output signal for next block (for overlap and add) */
vecrOldOutSignal = vecrFiltResult(iHalfBlockLen + 1, iBlockLen);
/* "Half-shifted" block for overlap and add ----------------------------- */
/* Build input vector for filtering the "half-shifted" blocks. It is the
second half of the very old signal plus the complete old signal and the
first half of the current signal */
vecrLongSignal.Merge(vecrVeryOldSignal(iHalfBlockLen + 1, iBlockLen),
vecrOldSignal, vecrIn(1, iHalfBlockLen));
/* Store old input signal blocks */
vecrVeryOldSignal = vecrOldSignal;
vecrOldSignal = vecrIn;
/* Update signal PSD estimation for "half-shifted" block and calculate
optimal filter */
veccSigFreq = rfft(vecrLongSignal, FftPlan);
vecrSqMagSigFreq = SqMag(veccSigFreq);
vecrFiltResult = OptimalFilter(veccSigFreq, vecrSqMagSigFreq, vecrNoisePSD);
/* Apply windowing */
vecrFiltResult *= vecrTriangWin;
/* Overlap and add operation */
vecrIn = vecrFiltResult + vecrOutSig1;
}
extern "C" void equ_makeTable(SuperEqState *state, REAL *lbc,void *_param,REAL fs)
{
paramlist *param = (paramlist *)_param;
int i,cires = state->cur_ires;
REAL *nires;
if (fs <= 0) return;
paramlist param2;
for (int ch = 0; ch < state->channels; ch++) {
process_param(lbc,param,param2,fs,ch);
for(i=0;i<state->winlen;i++)
state->irest[i] = hn(i-state->winlen/2,param2,fs)*win(i-state->winlen/2,state->winlen);
for(;i<state->tabsize;i++)
state->irest[i] = 0;
rfft(state->fft_bits,1,state->irest);
nires = cires == 1 ? state->lires2 : state->lires1;
nires += ch * state->tabsize;
for(i=0;i<state->tabsize;i++)
nires[i] = state->irest[i];
}
state->chg_ires = cires == 1 ? 2 : 1;
}
void fft (double *a, int n, int signum)
{ complex z;
double h;
int i;
if (n==0) return;
nn=n;
ram=newram;
if (!freeram(2*(1+n)*sizeof(complex))) outofram();
ff=(complex *)a;
zz=(complex *)ram;
ram+=n*sizeof(complex);
fh=(complex *)ram;
ram+=n*sizeof(complex);
/* compute zz[k]=e^{-2*pi*i*k/n}, k=0,...,n-1 */
h=2*M_PI/n; z[0]=cos(h); z[1]=signum*sin(h);
zz[0][0]=1; zz[0][1]=0;
for (i=1; i<n; i++)
{ if (i%16) { zz[i][0]=cos(i*h); zz[i][1]=signum*sin(i*h); }
else c_mult(zz[i-1],z,zz[i]);
}
rfft(0,n,1,1);
if (signum==1)
for (i=0; i<n; i++)
{ ff[i][0]*=n; ff[i][1]*=n;
}
}
void equ_makeTable(REAL *lbc,REAL *rbc,paramlist *param,REAL fs)
{
int i,cires = cur_ires;
REAL *nires;
if (fs <= 0) return;
paramlist param2;
// L
process_param(lbc,param,param2,fs,0);
for(i=0;i<winlen;i++)
irest[i] = hn(i-winlen/2,param2,fs)*win(i-winlen/2,winlen);
for(;i<tabsize;i++)
irest[i] = 0;
rfft(tabsize,1,irest);
nires = cires == 1 ? lires2 : lires1;
for(i=0;i<tabsize;i++)
nires[i] = irest[i];
process_param(rbc,param,param2,fs,1);
// R
for(i=0;i<winlen;i++)
irest[i] = hn(i-winlen/2,param2,fs)*win(i-winlen/2,winlen);
for(;i<tabsize;i++)
irest[i] = 0;
rfft(tabsize,1,irest);
nires = cires == 1 ? rires2 : rires1;
for(i=0;i<tabsize;i++)
nires[i] = irest[i];
//
chg_ires = cires == 1 ? 2 : 1;
}
int spectralFlux(Frame &inFrame, int winsize, std::vector<float> &fluxes)
{
int i, j;
int hoplen = (int)(((double)winsize) * hopsize);
// the number of windows including possibly fractional last window
int numwins = inFrame.wsize / hoplen + (inFrame.wsize % hoplen != 0);
Frame windows[2];
float magsums[2];
magsums[1] = 0.00001f;
windows[0].alloc_waveform(winsize);
windows[1].alloc_waveform(winsize);
for(i = 0; i < winsize; i++)
windows[1].waveform[i] = 0;
windows[1].alloc_pol();
for(i = 0; i < numwins; i++){
fluxes.push_back(0);
Frame &window = windows[i % 2];
Frame &lastwindow = windows[(i + 1) % 2];
float *magsum = &magsums[i % 2];
float *lastmagsum = &magsums[(i + 1) % 2];
*magsum = 0.00001f;
int winindex = i * hoplen;
if(i < numwins - 1)
for(j = 0; j < winsize; j++)
{
if( j + winindex < inFrame.wsize )
window.waveform[j] = inFrame.waveform[j + winindex];
else
window.waveform[j] = 0;
}
else{
for(j = winindex; j < inFrame.wsize; j++)
window.waveform[j - winindex] = inFrame.waveform[j];
for(j = inFrame.wsize - winindex; j < winsize; j++)
window.waveform[j] = 0;
}
rfft(window.waveform, window.len, FFT_FORWARD);
window.cmp2pol();
for(j = 0; j < window.len; j++)
*magsum += window.pol[j].mag;
for(j = 0; j < window.len; j++){
float diff = window.pol[j].mag / *magsum -
lastwindow.pol[j].mag / *lastmagsum;
fluxes[i] += diff * diff;
}
fluxes[i] = sqrt(fluxes[i]);
if(fluxes[i] != fluxes[i])
fluxes[i] = 0;
}
return fluxes.size();
}
/*
* Explicit divide-and-conqure FFT by recursion
*/
void rfft(dcomplex *v, int n, dcomplex *tmp) {
if ( n <= 1 ) /* do nothing and return */
return;
// n > 1
const int m = n >> 1;
dcomplex *v1, *v0;
v0 = tmp;
v1 = tmp + m;
for (int i = 0; i < n / 2; i++) {
v0[i] = v[2 * i];
v1[i] = v[2 * i + 1];
}
rfft(v0, m, v); /* FFT on even-indexed elements of v[] */
rfft(v1, m, v); /* FFT on odd-indexed elements of v[] */
for (int i = 0; i < m; i++) {
// w = (cos(2 * PI * i / (double) n)) + (-sin(2 * PI * i / (double) n))*I;
dcomplex w = cexp(- I * /* 2 * */ PI * i / (double) m /* n */);
dcomplex z = w * v1[i];
v[i] = v0[i] + z;
v[i + m] = v0[i] - z;
}
return;
}
static void source_init(void){
int progval=0;
memcpy(lyd2,lyd,samps_per_frame*N*sizeof(float));
for (int ch=0; ch<samps_per_frame; ch++) {
GUI_aboveprogressbar(ch,samps_per_frame);
rfft(lyd+ch*N, N/2, INVERSE);
}
normalize_val=get_normalize_val();
//fprintf(stderr,"source_init finished\n");
//if(synthandsave_normalize_gain)
// normalize();
}
int main(void)
{
std::size_t i;
scitbx::fftpack::complex_to_complex<double> cfft(10);
std::vector<std::complex<double> > vc(cfft.n());
for(i=0;i<cfft.n();i++) {
vc[i] = std::complex<double>(2.*i, 2.*i+1.);
}
cfft.forward(vc.begin());
for(i=0;i<cfft.n();i++) {
std::cout << vc[i].real() << " " << vc[i].imag() << std::endl;
}
cfft.backward(vc.begin());
for(i=0;i<cfft.n();i++) {
std::cout << vc[i].real() << " " << vc[i].imag() << std::endl;
}
scitbx::fftpack::real_to_complex<double> rfft(10);
std::vector<double> vr(2 * rfft.n_complex());
for(i=0;i<rfft.n_real();i++) {
vr[i] = 1.*i;
}
rfft.forward(vr.begin());
for(i=0;i<2*rfft.n_complex();i++) {
std::cout << vr[i] << std::endl;
}
rfft.backward(vr.begin());
for(i=0;i<rfft.n_real();i++) {
std::cout << vr[i] << std::endl;
}
scitbx::fftpack::complex_to_complex_3d<double> cfft3d(2, 3, 5);
scitbx::af::versa<std::complex<double>, scitbx::af::c_grid<3> >
c3dmap(scitbx::af::c_grid<3>(cfft3d.n()));
cfft3d.forward(c3dmap.ref());
cfft3d.backward(c3dmap.ref());
scitbx::fftpack::real_to_complex_3d<double> rfft3d(3, 4, 5);
scitbx::af::versa<double, scitbx::af::c_grid<3> >
r3dmap(scitbx::af::c_grid<3>(rfft3d.m_real()));
rfft3d.forward(r3dmap.ref());
rfft3d.backward(r3dmap.ref());
#ifdef NEVER_DEFINED
#endif
return 0;
}
/******************************************************************************\
* Frequency offset acquisition *
\******************************************************************************/
_BOOLEAN CFreqOffsAcq::Run(const CVector<_REAL>& vecrInpData)
{
/* Init return flag */
_BOOLEAN bNewAcqResAvailable = FALSE;
/* Only do new acquisition if requested */
if (bAcquisition == TRUE)
{
/* Add new symbol in history (shift register) */
vecrFFTHistory.AddEnd(vecrInpData, iBlockSize);
if (iAquisitionCounter > 0)
{
/* Decrease counter */
iAquisitionCounter--;
}
else
{
/* Copy vector to matlib vector and calculate real-valued FFTW */
for (int i = 0; i < iTotalBufferSize; i++)
vecrFFTInput[i] = vecrFFTHistory[i];
/* Calculate power spectrum (X = real(F) ^ 2 + imag(F) ^ 2) */
vecrPSD = SqMag(rfft(vecrFFTInput, FftPlanAcq));
/* Calculate frequency from maximum peak in spectrum */
CReal rMaxPeak; /* Not needed, dummy */
int iIndMaxPeak;
Max(rMaxPeak, iIndMaxPeak /* out */,
vecrPSD(iSearchWinStart + 1, iSearchWinEnd));
/* Calculate estimated relative frequency offset */
rCurNormFreqOffset =
(CReal) (iIndMaxPeak + iSearchWinStart) / iHalfBuffer / 2;
/* Reset acquisition flag and Set return flag to show that new
result is available*/
bAcquisition = FALSE;
bNewAcqResAvailable = TRUE;
}
}
return bNewAcqResAvailable;
}
int frameCentroids(Frame &inFrame, int winsize, std::vector<float> ¢roids)
{
Frame window;
window.alloc_waveform(winsize);
int hoplen = (int)(((double)winsize) * hopsize);
// the number of windows including possibly fractional last window
int numwins = inFrame.wsize / hoplen + (inFrame.wsize % hoplen != 0);
int i;
for(i = 0; i < numwins; i++){
int j, winindex = i * hoplen;
if(i < numwins - 1)
for(j = 0; j < winsize; j++)
{
if( j + winindex < inFrame.wsize ) // bug since winsize is bigger than hoplen, last-but-one win may go out of bounds
window.waveform[j] = inFrame.waveform[j + winindex];
else
window.waveform[j] = 0;
}
else{
for(j = winindex; j < inFrame.wsize; j++)
window.waveform[j - winindex] = inFrame.waveform[j];
for(j = inFrame.wsize - winindex; j < winsize; j++)
window.waveform[j] = 0;
}
rfft(window.waveform, window.len, FFT_FORWARD);
window.cmp2pol();
float sumpow = 0.000001f;
centroids.push_back(0);
for(j = 0; j < window.len; j++){
centroids[i] += window.pol[j].mag * (float)(j + 1);
sumpow += window.pol[j].mag;
}
centroids[i] /= sumpow * (float)window.len;
if(centroids[i] != centroids[i])
centroids[i] = 0;
}
return centroids.size();
}
extern "C" void equ_quit(SuperEqState *state)
{
equ_free(state->lires1);
equ_free(state->lires2);
equ_free(state->irest);
equ_free(state->fsamples);
equ_free(state->finbuf);
equ_free(state->outbuf);
equ_free(state->ditherbuf);
state->lires1 = NULL;
state->lires2 = NULL;
state->irest = NULL;
state->fsamples = NULL;
state->finbuf = NULL;
state->outbuf = NULL;
rfft(0,0,NULL);
}
void rfft (long m0, long p0, long q0, long n)
/***** rfft
make a fft on x[m],x[m+q0],...,x[m+(p0-1)*q0] (p points).
one has xi_p0 = xi_n^n = zz[n] ; i.e., p0*n=nn.
*****/
{ long p,q,m,l;
long mh,ml;
int found=0;
complex sum,h;
if (p0==1) return;
if (test_key()==escape) { error=301; return; }
if (p0%2==0) { p=p0/2; q=2; }
else
{ q=3;
while (q*q<=p0)
{ if (p0%q==0)
{ found=1; break; }
q+=2;
}
if (found) p=p0/q;
else { q=p0; p=1; }
}
if (p>1) for (m=0; m<q; m++)
rfft((m0+m*q0)%nn,p,q*q0,nn/p);
mh=m0;
for (l=0; l<p0; l++)
{ ml=l%p;
c_copy(sum,ff[(m0+ml*q*q0)%nn]);
for (m=1; m<q; m++)
{ c_mult(ff[(m0+(m+ml*q)*q0)%nn],zz[(n*l*m)%nn],h);
c_add(sum,h,sum);
}
sum[0]/=q; sum[1]/=q;
c_copy(fh[mh],sum);
mh+=q0; if (mh>=nn) mh-=nn;
}
for (l=0; l<p0; l++)
{ c_copy(ff[m0],fh[m0]);
m0+=q0;
}
}
CRealVector CNoiseReduction::OptimalFilter(const CComplexVector& veccSigFreq,
const CRealVector& vecrSqMagSigFreq,
const CRealVector& vecrNoisePSD)
{
CRealVector vecrReturn(iBlockLenLong);
/* Calculate optimal filter coefficients in the frequency domain:
G_opt = max(1 - S_nn(n) / S_xx(n), 0) */
veccOptFilt = Max(Zeros(iFreqBlLen), Ones(iFreqBlLen) -
vecrNoisePSD / vecrSqMagSigFreq);
/* Constrain the optimal filter in time domain to avoid aliasing */
vecrOptFiltTime = rifft(veccOptFilt, FftPlan);
vecrOptFiltTime.Merge(vecrOptFiltTime(1, iBlockLen), Zeros(iBlockLen));
veccOptFilt = rfft(vecrOptFiltTime, FftPlan);
/* Actual filtering in frequency domain */
vecrReturn = rifft(veccSigFreq * veccOptFilt, FftPlan);
/* Cut out correct samples (to get from cyclic convolution to linear
convolution) */
return vecrReturn(iBlockLen + 1, iBlockLenLong);
}
void equ_quit(void)
{
free(lires1);
free(lires2);
free(rires1);
free(rires2);
free(irest);
free(fsamples);
free(inbuf);
free(outbuf);
free(ditherbuf);
lires1 = NULL;
lires2 = NULL;
rires1 = NULL;
rires2 = NULL;
irest = NULL;
fsamples = NULL;
inbuf = NULL;
outbuf = NULL;
ditherbuf = NULL;
rfft(0,0,NULL);
}
void split_real_imag_ok(void)
{
int i,ch;
char filename[200]={0},tmpfn[200]={0};
char extension[20]={0};
char *extp;
int nch,nchN;
GUI_aboveprogressbar(0,samps_per_frame*2);
for (i=0; i<samps_per_frame*N; i++) lyd2[i]=lyd[i];
for (ch=0; ch<2; ch++) {
for(nch=0;nch<samps_per_frame;nch++){
nchN=nch*N;
for (i=0; i<N/2; i++) {
if (ch%2) {
lyd[i+i+nchN]=0.; lyd[i+i+1+nchN]=lyd2[i+i+1+nchN];
} else {
lyd[i+i+nchN]=lyd2[i+i+nchN]; lyd[i+i+1+nchN]=0.;
}
}
}
/*og så må vi lagre da*/
extp=strrchr(playfile,'.');
if(extp>strrchr(playfile,'/')) {
strcpy(extension,++extp);
strncpy(tmpfn,playfile,(extp-playfile)-1);
sprintf(filename,"%s-%d.%s",tmpfn,ch,extension);
} else
sprintf(filename,"%s-%d",playfile,ch);
/*
out_AFsetup=afNewFileSetup();
afInitChannels(out_AFsetup, AF_DEFAULT_TRACK, samps_per_frame);
afInitRate(out_AFsetup, AF_DEFAULT_TRACK, R);
afInitCompression(out_AFsetup, AF_DEFAULT_TRACK, compression);
afInitFileFormat(out_AFsetup, filefmt);
afInitSampleFormat(out_AFsetup, AF_DEFAULT_TRACK, samp_type, bits_per_samp);
outfile=afOpenFile(filename, "wb", out_AFsetup);
*/
outfile=sf_open_write(filename,&loadstruct.sfinfo);
if (outfile==NULL) {
fprintf(stderr,"Could not open file \"%s\".\n",filename);
continue;
}
for(nch=0;nch<samps_per_frame;nch++){
GUI_aboveprogressbar(ch*samps_per_frame + nch,samps_per_frame*2);
nchN=nch*N;
rfft(lyd+nchN,N/2,INVERSE);
}
writesound(SaveWaveConsumer,outfile);
sf_close(outfile);
}
for (i=0; i<samps_per_frame*N; i++) lyd[i]=lyd2[i];
}
void makeWaves(
struct FFTSound *fftsound,
struct RSynthData *rsd,
void(*waveconsumer)(struct FFTSound *fftsound,void *pointer,double **samples,int num_samples),
bool (*progressupdate)(int pr,void *pointer),
void *pointer,
int start,int end,
bool obank,
bool keep_formants
)
{
double a0=0.0;
long point=0, point2=0;
int ch;
int on=(-fftsound->Nw*fftsound->I)/fftsound->Dn, in=-fftsound->Nw;
int j,i;
double coef[fftsound->numcoef2+2];
int keepform=0;
if(obank){
fprintf(stderr,"\n\nWarning! The result of the additiv resynthesis might be buggy. Please listen to the result to check that its okey or use the IFFT type.\n\n");
}
// init_again();
// rsd=getRSynthData(fftsound);
memset(coef,0.0,sizeof(double)*(fftsound->numcoef2+2));
if (keep_formants && fftsound->numcoef2!=0)
keepform=1;
for (j=start; j<end; j++) {
on+=fftsound->I; in+=fftsound->Dn;
for (ch=0; ch<fftsound->samps_per_frame; ch++) {
point =
ch*fftsound->horiz_num*fftsound->numchannels
+ j*fftsound->numchannels;
point2=ch*lpc_horiz_num*(fftsound->numcoef2+2)+j*(fftsound->numcoef2+2);
if(fftsound->usemem==false) pthread_mutex_lock(&fftsound->mutex);
for (i=0; i<fftsound->numchannels; i++) {
rsd->channel[i+i]=MEGAMP_GET(point+i); //ch*fftsound->horiz_num + i,j
rsd->channel[i+i+1]=MEGFREQ_GET(point+i);
}
if(fftsound->usemem==false) pthread_mutex_unlock(&fftsound->mutex);
// printf("j2: %d\n",j);
if (obank==true) {
if (j==start){
for (i=0; i<fftsound->N2+1; i++) {
rsd->lastfreq[ch][i]=rsd->channel[i+i+1];
rsd->lastamp[ch][i]=rsd->channel[i+i];
rsd->indx[ch][i]=0.;
}
}
if (j>=lpc_horiz_num) keepform=0;
if (keepform) {
for (i=0; i<fftsound->numcoef2+2; i++) {
coef[i]=lpc_coef[point2]; point2++;
}
a0=coef[fftsound->numcoef2+1];
}
oscbank(
fftsound,
rsd,rsd->channel,
fftsound->N2, fftsound->R, fftsound->I,
rsd->output[ch], ch,
fftsound->Nw,coef,
fftsound->numcoef2*keepform,a0
);
} else {
unconvert(rsd, rsd->channel, rsd->buffer, fftsound->N2, fftsound->I, fftsound->R, ch);
rfft(rsd->buffer, fftsound->N2, INVERSE);
overlapadd(rsd->buffer, fftsound->N, fftsound->Wsyn, rsd->output[ch], fftsound->Nw, on);
}
} // end for(ch=0; ch<fftsound->samps_per_frame; ch++)
if (obank==true){
shiftout(
fftsound,
waveconsumer,
pointer,
rsd->output,
fftsound->Nw,
fftsound->I,
in+fftsound->Nw2+fftsound->Dn
);
}else{
shiftout(
fftsound,
waveconsumer,
pointer,
rsd->output,
fftsound->Nw,
fftsound->I,
on
);
}
if(progressupdate!=NULL){
if((*progressupdate)(j,pointer)==false)break;
}
} // end for(j=start; j<end; j++)
// returnRSynthData(rsd);
}
const Field3D DDZ(const Field3D &f, CELL_LOC outloc, DIFF_METHOD method, bool inc_xbndry)
{
deriv_func func = fDDZ; // Set to default function
DiffLookup *table = FirstDerivTable;
CELL_LOC inloc = f.getLocation(); // Input location
CELL_LOC diffloc = inloc; // Location of differential result
if(StaggerGrids && (outloc == CELL_DEFAULT)) {
// Take care of CELL_DEFAULT case
outloc = diffloc; // No shift (i.e. same as no stagger case)
}
if(StaggerGrids && (outloc != inloc)) {
// Shifting to a new location
if(((inloc == CELL_CENTRE) && (outloc == CELL_ZLOW)) ||
((inloc == CELL_ZLOW) && (outloc == CELL_CENTRE))) {
// Shifting in Z. Centre -> Zlow, or Zlow -> Centre
func = sfDDZ; // Set default
table = FirstStagDerivTable; // Set table for others
diffloc = (inloc == CELL_CENTRE) ? CELL_ZLOW : CELL_CENTRE;
} else {
// A more complicated shift. Get a result at cell centre, then shift.
if(inloc == CELL_ZLOW) {
// Shifting
func = sfDDZ; // Set default
table = FirstStagDerivTable; // Set table for others
diffloc = CELL_CENTRE;
} else if(inloc != CELL_CENTRE) {
// Interpolate then (centre -> centre) then interpolate
return DDZ(interp_to(f, CELL_CENTRE), outloc, method);
}
}
}
if(method != DIFF_DEFAULT) {
// Lookup function
func = lookupFunc(table, method);
}
Field3D result;
if(func == NULL) {
// Use FFT
real shift = 0.; // Shifting result in Z?
if(StaggerGrids) {
if((inloc == CELL_CENTRE) && (diffloc == CELL_ZLOW)) {
// Shifting down - multiply by exp(-0.5*i*k*dz)
shift = -1.;
} else if((inloc == CELL_ZLOW) && (diffloc == CELL_CENTRE)) {
// Shifting up
shift = 1.;
}
}
result.Allocate(); // Make sure data allocated
static dcomplex *cv = (dcomplex*) NULL;
int jx, jy, jz;
real kwave;
real flt;
int xge = MXG, xlt = ngx-MXG;
if(inc_xbndry) { // Include x boundary region (for mixed XZ derivatives)
xge = 0;
xlt = ngx;
}
if(cv == (dcomplex*) NULL)
cv = new dcomplex[ncz/2 + 1];
for(jx=xge; jx<xlt; jx++) {
for(jy=0; jy<ngy; jy++) {
rfft(f[jx][jy], ncz, cv); // Forward FFT
for(jz=0; jz<=ncz/2; jz++) {
kwave=jz*2.0*PI/zlength; // wave number is 1/[rad]
if (jz>0.4*ncz) flt=1e-10;
else flt=1.0;
cv[jz] *= dcomplex(0.0, kwave) * flt;
if(StaggerGrids)
cv[jz] *= exp(Im * (shift * kwave * dz));
}
irfft(cv, ncz, result[jx][jy]); // Reverse FFT
result[jx][jy][ncz] = result[jx][jy][0];
}
}
#ifdef CHECK
// Mark boundaries as invalid
result.bndry_xin = result.bndry_xout = result.bndry_yup = result.bndry_ydown = false;
#endif
} else {
// All other (non-FFT) functions
result = applyZdiff(f, func, dz);
}
result.setLocation(diffloc);
return interp_to(result, outloc);
}
void wapp2fb(FILE *input, FILE *output) /* includefile */
{
FILE *bptr, *fpou, *alfa[2];
int pixel[2];
double pra, pdec;
double bw, bandwidth, scale, power, hweight, tsamp_us, crate, lmst;
double *lag, *sum, *acf, *window, jan1, days, epoch, ras,des,rahr,dede;
float zerolag,*block,smin,smax;
int doit,i,j,k,two_nlags,nlags,stat,rec_size,idump,swap_bytes,ifnum,opened;
int filesize,headersize,beam,utsecs,iymdf[4],rah,ram,ded,dem;
unsigned char *cblock, zuc;
unsigned short *sblock, zus;
unsigned int zul;
char message[80], outfile[80];
void *dump;
static float realtime=0.0;
#ifdef FFTW
fftw_plan fftplan;
#endif
/* establish whether we need to swap bytes (WAPP is little endian) */
swap_bytes=big_endian();
/* initialise correlator parameters used below */
nlags=nchans;
two_nlags=2*nlags;
bandwidth=foff*nlags;
tsamp_us=tsamp*1.0e6;
if (bandwidth<0.0) bandwidth *= -1.0;
#ifdef FFTW
acf = fftw_malloc(sizeof(double) * two_nlags);
lag = fftw_malloc(sizeof(double) * two_nlags);
/* set up fftw table and acf array when computing power spectra */
fftplan=fftw_plan_r2r_1d(two_nlags,acf,lag,FFTW_R2HC,FFTW_PATIENT);
#endif
#ifndef FFTW
/* set up acf array when computing power spectra */
acf = (double *) malloc(two_nlags * sizeof(double));
lag = (double *) malloc(two_nlags * sizeof(double));
#endif
if (compute_spectra) {
/* ranges for scaling spectra */
smin=0.0;smax=3.0;
} else {
/* ranges for scaling correlation functions */
smin=-0.5;smax=1.0;
}
/* set up the weights for windowing of ACF to monimize FFT leakage */
if (hanning) {
/* Hanning window */
hweight=0.50;
} else if (hamming) {
/* Hamming window */
hweight=0.54;
} else {
/* no window (default) */
hweight=1.00;
}
/* define the smoothing window to be applied base on the above weight */
window = (double *) malloc(nlags * sizeof(double));
for (j=0; j<nlags; j++) window[j]=(hweight+(1.0-hweight)*cos(PI*j/nlags));
/* work out number of IFs to loop over */
if (sumifs && (nifs>1)) {
smin*=2.0;
smax*=2.0;
ifnum=2;
} else {
sumifs=0;
ifnum=nifs;
}
/* calculate required record size for reading - i.e. number of bytes/dump */
rec_size = nifs*nlags*(nbits/8);
dump = malloc(rec_size); /* pointer to the correlator dump */
/* correlator data rate */
crate = 1.0/(tsamp_us-WAPP_DEAD_TIME);
/* scale factor to normalize correlation functions */
if (bandwidth < 50.0)
bw=50.0; /* correct scaling for narrow-band use */
else
bw=bandwidth;
scale = crate/bw;
if (wapp_level==9) scale/=16.0; /* 9-level sampling */
if (wapp_sum) scale/=2.0; /* summed IFs (search mode) */
scale*=pow(2.0,(double)wapp_lagtrunc); /* needed for truncation modes */
/* now define a number of working arrays to store lags and spectra */
block = (float *) malloc(nlags * sizeof(float));
cblock = (unsigned char *) malloc(nlags * sizeof(unsigned char));
sblock = (unsigned short *) malloc(nlags * sizeof(unsigned short));
/* if the file is ALFA data --- do the demultiplexing to two files */
if (wapp_isalfa) {
angle_split(src_raj,&rah,&ram,&ras);
rahr=(double)rah+(double)ram/60.0+(double)ras/3600.0;
angle_split(src_dej,&ded,&dem,&des);
if (ded>0)
dede=(double)ded+(double)dem/60.0+(double)des/3600.0;
else
dede=(double)ded-(double)dem/60.0-(double)des/3600.0;
/* calculate local sidereal time in hours */
lmst=slaGmst(tstart)*12.0/4.0/atan(1.0)-4.4502051459439667;
if (lmst<0.0) lmst+=24.0;
slaDjcal(5,tstart,iymdf,&stat);
slaCaldj(iymdf[0],1,1,&jan1,&stat);
days=tstart-jan1+1.0;
epoch=(double)iymdf[0]+days/365.25;
utsecs=86400*(tstart-floor(tstart));
pixel[0]=(wapp_number-1)*2;
pixel[1]=pixel[0]+1;
puts("opening output files for demultiplexed ALFA data...");
for (i=0; i<2; i++) {
if (alfa_raj[pixel[i]] == 0.0) {
alfa_position(rahr,dede,lmst,epoch,alfa_ang,0.0,0.0,pixel[i],&pra,&pdec);
src_raj=h2hms(pra);
src_dej=deg2dms(pdec);
} else {
src_raj=h2hms(alfa_raj[pixel[i]]);
src_dej=deg2dms(alfa_dej[pixel[i]]);
}
sprintf(outfile,"%s_%.0f_%05d_%04d_%s_%d.fil",
project,floor(tstart),utsecs,
scan_number,source_name,pixel[i]);
alfa[i]=open_file(outfile,"wb");
puts(outfile);
filterbank_header(alfa[i]);
}
beam=0;
}
if (headerfile) {
/* write output ASCII header file */
fpou=open_file("head","w");
fprintf(fpou,"Original WAPP file: %s\n",inpfile);
fprintf(fpou,"Sample time (us): %f\n",tsamp_us);
fprintf(fpou,"Observation time (s): %f\n",wapp_obstime);
fprintf(fpou,"Time stamp (MJD): %18.12f\n",tstart);
fprintf(fpou,"Number of samples/record: %d\n",512);
fprintf(fpou,"Center freq (MHz): %f\n",fch1+(float)nlags*foff/2.0);
fprintf(fpou,"Channel band (kHz): %f\n",bandwidth*1000.0/nlags);
fprintf(fpou,"Number of channels/record: %d\n",nlags);
fprintf(fpou,"Nifs: %d\n",ifnum);
fprintf(fpou,"RA (J2000): %f\n",src_raj);
fprintf(fpou,"DEC (J2000): %f\n",src_dej);
fprintf(fpou,"Gal l: %.4f\n",srcl);
fprintf(fpou,"Gal b: %.4f\n",srcb);
fprintf(fpou,"Name: %s\n",source_name);
fprintf(fpou,"Lagformat: %d\n",wapp_lagformat);
fprintf(fpou,"Sum: %d\n",wapp_sum);
fprintf(fpou,"Level: %d\n",wapp_level);
fprintf(fpou,"AZ at start: %f\n",az_start);
fprintf(fpou,"ZA at start: %f\n",za_start);
fprintf(fpou,"AST at start: %f\n",ast0);
fprintf(fpou,"LST at start: %f\n",lst0);
fprintf(fpou,"Project ID: %s\n",project);
fprintf(fpou,"Observers: %s\n",culprits);
filesize=sizeof_file(inpfile);
fprintf(fpou,"File size (bytes): %d\n",filesize);
headersize=wapp_header_size+wapp_incfile_length;
fprintf(fpou,"Data size (bytes): %d\n",filesize-headersize);
fprintf(fpou,"Number of samples: %d\n",nsamples(inpfile,headersize,nbits,nifs,nchans));
fclose(fpou);
}
/* initialise various counters and flags */
opened=idump=i=j=0;
/* main loop reading data from infile until no more left to read */
while( (stat=read(wapp_file,dump,rec_size)) == rec_size) {
/* calculate elapsed time and determine whether we process this record */
realtime += (float) tsamp;
if ( (doit=process(realtime,start_time,final_time)) == -1) break;
if (doit) {
/* set ALFA beam output if necessary */
if (wapp_isalfa) {
output=alfa[beam]; /* set output file for this loop */
beam=!(beam); /* flip file for next iteration */
}
/* clear zerolag and blocksum arrays */
zerolag=0.0;
for (j=0; j<nlags; j++) block[j]=0.0;
/* loop over the IFs */
for (i=0; i<ifnum; i++) {
if (ifstream[i]=='Y') {
if (zerolagdump) {
/* only interested in the zero lag term for each IF */
switch (nbits) {
case 8:
zuc = *(((unsigned char *)dump)+i*nlags);
zerolag+=zuc;
break;
case 16:
zus = *(((unsigned short *)dump)+i*nlags);
if (swap_bytes) swap_short(&zus);
zerolag+=zus;
break;
case 32:
zul = *(((unsigned int *)dump)+i*nlags);
if (swap_bytes) swap_int(&zul);
zerolag+=zul;
break;
}
/* write out the data checking IF number for summed mode */
if ( (sumifs && (i==1)) || (!sumifs) ) {
if (obits==32) {
if (swapout) swap_float(&zerolag);
fwrite(&zerolag,sizeof(float),1,output);
} else {
sprintf(message,"cannot write %d bits in zerolag mode",obits);
error_message(message);
}
}
} else {
/* fill lag array with scaled CFs */
for (j=0; j<nlags; j++) {
switch (nbits) {
case 8:
zuc = *(((unsigned char *)dump)+j+i*nlags);
lag[j] = scale * (double) zuc - 1.0;
break;
case 16:
zus = *(((unsigned short *)dump)+j+i*nlags);
if (swap_bytes) swap_short(&zus);
lag[j] = scale * (double) zus - 1.0;
break;
case 32:
zul = *(((unsigned int *)dump)+j+i*nlags);
if (swap_bytes) swap_int(&zul);
lag[j] = scale * (double) zul - 1.0;
break;
}
}
/* calculate power and correct for finite level quantization */
power = inv_cerf(lag[0]);
power = 0.1872721836/power/power;
if (i<2) {
if (do_vanvleck) {
if (wapp_level==3) {
/* apply standard 3-level van vleck correction */
vanvleck3lev(lag,nlags);
} else if (wapp_level==9) {
/* apply 9-level van vleck correction */
vanvleck9lev(lag,nlags);
}
}
}
if (compute_spectra) {
/* form windowed even ACF in array */
for(j=1; j<nlags; j++) {
acf[j]=window[j]*lag[j]*power;
acf[two_nlags-j]=acf[j];
}
acf[nlags]=0.0;
acf[0]=lag[0]*power;
/* FFT the ACF (which is real and even) -> real and even FFT */
#ifdef FFTW
fftw_execute(fftplan);
#endif
#ifndef FFTW
rfft(two_nlags,acf,lag);
#endif
/* if the band needs to be flipped --- do it here */
if (wapp_flip) {
/* use acf as temporary array */
for (j=0;j<nlags;j++) acf[j]=lag[j];
k=nlags-1;
for (j=0;j<nlags;j++) {
lag[k]=acf[j];
k--;
}
}
/* add lags to block array */
for (j=0; j<nlags; j++) block[j]+=lag[j];
} else {
/* just copy correlation functions into block */
for (j=0; j<nlags; j++) block[j]=lag[j];
}
/* write out data block checking IF number for summed mode */
if ( (sumifs && (i==1)) || (!sumifs) ) {
if (obits==32) {
if (swapout) for (j=0; j<nlags; j++) swap_float(&block[j]);
fwrite(block,sizeof(float),nlags,output);
} else if (obits==16) {
float2short(block,nlags,smin,smax,sblock);
if (swapout) for (j=0; j<nlags; j++) swap_short(&sblock[j]);
fwrite(sblock,sizeof(unsigned short),nlags,output);
} else if (obits==8) {
float2char(block,nlags,smin,smax,cblock);
fwrite(cblock,sizeof(unsigned char),nlags,output);
} else if (obits==4) {
float2four(block,nlags,smin,smax,cblock);
fwrite(cblock,sizeof(unsigned char),nlags/2,output);
} else {
sprintf(message,"cannot write %d bits in wapp2fb",obits);
error_message(message);
}
}
} /* end of zerolagdump if */
if (!sumifs) { /* reset block and zerolag if not summing */
zerolag=0.0;
for (j=0; j<nlags; j++) block[j]=0.0;
}
} /* end of IFstream if */
} /* end of loop over IFs */
} /* end of processing if */
/* increment dump counter and update logfile every 512 dumps */
idump++;
if (idump%512 == 0) {
if (!opened) {
/* open up logfile */
open_log("filterbank.monitor");
opened=1;
}
sprintf(message,"time:%.1fs",realtime);
update_log(message);
}
} /* end of main read loop*/
/* job done - free up remaining arrays */
free(dump);free(block);free(sblock);free(cblock);free(window);
#ifdef FFTW
fftw_destroy_plan(fftplan);
fftw_free(acf); fftw_free(lag);
#endif
#ifndef FFTW
free(acf); free(lag);
#endif
}
static void PsyBufferUpdate( FFT_Tables *fft_tables, GlobalPsyInfo * gpsyInfo, PsyInfo * psyInfo,
double *newSamples, unsigned int bandwidth,
int *cb_width_short, int num_cb_short)
{
int win;
double transBuff[2 * BLOCK_LEN_LONG];
double transBuffS[2 * BLOCK_LEN_SHORT];
psydata_t *psydata = psyInfo->data;
psyfloat *tmp;
int sfb;
psydata->bandS = psyInfo->sizeS * bandwidth * 2 / gpsyInfo->sampleRate;
memcpy(transBuff, psyInfo->prevSamples, psyInfo->size * sizeof(double));
memcpy(transBuff + psyInfo->size, newSamples, psyInfo->size * sizeof(double));
for (win = 0; win < 8; win++)
{
int first = 0;
int last = 0;
memcpy(transBuffS, transBuff + (win * 128) + (512 - 64),
2 * psyInfo->sizeS * sizeof(double));
Hann(gpsyInfo, transBuffS, 2 * psyInfo->sizeS);
rfft( fft_tables, transBuffS, 8);
// shift bufs
tmp = psydata->fftEnrgPrevS[win];
psydata->fftEnrgPrevS[win] = psydata->fftEnrgS[win];
psydata->fftEnrgS[win] = psydata->fftEnrgNextS[win];
psydata->fftEnrgNextS[win] = psydata->fftEnrgNext2S[win];
psydata->fftEnrgNext2S[win] = tmp;
for (sfb = 0; sfb < num_cb_short; sfb++)
{
double e;
int l;
first = last;
last = first + cb_width_short[sfb];
if (first < 1)
first = 1;
//if (last > psydata->bandS) // band out of range
if (first >= psydata->bandS) // band out of range
break;
e = 0.0;
for (l = first; l < last; l++)
{
double a = transBuffS[l];
double b = transBuffS[l + psyInfo->sizeS];
e += a * a + b * b;
}
psydata->fftEnrgNext2S[win][sfb] = e;
}
psydata->lastband = sfb;
for (; sfb < num_cb_short; sfb++)
{
psydata->fftEnrgNext2S[win][sfb] = 0;
}
}
memcpy(psyInfo->prevSamples, newSamples, psyInfo->size * sizeof(double));
}
void combsplit_ok(void)
{
int i,ch;
int div,num;
char filename[200]={0},tmpfn[200]={0};
char extension[20]={0};
char *extp;
int nch,nchN;
GUI_aboveprogressbar(0,samps_per_frame*num);
div=combsplit_block_size;
num=combsplit_number_of_files;
for (i=0; i<samps_per_frame*N; i++) lyd2[i]=lyd[i];
/* rett kanal : (i/div)%num==kanalnr */
for (ch=0; ch<num; ch++) {
printf("ch: %d\n",ch);
for(nch=0;nch<samps_per_frame;nch++){
nchN=nch*N;
for (i=0; i<N/2; i++) {
if ( ((i/div)%num)==ch) {
lyd[i+i+nchN]=lyd2[i+i+nchN]; lyd[i+i+1+nchN]=lyd2[i+i+1+nchN];
} else {
lyd[i+i+nchN]=0.; lyd[i+i+1+nchN]=0.;
}
}
}
/*og så må vi lagre da*/
extp=strrchr(playfile,'.');
if(extp>strrchr(playfile,'/')) {
strcpy(extension,++extp);
strncpy(tmpfn,playfile,(extp-playfile)-1);
// tmpfn[extp-playfile-1]=0;
sprintf(filename,"%s-%d.%s",tmpfn,ch,extension);
}else{
sprintf(filename,"%s-%d",playfile,ch);
}
/*
out_AFsetup=afNewFileSetup();
afInitChannels(out_AFsetup, AF_DEFAULT_TRACK, samps_per_frame);
afInitRate(out_AFsetup, AF_DEFAULT_TRACK, R);
afInitCompression(out_AFsetup, AF_DEFAULT_TRACK, compression);
afInitFileFormat(out_AFsetup, filefmt);
afInitSampleFormat(out_AFsetup, AF_DEFAULT_TRACK, samp_type, bits_per_samp);
outfile=afOpenFile(filename, "wb", out_AFsetup);
*/
outfile=sf_open_write(filename,&loadstruct.sfinfo);
if (outfile==NULL) {
fprintf(stderr,"Could not open file. \"%s\".\n",filename);
continue;
}
for(nch=0;nch<samps_per_frame;nch++){
GUI_aboveprogressbar(ch*samps_per_frame + nch,samps_per_frame*num);
nchN=nch*N;
rfft(lyd+nchN,N/2,INVERSE);
}
writesound(SaveWaveConsumer,outfile);
// afCloseFile(outfile);
sf_close(outfile);
}
for (i=0; i<samps_per_frame*N; i++) lyd[i]=lyd2[i];
}
//-----------------------------------------------------------------------------
// name: fft_bandpass()
// desc: bandpass filter the samples x - pass through frequencies between
// bandStart * nyquist and bandEnd * nyquist unharmed, smoothly sloping down
// the amplitudes of frequencies outside the passband to 0 over
// rolloff * nyquist hz.
//-----------------------------------------------------------------------------
void fft_bandpass( Frame * x, float bandStart, float bandEnd, float rolloff )
{
float *window = 0;
int winsize = -1;
int i;
// not sure why we have at least 4 "pi" variables floating around
// (#define PIE, #define PI, float PI earlier in this file, double pi local to functions)
// got to combine them all some time.
// double pi = 4.*atan(1.0);
// make and apply hanning window
if( winsize != x->wlen ){
winsize = x->wlen;
if(window)
delete[] window;
window = new float[winsize];
hanning( window, winsize );
}
apply_window( x->waveform, window, x->wlen );
// fft
rfft( x->waveform, x->len, FFT_FORWARD );
BirdBrain::scale_fft( x->waveform, x->wlen, x->wlen, x->wsize );
x->cmp2pol();
// the core
int rollWidth = (int)(rolloff * (float)x->len);
int startIndex = (int)(bandStart * (float)x->len);
int endIndex = (int)(bandEnd * (float)x->len);
int lTail = startIndex - rollWidth;
int rTail = endIndex + rollWidth;
for(i = 0; i < lTail; i++){
x->pol[i].mag = 0;
}
for(i = lTail > 0 ? lTail : 0; i < startIndex; i++){
x->pol[i].mag *= 0.5 *
(1 + cos(((float)(startIndex - i)) * PIE / (float)rollWidth));
}
for(i = endIndex; (i < rTail) && (i < x->len); i++){
x->pol[i].mag *= 0.5 *
(1 + cos(((float)(i - endIndex)) * PIE / (float)rollWidth));
}
for(i = rTail; i < x->len; i++){
x->pol[i].mag = 0;
}
x->pol2cmp();
// inverse fft
rfft( x->waveform, x->len, FFT_INVERSE);
// inverse window!
hanning( window, winsize );
for(i = 0; i < winsize; i++){
if(window[i] < 0.15)
window[i] = 0.15f;
window[i] = 1 / window[i];
}
apply_window( x->waveform, window, x->wlen );
if( window )
delete[] window;
}
extern "C" int equ_modifySamples_float (SuperEqState *state, char *buf,int nsamples,int nch)
{
int i,p,ch;
REAL *ires;
float amax = 1.0f;
float amin = -1.0f;
static float hm1 = 0, hm2 = 0;
if (state->chg_ires) {
state->cur_ires = state->chg_ires;
state->lires = state->cur_ires == 1 ? state->lires1 : state->lires2;
state->chg_ires = 0;
}
p = 0;
while(state->nbufsamples+nsamples >= state->winlen)
{
for(i=0;i<(state->winlen-state->nbufsamples)*nch;i++)
{
state->finbuf[state->nbufsamples*nch+i] = ((float *)buf)[i+p*nch];
float s = state->outbuf[state->nbufsamples*nch+i];
//if (dither) s += ditherbuf[(ditherptr++) & (DITHERLEN-1)];
if (s < amin) s = amin;
if (amax < s) s = amax;
((float *)buf)[i+p*nch] = s;
}
for(i=state->winlen*nch;i<state->tabsize*nch;i++)
state->outbuf[i-state->winlen*nch] = state->outbuf[i];
p += state->winlen-state->nbufsamples;
nsamples -= state->winlen-state->nbufsamples;
state->nbufsamples = 0;
for(ch=0;ch<nch;ch++)
{
ires = state->lires + ch * state->tabsize;
for(i=0;i<state->winlen;i++)
state->fsamples[i] = state->finbuf[nch*i+ch];
for(i=state->winlen;i<state->tabsize;i++)
state->fsamples[i] = 0;
if (state->enable) {
rfft(state->fft_bits,1,state->fsamples);
state->fsamples[0] = ires[0]*state->fsamples[0];
state->fsamples[1] = ires[1]*state->fsamples[1];
for(i=1;i<state->tabsize/2;i++)
{
REAL re,im;
re = ires[i*2 ]*state->fsamples[i*2] - ires[i*2+1]*state->fsamples[i*2+1];
im = ires[i*2+1]*state->fsamples[i*2] + ires[i*2 ]*state->fsamples[i*2+1];
state->fsamples[i*2 ] = re;
state->fsamples[i*2+1] = im;
}
rfft(state->fft_bits,-1,state->fsamples);
} else {
for(i=state->winlen-1+state->winlen/2;i>=state->winlen/2;i--) state->fsamples[i] = state->fsamples[i-state->winlen/2]*state->tabsize/2;
for(;i>=0;i--) state->fsamples[i] = 0;
}
for(i=0;i<state->winlen;i++) state->outbuf[i*nch+ch] += state->fsamples[i]/state->tabsize*2;
for(i=state->winlen;i<state->tabsize;i++) state->outbuf[i*nch+ch] = state->fsamples[i]/state->tabsize*2;
}
}
for(i=0;i<nsamples*nch;i++)
{
state->finbuf[state->nbufsamples*nch+i] = ((float *)buf)[i+p*nch];
float s = state->outbuf[state->nbufsamples*nch+i];
if (state->dither) {
float u;
s -= hm1;
u = s;
// s += ditherbuf[(ditherptr++) & (DITHERLEN-1)];
if (s < amin) s = amin;
if (amax < s) s = amax;
hm1 = s - u;
((float *)buf)[i+p*nch] = s;
} else {
if (s < amin) s = amin;
if (amax < s) s = amax;
((float *)buf)[i+p*nch] = s;
}
}
p += nsamples;
state->nbufsamples += nsamples;
return p;
}
int equ_modifySamples(char *buf,int nsamples,int nch,int bps)
{
int i,p,ch;
REAL *ires;
int amax = (1 << (bps-1))-1;
int amin = -(1 << (bps-1));
static float hm1 = 0;
if (chg_ires) {
cur_ires = chg_ires;
lires = cur_ires == 1 ? lires1 : lires2;
rires = cur_ires == 1 ? rires1 : rires2;
chg_ires = 0;
}
p = 0;
while(nbufsamples+nsamples >= winlen)
{
switch(bps)
{
case 8:
for(i=0;i<(winlen-nbufsamples)*nch;i++)
{
inbuf[nbufsamples*nch+i] = ((unsigned char *)buf)[i+p*nch] - 0x80;
float s = outbuf[nbufsamples*nch+i];
if (dither) {
float u;
s -= hm1;
u = s;
s += ditherbuf[(ditherptr++) & (DITHERLEN-1)];
if (s < amin) s = amin;
if (amax < s) s = amax;
s = RINT(s);
hm1 = s - u;
((unsigned char *)buf)[i+p*nch] = (unsigned char)(s + 0x80);
} else {
if (s < amin) s = amin;
if (amax < s) s = amax;
((unsigned char *)buf)[i+p*nch] = (unsigned char) (RINT(s) + 0x80);
}
}
for(i=winlen*nch;i<tabsize*nch;i++)
outbuf[i-winlen*nch] = outbuf[i];
break;
case 16:
for(i=0;i<(winlen-nbufsamples)*nch;i++)
{
inbuf[nbufsamples*nch+i] = ((short *)buf)[i+p*nch];
float s = outbuf[nbufsamples*nch+i];
if (dither) {
float u;
s -= hm1;
u = s;
s += ditherbuf[(ditherptr++) & (DITHERLEN-1)];
if (s < amin) s = amin;
if (amax < s) s = amax;
s = RINT(s);
hm1 = s - u;
((short *)buf)[i+p*nch] = (short)s;
} else {
if (s < amin) s = amin;
if (amax < s) s = amax;
((short *)buf)[i+p*nch] = RINT(s);
}
}
for(i=winlen*nch;i<tabsize*nch;i++)
outbuf[i-winlen*nch] = outbuf[i];
break;
case 24:
for(i=0;i<(winlen-nbufsamples)*nch;i++)
{
((int *)inbuf)[nbufsamples*nch+i] =
(((unsigned char *)buf)[(i+p*nch)*3 ] ) +
(((unsigned char *)buf)[(i+p*nch)*3+1] << 8) +
((( signed char *)buf)[(i+p*nch)*3+2] << 16) ;
float s = outbuf[nbufsamples*nch+i];
//if (dither) s += ditherbuf[(ditherptr++) & (DITHERLEN-1)];
if (s < amin) s = amin;
if (amax < s) s = amax;
int s2 = RINT(s);
((signed char *)buf)[(i+p*nch)*3 ] = s2 & 255; s2 >>= 8;
((signed char *)buf)[(i+p*nch)*3+1] = s2 & 255; s2 >>= 8;
((signed char *)buf)[(i+p*nch)*3+2] = s2 & 255;
}
for(i=winlen*nch;i<tabsize*nch;i++)
outbuf[i-winlen*nch] = outbuf[i];
break;
default:
assert(0);
}
p += winlen-nbufsamples;
nsamples -= winlen-nbufsamples;
nbufsamples = 0;
for(ch=0;ch<nch;ch++)
{
ires = ch == 0 ? lires : rires;
if (bps == 24) {
for(i=0;i<winlen;i++)
fsamples[i] = ((int *)inbuf)[nch*i+ch];
} else {
for(i=0;i<winlen;i++)
fsamples[i] = inbuf[nch*i+ch];
}
for(i=winlen;i<tabsize;i++)
fsamples[i] = 0;
if (enable) {
rfft(tabsize,1,fsamples);
fsamples[0] = ires[0]*fsamples[0];
fsamples[1] = ires[1]*fsamples[1];
for(i=1;i<tabsize/2;i++)
{
REAL re,im;
re = ires[i*2 ]*fsamples[i*2] - ires[i*2+1]*fsamples[i*2+1];
im = ires[i*2+1]*fsamples[i*2] + ires[i*2 ]*fsamples[i*2+1];
fsamples[i*2 ] = re;
fsamples[i*2+1] = im;
}
rfft(tabsize,-1,fsamples);
} else {
for(i=winlen-1+winlen/2;i>=winlen/2;i--) fsamples[i] = fsamples[i-winlen/2]*tabsize/2;
for(;i>=0;i--) fsamples[i] = 0;
}
for(i=0;i<winlen;i++) outbuf[i*nch+ch] += fsamples[i]/tabsize*2;
for(i=winlen;i<tabsize;i++) outbuf[i*nch+ch] = fsamples[i]/tabsize*2;
}
}
switch(bps)
{
case 8:
for(i=0;i<nsamples*nch;i++)
{
inbuf[nbufsamples*nch+i] = ((unsigned char *)buf)[i+p*nch] - 0x80;
float s = outbuf[nbufsamples*nch+i];
if (dither) {
float u;
s -= hm1;
u = s;
s += ditherbuf[(ditherptr++) & (DITHERLEN-1)];
if (s < amin) s = amin;
if (amax < s) s = amax;
s = RINT(s);
hm1 = s - u;
((unsigned char *)buf)[i+p*nch] = (unsigned char)(s + 0x80);
} else {
if (s < amin) s = amin;
if (amax < s) s = amax;
((unsigned char *)buf)[i+p*nch] = RINT(s) + 0x80;
}
}
break;
case 16:
for(i=0;i<nsamples*nch;i++)
{
inbuf[nbufsamples*nch+i] = ((short *)buf)[i+p*nch];
float s = outbuf[nbufsamples*nch+i];
if (dither) {
float u;
s -= hm1;
u = s;
s += ditherbuf[(ditherptr++) & (DITHERLEN-1)];
if (s < amin) s = amin;
if (amax < s) s = amax;
s = RINT(s);
hm1 = s - u;
((short *)buf)[i+p*nch] = (short)s;
} else {
if (s < amin) s = amin;
if (amax < s) s = amax;
((short *)buf)[i+p*nch] = RINT(s);
}
}
break;
case 24:
for(i=0;i<nsamples*nch;i++)
{
((int *)inbuf)[nbufsamples*nch+i] =
(((unsigned char *)buf)[(i+p*nch)*3 ] ) +
(((unsigned char *)buf)[(i+p*nch)*3+1] << 8) +
((( signed char *)buf)[(i+p*nch)*3+2] << 16) ;
float s = outbuf[nbufsamples*nch+i];
//if (dither) s += ditherbuf[(ditherptr++) & (DITHERLEN-1)];
if (s < amin) s = amin;
if (amax < s) s = amax;
int s2 = RINT(s);
((signed char *)buf)[(i+p*nch)*3 ] = s2 & 255; s2 >>= 8;
((signed char *)buf)[(i+p*nch)*3+1] = s2 & 255; s2 >>= 8;
((signed char *)buf)[(i+p*nch)*3+2] = s2 & 255;
}
break;
default:
assert(0);
}
p += nsamples;
nbufsamples += nsamples;
return p;
}
//-----------------------------------------------------------------------------
// Name: displayFunc( )
// Desc: callback function invoked to draw the client area
//-----------------------------------------------------------------------------
void displayFunc( )
{
static const int LP = 4;
static long int count = 0;
static char str[1024];
static unsigned int wf = 0;
static SAMPLE buffer[LPC_BUFFER_SIZE], residue[LPC_BUFFER_SIZE], coefs[1024],
coefs_dct[1024], noise[LPC_BUFFER_SIZE];
float pitch, power, fval;
int i;
// clear the color and depth buffers
glClear( GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT );
glPushMatrix( );
// rotate the sphere about y axis
glRotatef( g_angle_y += g_inc, 0.0f, 1.0f, 0.0f );
// color waveform
glColor3f( 0.4f, 0.4f, 1.0f );
// wait for data
while( !g_ready )
#if !defined(__OS_WINDOWS__)
usleep( 0 );
#else
Sleep( 0 );
#endif
// g_mutex.lock();
// get the window
//memcpy( buffer, g_audio_buffer, g_buffer_size * sizeof(SAMPLE) );
//memset( g_another_buffer, 0, g_buffer_size * sizeof(SAMPLE) );
// g_mutex.unlock();
sf_readf_float( g_fin, buffer, LPC_BUFFER_SIZE );
memcpy( g_another_buffer, buffer, LPC_BUFFER_SIZE * sizeof(SAMPLE) );
//apply_window( (float*)buffer, g_window, g_buffer_size );
lpc_analyze( g_lpc, buffer, g_buffer_size, coefs, 40, &power, &pitch, residue );
if( !g_ctf )
{
lpc_synthesize( g_lpc, g_another_buffer, g_buffer_size, coefs, 40, power, 0 );
}
else
{
// inverse window
// dct
dct( residue, LPC_BUFFER_SIZE );
lpc_analyze( g_lpc_freq, residue, g_buffer_size, coefs_dct, 10, &power, &pitch, NULL );
for( i = 0; i < LPC_BUFFER_SIZE; i++ )
{
noise[i] = 2.0f * (float)rand() / RAND_MAX - 1.0f;
noise[i] *= power * 32.0f;
}
// dct
dct( noise, LPC_BUFFER_SIZE );
lpc_synthesize( g_lpc_freq, residue, g_buffer_size, coefs_dct, 10, power, pitch, noise );
// idct
idct( residue, LPC_BUFFER_SIZE );
// window?
//apply_window( (float *)residue, g_window, g_buffer_size );
lpc_synthesize( g_lpc, g_another_buffer, g_buffer_size, coefs, 40, power, pitch, residue );
memset(residue,0,LPC_BUFFER_SIZE*sizeof(SAMPLE));
}
g_ready = FALSE;
// apply the window
GLfloat x = -1.8f, inc = 3.6f / g_buffer_size, y = 1.0f;
// draw the time domain waveform
glBegin( GL_LINE_STRIP );
GLint ii = ( g_buffer_size - (g_buffer_size/g_time_view) ) / 2;
for( i = ii; i < ii + g_buffer_size / g_time_view; i++ )
{
glVertex3f( x, g_gain * g_time_scale * .75f * buffer[i] + y, 0.0f );
x += inc * g_time_view;
}
glEnd();
// apply the window
x = -1.8f, inc = 3.6f / g_buffer_size, y = .5f;
glColor3f( 0.4f, 0.8f, 1.0f );
// draw the prediction
glBegin( GL_LINE_STRIP );
for( i = ii; i < ii + g_buffer_size / g_time_view; i++ )
{
glVertex3f( x, g_gain * g_time_scale * .75f * (buffer[i]-residue[i]) + y, 0.0f );
x += inc * g_time_view;
}
glEnd();
// apply the window
x = -1.8f, inc = 3.6f / g_buffer_size, y = .0f;
// draw the residue
glColor3f( 0.8f, 0.8f, 0.4f );
glBegin( GL_LINE_STRIP );
for( i = ii; i < ii + g_buffer_size / g_time_view; i++ )
{
glVertex3f( x, g_gain * g_time_scale * 5.0f * residue[i] + y, 0.0f );
x += inc * g_time_view;
}
glEnd();
// apply the window
x = -1.8f, inc = 0.3f/g_order, y = -0.5f;
// draw the coefficients
if( pitch == 0 )
glColor3f( 1.0f, .4f, .4f );
else
glColor3f( 1.0f, 1.2f - pitch/g_buffer_size*4.0f, .4f );
glBegin( GL_LINE_STRIP );
for( i = 0; i < g_order; i++ )
{
glVertex3f( x, coefs[i] + y, 0.0f );
x += inc * g_time_view;
}
glEnd();
// fft
memcpy( residue, g_another_buffer, LPC_BUFFER_SIZE * sizeof(SAMPLE) );
rfft( (float *)residue, g_buffer_size/2, FFT_FORWARD );
memcpy( buffer, residue, LPC_BUFFER_SIZE * sizeof(SAMPLE) );
x = -1.8f;
y = -1.2f;
inc = 3.6f / g_buffer_size;
complex * cbuf = (complex *)buffer;
// color the spectrum
glColor3f( 0.4f, 1.0f, 0.4f );
// draw the frequency domain representation
glBegin( GL_LINE_STRIP );
for( i = 0; i < g_buffer_size/g_freq_view; i++ )
{
g_spectrums[wf][i].x = x;
if( !g_usedb )
g_spectrums[wf][i].y = g_gain * g_freq_scale * .7f *
::pow( 25 * cmp_abs( cbuf[i] ), .5 ) + y;
else
g_spectrums[wf][i].y = g_gain * g_freq_scale * .8f *
( 20.0f * log10( cmp_abs(cbuf[i])/8.0 ) + 80.0f ) / 80.0f + y + .73f;
x += inc * g_freq_view;
}
glEnd();
g_draw[wf] = true;
for( i = 0; i < g_depth; i++ )
{
if( g_draw[(wf+i)%g_depth] )
{
Pt2D * pt = g_spectrums[(wf+i)%g_depth];
fval = (g_depth-i)/(float)g_depth;
glColor3f( .4f * fval, 1.0f * fval, .4f * fval );
x = -1.8f;
glBegin( GL_LINE_STRIP );
for( int j = 0; j < g_buffer_size/g_freq_view; j++, pt++ )
glVertex3f( x + j*(inc*g_freq_view), pt->y, -i * g_space + g_z );
glEnd();
}
}
if( !g_wutrfall )
g_draw[wf] = false;
wf--;
wf %= g_depth;
/*
for( int i = 0; i < 9; i++ )
fprintf( stderr, "%.4f ", coefs[i] );
fprintf( stderr, "power: %.8f pitch: %.4f\n", power, pitch );
for( int i = 0; i < 9; i++ )
fprintf( stderr, "%.4f ", lpc(i) );
fprintf( stderr, "power: %.8f pitch: %.4f\n", lpc(10), lpc(9) );
fprintf( stderr, "\n" );
*/
draw_string( 0.4f, 0.8f, 0.0f, "original", .5f );
draw_string( 0.4f, 0.3f, 0.0f, "predicted", .5f );
draw_string( 0.4f, -.2f, 0.0f, "residue (error)", .5f );
draw_string( -1.8f, -.7f, 0.0f, "coefficients", .5f );
sprintf( str, "pitch factor: %.4f", g_speed );
glColor3f( 0.8f, .4f, .4f );
draw_string( 1.2f, -.25f, 0.0f, str, .35f );
sprintf( str, "LPC order: %i", g_order );
draw_string( 1.2f, -.35f, 0.0f, str, .35f );
SAMPLE sum = 0.0f;
for( i = 0; i < g_buffer_size; i++ )
sum += fabs(buffer[i]);
glPushMatrix();
if( pitch == 0 )
{
glColor3f( 1.0f, .4f, .4f );
glTranslatef( 1.28f, -.45f, 0.0f );
}
else
{
glColor3f( 1.0f, 1.2f - pitch/g_buffer_size*4.0f, .4f );
glTranslatef( 1.55f, -.45f, 0.0f );
}
glutSolidSphere( .001f + sum/g_buffer_size * 120.0f, 10, 10 );
glPopMatrix();
draw_string( 1.2f, -.55f, 0.0f, "non-pitch | pitched", .35f );
draw_string( 1.2f, -.65f, 0.0f, g_ctf ? "both" : "time-only", .35f );
glPopMatrix( );
// swap the buffers
glFlush( );
glutSwapBuffers( );
g_buffer_count_b++;
}
void CAMDemodulation::SetBPFilter(const CReal rNewBPNormBW,
const CReal rNewNormFreqOffset,
const EDemodType eDemodType)
{
/* Set internal parameter */
rBPNormBW = rNewBPNormBW;
/* Actual prototype filter design */
CRealVector vecrFilter(iHilFiltBlLen);
vecrFilter = FirLP(rBPNormBW, Nuttallwin(iHilFiltBlLen));
/* Adjust center of filter for respective demodulation types */
CReal rBPNormFreqOffset;
const CReal rSSBMargin = SSB_DC_MARGIN_HZ / SOUNDCRD_SAMPLE_RATE;
switch (eDemodType)
{
case DT_AM:
case DT_FM:
/* No offset */
rBPNormFreqOffset = (CReal) 0.0;
break;
case DT_LSB:
/* Shift filter to the left side of the carrier. Add a small offset
to the filter bandwidht because of the filter slope */
rBPNormFreqOffset = - rBPNormBW / 2 - rSSBMargin;
break;
case DT_USB:
/* Shift filter to the right side of the carrier. Add a small offset
to the filter bandwidht because of the filter slope */
rBPNormFreqOffset = rBPNormBW / 2 + rSSBMargin;
break;
case DT_CW:
/* Shift filter to the right side of the carrier according to the
special CW demodulation shift */
rBPNormFreqOffset = FREQ_OFFS_CW_DEMOD / SOUNDCRD_SAMPLE_RATE;
break;
}
/* Actual band-pass filter offset is the demodulation frequency plus the
additional offset for the demodulation type */
rBPNormCentOffsTot = rNewNormFreqOffset + rBPNormFreqOffset;
/* Set filter coefficients ---------------------------------------------- */
/* Make sure that the phase in the middle of the filter is always the same
to avaoid clicks when the filter coefficients are changed */
const CReal rStartPhase = (CReal) iHilFiltBlLen * crPi * rBPNormCentOffsTot;
/* Copy actual filter coefficients. It is important to initialize the
vectors with zeros because we also do a zero-padding */
CRealVector rvecBReal(2 * iHilFiltBlLen, (CReal) 0.0);
CRealVector rvecBImag(2 * iHilFiltBlLen, (CReal) 0.0);
for (int i = 0; i < iHilFiltBlLen; i++)
{
rvecBReal[i] = vecrFilter[i] *
Cos((CReal) 2.0 * crPi * rBPNormCentOffsTot * i - rStartPhase);
rvecBImag[i] = vecrFilter[i] *
Sin((CReal) 2.0 * crPi * rBPNormCentOffsTot * i - rStartPhase);
}
/* Transformation in frequency domain for fft filter */
cvecBReal = rfft(rvecBReal, FftPlansHilFilt);
cvecBImag = rfft(rvecBImag, FftPlansHilFilt);
/* Set filter coefficients for AM filter after demodulation (use same low-
pass design as for the bandpass filter) */
CRealVector rvecBAMAfterDem(2 * iHilFiltBlLen, (CReal) 0.0);
rvecBAMAfterDem.PutIn(1, iHilFiltBlLen, vecrFilter);
cvecBAMAfterDem = rfft(rvecBAMAfterDem, FftPlansHilFilt);
}
void MFCC::Process(char *_block, int _block_size, std::vector<float *> &cep_coeffs)
{
bool must_swap = (swap_specified || native_byte_order == IEEE_LE);
int block_size = _block_size / 2;
float *block = new float[block_size];
for (int i = 0; i < block_size; ++i) {
unsigned char a = _block[i * 2];
unsigned char b = _block[i * 2 + 1];
short s;
if (must_swap) {
s = (a) | (b << 8);
} else {
s = (a << 8) | (b);
}
block[i] = (float) s;
}
float log_energy, energy;
int frame_count = 0;
int read_bytes = 0;
int end_pos = ReadBlock(block, 0, block_size, circular_float_buffer, cfb_size, cfb_pointer + 2, frame_length - frame_shift);
DCOffsetFilter(circular_float_buffer, cfb_size, &cfb_pointer, frame_length - frame_shift);
/*----------------------------------------------------------------*/
/* Framing */
/*----------------------------------------------------------------*/
while ((end_pos = ReadBlock(block, end_pos, block_size, circular_float_buffer, cfb_size, (cfb_pointer + 2) % cfb_size, frame_shift)) != -1) {
/*-------------------*/
/* DC offset removal */
/*-------------------*/
DCOffsetFilter(circular_float_buffer, cfb_size, &cfb_pointer, frame_shift);
/*------------------*/
/* logE computation */
/*------------------*/
energy = 0.0;
for (int i = 0; i < frame_length; ++i) {
int ind = (cfb_pointer + i + 3) % cfb_size;
energy += circular_float_buffer[ind] * circular_float_buffer[ind];
}
if (minimum_energy > 0 && energy < minimum_energy) {
continue;
}
if (energy < energy_floor_log_e) {
log_energy = ENERGYFLOOR_logE;
} else {
log_energy = log(energy);
}
/*-----------------------------------------------------*/
/* Pre-emphasis, moving from circular to linear buffer */
/*-----------------------------------------------------*/
for (int i = 0; i < frame_length; i++) {
int a = (cfb_pointer + i + 3) % cfb_size;
int b = (cfb_pointer + i + 2) % cfb_size;
float_buffer[i] = circular_float_buffer[a] - PRE_EMPHASIS * circular_float_buffer[b];
}
/*-----------*/
/* Windowing */
/*-----------*/
Window(float_buffer, float_window, frame_length);
/*-----*/
/* FFT */
/*-----*/
/* Zero padding */
for (int i = frame_length; i < fft_length; i++) {
float_buffer[i] = 0.0f;
}
/* Real valued, in-place split-radix FFT */
int tmp_int = (int) (log10((float) fft_length) / log10(2.0f));
rfft(float_buffer, fft_length, tmp_int); /*tmp_int = log2(FFTLength)*/
/* Magnitude spectrum */
float_buffer[0] = abs(float_buffer[0]); /* DC */
for (int i = 1; i < fft_length / 2; ++i) { /* pi/(N/2), 2pi/(N/2), ..., (N/2-1)*pi/(N/2) */
float_buffer[i] = sqrt(float_buffer[i] * float_buffer[i] + float_buffer[fft_length - i] * float_buffer[fft_length - i]);
}
float_buffer[fft_length / 2] = abs(float_buffer[fft_length / 2]); /* pi/2 */
/*---------------*/
/* Mel filtering */
/*---------------*/
MelFilterBank(float_buffer, first_window);
/*-------------------------------*/
/* Natural logarithm computation */
/*-------------------------------*/
for (int i = 0; i < mel_filter_bank_size; ++i) {
if (float_buffer[i] < energy_floor_fb) {
float_buffer[i] = ENERGYFLOOR_FB;
} else {
float_buffer[i] = log(float_buffer[i]);
}
}
/*---------------------------*/
/* Discrete Cosine Transform */
/*---------------------------*/
DCT(float_buffer, dct_matrix, cepstral_coefficient_number, mel_filter_bank_size);
/*--------------------------------------*/
/* Append logE after c0 or overwrite c0 */
/*--------------------------------------*/
float_buffer[mel_filter_bank_size + cepstral_coefficient_number - (no_coefficient_zero ? 1:0)] = log_energy;
/*---------------*/
/* Output result */
/*---------------*/
int coeff_number = (int) (cepstral_coefficient_number - (no_coefficient_zero ? 1:0) + (no_log_energy ? 0:1));
float *cep_coeff = new float[coeff_number];
for (int i = 0; i < coeff_number; ++i) {
cep_coeff[i] = float_buffer[mel_filter_bank_size + i];
}
cep_coeffs.push_back(cep_coeff);
frame_count++;
}
delete[] block;
}
/*********************************************************************** Vocoder: tratto da Ceres di ([email protected]) ***********************************************************************/ ULONG WavVocoder (PCSZ name, LONG argc, const RXSTRING * argv, PCSZ queuename, PRXSTRING retstr) { PSHORT pCh, pCh2, ptemp; APIRET rc; int i, frame, FDec, nCamp; int campioni, puntifft, Kn2, Koverlap; double soglia; if (argc != 4) { SendMsg (FUNC_VOCODER, ERR_NUMERO_PARAMETRI); return INVALID_ROUTINE; } if (!sscanf (argv[0].strptr, "%d", &pCh)) { SendMsg (FUNC_VOCODER, ERR_PUNTATORE_ERRATO); return INVALID_ROUTINE; } if (!sscanf (argv[1].strptr, "%d", &pCh2)) { SendMsg (FUNC_VOCODER, ERR_PUNTATORE_ERRATO); return INVALID_ROUTINE; } nCamp = atol (argv[2].strptr); if (nCamp < 1) { SendMsg (FUNC_VOCODER, ERR_VALORE_INVALIDO); return INVALID_ROUTINE; } pCh = (PSHORT) AllineaCh (pCh, (ULONG) nCamp, FUNC_VOCODER); if (!pCh) return INVALID_ROUTINE; pCh2 = (PSHORT) AllineaCh (pCh2, (ULONG) nCamp, FUNC_VOCODER); if (!pCh2) return INVALID_ROUTINE; soglia = atof (argv[3].strptr) / 1000000; if ((soglia < 0) | (soglia > 1)) { SendMsg (FUNC_VOCODER, ERR_VALORE_INVALIDO); return INVALID_ROUTINE; } fvec (inout, K_PUNTI); fvec (buffer, K_PUNTI); fvec (dativoc, K_PUNTI); fvec (WAnalisi, K_PUNTI); fvec (WSintesi, K_PUNTI); campioni = K_PUNTI; puntifft = K_PUNTI / 2; Kn2 = K_PUNTI / 2; Koverlap = 2; FDec = campioni / Koverlap; makewindows (WAnalisi, WSintesi, campioni, campioni, FDec); for (frame = 0; frame < (10 * nCamp / K_PUNTI); frame++) { for (i = 0; i < campioni; i++) inout[i] = (double) *pCh++ / (double) MAX_CAMPIONE; fold (inout, WAnalisi, campioni, buffer, campioni, frame * campioni); for (i = 0; i < campioni; i++) inout[i] = (double) 0; /* */ rfft (buffer, puntifft, FORWARD); convert (buffer, dativoc, Kn2, FDec, FreqCamp, 1); /* if(frame==100) for (i=0; i<Kn2; i++) { printf("freq %f, amp %f\n", dativoc[i+i+1], dativoc[i+i] * 1000); } */ for (i = 0; i < Kn2; i++) { if (dativoc[i + i] < soglia) dativoc[i + i] = 0; } unconvert (dativoc, buffer, Kn2, FDec, FreqCamp, 1); rfft (buffer, puntifft, INVERSE); overlapadd (buffer, campioni, WSintesi, inout, campioni, frame * campioni); for (i = 0; i < campioni; i++) *pCh2++ = *pCh2 + (SHORT) (inout[i] * MAX_CAMPIONE); pCh = pCh - (SHORT) (K_PUNTI * 0.9); pCh2 = pCh2 - (SHORT) (K_PUNTI * 0.9); } free (inout); free (buffer); free (dativoc); free (WAnalisi); free (WSintesi); sprintf (retstr->strptr, "%f", ERR_OK); retstr->strlength = strlen (retstr->strptr); return VALID_ROUTINE; }
int main(int argc, char **argv)
{
int i;
int N, M, L;
FILE *fp;
M = 128; /* kernel size */
N = 2*M; /* input sub-section length (fft size) */
L = N - M + 1; /* number of "good" points per section */
if ( argc != 2 || isatty(fileno(stdin)) || isatty(fileno(stdout)) ) {
bu_exit(1, "Usage: dconv filter < doubles > doubles\nXXX Warning: kernal size must be 2^i - 1\n" );
}
#ifdef never
/* prepare the kernel(!) */
/* this is either the direct complex response,
* or the FT(impulse resp)
*/
for ( i = 0; i < N; i++ ) {
if ( i <= N/2 )
ibuf[i] = 1.0; /* Real part */
else
ibuf[i] = 0.0; /* Imag part */
}
#endif /* never */
if ( (fp = fopen( argv[1], "r" )) == NULL ) {
bu_exit(2, "dconv: can't open \"%s\"\n", argv[1] );
}
if ( (M = fread( ibuf, sizeof(*ibuf), 2*MAXM, fp )) == 0 ) {
bu_exit(3, "dconv: problem reading filter file\n" );
}
fclose( fp );
if ( M > MAXM ) {
bu_exit(4, "dconv: only compiled for up to %d sized filter kernels\n", MAXM );
}
/*XXX HACK HACK HACK HACK XXX*/
/* Assume M = 2^i - 1 */
M += 1;
N = 2*M; /* input sub-section length (fft size) */
L = N - M + 1; /* number of "good" points per section */
if ( N == 256 )
rfft256( ibuf );
else
rfft( ibuf, N );
while ( (i = fread(&xbuf[M-1], sizeof(*xbuf), L, stdin)) > 0 ) {
if ( i < L ) {
/* pad the end with zero's */
memset((char *)&xbuf[M-1+i], 0, (L-i)*sizeof(*savebuffer));
}
memcpy(xbuf, savebuffer, (M-1)*sizeof(*savebuffer));
memcpy(savebuffer, &xbuf[L], (M-1)*sizeof(*savebuffer));
/*xform( xbuf, N );*/
if ( N == 256 )
rfft256( xbuf );
else
rfft( xbuf, N );
/* Mult */
mult( xbuf, ibuf, N );
/*invxform( xbuf, N );*/
if ( N == 256 )
irfft256( xbuf );
else
irfft( xbuf, N );
fwrite( &xbuf[M-1], sizeof(*xbuf), L, stdout );
}
return 0;
}
void Visualizer::Update()
{
static float input_wave[512];
float fft_tmp[512];
unsigned int buffer_pos = 0;
for (int i = 0; i < 256; i++)
{
//Clear the buffers
fft_tmp[i] = 0;
//Decay previous values
fft[i] = fft[i] * (((float)decay) / 100.0f);
}
unsigned int nextPacketSize = 1;
unsigned int flags;
while(nextPacketSize > 0 )
{
float *buf;
pAudioCaptureClient->GetBuffer((BYTE**)&buf, &nextPacketSize, (DWORD *)&flags, NULL, NULL);
for (int i = 0; i < nextPacketSize; i+=4)
{
for (int j = 0; j < 255; j++)
{
input_wave[2 * j] = input_wave[2 * (j + 1)];
input_wave[(2 * j) + 1] = input_wave[2 * j];
}
input_wave[510] = buf[i] * 2.0f * amplitude;
input_wave[511] = input_wave[510];
}
buffer_pos += nextPacketSize/4;
pAudioCaptureClient->ReleaseBuffer(nextPacketSize);
}
memcpy(fft_tmp, input_wave, sizeof(input_wave));
//Apply selected window
switch (window_mode)
{
case 0:
break;
case 1:
apply_window(fft_tmp, win_hanning, 256);
break;
case 2:
apply_window(fft_tmp, win_hamming, 256);
break;
case 3:
apply_window(fft_tmp, win_blackman, 256);
break;
default:
break;
}
//Run the FFT calculation
rfft(fft_tmp, 256, 1);
fft_tmp[0] = fft_tmp[2];
apply_window(fft_tmp, fft_nrml, 256);
//Compute FFT magnitude
for (int i = 0; i < 128; i += 2)
{
float fftmag;
//Compute magnitude from real and imaginary components of FFT and apply simple LPF
fftmag = (float)sqrt((fft_tmp[i] * fft_tmp[i]) + (fft_tmp[i + 1] * fft_tmp[i + 1]));
//Apply a slight logarithmic filter to minimize noise from very low amplitude frequencies
fftmag = ( 0.5f * log10(1.1f * fftmag) ) + ( 0.9f * fftmag );
//Limit FFT magnitude to 1.0
if (fftmag > 1.0f)
{
fftmag = 1.0f;
}
//Update to new values only if greater than previous values
if (fftmag > fft[i*2])
{
fft[i*2] = fftmag;;
}
//Prevent from going negative
if (fft[i*2] < 0.0f)
{
fft[i*2] = 0.0f;
}
//Set odd indexes to match their corresponding even index, as the FFT input array uses two indices for one value (real+imaginary)
fft[(i * 2) + 1] = fft[i * 2];
fft[(i * 2) + 2] = fft[i * 2];
fft[(i * 2) + 3] = fft[i * 2];
}
if (avg_mode == 0)
{
//Apply averaging over given number of values
int k;
float sum1 = 0;
float sum2 = 0;
for (k = 0; k < avg_size; k++)
{
sum1 += fft[k];
sum2 += fft[255 - k];
}
//Compute averages for end bars
sum1 = sum1 / k;
sum2 = sum2 / k;
for (k = 0; k < avg_size; k++)
{
fft[k] = sum1;
fft[255 - k] = sum2;
}
for (int i = 0; i < (256 - avg_size); i += avg_size)
{
float sum = 0;
for (int j = 0; j < avg_size; j += 1)
{
sum += fft[i + j];
}
float avg = sum / avg_size;
for (int j = 0; j < avg_size; j += 1)
{
fft[i + j] = avg;
}
}
}
else if(avg_mode == 1)
{
for (int i = 0; i < avg_size; i++)
{
float sum1 = 0;
float sum2 = 0;
int j;
for (j = 0; j <= i + avg_size; j++)
{
sum1 += fft[j];
sum2 += fft[255 - j];
}
fft[i] = sum1 / j;
fft[255 - i] = sum2 / j;
}
for (int i = avg_size; i < 256 - avg_size; i++)
{
float sum = 0;
for (int j = 1; j <= avg_size; j++)
{
sum += fft[i - j];
sum += fft[i + j];
}
sum += fft[i];
fft[i] = sum / (2 * avg_size + 1);
}
}
}
//-----------------------------------------------------------------------------
// Name: displayFunc( )
// Desc: callback function invoked to draw the client area
//-----------------------------------------------------------------------------
void displayFunc( )
{
static const int LP = 4;
static long int count = 0;
static char str[1024];
static unsigned int wf = 0;
static SAMPLE buffer[PVC_BUFFER_SIZE];
static SAMPLE buffer2[PVC_BUFFER_SIZE];
static polar_window * win[2] = { NULL, NULL };
static int which = 0;
static float _inc = 0.0f;
int i;
float pitch, power, fval;
if( g_freeze ) return;
// clear the color and depth buffers
glClear( GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT );
glPushMatrix( );
// rotate the sphere about y axis
glRotatef( g_angle_y += g_inc, 0.0f, 1.0f, 0.0f );
// color waveform
glColor3f( 0.4f, 0.4f, 1.0f );
// wait for data
while( !g_ready )
#if !defined(__OS_WINDOWS__)
usleep( 0 );
#else
Sleep( 0 );
#endif
// g_mutex.lock();
// get the window
memcpy( buffer, g_audio_buffer, g_buffer_size * sizeof(SAMPLE) );
memset( g_another_buffer, 0, g_buffer_size * sizeof(SAMPLE) );
// g_mutex.unlock();
g_ready = FALSE;
// analyze buffer
pv_analyze( g_pvc, buffer, g_hop_size );
// get phase windows out of queue
while( win[which] = pv_deque( g_pvc ) )
{
// unwrap phase
pv_unwrap_phase( win[which] );
// fix phase using last window and expected hop_size
if( g_phase_fix && win[!which] )
pv_phase_fix( win[!which], win[which], g_factor );
// freq shift
pv_freq_shift( g_pvc, win[which], g_factor2 );
// ifft
pv_unwrap_phase( win[which] );
// overlap/add
pv_overlap_add( g_pvc, win[which], (int)(g_hop_size * g_factor + .5f) );
which = !which;
// reclaim the window
pv_reclaim( g_pvc, win[which] );
}
// get buffer
pv_synthesize( g_pvc, g_another_buffer );
// apply the window
GLfloat x = -1.8f, inc = 3.6f / g_buffer_size, y = 1.0f;
//apply_window( (float*)buffer, g_window, g_buffer_size );
// draw the time domain waveform
glBegin( GL_LINE_STRIP );
GLint ii = ( g_buffer_size - (g_buffer_size/g_time_view) ) / 2;
for( i = ii; i < ii + g_buffer_size / g_time_view; i++ )
{
glVertex3f( x, g_gain * g_time_scale * .75f * g_another_buffer[i] + y, 0.0f );
x += inc * g_time_view;
}
glEnd();
// apply the window
x = -1.8f, inc = 3.6f / g_buffer_size, y = .5f;
// fft
memcpy( buffer, g_another_buffer, g_buffer_size * sizeof(SAMPLE) );
rfft( (float *)buffer, g_buffer_size/2, FFT_FORWARD );
x = -1.8f;
y = -1.2f;
complex * cbuf = (complex *)buffer;
// color the spectrum
glColor3f( 0.4f, 1.0f, 0.4f );
// draw the frequency domain representation
glBegin( GL_LINE_STRIP );
for( i = 0; i < g_buffer_size/g_freq_view; i++ )
{
g_spectrums[wf][i].x = x;
if( !g_usedb )
g_spectrums[wf][i].y = g_gain * g_freq_scale * .7f *
::pow( 25 * cmp_abs( cbuf[i] ), .5 ) + y;
else
g_spectrums[wf][i].y = g_gain * g_freq_scale * .8f *
( 20.0f * log10( cmp_abs(cbuf[i])/8.0 ) + 80.0f ) / 80.0f + y + .73f;
x += inc * g_freq_view;
}
glEnd();
g_draw[wf] = true;
for( i = 0; i < g_depth; i++ )
{
if( g_draw[(wf+i)%g_depth] )
{
Pt2D * pt = g_spectrums[(wf+i)%g_depth];
fval = (g_depth-i)/(float)g_depth;
glColor3f( .4f * fval, 1.0f * fval, .4f * fval );
x = -1.8f;
glBegin( GL_LINE_STRIP );
for( int j = 0; j < g_buffer_size/g_freq_view; j++, pt++ )
glVertex3f( x + j*(inc*g_freq_view), pt->y, -i * g_space + g_z );
glEnd();
}
}
if( !g_wutrfall )
g_draw[wf] = false;
wf--;
wf %= g_depth;
// factor
sprintf( str, "time-expansion factor: %.2f", g_factor );
draw_string( .5f, 0.2f, 0.0f, str, .45f );
sprintf( str, "frequency-expansion factor: %.2f", g_factor2 );
draw_string( .5f, 0.1f, 0.0f, str, .45f );
sprintf( str, "analysis window: %i", g_window_size );
draw_string( .5f, 0.0f, 0.0f, str, .45f );
sprintf( str, "hop size: %i", g_hop_size );
draw_string( .5f, -0.1f, 0.0f, str, .45f );
sprintf( str, "phase adjustment: %s", g_phase_fix ? "ON" : "OFF" );
draw_string( .5f, -0.2f, 0.0f, str, .45f );
glPushMatrix();
glTranslatef( 1.25f, -0.175f, 0.0f );
if( g_phase_fix )
glColor3f( 1.0f, .7f, .3f );
else
glColor3f( .6f, .6f, 1.0f );
glutSolidSphere( g_phase_fix ? .05f + .005f * fabs(sin(_inc)) : .01f, 10, 10 );
glPopMatrix();
sprintf( str, "# delay: %i buffers (%.0f ms)", pv_get_ready_len( g_pvc ),
pv_get_ready_len( g_pvc ) * g_buffer_size / 44100.0f * 1000.0f );
draw_string( .5f, -0.4f, 0.0f, str, .45f );
glPopMatrix( );
// swap the buffers
glFlush( );
glutSwapBuffers( );
g_buffer_count_b++;
_inc += .1f;
}
