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;

    for (int i =0 ; i < atoms_.size(); ++i){
      if (atoms_[i]->isDirectional()) {
        atoms_[i]->setPrevA(refOrients_[i].transpose() * a);
      }
    }

  }

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

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