ViewVC Help
View File | Revision Log | Show Annotations | View Changeset | Root Listing
root/group/trunk/OOPSE/libmdtools/Roll.cpp
(Generate patch)

Comparing trunk/OOPSE/libmdtools/Roll.cpp (file contents):
Revision 1255 by tim, Wed Jun 9 16:59:03 2004 UTC vs.
Revision 1452 by tim, Mon Aug 23 15:11:36 2004 UTC

# Line 7 | Line 7
7   ////////////////////////////////////////////////////////////////////////////////
8   //Implementation of DCRollAFunctor
9   ////////////////////////////////////////////////////////////////////////////////
10 < int DCRollAFunctor::operator()(ConstraintRigidBody* consRB1, ConstraintRigidBody* consRB2){
10 > int DCRollAFunctor::operator()(ConstraintAtom* consAtom1, ConstraintAtom* consAtom2){
11    Vector3d posA;
12    Vector3d posB;
13    Vector3d oldPosA;
# Line 17 | Line 17 | int DCRollAFunctor::operator()(ConstraintRigidBody* co
17    Vector3d pab;
18    Vector3d tempPab;
19    Vector3d rab;
20 <  Vector3d rma;
21 <  Vector3d rmb;
20 >  Vector3d zetaA;
21 >  Vector3d zetaB;
22 >  Vector3d zeta;
23    Vector3d consForce;
24    Vector3d bondDirUnitVec;  
25    double dx, dy, dz;
# Line 27 | Line 28 | int DCRollAFunctor::operator()(ConstraintRigidBody* co
28    double diffsq;
29    double gab;
30    double dt;
31 <  double pabDotInvMassVec;
31 >  double pabDotZeta;
32 >  double pabDotZeta2;
33 >  double zeta2;
34 >  double forceScalar;
35  
32 –
33 –  const int conRBMaxIter = 10;
36    
37    dt = info->dt;
38  
39 <  consRB1->getOldAtomPos(oldPosA.vec);
40 <  consRB2->getOldAtomPos(oldPosB.vec);      
39 >  consAtom1->getOldPos(oldPosA.vec);
40 >  consAtom2->getOldPos(oldPosB.vec);      
41  
42  
43 <  for(int i=0 ; i < conRBMaxIter; i++){  
44 <    consRB1->getCurAtomPos(posA.vec);
43 <    consRB2->getCurAtomPos(posB.vec);
43 >    consAtom1->getPos(posA.vec);
44 >    consAtom2->getPos(posB.vec);
45  
46 <    pab = posA - posB;
46 >    //discard the vector convention in alan tidesley's code
47 >    //rij =  rj - ri;
48 >    pab = posB - posA;
49  
50      //periodic boundary condition
51  
# Line 51 | Line 54 | int DCRollAFunctor::operator()(ConstraintRigidBody* co
54      pabsq = dotProduct(pab, pab);
55  
56      rabsq = curPair->getBondLength2();
57 <    diffsq = rabsq - pabsq;
57 >    diffsq =  pabsq -rabsq;
58  
59      if (fabs(diffsq) > (consTolerance * rabsq * 2)){
60 <      rab = oldPosA - oldPosB;      
60 >      rab = oldPosB - oldPosA;      
61        info->wrapVector(rab.vec);
62  
63 <      rpab = dotProduct(rab, pab);
63 >      //rpab = dotProduct(rab, pab);
64  
65 <      rpabsq = rpab * rpab;
65 >      //rpabsq = rpab * rpab;
66  
67  
68        //if (rpabsq < (rabsq * -diffsq)){
# Line 69 | Line 72 | int DCRollAFunctor::operator()(ConstraintRigidBody* co
72        bondDirUnitVec = pab;
73        bondDirUnitVec.normalize();
74            
75 <      getEffInvMassVec(consRB1, bondDirUnitVec, rma);
75 >      calcZeta(consAtom1, bondDirUnitVec, zetaA);
76  
77 <      getEffInvMassVec(consRB2, -bondDirUnitVec, rmb);
77 >      calcZeta(consAtom2, bondDirUnitVec, zetaB);
78  
79 <      pabDotInvMassVec = dotProduct(pab,  rma + rmb);
79 >      zeta = zetaA + zetaB;      
80 >      zeta2 = dotProduct(zeta, zeta);
81        
82 <      consForce = diffsq /(2 * dt * dt * pabDotInvMassVec) * bondDirUnitVec;
82 >      pabDotZeta = dotProduct(pab,  zeta);
83 >      pabDotZeta2 = pabDotZeta * pabDotZeta;
84 >
85 >      //solve quadratic equation
86 >      //dt^4 * zeta^2 * G^2 + 2* h^2 * pab * zeta * G + pab^2 - d^2
87 >      //dt : time step
88 >      // pab :
89 >      //G : constraint force scalar
90 >      //d: equilibrium bond length
91 >      
92 >      if (pabDotZeta2 - zeta2 * diffsq < 0)  
93 >        return consFail;
94 >      
95 >      //forceScalar = (pabDotZeta + sqrt(pabDotZeta2 - zeta2 * diffsq)) / dt * dt * zeta2;
96 >      forceScalar = diffsq / (2 * dt * dt * pabDotZeta);
97 >      //forceScalar = 1 / forceScalar;
98 >      consForce = forceScalar * bondDirUnitVec;
99        //integrate consRB1 using constraint force;
100 <      integrate(consRB1,consForce);
100 >      integrate(consAtom1, consForce);
101  
102        //integrate consRB2 using constraint force;
103 +      integrate(consAtom2, -consForce);    
104 +
105 +      return consSuccess;
106 +    }
107 +    else
108 +        return consAlready;
109 +
110 +      
111 +
112 +
113 + }
114 + void DCRollAFunctor::calcZeta(ConstraintAtom* consAtom, const Vector3d& bondDir, Vector3d&zeta){
115 +  double invMass;
116 +  invMass = 1.0 / consAtom ->getMass();
117 +
118 +  zeta = invMass * bondDir;
119 + }
120 +
121 + void DCRollAFunctor::integrate(ConstraintAtom* consAtom, const Vector3d& force){
122 +  StuntDouble* sd;
123 +  Vector3d vel;
124 +  Vector3d pos;
125 +  Vector3d tempPos;
126 +  Vector3d tempVel;
127 +  double mass;
128 +  double dt;
129 +  
130 +  dt = info->dt;
131 +  sd = consAtom->getStuntDouble();
132 +  
133 +  sd->getVel(vel.vec);
134 +  sd->getPos(pos.vec);
135 +  
136 +  mass = sd->getMass();
137 +
138 +  tempVel = dt/mass * force;
139 +  tempPos = dt * tempVel;
140 +        
141 +  vel += tempVel;
142 +  pos += tempPos;
143 +
144 +  sd->setVel(vel.vec);
145 +  sd->setPos(pos.vec);
146 + }
147 +
148 + int DCRollAFunctor::operator()(ConstraintRigidBody* consRB1, ConstraintRigidBody* consRB2){
149 +  Vector3d posA;
150 +  Vector3d posB;
151 +  Vector3d oldPosA;
152 +  Vector3d oldPosB;
153 +  Vector3d velA;
154 +  Vector3d velB;
155 +  Vector3d pab;
156 +  Vector3d tempPab;
157 +  Vector3d rab;
158 +  Vector3d zetaA;
159 +  Vector3d zetaB;
160 +  Vector3d zeta;
161 +  Vector3d consForce;
162 +  Vector3d bondDirUnitVec;  
163 +  double dx, dy, dz;
164 +  double rpab;
165 +  double rabsq, pabsq, rpabsq;
166 +  double diffsq;
167 +  double gab;
168 +  double dt;
169 +  double pabDotZeta;
170 +  double pabDotZeta2;
171 +  double zeta2;
172 +  double forceScalar;
173 +  
174 +  const int conRBMaxIter = 100;
175 +  
176 +  dt = info->dt;
177 +
178 +  consRB1->getOldAtomPos(oldPosA.vec);
179 +  consRB2->getOldAtomPos(oldPosB.vec);      
180 +
181 +
182 +  for(int i=0 ; i < conRBMaxIter; i++){  
183 +    consRB1->getCurAtomPos(posA.vec);
184 +    consRB2->getCurAtomPos(posB.vec);
185 +
186 +    //discard the vector convention in alan tidesley's code
187 +    //rij =  rj - ri;
188 +    pab = posB - posA;
189 +
190 +    //periodic boundary condition
191 +
192 +    info->wrapVector(pab.vec);
193 +
194 +    pabsq = dotProduct(pab, pab);
195 +
196 +    rabsq = curPair->getBondLength2();
197 +    diffsq =  pabsq -rabsq;
198 +
199 +    if (fabs(diffsq) > (consTolerance * rabsq * 2)){
200 +      rab = oldPosB - oldPosA;      
201 +      info->wrapVector(rab.vec);
202 +
203 +      bondDirUnitVec = rab;
204 +      bondDirUnitVec.normalize();
205 +          
206 +      calcZeta(consRB1, bondDirUnitVec, zetaA);
207 +
208 +      calcZeta(consRB2, bondDirUnitVec, zetaB);
209 +
210 +      zeta = zetaA + zetaB;      
211 +      zeta2 = dotProduct(zeta, zeta);
212 +      
213 +      pabDotZeta = dotProduct(pab,  zeta);
214 +      pabDotZeta2 = pabDotZeta * pabDotZeta;
215 +
216 +      //solve quadratic equation
217 +      //dt^4 * zeta^2 * G^2 + 2* h^2 * pab * zeta * G + pab^2 - d^2
218 +      //dt : time step
219 +      // pab :
220 +      //G : constraint force scalar
221 +      //d: equilibrium bond length
222 +      
223 +      if (pabDotZeta2 - zeta2 * diffsq < 0){
224 +        cerr << "DCRollAFunctor::operator() Error: Constraint Fail at " << info->getTime() << endl;  
225 +        return consFail;
226 +      }
227 +      //if pabDotZeta is close to 0, we can't neglect the square term
228 +      if(fabs(pabDotZeta) < consTolerance)
229 +        forceScalar = (pabDotZeta - sqrt(pabDotZeta2 - zeta2 * diffsq)) / dt * dt * zeta2;
230 +      else
231 +        forceScalar = diffsq / (2 * dt * dt * pabDotZeta);
232 +        
233 +      //
234 +      consForce = forceScalar * bondDirUnitVec;
235 +      //integrate consRB1 using constraint force;
236 +      integrate(consRB1, consForce);
237 +
238 +      //integrate consRB2 using constraint force;
239        integrate(consRB2, -consForce);    
240        
241      }
# Line 91 | Line 247 | int DCRollAFunctor::operator()(ConstraintRigidBody* co
247      }
248    }
249  
250 +  cerr << "DCRollAFunctor::operator() Error: can not constrain the bond within maximum iteration at " << info->getTime() << endl;
251    return consExceedMaxIter;
252  
253   }
254  
255 < void DCRollAFunctor::getEffInvMassVec(ConstraintRigidBody* consRB, const Vector3d& bondDir, Vector3d& invMassVec){
255 > void DCRollAFunctor::calcZeta(ConstraintRigidBody* consRB, const Vector3d& bondDir, Vector3d& zeta){
256    double invMass;
257    Vector3d tempVec1;
258    Vector3d tempVec2;
259    Vector3d refCoor;
260    Vector3d refCrossBond;  
261    Mat3x3d IBody;
105 –  Mat3x3d IFrame;
262    Mat3x3d invIBody;
263 <  Mat3x3d invIFrame;
263 >  Mat3x3d invILab;
264    Mat3x3d a;
265    Mat3x3d aTrans;
266    
267    invMass = 1.0 / consRB ->getMass();
268  
269 <  invMassVec = invMass * bondDir;
269 >  zeta = invMass * bondDir;
270    
271    consRB->getRefCoor(refCoor.vec);
272 <  consRB->getA(a.element);
272 >  //consRB->getA(a.element);
273 >  consRB->getOldA(a.element);
274    consRB->getI(IBody.element);
275  
276    aTrans = a.transpose();
277    invIBody = IBody.inverse();
278  
279 <  IFrame = aTrans * invIBody * a;
279 >  invILab = aTrans * invIBody * a;
280  
281 <  refCrossBond = crossProduct(refCoor, bondDir);  
281 >  refCrossBond = crossProduct(aTrans *refCoor, bondDir);  
282  
283 <  tempVec1 = invIFrame * refCrossBond;
284 <  tempVec2 = crossProduct(tempVec1, refCoor);
283 >  tempVec1 = invILab * refCrossBond;
284 >  tempVec2 = crossProduct(tempVec1, aTrans *refCoor);
285  
286 <  invMassVec += tempVec2;
286 >  zeta += tempVec2;
287    
288   }
289  
290 < void DCRollAFunctor::integrate(ConstraintRigidBody* consRB, const Vector3d& force){
291 <  StuntDouble* sd;
290 > void DCRollAFunctor::integrate(ConstraintRigidBody* consRB, const Vector3d& consForce){
291 >  RigidBody* rb;
292 >  Vector3d frc;
293 >  Vector3d totConsForce;
294    Vector3d vel;
295    Vector3d pos;
296    Vector3d Tb;
297    Vector3d ji;
298 +  Vector3d refCoor;
299 +  Vector3d consTorque;
300 +  Vector3d totConsTorque;
301 +  Mat3x3d a;
302    double mass;
140 –  double dtOver2;
303    double dt;
304 +  double dtOver2;
305    const double eConvert = 4.184e-4;
306    
307    dt = info->dt;
308 <  dtOver2 = dt /2;  
309 <  sd = consRB->getStuntDouble();
308 >  dtOver2 = dt /2;
309 >
310 >  //restore to old status
311 >  consRB->restoreUnconsStatus();
312 >
313 >  //accumulate constraint force;
314 >  consRB->addConsForce(consForce/eConvert);
315 >  totConsForce = consRB->getConsForce();
316 >
317 >  rb = consRB->getRigidBody();
318    
319 <  sd->getVel(vel.vec);
149 <  sd->getPos(pos.vec);
319 >  rb->addFrc(totConsForce.vec);
320    
321 <  mass = sd->getMass();
321 >  rb->getVel(vel.vec);
322 >  rb->getPos(pos.vec);
323 >  rb->getFrc(frc.vec);
324 >  mass = rb->getMass();
325  
326 <  vel += eConvert * dtOver2/mass * force;
326 >  // velocity half step
327 >  vel += eConvert  * dtOver2 / mass * frc;
328 >  // position whole step
329    pos += dt * vel;
330  
331 <  sd->setVel(vel.vec);
332 <  sd->setPos(pos.vec);
331 >  rb->setVel(vel.vec);
332 >  rb->setPos(pos.vec);
333  
334 <  if (sd->isDirectional()){
334 >  //evolve orientational part
335 >  consRB->getRefCoor(refCoor.vec);
336 >  rb->getA(a.element);
337  
338 <    // get and convert the torque to body frame
338 >  //calculate constraint torque in lab frame
339 >  consTorque = crossProduct(a.transpose() * refCoor, consForce);
340 >  consRB->addConsTorque(consTorque/eConvert);
341  
342 <    sd->getTrq(Tb.vec);
343 <    sd->lab2Body(Tb.vec);
342 >  //add constraint torque
343 >  totConsTorque = consRB->getConsTorque();  
344 >  rb->addTrq(totConsTorque.vec);
345 >  
346 >  //get and convert the torque to body frame
347  
348 <    // get the angular momentum, and propagate a half step
348 >  rb->getTrq(Tb.vec);
349 >  rb->lab2Body(Tb.vec);
350  
351 <    sd->getJ(ji.vec);
351 >  //get the angular momentum, and propagate a half step
352  
353 <    ji += eConvert * dtOver2 * Tb;
353 >  rb->getJ(ji.vec);
354  
355 <    rotationPropagation( sd, ji.vec);
355 >  ji += eConvert * dtOver2 * Tb;
356  
357 <    sd->setJ(ji.vec);
358 <  }
357 >  rotationPropagation( rb, ji.vec );
358 >
359 >  rb->setJ(ji.vec);
360    
361   }
362  
# Line 181 | Line 365 | void DCRollAFunctor::rotationPropagation(StuntDouble*
365    double A[3][3], I[3][3];
366    int i, j, k;
367    double dtOver2;
368 <
369 <  dtOver2 = info->dt /2;
368 >  double dt;
369 >  dt = info->dt;
370 >  dtOver2 = dt /2;
371    // use the angular velocities to propagate the rotation matrix a
372    // full time step
373  
# Line 197 | Line 382 | void DCRollAFunctor::rotationPropagation(StuntDouble*
382      angle = dtOver2 * ji[j] / I[j][j];
383      this->rotate( k, i, angle, ji, A );
384  
385 <    angle = dtOver2 * ji[k] / I[k][k];
385 >    angle = dt* ji[k] / I[k][k];
386      this->rotate( i, j, angle, ji, A);
387  
388      angle = dtOver2 * ji[j] / I[j][j];
# Line 213 | Line 398 | void DCRollAFunctor::rotationPropagation(StuntDouble*
398      this->rotate( 2, 0, angle, ji, A );
399      
400      // rotate about the z-axis
401 <    angle = dtOver2 * ji[2] / I[2][2];
217 <    sd->addZangle(angle);
401 >    angle = dt * ji[2] / I[2][2];
402      this->rotate( 0, 1, angle, ji, A);
403      
404      // rotate about the y-axis
# Line 320 | Line 504 | int DCRollBFunctor::operator()(ConstraintRigidBody* co
504    Vector3d pab;
505    Vector3d rab;
506    Vector3d vab;
507 <  Vector3d rma;
508 <  Vector3d rmb;
507 >  Vector3d zetaA;
508 >  Vector3d zetaB;
509 >  Vector3d zeta;
510    Vector3d consForce;
511    Vector3d bondDirUnitVec;  
327 –  double dx, dy, dz;
328 –  double rpab;
329 –  double rabsq, pabsq, rpabsq;
330 –  double diffsq;
331 –  double gab;
512    double dt;
513 <  double pabcDotvab;
514 <  double pabDotInvMassVec;
513 >  double pabDotvab;
514 >  double pabDotZeta;
515 >  double pvab;
516  
517 <
337 <  const int conRBMaxIter = 10;
517 >  const int conRBMaxIter = 20;
518    
519    dt = info->dt;
520    
521    for(int i=0 ; i < conRBMaxIter; i++){  
522      consRB1->getCurAtomPos(posA.vec);
523      consRB2->getCurAtomPos(posB.vec);
524 <    pab = posA - posB;
345 <    
346 <    consRB1->getVel(velA.vec);
347 <    consRB2->getVel(velB.vec);
348 <    vab = velA -velB;
524 >    pab = posB - posA;
525  
526      //periodic boundary condition
351 –
527      info->wrapVector(pab.vec);
528 +    
529 +    consRB1->getCurAtomVel(velA.vec);
530 +    consRB2->getCurAtomVel(velB.vec);
531 +    vab = velB -velA;
532  
533 <    pabsq = pab.length2();
533 >    pvab = dotProduct(pab, vab);
534  
535 <    rabsq = curPair->getBondLength2();
357 <    diffsq = rabsq - pabsq;
535 >    if (fabs(pvab) > consTolerance ){
536  
359 –    if (fabs(diffsq) > (consTolerance * rabsq * 2)){
537  
361 –
538        bondDirUnitVec = pab;
539        bondDirUnitVec.normalize();
540            
541 <      getEffInvMassVec(consRB1, bondDirUnitVec, rma);
542 <
543 <      getEffInvMassVec(consRB2, -bondDirUnitVec, rmb);
368 <
369 <      pabcDotvab = dotProduct(pab, vab);
370 <      pabDotInvMassVec = dotProduct(pab,  rma + rmb);
541 >      getZeta(consRB1, bondDirUnitVec, zetaA);
542 >      getZeta(consRB2, bondDirUnitVec, zetaB);
543 >      zeta = zetaA + zetaB;
544        
545 <      consForce = pabcDotvab /(2 * dt * pabDotInvMassVec) * bondDirUnitVec;
545 >      pabDotZeta = dotProduct(pab,  zeta);
546 >      
547 >      consForce =  2 * pvab / (dt * pabDotZeta) * bondDirUnitVec;
548        //integrate consRB1 using constraint force;
549 <      integrate(consRB1,consForce);
549 >      integrate(consRB1, consForce);
550  
551        //integrate consRB2 using constraint force;
552        integrate(consRB2, -consForce);    
# Line 385 | Line 560 | int DCRollBFunctor::operator()(ConstraintRigidBody* co
560      }
561    }
562  
563 +  cerr << "DCRollBFunctor::operator() Error: can not constrain the bond within maximum iteration at " << info->getTime() << endl;  
564    return consExceedMaxIter;
565  
566   }
567  
568 < void DCRollBFunctor::getEffInvMassVec(ConstraintRigidBody* consRB, const Vector3d& bondDir, Vector3d& invMassVec){
568 > void DCRollBFunctor::getZeta(ConstraintRigidBody* consRB, const Vector3d& bondDir, Vector3d& zeta){
569    double invMass;
570    Vector3d tempVec1;
571    Vector3d tempVec2;
572    Vector3d refCoor;
573    Vector3d refCrossBond;  
574    Mat3x3d IBody;
575 <  Mat3x3d IFrame;
575 >  Mat3x3d ILab;
576    Mat3x3d invIBody;
577 <  Mat3x3d invIFrame;
577 >  Mat3x3d invILab;
578    Mat3x3d a;
403 –  Mat3x3d aTrans;
579    
580    invMass = 1.0 / consRB ->getMass();
581  
582 <  invMassVec = invMass * bondDir;
582 >  zeta = invMass * bondDir;
583    
584    consRB->getRefCoor(refCoor.vec);
585    consRB->getA(a.element);
586    consRB->getI(IBody.element);
587  
413 –  aTrans = a.transpose();
588    invIBody = IBody.inverse();
589  
590 <  IFrame = aTrans * invIBody * a;
590 >  
591 >  refCrossBond = crossProduct(refCoor, a * bondDir);  
592  
593 <  refCrossBond = crossProduct(refCoor, bondDir);  
593 >  tempVec1 = invIBody * refCrossBond;
594  
595 <  tempVec1 = invIFrame * refCrossBond;
596 <  tempVec2 = crossProduct(tempVec1, refCoor);
595 >  tempVec2 = (a * tempVec1.makeSkewMat()).transpose() * refCoor;
596 >  
597 >  zeta += tempVec2;
598  
423 –  invMassVec += tempVec2;
599   }
600  
601   void DCRollBFunctor::integrate(ConstraintRigidBody* consRB, const Vector3d& force){
427 –  const double eConvert = 4.184e-4;
602    Vector3d vel;
429 –  Vector3d pos;
430 –  Vector3d Tb;
603    Vector3d ji;
604 +  Vector3d tempJi;
605 +  Vector3d tempTrq;
606 +  Vector3d refCoor;
607    double mass;
608    double dtOver2;
609 +  Mat3x3d a;
610    StuntDouble* sd;
611    
612    sd = consRB->getStuntDouble();  
613    dtOver2 = info->dt/2;
614  
615 +  sd->getVel(vel.vec);
616    mass = sd->getMass();
617 <
441 <  // velocity half step
442 <
443 <  vel += eConvert * dtOver2 /mass * force;
444 <
617 >  vel +=dtOver2 /mass * force;
618    sd->setVel(vel.vec);
619  
620    if (sd->isDirectional()){
621 +    sd->getA(a.element);
622 +    consRB->getRefCoor(refCoor.vec);
623 +    tempTrq = crossProduct(refCoor, a *force);
624  
625 <    // get and convert the torque to body frame
450 <
451 <    sd->getTrq(Tb.vec);
452 <    sd->lab2Body(Tb.vec);
453 <
454 <    // get the angular momentum, and propagate a half step
455 <
625 >    tempJi = dtOver2* tempTrq;
626      sd->getJ(ji.vec);
627 <
458 <    ji += eConvert * dtOver2* Tb;
459 <
627 >    ji += tempJi;
628      sd->setJ(ji.vec);
629    }
630    

Diff Legend

– Removed lines
+ Added lines
< Changed lines
> Changed lines