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();

    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) {
        ef += atoms_[i]->getElectricField();
      }
    }         
    addFrc(frc);
    addTrq(trq);    

    if (sl & DataStorage::dslElectricField) {
      ef /= atoms_.size();
      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);
    
    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) {
        ef += atoms_[i]->getElectricField();
      }
      
      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 /= atoms_.size();
      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:	1842
Committed:	Tue Jan 29 19:10:04 2013 UTC (12 years, 3 months ago) by gezelter
File size:	16267 byte(s)
Log Message:	Only compute field for sites not excluded from the pairwise electrostatic contribution. Also, now compute the mean field felt by the sites in a rigid body, and report that as the rigid body's field.
#	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	1842
233			int sl = ((snapshotMan_->getCurrentSnapshot())->*storage_).getStorageLayout();
234
235	gezelter	1767	for (unsigned int i = 0; i < atoms_.size(); i++) {
236	gezelter	2
237	gezelter	507	afrc = atoms_[i]->getFrc();
238			apos = atoms_[i]->getPos();
239			rpos = apos - pos;
240	gezelter	246
241	gezelter	507	frc += afrc;
242	gezelter	2
243	gezelter	507	trq[0] += rpos[1]afrc[2] - rpos[2]afrc[1];
244			trq[1] += rpos[2]afrc[0] - rpos[0]afrc[2];
245			trq[2] += rpos[0]afrc[1] - rpos[1]afrc[0];
246	gezelter	2
247	gezelter	507	// If the atom has a torque associated with it, then we also need to
248			// migrate the torques onto the center of mass:
249	gezelter	2
250	gezelter	507	if (atoms_[i]->isDirectional()) {
251			atrq = atoms_[i]->getTrq();
252			trq += atrq;
253	gezelter	1842	}
254
255			if (sl & DataStorage::dslElectricField) {
256			ef += atoms_[i]->getElectricField();
257			}
258	gezelter	1126	}
259			addFrc(frc);
260			addTrq(trq);
261	gezelter	1842
262			if (sl & DataStorage::dslElectricField) {
263			ef /= atoms_.size();
264			setElectricField(ef);
265			}
266
267	gezelter	1126	}
268
269			Mat3x3d RigidBody::calcForcesAndTorquesAndVirial() {
270			Vector3d afrc;
271			Vector3d atrq;
272			Vector3d apos;
273			Vector3d rpos;
274	gezelter	1211	Vector3d dfrc;
275	gezelter	1126	Vector3d frc(0.0);
276	gezelter	1842	Vector3d trq(0.0);
277			Vector3d ef(0.0);
278
279	gezelter	1126	Vector3d pos = this->getPos();
280			Mat3x3d tau_(0.0);
281
282	gezelter	1842	int sl = ((snapshotMan_->getCurrentSnapshot())->*storage_).getStorageLayout();
283
284	gezelter	1767	for (unsigned int i = 0; i < atoms_.size(); i++) {
285	gezelter	1211
286	gezelter	1126	afrc = atoms_[i]->getFrc();
287			apos = atoms_[i]->getPos();
288			rpos = apos - pos;
289
290			frc += afrc;
291
292			trq[0] += rpos[1]afrc[2] - rpos[2]afrc[1];
293			trq[1] += rpos[2]afrc[0] - rpos[0]afrc[2];
294			trq[2] += rpos[0]afrc[1] - rpos[1]afrc[0];
295
296			// If the atom has a torque associated with it, then we also need to
297			// migrate the torques onto the center of mass:
298
299			if (atoms_[i]->isDirectional()) {
300			atrq = atoms_[i]->getTrq();
301			trq += atrq;
302	gezelter	507	}
303	gezelter	1842	if (sl & DataStorage::dslElectricField) {
304			ef += atoms_[i]->getElectricField();
305			}
306	gezelter	1126
307			tau_(0,0) -= rpos[0]*afrc[0];
308			tau_(0,1) -= rpos[0]*afrc[1];
309			tau_(0,2) -= rpos[0]*afrc[2];
310			tau_(1,0) -= rpos[1]*afrc[0];
311			tau_(1,1) -= rpos[1]*afrc[1];
312			tau_(1,2) -= rpos[1]*afrc[2];
313			tau_(2,0) -= rpos[2]*afrc[0];
314			tau_(2,1) -= rpos[2]*afrc[1];
315			tau_(2,2) -= rpos[2]*afrc[2];
316	gezelter	1211
317			}
318	tim	899	addFrc(frc);
319			addTrq(trq);
320	gezelter	1842
321			if (sl & DataStorage::dslElectricField) {
322			ef /= atoms_.size();
323			setElectricField(ef);
324			}
325
326	gezelter	1126	return tau_;
327	gezelter	507	}
328	gezelter	2
329	gezelter	507	void RigidBody::updateAtoms() {
330	gezelter	246	unsigned int i;
331			Vector3d ref;
332			Vector3d apos;
333			DirectionalAtom* dAtom;
334			Vector3d pos = getPos();
335			RotMat3x3d a = getA();
336	gezelter	2
337	gezelter	246	for (i = 0; i < atoms_.size(); i++) {
338
339	gezelter	507	ref = body2Lab(refCoords_[i]);
340	gezelter	2
341	gezelter	507	apos = pos + ref;
342	gezelter	2
343	gezelter	507	atoms_[i]->setPos(apos);
344	gezelter	2
345	gezelter	507	if (atoms_[i]->isDirectional()) {
346	gezelter	246
347	gezelter	507	dAtom = (DirectionalAtom *) atoms_[i];
348	gezelter	882	dAtom->setA(refOrients_[i].transpose() * a);
349	gezelter	507	}
350	gezelter	2
351			}
352
353	gezelter	507	}
354	gezelter	2
355
356	gezelter	507	void RigidBody::updateAtoms(int frame) {
357	tim	318	unsigned int i;
358			Vector3d ref;
359			Vector3d apos;
360			DirectionalAtom* dAtom;
361			Vector3d pos = getPos(frame);
362			RotMat3x3d a = getA(frame);
363
364			for (i = 0; i < atoms_.size(); i++) {
365
366	gezelter	507	ref = body2Lab(refCoords_[i], frame);
367	tim	318
368	gezelter	507	apos = pos + ref;
369	tim	318
370	gezelter	507	atoms_[i]->setPos(apos, frame);
371	tim	318
372	gezelter	507	if (atoms_[i]->isDirectional()) {
373	tim	318
374	gezelter	507	dAtom = (DirectionalAtom *) atoms_[i];
375	gezelter	882	dAtom->setA(refOrients_[i].transpose() * a, frame);
376	gezelter	507	}
377	tim	318
378			}
379
380	gezelter	507	}
381	tim	318
382	gezelter	507	void RigidBody::updateAtomVel() {
383	tim	318	Mat3x3d skewMat;;
384
385			Vector3d ji = getJ();
386			Mat3x3d I = getI();
387
388			skewMat(0, 0) =0;
389			skewMat(0, 1) = ji[2] /I(2, 2);
390			skewMat(0, 2) = -ji[1] /I(1, 1);
391
392			skewMat(1, 0) = -ji[2] /I(2, 2);
393			skewMat(1, 1) = 0;
394			skewMat(1, 2) = ji[0]/I(0, 0);
395
396			skewMat(2, 0) =ji[1] /I(1, 1);
397			skewMat(2, 1) = -ji[0]/I(0, 0);
398			skewMat(2, 2) = 0;
399
400			Mat3x3d mat = (getA() * skewMat).transpose();
401			Vector3d rbVel = getVel();
402
403
404			Vector3d velRot;
405	gezelter	1767	for (unsigned int i = 0 ; i < refCoords_.size(); ++i) {
406	gezelter	507	atoms_[i]->setVel(rbVel + mat * refCoords_[i]);
407	tim	318	}
408
409	gezelter	507	}
410	tim	318
411	gezelter	507	void RigidBody::updateAtomVel(int frame) {
412	tim	318	Mat3x3d skewMat;;
413
414			Vector3d ji = getJ(frame);
415			Mat3x3d I = getI();
416
417			skewMat(0, 0) =0;
418			skewMat(0, 1) = ji[2] /I(2, 2);
419			skewMat(0, 2) = -ji[1] /I(1, 1);
420
421			skewMat(1, 0) = -ji[2] /I(2, 2);
422			skewMat(1, 1) = 0;
423			skewMat(1, 2) = ji[0]/I(0, 0);
424
425			skewMat(2, 0) =ji[1] /I(1, 1);
426			skewMat(2, 1) = -ji[0]/I(0, 0);
427			skewMat(2, 2) = 0;
428
429			Mat3x3d mat = (getA(frame) * skewMat).transpose();
430			Vector3d rbVel = getVel(frame);
431
432
433			Vector3d velRot;
434	gezelter	1767	for (unsigned int i = 0 ; i < refCoords_.size(); ++i) {
435	gezelter	507	atoms_[i]->setVel(rbVel + mat * refCoords_[i], frame);
436	tim	318	}
437
438	gezelter	507	}
439	tim	318
440
441
442	gezelter	507	bool RigidBody::getAtomPos(Vector3d& pos, unsigned int index) {
443	gezelter	246	if (index < atoms_.size()) {
444	gezelter	2
445	gezelter	507	Vector3d ref = body2Lab(refCoords_[index]);
446			pos = getPos() + ref;
447			return true;
448	gezelter	246	} else {
449	gezelter	507	std::cerr << index << " is an invalid index, current rigid body contains "
450			<< atoms_.size() << "atoms" << std::endl;
451			return false;
452	gezelter	246	}
453	gezelter	507	}
454	gezelter	2
455	gezelter	507	bool RigidBody::getAtomPos(Vector3d& pos, 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			Vector3d ref = body2Lab(refCoords_[i - atoms_.begin()]);
461			pos = getPos() + ref;
462			return true;
463	gezelter	246	} else {
464	gezelter	507	std::cerr << "Atom " << atom->getGlobalIndex()
465			<<" does not belong to Rigid body "<< getGlobalIndex() << std::endl;
466			return false;
467	gezelter	2	}
468	gezelter	507	}
469			bool RigidBody::getAtomVel(Vector3d& vel, unsigned int index) {
470	gezelter	2
471	gezelter	246	//velRot = $(A\cdot skew(I^{-1}j))^{T}refCoor$
472	gezelter	2
473	gezelter	246	if (index < atoms_.size()) {
474	gezelter	2
475	gezelter	507	Vector3d velRot;
476			Mat3x3d skewMat;;
477			Vector3d ref = refCoords_[index];
478			Vector3d ji = getJ();
479			Mat3x3d I = getI();
480	gezelter	2
481	gezelter	507	skewMat(0, 0) =0;
482			skewMat(0, 1) = ji[2] /I(2, 2);
483			skewMat(0, 2) = -ji[1] /I(1, 1);
484	gezelter	2
485	gezelter	507	skewMat(1, 0) = -ji[2] /I(2, 2);
486			skewMat(1, 1) = 0;
487			skewMat(1, 2) = ji[0]/I(0, 0);
488	gezelter	2
489	gezelter	507	skewMat(2, 0) =ji[1] /I(1, 1);
490			skewMat(2, 1) = -ji[0]/I(0, 0);
491			skewMat(2, 2) = 0;
492	gezelter	2
493	gezelter	507	velRot = (getA() * skewMat).transpose() * ref;
494	gezelter	2
495	gezelter	507	vel =getVel() + velRot;
496			return true;
497	gezelter	246
498			} else {
499	gezelter	507	std::cerr << index << " is an invalid index, current rigid body contains "
500			<< atoms_.size() << "atoms" << std::endl;
501			return false;
502	gezelter	2	}
503	gezelter	507	}
504	gezelter	2
505	gezelter	507	bool RigidBody::getAtomVel(Vector3d& vel, Atom* atom) {
506	gezelter	2
507	gezelter	246	std::vector<Atom*>::iterator i;
508			i = std::find(atoms_.begin(), atoms_.end(), atom);
509			if (i != atoms_.end()) {
510	gezelter	507	return getAtomVel(vel, i - atoms_.begin());
511	gezelter	246	} else {
512	gezelter	507	std::cerr << "Atom " << atom->getGlobalIndex()
513			<<" does not belong to Rigid body "<< getGlobalIndex() << std::endl;
514			return false;
515	gezelter	246	}
516	gezelter	507	}
517	gezelter	2
518	gezelter	507	bool RigidBody::getAtomRefCoor(Vector3d& coor, unsigned int index) {
519	gezelter	246	if (index < atoms_.size()) {
520
521	gezelter	507	coor = refCoords_[index];
522			return true;
523	gezelter	246	} else {
524	gezelter	507	std::cerr << index << " is an invalid index, current rigid body contains "
525			<< atoms_.size() << "atoms" << std::endl;
526			return false;
527	gezelter	2	}
528
529	gezelter	507	}
530	gezelter	2
531	gezelter	507	bool RigidBody::getAtomRefCoor(Vector3d& coor, Atom* atom) {
532	gezelter	246	std::vector<Atom*>::iterator i;
533			i = std::find(atoms_.begin(), atoms_.end(), atom);
534			if (i != atoms_.end()) {
535	gezelter	507	//RigidBody class makes sure refCoords_ and atoms_ match each other
536			coor = refCoords_[i - atoms_.begin()];
537			return true;
538	gezelter	246	} else {
539	gezelter	507	std::cerr << "Atom " << atom->getGlobalIndex()
540			<<" does not belong to Rigid body "<< getGlobalIndex() << std::endl;
541			return false;
542	gezelter	246	}
543	gezelter	2
544	gezelter	507	}
545	gezelter	2
546
547	gezelter	507	void RigidBody::addAtom(Atom* at, AtomStamp* ats) {
548	gezelter	2
549	gezelter	507	Vector3d coords;
550			Vector3d euler;
551	gezelter	2
552
553	gezelter	507	atoms_.push_back(at);
554	gezelter	246
555	gezelter	507	if( !ats->havePosition() ){
556			sprintf( painCave.errMsg,
557			"RigidBody error.\n"
558			"\tAtom %s does not have a position specified.\n"
559			"\tThis means RigidBody cannot set up reference coordinates.\n",
560	tim	770	ats->getType().c_str() );
561	gezelter	507	painCave.isFatal = 1;
562			simError();
563			}
564	gezelter	2
565	gezelter	507	coords[0] = ats->getPosX();
566			coords[1] = ats->getPosY();
567			coords[2] = ats->getPosZ();
568	gezelter	2
569	gezelter	507	refCoords_.push_back(coords);
570	gezelter	2
571	gezelter	507	RotMat3x3d identMat = RotMat3x3d::identity();
572	gezelter	2
573	gezelter	507	if (at->isDirectional()) {
574	gezelter	2
575	gezelter	507	if( !ats->haveOrientation() ){
576			sprintf( painCave.errMsg,
577			"RigidBody error.\n"
578			"\tAtom %s does not have an orientation specified.\n"
579			"\tThis means RigidBody cannot set up reference orientations.\n",
580	tim	770	ats->getType().c_str() );
581	gezelter	507	painCave.isFatal = 1;
582			simError();
583			}
584	gezelter	246
585	gezelter	507	euler[0] = ats->getEulerPhi() * NumericConstant::PI /180.0;
586			euler[1] = ats->getEulerTheta() * NumericConstant::PI /180.0;
587			euler[2] = ats->getEulerPsi() * NumericConstant::PI /180.0;
588	gezelter	2
589	gezelter	507	RotMat3x3d Atmp(euler);
590			refOrients_.push_back(Atmp);
591	gezelter	2
592	gezelter	507	}else {
593			refOrients_.push_back(identMat);
594			}
595	gezelter	2
596
597	gezelter	507	}
598	gezelter	2
599			}
600