divers.c

来自「Time-Frequency Toolbox,其中包含很常用的MATLAB程序」· C语言 代码 · 共 1,075 行 · 第 1/3 页

/*====================================================================*
 * this file contains several programs used in a signal processing    *
 * context. The programs are :                                        *
 * - fft       : Computes the fast Fourier transform of a complex     *
 *               signal                                               *
 * - po2       : Computes the next higher power of two                *
 * - idx       : In a matrix stored like a vector, computes the       *
 *               vector index corresponding to given line and column  *
 *               numbers in the matrix                                *
 * - ifft      : Computes the inverse fft                             *
 * - sqr       : square of a number                                   *
 * - powof     : Computes x to the power of alpha                     *
 * - Recover_Signal : In programs used inside the Matlab              *
 *                environement, recovers a signal from a matlab       *
 *                function                                            *
 * - gradient  : Computes the bidimensional gradient of a surface     *
 *               (called 'field of potential')                        *
 *====================================================================*/


/*====================================================================*
 * Name of the function : powof (double)                               *
 * Author               : Manuel DAVY - IRCYN                         *
 * Date of creation     : 02 - 02 - 1999                              *
 *--------------------------------------------------------------------*
 * Action of the function                                             *
 *                                                                    *
 *  Computes x^alpha and takes the case x=0 into account              *
 *  !!!!!!!!!!!!!!!!!!!!! WARNING !!!!!!!!!!!!!!!!!!!!!!              *
 *  x must be non-negative when alpha is not an interger              *
 *  !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!              *
 *====================================================================*/

double
powof (double x, double alpha)
{
  double         resu;



  if (x >= 0) /* in this case, no problem */
    {
      if (x == 0)
	resu = 0.0;
      else
	resu = exp (alpha * log (x));
    }
  else /* there may be problems */
    {
      if (alpha == (int)(alpha)) /* if alpha is an integer */
	{
	  /* then x^alpha is real-valued */ 
	  resu = powof ( -x, alpha);
	  /* and the sign of the results depends on
	     wether alpha is ODD or EVEN */
	  if (ISODD(alpha) == 1)
	    {
	      /* alpha is ODD */
	      resu = -resu;
	    }
	}
      else
	{
	  printf ("Attempt to compute x^alpha with x<0 : complex valued result\n");
	  exit(0);
	}
    }
  return resu;
}


/*                                                                    *
 *====================================================================*
 * Name of the function :                			      *
 * Author               :                                             *
 * Date of creation     :                                             *
 *--------------------------------------------------------------------*
 * THE ALGORITHM             				              *
								      *
								      *
 *====================================================================*
 * INPUT VARIABLES   					              *
 * Name        | type  |              role               	      *
 *             |       | kernel 				      *
 *--------------------------------------------------------------------*
 * OUTPUT VARIABLES    						      *
 * Name        | type  |              role                	      *
 *								      *
 *--------------------------------------------------------------------*
 * INTERNAL VARIABLES 						      *
 * Name        | type  |              role                 	      *
 *								      *
 *====================================================================*
 * SUBROUTINES USED HERE				      	      *
 *--------------------------------------------------------------------*
 * Name   |                                          	     	      *
 * Action |							      *
 * Place  | 							      *
 *====================================================================*/

void
fft (int Signal_Length, int Nfft, double *sig_real, double *sig_imag)
{
  /*------------------------------------------------------------------*/
  /*          when the signal length is a power of two                */
  /*------------------------------------------------------------------*/
  if (Signal_Length == (int) powof (2.0, Nfft) + 1)
    {
      /* local variables */
      int            i, j, k, n, n2;
      double         c, s, e, a, t1, t2;

      j = 0;			/* bit-reverse  */
      n2 = Signal_Length / 2;
      for (i = 1; i < Signal_Length - 1; i++)
	{
	  n = n2;
	  while (j >= n)
	    {
	      j = j - n;
	      n = n / 2;
	    }
	  j = j + n;

	  if (i < j)
	    {
	      t1 = sig_real[i];
	      sig_real[i] = sig_real[j];
	      sig_real[j] = t1;
	      t1 = sig_imag[i];
	      sig_imag[i] = sig_imag[j];
	      sig_imag[j] = t1;
	    }
	}


      n = 0;			/* FFT  */
      n2 = 1;

      for (i = 0; i < Nfft; i++)
	{
	  n = n2;
	  n2 = n2 + n2;
	  e = -6.283185307179586 / n2;
	  a = 0.0;

	  for (j = 0; j < n; j++)
	    {
	      c = cos (a);
	      s = sin (a);
	      a = a + e;

	      for (k = j; k < Signal_Length; k = k + n2)
		{
		  t1 = c * sig_real[k + n] - s * sig_imag[k + n];
		  t2 = s * sig_real[k + n] + c * sig_imag[k + n];
		  sig_real[k + n] = sig_real[k] - t1;
		  sig_imag[k + n] = sig_imag[k] - t2;
		  sig_real[k] = sig_real[k] + t1;
		  sig_imag[k] = sig_imag[k] + t2;
		}
	    }
	}
    }
  /*------------------------------------------------------------------*/
  /*        when the signal length is NOT a power of two              */
  /*            Calls the matlab subroutine fft                       */
  /*------------------------------------------------------------------*/
  else
    {
      int            num_out, num_in, i;
      mxArray       *outputArray[1];
      mxArray       *inputArray[1];
      mxArray       *array_ptr;


      num_out = 1;
      num_in = 1;

      /* recopy the real and imag parts of the signal in matrices */
      array_ptr = mxCreateDoubleMatrix (1, Signal_Length, mxCOMPLEX);
      memcpy (mxGetPr (array_ptr), sig_real, Signal_Length * sizeof (double));
      memcpy (mxGetPi (array_ptr), sig_imag, Signal_Length * sizeof (double));
      inputArray[0] = array_ptr;

      /* calls the MATLAB function */
      mexCallMATLAB (num_out, outputArray, num_in, inputArray, "fft");
      memcpy (sig_real, mxGetPr (outputArray[0]), Signal_Length * sizeof (double));

      if (mxIsComplex (outputArray[0]))
	{
	  memcpy (sig_imag, mxGetPi (outputArray[0]), Signal_Length * sizeof (double));
	}
      else
	{
	  for (i = 0; i < Signal_Length; i++)
	    sig_imag[i] = 0;
	}
      /* free memory */
      mxDestroyArray (outputArray[0]);
      mxDestroyArray (inputArray[0]);
    }
  return;
}

/*====================================================================*
 * Name of the function : po2 (int)                                   *
 * Author               :                                             *
 * Date of creation     :                                             *
 *--------------------------------------------------------------------*
 * Action of the function                                             *
 * Computes the next power of to of an integer n, i.e.                *
 * the smallest k such that m=2^k>=n                                  *
 *====================================================================*/

int
po2 (int n)
{
  int            nextpo2, m;

  m = 1;
  nextpo2 = 0;
  while (m < n)
    {
      ++nextpo2;
      m = m + m;
    }

  return (nextpo2);
}

/*====================================================================*
 * Name of the function : sinc (double)                               *
 * Author               : Emmanuel ROY                                *
 * Date of creation     : 22 - 06 - 1999                              *
 *--------------------------------------------------------------------*
 * Action of the function                                             *
 * Computes the sinc function of a number, defined by :               *
 *                                                                    *
 *   for any x in R,                                                  *
 *                                                                    *
 *             /                                                      *
 *             | 1 if x = 0                                           *
 *             |                                                      *
 *   sinc(x) = | sin (pi * x)                                         *
 *             | ------------ if x != 0                               *
 *             |   pi * x                                             *
 *             \                                                      *
 *                                                                    *
 *====================================================================*/




double sinc (double x)
{
  double         resu;

  if (x == 0)
   	resu = 1.0;
  else
	resu = sin(pi*x)/(pi*x);

  return resu;
}


/*====================================================================*
 * Name of the function : irem (double x, double y)                   *
 * Author               : Emmanuel ROY                                *
 * Date of creation     : 22 - 06 -1999                               *
 *--------------------------------------------------------------------*
 * Action of the function                                             *
 *                                                                    *
 * Computes the remainder after Euclidean division of two doubles     *
 *                                                                    *
 *====================================================================*/

int irem( double x, double y)
{
 int result;

 if (y != 0)
   {
     result =  x-y* (int)(x/y);
   }
 else
   {
     result = 0;
     printf("irem.c : attempt to divide by 0\n");
   }

 return result;
}

/*====================================================================*
 * Name of the function : idx (int i_row, int j_col, int nb_row)      *
 * Author               : Emmanuel ROY                                *
 * Date of creation     :                                             *
 *--------------------------------------------------------------------*
 * Action of the function                                             *
 *                                                                    *
 * The matrices are stored as vectors, column by column               *
 * This program computes the vector index corresponding to the        *
 * specified line number (line), the column number (col) and the      *
 * number of lines   (nb_line)                                        *
 *====================================================================*/

int
idx (int i_row, int j_col, int nb_row)
{
  if (i_row >= nb_row)
    {
      printf("idx : incorrect row number\n");
      return 0;
      exit(0);
    }
  else
    {
      return (i_row + (j_col * nb_row));
    }
}

/*                                                		      *
 *====================================================================*
 * Name of the function :                			      *
 * Author               :                                             *
 * Date of creation     :                                             *
 *--------------------------------------------------------------------*
 * THE ALGORITHM             				              *
								      *
								      *
 *====================================================================*
 * INPUT VARIABLES   					              *
 * Name        | type  |              role               	      *
 *             |       | kernel 				      *
 *--------------------------------------------------------------------*
 * OUTPUT VARIABLES    						      *
 * Name        | type  |              role                	      *
 *								      *
 *--------------------------------------------------------------------*
 * INTERNAL VARIABLES 						      *
 * Name        | type  |              role                 	      *
 *								      *
 *====================================================================*
 * SUBROUTINES USED HERE				      	      *
 *--------------------------------------------------------------------*
 * Name   |                                          	     	      *
 * Action |							      *
 * Place  | 							      *
 *====================================================================*/

void
ifft (int Signal_Length, int Nfft, double *sig_real, double *sig_imag)
{
  /*------------------------------------------------------------------*/
  /*          when the signal length is a power of two                */
  /*------------------------------------------------------------------*/
  if (Signal_Length == (int) powof (2, Nfft) + 1)