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
#	Content
1	#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	}