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;
    }

    //project the inertial moment of directional atoms into this rigid body
    for (std::size_t i = 0; i < atoms_.size(); i++) {
        if (atoms_[i]->isDirectional()) {
            RectMatrix<double, 3, 3> Iproject = refOrients_[i].transpose() * atoms_[i]->getI();
            Itmp(0, 0) += Iproject(0, 0);
            Itmp(1, 1) += Iproject(1, 1);
            Itmp(2, 2) += Iproject(2, 2);
        }
    }

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