1 | /*****************************************************************************\ |
2 | Snes9x - Portable Super Nintendo Entertainment System (TM) emulator. |
3 | This file is licensed under the Snes9x License. |
4 | For further information, consult the LICENSE file in the root directory. |
5 | \*****************************************************************************/ |
6 | |
7 | #ifndef __NEW_RESAMPLER_H |
8 | #define __NEW_RESAMPLER_H |
9 | |
10 | #include <cstring> |
11 | #include <cassert> |
12 | #if __cplusplus >= 201103L |
13 | #include <cstdint> |
14 | #else |
15 | #include <stdint.h> |
16 | #endif |
17 | #include <cmath> |
18 | |
19 | class Resampler |
20 | { |
21 | public: |
22 | int size; |
23 | int buffer_size; |
24 | int start; |
25 | int16_t *buffer; |
26 | |
27 | float r_step; |
28 | float r_frac; |
29 | int r_left[4], r_right[4]; |
30 | |
31 | static inline int16_t short_clamp(int n) |
32 | { |
33 | return (int16_t)(((int16_t)n != n) ? (n >> 31) ^ 0x7fff : n); |
34 | } |
35 | |
36 | static inline int min(int a, int b) |
37 | { |
38 | return ((a) < (b) ? (a) : (b)); |
39 | } |
40 | |
41 | static inline float hermite(float mu1, float a, float b, float c, float d) |
42 | { |
43 | float mu2, mu3, m0, m1, a0, a1, a2, a3; |
44 | |
45 | mu2 = mu1 * mu1; |
46 | mu3 = mu2 * mu1; |
47 | |
48 | m0 = (c - a) * 0.5; |
49 | m1 = (d - b) * 0.5; |
50 | |
51 | a0 = +2 * mu3 - 3 * mu2 + 1; |
52 | a1 = mu3 - 2 * mu2 + mu1; |
53 | a2 = mu3 - mu2; |
54 | a3 = -2 * mu3 + 3 * mu2; |
55 | |
56 | return (a0 * b) + (a1 * m0) + (a2 * m1) + (a3 * c); |
57 | } |
58 | |
59 | Resampler() |
60 | { |
61 | this->buffer_size = 0; |
62 | buffer = NULL; |
63 | r_step = 1.0; |
64 | } |
65 | |
66 | Resampler(int num_samples) |
67 | { |
68 | this->buffer_size = num_samples; |
69 | buffer = new int16_t[this->buffer_size]; |
70 | r_step = 1.0; |
71 | clear(); |
72 | } |
73 | |
74 | ~Resampler() |
75 | { |
76 | delete[] buffer; |
77 | buffer = NULL; |
78 | } |
79 | |
80 | inline void time_ratio(double ratio) |
81 | { |
82 | r_step = ratio; |
83 | } |
84 | |
85 | inline void clear(void) |
86 | { |
87 | if (!buffer) |
88 | return; |
89 | |
90 | start = 0; |
91 | size = 0; |
92 | memset(buffer, 0, buffer_size * 2); |
93 | |
94 | r_frac = 0.0; |
95 | r_left[0] = r_left[1] = r_left[2] = r_left[3] = 0; |
96 | r_right[0] = r_right[1] = r_right[2] = r_right[3] = 0; |
97 | } |
98 | |
99 | inline bool pull(int16_t *dst, int num_samples) |
100 | { |
101 | if (space_filled() < num_samples) |
102 | return false; |
103 | |
104 | memcpy(dst, buffer + start, min(num_samples, buffer_size - start) * 2); |
105 | |
106 | if (num_samples > (buffer_size - start)) |
107 | memcpy(dst + (buffer_size - start), buffer, (num_samples - (buffer_size - start)) * 2); |
108 | |
109 | start = (start + num_samples) % buffer_size; |
110 | size -= num_samples; |
111 | |
112 | return true; |
113 | } |
114 | |
115 | inline void push_sample(int16_t l, int16_t r) |
116 | { |
117 | if (space_empty() >= 2) |
118 | { |
119 | int end = start + size; |
120 | if (end >= buffer_size) |
121 | end -= buffer_size; |
122 | buffer[end] = l; |
123 | buffer[end + 1] = r; |
124 | size += 2; |
125 | } |
126 | } |
127 | |
128 | inline bool push(int16_t *src, int num_samples) |
129 | { |
130 | if (space_empty() < num_samples) |
131 | return false; |
132 | |
133 | int end = start + size; |
134 | if (end > buffer_size) |
135 | end -= buffer_size; |
136 | int first_write_size = min(num_samples, buffer_size - end); |
137 | |
138 | memcpy(buffer + end, src, first_write_size * 2); |
139 | |
140 | if (num_samples > first_write_size) |
141 | memcpy(buffer, src + first_write_size, (num_samples - first_write_size) * 2); |
142 | |
143 | size += num_samples; |
144 | |
145 | return true; |
146 | } |
147 | |
148 | void read(int16_t *data, int num_samples) |
149 | { |
150 | //If we are outputting the exact same ratio as the input, pull directly from the input buffer |
151 | if (r_step == 1.0) |
152 | { |
153 | pull(data, num_samples); |
154 | return; |
155 | } |
156 | |
157 | assert((num_samples & 1) == 0); // resampler always processes both stereo samples |
158 | int o_position = 0; |
159 | |
160 | while (o_position < num_samples && size > 0) |
161 | { |
162 | int s_left = buffer[start]; |
163 | int s_right = buffer[start + 1]; |
164 | int hermite_val[2]; |
165 | |
166 | while (r_frac <= 1.0 && o_position < num_samples) |
167 | { |
168 | hermite_val[0] = (int)hermite(r_frac, (float)r_left[0], (float)r_left[1], (float)r_left[2], (float)r_left[3]); |
169 | hermite_val[1] = (int)hermite(r_frac, (float)r_right[0], (float)r_right[1], (float)r_right[2], (float)r_right[3]); |
170 | data[o_position] = short_clamp(hermite_val[0]); |
171 | data[o_position + 1] = short_clamp(hermite_val[1]); |
172 | |
173 | o_position += 2; |
174 | |
175 | r_frac += r_step; |
176 | } |
177 | |
178 | if (r_frac > 1.0) |
179 | { |
180 | r_left[0] = r_left[1]; |
181 | r_left[1] = r_left[2]; |
182 | r_left[2] = r_left[3]; |
183 | r_left[3] = s_left; |
184 | |
185 | r_right[0] = r_right[1]; |
186 | r_right[1] = r_right[2]; |
187 | r_right[2] = r_right[3]; |
188 | r_right[3] = s_right; |
189 | |
190 | r_frac -= 1.0; |
191 | |
192 | start += 2; |
193 | if (start >= buffer_size) |
194 | start -= buffer_size; |
195 | size -= 2; |
196 | } |
197 | } |
198 | } |
199 | |
200 | inline int space_empty(void) const |
201 | { |
202 | return buffer_size - size; |
203 | } |
204 | |
205 | inline int space_filled(void) const |
206 | { |
207 | return size; |
208 | } |
209 | |
210 | inline int avail(void) |
211 | { |
212 | //If we are outputting the exact same ratio as the input, find out directly from the input buffer |
213 | if (r_step == 1.0) |
214 | return size; |
215 | |
216 | return (int)trunc(((size >> 1) - r_frac) / r_step) * 2; |
217 | } |
218 | |
219 | void resize(int num_samples) |
220 | { |
221 | if (buffer) |
222 | delete[] buffer; |
223 | buffer_size = num_samples; |
224 | buffer = new int16_t[buffer_size]; |
225 | clear(); |
226 | } |
227 | }; |
228 | |
229 | #endif /* __NEW_RESAMPLER_H */ |