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