1 // Game_Music_Emu 0.6.0. http://www.slack.net/~ant/
2
3 #include "Fir_Resampler.h"
4
5 #include <string.h>
6 #include <stdlib.h>
7 #include <stdio.h>
8 #include <math.h>
9
10 /* Copyright (C) 2004-2006 Shay Green. This module is free software; you
11 can redistribute it and/or modify it under the terms of the GNU Lesser
12 General Public License as published by the Free Software Foundation; either
13 version 2.1 of the License, or (at your option) any later version. This
14 module is distributed in the hope that it will be useful, but WITHOUT ANY
15 WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
16 FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License for more
17 details. You should have received a copy of the GNU Lesser General Public
18 License along with this module; if not, write to the Free Software Foundation,
19 Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA */
20
21 #include "blargg_source.h"
22
23 #undef PI
24 #define PI 3.1415926535897932384626433832795029
25
gen_sinc(double rolloff,int width,double offset,double spacing,double scale,int count,short * out)26 static void gen_sinc( double rolloff, int width, double offset, double spacing, double scale,
27 int count, short* out )
28 {
29 double const maxh = 256;
30 double const step = PI / maxh * spacing;
31 double const to_w = maxh * 2 / width;
32 double const pow_a_n = pow( rolloff, maxh );
33 scale /= maxh * 2;
34
35 double angle = (count / 2 - 1 + offset) * -step;
36 while ( count-- )
37 {
38 *out++ = 0;
39 double w = angle * to_w;
40 if ( fabs( w ) < PI )
41 {
42 double rolloff_cos_a = rolloff * cos( angle );
43 double num = 1 - rolloff_cos_a -
44 pow_a_n * cos( maxh * angle ) +
45 pow_a_n * rolloff * cos( (maxh - 1) * angle );
46 double den = 1 - rolloff_cos_a - rolloff_cos_a + rolloff * rolloff;
47 double sinc = scale * num / den - scale;
48
49 out [-1] = (short) (cos( w ) * sinc + sinc);
50 }
51 angle += step;
52 }
53 }
54
Fir_Resampler_(int width,sample_t * impulses_)55 Fir_Resampler_::Fir_Resampler_( int width, sample_t* impulses_ ) :
56 width_( width ),
57 write_offset( width * stereo - stereo ),
58 impulses( impulses_ )
59 {
60 write_pos = 0;
61 res = 1;
62 imp_phase = 0;
63 skip_bits = 0;
64 step = stereo;
65 ratio_ = 1.0;
66 }
67
~Fir_Resampler_()68 Fir_Resampler_::~Fir_Resampler_() { }
69
clear()70 void Fir_Resampler_::clear()
71 {
72 imp_phase = 0;
73 if ( buf.size() )
74 {
75 write_pos = &buf [write_offset];
76 memset( buf.begin(), 0, write_offset * sizeof buf [0] );
77 }
78 }
79
buffer_size(int new_size)80 blargg_err_t Fir_Resampler_::buffer_size( int new_size )
81 {
82 RETURN_ERR( buf.resize( new_size + write_offset ) );
83 clear();
84 return 0;
85 }
86
time_ratio(double new_factor,double rolloff,double gain)87 double Fir_Resampler_::time_ratio( double new_factor, double rolloff, double gain )
88 {
89 ratio_ = new_factor;
90
91 double fstep = 0.0;
92 {
93 double least_error = 2;
94 double pos = 0;
95 res = -1;
96 for ( int r = 1; r <= max_res; r++ )
97 {
98 pos += ratio_;
99 double nearest = floor( pos + 0.5 );
100 double error = fabs( pos - nearest );
101 if ( error < least_error )
102 {
103 res = r;
104 fstep = nearest / res;
105 least_error = error;
106 }
107 }
108 }
109
110 skip_bits = 0;
111
112 step = stereo * (int) floor( fstep );
113
114 ratio_ = fstep;
115 fstep = fmod( fstep, 1.0 );
116
117 double filter = (ratio_ < 1.0) ? 1.0 : 1.0 / ratio_;
118 double pos = 0.0;
119 input_per_cycle = 0;
120 for ( int i = 0; i < res; i++ )
121 {
122 gen_sinc( rolloff, int (width_ * filter + 1) & ~1, pos, filter,
123 double (0x7FFF * gain * filter),
124 (int) width_, impulses + i * width_ );
125
126 pos += fstep;
127 input_per_cycle += step;
128 if ( pos >= 0.9999999 )
129 {
130 pos -= 1.0;
131 skip_bits |= 1 << i;
132 input_per_cycle++;
133 }
134 }
135
136 clear();
137
138 return ratio_;
139 }
140
input_needed(blargg_long output_count) const141 int Fir_Resampler_::input_needed( blargg_long output_count ) const
142 {
143 blargg_long input_count = 0;
144
145 unsigned long skip = skip_bits >> imp_phase;
146 int remain = res - imp_phase;
147 while ( (output_count -= 2) > 0 )
148 {
149 input_count += step + (skip & 1) * stereo;
150 skip >>= 1;
151 if ( !--remain )
152 {
153 skip = skip_bits;
154 remain = res;
155 }
156 output_count -= 2;
157 }
158
159 long input_extra = input_count - (write_pos - &buf [(width_ - 1) * stereo]);
160 if ( input_extra < 0 )
161 input_extra = 0;
162 return input_extra;
163 }
164
avail_(blargg_long input_count) const165 int Fir_Resampler_::avail_( blargg_long input_count ) const
166 {
167 int cycle_count = input_count / input_per_cycle;
168 int output_count = cycle_count * res * stereo;
169 input_count -= cycle_count * input_per_cycle;
170
171 blargg_ulong skip = skip_bits >> imp_phase;
172 int remain = res - imp_phase;
173 while ( input_count >= 0 )
174 {
175 input_count -= step + (skip & 1) * stereo;
176 skip >>= 1;
177 if ( !--remain )
178 {
179 skip = skip_bits;
180 remain = res;
181 }
182 output_count += 2;
183 }
184 return output_count;
185 }
186
skip_input(long count)187 int Fir_Resampler_::skip_input( long count )
188 {
189 int remain = write_pos - buf.begin();
190 int max_count = remain - width_ * stereo;
191 if ( count > max_count )
192 count = max_count;
193
194 remain -= count;
195 write_pos = &buf [remain];
196 memmove( buf.begin(), &buf [count], remain * sizeof buf [0] );
197
198 return count;
199 }
200