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