diff options
Diffstat (limited to '')
-rw-r--r-- | libraries/irrlicht-1.8.1/include/quaternion.h | 696 |
1 files changed, 696 insertions, 0 deletions
diff --git a/libraries/irrlicht-1.8.1/include/quaternion.h b/libraries/irrlicht-1.8.1/include/quaternion.h new file mode 100644 index 0000000..f2e3921 --- /dev/null +++ b/libraries/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 | |||
18 | namespace irr | ||
19 | { | ||
20 | namespace core | ||
21 | { | ||
22 | |||
23 | //! Quaternion class for representing rotations. | ||
24 | /** It provides cheap combinations and avoids gimbal locks. | ||
25 | Also useful for interpolations. */ | ||
26 | class 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 ¢er, 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 | ||
187 | inline 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 | ||
194 | inline 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 | ||
201 | inline quaternion::quaternion(const matrix4& mat) | ||
202 | { | ||
203 | (*this) = mat; | ||
204 | } | ||
205 | #endif | ||
206 | |||
207 | // equal operator | ||
208 | inline 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 | ||
217 | inline bool quaternion::operator!=(const quaternion& other) const | ||
218 | { | ||
219 | return !(*this == other); | ||
220 | } | ||
221 | |||
222 | // assignment operator | ||
223 | inline 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 | ||
234 | inline 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 | ||
294 | inline 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 | ||
308 | inline quaternion quaternion::operator*(f32 s) const | ||
309 | { | ||
310 | return quaternion(s*X, s*Y, s*Z, s*W); | ||
311 | } | ||
312 | |||
313 | |||
314 | // multiplication operator | ||
315 | inline 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 | ||
325 | inline quaternion& quaternion::operator*=(const quaternion& other) | ||
326 | { | ||
327 | return (*this = other * (*this)); | ||
328 | } | ||
329 | |||
330 | // add operator | ||
331 | inline 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 | ||
338 | inline 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 | */ | ||
349 | inline void quaternion::getMatrix(matrix4 &dest, | ||
350 | const core::vector3df ¢er) 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 | */ | ||
388 | inline void quaternion::getMatrixCenter(matrix4 &dest, | ||
389 | const core::vector3df ¢er, | ||
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 | ||
411 | inline 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 | ||
438 | inline quaternion& quaternion::makeInverse() | ||
439 | { | ||
440 | X = -X; Y = -Y; Z = -Z; | ||
441 | return *this; | ||
442 | } | ||
443 | |||
444 | |||
445 | // sets new quaternion | ||
446 | inline 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 | ||
457 | inline 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 | ||
487 | inline 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 | ||
493 | inline 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 | ||
500 | inline 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 | ||
510 | inline 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 | ||
523 | inline 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 | ||
531 | inline 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 | ||
556 | inline 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 | ||
563 | inline 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 | |||
575 | inline 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 | |||
596 | inline 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 | |||
634 | inline 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 | ||
649 | inline 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 | |||
658 | inline 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 | |||