# | Line 1 | Line 1 | |
---|---|---|
1 | < | /* |
2 | < | * Copyright (C) 2000-2004 Object Oriented Parallel Simulation Engine (OOPSE) project |
3 | < | * |
4 | < | * Contact: oopse@oopse.org |
5 | < | * |
6 | < | * This program is free software; you can redistribute it and/or |
7 | < | * modify it under the terms of the GNU Lesser General Public License |
8 | < | * as published by the Free Software Foundation; either version 2.1 |
9 | < | * of the License, or (at your option) any later version. |
10 | < | * All we ask is that proper credit is given for our work, which includes |
11 | < | * - but is not limited to - adding the above copyright notice to the beginning |
12 | < | * of your source code files, and to any copyright notice that you may distribute |
13 | < | * with programs based on this work. |
14 | < | * |
15 | < | * This program is distributed in the hope that it will be useful, |
16 | < | * but WITHOUT ANY WARRANTY; without even the implied warranty of |
17 | < | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the |
18 | < | * GNU Lesser General Public License for more details. |
19 | < | * |
20 | < | * You should have received a copy of the GNU Lesser General Public License |
21 | < | * along with this program; if not, write to the Free Software |
22 | < | * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. |
1 | > | /* |
2 | > | * Copyright (c) 2005 The University of Notre Dame. All Rights Reserved. |
3 | * | |
4 | + | * The University of Notre Dame grants you ("Licensee") a |
5 | + | * non-exclusive, royalty free, license to use, modify and |
6 | + | * redistribute this software in source and binary code form, provided |
7 | + | * that the following conditions are met: |
8 | + | * |
9 | + | * 1. Acknowledgement of the program authors must be made in any |
10 | + | * publication of scientific results based in part on use of the |
11 | + | * program. An acceptable form of acknowledgement is citation of |
12 | + | * the article in which the program was described (Matthew |
13 | + | * A. Meineke, Charles F. Vardeman II, Teng Lin, Christopher |
14 | + | * J. Fennell and J. Daniel Gezelter, "OOPSE: An Object-Oriented |
15 | + | * Parallel Simulation Engine for Molecular Dynamics," |
16 | + | * J. Comput. Chem. 26, pp. 252-271 (2005)) |
17 | + | * |
18 | + | * 2. Redistributions of source code must retain the above copyright |
19 | + | * notice, this list of conditions and the following disclaimer. |
20 | + | * |
21 | + | * 3. Redistributions in binary form must reproduce the above copyright |
22 | + | * notice, this list of conditions and the following disclaimer in the |
23 | + | * documentation and/or other materials provided with the |
24 | + | * distribution. |
25 | + | * |
26 | + | * This software is provided "AS IS," without a warranty of any |
27 | + | * kind. All express or implied conditions, representations and |
28 | + | * warranties, including any implied warranty of merchantability, |
29 | + | * fitness for a particular purpose or non-infringement, are hereby |
30 | + | * excluded. The University of Notre Dame and its licensors shall not |
31 | + | * be liable for any damages suffered by licensee as a result of |
32 | + | * using, modifying or distributing the software or its |
33 | + | * derivatives. In no event will the University of Notre Dame or its |
34 | + | * licensors be liable for any lost revenue, profit or data, or for |
35 | + | * direct, indirect, special, consequential, incidental or punitive |
36 | + | * damages, however caused and regardless of the theory of liability, |
37 | + | * arising out of the use of or inability to use software, even if the |
38 | + | * University of Notre Dame has been advised of the possibility of |
39 | + | * such damages. |
40 | */ | |
41 | < | |
41 | > | |
42 | /** | |
43 | * @file SquareMatrix3.hpp | |
44 | * @author Teng Lin | |
45 | * @date 10/11/2004 | |
46 | * @version 1.0 | |
47 | */ | |
48 | < | #ifndef MATH_SQUAREMATRIX3_HPP |
48 | > | #ifndef MATH_SQUAREMATRIX3_HPP |
49 | #define MATH_SQUAREMATRIX3_HPP | |
50 | ||
51 | #include "Quaternion.hpp" | |
# | Line 41 | Line 57 | namespace oopse { | |
57 | template<typename Real> | |
58 | class SquareMatrix3 : public SquareMatrix<Real, 3> { | |
59 | public: | |
60 | + | |
61 | + | typedef Real ElemType; |
62 | + | typedef Real* ElemPoinerType; |
63 | ||
64 | /** default constructor */ | |
65 | SquareMatrix3() : SquareMatrix<Real, 3>() { | |
66 | } | |
67 | ||
68 | + | /** Constructs and initializes every element of this matrix to a scalar */ |
69 | + | SquareMatrix3(Real s) : SquareMatrix<Real,3>(s){ |
70 | + | } |
71 | + | |
72 | + | /** Constructs and initializes from an array */ |
73 | + | SquareMatrix3(Real* array) : SquareMatrix<Real,3>(array){ |
74 | + | } |
75 | + | |
76 | + | |
77 | /** copy constructor */ | |
78 | SquareMatrix3(const SquareMatrix<Real, 3>& m) : SquareMatrix<Real, 3>(m) { | |
79 | } | |
80 | < | |
80 | > | |
81 | SquareMatrix3( const Vector3<Real>& eulerAngles) { | |
82 | setupRotMat(eulerAngles); | |
83 | } | |
# | Line 75 | Line 103 | namespace oopse { | |
103 | return *this; | |
104 | } | |
105 | ||
106 | + | |
107 | + | SquareMatrix3<Real>& operator =(const Quaternion<Real>& q) { |
108 | + | this->setupRotMat(q); |
109 | + | return *this; |
110 | + | } |
111 | + | |
112 | /** | |
113 | * Sets this matrix to a rotation matrix by three euler angles | |
114 | * @ param euler | |
# | Line 100 | Line 134 | namespace oopse { | |
134 | ctheta = cos(theta); | |
135 | cpsi = cos(psi); | |
136 | ||
137 | < | data_[0][0] = cpsi * cphi - ctheta * sphi * spsi; |
138 | < | data_[0][1] = cpsi * sphi + ctheta * cphi * spsi; |
139 | < | data_[0][2] = spsi * stheta; |
137 | > | this->data_[0][0] = cpsi * cphi - ctheta * sphi * spsi; |
138 | > | this->data_[0][1] = cpsi * sphi + ctheta * cphi * spsi; |
139 | > | this->data_[0][2] = spsi * stheta; |
140 | ||
141 | < | data_[1][0] = -spsi * ctheta - ctheta * sphi * cpsi; |
142 | < | data_[1][1] = -spsi * stheta + ctheta * cphi * cpsi; |
143 | < | data_[1][2] = cpsi * stheta; |
141 | > | this->data_[1][0] = -spsi * ctheta - ctheta * sphi * cpsi; |
142 | > | this->data_[1][1] = -spsi * stheta + ctheta * cphi * cpsi; |
143 | > | this->data_[1][2] = cpsi * stheta; |
144 | ||
145 | < | data_[2][0] = stheta * sphi; |
146 | < | data_[2][1] = -stheta * cphi; |
147 | < | data_[2][2] = ctheta; |
145 | > | this->data_[2][0] = stheta * sphi; |
146 | > | this->data_[2][1] = -stheta * cphi; |
147 | > | this->data_[2][2] = ctheta; |
148 | } | |
149 | ||
150 | ||
# | Line 143 | Line 177 | namespace oopse { | |
177 | Quaternion<Real> q; | |
178 | Real t, s; | |
179 | Real ad1, ad2, ad3; | |
180 | < | t = data_[0][0] + data_[1][1] + data_[2][2] + 1.0; |
180 | > | t = this->data_[0][0] + this->data_[1][1] + this->data_[2][2] + 1.0; |
181 | ||
182 | if( t > 0.0 ){ | |
183 | ||
184 | s = 0.5 / sqrt( t ); | |
185 | q[0] = 0.25 / s; | |
186 | < | q[1] = (data_[1][2] - data_[2][1]) * s; |
187 | < | q[2] = (data_[2][0] - data_[0][2]) * s; |
188 | < | q[3] = (data_[0][1] - data_[1][0]) * s; |
186 | > | q[1] = (this->data_[1][2] - this->data_[2][1]) * s; |
187 | > | q[2] = (this->data_[2][0] - this->data_[0][2]) * s; |
188 | > | q[3] = (this->data_[0][1] - this->data_[1][0]) * s; |
189 | } else { | |
190 | ||
191 | < | ad1 = fabs( data_[0][0] ); |
192 | < | ad2 = fabs( data_[1][1] ); |
193 | < | ad3 = fabs( data_[2][2] ); |
191 | > | ad1 = fabs( this->data_[0][0] ); |
192 | > | ad2 = fabs( this->data_[1][1] ); |
193 | > | ad3 = fabs( this->data_[2][2] ); |
194 | ||
195 | if( ad1 >= ad2 && ad1 >= ad3 ){ | |
196 | ||
197 | < | s = 2.0 * sqrt( 1.0 + data_[0][0] - data_[1][1] - data_[2][2] ); |
198 | < | q[0] = (data_[1][2] + data_[2][1]) / s; |
197 | > | s = 2.0 * sqrt( 1.0 + this->data_[0][0] - this->data_[1][1] - this->data_[2][2] ); |
198 | > | q[0] = (this->data_[1][2] + this->data_[2][1]) / s; |
199 | q[1] = 0.5 / s; | |
200 | < | q[2] = (data_[0][1] + data_[1][0]) / s; |
201 | < | q[3] = (data_[0][2] + data_[2][0]) / s; |
200 | > | q[2] = (this->data_[0][1] + this->data_[1][0]) / s; |
201 | > | q[3] = (this->data_[0][2] + this->data_[2][0]) / s; |
202 | } else if ( ad2 >= ad1 && ad2 >= ad3 ) { | |
203 | < | s = sqrt( 1.0 + data_[1][1] - data_[0][0] - data_[2][2] ) * 2.0; |
204 | < | q[0] = (data_[0][2] + data_[2][0]) / s; |
205 | < | q[1] = (data_[0][1] + data_[1][0]) / s; |
203 | > | s = sqrt( 1.0 + this->data_[1][1] - this->data_[0][0] - this->data_[2][2] ) * 2.0; |
204 | > | q[0] = (this->data_[0][2] + this->data_[2][0]) / s; |
205 | > | q[1] = (this->data_[0][1] + this->data_[1][0]) / s; |
206 | q[2] = 0.5 / s; | |
207 | < | q[3] = (data_[1][2] + data_[2][1]) / s; |
207 | > | q[3] = (this->data_[1][2] + this->data_[2][1]) / s; |
208 | } else { | |
209 | ||
210 | < | s = sqrt( 1.0 + data_[2][2] - data_[0][0] - data_[1][1] ) * 2.0; |
211 | < | q[0] = (data_[0][1] + data_[1][0]) / s; |
212 | < | q[1] = (data_[0][2] + data_[2][0]) / s; |
213 | < | q[2] = (data_[1][2] + data_[2][1]) / s; |
210 | > | s = sqrt( 1.0 + this->data_[2][2] - this->data_[0][0] - this->data_[1][1] ) * 2.0; |
211 | > | q[0] = (this->data_[0][1] + this->data_[1][0]) / s; |
212 | > | q[1] = (this->data_[0][2] + this->data_[2][0]) / s; |
213 | > | q[2] = (this->data_[1][2] + this->data_[2][1]) / s; |
214 | q[3] = 0.5 / s; | |
215 | } | |
216 | } | |
# | Line 197 | Line 231 | namespace oopse { | |
231 | */ | |
232 | Vector3<Real> toEulerAngles() { | |
233 | Vector3<Real> myEuler; | |
234 | < | Real phi,theta,psi,eps; |
235 | < | Real ctheta,stheta; |
234 | > | Real phi; |
235 | > | Real theta; |
236 | > | Real psi; |
237 | > | Real ctheta; |
238 | > | Real stheta; |
239 | ||
240 | // set the tolerance for Euler angles and rotation elements | |
241 | ||
242 | < | theta = acos(std::min(1.0, std::max(-1.0,data_[2][2]))); |
243 | < | ctheta = data_[2][2]; |
242 | > | theta = acos(std::min(1.0, std::max(-1.0,this->data_[2][2]))); |
243 | > | ctheta = this->data_[2][2]; |
244 | stheta = sqrt(1.0 - ctheta * ctheta); | |
245 | ||
246 | // when sin(theta) is close to 0, we need to consider singularity | |
# | Line 216 | Line 253 | namespace oopse { | |
253 | ||
254 | if (fabs(stheta) <= oopse::epsilon){ | |
255 | psi = 0.0; | |
256 | < | phi = atan2(-data_[1][0], data_[0][0]); |
256 | > | phi = atan2(-this->data_[1][0], this->data_[0][0]); |
257 | } | |
258 | // we only have one unique solution | |
259 | else{ | |
260 | < | phi = atan2(data_[2][0], -data_[2][1]); |
261 | < | psi = atan2(data_[0][2], data_[1][2]); |
260 | > | phi = atan2(this->data_[2][0], -this->data_[2][1]); |
261 | > | psi = atan2(this->data_[0][2], this->data_[1][2]); |
262 | } | |
263 | ||
264 | //wrap phi and psi, make sure they are in the range from 0 to 2*Pi | |
# | Line 242 | Line 279 | namespace oopse { | |
279 | Real determinant() const { | |
280 | Real x,y,z; | |
281 | ||
282 | < | x = data_[0][0] * (data_[1][1] * data_[2][2] - data_[1][2] * data_[2][1]); |
283 | < | y = data_[0][1] * (data_[1][2] * data_[2][0] - data_[1][0] * data_[2][2]); |
284 | < | z = data_[0][2] * (data_[1][0] * data_[2][1] - data_[1][1] * data_[2][0]); |
282 | > | x = this->data_[0][0] * (this->data_[1][1] * this->data_[2][2] - this->data_[1][2] * this->data_[2][1]); |
283 | > | y = this->data_[0][1] * (this->data_[1][2] * this->data_[2][0] - this->data_[1][0] * this->data_[2][2]); |
284 | > | z = this->data_[0][2] * (this->data_[1][0] * this->data_[2][1] - this->data_[1][1] * this->data_[2][0]); |
285 | ||
286 | return(x + y + z); | |
287 | } | |
288 | + | |
289 | + | /** Returns the trace of this matrix. */ |
290 | + | Real trace() const { |
291 | + | return this->data_[0][0] + this->data_[1][1] + this->data_[2][2]; |
292 | + | } |
293 | ||
294 | /** | |
295 | * Sets the value of this matrix to the inversion of itself. | |
296 | * @note since simple algorithm can be applied to inverse the 3 by 3 matrix, we hide the | |
297 | * implementation of inverse in SquareMatrix class | |
298 | */ | |
299 | < | SquareMatrix3<Real> inverse() { |
299 | > | SquareMatrix3<Real> inverse() const { |
300 | SquareMatrix3<Real> m; | |
301 | double det = determinant(); | |
302 | if (fabs(det) <= oopse::epsilon) { | |
# | Line 262 | Line 304 | namespace oopse { | |
304 | //"This is a runtime or a programming error in your application."); | |
305 | } | |
306 | ||
307 | < | m(0, 0) = data_[1][1]*data_[2][2] - data_[1][2]*data_[2][1]; |
308 | < | m(1, 0) = data_[1][2]*data_[2][0] - data_[1][0]*data_[2][2]; |
309 | < | m(2, 0) = data_[1][0]*data_[2][1] - data_[1][1]*data_[2][0]; |
310 | < | m(0, 1) = data_[2][1]*data_[0][2] - data_[2][2]*data_[0][1]; |
311 | < | m(1, 1) = data_[2][2]*data_[0][0] - data_[2][0]*data_[0][2]; |
312 | < | m(2, 1) = data_[2][0]*data_[0][1] - data_[2][1]*data_[0][0]; |
313 | < | m(0, 2) = data_[0][1]*data_[1][2] - data_[0][2]*data_[1][1]; |
314 | < | m(1, 2) = data_[0][2]*data_[1][0] - data_[0][0]*data_[1][2]; |
315 | < | m(2, 2) = data_[0][0]*data_[1][1] - data_[0][1]*data_[1][0]; |
307 | > | m(0, 0) = this->data_[1][1]*this->data_[2][2] - this->data_[1][2]*this->data_[2][1]; |
308 | > | m(1, 0) = this->data_[1][2]*this->data_[2][0] - this->data_[1][0]*this->data_[2][2]; |
309 | > | m(2, 0) = this->data_[1][0]*this->data_[2][1] - this->data_[1][1]*this->data_[2][0]; |
310 | > | m(0, 1) = this->data_[2][1]*this->data_[0][2] - this->data_[2][2]*this->data_[0][1]; |
311 | > | m(1, 1) = this->data_[2][2]*this->data_[0][0] - this->data_[2][0]*this->data_[0][2]; |
312 | > | m(2, 1) = this->data_[2][0]*this->data_[0][1] - this->data_[2][1]*this->data_[0][0]; |
313 | > | m(0, 2) = this->data_[0][1]*this->data_[1][2] - this->data_[0][2]*this->data_[1][1]; |
314 | > | m(1, 2) = this->data_[0][2]*this->data_[1][0] - this->data_[0][0]*this->data_[1][2]; |
315 | > | m(2, 2) = this->data_[0][0]*this->data_[1][1] - this->data_[0][1]*this->data_[1][0]; |
316 | ||
317 | m /= det; | |
318 | return m; | |
319 | } | |
320 | + | /** |
321 | + | * Extract the eigenvalues and eigenvectors from a 3x3 matrix. |
322 | + | * The eigenvectors (the columns of V) will be normalized. |
323 | + | * The eigenvectors are aligned optimally with the x, y, and z |
324 | + | * axes respectively. |
325 | + | * @param a symmetric matrix whose eigenvectors are to be computed. On return, the matrix is |
326 | + | * overwritten |
327 | + | * @param w will contain the eigenvalues of the matrix On return of this function |
328 | + | * @param v the columns of this matrix will contain the eigenvectors. The eigenvectors are |
329 | + | * normalized and mutually orthogonal. |
330 | + | * @warning a will be overwritten |
331 | + | */ |
332 | + | static void diagonalize(SquareMatrix3<Real>& a, Vector3<Real>& w, SquareMatrix3<Real>& v); |
333 | + | }; |
334 | + | /*========================================================================= |
335 | ||
336 | < | void diagonalize(SquareMatrix3<Real>& a, Vector3<Real>& w, SquareMatrix3<Real>& v) { |
337 | < | int i,j,k,maxI; |
281 | < | Real tmp, maxVal; |
282 | < | Vector3<Real> v_maxI, v_k, v_j; |
336 | > | Program: Visualization Toolkit |
337 | > | Module: $RCSfile: SquareMatrix3.hpp,v $ |
338 | ||
339 | < | // diagonalize using Jacobi |
340 | < | jacobi(a, w, v); |
339 | > | Copyright (c) Ken Martin, Will Schroeder, Bill Lorensen |
340 | > | All rights reserved. |
341 | > | See Copyright.txt or http://www.kitware.com/Copyright.htm for details. |
342 | ||
343 | < | // if all the eigenvalues are the same, return identity matrix |
344 | < | if (w[0] == w[1] && w[0] == w[2] ) { |
345 | < | v = SquareMatrix3<Real>::identity(); |
346 | < | return; |
343 | > | This software is distributed WITHOUT ANY WARRANTY; without even |
344 | > | the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR |
345 | > | PURPOSE. See the above copyright notice for more information. |
346 | > | |
347 | > | =========================================================================*/ |
348 | > | template<typename Real> |
349 | > | void SquareMatrix3<Real>::diagonalize(SquareMatrix3<Real>& a, Vector3<Real>& w, |
350 | > | SquareMatrix3<Real>& v) { |
351 | > | int i,j,k,maxI; |
352 | > | Real tmp, maxVal; |
353 | > | Vector3<Real> v_maxI, v_k, v_j; |
354 | > | |
355 | > | // diagonalize using Jacobi |
356 | > | jacobi(a, w, v); |
357 | > | // if all the eigenvalues are the same, return identity matrix |
358 | > | if (w[0] == w[1] && w[0] == w[2] ) { |
359 | > | v = SquareMatrix3<Real>::identity(); |
360 | > | return; |
361 | > | } |
362 | > | |
363 | > | // transpose temporarily, it makes it easier to sort the eigenvectors |
364 | > | v = v.transpose(); |
365 | > | |
366 | > | // if two eigenvalues are the same, re-orthogonalize to optimally line |
367 | > | // up the eigenvectors with the x, y, and z axes |
368 | > | for (i = 0; i < 3; i++) { |
369 | > | if (w((i+1)%3) == w((i+2)%3)) {// two eigenvalues are the same |
370 | > | // find maximum element of the independant eigenvector |
371 | > | maxVal = fabs(v(i, 0)); |
372 | > | maxI = 0; |
373 | > | for (j = 1; j < 3; j++) { |
374 | > | if (maxVal < (tmp = fabs(v(i, j)))){ |
375 | > | maxVal = tmp; |
376 | > | maxI = j; |
377 | } | |
378 | + | } |
379 | + | |
380 | + | // swap the eigenvector into its proper position |
381 | + | if (maxI != i) { |
382 | + | tmp = w(maxI); |
383 | + | w(maxI) = w(i); |
384 | + | w(i) = tmp; |
385 | ||
386 | < | // transpose temporarily, it makes it easier to sort the eigenvectors |
387 | < | v = v.transpose(); |
388 | < | |
389 | < | // if two eigenvalues are the same, re-orthogonalize to optimally line |
390 | < | // up the eigenvectors with the x, y, and z axes |
391 | < | for (i = 0; i < 3; i++) { |
392 | < | if (w((i+1)%3) == w((i+2)%3)) {// two eigenvalues are the same |
393 | < | // 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; |
386 | > | v.swapRow(i, maxI); |
387 | > | } |
388 | > | // maximum element of eigenvector should be positive |
389 | > | if (v(maxI, maxI) < 0) { |
390 | > | v(maxI, 0) = -v(maxI, 0); |
391 | > | v(maxI, 1) = -v(maxI, 1); |
392 | > | v(maxI, 2) = -v(maxI, 2); |
393 | > | } |
394 | ||
395 | < | v.swapRow(i, maxI); |
396 | < | } |
397 | < | // 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 | < | } |
395 | > | // re-orthogonalize the other two eigenvectors |
396 | > | j = (maxI+1)%3; |
397 | > | k = (maxI+2)%3; |
398 | ||
399 | < | // re-orthogonalize the other two eigenvectors |
400 | < | j = (maxI+1)%3; |
401 | < | k = (maxI+2)%3; |
399 | > | v(j, 0) = 0.0; |
400 | > | v(j, 1) = 0.0; |
401 | > | v(j, 2) = 0.0; |
402 | > | v(j, j) = 1.0; |
403 | ||
404 | < | v(j, 0) = 0.0; |
405 | < | v(j, 1) = 0.0; |
406 | < | v(j, 2) = 0.0; |
407 | < | v(j, j) = 1.0; |
404 | > | /** @todo */ |
405 | > | v_maxI = v.getRow(maxI); |
406 | > | v_j = v.getRow(j); |
407 | > | v_k = cross(v_maxI, v_j); |
408 | > | v_k.normalize(); |
409 | > | v_j = cross(v_k, v_maxI); |
410 | > | v.setRow(j, v_j); |
411 | > | v.setRow(k, v_k); |
412 | ||
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.normalize(); |
339 | – | v_j = cross(v_k, v_maxI); |
340 | – | v.setRow(j, v_j); |
341 | – | v.setRow(k, v_k); |
413 | ||
414 | + | // transpose vectors back to columns |
415 | + | v = v.transpose(); |
416 | + | return; |
417 | + | } |
418 | + | } |
419 | ||
420 | < | // transpose vectors back to columns |
421 | < | v = v.transpose(); |
346 | < | return; |
347 | < | } |
348 | < | } |
420 | > | // the three eigenvalues are different, just sort the eigenvectors |
421 | > | // to align them with the x, y, and z axes |
422 | ||
423 | < | // the three eigenvalues are different, just sort the eigenvectors |
424 | < | // to align them with the x, y, and z axes |
423 | > | // find the vector with the largest x element, make that vector |
424 | > | // the first vector |
425 | > | maxVal = fabs(v(0, 0)); |
426 | > | maxI = 0; |
427 | > | for (i = 1; i < 3; i++) { |
428 | > | if (maxVal < (tmp = fabs(v(i, 0)))) { |
429 | > | maxVal = tmp; |
430 | > | maxI = i; |
431 | > | } |
432 | > | } |
433 | ||
434 | < | // find the vector with the largest x element, make that vector |
435 | < | // the first vector |
436 | < | maxVal = fabs(v(0, 0)); |
437 | < | maxI = 0; |
438 | < | for (i = 1; i < 3; i++) { |
439 | < | if (maxVal < (tmp = fabs(v(i, 0)))) { |
440 | < | maxVal = tmp; |
441 | < | maxI = i; |
442 | < | } |
443 | < | } |
434 | > | // swap eigenvalue and eigenvector |
435 | > | if (maxI != 0) { |
436 | > | tmp = w(maxI); |
437 | > | w(maxI) = w(0); |
438 | > | w(0) = tmp; |
439 | > | v.swapRow(maxI, 0); |
440 | > | } |
441 | > | // do the same for the y element |
442 | > | if (fabs(v(1, 1)) < fabs(v(2, 1))) { |
443 | > | tmp = w(2); |
444 | > | w(2) = w(1); |
445 | > | w(1) = tmp; |
446 | > | v.swapRow(2, 1); |
447 | > | } |
448 | ||
449 | < | // swap eigenvalue and eigenvector |
450 | < | if (maxI != 0) { |
451 | < | tmp = w(maxI); |
452 | < | w(maxI) = w(0); |
453 | < | w(0) = tmp; |
454 | < | v.swapRow(maxI, 0); |
455 | < | } |
456 | < | // 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 | < | } |
449 | > | // ensure that the sign of the eigenvectors is correct |
450 | > | for (i = 0; i < 2; i++) { |
451 | > | if (v(i, i) < 0) { |
452 | > | v(i, 0) = -v(i, 0); |
453 | > | v(i, 1) = -v(i, 1); |
454 | > | v(i, 2) = -v(i, 2); |
455 | > | } |
456 | > | } |
457 | ||
458 | < | // ensure that the sign of the eigenvectors is correct |
459 | < | for (i = 0; i < 2; i++) { |
460 | < | if (v(i, i) < 0) { |
461 | < | v(i, 0) = -v(i, 0); |
462 | < | v(i, 1) = -v(i, 1); |
463 | < | v(i, 2) = -v(i, 2); |
385 | < | } |
386 | < | } |
458 | > | // set sign of final eigenvector to ensure that determinant is positive |
459 | > | if (v.determinant() < 0) { |
460 | > | v(2, 0) = -v(2, 0); |
461 | > | v(2, 1) = -v(2, 1); |
462 | > | v(2, 2) = -v(2, 2); |
463 | > | } |
464 | ||
465 | < | // set sign of final eigenvector to ensure that determinant is positive |
466 | < | if (v.determinant() < 0) { |
467 | < | v(2, 0) = -v(2, 0); |
468 | < | v(2, 1) = -v(2, 1); |
392 | < | v(2, 2) = -v(2, 2); |
393 | < | } |
465 | > | // transpose the eigenvectors back again |
466 | > | v = v.transpose(); |
467 | > | return ; |
468 | > | } |
469 | ||
470 | < | // transpose the eigenvectors back again |
471 | < | v = v.transpose(); |
472 | < | return ; |
470 | > | /** |
471 | > | * Return the multiplication of two matrixes (m1 * m2). |
472 | > | * @return the multiplication of two matrixes |
473 | > | * @param m1 the first matrix |
474 | > | * @param m2 the second matrix |
475 | > | */ |
476 | > | template<typename Real> |
477 | > | inline SquareMatrix3<Real> operator *(const SquareMatrix3<Real>& m1, const SquareMatrix3<Real>& m2) { |
478 | > | SquareMatrix3<Real> result; |
479 | > | |
480 | > | for (unsigned int i = 0; i < 3; i++) |
481 | > | for (unsigned int j = 0; j < 3; j++) |
482 | > | for (unsigned int k = 0; k < 3; k++) |
483 | > | result(i, j) += m1(i, k) * m2(k, j); |
484 | > | |
485 | > | return result; |
486 | > | } |
487 | > | |
488 | > | template<typename Real> |
489 | > | inline SquareMatrix3<Real> outProduct(const Vector3<Real>& v1, const Vector3<Real>& v2) { |
490 | > | SquareMatrix3<Real> result; |
491 | > | |
492 | > | for (unsigned int i = 0; i < 3; i++) { |
493 | > | for (unsigned int j = 0; j < 3; j++) { |
494 | > | result(i, j) = v1[i] * v2[j]; |
495 | > | } |
496 | } | |
497 | < | }; |
497 | > | |
498 | > | return result; |
499 | > | } |
500 | ||
501 | + | |
502 | typedef SquareMatrix3<double> Mat3x3d; | |
503 | typedef SquareMatrix3<double> RotMat3x3d; | |
504 | ||
505 | } //namespace oopse | |
506 | #endif // MATH_SQUAREMATRIX_HPP | |
507 | + |
– | Removed lines |
+ | Added lines |
< | Changed lines |
> | Changed lines |