InitialHEADmaster

1 files changed, 92 insertions, 0 deletions

diff --git a/lib/resample_func.h b/lib/resample_func.h
new file mode 100644
index 0000000..9e894d4
--- /dev/null
+++ b/lib/resample_func.h
@@ -0,0 +1,92 @@
+#pragma once
+
+#include <array>
+#include <algorithm>
+#include <cmath>
+#include <type_traits>
+#include <limits>
+#include "resample.h"
+#include "filter.h"
+
+namespace {
+	template<typename result_type, typename type>
+	constexpr result_type dither_truncate(type value, unsigned dither)
+	{
+		using limits = std::numeric_limits<result_type>;
+		using dither_limits = std::numeric_limits<decltype(dither)>;
+		if constexpr(std::is_floating_point_v<type>)
+		{
+			value += dither * type{1} / type{1ul << dither_limits::digits};
+			value = std::floor(value);
+		}
+		else
+		{
+			static_assert(std::is_integral_v<type>, "Unsupported acc_type");
+
+			value += dither >> (dither_limits::digits - limits::digits); // 17 for short
+			value >>= limits::digits; // 15 for short
+		}
+
+		return std::clamp<type>(value, limits::min(), limits::max());
+	}
+}
+
+template<typename sample_type_in, typename sample_type_out, typename filter_type, typename acc_type, size_t channels = 1, size_t INTERP_BIT = 14>
+void lfr_resample(lfr_fixed_t& pos, const lfr_fixed_t inv_ratio, unsigned& dither, void* out, std::size_t outlen, const void* in, std::size_t inlen, const lfr_filter& filter)
+{
+	static_assert(channels > 0, "0 channels do not make any sense");
+
+	auto inp = reinterpret_cast<const sample_type_in*>(in);
+	auto outp = reinterpret_cast<sample_type_out*>(out);
+	auto fd = reinterpret_cast<const filter_type*>(filter.data.data());
+
+	const auto flen = filter.nsamp;
+	const auto log2nfilt = filter.log2nfilt;
+	auto x = pos;
+	std::array<unsigned, channels> ds{dither};
+	for(std::size_t i = 1; i < channels; ++i)
+	{
+		ds[i] = LCG_A * ds[i - 1] + LCG_C;
+	}
+
+	for(std::size_t i = 0; i < outlen; ++i)
+	{
+		/* fn: filter number
+			ff0: filter factor for filter fn
+			ff1: filter factor for filter fn+1 */
+
+		const auto fn = (static_cast<unsigned>(x >> 1) >> (31 - log2nfilt)) & ((1u << log2nfilt) - 1);
+		int ff1 = (static_cast<unsigned>(x >> (32 - log2nfilt - INTERP_BIT))) & ((1u << INTERP_BIT) - 1);
+		int ff0 = (1u << INTERP_BIT) - ff1;
+
+		/* off: offset in input corresponding to first sample in filter */
+		auto off = static_cast<int>(x >> 32);
+		/* fidx0, fidx1: start, end indexes in FIR data */
+		int fidx0 = std::max(-off, 0);
+		int fidx1 = std::min<int>(inlen - off, flen);
+
+		/* acc: FIR accumulator */
+		std::array<acc_type, channels> acc{0};
+
+		for(int j = fidx0; j < fidx1; ++j)
+		{
+			acc_type f = (fd[(fn + 0) * flen + j] * ff0 + fd[(fn + 1) * flen + j] * ff1) / (1 << INTERP_BIT);
+			for(std::size_t c = 0; c < channels; ++c)
+			{
+				acc[c] += inp[(j + off) * channels + c] * f;
+			}
+		}
+
+		for(std::size_t c = 0; c < channels; ++c)
+		{
+			outp[i * channels + c] = dither_truncate<sample_type_out>(acc[c], ds[c]);
+
+			ds[c] = LCG_A2 * ds[c] + LCG_C2;
+		}
+
+		x += inv_ratio;
+	}
+
+	pos = x;
+	dither = ds[0];
+}