trunk/SHAPES/RigidBody.cpp

#include <math.h>
#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;
  vec3 euler;
  mat3x3 Atmp;

  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,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) {
          printf(
               "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"
               );
               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;
    }
  }
}

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::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 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;
  }
  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];
  
}
Revision:	1271
Committed:	Tue Jun 15 20:20:36 2004 UTC (20 years ago) by gezelter
File size:	10589 byte(s)
Log Message:	Major changes for rigidbodies
#	User	Rev	Content
1	gezelter	1271	#include <math.h>
2			#include "RigidBody.hpp"
3			#include "VDWAtom.hpp"
4			#include "MatVec3.h"
5
6			RigidBody::RigidBody() {
7			is_linear = false;
8			linear_axis = -1;
9			momIntTol = 1e-6;
10			}
11
12			RigidBody::~RigidBody() {
13			}
14
15			void RigidBody::addAtom(VDWAtom* at) {
16
17			vec3 coords;
18			vec3 euler;
19			mat3x3 Atmp;
20
21			myAtoms.push_back(at);
22
23			at->getPos(coords.vec);
24			refCoords.push_back(coords);
25			}
26
27			void RigidBody::getPos(double theP[3]){
28			for (int i = 0; i < 3 ; i++)
29			theP[i] = pos[i];
30			}
31
32			void RigidBody::setPos(double theP[3]){
33			for (int i = 0; i < 3 ; i++)
34			pos[i] = theP[i];
35			}
36
37
38			void RigidBody::setEuler( double phi, double theta, double psi ){
39
40			A[0][0] = (cos(phi) * cos(psi)) - (sin(phi) * cos(theta) * sin(psi));
41			A[0][1] = (sin(phi) * cos(psi)) + (cos(phi) * cos(theta) * sin(psi));
42			A[0][2] = sin(theta) * sin(psi);
43
44			A[1][0] = -(cos(phi) * sin(psi)) - (sin(phi) * cos(theta) * cos(psi));
45			A[1][1] = -(sin(phi) * sin(psi)) + (cos(phi) * cos(theta) * cos(psi));
46			A[1][2] = sin(theta) * cos(psi);
47
48			A[2][0] = sin(phi) * sin(theta);
49			A[2][1] = -cos(phi) * sin(theta);
50			A[2][2] = cos(theta);
51
52			}
53
54			void RigidBody::getQ( double q[4] ){
55
56			double t, s;
57			double ad1, ad2, ad3;
58
59			t = A[0][0] + A[1][1] + A[2][2] + 1.0;
60			if( t > 0.0 ){
61
62			s = 0.5 / sqrt( t );
63			q[0] = 0.25 / s;
64			q[1] = (A[1][2] - A[2][1]) * s;
65			q[2] = (A[2][0] - A[0][2]) * s;
66			q[3] = (A[0][1] - A[1][0]) * s;
67			}
68			else{
69
70			ad1 = fabs( A[0][0] );
71			ad2 = fabs( A[1][1] );
72			ad3 = fabs( A[2][2] );
73
74			if( ad1 >= ad2 && ad1 >= ad3 ){
75
76			s = 2.0 * sqrt( 1.0 + A[0][0] - A[1][1] - A[2][2] );
77			q[0] = (A[1][2] + A[2][1]) / s;
78			q[1] = 0.5 / s;
79			q[2] = (A[0][1] + A[1][0]) / s;
80			q[3] = (A[0][2] + A[2][0]) / s;
81			}
82			else if( ad2 >= ad1 && ad2 >= ad3 ){
83
84			s = sqrt( 1.0 + A[1][1] - A[0][0] - A[2][2] ) * 2.0;
85			q[0] = (A[0][2] + A[2][0]) / s;
86			q[1] = (A[0][1] + A[1][0]) / s;
87			q[2] = 0.5 / s;
88			q[3] = (A[1][2] + A[2][1]) / s;
89			}
90			else{
91
92			s = sqrt( 1.0 + A[2][2] - A[0][0] - A[1][1] ) * 2.0;
93			q[0] = (A[0][1] + A[1][0]) / s;
94			q[1] = (A[0][2] + A[2][0]) / s;
95			q[2] = (A[1][2] + A[2][1]) / s;
96			q[3] = 0.5 / s;
97			}
98			}
99			}
100
101			void RigidBody::setQ( double the_q[4] ){
102
103			double q0Sqr, q1Sqr, q2Sqr, q3Sqr;
104
105			q0Sqr = the_q[0] * the_q[0];
106			q1Sqr = the_q[1] * the_q[1];
107			q2Sqr = the_q[2] * the_q[2];
108			q3Sqr = the_q[3] * the_q[3];
109
110			A[0][0] = q0Sqr + q1Sqr - q2Sqr - q3Sqr;
111			A[0][1] = 2.0 * ( the_q[1] * the_q[2] + the_q[0] * the_q[3] );
112			A[0][2] = 2.0 * ( the_q[1] * the_q[3] - the_q[0] * the_q[2] );
113
114			A[1][0] = 2.0 * ( the_q[1] * the_q[2] - the_q[0] * the_q[3] );
115			A[1][1] = q0Sqr - q1Sqr + q2Sqr - q3Sqr;
116			A[1][2] = 2.0 * ( the_q[2] * the_q[3] + the_q[0] * the_q[1] );
117
118			A[2][0] = 2.0 * ( the_q[1] * the_q[3] + the_q[0] * the_q[2] );
119			A[2][1] = 2.0 * ( the_q[2] * the_q[3] - the_q[0] * the_q[1] );
120			A[2][2] = q0Sqr - q1Sqr -q2Sqr +q3Sqr;
121
122			}
123
124			void RigidBody::getA( double the_A[3][3] ){
125
126			for (int i = 0; i < 3; i++)
127			for (int j = 0; j < 3; j++)
128			the_A[i][j] = A[i][j];
129
130			}
131
132			void RigidBody::setA( double the_A[3][3] ){
133
134			for (int i = 0; i < 3; i++)
135			for (int j = 0; j < 3; j++)
136			A[i][j] = the_A[i][j];
137
138			}
139
140			void RigidBody::getI( double the_I[3][3] ){
141
142			for (int i = 0; i < 3; i++)
143			for (int j = 0; j < 3; j++)
144			the_I[i][j] = I[i][j];
145
146			}
147
148			void RigidBody::lab2Body( double r[3] ){
149
150			double rl[3]; // the lab frame vector
151
152			rl[0] = r[0];
153			rl[1] = r[1];
154			rl[2] = r[2];
155
156			r[0] = (A[0][0] * rl[0]) + (A[0][1] * rl[1]) + (A[0][2] * rl[2]);
157			r[1] = (A[1][0] * rl[0]) + (A[1][1] * rl[1]) + (A[1][2] * rl[2]);
158			r[2] = (A[2][0] * rl[0]) + (A[2][1] * rl[1]) + (A[2][2] * rl[2]);
159
160			}
161
162			void RigidBody::body2Lab( double r[3] ){
163
164			double rb[3]; // the body frame vector
165
166			rb[0] = r[0];
167			rb[1] = r[1];
168			rb[2] = r[2];
169
170			r[0] = (A[0][0] * rb[0]) + (A[1][0] * rb[1]) + (A[2][0] * rb[2]);
171			r[1] = (A[0][1] * rb[0]) + (A[1][1] * rb[1]) + (A[2][1] * rb[2]);
172			r[2] = (A[0][2] * rb[0]) + (A[1][2] * rb[1]) + (A[2][2] * rb[2]);
173
174			}
175
176			void RigidBody::calcRefCoords( ) {
177
178			int i,j,k, it, n_linear_coords;
179			double mtmp;
180			vec3 apos;
181			double refCOM[3];
182			vec3 ptmp;
183			double Itmp[3][3];
184			double evals[3];
185			double evects[3][3];
186			double r, r2, len;
187
188			// First, find the center of mass:
189
190			mass = 0.0;
191			for (j=0; j<3; j++)
192			refCOM[j] = 0.0;
193
194			for (i = 0; i < myAtoms.size(); i++) {
195			mtmp = myAtoms[i]->getMass();
196			mass += mtmp;
197
198			apos = refCoords[i];
199
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			"\tOOPSE found more than one axis in this rigid body with a vanishing \n"
264			"\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
273			// 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			}
286
287			void RigidBody::doEulerToRotMat(vec3 &euler, mat3x3 &myA ){
288
289			double phi, theta, psi;
290
291			phi = euler[0];
292			theta = euler[1];
293			psi = euler[2];
294
295			myA[0][0] = (cos(phi) * cos(psi)) - (sin(phi) * cos(theta) * sin(psi));
296			myA[0][1] = (sin(phi) * cos(psi)) + (cos(phi) * cos(theta) * sin(psi));
297			myA[0][2] = sin(theta) * sin(psi);
298
299			myA[1][0] = -(cos(phi) * sin(psi)) - (sin(phi) * cos(theta) * cos(psi));
300			myA[1][1] = -(sin(phi) * sin(psi)) + (cos(phi) * cos(theta) * cos(psi));
301			myA[1][2] = sin(theta) * cos(psi);
302
303			myA[2][0] = sin(phi) * sin(theta);
304			myA[2][1] = -cos(phi) * sin(theta);
305			myA[2][2] = cos(theta);
306
307			}
308
309			void RigidBody::updateAtoms() {
310			int i, j;
311			vec3 ref;
312			double apos[3];
313
314			for (i = 0; i < myAtoms.size(); i++) {
315
316			ref = refCoords[i];
317
318			body2Lab(ref.vec);
319
320			for (j = 0; j<3; j++)
321			apos[j] = pos[j] + ref.vec[j];
322
323			myAtoms[i]->setPos(apos);
324
325			}
326			}
327
328			/**
329			* getEulerAngles computes a set of Euler angle values consistent
330			* with an input rotation matrix. They are returned in the following
331			* order:
332			* myEuler[0] = phi;
333			* myEuler[1] = theta;
334			* myEuler[2] = psi;
335			*/
336			void RigidBody::getEulerAngles(double myEuler[3]) {
337
338			// We use so-called "x-convention", which is the most common
339			// definition. In this convention, the rotation given by Euler
340			// angles (phi, theta, psi), where the first rotation is by an angle
341			// phi about the z-axis, the second is by an angle theta (0 <= theta
342			// <= 180) about the x-axis, and the third is by an angle psi about
343			// the z-axis (again).
344
345
346			double phi,theta,psi,eps;
347			double pi;
348			double cphi,ctheta,cpsi;
349			double sphi,stheta,spsi;
350			double b[3];
351			int flip[3];
352
353			// set the tolerance for Euler angles and rotation elements
354
355			eps = 1.0e-8;
356
357			theta = acos(min(1.0,max(-1.0,A[2][2])));
358			ctheta = A[2][2];
359			stheta = sqrt(1.0 - ctheta * ctheta);
360
361			// when sin(theta) is close to 0, we need to consider the
362			// possibility of a singularity. In this case, we can assign an
363			// arbitary value to phi (or psi), and then determine the psi (or
364			// phi) or vice-versa. We'll assume that phi always gets the
365			// rotation, and psi is 0 in cases of singularity. we use atan2
366			// instead of atan, since atan2 will give us -Pi to Pi. Since 0 <=
367			// theta <= 180, sin(theta) will be always non-negative. Therefore,
368			// it never changes the sign of both of the parameters passed to
369			// atan2.
370
371			if (fabs(stheta) <= eps){
372			psi = 0.0;
373			phi = atan2(-A[1][0], A[0][0]);
374			}
375			// we only have one unique solution
376			else{
377			phi = atan2(A[2][0], -A[2][1]);
378			psi = atan2(A[0][2], A[1][2]);
379			}
380
381			//wrap phi and psi, make sure they are in the range from 0 to 2*Pi
382			//if (phi < 0)
383			// phi += M_PI;
384
385			//if (psi < 0)
386			// psi += M_PI;
387
388			myEuler[0] = phi;
389			myEuler[1] = theta;
390			myEuler[2] = psi;
391
392			return;
393			}
394
395			double RigidBody::max(double x, double y) {
396			return (x > y) ? x : y;
397			}
398
399			double RigidBody::min(double x, double y) {
400			return (x > y) ? y : x;
401			}
402
403			void RigidBody::findCOM() {
404
405			size_t i;
406			int j;
407			double mtmp;
408			double ptmp[3];
409			double vtmp[3];
410
411			for(j = 0; j < 3; j++) {
412			pos[j] = 0.0;
413			}
414			mass = 0.0;
415
416			for (i = 0; i < myAtoms.size(); i++) {
417
418			mtmp = myAtoms[i]->getMass();
419			myAtoms[i]->getPos(ptmp);
420
421			mass += mtmp;
422
423			for(j = 0; j < 3; j++) {
424			pos[j] += ptmp[j]*mtmp;
425			}
426
427			}
428
429			for(j = 0; j < 3; j++) {
430			pos[j] /= mass;
431			}
432
433			}
434
435			void RigidBody::getAtomPos(double theP[3], int index){
436			vec3 ref;
437
438			if (index >= myAtoms.size())
439			printf( "%d is an invalid index, current rigid body contains "
440			"%d atoms\n", index, myAtoms.size());
441
442			ref = refCoords[index];
443			body2Lab(ref.vec);
444
445			theP[0] = pos[0] + ref[0];
446			theP[1] = pos[1] + ref[1];
447			theP[2] = pos[2] + ref[2];
448			}
449
450
451			void RigidBody::getAtomRefCoor(double pos[3], int index){
452			vec3 ref;
453
454			ref = refCoords[index];
455			pos[0] = ref[0];
456			pos[1] = ref[1];
457			pos[2] = ref[2];
458
459			}