src/math/Quaternion.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 Quaternion.hpp
 * @author Teng Lin
 * @date 10/11/2004
 * @version 1.0
 */

#ifndef MATH_QUATERNION_HPP
#define MATH_QUATERNION_HPP

#include "math/Vector.hpp"
#include "math/SquareMatrix.hpp"

namespace oopse{

    /**
     * @class Quaternion Quaternion.hpp "math/Quaternion.hpp"
     * Quaternion is a sort of a higher-level complex number.
     * It is defined as Q = w + x*i + y*j + z*k,
     * where w, x, y, and z are numbers of type T (e.g. double), and
     * i*i = -1; j*j = -1; k*k = -1;
     * i*j = k; j*k = i; k*i = j;
     */
    template<typename Real>
    class Quaternion : public Vector<Real, 4> {
        public:
            Quaternion() : Vector<Real, 4>() {}

            /** Constructs and initializes a Quaternion from w, x, y, z values */     
            Quaternion(Real w, Real x, Real y, Real z) {
                data_[0] = w;
                data_[1] = x;
                data_[2] = y;
                data_[3] = z;                
            }
            
            /** Constructs and initializes a Quaternion from a  Vector<Real,4> */     
            Quaternion(const Vector<Real,4>& v) 
                : Vector<Real, 4>(v){
            }

            /** copy assignment */
            Quaternion& operator =(const Vector<Real, 4>& v){
                if (this == & v)
                    return *this;

                Vector<Real, 4>::operator=(v);
                
                return *this;
            }

            /**
             * Returns the value of the first element of this quaternion.
             * @return the value of the first element of this quaternion
             */
            Real w() const {
                return data_[0];
            }

            /**
             * Returns the reference of the first element of this quaternion.
             * @return the reference of the first element of this quaternion
             */
            Real& w() {
                return data_[0];    
            }

            /**
             * Returns the value of the first element of this quaternion.
             * @return the value of the first element of this quaternion
             */
            Real x() const {
                return data_[1];
            }

            /**
             * Returns the reference of the second element of this quaternion.
             * @return the reference of the second element of this quaternion
             */
            Real& x() {
                return data_[1];    
            }

            /**
             * Returns the value of the thirf element of this quaternion.
             * @return the value of the third element of this quaternion
             */
            Real y() const {
                return data_[2];
            }

            /**
             * Returns the reference of the third element of this quaternion.
             * @return the reference of the third element of this quaternion
             */           
            Real& y() {
                return data_[2];    
            }

            /**
             * Returns the value of the fourth element of this quaternion.
             * @return the value of the fourth element of this quaternion
             */
            Real z() const {
                return data_[3];
            }
            /**
             * Returns the reference of the fourth element of this quaternion.
             * @return the reference of the fourth element of this quaternion
             */
            Real& z() {
                return data_[3];    
            }

            /**
             * Tests if this quaternion is equal to other quaternion
             * @return true if equal, otherwise return false
             * @param q quaternion to be compared
             */
             inline bool operator ==(const Quaternion<Real>& q) {

                for (unsigned int i = 0; i < 4; i ++) {
                    if (!equal(data_[i], q[i])) {
                        return false;
                    }
                }
                
                return true;
            }
            
            /**
             * Returns the inverse of this quaternion
             * @return inverse
             * @note since quaternion is a complex number, the inverse of quaternion
             * q = w + xi + yj+ zk is inv_q = (w -xi - yj - zk)/(|q|^2)
             */
            Quaternion<Real> inverse() {
                Quaternion<Real> q;
                Real d = this->lengthSquare();
                
                q.w() = w() / d;
                q.x() = -x() / d;
                q.y() = -y() / d;
                q.z() = -z() / d;
                
                return q;
            }

            /**
             * Sets the value to the multiplication of itself and another quaternion
             * @param q the other quaternion
             */
            void mul(const Quaternion<Real>& q) {
                Quaternion<Real> tmp(*this);

                data_[0] = (tmp[0]*q[0]) -(tmp[1]*q[1]) - (tmp[2]*q[2]) - (tmp[3]*q[3]);
                data_[1] = (tmp[0]*q[1]) + (tmp[1]*q[0]) + (tmp[2]*q[3]) - (tmp[3]*q[2]);
                data_[2] = (tmp[0]*q[2]) + (tmp[2]*q[0]) + (tmp[3]*q[1]) - (tmp[1]*q[3]);
                data_[3] = (tmp[0]*q[3]) + (tmp[3]*q[0]) + (tmp[1]*q[2]) - (tmp[2]*q[1]);                
            }

            void mul(const Real& s) {
                data_[0] *= s;
                data_[1] *= s;
                data_[2] *= s;
                data_[3] *= s;
            }

            /** Set the value of this quaternion to the division of itself by another quaternion */
            void div(Quaternion<Real>& q) {
                mul(q.inverse());
            }

            void div(const Real& s) {
                data_[0] /= s;
                data_[1] /= s;
                data_[2] /= s;
                data_[3] /= s;
            }
            
            Quaternion<Real>& operator *=(const Quaternion<Real>& q) {
                mul(q);
                return *this;
            }

            Quaternion<Real>& operator *=(const Real& s) {
                mul(s);
                return *this;
            }
            
            Quaternion<Real>& operator /=(Quaternion<Real>& q) {                
                *this *= q.inverse();
                return *this;
            }

            Quaternion<Real>& operator /=(const Real& s) {
                div(s);
                return *this;
            }            
            /**
             * Returns the conjugate quaternion of this quaternion
             * @return the conjugate quaternion of this quaternion
             */
            Quaternion<Real> conjugate() {
                return Quaternion<Real>(w(), -x(), -y(), -z());            
            }

            /**
             * Returns the corresponding rotation matrix (3x3)
             * @return a 3x3 rotation matrix
             */
            SquareMatrix<Real, 3> toRotationMatrix3() {
                SquareMatrix<Real, 3> rotMat3;

                Real w2;
                Real x2;
                Real y2;
                Real z2;

                if (!isNormalized())
                    normalize();
                
                w2 = w() * w();
                x2 = x() * x();
                y2 = y() * y();
                z2 = z() * z();

                rotMat3(0, 0) = w2 + x2 - y2 - z2;
                rotMat3(0, 1) = 2.0 * ( x() * y() + w() * z() );
                rotMat3(0, 2) = 2.0 * ( x() * z() - w() * y() );

                rotMat3(1, 0) = 2.0 * ( x() * y() - w() * z() );
                rotMat3(1, 1) = w2 - x2 + y2 - z2;
                rotMat3(1, 2) = 2.0 * ( y() * z() + w() * x() );

                rotMat3(2, 0) = 2.0 * ( x() * z() + w() * y() );
                rotMat3(2, 1) = 2.0 * ( y() * z() - w() * x() );
                rotMat3(2, 2) = w2 - x2 -y2 +z2;

                return rotMat3;
            }

    };//end Quaternion


    /**
     * Returns the vaule of scalar multiplication of this quaterion q (q * s). 
     * @return  the vaule of scalar multiplication of this vector
     * @param q the source quaternion
     * @param s the scalar value
     */
    template<typename Real, unsigned int Dim>                 
    Quaternion<Real> operator * ( const Quaternion<Real>& q, Real s) {       
        Quaternion<Real> result(q);
        result.mul(s);
        return result;           
    }
    
    /**
     * Returns the vaule of scalar multiplication of this quaterion q (q * s). 
     * @return  the vaule of scalar multiplication of this vector
     * @param s the scalar value
     * @param q the source quaternion
     */  
    template<typename Real, unsigned int Dim>
    Quaternion<Real> operator * ( const Real& s, const Quaternion<Real>& q ) {
        Quaternion<Real> result(q);
        result.mul(s);
        return result;           
    }    

    /**
     * Returns the multiplication of two quaternion
     * @return the multiplication of two quaternion
     * @param q1 the first quaternion
     * @param q2 the second quaternion
     */
    template<typename Real>
    inline Quaternion<Real> operator *(const Quaternion<Real>& q1, const Quaternion<Real>& q2) {
        Quaternion<Real> result(q1);
        result *= q2;
        return result;
    }

    /**
     * Returns the division of two quaternion
     * @param q1 divisor
     * @param q2 dividen
     */

    template<typename Real>
    inline Quaternion<Real> operator /( Quaternion<Real>& q1,  Quaternion<Real>& q2) {
        return q1 * q2.inverse();
    }

    /**
     * Returns the value of the division of a scalar by a quaternion
     * @return the value of the division of a scalar by a quaternion
     * @param s scalar
     * @param q quaternion
     * @note for a quaternion q, 1/q = q.inverse()
     */
    template<typename Real>
    Quaternion<Real> operator /(const Real& s,  Quaternion<Real>& q) {

        Quaternion<Real> x;
        x = q.inverse();
        x *= s;
        return x;
    }
    
    template <class T>
    inline bool operator==(const Quaternion<T>& lhs, const Quaternion<T>& rhs) {
        return equal(lhs[0] ,rhs[0]) && equal(lhs[1] , rhs[1]) && equal(lhs[2], rhs[2]) && equal(lhs[3], rhs[3]);
    }
    
    typedef Quaternion<double> Quat4d;
}
#endif //MATH_QUATERNION_HPP 
Revision:	1603
Committed:	Tue Oct 19 21:28:55 2004 UTC (19 years, 8 months ago) by tim
File size:	11490 byte(s)
Log Message:	more bugs get fixed at math library
#	User	Rev	Content
1	tim	1585	/*
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 Quaternion.hpp
28			* @author Teng Lin
29			* @date 10/11/2004
30			* @version 1.0
31			*/
32
33			#ifndef MATH_QUATERNION_HPP
34			#define MATH_QUATERNION_HPP
35
36	tim	1586	#include "math/Vector.hpp"
37	tim	1603	#include "math/SquareMatrix.hpp"
38	tim	1586
39	tim	1585	namespace oopse{
40
41			/**
42			* @class Quaternion Quaternion.hpp "math/Quaternion.hpp"
43	tim	1586	* Quaternion is a sort of a higher-level complex number.
44			* It is defined as Q = w + xi + yj + z*k,
45			* where w, x, y, and z are numbers of type T (e.g. double), and
46			* ii = -1; jj = -1; k*k = -1;
47			* ij = k; jk = i; k*i = j;
48	tim	1585	*/
49			template<typename Real>
50			class Quaternion : public Vector<Real, 4> {
51	tim	1586	public:
52	tim	1603	Quaternion() : Vector<Real, 4>() {}
53	tim	1585
54	tim	1586	/** Constructs and initializes a Quaternion from w, x, y, z values */
55			Quaternion(Real w, Real x, Real y, Real z) {
56			data_[0] = w;
57			data_[1] = x;
58			data_[2] = y;
59			data_[3] = z;
60			}
61
62	tim	1603	/** Constructs and initializes a Quaternion from a Vector<Real,4> */
63	tim	1586	Quaternion(const Vector<Real,4>& v)
64			: Vector<Real, 4>(v){
65			}
66	tim	1585
67	tim	1603	/** copy assignment */
68	tim	1586	Quaternion& operator =(const Vector<Real, 4>& v){
69			if (this == & v)
70			return *this;
71
72			Vector<Real, 4>::operator=(v);
73
74			return *this;
75			}
76
77			/**
78			* Returns the value of the first element of this quaternion.
79			* @return the value of the first element of this quaternion
80			*/
81			Real w() const {
82			return data_[0];
83			}
84
85			/**
86			* Returns the reference of the first element of this quaternion.
87			* @return the reference of the first element of this quaternion
88			*/
89			Real& w() {
90			return data_[0];
91			}
92
93			/**
94			* Returns the value of the first element of this quaternion.
95			* @return the value of the first element of this quaternion
96			*/
97			Real x() const {
98			return data_[1];
99			}
100
101			/**
102			* Returns the reference of the second element of this quaternion.
103			* @return the reference of the second element of this quaternion
104			*/
105			Real& x() {
106			return data_[1];
107			}
108
109			/**
110			* Returns the value of the thirf element of this quaternion.
111			* @return the value of the third element of this quaternion
112			*/
113			Real y() const {
114			return data_[2];
115			}
116
117			/**
118			* Returns the reference of the third element of this quaternion.
119			* @return the reference of the third element of this quaternion
120			*/
121			Real& y() {
122			return data_[2];
123			}
124
125			/**
126			* Returns the value of the fourth element of this quaternion.
127			* @return the value of the fourth element of this quaternion
128			*/
129			Real z() const {
130			return data_[3];
131			}
132			/**
133			* Returns the reference of the fourth element of this quaternion.
134			* @return the reference of the fourth element of this quaternion
135			*/
136			Real& z() {
137			return data_[3];
138			}
139
140			/**
141	tim	1603	* Tests if this quaternion is equal to other quaternion
142			* @return true if equal, otherwise return false
143			* @param q quaternion to be compared
144			*/
145			inline bool operator ==(const Quaternion<Real>& q) {
146
147			for (unsigned int i = 0; i < 4; i ++) {
148			if (!equal(data_[i], q[i])) {
149			return false;
150			}
151			}
152
153			return true;
154			}
155
156			/**
157	tim	1586	* Returns the inverse of this quaternion
158			* @return inverse
159			* @note since quaternion is a complex number, the inverse of quaternion
160			* q = w + xi + yj+ zk is inv_q = (w -xi - yj - zk)/(\|q\|^2)
161			*/
162	tim	1603	Quaternion<Real> inverse() {
163	tim	1586	Quaternion<Real> q;
164	tim	1603	Real d = this->lengthSquare();
165	tim	1586
166			q.w() = w() / d;
167			q.x() = -x() / d;
168			q.y() = -y() / d;
169			q.z() = -z() / d;
170
171			return q;
172			}
173
174			/**
175			* Sets the value to the multiplication of itself and another quaternion
176			* @param q the other quaternion
177			*/
178			void mul(const Quaternion<Real>& q) {
179	tim	1603	Quaternion<Real> tmp(*this);
180	tim	1586
181	tim	1603	data_[0] = (tmp[0]q[0]) -(tmp[1]q[1]) - (tmp[2]q[2]) - (tmp[3]q[3]);
182			data_[1] = (tmp[0]q[1]) + (tmp[1]q[0]) + (tmp[2]q[3]) - (tmp[3]q[2]);
183			data_[2] = (tmp[0]q[2]) + (tmp[2]q[0]) + (tmp[3]q[1]) - (tmp[1]q[3]);
184			data_[3] = (tmp[0]q[3]) + (tmp[3]q[0]) + (tmp[1]q[2]) - (tmp[2]q[1]);
185			}
186	tim	1586
187	tim	1603	void mul(const Real& s) {
188			data_[0] *= s;
189			data_[1] *= s;
190			data_[2] *= s;
191			data_[3] *= s;
192	tim	1586	}
193
194			/** Set the value of this quaternion to the division of itself by another quaternion */
195	tim	1603	void div(Quaternion<Real>& q) {
196	tim	1586	mul(q.inverse());
197			}
198	tim	1603
199			void div(const Real& s) {
200			data_[0] /= s;
201			data_[1] /= s;
202			data_[2] /= s;
203			data_[3] /= s;
204			}
205	tim	1586
206			Quaternion<Real>& operator *=(const Quaternion<Real>& q) {
207			mul(q);
208			return *this;
209			}
210	tim	1603
211			Quaternion<Real>& operator *=(const Real& s) {
212			mul(s);
213	tim	1586	return *this;
214			}
215
216	tim	1603	Quaternion<Real>& operator /=(Quaternion<Real>& q) {
217			this = q.inverse();
218			return *this;
219			}
220
221			Quaternion<Real>& operator /=(const Real& s) {
222			div(s);
223			return *this;
224			}
225	tim	1586	/**
226			* Returns the conjugate quaternion of this quaternion
227			* @return the conjugate quaternion of this quaternion
228			*/
229			Quaternion<Real> conjugate() {
230			return Quaternion<Real>(w(), -x(), -y(), -z());
231			}
232
233			/**
234			* Returns the corresponding rotation matrix (3x3)
235			* @return a 3x3 rotation matrix
236			*/
237	tim	1592	SquareMatrix<Real, 3> toRotationMatrix3() {
238			SquareMatrix<Real, 3> rotMat3;
239	tim	1586
240			Real w2;
241			Real x2;
242			Real y2;
243			Real z2;
244
245			if (!isNormalized())
246			normalize();
247
248			w2 = w() * w();
249			x2 = x() * x();
250			y2 = y() * y();
251			z2 = z() * z();
252
253			rotMat3(0, 0) = w2 + x2 - y2 - z2;
254			rotMat3(0, 1) = 2.0 * ( x() * y() + w() * z() );
255			rotMat3(0, 2) = 2.0 * ( x() * z() - w() * y() );
256
257			rotMat3(1, 0) = 2.0 * ( x() * y() - w() * z() );
258			rotMat3(1, 1) = w2 - x2 + y2 - z2;
259			rotMat3(1, 2) = 2.0 * ( y() * z() + w() * x() );
260
261			rotMat3(2, 0) = 2.0 * ( x() * z() + w() * y() );
262			rotMat3(2, 1) = 2.0 * ( y() * z() - w() * x() );
263			rotMat3(2, 2) = w2 - x2 -y2 +z2;
264	tim	1603
265			return rotMat3;
266	tim	1586	}
267
268			};//end Quaternion
269
270	tim	1603
271	tim	1586	/**
272	tim	1603	* Returns the vaule of scalar multiplication of this quaterion q (q * s).
273			* @return the vaule of scalar multiplication of this vector
274			* @param q the source quaternion
275			* @param s the scalar value
276			*/
277			template<typename Real, unsigned int Dim>
278			Quaternion<Real> operator * ( const Quaternion<Real>& q, Real s) {
279			Quaternion<Real> result(q);
280			result.mul(s);
281			return result;
282			}
283
284			/**
285			* Returns the vaule of scalar multiplication of this quaterion q (q * s).
286			* @return the vaule of scalar multiplication of this vector
287			* @param s the scalar value
288			* @param q the source quaternion
289			*/
290			template<typename Real, unsigned int Dim>
291			Quaternion<Real> operator * ( const Real& s, const Quaternion<Real>& q ) {
292			Quaternion<Real> result(q);
293			result.mul(s);
294			return result;
295			}
296
297			/**
298	tim	1586	* Returns the multiplication of two quaternion
299			* @return the multiplication of two quaternion
300			* @param q1 the first quaternion
301			* @param q2 the second quaternion
302			*/
303			template<typename Real>
304			inline Quaternion<Real> operator *(const Quaternion<Real>& q1, const Quaternion<Real>& q2) {
305			Quaternion<Real> result(q1);
306			result *= q2;
307			return result;
308			}
309
310			/**
311			* Returns the division of two quaternion
312			* @param q1 divisor
313			* @param q2 dividen
314			*/
315
316			template<typename Real>
317	tim	1603	inline Quaternion<Real> operator /( Quaternion<Real>& q1, Quaternion<Real>& q2) {
318	tim	1586	return q1 * q2.inverse();
319			}
320
321			/**
322			* Returns the value of the division of a scalar by a quaternion
323			* @return the value of the division of a scalar by a quaternion
324			* @param s scalar
325			* @param q quaternion
326			* @note for a quaternion q, 1/q = q.inverse()
327			*/
328			template<typename Real>
329	tim	1603	Quaternion<Real> operator /(const Real& s, Quaternion<Real>& q) {
330	tim	1586
331	tim	1603	Quaternion<Real> x;
332			x = q.inverse();
333			x *= s;
334			return x;
335	tim	1586	}
336	tim	1603
337			template <class T>
338			inline bool operator==(const Quaternion<T>& lhs, const Quaternion<T>& rhs) {
339			return equal(lhs[0] ,rhs[0]) && equal(lhs[1] , rhs[1]) && equal(lhs[2], rhs[2]) && equal(lhs[3], rhs[3]);
340			}
341
342	tim	1586	typedef Quaternion<double> Quat4d;
343	tim	1585	}
344			#endif //MATH_QUATERNION_HPP