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