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, 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 pos = this->getPos();
    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;
      }      
    }         
    addFrc(frc);
    addTrq(trq);    
  }

  Mat3x3d RigidBody::calcForcesAndTorquesAndVirial() {
    Vector3d afrc;
    Vector3d atrq;
    Vector3d apos;
    Vector3d rpos;
    Vector3d dfrc;
    Vector3d frc(0.0);
    Vector3d trq(0.0);    
    Vector3d pos = this->getPos();
    Mat3x3d tau_(0.0);

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