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) {
                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:	1930
Committed:	Wed Jan 12 22:41:40 2005 UTC (19 years, 5 months ago) by gezelter
File size:	12420 byte(s)
Log Message:	merging new_design branch into OOPSE-2.0
#	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 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	#include "math/Vector.hpp"
53	#include "math/SquareMatrix.hpp"
54
55	namespace oopse{
56
57	/**
58	* @class Quaternion Quaternion.hpp "math/Quaternion.hpp"
59	* 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	*/
65	template<typename Real>
66	class Quaternion : public Vector<Real, 4> {
67	public:
68	Quaternion() : Vector<Real, 4>() {}
69
70	/** Constructs and initializes a Quaternion from w, x, y, z values */
71	Quaternion(Real w, Real x, Real y, Real z) {
72	data_[0] = w;
73	data_[1] = x;
74	data_[2] = y;
75	data_[3] = z;
76	}
77
78	/** Constructs and initializes a Quaternion from a Vector<Real,4> */
79	Quaternion(const Vector<Real,4>& v)
80	: Vector<Real, 4>(v){
81	}
82
83	/** copy assignment */
84	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	return data_[0];
99	}
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	return data_[0];
107	}
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	return data_[1];
115	}
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	return data_[1];
123	}
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	return data_[2];
131	}
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	return data_[2];
139	}
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	return data_[3];
147	}
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	return data_[3];
154	}
155
156	/**
157	* 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	if (!equal(data_[i], q[i])) {
165	return false;
166	}
167	}
168
169	return true;
170	}
171
172	/**
173	* 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	Quaternion<Real> inverse() {
179	Quaternion<Real> q;
180	Real d = this->lengthSquare();
181
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	Quaternion<Real> tmp(*this);
196
197	data_[0] = (tmp[0]q[0]) -(tmp[1]q[1]) - (tmp[2]q[2]) - (tmp[3]q[3]);
198	data_[1] = (tmp[0]q[1]) + (tmp[1]q[0]) + (tmp[2]q[3]) - (tmp[3]q[2]);
199	data_[2] = (tmp[0]q[2]) + (tmp[2]q[0]) + (tmp[3]q[1]) - (tmp[1]q[3]);
200	data_[3] = (tmp[0]q[3]) + (tmp[3]q[0]) + (tmp[1]q[2]) - (tmp[2]q[1]);
201	}
202
203	void mul(const Real& s) {
204	data_[0] *= s;
205	data_[1] *= s;
206	data_[2] *= s;
207	data_[3] *= s;
208	}
209
210	/** Set the value of this quaternion to the division of itself by another quaternion */
211	void div(Quaternion<Real>& q) {
212	mul(q.inverse());
213	}
214
215	void div(const Real& s) {
216	data_[0] /= s;
217	data_[1] /= s;
218	data_[2] /= s;
219	data_[3] /= s;
220	}
221
222	Quaternion<Real>& operator *=(const Quaternion<Real>& q) {
223	mul(q);
224	return *this;
225	}
226
227	Quaternion<Real>& operator *=(const Real& s) {
228	mul(s);
229	return *this;
230	}
231
232	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	/**
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	SquareMatrix<Real, 3> toRotationMatrix3() {
254	SquareMatrix<Real, 3> rotMat3;
255
256	Real w2;
257	Real x2;
258	Real y2;
259	Real z2;
260
261	if (!isNormalized())
262	normalize();
263
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
281	return rotMat3;
282	}
283
284	};//end Quaternion
285
286
287	/**
288	* 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	* 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	inline Quaternion<Real> operator /( Quaternion<Real>& q1, Quaternion<Real>& q2) {
334	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	Quaternion<Real> operator /(const Real& s, Quaternion<Real>& q) {
346
347	Quaternion<Real> x;
348	x = q.inverse();
349	x *= s;
350	return x;
351	}
352
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	typedef Quaternion<double> Quat4d;
359	}
360	#endif //MATH_QUATERNION_HPP