--- trunk/OOPSE/libmdtools/Shake.cpp	2004/06/03 21:51:55	1232
+++ trunk/OOPSE/libmdtools/Shake.cpp	2004/06/09 16:16:33	1254
@@ -2,6 +2,7 @@
 #include "SimInfo.hpp"
 #include <cmath>
 #include "simError.h"
+#include "MatVec3.h"
 ////////////////////////////////////////////////////////////////////////////////
 //Implementation of DCShakeFunctor
 ////////////////////////////////////////////////////////////////////////////////
@@ -71,38 +72,36 @@ int DCShakeFunctor::operator()(ConstraintAtom* consAto
     dy = rab[1] * gab;
     dz = rab[2] * gab;
 
+    //set atom1's position
     posA[0] += rma * dx;
     posA[1] += rma * dy;
     posA[2] += rma * dz;
-
     consAtom1->setPos(posA);
 
+    //set atom2's position
     posB[0] -= rmb * dx;
     posB[1] -= rmb * dy;
     posB[2] -= rmb * dz;
-
     consAtom2->setPos(posB);
 
-    dx = dx / dt;
-    dy = dy / dt;
-    dz = dz / dt;
+    //dx = dx / dt;
+    //dy = dy / dt;
+    //dz = dz / dt;
 
-    consAtom1->getVel(velA);
+    ////set atom1's velocity
+    //consAtom1->getVel(velA);
+    //velA[0] += rma * dx;
+    //velA[1] += rma * dy;
+    //velA[2] += rma * dz;
+    //consAtom1->setVel(velA);
 
-    velA[0] += rma * dx;
-    velA[1] += rma * dy;
-    velA[2] += rma * dz;
+    ////set atom2's velocity
+    //consAtom2->getVel(velB);
+    //velB[0] -= rmb * dx;
+    //velB[1] -= rmb * dy;
+    //velB[2] -= rmb * dz;
+    //consAtom2->setVel(velB);
 
-    consAtom1->setVel(velA);
-
-    consAtom2->getVel(velB);
-
-    velB[0] -= rmb * dx;
-    velB[1] -= rmb * dy;
-    velB[2] -= rmb * dz;
-
-    consAtom2->setVel(velB);
-
     return consSuccess;
   }
   else
@@ -115,10 +114,157 @@ int DCShakeFunctor::operator()(ConstraintAtom* consAto
   return consElemHandlerFail;
 } 
 
+/**
+  * QSHAKE Algorithm
+  * Reference
+  * T.R. Forester and W. Smith, SHAKE, Rattle and Roll: Efficient Constraint Algorithms for Linked 
+  * Rigid Bodies, J. Comp. Chem., 19(1), 102 -111 (1998)
+  */
 int DCShakeFunctor::operator()(ConstraintRigidBody* consRB1, ConstraintRigidBody* consRB2){
-  return consElemHandlerFail;
+  double posA[3];
+  double posB[3];
+  double oldPosA[3];
+  double oldPosB[3];
+  double velA[3];
+  double velB[3];
+  double pab[3];
+  double tempPab[3];
+  double rab[3];
+  double rma, rmb;
+  double dx, dy, dz;
+  double rpab;
+  double rabsq, pabsq, rpabsq;
+  double diffsq;
+  double gab;
+  double dt;
+  double dt2;
+  double consForce[3];
+
+  const int conRBMaxIter = 10;
+  
+  dt = info->dt;
+  dt2 = dt * dt;
+
+  consRB1->getOldAtomPos(oldPosA);
+  consRB2->getOldAtomPos(oldPosB);      
+
+
+  for(int i=0 ; i < conRBMaxIter; i++){   
+  consRB1->getCurAtomPos(posA);
+  consRB2->getCurAtomPos(posB);
+
+   
+  pab[0] = posA[0] - posB[0];
+  pab[1] = posA[1] - posB[1];
+  pab[2] = posA[2] - posB[2];
+
+  //periodic boundary condition
+
+  info->wrapVector(pab);
+
+  pabsq = pab[0] * pab[0] + pab[1] * pab[1] + pab[2] * pab[2];
+
+  rabsq = curPair->getBondLength2();
+  diffsq = rabsq - pabsq;
+
+  // the original rattle code from alan tidesley
+  if (fabs(diffsq) > (consTolerance * rabsq * 2)){
+       
+    rab[0] = oldPosA[0] - oldPosB[0];
+    rab[1] = oldPosA[1] - oldPosB[1];
+    rab[2] = oldPosA[2] - oldPosB[2];
+
+    info->wrapVector(rab);
+
+    rpab = rab[0] * pab[0] + rab[1] * pab[1] + rab[2] * pab[2];
+
+    rpabsq = rpab * rpab;
+
+
+    if (rpabsq < (rabsq * -diffsq)){
+      return consFail;
+    }
+
+    //rma = 1.0 / consRB1->getMass();
+    //rmb = 1.0 / consRB2->getMass();
+    
+    tempPab[0] = pab[0] / sqrt(pabsq);
+    tempPab[1] = pab[1] / sqrt(pabsq);
+    tempPab[2] = pab[2] / sqrt(pabsq);
+    
+    rma = getEffInvMass(consRB1, tempPab);
+
+    tempPab[0] = -tempPab[0];
+    tempPab[1] = -tempPab[1];
+    tempPab[2] = -tempPab[2];    
+    rmb = getEffInvMass(consRB2, tempPab);
+
+    gab = diffsq / (2.0* dt * dt * (rma + rmb) * rpab) ;
+    consForce[0] = rab[0] * gab;
+    consForce[1] = rab[1] * gab;
+    consForce[2] = rab[2] * gab;
+
+    //integrate consRB1 using constraint force;
+    integrate(consRB1,consForce);
+
+    //integrate consRB2 using constraint force;
+    consForce[0] = -consForce[0];
+    consForce[1] = -consForce[1];
+    consForce[2] = -consForce[2];
+    integrate(consRB2,consForce);    
+    
+  }
+  else{
+    if (i ==0)
+      return consAlready;
+    else
+      return consSuccess;
+  }
+  }
+
+  return consExceedMaxIter;
 }
 
+double DCShakeFunctor::getEffInvMass(ConstraintRigidBody* consRB, double bondDir[3]){
+  double effInvMass;  //effective inversse mass
+  double effInvMassCorr;  //correction for effective inverse mass
+  double aTrans[3][3];
+  double a[3][3];
+
+  double IFrame[3][3];
+  double IBody[3][3];
+  double invI[3][3];
+  double refCoor[3];
+  double refCrossBond[3];
+  double tempVec1[3];
+  double tempVec2[3];
+
+  effInvMass = 1.0 / consRB ->getMass();
+  
+  consRB->getRefCoor(refCoor);
+  consRB->getA(a);
+  consRB->getI(IBody);
+
+  crossProduct3(refCoor, bondDir, refCrossBond);
+
+  matMul3(a, IBody, IFrame);
+
+  invertMat3(IFrame, invI);
+
+  matVecMul3(invI, refCrossBond, tempVec1);
+  
+  crossProduct3(tempVec1, refCoor, tempVec2);
+
+  effInvMassCorr = dotProduct3(tempVec1, bondDir);
+    
+  effInvMass += effInvMassCorr;
+
+  return effInvMass;
+}
+
+void DCShakeFunctor::integrate(ConstraintRigidBody* consRB, double force[3]){
+
+}
 ////////////////////////////////////////////////////////////////////////////////
 //Implementation of JCShakeFunctor
 ////////////////////////////////////////////////////////////////////////////////
@@ -135,4 +281,4 @@ int JCShakeFunctor::operator()(ConstraintRigidBody* co
  
 int JCShakeFunctor::operator()(ConstraintRigidBody* consRB1, ConstraintRigidBody* consRB2){
   return consElemHandlerFail;
-}
\ No newline at end of file
+}