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"
#include "utils/NumericConstant.hpp"
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() * NumericConstant::PI /180.0;
    euler[1] = ats->getEulerTheta() * NumericConstant::PI /180.0;
    euler[2] = ats->getEulerPsi() * NumericConstant::PI /180.0;

    RotMat3x3d Atmp(euler);
    refOrients_.push_back(Atmp);
    
  }else {
    refOrients_.push_back(identMat);
  }
  
  
}

}

Revision:	2058
Committed:	Tue Feb 22 18:56:25 2005 UTC (19 years, 4 months ago) by tim
File size:	14826 byte(s)
Log Message:	reactionfield get fixed
#	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	tim	2058	#include "utils/NumericConstant.hpp"
46	gezelter	1930	namespace oopse {
47	gezelter	1490
48	gezelter	1930	RigidBody::RigidBody() : StuntDouble(otRigidBody, &Snapshot::rigidbodyData), inertiaTensor_(0.0){
49	gezelter	1490
50			}
51
52	gezelter	1930	void RigidBody::setPrevA(const RotMat3x3d& a) {
53			((snapshotMan_->getPrevSnapshot())->*storage_).aMat[localIndex_] = a;
54			//((snapshotMan_->getPrevSnapshot())->storage_).electroFrame[localIndex_] = a.transpose() sU_;
55	gezelter	1490
56	gezelter	1930	for (int i =0 ; i < atoms_.size(); ++i){
57			if (atoms_[i]->isDirectional()) {
58			atoms_[i]->setPrevA(a * refOrients_[i]);
59			}
60			}
61	gezelter	1490
62			}
63
64	gezelter	1930
65			void RigidBody::setA(const RotMat3x3d& a) {
66			((snapshotMan_->getCurrentSnapshot())->*storage_).aMat[localIndex_] = a;
67			//((snapshotMan_->getCurrentSnapshot())->storage_).electroFrame[localIndex_] = a.transpose() sU_;
68	gezelter	1490
69	gezelter	1930	for (int i =0 ; i < atoms_.size(); ++i){
70			if (atoms_[i]->isDirectional()) {
71			atoms_[i]->setA(a * refOrients_[i]);
72			}
73			}
74	gezelter	1490	}
75
76	gezelter	1930	void RigidBody::setA(const RotMat3x3d& a, int snapshotNo) {
77			((snapshotMan_->getSnapshot(snapshotNo))->*storage_).aMat[localIndex_] = a;
78			//((snapshotMan_->getSnapshot(snapshotNo))->storage_).electroFrame[localIndex_] = a.transpose() sU_;
79	gezelter	1490
80	gezelter	1930	for (int i =0 ; i < atoms_.size(); ++i){
81			if (atoms_[i]->isDirectional()) {
82			atoms_[i]->setA(a * refOrients_[i], snapshotNo);
83			}
84	gezelter	1490	}
85
86	gezelter	1930	}
87	gezelter	1490
88	gezelter	1930	Mat3x3d RigidBody::getI() {
89			return inertiaTensor_;
90			}
91	gezelter	1490
92	gezelter	1930	std::vector<double> RigidBody::getGrad() {
93			std::vector<double> grad(6, 0.0);
94			Vector3d force;
95			Vector3d torque;
96			Vector3d myEuler;
97			double phi, theta, psi;
98			double cphi, sphi, ctheta, stheta;
99			Vector3d ephi;
100			Vector3d etheta;
101			Vector3d epsi;
102	gezelter	1490
103	gezelter	1930	force = getFrc();
104			torque =getTrq();
105			myEuler = getA().toEulerAngles();
106	gezelter	1490
107	gezelter	1930	phi = myEuler[0];
108			theta = myEuler[1];
109			psi = myEuler[2];
110	gezelter	1490
111	gezelter	1930	cphi = cos(phi);
112			sphi = sin(phi);
113			ctheta = cos(theta);
114			stheta = sin(theta);
115	gezelter	1490
116	gezelter	1930	// get unit vectors along the phi, theta and psi rotation axes
117	gezelter	1490
118	gezelter	1930	ephi[0] = 0.0;
119			ephi[1] = 0.0;
120			ephi[2] = 1.0;
121	gezelter	1490
122	gezelter	1930	etheta[0] = cphi;
123			etheta[1] = sphi;
124			etheta[2] = 0.0;
125	gezelter	1490
126	gezelter	1930	epsi[0] = stheta * cphi;
127			epsi[1] = stheta * sphi;
128			epsi[2] = ctheta;
129	gezelter	1490
130	gezelter	1930	//gradient is equal to -force
131			for (int j = 0 ; j<3; j++)
132			grad[j] = -force[j];
133	gezelter	1490
134	gezelter	1930	for (int j = 0; j < 3; j++ ) {
135	gezelter	1490
136	gezelter	1930	grad[3] += torque[j]*ephi[j];
137			grad[4] += torque[j]*etheta[j];
138			grad[5] += torque[j]*epsi[j];
139	gezelter	1490
140	gezelter	1930	}
141
142			return grad;
143			}
144	gezelter	1490
145	gezelter	1930	void RigidBody::accept(BaseVisitor* v) {
146			v->visit(this);
147			}
148	gezelter	1490
149	gezelter	1930	/*@todo need modification /
150			void RigidBody::calcRefCoords() {
151			double mtmp;
152			Vector3d refCOM(0.0);
153			mass_ = 0.0;
154			for (std::size_t i = 0; i < atoms_.size(); ++i) {
155			mtmp = atoms_[i]->getMass();
156			mass_ += mtmp;
157			refCOM += refCoords_[i]*mtmp;
158			}
159			refCOM /= mass_;
160	gezelter	1490
161	gezelter	1930	// Next, move the origin of the reference coordinate system to the COM:
162			for (std::size_t i = 0; i < atoms_.size(); ++i) {
163			refCoords_[i] -= refCOM;
164			}
165	gezelter	1490
166	gezelter	1930	// Moment of Inertia calculation
167			Mat3x3d Itmp(0.0);
168	gezelter	1490
169	gezelter	1930	for (std::size_t i = 0; i < atoms_.size(); i++) {
170			mtmp = atoms_[i]->getMass();
171			Itmp -= outProduct(refCoords_[i], refCoords_[i]) * mtmp;
172			double r2 = refCoords_[i].lengthSquare();
173			Itmp(0, 0) += mtmp * r2;
174			Itmp(1, 1) += mtmp * r2;
175			Itmp(2, 2) += mtmp * r2;
176			}
177	gezelter	1490
178	tim	1957	//project the inertial moment of directional atoms into this rigid body
179			for (std::size_t i = 0; i < atoms_.size(); i++) {
180			if (atoms_[i]->isDirectional()) {
181			RectMatrix<double, 3, 3> Iproject = refOrients_[i].transpose() * atoms_[i]->getI();
182			Itmp(0, 0) += Iproject(0, 0);
183			Itmp(1, 1) += Iproject(1, 1);
184			Itmp(2, 2) += Iproject(2, 2);
185			}
186			}
187
188	gezelter	1930	//diagonalize
189			Vector3d evals;
190			Mat3x3d::diagonalize(Itmp, evals, sU_);
191	gezelter	1490
192	gezelter	1930	// zero out I and then fill the diagonals with the moments of inertia:
193			inertiaTensor_(0, 0) = evals[0];
194			inertiaTensor_(1, 1) = evals[1];
195			inertiaTensor_(2, 2) = evals[2];
196
197			int nLinearAxis = 0;
198			for (int i = 0; i < 3; i++) {
199			if (fabs(evals[i]) < oopse::epsilon) {
200			linear_ = true;
201			linearAxis_ = i;
202			++ nLinearAxis;
203			}
204			}
205	gezelter	1490
206	gezelter	1930	if (nLinearAxis > 1) {
207			sprintf( painCave.errMsg,
208			"RigidBody error.\n"
209			"\tOOPSE found more than one axis in this rigid body with a vanishing \n"
210			"\tmoment of inertia. This can happen in one of three ways:\n"
211			"\t 1) Only one atom was specified, or \n"
212			"\t 2) All atoms were specified at the same location, or\n"
213			"\t 3) The programmers did something stupid.\n"
214			"\tIt is silly to use a rigid body to describe this situation. Be smarter.\n"
215			);
216			painCave.isFatal = 1;
217			simError();
218			}
219	gezelter	1490
220			}
221
222	gezelter	1930	void RigidBody::calcForcesAndTorques() {
223			Vector3d afrc;
224			Vector3d atrq;
225			Vector3d apos;
226			Vector3d rpos;
227			Vector3d frc(0.0);
228			Vector3d trq(0.0);
229			Vector3d pos = this->getPos();
230			for (int i = 0; i < atoms_.size(); i++) {
231	gezelter	1490
232	gezelter	1930	afrc = atoms_[i]->getFrc();
233			apos = atoms_[i]->getPos();
234			rpos = apos - pos;
235
236			frc += afrc;
237	gezelter	1490
238	gezelter	1930	trq[0] += rpos[1]afrc[2] - rpos[2]afrc[1];
239			trq[1] += rpos[2]afrc[0] - rpos[0]afrc[2];
240			trq[2] += rpos[0]afrc[1] - rpos[1]afrc[0];
241	gezelter	1490
242	gezelter	1930	// If the atom has a torque associated with it, then we also need to
243			// migrate the torques onto the center of mass:
244	gezelter	1490
245	gezelter	1930	if (atoms_[i]->isDirectional()) {
246			atrq = atoms_[i]->getTrq();
247			trq += atrq;
248			}
249
250			}
251
252			setFrc(frc);
253			setTrq(trq);
254
255			}
256	gezelter	1490
257	gezelter	1930	void RigidBody::updateAtoms() {
258			unsigned int i;
259			Vector3d ref;
260			Vector3d apos;
261			DirectionalAtom* dAtom;
262			Vector3d pos = getPos();
263			RotMat3x3d a = getA();
264	gezelter	1490
265	gezelter	1930	for (i = 0; i < atoms_.size(); i++) {
266
267			ref = body2Lab(refCoords_[i]);
268	gezelter	1490
269	gezelter	1930	apos = pos + ref;
270	gezelter	1490
271	gezelter	1930	atoms_[i]->setPos(apos);
272	gezelter	1490
273	gezelter	1930	if (atoms_[i]->isDirectional()) {
274
275			dAtom = (DirectionalAtom *) atoms_[i];
276			dAtom->setA(a * refOrients_[i]);
277			//dAtom->rotateBy( A );
278			}
279	gezelter	1490
280			}
281
282	gezelter	1930	}
283	gezelter	1490
284
285	tim	2002	void RigidBody::updateAtoms(int frame) {
286			unsigned int i;
287			Vector3d ref;
288			Vector3d apos;
289			DirectionalAtom* dAtom;
290			Vector3d pos = getPos(frame);
291			RotMat3x3d a = getA(frame);
292
293			for (i = 0; i < atoms_.size(); i++) {
294
295	tim	2018	ref = body2Lab(refCoords_[i], frame);
296	tim	2002
297			apos = pos + ref;
298
299			atoms_[i]->setPos(apos, frame);
300
301			if (atoms_[i]->isDirectional()) {
302
303			dAtom = (DirectionalAtom *) atoms_[i];
304			dAtom->setA(a * refOrients_[i], frame);
305			}
306
307			}
308
309			}
310
311			void RigidBody::updateAtomVel() {
312			Mat3x3d skewMat;;
313
314			Vector3d ji = getJ();
315			Mat3x3d I = getI();
316
317			skewMat(0, 0) =0;
318			skewMat(0, 1) = ji[2] /I(2, 2);
319			skewMat(0, 2) = -ji[1] /I(1, 1);
320
321			skewMat(1, 0) = -ji[2] /I(2, 2);
322			skewMat(1, 1) = 0;
323			skewMat(1, 2) = ji[0]/I(0, 0);
324
325			skewMat(2, 0) =ji[1] /I(1, 1);
326			skewMat(2, 1) = -ji[0]/I(0, 0);
327			skewMat(2, 2) = 0;
328
329			Mat3x3d mat = (getA() * skewMat).transpose();
330			Vector3d rbVel = getVel();
331
332
333			Vector3d velRot;
334			for (int i =0 ; i < refCoords_.size(); ++i) {
335			atoms_[i]->setVel(rbVel + mat * refCoords_[i]);
336			}
337
338			}
339
340			void RigidBody::updateAtomVel(int frame) {
341			Mat3x3d skewMat;;
342
343			Vector3d ji = getJ(frame);
344			Mat3x3d I = getI();
345
346			skewMat(0, 0) =0;
347			skewMat(0, 1) = ji[2] /I(2, 2);
348			skewMat(0, 2) = -ji[1] /I(1, 1);
349
350			skewMat(1, 0) = -ji[2] /I(2, 2);
351			skewMat(1, 1) = 0;
352			skewMat(1, 2) = ji[0]/I(0, 0);
353
354			skewMat(2, 0) =ji[1] /I(1, 1);
355			skewMat(2, 1) = -ji[0]/I(0, 0);
356			skewMat(2, 2) = 0;
357
358			Mat3x3d mat = (getA(frame) * skewMat).transpose();
359			Vector3d rbVel = getVel(frame);
360
361
362			Vector3d velRot;
363			for (int i =0 ; i < refCoords_.size(); ++i) {
364			atoms_[i]->setVel(rbVel + mat * refCoords_[i], frame);
365			}
366
367			}
368
369
370
371	gezelter	1930	bool RigidBody::getAtomPos(Vector3d& pos, unsigned int index) {
372			if (index < atoms_.size()) {
373	gezelter	1490
374	gezelter	1930	Vector3d ref = body2Lab(refCoords_[index]);
375			pos = getPos() + ref;
376			return true;
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	gezelter	1930	bool RigidBody::getAtomPos(Vector3d& pos, Atom* atom) {
385			std::vector<Atom*>::iterator i;
386			i = std::find(atoms_.begin(), atoms_.end(), atom);
387			if (i != atoms_.end()) {
388			//RigidBody class makes sure refCoords_ and atoms_ match each other
389			Vector3d ref = body2Lab(refCoords_[i - atoms_.begin()]);
390			pos = getPos() + ref;
391			return true;
392			} else {
393			std::cerr << "Atom " << atom->getGlobalIndex()
394			<<" does not belong to Rigid body "<< getGlobalIndex() << std::endl;
395			return false;
396	gezelter	1490	}
397			}
398	gezelter	1930	bool RigidBody::getAtomVel(Vector3d& vel, unsigned int index) {
399	gezelter	1490
400	gezelter	1930	//velRot = $(A\cdot skew(I^{-1}j))^{T}refCoor$
401	gezelter	1490
402	gezelter	1930	if (index < atoms_.size()) {
403	gezelter	1490
404	gezelter	1930	Vector3d velRot;
405			Mat3x3d skewMat;;
406			Vector3d ref = refCoords_[index];
407			Vector3d ji = getJ();
408			Mat3x3d I = getI();
409	gezelter	1490
410	gezelter	1930	skewMat(0, 0) =0;
411			skewMat(0, 1) = ji[2] /I(2, 2);
412			skewMat(0, 2) = -ji[1] /I(1, 1);
413	gezelter	1490
414	gezelter	1930	skewMat(1, 0) = -ji[2] /I(2, 2);
415			skewMat(1, 1) = 0;
416			skewMat(1, 2) = ji[0]/I(0, 0);
417	gezelter	1490
418	gezelter	1930	skewMat(2, 0) =ji[1] /I(1, 1);
419			skewMat(2, 1) = -ji[0]/I(0, 0);
420			skewMat(2, 2) = 0;
421	gezelter	1490
422	gezelter	1930	velRot = (getA() * skewMat).transpose() * ref;
423	gezelter	1490
424	gezelter	1930	vel =getVel() + velRot;
425			return true;
426
427			} else {
428			std::cerr << index << " is an invalid index, current rigid body contains "
429			<< atoms_.size() << "atoms" << std::endl;
430			return false;
431	gezelter	1490	}
432	gezelter	1930	}
433	gezelter	1490
434	gezelter	1930	bool RigidBody::getAtomVel(Vector3d& vel, Atom* atom) {
435	gezelter	1490
436	gezelter	1930	std::vector<Atom*>::iterator i;
437			i = std::find(atoms_.begin(), atoms_.end(), atom);
438			if (i != atoms_.end()) {
439			return getAtomVel(vel, i - atoms_.begin());
440			} else {
441			std::cerr << "Atom " << atom->getGlobalIndex()
442			<<" does not belong to Rigid body "<< getGlobalIndex() << std::endl;
443			return false;
444			}
445			}
446	gezelter	1490
447	gezelter	1930	bool RigidBody::getAtomRefCoor(Vector3d& coor, unsigned int index) {
448			if (index < atoms_.size()) {
449
450			coor = refCoords_[index];
451			return true;
452			} else {
453			std::cerr << index << " is an invalid index, current rigid body contains "
454			<< atoms_.size() << "atoms" << std::endl;
455			return false;
456	gezelter	1490	}
457
458			}
459
460	gezelter	1930	bool RigidBody::getAtomRefCoor(Vector3d& coor, Atom* atom) {
461			std::vector<Atom*>::iterator i;
462			i = std::find(atoms_.begin(), atoms_.end(), atom);
463			if (i != atoms_.end()) {
464			//RigidBody class makes sure refCoords_ and atoms_ match each other
465			coor = refCoords_[i - atoms_.begin()];
466			return true;
467			} else {
468			std::cerr << "Atom " << atom->getGlobalIndex()
469			<<" does not belong to Rigid body "<< getGlobalIndex() << std::endl;
470			return false;
471			}
472	gezelter	1490
473			}
474
475
476	gezelter	1930	void RigidBody::addAtom(Atom* at, AtomStamp* ats) {
477	gezelter	1490
478	gezelter	1930	Vector3d coords;
479			Vector3d euler;
480	gezelter	1490
481
482	gezelter	1930	atoms_.push_back(at);
483
484			if( !ats->havePosition() ){
485			sprintf( painCave.errMsg,
486			"RigidBody error.\n"
487			"\tAtom %s does not have a position specified.\n"
488			"\tThis means RigidBody cannot set up reference coordinates.\n",
489			ats->getType() );
490			painCave.isFatal = 1;
491			simError();
492	gezelter	1490	}
493
494	gezelter	1930	coords[0] = ats->getPosX();
495			coords[1] = ats->getPosY();
496			coords[2] = ats->getPosZ();
497	gezelter	1490
498	gezelter	1930	refCoords_.push_back(coords);
499	gezelter	1490
500	gezelter	1930	RotMat3x3d identMat = RotMat3x3d::identity();
501	gezelter	1490
502	gezelter	1930	if (at->isDirectional()) {
503	gezelter	1490
504	gezelter	1930	if( !ats->haveOrientation() ){
505			sprintf( painCave.errMsg,
506			"RigidBody error.\n"
507			"\tAtom %s does not have an orientation specified.\n"
508			"\tThis means RigidBody cannot set up reference orientations.\n",
509			ats->getType() );
510			painCave.isFatal = 1;
511			simError();
512			}
513
514	tim	2058	euler[0] = ats->getEulerPhi() * NumericConstant::PI /180.0;
515			euler[1] = ats->getEulerTheta() * NumericConstant::PI /180.0;
516			euler[2] = ats->getEulerPsi() * NumericConstant::PI /180.0;
517	gezelter	1490
518	gezelter	1930	RotMat3x3d Atmp(euler);
519			refOrients_.push_back(Atmp);
520	gezelter	1490
521	gezelter	1930	}else {
522			refOrients_.push_back(identMat);
523	gezelter	1490	}
524
525
526			}
527
528			}
529