view sv/filter/DSP.cpp @ 79:afcf540ae3a2

add the real time filter stack to manage real time filters and their attributes

//#include  "stdafx.h"
#include  "DSP.h"
#include  "math.h"
#include	<cassert>
#include	<cmath>
extern int currentposition;
extern float lastfactor;
extern float hopfactor;


//These variables are only required for find peaks...the same variables are passed in to other functions
extern int numpeaks;
extern float *peak_locations; 
extern float *p_mags; 
extern float *c_mags;

void intobyte(int num, char* pbyte1 ,char* pbyte2)
{

		
		char firstByte = (num & 0xff);
		int secondByteInt = (num & 0xff00);
		secondByteInt = secondByteInt>>8;
		char secondByte = secondByteInt;

		/*int thirdByteInt = (array[i] & 0xff0000);
		thirdByteInt = thirdByteInt>>16;
		char thirdByte = thirdByteInt;

		int fourthByteInt = (array[i] & 0xff000000);
		fourthByteInt = fourthByteInt>>24;
		char fourthByte = fourthByteInt;*/


		*pbyte1 = firstByte;
		*pbyte2 = secondByte;
}


void cart2pol(float* cart, float* mags, float* phases, int framesize)
{
	int length=framesize/2;
	for (int f = 0; f<length; f++)
	{
	
	mags[f]=sqrt((cart[f]*cart[f])+(cart[length+f]*cart[length+f]));
	

	phases[f]=atan2((double)(-1*cart[f+length]),(double)(cart[f]));
	
	

	}


}

void pol2cart(float* cart, float* mags, float* phases, int framesize)
{
	int length=framesize/2;
	for (int f = 0; f<length; f++)
	{
		
		cart[f]=mags[f]*cos(phases[f]);
		cart[length+f]=-(mags[f]*sin(phases[f]));
	
	
	}


}

void hanning(float* window, int framesize)
{
	
	for (int f = 0; f<framesize; f++)
	{
		
		window[f]= (0.5*(1-cos(2*PI*(f+1)/(framesize+1))));
	
	}


}

void updatephases(float* c_phase,float* p_phase,float* c_synthphase,float* p_synthphase, int framesize,float hopfactor,float interpfactor)
{

	float synth_hopsize=framesize/4;
	float actual_anhop=floor(hopfactor*synth_hopsize);
	float anhop = floor((hopfactor*synth_hopsize)/interpfactor);
	float inst_freq;
	float omega_k;
	float delta_phi;
	float a, k;

	for (int f = 0; f<(framesize/2); f++)
	{
		
		//float bin = (f/(float)framesize);

		interpfactor;

		omega_k=(2*PI*f)/float(framesize);			//This is the predicted angular freqency rads/sec of the centre frequency
		
		delta_phi=(c_phase[f]-p_phase[f])-(anhop*omega_k);

		a = delta_phi/(2*PI);
		
		k = floor(a+0.5);
		
		delta_phi = delta_phi-k*2*PI;
		
		inst_freq=omega_k+delta_phi/anhop; 
	
		
		
		c_synthphase[f] = p_synthphase[f]+(inst_freq*synth_hopsize); 

		if (currentposition<actual_anhop){c_synthphase[f] = c_phase[f];}
		
		p_synthphase[f] = c_synthphase[f];

		p_phase[f] = c_phase[f];

		
		
		

	
	}
}

void updatephases2(float* c_phase,float* p_phase,float* c_synthphase,float* p_synthphase, int framesize,float hopfactor,float interpfactor)
{

	float synth_hopsize=framesize/4;
	float actual_anhop=actual_anhop=floor(hopfactor*synth_hopsize);
	float anhop = floor((hopfactor*synth_hopsize)/interpfactor);
	float inst_freq;
	float omega_k;
	float delta_phi;
	float a, k;

	for (int f = 0; f<(framesize/2); f++)
	{
		
		//float bin = (f/(float)framesize);

		interpfactor;

		omega_k=(2*PI*f)/float(framesize);			//This is the predicted angular freqency rads/sec of the centre frequency
		
		delta_phi=(c_phase[f]-p_phase[f])-(anhop*omega_k);

		a = delta_phi/(2*PI);
		
		k = floor(a+0.5);
		
		delta_phi = delta_phi-k*2*PI;
		
		inst_freq=omega_k+delta_phi/anhop; 
	
		
		
		c_synthphase[f] = p_synthphase[f]+(inst_freq*synth_hopsize); 

		

		if (currentposition<actual_anhop){c_synthphase[f] = c_phase[f];}
		
		if (!(anhop==lastfactor)){c_synthphase[f]=c_phase[f];}
		
		
		
		p_synthphase[f] = c_synthphase[f];
		

		p_phase[f] = c_phase[f];

		

		

	
	}
	lastfactor=anhop;
}

void rotatephases(float* c_phase, float* p_phase, float* c_synthphase, float* p_synthphase, int framesize, float interpfactor)
{
	float phase_diff;
	int diffhop = framesize/4;

	float anhop = floor((diffhop)/interpfactor);
	float inst_freq;
	float omega_k;
	float delta_phi;
	float a, k;
	
	//findpeaks(c_mags, p_mags, framesize, peak_locations, numpeaks);
	
	for (int i=0; i<framesize/2; i++)
	{
		
		phase_diff=(c_phase[i]-p_phase[i]);
		
		omega_k=(2*PI*i)/float(framesize);

		delta_phi=(phase_diff)-(anhop*omega_k);

		a = delta_phi/(2*PI);
		
		k = floor(a+0.5);
		
		delta_phi = delta_phi-k*2*PI;
		
		inst_freq=omega_k+delta_phi/anhop; 
					
		c_synthphase[i]=p_synthphase[i]+(inst_freq*diffhop);

		//c_synthphase[i]=p_synthphase[i]+(phase_diff);

		//c_synthphase[i]=p_synthphase[i]+((phase_diff*diffhop)*interpfactor);
		
		//if (currentposition<framesize/2 || (!(lastfactor==hopfactor))){c_synthphase[i] = c_phase[i];}
		if (currentposition<framesize/2){c_synthphase[i] = c_phase[i];}
		
		p_synthphase[i]=c_synthphase[i];
	}

	lastfactor=hopfactor;
}

void rotatephases_peaklocked(float* c_phase, float* p_phase, float* c_synthphase, float* p_synthphase, int framesize, float interpfactor)
{
	float phase_diff;
	int diffhop = framesize/4;

	float anhop = floor((diffhop)/interpfactor);
	float inst_freq;
	float omega_k;
	float delta_phi;
	float a, k;
	int start, end, i;
	float original_peak_phase;
	float cutoff;

	cutoff = 10000;

	cutoff = floor(cutoff/(44100/framesize));

	numpeaks = findpeaks(c_mags, p_mags, framesize, peak_locations);
	
	int p=1;
	//while(peak_locations[p]<cutoff)
		while(p<=numpeaks)
	{
		
		i=peak_locations[p];

		original_peak_phase=c_phase[i];
		
		//start=i-2;
		//end=i+2;
		
		
		if (p==1){
			start=0;
			end=floor(peak_locations[p]+((peak_locations[p+1]-peak_locations[p])/2));
		}

		else{
		if (p==numpeaks){
			start=floor(peak_locations[p-1]+((peak_locations[p]-peak_locations[p-1])/2));
			end=(framesize/2);
		}
		else {
			start=floor(peak_locations[p-1]+((peak_locations[p]-peak_locations[p-1])/2));
			end=floor(peak_locations[p]+((peak_locations[p+1]-peak_locations[p])/2));
		}
		}
			
		phase_diff=(c_phase[i]-p_phase[i]);
		omega_k=(2*PI*i)/float(framesize);
		delta_phi=(phase_diff)-(anhop*omega_k);
		a = delta_phi/(2*PI);
		k = floor(a+0.5);
		delta_phi = delta_phi-k*2*PI;
		inst_freq=omega_k+delta_phi/anhop; 
		c_synthphase[i]=p_synthphase[i]+(inst_freq*diffhop);
		//c_synthphase[i]=p_synthphase[i]+(phase_diff);
		//c_synthphase[i]=p_synthphase[i]+((phase_diff*diffhop)*interpfactor);
		//if (currentposition<framesize/2 || (!(lastfactor==hopfactor))){c_synthphase[i] = c_phase[i];}
		if (currentposition<framesize/2){c_synthphase[i] = c_phase[i];}
		p_synthphase[i]=c_synthphase[i];

		int j;
		for (j=start; j<i; j++)
		{
			c_synthphase[j]=c_synthphase[i]-(original_peak_phase-c_phase[j]);
			p_synthphase[j]=c_synthphase[j];
		}

		for (j=i+1; j<end; j++)
		{
			c_synthphase[j]=c_synthphase[i]-(original_peak_phase-c_phase[j]);
			p_synthphase[j]=c_synthphase[j];
		}


		 p++;
	}

	/*for (i=peak_locations[p]+1; i<(framesize/2); i++)
	{
		phase_diff=(c_phase[i]-p_phase[i]);
		
		omega_k=(2*PI*i)/float(framesize);

		delta_phi=(phase_diff)-(anhop*omega_k);

		a = delta_phi/(2*PI);
		
		k = floor(a+0.5);
		
		delta_phi = delta_phi-k*2*PI;
		
		inst_freq=omega_k+delta_phi/anhop; 
					
		c_synthphase[i]=p_synthphase[i]+(inst_freq*diffhop);

		p_synthphase[i]=c_synthphase[i];
	}  */

	lastfactor=hopfactor;
}



bool transient_detect(float* L_mags, float* R_mags, float* pL_mags, float* pR_mags, float drumthresh, float framesize)
{
	
	extern float *L_phase, *R_phase;
	int trans = 0;
	
	for (int f = 0; f<(framesize/2); f++)
	{
		
			if	((R_mags[f]-pR_mags[f])>0 &&
				(L_mags[f]-pL_mags[f])>0){trans++;}
				
				
				pR_mags[f]=R_mags[f];
				pL_mags[f]=L_mags[f];
				

	}
	drumthresh=(drumthresh/100)*(framesize/2);
	

	if (trans>=drumthresh)
	{

	return true;
	}
	else{return false;}
}

void sintable(float* sinewave, float length, float sr)
{
	double pi=3.14179;
	double time;
	double amp, freq;
	
	for (int i = 0; i<sr; i++)

	{
		time=double(i*(1/sr));
		sinewave[i]=(amp)*(cos(2*pi*freq*(time)));
	}
}

void cur2last(float* c_phase, float* c_synthphase, float* p_synthphase, int framesize)
{

for (int i=0; i<framesize/2; i++)
	{
		
		c_synthphase[i]=c_phase[i];
						
		p_synthphase[i]=c_synthphase[i];
	}


}


int findpeaks(float* c_mags, float* p_mags, float framesize, float* peak_locations)

{
	numpeaks=0;
	peak_locations[0]=0;
	float peakthreshold=0;

	for (int i=2; i<(framesize/2)-2; i++)
	{
		
		if(c_mags[i]>c_mags[i-1] && c_mags[i]>c_mags[i+1] && c_mags[i]>c_mags[i-2] && c_mags[i]>c_mags[i+2] && c_mags[i]>peakthreshold)
		{
			numpeaks++;
			peak_locations[numpeaks]=i;
			
		}
		
		
	}
	return numpeaks;
}