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"

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();

            /** 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;                
            }
            
            /**
             *  
             */
            Quaternion(const Vector<Real,4>& v) 
                : Vector<Real, 4>(v){
            }

            /** */
            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];    
            }

            /**
             * 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->lengthSquared();
                
                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) {

                Real a0( (z() - y()) * (q.y() - q.z()) );
                Real a1( (w() + x()) * (q.w() + q.x()) );
                Real a2( (w() - x()) * (q.y() + q.z()) );
                Real a3( (y() + z()) * (q.w() - q.x()) );
                Real b0( -(x() - z()) * (q.x() - q.y()) );
                Real b1( -(x() + z()) * (q.x() + q.y()) );
                Real b2( (w() + y()) * (q.w() - q.z()) );
                Real b3( (w() - y()) * (q.w() + q.z()) );

                data_[0] = a0 + 0.5*(b0 + b1 + b2 + b3),;
                data_[1] = a1 + 0.5*(b0 + b1 - b2 - b3);
                data_[2] = a2 + 0.5*(b0 - b1 + b2 - b3),
                data_[3] = a3 + 0.5*(b0 - b1 - b2 + b3) );
            }


            /** Set the value of this quaternion to the division of itself by another quaternion */
            void div(const Quaternion<Real>& q) {
                mul(q.inverse());
            }
            
            Quaternion<Real>& operator *=(const Quaternion<Real>& q) {
                mul(q);
                return *this;
            }
                        
            Quaternion<Real>& operator /=(const Quaternion<Real>& q) {
                mul(q.inverse());
                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;
            }

    };//end Quaternion

    /**
     * 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 /(const Quaternion<Real>& q1, const 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, const Quaternion<Real>& q) {

        Quaternion<Real> x = q.inv();
        return x * s;
    }

    typedef Quaternion<double> Quat4d;
}
#endif //MATH_QUATERNION_HPP 
Revision:	99
Committed:	Mon Oct 18 17:07:27 2004 UTC (20 years, 6 months ago) by tim
File size:	9298 byte(s)
Log Message:	fix some bugs in Quaternion class
#	User	Rev	Content
1	tim	92	/*
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	93	#include "math/Vector.hpp"
37
38	tim	92	namespace oopse{
39
40			/**
41			* @class Quaternion Quaternion.hpp "math/Quaternion.hpp"
42	tim	93	* Quaternion is a sort of a higher-level complex number.
43			* It is defined as Q = w + xi + yj + z*k,
44			* where w, x, y, and z are numbers of type T (e.g. double), and
45			* ii = -1; jj = -1; k*k = -1;
46			* ij = k; jk = i; k*i = j;
47	tim	92	*/
48			template<typename Real>
49			class Quaternion : public Vector<Real, 4> {
50	tim	93	public:
51			Quaternion();
52	tim	92
53	tim	93	/** Constructs and initializes a Quaternion from w, x, y, z values */
54			Quaternion(Real w, Real x, Real y, Real z) {
55			data_[0] = w;
56			data_[1] = x;
57			data_[2] = y;
58			data_[3] = z;
59			}
60
61			/**
62			*
63			*/
64			Quaternion(const Vector<Real,4>& v)
65			: Vector<Real, 4>(v){
66			}
67	tim	92
68	tim	93	/** */
69			Quaternion& operator =(const Vector<Real, 4>& v){
70			if (this == & v)
71			return *this;
72
73			Vector<Real, 4>::operator=(v);
74
75			return *this;
76			}
77
78			/**
79			* Returns the value of the first element of this quaternion.
80			* @return the value of the first element of this quaternion
81			*/
82			Real w() const {
83			return data_[0];
84			}
85
86			/**
87			* Returns the reference of the first element of this quaternion.
88			* @return the reference of the first element of this quaternion
89			*/
90			Real& w() {
91			return data_[0];
92			}
93
94			/**
95			* Returns the value of the first element of this quaternion.
96			* @return the value of the first element of this quaternion
97			*/
98			Real x() const {
99			return data_[1];
100			}
101
102			/**
103			* Returns the reference of the second element of this quaternion.
104			* @return the reference of the second element of this quaternion
105			*/
106			Real& x() {
107			return data_[1];
108			}
109
110			/**
111			* Returns the value of the thirf element of this quaternion.
112			* @return the value of the third element of this quaternion
113			*/
114			Real y() const {
115			return data_[2];
116			}
117
118			/**
119			* Returns the reference of the third element of this quaternion.
120			* @return the reference of the third element of this quaternion
121			*/
122			Real& y() {
123			return data_[2];
124			}
125
126			/**
127			* Returns the value of the fourth element of this quaternion.
128			* @return the value of the fourth element of this quaternion
129			*/
130			Real z() const {
131			return data_[3];
132			}
133			/**
134			* Returns the reference of the fourth element of this quaternion.
135			* @return the reference of the fourth element of this quaternion
136			*/
137			Real& z() {
138			return data_[3];
139			}
140
141			/**
142			* Returns the inverse of this quaternion
143			* @return inverse
144			* @note since quaternion is a complex number, the inverse of quaternion
145			* q = w + xi + yj+ zk is inv_q = (w -xi - yj - zk)/(\|q\|^2)
146			*/
147			Quaternion<Real> inverse(){
148			Quaternion<Real> q;
149			Real d = this->lengthSquared();
150
151			q.w() = w() / d;
152			q.x() = -x() / d;
153			q.y() = -y() / d;
154			q.z() = -z() / d;
155
156			return q;
157			}
158
159			/**
160			* Sets the value to the multiplication of itself and another quaternion
161			* @param q the other quaternion
162			*/
163			void mul(const Quaternion<Real>& q) {
164
165			Real a0( (z() - y()) * (q.y() - q.z()) );
166			Real a1( (w() + x()) * (q.w() + q.x()) );
167			Real a2( (w() - x()) * (q.y() + q.z()) );
168			Real a3( (y() + z()) * (q.w() - q.x()) );
169			Real b0( -(x() - z()) * (q.x() - q.y()) );
170			Real b1( -(x() + z()) * (q.x() + q.y()) );
171			Real b2( (w() + y()) * (q.w() - q.z()) );
172			Real b3( (w() - y()) * (q.w() + q.z()) );
173
174			data_[0] = a0 + 0.5*(b0 + b1 + b2 + b3),;
175			data_[1] = a1 + 0.5*(b0 + b1 - b2 - b3);
176			data_[2] = a2 + 0.5*(b0 - b1 + b2 - b3),
177			data_[3] = a3 + 0.5*(b0 - b1 - b2 + b3) );
178			}
179
180
181			/** Set the value of this quaternion to the division of itself by another quaternion */
182			void div(const Quaternion<Real>& q) {
183			mul(q.inverse());
184			}
185
186			Quaternion<Real>& operator *=(const Quaternion<Real>& q) {
187			mul(q);
188			return *this;
189			}
190
191			Quaternion<Real>& operator /=(const Quaternion<Real>& q) {
192			mul(q.inverse());
193			return *this;
194			}
195
196			/**
197			* Returns the conjugate quaternion of this quaternion
198			* @return the conjugate quaternion of this quaternion
199			*/
200			Quaternion<Real> conjugate() {
201			return Quaternion<Real>(w(), -x(), -y(), -z());
202			}
203
204			/**
205			* Returns the corresponding rotation matrix (3x3)
206			* @return a 3x3 rotation matrix
207			*/
208	tim	99	SquareMatrix<Real, 3> toRotationMatrix3() {
209			SquareMatrix<Real, 3> rotMat3;
210	tim	93
211			Real w2;
212			Real x2;
213			Real y2;
214			Real z2;
215
216			if (!isNormalized())
217			normalize();
218
219			w2 = w() * w();
220			x2 = x() * x();
221			y2 = y() * y();
222			z2 = z() * z();
223
224			rotMat3(0, 0) = w2 + x2 - y2 - z2;
225			rotMat3(0, 1) = 2.0 * ( x() * y() + w() * z() );
226			rotMat3(0, 2) = 2.0 * ( x() * z() - w() * y() );
227
228			rotMat3(1, 0) = 2.0 * ( x() * y() - w() * z() );
229			rotMat3(1, 1) = w2 - x2 + y2 - z2;
230			rotMat3(1, 2) = 2.0 * ( y() * z() + w() * x() );
231
232			rotMat3(2, 0) = 2.0 * ( x() * z() + w() * y() );
233			rotMat3(2, 1) = 2.0 * ( y() * z() - w() * x() );
234			rotMat3(2, 2) = w2 - x2 -y2 +z2;
235			}
236
237			};//end Quaternion
238
239			/**
240			* Returns the multiplication of two quaternion
241			* @return the multiplication of two quaternion
242			* @param q1 the first quaternion
243			* @param q2 the second quaternion
244			*/
245			template<typename Real>
246			inline Quaternion<Real> operator *(const Quaternion<Real>& q1, const Quaternion<Real>& q2) {
247			Quaternion<Real> result(q1);
248			result *= q2;
249			return result;
250			}
251
252			/**
253			* Returns the division of two quaternion
254			* @param q1 divisor
255			* @param q2 dividen
256			*/
257
258			template<typename Real>
259			inline Quaternion<Real> operator /(const Quaternion<Real>& q1, const Quaternion<Real>& q2) {
260			return q1 * q2.inverse();
261			}
262
263			/**
264			* Returns the value of the division of a scalar by a quaternion
265			* @return the value of the division of a scalar by a quaternion
266			* @param s scalar
267			* @param q quaternion
268			* @note for a quaternion q, 1/q = q.inverse()
269			*/
270			template<typename Real>
271	tim	99	Quaternion<Real> operator /(const Real& s, const Quaternion<Real>& q) {
272	tim	93
273			Quaternion<Real> x = q.inv();
274			return x * s;
275			}
276
277			typedef Quaternion<double> Quat4d;
278	tim	92	}
279			#endif //MATH_QUATERNION_HPP