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