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, 3> toRotationMatrix3() {
                SquareMatrix<Real, 3, 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 Quaternion<Real>& s, const Quaternion<Real>& q) {

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

    typedef Quaternion<double> Quat4d;
}
#endif //MATH_QUATERNION_HPP 
Revision:	1586
Committed:	Sun Oct 17 01:19:11 2004 UTC (19 years, 8 months ago) by tim
File size:	9316 byte(s)
Log Message:	math library in progress
#	Content
1	/*
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	#include "math/Vector.hpp"
37
38	namespace oopse{
39
40	/**
41	* @class Quaternion Quaternion.hpp "math/Quaternion.hpp"
42	* 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	*/
48	template<typename Real>
49	class Quaternion : public Vector<Real, 4> {
50	public:
51	Quaternion();
52
53	/** 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
68	/** */
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	SquareMatrix<Real, 3, 3> toRotationMatrix3() {
209	SquareMatrix<Real, 3, 3> rotMat3;
210
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	Quaternion<Real> operator /(const Quaternion<Real>& s, const Quaternion<Real>& q) {
272
273	Quaternion<Real> x = q.inv();
274	return x * s;
275	}
276
277	typedef Quaternion<double> Quat4d;
278	}
279	#endif //MATH_QUATERNION_HPP