--- trunk/OOPSE/libmdtools/Roll.cpp 2004/06/09 16:59:03 1255 +++ trunk/OOPSE/libmdtools/Roll.cpp 2004/06/21 18:52:21 1284 @@ -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,26 @@ 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 +56,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 +74,171 @@ 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); + + } + 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 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,26 +252,26 @@ 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); @@ -119,14 +280,14 @@ void DCRollAFunctor::getEffInvMassVec(ConstraintRigidB aTrans = a.transpose(); invIBody = IBody.inverse(); - IFrame = aTrans * invIBody * a; + invILab = aTrans * invIBody * a; refCrossBond = crossProduct(refCoor, bondDir); - tempVec1 = invIFrame * refCrossBond; + tempVec1 = invILab * refCrossBond; tempVec2 = crossProduct(tempVec1, refCoor); - invMassVec += tempVec2; + zeta += tempVec2; } @@ -136,13 +297,20 @@ void DCRollAFunctor::integrate(ConstraintRigidBody* co Vector3d pos; Vector3d Tb; Vector3d ji; + Vector3d tempPos; + Vector3d tempVel; + Vector3d tempTrqLab; + Vector3d tempTrqBody; + Vector3d tempJi; + Vector3d refCoor; double mass; - double dtOver2; + Mat3x3d oldA; double dt; - const double eConvert = 4.184e-4; - + double dtOver2; dt = info->dt; - dtOver2 = dt /2; + dtOver2 = dt /2; + + consRB->getOldA(oldA.element); sd = consRB->getStuntDouble(); sd->getVel(vel.vec); @@ -150,29 +318,35 @@ void DCRollAFunctor::integrate(ConstraintRigidBody* co mass = sd->getMass(); - vel += eConvert * dtOver2/mass * force; - pos += dt * vel; + tempVel = dtOver2/mass * force; + tempPos = dt * tempVel; + + vel += tempVel; + pos += tempPos; sd->setVel(vel.vec); sd->setPos(pos.vec); if (sd->isDirectional()){ - // get and convert the torque to body frame + consRB->getRefCoor(refCoor.vec); + tempTrqLab = crossProduct(refCoor, force); - sd->getTrq(Tb.vec); - sd->lab2Body(Tb.vec); + //convert torque in lab frame to torque in body frame using old rotation matrix + //tempTrqBody = oldA * tempTrqLab; + + //tempJi = dtOver2 * tempTrqBody; + sd->lab2Body(tempTrqLab.vec); + tempJi = dtOver2 * tempTrqLab; + rotationPropagation( sd, tempJi.vec); - // get the angular momentum, and propagate a half step - sd->getJ(ji.vec); - ji += eConvert * dtOver2 * Tb; + ji += tempJi; - rotationPropagation( sd, ji.vec); - sd->setJ(ji.vec); } + } @@ -320,58 +494,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 = 100; 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 = pvab / (dt * pabDotZeta) * bondDirUnitVec; //integrate consRB1 using constraint force; - integrate(consRB1,consForce); + integrate(consRB1, consForce); //integrate consRB2 using constraint force; integrate(consRB2, -consForce); @@ -385,26 +550,27 @@ 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); @@ -413,22 +579,22 @@ void DCRollBFunctor::getEffInvMassVec(ConstraintRigidB aTrans = a.transpose(); invIBody = IBody.inverse(); - IFrame = aTrans * invIBody * a; + invILab = aTrans * invIBody * a; refCrossBond = crossProduct(refCoor, bondDir); - tempVec1 = invIFrame * refCrossBond; + tempVec1 = invILab * refCrossBond; tempVec2 = crossProduct(tempVec1, refCoor); - invMassVec += tempVec2; + zeta += 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; StuntDouble* sd; @@ -436,27 +602,17 @@ void DCRollBFunctor::integrate(ConstraintRigidBody* co 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()){ - - // 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 - + tempTrq = crossProduct(refCoor, force); + sd->lab2Body(tempTrq.vec); + tempJi = dtOver2* tempTrq; sd->getJ(ji.vec); - - ji += eConvert * dtOver2* Tb; - + ji += tempJi; sd->setJ(ji.vec); }