--- trunk/src/math/SquareMatrix3.hpp 2006/02/22 20:35:16 891 +++ trunk/src/math/SquareMatrix3.hpp 2009/09/07 16:31:51 1360 @@ -47,7 +47,7 @@ */ #ifndef MATH_SQUAREMATRIX3_HPP #define MATH_SQUAREMATRIX3_HPP - +#include #include "Quaternion.hpp" #include "SquareMatrix.hpp" #include "Vector3.hpp" @@ -244,10 +244,10 @@ namespace oopse { * @return the euler angles in a vector * @exception invalid rotation matrix * We use so-called "x-convention", which is the most common definition. - * In this convention, the rotation given by Euler angles (phi, theta, psi), where the first - * rotation is by an angle phi about the z-axis, the second is by an angle - * theta (0 <= theta <= 180)about the x-axis, and thethird is by an angle psi about the - * z-axis (again). + * In this convention, the rotation given by Euler angles (phi, theta, + * psi), where the first rotation is by an angle phi about the z-axis, + * the second is by an angle theta (0 <= theta <= 180) about the x-axis, + * and the third is by an angle psi about the z-axis (again). */ Vector3 toEulerAngles() { Vector3 myEuler; @@ -259,19 +259,21 @@ namespace oopse { // set the tolerance for Euler angles and rotation elements - theta = acos(std::min(1.0, std::max(-1.0,this->data_[2][2]))); + theta = acos(std::min((RealType)1.0, std::max((RealType)-1.0,this->data_[2][2]))); ctheta = this->data_[2][2]; stheta = sqrt(1.0 - ctheta * ctheta); - // when sin(theta) is close to 0, we need to consider singularity - // In this case, we can assign an arbitary value to phi (or psi), and then determine - // the psi (or phi) or vice-versa. We'll assume that phi always gets the rotation, and psi is 0 - // in cases of singularity. + // when sin(theta) is close to 0, we need to consider + // singularity In this case, we can assign an arbitary value to + // phi (or psi), and then determine the psi (or phi) or + // vice-versa. We'll assume that phi always gets the rotation, + // and psi is 0 in cases of singularity. // we use atan2 instead of atan, since atan2 will give us -Pi to Pi. - // Since 0 <= theta <= 180, sin(theta) will be always non-negative. Therefore, it never - // change the sign of both of the parameters passed to atan2. + // Since 0 <= theta <= 180, sin(theta) will be always + // non-negative. Therefore, it will never change the sign of both of + // the parameters passed to atan2. - if (fabs(stheta) <= oopse::epsilon){ + if (fabs(stheta) < 1e-6){ psi = 0.0; phi = atan2(-this->data_[1][0], this->data_[0][0]); } @@ -283,10 +285,10 @@ namespace oopse { //wrap phi and psi, make sure they are in the range from 0 to 2*Pi if (phi < 0) - phi += M_PI; + phi += 2.0 * M_PI; if (psi < 0) - psi += M_PI; + psi += 2.0 * M_PI; myEuler[0] = phi; myEuler[1] = theta; @@ -318,23 +320,55 @@ namespace oopse { */ SquareMatrix3 inverse() const { SquareMatrix3 m; - double det = determinant(); + RealType det = determinant(); if (fabs(det) <= oopse::epsilon) { //"The method was called on a matrix with |determinant| <= 1e-6.", //"This is a runtime or a programming error in your application."); - } - - m(0, 0) = this->data_[1][1]*this->data_[2][2] - this->data_[1][2]*this->data_[2][1]; - m(1, 0) = this->data_[1][2]*this->data_[2][0] - this->data_[1][0]*this->data_[2][2]; - m(2, 0) = this->data_[1][0]*this->data_[2][1] - this->data_[1][1]*this->data_[2][0]; - m(0, 1) = this->data_[2][1]*this->data_[0][2] - this->data_[2][2]*this->data_[0][1]; - m(1, 1) = this->data_[2][2]*this->data_[0][0] - this->data_[2][0]*this->data_[0][2]; - m(2, 1) = this->data_[2][0]*this->data_[0][1] - this->data_[2][1]*this->data_[0][0]; - m(0, 2) = this->data_[0][1]*this->data_[1][2] - this->data_[0][2]*this->data_[1][1]; - m(1, 2) = this->data_[0][2]*this->data_[1][0] - this->data_[0][0]*this->data_[1][2]; - m(2, 2) = this->data_[0][0]*this->data_[1][1] - this->data_[0][1]*this->data_[1][0]; + std::vector zeroDiagElementIndex; + for (int i =0; i < 3; ++i) { + if (fabs(this->data_[i][i]) <= oopse::epsilon) { + zeroDiagElementIndex.push_back(i); + } + } - m /= det; + if (zeroDiagElementIndex.size() == 2) { + int index = zeroDiagElementIndex[0]; + m(index, index) = 1.0 / this->data_[index][index]; + }else if (zeroDiagElementIndex.size() == 1) { + + int a = (zeroDiagElementIndex[0] + 1) % 3; + int b = (zeroDiagElementIndex[0] + 2) %3; + RealType denom = this->data_[a][a] * this->data_[b][b] - this->data_[b][a]*this->data_[a][b]; + m(a, a) = this->data_[b][b] /denom; + m(b, a) = -this->data_[b][a]/denom; + + m(a,b) = -this->data_[a][b]/denom; + m(b, b) = this->data_[a][a]/denom; + + } + +/* + for(std::vector::iterator iter = zeroDiagElementIndex.begin(); iter != zeroDiagElementIndex.end() ++iter) { + if (this->data_[*iter][0] > oopse::epsilon || this->data_[*iter][1] ||this->data_[*iter][2] || + this->data_[0][*iter] > oopse::epsilon || this->data_[1][*iter] ||this->data_[2][*iter] ) { + std::cout << "can not inverse matrix" << std::endl; + } + } +*/ + } else { + + m(0, 0) = this->data_[1][1]*this->data_[2][2] - this->data_[1][2]*this->data_[2][1]; + m(1, 0) = this->data_[1][2]*this->data_[2][0] - this->data_[1][0]*this->data_[2][2]; + m(2, 0) = this->data_[1][0]*this->data_[2][1] - this->data_[1][1]*this->data_[2][0]; + m(0, 1) = this->data_[2][1]*this->data_[0][2] - this->data_[2][2]*this->data_[0][1]; + m(1, 1) = this->data_[2][2]*this->data_[0][0] - this->data_[2][0]*this->data_[0][2]; + m(2, 1) = this->data_[2][0]*this->data_[0][1] - this->data_[2][1]*this->data_[0][0]; + m(0, 2) = this->data_[0][1]*this->data_[1][2] - this->data_[0][2]*this->data_[1][1]; + m(1, 2) = this->data_[0][2]*this->data_[1][0] - this->data_[0][0]*this->data_[1][2]; + m(2, 2) = this->data_[0][0]*this->data_[1][1] - this->data_[0][1]*this->data_[1][0]; + + m /= det; + } return m; } @@ -529,8 +563,8 @@ namespace oopse { } - typedef SquareMatrix3 Mat3x3d; - typedef SquareMatrix3 RotMat3x3d; + typedef SquareMatrix3 Mat3x3d; + typedef SquareMatrix3 RotMat3x3d; } //namespace oopse #endif // MATH_SQUAREMATRIX_HPP