sample_rate_conversion.cpp (6898B)
1 /** @addtogroup dft 2 * @{ 3 */ 4 /* 5 Copyright (C) 2016-2023 Dan Cazarin (https://www.kfrlib.com) 6 This file is part of KFR 7 8 KFR 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 KFR 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 KFR. 20 21 If GPL is not suitable for your project, you must purchase a commercial license to use KFR. 22 Buying a commercial license is mandatory as soon as you develop commercial activities without 23 disclosing the source code of your own applications. 24 See https://www.kfrlib.com for details. 25 */ 26 #include <kfr/dsp/sample_rate_conversion.hpp> 27 #include <kfr/multiarch.h> 28 29 namespace kfr 30 { 31 CMT_MULTI_PROTO(namespace impl { 32 template <typename T> 33 struct samplerate_converter : public kfr::samplerate_converter<T> 34 { 35 public: 36 using itype = typename kfr::samplerate_converter<T>::itype; 37 using ftype = typename kfr::samplerate_converter<T>::ftype; 38 void init(sample_rate_conversion_quality quality, itype interpolation_factor, itype decimation_factor, 39 subtype<T> scale, subtype<T> cutoff); 40 size_t process_impl(univector_ref<T> output, univector_ref<const T> input); 41 }; 42 } // namespace impl 43 ) 44 45 inline namespace CMT_ARCH_NAME 46 { 47 namespace impl 48 { 49 50 template <typename T> 51 void samplerate_converter<T>::init(sample_rate_conversion_quality quality, itype interpolation_factor, 52 itype decimation_factor, subtype<T> scale, subtype<T> cutoff) 53 { 54 this->kaiser_beta = this->window_param(quality); 55 this->depth = static_cast<itype>(this->filter_order(quality)); 56 this->input_position = 0; 57 this->output_position = 0; 58 59 const i64 gcf = gcd(interpolation_factor, decimation_factor); 60 interpolation_factor /= gcf; 61 decimation_factor /= gcf; 62 63 this->taps = this->depth * interpolation_factor; 64 this->order = size_t(this->depth * interpolation_factor - 1); 65 66 this->interpolation_factor = interpolation_factor; 67 this->decimation_factor = decimation_factor; 68 69 const itype halftaps = this->taps / 2; 70 this->filter = univector<T>(size_t(this->taps), T()); 71 this->delay = univector<T>(size_t(this->depth), T()); 72 73 cutoff = cutoff - this->transition_width() / c_pi<ftype, 4>; 74 75 cutoff = cutoff / std::max(decimation_factor, interpolation_factor); 76 77 for (itype j = 0, jj = 0; j < this->taps; j++) 78 { 79 this->filter[size_t(j)] = 80 sinc((jj - halftaps) * cutoff * c_pi<ftype, 2>) * this->window(ftype(jj) / ftype(this->taps - 1)); 81 jj += size_t(interpolation_factor); 82 if (jj >= this->taps) 83 jj = jj - this->taps + 1; 84 } 85 86 const T s = reciprocal(sum(this->filter)) * static_cast<ftype>(interpolation_factor * scale); 87 this->filter = this->filter * s; 88 } 89 90 template <typename T> 91 size_t samplerate_converter<T>::process_impl(univector_ref<T> output, univector_ref<const T> input) 92 { 93 const itype required_input_size = this->input_size_for_output(output.size()); 94 95 const itype input_size = input.size(); 96 for (size_t i = 0; i < output.size(); i++) 97 { 98 const itype intermediate_index = 99 this->output_position_to_intermediate(static_cast<itype>(i) + this->output_position); 100 const itype intermediate_start = intermediate_index - this->taps + 1; 101 const std::lldiv_t input_pos = 102 floor_div(intermediate_start + this->interpolation_factor - 1, this->interpolation_factor); 103 const itype input_start = input_pos.quot; // first input sample 104 const itype tap_start = this->interpolation_factor - 1 - input_pos.rem; 105 const univector_ref<T> tap_ptr = this->filter.slice(static_cast<size_t>(tap_start * this->depth)); 106 107 if (input_start >= this->input_position + input_size) 108 { 109 output[i] = T(0); 110 } 111 else if (input_start >= this->input_position) 112 { 113 output[i] = dotproduct( 114 truncate(padded(input.slice(input_start - this->input_position, this->depth)), this->depth), 115 tap_ptr.truncate(this->depth)); 116 } 117 else 118 { 119 const itype prev_count = this->input_position - input_start; 120 output[i] = dotproduct(this->delay.slice(size_t(this->depth - prev_count)), 121 tap_ptr.truncate(prev_count)) + 122 dotproduct(truncate(padded(input.truncate(size_t(this->depth - prev_count))), 123 size_t(this->depth - prev_count)), 124 tap_ptr.slice(size_t(prev_count), size_t(this->depth - prev_count))); 125 } 126 } 127 128 if (required_input_size >= this->depth) 129 { 130 this->delay.slice(0, this->delay.size()) = 131 padded(input.slice(size_t(required_input_size - this->depth))); 132 } 133 else 134 { 135 this->delay.truncate(size_t(this->depth - required_input_size)) = 136 this->delay.slice(size_t(required_input_size)); 137 this->delay.slice(size_t(this->depth - required_input_size)) = padded(input); 138 } 139 140 this->input_position += required_input_size; 141 this->output_position += output.size(); 142 143 return required_input_size; 144 } 145 146 template struct samplerate_converter<float>; 147 template struct samplerate_converter<double>; 148 template struct samplerate_converter<complex<float>>; 149 template struct samplerate_converter<complex<double>>; 150 151 } // namespace impl 152 } // namespace CMT_ARCH_NAME 153 154 #ifdef CMT_MULTI_NEEDS_GATE 155 156 template <typename T> 157 samplerate_converter<T>::samplerate_converter(sample_rate_conversion_quality quality, 158 itype interpolation_factor, itype decimation_factor, 159 ftype scale, ftype cutoff) 160 { 161 CMT_MULTI_GATE(reinterpret_cast<ns::impl::samplerate_converter<T>*>(this)->init( 162 quality, interpolation_factor, decimation_factor, scale, cutoff)); 163 } 164 165 template <typename T> 166 size_t samplerate_converter<T>::process_impl(univector_ref<T> output, univector_ref<const T> input) 167 { 168 CMT_MULTI_GATE( 169 return reinterpret_cast<ns::impl::samplerate_converter<T>*>(this)->process_impl(output, input)); 170 } 171 172 template struct samplerate_converter<float>; 173 template struct samplerate_converter<double>; 174 template struct samplerate_converter<complex<float>>; 175 template struct samplerate_converter<complex<double>>; 176 177 #endif 178 179 } // namespace kfr