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