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

    }
  
}


void  RigidBody::updateAtoms(int frame) {
    unsigned int i;
    Vector3d ref;
    Vector3d apos;
    DirectionalAtom* dAtom;
    Vector3d pos = getPos(frame);
    RotMat3x3d a = getA(frame);
    
    for (i = 0; i < atoms_.size(); i++) {
     
        ref = body2Lab(refCoords_[i], frame);

        apos = pos + ref;

        atoms_[i]->setPos(apos, frame);

        if (atoms_[i]->isDirectional()) {
          
          dAtom = (DirectionalAtom *) atoms_[i];
          dAtom->setA(a * refOrients_[i], frame);
        }

    }
  
}

void RigidBody::updateAtomVel() {
    Mat3x3d skewMat;;

    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;

    Mat3x3d mat = (getA() * skewMat).transpose();
    Vector3d rbVel = getVel();


    Vector3d velRot;        
    for (int i =0 ; i < refCoords_.size(); ++i) {
        atoms_[i]->setVel(rbVel + mat * refCoords_[i]);
    }

}

void RigidBody::updateAtomVel(int frame) {
    Mat3x3d skewMat;;

    Vector3d ji = getJ(frame);
    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;

    Mat3x3d mat = (getA(frame) * skewMat).transpose();
    Vector3d rbVel = getVel(frame);


    Vector3d velRot;        
    for (int i =0 ; i < refCoords_.size(); ++i) {
        atoms_[i]->setVel(rbVel + mat * refCoords_[i], frame);
    }

}

        
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:	2018
Committed:	Mon Feb 14 17:57:01 2005 UTC (19 years, 4 months ago) by tim
File size:	14702 byte(s)
Log Message:	begin bug fix
#	User	Rev	Content
1	gezelter	1930	/*
2			* Copyright (c) 2005 The University of Notre Dame. All Rights Reserved.
3			*
4			* The University of Notre Dame grants you ("Licensee") a
5			* non-exclusive, royalty free, license to use, modify and
6			* redistribute this software in source and binary code form, provided
7			* that the following conditions are met:
8			*
9			* 1. Acknowledgement of the program authors must be made in any
10			* publication of scientific results based in part on use of the
11			* program. An acceptable form of acknowledgement is citation of
12			* the article in which the program was described (Matthew
13			* A. Meineke, Charles F. Vardeman II, Teng Lin, Christopher
14			* J. Fennell and J. Daniel Gezelter, "OOPSE: An Object-Oriented
15			* Parallel Simulation Engine for Molecular Dynamics,"
16			* J. Comput. Chem. 26, pp. 252-271 (2005))
17			*
18			* 2. Redistributions of source code must retain the above copyright
19			* notice, this list of conditions and the following disclaimer.
20			*
21			* 3. Redistributions in binary form must reproduce the above copyright
22			* notice, this list of conditions and the following disclaimer in the
23			* documentation and/or other materials provided with the
24			* distribution.
25			*
26			* This software is provided "AS IS," without a warranty of any
27			* kind. All express or implied conditions, representations and
28			* warranties, including any implied warranty of merchantability,
29			* fitness for a particular purpose or non-infringement, are hereby
30			* excluded. The University of Notre Dame and its licensors shall not
31			* be liable for any damages suffered by licensee as a result of
32			* using, modifying or distributing the software or its
33			* derivatives. In no event will the University of Notre Dame or its
34			* licensors be liable for any lost revenue, profit or data, or for
35			* direct, indirect, special, consequential, incidental or punitive
36			* damages, however caused and regardless of the theory of liability,
37			* arising out of the use of or inability to use software, even if the
38			* University of Notre Dame has been advised of the possibility of
39			* such damages.
40			*/
41			#include <algorithm>
42	tim	1937	#include <math.h>
43	tim	1492	#include "primitives/RigidBody.hpp"
44			#include "utils/simError.h"
45	gezelter	1930	namespace oopse {
46	gezelter	1490
47	gezelter	1930	RigidBody::RigidBody() : StuntDouble(otRigidBody, &Snapshot::rigidbodyData), inertiaTensor_(0.0){
48	gezelter	1490
49			}
50
51	gezelter	1930	void RigidBody::setPrevA(const RotMat3x3d& a) {
52			((snapshotMan_->getPrevSnapshot())->*storage_).aMat[localIndex_] = a;
53			//((snapshotMan_->getPrevSnapshot())->storage_).electroFrame[localIndex_] = a.transpose() sU_;
54	gezelter	1490
55	gezelter	1930	for (int i =0 ; i < atoms_.size(); ++i){
56			if (atoms_[i]->isDirectional()) {
57			atoms_[i]->setPrevA(a * refOrients_[i]);
58			}
59			}
60	gezelter	1490
61			}
62
63	gezelter	1930
64			void RigidBody::setA(const RotMat3x3d& a) {
65			((snapshotMan_->getCurrentSnapshot())->*storage_).aMat[localIndex_] = a;
66			//((snapshotMan_->getCurrentSnapshot())->storage_).electroFrame[localIndex_] = a.transpose() sU_;
67	gezelter	1490
68	gezelter	1930	for (int i =0 ; i < atoms_.size(); ++i){
69			if (atoms_[i]->isDirectional()) {
70			atoms_[i]->setA(a * refOrients_[i]);
71			}
72			}
73	gezelter	1490	}
74
75	gezelter	1930	void RigidBody::setA(const RotMat3x3d& a, int snapshotNo) {
76			((snapshotMan_->getSnapshot(snapshotNo))->*storage_).aMat[localIndex_] = a;
77			//((snapshotMan_->getSnapshot(snapshotNo))->storage_).electroFrame[localIndex_] = a.transpose() sU_;
78	gezelter	1490
79	gezelter	1930	for (int i =0 ; i < atoms_.size(); ++i){
80			if (atoms_[i]->isDirectional()) {
81			atoms_[i]->setA(a * refOrients_[i], snapshotNo);
82			}
83	gezelter	1490	}
84
85	gezelter	1930	}
86	gezelter	1490
87	gezelter	1930	Mat3x3d RigidBody::getI() {
88			return inertiaTensor_;
89			}
90	gezelter	1490
91	gezelter	1930	std::vector<double> RigidBody::getGrad() {
92			std::vector<double> grad(6, 0.0);
93			Vector3d force;
94			Vector3d torque;
95			Vector3d myEuler;
96			double phi, theta, psi;
97			double cphi, sphi, ctheta, stheta;
98			Vector3d ephi;
99			Vector3d etheta;
100			Vector3d epsi;
101	gezelter	1490
102	gezelter	1930	force = getFrc();
103			torque =getTrq();
104			myEuler = getA().toEulerAngles();
105	gezelter	1490
106	gezelter	1930	phi = myEuler[0];
107			theta = myEuler[1];
108			psi = myEuler[2];
109	gezelter	1490
110	gezelter	1930	cphi = cos(phi);
111			sphi = sin(phi);
112			ctheta = cos(theta);
113			stheta = sin(theta);
114	gezelter	1490
115	gezelter	1930	// get unit vectors along the phi, theta and psi rotation axes
116	gezelter	1490
117	gezelter	1930	ephi[0] = 0.0;
118			ephi[1] = 0.0;
119			ephi[2] = 1.0;
120	gezelter	1490
121	gezelter	1930	etheta[0] = cphi;
122			etheta[1] = sphi;
123			etheta[2] = 0.0;
124	gezelter	1490
125	gezelter	1930	epsi[0] = stheta * cphi;
126			epsi[1] = stheta * sphi;
127			epsi[2] = ctheta;
128	gezelter	1490
129	gezelter	1930	//gradient is equal to -force
130			for (int j = 0 ; j<3; j++)
131			grad[j] = -force[j];
132	gezelter	1490
133	gezelter	1930	for (int j = 0; j < 3; j++ ) {
134	gezelter	1490
135	gezelter	1930	grad[3] += torque[j]*ephi[j];
136			grad[4] += torque[j]*etheta[j];
137			grad[5] += torque[j]*epsi[j];
138	gezelter	1490
139	gezelter	1930	}
140
141			return grad;
142			}
143	gezelter	1490
144	gezelter	1930	void RigidBody::accept(BaseVisitor* v) {
145			v->visit(this);
146			}
147	gezelter	1490
148	gezelter	1930	/*@todo need modification /
149			void RigidBody::calcRefCoords() {
150			double mtmp;
151			Vector3d refCOM(0.0);
152			mass_ = 0.0;
153			for (std::size_t i = 0; i < atoms_.size(); ++i) {
154			mtmp = atoms_[i]->getMass();
155			mass_ += mtmp;
156			refCOM += refCoords_[i]*mtmp;
157			}
158			refCOM /= mass_;
159	gezelter	1490
160	gezelter	1930	// Next, move the origin of the reference coordinate system to the COM:
161			for (std::size_t i = 0; i < atoms_.size(); ++i) {
162			refCoords_[i] -= refCOM;
163			}
164	gezelter	1490
165	gezelter	1930	// Moment of Inertia calculation
166			Mat3x3d Itmp(0.0);
167	gezelter	1490
168	gezelter	1930	for (std::size_t i = 0; i < atoms_.size(); i++) {
169			mtmp = atoms_[i]->getMass();
170			Itmp -= outProduct(refCoords_[i], refCoords_[i]) * mtmp;
171			double r2 = refCoords_[i].lengthSquare();
172			Itmp(0, 0) += mtmp * r2;
173			Itmp(1, 1) += mtmp * r2;
174			Itmp(2, 2) += mtmp * r2;
175			}
176	gezelter	1490
177	tim	1957	//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	gezelter	1930	//diagonalize
188			Vector3d evals;
189			Mat3x3d::diagonalize(Itmp, evals, sU_);
190	gezelter	1490
191	gezelter	1930	// 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	gezelter	1490
205	gezelter	1930	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	gezelter	1490
219			}
220
221	gezelter	1930	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	gezelter	1490
231	gezelter	1930	afrc = atoms_[i]->getFrc();
232			apos = atoms_[i]->getPos();
233			rpos = apos - pos;
234
235			frc += afrc;
236	gezelter	1490
237	gezelter	1930	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	gezelter	1490
241	gezelter	1930	// If the atom has a torque associated with it, then we also need to
242			// migrate the torques onto the center of mass:
243	gezelter	1490
244	gezelter	1930	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	gezelter	1490
256	gezelter	1930	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	gezelter	1490
264	gezelter	1930	for (i = 0; i < atoms_.size(); i++) {
265
266			ref = body2Lab(refCoords_[i]);
267	gezelter	1490
268	gezelter	1930	apos = pos + ref;
269	gezelter	1490
270	gezelter	1930	atoms_[i]->setPos(apos);
271	gezelter	1490
272	gezelter	1930	if (atoms_[i]->isDirectional()) {
273
274			dAtom = (DirectionalAtom *) atoms_[i];
275			dAtom->setA(a * refOrients_[i]);
276			//dAtom->rotateBy( A );
277			}
278	gezelter	1490
279			}
280
281	gezelter	1930	}
282	gezelter	1490
283
284	tim	2002	void RigidBody::updateAtoms(int frame) {
285			unsigned int i;
286			Vector3d ref;
287			Vector3d apos;
288			DirectionalAtom* dAtom;
289			Vector3d pos = getPos(frame);
290			RotMat3x3d a = getA(frame);
291
292			for (i = 0; i < atoms_.size(); i++) {
293
294	tim	2018	ref = body2Lab(refCoords_[i], frame);
295	tim	2002
296			apos = pos + ref;
297
298			atoms_[i]->setPos(apos, frame);
299
300			if (atoms_[i]->isDirectional()) {
301
302			dAtom = (DirectionalAtom *) atoms_[i];
303			dAtom->setA(a * refOrients_[i], frame);
304			}
305
306			}
307
308			}
309
310			void RigidBody::updateAtomVel() {
311			Mat3x3d skewMat;;
312
313			Vector3d ji = getJ();
314			Mat3x3d I = getI();
315
316			skewMat(0, 0) =0;
317			skewMat(0, 1) = ji[2] /I(2, 2);
318			skewMat(0, 2) = -ji[1] /I(1, 1);
319
320			skewMat(1, 0) = -ji[2] /I(2, 2);
321			skewMat(1, 1) = 0;
322			skewMat(1, 2) = ji[0]/I(0, 0);
323
324			skewMat(2, 0) =ji[1] /I(1, 1);
325			skewMat(2, 1) = -ji[0]/I(0, 0);
326			skewMat(2, 2) = 0;
327
328			Mat3x3d mat = (getA() * skewMat).transpose();
329			Vector3d rbVel = getVel();
330
331
332			Vector3d velRot;
333			for (int i =0 ; i < refCoords_.size(); ++i) {
334			atoms_[i]->setVel(rbVel + mat * refCoords_[i]);
335			}
336
337			}
338
339			void RigidBody::updateAtomVel(int frame) {
340			Mat3x3d skewMat;;
341
342			Vector3d ji = getJ(frame);
343			Mat3x3d I = getI();
344
345			skewMat(0, 0) =0;
346			skewMat(0, 1) = ji[2] /I(2, 2);
347			skewMat(0, 2) = -ji[1] /I(1, 1);
348
349			skewMat(1, 0) = -ji[2] /I(2, 2);
350			skewMat(1, 1) = 0;
351			skewMat(1, 2) = ji[0]/I(0, 0);
352
353			skewMat(2, 0) =ji[1] /I(1, 1);
354			skewMat(2, 1) = -ji[0]/I(0, 0);
355			skewMat(2, 2) = 0;
356
357			Mat3x3d mat = (getA(frame) * skewMat).transpose();
358			Vector3d rbVel = getVel(frame);
359
360
361			Vector3d velRot;
362			for (int i =0 ; i < refCoords_.size(); ++i) {
363			atoms_[i]->setVel(rbVel + mat * refCoords_[i], frame);
364			}
365
366			}
367
368
369
370	gezelter	1930	bool RigidBody::getAtomPos(Vector3d& pos, unsigned int index) {
371			if (index < atoms_.size()) {
372	gezelter	1490
373	gezelter	1930	Vector3d ref = body2Lab(refCoords_[index]);
374			pos = getPos() + ref;
375			return true;
376			} else {
377			std::cerr << index << " is an invalid index, current rigid body contains "
378			<< atoms_.size() << "atoms" << std::endl;
379			return false;
380			}
381			}
382	gezelter	1490
383	gezelter	1930	bool RigidBody::getAtomPos(Vector3d& pos, Atom* atom) {
384			std::vector<Atom*>::iterator i;
385			i = std::find(atoms_.begin(), atoms_.end(), atom);
386			if (i != atoms_.end()) {
387			//RigidBody class makes sure refCoords_ and atoms_ match each other
388			Vector3d ref = body2Lab(refCoords_[i - atoms_.begin()]);
389			pos = getPos() + ref;
390			return true;
391			} else {
392			std::cerr << "Atom " << atom->getGlobalIndex()
393			<<" does not belong to Rigid body "<< getGlobalIndex() << std::endl;
394			return false;
395	gezelter	1490	}
396			}
397	gezelter	1930	bool RigidBody::getAtomVel(Vector3d& vel, unsigned int index) {
398	gezelter	1490
399	gezelter	1930	//velRot = $(A\cdot skew(I^{-1}j))^{T}refCoor$
400	gezelter	1490
401	gezelter	1930	if (index < atoms_.size()) {
402	gezelter	1490
403	gezelter	1930	Vector3d velRot;
404			Mat3x3d skewMat;;
405			Vector3d ref = refCoords_[index];
406			Vector3d ji = getJ();
407			Mat3x3d I = getI();
408	gezelter	1490
409	gezelter	1930	skewMat(0, 0) =0;
410			skewMat(0, 1) = ji[2] /I(2, 2);
411			skewMat(0, 2) = -ji[1] /I(1, 1);
412	gezelter	1490
413	gezelter	1930	skewMat(1, 0) = -ji[2] /I(2, 2);
414			skewMat(1, 1) = 0;
415			skewMat(1, 2) = ji[0]/I(0, 0);
416	gezelter	1490
417	gezelter	1930	skewMat(2, 0) =ji[1] /I(1, 1);
418			skewMat(2, 1) = -ji[0]/I(0, 0);
419			skewMat(2, 2) = 0;
420	gezelter	1490
421	gezelter	1930	velRot = (getA() * skewMat).transpose() * ref;
422	gezelter	1490
423	gezelter	1930	vel =getVel() + velRot;
424			return true;
425
426			} else {
427			std::cerr << index << " is an invalid index, current rigid body contains "
428			<< atoms_.size() << "atoms" << std::endl;
429			return false;
430	gezelter	1490	}
431	gezelter	1930	}
432	gezelter	1490
433	gezelter	1930	bool RigidBody::getAtomVel(Vector3d& vel, Atom* atom) {
434	gezelter	1490
435	gezelter	1930	std::vector<Atom*>::iterator i;
436			i = std::find(atoms_.begin(), atoms_.end(), atom);
437			if (i != atoms_.end()) {
438			return getAtomVel(vel, i - atoms_.begin());
439			} else {
440			std::cerr << "Atom " << atom->getGlobalIndex()
441			<<" does not belong to Rigid body "<< getGlobalIndex() << std::endl;
442			return false;
443			}
444			}
445	gezelter	1490
446	gezelter	1930	bool RigidBody::getAtomRefCoor(Vector3d& coor, unsigned int index) {
447			if (index < atoms_.size()) {
448
449			coor = refCoords_[index];
450			return true;
451			} else {
452			std::cerr << index << " is an invalid index, current rigid body contains "
453			<< atoms_.size() << "atoms" << std::endl;
454			return false;
455	gezelter	1490	}
456
457			}
458
459	gezelter	1930	bool RigidBody::getAtomRefCoor(Vector3d& coor, Atom* atom) {
460			std::vector<Atom*>::iterator i;
461			i = std::find(atoms_.begin(), atoms_.end(), atom);
462			if (i != atoms_.end()) {
463			//RigidBody class makes sure refCoords_ and atoms_ match each other
464			coor = refCoords_[i - atoms_.begin()];
465			return true;
466			} else {
467			std::cerr << "Atom " << atom->getGlobalIndex()
468			<<" does not belong to Rigid body "<< getGlobalIndex() << std::endl;
469			return false;
470			}
471	gezelter	1490
472			}
473
474
475	gezelter	1930	void RigidBody::addAtom(Atom* at, AtomStamp* ats) {
476	gezelter	1490
477	gezelter	1930	Vector3d coords;
478			Vector3d euler;
479	gezelter	1490
480
481	gezelter	1930	atoms_.push_back(at);
482
483			if( !ats->havePosition() ){
484			sprintf( painCave.errMsg,
485			"RigidBody error.\n"
486			"\tAtom %s does not have a position specified.\n"
487			"\tThis means RigidBody cannot set up reference coordinates.\n",
488			ats->getType() );
489			painCave.isFatal = 1;
490			simError();
491	gezelter	1490	}
492
493	gezelter	1930	coords[0] = ats->getPosX();
494			coords[1] = ats->getPosY();
495			coords[2] = ats->getPosZ();
496	gezelter	1490
497	gezelter	1930	refCoords_.push_back(coords);
498	gezelter	1490
499	gezelter	1930	RotMat3x3d identMat = RotMat3x3d::identity();
500	gezelter	1490
501	gezelter	1930	if (at->isDirectional()) {
502	gezelter	1490
503	gezelter	1930	if( !ats->haveOrientation() ){
504			sprintf( painCave.errMsg,
505			"RigidBody error.\n"
506			"\tAtom %s does not have an orientation specified.\n"
507			"\tThis means RigidBody cannot set up reference orientations.\n",
508			ats->getType() );
509			painCave.isFatal = 1;
510			simError();
511			}
512
513			euler[0] = ats->getEulerPhi();
514			euler[1] = ats->getEulerTheta();
515			euler[2] = ats->getEulerPsi();
516	gezelter	1490
517	gezelter	1930	RotMat3x3d Atmp(euler);
518			refOrients_.push_back(Atmp);
519	gezelter	1490
520	gezelter	1930	}else {
521			refOrients_.push_back(identMat);
522	gezelter	1490	}
523
524
525			}
526
527			}
528