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