1 // Copyright 2012 Olivier Gillet.
2 //
3 // Author: Olivier Gillet (ol.gillet@gmail.com)
4 //
5 // Permission is hereby granted, free of charge, to any person obtaining a copy
6 // of this software and associated documentation files (the "Software"), to deal
7 // in the Software without restriction, including without limitation the rights
8 // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
9 // copies of the Software, and to permit persons to whom the Software is
10 // furnished to do so, subject to the following conditions:
11 //
12 // The above copyright notice and this permission notice shall be included in
13 // all copies or substantial portions of the Software.
14 //
15 // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
16 // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
17 // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
18 // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
19 // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
20 // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
21 // THE SOFTWARE.
22 //
23 // See http://creativecommons.org/licenses/MIT/ for more information.
24 //
25 // -----------------------------------------------------------------------------
26 //
27 // DSP utility routines.
28
29 #ifndef STMLIB_UTILS_DSP_DSP_H_
30 #define STMLIB_UTILS_DSP_DSP_H_
31
32 #include "stmlib/stmlib.h"
33
34 #include <cmath>
35 #include <math.h>
36
37 namespace stmlib {
38
39 #define MAKE_INTEGRAL_FRACTIONAL(x) \
40 int32_t x ## _integral = static_cast<int32_t>(x); \
41 float x ## _fractional = x - static_cast<float>(x ## _integral);
42
Interpolate(const float * table,float index,float size)43 inline float Interpolate(const float* table, float index, float size) {
44 index *= size;
45 MAKE_INTEGRAL_FRACTIONAL(index)
46 float a = table[index_integral];
47 float b = table[index_integral + 1];
48 return a + (b - a) * index_fractional;
49 }
50
InterpolateWrap(const float * table,float index,float size)51 inline float InterpolateWrap(const float* table, float index, float size) {
52 index -= static_cast<float>(static_cast<int32_t>(index));
53 index *= size;
54 MAKE_INTEGRAL_FRACTIONAL(index)
55 float a = table[index_integral];
56 float b = table[index_integral + 1];
57 return a + (b - a) * index_fractional;
58 }
59
60 #define ONE_POLE(out, in, coefficient) out += (coefficient) * ((in) - out);
61 #define SLOPE(out, in, positive, negative) { \
62 float error = (in) - out; \
63 out += (error > 0 ? positive : negative) * error; \
64 }
65 #define SLEW(out, in, delta) { \
66 float error = (in) - out; \
67 float d = (delta); \
68 if (error > d) { \
69 error = d; \
70 } else if (error < -d) { \
71 error = -d; \
72 } \
73 out += error; \
74 }
75
Crossfade(float a,float b,float fade)76 inline float Crossfade(float a, float b, float fade) {
77 return a + (b - a) * fade;
78 }
79
SoftLimit(float x)80 inline float SoftLimit(float x) {
81 return x * (27.0f + x * x) / (27.0f + 9.0f * x * x);
82 }
83
SoftClip(float x)84 inline float SoftClip(float x) {
85 if (x < -3.0f) {
86 return -1.0f;
87 } else if (x > 3.0f) {
88 return 1.0f;
89 } else {
90 return SoftLimit(x);
91 }
92 }
93
94 #ifdef TEST
Clip16(int32_t x)95 inline int32_t Clip16(int32_t x) {
96 if (x < -32768) {
97 return -32768;
98 } else if (x > 32767) {
99 return 32767;
100 } else {
101 return x;
102 }
103 }
ClipU16(int32_t x)104 inline uint16_t ClipU16(int32_t x) {
105 if (x < 0) {
106 return 0;
107 } else if (x > 65535) {
108 return 65535;
109 } else {
110 return x;
111 }
112 }
113 #else
Clip16(int32_t x)114 inline int32_t Clip16(int32_t x) {
115 int32_t result;
116 __asm ("ssat %0, %1, %2" : "=r" (result) : "I" (16), "r" (x) );
117 return result;
118 }
ClipU16(int32_t x)119 inline uint32_t ClipU16(int32_t x) {
120 uint32_t result;
121 __asm ("usat %0, %1, %2" : "=r" (result) : "I" (16), "r" (x) );
122 return result;
123 }
124 #endif
125
126 #ifdef TEST
Sqrt(float x)127 inline float Sqrt(float x) {
128 return sqrtf(x);
129 }
130 #else
Sqrt(float x)131 inline float Sqrt(float x) {
132 float result;
133 __asm ("vsqrt.f32 %0, %1" : "=w" (result) : "w" (x) );
134 return result;
135 }
136 #endif
137
SoftConvert(float x)138 inline int16_t SoftConvert(float x) {
139 return Clip16(static_cast<int32_t>(SoftLimit(x * 0.5f) * 32768.0f));
140 }
141
142 } // namespace stmlib
143
144 #endif // STMLIB_UTILS_DSP_DSP_H_