OOPSE/libmdtools/RigidBody.cpp

#include <math.h>
#include "RigidBody.hpp"
#include "DirectionalAtom.hpp"
#include "simError.h"
#include "MatVec3.h"

RigidBody::RigidBody() : StuntDouble() {
  objType = OT_RIGIDBODY;
  is_linear = false;
  linear_axis =  -1;
  momIntTol = 1e-6;
}

RigidBody::~RigidBody() {
}

void RigidBody::addAtom(Atom* at, AtomStamp* ats) {

  vec3 coords;
  vec3 euler;
  mat3x3 Atmp;

  myAtoms.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() );
    painCave.isFatal = 1;
    simError();
  }
  
  coords[0] = ats->getPosX();
  coords[1] = ats->getPosY();
  coords[2] = ats->getPosZ();

  refCoords.push_back(coords);
  
  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() );
      painCave.isFatal = 1;
      simError();
    }    
    
    euler[0] = ats->getEulerPhi();
    euler[1] = ats->getEulerTheta();
    euler[2] = ats->getEulerPsi();
    
    doEulerToRotMat(euler, Atmp);
    
    refOrients.push_back(Atmp);
    
  }
}

void RigidBody::getPos(double theP[3]){
  for (int i = 0; i < 3 ; i++) 
    theP[i] = pos[i];
}       

void RigidBody::setPos(double theP[3]){
  for (int i = 0; i < 3 ; i++) 
    pos[i] = theP[i];
}       

void RigidBody::getVel(double theV[3]){
  for (int i = 0; i < 3 ; i++) 
    theV[i] = vel[i];
}       

void RigidBody::setVel(double theV[3]){
  for (int i = 0; i < 3 ; i++) 
    vel[i] = theV[i];
}       

void RigidBody::getFrc(double theF[3]){
  for (int i = 0; i < 3 ; i++) 
    theF[i] = frc[i];
}       

void RigidBody::setFrc(double theF[3]){
  for (int i = 0; i < 3 ; i++) 
    frc[i] = theF[i];
} 

void RigidBody::addFrc(double theF[3]){
  for (int i = 0; i < 3 ; i++) 
    frc[i] += theF[i];
}    

void RigidBody::zeroForces() {

  for (int i = 0; i < 3; i++) {
    frc[i] = 0.0;
    trq[i] = 0.0;
  }

}

void RigidBody::setEuler( double phi, double theta, double psi ){
  
    A[0][0] = (cos(phi) * cos(psi)) - (sin(phi) * cos(theta) * sin(psi));
    A[0][1] = (sin(phi) * cos(psi)) + (cos(phi) * cos(theta) * sin(psi));
    A[0][2] = sin(theta) * sin(psi);
    
    A[1][0] = -(cos(phi) * sin(psi)) - (sin(phi) * cos(theta) * cos(psi));
    A[1][1] = -(sin(phi) * sin(psi)) + (cos(phi) * cos(theta) * cos(psi));
    A[1][2] = sin(theta) * cos(psi);
    
    A[2][0] = sin(phi) * sin(theta);
    A[2][1] = -cos(phi) * sin(theta);
    A[2][2] = cos(theta);

}

void RigidBody::getQ( double q[4] ){
  
  double t, s;
  double ad1, ad2, ad3;
    
  t = A[0][0] + A[1][1] + A[2][2] + 1.0;
  if( t > 0.0 ){
    
    s = 0.5 / sqrt( t );
    q[0] = 0.25 / s;
    q[1] = (A[1][2] - A[2][1]) * s;
    q[2] = (A[2][0] - A[0][2]) * s;
    q[3] = (A[0][1] - A[1][0]) * s;
  }
  else{
    
    ad1 = fabs( A[0][0] );
    ad2 = fabs( A[1][1] );
    ad3 = fabs( A[2][2] );
    
    if( ad1 >= ad2 && ad1 >= ad3 ){
      
      s = 2.0 * sqrt( 1.0 + A[0][0] - A[1][1] - A[2][2] );
      q[0] = (A[1][2] + A[2][1]) / s;
      q[1] = 0.5 / s;
      q[2] = (A[0][1] + A[1][0]) / s;
      q[3] = (A[0][2] + A[2][0]) / s;
    }
    else if( ad2 >= ad1 && ad2 >= ad3 ){
      
      s = sqrt( 1.0 + A[1][1] - A[0][0] - A[2][2] ) * 2.0;
      q[0] = (A[0][2] + A[2][0]) / s;
      q[1] = (A[0][1] + A[1][0]) / s;
      q[2] = 0.5 / s;
      q[3] = (A[1][2] + A[2][1]) / s;
    }
    else{
      
      s = sqrt( 1.0 + A[2][2] - A[0][0] - A[1][1] ) * 2.0;
      q[0] = (A[0][1] + A[1][0]) / s;
      q[1] = (A[0][2] + A[2][0]) / s;
      q[2] = (A[1][2] + A[2][1]) / s;
      q[3] = 0.5 / s;
    }
  }
}

void RigidBody::setQ( double the_q[4] ){

  double q0Sqr, q1Sqr, q2Sqr, q3Sqr;
  
  q0Sqr = the_q[0] * the_q[0];
  q1Sqr = the_q[1] * the_q[1];
  q2Sqr = the_q[2] * the_q[2];
  q3Sqr = the_q[3] * the_q[3];
  
  A[0][0] = q0Sqr + q1Sqr - q2Sqr - q3Sqr;
  A[0][1] = 2.0 * ( the_q[1] * the_q[2] + the_q[0] * the_q[3] );
  A[0][2] = 2.0 * ( the_q[1] * the_q[3] - the_q[0] * the_q[2] );
  
  A[1][0] = 2.0 * ( the_q[1] * the_q[2] - the_q[0] * the_q[3] );
  A[1][1] = q0Sqr - q1Sqr + q2Sqr - q3Sqr;
  A[1][2] = 2.0 * ( the_q[2] * the_q[3] + the_q[0] * the_q[1] );
  
  A[2][0] = 2.0 * ( the_q[1] * the_q[3] + the_q[0] * the_q[2] );
  A[2][1] = 2.0 * ( the_q[2] * the_q[3] - the_q[0] * the_q[1] );
  A[2][2] = q0Sqr - q1Sqr -q2Sqr +q3Sqr;   
}

void RigidBody::getA( double the_A[3][3] ){
  
  for (int i = 0; i < 3; i++) 
    for (int j = 0; j < 3; j++) 
      the_A[i][j] = A[i][j];

}

void RigidBody::setA( double the_A[3][3] ){

  for (int i = 0; i < 3; i++) 
    for (int j = 0; j < 3; j++) 
      A[i][j] = the_A[i][j];
   
}

void RigidBody::getJ( double theJ[3] ){
  
  for (int i = 0; i < 3; i++)
    theJ[i] = ji[i];

}

void RigidBody::setJ( double theJ[3] ){
  
  for (int i = 0; i < 3; i++)
    ji[i] = theJ[i];

}

void RigidBody::getTrq(double theT[3]){
  for (int i = 0; i < 3 ; i++) 
    theT[i] = trq[i];
}       

void RigidBody::setTrq(double theT[3]){
  for (int i = 0; i < 3 ; i++) 
    trq[i] = theT[i];
}       

void RigidBody::addTrq(double theT[3]){
  for (int i = 0; i < 3 ; i++) 
    trq[i] += theT[i];
}       

void RigidBody::getI( double the_I[3][3] ){  

    for (int i = 0; i < 3; i++) 
      for (int j = 0; j < 3; j++) 
        the_I[i][j] = I[i][j];

}

void RigidBody::lab2Body( double r[3] ){

  double rl[3]; // the lab frame vector 
  
  rl[0] = r[0];
  rl[1] = r[1];
  rl[2] = r[2];
  
  r[0] = (A[0][0] * rl[0]) + (A[0][1] * rl[1]) + (A[0][2] * rl[2]);
  r[1] = (A[1][0] * rl[0]) + (A[1][1] * rl[1]) + (A[1][2] * rl[2]);
  r[2] = (A[2][0] * rl[0]) + (A[2][1] * rl[1]) + (A[2][2] * rl[2]);

}

void RigidBody::body2Lab( double r[3] ){

  double rb[3]; // the body frame vector 
  
  rb[0] = r[0];
  rb[1] = r[1];
  rb[2] = r[2];
  
  r[0] = (A[0][0] * rb[0]) + (A[1][0] * rb[1]) + (A[2][0] * rb[2]);
  r[1] = (A[0][1] * rb[0]) + (A[1][1] * rb[1]) + (A[2][1] * rb[2]);
  r[2] = (A[0][2] * rb[0]) + (A[1][2] * rb[1]) + (A[2][2] * rb[2]);

}

double RigidBody::getZangle( ){
    return zAngle;
}

void RigidBody::setZangle( double zAng ){
    zAngle = zAng;
}

void RigidBody::addZangle( double zAng ){
    zAngle += zAng;
}

void RigidBody::calcRefCoords( ) {

  int i,j,k, it, n_linear_coords;
  double mtmp;
  vec3 apos;
  double refCOM[3];
  vec3 ptmp;
  double Itmp[3][3];
  double evals[3];
  double evects[3][3];
  double r, r2, len;

  // First, find the center of mass:
  
  mass = 0.0;
  for (j=0; j<3; j++)
    refCOM[j] = 0.0;
  
  for (i = 0; i < myAtoms.size(); i++) {
    mtmp = myAtoms[i]->getMass();
    mass += mtmp;

    apos = refCoords[i];
    
    for(j = 0; j < 3; j++) {
      refCOM[j] += apos[j]*mtmp;     
    }    
  }
  
  for(j = 0; j < 3; j++) 
    refCOM[j] /= mass;

// Next, move the origin of the reference coordinate system to the COM:

  for (i = 0; i < myAtoms.size(); i++) {
    apos = refCoords[i];
    for (j=0; j < 3; j++) {
      apos[j] = apos[j] - refCOM[j];
    }
    refCoords[i] = apos;
  }

// Moment of Inertia calculation

  for (i = 0; i < 3; i++) 
    for (j = 0; j < 3; j++)
      Itmp[i][j] = 0.0;  
  
  for (it = 0; it < myAtoms.size(); it++) {

    mtmp = myAtoms[it]->getMass();
    ptmp = refCoords[it];
    r= norm3(ptmp.vec);
    r2 = r*r;
    
    for (i = 0; i < 3; i++) {
      for (j = 0; j < 3; j++) {
        
        if (i==j) Itmp[i][j] += mtmp * r2;

        Itmp[i][j] -= mtmp * ptmp.vec[i]*ptmp.vec[j];
      }
    }
  }
  
  diagonalize3x3(Itmp, evals, sU);
  
  // zero out I and then fill the diagonals with the moments of inertia:

  n_linear_coords = 0;

  for (i = 0; i < 3; i++) {
    for (j = 0; j < 3; j++) {
      I[i][j] = 0.0;  
    }
    I[i][i] = evals[i];

    if (fabs(evals[i]) < momIntTol) {
      is_linear = true;
      n_linear_coords++;
      linear_axis = i;
    }
  }

  if (n_linear_coords > 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();
  }
  
  // renormalize column vectors:
  
  for (i=0; i < 3; i++) {
    len = 0.0;
    for (j = 0; j < 3; j++) {
      len += sU[i][j]*sU[i][j];
    }
    len = sqrt(len);
    for (j = 0; j < 3; j++) {
      sU[i][j] /= len;
    }
  }
}

void RigidBody::doEulerToRotMat(vec3 &euler, mat3x3 &myA ){

  double phi, theta, psi;
  
  phi = euler[0];
  theta = euler[1];
  psi = euler[2];
  
  myA[0][0] = (cos(phi) * cos(psi)) - (sin(phi) * cos(theta) * sin(psi));
  myA[0][1] = (sin(phi) * cos(psi)) + (cos(phi) * cos(theta) * sin(psi));
  myA[0][2] = sin(theta) * sin(psi);
  
  myA[1][0] = -(cos(phi) * sin(psi)) - (sin(phi) * cos(theta) * cos(psi));
  myA[1][1] = -(sin(phi) * sin(psi)) + (cos(phi) * cos(theta) * cos(psi));
  myA[1][2] = sin(theta) * cos(psi);
  
  myA[2][0] = sin(phi) * sin(theta);
  myA[2][1] = -cos(phi) * sin(theta);
  myA[2][2] = cos(theta);

}

void RigidBody::calcForcesAndTorques() {

  // Convert Atomic forces and torques to total forces and torques:
  int i, j;
  double apos[3];
  double afrc[3];
  double atrq[3];
  double rpos[3];

  zeroForces();
  
  for (i = 0; i < myAtoms.size(); i++) {

    myAtoms[i]->getPos(apos);
    myAtoms[i]->getFrc(afrc);

    for (j=0; j<3; j++) {
      rpos[j] = apos[j] - pos[j];
      frc[j] += afrc[j];
    }
    
    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 (myAtoms[i]->isDirectional()) {

      myAtoms[i]->getTrq(atrq);
      
      for (j=0; j<3; j++) 
        trq[j] += atrq[j];
    }
  }

  // Convert Torque to Body-fixed coordinates:
  // (Actually, on second thought, don't.  Integrator does this now.)
  // lab2Body(trq);

}

void RigidBody::updateAtoms() {
  int i, j;
  vec3 ref;
  double apos[3];
  DirectionalAtom* dAtom;
  
  for (i = 0; i < myAtoms.size(); i++) {
     
    ref = refCoords[i];

    body2Lab(ref.vec);
    
    for (j = 0; j<3; j++) 
      apos[j] = pos[j] + ref.vec[j];
    
    myAtoms[i]->setPos(apos);
    
    if (myAtoms[i]->isDirectional()) {
      
      dAtom = (DirectionalAtom *) myAtoms[i];
      dAtom->rotateBy( A );
      
    }
  }  
}

void RigidBody::getGrad( double grad[6] ) {

  double myEuler[3];
  double phi, theta, psi;
  double cphi, sphi, ctheta, stheta;
  double ephi[3];
  double etheta[3];
  double epsi[3];
  
  this->getEulerAngles(myEuler);

  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;
  
  for (int j = 0 ; j<3; j++)
    grad[j] = frc[j];

  grad[3] = 0.0;
  grad[4] = 0.0;
  grad[5] = 0.0;
  
  for (int j = 0; j < 3; j++ ) {
    
    grad[3] += trq[j]*ephi[j];
    grad[4] += trq[j]*etheta[j];
    grad[5] += trq[j]*epsi[j];
    
  }
  
}

/**
  * getEulerAngles computes a set of Euler angle values consistent
  * with an input rotation matrix.  They are returned in the following
  * order:
  *  myEuler[0] = phi;
  *  myEuler[1] = theta;
  *  myEuler[2] = psi;
*/
void RigidBody::getEulerAngles(double myEuler[3]) {

  // We use so-called "x-convention", which is the most common
  // definition.  In this convention, the rotation given by Euler
  // angles (phi, theta, psi), where the first rotation is by an angle
  // phi about the z-axis, the second is by an angle theta (0 <= theta
  // <= 180) about the x-axis, and the third is by an angle psi about
  // the z-axis (again).
  
  
  double phi,theta,psi,eps;
  double pi;
  double cphi,ctheta,cpsi;
  double sphi,stheta,spsi;
  double b[3];
  int flip[3];
  
  // set the tolerance for Euler angles and rotation elements
  
  eps = 1.0e-8;

  theta = acos(min(1.0,max(-1.0,A[2][2])));
  ctheta = A[2][2]; 
  stheta = sqrt(1.0 - ctheta * ctheta);

  // when sin(theta) is close to 0, we need to consider the
  // possibility of a singularity. In this case, we can assign an
  // arbitary value to phi (or psi), and then determine the psi (or
  // phi) or vice-versa.  We'll assume that phi always gets the
  // rotation, and psi is 0 in cases of singularity.  we use atan2
  // instead of atan, since atan2 will give us -Pi to Pi.  Since 0 <=
  // theta <= 180, sin(theta) will be always non-negative. Therefore,
  // it never changes the sign of both of the parameters passed to
  // atan2.
  
  if (fabs(stheta) <= eps){
    psi = 0.0;
    phi = atan2(-A[1][0], A[0][0]);  
  }
  // we only have one unique solution
  else{    
    phi = atan2(A[2][0], -A[2][1]);
    psi = atan2(A[0][2], A[1][2]);
  }
  
  //wrap phi and psi, make sure they are in the range from 0 to 2*Pi
  //if (phi < 0)
  //  phi += M_PI;
  
  //if (psi < 0)
  //  psi += M_PI;
  
  myEuler[0] = phi;
  myEuler[1] = theta;
  myEuler[2] = psi;
  
  return;
}

double RigidBody::max(double x, double  y) {  
  return (x > y) ? x : y;
}

double RigidBody::min(double x, double  y) {  
  return (x > y) ? y : x;
}

void RigidBody::findCOM() {
  
  size_t i;
  int j;
  double mtmp;
  double ptmp[3];
  double vtmp[3];
  
  for(j = 0; j < 3; j++) {
    pos[j] = 0.0;
    vel[j] = 0.0;
  }
  mass = 0.0;
  
  for (i = 0; i < myAtoms.size(); i++) {
    
    mtmp = myAtoms[i]->getMass();    
    myAtoms[i]->getPos(ptmp);
    myAtoms[i]->getVel(vtmp);
    
    mass += mtmp;
    
    for(j = 0; j < 3; j++) {
      pos[j] += ptmp[j]*mtmp;
      vel[j] += vtmp[j]*mtmp;
    }
    
  }
  
  for(j = 0; j < 3; j++) {
    pos[j] /= mass;
    vel[j] /= mass;
  }

}

void RigidBody::accept(BaseVisitor* v){
  vector<Atom*>::iterator atomIter;
  v->visit(this);

  //for(atomIter = myAtoms.begin(); atomIter != myAtoms.end(); ++atomIter)
  //  (*atomIter)->accept(v);
}
void RigidBody::getAtomRefCoor(double pos[3], int index){
  vec3 ref;

  ref = refCoords[index];
  pos[0] = ref[0];
  pos[1] = ref[1];
  pos[2] = ref[2];
  
}


void RigidBody::getAtomPos(double theP[3], int index){
  vec3 ref;

  if (index >= myAtoms.size())
    cerr << index << " is an invalid index, current rigid body contains " << myAtoms.size() << "atoms" << endl;

  ref = refCoords[index];
  body2Lab(ref.vec);
  
  theP[0] = pos[0] + ref[0];
  theP[1] = pos[1] + ref[1];
  theP[2] = pos[2] + ref[2];
}


void RigidBody::getAtomVel(double theV[3], int index){
  vec3 ref;
  double velRot[3];
  double skewMat[3][3];
  double aSkewMat[3][3];
  double aSkewTransMat[3][3];
  
  //velRot = $(A\cdot skew(I^{-1}j))^{T}refCoor$

  if (index >= myAtoms.size())
    cerr << index << " is an invalid index, current rigid body contains " << myAtoms.size() << "atoms" << endl;

  ref = refCoords[index];

  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;
  
  matMul3(A, skewMat, aSkewMat);

  transposeMat3(aSkewMat, aSkewTransMat);

  matVecMul3(aSkewTransMat, ref.vec, velRot);
  theV[0] = vel[0] + velRot[0];
  theV[1] = vel[1] + velRot[1];
  theV[2] = vel[2] + velRot[2];
}


Revision:	1452
Committed:	Mon Aug 23 15:11:36 2004 UTC (19 years, 10 months ago) by tim
File size:	16123 byte(s)
Log Message:	* empty log message *
#	User	Rev	Content
1	gezelter	1100	#include <math.h>
2			#include "RigidBody.hpp"
3			#include "DirectionalAtom.hpp"
4			#include "simError.h"
5			#include "MatVec3.h"
6
7			RigidBody::RigidBody() : StuntDouble() {
8			objType = OT_RIGIDBODY;
9	tim	1118	is_linear = false;
10			linear_axis = -1;
11	gezelter	1174	momIntTol = 1e-6;
12	gezelter	1100	}
13
14			RigidBody::~RigidBody() {
15			}
16
17			void RigidBody::addAtom(Atom* at, AtomStamp* ats) {
18
19			vec3 coords;
20			vec3 euler;
21			mat3x3 Atmp;
22
23			myAtoms.push_back(at);
24
25			if( !ats->havePosition() ){
26			sprintf( painCave.errMsg,
27			"RigidBody error.\n"
28			"\tAtom %s does not have a position specified.\n"
29			"\tThis means RigidBody cannot set up reference coordinates.\n",
30			ats->getType() );
31			painCave.isFatal = 1;
32			simError();
33			}
34
35			coords[0] = ats->getPosX();
36			coords[1] = ats->getPosY();
37			coords[2] = ats->getPosZ();
38
39			refCoords.push_back(coords);
40
41			if (at->isDirectional()) {
42
43			if( !ats->haveOrientation() ){
44			sprintf( painCave.errMsg,
45			"RigidBody error.\n"
46			"\tAtom %s does not have an orientation specified.\n"
47			"\tThis means RigidBody cannot set up reference orientations.\n",
48			ats->getType() );
49			painCave.isFatal = 1;
50			simError();
51			}
52
53			euler[0] = ats->getEulerPhi();
54			euler[1] = ats->getEulerTheta();
55			euler[2] = ats->getEulerPsi();
56
57			doEulerToRotMat(euler, Atmp);
58
59			refOrients.push_back(Atmp);
60
61			}
62			}
63
64			void RigidBody::getPos(double theP[3]){
65			for (int i = 0; i < 3 ; i++)
66			theP[i] = pos[i];
67			}
68
69			void RigidBody::setPos(double theP[3]){
70			for (int i = 0; i < 3 ; i++)
71			pos[i] = theP[i];
72			}
73
74			void RigidBody::getVel(double theV[3]){
75			for (int i = 0; i < 3 ; i++)
76			theV[i] = vel[i];
77			}
78
79			void RigidBody::setVel(double theV[3]){
80			for (int i = 0; i < 3 ; i++)
81			vel[i] = theV[i];
82			}
83
84			void RigidBody::getFrc(double theF[3]){
85			for (int i = 0; i < 3 ; i++)
86			theF[i] = frc[i];
87			}
88
89	tim	1452	void RigidBody::setFrc(double theF[3]){
90			for (int i = 0; i < 3 ; i++)
91			frc[i] = theF[i];
92			}
93
94	gezelter	1100	void RigidBody::addFrc(double theF[3]){
95			for (int i = 0; i < 3 ; i++)
96			frc[i] += theF[i];
97			}
98
99			void RigidBody::zeroForces() {
100
101			for (int i = 0; i < 3; i++) {
102			frc[i] = 0.0;
103			trq[i] = 0.0;
104			}
105
106			}
107
108	tim	1113	void RigidBody::setEuler( double phi, double theta, double psi ){
109	gezelter	1100
110			A[0][0] = (cos(phi) * cos(psi)) - (sin(phi) * cos(theta) * sin(psi));
111			A[0][1] = (sin(phi) * cos(psi)) + (cos(phi) * cos(theta) * sin(psi));
112			A[0][2] = sin(theta) * sin(psi);
113
114			A[1][0] = -(cos(phi) * sin(psi)) - (sin(phi) * cos(theta) * cos(psi));
115			A[1][1] = -(sin(phi) * sin(psi)) + (cos(phi) * cos(theta) * cos(psi));
116			A[1][2] = sin(theta) * cos(psi);
117
118			A[2][0] = sin(phi) * sin(theta);
119			A[2][1] = -cos(phi) * sin(theta);
120			A[2][2] = cos(theta);
121
122			}
123
124			void RigidBody::getQ( double q[4] ){
125
126			double t, s;
127			double ad1, ad2, ad3;
128
129			t = A[0][0] + A[1][1] + A[2][2] + 1.0;
130			if( t > 0.0 ){
131
132			s = 0.5 / sqrt( t );
133			q[0] = 0.25 / s;
134			q[1] = (A[1][2] - A[2][1]) * s;
135			q[2] = (A[2][0] - A[0][2]) * s;
136			q[3] = (A[0][1] - A[1][0]) * s;
137			}
138			else{
139
140			ad1 = fabs( A[0][0] );
141			ad2 = fabs( A[1][1] );
142			ad3 = fabs( A[2][2] );
143
144			if( ad1 >= ad2 && ad1 >= ad3 ){
145
146			s = 2.0 * sqrt( 1.0 + A[0][0] - A[1][1] - A[2][2] );
147			q[0] = (A[1][2] + A[2][1]) / s;
148			q[1] = 0.5 / s;
149			q[2] = (A[0][1] + A[1][0]) / s;
150			q[3] = (A[0][2] + A[2][0]) / s;
151			}
152			else if( ad2 >= ad1 && ad2 >= ad3 ){
153
154			s = sqrt( 1.0 + A[1][1] - A[0][0] - A[2][2] ) * 2.0;
155			q[0] = (A[0][2] + A[2][0]) / s;
156			q[1] = (A[0][1] + A[1][0]) / s;
157			q[2] = 0.5 / s;
158			q[3] = (A[1][2] + A[2][1]) / s;
159			}
160			else{
161
162			s = sqrt( 1.0 + A[2][2] - A[0][0] - A[1][1] ) * 2.0;
163			q[0] = (A[0][1] + A[1][0]) / s;
164			q[1] = (A[0][2] + A[2][0]) / s;
165			q[2] = (A[1][2] + A[2][1]) / s;
166			q[3] = 0.5 / s;
167			}
168			}
169			}
170
171			void RigidBody::setQ( double the_q[4] ){
172
173			double q0Sqr, q1Sqr, q2Sqr, q3Sqr;
174
175			q0Sqr = the_q[0] * the_q[0];
176			q1Sqr = the_q[1] * the_q[1];
177			q2Sqr = the_q[2] * the_q[2];
178			q3Sqr = the_q[3] * the_q[3];
179
180			A[0][0] = q0Sqr + q1Sqr - q2Sqr - q3Sqr;
181			A[0][1] = 2.0 * ( the_q[1] * the_q[2] + the_q[0] * the_q[3] );
182			A[0][2] = 2.0 * ( the_q[1] * the_q[3] - the_q[0] * the_q[2] );
183
184			A[1][0] = 2.0 * ( the_q[1] * the_q[2] - the_q[0] * the_q[3] );
185			A[1][1] = q0Sqr - q1Sqr + q2Sqr - q3Sqr;
186			A[1][2] = 2.0 * ( the_q[2] * the_q[3] + the_q[0] * the_q[1] );
187
188			A[2][0] = 2.0 * ( the_q[1] * the_q[3] + the_q[0] * the_q[2] );
189			A[2][1] = 2.0 * ( the_q[2] * the_q[3] - the_q[0] * the_q[1] );
190			A[2][2] = q0Sqr - q1Sqr -q2Sqr +q3Sqr;
191			}
192
193			void RigidBody::getA( double the_A[3][3] ){
194
195			for (int i = 0; i < 3; i++)
196			for (int j = 0; j < 3; j++)
197	tim	1113	the_A[i][j] = A[i][j];
198	gezelter	1100
199			}
200
201			void RigidBody::setA( double the_A[3][3] ){
202
203			for (int i = 0; i < 3; i++)
204			for (int j = 0; j < 3; j++)
205			A[i][j] = the_A[i][j];
206
207			}
208
209			void RigidBody::getJ( double theJ[3] ){
210
211			for (int i = 0; i < 3; i++)
212			theJ[i] = ji[i];
213
214			}
215
216			void RigidBody::setJ( double theJ[3] ){
217
218			for (int i = 0; i < 3; i++)
219			ji[i] = theJ[i];
220
221			}
222
223			void RigidBody::getTrq(double theT[3]){
224			for (int i = 0; i < 3 ; i++)
225			theT[i] = trq[i];
226			}
227
228	tim	1452	void RigidBody::setTrq(double theT[3]){
229			for (int i = 0; i < 3 ; i++)
230			trq[i] = theT[i];
231			}
232
233	gezelter	1100	void RigidBody::addTrq(double theT[3]){
234			for (int i = 0; i < 3 ; i++)
235			trq[i] += theT[i];
236			}
237
238			void RigidBody::getI( double the_I[3][3] ){
239
240			for (int i = 0; i < 3; i++)
241			for (int j = 0; j < 3; j++)
242			the_I[i][j] = I[i][j];
243
244			}
245
246			void RigidBody::lab2Body( double r[3] ){
247
248			double rl[3]; // the lab frame vector
249
250			rl[0] = r[0];
251			rl[1] = r[1];
252			rl[2] = r[2];
253
254			r[0] = (A[0][0] * rl[0]) + (A[0][1] * rl[1]) + (A[0][2] * rl[2]);
255			r[1] = (A[1][0] * rl[0]) + (A[1][1] * rl[1]) + (A[1][2] * rl[2]);
256			r[2] = (A[2][0] * rl[0]) + (A[2][1] * rl[1]) + (A[2][2] * rl[2]);
257
258			}
259
260			void RigidBody::body2Lab( double r[3] ){
261
262			double rb[3]; // the body frame vector
263
264			rb[0] = r[0];
265			rb[1] = r[1];
266			rb[2] = r[2];
267
268			r[0] = (A[0][0] * rb[0]) + (A[1][0] * rb[1]) + (A[2][0] * rb[2]);
269			r[1] = (A[0][1] * rb[0]) + (A[1][1] * rb[1]) + (A[2][1] * rb[2]);
270			r[2] = (A[0][2] * rb[0]) + (A[1][2] * rb[1]) + (A[2][2] * rb[2]);
271
272			}
273
274	chrisfen	1187	double RigidBody::getZangle( ){
275			return zAngle;
276			}
277
278			void RigidBody::setZangle( double zAng ){
279			zAngle = zAng;
280			}
281
282			void RigidBody::addZangle( double zAng ){
283			zAngle += zAng;
284			}
285
286	gezelter	1100	void RigidBody::calcRefCoords( ) {
287
288	tim	1118	int i,j,k, it, n_linear_coords;
289	gezelter	1100	double mtmp;
290			vec3 apos;
291			double refCOM[3];
292	tim	1113	vec3 ptmp;
293			double Itmp[3][3];
294			double evals[3];
295			double evects[3][3];
296			double r, r2, len;
297	gezelter	1100
298	tim	1113	// First, find the center of mass:
299
300	gezelter	1100	mass = 0.0;
301			for (j=0; j<3; j++)
302			refCOM[j] = 0.0;
303
304			for (i = 0; i < myAtoms.size(); i++) {
305			mtmp = myAtoms[i]->getMass();
306			mass += mtmp;
307
308			apos = refCoords[i];
309
310			for(j = 0; j < 3; j++) {
311			refCOM[j] += apos[j]*mtmp;
312			}
313			}
314
315			for(j = 0; j < 3; j++)
316			refCOM[j] /= mass;
317
318	tim	1113	// Next, move the origin of the reference coordinate system to the COM:
319
320	gezelter	1100	for (i = 0; i < myAtoms.size(); i++) {
321			apos = refCoords[i];
322			for (j=0; j < 3; j++) {
323			apos[j] = apos[j] - refCOM[j];
324			}
325			refCoords[i] = apos;
326			}
327
328	tim	1113	// Moment of Inertia calculation
329
330			for (i = 0; i < 3; i++)
331			for (j = 0; j < 3; j++)
332			Itmp[i][j] = 0.0;
333
334			for (it = 0; it < myAtoms.size(); it++) {
335
336			mtmp = myAtoms[it]->getMass();
337			ptmp = refCoords[it];
338			r= norm3(ptmp.vec);
339			r2 = r*r;
340
341			for (i = 0; i < 3; i++) {
342			for (j = 0; j < 3; j++) {
343
344			if (i==j) Itmp[i][j] += mtmp * r2;
345
346			Itmp[i][j] -= mtmp * ptmp.vec[i]*ptmp.vec[j];
347			}
348			}
349			}
350
351			diagonalize3x3(Itmp, evals, sU);
352
353			// zero out I and then fill the diagonals with the moments of inertia:
354
355	tim	1118	n_linear_coords = 0;
356
357	tim	1113	for (i = 0; i < 3; i++) {
358			for (j = 0; j < 3; j++) {
359			I[i][j] = 0.0;
360			}
361			I[i][i] = evals[i];
362	tim	1118
363			if (fabs(evals[i]) < momIntTol) {
364			is_linear = true;
365			n_linear_coords++;
366			linear_axis = i;
367			}
368	tim	1113	}
369	tim	1118
370			if (n_linear_coords > 1) {
371			sprintf( painCave.errMsg,
372			"RigidBody error.\n"
373			"\tOOPSE found more than one axis in this rigid body with a vanishing \n"
374			"\tmoment of inertia. This can happen in one of three ways:\n"
375			"\t 1) Only one atom was specified, or \n"
376			"\t 2) All atoms were specified at the same location, or\n"
377			"\t 3) The programmers did something stupid.\n"
378			"\tIt is silly to use a rigid body to describe this situation. Be smarter.\n"
379			);
380			painCave.isFatal = 1;
381			simError();
382			}
383	tim	1113
384			// renormalize column vectors:
385
386			for (i=0; i < 3; i++) {
387			len = 0.0;
388			for (j = 0; j < 3; j++) {
389			len += sU[i][j]*sU[i][j];
390			}
391			len = sqrt(len);
392			for (j = 0; j < 3; j++) {
393			sU[i][j] /= len;
394			}
395			}
396	gezelter	1100	}
397
398			void RigidBody::doEulerToRotMat(vec3 &euler, mat3x3 &myA ){
399
400			double phi, theta, psi;
401
402			phi = euler[0];
403			theta = euler[1];
404			psi = euler[2];
405
406			myA[0][0] = (cos(phi) * cos(psi)) - (sin(phi) * cos(theta) * sin(psi));
407			myA[0][1] = (sin(phi) * cos(psi)) + (cos(phi) * cos(theta) * sin(psi));
408			myA[0][2] = sin(theta) * sin(psi);
409
410			myA[1][0] = -(cos(phi) * sin(psi)) - (sin(phi) * cos(theta) * cos(psi));
411			myA[1][1] = -(sin(phi) * sin(psi)) + (cos(phi) * cos(theta) * cos(psi));
412			myA[1][2] = sin(theta) * cos(psi);
413
414			myA[2][0] = sin(phi) * sin(theta);
415			myA[2][1] = -cos(phi) * sin(theta);
416			myA[2][2] = cos(theta);
417
418			}
419
420			void RigidBody::calcForcesAndTorques() {
421
422			// Convert Atomic forces and torques to total forces and torques:
423			int i, j;
424			double apos[3];
425			double afrc[3];
426			double atrq[3];
427			double rpos[3];
428
429			zeroForces();
430
431			for (i = 0; i < myAtoms.size(); i++) {
432
433			myAtoms[i]->getPos(apos);
434			myAtoms[i]->getFrc(afrc);
435
436			for (j=0; j<3; j++) {
437			rpos[j] = apos[j] - pos[j];
438			frc[j] += afrc[j];
439			}
440
441			trq[0] += rpos[1]afrc[2] - rpos[2]afrc[1];
442			trq[1] += rpos[2]afrc[0] - rpos[0]afrc[2];
443			trq[2] += rpos[0]afrc[1] - rpos[1]afrc[0];
444
445			// If the atom has a torque associated with it, then we also need to
446			// migrate the torques onto the center of mass:
447
448			if (myAtoms[i]->isDirectional()) {
449
450			myAtoms[i]->getTrq(atrq);
451
452			for (j=0; j<3; j++)
453			trq[j] += atrq[j];
454			}
455			}
456
457			// Convert Torque to Body-fixed coordinates:
458			// (Actually, on second thought, don't. Integrator does this now.)
459			// lab2Body(trq);
460
461			}
462
463			void RigidBody::updateAtoms() {
464			int i, j;
465			vec3 ref;
466			double apos[3];
467			DirectionalAtom* dAtom;
468
469			for (i = 0; i < myAtoms.size(); i++) {
470
471			ref = refCoords[i];
472
473			body2Lab(ref.vec);
474
475			for (j = 0; j<3; j++)
476			apos[j] = pos[j] + ref.vec[j];
477
478			myAtoms[i]->setPos(apos);
479
480			if (myAtoms[i]->isDirectional()) {
481
482			dAtom = (DirectionalAtom *) myAtoms[i];
483			dAtom->rotateBy( A );
484
485			}
486			}
487			}
488
489			void RigidBody::getGrad( double grad[6] ) {
490
491			double myEuler[3];
492			double phi, theta, psi;
493			double cphi, sphi, ctheta, stheta;
494			double ephi[3];
495			double etheta[3];
496			double epsi[3];
497
498			this->getEulerAngles(myEuler);
499
500			phi = myEuler[0];
501			theta = myEuler[1];
502			psi = myEuler[2];
503
504			cphi = cos(phi);
505			sphi = sin(phi);
506			ctheta = cos(theta);
507			stheta = sin(theta);
508
509			// get unit vectors along the phi, theta and psi rotation axes
510
511			ephi[0] = 0.0;
512			ephi[1] = 0.0;
513			ephi[2] = 1.0;
514
515			etheta[0] = cphi;
516			etheta[1] = sphi;
517			etheta[2] = 0.0;
518
519			epsi[0] = stheta * cphi;
520			epsi[1] = stheta * sphi;
521			epsi[2] = ctheta;
522
523			for (int j = 0 ; j<3; j++)
524			grad[j] = frc[j];
525
526			grad[3] = 0.0;
527			grad[4] = 0.0;
528			grad[5] = 0.0;
529
530			for (int j = 0; j < 3; j++ ) {
531
532			grad[3] += trq[j]*ephi[j];
533			grad[4] += trq[j]*etheta[j];
534			grad[5] += trq[j]*epsi[j];
535
536			}
537
538			}
539
540			/**
541			* getEulerAngles computes a set of Euler angle values consistent
542			* with an input rotation matrix. They are returned in the following
543			* order:
544			* myEuler[0] = phi;
545			* myEuler[1] = theta;
546			* myEuler[2] = psi;
547			*/
548			void RigidBody::getEulerAngles(double myEuler[3]) {
549
550			// We use so-called "x-convention", which is the most common
551			// definition. In this convention, the rotation given by Euler
552			// angles (phi, theta, psi), where the first rotation is by an angle
553			// phi about the z-axis, the second is by an angle theta (0 <= theta
554			// <= 180) about the x-axis, and the third is by an angle psi about
555			// the z-axis (again).
556
557
558			double phi,theta,psi,eps;
559			double pi;
560			double cphi,ctheta,cpsi;
561			double sphi,stheta,spsi;
562			double b[3];
563			int flip[3];
564
565			// set the tolerance for Euler angles and rotation elements
566
567			eps = 1.0e-8;
568
569			theta = acos(min(1.0,max(-1.0,A[2][2])));
570			ctheta = A[2][2];
571			stheta = sqrt(1.0 - ctheta * ctheta);
572
573			// when sin(theta) is close to 0, we need to consider the
574			// possibility of a singularity. In this case, we can assign an
575			// arbitary value to phi (or psi), and then determine the psi (or
576			// phi) or vice-versa. We'll assume that phi always gets the
577			// rotation, and psi is 0 in cases of singularity. we use atan2
578			// instead of atan, since atan2 will give us -Pi to Pi. Since 0 <=
579			// theta <= 180, sin(theta) will be always non-negative. Therefore,
580			// it never changes the sign of both of the parameters passed to
581			// atan2.
582
583			if (fabs(stheta) <= eps){
584			psi = 0.0;
585			phi = atan2(-A[1][0], A[0][0]);
586			}
587			// we only have one unique solution
588			else{
589			phi = atan2(A[2][0], -A[2][1]);
590			psi = atan2(A[0][2], A[1][2]);
591			}
592
593			//wrap phi and psi, make sure they are in the range from 0 to 2*Pi
594			//if (phi < 0)
595			// phi += M_PI;
596
597			//if (psi < 0)
598			// psi += M_PI;
599
600			myEuler[0] = phi;
601			myEuler[1] = theta;
602			myEuler[2] = psi;
603
604			return;
605			}
606
607			double RigidBody::max(double x, double y) {
608			return (x > y) ? x : y;
609			}
610
611			double RigidBody::min(double x, double y) {
612			return (x > y) ? y : x;
613			}
614
615			void RigidBody::findCOM() {
616
617			size_t i;
618			int j;
619			double mtmp;
620			double ptmp[3];
621			double vtmp[3];
622
623			for(j = 0; j < 3; j++) {
624			pos[j] = 0.0;
625			vel[j] = 0.0;
626			}
627			mass = 0.0;
628
629			for (i = 0; i < myAtoms.size(); i++) {
630
631			mtmp = myAtoms[i]->getMass();
632			myAtoms[i]->getPos(ptmp);
633			myAtoms[i]->getVel(vtmp);
634
635			mass += mtmp;
636
637			for(j = 0; j < 3; j++) {
638			pos[j] += ptmp[j]*mtmp;
639			vel[j] += vtmp[j]*mtmp;
640			}
641
642			}
643
644			for(j = 0; j < 3; j++) {
645			pos[j] /= mass;
646			vel[j] /= mass;
647			}
648
649			}
650	tim	1118
651			void RigidBody::accept(BaseVisitor* v){
652			vector<Atom*>::iterator atomIter;
653			v->visit(this);
654
655	tim	1120	//for(atomIter = myAtoms.begin(); atomIter != myAtoms.end(); ++atomIter)
656			// (*atomIter)->accept(v);
657	gezelter	1174	}
658	tim	1284	void RigidBody::getAtomRefCoor(double pos[3], int index){
659			vec3 ref;
660	tim	1234
661	tim	1284	ref = refCoords[index];
662			pos[0] = ref[0];
663			pos[1] = ref[1];
664			pos[2] = ref[2];
665
666			}
667
668
669	tim	1234	void RigidBody::getAtomPos(double theP[3], int index){
670			vec3 ref;
671
672			if (index >= myAtoms.size())
673			cerr << index << " is an invalid index, current rigid body contains " << myAtoms.size() << "atoms" << endl;
674
675			ref = refCoords[index];
676			body2Lab(ref.vec);
677
678			theP[0] = pos[0] + ref[0];
679			theP[1] = pos[1] + ref[1];
680			theP[2] = pos[2] + ref[2];
681			}
682
683	tim	1254
684	tim	1284	void RigidBody::getAtomVel(double theV[3], int index){
685	tim	1254	vec3 ref;
686	tim	1284	double velRot[3];
687			double skewMat[3][3];
688			double aSkewMat[3][3];
689			double aSkewTransMat[3][3];
690
691			//velRot = $(A\cdot skew(I^{-1}j))^{T}refCoor$
692	tim	1254
693	tim	1284	if (index >= myAtoms.size())
694			cerr << index << " is an invalid index, current rigid body contains " << myAtoms.size() << "atoms" << endl;
695
696	tim	1254	ref = refCoords[index];
697	tim	1284
698			skewMat[0][0] =0;
699			skewMat[0][1] = ji[2] /I[2][2];
700			skewMat[0][2] = -ji[1] /I[1][1];
701
702			skewMat[1][0] = -ji[2] /I[2][2];
703			skewMat[1][1] = 0;
704			skewMat[1][2] = ji[0]/I[0][0];
705
706			skewMat[2][0] =ji[1] /I[1][1];
707			skewMat[2][1] = -ji[0]/I[0][0];
708			skewMat[2][2] = 0;
709	tim	1254
710	tim	1284	matMul3(A, skewMat, aSkewMat);
711
712			transposeMat3(aSkewMat, aSkewTransMat);
713
714			matVecMul3(aSkewTransMat, ref.vec, velRot);
715			theV[0] = vel[0] + velRot[0];
716			theV[1] = vel[1] + velRot[1];
717			theV[2] = vel[2] + velRot[2];
718	tim	1254	}
719	tim	1284
720