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 1451 by tim, Mon Jun 21 18:52:21 2004 UTC vs.
Revision 1452 by tim, Mon Aug 23 15:11:36 2004 UTC

# Line 32 | Line 32 | int DCRollAFunctor::operator()(ConstraintAtom* consAto
32    double pabDotZeta2;
33    double zeta2;
34    double forceScalar;
35 +
36    
36 –  const int conRBMaxIter = 10;
37 –  
37    dt = info->dt;
38  
39    consAtom1->getOldPos(oldPosA.vec);
40    consAtom2->getOldPos(oldPosB.vec);      
41  
42  
44 –  for(int i=0 ; i < conRBMaxIter; i++){  
43      consAtom1->getPos(posA.vec);
44      consAtom2->getPos(posB.vec);
45  
# Line 103 | Line 101 | int DCRollAFunctor::operator()(ConstraintAtom* consAto
101  
102        //integrate consRB2 using constraint force;
103        integrate(consAtom2, -consForce);    
104 <      
104 >
105 >      return consSuccess;
106      }
107 <    else{
109 <      if (i ==0)
107 >    else
108          return consAlready;
109 <      else
110 <        return consSuccess;
113 <    }
114 <  }
109 >
110 >      
111  
116 –  return consExceedMaxIter;
112  
113   }
114   void DCRollAFunctor::calcZeta(ConstraintAtom* consAtom, const Vector3d& bondDir, Vector3d&zeta){
# Line 274 | Line 269 | void DCRollAFunctor::calcZeta(ConstraintRigidBody* con
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();
# Line 282 | Line 278 | void DCRollAFunctor::calcZeta(ConstraintRigidBody* con
278  
279    invILab = aTrans * invIBody * a;
280  
281 <  refCrossBond = crossProduct(refCoor, bondDir);  
281 >  refCrossBond = crossProduct(aTrans *refCoor, bondDir);  
282  
283    tempVec1 = invILab * refCrossBond;
284 <  tempVec2 = crossProduct(tempVec1, refCoor);
284 >  tempVec2 = crossProduct(tempVec1, aTrans *refCoor);
285  
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;
300 –  Vector3d tempPos;
301 –  Vector3d tempVel;
302 –  Vector3d tempTrqLab;
303 –  Vector3d tempTrqBody;  
304 –  Vector3d tempJi;
298    Vector3d refCoor;
299 +  Vector3d consTorque;
300 +  Vector3d totConsTorque;
301 +  Mat3x3d a;
302    double mass;
307 –  Mat3x3d oldA;
303    double dt;
304    double dtOver2;
305 +  const double eConvert = 4.184e-4;
306 +  
307    dt = info->dt;
308    dtOver2 = dt /2;
309  
310 <  consRB->getOldA(oldA.element);
311 <  sd = consRB->getStuntDouble();
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);
317 <  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 <  tempVel = dtOver2/mass * force;
327 <  tempPos = dt * tempVel;
328 <        
329 <        vel += tempVel;
325 <        pos += tempPos;
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()){
335 <
336 <    consRB->getRefCoor(refCoor.vec);
333 <    tempTrqLab = crossProduct(refCoor, force);
334 >  //evolve orientational part
335 >  consRB->getRefCoor(refCoor.vec);
336 >  rb->getA(a.element);
337  
338 <    //convert torque in lab frame to torque in body frame using old rotation matrix
339 <    //tempTrqBody = oldA * tempTrqLab;
340 <    
338 <    //tempJi = dtOver2 * tempTrqBody;
339 <    sd->lab2Body(tempTrqLab.vec);
340 <    tempJi = dtOver2 * tempTrqLab;
341 <    rotationPropagation( sd, tempJi.vec);
338 >  //calculate constraint torque in lab frame
339 >  consTorque = crossProduct(a.transpose() * refCoor, consForce);
340 >  consRB->addConsTorque(consTorque/eConvert);
341  
342 <    sd->getJ(ji.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 <    ji += tempJi;
348 >  rb->getTrq(Tb.vec);
349 >  rb->lab2Body(Tb.vec);
350  
351 <    sd->setJ(ji.vec);
348 <  }
351 >  //get the angular momentum, and propagate a half step
352  
353 +  rb->getJ(ji.vec);
354 +
355 +  ji += eConvert * dtOver2 * Tb;
356 +
357 +  rotationPropagation( rb, ji.vec );
358 +
359 +  rb->setJ(ji.vec);
360    
361   }
362  
# Line 355 | 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 371 | 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 387 | 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];
391 <    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 504 | Line 514 | int DCRollBFunctor::operator()(ConstraintRigidBody* co
514    double pabDotZeta;
515    double pvab;
516  
517 <  const int conRBMaxIter = 100;
517 >  const int conRBMaxIter = 20;
518    
519    dt = info->dt;
520    
# Line 534 | Line 544 | int DCRollBFunctor::operator()(ConstraintRigidBody* co
544        
545        pabDotZeta = dotProduct(pab,  zeta);
546        
547 <      consForce = pvab / (dt * pabDotZeta) * bondDirUnitVec;
547 >      consForce =  2 * pvab / (dt * pabDotZeta) * bondDirUnitVec;
548        //integrate consRB1 using constraint force;
549        integrate(consRB1, consForce);
550  
# Line 566 | Line 576 | void DCRollBFunctor::getZeta(ConstraintRigidBody* cons
576    Mat3x3d invIBody;
577    Mat3x3d invILab;
578    Mat3x3d a;
569 –  Mat3x3d aTrans;
579    
580    invMass = 1.0 / consRB ->getMass();
581  
# Line 576 | Line 585 | void DCRollBFunctor::getZeta(ConstraintRigidBody* cons
585    consRB->getA(a.element);
586    consRB->getI(IBody.element);
587  
579 –  aTrans = a.transpose();
588    invIBody = IBody.inverse();
589  
590 <  invILab = aTrans * invIBody * a;
590 >  
591 >  refCrossBond = crossProduct(refCoor, a * bondDir);  
592  
593 <  refCrossBond = crossProduct(refCoor, bondDir);  
593 >  tempVec1 = invIBody * refCrossBond;
594  
595 <  tempVec1 = invILab * refCrossBond;
596 <  tempVec2 = crossProduct(tempVec1, refCoor);
588 <
595 >  tempVec2 = (a * tempVec1.makeSkewMat()).transpose() * refCoor;
596 >  
597    zeta += tempVec2;
598 +
599   }
600  
601   void DCRollBFunctor::integrate(ConstraintRigidBody* consRB, const Vector3d& force){
# Line 597 | Line 606 | void DCRollBFunctor::integrate(ConstraintRigidBody* co
606    Vector3d refCoor;
607    double mass;
608    double dtOver2;
609 +  Mat3x3d a;
610    StuntDouble* sd;
611    
612    sd = consRB->getStuntDouble();  
# Line 608 | Line 618 | void DCRollBFunctor::integrate(ConstraintRigidBody* co
618    sd->setVel(vel.vec);
619  
620    if (sd->isDirectional()){
621 <    tempTrq = crossProduct(refCoor, force);
622 <    sd->lab2Body(tempTrq.vec);
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;

Diff Legend

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