convolution.c

su 的源代码库

/* Copyright (c) Colorado School of Mines, 2006.*/
/* All rights reserved.                       */

/*********************** self documentation **********************/
/*****************************************************************************
CONVOLUTION - Compute z = x convolved with y

conv	compute the convolution of two input vector arrays

******************************************************************************
Input:
lx		length of x array
ifx		sample index of first x
x		array[lx] to be convolved with y
ly		length of y array
ify		sample index of first y
y		array[ly] with which x is to be convolved
lz		length of z array
ifz		sample index of first z

Output:
z		array[lz] containing x convolved with y

******************************************************************************
Function Prototype:
void conv (int lx, int ifx, float *x, int ly, int ify, float *y,
	int lz, int ifz, float *z);

******************************************************************************
Notes:
The operation z = x convolved with y is defined to be
           ifx+lx-1
    z[i] =   sum    x[j]*y[i-j]  ;  i = ifz,...,ifz+lz-1
            j=ifx
The x samples are contained in x[0], x[1], ..., x[lx-1]; likewise for
the y and z samples.  The sample indices of the first x, y, and z values
determine the location of the origin for each array.  For example, if
z is to be a weighted average of the nearest 5 samples of y, one might
use 
	...
	x[0] = x[1] = x[2] = x[3] = x[4] = 1.0/5.0;
	conv(5,-2,x,lx,0,y,ly,0,z);
	...
In this example, the filter x is symmetric, with index of first sample = -2.

This function is optimized for architectures that can simultaneously perform
a multiply, add, and one load from memory; e.g., the IBM RISC System/6000.
Because, for each value of i, it accumulates the convolution sum z[i] in a
scalar, this function is not likely to be optimal for vector architectures.

******************************************************************************
Author:  Dave Hale, Colorado School of Mines, 11/23/91
*****************************************************************************/
/**************** end self doc ********************************/

#include "cwp.h"

/* prototype of function used internally */
static void convs(int,int,float*,int,int,float*,int,int,float*);

void conv (int lx, int ifx, float *x,
	   int ly, int ify, float *y,
	   int lz, int ifz, float *z)
/*****************************************************************************
Compute z = x convolved with y; i.e.,

           ifx+lx-1
    z[i] =   sum    x[j]*y[i-j]  ;  i = ifz,...,ifz+lz-1
            j=ifx
******************************************************************************
Input:
lx		length of x array
ifx		sample index of first x
x		array[lx] to be convolved with y
ly		length of y array
ify		sample index of first y
y		array[ly] with which x is to be convolved
lz		length of z array
ifz		sample index of first z

Output:
z		array[lz] containing x convolved with y
******************************************************************************
Notes:
The x samples are contained in x[0], x[1], ..., x[lx-1]; likewise for
the y and z samples.  The sample indices of the first x, y, and z values
determine the location of the origin for each array.  For example, if
z is to be a weighted average of the nearest 5 samples of y, one might
use 
	...
	x[0] = x[1] = x[2] = x[3] = x[4] = 1.0/5.0;
	conv(5,-2,x,lx,0,y,ly,0,z);
	...
In this example, the filter x is symmetric, with index of first sample = -2.

This function is optimized for architectures that can simultaneously perform
a multiply, add, and one load from memory; e.g., the IBM RISC System/6000.
Because, for each value of i, it accumulates the convolution sum z[i] in a
scalar, this function is not likely to be optimal for vector architectures.
******************************************************************************
Author:  Dave Hale, Colorado School of Mines, 11/23/91
*****************************************************************************/
#ifdef SIMPLE_CONV
{
	int ilx=ifx+lx-1,ily=ify+ly-1,ilz=ifz+lz-1,i,j,jlow,jhigh;
	float sum;
	
	x -= ifx;  y -= ify;  z -= ifz;
	for (i=ifz; i<=ilz; ++i) {
		jlow = i-ily;  if (jlow<ifx) jlow = ifx;
		jhigh = i-ify;  if (jhigh>ilx) jhigh = ilx;
		for (j=jlow,sum=0.0; j<=jhigh; ++j)
			sum += x[j]*y[i-j];
		z[i] = sum;
	}
}
#else
{
	int ilx=ifx+lx-1,ily=ify+ly-1,ilz=ifz+lz-1,
		i,j,ilow,ihigh,jlow,jhigh;
	float sa,sb,xa,xb,ya,yb,*t;

	/* if x is longer than y, swap x and y */
	if (lx>ly) {
		i = ifx;  ifx = ify;  ify = i;
		i = ilx;  ilx = ily;  ily = i;
		i = lx;  lx = ly;  ly = i;
		t = x;  x = y;  y = t;
	}
	
	/* handle short x with special code */
	if (lx>=1 && lx<=30) {
		convs(lx,ifx,x,ly,ify,y,lz,ifz,z);
		return;
	}
	
	/* adjust pointers for indices of first samples */
	x -= ifx;
	y -= ify;
	z -= ifz;
		
	/* OFF LEFT:  i < ify+ifx */
	
	/* zero output for all i */
	ilow = ifz;
	ihigh = ify+ifx-1;  if (ihigh>ilz) ihigh = ilz;
	for (i=ilow; i<=ihigh; ++i)
		z[i] = 0.0;

	/* ROLLING ON:  ify+ifx <= i < ify+ilx */
	
	/* if necessary, do one i so that number of j in overlap is odd */
	if (i<ify+ilx && i<=ilz) {
		jlow = ifx;
		jhigh = i-ify;
		if ((jhigh-jlow)%2) {
			sa = 0.0;
			for (j=jlow; j<=jhigh; ++j)
				sa += x[j]*y[i-j];
			z[i++] = sa;
		}
	}
	
	/* loop over pairs of i and j */
	ilow = i;
	ihigh = ilx+ify-1;  if (ihigh>ilz) ihigh = ilz;
	jlow = ifx;
	jhigh = ilow-ify;
	for (i=ilow; i<ihigh; i+=2,jhigh+=2) {
		sa = sb = 0.0;
		xb = x[jhigh+1];
		yb = 0.0;
		for (j=jhigh; j>=jlow; j-=2) {
			sa += xb*yb;
			ya = y[i-j];
			sb += xb*ya;
			xa = x[j];
			sa += xa*ya;
			yb = y[i+1-j];
			sb += xa*yb;
			xb = x[j-1];
		}
		z[i] = sa;
		z[i+1] = sb;
	}
	
	/* if number of i is odd */
	if (i==ihigh) {
		jlow = ifx;
		jhigh = i-ify;
		sa = 0.0;
		for (j=jlow; j<=jhigh; ++j)
			sa += x[j]*y[i-j];
		z[i++] = sa;
	}
	
	/* MIDDLE:  ify+ilx <= i <= ily+ifx */
	
	/* determine limits for i and j */
	ilow = i;
	ihigh = ily+ifx;  if (ihigh>ilz) ihigh = ilz;
	jlow = ifx;
	jhigh = ilx;
	
	/* if number of j is even, do j in pairs with no leftover */
	if ((jhigh-jlow)%2) {
		for (i=ilow; i<ihigh; i+=2) {
			sa = sb = 0.0;
			yb = y[i+1-jlow];
			xa = x[jlow];
			for (j=jlow; j<jhigh; j+=2) {
				sb += xa*yb;
				ya = y[i-j];
				sa += xa*ya;
				xb = x[j+1];
				sb += xb*ya;
				yb = y[i-1-j];
				sa += xb*yb;
				xa = x[j+2];
			}
			z[i] = sa;
			z[i+1] = sb;
		}
	
	/* else, number of j is odd, so do j in pairs with leftover */
	} else {
		for (i=ilow; i<ihigh; i+=2) {
			sa = sb = 0.0;
			yb = y[i+1-jlow];
			xa = x[jlow];
			for (j=jlow; j<jhigh; j+=2) {
				sb += xa*yb;
				ya = y[i-j];
				sa += xa*ya;
				xb = x[j+1];
				sb += xb*ya;
				yb = y[i-1-j];
				sa += xb*yb;
				xa = x[j+2];
			}
			z[i] = sa+x[jhigh]*y[i-jhigh];
			z[i+1] = sb+x[jhigh]*y[i+1-jhigh];
		}
	}
	
	/* if number of i is odd */
	if (i==ihigh) {
		sa = 0.0;
		for (j=jlow; j<=jhigh; ++j)
			sa += x[j]*y[i-j];
		z[i++] = sa;
	}

	/* ROLLING OFF:  ily+ifx < i <= ily+ilx */
	
	/* if necessary, do one i so that number of j in overlap is even */
	if (i<=ily+ilx && i<=ilz) {
		jlow = i-ily;
		jhigh = ilx;
		if (!((jhigh-jlow)%2)) {
			sa = 0.0;
			for (j=jlow; j<=jhigh; ++j)
				sa += x[j]*y[i-j];
			z[i++] = sa;
		}
	}
	
	/* number of j is now even, so loop over both i and j in pairs */
	ilow = i;
	ihigh = ily+ilx;  if (ihigh>ilz) ihigh = ilz;
	jlow = ilow-ily;
	jhigh = ilx-2; /* Dave's new patch */
        for (i=ilow; i<ihigh; i+=2,jlow+=2) {
                sa = sb = 0.0;
                xa = x[jlow];
                yb = 0.0;
                for (j=jlow; j<jhigh; j+=2) {
                        sb += xa*yb;
                        ya = y[i-j];
                        sa += xa*ya;
                        xb = x[j+1];
                        sb += xb*ya;
                        yb = y[i-1-j];
                        sa += xb*yb;
                        xa = x[j+2];
                }
                sb += xa*yb;
                ya = y[i-j];
                sa += xa*ya;
                xb = x[j+1];
                sb += xb*ya;
                yb = y[i-1-j];
                sa += xb*yb;
                z[i] = sa;
                z[i+1] = sb;
        }
	
	/* if number of i is odd */
	if (i==ihigh) {
		jlow = i-ily;
		jhigh = ilx;
		sa = 0.0;
		for (j=jlow; j<=jhigh; ++j)
			sa += x[j]*y[i-j];
		z[i++] = sa;
	}
	
	/* OFF RIGHT:  ily+ilx < i */
	
	/* zero output for all i */
	ilow = i;
	ihigh = ilz;
	for (i=ilow; i<=ihigh; ++i)
		z[i] = 0.0;
}

/* internal function optimized for short x */
static void convs (int lx, int ifx, float *x,
	   int ly, int ify, float *y, 
	   int lz, int ifz, float *z)
{
	int ilx=ifx+lx-1,ily=ify+ly-1,ilz=ifz+lz-1,
		i,j,ilow,ihigh,jlow,jhigh;
	float x0,x1,x2,x3,x4,x5,x6,x7,x8,x9,
		x10,x11,x12,x13,x14,x15,x16,x17,x18,x19,
		x20,x21,x22,x23,x24,x25,x26,x27,x28,x29,
		ya,yb,z0,z1,sum;
	
	x -= ifx;
	y -= ify;
	z -= ifz;
	
		
	/* OFF LEFT:  i < ifx+ify */
	ilow = ifz;
	ihigh = ify+ifx-1;  if (ihigh>ilz) ihigh = ilz;
	for (i=ilow; i<=ihigh; ++i)
		z[i] = 0.0;
	
	/* ROLLING ON:  ify+ifx <= i < ify+ilx */
	ilow = ify+ifx;  if (ilow<ifz) ilow = ifz;
	ihigh = ify+ilx-1;  if (ihigh>ilz) ihigh = ilz;
	jlow = ifx;
	jhigh = ilow-ify;
	for (i=ilow; i<=ihigh; ++i,++jhigh) {
		for (j=jlow,sum=0.0; j<=jhigh; ++j)
			sum += x[j]*y[i-j];
		z[i] = sum;
	}
	
	/* MIDDLE:  ify+ilx <= i <= ily+ifx */
	ilow = ify+ilx;  if (ilow<ifz) ilow = ifz;
	ihigh = ily+ifx;  if (ihigh>ilz) ihigh = ilz;
	if (lx==1) {
		x0 = x[ifx];
		for (i=ilow; i<=ihigh-1; i+=2) {
			ya = y[i+1-ifx];  z1 = x0*ya;
			yb = y[i-ifx];  z0 = x0*yb;
			z[i+1] = z1;
			z[i] = z0;
		}
	} else if (lx==2) {
		x0 = x[ifx];
		x1 = x[ifx+1];
		for (i=ilow; i<=ihigh-1; i+=2) {
			ya = y[i+1-ifx];  z1 = x0*ya;
			yb = y[i-ifx];  z0 = x0*yb;  z1 += x1*yb;
			ya = y[i-ifx-1];  z0 += x1*ya;
			z[i+1] = z1;
			z[i] = z0;
		}
	} else if (lx==3) {
		x0 = x[ifx];
		x1 = x[ifx+1];
		x2 = x[ifx+2];
		for (i=ilow; i<=ihigh-1; i+=2) {
			ya = y[i+1-ifx];  z1 = x0*ya;
			yb = y[i-ifx];  z0 = x0*yb;  z1 += x1*yb;
			ya = y[i-ifx-1];  z0 += x1*ya;  z1 += x2*ya;
			yb = y[i-ifx-2];  z0 += x2*yb;
			z[i+1] = z1;
			z[i] = z0;
		}
	} else if (lx==4) {
		x0 = x[ifx];
		x1 = x[ifx+1];
		x2 = x[ifx+2];
		x3 = x[ifx+3];
		for (i=ilow; i<=ihigh-1; i+=2) {
			ya = y[i+1-ifx];  z1 = x0*ya;
			yb = y[i-ifx];  z0 = x0*yb;  z1 += x1*yb;
			ya = y[i-ifx-1];  z0 += x1*ya;  z1 += x2*ya;
			yb = y[i-ifx-2];  z0 += x2*yb;  z1 += x3*yb;
			ya = y[i-ifx-3];  z0 += x3*ya;
			z[i+1] = z1;
			z[i] = z0;
		}
	} else if (lx==5) {
		x0 = x[ifx];
		x1 = x[ifx+1];
		x2 = x[ifx+2];
		x3 = x[ifx+3];
		x4 = x[ifx+4];
		for (i=ilow; i<=ihigh-1; i+=2) {
			ya = y[i+1-ifx];  z1 = x0*ya;
			yb = y[i-ifx];  z0 = x0*yb;  z1 += x1*yb;
			ya = y[i-ifx-1];  z0 += x1*ya;  z1 += x2*ya;
			yb = y[i-ifx-2];  z0 += x2*yb;  z1 += x3*yb;
			ya = y[i-ifx-3];  z0 += x3*ya;  z1 += x4*ya;
			yb = y[i-ifx-4];  z0 += x4*yb;
			z[i+1] = z1;
			z[i] = z0;
		}
	} else if (lx==6) {
		x0 = x[ifx];
		x1 = x[ifx+1];
		x2 = x[ifx+2];
		x3 = x[ifx+3];
		x4 = x[ifx+4];
		x5 = x[ifx+5];
		for (i=ilow; i<=ihigh-1; i+=2) {
			ya = y[i+1-ifx];  z1 = x0*ya;
			yb = y[i-ifx];  z0 = x0*yb;  z1 += x1*yb;
			ya = y[i-ifx-1];  z0 += x1*ya;  z1 += x2*ya;
			yb = y[i-ifx-2];  z0 += x2*yb;  z1 += x3*yb;
			ya = y[i-ifx-3];  z0 += x3*ya;  z1 += x4*ya;
			yb = y[i-ifx-4];  z0 += x4*yb;  z1 += x5*yb;
			ya = y[i-ifx-5];  z0 += x5*ya;
			z[i+1] = z1;
			z[i] = z0;
		}
	} else if (lx==7) {
		x0 = x[ifx];
		x1 = x[ifx+1];
		x2 = x[ifx+2];
		x3 = x[ifx+3];
		x4 = x[ifx+4];
		x5 = x[ifx+5];
		x6 = x[ifx+6];
		for (i=ilow; i<=ihigh-1; i+=2) {
			ya = y[i+1-ifx];  z1 = x0*ya;
			yb = y[i-ifx];  z0 = x0*yb;  z1 += x1*yb;
			ya = y[i-ifx-1];  z0 += x1*ya;  z1 += x2*ya;
			yb = y[i-ifx-2];  z0 += x2*yb;  z1 += x3*yb;
			ya = y[i-ifx-3];  z0 += x3*ya;  z1 += x4*ya;
			yb = y[i-ifx-4];  z0 += x4*yb;  z1 += x5*yb;
			ya = y[i-ifx-5];  z0 += x5*ya;  z1 += x6*ya;
			yb = y[i-ifx-6];  z0 += x6*yb;
			z[i+1] = z1;
			z[i] = z0;
		}
	} else if (lx==8) {
		x0 = x[ifx];
		x1 = x[ifx+1];
		x2 = x[ifx+2];
		x3 = x[ifx+3];
		x4 = x[ifx+4];
		x5 = x[ifx+5];
		x6 = x[ifx+6];
		x7 = x[ifx+7];
		for (i=ilow; i<=ihigh-1; i+=2) {
			ya = y[i+1-ifx];  z1 = x0*ya;
			yb = y[i-ifx];  z0 = x0*yb;  z1 += x1*yb;
			ya = y[i-ifx-1];  z0 += x1*ya;  z1 += x2*ya;
			yb = y[i-ifx-2];  z0 += x2*yb;  z1 += x3*yb;
			ya = y[i-ifx-3];  z0 += x3*ya;  z1 += x4*ya;
			yb = y[i-ifx-4];  z0 += x4*yb;  z1 += x5*yb;
			ya = y[i-ifx-5];  z0 += x5*ya;  z1 += x6*ya;
			yb = y[i-ifx-6];  z0 += x6*yb;  z1 += x7*yb;
			ya = y[i-ifx-7];  z0 += x7*ya;
			z[i+1] = z1;
			z[i] = z0;
		}
	} else if (lx==9) {
		x0 = x[ifx];
		x1 = x[ifx+1];
		x2 = x[ifx+2];
		x3 = x[ifx+3];
		x4 = x[ifx+4];
		x5 = x[ifx+5];
		x6 = x[ifx+6];
		x7 = x[ifx+7];
		x8 = x[ifx+8];
		for (i=ilow; i<=ihigh-1; i+=2) {
			ya = y[i+1-ifx];  z1 = x0*ya;
			yb = y[i-ifx];  z0 = x0*yb;  z1 += x1*yb;
			ya = y[i-ifx-1];  z0 += x1*ya;  z1 += x2*ya;
			yb = y[i-ifx-2];  z0 += x2*yb;  z1 += x3*yb;
			ya = y[i-ifx-3];  z0 += x3*ya;  z1 += x4*ya;
			yb = y[i-ifx-4];  z0 += x4*yb;  z1 += x5*yb;
			ya = y[i-ifx-5];  z0 += x5*ya;  z1 += x6*ya;
			yb = y[i-ifx-6];  z0 += x6*yb;  z1 += x7*yb;
			ya = y[i-ifx-7];  z0 += x7*ya;  z1 += x8*ya;
			yb = y[i-ifx-8];  z0 += x8*yb;
			z[i+1] = z1;
			z[i] = z0;
		}
	} else if (lx==10) {
		x0 = x[ifx];
		x1 = x[ifx+1];
		x2 = x[ifx+2];
		x3 = x[ifx+3];
		x4 = x[ifx+4];
		x5 = x[ifx+5];
		x6 = x[ifx+6];
		x7 = x[ifx+7];
		x8 = x[ifx+8];
		x9 = x[ifx+9];
		for (i=ilow; i<=ihigh-1; i+=2) {
			ya = y[i+1-ifx];  z1 = x0*ya;
			yb = y[i-ifx];  z0 = x0*yb;  z1 += x1*yb;
			ya = y[i-ifx-1];  z0 += x1*ya;  z1 += x2*ya;
			yb = y[i-ifx-2];  z0 += x2*yb;  z1 += x3*yb;
			ya = y[i-ifx-3];  z0 += x3*ya;  z1 += x4*ya;
			yb = y[i-ifx-4];  z0 += x4*yb;  z1 += x5*yb;
			ya = y[i-ifx-5];  z0 += x5*ya;  z1 += x6*ya;
			yb = y[i-ifx-6];  z0 += x6*yb;  z1 += x7*yb;
			ya = y[i-ifx-7];  z0 += x7*ya;  z1 += x8*ya;
			yb = y[i-ifx-8];  z0 += x8*yb;  z1 += x9*yb;
			ya = y[i-ifx-9];  z0 += x9*ya;
			z[i+1] = z1;
			z[i] = z0;