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++) {
      Mat3x3d IAtom(0.0);  
      mtmp = atoms_[i]->getMass();
      IAtom -= outProduct(refCoords_[i], refCoords_[i]) * mtmp;
      double r2 = refCoords_[i].lengthSquare();
      IAtom(0, 0) += mtmp * r2;
      IAtom(1, 1) += mtmp * r2;
      IAtom(2, 2) += mtmp * r2;
      Itmp += IAtom;

      //project the inertial moment of directional atoms into this rigid body
      if (atoms_[i]->isDirectional()) {
        Itmp += refOrients_[i].transpose() * atoms_[i]->getI() * refOrients_[i];
      } 
    }

    //    std::cout << Itmp << std::endl;

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