matrix.c

一个很好的分子动力学程序

#include <math.h>
#include <stdio.h>
#include <stdlib.h>

void setidentity (float *a, int n, int lrow) {
	int i, j;
	float *ai, *aij;
	ai = a;
	for (i=0;i<n;i++,ai+=lrow) {
		aij = ai;
		for (j=0;j<i;j++)
			*(aij++) = 0.0;
		*(aij++) = 1.0;
		for (j=i+1;j<n;j++)
			*(aij++) = 0.0;
	}
}

void matrixmult (float *ares, float *a1, float *a2, int n, int nrow) {
	int i,j,k;
	float t, *ari, *a1i, *arij, *a2kj, *a1ik;
	ari = ares;
	a1i = a1;
	for (i=0;i<n;i++,ari+=nrow,a1i+=nrow) {
		arij = ari;
		for (j=0;j<n;j++) {
			t = 0.0;
			a1ik = a1i;
			a2kj = a2+j;
			for (k=0;k<n;k++,a2kj+=nrow)
				t += *(a1ik++) * *a2kj;
			*(arij++) = t;
		}
	}
}

void transpose (float *a, int n, int nrow) {
	float t, *aii, *aij, *aji;
	int i,j, n1;
	n1 = nrow+1;
	aii = a;
	for (i=0;i<n-1;i++,aii+=n1) {
		aij = aii+1;
		aji = aii+nrow;
		for (j=i+1;j<n;j++,aji+=nrow,aij++) {
			t = *aji;
			*aji = *aij;
			*aij = t;
		}
	}
}

/*  LU DECOMPOSITION 		 */
/*  FROM NUMERICAL RECIPES  */
void ludcmp (float *a, int n, int nrow, int *indx, float *d) {
	#define TINY 1.0e-20
	int k,j,imax,i;
	float t,sum,dum,big, *vv, *ai, *aj, *ak, *aimax;

	vv = (float *) malloc (n*sizeof(float));

	*d = 1.0;
	ai = a;
	for (i=0;i<n;i++,ai+=nrow) {
		big = 0.0;
		for (j=0;j<n;j++) {
			if ( (t=fabs(ai[j])) > big)
				big = t;
		}
		if (big==0.0) {
			printf ("ERROR in LUDCMP - Matrix is singular.\n");
			exit (1);
		}
		vv[i] = 1.0 / big;
	}

	aj = a;
	for (j=0;j<n;j++,aj+=nrow) {
		if (j>0) {
			ai = a;
			for (i=0;i<j;i++,ai+=nrow) {
				sum = ai[j];
				if (i>0) {
					ak = a;
					for (k=0;k<i;k++,ak+=nrow)
						sum -= ai[k]*ak[j];
					ai[j] = sum;
				}
			}
		}

		big = 0.0;
		ai = a + j*nrow;
		for (i=j;i<n;i++,ai+=nrow) {
			sum = ai[j];
			if (j>0) {
				ak = a;
				for (k=0;k<j;k++,ak+=nrow)
					sum -= ai[k]*ak[j];
				ai[j] = sum;
			}
			dum = vv[i]*fabs(sum);
			if (dum > big) {
				big = dum;
				imax = i;
			}
		}

		if (j != imax) {
			aimax = a + imax*nrow;
			ak 	= a;
			for (k=0;k<n;k++,ak+=nrow) {
				dum = aimax[k];
				aimax[k] = aj[k];
				aj[k] = dum;
			}
			*d = - *d;
			vv[imax] = vv[j];
		}

		indx[j] = imax;
		if (j != n-1) {
			if (aj[j]==0.0)
				 aj[j] = TINY;
			dum = 1.0/aj[j];
			ai = a + (j+1)*nrow;
			for (i=j+1;i<n;i++,ai+=nrow)
				ai[j] *= dum;
		}
	}
	ai = a + n*nrow-1;
	if (*ai==0.0)
	  *ai = TINY;

	free (vv);
}

/*  BACKSUBSTITUTION 		  */
/*  FROM NUMBERICAL RECIPES  */
void lubksb (float *a, int n, int nrow, int *indx, float *b) {
	int i,ii,ip,j;
	float sum, *ai;

	ii = -1;
	ai = a;
	for (i=0;i<n;i++,ai+=nrow) {
		ip = indx[i];
		sum = b[ip];
		b[ip] = b[i];
		if (ii !=-1) {
			for (j=ii;j<i;j++)
				sum -= ai[j]*b[j];
		}
		else if (sum!=0.0)
			ii = i;
		b[i] = sum;
	}
	ai = a + (n-1)*nrow;
	for (i=n-1;i>=0;i--,ai-=nrow) {
		sum = b[i];
		if (i<n-1)
			for (j=i+1;j<n;j++)
				sum -= ai[j] * b[j];
		b[i] = sum/ai[i];
	}
}

void matrixinv (float *a, int n, int lrow) {
	float *y, *yt, *ai, *aji, d;
	int	i,j,*indx;
	/*  ALLOCATE SPACE FOR ARRAYS  */
	y	  = (float *) malloc (n*n*sizeof(float));
	indx = (int   *) malloc (n*sizeof(int));
	/*  MATRIX DECOMPOSITION  */
	ludcmp (a, n, lrow, indx, &d);
	/*  SET Y TO I  */
	setidentity (y, n, n);
	yt = y;
	/*  BACK SUBSTITUTION  */
	for (i=0;i<n;i++,yt+=n)
		lubksb (a, n, lrow, indx, yt);	/*  NOTE: this may not work if matrix */
	/*  TRANSPOSE Y BACK INTO A  */		/*  a and matrix y are not allocated  */
	yt = y;										/*  with same row size					  */
	ai = a;
	for (i=0;i<n;i++,ai++) {
		aji = ai;
		for (j=0;j<n;j++,aji+=lrow)
			*aji = *(yt++);
	}
	/* RELEASE STORAGE  */
	free (indx);
	free (y);
}

float matrixdet (float *a, int n, int lrow) {
	float d, *b, *ai, *aij, *bi, *bij;
	int	i,j, *indx, nrow1;
	/*  ALLOCATE SPACE FOR ARRAYS  */
	b	  = (float *) malloc (n*n*sizeof(float));
	indx = (int   *) malloc (n*sizeof(int));
	/*  COPY INPUT MATRIX INTO B	*/
	for (i=0,ai=a,  bi=b;	i<n; i++,ai+=lrow,bi+=n)
	for (j=0,aij=ai,bij=bi; j<n; j++ 				  )
		*(bij++) = *(aij++);
	/*  MATRIX DECOMPOSITION  */
	ludcmp (b, n, lrow, indx, &d);
	nrow1 = lrow+1;
	bij = b;
	for (i=0;i<n;i++,bij+=nrow1)
		d *= *bij;
	/* RELEASE STORAGE  */
	free (indx);
	free (b);
	return(d);
}

void	matvecmul	(float *vres, float *a, float *v, int n, int nrow) {
	int i,j;
	float *vri, *vi, *ai, *aij;
	i	 = n;
	ai  = a; 	 // row pointer to first matrix row
	vri = vres;
	while (i--) {
		*vri = 0;
		vi   = v;
		aij  = ai;	  // element pointer to first row element
		j	  = n;
		while (j--)
			*vri += *(aij++) * *(vi++);
		vri++;
		ai += nrow;   // move row pointer to next row
	}
}

void writematrix (float *a, int n, int nrow) {
	int i,j;
	for (i=0;i<n;i++) {
		for (j=0;j<n;j++)
			printf ("%10.4f", a[i*nrow+j]);
		printf ("\n");
	}
}

/*   EIGENVECTORS OF SYMETRIC MATRIX					  */
/* 	 a 	 input matrix									  */
/* 	 n 	 size of matrix								  */
/* 	 ncol  number of columns in storage 			  */
/* 	 d 	 eigenvalues									  */
/* 	 v 	 eigenvectors (in columns) 				  */
/* 	 nrot  number of jacobi rotiations performed   */

/*   eigenvectors returned in columns (v[*][i]) of array v	*/

void jacobi (float *ain, int n, int ncol, float *d, float *vin, int *nrot) {
	int imax =	50;
	int j,iq,ip,i;
	float tresh,theta,tau,t,sm,s,h,g,c;
	float *b   = (float * ) calloc ( (unsigned) n, sizeof(float ) );
	float *z   = (float * ) calloc ( (unsigned) n, sizeof(float ) );
	float **a  = (float **) calloc ( (unsigned) n, sizeof(float*) );
	float **v  = (float **) calloc ( (unsigned) n, sizeof(float*) );
	/*  SET MATRICES a AND v TO POINT TO ROWS OF ain AND vin  */
	for (ip=0;ip<n;ip++) {
		a[ip] = ain + ip*ncol;
		v[ip] = vin + ip*ncol;
	}
	/*  SET v TO IDENTITY MATRIX	*/
	for (ip=0;ip<n;ip++) {
		for (iq=0;iq<n;iq++)
			v[ip][iq] = 0;;
		v[ip][ip] = 1.0;
	}
	for (ip=0;ip<n;ip++) {
		for (iq=0;iq<n;iq++)
			v[ip][iq] = 0.0;
		v[ip][ip] = 1.0;
	}
	for (ip=0;ip<n;ip++) {
		b[ip] = a[ip][ip];
		d[ip] = b[ip];
		z[ip] = 0.0;
	}
	*nrot = 0;
	for (i=0;i<imax;i++) {
		sm = 0.0;
		for (ip=0;ip<n-1;ip++)
			for (iq=ip+1;iq<n;iq++)
				sm += fabs(a[ip][iq]);
		if (sm == 0.0)
			return;
		if (i < 4)
			tresh = 0.2*sm/(n*n);
		else
			tresh = 0.0;
		for (ip=0;ip<n-1;ip++)
			for (iq=ip+1;iq<n;iq++)  {
				g = 100.0*fabs(a[ip][iq]);
				if (	  (i>4)
					  && ( (fabs(d[ip])+g)==fabs(d[ip]) )
					  && ( (fabs(d[iq])+g)==fabs(d[iq]) )
					)
					a[ip][iq] = 0.0;
				else if (fabs(a[ip][iq]) > tresh)  {
					h = d[iq]-d[ip];
					if ((fabs(h)+g) == fabs(h))
						t = a[ip][iq]/h;
					else {
						theta = 0.5*h/a[ip][iq];
						t = 1.0/(fabs(theta)+sqrt(1.0+theta*theta));
						if (theta < 0.0)
							t = -t;
					}
					c = 1.0/sqrt(1+t*t);
					s = t*c;
					tau = s/(1.0+c);
					h = t*a[ip][iq];
					z[ip] = z[ip]-h;
					z[iq] = z[iq]+h;
					d[ip] = d[ip]-h;
					d[iq] = d[iq]+h;
					a[ip][iq] = 0.0;
					for (j=0;j<ip;j++) {
						g = a[j][ip];
						h = a[j][iq];
						a[j][ip] = g-s*(h+g*tau);
						a[j][iq] = h+s*(g-h*tau);
					}
					for (j=ip+1;j<iq;j++) {
						g = a[ip][j];
						h = a[j][iq];
						a[ip][j] = g-s*(h+g*tau);
						a[j][iq] = h+s*(g-h*tau);
					}
					for (j=iq+1;j<n;j++) {
						g = a[ip][j];
						h = a[iq][j];
						a[ip][j] = g-s*(h+g*tau);
						a[iq][j] = h+s*(g-h*tau);
					}
					for (j=0;j<n;j++) {
						g = v[j][ip];
						h = v[j][iq];
						v[j][ip] = g-s*(h+g*tau);
						v[j][iq] = h+s*(g-h*tau);
					}
					*nrot = *nrot+1;
				}
		}
	}
	for (ip=0;ip<n;ip++) {
		b[ip] = b[ip]+z[ip];
		d[ip] = b[ip];
		z[ip] = 0.0;
	}
	fprintf (stderr, "Too many interations (%i) in routine jacobi.\n", imax);
	fprintf (stderr, "(Symetric matrix eigenvector routine).\n");
}

void *__global;

int __ecmp (const void *iin, const void *jin) {
	float *v = (float *) __global;
	int i = * ( (int *) iin );
	int j = * ( (int *) jin );
	if (v[i]<v[j])
		return(-1);
	else if (v[i]>v[j])
		return( 1);
	return(0);
}

void sorteigen (float *d, float *vin, int n, int ncol) {
	int i,j;
	int *indx = (int *) calloc (n, sizeof(int) );
	float **v  = (float **) calloc ( n, sizeof(float*) );
	float **vt = (float **) calloc ( n, sizeof(float*) );
	float *dt  = (float * ) calloc ( n, sizeof(float ) );

	/*  SET MATRICES v TO POINT TO ROWS OF vin  */
	for (i=0;i<n;i++)
		v[i] = vin + i*ncol;

	/*  ALLOCATE ELEMENTS FOR vt MATRIX  */
	for (i=0;i<n;i++)
		vt[i] = (float *) calloc (n, sizeof(float) );

	/*  SORT INDICES BY EIGENVALUES	*/
	__global  = (void *) d;
	for (i=0;i<n;i++)
		indx[i] = i;
	qsort ( (void *) indx, n, sizeof(int), __ecmp);

	/*  REARRANGE EIGENVALUES AND VECTORS	*/
	for (i=0;i<n;i++) {
		for (j=0;j<n;j++)
			vt[j][i] = v[j][indx[i]];
		dt[i] = d[indx[i]];
	}

	/*  COPY BACK TO ORIGINAL STORAGE  */
	for (i=0;i<n;i++) {
		for (j=0;j<n;j++)
			v[i][j] = vt[i][j];
		d[i] = dt[i];
	}

	/*  FREE STORAGE	*/
	free (dt);
	free (vt);
	free (v );
	free (indx);

}