--- trunk/OOPSE/libmdtools/ShakeMin.cpp	2004/06/03 21:51:55	1232
+++ trunk/OOPSE/libmdtools/ShakeMin.cpp	2004/06/04 19:30:05	1248
@@ -1,9 +1,115 @@
+#include <cmath>
+#include "ConstraintElement.hpp"
 #include "ShakeMin.hpp"
+#include "SimInfo.hpp"
 ////////////////////////////////////////////////////////////////////////////////
 //Implementation of DCShakeMinRFunctor
 ////////////////////////////////////////////////////////////////////////////////
 int DCShakeRMinFunctor::operator()(ConstraintAtom* consAtom1, ConstraintAtom* consAtom2){
-  return consElemHandlerFail;
+  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();
+
+    rma = 1.0;
+    rmb= 1.0;
+    
+    gab = diffsq / (2.0 * (rma + rmb) * rpab);
+
+    dx = rab[0] * gab;
+    dy = rab[1] * gab;
+    dz = rab[2] * gab;
+
+    posA[0] += rma * dx;
+    posA[1] += rma * dy;
+    posA[2] += rma * dz;
+
+    consAtom1->setPos(posA);
+
+    posB[0] -= rmb * dx;
+    posB[1] -= rmb * dy;
+    posB[2] -= rmb * dz;
+
+    consAtom2->setPos(posB);
+
+    dx = dx / dt;
+    dy = dy / dt;
+    dz = dz / dt;
+
+    consAtom1->getVel(velA);
+
+    velA[0] += rma * dx;
+    velA[1] += rma * dy;
+    velA[2] += rma * dz;
+
+    consAtom1->setVel(velA);
+
+    consAtom2->getVel(velB);
+
+    velB[0] -= rmb * dx;
+    velB[1] -= rmb * dy;
+    velB[2] -= rmb * dz;
+
+    consAtom2->setVel(velB);
+
+    return consSuccess;
+  }
+  else
+    return consAlready;
 }
 
 
@@ -39,11 +145,75 @@ int DCShakeMinFFunctor::operator()(ConstraintAtom* con
 //Implementation of DCShakeMinFFunctor
 ////////////////////////////////////////////////////////////////////////////////
 int DCShakeMinFFunctor::operator()(ConstraintAtom* consAtom1, ConstraintAtom* consAtom2){
-  return consElemHandlerFail;
-}
 
+  double posA[3];
+  double posB[3];
+  double frcA[3];
+  double frcB[3];
+  double rab[3];
+  double fpab[3];
+  double rma;
+  double rmb;
+  double gab;
+  double rabsq;
+  double rfab;
 
 
+
+  consAtom1->getPos(posA);
+  consAtom2->getPos(posB);
+
+  
+  rab[0] = posA[0] - posB[0];
+  rab[1] = posA[1] - posB[1];
+  rab[2] = posA[2] - posB[2];
+
+  info->wrapVector(rab);
+
+  consAtom1->getFrc(frcA);
+  consAtom2->getFrc(frcB);
+
+  rma = 1.0;
+  rmb = 1.0;
+
+
+  fpab[0] = frcA[0] * rma - frcB[0] * rmb;
+  fpab[1] = frcA[1] * rma - frcB[1] * rmb;
+  fpab[2] = frcA[2] * rma - frcB[2] * rmb;
+
+
+  gab=fpab[0] * fpab[0] + fpab[1] * fpab[1] + fpab[2] * fpab[2];
+
+  if (gab < 1.0)
+    gab = 1.0;
+
+  rabsq = rab[0] * rab[0] + rab[1] * rab[1] + rab[2] * rab[2];
+  rfab = rab[0] * fpab[0] + rab[1] * fpab[1] + rab[2] * fpab[2];
+
+  if (fabs(rfab) > sqrt(rabsq*gab) * consTolerance){
+
+  gab = -rfab /(rabsq*(rma + rmb));
+
+    frcA[0] = rab[0] * gab;
+    frcA[1] = rab[1] * gab;
+    frcA[2] = rab[2] * gab;
+
+    consAtom1->addFrc(frcA);
+
+    frcB[0] = -rab[0] * gab;
+    frcB[1] = -rab[1] * gab;
+    frcB[2] = -rab[2] * gab;
+
+    consAtom2->addFrc(frcB);
+
+  }             
+
+   return consSuccess;
+    
+ }
+
+
+
 int DCShakeMinFFunctor::operator()(ConstraintAtom* consAtom,ConstraintRigidBody* consRB){
   return consElemHandlerFail;
 }
@@ -67,4 +237,4 @@ int JCShakeMinFFunctor::operator()(ConstraintRigidBody
  
 int JCShakeMinFFunctor::operator()(ConstraintRigidBody* consRB1, ConstraintRigidBody* consRB2){
   return consElemHandlerFail;
-}
\ No newline at end of file
+}