resample.c

完整的RTP RTSP代码库

		  /* fprintf(stderr,"Nproc %ld, creep %ld\n",Nproc,creep); */
		}
	}

	{
	long i,k;
	/* Copy back portion of input signal that must be re-used */
	k = r->Xp - r->Xoff;
	/*fprintf(stderr,"k %d, last %d\n",k,last);*/
	for (i=0; i<last - k; i++) 
	    r->X[i] = r->X[i+k];

	/* Pos in input buff to read new data into */
	r->Xread = i;                 
	r->Xp = r->Xoff;

	for(i=0; i < Nout; i++) {
	  float final = r->Y[i] * ISCALE;
	  if (final >= 32767.0) final = 32767.0;
	  else if (final <= -32768.0) final = -32768.0;
	  *obuf = final;
	  obuf += chans;
	}

	*isamp = Nx;
	*osamp = Nout;

	}
	return (0);
}

#if 0
/*
 * Process tail of input samples.
 */
int st_resample_drain(eff_t effp, st_sample_t *obuf, st_size_t *osamp)
{
	resample_t r = (resample_t) effp->priv;
	long isamp_res, osamp_res;
	st_sample_t *Obuf;
	int rc;

	/* fprintf(stderr,"Xoff %d, Xt %d  <--- DRAIN\n",r->Xoff, r->Xt); */

	/* stuff end with Xoff zeros */
	isamp_res = r->Xoff;
	osamp_res = *osamp;
	Obuf = obuf;
	while (isamp_res>0 && osamp_res>0) {
		st_sample_t Isamp, Osamp;
		Isamp = isamp_res;
		Osamp = osamp_res;
		rc = st_resample_flow(effp, NULL, Obuf, &Isamp, &Osamp);
		if (rc)
		    return rc;
	  /* fprintf(stderr,"DRAIN isamp,osamp  (%d,%d) -> (%d,%d)\n",
		     isamp_res,osamp_res,Isamp,Osamp); */
		Obuf += Osamp;
		osamp_res -= Osamp;
		isamp_res -= Isamp;
	}
	*osamp -= osamp_res;
	/* fprintf(stderr,"DRAIN osamp %d\n", *osamp); */
	if (isamp_res)
		st_warn("drain overran obuf by %d\n", isamp_res); fflush(stderr);
	return (ST_SUCCESS);
}

#endif
/*
 * Do anything required when you stop reading samples.  
 * Don't close input file! 
 */
void  st_resample_stop(resample_t r)
{
	free(r->Imp - 1);
	free(r->X);
	free(r);
	/* free(r->Y); Y is in same block starting at X */ 
	//return (ST_SUCCESS);
}
/* over 90% of CPU time spent in this iprodUD() function */
/* quadratic interpolation */
static double qprodUD(const Float Imp[], const Float *Xp, long Inc, double T0, 
	              long dhb, long ct)
{
  const double f = 1.0/(1<<La);
  double v;
  long Ho;

  Ho = T0 * dhb;
  Ho += (ct-1)*dhb; /* so Float sum starts with smallest coef's */
  Xp += (ct-1)*Inc;
  v = 0;
  do {
    Float coef;
    long Hoh;
    Hoh = Ho>>La;
    coef = Imp[Hoh];
    {
      Float dm,dp,t;
      dm = coef - Imp[Hoh-1];
      dp = Imp[Hoh+1] - coef;
      t =(Ho & Amask) * f;
      coef += ((dp-dm)*t + (dp+dm))*t*0.5;
    }
    /* filter coef, lower La bits by quadratic interpolation */
    v += coef * *Xp;   /* sum coeff * input sample */
    Xp -= Inc;     /* Input signal step. NO CHECK ON ARRAY BOUNDS */
    Ho -= dhb;     /* IR step */
  } while(--ct);
  return v;
}

/* linear interpolation */
static double iprodUD(const Float Imp[], const Float *Xp, long Inc, 
	              double T0, long dhb, long ct)
{
  const double f = 1.0/(1<<La);
  double v;
  long Ho;

  Ho = T0 * dhb;
  Ho += (ct-1)*dhb; /* so Float sum starts with smallest coef's */
  Xp += (ct-1)*Inc;
  v = 0;
  do {
    Float coef;
    long Hoh;
    Hoh = Ho>>La;
    /* if (Hoh >= End) break; */
    coef = Imp[Hoh] + (Imp[Hoh+1]-Imp[Hoh]) * (Ho & Amask) * f;
    /* filter coef, lower La bits by linear interpolation */
    v += coef * *Xp;   /* sum coeff * input sample */
    Xp -= Inc;     /* Input signal step. NO CHECK ON ARRAY BOUNDS */
    Ho -= dhb;     /* IR step */
  } while(--ct);
  return v;
}

/* From resample:filters.c */
/* Sampling rate conversion subroutine */

static long SrcUD(resample_t r, long Nx)
{
   Float *Ystart, *Y;
   double Factor;
   double dt;                  /* Step through input signal */
   double time;
   double (*prodUD)();
   int n;

   prodUD = (r->quadr)? qprodUD:iprodUD; /* quadratic or linear interp */
   Factor = r->Factor;
   time = r->Time;
   dt = 1.0/Factor;        /* Output sampling period */
   /*fprintf(stderr,"Factor %f, dt %f, ",Factor,dt); */
   /*fprintf(stderr,"Time %f, ",r->Time);*/
   /* (Xh * dhb)>>La is max index into Imp[] */
   /*fprintf(stderr,"ct=%d\n",ct);*/
   /*fprintf(stderr,"ct=%.2f %d\n",(double)r->Nwing*Na/r->dhb, r->Xh);*/
   /*fprintf(stderr,"ct=%ld, T=%.6f, dhb=%6f, dt=%.6f\n",
		         r->Xh, time-floor(time),(double)r->dhb/Na,dt);*/
   Ystart = Y = r->Y;
   n = (int)ceil((double)Nx/dt);
   while(n--)
      {
      Float *Xp;
      double v;
      double T;
      T = time-floor(time);        /* fractional part of Time */
      Xp = r->X + (long)time;      /* Ptr to current input sample */

      /* Past  inner product: */
      v = (*prodUD)(r->Imp, Xp, -1, T, r->dhb, r->Xh); /* needs Np*Nmult in 31 bits */
      /* Future inner product: */
      v += (*prodUD)(r->Imp, Xp+1, 1, (1.0-T), r->dhb, r->Xh); /* prefer even total */

      if (Factor < 1) v *= Factor;
      *Y++ = v;              /* Deposit output */
      time += dt;            /* Move to next sample by time increment */
      }
   r->Time = time;
   /*fprintf(stderr,"Time %f\n",r->Time);*/
   return (Y - Ystart);        /* Return the number of output samples */
}

/* exact coeff's */
static double prodEX(const Float Imp[], const Float *Xp, 
	             long Inc, long T0, long dhb, long ct)
{
  double v;
  const Float *Cp;

  Cp  = Imp + (ct-1)*dhb + T0; /* so Float sum starts with smallest coef's */
  Xp += (ct-1)*Inc;
  v = 0;
  do {
    v += *Cp * *Xp;   /* sum coeff * input sample */
    Cp -= dhb;     /* IR step */
    Xp -= Inc;     /* Input signal step. */
  } while(--ct);
  return v;
}

static long SrcEX(resample_t r, long Nx)
{
   Float *Ystart, *Y;
   double Factor;
   long a,b;
   long time;
   int n;

   Factor = r->Factor;
   time = r->t;
   a = r->a;
   b = r->b;
   Ystart = Y = r->Y;
   n = (Nx*b + (a-1))/a;
   while(n--)
      {
	Float *Xp;
	double v;
	long T;
	T = time % b;              /* fractional part of Time */
	Xp = r->X + (time/b);      /* Ptr to current input sample */

	/* Past  inner product: */
	v = prodEX(r->Imp, Xp, -1, T, b, r->Xh);
	/* Future inner product: */
	v += prodEX(r->Imp, Xp+1, 1, b-T, b, r->Xh);

	if (Factor < 1) v *= Factor;
	*Y++ = v;             /* Deposit output */
	time += a;            /* Move to next sample by time increment */
      }
   r->t = time;
   return (Y - Ystart);        /* Return the number of output samples */
}

int makeFilter(Float Imp[], long Nwing, double Froll, double Beta, 
	       long Num, int Normalize)
{
   double *ImpR;
   long Mwing, i;

   if (Nwing > MAXNWING)                      /* Check for valid parameters */
      return(-1);
   if ((Froll<=0) || (Froll>1))
      return(-2);

   /* it does help accuracy a bit to have the window stop at
    * a zero-crossing of the sinc function */
   Mwing = floor((double)Nwing/(Num/Froll))*(Num/Froll) +0.5;
   if (Mwing==0)
      return(-4);

   ImpR = (double *) malloc(sizeof(double) * Mwing);

   /* Design a Nuttall or Kaiser windowed Sinc low-pass filter */
   LpFilter(ImpR, Mwing, Froll, Beta, Num);

   if (Normalize) { /* 'correct' the DC gain of the lowpass filter */
      long Dh;
      double DCgain;
      DCgain = 0;
      Dh = Num;                  /* Filter sampling period for factors>=1 */
      for (i=Dh; i<Mwing; i+=Dh)
         DCgain += ImpR[i];
      DCgain = 2*DCgain + ImpR[0];    /* DC gain of real coefficients */
      /*st_report("DCgain err=%.12f",DCgain-1.0);*/
  
      DCgain = 1.0/DCgain;
      for (i=0; i<Mwing; i++)
         Imp[i] = ImpR[i]*DCgain;

   } else {
      for (i=0; i<Mwing; i++)
         Imp[i] = ImpR[i];
   }
   free(ImpR);
   for (i=Mwing; i<=Nwing; i++) Imp[i] = 0;
   /* Imp[Mwing] and Imp[-1] needed for quadratic interpolation */
   Imp[-1] = Imp[1];

   return(Mwing);
}

/* LpFilter()
 *
 * reference: "Digital Filters, 2nd edition"
 *            R.W. Hamming, pp. 178-179
 *
 * Izero() computes the 0th order modified bessel function of the first kind.
 *    (Needed to compute Kaiser window).
 *
 * LpFilter() computes the coeffs of a Kaiser-windowed low pass filter with
 *    the following characteristics:
 *
 *       c[]  = array in which to store computed coeffs
 *       frq  = roll-off frequency of filter
 *       N    = Half the window length in number of coeffs
 *       Beta = parameter of Kaiser window
 *       Num  = number of coeffs before 1/frq
 *
 * Beta trades the rejection of the lowpass filter against the transition
 *    width from passband to stopband.  Larger Beta means a slower
 *    transition and greater stopband rejection.  See Rabiner and Gold
 *    (Theory and Application of DSP) under Kaiser windows for more about
 *    Beta.  The following table from Rabiner and Gold gives some feel
 *    for the effect of Beta:
 *
 * All ripples in dB, width of transition band = D*N where N = window length
 *
 *               BETA    D       PB RIP   SB RIP
 *               2.120   1.50  +-0.27      -30
 *               3.384   2.23    0.0864    -40
 *               4.538   2.93    0.0274    -50
 *               5.658   3.62    0.00868   -60
 *               6.764   4.32    0.00275   -70
 *               7.865   5.0     0.000868  -80
 *               8.960   5.7     0.000275  -90
 *               10.056  6.4     0.000087  -100
 */


#define IzeroEPSILON 1E-21               /* Max error acceptable in Izero */

static double Izero(double x)
{
   double sum, u, halfx, temp;
   long n;

   sum = u = n = 1;
   halfx = x/2.0;
   do {
      temp = halfx/(double)n;
      n += 1;
      temp *= temp;
      u *= temp;
      sum += u;
   } while (u >= IzeroEPSILON*sum);
   return(sum);
}

static void LpFilter(double *c, long N, double frq, double Beta, long Num)
{
   long i;

   /* Calculate filter coeffs: */
   c[0] = frq;
   for (i=1; i<N; i++) {
      double x = M_PI*(double)i/(double)(Num);
      c[i] = sin(x*frq)/x;
   }
  
   if (Beta>2) { /* Apply Kaiser window to filter coeffs: */
      double IBeta = 1.0/Izero(Beta);
      for (i=1; i<N; i++) {
         double x = (double)i / (double)(N);
         c[i] *= Izero(Beta*sqrt(1.0-x*x)) * IBeta;
      }
   } else { /* Apply Nuttall window: */
      for(i = 0; i < N; i++) {
         double x = M_PI*i / N;
         c[i] *= 0.36335819 + 0.4891775*cos(x) + 0.1365995*cos(2*x) + 0.0106411*cos(3*x);
      }
   }
}