--- trunk/OOPSE-4/src/primitives/RigidBody.cpp 2004/09/24 16:27:58 1492 +++ trunk/OOPSE-4/src/primitives/RigidBody.cpp 2005/01/12 22:41:40 1930 @@ -1,711 +1,431 @@ -#include + /* + * Copyright (c) 2005 The University of Notre Dame. All Rights Reserved. + * + * The University of Notre Dame grants you ("Licensee") a + * non-exclusive, royalty free, license to use, modify and + * redistribute this software in source and binary code form, provided + * that the following conditions are met: + * + * 1. Acknowledgement of the program authors must be made in any + * publication of scientific results based in part on use of the + * program. An acceptable form of acknowledgement is citation of + * the article in which the program was described (Matthew + * A. Meineke, Charles F. Vardeman II, Teng Lin, Christopher + * J. Fennell and J. Daniel Gezelter, "OOPSE: An Object-Oriented + * Parallel Simulation Engine for Molecular Dynamics," + * J. Comput. Chem. 26, pp. 252-271 (2005)) + * + * 2. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * + * 3. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the + * distribution. + * + * This software is provided "AS IS," without a warranty of any + * kind. All express or implied conditions, representations and + * warranties, including any implied warranty of merchantability, + * fitness for a particular purpose or non-infringement, are hereby + * excluded. The University of Notre Dame and its licensors shall not + * be liable for any damages suffered by licensee as a result of + * using, modifying or distributing the software or its + * derivatives. In no event will the University of Notre Dame or its + * licensors be liable for any lost revenue, profit or data, or for + * direct, indirect, special, consequential, incidental or punitive + * damages, however caused and regardless of the theory of liability, + * arising out of the use of or inability to use software, even if the + * University of Notre Dame has been advised of the possibility of + * such damages. + */ +#include #include "primitives/RigidBody.hpp" -#include "primitives/DirectionalAtom.hpp" #include "utils/simError.h" -#include "math/MatVec3.h" +namespace oopse { -RigidBody::RigidBody() : StuntDouble() { - objType = OT_RIGIDBODY; - is_linear = false; - linear_axis = -1; - momIntTol = 1e-6; -} +RigidBody::RigidBody() : StuntDouble(otRigidBody, &Snapshot::rigidbodyData), inertiaTensor_(0.0){ -RigidBody::~RigidBody() { } -void RigidBody::addAtom(Atom* at, AtomStamp* ats) { +void RigidBody::setPrevA(const RotMat3x3d& a) { + ((snapshotMan_->getPrevSnapshot())->*storage_).aMat[localIndex_] = a; + //((snapshotMan_->getPrevSnapshot())->*storage_).electroFrame[localIndex_] = a.transpose() * sU_; - vec3 coords; - vec3 euler; - mat3x3 Atmp; + for (int i =0 ; i < atoms_.size(); ++i){ + if (atoms_[i]->isDirectional()) { + atoms_[i]->setPrevA(a * refOrients_[i]); + } + } - myAtoms.push_back(at); - - if( !ats->havePosition() ){ - sprintf( painCave.errMsg, - "RigidBody error.\n" - "\tAtom %s does not have a position specified.\n" - "\tThis means RigidBody cannot set up reference coordinates.\n", - ats->getType() ); - painCave.isFatal = 1; - simError(); - } - - coords[0] = ats->getPosX(); - coords[1] = ats->getPosY(); - coords[2] = ats->getPosZ(); +} - refCoords.push_back(coords); - - if (at->isDirectional()) { + +void RigidBody::setA(const RotMat3x3d& a) { + ((snapshotMan_->getCurrentSnapshot())->*storage_).aMat[localIndex_] = a; + //((snapshotMan_->getCurrentSnapshot())->*storage_).electroFrame[localIndex_] = a.transpose() * sU_; - if( !ats->haveOrientation() ){ - sprintf( painCave.errMsg, - "RigidBody error.\n" - "\tAtom %s does not have an orientation specified.\n" - "\tThis means RigidBody cannot set up reference orientations.\n", - ats->getType() ); - painCave.isFatal = 1; - simError(); - } + for (int i =0 ; i < atoms_.size(); ++i){ + if (atoms_[i]->isDirectional()) { + atoms_[i]->setA(a * refOrients_[i]); + } + } +} - euler[0] = ats->getEulerPhi(); - euler[1] = ats->getEulerTheta(); - euler[2] = ats->getEulerPsi(); - - doEulerToRotMat(euler, Atmp); - - refOrients.push_back(Atmp); - - } -} +void RigidBody::setA(const RotMat3x3d& a, int snapshotNo) { + ((snapshotMan_->getSnapshot(snapshotNo))->*storage_).aMat[localIndex_] = a; + //((snapshotMan_->getSnapshot(snapshotNo))->*storage_).electroFrame[localIndex_] = a.transpose() * sU_; -void RigidBody::getPos(double theP[3]){ - for (int i = 0; i < 3 ; i++) - theP[i] = pos[i]; -} + for (int i =0 ; i < atoms_.size(); ++i){ + if (atoms_[i]->isDirectional()) { + atoms_[i]->setA(a * refOrients_[i], snapshotNo); + } + } -void RigidBody::setPos(double theP[3]){ - for (int i = 0; i < 3 ; i++) - pos[i] = theP[i]; -} +} -void RigidBody::getVel(double theV[3]){ - for (int i = 0; i < 3 ; i++) - theV[i] = vel[i]; -} +Mat3x3d RigidBody::getI() { + return inertiaTensor_; +} -void RigidBody::setVel(double theV[3]){ - for (int i = 0; i < 3 ; i++) - vel[i] = theV[i]; -} +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; + Vector3d ephi; + Vector3d etheta; + Vector3d epsi; -void RigidBody::getFrc(double theF[3]){ - for (int i = 0; i < 3 ; i++) - theF[i] = frc[i]; -} + force = getFrc(); + torque =getTrq(); + myEuler = getA().toEulerAngles(); -void RigidBody::addFrc(double theF[3]){ - for (int i = 0; i < 3 ; i++) - frc[i] += theF[i]; -} + phi = myEuler[0]; + theta = myEuler[1]; + psi = myEuler[2]; -void RigidBody::zeroForces() { + cphi = cos(phi); + sphi = sin(phi); + ctheta = cos(theta); + stheta = sin(theta); - for (int i = 0; i < 3; i++) { - frc[i] = 0.0; - trq[i] = 0.0; - } + // get unit vectors along the phi, theta and psi rotation axes -} + ephi[0] = 0.0; + ephi[1] = 0.0; + ephi[2] = 1.0; -void RigidBody::setEuler( double phi, double theta, double psi ){ - - A[0][0] = (cos(phi) * cos(psi)) - (sin(phi) * cos(theta) * sin(psi)); - A[0][1] = (sin(phi) * cos(psi)) + (cos(phi) * cos(theta) * sin(psi)); - A[0][2] = sin(theta) * sin(psi); - - A[1][0] = -(cos(phi) * sin(psi)) - (sin(phi) * cos(theta) * cos(psi)); - A[1][1] = -(sin(phi) * sin(psi)) + (cos(phi) * cos(theta) * cos(psi)); - A[1][2] = sin(theta) * cos(psi); - - A[2][0] = sin(phi) * sin(theta); - A[2][1] = -cos(phi) * sin(theta); - A[2][2] = cos(theta); + etheta[0] = cphi; + etheta[1] = sphi; + etheta[2] = 0.0; -} + epsi[0] = stheta * cphi; + epsi[1] = stheta * sphi; + epsi[2] = ctheta; -void RigidBody::getQ( double q[4] ){ - - double t, s; - double ad1, ad2, ad3; + //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]; + + } - t = A[0][0] + A[1][1] + A[2][2] + 1.0; - if( t > 0.0 ){ - - s = 0.5 / sqrt( t ); - q[0] = 0.25 / s; - q[1] = (A[1][2] - A[2][1]) * s; - q[2] = (A[2][0] - A[0][2]) * s; - q[3] = (A[0][1] - A[1][0]) * s; - } - else{ - - ad1 = fabs( A[0][0] ); - ad2 = fabs( A[1][1] ); - ad3 = fabs( A[2][2] ); - - if( ad1 >= ad2 && ad1 >= ad3 ){ - - s = 2.0 * sqrt( 1.0 + A[0][0] - A[1][1] - A[2][2] ); - q[0] = (A[1][2] + A[2][1]) / s; - q[1] = 0.5 / s; - q[2] = (A[0][1] + A[1][0]) / s; - q[3] = (A[0][2] + A[2][0]) / s; + return grad; +} + +void RigidBody::accept(BaseVisitor* v) { + v->visit(this); +} + +/**@todo need modification */ +void RigidBody::calcRefCoords() { + double mtmp; + Vector3d refCOM(0.0); + mass_ = 0.0; + for (std::size_t i = 0; i < atoms_.size(); ++i) { + mtmp = atoms_[i]->getMass(); + mass_ += mtmp; + refCOM += refCoords_[i]*mtmp; } - else if( ad2 >= ad1 && ad2 >= ad3 ){ - - s = sqrt( 1.0 + A[1][1] - A[0][0] - A[2][2] ) * 2.0; - q[0] = (A[0][2] + A[2][0]) / s; - q[1] = (A[0][1] + A[1][0]) / s; - q[2] = 0.5 / s; - q[3] = (A[1][2] + A[2][1]) / s; + 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; } - else{ - - s = sqrt( 1.0 + A[2][2] - A[0][0] - A[1][1] ) * 2.0; - q[0] = (A[0][1] + A[1][0]) / s; - q[1] = (A[0][2] + A[2][0]) / s; - q[2] = (A[1][2] + A[2][1]) / s; - q[3] = 0.5 / s; + +// Moment of Inertia calculation + Mat3x3d Itmp(0.0); + + for (std::size_t i = 0; i < atoms_.size(); i++) { + 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; } - } -} -void RigidBody::setQ( double the_q[4] ){ + //diagonalize + Vector3d evals; + Mat3x3d::diagonalize(Itmp, evals, sU_); - double q0Sqr, q1Sqr, q2Sqr, q3Sqr; - - q0Sqr = the_q[0] * the_q[0]; - q1Sqr = the_q[1] * the_q[1]; - q2Sqr = the_q[2] * the_q[2]; - q3Sqr = the_q[3] * the_q[3]; - - A[0][0] = q0Sqr + q1Sqr - q2Sqr - q3Sqr; - A[0][1] = 2.0 * ( the_q[1] * the_q[2] + the_q[0] * the_q[3] ); - A[0][2] = 2.0 * ( the_q[1] * the_q[3] - the_q[0] * the_q[2] ); - - A[1][0] = 2.0 * ( the_q[1] * the_q[2] - the_q[0] * the_q[3] ); - A[1][1] = q0Sqr - q1Sqr + q2Sqr - q3Sqr; - A[1][2] = 2.0 * ( the_q[2] * the_q[3] + the_q[0] * the_q[1] ); - - A[2][0] = 2.0 * ( the_q[1] * the_q[3] + the_q[0] * the_q[2] ); - A[2][1] = 2.0 * ( the_q[2] * the_q[3] - the_q[0] * the_q[1] ); - A[2][2] = q0Sqr - q1Sqr -q2Sqr +q3Sqr; + // zero out I and then fill the diagonals with the moments of inertia: + inertiaTensor_(0, 0) = evals[0]; + inertiaTensor_(1, 1) = evals[1]; + inertiaTensor_(2, 2) = evals[2]; + + int nLinearAxis = 0; + for (int i = 0; i < 3; i++) { + if (fabs(evals[i]) < oopse::epsilon) { + linear_ = true; + linearAxis_ = i; + ++ nLinearAxis; + } + } + if (nLinearAxis > 1) { + sprintf( painCave.errMsg, + "RigidBody error.\n" + "\tOOPSE found more than one axis in this rigid body with a vanishing \n" + "\tmoment of inertia. This can happen in one of three ways:\n" + "\t 1) Only one atom was specified, or \n" + "\t 2) All atoms were specified at the same location, or\n" + "\t 3) The programmers did something stupid.\n" + "\tIt is silly to use a rigid body to describe this situation. Be smarter.\n" + ); + painCave.isFatal = 1; + simError(); + } + } -void RigidBody::getA( double the_A[3][3] ){ - - for (int i = 0; i < 3; i++) - for (int j = 0; j < 3; j++) - the_A[i][j] = A[i][j]; +void RigidBody::calcForcesAndTorques() { + Vector3d afrc; + Vector3d atrq; + Vector3d apos; + Vector3d rpos; + Vector3d frc(0.0); + Vector3d trq(0.0); + Vector3d pos = this->getPos(); + for (int i = 0; i < atoms_.size(); i++) { -} + afrc = atoms_[i]->getFrc(); + apos = atoms_[i]->getPos(); + rpos = apos - pos; + + frc += afrc; -void RigidBody::setA( double the_A[3][3] ){ + 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]; - for (int i = 0; i < 3; i++) - for (int j = 0; j < 3; j++) - A[i][j] = the_A[i][j]; - + // 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; + } + + } + + setFrc(frc); + setTrq(trq); + } -void RigidBody::getJ( double theJ[3] ){ - - for (int i = 0; i < 3; i++) - theJ[i] = ji[i]; +void RigidBody::updateAtoms() { + unsigned int i; + Vector3d ref; + Vector3d apos; + DirectionalAtom* dAtom; + Vector3d pos = getPos(); + RotMat3x3d a = getA(); + + for (i = 0; i < atoms_.size(); i++) { + + ref = body2Lab(refCoords_[i]); -} + apos = pos + ref; -void RigidBody::setJ( double theJ[3] ){ - - for (int i = 0; i < 3; i++) - ji[i] = theJ[i]; + atoms_[i]->setPos(apos); + if (atoms_[i]->isDirectional()) { + + dAtom = (DirectionalAtom *) atoms_[i]; + dAtom->setA(a * refOrients_[i]); + //dAtom->rotateBy( A ); + } + + } + } -void RigidBody::getTrq(double theT[3]){ - for (int i = 0; i < 3 ; i++) - theT[i] = trq[i]; -} -void RigidBody::addTrq(double theT[3]){ - for (int i = 0; i < 3 ; i++) - trq[i] += theT[i]; -} +bool RigidBody::getAtomPos(Vector3d& pos, unsigned int index) { + if (index < atoms_.size()) { -void RigidBody::getI( double the_I[3][3] ){ + Vector3d ref = body2Lab(refCoords_[index]); + pos = getPos() + ref; + return true; + } else { + std::cerr << index << " is an invalid index, current rigid body contains " + << atoms_.size() << "atoms" << std::endl; + return false; + } +} - for (int i = 0; i < 3; i++) - for (int j = 0; j < 3; j++) - the_I[i][j] = I[i][j]; - +bool RigidBody::getAtomPos(Vector3d& pos, Atom* atom) { + std::vector::iterator i; + i = std::find(atoms_.begin(), atoms_.end(), atom); + if (i != atoms_.end()) { + //RigidBody class makes sure refCoords_ and atoms_ match each other + Vector3d ref = body2Lab(refCoords_[i - atoms_.begin()]); + pos = getPos() + ref; + return true; + } else { + std::cerr << "Atom " << atom->getGlobalIndex() + <<" does not belong to Rigid body "<< getGlobalIndex() << std::endl; + return false; + } } +bool RigidBody::getAtomVel(Vector3d& vel, unsigned int index) { -void RigidBody::lab2Body( double r[3] ){ + //velRot = $(A\cdot skew(I^{-1}j))^{T}refCoor$ - double rl[3]; // the lab frame vector - - rl[0] = r[0]; - rl[1] = r[1]; - rl[2] = r[2]; - - r[0] = (A[0][0] * rl[0]) + (A[0][1] * rl[1]) + (A[0][2] * rl[2]); - r[1] = (A[1][0] * rl[0]) + (A[1][1] * rl[1]) + (A[1][2] * rl[2]); - r[2] = (A[2][0] * rl[0]) + (A[2][1] * rl[1]) + (A[2][2] * rl[2]); + if (index < atoms_.size()) { -} + Vector3d velRot; + Mat3x3d skewMat;; + Vector3d ref = refCoords_[index]; + Vector3d ji = getJ(); + Mat3x3d I = getI(); -void RigidBody::body2Lab( double r[3] ){ + skewMat(0, 0) =0; + skewMat(0, 1) = ji[2] /I(2, 2); + skewMat(0, 2) = -ji[1] /I(1, 1); - double rb[3]; // the body frame vector - - rb[0] = r[0]; - rb[1] = r[1]; - rb[2] = r[2]; - - r[0] = (A[0][0] * rb[0]) + (A[1][0] * rb[1]) + (A[2][0] * rb[2]); - r[1] = (A[0][1] * rb[0]) + (A[1][1] * rb[1]) + (A[2][1] * rb[2]); - r[2] = (A[0][2] * rb[0]) + (A[1][2] * rb[1]) + (A[2][2] * rb[2]); + skewMat(1, 0) = -ji[2] /I(2, 2); + skewMat(1, 1) = 0; + skewMat(1, 2) = ji[0]/I(0, 0); -} + skewMat(2, 0) =ji[1] /I(1, 1); + skewMat(2, 1) = -ji[0]/I(0, 0); + skewMat(2, 2) = 0; -double RigidBody::getZangle( ){ - return zAngle; -} + velRot = (getA() * skewMat).transpose() * ref; -void RigidBody::setZangle( double zAng ){ - zAngle = zAng; + vel =getVel() + velRot; + return true; + + } else { + std::cerr << index << " is an invalid index, current rigid body contains " + << atoms_.size() << "atoms" << std::endl; + return false; + } } -void RigidBody::addZangle( double zAng ){ - zAngle += zAng; -} +bool RigidBody::getAtomVel(Vector3d& vel, Atom* atom) { -void RigidBody::calcRefCoords( ) { - - int i,j,k, it, n_linear_coords; - double mtmp; - vec3 apos; - double refCOM[3]; - vec3 ptmp; - double Itmp[3][3]; - double evals[3]; - double evects[3][3]; - double r, r2, len; - - // First, find the center of mass: - - mass = 0.0; - for (j=0; j<3; j++) - refCOM[j] = 0.0; - - for (i = 0; i < myAtoms.size(); i++) { - mtmp = myAtoms[i]->getMass(); - mass += mtmp; - - apos = refCoords[i]; - - for(j = 0; j < 3; j++) { - refCOM[j] += apos[j]*mtmp; + std::vector::iterator i; + i = std::find(atoms_.begin(), atoms_.end(), atom); + if (i != atoms_.end()) { + return getAtomVel(vel, i - atoms_.begin()); + } else { + std::cerr << "Atom " << atom->getGlobalIndex() + <<" does not belong to Rigid body "<< getGlobalIndex() << std::endl; + return false; } - } - - for(j = 0; j < 3; j++) - refCOM[j] /= mass; +} -// Next, move the origin of the reference coordinate system to the COM: +bool RigidBody::getAtomRefCoor(Vector3d& coor, unsigned int index) { + if (index < atoms_.size()) { - for (i = 0; i < myAtoms.size(); i++) { - apos = refCoords[i]; - for (j=0; j < 3; j++) { - apos[j] = apos[j] - refCOM[j]; + coor = refCoords_[index]; + return true; + } else { + std::cerr << index << " is an invalid index, current rigid body contains " + << atoms_.size() << "atoms" << std::endl; + return false; } - refCoords[i] = apos; - } -// Moment of Inertia calculation +} - for (i = 0; i < 3; i++) - for (j = 0; j < 3; j++) - Itmp[i][j] = 0.0; - - for (it = 0; it < myAtoms.size(); it++) { - - mtmp = myAtoms[it]->getMass(); - ptmp = refCoords[it]; - r= norm3(ptmp.vec); - r2 = r*r; - - for (i = 0; i < 3; i++) { - for (j = 0; j < 3; j++) { - - if (i==j) Itmp[i][j] += mtmp * r2; - - Itmp[i][j] -= mtmp * ptmp.vec[i]*ptmp.vec[j]; - } +bool RigidBody::getAtomRefCoor(Vector3d& coor, Atom* atom) { + std::vector::iterator i; + i = std::find(atoms_.begin(), atoms_.end(), atom); + if (i != atoms_.end()) { + //RigidBody class makes sure refCoords_ and atoms_ match each other + coor = refCoords_[i - atoms_.begin()]; + return true; + } else { + std::cerr << "Atom " << atom->getGlobalIndex() + <<" does not belong to Rigid body "<< getGlobalIndex() << std::endl; + return false; } - } - - diagonalize3x3(Itmp, evals, sU); - - // zero out I and then fill the diagonals with the moments of inertia: - n_linear_coords = 0; - - for (i = 0; i < 3; i++) { - for (j = 0; j < 3; j++) { - I[i][j] = 0.0; - } - I[i][i] = evals[i]; - - if (fabs(evals[i]) < momIntTol) { - is_linear = true; - n_linear_coords++; - linear_axis = i; - } - } - - if (n_linear_coords > 1) { - sprintf( painCave.errMsg, - "RigidBody error.\n" - "\tOOPSE found more than one axis in this rigid body with a vanishing \n" - "\tmoment of inertia. This can happen in one of three ways:\n" - "\t 1) Only one atom was specified, or \n" - "\t 2) All atoms were specified at the same location, or\n" - "\t 3) The programmers did something stupid.\n" - "\tIt is silly to use a rigid body to describe this situation. Be smarter.\n" - ); - painCave.isFatal = 1; - simError(); - } - - // renormalize column vectors: - - for (i=0; i < 3; i++) { - len = 0.0; - for (j = 0; j < 3; j++) { - len += sU[i][j]*sU[i][j]; - } - len = sqrt(len); - for (j = 0; j < 3; j++) { - sU[i][j] /= len; - } - } } -void RigidBody::doEulerToRotMat(vec3 &euler, mat3x3 &myA ){ - double phi, theta, psi; - - phi = euler[0]; - theta = euler[1]; - psi = euler[2]; - - myA[0][0] = (cos(phi) * cos(psi)) - (sin(phi) * cos(theta) * sin(psi)); - myA[0][1] = (sin(phi) * cos(psi)) + (cos(phi) * cos(theta) * sin(psi)); - myA[0][2] = sin(theta) * sin(psi); - - myA[1][0] = -(cos(phi) * sin(psi)) - (sin(phi) * cos(theta) * cos(psi)); - myA[1][1] = -(sin(phi) * sin(psi)) + (cos(phi) * cos(theta) * cos(psi)); - myA[1][2] = sin(theta) * cos(psi); - - myA[2][0] = sin(phi) * sin(theta); - myA[2][1] = -cos(phi) * sin(theta); - myA[2][2] = cos(theta); +void RigidBody::addAtom(Atom* at, AtomStamp* ats) { -} - -void RigidBody::calcForcesAndTorques() { - - // Convert Atomic forces and torques to total forces and torques: - int i, j; - double apos[3]; - double afrc[3]; - double atrq[3]; - double rpos[3]; - - zeroForces(); + Vector3d coords; + Vector3d euler; - for (i = 0; i < myAtoms.size(); i++) { - myAtoms[i]->getPos(apos); - myAtoms[i]->getFrc(afrc); - - for (j=0; j<3; j++) { - rpos[j] = apos[j] - pos[j]; - frc[j] += afrc[j]; - } - - 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 (myAtoms[i]->isDirectional()) { - - myAtoms[i]->getTrq(atrq); - - for (j=0; j<3; j++) - trq[j] += atrq[j]; - } + atoms_.push_back(at); + + if( !ats->havePosition() ){ + sprintf( painCave.errMsg, + "RigidBody error.\n" + "\tAtom %s does not have a position specified.\n" + "\tThis means RigidBody cannot set up reference coordinates.\n", + ats->getType() ); + painCave.isFatal = 1; + simError(); } + + coords[0] = ats->getPosX(); + coords[1] = ats->getPosY(); + coords[2] = ats->getPosZ(); - // Convert Torque to Body-fixed coordinates: - // (Actually, on second thought, don't. Integrator does this now.) - // lab2Body(trq); + refCoords_.push_back(coords); -} - -void RigidBody::updateAtoms() { - int i, j; - vec3 ref; - double apos[3]; - DirectionalAtom* dAtom; + RotMat3x3d identMat = RotMat3x3d::identity(); - for (i = 0; i < myAtoms.size(); i++) { - - ref = refCoords[i]; + if (at->isDirectional()) { - body2Lab(ref.vec); + if( !ats->haveOrientation() ){ + sprintf( painCave.errMsg, + "RigidBody error.\n" + "\tAtom %s does not have an orientation specified.\n" + "\tThis means RigidBody cannot set up reference orientations.\n", + ats->getType() ); + painCave.isFatal = 1; + simError(); + } - for (j = 0; j<3; j++) - apos[j] = pos[j] + ref.vec[j]; - - myAtoms[i]->setPos(apos); - - if (myAtoms[i]->isDirectional()) { - - dAtom = (DirectionalAtom *) myAtoms[i]; - dAtom->rotateBy( A ); - - } - } -} + euler[0] = ats->getEulerPhi(); + euler[1] = ats->getEulerTheta(); + euler[2] = ats->getEulerPsi(); -void RigidBody::getGrad( double grad[6] ) { - - double myEuler[3]; - double phi, theta, psi; - double cphi, sphi, ctheta, stheta; - double ephi[3]; - double etheta[3]; - double epsi[3]; - - this->getEulerAngles(myEuler); - - 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; - - epsi[0] = stheta * cphi; - epsi[1] = stheta * sphi; - epsi[2] = ctheta; - - for (int j = 0 ; j<3; j++) - grad[j] = frc[j]; - - grad[3] = 0.0; - grad[4] = 0.0; - grad[5] = 0.0; - - for (int j = 0; j < 3; j++ ) { + RotMat3x3d Atmp(euler); + refOrients_.push_back(Atmp); - grad[3] += trq[j]*ephi[j]; - grad[4] += trq[j]*etheta[j]; - grad[5] += trq[j]*epsi[j]; - + }else { + refOrients_.push_back(identMat); } -} - -/** - * getEulerAngles computes a set of Euler angle values consistent - * with an input rotation matrix. They are returned in the following - * order: - * myEuler[0] = phi; - * myEuler[1] = theta; - * myEuler[2] = psi; -*/ -void RigidBody::getEulerAngles(double myEuler[3]) { - - // We use so-called "x-convention", which is the most common - // definition. In this convention, the rotation given by Euler - // angles (phi, theta, psi), where the first rotation is by an angle - // phi about the z-axis, the second is by an angle theta (0 <= theta - // <= 180) about the x-axis, and the third is by an angle psi about - // the z-axis (again). - - double phi,theta,psi,eps; - double pi; - double cphi,ctheta,cpsi; - double sphi,stheta,spsi; - double b[3]; - int flip[3]; - - // set the tolerance for Euler angles and rotation elements - - eps = 1.0e-8; - - theta = acos(min(1.0,max(-1.0,A[2][2]))); - ctheta = A[2][2]; - stheta = sqrt(1.0 - ctheta * ctheta); - - // when sin(theta) is close to 0, we need to consider the - // possibility of a singularity. In this case, we can assign an - // arbitary value to phi (or psi), and then determine the psi (or - // phi) or vice-versa. We'll assume that phi always gets the - // rotation, and psi is 0 in cases of singularity. we use atan2 - // instead of atan, since atan2 will give us -Pi to Pi. Since 0 <= - // theta <= 180, sin(theta) will be always non-negative. Therefore, - // it never changes the sign of both of the parameters passed to - // atan2. - - if (fabs(stheta) <= eps){ - psi = 0.0; - phi = atan2(-A[1][0], A[0][0]); - } - // we only have one unique solution - else{ - phi = atan2(A[2][0], -A[2][1]); - psi = atan2(A[0][2], A[1][2]); - } - - //wrap phi and psi, make sure they are in the range from 0 to 2*Pi - //if (phi < 0) - // phi += M_PI; - - //if (psi < 0) - // psi += M_PI; - - myEuler[0] = phi; - myEuler[1] = theta; - myEuler[2] = psi; - - return; } -double RigidBody::max(double x, double y) { - return (x > y) ? x : y; } -double RigidBody::min(double x, double y) { - return (x > y) ? y : x; -} - -void RigidBody::findCOM() { - - size_t i; - int j; - double mtmp; - double ptmp[3]; - double vtmp[3]; - - for(j = 0; j < 3; j++) { - pos[j] = 0.0; - vel[j] = 0.0; - } - mass = 0.0; - - for (i = 0; i < myAtoms.size(); i++) { - - mtmp = myAtoms[i]->getMass(); - myAtoms[i]->getPos(ptmp); - myAtoms[i]->getVel(vtmp); - - mass += mtmp; - - for(j = 0; j < 3; j++) { - pos[j] += ptmp[j]*mtmp; - vel[j] += vtmp[j]*mtmp; - } - - } - - for(j = 0; j < 3; j++) { - pos[j] /= mass; - vel[j] /= mass; - } - -} - -void RigidBody::accept(BaseVisitor* v){ - vector::iterator atomIter; - v->visit(this); - - //for(atomIter = myAtoms.begin(); atomIter != myAtoms.end(); ++atomIter) - // (*atomIter)->accept(v); -} -void RigidBody::getAtomRefCoor(double pos[3], int index){ - vec3 ref; - - ref = refCoords[index]; - pos[0] = ref[0]; - pos[1] = ref[1]; - pos[2] = ref[2]; - -} - - -void RigidBody::getAtomPos(double theP[3], int index){ - vec3 ref; - - if (index >= myAtoms.size()) - cerr << index << " is an invalid index, current rigid body contains " << myAtoms.size() << "atoms" << endl; - - ref = refCoords[index]; - body2Lab(ref.vec); - - theP[0] = pos[0] + ref[0]; - theP[1] = pos[1] + ref[1]; - theP[2] = pos[2] + ref[2]; -} - - -void RigidBody::getAtomVel(double theV[3], int index){ - vec3 ref; - double velRot[3]; - double skewMat[3][3]; - double aSkewMat[3][3]; - double aSkewTransMat[3][3]; - - //velRot = $(A\cdot skew(I^{-1}j))^{T}refCoor$ - - if (index >= myAtoms.size()) - cerr << index << " is an invalid index, current rigid body contains " << myAtoms.size() << "atoms" << endl; - - ref = refCoords[index]; - - skewMat[0][0] =0; - skewMat[0][1] = ji[2] /I[2][2]; - skewMat[0][2] = -ji[1] /I[1][1]; - - skewMat[1][0] = -ji[2] /I[2][2]; - skewMat[1][1] = 0; - skewMat[1][2] = ji[0]/I[0][0]; - - skewMat[2][0] =ji[1] /I[1][1]; - skewMat[2][1] = -ji[0]/I[0][0]; - skewMat[2][2] = 0; - - matMul3(A, skewMat, aSkewMat); - - transposeMat3(aSkewMat, aSkewTransMat); - - matVecMul3(aSkewTransMat, ref.vec, velRot); - theV[0] = vel[0] + velRot[0]; - theV[1] = vel[1] + velRot[1]; - theV[2] = vel[2] + velRot[2]; -} - -