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;

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

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