--- trunk/OOPSE/libmdtools/Roll.cpp 2004/06/11 17:16:21 1268 +++ trunk/OOPSE/libmdtools/Roll.cpp 2004/08/23 15:11:36 1452 @@ -32,16 +32,14 @@ int DCRollAFunctor::operator()(ConstraintAtom* consAto 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); @@ -91,30 +89,27 @@ int DCRollAFunctor::operator()(ConstraintAtom* consAto //G : constraint force scalar //d: equilibrium bond length - if (pabDotZeta2 - zeta2 * diffsq < 0) + 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(consAtom1, consForce); //integrate consRB2 using constraint force; integrate(consAtom2, -consForce); - + + return consSuccess; } - else{ - if (i ==0) + else return consAlready; - else - return consSuccess; - } - } - return consExceedMaxIter; + + } void DCRollAFunctor::calcZeta(ConstraintAtom* consAtom, const Vector3d& bondDir, Vector3d&zeta){ double invMass; @@ -129,14 +124,10 @@ void DCRollAFunctor::integrate(ConstraintAtom* consAto 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); @@ -144,7 +135,7 @@ void DCRollAFunctor::integrate(ConstraintAtom* consAto mass = sd->getMass(); - tempVel = eConvert * dtOver2/mass * force; + tempVel = dt/mass * force; tempPos = dt * tempVel; vel += tempVel; @@ -180,7 +171,7 @@ int DCRollAFunctor::operator()(ConstraintRigidBody* co double zeta2; double forceScalar; - const int conRBMaxIter = 10; + const int conRBMaxIter = 100; dt = info->dt; @@ -209,16 +200,7 @@ int DCRollAFunctor::operator()(ConstraintRigidBody* co rab = oldPosB - oldPosA; info->wrapVector(rab.vec); - //rpab = dotProduct(rab, pab); - - //rpabsq = rpab * rpab; - - - //if (rpabsq < (rabsq * -diffsq)){ - // return consFail; - //} - - bondDirUnitVec = pab; + bondDirUnitVec = rab; bondDirUnitVec.normalize(); calcZeta(consRB1, bondDirUnitVec, zetaA); @@ -238,11 +220,16 @@ int DCRollAFunctor::operator()(ConstraintRigidBody* co //G : constraint force scalar //d: equilibrium bond length - if (pabDotZeta2 - zeta2 * diffsq < 0) + if (pabDotZeta2 - zeta2 * diffsq < 0){ + cerr << "DCRollAFunctor::operator() Error: Constraint Fail at " << info->getTime() << endl; return consFail; - - //forceScalar = (pabDotZeta + sqrt(pabDotZeta2 - zeta2 * diffsq)) / dt * dt * zeta2; - forceScalar = diffsq / (2 * dt * dt * pabDotZeta); + } + //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; @@ -260,6 +247,7 @@ int DCRollAFunctor::operator()(ConstraintRigidBody* co } } + cerr << "DCRollAFunctor::operator() Error: can not constrain the bond within maximum iteration at " << info->getTime() << endl; return consExceedMaxIter; } @@ -271,9 +259,8 @@ void DCRollAFunctor::calcZeta(ConstraintRigidBody* con Vector3d refCoor; Vector3d refCrossBond; Mat3x3d IBody; - Mat3x3d IFrame; Mat3x3d invIBody; - Mat3x3d invIFrame; + Mat3x3d invILab; Mat3x3d a; Mat3x3d aTrans; @@ -282,73 +269,94 @@ void DCRollAFunctor::calcZeta(ConstraintRigidBody* con zeta = invMass * bondDir; consRB->getRefCoor(refCoor.vec); - consRB->getA(a.element); + //consRB->getA(a.element); + consRB->getOldA(a.element); consRB->getI(IBody.element); aTrans = a.transpose(); invIBody = IBody.inverse(); - IFrame = aTrans * invIBody * a; + invILab = aTrans * invIBody * a; - refCrossBond = crossProduct(refCoor, bondDir); + refCrossBond = crossProduct(aTrans *refCoor, bondDir); - tempVec1 = invIFrame * refCrossBond; - tempVec2 = crossProduct(tempVec1, refCoor); + tempVec1 = invILab * refCrossBond; + tempVec2 = crossProduct(tempVec1, aTrans *refCoor); zeta += tempVec2; } -void DCRollAFunctor::integrate(ConstraintRigidBody* consRB, const Vector3d& force){ - StuntDouble* sd; +void DCRollAFunctor::integrate(ConstraintRigidBody* consRB, const Vector3d& consForce){ + RigidBody* rb; + Vector3d frc; + Vector3d totConsForce; Vector3d vel; Vector3d pos; Vector3d Tb; Vector3d ji; - Vector3d tempPos; - Vector3d tempVel; - Vector3d tempTrq; - Vector3d tempJi; + Vector3d refCoor; + Vector3d consTorque; + Vector3d totConsTorque; + Mat3x3d a; double mass; - double dtOver2; double dt; + double dtOver2; const double eConvert = 4.184e-4; dt = info->dt; - dtOver2 = dt /2; - sd = consRB->getStuntDouble(); + dtOver2 = dt /2; + + //restore to old status + consRB->restoreUnconsStatus(); + + //accumulate constraint force; + consRB->addConsForce(consForce/eConvert); + totConsForce = consRB->getConsForce(); + + rb = consRB->getRigidBody(); - sd->getVel(vel.vec); - sd->getPos(pos.vec); + rb->addFrc(totConsForce.vec); - mass = sd->getMass(); + rb->getVel(vel.vec); + rb->getPos(pos.vec); + rb->getFrc(frc.vec); + mass = rb->getMass(); - tempVel = eConvert * dtOver2/mass * force; - tempPos = dt * tempVel; - - vel += tempVel; - pos += tempPos; + // velocity half step + vel += eConvert * dtOver2 / mass * frc; + // position whole step + pos += dt * vel; - sd->setVel(vel.vec); - sd->setPos(pos.vec); + rb->setVel(vel.vec); + rb->setPos(pos.vec); - if (sd->isDirectional()){ + //evolve orientational part + consRB->getRefCoor(refCoor.vec); + rb->getA(a.element); - // get and convert the torque to body frame + //calculate constraint torque in lab frame + consTorque = crossProduct(a.transpose() * refCoor, consForce); + consRB->addConsTorque(consTorque/eConvert); - sd->getTrq(Tb.vec); - sd->lab2Body(Tb.vec); + //add constraint torque + totConsTorque = consRB->getConsTorque(); + rb->addTrq(totConsTorque.vec); + + //get and convert the torque to body frame - // get the angular momentum, and propagate a half step + rb->getTrq(Tb.vec); + rb->lab2Body(Tb.vec); - sd->getJ(ji.vec); + //get the angular momentum, and propagate a half step - ji += eConvert * dtOver2 * Tb; + rb->getJ(ji.vec); + + ji += eConvert * dtOver2 * Tb; - rotationPropagation( sd, ji.vec); + rotationPropagation( rb, ji.vec ); - sd->setJ(ji.vec); - } + rb->setJ(ji.vec); } @@ -357,8 +365,9 @@ void DCRollAFunctor::rotationPropagation(StuntDouble* double A[3][3], I[3][3]; int i, j, k; double dtOver2; - - dtOver2 = info->dt /2; + double dt; + dt = info->dt; + dtOver2 = dt /2; // use the angular velocities to propagate the rotation matrix a // full time step @@ -373,7 +382,7 @@ void DCRollAFunctor::rotationPropagation(StuntDouble* angle = dtOver2 * ji[j] / I[j][j]; this->rotate( k, i, angle, ji, A ); - angle = dtOver2 * ji[k] / I[k][k]; + angle = dt* ji[k] / I[k][k]; this->rotate( i, j, angle, ji, A); angle = dtOver2 * ji[j] / I[j][j]; @@ -389,8 +398,7 @@ void DCRollAFunctor::rotationPropagation(StuntDouble* this->rotate( 2, 0, angle, ji, A ); // rotate about the z-axis - angle = dtOver2 * ji[2] / I[2][2]; - sd->addZangle(angle); + angle = dt * ji[2] / I[2][2]; this->rotate( 0, 1, angle, ji, A); // rotate about the y-axis @@ -496,58 +504,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 = 20; 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 = 2 * pvab / (dt * pabDotZeta) * bondDirUnitVec; //integrate consRB1 using constraint force; - integrate(consRB1,consForce); + integrate(consRB1, consForce); //integrate consRB2 using constraint force; integrate(consRB2, -consForce); @@ -561,78 +560,71 @@ 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); consRB->getI(IBody.element); - aTrans = a.transpose(); invIBody = IBody.inverse(); - IFrame = aTrans * invIBody * a; + + refCrossBond = crossProduct(refCoor, a * bondDir); - refCrossBond = crossProduct(refCoor, bondDir); + tempVec1 = invIBody * refCrossBond; - tempVec1 = invIFrame * refCrossBond; - tempVec2 = crossProduct(tempVec1, refCoor); + tempVec2 = (a * tempVec1.makeSkewMat()).transpose() * refCoor; + + zeta += tempVec2; - invMassVec += 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; + Mat3x3d a; StuntDouble* sd; 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()){ + sd->getA(a.element); + consRB->getRefCoor(refCoor.vec); + tempTrq = crossProduct(refCoor, a *force); - // 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 - + tempJi = dtOver2* tempTrq; sd->getJ(ji.vec); - - ji += eConvert * dtOver2* Tb; - + ji += tempJi; sd->setJ(ji.vec); }