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

    printf("A[2][x] = %lf\t%lf\t%lf\n", A[2][0], A[2][1], A[2][2]);

}

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