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() : objType_(otRigidBody), storage_(&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_;
}    

void RigidBody::setI(Mat3x3d& I) {
    inertiaTensor_ = I;
}    

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; 
    }
}
bool RigidBody::getAtomVel(Vector3d& vel, unsigned int index) {

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

    if (index < atoms_.size() {

        Vector3d ref;
        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;
        
    } else {
        std::cerr << "Atom " << atom->getGlobalIndex() 
                      <<" does not belong to Rigid body "<< getGlobalIndex() << 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:	1684
Committed:	Fri Oct 29 16:20:50 2004 UTC (19 years, 8 months ago) by tim
File size:	11136 byte(s)
Log Message:	More painful reconstruction is coming !!!
#	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			RigidBody::RigidBody() : objType_(otRigidBody), storage_(&Snapshot::rigidbodyData){
31
32			}
33
34	tim	1682	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	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			(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			(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			void RigidBody::setI(Mat3x3d& I) {
82			inertiaTensor_ = I;
83			}
84
85			std::vector<double> RigidBody::getGrad() {
86			vector<double> grad(6, 0.0);
87			Vector3d force;
88			Vector3d torque;
89			Vector3d myEuler;
90			double phi, theta, psi;
91			double cphi, sphi, ctheta, stheta;
92			Vector3d ephi;
93			Vector3d etheta;
94			Vector3d epsi;
95
96			force = getFrc();
97			torque =getTrq();
98			myEuler = getA().toEulerAngles();
99
100			phi = myEuler[0];
101			theta = myEuler[1];
102			psi = myEuler[2];
103
104			cphi = cos(phi);
105			sphi = sin(phi);
106			ctheta = cos(theta);
107			stheta = sin(theta);
108
109			// get unit vectors along the phi, theta and psi rotation axes
110
111			ephi[0] = 0.0;
112			ephi[1] = 0.0;
113			ephi[2] = 1.0;
114
115			etheta[0] = cphi;
116			etheta[1] = sphi;
117			etheta[2] = 0.0;
118
119			epsi[0] = stheta * cphi;
120			epsi[1] = stheta * sphi;
121			epsi[2] = ctheta;
122
123			//gradient is equal to -force
124			for (int j = 0 ; j<3; j++)
125			grad[j] = -force[j];
126
127			for (int j = 0; j < 3; j++ ) {
128
129			grad[3] += torque[j]*ephi[j];
130			grad[4] += torque[j]*etheta[j];
131			grad[5] += torque[j]*epsi[j];
132
133			}
134
135			return grad;
136			}
137
138			void RigidBody::accept(BaseVisitor* v) {
139			v->visit(this);
140			}
141
142	tim	1684	void RigidBody::calcRefCoords() {
143			/*
144			double mtmp;
145			vec3 apos;
146			double refCOM[3];
147			vec3 ptmp;
148			double Itmp[3][3];
149			double evals[3];
150			double evects[3][3];
151			double r, r2, len;
152
153			// First, find the center of mass:
154
155			mass = 0.0;
156			for (j=0; j<3; j++)
157			refCOM[j] = 0.0;
158
159			for (i = 0; i < atoms_.size(); i++) {
160			mtmp = atoms_[i]->getMass();
161			mass += mtmp;
162
163			apos = refCoords[i];
164
165			for(j = 0; j < 3; j++) {
166			refCOM[j] += apos[j]*mtmp;
167			}
168			}
169
170			for(j = 0; j < 3; j++)
171			refCOM[j] /= mass;
172
173			// Next, move the origin of the reference coordinate system to the COM:
174
175			for (i = 0; i < atoms_.size(); i++) {
176			apos = refCoords[i];
177			for (j=0; j < 3; j++) {
178			apos[j] = apos[j] - refCOM[j];
179			}
180			refCoords[i] = apos;
181			}
182
183			// Moment of Inertia calculation
184
185			for (i = 0; i < 3; i++)
186			for (j = 0; j < 3; j++)
187			Itmp[i][j] = 0.0;
188
189			for (it = 0; it < atoms_.size(); it++) {
190
191			mtmp = atoms_[it]->getMass();
192			ptmp = refCoords[it];
193			r= norm3(ptmp.vec);
194			r2 = r*r;
195
196			for (i = 0; i < 3; i++) {
197			for (j = 0; j < 3; j++) {
198
199			if (i==j) Itmp[i][j] += mtmp * r2;
200
201			Itmp[i][j] -= mtmp * ptmp.vec[i]*ptmp.vec[j];
202			}
203			}
204			}
205
206			diagonalize3x3(Itmp, evals, sU);
207
208			// zero out I and then fill the diagonals with the moments of inertia:
209
210			n_linear_coords = 0;
211
212			for (i = 0; i < 3; i++) {
213			for (j = 0; j < 3; j++) {
214			I[i][j] = 0.0;
215			}
216			I[i][i] = evals[i];
217
218			if (fabs(evals[i]) < momIntTol) {
219			is_linear = true;
220			n_linear_coords++;
221			linear_axis = i;
222			}
223			}
224
225			if (n_linear_coords > 1) {
226			sprintf( painCave.errMsg,
227			"RigidBody error.\n"
228			"\tOOPSE found more than one axis in this rigid body with a vanishing \n"
229			"\tmoment of inertia. This can happen in one of three ways:\n"
230			"\t 1) Only one atom was specified, or \n"
231			"\t 2) All atoms were specified at the same location, or\n"
232			"\t 3) The programmers did something stupid.\n"
233			"\tIt is silly to use a rigid body to describe this situation. Be smarter.\n"
234			);
235			painCave.isFatal = 1;
236			simError();
237			}
238
239			// renormalize column vectors:
240
241			for (i=0; i < 3; i++) {
242			len = 0.0;
243			for (j = 0; j < 3; j++) {
244			len += sU[i][j]*sU[i][j];
245			}
246			len = sqrt(len);
247			for (j = 0; j < 3; j++) {
248			sU[i][j] /= len;
249			}
250			}
251			*/
252	tim	1640	}
253
254	tim	1684	void RigidBody::calcForcesAndTorques() {
255			unsigned int i;
256			unsigned int j;
257			//Vector3d apos;
258			Vector3d afrc;
259			Vector3d atrq;
260			Vector3d rpos;
261			Vector3d frc;
262			Vector3d trq;
263			//Vector3d pos;
264
265			zeroForces();
266
267			//pos = getPos();
268			frc = getFrc();
269			trq = getTrq();
270
271			for (i = 0; i < atoms_.size(); i++) {
272
273			afrc = atoms_[i]->getFrc();
274
275			//apos = atoms_[i]->getPos(apos);
276			//rpos = apos - pos;
277			rpos = refCoords_[i];
278
279			frc += afrc;
280
281			trq[0] += rpos[1]afrc[2] - rpos[2]afrc[1];
282			trq[1] += rpos[2]afrc[0] - rpos[0]afrc[2];
283			trq[2] += rpos[0]afrc[1] - rpos[1]afrc[0];
284
285			// If the atom has a torque associated with it, then we also need to
286			// migrate the torques onto the center of mass:
287
288			if (atoms_[i]->isDirectional()) {
289			atrq = atoms_[i]->getTrq();
290			trq += atrq;
291			}
292
293			}
294
295			setFrc(frc);
296			setTrq(trq);
297
298			}
299
300			void RigidBody::updateAtoms() {
301			unsigned int i;
302			unsigned int j;
303			Vector3d ref;
304			Vector3d apos;
305			DirectionalAtom* dAtom;
306			Vector3d pos = getPos();
307			RotMat3x3d A = getA();
308
309			for (i = 0; i < atoms_.size(); i++) {
310
311			ref = body2Lab(refCoords_[i]);
312
313			apos = pos + ref;
314
315			atoms_[i]->setPos(apos);
316
317			if (atoms_[i]->isDirectional()) {
318
319			dAtom = (DirectionalAtom *) atoms_[i];
320			dAtom->rotateBy( A );
321			}
322
323			}
324
325			}
326
327
328			bool RigidBody::getAtomPos(Vector3d& pos, unsigned int index) {
329			if (index < atoms_.size() {
330
331			Vector3d ref = body2Lab(refCoords_[index]);
332			pos = getPos() + ref;
333			return true
334			} else {
335			std::cerr << index << " is an invalid index, current rigid body contains "
336			<< atoms_.size() << "atoms" << std::endl;
337			return false;
338			}
339			}
340
341			bool RigidBody::getAtomPos(Vector3d& pos, Atom* atom) {
342			std::vector<Atom*>::iterator i;
343			i = find(atoms_.begin(), atoms_.end(), atom);
344			if (i != atoms_.end()) {
345			//RigidBody class makes sure refCoords_ and atoms_ match each other
346			Vector3d ref = body2Lab(refCoords_[i - atoms_.begin()]);
347			pos = getPos() + ref;
348			return true;
349			} else {
350			std::cerr << "Atom " << atom->getGlobalIndex()
351			<<" does not belong to Rigid body "<< getGlobalIndex() << std::endl;
352			}
353			}
354			bool RigidBody::getAtomVel(Vector3d& vel, unsigned int index) {
355
356			//velRot = $(A\cdot skew(I^{-1}j))^{T}refCoor$
357
358			if (index < atoms_.size() {
359
360			Vector3d ref;
361			Vector3d velRot;
362			Mat3x3d skewMat;;
363			Vector3d ref = refCoords_[index];
364			Vector3d ji = getJ();
365			Mat3x3d I = getI();
366
367			skewMat(0, 0) =0;
368			skewMat(0, 1) = ji[2] /I(2, 2);
369			skewMat(0, 2) = -ji[1] /I(1, 1);
370
371			skewMat(1, 0) = -ji[2] /I(2, 2);
372			skewMat(1, 1) = 0;
373			skewMat(1, 2) = ji[0]/I(0, 0);
374
375			skewMat(2, 0) =ji[1] /I(1, 1);
376			skewMat(2, 1) = -ji[0]/I(0, 0);
377			skewMat(2, 2) = 0;
378
379			velRot = (getA() * skewMat).transpose() * ref;
380
381			vel =getVel() + velRot;
382
383			} else {
384			std::cerr << "Atom " << atom->getGlobalIndex()
385			<<" does not belong to Rigid body "<< getGlobalIndex() << std::endl;
386			return false;
387			}
388			}
389
390			bool RigidBody::getAtomVel(Vector3d& vel, Atom* atom) {
391
392			std::vector<Atom*>::iterator i;
393			i = find(atoms_.begin(), atoms_.end(), atom);
394			if (i != atoms_.end()) {
395			return getAtomVel(vel, i - atoms_.begin());
396			} else {
397			std::cerr << "Atom " << atom->getGlobalIndex()
398			<<" does not belong to Rigid body "<< getGlobalIndex() << std::endl;
399			return false;
400			}
401			}
402
403			bool RigidBody::getAtomRefCoor(Vector3d& coor, unsigned int index) {
404			if (index < atoms_.size() {
405
406			coor = refCoords_[index];
407			return true
408			} else {
409			std::cerr << index << " is an invalid index, current rigid body contains "
410			<< atoms_.size() << "atoms" << std::endl;
411			return false;
412			}
413
414			}
415
416			bool RigidBody::getAtomRefCoor(Vector3d& coor, Atom* atom) {
417			std::vector<Atom*>::iterator i;
418			i = find(atoms_.begin(), atoms_.end(), atom);
419			if (i != atoms_.end()) {
420			//RigidBody class makes sure refCoords_ and atoms_ match each other
421			coor = refCoords_[i - atoms_.begin()];
422			return true;
423			} else {
424			std::cerr << "Atom " << atom->getGlobalIndex()
425			<<" does not belong to Rigid body "<< getGlobalIndex() << std::endl;
426			return false;
427			}
428
429			}
430
431			}
432