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, 234107 (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++) {
      
      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++;
      }
      
      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 = dynamic_cast<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 = dynamic_cast<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:	1874
Committed:	Wed May 15 15:09:35 2013 UTC (11 years, 11 months ago) by gezelter
File size:	16550 byte(s)
Log Message:	Fixed a bunch of cppcheck warnings.
#	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	gezelter	1850	* [3] Sun, Lin & Gezelter, J. Chem. Phys. 128, 234107 (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	1845	atype = atoms_[i]->getAtomType();
294
295	gezelter	1126	afrc = atoms_[i]->getFrc();
296			apos = atoms_[i]->getPos();
297			rpos = apos - pos;
298
299			frc += afrc;
300
301			trq[0] += rpos[1]afrc[2] - rpos[2]afrc[1];
302			trq[1] += rpos[2]afrc[0] - rpos[0]afrc[2];
303			trq[2] += rpos[0]afrc[1] - rpos[1]afrc[0];
304
305			// If the atom has a torque associated with it, then we also need to
306			// migrate the torques onto the center of mass:
307
308			if (atoms_[i]->isDirectional()) {
309			atrq = atoms_[i]->getTrq();
310			trq += atrq;
311	gezelter	507	}
312	gezelter	1845
313	gezelter	1844	if ((sl & DataStorage::dslElectricField) && (atype->isElectrostatic())) {
314	gezelter	1842	ef += atoms_[i]->getElectricField();
315	gezelter	1844	eCount++;
316	gezelter	1842	}
317	gezelter	1126
318			tau_(0,0) -= rpos[0]*afrc[0];
319			tau_(0,1) -= rpos[0]*afrc[1];
320			tau_(0,2) -= rpos[0]*afrc[2];
321			tau_(1,0) -= rpos[1]*afrc[0];
322			tau_(1,1) -= rpos[1]*afrc[1];
323			tau_(1,2) -= rpos[1]*afrc[2];
324			tau_(2,0) -= rpos[2]*afrc[0];
325			tau_(2,1) -= rpos[2]*afrc[1];
326			tau_(2,2) -= rpos[2]*afrc[2];
327	gezelter	1211
328			}
329	tim	899	addFrc(frc);
330			addTrq(trq);
331	gezelter	1842
332			if (sl & DataStorage::dslElectricField) {
333	gezelter	1844	ef /= eCount;
334	gezelter	1842	setElectricField(ef);
335			}
336
337	gezelter	1126	return tau_;
338	gezelter	507	}
339	gezelter	2
340	gezelter	507	void RigidBody::updateAtoms() {
341	gezelter	246	unsigned int i;
342			Vector3d ref;
343			Vector3d apos;
344			DirectionalAtom* dAtom;
345			Vector3d pos = getPos();
346			RotMat3x3d a = getA();
347	gezelter	2
348	gezelter	246	for (i = 0; i < atoms_.size(); i++) {
349
350	gezelter	507	ref = body2Lab(refCoords_[i]);
351	gezelter	2
352	gezelter	507	apos = pos + ref;
353	gezelter	2
354	gezelter	507	atoms_[i]->setPos(apos);
355	gezelter	2
356	gezelter	507	if (atoms_[i]->isDirectional()) {
357	gezelter	246
358	gezelter	1874	dAtom = dynamic_cast<DirectionalAtom *>(atoms_[i]);
359	gezelter	882	dAtom->setA(refOrients_[i].transpose() * a);
360	gezelter	507	}
361	gezelter	2
362			}
363
364	gezelter	507	}
365	gezelter	2
366
367	gezelter	507	void RigidBody::updateAtoms(int frame) {
368	tim	318	unsigned int i;
369			Vector3d ref;
370			Vector3d apos;
371			DirectionalAtom* dAtom;
372			Vector3d pos = getPos(frame);
373			RotMat3x3d a = getA(frame);
374
375			for (i = 0; i < atoms_.size(); i++) {
376
377	gezelter	507	ref = body2Lab(refCoords_[i], frame);
378	tim	318
379	gezelter	507	apos = pos + ref;
380	tim	318
381	gezelter	507	atoms_[i]->setPos(apos, frame);
382	tim	318
383	gezelter	507	if (atoms_[i]->isDirectional()) {
384	tim	318
385	gezelter	1874	dAtom = dynamic_cast<DirectionalAtom *>(atoms_[i]);
386	gezelter	882	dAtom->setA(refOrients_[i].transpose() * a, frame);
387	gezelter	507	}
388	tim	318
389			}
390
391	gezelter	507	}
392	tim	318
393	gezelter	507	void RigidBody::updateAtomVel() {
394	tim	318	Mat3x3d skewMat;;
395
396			Vector3d ji = getJ();
397			Mat3x3d I = getI();
398
399			skewMat(0, 0) =0;
400			skewMat(0, 1) = ji[2] /I(2, 2);
401			skewMat(0, 2) = -ji[1] /I(1, 1);
402
403			skewMat(1, 0) = -ji[2] /I(2, 2);
404			skewMat(1, 1) = 0;
405			skewMat(1, 2) = ji[0]/I(0, 0);
406
407			skewMat(2, 0) =ji[1] /I(1, 1);
408			skewMat(2, 1) = -ji[0]/I(0, 0);
409			skewMat(2, 2) = 0;
410
411			Mat3x3d mat = (getA() * skewMat).transpose();
412			Vector3d rbVel = getVel();
413
414
415			Vector3d velRot;
416	gezelter	1767	for (unsigned int i = 0 ; i < refCoords_.size(); ++i) {
417	gezelter	507	atoms_[i]->setVel(rbVel + mat * refCoords_[i]);
418	tim	318	}
419
420	gezelter	507	}
421	tim	318
422	gezelter	507	void RigidBody::updateAtomVel(int frame) {
423	tim	318	Mat3x3d skewMat;;
424
425			Vector3d ji = getJ(frame);
426			Mat3x3d I = getI();
427
428			skewMat(0, 0) =0;
429			skewMat(0, 1) = ji[2] /I(2, 2);
430			skewMat(0, 2) = -ji[1] /I(1, 1);
431
432			skewMat(1, 0) = -ji[2] /I(2, 2);
433			skewMat(1, 1) = 0;
434			skewMat(1, 2) = ji[0]/I(0, 0);
435
436			skewMat(2, 0) =ji[1] /I(1, 1);
437			skewMat(2, 1) = -ji[0]/I(0, 0);
438			skewMat(2, 2) = 0;
439
440			Mat3x3d mat = (getA(frame) * skewMat).transpose();
441			Vector3d rbVel = getVel(frame);
442
443
444			Vector3d velRot;
445	gezelter	1767	for (unsigned int i = 0 ; i < refCoords_.size(); ++i) {
446	gezelter	507	atoms_[i]->setVel(rbVel + mat * refCoords_[i], frame);
447	tim	318	}
448
449	gezelter	507	}
450	tim	318
451
452
453	gezelter	507	bool RigidBody::getAtomPos(Vector3d& pos, unsigned int index) {
454	gezelter	246	if (index < atoms_.size()) {
455	gezelter	2
456	gezelter	507	Vector3d ref = body2Lab(refCoords_[index]);
457			pos = getPos() + ref;
458			return true;
459	gezelter	246	} else {
460	gezelter	507	std::cerr << index << " is an invalid index, current rigid body contains "
461			<< atoms_.size() << "atoms" << std::endl;
462			return false;
463	gezelter	246	}
464	gezelter	507	}
465	gezelter	2
466	gezelter	507	bool RigidBody::getAtomPos(Vector3d& pos, Atom* atom) {
467	gezelter	246	std::vector<Atom*>::iterator i;
468			i = std::find(atoms_.begin(), atoms_.end(), atom);
469			if (i != atoms_.end()) {
470	gezelter	507	//RigidBody class makes sure refCoords_ and atoms_ match each other
471			Vector3d ref = body2Lab(refCoords_[i - atoms_.begin()]);
472			pos = getPos() + ref;
473			return true;
474	gezelter	246	} else {
475	gezelter	507	std::cerr << "Atom " << atom->getGlobalIndex()
476			<<" does not belong to Rigid body "<< getGlobalIndex() << std::endl;
477			return false;
478	gezelter	2	}
479	gezelter	507	}
480			bool RigidBody::getAtomVel(Vector3d& vel, unsigned int index) {
481	gezelter	2
482	gezelter	246	//velRot = $(A\cdot skew(I^{-1}j))^{T}refCoor$
483	gezelter	2
484	gezelter	246	if (index < atoms_.size()) {
485	gezelter	2
486	gezelter	507	Vector3d velRot;
487			Mat3x3d skewMat;;
488			Vector3d ref = refCoords_[index];
489			Vector3d ji = getJ();
490			Mat3x3d I = getI();
491	gezelter	2
492	gezelter	507	skewMat(0, 0) =0;
493			skewMat(0, 1) = ji[2] /I(2, 2);
494			skewMat(0, 2) = -ji[1] /I(1, 1);
495	gezelter	2
496	gezelter	507	skewMat(1, 0) = -ji[2] /I(2, 2);
497			skewMat(1, 1) = 0;
498			skewMat(1, 2) = ji[0]/I(0, 0);
499	gezelter	2
500	gezelter	507	skewMat(2, 0) =ji[1] /I(1, 1);
501			skewMat(2, 1) = -ji[0]/I(0, 0);
502			skewMat(2, 2) = 0;
503	gezelter	2
504	gezelter	507	velRot = (getA() * skewMat).transpose() * ref;
505	gezelter	2
506	gezelter	507	vel =getVel() + velRot;
507			return true;
508	gezelter	246
509			} else {
510	gezelter	507	std::cerr << index << " is an invalid index, current rigid body contains "
511			<< atoms_.size() << "atoms" << std::endl;
512			return false;
513	gezelter	2	}
514	gezelter	507	}
515	gezelter	2
516	gezelter	507	bool RigidBody::getAtomVel(Vector3d& vel, Atom* atom) {
517	gezelter	2
518	gezelter	246	std::vector<Atom*>::iterator i;
519			i = std::find(atoms_.begin(), atoms_.end(), atom);
520			if (i != atoms_.end()) {
521	gezelter	507	return getAtomVel(vel, i - atoms_.begin());
522	gezelter	246	} else {
523	gezelter	507	std::cerr << "Atom " << atom->getGlobalIndex()
524			<<" does not belong to Rigid body "<< getGlobalIndex() << std::endl;
525			return false;
526	gezelter	246	}
527	gezelter	507	}
528	gezelter	2
529	gezelter	507	bool RigidBody::getAtomRefCoor(Vector3d& coor, unsigned int index) {
530	gezelter	246	if (index < atoms_.size()) {
531
532	gezelter	507	coor = refCoords_[index];
533			return true;
534	gezelter	246	} else {
535	gezelter	507	std::cerr << index << " is an invalid index, current rigid body contains "
536			<< atoms_.size() << "atoms" << std::endl;
537			return false;
538	gezelter	2	}
539
540	gezelter	507	}
541	gezelter	2
542	gezelter	507	bool RigidBody::getAtomRefCoor(Vector3d& coor, Atom* atom) {
543	gezelter	246	std::vector<Atom*>::iterator i;
544			i = std::find(atoms_.begin(), atoms_.end(), atom);
545			if (i != atoms_.end()) {
546	gezelter	507	//RigidBody class makes sure refCoords_ and atoms_ match each other
547			coor = refCoords_[i - atoms_.begin()];
548			return true;
549	gezelter	246	} else {
550	gezelter	507	std::cerr << "Atom " << atom->getGlobalIndex()
551			<<" does not belong to Rigid body "<< getGlobalIndex() << std::endl;
552			return false;
553	gezelter	246	}
554	gezelter	2
555	gezelter	507	}
556	gezelter	2
557
558	gezelter	507	void RigidBody::addAtom(Atom* at, AtomStamp* ats) {
559	gezelter	2
560	gezelter	507	Vector3d coords;
561			Vector3d euler;
562	gezelter	2
563
564	gezelter	507	atoms_.push_back(at);
565	gezelter	246
566	gezelter	507	if( !ats->havePosition() ){
567			sprintf( painCave.errMsg,
568			"RigidBody error.\n"
569			"\tAtom %s does not have a position specified.\n"
570			"\tThis means RigidBody cannot set up reference coordinates.\n",
571	tim	770	ats->getType().c_str() );
572	gezelter	507	painCave.isFatal = 1;
573			simError();
574			}
575	gezelter	2
576	gezelter	507	coords[0] = ats->getPosX();
577			coords[1] = ats->getPosY();
578			coords[2] = ats->getPosZ();
579	gezelter	2
580	gezelter	507	refCoords_.push_back(coords);
581	gezelter	2
582	gezelter	507	RotMat3x3d identMat = RotMat3x3d::identity();
583	gezelter	2
584	gezelter	507	if (at->isDirectional()) {
585	gezelter	2
586	gezelter	507	if( !ats->haveOrientation() ){
587			sprintf( painCave.errMsg,
588			"RigidBody error.\n"
589			"\tAtom %s does not have an orientation specified.\n"
590			"\tThis means RigidBody cannot set up reference orientations.\n",
591	tim	770	ats->getType().c_str() );
592	gezelter	507	painCave.isFatal = 1;
593			simError();
594			}
595	gezelter	246
596	gezelter	507	euler[0] = ats->getEulerPhi() * NumericConstant::PI /180.0;
597			euler[1] = ats->getEulerTheta() * NumericConstant::PI /180.0;
598			euler[2] = ats->getEulerPsi() * NumericConstant::PI /180.0;
599	gezelter	2
600	gezelter	507	RotMat3x3d Atmp(euler);
601			refOrients_.push_back(Atmp);
602	gezelter	2
603	gezelter	507	}else {
604			refOrients_.push_back(identMat);
605			}
606	gezelter	2
607
608	gezelter	507	}
609	gezelter	2
610			}
611