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


#ifndef __CGLA_QUATERNION_H__
#define __CGLA_QUATERNION_H__
 
#include "CGLA/Vec3f.h"
#include "CGLA/Vec4f.h"
#include "CGLA/Mat3x3f.h"
#include "CGLA/Mat4x4f.h"
 
namespace CGLA {
 
	/** 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(1) {}
 
		/// 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(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 2 → 5

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