src/primitives/RigidBody.cpp

 /*
 * 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 <algorithm>
#include <math.h>
#include "primitives/RigidBody.hpp"
#include "utils/simError.h"
namespace oopse {

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]);
        }
    }

}

      
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]);
        }
    }
}    
    
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);
        }
    }

}   

Mat3x3d RigidBody::getI() {
    return inertiaTensor_;
}    

std::vector<double> RigidBody::getGrad() {
     std::vector<double> 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;

    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;

    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;
    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;
    }
    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);
  
    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;
    }

    //project the inertial moment of directional atoms into this rigid body
    for (std::size_t i = 0; i < atoms_.size(); i++) {
        if (atoms_[i]->isDirectional()) {
            RectMatrix<double, 3, 3> Iproject = refOrients_[i].transpose() * atoms_[i]->getI();
            Itmp(0, 0) += Iproject(0, 0);
            Itmp(1, 1) += Iproject(1, 1);
            Itmp(2, 2) += Iproject(2, 2);
        }
    }

    //diagonalize 
    Vector3d evals;
    Mat3x3d::diagonalize(Itmp, evals, sU_);

    // 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::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;

        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;
        }
        
    }
    
    setFrc(frc);
    setTrq(trq);
    
}

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;

        atoms_[i]->setPos(apos);

        if (atoms_[i]->isDirectional()) {
          
          dAtom = (DirectionalAtom *) atoms_[i];
          dAtom->setA(a * refOrients_[i]);
          //dAtom->rotateBy( A );      
        }

    }
  
}


bool RigidBody::getAtomPos(Vector3d& pos, unsigned int index) {
    if (index < atoms_.size()) {

        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;
    }    
}

bool RigidBody::getAtomPos(Vector3d& pos, Atom* atom) {
    std::vector<Atom*>::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) {

    //velRot = $(A\cdot skew(I^{-1}j))^{T}refCoor$

    if (index < atoms_.size()) {

        Vector3d velRot;
        Mat3x3d skewMat;;
        Vector3d ref = refCoords_[index];
        Vector3d ji = getJ();
        Mat3x3d I =  getI();

        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;

        velRot = (getA() * skewMat).transpose() * ref;

        vel =getVel() + velRot;
        return true;
        
    } else {
        std::cerr << index << " is an invalid index, current rigid body contains " 
                      << atoms_.size() << "atoms" << std::endl;
        return false;
    }
}

bool RigidBody::getAtomVel(Vector3d& vel, Atom* atom) {

    std::vector<Atom*>::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;
    }    
}

bool RigidBody::getAtomRefCoor(Vector3d& coor, unsigned int index) {
    if (index < atoms_.size()) {

        coor = refCoords_[index];
        return true;
    } else {
        std::cerr << index << " is an invalid index, current rigid body contains " 
                      << atoms_.size() << "atoms" << std::endl;
        return false;
    }

}

bool RigidBody::getAtomRefCoor(Vector3d& coor, Atom* atom) {
    std::vector<Atom*>::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;
    }

}


void RigidBody::addAtom(Atom* at, AtomStamp* ats) {

  Vector3d coords;
  Vector3d euler;
  

  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();

  refCoords_.push_back(coords);

  RotMat3x3d identMat = RotMat3x3d::identity();
  
  if (at->isDirectional()) {   

    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();
    }    
    
    euler[0] = ats->getEulerPhi();
    euler[1] = ats->getEulerTheta();
    euler[2] = ats->getEulerPsi();

    RotMat3x3d Atmp(euler);
    refOrients_.push_back(Atmp);
    
  }else {
    refOrients_.push_back(identMat);
  }
  
  
}

}

Revision:	273
Committed:	Tue Jan 25 17:45:23 2005 UTC (20 years, 3 months ago) by tim
Original Path:	*trunk/src/primitives/RigidBody.cpp*
File size:	12877 byte(s)
Log Message:	(1) complete section parser's error message (2) add GhostTorsion (3) accumulate inertial tensor from the directional atoms before calculate rigidbody's inertial tensor
#	User	Rev	Content
1	gezelter	246	/*
2			* Copyright (c) 2005 The University of Notre Dame. All Rights Reserved.
3			*
4			* The University of Notre Dame grants you ("Licensee") a
5			* non-exclusive, royalty free, license to use, modify and
6			* redistribute this software in source and binary code form, provided
7			* that the following conditions are met:
8			*
9			* 1. Acknowledgement of the program authors must be made in any
10			* publication of scientific results based in part on use of the
11			* program. An acceptable form of acknowledgement is citation of
12			* the article in which the program was described (Matthew
13			* A. Meineke, Charles F. Vardeman II, Teng Lin, Christopher
14			* J. Fennell and J. Daniel Gezelter, "OOPSE: An Object-Oriented
15			* Parallel Simulation Engine for Molecular Dynamics,"
16			* J. Comput. Chem. 26, pp. 252-271 (2005))
17			*
18			* 2. Redistributions of source code must retain the above copyright
19			* notice, this list of conditions and the following disclaimer.
20			*
21			* 3. Redistributions in binary form must reproduce the above copyright
22			* notice, this list of conditions and the following disclaimer in the
23			* documentation and/or other materials provided with the
24			* distribution.
25			*
26			* This software is provided "AS IS," without a warranty of any
27			* kind. All express or implied conditions, representations and
28			* warranties, including any implied warranty of merchantability,
29			* fitness for a particular purpose or non-infringement, are hereby
30			* excluded. The University of Notre Dame and its licensors shall not
31			* be liable for any damages suffered by licensee as a result of
32			* using, modifying or distributing the software or its
33			* derivatives. In no event will the University of Notre Dame or its
34			* licensors be liable for any lost revenue, profit or data, or for
35			* direct, indirect, special, consequential, incidental or punitive
36			* damages, however caused and regardless of the theory of liability,
37			* arising out of the use of or inability to use software, even if the
38			* University of Notre Dame has been advised of the possibility of
39			* such damages.
40			*/
41			#include <algorithm>
42	tim	253	#include <math.h>
43	tim	3	#include "primitives/RigidBody.hpp"
44			#include "utils/simError.h"
45	gezelter	246	namespace oopse {
46	gezelter	2
47	gezelter	246	RigidBody::RigidBody() : StuntDouble(otRigidBody, &Snapshot::rigidbodyData), inertiaTensor_(0.0){
48	gezelter	2
49			}
50
51	gezelter	246	void RigidBody::setPrevA(const RotMat3x3d& a) {
52			((snapshotMan_->getPrevSnapshot())->*storage_).aMat[localIndex_] = a;
53			//((snapshotMan_->getPrevSnapshot())->storage_).electroFrame[localIndex_] = a.transpose() sU_;
54	gezelter	2
55	gezelter	246	for (int i =0 ; i < atoms_.size(); ++i){
56			if (atoms_[i]->isDirectional()) {
57			atoms_[i]->setPrevA(a * refOrients_[i]);
58			}
59			}
60	gezelter	2
61			}
62
63	gezelter	246
64			void RigidBody::setA(const RotMat3x3d& a) {
65			((snapshotMan_->getCurrentSnapshot())->*storage_).aMat[localIndex_] = a;
66			//((snapshotMan_->getCurrentSnapshot())->storage_).electroFrame[localIndex_] = a.transpose() sU_;
67	gezelter	2
68	gezelter	246	for (int i =0 ; i < atoms_.size(); ++i){
69			if (atoms_[i]->isDirectional()) {
70			atoms_[i]->setA(a * refOrients_[i]);
71			}
72			}
73	gezelter	2	}
74
75	gezelter	246	void RigidBody::setA(const RotMat3x3d& a, int snapshotNo) {
76			((snapshotMan_->getSnapshot(snapshotNo))->*storage_).aMat[localIndex_] = a;
77			//((snapshotMan_->getSnapshot(snapshotNo))->storage_).electroFrame[localIndex_] = a.transpose() sU_;
78	gezelter	2
79	gezelter	246	for (int i =0 ; i < atoms_.size(); ++i){
80			if (atoms_[i]->isDirectional()) {
81			atoms_[i]->setA(a * refOrients_[i], snapshotNo);
82			}
83	gezelter	2	}
84
85	gezelter	246	}
86	gezelter	2
87	gezelter	246	Mat3x3d RigidBody::getI() {
88			return inertiaTensor_;
89			}
90	gezelter	2
91	gezelter	246	std::vector<double> RigidBody::getGrad() {
92			std::vector<double> grad(6, 0.0);
93			Vector3d force;
94			Vector3d torque;
95			Vector3d myEuler;
96			double phi, theta, psi;
97			double cphi, sphi, ctheta, stheta;
98			Vector3d ephi;
99			Vector3d etheta;
100			Vector3d epsi;
101	gezelter	2
102	gezelter	246	force = getFrc();
103			torque =getTrq();
104			myEuler = getA().toEulerAngles();
105	gezelter	2
106	gezelter	246	phi = myEuler[0];
107			theta = myEuler[1];
108			psi = myEuler[2];
109	gezelter	2
110	gezelter	246	cphi = cos(phi);
111			sphi = sin(phi);
112			ctheta = cos(theta);
113			stheta = sin(theta);
114	gezelter	2
115	gezelter	246	// get unit vectors along the phi, theta and psi rotation axes
116	gezelter	2
117	gezelter	246	ephi[0] = 0.0;
118			ephi[1] = 0.0;
119			ephi[2] = 1.0;
120	gezelter	2
121	gezelter	246	etheta[0] = cphi;
122			etheta[1] = sphi;
123			etheta[2] = 0.0;
124	gezelter	2
125	gezelter	246	epsi[0] = stheta * cphi;
126			epsi[1] = stheta * sphi;
127			epsi[2] = ctheta;
128	gezelter	2
129	gezelter	246	//gradient is equal to -force
130			for (int j = 0 ; j<3; j++)
131			grad[j] = -force[j];
132	gezelter	2
133	gezelter	246	for (int j = 0; j < 3; j++ ) {
134	gezelter	2
135	gezelter	246	grad[3] += torque[j]*ephi[j];
136			grad[4] += torque[j]*etheta[j];
137			grad[5] += torque[j]*epsi[j];
138	gezelter	2
139	gezelter	246	}
140
141			return grad;
142			}
143	gezelter	2
144	gezelter	246	void RigidBody::accept(BaseVisitor* v) {
145			v->visit(this);
146			}
147	gezelter	2
148	gezelter	246	/*@todo need modification /
149			void RigidBody::calcRefCoords() {
150			double mtmp;
151			Vector3d refCOM(0.0);
152			mass_ = 0.0;
153			for (std::size_t i = 0; i < atoms_.size(); ++i) {
154			mtmp = atoms_[i]->getMass();
155			mass_ += mtmp;
156			refCOM += refCoords_[i]*mtmp;
157			}
158			refCOM /= mass_;
159	gezelter	2
160	gezelter	246	// Next, move the origin of the reference coordinate system to the COM:
161			for (std::size_t i = 0; i < atoms_.size(); ++i) {
162			refCoords_[i] -= refCOM;
163			}
164	gezelter	2
165	gezelter	246	// Moment of Inertia calculation
166			Mat3x3d Itmp(0.0);
167	gezelter	2
168	gezelter	246	for (std::size_t i = 0; i < atoms_.size(); i++) {
169			mtmp = atoms_[i]->getMass();
170			Itmp -= outProduct(refCoords_[i], refCoords_[i]) * mtmp;
171			double r2 = refCoords_[i].lengthSquare();
172			Itmp(0, 0) += mtmp * r2;
173			Itmp(1, 1) += mtmp * r2;
174			Itmp(2, 2) += mtmp * r2;
175			}
176	gezelter	2
177	tim	273	//project the inertial moment of directional atoms into this rigid body
178			for (std::size_t i = 0; i < atoms_.size(); i++) {
179			if (atoms_[i]->isDirectional()) {
180			RectMatrix<double, 3, 3> Iproject = refOrients_[i].transpose() * atoms_[i]->getI();
181			Itmp(0, 0) += Iproject(0, 0);
182			Itmp(1, 1) += Iproject(1, 1);
183			Itmp(2, 2) += Iproject(2, 2);
184			}
185			}
186
187	gezelter	246	//diagonalize
188			Vector3d evals;
189			Mat3x3d::diagonalize(Itmp, evals, sU_);
190	gezelter	2
191	gezelter	246	// zero out I and then fill the diagonals with the moments of inertia:
192			inertiaTensor_(0, 0) = evals[0];
193			inertiaTensor_(1, 1) = evals[1];
194			inertiaTensor_(2, 2) = evals[2];
195
196			int nLinearAxis = 0;
197			for (int i = 0; i < 3; i++) {
198			if (fabs(evals[i]) < oopse::epsilon) {
199			linear_ = true;
200			linearAxis_ = i;
201			++ nLinearAxis;
202			}
203			}
204	gezelter	2
205	gezelter	246	if (nLinearAxis > 1) {
206			sprintf( painCave.errMsg,
207			"RigidBody error.\n"
208			"\tOOPSE found more than one axis in this rigid body with a vanishing \n"
209			"\tmoment of inertia. This can happen in one of three ways:\n"
210			"\t 1) Only one atom was specified, or \n"
211			"\t 2) All atoms were specified at the same location, or\n"
212			"\t 3) The programmers did something stupid.\n"
213			"\tIt is silly to use a rigid body to describe this situation. Be smarter.\n"
214			);
215			painCave.isFatal = 1;
216			simError();
217			}
218	gezelter	2
219			}
220
221	gezelter	246	void RigidBody::calcForcesAndTorques() {
222			Vector3d afrc;
223			Vector3d atrq;
224			Vector3d apos;
225			Vector3d rpos;
226			Vector3d frc(0.0);
227			Vector3d trq(0.0);
228			Vector3d pos = this->getPos();
229			for (int i = 0; i < atoms_.size(); i++) {
230	gezelter	2
231	gezelter	246	afrc = atoms_[i]->getFrc();
232			apos = atoms_[i]->getPos();
233			rpos = apos - pos;
234
235			frc += afrc;
236	gezelter	2
237	gezelter	246	trq[0] += rpos[1]afrc[2] - rpos[2]afrc[1];
238			trq[1] += rpos[2]afrc[0] - rpos[0]afrc[2];
239			trq[2] += rpos[0]afrc[1] - rpos[1]afrc[0];
240	gezelter	2
241	gezelter	246	// If the atom has a torque associated with it, then we also need to
242			// migrate the torques onto the center of mass:
243	gezelter	2
244	gezelter	246	if (atoms_[i]->isDirectional()) {
245			atrq = atoms_[i]->getTrq();
246			trq += atrq;
247			}
248
249			}
250
251			setFrc(frc);
252			setTrq(trq);
253
254			}
255	gezelter	2
256	gezelter	246	void RigidBody::updateAtoms() {
257			unsigned int i;
258			Vector3d ref;
259			Vector3d apos;
260			DirectionalAtom* dAtom;
261			Vector3d pos = getPos();
262			RotMat3x3d a = getA();
263	gezelter	2
264	gezelter	246	for (i = 0; i < atoms_.size(); i++) {
265
266			ref = body2Lab(refCoords_[i]);
267	gezelter	2
268	gezelter	246	apos = pos + ref;
269	gezelter	2
270	gezelter	246	atoms_[i]->setPos(apos);
271	gezelter	2
272	gezelter	246	if (atoms_[i]->isDirectional()) {
273
274			dAtom = (DirectionalAtom *) atoms_[i];
275			dAtom->setA(a * refOrients_[i]);
276			//dAtom->rotateBy( A );
277			}
278	gezelter	2
279			}
280
281	gezelter	246	}
282	gezelter	2
283
284	gezelter	246	bool RigidBody::getAtomPos(Vector3d& pos, unsigned int index) {
285			if (index < atoms_.size()) {
286	gezelter	2
287	gezelter	246	Vector3d ref = body2Lab(refCoords_[index]);
288			pos = getPos() + ref;
289			return true;
290			} else {
291			std::cerr << index << " is an invalid index, current rigid body contains "
292			<< atoms_.size() << "atoms" << std::endl;
293			return false;
294			}
295			}
296	gezelter	2
297	gezelter	246	bool RigidBody::getAtomPos(Vector3d& pos, Atom* atom) {
298			std::vector<Atom*>::iterator i;
299			i = std::find(atoms_.begin(), atoms_.end(), atom);
300			if (i != atoms_.end()) {
301			//RigidBody class makes sure refCoords_ and atoms_ match each other
302			Vector3d ref = body2Lab(refCoords_[i - atoms_.begin()]);
303			pos = getPos() + ref;
304			return true;
305			} else {
306			std::cerr << "Atom " << atom->getGlobalIndex()
307			<<" does not belong to Rigid body "<< getGlobalIndex() << std::endl;
308			return false;
309	gezelter	2	}
310			}
311	gezelter	246	bool RigidBody::getAtomVel(Vector3d& vel, unsigned int index) {
312	gezelter	2
313	gezelter	246	//velRot = $(A\cdot skew(I^{-1}j))^{T}refCoor$
314	gezelter	2
315	gezelter	246	if (index < atoms_.size()) {
316	gezelter	2
317	gezelter	246	Vector3d velRot;
318			Mat3x3d skewMat;;
319			Vector3d ref = refCoords_[index];
320			Vector3d ji = getJ();
321			Mat3x3d I = getI();
322	gezelter	2
323	gezelter	246	skewMat(0, 0) =0;
324			skewMat(0, 1) = ji[2] /I(2, 2);
325			skewMat(0, 2) = -ji[1] /I(1, 1);
326	gezelter	2
327	gezelter	246	skewMat(1, 0) = -ji[2] /I(2, 2);
328			skewMat(1, 1) = 0;
329			skewMat(1, 2) = ji[0]/I(0, 0);
330	gezelter	2
331	gezelter	246	skewMat(2, 0) =ji[1] /I(1, 1);
332			skewMat(2, 1) = -ji[0]/I(0, 0);
333			skewMat(2, 2) = 0;
334	gezelter	2
335	gezelter	246	velRot = (getA() * skewMat).transpose() * ref;
336	gezelter	2
337	gezelter	246	vel =getVel() + velRot;
338			return true;
339
340			} else {
341			std::cerr << index << " is an invalid index, current rigid body contains "
342			<< atoms_.size() << "atoms" << std::endl;
343			return false;
344	gezelter	2	}
345	gezelter	246	}
346	gezelter	2
347	gezelter	246	bool RigidBody::getAtomVel(Vector3d& vel, Atom* atom) {
348	gezelter	2
349	gezelter	246	std::vector<Atom*>::iterator i;
350			i = std::find(atoms_.begin(), atoms_.end(), atom);
351			if (i != atoms_.end()) {
352			return getAtomVel(vel, i - atoms_.begin());
353			} else {
354			std::cerr << "Atom " << atom->getGlobalIndex()
355			<<" does not belong to Rigid body "<< getGlobalIndex() << std::endl;
356			return false;
357			}
358			}
359	gezelter	2
360	gezelter	246	bool RigidBody::getAtomRefCoor(Vector3d& coor, unsigned int index) {
361			if (index < atoms_.size()) {
362
363			coor = refCoords_[index];
364			return true;
365			} else {
366			std::cerr << index << " is an invalid index, current rigid body contains "
367			<< atoms_.size() << "atoms" << std::endl;
368			return false;
369	gezelter	2	}
370
371			}
372
373	gezelter	246	bool RigidBody::getAtomRefCoor(Vector3d& coor, Atom* atom) {
374			std::vector<Atom*>::iterator i;
375			i = std::find(atoms_.begin(), atoms_.end(), atom);
376			if (i != atoms_.end()) {
377			//RigidBody class makes sure refCoords_ and atoms_ match each other
378			coor = refCoords_[i - atoms_.begin()];
379			return true;
380			} else {
381			std::cerr << "Atom " << atom->getGlobalIndex()
382			<<" does not belong to Rigid body "<< getGlobalIndex() << std::endl;
383			return false;
384			}
385	gezelter	2
386			}
387
388
389	gezelter	246	void RigidBody::addAtom(Atom* at, AtomStamp* ats) {
390	gezelter	2
391	gezelter	246	Vector3d coords;
392			Vector3d euler;
393	gezelter	2
394
395	gezelter	246	atoms_.push_back(at);
396
397			if( !ats->havePosition() ){
398			sprintf( painCave.errMsg,
399			"RigidBody error.\n"
400			"\tAtom %s does not have a position specified.\n"
401			"\tThis means RigidBody cannot set up reference coordinates.\n",
402			ats->getType() );
403			painCave.isFatal = 1;
404			simError();
405	gezelter	2	}
406
407	gezelter	246	coords[0] = ats->getPosX();
408			coords[1] = ats->getPosY();
409			coords[2] = ats->getPosZ();
410	gezelter	2
411	gezelter	246	refCoords_.push_back(coords);
412	gezelter	2
413	gezelter	246	RotMat3x3d identMat = RotMat3x3d::identity();
414	gezelter	2
415	gezelter	246	if (at->isDirectional()) {
416	gezelter	2
417	gezelter	246	if( !ats->haveOrientation() ){
418			sprintf( painCave.errMsg,
419			"RigidBody error.\n"
420			"\tAtom %s does not have an orientation specified.\n"
421			"\tThis means RigidBody cannot set up reference orientations.\n",
422			ats->getType() );
423			painCave.isFatal = 1;
424			simError();
425			}
426
427			euler[0] = ats->getEulerPhi();
428			euler[1] = ats->getEulerTheta();
429			euler[2] = ats->getEulerPsi();
430	gezelter	2
431	gezelter	246	RotMat3x3d Atmp(euler);
432			refOrients_.push_back(Atmp);
433	gezelter	2
434	gezelter	246	}else {
435			refOrients_.push_back(identMat);
436	gezelter	2	}
437
438
439			}
440
441			}
442