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. Acknowledgement of the program authors must be made in any
 *    publication of scientific results based in part on use of the
 *    program.  An acceptable form of acknowledgement is citation of
 *    the article in which the program was described (Matthew
 *    A. Meineke, Charles F. Vardeman II, Teng Lin, Christopher
 *    J. Fennell and J. Daniel Gezelter, "OOPSE: An Object-Oriented
 *    Parallel Simulation Engine for Molecular Dynamics,"
 *    J. Comput. Chem. 26, pp. 252-271 (2005))
 *
 * 2. Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 *
 * 3. 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.
 */
#include <algorithm>
#include <math.h>
#include "primitives/RigidBody.hpp"
#include "utils/simError.h"
#include "utils/NumericConstant.hpp"
namespace oopse {

  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] = 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]) < oopse::epsilon) {
        linear_ = true;
        linearAxis_ = i;
        ++ nLinearAxis;
      }
    }

    if (nLinearAxis > 1) {
      sprintf( painCave.errMsg,
               "RigidBody error.\n"
               "\tOOPSE 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 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:	3126
Committed:	Fri Apr 6 21:53:43 2007 UTC (18 years, 3 months ago) by gezelter
File size:	15353 byte(s)
Log Message:	Massive update to do virials (both atomic and cutoff-group) correctly. The rigid body constraint contributions had been missing and this was masked by the use of cutoff groups...
#	User	Rev	Content
1	gezelter	2204	/*
2	gezelter	1930	* 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			* 1. Acknowledgement of the program authors must be made in any
10			* publication of scientific results based in part on use of the
11			* program. An acceptable form of acknowledgement is citation of
12			* the article in which the program was described (Matthew
13			* A. Meineke, Charles F. Vardeman II, Teng Lin, Christopher
14			* J. Fennell and J. Daniel Gezelter, "OOPSE: An Object-Oriented
15			* Parallel Simulation Engine for Molecular Dynamics,"
16			* J. Comput. Chem. 26, pp. 252-271 (2005))
17			*
18			* 2. Redistributions of source code must retain the above copyright
19			* notice, this list of conditions and the following disclaimer.
20			*
21			* 3. Redistributions in binary form must reproduce the above copyright
22			* notice, this list of conditions and the following disclaimer in the
23			* documentation and/or other materials provided with the
24			* distribution.
25			*
26			* This software is provided "AS IS," without a warranty of any
27			* kind. All express or implied conditions, representations and
28			* warranties, including any implied warranty of merchantability,
29			* fitness for a particular purpose or non-infringement, are hereby
30			* excluded. The University of Notre Dame and its licensors shall not
31			* be liable for any damages suffered by licensee as a result of
32			* using, modifying or distributing the software or its
33			* derivatives. In no event will the University of Notre Dame or its
34			* licensors be liable for any lost revenue, profit or data, or for
35			* direct, indirect, special, consequential, incidental or punitive
36			* damages, however caused and regardless of the theory of liability,
37			* arising out of the use of or inability to use software, even if the
38			* University of Notre Dame has been advised of the possibility of
39			* such damages.
40			*/
41			#include <algorithm>
42	tim	1937	#include <math.h>
43	tim	1492	#include "primitives/RigidBody.hpp"
44			#include "utils/simError.h"
45	tim	2058	#include "utils/NumericConstant.hpp"
46	gezelter	1930	namespace oopse {
47	gezelter	1490
48	gezelter	2204	RigidBody::RigidBody() : StuntDouble(otRigidBody, &Snapshot::rigidbodyData), inertiaTensor_(0.0){
49	gezelter	1490
50	gezelter	2204	}
51	gezelter	1490
52	gezelter	2204	void RigidBody::setPrevA(const RotMat3x3d& a) {
53	gezelter	1930	((snapshotMan_->getPrevSnapshot())->*storage_).aMat[localIndex_] = a;
54	gezelter	1490
55	gezelter	1930	for (int i =0 ; i < atoms_.size(); ++i){
56	gezelter	2204	if (atoms_[i]->isDirectional()) {
57	gezelter	2582	atoms_[i]->setPrevA(refOrients_[i].transpose() * a);
58	gezelter	2204	}
59	gezelter	1930	}
60	gezelter	1490
61	gezelter	2204	}
62	gezelter	1490
63	gezelter	1930
64	gezelter	2204	void RigidBody::setA(const RotMat3x3d& a) {
65	gezelter	1930	((snapshotMan_->getCurrentSnapshot())->*storage_).aMat[localIndex_] = a;
66	gezelter	1490
67	gezelter	1930	for (int i =0 ; i < atoms_.size(); ++i){
68	gezelter	2204	if (atoms_[i]->isDirectional()) {
69	gezelter	2582	atoms_[i]->setA(refOrients_[i].transpose() * a);
70	gezelter	2204	}
71	gezelter	1930	}
72	gezelter	2204	}
73	gezelter	1490
74	gezelter	2204	void RigidBody::setA(const RotMat3x3d& a, int snapshotNo) {
75	gezelter	1930	((snapshotMan_->getSnapshot(snapshotNo))->*storage_).aMat[localIndex_] = a;
76			//((snapshotMan_->getSnapshot(snapshotNo))->storage_).electroFrame[localIndex_] = a.transpose() sU_;
77	gezelter	1490
78	gezelter	1930	for (int i =0 ; i < atoms_.size(); ++i){
79	gezelter	2204	if (atoms_[i]->isDirectional()) {
80	gezelter	2582	atoms_[i]->setA(refOrients_[i].transpose() * a, snapshotNo);
81	gezelter	2204	}
82	gezelter	1490	}
83
84	gezelter	2204	}
85	gezelter	1490
86	gezelter	2204	Mat3x3d RigidBody::getI() {
87	gezelter	1930	return inertiaTensor_;
88	gezelter	2204	}
89	gezelter	1490
90	tim	2759	std::vector<RealType> RigidBody::getGrad() {
91			std::vector<RealType> grad(6, 0.0);
92	gezelter	1930	Vector3d force;
93			Vector3d torque;
94			Vector3d myEuler;
95	tim	2759	RealType phi, theta, psi;
96			RealType cphi, sphi, ctheta, stheta;
97	gezelter	1930	Vector3d ephi;
98			Vector3d etheta;
99			Vector3d epsi;
100	gezelter	1490
101	gezelter	1930	force = getFrc();
102			torque =getTrq();
103			myEuler = getA().toEulerAngles();
104	gezelter	1490
105	gezelter	1930	phi = myEuler[0];
106			theta = myEuler[1];
107			psi = myEuler[2];
108	gezelter	1490
109	gezelter	1930	cphi = cos(phi);
110			sphi = sin(phi);
111			ctheta = cos(theta);
112			stheta = sin(theta);
113	gezelter	1490
114	gezelter	1930	// get unit vectors along the phi, theta and psi rotation axes
115	gezelter	1490
116	gezelter	1930	ephi[0] = 0.0;
117			ephi[1] = 0.0;
118			ephi[2] = 1.0;
119	gezelter	1490
120	gezelter	1930	etheta[0] = cphi;
121			etheta[1] = sphi;
122			etheta[2] = 0.0;
123	gezelter	1490
124	gezelter	1930	epsi[0] = stheta * cphi;
125			epsi[1] = stheta * sphi;
126			epsi[2] = ctheta;
127	gezelter	1490
128	gezelter	1930	//gradient is equal to -force
129			for (int j = 0 ; j<3; j++)
130	gezelter	2204	grad[j] = -force[j];
131	gezelter	1490
132	gezelter	1930	for (int j = 0; j < 3; j++ ) {
133	gezelter	1490
134	gezelter	2204	grad[3] += torque[j]*ephi[j];
135			grad[4] += torque[j]*etheta[j];
136			grad[5] += torque[j]*epsi[j];
137	gezelter	1490
138	gezelter	1930	}
139
140			return grad;
141	gezelter	2204	}
142	gezelter	1490
143	gezelter	2204	void RigidBody::accept(BaseVisitor* v) {
144	gezelter	1930	v->visit(this);
145	gezelter	2204	}
146	gezelter	1490
147	gezelter	2204	/*@todo need modification /
148			void RigidBody::calcRefCoords() {
149	tim	2759	RealType mtmp;
150	gezelter	1930	Vector3d refCOM(0.0);
151			mass_ = 0.0;
152			for (std::size_t i = 0; i < atoms_.size(); ++i) {
153	gezelter	2204	mtmp = atoms_[i]->getMass();
154			mass_ += mtmp;
155			refCOM += refCoords_[i]*mtmp;
156	gezelter	1930	}
157			refCOM /= mass_;
158	gezelter	1490
159	gezelter	1930	// Next, move the origin of the reference coordinate system to the COM:
160			for (std::size_t i = 0; i < atoms_.size(); ++i) {
161	gezelter	2204	refCoords_[i] -= refCOM;
162	gezelter	1930	}
163	gezelter	1490
164	gezelter	2204	// Moment of Inertia calculation
165	tim	2341	Mat3x3d Itmp(0.0);
166	gezelter	1930	for (std::size_t i = 0; i < atoms_.size(); i++) {
167	tim	2341	Mat3x3d IAtom(0.0);
168	gezelter	2204	mtmp = atoms_[i]->getMass();
169	tim	2341	IAtom -= outProduct(refCoords_[i], refCoords_[i]) * mtmp;
170	tim	2759	RealType r2 = refCoords_[i].lengthSquare();
171	tim	2341	IAtom(0, 0) += mtmp * r2;
172			IAtom(1, 1) += mtmp * r2;
173			IAtom(2, 2) += mtmp * r2;
174	tim	2347	Itmp += IAtom;
175	gezelter	1490
176	tim	2341	//project the inertial moment of directional atoms into this rigid body
177	gezelter	2204	if (atoms_[i]->isDirectional()) {
178	tim	2345	Itmp += refOrients_[i].transpose() * atoms_[i]->getI() * refOrients_[i];
179	tim	2347	}
180	tim	1957	}
181
182	chrisfen	2394	// std::cout << Itmp << std::endl;
183	gezelter	2362
184	gezelter	1930	//diagonalize
185			Vector3d evals;
186			Mat3x3d::diagonalize(Itmp, evals, sU_);
187	gezelter	1490
188	gezelter	1930	// zero out I and then fill the diagonals with the moments of inertia:
189			inertiaTensor_(0, 0) = evals[0];
190			inertiaTensor_(1, 1) = evals[1];
191			inertiaTensor_(2, 2) = evals[2];
192
193			int nLinearAxis = 0;
194			for (int i = 0; i < 3; i++) {
195	gezelter	2204	if (fabs(evals[i]) < oopse::epsilon) {
196			linear_ = true;
197			linearAxis_ = i;
198			++ nLinearAxis;
199			}
200	gezelter	1930	}
201	gezelter	1490
202	gezelter	1930	if (nLinearAxis > 1) {
203	gezelter	2204	sprintf( painCave.errMsg,
204			"RigidBody error.\n"
205			"\tOOPSE found more than one axis in this rigid body with a vanishing \n"
206			"\tmoment of inertia. This can happen in one of three ways:\n"
207			"\t 1) Only one atom was specified, or \n"
208			"\t 2) All atoms were specified at the same location, or\n"
209			"\t 3) The programmers did something stupid.\n"
210			"\tIt is silly to use a rigid body to describe this situation. Be smarter.\n"
211			);
212			painCave.isFatal = 1;
213			simError();
214	gezelter	1930	}
215	gezelter	1490
216	gezelter	2204	}
217	gezelter	1490
218	gezelter	2204	void RigidBody::calcForcesAndTorques() {
219	gezelter	1930	Vector3d afrc;
220			Vector3d atrq;
221			Vector3d apos;
222			Vector3d rpos;
223			Vector3d frc(0.0);
224	gezelter	3126	Vector3d trq(0.0);
225	gezelter	1930	Vector3d pos = this->getPos();
226			for (int i = 0; i < atoms_.size(); i++) {
227	gezelter	1490
228	gezelter	2204	afrc = atoms_[i]->getFrc();
229			apos = atoms_[i]->getPos();
230			rpos = apos - pos;
231	gezelter	1930
232	gezelter	2204	frc += afrc;
233	gezelter	1490
234	gezelter	2204	trq[0] += rpos[1]afrc[2] - rpos[2]afrc[1];
235			trq[1] += rpos[2]afrc[0] - rpos[0]afrc[2];
236			trq[2] += rpos[0]afrc[1] - rpos[1]afrc[0];
237	gezelter	1490
238	gezelter	2204	// If the atom has a torque associated with it, then we also need to
239			// migrate the torques onto the center of mass:
240	gezelter	1490
241	gezelter	2204	if (atoms_[i]->isDirectional()) {
242			atrq = atoms_[i]->getTrq();
243			trq += atrq;
244	gezelter	3126	}
245			}
246			addFrc(frc);
247			addTrq(trq);
248			}
249
250			Mat3x3d RigidBody::calcForcesAndTorquesAndVirial() {
251			Vector3d afrc;
252			Vector3d atrq;
253			Vector3d apos;
254			Vector3d rpos;
255			Vector3d frc(0.0);
256			Vector3d trq(0.0);
257			Vector3d pos = this->getPos();
258			Mat3x3d tau_(0.0);
259
260			for (int i = 0; i < atoms_.size(); i++) {
261
262			afrc = atoms_[i]->getFrc();
263			apos = atoms_[i]->getPos();
264			rpos = apos - pos;
265
266			frc += afrc;
267
268			trq[0] += rpos[1]afrc[2] - rpos[2]afrc[1];
269			trq[1] += rpos[2]afrc[0] - rpos[0]afrc[2];
270			trq[2] += rpos[0]afrc[1] - rpos[1]afrc[0];
271
272			// If the atom has a torque associated with it, then we also need to
273			// migrate the torques onto the center of mass:
274
275			if (atoms_[i]->isDirectional()) {
276			atrq = atoms_[i]->getTrq();
277			trq += atrq;
278	gezelter	2204	}
279	gezelter	3126
280			tau_(0,0) -= rpos[0]*afrc[0];
281			tau_(0,1) -= rpos[0]*afrc[1];
282			tau_(0,2) -= rpos[0]*afrc[2];
283			tau_(1,0) -= rpos[1]*afrc[0];
284			tau_(1,1) -= rpos[1]*afrc[1];
285			tau_(1,2) -= rpos[1]*afrc[2];
286			tau_(2,0) -= rpos[2]*afrc[0];
287			tau_(2,1) -= rpos[2]*afrc[1];
288			tau_(2,2) -= rpos[2]*afrc[2];
289
290			}
291	tim	2625	addFrc(frc);
292			addTrq(trq);
293	gezelter	3126	return tau_;
294	gezelter	2204	}
295	gezelter	1490
296	gezelter	2204	void RigidBody::updateAtoms() {
297	gezelter	1930	unsigned int i;
298			Vector3d ref;
299			Vector3d apos;
300			DirectionalAtom* dAtom;
301			Vector3d pos = getPos();
302			RotMat3x3d a = getA();
303	gezelter	1490
304	gezelter	1930	for (i = 0; i < atoms_.size(); i++) {
305
306	gezelter	2204	ref = body2Lab(refCoords_[i]);
307	gezelter	1490
308	gezelter	2204	apos = pos + ref;
309	gezelter	1490
310	gezelter	2204	atoms_[i]->setPos(apos);
311	gezelter	1490
312	gezelter	2204	if (atoms_[i]->isDirectional()) {
313	gezelter	1930
314	gezelter	2204	dAtom = (DirectionalAtom *) atoms_[i];
315	gezelter	2582	dAtom->setA(refOrients_[i].transpose() * a);
316	gezelter	2204	}
317	gezelter	1490
318			}
319
320	gezelter	2204	}
321	gezelter	1490
322
323	gezelter	2204	void RigidBody::updateAtoms(int frame) {
324	tim	2002	unsigned int i;
325			Vector3d ref;
326			Vector3d apos;
327			DirectionalAtom* dAtom;
328			Vector3d pos = getPos(frame);
329			RotMat3x3d a = getA(frame);
330
331			for (i = 0; i < atoms_.size(); i++) {
332
333	gezelter	2204	ref = body2Lab(refCoords_[i], frame);
334	tim	2002
335	gezelter	2204	apos = pos + ref;
336	tim	2002
337	gezelter	2204	atoms_[i]->setPos(apos, frame);
338	tim	2002
339	gezelter	2204	if (atoms_[i]->isDirectional()) {
340	tim	2002
341	gezelter	2204	dAtom = (DirectionalAtom *) atoms_[i];
342	gezelter	2582	dAtom->setA(refOrients_[i].transpose() * a, frame);
343	gezelter	2204	}
344	tim	2002
345			}
346
347	gezelter	2204	}
348	tim	2002
349	gezelter	2204	void RigidBody::updateAtomVel() {
350	tim	2002	Mat3x3d skewMat;;
351
352			Vector3d ji = getJ();
353			Mat3x3d I = getI();
354
355			skewMat(0, 0) =0;
356			skewMat(0, 1) = ji[2] /I(2, 2);
357			skewMat(0, 2) = -ji[1] /I(1, 1);
358
359			skewMat(1, 0) = -ji[2] /I(2, 2);
360			skewMat(1, 1) = 0;
361			skewMat(1, 2) = ji[0]/I(0, 0);
362
363			skewMat(2, 0) =ji[1] /I(1, 1);
364			skewMat(2, 1) = -ji[0]/I(0, 0);
365			skewMat(2, 2) = 0;
366
367			Mat3x3d mat = (getA() * skewMat).transpose();
368			Vector3d rbVel = getVel();
369
370
371			Vector3d velRot;
372			for (int i =0 ; i < refCoords_.size(); ++i) {
373	gezelter	2204	atoms_[i]->setVel(rbVel + mat * refCoords_[i]);
374	tim	2002	}
375
376	gezelter	2204	}
377	tim	2002
378	gezelter	2204	void RigidBody::updateAtomVel(int frame) {
379	tim	2002	Mat3x3d skewMat;;
380
381			Vector3d ji = getJ(frame);
382			Mat3x3d I = getI();
383
384			skewMat(0, 0) =0;
385			skewMat(0, 1) = ji[2] /I(2, 2);
386			skewMat(0, 2) = -ji[1] /I(1, 1);
387
388			skewMat(1, 0) = -ji[2] /I(2, 2);
389			skewMat(1, 1) = 0;
390			skewMat(1, 2) = ji[0]/I(0, 0);
391
392			skewMat(2, 0) =ji[1] /I(1, 1);
393			skewMat(2, 1) = -ji[0]/I(0, 0);
394			skewMat(2, 2) = 0;
395
396			Mat3x3d mat = (getA(frame) * skewMat).transpose();
397			Vector3d rbVel = getVel(frame);
398
399
400			Vector3d velRot;
401			for (int i =0 ; i < refCoords_.size(); ++i) {
402	gezelter	2204	atoms_[i]->setVel(rbVel + mat * refCoords_[i], frame);
403	tim	2002	}
404
405	gezelter	2204	}
406	tim	2002
407
408
409	gezelter	2204	bool RigidBody::getAtomPos(Vector3d& pos, unsigned int index) {
410	gezelter	1930	if (index < atoms_.size()) {
411	gezelter	1490
412	gezelter	2204	Vector3d ref = body2Lab(refCoords_[index]);
413			pos = getPos() + ref;
414			return true;
415	gezelter	1930	} else {
416	gezelter	2204	std::cerr << index << " is an invalid index, current rigid body contains "
417			<< atoms_.size() << "atoms" << std::endl;
418			return false;
419	gezelter	1930	}
420	gezelter	2204	}
421	gezelter	1490
422	gezelter	2204	bool RigidBody::getAtomPos(Vector3d& pos, Atom* atom) {
423	gezelter	1930	std::vector<Atom*>::iterator i;
424			i = std::find(atoms_.begin(), atoms_.end(), atom);
425			if (i != atoms_.end()) {
426	gezelter	2204	//RigidBody class makes sure refCoords_ and atoms_ match each other
427			Vector3d ref = body2Lab(refCoords_[i - atoms_.begin()]);
428			pos = getPos() + ref;
429			return true;
430	gezelter	1930	} else {
431	gezelter	2204	std::cerr << "Atom " << atom->getGlobalIndex()
432			<<" does not belong to Rigid body "<< getGlobalIndex() << std::endl;
433			return false;
434	gezelter	1490	}
435	gezelter	2204	}
436			bool RigidBody::getAtomVel(Vector3d& vel, unsigned int index) {
437	gezelter	1490
438	gezelter	1930	//velRot = $(A\cdot skew(I^{-1}j))^{T}refCoor$
439	gezelter	1490
440	gezelter	1930	if (index < atoms_.size()) {
441	gezelter	1490
442	gezelter	2204	Vector3d velRot;
443			Mat3x3d skewMat;;
444			Vector3d ref = refCoords_[index];
445			Vector3d ji = getJ();
446			Mat3x3d I = getI();
447	gezelter	1490
448	gezelter	2204	skewMat(0, 0) =0;
449			skewMat(0, 1) = ji[2] /I(2, 2);
450			skewMat(0, 2) = -ji[1] /I(1, 1);
451	gezelter	1490
452	gezelter	2204	skewMat(1, 0) = -ji[2] /I(2, 2);
453			skewMat(1, 1) = 0;
454			skewMat(1, 2) = ji[0]/I(0, 0);
455	gezelter	1490
456	gezelter	2204	skewMat(2, 0) =ji[1] /I(1, 1);
457			skewMat(2, 1) = -ji[0]/I(0, 0);
458			skewMat(2, 2) = 0;
459	gezelter	1490
460	gezelter	2204	velRot = (getA() * skewMat).transpose() * ref;
461	gezelter	1490
462	gezelter	2204	vel =getVel() + velRot;
463			return true;
464	gezelter	1930
465			} else {
466	gezelter	2204	std::cerr << index << " is an invalid index, current rigid body contains "
467			<< atoms_.size() << "atoms" << std::endl;
468			return false;
469	gezelter	1490	}
470	gezelter	2204	}
471	gezelter	1490
472	gezelter	2204	bool RigidBody::getAtomVel(Vector3d& vel, Atom* atom) {
473	gezelter	1490
474	gezelter	1930	std::vector<Atom*>::iterator i;
475			i = std::find(atoms_.begin(), atoms_.end(), atom);
476			if (i != atoms_.end()) {
477	gezelter	2204	return getAtomVel(vel, i - atoms_.begin());
478	gezelter	1930	} else {
479	gezelter	2204	std::cerr << "Atom " << atom->getGlobalIndex()
480			<<" does not belong to Rigid body "<< getGlobalIndex() << std::endl;
481			return false;
482	gezelter	1930	}
483	gezelter	2204	}
484	gezelter	1490
485	gezelter	2204	bool RigidBody::getAtomRefCoor(Vector3d& coor, unsigned int index) {
486	gezelter	1930	if (index < atoms_.size()) {
487
488	gezelter	2204	coor = refCoords_[index];
489			return true;
490	gezelter	1930	} else {
491	gezelter	2204	std::cerr << index << " is an invalid index, current rigid body contains "
492			<< atoms_.size() << "atoms" << std::endl;
493			return false;
494	gezelter	1490	}
495
496	gezelter	2204	}
497	gezelter	1490
498	gezelter	2204	bool RigidBody::getAtomRefCoor(Vector3d& coor, Atom* atom) {
499	gezelter	1930	std::vector<Atom*>::iterator i;
500			i = std::find(atoms_.begin(), atoms_.end(), atom);
501			if (i != atoms_.end()) {
502	gezelter	2204	//RigidBody class makes sure refCoords_ and atoms_ match each other
503			coor = refCoords_[i - atoms_.begin()];
504			return true;
505	gezelter	1930	} else {
506	gezelter	2204	std::cerr << "Atom " << atom->getGlobalIndex()
507			<<" does not belong to Rigid body "<< getGlobalIndex() << std::endl;
508			return false;
509	gezelter	1930	}
510	gezelter	1490
511	gezelter	2204	}
512	gezelter	1490
513
514	gezelter	2204	void RigidBody::addAtom(Atom* at, AtomStamp* ats) {
515	gezelter	1490
516	gezelter	2204	Vector3d coords;
517			Vector3d euler;
518	gezelter	1490
519
520	gezelter	2204	atoms_.push_back(at);
521	gezelter	1930
522	gezelter	2204	if( !ats->havePosition() ){
523			sprintf( painCave.errMsg,
524			"RigidBody error.\n"
525			"\tAtom %s does not have a position specified.\n"
526			"\tThis means RigidBody cannot set up reference coordinates.\n",
527	tim	2469	ats->getType().c_str() );
528	gezelter	2204	painCave.isFatal = 1;
529			simError();
530			}
531	gezelter	1490
532	gezelter	2204	coords[0] = ats->getPosX();
533			coords[1] = ats->getPosY();
534			coords[2] = ats->getPosZ();
535	gezelter	1490
536	gezelter	2204	refCoords_.push_back(coords);
537	gezelter	1490
538	gezelter	2204	RotMat3x3d identMat = RotMat3x3d::identity();
539	gezelter	1490
540	gezelter	2204	if (at->isDirectional()) {
541	gezelter	1490
542	gezelter	2204	if( !ats->haveOrientation() ){
543			sprintf( painCave.errMsg,
544			"RigidBody error.\n"
545			"\tAtom %s does not have an orientation specified.\n"
546			"\tThis means RigidBody cannot set up reference orientations.\n",
547	tim	2469	ats->getType().c_str() );
548	gezelter	2204	painCave.isFatal = 1;
549			simError();
550			}
551	gezelter	1930
552	gezelter	2204	euler[0] = ats->getEulerPhi() * NumericConstant::PI /180.0;
553			euler[1] = ats->getEulerTheta() * NumericConstant::PI /180.0;
554			euler[2] = ats->getEulerPsi() * NumericConstant::PI /180.0;
555	gezelter	1490
556	gezelter	2204	RotMat3x3d Atmp(euler);
557			refOrients_.push_back(Atmp);
558	gezelter	1490
559	gezelter	2204	}else {
560			refOrients_.push_back(identMat);
561			}
562	gezelter	1490
563
564	gezelter	2204	}
565	gezelter	1490
566			}
567