src/primitives/RigidBody.cpp

 /*
 * Copyright (c) 2005 The University of Notre Dame. All Rights Reserved.
 *
 * The University of Notre Dame grants you ("Licensee") a
 * non-exclusive, royalty free, license to use, modify and
 * redistribute this software in source and binary code form, provided
 * that the following conditions are met:
 *
 * 1. Acknowledgement of the program authors must be made in any
 *    publication of scientific results based in part on use of the
 *    program.  An acceptable form of acknowledgement is citation of
 *    the article in which the program was described (Matthew
 *    A. Meineke, Charles F. Vardeman II, Teng Lin, Christopher
 *    J. Fennell and J. Daniel Gezelter, "OOPSE: An Object-Oriented
 *    Parallel Simulation Engine for Molecular Dynamics,"
 *    J. Comput. Chem. 26, pp. 252-271 (2005))
 *
 * 2. Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 *
 * 3. Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in the
 *    documentation and/or other materials provided with the
 *    distribution.
 *
 * This software is provided "AS IS," without a warranty of any
 * kind. All express or implied conditions, representations and
 * warranties, including any implied warranty of merchantability,
 * fitness for a particular purpose or non-infringement, are hereby
 * excluded.  The University of Notre Dame and its licensors shall not
 * be liable for any damages suffered by licensee as a result of
 * using, modifying or distributing the software or its
 * derivatives. In no event will the University of Notre Dame or its
 * licensors be liable for any lost revenue, profit or data, or for
 * direct, indirect, special, consequential, incidental or punitive
 * damages, however caused and regardless of the theory of liability,
 * arising out of the use of or inability to use software, even if the
 * University of Notre Dame has been advised of the possibility of
 * such damages.
 */
#include <algorithm>
#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 = std::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 = std::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 = std::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:	1930
Committed:	Wed Jan 12 22:41:40 2005 UTC (19 years, 5 months ago) by gezelter
File size:	12448 byte(s)
Log Message:	merging new_design branch into OOPSE-2.0
#	User	Rev	Content
1	gezelter	1930	/*
2			* Copyright (c) 2005 The University of Notre Dame. All Rights Reserved.
3			*
4			* The University of Notre Dame grants you ("Licensee") a
5			* non-exclusive, royalty free, license to use, modify and
6			* redistribute this software in source and binary code form, provided
7			* that the following conditions are met:
8			*
9			* 1. Acknowledgement of the program authors must be made in any
10			* publication of scientific results based in part on use of the
11			* program. An acceptable form of acknowledgement is citation of
12			* the article in which the program was described (Matthew
13			* A. Meineke, Charles F. Vardeman II, Teng Lin, Christopher
14			* J. Fennell and J. Daniel Gezelter, "OOPSE: An Object-Oriented
15			* Parallel Simulation Engine for Molecular Dynamics,"
16			* J. Comput. Chem. 26, pp. 252-271 (2005))
17			*
18			* 2. Redistributions of source code must retain the above copyright
19			* notice, this list of conditions and the following disclaimer.
20			*
21			* 3. Redistributions in binary form must reproduce the above copyright
22			* notice, this list of conditions and the following disclaimer in the
23			* documentation and/or other materials provided with the
24			* distribution.
25			*
26			* This software is provided "AS IS," without a warranty of any
27			* kind. All express or implied conditions, representations and
28			* warranties, including any implied warranty of merchantability,
29			* fitness for a particular purpose or non-infringement, are hereby
30			* excluded. The University of Notre Dame and its licensors shall not
31			* be liable for any damages suffered by licensee as a result of
32			* using, modifying or distributing the software or its
33			* derivatives. In no event will the University of Notre Dame or its
34			* licensors be liable for any lost revenue, profit or data, or for
35			* direct, indirect, special, consequential, incidental or punitive
36			* damages, however caused and regardless of the theory of liability,
37			* arising out of the use of or inability to use software, even if the
38			* University of Notre Dame has been advised of the possibility of
39			* such damages.
40			*/
41			#include <algorithm>
42	tim	1492	#include "primitives/RigidBody.hpp"
43			#include "utils/simError.h"
44	gezelter	1930	namespace oopse {
45	gezelter	1490
46	gezelter	1930	RigidBody::RigidBody() : StuntDouble(otRigidBody, &Snapshot::rigidbodyData), inertiaTensor_(0.0){
47	gezelter	1490
48			}
49
50	gezelter	1930	void RigidBody::setPrevA(const RotMat3x3d& a) {
51			((snapshotMan_->getPrevSnapshot())->*storage_).aMat[localIndex_] = a;
52			//((snapshotMan_->getPrevSnapshot())->storage_).electroFrame[localIndex_] = a.transpose() sU_;
53	gezelter	1490
54	gezelter	1930	for (int i =0 ; i < atoms_.size(); ++i){
55			if (atoms_[i]->isDirectional()) {
56			atoms_[i]->setPrevA(a * refOrients_[i]);
57			}
58			}
59	gezelter	1490
60			}
61
62	gezelter	1930
63			void RigidBody::setA(const RotMat3x3d& a) {
64			((snapshotMan_->getCurrentSnapshot())->*storage_).aMat[localIndex_] = a;
65			//((snapshotMan_->getCurrentSnapshot())->storage_).electroFrame[localIndex_] = a.transpose() sU_;
66	gezelter	1490
67	gezelter	1930	for (int i =0 ; i < atoms_.size(); ++i){
68			if (atoms_[i]->isDirectional()) {
69			atoms_[i]->setA(a * refOrients_[i]);
70			}
71			}
72	gezelter	1490	}
73
74	gezelter	1930	void RigidBody::setA(const RotMat3x3d& a, int snapshotNo) {
75			((snapshotMan_->getSnapshot(snapshotNo))->*storage_).aMat[localIndex_] = a;
76			//((snapshotMan_->getSnapshot(snapshotNo))->storage_).electroFrame[localIndex_] = a.transpose() sU_;
77	gezelter	1490
78	gezelter	1930	for (int i =0 ; i < atoms_.size(); ++i){
79			if (atoms_[i]->isDirectional()) {
80			atoms_[i]->setA(a * refOrients_[i], snapshotNo);
81			}
82	gezelter	1490	}
83
84	gezelter	1930	}
85	gezelter	1490
86	gezelter	1930	Mat3x3d RigidBody::getI() {
87			return inertiaTensor_;
88			}
89	gezelter	1490
90	gezelter	1930	std::vector<double> RigidBody::getGrad() {
91			std::vector<double> grad(6, 0.0);
92			Vector3d force;
93			Vector3d torque;
94			Vector3d myEuler;
95			double phi, theta, psi;
96			double cphi, sphi, ctheta, stheta;
97			Vector3d ephi;
98			Vector3d etheta;
99			Vector3d epsi;
100	gezelter	1490
101	gezelter	1930	force = getFrc();
102			torque =getTrq();
103			myEuler = getA().toEulerAngles();
104	gezelter	1490
105	gezelter	1930	phi = myEuler[0];
106			theta = myEuler[1];
107			psi = myEuler[2];
108	gezelter	1490
109	gezelter	1930	cphi = cos(phi);
110			sphi = sin(phi);
111			ctheta = cos(theta);
112			stheta = sin(theta);
113	gezelter	1490
114	gezelter	1930	// get unit vectors along the phi, theta and psi rotation axes
115	gezelter	1490
116	gezelter	1930	ephi[0] = 0.0;
117			ephi[1] = 0.0;
118			ephi[2] = 1.0;
119	gezelter	1490
120	gezelter	1930	etheta[0] = cphi;
121			etheta[1] = sphi;
122			etheta[2] = 0.0;
123	gezelter	1490
124	gezelter	1930	epsi[0] = stheta * cphi;
125			epsi[1] = stheta * sphi;
126			epsi[2] = ctheta;
127	gezelter	1490
128	gezelter	1930	//gradient is equal to -force
129			for (int j = 0 ; j<3; j++)
130			grad[j] = -force[j];
131	gezelter	1490
132	gezelter	1930	for (int j = 0; j < 3; j++ ) {
133	gezelter	1490
134	gezelter	1930	grad[3] += torque[j]*ephi[j];
135			grad[4] += torque[j]*etheta[j];
136			grad[5] += torque[j]*epsi[j];
137	gezelter	1490
138	gezelter	1930	}
139
140			return grad;
141			}
142	gezelter	1490
143	gezelter	1930	void RigidBody::accept(BaseVisitor* v) {
144			v->visit(this);
145			}
146	gezelter	1490
147	gezelter	1930	/*@todo need modification /
148			void RigidBody::calcRefCoords() {
149			double mtmp;
150			Vector3d refCOM(0.0);
151			mass_ = 0.0;
152			for (std::size_t i = 0; i < atoms_.size(); ++i) {
153			mtmp = atoms_[i]->getMass();
154			mass_ += mtmp;
155			refCOM += refCoords_[i]*mtmp;
156			}
157			refCOM /= mass_;
158	gezelter	1490
159	gezelter	1930	// Next, move the origin of the reference coordinate system to the COM:
160			for (std::size_t i = 0; i < atoms_.size(); ++i) {
161			refCoords_[i] -= refCOM;
162			}
163	gezelter	1490
164	gezelter	1930	// Moment of Inertia calculation
165			Mat3x3d Itmp(0.0);
166	gezelter	1490
167	gezelter	1930	for (std::size_t i = 0; i < atoms_.size(); i++) {
168			mtmp = atoms_[i]->getMass();
169			Itmp -= outProduct(refCoords_[i], refCoords_[i]) * mtmp;
170			double r2 = refCoords_[i].lengthSquare();
171			Itmp(0, 0) += mtmp * r2;
172			Itmp(1, 1) += mtmp * r2;
173			Itmp(2, 2) += mtmp * r2;
174			}
175	gezelter	1490
176	gezelter	1930	//diagonalize
177			Vector3d evals;
178			Mat3x3d::diagonalize(Itmp, evals, sU_);
179	gezelter	1490
180	gezelter	1930	// zero out I and then fill the diagonals with the moments of inertia:
181			inertiaTensor_(0, 0) = evals[0];
182			inertiaTensor_(1, 1) = evals[1];
183			inertiaTensor_(2, 2) = evals[2];
184
185			int nLinearAxis = 0;
186			for (int i = 0; i < 3; i++) {
187			if (fabs(evals[i]) < oopse::epsilon) {
188			linear_ = true;
189			linearAxis_ = i;
190			++ nLinearAxis;
191			}
192			}
193	gezelter	1490
194	gezelter	1930	if (nLinearAxis > 1) {
195			sprintf( painCave.errMsg,
196			"RigidBody error.\n"
197			"\tOOPSE found more than one axis in this rigid body with a vanishing \n"
198			"\tmoment of inertia. This can happen in one of three ways:\n"
199			"\t 1) Only one atom was specified, or \n"
200			"\t 2) All atoms were specified at the same location, or\n"
201			"\t 3) The programmers did something stupid.\n"
202			"\tIt is silly to use a rigid body to describe this situation. Be smarter.\n"
203			);
204			painCave.isFatal = 1;
205			simError();
206			}
207	gezelter	1490
208			}
209
210	gezelter	1930	void RigidBody::calcForcesAndTorques() {
211			Vector3d afrc;
212			Vector3d atrq;
213			Vector3d apos;
214			Vector3d rpos;
215			Vector3d frc(0.0);
216			Vector3d trq(0.0);
217			Vector3d pos = this->getPos();
218			for (int i = 0; i < atoms_.size(); i++) {
219	gezelter	1490
220	gezelter	1930	afrc = atoms_[i]->getFrc();
221			apos = atoms_[i]->getPos();
222			rpos = apos - pos;
223
224			frc += afrc;
225	gezelter	1490
226	gezelter	1930	trq[0] += rpos[1]afrc[2] - rpos[2]afrc[1];
227			trq[1] += rpos[2]afrc[0] - rpos[0]afrc[2];
228			trq[2] += rpos[0]afrc[1] - rpos[1]afrc[0];
229	gezelter	1490
230	gezelter	1930	// If the atom has a torque associated with it, then we also need to
231			// migrate the torques onto the center of mass:
232	gezelter	1490
233	gezelter	1930	if (atoms_[i]->isDirectional()) {
234			atrq = atoms_[i]->getTrq();
235			trq += atrq;
236			}
237
238			}
239
240			setFrc(frc);
241			setTrq(trq);
242
243			}
244	gezelter	1490
245	gezelter	1930	void RigidBody::updateAtoms() {
246			unsigned int i;
247			Vector3d ref;
248			Vector3d apos;
249			DirectionalAtom* dAtom;
250			Vector3d pos = getPos();
251			RotMat3x3d a = getA();
252	gezelter	1490
253	gezelter	1930	for (i = 0; i < atoms_.size(); i++) {
254
255			ref = body2Lab(refCoords_[i]);
256	gezelter	1490
257	gezelter	1930	apos = pos + ref;
258	gezelter	1490
259	gezelter	1930	atoms_[i]->setPos(apos);
260	gezelter	1490
261	gezelter	1930	if (atoms_[i]->isDirectional()) {
262
263			dAtom = (DirectionalAtom *) atoms_[i];
264			dAtom->setA(a * refOrients_[i]);
265			//dAtom->rotateBy( A );
266			}
267	gezelter	1490
268			}
269
270	gezelter	1930	}
271	gezelter	1490
272
273	gezelter	1930	bool RigidBody::getAtomPos(Vector3d& pos, unsigned int index) {
274			if (index < atoms_.size()) {
275	gezelter	1490
276	gezelter	1930	Vector3d ref = body2Lab(refCoords_[index]);
277			pos = getPos() + ref;
278			return true;
279			} else {
280			std::cerr << index << " is an invalid index, current rigid body contains "
281			<< atoms_.size() << "atoms" << std::endl;
282			return false;
283			}
284			}
285	gezelter	1490
286	gezelter	1930	bool RigidBody::getAtomPos(Vector3d& pos, Atom* atom) {
287			std::vector<Atom*>::iterator i;
288			i = std::find(atoms_.begin(), atoms_.end(), atom);
289			if (i != atoms_.end()) {
290			//RigidBody class makes sure refCoords_ and atoms_ match each other
291			Vector3d ref = body2Lab(refCoords_[i - atoms_.begin()]);
292			pos = getPos() + ref;
293			return true;
294			} else {
295			std::cerr << "Atom " << atom->getGlobalIndex()
296			<<" does not belong to Rigid body "<< getGlobalIndex() << std::endl;
297			return false;
298	gezelter	1490	}
299			}
300	gezelter	1930	bool RigidBody::getAtomVel(Vector3d& vel, unsigned int index) {
301	gezelter	1490
302	gezelter	1930	//velRot = $(A\cdot skew(I^{-1}j))^{T}refCoor$
303	gezelter	1490
304	gezelter	1930	if (index < atoms_.size()) {
305	gezelter	1490
306	gezelter	1930	Vector3d velRot;
307			Mat3x3d skewMat;;
308			Vector3d ref = refCoords_[index];
309			Vector3d ji = getJ();
310			Mat3x3d I = getI();
311	gezelter	1490
312	gezelter	1930	skewMat(0, 0) =0;
313			skewMat(0, 1) = ji[2] /I(2, 2);
314			skewMat(0, 2) = -ji[1] /I(1, 1);
315	gezelter	1490
316	gezelter	1930	skewMat(1, 0) = -ji[2] /I(2, 2);
317			skewMat(1, 1) = 0;
318			skewMat(1, 2) = ji[0]/I(0, 0);
319	gezelter	1490
320	gezelter	1930	skewMat(2, 0) =ji[1] /I(1, 1);
321			skewMat(2, 1) = -ji[0]/I(0, 0);
322			skewMat(2, 2) = 0;
323	gezelter	1490
324	gezelter	1930	velRot = (getA() * skewMat).transpose() * ref;
325	gezelter	1490
326	gezelter	1930	vel =getVel() + velRot;
327			return true;
328
329			} else {
330			std::cerr << index << " is an invalid index, current rigid body contains "
331			<< atoms_.size() << "atoms" << std::endl;
332			return false;
333	gezelter	1490	}
334	gezelter	1930	}
335	gezelter	1490
336	gezelter	1930	bool RigidBody::getAtomVel(Vector3d& vel, Atom* atom) {
337	gezelter	1490
338	gezelter	1930	std::vector<Atom*>::iterator i;
339			i = std::find(atoms_.begin(), atoms_.end(), atom);
340			if (i != atoms_.end()) {
341			return getAtomVel(vel, i - atoms_.begin());
342			} else {
343			std::cerr << "Atom " << atom->getGlobalIndex()
344			<<" does not belong to Rigid body "<< getGlobalIndex() << std::endl;
345			return false;
346			}
347			}
348	gezelter	1490
349	gezelter	1930	bool RigidBody::getAtomRefCoor(Vector3d& coor, unsigned int index) {
350			if (index < atoms_.size()) {
351
352			coor = refCoords_[index];
353			return true;
354			} else {
355			std::cerr << index << " is an invalid index, current rigid body contains "
356			<< atoms_.size() << "atoms" << std::endl;
357			return false;
358	gezelter	1490	}
359
360			}
361
362	gezelter	1930	bool RigidBody::getAtomRefCoor(Vector3d& coor, Atom* atom) {
363			std::vector<Atom*>::iterator i;
364			i = std::find(atoms_.begin(), atoms_.end(), atom);
365			if (i != atoms_.end()) {
366			//RigidBody class makes sure refCoords_ and atoms_ match each other
367			coor = refCoords_[i - atoms_.begin()];
368			return true;
369			} else {
370			std::cerr << "Atom " << atom->getGlobalIndex()
371			<<" does not belong to Rigid body "<< getGlobalIndex() << std::endl;
372			return false;
373			}
374	gezelter	1490
375			}
376
377
378	gezelter	1930	void RigidBody::addAtom(Atom* at, AtomStamp* ats) {
379	gezelter	1490
380	gezelter	1930	Vector3d coords;
381			Vector3d euler;
382	gezelter	1490
383
384	gezelter	1930	atoms_.push_back(at);
385
386			if( !ats->havePosition() ){
387			sprintf( painCave.errMsg,
388			"RigidBody error.\n"
389			"\tAtom %s does not have a position specified.\n"
390			"\tThis means RigidBody cannot set up reference coordinates.\n",
391			ats->getType() );
392			painCave.isFatal = 1;
393			simError();
394	gezelter	1490	}
395
396	gezelter	1930	coords[0] = ats->getPosX();
397			coords[1] = ats->getPosY();
398			coords[2] = ats->getPosZ();
399	gezelter	1490
400	gezelter	1930	refCoords_.push_back(coords);
401	gezelter	1490
402	gezelter	1930	RotMat3x3d identMat = RotMat3x3d::identity();
403	gezelter	1490
404	gezelter	1930	if (at->isDirectional()) {
405	gezelter	1490
406	gezelter	1930	if( !ats->haveOrientation() ){
407			sprintf( painCave.errMsg,
408			"RigidBody error.\n"
409			"\tAtom %s does not have an orientation specified.\n"
410			"\tThis means RigidBody cannot set up reference orientations.\n",
411			ats->getType() );
412			painCave.isFatal = 1;
413			simError();
414			}
415
416			euler[0] = ats->getEulerPhi();
417			euler[1] = ats->getEulerTheta();
418			euler[2] = ats->getEulerPsi();
419	gezelter	1490
420	gezelter	1930	RotMat3x3d Atmp(euler);
421			refOrients_.push_back(Atmp);
422	gezelter	1490
423	gezelter	1930	}else {
424			refOrients_.push_back(identMat);
425	gezelter	1490	}
426
427
428			}
429
430			}
431