429 |
|
ephi[0] = 0.0; |
430 |
|
ephi[1] = 0.0; |
431 |
|
ephi[2] = 1.0; |
432 |
< |
etheta[0] = -sphi; |
433 |
< |
etheta[1] = cphi; |
432 |
> |
|
433 |
> |
etheta[0] = cphi; |
434 |
> |
etheta[1] = sphi; |
435 |
|
etheta[2] = 0.0; |
436 |
< |
epsi[0] = ctheta * cphi; |
437 |
< |
epsi[1] = ctheta * sphi; |
438 |
< |
epsi[2] = -stheta; |
436 |
> |
|
437 |
> |
epsi[0] = stheta * cphi; |
438 |
> |
epsi[1] = stheta * sphi; |
439 |
> |
epsi[2] = ctheta; |
440 |
|
|
441 |
|
for (int j = 0 ; j<3; j++) |
442 |
|
grad[j] = frc[j]; |
443 |
|
|
444 |
+ |
grad[3] = 0; |
445 |
+ |
grad[4] = 0; |
446 |
+ |
grad[5] = 0; |
447 |
+ |
|
448 |
|
for (int j = 0; j < 3; j++ ) { |
449 |
|
|
450 |
|
grad[3] += trq[j]*ephi[j]; |
455 |
|
|
456 |
|
} |
457 |
|
|
458 |
< |
|
458 |
> |
/** |
459 |
> |
* getEulerAngles computes a set of Euler angle values consistent |
460 |
> |
* with an input rotation matrix. They are returned in the following |
461 |
> |
* order: |
462 |
> |
* myEuler[0] = phi; |
463 |
> |
* myEuler[1] = theta; |
464 |
> |
* myEuler[2] = psi; |
465 |
> |
*/ |
466 |
|
void DirectionalAtom::getEulerAngles(double myEuler[3]) { |
467 |
|
|
468 |
< |
// getEulerAngles computes a set of Euler angle values consistent |
469 |
< |
// with an input rotation matrix. They are returned in the following |
470 |
< |
// order: |
471 |
< |
// myEuler[0] = phi; |
472 |
< |
// myEuler[1] = theta; |
460 |
< |
// myEuler[2] = psi; |
468 |
> |
// We use so-called "x-convention", which is the most common definition. |
469 |
> |
// In this convention, the rotation given by Euler angles (phi, theta, psi), where the first |
470 |
> |
// rotation is by an angle phi about the z-axis, the second is by an angle |
471 |
> |
// theta (0 <= theta <= 180)about the x-axis, and thethird is by an angle psi about the |
472 |
> |
//z-axis (again). |
473 |
|
|
474 |
+ |
|
475 |
|
double phi,theta,psi,eps; |
476 |
|
double pi; |
477 |
|
double cphi,ctheta,cpsi; |
482 |
|
// set the tolerance for Euler angles and rotation elements |
483 |
|
|
484 |
|
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 |
– |
} |
485 |
|
|
486 |
< |
// set the phi and psi values for all other theta values |
486 |
> |
theta = acos(min(1.0,max(-1.0,Amat[Azz]))); |
487 |
> |
ctheta = Amat[Azz]; |
488 |
> |
stheta = sqrt(1.0 - ctheta * ctheta); |
489 |
> |
|
490 |
> |
// when sin(theta) is close to 0, we need to consider singularity |
491 |
> |
// In this case, we can assign an arbitary value to phi (or psi), and then determine |
492 |
> |
// the psi (or phi) or vice-versa. We'll assume that phi always gets the rotation, and psi is 0 |
493 |
> |
// in cases of singularity. |
494 |
> |
// we use atan2 instead of atan, since atan2 will give us -Pi to Pi. |
495 |
> |
// Since 0 <= theta <= 180, sin(theta) will be always non-negative. Therefore, it never |
496 |
> |
// change the sign of both of the parameters passed to atan2. |
497 |
|
|
498 |
< |
else { |
499 |
< |
if (fabs(Amat[Axx]) < eps) { |
500 |
< |
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 |
< |
} |
498 |
> |
if (fabs(stheta) <= eps){ |
499 |
> |
psi = 0.0; |
500 |
> |
phi = atan2(-Amat[Ayx], Amat[Axx]); |
501 |
|
} |
502 |
< |
|
503 |
< |
// find sine and cosine of the trial phi and psi values |
504 |
< |
|
505 |
< |
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; |
502 |
> |
// we only have one unique solution |
503 |
> |
else{ |
504 |
> |
phi = atan2(Amat[Azx], -Amat[Azy]); |
505 |
> |
psi = atan2(Amat[Axz], Amat[Ayz]); |
506 |
|
} |
507 |
|
|
508 |
< |
// alter Euler angles to get correct rotation matrix values |
509 |
< |
|
510 |
< |
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); |
508 |
> |
//wrap phi and psi, make sure they are in the range from 0 to 2*Pi |
509 |
> |
//if (phi < 0) |
510 |
> |
// phi += M_PI; |
511 |
|
|
512 |
+ |
//if (psi < 0) |
513 |
+ |
// psi += M_PI; |
514 |
+ |
|
515 |
|
myEuler[0] = phi; |
516 |
|
myEuler[1] = theta; |
517 |
|
myEuler[2] = psi; |
518 |
< |
|
518 |
> |
|
519 |
|
return; |
520 |
|
} |
521 |
|
|