예제 #1
0
void fftfilt_free(struct fftfilt *s)
{
	if (s) {
		fft_free(s->fft);
		fft_free(s->ift);
		fft_free(s->tmpfft);
		g_free(s->ovlbuf);
		g_free(s->filter);
		g_free(s);
	}
}
예제 #2
0
파일: someops.c 프로젝트: welcheb/bart
static void fft_linop_free(const linop_data_t* _data)
{
	const struct fft_linop_s* data = CAST_DOWN(fft_linop_s, _data);

	fft_free(data->frw);
	fft_free(data->adj);

	free(data->dims);
	free(data->strs);

	free((void*)data);
}
예제 #3
0
파일: nc.c 프로젝트: LgJianzhao/httping
void end_ncurses(void)
{
	int index = 0;

	for(index=0; index<window_history_n; index++)
	{
		free(slow_history[index]);
		free(fast_history[index]);
	}

	free(slow_history);
	free(fast_history);

	if (w_stats)
	{
		delwin(w_stats);
		delwin(w_line1);
		delwin(w_slow);
		delwin(w_line2);
		delwin(w_fast);
	}

	endwin();

	free(history);
	free(history_set);

#ifdef FW
	fft_free();
	fft_stop();
#endif
	free(history_temp);
	free(history_fft_phase);
	free(history_fft_magn);
}
예제 #4
0
파일: eq.c 프로젝트: OV2/RetroArch
static void create_filter(struct eq_data *eq, unsigned size_log2,
      struct eq_gain *gains, unsigned num_gains, double beta)
{
   int i;
   int half_block_size = eq->block_size >> 1;
   double window_mod = 1.0 / kaiser_window(0.0, beta);

   fft_t *fft = fft_new(size_log2);
   float *time_filter = (float*)calloc(eq->block_size * 2, sizeof(*time_filter));
   if (!fft || !time_filter)
      goto end;

   // Make sure bands are in correct order.
   qsort(gains, num_gains, sizeof(*gains), gains_cmp);

   // Compute desired filter response.
   generate_response(eq->filter, gains, num_gains, half_block_size);

   // Get equivalent time-domain filter.
   fft_process_inverse(fft, time_filter, eq->filter, 1);

   // ifftshift() to create the correct linear phase filter.
   // The filter response was designed with zero phase, which won't work unless we compensate
   // for the repeating property of the FFT here by flipping left and right blocks.
   for (i = 0; i < half_block_size; i++)
   {
      float tmp = time_filter[i + half_block_size];
      time_filter[i + half_block_size] = time_filter[i];
      time_filter[i] = tmp;
   }

   // Apply a window to smooth out the frequency repsonse.
   for (i = 0; i < (int)eq->block_size; i++)
   {
      // Kaiser window.
      double phase = (double)i / (eq->block_size - 1);
      phase = 2.0 * (phase - 0.5);
      time_filter[i] *= window_mod * kaiser_window(phase, beta);
   }

   // Debugging.
#if 0
   FILE *file = fopen("/tmp/test.txt", "w");
   if (file)
   {
      for (i = 0; i < (int)eq->block_size; i++)
         fprintf(file, "%.6f\n", time_filter[i]);
      fclose(file);
   }
#endif

   // Padded FFT to create our FFT filter.
   fft_process_forward(eq->fft, eq->filter, time_filter, 1);

end:
   fft_free(fft);
   free(time_filter);
}
예제 #5
0
파일: eq.c 프로젝트: AlexFolland/RetroArch
static void eq_free(void *data)
{
   struct eq_data *eq = (struct eq_data*)data;
   if (!eq)
      return;

   fft_free(eq->fft);
   free(eq->save);
   free(eq->block);
   free(eq->fftblock);
   free(eq->filter);
   free(eq);
}
예제 #6
0
static void dominoex_free(struct dominoex *s)
{
    if (s) {
        fft_free(s->fft);
        sfft_free(s->sfft);
        filter_free(s->hilbert);

        g_free(s->pipe);

        filter_free(s->filt);

        g_free(s);
    }
}
예제 #7
0
파일: fourier.c 프로젝트: xelmirage/libpano
void fwiener( TrformStr *TrPtr, Image *nf, Image *psf, double gamma , double frame)
{
	int 		dims[2], i, k, dim, prog=0, delta = 100/27;
	double		**d1 = NULL, **d2 = NULL, **d3 = NULL, **d4 = NULL; // double arrays
	char		percent[25];

	dims[0] = TrPtr->src->width;
	dims[1] = TrPtr->src->height;
	dim = TrPtr->src->width * TrPtr->src->height;

	Progress( _initProgress, "Wiener Filter" );
	
	d1 	= (double**)mymalloc( dim * sizeof( double ) );
	d2 	= (double**)mymalloc( dim * sizeof( double ) );
	d3 =  (double**)mymalloc( dim * sizeof( double ) );
	d4 =  (double**)mymalloc( dim * sizeof( double ) );

	if( d1 == NULL || d2 == NULL || d3 == NULL || d4 == NULL )
	{
		PrintError("Not enough memory");
		TrPtr->success = 0;
		goto _fwiener_exit;
	}

	
	for( i=0; i<3; i++ )
	{
		register double x,y,si,sn,rden,a,b;
		
		UPDATE_PROGRESS_WIENER
		
		if( makeDoubleImage ( nf, *d1, *d2, i, TrPtr->gamma ) != 0 )
		{
			PrintError("Could not make Real-Version of image");
			TrPtr->success = 0;
			goto _fwiener_exit;
		}
			
		fftn (2, dims, *d1, *d2, 1, 1.0 ); // Noise filtered image

		UPDATE_PROGRESS_WIENER	
		
		makeDoubleDiffImage ( TrPtr->src, nf, *d3, *d4, i );
		fftn (2, dims, *d3, *d4, 1, 1.0 ); // Noise in image

		UPDATE_PROGRESS_WIENER

		for( k= 0; k<dim; k++)
		{
			x = (*d1)[k]; y = (*d2)[k];
			
			(*d1)[k] = x * x + y * y;	// S_ii
			
			x = (*d3)[k]; y = (*d4)[k];
			
			(*d2)[k] = x * x + y * y;	// S_nn
		}

		UPDATE_PROGRESS_WIENER
		
		makePSF( TrPtr->src->width, TrPtr->src->height, psf, *d3, *d4, i, -1 );
		
		fftn (2, dims, *d3, *d4, 1, 1.0 ); // H(w1,w2)

		UPDATE_PROGRESS_WIENER

		for( k= 0; k<dim; k++)
		{

			x = (*d3)[k]; y = (*d4)[k];
			
			si = (*d1)[k];
			sn = (*d2)[k];
			
			if( si == 0.0 )
			{
				(*d1)[k] = 0.0;
				(*d2)[k] = 0.0;
			}
			else
			{
				rden 		= x*x +y*y + gamma * sn/si;
				(*d1)[k] = x / rden ;
				(*d2)[k] = y / rden ;
			}
		}

		UPDATE_PROGRESS_WIENER
		
		if( makeDoubleImage	( TrPtr->src, *d3, *d4, i, TrPtr->gamma ) != 0 )
		{
			PrintError("Could not make Real-Version of image");
			TrPtr->success = 0;
			goto _fwiener_exit;
		}

		if( frame > 0.0)
			windowFunction( *d3, TrPtr->src->width, TrPtr->src->height, frame );

		fftn (2, dims, *d3, *d4, 1, 1.0 ); // v(w1,w2)

		UPDATE_PROGRESS_WIENER


		for( k= 0; k<dim; k++)
		{

			x = (*d3)[k]; y = (*d4)[k];
			a = (*d1)[k]; b = (*d2)[k];
			
			(*d1)[k] = x*a - y*b;
			(*d2)[k] = x*b + y*a;
		}
		
		UPDATE_PROGRESS_WIENER
			
		
 		fftn (2, dims, *d1, *d2, -1, -1.0 ); // backward transform; 

		// invWindowFunction( *d1, TrPtr->src->width, TrPtr->src->height, 25.0);

		UPDATE_PROGRESS_WIENER
		
		makeUcharImage ( TrPtr->dest, *d1,  i );
	}
	TrPtr->success = 1;

_fwiener_exit:	
	Progress( _disposeProgress, percent );
	fft_free();	
	if( d1  != NULL ) myfree( (void**)d1  ); 
	if( d2  != NULL ) myfree( (void**)d2  );  
	if( d3  != NULL ) myfree( (void**)d3  );
	if( d4  != NULL ) myfree( (void**)d4  );

}
예제 #8
0
파일: fftn.c 프로젝트: xelmirage/libpano
/*
 * singleton's mixed radix routine
 *
 * could move allocation out to fftn(), but leave it here so that it's
 * possible to make this a standalone function
 */
static int
FFTRADIX (REAL Re [],
	  REAL Im [],
	  unsigned int nTotal,
	  unsigned int nPass,
	  unsigned int nSpan,
	  int iSign,
	  unsigned int maxFactors,
	  unsigned int maxPerm)
{
   int ii, nFactor, kspan, ispan, inc;
   int j, jc, jf, jj, k, k1, k3, kk, kt, nn, ns, nt;

   REAL radf;
   REAL c1, c2, c3, cd;
   REAL s1, s2, s3, sd;

   REAL * Rtmp = NULL;		/* temp space for real part*/
   REAL * Itmp = NULL;		/* temp space for imaginary part */
   REAL * Cos = NULL;		/* Cosine values */
   REAL * Sin = NULL;		/* Sine values */

#ifndef FFT_RADIX4
   REAL s60 = SIN60;		/* sin(60 deg) */
   REAL s72 = SIN72;		/* sin(72 deg) */
   REAL c72 = COS72;		/* cos(72 deg) */
#endif
   REAL pi2 = M_PI;		/* use PI first, 2 PI later */

   /* gcc complains about k3 being uninitialized, but I can't find out where
    * or why ... it looks okay to me.
    *
    * initialize to make gcc happy
    */
   k3 = 0;

   /* gcc complains about c2, c3, s2,s3 being uninitialized, but they're
    * only used for the radix 4 case and only AFTER the (s1 == 0.0) pass
    * through the loop at which point they will have been calculated.
    *
    * initialize to make gcc happy
    */
   c2 = c3 = s2 = s3 = 0.0;

   /* Parameter adjustments, was fortran so fix zero-offset */
   Re--;
   Im--;

   if (nPass < 2)
     return 0;

   /* allocate storage */
   if (SpaceAlloced < maxFactors * sizeof (REAL))
     {
#ifdef SUN_BROKEN_REALLOC
	if (!SpaceAlloced)	/* first time */
	  {
	     SpaceAlloced = maxFactors * sizeof (REAL);
	     Tmp0 = malloc (SpaceAlloced);
	     Tmp1 = malloc (SpaceAlloced);
	     Tmp2 = malloc (SpaceAlloced);
	     Tmp3 = malloc (SpaceAlloced);
	  }
	else
	  {
#endif
	     SpaceAlloced = maxFactors * sizeof (REAL);
	     Tmp0 = realloc (Tmp0, SpaceAlloced);
	     Tmp1 = realloc (Tmp1, SpaceAlloced);
	     Tmp2 = realloc (Tmp2, SpaceAlloced);
	     Tmp3 = realloc (Tmp3, SpaceAlloced);
#ifdef SUN_BROKEN_REALLOC
	  }
#endif
     }
   else
     {
	/* allow full use of alloc'd space */
	maxFactors = SpaceAlloced / sizeof (REAL);
     }
   if (MaxPermAlloced < (size_t)maxPerm)
     {
#ifdef SUN_BROKEN_REALLOC
	if (!MaxPermAlloced)	/* first time */
	  Perm = malloc (maxPerm * sizeof(int));
	else
#endif
	  Perm = realloc (Perm, maxPerm * sizeof(int));
	MaxPermAlloced = maxPerm;
     }
   else
     {
	/* allow full use of alloc'd space */
	maxPerm = MaxPermAlloced;
     }
   if (!Tmp0 || !Tmp1 || !Tmp2 || !Tmp3 || !Perm) goto Memory_Error;

   /* assign pointers */
   Rtmp = (REAL *) Tmp0;
   Itmp = (REAL *) Tmp1;
   Cos  = (REAL *) Tmp2;
   Sin  = (REAL *) Tmp3;

   /*
    * Function Body
    */
   inc = iSign;
   if (iSign < 0)
     {
#ifndef FFT_RADIX4
	s60 = -s60;
	s72 = -s72;
#endif
	pi2 = -pi2;
	inc = -inc;		/* absolute value */
     }

   /* adjust for strange increments */
   nt = inc * nTotal;
   ns = inc * nSpan;
   kspan = ns;

   nn = nt - inc;
   jc = ns / nPass;
   radf = pi2 * (double) jc;
   pi2 *= 2.0;			/* use 2 PI from here on */

   ii = 0;
   jf = 0;
   /* determine the factors of n */

   nFactor = factorize (nPass, &kt);
   /* test that nFactors is in range */
   if (nFactor > NFACTOR)
     {
	fprintf (stderr, "Error: " FFTRADIXS "() - exceeded number of factors\n");
	goto Memory_Error;
     }

   /* compute fourier transform */
   for (;;) {
      sd = radf / (double) kspan;
      cd = sin (sd);
      cd = 2.0 * cd * cd;
      sd = sin (sd + sd);
      kk = 1;
      ii++;

      switch (factor [ii - 1]) {
       case 2:
	 /* transform for factor of 2 (including rotation factor) */
	 kspan /= 2;
	 k1 = kspan + 2;
	 do {
	    do {
	       REAL tmpr;
	       REAL tmpi;
	       int k2;

	       k2 = kk + kspan;
	       tmpr = Re_Data (k2);
	       tmpi = Im_Data (k2);
	       Re_Data (k2) = Re_Data (kk) - tmpr;
	       Im_Data (k2) = Im_Data (kk) - tmpi;
	       Re_Data (kk) += tmpr;
	       Im_Data (kk) += tmpi;
	       kk = k2 + kspan;
	    } while (kk <= nn);
	    kk -= nn;
	 } while (kk <= jc);
	 if (kk > kspan)
	   goto Permute_Results;	/* exit infinite loop */
	 do {
	    int k2;

	    c1 = 1.0 - cd;
	    s1 = sd;
	    do {
	       REAL tmp;
	       do {
		  do {
		     REAL tmpr;
		     REAL tmpi;

		     k2 = kk + kspan;
		     tmpr = Re_Data (kk) - Re_Data (k2);
		     tmpi = Im_Data (kk) - Im_Data (k2);
		     Re_Data (kk) += Re_Data (k2);
		     Im_Data (kk) += Im_Data (k2);
		     Re_Data (k2) = c1 * tmpr - s1 * tmpi;
		     Im_Data (k2) = s1 * tmpr + c1 * tmpi;
		     kk = k2 + kspan;
		  } while (kk < nt);
		  k2 = kk - nt;
		  c1 = -c1;
		  kk = k1 - k2;
	       } while (kk > k2);
	       tmp = c1 - (cd * c1 + sd * s1);
	       s1 = sd * c1 - cd * s1 + s1;
	       c1 = 2.0 - (tmp * tmp + s1 * s1);
	       s1 *= c1;
	       c1 *= tmp;
	       kk += jc;
	    } while (kk < k2);
	    k1 += (inc + inc);
	    kk = (k1 - kspan) / 2 + jc;
	 } while (kk <= jc + jc);
	 break;

       case 4:			/* transform for factor of 4 */
	 ispan = kspan;
	 kspan /= 4;

	 do {
	    c1 = 1.0;
	    s1 = 0.0;
	    do {
	       do {
		  REAL ajm, ajp, akm, akp;
		  REAL bjm, bjp, bkm, bkp;
		  int k2;

		  k1 = kk + kspan;
		  k2 = k1 + kspan;
		  k3 = k2 + kspan;
		  akp = Re_Data (kk) + Re_Data (k2);
		  akm = Re_Data (kk) - Re_Data (k2);

		  ajp = Re_Data (k1) + Re_Data (k3);
		  ajm = Re_Data (k1) - Re_Data (k3);

		  bkp = Im_Data (kk) + Im_Data (k2);
		  bkm = Im_Data (kk) - Im_Data (k2);

		  bjp = Im_Data (k1) + Im_Data (k3);
		  bjm = Im_Data (k1) - Im_Data (k3);

		  Re_Data (kk) = akp + ajp;
		  Im_Data (kk) = bkp + bjp;
		  ajp = akp - ajp;
		  bjp = bkp - bjp;
		  if (iSign < 0)
		    {
		       akp = akm + bjm;
		       bkp = bkm - ajm;
		       akm -= bjm;
		       bkm += ajm;
		    }
		  else
		    {
		       akp = akm - bjm;
		       bkp = bkm + ajm;
		       akm += bjm;
		       bkm -= ajm;
		    }
		  /* avoid useless multiplies */
		  if (s1 == 0.0)
		    {
		       Re_Data (k1) = akp;
		       Re_Data (k2) = ajp;
		       Re_Data (k3) = akm;
		       Im_Data (k1) = bkp;
		       Im_Data (k2) = bjp;
		       Im_Data (k3) = bkm;
		    }
		  else
		    {
		       Re_Data (k1) = akp * c1 - bkp * s1;
		       Re_Data (k2) = ajp * c2 - bjp * s2;
		       Re_Data (k3) = akm * c3 - bkm * s3;
		       Im_Data (k1) = akp * s1 + bkp * c1;
		       Im_Data (k2) = ajp * s2 + bjp * c2;
		       Im_Data (k3) = akm * s3 + bkm * c3;
		    }
		  kk = k3 + kspan;
	       } while (kk <= nt);

	       c2 = c1 - (cd * c1 + sd * s1);
	       s1 = sd * c1 - cd * s1 + s1;
	       c1 = 2.0 - (c2 * c2 + s1 * s1);
	       s1 *= c1;
	       c1 *= c2;
	       /* values of c2, c3, s2, s3 that will get used next time */
	       c2 = c1 * c1 - s1 * s1;
	       s2 = 2.0 * c1 * s1;
	       c3 = c2 * c1 - s2 * s1;
	       s3 = c2 * s1 + s2 * c1;
	       kk = kk - nt + jc;
	    } while (kk <= kspan);
	    kk = kk - kspan + inc;
	 } while (kk <= jc);
	 if (kspan == jc)
	   goto Permute_Results;	/* exit infinite loop */
	 break;

       default:
	 /* transform for odd factors */
#ifdef FFT_RADIX4
	 fprintf (stderr, "Error: " FFTRADIXS "(): compiled for radix 2/4 only\n");
	 fft_free ();		/* free-up memory */
	 return -1;
	 break;
#else	/* FFT_RADIX4 */
	 ispan = kspan;
	 k = factor [ii - 1];
	 kspan /= factor [ii - 1];

	 switch (factor [ii - 1]) {
	  case 3:	/* transform for factor of 3 (optional code) */
	    do {
	       do {
		  REAL aj, tmpr;
		  REAL bj, tmpi;
		  int k2;

		  k1 = kk + kspan;
		  k2 = k1 + kspan;
		  tmpr = Re_Data (kk);
		  tmpi = Im_Data (kk);
		  aj = Re_Data (k1) + Re_Data (k2);
		  bj = Im_Data (k1) + Im_Data (k2);
		  Re_Data (kk) = tmpr + aj;
		  Im_Data (kk) = tmpi + bj;
		  tmpr -= 0.5 * aj;
		  tmpi -= 0.5 * bj;
		  aj = (Re_Data (k1) - Re_Data (k2)) * s60;
		  bj = (Im_Data (k1) - Im_Data (k2)) * s60;
		  Re_Data (k1) = tmpr - bj;
		  Re_Data (k2) = tmpr + bj;
		  Im_Data (k1) = tmpi + aj;
		  Im_Data (k2) = tmpi - aj;
		  kk = k2 + kspan;
	       } while (kk < nn);
	       kk -= nn;
	    } while (kk <= kspan);
	    break;

	  case 5:	/* transform for factor of 5 (optional code) */
	    c2 = c72 * c72 - s72 * s72;
	    s2 = 2.0 * c72 * s72;
	    do {
	       do {
		  REAL aa, aj, ak, ajm, ajp, akm, akp;
		  REAL bb, bj, bk, bjm, bjp, bkm, bkp;
		  int k2, k4;

		  k1 = kk + kspan;
		  k2 = k1 + kspan;
		  k3 = k2 + kspan;
		  k4 = k3 + kspan;
		  akp = Re_Data (k1) + Re_Data (k4);
		  akm = Re_Data (k1) - Re_Data (k4);
		  bkp = Im_Data (k1) + Im_Data (k4);
		  bkm = Im_Data (k1) - Im_Data (k4);
		  ajp = Re_Data (k2) + Re_Data (k3);
		  ajm = Re_Data (k2) - Re_Data (k3);
		  bjp = Im_Data (k2) + Im_Data (k3);
		  bjm = Im_Data (k2) - Im_Data (k3);
		  aa = Re_Data (kk);
		  bb = Im_Data (kk);
		  Re_Data (kk) = aa + akp + ajp;
		  Im_Data (kk) = bb + bkp + bjp;
		  ak = akp * c72 + ajp * c2 + aa;
		  bk = bkp * c72 + bjp * c2 + bb;
		  aj = akm * s72 + ajm * s2;
		  bj = bkm * s72 + bjm * s2;
		  Re_Data (k1) = ak - bj;
		  Re_Data (k4) = ak + bj;
		  Im_Data (k1) = bk + aj;
		  Im_Data (k4) = bk - aj;
		  ak = akp * c2 + ajp * c72 + aa;
		  bk = bkp * c2 + bjp * c72 + bb;
		  aj = akm * s2 - ajm * s72;
		  bj = bkm * s2 - bjm * s72;
		  Re_Data (k2) = ak - bj;
		  Re_Data (k3) = ak + bj;
		  Im_Data (k2) = bk + aj;
		  Im_Data (k3) = bk - aj;
		  kk = k4 + kspan;
	       } while (kk < nn);
	       kk -= nn;
	    } while (kk <= kspan);
	    break;

	  default:
	    k = factor [ii - 1];
	    if (jf != k)
	      {
		 jf = k;
		 s1 = pi2 / (double) jf;
		 c1 = cos (s1);
		 s1 = sin (s1);
		 if (jf > maxFactors)
		   goto Memory_Error;
		 Cos [jf - 1] = 1.0;
		 Sin [jf - 1] = 0.0;
		 j = 1;
		 do {
		    Cos [j - 1] = Cos [k - 1] * c1 + Sin [k - 1] * s1;
		    Sin [j - 1] = Cos [k - 1] * s1 - Sin [k - 1] * c1;
		    k--;
		    Cos [k - 1] = Cos [j - 1];
		    Sin [k - 1] = -Sin [j - 1];
		    j++;
		 } while (j < k);
	      }
	    do {
	       do {
		  REAL aa, ak;
		  REAL bb, bk;
		  int k2;

		  aa = ak = Re_Data (kk);
		  bb = bk = Im_Data (kk);

		  k1 = kk;
		  k2 = kk + ispan;
		  j = 1;
		  k1 += kspan;
		  do {
		     k2 -= kspan;
		     Rtmp [j] = Re_Data (k1) + Re_Data (k2);
		     ak += Rtmp [j];
		     Itmp [j] = Im_Data (k1) + Im_Data (k2);
		     bk += Itmp [j];
		     j++;
		     Rtmp [j] = Re_Data (k1) - Re_Data (k2);
		     Itmp [j] = Im_Data (k1) - Im_Data (k2);
		     j++;
		     k1 += kspan;
		  } while (k1 < k2);
		  Re_Data (kk) = ak;
		  Im_Data (kk) = bk;

		  k1 = kk;
		  k2 = kk + ispan;
		  j = 1;
		  do {
		     REAL aj = 0.0;
		     REAL bj = 0.0;

		     k1 += kspan;
		     k2 -= kspan;
		     jj = j;
		     ak = aa;
		     bk = bb;
		     k = 1;
		     do {
			ak += Rtmp [k] * Cos [jj - 1];
			bk += Itmp [k] * Cos [jj - 1];
			k++;
			aj += Rtmp [k] * Sin [jj - 1];
			bj += Itmp [k] * Sin [jj - 1];
			k++;
			jj += j;
			if (jj > jf)
			  jj -= jf;
		     } while (k < jf);
		     k = jf - j;
		     Re_Data (k1) = ak - bj;
		     Im_Data (k1) = bk + aj;
		     Re_Data (k2) = ak + bj;
		     Im_Data (k2) = bk - aj;
		     j++;
		  } while (j < k);
		  kk += ispan;
	       } while (kk <= nn);
	       kk -= nn;
	    } while (kk <= kspan);
	    break;
	 }
	 /* multiply by rotation factor (except for factors of 2 and 4) */
	 if (ii == nFactor)
	   goto Permute_Results;	/* exit infinite loop */
	 kk = jc + 1;
	 do {
	    c2 = 1.0 - cd;
	    s1 = sd;
	    do {
	       c1 = c2;
	       s2 = s1;
	       kk += kspan;
	       do {
		  REAL tmp;
		  do {
		     REAL ak;
		     ak = Re_Data (kk);
		     Re_Data (kk) = c2 * ak - s2 * Im_Data (kk);
		     Im_Data (kk) = s2 * ak + c2 * Im_Data (kk);
		     kk += ispan;
		  } while (kk <= nt);
		  tmp = s1 * s2;
		  s2 = s1 * c2 + c1 * s2;
		  c2 = c1 * c2 - tmp;
		  kk = kk - nt + kspan;
	       } while (kk <= ispan);
	       c2 = c1 - (cd * c1 + sd * s1);
	       s1 += sd * c1 - cd * s1;
	       c1 = 2.0 - (c2 * c2 + s1 * s1);
	       s1 *= c1;
	       c2 *= c1;
	       kk = kk - ispan + jc;
	    } while (kk <= kspan);
	    kk = kk - kspan + jc + inc;
	 } while (kk <= jc + jc);
	 break;
#endif	/* FFT_RADIX4 */
      }
   }

/* permute the results to normal order -- done in two stages */
/* permutation for square factors of n */
Permute_Results:
   Perm [0] = ns;
   if (kt)
     {
	int k2;

	k = kt + kt + 1;
	if (k > nFactor)
	  k--;
	Perm [k] = jc;
	j = 1;
	do {
	   Perm [j] = Perm [j - 1] / factor [j - 1];
	   Perm [k - 1] = Perm [k] * factor [j - 1];
	   j++;
	   k--;
	} while (j < k);
	k3 = Perm [k];
	kspan = Perm [1];
	kk = jc + 1;
	k2 = kspan + 1;
	j = 1;
	if (nPass != nTotal)
	  {
	     /* permutation for multivariate transform */
	     Permute_Multi:
	     do {
		do {
		   k = kk + jc;
		   do {
		      /* swap
		       * Re_Data (kk) <> Re_Data (k2)
		       * Im_Data (kk) <> Im_Data (k2)
		       */
		      REAL tmp;
		      tmp = Re_Data (kk); Re_Data (kk) = Re_Data (k2); Re_Data (k2) = tmp;
		      tmp = Im_Data (kk); Im_Data (kk) = Im_Data (k2); Im_Data (k2) = tmp;
		      kk += inc;
		      k2 += inc;
		   } while (kk < k);
		   kk += (ns - jc);
		   k2 += (ns - jc);
		} while (kk < nt);
		k2 = k2 - nt + kspan;
		kk = kk - nt + jc;
	     } while (k2 < ns);
	     do {
		do {
		   k2 -= Perm [j - 1];
		   j++;
		   k2 = Perm [j] + k2;
		} while (k2 > Perm [j - 1]);
		j = 1;
		do {
		   if (kk < k2)
		     goto Permute_Multi;
		   kk += jc;
		   k2 += kspan;
		} while (k2 < ns);
	     } while (kk < ns);
	  }
	else
	  {
	     /* permutation for single-variate transform (optional code) */
Permute_Single:
	     do {
		/* swap
		 * Re_Data (kk) <> Re_Data (k2)
		 * Im_Data (kk) <> Im_Data (k2)
		 */
		REAL t;
		t = Re_Data (kk); Re_Data (kk) = Re_Data (k2); Re_Data (k2) = t;
		t = Im_Data (kk); Im_Data (kk) = Im_Data (k2); Im_Data (k2) = t;
		kk += inc;
		k2 += kspan;
	     } while (k2 < ns);
	     do {
		do {
		   k2 -= Perm [j - 1];
		   j++;
		   k2 = Perm [j] + k2;
		} while (k2 > Perm [j - 1]);
		j = 1;
		do {
		   if (kk < k2)
		     goto Permute_Single;
		   kk += inc;
		   k2 += kspan;
		} while (k2 < ns);
	     } while (kk < ns);
	  }
	jc = k3;
     }

   if ((kt << 1) + 1 >= nFactor)
     return 0;
   ispan = Perm [kt];

   /* permutation for square-free factors of n */
   j = nFactor - kt;
   factor [j] = 1;
   do {
      factor [j - 1] *= factor [j];
      j--;
   } while (j != kt);
   nn = factor [kt] - 1;
   kt++;
   if (nn > maxPerm)
     goto Memory_Error;

   j = jj = 0;
   for (;;) {
      int k2;

      k = kt + 1;
      k2 = factor [kt - 1];
      kk = factor [k - 1];
      j++;
      if (j > nn)
	break;				/* exit infinite loop */
      jj += kk;
      while (jj >= k2)
	{
	   jj -= k2;
	   k2 = kk;
	   kk = factor [k++];
	   jj += kk;
	}
      Perm [j - 1] = jj;
   }
   /* determine the permutation cycles of length greater than 1 */
   j = 0;
   for (;;) {
      do {
	 kk = Perm [j++];
      } while (kk < 0);
      if (kk != j)
	{
	   do {
	      k = kk;
	      kk = Perm [k - 1];
	      Perm [k - 1] = -kk;
	   } while (kk != j);
	   k3 = kk;
	}
      else
	{
	   Perm [j - 1] = -j;
	   if (j == nn)
	     break;		/* exit infinite loop */
	}
   }

   maxFactors *= inc;

   /* reorder a and b, following the permutation cycles */
   for (;;) {
      j = k3 + 1;
      nt -= ispan;
      ii = nt - inc + 1;
      if (nt < 0)
	break;			/* exit infinite loop */
      do {
	 do {
	    j--;
	 } while (Perm [j - 1] < 0);
	 jj = jc;
	 do {
	    int k2;

	    if (jj < maxFactors) kspan = jj; else kspan = maxFactors;

	    jj -= kspan;
	    k = Perm [j - 1];
	    kk = jc * k + ii + jj;

	    k1 = kk + kspan;
	    k2 = 0;
	    do {
	       Rtmp [k2] = Re_Data (k1);
	       Itmp [k2] = Im_Data (k1);
	       k2++;
	       k1 -= inc;
	    } while (k1 != kk);

	    do {
	       k1 = kk + kspan;
	       k2 = k1 - jc * (k + Perm [k - 1]);
	       k = -Perm [k - 1];
	       do {
		  Re_Data (k1) = Re_Data (k2);
		  Im_Data (k1) = Im_Data (k2);
		  k1 -= inc;
		  k2 -= inc;
	       } while (k1 != kk);
	       kk = k2;
	    } while (k != j);

	    k1 = kk + kspan;
	    k2 = 0;
	    do {
	       Re_Data (k1) = Rtmp [k2];
	       Im_Data (k1) = Itmp [k2];
	       k2++;
	       k1 -= inc;
	    } while (k1 != kk);
	 } while (jj);
      } while (j != 1);
   }
   return 0;			/* exit point here */

   /* alloc or other problem, do some clean-up */
Memory_Error:
   fprintf (stderr, "Error: " FFTRADIXS "() - insufficient memory.\n");
   fft_free ();			/* free-up memory */
   return -1;
}
예제 #9
0
파일: fftn.c 프로젝트: xelmirage/libpano
int
FFTN (int ndim,
      const unsigned int dims [],
      REAL Re [],
      REAL Im [],
      int iSign,
      double scaling)
{
   unsigned int nTotal;
   unsigned int maxFactors, maxPerm;

   /*
    * tally the number of elements in the data array
    * and determine the number of dimensions
    */
   nTotal = 1;
   if (ndim)
     {
	if (dims != NULL)
	  {
	     int i;
	     /* number of dimensions was specified */
	     for (i = 0; i < ndim; i++)
	       {
		  if (dims [i] <= 0) goto Dimension_Error;
		  nTotal *= dims [i];
	       }
	  }
	else
	  nTotal *= ndim;
     }
   else
     {
	int i;
	/* determine # of dimensions from zero-terminated list */
	if (dims == NULL) goto Dimension_Error;
	for (ndim = i = 0; dims [i]; i++)
	  {
	     if (dims [i] <= 0)
	       goto Dimension_Error;
	     nTotal *= dims [i];
	     ndim++;
	  }
     }

   /* determine maximum number of factors and permuations */
#if 1
   /*
    * follow John Beale's example, just use the largest dimension and don't
    * worry about excess allocation.  May be someone else will do it?
    */
   if (dims != NULL)
     {
	int i;
	for (maxFactors = maxPerm = 1, i = 0; i < ndim; i++)
	  {
	     if (dims [i] > maxFactors) maxFactors = dims [i];
	     if (dims [i] > maxPerm) maxPerm = dims [i];
	  }
     }
   else
     {
	maxFactors = maxPerm = nTotal;
     }
#else
   /* use the constants used in the original Fortran code */
   maxFactors = 23;
   maxPerm = 209;
#endif
   /* loop over the dimensions: */
   if (dims != NULL)
     {
	unsigned int nSpan = 1;
	int i;

	for (i = 0; i < ndim; i++)
	  {
	     int ret;
	     nSpan *= dims [i];
	     ret = FFTRADIX (Re, Im, nTotal, dims [i], nSpan, iSign,
			     maxFactors, maxPerm);
	     /* exit, clean-up already done */
	     if (ret)
	       return ret;
          }
     }
   else
     {
	int ret;
	ret = FFTRADIX (Re, Im, nTotal, nTotal, nTotal, iSign,
			maxFactors, maxPerm);
	/* exit, clean-up already done */
	if (ret)
	  return ret;
     }

   /* Divide through by the normalizing constant: */
   if (scaling && scaling != 1.0)
     {
	size_t ist;

	if (iSign < 0) iSign = -iSign;
	if (scaling < 0.0)
	  scaling = (scaling < -1.0) ? sqrt (nTotal) : nTotal;
	scaling = 1.0 / scaling;	/* multiply is often faster */
	for (ist = 0; ist < nTotal; ist += iSign)
	  {
	     Re_Data (ist) *= scaling;
	     Im_Data (ist) *= scaling;
	  }
     }
   return 0;

   Dimension_Error:
   fprintf (stderr, "Error: " FFTNS "() - dimension error\n");
   fft_free ();	/* free-up memory */
   return -1;
}
예제 #10
0
파일: fft.c 프로젝트: Markusjsommer/bart
void fft2(unsigned int D, const long dimensions[D], unsigned long flags, const long ostrides[D], complex float* dst, const long istrides[D], const complex float* src)
{
	const struct operator_s* plan = fft_create2(D, dimensions, flags, ostrides, dst, istrides, src, false);
	fft_exec(plan, dst, src);
	fft_free(plan);
}
예제 #11
0
파일: fft.c 프로젝트: Markusjsommer/bart
void ifft(unsigned int D, const long dimensions[D], unsigned long flags, complex float* dst, const complex float* src)
{
	const struct operator_s* plan = fft_create(D, dimensions, flags, dst, src, true);
	fft_exec(plan, dst, src);
	fft_free(plan);
}
예제 #12
0
파일: fmm_kottke.cpp 프로젝트: soamaven/S4
int FMMGetEpsilon_Kottke(const S4_Simulation *S, const S4_Layer *L, const int n, std::complex<double> *Epsilon2, std::complex<double> *Epsilon_inv){
	const int n2 = 2*n;
	const int *G = S->G;

	// Make grid
	// Determine size of the grid
	int ngrid[2] = {1,1};
	for(int i = 0; i < 2; ++i){ // choose grid size
		for(int j = 0; j < n; ++j){
			if(abs(G[2*j+i]) > ngrid[i]){ ngrid[i] = abs(G[2*j+i]); }
		}
		if(ngrid[i] < 1){ ngrid[i] = 1; }
		ngrid[i] *= S->options.resolution;
		ngrid[i] = fft_next_fast_size(ngrid[i]);
	}
	S4_TRACE("I  Subpixel smoothing on %d x %d grid\n", ngrid[0], ngrid[1]);
	const int ng2 = ngrid[0]*ngrid[1];
	const double ing2 = 1./(double)ng2;
	// The grid needs to hold 5 matrix elements: xx,xy,yx,yy,zz
	// We actually make 5 different grids to facilitate the fft routines

	//std::complex<double> *work = (std::complex<double>*)S4_malloc(sizeof(std::complex<double>)*(6*ng2));
	std::complex<double> *work = fft_alloc_complex(6*ng2);
	std::complex<double>*fxx = work;
	std::complex<double>*fxy = fxx + ng2;
	std::complex<double>*fyx = fxy + ng2;
	std::complex<double>*fyy = fyx + ng2;
	std::complex<double>*fzz = fyy + ng2;
	std::complex<double>*Fto = fzz + ng2;
//memset(work, 0, sizeof(std::complex<double>) * 6*ng2);
	double *discval = (double*)S4_malloc(sizeof(double)*(L->pattern.nshapes+1));

	fft_plan plans[5];
	for(int i = 0; i <= 4; ++i){
		plans[i] = fft_plan_dft_2d(ngrid, fxx+i*ng2, Fto, 1);
	}

	int ii[2];
	for(ii[0] = 0; ii[0] < ngrid[0]; ++ii[0]){
		const int si0 = ii[0] >= ngrid[0]/2 ? ii[0]-ngrid[0]/2 : ii[0]+ngrid[0]/2;
		for(ii[1] = 0; ii[1] < ngrid[1]; ++ii[1]){
			const int si1 = ii[1] >= ngrid[1]/2 ? ii[1]-ngrid[1]/2 : ii[1]+ngrid[1]/2;
			Pattern_DiscretizeCell(&L->pattern, S->Lr, ngrid[0], ngrid[1], ii[0], ii[1], discval);
			int nnz = 0;
			int imat[2] = {-1,-1};
			for(int i = 0; i <= L->pattern.nshapes; ++i){
				if(fabs(discval[i]) > 2*std::numeric_limits<double>::epsilon()){
					if(0 == nnz){ imat[0] = i; }
					else if(1 == nnz){ imat[1] = i; }
					++nnz;
				}
			}
//S4_TRACE("I   %d,%d nnz = %d\n", ii[0], ii[1], nnz);

			if(nnz < 2){ // just one material
//fprintf(stderr, "%d\t%d\t0\t0\n", ii[0], ii[1]);
				const S4_Material *M;
				if(0 == imat[0]){
					M = &S->material[L->material];
				}else{
					M = &S->material[L->pattern.shapes[imat[0]-1].tag];
				}
				if(0 == M->type){
					std::complex<double> eps_scalar(M->eps.s[0], M->eps.s[1]);
					fxx[si1+si0*ngrid[1]] = eps_scalar;
					fxy[si1+si0*ngrid[1]] = 0;
					fyx[si1+si0*ngrid[1]] = 0;
					fyy[si1+si0*ngrid[1]] = eps_scalar;
					fzz[si1+si0*ngrid[1]] = eps_scalar;
				}else{
					fxx[si1+si0*ngrid[1]] = std::complex<double>(M->eps.abcde[0],M->eps.abcde[1]);
					fyy[si1+si0*ngrid[1]] = std::complex<double>(M->eps.abcde[6],M->eps.abcde[7]);
					fxy[si1+si0*ngrid[1]] = std::complex<double>(M->eps.abcde[2],M->eps.abcde[3]);
					fyx[si1+si0*ngrid[1]] = std::complex<double>(M->eps.abcde[4],M->eps.abcde[5]);
					fzz[si1+si0*ngrid[1]] = std::complex<double>(M->eps.abcde[8],M->eps.abcde[9]);
				}
			}else{
				if(imat[1] > imat[0]){
					std::swap(imat[0],imat[1]);
				}
				// imat[0] is the more-contained shape

				double nvec[2];
				const double nxvec[2] = { ((double)ii[0]+0.5)/(double)ngrid[0]-0.5, ((double)ii[1]+0.5)/(double)ngrid[1]-0.5 };
				const double xvec[2] = { // center of current parallelogramic pixel
					S->Lr[0]*nxvec[0] + S->Lr[2]*nxvec[1],
					S->Lr[1]*nxvec[0] + S->Lr[3]*nxvec[1]
				};
				shape_get_normal(&(L->pattern.shapes[imat[0]-1]), xvec, nvec);

				if(2 == nnz && imat[1] != imat[0] && !(0 == nvec[0] && 0 == nvec[1])){ // use Kottke averaging
//fprintf(stderr, "%d\t%d\t%f\t%f\n", ii[0], ii[1], nvec[0], nvec[1]);
					const double fill = discval[imat[0]];

					// Get the two tensors
					std::complex<double> abcde[2][5];
					for(int i = 0; i < 2; ++i){
						const S4_Material *M;
						if(0 == imat[i]){
							M = &S->material[L->material];
						}else{
							M = &S->material[L->pattern.shapes[imat[i]-1].tag];
						}
						if(0 == M->type){
							std::complex<double> eps_scalar(M->eps.s[0], M->eps.s[1]);
							abcde[i][0] = eps_scalar;
							abcde[i][1] = 0;
							abcde[i][2] = 0;
							abcde[i][3] = eps_scalar;
							abcde[i][4] = eps_scalar;
						}else{
							for(int j = 0; j < 4; ++j){
								abcde[i][j] = std::complex<double>(M->eps.abcde[2*j+0],M->eps.abcde[2*j+1]);
							}
							abcde[i][4] = std::complex<double>(M->eps.abcde[8],M->eps.abcde[9]);
						}
					}
					// The zz component is just directly obtained by averaging
					fzz[si1+si0*ngrid[1]] = discval[imat[0]] * abcde[0][4] + discval[imat[1]] * abcde[1][4];

					// Compute rotated tensors
					// abcd1 = Rot^T abcd1 Rot
					// Rot = [ nvec[0] -nvec[1] ]
					//       [ nvec[1]  nvec[0] ]
					std::complex<double> abcd[2][4];
//fprintf(stderr, " nvec = {%f,%f}, fill = %f\n", nvec[0], nvec[1], fill);
//fprintf(stderr, " abcde[0] = {%f,%f,%f,%f}\n", abcde[0][0].real(), abcde[0][1].real(), abcde[0][2].real(), abcde[0][3].real());
//fprintf(stderr, " abcde[1]  = {%f,%f,%f,%f}\n", abcde[1][0].real(), abcde[1][1].real(), abcde[1][2].real(), abcde[1][3].real());
					sym2x2rot(abcde[0], nvec, abcd[0]);
					sym2x2rot(abcde[1], nvec, abcd[1]);

					// Compute the average tau tensor into abcde[0][0-3]
					// tau(e__) = [ -1/e11         e12/e11      ]
					//            [ e21/e11   e22 - e21 e12/e11 ]
					abcde[0][0] = fill * (-1./abcd[0][0]) + (1.-fill) * (-1./abcd[1][0]);
					abcde[0][1] = fill * (abcd[0][1]/abcd[0][0]) + (1.-fill) * (abcd[1][1]/abcd[1][0]);
					abcde[0][2] = fill * (abcd[0][2]/abcd[0][0]) + (1.-fill) * (abcd[1][2]/abcd[1][0]);
					abcde[0][3] = fill * (abcd[0][3]-abcd[0][1]*abcd[0][2]/abcd[0][0]) + (1.-fill) * (abcd[1][3]-abcd[1][1]*abcd[1][2]/abcd[1][0]);

					// Invert the tau transform into abcd[1]
					// invtau(t__) = [ -1/t11         -t12/t11      ]
					//               [ -t21/t11   t22 - t21 t12/t11 ]
					abcd[1][0] = -1./abcde[0][0];
					abcd[1][1] = -abcde[0][1]/abcde[0][0];
					abcd[1][2] = -abcde[0][2]/abcde[0][0];
					abcd[1][3] = abcde[0][3] - abcde[0][1]*abcde[0][2]/abcde[0][0];
//fprintf(stderr, " abcd[1]  = {%f,%f,%f,%f}\n", abcd[1][0].real(), abcd[1][1].real(), abcd[1][2].real(), abcd[1][3].real());

					// Unrotate abcd[1] into abcd[0]
					nvec[1] = -nvec[1];
					sym2x2rot(abcd[1], nvec, abcd[0]);
//fprintf(stderr, " abcd[0] = {%f,%f,%f,%f}\n\n", abcd[0][0].real(), abcd[0][1].real(), abcd[0][2].real(), abcd[0][3].real());

					fxx[si1+si0*ngrid[1]] = abcd[0][0];
					fxy[si1+si0*ngrid[1]] = abcd[0][1];
					fyx[si1+si0*ngrid[1]] = abcd[0][2];
					fyy[si1+si0*ngrid[1]] = abcd[0][3];
				}else{ // too many, just use the area weighting
//fprintf(stderr, "%d\t%d\t3\n", ii[0], ii[1]);
					fxx[si1+si0*ngrid[1]] = 0;
					fxy[si1+si0*ngrid[1]] = 0;
					fyx[si1+si0*ngrid[1]] = 0;
					fyy[si1+si0*ngrid[1]] = 0;
					fzz[si1+si0*ngrid[1]] = 0;
					for(int i = 0; i <= L->pattern.nshapes; ++i){
						if(0 == discval[i]){ continue; }
						int j = i-1;
						const S4_Material *M;
						if(-1 == j){
							M = &S->material[L->material];
						}else{
							M = &S->material[L->pattern.shapes[j].tag];
						}
						if(0 == M->type){
							std::complex<double> eps_scalar(M->eps.s[0], M->eps.s[1]);
							fxx[si1+si0*ngrid[0]] += discval[i]*eps_scalar;
							fyy[si1+si0*ngrid[0]] += discval[i]*eps_scalar;
							fzz[si1+si0*ngrid[0]] += discval[i]*eps_scalar;
						}else{
							std::complex<double> ea(M->eps.abcde[0],M->eps.abcde[1]);
							std::complex<double> eb(M->eps.abcde[2],M->eps.abcde[3]);
							std::complex<double> ec(M->eps.abcde[4],M->eps.abcde[5]);
							std::complex<double> ed(M->eps.abcde[6],M->eps.abcde[7]);
							fxx[si1+si0*ngrid[1]] += discval[i]*ea;
							fxy[si1+si0*ngrid[1]] += discval[i]*eb;
							fyx[si1+si0*ngrid[1]] += discval[i]*ec;
							fyy[si1+si0*ngrid[1]] += discval[i]*ed;
							fzz[si1+si0*ngrid[1]] += discval[i]*std::complex<double>(M->eps.abcde[8],M->eps.abcde[9]);
						}
					}
				}
			}
/*
fprintf(stderr, "%d\t%d\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\n", ii[0], ii[1],
fxx[si1+si0*ngrid[1]].real(), fxx[si1+si0*ngrid[1]].imag(),
fxy[si1+si0*ngrid[1]].real(), fxy[si1+si0*ngrid[1]].imag(),
fyx[si1+si0*ngrid[1]].real(), fyx[si1+si0*ngrid[1]].imag(),
fyy[si1+si0*ngrid[1]].real(), fyy[si1+si0*ngrid[1]].imag(),
fzz[si1+si0*ngrid[1]].real(), fzz[si1+si0*ngrid[1]].imag()
);//*/
		}
//fprintf(stderr, "\n");
	}

	// Make Epsilon_inv first
	{
//fprintf(stderr, "fzz1[0] = %f+I %f\n", fzz[0].real(), fzz[0].imag());
//memset(Fto, 0, sizeof(std::complex<double>)*ng2);
		fft_plan_exec(plans[4]);
//fprintf(stderr, "fzz2[0] = %f+I %f\n", fzz[0].real(), fzz[0].imag());
//fprintf(stderr, "Fto[0] = %f+I %f\n", Fto[0].real(), Fto[0].imag());
		for(int j = 0; j < n; ++j){
			for(int i = 0; i < n; ++i){
				int f[2] = {G[2*i+0]-G[2*j+0],G[2*i+1]-G[2*j+1]};
				if(f[0] < 0){ f[0] += ngrid[0]; }
				if(f[1] < 0){ f[1] += ngrid[1]; }
				Epsilon2[i+j*n] = ing2 * Fto[f[1]+f[0]*ngrid[1]];
			}
		}
	}
//fprintf(stderr, "Epsilon2[0] = %f+I %f\n", Epsilon2[0].real(), Epsilon2[0].imag());
	// Epsilon_inv needs inverting
	RNP::TBLAS::SetMatrix<'A'>(n,n, 0.,1., Epsilon_inv,n);
	int solve_info;
	RNP::LinearSolve<'N'>(n,n, Epsilon2,n, Epsilon_inv,n, &solve_info, NULL);
//fprintf(stderr, "Epsilon_inv[0] = %f+I %f\n", Epsilon_inv[0].real(), Epsilon_inv[0].imag());

	// We fill in the quarter blocks of F in Fortran order
	for(int w = 0; w < 4; ++w){
		int Ecol = (w&1 ? n : 0);
		int Erow = (w&2 ? n : 0);
//memset(Fto, 0, sizeof(std::complex<double>)*ng2);
		fft_plan_exec(plans[w]);

//fprintf(stderr, "Fto(%d)[0] = %f+I %f\n", w, Fto[0].real(), Fto[0].imag());
		for(int j = 0; j < n; ++j){
			for(int i = 0; i < n; ++i){
				int f[2] = {G[2*i+0]-G[2*j+0],G[2*i+1]-G[2*j+1]};
				if(f[0] < 0){ f[0] += ngrid[0]; }
				if(f[1] < 0){ f[1] += ngrid[1]; }
				Epsilon2[Erow+i+(Ecol+j)*n2] = ing2 * Fto[f[1]+f[0]*ngrid[1]];
			}
		}
	}
//fprintf(stderr, "Epsilon2[0] = %f+I %f\n", Epsilon2[0].real(), Epsilon2[0].imag());
	for(int i = 0; i <= 4; ++i){
		fft_plan_destroy(plans[i]);
	}

	S4_free(discval);
	//S4_free(work);
	fft_free(work);

	return 0;
}
예제 #13
0
void vspectra(void)
{

    int i, j, k, kk, num, numm, i4, maxi, r;
    int m, mm, blsiz, blsiz2, nsam, n_read;
    double avsig, av, max, min, noise, wid;
    uint8_t *bufferRead = malloc((NSAM) * sizeof(uint8_t));
    static double vspec[NSPEC];
    static int wtt[NSPEC];
    static double re[NSPEC * 2], am[NSPEC * 2];
    double smax;
    fftwf_plan p0;
    float *reamin0, *reamout0;


    blsiz = NSPEC * 2;
    blsiz2 = blsiz / 2;
    d1.bw = 2.4;                // fixed 2.4 for TV dongle 10 MHz for ADC card
    if (d1.fbw == 0.0)
        d1.fbw = 2.0;           //  use bandwidth default if not set in srt.cat
    d1.f1 = 0.5 - (d1.fbw / d1.bw) * 0.5;
    d1.f2 = 0.5 + (d1.fbw / d1.bw) * 0.5;
    d1.fc = (d1.f1 + d1.f2) * 0.5;
    d1.lofreq = 0;              // not used but needs to be set to zero to flag the use of the dongle 
    d1.efflofreq = d1.freq - d1.bw * 0.5;

    if (!d1.fftsim) {
        fft_init(blsiz2, &p0, &reamin0, &reamout0);
    }

    num = d1.nblk;              //was 20   // was 100
    nsam = NSAM;
    d1.nsam = NSAM * num;
    avsig = 0;
    numm = 0;
    smax = 0;
    max = -1e99;
    min = 1e99;
    for (i = 0; i < blsiz2; i++)
        vspec[i] = 0.0;
    for (k = 0; k < num; k++) {
        if (!d1.radiosim)
// Read the raw data from the RTL Dongle
            r = rtlsdr_read_sync(dev, bufferRead, nsam, &n_read);
        else {
            av = 5.0;
            if (d1.elnow < 5.0)
                av = av * (d1.tsys + d1.tcal) / d1.tsys;
            if (strstr(soutrack, "Sun")) {
                av = sqrt(d1.eloff * d1.eloff +
                          d1.azoff * d1.azoff * cos(d1.elnow * PI / 180.0) * cos(d1.elnow * PI / 180.0) +
                          1e-6);
                if (av > d1.beamw)
                    av = d1.beamw;
                av = 5.0 + 25.0 * cos(av * PI * 0.5 / d1.beamw) * cos(av * PI * 0.5 / d1.beamw);
            }
            for (i = 0; i < nsam; i++)
                bufferRead[i] = (uint8_t) (sqrt(av) * gauss() + 127.397 + 0.5 + 0 * sin(2.0 * PI * 0.5 * (i / 2) / d1.bw)); // simulate data
        }

//   for(i=0;i<nsam;i+=0x10000)  printf("%d %f\n",i,(double)(bufferRead[i]-127.397));
        for (kk = 0; kk < nsam / blsiz; kk++) {
            if (d1.fftsim)
                for (i = 0; i < blsiz2; i++) {
                    re[i] = (double) (bufferRead[2 * i + kk * blsiz] - 127.397);
                    am[i] = (double) (bufferRead[2 * i + 1 + kk * blsiz] - 127.397);
                    if (re[i] > smax)
                        smax = re[i];
            } else
                for (i = 0; i < blsiz2; i++) {
                    reamin0[2 * i] = (float) (bufferRead[2 * i + kk * blsiz] - 127.397);
                    reamin0[2 * i + 1] = (float) (bufferRead[2 * i + 1 + kk * blsiz] - 127.397);
                    if (reamin0[2 * i] > smax)
                        smax = reamin0[2 * i];
                }

            if (d1.fftsim)
                Four(re, am, blsiz2);
            else {
                cfft(&p0);
                for (i = 0; i < blsiz2; i++) {
                    re[i] = reamout0[2 * i];
                    am[i] = reamout0[2 * i + 1];
                }
            }
// for(i = 0; i < blsiz2; i++) if(re[i] > max) max=re[i];
// for(i = 0; i < blsiz2; i++) if(re[i] < min) min=re[i];
            for (i = 0; i < blsiz2; i++) {
                if (i < blsiz2 / 2)
                    j = i + blsiz2 / 2;
                else
                    j = i - blsiz2 / 2;
                vspec[j] += re[i] * re[i] + am[i] * am[i];
                numm++;
            }
        }
    }
//   printf("max %f min %f\n",max,min);
    max = av = 0;
    maxi = 0;
    for (i = 0; i < blsiz2; i++)
        wtt[i] = 1;
    if (numm > 0) {
        if (d1.nfreq == blsiz2) {
            for (i = 0; i < blsiz2; i++) {
                if (i > 10)
                    spec[i] = vspec[i] / (double) numm;
                else
                    spec[i] = 0;
            }
        } else {
            m = blsiz2 / d1.nfreq;
            for (i = 0; i < d1.nrfi; i++) {
                i4 = (d1.rfi[i] - d1.freq + d1.bw * 0.5) * blsiz2 / d1.bw + 0.5; // index of rfi MHz 
                wid = 0.5 * d1.rfiwid[i] / (d1.bw / NSPEC);
                for (j = -wid; j <= wid; j++)
                    if ((i4 + j) >= 0 && (i4 + j) < blsiz2)
                        wtt[i4 + j] = 0;
            }
            for (j = 0; j < d1.nfreq; j++) {
                av = mm = 0;
                for (i = j * m - m / 2; i <= j * m + m / 2; i++) {
                    if (i > 10 && i < blsiz2 && wtt[i]) { // wtt=0 removal of spurs
                        av += vspec[i] / (double) numm;
                        if (vspec[i] > max) {
                            max = vspec[i];
                            maxi = i;
                        }
                        mm++;
                    }
                }
                if (mm > 0)
                    spec[j] = av / mm;
                else {
                    spec[j] = 0;
                    if (j > 10)
                        printf("check RFI settings in srt.cat data deleted at %8.3f\n",
                               j * d1.bw / d1.nfreq + d1.freq - d1.bw * 0.5);
                }
            }
            max = max / (double) numm;
            noise = spec[maxi / m] * sqrt(2.0 * blsiz2 / (double) d1.nsam);
            if (max > spec[maxi / m] + d1.rfisigma * noise && d1.printout) // rfisigma sigma
                printf("check for RFI at %8.4f MHz max %5.0e av %5.0e smax %5.0f %3.0f sigma\n",
                       maxi * d1.bw / blsiz2 + d1.freq - d1.bw * 0.5, max, spec[maxi / m], smax,
                       (max - spec[maxi / m]) / noise);
        }
    }
    d1.smax = smax;
    if (!d1.fftsim)
        fft_free(&p0, &reamin0, &reamout0);
    free(bufferRead);
}
예제 #14
0
파일: eq.c 프로젝트: AlexFolland/RetroArch
static void create_filter(struct eq_data *eq, unsigned size_log2,
      struct eq_gain *gains, unsigned num_gains, double beta, const char *filter_path)
{
   int i;
   int half_block_size = eq->block_size >> 1;
   double window_mod = 1.0 / kaiser_window_function(0.0, beta);

   fft_t *fft = fft_new(size_log2);
   float *time_filter = (float*)calloc(eq->block_size * 2 + 1, sizeof(*time_filter));
   if (!fft || !time_filter)
      goto end;

   /* Make sure bands are in correct order. */
   qsort(gains, num_gains, sizeof(*gains), gains_cmp);

   /* Compute desired filter response. */
   generate_response(eq->filter, gains, num_gains, half_block_size);

   /* Get equivalent time-domain filter. */
   fft_process_inverse(fft, time_filter, eq->filter, 1);

   // ifftshift() to create the correct linear phase filter.
   // The filter response was designed with zero phase, which won't work unless we compensate
   // for the repeating property of the FFT here by flipping left and right blocks.
   for (i = 0; i < half_block_size; i++)
   {
      float tmp = time_filter[i + half_block_size];
      time_filter[i + half_block_size] = time_filter[i];
      time_filter[i] = tmp;
   }

   /* Apply a window to smooth out the frequency repsonse. */
   for (i = 0; i < (int)eq->block_size; i++)
   {
      /* Kaiser window. */
      double phase = (double)i / eq->block_size;
      phase = 2.0 * (phase - 0.5);
      time_filter[i] *= window_mod * kaiser_window_function(phase, beta);
   }

   /* Debugging. */
   if (filter_path)
   {
      FILE *file = fopen(filter_path, "w");
      if (file)
      {
         for (i = 0; i < (int)eq->block_size - 1; i++)
            fprintf(file, "%.8f\n", time_filter[i + 1]);
         fclose(file);
      }
   }

   /* Padded FFT to create our FFT filter.
    * Make our even-length filter odd by discarding the first coefficient.
    * For some interesting reason, this allows us to design an odd-length linear phase filter.
    */
   fft_process_forward(eq->fft, eq->filter, time_filter + 1, 1);

end:
   fft_free(fft);
   free(time_filter);
}
예제 #15
0
AUD_Int32s denoise_mmse( AUD_Int16s *pInBuf, AUD_Int16s *pOutBuf, AUD_Int32s inLen )
{
    Fft_16s        *hFft        = NULL;
    Ifft_16s       *hIfft       = NULL;
    AUD_Window16s  *hWin        = NULL;

    AUD_Int32s     frameSize    = 512;
    AUD_Int32s     frameStride  = 256;
    AUD_Int32s     frameOverlap = 256;
    AUD_Int32s     nFFT         = frameSize;
    AUD_Int32s     nSpecLen     = nFFT / 2 + 1;
    AUD_Int32s     nNoiseFrame  = (AUD_Int32s)( ( 0.25 * SAMPLE_RATE - frameSize ) / frameStride + 1 );;

    AUD_Int32s     i, j, k;

    AUD_Int32s     cleanLen = 0;

    // pre-emphasis
    // sig_preemphasis( pInBuf, pInBuf, inLen );

    // init hamming module
    win16s_init( &hWin, AUD_WIN_HAMM, frameSize, 14 );
    AUD_ASSERT( hWin );

    // init fft handle
    fft_init( &hFft, nFFT, 15 );
    AUD_ASSERT( hFft );

    // init ifft handle
    ifft_init( &hIfft, nFFT, 15 );
    AUD_ASSERT( hIfft );

    AUD_Int16s *pFrame  = (AUD_Int16s*)calloc( frameSize * sizeof(AUD_Int16s), 1 );
    AUD_ASSERT( pFrame );

    // FFT
    AUD_Int32s *pFFTMag  = (AUD_Int32s*)calloc( nFFT * sizeof(AUD_Int32s), 1 );
    AUD_ASSERT( pFFTMag );
    AUD_Int32s *pFFTRe   = (AUD_Int32s*)calloc( nFFT * sizeof(AUD_Int32s), 1 );
    AUD_ASSERT( pFFTRe );
    AUD_Int32s *pFFTIm   = (AUD_Int32s*)calloc( nFFT * sizeof(AUD_Int32s), 1 );
    AUD_ASSERT( pFFTIm );

    AUD_Int32s *pFFTCleanRe = (AUD_Int32s*)calloc( nFFT * sizeof(AUD_Int32s), 1 );
    AUD_ASSERT( pFFTCleanRe );
    AUD_Int32s *pFFTCleanIm = (AUD_Int32s*)calloc( nFFT * sizeof(AUD_Int32s), 1 );
    AUD_ASSERT( pFFTCleanIm );

    AUD_Double *pNoiseMean = (AUD_Double*)calloc( nSpecLen * sizeof(AUD_Double), 1 );
    AUD_ASSERT( pNoiseMean );
    AUD_Int32s *pFFTCleanMag = (AUD_Int32s*)calloc( nSpecLen * sizeof(AUD_Int32s), 1 );
    AUD_ASSERT( pFFTCleanMag );

    AUD_Int16s *pxOld   = (AUD_Int16s*)calloc( frameSize * sizeof(AUD_Int16s), 1 );
    AUD_ASSERT( pxOld );
    AUD_Int16s *pxClean = (AUD_Int16s*)calloc( frameSize * sizeof(AUD_Int16s), 1 );
    AUD_ASSERT( pxClean );


    for ( i = 0; (i < nNoiseFrame) && ( (i * frameStride + frameSize) < inLen ); i++ )
    {
        win16s_calc( hWin, pInBuf + i * frameSize, pFrame );

        fft_mag( hFft, pFrame, frameSize, pFFTMag );

        for ( j = 0; j < nSpecLen; j++ )
        {
            pNoiseMean[j] += (AUD_Double)pFFTMag[j];
        }
    }
    // compute noise mean
    for ( j = 0; j < nSpecLen; j++ )
    {
        pNoiseMean[j] /= nNoiseFrame;
    }

    AUD_Double thres = 3.;
    AUD_Double alpha = 2.;
    AUD_Double flr   = 0.002;
    AUD_Double G     = 0.9;
    AUD_Double snr, beta;
    AUD_Double tmp;
    AUD_Double sigEnergy, noiseEnergy;

    k = 0;
    // start processing
    for ( i = 0; (i * frameStride + frameSize) < inLen; i++ )
    {
        win16s_calc( hWin, pInBuf + i * frameStride, pFrame );

        fft_calc( hFft, pFrame, frameSize, pFFTRe, pFFTIm );

        // compute SNR
        sigEnergy   = 0.;
        noiseEnergy = 0.;
        for ( j = 0; j < nSpecLen; j++ )
        {
            tmp          = pFFTRe[j] / 32768. * pFFTRe[j] / 32768. + pFFTIm[j] / 32768. * pFFTIm[j] / 32768.;
            sigEnergy   += tmp;
            noiseEnergy += pNoiseMean[j] / 32768. * pNoiseMean[j] / 32768.;

            tmp = sqrt( tmp ) * 32768.;
            if ( tmp > MAX_INT32S )
            {
                AUD_ECHO
                pFFTMag[j] = MAX_INT32S;
            }
            else
            {
                pFFTMag[j] = (AUD_Int32s)round( tmp );
            }
        }

        snr  = 10. * log10( sigEnergy / noiseEnergy );
        beta = berouti( snr );

        // AUDLOG( "signal energy: %.2f, noise energy: %.2f, snr: %.2f, beta: %.2f\n", sigEnergy, noiseEnergy, snr, beta );

#if 0
        AUDLOG( "noisy FFT:\n" );
        for ( j = 0; j < nSpecLen; j++ )
        {
            AUDLOG( "%d, ", pFFTMag[j] );
        }
        AUDLOG( "\n" );
#endif

        tmp = 0.;
        for ( j = 0; j < nSpecLen; j++ )
        {
            tmp  = AUD_MAX( pow( pFFTMag[j], alpha ) - beta * pow( pNoiseMean[j], alpha ), flr * pow( pNoiseMean[j], alpha ) );

            pFFTCleanMag[j] = (AUD_Int32s)round( pow( tmp, 1. / alpha ) );
        }

        // re-estimate noise
        if ( snr < thres )
        {
            AUD_Double tmpNoise;
            for ( j = 0; j < nSpecLen; j++ )
            {
                tmpNoise = G * pow( pNoiseMean[j], alpha ) + ( 1 - G ) * pow( pFFTMag[j], alpha );
                pNoiseMean[j] = pow( tmpNoise, 1. / alpha );
            }
        }

#if 0
        AUDLOG( "clean FFT:\n" );
        for ( j = 0; j < nSpecLen; j++ )
        {
            AUDLOG( "%d, ", pFFTCleanMag[j] );
        }
        AUDLOG( "\n" );
#endif

        pFFTCleanRe[0] = pFFTCleanMag[0];
        pFFTCleanIm[0] = 0;
        AUD_Double costheta, sintheta;
        for ( j = 1; j < nSpecLen; j++ )
        {
            if ( pFFTMag[j] != 0 )
            {
                costheta = (AUD_Double)pFFTRe[j] / (AUD_Double)pFFTMag[j];
                sintheta = (AUD_Double)pFFTIm[j] / (AUD_Double)pFFTMag[j];
                pFFTCleanRe[nFFT - j] = pFFTCleanRe[j] = (AUD_Int32s)round( costheta * pFFTCleanMag[j] );
                pFFTCleanIm[j]        = (AUD_Int32s)round( sintheta * pFFTCleanMag[j] );
                pFFTCleanIm[nFFT - j] = -pFFTCleanIm[j];
            }
            else
            {
                pFFTCleanRe[nFFT - j] = pFFTCleanRe[j] = pFFTCleanMag[j];
                pFFTCleanIm[nFFT - j] = pFFTCleanIm[j] = 0;
            }
        }

#if 0
        AUDLOG( "clean FFT with phase:\n" );
        for ( j = 0; j < nFFT; j++ )
        {
            AUDLOG( "%d + j%d, ", pFFTCleanRe[j], pFFTCleanIm[j] );
        }
        AUDLOG( "\n" );
#endif

        ifft_real( hIfft, pFFTCleanRe, pFFTCleanIm, nFFT, pxClean );

#if 0
        AUDLOG( "clean speech:\n" );
        for ( j = 0; j < nFFT; j++ )
        {
            AUDLOG( "%d, ", pxClean[j] );
        }
        AUDLOG( "\n" );
#endif

        for ( j = 0; j < frameStride; j++ )
        {
            pOutBuf[k + j] = pxOld[j] + pxClean[j];
            pxOld[j]       = pxClean[frameOverlap + j];
        }

        k        += frameStride;
        cleanLen += frameStride;
    }

    // de-emphasis
    // sig_deemphasis( pOutBuf, pOutBuf, cleanLen );

    win16s_free( &hWin );
    fft_free( &hFft );
    ifft_free( &hIfft );

    free( pFrame );
    free( pNoiseMean );

    free( pFFTMag );
    free( pFFTRe );
    free( pFFTIm );

    free( pFFTCleanMag );
    free( pFFTCleanRe );
    free( pFFTCleanIm );

    free( pxOld );
    free( pxClean );

    return cleanLen;
}
예제 #16
0
파일: fourier.c 프로젝트: xelmirage/libpano
static void fconvolution( TrformStr *TrPtr, Image *psf )
{
	int 		dims[2], i, k, dim, prog=0, delta = 100/15;
	double		**Re = NULL, **Im = NULL, **PRe = NULL, **PIm = NULL;
	char		percent[25];

	dims[0] = TrPtr->src->width;
	dims[1] = TrPtr->src->height;
	dim = TrPtr->src->width * TrPtr->src->height;
	
	Progress( _initProgress, "Convolution Filter" );
	
	Re 	= (double**)mymalloc( dim * sizeof( double ) );
	Im 	= (double**)mymalloc( dim * sizeof( double ) );
	PRe = (double**)mymalloc( dim * sizeof( double ) );
	PIm = (double**)mymalloc( dim * sizeof( double ) );
	if( Re == NULL || Im == NULL || PRe == NULL || PIm == NULL)
	{
		PrintError("Not enough memory");
		TrPtr->success = 0;
		goto _fconvolution_exit;
	}

	
	for( i=0; i<3; i++ )
	{
		register double x,y,a,b;
		
		UPDATE_PROGRESS_CONVOLUTION

		makePSF( TrPtr->src->width, TrPtr->src->height, psf, *PRe, *PIm, i, 1 );
		
		fftn (2, dims, *PRe, *PIm, 1, 1.0 );

		UPDATE_PROGRESS_CONVOLUTION

		
		if(  makeDoubleImage ( TrPtr->src, *Re, *Im, i, TrPtr->gamma ) != 0 )
		{
			PrintError("Could not make Real-version of image");
			TrPtr->success = 0;
			goto _fconvolution_exit;
		}
		fftn (2, dims, *Re, *Im, 1, 1.0 ); // forward transform; don't scale

		UPDATE_PROGRESS_CONVOLUTION

		
		// Multiply with psf
		
		for( k= 0; k<dim; k++)
		{
			x = (*Re)[k]; 	y = (*Im)[k];
			a = (*PRe)[k];	b = (*PIm)[k];
			(*Re)[k] = x*a-y*b;
			(*Im)[k] = x*b+y*a;
		}

		UPDATE_PROGRESS_CONVOLUTION

		
 		fftn (2, dims, *Re, *Im, -1, -1.0 ); // backward transform; scale by dim;

		UPDATE_PROGRESS_CONVOLUTION


		makeUcharImage ( TrPtr->dest, *Re, i );
	}
	TrPtr->success = 1;

_fconvolution_exit:	
	Progress( _disposeProgress, percent );
	fft_free();	
	if( Re  != NULL ) myfree( (void**)Re  ); 
	if( Im  != NULL ) myfree( (void**)Im  );  
	if( PRe != NULL ) myfree( (void**)PRe );
	if( PIm != NULL ) myfree( (void**)PIm );

}
예제 #17
0
AUD_Int32s denoise_aud( AUD_Int16s *pInBuf, AUD_Int16s *pOutBuf, AUD_Int32s inLen )
{
    Fft_16s        *hFft        = NULL;
    Ifft_16s       *hIfft       = NULL;
    AUD_Window16s  *hWin        = NULL;

    AUD_Int32s     frameSize    = 512;
    AUD_Int32s     frameStride  = 256;
    AUD_Int32s     frameOverlap = 256;
    AUD_Int32s     nFFT         = frameSize;
    AUD_Int32s     nSpecLen     = nFFT / 2 + 1;
    AUD_Int32s     nNoiseFrame  = 6; // (AUD_Int32s)( ( 0.25 * SAMPLE_RATE - frameSize ) / frameStride + 1 );

    AUD_Int32s     i, j, k, m, n, ret;

    AUD_Int32s     cleanLen     = 0;

    // pre-emphasis
    // sig_preemphasis( pInBuf, pInBuf, inLen );

    // init hamming module
    win16s_init( &hWin, AUD_WIN_HAMM, frameSize, 14 );
    AUD_ASSERT( hWin );

    // init fft handle
    fft_init( &hFft, nFFT, 15 );
    AUD_ASSERT( hFft );

    // init ifft handle
    ifft_init( &hIfft, nFFT, 15 );
    AUD_ASSERT( hIfft );

    AUD_Int16s *pFrame  = (AUD_Int16s*)calloc( frameSize * sizeof(AUD_Int16s), 1 );
    AUD_ASSERT( pFrame );

    // FFT
    AUD_Int32s *pFFTMag  = (AUD_Int32s*)calloc( nFFT * sizeof(AUD_Int32s), 1 );
    AUD_ASSERT( pFFTMag );
    AUD_Int32s *pFFTRe   = (AUD_Int32s*)calloc( nFFT * sizeof(AUD_Int32s), 1 );
    AUD_ASSERT( pFFTRe );
    AUD_Int32s *pFFTIm   = (AUD_Int32s*)calloc( nFFT * sizeof(AUD_Int32s), 1 );
    AUD_ASSERT( pFFTIm );

    AUD_Int32s *pFFTCleanRe = (AUD_Int32s*)calloc( nFFT * sizeof(AUD_Int32s), 1 );
    AUD_ASSERT( pFFTCleanRe );
    AUD_Int32s *pFFTCleanIm = (AUD_Int32s*)calloc( nFFT * sizeof(AUD_Int32s), 1 );
    AUD_ASSERT( pFFTCleanIm );

    // noise spectrum
    AUD_Double *pNoiseEn = (AUD_Double*)calloc( nSpecLen * sizeof(AUD_Double), 1 );
    AUD_ASSERT( pNoiseEn );

    AUD_Double *pNoiseB = (AUD_Double*)calloc( nSpecLen * sizeof(AUD_Double), 1 );
    AUD_ASSERT( pNoiseB );

    AUD_Double *pXPrev = (AUD_Double*)calloc( nSpecLen * sizeof(AUD_Double), 1 );
    AUD_ASSERT( pXPrev );

    AUD_Double *pAb = (AUD_Double*)calloc( nSpecLen * sizeof(AUD_Double), 1 );
    AUD_ASSERT( pAb );

    AUD_Double *pH = (AUD_Double*)calloc( nSpecLen * sizeof(AUD_Double), 1 );
    AUD_ASSERT( pH );

    AUD_Double *pGammak = (AUD_Double*)calloc( nSpecLen * sizeof(AUD_Double), 1 );
    AUD_ASSERT( pGammak );

    AUD_Double *pKsi = (AUD_Double*)calloc( nSpecLen * sizeof(AUD_Double), 1 );
    AUD_ASSERT( pKsi );

    AUD_Double *pLogSigmak = (AUD_Double*)calloc( nSpecLen * sizeof(AUD_Double), 1 );
    AUD_ASSERT( pLogSigmak );

    AUD_Double *pAlpha = (AUD_Double*)calloc( nSpecLen * sizeof(AUD_Double), 1 );
    AUD_ASSERT( pAlpha );

    AUD_Int32s *pLinToBark = (AUD_Int32s*)calloc( nSpecLen * sizeof(AUD_Int32s), 1 );
    AUD_ASSERT( pLinToBark );

    AUD_Int16s *pxOld   = (AUD_Int16s*)calloc( frameOverlap * sizeof(AUD_Int16s), 1 );
    AUD_ASSERT( pxOld );
    AUD_Int16s *pxClean = (AUD_Int16s*)calloc( nFFT * sizeof(AUD_Int16s), 1 );
    AUD_ASSERT( pxClean );

    /*
    AUD_Int32s critBandEnds[22] = { 0, 100, 200, 300, 400, 510, 630, 770, 920, 1080, 1270,
                                    1480, 1720, 2000, 2320, 2700, 3150, 3700, 4400, 5300, 6400, 7700 };
    */
    AUD_Int32s critFFTEnds[CRITICAL_BAND_NUM + 1]  = { 0, 4, 7, 10, 13, 17, 21, 25, 30, 35, 41, 48, 56, 64,
                                                       75, 87, 101, 119, 141, 170, 205, 247, 257 };

    // generate linear->bark transform mapping
    k = 0;
    for ( i = 0; i < CRITICAL_BAND_NUM; i++ )
    {
        while ( k >= critFFTEnds[i] &&  k < critFFTEnds[i + 1] )
        {
            pLinToBark[k] = i;
            k++;
        }
    }

    AUD_Double absThr[CRITICAL_BAND_NUM] = { 38, 31, 22, 18.5, 15.5, 13, 11, 9.5, 8.75, 7.25, 4.75, 2.75,
                                             1.5, 0.5, 0, 0, 0, 0, 2, 7, 12, 15.5 };

    AUD_Double dbOffset[CRITICAL_BAND_NUM];
    AUD_Double sumn[CRITICAL_BAND_NUM];
    AUD_Double spread[CRITICAL_BAND_NUM];

    for ( i = 0; i < CRITICAL_BAND_NUM; i++ )
    {
        absThr[i]   = pow( 10., absThr[i] / 10. ) / nFFT / ( 65535. * 65535. );
        dbOffset[i] = 10. + i;

        sumn[i]   = 0.474 + i;
        spread[i] = pow( 10., ( 15.81 + 7.5 * sumn[i] - 17.5 * sqrt( 1. + sumn[i] * sumn[i] ) ) / 10. );
    }

    AUD_Double dcGain[CRITICAL_BAND_NUM];
    for ( i = 0; i < CRITICAL_BAND_NUM; i++ )
    {
        dcGain[i] = 0.;
        for ( j = 0; j < CRITICAL_BAND_NUM; j++ )
        {
            dcGain[i] += spread[MABS( i - j )];
        }
    }

    AUD_Matrix exPatMatrix;
    exPatMatrix.rows     = CRITICAL_BAND_NUM;
    exPatMatrix.cols     = nSpecLen;
    exPatMatrix.dataType = AUD_DATATYPE_DOUBLE;
    ret = createMatrix( &exPatMatrix );
    AUD_ASSERT( ret == 0 );

    // excitation pattern
    AUD_Int32s index = 0;
    for ( i = 0; i < exPatMatrix.rows; i++ )
    {
        AUD_Double *pExpatRow = exPatMatrix.pDouble + i * exPatMatrix.cols;

        for ( j = 0; j < exPatMatrix.cols; j++ )
        {
            index = MABS( i - pLinToBark[j] );
            pExpatRow[j] = spread[index];
        }
    }

    AUD_Int32s frameNum = (inLen - frameSize) / frameStride + 1;
    AUD_ASSERT( frameNum > nNoiseFrame );

    // compute noise mean
    for ( i = 0; i < nNoiseFrame; i++ )
    {
        win16s_calc( hWin, pInBuf + i * frameSize, pFrame );

        fft_mag( hFft, pFrame, frameSize, pFFTMag );

        for ( j = 0; j < nSpecLen; j++ )
        {
            pNoiseEn[j] += pFFTMag[j] / 32768. * pFFTMag[j] / 32768.;
        }
    }
    for ( j = 0; j < nSpecLen; j++ )
    {
        pNoiseEn[j] /= nNoiseFrame;
    }

    // get cirtical band mean filtered noise power
    AUD_Int32s k1 = 0, k2 = 0;
    for ( i = 0; i < CRITICAL_BAND_NUM; i++ )
    {
        k1 = k2;
        AUD_Double segSum = 0.;
        while ( k2 >= critFFTEnds[i] &&  k2 < critFFTEnds[i + 1] )
        {
            segSum += pNoiseEn[k2];
            k2++;
        }
        segSum /= ( k2 - k1 );

        for ( m = k1; m < k2; m++ )
        {
            pNoiseB[m] = segSum;
        }
    }

#if 0
    AUDLOG( "noise band spectrum:\n" );
    for ( j = 0; j < nSpecLen; j++ )
    {
        AUDLOG( "%.2f, ", pNoiseB[j] );
    }
    AUDLOG( "\n" );
#endif

    AUD_Matrix frameMatrix;
    frameMatrix.rows     = nSpecLen;
    frameMatrix.cols     = 1;
    frameMatrix.dataType = AUD_DATATYPE_DOUBLE;
    ret = createMatrix( &frameMatrix );
    AUD_ASSERT( ret == 0 );
    AUD_Double *pFrameEn = frameMatrix.pDouble;

    AUD_Matrix xMatrix;
    xMatrix.rows     = nSpecLen;
    xMatrix.cols     = 1;
    xMatrix.dataType = AUD_DATATYPE_DOUBLE;
    ret = createMatrix( &xMatrix );
    AUD_ASSERT( ret == 0 );
    AUD_Double *pX = xMatrix.pDouble;

    AUD_Matrix cMatrix;
    cMatrix.rows     = CRITICAL_BAND_NUM;
    cMatrix.cols     = 1;
    cMatrix.dataType = AUD_DATATYPE_DOUBLE;
    ret = createMatrix( &cMatrix );
    AUD_ASSERT( ret == 0 );
    AUD_Double *pC = cMatrix.pDouble;

    AUD_Matrix tMatrix;
    tMatrix.rows     = 1;
    tMatrix.cols     = CRITICAL_BAND_NUM;
    tMatrix.dataType = AUD_DATATYPE_DOUBLE;
    ret = createMatrix( &tMatrix );
    AUD_ASSERT( ret == 0 );
    AUD_Double *pT = tMatrix.pDouble;

    AUD_Matrix tkMatrix;
    tkMatrix.rows     = 1;
    tkMatrix.cols     = nSpecLen;
    tkMatrix.dataType = AUD_DATATYPE_DOUBLE;
    ret = createMatrix( &tkMatrix );
    AUD_ASSERT( ret == 0 );
    AUD_Double *pTk = tkMatrix.pDouble;

    AUD_Double dB0[CRITICAL_BAND_NUM];
    AUD_Double epsilon = pow( 2, -52 );

    #define ESTIMATE_MASKTHRESH( sigMatrix, tkMatrix )\
    do {\
        AUD_Double *pSig = sigMatrix.pDouble; \
        for ( m = 0; m < exPatMatrix.rows; m++ ) \
        { \
            AUD_Double suma = 0.; \
            AUD_Double *pExpatRow = exPatMatrix.pDouble + m * exPatMatrix.cols; \
            for ( n = 0; n < exPatMatrix.cols; n++ ) \
            { \
                suma += pExpatRow[n] * pSig[n]; \
            } \
            pC[m] = suma; \
        } \
        AUD_Double product = 1.; \
        AUD_Double sum     = 0.; \
        for ( m = 0; m < sigMatrix.rows; m++ ) \
        { \
            product *= pSig[m]; \
            sum     += pSig[m]; \
        } \
        AUD_Double power = 1. / sigMatrix.rows;\
        AUD_Double sfmDB = 10. * log10( pow( product, power ) / sum / sigMatrix.rows + epsilon ); \
        AUD_Double alpha = AUD_MIN( 1., sfmDB / (-60.) ); \
        for ( m = 0; m < tMatrix.cols; m++ ) \
        { \
            dB0[m] = dbOffset[m] * alpha + 5.5; \
            pT[m]  = pC[m] / pow( 10., dB0[m] / 10. ) / dcGain[m]; \
            pT[m]  = AUD_MAX( pT[m], absThr[m] ); \
        } \
        for ( m = 0; m < tkMatrix.cols; m++ ) \
        { \
            pTk[m] = pT[pLinToBark[m]]; \
        } \
       } while ( 0 )

    AUD_Double aa    = 0.98;
    AUD_Double mu    = 0.98;
    AUD_Double eta   = 0.15;
    AUD_Double vadDecision;

    k = 0;
    // start processing
    for ( i = 0; i < frameNum; i++ )
    {
        win16s_calc( hWin, pInBuf + i * frameStride, pFrame );

        fft_calc( hFft, pFrame, frameSize, pFFTRe, pFFTIm );

        // compute SNR
        vadDecision = 0.;
        for ( j = 0; j < nSpecLen; j++ )
        {
            pFrameEn[j] = pFFTRe[j] / 32768. * pFFTRe[j] / 32768. + pFFTIm[j] / 32768. * pFFTIm[j] / 32768.;

            pGammak[j]  = AUD_MIN( pFrameEn[j] / pNoiseEn[j], 40. );

            if ( i > 0 )
            {
                pKsi[j] = aa * pXPrev[j] / pNoiseEn[j] + ( 1 - aa ) * AUD_MAX( pGammak[j] - 1., 0. );
            }
            else
            {
                pKsi[j] = aa + ( 1. - aa ) * AUD_MAX( pGammak[j] - 1., 0. );
            }

            pLogSigmak[j] = pGammak[j] * pKsi[j] / ( 1. + pKsi[j] ) - log( 1. + pKsi[j] );

            vadDecision += ( j > 0 ? 2 : 1 ) * pLogSigmak[j];
        }
        vadDecision /= nFFT;

#if 0
        AUDLOG( "X prev:\n" );
        for ( j = 0; j < nSpecLen; j++ )
        {
            AUDLOG( "%.2f, ", pXPrev[j] );
        }
        AUDLOG( "\n" );
#endif

#if 0
        AUDLOG( "gamma k:\n" );
        for ( j = 0; j < nSpecLen; j++ )
        {
            AUDLOG( "%.2f, ", pGammak[j] );
        }
        AUDLOG( "\n" );
#endif

#if 0
        AUDLOG( "ksi:\n" );
        for ( j = 0; j < nSpecLen; j++ )
        {
            AUDLOG( "%.2f, ", pKsi[j] );
        }
        AUDLOG( "\n" );
#endif

#if 0
        AUDLOG( "log sigma k:\n" );
        for ( j = 0; j < nSpecLen; j++ )
        {
            AUDLOG( "%.2f, ", pLogSigmak[j] );
        }
        AUDLOG( "\n" );
#endif

        // AUDLOG( "vadDecision: %.2f\n", vadDecision );
        // re-estimate noise
        if ( vadDecision < eta )
        {
            for ( j = 0; j < nSpecLen; j++ )
            {
                pNoiseEn[j] = mu * pNoiseEn[j] + ( 1. - mu ) * pFrameEn[j];
            }

            // re-estimate crital band based noise
            AUD_Int32s k1 = 0, k2 = 0;
            for ( int band = 0; band < CRITICAL_BAND_NUM; band++ )
            {
                k1 = k2;
                AUD_Double segSum = 0.;
                while ( k2 >= critFFTEnds[band] &&  k2 < critFFTEnds[band + 1] )
                {
                    segSum += pNoiseEn[k2];
                    k2++;
                }
                segSum /= ( k2 - k1 );

                for ( m = k1; m < k2; m++ )
                {
                    pNoiseB[m] = segSum;
                }
            }

#if 0
            AUDLOG( "noise band spectrum:\n" );
            for ( j = 0; j < nSpecLen; j++ )
            {
                AUDLOG( "%.2f, ", pNoiseB[j] );
            }
            AUDLOG( "\n" );
#endif
        }

        for ( j = 0; j < nSpecLen; j++ )
        {
            pX[j]     = AUD_MAX( pFrameEn[j] - pNoiseEn[j], 0.001 );
            pXPrev[j] = pFrameEn[j];
        }

        ESTIMATE_MASKTHRESH( xMatrix, tkMatrix );
        for ( int iter = 0; iter < 2; iter++ )
        {
            for ( j = 0; j < nSpecLen; j++ )
            {
                pAb[j]      = pNoiseB[j] + pNoiseB[j] * pNoiseB[j] / pTk[j];
                pFrameEn[j] = pFrameEn[j] * pFrameEn[j] / ( pFrameEn[j] + pAb[j] );
                ESTIMATE_MASKTHRESH( frameMatrix, tkMatrix );

#if 0
                showMatrix( &tMatrix );
#endif
            }
        }

#if 0
        AUDLOG( "tk:\n" );
        for ( j = 0; j < nSpecLen; j++ )
        {
            AUDLOG( "%.2f, ", pTk[j] );
        }
        AUDLOG( "\n" );
#endif

        pAlpha[0]      = ( pNoiseB[0] + pTk[0] ) * ( pNoiseB[0] / pTk[0] );
        pH[0]          = pFrameEn[0] / ( pFrameEn[0] + pAlpha[0] );
        pXPrev[0]     *= pH[0] * pH[0];
        pFFTCleanRe[0] = 0;
        pFFTCleanIm[0] = 0;
        for ( j = 1; j < nSpecLen; j++ )
        {
            pAlpha[j] = ( pNoiseB[j] + pTk[j] ) * ( pNoiseB[j] / pTk[j] );

            pH[j]     = pFrameEn[j] / ( pFrameEn[j] + pAlpha[j] );

            pFFTCleanRe[j] = pFFTCleanRe[nFFT - j] = (AUD_Int32s)round( pH[j] * pFFTRe[j] );
            pFFTCleanIm[j]        = (AUD_Int32s)round( pH[j] * pFFTIm[j] );
            pFFTCleanIm[nFFT - j] = -pFFTCleanIm[j];

            pXPrev[j] *= pH[j] * pH[j];
        }

#if 0
        AUDLOG( "denoise transfer function:\n" );
        for ( j = 0; j < nSpecLen; j++ )
        {
            AUDLOG( "%.2f, ", pH[j] );
        }
        AUDLOG( "\n" );
#endif

#if 0
        AUDLOG( "clean FFT with phase:\n" );
        for ( j = 0; j < nFFT; j++ )
        {
            AUDLOG( "%d + j%d, ", pFFTCleanRe[j], pFFTCleanIm[j] );
        }
        AUDLOG( "\n" );
#endif

        ifft_real( hIfft, pFFTCleanRe, pFFTCleanIm, nFFT, pxClean );

#if 0
        AUDLOG( "clean speech:\n" );
        for ( j = 0; j < nFFT; j++ )
        {
            AUDLOG( "%d, ", pxClean[j] );
        }
        AUDLOG( "\n" );
#endif

        for ( j = 0; j < frameStride; j++ )
        {
            if ( j < frameOverlap )
            {
                pOutBuf[k + j] = pxOld[j] + pxClean[j];
                pxOld[j] = pxClean[frameStride + j];
            }
            else
            {
                pOutBuf[k + j] = pxClean[j];
            }
        }

        k        += frameStride;
        cleanLen += frameStride;
    }

    // de-emphasis
    // sig_deemphasis( pOutBuf, pOutBuf, cleanLen );

    win16s_free( &hWin );
    fft_free( &hFft );
    ifft_free( &hIfft );

    free( pFrame );
    free( pNoiseEn );
    free( pNoiseB );

    free( pFFTMag );
    free( pFFTRe );
    free( pFFTIm );
    free( pXPrev );
    free( pAb );
    free( pH );

    free( pFFTCleanRe );
    free( pFFTCleanIm );

    free( pxOld );
    free( pxClean );

    free( pGammak );
    free( pKsi );
    free( pLogSigmak );
    free( pAlpha );
    free( pLinToBark );

    ret = createMatrix( &xMatrix );
    AUD_ASSERT( ret == 0 );

    ret = destroyMatrix( &exPatMatrix );
    AUD_ASSERT( ret == 0 );

    ret = destroyMatrix( &frameMatrix );
    AUD_ASSERT( ret == 0 );

    ret = destroyMatrix( &cMatrix );
    AUD_ASSERT( ret == 0 );

    ret = destroyMatrix( &tMatrix );
    AUD_ASSERT( ret == 0 );

    ret = destroyMatrix( &tkMatrix );
    AUD_ASSERT( ret == 0 );

    return cleanLen;
}
예제 #18
0
/*
 * singleton's mixed radix routine
 *
 * could move allocation out to fftn(), but leave it here so that it's
 * possible to make this a standalone function
 */
static int
FFTRADIX (REAL Re[],
	  REAL Im[],
	  size_t nTotal,
	  size_t nPass,
	  size_t nSpan,
	  int iSign,
	  int max_factors,
	  int max_perm)
{
   int ii, mfactor, kspan, ispan, inc;
   int j, jc, jf, jj, k, k1, k2, k3, k4, kk, kt, nn, ns, nt;

   REAL radf;
   REAL c1, c2, c3, cd, aa, aj, ak, ajm, ajp, akm, akp;
   REAL s1, s2, s3, sd, bb, bj, bk, bjm, bjp, bkm, bkp;

   REAL *Rtmp = NULL;	/* temp space for real part*/
   REAL *Itmp = NULL;	/* temp space for imaginary part */
   REAL *Cos = NULL;	/* Cosine values */
   REAL *Sin = NULL;	/* Sine values */

   REAL s60 = SIN60;		/* sin(60 deg) */
   REAL c72 = COS72;		/* cos(72 deg) */
   REAL s72 = SIN72;		/* sin(72 deg) */
   REAL pi2 = M_PI;		/* use PI first, 2 PI later */

   /* gcc complains about k3 being uninitialized, but I can't find out where
    * or why ... it looks okay to me.
    *
    * initialize to make gcc happy
    */
   k3 = 0;

   /* gcc complains about c2, c3, s2,s3 being uninitialized, but they're
    * only used for the radix 4 case and only AFTER the (s1 == 0.0) pass
    * through the loop at which point they will have been calculated.
    *
    * initialize to make gcc happy
    */
   c2 = c3 = s2 = s3 = 0.0;

   /* Parameter adjustments, was fortran so fix zero-offset */
   Re--;
   Im--;

   if (nPass < 2)
     return 0;

   /* assign pointers */
   Rtmp = (REAL *) Tmp0;
   Itmp = (REAL *) Tmp1;
   Cos  = (REAL *) Tmp2;
   Sin  = (REAL *) Tmp3;

   /*
    * Function Body
    */
   inc = iSign;
   if (iSign < 0) {
      s72 = -s72;
      s60 = -s60;
      pi2 = -pi2;
      inc = -inc;		/* absolute value */
   }

   /* adjust for strange increments */
   nt = inc * nTotal;
   ns = inc * nSpan;
   kspan = ns;

   nn = nt - inc;
   jc = ns / nPass;
   radf = pi2 * (double) jc;
   pi2 *= 2.0;			/* use 2 PI from here on */

   ii = 0;
   jf = 0;
   /*  determine the factors of n */
   mfactor = 0;
   k = nPass;
   while (k % 16 == 0) {
      mfactor++;
      factor [mfactor - 1] = 4;
      k /= 16;
   }
   j = 3;
   jj = 9;
   do {
      while (k % jj == 0) {
	 mfactor++;
	 factor [mfactor - 1] = j;
	 k /= jj;
      }
      j += 2;
      jj = j * j;
   } while (jj <= k);
   if (k <= 4) {
      kt = mfactor;
      factor [mfactor] = k;
      if (k != 1)
	mfactor++;
   } else {
      if (k - (k / 4 << 2) == 0) {
	 mfactor++;
	 factor [mfactor - 1] = 2;
	 k /= 4;
      }
      kt = mfactor;
      j = 2;
      do {
	 if (k % j == 0) {
	    mfactor++;
	    factor [mfactor - 1] = j;
	    k /= j;
	 }
	 j = ((j + 1) / 2 << 1) + 1;
      } while (j <= k);
   }
   if (kt) {
      j = kt;
      do {
	 mfactor++;
	 factor [mfactor - 1] = factor [j - 1];
	 j--;
      } while (j);
   }

   /* test that mfactors is in range */
   if (mfactor > NFACTOR)
     {
	fputs ("Error: " FFTRADIXS "() - exceeded number of factors\n", stderr);
	goto Memory_Error_Label;
      }

   /* compute fourier transform */
   for (;;) {
      sd = radf / (double) kspan;
      cd = sin(sd);
      cd = 2.0 * cd * cd;
      sd = sin(sd + sd);
      kk = 1;
      ii++;

      switch (factor [ii - 1]) {
       case 2:
	 /* transform for factor of 2 (including rotation factor) */
	 kspan /= 2;
	 k1 = kspan + 2;
	 do {
	    do {
	       k2 = kk + kspan;
	       ak = Re [k2];
	       bk = Im [k2];
	       Re [k2] = Re [kk] - ak;
	       Im [k2] = Im [kk] - bk;
	       Re [kk] += ak;
	       Im [kk] += bk;
	       kk = k2 + kspan;
	    } while (kk <= nn);
	    kk -= nn;
	 } while (kk <= jc);
	 if (kk > kspan)
	   goto Permute_Results_Label;		/* exit infinite loop */
	 do {
	    c1 = 1.0 - cd;
	    s1 = sd;
	    do {
	       do {
		  do {
		     k2 = kk + kspan;
		     ak = Re [kk] - Re [k2];
		     bk = Im [kk] - Im [k2];
		     Re [kk] += Re [k2];
		     Im [kk] += Im [k2];
		     Re [k2] = c1 * ak - s1 * bk;
		     Im [k2] = s1 * ak + c1 * bk;
		     kk = k2 + kspan;
		  } while (kk < nt);
		  k2 = kk - nt;
		  c1 = -c1;
		  kk = k1 - k2;
	       } while (kk > k2);
	       ak = c1 - (cd * c1 + sd * s1);
	       s1 = sd * c1 - cd * s1 + s1;
	       c1 = 2.0 - (ak * ak + s1 * s1);
	       s1 *= c1;
	       c1 *= ak;
	       kk += jc;
	    } while (kk < k2);
	    k1 += inc + inc;
	    kk = (k1 - kspan) / 2 + jc;
	 } while (kk <= jc + jc);
	 break;

       case 4:			/* transform for factor of 4 */
	 ispan = kspan;
	 kspan /= 4;

	 do {
	    c1 = 1.0;
	    s1 = 0.0;
	    do {
	       do {
		  k1 = kk + kspan;
		  k2 = k1 + kspan;
		  k3 = k2 + kspan;
		  akp = Re [kk] + Re [k2];
		  akm = Re [kk] - Re [k2];
		  ajp = Re [k1] + Re [k3];
		  ajm = Re [k1] - Re [k3];
		  bkp = Im [kk] + Im [k2];
		  bkm = Im [kk] - Im [k2];
		  bjp = Im [k1] + Im [k3];
		  bjm = Im [k1] - Im [k3];
		  Re [kk] = akp + ajp;
		  Im [kk] = bkp + bjp;
		  ajp = akp - ajp;
		  bjp = bkp - bjp;
		  if (iSign < 0) {
		     akp = akm + bjm;
		     bkp = bkm - ajm;
		     akm -= bjm;
		     bkm += ajm;
		  } else {
		     akp = akm - bjm;
		     bkp = bkm + ajm;
		     akm += bjm;
		     bkm -= ajm;
		  }
		  /* avoid useless multiplies */
		  if (s1 == 0.0) {
		     Re [k1] = akp;
		     Re [k2] = ajp;
		     Re [k3] = akm;
		     Im [k1] = bkp;
		     Im [k2] = bjp;
		     Im [k3] = bkm;
		  } else {
		     Re [k1] = akp * c1 - bkp * s1;
		     Re [k2] = ajp * c2 - bjp * s2;
		     Re [k3] = akm * c3 - bkm * s3;
		     Im [k1] = akp * s1 + bkp * c1;
		     Im [k2] = ajp * s2 + bjp * c2;
		     Im [k3] = akm * s3 + bkm * c3;
		  }
		  kk = k3 + kspan;
	       } while (kk <= nt);

	       c2 = c1 - (cd * c1 + sd * s1);
	       s1 = sd * c1 - cd * s1 + s1;
	       c1 = 2.0 - (c2 * c2 + s1 * s1);
	       s1 *= c1;
	       c1 *= c2;
	       /* values of c2, c3, s2, s3 that will get used next time */
	       c2 = c1 * c1 - s1 * s1;
	       s2 = 2.0 * c1 * s1;
	       c3 = c2 * c1 - s2 * s1;
	       s3 = c2 * s1 + s2 * c1;
	       kk = kk - nt + jc;
	    } while (kk <= kspan);
	    kk = kk - kspan + inc;
	 } while (kk <= jc);
	 if (kspan == jc)
	   goto Permute_Results_Label;		/* exit infinite loop */
	 break;

       default:
	 /*  transform for odd factors */
#ifdef FFT_RADIX4
	 fputs ("Error: " FFTRADIXS "(): compiled for radix 2/4 only\n", stderr);
	 fft_free ();		/* free-up memory */
	 return -1;
	 break;
#else	/* FFT_RADIX4 */
	 k = factor [ii - 1];
	 ispan = kspan;
	 kspan /= k;

	 switch (k) {
	  case 3:	/* transform for factor of 3 (optional code) */
	    do {
	       do {
		  k1 = kk + kspan;
		  k2 = k1 + kspan;
		  ak = Re [kk];
		  bk = Im [kk];
		  aj = Re [k1] + Re [k2];
		  bj = Im [k1] + Im [k2];
		  Re [kk] = ak + aj;
		  Im [kk] = bk + bj;
		  ak -= 0.5 * aj;
		  bk -= 0.5 * bj;
		  aj = (Re [k1] - Re [k2]) * s60;
		  bj = (Im [k1] - Im [k2]) * s60;
		  Re [k1] = ak - bj;
		  Re [k2] = ak + bj;
		  Im [k1] = bk + aj;
		  Im [k2] = bk - aj;
		  kk = k2 + kspan;
	       } while (kk < nn);
	       kk -= nn;
	    } while (kk <= kspan);
	    break;

	  case 5:	/*  transform for factor of 5 (optional code) */
	    c2 = c72 * c72 - s72 * s72;
	    s2 = 2.0 * c72 * s72;
	    do {
	       do {
		  k1 = kk + kspan;
		  k2 = k1 + kspan;
		  k3 = k2 + kspan;
		  k4 = k3 + kspan;
		  akp = Re [k1] + Re [k4];
		  akm = Re [k1] - Re [k4];
		  bkp = Im [k1] + Im [k4];
		  bkm = Im [k1] - Im [k4];
		  ajp = Re [k2] + Re [k3];
		  ajm = Re [k2] - Re [k3];
		  bjp = Im [k2] + Im [k3];
		  bjm = Im [k2] - Im [k3];
		  aa = Re [kk];
		  bb = Im [kk];
		  Re [kk] = aa + akp + ajp;
		  Im [kk] = bb + bkp + bjp;
		  ak = akp * c72 + ajp * c2 + aa;
		  bk = bkp * c72 + bjp * c2 + bb;
		  aj = akm * s72 + ajm * s2;
		  bj = bkm * s72 + bjm * s2;
		  Re [k1] = ak - bj;
		  Re [k4] = ak + bj;
		  Im [k1] = bk + aj;
		  Im [k4] = bk - aj;
		  ak = akp * c2 + ajp * c72 + aa;
		  bk = bkp * c2 + bjp * c72 + bb;
		  aj = akm * s2 - ajm * s72;
		  bj = bkm * s2 - bjm * s72;
		  Re [k2] = ak - bj;
		  Re [k3] = ak + bj;
		  Im [k2] = bk + aj;
		  Im [k3] = bk - aj;
		  kk = k4 + kspan;
	       } while (kk < nn);
	       kk -= nn;
	    } while (kk <= kspan);
	    break;

	  default:
	    if (k != jf) {
	       jf = k;
	       s1 = pi2 / (double) k;
	       c1 = cos(s1);
	       s1 = sin(s1);
	       if (jf > max_factors)
		 goto Memory_Error_Label;
	       Cos [jf - 1] = 1.0;
	       Sin [jf - 1] = 0.0;
	       j = 1;
	       do {
		  Cos [j - 1] = Cos [k - 1] * c1 + Sin [k - 1] * s1;
		  Sin [j - 1] = Cos [k - 1] * s1 - Sin [k - 1] * c1;
		  k--;
		  Cos [k - 1] = Cos [j - 1];
		  Sin [k - 1] = -Sin [j - 1];
		  j++;
	       } while (j < k);
	    }
	    do {
	       do {
		  k1 = kk;
		  k2 = kk + ispan;
		  ak = aa = Re [kk];
		  bk = bb = Im [kk];
		  j = 1;
		  k1 += kspan;
		  do {
		     k2 -= kspan;
		     j++;
		     Rtmp [j - 1] = Re [k1] + Re [k2];
		     ak += Rtmp [j - 1];
		     Itmp [j - 1] = Im [k1] + Im [k2];
		     bk += Itmp [j - 1];
		     j++;
		     Rtmp [j - 1] = Re [k1] - Re [k2];
		     Itmp [j - 1] = Im [k1] - Im [k2];
		     k1 += kspan;
		  } while (k1 < k2);
		  Re [kk] = ak;
		  Im [kk] = bk;
		  k1 = kk;
		  k2 = kk + ispan;
		  j = 1;
		  do {
		     k1 += kspan;
		     k2 -= kspan;
		     jj = j;
		     ak = aa;
		     bk = bb;
		     aj = 0.0;
		     bj = 0.0;
		     k = 1;
		     do {
			k++;
			ak += Rtmp [k - 1] * Cos [jj - 1];
			bk += Itmp [k - 1] * Cos [jj - 1];
			k++;
			aj += Rtmp [k - 1] * Sin [jj - 1];
			bj += Itmp [k - 1] * Sin [jj - 1];
			jj += j;
			if (jj > jf) {
			   jj -= jf;
			}
		     } while (k < jf);
		     k = jf - j;
		     Re [k1] = ak - bj;
		     Im [k1] = bk + aj;
		     Re [k2] = ak + bj;
		     Im [k2] = bk - aj;
		     j++;
		  } while (j < k);
		  kk += ispan;
	       } while (kk <= nn);
	       kk -= nn;
	    } while (kk <= kspan);
	    break;
	 }
	 /*  multiply by rotation factor (except for factors of 2 and 4) */
	 if (ii == mfactor)
	   goto Permute_Results_Label;		/* exit infinite loop */
	 kk = jc + 1;
	 do {
	    c2 = 1.0 - cd;
	    s1 = sd;
	    do {
	       c1 = c2;
	       s2 = s1;
	       kk += kspan;
	       do {
		  do {
		     ak = Re [kk];
		     Re [kk] = c2 * ak - s2 * Im [kk];
		     Im [kk] = s2 * ak + c2 * Im [kk];
		     kk += ispan;
		  } while (kk <= nt);
		  ak = s1 * s2;
		  s2 = s1 * c2 + c1 * s2;
		  c2 = c1 * c2 - ak;
		  kk = kk - nt + kspan;
	       } while (kk <= ispan);
	       c2 = c1 - (cd * c1 + sd * s1);
	       s1 += sd * c1 - cd * s1;
	       c1 = 2.0 - (c2 * c2 + s1 * s1);
	       s1 *= c1;
	       c2 *= c1;
	       kk = kk - ispan + jc;
	    } while (kk <= kspan);
	    kk = kk - kspan + jc + inc;
	 } while (kk <= jc + jc);
	 break;
#endif	/* FFT_RADIX4 */
      }
   }

/*  permute the results to normal order---done in two stages */
/*  permutation for square factors of n */
Permute_Results_Label:
   Perm [0] = ns;
   if (kt) {
      k = kt + kt + 1;
      if (mfactor < k)
	k--;
      j = 1;
      Perm [k] = jc;
      do {
	 Perm [j] = Perm [j - 1] / factor [j - 1];
	 Perm [k - 1] = Perm [k] * factor [j - 1];
	 j++;
	 k--;
      } while (j < k);
      k3 = Perm [k];
      kspan = Perm [1];
      kk = jc + 1;
      k2 = kspan + 1;
      j = 1;
      if (nPass != nTotal) {
/*  permutation for multivariate transform */
Permute_Multi_Label:
	 do {
	    do {
	       k = kk + jc;
	       do {
		  /* swap Re [kk] <> Re [k2], Im [kk] <> Im [k2] */
		  ak = Re [kk]; Re [kk] = Re [k2]; Re [k2] = ak;
		  bk = Im [kk]; Im [kk] = Im [k2]; Im [k2] = bk;
		  kk += inc;
		  k2 += inc;
	       } while (kk < k);
	       kk += ns - jc;
	       k2 += ns - jc;
	    } while (kk < nt);
	    k2 = k2 - nt + kspan;
	    kk = kk - nt + jc;
	 } while (k2 < ns);
	 do {
	    do {
	       k2 -= Perm [j - 1];
	       j++;
	       k2 = Perm [j] + k2;
	    } while (k2 > Perm [j - 1]);
	    j = 1;
	    do {
	       if (kk < k2)
		 goto Permute_Multi_Label;
	       kk += jc;
	       k2 += kspan;
	    } while (k2 < ns);
	 } while (kk < ns);
      } else {
/*  permutation for single-variate transform (optional code) */
Permute_Single_Label:
	 do {
	    /* swap Re [kk] <> Re [k2], Im [kk] <> Im [k2] */
	    ak = Re [kk]; Re [kk] = Re [k2]; Re [k2] = ak;
	    bk = Im [kk]; Im [kk] = Im [k2]; Im [k2] = bk;
	    kk += inc;
	    k2 += kspan;
	 } while (k2 < ns);
	 do {
	    do {
	       k2 -= Perm [j - 1];
	       j++;
	       k2 = Perm [j] + k2;
	    } while (k2 > Perm [j - 1]);
	    j = 1;
	    do {
	       if (kk < k2)
		 goto Permute_Single_Label;
	       kk += inc;
	       k2 += kspan;
	    } while (k2 < ns);
	 } while (kk < ns);
      }
      jc = k3;
   }

   if ((kt << 1) + 1 >= mfactor)
     return 0;
   ispan = Perm [kt];
   /* permutation for square-free factors of n */
   j = mfactor - kt;
   factor [j] = 1;
   do {
      factor [j - 1] *= factor [j];
      j--;
   } while (j != kt);
   kt++;
   nn = factor [kt - 1] - 1;
   if (nn > max_perm)
     goto Memory_Error_Label;
   j = jj = 0;
   for (;;) {
      k = kt + 1;
      k2 = factor [kt - 1];
      kk = factor [k - 1];
      j++;
      if (j > nn)
	break;				/* exit infinite loop */
      jj += kk;
      while (jj >= k2) {
	 jj -= k2;
	 k2 = kk;
	 k++;
	 kk = factor [k - 1];
	 jj += kk;
      }
      Perm [j - 1] = jj;
   }
   /*  determine the permutation cycles of length greater than 1 */
   j = 0;
   for (;;) {
      do {
	 j++;
	 kk = Perm [j - 1];
      } while (kk < 0);
      if (kk != j) {
	 do {
	    k = kk;
	    kk = Perm [k - 1];
	    Perm [k - 1] = -kk;
	 } while (kk != j);
	 k3 = kk;
      } else {
	 Perm [j - 1] = -j;
	 if (j == nn)
	   break;		/* exit infinite loop */
      }
   }
   max_factors *= inc;
   /*  reorder a and b, following the permutation cycles */
   for (;;) {
      j = k3 + 1;
      nt -= ispan;
      ii = nt - inc + 1;
      if (nt < 0)
	break;			/* exit infinite loop */
      do {
	 do {
	    j--;
	 } while (Perm [j - 1] < 0);
	 jj = jc;
	 do {
	    kspan = jj;
	    if (jj > max_factors) {
	       kspan = max_factors;
	    }
	    jj -= kspan;
	    k = Perm [j - 1];
	    kk = jc * k + ii + jj;
	    k1 = kk + kspan;
	    k2 = 0;
	    do {
	       k2++;
	       Rtmp [k2 - 1] = Re [k1];
	       Itmp [k2 - 1] = Im [k1];
	       k1 -= inc;
	    } while (k1 != kk);
	    do {
	       k1 = kk + kspan;
	       k2 = k1 - jc * (k + Perm [k - 1]);
	       k = -Perm [k - 1];
	       do {
		  Re [k1] = Re [k2];
		  Im [k1] = Im [k2];
		  k1 -= inc;
		  k2 -= inc;
	       } while (k1 != kk);
	       kk = k2;
	    } while (k != j);
	    k1 = kk + kspan;
	    k2 = 0;
	    do {
	       k2++;
	       Re [k1] = Rtmp [k2 - 1];
	       Im [k1] = Itmp [k2 - 1];
	       k1 -= inc;
	    } while (k1 != kk);
	 } while (jj);
      } while (j != 1);
   }
   return 0;			/* exit point here */

   /* alloc or other problem, do some clean-up */
Memory_Error_Label:
   fputs ("Error: " FFTRADIXS "() - insufficient memory.\n", stderr);
   fft_free ();			/* free-up memory */
   return -1;
}
예제 #19
0
int
FFTN (int ndim, const int dims[],
      REAL Re [],
      REAL Im [],
      int iSign,
      double scaling)
{
   size_t nSpan, nPass, nTotal;
   int ret, max_factors, max_perm;
   int i;

   /*
    * tally the number of elements in the data array
    * and determine the number of dimensions
    */
   nTotal = 1;
   if (ndim && dims [0])
     {
	for (i = 0; i < ndim; i++)
	  {
	     if (dims [i] <= 0)
	       {
		  fputs ("Error: " FFTNS "() - dimension error\n", stderr);
		  fft_free ();	/* free-up memory */
		  return -1;
	       }
	     nTotal *= dims [i];
	  }
     }
   else
     {
	ndim = 0;
	for (i = 0; dims [i]; i++)
	  {
	     if (dims [i] <= 0)
	       {
		  fputs ("Error: " FFTNS "() - dimension error\n", stderr);
		  fft_free ();	/* free-up memory */
		  return -1;
	       }
	     nTotal *= dims [i];
	     ndim++;
	  }
     }

   /* determine maximum number of factors and permuations */
#if 1
   /*
    * follow John Beale's example, just use the largest dimension and don't
    * worry about excess allocation.  May be someone else will do it?
    */
   max_factors = max_perm = 1;
   for (i = 0; i < ndim; i++)
     {
	nSpan = dims [i];
	if (nSpan > (unsigned int)max_factors) max_factors = nSpan;
	if (nSpan > (unsigned int)max_perm) max_perm = nSpan;
     }
#else
   /* use the constants used in the original Fortran code */
   max_factors = 23;
   max_perm = 209;
#endif

   /* loop over the dimensions: */
   nPass = 1;
   for (i = 0; i < ndim; i++)
     {
	nSpan = dims [i];
	nPass *= nSpan;
	ret = FFTRADIX (Re, Im, nTotal, nSpan, nPass, iSign,
			max_factors, max_perm);
	/* exit, clean-up already done */
	if (ret)
	  return ret;
     }

   /* Divide through by the normalizing constant: */
   if (scaling && scaling != 1.0)
     {
	if (iSign < 0) iSign = -iSign;
	if (scaling < 0.0)
	  {
	     scaling = nTotal;
	     if (scaling < -1.0)
	       scaling = sqrt (scaling);
	  }
	scaling = 1.0 / scaling;	/* multiply is often faster */
	for (i = 0; i < (int)nTotal; i += iSign)
	  {
	     Re [i] *= scaling;
	     Im [i] *= scaling;
	  }
     }
   return 0;
}
예제 #20
0
파일: nc.c 프로젝트: LgJianzhao/httping
void create_windows(void)
{
	char *r_a = gettext("realloc issue");
	int nr = 0;

	if (w_stats)
	{
		delwin(w_stats);
		delwin(w_line1);
		delwin(w_slow);
		delwin(w_line2);
		delwin(w_fast);
	}

#ifdef FW
	fft_free();
	fft_init(max_x);
#endif

	if (max_x > history_n)
	{
		history = (double *)realloc(history, sizeof(double) * max_x);
		if (!history)
			error_exit(r_a);

		history_temp = (double *)realloc(history_temp, sizeof(double) * max_x);
		if (!history_temp)
			error_exit(r_a);

		/* halve of it is enough really */
		history_fft_magn = (double *)realloc(history_fft_magn, sizeof(double) * max_x);
		if (!history_fft_magn)
			error_exit(r_a);

		history_fft_phase = (double *)realloc(history_fft_phase, sizeof(double) * max_x);
		if (!history_fft_phase)
			error_exit(r_a);

		history_set = (char *)realloc(history_set, sizeof(char) * max_x);
		if (!history_set)
			error_exit(r_a);

		memset(&history[history_n], 0x00, (max_x - history_n) * sizeof(double));
		memset(&history_set[history_n], 0x00, (max_x - history_n) * sizeof(char));

		history_n = max_x;
	}

	if ((int)max_y > window_history_n)
	{
		slow_history = (char **)realloc(slow_history, sizeof(char *) * max_y);
		if (!slow_history)
			error_exit(r_a);

		fast_history = (char **)realloc(fast_history, sizeof(char *) * max_y);
		if (!fast_history)
			error_exit(r_a);

		memset(&slow_history[window_history_n], 0x00, (max_y - window_history_n) * sizeof(char *));
		memset(&fast_history[window_history_n], 0x00, (max_y - window_history_n) * sizeof(char *));

		window_history_n = max_y;
	}

	w_stats = newwin(stats_h, max_x,  0, 0);
	scrollok(w_stats, false);

	w_line1 = newwin(1, max_x, stats_h, 0);
	scrollok(w_line1, false);
	wnoutrefresh(w_line1);

	logs_n = max_y - (stats_h + 1 + 1);
	fast_n = logs_n * 11 / 20;
	slow_n = logs_n - fast_n;

	w_slow  = newwin(slow_n, max_x, (stats_h + 1), 0);
	scrollok(w_slow, true);

	w_line2 = newwin(1, max_x, (stats_h + 1) + slow_n, 0);
	scrollok(w_line2, false);
	wnoutrefresh(w_line2);

	w_fast  = newwin(fast_n, max_x, (stats_h + 1) + slow_n + 1, 0);
	scrollok(w_fast, true);

	wattron(w_line1, A_REVERSE);
	wattron(w_line2, A_REVERSE);
	for(nr=0; nr<max_x; nr++)
	{
		wprintw(w_line1, " ");
		wprintw(w_line2, " ");
	}
	wattroff(w_line2, A_REVERSE);
	wattroff(w_line1, A_REVERSE);

        wnoutrefresh(w_line1);
        wnoutrefresh(w_line2);

	doupdate();

	signal(SIGWINCH, handler);
}
예제 #21
0
파일: fourier.c 프로젝트: xelmirage/libpano
static void fresize( TrformStr *TrPtr )
{
	int 		dims[2], dest_dims[2], i, dim, prog=0, delta = 100/12;
	double		**Re = NULL, **Im = NULL;
	char		percent[25];
	double 		*re,*im;
	// unsigned char *ch;
	// int 		x,y,dy,sy,x1,x2,y1,y2,rx,ry;
	int 		x,y,dy,sy,x1,y1,rx,ry;

	
	dims[0] = TrPtr->src->width;
	dims[1] = TrPtr->src->height;
	dim = max( TrPtr->src->width * TrPtr->src->height, TrPtr->dest->width * TrPtr->dest->height );

	dest_dims[1] = TrPtr->dest->height;
	dest_dims[0] = TrPtr->dest->width;

	ry = (TrPtr->src->height - TrPtr->dest->height);
	rx = (TrPtr->src->width  - TrPtr->dest->width);
		
	x1 = min( TrPtr->dest->width/2, TrPtr->src->width/2) ; 
		
	y1 = min( TrPtr->dest->height/2, TrPtr->src->height/2) ;

	
	Progress( _initProgress, "Resize Filter" );
	
	Re 	= (double**)mymalloc( dim * sizeof( double ) );
	Im 	= (double**)mymalloc( dim * sizeof( double ) );
	if( Re == NULL || Im == NULL )
	{
		PrintError("Not enough memory");
		TrPtr->success = 0;
		goto _antialias_exit;
	}
	re = *Re; im = *Im;
	
	for( i=0; i<3; i++ )
	{
		
		UPDATE_PROGRESS_ANTIALIAS
		
		if(  makeDoubleImage ( TrPtr->src, *Re, *Im, i, TrPtr->gamma ) != 0 )
		{
			PrintError("Could not make Real-version of image");
			TrPtr->success = 0;
			goto _antialias_exit;
		}
		fftn (2, dims, *Re, *Im, 1, -1.0 ); // forward transform; don't scale

		UPDATE_PROGRESS_ANTIALIAS

		
		if( TrPtr->dest->width < TrPtr->src->width )
		{
			// Cut frame if decimating	
		
			for( y=0; y<y1; y++)
			{
				sy = y * TrPtr->src->width;
				dy = y * TrPtr->dest->width;
				for(x=0; x<TrPtr->dest->width; x++)
				{
					if(x<x1)
					{
						re[dy+x] = re[sy+x];im[dy+x] = im[sy+x];
					}
					if(x>=x1)
					{
						re[dy+x] = re[sy+x+rx];im[dy+x] = im[sy+x+rx];
					}
				}
			}
			for( y=y1; y<TrPtr->dest->height; y++ )
			{
				sy = (y+ry) * TrPtr->src->width;
				dy = y * TrPtr->dest->width;
				
				for(x=0; x<TrPtr->dest->width; x++)
				{
					if(x<x1)
					{
						re[dy+x] = re[sy+x];im[dy+x] = im[sy+x];
					}
					if(x>=x1)
					{
						re[dy+x] = re[sy+x+rx];im[dy+x] = im[sy+x+rx];
					}
				}
			}
		}
		else
		{
			// Pad if enlarging	
		
			for( y=TrPtr->dest->height-1;y>=y1;  y-- )
			{
				sy = (y+ry) * TrPtr->src->width;
				dy = y * TrPtr->dest->width;
				
				for(x=TrPtr->dest->width-1;x>=0; x--)
				{
					if(x<x1)
					{
						re[dy+x] = re[sy+x];im[dy+x] = im[sy+x];
					}
					else if(x>=x1)
					{
						re[dy+x] = re[sy+x+rx];im[dy+x] = im[sy+x+rx];
					}
					else
					{
						re[dy+x] = 0.0;im[dy+x] = 0.0;
					}
				}
			}
			for( y=y1-1;y>=0;y--)
			{
				sy = y * TrPtr->src->width;
				dy = y * TrPtr->dest->width;
				for(x=TrPtr->dest->width-1;x>=0; x--)
				{
					if(x<x1)
					{
						re[dy+x] = re[sy+x];im[dy+x] = im[sy+x];
					}
					else if(x>=x1)
					{
						re[dy+x] = re[sy+x+rx];im[dy+x] = im[sy+x+rx];
					}
					else
					{
						re[dy+x] = 0.0;im[dy+x] = 0.0;
					}
				}
			}
		}		
		
							
		UPDATE_PROGRESS_ANTIALIAS

		
 		fftn (2, dest_dims, *Re, *Im, -1, 1.0 ); // backward transform; scale by dim;

		UPDATE_PROGRESS_ANTIALIAS
		
		makeUcharImage ( TrPtr->dest, *Re, i );
		
		
	}
	TrPtr->success = 1;

_antialias_exit:	
	Progress( _disposeProgress, percent );
	fft_free();	
	if( Re  != NULL ) myfree( (void**)Re  ); 
	if( Im  != NULL ) myfree( (void**)Im  );  
}
예제 #22
0
AUD_Int32s denoise_wiener( AUD_Int16s *pInBuf, AUD_Int16s *pOutBuf, AUD_Int32s inLen )
{
    Fft_16s        *hFft        = NULL;
    Ifft_16s       *hIfft       = NULL;
    AUD_Window16s  *hWin        = NULL;
    _VAD_Nest      vadState;

    AUD_Int32s     frameSize    = 512;
    AUD_Int32s     frameStride  = 256;
    AUD_Int32s     nFFT         = frameSize;
    AUD_Int32s     nSpecLen     = nFFT / 2 + 1;
    AUD_Int32s     nNoiseFrame  = (AUD_Int32s)( ( 0.25 * SAMPLE_RATE - frameSize ) / frameStride + 1 );

    AUD_Int32s     i, j, k;

    AUD_Int32s     cleanLen = 0;

    vadState.meanEn        = 280;
    vadState.nbSpeechFrame = 0;
    vadState.hangOver      = 0;

    AUD_Int16s *pFrame  = (AUD_Int16s*)calloc( frameSize * sizeof(AUD_Int16s), 1 );
    AUD_ASSERT( pFrame );

    AUD_Double *pNoiseMean = (AUD_Double*)calloc( nSpecLen * sizeof(AUD_Double), 1 );
    AUD_ASSERT( pNoiseMean );

    AUD_Double *pLamdaD = (AUD_Double*)calloc( nSpecLen * sizeof(AUD_Double), 1 );
    AUD_ASSERT( pLamdaD );

    AUD_Double *pXi = (AUD_Double*)calloc( nSpecLen * sizeof(AUD_Double), 1 );
    AUD_ASSERT( pXi );

    AUD_Double *pGamma = (AUD_Double*)calloc( nSpecLen * sizeof(AUD_Double), 1 );
    AUD_ASSERT( pGamma );

    AUD_Double *pG = (AUD_Double*)calloc( nSpecLen * sizeof(AUD_Double), 1 );
    AUD_ASSERT( pG );

    AUD_Double *pGammaNew = (AUD_Double*)calloc( nSpecLen * sizeof(AUD_Double), 1 );
    AUD_ASSERT( pGammaNew );

    for ( j = 0; j < nSpecLen; j++ )
    {
        pG[j]         = 1.;
        pGamma[j]     = 1.;
    }

    // FFT
    AUD_Int32s *pFFTMag  = (AUD_Int32s*)calloc( nFFT * sizeof(AUD_Int32s), 1 );
    AUD_ASSERT( pFFTMag );
    AUD_Int32s *pFFTRe   = (AUD_Int32s*)calloc( nFFT * sizeof(AUD_Int32s), 1 );
    AUD_ASSERT( pFFTRe );
    AUD_Int32s *pFFTIm   = (AUD_Int32s*)calloc( nFFT * sizeof(AUD_Int32s), 1 );
    AUD_ASSERT( pFFTIm );

    AUD_Double *pFFTCleanMag = (AUD_Double*)calloc( nFFT * sizeof(AUD_Double), 1 );
    AUD_ASSERT( pFFTCleanMag );

    AUD_Int32s *pFFTCleanRe = (AUD_Int32s*)calloc( nFFT * sizeof(AUD_Int32s), 1 );
    AUD_ASSERT( pFFTCleanRe );
    AUD_Int32s *pFFTCleanIm = (AUD_Int32s*)calloc( nFFT * sizeof(AUD_Int32s), 1 );
    AUD_ASSERT( pFFTCleanIm );

    AUD_Int16s *pxClean = (AUD_Int16s*)calloc( nFFT * sizeof(AUD_Int16s), 1 );
    AUD_ASSERT( pxClean );

    memset( pOutBuf, 0, inLen * sizeof(AUD_Int16s) );

    // init hamming module
    win16s_init( &hWin, AUD_WIN_HAMM, frameSize, 14 );
    AUD_ASSERT( hWin );

    // init fft handle
    fft_init( &hFft, nFFT, 15 );
    AUD_ASSERT( hFft );

    // init ifft handle
    ifft_init( &hIfft, nFFT, 15 );
    AUD_ASSERT( hIfft );

    // noise manipulation
    for ( i = 0; (i < nNoiseFrame) && ( (i * frameStride + frameSize) < inLen ); i++ )
    {
        win16s_calc( hWin, pInBuf + i * frameStride, pFrame );

        fft_mag( hFft, pFrame, frameSize, pFFTMag );

        for ( j = 0; j < nSpecLen; j++ )
        {
            pNoiseMean[j] += (AUD_Double)pFFTMag[j];
            pLamdaD[j]    += (AUD_Double)pFFTMag[j] * (AUD_Double)pFFTMag[j];
        }
    }
    // compute noise mean
    for ( j = 0; j < nSpecLen; j++ )
    {
        pNoiseMean[j] /= nNoiseFrame;
        pLamdaD[j]    /= nNoiseFrame;
    }

    AUD_Int32s vadFlag  = 0;
    AUD_Double noiseLen = 9.;
    AUD_Double alpha    = 0.99;
    k = 0;
    for ( i = 0; (i * frameStride + frameSize) < inLen; i++ )
    {
        win16s_calc( hWin, pInBuf + i * frameStride, pFrame );

        fft_calc( hFft, pFrame, frameSize, pFFTRe, pFFTIm );

        for ( j = 0; j < nSpecLen; j++ )
        {
            pFFTMag[j] = (AUD_Int32s)round( sqrt( (AUD_Double)pFFTRe[j] * pFFTRe[j] + (AUD_Double)pFFTIm[j] * pFFTIm[j] ) );
        }

#if 0
        AUDLOG( "noisy FFT:\n" );
        for ( j = 0; j < nSpecLen; j++ )
        {
            AUDLOG( "%d, ", pFFTMag[j] );
        }
        AUDLOG( "\n" );
#endif

        vadFlag = vad_nest( &vadState, pFrame, frameSize );
        if ( vadFlag == 0 )
        {
            for ( j = 0; j < nSpecLen; j++ )
            {
                pNoiseMean[j] = ( noiseLen * pNoiseMean[j] + (AUD_Double)pFFTMag[j] ) / ( noiseLen + 1. );
                pLamdaD[j]    = ( noiseLen * pLamdaD[j] + (AUD_Double)pFFTMag[j] * pFFTMag[j] ) / ( noiseLen + 1. );
            }
        }

        for ( j = 0; j < nSpecLen; j++ )
        {
            pGammaNew[j] = (AUD_Double)pFFTMag[j] * pFFTMag[j] / pLamdaD[j];
            pXi[j]       = alpha * pG[j] * pG[j] * pGamma[j] + ( 1. - alpha ) * AUD_MAX( pGammaNew[j] - 1., 0. );
            pGamma[j]    = pGammaNew[j];
            pG[j]        = pXi[j]  / ( pXi[j] + 1. );

            pFFTCleanMag[j] = pG[j] * pFFTMag[j];
        }

#if 0
        AUDLOG( "clean FFT:\n" );
        for ( j = 0; j < nSpecLen; j++ )
        {
            AUDLOG( "%.2f, ", pFFTCleanMag[j] );
        }
        AUDLOG( "\n" );
#endif

        // combine to real/im part of IFFT
        pFFTCleanRe[0] = pFFTCleanMag[0];
        pFFTCleanIm[0] = 0;
        AUD_Double costheta, sintheta;
        for ( j = 1; j < nSpecLen; j++ )
        {
            if ( pFFTMag[j] != 0 )
            {
                costheta = (AUD_Double)pFFTRe[j] / (AUD_Double)pFFTMag[j];
                sintheta = (AUD_Double)pFFTIm[j] / (AUD_Double)pFFTMag[j];
                pFFTCleanRe[nFFT - j] = pFFTCleanRe[j] = (AUD_Int32s)round( costheta * pFFTCleanMag[j] );
                pFFTCleanIm[j] = (AUD_Int32s)round( sintheta * pFFTCleanMag[j] );
                pFFTCleanIm[nFFT - j] = -pFFTCleanIm[j];
            }
            else
            {
                pFFTCleanRe[nFFT - j] = pFFTCleanRe[j] = pFFTCleanMag[j];
                pFFTCleanIm[nFFT - j] = pFFTCleanIm[j] = 0;
            }
        }

        ifft_real( hIfft, pFFTCleanRe, pFFTCleanIm, nFFT, pxClean );

#if 0
        AUDLOG( "clean FFT with phase:\n" );
        for ( j = 0; j < nFFT; j++ )
        {
            AUDLOG( "%d + j%d, ", pFFTCleanRe[j], pFFTCleanIm[j] );
        }
        AUDLOG( "\n" );
#endif

        for ( j = 0; j < frameSize; j++ )
        {
            pOutBuf[k + j] = pOutBuf[k + j] + pxClean[j];
        }

        k        += frameStride;
        cleanLen += frameStride;
    }

    win16s_free( &hWin );
    fft_free( &hFft );
    ifft_free( &hIfft );

    free( pFrame );
    free( pNoiseMean );
    free( pLamdaD );
    free( pXi );
    free( pGamma );
    free( pG );
    free( pGammaNew );

    free( pFFTMag );
    free( pFFTRe );
    free( pFFTIm );

    free( pFFTCleanMag );
    free( pFFTCleanRe );
    free( pFFTCleanIm );

    free( pxClean );

    return cleanLen;
}