--- trunk/OOPSE/libmdtools/Roll.cpp 2004/08/09 14:50:35 1451 +++ 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); @@ -103,17 +101,14 @@ int DCRollAFunctor::operator()(ConstraintAtom* consAto //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){ @@ -274,7 +269,8 @@ 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(); @@ -282,71 +278,85 @@ void DCRollAFunctor::calcZeta(ConstraintRigidBody* con invILab = aTrans * invIBody * a; - refCrossBond = crossProduct(refCoor, bondDir); + refCrossBond = crossProduct(aTrans *refCoor, bondDir); tempVec1 = invILab * refCrossBond; - tempVec2 = crossProduct(tempVec1, refCoor); + 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 tempTrqLab; - Vector3d tempTrqBody; - Vector3d tempJi; Vector3d refCoor; + Vector3d consTorque; + Vector3d totConsTorque; + Mat3x3d a; double mass; - Mat3x3d oldA; double dt; double dtOver2; + const double eConvert = 4.184e-4; + dt = info->dt; dtOver2 = dt /2; - consRB->getOldA(oldA.element); - sd = consRB->getStuntDouble(); + //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 = 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()){ - - consRB->getRefCoor(refCoor.vec); - tempTrqLab = crossProduct(refCoor, force); + //evolve orientational part + consRB->getRefCoor(refCoor.vec); + rb->getA(a.element); - //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); + //calculate constraint torque in lab frame + consTorque = crossProduct(a.transpose() * refCoor, consForce); + consRB->addConsTorque(consTorque/eConvert); - sd->getJ(ji.vec); + //add constraint torque + totConsTorque = consRB->getConsTorque(); + rb->addTrq(totConsTorque.vec); + + //get and convert the torque to body frame - ji += tempJi; + rb->getTrq(Tb.vec); + rb->lab2Body(Tb.vec); - sd->setJ(ji.vec); - } + //get the angular momentum, and propagate a half step + rb->getJ(ji.vec); + + ji += eConvert * dtOver2 * Tb; + + rotationPropagation( rb, ji.vec ); + + rb->setJ(ji.vec); } @@ -355,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 @@ -371,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]; @@ -387,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 @@ -504,7 +514,7 @@ int DCRollBFunctor::operator()(ConstraintRigidBody* co double pabDotZeta; double pvab; - const int conRBMaxIter = 100; + const int conRBMaxIter = 20; dt = info->dt; @@ -534,7 +544,7 @@ int DCRollBFunctor::operator()(ConstraintRigidBody* co pabDotZeta = dotProduct(pab, zeta); - consForce = pvab / (dt * pabDotZeta) * bondDirUnitVec; + consForce = 2 * pvab / (dt * pabDotZeta) * bondDirUnitVec; //integrate consRB1 using constraint force; integrate(consRB1, consForce); @@ -566,7 +576,6 @@ void DCRollBFunctor::getZeta(ConstraintRigidBody* cons Mat3x3d invIBody; Mat3x3d invILab; Mat3x3d a; - Mat3x3d aTrans; invMass = 1.0 / consRB ->getMass(); @@ -576,17 +585,17 @@ void DCRollBFunctor::getZeta(ConstraintRigidBody* cons consRB->getA(a.element); consRB->getI(IBody.element); - aTrans = a.transpose(); invIBody = IBody.inverse(); - invILab = aTrans * invIBody * a; + + refCrossBond = crossProduct(refCoor, a * bondDir); - refCrossBond = crossProduct(refCoor, bondDir); + tempVec1 = invIBody * refCrossBond; - tempVec1 = invILab * refCrossBond; - tempVec2 = crossProduct(tempVec1, refCoor); - + tempVec2 = (a * tempVec1.makeSkewMat()).transpose() * refCoor; + zeta += tempVec2; + } void DCRollBFunctor::integrate(ConstraintRigidBody* consRB, const Vector3d& force){ @@ -597,6 +606,7 @@ void DCRollBFunctor::integrate(ConstraintRigidBody* co Vector3d refCoor; double mass; double dtOver2; + Mat3x3d a; StuntDouble* sd; sd = consRB->getStuntDouble(); @@ -608,8 +618,10 @@ void DCRollBFunctor::integrate(ConstraintRigidBody* co sd->setVel(vel.vec); if (sd->isDirectional()){ - tempTrq = crossProduct(refCoor, force); - sd->lab2Body(tempTrq.vec); + sd->getA(a.element); + consRB->getRefCoor(refCoor.vec); + tempTrq = crossProduct(refCoor, a *force); + tempJi = dtOver2* tempTrq; sd->getJ(ji.vec); ji += tempJi;