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. Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 *
 * 2. 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.
 *
 * SUPPORT OPEN SCIENCE!  If you use OpenMD or its source code in your
 * research, please cite the appropriate papers when you publish your
 * work.  Good starting points are:
 *                                                                      
 * [1]  Meineke, et al., J. Comp. Chem. 26, 252-271 (2005).             
 * [2]  Fennell & Gezelter, J. Chem. Phys. 124, 234104 (2006).          
 * [3]  Sun, Lin & Gezelter, J. Chem. Phys. 128, 24107 (2008).          
 * [4]  Kuang & Gezelter,  J. Chem. Phys. 133, 164101 (2010).
 * [5]  Vardeman, Stocker & Gezelter, J. Chem. Theory Comput. 7, 834 (2011).
 */
#include <algorithm>
#include <math.h>
#include "primitives/RigidBody.hpp"
#include "utils/simError.h"
#include "utils/NumericConstant.hpp"
namespace OpenMD {
  
  RigidBody::RigidBody() : StuntDouble(otRigidBody, &Snapshot::rigidbodyData),
                           inertiaTensor_(0.0){    
  }
  
  void RigidBody::setPrevA(const RotMat3x3d& a) {
    ((snapshotMan_->getPrevSnapshot())->*storage_).aMat[localIndex_] = a;
    
    for (unsigned 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 (unsigned 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;
        
    for (unsigned 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<RealType> RigidBody::getGrad() {
    std::vector<RealType> grad(6, 0.0);
    Vector3d force;
    Vector3d torque;
    Vector3d myEuler;
    RealType phi, theta;
    // RealType psi;
    RealType 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] = -sphi;
    //etheta[1] =  cphi;
    //etheta[2] =  0.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() {
    RealType 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;
      RealType 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]) < OpenMD::epsilon) {
        linear_ = true;
        linearAxis_ = i;
        ++ nLinearAxis;
      }
    }

    if (nLinearAxis > 1) {
      sprintf( painCave.errMsg,
               "RigidBody error.\n"
               "\tOpenMD 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 ef(0.0);
    Vector3d pos = this->getPos();
    AtomType* atype;
    int eCount = 0;
    
    int sl = ((snapshotMan_->getCurrentSnapshot())->*storage_).getStorageLayout();
    
    for (unsigned int i = 0; i < atoms_.size(); i++) {

      atype = atoms_[i]->getAtomType();

      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;
      }

      if ((sl & DataStorage::dslElectricField) && (atype->isElectrostatic())) {
        ef += atoms_[i]->getElectricField();
        eCount++;
      }
    }         
    addFrc(frc);
    addTrq(trq);    

    if (sl & DataStorage::dslElectricField)  {
      ef /= eCount;
      setElectricField(ef);
    }

  }

  Mat3x3d RigidBody::calcForcesAndTorquesAndVirial() {
    Vector3d afrc;
    Vector3d atrq;
    Vector3d apos;
    Vector3d rpos;
    Vector3d dfrc;
    Vector3d frc(0.0);
    Vector3d trq(0.0);
    Vector3d ef(0.0);
    AtomType* atype;
    int eCount = 0;

    Vector3d pos = this->getPos();
    Mat3x3d tau_(0.0);

    int sl = ((snapshotMan_->getCurrentSnapshot())->*storage_).getStorageLayout();

    for (unsigned 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;
      }
      if ((sl & DataStorage::dslElectricField) && (atype->isElectrostatic())) {
        ef += atoms_[i]->getElectricField();
        eCount++;
      }
      
      tau_(0,0) -= rpos[0]*afrc[0];
      tau_(0,1) -= rpos[0]*afrc[1];
      tau_(0,2) -= rpos[0]*afrc[2];
      tau_(1,0) -= rpos[1]*afrc[0];
      tau_(1,1) -= rpos[1]*afrc[1];
      tau_(1,2) -= rpos[1]*afrc[2];
      tau_(2,0) -= rpos[2]*afrc[0];
      tau_(2,1) -= rpos[2]*afrc[1];
      tau_(2,2) -= rpos[2]*afrc[2];

    }
    addFrc(frc);
    addTrq(trq);

    if (sl & DataStorage::dslElectricField) {
      ef /= eCount;
      setElectricField(ef);
    }

    return tau_;
  }

  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 (unsigned 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 (unsigned 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:	1844
Committed:	Wed Jan 30 14:43:08 2013 UTC (12 years, 3 months ago) by gezelter
File size:	16481 byte(s)
Log Message:	Only compute fields for electrostatic AtomTypes.
#	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	gezelter	1390	* 1. Redistributions of source code must retain the above copyright
10	gezelter	246	* notice, this list of conditions and the following disclaimer.
11			*
12	gezelter	1390	* 2. Redistributions in binary form must reproduce the above copyright
13	gezelter	246	* notice, this list of conditions and the following disclaimer in the
14			* documentation and/or other materials provided with the
15			* distribution.
16			*
17			* This software is provided "AS IS," without a warranty of any
18			* kind. All express or implied conditions, representations and
19			* warranties, including any implied warranty of merchantability,
20			* fitness for a particular purpose or non-infringement, are hereby
21			* excluded. The University of Notre Dame and its licensors shall not
22			* be liable for any damages suffered by licensee as a result of
23			* using, modifying or distributing the software or its
24			* derivatives. In no event will the University of Notre Dame or its
25			* licensors be liable for any lost revenue, profit or data, or for
26			* direct, indirect, special, consequential, incidental or punitive
27			* damages, however caused and regardless of the theory of liability,
28			* arising out of the use of or inability to use software, even if the
29			* University of Notre Dame has been advised of the possibility of
30			* such damages.
31	gezelter	1390	*
32			* SUPPORT OPEN SCIENCE! If you use OpenMD or its source code in your
33			* research, please cite the appropriate papers when you publish your
34			* work. Good starting points are:
35			*
36			* [1] Meineke, et al., J. Comp. Chem. 26, 252-271 (2005).
37			* [2] Fennell & Gezelter, J. Chem. Phys. 124, 234104 (2006).
38			* [3] Sun, Lin & Gezelter, J. Chem. Phys. 128, 24107 (2008).
39	gezelter	1665	* [4] Kuang & Gezelter, J. Chem. Phys. 133, 164101 (2010).
40			* [5] Vardeman, Stocker & Gezelter, J. Chem. Theory Comput. 7, 834 (2011).
41	gezelter	246	*/
42			#include <algorithm>
43	tim	253	#include <math.h>
44	tim	3	#include "primitives/RigidBody.hpp"
45			#include "utils/simError.h"
46	tim	374	#include "utils/NumericConstant.hpp"
47	gezelter	1390	namespace OpenMD {
48	gezelter	1211
49			RigidBody::RigidBody() : StuntDouble(otRigidBody, &Snapshot::rigidbodyData),
50			inertiaTensor_(0.0){
51	gezelter	507	}
52	gezelter	1211
53	gezelter	507	void RigidBody::setPrevA(const RotMat3x3d& a) {
54	gezelter	246	((snapshotMan_->getPrevSnapshot())->*storage_).aMat[localIndex_] = a;
55	gezelter	1211
56	gezelter	1767	for (unsigned int i = 0 ; i < atoms_.size(); ++i){
57	gezelter	507	if (atoms_[i]->isDirectional()) {
58	gezelter	882	atoms_[i]->setPrevA(refOrients_[i].transpose() * a);
59	gezelter	507	}
60	gezelter	246	}
61	gezelter	1211
62	gezelter	507	}
63	gezelter	1211
64
65	gezelter	507	void RigidBody::setA(const RotMat3x3d& a) {
66	gezelter	246	((snapshotMan_->getCurrentSnapshot())->*storage_).aMat[localIndex_] = a;
67	gezelter	2
68	gezelter	1767	for (unsigned int i = 0 ; i < atoms_.size(); ++i){
69	gezelter	507	if (atoms_[i]->isDirectional()) {
70	gezelter	882	atoms_[i]->setA(refOrients_[i].transpose() * a);
71	gezelter	507	}
72	gezelter	246	}
73	gezelter	507	}
74	gezelter	1211
75	gezelter	507	void RigidBody::setA(const RotMat3x3d& a, int snapshotNo) {
76	gezelter	246	((snapshotMan_->getSnapshot(snapshotNo))->*storage_).aMat[localIndex_] = a;
77	gezelter	1794
78	gezelter	1767	for (unsigned 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	gezelter	1211
84	gezelter	507	}
85	gezelter	1211
86	gezelter	507	Mat3x3d RigidBody::getI() {
87	gezelter	246	return inertiaTensor_;
88	gezelter	507	}
89	gezelter	1211
90	tim	963	std::vector<RealType> RigidBody::getGrad() {
91			std::vector<RealType> grad(6, 0.0);
92	gezelter	246	Vector3d force;
93			Vector3d torque;
94			Vector3d myEuler;
95	gezelter	1798	RealType phi, theta;
96			// RealType psi;
97	tim	963	RealType cphi, sphi, ctheta, stheta;
98	gezelter	246	Vector3d ephi;
99			Vector3d etheta;
100			Vector3d epsi;
101	gezelter	1211
102	gezelter	246	force = getFrc();
103			torque =getTrq();
104			myEuler = getA().toEulerAngles();
105	gezelter	1211
106	gezelter	246	phi = myEuler[0];
107			theta = myEuler[1];
108	gezelter	1798	// psi = myEuler[2];
109	gezelter	1211
110	gezelter	246	cphi = cos(phi);
111			sphi = sin(phi);
112			ctheta = cos(theta);
113			stheta = sin(theta);
114	gezelter	1211
115	gezelter	246	// get unit vectors along the phi, theta and psi rotation axes
116	gezelter	1211
117	gezelter	246	ephi[0] = 0.0;
118			ephi[1] = 0.0;
119			ephi[2] = 1.0;
120	gezelter	1211
121	gezelter	1424	//etheta[0] = -sphi;
122			//etheta[1] = cphi;
123			//etheta[2] = 0.0;
124
125			etheta[0] = cphi;
126			etheta[1] = sphi;
127	cli2	1360	etheta[2] = 0.0;
128	gezelter	1211
129	gezelter	246	epsi[0] = stheta * cphi;
130			epsi[1] = stheta * sphi;
131			epsi[2] = ctheta;
132	gezelter	1211
133	gezelter	246	//gradient is equal to -force
134			for (int j = 0 ; j<3; j++)
135	gezelter	507	grad[j] = -force[j];
136	gezelter	1211
137	gezelter	246	for (int j = 0; j < 3; j++ ) {
138	gezelter	1211
139	gezelter	507	grad[3] += torque[j]*ephi[j];
140			grad[4] += torque[j]*etheta[j];
141			grad[5] += torque[j]*epsi[j];
142	gezelter	1211
143	gezelter	246	}
144
145			return grad;
146	gezelter	507	}
147	gezelter	1211
148	gezelter	507	void RigidBody::accept(BaseVisitor* v) {
149	gezelter	246	v->visit(this);
150	gezelter	507	}
151	gezelter	2
152	gezelter	507	/*@todo need modification /
153			void RigidBody::calcRefCoords() {
154	tim	963	RealType mtmp;
155	gezelter	246	Vector3d refCOM(0.0);
156			mass_ = 0.0;
157			for (std::size_t i = 0; i < atoms_.size(); ++i) {
158	gezelter	507	mtmp = atoms_[i]->getMass();
159			mass_ += mtmp;
160			refCOM += refCoords_[i]*mtmp;
161	gezelter	246	}
162			refCOM /= mass_;
163	gezelter	1211
164	gezelter	246	// Next, move the origin of the reference coordinate system to the COM:
165			for (std::size_t i = 0; i < atoms_.size(); ++i) {
166	gezelter	507	refCoords_[i] -= refCOM;
167	gezelter	246	}
168	gezelter	2
169	gezelter	507	// Moment of Inertia calculation
170	tim	642	Mat3x3d Itmp(0.0);
171	gezelter	246	for (std::size_t i = 0; i < atoms_.size(); i++) {
172	tim	642	Mat3x3d IAtom(0.0);
173	gezelter	507	mtmp = atoms_[i]->getMass();
174	tim	642	IAtom -= outProduct(refCoords_[i], refCoords_[i]) * mtmp;
175	tim	963	RealType r2 = refCoords_[i].lengthSquare();
176	tim	642	IAtom(0, 0) += mtmp * r2;
177			IAtom(1, 1) += mtmp * r2;
178			IAtom(2, 2) += mtmp * r2;
179	tim	648	Itmp += IAtom;
180	gezelter	1211
181	tim	642	//project the inertial moment of directional atoms into this rigid body
182	gezelter	507	if (atoms_[i]->isDirectional()) {
183	tim	646	Itmp += refOrients_[i].transpose() * atoms_[i]->getI() * refOrients_[i];
184	tim	648	}
185	tim	273	}
186
187	chrisfen	695	// std::cout << Itmp << std::endl;
188	gezelter	663
189	gezelter	246	//diagonalize
190			Vector3d evals;
191			Mat3x3d::diagonalize(Itmp, evals, sU_);
192	gezelter	2
193	gezelter	246	// zero out I and then fill the diagonals with the moments of inertia:
194			inertiaTensor_(0, 0) = evals[0];
195			inertiaTensor_(1, 1) = evals[1];
196			inertiaTensor_(2, 2) = evals[2];
197
198			int nLinearAxis = 0;
199			for (int i = 0; i < 3; i++) {
200	gezelter	1390	if (fabs(evals[i]) < OpenMD::epsilon) {
201	gezelter	507	linear_ = true;
202			linearAxis_ = i;
203			++ nLinearAxis;
204			}
205	gezelter	246	}
206	gezelter	2
207	gezelter	246	if (nLinearAxis > 1) {
208	gezelter	507	sprintf( painCave.errMsg,
209			"RigidBody error.\n"
210	gezelter	1390	"\tOpenMD found more than one axis in this rigid body with a vanishing \n"
211	gezelter	507	"\tmoment of inertia. This can happen in one of three ways:\n"
212			"\t 1) Only one atom was specified, or \n"
213			"\t 2) All atoms were specified at the same location, or\n"
214			"\t 3) The programmers did something stupid.\n"
215			"\tIt is silly to use a rigid body to describe this situation. Be smarter.\n"
216			);
217			painCave.isFatal = 1;
218			simError();
219	gezelter	246	}
220	gezelter	2
221	gezelter	507	}
222	gezelter	2
223	gezelter	507	void RigidBody::calcForcesAndTorques() {
224	gezelter	246	Vector3d afrc;
225			Vector3d atrq;
226			Vector3d apos;
227			Vector3d rpos;
228			Vector3d frc(0.0);
229	gezelter	1842	Vector3d trq(0.0);
230			Vector3d ef(0.0);
231	gezelter	246	Vector3d pos = this->getPos();
232	gezelter	1844	AtomType* atype;
233			int eCount = 0;
234
235	gezelter	1842	int sl = ((snapshotMan_->getCurrentSnapshot())->*storage_).getStorageLayout();
236	gezelter	1844
237	gezelter	1767	for (unsigned int i = 0; i < atoms_.size(); i++) {
238	gezelter	2
239	gezelter	1844	atype = atoms_[i]->getAtomType();
240
241	gezelter	507	afrc = atoms_[i]->getFrc();
242			apos = atoms_[i]->getPos();
243			rpos = apos - pos;
244	gezelter	246
245	gezelter	507	frc += afrc;
246	gezelter	2
247	gezelter	507	trq[0] += rpos[1]afrc[2] - rpos[2]afrc[1];
248			trq[1] += rpos[2]afrc[0] - rpos[0]afrc[2];
249			trq[2] += rpos[0]afrc[1] - rpos[1]afrc[0];
250	gezelter	2
251	gezelter	507	// If the atom has a torque associated with it, then we also need to
252			// migrate the torques onto the center of mass:
253	gezelter	2
254	gezelter	507	if (atoms_[i]->isDirectional()) {
255			atrq = atoms_[i]->getTrq();
256			trq += atrq;
257	gezelter	1842	}
258
259	gezelter	1844	if ((sl & DataStorage::dslElectricField) && (atype->isElectrostatic())) {
260	gezelter	1842	ef += atoms_[i]->getElectricField();
261	gezelter	1844	eCount++;
262	gezelter	1842	}
263	gezelter	1126	}
264			addFrc(frc);
265			addTrq(trq);
266	gezelter	1842
267	gezelter	1844	if (sl & DataStorage::dslElectricField) {
268			ef /= eCount;
269	gezelter	1842	setElectricField(ef);
270			}
271
272	gezelter	1126	}
273
274			Mat3x3d RigidBody::calcForcesAndTorquesAndVirial() {
275			Vector3d afrc;
276			Vector3d atrq;
277			Vector3d apos;
278			Vector3d rpos;
279	gezelter	1211	Vector3d dfrc;
280	gezelter	1126	Vector3d frc(0.0);
281	gezelter	1842	Vector3d trq(0.0);
282			Vector3d ef(0.0);
283	gezelter	1844	AtomType* atype;
284			int eCount = 0;
285
286	gezelter	1126	Vector3d pos = this->getPos();
287			Mat3x3d tau_(0.0);
288
289	gezelter	1842	int sl = ((snapshotMan_->getCurrentSnapshot())->*storage_).getStorageLayout();
290
291	gezelter	1767	for (unsigned int i = 0; i < atoms_.size(); i++) {
292	gezelter	1211
293	gezelter	1126	afrc = atoms_[i]->getFrc();
294			apos = atoms_[i]->getPos();
295			rpos = apos - pos;
296
297			frc += afrc;
298
299			trq[0] += rpos[1]afrc[2] - rpos[2]afrc[1];
300			trq[1] += rpos[2]afrc[0] - rpos[0]afrc[2];
301			trq[2] += rpos[0]afrc[1] - rpos[1]afrc[0];
302
303			// If the atom has a torque associated with it, then we also need to
304			// migrate the torques onto the center of mass:
305
306			if (atoms_[i]->isDirectional()) {
307			atrq = atoms_[i]->getTrq();
308			trq += atrq;
309	gezelter	507	}
310	gezelter	1844	if ((sl & DataStorage::dslElectricField) && (atype->isElectrostatic())) {
311	gezelter	1842	ef += atoms_[i]->getElectricField();
312	gezelter	1844	eCount++;
313	gezelter	1842	}
314	gezelter	1126
315			tau_(0,0) -= rpos[0]*afrc[0];
316			tau_(0,1) -= rpos[0]*afrc[1];
317			tau_(0,2) -= rpos[0]*afrc[2];
318			tau_(1,0) -= rpos[1]*afrc[0];
319			tau_(1,1) -= rpos[1]*afrc[1];
320			tau_(1,2) -= rpos[1]*afrc[2];
321			tau_(2,0) -= rpos[2]*afrc[0];
322			tau_(2,1) -= rpos[2]*afrc[1];
323			tau_(2,2) -= rpos[2]*afrc[2];
324	gezelter	1211
325			}
326	tim	899	addFrc(frc);
327			addTrq(trq);
328	gezelter	1842
329			if (sl & DataStorage::dslElectricField) {
330	gezelter	1844	ef /= eCount;
331	gezelter	1842	setElectricField(ef);
332			}
333
334	gezelter	1126	return tau_;
335	gezelter	507	}
336	gezelter	2
337	gezelter	507	void RigidBody::updateAtoms() {
338	gezelter	246	unsigned int i;
339			Vector3d ref;
340			Vector3d apos;
341			DirectionalAtom* dAtom;
342			Vector3d pos = getPos();
343			RotMat3x3d a = getA();
344	gezelter	2
345	gezelter	246	for (i = 0; i < atoms_.size(); i++) {
346
347	gezelter	507	ref = body2Lab(refCoords_[i]);
348	gezelter	2
349	gezelter	507	apos = pos + ref;
350	gezelter	2
351	gezelter	507	atoms_[i]->setPos(apos);
352	gezelter	2
353	gezelter	507	if (atoms_[i]->isDirectional()) {
354	gezelter	246
355	gezelter	507	dAtom = (DirectionalAtom *) atoms_[i];
356	gezelter	882	dAtom->setA(refOrients_[i].transpose() * a);
357	gezelter	507	}
358	gezelter	2
359			}
360
361	gezelter	507	}
362	gezelter	2
363
364	gezelter	507	void RigidBody::updateAtoms(int frame) {
365	tim	318	unsigned int i;
366			Vector3d ref;
367			Vector3d apos;
368			DirectionalAtom* dAtom;
369			Vector3d pos = getPos(frame);
370			RotMat3x3d a = getA(frame);
371
372			for (i = 0; i < atoms_.size(); i++) {
373
374	gezelter	507	ref = body2Lab(refCoords_[i], frame);
375	tim	318
376	gezelter	507	apos = pos + ref;
377	tim	318
378	gezelter	507	atoms_[i]->setPos(apos, frame);
379	tim	318
380	gezelter	507	if (atoms_[i]->isDirectional()) {
381	tim	318
382	gezelter	507	dAtom = (DirectionalAtom *) atoms_[i];
383	gezelter	882	dAtom->setA(refOrients_[i].transpose() * a, frame);
384	gezelter	507	}
385	tim	318
386			}
387
388	gezelter	507	}
389	tim	318
390	gezelter	507	void RigidBody::updateAtomVel() {
391	tim	318	Mat3x3d skewMat;;
392
393			Vector3d ji = getJ();
394			Mat3x3d I = getI();
395
396			skewMat(0, 0) =0;
397			skewMat(0, 1) = ji[2] /I(2, 2);
398			skewMat(0, 2) = -ji[1] /I(1, 1);
399
400			skewMat(1, 0) = -ji[2] /I(2, 2);
401			skewMat(1, 1) = 0;
402			skewMat(1, 2) = ji[0]/I(0, 0);
403
404			skewMat(2, 0) =ji[1] /I(1, 1);
405			skewMat(2, 1) = -ji[0]/I(0, 0);
406			skewMat(2, 2) = 0;
407
408			Mat3x3d mat = (getA() * skewMat).transpose();
409			Vector3d rbVel = getVel();
410
411
412			Vector3d velRot;
413	gezelter	1767	for (unsigned int i = 0 ; i < refCoords_.size(); ++i) {
414	gezelter	507	atoms_[i]->setVel(rbVel + mat * refCoords_[i]);
415	tim	318	}
416
417	gezelter	507	}
418	tim	318
419	gezelter	507	void RigidBody::updateAtomVel(int frame) {
420	tim	318	Mat3x3d skewMat;;
421
422			Vector3d ji = getJ(frame);
423			Mat3x3d I = getI();
424
425			skewMat(0, 0) =0;
426			skewMat(0, 1) = ji[2] /I(2, 2);
427			skewMat(0, 2) = -ji[1] /I(1, 1);
428
429			skewMat(1, 0) = -ji[2] /I(2, 2);
430			skewMat(1, 1) = 0;
431			skewMat(1, 2) = ji[0]/I(0, 0);
432
433			skewMat(2, 0) =ji[1] /I(1, 1);
434			skewMat(2, 1) = -ji[0]/I(0, 0);
435			skewMat(2, 2) = 0;
436
437			Mat3x3d mat = (getA(frame) * skewMat).transpose();
438			Vector3d rbVel = getVel(frame);
439
440
441			Vector3d velRot;
442	gezelter	1767	for (unsigned int i = 0 ; i < refCoords_.size(); ++i) {
443	gezelter	507	atoms_[i]->setVel(rbVel + mat * refCoords_[i], frame);
444	tim	318	}
445
446	gezelter	507	}
447	tim	318
448
449
450	gezelter	507	bool RigidBody::getAtomPos(Vector3d& pos, unsigned int index) {
451	gezelter	246	if (index < atoms_.size()) {
452	gezelter	2
453	gezelter	507	Vector3d ref = body2Lab(refCoords_[index]);
454			pos = getPos() + ref;
455			return true;
456	gezelter	246	} else {
457	gezelter	507	std::cerr << index << " is an invalid index, current rigid body contains "
458			<< atoms_.size() << "atoms" << std::endl;
459			return false;
460	gezelter	246	}
461	gezelter	507	}
462	gezelter	2
463	gezelter	507	bool RigidBody::getAtomPos(Vector3d& pos, Atom* atom) {
464	gezelter	246	std::vector<Atom*>::iterator i;
465			i = std::find(atoms_.begin(), atoms_.end(), atom);
466			if (i != atoms_.end()) {
467	gezelter	507	//RigidBody class makes sure refCoords_ and atoms_ match each other
468			Vector3d ref = body2Lab(refCoords_[i - atoms_.begin()]);
469			pos = getPos() + ref;
470			return true;
471	gezelter	246	} else {
472	gezelter	507	std::cerr << "Atom " << atom->getGlobalIndex()
473			<<" does not belong to Rigid body "<< getGlobalIndex() << std::endl;
474			return false;
475	gezelter	2	}
476	gezelter	507	}
477			bool RigidBody::getAtomVel(Vector3d& vel, unsigned int index) {
478	gezelter	2
479	gezelter	246	//velRot = $(A\cdot skew(I^{-1}j))^{T}refCoor$
480	gezelter	2
481	gezelter	246	if (index < atoms_.size()) {
482	gezelter	2
483	gezelter	507	Vector3d velRot;
484			Mat3x3d skewMat;;
485			Vector3d ref = refCoords_[index];
486			Vector3d ji = getJ();
487			Mat3x3d I = getI();
488	gezelter	2
489	gezelter	507	skewMat(0, 0) =0;
490			skewMat(0, 1) = ji[2] /I(2, 2);
491			skewMat(0, 2) = -ji[1] /I(1, 1);
492	gezelter	2
493	gezelter	507	skewMat(1, 0) = -ji[2] /I(2, 2);
494			skewMat(1, 1) = 0;
495			skewMat(1, 2) = ji[0]/I(0, 0);
496	gezelter	2
497	gezelter	507	skewMat(2, 0) =ji[1] /I(1, 1);
498			skewMat(2, 1) = -ji[0]/I(0, 0);
499			skewMat(2, 2) = 0;
500	gezelter	2
501	gezelter	507	velRot = (getA() * skewMat).transpose() * ref;
502	gezelter	2
503	gezelter	507	vel =getVel() + velRot;
504			return true;
505	gezelter	246
506			} else {
507	gezelter	507	std::cerr << index << " is an invalid index, current rigid body contains "
508			<< atoms_.size() << "atoms" << std::endl;
509			return false;
510	gezelter	2	}
511	gezelter	507	}
512	gezelter	2
513	gezelter	507	bool RigidBody::getAtomVel(Vector3d& vel, Atom* atom) {
514	gezelter	2
515	gezelter	246	std::vector<Atom*>::iterator i;
516			i = std::find(atoms_.begin(), atoms_.end(), atom);
517			if (i != atoms_.end()) {
518	gezelter	507	return getAtomVel(vel, i - atoms_.begin());
519	gezelter	246	} else {
520	gezelter	507	std::cerr << "Atom " << atom->getGlobalIndex()
521			<<" does not belong to Rigid body "<< getGlobalIndex() << std::endl;
522			return false;
523	gezelter	246	}
524	gezelter	507	}
525	gezelter	2
526	gezelter	507	bool RigidBody::getAtomRefCoor(Vector3d& coor, unsigned int index) {
527	gezelter	246	if (index < atoms_.size()) {
528
529	gezelter	507	coor = refCoords_[index];
530			return true;
531	gezelter	246	} else {
532	gezelter	507	std::cerr << index << " is an invalid index, current rigid body contains "
533			<< atoms_.size() << "atoms" << std::endl;
534			return false;
535	gezelter	2	}
536
537	gezelter	507	}
538	gezelter	2
539	gezelter	507	bool RigidBody::getAtomRefCoor(Vector3d& coor, Atom* atom) {
540	gezelter	246	std::vector<Atom*>::iterator i;
541			i = std::find(atoms_.begin(), atoms_.end(), atom);
542			if (i != atoms_.end()) {
543	gezelter	507	//RigidBody class makes sure refCoords_ and atoms_ match each other
544			coor = refCoords_[i - atoms_.begin()];
545			return true;
546	gezelter	246	} else {
547	gezelter	507	std::cerr << "Atom " << atom->getGlobalIndex()
548			<<" does not belong to Rigid body "<< getGlobalIndex() << std::endl;
549			return false;
550	gezelter	246	}
551	gezelter	2
552	gezelter	507	}
553	gezelter	2
554
555	gezelter	507	void RigidBody::addAtom(Atom* at, AtomStamp* ats) {
556	gezelter	2
557	gezelter	507	Vector3d coords;
558			Vector3d euler;
559	gezelter	2
560
561	gezelter	507	atoms_.push_back(at);
562	gezelter	246
563	gezelter	507	if( !ats->havePosition() ){
564			sprintf( painCave.errMsg,
565			"RigidBody error.\n"
566			"\tAtom %s does not have a position specified.\n"
567			"\tThis means RigidBody cannot set up reference coordinates.\n",
568	tim	770	ats->getType().c_str() );
569	gezelter	507	painCave.isFatal = 1;
570			simError();
571			}
572	gezelter	2
573	gezelter	507	coords[0] = ats->getPosX();
574			coords[1] = ats->getPosY();
575			coords[2] = ats->getPosZ();
576	gezelter	2
577	gezelter	507	refCoords_.push_back(coords);
578	gezelter	2
579	gezelter	507	RotMat3x3d identMat = RotMat3x3d::identity();
580	gezelter	2
581	gezelter	507	if (at->isDirectional()) {
582	gezelter	2
583	gezelter	507	if( !ats->haveOrientation() ){
584			sprintf( painCave.errMsg,
585			"RigidBody error.\n"
586			"\tAtom %s does not have an orientation specified.\n"
587			"\tThis means RigidBody cannot set up reference orientations.\n",
588	tim	770	ats->getType().c_str() );
589	gezelter	507	painCave.isFatal = 1;
590			simError();
591			}
592	gezelter	246
593	gezelter	507	euler[0] = ats->getEulerPhi() * NumericConstant::PI /180.0;
594			euler[1] = ats->getEulerTheta() * NumericConstant::PI /180.0;
595			euler[2] = ats->getEulerPsi() * NumericConstant::PI /180.0;
596	gezelter	2
597	gezelter	507	RotMat3x3d Atmp(euler);
598			refOrients_.push_back(Atmp);
599	gezelter	2
600	gezelter	507	}else {
601			refOrients_.push_back(identMat);
602			}
603	gezelter	2
604
605	gezelter	507	}
606	gezelter	2
607			}
608