--- trunk/OOPSE/libmdtools/Rattle.cpp	2004/06/03 21:51:55	1232
+++ trunk/OOPSE/libmdtools/Rattle.cpp	2004/06/21 18:52:21	1284
@@ -1,8 +1,147 @@
 #include "Rattle.hpp"
 #include <cmath>
 #include "SimInfo.hpp"
+#include "MatVec3.h"
+////////////////////////////////////////////////////////////////////////////////
+//Implementation of DCShakeFunctor
+////////////////////////////////////////////////////////////////////////////////
+int DCRattleAFunctor::operator()(ConstraintAtom* consAtom1, ConstraintAtom* consAtom2){
+  double posA[3];
+  double posB[3];
+  double oldPosA[3];
+  double oldPosB[3];
+  double velA[3];
+  double velB[3];
+  double pab[3];
+  double rab[3];
+  double rma, rmb;
+  double dx, dy, dz;
+  double rpab;
+  double rabsq, pabsq, rpabsq;
+  double diffsq;
+  double gab;
+  double dt;
+  
+  dt = info->dt;
+  
+  consAtom1->getPos(posA);
+  consAtom2->getPos(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)){
+    
+    consAtom1->getOldPos(oldPosA);
+    consAtom2->getOldPos(oldPosB);      
+    
+    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 / consAtom1->getMass();
+    rmb = 1.0 / consAtom2->getMass();
+
+    gab = diffsq / (2.0 * (rma + rmb) * rpab);
+
+    dx = rab[0] * gab;
+    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;
+
+    //set atom1's velocity
+    consAtom1->getVel(velA);
+    velA[0] += rma * dx;
+    velA[1] += rma * dy;
+    velA[2] += rma * dz;
+    consAtom1->setVel(velA);
+
+    //set atom2's velocity
+    consAtom2->getVel(velB);
+    velB[0] -= rmb * dx;
+    velB[1] -= rmb * dy;
+    velB[2] -= rmb * dz;
+    consAtom2->setVel(velB);
+
+    return consSuccess;
+  }
+  else
+    return consAlready;
+  
+}
+
+
+int DCRattleAFunctor::operator()(ConstraintAtom* consAtom,ConstraintRigidBody* consRB){
+  return consElemHandlerFail;
+} 
+
+int DCRattleAFunctor::operator()(ConstraintRigidBody* consRB1, ConstraintRigidBody* consRB2){
+  //calculate lamda
+
+  //integrate
+
+  //
+  return consElemHandlerFail;
+}
 ////////////////////////////////////////////////////////////////////////////////
+//Implementation of JCShakeFunctor
+////////////////////////////////////////////////////////////////////////////////
+int JCRattleAFunctor::operator()(ConstraintAtom* consAtom1, ConstraintAtom* consAtom2){
+  return consElemHandlerFail;
+}
+
+int JCRattleAFunctor::operator()(ConstraintAtom* consAtom,ConstraintRigidBody* consRB){
+  return consElemHandlerFail;
+
+}
+ 
+int JCRattleAFunctor::operator()(ConstraintRigidBody* consRB1, ConstraintRigidBody* consRB2){
+  //calculate lamda,
+  //integrate
+  //
+  return consElemHandlerFail;
+}
+
+
+////////////////////////////////////////////////////////////////////////////////
 //Implementation of DCRattleBFunctor
 ////////////////////////////////////////////////////////////////////////////////
 int DCRattleBFunctor::operator()(ConstraintAtom* consAtom1, ConstraintAtom* consAtom2){
@@ -54,8 +193,11 @@ int DCRattleBFunctor::operator()(ConstraintAtom* consA
       velB[2] -= rmb * dz;
 
       consAtom2->setVel(velB);
+      return consSuccess;
     }
-    return consSuccess;
+    else
+      return consAlready;
+
 }