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 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 pabDotInvMassVec;
32 <
33 <
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 <  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 51 | 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 69 | 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 <      pabDotInvMassVec = dotProduct(pab,  rma + rmb);
81 >      zeta = zetaA + zetaB;      
82 >      zeta2 = dotProduct(zeta, zeta);
83        
84 <      consForce = diffsq /(2 * dt * dt * pabDotInvMassVec) * 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 91 | 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;
105 –  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 119 | 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 136 | 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;
143 <  
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 150 | 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);
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  
166 –    // get the angular momentum, and propagate a half step
167 –
343      sd->getJ(ji.vec);
344  
345 <    ji += eConvert * dtOver2 * Tb;
345 >    ji += tempJi;
346  
172 –    rotationPropagation( sd, ji.vec);
173 –
347      sd->setJ(ji.vec);
348    }
349 +
350    
351   }
352  
# Line 320 | 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;  
327 –  double dx, dy, dz;
328 –  double rpab;
329 –  double rabsq, pabsq, rpabsq;
330 –  double diffsq;
331 –  double gab;
502    double dt;
503 <  double pabcDotvab;
504 <  double pabDotInvMassVec;
503 >  double pabDotvab;
504 >  double pabDotZeta;
505 >  double pvab;
506  
507 <
337 <  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;
345 <    
346 <    consRB1->getVel(velA.vec);
347 <    consRB2->getVel(velB.vec);
348 <    vab = velA -velB;
514 >    pab = posB - posA;
515  
516      //periodic boundary condition
351 –
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();
357 <    diffsq = rabsq - pabsq;
525 >    if (fabs(pvab) > consTolerance ){
526  
359 –    if (fabs(diffsq) > (consTolerance * rabsq * 2)){
527  
361 –
528        bondDirUnitVec = pab;
529        bondDirUnitVec.normalize();
530            
531 <      getEffInvMassVec(consRB1, bondDirUnitVec, rma);
532 <
533 <      getEffInvMassVec(consRB2, -bondDirUnitVec, rmb);
368 <
369 <      pabcDotvab = dotProduct(pab, vab);
370 <      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 385 | 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 413 | 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){
427 –  const double eConvert = 4.184e-4;
593    Vector3d vel;
429 –  Vector3d pos;
430 –  Vector3d Tb;
594    Vector3d ji;
595 +  Vector3d tempJi;
596 +  Vector3d tempTrq;
597 +  Vector3d refCoor;
598    double mass;
599    double dtOver2;
600    StuntDouble* sd;
# Line 436 | 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 <
441 <  // velocity half step
442 <
443 <  vel += eConvert * dtOver2 /mass * force;
444 <
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 <
451 <    sd->getTrq(Tb.vec);
452 <    sd->lab2Body(Tb.vec);
453 <
454 <    // get the angular momentum, and propagate a half step
455 <
611 >    tempTrq = crossProduct(refCoor, force);
612 >    sd->lab2Body(tempTrq.vec);
613 >    tempJi = dtOver2* tempTrq;
614      sd->getJ(ji.vec);
615 <
458 <    ji += eConvert * dtOver2* Tb;
459 <
615 >    ji += tempJi;
616      sd->setJ(ji.vec);
617    }
618    

Diff Legend

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