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

}

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