server/plugins/PartitionedConvolution.cpp

   1 /*
   2         SuperCollider real time audio synthesis system
   3  Copyright (c) 2002 James McCartney. All rights reserved.
   4         http://www.audiosynth.com
   5
   6  This program is free software; you can redistribute it and/or modify
   7  it under the terms of the GNU General Public License as published by
   8  the Free Software Foundation; either version 2 of the License, or
   9  (at your option) any later version.
  10
  11  This program is distributed in the hope that it will be useful,
  12  but WITHOUT ANY WARRANTY; without even the implied warranty of
  13  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  14  GNU General Public License for more details.
  15
  16  You should have received a copy of the GNU General Public License
  17  along with this program; if not, write to the Free Software
  18  Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301  USA
  19  */
  20
  21 //Partitioned Convolution, Nick Collins mid October 2008
  22 //PartConv(in, fftsize, irbufnum, accumbufnum)
  23
  24
  25 #include "FFT_UGens.h"
  26
  27 #include <stdio.h>
  28
  29 struct PartConv : public Unit {
  30         int m_counter;
  31         uint32 m_specbufnumcheck;
  32         float* m_fd_accumulate; //will be exactly fftsize*frames in size
  33         float * m_irspectra;
  34         int m_fd_accum_pos;
  35         int m_partitions; //number of frames impulse response is partitioned into
  36         int m_fullsize;
  37
  38         //sliding window code
  39         int m_fftsize; //must be power of two
  40         //int m_windowsize; //also half fftsize, was partition size, just use nover2 for this
  41         int m_nover2;
  42
  43         //int m_hopsize; //hopsize will be half fftsize
  44         //int m_shuntsize;
  45         int m_pos;
  46         float * m_inputbuf;
  47         float * m_spectrum;
  48         scfft* m_scfft;
  49         float * m_inputbuf2;
  50         float * m_spectrum2;
  51         scfft* m_scifft; //inverse
  52         int m_outputpos;
  53         float * m_output;
  54
  55         //amortisation state
  56         int m_blocksize, m_sr;
  57         int m_spareblocks;
  58         int m_numamort; //will relate number of partitions to number of spare blocks
  59         int m_lastamort;
  60         int m_amortcount;
  61         int m_partitionsdone;
  62 };
  63
  64 extern "C" {
  65
  66         void PartConv_next(PartConv *unit, int inNumSamples);
  67         void PartConv_Ctor(PartConv* unit);
  68         void PartConv_Dtor(PartConv* unit);
  69
  70 }
  71
  72 void PartConv_Ctor( PartConv* unit )
  73 {
  74
  75         unit->m_fftsize= (int) ZIN0(1);
  76         unit->m_nover2= unit->m_fftsize>>1;
  77         //unit->m_windowsize= unit->m_fftsize>>1;
  78
  79         //unit->m_hopsize= unit->m_fftsize>>1;
  80         //unit->m_shuntsize== unit->hopsize;
  81
  82
  83
  84         //integer division
  85
  86         unit->m_inputbuf= (float*)RTAlloc(unit->mWorld, unit->m_fftsize * sizeof(float));
  87         unit->m_spectrum= (float*)RTAlloc(unit->mWorld, unit->m_fftsize * sizeof(float));
  88
  89         SCWorld_Allocator alloc(ft, unit->mWorld);
  90         unit->m_scfft = scfft_create(unit->m_fftsize, unit->m_fftsize, kRectWindow, unit->m_inputbuf, unit->m_spectrum, kForward, alloc);
  91
  92         //inverse
  93         unit->m_inputbuf2= (float*)RTAlloc(unit->mWorld, unit->m_fftsize * sizeof(float));
  94         unit->m_spectrum2= (float*)RTAlloc(unit->mWorld, unit->m_fftsize * sizeof(float));
  95         //in place this time
  96         unit->m_scifft = scfft_create(unit->m_fftsize, unit->m_fftsize, kRectWindow, unit->m_inputbuf2, unit->m_spectrum2, kBackward, alloc);
  97
  98         //debug test: changing scale factors in case amplitude summation is a problem
  99         //unit->m_scfft->scalefac=1.0/45.254833995939;
 100         //unit->m_scifft->scalefac=1.0/45.254833995939;
 101
 102
 103
 104         unit->m_output= (float*)RTAlloc(unit->mWorld, unit->m_fftsize * sizeof(float));
 105         unit->m_outputpos=0;
 106
 107         memset(unit->m_output, 0, unit->m_fftsize * sizeof(float));
 108
 109         memset(unit->m_inputbuf, 0, unit->m_fftsize * sizeof(float));
 110         unit->m_pos=0;
 111
 112         //get passed in buffer
 113         unit->m_fd_accumulate=NULL;
 114
 115         uint32 bufnum = (uint32)ZIN0(2);
 116         SndBuf *buf;
 117
 118         if (bufnum >=  unit->mWorld->mNumSndBufs) {
 119
 120                 int localBufNum = bufnum - unit->mWorld->mNumSndBufs;
 121                 Graph *parent = unit->mParent;
 122                 if(localBufNum <= parent->localMaxBufNum) {
 123                         buf = parent->mLocalSndBufs + localBufNum;
 124
 125                 } else {
 126
 127                         printf("PartConv Error: Invalid Spectral data bufnum %d \n", bufnum);
 128                 SETCALC(*ClearUnitOutputs);
 129                 unit->mDone = true;
 130                 return;
 131
 132                 }
 133         }
 134
 135         buf = unit->mWorld->mSndBufs + bufnum;
 136
 137         unit->m_specbufnumcheck = bufnum;
 138
 139         //buf = unit->mWorld->mSndBufs + bufnum;
 140
 141
 142         if (!buf->data) {
 143                 //unit->mDone = true;
 144                 printf("PartConv Error: Spectral data buffer not allocated \n");
 145                 SETCALC(*ClearUnitOutputs);
 146                 unit->mDone = true;
 147                 return;
 148         }
 149
 150         unit->m_irspectra = buf->data;
 151
 152         unit->m_fullsize = buf->samples;
 153
 154         unit->m_partitions=buf->samples/(unit->m_fftsize); //should be exact
 155         //printf("another check partitions %d irspecsize %d fftsize %d \n", unit->m_partitions,unit->m_fullsize, unit->m_fftsize);
 156
 157         if((buf->samples % unit->m_fftsize !=0) || (buf->samples==0)) {
 158                 printf("PartConv Error: fftsize doesn't divide spectral data buffer size or spectral data buffer size is zero\n");
 159                 SETCALC(*ClearUnitOutputs);
 160                 unit->mDone = true;
 161                 return;
 162         }
 163
 164         //CHECK SAMPLING RATE AND BUFFER SIZE
 165         unit->m_blocksize = unit->mWorld->mFullRate.mBufLength;
 166         //if(unit->m_blocksize!=64) printf("TPV complains: block size not 64, you have %d\n", unit->m_blocksize);
 167         unit->m_sr = unit->mWorld->mSampleRate;
 168         //if(unit->m_sr!=44100) printf("TPV complains: sample rate not 44100, you have %d\n", unit->m_sr);
 169
 170         if(unit->m_nover2 % unit->m_blocksize !=0) {
 171                 printf("PartConv Error: block size doesn't divide partition size\n");
 172                 SETCALC(*ClearUnitOutputs);
 173                 unit->mDone = true;
 174                 return;
 175         } else {
 176
 177                 //must be exact divisor
 178                 int blocksperpartition = unit->m_nover2/unit->m_blocksize;
 179
 180                 unit->m_spareblocks = blocksperpartition-1;
 181
 182                 if(unit->m_spareblocks<1) {
 183                         printf("PartConv Error: no spareblocks, amortisation not possible! \n");
 184                         SETCALC(*ClearUnitOutputs);
 185                         unit->mDone = true;
 186                         return;
 187                 }
 188
 189                 //won't be exact
 190                 unit->m_numamort = (unit->m_partitions-1)/unit->m_spareblocks; //will relate number of partitions to number of spare blocks
 191                 unit->m_lastamort= (unit->m_partitions-1)- ((unit->m_spareblocks-1)*(unit->m_numamort)); //allow for error on last one
 192                 unit->m_amortcount= -1; //starts as flag to avoid any amortisation before have first fft done
 193                 unit->m_partitionsdone=1;
 194
 195                 //printf("Amortisation stats partitions %d nover2 %d blocksize %d spareblocks %d numamort %d lastamort %d \n", unit->m_partitions,unit->m_nover2, unit->m_blocksize, unit->m_spareblocks, unit->m_numamort, unit->m_lastamort);
 196
 197                 unit->m_fd_accumulate= (float*)RTAlloc(unit->mWorld, unit->m_fullsize * sizeof(float));
 198                 memset(unit->m_fd_accumulate, 0, unit->m_fullsize * sizeof(float));
 199                 unit->m_fd_accum_pos=0;
 200
 201 //      used to be passed in as buffer, simplified interface
 202 //      bufnum = (uint32)ZIN0(3);
 203 //      if (bufnum >= unit->mWorld->mNumSndBufs) {
 204 //              bufnum = 1;
 205 //      }
 206 //      buf =  unit->mWorld->mSndBufs + bufnum;
 207 //
 208 //      if (!buf->data) {
 209 //              printf("PartConv Error: Accumulation buffer not allocated \n");
 210 //              SETCALC(*ClearUnitOutputs);
 211 //              unit->mDone = true;
 212 //              return;
 213 //      }
 214 //
 215 //      unit->m_fd_accumulate = buf->data;
 216
 217
 218                 SETCALC(PartConv_next);
 219         }
 220 }
 221
 222 void PartConv_Dtor(PartConv *unit)
 223 {
 224         RTFree(unit->mWorld, unit->m_inputbuf);
 225         RTFree(unit->mWorld, unit->m_inputbuf2);
 226         RTFree(unit->mWorld, unit->m_spectrum);
 227         RTFree(unit->mWorld, unit->m_spectrum2);
 228         RTFree(unit->mWorld, unit->m_output);
 229         if (unit->m_fd_accumulate) RTFree(unit->mWorld, unit->m_fd_accumulate);
 230
 231         SCWorld_Allocator alloc(ft, unit->mWorld);
 232         if(unit->m_scfft)
 233                 scfft_destroy(unit->m_scfft, alloc);
 234
 235         if(unit->m_scifft)
 236                 scfft_destroy(unit->m_scifft, alloc);
 237 }
 238
 239 void PartConv_next( PartConv *unit, int inNumSamples )
 240 {
 241         float *in = IN(0);
 242         float *out = OUT(0);
 243         int pos = unit->m_pos;
 244
 245         //safety check
 246         if (!(unit->mWorld->mSndBufs + unit->m_specbufnumcheck)->data) {
 247                 //unit->mDone = true;
 248                 printf("PartConv Error: Spectral data buffer not allocated \n");
 249                 ClearUnitOutputs(unit, inNumSamples);
 250                 SETCALC(*ClearUnitOutputs);
 251                 unit->mDone = true;
 252                 return;
 253         }
 254
 255         float * input= unit->m_inputbuf;
 256         float * output= unit->m_output;
 257         int outputpos= unit->m_outputpos;
 258
 259         //into input buffer
 260         memcpy(input+pos, in, inNumSamples * sizeof(float));
 261
 262         pos+=inNumSamples;
 263
 264         //if ready for new FFT
 265         int nover2= unit->m_nover2;
 266
 267         //assumes that blocksize perfectly divides windowsize
 268         if (pos==nover2) {
 269                 //FFT this input, second half of input always zero
 270
 271                 //memset(input+unit->m_nover2, 0, sizeof(float)*unit->m_nover2);
 272
 273                 scfft_dofft(unit->m_scfft);
 274
 275                 //reset pos into input buffer
 276                 pos=0;
 277                 //reset outputpos
 278                 outputpos= 0;
 279
 280                 float * spectrum= unit->m_spectrum;
 281                 float * spectrum2= unit->m_spectrum2;
 282
 283                 //multiply spectra and accumulate for all ir spectra across storage buffer
 284
 285                 int fftsize= unit->m_fftsize;
 286                 //int frames= unit->m_partitions;
 287                 int accumpos= unit->m_fd_accum_pos;
 288                 float * accumbuffer= unit->m_fd_accumulate;
 289
 290                 float * irspectrum= unit->m_irspectra;
 291
 292                 int fullsize= unit->m_fullsize;
 293
 294                 //JUST DO FIRST ONE FOR NOW, AMORTISED FOR OTHERS
 295                 //frames
 296                 for (int i=0; i<1; ++i) {
 297
 298                         int irpos= (i*fftsize);
 299                         int posnow= (accumpos+irpos)%fullsize;
 300                         float * target= accumbuffer+posnow;
 301                         float * ir= irspectrum+irpos;
 302
 303                         //real multiply for dc and nyquist
 304                         target[0] += ir[0]*spectrum[0];
 305                         target[1] += ir[1]*spectrum[1];
 306
 307                         //complex multiply for frequency bins
 308                         for (int j=1; j<nover2; ++j) {
 309
 310                                 int binposr= 2*j;
 311                                 int binposi= binposr+1;
 312
 313                                 //complex multiply
 314                                 target[binposr] += (ir[binposr]*spectrum[binposr]) - (ir[binposi]*spectrum[binposi]);
 315                                 target[binposi] += (ir[binposi]*spectrum[binposr]) + (ir[binposr]*spectrum[binposi]);
 316                         }
 317
 318
 319         //LESS efficient!
 320                 //      float * irreal = ir;
 321 //                      float * irimag = ir+1;
 322 //                      float * specreal = spectrum;
 323 //                      float * specimag = spectrum+1;
 324 //                      float * targetreal= target;
 325 //                      float * targetimag= target+1;
 326 //
 327 //                      for (j=1; j<nover2; ++j) {
 328 //
 329 //                              irreal+=2; irimag+=2;
 330 //                              specreal+=2; specimag+=2;
 331 //                              targetreal+=2; targetimag+=2;
 332 //
 333 //                              //complex multiply
 334 //                              (*targetreal)+=  (*irreal)*(*specreal) - (*irimag)*(*specimag);
 335 //                              (*targetimag)+=  (*irimag)*(*specreal) - (*irreal)*(*specimag);
 336 //                      }
 337
 338
 339
 340                 }
 341
 342                 //need separate scfft object with forward= false
 343                 //IFFT this partition
 344                 float * input2= unit->m_inputbuf2;
 345                 memcpy(input2, accumbuffer+accumpos, fftsize * sizeof(float));
 346                 scfft_doifft(unit->m_scifft);
 347
 348                 //shunt output data down and zero top bit
 349                 memcpy(output, output+nover2, nover2 * sizeof(float));
 350                 memset(output+nover2, 0, nover2 * sizeof(float));
 351
 352                 //surely last entry should be zero?
 353                 //printf("sanity check %f \n",spectrum2[0]); //spectrum2[fftsize-1]
 354                 //(fftsize-1)
 355
 356                 //sum into output
 357                 for (int j=0; j<fftsize; ++j)
 358                         output[j] += spectrum2[j];
 359
 360                 //int testindex2= rgen.irand(fftsize-1);
 361                 //printf("outputtest j %d value %f index %d value2 %f \n",testindex2 , output[testindex2], testindex2, *(accumbuffer+accumpos+testindex2));
 362
 363                 //zero this partition
 364                 memset(accumbuffer+accumpos, 0, fftsize * sizeof(float));
 365
 366                 //ONLY DO AT END OF AMORTISATION???? no, amort code has extra -1 in indexing to cope
 367                 //update partition counter
 368                 accumpos= (accumpos+fftsize)%fullsize;
 369                 unit->m_fd_accum_pos= accumpos;
 370
 371                 //set up for amortisation (calculate output for other partitions of impulse response)
 372                 unit->m_amortcount=0;
 373                 unit->m_partitionsdone=1;
 374         }
 375         else {
 376         //amortisation steps:
 377         //complex multiply of this new fft spectrum against existing irspectrums and sum to accumbuffer
 378                 if (unit->m_amortcount>=0) {
 379
 380                 float * spectrum= unit->m_spectrum;
 381
 382                 //multiply spectra and accumulate for all ir spectra across storage buffer
 383
 384                 int fftsize= unit->m_fftsize;
 385                 int nover2= unit->m_nover2;
 386                 //int frames= unit->m_partitions;
 387                 int accumpos= unit->m_fd_accum_pos;
 388                 float * accumbuffer= unit->m_fd_accumulate;
 389
 390                 float * irspectrum= unit->m_irspectra;
 391
 392                 int fullsize= unit->m_fullsize;
 393
 394                 int starti, stopi;
 395                 int number;
 396
 397                 if(unit->m_amortcount==(unit->m_spareblocks-1)) {
 398                         number= unit->m_lastamort;
 399                 }
 400                 else
 401                 {
 402                         number= unit->m_numamort;
 403                 }
 404
 405                 starti= unit->m_partitionsdone-1;
 406                 stopi= starti+number-1;
 407
 408                 //printf("amort check count %d starti %d stopi %d number %d framesdone %d \n",unit->m_amortcount, starti, stopi, number, unit->m_partitionsdone);
 409
 410                 unit->m_partitionsdone += number;
 411                 ++unit->m_amortcount;
 412
 413
 414                 //JUST DO FIRST ONE FOR NOW
 415                 //frames
 416                 for (int i=starti; i<=stopi; ++i) {
 417
 418                         int posnow= (accumpos+(i*fftsize))%fullsize;
 419                         float * target= accumbuffer+posnow;
 420                         int irpos= (i*fftsize);
 421                         float * ir= irspectrum+irpos;
 422
 423                         //real multiply for dc and nyquist
 424                         target[0]+=  ir[0]*spectrum[0];
 425                         target[1]+=      ir[1]*spectrum[1];
 426
 427                         //complex multiply for frequency bins
 428                         for (int j=1; j<nover2; ++j) {
 429
 430                                 int binposr= 2*j;
 431                                 int binposi= binposr+1;
 432
 433                                 //complex multiply
 434                                 target[binposr]+=  (ir[binposr]*spectrum[binposr]) - (ir[binposi]*spectrum[binposi]);
 435                                 target[binposi]+=  (ir[binposi]*spectrum[binposr]) + (ir[binposr]*spectrum[binposi]);
 436
 437                         }
 438
 439                         //printf("posnow %d i %d dc %f nyquist %f\n", posnow, i, target[400], target[405]);
 440
 441                 }
 442
 443                 //if(unit->m_amortcount==(unit->m_spareblocks-1)) {
 444                 //      number= unit->m_numamort;
 445                 //}
 446
 447                 }
 448
 449         }
 450
 451
 452         //control rate, just one out value
 453         //output max
 454         //for(j=0; j<inNumSamples; ++j) {
 455         //out[j]= outputbuf[outputpos+j];
 456         //}
 457
 458
 459         //do this second!
 460         memcpy(out, output+outputpos, inNumSamples * sizeof(float));
 461
 462 //debugging tests: output values tend to be fftsize too big, probably due to complex multiply and also summation over all partitions
 463 //      RGen& rgen = *unit->mParent->mRGen;
 464 //      int testindex= rgen.irand(inNumSamples-1);
 465 //      printf("inNumSamples %d testindex %d out %f output %f \n",inNumSamples, testindex, out[testindex], *(output+outputpos+testindex));
 466 //
 467         outputpos+=inNumSamples;
 468
 469         unit->m_outputpos= outputpos;
 470
 471         unit->m_pos= pos;
 472 }
 473
 474
 475
 476
 477
 478 //channels not used- should just be mono, num frames= num samples
 479 //buffer preparation
 480 void PreparePartConv(World *world, struct SndBuf *buf, struct sc_msg_iter *msg)
 481 {
 482         int frames1 = buf->frames;
 483         //int channels1 = buf->channels;
 484         float *data1 = buf->data;
 485
 486         uint32 frombufnum = msg->geti();
 487         int fftsize = msg->geti();
 488
 489         //output size must be frombuf->frames*2
 490
 491         if (frombufnum >= world->mNumSndBufs) frombufnum = 0;
 492         SndBuf* frombuf = world->mSndBufs + frombufnum;
 493
 494         int frames2 = frombuf->frames;
 495         //int channels2 = frombuf->channels;
 496
 497         float *data2 = frombuf->data;
 498         //int size = buf->samples;
 499
 500         //scfft
 501         int nover2= fftsize>>1;
 502
 503         //int numpartitions = msg->geti();
 504
 505         int numpartitions;
 506         if(frames2%nover2==0)
 507                 numpartitions= frames2/nover2;
 508         else
 509                 numpartitions= (frames2/nover2)+1;
 510
 511         //check numpartitions*fftsize= frames2
 512
 513         //printf("reality check numpartitions %d fftsize %d product %d numirframes %d numinputframes %d \n", numpartitions, fftsize, numpartitions*fftsize, frames1, frames2);
 514
 515         //integer division
 516
 517         float * inputbuf= (float*)RTAlloc(world, fftsize * sizeof(float));
 518         float * spectrum= (float*)RTAlloc(world, fftsize * sizeof(float));
 519
 520         SCWorld_Allocator alloc(ft, world);
 521         scfft* m_scfft = scfft_create(fftsize, fftsize, kRectWindow, inputbuf, spectrum, kForward, alloc);
 522
 523         //for zero padding
 524         memset(inputbuf, 0, sizeof(float)*fftsize);
 525
 526         //run through input data buffer, taking nover2 chunks, zero padding each
 527         for (int i=0; i<numpartitions; ++i) {
 528                 int indexnow= nover2*i;
 529                 int indexout= fftsize*i;
 530
 531                 if(i<(numpartitions-1))
 532                 //memset(inputbuf, 0, sizeof(float)*fftsize);
 533                         memcpy(inputbuf, data2+indexnow, nover2 * sizeof(float));
 534                 else
 535                 {
 536                         int takenow= frames2%nover2;
 537
 538                         if(frames2==nover2)
 539                                 takenow= nover2;
 540
 541                         memcpy(inputbuf, data2+indexnow, takenow * sizeof(float));
 542
 543                         if(takenow<nover2)
 544                                 memset(inputbuf+takenow, 0, (nover2-takenow)*sizeof(float));
 545                 }
 546
 547                 scfft_dofft(m_scfft);
 548
 549                 memcpy(data1+indexout, spectrum, fftsize * sizeof(float));
 550
 551         }
 552
 553         //clean up
 554         RTFree(world, inputbuf);
 555         RTFree(world, spectrum);
 556
 557         if(m_scfft)
 558                 scfft_destroy(m_scfft, alloc);
 559 }
 560
 561
 562
 563 void initPartConv(InterfaceTable *inTable)
 564 {
 565         ft = inTable;
 566
 567         DefineDtorCantAliasUnit(PartConv);
 568
 569         DefineBufGen("PreparePartConv", PreparePartConv);
 570 }