src/math/Quaternion.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 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) {
                this->data_[0] = w;
                this->data_[1] = x;
                this->data_[2] = y;
                this->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 this->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 this->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 this->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 this->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 this->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 this->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 this->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 this->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(this->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);

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

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