ViewVC Help
View File | Revision Log | Show Annotations | View Changeset | Root Listing
root/OpenMD/trunk/src/math/SquareMatrix3.hpp
(Generate patch)

Comparing trunk/src/math/SquareMatrix3.hpp (file contents):
Revision 76 by tim, Thu Oct 14 23:28:09 2004 UTC vs.
Revision 123 by tim, Wed Oct 20 18:07:08 2004 UTC

# 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 +                setupRotMat(q);
63 +
64 +            }
65 +
66 +            SquareMatrix3(Real w, Real x, Real y, Real z) {
67 +                setupRotMat(w, x, y, z);
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 >                setupRotMat(quat.w(), quat.x(), quat.y(), quat.z());
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 >                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(std::min(1.0, std::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();
258 <
259 <            void diagonalize();
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 <    }
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 +                m /= det;
276 +                return m;
277 +            }
278 +            /**
279 +             * Extract the eigenvalues and eigenvectors from a 3x3 matrix.
280 +             * The eigenvectors (the columns of V) will be normalized.
281 +             * The eigenvectors are aligned optimally with the x, y, and z
282 +             * axes respectively.
283 +             * @param a symmetric matrix whose eigenvectors are to be computed. On return, the matrix is
284 +             *     overwritten            
285 +             * @param w will contain the eigenvalues of the matrix On return of this function
286 +             * @param v the columns of this matrix will contain the eigenvectors. The eigenvectors are
287 +             *    normalized and mutually orthogonal.              
288 +             * @warning a will be overwritten
289 +             */
290 +            static void diagonalize(SquareMatrix3<Real>& a, Vector3<Real>& w, SquareMatrix3<Real>& v);
291      };
292 + /*=========================================================================
293  
294 < }
295 < #endif // MATH_SQUAREMATRIX#_HPP
294 >  Program:   Visualization Toolkit
295 >  Module:    $RCSfile: SquareMatrix3.hpp,v $
296 >
297 >  Copyright (c) Ken Martin, Will Schroeder, Bill Lorensen
298 >  All rights reserved.
299 >  See Copyright.txt or http://www.kitware.com/Copyright.htm for details.
300 >
301 >     This software is distributed WITHOUT ANY WARRANTY; without even
302 >     the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
303 >     PURPOSE.  See the above copyright notice for more information.
304 >
305 > =========================================================================*/
306 >    template<typename Real>
307 >    void SquareMatrix3<Real>::diagonalize(SquareMatrix3<Real>& a, Vector3<Real>& w,
308 >                                                                           SquareMatrix3<Real>& v) {
309 >        int i,j,k,maxI;
310 >        Real tmp, maxVal;
311 >        Vector3<Real> v_maxI, v_k, v_j;
312 >
313 >        // diagonalize using Jacobi
314 >        jacobi(a, w, v);
315 >        // if all the eigenvalues are the same, return identity matrix
316 >        if (w[0] == w[1] && w[0] == w[2] ) {
317 >              v = SquareMatrix3<Real>::identity();
318 >              return;
319 >        }
320 >
321 >        // transpose temporarily, it makes it easier to sort the eigenvectors
322 >        v = v.transpose();
323 >        
324 >        // if two eigenvalues are the same, re-orthogonalize to optimally line
325 >        // up the eigenvectors with the x, y, and z axes
326 >        for (i = 0; i < 3; i++) {
327 >            if (w((i+1)%3) == w((i+2)%3)) {// two eigenvalues are the same
328 >            // find maximum element of the independant eigenvector
329 >            maxVal = fabs(v(i, 0));
330 >            maxI = 0;
331 >            for (j = 1; j < 3; j++) {
332 >                if (maxVal < (tmp = fabs(v(i, j)))){
333 >                    maxVal = tmp;
334 >                    maxI = j;
335 >                }
336 >            }
337 >            
338 >            // swap the eigenvector into its proper position
339 >            if (maxI != i) {
340 >                tmp = w(maxI);
341 >                w(maxI) = w(i);
342 >                w(i) = tmp;
343 >
344 >                v.swapRow(i, maxI);
345 >            }
346 >            // maximum element of eigenvector should be positive
347 >            if (v(maxI, maxI) < 0) {
348 >                v(maxI, 0) = -v(maxI, 0);
349 >                v(maxI, 1) = -v(maxI, 1);
350 >                v(maxI, 2) = -v(maxI, 2);
351 >            }
352 >
353 >            // re-orthogonalize the other two eigenvectors
354 >            j = (maxI+1)%3;
355 >            k = (maxI+2)%3;
356 >
357 >            v(j, 0) = 0.0;
358 >            v(j, 1) = 0.0;
359 >            v(j, 2) = 0.0;
360 >            v(j, j) = 1.0;
361 >
362 >            /** @todo */
363 >            v_maxI = v.getRow(maxI);
364 >            v_j = v.getRow(j);
365 >            v_k = cross(v_maxI, v_j);
366 >            v_k.normalize();
367 >            v_j = cross(v_k, v_maxI);
368 >            v.setRow(j, v_j);
369 >            v.setRow(k, v_k);
370 >
371 >
372 >            // transpose vectors back to columns
373 >            v = v.transpose();
374 >            return;
375 >            }
376 >        }
377 >
378 >        // the three eigenvalues are different, just sort the eigenvectors
379 >        // to align them with the x, y, and z axes
380 >
381 >        // find the vector with the largest x element, make that vector
382 >        // the first vector
383 >        maxVal = fabs(v(0, 0));
384 >        maxI = 0;
385 >        for (i = 1; i < 3; i++) {
386 >            if (maxVal < (tmp = fabs(v(i, 0)))) {
387 >                maxVal = tmp;
388 >                maxI = i;
389 >            }
390 >        }
391 >
392 >        // swap eigenvalue and eigenvector
393 >        if (maxI != 0) {
394 >            tmp = w(maxI);
395 >            w(maxI) = w(0);
396 >            w(0) = tmp;
397 >            v.swapRow(maxI, 0);
398 >        }
399 >        // do the same for the y element
400 >        if (fabs(v(1, 1)) < fabs(v(2, 1))) {
401 >            tmp = w(2);
402 >            w(2) = w(1);
403 >            w(1) = tmp;
404 >            v.swapRow(2, 1);
405 >        }
406 >
407 >        // ensure that the sign of the eigenvectors is correct
408 >        for (i = 0; i < 2; i++) {
409 >            if (v(i, i) < 0) {
410 >                v(i, 0) = -v(i, 0);
411 >                v(i, 1) = -v(i, 1);
412 >                v(i, 2) = -v(i, 2);
413 >            }
414 >        }
415 >
416 >        // set sign of final eigenvector to ensure that determinant is positive
417 >        if (v.determinant() < 0) {
418 >            v(2, 0) = -v(2, 0);
419 >            v(2, 1) = -v(2, 1);
420 >            v(2, 2) = -v(2, 2);
421 >        }
422 >
423 >        // transpose the eigenvectors back again
424 >        v = v.transpose();
425 >        return ;
426 >    }
427 >    typedef SquareMatrix3<double> Mat3x3d;
428 >    typedef SquareMatrix3<double> RotMat3x3d;
429 >
430 > } //namespace oopse
431 > #endif // MATH_SQUAREMATRIX_HPP
432 >

Diff Legend

– Removed lines
+ Added lines
< Changed lines
> Changed lines