ViewVC Help

View File | Revision Log | Show Annotations | View Changeset | Root Listing

root/OpenMD/branches/development/src/math/Quaternion.hpp

Comparing:
trunk/src/math/Quaternion.hpp (file contents), Revision 110 by tim, Tue Oct 19 21:28:55 2004 UTC vs.
branches/development/src/math/Quaternion.hpp (file contents), Revision 1850 by gezelter, Wed Feb 20 15:39:39 2013 UTC

#	Line 1 | Line 1
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.
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. Redistributions of source code must retain the above copyright
10	+	*    notice, this list of conditions and the following disclaimer.
11	+	*
12	+	* 2. Redistributions in binary form must reproduce the above copyright
13	+	*    notice, this list of conditions and the following disclaimer in the
14	+	*    documentation and/or other materials provided with the
15	+	*    distribution.
16	+	*
17	+	* This software is provided "AS IS," without a warranty of any
18	+	* kind. All express or implied conditions, representations and
19	+	* warranties, including any implied warranty of merchantability,
20	+	* fitness for a particular purpose or non-infringement, are hereby
21	+	* excluded.  The University of Notre Dame and its licensors shall not
22	+	* be liable for any damages suffered by licensee as a result of
23	+	* using, modifying or distributing the software or its
24	+	* derivatives. In no event will the University of Notre Dame or its
25	+	* licensors be liable for any lost revenue, profit or data, or for
26	+	* direct, indirect, special, consequential, incidental or punitive
27	+	* damages, however caused and regardless of the theory of liability,
28	+	* arising out of the use of or inability to use software, even if the
29	+	* University of Notre Dame has been advised of the possibility of
30	+	* such damages.
31	+	*
32	+	* SUPPORT OPEN SCIENCE!  If you use OpenMD or its source code in your
33	+	* research, please cite the appropriate papers when you publish your
34	+	* work.  Good starting points are:
35	+	*
36	+	* [1]  Meineke, et al., J. Comp. Chem. 26, 252-271 (2005).
37	+	* [2]  Fennell & Gezelter, J. Chem. Phys. 124, 234104 (2006).
38	+	* [3]  Sun, Lin & Gezelter, J. Chem. Phys. 128, 234107 (2008).
39	+	* [4]  Kuang & Gezelter,  J. Chem. Phys. 133, 164101 (2010).
40	+	* [5]  Vardeman, Stocker & Gezelter, J. Chem. Theory Comput. 7, 834 (2011).
41		*/
42	<
42	>
43		/**
44		* @file Quaternion.hpp
45		* @author Teng Lin
#	Line 32 | Line 49
49
50		#ifndef MATH_QUATERNION_HPP
51		#define MATH_QUATERNION_HPP
52	+	#include "config.h"
53	+	#include <cmath>
54
55	<	#include "math/Vector.hpp"
55	>	#include "math/Vector3.hpp"
56		#include "math/SquareMatrix.hpp"
57	+	#define ISZERO(a,eps) ( (a)>-(eps) && (a)<(eps) )
58	+	const RealType tiny=1.0e-6;
59
60	<	namespace oopse{
60	>	namespace OpenMD{
61
62	<	/**
63	<	* @class Quaternion Quaternion.hpp "math/Quaternion.hpp"
64	<	* Quaternion is a sort of a higher-level complex number.
65	<	* It is defined as Q = w + x*i + y*j + z*k,
66	<	* where w, x, y, and z are numbers of type T (e.g. double), and
67	<	* i*i = -1; j*j = -1; k*k = -1;
68	<	* i*j = k; j*k = i; k*i = j;
69	<	*/
70	<	template<typename Real>
71	<	class Quaternion : public Vector<Real, 4> {
51	<	public:
52	<	Quaternion() : Vector<Real, 4>() {}
62	>	/**
63	>	* @class Quaternion Quaternion.hpp "math/Quaternion.hpp"
64	>	* Quaternion is a sort of a higher-level complex number.
65	>	* It is defined as Q = w + x*i + y*j + z*k,
66	>	* where w, x, y, and z are numbers of type T (e.g. RealType), and
67	>	* i*i = -1; j*j = -1; k*k = -1;
68	>	* i*j = k; j*k = i; k*i = j;
69	>	*/
70	>	template<typename Real>
71	>	class Quaternion : public Vector<Real, 4> {
72
73	<	/** Constructs and initializes a Quaternion from w, x, y, z values */
74	<	Quaternion(Real w, Real x, Real y, Real z) {
75	<	data_[0] = w;
76	<	data_[1] = x;
77	<	data_[2] = y;
78	<	data_[3] = z;
79	<	}
73	>	public:
74	>	Quaternion() : Vector<Real, 4>() {}
75	>
76	>	/** Constructs and initializes a Quaternion from w, x, y, z values */
77	>	Quaternion(Real w, Real x, Real y, Real z) {
78	>	this->data_[0] = w;
79	>	this->data_[1] = x;
80	>	this->data_[2] = y;
81	>	this->data_[3] = z;
82	>	}
83
84	<	/** Constructs and initializes a Quaternion from a  Vector<Real,4> */
85	<	Quaternion(const Vector<Real,4>& v)
86	<	: Vector<Real, 4>(v){
87	<	}
84	>	/** Constructs and initializes a Quaternion from a  Vector<Real,4> */
85	>	Quaternion(const Vector<Real,4>& v)
86	>	: Vector<Real, 4>(v){
87	>	}
88
89	<	/** copy assignment */
90	<	Quaternion& operator =(const Vector<Real, 4>& v){
91	<	if (this == & v)
92	<	return *this;
89	>	/** copy assignment */
90	>	Quaternion& operator =(const Vector<Real, 4>& v){
91	>	if (this == & v)
92	>	return *this;
93	>
94	>	Vector<Real, 4>::operator=(v);
95	>
96	>	return *this;
97	>	}
98	>
99	>	/**
100	>	* Returns the value of the first element of this quaternion.
101	>	* @return the value of the first element of this quaternion
102	>	*/
103	>	Real w() const {
104	>	return this->data_[0];
105	>	}
106
107	<	Vector<Real, 4>::operator=(v);
108	<
109	<	return *this;
110	<	}
107	>	/**
108	>	* Returns the reference of the first element of this quaternion.
109	>	* @return the reference of the first element of this quaternion
110	>	*/
111	>	Real& w() {
112	>	return this->data_[0];
113	>	}
114
115	<	/**
116	<	* Returns the value of the first element of this quaternion.
117	<	* @return the value of the first element of this quaternion
118	<	*/
119	<	Real w() const {
120	<	return data_[0];
121	<	}
115	>	/**
116	>	* Returns the value of the first element of this quaternion.
117	>	* @return the value of the first element of this quaternion
118	>	*/
119	>	Real x() const {
120	>	return this->data_[1];
121	>	}
122
123	<	/**
124	<	* Returns the reference of the first element of this quaternion.
125	<	* @return the reference of the first element of this quaternion
126	<	*/
127	<	Real& w() {
128	<	return data_[0];
129	<	}
123	>	/**
124	>	* Returns the reference of the second element of this quaternion.
125	>	* @return the reference of the second element of this quaternion
126	>	*/
127	>	Real& x() {
128	>	return this->data_[1];
129	>	}
130
131	<	/**
132	<	* Returns the value of the first element of this quaternion.
133	<	* @return the value of the first element of this quaternion
134	<	*/
135	<	Real x() const {
136	<	return data_[1];
137	<	}
131	>	/**
132	>	* Returns the value of the thirf element of this quaternion.
133	>	* @return the value of the third element of this quaternion
134	>	*/
135	>	Real y() const {
136	>	return this->data_[2];
137	>	}
138
139	<	/**
140	<	* Returns the reference of the second element of this quaternion.
141	<	* @return the reference of the second element of this quaternion
142	<	*/
143	<	Real& x() {
144	<	return data_[1];
145	<	}
139	>	/**
140	>	* Returns the reference of the third element of this quaternion.
141	>	* @return the reference of the third element of this quaternion
142	>	*/
143	>	Real& y() {
144	>	return this->data_[2];
145	>	}
146
147	<	/**
148	<	* Returns the value of the thirf element of this quaternion.
149	<	* @return the value of the third element of this quaternion
150	<	*/
151	<	Real y() const {
152	<	return data_[2];
153	<	}
147	>	/**
148	>	* Returns the value of the fourth element of this quaternion.
149	>	* @return the value of the fourth element of this quaternion
150	>	*/
151	>	Real z() const {
152	>	return this->data_[3];
153	>	}
154	>	/**
155	>	* Returns the reference of the fourth element of this quaternion.
156	>	* @return the reference of the fourth element of this quaternion
157	>	*/
158	>	Real& z() {
159	>	return this->data_[3];
160	>	}
161
162	<	/**
163	<	* Returns the reference of the third element of this quaternion.
164	<	* @return the reference of the third element of this quaternion
165	<	*/
166	<	Real& y() {
167	<	return data_[2];
123	<	}
162	>	/**
163	>	* Tests if this quaternion is equal to other quaternion
164	>	* @return true if equal, otherwise return false
165	>	* @param q quaternion to be compared
166	>	*/
167	>	inline bool operator ==(const Quaternion<Real>& q) {
168
169	<	/**
170	<	* Returns the value of the fourth element of this quaternion.
171	<	* @return the value of the fourth element of this quaternion
172	<	*/
173	<	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	<	* 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	<	}
169	>	for (unsigned int i = 0; i < 4; i ++) {
170	>	if (!equal(this->data_[i], q[i])) {
171	>	return false;
172	>	}
173	>	}
174
175	<	return true;
176	<	}
175	>	return true;
176	>	}
177
178	<	/**
179	<	* Returns the inverse of this quaternion
180	<	* @return inverse
181	<	* @note since quaternion is a complex number, the inverse of quaternion
182	<	* q = w + xi + yj+ zk is inv_q = (w -xi - yj - zk)/(|q|^2)
183	<	*/
184	<	Quaternion<Real> inverse() {
185	<	Quaternion<Real> q;
186	<	Real d = this->lengthSquare();
178	>	/**
179	>	* Returns the inverse of this quaternion
180	>	* @return inverse
181	>	* @note since quaternion is a complex number, the inverse of quaternion
182	>	* q = w + xi + yj+ zk is inv_q = (w -xi - yj - zk)/(|q|^2)
183	>	*/
184	>	Quaternion<Real> inverse() {
185	>	Quaternion<Real> q;
186	>	Real d = this->lengthSquare();
187
188	<	q.w() = w() / d;
189	<	q.x() = -x() / d;
190	<	q.y() = -y() / d;
191	<	q.z() = -z() / d;
188	>	q.w() = w() / d;
189	>	q.x() = -x() / d;
190	>	q.y() = -y() / d;
191	>	q.z() = -z() / d;
192
193	<	return q;
194	<	}
193	>	return q;
194	>	}
195
196	<	/**
197	<	* Sets the value to the multiplication of itself and another quaternion
198	<	* @param q the other quaternion
199	<	*/
200	<	void mul(const Quaternion<Real>& q) {
201	<	Quaternion<Real> tmp(*this);
196	>	/**
197	>	* Sets the value to the multiplication of itself and another quaternion
198	>	* @param q the other quaternion
199	>	*/
200	>	void mul(const Quaternion<Real>& q) {
201	>	Quaternion<Real> tmp(*this);
202
203	<	data_[0] = (tmp[0]*q[0]) -(tmp[1]*q[1]) - (tmp[2]*q[2]) - (tmp[3]*q[3]);
204	<	data_[1] = (tmp[0]*q[1]) + (tmp[1]*q[0]) + (tmp[2]*q[3]) - (tmp[3]*q[2]);
205	<	data_[2] = (tmp[0]*q[2]) + (tmp[2]*q[0]) + (tmp[3]*q[1]) - (tmp[1]*q[3]);
206	<	data_[3] = (tmp[0]*q[3]) + (tmp[3]*q[0]) + (tmp[1]*q[2]) - (tmp[2]*q[1]);
207	<	}
203	>	this->data_[0] = (tmp[0]*q[0]) -(tmp[1]*q[1]) - (tmp[2]*q[2]) - (tmp[3]*q[3]);
204	>	this->data_[1] = (tmp[0]*q[1]) + (tmp[1]*q[0]) + (tmp[2]*q[3]) - (tmp[3]*q[2]);
205	>	this->data_[2] = (tmp[0]*q[2]) + (tmp[2]*q[0]) + (tmp[3]*q[1]) - (tmp[1]*q[3]);
206	>	this->data_[3] = (tmp[0]*q[3]) + (tmp[3]*q[0]) + (tmp[1]*q[2]) - (tmp[2]*q[1]);
207	>	}
208
209	<	void mul(const Real& s) {
210	<	data_[0] *= s;
211	<	data_[1] *= s;
212	<	data_[2] *= s;
213	<	data_[3] *= s;
214	<	}
209	>	void mul(const Real& s) {
210	>	this->data_[0] *= s;
211	>	this->data_[1] *= s;
212	>	this->data_[2] *= s;
213	>	this->data_[3] *= s;
214	>	}
215
216	<	/** Set the value of this quaternion to the division of itself by another quaternion */
217	<	void div(Quaternion<Real>& q) {
218	<	mul(q.inverse());
219	<	}
216	>	/** Set the value of this quaternion to the division of itself by another quaternion */
217	>	void div(Quaternion<Real>& q) {
218	>	mul(q.inverse());
219	>	}
220
221	<	void div(const Real& s) {
222	<	data_[0] /= s;
223	<	data_[1] /= s;
224	<	data_[2] /= s;
225	<	data_[3] /= s;
226	<	}
221	>	void div(const Real& s) {
222	>	this->data_[0] /= s;
223	>	this->data_[1] /= s;
224	>	this->data_[2] /= s;
225	>	this->data_[3] /= s;
226	>	}
227
228	<	Quaternion<Real>& operator *=(const Quaternion<Real>& q) {
229	<	mul(q);
230	<	return *this;
231	<	}
228	>	Quaternion<Real>& operator *=(const Quaternion<Real>& q) {
229	>	mul(q);
230	>	return *this;
231	>	}
232
233	<	Quaternion<Real>& operator *=(const Real& s) {
234	<	mul(s);
235	<	return *this;
236	<	}
233	>	Quaternion<Real>& operator *=(const Real& s) {
234	>	mul(s);
235	>	return *this;
236	>	}
237
238	<	Quaternion<Real>& operator /=(Quaternion<Real>& q) {
239	<	*this *= q.inverse();
240	<	return *this;
241	<	}
238	>	Quaternion<Real>& operator /=(Quaternion<Real>& q) {
239	>	*this *= q.inverse();
240	>	return *this;
241	>	}
242
243	<	Quaternion<Real>& operator /=(const Real& s) {
244	<	div(s);
245	<	return *this;
246	<	}
247	<	/**
248	<	* Returns the conjugate quaternion of this quaternion
249	<	* @return the conjugate quaternion of this quaternion
250	<	*/
251	<	Quaternion<Real> conjugate() {
252	<	return Quaternion<Real>(w(), -x(), -y(), -z());
253	<	}
243	>	Quaternion<Real>& operator /=(const Real& s) {
244	>	div(s);
245	>	return *this;
246	>	}
247	>	/**
248	>	* Returns the conjugate quaternion of this quaternion
249	>	* @return the conjugate quaternion of this quaternion
250	>	*/
251	>	Quaternion<Real> conjugate() const {
252	>	return Quaternion<Real>(w(), -x(), -y(), -z());
253	>	}
254
233	–	/**
234	–	* Returns the corresponding rotation matrix (3x3)
235	–	* @return a 3x3 rotation matrix
236	–	*/
237	–	SquareMatrix<Real, 3> toRotationMatrix3() {
238	–	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();
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	<
265	<	return rotMat3;
266	<	}
267	<
268	<	};//end Quaternion
256	>	/**
257	>	return rotation angle from -PI to PI
258	>	*/
259	>	inline Real get_rotation_angle() const{
260	>	if( w() < (Real)0.0 )
261	>	return 2.0*atan2(-sqrt( x()*x() + y()*y() + z()*z() ), -w() );
262	>	else
263	>	return 2.0*atan2( sqrt( x()*x() + y()*y() + z()*z() ),  w() );
264	>	}
265
270	–
266		/**
267	<	* Returns the vaule of scalar multiplication of this quaterion q (q * s).
268	<	* @return  the vaule of scalar multiplication of this vector
269	<	* @param q the source quaternion
270	<	* @param s the scalar value
271	<	*/
272	<	template<typename Real, unsigned int Dim>
273	<	Quaternion<Real> operator * ( const Quaternion<Real>& q, Real s) {
274	<	Quaternion<Real> result(q);
275	<	result.mul(s);
276	<	return result;
267	>	create a unit quaternion from axis angle representation
268	>	*/
269	>	Quaternion<Real> fromAxisAngle(const Vector3<Real>& axis,
270	>	const Real& angle){
271	>	Vector3<Real> v(axis);
272	>	v.normalize();
273	>	Real half_angle = angle*0.5;
274	>	Real sin_a = sin(half_angle);
275	>	*this = Quaternion<Real>(cos(half_angle),
276	>	v.x()*sin_a,
277	>	v.y()*sin_a,
278	>	v.z()*sin_a);
279	>	return *this;
280		}
281
282		/**
283	<	* Returns the vaule of scalar multiplication of this quaterion q (q * s).
284	<	* @return  the vaule of scalar multiplication of this vector
285	<	* @param s the scalar value
286	<	* @param q the source quaternion
287	<	*/
288	<	template<typename Real, unsigned int Dim>
289	<	Quaternion<Real> operator * ( const Real& s, const Quaternion<Real>& q ) {
290	<	Quaternion<Real> result(q);
291	<	result.mul(s);
292	<	return result;
295	<	}
283	>	convert a quaternion to axis angle representation,
284	>	preserve the axis direction and angle from -PI to +PI
285	>	*/
286	>	void toAxisAngle(Vector3<Real>& axis, Real& angle)const {
287	>	Real vl = sqrt( x()*x() + y()*y() + z()*z() );
288	>	if( vl > tiny ) {
289	>	Real ivl = 1.0/vl;
290	>	axis.x() = x() * ivl;
291	>	axis.y() = y() * ivl;
292	>	axis.z() = z() * ivl;
293
294	+	if( w() < 0 )
295	+	angle = 2.0*atan2(-vl, -w()); //-PI,0
296	+	else
297	+	angle = 2.0*atan2( vl,  w()); //0,PI
298	+	} else {
299	+	axis = Vector3<Real>(0.0,0.0,0.0);
300	+	angle = 0.0;
301	+	}
302	+	}
303	+
304		/**
305	<	* Returns the multiplication of two quaternion
306	<	* @return the multiplication of two quaternion
307	<	* @param q1 the first quaternion
308	<	* @param q2 the second quaternion
309	<	*/
310	<	template<typename Real>
311	<	inline Quaternion<Real> operator *(const Quaternion<Real>& q1, const Quaternion<Real>& q2) {
312	<	Quaternion<Real> result(q1);
313	<	result *= q2;
314	<	return result;
305	>	shortest arc quaternion rotate one vector to another by shortest path.
306	>	create rotation from -> to, for any length vectors.
307	>	*/
308	>	Quaternion<Real> fromShortestArc(const Vector3d& from,
309	>	const Vector3d& to ) {
310	>
311	>	Vector3d c( cross(from,to) );
312	>	*this = Quaternion<Real>(dot(from,to),
313	>	c.x(),
314	>	c.y(),
315	>	c.z());
316	>
317	>	this->normalize();    // if "from" or "to" not unit, normalize quat
318	>	w() += 1.0f;            // reducing angle to halfangle
319	>	if( w() <= 1e-6 ) {     // angle close to PI
320	>	if( ( from.z()*from.z() ) > ( from.x()*from.x() ) ) {
321	>	this->data_[0] =  w();
322	>	this->data_[1] =  0.0;       //cross(from , Vector3d(1,0,0))
323	>	this->data_[2] =  from.z();
324	>	this->data_[3] = -from.y();
325	>	} else {
326	>	this->data_[0] =  w();
327	>	this->data_[1] =  from.y();  //cross(from, Vector3d(0,0,1))
328	>	this->data_[2] = -from.x();
329	>	this->data_[3] =  0.0;
330	>	}
331	>	}
332	>	this->normalize();
333		}
334
335	+	Real ComputeTwist(const Quaternion& q) {
336	+	return  (Real)2.0 * atan2(q.z(), q.w());
337	+	}
338	+
339	+	void RemoveTwist(Quaternion& q) {
340	+	Real t = ComputeTwist(q);
341	+	Quaternion rt = fromAxisAngle(V3Z, t);
342	+
343	+	q *= rt.inverse();
344	+	}
345	+
346	+	void getTwistSwingAxisAngle(Real& twistAngle, Real& swingAngle,
347	+	Vector3<Real>& swingAxis) {
348	+
349	+	twistAngle = (Real)2.0 * atan2(z(), w());
350	+	Quaternion rt, rs;
351	+	rt.fromAxisAngle(V3Z, twistAngle);
352	+	rs = *this * rt.inverse();
353	+
354	+	Real vl = sqrt( rs.x()*rs.x() + rs.y()*rs.y() + rs.z()*rs.z() );
355	+	if( vl > tiny ) {
356	+	Real ivl = 1.0 / vl;
357	+	swingAxis.x() = rs.x() * ivl;
358	+	swingAxis.y() = rs.y() * ivl;
359	+	swingAxis.z() = rs.z() * ivl;
360	+
361	+	if( rs.w() < 0.0 )
362	+	swingAngle = 2.0*atan2(-vl, -rs.w()); //-PI,0
363	+	else
364	+	swingAngle = 2.0*atan2( vl,  rs.w()); //0,PI
365	+	} else {
366	+	swingAxis = Vector3<Real>(1.0,0.0,0.0);
367	+	swingAngle = 0.0;
368	+	}
369	+	}
370	+
371	+
372	+	Vector3<Real> rotate(const Vector3<Real>& v) {
373	+
374	+	Quaternion<Real> q(v.x() * w() + v.z() * y() - v.y() * z(),
375	+	v.y() * w() + v.x() * z() - v.z() * x(),
376	+	v.z() * w() + v.y() * x() - v.x() * y(),
377	+	v.x() * x() + v.y() * y() + v.z() * z());
378	+
379	+	return Vector3<Real>(w()*q.x() + x()*q.w() + y()*q.z() - z()*q.y(),
380	+	w()*q.y() + y()*q.w() + z()*q.x() - x()*q.z(),
381	+	w()*q.z() + z()*q.w() + x()*q.y() - y()*q.x())*
382	+	( 1.0/this->lengthSquare() );
383	+	}
384	+
385	+	Quaternion<Real>& align (const Vector3<Real>& V1,
386	+	const Vector3<Real>& V2) {
387	+
388	+	// If V1 and V2 are not parallel, the axis of rotation is the unit-length
389	+	// vector U = Cross(V1,V2)/Length(Cross(V1,V2)).  The angle of rotation,
390	+	// A, is the angle between V1 and V2.  The quaternion for the rotation is
391	+	// q = cos(A/2) + sin(A/2)*(ux*i+uy*j+uz*k) where U = (ux,uy,uz).
392	+	//
393	+	// (1) Rather than extract A = acos(Dot(V1,V2)), multiply by 1/2, then
394	+	//     compute sin(A/2) and cos(A/2), we reduce the computational costs
395	+	//     by computing the bisector B = (V1+V2)/Length(V1+V2), so cos(A/2) =
396	+	//     Dot(V1,B).
397	+	//
398	+	// (2) The rotation axis is U = Cross(V1,B)/Length(Cross(V1,B)), but
399	+	//     Length(Cross(V1,B)) = Length(V1)*Length(B)*sin(A/2) = sin(A/2), in
400	+	//     which case sin(A/2)*(ux*i+uy*j+uz*k) = (cx*i+cy*j+cz*k) where
401	+	//     C = Cross(V1,B).
402	+	//
403	+	// If V1 = V2, then B = V1, cos(A/2) = 1, and U = (0,0,0).  If V1 = -V2,
404	+	// then B = 0.  This can happen even if V1 is approximately -V2 using
405	+	// floating point arithmetic, since Vector3::Normalize checks for
406	+	// closeness to zero and returns the zero vector accordingly.  The test
407	+	// for exactly zero is usually not recommend for floating point
408	+	// arithmetic, but the implementation of Vector3::Normalize guarantees
409	+	// the comparison is robust.  In this case, the A = pi and any axis
410	+	// perpendicular to V1 may be used as the rotation axis.
411	+
412	+	Vector3<Real> Bisector = V1 + V2;
413	+	Bisector.normalize();
414	+
415	+	Real CosHalfAngle = dot(V1,Bisector);
416	+
417	+	this->data_[0] = CosHalfAngle;
418	+
419	+	if (CosHalfAngle != (Real)0.0) {
420	+	Vector3<Real> Cross = cross(V1, Bisector);
421	+	this->data_[1] = Cross.x();
422	+	this->data_[2] = Cross.y();
423	+	this->data_[3] = Cross.z();
424	+	} else {
425	+	Real InvLength;
426	+	if (fabs(V1[0]) >= fabs(V1[1])) {
427	+	// V1.x or V1.z is the largest magnitude component
428	+	InvLength = (Real)1.0/sqrt(V1[0]*V1[0] + V1[2]*V1[2]);
429	+
430	+	this->data_[1] = -V1[2]*InvLength;
431	+	this->data_[2] = (Real)0.0;
432	+	this->data_[3] = +V1[0]*InvLength;
433	+	} else {
434	+	// V1.y or V1.z is the largest magnitude component
435	+	InvLength = (Real)1.0 / sqrt(V1[1]*V1[1] + V1[2]*V1[2]);
436	+
437	+	this->data_[1] = (Real)0.0;
438	+	this->data_[2] = +V1[2]*InvLength;
439	+	this->data_[3] = -V1[1]*InvLength;
440	+	}
441	+	}
442	+	return *this;
443	+	}
444	+
445	+	void toTwistSwing ( Real& tw, Real& sx, Real& sy ) {
446	+
447	+	// First test if the swing is in the singularity:
448	+
449	+	if ( ISZERO(z(),tiny) && ISZERO(w(),tiny) ) { sx=sy=M_PI; tw=0; return; }
450	+
451	+	// Decompose into twist-swing by solving the equation:
452	+	//
453	+	//                       Qtwist(t*2) * Qswing(s*2) = q
454	+	//
455	+	// note: (x,y) is the normalized swing axis (x*x+y*y=1)
456	+	//
457	+	//          ( Ct 0 0 St ) * ( Cs xSs ySs 0 ) = ( qw qx qy qz )
458	+	//  ( CtCs  xSsCt-yStSs  xStSs+ySsCt  StCs ) = ( qw qx qy qz )      (1)
459	+	// From (1): CtCs / StCs = qw/qz => Ct/St = qw/qz => tan(t) = qz/qw (2)
460	+	//
461	+	// The swing rotation/2 s comes from:
462	+	//
463	+	// From (1): (CtCs)^2 + (StCs)^2 = qw^2 + qz^2 =>
464	+	//                                       Cs = sqrt ( qw^2 + qz^2 ) (3)
465	+	//
466	+	// From (1): (xSsCt-yStSs)^2 + (xStSs+ySsCt)^2 = qx^2 + qy^2 =>
467	+	//                                       Ss = sqrt ( qx^2 + qy^2 ) (4)
468	+	// From (1):  |SsCt -StSs| |x| = |qx|
469	+	//            |StSs +SsCt| |y|   |qy|                              (5)
470	+
471	+	Real qw, qx, qy, qz;
472	+
473	+	if ( w()<0 ) {
474	+	qw=-w();
475	+	qx=-x();
476	+	qy=-y();
477	+	qz=-z();
478	+	} else {
479	+	qw=w();
480	+	qx=x();
481	+	qy=y();
482	+	qz=z();
483	+	}
484	+
485	+	Real t = atan2 ( qz, qw ); // from (2)
486	+	Real s = atan2( sqrt(qx*qx+qy*qy), sqrt(qz*qz+qw*qw) ); // from (3)
487	+	// and (4)
488	+
489	+	Real x=0.0, y=0.0, sins=sin(s);
490	+
491	+	if ( !ISZERO(sins,tiny) ) {
492	+	Real sint = sin(t);
493	+	Real cost = cos(t);
494	+
495	+	// by solving the linear system in (5):
496	+	y = (-qx*sint + qy*cost)/sins;
497	+	x = ( qx*cost + qy*sint)/sins;
498	+	}
499	+
500	+	tw = (Real)2.0*t;
501	+	sx = (Real)2.0*x*s;
502	+	sy = (Real)2.0*y*s;
503	+	}
504	+
505	+	void toSwingTwist(Real& sx, Real& sy, Real& tw ) {
506	+
507	+	// Decompose q into swing-twist using a similar development as
508	+	// in function toTwistSwing
509	+
510	+	if ( ISZERO(z(),tiny) && ISZERO(w(),tiny) ) { sx=sy=M_PI; tw=0; }
511	+
512	+	Real qw, qx, qy, qz;
513	+	if ( w() < 0 ){
514	+	qw=-w();
515	+	qx=-x();
516	+	qy=-y();
517	+	qz=-z();
518	+	} else {
519	+	qw=w();
520	+	qx=x();
521	+	qy=y();
522	+	qz=z();
523	+	}
524	+
525	+	// Get the twist t:
526	+	Real t = 2.0 * atan2(qz,qw);
527	+
528	+	Real bet = atan2( sqrt(qx*qx+qy*qy), sqrt(qz*qz+qw*qw) );
529	+	Real gam = t/2.0;
530	+	Real sinc = ISZERO(bet,tiny)? 1.0 : sin(bet)/bet;
531	+	Real singam = sin(gam);
532	+	Real cosgam = cos(gam);
533	+
534	+	sx = Real( (2.0/sinc) * (cosgam*qx - singam*qy) );
535	+	sy = Real( (2.0/sinc) * (singam*qx + cosgam*qy) );
536	+	tw = Real( t );
537	+	}
538	+
539	+
540	+
541		/**
542	<	* Returns the division of two quaternion
543	<	* @param q1 divisor
313	<	* @param q2 dividen
542	>	* Returns the corresponding rotation matrix (3x3)
543	>	* @return a 3x3 rotation matrix
544		*/
545	+	SquareMatrix<Real, 3> toRotationMatrix3() {
546	+	SquareMatrix<Real, 3> rotMat3;
547	+
548	+	Real w2;
549	+	Real x2;
550	+	Real y2;
551	+	Real z2;
552
553	<	template<typename Real>
554	<	inline Quaternion<Real> operator /( Quaternion<Real>& q1,  Quaternion<Real>& q2) {
555	<	return q1 * q2.inverse();
553	>	if (!this->isNormalized())
554	>	this->normalize();
555	>
556	>	w2 = w() * w();
557	>	x2 = x() * x();
558	>	y2 = y() * y();
559	>	z2 = z() * z();
560	>
561	>	rotMat3(0, 0) = w2 + x2 - y2 - z2;
562	>	rotMat3(0, 1) = 2.0 * ( x() * y() + w() * z() );
563	>	rotMat3(0, 2) = 2.0 * ( x() * z() - w() * y() );
564	>
565	>	rotMat3(1, 0) = 2.0 * ( x() * y() - w() * z() );
566	>	rotMat3(1, 1) = w2 - x2 + y2 - z2;
567	>	rotMat3(1, 2) = 2.0 * ( y() * z() + w() * x() );
568	>
569	>	rotMat3(2, 0) = 2.0 * ( x() * z() + w() * y() );
570	>	rotMat3(2, 1) = 2.0 * ( y() * z() - w() * x() );
571	>	rotMat3(2, 2) = w2 - x2 -y2 +z2;
572	>
573	>	return rotMat3;
574		}
575
576	+	};//end Quaternion
577	+
578	+
579		/**
580	<	* Returns the value of the division of a scalar by a quaternion
581	<	* @return the value of the division of a scalar by a quaternion
582	<	* @param s scalar
583	<	* @param q quaternion
326	<	* @note for a quaternion q, 1/q = q.inverse()
580	>	* Returns the vaule of scalar multiplication of this quaterion q (q * s).
581	>	* @return  the vaule of scalar multiplication of this vector
582	>	* @param q the source quaternion
583	>	* @param s the scalar value
584		*/
585	<	template<typename Real>
586	<	Quaternion<Real> operator /(const Real& s,  Quaternion<Real>& q) {
585	>	template<typename Real, unsigned int Dim>
586	>	Quaternion<Real> operator * ( const Quaternion<Real>& q, Real s) {
587	>	Quaternion<Real> result(q);
588	>	result.mul(s);
589	>	return result;
590	>	}
591	>
592	>	/**
593	>	* Returns the vaule of scalar multiplication of this quaterion q (q * s).
594	>	* @return  the vaule of scalar multiplication of this vector
595	>	* @param s the scalar value
596	>	* @param q the source quaternion
597	>	*/
598	>	template<typename Real, unsigned int Dim>
599	>	Quaternion<Real> operator * ( const Real& s, const Quaternion<Real>& q ) {
600	>	Quaternion<Real> result(q);
601	>	result.mul(s);
602	>	return result;
603	>	}
604
605	<	Quaternion<Real> x;
606	<	x = q.inverse();
607	<	x *= s;
608	<	return x;
609	<	}
605	>	/**
606	>	* Returns the multiplication of two quaternion
607	>	* @return the multiplication of two quaternion
608	>	* @param q1 the first quaternion
609	>	* @param q2 the second quaternion
610	>	*/
611	>	template<typename Real>
612	>	inline Quaternion<Real> operator *(const Quaternion<Real>& q1, const Quaternion<Real>& q2) {
613	>	Quaternion<Real> result(q1);
614	>	result *= q2;
615	>	return result;
616	>	}
617	>
618	>	/**
619	>	* Returns the division of two quaternion
620	>	* @param q1 divisor
621	>	* @param q2 dividen
622	>	*/
623	>
624	>	template<typename Real>
625	>	inline Quaternion<Real> operator /( Quaternion<Real>& q1,  Quaternion<Real>& q2) {
626	>	return q1 * q2.inverse();
627	>	}
628	>
629	>	/**
630	>	* Returns the value of the division of a scalar by a quaternion
631	>	* @return the value of the division of a scalar by a quaternion
632	>	* @param s scalar
633	>	* @param q quaternion
634	>	* @note for a quaternion q, 1/q = q.inverse()
635	>	*/
636	>	template<typename Real>
637	>	Quaternion<Real> operator /(const Real& s,  Quaternion<Real>& q) {
638	>
639	>	Quaternion<Real> x;
640	>	x = q.inverse();
641	>	x *= s;
642	>	return x;
643	>	}
644
645	<	template <class T>
646	<	inline bool operator==(const Quaternion<T>& lhs, const Quaternion<T>& rhs) {
647	<	return equal(lhs[0] ,rhs[0]) && equal(lhs[1] , rhs[1]) && equal(lhs[2], rhs[2]) && equal(lhs[3], rhs[3]);
648	<	}
645	>	template <class T>
646	>	inline bool operator==(const Quaternion<T>& lhs, const Quaternion<T>& rhs) {
647	>	return equal(lhs[0] ,rhs[0]) && equal(lhs[1] , rhs[1]) && equal(lhs[2], rhs[2]) && equal(lhs[3], rhs[3]);
648	>	}
649
650	<	typedef Quaternion<double> Quat4d;
650	>	typedef Quaternion<RealType> Quat4d;
651		}
652		#endif //MATH_QUATERNION_HPP

Comparing:
trunk/src/math/Quaternion.hpp (property svn:keywords), Revision 110 by tim, Tue Oct 19 21:28:55 2004 UTC vs.
branches/development/src/math/Quaternion.hpp (property svn:keywords), Revision 1850 by gezelter, Wed Feb 20 15:39:39 2013 UTC

#	Line 0 | Line 1
1	+	Author Id Revision Date

Diff Legend

–	Removed lines
+	Added lines
<	Changed lines
>	Changed lines