Line data Source code
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 0 : quaternion() : X(0.0f), Y(0.0f), Z(0.0f), W(1.0f) {}
32 :
33 : //! Constructor
34 0 : 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 0 : inline quaternion::quaternion(const vector3df& vec)
195 : {
196 0 : set(vec.X,vec.Y,vec.Z);
197 0 : }
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 0 : inline quaternion& quaternion::operator=(const quaternion& other)
224 : {
225 0 : X = other.X;
226 0 : Y = other.Y;
227 0 : Z = other.Z;
228 0 : W = other.W;
229 0 : 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 0 : inline quaternion quaternion::operator*(f32 s) const
309 : {
310 0 : return quaternion(s*X, s*Y, s*Z, s*W);
311 : }
312 :
313 :
314 : // multiplication operator
315 0 : inline quaternion& quaternion::operator*=(f32 s)
316 : {
317 0 : X*=s;
318 0 : Y*=s;
319 0 : Z*=s;
320 0 : W*=s;
321 0 : 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 0 : inline quaternion quaternion::operator+(const quaternion& b) const
332 : {
333 0 : 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 0 : inline quaternion& quaternion::set(f32 x, f32 y, f32 z)
458 : {
459 : f64 angle;
460 :
461 0 : angle = x * 0.5;
462 0 : const f64 sr = sin(angle);
463 0 : const f64 cr = cos(angle);
464 :
465 0 : angle = y * 0.5;
466 0 : const f64 sp = sin(angle);
467 0 : const f64 cp = cos(angle);
468 :
469 0 : angle = z * 0.5;
470 0 : const f64 sy = sin(angle);
471 0 : const f64 cy = cos(angle);
472 :
473 0 : const f64 cpcy = cp * cy;
474 0 : const f64 spcy = sp * cy;
475 0 : const f64 cpsy = cp * sy;
476 0 : const f64 spsy = sp * sy;
477 :
478 0 : X = (f32)(sr * cpcy - cr * spsy);
479 0 : Y = (f32)(cr * spcy + sr * cpsy);
480 0 : Z = (f32)(cr * cpsy - sr * spcy);
481 0 : W = (f32)(cr * cpcy + sr * spsy);
482 :
483 0 : 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 0 : inline quaternion& quaternion::normalize()
511 : {
512 0 : const f32 n = X*X + Y*Y + Z*Z + W*W;
513 :
514 0 : if (n == 1)
515 0 : return *this;
516 :
517 : //n = 1.0f / sqrtf(n);
518 0 : return (*this *= reciprocal_squareroot ( n ));
519 : }
520 :
521 :
522 : // set this quaternion to the result of the linear interpolation between two quaternions
523 0 : inline quaternion& quaternion::lerp(quaternion q1, quaternion q2, f32 time)
524 : {
525 0 : const f32 scale = 1.0f - time;
526 0 : return (*this = (q1*scale) + (q2*time));
527 : }
528 :
529 :
530 : // set this quaternion to the result of the interpolation between two quaternions
531 0 : inline quaternion& quaternion::slerp(quaternion q1, quaternion q2, f32 time, f32 threshold)
532 : {
533 0 : f32 angle = q1.dotProduct(q2);
534 :
535 : // make sure we use the short rotation
536 0 : if (angle < 0.0f)
537 : {
538 0 : q1 *= -1.0f;
539 0 : angle *= -1.0f;
540 : }
541 :
542 0 : if (angle <= (1-threshold)) // spherical interpolation
543 : {
544 0 : const f32 theta = acosf(angle);
545 0 : const f32 invsintheta = reciprocal(sinf(theta));
546 0 : const f32 scale = sinf(theta * (1.0f-time)) * invsintheta;
547 0 : const f32 invscale = sinf(theta * time) * invsintheta;
548 0 : return (*this = (q1*scale) + (q2*invscale));
549 : }
550 : else // linear interploation
551 0 : return lerp(q1,q2,time);
552 : }
553 :
554 :
555 : // calculates the dot product
556 0 : inline f32 quaternion::dotProduct(const quaternion& q2) const
557 : {
558 0 : 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 0 : inline void quaternion::toEuler(vector3df& euler) const
597 : {
598 0 : const f64 sqw = W*W;
599 0 : const f64 sqx = X*X;
600 0 : const f64 sqy = Y*Y;
601 0 : const f64 sqz = Z*Z;
602 0 : const f64 test = 2.0 * (Y*W - X*Z);
603 :
604 0 : if (core::equals(test, 1.0, 0.000001))
605 : {
606 : // heading = rotation about z-axis
607 0 : euler.Z = (f32) (-2.0*atan2(X, W));
608 : // bank = rotation about x-axis
609 0 : euler.X = 0;
610 : // attitude = rotation about y-axis
611 0 : euler.Y = (f32) (core::PI64/2.0);
612 : }
613 0 : else if (core::equals(test, -1.0, 0.000001))
614 : {
615 : // heading = rotation about z-axis
616 0 : euler.Z = (f32) (2.0*atan2(X, W));
617 : // bank = rotation about x-axis
618 0 : euler.X = 0;
619 : // attitude = rotation about y-axis
620 0 : euler.Y = (f32) (core::PI64/-2.0);
621 : }
622 : else
623 : {
624 : // heading = rotation about z-axis
625 0 : euler.Z = (f32) atan2(2.0 * (X*Y +Z*W),(sqx - sqy - sqz + sqw));
626 : // bank = rotation about x-axis
627 0 : euler.X = (f32) atan2(2.0 * (Y*Z +X*W),(-sqx - sqy + sqz + sqw));
628 : // attitude = rotation about y-axis
629 0 : euler.Y = (f32) asin( clamp(test, -1.0, 1.0) );
630 : }
631 0 : }
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 :
|
