1/**************************************************************************/
2/* audio_rb_resampler.cpp */
3/**************************************************************************/
4/* This file is part of: */
5/* GODOT ENGINE */
6/* https://godotengine.org */
7/**************************************************************************/
8/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
9/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */
10/* */
11/* Permission is hereby granted, free of charge, to any person obtaining */
12/* a copy of this software and associated documentation files (the */
13/* "Software"), to deal in the Software without restriction, including */
14/* without limitation the rights to use, copy, modify, merge, publish, */
15/* distribute, sublicense, and/or sell copies of the Software, and to */
16/* permit persons to whom the Software is furnished to do so, subject to */
17/* the following conditions: */
18/* */
19/* The above copyright notice and this permission notice shall be */
20/* included in all copies or substantial portions of the Software. */
21/* */
22/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */
23/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */
24/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */
25/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */
26/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */
27/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */
28/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */
29/**************************************************************************/
30
31#include "audio_rb_resampler.h"
32#include "core/math/math_funcs.h"
33#include "core/os/os.h"
34#include "servers/audio_server.h"
35
36int AudioRBResampler::get_channel_count() const {
37 if (!rb) {
38 return 0;
39 }
40
41 return channels;
42}
43
44// Linear interpolation based sample rate conversion (low quality)
45// Note that AudioStreamPlaybackResampled::mix has better algorithm,
46// but it wasn't obvious to integrate that with VideoStreamPlayer
47template <int C>
48uint32_t AudioRBResampler::_resample(AudioFrame *p_dest, int p_todo, int32_t p_increment) {
49 uint32_t read = offset & MIX_FRAC_MASK;
50
51 for (int i = 0; i < p_todo; i++) {
52 offset = (offset + p_increment) & (((1 << (rb_bits + MIX_FRAC_BITS)) - 1));
53 read += p_increment;
54 uint32_t pos = offset >> MIX_FRAC_BITS;
55 float frac = float(offset & MIX_FRAC_MASK) / float(MIX_FRAC_LEN);
56 ERR_FAIL_COND_V(pos >= rb_len, 0);
57 uint32_t pos_next = (pos + 1) & rb_mask;
58
59 // since this is a template with a known compile time value (C), conditionals go away when compiling.
60 if constexpr (C == 1) {
61 float v0 = rb[pos];
62 float v0n = rb[pos_next];
63 v0 += (v0n - v0) * frac;
64 p_dest[i] = AudioFrame(v0, v0);
65 }
66
67 if constexpr (C == 2) {
68 float v0 = rb[(pos << 1) + 0];
69 float v1 = rb[(pos << 1) + 1];
70 float v0n = rb[(pos_next << 1) + 0];
71 float v1n = rb[(pos_next << 1) + 1];
72
73 v0 += (v0n - v0) * frac;
74 v1 += (v1n - v1) * frac;
75 p_dest[i] = AudioFrame(v0, v1);
76 }
77
78 // This will probably never be used, but added anyway
79 if constexpr (C == 4) {
80 float v0 = rb[(pos << 2) + 0];
81 float v1 = rb[(pos << 2) + 1];
82 float v0n = rb[(pos_next << 2) + 0];
83 float v1n = rb[(pos_next << 2) + 1];
84 v0 += (v0n - v0) * frac;
85 v1 += (v1n - v1) * frac;
86 p_dest[i] = AudioFrame(v0, v1);
87 }
88
89 if constexpr (C == 6) {
90 float v0 = rb[(pos * 6) + 0];
91 float v1 = rb[(pos * 6) + 1];
92 float v0n = rb[(pos_next * 6) + 0];
93 float v1n = rb[(pos_next * 6) + 1];
94
95 v0 += (v0n - v0) * frac;
96 v1 += (v1n - v1) * frac;
97 p_dest[i] = AudioFrame(v0, v1);
98 }
99 }
100
101 return read >> MIX_FRAC_BITS; //rb_read_pos = offset >> MIX_FRAC_BITS;
102}
103
104bool AudioRBResampler::mix(AudioFrame *p_dest, int p_frames) {
105 if (!rb) {
106 return false;
107 }
108
109 int32_t increment = (src_mix_rate * MIX_FRAC_LEN) / target_mix_rate;
110 int read_space = get_reader_space();
111 int target_todo = MIN(get_num_of_ready_frames(), p_frames);
112
113 {
114 int src_read = 0;
115 switch (channels) {
116 case 1:
117 src_read = _resample<1>(p_dest, target_todo, increment);
118 break;
119 case 2:
120 src_read = _resample<2>(p_dest, target_todo, increment);
121 break;
122 case 4:
123 src_read = _resample<4>(p_dest, target_todo, increment);
124 break;
125 case 6:
126 src_read = _resample<6>(p_dest, target_todo, increment);
127 break;
128 }
129
130 if (src_read > read_space) {
131 src_read = read_space;
132 }
133
134 rb_read_pos.set((rb_read_pos.get() + src_read) & rb_mask);
135
136 // Create fadeout effect for the end of stream (note that it can be because of slow writer)
137 if (p_frames - target_todo > 0) {
138 for (int i = 0; i < target_todo; i++) {
139 p_dest[i] = p_dest[i] * float(target_todo - i) / float(target_todo);
140 }
141 }
142
143 // Fill zeros (silence) for the rest of frames
144 for (int i = target_todo; i < p_frames; i++) {
145 p_dest[i] = AudioFrame(0, 0);
146 }
147 }
148
149 return true;
150}
151
152int AudioRBResampler::get_num_of_ready_frames() {
153 if (!is_ready()) {
154 return 0;
155 }
156 int32_t increment = (src_mix_rate * MIX_FRAC_LEN) / target_mix_rate;
157 int read_space = get_reader_space();
158 return (int64_t(read_space) << MIX_FRAC_BITS) / increment;
159}
160
161Error AudioRBResampler::setup(int p_channels, int p_src_mix_rate, int p_target_mix_rate, int p_buffer_msec, int p_minbuff_needed) {
162 ERR_FAIL_COND_V(p_channels != 1 && p_channels != 2 && p_channels != 4 && p_channels != 6, ERR_INVALID_PARAMETER);
163
164 int desired_rb_bits = nearest_shift(MAX((p_buffer_msec / 1000.0) * p_src_mix_rate, p_minbuff_needed));
165
166 bool recreate = !rb;
167
168 if (rb && (uint32_t(desired_rb_bits) != rb_bits || channels != uint32_t(p_channels))) {
169 memdelete_arr(rb);
170 memdelete_arr(read_buf);
171 recreate = true;
172 }
173
174 if (recreate) {
175 channels = p_channels;
176 rb_bits = desired_rb_bits;
177 rb_len = (1 << rb_bits);
178 rb_mask = rb_len - 1;
179 const size_t array_size = rb_len * (size_t)p_channels;
180 rb = memnew_arr(float, array_size);
181 read_buf = memnew_arr(float, array_size);
182 }
183
184 src_mix_rate = p_src_mix_rate;
185 target_mix_rate = p_target_mix_rate;
186 offset = 0;
187 rb_read_pos.set(0);
188 rb_write_pos.set(0);
189
190 //avoid maybe strange noises upon load
191 for (unsigned int i = 0; i < (rb_len * channels); i++) {
192 rb[i] = 0;
193 read_buf[i] = 0;
194 }
195
196 return OK;
197}
198
199void AudioRBResampler::clear() {
200 if (!rb) {
201 return;
202 }
203
204 //should be stopped at this point but just in case
205 memdelete_arr(rb);
206 memdelete_arr(read_buf);
207 rb = nullptr;
208 offset = 0;
209 rb_read_pos.set(0);
210 rb_write_pos.set(0);
211 read_buf = nullptr;
212}
213
214AudioRBResampler::AudioRBResampler() {
215 rb = nullptr;
216 offset = 0;
217 read_buf = nullptr;
218 rb_read_pos.set(0);
219 rb_write_pos.set(0);
220
221 rb_bits = 0;
222 rb_len = 0;
223 rb_mask = 0;
224 read_buff_len = 0;
225 channels = 0;
226 src_mix_rate = 0;
227 target_mix_rate = 0;
228}
229
230AudioRBResampler::~AudioRBResampler() {
231 if (rb) {
232 memdelete_arr(rb);
233 memdelete_arr(read_buf);
234 }
235}
236