ViewVC Help

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

root/OpenMD/trunk/src/math/Quaternion.hpp

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

Comparing trunk/src/math/Quaternion.hpp (property svn:keywords):
Revision 99 by tim, Mon Oct 18 17:07:27 2004 UTC vs.
Revision 1687 by gezelter, Sat Mar 10 16:00:24 2012 UTC

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

Diff Legend

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