src/calf/primitives.h

   1 /* Calf DSP Library
   2  * DSP primitives.
   3  *
   4  * Copyright (C) 2001-2007 Krzysztof Foltman
   5  *
   6  * This program is free software; you can redistribute it and/or
   7  * modify it under the terms of the GNU Lesser General Public
   8  * License as published by the Free Software Foundation; either
   9  * version 2 of the License, or (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 GNU
  14  * Lesser General Public License for more details.
  15  *
  16  * You should have received a copy of the GNU Lesser General
  17  * Public License along with this program; if not, write to the
  18  * Free Software Foundation, Inc., 51 Franklin Street, Fifth Floor,
  19  * Boston, MA 02111-1307, USA.
  20  */
  21 #ifndef __CALF_PRIMITIVES_H
  22 #define __CALF_PRIMITIVES_H
  23
  24 #include <assert.h>
  25 #include <stdint.h>
  26 #include <stdio.h>
  27 #include <string.h>
  28 #include <cmath>
  29 #include <cstdlib>
  30 #include <map>
  31
  32 namespace dsp {
  33
  34 /// Set a float to zero
  35 inline void zero(float &v) {
  36     v = 0.f;
  37 };
  38
  39 /// Set a double to zero
  40 inline void zero(double &v) {
  41     v = 0.0;
  42 };
  43
  44 /// Set 64-bit unsigned integer value to zero
  45 inline void zero(uint64_t &v) { v = 0; };
  46 /// Set 32-bit unsigned integer value to zero
  47 inline void zero(uint32_t &v) { v = 0; };
  48 /// Set 16-bit unsigned integer value to zero
  49 inline void zero(uint16_t &v) { v = 0; };
  50 /// Set 8-bit unsigned integer value to zero
  51 inline void zero(uint8_t &v) { v = 0; };
  52 /// Set 64-bit signed integer value to zero
  53 inline void zero(int64_t &v) { v = 0; };
  54 /// Set 32-bit signed integer value to zero
  55 inline void zero(int32_t &v) { v = 0; };
  56 /// Set 16-bit signed integer value to zero
  57 inline void zero(int16_t &v) { v = 0; };
  58 /// Set 8-bit signed integer value to zero
  59 inline void zero(int8_t &v) { v = 0; };
  60
  61 /// Set array (buffer or anything similar) to vector of zeroes
  62 template<class T>
  63 void zero(T *data, unsigned int size) {
  64     T value;
  65     dsp::zero(value);
  66     for (unsigned int i=0; i<size; i++)
  67         *data++ = value;
  68 }
  69
  70 /// Set array (buffer or anything similar) to vector of values
  71 template<class T>
  72 void fill(T *data, T value, unsigned int size) {
  73     for (unsigned int i=0; i<size; i++)
  74         *data++ = value;
  75 }
  76
  77 template<class T = float>struct stereo_sample {
  78     T left;
  79     T right;
  80     /// default constructor - preserves T's semantics (ie. no implicit initialization to 0)
  81     inline stereo_sample() {
  82     }
  83     inline stereo_sample(T _left, T _right) {
  84         left = _left;
  85         right = _right;
  86     }
  87     inline stereo_sample(T _both) {
  88         left = right = _both;
  89     }
  90     template<typename U>
  91     inline stereo_sample(const stereo_sample<U> &value) {
  92         left = value.left;
  93         right = value.right;
  94     }
  95     inline stereo_sample& operator=(const T &value) {
  96         left = right = value;
  97         return *this;
  98     }
  99     template<typename U>
 100     inline stereo_sample& operator=(const stereo_sample<U> &value) {
 101         left = value.left;
 102         right = value.right;
 103         return *this;
 104     }
 105 /*
 106     inline operator T() const {
 107         return (left+right)/2;
 108     }
 109 */
 110     inline stereo_sample& operator*=(const T &multiplier) {
 111         left *= multiplier;
 112         right *= multiplier;
 113         return *this;
 114     }
 115     inline stereo_sample& operator+=(const stereo_sample<T> &value) {
 116         left += value.left;
 117         right += value.right;
 118         return *this;
 119     }
 120     inline stereo_sample& operator-=(const stereo_sample<T> &value) {
 121         left -= value.left;
 122         right -= value.right;
 123         return *this;
 124     }
 125     template<typename U> inline stereo_sample<U> operator*(const U &value) const {
 126         return stereo_sample<U>(left*value, right*value);
 127     }
 128     /*inline stereo_sample<float> operator*(float value) const {
 129         return stereo_sample<float>(left*value, right*value);
 130     }
 131     inline stereo_sample<double> operator*(double value) const {
 132         return stereo_sample<double>(left*value, right*value);
 133     }*/
 134     inline stereo_sample<T> operator+(const stereo_sample<T> &value) {
 135         return stereo_sample(left+value.left, right+value.right);
 136     }
 137     inline stereo_sample<T> operator-(const stereo_sample<T> &value) {
 138         return stereo_sample(left-value.left, right-value.right);
 139     }
 140     inline stereo_sample<T> operator+(const T &value) {
 141         return stereo_sample(left+value, right+value);
 142     }
 143     inline stereo_sample<T> operator-(const T &value) {
 144         return stereo_sample(left-value, right-value);
 145     }
 146     inline stereo_sample<float> operator+(float value) {
 147         return stereo_sample<float>(left+value, right+value);
 148     }
 149     inline stereo_sample<float> operator-(float value) {
 150         return stereo_sample<float>(left-value, right-value);
 151     }
 152     inline stereo_sample<double> operator+(double value) {
 153         return stereo_sample<double>(left+value, right+value);
 154     }
 155     inline stereo_sample<double> operator-(double value) {
 156         return stereo_sample<double>(left-value, right-value);
 157     }
 158 };
 159
 160 /// Multiply constant by stereo_value
 161 template<class T>
 162 inline stereo_sample<T> operator*(const T &value, const stereo_sample<T> &value2) {
 163     return stereo_sample<T>(value2.left*value, value2.right*value);
 164 }
 165
 166 /// Add constant to stereo_value
 167 template<class T>
 168 inline stereo_sample<T> operator+(const T &value, const stereo_sample<T> &value2) {
 169     return stereo_sample<T>(value2.left+value, value2.right+value);
 170 }
 171
 172 /// Subtract stereo_value from constant (yields stereo_value of course)
 173 template<class T>
 174 inline stereo_sample<T> operator-(const T &value, const stereo_sample<T> &value2) {
 175     return stereo_sample<T>(value-value2.left, value-value2.right);
 176 }
 177
 178 /// Shift value right by 'bits' bits (multiply by 2^-bits)
 179 template<typename T>
 180 inline stereo_sample<T> shr(stereo_sample<T> v, int bits = 1) {
 181     v.left = shr(v.left, bits);
 182     v.right = shr(v.right, bits);
 183     return v;
 184 }
 185
 186 /// Set a stereo_sample<T> value to zero
 187 template<typename T>
 188 inline void zero(stereo_sample<T> &v) {
 189     dsp::zero(v.left);
 190     dsp::zero(v.right);
 191 }
 192
 193 /// 'Small value' for integer and other types
 194 template<typename T>
 195 inline T small_value() {
 196     return 0;
 197 }
 198
 199 /// 'Small value' for floats (2^-24) - used for primitive underrun prevention. The value is pretty much arbitrary (allowing for 24-bit signals normalized to 1.0).
 200 template<>
 201 inline float small_value<float>() {
 202     return (1.0/16777216.0); // allows for 2^-24, should be enough for 24-bit DACs at least :)
 203 }
 204
 205 /// 'Small value' for doubles (2^-24) - used for primitive underrun prevention. The value is pretty much arbitrary.
 206 template<>
 207 inline double small_value<double>() {
 208     return (1.0/16777216.0);
 209 }
 210
 211 /// Convert a single value to single value = do nothing :) (but it's a generic with specialisation for stereo_sample)
 212 template<typename T>
 213 inline float mono(T v) {
 214     return v;
 215 }
 216
 217 /// Convert a stereo_sample to single value by averaging two channels
 218 template<typename T>
 219 inline T mono(stereo_sample<T> v) {
 220     return shr(v.left+v.right);
 221 }
 222
 223 /// Clip a value to [min, max]
 224 template<typename T>
 225 inline T clip(T value, T min, T max) {
 226     if (value < min) return min;
 227     if (value > max) return max;
 228     return value;
 229 }
 230
 231 /// Clip a double to [-1.0, +1.0]
 232 inline double clip11(double value) {
 233     double a = fabs(value);
 234     if (a<=1) return value;
 235     return (value<0) ? -1.0 : 1.0;
 236 }
 237
 238 /// Clip a float to [-1.0f, +1.0f]
 239 inline float clip11(float value) {
 240     float a = fabsf(value);
 241     if (a<=1) return value;
 242     return (value<0) ? -1.0f : 1.0f;
 243 }
 244
 245 /// Clip a double to [0.0, +1.0]
 246 inline double clip01(double value) {
 247     double a = fabs(value-0.5);
 248     if (a<=0.5) return value;
 249     return (a<0) ? -0.0 : 1.0;
 250 }
 251
 252 /// Clip a float to [0.0f, +1.0f]
 253 inline float clip01(float value) {
 254     float a = fabsf(value-0.5f);
 255     if (a<=0.5f) return value;
 256     return (value < 0) ? -0.0f : 1.0f;
 257 }
 258
 259 // Linear interpolation (mix-way between v1 and v2).
 260 template<typename T, typename U>
 261 inline T lerp(T v1, T v2, U mix) {
 262     return v1+(v2-v1)*mix;
 263 }
 264
 265 // Linear interpolation for stereo values (mix-way between v1 and v2).
 266 template<typename T>
 267 inline stereo_sample<T> lerp(stereo_sample<T> &v1, stereo_sample<T> &v2, float mix) {
 268     return stereo_sample<T>(v1.left+(v2.left-v1.left)*mix, v1.right+(v2.right-v1.right)*mix);
 269 }
 270
 271 /**
 272  * decay-only envelope (linear or exponential); deactivates itself when it goes below a set point (epsilon)
 273  */
 274 class decay
 275 {
 276     double value, initial;
 277     unsigned int age, mask;
 278     bool active;
 279 public:
 280     decay() {
 281         active = false;
 282         mask = 127;
 283         initial = value = 0.0;
 284     }
 285     inline bool get_active() {
 286         return active;
 287     }
 288     inline double get() {
 289         return active ? value : 0.0;
 290     }
 291     inline void set(double v) {
 292         initial = value = v;
 293         active = true;
 294         age = 0;
 295     }
 296     /// reinitialise envelope (must be called if shape changes from linear to exponential or vice versa in the middle of envelope)
 297     inline void reinit()
 298     {
 299         initial = value;
 300         age = 1;
 301     }
 302     inline void add(double v) {
 303         if (active)
 304             value += v;
 305         else
 306             value = v;
 307         initial = value;
 308         age = 0;
 309         active = true;
 310     }
 311     static inline double calc_exp_constant(double times, double cycles)
 312     {
 313         if (cycles < 1.0)
 314             cycles = 1.0;
 315         return pow(times, 1.0 / cycles);
 316     }
 317     inline void age_exp(double constant, double epsilon) {
 318         if (active) {
 319             if (!(age & mask))
 320                 value = initial * pow(constant, (double)age);
 321             else
 322                 value *= constant;
 323             if (value < epsilon)
 324                 active = false;
 325             age++;
 326         }
 327     }
 328     inline void age_lin(double constant, double epsilon) {
 329         if (active) {
 330             if (!(age & mask))
 331                 value = initial - constant * age;
 332             else
 333                 value -= constant;
 334             if (value < epsilon)
 335                 active = false;
 336             age++;
 337         }
 338     }
 339     inline void deactivate() {
 340         active = false;
 341         value = 0;
 342     }
 343 };
 344
 345 class scheduler;
 346
 347 class task {
 348 public:
 349     virtual void execute(scheduler *s)=0;
 350     virtual void dispose() { delete this; }
 351     virtual ~task() {}
 352 };
 353
 354 /// this scheduler is based on std::multimap, so it isn't very fast, I guess
 355 /// maybe some day it should be rewritten to use heapsort or something
 356 /// work in progress, don't use!
 357 class scheduler {
 358     std::multimap<unsigned int, task *> timeline;
 359     unsigned int time, next_task;
 360     bool eob;
 361     class end_buf_task: public task {
 362     public:
 363         scheduler *p;
 364         end_buf_task(scheduler *_p) : p(_p) {}
 365         virtual void execute(scheduler *s) { p->eob = true; }
 366         virtual void dispose() { }
 367     } eobt;
 368 public:
 369
 370     scheduler()
 371     : time(0)
 372     , next_task((unsigned)-1)
 373     , eob(true)
 374     , eobt (this)
 375     {
 376         time = 0;
 377         next_task = (unsigned)-1;
 378         eob = false;
 379     }
 380     inline bool is_next_tick() {
 381         if (time < next_task)
 382             return true;
 383         do_tasks();
 384         return false;
 385     }
 386     inline void next_tick() {
 387         time++;
 388     }
 389     void set(int pos, task *t) {
 390         timeline.insert(std::pair<unsigned int, task *>(time+pos, t));
 391         next_task = timeline.begin()->first;
 392     }
 393     void do_tasks() {
 394         std::multimap<unsigned int, task *>::iterator i = timeline.begin();
 395         while(i != timeline.end() && i->first == time) {
 396             i->second->execute(this);
 397             i->second->dispose();
 398             timeline.erase(i);
 399         }
 400     }
 401     bool is_eob() {
 402         return eob;
 403     }
 404     void set_buffer_size(int count) {
 405         set(count, &eobt);
 406     }
 407 };
 408
 409 /**
 410  * Force "small enough" float value to zero
 411  */
 412 inline void sanitize(float &value)
 413 {
 414     // real number?
 415     if (std::abs(value) < small_value<float>())
 416         value = 0.f;
 417     // close to 0?
 418     const int val = *reinterpret_cast <const int *> (&value);
 419     if ((val & 0x7F800000) == 0 && (val & 0x007FFFFF) != 0)
 420         value = 0.f;
 421 }
 422 inline float _sanitize(float value)
 423 {
 424     if (std::abs(value) < small_value<float>())
 425         return 0.f;
 426     return value;
 427 }
 428
 429 /**
 430  * Force already-denormal float value to zero
 431  */
 432 inline void sanitize_denormal(float& value)
 433 {
 434     if (!std::isnormal(value))
 435          value = 0.f;
 436 }
 437
 438 /**
 439  * Force already-denormal float value to zero
 440  */
 441 inline void sanitize_denormal(double & value)
 442 {
 443     if (!std::isnormal(value))
 444          value = 0.f;
 445 }
 446
 447 /**
 448  * Force "small enough" double value to zero
 449  */
 450 inline void sanitize(double &value)
 451 {
 452     if (std::abs(value) < small_value<double>())
 453         value = 0.0;
 454 }
 455
 456 inline double _sanitize(double value)
 457 {
 458     if (std::abs(value) < small_value<double>())
 459         return 0.0;
 460
 461     return value;
 462 }
 463 /**
 464  * Force "small enough" stereo value to zero
 465  */
 466 template<class T>
 467 inline void sanitize(stereo_sample<T> &value)
 468 {
 469     sanitize(value.left);
 470     sanitize(value.right);
 471 }
 472
 473 inline float fract16(unsigned int value)
 474 {
 475     return (value & 0xFFFF) * (1.0 / 65536.0);
 476 }
 477
 478 /**
 479  * typical precalculated sine table
 480  */
 481 template<class T, int N, int Multiplier>
 482 class sine_table
 483 {
 484 public:
 485     static bool initialized;
 486     static T data[N+1];
 487     sine_table() {
 488         if (initialized)
 489             return;
 490         initialized = true;
 491         for (int i=0; i<N+1; i++)
 492             data[i] = (T)(Multiplier*sin(i*2*M_PI*(1.0/N)));
 493     }
 494 };
 495
 496 template<class T, int N, int Multiplier>
 497 bool sine_table<T,N,Multiplier>::initialized = false;
 498
 499 template<class T, int N, int Multiplier>
 500 T sine_table<T,N,Multiplier>::data[N+1];
 501
 502 /// fast float to int conversion using default rounding mode
 503 inline int fastf2i_drm(float f)
 504 {
 505 #ifdef __X86__
 506     volatile int v;
 507     __asm ( "flds %1; fistpl %0" : "=m"(v) : "m"(f));
 508     return v;
 509 #else
 510     return (int)nearbyintf(f);
 511 #endif
 512 }
 513
 514 /// Convert MIDI note to frequency in Hz.
 515 inline float note_to_hz(double note, double detune_cents = 0.0)
 516 {
 517     return 440 * pow(2.0, (note - 69 + detune_cents/100.0) / 12.0);
 518 }
 519
 520 /// Hermite interpolation between two points and slopes in normalized range (written after Wikipedia article)
 521 /// @arg t normalized x coordinate (0-1 over the interval in question)
 522 /// @arg p0 first point
 523 /// @arg p1 second point
 524 /// @arg m0 first slope (multiply by interval width when using over non-1-wide interval)
 525 /// @arg m1 second slope (multiply by interval width when using over non-1-wide interval)
 526 inline float normalized_hermite(float t, float p0, float p1, float m0, float m1)
 527 {
 528     float t2 = t*t;
 529     float t3 = t2*t;
 530     return (2*t3 - 3*t2 + 1) * p0 + (t3 - 2*t2 + t) * m0 + (-2*t3 + 3*t2) * p1 + (t3-t2) * m1;
 531 }
 532
 533 /// Hermite interpolation between two points and slopes
 534 /// @arg x point within interval (x0 <= x <= x1)
 535 /// @arg x0 interval start
 536 /// @arg x1 interval end
 537 /// @arg p0 value at x0
 538 /// @arg p1 value at x1
 539 /// @arg m0 slope (steepness, tangent) at x0
 540 /// @arg m1 slope at x1
 541 inline float hermite_interpolation(float x, float x0, float x1, float p0, float p1, float m0, float m1)
 542 {
 543     float width = x1 - x0;
 544     float t = (x - x0) / width;
 545     m0 *= width;
 546     m1 *= width;
 547     float t2 = t*t;
 548     float t3 = t2*t;
 549
 550     float ct0 = p0;
 551     float ct1 = m0;
 552     float ct2 = -3 * p0 - 2 * m0 + 3 * p1 - m1;
 553     float ct3 = 2 * p0 + m0  - 2 * p1 + m1;
 554
 555     return ct3 * t3 + ct2 * t2 + ct1 * t + ct0;
 556     //return (2*t3 - 3*t2 + 1) * p0 + (t3 - 2*t2 + t) * m0 + (-2*t3 + 3*t2) * p1 + (t3-t2) * m1;
 557 }
 558
 559 /// convert amplitude value to dB
 560 inline float amp2dB(float amp)
 561 {
 562     return 20 * log10(amp);
 563 }
 564 /// convert dB to amplitude value
 565 inline float dB2amp(float db)
 566 {
 567     return exp((db / 20.0) * log(10.0));
 568 }
 569
 570 /// print binary of any data type
 571 /// assumes little endian
 572 inline void print_bits(size_t const size, void const * const ptr)
 573 {
 574     unsigned char *b = (unsigned char*) ptr;
 575     unsigned char byte;
 576     for (int i = size - 1; i >=0 ; i--) {
 577         for (int j = 7; j >= 0; j--) {
 578             byte = b[i] & (1<<j);
 579             byte >>= j;
 580             printf("%u", byte);
 581         }
 582     }
 583     puts("");
 584 }
 585
 586
 587 struct note_desc
 588 {
 589     int note;
 590     double cents;
 591     double freq;
 592     int octave;
 593     const char *name;
 594 };
 595
 596 inline note_desc hz_to_note (double hz, double tune)
 597 {
 598     note_desc desc;
 599     static const char notenames[] = "C\0\0C#\0D\0\0D#\0E\0\0F\0\0F#\0G\0\0G#\0A\0\0A#\0B\0\0";
 600     double f2 = hz / tune;
 601     double lf2 = logf(f2);
 602     double rf2 = 1200 * lf2 / logf(2.f) - 300;
 603     rf2 -= 1200.f * floor(rf2 / 1200.f);
 604     int note = round(rf2 / 100.f);
 605     rf2 -= note * 100;
 606     if (note == 12)
 607         note -= 12;
 608     int mnote = round(12 * log2(f2)) + 57;
 609     desc.freq   = hz;
 610     desc.note   = mnote;
 611     desc.cents  = rf2;
 612     desc.octave = int(mnote / 12) - 2;
 613     desc.name   = notenames + (mnote % 12) * 3;
 614     return desc;
 615 }
 616
 617 };
 618 #endif