ViewVC Help
View File | Revision Log | Show Annotations | View Changeset | Root Listing
root/group/trunk/SHAPES/RigidBody.cpp
Revision: 1288
Committed: Wed Jun 23 20:28:39 2004 UTC (20 years ago) by chrisfen
File size: 12842 byte(s)
Log Message:
touch up changes

File Contents

# Content
1 #include <math.h>
2 #include <iostream>
3 #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 int i, j, it, n_linear_coords, pAxis, maxAxis, midAxis;
178 double mtmp;
179 vec3 apos;
180 double refCOM[3];
181 vec3 ptmp;
182 double Itmp[3][3];
183 double pAxisMat[3][3], pAxisRotMat[3][3];
184 double evals[3];
185 double r, r2, len;
186 double iMat[3][3];
187 double test[3];
188
189 // 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 "\tSHAPES 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 //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
325 //rotate the rigid body to the principle axis frame
326 for (i = 0; i < myAtoms.size(); i++) {
327 matVecMul3(pAxisRotMat, refCoords[i].vec, refCoords[i].vec);
328 myAtoms[i]->setPos(refCoords[i].vec);
329 }
330
331 identityMat3(iMat);
332 setA(iMat);
333 }
334
335 void RigidBody::doEulerToRotMat(double euler[3], double myA[3][3] ){
336
337 double phi, theta, psi;
338
339 phi = euler[0];
340 theta = euler[1];
341 psi = euler[2];
342
343 myA[0][0] = (cos(phi) * cos(psi)) - (sin(phi) * cos(theta) * sin(psi));
344 myA[0][1] = (sin(phi) * cos(psi)) + (cos(phi) * cos(theta) * sin(psi));
345 myA[0][2] = sin(theta) * sin(psi);
346
347 myA[1][0] = -(cos(phi) * sin(psi)) - (sin(phi) * cos(theta) * cos(psi));
348 myA[1][1] = -(sin(phi) * sin(psi)) + (cos(phi) * cos(theta) * cos(psi));
349 myA[1][2] = sin(theta) * cos(psi);
350
351 myA[2][0] = sin(phi) * sin(theta);
352 myA[2][1] = -cos(phi) * sin(theta);
353 myA[2][2] = cos(theta);
354
355 }
356
357 void RigidBody::updateAtoms() {
358 int i, j;
359 vec3 ref;
360 double apos[3];
361
362 for (i = 0; i < myAtoms.size(); i++) {
363
364 ref = refCoords[i];
365
366 body2Lab(ref.vec);
367
368 for (j = 0; j<3; j++)
369 apos[j] = pos[j] + ref.vec[j];
370
371 myAtoms[i]->setPos(apos);
372
373 }
374 }
375
376 /**
377 * getEulerAngles computes a set of Euler angle values consistent
378 * with an input rotation matrix. They are returned in the following
379 * order:
380 * myEuler[0] = phi;
381 * myEuler[1] = theta;
382 * myEuler[2] = psi;
383 */
384 void RigidBody::getEulerAngles(double myEuler[3]) {
385
386 // We use so-called "x-convention", which is the most common
387 // definition. In this convention, the rotation given by Euler
388 // angles (phi, theta, psi), where the first rotation is by an angle
389 // phi about the z-axis, the second is by an angle theta (0 <= theta
390 // <= 180) about the x-axis, and the third is by an angle psi about
391 // the z-axis (again).
392
393
394 double phi,theta,psi,eps;
395 double ctheta;
396 double stheta;
397
398 // set the tolerance for Euler angles and rotation elements
399
400 eps = 1.0e-8;
401
402 theta = acos(min(1.0,max(-1.0,A[2][2])));
403 ctheta = A[2][2];
404 stheta = sqrt(1.0 - ctheta * ctheta);
405
406 // when sin(theta) is close to 0, we need to consider the
407 // possibility of a singularity. In this case, we can assign an
408 // arbitary value to phi (or psi), and then determine the psi (or
409 // phi) or vice-versa. We'll assume that phi always gets the
410 // rotation, and psi is 0 in cases of singularity. we use atan2
411 // instead of atan, since atan2 will give us -Pi to Pi. Since 0 <=
412 // theta <= 180, sin(theta) will be always non-negative. Therefore,
413 // it never changes the sign of both of the parameters passed to
414 // atan2.
415
416 if (fabs(stheta) <= eps){
417 psi = 0.0;
418 phi = atan2(-A[1][0], A[0][0]);
419 }
420 // we only have one unique solution
421 else{
422 phi = atan2(A[2][0], -A[2][1]);
423 psi = atan2(A[0][2], A[1][2]);
424 }
425
426 //wrap phi and psi, make sure they are in the range from 0 to 2*Pi
427 //if (phi < 0)
428 // phi += M_PI;
429
430 //if (psi < 0)
431 // psi += M_PI;
432
433 myEuler[0] = phi;
434 myEuler[1] = theta;
435 myEuler[2] = psi;
436
437 return;
438 }
439
440 double RigidBody::max(double x, double y) {
441 return (x > y) ? x : y;
442 }
443
444 double RigidBody::min(double x, double y) {
445 return (x > y) ? y : x;
446 }
447
448 double RigidBody::findMaxExtent(){
449 int i;
450 double refAtomPos[3];
451 double maxExtent;
452 double tempExtent;
453
454 //zero the extent variables
455 maxExtent = 0.0;
456 tempExtent = 0.0;
457 for (i=0; i<3; i++)
458 refAtomPos[i] = 0.0;
459
460 //loop over all atoms
461 for (i=0; i<myAtoms.size(); i++){
462 getAtomRefCoor(refAtomPos, i);
463 tempExtent = sqrt(refAtomPos[0]*refAtomPos[0] + refAtomPos[1]*refAtomPos[1]
464 + refAtomPos[2]*refAtomPos[2]);
465 if (tempExtent > maxExtent)
466 maxExtent = tempExtent;
467 }
468 return maxExtent;
469 }
470
471 void RigidBody::findCOM() {
472
473 size_t i;
474 int j;
475 double mtmp;
476 double ptmp[3];
477
478 for(j = 0; j < 3; j++) {
479 pos[j] = 0.0;
480 }
481 mass = 0.0;
482
483 for (i = 0; i < myAtoms.size(); i++) {
484
485 mtmp = myAtoms[i]->getMass();
486 myAtoms[i]->getPos(ptmp);
487
488 mass += mtmp;
489
490 for(j = 0; j < 3; j++) {
491 pos[j] += ptmp[j]*mtmp;
492 }
493
494 }
495
496 for(j = 0; j < 3; j++) {
497 pos[j] /= mass;
498 }
499
500 }
501
502 void RigidBody::getAtomPos(double theP[3], int index){
503 vec3 ref;
504
505 if (index >= myAtoms.size())
506 printf( "%d is an invalid index, current rigid body contains "
507 "%d atoms\n", index, myAtoms.size());
508
509 ref = refCoords[index];
510 body2Lab(ref.vec);
511
512 theP[0] = pos[0] + ref[0];
513 theP[1] = pos[1] + ref[1];
514 theP[2] = pos[2] + ref[2];
515 }
516
517
518 void RigidBody::getAtomRefCoor(double pos[3], int index){
519 vec3 ref;
520
521 ref = refCoords[index];
522 pos[0] = ref[0];
523 pos[1] = ref[1];
524 pos[2] = ref[2];
525
526 }
527
528 double RigidBody::getAtomRpar(int index){
529
530 return myAtoms[index]->getRpar();
531
532 }
533
534 double RigidBody::getAtomEps(int index){
535
536 return myAtoms[index]->getEps();
537
538 }
539
540 char *RigidBody::getAtomBase(int index){
541
542 return myAtoms[index]->getBase();
543
544 }