WebSVN – gelsvn – Diff – /trunk/src/CGLA/Quaternion.h


#ifndef __CGLA_QUATERNION_H__
#define __CGLA_QUATERNION_H__
 
#include "Vec3f.h"
#include "Vec4f.h"
#include "Mat3x3f.h"
#include "Mat4x4f.h"
 
namespace CGLA {
 
	/** \brief A Quaterinion class. 
 
	Quaternions are algebraic entities useful for rotation. */
 
	class Quaternion
	{
	public:
 
		/// Vector part of quaternion
		Vec3f qv;
 
		/// Scalar part of quaternion
		float qw;
 
		/// Construct 0 quaternion
		Quaternion(): qw(CGLA_INIT_VALUE) {}
 
		/// Construct quaternion from vector and scalar
		Quaternion(const Vec3f _qv, float _qw=1) : 
			qv(_qv) , qw(_qw) {}
 
		/// Construct quaternion from four scalars
		Quaternion(float x, float y, float z, float _qw) : 
			qv(x,y,z), qw(_qw) {}
 
		/// Assign values to a quaternion
		void set(float x, float y, float z, float _qw) 
		{
			qv.set(x,y,z);
			qw = _qw;
		}
 
		/// Get values from a quaternion
		void get(float& x, float& y, float& z, float& _qw) const
		{
			x  = qv[0];
			y  = qv[1];
			z  = qv[2];
			_qw = qw;
		}
 
		/// Get a 3x3 rotation matrix from a quaternion
		Mat3x3f get_mat3x3f() const;
 
		/// Get a 4x4 rotation matrix from a quaternion
		Mat4x4f get_mat4x4f() const;
 
		/// Construct a Quaternion from an angle and axis of rotation.
		void make_rot(float angle, const Vec3f&);
 
		/** Construct a Quaternion rotating from the direction given
				by the first argument to the direction given by the second.*/
		void make_rot(const Vec3f&,const Vec3f&);
 
		/// Obtain angle of rotation and axis
		void get_rot(float& angle, Vec3f&);
 
		/// Multiply two quaternions. (Combine their rotation)
		Quaternion operator *(Quaternion quat) const;
 
		/// Multiply scalar onto quaternion.
		Quaternion operator *(float scalar) const;
 
		/// Add two quaternions.
		Quaternion operator +(Quaternion quat) const;
		
		/// Invert quaternion
		Quaternion inverse() const;
 
		/// Return conjugate quaternion
		Quaternion conjugate() const;
 
		/// Compute norm of quaternion
		float norm() const;
 
		/// Normalize quaternion.
		Quaternion normalize();
 
		/// Rotate vector according to quaternion
		Vec3f apply(const Vec3f& vec) const;
	};
 
	/// Compare for equality.
	inline bool operator==(const Quaternion& q0, const Quaternion& q1)
	{
		return (q0.qw == q1.qw && q0.qv == q1.qv);
	}
 
	/// Print quaternion to stream.
	inline std::ostream& operator<<(std::ostream&os, const Quaternion v)
	{
		os << "[ ";
		for(unsigned int i=0;i<3;i++) os << v.qv[i] << " ";
		os << " ~ " << v.qw << " ";
		os << "]";
 
		return os;
	}
 
	inline Quaternion Quaternion::operator *(Quaternion quat) const
	{
		return Quaternion(cross(qv,quat.qv) + quat.qw*qv + qw*quat.qv, 
											qw*quat.qw - dot(qv,quat.qv));
	}
 
	inline Quaternion Quaternion::operator *(float scalar) const
	{
		return Quaternion(scalar*qv,scalar*qw);
	}
 
	/// Multiply scalar onto quaternion
	inline Quaternion operator *(float scalar, Quaternion quat) 
	{
		return Quaternion(scalar*quat.qv,scalar*quat.qw);
	}
 
	
	inline Quaternion Quaternion::operator +(Quaternion quat) const
	{
		return Quaternion(qv + quat.qv,qw + quat.qw);
	}
 
	inline float Quaternion::norm() const
	{
		return qv[0]*qv[0] + qv[1]*qv[1] + qv[2]*qv[2] + qw*qw;
	}
 
	inline Quaternion Quaternion::normalize() 
	{
		return Quaternion(1.0/norm()*(*this));
	}
 
	inline Quaternion Quaternion::conjugate() const
	{
		return Quaternion(-qv,qw);
	}
 
	inline Quaternion Quaternion::inverse() const
	{
		return Quaternion(1.0/norm()*conjugate());
	}
 
 
	/** Perform linear interpolation of two quaternions. 
			The last argument is the parameter used to interpolate
			between the two first. SLERP - invented by Shoemake -
			is a good way to interpolate because the interpolation
			is performed on the unit sphere. 	
	*/
	inline Quaternion slerp(Quaternion q0, Quaternion q1, float t) 
	{
		float angle=acos(q0.qv[0]*q1.qv[0]+q0.qv[1]*q1.qv[1]+q0.qv[2]*q1.qv[2]+q0.qw*q1.qw);
		return (q0*sin((1-t)*angle)+q1*sin(t*angle))*(1/sin(angle));
	}
 
}
#endif
 

Subversion Repositories gelsvn

(root)/trunk/src/CGLA/Quaternion.h – Rev 45 → 89

Rev 45		Rev 89
1	`#ifndef __CGLA_QUATERNION_H__`	1	`#ifndef __CGLA_QUATERNION_H__`
2	`#define __CGLA_QUATERNION_H__`	2	`#define __CGLA_QUATERNION_H__`
3		3
4	`#include "Vec3f.h"`	4	`#include "Vec3f.h"`
5	`#include "Vec4f.h"`	5	`#include "Vec4f.h"`
6	`#include "Mat3x3f.h"`	6	`#include "Mat3x3f.h"`
7	`#include "Mat4x4f.h"`	7	`#include "Mat4x4f.h"`
8		8
9	`namespace CGLA {`	9	`namespace CGLA {`
10		10
11	`/** A Quaterinion class. Quaternions are algebraic entities`	11	`/** \brief A Quaterinion class.`
-		12
12	`useful for rotation. */`	13	`Quaternions are algebraic entities useful for rotation. */`
13		14
14	`class Quaternion`	15	`class Quaternion`
15	`{`	16	`{`
16	`public:`	17	`public:`
17		18
18	`/// Vector part of quaternion`	19	`/// Vector part of quaternion`
19	`Vec3f qv;`	20	`Vec3f qv;`
20		21
21	`/// Scalar part of quaternion`	22	`/// Scalar part of quaternion`
22	`float qw;`	23	`float qw;`
23		24
24	`/// Construct 0 quaternion`	25	`/// Construct 0 quaternion`
25	`Quaternion(): qw(CGLA_INIT_VALUE) {}`	26	`Quaternion(): qw(CGLA_INIT_VALUE) {}`
26		27
27	`/// Construct quaternion from vector and scalar`	28	`/// Construct quaternion from vector and scalar`
28	`Quaternion(const Vec3f _qv, float _qw=1) :`	29	`Quaternion(const Vec3f _qv, float _qw=1) :`
29	`qv(_qv) , qw(_qw) {}`	30	`qv(_qv) , qw(_qw) {}`
30		31
31	`/// Construct quaternion from four scalars`	32	`/// Construct quaternion from four scalars`
32	`Quaternion(float x, float y, float z, float _qw) :`	33	`Quaternion(float x, float y, float z, float _qw) :`
33	`qv(x,y,z), qw(_qw) {}`	34	`qv(x,y,z), qw(_qw) {}`
34		35
35	`/// Assign values to a quaternion`	36	`/// Assign values to a quaternion`
36	`void set(float x, float y, float z, float _qw)`	37	`void set(float x, float y, float z, float _qw)`
37	`{`	38	`{`
38	`qv.set(x,y,z);`	39	`qv.set(x,y,z);`
39	`qw = _qw;`	40	`qw = _qw;`
40	`}`	41	`}`
41		42
42	`/// Get values from a quaternion`	43	`/// Get values from a quaternion`
43	`void get(float& x, float& y, float& z, float& _qw) const`	44	`void get(float& x, float& y, float& z, float& _qw) const`
44	`{`	45	`{`
45	`x = qv[0];`	46	`x = qv[0];`
46	`y = qv[1];`	47	`y = qv[1];`
47	`z = qv[2];`	48	`z = qv[2];`
48	`_qw = qw;`	49	`_qw = qw;`
49	`}`	50	`}`
50		51
51	`/// Get a 3x3 rotation matrix from a quaternion`	52	`/// Get a 3x3 rotation matrix from a quaternion`
52	`Mat3x3f get_mat3x3f() const;`	53	`Mat3x3f get_mat3x3f() const;`
53		54
54	`/// Get a 4x4 rotation matrix from a quaternion`	55	`/// Get a 4x4 rotation matrix from a quaternion`
55	`Mat4x4f get_mat4x4f() const;`	56	`Mat4x4f get_mat4x4f() const;`
56		57
57	`/// Construct a Quaternion from an angle and axis of rotation.`	58	`/// Construct a Quaternion from an angle and axis of rotation.`
58	`void make_rot(float angle, const Vec3f&);`	59	`void make_rot(float angle, const Vec3f&);`
59		60
60	`/** Construct a Quaternion rotating from the direction given`	61	`/** Construct a Quaternion rotating from the direction given`
61	`by the first argument to the direction given by the second.*/`	62	`by the first argument to the direction given by the second.*/`
62	`void make_rot(const Vec3f&,const Vec3f&);`	63	`void make_rot(const Vec3f&,const Vec3f&);`
63		64
64	`/// Obtain angle of rotation and axis`	65	`/// Obtain angle of rotation and axis`
65	`void get_rot(float& angle, Vec3f&);`	66	`void get_rot(float& angle, Vec3f&);`
66		67
67	`/// Multiply two quaternions. (Combine their rotation)`	68	`/// Multiply two quaternions. (Combine their rotation)`
68	`Quaternion operator *(Quaternion quat) const;`	69	`Quaternion operator *(Quaternion quat) const;`
69		70
70	`/// Multiply scalar onto quaternion.`	71	`/// Multiply scalar onto quaternion.`
71	`Quaternion operator *(float scalar) const;`	72	`Quaternion operator *(float scalar) const;`
72		73
73	`/// Add two quaternions.`	74	`/// Add two quaternions.`
74	`Quaternion operator +(Quaternion quat) const;`	75	`Quaternion operator +(Quaternion quat) const;`
75		76
76	`/// Invert quaternion`	77	`/// Invert quaternion`
77	`Quaternion inverse() const;`	78	`Quaternion inverse() const;`
78		79
79	`/// Return conjugate quaternion`	80	`/// Return conjugate quaternion`
80	`Quaternion conjugate() const;`	81	`Quaternion conjugate() const;`
81		82
82	`/// Compute norm of quaternion`	83	`/// Compute norm of quaternion`
83	`float norm() const;`	84	`float norm() const;`
84		85
85	`/// Normalize quaternion.`	86	`/// Normalize quaternion.`
86	`Quaternion normalize();`	87	`Quaternion normalize();`
87		88
88	`/// Rotate vector according to quaternion`	89	`/// Rotate vector according to quaternion`
89	`Vec3f apply(const Vec3f& vec) const;`	90	`Vec3f apply(const Vec3f& vec) const;`
90	`};`	91	`};`
91		92
92	`/// Compare for equality.`	93	`/// Compare for equality.`
93	`inline bool operator==(const Quaternion& q0, const Quaternion& q1)`	94	`inline bool operator==(const Quaternion& q0, const Quaternion& q1)`
94	`{`	95	`{`
95	`return (q0.qw == q1.qw && q0.qv == q1.qv);`	96	`return (q0.qw == q1.qw && q0.qv == q1.qv);`
96	`}`	97	`}`
97		98
98	`/// Print quaternion to stream.`	99	`/// Print quaternion to stream.`
99	`inline std::ostream& operator<<(std::ostream&os, const Quaternion v)`	100	`inline std::ostream& operator<<(std::ostream&os, const Quaternion v)`
100	`{`	101	`{`
101	`os << "[ ";`	102	`os << "[ ";`
102	`for(unsigned int i=0;i<3;i++) os << v.qv[i] << " ";`	103	`for(unsigned int i=0;i<3;i++) os << v.qv[i] << " ";`
103	`os << " ~ " << v.qw << " ";`	104	`os << " ~ " << v.qw << " ";`
104	`os << "]";`	105	`os << "]";`
105		106
106	`return os;`	107	`return os;`
107	`}`	108	`}`
108		109
109	`inline Quaternion Quaternion::operator *(Quaternion quat) const`	110	`inline Quaternion Quaternion::operator *(Quaternion quat) const`
110	`{`	111	`{`
111	`return Quaternion(cross(qv,quat.qv) + quat.qwqv + qwquat.qv,`	112	`return Quaternion(cross(qv,quat.qv) + quat.qwqv + qwquat.qv,`
112	`qw*quat.qw - dot(qv,quat.qv));`	113	`qw*quat.qw - dot(qv,quat.qv));`
113	`}`	114	`}`
114		115
115	`inline Quaternion Quaternion::operator *(float scalar) const`	116	`inline Quaternion Quaternion::operator *(float scalar) const`
116	`{`	117	`{`
117	`return Quaternion(scalarqv,scalarqw);`	118	`return Quaternion(scalarqv,scalarqw);`
118	`}`	119	`}`
119		120
120	`/// Multiply scalar onto quaternion`	121	`/// Multiply scalar onto quaternion`
121	`inline Quaternion operator *(float scalar, Quaternion quat)`	122	`inline Quaternion operator *(float scalar, Quaternion quat)`
122	`{`	123	`{`
123	`return Quaternion(scalarquat.qv,scalarquat.qw);`	124	`return Quaternion(scalarquat.qv,scalarquat.qw);`
124	`}`	125	`}`
125		126
126		127
127	`inline Quaternion Quaternion::operator +(Quaternion quat) const`	128	`inline Quaternion Quaternion::operator +(Quaternion quat) const`
128	`{`	129	`{`
129	`return Quaternion(qv + quat.qv,qw + quat.qw);`	130	`return Quaternion(qv + quat.qv,qw + quat.qw);`
130	`}`	131	`}`
131		132
132	`inline float Quaternion::norm() const`	133	`inline float Quaternion::norm() const`
133	`{`	134	`{`
134	`return qv[0]qv[0] + qv[1]qv[1] + qv[2]qv[2] + qwqw;`	135	`return qv[0]qv[0] + qv[1]qv[1] + qv[2]qv[2] + qwqw;`
135	`}`	136	`}`
136		137
137	`inline Quaternion Quaternion::normalize()`	138	`inline Quaternion Quaternion::normalize()`
138	`{`	139	`{`
139	`return Quaternion(1.0/norm()(this));`	140	`return Quaternion(1.0/norm()(this));`
140	`}`	141	`}`
141		142
142	`inline Quaternion Quaternion::conjugate() const`	143	`inline Quaternion Quaternion::conjugate() const`
143	`{`	144	`{`
144	`return Quaternion(-qv,qw);`	145	`return Quaternion(-qv,qw);`
145	`}`	146	`}`
146		147
147	`inline Quaternion Quaternion::inverse() const`	148	`inline Quaternion Quaternion::inverse() const`
148	`{`	149	`{`
149	`return Quaternion(1.0/norm()*conjugate());`	150	`return Quaternion(1.0/norm()*conjugate());`
150	`}`	151	`}`
151		152
152		153
153	`/** Perform linear interpolation of two quaternions.`	154	`/** Perform linear interpolation of two quaternions.`
154	`The last argument is the parameter used to interpolate`	155	`The last argument is the parameter used to interpolate`
155	`between the two first. SLERP - invented by Shoemake -`	156	`between the two first. SLERP - invented by Shoemake -`
156	`is a good way to interpolate because the interpolation`	157	`is a good way to interpolate because the interpolation`
157	`is performed on the unit sphere.`	158	`is performed on the unit sphere.`
158	`*/`	159	`*/`
159	`inline Quaternion slerp(Quaternion q0, Quaternion q1, float t)`	160	`inline Quaternion slerp(Quaternion q0, Quaternion q1, float t)`
160	`{`	161	`{`
161	`float angle=acos(q0.qv[0]q1.qv[0]+q0.qv[1]q1.qv[1]+q0.qv[2]q1.qv[2]+q0.qwq1.qw);`	162	`float angle=acos(q0.qv[0]q1.qv[0]+q0.qv[1]q1.qv[1]+q0.qv[2]q1.qv[2]+q0.qwq1.qw);`
162	`return (q0sin((1-t)angle)+q1sin(tangle))*(1/sin(angle));`	163	`return (q0sin((1-t)angle)+q1sin(tangle))*(1/sin(angle));`
163	`}`	164	`}`
164		165
165	`}`	166	`}`
166	`#endif`	167	`#endif`
167		168