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;
}

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::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::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]);

}

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