diff options
Diffstat (limited to 'plugins/supereq/Equ.cpp')
| -rw-r--r-- | plugins/supereq/Equ.cpp | 647 |
1 files changed, 647 insertions, 0 deletions
diff --git a/plugins/supereq/Equ.cpp b/plugins/supereq/Equ.cpp new file mode 100644 index 00000000..42fdcc13 --- /dev/null +++ b/plugins/supereq/Equ.cpp @@ -0,0 +1,647 @@ +#include <stdio.h>
+#include <stdlib.h>
+#include <math.h>
+#include <assert.h>
+#include "paramlist.hpp"
+
+typedef float REAL;
+void rfft(int n,int isign,REAL x[]);
+
+#define M 15
+
+#define PI 3.1415926535897932384626433832795
+
+#define RINT(x) ((x) >= 0 ? ((int)((x) + 0.5)) : ((int)((x) - 0.5)))
+
+#define DITHERLEN 65536
+
+// play -c 2 -r 44100 -fs -sw
+
+static REAL fact[M+1];
+static REAL aa = 96;
+static REAL iza;
+static REAL *lires,*lires1,*lires2,*rires,*rires1,*rires2,*irest;
+static REAL *fsamples;
+static REAL *ditherbuf;
+static int ditherptr = 0;
+static volatile int chg_ires,cur_ires;
+static int winlen,winlenbit,tabsize,nbufsamples;
+static short *inbuf;
+static REAL *outbuf;
+static int maxamp;
+int enable = 1, dither = 0;
+
+#define NCH 2
+
+#define NBANDS 17
+static REAL bands[] = {
+ 65.406392,92.498606,130.81278,184.99721,261.62557,369.99442,523.25113,
+ 739.9884 ,1046.5023,1479.9768,2093.0045,2959.9536,4186.0091,5919.9072,
+ 8372.0181,11839.814,16744.036
+};
+
+static REAL alpha(REAL a)
+{
+ if (a <= 21) return 0;
+ if (a <= 50) return 0.5842*pow(a-21,0.4)+0.07886*(a-21);
+ return 0.1102*(a-8.7);
+}
+
+static REAL izero(REAL x)
+{
+ REAL ret = 1;
+ int m;
+
+ for(m=1;m<=M;m++)
+ {
+ REAL t;
+ t = pow(x/2,m)/fact[m];
+ ret += t*t;
+ }
+
+ return ret;
+}
+
+extern "C" void equ_init(int wb)
+{
+ int i,j;
+
+ if (lires1 != NULL) free(lires1);
+ if (lires2 != NULL) free(lires2);
+ if (rires1 != NULL) free(rires1);
+ if (rires2 != NULL) free(rires2);
+ if (irest != NULL) free(irest);
+ if (fsamples != NULL) free(fsamples);
+ if (inbuf != NULL) free(inbuf);
+ if (outbuf != NULL) free(outbuf);
+ if (ditherbuf != NULL) free(ditherbuf);
+
+ winlen = (1 << (wb-1))-1;
+ winlenbit = wb;
+ tabsize = 1 << wb;
+
+ lires1 = (REAL *)malloc(sizeof(REAL)*tabsize);
+ lires2 = (REAL *)malloc(sizeof(REAL)*tabsize);
+ rires1 = (REAL *)malloc(sizeof(REAL)*tabsize);
+ rires2 = (REAL *)malloc(sizeof(REAL)*tabsize);
+ irest = (REAL *)malloc(sizeof(REAL)*tabsize);
+ fsamples = (REAL *)malloc(sizeof(REAL)*tabsize);
+ inbuf = (short *)calloc(winlen*NCH,sizeof(int));
+ outbuf = (REAL *)calloc(tabsize*NCH,sizeof(REAL));
+ ditherbuf = (REAL *)malloc(sizeof(REAL)*DITHERLEN);
+
+ lires = lires1;
+ rires = rires1;
+ cur_ires = 1;
+ chg_ires = 1;
+
+ for(i=0;i<DITHERLEN;i++)
+ ditherbuf[i] = (float(rand())/RAND_MAX-0.5);
+
+ for(i=0;i<=M;i++)
+ {
+ fact[i] = 1;
+ for(j=1;j<=i;j++) fact[i] *= j;
+ }
+
+ iza = izero(alpha(aa));
+}
+
+// -(N-1)/2 <= n <= (N-1)/2
+static REAL win(REAL n,int N)
+{
+ return izero(alpha(aa)*sqrt(1-4*n*n/((N-1)*(N-1))))/iza;
+}
+
+static REAL sinc(REAL x)
+{
+ return x == 0 ? 1 : sin(x)/x;
+}
+
+static REAL hn_lpf(int n,REAL f,REAL fs)
+{
+ REAL t = 1/fs;
+ REAL omega = 2*PI*f;
+ return 2*f*t*sinc(n*omega*t);
+}
+
+static REAL hn_imp(int n)
+{
+ return n == 0 ? 1.0 : 0.0;
+}
+
+static REAL hn(int n,paramlist ¶m2,REAL fs)
+{
+ paramlistelm *e;
+ REAL ret,lhn;
+
+ lhn = hn_lpf(n,param2.elm->upper,fs);
+ ret = param2.elm->gain*lhn;
+
+ for(e=param2.elm->next;e->next != NULL && e->upper < fs/2;e = e->next)
+ {
+ REAL lhn2 = hn_lpf(n,e->upper,fs);
+ ret += e->gain*(lhn2-lhn);
+ lhn = lhn2;
+ }
+
+ ret += e->gain*(hn_imp(n)-lhn);
+
+ return ret;
+}
+
+void process_param(REAL *bc,paramlist *param,paramlist ¶m2,REAL fs,int ch)
+{
+ paramlistelm **pp,*p,*e,*e2;
+ int i;
+
+ delete param2.elm;
+ param2.elm = NULL;
+
+ for(i=0,pp=¶m2.elm;i<=NBANDS;i++,pp = &(*pp)->next)
+ {
+ (*pp) = new paramlistelm;
+ (*pp)->lower = i == 0 ? 0 : bands[i-1];
+ (*pp)->upper = i == NBANDS-1 ? fs : bands[i ];
+ (*pp)->gain = bc[i];
+ }
+
+ for(e = param->elm;e != NULL;e = e->next)
+ {
+ if ((ch == 0 && !e->left) || (ch == 1 && !e->right)) continue;
+ if (e->lower >= e->upper) continue;
+
+ for(p=param2.elm;p != NULL;p = p->next)
+ if (p->upper > e->lower) break;
+
+ while(p != NULL && p->lower < e->upper)
+ {
+ if (e->lower <= p->lower && p->upper <= e->upper) {
+ p->gain *= pow(10,e->gain/20);
+ p = p->next;
+ continue;
+ }
+ if (p->lower < e->lower && e->upper < p->upper) {
+ e2 = new paramlistelm;
+ e2->lower = e->upper;
+ e2->upper = p->upper;
+ e2->gain = p->gain;
+ e2->next = p->next;
+ p->next = e2;
+
+ e2 = new paramlistelm;
+ e2->lower = e->lower;
+ e2->upper = e->upper;
+ e2->gain = p->gain * pow(10,e->gain/20);
+ e2->next = p->next;
+ p->next = e2;
+
+ p->upper = e->lower;
+
+ p = p->next->next->next;
+ continue;
+ }
+ if (p->lower < e->lower) {
+ e2 = new paramlistelm;
+ e2->lower = e->lower;
+ e2->upper = p->upper;
+ e2->gain = p->gain * pow(10,e->gain/20);
+ e2->next = p->next;
+ p->next = e2;
+
+ p->upper = e->lower;
+ p = p->next->next;
+ continue;
+ }
+ if (e->upper < p->upper) {
+ e2 = new paramlistelm;
+ e2->lower = e->upper;
+ e2->upper = p->upper;
+ e2->gain = p->gain;
+ e2->next = p->next;
+ p->next = e2;
+
+ p->upper = e->upper;
+ p->gain = p->gain * pow(10,e->gain/20);
+ p = p->next->next;
+ continue;
+ }
+ abort();
+ }
+ }
+}
+
+extern "C" void equ_makeTable(REAL *lbc,REAL *rbc,paramlist *param,REAL fs)
+{
+ int i,cires = cur_ires;
+ REAL *nires;
+
+ if (fs <= 0) return;
+
+ paramlist param2;
+
+ // L
+
+ process_param(lbc,param,param2,fs,0);
+
+ for(i=0;i<winlen;i++)
+ irest[i] = hn(i-winlen/2,param2,fs)*win(i-winlen/2,winlen);
+
+ for(;i<tabsize;i++)
+ irest[i] = 0;
+
+ rfft(tabsize,1,irest);
+
+ nires = cires == 1 ? lires2 : lires1;
+
+ for(i=0;i<tabsize;i++)
+ nires[i] = irest[i];
+
+ process_param(rbc,param,param2,fs,1);
+
+ // R
+
+ for(i=0;i<winlen;i++)
+ irest[i] = hn(i-winlen/2,param2,fs)*win(i-winlen/2,winlen);
+
+ for(;i<tabsize;i++)
+ irest[i] = 0;
+
+ rfft(tabsize,1,irest);
+
+ nires = cires == 1 ? rires2 : rires1;
+
+ for(i=0;i<tabsize;i++)
+ nires[i] = irest[i];
+
+ //
+
+ chg_ires = cires == 1 ? 2 : 1;
+}
+
+extern "C" void equ_quit(void)
+{
+ free(lires1);
+ free(lires2);
+ free(rires1);
+ free(rires2);
+ free(irest);
+ free(fsamples);
+ free(inbuf);
+ free(outbuf);
+
+ lires1 = NULL;
+ lires2 = NULL;
+ rires1 = NULL;
+ rires2 = NULL;
+ irest = NULL;
+ fsamples = NULL;
+ inbuf = NULL;
+ outbuf = NULL;
+
+ rfft(0,0,NULL);
+}
+
+extern "C" void equ_clearbuf(int bps,int srate)
+{
+ int i;
+
+ nbufsamples = 0;
+ for(i=0;i<tabsize*NCH;i++) outbuf[i] = 0;
+}
+
+extern "C" int equ_modifySamples(char *buf,int nsamples,int nch,int bps)
+{
+ int i,p,ch;
+ REAL *ires;
+ int amax = (1 << (bps-1))-1;
+ int amin = -(1 << (bps-1));
+ static float hm1 = 0, hm2 = 0;
+
+ if (chg_ires) {
+ cur_ires = chg_ires;
+ lires = cur_ires == 1 ? lires1 : lires2;
+ rires = cur_ires == 1 ? rires1 : rires2;
+ chg_ires = 0;
+ }
+
+ p = 0;
+
+ while(nbufsamples+nsamples >= winlen)
+ {
+ switch(bps)
+ {
+ case 8:
+ for(i=0;i<(winlen-nbufsamples)*nch;i++)
+ {
+ inbuf[nbufsamples*nch+i] = ((unsigned char *)buf)[i+p*nch] - 0x80;
+ float s = outbuf[nbufsamples*nch+i];
+ if (dither) {
+ float u;
+ s -= hm1;
+ u = s;
+ s += ditherbuf[(ditherptr++) & (DITHERLEN-1)];
+ if (s < amin) s = amin;
+ if (amax < s) s = amax;
+ s = RINT(s);
+ hm1 = s - u;
+ ((unsigned char *)buf)[i+p*nch] = s + 0x80;
+ } else {
+ if (s < amin) s = amin;
+ if (amax < s) s = amax;
+ ((unsigned char *)buf)[i+p*nch] = RINT(s) + 0x80;
+ }
+ }
+ for(i=winlen*nch;i<tabsize*nch;i++)
+ outbuf[i-winlen*nch] = outbuf[i];
+
+ break;
+
+ case 16:
+ for(i=0;i<(winlen-nbufsamples)*nch;i++)
+ {
+ inbuf[nbufsamples*nch+i] = ((short *)buf)[i+p*nch];
+ float s = outbuf[nbufsamples*nch+i];
+ if (dither) {
+ float u;
+ s -= hm1;
+ u = s;
+ s += ditherbuf[(ditherptr++) & (DITHERLEN-1)];
+ if (s < amin) s = amin;
+ if (amax < s) s = amax;
+ s = RINT(s);
+ hm1 = s - u;
+ ((short *)buf)[i+p*nch] = s;
+ } else {
+ if (s < amin) s = amin;
+ if (amax < s) s = amax;
+ ((short *)buf)[i+p*nch] = RINT(s);
+ }
+ }
+ for(i=winlen*nch;i<tabsize*nch;i++)
+ outbuf[i-winlen*nch] = outbuf[i];
+
+ break;
+
+ case 24:
+ for(i=0;i<(winlen-nbufsamples)*nch;i++)
+ {
+ ((int *)inbuf)[nbufsamples*nch+i] =
+ (((unsigned char *)buf)[(i+p*nch)*3 ] ) +
+ (((unsigned char *)buf)[(i+p*nch)*3+1] << 8) +
+ ((( signed char *)buf)[(i+p*nch)*3+2] << 16) ;
+
+ float s = outbuf[nbufsamples*nch+i];
+ //if (dither) s += ditherbuf[(ditherptr++) & (DITHERLEN-1)];
+ if (s < amin) s = amin;
+ if (amax < s) s = amax;
+ int s2 = RINT(s);
+ ((signed char *)buf)[(i+p*nch)*3 ] = s2 & 255; s2 >>= 8;
+ ((signed char *)buf)[(i+p*nch)*3+1] = s2 & 255; s2 >>= 8;
+ ((signed char *)buf)[(i+p*nch)*3+2] = s2 & 255;
+ }
+ for(i=winlen*nch;i<tabsize*nch;i++)
+ outbuf[i-winlen*nch] = outbuf[i];
+
+ break;
+
+ default:
+ assert(0);
+ }
+
+ p += winlen-nbufsamples;
+ nsamples -= winlen-nbufsamples;
+ nbufsamples = 0;
+
+ for(ch=0;ch<nch;ch++)
+ {
+ ires = ch == 0 ? lires : rires;
+
+ if (bps == 24) {
+ for(i=0;i<winlen;i++)
+ fsamples[i] = ((int *)inbuf)[nch*i+ch];
+ } else {
+ for(i=0;i<winlen;i++)
+ fsamples[i] = inbuf[nch*i+ch];
+ }
+
+ for(i=winlen;i<tabsize;i++)
+ fsamples[i] = 0;
+
+ if (enable) {
+ rfft(tabsize,1,fsamples);
+
+ fsamples[0] = ires[0]*fsamples[0];
+ fsamples[1] = ires[1]*fsamples[1];
+
+ for(i=1;i<tabsize/2;i++)
+ {
+ REAL re,im;
+
+ re = ires[i*2 ]*fsamples[i*2] - ires[i*2+1]*fsamples[i*2+1];
+ im = ires[i*2+1]*fsamples[i*2] + ires[i*2 ]*fsamples[i*2+1];
+
+ fsamples[i*2 ] = re;
+ fsamples[i*2+1] = im;
+ }
+
+ rfft(tabsize,-1,fsamples);
+ } else {
+ for(i=winlen-1+winlen/2;i>=winlen/2;i--) fsamples[i] = fsamples[i-winlen/2]*tabsize/2;
+ for(;i>=0;i--) fsamples[i] = 0;
+ }
+
+ for(i=0;i<winlen;i++) outbuf[i*nch+ch] += fsamples[i]/tabsize*2;
+
+ for(i=winlen;i<tabsize;i++) outbuf[i*nch+ch] = fsamples[i]/tabsize*2;
+ }
+ }
+
+ switch(bps)
+ {
+ case 8:
+ for(i=0;i<nsamples*nch;i++)
+ {
+ inbuf[nbufsamples*nch+i] = ((unsigned char *)buf)[i+p*nch] - 0x80;
+ float s = outbuf[nbufsamples*nch+i];
+ if (dither) {
+ float u;
+ s -= hm1;
+ u = s;
+ s += ditherbuf[(ditherptr++) & (DITHERLEN-1)];
+ if (s < amin) s = amin;
+ if (amax < s) s = amax;
+ s = RINT(s);
+ hm1 = s - u;
+ ((unsigned char *)buf)[i+p*nch] = s + 0x80;
+ } else {
+ if (s < amin) s = amin;
+ if (amax < s) s = amax;
+ ((unsigned char *)buf)[i+p*nch] = RINT(s) + 0x80;
+ }
+ }
+ break;
+
+ case 16:
+ for(i=0;i<nsamples*nch;i++)
+ {
+ inbuf[nbufsamples*nch+i] = ((short *)buf)[i+p*nch];
+ float s = outbuf[nbufsamples*nch+i];
+ if (dither) {
+ float u;
+ s -= hm1;
+ u = s;
+ s += ditherbuf[(ditherptr++) & (DITHERLEN-1)];
+ if (s < amin) s = amin;
+ if (amax < s) s = amax;
+ s = RINT(s);
+ hm1 = s - u;
+ ((short *)buf)[i+p*nch] = s;
+ } else {
+ if (s < amin) s = amin;
+ if (amax < s) s = amax;
+ ((short *)buf)[i+p*nch] = RINT(s);
+ }
+ }
+ break;
+
+ case 24:
+ for(i=0;i<nsamples*nch;i++)
+ {
+ ((int *)inbuf)[nbufsamples*nch+i] =
+ (((unsigned char *)buf)[(i+p*nch)*3 ] ) +
+ (((unsigned char *)buf)[(i+p*nch)*3+1] << 8) +
+ ((( signed char *)buf)[(i+p*nch)*3+2] << 16) ;
+
+ float s = outbuf[nbufsamples*nch+i];
+ //if (dither) s += ditherbuf[(ditherptr++) & (DITHERLEN-1)];
+ if (s < amin) s = amin;
+ if (amax < s) s = amax;
+ int s2 = RINT(s);
+ ((signed char *)buf)[(i+p*nch)*3 ] = s2 & 255; s2 >>= 8;
+ ((signed char *)buf)[(i+p*nch)*3+1] = s2 & 255; s2 >>= 8;
+ ((signed char *)buf)[(i+p*nch)*3+2] = s2 & 255;
+ }
+ break;
+
+ default:
+ assert(0);
+ }
+
+ p += nsamples;
+ nbufsamples += nsamples;
+
+ return p;
+}
+
+#if 0
+void usage(void)
+{
+ fprintf(stderr,"Ouch!\n");
+}
+
+int main(int argc,char **argv)
+{
+ FILE *fpi,*fpo;
+ char buf[576*2*2];
+
+ static REAL bc[] =
+ {1.0, 0,1.0, 0,1.0, 0,1.0, 0,1.0, 0,1.0, 0,1.0, 0,1.0, 0,1.0, 0};
+
+ init(14);
+ makeTable(bc,44100);
+
+ if (argc != 3 && argc != 4) exit(-1);
+
+ fpi = fopen(argv[1],"r");
+ fpo = fopen(argv[2],"w");
+
+ if (!fpi || !fpo) exit(-1);
+
+ /* generate wav header */
+
+ {
+ short word;
+ int dword;
+
+ fwrite("RIFF",4,1,fpo);
+ dword = 0;
+ fwrite(&dword,4,1,fpo);
+
+ fwrite("WAVEfmt ",8,1,fpo);
+ dword = 16;
+ fwrite(&dword,4,1,fpo);
+ word = 1;
+ fwrite(&word,2,1,fpo); /* format category, PCM */
+ word = 2;
+ fwrite(&word,2,1,fpo); /* channels */
+ dword = 44100;
+ fwrite(&dword,4,1,fpo); /* sampling rate */
+ dword = 44100*2*2;
+ fwrite(&dword,4,1,fpo); /* bytes per sec */
+ word = 4;
+ fwrite(&word,2,1,fpo); /* block alignment */
+ word = 16;
+ fwrite(&word,2,1,fpo); /* ??? */
+
+ fwrite("data",4,1,fpo);
+ dword = 0;
+ fwrite(&dword,4,1,fpo);
+ }
+
+ preamp = 65536;
+ maxamp = 0;
+
+ if (argc == 4) {
+ preamp = 32767*65536/atoi(argv[3]);
+ fprintf(stderr,"preamp = %d\n",preamp);
+ }
+
+ for(;;)
+ {
+ int n,m;
+
+ n = fread(buf,1,576*2*2,fpi);
+ if (n <= 0) break;
+ m = modifySamples((short *)buf,n/4,2);
+ fwrite(buf,4,m,fpo);
+ }
+
+#if 0
+ for(;;)
+ {
+ int n = flushbuf((short *)buf,576);
+ if (n == 0) break;
+ fwrite(buf,4,n,fpo);
+ }
+#endif
+
+ {
+ short word;
+ int dword;
+ int len = ftell(fpo);
+
+ fseek(fpo,4,SEEK_SET);
+ dword = len-8;
+ fwrite(&dword,4,1,fpo);
+
+ fseek(fpo,40,SEEK_SET);
+ dword = len-44;
+ fwrite(&dword,4,1,fpo);
+ }
+
+ if (maxamp != 0) {
+ fprintf(stderr,"maxamp = %d\n",maxamp);
+ }
+
+ quit();
+}
+#endif
+
+extern "C" void *paramlist_alloc (void) {
+ return (void *)(new paramlist);
+}
+extern "C" void paramlist_free (void *pl) {
+ delete ((paramlist *)pl);
+}
+
|