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() {
    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->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:	1815
Committed:	Wed Dec 1 18:42:45 2004 UTC (19 years, 7 months ago) by tim
File size:	11510 byte(s)
Log Message:	except integrator and constraint, other directories get built
#	User	Rev	Content
1	tim	1692	/*
2			* Copyright (C) 2000-2004 Object Oriented Parallel Simulation Engine (OOPSE) project
3			*
4			* Contact: oopse@oopse.org
5			*
6			* This program is free software; you can redistribute it and/or
7			* modify it under the terms of the GNU Lesser General Public License
8			* as published by the Free Software Foundation; either version 2.1
9			* of the License, or (at your option) any later version.
10			* All we ask is that proper credit is given for our work, which includes
11			* - but is not limited to - adding the above copyright notice to the beginning
12			* of your source code files, and to any copyright notice that you may distribute
13			* with programs based on this work.
14			*
15			* This program is distributed in the hope that it will be useful,
16			* but WITHOUT ANY WARRANTY; without even the implied warranty of
17			* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
18			* GNU Lesser General Public License for more details.
19			*
20			* You should have received a copy of the GNU Lesser General Public License
21			* along with this program; if not, write to the Free Software
22			* Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
23			*
24			*/
25
26	tim	1492	#include "primitives/RigidBody.hpp"
27	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			vector<double> grad(6, 0.0);
76			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			unsigned int i;
196			unsigned int j;
197			Vector3d afrc;
198			Vector3d atrq;
199			Vector3d rpos;
200			Vector3d frc;
201			Vector3d trq;
202	gezelter	1490
203	tim	1738
204	tim	1692	zeroForces();
205
206			frc = getFrc();
207			trq = getTrq();
208	gezelter	1490
209	tim	1692	for (i = 0; i < atoms_.size(); i++) {
210	gezelter	1490
211	tim	1692	afrc = atoms_[i]->getFrc();
212	gezelter	1490
213	tim	1692	rpos = refCoords_[i];
214
215			frc += afrc;
216	gezelter	1490
217	tim	1692	trq[0] += rpos[1]afrc[2] - rpos[2]afrc[1];
218			trq[1] += rpos[2]afrc[0] - rpos[0]afrc[2];
219			trq[2] += rpos[0]afrc[1] - rpos[1]afrc[0];
220	gezelter	1490
221	tim	1692	// If the atom has a torque associated with it, then we also need to
222			// migrate the torques onto the center of mass:
223	gezelter	1490
224	tim	1692	if (atoms_[i]->isDirectional()) {
225			atrq = atoms_[i]->getTrq();
226			trq += atrq;
227			}
228
229	gezelter	1490	}
230
231	tim	1692	setFrc(frc);
232			setTrq(trq);
233
234	gezelter	1490	}
235
236	tim	1692	void RigidBody::updateAtoms() {
237			unsigned int i;
238			unsigned int j;
239			Vector3d ref;
240			Vector3d apos;
241			DirectionalAtom* dAtom;
242			Vector3d pos = getPos();
243	tim	1813	RotMat3x3d a = getA();
244	tim	1692
245			for (i = 0; i < atoms_.size(); i++) {
246	gezelter	1490
247	tim	1692	ref = body2Lab(refCoords_[i]);
248	gezelter	1490
249	tim	1692	apos = pos + ref;
250	gezelter	1490
251	tim	1692	atoms_[i]->setPos(apos);
252	gezelter	1490
253	tim	1692	if (atoms_[i]->isDirectional()) {
254
255			dAtom = (DirectionalAtom *) atoms_[i];
256	tim	1813	dAtom->setA(a * refOrients_[i]);
257			//dAtom->rotateBy( A );
258	tim	1692	}
259	gezelter	1490
260	tim	1692	}
261	gezelter	1490
262			}
263
264
265	tim	1692	bool RigidBody::getAtomPos(Vector3d& pos, unsigned int index) {
266			if (index < atoms_.size()) {
267	gezelter	1490
268	tim	1692	Vector3d ref = body2Lab(refCoords_[index]);
269			pos = getPos() + ref;
270			return true;
271			} else {
272			std::cerr << index << " is an invalid index, current rigid body contains "
273			<< atoms_.size() << "atoms" << std::endl;
274			return false;
275			}
276	gezelter	1490	}
277
278	tim	1692	bool RigidBody::getAtomPos(Vector3d& pos, Atom* atom) {
279			std::vector<Atom*>::iterator i;
280			i = find(atoms_.begin(), atoms_.end(), atom);
281			if (i != atoms_.end()) {
282			//RigidBody class makes sure refCoords_ and atoms_ match each other
283			Vector3d ref = body2Lab(refCoords_[i - atoms_.begin()]);
284			pos = getPos() + ref;
285			return true;
286			} else {
287			std::cerr << "Atom " << atom->getGlobalIndex()
288			<<" does not belong to Rigid body "<< getGlobalIndex() << std::endl;
289			return false;
290			}
291	gezelter	1490	}
292	tim	1692	bool RigidBody::getAtomVel(Vector3d& vel, unsigned int index) {
293	gezelter	1490
294	tim	1692	//velRot = $(A\cdot skew(I^{-1}j))^{T}refCoor$
295	gezelter	1490
296	tim	1692	if (index < atoms_.size()) {
297	gezelter	1490
298	tim	1692	Vector3d velRot;
299			Mat3x3d skewMat;;
300			Vector3d ref = refCoords_[index];
301			Vector3d ji = getJ();
302			Mat3x3d I = getI();
303	gezelter	1490
304	tim	1692	skewMat(0, 0) =0;
305			skewMat(0, 1) = ji[2] /I(2, 2);
306			skewMat(0, 2) = -ji[1] /I(1, 1);
307	gezelter	1490
308	tim	1692	skewMat(1, 0) = -ji[2] /I(2, 2);
309			skewMat(1, 1) = 0;
310			skewMat(1, 2) = ji[0]/I(0, 0);
311	gezelter	1490
312	tim	1692	skewMat(2, 0) =ji[1] /I(1, 1);
313			skewMat(2, 1) = -ji[0]/I(0, 0);
314			skewMat(2, 2) = 0;
315	gezelter	1490
316	tim	1692	velRot = (getA() * skewMat).transpose() * ref;
317	gezelter	1490
318	tim	1692	vel =getVel() + velRot;
319			return true;
320
321			} else {
322			std::cerr << index << " is an invalid index, current rigid body contains "
323			<< atoms_.size() << "atoms" << std::endl;
324			return false;
325			}
326			}
327	gezelter	1490
328	tim	1692	bool RigidBody::getAtomVel(Vector3d& vel, Atom* atom) {
329	gezelter	1490
330	tim	1692	std::vector<Atom*>::iterator i;
331			i = find(atoms_.begin(), atoms_.end(), atom);
332			if (i != atoms_.end()) {
333			return getAtomVel(vel, i - atoms_.begin());
334			} else {
335			std::cerr << "Atom " << atom->getGlobalIndex()
336			<<" does not belong to Rigid body "<< getGlobalIndex() << std::endl;
337			return false;
338			}
339	gezelter	1490	}
340
341	tim	1692	bool RigidBody::getAtomRefCoor(Vector3d& coor, unsigned int index) {
342			if (index < atoms_.size()) {
343	gezelter	1490
344	tim	1692	coor = refCoords_[index];
345			return true;
346			} else {
347			std::cerr << index << " is an invalid index, current rigid body contains "
348			<< atoms_.size() << "atoms" << std::endl;
349			return false;
350			}
351	gezelter	1490
352	tim	1692	}
353	gezelter	1490
354	tim	1692	bool RigidBody::getAtomRefCoor(Vector3d& coor, Atom* atom) {
355			std::vector<Atom*>::iterator i;
356			i = find(atoms_.begin(), atoms_.end(), atom);
357			if (i != atoms_.end()) {
358			//RigidBody class makes sure refCoords_ and atoms_ match each other
359			coor = refCoords_[i - atoms_.begin()];
360			return true;
361			} else {
362			std::cerr << "Atom " << atom->getGlobalIndex()
363			<<" does not belong to Rigid body "<< getGlobalIndex() << std::endl;
364			return false;
365			}
366	gezelter	1490
367	tim	1692	}
368	gezelter	1490
369	tim	1805
370			void RigidBody::addAtom(Atom* at, AtomStamp* ats) {
371
372			Vector3d coords;
373			Vector3d euler;
374	tim	1813
375	tim	1805
376			atoms_.push_back(at);
377
378			if( !ats->havePosition() ){
379			sprintf( painCave.errMsg,
380			"RigidBody error.\n"
381			"\tAtom %s does not have a position specified.\n"
382			"\tThis means RigidBody cannot set up reference coordinates.\n",
383			ats->getType() );
384			painCave.isFatal = 1;
385			simError();
386			}
387
388			coords[0] = ats->getPosX();
389			coords[1] = ats->getPosY();
390			coords[2] = ats->getPosZ();
391
392			refCoords_.push_back(coords);
393
394	tim	1813	RotMat3x3d identMat = RotMat3x3d::identity();
395
396	tim	1805	if (at->isDirectional()) {
397
398			if( !ats->haveOrientation() ){
399			sprintf( painCave.errMsg,
400			"RigidBody error.\n"
401			"\tAtom %s does not have an orientation specified.\n"
402			"\tThis means RigidBody cannot set up reference orientations.\n",
403			ats->getType() );
404			painCave.isFatal = 1;
405			simError();
406			}
407
408			euler[0] = ats->getEulerPhi();
409			euler[1] = ats->getEulerTheta();
410			euler[2] = ats->getEulerPsi();
411	tim	1813
412			RotMat3x3d Atmp(euler);
413			refOrients_.push_back(Atmp);
414	tim	1805
415	tim	1813	}else {
416			refOrients_.push_back(identMat);
417	tim	1805	}
418
419	tim	1813
420	gezelter	1490	}
421
422	tim	1805	}
423