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

Comparing trunk/OOPSE-3.0/src/math/SquareMatrix3.hpp (file contents):
Revision 1592 by tim, Mon Oct 18 17:07:27 2004 UTC vs.
Revision 1616 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_SQUAREMATRIX3_HPP
32 > #ifndef MATH_SQUAREMATRIX3_HPP
33   #define  MATH_SQUAREMATRIX3_HPP
34  
35   #include "Quaternion.hpp"
# Line 59 | Line 59 | namespace oopse {
59              }
60  
61              SquareMatrix3(const Quaternion<Real>& q) {
62 <                *this = q.toRotationMatrix3();
62 >                setupRotMat(q);
63 >
64              }
65  
66              SquareMatrix3(Real w, Real x, Real y, Real z) {
67 <                Quaternion<Real> q(w, x, y, z);
67 <                *this = q.toRotationMatrix3();
67 >                setupRotMat(w, x, y, z);
68              }
69              
70              /** copy assignment operator */
# Line 72 | Line 72 | namespace oopse {
72                  if (this == &m)
73                      return *this;
74                   SquareMatrix<Real, 3>::operator=(m);
75 +                 return *this;
76              }
77  
78              /**
# Line 118 | Line 119 | namespace oopse {
119               * @param quat
120              */
121              void setupRotMat(const Quaternion<Real>& quat) {
122 <                *this = quat.toRotationMatrix3();
122 >                setupRotMat(quat.w(), quat.x(), quat.y(), quat.z());
123              }
124  
125              /**
# Line 126 | Line 127 | namespace oopse {
127               * @param w the first element
128               * @param x the second element
129               * @param y the third element
130 <             * @parma z the fourth element
130 >             * @param z the fourth element
131              */
132              void setupRotMat(Real w, Real x, Real y, Real z) {
133                  Quaternion<Real> q(w, x, y, z);
# Line 195 | Line 196 | namespace oopse {
196               * z-axis (again).
197              */            
198              Vector3<Real> toEulerAngles() {
199 <                Vector<Real> myEuler;
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(min(1.0,max(-1.0,data_[2][2])));
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  
# Line 236 | Line 237 | namespace oopse {
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 <            }
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 <            void diagonalize() {
276 <
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 +  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