trunk/SHAPES/RigidBody.cpp

#include <math.h>
#include <iostream>
#include "RigidBody.hpp"
#include "VDWAtom.hpp"
#include "MatVec3.h"

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

RigidBody::~RigidBody() {
}

void RigidBody::addAtom(VDWAtom* at) {

  vec3 coords;

  myAtoms.push_back(at);
   
  at->getPos(coords.vec);
  refCoords.push_back(coords);      
}

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::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::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, it, n_linear_coords, pAxis, maxAxis, midAxis;
  double mtmp;
  vec3 apos;
  double refCOM[3];
  vec3 ptmp;
  double Itmp[3][3];
  double pAxisMat[3][3], pAxisRotMat[3][3];
  double evals[3];
  double r, r2, len;
  double iMat[3][3];
  double test[3];
  
  // 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) {
          printf(
               "RigidBody error.\n"
               "\tSHAPES 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"
               );
               exit(-1);     
  }

  // 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;
    }
  }
  
  //sort and reorder the moment axes

  // The only problem below is for molecules like C60 with 3 nearly identical 
  // non-zero moments of inertia.  In this case it doesn't really matter which is 
  // the principal axis, so they get assigned nearly randomly depending on the 
  // floating point comparison between eigenvalues
  if (! is_linear) {
    pAxis = 0;
    maxAxis = 0;
  
    for (i = 0; i < 3; i++) {
      if (evals[i] < evals[pAxis]) pAxis = i;
      if (evals[i] > evals[maxAxis]) maxAxis = i;
    }
  
    midAxis = 0;
    for (i=0; i < 3; i++) {
      if (pAxis != i && maxAxis != i) midAxis = i;
    }
  } else {
    pAxis = linear_axis;
    // linear molecules have one zero moment of inertia and two identical
    // moments of inertia.  In this case, it doesn't matter which is chosen
    // as mid and which is max, so just permute from the pAxis:
    midAxis = (pAxis + 1)%3;
    maxAxis = (pAxis + 2)%3;
  }
      
  //let z be the smallest and x be the largest eigenvalue axes
  for (i=0; i<3; i++){
    pAxisMat[i][2] = sU[i][pAxis];
    pAxisMat[i][1] = sU[i][midAxis];
    pAxisMat[i][0] = sU[i][maxAxis];
  }
    
  //calculate the proper rotation matrix
  transposeMat3(pAxisMat, pAxisRotMat);
  
  
  for (i=0; i<myAtoms.size(); i++){
    apos = refCoords[i];
    printf("%f\t%f\t%f\n",apos[0],apos[1],apos[2]);
  }
    
  //rotate the rigid body to the principle axis frame
  for (i = 0; i < myAtoms.size(); i++) {
    matVecMul3(pAxisRotMat, refCoords[i].vec, refCoords[i].vec);
    myAtoms[i]->setPos(refCoords[i].vec);
  }
   
  for (i=0; i<myAtoms.size(); i++){
    apos = refCoords[i];
    printf("%f\t%f\t%f\n",apos[0],apos[1],apos[2]);
  }
  
  identityMat3(iMat);
  setA(iMat);
}

void RigidBody::doEulerToRotMat(double euler[3], double myA[3][3] ){

  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::updateAtoms() {
  int i, j;
  vec3 ref;
  double apos[3];
  
  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);
    
  }  
}

/**
  * 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 ctheta;
  double stheta;
    
  // 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;
}

double RigidBody::findMaxExtent(){
        int i;
        double refAtomPos[3];
        double maxExtent;
        double tempExtent;
        
        //zero the extent variables
        maxExtent = 0.0;
        tempExtent = 0.0;
        for (i=0; i<3; i++)
                refAtomPos[i] = 0.0;
        
        //loop over all atoms
        for (i=0; i<myAtoms.size(); i++){
                getAtomRefCoor(refAtomPos, i);
                tempExtent = sqrt(refAtomPos[0]*refAtomPos[0] + refAtomPos[1]*refAtomPos[1] 
                                                  + refAtomPos[2]*refAtomPos[2]);
                if (tempExtent > maxExtent)
                        maxExtent = tempExtent;
        }
        return maxExtent;
}

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

}

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

  if (index >= myAtoms.size())
    printf( "%d is an invalid index, current rigid body contains "
            "%d atoms\n", index, myAtoms.size());

  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::getAtomRefCoor(double pos[3], int index){
  vec3 ref;

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

double RigidBody::getAtomRpar(int index){
  
  return myAtoms[index]->getRpar();
  
}

double RigidBody::getAtomEps(int index){
  
  return myAtoms[index]->getEps();
  
}
Revision:	1282
Committed:	Mon Jun 21 15:10:29 2004 UTC (20 years ago) by chrisfen
File size:	12999 byte(s)
Log Message:	Fixed the grid builder rotation problems
#	User	Rev	Content
1	gezelter	1271	#include <math.h>
2	chrisfen	1281	#include <iostream>
3	gezelter	1271	#include "RigidBody.hpp"
4			#include "VDWAtom.hpp"
5			#include "MatVec3.h"
6
7			RigidBody::RigidBody() {
8			is_linear = false;
9			linear_axis = -1;
10			momIntTol = 1e-6;
11			}
12
13			RigidBody::~RigidBody() {
14			}
15
16			void RigidBody::addAtom(VDWAtom* at) {
17
18			vec3 coords;
19
20			myAtoms.push_back(at);
21
22			at->getPos(coords.vec);
23			refCoords.push_back(coords);
24			}
25
26			void RigidBody::getPos(double theP[3]){
27			for (int i = 0; i < 3 ; i++)
28			theP[i] = pos[i];
29			}
30
31			void RigidBody::setPos(double theP[3]){
32			for (int i = 0; i < 3 ; i++)
33			pos[i] = theP[i];
34			}
35
36
37			void RigidBody::setEuler( double phi, double theta, double psi ){
38
39			A[0][0] = (cos(phi) * cos(psi)) - (sin(phi) * cos(theta) * sin(psi));
40			A[0][1] = (sin(phi) * cos(psi)) + (cos(phi) * cos(theta) * sin(psi));
41			A[0][2] = sin(theta) * sin(psi);
42
43			A[1][0] = -(cos(phi) * sin(psi)) - (sin(phi) * cos(theta) * cos(psi));
44			A[1][1] = -(sin(phi) * sin(psi)) + (cos(phi) * cos(theta) * cos(psi));
45			A[1][2] = sin(theta) * cos(psi);
46
47			A[2][0] = sin(phi) * sin(theta);
48			A[2][1] = -cos(phi) * sin(theta);
49			A[2][2] = cos(theta);
50
51			}
52
53			void RigidBody::getQ( double q[4] ){
54
55			double t, s;
56			double ad1, ad2, ad3;
57
58			t = A[0][0] + A[1][1] + A[2][2] + 1.0;
59			if( t > 0.0 ){
60
61			s = 0.5 / sqrt( t );
62			q[0] = 0.25 / s;
63			q[1] = (A[1][2] - A[2][1]) * s;
64			q[2] = (A[2][0] - A[0][2]) * s;
65			q[3] = (A[0][1] - A[1][0]) * s;
66			}
67			else{
68
69			ad1 = fabs( A[0][0] );
70			ad2 = fabs( A[1][1] );
71			ad3 = fabs( A[2][2] );
72
73			if( ad1 >= ad2 && ad1 >= ad3 ){
74
75			s = 2.0 * sqrt( 1.0 + A[0][0] - A[1][1] - A[2][2] );
76			q[0] = (A[1][2] + A[2][1]) / s;
77			q[1] = 0.5 / s;
78			q[2] = (A[0][1] + A[1][0]) / s;
79			q[3] = (A[0][2] + A[2][0]) / s;
80			}
81			else if( ad2 >= ad1 && ad2 >= ad3 ){
82
83			s = sqrt( 1.0 + A[1][1] - A[0][0] - A[2][2] ) * 2.0;
84			q[0] = (A[0][2] + A[2][0]) / s;
85			q[1] = (A[0][1] + A[1][0]) / s;
86			q[2] = 0.5 / s;
87			q[3] = (A[1][2] + A[2][1]) / s;
88			}
89			else{
90
91			s = sqrt( 1.0 + A[2][2] - A[0][0] - A[1][1] ) * 2.0;
92			q[0] = (A[0][1] + A[1][0]) / s;
93			q[1] = (A[0][2] + A[2][0]) / s;
94			q[2] = (A[1][2] + A[2][1]) / s;
95			q[3] = 0.5 / s;
96			}
97			}
98			}
99
100			void RigidBody::setQ( double the_q[4] ){
101
102			double q0Sqr, q1Sqr, q2Sqr, q3Sqr;
103
104			q0Sqr = the_q[0] * the_q[0];
105			q1Sqr = the_q[1] * the_q[1];
106			q2Sqr = the_q[2] * the_q[2];
107			q3Sqr = the_q[3] * the_q[3];
108
109			A[0][0] = q0Sqr + q1Sqr - q2Sqr - q3Sqr;
110			A[0][1] = 2.0 * ( the_q[1] * the_q[2] + the_q[0] * the_q[3] );
111			A[0][2] = 2.0 * ( the_q[1] * the_q[3] - the_q[0] * the_q[2] );
112
113			A[1][0] = 2.0 * ( the_q[1] * the_q[2] - the_q[0] * the_q[3] );
114			A[1][1] = q0Sqr - q1Sqr + q2Sqr - q3Sqr;
115			A[1][2] = 2.0 * ( the_q[2] * the_q[3] + the_q[0] * the_q[1] );
116
117			A[2][0] = 2.0 * ( the_q[1] * the_q[3] + the_q[0] * the_q[2] );
118			A[2][1] = 2.0 * ( the_q[2] * the_q[3] - the_q[0] * the_q[1] );
119			A[2][2] = q0Sqr - q1Sqr -q2Sqr +q3Sqr;
120
121			}
122
123			void RigidBody::getA( double the_A[3][3] ){
124
125			for (int i = 0; i < 3; i++)
126			for (int j = 0; j < 3; j++)
127			the_A[i][j] = A[i][j];
128
129			}
130
131			void RigidBody::setA( double the_A[3][3] ){
132
133			for (int i = 0; i < 3; i++)
134			for (int j = 0; j < 3; j++)
135			A[i][j] = the_A[i][j];
136
137			}
138
139			void RigidBody::getI( double the_I[3][3] ){
140
141			for (int i = 0; i < 3; i++)
142			for (int j = 0; j < 3; j++)
143			the_I[i][j] = I[i][j];
144
145			}
146
147			void RigidBody::lab2Body( double r[3] ){
148
149			double rl[3]; // the lab frame vector
150
151			rl[0] = r[0];
152			rl[1] = r[1];
153			rl[2] = r[2];
154
155			r[0] = (A[0][0] * rl[0]) + (A[0][1] * rl[1]) + (A[0][2] * rl[2]);
156			r[1] = (A[1][0] * rl[0]) + (A[1][1] * rl[1]) + (A[1][2] * rl[2]);
157			r[2] = (A[2][0] * rl[0]) + (A[2][1] * rl[1]) + (A[2][2] * rl[2]);
158
159			}
160
161			void RigidBody::body2Lab( double r[3] ){
162
163			double rb[3]; // the body frame vector
164
165			rb[0] = r[0];
166			rb[1] = r[1];
167			rb[2] = r[2];
168
169			r[0] = (A[0][0] * rb[0]) + (A[1][0] * rb[1]) + (A[2][0] * rb[2]);
170			r[1] = (A[0][1] * rb[0]) + (A[1][1] * rb[1]) + (A[2][1] * rb[2]);
171			r[2] = (A[0][2] * rb[0]) + (A[1][2] * rb[1]) + (A[2][2] * rb[2]);
172
173			}
174
175			void RigidBody::calcRefCoords( ) {
176
177	chrisfen	1279	int i, j, it, n_linear_coords, pAxis, maxAxis, midAxis;
178	gezelter	1271	double mtmp;
179			vec3 apos;
180			double refCOM[3];
181			vec3 ptmp;
182			double Itmp[3][3];
183	chrisfen	1279	double pAxisMat[3][3], pAxisRotMat[3][3];
184	gezelter	1271	double evals[3];
185			double r, r2, len;
186	chrisfen	1279	double iMat[3][3];
187	chrisfen	1281	double test[3];
188
189	gezelter	1271	// First, find the center of mass:
190
191			mass = 0.0;
192			for (j=0; j<3; j++)
193			refCOM[j] = 0.0;
194
195			for (i = 0; i < myAtoms.size(); i++) {
196			mtmp = myAtoms[i]->getMass();
197			mass += mtmp;
198
199			apos = refCoords[i];
200			for(j = 0; j < 3; j++) {
201			refCOM[j] += apos[j]*mtmp;
202			}
203			}
204
205			for(j = 0; j < 3; j++)
206			refCOM[j] /= mass;
207
208			// Next, move the origin of the reference coordinate system to the COM:
209
210			for (i = 0; i < myAtoms.size(); i++) {
211			apos = refCoords[i];
212			for (j=0; j < 3; j++) {
213			apos[j] = apos[j] - refCOM[j];
214			}
215			refCoords[i] = apos;
216			}
217
218			// Moment of Inertia calculation
219
220			for (i = 0; i < 3; i++)
221			for (j = 0; j < 3; j++)
222			Itmp[i][j] = 0.0;
223
224			for (it = 0; it < myAtoms.size(); it++) {
225
226			mtmp = myAtoms[it]->getMass();
227			ptmp = refCoords[it];
228			r= norm3(ptmp.vec);
229			r2 = r*r;
230
231			for (i = 0; i < 3; i++) {
232			for (j = 0; j < 3; j++) {
233
234			if (i==j) Itmp[i][j] += mtmp * r2;
235
236			Itmp[i][j] -= mtmp * ptmp.vec[i]*ptmp.vec[j];
237			}
238			}
239			}
240
241			diagonalize3x3(Itmp, evals, sU);
242
243			// zero out I and then fill the diagonals with the moments of inertia:
244
245			n_linear_coords = 0;
246
247			for (i = 0; i < 3; i++) {
248			for (j = 0; j < 3; j++) {
249			I[i][j] = 0.0;
250			}
251			I[i][i] = evals[i];
252
253			if (fabs(evals[i]) < momIntTol) {
254			is_linear = true;
255			n_linear_coords++;
256			linear_axis = i;
257			}
258			}
259
260			if (n_linear_coords > 1) {
261			printf(
262			"RigidBody error.\n"
263	chrisfen	1281	"\tSHAPES found more than one axis in this rigid body with a vanishing \n"
264	gezelter	1271	"\tmoment of inertia. This can happen in one of three ways:\n"
265			"\t 1) Only one atom was specified, or \n"
266			"\t 2) All atoms were specified at the same location, or\n"
267			"\t 3) The programmers did something stupid.\n"
268			"\tIt is silly to use a rigid body to describe this situation. Be smarter.\n"
269			);
270			exit(-1);
271			}
272	chrisfen	1281
273	gezelter	1271	// renormalize column vectors:
274
275			for (i=0; i < 3; i++) {
276			len = 0.0;
277			for (j = 0; j < 3; j++) {
278			len += sU[i][j]*sU[i][j];
279			}
280			len = sqrt(len);
281			for (j = 0; j < 3; j++) {
282			sU[i][j] /= len;
283			}
284			}
285	chrisfen	1281
286			//sort and reorder the moment axes
287
288			// The only problem below is for molecules like C60 with 3 nearly identical
289			// non-zero moments of inertia. In this case it doesn't really matter which is
290			// the principal axis, so they get assigned nearly randomly depending on the
291			// floating point comparison between eigenvalues
292			if (! is_linear) {
293			pAxis = 0;
294			maxAxis = 0;
295
296			for (i = 0; i < 3; i++) {
297			if (evals[i] < evals[pAxis]) pAxis = i;
298			if (evals[i] > evals[maxAxis]) maxAxis = i;
299			}
300
301			midAxis = 0;
302			for (i=0; i < 3; i++) {
303			if (pAxis != i && maxAxis != i) midAxis = i;
304			}
305			} else {
306			pAxis = linear_axis;
307			// linear molecules have one zero moment of inertia and two identical
308			// moments of inertia. In this case, it doesn't matter which is chosen
309			// as mid and which is max, so just permute from the pAxis:
310			midAxis = (pAxis + 1)%3;
311			maxAxis = (pAxis + 2)%3;
312			}
313
314			//let z be the smallest and x be the largest eigenvalue axes
315			for (i=0; i<3; i++){
316			pAxisMat[i][2] = sU[i][pAxis];
317			pAxisMat[i][1] = sU[i][midAxis];
318			pAxisMat[i][0] = sU[i][maxAxis];
319			}
320
321			//calculate the proper rotation matrix
322			transposeMat3(pAxisMat, pAxisRotMat);
323
324	chrisfen	1282
325	chrisfen	1281	for (i=0; i<myAtoms.size(); i++){
326	chrisfen	1282	apos = refCoords[i];
327			printf("%f\t%f\t%f\n",apos[0],apos[1],apos[2]);
328	chrisfen	1281	}
329
330			//rotate the rigid body to the principle axis frame
331			for (i = 0; i < myAtoms.size(); i++) {
332			matVecMul3(pAxisRotMat, refCoords[i].vec, refCoords[i].vec);
333			myAtoms[i]->setPos(refCoords[i].vec);
334			}
335
336			for (i=0; i<myAtoms.size(); i++){
337	chrisfen	1282	apos = refCoords[i];
338			printf("%f\t%f\t%f\n",apos[0],apos[1],apos[2]);
339	chrisfen	1281	}
340
341			identityMat3(iMat);
342			setA(iMat);
343	gezelter	1271	}
344
345	chrisfen	1276	void RigidBody::doEulerToRotMat(double euler[3], double myA[3][3] ){
346	gezelter	1271
347			double phi, theta, psi;
348
349			phi = euler[0];
350			theta = euler[1];
351			psi = euler[2];
352
353			myA[0][0] = (cos(phi) * cos(psi)) - (sin(phi) * cos(theta) * sin(psi));
354			myA[0][1] = (sin(phi) * cos(psi)) + (cos(phi) * cos(theta) * sin(psi));
355			myA[0][2] = sin(theta) * sin(psi);
356
357			myA[1][0] = -(cos(phi) * sin(psi)) - (sin(phi) * cos(theta) * cos(psi));
358			myA[1][1] = -(sin(phi) * sin(psi)) + (cos(phi) * cos(theta) * cos(psi));
359			myA[1][2] = sin(theta) * cos(psi);
360
361			myA[2][0] = sin(phi) * sin(theta);
362			myA[2][1] = -cos(phi) * sin(theta);
363			myA[2][2] = cos(theta);
364
365			}
366
367			void RigidBody::updateAtoms() {
368			int i, j;
369			vec3 ref;
370			double apos[3];
371
372			for (i = 0; i < myAtoms.size(); i++) {
373
374			ref = refCoords[i];
375
376			body2Lab(ref.vec);
377
378			for (j = 0; j<3; j++)
379			apos[j] = pos[j] + ref.vec[j];
380
381			myAtoms[i]->setPos(apos);
382
383			}
384			}
385
386			/**
387			* getEulerAngles computes a set of Euler angle values consistent
388			* with an input rotation matrix. They are returned in the following
389			* order:
390			* myEuler[0] = phi;
391			* myEuler[1] = theta;
392			* myEuler[2] = psi;
393			*/
394			void RigidBody::getEulerAngles(double myEuler[3]) {
395
396			// We use so-called "x-convention", which is the most common
397			// definition. In this convention, the rotation given by Euler
398			// angles (phi, theta, psi), where the first rotation is by an angle
399			// phi about the z-axis, the second is by an angle theta (0 <= theta
400			// <= 180) about the x-axis, and the third is by an angle psi about
401			// the z-axis (again).
402
403
404			double phi,theta,psi,eps;
405	chrisfen	1276	double ctheta;
406			double stheta;
407
408	gezelter	1271	// set the tolerance for Euler angles and rotation elements
409
410			eps = 1.0e-8;
411
412			theta = acos(min(1.0,max(-1.0,A[2][2])));
413			ctheta = A[2][2];
414			stheta = sqrt(1.0 - ctheta * ctheta);
415
416			// when sin(theta) is close to 0, we need to consider the
417			// possibility of a singularity. In this case, we can assign an
418			// arbitary value to phi (or psi), and then determine the psi (or
419			// phi) or vice-versa. We'll assume that phi always gets the
420			// rotation, and psi is 0 in cases of singularity. we use atan2
421			// instead of atan, since atan2 will give us -Pi to Pi. Since 0 <=
422			// theta <= 180, sin(theta) will be always non-negative. Therefore,
423			// it never changes the sign of both of the parameters passed to
424			// atan2.
425
426			if (fabs(stheta) <= eps){
427			psi = 0.0;
428			phi = atan2(-A[1][0], A[0][0]);
429			}
430			// we only have one unique solution
431			else{
432			phi = atan2(A[2][0], -A[2][1]);
433			psi = atan2(A[0][2], A[1][2]);
434			}
435
436			//wrap phi and psi, make sure they are in the range from 0 to 2*Pi
437			//if (phi < 0)
438			// phi += M_PI;
439
440			//if (psi < 0)
441			// psi += M_PI;
442
443			myEuler[0] = phi;
444			myEuler[1] = theta;
445			myEuler[2] = psi;
446
447			return;
448			}
449
450			double RigidBody::max(double x, double y) {
451			return (x > y) ? x : y;
452			}
453
454			double RigidBody::min(double x, double y) {
455			return (x > y) ? y : x;
456			}
457
458	chrisfen	1276	double RigidBody::findMaxExtent(){
459			int i;
460			double refAtomPos[3];
461			double maxExtent;
462			double tempExtent;
463
464			//zero the extent variables
465			maxExtent = 0.0;
466			tempExtent = 0.0;
467			for (i=0; i<3; i++)
468			refAtomPos[i] = 0.0;
469
470			//loop over all atoms
471			for (i=0; i<myAtoms.size(); i++){
472			getAtomRefCoor(refAtomPos, i);
473			tempExtent = sqrt(refAtomPos[0]refAtomPos[0] + refAtomPos[1]refAtomPos[1]
474			+ refAtomPos[2]*refAtomPos[2]);
475			if (tempExtent > maxExtent)
476			maxExtent = tempExtent;
477			}
478			return maxExtent;
479			}
480
481	gezelter	1271	void RigidBody::findCOM() {
482
483			size_t i;
484			int j;
485			double mtmp;
486			double ptmp[3];
487	chrisfen	1276
488	gezelter	1271	for(j = 0; j < 3; j++) {
489			pos[j] = 0.0;
490			}
491			mass = 0.0;
492
493			for (i = 0; i < myAtoms.size(); i++) {
494
495			mtmp = myAtoms[i]->getMass();
496			myAtoms[i]->getPos(ptmp);
497
498			mass += mtmp;
499
500			for(j = 0; j < 3; j++) {
501			pos[j] += ptmp[j]*mtmp;
502			}
503
504			}
505
506			for(j = 0; j < 3; j++) {
507			pos[j] /= mass;
508			}
509
510			}
511
512			void RigidBody::getAtomPos(double theP[3], int index){
513			vec3 ref;
514
515			if (index >= myAtoms.size())
516			printf( "%d is an invalid index, current rigid body contains "
517			"%d atoms\n", index, myAtoms.size());
518
519			ref = refCoords[index];
520			body2Lab(ref.vec);
521
522			theP[0] = pos[0] + ref[0];
523			theP[1] = pos[1] + ref[1];
524			theP[2] = pos[2] + ref[2];
525			}
526
527
528			void RigidBody::getAtomRefCoor(double pos[3], int index){
529			vec3 ref;
530
531			ref = refCoords[index];
532			pos[0] = ref[0];
533			pos[1] = ref[1];
534			pos[2] = ref[2];
535
536			}
537	chrisfen	1281
538			double RigidBody::getAtomRpar(int index){
539
540			return myAtoms[index]->getRpar();
541
542			}
543
544			double RigidBody::getAtomEps(int index){
545
546			return myAtoms[index]->getEps();
547
548			}