# | 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. |
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. Redistributions of source code must retain the above copyright |
10 | + | * notice, this list of conditions and the following disclaimer. |
11 | + | * |
12 | + | * 2. Redistributions in binary form must reproduce the above copyright |
13 | + | * notice, this list of conditions and the following disclaimer in the |
14 | + | * documentation and/or other materials provided with the |
15 | + | * distribution. |
16 | + | * |
17 | + | * This software is provided "AS IS," without a warranty of any |
18 | + | * kind. All express or implied conditions, representations and |
19 | + | * warranties, including any implied warranty of merchantability, |
20 | + | * fitness for a particular purpose or non-infringement, are hereby |
21 | + | * excluded. The University of Notre Dame and its licensors shall not |
22 | + | * be liable for any damages suffered by licensee as a result of |
23 | + | * using, modifying or distributing the software or its |
24 | + | * derivatives. In no event will the University of Notre Dame or its |
25 | + | * licensors be liable for any lost revenue, profit or data, or for |
26 | + | * direct, indirect, special, consequential, incidental or punitive |
27 | + | * damages, however caused and regardless of the theory of liability, |
28 | + | * arising out of the use of or inability to use software, even if the |
29 | + | * University of Notre Dame has been advised of the possibility of |
30 | + | * such damages. |
31 | + | * |
32 | + | * SUPPORT OPEN SCIENCE! If you use OpenMD or its source code in your |
33 | + | * research, please cite the appropriate papers when you publish your |
34 | + | * work. Good starting points are: |
35 | + | * |
36 | + | * [1] Meineke, et al., J. Comp. Chem. 26, 252-271 (2005). |
37 | + | * [2] Fennell & Gezelter, J. Chem. Phys. 124, 234104 (2006). |
38 | + | * [3] Sun, Lin & Gezelter, J. Chem. Phys. 128, 24107 (2008). |
39 | + | * [4] Vardeman & Gezelter, in progress (2009). |
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_SQUAREMATRIX#_HPP |
49 | < | #define MATH_SQUAREMATRIX#_HPP |
50 | < | |
48 | > | #ifndef MATH_SQUAREMATRIX3_HPP |
49 | > | #define MATH_SQUAREMATRIX3_HPP |
50 | > | #include <vector> |
51 | > | #include "Quaternion.hpp" |
52 | #include "SquareMatrix.hpp" | |
53 | < | namespace oopse { |
53 | > | #include "Vector3.hpp" |
54 | > | #include "utils/NumericConstant.hpp" |
55 | > | namespace OpenMD { |
56 | ||
57 | < | template<typename Real> |
58 | < | class SquareMatrix3 : public SquareMatrix<Real, 3> { |
59 | < | public: |
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 | < | } |
64 | > | /** default constructor */ |
65 | > | SquareMatrix3() : SquareMatrix<Real, 3>() { |
66 | > | } |
67 | ||
68 | < | /** copy constructor */ |
69 | < | SquareMatrix3(const SquareMatrix<Real, 3>& m) : SquareMatrix<Real, 3>(m) { |
70 | < | } |
68 | > | /** Constructs and initializes every element of this matrix to a scalar */ |
69 | > | SquareMatrix3(Real s) : SquareMatrix<Real,3>(s){ |
70 | > | } |
71 | ||
72 | < | /** copy assignment operator */ |
73 | < | SquareMatrix3<Real>& operator =(const SquareMatrix<Real, 3>& m) { |
74 | < | if (this == &m) |
53 | < | return *this; |
54 | < | SquareMatrix<Real, 3>::operator=(m); |
55 | < | } |
72 | > | /** Constructs and initializes from an array */ |
73 | > | SquareMatrix3(Real* array) : SquareMatrix<Real,3>(array){ |
74 | > | } |
75 | ||
57 | – | /** |
58 | – | * Sets this matrix to a rotation matrix by three euler angles |
59 | – | * @ param euler |
60 | – | */ |
61 | – | void setupRotMat(const Vector3d& euler); |
76 | ||
77 | < | /** |
78 | < | * Sets this matrix to a rotation matrix by three euler angles |
79 | < | * @param phi |
80 | < | * @param theta |
81 | < | * @psi theta |
82 | < | */ |
83 | < | void setupRotMat(double phi, double theta, double psi); |
77 | > | /** copy constructor */ |
78 | > | SquareMatrix3(const SquareMatrix<Real, 3>& m) : SquareMatrix<Real, 3>(m) { |
79 | > | } |
80 | > | |
81 | > | SquareMatrix3( const Vector3<Real>& eulerAngles) { |
82 | > | setupRotMat(eulerAngles); |
83 | > | } |
84 | > | |
85 | > | SquareMatrix3(Real phi, Real theta, Real psi) { |
86 | > | setupRotMat(phi, theta, psi); |
87 | > | } |
88 | ||
89 | + | SquareMatrix3(const Quaternion<Real>& q) { |
90 | + | setupRotMat(q); |
91 | ||
92 | < | /** |
73 | < | * Sets this matrix to a rotation matrix by quaternion |
74 | < | * @param quat |
75 | < | */ |
76 | < | void setupRotMat(const Vector4d& quat); |
92 | > | } |
93 | ||
94 | < | /** |
95 | < | * Sets this matrix to a rotation matrix by quaternion |
96 | < | * @param q0 |
97 | < | * @param q1 |
98 | < | * @param q2 |
99 | < | * @parma q3 |
100 | < | */ |
101 | < | void setupRotMat(double q0, double q1, double q2, double q4); |
94 | > | SquareMatrix3(Real w, Real x, Real y, Real z) { |
95 | > | setupRotMat(w, x, y, z); |
96 | > | } |
97 | > | |
98 | > | /** copy assignment operator */ |
99 | > | SquareMatrix3<Real>& operator =(const SquareMatrix<Real, 3>& m) { |
100 | > | if (this == &m) |
101 | > | return *this; |
102 | > | SquareMatrix<Real, 3>::operator=(m); |
103 | > | return *this; |
104 | > | } |
105 | ||
87 | – | /** |
88 | – | * Returns the quaternion from this rotation matrix |
89 | – | * @return the quaternion from this rotation matrix |
90 | – | * @exception invalid rotation matrix |
91 | – | */ |
92 | – | Quaternion rotMatToQuat(); |
106 | ||
107 | < | /** |
108 | < | * Returns the euler angles from this rotation matrix |
109 | < | * @return the quaternion from this rotation matrix |
110 | < | * @exception invalid rotation matrix |
98 | < | */ |
99 | < | Vector3d rotMatToEuler(); |
100 | < | |
101 | < | /** |
102 | < | * Sets the value of this matrix to the inversion of itself. |
103 | < | * @note since simple algorithm can be applied to inverse the 3 by 3 matrix, we hide the |
104 | < | * implementation of inverse in SquareMatrix class |
105 | < | */ |
106 | < | void inverse(); |
107 | > | SquareMatrix3<Real>& operator =(const Quaternion<Real>& q) { |
108 | > | this->setupRotMat(q); |
109 | > | return *this; |
110 | > | } |
111 | ||
112 | < | void diagonalize(); |
112 | > | /** |
113 | > | * Sets this matrix to a rotation matrix by three euler angles |
114 | > | * @ param euler |
115 | > | */ |
116 | > | void setupRotMat(const Vector3<Real>& eulerAngles) { |
117 | > | setupRotMat(eulerAngles[0], eulerAngles[1], eulerAngles[2]); |
118 | > | } |
119 | ||
120 | + | /** |
121 | + | * Sets this matrix to a rotation matrix by three euler angles |
122 | + | * @param phi |
123 | + | * @param theta |
124 | + | * @psi theta |
125 | + | */ |
126 | + | void setupRotMat(Real phi, Real theta, Real psi) { |
127 | + | Real sphi, stheta, spsi; |
128 | + | Real cphi, ctheta, cpsi; |
129 | + | |
130 | + | sphi = sin(phi); |
131 | + | stheta = sin(theta); |
132 | + | spsi = sin(psi); |
133 | + | cphi = cos(phi); |
134 | + | ctheta = cos(theta); |
135 | + | cpsi = cos(psi); |
136 | + | |
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 | + | 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 | + | this->data_[2][0] = stheta * sphi; |
146 | + | this->data_[2][1] = -stheta * cphi; |
147 | + | this->data_[2][2] = ctheta; |
148 | } | |
149 | ||
112 | – | }; |
150 | ||
151 | < | } |
152 | < | #endif // MATH_SQUAREMATRIX#_HPP |
151 | > | /** |
152 | > | * Sets this matrix to a rotation matrix by quaternion |
153 | > | * @param quat |
154 | > | */ |
155 | > | void setupRotMat(const Quaternion<Real>& quat) { |
156 | > | setupRotMat(quat.w(), quat.x(), quat.y(), quat.z()); |
157 | > | } |
158 | > | |
159 | > | /** |
160 | > | * Sets this matrix to a rotation matrix by quaternion |
161 | > | * @param w the first element |
162 | > | * @param x the second element |
163 | > | * @param y the third element |
164 | > | * @param z the fourth element |
165 | > | */ |
166 | > | void setupRotMat(Real w, Real x, Real y, Real z) { |
167 | > | Quaternion<Real> q(w, x, y, z); |
168 | > | *this = q.toRotationMatrix3(); |
169 | > | } |
170 | > | |
171 | > | void setupSkewMat(Vector3<Real> v) { |
172 | > | setupSkewMat(v[0], v[1], v[2]); |
173 | > | } |
174 | > | |
175 | > | void setupSkewMat(Real v1, Real v2, Real v3) { |
176 | > | this->data_[0][0] = 0; |
177 | > | this->data_[0][1] = -v3; |
178 | > | this->data_[0][2] = v2; |
179 | > | this->data_[1][0] = v3; |
180 | > | this->data_[1][1] = 0; |
181 | > | this->data_[1][2] = -v1; |
182 | > | this->data_[2][0] = -v2; |
183 | > | this->data_[2][1] = v1; |
184 | > | this->data_[2][2] = 0; |
185 | > | |
186 | > | |
187 | > | } |
188 | > | |
189 | > | |
190 | > | |
191 | > | /** |
192 | > | * Returns the quaternion from this rotation matrix |
193 | > | * @return the quaternion from this rotation matrix |
194 | > | * @exception invalid rotation matrix |
195 | > | */ |
196 | > | Quaternion<Real> toQuaternion() { |
197 | > | Quaternion<Real> q; |
198 | > | Real t, s; |
199 | > | Real ad1, ad2, ad3; |
200 | > | t = this->data_[0][0] + this->data_[1][1] + this->data_[2][2] + 1.0; |
201 | > | |
202 | > | if( t > NumericConstant::epsilon ){ |
203 | > | |
204 | > | s = 0.5 / sqrt( t ); |
205 | > | q[0] = 0.25 / s; |
206 | > | q[1] = (this->data_[1][2] - this->data_[2][1]) * s; |
207 | > | q[2] = (this->data_[2][0] - this->data_[0][2]) * s; |
208 | > | q[3] = (this->data_[0][1] - this->data_[1][0]) * s; |
209 | > | } else { |
210 | > | |
211 | > | ad1 = this->data_[0][0]; |
212 | > | ad2 = this->data_[1][1]; |
213 | > | ad3 = this->data_[2][2]; |
214 | > | |
215 | > | if( ad1 >= ad2 && ad1 >= ad3 ){ |
216 | > | |
217 | > | s = 0.5 / sqrt( 1.0 + this->data_[0][0] - this->data_[1][1] - this->data_[2][2] ); |
218 | > | q[0] = (this->data_[1][2] - this->data_[2][1]) * s; |
219 | > | q[1] = 0.25 / s; |
220 | > | q[2] = (this->data_[0][1] + this->data_[1][0]) * s; |
221 | > | q[3] = (this->data_[0][2] + this->data_[2][0]) * s; |
222 | > | } else if ( ad2 >= ad1 && ad2 >= ad3 ) { |
223 | > | s = 0.5 / sqrt( 1.0 + this->data_[1][1] - this->data_[0][0] - this->data_[2][2] ); |
224 | > | q[0] = (this->data_[2][0] - this->data_[0][2] ) * s; |
225 | > | q[1] = (this->data_[0][1] + this->data_[1][0]) * s; |
226 | > | q[2] = 0.25 / s; |
227 | > | q[3] = (this->data_[1][2] + this->data_[2][1]) * s; |
228 | > | } else { |
229 | > | |
230 | > | s = 0.5 / sqrt( 1.0 + this->data_[2][2] - this->data_[0][0] - this->data_[1][1] ); |
231 | > | q[0] = (this->data_[0][1] - this->data_[1][0]) * s; |
232 | > | q[1] = (this->data_[0][2] + this->data_[2][0]) * s; |
233 | > | q[2] = (this->data_[1][2] + this->data_[2][1]) * s; |
234 | > | q[3] = 0.25 / s; |
235 | > | } |
236 | > | } |
237 | > | |
238 | > | return q; |
239 | > | |
240 | > | } |
241 | > | |
242 | > | /** |
243 | > | * Returns the euler angles from this rotation matrix |
244 | > | * @return the euler angles in a vector |
245 | > | * @exception invalid rotation matrix |
246 | > | * We use so-called "x-convention", which is the most common definition. |
247 | > | * In this convention, the rotation given by Euler angles (phi, theta, |
248 | > | * psi), where the first rotation is by an angle phi about the z-axis, |
249 | > | * the second is by an angle theta (0 <= theta <= 180) about the x-axis, |
250 | > | * and the third is by an angle psi about the z-axis (again). |
251 | > | */ |
252 | > | Vector3<Real> toEulerAngles() { |
253 | > | Vector3<Real> myEuler; |
254 | > | Real phi; |
255 | > | Real theta; |
256 | > | Real psi; |
257 | > | Real ctheta; |
258 | > | Real stheta; |
259 | > | |
260 | > | // set the tolerance for Euler angles and rotation elements |
261 | > | |
262 | > | theta = acos(std::min((RealType)1.0, std::max((RealType)-1.0,this->data_[2][2]))); |
263 | > | ctheta = this->data_[2][2]; |
264 | > | stheta = sqrt(1.0 - ctheta * ctheta); |
265 | > | |
266 | > | // when sin(theta) is close to 0, we need to consider |
267 | > | // singularity In this case, we can assign an arbitary value to |
268 | > | // phi (or psi), and then determine the psi (or phi) or |
269 | > | // vice-versa. We'll assume that phi always gets the rotation, |
270 | > | // and psi is 0 in cases of singularity. |
271 | > | // we use atan2 instead of atan, since atan2 will give us -Pi to Pi. |
272 | > | // Since 0 <= theta <= 180, sin(theta) will be always |
273 | > | // non-negative. Therefore, it will never change the sign of both of |
274 | > | // the parameters passed to atan2. |
275 | > | |
276 | > | if (fabs(stheta) < 1e-6){ |
277 | > | psi = 0.0; |
278 | > | phi = atan2(-this->data_[1][0], this->data_[0][0]); |
279 | > | } |
280 | > | // we only have one unique solution |
281 | > | else{ |
282 | > | phi = atan2(this->data_[2][0], -this->data_[2][1]); |
283 | > | psi = atan2(this->data_[0][2], this->data_[1][2]); |
284 | > | } |
285 | > | |
286 | > | //wrap phi and psi, make sure they are in the range from 0 to 2*Pi |
287 | > | if (phi < 0) |
288 | > | phi += 2.0 * M_PI; |
289 | > | |
290 | > | if (psi < 0) |
291 | > | psi += 2.0 * M_PI; |
292 | > | |
293 | > | myEuler[0] = phi; |
294 | > | myEuler[1] = theta; |
295 | > | myEuler[2] = psi; |
296 | > | |
297 | > | return myEuler; |
298 | > | } |
299 | > | |
300 | > | /** Returns the determinant of this matrix. */ |
301 | > | Real determinant() const { |
302 | > | Real x,y,z; |
303 | > | |
304 | > | x = this->data_[0][0] * (this->data_[1][1] * this->data_[2][2] - this->data_[1][2] * this->data_[2][1]); |
305 | > | y = this->data_[0][1] * (this->data_[1][2] * this->data_[2][0] - this->data_[1][0] * this->data_[2][2]); |
306 | > | z = this->data_[0][2] * (this->data_[1][0] * this->data_[2][1] - this->data_[1][1] * this->data_[2][0]); |
307 | > | |
308 | > | return(x + y + z); |
309 | > | } |
310 | > | |
311 | > | /** Returns the trace of this matrix. */ |
312 | > | Real trace() const { |
313 | > | return this->data_[0][0] + this->data_[1][1] + this->data_[2][2]; |
314 | > | } |
315 | > | |
316 | > | /** |
317 | > | * Sets the value of this matrix to the inversion of itself. |
318 | > | * @note since simple algorithm can be applied to inverse the 3 by 3 matrix, we hide the |
319 | > | * implementation of inverse in SquareMatrix class |
320 | > | */ |
321 | > | SquareMatrix3<Real> inverse() const { |
322 | > | SquareMatrix3<Real> m; |
323 | > | RealType det = determinant(); |
324 | > | if (fabs(det) <= OpenMD::epsilon) { |
325 | > | //"The method was called on a matrix with |determinant| <= 1e-6.", |
326 | > | //"This is a runtime or a programming error in your application."); |
327 | > | std::vector<int> zeroDiagElementIndex; |
328 | > | for (int i =0; i < 3; ++i) { |
329 | > | if (fabs(this->data_[i][i]) <= OpenMD::epsilon) { |
330 | > | zeroDiagElementIndex.push_back(i); |
331 | > | } |
332 | > | } |
333 | > | |
334 | > | if (zeroDiagElementIndex.size() == 2) { |
335 | > | int index = zeroDiagElementIndex[0]; |
336 | > | m(index, index) = 1.0 / this->data_[index][index]; |
337 | > | }else if (zeroDiagElementIndex.size() == 1) { |
338 | > | |
339 | > | int a = (zeroDiagElementIndex[0] + 1) % 3; |
340 | > | int b = (zeroDiagElementIndex[0] + 2) %3; |
341 | > | RealType denom = this->data_[a][a] * this->data_[b][b] - this->data_[b][a]*this->data_[a][b]; |
342 | > | m(a, a) = this->data_[b][b] /denom; |
343 | > | m(b, a) = -this->data_[b][a]/denom; |
344 | > | |
345 | > | m(a,b) = -this->data_[a][b]/denom; |
346 | > | m(b, b) = this->data_[a][a]/denom; |
347 | > | |
348 | > | } |
349 | > | |
350 | > | /* |
351 | > | for(std::vector<int>::iterator iter = zeroDiagElementIndex.begin(); iter != zeroDiagElementIndex.end() ++iter) { |
352 | > | if (this->data_[*iter][0] > OpenMD::epsilon || this->data_[*iter][1] ||this->data_[*iter][2] || |
353 | > | this->data_[0][*iter] > OpenMD::epsilon || this->data_[1][*iter] ||this->data_[2][*iter] ) { |
354 | > | std::cout << "can not inverse matrix" << std::endl; |
355 | > | } |
356 | > | } |
357 | > | */ |
358 | > | } else { |
359 | > | |
360 | > | m(0, 0) = this->data_[1][1]*this->data_[2][2] - this->data_[1][2]*this->data_[2][1]; |
361 | > | m(1, 0) = this->data_[1][2]*this->data_[2][0] - this->data_[1][0]*this->data_[2][2]; |
362 | > | m(2, 0) = this->data_[1][0]*this->data_[2][1] - this->data_[1][1]*this->data_[2][0]; |
363 | > | m(0, 1) = this->data_[2][1]*this->data_[0][2] - this->data_[2][2]*this->data_[0][1]; |
364 | > | m(1, 1) = this->data_[2][2]*this->data_[0][0] - this->data_[2][0]*this->data_[0][2]; |
365 | > | m(2, 1) = this->data_[2][0]*this->data_[0][1] - this->data_[2][1]*this->data_[0][0]; |
366 | > | m(0, 2) = this->data_[0][1]*this->data_[1][2] - this->data_[0][2]*this->data_[1][1]; |
367 | > | m(1, 2) = this->data_[0][2]*this->data_[1][0] - this->data_[0][0]*this->data_[1][2]; |
368 | > | m(2, 2) = this->data_[0][0]*this->data_[1][1] - this->data_[0][1]*this->data_[1][0]; |
369 | > | |
370 | > | m /= det; |
371 | > | } |
372 | > | return m; |
373 | > | } |
374 | > | |
375 | > | SquareMatrix3<Real> transpose() const{ |
376 | > | SquareMatrix3<Real> result; |
377 | > | |
378 | > | for (unsigned int i = 0; i < 3; i++) |
379 | > | for (unsigned int j = 0; j < 3; j++) |
380 | > | result(j, i) = this->data_[i][j]; |
381 | > | |
382 | > | return result; |
383 | > | } |
384 | > | /** |
385 | > | * Extract the eigenvalues and eigenvectors from a 3x3 matrix. |
386 | > | * The eigenvectors (the columns of V) will be normalized. |
387 | > | * The eigenvectors are aligned optimally with the x, y, and z |
388 | > | * axes respectively. |
389 | > | * @param a symmetric matrix whose eigenvectors are to be computed. On return, the matrix is |
390 | > | * overwritten |
391 | > | * @param w will contain the eigenvalues of the matrix On return of this function |
392 | > | * @param v the columns of this matrix will contain the eigenvectors. The eigenvectors are |
393 | > | * normalized and mutually orthogonal. |
394 | > | * @warning a will be overwritten |
395 | > | */ |
396 | > | static void diagonalize(SquareMatrix3<Real>& a, Vector3<Real>& w, SquareMatrix3<Real>& v); |
397 | > | }; |
398 | > | /*========================================================================= |
399 | > | |
400 | > | Program: Visualization Toolkit |
401 | > | Module: $RCSfile: SquareMatrix3.hpp,v $ |
402 | > | |
403 | > | Copyright (c) Ken Martin, Will Schroeder, Bill Lorensen |
404 | > | All rights reserved. |
405 | > | See Copyright.txt or http://www.kitware.com/Copyright.htm for details. |
406 | > | |
407 | > | This software is distributed WITHOUT ANY WARRANTY; without even |
408 | > | the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR |
409 | > | PURPOSE. See the above copyright notice for more information. |
410 | > | |
411 | > | =========================================================================*/ |
412 | > | template<typename Real> |
413 | > | void SquareMatrix3<Real>::diagonalize(SquareMatrix3<Real>& a, Vector3<Real>& w, |
414 | > | SquareMatrix3<Real>& v) { |
415 | > | int i,j,k,maxI; |
416 | > | Real tmp, maxVal; |
417 | > | Vector3<Real> v_maxI, v_k, v_j; |
418 | > | |
419 | > | // diagonalize using Jacobi |
420 | > | jacobi(a, w, v); |
421 | > | // if all the eigenvalues are the same, return identity matrix |
422 | > | if (w[0] == w[1] && w[0] == w[2] ) { |
423 | > | v = SquareMatrix3<Real>::identity(); |
424 | > | return; |
425 | > | } |
426 | > | |
427 | > | // transpose temporarily, it makes it easier to sort the eigenvectors |
428 | > | v = v.transpose(); |
429 | > | |
430 | > | // if two eigenvalues are the same, re-orthogonalize to optimally line |
431 | > | // up the eigenvectors with the x, y, and z axes |
432 | > | for (i = 0; i < 3; i++) { |
433 | > | if (w((i+1)%3) == w((i+2)%3)) {// two eigenvalues are the same |
434 | > | // find maximum element of the independant eigenvector |
435 | > | maxVal = fabs(v(i, 0)); |
436 | > | maxI = 0; |
437 | > | for (j = 1; j < 3; j++) { |
438 | > | if (maxVal < (tmp = fabs(v(i, j)))){ |
439 | > | maxVal = tmp; |
440 | > | maxI = j; |
441 | > | } |
442 | > | } |
443 | > | |
444 | > | // swap the eigenvector into its proper position |
445 | > | if (maxI != i) { |
446 | > | tmp = w(maxI); |
447 | > | w(maxI) = w(i); |
448 | > | w(i) = tmp; |
449 | > | |
450 | > | v.swapRow(i, maxI); |
451 | > | } |
452 | > | // maximum element of eigenvector should be positive |
453 | > | if (v(maxI, maxI) < 0) { |
454 | > | v(maxI, 0) = -v(maxI, 0); |
455 | > | v(maxI, 1) = -v(maxI, 1); |
456 | > | v(maxI, 2) = -v(maxI, 2); |
457 | > | } |
458 | > | |
459 | > | // re-orthogonalize the other two eigenvectors |
460 | > | j = (maxI+1)%3; |
461 | > | k = (maxI+2)%3; |
462 | > | |
463 | > | v(j, 0) = 0.0; |
464 | > | v(j, 1) = 0.0; |
465 | > | v(j, 2) = 0.0; |
466 | > | v(j, j) = 1.0; |
467 | > | |
468 | > | /** @todo */ |
469 | > | v_maxI = v.getRow(maxI); |
470 | > | v_j = v.getRow(j); |
471 | > | v_k = cross(v_maxI, v_j); |
472 | > | v_k.normalize(); |
473 | > | v_j = cross(v_k, v_maxI); |
474 | > | v.setRow(j, v_j); |
475 | > | v.setRow(k, v_k); |
476 | > | |
477 | > | |
478 | > | // transpose vectors back to columns |
479 | > | v = v.transpose(); |
480 | > | return; |
481 | > | } |
482 | > | } |
483 | > | |
484 | > | // the three eigenvalues are different, just sort the eigenvectors |
485 | > | // to align them with the x, y, and z axes |
486 | > | |
487 | > | // find the vector with the largest x element, make that vector |
488 | > | // the first vector |
489 | > | maxVal = fabs(v(0, 0)); |
490 | > | maxI = 0; |
491 | > | for (i = 1; i < 3; i++) { |
492 | > | if (maxVal < (tmp = fabs(v(i, 0)))) { |
493 | > | maxVal = tmp; |
494 | > | maxI = i; |
495 | > | } |
496 | > | } |
497 | > | |
498 | > | // swap eigenvalue and eigenvector |
499 | > | if (maxI != 0) { |
500 | > | tmp = w(maxI); |
501 | > | w(maxI) = w(0); |
502 | > | w(0) = tmp; |
503 | > | v.swapRow(maxI, 0); |
504 | > | } |
505 | > | // do the same for the y element |
506 | > | if (fabs(v(1, 1)) < fabs(v(2, 1))) { |
507 | > | tmp = w(2); |
508 | > | w(2) = w(1); |
509 | > | w(1) = tmp; |
510 | > | v.swapRow(2, 1); |
511 | > | } |
512 | > | |
513 | > | // ensure that the sign of the eigenvectors is correct |
514 | > | for (i = 0; i < 2; i++) { |
515 | > | if (v(i, i) < 0) { |
516 | > | v(i, 0) = -v(i, 0); |
517 | > | v(i, 1) = -v(i, 1); |
518 | > | v(i, 2) = -v(i, 2); |
519 | > | } |
520 | > | } |
521 | > | |
522 | > | // set sign of final eigenvector to ensure that determinant is positive |
523 | > | if (v.determinant() < 0) { |
524 | > | v(2, 0) = -v(2, 0); |
525 | > | v(2, 1) = -v(2, 1); |
526 | > | v(2, 2) = -v(2, 2); |
527 | > | } |
528 | > | |
529 | > | // transpose the eigenvectors back again |
530 | > | v = v.transpose(); |
531 | > | return ; |
532 | > | } |
533 | > | |
534 | > | /** |
535 | > | * Return the multiplication of two matrixes (m1 * m2). |
536 | > | * @return the multiplication of two matrixes |
537 | > | * @param m1 the first matrix |
538 | > | * @param m2 the second matrix |
539 | > | */ |
540 | > | template<typename Real> |
541 | > | inline SquareMatrix3<Real> operator *(const SquareMatrix3<Real>& m1, const SquareMatrix3<Real>& m2) { |
542 | > | SquareMatrix3<Real> result; |
543 | > | |
544 | > | for (unsigned int i = 0; i < 3; i++) |
545 | > | for (unsigned int j = 0; j < 3; j++) |
546 | > | for (unsigned int k = 0; k < 3; k++) |
547 | > | result(i, j) += m1(i, k) * m2(k, j); |
548 | > | |
549 | > | return result; |
550 | > | } |
551 | > | |
552 | > | template<typename Real> |
553 | > | inline SquareMatrix3<Real> outProduct(const Vector3<Real>& v1, const Vector3<Real>& v2) { |
554 | > | SquareMatrix3<Real> result; |
555 | > | |
556 | > | for (unsigned int i = 0; i < 3; i++) { |
557 | > | for (unsigned int j = 0; j < 3; j++) { |
558 | > | result(i, j) = v1[i] * v2[j]; |
559 | > | } |
560 | > | } |
561 | > | |
562 | > | return result; |
563 | > | } |
564 | > | |
565 | > | |
566 | > | typedef SquareMatrix3<RealType> Mat3x3d; |
567 | > | typedef SquareMatrix3<RealType> RotMat3x3d; |
568 | > | |
569 | > | } //namespace OpenMD |
570 | > | #endif // MATH_SQUAREMATRIX_HPP |
571 | > |
– | Removed lines |
+ | Added lines |
< | Changed lines |
> | Changed lines |