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
#	Content
1	#include <cmath>
2	#include "ConstraintElement.hpp"
3	#include "ShakeMin.hpp"
4	#include "SimInfo.hpp"
5	////////////////////////////////////////////////////////////////////////////////
6	//Implementation of DCShakeMinRFunctor
7	////////////////////////////////////////////////////////////////////////////////
8	int DCShakeRMinFunctor::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
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	}
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	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
161
162
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	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	}