static unsafe void cubic_spline(stage_t p, fifo_t output_fifo) { int i, num_in = p.occupancy; int max_num_out = 1 + (int)(num_in * p.out_in_ratio); int output_offs = output_fifo.reserve(max_num_out); fixed(byte *pinput = &p.fifo.data[p.offset], poutput = &output_fifo.data[output_offs]) { double *input = (double *)pinput; double *output = (double *)poutput; for (i = 0; (p.at >> 32) < num_in; ++i, p.at += p.step) { double *s = input + (p.at >> 32); double x = (p.at & 0xffffffff) * (1 / MULT32); double b = 0.5 * (s[1] + s[-1]) - *s, a = (1 / 6.0) * (s[2] - s[1] + s[-1] - *s - 4 * b); double c = s[1] - *s - a - b; output[i] = ((a * x + b) * x + c) * x + *s; } } //assert(max_num_out - i >= 0); output_fifo.trim_by(max_num_out - i); p.fifo.read((int)(p.at >> 32), null); p.at &= 0xffffffff; }
static unsafe void double_sample(stage_t p, fifo_t output_fifo) { int num_in = Math.Max(0, p.fifo.occupancy); dft_filter_t f = p.shared.half_band[1]; int overlap = f.num_taps - 1; while (num_in > f.dft_length >> 1) { int input_offs = p.fifo.offset; p.fifo.read((f.dft_length - overlap) >> 1, null); num_in -= (f.dft_length - overlap) >> 1; int output_offs = output_fifo.reserve(f.dft_length); output_fifo.trim_by(overlap); fixed(byte *pinput = &p.fifo.data[input_offs]) fixed(byte *poutput = &output_fifo.data[output_offs]) fixed(double *lsx_fft_sc = p.shared.info.lsx_fft_sc) fixed(int *lsx_fft_br = p.shared.info.lsx_fft_br) { double *input = (double *)pinput; double *output = (double *)poutput; for (int j = 0, i = 0; i < f.dft_length; ++j, i += 2) { output[i] = input[j]; output[i + 1] = 0; } SOXFft.rdft(f.dft_length, 1, output, lsx_fft_br, lsx_fft_sc); output[0] *= f.coefs[0]; output[1] *= f.coefs[1]; for (int i = 2; i < f.dft_length; i += 2) { double tmp = output[i]; output[i] = f.coefs[i] * tmp - f.coefs[i + 1] * output[i + 1]; output[i + 1] = f.coefs[i + 1] * tmp + f.coefs[i] * output[i + 1]; } SOXFft.rdft(f.dft_length, -1, output, lsx_fft_br, lsx_fft_sc); } } }
static unsafe void half_sample(stage_t p, fifo_t output_fifo) { int num_in = Math.Max(0, p.fifo.occupancy); dft_filter_t f = p.shared.half_band[p.which]; int overlap = f.num_taps - 1; while (num_in >= f.dft_length) { int input_offs = p.fifo.offset; p.fifo.read(f.dft_length - overlap, null); num_in -= f.dft_length - overlap; int output_offs = output_fifo.reserve(f.dft_length); output_fifo.trim_by((f.dft_length + overlap) >> 1); Buffer.BlockCopy(p.fifo.data, input_offs, output_fifo.data, output_offs, f.dft_length * sizeof(double)); fixed(byte *poutput = &output_fifo.data[output_offs]) fixed(double *lsx_fft_sc = p.shared.info.lsx_fft_sc) fixed(int *lsx_fft_br = p.shared.info.lsx_fft_br) { double *output = (double *)poutput; SOXFft.rdft(f.dft_length, 1, output, lsx_fft_br, lsx_fft_sc); output[0] *= f.coefs[0]; output[1] *= f.coefs[1]; for (int i = 2; i < f.dft_length; i += 2) { double tmp = output[i]; output[i] = f.coefs[i] * tmp - f.coefs[i + 1] * output[i + 1]; output[i + 1] = f.coefs[i + 1] * tmp + f.coefs[i] * output[i + 1]; } SOXFft.rdft(f.dft_length, -1, output, lsx_fft_br, lsx_fft_sc); for (int j = 1, i = 2; i < f.dft_length - overlap; ++j, i += 2) { output[j] = output[i]; } } } }
static unsafe void half_sample_low(stage_t p, fifo_t output_fifo) { int num_out = (p.occupancy + 1) / 2; int output_offs = output_fifo.reserve(num_out); fixed(byte *pinput = &p.fifo.data[p.offset], poutput = &output_fifo.data[output_offs]) { double *input = (double *)pinput; double *output = (double *)poutput; for (int i = 0; i < num_out; ++i, input += 2) { double sum = input[0] * half_fir_coefs_low[0]; for (int j = 1; j < half_fir_coefs_low.Length; j++) { sum += (input[-j] + input[j]) * half_fir_coefs_low[j]; } output[i] = sum; } } p.fifo.read(2 * num_out, null); }
static unsafe void cubic_spline(stage_t p, fifo_t output_fifo) { int i, num_in = p.occupancy; int max_num_out = 1 + (int)(num_in * p.out_in_ratio); int output_offs = output_fifo.reserve(max_num_out); fixed (byte* pinput = &p.fifo.data[p.offset], poutput = &output_fifo.data[output_offs]) { double* input = (double*)pinput; double* output = (double*)poutput; for (i = 0; (p.at >> 32) < num_in; ++i, p.at += p.step) { double* s = input + (p.at >> 32); double x = (p.at & 0xffffffff) * (1 / MULT32); double b = 0.5 * (s[1] + s[-1]) - *s, a = (1 / 6.0) * (s[2] - s[1] + s[-1] - *s - 4 * b); double c = s[1] - *s - a - b; output[i] = ((a * x + b) * x + c) * x + *s; } } //assert(max_num_out - i >= 0); output_fifo.trim_by(max_num_out - i); p.fifo.read((int)(p.at >> 32), null); p.at &= 0xffffffff; }
public rate_t(int in_samplerate, int out_samplerate, rate_shared_t shared, double factor, SOXResamplerQuality quality, int interp_order, double phase, double bandwidth, bool allow_aliasing) { this.in_samplerate = in_samplerate; this.out_samplerate = out_samplerate; int i, mult, divisor = 1; //assert(factor > 0); this.factor = factor; if (quality < SOXResamplerQuality.Quick || quality > SOXResamplerQuality.Very) { quality = SOXResamplerQuality.High; } if (quality != SOXResamplerQuality.Quick) { const int max_divisor = 2048; /* Keep coef table size ~< 500kb */ const double epsilon = 4 / MULT32; /* Scaled to half this at max_divisor */ this.upsample = this.factor < 1; this.level = 0; for (int fi = (int)factor >> 1; fi != 0; fi >>= 1) { ++this.level;/* log base 2 */ } factor /= 1 << (this.level + (this.upsample ? 0 : 1)); for (i = 2; i <= max_divisor && divisor == 1; ++i) { double try_d = factor * i; int itry = (int)(try_d + .5); if (Math.Abs(itry - try_d) < itry * epsilon * (1 - (.5 / max_divisor) * i)) { if (itry == i) /* Rounded to 1:1? */ { factor = 1; divisor = 2; this.upsample = false; } else { factor = itry; divisor = i; } } } } this.stages = new stage_t[this.level + 4]; // offset by 1!!! + 3? for (i = 0; i < this.level + 4; ++i) { this.stages[i] = new stage_t(shared); } this.pre_stage = this.stages[0]; this.last_stage = this.stages[this.level + 1]; this.post_stage = this.stages[this.level + 2]; this.last_stage.step = (long)(factor * MULT32 + .5); this.last_stage.out_in_ratio = MULT32 * divisor / this.last_stage.step; //if (divisor != 1) // assert(!last_stage.step.parts.fraction); //else if (quality != Quick) // assert(!last_stage.step.parts.integer); //lsx_debug("i/o=%g; %.9g:%i @ level %i", this.factor, factor, divisor, this.level); mult = 1 + (this.upsample ? 1 : 0); /* Compensate for zero-stuffing in double_sample */ this.input_stage_num = this.upsample ? 0 : 1; this.output_stage_num = this.level + 1; if (quality == SOXResamplerQuality.Quick) { ++this.output_stage_num; last_stage.fn = cubic_spline; last_stage.pre_post = Math.Max(3, (int)(last_stage.step >> 32)); last_stage.preload = last_stage.pre = 1; } else if (last_stage.out_in_ratio != 2 || (this.upsample && quality == SOXResamplerQuality.Low)) { int n = (this.upsample ? 4 : 0) + range_limit((int)quality, (int)SOXResamplerQuality.Medium, (int)SOXResamplerQuality.Very) - (int)SOXResamplerQuality.Medium; if (interp_order < 0) { interp_order = quality > SOXResamplerQuality.High ? 1 : 0; } interp_order = divisor == 1 ? 1 + interp_order : 0; last_stage.divisor = divisor; this.output_stage_num += 2; if (this.upsample && quality == SOXResamplerQuality.Low) { mult = 1; ++this.input_stage_num; --this.output_stage_num; --n; } poly_fir_t f = poly_firs[n]; poly_fir1_t f1 = f.interp[interp_order]; if (last_stage.shared.poly_fir_coefs == null) { int num_taps = 0, phases = divisor == 1 ? (1 << f1.phase_bits) : divisor; double[] coefs = lsx_design_lpf( f.pass, f.stop, 1.0, false, f.att, ref num_taps, phases); //assert(num_taps == f->num_coefs * phases - 1); last_stage.shared.poly_fir_coefs = prepare_coefs(coefs, f.num_coefs, phases, interp_order, mult); //lsx_debug("fir_len=%i phases=%i coef_interp=%i mult=%i size=%s", // f->num_coefs, phases, interp_order, mult, // lsx_sigfigs3((num_taps +1.) * (interp_order + 1) * sizeof(double))); //free(coefs); } last_stage.fn = f1.fn; last_stage.pre_post = f.num_coefs - 1; last_stage.pre = 0; last_stage.preload = last_stage.pre_post >> 1; mult = 1; } if (quality > SOXResamplerQuality.Low) { // typedef struct {int len; double const * h; double bw, a;} filter_t; // static filter_t const filters[] = { // {2 * array_length(half_fir_coefs_low) - 1, half_fir_coefs_low, 0,0}, // {0, NULL, .931, 110}, {0, NULL, .931, 125}, {0, NULL, .931, 170}}; // filter_t const * f = &filters[quality - Low]; int[] fa = new int[] { 0, 110, 125, 170 }; double[] fbw = new double[] { 0.0, 0.931, 0.931, 0.931 }; double a = fa[quality - SOXResamplerQuality.Low]; double att = allow_aliasing ? (34.0 / 33) * a : a; /* negate att degrade */ double bw = bandwidth != 0 ? 1 - (1 - bandwidth / 100) / LSX_TO_3dB : fbw[quality - SOXResamplerQuality.Low]; double min = 1 - (allow_aliasing ? LSX_MAX_TBW0A : LSX_MAX_TBW0) / 100; // assert((size_t)(quality - Low) < array_length(filters)); half_band_filter_init(shared, this.upsample ? 1 : 0, 0, null, bw, att, mult, phase, allow_aliasing); if (this.upsample) { pre_stage.fn = double_sample; /* Finish off setting up pre-stage */ pre_stage.preload = shared.half_band[1].post_peak >> 1; /* Start setting up post-stage; TODO don't use dft for short filters */ if ((1 - this.factor) / (1 - bw) > 2) { half_band_filter_init(shared, 0, 0, null, Math.Max(this.factor, min), att, 1, phase, allow_aliasing); } else { shared.half_band[0] = shared.half_band[1]; } } else if (this.level > 0 && this.output_stage_num > this.level) { double pass = bw * divisor / factor / 2; if ((1 - pass) / (1 - bw) > 2) { half_band_filter_init(shared, 1, 0, null, Math.Max(pass, min), att, 1, phase, allow_aliasing); } } post_stage.fn = half_sample; post_stage.preload = shared.half_band[0].post_peak; } else if (quality == SOXResamplerQuality.Low && !this.upsample) { /* dft is slower here, so */ post_stage.fn = half_sample_low; /* use normal convolution */ post_stage.pre_post = 2 * (half_fir_coefs_low.Length - 1); post_stage.preload = post_stage.pre = post_stage.pre_post >> 1; } if (this.level > 0) { stage_t s = this.stages[this.level]; if (shared.half_band[1].num_taps != 0) { s.fn = half_sample; s.preload = shared.half_band[1].post_peak; s.which = 1; } else { //*s = post_stage this.stages[this.level] = post_stage; // ????????? this.stages[this.level + 2] = s; } } for (i = this.input_stage_num; i <= this.output_stage_num; ++i) { stage_t s = this.stages[i]; if (i > 0 && i < this.level) { s.fn = half_sample_25; s.pre_post = 2 * (half_fir_coefs_25.Length - 1); s.preload = s.pre = s.pre_post >> 1; } s.fifo = new fifo_t(sizeof(double)); s.fifo.reserve(s.preload); // memset(fifo_reserve(&s->fifo, s->preload), 0, sizeof(double)*s->preload); // if (i < this.output_stage_num) // lsx_debug("stage=%-3ipre_post=%-3ipre=%-3ipreload=%i", // i, s->pre_post, s->pre, s->preload); } }
static unsafe void half_sample_low(stage_t p, fifo_t output_fifo) { int num_out = (p.occupancy + 1) / 2; int output_offs = output_fifo.reserve(num_out); fixed (byte* pinput = &p.fifo.data[p.offset], poutput = &output_fifo.data[output_offs]) { double* input = (double*)pinput; double* output = (double*)poutput; for (int i = 0; i < num_out; ++i, input += 2) { double sum = input[0] * half_fir_coefs_low[0]; for (int j = 1; j < half_fir_coefs_low.Length; j++) sum += (input[-j] + input[j]) * half_fir_coefs_low[j]; output[i] = sum; } } p.fifo.read(2 * num_out, null); }
public rate_t(int in_samplerate, int out_samplerate, rate_shared_t shared, double factor, SOXResamplerQuality quality, int interp_order, double phase, double bandwidth, bool allow_aliasing) { this.in_samplerate = in_samplerate; this.out_samplerate = out_samplerate; int i, mult, divisor = 1; //assert(factor > 0); this.factor = factor; if (quality < SOXResamplerQuality.Quick || quality > SOXResamplerQuality.Very) quality = SOXResamplerQuality.High; if (quality != SOXResamplerQuality.Quick) { const int max_divisor = 2048; /* Keep coef table size ~< 500kb */ const double epsilon = 4 / MULT32; /* Scaled to half this at max_divisor */ this.upsample = this.factor < 1; this.level = 0; for (int fi = (int)factor >> 1; fi != 0; fi >>= 1) ++this.level;/* log base 2 */ factor /= 1 << (this.level + (this.upsample ? 0 : 1)); for (i = 2; i <= max_divisor && divisor == 1; ++i) { double try_d = factor * i; int itry = (int)(try_d + .5); if (Math.Abs(itry - try_d) < itry * epsilon * (1 - (.5 / max_divisor) * i)) { if (itry == i) /* Rounded to 1:1? */ { factor = 1; divisor = 2; this.upsample = false; } else { factor = itry; divisor = i; } } } } this.stages = new stage_t[this.level + 4]; // offset by 1!!! + 3? for (i = 0; i < this.level + 4; ++i) this.stages[i] = new stage_t(shared); this.pre_stage = this.stages[0]; this.last_stage = this.stages[this.level + 1]; this.post_stage = this.stages[this.level + 2]; this.last_stage.step = (long)(factor * MULT32 + .5); this.last_stage.out_in_ratio = MULT32 * divisor / this.last_stage.step; //if (divisor != 1) // assert(!last_stage.step.parts.fraction); //else if (quality != Quick) // assert(!last_stage.step.parts.integer); //lsx_debug("i/o=%g; %.9g:%i @ level %i", this.factor, factor, divisor, this.level); mult = 1 + (this.upsample ? 1 : 0); /* Compensate for zero-stuffing in double_sample */ this.input_stage_num = this.upsample ? 0 : 1; this.output_stage_num = this.level + 1; if (quality == SOXResamplerQuality.Quick) { ++this.output_stage_num; last_stage.fn = cubic_spline; last_stage.pre_post = Math.Max(3, (int)(last_stage.step >> 32)); last_stage.preload = last_stage.pre = 1; } else if (last_stage.out_in_ratio != 2 || (this.upsample && quality == SOXResamplerQuality.Low)) { int n = (this.upsample ? 4 : 0) + range_limit((int)quality, (int)SOXResamplerQuality.Medium, (int)SOXResamplerQuality.Very) - (int)SOXResamplerQuality.Medium; if (interp_order < 0) interp_order = quality > SOXResamplerQuality.High ? 1 : 0; interp_order = divisor == 1 ? 1 + interp_order : 0; last_stage.divisor = divisor; this.output_stage_num += 2; if (this.upsample && quality == SOXResamplerQuality.Low) { mult = 1; ++this.input_stage_num; --this.output_stage_num; --n; } poly_fir_t f = poly_firs[n]; poly_fir1_t f1 = f.interp[interp_order]; if (last_stage.shared.poly_fir_coefs == null) { int num_taps = 0, phases = divisor == 1 ? (1 << f1.phase_bits) : divisor; double[] coefs = lsx_design_lpf( f.pass, f.stop, 1.0, false, f.att, ref num_taps, phases); //assert(num_taps == f->num_coefs * phases - 1); last_stage.shared.poly_fir_coefs = prepare_coefs(coefs, f.num_coefs, phases, interp_order, mult); //lsx_debug("fir_len=%i phases=%i coef_interp=%i mult=%i size=%s", // f->num_coefs, phases, interp_order, mult, // lsx_sigfigs3((num_taps +1.) * (interp_order + 1) * sizeof(double))); //free(coefs); } last_stage.fn = f1.fn; last_stage.pre_post = f.num_coefs - 1; last_stage.pre = 0; last_stage.preload = last_stage.pre_post >> 1; mult = 1; } if (quality > SOXResamplerQuality.Low) { // typedef struct {int len; double const * h; double bw, a;} filter_t; // static filter_t const filters[] = { // {2 * array_length(half_fir_coefs_low) - 1, half_fir_coefs_low, 0,0}, // {0, NULL, .931, 110}, {0, NULL, .931, 125}, {0, NULL, .931, 170}}; // filter_t const * f = &filters[quality - Low]; int[] fa = new int[] { 0, 110, 125, 170 }; double[] fbw = new double[] { 0.0, 0.931, 0.931, 0.931 }; double a = fa[quality - SOXResamplerQuality.Low]; double att = allow_aliasing ? (34.0 / 33) * a : a; /* negate att degrade */ double bw = bandwidth != 0 ? 1 - (1 - bandwidth / 100) / LSX_TO_3dB : fbw[quality - SOXResamplerQuality.Low]; double min = 1 - (allow_aliasing ? LSX_MAX_TBW0A : LSX_MAX_TBW0) / 100; // assert((size_t)(quality - Low) < array_length(filters)); half_band_filter_init(shared, this.upsample ? 1 : 0, 0, null, bw, att, mult, phase, allow_aliasing); if (this.upsample) { pre_stage.fn = double_sample; /* Finish off setting up pre-stage */ pre_stage.preload = shared.half_band[1].post_peak >> 1; /* Start setting up post-stage; TODO don't use dft for short filters */ if ((1 - this.factor) / (1 - bw) > 2) half_band_filter_init(shared, 0, 0, null, Math.Max(this.factor, min), att, 1, phase, allow_aliasing); else shared.half_band[0] = shared.half_band[1]; } else if (this.level > 0 && this.output_stage_num > this.level) { double pass = bw * divisor / factor / 2; if ((1 - pass) / (1 - bw) > 2) half_band_filter_init(shared, 1, 0, null, Math.Max(pass, min), att, 1, phase, allow_aliasing); } post_stage.fn = half_sample; post_stage.preload = shared.half_band[0].post_peak; } else if (quality == SOXResamplerQuality.Low && !this.upsample) { /* dft is slower here, so */ post_stage.fn = half_sample_low; /* use normal convolution */ post_stage.pre_post = 2 * (half_fir_coefs_low.Length - 1); post_stage.preload = post_stage.pre = post_stage.pre_post >> 1; } if (this.level > 0) { stage_t s = this.stages[this.level]; if (shared.half_band[1].num_taps != 0) { s.fn = half_sample; s.preload = shared.half_band[1].post_peak; s.which = 1; } else { //*s = post_stage this.stages[this.level] = post_stage; // ????????? this.stages[this.level + 2] = s; } } for (i = this.input_stage_num; i <= this.output_stage_num; ++i) { stage_t s = this.stages[i]; if (i > 0 && i < this.level) { s.fn = half_sample_25; s.pre_post = 2 * (half_fir_coefs_25.Length - 1); s.preload = s.pre = s.pre_post >> 1; } s.fifo = new fifo_t(sizeof(double)); s.fifo.reserve(s.preload); // memset(fifo_reserve(&s->fifo, s->preload), 0, sizeof(double)*s->preload); // if (i < this.output_stage_num) // lsx_debug("stage=%-3ipre_post=%-3ipre=%-3ipreload=%i", // i, s->pre_post, s->pre, s->preload); } }
static unsafe void double_sample(stage_t p, fifo_t output_fifo) { int num_in = Math.Max(0, p.fifo.occupancy); dft_filter_t f = p.shared.half_band[1]; int overlap = f.num_taps - 1; while (num_in > f.dft_length >> 1) { int input_offs = p.fifo.offset; p.fifo.read((f.dft_length - overlap) >> 1, null); num_in -= (f.dft_length - overlap) >> 1; int output_offs = output_fifo.reserve(f.dft_length); output_fifo.trim_by(overlap); fixed (byte* pinput = &p.fifo.data[input_offs]) fixed (byte* poutput = &output_fifo.data[output_offs]) fixed (double* lsx_fft_sc = p.shared.info.lsx_fft_sc) fixed (int* lsx_fft_br = p.shared.info.lsx_fft_br) { double* input = (double*)pinput; double* output = (double*)poutput; for (int j = 0, i = 0; i < f.dft_length; ++j, i += 2) { output[i] = input[j]; output[i + 1] = 0; } SOXFft.rdft(f.dft_length, 1, output, lsx_fft_br, lsx_fft_sc); output[0] *= f.coefs[0]; output[1] *= f.coefs[1]; for (int i = 2; i < f.dft_length; i += 2) { double tmp = output[i]; output[i] = f.coefs[i] * tmp - f.coefs[i + 1] * output[i + 1]; output[i + 1] = f.coefs[i + 1] * tmp + f.coefs[i] * output[i + 1]; } SOXFft.rdft(f.dft_length, -1, output, lsx_fft_br, lsx_fft_sc); } } }
static unsafe void half_sample(stage_t p, fifo_t output_fifo) { int num_in = Math.Max(0, p.fifo.occupancy); dft_filter_t f = p.shared.half_band[p.which]; int overlap = f.num_taps - 1; while (num_in >= f.dft_length) { int input_offs = p.fifo.offset; p.fifo.read(f.dft_length - overlap, null); num_in -= f.dft_length - overlap; int output_offs = output_fifo.reserve(f.dft_length); output_fifo.trim_by((f.dft_length + overlap) >> 1); Buffer.BlockCopy(p.fifo.data, input_offs, output_fifo.data, output_offs, f.dft_length * sizeof(double)); fixed (byte* poutput = &output_fifo.data[output_offs]) fixed (double* lsx_fft_sc = p.shared.info.lsx_fft_sc) fixed (int* lsx_fft_br = p.shared.info.lsx_fft_br) { double* output = (double*)poutput; SOXFft.rdft(f.dft_length, 1, output, lsx_fft_br, lsx_fft_sc); output[0] *= f.coefs[0]; output[1] *= f.coefs[1]; for (int i = 2; i < f.dft_length; i += 2) { double tmp = output[i]; output[i] = f.coefs[i] * tmp - f.coefs[i + 1] * output[i + 1]; output[i + 1] = f.coefs[i + 1] * tmp + f.coefs[i] * output[i + 1]; } SOXFft.rdft(f.dft_length, -1, output, lsx_fft_br, lsx_fft_sc); for (int j = 1, i = 2; i < f.dft_length - overlap; ++j, i += 2) output[j] = output[i]; } } }