1 /**
2 DSP utility functions. 
3 
4 Copyright: Guillaume Piolat 2015-2016.
5 License:   $(LINK2 http://www.boost.org/LICENSE_1_0.txt, Boost License 1.0)
6 */
7 module dplug.core.math;
8 
9 import std.math;
10 
11 version(LDC)
12 {
13     import ldc.intrinsics;
14 }
15 
16 immutable real TAU = PI * 2;
17 
18 /// Map linearly x from the range [a, b] to the range [c, d]
19 T linmap(T)(T value, T a, T b, T c, T d) pure nothrow @nogc
20 {
21     return c + (d - c) * (value - a) / (b - a);
22 }
23 
24 /// map [0..1] to [min..max] logarithmically
25 /// min and max must be all > 0, t in [0..1]
26 T logmap(T)(T t, T min, T max) pure nothrow @nogc
27 {
28     assert(min < max);
29     return min * exp(t * log(max / min));
30 }
31 
32 /// Hermite interpolation.
33 T hermite(T)(T frac_pos, T xm1, T x0, T x1, T x2) pure nothrow @nogc
34 {
35     T c = (x1 - xm1) * 0.5f;
36     T v = x0 - x1;
37     T w = c + v;
38     T a = w + v + (x2 - x0) * 0.5f;
39     T b_neg = w + a;
40     return ((((a * frac_pos) - b_neg) * frac_pos + c) * frac_pos + x0);
41 }
42 
43 /// Convert from dB to float.
44 /// Precision: This uses fast_exp which under normal conditions has a peak error under -135dB over the useful range.
45 T convertDecibelToLinearGain(T)(T dB) pure nothrow @nogc
46 {
47     static immutable T ln10_20 = cast(T)LN10 / 20;
48     return fast_exp(dB * ln10_20);
49 }
50 
51 /// Convert from float to dB
52 /// Precision: This uses fast_exp which under normal conditions has a peak error under -135dB over the useful range.
53 T convertLinearGainToDecibel(T)(T x) pure nothrow @nogc
54 {
55     static immutable T f20_ln10 = 20 / cast(T)LN10;
56     return fast_log(x) * f20_ln10;
57 }
58 
59 /// Is this integer odd?
60 bool isOdd(T)(T i) pure nothrow @nogc
61 {
62     return (i & 1) != 0;
63 }
64 
65 /// Is this integer even?
66 bool isEven(T)(T i) pure nothrow @nogc
67 {
68     return (i & 1) == 0;
69 }
70 
71 /// Returns: true of i is a power of 2.
72 bool isPowerOfTwo(int i) pure nothrow @nogc
73 {
74     assert(i >= 0);
75     return (i != 0) && ((i & (i - 1)) == 0);
76 }
77 
78 
79 /// Computes next power of 2.
80 /// Returns: N so that N is a power of 2 and N >= i.
81 /// Note: This function is not equivalent to the builtin `std.math.nextPow2` when the input is a power of 2.
82 int nextPow2HigherOrEqual(int i) pure nothrow @nogc
83 {
84     int v = i - 1;
85     v |= v >> 1;
86     v |= v >> 2;
87     v |= v >> 4;
88     v |= v >> 8;
89     v |= v >> 16;
90     v++;
91     return v;
92 }
93 
94 ///ditto
95 long nextPow2HigherOrEqual(long i) pure nothrow @nogc
96 {
97     long v = i - 1;
98     v |= v >> 1;
99     v |= v >> 2;
100     v |= v >> 4;
101     v |= v >> 8;
102     v |= v >> 16;
103     v |= v >> 32;
104     v++;
105     return v;
106 }
107 
108 /// Returns: x so that (1 << x) >= i
109 int iFloorLog2(int i) pure nothrow @nogc
110 {
111     assert(i >= 1);
112     int result = 0;
113     while (i > 1)
114     {
115         i = i / 2;
116         result = result + 1;
117     }
118     return result;
119 }
120 
121 /// Mapping from MIDI notes to frequency.
122 float convertMIDINoteToFrequency(float note) pure nothrow @nogc
123 {
124     return 440.0f * pow(2.0, (note - 69.0f) / 12.0f);
125 }
126 
127 /// Mapping from frequency to MIDI notes.
128 float convertFrequencyToMIDINote(float frequency) pure nothrow @nogc
129 {
130     return 69.0f + 12.0f * log2(frequency / 440.0f);
131 }
132 
133 /// Fletcher and Munson equal-loudness curve
134 /// Reference: Xavier Serra thesis (1989).
135 T equalLoudnessCurve(T)(T frequency) pure nothrow @nogc
136 {
137     T x = cast(T)0.05 + 4000 / frequency;
138     return x * ( cast(T)10 ^^ x);
139 }
140 
141 /// Cardinal sine
142 T sinc(T)(T x) pure nothrow @nogc
143 {
144     if (cast(T)(1) + x * x == cast(T)(1))
145         return 1;
146     else
147         return sin(cast(T)PI * x) / (cast(T)PI * x);
148 }
149 
150 /// Gets a factor for making exponential decay curves, which are the same thing
151 /// as a 6dB/oct lowpass filter.
152 ///
153 /// Returns: Multiplier for this RC time constant and sampling rate.
154 ///
155 /// Params:
156 ///    timeConstantInSeconds = Time after which the amplitude is only 37% of the original.
157 ///    samplerate = Sampling rate.
158 ///
159 /// Note: Using `fast_exp` yield a decay-factor within -180 dB (at 384000hz) of the one obtained with `expm1`.
160 ///       The alleged "innacuracies" of plain exp just did not show up so we don't prefer `expm1` anymore.
161 ///       It doesn't change the length of an iterated envelope like `ExpSmoother`.
162 ///
163 double expDecayFactor(double timeConstantInSeconds, double samplerate) pure nothrow @nogc
164 {
165     return 1.0 - fast_exp(-1.0 / (timeConstantInSeconds * samplerate));
166 }
167 
168 /// Give back a phase between -PI and PI
169 T normalizePhase(T)(T phase) nothrow @nogc
170 {
171     enum bool Assembly = D_InlineAsm_Any && !(is(Unqual!T == real));
172 
173     static if (Assembly)
174     {
175         T k_TAU = PI * 2;
176         T result = phase;
177         asm nothrow @nogc
178         {
179             fld k_TAU;    // TAU
180             fld result;    // phase | TAU
181             fprem1;       // normalized(phase) | TAU
182             fstp result;   // TAU
183             fstp ST(0);   //
184         }
185         return result;
186     }
187     else
188     {
189         T res = fmod(phase, cast(T)TAU);
190         if (res > PI)
191             res -= TAU;
192         if (res < -PI)
193             res += TAU;
194         return res;
195     }
196 }
197 
198 unittest
199 {
200     assert(approxEqual(normalizePhase!real(TAU), 0));
201 
202     assert(approxEqual(normalizePhase!float(0.1f), 0.1f));
203     assert(approxEqual(normalizePhase!float(TAU + 0.1f), 0.1f));
204 
205     assert(approxEqual(normalizePhase!double(-0.1f), -0.1f));
206     assert(approxEqual(normalizePhase!double(-TAU - 0.1f), -0.1f));
207 
208     bool approxEqual(T)(T a, T b) nothrow @nogc
209     {
210         return (a - b) < 1e-7;
211     }
212 }
213 
214 /// Quick and dirty sawtooth for testing purposes.
215 T rawSawtooth(T)(T x) pure nothrow @nogc
216 {
217     return normalizePhase(x) / (cast(T)PI);
218 }
219 
220 /// Quick and dirty triangle for testing purposes.
221 T rawTriangle(T)(T x) pure nothrow @nogc
222 {
223     return 1 - normalizePhase(x) / cast(T)PI_2;
224 }
225 
226 /// Quick and dirty square for testing purposes.
227 T rawSquare(T)(T x) pure nothrow @nogc
228 {
229     return normalizePhase(x) > 0 ? 1 : -1;
230 }
231 
232 T computeRMS(T)(T[] samples) pure nothrow @nogc
233 {
234     double sum = 0;
235     foreach(sample; samples)
236         sum += sample * sample;
237     return sqrt(sum / cast(int)samples.length);
238 }
239 
240 unittest
241 {
242     double[] d = [4, 5, 6];
243     computeRMS(d);
244 }
245 
246 
247 version(D_InlineAsm_X86)
248     private enum D_InlineAsm_Any = true;
249 else version(D_InlineAsm_X86_64)
250     private enum D_InlineAsm_Any = true;
251 else
252     private enum D_InlineAsm_Any = false;
253 
254 // Expose LDC intrinsics, but usable with DMD too.
255 
256 version(LDC)
257 {
258     // Note: wrapper functions wouldn't work (depend on inlining), 
259     //       it's much more reliable to use alias for speed gain.
260 
261     // Gives considerable speed improvement over `std.math.exp`.
262     // Exhaustive testing for 32-bit `float` shows
263     // Relative accuracy is within < 0.0002% of std.math.exp
264     // for every possible input.
265     // So a -120 dB inaccuracy, and -140dB the vast majority of the time.
266     alias fast_exp = llvm_exp; 
267 
268 
269     alias fast_log = llvm_log;
270 
271     // Note: fast_pow with a float argument (`powf`) can be a lot faster that with a double argument.
272     alias fast_pow = llvm_pow;
273 
274     // Gives measurable speed improvement at audio-rate, without change for any input.
275     alias fast_fabs = llvm_fabs;
276 
277 
278     alias fast_log2 = llvm_log2;
279     alias fast_exp2 = llvm_exp2;
280     alias fast_log10 = llvm_log10;
281 
282     alias fast_floor = llvm_floor;
283     alias fast_ceil = llvm_ceil;
284     alias fast_trunc = llvm_trunc;
285     alias fast_round = llvm_round;
286 
287     alias fast_sqrt = llvm_sqrt;
288     alias fast_sin = llvm_sin;
289     alias fast_cos = llvm_cos; // no speed change seen when using it
290 }
291 else
292 {
293     alias fast_exp = exp;
294     alias fast_log = log;
295     alias fast_pow = pow;
296 
297     alias fast_fabs = fabs;
298     alias fast_log2 = log2;
299     alias fast_êxp2 = exp2;
300     alias fast_log10 = log10;
301 
302     alias fast_floor = floor;
303     alias fast_ceil = ceil;
304     alias fast_trunc = trunc;
305     alias fast_round = round;
306 
307     alias fast_sqrt = sqrt; // Undefined behaviour for operands below -0
308     alias fast_sin = sin;
309     alias fast_cos = cos;
310 }
311 
312 /// Linear interpolation, akin to GLSL's mix.
313 S lerp(S, T)(S a, S b, T t) pure nothrow @nogc 
314     if (is(typeof(t * b + (1 - t) * a) : S))
315 {
316     return t * b + (1 - t) * a;
317 }
318 
319 /// Same as GLSL smoothstep function.
320 /// See: http://en.wikipedia.org/wiki/Smoothstep
321 T smoothStep(T)(T a, T b, T t) pure nothrow @nogc 
322 {
323     if (t <= a)
324         return 0;
325     else if (t >= b)
326         return 1;
327     else
328     {
329         T x = (t - a) / (b - a);
330         return x * x * (3 - 2 * x);
331     }
332 }
333 
334 /// SSE approximation of reciprocal square root.
335 T inverseSqrt(T)(T x) pure nothrow @nogc if (is(T : float) || is(T: double))
336 {
337     version(AsmX86)
338     {
339         static if (is(T == float))
340         {
341             float result;
342 
343             asm pure nothrow @nogc 
344             {
345                 movss XMM0, x; 
346                 rsqrtss XMM0, XMM0; 
347                 movss result, XMM0; 
348             }
349             return result;
350         }
351         else
352             return 1 / sqrt(x);
353     }
354     else
355         return 1 / sqrt(x);
356 }
357 
358 unittest
359 {
360     assert(abs( inverseSqrt!float(1) - 1) < 1e-3 );
361     assert(abs( inverseSqrt!double(1) - 1) < 1e-3 );
362 }
363 
364 /// Computes a normalized frequency form a frequency.
365 float convertFrequencyToNormalizedFrequency(float frequencyHz, float samplingRate) pure nothrow @nogc
366 {
367     return frequencyHz / samplingRate;
368 }
369 
370 /// Computes a frequency.
371 float convertNormalizedFrequencyToFrequency(float frequencyCyclesPerSample, float samplingRate) pure nothrow @nogc
372 {
373     return frequencyCyclesPerSample * samplingRate;
374 }
375 
376 
377 
378 version(D_InlineAsm_X86)
379     version = hasX86Asm;
380 else version(D_InlineAsm_X86_64)
381     version = hasX86Asm;
382 else
383     static assert(false, "Unable to implement Real80 on this architecture");
384 
385 /// `Real80` is a scalar numerical type similar to `real` when using x86 DMD using FPU.
386 /// This ensures 80-bit computation, when `double` isn't enough and `real` is a 64-bit
387 /// floating-point number.
388 /// This can happen in pole/zero computations for some IIR filters.
389 // TODO: bug with LDC(???), it seems impossible to have a FPU which works with 80-bit precision!
390 //       but works as expected in DMD.
391 struct Real80
392 {
393 public:
394 nothrow:
395 @nogc:
396 
397     /// Builds a `Real80` from an integer type.
398     this(long x)
399     {
400         void* p = valuePtr();
401         version(D_InlineAsm_X86)
402         {            
403             asm nothrow @nogc
404             {
405                 fild x;
406                 mov EAX, p;
407                 fstp real ptr [EAX];
408             }
409         }
410         else version(D_InlineAsm_X86_64)
411         {
412             asm nothrow @nogc
413             {
414                 fild x;
415                 mov RAX, p;
416                 fstp real ptr [RAX];
417             }
418         }
419         else
420             static assert(false);
421     }
422 
423     /// Builds a `Real80` from a floating-point type that may be any number of bits.
424     this(double number)
425     {
426         void* p = valuePtr();
427         version(D_InlineAsm_X86)
428         {            
429             asm nothrow @nogc
430             {
431                 fld number;
432                 mov EAX, p;
433                 fstp real ptr [EAX];
434             }
435         }
436         else version(D_InlineAsm_X86_64)
437         {
438             asm nothrow @nogc
439             {
440                 fld number;
441                 mov RAX, p;
442                 fstp real ptr [RAX];
443             }
444         }
445         else
446             static assert(false);
447     }
448 
449     Real80 opAssign(Real80 number)
450     {
451         this._value = number._value;
452         return this;
453     }
454 
455     /// Casts to `double`.
456     double toDouble()
457     {
458         void* p = valuePtr();
459         double result;
460         version(D_InlineAsm_X86)
461         {
462             asm nothrow @nogc
463             {
464                 mov EAX, p;
465                 fld real ptr [EAX];
466                 fstp result;
467             }
468         }
469         else version(D_InlineAsm_X86_64)
470         {
471             asm nothrow @nogc
472             {
473                 mov RAX, p;
474                 fld real ptr [RAX];
475                 fstp result;
476             }
477         }
478         else
479             static assert(false);
480         return result;
481     }
482 
483     /// Casts to `double`.
484     double opCast(T)() if (is(T == double))
485     {
486         return toDouble();
487     }
488 
489     /// Computes this * other
490     Real80 opUnary(string op)() if (op == "-")
491     {
492         version(LittleEndian)
493         {
494             Real80 result = this;
495             result. _value[0] ^= 128; // invert sign bit
496             return result;
497         }
498         else
499             static assert(false);
500     }
501 
502     /// Promotes double and float operands to Real80 first
503     Real80 opBinary(string op)(real other)
504     {
505         mixin("return this " ~ op ~ "Real80(other);");
506     }
507 
508     /// Multiplication of two `Real80` number.
509     Real80 opBinary(string op)(Real80 other) if (op == "*")
510     {
511         void* pA = this.valuePtr();
512         void* pB = other.valuePtr();
513         Real80 result;
514         void* pR = result.valuePtr();
515         version(D_InlineAsm_X86)
516         {
517             asm nothrow @nogc
518             {
519                 mov EAX, pA;
520                 mov ECX, pB;
521                 mov EDX, pR;
522                 fld real ptr [EAX];
523                 fld real ptr [ECX];
524                 fmulp;
525                 fstp real ptr [EDX];
526             }
527         }
528         else version(D_InlineAsm_X86_64)
529         {
530             asm nothrow @nogc
531             {
532                 mov RAX, pA;
533                 mov RCX, pB;
534                 mov RDX, pR;
535                 fld real ptr [RAX];
536                 fld real ptr [RCX];
537                 fmulp;
538                 fstp real ptr [RDX];
539             }
540         }
541         else
542             static assert(false);
543         return result;
544     }
545 
546     /// Addition of two `Real80` number.
547     Real80 opBinary(string op)(Real80 other) if (op == "+")
548     {
549         void* pA = this.valuePtr();
550         void* pB = other.valuePtr();
551         Real80 result;
552         void* pR = result.valuePtr();
553         version(D_InlineAsm_X86)
554         {
555             asm nothrow @nogc
556             {                
557                 mov EAX, pA;
558                 mov ECX, pB;
559                 mov EDX, pR;
560                 fld real ptr [EAX];
561                 fld real ptr [ECX];
562                 faddp;
563                 fstp real ptr [EDX];
564             }
565         }
566         else version(D_InlineAsm_X86_64)
567         {
568             asm nothrow @nogc
569             {
570                 mov RAX, pA;
571                 mov RCX, pB;
572                 mov RDX, pR;
573                 fld real ptr [RAX];
574                 fld real ptr [RCX];
575                 faddp;
576                 fstp real ptr [RDX];
577                 
578             }
579         }
580         else
581             static assert(false);
582         return result;
583     }
584 
585     /// Division of two `Real80` number. Computes this / other.
586     Real80 opBinary(string op)(Real80 other) if (op == "/")
587     {
588         void* pA = this.valuePtr();
589         void* pB = other.valuePtr();
590         Real80 result;
591         void* pR = result.valuePtr();
592         version(D_InlineAsm_X86)
593         {
594             asm nothrow @nogc
595             {
596 
597                 mov EAX, pA;
598                 mov ECX, pB;
599                 mov EDX, pR;
600                 fld real ptr [EAX];
601                 fld real ptr [ECX];
602                 fdivp;
603                 fstp real ptr [EDX];
604             }
605         }
606         else version(D_InlineAsm_X86_64)
607         {
608             asm nothrow @nogc
609             {
610                 mov RAX, pA;
611                 mov RCX, pB;
612                 mov RDX, pR;
613                 fld real ptr [RAX];
614                 fld real ptr [RCX];
615                 fdivp;
616                 fstp real ptr [RDX];
617 
618             }
619         }
620         else
621             static assert(false);
622         return result;
623     }
624 
625     /// Computes this + other
626     Real80 opBinary(string op)(Real80 other) if (op == "-")
627     {
628         return this + (-other);
629     }
630 
631 private:
632     void* valuePtr()
633     {
634         return _value.ptr;
635     }
636 
637     ubyte[10] _value = 
638     [
639         0, 0, 0, 0, 0, 0, 0, 0xa0, 0xff, 0x7f
640     ];
641 }
642 
643 unittest
644 {
645     import dplug.core.fpcontrol;
646     FPControl fpControl;
647     fpControl.initialize();
648 
649     // take a number with a full mantissa
650     double target = cast(double)PI;
651 
652     Real80 x = target;
653     double y = target;
654     Real80 orig = x;
655     
656     assert(cast(double)x == target);
657     
658     //import std.stdio;
659     for(int i = 0; i < 1020; ++i) // TODO: augment this to 4000
660     {
661         x = x / 2;
662         y = y / 2;
663       //  if (i >= 1020)
664       //  writeln("Dec ", x._value, " vs ", orig._value);
665     }
666     for(int i = 0; i < 1020; ++i)  // TODO: augment this to 4000
667     {
668         x = x * 2;
669         y = y * 2;
670     //    if (i <= 10)
671     //    writeln("Inc ", x._value, " vs ", orig._value);
672     }
673 
674 
675 //    writeln(x._value, " vs ", orig._value);
676 
677     // Should not have lost any precision though these multiplication since the exponent handles it.
678     assert(x._value == orig._value);
679     assert(target == cast(double)x); // x has maintained the full mantissa
680 
681   //  assert(target != y);             // y can not possibly maintain precision
682 }