test/brains/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:	1691
Committed:	Mon Nov 1 19:57:07 2004 UTC (19 years, 8 months ago) by tim
File size:	11101 byte(s)
Log Message:	these core classes get compiled
#	User	Rev	Content
1	tim	1640	/*
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			#include "primitives/RigidBody.hpp"
27
28			namespace oopse {
29
30	tim	1691	RigidBody::RigidBody() : StuntDouble(otRigidBody, &Snapshot::rigidbodyData){
31	tim	1640
32			}
33
34	tim	1682	void RigidBody::setPrevA(const RotMat3x3d& a) {
35	tim	1691	((snapshotMan_->getPrevSnapshot())->*storage_).aMat[localIndex_] = a;
36			((snapshotMan_->getPrevSnapshot())->storage_).unitVector[localIndex_] = a.inverse() sU_.getColum(2);
37	tim	1684
38			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
45	tim	1682	}
46
47
48			void RigidBody::setA(const RotMat3x3d& a) {
49	tim	1691	((snapshotMan_->getCurrentSnapshot())->*storage_).aMat[localIndex_] = a;
50			((snapshotMan_->getCurrentSnapshot())->storage_).unitVector[localIndex_] = a.inverse() sU_.getColum(2);
51	tim	1684
52			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	tim	1682	}
59
60			void RigidBody::setA(const RotMat3x3d& a, int snapshotNo) {
61	tim	1691	((snapshotMan_->getSnapshot(snapshotNo))->*storage_).aMat[localIndex_] = a;
62			((snapshotMan_->getSnapshot(snapshotNo))->storage_).unitVector[localIndex_] = a.inverse() sU_.getColum(2);
63	tim	1684
64			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			}
70
71	tim	1682	}
72
73	tim	1684	void DirectionalAtom::setUnitFrameFromEuler(double phi, double theta, double psi) {
74			sU_.setupRotMat(phi,theta,psi);
75			}
76
77	tim	1640	Mat3x3d RigidBody::getI() {
78			return inertiaTensor_;
79			}
80
81			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
92			force = getFrc();
93			torque =getTrq();
94			myEuler = getA().toEulerAngles();
95
96			phi = myEuler[0];
97			theta = myEuler[1];
98			psi = myEuler[2];
99
100			cphi = cos(phi);
101			sphi = sin(phi);
102			ctheta = cos(theta);
103			stheta = sin(theta);
104
105			// get unit vectors along the phi, theta and psi rotation axes
106
107			ephi[0] = 0.0;
108			ephi[1] = 0.0;
109			ephi[2] = 1.0;
110
111			etheta[0] = cphi;
112			etheta[1] = sphi;
113			etheta[2] = 0.0;
114
115			epsi[0] = stheta * cphi;
116			epsi[1] = stheta * sphi;
117			epsi[2] = ctheta;
118
119			//gradient is equal to -force
120			for (int j = 0 ; j<3; j++)
121			grad[j] = -force[j];
122
123			for (int j = 0; j < 3; j++ ) {
124
125			grad[3] += torque[j]*ephi[j];
126			grad[4] += torque[j]*etheta[j];
127			grad[5] += torque[j]*epsi[j];
128
129			}
130
131			return grad;
132			}
133
134			void RigidBody::accept(BaseVisitor* v) {
135			v->visit(this);
136			}
137
138	tim	1684	void RigidBody::calcRefCoords() {
139			/*
140			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			for (i = 0; i < atoms_.size(); i++) {
156			mtmp = atoms_[i]->getMass();
157			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			for (i = 0; i < atoms_.size(); i++) {
172			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			for (it = 0; it < atoms_.size(); it++) {
186
187			mtmp = atoms_[it]->getMass();
188			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			*/
248	tim	1640	}
249
250	tim	1684	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
261			zeroForces();
262
263			//pos = getPos();
264			frc = getFrc();
265			trq = getTrq();
266
267			for (i = 0; i < atoms_.size(); i++) {
268
269			afrc = atoms_[i]->getFrc();
270
271			//apos = atoms_[i]->getPos(apos);
272			//rpos = apos - pos;
273			rpos = refCoords_[i];
274
275			frc += afrc;
276
277			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
281			// If the atom has a torque associated with it, then we also need to
282			// migrate the torques onto the center of mass:
283
284			if (atoms_[i]->isDirectional()) {
285			atrq = atoms_[i]->getTrq();
286			trq += atrq;
287			}
288
289			}
290
291			setFrc(frc);
292			setTrq(trq);
293
294			}
295
296			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
307			ref = body2Lab(refCoords_[i]);
308
309			apos = pos + ref;
310
311			atoms_[i]->setPos(apos);
312
313			if (atoms_[i]->isDirectional()) {
314
315			dAtom = (DirectionalAtom *) atoms_[i];
316			dAtom->rotateBy( A );
317			}
318
319			}
320
321			}
322
323
324			bool RigidBody::getAtomPos(Vector3d& pos, unsigned int index) {
325	tim	1691	if (index < atoms_.size()) {
326	tim	1684
327			Vector3d ref = body2Lab(refCoords_[index]);
328			pos = getPos() + ref;
329	tim	1691	return true;
330	tim	1684	} else {
331			std::cerr << index << " is an invalid index, current rigid body contains "
332			<< atoms_.size() << "atoms" << std::endl;
333			return false;
334			}
335			}
336
337			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	tim	1691	return false;
349	tim	1684	}
350			}
351			bool RigidBody::getAtomVel(Vector3d& vel, unsigned int index) {
352
353			//velRot = $(A\cdot skew(I^{-1}j))^{T}refCoor$
354
355	tim	1691	if (index < atoms_.size()) {
356	tim	1684
357			Vector3d velRot;
358			Mat3x3d skewMat;;
359			Vector3d ref = refCoords_[index];
360			Vector3d ji = getJ();
361			Mat3x3d I = getI();
362
363			skewMat(0, 0) =0;
364			skewMat(0, 1) = ji[2] /I(2, 2);
365			skewMat(0, 2) = -ji[1] /I(1, 1);
366
367			skewMat(1, 0) = -ji[2] /I(2, 2);
368			skewMat(1, 1) = 0;
369			skewMat(1, 2) = ji[0]/I(0, 0);
370
371			skewMat(2, 0) =ji[1] /I(1, 1);
372			skewMat(2, 1) = -ji[0]/I(0, 0);
373			skewMat(2, 2) = 0;
374
375			velRot = (getA() * skewMat).transpose() * ref;
376
377			vel =getVel() + velRot;
378	tim	1691	return true;
379	tim	1684
380			} else {
381	tim	1691	std::cerr << index << " is an invalid index, current rigid body contains "
382			<< atoms_.size() << "atoms" << std::endl;
383	tim	1684	return false;
384			}
385			}
386
387			bool RigidBody::getAtomVel(Vector3d& vel, Atom* atom) {
388
389			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			}
399
400			bool RigidBody::getAtomRefCoor(Vector3d& coor, unsigned int index) {
401	tim	1691	if (index < atoms_.size()) {
402	tim	1684
403			coor = refCoords_[index];
404	tim	1691	return true;
405	tim	1684	} else {
406			std::cerr << index << " is an invalid index, current rigid body contains "
407			<< atoms_.size() << "atoms" << std::endl;
408			return false;
409			}
410
411			}
412
413			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
426			}
427
428			}
429