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" |
166 |
|
void setupRotMat(Real w, Real x, Real y, Real z) { |
167 |
|
Quaternion<Real> q(w, x, y, z); |
168 |
|
*this = q.toRotationMatrix3(); |
169 |
+ |
} |
170 |
+ |
|
171 |
+ |
void setupSkewMat(Vector3<Real> v) { |
172 |
+ |
setupSkewMat(v[0], v[1], v[2]); |
173 |
+ |
} |
174 |
+ |
|
175 |
+ |
void setupSkewMat(Real v1, Real v2, Real v3) { |
176 |
+ |
this->data_[0][0] = 0; |
177 |
+ |
this->data_[0][1] = -v3; |
178 |
+ |
this->data_[0][2] = v2; |
179 |
+ |
this->data_[1][0] = v3; |
180 |
+ |
this->data_[1][1] = 0; |
181 |
+ |
this->data_[1][2] = -v1; |
182 |
+ |
this->data_[2][0] = -v2; |
183 |
+ |
this->data_[2][1] = v1; |
184 |
+ |
this->data_[2][2] = 0; |
185 |
+ |
|
186 |
+ |
|
187 |
|
} |
188 |
|
|
189 |
+ |
|
190 |
+ |
|
191 |
|
/** |
192 |
|
* Returns the quaternion from this rotation matrix |
193 |
|
* @return the quaternion from this rotation matrix |
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."); |
325 |
< |
} |
325 |
> |
std::vector<int> zeroDiagElementIndex; |
326 |
> |
for (int i =0; i < 3; ++i) { |
327 |
> |
if (fabs(this->data_[i][i]) <= oopse::epsilon) { |
328 |
> |
zeroDiagElementIndex.push_back(i); |
329 |
> |
} |
330 |
> |
} |
331 |
|
|
332 |
< |
m(0, 0) = this->data_[1][1]*this->data_[2][2] - this->data_[1][2]*this->data_[2][1]; |
333 |
< |
m(1, 0) = this->data_[1][2]*this->data_[2][0] - this->data_[1][0]*this->data_[2][2]; |
334 |
< |
m(2, 0) = this->data_[1][0]*this->data_[2][1] - this->data_[1][1]*this->data_[2][0]; |
335 |
< |
m(0, 1) = this->data_[2][1]*this->data_[0][2] - this->data_[2][2]*this->data_[0][1]; |
311 |
< |
m(1, 1) = this->data_[2][2]*this->data_[0][0] - this->data_[2][0]*this->data_[0][2]; |
312 |
< |
m(2, 1) = this->data_[2][0]*this->data_[0][1] - this->data_[2][1]*this->data_[0][0]; |
313 |
< |
m(0, 2) = this->data_[0][1]*this->data_[1][2] - this->data_[0][2]*this->data_[1][1]; |
314 |
< |
m(1, 2) = this->data_[0][2]*this->data_[1][0] - this->data_[0][0]*this->data_[1][2]; |
315 |
< |
m(2, 2) = this->data_[0][0]*this->data_[1][1] - this->data_[0][1]*this->data_[1][0]; |
332 |
> |
if (zeroDiagElementIndex.size() == 2) { |
333 |
> |
int index = zeroDiagElementIndex[0]; |
334 |
> |
m(index, index) = 1.0 / this->data_[index][index]; |
335 |
> |
}else if (zeroDiagElementIndex.size() == 1) { |
336 |
|
|
337 |
< |
m /= det; |
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]; |
340 |
> |
m(a, a) = this->data_[b][b] /denom; |
341 |
> |
m(b, a) = -this->data_[b][a]/denom; |
342 |
> |
|
343 |
> |
m(a,b) = -this->data_[a][b]/denom; |
344 |
> |
m(b, b) = this->data_[a][a]/denom; |
345 |
> |
|
346 |
> |
} |
347 |
> |
|
348 |
> |
/* |
349 |
> |
for(std::vector<int>::iterator iter = zeroDiagElementIndex.begin(); iter != zeroDiagElementIndex.end() ++iter) { |
350 |
> |
if (this->data_[*iter][0] > oopse::epsilon || this->data_[*iter][1] ||this->data_[*iter][2] || |
351 |
> |
this->data_[0][*iter] > oopse::epsilon || this->data_[1][*iter] ||this->data_[2][*iter] ) { |
352 |
> |
std::cout << "can not inverse matrix" << std::endl; |
353 |
> |
} |
354 |
> |
} |
355 |
> |
*/ |
356 |
> |
} else { |
357 |
> |
|
358 |
> |
m(0, 0) = this->data_[1][1]*this->data_[2][2] - this->data_[1][2]*this->data_[2][1]; |
359 |
> |
m(1, 0) = this->data_[1][2]*this->data_[2][0] - this->data_[1][0]*this->data_[2][2]; |
360 |
> |
m(2, 0) = this->data_[1][0]*this->data_[2][1] - this->data_[1][1]*this->data_[2][0]; |
361 |
> |
m(0, 1) = this->data_[2][1]*this->data_[0][2] - this->data_[2][2]*this->data_[0][1]; |
362 |
> |
m(1, 1) = this->data_[2][2]*this->data_[0][0] - this->data_[2][0]*this->data_[0][2]; |
363 |
> |
m(2, 1) = this->data_[2][0]*this->data_[0][1] - this->data_[2][1]*this->data_[0][0]; |
364 |
> |
m(0, 2) = this->data_[0][1]*this->data_[1][2] - this->data_[0][2]*this->data_[1][1]; |
365 |
> |
m(1, 2) = this->data_[0][2]*this->data_[1][0] - this->data_[0][0]*this->data_[1][2]; |
366 |
> |
m(2, 2) = this->data_[0][0]*this->data_[1][1] - this->data_[0][1]*this->data_[1][0]; |
367 |
> |
|
368 |
> |
m /= det; |
369 |
> |
} |
370 |
|
return m; |
371 |
|
} |
372 |
+ |
|
373 |
+ |
SquareMatrix3<Real> transpose() const{ |
374 |
+ |
SquareMatrix3<Real> result; |
375 |
+ |
|
376 |
+ |
for (unsigned int i = 0; i < 3; i++) |
377 |
+ |
for (unsigned int j = 0; j < 3; j++) |
378 |
+ |
result(j, i) = this->data_[i][j]; |
379 |
+ |
|
380 |
+ |
return result; |
381 |
+ |
} |
382 |
|
/** |
383 |
|
* Extract the eigenvalues and eigenvectors from a 3x3 matrix. |
384 |
|
* The eigenvectors (the columns of V) will be normalized. |