123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239 |
- #include "audio_rb_resampler.h"
- #include "core/math/math_funcs.h"
- #include "core/os/os.h"
- #include "servers/audio_server.h"
- int AudioRBResampler::get_channel_count() const {
- if (!rb)
- return 0;
- return channels;
- }
- template <int C>
- uint32_t AudioRBResampler::_resample(AudioFrame *p_dest, int p_todo, int32_t p_increment) {
- uint32_t read = offset & MIX_FRAC_MASK;
- for (int i = 0; i < p_todo; i++) {
- offset = (offset + p_increment) & (((1 << (rb_bits + MIX_FRAC_BITS)) - 1));
- read += p_increment;
- uint32_t pos = offset >> MIX_FRAC_BITS;
- float frac = float(offset & MIX_FRAC_MASK) / float(MIX_FRAC_LEN);
- ERR_FAIL_COND_V(pos >= rb_len, 0);
- uint32_t pos_next = (pos + 1) & rb_mask;
-
- if (C == 1) {
- float v0 = rb[pos];
- float v0n = rb[pos_next];
- v0 += (v0n - v0) * frac;
- p_dest[i] = AudioFrame(v0, v0);
- }
- if (C == 2) {
- float v0 = rb[(pos << 1) + 0];
- float v1 = rb[(pos << 1) + 1];
- float v0n = rb[(pos_next << 1) + 0];
- float v1n = rb[(pos_next << 1) + 1];
- v0 += (v0n - v0) * frac;
- v1 += (v1n - v1) * frac;
- p_dest[i] = AudioFrame(v0, v1);
- }
-
- if (C == 4) {
- float v0 = rb[(pos << 2) + 0];
- float v1 = rb[(pos << 2) + 1];
- float v0n = rb[(pos_next << 2) + 0];
- float v1n = rb[(pos_next << 2) + 1];
- v0 += (v0n - v0) * frac;
- v1 += (v1n - v1) * frac;
- p_dest[i] = AudioFrame(v0, v1);
- }
- if (C == 6) {
- float v0 = rb[(pos * 6) + 0];
- float v1 = rb[(pos * 6) + 1];
- float v0n = rb[(pos_next * 6) + 0];
- float v1n = rb[(pos_next * 6) + 1];
- v0 += (v0n - v0) * frac;
- v1 += (v1n - v1) * frac;
- p_dest[i] = AudioFrame(v0, v1);
- }
- }
- return read >> MIX_FRAC_BITS;
- }
- bool AudioRBResampler::mix(AudioFrame *p_dest, int p_frames) {
- if (!rb)
- return false;
- int32_t increment = (src_mix_rate * MIX_FRAC_LEN) / target_mix_rate;
- int read_space = get_reader_space();
- int target_todo = MIN(get_num_of_ready_frames(), p_frames);
- {
- int src_read = 0;
- switch (channels) {
- case 1: src_read = _resample<1>(p_dest, target_todo, increment); break;
- case 2: src_read = _resample<2>(p_dest, target_todo, increment); break;
- case 4: src_read = _resample<4>(p_dest, target_todo, increment); break;
- case 6: src_read = _resample<6>(p_dest, target_todo, increment); break;
- }
- if (src_read > read_space)
- src_read = read_space;
- rb_read_pos = (rb_read_pos + src_read) & rb_mask;
-
- if (p_frames - target_todo > 0) {
- for (int i = 0; i < target_todo; i++) {
- p_dest[i] = p_dest[i] * float(target_todo - i) / float(target_todo);
- }
- }
-
- for (int i = target_todo; i < p_frames; i++) {
- p_dest[i] = AudioFrame(0, 0);
- }
- }
- return true;
- }
- int AudioRBResampler::get_num_of_ready_frames() {
- if (!is_ready())
- return 0;
- int32_t increment = (src_mix_rate * MIX_FRAC_LEN) / target_mix_rate;
- int read_space = get_reader_space();
- return (int64_t(read_space) << MIX_FRAC_BITS) / increment;
- }
- Error AudioRBResampler::setup(int p_channels, int p_src_mix_rate, int p_target_mix_rate, int p_buffer_msec, int p_minbuff_needed) {
- ERR_FAIL_COND_V(p_channels != 1 && p_channels != 2 && p_channels != 4 && p_channels != 6, ERR_INVALID_PARAMETER);
- int desired_rb_bits = nearest_shift(MAX((p_buffer_msec / 1000.0) * p_src_mix_rate, p_minbuff_needed));
- bool recreate = !rb;
- if (rb && (uint32_t(desired_rb_bits) != rb_bits || channels != uint32_t(p_channels))) {
- memdelete_arr(rb);
- memdelete_arr(read_buf);
- recreate = true;
- }
- if (recreate) {
- channels = p_channels;
- rb_bits = desired_rb_bits;
- rb_len = (1 << rb_bits);
- rb_mask = rb_len - 1;
- rb = memnew_arr(float, rb_len *p_channels);
- read_buf = memnew_arr(float, rb_len *p_channels);
- }
- src_mix_rate = p_src_mix_rate;
- target_mix_rate = p_target_mix_rate;
- offset = 0;
- rb_read_pos = 0;
- rb_write_pos = 0;
-
- for (unsigned int i = 0; i < (rb_len * channels); i++) {
- rb[i] = 0;
- read_buf[i] = 0;
- }
- return OK;
- }
- void AudioRBResampler::clear() {
- if (!rb)
- return;
-
- if (rb) {
- memdelete_arr(rb);
- memdelete_arr(read_buf);
- }
- rb = NULL;
- offset = 0;
- rb_read_pos = 0;
- rb_write_pos = 0;
- read_buf = NULL;
- }
- AudioRBResampler::AudioRBResampler() {
- rb = NULL;
- offset = 0;
- read_buf = NULL;
- rb_read_pos = 0;
- rb_write_pos = 0;
- rb_bits = 0;
- rb_len = 0;
- rb_mask = 0;
- read_buff_len = 0;
- channels = 0;
- src_mix_rate = 0;
- target_mix_rate = 0;
- }
- AudioRBResampler::~AudioRBResampler() {
- if (rb) {
- memdelete_arr(rb);
- memdelete_arr(read_buf);
- }
- }
|