PageRenderTime 121ms CodeModel.GetById 16ms app.highlight 98ms RepoModel.GetById 0ms app.codeStats 1ms

/xbmc/screensavers/rsxs-0.9/src/vector.hh

http://github.com/xbmc/xbmc
C++ Header | 797 lines | 668 code | 101 blank | 28 comment | 19 complexity | da00a397ca2381f701e962ae1b9fcf17 MD5 | raw file
  1/*
  2 * Really Slick XScreenSavers
  3 * Copyright (C) 2002-2006  Michael Chapman
  4 *
  5 * This program is free software; you can redistribute it and/or modify
  6 * it under the terms of the GNU General Public License version 2 as
  7 * published by the Free Software Foundation.
  8 *
  9 * This program is distributed in the hope that it will be useful,
 10 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 11 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 12 * GNU General Public License for more details.
 13 *
 14 * You should have received a copy of the GNU General Public License
 15 * along with this program; if not, write to the Free Software
 16 * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
 17 *
 18 *****************************************************************************
 19 *
 20 * This is a Linux port of the Really Slick Screensavers,
 21 * Copyright (C) 2002 Terence M. Welsh, available from www.reallyslick.com
 22 */
 23#ifndef _VECTOR_HH
 24#define _VECTOR_HH
 25
 26#include <common.hh>
 27
 28#define R2D (180.0f / M_PI)
 29#define D2R (M_PI / 180.0f)
 30
 31template <typename T>
 32class VectorBase {
 33protected:
 34	T _v[3];
 35public:
 36	VectorBase(const T* v) {
 37		set(v[0], v[1], v[2]);
 38	}
 39
 40	VectorBase(const T& x, const T& y, const T& z) {
 41		set(x, y, z);
 42	}
 43
 44	void set(const T& x, const T& y, const T& z) {
 45		_v[0] = x;
 46		_v[1] = y;
 47		_v[2] = z;
 48	}
 49
 50	const T& x() const { return _v[0]; }
 51	T& x() { return _v[0]; }
 52	const T& y() const { return _v[1]; }
 53	T& y() { return _v[1]; }
 54	const T& z() const { return _v[2]; }
 55	T& z() { return _v[2]; }
 56
 57	const T* get() const {
 58		return _v;
 59	}
 60
 61	T lengthSquared() const {
 62		return _v[0] * _v[0] + _v[1] * _v[1] + _v[2] * _v[2];
 63	}
 64};
 65
 66class Vector : public VectorBase<float> {
 67public:
 68	Vector(const float* v) : VectorBase<float>(v) {}
 69
 70	Vector(float x = 0.0f, float y = 0.0f, float z = 0.0f)
 71		: VectorBase<float>(x, y, z) {}
 72
 73	float length() const {
 74		return std::sqrt(lengthSquared());
 75	}
 76
 77	float normalize() {
 78		float l = length();
 79		if (l == 0.0f)
 80			return 0.0f;
 81		_v[0] /= l;
 82		_v[1] /= l;
 83		_v[2] /= l;
 84		return l;
 85	}
 86
 87	Vector operator+(const Vector& v) const {
 88		return Vector(_v[0] + v._v[0], _v[1] + v._v[1], _v[2] + v._v[2]);
 89	}
 90
 91	Vector operator-(const Vector& v) const {
 92		return Vector(_v[0] - v._v[0], _v[1] - v._v[1], _v[2] - v._v[2]);
 93	}
 94
 95	Vector operator*(float f) const {
 96		return Vector(_v[0] * f, _v[1] * f, _v[2] * f);
 97	}
 98
 99	Vector operator/(float f) const {
100		return Vector(_v[0] / f, _v[1] / f, _v[2] / f);
101	}
102
103	Vector& operator+=(const Vector& v) {
104		_v[0] += v._v[0];
105		_v[1] += v._v[1];
106		_v[2] += v._v[2];
107		return *this;
108	}
109
110	Vector& operator-=(const Vector& v) {
111		_v[0] -= v._v[0];
112		_v[1] -= v._v[1];
113		_v[2] -= v._v[2];
114		return *this;
115	}
116
117	Vector& operator*=(float f) {
118		_v[0] *= f;
119		_v[1] *= f;
120		_v[2] *= f;
121		return *this;
122	}
123
124	Vector& operator/=(float f) {
125		_v[0] /= f;
126		_v[1] /= f;
127		_v[2] /= f;
128		return *this;
129	}
130
131	static float dot(const Vector& a, const Vector& b) {
132		return a.x() * b.x() + a.y() * b.y() + a.z() * b.z();
133	}
134
135	static Vector cross(const Vector& a, const Vector& b) {
136		return Vector(
137			a.y() * b.z() - b.y() * a.z(),
138			a.z() * b.x() - b.z() * a.x(),
139			a.x() * b.y() - b.x() * a.y()
140		);
141	};
142};
143
144class UVector : public VectorBase<unsigned int> {
145public:
146	UVector(const unsigned int* v) : VectorBase<unsigned int>(v) {}
147
148	UVector(unsigned int x = 0, unsigned int y = 0, unsigned int z = 0)
149		: VectorBase<unsigned int>(x, y, z) {}
150
151	UVector operator+(const UVector& v) const {
152		return UVector(_v[0] + v._v[0], _v[1] + v._v[1], _v[2] + v._v[2]);
153	}
154
155	UVector operator-(const UVector& v) const {
156		return UVector(_v[0] - v._v[0], _v[1] - v._v[1], _v[2] - v._v[2]);
157	}
158
159	UVector& operator+=(const UVector& v) {
160		_v[0] += v._v[0];
161		_v[1] += v._v[1];
162		_v[2] += v._v[2];
163		return *this;
164	}
165
166	UVector& operator-=(const UVector& v) {
167		_v[0] -= v._v[0];
168		_v[1] -= v._v[1];
169		_v[2] -= v._v[2];
170		return *this;
171	}
172};
173
174class UnitVector : public Vector {
175protected:
176	using Vector::set;
177	using Vector::length;
178	using Vector::lengthSquared;
179	using Vector::normalize;
180
181	UnitVector(const float* v) {
182		set(v[0], v[1], v[2]);
183	}
184
185	UnitVector(float x, float y, float z) {
186		set(x, y, z);
187	}
188
189	friend class Vector;
190	friend class RotationMatrix;
191	friend class UnitQuat;
192public:
193	UnitVector(const Vector& v) {
194		float l = v.length();
195		if (l == 0.0f)
196			set(1.0f, 0.0f, 0.0f);
197		else
198			set(v.x() / l, v.y() / l, v.z() / l);
199	}
200
201	UnitVector() {
202		set(1.0f, 0.0f, 0.0f);
203	}
204
205	static UnitVector of(const Vector& v) {
206		float l = v.length();
207		if (l == 0.0f)
208			return UnitVector();
209		return UnitVector(
210			v.x() / l,
211			v.y() / l,
212			v.z() / l
213		);
214	}
215};
216
217class Matrix {
218protected:
219	float _m[16];
220public:
221	Matrix(
222		float m0 = 1.0f, float m1 = 0.0f, float m2 = 0.0f, float m3 = 0.0f,
223		float m4 = 0.0f, float m5 = 1.0f, float m6 = 0.0f, float m7 = 0.0f,
224		float m8 = 0.0f, float m9 = 0.0f, float m10 = 1.0f, float m11 = 0.0f,
225		float m12 = 0.0f, float m13 = 0.0f, float m14 = 0.0f, float m15 = 1.0f
226	) {
227		_m[0] = m0; _m[1] = m1; _m[2] = m2; _m[3] = m3;
228		_m[4] = m4; _m[5] = m5; _m[6] = m6; _m[7] = m7;
229		_m[8] = m8; _m[9] = m9; _m[10] = m10; _m[11] = m11;
230		_m[12] = m12; _m[13] = m13; _m[14] = m14; _m[15] = m15;
231	}
232
233	float get(int i) const {
234		return _m[i];
235	}
236
237	const float* get() const {
238		return _m;
239	}
240
241	Matrix transposed() const {
242		return Matrix(
243			_m[0], _m[4], _m[8], _m[12],
244			_m[1], _m[5], _m[9], _m[13],
245			_m[2], _m[6], _m[10], _m[14],
246			_m[3], _m[7], _m[11], _m[15]
247		);
248	}
249
250	Vector transform(const Vector& v) const {
251		float w = _m[3] * v.x() + _m[7] * v.y() + _m[11] * v.z() + _m[15];
252		return Vector(
253			(_m[0] * v.x() + _m[4] * v.y() + _m[8] * v.z() + _m[12]) / w,
254			(_m[1] * v.x() + _m[5] * v.y() + _m[9] * v.z() + _m[13]) / w,
255			(_m[2] * v.x() + _m[6] * v.y() + _m[10] * v.z() + _m[14]) / w
256		);
257	}
258
259	Matrix operator*(const Matrix& m) const {
260		return Matrix(
261			_m[0] * m._m[0] + _m[1] * m._m[4] +
262				_m[2] * m._m[8] + _m[3] * m._m[12],
263			_m[0] * m._m[1] + _m[1] * m._m[5] +
264				_m[2] * m._m[9] + _m[3] * m._m[13],
265			_m[0] * m._m[2] + _m[1] * m._m[6] +
266				_m[2] * m._m[10] + _m[3] * m._m[14],
267			_m[0] * m._m[3] + _m[1] * m._m[7] +
268				_m[2] * m._m[11] + _m[3] * m._m[15]
269		,
270			_m[4] * m._m[0] + _m[5] * m._m[4] +
271				_m[6] * m._m[8] + _m[7] * m._m[12],
272			_m[4] * m._m[1] + _m[5] * m._m[5] +
273				_m[6] * m._m[9] + _m[7] * m._m[13],
274			_m[4] * m._m[2] + _m[5] * m._m[6] +
275				_m[6] * m._m[10] + _m[7] * m._m[14],
276			_m[4] * m._m[3] + _m[5] * m._m[7] +
277				_m[6] * m._m[11] + _m[7] * m._m[15]
278		,
279			_m[8] * m._m[0] + _m[9] * m._m[4] +
280				_m[10] * m._m[8] + _m[11] * m._m[12],
281			_m[8] * m._m[1] + _m[9] * m._m[5] +
282				_m[10] * m._m[9] + _m[11] * m._m[13],
283			_m[8] * m._m[2] + _m[9] * m._m[6] +
284				_m[10] * m._m[10] + _m[11] * m._m[14],
285			_m[8] * m._m[3] + _m[9] * m._m[7] +
286				_m[10] * m._m[11] + _m[11] * m._m[15]
287		,
288			_m[12] * m._m[0] + _m[13] * m._m[4] +
289				_m[14] * m._m[8] + _m[15] * m._m[12],
290			_m[12] * m._m[1] + _m[13] * m._m[5] +
291				_m[14] * m._m[9] + _m[15] * m._m[13],
292			_m[12] * m._m[2] + _m[13] * m._m[6] +
293				_m[14] * m._m[10] + _m[15] * m._m[14],
294			_m[12] * m._m[3] + _m[13] * m._m[7] +
295				_m[14] * m._m[11] + _m[15] * m._m[15]
296		);
297	}
298};
299
300class TranslationMatrix : public Matrix {
301protected:
302	float _dx, _dy, _dz;
303public:
304	TranslationMatrix(float dx = 0.0f, float dy = 0.0f, float dz = 0.0f) :
305		Matrix(
306			1.0f, 0.0f, 0.0f, 0.0f,
307			0.0f, 1.0f, 0.0f, 0.0f,
308			0.0f, 0.0f, 1.0f, 0.0f,
309			dz, dy, dz, 1.0f
310		), _dx(dx), _dy(dy), _dz(dz) {}
311
312	TranslationMatrix inverted() const {
313		return TranslationMatrix(-_dx, -_dy, -_dz);
314	}
315
316	Vector transform(const Vector& v) const {
317		return v + Vector(_dx, _dy, _dz);
318	}
319
320	TranslationMatrix operator*(const TranslationMatrix& m) const {
321		return TranslationMatrix(_dx + m._dx, _dy + m._dy, _dz + m._dz);
322	}
323};
324
325class ScalingMatrix : public Matrix {
326protected:
327	float _sx, _sy, _sz;
328public:
329	ScalingMatrix(float sx = 0.0f, float sy = 0.0f, float sz = 0.0f) :
330		Matrix(
331			sx, 0.0f, 0.0f, 0.0f,
332			0.0f, sy, 0.0f, 0.0f,
333			0.0f, 0.0f, sz, 0.0f,
334			0.0f, 0.0f, 0.0f, 1.0f
335		), _sx(sx), _sy(sy), _sz(sz) {}
336
337	ScalingMatrix inverted() const {
338		return ScalingMatrix(-_sx, -_sy, -_sz);
339	}
340
341	ScalingMatrix transposed() const {
342		return ScalingMatrix(*this);
343	}
344
345	Vector transform(const Vector& v) const {
346		return Vector(_sx * v.x(), _sy * v.y(), _sz * v.z());
347	}
348
349	ScalingMatrix operator*(const ScalingMatrix& m) const {
350		return ScalingMatrix(_sx + m._sx, _sy + m._sy, _sz + m._sz);
351	}
352};
353
354class ShearingMatrix : public Matrix {
355protected:
356	float _sxy, _sxz, _syx, _syz, _szx, _szy;
357public:
358	ShearingMatrix(
359		float sxy = 0.0f, float sxz = 0.0f,
360		float syx = 0.0f, float syz = 0.0f,
361		float szx = 0.0f, float szy = 0.0f
362	) :
363		Matrix(
364			1.0f, sxy, sxz, 0.0f,
365			syx, 1.0f, syz, 0.0f,
366			szx, szy, 1.0f, 0.0f,
367			0.0f, 0.0f, 0.0f, 1.0f
368		),
369		_sxy(sxy), _sxz(sxz), _syx(syx),
370		_syz(syz), _szx(szx), _szy(szy) {}
371
372	ShearingMatrix transposed() const {
373		return ShearingMatrix(_syx, _szx, _sxy, _szy, _sxz, _syz);
374	}
375
376	Vector transform(const Vector& v) const {
377		return Vector(
378			v.x() + _syx * v.y() + _szx * v.z(),
379			v.y() + _sxy * v.x() + _szy * v.z(),
380			v.z() + _sxz * v.x() + _syz * v.y()
381		);
382	}
383};
384
385class RotationMatrix : public Matrix {
386protected:
387	RotationMatrix(
388		float m0, float m1, float m2,
389		float m4, float m5, float m6,
390		float m8, float m9, float m10
391	) : Matrix(
392		m0, m1, m2, 0.0f,
393		m4, m5, m6, 0.0f,
394		m8, m9, m10, 0.0f,
395		0.0f, 0.0f, 0.0f, 1.0f
396	) {}
397
398	RotationMatrix(const Vector& x, const Vector& y, const Vector& z) :
399		Matrix(
400			x.x(), x.y(), x.z(), 0.0f,
401			y.x(), y.y(), y.z(), 0.0f,
402			z.x(), z.y(), z.z(), 0.0f,
403			0.0f, 0.0f, 0.0f, 1.0f
404		) {}
405
406	friend class UnitQuat;
407public:
408	RotationMatrix() :
409		Matrix(
410			1.0f, 0.0f, 0.0f, 0.0f,
411			0.0f, 1.0f, 0.0f, 0.0f,
412			0.0f, 0.0f, 1.0f, 0.0f,
413			0.0f, 0.0f, 0.0f, 1.0f
414		) {}
415
416	// Emulate gluLookAt
417	static RotationMatrix lookAt(
418		const Vector& eye, const Vector& center, const Vector& up
419	) {
420		Vector f(eye - center);
421		f.normalize();
422		Vector x(Vector::cross(up, f));
423		x.normalize();
424		return RotationMatrix(x, Vector::cross(f, x), f);
425	}
426
427	static RotationMatrix heading(float a) {
428		float sa = std::sin(a);
429		float ca = std::cos(a);
430
431		return RotationMatrix(
432			1.0f, 0.0f, 0.0f,
433			0.0f, ca, -sa,
434			0.0f, sa, ca
435		);
436	}
437
438	static RotationMatrix pitch(float a) {
439		float sa = std::sin(a);
440		float ca = std::cos(a);
441
442		return RotationMatrix(
443			ca, 0.0f, -sa,
444			0.0f, 1.0f, 0.0f,
445			sa, 0.0f, ca
446		);
447	}
448
449	static RotationMatrix roll(float a) {
450		float sa = std::sin(a);
451		float ca = std::cos(a);
452
453		return RotationMatrix(
454			ca, -sa, 0.0f,
455			sa, ca, 0.0f,
456			0.0f, 0.0f, 1.0f
457		);
458	}
459
460	RotationMatrix inverted() const {
461		return RotationMatrix(
462			_m[0], _m[4], _m[8],
463			_m[1], _m[5], _m[9],
464			_m[2], _m[6], _m[10]
465		);
466	}
467
468	RotationMatrix transposed() const {
469		return RotationMatrix(
470			_m[0], _m[4], _m[8],
471			_m[1], _m[5], _m[9],
472			_m[2], _m[6], _m[10]
473		);
474	}
475
476	Vector transform(const Vector& v) const {
477		return Vector(
478			_m[0] * v.x() + _m[4] * v.y() + _m[8] * v.z(),
479			_m[1] * v.x() + _m[5] * v.y() + _m[9] * v.z(),
480			_m[2] * v.x() + _m[6] * v.y() + _m[10] * v.z()
481		);
482	}
483
484	UnitVector transform(const UnitVector& v) const {
485		return UnitVector(
486			_m[0] * v.x() + _m[4] * v.y() + _m[8] * v.z(),
487			_m[1] * v.x() + _m[5] * v.y() + _m[9] * v.z(),
488			_m[2] * v.x() + _m[6] * v.y() + _m[10] * v.z()
489		);
490	}
491
492	RotationMatrix operator*(const RotationMatrix& m) const {
493		return RotationMatrix(
494			_m[0] * m._m[0] + _m[1] * m._m[4] + _m[2] * m._m[8],
495			_m[0] * m._m[1] + _m[1] * m._m[5] + _m[2] * m._m[9],
496			_m[0] * m._m[2] + _m[1] * m._m[6] + _m[2] * m._m[10]
497		,
498			_m[4] * m._m[0] + _m[5] * m._m[4] + _m[6] * m._m[8],
499			_m[4] * m._m[1] + _m[5] * m._m[5] + _m[6] * m._m[9],
500			_m[4] * m._m[2] + _m[5] * m._m[6] + _m[6] * m._m[10]
501		,
502			_m[8] * m._m[0] + _m[9] * m._m[4] + _m[10] * m._m[8],
503			_m[8] * m._m[1] + _m[9] * m._m[5] + _m[10] * m._m[9],
504			_m[8] * m._m[2] + _m[9] * m._m[6] + _m[10] * m._m[10]
505		);
506	}
507};
508
509class UnitQuat {
510protected:
511	float _q[4];
512	unsigned int _renormalizeCount;
513
514	// These two are internal, since they do not guarantee length == 1
515	UnitQuat(float x, float y, float z, float w) : _renormalizeCount(0) {
516		_q[0] = x;
517		_q[1] = y;
518		_q[2] = z;
519		_q[3] = w;
520	}
521
522	void set(float x, float y, float z, float w) {
523		_q[0] = x;
524		_q[1] = y;
525		_q[2] = z;
526		_q[3] = w;
527	}
528public:
529	UnitQuat() : _renormalizeCount(0) {
530		_q[0] = _q[1] = _q[2] = 0.0f;
531		_q[3] = 1.0f;
532	}
533
534	UnitQuat(float a, const UnitVector& axis) : _renormalizeCount(0) {
535		a *= 0.5f;
536		float sinA = std::sin(a);
537		set(
538			sinA * axis.x(),
539			sinA * axis.y(),
540			sinA * axis.z(),
541			std::cos(a)
542		);
543	}
544
545	UnitQuat(const RotationMatrix& r) : _renormalizeCount(0) {
546		float trace = r.get(0) + r.get(5) + r.get(10) + 1.0f;
547		if (trace > 0) {
548			float s = 2.0f * std::sqrt(trace);
549			set(
550				(r.get(9) - r.get(6)) / s,
551				(r.get(2) - r.get(8)) / s,
552				(r.get(4) - r.get(1)) / s,
553				s / 4.0f
554			);
555		} else {
556			if (r.get(0) > r.get(5)) {
557				if (r.get(0) > r.get(10)) {
558					// r.get(0) is largest
559					float s = 2.0f *
560						std::sqrt(1.0f + r.get(0) - r.get(5) - r.get(10));
561					set(
562						s / 4.0f,
563						(r.get(1) + r.get(4)) / s,
564						(r.get(2) + r.get(8)) / s,
565						(r.get(9) - r.get(6)) / s
566					);
567				} else {
568					// r.get(10) is largest
569					float s = 2.0f *
570						std::sqrt(1.0f + r.get(10) - r.get(0) - r.get(5));
571					set(
572						(r.get(2) + r.get(8)) / s,
573						(r.get(6) + r.get(9)) / s,
574						s / 4,
575						(r.get(4) - r.get(1)) / s
576					);
577				}
578			} else {
579				if (r.get(5) > r.get(10)) {
580					// r.get(5) is largest
581					float s = 2.0f *
582						std::sqrt(1.0f + r.get(5) - r.get(0) - r.get(10));
583					set(
584						(r.get(1) + r.get(4)) / s,
585						s / 4,
586						(r.get(6) + r.get(9)) / s,
587						(r.get(2) - r.get(8)) / s
588					);
589				} else {
590					// r.get(10) is largest
591					float s = 2.0f *
592						std::sqrt(1.0f + r.get(10) - r.get(0) - r.get(5));
593					set(
594						(r.get(2) + r.get(8)) / s,
595						(r.get(6) + r.get(9)) / s,
596						s / 4,
597						(r.get(4) - r.get(1)) / s
598					);
599				}
600			}
601		}
602	}
603
604	operator RotationMatrix() const {
605		float wx = _q[3] * _q[0] * 2.0f;
606		float wy = _q[3] * _q[1] * 2.0f;
607		float wz = _q[3] * _q[2] * 2.0f;
608		float xx = _q[0] * _q[0] * 2.0f;
609		float xy = _q[0] * _q[1] * 2.0f;
610		float xz = _q[0] * _q[2] * 2.0f;
611		float yy = _q[1] * _q[1] * 2.0f;
612		float yz = _q[1] * _q[2] * 2.0f;
613		float zz = _q[2] * _q[2] * 2.0f;
614
615		return RotationMatrix(
616			1.0f - yy - zz, xy + wz, xz - wy,
617			xy - wz, 1.0f - xx - zz, yz + wx,
618			xz + wy, yz - wx, 1.0f - xx - yy
619		);
620	}
621
622	void toAngleAxis(float* a, UnitVector* axis) const {
623		*a = 2.0f * std::acos(_q[3]);
624		float sinA = std::sqrt(1.0f - _q[3] * _q[3]);
625		if (std::abs(sinA) < 0.00001f)
626			axis->set(1.0f, 0.0f, 0.0f);
627		else
628			axis->set(
629				_q[0] / sinA,
630				_q[1] / sinA,
631				_q[2] / sinA
632			);
633	}
634
635	UnitVector forward() const {
636		float wx = _q[3] * _q[0] * 2.0f;
637		float wy = _q[3] * _q[1] * 2.0f;
638		float xx = _q[0] * _q[0] * 2.0f;
639		float xz = _q[0] * _q[2] * 2.0f;
640		float yy = _q[1] * _q[1] * 2.0f;
641		float yz = _q[1] * _q[2] * 2.0f;
642
643		return UnitVector(-xz - wy, -yz + wx, -1.0f + xx + yy);
644	}
645
646	UnitVector backward() const {
647		float wx = _q[3] * _q[0] * 2.0f;
648		float wy = _q[3] * _q[1] * 2.0f;
649		float xx = _q[0] * _q[0] * 2.0f;
650		float xz = _q[0] * _q[2] * 2.0f;
651		float yy = _q[1] * _q[1] * 2.0f;
652		float yz = _q[1] * _q[2] * 2.0f;
653
654		return UnitVector(xz + wy, yz - wx, 1.0f - xx - yy);
655	}
656
657	UnitVector left() const {
658		float wy = _q[3] * _q[1] * 2.0f;
659		float wz = _q[3] * _q[2] * 2.0f;
660		float xy = _q[0] * _q[1] * 2.0f;
661		float xz = _q[0] * _q[2] * 2.0f;
662		float yy = _q[1] * _q[1] * 2.0f;
663		float zz = _q[2] * _q[2] * 2.0f;
664
665		return UnitVector(-1.0f + yy + zz, -xy - wz, -xz + wy);
666	}
667
668	UnitVector right() const {
669		float wy = _q[3] * _q[1] * 2.0f;
670		float wz = _q[3] * _q[2] * 2.0f;
671		float xy = _q[0] * _q[1] * 2.0f;
672		float xz = _q[0] * _q[2] * 2.0f;
673		float yy = _q[1] * _q[1] * 2.0f;
674		float zz = _q[2] * _q[2] * 2.0f;
675
676		return UnitVector(1.0f - yy - zz, xy + wz, xz - wy);
677	}
678
679	UnitVector up() const {
680		float wx = _q[3] * _q[0] * 2.0f;
681		float wz = _q[3] * _q[2] * 2.0f;
682		float xx = _q[0] * _q[0] * 2.0f;
683		float xy = _q[0] * _q[1] * 2.0f;
684		float yz = _q[1] * _q[2] * 2.0f;
685		float zz = _q[2] * _q[2] * 2.0f;
686
687		return UnitVector(xy - wz, 1.0f - xx - zz, yz + wx);
688	}
689
690	UnitVector down() const {
691		float wx = _q[3] * _q[0] * 2.0f;
692		float wz = _q[3] * _q[2] * 2.0f;
693		float xx = _q[0] * _q[0] * 2.0f;
694		float xy = _q[0] * _q[1] * 2.0f;
695		float yz = _q[1] * _q[2] * 2.0f;
696		float zz = _q[2] * _q[2] * 2.0f;
697
698		return UnitVector(-xy + wz, -1.0f + xx + zz, -yz + wx);
699	}
700
701	float getX() const {
702		return _q[0];
703	}
704
705	float getY() const {
706		return _q[1];
707	}
708
709	float getZ() const {
710		return _q[2];
711	}
712
713	float getW() const {
714		return _q[3];
715	}
716
717	void multiplyBy(const UnitQuat& q) {
718		float tempX = _q[0];
719		float tempY = _q[1];
720		float tempZ = _q[2];
721		float tempW = _q[3];
722
723		_q[0] = tempW * q._q[0] + q._q[3] * tempX
724			+ tempY * q._q[2] - q._q[1] * tempZ;
725		_q[1] = tempW * q._q[1] + q._q[3] * tempY
726			+ tempZ * q._q[0] - q._q[2] * tempX;
727		_q[2] = tempW * q._q[2] + q._q[3] * tempZ
728			+ tempX * q._q[1] - q._q[0] * tempY;
729		_q[3] = tempW * q._q[3]
730			- tempX * q._q[0]
731			- tempY * q._q[1]
732			- tempZ * q._q[2];
733
734		if (++_renormalizeCount == 5) {
735			float length = std::sqrt(
736				_q[0] * _q[0] +
737				_q[1] * _q[1] +
738				_q[2] * _q[2] +
739				_q[3] * _q[3]
740			);
741			_q[0] /= length;
742			_q[1] /= length;
743			_q[2] /= length;
744			_q[3] /= length;
745			_renormalizeCount = 0;
746		}
747	}
748
749	void preMultiplyBy(const UnitQuat& q) {
750		float tempX = _q[0];
751		float tempY = _q[1];
752		float tempZ = _q[2];
753		float tempW = _q[3];
754
755		_q[0] = q._q[3] * tempX + tempW * q._q[0]
756			+ q._q[1] * tempZ - tempY * q._q[2];
757		_q[1] = q._q[3] * tempY + tempW * q._q[1]
758			+ q._q[2] * tempX - tempZ * q._q[0];
759		_q[2] = q._q[3] * tempZ + tempW * q._q[2]
760			+ q._q[0] * tempY - tempX * q._q[1];
761		_q[3] = q._q[3] * tempW
762			- q._q[0] * tempX
763			- q._q[1] * tempY
764			- q._q[2] * tempZ;
765
766		if (++_renormalizeCount == 5) {
767			float length = std::sqrt(
768				_q[0] * _q[0] +
769				_q[1] * _q[1] +
770				_q[2] * _q[2] +
771				_q[3] * _q[3]
772			);
773			_q[0] /= length;
774			_q[1] /= length;
775			_q[2] /= length;
776			_q[3] /= length;
777			_renormalizeCount = 0;
778		}
779	}
780
781	static UnitQuat pitch(float a) {
782		a *= 0.5f;
783		return UnitQuat(std::sin(a), 0.0f, 0.0f, std::cos(a));
784	}
785
786	static UnitQuat heading(float a) {
787		a *= 0.5f;
788		return UnitQuat(0.0f, std::sin(a), 0.0f, std::cos(a));
789	}
790
791	static UnitQuat roll(float a) {
792		a *= 0.5f;
793		return UnitQuat(0.0f, 0.0f, std::sin(a), std::cos(a));
794	}
795};
796
797#endif // _VECTOR_HH