src/primitives/RigidBody.cpp

/*
 * Copyright (C) 2000-2004  Object Oriented Parallel Simulation Engine (OOPSE) project
 * 
 * Contact: oopse@oopse.org
 * 
 * This program is free software; you can redistribute it and/or
 * modify it under the terms of the GNU Lesser General Public License
 * as published by the Free Software Foundation; either version 2.1
 * of the License, or (at your option) any later version.
 * All we ask is that proper credit is given for our work, which includes
 * - but is not limited to - adding the above copyright notice to the beginning
 * of your source code files, and to any copyright notice that you may distribute
 * with programs based on this work.
 * 
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU Lesser General Public License for more details.
 * 
 * You should have received a copy of the GNU Lesser General Public License
 * along with this program; if not, write to the Free Software
 * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.
 *
 */

#include "primitives/RigidBody.hpp"

namespace oopse {

RigidBody::RigidBody() : StuntDouble(otRigidBody, &Snapshot::rigidbodyData){

}

void RigidBody::setPrevA(const RotMat3x3d& a) {
    ((snapshotMan_->getPrevSnapshot())->*storage_).aMat[localIndex_] = a;
    ((snapshotMan_->getPrevSnapshot())->*storage_).unitVector[localIndex_] = a.inverse() * sU_.getColum(2);

    std::vector<Atom*>::iterator i;
    for (i = atoms_.begin(); i != atoms_.end(); ++i) {
        if ((*i)->isDirectional()) {
            (*i)->setPrevA(a * (*i)->getPrevA());
        }
    }

}

      
void RigidBody::setA(const RotMat3x3d& a) {
    ((snapshotMan_->getCurrentSnapshot())->*storage_).aMat[localIndex_] = a;
    ((snapshotMan_->getCurrentSnapshot())->*storage_).unitVector[localIndex_] = a.inverse() * sU_.getColum(2);

    std::vector<Atom*>::iterator i;
    for (i = atoms_.begin(); i != atoms_.end(); ++i) {
        if ((*i)->isDirectional()) {
            (*i)->setA(a * (*i)->getA());
        }
    }
}    
    
void RigidBody::setA(const RotMat3x3d& a, int snapshotNo) {
    ((snapshotMan_->getSnapshot(snapshotNo))->*storage_).aMat[localIndex_] = a;
    ((snapshotMan_->getSnapshot(snapshotNo))->*storage_).unitVector[localIndex_] = a.inverse() * sU_.getColum(2);    

    std::vector<Atom*>::iterator i;
    for (i = atoms_.begin(); i != atoms_.end(); ++i) {
        if ((*i)->isDirectional()) {
            (*i)->setA(a * (*i)->getA(snapshotNo), snapshotNo);
        }
    }

}   

void  DirectionalAtom::setUnitFrameFromEuler(double phi, double theta, double psi) {
    sU_.setupRotMat(phi,theta,psi);
}

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

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

void  RigidBody::calcRefCoords() {
  /*
  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 < atoms_.size(); i++) {
    mtmp = atoms_[i]->getMass();
    mass += mtmp;

    apos = refCoords[i];
    
    for(j = 0; j < 3; j++) {
      refCOM[j] += apos[j]*mtmp;     
    }    
  }
  
  for(j = 0; j < 3; j++) 
    refCOM[j] /= mass;

// Next, move the origin of the reference coordinate system to the COM:

  for (i = 0; i < atoms_.size(); i++) {
    apos = refCoords[i];
    for (j=0; j < 3; j++) {
      apos[j] = apos[j] - refCOM[j];
    }
    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 < atoms_.size(); it++) {

    mtmp = atoms_[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];
      }
    }
  }
  
  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::calcForcesAndTorques() {
    unsigned int i;
    unsigned int j;
    //Vector3d apos;
    Vector3d afrc;
    Vector3d atrq;
    Vector3d rpos;
    Vector3d frc;
    Vector3d trq;
    //Vector3d pos;

    zeroForces();
    
    //pos = getPos();
    frc = getFrc();
    trq = getTrq();

    for (i = 0; i < atoms_.size(); i++) {

        afrc = atoms_[i]->getFrc();

        //apos = atoms_[i]->getPos(apos);
        //rpos = apos - pos;
        rpos = refCoords_[i];
        
        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;
    unsigned int j;
    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->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 = 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 = 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 = 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;
    }

}

}

Revision:	1692
Committed:	Mon Nov 1 20:15:58 2004 UTC (19 years, 9 months ago) by tim
File size:	11101 byte(s)
Log Message:	break, break and break.....
#	User	Rev	Content
1	tim	1692	/*
2			* Copyright (C) 2000-2004 Object Oriented Parallel Simulation Engine (OOPSE) project
3			*
4			* Contact: oopse@oopse.org
5			*
6			* This program is free software; you can redistribute it and/or
7			* modify it under the terms of the GNU Lesser General Public License
8			* as published by the Free Software Foundation; either version 2.1
9			* of the License, or (at your option) any later version.
10			* All we ask is that proper credit is given for our work, which includes
11			* - but is not limited to - adding the above copyright notice to the beginning
12			* of your source code files, and to any copyright notice that you may distribute
13			* with programs based on this work.
14			*
15			* This program is distributed in the hope that it will be useful,
16			* but WITHOUT ANY WARRANTY; without even the implied warranty of
17			* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
18			* GNU Lesser General Public License for more details.
19			*
20			* You should have received a copy of the GNU Lesser General Public License
21			* along with this program; if not, write to the Free Software
22			* Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
23			*
24			*/
25
26	tim	1492	#include "primitives/RigidBody.hpp"
27	gezelter	1490
28	tim	1692	namespace oopse {
29	gezelter	1490
30	tim	1692	RigidBody::RigidBody() : StuntDouble(otRigidBody, &Snapshot::rigidbodyData){
31
32	gezelter	1490	}
33
34	tim	1692	void RigidBody::setPrevA(const RotMat3x3d& a) {
35			((snapshotMan_->getPrevSnapshot())->*storage_).aMat[localIndex_] = a;
36			((snapshotMan_->getPrevSnapshot())->storage_).unitVector[localIndex_] = a.inverse() sU_.getColum(2);
37	gezelter	1490
38	tim	1692	std::vector<Atom*>::iterator i;
39			for (i = atoms_.begin(); i != atoms_.end(); ++i) {
40			if ((*i)->isDirectional()) {
41			(i)->setPrevA(a (*i)->getPrevA());
42			}
43			}
44	gezelter	1490
45			}
46
47	tim	1692
48			void RigidBody::setA(const RotMat3x3d& a) {
49			((snapshotMan_->getCurrentSnapshot())->*storage_).aMat[localIndex_] = a;
50			((snapshotMan_->getCurrentSnapshot())->storage_).unitVector[localIndex_] = a.inverse() sU_.getColum(2);
51	gezelter	1490
52	tim	1692	std::vector<Atom*>::iterator i;
53			for (i = atoms_.begin(); i != atoms_.end(); ++i) {
54			if ((*i)->isDirectional()) {
55			(i)->setA(a (*i)->getA());
56			}
57			}
58	gezelter	1490	}
59
60	tim	1692	void RigidBody::setA(const RotMat3x3d& a, int snapshotNo) {
61			((snapshotMan_->getSnapshot(snapshotNo))->*storage_).aMat[localIndex_] = a;
62			((snapshotMan_->getSnapshot(snapshotNo))->storage_).unitVector[localIndex_] = a.inverse() sU_.getColum(2);
63	gezelter	1490
64	tim	1692	std::vector<Atom*>::iterator i;
65			for (i = atoms_.begin(); i != atoms_.end(); ++i) {
66			if ((*i)->isDirectional()) {
67			(i)->setA(a (*i)->getA(snapshotNo), snapshotNo);
68			}
69	gezelter	1490	}
70
71	tim	1692	}
72	gezelter	1490
73	tim	1692	void DirectionalAtom::setUnitFrameFromEuler(double phi, double theta, double psi) {
74			sU_.setupRotMat(phi,theta,psi);
75	gezelter	1490	}
76
77	tim	1692	Mat3x3d RigidBody::getI() {
78			return inertiaTensor_;
79			}
80	gezelter	1490
81	tim	1692	std::vector<double> RigidBody::getGrad() {
82			vector<double> grad(6, 0.0);
83			Vector3d force;
84			Vector3d torque;
85			Vector3d myEuler;
86			double phi, theta, psi;
87			double cphi, sphi, ctheta, stheta;
88			Vector3d ephi;
89			Vector3d etheta;
90			Vector3d epsi;
91	gezelter	1490
92	tim	1692	force = getFrc();
93			torque =getTrq();
94			myEuler = getA().toEulerAngles();
95	gezelter	1490
96	tim	1692	phi = myEuler[0];
97			theta = myEuler[1];
98			psi = myEuler[2];
99	gezelter	1490
100	tim	1692	cphi = cos(phi);
101			sphi = sin(phi);
102			ctheta = cos(theta);
103			stheta = sin(theta);
104	gezelter	1490
105	tim	1692	// get unit vectors along the phi, theta and psi rotation axes
106	gezelter	1490
107	tim	1692	ephi[0] = 0.0;
108			ephi[1] = 0.0;
109			ephi[2] = 1.0;
110	gezelter	1490
111	tim	1692	etheta[0] = cphi;
112			etheta[1] = sphi;
113			etheta[2] = 0.0;
114	gezelter	1490
115	tim	1692	epsi[0] = stheta * cphi;
116			epsi[1] = stheta * sphi;
117			epsi[2] = ctheta;
118	gezelter	1490
119	tim	1692	//gradient is equal to -force
120			for (int j = 0 ; j<3; j++)
121			grad[j] = -force[j];
122	gezelter	1490
123	tim	1692	for (int j = 0; j < 3; j++ ) {
124	gezelter	1490
125	tim	1692	grad[3] += torque[j]*ephi[j];
126			grad[4] += torque[j]*etheta[j];
127			grad[5] += torque[j]*epsi[j];
128	gezelter	1490
129	tim	1692	}
130
131			return grad;
132			}
133	gezelter	1490
134	tim	1692	void RigidBody::accept(BaseVisitor* v) {
135			v->visit(this);
136			}
137	gezelter	1490
138	tim	1692	void RigidBody::calcRefCoords() {
139			/*
140	gezelter	1490	double mtmp;
141			vec3 apos;
142			double refCOM[3];
143			vec3 ptmp;
144			double Itmp[3][3];
145			double evals[3];
146			double evects[3][3];
147			double r, r2, len;
148
149			// First, find the center of mass:
150
151			mass = 0.0;
152			for (j=0; j<3; j++)
153			refCOM[j] = 0.0;
154
155	tim	1692	for (i = 0; i < atoms_.size(); i++) {
156			mtmp = atoms_[i]->getMass();
157	gezelter	1490	mass += mtmp;
158
159			apos = refCoords[i];
160
161			for(j = 0; j < 3; j++) {
162			refCOM[j] += apos[j]*mtmp;
163			}
164			}
165
166			for(j = 0; j < 3; j++)
167			refCOM[j] /= mass;
168
169			// Next, move the origin of the reference coordinate system to the COM:
170
171	tim	1692	for (i = 0; i < atoms_.size(); i++) {
172	gezelter	1490	apos = refCoords[i];
173			for (j=0; j < 3; j++) {
174			apos[j] = apos[j] - refCOM[j];
175			}
176			refCoords[i] = apos;
177			}
178
179			// Moment of Inertia calculation
180
181			for (i = 0; i < 3; i++)
182			for (j = 0; j < 3; j++)
183			Itmp[i][j] = 0.0;
184
185	tim	1692	for (it = 0; it < atoms_.size(); it++) {
186	gezelter	1490
187	tim	1692	mtmp = atoms_[it]->getMass();
188	gezelter	1490	ptmp = refCoords[it];
189			r= norm3(ptmp.vec);
190			r2 = r*r;
191
192			for (i = 0; i < 3; i++) {
193			for (j = 0; j < 3; j++) {
194
195			if (i==j) Itmp[i][j] += mtmp * r2;
196
197			Itmp[i][j] -= mtmp * ptmp.vec[i]*ptmp.vec[j];
198			}
199			}
200			}
201
202			diagonalize3x3(Itmp, evals, sU);
203
204			// zero out I and then fill the diagonals with the moments of inertia:
205
206			n_linear_coords = 0;
207
208			for (i = 0; i < 3; i++) {
209			for (j = 0; j < 3; j++) {
210			I[i][j] = 0.0;
211			}
212			I[i][i] = evals[i];
213
214			if (fabs(evals[i]) < momIntTol) {
215			is_linear = true;
216			n_linear_coords++;
217			linear_axis = i;
218			}
219			}
220
221			if (n_linear_coords > 1) {
222			sprintf( painCave.errMsg,
223			"RigidBody error.\n"
224			"\tOOPSE found more than one axis in this rigid body with a vanishing \n"
225			"\tmoment of inertia. This can happen in one of three ways:\n"
226			"\t 1) Only one atom was specified, or \n"
227			"\t 2) All atoms were specified at the same location, or\n"
228			"\t 3) The programmers did something stupid.\n"
229			"\tIt is silly to use a rigid body to describe this situation. Be smarter.\n"
230			);
231			painCave.isFatal = 1;
232			simError();
233			}
234
235			// renormalize column vectors:
236
237			for (i=0; i < 3; i++) {
238			len = 0.0;
239			for (j = 0; j < 3; j++) {
240			len += sU[i][j]*sU[i][j];
241			}
242			len = sqrt(len);
243			for (j = 0; j < 3; j++) {
244			sU[i][j] /= len;
245			}
246			}
247	tim	1692	*/
248	gezelter	1490	}
249
250	tim	1692	void RigidBody::calcForcesAndTorques() {
251			unsigned int i;
252			unsigned int j;
253			//Vector3d apos;
254			Vector3d afrc;
255			Vector3d atrq;
256			Vector3d rpos;
257			Vector3d frc;
258			Vector3d trq;
259			//Vector3d pos;
260	gezelter	1490
261	tim	1692	zeroForces();
262
263			//pos = getPos();
264			frc = getFrc();
265			trq = getTrq();
266	gezelter	1490
267	tim	1692	for (i = 0; i < atoms_.size(); i++) {
268	gezelter	1490
269	tim	1692	afrc = atoms_[i]->getFrc();
270	gezelter	1490
271	tim	1692	//apos = atoms_[i]->getPos(apos);
272			//rpos = apos - pos;
273			rpos = refCoords_[i];
274
275			frc += afrc;
276	gezelter	1490
277	tim	1692	trq[0] += rpos[1]afrc[2] - rpos[2]afrc[1];
278			trq[1] += rpos[2]afrc[0] - rpos[0]afrc[2];
279			trq[2] += rpos[0]afrc[1] - rpos[1]afrc[0];
280	gezelter	1490
281	tim	1692	// If the atom has a torque associated with it, then we also need to
282			// migrate the torques onto the center of mass:
283	gezelter	1490
284	tim	1692	if (atoms_[i]->isDirectional()) {
285			atrq = atoms_[i]->getTrq();
286			trq += atrq;
287			}
288
289	gezelter	1490	}
290
291	tim	1692	setFrc(frc);
292			setTrq(trq);
293
294	gezelter	1490	}
295
296	tim	1692	void RigidBody::updateAtoms() {
297			unsigned int i;
298			unsigned int j;
299			Vector3d ref;
300			Vector3d apos;
301			DirectionalAtom* dAtom;
302			Vector3d pos = getPos();
303			RotMat3x3d A = getA();
304
305			for (i = 0; i < atoms_.size(); i++) {
306	gezelter	1490
307	tim	1692	ref = body2Lab(refCoords_[i]);
308	gezelter	1490
309	tim	1692	apos = pos + ref;
310	gezelter	1490
311	tim	1692	atoms_[i]->setPos(apos);
312	gezelter	1490
313	tim	1692	if (atoms_[i]->isDirectional()) {
314
315			dAtom = (DirectionalAtom *) atoms_[i];
316			dAtom->rotateBy( A );
317			}
318	gezelter	1490
319	tim	1692	}
320	gezelter	1490
321			}
322
323
324	tim	1692	bool RigidBody::getAtomPos(Vector3d& pos, unsigned int index) {
325			if (index < atoms_.size()) {
326	gezelter	1490
327	tim	1692	Vector3d ref = body2Lab(refCoords_[index]);
328			pos = getPos() + ref;
329			return true;
330			} else {
331			std::cerr << index << " is an invalid index, current rigid body contains "
332			<< atoms_.size() << "atoms" << std::endl;
333			return false;
334			}
335	gezelter	1490	}
336
337	tim	1692	bool RigidBody::getAtomPos(Vector3d& pos, Atom* atom) {
338			std::vector<Atom*>::iterator i;
339			i = find(atoms_.begin(), atoms_.end(), atom);
340			if (i != atoms_.end()) {
341			//RigidBody class makes sure refCoords_ and atoms_ match each other
342			Vector3d ref = body2Lab(refCoords_[i - atoms_.begin()]);
343			pos = getPos() + ref;
344			return true;
345			} else {
346			std::cerr << "Atom " << atom->getGlobalIndex()
347			<<" does not belong to Rigid body "<< getGlobalIndex() << std::endl;
348			return false;
349			}
350	gezelter	1490	}
351	tim	1692	bool RigidBody::getAtomVel(Vector3d& vel, unsigned int index) {
352	gezelter	1490
353	tim	1692	//velRot = $(A\cdot skew(I^{-1}j))^{T}refCoor$
354	gezelter	1490
355	tim	1692	if (index < atoms_.size()) {
356	gezelter	1490
357	tim	1692	Vector3d velRot;
358			Mat3x3d skewMat;;
359			Vector3d ref = refCoords_[index];
360			Vector3d ji = getJ();
361			Mat3x3d I = getI();
362	gezelter	1490
363	tim	1692	skewMat(0, 0) =0;
364			skewMat(0, 1) = ji[2] /I(2, 2);
365			skewMat(0, 2) = -ji[1] /I(1, 1);
366	gezelter	1490
367	tim	1692	skewMat(1, 0) = -ji[2] /I(2, 2);
368			skewMat(1, 1) = 0;
369			skewMat(1, 2) = ji[0]/I(0, 0);
370	gezelter	1490
371	tim	1692	skewMat(2, 0) =ji[1] /I(1, 1);
372			skewMat(2, 1) = -ji[0]/I(0, 0);
373			skewMat(2, 2) = 0;
374	gezelter	1490
375	tim	1692	velRot = (getA() * skewMat).transpose() * ref;
376	gezelter	1490
377	tim	1692	vel =getVel() + velRot;
378			return true;
379
380			} else {
381			std::cerr << index << " is an invalid index, current rigid body contains "
382			<< atoms_.size() << "atoms" << std::endl;
383			return false;
384			}
385			}
386	gezelter	1490
387	tim	1692	bool RigidBody::getAtomVel(Vector3d& vel, Atom* atom) {
388	gezelter	1490
389	tim	1692	std::vector<Atom*>::iterator i;
390			i = find(atoms_.begin(), atoms_.end(), atom);
391			if (i != atoms_.end()) {
392			return getAtomVel(vel, i - atoms_.begin());
393			} else {
394			std::cerr << "Atom " << atom->getGlobalIndex()
395			<<" does not belong to Rigid body "<< getGlobalIndex() << std::endl;
396			return false;
397			}
398	gezelter	1490	}
399
400	tim	1692	bool RigidBody::getAtomRefCoor(Vector3d& coor, unsigned int index) {
401			if (index < atoms_.size()) {
402	gezelter	1490
403	tim	1692	coor = refCoords_[index];
404			return true;
405			} else {
406			std::cerr << index << " is an invalid index, current rigid body contains "
407			<< atoms_.size() << "atoms" << std::endl;
408			return false;
409			}
410	gezelter	1490
411	tim	1692	}
412	gezelter	1490
413	tim	1692	bool RigidBody::getAtomRefCoor(Vector3d& coor, Atom* atom) {
414			std::vector<Atom*>::iterator i;
415			i = find(atoms_.begin(), atoms_.end(), atom);
416			if (i != atoms_.end()) {
417			//RigidBody class makes sure refCoords_ and atoms_ match each other
418			coor = refCoords_[i - atoms_.begin()];
419			return true;
420			} else {
421			std::cerr << "Atom " << atom->getGlobalIndex()
422			<<" does not belong to Rigid body "<< getGlobalIndex() << std::endl;
423			return false;
424			}
425	gezelter	1490
426	tim	1692	}
427	gezelter	1490
428			}
429