--- trunk/OOPSE/libmdtools/Roll.cpp	2004/06/11 17:16:21	1268
+++ trunk/OOPSE/libmdtools/Roll.cpp	2004/06/21 18:52:21	1284
@@ -91,12 +91,12 @@ int DCRollAFunctor::operator()(ConstraintAtom* consAto
       //G : constraint force scalar
       //d: equilibrium bond length
       
-      if (pabDotZeta2 - zeta2 * diffsq < 0)
+      if (pabDotZeta2 - zeta2 * diffsq < 0)   
         return consFail;
-
+      
       //forceScalar = (pabDotZeta + sqrt(pabDotZeta2 - zeta2 * diffsq)) / dt * dt * zeta2;
       forceScalar = diffsq / (2 * dt * dt * pabDotZeta); 
-      //
+      //forceScalar = 1 / forceScalar;
       consForce = forceScalar * bondDirUnitVec;
       //integrate consRB1 using constraint force;
       integrate(consAtom1, consForce);
@@ -129,14 +129,10 @@ void DCRollAFunctor::integrate(ConstraintAtom* consAto
   Vector3d pos;
   Vector3d tempPos;
   Vector3d tempVel;
-
   double mass;
-  double dtOver2;
   double dt;
-  const double eConvert = 4.184e-4;
   
   dt = info->dt;
-  dtOver2 = dt /2;  
   sd = consAtom->getStuntDouble();
   
   sd->getVel(vel.vec);
@@ -144,7 +140,7 @@ void DCRollAFunctor::integrate(ConstraintAtom* consAto
   
   mass = sd->getMass();
 
-  tempVel = eConvert * dtOver2/mass * force;
+  tempVel = dt/mass * force;
   tempPos = dt * tempVel;
 	
   vel += tempVel;
@@ -180,7 +176,7 @@ int DCRollAFunctor::operator()(ConstraintRigidBody* co
   double zeta2;
   double forceScalar;
   
-  const int conRBMaxIter = 10;
+  const int conRBMaxIter = 100;
   
   dt = info->dt;
 
@@ -209,16 +205,7 @@ int DCRollAFunctor::operator()(ConstraintRigidBody* co
       rab = oldPosB - oldPosA;       
       info->wrapVector(rab.vec);
 
-      //rpab = dotProduct(rab, pab);
-
-      //rpabsq = rpab * rpab;
-
-
-      //if (rpabsq < (rabsq * -diffsq)){
-      //  return consFail;
-      //}
-
-      bondDirUnitVec = pab;
+      bondDirUnitVec = rab;
       bondDirUnitVec.normalize();
           
       calcZeta(consRB1, bondDirUnitVec, zetaA);
@@ -238,11 +225,16 @@ int DCRollAFunctor::operator()(ConstraintRigidBody* co
       //G : constraint force scalar
       //d: equilibrium bond length
       
-      if (pabDotZeta2 - zeta2 * diffsq < 0)
+      if (pabDotZeta2 - zeta2 * diffsq < 0){
+        cerr << "DCRollAFunctor::operator() Error: Constraint Fail at " << info->getTime() << endl;   
         return consFail;
-
-      //forceScalar = (pabDotZeta + sqrt(pabDotZeta2 - zeta2 * diffsq)) / dt * dt * zeta2;
-      forceScalar = diffsq / (2 * dt * dt * pabDotZeta); 
+      }
+      //if pabDotZeta is close to 0, we can't neglect the square term
+      if(fabs(pabDotZeta) < consTolerance)
+        forceScalar = (pabDotZeta - sqrt(pabDotZeta2 - zeta2 * diffsq)) / dt * dt * zeta2;
+      else
+        forceScalar = diffsq / (2 * dt * dt * pabDotZeta); 
+        
       //
       consForce = forceScalar * bondDirUnitVec;
       //integrate consRB1 using constraint force;
@@ -260,6 +252,7 @@ int DCRollAFunctor::operator()(ConstraintRigidBody* co
     }
   }
 
+  cerr << "DCRollAFunctor::operator() Error: can not constrain the bond within maximum iteration at " << info->getTime() << endl; 
   return consExceedMaxIter;
 
 }
@@ -271,9 +264,8 @@ void DCRollAFunctor::calcZeta(ConstraintRigidBody* con
   Vector3d refCoor;
   Vector3d refCrossBond;  
   Mat3x3d IBody;
-  Mat3x3d IFrame;
   Mat3x3d invIBody;
-  Mat3x3d invIFrame;
+  Mat3x3d invILab;
   Mat3x3d a;
   Mat3x3d aTrans;
   
@@ -288,11 +280,11 @@ void DCRollAFunctor::calcZeta(ConstraintRigidBody* con
   aTrans = a.transpose();
   invIBody = IBody.inverse();
 
-  IFrame = aTrans * invIBody * a;
+  invILab = aTrans * invIBody * a;
 
   refCrossBond = crossProduct(refCoor, bondDir);  
 
-  tempVec1 = invIFrame * refCrossBond;
+  tempVec1 = invILab * refCrossBond;
   tempVec2 = crossProduct(tempVec1, refCoor);
 
   zeta += tempVec2;
@@ -305,17 +297,20 @@ void DCRollAFunctor::integrate(ConstraintRigidBody* co
   Vector3d pos;
   Vector3d Tb;
   Vector3d ji;
-	Vector3d tempPos;
-	Vector3d tempVel;
-	Vector3d tempTrq;
-	Vector3d tempJi;
+  Vector3d tempPos;
+  Vector3d tempVel;
+  Vector3d tempTrqLab;
+  Vector3d tempTrqBody;  
+  Vector3d tempJi;
+  Vector3d refCoor;
   double mass;
-  double dtOver2;
+  Mat3x3d oldA;
   double dt;
-  const double eConvert = 4.184e-4;
-  
+  double dtOver2;
   dt = info->dt;
-  dtOver2 = dt /2;  
+  dtOver2 = dt /2;
+
+  consRB->getOldA(oldA.element);
   sd = consRB->getStuntDouble();
   
   sd->getVel(vel.vec);
@@ -323,7 +318,7 @@ void DCRollAFunctor::integrate(ConstraintRigidBody* co
   
   mass = sd->getMass();
 
-  tempVel = eConvert * dtOver2/mass * force;
+  tempVel = dtOver2/mass * force;
   tempPos = dt * tempVel;
 	
 	vel += tempVel;
@@ -334,21 +329,24 @@ void DCRollAFunctor::integrate(ConstraintRigidBody* co
 
   if (sd->isDirectional()){
 
-    // get and convert the torque to body frame
+    consRB->getRefCoor(refCoor.vec);
+    tempTrqLab = crossProduct(refCoor, force);
 
-    sd->getTrq(Tb.vec);
-    sd->lab2Body(Tb.vec);
+    //convert torque in lab frame to torque in body frame using old rotation matrix
+    //tempTrqBody = oldA * tempTrqLab;
+    
+    //tempJi = dtOver2 * tempTrqBody;
+    sd->lab2Body(tempTrqLab.vec);
+    tempJi = dtOver2 * tempTrqLab;
+    rotationPropagation( sd, tempJi.vec);
 
-    // get the angular momentum, and propagate a half step
-
     sd->getJ(ji.vec);
 
-    ji += eConvert * dtOver2 * Tb;
+    ji += tempJi;
 
-    rotationPropagation( sd, ji.vec);
-
     sd->setJ(ji.vec);
   }
+
   
 }
 
@@ -496,58 +494,49 @@ int DCRollBFunctor::operator()(ConstraintRigidBody* co
   Vector3d pab;
   Vector3d rab;
   Vector3d vab;
-  Vector3d rma;
-  Vector3d rmb;
+  Vector3d zetaA;
+  Vector3d zetaB;
+  Vector3d zeta;
   Vector3d consForce;
   Vector3d bondDirUnitVec;  
-  double dx, dy, dz;
-  double rpab;
-  double rabsq, pabsq, rpabsq;
-  double diffsq;
-  double gab;
   double dt;
-  double pabcDotvab;
-  double pabDotInvMassVec;
+  double pabDotvab;
+  double pabDotZeta;
+  double pvab;
 
-
-  const int conRBMaxIter = 10;
+  const int conRBMaxIter = 100;
   
   dt = info->dt;
   
   for(int i=0 ; i < conRBMaxIter; i++){   
     consRB1->getCurAtomPos(posA.vec);
     consRB2->getCurAtomPos(posB.vec);
-    pab = posA - posB;
-    
-    consRB1->getVel(velA.vec);
-    consRB2->getVel(velB.vec);
-    vab = velA -velB;
+    pab = posB - posA;
 
     //periodic boundary condition
-
     info->wrapVector(pab.vec);
+    
+    consRB1->getCurAtomVel(velA.vec);
+    consRB2->getCurAtomVel(velB.vec);
+    vab = velB -velA;
 
-    pabsq = pab.length2();
+    pvab = dotProduct(pab, vab);
 
-    rabsq = curPair->getBondLength2();
-    diffsq = rabsq - pabsq;
+    if (fabs(pvab) > consTolerance ){
 
-    if (fabs(diffsq) > (consTolerance * rabsq * 2)){
 
-
       bondDirUnitVec = pab;
       bondDirUnitVec.normalize();
           
-      getEffInvMassVec(consRB1, bondDirUnitVec, rma);
-
-      getEffInvMassVec(consRB2, bondDirUnitVec, rmb);
-
-      pabcDotvab = dotProduct(pab, vab);
-      pabDotInvMassVec = dotProduct(pab,  rma + rmb);
+      getZeta(consRB1, bondDirUnitVec, zetaA);
+      getZeta(consRB2, bondDirUnitVec, zetaB);
+      zeta = zetaA + zetaB;
       
-      consForce = pabcDotvab /(2 * dt * pabDotInvMassVec) * bondDirUnitVec;
+      pabDotZeta = dotProduct(pab,  zeta);
+      
+      consForce = pvab / (dt * pabDotZeta) * bondDirUnitVec;
       //integrate consRB1 using constraint force;
-      integrate(consRB1,consForce);
+      integrate(consRB1, consForce);
 
       //integrate consRB2 using constraint force;
       integrate(consRB2, -consForce);    
@@ -561,26 +550,27 @@ int DCRollBFunctor::operator()(ConstraintRigidBody* co
     }
   }
 
+  cerr << "DCRollBFunctor::operator() Error: can not constrain the bond within maximum iteration at " << info->getTime() << endl;  
   return consExceedMaxIter;
 
 }
 
-void DCRollBFunctor::getEffInvMassVec(ConstraintRigidBody* consRB, const Vector3d& bondDir, Vector3d& invMassVec){
+void DCRollBFunctor::getZeta(ConstraintRigidBody* consRB, const Vector3d& bondDir, Vector3d& zeta){
   double invMass;
   Vector3d tempVec1;
   Vector3d tempVec2;
   Vector3d refCoor;
   Vector3d refCrossBond;  
   Mat3x3d IBody;
-  Mat3x3d IFrame;
+  Mat3x3d ILab;
   Mat3x3d invIBody;
-  Mat3x3d invIFrame;
+  Mat3x3d invILab;
   Mat3x3d a;
   Mat3x3d aTrans;
   
   invMass = 1.0 / consRB ->getMass();
 
-  invMassVec = invMass * bondDir;
+  zeta = invMass * bondDir;
   
   consRB->getRefCoor(refCoor.vec);
   consRB->getA(a.element);
@@ -589,22 +579,22 @@ void DCRollBFunctor::getEffInvMassVec(ConstraintRigidB
   aTrans = a.transpose();
   invIBody = IBody.inverse();
 
-  IFrame = aTrans * invIBody * a;
+  invILab = aTrans * invIBody * a;
 
   refCrossBond = crossProduct(refCoor, bondDir);  
 
-  tempVec1 = invIFrame * refCrossBond;
+  tempVec1 = invILab * refCrossBond;
   tempVec2 = crossProduct(tempVec1, refCoor);
 
-  invMassVec += tempVec2;
+  zeta += tempVec2;
 }
 
 void DCRollBFunctor::integrate(ConstraintRigidBody* consRB, const Vector3d& force){
-  const double eConvert = 4.184e-4;
   Vector3d vel;
-  Vector3d pos;
-  Vector3d Tb;
   Vector3d ji;
+  Vector3d tempJi;
+  Vector3d tempTrq;
+  Vector3d refCoor;
   double mass;
   double dtOver2;
   StuntDouble* sd;
@@ -612,27 +602,17 @@ void DCRollBFunctor::integrate(ConstraintRigidBody* co
   sd = consRB->getStuntDouble();  
   dtOver2 = info->dt/2;
 
+  sd->getVel(vel.vec);
   mass = sd->getMass();
-
-  // velocity half step
-
-  vel += eConvert * dtOver2 /mass * force;
-
+  vel +=dtOver2 /mass * force;
   sd->setVel(vel.vec);
 
   if (sd->isDirectional()){
-
-    // get and convert the torque to body frame
-
-    sd->getTrq(Tb.vec);
-    sd->lab2Body(Tb.vec);
-
-    // get the angular momentum, and propagate a half step
-
+    tempTrq = crossProduct(refCoor, force);
+    sd->lab2Body(tempTrq.vec);
+    tempJi = dtOver2* tempTrq;
     sd->getJ(ji.vec);
-
-    ji += eConvert * dtOver2* Tb;
-
+    ji += tempJi;
     sd->setJ(ji.vec);
   }