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

Diff Legend

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