sugain.c

su 的源代码库

                        *dataptr = clip;
                } else if (*dataptr < mclip) {
                        *dataptr = mclip;
                }
                dataptr++;
        }
}


/* Quantile clip on magnitudes of trace values */
void do_qclip(
	float *data,	/* the data			*/
        float qclip,    /* quantile at which to clip    */
        int nt          /* number of sample points      */
)
{
        register int i;
        static cwp_Bool first = cwp_true;   /* first entry flag             */
        static float *absdata;          /* absolute value trace         */
        static int iq;                  /* index of qclipth quantile    */
        float clip;                     /* ... value of rank[iq]        */

        if (first) {
                absdata = ealloc1float(nt);
                iq = (int) (qclip * nt - 0.5); /* round, don't truncate */
                first = cwp_false;
        }

        /* Clip on value corresponding to qth quantile */
        for (i = 0; i < nt; ++i)  absdata[i] = ABS(data[i]);
        clip = quant(absdata, iq, nt);
        do_clip(data, clip, nt);
}


/* Quantile balance */
void do_qbal(
	float *data,	/* the data			*/
        float qclip,    /* quantile at which to clip    */
        int nt          /* number of sample points      */
)
{
        register int i;
        static cwp_Bool first = cwp_true;   /* first entry flag             */
        static float *absdata;          /* absolute value trace         */
        static int iq;                  /* index of qclipth quantile    */
        float bal;                      /* value used to balance trace  */

        if (qclip == 1.0) { /* balance by max magnitude on trace */
                bal = ABS(data[0]);
                for (i = 1; i < nt; ++i)  bal = MAX(bal, ABS(data[i]));

                if ((bal == 0.0)) {
                        return;
                } else {
                        for (i = 0; i < nt; ++i)  data[i] /= bal;
                        return;
                }
        } else if (first) {
                absdata = ealloc1float(nt);
                iq = (int) (qclip * nt - 0.5); /* round, don't truncate */
                first = cwp_false;
        }

        /* Balance by quantile value (qclip < 1.0) */
        for (i = 0; i < nt; ++i)  absdata[i] = ABS(data[i]);
        bal = quant(absdata, iq, nt);

        if ((bal == 0.0)) {
                return;
        } else {
                for (i = 0; i < nt; ++i)  data[i] /= bal;
                do_clip(data, 1.0, nt);
                return;
        }
}


/* Automatic Gain Control--standard box */
void do_agc(float *data, int iwagc, int nt)
{
        static cwp_Bool first = cwp_true;
        static float *agcdata;
        register int i;
        register float val;
        register float sum;
        register int nwin;
        register float rms;


        /* allocate room for agc'd data */
        if (first) {
                first = cwp_false;
                agcdata = ealloc1float(nt);
        }


        /* compute initial window for first datum */
        sum = 0.0;
        for (i = 0; i < iwagc+1; ++i) {
                val = data[i];
                sum += val*val;
        }
        nwin = 2*iwagc+1;
        rms = sum/nwin;
        agcdata[0] = (rms <= 0.0) ? 0.0 : data[0]/sqrt(rms);

	/* ramping on */
        for (i = 1; i <= iwagc; ++i) {
                val = data[i+iwagc];
                sum += val*val;
                ++nwin;
                rms = sum/nwin;
                agcdata[i] = (rms <= 0.0) ? 0.0 : data[i]/sqrt(rms);
        }

        /* middle range -- full rms window */
        for (i = iwagc + 1; i <= nt-1-iwagc; ++i) {
                val = data[i+iwagc];
                sum += val*val;
                val = data[i-iwagc];
                sum -= val*val; /* rounding could make sum negative! */
                rms = sum/nwin;
                agcdata[i] = (rms <= 0.0) ? 0.0 : data[i]/sqrt(rms);
        }

        /* ramping off */
        for (i = nt - iwagc; i <= nt-1; ++i) {
                val = data[i-iwagc];
                sum -= val*val; /* rounding could make sum negative! */
                --nwin;
                rms = sum/nwin;
                agcdata[i] = (rms <= 0.0) ? 0.0 : data[i]/sqrt(rms);
        }

        /* copy data back into trace */
        memcpy( (void *) data, (const void *) agcdata, nt*FSIZE);

        return;
}


#define EPS     3.8090232       /* exp(-EPS*EPS) = 5e-7, "noise" level  */

/* Automatic Gain Control--gaussian taper */
void do_gagc(float *data, int iwagc, int nt)
{
        static cwp_Bool first=cwp_true; /* first entry flag                 */
        static float *agcdata;  /* agc'd data                           */
        static float *w;        /* Gaussian window weights              */
        static float *d2;       /* square of input data                 */
        static float *s;        /* weighted sum of squares of the data  */
        float u;                /* related to reciprocal of std dev     */
        float usq;              /* u*u                                  */


        if (first) {
                first = cwp_false;

                /* Allocate room for agc'd data */
                agcdata = ealloc1float(nt);

                /* Allocate and compute Gaussian window weights */
                w = ealloc1float(iwagc);  /* recall iwagc is HALF window */
                u = EPS / ((float) iwagc);
                usq = u*u;
                {
                        register int i;
                        float floati;

                        for (i = 1; i < iwagc; ++i) {
                                floati = (float) i;
                                w[i] = exp(-(usq*floati*floati));
                        }
                }

                /* Allocate sum of squares and weighted sum of squares */
                d2 = ealloc1float(nt);
                s  = ealloc1float(nt);
        }


        /* Agc the trace */
        {
                register int i, j, k;
                register float val;
                register float wtmp;
                register float stmp;

                /* Put sum of squares of data in d2 and */
                /* initialize s to d2 to get center point set */
                for (i = 0; i < nt; ++i) {
                        val = data[i];
                        s[i] = d2[i] = val * val;
                }

                /* Compute weighted sum s; use symmetry of Gaussian */
                for (j = 1; j < iwagc; ++j) {
                        wtmp = w[j];
                        for (i = j; i < nt; ++i)  s[i] += wtmp*d2[i-j]; 
                        k = nt - j;
                        for (i = 0; i < k; ++i)   s[i] += wtmp*d2[i+j]; 
                }

                for (i = 0; i < nt; ++i) {
                        stmp = s[i];
                        agcdata[i] = (!stmp) ? 0.0 : data[i]/sqrt(stmp);
                }

                /* Copy data back into trace */
                memcpy( (void *) data, (const void *) agcdata, nt*FSIZE);
        }


        return;
}


/*
 * QUANT - find k/n th quantile of a[]
 *
 * Works by reordering a so a[j] < a[k] if j < k.
 *
 * Parameters:
 *    a         - data
 *    k         - indicates quantile
 *    n         - number of points in data
 *
 * This is Hoare's algorithm worked over by SEP (#10, p100) and Brian.
 */

float quant(float *a, int k, int n)
{
        register int i, j;
        int low, hi;
        register float ak, aa;

        low = 0; hi = n-1;

        while (low < hi) {
                ak = a[k];
                i = low;
                j = hi;
                do {
                        while (a[i] < ak) i++;
                        while (a[j] > ak) j--;
                        if (i <= j) {
                                aa = a[i]; a[i] = a[j]; a[j] = aa;
                                i++;
                                j--;
                        }
                } while (i <= j);

                if (j < k) low = i;

                if (k < i) hi = j;
        }

        return(a[k]);
}

/*
 * GAIN - apply all the various gains
 *
 */
void gain(float *data, float tpow, float epow, float gpow,
	  int agc, int gagc, int qbal, int pbal, int mbal, float scale, float bias,
	  register float trap, register float clip, float qclip, int iwagc,
	  register float tmin, register float dt, int nt, int maxbal )
{
    float f_two  = 2.0;
    float f_one  = 1.0;
    float f_half = 0.5;
    register int i;

	if (bias) {
		for (i = 0; i < nt; ++i)  data[i]+=bias ;
	}
	if (tpow) {
		do_tpow(data, tpow, tmin, dt, nt);
	}
	if (epow) {
		do_epow(data, epow, tmin, dt, nt);
	}
	if (!CLOSETO(gpow, f_one)) {
		register float val;

		if (CLOSETO(gpow, f_half)) {
			for (i = 0; i < nt; ++i) {
				val = data[i];
				data[i] = (val >= 0.0) ?
					sqrt(val) :
				-sqrt(-val);
			}
		} else if (CLOSETO(gpow, f_two)) {
			for (i = 0; i < nt; ++i) {
				val = data[i];
				data[i] = val * ABS(val);
			}
		} else {
			for (i = 0; i < nt; ++i) {
				val = data[i];
				data[i] = (val >= 0.0) ?
					pow(val, gpow) :
				-pow(-val, gpow);
			}
		}
	}
	if (agc)                   do_agc(data, iwagc, nt);
	if (gagc)                  do_gagc(data, iwagc, nt);
	if (trap > 0.0)            do_trap(data, trap, nt);
	if (clip > 0.0)            do_clip(data, clip, nt);
	if (qclip < 1.0 && !qbal)  do_qclip(data, qclip, nt);
	if (qbal)                  do_qbal(data, qclip, nt);
	if (pbal) {
		register int i;
		register float val;
		register float rmsq = 0.0;
		
		/* rmsq = sqrt (SUM( a()*a() ) / nt) */
		for (i = 0; i < nt; ++i) {
			val = data[i];
			rmsq += val * val;
		}
		rmsq = sqrt(rmsq / nt);

		if (rmsq) {
			for (i = 0; i < nt; ++i)
				data[i] /= rmsq;
		}
	}
	if (mbal) {
		register int i;
		register float mean = 0.0;
		
		/* mean = SUM (data[i] / nt) */
		for (i = 0; i < nt; ++i) {
			mean+=data[i];
		}
		/* compute the mean */
		mean/=nt;

		/* subtract the mean from each sample */
		if (mean) {
			for (i = 0; i < nt; ++i)
				data[i]-=mean;
		}
	}

	if (maxbal) {
		register int i;
		register float max = data[0];
		
		/* max */
		for (i = 0; i < nt; ++i) {
			if( data[i] > max ) max = data[i];
		}

		/* subtract max */
		for (i = 0; i < nt; ++i) data[i]-=max;
	}


	if (!CLOSETO(scale, f_one)) {
		register int i;

		for (i = 0; i < nt; ++i)  data[i] *= scale;
	}
}

/* for graceful interrupt termination */
static void closefiles(void)
{
	efclose(headerfp);
	efclose(tracefp);
	eremove(headerfile);
	eremove(tracefile);
	exit(EXIT_FAILURE);
}