| 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 |
|
|
