OOPSE/libmdtools/Shake.cpp

#include "Shake.hpp"
#include "SimInfo.hpp"
#include <cmath>
#include "simError.h"
#include "MatVec3.h"
////////////////////////////////////////////////////////////////////////////////
//Implementation of DCShakeFunctor
////////////////////////////////////////////////////////////////////////////////
int DCShakeFunctor::operator()(ConstraintAtom* consAtom1, ConstraintAtom* consAtom2){
  double posA[3];
  double posB[3];
  double oldPosA[3];
  double oldPosB[3];
  double velA[3];
  double velB[3];
  double pab[3];
  double rab[3];
  double rma, rmb;
  double dx, dy, dz;
  double rpab;
  double rabsq, pabsq, rpabsq;
  double diffsq;
  double gab;
  double dt;
  
  dt = info->dt;
  
  consAtom1->getPos(posA);
  consAtom2->getPos(posB);

   
  pab[0] = posA[0] - posB[0];
  pab[1] = posA[1] - posB[1];
  pab[2] = posA[2] - posB[2];

  //periodic boundary condition

  info->wrapVector(pab);

  pabsq = pab[0] * pab[0] + pab[1] * pab[1] + pab[2] * pab[2];

  rabsq = curPair->getBondLength2();
  diffsq = rabsq - pabsq;

  // the original rattle code from alan tidesley
  if (fabs(diffsq) > (consTolerance * rabsq * 2)){
    
    consAtom1->getOldPos(oldPosA);
    consAtom2->getOldPos(oldPosB);      
    
    rab[0] = oldPosA[0] - oldPosB[0];
    rab[1] = oldPosA[1] - oldPosB[1];
    rab[2] = oldPosA[2] - oldPosB[2];

    info->wrapVector(rab);

    rpab = rab[0] * pab[0] + rab[1] * pab[1] + rab[2] * pab[2];

    rpabsq = rpab * rpab;


    if (rpabsq < (rabsq * -diffsq)){
      return consFail;
    }

    rma = 1.0 / consAtom1->getMass();
    rmb = 1.0 / consAtom2->getMass();

    gab = diffsq / (2.0 * (rma + rmb) * rpab);

    dx = rab[0] * gab;
    dy = rab[1] * gab;
    dz = rab[2] * gab;

    //set atom1's position
    posA[0] += rma * dx;
    posA[1] += rma * dy;
    posA[2] += rma * dz;
    consAtom1->setPos(posA);

    //set atom2's position
    posB[0] -= rmb * dx;
    posB[1] -= rmb * dy;
    posB[2] -= rmb * dz;
    consAtom2->setPos(posB);

    //dx = dx / dt;
    //dy = dy / dt;
    //dz = dz / dt;

    ////set atom1's velocity
    //consAtom1->getVel(velA);
    //velA[0] += rma * dx;
    //velA[1] += rma * dy;
    //velA[2] += rma * dz;
    //consAtom1->setVel(velA);

    ////set atom2's velocity
    //consAtom2->getVel(velB);
    //velB[0] -= rmb * dx;
    //velB[1] -= rmb * dy;
    //velB[2] -= rmb * dz;
    //consAtom2->setVel(velB);

    return consSuccess;
  }
  else
    return consAlready;
  
}


int DCShakeFunctor::operator()(ConstraintAtom* consAtom,ConstraintRigidBody* consRB){
  return consElemHandlerFail;
} 

/**
  * QSHAKE Algorithm
  * Reference
  * T.R. Forester and W. Smith, SHAKE, Rattle and Roll: Efficient Constraint Algorithms for Linked 
  * Rigid Bodies, J. Comp. Chem., 19(1), 102 -111 (1998)
  */
int DCShakeFunctor::operator()(ConstraintRigidBody* consRB1, ConstraintRigidBody* consRB2){
  double posA[3];
  double posB[3];
  double oldPosA[3];
  double oldPosB[3];
  double velA[3];
  double velB[3];
  double pab[3];
  double tempPab[3];
  double rab[3];
  double rma, rmb;
  double dx, dy, dz;
  double rpab;
  double rabsq, pabsq, rpabsq;
  double diffsq;
  double gab;
  double dt;
  double dt2;
  double consForce[3];

  const int conRBMaxIter = 10;
  
  dt = info->dt;
  dt2 = dt * dt;

  consRB1->getOldAtomPos(oldPosA);
  consRB2->getOldAtomPos(oldPosB);      


  for(int i=0 ; i < conRBMaxIter; i++){   
  consRB1->getCurAtomPos(posA);
  consRB2->getCurAtomPos(posB);

   
  pab[0] = posA[0] - posB[0];
  pab[1] = posA[1] - posB[1];
  pab[2] = posA[2] - posB[2];

  //periodic boundary condition

  info->wrapVector(pab);

  pabsq = pab[0] * pab[0] + pab[1] * pab[1] + pab[2] * pab[2];

  rabsq = curPair->getBondLength2();
  diffsq = rabsq - pabsq;

  // the original rattle code from alan tidesley
  if (fabs(diffsq) > (consTolerance * rabsq * 2)){
       
    rab[0] = oldPosA[0] - oldPosB[0];
    rab[1] = oldPosA[1] - oldPosB[1];
    rab[2] = oldPosA[2] - oldPosB[2];

    info->wrapVector(rab);

    rpab = rab[0] * pab[0] + rab[1] * pab[1] + rab[2] * pab[2];

    rpabsq = rpab * rpab;


    if (rpabsq < (rabsq * -diffsq)){
      return consFail;
    }

    //rma = 1.0 / consRB1->getMass();
    //rmb = 1.0 / consRB2->getMass();
    
    tempPab[0] = pab[0] / sqrt(pabsq);
    tempPab[1] = pab[1] / sqrt(pabsq);
    tempPab[2] = pab[2] / sqrt(pabsq);
    
    rma = getEffInvMass(consRB1, tempPab);

    tempPab[0] = -tempPab[0];
    tempPab[1] = -tempPab[1];
    tempPab[2] = -tempPab[2];    
    rmb = getEffInvMass(consRB2, tempPab);

    gab = diffsq / (2.0* dt * dt * (rma + rmb) * rpab) ;
    consForce[0] = rab[0] * gab;
    consForce[1] = rab[1] * gab;
    consForce[2] = rab[2] * gab;

    //integrate consRB1 using constraint force;
    integrate(consRB1,consForce);

    //integrate consRB2 using constraint force;
    consForce[0] = -consForce[0];
    consForce[1] = -consForce[1];
    consForce[2] = -consForce[2];
    integrate(consRB2,consForce);    
    
  }
  else{
    if (i ==0)
      return consAlready;
    else
      return consSuccess;
  }
  }

  return consExceedMaxIter;
}

double DCShakeFunctor::getEffInvMass(ConstraintRigidBody* consRB, double bondDir[3]){
  double effInvMass;  //effective inversse mass
  double effInvMassCorr;  //correction for effective inverse mass
  double aTrans[3][3];
  double a[3][3];

  double IFrame[3][3];
  double IBody[3][3];
  double invI[3][3];
  double refCoor[3];
  double refCrossBond[3];
  double tempVec1[3];
  double tempVec2[3];

  effInvMass = 1.0 / consRB ->getMass();
  
  consRB->getRefCoor(refCoor);
  consRB->getA(a);
  consRB->getI(IBody);

  crossProduct3(refCoor, bondDir, refCrossBond);

  matMul3(a, IBody, IFrame);

  invertMat3(IFrame, invI);

  matVecMul3(invI, refCrossBond, tempVec1);
  
  crossProduct3(tempVec1, refCoor, tempVec2);

  effInvMassCorr = dotProduct3(tempVec1, bondDir);
    
  effInvMass += effInvMassCorr;

  return effInvMass;
}

void DCShakeFunctor::integrate(ConstraintRigidBody* consRB, double force[3]){

}
////////////////////////////////////////////////////////////////////////////////
//Implementation of JCShakeFunctor
////////////////////////////////////////////////////////////////////////////////
int JCShakeFunctor::operator()(ConstraintAtom* consAtom1, ConstraintAtom* consAtom2){
  return consElemHandlerFail;
}


int JCShakeFunctor::operator()(ConstraintAtom* consAtom,ConstraintRigidBody* consRB){
  return consElemHandlerFail;

}
 
int JCShakeFunctor::operator()(ConstraintRigidBody* consRB1, ConstraintRigidBody* consRB2){
  return consElemHandlerFail;
}
Revision:	1254
Committed:	Wed Jun 9 16:16:33 2004 UTC (20 years, 3 months ago) by tim
File size:	6568 byte(s)
Log Message:	1. adding some useful math classes(Mat3x3d, Vector3d, Quaternion, Euler3) these classes use anonymous union and struct to support double[3], double[3][3] and double[4] 2. adding roll constraint algorithm
#	User	Rev	Content
1	tim	1232	#include "Shake.hpp"
2			#include "SimInfo.hpp"
3			#include <cmath>
4			#include "simError.h"
5	tim	1254	#include "MatVec3.h"
6	tim	1232	////////////////////////////////////////////////////////////////////////////////
7			//Implementation of DCShakeFunctor
8			////////////////////////////////////////////////////////////////////////////////
9			int DCShakeFunctor::operator()(ConstraintAtom* consAtom1, ConstraintAtom* consAtom2){
10			double posA[3];
11			double posB[3];
12			double oldPosA[3];
13			double oldPosB[3];
14			double velA[3];
15			double velB[3];
16			double pab[3];
17			double rab[3];
18			double rma, rmb;
19			double dx, dy, dz;
20			double rpab;
21			double rabsq, pabsq, rpabsq;
22			double diffsq;
23			double gab;
24			double dt;
25
26			dt = info->dt;
27
28			consAtom1->getPos(posA);
29			consAtom2->getPos(posB);
30
31
32			pab[0] = posA[0] - posB[0];
33			pab[1] = posA[1] - posB[1];
34			pab[2] = posA[2] - posB[2];
35
36			//periodic boundary condition
37
38			info->wrapVector(pab);
39
40			pabsq = pab[0] * pab[0] + pab[1] * pab[1] + pab[2] * pab[2];
41
42			rabsq = curPair->getBondLength2();
43			diffsq = rabsq - pabsq;
44
45			// the original rattle code from alan tidesley
46			if (fabs(diffsq) > (consTolerance * rabsq * 2)){
47
48			consAtom1->getOldPos(oldPosA);
49			consAtom2->getOldPos(oldPosB);
50
51			rab[0] = oldPosA[0] - oldPosB[0];
52			rab[1] = oldPosA[1] - oldPosB[1];
53			rab[2] = oldPosA[2] - oldPosB[2];
54
55			info->wrapVector(rab);
56
57			rpab = rab[0] * pab[0] + rab[1] * pab[1] + rab[2] * pab[2];
58
59			rpabsq = rpab * rpab;
60
61
62			if (rpabsq < (rabsq * -diffsq)){
63			return consFail;
64			}
65
66			rma = 1.0 / consAtom1->getMass();
67			rmb = 1.0 / consAtom2->getMass();
68
69			gab = diffsq / (2.0 * (rma + rmb) * rpab);
70
71			dx = rab[0] * gab;
72			dy = rab[1] * gab;
73			dz = rab[2] * gab;
74
75	tim	1254	//set atom1's position
76	tim	1232	posA[0] += rma * dx;
77			posA[1] += rma * dy;
78			posA[2] += rma * dz;
79			consAtom1->setPos(posA);
80
81	tim	1254	//set atom2's position
82	tim	1232	posB[0] -= rmb * dx;
83			posB[1] -= rmb * dy;
84			posB[2] -= rmb * dz;
85			consAtom2->setPos(posB);
86
87	tim	1254	//dx = dx / dt;
88			//dy = dy / dt;
89			//dz = dz / dt;
90	tim	1232
91	tim	1254	////set atom1's velocity
92			//consAtom1->getVel(velA);
93			//velA[0] += rma * dx;
94			//velA[1] += rma * dy;
95			//velA[2] += rma * dz;
96			//consAtom1->setVel(velA);
97	tim	1232
98	tim	1254	////set atom2's velocity
99			//consAtom2->getVel(velB);
100			//velB[0] -= rmb * dx;
101			//velB[1] -= rmb * dy;
102			//velB[2] -= rmb * dz;
103			//consAtom2->setVel(velB);
104	tim	1232
105			return consSuccess;
106			}
107			else
108			return consAlready;
109
110			}
111
112
113			int DCShakeFunctor::operator()(ConstraintAtom* consAtom,ConstraintRigidBody* consRB){
114			return consElemHandlerFail;
115			}
116
117	tim	1254	/**
118			* QSHAKE Algorithm
119			* Reference
120			* T.R. Forester and W. Smith, SHAKE, Rattle and Roll: Efficient Constraint Algorithms for Linked
121			* Rigid Bodies, J. Comp. Chem., 19(1), 102 -111 (1998)
122			*/
123	tim	1232	int DCShakeFunctor::operator()(ConstraintRigidBody* consRB1, ConstraintRigidBody* consRB2){
124	tim	1254	double posA[3];
125			double posB[3];
126			double oldPosA[3];
127			double oldPosB[3];
128			double velA[3];
129			double velB[3];
130			double pab[3];
131			double tempPab[3];
132			double rab[3];
133			double rma, rmb;
134			double dx, dy, dz;
135			double rpab;
136			double rabsq, pabsq, rpabsq;
137			double diffsq;
138			double gab;
139			double dt;
140			double dt2;
141			double consForce[3];
142
143			const int conRBMaxIter = 10;
144
145			dt = info->dt;
146			dt2 = dt * dt;
147
148			consRB1->getOldAtomPos(oldPosA);
149			consRB2->getOldAtomPos(oldPosB);
150
151
152			for(int i=0 ; i < conRBMaxIter; i++){
153			consRB1->getCurAtomPos(posA);
154			consRB2->getCurAtomPos(posB);
155
156
157			pab[0] = posA[0] - posB[0];
158			pab[1] = posA[1] - posB[1];
159			pab[2] = posA[2] - posB[2];
160
161			//periodic boundary condition
162
163			info->wrapVector(pab);
164
165			pabsq = pab[0] * pab[0] + pab[1] * pab[1] + pab[2] * pab[2];
166
167			rabsq = curPair->getBondLength2();
168			diffsq = rabsq - pabsq;
169
170			// the original rattle code from alan tidesley
171			if (fabs(diffsq) > (consTolerance * rabsq * 2)){
172
173			rab[0] = oldPosA[0] - oldPosB[0];
174			rab[1] = oldPosA[1] - oldPosB[1];
175			rab[2] = oldPosA[2] - oldPosB[2];
176
177			info->wrapVector(rab);
178
179			rpab = rab[0] * pab[0] + rab[1] * pab[1] + rab[2] * pab[2];
180
181			rpabsq = rpab * rpab;
182
183
184			if (rpabsq < (rabsq * -diffsq)){
185			return consFail;
186			}
187
188			//rma = 1.0 / consRB1->getMass();
189			//rmb = 1.0 / consRB2->getMass();
190
191			tempPab[0] = pab[0] / sqrt(pabsq);
192			tempPab[1] = pab[1] / sqrt(pabsq);
193			tempPab[2] = pab[2] / sqrt(pabsq);
194
195			rma = getEffInvMass(consRB1, tempPab);
196
197			tempPab[0] = -tempPab[0];
198			tempPab[1] = -tempPab[1];
199			tempPab[2] = -tempPab[2];
200			rmb = getEffInvMass(consRB2, tempPab);
201
202			gab = diffsq / (2.0* dt * dt * (rma + rmb) * rpab) ;
203			consForce[0] = rab[0] * gab;
204			consForce[1] = rab[1] * gab;
205			consForce[2] = rab[2] * gab;
206
207			//integrate consRB1 using constraint force;
208			integrate(consRB1,consForce);
209
210			//integrate consRB2 using constraint force;
211			consForce[0] = -consForce[0];
212			consForce[1] = -consForce[1];
213			consForce[2] = -consForce[2];
214			integrate(consRB2,consForce);
215
216			}
217			else{
218			if (i ==0)
219			return consAlready;
220			else
221			return consSuccess;
222			}
223			}
224
225			return consExceedMaxIter;
226	tim	1232	}
227
228	tim	1254	double DCShakeFunctor::getEffInvMass(ConstraintRigidBody* consRB, double bondDir[3]){
229			double effInvMass; //effective inversse mass
230			double effInvMassCorr; //correction for effective inverse mass
231			double aTrans[3][3];
232			double a[3][3];
233
234			double IFrame[3][3];
235			double IBody[3][3];
236			double invI[3][3];
237			double refCoor[3];
238			double refCrossBond[3];
239			double tempVec1[3];
240			double tempVec2[3];
241
242			effInvMass = 1.0 / consRB ->getMass();
243
244			consRB->getRefCoor(refCoor);
245			consRB->getA(a);
246			consRB->getI(IBody);
247
248			crossProduct3(refCoor, bondDir, refCrossBond);
249
250			matMul3(a, IBody, IFrame);
251
252			invertMat3(IFrame, invI);
253
254			matVecMul3(invI, refCrossBond, tempVec1);
255
256			crossProduct3(tempVec1, refCoor, tempVec2);
257
258			effInvMassCorr = dotProduct3(tempVec1, bondDir);
259
260			effInvMass += effInvMassCorr;
261
262			return effInvMass;
263			}
264
265			void DCShakeFunctor::integrate(ConstraintRigidBody* consRB, double force[3]){
266
267			}
268	tim	1232	////////////////////////////////////////////////////////////////////////////////
269			//Implementation of JCShakeFunctor
270			////////////////////////////////////////////////////////////////////////////////
271			int JCShakeFunctor::operator()(ConstraintAtom* consAtom1, ConstraintAtom* consAtom2){
272			return consElemHandlerFail;
273			}
274
275
276
277			int JCShakeFunctor::operator()(ConstraintAtom* consAtom,ConstraintRigidBody* consRB){
278			return consElemHandlerFail;
279
280			}
281
282			int JCShakeFunctor::operator()(ConstraintRigidBody* consRB1, ConstraintRigidBody* consRB2){
283			return consElemHandlerFail;
284	tim	1254	}