5 #ifndef __IRR_QUATERNION_H_INCLUDED__
6 #define __IRR_QUATERNION_H_INCLUDED__
16 #define IRR_TEST_BROKEN_QUATERNION_USE 0
42 #if !IRR_TEST_BROKEN_QUATERNION_USE
56 #if !IRR_TEST_BROKEN_QUATERNION_USE
101 #if !IRR_TEST_BROKEN_QUATERNION_USE
156 f32 time,
f32 threshold=.05f);
196 set(vec.
X,vec.
Y,vec.
Z);
199 #if !IRR_TEST_BROKEN_QUATERNION_USE
210 return ((
X == other.
X) &&
219 return !(*
this == other);
232 #if !IRR_TEST_BROKEN_QUATERNION_USE
236 const f32 diag = m[0] + m[5] + m[10] + 1;
240 const f32 scale = sqrtf(diag) * 2.0f;
243 X = (m[6] - m[9]) / scale;
244 Y = (m[8] - m[2]) / scale;
245 Z = (m[1] - m[4]) / scale;
250 if (m[0]>m[5] && m[0]>m[10])
254 const f32 scale = sqrtf(1.0f + m[0] - m[5] - m[10]) * 2.0f;
258 Y = (m[4] + m[1]) / scale;
259 Z = (m[2] + m[8]) / scale;
260 W = (m[6] - m[9]) / scale;
266 const f32 scale = sqrtf(1.0f + m[5] - m[0] - m[10]) * 2.0f;
269 X = (m[4] + m[1]) / scale;
271 Z = (m[9] + m[6]) / scale;
272 W = (m[8] - m[2]) / scale;
278 const f32 scale = sqrtf(1.0f + m[10] - m[0] - m[5]) * 2.0f;
281 X = (m[8] + m[2]) / scale;
282 Y = (m[9] + m[6]) / scale;
284 W = (m[1] - m[4]) / scale;
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);
327 return (*
this = other * (*
this));
336 #if !IRR_TEST_BROKEN_QUATERNION_USE
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;
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;
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;
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;
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;
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;
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;
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;
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;
462 const f64 sr = sin(angle);
463 const f64 cr = cos(angle);
466 const f64 sp = sin(angle);
467 const f64 cp = cos(angle);
470 const f64 sy = sin(angle);
471 const f64 cy = cos(angle);
473 const f64 cpcy = cp * cy;
474 const f64 spcy = sp * cy;
475 const f64 cpsy = cp * sy;
476 const f64 spsy = sp * sy;
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);
489 return set(vec.
X, vec.
Y, vec.
Z);
525 const f32 scale = 1.0f - time;
526 return (*
this = (q1*scale) + (q2*time));
542 if (angle <= (1-threshold))
544 const f32 theta = acosf(angle);
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));
551 return lerp(q1,q2,time);
558 return (
X * q2.
X) + (
Y * q2.
Y) + (
Z * q2.
Z) + (
W * q2.
W);
565 const f32 fHalfAngle = 0.5f*angle;
566 const f32 fSin = sinf(fHalfAngle);
567 W = cosf(fHalfAngle);
577 const f32 scale = sqrtf(
X*
X +
Y*
Y +
Z*
Z);
589 angle = 2.0f * acosf(
W);
590 axis.
X =
X * invscale;
591 axis.
Y =
Y * invscale;
592 axis.
Z = Z * invscale;
602 const f64 test = 2.0 * (Y*W - X*
Z);
607 euler.
Z = (
f32) (-2.0*atan2(X, W));
616 euler.
Z = (
f32) (2.0*atan2(X, W));
625 euler.
Z = (
f32) atan2(2.0 * (X*Y +Z*W),(sqx - sqy - sqz + sqw));
627 euler.
X = (
f32) atan2(2.0 * (Y*Z +X*W),(-sqx - sqy + sqz + sqw));
629 euler.
Y = (
f32) asin(
clamp(test, -1.0, 1.0) );
678 axis.
set(0.f,1.f,0.f);
685 const f32 s = sqrtf( (1+d)*2 );
686 const f32 invs = 1.f / s;