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:	1929
Committed:	Wed Jan 12 18:13:36 2005 UTC (20 years, 6 months ago) by tim
File size:	12448 byte(s)
Log Message:	forget to include <algorithm>
#	User	Rev	Content
1	tim	1927	/*
2			* Copyright (c) 2005 The University of Notre Dame. All Rights Reserved.
3	tim	1692	*
4	tim	1927	* 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	tim	1692	*/
41	tim	1929	#include <algorithm>
42	tim	1492	#include "primitives/RigidBody.hpp"
43	tim	1809	#include "utils/simError.h"
44	tim	1692	namespace oopse {
45	gezelter	1490
46	tim	1811	RigidBody::RigidBody() : StuntDouble(otRigidBody, &Snapshot::rigidbodyData), inertiaTensor_(0.0){
47	tim	1692
48	gezelter	1490	}
49
50	tim	1692	void RigidBody::setPrevA(const RotMat3x3d& a) {
51			((snapshotMan_->getPrevSnapshot())->*storage_).aMat[localIndex_] = a;
52	tim	1813	//((snapshotMan_->getPrevSnapshot())->storage_).electroFrame[localIndex_] = a.transpose() sU_;
53	gezelter	1490
54	tim	1903	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	tim	1692
63			void RigidBody::setA(const RotMat3x3d& a) {
64			((snapshotMan_->getCurrentSnapshot())->*storage_).aMat[localIndex_] = a;
65	tim	1813	//((snapshotMan_->getCurrentSnapshot())->storage_).electroFrame[localIndex_] = a.transpose() sU_;
66	gezelter	1490
67	tim	1903	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	tim	1692	void RigidBody::setA(const RotMat3x3d& a, int snapshotNo) {
75			((snapshotMan_->getSnapshot(snapshotNo))->*storage_).aMat[localIndex_] = a;
76	tim	1813	//((snapshotMan_->getSnapshot(snapshotNo))->storage_).electroFrame[localIndex_] = a.transpose() sU_;
77	gezelter	1490
78	tim	1903	for (int i =0 ; i < atoms_.size(); ++i){
79			if (atoms_[i]->isDirectional()) {
80			atoms_[i]->setA(a * refOrients_[i], snapshotNo);
81			}
82			}
83	gezelter	1490
84	tim	1692	}
85	gezelter	1490
86	tim	1692	Mat3x3d RigidBody::getI() {
87			return inertiaTensor_;
88			}
89	gezelter	1490
90	tim	1692	std::vector<double> RigidBody::getGrad() {
91	tim	1826	std::vector<double> grad(6, 0.0);
92	tim	1692	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	tim	1692	force = getFrc();
102			torque =getTrq();
103			myEuler = getA().toEulerAngles();
104	gezelter	1490
105	tim	1692	phi = myEuler[0];
106			theta = myEuler[1];
107			psi = myEuler[2];
108	gezelter	1490
109	tim	1692	cphi = cos(phi);
110			sphi = sin(phi);
111			ctheta = cos(theta);
112			stheta = sin(theta);
113	gezelter	1490
114	tim	1692	// get unit vectors along the phi, theta and psi rotation axes
115	gezelter	1490
116	tim	1692	ephi[0] = 0.0;
117			ephi[1] = 0.0;
118			ephi[2] = 1.0;
119	gezelter	1490
120	tim	1692	etheta[0] = cphi;
121			etheta[1] = sphi;
122			etheta[2] = 0.0;
123	gezelter	1490
124	tim	1692	epsi[0] = stheta * cphi;
125			epsi[1] = stheta * sphi;
126			epsi[2] = ctheta;
127	gezelter	1490
128	tim	1692	//gradient is equal to -force
129			for (int j = 0 ; j<3; j++)
130			grad[j] = -force[j];
131	gezelter	1490
132	tim	1692	for (int j = 0; j < 3; j++ ) {
133	gezelter	1490
134	tim	1692	grad[3] += torque[j]*ephi[j];
135			grad[4] += torque[j]*etheta[j];
136			grad[5] += torque[j]*epsi[j];
137	gezelter	1490
138	tim	1692	}
139
140			return grad;
141			}
142	gezelter	1490
143	tim	1692	void RigidBody::accept(BaseVisitor* v) {
144			v->visit(this);
145			}
146	gezelter	1490
147	tim	1805	/*@todo need modification /
148	tim	1692	void RigidBody::calcRefCoords() {
149	tim	1811	double mtmp;
150			Vector3d refCOM(0.0);
151			mass_ = 0.0;
152	tim	1883	for (std::size_t i = 0; i < atoms_.size(); ++i) {
153	tim	1811	mtmp = atoms_[i]->getMass();
154			mass_ += mtmp;
155			refCOM += refCoords_[i]*mtmp;
156			}
157			refCOM /= mass_;
158	gezelter	1490
159	tim	1811	// Next, move the origin of the reference coordinate system to the COM:
160	tim	1883	for (std::size_t i = 0; i < atoms_.size(); ++i) {
161	tim	1811	refCoords_[i] -= refCOM;
162	gezelter	1490	}
163
164			// Moment of Inertia calculation
165	tim	1811	Mat3x3d Itmp(0.0);
166	gezelter	1490
167	tim	1883	for (std::size_t i = 0; i < atoms_.size(); i++) {
168	tim	1811	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	gezelter	1490	}
175
176	tim	1811	//diagonalize
177			Vector3d evals;
178	tim	1815	Mat3x3d::diagonalize(Itmp, evals, sU_);
179	gezelter	1490
180	tim	1811	// 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	gezelter	1490	}
193
194	tim	1811	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	gezelter	1490	}
207
208			}
209
210	tim	1692	void RigidBody::calcForcesAndTorques() {
211			Vector3d afrc;
212			Vector3d atrq;
213	tim	1870	Vector3d apos;
214	tim	1692	Vector3d rpos;
215	tim	1870	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	tim	1692	afrc = atoms_[i]->getFrc();
221	tim	1870	apos = atoms_[i]->getPos();
222			rpos = apos - pos;
223	tim	1692
224			frc += afrc;
225	gezelter	1490
226	tim	1692	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	tim	1692	// 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	tim	1692	if (atoms_[i]->isDirectional()) {
234			atrq = atoms_[i]->getTrq();
235			trq += atrq;
236			}
237
238	gezelter	1490	}
239
240	tim	1692	setFrc(frc);
241			setTrq(trq);
242
243	gezelter	1490	}
244
245	tim	1692	void RigidBody::updateAtoms() {
246			unsigned int i;
247			Vector3d ref;
248			Vector3d apos;
249			DirectionalAtom* dAtom;
250			Vector3d pos = getPos();
251	tim	1813	RotMat3x3d a = getA();
252	tim	1692
253			for (i = 0; i < atoms_.size(); i++) {
254	gezelter	1490
255	tim	1692	ref = body2Lab(refCoords_[i]);
256	gezelter	1490
257	tim	1692	apos = pos + ref;
258	gezelter	1490
259	tim	1692	atoms_[i]->setPos(apos);
260	gezelter	1490
261	tim	1692	if (atoms_[i]->isDirectional()) {
262
263			dAtom = (DirectionalAtom *) atoms_[i];
264	tim	1813	dAtom->setA(a * refOrients_[i]);
265			//dAtom->rotateBy( A );
266	tim	1692	}
267	gezelter	1490
268	tim	1692	}
269	gezelter	1490
270			}
271
272
273	tim	1692	bool RigidBody::getAtomPos(Vector3d& pos, unsigned int index) {
274			if (index < atoms_.size()) {
275	gezelter	1490
276	tim	1692	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	gezelter	1490	}
285
286	tim	1692	bool RigidBody::getAtomPos(Vector3d& pos, Atom* atom) {
287			std::vector<Atom*>::iterator i;
288	tim	1927	i = std::find(atoms_.begin(), atoms_.end(), atom);
289	tim	1692	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			}
299	gezelter	1490	}
300	tim	1692	bool RigidBody::getAtomVel(Vector3d& vel, unsigned int index) {
301	gezelter	1490
302	tim	1692	//velRot = $(A\cdot skew(I^{-1}j))^{T}refCoor$
303	gezelter	1490
304	tim	1692	if (index < atoms_.size()) {
305	gezelter	1490
306	tim	1692	Vector3d velRot;
307			Mat3x3d skewMat;;
308			Vector3d ref = refCoords_[index];
309			Vector3d ji = getJ();
310			Mat3x3d I = getI();
311	gezelter	1490
312	tim	1692	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	tim	1692	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	tim	1692	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	tim	1692	velRot = (getA() * skewMat).transpose() * ref;
325	gezelter	1490
326	tim	1692	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			}
334			}
335	gezelter	1490
336	tim	1692	bool RigidBody::getAtomVel(Vector3d& vel, Atom* atom) {
337	gezelter	1490
338	tim	1692	std::vector<Atom*>::iterator i;
339	tim	1927	i = std::find(atoms_.begin(), atoms_.end(), atom);
340	tim	1692	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	gezelter	1490	}
348
349	tim	1692	bool RigidBody::getAtomRefCoor(Vector3d& coor, unsigned int index) {
350			if (index < atoms_.size()) {
351	gezelter	1490
352	tim	1692	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			}
359	gezelter	1490
360	tim	1692	}
361	gezelter	1490
362	tim	1692	bool RigidBody::getAtomRefCoor(Vector3d& coor, Atom* atom) {
363			std::vector<Atom*>::iterator i;
364	tim	1927	i = std::find(atoms_.begin(), atoms_.end(), atom);
365	tim	1692	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	tim	1692	}
376	gezelter	1490
377	tim	1805
378			void RigidBody::addAtom(Atom* at, AtomStamp* ats) {
379
380			Vector3d coords;
381			Vector3d euler;
382	tim	1813
383	tim	1805
384			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			}
395
396			coords[0] = ats->getPosX();
397			coords[1] = ats->getPosY();
398			coords[2] = ats->getPosZ();
399
400			refCoords_.push_back(coords);
401
402	tim	1813	RotMat3x3d identMat = RotMat3x3d::identity();
403
404	tim	1805	if (at->isDirectional()) {
405
406			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	tim	1813
420			RotMat3x3d Atmp(euler);
421			refOrients_.push_back(Atmp);
422	tim	1805
423	tim	1813	}else {
424			refOrients_.push_back(identMat);
425	tim	1805	}
426
427	tim	1813
428	gezelter	1490	}
429
430	tim	1805	}
431