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 a rotation matrix.
		/// Matrix must be orthogonal
		void make_rot(const Mat4x4f& m);
	
		/// 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 89 → 171

Rev 89		Rev 171
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
58	`/// Construct a Quaternion from an angle and axis of rotation.`	62	`/// Construct a Quaternion from an angle and axis of rotation.`
59	`void make_rot(float angle, const Vec3f&);`	63	`void make_rot(float angle, const Vec3f&);`
60		64
61	`/** Construct a Quaternion rotating from the direction given`	65	`/** Construct a Quaternion rotating from the direction given`
62	`by the first argument to the direction given by the second.*/`	66	`by the first argument to the direction given by the second.*/`
63	`void make_rot(const Vec3f&,const Vec3f&);`	67	`void make_rot(const Vec3f&,const Vec3f&);`
64		68
65	`/// Obtain angle of rotation and axis`	69	`/// Obtain angle of rotation and axis`
66	`void get_rot(float& angle, Vec3f&);`	70	`void get_rot(float& angle, Vec3f&);`
67		71
68	`/// Multiply two quaternions. (Combine their rotation)`	72	`/// Multiply two quaternions. (Combine their rotation)`
69	`Quaternion operator *(Quaternion quat) const;`	73	`Quaternion operator *(Quaternion quat) const;`
70		74
71	`/// Multiply scalar onto quaternion.`	75	`/// Multiply scalar onto quaternion.`
72	`Quaternion operator *(float scalar) const;`	76	`Quaternion operator *(float scalar) const;`
73		77
74	`/// Add two quaternions.`	78	`/// Add two quaternions.`
75	`Quaternion operator +(Quaternion quat) const;`	79	`Quaternion operator +(Quaternion quat) const;`
76		80
77	`/// Invert quaternion`	81	`/// Invert quaternion`
78	`Quaternion inverse() const;`	82	`Quaternion inverse() const;`
79		83
80	`/// Return conjugate quaternion`	84	`/// Return conjugate quaternion`
81	`Quaternion conjugate() const;`	85	`Quaternion conjugate() const;`
82		86
83	`/// Compute norm of quaternion`	87	`/// Compute norm of quaternion`
84	`float norm() const;`	88	`float norm() const;`
85		89
86	`/// Normalize quaternion.`	90	`/// Normalize quaternion.`
87	`Quaternion normalize();`	91	`Quaternion normalize();`
88		92
89	`/// Rotate vector according to quaternion`	93	`/// Rotate vector according to quaternion`
90	`Vec3f apply(const Vec3f& vec) const;`	94	`Vec3f apply(const Vec3f& vec) const;`
91	`};`	95	`};`
92		96
93	`/// Compare for equality.`	97	`/// Compare for equality.`
94	`inline bool operator==(const Quaternion& q0, const Quaternion& q1)`	98	`inline bool operator==(const Quaternion& q0, const Quaternion& q1)`
95	`{`	99	`{`
96	`return (q0.qw == q1.qw && q0.qv == q1.qv);`	100	`return (q0.qw == q1.qw && q0.qv == q1.qv);`
97	`}`	101	`}`
98		102
99	`/// Print quaternion to stream.`	103	`/// Print quaternion to stream.`
100	`inline std::ostream& operator<<(std::ostream&os, const Quaternion v)`	104	`inline std::ostream& operator<<(std::ostream&os, const Quaternion v)`
101	`{`	105	`{`
102	`os << "[ ";`	106	`os << "[ ";`
103	`for(unsigned int i=0;i<3;i++) os << v.qv[i] << " ";`	107	`for(unsigned int i=0;i<3;i++) os << v.qv[i] << " ";`
104	`os << " ~ " << v.qw << " ";`	108	`os << " ~ " << v.qw << " ";`
105	`os << "]";`	109	`os << "]";`
106		110
107	`return os;`	111	`return os;`
108	`}`	112	`}`
109		113
110	`inline Quaternion Quaternion::operator *(Quaternion quat) const`	114	`inline Quaternion Quaternion::operator *(Quaternion quat) const`
111	`{`	115	`{`
112	`return Quaternion(cross(qv,quat.qv) + quat.qwqv + qwquat.qv,`	116	`return Quaternion(cross(qv,quat.qv) + quat.qwqv + qwquat.qv,`
113	`qw*quat.qw - dot(qv,quat.qv));`	117	`qw*quat.qw - dot(qv,quat.qv));`
114	`}`	118	`}`
115		119
116	`inline Quaternion Quaternion::operator *(float scalar) const`	120	`inline Quaternion Quaternion::operator *(float scalar) const`
117	`{`	121	`{`
118	`return Quaternion(scalarqv,scalarqw);`	122	`return Quaternion(scalarqv,scalarqw);`
119	`}`	123	`}`
120		124
121	`/// Multiply scalar onto quaternion`	125	`/// Multiply scalar onto quaternion`
122	`inline Quaternion operator *(float scalar, Quaternion quat)`	126	`inline Quaternion operator *(float scalar, Quaternion quat)`
123	`{`	127	`{`
124	`return Quaternion(scalarquat.qv,scalarquat.qw);`	128	`return Quaternion(scalarquat.qv,scalarquat.qw);`
125	`}`	129	`}`
126		130
127		131
128	`inline Quaternion Quaternion::operator +(Quaternion quat) const`	132	`inline Quaternion Quaternion::operator +(Quaternion quat) const`
129	`{`	133	`{`
130	`return Quaternion(qv + quat.qv,qw + quat.qw);`	134	`return Quaternion(qv + quat.qv,qw + quat.qw);`
131	`}`	135	`}`
132		136
133	`inline float Quaternion::norm() const`	137	`inline float Quaternion::norm() const`
134	`{`	138	`{`
135	`return qv[0]qv[0] + qv[1]qv[1] + qv[2]qv[2] + qwqw;`	139	`return qv[0]qv[0] + qv[1]qv[1] + qv[2]qv[2] + qwqw;`
136	`}`	140	`}`
137		141
138	`inline Quaternion Quaternion::normalize()`	142	`inline Quaternion Quaternion::normalize()`
139	`{`	143	`{`
140	`return Quaternion(1.0/norm()(this));`	144	`return Quaternion(1.0/norm()(this));`
141	`}`	145	`}`
142		146
143	`inline Quaternion Quaternion::conjugate() const`	147	`inline Quaternion Quaternion::conjugate() const`
144	`{`	148	`{`
145	`return Quaternion(-qv,qw);`	149	`return Quaternion(-qv,qw);`
146	`}`	150	`}`
147		151
148	`inline Quaternion Quaternion::inverse() const`	152	`inline Quaternion Quaternion::inverse() const`
149	`{`	153	`{`
150	`return Quaternion(1.0/norm()*conjugate());`	154	`return Quaternion(1.0/norm()*conjugate());`
151	`}`	155	`}`
152		156
153		157
154	`/** Perform linear interpolation of two quaternions.`	158	`/** Perform linear interpolation of two quaternions.`
155	`The last argument is the parameter used to interpolate`	159	`The last argument is the parameter used to interpolate`
156	`between the two first. SLERP - invented by Shoemake -`	160	`between the two first. SLERP - invented by Shoemake -`
157	`is a good way to interpolate because the interpolation`	161	`is a good way to interpolate because the interpolation`
158	`is performed on the unit sphere.`	162	`is performed on the unit sphere.`
159	`*/`	163	`*/`
160	`inline Quaternion slerp(Quaternion q0, Quaternion q1, float t)`	164	`inline Quaternion slerp(Quaternion q0, Quaternion q1, float t)`
161	`{`	165	`{`
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);`	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);`
163	`return (q0sin((1-t)angle)+q1sin(tangle))*(1/sin(angle));`	167	`return (q0sin((1-t)angle)+q1sin(tangle))*(1/sin(angle));`
164	`}`	168	`}`
165		169
166	`}`	170	`}`
167	`#endif`	171	`#endif`
168		172