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:
            
            /** default constructor */
            SquareMatrix3() : SquareMatrix<Real, 3>() {
            }

            /** 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) {
                *this = q.toRotationMatrix3();
            }

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

            /**
             * 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) {
                *this = quat.toRotationMatrix3();
            }

            /**
             * Sets this matrix to a rotation matrix by quaternion
             * @param w the first element 
             * @param x the second element
             * @param y the third element
             * @parma 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() {
                Vector<Real> myEuler;
                Real phi,theta,psi,eps;
                Real ctheta,stheta;
                
                // set the tolerance for Euler angles and rotation elements

                theta = acos(min(1.0,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;
            }
            
            /**
             * 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
             */
            void  inverse() {

            }

            void diagonalize() {

            }
    };

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

} //namespace oopse
#endif // MATH_SQUAREMATRIX_HPP
Revision:	1592
Committed:	Mon Oct 18 17:07:27 2004 UTC (19 years, 8 months ago) by tim
File size:	9645 byte(s)
Log Message:	fix some bugs in Quaternion class
#	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	1592	#ifndef MATH_SQUAREMATRIX3_HPP
33			#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
45			/** default constructor */
46			SquareMatrix3() : SquareMatrix<Real, 3>() {
47			}
48
49			/** copy constructor */
50			SquareMatrix3(const SquareMatrix<Real, 3>& m) : SquareMatrix<Real, 3>(m) {
51			}
52
53	tim	1586	SquareMatrix3( const Vector3<Real>& eulerAngles) {
54			setupRotMat(eulerAngles);
55			}
56
57			SquareMatrix3(Real phi, Real theta, Real psi) {
58			setupRotMat(phi, theta, psi);
59			}
60
61			SquareMatrix3(const Quaternion<Real>& q) {
62			*this = q.toRotationMatrix3();
63			}
64
65			SquareMatrix3(Real w, Real x, Real y, Real z) {
66			Quaternion<Real> q(w, x, y, z);
67			*this = q.toRotationMatrix3();
68			}
69
70	tim	1563	/** copy assignment operator */
71			SquareMatrix3<Real>& operator =(const SquareMatrix<Real, 3>& m) {
72			if (this == &m)
73			return *this;
74			SquareMatrix<Real, 3>::operator=(m);
75			}
76	tim	1569
77			/**
78			* Sets this matrix to a rotation matrix by three euler angles
79			* @ param euler
80			*/
81	tim	1586	void setupRotMat(const Vector3<Real>& eulerAngles) {
82			setupRotMat(eulerAngles[0], eulerAngles[1], eulerAngles[2]);
83			}
84	tim	1569
85			/**
86			* Sets this matrix to a rotation matrix by three euler angles
87			* @param phi
88			* @param theta
89			* @psi theta
90			*/
91	tim	1586	void setupRotMat(Real phi, Real theta, Real psi) {
92			Real sphi, stheta, spsi;
93			Real cphi, ctheta, cpsi;
94	tim	1569
95	tim	1586	sphi = sin(phi);
96			stheta = sin(theta);
97			spsi = sin(psi);
98			cphi = cos(phi);
99			ctheta = cos(theta);
100			cpsi = cos(psi);
101	tim	1569
102	tim	1586	data_[0][0] = cpsi * cphi - ctheta * sphi * spsi;
103			data_[0][1] = cpsi * sphi + ctheta * cphi * spsi;
104			data_[0][2] = spsi * stheta;
105
106			data_[1][0] = -spsi * ctheta - ctheta * sphi * cpsi;
107			data_[1][1] = -spsi * stheta + ctheta * cphi * cpsi;
108			data_[1][2] = cpsi * stheta;
109
110			data_[2][0] = stheta * sphi;
111			data_[2][1] = -stheta * cphi;
112			data_[2][2] = ctheta;
113			}
114
115
116	tim	1569	/**
117			* Sets this matrix to a rotation matrix by quaternion
118			* @param quat
119			*/
120	tim	1586	void setupRotMat(const Quaternion<Real>& quat) {
121			*this = quat.toRotationMatrix3();
122			}
123	tim	1569
124			/**
125			* Sets this matrix to a rotation matrix by quaternion
126	tim	1586	* @param w the first element
127			* @param x the second element
128			* @param y the third element
129			* @parma z the fourth element
130	tim	1569	*/
131	tim	1586	void setupRotMat(Real w, Real x, Real y, Real z) {
132			Quaternion<Real> q(w, x, y, z);
133			*this = q.toRotationMatrix3();
134			}
135	tim	1569
136			/**
137			* Returns the quaternion from this rotation matrix
138			* @return the quaternion from this rotation matrix
139			* @exception invalid rotation matrix
140			*/
141	tim	1586	Quaternion<Real> toQuaternion() {
142			Quaternion<Real> q;
143			Real t, s;
144			Real ad1, ad2, ad3;
145			t = data_[0][0] + data_[1][1] + data_[2][2] + 1.0;
146	tim	1569
147	tim	1586	if( t > 0.0 ){
148
149			s = 0.5 / sqrt( t );
150			q[0] = 0.25 / s;
151			q[1] = (data_[1][2] - data_[2][1]) * s;
152			q[2] = (data_[2][0] - data_[0][2]) * s;
153			q[3] = (data_[0][1] - data_[1][0]) * s;
154			} else {
155
156			ad1 = fabs( data_[0][0] );
157			ad2 = fabs( data_[1][1] );
158			ad3 = fabs( data_[2][2] );
159
160			if( ad1 >= ad2 && ad1 >= ad3 ){
161
162			s = 2.0 * sqrt( 1.0 + data_[0][0] - data_[1][1] - data_[2][2] );
163			q[0] = (data_[1][2] + data_[2][1]) / s;
164			q[1] = 0.5 / s;
165			q[2] = (data_[0][1] + data_[1][0]) / s;
166			q[3] = (data_[0][2] + data_[2][0]) / s;
167			} else if ( ad2 >= ad1 && ad2 >= ad3 ) {
168			s = sqrt( 1.0 + data_[1][1] - data_[0][0] - data_[2][2] ) * 2.0;
169			q[0] = (data_[0][2] + data_[2][0]) / s;
170			q[1] = (data_[0][1] + data_[1][0]) / s;
171			q[2] = 0.5 / s;
172			q[3] = (data_[1][2] + data_[2][1]) / s;
173			} else {
174
175			s = sqrt( 1.0 + data_[2][2] - data_[0][0] - data_[1][1] ) * 2.0;
176			q[0] = (data_[0][1] + data_[1][0]) / s;
177			q[1] = (data_[0][2] + data_[2][0]) / s;
178			q[2] = (data_[1][2] + data_[2][1]) / s;
179			q[3] = 0.5 / s;
180			}
181			}
182
183			return q;
184
185			}
186
187	tim	1569	/**
188			* Returns the euler angles from this rotation matrix
189	tim	1586	* @return the euler angles in a vector
190	tim	1569	* @exception invalid rotation matrix
191	tim	1586	* We use so-called "x-convention", which is the most common definition.
192			* In this convention, the rotation given by Euler angles (phi, theta, psi), where the first
193			* rotation is by an angle phi about the z-axis, the second is by an angle
194			* theta (0 <= theta <= 180)about the x-axis, and thethird is by an angle psi about the
195			* z-axis (again).
196	tim	1569	*/
197	tim	1586	Vector3<Real> toEulerAngles() {
198			Vector<Real> myEuler;
199			Real phi,theta,psi,eps;
200			Real ctheta,stheta;
201
202			// set the tolerance for Euler angles and rotation elements
203
204			theta = acos(min(1.0,max(-1.0,data_[2][2])));
205			ctheta = data_[2][2];
206			stheta = sqrt(1.0 - ctheta * ctheta);
207
208			// when sin(theta) is close to 0, we need to consider singularity
209			// In this case, we can assign an arbitary value to phi (or psi), and then determine
210			// the psi (or phi) or vice-versa. We'll assume that phi always gets the rotation, and psi is 0
211			// in cases of singularity.
212			// we use atan2 instead of atan, since atan2 will give us -Pi to Pi.
213			// Since 0 <= theta <= 180, sin(theta) will be always non-negative. Therefore, it never
214			// change the sign of both of the parameters passed to atan2.
215
216			if (fabs(stheta) <= oopse::epsilon){
217			psi = 0.0;
218			phi = atan2(-data_[1][0], data_[0][0]);
219			}
220			// we only have one unique solution
221			else{
222			phi = atan2(data_[2][0], -data_[2][1]);
223			psi = atan2(data_[0][2], data_[1][2]);
224			}
225
226			//wrap phi and psi, make sure they are in the range from 0 to 2*Pi
227			if (phi < 0)
228			phi += M_PI;
229
230			if (psi < 0)
231			psi += M_PI;
232
233			myEuler[0] = phi;
234			myEuler[1] = theta;
235			myEuler[2] = psi;
236
237			return myEuler;
238			}
239	tim	1563
240			/**
241			* Sets the value of this matrix to the inversion of itself.
242			* @note since simple algorithm can be applied to inverse the 3 by 3 matrix, we hide the
243			* implementation of inverse in SquareMatrix class
244			*/
245	tim	1592	void inverse() {
246	tim	1563
247	tim	1592	}
248	tim	1569
249	tim	1592	void diagonalize() {
250
251			}
252	tim	1563	};
253
254	tim	1592	typedef SquareMatrix3<double> Mat3x3d;
255			typedef SquareMatrix3<double> RotMat3x3d;
256	tim	1586
257			} //namespace oopse
258			#endif // MATH_SQUAREMATRIX_HPP