| 244 |
|
* @return the euler angles in a vector |
| 245 |
|
* @exception invalid rotation matrix |
| 246 |
|
* We use so-called "x-convention", which is the most common definition. |
| 247 |
< |
* In this convention, the rotation given by Euler angles (phi, theta, psi), where the first |
| 248 |
< |
* rotation is by an angle phi about the z-axis, the second is by an angle |
| 249 |
< |
* theta (0 <= theta <= 180)about the x-axis, and thethird is by an angle psi about the |
| 250 |
< |
* z-axis (again). |
| 247 |
> |
* In this convention, the rotation given by Euler angles (phi, theta, |
| 248 |
> |
* psi), where the first rotation is by an angle phi about the z-axis, |
| 249 |
> |
* the second is by an angle theta (0 <= theta <= 180) about the x-axis, |
| 250 |
> |
* and the third is by an angle psi about the z-axis (again). |
| 251 |
|
*/ |
| 252 |
|
Vector3<Real> toEulerAngles() { |
| 253 |
|
Vector3<Real> myEuler; |
| 263 |
|
ctheta = this->data_[2][2]; |
| 264 |
|
stheta = sqrt(1.0 - ctheta * ctheta); |
| 265 |
|
|
| 266 |
< |
// when sin(theta) is close to 0, we need to consider singularity |
| 267 |
< |
// In this case, we can assign an arbitary value to phi (or psi), and then determine |
| 268 |
< |
// the psi (or phi) or vice-versa. We'll assume that phi always gets the rotation, and psi is 0 |
| 269 |
< |
// in cases of singularity. |
| 270 |
< |
// we use atan2 instead of atan, since atan2 will give us -Pi to Pi. |
| 271 |
< |
// Since 0 <= theta <= 180, sin(theta) will be always non-negative. Therefore, it never |
| 272 |
< |
// change the sign of both of the parameters passed to atan2. |
| 266 |
> |
// when sin(theta) is close to 0, we need to consider |
| 267 |
> |
// singularity In this case, we can assign an arbitary value to |
| 268 |
> |
// phi (or psi), and then determine the psi (or phi) or |
| 269 |
> |
// vice-versa. We'll assume that phi always gets the rotation, |
| 270 |
> |
// and psi is 0 in cases of singularity. |
| 271 |
> |
// we use atan2 instead of atan, since atan2 will give us -Pi to Pi. |
| 272 |
> |
// Since 0 <= theta <= 180, sin(theta) will be always |
| 273 |
> |
// non-negative. Therefore, it will never change the sign of both of |
| 274 |
> |
// the parameters passed to atan2. |
| 275 |
|
|
| 276 |
< |
if (fabs(stheta) <= oopse::epsilon){ |
| 276 |
> |
if (fabs(stheta) < 1e-6){ |
| 277 |
|
psi = 0.0; |
| 278 |
|
phi = atan2(-this->data_[1][0], this->data_[0][0]); |
| 279 |
|
} |
| 285 |
|
|
| 286 |
|
//wrap phi and psi, make sure they are in the range from 0 to 2*Pi |
| 287 |
|
if (phi < 0) |
| 288 |
< |
phi += M_PI; |
| 288 |
> |
phi += 2.0 * M_PI; |
| 289 |
|
|
| 290 |
|
if (psi < 0) |
| 291 |
< |
psi += M_PI; |
| 291 |
> |
psi += 2.0 * M_PI; |
| 292 |
|
|
| 293 |
|
myEuler[0] = phi; |
| 294 |
|
myEuler[1] = theta; |
