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 1284 by tim, Mon Jun 21 18:52: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;
33 <
34 <
31 >  double pabDotZeta;
32 >  double pabDotZeta2;
33 >  double zeta2;
34 >  double forceScalar;
35 >  
36    const int conRBMaxIter = 10;
37    
38    dt = info->dt;
37 –  dt2 = dt * dt;
39  
40 <  consRB1->getOldAtomPos(oldPosA.vec);
41 <  consRB2->getOldAtomPos(oldPosB.vec);      
40 >  consAtom1->getOldPos(oldPosA.vec);
41 >  consAtom2->getOldPos(oldPosB.vec);      
42  
43  
44    for(int i=0 ; i < conRBMaxIter; i++){  
45 <    consRB1->getCurAtomPos(posA.vec);
46 <    consRB2->getCurAtomPos(posB.vec);
45 >    consAtom1->getPos(posA.vec);
46 >    consAtom2->getPos(posB.vec);
47  
48 <    pab = posA - posB;
48 >    //discard the vector convention in alan tidesley's code
49 >    //rij =  rj - ri;
50 >    pab = posB - posA;
51  
52      //periodic boundary condition
53  
# Line 53 | Line 56 | int DCRollAFunctor::operator()(ConstraintRigidBody* co
56      pabsq = dotProduct(pab, pab);
57  
58      rabsq = curPair->getBondLength2();
59 <    diffsq = rabsq - pabsq;
59 >    diffsq =  pabsq -rabsq;
60  
61      if (fabs(diffsq) > (consTolerance * rabsq * 2)){
62 <      rab = oldPosA - oldPosB;      
62 >      rab = oldPosB - oldPosA;      
63        info->wrapVector(rab.vec);
64  
65 <      rpab = dotProduct(rab, pab);
65 >      //rpab = dotProduct(rab, pab);
66  
67 <      rpabsq = rpab * rpab;
67 >      //rpabsq = rpab * rpab;
68  
69  
70        //if (rpabsq < (rabsq * -diffsq)){
# Line 71 | Line 74 | int DCRollAFunctor::operator()(ConstraintRigidBody* co
74        bondDirUnitVec = pab;
75        bondDirUnitVec.normalize();
76            
77 <      getEffInvMassVec(consRB1, bondDirUnitVec, rma);
77 >      calcZeta(consAtom1, bondDirUnitVec, zetaA);
78  
79 <      getEffInvMassVec(consRB2, -bondDirUnitVec, rmb);
79 >      calcZeta(consAtom2, bondDirUnitVec, zetaB);
80  
81 <      rijcDotInvMassVec = dotProduct(pab,  rma + rmb);
81 >      zeta = zetaA + zetaB;      
82 >      zeta2 = dotProduct(zeta, zeta);
83        
84 <      consForce = diffsq /(2 * dt * dt * rijcDotInvMassVec) * bondDirUnitVec;
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 >      //forceScalar = 1 / forceScalar;
100 >      consForce = forceScalar * bondDirUnitVec;
101        //integrate consRB1 using constraint force;
102 <      integrate(consRB1,consForce);
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 >  double mass;
133 >  double dt;
134 >  
135 >  dt = info->dt;
136 >  sd = consAtom->getStuntDouble();
137 >  
138 >  sd->getVel(vel.vec);
139 >  sd->getPos(pos.vec);
140 >  
141 >  mass = sd->getMass();
142 >
143 >  tempVel = dt/mass * force;
144 >  tempPos = dt * tempVel;
145 >        
146 >  vel += tempVel;
147 >  pos += tempPos;
148 >
149 >  sd->setVel(vel.vec);
150 >  sd->setPos(pos.vec);
151 > }
152 >
153 > int DCRollAFunctor::operator()(ConstraintRigidBody* consRB1, ConstraintRigidBody* consRB2){
154 >  Vector3d posA;
155 >  Vector3d posB;
156 >  Vector3d oldPosA;
157 >  Vector3d oldPosB;
158 >  Vector3d velA;
159 >  Vector3d velB;
160 >  Vector3d pab;
161 >  Vector3d tempPab;
162 >  Vector3d rab;
163 >  Vector3d zetaA;
164 >  Vector3d zetaB;
165 >  Vector3d zeta;
166 >  Vector3d consForce;
167 >  Vector3d bondDirUnitVec;  
168 >  double dx, dy, dz;
169 >  double rpab;
170 >  double rabsq, pabsq, rpabsq;
171 >  double diffsq;
172 >  double gab;
173 >  double dt;
174 >  double pabDotZeta;
175 >  double pabDotZeta2;
176 >  double zeta2;
177 >  double forceScalar;
178 >  
179 >  const int conRBMaxIter = 100;
180 >  
181 >  dt = info->dt;
182 >
183 >  consRB1->getOldAtomPos(oldPosA.vec);
184 >  consRB2->getOldAtomPos(oldPosB.vec);      
185 >
186 >
187 >  for(int i=0 ; i < conRBMaxIter; i++){  
188 >    consRB1->getCurAtomPos(posA.vec);
189 >    consRB2->getCurAtomPos(posB.vec);
190 >
191 >    //discard the vector convention in alan tidesley's code
192 >    //rij =  rj - ri;
193 >    pab = posB - posA;
194 >
195 >    //periodic boundary condition
196 >
197 >    info->wrapVector(pab.vec);
198 >
199 >    pabsq = dotProduct(pab, pab);
200 >
201 >    rabsq = curPair->getBondLength2();
202 >    diffsq =  pabsq -rabsq;
203 >
204 >    if (fabs(diffsq) > (consTolerance * rabsq * 2)){
205 >      rab = oldPosB - oldPosA;      
206 >      info->wrapVector(rab.vec);
207 >
208 >      bondDirUnitVec = rab;
209 >      bondDirUnitVec.normalize();
210 >          
211 >      calcZeta(consRB1, bondDirUnitVec, zetaA);
212 >
213 >      calcZeta(consRB2, bondDirUnitVec, zetaB);
214 >
215 >      zeta = zetaA + zetaB;      
216 >      zeta2 = dotProduct(zeta, zeta);
217 >      
218 >      pabDotZeta = dotProduct(pab,  zeta);
219 >      pabDotZeta2 = pabDotZeta * pabDotZeta;
220 >
221 >      //solve quadratic equation
222 >      //dt^4 * zeta^2 * G^2 + 2* h^2 * pab * zeta * G + pab^2 - d^2
223 >      //dt : time step
224 >      // pab :
225 >      //G : constraint force scalar
226 >      //d: equilibrium bond length
227 >      
228 >      if (pabDotZeta2 - zeta2 * diffsq < 0){
229 >        cerr << "DCRollAFunctor::operator() Error: Constraint Fail at " << info->getTime() << endl;  
230 >        return consFail;
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;
241 >      integrate(consRB1, consForce);
242  
243        //integrate consRB2 using constraint force;
244        integrate(consRB2, -consForce);    
# Line 93 | 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   }
259  
260 < void DCRollAFunctor::getEffInvMassVec(ConstraintRigidBody* consRB, const Vector3d& bondDir, Vector3d& invMassVec){
260 > void DCRollAFunctor::calcZeta(ConstraintRigidBody* consRB, const Vector3d& bondDir, Vector3d& zeta){
261    double invMass;
262    Vector3d tempVec1;
263    Vector3d tempVec2;
264    Vector3d refCoor;
265    Vector3d refCrossBond;  
266    Mat3x3d IBody;
107 –  Mat3x3d IFrame;
267    Mat3x3d invIBody;
268 <  Mat3x3d invIFrame;
268 >  Mat3x3d invILab;
269    Mat3x3d a;
270    Mat3x3d aTrans;
271    
272    invMass = 1.0 / consRB ->getMass();
273  
274 <  invMassVec = invMass * bondDir;
274 >  zeta = invMass * bondDir;
275    
276    consRB->getRefCoor(refCoor.vec);
277    consRB->getA(a.element);
# Line 121 | Line 280 | void DCRollAFunctor::getEffInvMassVec(ConstraintRigidB
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 <  invMassVec += tempVec2;
290 >  zeta += tempVec2;
291    
292   }
293  
# Line 138 | Line 297 | void DCRollAFunctor::integrate(ConstraintRigidBody* co
297    Vector3d pos;
298    Vector3d Tb;
299    Vector3d ji;
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;
145 <  
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 152 | Line 318 | void DCRollAFunctor::integrate(ConstraintRigidBody* co
318    
319    mass = sd->getMass();
320  
321 <  vel += eConvert * dtOver2/mass * force;
322 <  pos += dt * vel;
321 >  tempVel = dtOver2/mass * force;
322 >  tempPos = dt * tempVel;
323 >        
324 >        vel += tempVel;
325 >        pos += tempPos;
326  
327    sd->setVel(vel.vec);
328    sd->setPos(pos.vec);
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);
337 <
338 <    // get the angular momentum, and propagate a half step
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  
343      sd->getJ(ji.vec);
344  
345 <    ji += eConvert * dtOver2 * Tb;
345 >    ji += tempJi;
346  
174 –    rotationPropagation( sd, ji.vec);
175 –
347      sd->setJ(ji.vec);
348    }
349 +
350    
351   }
352  
# Line 315 | Line 487 | int DCRollBFunctor::operator()(ConstraintRigidBody* co
487   //Implementation of DCRollBFunctor
488   ////////////////////////////////////////////////////////////////////////////////
489   int DCRollBFunctor::operator()(ConstraintRigidBody* consRB1, ConstraintRigidBody* consRB2){
490 <  return consElemHandlerFail;
491 < }
490 >  Vector3d posA;
491 >  Vector3d posB;
492 >  Vector3d velA;
493 >  Vector3d velB;
494 >  Vector3d pab;
495 >  Vector3d rab;
496 >  Vector3d vab;
497 >  Vector3d zetaA;
498 >  Vector3d zetaB;
499 >  Vector3d zeta;
500 >  Vector3d consForce;
501 >  Vector3d bondDirUnitVec;  
502 >  double dt;
503 >  double pabDotvab;
504 >  double pabDotZeta;
505 >  double pvab;
506  
507 < void DCRollBFunctor::getEffInvMassVec(ConstraintRigidBody* consRB, const Vector3d& bondDir, Vector3d& invMassVec){
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 = posB - posA;
515  
516 +    //periodic boundary condition
517 +    info->wrapVector(pab.vec);
518 +    
519 +    consRB1->getCurAtomVel(velA.vec);
520 +    consRB2->getCurAtomVel(velB.vec);
521 +    vab = velB -velA;
522 +
523 +    pvab = dotProduct(pab, vab);
524 +
525 +    if (fabs(pvab) > consTolerance ){
526 +
527 +
528 +      bondDirUnitVec = pab;
529 +      bondDirUnitVec.normalize();
530 +          
531 +      getZeta(consRB1, bondDirUnitVec, zetaA);
532 +      getZeta(consRB2, bondDirUnitVec, zetaB);
533 +      zeta = zetaA + zetaB;
534 +      
535 +      pabDotZeta = dotProduct(pab,  zeta);
536 +      
537 +      consForce = pvab / (dt * pabDotZeta) * bondDirUnitVec;
538 +      //integrate consRB1 using constraint force;
539 +      integrate(consRB1, consForce);
540 +
541 +      //integrate consRB2 using constraint force;
542 +      integrate(consRB2, -consForce);    
543 +      
544 +    }
545 +    else{
546 +      if (i ==0)
547 +        return consAlready;
548 +      else
549 +        return consSuccess;
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::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 ILab;
566 +  Mat3x3d invIBody;
567 +  Mat3x3d invILab;
568 +  Mat3x3d a;
569 +  Mat3x3d aTrans;
570 +  
571 +  invMass = 1.0 / consRB ->getMass();
572 +
573 +  zeta = invMass * bondDir;
574 +  
575 +  consRB->getRefCoor(refCoor.vec);
576 +  consRB->getA(a.element);
577 +  consRB->getI(IBody.element);
578 +
579 +  aTrans = a.transpose();
580 +  invIBody = IBody.inverse();
581 +
582 +  invILab = aTrans * invIBody * a;
583 +
584 +  refCrossBond = crossProduct(refCoor, bondDir);  
585 +
586 +  tempVec1 = invILab * refCrossBond;
587 +  tempVec2 = crossProduct(tempVec1, refCoor);
588 +
589 +  zeta += tempVec2;
590 + }
591 +
592   void DCRollBFunctor::integrate(ConstraintRigidBody* consRB, const Vector3d& force){
593 +  Vector3d vel;
594 +  Vector3d ji;
595 +  Vector3d tempJi;
596 +  Vector3d tempTrq;
597 +  Vector3d refCoor;
598 +  double mass;
599 +  double dtOver2;
600 +  StuntDouble* sd;
601 +  
602 +  sd = consRB->getStuntDouble();  
603 +  dtOver2 = info->dt/2;
604  
605 < }
605 >  sd->getVel(vel.vec);
606 >  mass = sd->getMass();
607 >  vel +=dtOver2 /mass * force;
608 >  sd->setVel(vel.vec);
609 >
610 >  if (sd->isDirectional()){
611 >    tempTrq = crossProduct(refCoor, force);
612 >    sd->lab2Body(tempTrq.vec);
613 >    tempJi = dtOver2* tempTrq;
614 >    sd->getJ(ji.vec);
615 >    ji += tempJi;
616 >    sd->setJ(ji.vec);
617 >  }
618 >  
619 > }

Diff Legend

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