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

Comparing:
trunk/OOPSE-4/src/math/SquareMatrix3.hpp (file contents), Revision 1594 by tim, Mon Oct 18 23:13:23 2004 UTC vs.
branches/new_design/OOPSE-4/src/math/SquareMatrix3.hpp (file contents), Revision 1822 by tim, Thu Dec 2 02:08:29 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 41 | Line 41 | namespace oopse {
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 <
64 >            
65              SquareMatrix3( const Vector3<Real>& eulerAngles) {
66                  setupRotMat(eulerAngles);
67              }
# Line 59 | Line 71 | namespace oopse {
71              }
72  
73              SquareMatrix3(const Quaternion<Real>& q) {
74 <                *this = q.toRotationMatrix3();
74 >                setupRotMat(q);
75 >
76              }
77  
78              SquareMatrix3(Real w, Real x, Real y, Real z) {
79 <                Quaternion<Real> q(w, x, y, z);
67 <                *this = q.toRotationMatrix3();
79 >                setupRotMat(w, x, y, z);
80              }
81              
82              /** copy assignment operator */
# Line 119 | Line 131 | namespace oopse {
131               * @param quat
132              */
133              void setupRotMat(const Quaternion<Real>& quat) {
134 <                *this = quat.toRotationMatrix3();
134 >                setupRotMat(quat.w(), quat.x(), quat.y(), quat.z());
135              }
136  
137              /**
# Line 196 | Line 208 | namespace oopse {
208               * z-axis (again).
209              */            
210              Vector3<Real> toEulerAngles() {
211 <                Vector<Real> myEuler;
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(min(1.0,max(-1.0,data_[2][2])));
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  
# Line 248 | Line 260 | namespace oopse {
260  
261                  return(x + y + z);
262              }            
263 +
264 +            /** Returns the trace of this matrix. */
265 +            Real trace() const {
266 +                return data_[0][0] + data_[1][1] + data_[2][2];
267 +            }
268              
269              /**
270               * Sets the value of this matrix to  the inversion of itself.
271               * @note since simple algorithm can be applied to inverse the 3 by 3 matrix, we hide the
272               * implementation of inverse in SquareMatrix class
273               */
274 <            SquareMatrix3<Real>  inverse() {
274 >            SquareMatrix3<Real>  inverse() const {
275                  SquareMatrix3<Real> m;
276                  double det = determinant();
277                  if (fabs(det) <= oopse::epsilon) {
# Line 275 | Line 292 | namespace oopse {
292                  m /= det;
293                  return m;
294              }
295 <
296 <            void diagonalize(SquareMatrix3<Real>& a, Vector3<Real>& w, SquareMatrix3<Real>& v) {
297 <                int i,j,k,maxI;
298 <                Real tmp, maxVal;
299 <                Vector3<Real> v_maxI, v_k, v_j;
300 <
301 <                // diagonalize using Jacobi
302 <                jacobi(a, w, v);
295 >            /**
296 >             * Extract the eigenvalues and eigenvectors from a 3x3 matrix.
297 >             * The eigenvectors (the columns of V) will be normalized.
298 >             * The eigenvectors are aligned optimally with the x, y, and z
299 >             * axes respectively.
300 >             * @param a symmetric matrix whose eigenvectors are to be computed. On return, the matrix is
301 >             *     overwritten            
302 >             * @param w will contain the eigenvalues of the matrix On return of this function
303 >             * @param v the columns of this matrix will contain the eigenvectors. The eigenvectors are
304 >             *    normalized and mutually orthogonal.              
305 >             * @warning a will be overwritten
306 >             */
307 >            static void diagonalize(SquareMatrix3<Real>& a, Vector3<Real>& w, SquareMatrix3<Real>& v);
308 >    };
309 > /*=========================================================================
310  
311 <                // if all the eigenvalues are the same, return identity matrix
312 <                if (w[0] == w[1] && w[0] == w[2] ){
313 <                      v = SquareMatrix3<Real>::identity();
314 <                      return
311 >  Program:   Visualization Toolkit
312 >  Module:    $RCSfile: SquareMatrix3.hpp,v $
313 >
314 >  Copyright (c) Ken Martin, Will Schroeder, Bill Lorensen
315 >  All rights reserved.
316 >  See Copyright.txt or http://www.kitware.com/Copyright.htm for details.
317 >
318 >     This software is distributed WITHOUT ANY WARRANTY; without even
319 >     the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
320 >     PURPOSE.  See the above copyright notice for more information.
321 >
322 > =========================================================================*/
323 >    template<typename Real>
324 >    void SquareMatrix3<Real>::diagonalize(SquareMatrix3<Real>& a, Vector3<Real>& w,
325 >                                                                           SquareMatrix3<Real>& v) {
326 >        int i,j,k,maxI;
327 >        Real tmp, maxVal;
328 >        Vector3<Real> v_maxI, v_k, v_j;
329 >
330 >        // diagonalize using Jacobi
331 >        jacobi(a, w, v);
332 >        // if all the eigenvalues are the same, return identity matrix
333 >        if (w[0] == w[1] && w[0] == w[2] ) {
334 >              v = SquareMatrix3<Real>::identity();
335 >              return;
336 >        }
337 >
338 >        // transpose temporarily, it makes it easier to sort the eigenvectors
339 >        v = v.transpose();
340 >        
341 >        // if two eigenvalues are the same, re-orthogonalize to optimally line
342 >        // up the eigenvectors with the x, y, and z axes
343 >        for (i = 0; i < 3; i++) {
344 >            if (w((i+1)%3) == w((i+2)%3)) {// two eigenvalues are the same
345 >            // find maximum element of the independant eigenvector
346 >            maxVal = fabs(v(i, 0));
347 >            maxI = 0;
348 >            for (j = 1; j < 3; j++) {
349 >                if (maxVal < (tmp = fabs(v(i, j)))){
350 >                    maxVal = tmp;
351 >                    maxI = j;
352                  }
353 +            }
354 +            
355 +            // swap the eigenvector into its proper position
356 +            if (maxI != i) {
357 +                tmp = w(maxI);
358 +                w(maxI) = w(i);
359 +                w(i) = tmp;
360  
361 <                // transpose temporarily, it makes it easier to sort the eigenvectors
362 <                v = v.tanspose();
363 <                
364 <                // if two eigenvalues are the same, re-orthogonalize to optimally line
365 <                // up the eigenvectors with the x, y, and z axes
366 <                for (i = 0; i < 3; i++) {
367 <                    if (w((i+1)%3) == w((i+2)%3)) {// two eigenvalues are the same
368 <                    // 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;
361 >                v.swapRow(i, maxI);
362 >            }
363 >            // maximum element of eigenvector should be positive
364 >            if (v(maxI, maxI) < 0) {
365 >                v(maxI, 0) = -v(maxI, 0);
366 >                v(maxI, 1) = -v(maxI, 1);
367 >                v(maxI, 2) = -v(maxI, 2);
368 >            }
369  
370 <                        v.swapRow(i, maxI);
371 <                    }
372 <                    // 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 <                    }
324 <
325 <                    // re-orthogonalize the other two eigenvectors
326 <                    j = (maxI+1)%3;
327 <                    k = (maxI+2)%3;
328 <
329 <                    v(j, 0) = 0.0;
330 <                    v(j, 1) = 0.0;
331 <                    v(j, 2) = 0.0;
332 <                    v(j, j) = 1.0;
333 <
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.normailze();
339 <                    v_j = cross(v_k, v_maxI);
340 <                    v.setRow(j, v_j);
341 <                    v.setRow(k, v_k);
370 >            // re-orthogonalize the other two eigenvectors
371 >            j = (maxI+1)%3;
372 >            k = (maxI+2)%3;
373  
374 +            v(j, 0) = 0.0;
375 +            v(j, 1) = 0.0;
376 +            v(j, 2) = 0.0;
377 +            v(j, j) = 1.0;
378  
379 <                    // transpose vectors back to columns
380 <                    v = v.transpose();
381 <                    return;
382 <                    }
383 <                }
379 >            /** @todo */
380 >            v_maxI = v.getRow(maxI);
381 >            v_j = v.getRow(j);
382 >            v_k = cross(v_maxI, v_j);
383 >            v_k.normalize();
384 >            v_j = cross(v_k, v_maxI);
385 >            v.setRow(j, v_j);
386 >            v.setRow(k, v_k);
387  
350 –                // the three eigenvalues are different, just sort the eigenvectors
351 –                // to align them with the x, y, and z axes
388  
389 <                // find the vector with the largest x element, make that vector
390 <                // the first vector
391 <                maxVal = fabs(v(0, 0));
392 <                maxI = 0;
393 <                for (i = 1; i < 3; i++) {
358 <                    if (maxVal < (tmp = fabs(v(i, 0)))) {
359 <                        maxVal = tmp;
360 <                        maxI = i;
361 <                    }
362 <                }
389 >            // transpose vectors back to columns
390 >            v = v.transpose();
391 >            return;
392 >            }
393 >        }
394  
395 <                // swap eigenvalue and eigenvector
396 <                if (maxI != 0) {
366 <                    tmp = w(maxI);
367 <                    w(maxI) = w(0);
368 <                    w(0) = tmp;
369 <                    v.swapRow(maxI, 0);
370 <                }
371 <                // 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 <                }
395 >        // the three eigenvalues are different, just sort the eigenvectors
396 >        // to align them with the x, y, and z axes
397  
398 <                // ensure that the sign of the eigenvectors is correct
399 <                for (i = 0; i < 2; i++) {
400 <                    if (v(i, i) < 0) {
401 <                        v(i, 0) = -v(i, 0);
402 <                        v(i, 1) = -v(i, 1);
403 <                        v(i, 2) = -v(i, 2);
404 <                    }
405 <                }
398 >        // find the vector with the largest x element, make that vector
399 >        // the first vector
400 >        maxVal = fabs(v(0, 0));
401 >        maxI = 0;
402 >        for (i = 1; i < 3; i++) {
403 >            if (maxVal < (tmp = fabs(v(i, 0)))) {
404 >                maxVal = tmp;
405 >                maxI = i;
406 >            }
407 >        }
408  
409 <                // set sign of final eigenvector to ensure that determinant is positive
410 <                if (determinant(v) < 0) {
411 <                    v(2, 0) = -v(2, 0);
412 <                    v(2, 1) = -v(2, 1);
413 <                    v(2, 2) = -v(2, 2);
414 <                }
409 >        // swap eigenvalue and eigenvector
410 >        if (maxI != 0) {
411 >            tmp = w(maxI);
412 >            w(maxI) = w(0);
413 >            w(0) = tmp;
414 >            v.swapRow(maxI, 0);
415 >        }
416 >        // do the same for the y element
417 >        if (fabs(v(1, 1)) < fabs(v(2, 1))) {
418 >            tmp = w(2);
419 >            w(2) = w(1);
420 >            w(1) = tmp;
421 >            v.swapRow(2, 1);
422 >        }
423  
424 <                // transpose the eigenvectors back again
425 <                v = v.transpose();
426 <                return ;
424 >        // ensure that the sign of the eigenvectors is correct
425 >        for (i = 0; i < 2; i++) {
426 >            if (v(i, i) < 0) {
427 >                v(i, 0) = -v(i, 0);
428 >                v(i, 1) = -v(i, 1);
429 >                v(i, 2) = -v(i, 2);
430              }
431 <    };
431 >        }
432  
433 +        // set sign of final eigenvector to ensure that determinant is positive
434 +        if (v.determinant() < 0) {
435 +            v(2, 0) = -v(2, 0);
436 +            v(2, 1) = -v(2, 1);
437 +            v(2, 2) = -v(2, 2);
438 +        }
439 +
440 +        // transpose the eigenvectors back again
441 +        v = v.transpose();
442 +        return ;
443 +    }
444 +
445 +    /**
446 +    * Return the multiplication of two matrixes  (m1 * m2).
447 +    * @return the multiplication of two matrixes
448 +    * @param m1 the first matrix
449 +    * @param m2 the second matrix
450 +    */
451 +    template<typename Real>
452 +    inline SquareMatrix3<Real> operator *(const SquareMatrix3<Real>& m1, const SquareMatrix3<Real>& m2) {
453 +        SquareMatrix3<Real> result;
454 +
455 +            for (unsigned int i = 0; i < 3; i++)
456 +                for (unsigned int j = 0; j < 3; j++)
457 +                    for (unsigned int k = 0; k < 3; k++)
458 +                        result(i, j)  += m1(i, k) * m2(k, j);                
459 +
460 +        return result;
461 +    }
462 +
463 +    template<typename Real>
464 +    inline SquareMatrix3<Real> outProduct(const Vector3<Real>& v1, const Vector3<Real>& v2) {
465 +        SquareMatrix3<Real> result;
466 +
467 +            for (unsigned int i = 0; i < 3; i++) {
468 +                for (unsigned int j = 0; j < 3; j++) {
469 +                        result(i, j)  = v1[i] * v2[j];                
470 +                }
471 +            }
472 +            
473 +        return result;        
474 +    }
475 +
476 +    
477      typedef SquareMatrix3<double> Mat3x3d;
478      typedef SquareMatrix3<double> RotMat3x3d;
479  
480   } //namespace oopse
481   #endif // MATH_SQUAREMATRIX_HPP
482 +

Diff Legend

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