OOPSE/libmdtools/Rattle.cpp

#include "Rattle.hpp"
#include <cmath>
#include "SimInfo.hpp"
#include "MatVec3.h"
////////////////////////////////////////////////////////////////////////////////
//Implementation of DCShakeFunctor
////////////////////////////////////////////////////////////////////////////////
int DCRattleAFunctor::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 DCRattleAFunctor::operator()(ConstraintAtom* consAtom,ConstraintRigidBody* consRB){
  return consElemHandlerFail;
} 

int DCRattleAFunctor::operator()(ConstraintRigidBody* consRB1, ConstraintRigidBody* consRB2){
  //calculate lamda

  //integrate

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

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

}
 
int JCRattleAFunctor::operator()(ConstraintRigidBody* consRB1, ConstraintRigidBody* consRB2){
  //calculate lamda,
  //integrate
  //
  return consElemHandlerFail;
}


////////////////////////////////////////////////////////////////////////////////
//Implementation of DCRattleBFunctor
////////////////////////////////////////////////////////////////////////////////
int DCRattleBFunctor::operator()(ConstraintAtom* consAtom1, ConstraintAtom* consAtom2){
  double posA[3], posB[3];
  double velA[3], velB[3];
  double vxab, vyab, vzab;
  double rab[3];
  double rma, rmb;
  double dx, dy, dz;
  double rvab;
  double gab;
  
    consAtom1->getVel(velA);
    consAtom2->getVel(velB);

    vxab = velA[0] - velB[0];
    vyab = velA[1] - velB[1];
    vzab = velA[2] - velB[2];

    consAtom1->getPos(posA);
    consAtom2->getPos(posB);

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

    info->wrapVector(rab);

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

    rvab = rab[0] * vxab + rab[1] * vyab + rab[2] * vzab;

    gab = -rvab / ((rma + rmb) * curPair->getBondLength2());

    if (fabs(gab) > consTolerance){
      dx = rab[0] * gab;
      dy = rab[1] * gab;
      dz = rab[2] * gab;

      velA[0] += rma * dx;
      velA[1] += rma * dy;
      velA[2] += rma * dz;

      consAtom1->setVel(velA);

      velB[0] -= rmb * dx;
      velB[1] -= rmb * dy;
      velB[2] -= rmb * dz;

      consAtom2->setVel(velB);
    }
    return consSuccess;
}


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

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


int JCRattleBFunctor::operator()(ConstraintAtom* consAtom,ConstraintRigidBody* consRB){
  return consElemHandlerFail;
}
 
int JCRattleBFunctor::operator()(ConstraintRigidBody* consRB1, ConstraintRigidBody* consRB2){
  return consElemHandlerFail;
}
Revision:	1254
Committed:	Wed Jun 9 16:16:33 2004 UTC (21 years, 5 months ago) by tim
File size:	5477 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 "Rattle.hpp"
2			#include <cmath>
3			#include "SimInfo.hpp"
4	tim	1254	#include "MatVec3.h"
5			////////////////////////////////////////////////////////////////////////////////
6			//Implementation of DCShakeFunctor
7			////////////////////////////////////////////////////////////////////////////////
8			int DCRattleAFunctor::operator()(ConstraintAtom* consAtom1, ConstraintAtom* consAtom2){
9			double posA[3];
10			double posB[3];
11			double oldPosA[3];
12			double oldPosB[3];
13			double velA[3];
14			double velB[3];
15			double pab[3];
16			double rab[3];
17			double rma, rmb;
18			double dx, dy, dz;
19			double rpab;
20			double rabsq, pabsq, rpabsq;
21			double diffsq;
22			double gab;
23			double dt;
24
25			dt = info->dt;
26
27			consAtom1->getPos(posA);
28			consAtom2->getPos(posB);
29	tim	1232
30	tim	1254
31			pab[0] = posA[0] - posB[0];
32			pab[1] = posA[1] - posB[1];
33			pab[2] = posA[2] - posB[2];
34
35			//periodic boundary condition
36
37			info->wrapVector(pab);
38
39			pabsq = pab[0] * pab[0] + pab[1] * pab[1] + pab[2] * pab[2];
40
41			rabsq = curPair->getBondLength2();
42			diffsq = rabsq - pabsq;
43
44			// the original rattle code from alan tidesley
45			if (fabs(diffsq) > (consTolerance * rabsq * 2)){
46
47			consAtom1->getOldPos(oldPosA);
48			consAtom2->getOldPos(oldPosB);
49
50			rab[0] = oldPosA[0] - oldPosB[0];
51			rab[1] = oldPosA[1] - oldPosB[1];
52			rab[2] = oldPosA[2] - oldPosB[2];
53
54			info->wrapVector(rab);
55
56			rpab = rab[0] * pab[0] + rab[1] * pab[1] + rab[2] * pab[2];
57
58			rpabsq = rpab * rpab;
59
60
61			if (rpabsq < (rabsq * -diffsq)){
62			return consFail;
63			}
64
65			rma = 1.0 / consAtom1->getMass();
66			rmb = 1.0 / consAtom2->getMass();
67
68			gab = diffsq / (2.0 * (rma + rmb) * rpab);
69
70			dx = rab[0] * gab;
71			dy = rab[1] * gab;
72			dz = rab[2] * gab;
73
74			//set atom1's position
75			posA[0] += rma * dx;
76			posA[1] += rma * dy;
77			posA[2] += rma * dz;
78			consAtom1->setPos(posA);
79
80			//set atom2's position
81			posB[0] -= rmb * dx;
82			posB[1] -= rmb * dy;
83			posB[2] -= rmb * dz;
84			consAtom2->setPos(posB);
85
86			dx = dx / dt;
87			dy = dy / dt;
88			dz = dz / dt;
89
90			//set atom1's velocity
91			consAtom1->getVel(velA);
92			velA[0] += rma * dx;
93			velA[1] += rma * dy;
94			velA[2] += rma * dz;
95			consAtom1->setVel(velA);
96
97			//set atom2's velocity
98			consAtom2->getVel(velB);
99			velB[0] -= rmb * dx;
100			velB[1] -= rmb * dy;
101			velB[2] -= rmb * dz;
102			consAtom2->setVel(velB);
103
104			return consSuccess;
105			}
106			else
107			return consAlready;
108
109			}
110
111
112			int DCRattleAFunctor::operator()(ConstraintAtom* consAtom,ConstraintRigidBody* consRB){
113			return consElemHandlerFail;
114			}
115
116			int DCRattleAFunctor::operator()(ConstraintRigidBody* consRB1, ConstraintRigidBody* consRB2){
117			//calculate lamda
118
119			//integrate
120
121			//
122			return consElemHandlerFail;
123			}
124	tim	1232	////////////////////////////////////////////////////////////////////////////////
125	tim	1254	//Implementation of JCShakeFunctor
126			////////////////////////////////////////////////////////////////////////////////
127			int JCRattleAFunctor::operator()(ConstraintAtom* consAtom1, ConstraintAtom* consAtom2){
128			return consElemHandlerFail;
129			}
130
131			int JCRattleAFunctor::operator()(ConstraintAtom* consAtom,ConstraintRigidBody* consRB){
132			return consElemHandlerFail;
133
134			}
135
136			int JCRattleAFunctor::operator()(ConstraintRigidBody* consRB1, ConstraintRigidBody* consRB2){
137			//calculate lamda,
138			//integrate
139			//
140			return consElemHandlerFail;
141			}
142
143
144			////////////////////////////////////////////////////////////////////////////////
145	tim	1232	//Implementation of DCRattleBFunctor
146			////////////////////////////////////////////////////////////////////////////////
147			int DCRattleBFunctor::operator()(ConstraintAtom* consAtom1, ConstraintAtom* consAtom2){
148			double posA[3], posB[3];
149			double velA[3], velB[3];
150			double vxab, vyab, vzab;
151			double rab[3];
152			double rma, rmb;
153			double dx, dy, dz;
154			double rvab;
155			double gab;
156
157			consAtom1->getVel(velA);
158			consAtom2->getVel(velB);
159
160			vxab = velA[0] - velB[0];
161			vyab = velA[1] - velB[1];
162			vzab = velA[2] - velB[2];
163
164			consAtom1->getPos(posA);
165			consAtom2->getPos(posB);
166
167			rab[0] = posA[0] - posB[0];
168			rab[1] = posA[1] - posB[1];
169			rab[2] = posA[2] - posB[2];
170
171			info->wrapVector(rab);
172
173			rma = 1.0 / consAtom1->getMass();
174			rmb = 1.0 / consAtom2->getMass();
175
176			rvab = rab[0] * vxab + rab[1] * vyab + rab[2] * vzab;
177
178			gab = -rvab / ((rma + rmb) * curPair->getBondLength2());
179
180			if (fabs(gab) > consTolerance){
181			dx = rab[0] * gab;
182			dy = rab[1] * gab;
183			dz = rab[2] * gab;
184
185			velA[0] += rma * dx;
186			velA[1] += rma * dy;
187			velA[2] += rma * dz;
188
189			consAtom1->setVel(velA);
190
191			velB[0] -= rmb * dx;
192			velB[1] -= rmb * dy;
193			velB[2] -= rmb * dz;
194
195			consAtom2->setVel(velB);
196			}
197			return consSuccess;
198			}
199
200
201
202			int DCRattleBFunctor::operator()(ConstraintAtom* consAtom,ConstraintRigidBody* consRB){
203			return consElemHandlerFail;
204			}
205
206			int DCRattleBFunctor::operator()(ConstraintRigidBody* consRB1, ConstraintRigidBody* consRB2){
207			return consElemHandlerFail;
208			}
209
210			////////////////////////////////////////////////////////////////////////////////
211			//Implementation of JCRattleBFunctor
212			////////////////////////////////////////////////////////////////////////////////
213			int JCRattleBFunctor::operator()(ConstraintAtom* consAtom1, ConstraintAtom* consAtom2){
214			return consElemHandlerFail;
215			}
216
217
218
219			int JCRattleBFunctor::operator()(ConstraintAtom* consAtom,ConstraintRigidBody* consRB){
220			return consElemHandlerFail;
221			}
222
223			int JCRattleBFunctor::operator()(ConstraintRigidBody* consRB1, ConstraintRigidBody* consRB2){
224			return consElemHandlerFail;
225			}