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;
  com_good = false;
  precalc_done = false;
}

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

  forces_good = false;

}

void RigidBody::setEulerAngles( 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] = the_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] ){  

  if (precalc_done) {

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

  } else {

  }
}

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;
  double mtmp;
  vec3 apos;
  double refCOM[3];

  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;

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

}

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

  forces_good = true;

}

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

  com_good = true;
}
  
void RigidBody::findOrient() {
  
  size_t it;
  int i, j;
  double ptmp[3];
  double Itmp[3][3];
  double evals[3];
  double evects[3][3];
  double r2, mtmp, len;

  if (!com_good) findCOM();

  // Calculate inertial tensor matrix elements:

  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();    
    myAtoms[it]->getPos(ptmp);

    for (j = 0; j < 3; j++) 
      ptmp[j] = pos[j] - ptmp[j];

    r2 = norm3(ptmp);
    
    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[i]*ptmp[j];
      }
    }
  }
  
  diagonalize3x3(Itmp, evals, sU);
  
  // zero out I and then fill the diagonals with the moments of inertia:

  for (i = 0; i < 3; i++) {
    for (j = 0; j < 3; j++) {
      I[i][j] = 0.0;  
    }
    I[i][i] = evals[i];
  }
  
  // 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;
    }
  }
  
  // sU now contains the coordinates of the 'special' frame;
  
  orient_good = true;

}
Revision:	1100
Committed:	Mon Apr 12 21:02:01 2004 UTC (20 years, 2 months ago) by gezelter
File size:	13598 byte(s)
Log Message:	Adding RigidBody code
#	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			com_good = false;
10			precalc_done = false;
11			}
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			forces_good = false;
101
102			}
103
104			void RigidBody::setEulerAngles( double phi, double theta, double psi ){
105
106			A[0][0] = (cos(phi) * cos(psi)) - (sin(phi) * cos(theta) * sin(psi));
107			A[0][1] = (sin(phi) * cos(psi)) + (cos(phi) * cos(theta) * sin(psi));
108			A[0][2] = sin(theta) * sin(psi);
109
110			A[1][0] = -(cos(phi) * sin(psi)) - (sin(phi) * cos(theta) * cos(psi));
111			A[1][1] = -(sin(phi) * sin(psi)) + (cos(phi) * cos(theta) * cos(psi));
112			A[1][2] = sin(theta) * cos(psi);
113
114			A[2][0] = sin(phi) * sin(theta);
115			A[2][1] = -cos(phi) * sin(theta);
116			A[2][2] = cos(theta);
117
118			}
119
120			void RigidBody::getQ( double q[4] ){
121
122			double t, s;
123			double ad1, ad2, ad3;
124
125			t = A[0][0] + A[1][1] + A[2][2] + 1.0;
126			if( t > 0.0 ){
127
128			s = 0.5 / sqrt( t );
129			q[0] = 0.25 / s;
130			q[1] = (A[1][2] - A[2][1]) * s;
131			q[2] = (A[2][0] - A[0][2]) * s;
132			q[3] = (A[0][1] - A[1][0]) * s;
133			}
134			else{
135
136			ad1 = fabs( A[0][0] );
137			ad2 = fabs( A[1][1] );
138			ad3 = fabs( A[2][2] );
139
140			if( ad1 >= ad2 && ad1 >= ad3 ){
141
142			s = 2.0 * sqrt( 1.0 + A[0][0] - A[1][1] - A[2][2] );
143			q[0] = (A[1][2] + A[2][1]) / s;
144			q[1] = 0.5 / s;
145			q[2] = (A[0][1] + A[1][0]) / s;
146			q[3] = (A[0][2] + A[2][0]) / s;
147			}
148			else if( ad2 >= ad1 && ad2 >= ad3 ){
149
150			s = sqrt( 1.0 + A[1][1] - A[0][0] - A[2][2] ) * 2.0;
151			q[0] = (A[0][2] + A[2][0]) / s;
152			q[1] = (A[0][1] + A[1][0]) / s;
153			q[2] = 0.5 / s;
154			q[3] = (A[1][2] + A[2][1]) / s;
155			}
156			else{
157
158			s = sqrt( 1.0 + A[2][2] - A[0][0] - A[1][1] ) * 2.0;
159			q[0] = (A[0][1] + A[1][0]) / s;
160			q[1] = (A[0][2] + A[2][0]) / s;
161			q[2] = (A[1][2] + A[2][1]) / s;
162			q[3] = 0.5 / s;
163			}
164			}
165			}
166
167			void RigidBody::setQ( double the_q[4] ){
168
169			double q0Sqr, q1Sqr, q2Sqr, q3Sqr;
170
171			q0Sqr = the_q[0] * the_q[0];
172			q1Sqr = the_q[1] * the_q[1];
173			q2Sqr = the_q[2] * the_q[2];
174			q3Sqr = the_q[3] * the_q[3];
175
176			A[0][0] = q0Sqr + q1Sqr - q2Sqr - q3Sqr;
177			A[0][1] = 2.0 * ( the_q[1] * the_q[2] + the_q[0] * the_q[3] );
178			A[0][2] = 2.0 * ( the_q[1] * the_q[3] - the_q[0] * the_q[2] );
179
180			A[1][0] = 2.0 * ( the_q[1] * the_q[2] - the_q[0] * the_q[3] );
181			A[1][1] = q0Sqr - q1Sqr + q2Sqr - q3Sqr;
182			A[1][2] = 2.0 * ( the_q[2] * the_q[3] + the_q[0] * the_q[1] );
183
184			A[2][0] = 2.0 * ( the_q[1] * the_q[3] + the_q[0] * the_q[2] );
185			A[2][1] = 2.0 * ( the_q[2] * the_q[3] - the_q[0] * the_q[1] );
186			A[2][2] = q0Sqr - q1Sqr -q2Sqr +q3Sqr;
187
188			}
189
190			void RigidBody::getA( double the_A[3][3] ){
191
192			for (int i = 0; i < 3; i++)
193			for (int j = 0; j < 3; j++)
194			the_A[i][j] = the_A[i][j];
195
196			}
197
198			void RigidBody::setA( double the_A[3][3] ){
199
200			for (int i = 0; i < 3; i++)
201			for (int j = 0; j < 3; j++)
202			A[i][j] = the_A[i][j];
203
204			}
205
206			void RigidBody::getJ( double theJ[3] ){
207
208			for (int i = 0; i < 3; i++)
209			theJ[i] = ji[i];
210
211			}
212
213			void RigidBody::setJ( double theJ[3] ){
214
215			for (int i = 0; i < 3; i++)
216			ji[i] = theJ[i];
217
218			}
219
220			void RigidBody::getTrq(double theT[3]){
221			for (int i = 0; i < 3 ; i++)
222			theT[i] = trq[i];
223			}
224
225			void RigidBody::addTrq(double theT[3]){
226			for (int i = 0; i < 3 ; i++)
227			trq[i] += theT[i];
228			}
229
230			void RigidBody::getI( double the_I[3][3] ){
231
232			if (precalc_done) {
233
234			for (int i = 0; i < 3; i++)
235			for (int j = 0; j < 3; j++)
236			the_I[i][j] = I[i][j];
237
238			} else {
239
240			}
241			}
242
243			void RigidBody::lab2Body( double r[3] ){
244
245			double rl[3]; // the lab frame vector
246
247			rl[0] = r[0];
248			rl[1] = r[1];
249			rl[2] = r[2];
250
251			r[0] = (A[0][0] * rl[0]) + (A[0][1] * rl[1]) + (A[0][2] * rl[2]);
252			r[1] = (A[1][0] * rl[0]) + (A[1][1] * rl[1]) + (A[1][2] * rl[2]);
253			r[2] = (A[2][0] * rl[0]) + (A[2][1] * rl[1]) + (A[2][2] * rl[2]);
254
255			}
256
257			void RigidBody::body2Lab( double r[3] ){
258
259			double rb[3]; // the body frame vector
260
261			rb[0] = r[0];
262			rb[1] = r[1];
263			rb[2] = r[2];
264
265			r[0] = (A[0][0] * rb[0]) + (A[1][0] * rb[1]) + (A[2][0] * rb[2]);
266			r[1] = (A[0][1] * rb[0]) + (A[1][1] * rb[1]) + (A[2][1] * rb[2]);
267			r[2] = (A[0][2] * rb[0]) + (A[1][2] * rb[1]) + (A[2][2] * rb[2]);
268
269			}
270
271			void RigidBody::calcRefCoords( ) {
272
273			int i,j,k;
274			double mtmp;
275			vec3 apos;
276			double refCOM[3];
277
278			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			for (i = 0; i < myAtoms.size(); i++) {
297			apos = refCoords[i];
298			for (j=0; j < 3; j++) {
299			apos[j] = apos[j] - refCOM[j];
300			}
301			refCoords[i] = apos;
302			}
303
304			}
305
306			void RigidBody::doEulerToRotMat(vec3 &euler, mat3x3 &myA ){
307
308			double phi, theta, psi;
309
310			phi = euler[0];
311			theta = euler[1];
312			psi = euler[2];
313
314			myA[0][0] = (cos(phi) * cos(psi)) - (sin(phi) * cos(theta) * sin(psi));
315			myA[0][1] = (sin(phi) * cos(psi)) + (cos(phi) * cos(theta) * sin(psi));
316			myA[0][2] = sin(theta) * sin(psi);
317
318			myA[1][0] = -(cos(phi) * sin(psi)) - (sin(phi) * cos(theta) * cos(psi));
319			myA[1][1] = -(sin(phi) * sin(psi)) + (cos(phi) * cos(theta) * cos(psi));
320			myA[1][2] = sin(theta) * cos(psi);
321
322			myA[2][0] = sin(phi) * sin(theta);
323			myA[2][1] = -cos(phi) * sin(theta);
324			myA[2][2] = cos(theta);
325
326			}
327
328			void RigidBody::calcForcesAndTorques() {
329
330			// Convert Atomic forces and torques to total forces and torques:
331			int i, j;
332			double apos[3];
333			double afrc[3];
334			double atrq[3];
335			double rpos[3];
336
337			zeroForces();
338
339			for (i = 0; i < myAtoms.size(); i++) {
340
341			myAtoms[i]->getPos(apos);
342			myAtoms[i]->getFrc(afrc);
343
344			for (j=0; j<3; j++) {
345			rpos[j] = apos[j] - pos[j];
346			frc[j] += afrc[j];
347			}
348
349			trq[0] += rpos[1]afrc[2] - rpos[2]afrc[1];
350			trq[1] += rpos[2]afrc[0] - rpos[0]afrc[2];
351			trq[2] += rpos[0]afrc[1] - rpos[1]afrc[0];
352
353			// If the atom has a torque associated with it, then we also need to
354			// migrate the torques onto the center of mass:
355
356			if (myAtoms[i]->isDirectional()) {
357
358			myAtoms[i]->getTrq(atrq);
359
360			for (j=0; j<3; j++)
361			trq[j] += atrq[j];
362			}
363			}
364
365			// Convert Torque to Body-fixed coordinates:
366			// (Actually, on second thought, don't. Integrator does this now.)
367			// lab2Body(trq);
368
369			forces_good = true;
370
371			}
372
373			void RigidBody::updateAtoms() {
374			int i, j;
375			vec3 ref;
376			double apos[3];
377			DirectionalAtom* dAtom;
378
379			for (i = 0; i < myAtoms.size(); i++) {
380
381			ref = refCoords[i];
382
383			body2Lab(ref.vec);
384
385			for (j = 0; j<3; j++)
386			apos[j] = pos[j] + ref.vec[j];
387
388			myAtoms[i]->setPos(apos);
389
390			if (myAtoms[i]->isDirectional()) {
391
392			dAtom = (DirectionalAtom *) myAtoms[i];
393			dAtom->rotateBy( A );
394
395			}
396			}
397			}
398
399			void RigidBody::getGrad( double grad[6] ) {
400
401			double myEuler[3];
402			double phi, theta, psi;
403			double cphi, sphi, ctheta, stheta;
404			double ephi[3];
405			double etheta[3];
406			double epsi[3];
407
408			this->getEulerAngles(myEuler);
409
410			phi = myEuler[0];
411			theta = myEuler[1];
412			psi = myEuler[2];
413
414			cphi = cos(phi);
415			sphi = sin(phi);
416			ctheta = cos(theta);
417			stheta = sin(theta);
418
419			// get unit vectors along the phi, theta and psi rotation axes
420
421			ephi[0] = 0.0;
422			ephi[1] = 0.0;
423			ephi[2] = 1.0;
424
425			etheta[0] = cphi;
426			etheta[1] = sphi;
427			etheta[2] = 0.0;
428
429			epsi[0] = stheta * cphi;
430			epsi[1] = stheta * sphi;
431			epsi[2] = ctheta;
432
433			for (int j = 0 ; j<3; j++)
434			grad[j] = frc[j];
435
436			grad[3] = 0.0;
437			grad[4] = 0.0;
438			grad[5] = 0.0;
439
440			for (int j = 0; j < 3; j++ ) {
441
442			grad[3] += trq[j]*ephi[j];
443			grad[4] += trq[j]*etheta[j];
444			grad[5] += trq[j]*epsi[j];
445
446			}
447
448			}
449
450			/**
451			* getEulerAngles computes a set of Euler angle values consistent
452			* with an input rotation matrix. They are returned in the following
453			* order:
454			* myEuler[0] = phi;
455			* myEuler[1] = theta;
456			* myEuler[2] = psi;
457			*/
458			void RigidBody::getEulerAngles(double myEuler[3]) {
459
460			// We use so-called "x-convention", which is the most common
461			// definition. In this convention, the rotation given by Euler
462			// angles (phi, theta, psi), where the first rotation is by an angle
463			// phi about the z-axis, the second is by an angle theta (0 <= theta
464			// <= 180) about the x-axis, and the third is by an angle psi about
465			// the z-axis (again).
466
467
468			double phi,theta,psi,eps;
469			double pi;
470			double cphi,ctheta,cpsi;
471			double sphi,stheta,spsi;
472			double b[3];
473			int flip[3];
474
475			// set the tolerance for Euler angles and rotation elements
476
477			eps = 1.0e-8;
478
479			theta = acos(min(1.0,max(-1.0,A[2][2])));
480			ctheta = A[2][2];
481			stheta = sqrt(1.0 - ctheta * ctheta);
482
483			// when sin(theta) is close to 0, we need to consider the
484			// possibility of a singularity. In this case, we can assign an
485			// arbitary value to phi (or psi), and then determine the psi (or
486			// phi) or vice-versa. We'll assume that phi always gets the
487			// rotation, and psi is 0 in cases of singularity. we use atan2
488			// instead of atan, since atan2 will give us -Pi to Pi. Since 0 <=
489			// theta <= 180, sin(theta) will be always non-negative. Therefore,
490			// it never changes the sign of both of the parameters passed to
491			// atan2.
492
493			if (fabs(stheta) <= eps){
494			psi = 0.0;
495			phi = atan2(-A[1][0], A[0][0]);
496			}
497			// we only have one unique solution
498			else{
499			phi = atan2(A[2][0], -A[2][1]);
500			psi = atan2(A[0][2], A[1][2]);
501			}
502
503			//wrap phi and psi, make sure they are in the range from 0 to 2*Pi
504			//if (phi < 0)
505			// phi += M_PI;
506
507			//if (psi < 0)
508			// psi += M_PI;
509
510			myEuler[0] = phi;
511			myEuler[1] = theta;
512			myEuler[2] = psi;
513
514			return;
515			}
516
517			double RigidBody::max(double x, double y) {
518			return (x > y) ? x : y;
519			}
520
521			double RigidBody::min(double x, double y) {
522			return (x > y) ? y : x;
523			}
524
525			void RigidBody::findCOM() {
526
527			size_t i;
528			int j;
529			double mtmp;
530			double ptmp[3];
531			double vtmp[3];
532
533			for(j = 0; j < 3; j++) {
534			pos[j] = 0.0;
535			vel[j] = 0.0;
536			}
537			mass = 0.0;
538
539			for (i = 0; i < myAtoms.size(); i++) {
540
541			mtmp = myAtoms[i]->getMass();
542			myAtoms[i]->getPos(ptmp);
543			myAtoms[i]->getVel(vtmp);
544
545			mass += mtmp;
546
547			for(j = 0; j < 3; j++) {
548			pos[j] += ptmp[j]*mtmp;
549			vel[j] += vtmp[j]*mtmp;
550			}
551
552			}
553
554			for(j = 0; j < 3; j++) {
555			pos[j] /= mass;
556			vel[j] /= mass;
557			}
558
559			com_good = true;
560			}
561
562			void RigidBody::findOrient() {
563
564			size_t it;
565			int i, j;
566			double ptmp[3];
567			double Itmp[3][3];
568			double evals[3];
569			double evects[3][3];
570			double r2, mtmp, len;
571
572			if (!com_good) findCOM();
573
574			// Calculate inertial tensor matrix elements:
575
576			for (i = 0; i < 3; i++)
577			for (j = 0; j < 3; j++)
578			Itmp[i][j] = 0.0;
579
580			for (it = 0; it < myAtoms.size(); it++) {
581
582			mtmp = myAtoms[it]->getMass();
583			myAtoms[it]->getPos(ptmp);
584
585			for (j = 0; j < 3; j++)
586			ptmp[j] = pos[j] - ptmp[j];
587
588			r2 = norm3(ptmp);
589
590			for (i = 0; i < 3; i++) {
591			for (j = 0; j < 3; j++) {
592
593			if (i==j) Itmp[i][j] = mtmp * r2;
594
595			Itmp[i][j] -= mtmp * ptmp[i]*ptmp[j];
596			}
597			}
598			}
599
600			diagonalize3x3(Itmp, evals, sU);
601
602			// zero out I and then fill the diagonals with the moments of inertia:
603
604			for (i = 0; i < 3; i++) {
605			for (j = 0; j < 3; j++) {
606			I[i][j] = 0.0;
607			}
608			I[i][i] = evals[i];
609			}
610
611			// renormalize column vectors:
612
613			for (i=0; i < 3; i++) {
614			len = 0.0;
615			for (j = 0; j < 3; j++) {
616			len += sU[i][j]*sU[i][j];
617			}
618			len = sqrt(len);
619			for (j = 0; j < 3; j++) {
620			sU[i][j] /= len;
621			}
622			}
623
624			// sU now contains the coordinates of the 'special' frame;
625
626			orient_good = true;
627
628			}