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 1268 by tim, Fri Jun 11 17:16:21 2004 UTC vs.
Revision 1452 by tim, Mon Aug 23 15:11:36 2004 UTC

# Line 32 | Line 32 | int DCRollAFunctor::operator()(ConstraintAtom* consAto
32    double pabDotZeta2;
33    double zeta2;
34    double forceScalar;
35 +
36    
36 –  const int conRBMaxIter = 10;
37 –  
37    dt = info->dt;
38  
39    consAtom1->getOldPos(oldPosA.vec);
40    consAtom2->getOldPos(oldPosB.vec);      
41  
42  
44 –  for(int i=0 ; i < conRBMaxIter; i++){  
43      consAtom1->getPos(posA.vec);
44      consAtom2->getPos(posB.vec);
45  
# Line 91 | Line 89 | int DCRollAFunctor::operator()(ConstraintAtom* consAto
89        //G : constraint force scalar
90        //d: equilibrium bond length
91        
92 <      if (pabDotZeta2 - zeta2 * diffsq < 0)
92 >      if (pabDotZeta2 - zeta2 * diffsq < 0)  
93          return consFail;
94 <
94 >      
95        //forceScalar = (pabDotZeta + sqrt(pabDotZeta2 - zeta2 * diffsq)) / dt * dt * zeta2;
96        forceScalar = diffsq / (2 * dt * dt * pabDotZeta);
97 <      //
97 >      //forceScalar = 1 / forceScalar;
98        consForce = forceScalar * bondDirUnitVec;
99        //integrate consRB1 using constraint force;
100        integrate(consAtom1, consForce);
101  
102        //integrate consRB2 using constraint force;
103        integrate(consAtom2, -consForce);    
104 <      
104 >
105 >      return consSuccess;
106      }
107 <    else{
109 <      if (i ==0)
107 >    else
108          return consAlready;
111 –      else
112 –        return consSuccess;
113 –    }
114 –  }
109  
110 <  return consExceedMaxIter;
110 >      
111  
112 +
113   }
114   void DCRollAFunctor::calcZeta(ConstraintAtom* consAtom, const Vector3d& bondDir, Vector3d&zeta){
115    double invMass;
# Line 129 | Line 124 | void DCRollAFunctor::integrate(ConstraintAtom* consAto
124    Vector3d pos;
125    Vector3d tempPos;
126    Vector3d tempVel;
132 –
127    double mass;
134 –  double dtOver2;
128    double dt;
136 –  const double eConvert = 4.184e-4;
129    
130    dt = info->dt;
139 –  dtOver2 = dt /2;  
131    sd = consAtom->getStuntDouble();
132    
133    sd->getVel(vel.vec);
# Line 144 | Line 135 | void DCRollAFunctor::integrate(ConstraintAtom* consAto
135    
136    mass = sd->getMass();
137  
138 <  tempVel = eConvert * dtOver2/mass * force;
138 >  tempVel = dt/mass * force;
139    tempPos = dt * tempVel;
140          
141    vel += tempVel;
# Line 180 | Line 171 | int DCRollAFunctor::operator()(ConstraintRigidBody* co
171    double zeta2;
172    double forceScalar;
173    
174 <  const int conRBMaxIter = 10;
174 >  const int conRBMaxIter = 100;
175    
176    dt = info->dt;
177  
# Line 209 | Line 200 | int DCRollAFunctor::operator()(ConstraintRigidBody* co
200        rab = oldPosB - oldPosA;      
201        info->wrapVector(rab.vec);
202  
203 <      //rpab = dotProduct(rab, pab);
213 <
214 <      //rpabsq = rpab * rpab;
215 <
216 <
217 <      //if (rpabsq < (rabsq * -diffsq)){
218 <      //  return consFail;
219 <      //}
220 <
221 <      bondDirUnitVec = pab;
203 >      bondDirUnitVec = rab;
204        bondDirUnitVec.normalize();
205            
206        calcZeta(consRB1, bondDirUnitVec, zetaA);
# Line 238 | Line 220 | int DCRollAFunctor::operator()(ConstraintRigidBody* co
220        //G : constraint force scalar
221        //d: equilibrium bond length
222        
223 <      if (pabDotZeta2 - zeta2 * diffsq < 0)
223 >      if (pabDotZeta2 - zeta2 * diffsq < 0){
224 >        cerr << "DCRollAFunctor::operator() Error: Constraint Fail at " << info->getTime() << endl;  
225          return consFail;
226 <
227 <      //forceScalar = (pabDotZeta + sqrt(pabDotZeta2 - zeta2 * diffsq)) / dt * dt * zeta2;
228 <      forceScalar = diffsq / (2 * dt * dt * pabDotZeta);
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;
# Line 260 | 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   }
# Line 271 | Line 259 | void DCRollAFunctor::calcZeta(ConstraintRigidBody* con
259    Vector3d refCoor;
260    Vector3d refCrossBond;  
261    Mat3x3d IBody;
274 –  Mat3x3d IFrame;
262    Mat3x3d invIBody;
263 <  Mat3x3d invIFrame;
263 >  Mat3x3d invILab;
264    Mat3x3d a;
265    Mat3x3d aTrans;
266    
# Line 282 | Line 269 | void DCRollAFunctor::calcZeta(ConstraintRigidBody* con
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    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 tempPos;
299 <        Vector3d tempVel;
300 <        Vector3d tempTrq;
301 <        Vector3d tempJi;
298 >  Vector3d refCoor;
299 >  Vector3d consTorque;
300 >  Vector3d totConsTorque;
301 >  Mat3x3d a;
302    double mass;
313 –  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);
322 <  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 <  tempVel = eConvert * dtOver2/mass * force;
327 <  tempPos = dt * tempVel;
328 <        
329 <        vel += tempVel;
330 <        pos += tempPos;
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 >  ji += eConvert * dtOver2 * Tb;
356  
357 <    rotationPropagation( sd, ji.vec);
357 >  rotationPropagation( rb, ji.vec );
358  
359 <    sd->setJ(ji.vec);
351 <  }
359 >  rb->setJ(ji.vec);
360    
361   }
362  
# Line 357 | 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 373 | 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 389 | 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];
393 <    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 496 | 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;  
503 –  double dx, dy, dz;
504 –  double rpab;
505 –  double rabsq, pabsq, rpabsq;
506 –  double diffsq;
507 –  double gab;
512    double dt;
513 <  double pabcDotvab;
514 <  double pabDotInvMassVec;
513 >  double pabDotvab;
514 >  double pabDotZeta;
515 >  double pvab;
516  
517 <
513 <  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;
521 <    
522 <    consRB1->getVel(velA.vec);
523 <    consRB2->getVel(velB.vec);
524 <    vab = velA -velB;
524 >    pab = posB - posA;
525  
526      //periodic boundary condition
527 –
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();
533 <    diffsq = rabsq - pabsq;
535 >    if (fabs(pvab) > consTolerance ){
536  
535 –    if (fabs(diffsq) > (consTolerance * rabsq * 2)){
537  
537 –
538        bondDirUnitVec = pab;
539        bondDirUnitVec.normalize();
540            
541 <      getEffInvMassVec(consRB1, bondDirUnitVec, rma);
542 <
543 <      getEffInvMassVec(consRB2, bondDirUnitVec, rmb);
544 <
545 <      pabcDotvab = dotProduct(pab, vab);
546 <      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 561 | 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;
579 –  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  
589 –  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  
599 –  invMassVec += tempVec2;
599   }
600  
601   void DCRollBFunctor::integrate(ConstraintRigidBody* consRB, const Vector3d& force){
603 –  const double eConvert = 4.184e-4;
602    Vector3d vel;
605 –  Vector3d pos;
606 –  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 <
617 <  // velocity half step
618 <
619 <  vel += eConvert * dtOver2 /mass * force;
620 <
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
626 <
627 <    sd->getTrq(Tb.vec);
628 <    sd->lab2Body(Tb.vec);
629 <
630 <    // get the angular momentum, and propagate a half step
631 <
625 >    tempJi = dtOver2* tempTrq;
626      sd->getJ(ji.vec);
627 <
634 <    ji += eConvert * dtOver2* Tb;
635 <
627 >    ji += tempJi;
628      sd->setJ(ji.vec);
629    }
630    

Diff Legend

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