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); |