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

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

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

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

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