src/math/SquareMatrix3.hpp

 /*
 * Copyright (c) 2005 The University of Notre Dame. All Rights Reserved.
 *
 * The University of Notre Dame grants you ("Licensee") a
 * non-exclusive, royalty free, license to use, modify and
 * redistribute this software in source and binary code form, provided
 * that the following conditions are met:
 *
 * 1. Acknowledgement of the program authors must be made in any
 *    publication of scientific results based in part on use of the
 *    program.  An acceptable form of acknowledgement is citation of
 *    the article in which the program was described (Matthew
 *    A. Meineke, Charles F. Vardeman II, Teng Lin, Christopher
 *    J. Fennell and J. Daniel Gezelter, "OOPSE: An Object-Oriented
 *    Parallel Simulation Engine for Molecular Dynamics,"
 *    J. Comput. Chem. 26, pp. 252-271 (2005))
 *
 * 2. Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 *
 * 3. Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in the
 *    documentation and/or other materials provided with the
 *    distribution.
 *
 * This software is provided "AS IS," without a warranty of any
 * kind. All express or implied conditions, representations and
 * warranties, including any implied warranty of merchantability,
 * fitness for a particular purpose or non-infringement, are hereby
 * excluded.  The University of Notre Dame and its licensors shall not
 * be liable for any damages suffered by licensee as a result of
 * using, modifying or distributing the software or its
 * derivatives. In no event will the University of Notre Dame or its
 * licensors be liable for any lost revenue, profit or data, or for
 * direct, indirect, special, consequential, incidental or punitive
 * damages, however caused and regardless of the theory of liability,
 * arising out of the use of or inability to use software, even if the
 * University of Notre Dame has been advised of the possibility of
 * such damages.
 */
 
/**
 * @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);

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

                this->data_[2][0] = stheta * sphi;
                this->data_[2][1] = -stheta * cphi;
                this->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 = this->data_[0][0] + this->data_[1][1] + this->data_[2][2] + 1.0;

                if( t > 0.0 ){

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

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

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

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

                        s = sqrt( 1.0 + this->data_[2][2] - this->data_[0][0] - this->data_[1][1] ) * 2.0;
                        q[0] = (this->data_[0][1] + this->data_[1][0]) / s;
                        q[1] = (this->data_[0][2] + this->data_[2][0]) / s;
                        q[2] = (this->data_[1][2] + this->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,this->data_[2][2])));
                ctheta = this->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(-this->data_[1][0], this->data_[0][0]);  
                }
                // we only have one unique solution
                else{    
                    phi = atan2(this->data_[2][0], -this->data_[2][1]);
                    psi = atan2(this->data_[0][2], this->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 = this->data_[0][0] * (this->data_[1][1] * this->data_[2][2] - this->data_[1][2] * this->data_[2][1]);
                y = this->data_[0][1] * (this->data_[1][2] * this->data_[2][0] - this->data_[1][0] * this->data_[2][2]);
                z = this->data_[0][2] * (this->data_[1][0] * this->data_[2][1] - this->data_[1][1] * this->data_[2][0]);

                return(x + y + z);
            }            

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