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 171 → 178

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