--- trunk/OOPSE/libmdtools/Roll.cpp	2004/06/09 16:59:03	1255
+++ trunk/OOPSE/libmdtools/Roll.cpp	2004/08/23 15:11:36	1452
@@ -7,7 +7,7 @@
 ////////////////////////////////////////////////////////////////////////////////
 //Implementation of DCRollAFunctor
 ////////////////////////////////////////////////////////////////////////////////
-int DCRollAFunctor::operator()(ConstraintRigidBody* consRB1, ConstraintRigidBody* consRB2){
+int DCRollAFunctor::operator()(ConstraintAtom* consAtom1, ConstraintAtom* consAtom2){
   Vector3d posA;
   Vector3d posB;
   Vector3d oldPosA;
@@ -17,8 +17,9 @@ int DCRollAFunctor::operator()(ConstraintRigidBody* co
   Vector3d pab;
   Vector3d tempPab;
   Vector3d rab;
-  Vector3d rma;
-  Vector3d rmb;
+  Vector3d zetaA;
+  Vector3d zetaB;
+  Vector3d zeta;
   Vector3d consForce;
   Vector3d bondDirUnitVec;  
   double dx, dy, dz;
@@ -27,22 +28,24 @@ int DCRollAFunctor::operator()(ConstraintRigidBody* co
   double diffsq;
   double gab;
   double dt;
-  double pabDotInvMassVec;
+  double pabDotZeta;
+  double pabDotZeta2;
+  double zeta2;
+  double forceScalar;
 
-
-  const int conRBMaxIter = 10;
   
   dt = info->dt;
 
-  consRB1->getOldAtomPos(oldPosA.vec);
-  consRB2->getOldAtomPos(oldPosB.vec);      
+  consAtom1->getOldPos(oldPosA.vec);
+  consAtom2->getOldPos(oldPosB.vec);      
 
 
-  for(int i=0 ; i < conRBMaxIter; i++){   
-    consRB1->getCurAtomPos(posA.vec);
-    consRB2->getCurAtomPos(posB.vec);
+    consAtom1->getPos(posA.vec);
+    consAtom2->getPos(posB.vec);
 
-    pab = posA - posB;
+    //discard the vector convention in alan tidesley's code
+    //rij =  rj - ri;
+    pab = posB - posA;
 
     //periodic boundary condition
 
@@ -51,15 +54,15 @@ int DCRollAFunctor::operator()(ConstraintRigidBody* co
     pabsq = dotProduct(pab, pab);
 
     rabsq = curPair->getBondLength2();
-    diffsq = rabsq - pabsq;
+    diffsq =  pabsq -rabsq;
 
     if (fabs(diffsq) > (consTolerance * rabsq * 2)){
-      rab = oldPosA - oldPosB;       
+      rab = oldPosB - oldPosA;       
       info->wrapVector(rab.vec);
 
-      rpab = dotProduct(rab, pab);
+      //rpab = dotProduct(rab, pab);
 
-      rpabsq = rpab * rpab;
+      //rpabsq = rpab * rpab;
 
 
       //if (rpabsq < (rabsq * -diffsq)){
@@ -69,17 +72,170 @@ int DCRollAFunctor::operator()(ConstraintRigidBody* co
       bondDirUnitVec = pab;
       bondDirUnitVec.normalize();
           
-      getEffInvMassVec(consRB1, bondDirUnitVec, rma);
+      calcZeta(consAtom1, bondDirUnitVec, zetaA);
 
-      getEffInvMassVec(consRB2, -bondDirUnitVec, rmb);
+      calcZeta(consAtom2, bondDirUnitVec, zetaB);
 
-      pabDotInvMassVec = dotProduct(pab,  rma + rmb);
+      zeta = zetaA + zetaB;      
+      zeta2 = dotProduct(zeta, zeta);
       
-      consForce = diffsq /(2 * dt * dt * pabDotInvMassVec) * bondDirUnitVec;
+      pabDotZeta = dotProduct(pab,  zeta);
+      pabDotZeta2 = pabDotZeta * pabDotZeta;
+
+      //solve quadratic equation
+      //dt^4 * zeta^2 * G^2 + 2* h^2 * pab * zeta * G + pab^2 - d^2
+      //dt : time step
+      // pab : 
+      //G : constraint force scalar
+      //d: equilibrium bond length
+      
+      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(consRB1,consForce);
+      integrate(consAtom1, consForce);
 
       //integrate consRB2 using constraint force;
+      integrate(consAtom2, -consForce);    
+
+      return consSuccess;
+    }
+    else
+        return consAlready;
+
+       
+
+
+}
+void DCRollAFunctor::calcZeta(ConstraintAtom* consAtom, const Vector3d& bondDir, Vector3d&zeta){
+  double invMass;
+  invMass = 1.0 / consAtom ->getMass();
+
+  zeta = invMass * bondDir;
+}
+
+void DCRollAFunctor::integrate(ConstraintAtom* consAtom, const Vector3d& force){
+  StuntDouble* sd;
+  Vector3d vel;
+  Vector3d pos;
+  Vector3d tempPos;
+  Vector3d tempVel;
+  double mass;
+  double dt;
+  
+  dt = info->dt;
+  sd = consAtom->getStuntDouble();
+  
+  sd->getVel(vel.vec);
+  sd->getPos(pos.vec);
+  
+  mass = sd->getMass();
+
+  tempVel = dt/mass * force;
+  tempPos = dt * tempVel;
+	
+  vel += tempVel;
+  pos += tempPos;
+
+  sd->setVel(vel.vec);
+  sd->setPos(pos.vec);
+}
+
+int DCRollAFunctor::operator()(ConstraintRigidBody* consRB1, ConstraintRigidBody* consRB2){
+  Vector3d posA;
+  Vector3d posB;
+  Vector3d oldPosA;
+  Vector3d oldPosB;
+  Vector3d velA;
+  Vector3d velB;
+  Vector3d pab;
+  Vector3d tempPab;
+  Vector3d rab;
+  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 pabDotZeta;
+  double pabDotZeta2;
+  double zeta2;
+  double forceScalar;
+  
+  const int conRBMaxIter = 100;
+  
+  dt = info->dt;
+
+  consRB1->getOldAtomPos(oldPosA.vec);
+  consRB2->getOldAtomPos(oldPosB.vec);      
+
+
+  for(int i=0 ; i < conRBMaxIter; i++){   
+    consRB1->getCurAtomPos(posA.vec);
+    consRB2->getCurAtomPos(posB.vec);
+
+    //discard the vector convention in alan tidesley's code
+    //rij =  rj - ri;
+    pab = posB - posA;
+
+    //periodic boundary condition
+
+    info->wrapVector(pab.vec);
+
+    pabsq = dotProduct(pab, pab);
+
+    rabsq = curPair->getBondLength2();
+    diffsq =  pabsq -rabsq;
+
+    if (fabs(diffsq) > (consTolerance * rabsq * 2)){
+      rab = oldPosB - oldPosA;       
+      info->wrapVector(rab.vec);
+
+      bondDirUnitVec = rab;
+      bondDirUnitVec.normalize();
+          
+      calcZeta(consRB1, bondDirUnitVec, zetaA);
+
+      calcZeta(consRB2, bondDirUnitVec, zetaB);
+
+      zeta = zetaA + zetaB;      
+      zeta2 = dotProduct(zeta, zeta);
+      
+      pabDotZeta = dotProduct(pab,  zeta);
+      pabDotZeta2 = pabDotZeta * pabDotZeta;
+
+      //solve quadratic equation
+      //dt^4 * zeta^2 * G^2 + 2* h^2 * pab * zeta * G + pab^2 - d^2
+      //dt : time step
+      // pab : 
+      //G : constraint force scalar
+      //d: equilibrium bond length
+      
+      if (pabDotZeta2 - zeta2 * diffsq < 0){
+        cerr << "DCRollAFunctor::operator() Error: Constraint Fail at " << info->getTime() << endl;   
+        return consFail;
+      }
+      //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;
+      integrate(consRB1, consForce);
+
+      //integrate consRB2 using constraint force;
       integrate(consRB2, -consForce);    
       
     }
@@ -91,88 +247,116 @@ int DCRollAFunctor::operator()(ConstraintRigidBody* co
     }
   }
 
+  cerr << "DCRollAFunctor::operator() Error: can not constrain the bond within maximum iteration at " << info->getTime() << endl; 
   return consExceedMaxIter;
 
 }
 
-void DCRollAFunctor::getEffInvMassVec(ConstraintRigidBody* consRB, const Vector3d& bondDir, Vector3d& invMassVec){
+void DCRollAFunctor::calcZeta(ConstraintRigidBody* consRB, const Vector3d& bondDir, Vector3d& zeta){
   double invMass;
   Vector3d tempVec1;
   Vector3d tempVec2;
   Vector3d refCoor;
   Vector3d refCrossBond;  
   Mat3x3d IBody;
-  Mat3x3d IFrame;
   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);
+  //consRB->getA(a.element);
+  consRB->getOldA(a.element);
   consRB->getI(IBody.element);
 
   aTrans = a.transpose();
   invIBody = IBody.inverse();
 
-  IFrame = aTrans * invIBody * a;
+  invILab = aTrans * invIBody * a;
 
-  refCrossBond = crossProduct(refCoor, bondDir);  
+  refCrossBond = crossProduct(aTrans *refCoor, bondDir);  
 
-  tempVec1 = invIFrame * refCrossBond;
-  tempVec2 = crossProduct(tempVec1, refCoor);
+  tempVec1 = invILab * refCrossBond;
+  tempVec2 = crossProduct(tempVec1, aTrans *refCoor);
 
-  invMassVec += tempVec2;
+  zeta += tempVec2;
    
 }
 
-void DCRollAFunctor::integrate(ConstraintRigidBody* consRB, const Vector3d& force){
-  StuntDouble* sd;
+void DCRollAFunctor::integrate(ConstraintRigidBody* consRB, const Vector3d& consForce){
+  RigidBody* rb;
+  Vector3d frc;
+  Vector3d totConsForce;
   Vector3d vel;
   Vector3d pos;
   Vector3d Tb;
   Vector3d ji;
+  Vector3d refCoor;
+  Vector3d consTorque;
+  Vector3d totConsTorque;
+  Mat3x3d a;
   double mass;
-  double dtOver2;
   double dt;
+  double dtOver2;
   const double eConvert = 4.184e-4;
   
   dt = info->dt;
-  dtOver2 = dt /2;  
-  sd = consRB->getStuntDouble();
+  dtOver2 = dt /2;
+
+  //restore to old status
+  consRB->restoreUnconsStatus();
+
+  //accumulate constraint force;
+  consRB->addConsForce(consForce/eConvert);
+  totConsForce = consRB->getConsForce();
+
+  rb = consRB->getRigidBody();
   
-  sd->getVel(vel.vec);
-  sd->getPos(pos.vec);
+  rb->addFrc(totConsForce.vec);
   
-  mass = sd->getMass();
+  rb->getVel(vel.vec);
+  rb->getPos(pos.vec);
+  rb->getFrc(frc.vec);
+  mass = rb->getMass();
 
-  vel += eConvert * dtOver2/mass * force;
+  // velocity half step
+  vel += eConvert  * dtOver2 / mass * frc;
+  // position whole step
   pos += dt * vel;
 
-  sd->setVel(vel.vec);
-  sd->setPos(pos.vec);
+  rb->setVel(vel.vec);
+  rb->setPos(pos.vec);
 
-  if (sd->isDirectional()){
+  //evolve orientational part
+  consRB->getRefCoor(refCoor.vec);
+  rb->getA(a.element);
 
-    // get and convert the torque to body frame
+  //calculate constraint torque in lab frame
+  consTorque = crossProduct(a.transpose() * refCoor, consForce);
+  consRB->addConsTorque(consTorque/eConvert);
 
-    sd->getTrq(Tb.vec);
-    sd->lab2Body(Tb.vec);
+  //add constraint torque
+  totConsTorque = consRB->getConsTorque();  
+  rb->addTrq(totConsTorque.vec);
+  
+  //get and convert the torque to body frame
 
-    // get the angular momentum, and propagate a half step
+  rb->getTrq(Tb.vec);
+  rb->lab2Body(Tb.vec);
 
-    sd->getJ(ji.vec);
+  //get the angular momentum, and propagate a half step
 
-    ji += eConvert * dtOver2 * Tb;
+  rb->getJ(ji.vec);
 
-    rotationPropagation( sd, ji.vec);
+  ji += eConvert * dtOver2 * Tb;
 
-    sd->setJ(ji.vec);
-  }
+  rotationPropagation( rb, ji.vec );
+
+  rb->setJ(ji.vec);
   
 }
 
@@ -181,8 +365,9 @@ void DCRollAFunctor::rotationPropagation(StuntDouble* 
   double A[3][3], I[3][3];
   int i, j, k;
   double dtOver2;
-
-  dtOver2 = info->dt /2;
+  double dt;
+  dt = info->dt;
+  dtOver2 = dt /2;
   // use the angular velocities to propagate the rotation matrix a
   // full time step
 
@@ -197,7 +382,7 @@ void DCRollAFunctor::rotationPropagation(StuntDouble* 
     angle = dtOver2 * ji[j] / I[j][j];
     this->rotate( k, i, angle, ji, A );
 
-    angle = dtOver2 * ji[k] / I[k][k];
+    angle = dt* ji[k] / I[k][k];
     this->rotate( i, j, angle, ji, A);
 
     angle = dtOver2 * ji[j] / I[j][j];
@@ -213,8 +398,7 @@ void DCRollAFunctor::rotationPropagation(StuntDouble* 
     this->rotate( 2, 0, angle, ji, A );
     
     // rotate about the z-axis
-    angle = dtOver2 * ji[2] / I[2][2];
-    sd->addZangle(angle);
+    angle = dt * ji[2] / I[2][2];
     this->rotate( 0, 1, angle, ji, A);
     
     // rotate about the y-axis
@@ -320,58 +504,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 = 20;
   
   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 =  2 * pvab / (dt * pabDotZeta) * bondDirUnitVec;
       //integrate consRB1 using constraint force;
-      integrate(consRB1,consForce);
+      integrate(consRB1, consForce);
 
       //integrate consRB2 using constraint force;
       integrate(consRB2, -consForce);    
@@ -385,78 +560,71 @@ 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);
   consRB->getI(IBody.element);
 
-  aTrans = a.transpose();
   invIBody = IBody.inverse();
 
-  IFrame = aTrans * invIBody * a;
+  
+  refCrossBond = crossProduct(refCoor, a * bondDir);  
 
-  refCrossBond = crossProduct(refCoor, bondDir);  
+  tempVec1 = invIBody * refCrossBond;
 
-  tempVec1 = invIFrame * refCrossBond;
-  tempVec2 = crossProduct(tempVec1, refCoor);
+  tempVec2 = (a * tempVec1.makeSkewMat()).transpose() * refCoor;
+  
+  zeta += tempVec2;
 
-  invMassVec += 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;
+  Mat3x3d a;
   StuntDouble* sd;
   
   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()){
+    sd->getA(a.element);
+    consRB->getRefCoor(refCoor.vec);
+    tempTrq = crossProduct(refCoor, a *force);
 
-    // 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
-
+    tempJi = dtOver2* tempTrq;
     sd->getJ(ji.vec);
-
-    ji += eConvert * dtOver2* Tb;
-
+    ji += tempJi;
     sd->setJ(ji.vec);
   }