/* Resampling library * Copyright (C) <2001> David A. Schleef <ds@schleef.org> * * This library is free software; you can redistribute it and/or * modify it under the terms of the GNU Library General Public * License as published by the Free Software Foundation; either * version 2 of the License, or any later version. * * This library is distributed in the hope that it will be useful, * but WITHOUT ANY WARRANTY; without even the implied warranty of * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU * Library General Public License for more details. * * You should have received a copy of the GNU Library General Public * License along with this library; if not, write to the * Free Software Foundation, Inc., 59 Temple Place - Suite 330, * Boston, MA 02111-1307, USA. */ #ifdef HAVE_CONFIG_H #include "config.h" #endif #include <string.h> #include <math.h> #include <stdio.h> #include <stdlib.h> #include "private.h" double functable_sinc(void *p,double x) { if(x==0)return 1; return sin(x)/x; } double functable_dsinc(void *p,double x) { if(x==0)return 0; return cos(x)/x - sin(x)/(x*x); } double functable_window_boxcar(void *p,double x) { if(x<-1 || x>1)return 0; return 1; } double functable_window_dboxcar(void *p,double x) { return 0; } double functable_window_std(void *p,double x) { if(x<-1 || x>1)return 0; return (1-x*x)*(1-x*x); } double functable_window_dstd(void *p,double x) { if(x<-1 || x>1)return 0; return -4*x*(1-x*x); } void functable_init(functable_t *t) { int i; double x; t->fx = malloc(sizeof(double)*(t->len+1)); t->fdx = malloc(sizeof(double)*(t->len+1)); t->invoffset = 1.0 / t->offset; for(i=0;i<t->len+1;i++){ x = t->start + t->offset * i; x *= t->scale; t->fx[i] = t->func_x(t->priv,x); t->fdx[i] = t->scale * t->func_dx(t->priv,x); } if(t->func2_x){ double f1x,f1dx; double f2x,f2dx; for(i=0;i<t->len+1;i++){ x = t->start + t->offset * i; x *= t->scale2; f2x = t->func2_x(t->priv,x); f2dx = t->scale2 * t->func2_dx(t->priv,x); f1x = t->fx[i]; f1dx = t->fdx[i]; t->fx[i] = f1x * f2x; t->fdx[i] = f1x * f2dx + f1dx * f2x; } } } double functable_eval(functable_t *t,double x) { int i; double f0, f1, w0, w1; double x2, x3; double w; if(x<t->start || x>(t->start+(t->len+1)*t->offset)){ printf("x out of range %g\n",x); } x -= t->start; x /= t->offset; i = floor(x); x -= i; x2 = x * x; x3 = x2 * x; f1 = 3 * x2 - 2 * x3; f0 = 1 - f1; w0 = (x - 2 * x2 + x3) * t->offset; w1 = (-x2 + x3) * t->offset; /*printf("i=%d x=%g f0=%g f1=%g w0=%g w1=%g\n",i,x,f0,f1,w0,w1); */ w = t->fx[i] * f0 + t->fx[i + 1] * f1 + t->fdx[i] * w0 + t->fdx[i + 1] * w1; /*w = t->fx[i] * (1-x) + t->fx[i+1] * x; */ return w; } double functable_fir(functable_t *t, double x, int n, double *data, int len) { int i,j; double f0, f1, w0, w1; double x2, x3; double w; double sum; x -= t->start; x /= t->offset; i = floor(x); x -= i; x2 = x * x; x3 = x2 * x; f1 = 3 * x2 - 2 * x3; f0 = 1 - f1; w0 = (x - 2 * x2 + x3) * t->offset; w1 = (-x2 + x3) * t->offset; sum = 0; for(j=0;j<len;j++){ w = t->fx[i] * f0 + t->fx[i + 1] * f1 + t->fdx[i] * w0 + t->fdx[i + 1] * w1; sum += data[j*2] * w; i += n; } return sum; } void functable_fir2(functable_t *t, double *r0, double *r1, double x, int n, double *data, int len) { int i,j; double f0, f1, w0, w1; double x2, x3; double w; double sum0, sum1; double floor_x; x -= t->start; x *= t->invoffset; floor_x = floor(x); i = floor_x; x -= floor_x; x2 = x * x; x3 = x2 * x; f1 = 3 * x2 - 2 * x3; f0 = 1 - f1; w0 = (x - 2 * x2 + x3) * t->offset; w1 = (-x2 + x3) * t->offset; sum0 = 0; sum1 = 0; for(j=0;j<len;j++){ w = t->fx[i] * f0 + t->fx[i + 1] * f1 + t->fdx[i] * w0 + t->fdx[i + 1] * w1; sum0 += data[j*2] * w; sum1 += data[j*2+1] * w; i += n; #define unroll2 #define unroll3 #define unroll4 #ifdef unroll2 j++; w = t->fx[i] * f0 + t->fx[i + 1] * f1 + t->fdx[i] * w0 + t->fdx[i + 1] * w1; sum0 += data[j*2] * w; sum1 += data[j*2+1] * w; i += n; #endif #ifdef unroll3 j++; w = t->fx[i] * f0 + t->fx[i + 1] * f1 + t->fdx[i] * w0 + t->fdx[i + 1] * w1; sum0 += data[j*2] * w; sum1 += data[j*2+1] * w; i += n; #endif #ifdef unroll4 j++; w = t->fx[i] * f0 + t->fx[i + 1] * f1 + t->fdx[i] * w0 + t->fdx[i + 1] * w1; sum0 += data[j*2] * w; sum1 += data[j*2+1] * w; i += n; #endif } *r0 = sum0; *r1 = sum1; } #ifdef unused void functable_fir2_altivec(functable_t *t, float *r0, float *r1, double x, int n, float *data, int len) { int i,j; double f0, f1, w0, w1; double x2, x3; double w; double sum0, sum1; double floor_x; x -= t->start; x *= t->invoffset; floor_x = floor(x); i = floor_x; x -= floor_x; x2 = x * x; x3 = x2 * x; f1 = 3 * x2 - 2 * x3; f0 = 1 - f1; w0 = (x - 2 * x2 + x3) * t->offset; w1 = (-x2 + x3) * t->offset; sum0 = 0; sum1 = 0; for(j=0;j<len;j++){ /* t->fx, t->fdx needs to be multiplexed by n */ /* we need 5 consecutive floats, which fit into 2 vecs */ /* load v0, t->fx[i] */ /* load v1, t->fx[i+n] */ /* v2 = v0 (not correct) */ /* v3 = (v0>>32) || (v1<<3*32) (not correct) */ /* */ /* load v4, t->dfx[i] */ /* load v5, t->dfx[i+n] */ /* v6 = v4 (not correct) */ /* v7 = (v4>>32) || (v5<<3*32) (not correct) */ /* */ /* v8 = splat(f0) */ /* v9 = splat(f1) */ /* v10 = splat(w0) */ /* v11 = splat(w1) */ /* */ /* v12 = v2 * v8 */ /* v12 += v3 * v9 */ /* v12 += v6 * v10 */ /* v12 += v7 * v11 */ w = t->fx[i] * f0 + t->fx[i + 1] * f1 + t->fdx[i] * w0 + t->fdx[i + 1] * w1; /* v13 = data[j*2] */ /* v14 = data[j*2+4] */ /* v15 = deinterlace_high(v13,v14) */ /* v16 = deinterlace_low(v13,v14) */ /* (sum0) v17 += multsum(v13,v15) */ /* (sum1) v18 += multsum(v14,v16) */ sum0 += data[j*2] * w; sum1 += data[j*2+1] * w; i += n; } *r0 = sum0; *r1 = sum1; } #endif