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 |
|
|
318 |
|
*/ |
319 |
|
SquareMatrix3<Real> inverse() const { |
320 |
|
SquareMatrix3<Real> m; |
321 |
< |
double det = determinant(); |
321 |
> |
RealType det = determinant(); |
322 |
|
if (fabs(det) <= oopse::epsilon) { |
323 |
|
//"The method was called on a matrix with |determinant| <= 1e-6.", |
324 |
|
//"This is a runtime or a programming error in your application."); |
336 |
|
|
337 |
|
int a = (zeroDiagElementIndex[0] + 1) % 3; |
338 |
|
int b = (zeroDiagElementIndex[0] + 2) %3; |
339 |
< |
double denom = this->data_[a][a] * this->data_[b][b] - this->data_[b][a]*this->data_[a][b]; |
339 |
> |
RealType denom = this->data_[a][a] * this->data_[b][b] - this->data_[b][a]*this->data_[a][b]; |
340 |
|
m(a, a) = this->data_[b][b] /denom; |
341 |
|
m(b, a) = -this->data_[b][a]/denom; |
342 |
|
|
561 |
|
} |
562 |
|
|
563 |
|
|
564 |
< |
typedef SquareMatrix3<double> Mat3x3d; |
565 |
< |
typedef SquareMatrix3<double> RotMat3x3d; |
564 |
> |
typedef SquareMatrix3<RealType> Mat3x3d; |
565 |
> |
typedef SquareMatrix3<RealType> RotMat3x3d; |
566 |
|
|
567 |
|
} //namespace oopse |
568 |
|
#endif // MATH_SQUAREMATRIX_HPP |