| 59 |
|
} |
| 60 |
|
|
| 61 |
|
SquareMatrix3(const Quaternion<Real>& q) { |
| 62 |
< |
*this = q.toRotationMatrix3(); |
| 62 |
> |
setupRotMat(q); |
| 63 |
> |
|
| 64 |
|
} |
| 65 |
|
|
| 66 |
|
SquareMatrix3(Real w, Real x, Real y, Real z) { |
| 67 |
< |
Quaternion<Real> q(w, x, y, z); |
| 67 |
< |
*this = q.toRotationMatrix3(); |
| 67 |
> |
setupRotMat(w, x, y, z); |
| 68 |
|
} |
| 69 |
|
|
| 70 |
|
/** copy assignment operator */ |
| 119 |
|
* @param quat |
| 120 |
|
*/ |
| 121 |
|
void setupRotMat(const Quaternion<Real>& quat) { |
| 122 |
< |
*this = quat.toRotationMatrix3(); |
| 122 |
> |
setupRotMat(quat.w(), quat.x(), quat.y(), quat.z()); |
| 123 |
|
} |
| 124 |
|
|
| 125 |
|
/** |
| 196 |
|
* z-axis (again). |
| 197 |
|
*/ |
| 198 |
|
Vector3<Real> toEulerAngles() { |
| 199 |
< |
Vector<Real> myEuler; |
| 199 |
> |
Vector3<Real> myEuler; |
| 200 |
|
Real phi,theta,psi,eps; |
| 201 |
|
Real ctheta,stheta; |
| 202 |
|
|
| 203 |
|
// set the tolerance for Euler angles and rotation elements |
| 204 |
|
|
| 205 |
< |
theta = acos(min(1.0,max(-1.0,data_[2][2]))); |
| 205 |
> |
theta = acos(std::min(1.0, std::max(-1.0,data_[2][2]))); |
| 206 |
|
ctheta = data_[2][2]; |
| 207 |
|
stheta = sqrt(1.0 - ctheta * ctheta); |
| 208 |
|
|
| 285 |
|
jacobi(a, w, v); |
| 286 |
|
|
| 287 |
|
// if all the eigenvalues are the same, return identity matrix |
| 288 |
< |
if (w[0] == w[1] && w[0] == w[2] ){ |
| 288 |
> |
if (w[0] == w[1] && w[0] == w[2] ) { |
| 289 |
|
v = SquareMatrix3<Real>::identity(); |
| 290 |
< |
return |
| 290 |
> |
return; |
| 291 |
|
} |
| 292 |
|
|
| 293 |
|
// transpose temporarily, it makes it easier to sort the eigenvectors |
| 294 |
< |
v = v.tanspose(); |
| 294 |
> |
v = v.transpose(); |
| 295 |
|
|
| 296 |
|
// if two eigenvalues are the same, re-orthogonalize to optimally line |
| 297 |
|
// up the eigenvectors with the x, y, and z axes |
| 335 |
|
v_maxI = v.getRow(maxI); |
| 336 |
|
v_j = v.getRow(j); |
| 337 |
|
v_k = cross(v_maxI, v_j); |
| 338 |
< |
v_k.normailze(); |
| 338 |
> |
v_k.normalize(); |
| 339 |
|
v_j = cross(v_k, v_maxI); |
| 340 |
|
v.setRow(j, v_j); |
| 341 |
|
v.setRow(k, v_k); |
| 386 |
|
} |
| 387 |
|
|
| 388 |
|
// set sign of final eigenvector to ensure that determinant is positive |
| 389 |
< |
if (determinant(v) < 0) { |
| 389 |
> |
if (v.determinant() < 0) { |
| 390 |
|
v(2, 0) = -v(2, 0); |
| 391 |
|
v(2, 1) = -v(2, 1); |
| 392 |
|
v(2, 2) = -v(2, 2); |