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 1452 by tim, Mon Aug 23 15:11:36 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  
33 –
34 –  const int conRBMaxIter = 10;
36    
37    dt = info->dt;
37 –  dt2 = dt * dt;
38  
39 <  consRB1->getOldAtomPos(oldPosA.vec);
40 <  consRB2->getOldAtomPos(oldPosB.vec);      
39 >  consAtom1->getOldPos(oldPosA.vec);
40 >  consAtom2->getOldPos(oldPosB.vec);      
41  
42  
43 <  for(int i=0 ; i < conRBMaxIter; i++){  
44 <    consRB1->getCurAtomPos(posA.vec);
45 <    consRB2->getCurAtomPos(posB.vec);
43 >    consAtom1->getPos(posA.vec);
44 >    consAtom2->getPos(posB.vec);
45  
46 <    pab = posA - posB;
46 >    //discard the vector convention in alan tidesley's code
47 >    //rij =  rj - ri;
48 >    pab = posB - posA;
49  
50      //periodic boundary condition
51  
# Line 53 | Line 54 | int DCRollAFunctor::operator()(ConstraintRigidBody* co
54      pabsq = dotProduct(pab, pab);
55  
56      rabsq = curPair->getBondLength2();
57 <    diffsq = rabsq - pabsq;
57 >    diffsq =  pabsq -rabsq;
58  
59      if (fabs(diffsq) > (consTolerance * rabsq * 2)){
60 <      rab = oldPosA - oldPosB;      
60 >      rab = oldPosB - oldPosA;      
61        info->wrapVector(rab.vec);
62  
63 <      rpab = dotProduct(rab, pab);
63 >      //rpab = dotProduct(rab, pab);
64  
65 <      rpabsq = rpab * rpab;
65 >      //rpabsq = rpab * rpab;
66  
67  
68        //if (rpabsq < (rabsq * -diffsq)){
# Line 71 | Line 72 | int DCRollAFunctor::operator()(ConstraintRigidBody* co
72        bondDirUnitVec = pab;
73        bondDirUnitVec.normalize();
74            
75 <      getEffInvMassVec(consRB1, bondDirUnitVec, rma);
75 >      calcZeta(consAtom1, bondDirUnitVec, zetaA);
76  
77 <      getEffInvMassVec(consRB2, -bondDirUnitVec, rmb);
77 >      calcZeta(consAtom2, bondDirUnitVec, zetaB);
78  
79 <      rijcDotInvMassVec = dotProduct(pab,  rma + rmb);
79 >      zeta = zetaA + zetaB;      
80 >      zeta2 = dotProduct(zeta, zeta);
81        
82 <      consForce = diffsq /(2 * dt * dt * rijcDotInvMassVec) * bondDirUnitVec;
82 >      pabDotZeta = dotProduct(pab,  zeta);
83 >      pabDotZeta2 = pabDotZeta * pabDotZeta;
84 >
85 >      //solve quadratic equation
86 >      //dt^4 * zeta^2 * G^2 + 2* h^2 * pab * zeta * G + pab^2 - d^2
87 >      //dt : time step
88 >      // pab :
89 >      //G : constraint force scalar
90 >      //d: equilibrium bond length
91 >      
92 >      if (pabDotZeta2 - zeta2 * diffsq < 0)  
93 >        return consFail;
94 >      
95 >      //forceScalar = (pabDotZeta + sqrt(pabDotZeta2 - zeta2 * diffsq)) / dt * dt * zeta2;
96 >      forceScalar = diffsq / (2 * dt * dt * pabDotZeta);
97 >      //forceScalar = 1 / forceScalar;
98 >      consForce = forceScalar * bondDirUnitVec;
99        //integrate consRB1 using constraint force;
100 <      integrate(consRB1,consForce);
100 >      integrate(consAtom1, consForce);
101  
102        //integrate consRB2 using constraint force;
103 +      integrate(consAtom2, -consForce);    
104 +
105 +      return consSuccess;
106 +    }
107 +    else
108 +        return consAlready;
109 +
110 +      
111 +
112 +
113 + }
114 + void DCRollAFunctor::calcZeta(ConstraintAtom* consAtom, const Vector3d& bondDir, Vector3d&zeta){
115 +  double invMass;
116 +  invMass = 1.0 / consAtom ->getMass();
117 +
118 +  zeta = invMass * bondDir;
119 + }
120 +
121 + void DCRollAFunctor::integrate(ConstraintAtom* consAtom, const Vector3d& force){
122 +  StuntDouble* sd;
123 +  Vector3d vel;
124 +  Vector3d pos;
125 +  Vector3d tempPos;
126 +  Vector3d tempVel;
127 +  double mass;
128 +  double dt;
129 +  
130 +  dt = info->dt;
131 +  sd = consAtom->getStuntDouble();
132 +  
133 +  sd->getVel(vel.vec);
134 +  sd->getPos(pos.vec);
135 +  
136 +  mass = sd->getMass();
137 +
138 +  tempVel = dt/mass * force;
139 +  tempPos = dt * tempVel;
140 +        
141 +  vel += tempVel;
142 +  pos += tempPos;
143 +
144 +  sd->setVel(vel.vec);
145 +  sd->setPos(pos.vec);
146 + }
147 +
148 + int DCRollAFunctor::operator()(ConstraintRigidBody* consRB1, ConstraintRigidBody* consRB2){
149 +  Vector3d posA;
150 +  Vector3d posB;
151 +  Vector3d oldPosA;
152 +  Vector3d oldPosB;
153 +  Vector3d velA;
154 +  Vector3d velB;
155 +  Vector3d pab;
156 +  Vector3d tempPab;
157 +  Vector3d rab;
158 +  Vector3d zetaA;
159 +  Vector3d zetaB;
160 +  Vector3d zeta;
161 +  Vector3d consForce;
162 +  Vector3d bondDirUnitVec;  
163 +  double dx, dy, dz;
164 +  double rpab;
165 +  double rabsq, pabsq, rpabsq;
166 +  double diffsq;
167 +  double gab;
168 +  double dt;
169 +  double pabDotZeta;
170 +  double pabDotZeta2;
171 +  double zeta2;
172 +  double forceScalar;
173 +  
174 +  const int conRBMaxIter = 100;
175 +  
176 +  dt = info->dt;
177 +
178 +  consRB1->getOldAtomPos(oldPosA.vec);
179 +  consRB2->getOldAtomPos(oldPosB.vec);      
180 +
181 +
182 +  for(int i=0 ; i < conRBMaxIter; i++){  
183 +    consRB1->getCurAtomPos(posA.vec);
184 +    consRB2->getCurAtomPos(posB.vec);
185 +
186 +    //discard the vector convention in alan tidesley's code
187 +    //rij =  rj - ri;
188 +    pab = posB - posA;
189 +
190 +    //periodic boundary condition
191 +
192 +    info->wrapVector(pab.vec);
193 +
194 +    pabsq = dotProduct(pab, pab);
195 +
196 +    rabsq = curPair->getBondLength2();
197 +    diffsq =  pabsq -rabsq;
198 +
199 +    if (fabs(diffsq) > (consTolerance * rabsq * 2)){
200 +      rab = oldPosB - oldPosA;      
201 +      info->wrapVector(rab.vec);
202 +
203 +      bondDirUnitVec = rab;
204 +      bondDirUnitVec.normalize();
205 +          
206 +      calcZeta(consRB1, bondDirUnitVec, zetaA);
207 +
208 +      calcZeta(consRB2, bondDirUnitVec, zetaB);
209 +
210 +      zeta = zetaA + zetaB;      
211 +      zeta2 = dotProduct(zeta, zeta);
212 +      
213 +      pabDotZeta = dotProduct(pab,  zeta);
214 +      pabDotZeta2 = pabDotZeta * pabDotZeta;
215 +
216 +      //solve quadratic equation
217 +      //dt^4 * zeta^2 * G^2 + 2* h^2 * pab * zeta * G + pab^2 - d^2
218 +      //dt : time step
219 +      // pab :
220 +      //G : constraint force scalar
221 +      //d: equilibrium bond length
222 +      
223 +      if (pabDotZeta2 - zeta2 * diffsq < 0){
224 +        cerr << "DCRollAFunctor::operator() Error: Constraint Fail at " << info->getTime() << endl;  
225 +        return consFail;
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;
236 +      integrate(consRB1, consForce);
237 +
238 +      //integrate consRB2 using constraint force;
239        integrate(consRB2, -consForce);    
240        
241      }
# Line 93 | 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   }
254  
255 < void DCRollAFunctor::getEffInvMassVec(ConstraintRigidBody* consRB, const Vector3d& bondDir, Vector3d& invMassVec){
255 > void DCRollAFunctor::calcZeta(ConstraintRigidBody* consRB, const Vector3d& bondDir, Vector3d& zeta){
256    double invMass;
257    Vector3d tempVec1;
258    Vector3d tempVec2;
259    Vector3d refCoor;
260    Vector3d refCrossBond;  
261    Mat3x3d IBody;
107 –  Mat3x3d IFrame;
262    Mat3x3d invIBody;
263 <  Mat3x3d invIFrame;
263 >  Mat3x3d invILab;
264    Mat3x3d a;
265    Mat3x3d aTrans;
266    
267    invMass = 1.0 / consRB ->getMass();
268  
269 <  invMassVec = invMass * bondDir;
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 <  invMassVec += tempVec2;
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 refCoor;
299 +  Vector3d consTorque;
300 +  Vector3d totConsTorque;
301 +  Mat3x3d a;
302    double mass;
142 –  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);
151 <  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 <  vel += eConvert * dtOver2/mass * force;
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 <    rotationPropagation( sd, ji.vec);
355 >  ji += eConvert * dtOver2 * Tb;
356  
357 <    sd->setJ(ji.vec);
358 <  }
357 >  rotationPropagation( rb, ji.vec );
358 >
359 >  rb->setJ(ji.vec);
360    
361   }
362  
# Line 183 | 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 199 | 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 215 | 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];
219 <    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 315 | Line 497 | int DCRollBFunctor::operator()(ConstraintRigidBody* co
497   //Implementation of DCRollBFunctor
498   ////////////////////////////////////////////////////////////////////////////////
499   int DCRollBFunctor::operator()(ConstraintRigidBody* consRB1, ConstraintRigidBody* consRB2){
500 <  return consElemHandlerFail;
500 >  Vector3d posA;
501 >  Vector3d posB;
502 >  Vector3d velA;
503 >  Vector3d velB;
504 >  Vector3d pab;
505 >  Vector3d rab;
506 >  Vector3d vab;
507 >  Vector3d zetaA;
508 >  Vector3d zetaB;
509 >  Vector3d zeta;
510 >  Vector3d consForce;
511 >  Vector3d bondDirUnitVec;  
512 >  double dt;
513 >  double pabDotvab;
514 >  double pabDotZeta;
515 >  double pvab;
516 >
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 = posB - posA;
525 >
526 >    //periodic boundary condition
527 >    info->wrapVector(pab.vec);
528 >    
529 >    consRB1->getCurAtomVel(velA.vec);
530 >    consRB2->getCurAtomVel(velB.vec);
531 >    vab = velB -velA;
532 >
533 >    pvab = dotProduct(pab, vab);
534 >
535 >    if (fabs(pvab) > consTolerance ){
536 >
537 >
538 >      bondDirUnitVec = pab;
539 >      bondDirUnitVec.normalize();
540 >          
541 >      getZeta(consRB1, bondDirUnitVec, zetaA);
542 >      getZeta(consRB2, bondDirUnitVec, zetaB);
543 >      zeta = zetaA + zetaB;
544 >      
545 >      pabDotZeta = dotProduct(pab,  zeta);
546 >      
547 >      consForce =  2 * pvab / (dt * pabDotZeta) * bondDirUnitVec;
548 >      //integrate consRB1 using constraint force;
549 >      integrate(consRB1, consForce);
550 >
551 >      //integrate consRB2 using constraint force;
552 >      integrate(consRB2, -consForce);    
553 >      
554 >    }
555 >    else{
556 >      if (i ==0)
557 >        return consAlready;
558 >      else
559 >        return consSuccess;
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 ILab;
576 >  Mat3x3d invIBody;
577 >  Mat3x3d invILab;
578 >  Mat3x3d a;
579 >  
580 >  invMass = 1.0 / consRB ->getMass();
581  
582 +  zeta = invMass * bondDir;
583 +  
584 +  consRB->getRefCoor(refCoor.vec);
585 +  consRB->getA(a.element);
586 +  consRB->getI(IBody.element);
587 +
588 +  invIBody = IBody.inverse();
589 +
590 +  
591 +  refCrossBond = crossProduct(refCoor, a * bondDir);  
592 +
593 +  tempVec1 = invIBody * refCrossBond;
594 +
595 +  tempVec2 = (a * tempVec1.makeSkewMat()).transpose() * refCoor;
596 +  
597 +  zeta += tempVec2;
598 +
599   }
600  
601   void DCRollBFunctor::integrate(ConstraintRigidBody* consRB, const Vector3d& force){
602 +  Vector3d vel;
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 < }
615 >  sd->getVel(vel.vec);
616 >  mass = sd->getMass();
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 >    tempJi = dtOver2* tempTrq;
626 >    sd->getJ(ji.vec);
627 >    ji += tempJi;
628 >    sd->setJ(ji.vec);
629 >  }
630 >  
631 > }

Diff Legend

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