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

Comparing:
trunk/OOPSE-3.0/src/math/SquareMatrix3.hpp (file contents), Revision 1586 by tim, Sun Oct 17 01:19:11 2004 UTC vs.
branches/new_design/OOPSE-3.0/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_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"
# 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 72 | Line 84 | namespace oopse {
84                  if (this == &m)
85                      return *this;
86                   SquareMatrix<Real, 3>::operator=(m);
87 +                 return *this;
88              }
89  
90              /**
# Line 118 | 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 126 | Line 139 | namespace oopse {
139               * @param w the first element
140               * @param x the second element
141               * @param y the third element
142 <             * @parma z the fourth element
142 >             * @param z the fourth element
143              */
144              void setupRotMat(Real w, Real x, Real y, Real z) {
145                  Quaternion<Real> q(w, x, y, z);
# Line 195 | 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 237 | Line 250 | namespace oopse {
250                  return myEuler;
251              }
252              
253 +            /** Returns the determinant of this matrix. */
254 +            Real determinant() const {
255 +                Real x,y,z;
256 +
257 +                x = data_[0][0] * (data_[1][1] * data_[2][2] - data_[1][2] * data_[2][1]);
258 +                y = data_[0][1] * (data_[1][2] * data_[2][0] - data_[1][0] * data_[2][2]);
259 +                z = data_[0][2] * (data_[1][0] * data_[2][1] - data_[1][1] * data_[2][0]);
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 <            void  inverse();
274 >            SquareMatrix3<Real>  inverse() const {
275 >                SquareMatrix3<Real> m;
276 >                double det = determinant();
277 >                if (fabs(det) <= oopse::epsilon) {
278 >                //"The method was called on a matrix with |determinant| <= 1e-6.",
279 >                //"This is a runtime or a programming error in your application.");
280 >                }
281  
282 <            void diagonalize();
282 >                m(0, 0) = data_[1][1]*data_[2][2] - data_[1][2]*data_[2][1];
283 >                m(1, 0) = data_[1][2]*data_[2][0] - data_[1][0]*data_[2][2];
284 >                m(2, 0) = data_[1][0]*data_[2][1] - data_[1][1]*data_[2][0];
285 >                m(0, 1) = data_[2][1]*data_[0][2] - data_[2][2]*data_[0][1];
286 >                m(1, 1) = data_[2][2]*data_[0][0] - data_[2][0]*data_[0][2];
287 >                m(2, 1) = data_[2][0]*data_[0][1] - data_[2][1]*data_[0][0];
288 >                m(0, 2) = data_[0][1]*data_[1][2] - data_[0][2]*data_[1][1];
289 >                m(1, 2) = data_[0][2]*data_[1][0] - data_[0][0]*data_[1][2];
290 >                m(2, 2) = data_[0][0]*data_[1][1] - data_[0][1]*data_[1][0];
291  
292 +                m /= det;
293 +                return m;
294 +            }
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 <    typedef template SquareMatrix3<double> Mat3x3d
312 <    typedef template SquareMatrix3<double> RotMat3x3d;
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 +                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 +            // 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 +            /** @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 +
388 +
389 +            // transpose vectors back to columns
390 +            v = v.transpose();
391 +            return;
392 +            }
393 +        }
394 +
395 +        // the three eigenvalues are different, just sort the eigenvectors
396 +        // to align them with the x, y, and z axes
397 +
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 +        // 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 +        // 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 +        }
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