5 #ifndef __IRR_QUATERNION_H_INCLUDED__
6 #define __IRR_QUATERNION_H_INCLUDED__
164 set(vec.
X,vec.
Y,vec.
Z);
178 return ((
X == other.
X) &&
187 return !(*
this == other);
204 const f32 diag = m(0,0) + m(1,1) + m(2,2) + 1;
208 const f32 scale = sqrtf(diag) * 2.0f;
211 X = ( m(2,1) - m(1,2)) / scale;
212 Y = ( m(0,2) - m(2,0)) / scale;
213 Z = ( m(1,0) - m(0,1)) / scale;
218 if ( m(0,0) > m(1,1) && m(0,0) > m(2,2))
222 const f32 scale = sqrtf( 1.0f + m(0,0) - m(1,1) - m(2,2)) * 2.0f;
226 Y = (m(0,1) + m(1,0)) / scale;
227 Z = (m(2,0) + m(0,2)) / scale;
228 W = (m(2,1) - m(1,2)) / scale;
230 else if ( m(1,1) > m(2,2))
234 const f32 scale = sqrtf( 1.0f + m(1,1) - m(0,0) - m(2,2)) * 2.0f;
237 X = (m(0,1) + m(1,0) ) / scale;
239 Z = (m(1,2) + m(2,1) ) / scale;
240 W = (m(0,2) - m(2,0) ) / scale;
246 const f32 scale = sqrtf( 1.0f + m(2,2) - m(0,0) - m(1,1)) * 2.0f;
249 X = (m(0,2) + m(2,0)) / scale;
250 Y = (m(1,2) + m(2,1)) / scale;
252 W = (m(1,0) - m(0,1)) / scale;
265 tmp.
W = (other.
W *
W) - (other.
X *
X) - (other.
Y *
Y) - (other.
Z *
Z);
266 tmp.
X = (other.
W *
X) + (other.
X *
W) + (other.
Y *
Z) - (other.
Z *
Y);
267 tmp.
Y = (other.
W *
Y) + (other.
Y *
W) + (other.
Z *
X) - (other.
X *
Z);
268 tmp.
Z = (other.
W *
Z) + (other.
Z *
W) + (other.
X *
Y) - (other.
Y *
X);
293 return (*
this = other * (*
this));
319 m[0] = 1.0f - 2.0f*
Y*
Y - 2.0f*
Z*
Z;
320 m[1] = 2.0f*
X*
Y + 2.0f*Z*
W;
321 m[2] = 2.0f*
X*Z - 2.0f*
Y*
W;
324 m[4] = 2.0f*
X*
Y - 2.0f*Z*
W;
325 m[5] = 1.0f - 2.0f*
X*
X - 2.0f*Z*
Z;
326 m[6] = 2.0f*Z*
Y + 2.0f*
X*
W;
329 m[8] = 2.0f*
X*Z + 2.0f*
Y*
W;
330 m[9] = 2.0f*Z*
Y - 2.0f*
X*
W;
331 m[10] = 1.0f - 2.0f*
X*
X - 2.0f*
Y*
Y;
363 m[0] = 1.0f - 2.0f*
Y*
Y - 2.0f*
Z*
Z;
364 m[1] = 2.0f*
X*
Y + 2.0f*Z*
W;
365 m[2] = 2.0f*
X*Z - 2.0f*
Y*
W;
368 m[4] = 2.0f*
X*
Y - 2.0f*Z*
W;
369 m[5] = 1.0f - 2.0f*
X*
X - 2.0f*Z*
Z;
370 m[6] = 2.0f*Z*
Y + 2.0f*
X*
W;
373 m[8] = 2.0f*
X*Z + 2.0f*
Y*
W;
374 m[9] = 2.0f*Z*
Y - 2.0f*
X*
W;
375 m[10] = 1.0f - 2.0f*
X*
X - 2.0f*
Y*
Y;
384 dest[0] = 1.0f - 2.0f*
Y*
Y - 2.0f*
Z*
Z;
385 dest[4] = 2.0f*
X*
Y + 2.0f*Z*
W;
386 dest[8] = 2.0f*
X*Z - 2.0f*
Y*
W;
389 dest[1] = 2.0f*
X*
Y - 2.0f*Z*
W;
390 dest[5] = 1.0f - 2.0f*
X*
X - 2.0f*Z*
Z;
391 dest[9] = 2.0f*Z*
Y + 2.0f*
X*
W;
394 dest[2] = 2.0f*
X*Z + 2.0f*
Y*
W;
395 dest[6] = 2.0f*Z*
Y - 2.0f*
X*
W;
396 dest[10] = 1.0f - 2.0f*
X*
X - 2.0f*
Y*
Y;
433 const f64 sr = sin(angle);
434 const f64 cr = cos(angle);
437 const f64 sp = sin(angle);
438 const f64 cp = cos(angle);
441 const f64 sy = sin(angle);
442 const f64 cy = cos(angle);
444 const f64 cpcy = cp * cy;
445 const f64 spcy = sp * cy;
446 const f64 cpsy = cp * sy;
447 const f64 spsy = sp * sy;
449 X = (
f32)(sr * cpcy - cr * spsy);
450 Y = (
f32)(cr * spcy + sr * cpsy);
451 Z = (
f32)(cr * cpsy - sr * spcy);
452 W = (
f32)(cr * cpcy + sr * spsy);
460 return set(vec.
X, vec.
Y, vec.
Z);
507 if ((angle + 1.0f) > 0.05f)
509 if ((1.0f - angle) >= 0.05f)
511 const f32 theta = acosf(angle);
513 scale = sinf(theta * (1.0f-time)) * invsintheta;
514 invscale = sinf(theta * time) * invsintheta;
524 q2.
set(-q1.
Y, q1.
X, -q1.
W, q1.
Z);
525 scale = sinf(
PI * (0.5f - time));
526 invscale = sinf(
PI * time);
529 return (*
this = (q1*scale) + (q2*invscale));
536 return (
X * q2.
X) + (
Y * q2.
Y) + (
Z * q2.
Z) + (
W * q2.
W);
544 const f32 fHalfAngle = 0.5f*angle;
545 const f32 fSin = sinf(fHalfAngle);
546 W = cosf(fHalfAngle);
556 const f32 scale = sqrtf(
X*
X +
Y*
Y +
Z*
Z);
568 angle = 2.0f * acosf(
W);
569 axis.
X =
X * invscale;
570 axis.
Y =
Y * invscale;
571 axis.
Z = Z * invscale;
583 euler.
Z = (
f32) (atan2(2.0 * (X*Y +Z*W),(sqx - sqy - sqz + sqw)));
586 euler.
X = (
f32) (atan2(2.0 * (Y*Z +X*W),(-sqx - sqy + sqz + sqw)));
589 euler.
Y = asinf(
clamp(-2.0f * (X*Z - Y*W), -1.0f, 1.0f) );
637 axis.
set(0.f,1.f,0.f);
643 const f32 s = sqrtf( (1+d)*2 );
644 const f32 invs = 1.f / s;