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 1254 by tim, Wed Jun 9 16:16:33 2004 UTC vs.
Revision 1268 by tim, Fri Jun 11 17:16:21 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 dt2;
32 <  double rijcDotInvMassVec;
31 >  double pabDotZeta;
32 >  double pabDotZeta2;
33 >  double zeta2;
34 >  double forceScalar;
35 >  
36 >  const int conRBMaxIter = 10;
37 >  
38 >  dt = info->dt;
39  
40 +  consAtom1->getOldPos(oldPosA.vec);
41 +  consAtom2->getOldPos(oldPosB.vec);      
42  
43 +
44 +  for(int i=0 ; i < conRBMaxIter; i++){  
45 +    consAtom1->getPos(posA.vec);
46 +    consAtom2->getPos(posB.vec);
47 +
48 +    //discard the vector convention in alan tidesley's code
49 +    //rij =  rj - ri;
50 +    pab = posB - posA;
51 +
52 +    //periodic boundary condition
53 +
54 +    info->wrapVector(pab.vec);
55 +
56 +    pabsq = dotProduct(pab, pab);
57 +
58 +    rabsq = curPair->getBondLength2();
59 +    diffsq =  pabsq -rabsq;
60 +
61 +    if (fabs(diffsq) > (consTolerance * rabsq * 2)){
62 +      rab = oldPosB - oldPosA;      
63 +      info->wrapVector(rab.vec);
64 +
65 +      //rpab = dotProduct(rab, pab);
66 +
67 +      //rpabsq = rpab * rpab;
68 +
69 +
70 +      //if (rpabsq < (rabsq * -diffsq)){
71 +      //  return consFail;
72 +      //}
73 +
74 +      bondDirUnitVec = pab;
75 +      bondDirUnitVec.normalize();
76 +          
77 +      calcZeta(consAtom1, bondDirUnitVec, zetaA);
78 +
79 +      calcZeta(consAtom2, bondDirUnitVec, zetaB);
80 +
81 +      zeta = zetaA + zetaB;      
82 +      zeta2 = dotProduct(zeta, zeta);
83 +      
84 +      pabDotZeta = dotProduct(pab,  zeta);
85 +      pabDotZeta2 = pabDotZeta * pabDotZeta;
86 +
87 +      //solve quadratic equation
88 +      //dt^4 * zeta^2 * G^2 + 2* h^2 * pab * zeta * G + pab^2 - d^2
89 +      //dt : time step
90 +      // pab :
91 +      //G : constraint force scalar
92 +      //d: equilibrium bond length
93 +      
94 +      if (pabDotZeta2 - zeta2 * diffsq < 0)
95 +        return consFail;
96 +
97 +      //forceScalar = (pabDotZeta + sqrt(pabDotZeta2 - zeta2 * diffsq)) / dt * dt * zeta2;
98 +      forceScalar = diffsq / (2 * dt * dt * pabDotZeta);
99 +      //
100 +      consForce = forceScalar * bondDirUnitVec;
101 +      //integrate consRB1 using constraint force;
102 +      integrate(consAtom1, consForce);
103 +
104 +      //integrate consRB2 using constraint force;
105 +      integrate(consAtom2, -consForce);    
106 +      
107 +    }
108 +    else{
109 +      if (i ==0)
110 +        return consAlready;
111 +      else
112 +        return consSuccess;
113 +    }
114 +  }
115 +
116 +  return consExceedMaxIter;
117 +
118 + }
119 + void DCRollAFunctor::calcZeta(ConstraintAtom* consAtom, const Vector3d& bondDir, Vector3d&zeta){
120 +  double invMass;
121 +  invMass = 1.0 / consAtom ->getMass();
122 +
123 +  zeta = invMass * bondDir;
124 + }
125 +
126 + void DCRollAFunctor::integrate(ConstraintAtom* consAtom, const Vector3d& force){
127 +  StuntDouble* sd;
128 +  Vector3d vel;
129 +  Vector3d pos;
130 +  Vector3d tempPos;
131 +  Vector3d tempVel;
132 +
133 +  double mass;
134 +  double dtOver2;
135 +  double dt;
136 +  const double eConvert = 4.184e-4;
137 +  
138 +  dt = info->dt;
139 +  dtOver2 = dt /2;  
140 +  sd = consAtom->getStuntDouble();
141 +  
142 +  sd->getVel(vel.vec);
143 +  sd->getPos(pos.vec);
144 +  
145 +  mass = sd->getMass();
146 +
147 +  tempVel = eConvert * dtOver2/mass * force;
148 +  tempPos = dt * tempVel;
149 +        
150 +  vel += tempVel;
151 +  pos += tempPos;
152 +
153 +  sd->setVel(vel.vec);
154 +  sd->setPos(pos.vec);
155 + }
156 +
157 + int DCRollAFunctor::operator()(ConstraintRigidBody* consRB1, ConstraintRigidBody* consRB2){
158 +  Vector3d posA;
159 +  Vector3d posB;
160 +  Vector3d oldPosA;
161 +  Vector3d oldPosB;
162 +  Vector3d velA;
163 +  Vector3d velB;
164 +  Vector3d pab;
165 +  Vector3d tempPab;
166 +  Vector3d rab;
167 +  Vector3d zetaA;
168 +  Vector3d zetaB;
169 +  Vector3d zeta;
170 +  Vector3d consForce;
171 +  Vector3d bondDirUnitVec;  
172 +  double dx, dy, dz;
173 +  double rpab;
174 +  double rabsq, pabsq, rpabsq;
175 +  double diffsq;
176 +  double gab;
177 +  double dt;
178 +  double pabDotZeta;
179 +  double pabDotZeta2;
180 +  double zeta2;
181 +  double forceScalar;
182 +  
183    const int conRBMaxIter = 10;
184    
185    dt = info->dt;
37 –  dt2 = dt * dt;
186  
187    consRB1->getOldAtomPos(oldPosA.vec);
188    consRB2->getOldAtomPos(oldPosB.vec);      
# Line 44 | Line 192 | int DCRollAFunctor::operator()(ConstraintRigidBody* co
192      consRB1->getCurAtomPos(posA.vec);
193      consRB2->getCurAtomPos(posB.vec);
194  
195 <    pab = posA - posB;
195 >    //discard the vector convention in alan tidesley's code
196 >    //rij =  rj - ri;
197 >    pab = posB - posA;
198  
199      //periodic boundary condition
200  
# Line 53 | Line 203 | int DCRollAFunctor::operator()(ConstraintRigidBody* co
203      pabsq = dotProduct(pab, pab);
204  
205      rabsq = curPair->getBondLength2();
206 <    diffsq = rabsq - pabsq;
206 >    diffsq =  pabsq -rabsq;
207  
208      if (fabs(diffsq) > (consTolerance * rabsq * 2)){
209 <      rab = oldPosA - oldPosB;      
209 >      rab = oldPosB - oldPosA;      
210        info->wrapVector(rab.vec);
211  
212 <      rpab = dotProduct(rab, pab);
212 >      //rpab = dotProduct(rab, pab);
213  
214 <      rpabsq = rpab * rpab;
214 >      //rpabsq = rpab * rpab;
215  
216  
217        //if (rpabsq < (rabsq * -diffsq)){
# Line 71 | Line 221 | int DCRollAFunctor::operator()(ConstraintRigidBody* co
221        bondDirUnitVec = pab;
222        bondDirUnitVec.normalize();
223            
224 <      getEffInvMassVec(consRB1, bondDirUnitVec, rma);
224 >      calcZeta(consRB1, bondDirUnitVec, zetaA);
225  
226 <      getEffInvMassVec(consRB2, -bondDirUnitVec, rmb);
226 >      calcZeta(consRB2, bondDirUnitVec, zetaB);
227  
228 <      rijcDotInvMassVec = dotProduct(pab,  rma + rmb);
229 <      
230 <      consForce = diffsq /(2 * dt * dt * rijcDotInvMassVec) * bondDirUnitVec;
228 >      zeta = zetaA + zetaB;      
229 >      zeta2 = dotProduct(zeta, zeta);
230 >      
231 >      pabDotZeta = dotProduct(pab,  zeta);
232 >      pabDotZeta2 = pabDotZeta * pabDotZeta;
233 >
234 >      //solve quadratic equation
235 >      //dt^4 * zeta^2 * G^2 + 2* h^2 * pab * zeta * G + pab^2 - d^2
236 >      //dt : time step
237 >      // pab :
238 >      //G : constraint force scalar
239 >      //d: equilibrium bond length
240 >      
241 >      if (pabDotZeta2 - zeta2 * diffsq < 0)
242 >        return consFail;
243 >
244 >      //forceScalar = (pabDotZeta + sqrt(pabDotZeta2 - zeta2 * diffsq)) / dt * dt * zeta2;
245 >      forceScalar = diffsq / (2 * dt * dt * pabDotZeta);
246 >      //
247 >      consForce = forceScalar * bondDirUnitVec;
248        //integrate consRB1 using constraint force;
249 <      integrate(consRB1,consForce);
249 >      integrate(consRB1, consForce);
250  
251        //integrate consRB2 using constraint force;
252        integrate(consRB2, -consForce);    
# Line 97 | Line 264 | int DCRollAFunctor::operator()(ConstraintRigidBody* co
264  
265   }
266  
267 < void DCRollAFunctor::getEffInvMassVec(ConstraintRigidBody* consRB, const Vector3d& bondDir, Vector3d& invMassVec){
267 > void DCRollAFunctor::calcZeta(ConstraintRigidBody* consRB, const Vector3d& bondDir, Vector3d& zeta){
268    double invMass;
269    Vector3d tempVec1;
270    Vector3d tempVec2;
# Line 112 | Line 279 | void DCRollAFunctor::getEffInvMassVec(ConstraintRigidB
279    
280    invMass = 1.0 / consRB ->getMass();
281  
282 <  invMassVec = invMass * bondDir;
282 >  zeta = invMass * bondDir;
283    
284    consRB->getRefCoor(refCoor.vec);
285    consRB->getA(a.element);
# Line 128 | Line 295 | void DCRollAFunctor::getEffInvMassVec(ConstraintRigidB
295    tempVec1 = invIFrame * refCrossBond;
296    tempVec2 = crossProduct(tempVec1, refCoor);
297  
298 <  invMassVec += tempVec2;
298 >  zeta += tempVec2;
299    
300   }
301  
# Line 138 | Line 305 | void DCRollAFunctor::integrate(ConstraintRigidBody* co
305    Vector3d pos;
306    Vector3d Tb;
307    Vector3d ji;
308 +        Vector3d tempPos;
309 +        Vector3d tempVel;
310 +        Vector3d tempTrq;
311 +        Vector3d tempJi;
312    double mass;
313    double dtOver2;
314    double dt;
# Line 152 | Line 323 | void DCRollAFunctor::integrate(ConstraintRigidBody* co
323    
324    mass = sd->getMass();
325  
326 <  vel += eConvert * dtOver2/mass * force;
327 <  pos += dt * vel;
326 >  tempVel = eConvert * dtOver2/mass * force;
327 >  tempPos = dt * tempVel;
328 >        
329 >        vel += tempVel;
330 >        pos += tempPos;
331  
332    sd->setVel(vel.vec);
333    sd->setPos(pos.vec);
# Line 315 | Line 489 | int DCRollBFunctor::operator()(ConstraintRigidBody* co
489   //Implementation of DCRollBFunctor
490   ////////////////////////////////////////////////////////////////////////////////
491   int DCRollBFunctor::operator()(ConstraintRigidBody* consRB1, ConstraintRigidBody* consRB2){
492 <  return consElemHandlerFail;
492 >  Vector3d posA;
493 >  Vector3d posB;
494 >  Vector3d velA;
495 >  Vector3d velB;
496 >  Vector3d pab;
497 >  Vector3d rab;
498 >  Vector3d vab;
499 >  Vector3d rma;
500 >  Vector3d rmb;
501 >  Vector3d consForce;
502 >  Vector3d bondDirUnitVec;  
503 >  double dx, dy, dz;
504 >  double rpab;
505 >  double rabsq, pabsq, rpabsq;
506 >  double diffsq;
507 >  double gab;
508 >  double dt;
509 >  double pabcDotvab;
510 >  double pabDotInvMassVec;
511 >
512 >
513 >  const int conRBMaxIter = 10;
514 >  
515 >  dt = info->dt;
516 >  
517 >  for(int i=0 ; i < conRBMaxIter; i++){  
518 >    consRB1->getCurAtomPos(posA.vec);
519 >    consRB2->getCurAtomPos(posB.vec);
520 >    pab = posA - posB;
521 >    
522 >    consRB1->getVel(velA.vec);
523 >    consRB2->getVel(velB.vec);
524 >    vab = velA -velB;
525 >
526 >    //periodic boundary condition
527 >
528 >    info->wrapVector(pab.vec);
529 >
530 >    pabsq = pab.length2();
531 >
532 >    rabsq = curPair->getBondLength2();
533 >    diffsq = rabsq - pabsq;
534 >
535 >    if (fabs(diffsq) > (consTolerance * rabsq * 2)){
536 >
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);
547 >      
548 >      consForce = pabcDotvab /(2 * dt * pabDotInvMassVec) * bondDirUnitVec;
549 >      //integrate consRB1 using constraint force;
550 >      integrate(consRB1,consForce);
551 >
552 >      //integrate consRB2 using constraint force;
553 >      integrate(consRB2, -consForce);    
554 >      
555 >    }
556 >    else{
557 >      if (i ==0)
558 >        return consAlready;
559 >      else
560 >        return consSuccess;
561 >    }
562 >  }
563 >
564 >  return consExceedMaxIter;
565 >
566   }
567  
568   void DCRollBFunctor::getEffInvMassVec(ConstraintRigidBody* consRB, const Vector3d& bondDir, Vector3d& invMassVec){
569 +  double invMass;
570 +  Vector3d tempVec1;
571 +  Vector3d tempVec2;
572 +  Vector3d refCoor;
573 +  Vector3d refCrossBond;  
574 +  Mat3x3d IBody;
575 +  Mat3x3d IFrame;
576 +  Mat3x3d invIBody;
577 +  Mat3x3d invIFrame;
578 +  Mat3x3d a;
579 +  Mat3x3d aTrans;
580 +  
581 +  invMass = 1.0 / consRB ->getMass();
582  
583 +  invMassVec = invMass * bondDir;
584 +  
585 +  consRB->getRefCoor(refCoor.vec);
586 +  consRB->getA(a.element);
587 +  consRB->getI(IBody.element);
588 +
589 +  aTrans = a.transpose();
590 +  invIBody = IBody.inverse();
591 +
592 +  IFrame = aTrans * invIBody * a;
593 +
594 +  refCrossBond = crossProduct(refCoor, bondDir);  
595 +
596 +  tempVec1 = invIFrame * refCrossBond;
597 +  tempVec2 = crossProduct(tempVec1, refCoor);
598 +
599 +  invMassVec += tempVec2;
600   }
601  
602   void DCRollBFunctor::integrate(ConstraintRigidBody* consRB, const Vector3d& force){
603 +  const double eConvert = 4.184e-4;
604 +  Vector3d vel;
605 +  Vector3d pos;
606 +  Vector3d Tb;
607 +  Vector3d ji;
608 +  double mass;
609 +  double dtOver2;
610 +  StuntDouble* sd;
611 +  
612 +  sd = consRB->getStuntDouble();  
613 +  dtOver2 = info->dt/2;
614  
615 < }
615 >  mass = sd->getMass();
616 >
617 >  // velocity half step
618 >
619 >  vel += eConvert * dtOver2 /mass * force;
620 >
621 >  sd->setVel(vel.vec);
622 >
623 >  if (sd->isDirectional()){
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 >
632 >    sd->getJ(ji.vec);
633 >
634 >    ji += eConvert * dtOver2* Tb;
635 >
636 >    sd->setJ(ji.vec);
637 >  }
638 >  
639 > }

Diff Legend

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