cdvect.c

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

/*
------------------------------------------------------------------------
Include files
------------------------------------------------------------------------
*/
#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include "cdhouse.h"
#include "cdvect.h"




/*
------------------------------------------------------------------------
Defines
------------------------------------------------------------------------
*/
#define X 0
#define Y 1
#define Z 2
#define NDIR 3



/*
------------------------------------------------------------------------
Macros
------------------------------------------------------------------------
*/
#define CLEAN_AFTER_ERROR CleanAfterError();



/*
------------------------------------------------------------------------
Exported subroutines
------------------------------------------------------------------------
*/
double GetMagnitude (double v[NDIR])
	{
	return sqrt(v[X]*v[X] + v[Y]*v[Y] + v[Z]*v[Z]);
	}


void Normalize (double v[NDIR])
	{
	double Magnitude;

	Magnitude = GetMagnitude(v);
 	if (Magnitude==0.0)
		return;
	v[X] /= Magnitude;
	v[Y] /= Magnitude;
	v[Z] /= Magnitude;
	}


double GetDot (double a[NDIR], double b[NDIR])
	{
	return a[X]*b[X] + a[Y]*b[Y] + a[Z]*b[Z];
	}


/*  
Orthogonalize a[] with respect to b[] 
(assume  b[] is already normalized) 
*/
void Orthogonalize (double a[NDIR], double b[NDIR])
	{
	double Dot;

	Dot = GetDot (a,b);
	a[X] -= Dot * b[X];
	a[Y] -= Dot * b[Y];
	a[Z] -= Dot * b[Z];
	}


/*  
Calculate cross product, c, of a and b  
(c can be same vector as a and/or b 
*/
void CalcCross (double c[NDIR], double a[NDIR], double b[NDIR])
	{
	double TempResult[NDIR];

	TempResult[X] = a[Y]*b[Z] - a[Z]*b[Y];
	TempResult[Y] = a[Z]*b[X] - a[X]*b[Z];
	TempResult[Z] = a[X]*b[Y] - a[Y]*b[X];
	c[X] = TempResult[X];
	c[Y] = TempResult[Y];
	c[Z] = TempResult[Z];
	}


/*  Multiply matrix times vector*/
void MatrixVectorMult
(
double Result [NDIR],
double Matrix [NDIR][NDIR],
double Vector [NDIR]
)
   {
	double TempResult[NDIR];
   TempResult[X] =
      Matrix[X][X] * Vector[X] +
      Matrix[X][Y] * Vector[Y] +
      Matrix[X][Z] * Vector[Z];
   TempResult[Y] =
      Matrix[Y][X] * Vector[X] +
      Matrix[Y][Y] * Vector[Y] +
      Matrix[Y][Z] * Vector[Z];
   TempResult[Z] =
      Matrix[Z][X] * Vector[X] +
      Matrix[Z][Y] * Vector[Y] +
      Matrix[Z][Z] * Vector[Z];
	Result[X] = TempResult[X];
	Result[Y] = TempResult[Y];
	Result[Z] = TempResult[Z];
   }



/*  Multiply matrix times n vectors  */
void MatrixVectorMultN
(
double (*Result)[NDIR],
double Matrix[NDIR][NDIR],
double (*Vector)[NDIR],
int    NumVect
)
	{
	int idir;
	int jdir;
	int ivect;
	double Temp;

	ASSERT(Result!=NULL)
	ASSERT(Vector!=NULL)

	LOOP (ivect, NumVect)
	LOOP (idir, NDIR)
		{
		Temp = 0;
		LOOP (jdir, NDIR)
			{
			Temp += Matrix[idir][jdir] * Vector[ivect][jdir];
			}
		Result[ivect][idir] = Temp;
		}
	
	}



/*  Multiply matrix times matrix  */
void MatrixMatrixMult
(
double Result [NDIR][NDIR],
double Matrix1[NDIR][NDIR],
double Matrix2[NDIR][NDIR]
)
   {
	int i;
	int j;
	int k;
	double Term;
	double TempResult[NDIR][NDIR];
	
	for (i=0; i<NDIR; i++)
	for (j=0; j<NDIR; j++)
		{
		Term = 0.0;
		for (k=0; k<NDIR; k++)
			{
			Term += Matrix1[i][k]*Matrix2[k][j];
			}
		TempResult[i][j] = Term;
		}
	for (i=0; i<NDIR; i++)
	for (j=0; j<NDIR; j++)
		{
		Result[i][j] = TempResult[i][j];
		}	
   }



void TransposeMatrix (double Matrix[NDIR][NDIR])
	{
	int i;
	int j;
	double Swap;
	
	for (i=0; i<NDIR; i++)
	for (j=0; j<i;    j++)
		{
		Swap         = Matrix[i][j];
		Matrix[i][j] = Matrix[j][i];
		Matrix[j][i] = Swap;
		}
	}


/*
Make rotation matrix which rotatates OrigVector into RotVector
*/
void MakeRotationMatrix 
(
double Matrix[NDIR][NDIR],
double RotVector[NDIR],
double OriVector[NDIR]
)
	{
	int idir;
	int jdir;
	double   Asym[NDIR][NDIR];
	double   Axis[NDIR];
	double   Sine;
	double   Cosine;
	double   CroMagnitude;
	double   DotMagnitude;
	double   OriMagnitude;
	double   RotMagnitude;
	

	/*  Axis is cross product of two vectors  */
	CalcCross (Axis, OriVector, RotVector);

	/*  Get magnitude of vectors  */
	CroMagnitude = GetMagnitude(Axis);
	DotMagnitude = GetDot      (OriVector, RotVector);
	OriMagnitude = GetMagnitude(OriVector);
	RotMagnitude = GetMagnitude(RotVector);
	

	/*  Normalize Dot and Cross products to get Cos and Sine */
	Cosine = DotMagnitude/(OriMagnitude*RotMagnitude);
	Sine   = CroMagnitude/(OriMagnitude*RotMagnitude);

	/*  Normalize axis  */
	Normalize (Axis);

   /*  SET UP ASYMMETRIC MATRIX (REPRESENTS CROSS PRODUCT)  */
   Asym[X][X] = Asym[Y][Y] = Asym[Z][Z] = 0;
   Asym[X][Y] = -Axis[Z];
   Asym[Y][Z] = -Axis[X];
   Asym[Z][X] = -Axis[Y];
   Asym[Y][X] = -Asym[X][Y];
   Asym[Z][Y] = -Asym[Y][Z];
   Asym[X][Z] = -Asym[Z][X];

   /*  SET UP MATRIX  */
   for (idir=0; idir<NDIR; idir++)
   for (jdir=0; jdir<NDIR; jdir++)
      if (idir==jdir)
         {
         Matrix[idir][jdir] =
            Axis[idir]*Axis[jdir] +
            Cosine * (1 - Axis[idir]*Axis[jdir]) +
            Sine   * Asym[idir][jdir];
         }
      else
         Matrix[idir][jdir] =
            Axis[idir]*Axis[jdir] * (1 - Cosine) +
            Sine * Asym[idir][jdir];
	}


void CopyMatrix (double New[NDIR][NDIR], double Old[NDIR][NDIR])
	{
	int i;
	int j;
	for (i=0; i<NDIR; i++)
	for (j=0; j<NDIR; j++)
		New[i][j] = Old[i][j];
	}


void CopyVector (double New[NDIR], double Old[NDIR])
	{
	int i;
	for (i=0; i<NDIR; i++)
		New[i] = Old[i];
	}


void InvertMatrix
(
double InvMatrix[NDIR][NDIR],
double Matrix   [NDIR][NDIR]
)
   {
   int idir;
   int jdir;
   double Magnitude;
   double NormFactor;

   CalcCross (InvMatrix[0], Matrix[1], Matrix[2]);
   CalcCross (InvMatrix[1], Matrix[2], Matrix[0]);
   CalcCross (InvMatrix[2], Matrix[0], Matrix[1]);

   NormFactor =
      GetDot (InvMatrix[0], Matrix[0]) *
      GetDot (InvMatrix[1], Matrix[1]) *
      GetDot (InvMatrix[2], Matrix[2]);
   if (NormFactor<0)
      {
      NormFactor = -pow(-NormFactor, (1.0/3.0) );
      }
   else
      {
      NormFactor = pow(NormFactor, (1.0/3.0) );
      }

   TransposeMatrix (InvMatrix);

   /*
   Determine if matrix is singular by comparing Normalization
   factor (which is equivalent to volume enclosed by the row
   vectors of Matrix) with Matrix magnitude
    */
   Magnitude = 0.0;
	for (idir=0; idir<NDIR; idir++)
	for (jdir=0; jdir<NDIR; jdir++)
      {
      Magnitude += Matrix[idir][jdir]*Matrix[idir][jdir];
      }
   Magnitude = sqrt(Magnitude/3.0);

   if (fabs(NormFactor) < 1e-6*pow(Magnitude,3.0))
      {
      printf
         ("INTERNAL ERROR (InvertMatrix):  Input matrix is singular\n");
      CLEAN_AFTER_ERROR
      }

   /*  Normalize inverse so that product is one  */
   NormFactor = 1.0/NormFactor;
	for (idir=0; idir<NDIR; idir++)
	for (jdir=0; jdir<NDIR; jdir++)
      {
      InvMatrix[idir][jdir] *= NormFactor;
      }
   }