aboutsummaryrefslogtreecommitdiffstatshomepage
path: root/src/others/irrlicht-1.8.1/include/quaternion.h
diff options
context:
space:
mode:
Diffstat (limited to 'src/others/irrlicht-1.8.1/include/quaternion.h')
-rw-r--r--src/others/irrlicht-1.8.1/include/quaternion.h696
1 files changed, 696 insertions, 0 deletions
diff --git a/src/others/irrlicht-1.8.1/include/quaternion.h b/src/others/irrlicht-1.8.1/include/quaternion.h
new file mode 100644
index 0000000..f2e3921
--- /dev/null
+++ b/src/others/irrlicht-1.8.1/include/quaternion.h
@@ -0,0 +1,696 @@
1// Copyright (C) 2002-2012 Nikolaus Gebhardt
2// This file is part of the "Irrlicht Engine".
3// For conditions of distribution and use, see copyright notice in irrlicht.h
4
5#ifndef __IRR_QUATERNION_H_INCLUDED__
6#define __IRR_QUATERNION_H_INCLUDED__
7
8#include "irrTypes.h"
9#include "irrMath.h"
10#include "matrix4.h"
11#include "vector3d.h"
12
13// Between Irrlicht 1.7 and Irrlicht 1.8 the quaternion-matrix conversions got fixed.
14// This define disables all involved functions completely to allow finding all places
15// where the wrong conversions had been in use.
16#define IRR_TEST_BROKEN_QUATERNION_USE 0
17
18namespace irr
19{
20namespace core
21{
22
23//! Quaternion class for representing rotations.
24/** It provides cheap combinations and avoids gimbal locks.
25Also useful for interpolations. */
26class quaternion
27{
28 public:
29
30 //! Default Constructor
31 quaternion() : X(0.0f), Y(0.0f), Z(0.0f), W(1.0f) {}
32
33 //! Constructor
34 quaternion(f32 x, f32 y, f32 z, f32 w) : X(x), Y(y), Z(z), W(w) { }
35
36 //! Constructor which converts euler angles (radians) to a quaternion
37 quaternion(f32 x, f32 y, f32 z);
38
39 //! Constructor which converts euler angles (radians) to a quaternion
40 quaternion(const vector3df& vec);
41
42#if !IRR_TEST_BROKEN_QUATERNION_USE
43 //! Constructor which converts a matrix to a quaternion
44 quaternion(const matrix4& mat);
45#endif
46
47 //! Equalilty operator
48 bool operator==(const quaternion& other) const;
49
50 //! inequality operator
51 bool operator!=(const quaternion& other) const;
52
53 //! Assignment operator
54 inline quaternion& operator=(const quaternion& other);
55
56#if !IRR_TEST_BROKEN_QUATERNION_USE
57 //! Matrix assignment operator
58 inline quaternion& operator=(const matrix4& other);
59#endif
60
61 //! Add operator
62 quaternion operator+(const quaternion& other) const;
63
64 //! Multiplication operator
65 quaternion operator*(const quaternion& other) const;
66
67 //! Multiplication operator with scalar
68 quaternion operator*(f32 s) const;
69
70 //! Multiplication operator with scalar
71 quaternion& operator*=(f32 s);
72
73 //! Multiplication operator
74 vector3df operator*(const vector3df& v) const;
75
76 //! Multiplication operator
77 quaternion& operator*=(const quaternion& other);
78
79 //! Calculates the dot product
80 inline f32 dotProduct(const quaternion& other) const;
81
82 //! Sets new quaternion
83 inline quaternion& set(f32 x, f32 y, f32 z, f32 w);
84
85 //! Sets new quaternion based on euler angles (radians)
86 inline quaternion& set(f32 x, f32 y, f32 z);
87
88 //! Sets new quaternion based on euler angles (radians)
89 inline quaternion& set(const core::vector3df& vec);
90
91 //! Sets new quaternion from other quaternion
92 inline quaternion& set(const core::quaternion& quat);
93
94 //! returns if this quaternion equals the other one, taking floating point rounding errors into account
95 inline bool equals(const quaternion& other,
96 const f32 tolerance = ROUNDING_ERROR_f32 ) const;
97
98 //! Normalizes the quaternion
99 inline quaternion& normalize();
100
101#if !IRR_TEST_BROKEN_QUATERNION_USE
102 //! Creates a matrix from this quaternion
103 matrix4 getMatrix() const;
104#endif
105
106 //! Creates a matrix from this quaternion
107 void getMatrix( matrix4 &dest, const core::vector3df &translation=core::vector3df() ) const;
108
109 /*!
110 Creates a matrix from this quaternion
111 Rotate about a center point
112 shortcut for
113 core::quaternion q;
114 q.rotationFromTo ( vin[i].Normal, forward );
115 q.getMatrixCenter ( lookat, center, newPos );
116
117 core::matrix4 m2;
118 m2.setInverseTranslation ( center );
119 lookat *= m2;
120
121 core::matrix4 m3;
122 m2.setTranslation ( newPos );
123 lookat *= m3;
124
125 */
126 void getMatrixCenter( matrix4 &dest, const core::vector3df &center, const core::vector3df &translation ) const;
127
128 //! Creates a matrix from this quaternion
129 inline void getMatrix_transposed( matrix4 &dest ) const;
130
131 //! Inverts this quaternion
132 quaternion& makeInverse();
133
134 //! Set this quaternion to the linear interpolation between two quaternions
135 /** \param q1 First quaternion to be interpolated.
136 \param q2 Second quaternion to be interpolated.
137 \param time Progress of interpolation. For time=0 the result is
138 q1, for time=1 the result is q2. Otherwise interpolation
139 between q1 and q2.
140 */
141 quaternion& lerp(quaternion q1, quaternion q2, f32 time);
142
143 //! Set this quaternion to the result of the spherical interpolation between two quaternions
144 /** \param q1 First quaternion to be interpolated.
145 \param q2 Second quaternion to be interpolated.
146 \param time Progress of interpolation. For time=0 the result is
147 q1, for time=1 the result is q2. Otherwise interpolation
148 between q1 and q2.
149 \param threshold To avoid inaccuracies at the end (time=1) the
150 interpolation switches to linear interpolation at some point.
151 This value defines how much of the remaining interpolation will
152 be calculated with lerp. Everything from 1-threshold up will be
153 linear interpolation.
154 */
155 quaternion& slerp(quaternion q1, quaternion q2,
156 f32 time, f32 threshold=.05f);
157
158 //! Create quaternion from rotation angle and rotation axis.
159 /** Axis must be unit length.
160 The quaternion representing the rotation is
161 q = cos(A/2)+sin(A/2)*(x*i+y*j+z*k).
162 \param angle Rotation Angle in radians.
163 \param axis Rotation axis. */
164 quaternion& fromAngleAxis (f32 angle, const vector3df& axis);
165
166 //! Fills an angle (radians) around an axis (unit vector)
167 void toAngleAxis (f32 &angle, core::vector3df& axis) const;
168
169 //! Output this quaternion to an euler angle (radians)
170 void toEuler(vector3df& euler) const;
171
172 //! Set quaternion to identity
173 quaternion& makeIdentity();
174
175 //! Set quaternion to represent a rotation from one vector to another.
176 quaternion& rotationFromTo(const vector3df& from, const vector3df& to);
177
178 //! Quaternion elements.
179 f32 X; // vectorial (imaginary) part
180 f32 Y;
181 f32 Z;
182 f32 W; // real part
183};
184
185
186// Constructor which converts euler angles to a quaternion
187inline quaternion::quaternion(f32 x, f32 y, f32 z)
188{
189 set(x,y,z);
190}
191
192
193// Constructor which converts euler angles to a quaternion
194inline quaternion::quaternion(const vector3df& vec)
195{
196 set(vec.X,vec.Y,vec.Z);
197}
198
199#if !IRR_TEST_BROKEN_QUATERNION_USE
200// Constructor which converts a matrix to a quaternion
201inline quaternion::quaternion(const matrix4& mat)
202{
203 (*this) = mat;
204}
205#endif
206
207// equal operator
208inline bool quaternion::operator==(const quaternion& other) const
209{
210 return ((X == other.X) &&
211 (Y == other.Y) &&
212 (Z == other.Z) &&
213 (W == other.W));
214}
215
216// inequality operator
217inline bool quaternion::operator!=(const quaternion& other) const
218{
219 return !(*this == other);
220}
221
222// assignment operator
223inline quaternion& quaternion::operator=(const quaternion& other)
224{
225 X = other.X;
226 Y = other.Y;
227 Z = other.Z;
228 W = other.W;
229 return *this;
230}
231
232#if !IRR_TEST_BROKEN_QUATERNION_USE
233// matrix assignment operator
234inline quaternion& quaternion::operator=(const matrix4& m)
235{
236 const f32 diag = m[0] + m[5] + m[10] + 1;
237
238 if( diag > 0.0f )
239 {
240 const f32 scale = sqrtf(diag) * 2.0f; // get scale from diagonal
241
242 // TODO: speed this up
243 X = (m[6] - m[9]) / scale;
244 Y = (m[8] - m[2]) / scale;
245 Z = (m[1] - m[4]) / scale;
246 W = 0.25f * scale;
247 }
248 else
249 {
250 if (m[0]>m[5] && m[0]>m[10])
251 {
252 // 1st element of diag is greatest value
253 // find scale according to 1st element, and double it
254 const f32 scale = sqrtf(1.0f + m[0] - m[5] - m[10]) * 2.0f;
255
256 // TODO: speed this up
257 X = 0.25f * scale;
258 Y = (m[4] + m[1]) / scale;
259 Z = (m[2] + m[8]) / scale;
260 W = (m[6] - m[9]) / scale;
261 }
262 else if (m[5]>m[10])
263 {
264 // 2nd element of diag is greatest value
265 // find scale according to 2nd element, and double it
266 const f32 scale = sqrtf(1.0f + m[5] - m[0] - m[10]) * 2.0f;
267
268 // TODO: speed this up
269 X = (m[4] + m[1]) / scale;
270 Y = 0.25f * scale;
271 Z = (m[9] + m[6]) / scale;
272 W = (m[8] - m[2]) / scale;
273 }
274 else
275 {
276 // 3rd element of diag is greatest value
277 // find scale according to 3rd element, and double it
278 const f32 scale = sqrtf(1.0f + m[10] - m[0] - m[5]) * 2.0f;
279
280 // TODO: speed this up
281 X = (m[8] + m[2]) / scale;
282 Y = (m[9] + m[6]) / scale;
283 Z = 0.25f * scale;
284 W = (m[1] - m[4]) / scale;
285 }
286 }
287
288 return normalize();
289}
290#endif
291
292
293// multiplication operator
294inline quaternion quaternion::operator*(const quaternion& other) const
295{
296 quaternion tmp;
297
298 tmp.W = (other.W * W) - (other.X * X) - (other.Y * Y) - (other.Z * Z);
299 tmp.X = (other.W * X) + (other.X * W) + (other.Y * Z) - (other.Z * Y);
300 tmp.Y = (other.W * Y) + (other.Y * W) + (other.Z * X) - (other.X * Z);
301 tmp.Z = (other.W * Z) + (other.Z * W) + (other.X * Y) - (other.Y * X);
302
303 return tmp;
304}
305
306
307// multiplication operator
308inline quaternion quaternion::operator*(f32 s) const
309{
310 return quaternion(s*X, s*Y, s*Z, s*W);
311}
312
313
314// multiplication operator
315inline quaternion& quaternion::operator*=(f32 s)
316{
317 X*=s;
318 Y*=s;
319 Z*=s;
320 W*=s;
321 return *this;
322}
323
324// multiplication operator
325inline quaternion& quaternion::operator*=(const quaternion& other)
326{
327 return (*this = other * (*this));
328}
329
330// add operator
331inline quaternion quaternion::operator+(const quaternion& b) const
332{
333 return quaternion(X+b.X, Y+b.Y, Z+b.Z, W+b.W);
334}
335
336#if !IRR_TEST_BROKEN_QUATERNION_USE
337// Creates a matrix from this quaternion
338inline matrix4 quaternion::getMatrix() const
339{
340 core::matrix4 m;
341 getMatrix(m);
342 return m;
343}
344#endif
345
346/*!
347 Creates a matrix from this quaternion
348*/
349inline void quaternion::getMatrix(matrix4 &dest,
350 const core::vector3df &center) const
351{
352 dest[0] = 1.0f - 2.0f*Y*Y - 2.0f*Z*Z;
353 dest[1] = 2.0f*X*Y + 2.0f*Z*W;
354 dest[2] = 2.0f*X*Z - 2.0f*Y*W;
355 dest[3] = 0.0f;
356
357 dest[4] = 2.0f*X*Y - 2.0f*Z*W;
358 dest[5] = 1.0f - 2.0f*X*X - 2.0f*Z*Z;
359 dest[6] = 2.0f*Z*Y + 2.0f*X*W;
360 dest[7] = 0.0f;
361
362 dest[8] = 2.0f*X*Z + 2.0f*Y*W;
363 dest[9] = 2.0f*Z*Y - 2.0f*X*W;
364 dest[10] = 1.0f - 2.0f*X*X - 2.0f*Y*Y;
365 dest[11] = 0.0f;
366
367 dest[12] = center.X;
368 dest[13] = center.Y;
369 dest[14] = center.Z;
370 dest[15] = 1.f;
371
372 dest.setDefinitelyIdentityMatrix ( false );
373}
374
375
376/*!
377 Creates a matrix from this quaternion
378 Rotate about a center point
379 shortcut for
380 core::quaternion q;
381 q.rotationFromTo(vin[i].Normal, forward);
382 q.getMatrix(lookat, center);
383
384 core::matrix4 m2;
385 m2.setInverseTranslation(center);
386 lookat *= m2;
387*/
388inline void quaternion::getMatrixCenter(matrix4 &dest,
389 const core::vector3df &center,
390 const core::vector3df &translation) const
391{
392 dest[0] = 1.0f - 2.0f*Y*Y - 2.0f*Z*Z;
393 dest[1] = 2.0f*X*Y + 2.0f*Z*W;
394 dest[2] = 2.0f*X*Z - 2.0f*Y*W;
395 dest[3] = 0.0f;
396
397 dest[4] = 2.0f*X*Y - 2.0f*Z*W;
398 dest[5] = 1.0f - 2.0f*X*X - 2.0f*Z*Z;
399 dest[6] = 2.0f*Z*Y + 2.0f*X*W;
400 dest[7] = 0.0f;
401
402 dest[8] = 2.0f*X*Z + 2.0f*Y*W;
403 dest[9] = 2.0f*Z*Y - 2.0f*X*W;
404 dest[10] = 1.0f - 2.0f*X*X - 2.0f*Y*Y;
405 dest[11] = 0.0f;
406
407 dest.setRotationCenter ( center, translation );
408}
409
410// Creates a matrix from this quaternion
411inline void quaternion::getMatrix_transposed(matrix4 &dest) const
412{
413 dest[0] = 1.0f - 2.0f*Y*Y - 2.0f*Z*Z;
414 dest[4] = 2.0f*X*Y + 2.0f*Z*W;
415 dest[8] = 2.0f*X*Z - 2.0f*Y*W;
416 dest[12] = 0.0f;
417
418 dest[1] = 2.0f*X*Y - 2.0f*Z*W;
419 dest[5] = 1.0f - 2.0f*X*X - 2.0f*Z*Z;
420 dest[9] = 2.0f*Z*Y + 2.0f*X*W;
421 dest[13] = 0.0f;
422
423 dest[2] = 2.0f*X*Z + 2.0f*Y*W;
424 dest[6] = 2.0f*Z*Y - 2.0f*X*W;
425 dest[10] = 1.0f - 2.0f*X*X - 2.0f*Y*Y;
426 dest[14] = 0.0f;
427
428 dest[3] = 0.f;
429 dest[7] = 0.f;
430 dest[11] = 0.f;
431 dest[15] = 1.f;
432
433 dest.setDefinitelyIdentityMatrix(false);
434}
435
436
437// Inverts this quaternion
438inline quaternion& quaternion::makeInverse()
439{
440 X = -X; Y = -Y; Z = -Z;
441 return *this;
442}
443
444
445// sets new quaternion
446inline quaternion& quaternion::set(f32 x, f32 y, f32 z, f32 w)
447{
448 X = x;
449 Y = y;
450 Z = z;
451 W = w;
452 return *this;
453}
454
455
456// sets new quaternion based on euler angles
457inline quaternion& quaternion::set(f32 x, f32 y, f32 z)
458{
459 f64 angle;
460
461 angle = x * 0.5;
462 const f64 sr = sin(angle);
463 const f64 cr = cos(angle);
464
465 angle = y * 0.5;
466 const f64 sp = sin(angle);
467 const f64 cp = cos(angle);
468
469 angle = z * 0.5;
470 const f64 sy = sin(angle);
471 const f64 cy = cos(angle);
472
473 const f64 cpcy = cp * cy;
474 const f64 spcy = sp * cy;
475 const f64 cpsy = cp * sy;
476 const f64 spsy = sp * sy;
477
478 X = (f32)(sr * cpcy - cr * spsy);
479 Y = (f32)(cr * spcy + sr * cpsy);
480 Z = (f32)(cr * cpsy - sr * spcy);
481 W = (f32)(cr * cpcy + sr * spsy);
482
483 return normalize();
484}
485
486// sets new quaternion based on euler angles
487inline quaternion& quaternion::set(const core::vector3df& vec)
488{
489 return set(vec.X, vec.Y, vec.Z);
490}
491
492// sets new quaternion based on other quaternion
493inline quaternion& quaternion::set(const core::quaternion& quat)
494{
495 return (*this=quat);
496}
497
498
499//! returns if this quaternion equals the other one, taking floating point rounding errors into account
500inline bool quaternion::equals(const quaternion& other, const f32 tolerance) const
501{
502 return core::equals(X, other.X, tolerance) &&
503 core::equals(Y, other.Y, tolerance) &&
504 core::equals(Z, other.Z, tolerance) &&
505 core::equals(W, other.W, tolerance);
506}
507
508
509// normalizes the quaternion
510inline quaternion& quaternion::normalize()
511{
512 const f32 n = X*X + Y*Y + Z*Z + W*W;
513
514 if (n == 1)
515 return *this;
516
517 //n = 1.0f / sqrtf(n);
518 return (*this *= reciprocal_squareroot ( n ));
519}
520
521
522// set this quaternion to the result of the linear interpolation between two quaternions
523inline quaternion& quaternion::lerp(quaternion q1, quaternion q2, f32 time)
524{
525 const f32 scale = 1.0f - time;
526 return (*this = (q1*scale) + (q2*time));
527}
528
529
530// set this quaternion to the result of the interpolation between two quaternions
531inline quaternion& quaternion::slerp(quaternion q1, quaternion q2, f32 time, f32 threshold)
532{
533 f32 angle = q1.dotProduct(q2);
534
535 // make sure we use the short rotation
536 if (angle < 0.0f)
537 {
538 q1 *= -1.0f;
539 angle *= -1.0f;
540 }
541
542 if (angle <= (1-threshold)) // spherical interpolation
543 {
544 const f32 theta = acosf(angle);
545 const f32 invsintheta = reciprocal(sinf(theta));
546 const f32 scale = sinf(theta * (1.0f-time)) * invsintheta;
547 const f32 invscale = sinf(theta * time) * invsintheta;
548 return (*this = (q1*scale) + (q2*invscale));
549 }
550 else // linear interploation
551 return lerp(q1,q2,time);
552}
553
554
555// calculates the dot product
556inline f32 quaternion::dotProduct(const quaternion& q2) const
557{
558 return (X * q2.X) + (Y * q2.Y) + (Z * q2.Z) + (W * q2.W);
559}
560
561
562//! axis must be unit length, angle in radians
563inline quaternion& quaternion::fromAngleAxis(f32 angle, const vector3df& axis)
564{
565 const f32 fHalfAngle = 0.5f*angle;
566 const f32 fSin = sinf(fHalfAngle);
567 W = cosf(fHalfAngle);
568 X = fSin*axis.X;
569 Y = fSin*axis.Y;
570 Z = fSin*axis.Z;
571 return *this;
572}
573
574
575inline void quaternion::toAngleAxis(f32 &angle, core::vector3df &axis) const
576{
577 const f32 scale = sqrtf(X*X + Y*Y + Z*Z);
578
579 if (core::iszero(scale) || W > 1.0f || W < -1.0f)
580 {
581 angle = 0.0f;
582 axis.X = 0.0f;
583 axis.Y = 1.0f;
584 axis.Z = 0.0f;
585 }
586 else
587 {
588 const f32 invscale = reciprocal(scale);
589 angle = 2.0f * acosf(W);
590 axis.X = X * invscale;
591 axis.Y = Y * invscale;
592 axis.Z = Z * invscale;
593 }
594}
595
596inline void quaternion::toEuler(vector3df& euler) const
597{
598 const f64 sqw = W*W;
599 const f64 sqx = X*X;
600 const f64 sqy = Y*Y;
601 const f64 sqz = Z*Z;
602 const f64 test = 2.0 * (Y*W - X*Z);
603
604 if (core::equals(test, 1.0, 0.000001))
605 {
606 // heading = rotation about z-axis
607 euler.Z = (f32) (-2.0*atan2(X, W));
608 // bank = rotation about x-axis
609 euler.X = 0;
610 // attitude = rotation about y-axis
611 euler.Y = (f32) (core::PI64/2.0);
612 }
613 else if (core::equals(test, -1.0, 0.000001))
614 {
615 // heading = rotation about z-axis
616 euler.Z = (f32) (2.0*atan2(X, W));
617 // bank = rotation about x-axis
618 euler.X = 0;
619 // attitude = rotation about y-axis
620 euler.Y = (f32) (core::PI64/-2.0);
621 }
622 else
623 {
624 // heading = rotation about z-axis
625 euler.Z = (f32) atan2(2.0 * (X*Y +Z*W),(sqx - sqy - sqz + sqw));
626 // bank = rotation about x-axis
627 euler.X = (f32) atan2(2.0 * (Y*Z +X*W),(-sqx - sqy + sqz + sqw));
628 // attitude = rotation about y-axis
629 euler.Y = (f32) asin( clamp(test, -1.0, 1.0) );
630 }
631}
632
633
634inline vector3df quaternion::operator* (const vector3df& v) const
635{
636 // nVidia SDK implementation
637
638 vector3df uv, uuv;
639 vector3df qvec(X, Y, Z);
640 uv = qvec.crossProduct(v);
641 uuv = qvec.crossProduct(uv);
642 uv *= (2.0f * W);
643 uuv *= 2.0f;
644
645 return v + uv + uuv;
646}
647
648// set quaternion to identity
649inline core::quaternion& quaternion::makeIdentity()
650{
651 W = 1.f;
652 X = 0.f;
653 Y = 0.f;
654 Z = 0.f;
655 return *this;
656}
657
658inline core::quaternion& quaternion::rotationFromTo(const vector3df& from, const vector3df& to)
659{
660 // Based on Stan Melax's article in Game Programming Gems
661 // Copy, since cannot modify local
662 vector3df v0 = from;
663 vector3df v1 = to;
664 v0.normalize();
665 v1.normalize();
666
667 const f32 d = v0.dotProduct(v1);
668 if (d >= 1.0f) // If dot == 1, vectors are the same
669 {
670 return makeIdentity();
671 }
672 else if (d <= -1.0f) // exactly opposite
673 {
674 core::vector3df axis(1.0f, 0.f, 0.f);
675 axis = axis.crossProduct(v0);
676 if (axis.getLength()==0)
677 {
678 axis.set(0.f,1.f,0.f);
679 axis = axis.crossProduct(v0);
680 }
681 // same as fromAngleAxis(core::PI, axis).normalize();
682 return set(axis.X, axis.Y, axis.Z, 0).normalize();
683 }
684
685 const f32 s = sqrtf( (1+d)*2 ); // optimize inv_sqrt
686 const f32 invs = 1.f / s;
687 const vector3df c = v0.crossProduct(v1)*invs;
688 return set(c.X, c.Y, c.Z, s * 0.5f).normalize();
689}
690
691
692} // end namespace core
693} // end namespace irr
694
695#endif
696
generated by cgit v1.1 at 2024-05-15 18:38:36 +0000