src/primitives/RigidBody.cpp

/*
 * Copyright (C) 2000-2004  Object Oriented Parallel Simulation Engine (OOPSE) project
 * 
 * Contact: oopse@oopse.org
 * 
 * This program is free software; you can redistribute it and/or
 * modify it under the terms of the GNU Lesser General Public License
 * as published by the Free Software Foundation; either version 2.1
 * of the License, or (at your option) any later version.
 * All we ask is that proper credit is given for our work, which includes
 * - but is not limited to - adding the above copyright notice to the beginning
 * of your source code files, and to any copyright notice that you may distribute
 * with programs based on this work.
 * 
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU Lesser General Public License for more details.
 * 
 * You should have received a copy of the GNU Lesser General Public License
 * along with this program; if not, write to the Free Software
 * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.
 *
 */

#include "primitives/RigidBody.hpp"

namespace oopse {

RigidBody::RigidBody() : StuntDouble(otRigidBody, &Snapshot::rigidbodyData){

}

void RigidBody::setPrevA(const RotMat3x3d& a) {
    ((snapshotMan_->getPrevSnapshot())->*storage_).aMat[localIndex_] = a;
    ((snapshotMan_->getPrevSnapshot())->*storage_).unitFrame[localIndex_] = a.transpose() * sU_;

    std::vector<Atom*>::iterator i;
    for (i = atoms_.begin(); i != atoms_.end(); ++i) {
        if ((*i)->isDirectional()) {
            (*i)->setPrevA(a * (*i)->getPrevA());
        }
    }

}

      
void RigidBody::setA(const RotMat3x3d& a) {
    ((snapshotMan_->getCurrentSnapshot())->*storage_).aMat[localIndex_] = a;
    ((snapshotMan_->getCurrentSnapshot())->*storage_).unitFrame[localIndex_] = a.transpose() * sU_;

    std::vector<Atom*>::iterator i;
    for (i = atoms_.begin(); i != atoms_.end(); ++i) {
        if ((*i)->isDirectional()) {
            (*i)->setA(a * (*i)->getA());
        }
    }
}    
    
void RigidBody::setA(const RotMat3x3d& a, int snapshotNo) {
    ((snapshotMan_->getSnapshot(snapshotNo))->*storage_).aMat[localIndex_] = a;
    ((snapshotMan_->getSnapshot(snapshotNo))->*storage_).unitFrame[localIndex_] = a.transpose() * sU_;    

    std::vector<Atom*>::iterator i;
    for (i = atoms_.begin(); i != atoms_.end(); ++i) {
        if ((*i)->isDirectional()) {
            (*i)->setA(a * (*i)->getA(snapshotNo), snapshotNo);
        }
    }

}   

void  DirectionalAtom::setUnitFrameFromEuler(double phi, double theta, double psi) {
    sU_.setupRotMat(phi,theta,psi);
}

Mat3x3d RigidBody::getI() {
    return inertiaTensor_;
}    

std::vector<double> RigidBody::getGrad() {
    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;
  vec3 apos;
  double refCOM[3];
  vec3 ptmp;
  double Itmp[3][3];
  double evals[3];
  double evects[3][3];
  double r, r2, len;

  // First, find the center of mass:
  
  mass = 0.0;
  for (j=0; j<3; j++)
    refCOM[j] = 0.0;
  
  for (i = 0; i < atoms_.size(); i++) {
    mtmp = atoms_[i]->getMass();
    mass += mtmp;

    apos = refCoords[i];
    
    for(j = 0; j < 3; j++) {
      refCOM[j] += apos[j]*mtmp;     
    }    
  }
  
  for(j = 0; j < 3; j++) 
    refCOM[j] /= mass;

// Next, move the origin of the reference coordinate system to the COM:

  for (i = 0; i < atoms_.size(); i++) {
    apos = refCoords[i];
    for (j=0; j < 3; j++) {
      apos[j] = apos[j] - refCOM[j];
    }
    refCoords[i] = apos;
  }

// Moment of Inertia calculation

  for (i = 0; i < 3; i++) 
    for (j = 0; j < 3; j++)
      Itmp[i][j] = 0.0;  
  
  for (it = 0; it < atoms_.size(); it++) {

    mtmp = atoms_[it]->getMass();
    ptmp = refCoords[it];
    r= norm3(ptmp.vec);
    r2 = r*r;
    
    for (i = 0; i < 3; i++) {
      for (j = 0; j < 3; j++) {
        
        if (i==j) Itmp[i][j] += mtmp * r2;

        Itmp[i][j] -= mtmp * ptmp.vec[i]*ptmp.vec[j];
      }
    }
  }
  
  diagonalize3x3(Itmp, evals, sU);
  
  // zero out I and then fill the diagonals with the moments of inertia:

  n_linear_coords = 0;

  for (i = 0; i < 3; i++) {
    for (j = 0; j < 3; j++) {
      I[i][j] = 0.0;  
    }
    I[i][i] = evals[i];

    if (fabs(evals[i]) < momIntTol) {
      is_linear = true;
      n_linear_coords++;
      linear_axis = i;
    }
  }

  if (n_linear_coords > 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();
  }
  
  // renormalize column vectors:
  
  for (i=0; i < 3; i++) {
    len = 0.0;
    for (j = 0; j < 3; j++) {
      len += sU[i][j]*sU[i][j];
    }
    len = sqrt(len);
    for (j = 0; j < 3; j++) {
      sU[i][j] /= len;
    }
  }
  */
}

void  RigidBody::calcForcesAndTorques() {
    unsigned int i;
    unsigned int j;
    Vector3d afrc;
    Vector3d atrq;
    Vector3d rpos;
    Vector3d frc;
    Vector3d trq;


    zeroForces();
    
    frc = getFrc();
    trq = getTrq();

    for (i = 0; i < atoms_.size(); i++) {

        afrc = atoms_[i]->getFrc();

        rpos = refCoords_[i];
        
        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;
    unsigned int j;
    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->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 = 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 = 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 = 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;
  Mat3x3d Atmp;

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

  /*
  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();
    
    doEulerToRotMat(euler, Atmp);
    refOrients.push_back(Atmp);
    
  }
  */
  
}

}

Revision:	1805
Committed:	Tue Nov 30 20:50:47 2004 UTC (19 years, 9 months ago) by tim
File size:	12121 byte(s)
Log Message:	brains get built, io is next
#	User	Rev	Content
1	tim	1692	/*
2			* Copyright (C) 2000-2004 Object Oriented Parallel Simulation Engine (OOPSE) project
3			*
4			* Contact: oopse@oopse.org
5			*
6			* This program is free software; you can redistribute it and/or
7			* modify it under the terms of the GNU Lesser General Public License
8			* as published by the Free Software Foundation; either version 2.1
9			* of the License, or (at your option) any later version.
10			* All we ask is that proper credit is given for our work, which includes
11			* - but is not limited to - adding the above copyright notice to the beginning
12			* of your source code files, and to any copyright notice that you may distribute
13			* with programs based on this work.
14			*
15			* This program is distributed in the hope that it will be useful,
16			* but WITHOUT ANY WARRANTY; without even the implied warranty of
17			* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
18			* GNU Lesser General Public License for more details.
19			*
20			* You should have received a copy of the GNU Lesser General Public License
21			* along with this program; if not, write to the Free Software
22			* Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
23			*
24			*/
25
26	tim	1492	#include "primitives/RigidBody.hpp"
27	gezelter	1490
28	tim	1692	namespace oopse {
29	gezelter	1490
30	tim	1692	RigidBody::RigidBody() : StuntDouble(otRigidBody, &Snapshot::rigidbodyData){
31
32	gezelter	1490	}
33
34	tim	1692	void RigidBody::setPrevA(const RotMat3x3d& a) {
35			((snapshotMan_->getPrevSnapshot())->*storage_).aMat[localIndex_] = a;
36	tim	1782	((snapshotMan_->getPrevSnapshot())->storage_).unitFrame[localIndex_] = a.transpose() sU_;
37	gezelter	1490
38	tim	1692	std::vector<Atom*>::iterator i;
39			for (i = atoms_.begin(); i != atoms_.end(); ++i) {
40			if ((*i)->isDirectional()) {
41			(i)->setPrevA(a (*i)->getPrevA());
42			}
43			}
44	gezelter	1490
45			}
46
47	tim	1692
48			void RigidBody::setA(const RotMat3x3d& a) {
49			((snapshotMan_->getCurrentSnapshot())->*storage_).aMat[localIndex_] = a;
50	tim	1782	((snapshotMan_->getCurrentSnapshot())->storage_).unitFrame[localIndex_] = a.transpose() sU_;
51	gezelter	1490
52	tim	1692	std::vector<Atom*>::iterator i;
53			for (i = atoms_.begin(); i != atoms_.end(); ++i) {
54			if ((*i)->isDirectional()) {
55			(i)->setA(a (*i)->getA());
56			}
57			}
58	gezelter	1490	}
59
60	tim	1692	void RigidBody::setA(const RotMat3x3d& a, int snapshotNo) {
61			((snapshotMan_->getSnapshot(snapshotNo))->*storage_).aMat[localIndex_] = a;
62	tim	1782	((snapshotMan_->getSnapshot(snapshotNo))->storage_).unitFrame[localIndex_] = a.transpose() sU_;
63	gezelter	1490
64	tim	1692	std::vector<Atom*>::iterator i;
65			for (i = atoms_.begin(); i != atoms_.end(); ++i) {
66			if ((*i)->isDirectional()) {
67			(i)->setA(a (*i)->getA(snapshotNo), snapshotNo);
68			}
69	gezelter	1490	}
70
71	tim	1692	}
72	gezelter	1490
73	tim	1692	void DirectionalAtom::setUnitFrameFromEuler(double phi, double theta, double psi) {
74			sU_.setupRotMat(phi,theta,psi);
75	gezelter	1490	}
76
77	tim	1692	Mat3x3d RigidBody::getI() {
78			return inertiaTensor_;
79			}
80	gezelter	1490
81	tim	1692	std::vector<double> RigidBody::getGrad() {
82			vector<double> grad(6, 0.0);
83			Vector3d force;
84			Vector3d torque;
85			Vector3d myEuler;
86			double phi, theta, psi;
87			double cphi, sphi, ctheta, stheta;
88			Vector3d ephi;
89			Vector3d etheta;
90			Vector3d epsi;
91	gezelter	1490
92	tim	1692	force = getFrc();
93			torque =getTrq();
94			myEuler = getA().toEulerAngles();
95	gezelter	1490
96	tim	1692	phi = myEuler[0];
97			theta = myEuler[1];
98			psi = myEuler[2];
99	gezelter	1490
100	tim	1692	cphi = cos(phi);
101			sphi = sin(phi);
102			ctheta = cos(theta);
103			stheta = sin(theta);
104	gezelter	1490
105	tim	1692	// get unit vectors along the phi, theta and psi rotation axes
106	gezelter	1490
107	tim	1692	ephi[0] = 0.0;
108			ephi[1] = 0.0;
109			ephi[2] = 1.0;
110	gezelter	1490
111	tim	1692	etheta[0] = cphi;
112			etheta[1] = sphi;
113			etheta[2] = 0.0;
114	gezelter	1490
115	tim	1692	epsi[0] = stheta * cphi;
116			epsi[1] = stheta * sphi;
117			epsi[2] = ctheta;
118	gezelter	1490
119	tim	1692	//gradient is equal to -force
120			for (int j = 0 ; j<3; j++)
121			grad[j] = -force[j];
122	gezelter	1490
123	tim	1692	for (int j = 0; j < 3; j++ ) {
124	gezelter	1490
125	tim	1692	grad[3] += torque[j]*ephi[j];
126			grad[4] += torque[j]*etheta[j];
127			grad[5] += torque[j]*epsi[j];
128	gezelter	1490
129	tim	1692	}
130
131			return grad;
132			}
133	gezelter	1490
134	tim	1692	void RigidBody::accept(BaseVisitor* v) {
135			v->visit(this);
136			}
137	gezelter	1490
138	tim	1805	/*@todo need modification /
139	tim	1692	void RigidBody::calcRefCoords() {
140			/*
141	gezelter	1490	double mtmp;
142			vec3 apos;
143			double refCOM[3];
144			vec3 ptmp;
145			double Itmp[3][3];
146			double evals[3];
147			double evects[3][3];
148			double r, r2, len;
149
150			// First, find the center of mass:
151
152			mass = 0.0;
153			for (j=0; j<3; j++)
154			refCOM[j] = 0.0;
155
156	tim	1692	for (i = 0; i < atoms_.size(); i++) {
157			mtmp = atoms_[i]->getMass();
158	gezelter	1490	mass += mtmp;
159
160			apos = refCoords[i];
161
162			for(j = 0; j < 3; j++) {
163			refCOM[j] += apos[j]*mtmp;
164			}
165			}
166
167			for(j = 0; j < 3; j++)
168			refCOM[j] /= mass;
169
170			// Next, move the origin of the reference coordinate system to the COM:
171
172	tim	1692	for (i = 0; i < atoms_.size(); i++) {
173	gezelter	1490	apos = refCoords[i];
174			for (j=0; j < 3; j++) {
175			apos[j] = apos[j] - refCOM[j];
176			}
177			refCoords[i] = apos;
178			}
179
180			// Moment of Inertia calculation
181
182			for (i = 0; i < 3; i++)
183			for (j = 0; j < 3; j++)
184			Itmp[i][j] = 0.0;
185
186	tim	1692	for (it = 0; it < atoms_.size(); it++) {
187	gezelter	1490
188	tim	1692	mtmp = atoms_[it]->getMass();
189	gezelter	1490	ptmp = refCoords[it];
190			r= norm3(ptmp.vec);
191			r2 = r*r;
192
193			for (i = 0; i < 3; i++) {
194			for (j = 0; j < 3; j++) {
195
196			if (i==j) Itmp[i][j] += mtmp * r2;
197
198			Itmp[i][j] -= mtmp * ptmp.vec[i]*ptmp.vec[j];
199			}
200			}
201			}
202
203			diagonalize3x3(Itmp, evals, sU);
204
205			// zero out I and then fill the diagonals with the moments of inertia:
206
207			n_linear_coords = 0;
208
209			for (i = 0; i < 3; i++) {
210			for (j = 0; j < 3; j++) {
211			I[i][j] = 0.0;
212			}
213			I[i][i] = evals[i];
214
215			if (fabs(evals[i]) < momIntTol) {
216			is_linear = true;
217			n_linear_coords++;
218			linear_axis = i;
219			}
220			}
221
222			if (n_linear_coords > 1) {
223			sprintf( painCave.errMsg,
224			"RigidBody error.\n"
225			"\tOOPSE found more than one axis in this rigid body with a vanishing \n"
226			"\tmoment of inertia. This can happen in one of three ways:\n"
227			"\t 1) Only one atom was specified, or \n"
228			"\t 2) All atoms were specified at the same location, or\n"
229			"\t 3) The programmers did something stupid.\n"
230			"\tIt is silly to use a rigid body to describe this situation. Be smarter.\n"
231			);
232			painCave.isFatal = 1;
233			simError();
234			}
235
236			// renormalize column vectors:
237
238			for (i=0; i < 3; i++) {
239			len = 0.0;
240			for (j = 0; j < 3; j++) {
241			len += sU[i][j]*sU[i][j];
242			}
243			len = sqrt(len);
244			for (j = 0; j < 3; j++) {
245			sU[i][j] /= len;
246			}
247			}
248	tim	1692	*/
249	gezelter	1490	}
250
251	tim	1692	void RigidBody::calcForcesAndTorques() {
252			unsigned int i;
253			unsigned int j;
254			Vector3d afrc;
255			Vector3d atrq;
256			Vector3d rpos;
257			Vector3d frc;
258			Vector3d trq;
259	gezelter	1490
260	tim	1738
261	tim	1692	zeroForces();
262
263			frc = getFrc();
264			trq = getTrq();
265	gezelter	1490
266	tim	1692	for (i = 0; i < atoms_.size(); i++) {
267	gezelter	1490
268	tim	1692	afrc = atoms_[i]->getFrc();
269	gezelter	1490
270	tim	1692	rpos = refCoords_[i];
271
272			frc += afrc;
273	gezelter	1490
274	tim	1692	trq[0] += rpos[1]afrc[2] - rpos[2]afrc[1];
275			trq[1] += rpos[2]afrc[0] - rpos[0]afrc[2];
276			trq[2] += rpos[0]afrc[1] - rpos[1]afrc[0];
277	gezelter	1490
278	tim	1692	// If the atom has a torque associated with it, then we also need to
279			// migrate the torques onto the center of mass:
280	gezelter	1490
281	tim	1692	if (atoms_[i]->isDirectional()) {
282			atrq = atoms_[i]->getTrq();
283			trq += atrq;
284			}
285
286	gezelter	1490	}
287
288	tim	1692	setFrc(frc);
289			setTrq(trq);
290
291	gezelter	1490	}
292
293	tim	1692	void RigidBody::updateAtoms() {
294			unsigned int i;
295			unsigned int j;
296			Vector3d ref;
297			Vector3d apos;
298			DirectionalAtom* dAtom;
299			Vector3d pos = getPos();
300			RotMat3x3d A = getA();
301
302			for (i = 0; i < atoms_.size(); i++) {
303	gezelter	1490
304	tim	1692	ref = body2Lab(refCoords_[i]);
305	gezelter	1490
306	tim	1692	apos = pos + ref;
307	gezelter	1490
308	tim	1692	atoms_[i]->setPos(apos);
309	gezelter	1490
310	tim	1692	if (atoms_[i]->isDirectional()) {
311
312			dAtom = (DirectionalAtom *) atoms_[i];
313			dAtom->rotateBy( A );
314			}
315	gezelter	1490
316	tim	1692	}
317	gezelter	1490
318			}
319
320
321	tim	1692	bool RigidBody::getAtomPos(Vector3d& pos, unsigned int index) {
322			if (index < atoms_.size()) {
323	gezelter	1490
324	tim	1692	Vector3d ref = body2Lab(refCoords_[index]);
325			pos = getPos() + ref;
326			return true;
327			} else {
328			std::cerr << index << " is an invalid index, current rigid body contains "
329			<< atoms_.size() << "atoms" << std::endl;
330			return false;
331			}
332	gezelter	1490	}
333
334	tim	1692	bool RigidBody::getAtomPos(Vector3d& pos, Atom* atom) {
335			std::vector<Atom*>::iterator i;
336			i = find(atoms_.begin(), atoms_.end(), atom);
337			if (i != atoms_.end()) {
338			//RigidBody class makes sure refCoords_ and atoms_ match each other
339			Vector3d ref = body2Lab(refCoords_[i - atoms_.begin()]);
340			pos = getPos() + ref;
341			return true;
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	tim	1692	bool RigidBody::getAtomVel(Vector3d& vel, unsigned int index) {
349	gezelter	1490
350	tim	1692	//velRot = $(A\cdot skew(I^{-1}j))^{T}refCoor$
351	gezelter	1490
352	tim	1692	if (index < atoms_.size()) {
353	gezelter	1490
354	tim	1692	Vector3d velRot;
355			Mat3x3d skewMat;;
356			Vector3d ref = refCoords_[index];
357			Vector3d ji = getJ();
358			Mat3x3d I = getI();
359	gezelter	1490
360	tim	1692	skewMat(0, 0) =0;
361			skewMat(0, 1) = ji[2] /I(2, 2);
362			skewMat(0, 2) = -ji[1] /I(1, 1);
363	gezelter	1490
364	tim	1692	skewMat(1, 0) = -ji[2] /I(2, 2);
365			skewMat(1, 1) = 0;
366			skewMat(1, 2) = ji[0]/I(0, 0);
367	gezelter	1490
368	tim	1692	skewMat(2, 0) =ji[1] /I(1, 1);
369			skewMat(2, 1) = -ji[0]/I(0, 0);
370			skewMat(2, 2) = 0;
371	gezelter	1490
372	tim	1692	velRot = (getA() * skewMat).transpose() * ref;
373	gezelter	1490
374	tim	1692	vel =getVel() + velRot;
375			return true;
376
377			} else {
378			std::cerr << index << " is an invalid index, current rigid body contains "
379			<< atoms_.size() << "atoms" << std::endl;
380			return false;
381			}
382			}
383	gezelter	1490
384	tim	1692	bool RigidBody::getAtomVel(Vector3d& vel, Atom* atom) {
385	gezelter	1490
386	tim	1692	std::vector<Atom*>::iterator i;
387			i = find(atoms_.begin(), atoms_.end(), atom);
388			if (i != atoms_.end()) {
389			return getAtomVel(vel, i - atoms_.begin());
390			} else {
391			std::cerr << "Atom " << atom->getGlobalIndex()
392			<<" does not belong to Rigid body "<< getGlobalIndex() << std::endl;
393			return false;
394			}
395	gezelter	1490	}
396
397	tim	1692	bool RigidBody::getAtomRefCoor(Vector3d& coor, unsigned int index) {
398			if (index < atoms_.size()) {
399	gezelter	1490
400	tim	1692	coor = refCoords_[index];
401			return true;
402			} else {
403			std::cerr << index << " is an invalid index, current rigid body contains "
404			<< atoms_.size() << "atoms" << std::endl;
405			return false;
406			}
407	gezelter	1490
408	tim	1692	}
409	gezelter	1490
410	tim	1692	bool RigidBody::getAtomRefCoor(Vector3d& coor, Atom* atom) {
411			std::vector<Atom*>::iterator i;
412			i = find(atoms_.begin(), atoms_.end(), atom);
413			if (i != atoms_.end()) {
414			//RigidBody class makes sure refCoords_ and atoms_ match each other
415			coor = refCoords_[i - atoms_.begin()];
416			return true;
417			} else {
418			std::cerr << "Atom " << atom->getGlobalIndex()
419			<<" does not belong to Rigid body "<< getGlobalIndex() << std::endl;
420			return false;
421			}
422	gezelter	1490
423	tim	1692	}
424	gezelter	1490
425	tim	1805
426			void RigidBody::addAtom(Atom* at, AtomStamp* ats) {
427
428			Vector3d coords;
429			Vector3d euler;
430			Mat3x3d Atmp;
431
432			atoms_.push_back(at);
433
434			if( !ats->havePosition() ){
435			sprintf( painCave.errMsg,
436			"RigidBody error.\n"
437			"\tAtom %s does not have a position specified.\n"
438			"\tThis means RigidBody cannot set up reference coordinates.\n",
439			ats->getType() );
440			painCave.isFatal = 1;
441			simError();
442			}
443
444			coords[0] = ats->getPosX();
445			coords[1] = ats->getPosY();
446			coords[2] = ats->getPosZ();
447
448			refCoords_.push_back(coords);
449
450			/*
451			if (at->isDirectional()) {
452
453			if( !ats->haveOrientation() ){
454			sprintf( painCave.errMsg,
455			"RigidBody error.\n"
456			"\tAtom %s does not have an orientation specified.\n"
457			"\tThis means RigidBody cannot set up reference orientations.\n",
458			ats->getType() );
459			painCave.isFatal = 1;
460			simError();
461			}
462
463			euler[0] = ats->getEulerPhi();
464			euler[1] = ats->getEulerTheta();
465			euler[2] = ats->getEulerPsi();
466
467			doEulerToRotMat(euler, Atmp);
468			refOrients.push_back(Atmp);
469
470			}
471			*/
472
473	gezelter	1490	}
474
475	tim	1805	}
476