0f7a038c88e43e6ec9dd7ea3b9abc28460dd0c06
[blender.git] / intern / audaspace / intern / AUD_JOSResampleReader.cpp
1 /*
2  * ***** BEGIN GPL LICENSE BLOCK *****
3  *
4  * Copyright 2009-2011 Jörg Hermann Müller
5  *
6  * This file is part of AudaSpace.
7  *
8  * Audaspace is free software; you can redistribute it and/or modify
9  * it under the terms of the GNU General Public License as published by
10  * the Free Software Foundation; either version 2 of the License, or
11  * (at your option) any later version.
12  *
13  * AudaSpace is distributed in the hope that it will be useful,
14  * but WITHOUT ANY WARRANTY; without even the implied warranty of
15  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
16  * GNU General Public License for more details.
17  *
18  * You should have received a copy of the GNU General Public License
19  * along with Audaspace; if not, write to the Free Software Foundation,
20  * Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
21  *
22  * ***** END GPL LICENSE BLOCK *****
23  */
24
25 /** \file audaspace/intern/AUD_JOSResampleReader.cpp
26  *  \ingroup audaspaceintern
27  */
28
29 #include "AUD_JOSResampleReader.h"
30
31 #include "AUD_JOSResampleReaderCoeff.cpp"
32
33 #include <cmath>
34 #include <cstring>
35 #include <iostream>
36
37 /* MSVC does not have lrint */
38 #ifdef _MSC_VER
39 #ifdef _M_X64
40 #include <emmintrin.h>
41 static inline int lrint(double d)
42 {
43                 return _mm_cvtsd_si32(_mm_load_sd(&d));
44 }
45 #else
46 static inline int lrint(double d)
47 {
48         int i;
49
50         _asm{
51                 fld d
52                 fistp i
53         };
54
55         return i;
56 }
57 #endif
58 #endif
59
60 #define CC m_channels + channel
61
62 #define AUD_RATE_MAX 256
63 #define SHIFT_BITS 12
64 #define double_to_fp(x) (lrint(x * double(1 << SHIFT_BITS)))
65 #define int_to_fp(x) (x << SHIFT_BITS)
66 #define fp_to_int(x) (x >> SHIFT_BITS)
67 #define fp_to_double(x) (x * 1.0/(1 << SHIFT_BITS))
68 #define fp_rest(x) (x & ((1 << SHIFT_BITS) - 1))
69 #define fp_rest_to_double(x) fp_to_double(fp_rest(x))
70
71 AUD_JOSResampleReader::AUD_JOSResampleReader(AUD_Reference<AUD_IReader> reader, AUD_Specs specs) :
72         AUD_ResampleReader(reader, specs.rate),
73         m_channels(AUD_CHANNELS_INVALID),
74         m_n(0),
75         m_P(0),
76         m_cache_valid(0),
77         m_last_factor(0)
78 {
79 }
80
81 void AUD_JOSResampleReader::reset()
82 {
83         m_cache_valid = 0;
84         m_n = 0;
85         m_P = 0;
86         m_last_factor = 0;
87 }
88
89 void AUD_JOSResampleReader::updateBuffer(int size, double factor, int samplesize)
90 {
91         unsigned int len;
92         double num_samples = double(m_len) / double(m_L);
93         // first calculate what length we need right now
94         if(factor >= 1)
95                 len = ceil(num_samples);
96         else
97                 len = (unsigned int)(ceil(num_samples / factor));
98
99         // then check if afterwards the length is enough for the maximum rate
100         if(len + size < num_samples * AUD_RATE_MAX)
101                 len = num_samples * AUD_RATE_MAX - size;
102
103         if(m_n > len)
104         {
105                 sample_t* buf = m_buffer.getBuffer();
106                 len = m_n - len;
107                 memmove(buf, buf + len * m_channels, (m_cache_valid - len) * samplesize);
108                 m_n -= len;
109                 m_cache_valid -= len;
110         }
111
112         m_buffer.assureSize((m_cache_valid + size) * samplesize, true);
113 }
114
115 #define RESAMPLE_METHOD(name, left, right) void AUD_JOSResampleReader::name(double target_factor, int length, sample_t* buffer)\
116 {\
117         sample_t* buf = m_buffer.getBuffer();\
118 \
119         unsigned int P, l;\
120         int end, channel, i;\
121         double eta, v, f_increment, factor;\
122 \
123         m_sums.assureSize(m_channels * sizeof(double));\
124         double* sums = reinterpret_cast<double*>(m_sums.getBuffer());\
125         sample_t* data;\
126         const float* coeff = m_coeff;\
127 \
128         unsigned int P_increment;\
129 \
130         for(unsigned int t = 0; t < length; t++)\
131         {\
132                 factor = (m_last_factor * (length - t - 1) + target_factor * (t + 1)) / length;\
133 \
134                 memset(sums, 0, sizeof(double) * m_channels);\
135 \
136                 if(factor >= 1)\
137                 {\
138                         P = double_to_fp(m_P * m_L);\
139 \
140                         end = floor(m_len / double(m_L) - m_P) - 1;\
141                         if(m_n < end)\
142                                 end = m_n;\
143 \
144                         data = buf + (m_n - end) * m_channels;\
145                         l = fp_to_int(P);\
146                         eta = fp_rest_to_double(P);\
147                         l += m_L * end;\
148 \
149                         for(i = 0; i <= end; i++)\
150                         {\
151                                 v = coeff[l] + eta * (coeff[l+1] - coeff[l]);\
152                                 l -= m_L;\
153                                 left\
154                         }\
155 \
156                         P = int_to_fp(m_L) - P;\
157 \
158                         end = floor((m_len - 1) / double(m_L) + m_P) - 1;\
159                         if(m_cache_valid - m_n - 2 < end)\
160                                 end = m_cache_valid - m_n - 2;\
161 \
162                         data = buf + (m_n + 2 + end) * m_channels - 1;\
163                         l = fp_to_int(P);\
164                         eta = fp_rest_to_double(P);\
165                         l += m_L * end;\
166 \
167                         for(i = 0; i <= end; i++)\
168                         {\
169                                 v = coeff[l] + eta * (coeff[l+1] - coeff[l]);\
170                                 l -= m_L;\
171                                 right\
172                         }\
173 \
174                         for(channel = 0; channel < m_channels; channel++)\
175                         {\
176                                 *buffer = sums[channel];\
177                                 buffer++;\
178                         }\
179                 }\
180                 else\
181                 {\
182                         f_increment = factor * m_L;\
183                         P_increment = double_to_fp(f_increment);\
184                         P = double_to_fp(m_P * f_increment);\
185 \
186                         end = (int_to_fp(m_len) - P) / P_increment - 1;\
187                         if(m_n < end)\
188                                 end = m_n;\
189 \
190                         P += P_increment * end;\
191                         data = buf + (m_n - end) * m_channels;\
192                         l = fp_to_int(P);\
193 \
194                         for(i = 0; i <= end; i++)\
195                         {\
196                                 eta = fp_rest_to_double(P);\
197                                 v = coeff[l] + eta * (coeff[l+1] - coeff[l]);\
198                                 P -= P_increment;\
199                                 l = fp_to_int(P);\
200                                 left\
201                         }\
202 \
203                         P = 0 - P;\
204 \
205                         end = (int_to_fp(m_len) - P) / P_increment - 1;\
206                         if(m_cache_valid - m_n - 2 < end)\
207                                 end = m_cache_valid - m_n - 2;\
208 \
209                         P += P_increment * end;\
210                         data = buf + (m_n + 2 + end) * m_channels - 1;\
211                         l = fp_to_int(P);\
212 \
213                         for(i = 0; i <= end; i++)\
214                         {\
215                                 eta = fp_rest_to_double(P);\
216                                 v = coeff[l] + eta * (coeff[l+1] - coeff[l]);\
217                                 P -= P_increment;\
218                                 l = fp_to_int(P);\
219                                 right\
220                         }\
221 \
222                         for(channel = 0; channel < m_channels; channel++)\
223                         {\
224                                                                 *buffer = factor * sums[channel];\
225                                 buffer++;\
226                         }\
227                 }\
228 \
229                 m_P += fmod(1.0 / factor, 1.0);\
230                 m_n += floor(1.0 / factor);\
231 \
232                 while(m_P >= 1.0)\
233                 {\
234                         m_P -= 1.0;\
235                         m_n++;\
236                 }\
237         }\
238 }
239
240 RESAMPLE_METHOD(resample, {
241                                 channel = 0;
242                                 do
243                                 {
244                                         sums[channel] += *data * v;
245                                         channel++;
246                                         data++;
247                                 }
248                                 while(channel < m_channels);
249 }, {
250                                 channel = m_channels;
251                                 do
252                                 {
253                                         channel--;
254                                         sums[channel] += *data * v;
255                                         data--;
256                                 }
257                                 while(channel);
258 })
259
260 RESAMPLE_METHOD(resample_mono, {
261                                 *sums += *data * v;
262                                 data++;
263 }, {
264                                 *sums += *data * v;
265                                 data--;
266 })
267
268 RESAMPLE_METHOD(resample_stereo, {
269                                 sums[0] += data[0] * v;
270                                 sums[1] += data[1] * v;
271                                 data+=2;
272 }, {
273                                 data-=2;
274                                 sums[0] += data[1] * v;
275                                 sums[1] += data[2] * v;
276 })
277
278 void AUD_JOSResampleReader::seek(int position)
279 {
280         position = floor(position * double(m_reader->getSpecs().rate) / double(m_rate));
281         m_reader->seek(position);
282         reset();
283 }
284
285 int AUD_JOSResampleReader::getLength() const
286 {
287         return floor(m_reader->getLength() * double(m_rate) / double(m_reader->getSpecs().rate));
288 }
289
290 int AUD_JOSResampleReader::getPosition() const
291 {
292         return floor((m_reader->getPosition() + double(m_P))
293                                  * m_rate / m_reader->getSpecs().rate);
294 }
295
296 AUD_Specs AUD_JOSResampleReader::getSpecs() const
297 {
298         AUD_Specs specs = m_reader->getSpecs();
299         specs.rate = m_rate;
300         return specs;
301 }
302
303 void AUD_JOSResampleReader::read(int& length, bool& eos, sample_t* buffer)
304 {
305         if(length == 0)
306                 return;
307
308         AUD_Specs specs = m_reader->getSpecs();
309
310         int samplesize = AUD_SAMPLE_SIZE(specs);
311         double target_factor = double(m_rate) / double(specs.rate);
312         eos = false;
313         int len;
314         double num_samples = double(m_len) / double(m_L);
315
316         // check for channels changed
317         if(specs.channels != m_channels)
318         {
319                 m_channels = specs.channels;
320                 reset();
321
322                 switch(m_channels)
323                 {
324                 case AUD_CHANNELS_MONO:
325                         m_resample = &AUD_JOSResampleReader::resample_mono;
326                         break;
327                 case AUD_CHANNELS_STEREO:
328                         m_resample = &AUD_JOSResampleReader::resample_stereo;
329                         break;
330                 default:
331                         m_resample = &AUD_JOSResampleReader::resample;
332                         break;
333                 }
334         }
335
336         if(m_last_factor == 0)
337                 m_last_factor = target_factor;
338
339         if(target_factor == 1 && m_last_factor == 1 && (m_P == 0))
340         {
341                 // can read directly!
342
343                 len = length - (m_cache_valid - m_n);
344
345                 updateBuffer(len, target_factor, samplesize);
346                 sample_t* buf = m_buffer.getBuffer();
347
348                 m_reader->read(len, eos, buf + m_cache_valid * m_channels);
349                 m_cache_valid += len;
350
351                 length = m_cache_valid - m_n;
352
353                 if(length > 0)
354                 {
355                         memcpy(buffer, buf + m_n * m_channels, length * samplesize);
356                         m_n += length;
357                 }
358
359                 return;
360         }
361
362         // use minimum for the following calculations
363         double factor = AUD_MIN(target_factor, m_last_factor);
364
365         if(factor >= 1)
366                 len = (int(m_n) - m_cache_valid) + int(ceil(length / factor)) + ceil(num_samples);
367         else
368                 len = (int(m_n) - m_cache_valid) + int(ceil(length / factor) + ceil(num_samples / factor));
369
370         if(len > 0)
371         {
372                 int should = len;
373
374                 updateBuffer(len, factor, samplesize);
375
376                 m_reader->read(len, eos, m_buffer.getBuffer() + m_cache_valid * m_channels);
377                 m_cache_valid += len;
378
379                 if(len < should)
380                 {
381                         if(len == 0 && eos)
382                                 length = 0;
383                         else
384                         {
385                                 // use maximum for the following calculations
386                                 factor = AUD_MAX(target_factor, m_last_factor);
387
388                                 if(eos)
389                                 {
390                                         // end of stream, let's check how many more samples we can produce
391                                         len = floor((m_cache_valid - m_n) * factor);
392                                         if(len < length)
393                                                 length = len;
394                                 }
395                                 else
396                                 {
397                                         // not enough data available yet, so we recalculate how many samples we can calculate
398                                         if(factor >= 1)
399                                                 len = floor((num_samples + m_cache_valid - m_n) * factor);
400                                         else
401                                                 len = floor((num_samples * factor + m_cache_valid - m_n) * factor);
402                                         if(len < length)
403                                                 length = len;
404                                 }
405                         }
406                 }
407         }
408
409         (this->*m_resample)(target_factor, length, buffer);
410
411         m_last_factor = target_factor;
412
413         if(m_n > m_cache_valid)
414         {
415                 m_n = m_cache_valid;
416         }
417
418         eos = eos && ((m_n == m_cache_valid) || (length == 0));
419 }