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

    //for (int i =0 ; i < atoms_.size(); ++i){
    //    if (atoms_[i]->isDirectional()) {
    //        atoms_[i]->setPrevA(a * refOrients_[i]);
    //    }
    //}

}

      
void RigidBody::setA(const RotMat3x3d& a) {
    ((snapshotMan_->getCurrentSnapshot())->*storage_).aMat[localIndex_] = a;
    //((snapshotMan_->getCurrentSnapshot())->*storage_).electroFrame[localIndex_] = a.transpose() * sU_;

    //for (int i =0 ; i < atoms_.size(); ++i){
    //    if (atoms_[i]->isDirectional()) {
    //        atoms_[i]->setA(a * refOrients_[i]);
    //    }
    //}
}    
    
void RigidBody::setA(const RotMat3x3d& a, int snapshotNo) {
    ((snapshotMan_->getSnapshot(snapshotNo))->*storage_).aMat[localIndex_] = a;
    //((snapshotMan_->getSnapshot(snapshotNo))->*storage_).electroFrame[localIndex_] = a.transpose() * sU_;    

    //for (int i =0 ; i < atoms_.size(); ++i){
    //    if (atoms_[i]->isDirectional()) {
    //        atoms_[i]->setA(a * refOrients_[i], snapshotNo);
    //    }
    //}

}   

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

std::vector<double> RigidBody::getGrad() {
     std::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() {
    Vector3d afrc;
    Vector3d atrq;
    Vector3d apos;
    Vector3d rpos;
    Vector3d frc(0.0);
    Vector3d trq(0.0);
    Vector3d pos = this->getPos();
    for (int i = 0; i < atoms_.size(); i++) {

        afrc = atoms_[i]->getFrc();
        apos = atoms_[i]->getPos();
        rpos = apos - pos;
        
        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->setA(a * refOrients_[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;
  

  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);

  RotMat3x3d identMat = RotMat3x3d::identity();
  
  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();

    RotMat3x3d Atmp(euler);
    refOrients_.push_back(Atmp);
    
  }else {
    refOrients_.push_back(identMat);
  }
  
  
}

}

Revision:	1870
Committed:	Thu Dec 9 15:45:21 2004 UTC (20 years, 7 months ago) by tim
File size:	11510 byte(s)
Log Message:	Fix a bug in calculating torque in rigid body
#	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	1813	//((snapshotMan_->getPrevSnapshot())->storage_).electroFrame[localIndex_] = a.transpose() sU_;
37	gezelter	1490
38	tim	1813	//for (int i =0 ; i < atoms_.size(); ++i){
39			// if (atoms_[i]->isDirectional()) {
40			// atoms_[i]->setPrevA(a * refOrients_[i]);
41			// }
42			//}
43	gezelter	1490
44			}
45
46	tim	1692
47			void RigidBody::setA(const RotMat3x3d& a) {
48			((snapshotMan_->getCurrentSnapshot())->*storage_).aMat[localIndex_] = a;
49	tim	1813	//((snapshotMan_->getCurrentSnapshot())->storage_).electroFrame[localIndex_] = a.transpose() sU_;
50	gezelter	1490
51	tim	1813	//for (int i =0 ; i < atoms_.size(); ++i){
52			// if (atoms_[i]->isDirectional()) {
53			// atoms_[i]->setA(a * refOrients_[i]);
54			// }
55			//}
56	gezelter	1490	}
57
58	tim	1692	void RigidBody::setA(const RotMat3x3d& a, int snapshotNo) {
59			((snapshotMan_->getSnapshot(snapshotNo))->*storage_).aMat[localIndex_] = a;
60	tim	1813	//((snapshotMan_->getSnapshot(snapshotNo))->storage_).electroFrame[localIndex_] = a.transpose() sU_;
61	gezelter	1490
62	tim	1813	//for (int i =0 ; i < atoms_.size(); ++i){
63			// if (atoms_[i]->isDirectional()) {
64			// atoms_[i]->setA(a * refOrients_[i], snapshotNo);
65			// }
66			//}
67	gezelter	1490
68	tim	1692	}
69	gezelter	1490
70	tim	1692	Mat3x3d RigidBody::getI() {
71			return inertiaTensor_;
72			}
73	gezelter	1490
74	tim	1692	std::vector<double> RigidBody::getGrad() {
75	tim	1826	std::vector<double> grad(6, 0.0);
76	tim	1692	Vector3d force;
77			Vector3d torque;
78			Vector3d myEuler;
79			double phi, theta, psi;
80			double cphi, sphi, ctheta, stheta;
81			Vector3d ephi;
82			Vector3d etheta;
83			Vector3d epsi;
84	gezelter	1490
85	tim	1692	force = getFrc();
86			torque =getTrq();
87			myEuler = getA().toEulerAngles();
88	gezelter	1490
89	tim	1692	phi = myEuler[0];
90			theta = myEuler[1];
91			psi = myEuler[2];
92	gezelter	1490
93	tim	1692	cphi = cos(phi);
94			sphi = sin(phi);
95			ctheta = cos(theta);
96			stheta = sin(theta);
97	gezelter	1490
98	tim	1692	// get unit vectors along the phi, theta and psi rotation axes
99	gezelter	1490
100	tim	1692	ephi[0] = 0.0;
101			ephi[1] = 0.0;
102			ephi[2] = 1.0;
103	gezelter	1490
104	tim	1692	etheta[0] = cphi;
105			etheta[1] = sphi;
106			etheta[2] = 0.0;
107	gezelter	1490
108	tim	1692	epsi[0] = stheta * cphi;
109			epsi[1] = stheta * sphi;
110			epsi[2] = ctheta;
111	gezelter	1490
112	tim	1692	//gradient is equal to -force
113			for (int j = 0 ; j<3; j++)
114			grad[j] = -force[j];
115	gezelter	1490
116	tim	1692	for (int j = 0; j < 3; j++ ) {
117	gezelter	1490
118	tim	1692	grad[3] += torque[j]*ephi[j];
119			grad[4] += torque[j]*etheta[j];
120			grad[5] += torque[j]*epsi[j];
121	gezelter	1490
122	tim	1692	}
123
124			return grad;
125			}
126	gezelter	1490
127	tim	1692	void RigidBody::accept(BaseVisitor* v) {
128			v->visit(this);
129			}
130	gezelter	1490
131	tim	1805	/*@todo need modification /
132	tim	1692	void RigidBody::calcRefCoords() {
133	tim	1811	double mtmp;
134			Vector3d refCOM(0.0);
135			mass_ = 0.0;
136			for (int i = 0; i < atoms_.size(); ++i) {
137			mtmp = atoms_[i]->getMass();
138			mass_ += mtmp;
139			refCOM += refCoords_[i]*mtmp;
140			}
141			refCOM /= mass_;
142	gezelter	1490
143	tim	1811	// Next, move the origin of the reference coordinate system to the COM:
144			for (int i = 0; i < atoms_.size(); ++i) {
145			refCoords_[i] -= refCOM;
146	gezelter	1490	}
147
148			// Moment of Inertia calculation
149	tim	1811	Mat3x3d Itmp(0.0);
150	gezelter	1490
151	tim	1811	for (int i = 0; i < atoms_.size(); i++) {
152			mtmp = atoms_[i]->getMass();
153			Itmp -= outProduct(refCoords_[i], refCoords_[i]) * mtmp;
154			double r2 = refCoords_[i].lengthSquare();
155			Itmp(0, 0) += mtmp * r2;
156			Itmp(1, 1) += mtmp * r2;
157			Itmp(2, 2) += mtmp * r2;
158	gezelter	1490	}
159
160	tim	1811	//diagonalize
161			Vector3d evals;
162	tim	1815	Mat3x3d::diagonalize(Itmp, evals, sU_);
163	gezelter	1490
164	tim	1811	// zero out I and then fill the diagonals with the moments of inertia:
165			inertiaTensor_(0, 0) = evals[0];
166			inertiaTensor_(1, 1) = evals[1];
167			inertiaTensor_(2, 2) = evals[2];
168
169			int nLinearAxis = 0;
170			for (int i = 0; i < 3; i++) {
171			if (fabs(evals[i]) < oopse::epsilon) {
172			linear_ = true;
173			linearAxis_ = i;
174			++ nLinearAxis;
175			}
176	gezelter	1490	}
177
178	tim	1811	if (nLinearAxis > 1) {
179			sprintf( painCave.errMsg,
180			"RigidBody error.\n"
181			"\tOOPSE found more than one axis in this rigid body with a vanishing \n"
182			"\tmoment of inertia. This can happen in one of three ways:\n"
183			"\t 1) Only one atom was specified, or \n"
184			"\t 2) All atoms were specified at the same location, or\n"
185			"\t 3) The programmers did something stupid.\n"
186			"\tIt is silly to use a rigid body to describe this situation. Be smarter.\n"
187			);
188			painCave.isFatal = 1;
189			simError();
190	gezelter	1490	}
191
192			}
193
194	tim	1692	void RigidBody::calcForcesAndTorques() {
195			Vector3d afrc;
196			Vector3d atrq;
197	tim	1870	Vector3d apos;
198	tim	1692	Vector3d rpos;
199	tim	1870	Vector3d frc(0.0);
200			Vector3d trq(0.0);
201			Vector3d pos = this->getPos();
202			for (int i = 0; i < atoms_.size(); i++) {
203	gezelter	1490
204	tim	1692	afrc = atoms_[i]->getFrc();
205	tim	1870	apos = atoms_[i]->getPos();
206			rpos = apos - pos;
207	tim	1692
208			frc += afrc;
209	gezelter	1490
210	tim	1692	trq[0] += rpos[1]afrc[2] - rpos[2]afrc[1];
211			trq[1] += rpos[2]afrc[0] - rpos[0]afrc[2];
212			trq[2] += rpos[0]afrc[1] - rpos[1]afrc[0];
213	gezelter	1490
214	tim	1692	// If the atom has a torque associated with it, then we also need to
215			// migrate the torques onto the center of mass:
216	gezelter	1490
217	tim	1692	if (atoms_[i]->isDirectional()) {
218			atrq = atoms_[i]->getTrq();
219			trq += atrq;
220			}
221
222	gezelter	1490	}
223
224	tim	1692	setFrc(frc);
225			setTrq(trq);
226
227	gezelter	1490	}
228
229	tim	1692	void RigidBody::updateAtoms() {
230			unsigned int i;
231			unsigned int j;
232			Vector3d ref;
233			Vector3d apos;
234			DirectionalAtom* dAtom;
235			Vector3d pos = getPos();
236	tim	1813	RotMat3x3d a = getA();
237	tim	1692
238			for (i = 0; i < atoms_.size(); i++) {
239	gezelter	1490
240	tim	1692	ref = body2Lab(refCoords_[i]);
241	gezelter	1490
242	tim	1692	apos = pos + ref;
243	gezelter	1490
244	tim	1692	atoms_[i]->setPos(apos);
245	gezelter	1490
246	tim	1692	if (atoms_[i]->isDirectional()) {
247
248			dAtom = (DirectionalAtom *) atoms_[i];
249	tim	1813	dAtom->setA(a * refOrients_[i]);
250			//dAtom->rotateBy( A );
251	tim	1692	}
252	gezelter	1490
253	tim	1692	}
254	gezelter	1490
255			}
256
257
258	tim	1692	bool RigidBody::getAtomPos(Vector3d& pos, unsigned int index) {
259			if (index < atoms_.size()) {
260	gezelter	1490
261	tim	1692	Vector3d ref = body2Lab(refCoords_[index]);
262			pos = getPos() + ref;
263			return true;
264			} else {
265			std::cerr << index << " is an invalid index, current rigid body contains "
266			<< atoms_.size() << "atoms" << std::endl;
267			return false;
268			}
269	gezelter	1490	}
270
271	tim	1692	bool RigidBody::getAtomPos(Vector3d& pos, Atom* atom) {
272			std::vector<Atom*>::iterator i;
273			i = find(atoms_.begin(), atoms_.end(), atom);
274			if (i != atoms_.end()) {
275			//RigidBody class makes sure refCoords_ and atoms_ match each other
276			Vector3d ref = body2Lab(refCoords_[i - atoms_.begin()]);
277			pos = getPos() + ref;
278			return true;
279			} else {
280			std::cerr << "Atom " << atom->getGlobalIndex()
281			<<" does not belong to Rigid body "<< getGlobalIndex() << std::endl;
282			return false;
283			}
284	gezelter	1490	}
285	tim	1692	bool RigidBody::getAtomVel(Vector3d& vel, unsigned int index) {
286	gezelter	1490
287	tim	1692	//velRot = $(A\cdot skew(I^{-1}j))^{T}refCoor$
288	gezelter	1490
289	tim	1692	if (index < atoms_.size()) {
290	gezelter	1490
291	tim	1692	Vector3d velRot;
292			Mat3x3d skewMat;;
293			Vector3d ref = refCoords_[index];
294			Vector3d ji = getJ();
295			Mat3x3d I = getI();
296	gezelter	1490
297	tim	1692	skewMat(0, 0) =0;
298			skewMat(0, 1) = ji[2] /I(2, 2);
299			skewMat(0, 2) = -ji[1] /I(1, 1);
300	gezelter	1490
301	tim	1692	skewMat(1, 0) = -ji[2] /I(2, 2);
302			skewMat(1, 1) = 0;
303			skewMat(1, 2) = ji[0]/I(0, 0);
304	gezelter	1490
305	tim	1692	skewMat(2, 0) =ji[1] /I(1, 1);
306			skewMat(2, 1) = -ji[0]/I(0, 0);
307			skewMat(2, 2) = 0;
308	gezelter	1490
309	tim	1692	velRot = (getA() * skewMat).transpose() * ref;
310	gezelter	1490
311	tim	1692	vel =getVel() + velRot;
312			return true;
313
314			} else {
315			std::cerr << index << " is an invalid index, current rigid body contains "
316			<< atoms_.size() << "atoms" << std::endl;
317			return false;
318			}
319			}
320	gezelter	1490
321	tim	1692	bool RigidBody::getAtomVel(Vector3d& vel, Atom* atom) {
322	gezelter	1490
323	tim	1692	std::vector<Atom*>::iterator i;
324			i = find(atoms_.begin(), atoms_.end(), atom);
325			if (i != atoms_.end()) {
326			return getAtomVel(vel, i - atoms_.begin());
327			} else {
328			std::cerr << "Atom " << atom->getGlobalIndex()
329			<<" does not belong to Rigid body "<< getGlobalIndex() << std::endl;
330			return false;
331			}
332	gezelter	1490	}
333
334	tim	1692	bool RigidBody::getAtomRefCoor(Vector3d& coor, unsigned int index) {
335			if (index < atoms_.size()) {
336	gezelter	1490
337	tim	1692	coor = refCoords_[index];
338			return true;
339			} else {
340			std::cerr << index << " is an invalid index, current rigid body contains "
341			<< atoms_.size() << "atoms" << std::endl;
342			return false;
343			}
344	gezelter	1490
345	tim	1692	}
346	gezelter	1490
347	tim	1692	bool RigidBody::getAtomRefCoor(Vector3d& coor, Atom* atom) {
348			std::vector<Atom*>::iterator i;
349			i = find(atoms_.begin(), atoms_.end(), atom);
350			if (i != atoms_.end()) {
351			//RigidBody class makes sure refCoords_ and atoms_ match each other
352			coor = refCoords_[i - atoms_.begin()];
353			return true;
354			} else {
355			std::cerr << "Atom " << atom->getGlobalIndex()
356			<<" does not belong to Rigid body "<< getGlobalIndex() << std::endl;
357			return false;
358			}
359	gezelter	1490
360	tim	1692	}
361	gezelter	1490
362	tim	1805
363			void RigidBody::addAtom(Atom* at, AtomStamp* ats) {
364
365			Vector3d coords;
366			Vector3d euler;
367	tim	1813
368	tim	1805
369			atoms_.push_back(at);
370
371			if( !ats->havePosition() ){
372			sprintf( painCave.errMsg,
373			"RigidBody error.\n"
374			"\tAtom %s does not have a position specified.\n"
375			"\tThis means RigidBody cannot set up reference coordinates.\n",
376			ats->getType() );
377			painCave.isFatal = 1;
378			simError();
379			}
380
381			coords[0] = ats->getPosX();
382			coords[1] = ats->getPosY();
383			coords[2] = ats->getPosZ();
384
385			refCoords_.push_back(coords);
386
387	tim	1813	RotMat3x3d identMat = RotMat3x3d::identity();
388
389	tim	1805	if (at->isDirectional()) {
390
391			if( !ats->haveOrientation() ){
392			sprintf( painCave.errMsg,
393			"RigidBody error.\n"
394			"\tAtom %s does not have an orientation specified.\n"
395			"\tThis means RigidBody cannot set up reference orientations.\n",
396			ats->getType() );
397			painCave.isFatal = 1;
398			simError();
399			}
400
401			euler[0] = ats->getEulerPhi();
402			euler[1] = ats->getEulerTheta();
403			euler[2] = ats->getEulerPsi();
404	tim	1813
405			RotMat3x3d Atmp(euler);
406			refOrients_.push_back(Atmp);
407	tim	1805
408	tim	1813	}else {
409			refOrients_.push_back(identMat);
410	tim	1805	}
411
412	tim	1813
413	gezelter	1490	}
414
415	tim	1805	}
416