| 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; |
| 259 |
|
|
| 260 |
|
// set the tolerance for Euler angles and rotation elements |
| 261 |
|
|
| 262 |
< |
theta = acos(std::min(1.0, std::max(-1.0,this->data_[2][2]))); |
| 262 |
> |
theta = acos(std::min((RealType)1.0, std::max((RealType)-1.0,this->data_[2][2]))); |
| 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; |
| 320 |
|
*/ |
| 321 |
|
SquareMatrix3<Real> inverse() const { |
| 322 |
|
SquareMatrix3<Real> m; |
| 323 |
< |
double det = determinant(); |
| 323 |
> |
RealType det = determinant(); |
| 324 |
|
if (fabs(det) <= oopse::epsilon) { |
| 325 |
|
//"The method was called on a matrix with |determinant| <= 1e-6.", |
| 326 |
|
//"This is a runtime or a programming error in your application."); |
| 338 |
|
|
| 339 |
|
int a = (zeroDiagElementIndex[0] + 1) % 3; |
| 340 |
|
int b = (zeroDiagElementIndex[0] + 2) %3; |
| 341 |
< |
double denom = this->data_[a][a] * this->data_[b][b] - this->data_[b][a]*this->data_[a][b]; |
| 341 |
> |
RealType denom = this->data_[a][a] * this->data_[b][b] - this->data_[b][a]*this->data_[a][b]; |
| 342 |
|
m(a, a) = this->data_[b][b] /denom; |
| 343 |
|
m(b, a) = -this->data_[b][a]/denom; |
| 344 |
|
|
| 563 |
|
} |
| 564 |
|
|
| 565 |
|
|
| 566 |
< |
typedef SquareMatrix3<double> Mat3x3d; |
| 567 |
< |
typedef SquareMatrix3<double> RotMat3x3d; |
| 566 |
> |
typedef SquareMatrix3<RealType> Mat3x3d; |
| 567 |
> |
typedef SquareMatrix3<RealType> RotMat3x3d; |
| 568 |
|
|
| 569 |
|
} //namespace oopse |
| 570 |
|
#endif // MATH_SQUAREMATRIX_HPP |
