1/*
2 * Copyright 2008 Haiku Inc. All rights reserved.
3 * Distributed under the terms of the MIT License.
4 *
5 * Authors:
6 *		Alexandre Deckner
7 *
8 */
9
10/*
11 *
12 * This is a refactored and stripped down version of bullet-2.66 src\LinearMath\btQuaternion.h
13 * The dependancies on base class btQuadWord have been removed for simplification.
14 * Added gl matrix conversion method.
15 *
16 */
17
18/*
19Copyright (c) 2003-2006 Gino van den Bergen / Erwin Coumans  http://continuousphysics.com/Bullet/
20
21This software is provided 'as-is', without any express or implied warranty.
22In no event will the authors be held liable for any damages arising from the use of this software.
23Permission is granted to anyone to use this software for any purpose,
24including commercial applications, and to alter it and redistribute it freely,
25subject to the following restrictions:
26
271. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
282. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
293. This notice may not be removed or altered from any source distribution.
30*/
31
32
33#ifndef __QUATERNION_H__
34#define __QUATERNION_H__
35
36#include "Vector3.h"
37
38class Quaternion {
39protected:
40	float	m_x;
41	float	m_y;
42	float	m_z;
43	float	m_w;
44public:
45	Quaternion() {}
46
47	Quaternion(const Quaternion& q)
48	{
49		*((Quaternion*)this) = q;
50	}
51
52	Quaternion(const float& x, const float& y, const float& z,const float& w)
53	{
54		m_x = x, m_y = y, m_z = z, m_w = w;
55	}
56
57	Quaternion(const Vector3& axis, const float& angle)
58	{
59		setRotation(axis, angle);
60	}
61
62	Quaternion(const float& yaw, const float& pitch, const float& roll)
63	{
64		setEuler(yaw, pitch, roll);
65	}
66
67	inline const float& x() const { return m_x; }
68
69
70	inline const float& y() const { return m_y; }
71
72
73	inline const float& z() const { return m_z; }
74
75
76	inline const float& w() const { return m_w; }
77
78
79	void setValue(const float& x, const float& y, const float& z)
80	{
81		m_x=x;
82		m_y=y;
83		m_z=z;
84		m_w = 0.f;
85	}
86
87
88	void setValue(const float& x, const float& y, const float& z,const float& w)
89	{
90		m_x=x;
91		m_y=y;
92		m_z=z;
93		m_w=w;
94	}
95
96
97	void setRotation(const Vector3& axis, const float& angle)
98	{
99		float d = axis.length();
100		assert(d != 0.0f);
101		float s = sin(angle * 0.5f) / d;
102		setValue(axis.x() * s, axis.y() * s, axis.z() * s,
103			cos(angle * 0.5f));
104	}
105
106
107	void setEuler(const float& yaw, const float& pitch, const float& roll)
108	{
109		float halfYaw = yaw * 0.5f;
110		float halfPitch = pitch * 0.5f;
111		float halfRoll = roll * 0.5f;
112		float cosYaw = cos(halfYaw);
113		float sinYaw = sin(halfYaw);
114		float cosPitch = cos(halfPitch);
115		float sinPitch = sin(halfPitch);
116		float cosRoll = cos(halfRoll);
117		float sinRoll = sin(halfRoll);
118		setValue(cosRoll * sinPitch * cosYaw + sinRoll * cosPitch * sinYaw,
119			cosRoll * cosPitch * sinYaw - sinRoll * sinPitch * cosYaw,
120			sinRoll * cosPitch * cosYaw - cosRoll * sinPitch * sinYaw,
121			cosRoll * cosPitch * cosYaw + sinRoll * sinPitch * sinYaw);
122	}
123
124
125	Quaternion& operator+=(const Quaternion& q)
126	{
127		m_x += q.x(); m_y += q.y(); m_z += q.z(); m_w += q.m_w;
128		return *this;
129	}
130
131
132	Quaternion& operator-=(const Quaternion& q)
133	{
134		m_x -= q.x(); m_y -= q.y(); m_z -= q.z(); m_w -= q.m_w;
135		return *this;
136	}
137
138
139	Quaternion& operator*=(const float& s)
140	{
141		m_x *= s; m_y *= s; m_z *= s; m_w *= s;
142		return *this;
143	}
144
145
146	Quaternion& operator*=(const Quaternion& q)
147	{
148		setValue(m_w * q.x() + m_x * q.m_w + m_y * q.z() - m_z * q.y(),
149			m_w * q.y() + m_y * q.m_w + m_z * q.x() - m_x * q.z(),
150			m_w * q.z() + m_z * q.m_w + m_x * q.y() - m_y * q.x(),
151			m_w * q.m_w - m_x * q.x() - m_y * q.y() - m_z * q.z());
152		return *this;
153	}
154
155
156	float dot(const Quaternion& q) const
157	{
158		return m_x * q.x() + m_y * q.y() + m_z * q.z() + m_w * q.m_w;
159	}
160
161
162	float length2() const
163	{
164		return dot(*this);
165	}
166
167
168	float length() const
169	{
170		return sqrt(length2());
171	}
172
173
174	Quaternion& normalize()
175	{
176		return *this /= length();
177	}
178
179
180	inline Quaternion
181	operator*(const float& s) const
182	{
183		return Quaternion(x() * s, y() * s, z() * s, m_w * s);
184	}
185
186
187	Quaternion operator/(const float& s) const
188	{
189		assert(s != 0.0f);
190		return *this * (1.0f / s);
191	}
192
193
194	Quaternion& operator/=(const float& s)
195	{
196		assert(s != 0.0f);
197		return *this *= 1.0f / s;
198	}
199
200
201	Quaternion normalized() const
202	{
203		return *this / length();
204	}
205
206
207	float angle(const Quaternion& q) const
208	{
209		float s = sqrt(length2() * q.length2());
210		assert(s != 0.0f);
211		return acos(dot(q) / s);
212	}
213
214
215	float getAngle() const
216	{
217		float s = 2.0f * acos(m_w);
218		return s;
219	}
220
221
222	Quaternion inverse() const
223	{
224		return Quaternion(m_x, m_y, m_z, -m_w);
225	}
226
227
228	inline Quaternion
229	operator+(const Quaternion& q2) const
230	{
231		const Quaternion& q1 = *this;
232		return Quaternion(q1.x() + q2.x(), q1.y() + q2.y(), q1.z() + q2.z(), q1.m_w + q2.m_w);
233	}
234
235
236	inline Quaternion
237	operator-(const Quaternion& q2) const
238	{
239		const Quaternion& q1 = *this;
240		return Quaternion(q1.x() - q2.x(), q1.y() - q2.y(), q1.z() - q2.z(), q1.m_w - q2.m_w);
241	}
242
243
244	inline Quaternion operator-() const
245	{
246		const Quaternion& q2 = *this;
247		return Quaternion( - q2.x(), - q2.y(),  - q2.z(),  - q2.m_w);
248	}
249
250
251	inline Quaternion farthest( const Quaternion& qd) const
252	{
253		Quaternion diff,sum;
254		diff = *this - qd;
255		sum = *this + qd;
256		if( diff.dot(diff) > sum.dot(sum) )
257			return qd;
258		return (-qd);
259	}
260
261
262	Quaternion slerp(const Quaternion& q, const float& t) const
263	{
264		float theta = angle(q);
265		if (theta != 0.0f)
266		{
267			float d = 1.0f / sin(theta);
268			float s0 = sin((1.0f - t) * theta);
269			float s1 = sin(t * theta);
270			return Quaternion((m_x * s0 + q.x() * s1) * d,
271				(m_y * s0 + q.y() * s1) * d,
272				(m_z * s0 + q.z() * s1) * d,
273				(m_w * s0 + q.m_w * s1) * d);
274		}
275		else
276		{
277			return *this;
278		}
279	}
280
281
282	void toOpenGLMatrix(float m[4][4]){
283
284	    float wx, wy, wz, xx, yy, yz, xy, xz, zz, x2, y2, z2;
285
286	    // calculate coefficients
287	    x2 = m_x + m_x; y2 = m_y + m_y;
288	    z2 = m_z + m_z;
289	    xx = m_x * x2; xy = m_x * y2; xz = m_x * z2;
290	    yy = m_y * y2; yz = m_y * z2; zz = m_z * z2;
291	    wx = m_w * x2; wy = m_w * y2; wz = m_w * z2;
292
293
294	    m[0][0] = 1.0 - (yy + zz); m[1][0] = xy - wz;
295	    m[2][0] = xz + wy; m[3][0] = 0.0;
296
297	    m[0][1] = xy + wz; m[1][1] = 1.0 - (xx + zz);
298	    m[2][1] = yz - wx; m[3][1] = 0.0;
299
300
301	    m[0][2] = xz - wy; m[1][2] = yz + wx;
302	    m[2][2] = 1.0 - (xx + yy); m[3][2] = 0.0;
303
304
305	    m[0][3] = 0; m[1][3] = 0;
306	    m[2][3] = 0; m[3][3] = 1;
307	}
308};
309
310
311inline Quaternion
312operator-(const Quaternion& q)
313{
314	return Quaternion(-q.x(), -q.y(), -q.z(), -q.w());
315}
316
317
318inline Quaternion
319operator*(const Quaternion& q1, const Quaternion& q2) {
320	return Quaternion(q1.w() * q2.x() + q1.x() * q2.w() + q1.y() * q2.z() - q1.z() * q2.y(),
321		q1.w() * q2.y() + q1.y() * q2.w() + q1.z() * q2.x() - q1.x() * q2.z(),
322		q1.w() * q2.z() + q1.z() * q2.w() + q1.x() * q2.y() - q1.y() * q2.x(),
323		q1.w() * q2.w() - q1.x() * q2.x() - q1.y() * q2.y() - q1.z() * q2.z());
324}
325
326
327inline Quaternion
328operator*(const Quaternion& q, const Vector3& w)
329{
330	return Quaternion( q.w() * w.x() + q.y() * w.z() - q.z() * w.y(),
331		q.w() * w.y() + q.z() * w.x() - q.x() * w.z(),
332		q.w() * w.z() + q.x() * w.y() - q.y() * w.x(),
333		-q.x() * w.x() - q.y() * w.y() - q.z() * w.z());
334}
335
336
337inline Quaternion
338operator*(const Vector3& w, const Quaternion& q)
339{
340	return Quaternion( w.x() * q.w() + w.y() * q.z() - w.z() * q.y(),
341		w.y() * q.w() + w.z() * q.x() - w.x() * q.z(),
342		w.z() * q.w() + w.x() * q.y() - w.y() * q.x(),
343		-w.x() * q.x() - w.y() * q.y() - w.z() * q.z());
344}
345
346
347inline float
348dot(const Quaternion& q1, const Quaternion& q2)
349{
350	return q1.dot(q2);
351}
352
353
354inline float
355length(const Quaternion& q)
356{
357	return q.length();
358}
359
360
361inline float
362angle(const Quaternion& q1, const Quaternion& q2)
363{
364	return q1.angle(q2);
365}
366
367
368inline Quaternion
369inverse(const Quaternion& q)
370{
371	return q.inverse();
372}
373
374
375inline Quaternion
376slerp(const Quaternion& q1, const Quaternion& q2, const float& t)
377{
378	return q1.slerp(q2, t);
379}
380
381
382inline Quaternion
383shortestArcQuat(const Vector3& v0, const Vector3& v1) // Game Programming Gems 2.10. make sure v0,v1 are normalized
384{
385	Vector3 c = v0.cross(v1);
386	float  d = v0.dot(v1);
387
388	if (d < -1.0 + FLT_EPSILON)
389		return Quaternion(0.0f,1.0f,0.0f,0.0f); // just pick any vector
390
391	float  s = sqrt((1.0f + d) * 2.0f);
392	float rs = 1.0f / s;
393
394	return Quaternion(c.x()*rs, c.y()*rs, c.z()*rs, s * 0.5f);
395}
396
397
398inline Quaternion
399shortestArcQuatNormalize2(Vector3& v0,Vector3& v1)
400{
401	v0.normalize();
402	v1.normalize();
403	return shortestArcQuat(v0,v1);
404}
405#endif
406
407
408
409