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 += IAtom;
        Itmp += refOrients_[i].transpose() * atoms_[i]->getI() * refOrients_[i];
      } 
    }

    //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() );
      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:	648
Committed:	Wed Oct 5 19:35:28 2005 UTC (19 years, 6 months ago) by tim
Original Path:	*trunk/src/primitives/RigidBody.cpp*
File size:	14254 byte(s)
Log Message:	erase output of inertia tensor. There is still something wrong with current implementation.
#	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 += IAtom;
181			Itmp += refOrients_[i].transpose() * atoms_[i]->getI() * refOrients_[i];
182	tim	648	}
183	tim	273	}
184
185	gezelter	246	//diagonalize
186			Vector3d evals;
187			Mat3x3d::diagonalize(Itmp, evals, sU_);
188	gezelter	2
189	gezelter	246	// zero out I and then fill the diagonals with the moments of inertia:
190			inertiaTensor_(0, 0) = evals[0];
191			inertiaTensor_(1, 1) = evals[1];
192			inertiaTensor_(2, 2) = evals[2];
193
194			int nLinearAxis = 0;
195			for (int i = 0; i < 3; i++) {
196	gezelter	507	if (fabs(evals[i]) < oopse::epsilon) {
197			linear_ = true;
198			linearAxis_ = i;
199			++ nLinearAxis;
200			}
201	gezelter	246	}
202	gezelter	2
203	gezelter	246	if (nLinearAxis > 1) {
204	gezelter	507	sprintf( painCave.errMsg,
205			"RigidBody error.\n"
206			"\tOOPSE found more than one axis in this rigid body with a vanishing \n"
207			"\tmoment of inertia. This can happen in one of three ways:\n"
208			"\t 1) Only one atom was specified, or \n"
209			"\t 2) All atoms were specified at the same location, or\n"
210			"\t 3) The programmers did something stupid.\n"
211			"\tIt is silly to use a rigid body to describe this situation. Be smarter.\n"
212			);
213			painCave.isFatal = 1;
214			simError();
215	gezelter	246	}
216	gezelter	2
217	gezelter	507	}
218	gezelter	2
219	gezelter	507	void RigidBody::calcForcesAndTorques() {
220	gezelter	246	Vector3d afrc;
221			Vector3d atrq;
222			Vector3d apos;
223			Vector3d rpos;
224			Vector3d frc(0.0);
225			Vector3d trq(0.0);
226			Vector3d pos = this->getPos();
227			for (int i = 0; i < atoms_.size(); i++) {
228	gezelter	2
229	gezelter	507	afrc = atoms_[i]->getFrc();
230			apos = atoms_[i]->getPos();
231			rpos = apos - pos;
232	gezelter	246
233	gezelter	507	frc += afrc;
234	gezelter	2
235	gezelter	507	trq[0] += rpos[1]afrc[2] - rpos[2]afrc[1];
236			trq[1] += rpos[2]afrc[0] - rpos[0]afrc[2];
237			trq[2] += rpos[0]afrc[1] - rpos[1]afrc[0];
238	gezelter	2
239	gezelter	507	// If the atom has a torque associated with it, then we also need to
240			// migrate the torques onto the center of mass:
241	gezelter	2
242	gezelter	507	if (atoms_[i]->isDirectional()) {
243			atrq = atoms_[i]->getTrq();
244			trq += atrq;
245			}
246	gezelter	246
247			}
248
249			setFrc(frc);
250			setTrq(trq);
251
252	gezelter	507	}
253	gezelter	2
254	gezelter	507	void RigidBody::updateAtoms() {
255	gezelter	246	unsigned int i;
256			Vector3d ref;
257			Vector3d apos;
258			DirectionalAtom* dAtom;
259			Vector3d pos = getPos();
260			RotMat3x3d a = getA();
261	gezelter	2
262	gezelter	246	for (i = 0; i < atoms_.size(); i++) {
263
264	gezelter	507	ref = body2Lab(refCoords_[i]);
265	gezelter	2
266	gezelter	507	apos = pos + ref;
267	gezelter	2
268	gezelter	507	atoms_[i]->setPos(apos);
269	gezelter	2
270	gezelter	507	if (atoms_[i]->isDirectional()) {
271	gezelter	246
272	gezelter	507	dAtom = (DirectionalAtom *) atoms_[i];
273	gezelter	636	dAtom->setA(refOrients_[i] * a);
274	gezelter	507	}
275	gezelter	2
276			}
277
278	gezelter	507	}
279	gezelter	2
280
281	gezelter	507	void RigidBody::updateAtoms(int frame) {
282	tim	318	unsigned int i;
283			Vector3d ref;
284			Vector3d apos;
285			DirectionalAtom* dAtom;
286			Vector3d pos = getPos(frame);
287			RotMat3x3d a = getA(frame);
288
289			for (i = 0; i < atoms_.size(); i++) {
290
291	gezelter	507	ref = body2Lab(refCoords_[i], frame);
292	tim	318
293	gezelter	507	apos = pos + ref;
294	tim	318
295	gezelter	507	atoms_[i]->setPos(apos, frame);
296	tim	318
297	gezelter	507	if (atoms_[i]->isDirectional()) {
298	tim	318
299	gezelter	507	dAtom = (DirectionalAtom *) atoms_[i];
300	gezelter	636	dAtom->setA(refOrients_[i] * a, frame);
301	gezelter	507	}
302	tim	318
303			}
304
305	gezelter	507	}
306	tim	318
307	gezelter	507	void RigidBody::updateAtomVel() {
308	tim	318	Mat3x3d skewMat;;
309
310			Vector3d ji = getJ();
311			Mat3x3d I = getI();
312
313			skewMat(0, 0) =0;
314			skewMat(0, 1) = ji[2] /I(2, 2);
315			skewMat(0, 2) = -ji[1] /I(1, 1);
316
317			skewMat(1, 0) = -ji[2] /I(2, 2);
318			skewMat(1, 1) = 0;
319			skewMat(1, 2) = ji[0]/I(0, 0);
320
321			skewMat(2, 0) =ji[1] /I(1, 1);
322			skewMat(2, 1) = -ji[0]/I(0, 0);
323			skewMat(2, 2) = 0;
324
325			Mat3x3d mat = (getA() * skewMat).transpose();
326			Vector3d rbVel = getVel();
327
328
329			Vector3d velRot;
330			for (int i =0 ; i < refCoords_.size(); ++i) {
331	gezelter	507	atoms_[i]->setVel(rbVel + mat * refCoords_[i]);
332	tim	318	}
333
334	gezelter	507	}
335	tim	318
336	gezelter	507	void RigidBody::updateAtomVel(int frame) {
337	tim	318	Mat3x3d skewMat;;
338
339			Vector3d ji = getJ(frame);
340			Mat3x3d I = getI();
341
342			skewMat(0, 0) =0;
343			skewMat(0, 1) = ji[2] /I(2, 2);
344			skewMat(0, 2) = -ji[1] /I(1, 1);
345
346			skewMat(1, 0) = -ji[2] /I(2, 2);
347			skewMat(1, 1) = 0;
348			skewMat(1, 2) = ji[0]/I(0, 0);
349
350			skewMat(2, 0) =ji[1] /I(1, 1);
351			skewMat(2, 1) = -ji[0]/I(0, 0);
352			skewMat(2, 2) = 0;
353
354			Mat3x3d mat = (getA(frame) * skewMat).transpose();
355			Vector3d rbVel = getVel(frame);
356
357
358			Vector3d velRot;
359			for (int i =0 ; i < refCoords_.size(); ++i) {
360	gezelter	507	atoms_[i]->setVel(rbVel + mat * refCoords_[i], frame);
361	tim	318	}
362
363	gezelter	507	}
364	tim	318
365
366
367	gezelter	507	bool RigidBody::getAtomPos(Vector3d& pos, unsigned int index) {
368	gezelter	246	if (index < atoms_.size()) {
369	gezelter	2
370	gezelter	507	Vector3d ref = body2Lab(refCoords_[index]);
371			pos = getPos() + ref;
372			return true;
373	gezelter	246	} else {
374	gezelter	507	std::cerr << index << " is an invalid index, current rigid body contains "
375			<< atoms_.size() << "atoms" << std::endl;
376			return false;
377	gezelter	246	}
378	gezelter	507	}
379	gezelter	2
380	gezelter	507	bool RigidBody::getAtomPos(Vector3d& pos, Atom* atom) {
381	gezelter	246	std::vector<Atom*>::iterator i;
382			i = std::find(atoms_.begin(), atoms_.end(), atom);
383			if (i != atoms_.end()) {
384	gezelter	507	//RigidBody class makes sure refCoords_ and atoms_ match each other
385			Vector3d ref = body2Lab(refCoords_[i - atoms_.begin()]);
386			pos = getPos() + ref;
387			return true;
388	gezelter	246	} else {
389	gezelter	507	std::cerr << "Atom " << atom->getGlobalIndex()
390			<<" does not belong to Rigid body "<< getGlobalIndex() << std::endl;
391			return false;
392	gezelter	2	}
393	gezelter	507	}
394			bool RigidBody::getAtomVel(Vector3d& vel, unsigned int index) {
395	gezelter	2
396	gezelter	246	//velRot = $(A\cdot skew(I^{-1}j))^{T}refCoor$
397	gezelter	2
398	gezelter	246	if (index < atoms_.size()) {
399	gezelter	2
400	gezelter	507	Vector3d velRot;
401			Mat3x3d skewMat;;
402			Vector3d ref = refCoords_[index];
403			Vector3d ji = getJ();
404			Mat3x3d I = getI();
405	gezelter	2
406	gezelter	507	skewMat(0, 0) =0;
407			skewMat(0, 1) = ji[2] /I(2, 2);
408			skewMat(0, 2) = -ji[1] /I(1, 1);
409	gezelter	2
410	gezelter	507	skewMat(1, 0) = -ji[2] /I(2, 2);
411			skewMat(1, 1) = 0;
412			skewMat(1, 2) = ji[0]/I(0, 0);
413	gezelter	2
414	gezelter	507	skewMat(2, 0) =ji[1] /I(1, 1);
415			skewMat(2, 1) = -ji[0]/I(0, 0);
416			skewMat(2, 2) = 0;
417	gezelter	2
418	gezelter	507	velRot = (getA() * skewMat).transpose() * ref;
419	gezelter	2
420	gezelter	507	vel =getVel() + velRot;
421			return true;
422	gezelter	246
423			} else {
424	gezelter	507	std::cerr << index << " is an invalid index, current rigid body contains "
425			<< atoms_.size() << "atoms" << std::endl;
426			return false;
427	gezelter	2	}
428	gezelter	507	}
429	gezelter	2
430	gezelter	507	bool RigidBody::getAtomVel(Vector3d& vel, Atom* atom) {
431	gezelter	2
432	gezelter	246	std::vector<Atom*>::iterator i;
433			i = std::find(atoms_.begin(), atoms_.end(), atom);
434			if (i != atoms_.end()) {
435	gezelter	507	return getAtomVel(vel, i - atoms_.begin());
436	gezelter	246	} else {
437	gezelter	507	std::cerr << "Atom " << atom->getGlobalIndex()
438			<<" does not belong to Rigid body "<< getGlobalIndex() << std::endl;
439			return false;
440	gezelter	246	}
441	gezelter	507	}
442	gezelter	2
443	gezelter	507	bool RigidBody::getAtomRefCoor(Vector3d& coor, unsigned int index) {
444	gezelter	246	if (index < atoms_.size()) {
445
446	gezelter	507	coor = refCoords_[index];
447			return true;
448	gezelter	246	} else {
449	gezelter	507	std::cerr << index << " is an invalid index, current rigid body contains "
450			<< atoms_.size() << "atoms" << std::endl;
451			return false;
452	gezelter	2	}
453
454	gezelter	507	}
455	gezelter	2
456	gezelter	507	bool RigidBody::getAtomRefCoor(Vector3d& coor, Atom* atom) {
457	gezelter	246	std::vector<Atom*>::iterator i;
458			i = std::find(atoms_.begin(), atoms_.end(), atom);
459			if (i != atoms_.end()) {
460	gezelter	507	//RigidBody class makes sure refCoords_ and atoms_ match each other
461			coor = refCoords_[i - atoms_.begin()];
462			return true;
463	gezelter	246	} else {
464	gezelter	507	std::cerr << "Atom " << atom->getGlobalIndex()
465			<<" does not belong to Rigid body "<< getGlobalIndex() << std::endl;
466			return false;
467	gezelter	246	}
468	gezelter	2
469	gezelter	507	}
470	gezelter	2
471
472	gezelter	507	void RigidBody::addAtom(Atom* at, AtomStamp* ats) {
473	gezelter	2
474	gezelter	507	Vector3d coords;
475			Vector3d euler;
476	gezelter	2
477
478	gezelter	507	atoms_.push_back(at);
479	gezelter	246
480	gezelter	507	if( !ats->havePosition() ){
481			sprintf( painCave.errMsg,
482			"RigidBody error.\n"
483			"\tAtom %s does not have a position specified.\n"
484			"\tThis means RigidBody cannot set up reference coordinates.\n",
485			ats->getType() );
486			painCave.isFatal = 1;
487			simError();
488			}
489	gezelter	2
490	gezelter	507	coords[0] = ats->getPosX();
491			coords[1] = ats->getPosY();
492			coords[2] = ats->getPosZ();
493	gezelter	2
494	gezelter	507	refCoords_.push_back(coords);
495	gezelter	2
496	gezelter	507	RotMat3x3d identMat = RotMat3x3d::identity();
497	gezelter	2
498	gezelter	507	if (at->isDirectional()) {
499	gezelter	2
500	gezelter	507	if( !ats->haveOrientation() ){
501			sprintf( painCave.errMsg,
502			"RigidBody error.\n"
503			"\tAtom %s does not have an orientation specified.\n"
504			"\tThis means RigidBody cannot set up reference orientations.\n",
505			ats->getType() );
506			painCave.isFatal = 1;
507			simError();
508			}
509	gezelter	246
510	gezelter	507	euler[0] = ats->getEulerPhi() * NumericConstant::PI /180.0;
511			euler[1] = ats->getEulerTheta() * NumericConstant::PI /180.0;
512			euler[2] = ats->getEulerPsi() * NumericConstant::PI /180.0;
513	gezelter	2
514	gezelter	507	RotMat3x3d Atmp(euler);
515			refOrients_.push_back(Atmp);
516	gezelter	2
517	gezelter	507	}else {
518			refOrients_.push_back(identMat);
519			}
520	gezelter	2
521
522	gezelter	507	}
523	gezelter	2
524			}
525