psych.c

网络MPEG4IP流媒体开发源代码

/**********************************************************************
MPEG-4 Audio VM



This software module was originally developed by

Fraunhofer Gesellschaft IIS / University of Erlangen (UER

and edited by

in the course of development of the MPEG-2 NBC/MPEG-4 Audio standard
ISO/IEC 13818-7, 14496-1,2 and 3. This software module is an
implementation of a part of one or more MPEG-2 NBC/MPEG-4 Audio tools
as specified by the MPEG-2 NBC/MPEG-4 Audio standard. ISO/IEC gives
users of the MPEG-2 NBC/MPEG-4 Audio standards free license to this
software module or modifications thereof for use in hardware or
software products claiming conformance to the MPEG-2 NBC/ MPEG-4 Audio
standards. Those intending to use this software module in hardware or
software products are advised that this use may infringe existing
patents. The original developer of this software module and his/her
company, the subsequent editors and their companies, and ISO/IEC have
no liability for use of this software module or modifications thereof
in an implementation. Copyright is not released for non MPEG-2
NBC/MPEG-4 Audio conforming products. The original developer retains
full right to use the code for his/her own purpose, assign or donate
the code to a third party and to inhibit third party from using the
code for non MPEG-2 NBC/MPEG-4 Audio conforming products. This
copyright notice must be included in all copies or derivative works.

Copyright (c) 1996.

-----
This software module was modified by

Tadashi Araki (Ricoh Company, ltd.)
Tatsuya Okada (Waseda Univ.)
Itaru Kaneko (Graphics Communication Laboratories)

and edited by

in the course of development of the MPEG-2 NBC/MPEG-4 Audio standard
ISO/IEC 13818-7, 14496-1,2 and 3.

Almost all part of the function EncTf_psycho_acoustic() is made by
T. Araki and T. Okada and its copyright belongs to Ricoh.
The function psy_get_absthr() is made by I. Kaneko
and its copyright belongs to Graphics Communication Laboratories.

Copyright (c) 1997.

**********************************************************************/
/* CREATED BY :  Bernhard Grill -- August-96  */

#include <math.h>
#ifdef _DEBUG
#include <stdio.h>
#endif

#include "psych.h"
#include "coder.h"
#include "fft.h"
#include "util.h"

#define NS_INTERP(x,y,r) (pow((x),(r))*pow((y),1-(r)))
#define SQRT2 1.41421356237309504880

void PsyInit(GlobalPsyInfo *gpsyInfo, PsyInfo *psyInfo, unsigned int numChannels,
			 unsigned int sampleRate, unsigned int sampleRateIdx)
{
	unsigned int channel;
	int i, j, b, bb, high, low, size;
	double tmpx,tmpy,tmp,x;
	double bval[MAX_NPART], SNR;

	gpsyInfo->ath = (double*)AllocMemory(NPART_LONG*sizeof(double));
	gpsyInfo->athS = (double*)AllocMemory(MAX_SCFAC_BANDS*sizeof(double));
	gpsyInfo->mld = (double*)AllocMemory(NPART_LONG*sizeof(double));
	gpsyInfo->mldS = (double*)AllocMemory(MAX_SCFAC_BANDS*sizeof(double));
	gpsyInfo->window = (double*)AllocMemory(2*BLOCK_LEN_LONG*sizeof(double));
	gpsyInfo->windowS = (double*)AllocMemory(2*BLOCK_LEN_SHORT*sizeof(double));

	for(i = 0; i < BLOCK_LEN_LONG*2; i++)
		gpsyInfo->window[i] = 0.42-0.5*cos(2*M_PI*(i+.5)/(BLOCK_LEN_LONG*2))+
			0.08*cos(4*M_PI*(i+.5)/(BLOCK_LEN_LONG*2));
	for(i = 0; i < BLOCK_LEN_SHORT*2; i++)
		gpsyInfo->windowS[i] = 0.5 * (1-cos(2.0*M_PI*(i+0.5)/(BLOCK_LEN_SHORT*2)));
	gpsyInfo->sampleRate = (double)sampleRate;

	size = BLOCK_LEN_LONG;
	for (channel = 0; channel < numChannels; channel++) {
		psyInfo[channel].size = size;

		psyInfo[channel].lastPe = 0.0;
		psyInfo[channel].lastEnr = 0.0;
		psyInfo[channel].threeInARow = 0;
		psyInfo[channel].tonality = (double*)AllocMemory(NPART_LONG*sizeof(double));
		psyInfo[channel].nb = (double*)AllocMemory(NPART_LONG*sizeof(double));
		psyInfo[channel].maskThr = (double*)AllocMemory(MAX_SCFAC_BANDS*sizeof(double));
		psyInfo[channel].maskEn = (double*)AllocMemory(MAX_SCFAC_BANDS*sizeof(double));
		psyInfo[channel].maskThrNext = (double*)AllocMemory(MAX_SCFAC_BANDS*sizeof(double));
		psyInfo[channel].maskEnNext = (double*)AllocMemory(MAX_SCFAC_BANDS*sizeof(double));
		psyInfo[channel].maskThrMS = (double*)AllocMemory(MAX_SCFAC_BANDS*sizeof(double));
		psyInfo[channel].maskEnMS = (double*)AllocMemory(MAX_SCFAC_BANDS*sizeof(double));
		psyInfo[channel].maskThrNextMS = (double*)AllocMemory(MAX_SCFAC_BANDS*sizeof(double));
		psyInfo[channel].maskEnNextMS = (double*)AllocMemory(MAX_SCFAC_BANDS*sizeof(double));
		psyInfo[channel].prevSamples = (double*)AllocMemory(size*sizeof(double));
		SetMemory(psyInfo[channel].prevSamples, 0, size*sizeof(double));

		psyInfo[channel].lastNb = (double*)AllocMemory(NPART_LONG*sizeof(double));
		psyInfo[channel].lastNbMS = (double*)AllocMemory(NPART_LONG*sizeof(double));
		for (j = 0; j < NPART_LONG; j++) {
			psyInfo[channel].lastNb[j] = 2.;
			psyInfo[channel].lastNbMS[j] = 2.;
		}

		psyInfo[channel].energy = (double*)AllocMemory(size*sizeof(double));
		psyInfo[channel].energyMS = (double*)AllocMemory(size*sizeof(double));
		psyInfo[channel].transBuff = (double*)AllocMemory(2*size*sizeof(double));
	}

	gpsyInfo->psyPart = &psyPartTableLong[sampleRateIdx];
	gpsyInfo->psyPartS = &psyPartTableShort[sampleRateIdx];

	size = BLOCK_LEN_SHORT;
	for (channel = 0; channel < numChannels; channel++) {
		psyInfo[channel].sizeS = size;

		psyInfo[channel].prevSamplesS = (double*)AllocMemory(size*sizeof(double));
		SetMemory(psyInfo[channel].prevSamplesS, 0, size*sizeof(double));

		for (j = 0; j < 8; j++) {
			psyInfo[channel].nbS[j] = (double*)AllocMemory(NPART_SHORT*sizeof(double));
			psyInfo[channel].maskThrS[j] = (double*)AllocMemory(MAX_SCFAC_BANDS*sizeof(double));
			psyInfo[channel].maskEnS[j] = (double*)AllocMemory(MAX_SCFAC_BANDS*sizeof(double));
			psyInfo[channel].maskThrNextS[j] = (double*)AllocMemory(MAX_SCFAC_BANDS*sizeof(double));
			psyInfo[channel].maskEnNextS[j] = (double*)AllocMemory(MAX_SCFAC_BANDS*sizeof(double));
			psyInfo[channel].maskThrSMS[j] = (double*)AllocMemory(MAX_SCFAC_BANDS*sizeof(double));
			psyInfo[channel].maskEnSMS[j] = (double*)AllocMemory(MAX_SCFAC_BANDS*sizeof(double));
			psyInfo[channel].maskThrNextSMS[j] = (double*)AllocMemory(MAX_SCFAC_BANDS*sizeof(double));
			psyInfo[channel].maskEnNextSMS[j] = (double*)AllocMemory(MAX_SCFAC_BANDS*sizeof(double));

			psyInfo[channel].energyS[j] = (double*)AllocMemory(size*sizeof(double));
			psyInfo[channel].energySMS[j] = (double*)AllocMemory(size*sizeof(double));
			psyInfo[channel].transBuffS[j] = (double*)AllocMemory(2*size*sizeof(double));
		}
	}

	size = BLOCK_LEN_LONG;
	high = 0;
	for(b = 0; b < gpsyInfo->psyPart->len; b++) {
		low = high;
		high += gpsyInfo->psyPart->width[b];

		bval[b] = 0.5 * (freq2bark(gpsyInfo->sampleRate*low/(2*size)) + 
			freq2bark(gpsyInfo->sampleRate*(high-1)/(2*size)));
	}

	for(b = 0; b < gpsyInfo->psyPart->len; b++) {
		for(bb = 0; bb < gpsyInfo->psyPart->len; bb++) {
			if (bval[b] >= bval[bb]) tmpx = (bval[b] - bval[bb])*3.0;
			else tmpx = (bval[b] - bval[bb])*1.5;

			if(tmpx >= 0.5 && tmpx <= 2.5)
			{
				tmp = tmpx - 0.5;
				x = 8.0 * (tmp*tmp - 2.0 * tmp);
			} else
				x = 0.0;

			tmpx += 0.474;
			tmpy = 15.811389 + 7.5*tmpx - 17.5*sqrt(1.0+tmpx*tmpx);

			if (tmpy < -100.0) gpsyInfo->spreading[b][bb] = 0.0;
			else gpsyInfo->spreading[b][bb] = exp((x + tmpy)*0.2302585093);
		}
	}
	for(b = 0; b < gpsyInfo->psyPart->len; b++) {
		for(bb = 0; bb < gpsyInfo->psyPart->len; bb++) {
			if (gpsyInfo->spreading[b][bb] != 0.0)
				break;
		}
		gpsyInfo->sprInd[b][0] = bb;
		for(bb = gpsyInfo->psyPart->len-1; bb > 0; bb--) {
			if (gpsyInfo->spreading[b][bb] != 0.0)
				break;
		}
		gpsyInfo->sprInd[b][1] = bb;
	}

    for( b = 0; b < gpsyInfo->psyPart->len; b++){
		tmp = 0.0;
		for( bb = gpsyInfo->sprInd[b][0]; bb < gpsyInfo->sprInd[b][1]; bb++)
			tmp += gpsyInfo->spreading[b][bb];
		for( bb = gpsyInfo->sprInd[b][0]; bb < gpsyInfo->sprInd[b][1]; bb++)
			gpsyInfo->spreading[b][bb] /= tmp;
    }

	j = 0;
    for( b = 0; b < gpsyInfo->psyPart->len; b++){
		gpsyInfo->ath[b] = 1.e37;

		for (bb = 0; bb < gpsyInfo->psyPart->width[b]; bb++, j++) {
			double freq = gpsyInfo->sampleRate*j/(1000.0*2*size);
			double level;
			level = ATHformula(freq*1000.0) - 20.0;
			level = pow(10., 0.1*level);
			level *= gpsyInfo->psyPart->width[b];
			if (level < gpsyInfo->ath[b])
				gpsyInfo->ath[b] = level;
		}
    }

	low = 0;
	for (b = 0; b < gpsyInfo->psyPart->len; b++) {
		tmp = freq2bark(gpsyInfo->sampleRate*low/(2*size));
		tmp = (min(tmp, 15.5)/15.5);

		gpsyInfo->mld[b] = pow(10.0, 1.25*(1-cos(M_PI*tmp))-2.5);
		low += gpsyInfo->psyPart->width[b];
	}


	size = BLOCK_LEN_SHORT;
	high = 0;
	for(b = 0; b < gpsyInfo->psyPartS->len; b++) {
		low = high;
		high += gpsyInfo->psyPartS->width[b];

		bval[b] = 0.5 * (freq2bark(gpsyInfo->sampleRate*low/(2*size)) + 
			freq2bark(gpsyInfo->sampleRate*(high-1)/(2*size)));
	}

	for(b = 0; b < gpsyInfo->psyPartS->len; b++) {
		for(bb = 0; bb < gpsyInfo->psyPartS->len; bb++) {
			if (bval[b] >= bval[bb]) tmpx = (bval[b] - bval[bb])*3.0;
			else tmpx = (bval[b] - bval[bb])*1.5;

			if(tmpx >= 0.5 && tmpx <= 2.5)
			{
				tmp = tmpx - 0.5;
				x = 8.0 * (tmp*tmp - 2.0 * tmp);
			} else
				x = 0.0;

			tmpx += 0.474;
			tmpy = 15.811389 + 7.5*tmpx - 17.5*sqrt(1.0+tmpx*tmpx);

			if (tmpy < -100.0) gpsyInfo->spreadingS[b][bb] = 0.0;
			else gpsyInfo->spreadingS[b][bb] = exp((x + tmpy)*0.2302585093);
		}
	}
	for(b = 0; b < gpsyInfo->psyPartS->len; b++) {
		for(bb = 0; bb < gpsyInfo->psyPartS->len; bb++) {
			if (gpsyInfo->spreadingS[b][bb] != 0.0)
				break;
		}
		gpsyInfo->sprIndS[b][0] = bb;
		for(bb = gpsyInfo->psyPartS->len-1; bb > 0; bb--) {
			if (gpsyInfo->spreadingS[b][bb] != 0.0)
				break;
		}
		gpsyInfo->sprIndS[b][1] = bb;
	}

	j = 0;
    for( b = 0; b < gpsyInfo->psyPartS->len; b++){
		gpsyInfo->athS[b] = 1.e37;

		for (bb = 0; bb < gpsyInfo->psyPartS->width[b]; bb++, j++) {
			double freq = gpsyInfo->sampleRate*j/(1000.0*2*size);
			double level;
			level = ATHformula(freq*1000.0) - 20.0;
			level = pow(10., 0.1*level);
			level *= gpsyInfo->psyPartS->width[b];
			if (level < gpsyInfo->athS[b])
				gpsyInfo->athS[b] = level;
		}
    }

    for( b = 0; b < gpsyInfo->psyPartS->len; b++){
		tmp = 0.0;
		for( bb = gpsyInfo->sprIndS[b][0]; bb < gpsyInfo->sprIndS[b][1]; bb++)
			tmp += gpsyInfo->spreadingS[b][bb];

		/* SNR formula */
		if (bval[b] < 13) SNR = -8.25;
		else SNR = -4.5 * (bval[b]-13)/(24.0-13.0) +
			-8.25*(bval[b]-24)/(13.0-24.0);
		SNR = pow(10.0, SNR/10.0);

		for( bb = gpsyInfo->sprIndS[b][0]; bb < gpsyInfo->sprIndS[b][1]; bb++)
			gpsyInfo->spreadingS[b][bb] *= SNR / tmp;
    }

	low = 0;
	for (b = 0; b < gpsyInfo->psyPartS->len; b++) {
		tmp = freq2bark(gpsyInfo->sampleRate*low/(2*size));
		tmp = (min(tmp, 15.5)/15.5);

		gpsyInfo->mldS[b] = pow(10.0, 1.25*(1-cos(M_PI*tmp))-2.5);
		low += gpsyInfo->psyPartS->width[b];
	}
}

void PsyEnd(GlobalPsyInfo *gpsyInfo, PsyInfo *psyInfo, unsigned int numChannels)
{
	unsigned int channel;
	int j;

	if (gpsyInfo->ath) FreeMemory(gpsyInfo->ath);
	if (gpsyInfo->athS) FreeMemory(gpsyInfo->athS);
	if (gpsyInfo->mld) FreeMemory(gpsyInfo->mld);
	if (gpsyInfo->mldS) FreeMemory(gpsyInfo->mldS);
	if (gpsyInfo->window) FreeMemory(gpsyInfo->window);
	if (gpsyInfo->windowS) FreeMemory(gpsyInfo->windowS);

	for (channel = 0; channel < numChannels; channel++) {
		if (psyInfo[channel].nb) FreeMemory(psyInfo[channel].nb);
		if (psyInfo[channel].tonality) FreeMemory(psyInfo[channel].tonality);
		if (psyInfo[channel].prevSamples) FreeMemory(psyInfo[channel].prevSamples);
		if (psyInfo[channel].maskThr) FreeMemory(psyInfo[channel].maskThr);
		if (psyInfo[channel].maskEn) FreeMemory(psyInfo[channel].maskEn);
		if (psyInfo[channel].maskThrNext) FreeMemory(psyInfo[channel].maskThrNext);
		if (psyInfo[channel].maskEnNext) FreeMemory(psyInfo[channel].maskEnNext);
		if (psyInfo[channel].maskThrMS) FreeMemory(psyInfo[channel].maskThrMS);
		if (psyInfo[channel].maskEnMS) FreeMemory(psyInfo[channel].maskEnMS);
		if (psyInfo[channel].maskThrNextMS) FreeMemory(psyInfo[channel].maskThrNextMS);
		if (psyInfo[channel].maskEnNextMS) FreeMemory(psyInfo[channel].maskEnNextMS);
		
		if (psyInfo[channel].lastNb) FreeMemory(psyInfo[channel].lastNb);
		if (psyInfo[channel].lastNbMS) FreeMemory(psyInfo[channel].lastNbMS);

		if (psyInfo[channel].energy) FreeMemory(psyInfo[channel].energy);
		if (psyInfo[channel].energyMS) FreeMemory(psyInfo[channel].energyMS);
		if (psyInfo[channel].transBuff) FreeMemory(psyInfo[channel].transBuff);
	}

	for (channel = 0; channel < numChannels; channel++) {
		if(psyInfo[channel].prevSamplesS) FreeMemory(psyInfo[channel].prevSamplesS);
		for (j = 0; j < 8; j++) {
			if (psyInfo[channel].nbS[j]) FreeMemory(psyInfo[channel].nbS[j]);
			if (psyInfo[channel].maskThrS[j]) FreeMemory(psyInfo[channel].maskThrS[j]);
			if (psyInfo[channel].maskEnS[j]) FreeMemory(psyInfo[channel].maskEnS[j]);
			if (psyInfo[channel].maskThrNextS[j]) FreeMemory(psyInfo[channel].maskThrNextS[j]);
			if (psyInfo[channel].maskEnNextS[j]) FreeMemory(psyInfo[channel].maskEnNextS[j]);
			if (psyInfo[channel].maskThrSMS[j]) FreeMemory(psyInfo[channel].maskThrSMS[j]);
			if (psyInfo[channel].maskEnSMS[j]) FreeMemory(psyInfo[channel].maskEnSMS[j]);
			if (psyInfo[channel].maskThrNextSMS[j]) FreeMemory(psyInfo[channel].maskThrNextSMS[j]);
			if (psyInfo[channel].maskEnNextSMS[j]) FreeMemory(psyInfo[channel].maskEnNextSMS[j]);

			if (psyInfo[channel].energyS[j]) FreeMemory(psyInfo[channel].energyS[j]);
			if (psyInfo[channel].energySMS[j]) FreeMemory(psyInfo[channel].energySMS[j]);
			if (psyInfo[channel].transBuffS[j]) FreeMemory(psyInfo[channel].transBuffS[j]);
		}
	}
}

/* Do psychoacoustical analysis */
void PsyCalculate(ChannelInfo *channelInfo, GlobalPsyInfo *gpsyInfo, PsyInfo *psyInfo,
				  int *cb_width_long, int num_cb_long, int *cb_width_short,
				  int num_cb_short, unsigned int numChannels)
{
	unsigned int channel;

	for (channel = 0; channel < numChannels; channel++) {
		if (channelInfo[channel].present) {

			if (channelInfo[channel].cpe &&
				channelInfo[channel].ch_is_left) { /* CPE */

				int leftChan = channel;
				int rightChan = channelInfo[channel].paired_ch;

				PsyBufferUpdateMS(gpsyInfo, &psyInfo[leftChan], &psyInfo[rightChan]);

				/* Calculate the threshold */
				PsyThreshold(gpsyInfo, &psyInfo[leftChan], cb_width_long, num_cb_long,
					cb_width_short, num_cb_short);
				PsyThreshold(gpsyInfo, &psyInfo[rightChan], cb_width_long, num_cb_long,
					cb_width_short, num_cb_short);

				/* And for MS */
				PsyThresholdMS(&channelInfo[leftChan], gpsyInfo, &psyInfo[leftChan],
					&psyInfo[rightChan], cb_width_long, num_cb_long, cb_width_short,
					num_cb_short);

			} else if (!channelInfo[channel].cpe &&
				channelInfo[channel].lfe) { /* LFE */

				/* NOT FINISHED */

			} else if (!channelInfo[channel].cpe) { /* SCE */

				/* Calculate the threshold */
				PsyThreshold(gpsyInfo, &psyInfo[channel], cb_width_long, num_cb_long,
					cb_width_short, num_cb_short);
			}
		}
	}
}

static void Hann(GlobalPsyInfo *gpsyInfo, double *inSamples, int size)
{
	int i;

	/* Applying Hann window */
	if (size == BLOCK_LEN_LONG*2) {
		for(i = 0; i < size; i++)
			inSamples[i] *= gpsyInfo->window[i];
	} else {
		for(i = 0; i < size; i++)
			inSamples[i] *= gpsyInfo->windowS[i];
	}
}

void PsyBufferUpdate(GlobalPsyInfo *gpsyInfo, PsyInfo *psyInfo, double *newSamples)
{
	int i, j;
	double a, b;
	double temp[2048];

	memcpy(psyInfo->transBuff, psyInfo->prevSamples, psyInfo->size*sizeof(double));
	memcpy(psyInfo->transBuff + psyInfo->size, newSamples, psyInfo->size*sizeof(double));


	Hann(gpsyInfo, psyInfo->transBuff, 2*psyInfo->size);
	rsfft(psyInfo->transBuff, 11);

	/* Calculate magnitude of new data */
	for (i = 0; i < psyInfo->size; i++) {
		a = psyInfo->transBuff[i];
		b = psyInfo->transBuff[i+psyInfo->size];
		psyInfo->energy[i] = 0.5 * (a*a + b*b);
	}

	memcpy(temp, psyInfo->prevSamples, psyInfo->size*sizeof(double));
	memcpy(temp + psyInfo->size, newSamples, psyInfo->size*sizeof(double));

	for (j = 0; j < 8; j++) {

		memcpy(psyInfo->transBuffS[j], temp+(j*128)+(1024-128), 2*psyInfo->sizeS*sizeof(double));

		Hann(gpsyInfo, psyInfo->transBuffS[j], 2*psyInfo->sizeS);
		rsfft(psyInfo->transBuffS[j], 8);

		/* Calculate magnitude of new data */
		for(i = 0; i < psyInfo->sizeS; i++){
			a = psyInfo->transBuffS[j][i];
			b = psyInfo->transBuffS[j][i+psyInfo->sizeS];
			psyInfo->energyS[j][i] = 0.5 * (a*a + b*b);
		}
	}

	memcpy(psyInfo->prevSamples, newSamples, psyInfo->size*sizeof(double));
}

void PsyBufferUpdateMS(GlobalPsyInfo *gpsyInfo, PsyInfo *psyInfoL, PsyInfo *psyInfoR)
{
	int i, j;
	double a, b;