src/math/SquareMatrix3.hpp

/*
 * Copyright (C) 2000-2004  Object Oriented Parallel Simulation Engine (OOPSE) project
 * 
 * Contact: oopse@oopse.org
 * 
 * This program is free software; you can redistribute it and/or
 * modify it under the terms of the GNU Lesser General Public License
 * as published by the Free Software Foundation; either version 2.1
 * of the License, or (at your option) any later version.
 * All we ask is that proper credit is given for our work, which includes
 * - but is not limited to - adding the above copyright notice to the beginning
 * of your source code files, and to any copyright notice that you may distribute
 * with programs based on this work.
 * 
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU Lesser General Public License for more details.
 * 
 * You should have received a copy of the GNU Lesser General Public License
 * along with this program; if not, write to the Free Software
 * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.
 *
 */

/**
 * @file SquareMatrix3.hpp
 * @author Teng Lin
 * @date 10/11/2004
 * @version 1.0
 */
 #ifndef MATH_SQUAREMATRIX3_HPP
#define  MATH_SQUAREMATRIX3_HPP

#include "Quaternion.hpp"
#include "SquareMatrix.hpp"
#include "Vector3.hpp"

namespace oopse {

    template<typename Real>
    class SquareMatrix3 : public SquareMatrix<Real, 3> {
        public:

            typedef Real ElemType;
            typedef Real* ElemPoinerType;
            
            /** default constructor */
            SquareMatrix3() : SquareMatrix<Real, 3>() {
            }

            /** Constructs and initializes every element of this matrix to a scalar */ 
            SquareMatrix3(Real s) : SquareMatrix<Real,3>(s){
            }

            /** Constructs and initializes from an array */ 
            SquareMatrix3(Real* array) : SquareMatrix<Real,3>(array){
            }


            /** copy  constructor */
            SquareMatrix3(const SquareMatrix<Real, 3>& m)  : SquareMatrix<Real, 3>(m) {
            }
            
            SquareMatrix3( const Vector3<Real>& eulerAngles) {
                setupRotMat(eulerAngles);
            }
            
            SquareMatrix3(Real phi, Real theta, Real psi) {
                setupRotMat(phi, theta, psi);
            }

            SquareMatrix3(const Quaternion<Real>& q) {
                setupRotMat(q);

            }

            SquareMatrix3(Real w, Real x, Real y, Real z) {
                setupRotMat(w, x, y, z);
            }
            
            /** copy assignment operator */
            SquareMatrix3<Real>& operator =(const SquareMatrix<Real, 3>& m) {
                if (this == &m)
                    return *this;
                 SquareMatrix<Real, 3>::operator=(m);
                 return *this;
            }


            SquareMatrix3<Real>& operator =(const Quaternion<Real>& q) {
                this->setupRotMat(q);
                return *this;
            }

            /**
             * Sets this matrix to a rotation matrix by three euler angles
             * @ param euler
             */
            void setupRotMat(const Vector3<Real>& eulerAngles) {
                setupRotMat(eulerAngles[0], eulerAngles[1], eulerAngles[2]);
            }

            /**
             * Sets this matrix to a rotation matrix by three euler angles
             * @param phi
             * @param theta
             * @psi theta
             */
            void setupRotMat(Real phi, Real theta, Real psi) {
                Real sphi, stheta, spsi;
                Real cphi, ctheta, cpsi;

                sphi = sin(phi);
                stheta = sin(theta);
                spsi = sin(psi);
                cphi = cos(phi);
                ctheta = cos(theta);
                cpsi = cos(psi);

                data_[0][0] = cpsi * cphi - ctheta * sphi * spsi;
                data_[0][1] = cpsi * sphi + ctheta * cphi * spsi;
                data_[0][2] = spsi * stheta;
                
                data_[1][0] = -spsi * ctheta - ctheta * sphi * cpsi;
                data_[1][1] = -spsi * stheta + ctheta * cphi * cpsi;
                data_[1][2] = cpsi * stheta;

                data_[2][0] = stheta * sphi;
                data_[2][1] = -stheta * cphi;
                data_[2][2] = ctheta;
            }


            /**
             * Sets this matrix to a rotation matrix by quaternion
             * @param quat
            */
            void setupRotMat(const Quaternion<Real>& quat) {
                setupRotMat(quat.w(), quat.x(), quat.y(), quat.z());
            }

            /**
             * Sets this matrix to a rotation matrix by quaternion
             * @param w the first element 
             * @param x the second element
             * @param y the third element
             * @param z the fourth element
            */
            void setupRotMat(Real w, Real x, Real y, Real z) {
                Quaternion<Real> q(w, x, y, z);
                *this = q.toRotationMatrix3();
            }

            /**
             * Returns the quaternion from this rotation matrix
             * @return the quaternion from this rotation matrix
             * @exception invalid rotation matrix
            */            
            Quaternion<Real> toQuaternion() {
                Quaternion<Real> q;
                Real t, s;
                Real ad1, ad2, ad3;    
                t = data_[0][0] + data_[1][1] + data_[2][2] + 1.0;

                if( t > 0.0 ){

                    s = 0.5 / sqrt( t );
                    q[0] = 0.25 / s;
                    q[1] = (data_[1][2] - data_[2][1]) * s;
                    q[2] = (data_[2][0] - data_[0][2]) * s;
                    q[3] = (data_[0][1] - data_[1][0]) * s;
                } else {

                    ad1 = fabs( data_[0][0] );
                    ad2 = fabs( data_[1][1] );
                    ad3 = fabs( data_[2][2] );

                    if( ad1 >= ad2 && ad1 >= ad3 ){

                        s = 2.0 * sqrt( 1.0 + data_[0][0] - data_[1][1] - data_[2][2] );
                        q[0] = (data_[1][2] + data_[2][1]) / s;
                        q[1] = 0.5 / s;
                        q[2] = (data_[0][1] + data_[1][0]) / s;
                        q[3] = (data_[0][2] + data_[2][0]) / s;
                    } else if ( ad2 >= ad1 && ad2 >= ad3 ) {
                        s = sqrt( 1.0 + data_[1][1] - data_[0][0] - data_[2][2] ) * 2.0;
                        q[0] = (data_[0][2] + data_[2][0]) / s;
                        q[1] = (data_[0][1] + data_[1][0]) / s;
                        q[2] = 0.5 / s;
                        q[3] = (data_[1][2] + data_[2][1]) / s;
                    } else {

                        s = sqrt( 1.0 + data_[2][2] - data_[0][0] - data_[1][1] ) * 2.0;
                        q[0] = (data_[0][1] + data_[1][0]) / s;
                        q[1] = (data_[0][2] + data_[2][0]) / s;
                        q[2] = (data_[1][2] + data_[2][1]) / s;
                        q[3] = 0.5 / s;
                    }
                }             

                return q;
                
            }

            /**
             * Returns the euler angles from this rotation matrix
             * @return the euler angles in a vector 
             * @exception invalid rotation matrix
             * We use so-called "x-convention", which is the most common definition. 
             * In this convention, the rotation given by Euler angles (phi, theta, psi), where the first 
             * rotation is by an angle phi about the z-axis, the second is by an angle  
             * theta (0 <= theta <= 180)about the x-axis, and thethird is by an angle psi about the
             * z-axis (again). 
            */            
            Vector3<Real> toEulerAngles() {
                Vector3<Real> myEuler;
                Real phi;
                Real theta;
                Real psi;
                Real ctheta;
                Real stheta;
                
                // set the tolerance for Euler angles and rotation elements

                theta = acos(std::min(1.0, std::max(-1.0,data_[2][2])));
                ctheta = data_[2][2]; 
                stheta = sqrt(1.0 - ctheta * ctheta);

                // when sin(theta) is close to 0, we need to consider singularity
                // In this case, we can assign an arbitary value to phi (or psi), and then determine 
                // the psi (or phi) or vice-versa. We'll assume that phi always gets the rotation, and psi is 0
                // in cases of singularity.  
                // we use atan2 instead of atan, since atan2 will give us -Pi to Pi. 
                // Since 0 <= theta <= 180, sin(theta) will be always non-negative. Therefore, it never
                // change the sign of both of the parameters passed to atan2.

                if (fabs(stheta) <= oopse::epsilon){
                    psi = 0.0;
                    phi = atan2(-data_[1][0], data_[0][0]);  
                }
                // we only have one unique solution
                else{    
                    phi = atan2(data_[2][0], -data_[2][1]);
                    psi = atan2(data_[0][2], data_[1][2]);
                }

                //wrap phi and psi, make sure they are in the range from 0 to 2*Pi
                if (phi < 0)
                  phi += M_PI;

                if (psi < 0)
                  psi += M_PI;

                myEuler[0] = phi;
                myEuler[1] = theta;
                myEuler[2] = psi;

                return myEuler;
            }
            
            /** Returns the determinant of this matrix. */
            Real determinant() const {
                Real x,y,z;

                x = data_[0][0] * (data_[1][1] * data_[2][2] - data_[1][2] * data_[2][1]);
                y = data_[0][1] * (data_[1][2] * data_[2][0] - data_[1][0] * data_[2][2]);
                z = data_[0][2] * (data_[1][0] * data_[2][1] - data_[1][1] * data_[2][0]);

                return(x + y + z);
            }            

            /** Returns the trace of this matrix. */
            Real trace() const {
                return data_[0][0] + data_[1][1] + data_[2][2];
            }
            
            /**
             * Sets the value of this matrix to  the inversion of itself. 
             * @note since simple algorithm can be applied to inverse the 3 by 3 matrix, we hide the 
             * implementation of inverse in SquareMatrix class
             */
            SquareMatrix3<Real>  inverse() const {
                SquareMatrix3<Real> m;
                double det = determinant();
                if (fabs(det) <= oopse::epsilon) {
                //"The method was called on a matrix with |determinant| <= 1e-6.",
                //"This is a runtime or a programming error in your application.");
                }

                m(0, 0) = data_[1][1]*data_[2][2] - data_[1][2]*data_[2][1];
                m(1, 0) = data_[1][2]*data_[2][0] - data_[1][0]*data_[2][2];
                m(2, 0) = data_[1][0]*data_[2][1] - data_[1][1]*data_[2][0];
                m(0, 1) = data_[2][1]*data_[0][2] - data_[2][2]*data_[0][1];
                m(1, 1) = data_[2][2]*data_[0][0] - data_[2][0]*data_[0][2];
                m(2, 1) = data_[2][0]*data_[0][1] - data_[2][1]*data_[0][0];
                m(0, 2) = data_[0][1]*data_[1][2] - data_[0][2]*data_[1][1];
                m(1, 2) = data_[0][2]*data_[1][0] - data_[0][0]*data_[1][2];
                m(2, 2) = data_[0][0]*data_[1][1] - data_[0][1]*data_[1][0];

                m /= det;
                return m;
            }
            /**
             * Extract the eigenvalues and eigenvectors from a 3x3 matrix.
             * The eigenvectors (the columns of V) will be normalized. 
             * The eigenvectors are aligned optimally with the x, y, and z
             * axes respectively.
             * @param a symmetric matrix whose eigenvectors are to be computed. On return, the matrix is
             *     overwritten             
             * @param w will contain the eigenvalues of the matrix On return of this function
             * @param v the columns of this matrix will contain the eigenvectors. The eigenvectors are 
             *    normalized and mutually orthogonal.              
             * @warning a will be overwritten
             */
            static void diagonalize(SquareMatrix3<Real>& a, Vector3<Real>& w, SquareMatrix3<Real>& v); 
    };
/*=========================================================================

  Program:   Visualization Toolkit
  Module:    $RCSfile: SquareMatrix3.hpp,v $

  Copyright (c) Ken Martin, Will Schroeder, Bill Lorensen
  All rights reserved.
  See Copyright.txt or http://www.kitware.com/Copyright.htm for details.

     This software is distributed WITHOUT ANY WARRANTY; without even
     the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
     PURPOSE.  See the above copyright notice for more information.

=========================================================================*/
    template<typename Real>
    void SquareMatrix3<Real>::diagonalize(SquareMatrix3<Real>& a, Vector3<Real>& w, 
                                                                           SquareMatrix3<Real>& v) {
        int i,j,k,maxI;
        Real tmp, maxVal;
        Vector3<Real> v_maxI, v_k, v_j;

        // diagonalize using Jacobi
        jacobi(a, w, v);
        // if all the eigenvalues are the same, return identity matrix
        if (w[0] == w[1] && w[0] == w[2] ) {
              v = SquareMatrix3<Real>::identity();
              return;
        }

        // transpose temporarily, it makes it easier to sort the eigenvectors
        v = v.transpose(); 
        
        // if two eigenvalues are the same, re-orthogonalize to optimally line
        // up the eigenvectors with the x, y, and z axes
        for (i = 0; i < 3; i++) {
            if (w((i+1)%3) == w((i+2)%3)) {// two eigenvalues are the same
            // find maximum element of the independant eigenvector
            maxVal = fabs(v(i, 0));
            maxI = 0;
            for (j = 1; j < 3; j++) {
                if (maxVal < (tmp = fabs(v(i, j)))){
                    maxVal = tmp;
                    maxI = j;
                }
            }
            
            // swap the eigenvector into its proper position
            if (maxI != i) {
                tmp = w(maxI);
                w(maxI) = w(i);
                w(i) = tmp;

                v.swapRow(i, maxI);
            }
            // maximum element of eigenvector should be positive
            if (v(maxI, maxI) < 0) {
                v(maxI, 0) = -v(maxI, 0);
                v(maxI, 1) = -v(maxI, 1);
                v(maxI, 2) = -v(maxI, 2);
            }

            // re-orthogonalize the other two eigenvectors
            j = (maxI+1)%3;
            k = (maxI+2)%3;

            v(j, 0) = 0.0; 
            v(j, 1) = 0.0; 
            v(j, 2) = 0.0;
            v(j, j) = 1.0;

            /** @todo */
            v_maxI = v.getRow(maxI);
            v_j = v.getRow(j);
            v_k = cross(v_maxI, v_j);
            v_k.normalize();
            v_j = cross(v_k, v_maxI);
            v.setRow(j, v_j);
            v.setRow(k, v_k);


            // transpose vectors back to columns
            v = v.transpose();
            return;
            }
        }

        // the three eigenvalues are different, just sort the eigenvectors
        // to align them with the x, y, and z axes

        // find the vector with the largest x element, make that vector
        // the first vector
        maxVal = fabs(v(0, 0));
        maxI = 0;
        for (i = 1; i < 3; i++) {
            if (maxVal < (tmp = fabs(v(i, 0)))) {
                maxVal = tmp;
                maxI = i;
            }
        }

        // swap eigenvalue and eigenvector
        if (maxI != 0) {
            tmp = w(maxI);
            w(maxI) = w(0);
            w(0) = tmp;
            v.swapRow(maxI, 0);
        }
        // do the same for the y element
        if (fabs(v(1, 1)) < fabs(v(2, 1))) {
            tmp = w(2);
            w(2) = w(1);
            w(1) = tmp;
            v.swapRow(2, 1);
        }

        // ensure that the sign of the eigenvectors is correct
        for (i = 0; i < 2; i++) {
            if (v(i, i) < 0) {
                v(i, 0) = -v(i, 0);
                v(i, 1) = -v(i, 1);
                v(i, 2) = -v(i, 2);
            }
        }

        // set sign of final eigenvector to ensure that determinant is positive
        if (v.determinant() < 0) {
            v(2, 0) = -v(2, 0);
            v(2, 1) = -v(2, 1);
            v(2, 2) = -v(2, 2);
        }

        // transpose the eigenvectors back again
        v = v.transpose();
        return ;
    }

    /**
    * Return the multiplication of two matrixes  (m1 * m2). 
    * @return the multiplication of two matrixes
    * @param m1 the first matrix
    * @param m2 the second matrix
    */
    template<typename Real> 
    inline SquareMatrix3<Real> operator *(const SquareMatrix3<Real>& m1, const SquareMatrix3<Real>& m2) {
        SquareMatrix3<Real> result;

            for (unsigned int i = 0; i < 3; i++)
                for (unsigned int j = 0; j < 3; j++)
                    for (unsigned int k = 0; k < 3; k++)
                        result(i, j)  += m1(i, k) * m2(k, j);                

        return result;
    }

    template<typename Real> 
    inline SquareMatrix3<Real> outProduct(const Vector3<Real>& v1, const Vector3<Real>& v2) {
        SquareMatrix3<Real> result;

            for (unsigned int i = 0; i < 3; i++) {
                for (unsigned int j = 0; j < 3; j++) {
                        result(i, j)  = v1[i] * v2[j];                
                }
            }
            
        return result;        
    }

    
    typedef SquareMatrix3<double> Mat3x3d;
    typedef SquareMatrix3<double> RotMat3x3d;

} //namespace oopse
#endif // MATH_SQUAREMATRIX_HPP

Revision:	1883
Committed:	Mon Dec 13 22:30:27 2004 UTC (19 years, 9 months ago) by tim
File size:	18198 byte(s)
Log Message:	MPI version is built
#	User	Rev	Content
1	tim	1563	/*
2			* Copyright (C) 2000-2004 Object Oriented Parallel Simulation Engine (OOPSE) project
3			*
4			* Contact: oopse@oopse.org
5			*
6			* This program is free software; you can redistribute it and/or
7			* modify it under the terms of the GNU Lesser General Public License
8			* as published by the Free Software Foundation; either version 2.1
9			* of the License, or (at your option) any later version.
10			* All we ask is that proper credit is given for our work, which includes
11			* - but is not limited to - adding the above copyright notice to the beginning
12			* of your source code files, and to any copyright notice that you may distribute
13			* with programs based on this work.
14			*
15			* This program is distributed in the hope that it will be useful,
16			* but WITHOUT ANY WARRANTY; without even the implied warranty of
17			* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
18			* GNU Lesser General Public License for more details.
19			*
20			* You should have received a copy of the GNU Lesser General Public License
21			* along with this program; if not, write to the Free Software
22			* Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
23			*
24			*/
25
26			/**
27			* @file SquareMatrix3.hpp
28			* @author Teng Lin
29			* @date 10/11/2004
30			* @version 1.0
31			*/
32	tim	1616	#ifndef MATH_SQUAREMATRIX3_HPP
33	tim	1592	#define MATH_SQUAREMATRIX3_HPP
34	tim	1563
35	tim	1586	#include "Quaternion.hpp"
36	tim	1563	#include "SquareMatrix.hpp"
37	tim	1586	#include "Vector3.hpp"
38
39	tim	1563	namespace oopse {
40
41			template<typename Real>
42			class SquareMatrix3 : public SquareMatrix<Real, 3> {
43			public:
44	tim	1630
45			typedef Real ElemType;
46			typedef Real* ElemPoinerType;
47	tim	1563
48			/** default constructor */
49			SquareMatrix3() : SquareMatrix<Real, 3>() {
50			}
51
52	tim	1644	/** 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	tim	1563	/** copy constructor */
62			SquareMatrix3(const SquareMatrix<Real, 3>& m) : SquareMatrix<Real, 3>(m) {
63			}
64	tim	1695
65	tim	1586	SquareMatrix3( const Vector3<Real>& eulerAngles) {
66			setupRotMat(eulerAngles);
67			}
68
69			SquareMatrix3(Real phi, Real theta, Real psi) {
70			setupRotMat(phi, theta, psi);
71			}
72
73			SquareMatrix3(const Quaternion<Real>& q) {
74	tim	1606	setupRotMat(q);
75
76	tim	1586	}
77
78			SquareMatrix3(Real w, Real x, Real y, Real z) {
79	tim	1606	setupRotMat(w, x, y, z);
80	tim	1586	}
81
82	tim	1563	/** copy assignment operator */
83			SquareMatrix3<Real>& operator =(const SquareMatrix<Real, 3>& m) {
84			if (this == &m)
85			return *this;
86			SquareMatrix<Real, 3>::operator=(m);
87	tim	1594	return *this;
88	tim	1563	}
89	tim	1569
90	tim	1870
91			SquareMatrix3<Real>& operator =(const Quaternion<Real>& q) {
92			this->setupRotMat(q);
93			return *this;
94			}
95
96	tim	1569	/**
97			* Sets this matrix to a rotation matrix by three euler angles
98			* @ param euler
99			*/
100	tim	1586	void setupRotMat(const Vector3<Real>& eulerAngles) {
101			setupRotMat(eulerAngles[0], eulerAngles[1], eulerAngles[2]);
102			}
103	tim	1569
104			/**
105			* Sets this matrix to a rotation matrix by three euler angles
106			* @param phi
107			* @param theta
108			* @psi theta
109			*/
110	tim	1586	void setupRotMat(Real phi, Real theta, Real psi) {
111			Real sphi, stheta, spsi;
112			Real cphi, ctheta, cpsi;
113	tim	1569
114	tim	1586	sphi = sin(phi);
115			stheta = sin(theta);
116			spsi = sin(psi);
117			cphi = cos(phi);
118			ctheta = cos(theta);
119			cpsi = cos(psi);
120	tim	1569
121	tim	1586	data_[0][0] = cpsi * cphi - ctheta * sphi * spsi;
122			data_[0][1] = cpsi * sphi + ctheta * cphi * spsi;
123			data_[0][2] = spsi * stheta;
124
125			data_[1][0] = -spsi * ctheta - ctheta * sphi * cpsi;
126			data_[1][1] = -spsi * stheta + ctheta * cphi * cpsi;
127			data_[1][2] = cpsi * stheta;
128
129			data_[2][0] = stheta * sphi;
130			data_[2][1] = -stheta * cphi;
131			data_[2][2] = ctheta;
132			}
133
134
135	tim	1569	/**
136			* Sets this matrix to a rotation matrix by quaternion
137			* @param quat
138			*/
139	tim	1586	void setupRotMat(const Quaternion<Real>& quat) {
140	tim	1606	setupRotMat(quat.w(), quat.x(), quat.y(), quat.z());
141	tim	1586	}
142	tim	1569
143			/**
144			* Sets this matrix to a rotation matrix by quaternion
145	tim	1586	* @param w the first element
146			* @param x the second element
147			* @param y the third element
148	tim	1594	* @param z the fourth element
149	tim	1569	*/
150	tim	1586	void setupRotMat(Real w, Real x, Real y, Real z) {
151			Quaternion<Real> q(w, x, y, z);
152			*this = q.toRotationMatrix3();
153			}
154	tim	1569
155			/**
156			* Returns the quaternion from this rotation matrix
157			* @return the quaternion from this rotation matrix
158			* @exception invalid rotation matrix
159			*/
160	tim	1586	Quaternion<Real> toQuaternion() {
161			Quaternion<Real> q;
162			Real t, s;
163			Real ad1, ad2, ad3;
164			t = data_[0][0] + data_[1][1] + data_[2][2] + 1.0;
165	tim	1569
166	tim	1586	if( t > 0.0 ){
167
168			s = 0.5 / sqrt( t );
169			q[0] = 0.25 / s;
170			q[1] = (data_[1][2] - data_[2][1]) * s;
171			q[2] = (data_[2][0] - data_[0][2]) * s;
172			q[3] = (data_[0][1] - data_[1][0]) * s;
173			} else {
174
175			ad1 = fabs( data_[0][0] );
176			ad2 = fabs( data_[1][1] );
177			ad3 = fabs( data_[2][2] );
178
179			if( ad1 >= ad2 && ad1 >= ad3 ){
180
181			s = 2.0 * sqrt( 1.0 + data_[0][0] - data_[1][1] - data_[2][2] );
182			q[0] = (data_[1][2] + data_[2][1]) / s;
183			q[1] = 0.5 / s;
184			q[2] = (data_[0][1] + data_[1][0]) / s;
185			q[3] = (data_[0][2] + data_[2][0]) / s;
186			} else if ( ad2 >= ad1 && ad2 >= ad3 ) {
187			s = sqrt( 1.0 + data_[1][1] - data_[0][0] - data_[2][2] ) * 2.0;
188			q[0] = (data_[0][2] + data_[2][0]) / s;
189			q[1] = (data_[0][1] + data_[1][0]) / s;
190			q[2] = 0.5 / s;
191			q[3] = (data_[1][2] + data_[2][1]) / s;
192			} else {
193
194			s = sqrt( 1.0 + data_[2][2] - data_[0][0] - data_[1][1] ) * 2.0;
195			q[0] = (data_[0][1] + data_[1][0]) / s;
196			q[1] = (data_[0][2] + data_[2][0]) / s;
197			q[2] = (data_[1][2] + data_[2][1]) / s;
198			q[3] = 0.5 / s;
199			}
200			}
201
202			return q;
203
204			}
205
206	tim	1569	/**
207			* Returns the euler angles from this rotation matrix
208	tim	1586	* @return the euler angles in a vector
209	tim	1569	* @exception invalid rotation matrix
210	tim	1586	* We use so-called "x-convention", which is the most common definition.
211			* In this convention, the rotation given by Euler angles (phi, theta, psi), where the first
212			* rotation is by an angle phi about the z-axis, the second is by an angle
213			* theta (0 <= theta <= 180)about the x-axis, and thethird is by an angle psi about the
214			* z-axis (again).
215	tim	1569	*/
216	tim	1586	Vector3<Real> toEulerAngles() {
217	tim	1606	Vector3<Real> myEuler;
218	tim	1883	Real phi;
219			Real theta;
220			Real psi;
221			Real ctheta;
222			Real stheta;
223	tim	1586
224			// set the tolerance for Euler angles and rotation elements
225
226	tim	1606	theta = acos(std::min(1.0, std::max(-1.0,data_[2][2])));
227	tim	1586	ctheta = data_[2][2];
228			stheta = sqrt(1.0 - ctheta * ctheta);
229
230			// when sin(theta) is close to 0, we need to consider singularity
231			// In this case, we can assign an arbitary value to phi (or psi), and then determine
232			// the psi (or phi) or vice-versa. We'll assume that phi always gets the rotation, and psi is 0
233			// in cases of singularity.
234			// we use atan2 instead of atan, since atan2 will give us -Pi to Pi.
235			// Since 0 <= theta <= 180, sin(theta) will be always non-negative. Therefore, it never
236			// change the sign of both of the parameters passed to atan2.
237
238			if (fabs(stheta) <= oopse::epsilon){
239			psi = 0.0;
240			phi = atan2(-data_[1][0], data_[0][0]);
241			}
242			// we only have one unique solution
243			else{
244			phi = atan2(data_[2][0], -data_[2][1]);
245			psi = atan2(data_[0][2], data_[1][2]);
246			}
247
248			//wrap phi and psi, make sure they are in the range from 0 to 2*Pi
249			if (phi < 0)
250			phi += M_PI;
251
252			if (psi < 0)
253			psi += M_PI;
254
255			myEuler[0] = phi;
256			myEuler[1] = theta;
257			myEuler[2] = psi;
258
259			return myEuler;
260			}
261	tim	1563
262	tim	1594	/** Returns the determinant of this matrix. */
263			Real determinant() const {
264			Real x,y,z;
265
266			x = data_[0][0] * (data_[1][1] * data_[2][2] - data_[1][2] * data_[2][1]);
267			y = data_[0][1] * (data_[1][2] * data_[2][0] - data_[1][0] * data_[2][2]);
268			z = data_[0][2] * (data_[1][0] * data_[2][1] - data_[1][1] * data_[2][0]);
269
270			return(x + y + z);
271			}
272	tim	1822
273			/** Returns the trace of this matrix. */
274			Real trace() const {
275			return data_[0][0] + data_[1][1] + data_[2][2];
276			}
277	tim	1594
278	tim	1563	/**
279			* Sets the value of this matrix to the inversion of itself.
280			* @note since simple algorithm can be applied to inverse the 3 by 3 matrix, we hide the
281			* implementation of inverse in SquareMatrix class
282			*/
283	tim	1695	SquareMatrix3<Real> inverse() const {
284	tim	1594	SquareMatrix3<Real> m;
285			double det = determinant();
286			if (fabs(det) <= oopse::epsilon) {
287			//"The method was called on a matrix with \|determinant\| <= 1e-6.",
288			//"This is a runtime or a programming error in your application.");
289			}
290	tim	1563
291	tim	1594	m(0, 0) = data_[1][1]data_[2][2] - data_[1][2]data_[2][1];
292			m(1, 0) = data_[1][2]data_[2][0] - data_[1][0]data_[2][2];
293			m(2, 0) = data_[1][0]data_[2][1] - data_[1][1]data_[2][0];
294			m(0, 1) = data_[2][1]data_[0][2] - data_[2][2]data_[0][1];
295			m(1, 1) = data_[2][2]data_[0][0] - data_[2][0]data_[0][2];
296			m(2, 1) = data_[2][0]data_[0][1] - data_[2][1]data_[0][0];
297			m(0, 2) = data_[0][1]data_[1][2] - data_[0][2]data_[1][1];
298			m(1, 2) = data_[0][2]data_[1][0] - data_[0][0]data_[1][2];
299			m(2, 2) = data_[0][0]data_[1][1] - data_[0][1]data_[1][0];
300
301			m /= det;
302			return m;
303	tim	1592	}
304	tim	1616	/**
305			* Extract the eigenvalues and eigenvectors from a 3x3 matrix.
306			* The eigenvectors (the columns of V) will be normalized.
307			* The eigenvectors are aligned optimally with the x, y, and z
308			* axes respectively.
309			* @param a symmetric matrix whose eigenvectors are to be computed. On return, the matrix is
310			* overwritten
311			* @param w will contain the eigenvalues of the matrix On return of this function
312			* @param v the columns of this matrix will contain the eigenvectors. The eigenvectors are
313			* normalized and mutually orthogonal.
314			* @warning a will be overwritten
315			*/
316			static void diagonalize(SquareMatrix3<Real>& a, Vector3<Real>& w, SquareMatrix3<Real>& v);
317			};
318			/*=========================================================================
319	tim	1569
320	tim	1616	Program: Visualization Toolkit
321			Module: $RCSfile: SquareMatrix3.hpp,v $
322	tim	1592
323	tim	1616	Copyright (c) Ken Martin, Will Schroeder, Bill Lorensen
324			All rights reserved.
325			See Copyright.txt or http://www.kitware.com/Copyright.htm for details.
326	tim	1594
327	tim	1616	This software is distributed WITHOUT ANY WARRANTY; without even
328			the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
329			PURPOSE. See the above copyright notice for more information.
330	tim	1594
331	tim	1616	=========================================================================*/
332			template<typename Real>
333			void SquareMatrix3<Real>::diagonalize(SquareMatrix3<Real>& a, Vector3<Real>& w,
334			SquareMatrix3<Real>& v) {
335			int i,j,k,maxI;
336			Real tmp, maxVal;
337			Vector3<Real> v_maxI, v_k, v_j;
338	tim	1594
339	tim	1616	// diagonalize using Jacobi
340			jacobi(a, w, v);
341			// if all the eigenvalues are the same, return identity matrix
342			if (w[0] == w[1] && w[0] == w[2] ) {
343			v = SquareMatrix3<Real>::identity();
344			return;
345			}
346	tim	1594
347	tim	1616	// transpose temporarily, it makes it easier to sort the eigenvectors
348			v = v.transpose();
349
350			// if two eigenvalues are the same, re-orthogonalize to optimally line
351			// up the eigenvectors with the x, y, and z axes
352			for (i = 0; i < 3; i++) {
353			if (w((i+1)%3) == w((i+2)%3)) {// two eigenvalues are the same
354			// find maximum element of the independant eigenvector
355			maxVal = fabs(v(i, 0));
356			maxI = 0;
357			for (j = 1; j < 3; j++) {
358			if (maxVal < (tmp = fabs(v(i, j)))){
359			maxVal = tmp;
360			maxI = j;
361			}
362			}
363
364			// swap the eigenvector into its proper position
365			if (maxI != i) {
366			tmp = w(maxI);
367			w(maxI) = w(i);
368			w(i) = tmp;
369	tim	1594
370	tim	1616	v.swapRow(i, maxI);
371			}
372			// maximum element of eigenvector should be positive
373			if (v(maxI, maxI) < 0) {
374			v(maxI, 0) = -v(maxI, 0);
375			v(maxI, 1) = -v(maxI, 1);
376			v(maxI, 2) = -v(maxI, 2);
377			}
378	tim	1594
379	tim	1616	// re-orthogonalize the other two eigenvectors
380			j = (maxI+1)%3;
381			k = (maxI+2)%3;
382	tim	1594
383	tim	1616	v(j, 0) = 0.0;
384			v(j, 1) = 0.0;
385			v(j, 2) = 0.0;
386			v(j, j) = 1.0;
387	tim	1594
388	tim	1616	/** @todo */
389			v_maxI = v.getRow(maxI);
390			v_j = v.getRow(j);
391			v_k = cross(v_maxI, v_j);
392			v_k.normalize();
393			v_j = cross(v_k, v_maxI);
394			v.setRow(j, v_j);
395			v.setRow(k, v_k);
396	tim	1594
397
398	tim	1616	// transpose vectors back to columns
399			v = v.transpose();
400			return;
401			}
402			}
403	tim	1594
404	tim	1616	// the three eigenvalues are different, just sort the eigenvectors
405			// to align them with the x, y, and z axes
406	tim	1594
407	tim	1616	// find the vector with the largest x element, make that vector
408			// the first vector
409			maxVal = fabs(v(0, 0));
410			maxI = 0;
411			for (i = 1; i < 3; i++) {
412			if (maxVal < (tmp = fabs(v(i, 0)))) {
413			maxVal = tmp;
414			maxI = i;
415			}
416			}
417	tim	1594
418	tim	1616	// swap eigenvalue and eigenvector
419			if (maxI != 0) {
420			tmp = w(maxI);
421			w(maxI) = w(0);
422			w(0) = tmp;
423			v.swapRow(maxI, 0);
424			}
425			// do the same for the y element
426			if (fabs(v(1, 1)) < fabs(v(2, 1))) {
427			tmp = w(2);
428			w(2) = w(1);
429			w(1) = tmp;
430			v.swapRow(2, 1);
431			}
432	tim	1594
433	tim	1616	// ensure that the sign of the eigenvectors is correct
434			for (i = 0; i < 2; i++) {
435			if (v(i, i) < 0) {
436			v(i, 0) = -v(i, 0);
437			v(i, 1) = -v(i, 1);
438			v(i, 2) = -v(i, 2);
439	tim	1592	}
440	tim	1616	}
441	tim	1563
442	tim	1616	// set sign of final eigenvector to ensure that determinant is positive
443			if (v.determinant() < 0) {
444			v(2, 0) = -v(2, 0);
445			v(2, 1) = -v(2, 1);
446			v(2, 2) = -v(2, 2);
447			}
448
449			// transpose the eigenvectors back again
450			v = v.transpose();
451			return ;
452			}
453	tim	1695
454			/**
455			* Return the multiplication of two matrixes (m1 * m2).
456			* @return the multiplication of two matrixes
457			* @param m1 the first matrix
458			* @param m2 the second matrix
459			*/
460			template<typename Real>
461			inline SquareMatrix3<Real> operator *(const SquareMatrix3<Real>& m1, const SquareMatrix3<Real>& m2) {
462			SquareMatrix3<Real> result;
463
464			for (unsigned int i = 0; i < 3; i++)
465			for (unsigned int j = 0; j < 3; j++)
466			for (unsigned int k = 0; k < 3; k++)
467			result(i, j) += m1(i, k) * m2(k, j);
468
469			return result;
470			}
471
472	tim	1804	template<typename Real>
473			inline SquareMatrix3<Real> outProduct(const Vector3<Real>& v1, const Vector3<Real>& v2) {
474			SquareMatrix3<Real> result;
475
476			for (unsigned int i = 0; i < 3; i++) {
477			for (unsigned int j = 0; j < 3; j++) {
478			result(i, j) = v1[i] * v2[j];
479			}
480			}
481
482			return result;
483			}
484
485
486	tim	1592	typedef SquareMatrix3<double> Mat3x3d;
487			typedef SquareMatrix3<double> RotMat3x3d;
488	tim	1586
489			} //namespace oopse
490			#endif // MATH_SQUAREMATRIX_HPP
491	tim	1616