--- trunk/OOPSE/libmdtools/Roll.cpp 2004/06/11 16:46:22 1267 +++ trunk/OOPSE/libmdtools/Roll.cpp 2004/06/11 17:16:21 1268 @@ -7,7 +7,7 @@ int DCRollAFunctor::operator()(ConstraintRigidBody* co //////////////////////////////////////////////////////////////////////////////// //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,9 +28,158 @@ 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; + + consAtom1->getOldPos(oldPosA.vec); + consAtom2->getOldPos(oldPosB.vec); + + + for(int i=0 ; i < conRBMaxIter; i++){ + consAtom1->getPos(posA.vec); + consAtom2->getPos(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); + + //rpab = dotProduct(rab, pab); + + //rpabsq = rpab * rpab; + + + //if (rpabsq < (rabsq * -diffsq)){ + // return consFail; + //} + + bondDirUnitVec = pab; + bondDirUnitVec.normalize(); + + calcZeta(consAtom1, bondDirUnitVec, zetaA); + + calcZeta(consAtom2, 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) + return consFail; + + //forceScalar = (pabDotZeta + sqrt(pabDotZeta2 - zeta2 * diffsq)) / dt * dt * zeta2; + forceScalar = diffsq / (2 * dt * dt * pabDotZeta); + // + consForce = forceScalar * bondDirUnitVec; + //integrate consRB1 using constraint force; + integrate(consAtom1, consForce); + + //integrate consRB2 using constraint force; + integrate(consAtom2, -consForce); + + } + else{ + if (i ==0) + return consAlready; + else + return consSuccess; + } + } + + return consExceedMaxIter; + +} +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 dtOver2; + double dt; + const double eConvert = 4.184e-4; + + dt = info->dt; + dtOver2 = dt /2; + sd = consAtom->getStuntDouble(); + + sd->getVel(vel.vec); + sd->getPos(pos.vec); + + mass = sd->getMass(); + tempVel = eConvert * dtOver2/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 = 10; dt = info->dt; @@ -42,7 +192,9 @@ int DCRollAFunctor::operator()(ConstraintRigidBody* co consRB1->getCurAtomPos(posA.vec); consRB2->getCurAtomPos(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 +203,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,15 +221,32 @@ int DCRollAFunctor::operator()(ConstraintRigidBody* co bondDirUnitVec = pab; bondDirUnitVec.normalize(); - getEffInvMassVec(consRB1, bondDirUnitVec, rma); + calcZeta(consRB1, bondDirUnitVec, zetaA); - getEffInvMassVec(consRB2, -bondDirUnitVec, rmb); + calcZeta(consRB2, 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); + // + consForce = forceScalar * bondDirUnitVec; //integrate consRB1 using constraint force; - integrate(consRB1,consForce); + integrate(consRB1, consForce); //integrate consRB2 using constraint force; integrate(consRB2, -consForce); @@ -95,7 +264,7 @@ void DCRollAFunctor::getEffInvMassVec(ConstraintRigidB } -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; @@ -110,7 +279,7 @@ void DCRollAFunctor::getEffInvMassVec(ConstraintRigidB invMass = 1.0 / consRB ->getMass(); - invMassVec = invMass * bondDir; + zeta = invMass * bondDir; consRB->getRefCoor(refCoor.vec); consRB->getA(a.element); @@ -126,7 +295,7 @@ void DCRollAFunctor::getEffInvMassVec(ConstraintRigidB tempVec1 = invIFrame * refCrossBond; tempVec2 = crossProduct(tempVec1, refCoor); - invMassVec += tempVec2; + zeta += tempVec2; } @@ -136,6 +305,10 @@ void DCRollAFunctor::integrate(ConstraintRigidBody* co Vector3d pos; Vector3d Tb; Vector3d ji; + Vector3d tempPos; + Vector3d tempVel; + Vector3d tempTrq; + Vector3d tempJi; double mass; double dtOver2; double dt; @@ -150,8 +323,11 @@ void DCRollAFunctor::integrate(ConstraintRigidBody* co mass = sd->getMass(); - vel += eConvert * dtOver2/mass * force; - pos += dt * vel; + tempVel = eConvert * dtOver2/mass * force; + tempPos = dt * tempVel; + + vel += tempVel; + pos += tempPos; sd->setVel(vel.vec); sd->setPos(pos.vec); @@ -364,7 +540,7 @@ int DCRollBFunctor::operator()(ConstraintRigidBody* co getEffInvMassVec(consRB1, bondDirUnitVec, rma); - getEffInvMassVec(consRB2, -bondDirUnitVec, rmb); + getEffInvMassVec(consRB2, bondDirUnitVec, rmb); pabcDotvab = dotProduct(pab, vab); pabDotInvMassVec = dotProduct(pab, rma + rmb);