# | Line 29 | Line 29 | |
---|---|---|
29 | * @date 10/11/2004 | |
30 | * @version 1.0 | |
31 | */ | |
32 | < | #ifndef MATH_SQUAREMATRIX#_HPP |
33 | < | #define MATH_SQUAREMATRIX#_HPP |
32 | > | #ifndef MATH_SQUAREMATRIX3_HPP |
33 | > | #define MATH_SQUAREMATRIX3_HPP |
34 | ||
35 | + | #include "Quaternion.hpp" |
36 | #include "SquareMatrix.hpp" | |
37 | + | #include "Vector3.hpp" |
38 | + | |
39 | namespace oopse { | |
40 | ||
41 | template<typename Real> | |
# | Line 47 | Line 50 | namespace oopse { | |
50 | SquareMatrix3(const SquareMatrix<Real, 3>& m) : SquareMatrix<Real, 3>(m) { | |
51 | } | |
52 | ||
53 | + | SquareMatrix3( const Vector3<Real>& eulerAngles) { |
54 | + | setupRotMat(eulerAngles); |
55 | + | } |
56 | + | |
57 | + | SquareMatrix3(Real phi, Real theta, Real psi) { |
58 | + | setupRotMat(phi, theta, psi); |
59 | + | } |
60 | + | |
61 | + | SquareMatrix3(const Quaternion<Real>& q) { |
62 | + | *this = q.toRotationMatrix3(); |
63 | + | } |
64 | + | |
65 | + | SquareMatrix3(Real w, Real x, Real y, Real z) { |
66 | + | Quaternion<Real> q(w, x, y, z); |
67 | + | *this = q.toRotationMatrix3(); |
68 | + | } |
69 | + | |
70 | /** copy assignment operator */ | |
71 | SquareMatrix3<Real>& operator =(const SquareMatrix<Real, 3>& m) { | |
72 | if (this == &m) | |
73 | return *this; | |
74 | SquareMatrix<Real, 3>::operator=(m); | |
75 | + | return *this; |
76 | } | |
77 | ||
78 | /** | |
79 | * Sets this matrix to a rotation matrix by three euler angles | |
80 | * @ param euler | |
81 | */ | |
82 | < | void setupRotMat(const Vector3d& euler); |
82 | > | void setupRotMat(const Vector3<Real>& eulerAngles) { |
83 | > | setupRotMat(eulerAngles[0], eulerAngles[1], eulerAngles[2]); |
84 | > | } |
85 | ||
86 | /** | |
87 | * Sets this matrix to a rotation matrix by three euler angles | |
# | Line 66 | Line 89 | namespace oopse { | |
89 | * @param theta | |
90 | * @psi theta | |
91 | */ | |
92 | < | void setupRotMat(double phi, double theta, double psi); |
92 | > | void setupRotMat(Real phi, Real theta, Real psi) { |
93 | > | Real sphi, stheta, spsi; |
94 | > | Real cphi, ctheta, cpsi; |
95 | ||
96 | + | sphi = sin(phi); |
97 | + | stheta = sin(theta); |
98 | + | spsi = sin(psi); |
99 | + | cphi = cos(phi); |
100 | + | ctheta = cos(theta); |
101 | + | cpsi = cos(psi); |
102 | ||
103 | + | data_[0][0] = cpsi * cphi - ctheta * sphi * spsi; |
104 | + | data_[0][1] = cpsi * sphi + ctheta * cphi * spsi; |
105 | + | data_[0][2] = spsi * stheta; |
106 | + | |
107 | + | data_[1][0] = -spsi * ctheta - ctheta * sphi * cpsi; |
108 | + | data_[1][1] = -spsi * stheta + ctheta * cphi * cpsi; |
109 | + | data_[1][2] = cpsi * stheta; |
110 | + | |
111 | + | data_[2][0] = stheta * sphi; |
112 | + | data_[2][1] = -stheta * cphi; |
113 | + | data_[2][2] = ctheta; |
114 | + | } |
115 | + | |
116 | + | |
117 | /** | |
118 | * Sets this matrix to a rotation matrix by quaternion | |
119 | * @param quat | |
120 | */ | |
121 | < | void setupRotMat(const Vector4d& quat); |
121 | > | void setupRotMat(const Quaternion<Real>& quat) { |
122 | > | *this = quat.toRotationMatrix3(); |
123 | > | } |
124 | ||
125 | /** | |
126 | * Sets this matrix to a rotation matrix by quaternion | |
127 | < | * @param q0 |
128 | < | * @param q1 |
129 | < | * @param q2 |
130 | < | * @parma q3 |
127 | > | * @param w the first element |
128 | > | * @param x the second element |
129 | > | * @param y the third element |
130 | > | * @param z the fourth element |
131 | */ | |
132 | < | void setupRotMat(double q0, double q1, double q2, double q4); |
132 | > | void setupRotMat(Real w, Real x, Real y, Real z) { |
133 | > | Quaternion<Real> q(w, x, y, z); |
134 | > | *this = q.toRotationMatrix3(); |
135 | > | } |
136 | ||
137 | /** | |
138 | * Returns the quaternion from this rotation matrix | |
139 | * @return the quaternion from this rotation matrix | |
140 | * @exception invalid rotation matrix | |
141 | */ | |
142 | < | Quaternion rotMatToQuat(); |
142 | > | Quaternion<Real> toQuaternion() { |
143 | > | Quaternion<Real> q; |
144 | > | Real t, s; |
145 | > | Real ad1, ad2, ad3; |
146 | > | t = data_[0][0] + data_[1][1] + data_[2][2] + 1.0; |
147 | ||
148 | + | if( t > 0.0 ){ |
149 | + | |
150 | + | s = 0.5 / sqrt( t ); |
151 | + | q[0] = 0.25 / s; |
152 | + | q[1] = (data_[1][2] - data_[2][1]) * s; |
153 | + | q[2] = (data_[2][0] - data_[0][2]) * s; |
154 | + | q[3] = (data_[0][1] - data_[1][0]) * s; |
155 | + | } else { |
156 | + | |
157 | + | ad1 = fabs( data_[0][0] ); |
158 | + | ad2 = fabs( data_[1][1] ); |
159 | + | ad3 = fabs( data_[2][2] ); |
160 | + | |
161 | + | if( ad1 >= ad2 && ad1 >= ad3 ){ |
162 | + | |
163 | + | s = 2.0 * sqrt( 1.0 + data_[0][0] - data_[1][1] - data_[2][2] ); |
164 | + | q[0] = (data_[1][2] + data_[2][1]) / s; |
165 | + | q[1] = 0.5 / s; |
166 | + | q[2] = (data_[0][1] + data_[1][0]) / s; |
167 | + | q[3] = (data_[0][2] + data_[2][0]) / s; |
168 | + | } else if ( ad2 >= ad1 && ad2 >= ad3 ) { |
169 | + | s = sqrt( 1.0 + data_[1][1] - data_[0][0] - data_[2][2] ) * 2.0; |
170 | + | q[0] = (data_[0][2] + data_[2][0]) / s; |
171 | + | q[1] = (data_[0][1] + data_[1][0]) / s; |
172 | + | q[2] = 0.5 / s; |
173 | + | q[3] = (data_[1][2] + data_[2][1]) / s; |
174 | + | } else { |
175 | + | |
176 | + | s = sqrt( 1.0 + data_[2][2] - data_[0][0] - data_[1][1] ) * 2.0; |
177 | + | q[0] = (data_[0][1] + data_[1][0]) / s; |
178 | + | q[1] = (data_[0][2] + data_[2][0]) / s; |
179 | + | q[2] = (data_[1][2] + data_[2][1]) / s; |
180 | + | q[3] = 0.5 / s; |
181 | + | } |
182 | + | } |
183 | + | |
184 | + | return q; |
185 | + | |
186 | + | } |
187 | + | |
188 | /** | |
189 | * Returns the euler angles from this rotation matrix | |
190 | < | * @return the quaternion from this rotation matrix |
190 | > | * @return the euler angles in a vector |
191 | * @exception invalid rotation matrix | |
192 | + | * We use so-called "x-convention", which is the most common definition. |
193 | + | * In this convention, the rotation given by Euler angles (phi, theta, psi), where the first |
194 | + | * rotation is by an angle phi about the z-axis, the second is by an angle |
195 | + | * theta (0 <= theta <= 180)about the x-axis, and thethird is by an angle psi about the |
196 | + | * z-axis (again). |
197 | */ | |
198 | < | Vector3d rotMatToEuler(); |
198 | > | Vector3<Real> toEulerAngles() { |
199 | > | Vector<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]))); |
206 | > | ctheta = data_[2][2]; |
207 | > | stheta = sqrt(1.0 - ctheta * ctheta); |
208 | > | |
209 | > | // when sin(theta) is close to 0, we need to consider singularity |
210 | > | // In this case, we can assign an arbitary value to phi (or psi), and then determine |
211 | > | // the psi (or phi) or vice-versa. We'll assume that phi always gets the rotation, and psi is 0 |
212 | > | // in cases of singularity. |
213 | > | // we use atan2 instead of atan, since atan2 will give us -Pi to Pi. |
214 | > | // Since 0 <= theta <= 180, sin(theta) will be always non-negative. Therefore, it never |
215 | > | // change the sign of both of the parameters passed to atan2. |
216 | > | |
217 | > | if (fabs(stheta) <= oopse::epsilon){ |
218 | > | psi = 0.0; |
219 | > | phi = atan2(-data_[1][0], data_[0][0]); |
220 | > | } |
221 | > | // we only have one unique solution |
222 | > | else{ |
223 | > | phi = atan2(data_[2][0], -data_[2][1]); |
224 | > | psi = atan2(data_[0][2], data_[1][2]); |
225 | > | } |
226 | > | |
227 | > | //wrap phi and psi, make sure they are in the range from 0 to 2*Pi |
228 | > | if (phi < 0) |
229 | > | phi += M_PI; |
230 | > | |
231 | > | if (psi < 0) |
232 | > | psi += M_PI; |
233 | > | |
234 | > | myEuler[0] = phi; |
235 | > | myEuler[1] = theta; |
236 | > | myEuler[2] = psi; |
237 | > | |
238 | > | return myEuler; |
239 | > | } |
240 | > | |
241 | > | /** Returns the determinant of this matrix. */ |
242 | > | Real determinant() const { |
243 | > | Real x,y,z; |
244 | > | |
245 | > | x = data_[0][0] * (data_[1][1] * data_[2][2] - data_[1][2] * data_[2][1]); |
246 | > | y = data_[0][1] * (data_[1][2] * data_[2][0] - data_[1][0] * data_[2][2]); |
247 | > | z = data_[0][2] * (data_[1][0] * data_[2][1] - data_[1][1] * data_[2][0]); |
248 | > | |
249 | > | return(x + y + z); |
250 | > | } |
251 | ||
252 | /** | |
253 | * Sets the value of this matrix to the inversion of itself. | |
254 | * @note since simple algorithm can be applied to inverse the 3 by 3 matrix, we hide the | |
255 | * implementation of inverse in SquareMatrix class | |
256 | */ | |
257 | < | void inverse(); |
257 | > | SquareMatrix3<Real> inverse() { |
258 | > | SquareMatrix3<Real> m; |
259 | > | double det = determinant(); |
260 | > | if (fabs(det) <= oopse::epsilon) { |
261 | > | //"The method was called on a matrix with |determinant| <= 1e-6.", |
262 | > | //"This is a runtime or a programming error in your application."); |
263 | > | } |
264 | ||
265 | < | void diagonalize(); |
265 | > | m(0, 0) = data_[1][1]*data_[2][2] - data_[1][2]*data_[2][1]; |
266 | > | m(1, 0) = data_[1][2]*data_[2][0] - data_[1][0]*data_[2][2]; |
267 | > | m(2, 0) = data_[1][0]*data_[2][1] - data_[1][1]*data_[2][0]; |
268 | > | m(0, 1) = data_[2][1]*data_[0][2] - data_[2][2]*data_[0][1]; |
269 | > | m(1, 1) = data_[2][2]*data_[0][0] - data_[2][0]*data_[0][2]; |
270 | > | m(2, 1) = data_[2][0]*data_[0][1] - data_[2][1]*data_[0][0]; |
271 | > | m(0, 2) = data_[0][1]*data_[1][2] - data_[0][2]*data_[1][1]; |
272 | > | m(1, 2) = data_[0][2]*data_[1][0] - data_[0][0]*data_[1][2]; |
273 | > | m(2, 2) = data_[0][0]*data_[1][1] - data_[0][1]*data_[1][0]; |
274 | ||
275 | < | } |
275 | > | m /= det; |
276 | > | return m; |
277 | > | } |
278 | ||
279 | + | void diagonalize(SquareMatrix3<Real>& a, Vector3<Real>& w, SquareMatrix3<Real>& v) { |
280 | + | int i,j,k,maxI; |
281 | + | Real tmp, maxVal; |
282 | + | Vector3<Real> v_maxI, v_k, v_j; |
283 | + | |
284 | + | // diagonalize using Jacobi |
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] ){ |
289 | + | v = SquareMatrix3<Real>::identity(); |
290 | + | return |
291 | + | } |
292 | + | |
293 | + | // transpose temporarily, it makes it easier to sort the eigenvectors |
294 | + | v = v.tanspose(); |
295 | + | |
296 | + | // if two eigenvalues are the same, re-orthogonalize to optimally line |
297 | + | // up the eigenvectors with the x, y, and z axes |
298 | + | for (i = 0; i < 3; i++) { |
299 | + | if (w((i+1)%3) == w((i+2)%3)) {// two eigenvalues are the same |
300 | + | // find maximum element of the independant eigenvector |
301 | + | maxVal = fabs(v(i, 0)); |
302 | + | maxI = 0; |
303 | + | for (j = 1; j < 3; j++) { |
304 | + | if (maxVal < (tmp = fabs(v(i, j)))){ |
305 | + | maxVal = tmp; |
306 | + | maxI = j; |
307 | + | } |
308 | + | } |
309 | + | |
310 | + | // swap the eigenvector into its proper position |
311 | + | if (maxI != i) { |
312 | + | tmp = w(maxI); |
313 | + | w(maxI) = w(i); |
314 | + | w(i) = tmp; |
315 | + | |
316 | + | v.swapRow(i, maxI); |
317 | + | } |
318 | + | // maximum element of eigenvector should be positive |
319 | + | if (v(maxI, maxI) < 0) { |
320 | + | v(maxI, 0) = -v(maxI, 0); |
321 | + | v(maxI, 1) = -v(maxI, 1); |
322 | + | v(maxI, 2) = -v(maxI, 2); |
323 | + | } |
324 | + | |
325 | + | // re-orthogonalize the other two eigenvectors |
326 | + | j = (maxI+1)%3; |
327 | + | k = (maxI+2)%3; |
328 | + | |
329 | + | v(j, 0) = 0.0; |
330 | + | v(j, 1) = 0.0; |
331 | + | v(j, 2) = 0.0; |
332 | + | v(j, j) = 1.0; |
333 | + | |
334 | + | /** @todo */ |
335 | + | v_maxI = v.getRow(maxI); |
336 | + | v_j = v.getRow(j); |
337 | + | v_k = cross(v_maxI, v_j); |
338 | + | v_k.normailze(); |
339 | + | v_j = cross(v_k, v_maxI); |
340 | + | v.setRow(j, v_j); |
341 | + | v.setRow(k, v_k); |
342 | + | |
343 | + | |
344 | + | // transpose vectors back to columns |
345 | + | v = v.transpose(); |
346 | + | return; |
347 | + | } |
348 | + | } |
349 | + | |
350 | + | // the three eigenvalues are different, just sort the eigenvectors |
351 | + | // to align them with the x, y, and z axes |
352 | + | |
353 | + | // find the vector with the largest x element, make that vector |
354 | + | // the first vector |
355 | + | maxVal = fabs(v(0, 0)); |
356 | + | maxI = 0; |
357 | + | for (i = 1; i < 3; i++) { |
358 | + | if (maxVal < (tmp = fabs(v(i, 0)))) { |
359 | + | maxVal = tmp; |
360 | + | maxI = i; |
361 | + | } |
362 | + | } |
363 | + | |
364 | + | // swap eigenvalue and eigenvector |
365 | + | if (maxI != 0) { |
366 | + | tmp = w(maxI); |
367 | + | w(maxI) = w(0); |
368 | + | w(0) = tmp; |
369 | + | v.swapRow(maxI, 0); |
370 | + | } |
371 | + | // do the same for the y element |
372 | + | if (fabs(v(1, 1)) < fabs(v(2, 1))) { |
373 | + | tmp = w(2); |
374 | + | w(2) = w(1); |
375 | + | w(1) = tmp; |
376 | + | v.swapRow(2, 1); |
377 | + | } |
378 | + | |
379 | + | // ensure that the sign of the eigenvectors is correct |
380 | + | for (i = 0; i < 2; i++) { |
381 | + | if (v(i, i) < 0) { |
382 | + | v(i, 0) = -v(i, 0); |
383 | + | v(i, 1) = -v(i, 1); |
384 | + | v(i, 2) = -v(i, 2); |
385 | + | } |
386 | + | } |
387 | + | |
388 | + | // set sign of final eigenvector to ensure that determinant is positive |
389 | + | if (determinant(v) < 0) { |
390 | + | v(2, 0) = -v(2, 0); |
391 | + | v(2, 1) = -v(2, 1); |
392 | + | v(2, 2) = -v(2, 2); |
393 | + | } |
394 | + | |
395 | + | // transpose the eigenvectors back again |
396 | + | v = v.transpose(); |
397 | + | return ; |
398 | + | } |
399 | }; | |
400 | ||
401 | < | } |
402 | < | #endif // MATH_SQUAREMATRIX#_HPP |
401 | > | typedef SquareMatrix3<double> Mat3x3d; |
402 | > | typedef SquareMatrix3<double> RotMat3x3d; |
403 | > | |
404 | > | } //namespace oopse |
405 | > | #endif // MATH_SQUAREMATRIX_HPP |
– | Removed lines |
+ | Added lines |
< | Changed lines |
> | Changed lines |