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_).unitFrame[localIndex_] = a.transpose() * sU_;

    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_).unitFrame[localIndex_] = a.transpose() * sU_;

    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_).unitFrame[localIndex_] = a.transpose() * sU_;    

    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 afrc;
    Vector3d atrq;
    Vector3d rpos;
    Vector3d frc;
    Vector3d trq;


    zeroForces();
    
    frc = getFrc();
    trq = getTrq();

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

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

        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:	1782
Committed:	Wed Nov 24 20:55:03 2004 UTC (19 years, 9 months ago) by tim
File size:	10935 byte(s)
Log Message:	types and primitives get built
#	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	tim	1782	((snapshotMan_->getPrevSnapshot())->storage_).unitFrame[localIndex_] = a.transpose() sU_;
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	tim	1782	((snapshotMan_->getCurrentSnapshot())->storage_).unitFrame[localIndex_] = a.transpose() sU_;
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	tim	1782	((snapshotMan_->getSnapshot(snapshotNo))->storage_).unitFrame[localIndex_] = a.transpose() sU_;
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 afrc;
254			Vector3d atrq;
255			Vector3d rpos;
256			Vector3d frc;
257			Vector3d trq;
258	gezelter	1490
259	tim	1738
260	tim	1692	zeroForces();
261
262			frc = getFrc();
263			trq = getTrq();
264	gezelter	1490
265	tim	1692	for (i = 0; i < atoms_.size(); i++) {
266	gezelter	1490
267	tim	1692	afrc = atoms_[i]->getFrc();
268	gezelter	1490
269	tim	1692	rpos = refCoords_[i];
270
271			frc += afrc;
272	gezelter	1490
273	tim	1692	trq[0] += rpos[1]afrc[2] - rpos[2]afrc[1];
274			trq[1] += rpos[2]afrc[0] - rpos[0]afrc[2];
275			trq[2] += rpos[0]afrc[1] - rpos[1]afrc[0];
276	gezelter	1490
277	tim	1692	// If the atom has a torque associated with it, then we also need to
278			// migrate the torques onto the center of mass:
279	gezelter	1490
280	tim	1692	if (atoms_[i]->isDirectional()) {
281			atrq = atoms_[i]->getTrq();
282			trq += atrq;
283			}
284
285	gezelter	1490	}
286
287	tim	1692	setFrc(frc);
288			setTrq(trq);
289
290	gezelter	1490	}
291
292	tim	1692	void RigidBody::updateAtoms() {
293			unsigned int i;
294			unsigned int j;
295			Vector3d ref;
296			Vector3d apos;
297			DirectionalAtom* dAtom;
298			Vector3d pos = getPos();
299			RotMat3x3d A = getA();
300
301			for (i = 0; i < atoms_.size(); i++) {
302	gezelter	1490
303	tim	1692	ref = body2Lab(refCoords_[i]);
304	gezelter	1490
305	tim	1692	apos = pos + ref;
306	gezelter	1490
307	tim	1692	atoms_[i]->setPos(apos);
308	gezelter	1490
309	tim	1692	if (atoms_[i]->isDirectional()) {
310
311			dAtom = (DirectionalAtom *) atoms_[i];
312			dAtom->rotateBy( A );
313			}
314	gezelter	1490
315	tim	1692	}
316	gezelter	1490
317			}
318
319
320	tim	1692	bool RigidBody::getAtomPos(Vector3d& pos, unsigned int index) {
321			if (index < atoms_.size()) {
322	gezelter	1490
323	tim	1692	Vector3d ref = body2Lab(refCoords_[index]);
324			pos = getPos() + ref;
325			return true;
326			} else {
327			std::cerr << index << " is an invalid index, current rigid body contains "
328			<< atoms_.size() << "atoms" << std::endl;
329			return false;
330			}
331	gezelter	1490	}
332
333	tim	1692	bool RigidBody::getAtomPos(Vector3d& pos, Atom* atom) {
334			std::vector<Atom*>::iterator i;
335			i = find(atoms_.begin(), atoms_.end(), atom);
336			if (i != atoms_.end()) {
337			//RigidBody class makes sure refCoords_ and atoms_ match each other
338			Vector3d ref = body2Lab(refCoords_[i - atoms_.begin()]);
339			pos = getPos() + ref;
340			return true;
341			} else {
342			std::cerr << "Atom " << atom->getGlobalIndex()
343			<<" does not belong to Rigid body "<< getGlobalIndex() << std::endl;
344			return false;
345			}
346	gezelter	1490	}
347	tim	1692	bool RigidBody::getAtomVel(Vector3d& vel, unsigned int index) {
348	gezelter	1490
349	tim	1692	//velRot = $(A\cdot skew(I^{-1}j))^{T}refCoor$
350	gezelter	1490
351	tim	1692	if (index < atoms_.size()) {
352	gezelter	1490
353	tim	1692	Vector3d velRot;
354			Mat3x3d skewMat;;
355			Vector3d ref = refCoords_[index];
356			Vector3d ji = getJ();
357			Mat3x3d I = getI();
358	gezelter	1490
359	tim	1692	skewMat(0, 0) =0;
360			skewMat(0, 1) = ji[2] /I(2, 2);
361			skewMat(0, 2) = -ji[1] /I(1, 1);
362	gezelter	1490
363	tim	1692	skewMat(1, 0) = -ji[2] /I(2, 2);
364			skewMat(1, 1) = 0;
365			skewMat(1, 2) = ji[0]/I(0, 0);
366	gezelter	1490
367	tim	1692	skewMat(2, 0) =ji[1] /I(1, 1);
368			skewMat(2, 1) = -ji[0]/I(0, 0);
369			skewMat(2, 2) = 0;
370	gezelter	1490
371	tim	1692	velRot = (getA() * skewMat).transpose() * ref;
372	gezelter	1490
373	tim	1692	vel =getVel() + velRot;
374			return true;
375
376			} else {
377			std::cerr << index << " is an invalid index, current rigid body contains "
378			<< atoms_.size() << "atoms" << std::endl;
379			return false;
380			}
381			}
382	gezelter	1490
383	tim	1692	bool RigidBody::getAtomVel(Vector3d& vel, Atom* atom) {
384	gezelter	1490
385	tim	1692	std::vector<Atom*>::iterator i;
386			i = find(atoms_.begin(), atoms_.end(), atom);
387			if (i != atoms_.end()) {
388			return getAtomVel(vel, i - atoms_.begin());
389			} else {
390			std::cerr << "Atom " << atom->getGlobalIndex()
391			<<" does not belong to Rigid body "<< getGlobalIndex() << std::endl;
392			return false;
393			}
394	gezelter	1490	}
395
396	tim	1692	bool RigidBody::getAtomRefCoor(Vector3d& coor, unsigned int index) {
397			if (index < atoms_.size()) {
398	gezelter	1490
399	tim	1692	coor = refCoords_[index];
400			return true;
401			} else {
402			std::cerr << index << " is an invalid index, current rigid body contains "
403			<< atoms_.size() << "atoms" << std::endl;
404			return false;
405			}
406	gezelter	1490
407	tim	1692	}
408	gezelter	1490
409	tim	1692	bool RigidBody::getAtomRefCoor(Vector3d& coor, Atom* atom) {
410			std::vector<Atom*>::iterator i;
411			i = find(atoms_.begin(), atoms_.end(), atom);
412			if (i != atoms_.end()) {
413			//RigidBody class makes sure refCoords_ and atoms_ match each other
414			coor = refCoords_[i - atoms_.begin()];
415			return true;
416			} else {
417			std::cerr << "Atom " << atom->getGlobalIndex()
418			<<" does not belong to Rigid body "<< getGlobalIndex() << std::endl;
419			return false;
420			}
421	gezelter	1490
422	tim	1692	}
423	gezelter	1490
424			}
425