2 |
|
|
3 |
|
#include "Atom.hpp" |
4 |
|
#include "simError.h" |
5 |
+ |
|
6 |
+ |
|
7 |
|
|
8 |
|
void DirectionalAtom::zeroForces() { |
9 |
|
if( hasCoords ){ |
402 |
|
the_I[2][0] = Izx; |
403 |
|
the_I[2][1] = Izy; |
404 |
|
the_I[2][2] = Izz; |
405 |
+ |
} |
406 |
+ |
|
407 |
+ |
void DirectionalAtom::getGrad( double grad[6] ) { |
408 |
+ |
|
409 |
+ |
double myEuler[3]; |
410 |
+ |
double phi, theta, psi; |
411 |
+ |
double cphi, sphi, ctheta, stheta; |
412 |
+ |
double ephi[3]; |
413 |
+ |
double etheta[3]; |
414 |
+ |
double epsi[3]; |
415 |
+ |
|
416 |
+ |
this->getEulerAngles(myEuler); |
417 |
+ |
|
418 |
+ |
phi = myEuler[0]; |
419 |
+ |
theta = myEuler[1]; |
420 |
+ |
psi = myEuler[2]; |
421 |
+ |
|
422 |
+ |
cphi = cos(phi); |
423 |
+ |
sphi = sin(phi); |
424 |
+ |
ctheta = cos(theta); |
425 |
+ |
stheta = sin(theta); |
426 |
+ |
|
427 |
+ |
// get unit vectors along the phi, theta and psi rotation axes |
428 |
+ |
|
429 |
+ |
ephi[0] = 0.0; |
430 |
+ |
ephi[1] = 0.0; |
431 |
+ |
ephi[2] = 1.0; |
432 |
+ |
etheta[0] = -sphi; |
433 |
+ |
etheta[1] = cphi; |
434 |
+ |
etheta[2] = 0.0; |
435 |
+ |
epsi[0] = ctheta * cphi; |
436 |
+ |
epsi[1] = ctheta * sphi; |
437 |
+ |
epsi[2] = -stheta; |
438 |
+ |
|
439 |
+ |
for (int j = 0 ; j<3; j++) |
440 |
+ |
grad[j] = frc[j]; |
441 |
+ |
|
442 |
+ |
for (int j = 0; j < 3; j++ ) { |
443 |
+ |
|
444 |
+ |
grad[3] += trq[j]*ephi[j]; |
445 |
+ |
grad[4] += trq[j]*etheta[j]; |
446 |
+ |
grad[5] += trq[j]*epsi[j]; |
447 |
+ |
|
448 |
+ |
} |
449 |
+ |
|
450 |
+ |
} |
451 |
+ |
|
452 |
+ |
|
453 |
+ |
void DirectionalAtom::getEulerAngles(double myEuler[3]) { |
454 |
+ |
|
455 |
+ |
// getEulerAngles computes a set of Euler angle values consistent |
456 |
+ |
// with an input rotation matrix. They are returned in the following |
457 |
+ |
// order: |
458 |
+ |
// myEuler[0] = phi; |
459 |
+ |
// myEuler[1] = theta; |
460 |
+ |
// myEuler[2] = psi; |
461 |
+ |
|
462 |
+ |
double phi,theta,psi,eps; |
463 |
+ |
double pi; |
464 |
+ |
double cphi,ctheta,cpsi; |
465 |
+ |
double sphi,stheta,spsi; |
466 |
+ |
double b[3]; |
467 |
+ |
int flip[3]; |
468 |
+ |
|
469 |
+ |
// set the tolerance for Euler angles and rotation elements |
470 |
+ |
|
471 |
+ |
eps = 1.0e-8; |
472 |
+ |
|
473 |
+ |
// get a trial value of theta from a single rotation element |
474 |
+ |
|
475 |
+ |
theta = asin(min(1.0,max(-1.0,-Amat[Axz]))); |
476 |
+ |
ctheta = cos(theta); |
477 |
+ |
stheta = -Amat[Axz]; |
478 |
+ |
|
479 |
+ |
// set the phi/psi difference when theta is either 90 or -90 |
480 |
+ |
|
481 |
+ |
if (fabs(ctheta) <= eps) { |
482 |
+ |
phi = 0.0; |
483 |
+ |
if (fabs(Amat[Azx]) < eps) { |
484 |
+ |
psi = asin(min(1.0,max(-1.0,-Amat[Ayx]/Amat[Axz]))); |
485 |
+ |
} else { |
486 |
+ |
if (fabs(Amat[Ayx]) < eps) { |
487 |
+ |
psi = acos(min(1.0,max(-1.0,-Amat[Azx]/Amat[Axz]))); |
488 |
+ |
} else { |
489 |
+ |
psi = atan(Amat[Ayx]/Amat[Azx]); |
490 |
+ |
} |
491 |
+ |
} |
492 |
+ |
} |
493 |
+ |
|
494 |
+ |
// set the phi and psi values for all other theta values |
495 |
+ |
|
496 |
+ |
else { |
497 |
+ |
if (fabs(Amat[Axx]) < eps) { |
498 |
+ |
phi = asin(min(1.0,max(-1.0,Amat[Axy]/ctheta))); |
499 |
+ |
} else { |
500 |
+ |
if (fabs(Amat[Axy]) < eps) { |
501 |
+ |
phi = acos(min(1.0,max(-1.0,Amat[Axx]/ctheta))); |
502 |
+ |
} else { |
503 |
+ |
phi = atan(Amat[Axy]/Amat[Axx]); |
504 |
+ |
} |
505 |
+ |
} |
506 |
+ |
if (fabs(Amat[Azz]) < eps) { |
507 |
+ |
psi = asin(min(1.0,max(-1.0,Amat[Ayz]/ctheta))); |
508 |
+ |
} else { |
509 |
+ |
if (fabs(Amat[Ayz]) < eps) { |
510 |
+ |
psi = acos(min(1.0,max(-1.0,Amat[Azz]/ctheta))); |
511 |
+ |
} |
512 |
+ |
psi = atan(Amat[Ayz]/Amat[Azz]); |
513 |
+ |
} |
514 |
+ |
} |
515 |
+ |
|
516 |
+ |
// find sine and cosine of the trial phi and psi values |
517 |
+ |
|
518 |
+ |
cphi = cos(phi); |
519 |
+ |
sphi = sin(phi); |
520 |
+ |
cpsi = cos(psi); |
521 |
+ |
spsi = sin(psi); |
522 |
+ |
|
523 |
+ |
// reconstruct the diagonal of the rotation matrix |
524 |
+ |
|
525 |
+ |
b[0] = ctheta * cphi; |
526 |
+ |
b[1] = spsi*stheta*sphi + cpsi*cphi; |
527 |
+ |
b[2] = ctheta * cpsi; |
528 |
+ |
|
529 |
+ |
// compare the correct matrix diagonal to rebuilt diagonal |
530 |
+ |
|
531 |
+ |
for (int i = 0; i < 3; i++) { |
532 |
+ |
flip[i] = 0; |
533 |
+ |
if (fabs(Amat[3*i + i] - b[i]) > eps) flip[i] = 1; |
534 |
+ |
} |
535 |
+ |
|
536 |
+ |
// alter Euler angles to get correct rotation matrix values |
537 |
+ |
|
538 |
+ |
if (flip[0] && flip[1]) phi = phi - copysign(M_PI,phi); |
539 |
+ |
if (flip[0] && flip[2]) theta = -theta + copysign(M_PI, theta); |
540 |
+ |
if (flip[1] && flip[2]) psi = psi - copysign(M_PI, psi); |
541 |
+ |
|
542 |
+ |
myEuler[0] = phi; |
543 |
+ |
myEuler[1] = theta; |
544 |
+ |
myEuler[2] = psi; |
545 |
+ |
|
546 |
+ |
return; |
547 |
|
} |
548 |
+ |
|
549 |
+ |
double DirectionalAtom::max(double x, double y) { |
550 |
+ |
return (x > y) ? x : y; |
551 |
+ |
} |
552 |
+ |
|
553 |
+ |
double DirectionalAtom::min(double x, double y) { |
554 |
+ |
return (x > y) ? y : x; |
555 |
+ |
} |