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 (std::size_t 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 (std::size_t i = 0; i < atoms_.size(); ++i) {
        refCoords_[i] -= refCOM;
    }

// Moment of Inertia calculation
    Mat3x3d Itmp(0.0);
  
    for (std::size_t 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;
    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:	1883
Committed:	Mon Dec 13 22:30:27 2004 UTC (19 years, 7 months ago) by tim
File size:	11514 byte(s)
Log Message:	MPI version is 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	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	tim	1883	for (std::size_t i = 0; i < atoms_.size(); ++i) {
137	tim	1811	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	tim	1883	for (std::size_t i = 0; i < atoms_.size(); ++i) {
145	tim	1811	refCoords_[i] -= refCOM;
146	gezelter	1490	}
147
148			// Moment of Inertia calculation
149	tim	1811	Mat3x3d Itmp(0.0);
150	gezelter	1490
151	tim	1883	for (std::size_t i = 0; i < atoms_.size(); i++) {
152	tim	1811	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			Vector3d ref;
232			Vector3d apos;
233			DirectionalAtom* dAtom;
234			Vector3d pos = getPos();
235	tim	1813	RotMat3x3d a = getA();
236	tim	1692
237			for (i = 0; i < atoms_.size(); i++) {
238	gezelter	1490
239	tim	1692	ref = body2Lab(refCoords_[i]);
240	gezelter	1490
241	tim	1692	apos = pos + ref;
242	gezelter	1490
243	tim	1692	atoms_[i]->setPos(apos);
244	gezelter	1490
245	tim	1692	if (atoms_[i]->isDirectional()) {
246
247			dAtom = (DirectionalAtom *) atoms_[i];
248	tim	1813	dAtom->setA(a * refOrients_[i]);
249			//dAtom->rotateBy( A );
250	tim	1692	}
251	gezelter	1490
252	tim	1692	}
253	gezelter	1490
254			}
255
256
257	tim	1692	bool RigidBody::getAtomPos(Vector3d& pos, unsigned int index) {
258			if (index < atoms_.size()) {
259	gezelter	1490
260	tim	1692	Vector3d ref = body2Lab(refCoords_[index]);
261			pos = getPos() + ref;
262			return true;
263			} else {
264			std::cerr << index << " is an invalid index, current rigid body contains "
265			<< atoms_.size() << "atoms" << std::endl;
266			return false;
267			}
268	gezelter	1490	}
269
270	tim	1692	bool RigidBody::getAtomPos(Vector3d& pos, Atom* atom) {
271			std::vector<Atom*>::iterator i;
272			i = find(atoms_.begin(), atoms_.end(), atom);
273			if (i != atoms_.end()) {
274			//RigidBody class makes sure refCoords_ and atoms_ match each other
275			Vector3d ref = body2Lab(refCoords_[i - atoms_.begin()]);
276			pos = getPos() + ref;
277			return true;
278			} else {
279			std::cerr << "Atom " << atom->getGlobalIndex()
280			<<" does not belong to Rigid body "<< getGlobalIndex() << std::endl;
281			return false;
282			}
283	gezelter	1490	}
284	tim	1692	bool RigidBody::getAtomVel(Vector3d& vel, unsigned int index) {
285	gezelter	1490
286	tim	1692	//velRot = $(A\cdot skew(I^{-1}j))^{T}refCoor$
287	gezelter	1490
288	tim	1692	if (index < atoms_.size()) {
289	gezelter	1490
290	tim	1692	Vector3d velRot;
291			Mat3x3d skewMat;;
292			Vector3d ref = refCoords_[index];
293			Vector3d ji = getJ();
294			Mat3x3d I = getI();
295	gezelter	1490
296	tim	1692	skewMat(0, 0) =0;
297			skewMat(0, 1) = ji[2] /I(2, 2);
298			skewMat(0, 2) = -ji[1] /I(1, 1);
299	gezelter	1490
300	tim	1692	skewMat(1, 0) = -ji[2] /I(2, 2);
301			skewMat(1, 1) = 0;
302			skewMat(1, 2) = ji[0]/I(0, 0);
303	gezelter	1490
304	tim	1692	skewMat(2, 0) =ji[1] /I(1, 1);
305			skewMat(2, 1) = -ji[0]/I(0, 0);
306			skewMat(2, 2) = 0;
307	gezelter	1490
308	tim	1692	velRot = (getA() * skewMat).transpose() * ref;
309	gezelter	1490
310	tim	1692	vel =getVel() + velRot;
311			return true;
312
313			} else {
314			std::cerr << index << " is an invalid index, current rigid body contains "
315			<< atoms_.size() << "atoms" << std::endl;
316			return false;
317			}
318			}
319	gezelter	1490
320	tim	1692	bool RigidBody::getAtomVel(Vector3d& vel, Atom* atom) {
321	gezelter	1490
322	tim	1692	std::vector<Atom*>::iterator i;
323			i = find(atoms_.begin(), atoms_.end(), atom);
324			if (i != atoms_.end()) {
325			return getAtomVel(vel, i - atoms_.begin());
326			} else {
327			std::cerr << "Atom " << atom->getGlobalIndex()
328			<<" does not belong to Rigid body "<< getGlobalIndex() << std::endl;
329			return false;
330			}
331	gezelter	1490	}
332
333	tim	1692	bool RigidBody::getAtomRefCoor(Vector3d& coor, unsigned int index) {
334			if (index < atoms_.size()) {
335	gezelter	1490
336	tim	1692	coor = refCoords_[index];
337			return true;
338			} else {
339			std::cerr << index << " is an invalid index, current rigid body contains "
340			<< atoms_.size() << "atoms" << std::endl;
341			return false;
342			}
343	gezelter	1490
344	tim	1692	}
345	gezelter	1490
346	tim	1692	bool RigidBody::getAtomRefCoor(Vector3d& coor, Atom* atom) {
347			std::vector<Atom*>::iterator i;
348			i = find(atoms_.begin(), atoms_.end(), atom);
349			if (i != atoms_.end()) {
350			//RigidBody class makes sure refCoords_ and atoms_ match each other
351			coor = refCoords_[i - atoms_.begin()];
352			return true;
353			} else {
354			std::cerr << "Atom " << atom->getGlobalIndex()
355			<<" does not belong to Rigid body "<< getGlobalIndex() << std::endl;
356			return false;
357			}
358	gezelter	1490
359	tim	1692	}
360	gezelter	1490
361	tim	1805
362			void RigidBody::addAtom(Atom* at, AtomStamp* ats) {
363
364			Vector3d coords;
365			Vector3d euler;
366	tim	1813
367	tim	1805
368			atoms_.push_back(at);
369
370			if( !ats->havePosition() ){
371			sprintf( painCave.errMsg,
372			"RigidBody error.\n"
373			"\tAtom %s does not have a position specified.\n"
374			"\tThis means RigidBody cannot set up reference coordinates.\n",
375			ats->getType() );
376			painCave.isFatal = 1;
377			simError();
378			}
379
380			coords[0] = ats->getPosX();
381			coords[1] = ats->getPosY();
382			coords[2] = ats->getPosZ();
383
384			refCoords_.push_back(coords);
385
386	tim	1813	RotMat3x3d identMat = RotMat3x3d::identity();
387
388	tim	1805	if (at->isDirectional()) {
389
390			if( !ats->haveOrientation() ){
391			sprintf( painCave.errMsg,
392			"RigidBody error.\n"
393			"\tAtom %s does not have an orientation specified.\n"
394			"\tThis means RigidBody cannot set up reference orientations.\n",
395			ats->getType() );
396			painCave.isFatal = 1;
397			simError();
398			}
399
400			euler[0] = ats->getEulerPhi();
401			euler[1] = ats->getEulerTheta();
402			euler[2] = ats->getEulerPsi();
403	tim	1813
404			RotMat3x3d Atmp(euler);
405			refOrients_.push_back(Atmp);
406	tim	1805
407	tim	1813	}else {
408			refOrients_.push_back(identMat);
409	tim	1805	}
410
411	tim	1813
412	gezelter	1490	}
413
414	tim	1805	}
415