dsp.c

   1 /* dsp routines */
   2
   3 #include <math.h>
   4 #include <stdlib.h>
   5
   6 #include <dsp.h>
   7
   8
   9 #define PI2 2*3.14159265358
  10
  11 int sample_rate = 0;
  12
  13
  14
  15
  16 void setup_lowpass(lowpass* lp, float f0, float srate){
  17         float T = 1/srate;
  18         float RC = 1/(PI2*f0);
  19
  20         lp->alpha = T / (RC + T);
  21         lp->y = 0;
  22 }
  23
  24 void apply_lowpass(lowpass* lp, float buf[], int count){
  25         int i;
  26         for(i=0; i<count; i++){
  27                 buf[i] = lp->y + lp->alpha*(buf[i] - lp->y);
  28                 lp->y = buf[i];
  29         }
  30 }
  31
  32
  33 void setup_highpass(highpass* hp, float f0, float srate){
  34         float T = 1/srate;
  35         float RC = 1/(PI2*f0);
  36
  37         hp->alpha = RC / (RC + T);
  38         hp->y = 0;
  39         hp->x = 0;
  40 }
  41
  42 void apply_highpass(highpass* hp, float buf[], int count){
  43         int i;
  44         for(i=0; i<count; i++){
  45                 float x = buf[i];
  46                 buf[i] = hp->alpha * (buf[i] + hp->y - hp->x);
  47                 hp->x = x;
  48                 hp->y = buf[i];
  49         }
  50 }
  51
  52
  53 void setup_lp2(biquad* bq, float f0, float Q, float srate){
  54         float w0 = PI2*f0 / srate;
  55         float s = sin(w0);
  56         float c = cos(w0);
  57         float alpha = s / (2*Q);
  58
  59         bq->a0 = 1+alpha;
  60         bq->a1 = -2*c;
  61         bq->a2 = 1-alpha;
  62         bq->b0 = (1-c)/2;
  63         bq->b1 = 1-c;
  64         bq->b2 = (1-c)/2;
  65         bq->a0r = 1/bq->a0;
  66 }
  67
  68 void setup_hp2(biquad* bq, float f0, float Q, float srate){
  69         float w0 = PI2*f0 / srate;
  70         float s = sin(w0);
  71         float c = cos(w0);
  72         float alpha = s / (2*Q);
  73
  74         bq->a0 = 1+alpha;
  75         bq->a1 = -2*c;
  76         bq->a2 = 1-alpha;
  77         bq->b0 = (1+c)/2;
  78         bq->b1 = -1-c;
  79         bq->b2 = (1+c)/2;
  80         bq->a0r = 1/bq->a0;
  81 }
  82
  83 void setup_bp2(biquad* bq, float f0, float BW, float srate){
  84         float w0 = PI2*f0 / srate;
  85         float s = sin(w0);
  86         float c = cos(w0);
  87         float alpha = s*sinh( log(2)/2 * BW * w0/s );
  88
  89         bq->a0 = 1+alpha;
  90         bq->a1 = -2*c;
  91         bq->a2 = 1-alpha;
  92         bq->b0 = alpha;
  93         bq->b1 = 0;
  94         bq->b2 = -alpha;
  95         bq->a0r = 1/bq->a0;
  96 }
  97
  98 void apply_biquad(biquad* bq, float buf[], int count){
  99         int i;
 100         for(i=0; i<count; i++){
 101                 float x = buf[i];
 102
 103                 buf[i] = bq->a0r*(bq->b0*buf[i] + bq->b1*bq->x[0] + bq->b2*bq->x[1]
 104                                 - bq->a1*bq->y[0] - bq->a2*bq->y[1]);
 105
 106                 bq->x[1] = bq->x[0];
 107                 bq->x[0] = x;
 108                 bq->y[1] = bq->y[0];
 109                 bq->y[0] = buf[i];
 110         }
 111 }
 112
 113
 114
 115 int reverse(int x){
 116         x = (((x & 0xaaaaaaaa) >> 1) | ((x & 0x55555555) << 1));
 117         x = (((x & 0xcccccccc) >> 2) | ((x & 0x33333333) << 2));
 118         x = (((x & 0xf0f0f0f0) >> 4) | ((x & 0x0f0f0f0f) << 4));
 119         x = (((x & 0xff00ff00) >> 8) | ((x & 0x00ff00ff) << 8));
 120         return((x >> 16) | (x << 16));
 121 }
 122
 123 void ifft(float in[][2], float out[], const int N){
 124
 125         int D = log2(N)-1;
 126         float T[D][N][2];
 127
 128         int n, L;
 129         int m = 0;
 130         for(n=0; n<N; n++){/* computes top level by reordering input */
 131                 int n1 = reverse(m++);
 132                 int n2 = reverse(m++);
 133                 if(n < 32){
 134                         T[D-1][n][0] = in[n1][0] + in[n2][0];
 135                         T[D-1][n][1] = in[n1][1] + in[n2][1];
 136                 }
 137                 else{
 138                         T[D-1][n][0] = in[n1][0] - in[n2][0];
 139                         T[D-1][n][1] = in[n1][1] - in[n2][1];
 140                 }
 141         }
 142
 143         for(L=D-2; L>=0; L--){/* compute lower levels in terms of one above */
 144                 int j = 0;
 145                 for(n=0; n<64; n++){
 146                         int A = D+1;
 147                         int J = 1<<A;
 148                         float* T0 = T[A][j];
 149                         float* T1 = T[A][j+1];
 150                         float angle = PI2*((n>>A)<<A)/N;
 151                         float C[2] = {cos(angle), sin(angle)};
 152
 153                         //T[L][n] = T0 + T1*C; /* mini fourier transform */
 154                         T[L][n][0] = T1[0]*C[0] - T1[1]*C[1];
 155                         T[L][n][1] = T1[0]*C[1] + T1[1]*C[0];
 156                         T[L][n][0] += T0[0];
 157                         T[L][n][1] += T0[1];
 158
 159                         j+=2;
 160                         if(j == J) j=0;
 161                 }
 162         }
 163
 164         for(n=0; n<N; n++){/* compute output from bottom level */
 165                 float* T0 = T[0][0];
 166                 float* T1 = T[0][1];
 167                 float angle = PI2*n/N;
 168                 float C[2] = {cos(angle), sin(angle)};
 169
 170                 //out[n] = T0 + T1*circle[n];
 171                 out[n] = T1[0]*C[0] - T1[1]*C[1];
 172                 out[n] += T0[0];
 173         }
 174
 175 }
 176
 177
 178
 179
 180
 181 /* delay line routines for use with string synth, reverb chorus echo effects*/
 182 typedef struct {
 183         float* buf;
 184         int ptr_in;
 185         int ptr_out;
 186         int len;
 187         int size;
 188 } delayline;
 189
 190
 191 /* write count samples to the beginning of the delay line */
 192 void delay_write(delayline* d, float in[], int count){
 193         int i;
 194         if(d->ptr_in + count < d->size){
 195                 for(i=0; i<count; i++){
 196                         d->buf[d->ptr_in++] = in[i];
 197                 }
 198         }
 199         else{
 200                 int N = d->size - d->ptr_in;
 201                 for(i=0; i<N; i++){
 202                         d->buf[d->ptr_in++] = in[i];
 203                 }
 204                 d->ptr_in = 0;
 205                 for(i=N; i < count; i++){
 206                         d->buf[d->ptr_in++] = in[i];
 207                 }
 208         }
 209 }
 210
 211 /* read count samples from the end of the delay line */
 212 void delay_read(delayline* d, float out[], int count){
 213         int i;
 214         if(d->ptr_out + count < d->size){
 215                 for(i=0; i<count; i++){
 216                         out[i] = d->buf[d->ptr_out++];
 217                 }
 218         }
 219         else{
 220                 int N = d->size - d->ptr_out;
 221                 for(i=0; i<N; i++){
 222                         out[i] = d->buf[d->ptr_out++];
 223                 }
 224                 d->ptr_out = 0;
 225                 for(i=N; i < count; i++){
 226                         out[i] = d->buf[d->ptr_out++];
 227                 }
 228         }
 229 }
 230
 231 /* same as delay_read but does not actually read anything */
 232 /*
 233    void delay_drop(delayline* d, int count){
 234    if(d->ptr_out + count < d->size){
 235    d->ptr_count += count;
 236    }
 237    else{
 238    d->ptr_count = count - (d->size - d->ptr_out);
 239    }
 240    }*/
 241
 242 /* read count interpolated samples from t samples in the past */
 243 /*
 244    void delay_extract(delayline* d, float t, float out[], int count){
 245    float x = d->ptr_in - t;
 246    if(x < 0) x += d->size;
 247
 248    if(x + count < d->size){
 249    float x0 = floor(x);
 250    float x1 = x0+1.0;
 251    float y0 = d->buf[x0];
 252    float y1 = d->buf[x1];
 253    for(int i=0; i<count; i++){
 254    float m = (y1-y0)/(x1-x0);
 255    out[i] = m*(x-x0) + y0;
 256    x += 1.0;
 257    x0 += x1;
 258    x1 += 1.0;
 259    y0 = y1;
 260    y1 = d->buf[x1];
 261    }
 262    }
 263    else{
 264    int N = d->size - d->ptr_out;
 265    float x0 = floor(x);
 266    float x1 = x0+1.0;
 267    float y0 = d->buf[x0];
 268    float y1 = d->buf[x1];
 269    for(int i=0; i<N; i++){
 270    float m = (y1-y0)/(x1-x0);
 271    out[i] = m*(x-x0) + y0;
 272    x += 1.0;
 273    x0 += x1;
 274    x1 += 1.0;
 275    y0 = y1;
 276    y1 = d->buf[x1];
 277    }
 278
 279    x0 = d->size - 1;
 280    x1 = d->size;
 281    y0 = d->buf[x0];
 282    y1 = d->buf[0];
 283    float m = (y1-y0)/(x1-x0);
 284    out[N] = m*(x-x0) + y0;
 285
 286    x += 1.0;
 287    x -= d->size;
 288    x0 = 0;
 289    x1 = 1;
 290    y0 = d->buf[0];
 291    y1 = d->buf[1];
 292    for(int i=N+1; i < count; i++){
 293    float m = (y1-y0)/(x1-x0);
 294    out[i] = m*(x-x0) + y0;
 295    x += 1.0;
 296    x0 += x1;
 297    x1 += 1.0;
 298    y0 = y1;
 299    y1 = d->buf[x1];
 300    }
 301    }
 302    }*/
 303
 304 /* read samples using a variable delay signal */
 305 void delay_variable(delayline* d, float t[], float out[], int count){
 306         int i;
 307         for(i=0; i<count; i++){
 308                 //delay_extract(d, t[i]+i, out+i, count);
 309         }
 310 }
 311
 312
 313
 314
 315 /* synthesizes a band limited square/saw wave using 16x oversampling */
 316 typedef struct {
 317         float dy; /* frequency */
 318         float y; /* phase counter */
 319         float x; /* low pass state */
 320 } pulse;
 321
 322 void set_pulse_freq(pulse* s, float f){
 323         s->dy = 2*f/(sample_rate*16);
 324 }
 325
 326 void generate_pulse(pulse* s, float step[], float out[], int count, int type){
 327         int N = count*16;
 328         float buf[N];
 329         int i;
 330
 331         const float T = (1.0f/(sample_rate*16));
 332         const float A = T/(1.0f/(PI2*10000) + T);
 333
 334         /* generate */
 335         if(type==0){
 336                 for(i=0; i<N; i++){
 337                         //s->y += s->dy;
 338                         s->y += step[i];
 339                         while(s->y > 1){s->y -= 2;}
 340                         buf[i] = s->y < 0 ? 1 : -1;
 341                 }
 342         }
 343         else{
 344                 for(i=0; i<N; i++){
 345                         buf[i] = s->y;
 346                         //s->y += s->dy;
 347                         s->y += step[i];
 348                         while(s->y > 1){s->y -= 2;}
 349                 }
 350         }
 351
 352         /* low pass */
 353         for(i=0; i<N; i++){
 354                 buf[i] = s->x + A*(buf[i] - s->x);
 355                 s->x = buf[i];
 356         }
 357
 358         /* downsample */
 359         int j = 0;
 360         for(i=0; i<count; i++){
 361                 out[i] = buf[j];
 362                 j+=16;
 363         }
 364 }
 365
 366
 367
 368
 369 /* PAD synth algorithm */
 370 void pad_synth(float amp[], float out[], int size){
 371         int i;
 372         float (*in)[][2] = malloc(size*sizeof(float[2]));
 373         for(i=0; i<size; i++){
 374                 float A = amp[i];
 375                 float p = (rand()*PI2)/RAND_MAX;
 376                 (*in)[i][0] = A*cos(p);
 377                 (*in)[i][1] = A*sin(p);
 378         }
 379         ifft(*in, out, size);
 380         free(in);
 381 }
 382
 383
 384