| 47 |
|
*/ |
| 48 |
|
#ifndef MATH_SQUAREMATRIX3_HPP |
| 49 |
|
#define MATH_SQUAREMATRIX3_HPP |
| 50 |
< |
|
| 50 |
> |
#include <vector> |
| 51 |
|
#include "Quaternion.hpp" |
| 52 |
|
#include "SquareMatrix.hpp" |
| 53 |
|
#include "Vector3.hpp" |
| 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. |
| 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 non-negative. Therefore, it never |
| 273 |
< |
// change the sign of both of the parameters passed to atan2. |
| 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."); |
| 327 |
< |
} |
| 328 |
< |
|
| 329 |
< |
m(0, 0) = this->data_[1][1]*this->data_[2][2] - this->data_[1][2]*this->data_[2][1]; |
| 330 |
< |
m(1, 0) = this->data_[1][2]*this->data_[2][0] - this->data_[1][0]*this->data_[2][2]; |
| 331 |
< |
m(2, 0) = this->data_[1][0]*this->data_[2][1] - this->data_[1][1]*this->data_[2][0]; |
| 332 |
< |
m(0, 1) = this->data_[2][1]*this->data_[0][2] - this->data_[2][2]*this->data_[0][1]; |
| 331 |
< |
m(1, 1) = this->data_[2][2]*this->data_[0][0] - this->data_[2][0]*this->data_[0][2]; |
| 332 |
< |
m(2, 1) = this->data_[2][0]*this->data_[0][1] - this->data_[2][1]*this->data_[0][0]; |
| 333 |
< |
m(0, 2) = this->data_[0][1]*this->data_[1][2] - this->data_[0][2]*this->data_[1][1]; |
| 334 |
< |
m(1, 2) = this->data_[0][2]*this->data_[1][0] - this->data_[0][0]*this->data_[1][2]; |
| 335 |
< |
m(2, 2) = this->data_[0][0]*this->data_[1][1] - this->data_[0][1]*this->data_[1][0]; |
| 327 |
> |
std::vector<int> zeroDiagElementIndex; |
| 328 |
> |
for (int i =0; i < 3; ++i) { |
| 329 |
> |
if (fabs(this->data_[i][i]) <= oopse::epsilon) { |
| 330 |
> |
zeroDiagElementIndex.push_back(i); |
| 331 |
> |
} |
| 332 |
> |
} |
| 333 |
|
|
| 334 |
< |
m /= det; |
| 334 |
> |
if (zeroDiagElementIndex.size() == 2) { |
| 335 |
> |
int index = zeroDiagElementIndex[0]; |
| 336 |
> |
m(index, index) = 1.0 / this->data_[index][index]; |
| 337 |
> |
}else if (zeroDiagElementIndex.size() == 1) { |
| 338 |
> |
|
| 339 |
> |
int a = (zeroDiagElementIndex[0] + 1) % 3; |
| 340 |
> |
int b = (zeroDiagElementIndex[0] + 2) %3; |
| 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 |
> |
|
| 345 |
> |
m(a,b) = -this->data_[a][b]/denom; |
| 346 |
> |
m(b, b) = this->data_[a][a]/denom; |
| 347 |
> |
|
| 348 |
> |
} |
| 349 |
> |
|
| 350 |
> |
/* |
| 351 |
> |
for(std::vector<int>::iterator iter = zeroDiagElementIndex.begin(); iter != zeroDiagElementIndex.end() ++iter) { |
| 352 |
> |
if (this->data_[*iter][0] > oopse::epsilon || this->data_[*iter][1] ||this->data_[*iter][2] || |
| 353 |
> |
this->data_[0][*iter] > oopse::epsilon || this->data_[1][*iter] ||this->data_[2][*iter] ) { |
| 354 |
> |
std::cout << "can not inverse matrix" << std::endl; |
| 355 |
> |
} |
| 356 |
> |
} |
| 357 |
> |
*/ |
| 358 |
> |
} else { |
| 359 |
> |
|
| 360 |
> |
m(0, 0) = this->data_[1][1]*this->data_[2][2] - this->data_[1][2]*this->data_[2][1]; |
| 361 |
> |
m(1, 0) = this->data_[1][2]*this->data_[2][0] - this->data_[1][0]*this->data_[2][2]; |
| 362 |
> |
m(2, 0) = this->data_[1][0]*this->data_[2][1] - this->data_[1][1]*this->data_[2][0]; |
| 363 |
> |
m(0, 1) = this->data_[2][1]*this->data_[0][2] - this->data_[2][2]*this->data_[0][1]; |
| 364 |
> |
m(1, 1) = this->data_[2][2]*this->data_[0][0] - this->data_[2][0]*this->data_[0][2]; |
| 365 |
> |
m(2, 1) = this->data_[2][0]*this->data_[0][1] - this->data_[2][1]*this->data_[0][0]; |
| 366 |
> |
m(0, 2) = this->data_[0][1]*this->data_[1][2] - this->data_[0][2]*this->data_[1][1]; |
| 367 |
> |
m(1, 2) = this->data_[0][2]*this->data_[1][0] - this->data_[0][0]*this->data_[1][2]; |
| 368 |
> |
m(2, 2) = this->data_[0][0]*this->data_[1][1] - this->data_[0][1]*this->data_[1][0]; |
| 369 |
> |
|
| 370 |
> |
m /= det; |
| 371 |
> |
} |
| 372 |
|
return m; |
| 373 |
|
} |
| 374 |
|
|
| 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 |
