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"
#include "utils/simError.h"
namespace oopse {

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

}

void RigidBody::setPrevA(const RotMat3x3d& a) {
    ((snapshotMan_->getPrevSnapshot())->*storage_).aMat[localIndex_] = a;
    ((snapshotMan_->getPrevSnapshot())->*storage_).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);
}    

/**@todo need modification */
void  RigidBody::calcRefCoords() {
    double mtmp;
    Vector3d refCOM(0.0);
    mass_ = 0.0;
    for (int i = 0; i < atoms_.size(); ++i) {
        mtmp = atoms_[i]->getMass();
        mass_ += mtmp;
        refCOM += refCoords_[i]*mtmp;
    }
    refCOM /= mass_;

    // Next, move the origin of the reference coordinate system to the COM:
    for (int i = 0; i < atoms_.size(); ++i) {
        refCoords_[i] -= refCOM;
    }

// Moment of Inertia calculation
    Mat3x3d Itmp(0.0);
  
    for (int i = 0; i < atoms_.size(); i++) {
        mtmp = atoms_[i]->getMass();
        Itmp -= outProduct(refCoords_[i], refCoords_[i]) * mtmp;
        double r2 = refCoords_[i].lengthSquare();
        Itmp(0, 0) += mtmp * r2;
        Itmp(1, 1) += mtmp * r2;
        Itmp(2, 2) += mtmp * r2;
    }

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

    // zero out I and then fill the diagonals with the moments of inertia:
    inertiaTensor_(0, 0) = evals[0];
    inertiaTensor_(1, 1) = evals[1];
    inertiaTensor_(2, 2) = evals[2];
        
    int nLinearAxis = 0;
    for (int i = 0; i < 3; i++) {    
        if (fabs(evals[i]) < oopse::epsilon) {
            linear_ = true;
            linearAxis_ = i;
            ++ nLinearAxis;
        }
    }

    if (nLinearAxis > 1) {
        sprintf( painCave.errMsg,
            "RigidBody error.\n"
            "\tOOPSE found more than one axis in this rigid body with a vanishing \n"
            "\tmoment of inertia.  This can happen in one of three ways:\n"
            "\t 1) Only one atom was specified, or \n"
            "\t 2) All atoms were specified at the same location, or\n"
            "\t 3) The programmers did something stupid.\n"
            "\tIt is silly to use a rigid body to describe this situation.  Be smarter.\n"
            );
        painCave.isFatal = 1;
        simError();
    }
  
}

void  RigidBody::calcForcesAndTorques() {
    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;
    }

}


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

  Vector3d coords;
  Vector3d euler;
  Mat3x3d Atmp;

  atoms_.push_back(at);
 
  if( !ats->havePosition() ){
    sprintf( painCave.errMsg,
             "RigidBody error.\n"
             "\tAtom %s does not have a position specified.\n"
             "\tThis means RigidBody cannot set up reference coordinates.\n",
             ats->getType() );
    painCave.isFatal = 1;
    simError();
  }
  
  coords[0] = ats->getPosX();
  coords[1] = ats->getPosY();
  coords[2] = ats->getPosZ();

  refCoords_.push_back(coords);

  /*
  if (at->isDirectional()) {   

    if( !ats->haveOrientation() ){
      sprintf( painCave.errMsg,
               "RigidBody error.\n"
               "\tAtom %s does not have an orientation specified.\n"
               "\tThis means RigidBody cannot set up reference orientations.\n",
               ats->getType() );
      painCave.isFatal = 1;
      simError();
    }    
    
    euler[0] = ats->getEulerPhi();
    euler[1] = ats->getEulerTheta();
    euler[2] = ats->getEulerPsi();
    
    doEulerToRotMat(euler, Atmp);
    refOrients.push_back(Atmp);
    
  }
  */
  
}

}

Revision:	1811
Committed:	Wed Dec 1 03:11:29 2004 UTC (19 years, 9 months ago) by tim
File size:	11590 byte(s)
Log Message:	minor change
#	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	tim	1809	#include "utils/simError.h"
28	tim	1692	namespace oopse {
29	gezelter	1490
30	tim	1811	RigidBody::RigidBody() : StuntDouble(otRigidBody, &Snapshot::rigidbodyData), inertiaTensor_(0.0){
31	tim	1692
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	1805	/*@todo need modification /
139	tim	1692	void RigidBody::calcRefCoords() {
140	tim	1811	double mtmp;
141			Vector3d refCOM(0.0);
142			mass_ = 0.0;
143			for (int i = 0; i < atoms_.size(); ++i) {
144			mtmp = atoms_[i]->getMass();
145			mass_ += mtmp;
146			refCOM += refCoords_[i]*mtmp;
147			}
148			refCOM /= mass_;
149	gezelter	1490
150	tim	1811	// Next, move the origin of the reference coordinate system to the COM:
151			for (int i = 0; i < atoms_.size(); ++i) {
152			refCoords_[i] -= refCOM;
153	gezelter	1490	}
154
155			// Moment of Inertia calculation
156	tim	1811	Mat3x3d Itmp(0.0);
157	gezelter	1490
158	tim	1811	for (int i = 0; i < atoms_.size(); i++) {
159			mtmp = atoms_[i]->getMass();
160			Itmp -= outProduct(refCoords_[i], refCoords_[i]) * mtmp;
161			double r2 = refCoords_[i].lengthSquare();
162			Itmp(0, 0) += mtmp * r2;
163			Itmp(1, 1) += mtmp * r2;
164			Itmp(2, 2) += mtmp * r2;
165	gezelter	1490	}
166
167	tim	1811	//diagonalize
168			Vector3d evals;
169			Mat3x3d::diagonalize(Itmp, evals, sU_)
170	gezelter	1490
171	tim	1811	// zero out I and then fill the diagonals with the moments of inertia:
172			inertiaTensor_(0, 0) = evals[0];
173			inertiaTensor_(1, 1) = evals[1];
174			inertiaTensor_(2, 2) = evals[2];
175
176			int nLinearAxis = 0;
177			for (int i = 0; i < 3; i++) {
178			if (fabs(evals[i]) < oopse::epsilon) {
179			linear_ = true;
180			linearAxis_ = i;
181			++ nLinearAxis;
182			}
183	gezelter	1490	}
184
185	tim	1811	if (nLinearAxis > 1) {
186			sprintf( painCave.errMsg,
187			"RigidBody error.\n"
188			"\tOOPSE found more than one axis in this rigid body with a vanishing \n"
189			"\tmoment of inertia. This can happen in one of three ways:\n"
190			"\t 1) Only one atom was specified, or \n"
191			"\t 2) All atoms were specified at the same location, or\n"
192			"\t 3) The programmers did something stupid.\n"
193			"\tIt is silly to use a rigid body to describe this situation. Be smarter.\n"
194			);
195			painCave.isFatal = 1;
196			simError();
197	gezelter	1490	}
198
199			}
200
201	tim	1692	void RigidBody::calcForcesAndTorques() {
202			unsigned int i;
203			unsigned int j;
204			Vector3d afrc;
205			Vector3d atrq;
206			Vector3d rpos;
207			Vector3d frc;
208			Vector3d trq;
209	gezelter	1490
210	tim	1738
211	tim	1692	zeroForces();
212
213			frc = getFrc();
214			trq = getTrq();
215	gezelter	1490
216	tim	1692	for (i = 0; i < atoms_.size(); i++) {
217	gezelter	1490
218	tim	1692	afrc = atoms_[i]->getFrc();
219	gezelter	1490
220	tim	1692	rpos = refCoords_[i];
221
222			frc += afrc;
223	gezelter	1490
224	tim	1692	trq[0] += rpos[1]afrc[2] - rpos[2]afrc[1];
225			trq[1] += rpos[2]afrc[0] - rpos[0]afrc[2];
226			trq[2] += rpos[0]afrc[1] - rpos[1]afrc[0];
227	gezelter	1490
228	tim	1692	// If the atom has a torque associated with it, then we also need to
229			// migrate the torques onto the center of mass:
230	gezelter	1490
231	tim	1692	if (atoms_[i]->isDirectional()) {
232			atrq = atoms_[i]->getTrq();
233			trq += atrq;
234			}
235
236	gezelter	1490	}
237
238	tim	1692	setFrc(frc);
239			setTrq(trq);
240
241	gezelter	1490	}
242
243	tim	1692	void RigidBody::updateAtoms() {
244			unsigned int i;
245			unsigned int j;
246			Vector3d ref;
247			Vector3d apos;
248			DirectionalAtom* dAtom;
249			Vector3d pos = getPos();
250			RotMat3x3d A = getA();
251
252			for (i = 0; i < atoms_.size(); i++) {
253	gezelter	1490
254	tim	1692	ref = body2Lab(refCoords_[i]);
255	gezelter	1490
256	tim	1692	apos = pos + ref;
257	gezelter	1490
258	tim	1692	atoms_[i]->setPos(apos);
259	gezelter	1490
260	tim	1692	if (atoms_[i]->isDirectional()) {
261
262			dAtom = (DirectionalAtom *) atoms_[i];
263			dAtom->rotateBy( A );
264			}
265	gezelter	1490
266	tim	1692	}
267	gezelter	1490
268			}
269
270
271	tim	1692	bool RigidBody::getAtomPos(Vector3d& pos, unsigned int index) {
272			if (index < atoms_.size()) {
273	gezelter	1490
274	tim	1692	Vector3d ref = body2Lab(refCoords_[index]);
275			pos = getPos() + ref;
276			return true;
277			} else {
278			std::cerr << index << " is an invalid index, current rigid body contains "
279			<< atoms_.size() << "atoms" << std::endl;
280			return false;
281			}
282	gezelter	1490	}
283
284	tim	1692	bool RigidBody::getAtomPos(Vector3d& pos, Atom* atom) {
285			std::vector<Atom*>::iterator i;
286			i = find(atoms_.begin(), atoms_.end(), atom);
287			if (i != atoms_.end()) {
288			//RigidBody class makes sure refCoords_ and atoms_ match each other
289			Vector3d ref = body2Lab(refCoords_[i - atoms_.begin()]);
290			pos = getPos() + ref;
291			return true;
292			} else {
293			std::cerr << "Atom " << atom->getGlobalIndex()
294			<<" does not belong to Rigid body "<< getGlobalIndex() << std::endl;
295			return false;
296			}
297	gezelter	1490	}
298	tim	1692	bool RigidBody::getAtomVel(Vector3d& vel, unsigned int index) {
299	gezelter	1490
300	tim	1692	//velRot = $(A\cdot skew(I^{-1}j))^{T}refCoor$
301	gezelter	1490
302	tim	1692	if (index < atoms_.size()) {
303	gezelter	1490
304	tim	1692	Vector3d velRot;
305			Mat3x3d skewMat;;
306			Vector3d ref = refCoords_[index];
307			Vector3d ji = getJ();
308			Mat3x3d I = getI();
309	gezelter	1490
310	tim	1692	skewMat(0, 0) =0;
311			skewMat(0, 1) = ji[2] /I(2, 2);
312			skewMat(0, 2) = -ji[1] /I(1, 1);
313	gezelter	1490
314	tim	1692	skewMat(1, 0) = -ji[2] /I(2, 2);
315			skewMat(1, 1) = 0;
316			skewMat(1, 2) = ji[0]/I(0, 0);
317	gezelter	1490
318	tim	1692	skewMat(2, 0) =ji[1] /I(1, 1);
319			skewMat(2, 1) = -ji[0]/I(0, 0);
320			skewMat(2, 2) = 0;
321	gezelter	1490
322	tim	1692	velRot = (getA() * skewMat).transpose() * ref;
323	gezelter	1490
324	tim	1692	vel =getVel() + velRot;
325			return true;
326
327			} else {
328			std::cerr << index << " is an invalid index, current rigid body contains "
329			<< atoms_.size() << "atoms" << std::endl;
330			return false;
331			}
332			}
333	gezelter	1490
334	tim	1692	bool RigidBody::getAtomVel(Vector3d& vel, Atom* atom) {
335	gezelter	1490
336	tim	1692	std::vector<Atom*>::iterator i;
337			i = find(atoms_.begin(), atoms_.end(), atom);
338			if (i != atoms_.end()) {
339			return getAtomVel(vel, i - atoms_.begin());
340			} else {
341			std::cerr << "Atom " << atom->getGlobalIndex()
342			<<" does not belong to Rigid body "<< getGlobalIndex() << std::endl;
343			return false;
344			}
345	gezelter	1490	}
346
347	tim	1692	bool RigidBody::getAtomRefCoor(Vector3d& coor, unsigned int index) {
348			if (index < atoms_.size()) {
349	gezelter	1490
350	tim	1692	coor = refCoords_[index];
351			return true;
352			} else {
353			std::cerr << index << " is an invalid index, current rigid body contains "
354			<< atoms_.size() << "atoms" << std::endl;
355			return false;
356			}
357	gezelter	1490
358	tim	1692	}
359	gezelter	1490
360	tim	1692	bool RigidBody::getAtomRefCoor(Vector3d& coor, Atom* atom) {
361			std::vector<Atom*>::iterator i;
362			i = find(atoms_.begin(), atoms_.end(), atom);
363			if (i != atoms_.end()) {
364			//RigidBody class makes sure refCoords_ and atoms_ match each other
365			coor = refCoords_[i - atoms_.begin()];
366			return true;
367			} else {
368			std::cerr << "Atom " << atom->getGlobalIndex()
369			<<" does not belong to Rigid body "<< getGlobalIndex() << std::endl;
370			return false;
371			}
372	gezelter	1490
373	tim	1692	}
374	gezelter	1490
375	tim	1805
376			void RigidBody::addAtom(Atom* at, AtomStamp* ats) {
377
378			Vector3d coords;
379			Vector3d euler;
380			Mat3x3d Atmp;
381
382			atoms_.push_back(at);
383
384			if( !ats->havePosition() ){
385			sprintf( painCave.errMsg,
386			"RigidBody error.\n"
387			"\tAtom %s does not have a position specified.\n"
388			"\tThis means RigidBody cannot set up reference coordinates.\n",
389			ats->getType() );
390			painCave.isFatal = 1;
391			simError();
392			}
393
394			coords[0] = ats->getPosX();
395			coords[1] = ats->getPosY();
396			coords[2] = ats->getPosZ();
397
398			refCoords_.push_back(coords);
399
400			/*
401			if (at->isDirectional()) {
402
403			if( !ats->haveOrientation() ){
404			sprintf( painCave.errMsg,
405			"RigidBody error.\n"
406			"\tAtom %s does not have an orientation specified.\n"
407			"\tThis means RigidBody cannot set up reference orientations.\n",
408			ats->getType() );
409			painCave.isFatal = 1;
410			simError();
411			}
412
413			euler[0] = ats->getEulerPhi();
414			euler[1] = ats->getEulerTheta();
415			euler[2] = ats->getEulerPsi();
416
417			doEulerToRotMat(euler, Atmp);
418			refOrients.push_back(Atmp);
419
420			}
421			*/
422
423	gezelter	1490	}
424
425	tim	1805	}
426