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 1283 by tim, Fri Jun 11 17:16:21 2004 UTC vs.
Revision 1284 by tim, Mon Jun 21 18:52:21 2004 UTC

# Line 91 | Line 91 | int DCRollAFunctor::operator()(ConstraintAtom* consAto
91        //G : constraint force scalar
92        //d: equilibrium bond length
93        
94 <      if (pabDotZeta2 - zeta2 * diffsq < 0)
94 >      if (pabDotZeta2 - zeta2 * diffsq < 0)  
95          return consFail;
96 <
96 >      
97        //forceScalar = (pabDotZeta + sqrt(pabDotZeta2 - zeta2 * diffsq)) / dt * dt * zeta2;
98        forceScalar = diffsq / (2 * dt * dt * pabDotZeta);
99 <      //
99 >      //forceScalar = 1 / forceScalar;
100        consForce = forceScalar * bondDirUnitVec;
101        //integrate consRB1 using constraint force;
102        integrate(consAtom1, consForce);
# Line 129 | Line 129 | void DCRollAFunctor::integrate(ConstraintAtom* consAto
129    Vector3d pos;
130    Vector3d tempPos;
131    Vector3d tempVel;
132 –
132    double mass;
134 –  double dtOver2;
133    double dt;
136 –  const double eConvert = 4.184e-4;
134    
135    dt = info->dt;
139 –  dtOver2 = dt /2;  
136    sd = consAtom->getStuntDouble();
137    
138    sd->getVel(vel.vec);
# Line 144 | Line 140 | void DCRollAFunctor::integrate(ConstraintAtom* consAto
140    
141    mass = sd->getMass();
142  
143 <  tempVel = eConvert * dtOver2/mass * force;
143 >  tempVel = dt/mass * force;
144    tempPos = dt * tempVel;
145          
146    vel += tempVel;
# Line 180 | Line 176 | int DCRollAFunctor::operator()(ConstraintRigidBody* co
176    double zeta2;
177    double forceScalar;
178    
179 <  const int conRBMaxIter = 10;
179 >  const int conRBMaxIter = 100;
180    
181    dt = info->dt;
182  
# Line 209 | Line 205 | int DCRollAFunctor::operator()(ConstraintRigidBody* co
205        rab = oldPosB - oldPosA;      
206        info->wrapVector(rab.vec);
207  
208 <      //rpab = dotProduct(rab, pab);
213 <
214 <      //rpabsq = rpab * rpab;
215 <
216 <
217 <      //if (rpabsq < (rabsq * -diffsq)){
218 <      //  return consFail;
219 <      //}
220 <
221 <      bondDirUnitVec = pab;
208 >      bondDirUnitVec = rab;
209        bondDirUnitVec.normalize();
210            
211        calcZeta(consRB1, bondDirUnitVec, zetaA);
# Line 238 | Line 225 | int DCRollAFunctor::operator()(ConstraintRigidBody* co
225        //G : constraint force scalar
226        //d: equilibrium bond length
227        
228 <      if (pabDotZeta2 - zeta2 * diffsq < 0)
228 >      if (pabDotZeta2 - zeta2 * diffsq < 0){
229 >        cerr << "DCRollAFunctor::operator() Error: Constraint Fail at " << info->getTime() << endl;  
230          return consFail;
231 <
232 <      //forceScalar = (pabDotZeta + sqrt(pabDotZeta2 - zeta2 * diffsq)) / dt * dt * zeta2;
233 <      forceScalar = diffsq / (2 * dt * dt * pabDotZeta);
231 >      }
232 >      //if pabDotZeta is close to 0, we can't neglect the square term
233 >      if(fabs(pabDotZeta) < consTolerance)
234 >        forceScalar = (pabDotZeta - sqrt(pabDotZeta2 - zeta2 * diffsq)) / dt * dt * zeta2;
235 >      else
236 >        forceScalar = diffsq / (2 * dt * dt * pabDotZeta);
237 >        
238        //
239        consForce = forceScalar * bondDirUnitVec;
240        //integrate consRB1 using constraint force;
# Line 260 | Line 252 | int DCRollAFunctor::operator()(ConstraintRigidBody* co
252      }
253    }
254  
255 +  cerr << "DCRollAFunctor::operator() Error: can not constrain the bond within maximum iteration at " << info->getTime() << endl;
256    return consExceedMaxIter;
257  
258   }
# Line 271 | Line 264 | void DCRollAFunctor::calcZeta(ConstraintRigidBody* con
264    Vector3d refCoor;
265    Vector3d refCrossBond;  
266    Mat3x3d IBody;
274 –  Mat3x3d IFrame;
267    Mat3x3d invIBody;
268 <  Mat3x3d invIFrame;
268 >  Mat3x3d invILab;
269    Mat3x3d a;
270    Mat3x3d aTrans;
271    
# Line 288 | Line 280 | void DCRollAFunctor::calcZeta(ConstraintRigidBody* con
280    aTrans = a.transpose();
281    invIBody = IBody.inverse();
282  
283 <  IFrame = aTrans * invIBody * a;
283 >  invILab = aTrans * invIBody * a;
284  
285    refCrossBond = crossProduct(refCoor, bondDir);  
286  
287 <  tempVec1 = invIFrame * refCrossBond;
287 >  tempVec1 = invILab * refCrossBond;
288    tempVec2 = crossProduct(tempVec1, refCoor);
289  
290    zeta += tempVec2;
# Line 305 | Line 297 | void DCRollAFunctor::integrate(ConstraintRigidBody* co
297    Vector3d pos;
298    Vector3d Tb;
299    Vector3d ji;
300 <        Vector3d tempPos;
301 <        Vector3d tempVel;
302 <        Vector3d tempTrq;
303 <        Vector3d tempJi;
300 >  Vector3d tempPos;
301 >  Vector3d tempVel;
302 >  Vector3d tempTrqLab;
303 >  Vector3d tempTrqBody;  
304 >  Vector3d tempJi;
305 >  Vector3d refCoor;
306    double mass;
307 <  double dtOver2;
307 >  Mat3x3d oldA;
308    double dt;
309 <  const double eConvert = 4.184e-4;
316 <  
309 >  double dtOver2;
310    dt = info->dt;
311 <  dtOver2 = dt /2;  
311 >  dtOver2 = dt /2;
312 >
313 >  consRB->getOldA(oldA.element);
314    sd = consRB->getStuntDouble();
315    
316    sd->getVel(vel.vec);
# Line 323 | Line 318 | void DCRollAFunctor::integrate(ConstraintRigidBody* co
318    
319    mass = sd->getMass();
320  
321 <  tempVel = eConvert * dtOver2/mass * force;
321 >  tempVel = dtOver2/mass * force;
322    tempPos = dt * tempVel;
323          
324          vel += tempVel;
# Line 334 | Line 329 | void DCRollAFunctor::integrate(ConstraintRigidBody* co
329  
330    if (sd->isDirectional()){
331  
332 <    // get and convert the torque to body frame
332 >    consRB->getRefCoor(refCoor.vec);
333 >    tempTrqLab = crossProduct(refCoor, force);
334  
335 <    sd->getTrq(Tb.vec);
336 <    sd->lab2Body(Tb.vec);
335 >    //convert torque in lab frame to torque in body frame using old rotation matrix
336 >    //tempTrqBody = oldA * tempTrqLab;
337 >    
338 >    //tempJi = dtOver2 * tempTrqBody;
339 >    sd->lab2Body(tempTrqLab.vec);
340 >    tempJi = dtOver2 * tempTrqLab;
341 >    rotationPropagation( sd, tempJi.vec);
342  
342 –    // get the angular momentum, and propagate a half step
343 –
343      sd->getJ(ji.vec);
344  
345 <    ji += eConvert * dtOver2 * Tb;
345 >    ji += tempJi;
346  
348 –    rotationPropagation( sd, ji.vec);
349 –
347      sd->setJ(ji.vec);
348    }
349 +
350    
351   }
352  
# Line 496 | Line 494 | int DCRollBFunctor::operator()(ConstraintRigidBody* co
494    Vector3d pab;
495    Vector3d rab;
496    Vector3d vab;
497 <  Vector3d rma;
498 <  Vector3d rmb;
497 >  Vector3d zetaA;
498 >  Vector3d zetaB;
499 >  Vector3d zeta;
500    Vector3d consForce;
501    Vector3d bondDirUnitVec;  
503 –  double dx, dy, dz;
504 –  double rpab;
505 –  double rabsq, pabsq, rpabsq;
506 –  double diffsq;
507 –  double gab;
502    double dt;
503 <  double pabcDotvab;
504 <  double pabDotInvMassVec;
503 >  double pabDotvab;
504 >  double pabDotZeta;
505 >  double pvab;
506  
507 <
513 <  const int conRBMaxIter = 10;
507 >  const int conRBMaxIter = 100;
508    
509    dt = info->dt;
510    
511    for(int i=0 ; i < conRBMaxIter; i++){  
512      consRB1->getCurAtomPos(posA.vec);
513      consRB2->getCurAtomPos(posB.vec);
514 <    pab = posA - posB;
521 <    
522 <    consRB1->getVel(velA.vec);
523 <    consRB2->getVel(velB.vec);
524 <    vab = velA -velB;
514 >    pab = posB - posA;
515  
516      //periodic boundary condition
527 –
517      info->wrapVector(pab.vec);
518 +    
519 +    consRB1->getCurAtomVel(velA.vec);
520 +    consRB2->getCurAtomVel(velB.vec);
521 +    vab = velB -velA;
522  
523 <    pabsq = pab.length2();
523 >    pvab = dotProduct(pab, vab);
524  
525 <    rabsq = curPair->getBondLength2();
533 <    diffsq = rabsq - pabsq;
525 >    if (fabs(pvab) > consTolerance ){
526  
535 –    if (fabs(diffsq) > (consTolerance * rabsq * 2)){
527  
537 –
528        bondDirUnitVec = pab;
529        bondDirUnitVec.normalize();
530            
531 <      getEffInvMassVec(consRB1, bondDirUnitVec, rma);
532 <
533 <      getEffInvMassVec(consRB2, bondDirUnitVec, rmb);
544 <
545 <      pabcDotvab = dotProduct(pab, vab);
546 <      pabDotInvMassVec = dotProduct(pab,  rma + rmb);
531 >      getZeta(consRB1, bondDirUnitVec, zetaA);
532 >      getZeta(consRB2, bondDirUnitVec, zetaB);
533 >      zeta = zetaA + zetaB;
534        
535 <      consForce = pabcDotvab /(2 * dt * pabDotInvMassVec) * bondDirUnitVec;
535 >      pabDotZeta = dotProduct(pab,  zeta);
536 >      
537 >      consForce = pvab / (dt * pabDotZeta) * bondDirUnitVec;
538        //integrate consRB1 using constraint force;
539 <      integrate(consRB1,consForce);
539 >      integrate(consRB1, consForce);
540  
541        //integrate consRB2 using constraint force;
542        integrate(consRB2, -consForce);    
# Line 561 | Line 550 | int DCRollBFunctor::operator()(ConstraintRigidBody* co
550      }
551    }
552  
553 +  cerr << "DCRollBFunctor::operator() Error: can not constrain the bond within maximum iteration at " << info->getTime() << endl;  
554    return consExceedMaxIter;
555  
556   }
557  
558 < void DCRollBFunctor::getEffInvMassVec(ConstraintRigidBody* consRB, const Vector3d& bondDir, Vector3d& invMassVec){
558 > void DCRollBFunctor::getZeta(ConstraintRigidBody* consRB, const Vector3d& bondDir, Vector3d& zeta){
559    double invMass;
560    Vector3d tempVec1;
561    Vector3d tempVec2;
562    Vector3d refCoor;
563    Vector3d refCrossBond;  
564    Mat3x3d IBody;
565 <  Mat3x3d IFrame;
565 >  Mat3x3d ILab;
566    Mat3x3d invIBody;
567 <  Mat3x3d invIFrame;
567 >  Mat3x3d invILab;
568    Mat3x3d a;
569    Mat3x3d aTrans;
570    
571    invMass = 1.0 / consRB ->getMass();
572  
573 <  invMassVec = invMass * bondDir;
573 >  zeta = invMass * bondDir;
574    
575    consRB->getRefCoor(refCoor.vec);
576    consRB->getA(a.element);
# Line 589 | Line 579 | void DCRollBFunctor::getEffInvMassVec(ConstraintRigidB
579    aTrans = a.transpose();
580    invIBody = IBody.inverse();
581  
582 <  IFrame = aTrans * invIBody * a;
582 >  invILab = aTrans * invIBody * a;
583  
584    refCrossBond = crossProduct(refCoor, bondDir);  
585  
586 <  tempVec1 = invIFrame * refCrossBond;
586 >  tempVec1 = invILab * refCrossBond;
587    tempVec2 = crossProduct(tempVec1, refCoor);
588  
589 <  invMassVec += tempVec2;
589 >  zeta += tempVec2;
590   }
591  
592   void DCRollBFunctor::integrate(ConstraintRigidBody* consRB, const Vector3d& force){
603 –  const double eConvert = 4.184e-4;
593    Vector3d vel;
605 –  Vector3d pos;
606 –  Vector3d Tb;
594    Vector3d ji;
595 +  Vector3d tempJi;
596 +  Vector3d tempTrq;
597 +  Vector3d refCoor;
598    double mass;
599    double dtOver2;
600    StuntDouble* sd;
# Line 612 | Line 602 | void DCRollBFunctor::integrate(ConstraintRigidBody* co
602    sd = consRB->getStuntDouble();  
603    dtOver2 = info->dt/2;
604  
605 +  sd->getVel(vel.vec);
606    mass = sd->getMass();
607 <
617 <  // velocity half step
618 <
619 <  vel += eConvert * dtOver2 /mass * force;
620 <
607 >  vel +=dtOver2 /mass * force;
608    sd->setVel(vel.vec);
609  
610    if (sd->isDirectional()){
611 <
612 <    // get and convert the torque to body frame
613 <
627 <    sd->getTrq(Tb.vec);
628 <    sd->lab2Body(Tb.vec);
629 <
630 <    // get the angular momentum, and propagate a half step
631 <
611 >    tempTrq = crossProduct(refCoor, force);
612 >    sd->lab2Body(tempTrq.vec);
613 >    tempJi = dtOver2* tempTrq;
614      sd->getJ(ji.vec);
615 <
634 <    ji += eConvert * dtOver2* Tb;
635 <
615 >    ji += tempJi;
616      sd->setJ(ji.vec);
617    }
618    

Diff Legend

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