--- trunk/src/primitives/RigidBody.cpp 2005/10/03 14:31:31 641 +++ trunk/src/primitives/RigidBody.cpp 2009/09/07 16:31:51 1360 @@ -44,111 +44,110 @@ namespace oopse { #include "utils/simError.h" #include "utils/NumericConstant.hpp" namespace oopse { - - RigidBody::RigidBody() : StuntDouble(otRigidBody, &Snapshot::rigidbodyData), inertiaTensor_(0.0){ - + + RigidBody::RigidBody() : StuntDouble(otRigidBody, &Snapshot::rigidbodyData), + inertiaTensor_(0.0){ } - + void RigidBody::setPrevA(const RotMat3x3d& a) { ((snapshotMan_->getPrevSnapshot())->*storage_).aMat[localIndex_] = a; - //((snapshotMan_->getPrevSnapshot())->*storage_).electroFrame[localIndex_] = a.transpose() * sU_; - + for (int i =0 ; i < atoms_.size(); ++i){ if (atoms_[i]->isDirectional()) { - atoms_[i]->setPrevA(a * refOrients_[i]); + atoms_[i]->setPrevA(refOrients_[i].transpose() * a); } } - + } - - + + void RigidBody::setA(const RotMat3x3d& a) { ((snapshotMan_->getCurrentSnapshot())->*storage_).aMat[localIndex_] = a; - //((snapshotMan_->getCurrentSnapshot())->*storage_).electroFrame[localIndex_] = a.transpose() * sU_; for (int i =0 ; i < atoms_.size(); ++i){ if (atoms_[i]->isDirectional()) { - atoms_[i]->setA(a * refOrients_[i]); + atoms_[i]->setA(refOrients_[i].transpose() * a); } } } - + void RigidBody::setA(const RotMat3x3d& a, int snapshotNo) { ((snapshotMan_->getSnapshot(snapshotNo))->*storage_).aMat[localIndex_] = a; + //((snapshotMan_->getSnapshot(snapshotNo))->*storage_).electroFrame[localIndex_] = a.transpose() * sU_; - + for (int i =0 ; i < atoms_.size(); ++i){ if (atoms_[i]->isDirectional()) { - atoms_[i]->setA(a * refOrients_[i], snapshotNo); + atoms_[i]->setA(refOrients_[i].transpose() * a, snapshotNo); } } - + } - + Mat3x3d RigidBody::getI() { return inertiaTensor_; } - - std::vector RigidBody::getGrad() { - std::vector grad(6, 0.0); + + std::vector RigidBody::getGrad() { + std::vector grad(6, 0.0); Vector3d force; Vector3d torque; Vector3d myEuler; - double phi, theta, psi; - double cphi, sphi, ctheta, stheta; + RealType phi, theta, psi; + RealType cphi, sphi, ctheta, stheta; Vector3d ephi; Vector3d etheta; Vector3d epsi; - + force = getFrc(); torque =getTrq(); myEuler = getA().toEulerAngles(); - + phi = myEuler[0]; theta = myEuler[1]; psi = myEuler[2]; - + cphi = cos(phi); sphi = sin(phi); ctheta = cos(theta); stheta = sin(theta); - + // get unit vectors along the phi, theta and psi rotation axes - + ephi[0] = 0.0; ephi[1] = 0.0; ephi[2] = 1.0; - - etheta[0] = cphi; - etheta[1] = sphi; - etheta[2] = 0.0; - + + etheta[0] = -sphi; + etheta[1] = cphi; + etheta[2] = 0.0; + epsi[0] = stheta * cphi; epsi[1] = stheta * sphi; epsi[2] = ctheta; - + //gradient is equal to -force for (int j = 0 ; j<3; j++) grad[j] = -force[j]; - + for (int j = 0; j < 3; j++ ) { - + grad[3] += torque[j]*ephi[j]; grad[4] += torque[j]*etheta[j]; grad[5] += torque[j]*epsi[j]; - + } return grad; } - + void RigidBody::accept(BaseVisitor* v) { v->visit(this); } /**@todo need modification */ void RigidBody::calcRefCoords() { - double mtmp; + RealType mtmp; Vector3d refCOM(0.0); mass_ = 0.0; for (std::size_t i = 0; i < atoms_.size(); ++i) { @@ -157,31 +156,32 @@ namespace oopse { refCOM += refCoords_[i]*mtmp; } refCOM /= mass_; - + // Next, move the origin of the reference coordinate system to the COM: for (std::size_t i = 0; i < atoms_.size(); ++i) { refCoords_[i] -= refCOM; } // Moment of Inertia calculation - Mat3x3d Itmp(0.0); - + Mat3x3d Itmp(0.0); for (std::size_t i = 0; i < atoms_.size(); i++) { + Mat3x3d IAtom(0.0); mtmp = atoms_[i]->getMass(); - Itmp -= outProduct(refCoords_[i], refCoords_[i]) * mtmp; - double r2 = refCoords_[i].lengthSquare(); - Itmp(0, 0) += mtmp * r2; - Itmp(1, 1) += mtmp * r2; - Itmp(2, 2) += mtmp * r2; - } - - //project the inertial moment of directional atoms into this rigid body - for (std::size_t i = 0; i < atoms_.size(); i++) { + IAtom -= outProduct(refCoords_[i], refCoords_[i]) * mtmp; + RealType r2 = refCoords_[i].lengthSquare(); + IAtom(0, 0) += mtmp * r2; + IAtom(1, 1) += mtmp * r2; + IAtom(2, 2) += mtmp * r2; + Itmp += IAtom; + + //project the inertial moment of directional atoms into this rigid body if (atoms_[i]->isDirectional()) { - Itmp += refOrients_[i].transpose() * atoms_[i]->getI() * refOrients_[i]; - } + Itmp += refOrients_[i].transpose() * atoms_[i]->getI() * refOrients_[i]; + } } + // std::cout << Itmp << std::endl; + //diagonalize Vector3d evals; Mat3x3d::diagonalize(Itmp, evals, sU_); @@ -222,7 +222,7 @@ namespace oopse { Vector3d apos; Vector3d rpos; Vector3d frc(0.0); - Vector3d trq(0.0); + Vector3d trq(0.0); Vector3d pos = this->getPos(); for (int i = 0; i < atoms_.size(); i++) { @@ -242,13 +242,57 @@ namespace oopse { if (atoms_[i]->isDirectional()) { atrq = atoms_[i]->getTrq(); trq += atrq; - } + } + } + addFrc(frc); + addTrq(trq); + } + + Mat3x3d RigidBody::calcForcesAndTorquesAndVirial() { + Vector3d afrc; + Vector3d atrq; + Vector3d apos; + Vector3d rpos; + Vector3d dfrc; + Vector3d frc(0.0); + Vector3d trq(0.0); + Vector3d pos = this->getPos(); + Mat3x3d tau_(0.0); + + for (int i = 0; i < atoms_.size(); i++) { + + afrc = atoms_[i]->getFrc(); + apos = atoms_[i]->getPos(); + rpos = apos - pos; + frc += afrc; + + trq[0] += rpos[1]*afrc[2] - rpos[2]*afrc[1]; + trq[1] += rpos[2]*afrc[0] - rpos[0]*afrc[2]; + trq[2] += rpos[0]*afrc[1] - rpos[1]*afrc[0]; + + // If the atom has a torque associated with it, then we also need to + // migrate the torques onto the center of mass: + + if (atoms_[i]->isDirectional()) { + atrq = atoms_[i]->getTrq(); + trq += atrq; + } + + tau_(0,0) -= rpos[0]*afrc[0]; + tau_(0,1) -= rpos[0]*afrc[1]; + tau_(0,2) -= rpos[0]*afrc[2]; + tau_(1,0) -= rpos[1]*afrc[0]; + tau_(1,1) -= rpos[1]*afrc[1]; + tau_(1,2) -= rpos[1]*afrc[2]; + tau_(2,0) -= rpos[2]*afrc[0]; + tau_(2,1) -= rpos[2]*afrc[1]; + tau_(2,2) -= rpos[2]*afrc[2]; + } - - setFrc(frc); - setTrq(trq); - + addFrc(frc); + addTrq(trq); + return tau_; } void RigidBody::updateAtoms() { @@ -270,7 +314,7 @@ namespace oopse { if (atoms_[i]->isDirectional()) { dAtom = (DirectionalAtom *) atoms_[i]; - dAtom->setA(refOrients_[i] * a); + dAtom->setA(refOrients_[i].transpose() * a); } } @@ -297,7 +341,7 @@ namespace oopse { if (atoms_[i]->isDirectional()) { dAtom = (DirectionalAtom *) atoms_[i]; - dAtom->setA(refOrients_[i] * a, frame); + dAtom->setA(refOrients_[i].transpose() * a, frame); } } @@ -482,7 +526,7 @@ namespace oopse { "RigidBody error.\n" "\tAtom %s does not have a position specified.\n" "\tThis means RigidBody cannot set up reference coordinates.\n", - ats->getType() ); + ats->getType().c_str() ); painCave.isFatal = 1; simError(); } @@ -502,7 +546,7 @@ namespace oopse { "RigidBody error.\n" "\tAtom %s does not have an orientation specified.\n" "\tThis means RigidBody cannot set up reference orientations.\n", - ats->getType() ); + ats->getType().c_str() ); painCave.isFatal = 1; simError(); }