# | Line 59 | Line 59 | namespace oopse { | |
---|---|---|
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 */ | |
# | Line 119 | Line 119 | namespace oopse { | |
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 | /** | |
# | Line 196 | Line 196 | namespace oopse { | |
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 | ||
# | Line 285 | Line 285 | namespace oopse { | |
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 | |
# | Line 335 | Line 335 | namespace oopse { | |
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); | |
# | Line 386 | Line 386 | namespace oopse { | |
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); |
– | Removed lines |
+ | Added lines |
< | Changed lines |
> | Changed lines |