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