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:	246
Committed:	Wed Jan 12 22:41:40 2005 UTC (20 years, 3 months ago) by gezelter
Original Path:	*trunk/src/primitives/RigidBody.cpp*
File size:	12448 byte(s)
Log Message:	merging new_design branch into OOPSE-2.0
#	User	Rev	Content
1	gezelter	246	/*
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	3	#include "primitives/RigidBody.hpp"
43			#include "utils/simError.h"
44	gezelter	246	namespace oopse {
45	gezelter	2
46	gezelter	246	RigidBody::RigidBody() : StuntDouble(otRigidBody, &Snapshot::rigidbodyData), inertiaTensor_(0.0){
47	gezelter	2
48			}
49
50	gezelter	246	void RigidBody::setPrevA(const RotMat3x3d& a) {
51			((snapshotMan_->getPrevSnapshot())->*storage_).aMat[localIndex_] = a;
52			//((snapshotMan_->getPrevSnapshot())->storage_).electroFrame[localIndex_] = a.transpose() sU_;
53	gezelter	2
54	gezelter	246	for (int i =0 ; i < atoms_.size(); ++i){
55			if (atoms_[i]->isDirectional()) {
56			atoms_[i]->setPrevA(a * refOrients_[i]);
57			}
58			}
59	gezelter	2
60			}
61
62	gezelter	246
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	2
67	gezelter	246	for (int i =0 ; i < atoms_.size(); ++i){
68			if (atoms_[i]->isDirectional()) {
69			atoms_[i]->setA(a * refOrients_[i]);
70			}
71			}
72	gezelter	2	}
73
74	gezelter	246	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	2
78	gezelter	246	for (int i =0 ; i < atoms_.size(); ++i){
79			if (atoms_[i]->isDirectional()) {
80			atoms_[i]->setA(a * refOrients_[i], snapshotNo);
81			}
82	gezelter	2	}
83
84	gezelter	246	}
85	gezelter	2
86	gezelter	246	Mat3x3d RigidBody::getI() {
87			return inertiaTensor_;
88			}
89	gezelter	2
90	gezelter	246	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	2
101	gezelter	246	force = getFrc();
102			torque =getTrq();
103			myEuler = getA().toEulerAngles();
104	gezelter	2
105	gezelter	246	phi = myEuler[0];
106			theta = myEuler[1];
107			psi = myEuler[2];
108	gezelter	2
109	gezelter	246	cphi = cos(phi);
110			sphi = sin(phi);
111			ctheta = cos(theta);
112			stheta = sin(theta);
113	gezelter	2
114	gezelter	246	// get unit vectors along the phi, theta and psi rotation axes
115	gezelter	2
116	gezelter	246	ephi[0] = 0.0;
117			ephi[1] = 0.0;
118			ephi[2] = 1.0;
119	gezelter	2
120	gezelter	246	etheta[0] = cphi;
121			etheta[1] = sphi;
122			etheta[2] = 0.0;
123	gezelter	2
124	gezelter	246	epsi[0] = stheta * cphi;
125			epsi[1] = stheta * sphi;
126			epsi[2] = ctheta;
127	gezelter	2
128	gezelter	246	//gradient is equal to -force
129			for (int j = 0 ; j<3; j++)
130			grad[j] = -force[j];
131	gezelter	2
132	gezelter	246	for (int j = 0; j < 3; j++ ) {
133	gezelter	2
134	gezelter	246	grad[3] += torque[j]*ephi[j];
135			grad[4] += torque[j]*etheta[j];
136			grad[5] += torque[j]*epsi[j];
137	gezelter	2
138	gezelter	246	}
139
140			return grad;
141			}
142	gezelter	2
143	gezelter	246	void RigidBody::accept(BaseVisitor* v) {
144			v->visit(this);
145			}
146	gezelter	2
147	gezelter	246	/*@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	2
159	gezelter	246	// 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	2
164	gezelter	246	// Moment of Inertia calculation
165			Mat3x3d Itmp(0.0);
166	gezelter	2
167	gezelter	246	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	2
176	gezelter	246	//diagonalize
177			Vector3d evals;
178			Mat3x3d::diagonalize(Itmp, evals, sU_);
179	gezelter	2
180	gezelter	246	// 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	2
194	gezelter	246	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	2
208			}
209
210	gezelter	246	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	2
220	gezelter	246	afrc = atoms_[i]->getFrc();
221			apos = atoms_[i]->getPos();
222			rpos = apos - pos;
223
224			frc += afrc;
225	gezelter	2
226	gezelter	246	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	2
230	gezelter	246	// 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	2
233	gezelter	246	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	2
245	gezelter	246	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	2
253	gezelter	246	for (i = 0; i < atoms_.size(); i++) {
254
255			ref = body2Lab(refCoords_[i]);
256	gezelter	2
257	gezelter	246	apos = pos + ref;
258	gezelter	2
259	gezelter	246	atoms_[i]->setPos(apos);
260	gezelter	2
261	gezelter	246	if (atoms_[i]->isDirectional()) {
262
263			dAtom = (DirectionalAtom *) atoms_[i];
264			dAtom->setA(a * refOrients_[i]);
265			//dAtom->rotateBy( A );
266			}
267	gezelter	2
268			}
269
270	gezelter	246	}
271	gezelter	2
272
273	gezelter	246	bool RigidBody::getAtomPos(Vector3d& pos, unsigned int index) {
274			if (index < atoms_.size()) {
275	gezelter	2
276	gezelter	246	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	2
286	gezelter	246	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	2	}
299			}
300	gezelter	246	bool RigidBody::getAtomVel(Vector3d& vel, unsigned int index) {
301	gezelter	2
302	gezelter	246	//velRot = $(A\cdot skew(I^{-1}j))^{T}refCoor$
303	gezelter	2
304	gezelter	246	if (index < atoms_.size()) {
305	gezelter	2
306	gezelter	246	Vector3d velRot;
307			Mat3x3d skewMat;;
308			Vector3d ref = refCoords_[index];
309			Vector3d ji = getJ();
310			Mat3x3d I = getI();
311	gezelter	2
312	gezelter	246	skewMat(0, 0) =0;
313			skewMat(0, 1) = ji[2] /I(2, 2);
314			skewMat(0, 2) = -ji[1] /I(1, 1);
315	gezelter	2
316	gezelter	246	skewMat(1, 0) = -ji[2] /I(2, 2);
317			skewMat(1, 1) = 0;
318			skewMat(1, 2) = ji[0]/I(0, 0);
319	gezelter	2
320	gezelter	246	skewMat(2, 0) =ji[1] /I(1, 1);
321			skewMat(2, 1) = -ji[0]/I(0, 0);
322			skewMat(2, 2) = 0;
323	gezelter	2
324	gezelter	246	velRot = (getA() * skewMat).transpose() * ref;
325	gezelter	2
326	gezelter	246	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	2	}
334	gezelter	246	}
335	gezelter	2
336	gezelter	246	bool RigidBody::getAtomVel(Vector3d& vel, Atom* atom) {
337	gezelter	2
338	gezelter	246	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	2
349	gezelter	246	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	2	}
359
360			}
361
362	gezelter	246	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	2
375			}
376
377
378	gezelter	246	void RigidBody::addAtom(Atom* at, AtomStamp* ats) {
379	gezelter	2
380	gezelter	246	Vector3d coords;
381			Vector3d euler;
382	gezelter	2
383
384	gezelter	246	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	2	}
395
396	gezelter	246	coords[0] = ats->getPosX();
397			coords[1] = ats->getPosY();
398			coords[2] = ats->getPosZ();
399	gezelter	2
400	gezelter	246	refCoords_.push_back(coords);
401	gezelter	2
402	gezelter	246	RotMat3x3d identMat = RotMat3x3d::identity();
403	gezelter	2
404	gezelter	246	if (at->isDirectional()) {
405	gezelter	2
406	gezelter	246	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	2
420	gezelter	246	RotMat3x3d Atmp(euler);
421			refOrients_.push_back(Atmp);
422	gezelter	2
423	gezelter	246	}else {
424			refOrients_.push_back(identMat);
425	gezelter	2	}
426
427
428			}
429
430			}
431