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