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

Comparing trunk/OOPSE-2.0/src/math/SquareMatrix3.hpp (file contents):
Revision 1586 by tim, Sun Oct 17 01:19:11 2004 UTC vs.
Revision 1630 by tim, Thu Oct 21 21:31:39 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>() {
# Line 59 | Line 62 | namespace oopse {
62              }
63  
64              SquareMatrix3(const Quaternion<Real>& q) {
65 <                *this = q.toRotationMatrix3();
65 >                setupRotMat(q);
66 >
67              }
68  
69              SquareMatrix3(Real w, Real x, Real y, Real z) {
70 <                Quaternion<Real> q(w, x, y, z);
67 <                *this = q.toRotationMatrix3();
70 >                setupRotMat(w, x, y, z);
71              }
72              
73              /** copy assignment operator */
# Line 72 | Line 75 | namespace oopse {
75                  if (this == &m)
76                      return *this;
77                   SquareMatrix<Real, 3>::operator=(m);
78 +                 return *this;
79              }
80  
81              /**
# Line 118 | Line 122 | namespace oopse {
122               * @param quat
123              */
124              void setupRotMat(const Quaternion<Real>& quat) {
125 <                *this = quat.toRotationMatrix3();
125 >                setupRotMat(quat.w(), quat.x(), quat.y(), quat.z());
126              }
127  
128              /**
# Line 126 | Line 130 | namespace oopse {
130               * @param w the first element
131               * @param x the second element
132               * @param y the third element
133 <             * @parma z the fourth element
133 >             * @param z the fourth element
134              */
135              void setupRotMat(Real w, Real x, Real y, Real z) {
136                  Quaternion<Real> q(w, x, y, z);
# Line 195 | Line 199 | namespace oopse {
199               * z-axis (again).
200              */            
201              Vector3<Real> toEulerAngles() {
202 <                Vector<Real> myEuler;
202 >                Vector3<Real> myEuler;
203                  Real phi,theta,psi,eps;
204                  Real ctheta,stheta;
205                  
206                  // set the tolerance for Euler angles and rotation elements
207  
208 <                theta = acos(min(1.0,max(-1.0,data_[2][2])));
208 >                theta = acos(std::min(1.0, std::max(-1.0,data_[2][2])));
209                  ctheta = data_[2][2];
210                  stheta = sqrt(1.0 - ctheta * ctheta);
211  
# Line 237 | Line 241 | namespace oopse {
241                  return myEuler;
242              }
243              
244 +            /** Returns the determinant of this matrix. */
245 +            Real determinant() const {
246 +                Real x,y,z;
247 +
248 +                x = data_[0][0] * (data_[1][1] * data_[2][2] - data_[1][2] * data_[2][1]);
249 +                y = data_[0][1] * (data_[1][2] * data_[2][0] - data_[1][0] * data_[2][2]);
250 +                z = data_[0][2] * (data_[1][0] * data_[2][1] - data_[1][1] * data_[2][0]);
251 +
252 +                return(x + y + z);
253 +            }            
254 +            
255              /**
256               * Sets the value of this matrix to  the inversion of itself.
257               * @note since simple algorithm can be applied to inverse the 3 by 3 matrix, we hide the
258               * implementation of inverse in SquareMatrix class
259               */
260 <            void  inverse();
260 >            SquareMatrix3<Real>  inverse() {
261 >                SquareMatrix3<Real> m;
262 >                double det = determinant();
263 >                if (fabs(det) <= oopse::epsilon) {
264 >                //"The method was called on a matrix with |determinant| <= 1e-6.",
265 >                //"This is a runtime or a programming error in your application.");
266 >                }
267  
268 <            void diagonalize();
268 >                m(0, 0) = data_[1][1]*data_[2][2] - data_[1][2]*data_[2][1];
269 >                m(1, 0) = data_[1][2]*data_[2][0] - data_[1][0]*data_[2][2];
270 >                m(2, 0) = data_[1][0]*data_[2][1] - data_[1][1]*data_[2][0];
271 >                m(0, 1) = data_[2][1]*data_[0][2] - data_[2][2]*data_[0][1];
272 >                m(1, 1) = data_[2][2]*data_[0][0] - data_[2][0]*data_[0][2];
273 >                m(2, 1) = data_[2][0]*data_[0][1] - data_[2][1]*data_[0][0];
274 >                m(0, 2) = data_[0][1]*data_[1][2] - data_[0][2]*data_[1][1];
275 >                m(1, 2) = data_[0][2]*data_[1][0] - data_[0][0]*data_[1][2];
276 >                m(2, 2) = data_[0][0]*data_[1][1] - data_[0][1]*data_[1][0];
277  
278 +                m /= det;
279 +                return m;
280 +            }
281 +            /**
282 +             * Extract the eigenvalues and eigenvectors from a 3x3 matrix.
283 +             * The eigenvectors (the columns of V) will be normalized.
284 +             * The eigenvectors are aligned optimally with the x, y, and z
285 +             * axes respectively.
286 +             * @param a symmetric matrix whose eigenvectors are to be computed. On return, the matrix is
287 +             *     overwritten            
288 +             * @param w will contain the eigenvalues of the matrix On return of this function
289 +             * @param v the columns of this matrix will contain the eigenvectors. The eigenvectors are
290 +             *    normalized and mutually orthogonal.              
291 +             * @warning a will be overwritten
292 +             */
293 +            static void diagonalize(SquareMatrix3<Real>& a, Vector3<Real>& w, SquareMatrix3<Real>& v);
294      };
295 + /*=========================================================================
296  
297 <    typedef template SquareMatrix3<double> Mat3x3d
298 <    typedef template SquareMatrix3<double> RotMat3x3d;
297 >  Program:   Visualization Toolkit
298 >  Module:    $RCSfile: SquareMatrix3.hpp,v $
299 >
300 >  Copyright (c) Ken Martin, Will Schroeder, Bill Lorensen
301 >  All rights reserved.
302 >  See Copyright.txt or http://www.kitware.com/Copyright.htm for details.
303 >
304 >     This software is distributed WITHOUT ANY WARRANTY; without even
305 >     the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
306 >     PURPOSE.  See the above copyright notice for more information.
307 >
308 > =========================================================================*/
309 >    template<typename Real>
310 >    void SquareMatrix3<Real>::diagonalize(SquareMatrix3<Real>& a, Vector3<Real>& w,
311 >                                                                           SquareMatrix3<Real>& v) {
312 >        int i,j,k,maxI;
313 >        Real tmp, maxVal;
314 >        Vector3<Real> v_maxI, v_k, v_j;
315 >
316 >        // diagonalize using Jacobi
317 >        jacobi(a, w, v);
318 >        // if all the eigenvalues are the same, return identity matrix
319 >        if (w[0] == w[1] && w[0] == w[2] ) {
320 >              v = SquareMatrix3<Real>::identity();
321 >              return;
322 >        }
323 >
324 >        // transpose temporarily, it makes it easier to sort the eigenvectors
325 >        v = v.transpose();
326 >        
327 >        // if two eigenvalues are the same, re-orthogonalize to optimally line
328 >        // up the eigenvectors with the x, y, and z axes
329 >        for (i = 0; i < 3; i++) {
330 >            if (w((i+1)%3) == w((i+2)%3)) {// two eigenvalues are the same
331 >            // find maximum element of the independant eigenvector
332 >            maxVal = fabs(v(i, 0));
333 >            maxI = 0;
334 >            for (j = 1; j < 3; j++) {
335 >                if (maxVal < (tmp = fabs(v(i, j)))){
336 >                    maxVal = tmp;
337 >                    maxI = j;
338 >                }
339 >            }
340 >            
341 >            // swap the eigenvector into its proper position
342 >            if (maxI != i) {
343 >                tmp = w(maxI);
344 >                w(maxI) = w(i);
345 >                w(i) = tmp;
346  
347 +                v.swapRow(i, maxI);
348 +            }
349 +            // maximum element of eigenvector should be positive
350 +            if (v(maxI, maxI) < 0) {
351 +                v(maxI, 0) = -v(maxI, 0);
352 +                v(maxI, 1) = -v(maxI, 1);
353 +                v(maxI, 2) = -v(maxI, 2);
354 +            }
355 +
356 +            // re-orthogonalize the other two eigenvectors
357 +            j = (maxI+1)%3;
358 +            k = (maxI+2)%3;
359 +
360 +            v(j, 0) = 0.0;
361 +            v(j, 1) = 0.0;
362 +            v(j, 2) = 0.0;
363 +            v(j, j) = 1.0;
364 +
365 +            /** @todo */
366 +            v_maxI = v.getRow(maxI);
367 +            v_j = v.getRow(j);
368 +            v_k = cross(v_maxI, v_j);
369 +            v_k.normalize();
370 +            v_j = cross(v_k, v_maxI);
371 +            v.setRow(j, v_j);
372 +            v.setRow(k, v_k);
373 +
374 +
375 +            // transpose vectors back to columns
376 +            v = v.transpose();
377 +            return;
378 +            }
379 +        }
380 +
381 +        // the three eigenvalues are different, just sort the eigenvectors
382 +        // to align them with the x, y, and z axes
383 +
384 +        // find the vector with the largest x element, make that vector
385 +        // the first vector
386 +        maxVal = fabs(v(0, 0));
387 +        maxI = 0;
388 +        for (i = 1; i < 3; i++) {
389 +            if (maxVal < (tmp = fabs(v(i, 0)))) {
390 +                maxVal = tmp;
391 +                maxI = i;
392 +            }
393 +        }
394 +
395 +        // swap eigenvalue and eigenvector
396 +        if (maxI != 0) {
397 +            tmp = w(maxI);
398 +            w(maxI) = w(0);
399 +            w(0) = tmp;
400 +            v.swapRow(maxI, 0);
401 +        }
402 +        // do the same for the y element
403 +        if (fabs(v(1, 1)) < fabs(v(2, 1))) {
404 +            tmp = w(2);
405 +            w(2) = w(1);
406 +            w(1) = tmp;
407 +            v.swapRow(2, 1);
408 +        }
409 +
410 +        // ensure that the sign of the eigenvectors is correct
411 +        for (i = 0; i < 2; i++) {
412 +            if (v(i, i) < 0) {
413 +                v(i, 0) = -v(i, 0);
414 +                v(i, 1) = -v(i, 1);
415 +                v(i, 2) = -v(i, 2);
416 +            }
417 +        }
418 +
419 +        // set sign of final eigenvector to ensure that determinant is positive
420 +        if (v.determinant() < 0) {
421 +            v(2, 0) = -v(2, 0);
422 +            v(2, 1) = -v(2, 1);
423 +            v(2, 2) = -v(2, 2);
424 +        }
425 +
426 +        // transpose the eigenvectors back again
427 +        v = v.transpose();
428 +        return ;
429 +    }
430 +    typedef SquareMatrix3<double> Mat3x3d;
431 +    typedef SquareMatrix3<double> RotMat3x3d;
432 +
433   } //namespace oopse
434   #endif // MATH_SQUAREMATRIX_HPP
435 +

Diff Legend

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