shithub: aacenc

ref: 1dfd8c08ece7cba0be8eab1ffb2f00f80aa10ffc
dir: /transfo.c/

View raw version
#include <math.h>
#include <string.h>
#include "all.h"
#include "transfo.h"
#include "shape_win.h"

#define NFLAT_LS 448  // (BLOCK_LEN_LONG - BLOCK_LEN_SHORT) / 2

#ifndef M_PI
#define M_PI        3.14159265358979323846
#endif

#ifndef M_PI_2
#define M_PI_2      1.57079632679489661923
#endif

int sizedouble;
double wpi256,wpr256;
double wpi2048,wpr2048;

/*******************************************************************************
                               Fast MDCT & IMDCT Code
*******************************************************************************/
void MDCT (double *data, int N)
{
  double tempr, tempi, c, s, cold, cfreq, sfreq; /* temps for pre and post twiddle */
  double freq = 2.0 * M_PI / N;
  double fac,cosfreq8,sinfreq8;
  int i, n;
  int b = N >> 1;
  int N4 = N >> 2;
  int N2 = N >> 1;
  int a = N - b;
  int a2 = a >> 1;
  int a4 = a >> 2;
  int b4 = b >> 2;
  int unscambled;

  /* Choosing to allocate 2/N factor to Inverse Xform! */
  fac = 2.; /* 2 from MDCT inverse  to forward */

  /* prepare for recurrence relation in pre-twiddle */
  cfreq = cos (freq);
  sfreq = sin (freq);
  cosfreq8 = cos (freq * 0.125);
  sinfreq8 = sin (freq * 0.125);
  c = cosfreq8;
  s = sinfreq8;

  for (i = 0; i < N4; i++) {
    /* calculate real and imaginary parts of g(n) or G(p) */
    n = N / 2 - 1 - 2 * i;

    if (i < b4)
      tempr = data [a2 + n] + data [N + a2 - 1 - n]; /* use second form of e(n) for n = N / 2 - 1 - 2i */
    else
      tempr = data [a2 + n] - data [a2 - 1 - n]; /* use first form of e(n) for n = N / 2 - 1 - 2i */

    n = 2 * i;
    if (i < a4)
      tempi = data [a2 + n] - data [a2 - 1 - n]; /* use first form of e(n) for n=2i */
    else
      tempi = data [a2 + n] + data [N + a2 - 1 - n]; /* use second form of e(n) for n=2i*/

    /* calculate pre-twiddled FFT input */
    FFTarray [i].re = tempr * c + tempi * s;
    FFTarray [i].im = tempi * c - tempr * s;

    /* use recurrence to prepare cosine and sine for next value of i */
    cold = c;
    c = c * cfreq - s * sfreq;
    s = s * cfreq + cold * sfreq;
  }

  /* Perform in-place complex FFT of length N/4 */
  switch (N) {
    case 256:
      pfftw_64(FFTarray);
      break;
    case 2048:
      pfftw_512(FFTarray);
  }

  /* prepare for recurrence relations in post-twiddle */
  c = cosfreq8;
  s = sinfreq8;

  /* post-twiddle FFT output and then get output data */
  for (i = 0; i < N4; i++) {
    /* get post-twiddled FFT output  */
    /* Note: fac allocates 4/N factor from IFFT to forward and inverse */
    switch (N) {
      case 256:
      	unscambled = unscambled64[i];
      	break;
      case 2048:
	unscambled = unscambled512[i];
    }
    tempr = fac * (FFTarray [unscambled].re * c + FFTarray [unscambled].im * s);
    tempi = fac * (FFTarray [unscambled].im * c - FFTarray [unscambled].re * s);

    /* fill in output values */
    data [2 * i] = -tempr;   /* first half even */
    data [N2 - 1 - 2 * i] = tempi;  /* first half odd */
    data [N2 + 2 * i] = -tempi;  /* second half even */
    data [N - 1 - 2 * i] = tempr;  /* second half odd */

    /* use recurrence to prepare cosine and sine for next value of i */
    cold = c;
    c = c * cfreq - s * sfreq;
    s = s * cfreq + cold * sfreq;
  }
}

void IMDCT(double *data, int N)
{
  double tempr, tempi, c, s, cold, cfreq, sfreq; /* temps for pre and post twiddle */
  double freq = 2.0 * M_PI / N;
  double fac, cosfreq8, sinfreq8;
  int i;
  int Nd2 = N >> 1;
  int Nd4 = N >> 2;
  int Nd8 = N >> 3;
  int unscambled;

  /* Choosing to allocate 2/N factor to Inverse Xform! */
  fac = 2. / N; /* remaining 2/N from 4/N IFFT factor */

  /* prepare for recurrence relation in pre-twiddle */
  cfreq = cos (freq);
  sfreq = sin (freq);
  cosfreq8 = cos (freq * 0.125);
  sinfreq8 = sin (freq * 0.125);
  c = cosfreq8;
  s = sinfreq8;

  for (i = 0; i < Nd4; i++) {
    /* calculate real and imaginary parts of g(n) or G(p) */
    tempr = -data [2 * i];
    tempi = data [Nd2 - 1 - 2 * i];

    /* calculate pre-twiddled FFT input */
    switch (N) {
      case 256:
	unscambled = unscambled64[i];
	break;
      case 2048:
	unscambled = unscambled512[i];
    }

    FFTarray [unscambled].re = tempr * c - tempi * s;
    FFTarray [unscambled].im = tempi * c + tempr * s;

    /* use recurrence to prepare cosine and sine for next value of i */
    cold = c;
    c = c * cfreq - s * sfreq;
    s = s * cfreq + cold * sfreq;
	}

    /* Perform in-place complex IFFT of length N/4 */
    switch (N) {
      case 256:
        pfftwi_64(FFTarray);
	break;
      case 2048:pfftwi_512(FFTarray);
    }

    /* prepare for recurrence relations in post-twiddle */
    c = cosfreq8;
    s = sinfreq8;

    /* post-twiddle FFT output and then get output data */
    for (i = 0; i < Nd4; i++) {

    /* get post-twiddled FFT output  */
    tempr = fac * (FFTarray[i].re * c - FFTarray[i].im * s);
    tempi = fac * (FFTarray[i].im * c + FFTarray[i].re * s);

    /* fill in output values */
    data [Nd2 + Nd4 - 1 - 2 * i] = tempr;
    if (i < Nd8)
      data [Nd2 + Nd4 + 2 * i] = tempr;
    else
      data [2 * i - Nd4] = -tempr;

    data [Nd4 + 2 * i] = tempi;
    if (i < Nd8)
      data [Nd4 - 1 - 2 * i] = -tempi;
    else
      data [Nd4 + N - 1 - 2*i] = tempi;

    /* use recurrence to prepare cosine and sine for next value of i */
    cold = c;
    c = c * cfreq - s * sfreq;
    s = s * cfreq + cold * sfreq;
  }
}

void make_MDCT_windows(void)
{
  int i;
  for( i=0; i<BLOCK_LEN_LONG; i++ )
    sin_window_long[i] = sin((M_PI/(2*BLOCK_LEN_LONG)) * (i + 0.5));
  for( i=0; i<BLOCK_LEN_SHORT; i++ )
    sin_window_short[i] = sin((M_PI/(2*BLOCK_LEN_SHORT)) * (i + 0.5));
  sizedouble = sizeof (double);
}
/*******************************************************************************
                                    T/F mapping
*******************************************************************************/
void buffer2freq(double           p_in_data[],
		 double           p_out_mdct[],
		 double           p_overlap[],
		 enum WINDOW_TYPE block_type,
		 Window_shape     wfun_select,      /*  current window shape */
		 Window_shape     wfun_select_prev, /*  previous window shape */
		 Mdct_in          overlap_select
		 )
{
  double         transf_buf[ 2*BLOCK_LEN_LONG ];
  double         *p_o_buf, *first_window, *second_window;
  int            k,i;

  static int firstTime=1;

  /* create / shift old values */
  /* We use p_overlap here as buffer holding the last frame time signal*/
  if(overlap_select != MNON_OVERLAPPED){
    if (firstTime){
      firstTime=0;
      memset(transf_buf,0,BLOCK_LEN_LONG*sizedouble);
    }
    else
      memcpy(transf_buf,p_overlap,BLOCK_LEN_LONG*sizedouble);
    memcpy(transf_buf+BLOCK_LEN_LONG,p_in_data,BLOCK_LEN_LONG*sizedouble);
    memcpy(p_overlap,p_in_data,BLOCK_LEN_LONG*sizedouble);
  }
  else {
    memcpy(transf_buf,p_in_data,2*BLOCK_LEN_LONG*sizedouble);
  }

  /*  Window shape processing */
  switch (wfun_select_prev){
    case WS_SIN:
      if ( (block_type == ONLY_LONG_WINDOW) || (block_type == LONG_SHORT_WINDOW))
        first_window = sin_window_long;
      else
        first_window = sin_window_short;
      break;
    case WS_KBD:
      if ( (block_type == ONLY_LONG_WINDOW) || (block_type == LONG_SHORT_WINDOW))
        first_window = kbd_window_long;
      else
        first_window = kbd_window_short;
      break;
  }

  switch (wfun_select){
    case WS_SIN:
      if ( (block_type == ONLY_LONG_WINDOW) || (block_type == SHORT_LONG_WINDOW))
        second_window = sin_window_long;
      else
        second_window = sin_window_short;
      break;
    case WS_KBD:
      if ( (block_type == ONLY_LONG_WINDOW) || (block_type == SHORT_LONG_WINDOW))
        second_window = kbd_window_long;
      else
        second_window = kbd_window_short;
      break;
  }

  /* Set ptr to transf-Buffer */
  p_o_buf = transf_buf;

  /* Separate action for each Block Type */
  switch( block_type ) {
    case ONLY_LONG_WINDOW :
      for ( i = 0 ; i < BLOCK_LEN_LONG ; i++){
        p_out_mdct[i] = p_o_buf[i] * first_window[i];
        p_out_mdct[i+BLOCK_LEN_LONG] = p_o_buf[i+BLOCK_LEN_LONG] * second_window[BLOCK_LEN_LONG-i-1];
      }
      MDCT( p_out_mdct, 2*BLOCK_LEN_LONG );
      break;

    case LONG_SHORT_WINDOW :
      for ( i = 0 ; i < BLOCK_LEN_LONG ; i++)
         p_out_mdct[i] = p_o_buf[i] * first_window[i];
      memcpy(p_out_mdct+BLOCK_LEN_LONG,p_o_buf+BLOCK_LEN_LONG,NFLAT_LS*sizedouble);
//    for ( ; i < NFLAT_LS + BLOCK_LEN_LONG; i++){
//      p_out_mdct[i] = 1.0;
//      p_out_mdct[i+NFLAT_LS+BLOCK_LEN_SHORT] = 0.0;
//    }
      for ( i = 0 ; i < BLOCK_LEN_SHORT ; i++)
        p_out_mdct[i+BLOCK_LEN_LONG+NFLAT_LS] = p_o_buf[i+BLOCK_LEN_LONG+NFLAT_LS] * second_window[BLOCK_LEN_SHORT-i-1];
      memset(p_out_mdct+BLOCK_LEN_LONG+NFLAT_LS+BLOCK_LEN_SHORT,0,NFLAT_LS*sizedouble);
      MDCT( p_out_mdct, 2*BLOCK_LEN_LONG );
      break;

    case SHORT_LONG_WINDOW :
      memset(p_out_mdct,0,NFLAT_LS*sizedouble);
//    for ( i = 0 ; i < NFLAT_LS ; i++){
//      p_out_mdct[i] = 0.0;
//      p_out_mdct[i+NFLAT_LS+BLOCK_LEN_SHORT] = 1.0;
//    }
      for ( i = 0 ; i < BLOCK_LEN_SHORT ; i++)
        p_out_mdct[i+NFLAT_LS] = p_o_buf[i+NFLAT_LS] * first_window[i];
      memcpy(p_out_mdct+NFLAT_LS+BLOCK_LEN_SHORT,p_o_buf+NFLAT_LS+BLOCK_LEN_SHORT,NFLAT_LS*sizedouble);
      for ( i = 0 ; i < BLOCK_LEN_LONG ; i++)
        p_out_mdct[i+BLOCK_LEN_LONG] = p_o_buf[i+BLOCK_LEN_LONG] * second_window[BLOCK_LEN_LONG-i-1];
	MDCT( p_out_mdct, 2*BLOCK_LEN_LONG );
      break;

    case ONLY_SHORT_WINDOW :
      if(overlap_select != MNON_OVERLAPPED){ /* YT 970615 for sonPP */
      	p_o_buf += NFLAT_LS;
      }
      for ( k=0; k < MAX_SHORT_WINDOWS; k++ ) {
        for ( i = 0 ; i < BLOCK_LEN_SHORT ; i++ ){
          p_out_mdct[i] = p_o_buf[i] * first_window[i];
          p_out_mdct[i+BLOCK_LEN_SHORT] = p_o_buf[i+BLOCK_LEN_SHORT] * second_window[BLOCK_LEN_SHORT-i-1];
          }
        MDCT( p_out_mdct, 2*BLOCK_LEN_SHORT );
        p_out_mdct += BLOCK_LEN_SHORT;
	if(overlap_select != MNON_OVERLAPPED)
          p_o_buf += BLOCK_LEN_SHORT;
        else
          p_o_buf += 2*BLOCK_LEN_SHORT;
        first_window = second_window;
      }
      break;
  }
}

void freq2buffer(double           p_in_data[],
		 double           p_out_data[],
		 double           p_overlap[],
		 enum WINDOW_TYPE block_type,
		 Window_shape     wfun_select,      /*  current window shape */
		 Window_shape     wfun_select_prev, /*  previous window shape */
		 Mdct_in	  overlap_select
		 )
{
  double           *o_buf, transf_buf[ 2*BLOCK_LEN_LONG ];
  double           overlap_buf[ 2*BLOCK_LEN_LONG ], *first_window, *second_window;

  double  *fp;
  int     k,i;

  /*  Window shape processing */
  switch (wfun_select_prev){
    case WS_SIN:
      if ( (block_type == ONLY_LONG_WINDOW) || (block_type == LONG_SHORT_WINDOW))
        first_window = sin_window_long;
      else
        first_window = sin_window_short;
      break;
    case WS_KBD:
      if ( (block_type == ONLY_LONG_WINDOW) || (block_type == LONG_SHORT_WINDOW))
        first_window = kbd_window_long;
      else
        first_window = kbd_window_short;
      break;
  }

  switch (wfun_select){
    case WS_SIN:
      if ( (block_type == ONLY_LONG_WINDOW) || (block_type == SHORT_LONG_WINDOW))
        second_window = sin_window_long;
      else
        second_window = sin_window_short;
      break;
    case WS_KBD:
      if ( (block_type == ONLY_LONG_WINDOW) || (block_type == SHORT_LONG_WINDOW))
        second_window = kbd_window_long;
      else
        second_window = kbd_window_short;
      break;
  }

  /* Assemble overlap buffer */
  memcpy(overlap_buf,p_overlap,BLOCK_LEN_LONG*sizedouble);
  o_buf = overlap_buf;

  /* Separate action for each Block Type */
  switch( block_type ) {
    case ONLY_LONG_WINDOW :
      memcpy(transf_buf, p_in_data,BLOCK_LEN_LONG*sizedouble);
      IMDCT( transf_buf, 2*BLOCK_LEN_LONG );
      for ( i = 0 ; i < BLOCK_LEN_LONG ; i++)
        transf_buf[i] *= first_window[i];
      if (overlap_select != MNON_OVERLAPPED) {
        for ( i = 0 ; i < BLOCK_LEN_LONG; i++ ){
          o_buf[i] += transf_buf[i];
          o_buf[i+BLOCK_LEN_LONG] = transf_buf[i+BLOCK_LEN_LONG] * second_window[BLOCK_LEN_LONG-i-1];
        }
      }
      else { /* overlap_select == NON_OVERLAPPED */
        for ( i = 0 ; i < BLOCK_LEN_LONG; i++ )
          transf_buf[i+BLOCK_LEN_LONG] *= second_window[BLOCK_LEN_LONG-i-1];
        }
      break;

    case LONG_SHORT_WINDOW :
      memcpy(transf_buf, p_in_data,BLOCK_LEN_LONG*sizedouble);
      IMDCT( transf_buf, 2*BLOCK_LEN_LONG );
      for ( i = 0 ; i < BLOCK_LEN_LONG ; i++)
        transf_buf[i] *= first_window[i];
        if (overlap_select != MNON_OVERLAPPED) {
          for ( i = 0 ; i < BLOCK_LEN_LONG; i++ )
            o_buf[i] += transf_buf[i];
          memcpy(o_buf+BLOCK_LEN_LONG,transf_buf+BLOCK_LEN_LONG,NFLAT_LS*sizedouble);
          for ( i = 0 ; i < BLOCK_LEN_SHORT ; i++)
            o_buf[i+BLOCK_LEN_LONG+NFLAT_LS] = transf_buf[i+BLOCK_LEN_LONG+NFLAT_LS] * second_window[BLOCK_LEN_SHORT-i-1];
          memset(o_buf+BLOCK_LEN_LONG+NFLAT_LS+BLOCK_LEN_SHORT,0,NFLAT_LS*sizedouble);
        }
        else { /* overlap_select == NON_OVERLAPPED */
          for ( i = 0 ; i < BLOCK_LEN_SHORT ; i++)
            transf_buf[i+BLOCK_LEN_LONG+NFLAT_LS] *= second_window[BLOCK_LEN_SHORT-i-1];
          memset(transf_buf+BLOCK_LEN_LONG+NFLAT_LS+BLOCK_LEN_SHORT,0,NFLAT_LS*sizedouble);
        }
        break;

    case SHORT_LONG_WINDOW :
      memcpy(transf_buf, p_in_data,BLOCK_LEN_LONG*sizedouble);
      IMDCT( transf_buf, 2*BLOCK_LEN_LONG );
      for ( i = 0 ; i < BLOCK_LEN_SHORT ; i++)
        transf_buf[i+NFLAT_LS] *= first_window[i];
      if (overlap_select != MNON_OVERLAPPED) {
        for ( i = 0 ; i < BLOCK_LEN_SHORT; i++ )
          o_buf[i+NFLAT_LS] += transf_buf[i+NFLAT_LS];
        memcpy(o_buf+BLOCK_LEN_SHORT+NFLAT_LS,transf_buf+BLOCK_LEN_SHORT+NFLAT_LS,NFLAT_LS*sizedouble);
        for ( i = 0 ; i < BLOCK_LEN_LONG ; i++)
          o_buf[i+BLOCK_LEN_LONG] = transf_buf[i+BLOCK_LEN_LONG] * second_window[BLOCK_LEN_LONG-i-1];
      }
      else { /* overlap_select == NON_OVERLAPPED */
        memset(transf_buf,0,NFLAT_LS*sizedouble);
        for ( i = 0 ; i < BLOCK_LEN_LONG ; i++)
          transf_buf[i+BLOCK_LEN_LONG] *= second_window[BLOCK_LEN_LONG-i-1];
      }
      break;

    case ONLY_SHORT_WINDOW :
      if (overlap_select != MNON_OVERLAPPED) {
        fp = o_buf + NFLAT_LS;
      }
      else { /* overlap_select == NON_OVERLAPPED */
      fp = transf_buf;
      }
      for ( k=0; k < MAX_SHORT_WINDOWS; k++ ) {
        memcpy(transf_buf,p_in_data,BLOCK_LEN_SHORT*sizedouble);
        IMDCT( transf_buf, 2*BLOCK_LEN_SHORT );
        p_in_data += BLOCK_LEN_SHORT;
        if (overlap_select != MNON_OVERLAPPED) {
          for ( i = 0 ; i < BLOCK_LEN_SHORT ; i++){
            transf_buf[i] *= first_window[i];
            fp[i] += transf_buf[i];
            fp[i+BLOCK_LEN_SHORT] = transf_buf[i+BLOCK_LEN_SHORT] * second_window[BLOCK_LEN_SHORT-i-1];
          }
    	fp    += BLOCK_LEN_SHORT;
        }
        else { /* overlap_select == NON_OVERLAPPED */
          for ( i = 0 ; i < BLOCK_LEN_SHORT ; i++){
            fp[i] *= first_window[i];
            fp[i+BLOCK_LEN_SHORT] *= second_window[BLOCK_LEN_SHORT-i-1];
          }
      	  fp += 2*BLOCK_LEN_SHORT;
        }
        first_window = second_window;
      }
      memset(o_buf+BLOCK_LEN_LONG+NFLAT_LS+BLOCK_LEN_SHORT,0,NFLAT_LS*sizedouble);
      break;
  }

  if (overlap_select != MNON_OVERLAPPED)
    memcpy(p_out_data,o_buf,BLOCK_LEN_LONG*sizedouble);
  else  /* overlap_select == NON_OVERLAPPED */
    memcpy(p_out_data,transf_buf,2*BLOCK_LEN_LONG*sizedouble);

  /* save unused output data */
  memcpy(p_overlap,o_buf+BLOCK_LEN_LONG,BLOCK_LEN_LONG*sizedouble);
}

/******************************************************************************/

void specFilter (double p_in[],
                 double p_out[],
		 int  samp_rate,
		 int lowpass_freq,
		 int    specLen
		 )
{
  int lowpass,xlowpass;

  /* calculate the last line which is not zero */
  lowpass = (lowpass_freq * specLen) / (samp_rate>>1) + 1;
  xlowpass = (lowpass < specLen) ? lowpass : specLen ;

  if( p_out != p_in )
    memcpy(p_out,p_in,specLen*sizedouble);
  memset(p_out+xlowpass,0,(specLen-xlowpass)*sizedouble);
}

void initrft(void)
{
  double theta,wtemp;

  theta  = -M_PI/128.0;
  wpi256 = sin(theta);
  wtemp  = sin(0.5*theta);
  wpr256 = -2.0*wtemp*wtemp;

  theta   = -M_PI/1024.0;
  wpi2048 = sin(theta);
  wtemp   = sin(0.5*theta);
  wpr2048 = -2.0*wtemp*wtemp;
}

void realft256(double *data)
{
  int i,i1,i2,i3,i4;
  double h1r,h1i,h2r,h2i,t1,t2,t3;
  double wr,wi,wtemp;

  pfftw_128(FFTarray);
  for (i = 0; i < 128; i++){
    data[(i<<1)] = FFTarray[unscambled128[i]].re;
    data[(i<<1)+1]= FFTarray[unscambled128[i]].im;
  }

  wr=1.0+wpr256;
  wi=wpi256;
  for (i=1;i<64;i++) {
    i4=1+(i3=257-(i2=1+(i1=i+i)));
    h1r=0.5*(data[i1]+data[i3]);
    h1i=0.5*(data[i2]-data[i4]);
    h2r=0.5*(data[i2]+data[i4]);
    h2i=-0.5*(data[i1]-data[i3]);
    t1=wr*h2r;
    t2=wi*h2i;
    t3=wr*h2i+wi*h2r;

    data[i1]=h1r+t1-t2;
    data[i2]=h1i+t3;
    data[i3]=h1r-t1+t2;
    data[i4]=-h1i+t3;
    wtemp=wr;
    wr=wtemp*wpr256-wi*wpi256+wr;
    wi=wi*wpr256+wtemp*wpi256+wi;
  }
  data[0] = data[0]+data[1];
  data[1] = 0;
  data[129] = -data[129]; // hack to emulate complex FFT
}

void realft2048(double *data)
{
  int i,i1,i2,i3,i4;
  double h1r,h1i,h2r,h2i,t1,t2,t3;
  double wr,wi,wtemp;

  pfftw_1024(FFTarray);
  for (i = 0; i < 1024; i++){
    data[(i<<1)] = FFTarray[unscambled1024[i]].re;
    data[(i<<1)+1]= FFTarray[unscambled1024[i]].im;
  }

  wr=1.0+wpr2048;
  wi=wpi2048;
  for (i=1;i<512;i++) {
    i4=1+(i3=2049-(i2=1+(i1=i+i)));
    h1r=0.5*(data[i1]+data[i3]);
    h1i=0.5*(data[i2]-data[i4]);
    h2r=0.5*(data[i2]+data[i4]);
    h2i=-0.5*(data[i1]-data[i3]);
    t1=wr*h2r;
    t2=wi*h2i;
    t3=wr*h2i+wi*h2r;

    data[i1]=h1r+t1-t2;
    data[i2]=h1i+t3;
    data[i3]=h1r-t1+t2;
    data[i4]=-h1i+t3;
    wtemp=wr;
    wr=wtemp*wpr2048-wi*wpi2048+wr;
    wi=wi*wpr2048+wtemp*wpi2048+wi;
  }
  data[0] = data[0]+data[1];
  data[1] = 0;
  data[1025] = -data[1025]; // hack to emulate complex FFT
}