// ana -> buffer de l'analyse ; samp -> sample ; inc -> incrementation de l'analyse
// bits -> taille analyse (max (2^11=2048)/2
void FFT::fftanalyseall(unsigned short *ana, const short *samp, const int inc, const int bits)
{
const unsigned int full=1<<bits;
const unsigned int half=full>>1;
unsigned int i,i2;
long xr1,xr2;
if (!inited)//initialisation des tables de permutation et cos sin
{
fftInit();
}
//copie buffer sample dans buffer travail
for (i=0; i<full; ++i)
{
DecompoTab[i][0]=*samp<<9;//12 Reel
DecompoTab[i][1]=0; //I
samp=samp+inc;
}
dofft86(DecompoTab, bits);
//Permutation finale + module
for (i=1; i<=half; ++i)
{
i2=permtab[i]>>(POW-bits);
xr1=DecompoTab[i2][0]>>12;
xr2=DecompoTab[i2][1]>>12;
ana[i-1]=(unsigned short)sqrt((xr1*xr1+xr2*xr2)*i);//Module
}
}
void
jack_client_init(long rb_size)
{
jack_client_t *client;
char *client_name;
double aFreq = 440.0;
pthread_attr_t attr;
fftInit(latency);
/* Initialize tuning */
int i;
freqs[0]=aFreq;
lfreqs[0]=log(freqs[0]);
for (i=1; i<12; i++) {
freqs[i] = freqs[i-1] * D_NOTE;
lfreqs[i] = lfreqs[i-1] + LOG_D_NOTE;
}
client_name = g_strdup_printf("show_note_%u", getpid());
if ((client = jack_client_new(client_name)) == 0) {
fprintf (stderr, "jack server not running?\n");
exit (1);
}
/* try and run detect_note thread in realtime */
pthread_attr_init(&attr);
/*
pthread_attr_setscope(&attr, PTHREAD_EXPLICIT_SCHED);
pthread_attr_setscope(&attr, PTHREAD_SCOPE_SYSTEM);
*/
memset (&thread_info, 0, sizeof (thread_info));
thread_info.rb_size = rb_size;
thread_info.client = client;
thread_info.channels = 1;
thread_info.can_process = 0;
if (pthread_create (&thread_info.thread_id, &attr, detect_note, &thread_info)) {
fprintf(stderr, "Error creating realtime thread!\n");
abort();
}
jack_set_process_callback(client, jack_process, &thread_info);
jack_on_shutdown(client, jack_shutdown, &thread_info);
rate = jack_get_sample_rate(client);
if (jack_activate(client)) {
fprintf(stderr, "cannot activate client");
}
}
float getFFTCorrelation(complexFloat *igram,int sizeX,int sizeY)
{
int line, samp;
int fftpowr;
float ampTmp=0;
float maxAmp=0;
complexFloat *fftBuf;
complexFloat *fftTemp;
complexFloat *fft;
/* fftBuf=(complexFloat *)MALLOC(sizeof(complexFloat)*sizeX);*/
fft=(complexFloat *)MALLOC(sizeof(complexFloat)*sizeX*sizeX);
fftTemp=(complexFloat *)MALLOC(sizeof(complexFloat)*sizeX*sizeX);
fftpowr=(log(sizeX)/log(2));
fftInit(fftpowr);
/* Compute the 2d complex FFT since no libraries exist to do this */
/* First do the FFT of each line*/
for(line=0;line<sizeX;line++)
{
fftBuf=&igram[line*sizeX];
ffts((float *)fftBuf,fftpowr,1);
for(samp=0;samp<sizeX;samp++)
{
fftTemp[line*sizeX+samp].real=fftBuf[samp].real;
fftTemp[line*sizeX+samp].imag=fftBuf[samp].imag;
}
}
/* Now do the FFT of the columns of the FFT'd lines */
for(samp=0;samp<sizeX;samp++)
{
/* Fill up the FFT buffer */
for(line=0;line<sizeX;line++)
{
fftBuf[line].real=fftTemp[line*sizeX+samp].real;
fftBuf[line].imag=fftTemp[line*sizeX+samp].imag;
}
/* Do the FFT */
ffts((float *)fftBuf,fftpowr,1);
for(line=0;line<sizeX;line++)
{
fft[line*sizeX+samp].real=fftBuf[line].real;
fft[line*sizeX+samp].imag=fftBuf[line].imag;
}
}
free(fftTemp);
fftFree();
/* Now we have a two dimension FFT that we can search to find the max value */
for(line=0;line<sizeX;line++)
{
for(samp=0;samp<sizeX;samp++)
{
ampTmp=sqrt(fft[line*sizeX+samp].real*fft[line*sizeX+samp].real+fft[line*sizeX+samp].imag*fft[line*sizeX+samp].imag);
if(ampTmp>maxAmp)
maxAmp=ampTmp;
}
}
free(fft);
return maxAmp;
}
void
f_alpha(int n_pts, int n_exp, double X[], double Q_d, double alpha)
{
int i, length;
double ha;
double *hfa, *wfa;
#ifdef HAVE_LIBFFTW3
fftw_complex *out = NULL;
fftw_plan plan_forward = NULL;
fftw_plan plan_backward = NULL;
NG_IGNORE(n_exp);
#endif
ha = alpha/2.0;
// Q_d = sqrt(Q_d); /* find the deviation of the noise */
#ifdef HAVE_LIBFFTW3
length = 2 * (n_pts/2 + 1);
#else
length = n_pts;
#endif
hfa = TMALLOC(double, length);
wfa = TMALLOC(double, length);
hfa[0] = 1.0;
wfa[0] = Q_d * GaussWa;
/* generate the coefficients hk */
for (i = 1; i < n_pts; i++) {
/* generate the coefficients hk */
hfa[i] = hfa[i-1] * (ha + (double)(i-1)) / ((double)(i));
/* fill the sequence wk with white noise */
wfa[i] = Q_d * GaussWa;
}
#ifdef HAVE_LIBFFTW3
/* in-place transformation needs zero padding on the end */
hfa[n_pts] = 0.0;
wfa[n_pts] = 0.0;
hfa[n_pts+1] = 0.0;
wfa[n_pts+1] = 0.0;
/* perform the discrete Fourier transform */
plan_forward = fftw_plan_dft_r2c_1d(n_pts, hfa, (fftw_complex *)hfa, FFTW_ESTIMATE);
fftw_execute(plan_forward);
fftw_destroy_plan(plan_forward);
plan_forward = fftw_plan_dft_r2c_1d(n_pts, wfa, (fftw_complex *)wfa, FFTW_ESTIMATE);
fftw_execute(plan_forward);
fftw_destroy_plan(plan_forward);
out = fftw_malloc(sizeof(fftw_complex) * (unsigned int) (n_pts/2 + 1));
/* multiply the two complex vectors */
for (i = 0; i < n_pts/2 + 1; i++) {
out[i][0] = hfa[i]*wfa[i] - hfa[i+1]*wfa[i+1];
out[i][1] = hfa[i]*wfa[i+1] + hfa[i+1]*wfa[i];
}
/* inverse transform */
plan_backward = fftw_plan_dft_c2r_1d(n_pts, out, X, FFTW_ESTIMATE);
fftw_execute(plan_backward);
fftw_destroy_plan(plan_backward);
for (i = 0; i < n_pts; i++) {
X[i] = X[i] / (double) n_pts;
}
fftw_free(out);
#else /* Green's FFT */
/* perform the discrete Fourier transform */
fftInit(n_exp);
rffts(hfa, n_exp, 1);
rffts(wfa, n_exp, 1);
/* multiply the two complex vectors */
rspectprod(hfa, wfa, X, n_pts);
/* inverse transform */
riffts(X, n_exp, 1);
#endif
free(hfa);
free(wfa);
/* fft tables will be freed in vsrcaccept.c and isrcaccept.c
fftFree(); */
fprintf(stdout, "%d 1/f noise values in time domain created\n", n_pts);
}
void main(){
const long N2 = 2; /* the number ffts to test */
long N = 2048; /* size of FFTs, must be power of 2 */
long kernSize = 1003; /* kernal size must be less than N */
long dataSize = N-kernSize+1; /* data size */
float *a;
float *b;
long i1;
long i2;
long TheErr;
long M;
FILE *fdataout; /* output file */
unsigned int randseed = 777;
int rannum;
#if macintosh
UnsignedWide TheTime1;
Microseconds(&TheTime1);
randseed = TheTime1.lo;
#endif
printf(" %6d Byte Floats \n", sizeof(a[0]));
printf(" randseed = %10u\n", randseed);
srand(randseed);
M = roundtol(LOG2(N));
N = POW2(M);
printf("fft size = %6d, ", N);
if (dataSize <= 0) TheErr = 22;
else TheErr = 0;
if(!TheErr){
TheErr = fftInit(M);
}
a = (float *) calloc(N2*N,sizeof(float) ); // calloc to zero pad data to fill N
if (a == 0) TheErr = 2;
if(!TheErr){
b = (float *) calloc(N2*N,sizeof(float) ); // calloc to zero pad data to fill N
if (b == 0) TheErr = 2;
}
if(!TheErr){
fdataout = fopen("convdat.cnv", "wb");
if (fdataout == NULL) TheErr = -50;
}
if(!TheErr){
/* write sizes to fdataout */
fwrite(&dataSize, sizeof(dataSize), 1, fdataout);
fwrite(&kernSize, sizeof(kernSize), 1, fdataout);
fwrite(&N2, sizeof(N2), 1, fdataout);
/* set up a simple test case and write to fdataout */
for (i2=0; i2<N2; i2++){
for (i1=0; i1<dataSize; i1++){
rannum = rand();
a[i2*N+i1] = BIPRAND(rannum);
}
fwrite(&a[i2*N], dataSize*sizeof(float), 1, fdataout);
}
for (i2=0; i2<N2; i2++){
for (i1=0; i1<kernSize; i1++){
rannum = rand();
b[i2*N+i1] = BIPRAND(rannum);
}
fwrite(&b[i2*N], kernSize*sizeof(float), 1, fdataout);
}
/* fast convolution of zero padded sequences */
rffts(a, M, N2);
rffts(b, M, N2);
for (i2=0; i2<N2*N; i2+=N){
rspectprod(&a[i2], &b[i2], &a[i2], N);
}
riffts(a, M, N2);
/* write out answer */
fwrite(a, N2*N*sizeof(float), 1, fdataout);
fclose(fdataout);
free(b);
free(a);
fftFree();
}
else{
if(TheErr==2) printf(" out of memory \n");
else printf(" error \n");
fftFree();
}
printf(" Done. \n");
return;
}
void convolve_s_fetch(snd_susp_type a_susp, snd_list_type snd_list)
{
convolve_susp_type susp = (convolve_susp_type) a_susp;
int cnt = 0; /* how many samples computed */
int togo;
int n;
sample_block_type out;
register sample_block_values_type out_ptr;
register sample_block_values_type out_ptr_reg;
sample_type *R = susp->R;
sample_type *R_current;
int N = susp->N;
falloc_sample_block(out, "convolve_s_fetch");
out_ptr = out->samples;
snd_list->block = out;
while (cnt < max_sample_block_len) { /* outer loop */
/* first compute how many samples to generate in inner loop: */
/* don't overflow the output sample block: */
togo = max_sample_block_len - cnt;
/* if we need output samples, generate them here */
if (susp->R_current >= R + N) {
/* Copy N samples of x_snd into X and zero fill to size 2N */
int i = 0;
sample_type *X = susp->X;
sample_type *H = susp->H;
int to_copy;
while (i < N) {
if (susp->x_snd_cnt == 0) {
susp_get_samples(x_snd, x_snd_ptr, x_snd_cnt);
if (susp->x_snd->logical_stop_cnt ==
susp->x_snd->current - susp->x_snd_cnt) {
min_cnt(&susp->susp.log_stop_cnt, susp->x_snd,
(snd_susp_type) susp, susp->x_snd_cnt);
}
}
if (susp->x_snd_ptr == zero_block->samples) {
min_cnt(&susp->terminate_cnt, susp->x_snd,
(snd_susp_type) susp, susp->x_snd_cnt);
/* extend the output to include impulse response */
susp->terminate_cnt += susp->h_len;
}
/* copy no more than the remaining space and no more than
* the amount remaining in the block
*/
to_copy = min(N - i, susp->x_snd_cnt);
memcpy(X + i, susp->x_snd_ptr,
to_copy * sizeof(*susp->x_snd_ptr));
susp->x_snd_ptr += to_copy;
susp->x_snd_cnt -= to_copy;
i += to_copy;
}
/* zero fill to size 2N */
memset(X + N, 0, N * sizeof(X[0]));
/* Compute FFT of X in place */
fftInit(susp->M);
rffts(X, susp->M, 1);
/* Multiply X by H (result goes into X) */
rspectprod(X, H, X, N * 2);
/* Compute IFFT of X in place */
riffts(X, susp->M, 1);
/* Shift R, zero fill, add X, all in one loop */
for (i = 0; i < N; i++) {
R[i] = R[i + N] + X[i];
R[i + N] = X[i + N];
}
/* now N samples of R can be output */
susp->R_current = R;
}
/* compute togo, the number of samples to "compute" */
/* can't use more than what's left in R. R_current is
the next sample of R, so what's left is N - (R - R_current) */
R_current = susp->R_current;
togo = min(togo, N - (R_current - R));
/* don't run past terminate time */
if (susp->terminate_cnt != UNKNOWN &&
susp->terminate_cnt <= susp->susp.current + cnt + togo) {
togo = susp->terminate_cnt - (susp->susp.current + cnt);
if (togo == 0) break;
}
/* don't run past logical stop time */
if (!susp->logically_stopped &&
susp->susp.log_stop_cnt != UNKNOWN &&
susp->susp.log_stop_cnt <= susp->susp.current + cnt + togo) {
togo = susp->susp.log_stop_cnt - (susp->susp.current + cnt);
if (togo == 0) break;
}
n = togo;
out_ptr_reg = out_ptr;
if (n) do { /* the inner sample computation loop */
*out_ptr_reg++ = (sample_type) *R_current++;
} while (--n); /* inner loop */
/* using R_current is a bad idea on RS/6000: */
susp->R_current += togo;
out_ptr += togo;
cnt += togo;
} /* outer loop */
/* test for termination */
if (togo == 0 && cnt == 0) {
snd_list_terminate(snd_list);
} else {
snd_list->block_len = cnt;
susp->susp.current += cnt;
}
/* test for logical stop */
if (susp->logically_stopped) {
snd_list->logically_stopped = true;
} else if (susp->susp.log_stop_cnt == susp->susp.current) {
susp->logically_stopped = true;
}
} /* convolve_s_fetch */
sound_type snd_make_convolve(sound_type x_snd, sound_type h_snd)
{
register convolve_susp_type susp;
rate_type sr = x_snd->sr;
time_type t0 = x_snd->t0;
sample_type scale_factor = 1.0F;
time_type t0_min = t0;
table_type table;
double log_len;
falloc_generic(susp, convolve_susp_node, "snd_make_convolve");
table = sound_to_table(h_snd);
susp->h_len = table->length;
log_len = log(table->length) / M_LN2; /* compute log-base-2(length) */
susp->M = (int) log_len;
if (susp->M != log_len) susp->M++; /* round up */
susp->N = 1 << susp->M; /* size of data blocks */
susp->M++; /* M = log2(2 * N) */
susp->H = (sample_type *) calloc(2 * susp->N, sizeof(susp->H[0]));
if (!susp->H) {
xlabort("memory allocation failure in convolve");
}
memcpy(susp->H, table->samples, sizeof(susp->H[0]) * susp->N);
table_unref(table); /* don't need table now */
/* remaining N samples are already zero-filled */
if (fftInit(susp->M)) {
free(susp->H);
xlabort("fft initialization error in convolve");
}
rffts(susp->H, susp->M, 1);
susp->X = (sample_type *) calloc(2 * susp->N, sizeof(susp->X[0]));
susp->R = (sample_type *) calloc(2 * susp->N, sizeof(susp->R[0]));
if (!susp->X || !susp->R) {
free(susp->H);
if (susp->X) free(susp->X);
if (susp->R) free(susp->R);
xlabort("memory allocation failed in convolve");
}
susp->R_current = susp->R + susp->N;
susp->susp.fetch = &convolve_s_fetch;
susp->terminate_cnt = UNKNOWN;
/* handle unequal start times, if any */
if (t0 < x_snd->t0) sound_prepend_zeros(x_snd, t0);
/* minimum start time over all inputs: */
t0_min = min(x_snd->t0, t0);
/* how many samples to toss before t0: */
susp->susp.toss_cnt = (long) ((t0 - t0_min) * sr + 0.5);
if (susp->susp.toss_cnt > 0) {
susp->susp.keep_fetch = susp->susp.fetch;
susp->susp.fetch = convolve_toss_fetch;
}
/* initialize susp state */
susp->susp.free = convolve_free;
susp->susp.sr = sr;
susp->susp.t0 = t0;
susp->susp.mark = convolve_mark;
susp->susp.print_tree = convolve_print_tree;
susp->susp.name = "convolve";
susp->logically_stopped = false;
susp->susp.log_stop_cnt = logical_stop_cnt_cvt(x_snd);
susp->susp.current = 0;
susp->x_snd = x_snd;
susp->x_snd_cnt = 0;
return sound_create((snd_susp_type)susp, t0, sr, scale_factor);
}
void ifft__fetch(register ifft_susp_type susp, snd_list_type snd_list)
{
int cnt = 0; /* how many samples computed */
int togo;
int n;
sample_block_type out;
register sample_block_values_type out_ptr;
register sample_block_values_type out_ptr_reg;
register long index_reg;
register sample_type * outbuf_reg;
falloc_sample_block(out, "ifft__fetch");
out_ptr = out->samples;
snd_list->block = out;
while (cnt < max_sample_block_len) { /* outer loop */
/* first compute how many samples to generate in inner loop: */
/* don't overflow the output sample block: */
togo = max_sample_block_len - cnt;
if (susp->src == NULL) {
out: togo = 0; /* indicate termination */
break; /* we're done */
}
if (susp->index >= susp->stepsize) {
long i;
long m, n;
LVAL elem;
susp->index = 0;
susp->array =
xleval(cons(s_send, cons(susp->src, consa(s_next))));
if (susp->array == NULL) {
susp->src = NULL;
goto out;
} else if (!vectorp(susp->array)) {
xlerror("array expected", susp->array);
} else if (susp->samples == NULL) {
/* assume arrays are all the same size as first one;
now that we know the size, we just have to do this
first allocation.
*/
susp->length = getsize(susp->array);
if (susp->length < 1)
xlerror("array has no elements", susp->array);
if (susp->window && (susp->window_len != susp->length))
xlerror("window size and spectrum size differ",
susp->array);
/* tricky non-power of 2 detector: only if this is a
* power of 2 will the highest 1 bit be cleared when
* we subtract 1 ...
*/
if (susp->length & (susp->length - 1))
xlfail("spectrum size must be a power of 2");
susp->samples = (sample_type *) calloc(susp->length,
sizeof(sample_type));
susp->outbuf = (sample_type *) calloc(susp->length,
sizeof(sample_type));
} else if (getsize(susp->array) != susp->length) {
xlerror("arrays must all be the same length", susp->array);
}
/* at this point, we have a new array to put samples */
/* the incoming array format is [DC, R1, I1, R2, I2, ... RN]
* where RN is the real coef at the Nyquist frequency
* but susp->samples should be organized as [DC, RN, R1, I1, ...]
*/
n = susp->length;
/* get the DC (real) coef */
elem = getelement(susp->array, 0);
MUST_BE_FLONUM(elem)
susp->samples[0] = (sample_type) getflonum(elem);
/* get the Nyquist (real) coef */
elem = getelement(susp->array, n - 1);
MUST_BE_FLONUM(elem);
susp->samples[1] = (sample_type) getflonum(elem);
/* get the remaining coef */
for (i = 1; i < n - 1; i++) {
elem = getelement(susp->array, i);
MUST_BE_FLONUM(elem)
susp->samples[i + 1] = (sample_type) getflonum(elem);
}
susp->array = NULL; /* free the array */
/* here is where the IFFT and windowing should take place */
//fftnf(1, &n, susp->samples, susp->samples + n, -1, 1.0);
m = round(log(n) / M_LN2);
if (!fftInit(m)) riffts(susp->samples, m, 1);
else xlfail("FFT initialization error");
if (susp->window) {
n = susp->length;
for (i = 0; i < n; i++) {
susp->samples[i] *= susp->window[i];
}
}
/* shift the outbuf */
n = susp->length - susp->stepsize;
for (i = 0; i < n; i++) {
susp->outbuf[i] = susp->outbuf[i + susp->stepsize];
}
/* clear end of outbuf */
for (i = n; i < susp->length; i++) {
susp->outbuf[i] = 0;
}
/* add in the ifft result */
n = susp->length;
for (i = 0; i < n; i++) {
susp->outbuf[i] += susp->samples[i];
}
}
togo = min(togo, susp->stepsize - susp->index);
n = togo;
index_reg = susp->index;
outbuf_reg = susp->outbuf;
out_ptr_reg = out_ptr;
if (n) do { /* the inner sample computation loop */
*out_ptr_reg++ = outbuf_reg[index_reg++];;
} while (--n); /* inner loop */
susp->index = index_reg;
susp->outbuf = outbuf_reg;
out_ptr += togo;
cnt += togo;
} /* outer loop */
/* test for termination */
if (togo == 0 && cnt == 0) {
snd_list_terminate(snd_list);
} else {
snd_list->block_len = cnt;
susp->susp.current += cnt;
}
} /* ifft__fetch */
void preprocessorInit(struct objPreprocessor *myPreprocessor, struct ParametersStruct *myParameters, struct objMicrophones* myMicrophones)
{
unsigned int indexMic;
/***************************************************************************
* Step 1: Load parameters *
***************************************************************************/
// +-------------------------------------------------------------------+
// | General |
// +-------------------------------------------------------------------+
// Define the size of the frame
myPreprocessor->PP_FRAMESIZE = GLOBAL_FRAMESIZE;
/***************************************************************************
* Step 2: Initialize context *
***************************************************************************/
// +-------------------------------------------------------------------+
// | Step A: Microphones |
// +-------------------------------------------------------------------+
myPreprocessor->myMicrophones = (struct objMicrophones*) malloc(sizeof(struct objMicrophones));
microphonesClone(myMicrophones, myPreprocessor->myMicrophones);
// +-------------------------------------------------------------------+
// | Step B: Create arrays |
// +-------------------------------------------------------------------+
myPreprocessor->micArray = (struct objMicST**) newTable2D(myPreprocessor->myMicrophones->nMics, 1, sizeof(struct objMicST));
myPreprocessor->myFFT = (struct objFFT*) malloc(sizeof(struct objFFT));
myPreprocessor->window = (float*) newTable1D(myPreprocessor->PP_FRAMESIZE, sizeof(float));
myPreprocessor->workingArray1Real = (float*) newTable1D(myPreprocessor->PP_FRAMESIZE, sizeof(float));
myPreprocessor->workingArray1Imag = (float*) newTable1D(myPreprocessor->PP_FRAMESIZE, sizeof(float));
myPreprocessor->workingArray2Real = (float*) newTable1D(myPreprocessor->PP_FRAMESIZE, sizeof(float));
myPreprocessor->workingArray2Imag = (float*) newTable1D(myPreprocessor->PP_FRAMESIZE, sizeof(float));
// +-------------------------------------------------------------------+
// | Step C: Initialize the FFT object |
// +-------------------------------------------------------------------+
fftInit(myPreprocessor->myFFT, myParameters, myPreprocessor->PP_FRAMESIZE);
// +-------------------------------------------------------------------+
// | Step D: Initialize window |
// +-------------------------------------------------------------------+
generatePowerComplementaryWindow(myPreprocessor->window, myPreprocessor->PP_FRAMESIZE);
// +-------------------------------------------------------------------+
// | Step E: Initialize the sound track for each microphone |
// +-------------------------------------------------------------------+
for (indexMic = 0; indexMic < myPreprocessor->myMicrophones->nMics; indexMic++)
{
micstInit(myPreprocessor->micArray[indexMic], myParameters,
microphonesGetPosition(myPreprocessor->myMicrophones,indexMic),
microphonesGetGain(myPreprocessor->myMicrophones,indexMic));
}
}
