# | 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> | |
42 | class SquareMatrix3 : public SquareMatrix<Real, 3> { | |
43 | public: | |
44 | + | |
45 | + | typedef Real ElemType; |
46 | + | typedef Real* ElemPoinerType; |
47 | ||
48 | /** default constructor */ | |
49 | SquareMatrix3() : SquareMatrix<Real, 3>() { | |
50 | } | |
51 | ||
52 | + | /** Constructs and initializes every element of this matrix to a scalar */ |
53 | + | SquareMatrix3(Real s) : SquareMatrix<Real,3>(s){ |
54 | + | } |
55 | + | |
56 | + | /** Constructs and initializes from an array */ |
57 | + | SquareMatrix3(Real* array) : SquareMatrix<Real,3>(array){ |
58 | + | } |
59 | + | |
60 | + | |
61 | /** copy constructor */ | |
62 | SquareMatrix3(const SquareMatrix<Real, 3>& m) : SquareMatrix<Real, 3>(m) { | |
63 | } | |
64 | ||
65 | + | SquareMatrix3( const Vector3<Real>& eulerAngles) { |
66 | + | setupRotMat(eulerAngles); |
67 | + | } |
68 | + | |
69 | + | SquareMatrix3(Real phi, Real theta, Real psi) { |
70 | + | setupRotMat(phi, theta, psi); |
71 | + | } |
72 | + | |
73 | + | SquareMatrix3(const Quaternion<Real>& q) { |
74 | + | setupRotMat(q); |
75 | + | |
76 | + | } |
77 | + | |
78 | + | SquareMatrix3(Real w, Real x, Real y, Real z) { |
79 | + | setupRotMat(w, x, y, z); |
80 | + | } |
81 | + | |
82 | /** copy assignment operator */ | |
83 | SquareMatrix3<Real>& operator =(const SquareMatrix<Real, 3>& m) { | |
84 | if (this == &m) | |
85 | return *this; | |
86 | SquareMatrix<Real, 3>::operator=(m); | |
87 | + | return *this; |
88 | } | |
89 | ||
90 | /** | |
91 | * Sets this matrix to a rotation matrix by three euler angles | |
92 | * @ param euler | |
93 | */ | |
94 | < | void setupRotMat(const Vector3d& euler); |
94 | > | void setupRotMat(const Vector3<Real>& eulerAngles) { |
95 | > | setupRotMat(eulerAngles[0], eulerAngles[1], eulerAngles[2]); |
96 | > | } |
97 | ||
98 | /** | |
99 | * Sets this matrix to a rotation matrix by three euler angles | |
# | Line 66 | Line 101 | namespace oopse { | |
101 | * @param theta | |
102 | * @psi theta | |
103 | */ | |
104 | < | void setupRotMat(double phi, double theta, double psi); |
104 | > | void setupRotMat(Real phi, Real theta, Real psi) { |
105 | > | Real sphi, stheta, spsi; |
106 | > | Real cphi, ctheta, cpsi; |
107 | ||
108 | + | sphi = sin(phi); |
109 | + | stheta = sin(theta); |
110 | + | spsi = sin(psi); |
111 | + | cphi = cos(phi); |
112 | + | ctheta = cos(theta); |
113 | + | cpsi = cos(psi); |
114 | ||
115 | + | data_[0][0] = cpsi * cphi - ctheta * sphi * spsi; |
116 | + | data_[0][1] = cpsi * sphi + ctheta * cphi * spsi; |
117 | + | data_[0][2] = spsi * stheta; |
118 | + | |
119 | + | data_[1][0] = -spsi * ctheta - ctheta * sphi * cpsi; |
120 | + | data_[1][1] = -spsi * stheta + ctheta * cphi * cpsi; |
121 | + | data_[1][2] = cpsi * stheta; |
122 | + | |
123 | + | data_[2][0] = stheta * sphi; |
124 | + | data_[2][1] = -stheta * cphi; |
125 | + | data_[2][2] = ctheta; |
126 | + | } |
127 | + | |
128 | + | |
129 | /** | |
130 | * Sets this matrix to a rotation matrix by quaternion | |
131 | * @param quat | |
132 | */ | |
133 | < | void setupRotMat(const Vector4d& quat); |
133 | > | void setupRotMat(const Quaternion<Real>& quat) { |
134 | > | setupRotMat(quat.w(), quat.x(), quat.y(), quat.z()); |
135 | > | } |
136 | ||
137 | /** | |
138 | * Sets this matrix to a rotation matrix by quaternion | |
139 | < | * @param q0 |
140 | < | * @param q1 |
141 | < | * @param q2 |
142 | < | * @parma q3 |
139 | > | * @param w the first element |
140 | > | * @param x the second element |
141 | > | * @param y the third element |
142 | > | * @param z the fourth element |
143 | */ | |
144 | < | void setupRotMat(double q0, double q1, double q2, double q4); |
144 | > | void setupRotMat(Real w, Real x, Real y, Real z) { |
145 | > | Quaternion<Real> q(w, x, y, z); |
146 | > | *this = q.toRotationMatrix3(); |
147 | > | } |
148 | ||
149 | /** | |
150 | * Returns the quaternion from this rotation matrix | |
151 | * @return the quaternion from this rotation matrix | |
152 | * @exception invalid rotation matrix | |
153 | */ | |
154 | < | Quaternion rotMatToQuat(); |
154 | > | Quaternion<Real> toQuaternion() { |
155 | > | Quaternion<Real> q; |
156 | > | Real t, s; |
157 | > | Real ad1, ad2, ad3; |
158 | > | t = data_[0][0] + data_[1][1] + data_[2][2] + 1.0; |
159 | ||
160 | + | if( t > 0.0 ){ |
161 | + | |
162 | + | s = 0.5 / sqrt( t ); |
163 | + | q[0] = 0.25 / s; |
164 | + | q[1] = (data_[1][2] - data_[2][1]) * s; |
165 | + | q[2] = (data_[2][0] - data_[0][2]) * s; |
166 | + | q[3] = (data_[0][1] - data_[1][0]) * s; |
167 | + | } else { |
168 | + | |
169 | + | ad1 = fabs( data_[0][0] ); |
170 | + | ad2 = fabs( data_[1][1] ); |
171 | + | ad3 = fabs( data_[2][2] ); |
172 | + | |
173 | + | if( ad1 >= ad2 && ad1 >= ad3 ){ |
174 | + | |
175 | + | s = 2.0 * sqrt( 1.0 + data_[0][0] - data_[1][1] - data_[2][2] ); |
176 | + | q[0] = (data_[1][2] + data_[2][1]) / s; |
177 | + | q[1] = 0.5 / s; |
178 | + | q[2] = (data_[0][1] + data_[1][0]) / s; |
179 | + | q[3] = (data_[0][2] + data_[2][0]) / s; |
180 | + | } else if ( ad2 >= ad1 && ad2 >= ad3 ) { |
181 | + | s = sqrt( 1.0 + data_[1][1] - data_[0][0] - data_[2][2] ) * 2.0; |
182 | + | q[0] = (data_[0][2] + data_[2][0]) / s; |
183 | + | q[1] = (data_[0][1] + data_[1][0]) / s; |
184 | + | q[2] = 0.5 / s; |
185 | + | q[3] = (data_[1][2] + data_[2][1]) / s; |
186 | + | } else { |
187 | + | |
188 | + | s = sqrt( 1.0 + data_[2][2] - data_[0][0] - data_[1][1] ) * 2.0; |
189 | + | q[0] = (data_[0][1] + data_[1][0]) / s; |
190 | + | q[1] = (data_[0][2] + data_[2][0]) / s; |
191 | + | q[2] = (data_[1][2] + data_[2][1]) / s; |
192 | + | q[3] = 0.5 / s; |
193 | + | } |
194 | + | } |
195 | + | |
196 | + | return q; |
197 | + | |
198 | + | } |
199 | + | |
200 | /** | |
201 | * Returns the euler angles from this rotation matrix | |
202 | < | * @return the quaternion from this rotation matrix |
202 | > | * @return the euler angles in a vector |
203 | * @exception invalid rotation matrix | |
204 | + | * We use so-called "x-convention", which is the most common definition. |
205 | + | * In this convention, the rotation given by Euler angles (phi, theta, psi), where the first |
206 | + | * rotation is by an angle phi about the z-axis, the second is by an angle |
207 | + | * theta (0 <= theta <= 180)about the x-axis, and thethird is by an angle psi about the |
208 | + | * z-axis (again). |
209 | */ | |
210 | < | Vector3d rotMatToEuler(); |
210 | > | Vector3<Real> toEulerAngles() { |
211 | > | Vector3<Real> myEuler; |
212 | > | Real phi,theta,psi,eps; |
213 | > | Real ctheta,stheta; |
214 | > | |
215 | > | // set the tolerance for Euler angles and rotation elements |
216 | > | |
217 | > | theta = acos(std::min(1.0, std::max(-1.0,data_[2][2]))); |
218 | > | ctheta = data_[2][2]; |
219 | > | stheta = sqrt(1.0 - ctheta * ctheta); |
220 | > | |
221 | > | // when sin(theta) is close to 0, we need to consider singularity |
222 | > | // In this case, we can assign an arbitary value to phi (or psi), and then determine |
223 | > | // the psi (or phi) or vice-versa. We'll assume that phi always gets the rotation, and psi is 0 |
224 | > | // in cases of singularity. |
225 | > | // we use atan2 instead of atan, since atan2 will give us -Pi to Pi. |
226 | > | // Since 0 <= theta <= 180, sin(theta) will be always non-negative. Therefore, it never |
227 | > | // change the sign of both of the parameters passed to atan2. |
228 | > | |
229 | > | if (fabs(stheta) <= oopse::epsilon){ |
230 | > | psi = 0.0; |
231 | > | phi = atan2(-data_[1][0], data_[0][0]); |
232 | > | } |
233 | > | // we only have one unique solution |
234 | > | else{ |
235 | > | phi = atan2(data_[2][0], -data_[2][1]); |
236 | > | psi = atan2(data_[0][2], data_[1][2]); |
237 | > | } |
238 | > | |
239 | > | //wrap phi and psi, make sure they are in the range from 0 to 2*Pi |
240 | > | if (phi < 0) |
241 | > | phi += M_PI; |
242 | > | |
243 | > | if (psi < 0) |
244 | > | psi += M_PI; |
245 | > | |
246 | > | myEuler[0] = phi; |
247 | > | myEuler[1] = theta; |
248 | > | myEuler[2] = psi; |
249 | > | |
250 | > | return myEuler; |
251 | > | } |
252 | ||
253 | + | /** Returns the determinant of this matrix. */ |
254 | + | Real determinant() const { |
255 | + | Real x,y,z; |
256 | + | |
257 | + | x = data_[0][0] * (data_[1][1] * data_[2][2] - data_[1][2] * data_[2][1]); |
258 | + | y = data_[0][1] * (data_[1][2] * data_[2][0] - data_[1][0] * data_[2][2]); |
259 | + | z = data_[0][2] * (data_[1][0] * data_[2][1] - data_[1][1] * data_[2][0]); |
260 | + | |
261 | + | return(x + y + z); |
262 | + | } |
263 | + | |
264 | /** | |
265 | * Sets the value of this matrix to the inversion of itself. | |
266 | * @note since simple algorithm can be applied to inverse the 3 by 3 matrix, we hide the | |
267 | * implementation of inverse in SquareMatrix class | |
268 | */ | |
269 | < | void inverse(); |
269 | > | SquareMatrix3<Real> inverse() { |
270 | > | SquareMatrix3<Real> m; |
271 | > | double det = determinant(); |
272 | > | if (fabs(det) <= oopse::epsilon) { |
273 | > | //"The method was called on a matrix with |determinant| <= 1e-6.", |
274 | > | //"This is a runtime or a programming error in your application."); |
275 | > | } |
276 | ||
277 | < | void diagonalize(); |
277 | > | m(0, 0) = data_[1][1]*data_[2][2] - data_[1][2]*data_[2][1]; |
278 | > | m(1, 0) = data_[1][2]*data_[2][0] - data_[1][0]*data_[2][2]; |
279 | > | m(2, 0) = data_[1][0]*data_[2][1] - data_[1][1]*data_[2][0]; |
280 | > | m(0, 1) = data_[2][1]*data_[0][2] - data_[2][2]*data_[0][1]; |
281 | > | m(1, 1) = data_[2][2]*data_[0][0] - data_[2][0]*data_[0][2]; |
282 | > | m(2, 1) = data_[2][0]*data_[0][1] - data_[2][1]*data_[0][0]; |
283 | > | m(0, 2) = data_[0][1]*data_[1][2] - data_[0][2]*data_[1][1]; |
284 | > | m(1, 2) = data_[0][2]*data_[1][0] - data_[0][0]*data_[1][2]; |
285 | > | m(2, 2) = data_[0][0]*data_[1][1] - data_[0][1]*data_[1][0]; |
286 | ||
287 | + | m /= det; |
288 | + | return m; |
289 | + | } |
290 | + | /** |
291 | + | * Extract the eigenvalues and eigenvectors from a 3x3 matrix. |
292 | + | * The eigenvectors (the columns of V) will be normalized. |
293 | + | * The eigenvectors are aligned optimally with the x, y, and z |
294 | + | * axes respectively. |
295 | + | * @param a symmetric matrix whose eigenvectors are to be computed. On return, the matrix is |
296 | + | * overwritten |
297 | + | * @param w will contain the eigenvalues of the matrix On return of this function |
298 | + | * @param v the columns of this matrix will contain the eigenvectors. The eigenvectors are |
299 | + | * normalized and mutually orthogonal. |
300 | + | * @warning a will be overwritten |
301 | + | */ |
302 | + | static void diagonalize(SquareMatrix3<Real>& a, Vector3<Real>& w, SquareMatrix3<Real>& v); |
303 | + | }; |
304 | + | /*========================================================================= |
305 | + | |
306 | + | Program: Visualization Toolkit |
307 | + | Module: $RCSfile: SquareMatrix3.hpp,v $ |
308 | + | |
309 | + | Copyright (c) Ken Martin, Will Schroeder, Bill Lorensen |
310 | + | All rights reserved. |
311 | + | See Copyright.txt or http://www.kitware.com/Copyright.htm for details. |
312 | + | |
313 | + | This software is distributed WITHOUT ANY WARRANTY; without even |
314 | + | the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR |
315 | + | PURPOSE. See the above copyright notice for more information. |
316 | + | |
317 | + | =========================================================================*/ |
318 | + | template<typename Real> |
319 | + | void SquareMatrix3<Real>::diagonalize(SquareMatrix3<Real>& a, Vector3<Real>& w, |
320 | + | SquareMatrix3<Real>& v) { |
321 | + | int i,j,k,maxI; |
322 | + | Real tmp, maxVal; |
323 | + | Vector3<Real> v_maxI, v_k, v_j; |
324 | + | |
325 | + | // diagonalize using Jacobi |
326 | + | jacobi(a, w, v); |
327 | + | // if all the eigenvalues are the same, return identity matrix |
328 | + | if (w[0] == w[1] && w[0] == w[2] ) { |
329 | + | v = SquareMatrix3<Real>::identity(); |
330 | + | return; |
331 | + | } |
332 | + | |
333 | + | // transpose temporarily, it makes it easier to sort the eigenvectors |
334 | + | v = v.transpose(); |
335 | + | |
336 | + | // if two eigenvalues are the same, re-orthogonalize to optimally line |
337 | + | // up the eigenvectors with the x, y, and z axes |
338 | + | for (i = 0; i < 3; i++) { |
339 | + | if (w((i+1)%3) == w((i+2)%3)) {// two eigenvalues are the same |
340 | + | // find maximum element of the independant eigenvector |
341 | + | maxVal = fabs(v(i, 0)); |
342 | + | maxI = 0; |
343 | + | for (j = 1; j < 3; j++) { |
344 | + | if (maxVal < (tmp = fabs(v(i, j)))){ |
345 | + | maxVal = tmp; |
346 | + | maxI = j; |
347 | + | } |
348 | + | } |
349 | + | |
350 | + | // swap the eigenvector into its proper position |
351 | + | if (maxI != i) { |
352 | + | tmp = w(maxI); |
353 | + | w(maxI) = w(i); |
354 | + | w(i) = tmp; |
355 | + | |
356 | + | v.swapRow(i, maxI); |
357 | + | } |
358 | + | // maximum element of eigenvector should be positive |
359 | + | if (v(maxI, maxI) < 0) { |
360 | + | v(maxI, 0) = -v(maxI, 0); |
361 | + | v(maxI, 1) = -v(maxI, 1); |
362 | + | v(maxI, 2) = -v(maxI, 2); |
363 | + | } |
364 | + | |
365 | + | // re-orthogonalize the other two eigenvectors |
366 | + | j = (maxI+1)%3; |
367 | + | k = (maxI+2)%3; |
368 | + | |
369 | + | v(j, 0) = 0.0; |
370 | + | v(j, 1) = 0.0; |
371 | + | v(j, 2) = 0.0; |
372 | + | v(j, j) = 1.0; |
373 | + | |
374 | + | /** @todo */ |
375 | + | v_maxI = v.getRow(maxI); |
376 | + | v_j = v.getRow(j); |
377 | + | v_k = cross(v_maxI, v_j); |
378 | + | v_k.normalize(); |
379 | + | v_j = cross(v_k, v_maxI); |
380 | + | v.setRow(j, v_j); |
381 | + | v.setRow(k, v_k); |
382 | + | |
383 | + | |
384 | + | // transpose vectors back to columns |
385 | + | v = v.transpose(); |
386 | + | return; |
387 | + | } |
388 | + | } |
389 | + | |
390 | + | // the three eigenvalues are different, just sort the eigenvectors |
391 | + | // to align them with the x, y, and z axes |
392 | + | |
393 | + | // find the vector with the largest x element, make that vector |
394 | + | // the first vector |
395 | + | maxVal = fabs(v(0, 0)); |
396 | + | maxI = 0; |
397 | + | for (i = 1; i < 3; i++) { |
398 | + | if (maxVal < (tmp = fabs(v(i, 0)))) { |
399 | + | maxVal = tmp; |
400 | + | maxI = i; |
401 | + | } |
402 | + | } |
403 | + | |
404 | + | // swap eigenvalue and eigenvector |
405 | + | if (maxI != 0) { |
406 | + | tmp = w(maxI); |
407 | + | w(maxI) = w(0); |
408 | + | w(0) = tmp; |
409 | + | v.swapRow(maxI, 0); |
410 | + | } |
411 | + | // do the same for the y element |
412 | + | if (fabs(v(1, 1)) < fabs(v(2, 1))) { |
413 | + | tmp = w(2); |
414 | + | w(2) = w(1); |
415 | + | w(1) = tmp; |
416 | + | v.swapRow(2, 1); |
417 | + | } |
418 | + | |
419 | + | // ensure that the sign of the eigenvectors is correct |
420 | + | for (i = 0; i < 2; i++) { |
421 | + | if (v(i, i) < 0) { |
422 | + | v(i, 0) = -v(i, 0); |
423 | + | v(i, 1) = -v(i, 1); |
424 | + | v(i, 2) = -v(i, 2); |
425 | + | } |
426 | + | } |
427 | + | |
428 | + | // set sign of final eigenvector to ensure that determinant is positive |
429 | + | if (v.determinant() < 0) { |
430 | + | v(2, 0) = -v(2, 0); |
431 | + | v(2, 1) = -v(2, 1); |
432 | + | v(2, 2) = -v(2, 2); |
433 | + | } |
434 | + | |
435 | + | // transpose the eigenvectors back again |
436 | + | v = v.transpose(); |
437 | + | return ; |
438 | } | |
439 | + | typedef SquareMatrix3<double> Mat3x3d; |
440 | + | typedef SquareMatrix3<double> RotMat3x3d; |
441 | ||
442 | < | }; |
442 | > | } //namespace oopse |
443 | > | #endif // MATH_SQUAREMATRIX_HPP |
444 | ||
114 | – | } |
115 | – | #endif // MATH_SQUAREMATRIX#_HPP |
– | Removed lines |
+ | Added lines |
< | Changed lines |
> | Changed lines |