OOPSE/libmdtools/ShakeMin.cpp

#include <cmath>
#include "ConstraintElement.hpp"
#include "ShakeMin.hpp"
#include "SimInfo.hpp"
////////////////////////////////////////////////////////////////////////////////
//Implementation of DCShakeMinRFunctor
////////////////////////////////////////////////////////////////////////////////
int DCShakeRMinFunctor::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();

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

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

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

    consAtom1->setPos(posA);

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

    consAtom2->setPos(posB);

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

    consAtom1->getVel(velA);

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

    consAtom1->setVel(velA);

    consAtom2->getVel(velB);

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

    consAtom2->setVel(velB);

    return consSuccess;
  }
  else
    return consAlready;
}


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

}
 
int DCShakeRMinFunctor::operator()(ConstraintRigidBody* consRB1, ConstraintRigidBody* consRB2){
  return consElemHandlerFail;
}

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


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


////////////////////////////////////////////////////////////////////////////////
//Implementation of DCShakeMinFFunctor
////////////////////////////////////////////////////////////////////////////////
int DCShakeMinFFunctor::operator()(ConstraintAtom* consAtom1, ConstraintAtom* consAtom2){

  double posA[3];
  double posB[3];
  double frcA[3];
  double frcB[3];
  double rab[3];
  double fpab[3];
  double rma;
  double rmb;
  double gab;
  double rabsq;
  double rfab;


  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);

  consAtom1->getFrc(frcA);
  consAtom2->getFrc(frcB);

  rma = 1.0;
  rmb = 1.0;


  fpab[0] = frcA[0] * rma - frcB[0] * rmb;
  fpab[1] = frcA[1] * rma - frcB[1] * rmb;
  fpab[2] = frcA[2] * rma - frcB[2] * rmb;


  gab=fpab[0] * fpab[0] + fpab[1] * fpab[1] + fpab[2] * fpab[2];

  if (gab < 1.0)
    gab = 1.0;

  rabsq = rab[0] * rab[0] + rab[1] * rab[1] + rab[2] * rab[2];
  rfab = rab[0] * fpab[0] + rab[1] * fpab[1] + rab[2] * fpab[2];

  if (fabs(rfab) > sqrt(rabsq*gab) * consTolerance){

  gab = -rfab /(rabsq*(rma + rmb));

    frcA[0] = rab[0] * gab;
    frcA[1] = rab[1] * gab;
    frcA[2] = rab[2] * gab;

    consAtom1->addFrc(frcA);

    frcB[0] = -rab[0] * gab;
    frcB[1] = -rab[1] * gab;
    frcB[2] = -rab[2] * gab;

    consAtom2->addFrc(frcB);

  }             

   return consSuccess;
    
 }


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

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


int JCShakeMinFFunctor::operator()(ConstraintAtom* consAtom,ConstraintRigidBody* consRB){
  return consElemHandlerFail;
}
 
int JCShakeMinFFunctor::operator()(ConstraintRigidBody* consRB1, ConstraintRigidBody* consRB2){
  return consElemHandlerFail;
}
Revision:	1248
Committed:	Fri Jun 4 19:30:05 2004 UTC (20 years, 1 month ago) by tim
File size:	5484 byte(s)
Log Message:	constraint algorithm for minimization is working
#	User	Rev	Content
1	tim	1248	#include <cmath>
2			#include "ConstraintElement.hpp"
3	tim	1232	#include "ShakeMin.hpp"
4	tim	1248	#include "SimInfo.hpp"
5	tim	1232	////////////////////////////////////////////////////////////////////////////////
6			//Implementation of DCShakeMinRFunctor
7			////////////////////////////////////////////////////////////////////////////////
8			int DCShakeRMinFunctor::operator()(ConstraintAtom* consAtom1, ConstraintAtom* consAtom2){
9	tim	1248	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
30
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			rma = 1.0;
69			rmb= 1.0;
70
71			gab = diffsq / (2.0 * (rma + rmb) * rpab);
72
73			dx = rab[0] * gab;
74			dy = rab[1] * gab;
75			dz = rab[2] * gab;
76
77			posA[0] += rma * dx;
78			posA[1] += rma * dy;
79			posA[2] += rma * dz;
80
81			consAtom1->setPos(posA);
82
83			posB[0] -= rmb * dx;
84			posB[1] -= rmb * dy;
85			posB[2] -= rmb * dz;
86
87			consAtom2->setPos(posB);
88
89			dx = dx / dt;
90			dy = dy / dt;
91			dz = dz / dt;
92
93			consAtom1->getVel(velA);
94
95			velA[0] += rma * dx;
96			velA[1] += rma * dy;
97			velA[2] += rma * dz;
98
99			consAtom1->setVel(velA);
100
101			consAtom2->getVel(velB);
102
103			velB[0] -= rmb * dx;
104			velB[1] -= rmb * dy;
105			velB[2] -= rmb * dz;
106
107			consAtom2->setVel(velB);
108
109			return consSuccess;
110			}
111			else
112			return consAlready;
113	tim	1232	}
114
115
116
117			int DCShakeRMinFunctor::operator()(ConstraintAtom* consAtom,ConstraintRigidBody* consRB){
118			return consElemHandlerFail;
119
120			}
121
122			int DCShakeRMinFunctor::operator()(ConstraintRigidBody* consRB1, ConstraintRigidBody* consRB2){
123			return consElemHandlerFail;
124			}
125
126			////////////////////////////////////////////////////////////////////////////////
127			//Implementation of JCShakeMinRFunctor
128			////////////////////////////////////////////////////////////////////////////////
129			int JCShakeRMInFunctor::operator()(ConstraintAtom* consAtom1, ConstraintAtom* consAtom2){
130			return consElemHandlerFail;
131			}
132
133
134
135			int JCShakeRMInFunctor::operator()(ConstraintAtom* consAtom,ConstraintRigidBody* consRB){
136			return consElemHandlerFail;
137			}
138
139			int JCShakeRMInFunctor::operator()(ConstraintRigidBody* consRB1, ConstraintRigidBody* consRB2){
140			return consElemHandlerFail;
141			}
142
143
144			////////////////////////////////////////////////////////////////////////////////
145			//Implementation of DCShakeMinFFunctor
146			////////////////////////////////////////////////////////////////////////////////
147			int DCShakeMinFFunctor::operator()(ConstraintAtom* consAtom1, ConstraintAtom* consAtom2){
148
149	tim	1248	double posA[3];
150			double posB[3];
151			double frcA[3];
152			double frcB[3];
153			double rab[3];
154			double fpab[3];
155			double rma;
156			double rmb;
157			double gab;
158			double rabsq;
159			double rfab;
160	tim	1232
161
162	tim	1248
163			consAtom1->getPos(posA);
164			consAtom2->getPos(posB);
165
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			consAtom1->getFrc(frcA);
174			consAtom2->getFrc(frcB);
175
176			rma = 1.0;
177			rmb = 1.0;
178
179
180			fpab[0] = frcA[0] * rma - frcB[0] * rmb;
181			fpab[1] = frcA[1] * rma - frcB[1] * rmb;
182			fpab[2] = frcA[2] * rma - frcB[2] * rmb;
183
184
185			gab=fpab[0] * fpab[0] + fpab[1] * fpab[1] + fpab[2] * fpab[2];
186
187			if (gab < 1.0)
188			gab = 1.0;
189
190			rabsq = rab[0] * rab[0] + rab[1] * rab[1] + rab[2] * rab[2];
191			rfab = rab[0] * fpab[0] + rab[1] * fpab[1] + rab[2] * fpab[2];
192
193			if (fabs(rfab) > sqrt(rabsqgab) consTolerance){
194
195			gab = -rfab /(rabsq*(rma + rmb));
196
197			frcA[0] = rab[0] * gab;
198			frcA[1] = rab[1] * gab;
199			frcA[2] = rab[2] * gab;
200
201			consAtom1->addFrc(frcA);
202
203			frcB[0] = -rab[0] * gab;
204			frcB[1] = -rab[1] * gab;
205			frcB[2] = -rab[2] * gab;
206
207			consAtom2->addFrc(frcB);
208
209			}
210
211			return consSuccess;
212
213			}
214
215
216
217	tim	1232	int DCShakeMinFFunctor::operator()(ConstraintAtom* consAtom,ConstraintRigidBody* consRB){
218			return consElemHandlerFail;
219			}
220
221			int DCShakeMinFFunctor::operator()(ConstraintRigidBody* consRB1, ConstraintRigidBody* consRB2){
222			return consElemHandlerFail;
223			}
224
225			////////////////////////////////////////////////////////////////////////////////
226			//Implementation of JCShakeMinFFunctor
227			////////////////////////////////////////////////////////////////////////////////
228			int JCShakeMinFFunctor::operator()(ConstraintAtom* consAtom1, ConstraintAtom* consAtom2){
229			return consElemHandlerFail;
230			}
231
232
233
234			int JCShakeMinFFunctor::operator()(ConstraintAtom* consAtom,ConstraintRigidBody* consRB){
235			return consElemHandlerFail;
236			}
237
238			int JCShakeMinFFunctor::operator()(ConstraintRigidBody* consRB1, ConstraintRigidBody* consRB2){
239			return consElemHandlerFail;
240	tim	1248	}