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 507 by gezelter, Fri Apr 15 22:04:00 2005 UTC vs.
branches/development/src/math/Quaternion.hpp (file contents), Revision 1665 by gezelter, Tue Nov 22 20:38:56 2011 UTC

#	Line 6 | Line 6
6		* redistribute this software in source and binary code form, provided
7		* that the following conditions are met:
8		*
9	<	* 1. Acknowledgement of the program authors must be made in any
10	<	*    publication of scientific results based in part on use of the
11	<	*    program.  An acceptable form of acknowledgement is citation of
12	<	*    the article in which the program was described (Matthew
13	<	*    A. Meineke, Charles F. Vardeman II, Teng Lin, Christopher
14	<	*    J. Fennell and J. Daniel Gezelter, "OOPSE: An Object-Oriented
15	<	*    Parallel Simulation Engine for Molecular Dynamics,"
16	<	*    J. Comput. Chem. 26, pp. 252-271 (2005))
17	<	*
18	<	* 2. Redistributions of source code must retain the above copyright
9	>	* 1. Redistributions of source code must retain the above copyright
10		*    notice, this list of conditions and the following disclaimer.
11		*
12	<	* 3. Redistributions in binary form must reproduce the above copyright
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.
#	Line 37 | Line 28
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
43		/**
#	Line 49 | 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
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		public:
72		Quaternion() : Vector<Real, 4>() {}
73
#	Line 78 | Line 82 | namespace oopse{
82		/** Constructs and initializes a Quaternion from a  Vector<Real,4> */
83		Quaternion(const Vector<Real,4>& v)
84		: Vector<Real, 4>(v){
85	<	}
85	>	}
86
87		/** copy assignment */
88		Quaternion& operator =(const Vector<Real, 4>& v){
89		if (this == & v)
90		return *this;
91	<
91	>
92		Vector<Real, 4>::operator=(v);
93	<
93	>
94		return *this;
95		}
96	<
96	>
97		/**
98		* Returns the value of the first element of this quaternion.
99		* @return the value of the first element of this quaternion
#	Line 242 | Line 246 | namespace oopse{
246		* Returns the conjugate quaternion of this quaternion
247		* @return the conjugate quaternion of this quaternion
248		*/
249	<	Quaternion<Real> conjugate() {
249	>	Quaternion<Real> conjugate() const {
250		return Quaternion<Real>(w(), -x(), -y(), -z());
251	+	}
252	+
253	+
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	+	/**
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	+	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	+
302	+	/**
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 corresponding rotation matrix (3x3)
541		* @return a 3x3 rotation matrix
542		*/
543		SquareMatrix<Real, 3> toRotationMatrix3() {
544		SquareMatrix<Real, 3> rotMat3;
545	<
545	>
546		Real w2;
547		Real x2;
548		Real y2;
#	Line 355 | Line 645 | namespace oopse{
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<double> Quat4d;
648	>	typedef Quaternion<RealType> Quat4d;
649		}
650		#endif //MATH_QUATERNION_HPP

Comparing:
trunk/src/math/Quaternion.hpp (property svn:keywords), Revision 507 by gezelter, Fri Apr 15 22:04:00 2005 UTC vs.
branches/development/src/math/Quaternion.hpp (property svn:keywords), Revision 1665 by gezelter, Tue Nov 22 20:38:56 2011 UTC

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

Diff Legend

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