00001
00002
00003
00004
00005 #ifndef __IRR_QUATERNION_H_INCLUDED__
00006 #define __IRR_QUATERNION_H_INCLUDED__
00007
00008 #include "irrTypes.h"
00009 #include "irrMath.h"
00010 #include "matrix4.h"
00011 #include "vector3d.h"
00012
00013 namespace irr
00014 {
00015 namespace core
00016 {
00017
00019
00021 class quaternion
00022 {
00023 public:
00024
00026 quaternion() : X(0.0f), Y(0.0f), Z(0.0f), W(1.0f) {}
00027
00029 quaternion(f32 x, f32 y, f32 z, f32 w) : X(x), Y(y), Z(z), W(w) { }
00030
00032 quaternion(f32 x, f32 y, f32 z);
00033
00035 quaternion(const vector3df& vec);
00036
00038 quaternion(const matrix4& mat);
00039
00041 bool operator==(const quaternion& other) const;
00042
00044 inline quaternion& operator=(const quaternion& other);
00045
00047 inline quaternion& operator=(const matrix4& other);
00048
00050 quaternion operator+(const quaternion& other) const;
00051
00053 quaternion operator*(const quaternion& other) const;
00054
00056 quaternion operator*(f32 s) const;
00057
00059 quaternion& operator*=(f32 s);
00060
00062 vector3df operator*(const vector3df& v) const;
00063
00065 quaternion& operator*=(const quaternion& other);
00066
00068 inline f32 dotProduct(const quaternion& other) const;
00069
00071 inline quaternion& set(f32 x, f32 y, f32 z, f32 w);
00072
00074 inline quaternion& set(f32 x, f32 y, f32 z);
00075
00077 inline quaternion& set(const core::vector3df& vec);
00078
00080 inline quaternion& normalize();
00081
00083 matrix4 getMatrix() const;
00084
00086 void getMatrix( matrix4 &dest, const core::vector3df &translation ) const;
00087
00105 void getMatrixCenter( matrix4 &dest, const core::vector3df ¢er, const core::vector3df &translation ) const;
00106
00108 inline void getMatrix_transposed( matrix4 &dest ) const;
00109
00111 quaternion& makeInverse();
00112
00114 quaternion& slerp( quaternion q1, quaternion q2, f32 interpolate );
00115
00117
00122 quaternion& fromAngleAxis (f32 angle, const vector3df& axis);
00123
00125 void toAngleAxis (f32 &angle, core::vector3df& axis) const;
00126
00128 void toEuler(vector3df& euler) const;
00129
00131 quaternion& makeIdentity();
00132
00134 quaternion& rotationFromTo(const vector3df& from, const vector3df& to);
00135
00137 f32 X;
00138 f32 Y;
00139 f32 Z;
00140 f32 W;
00141 };
00142
00143
00144
00145 inline quaternion::quaternion(f32 x, f32 y, f32 z)
00146 {
00147 set(x,y,z);
00148 }
00149
00150
00151
00152 inline quaternion::quaternion(const vector3df& vec)
00153 {
00154 set(vec.X,vec.Y,vec.Z);
00155 }
00156
00157
00158
00159 inline quaternion::quaternion(const matrix4& mat)
00160 {
00161 (*this) = mat;
00162 }
00163
00164
00165
00166 inline bool quaternion::operator==(const quaternion& other) const
00167 {
00168 return ((X == other.X) &&
00169 (Y == other.Y) &&
00170 (Z == other.Z) &&
00171 (W == other.W));
00172 }
00173
00174
00175
00176 inline quaternion& quaternion::operator=(const quaternion& other)
00177 {
00178 X = other.X;
00179 Y = other.Y;
00180 Z = other.Z;
00181 W = other.W;
00182 return *this;
00183 }
00184
00185
00186
00187 inline quaternion& quaternion::operator=(const matrix4& m)
00188 {
00189 const f32 diag = m(0,0) + m(1,1) + m(2,2) + 1;
00190
00191 if( diag > 0.0f )
00192 {
00193 const f32 scale = sqrtf(diag) * 2.0f;
00194
00195
00196 X = ( m(2,1) - m(1,2)) / scale;
00197 Y = ( m(0,2) - m(2,0)) / scale;
00198 Z = ( m(1,0) - m(0,1)) / scale;
00199 W = 0.25f * scale;
00200 }
00201 else
00202 {
00203 if ( m(0,0) > m(1,1) && m(0,0) > m(2,2))
00204 {
00205
00206
00207 const f32 scale = sqrtf( 1.0f + m(0,0) - m(1,1) - m(2,2)) * 2.0f;
00208
00209
00210 X = 0.25f * scale;
00211 Y = (m(0,1) + m(1,0)) / scale;
00212 Z = (m(2,0) + m(0,2)) / scale;
00213 W = (m(2,1) - m(1,2)) / scale;
00214 }
00215 else if ( m(1,1) > m(2,2))
00216 {
00217
00218
00219 const f32 scale = sqrtf( 1.0f + m(1,1) - m(0,0) - m(2,2)) * 2.0f;
00220
00221
00222 X = (m(0,1) + m(1,0) ) / scale;
00223 Y = 0.25f * scale;
00224 Z = (m(1,2) + m(2,1) ) / scale;
00225 W = (m(0,2) - m(2,0) ) / scale;
00226 }
00227 else
00228 {
00229
00230
00231 const f32 scale = sqrtf( 1.0f + m(2,2) - m(0,0) - m(1,1)) * 2.0f;
00232
00233
00234 X = (m(0,2) + m(2,0)) / scale;
00235 Y = (m(1,2) + m(2,1)) / scale;
00236 Z = 0.25f * scale;
00237 W = (m(1,0) - m(0,1)) / scale;
00238 }
00239 }
00240
00241 return normalize();
00242 }
00243
00244
00245
00246 inline quaternion quaternion::operator*(const quaternion& other) const
00247 {
00248 quaternion tmp;
00249
00250 tmp.W = (other.W * W) - (other.X * X) - (other.Y * Y) - (other.Z * Z);
00251 tmp.X = (other.W * X) + (other.X * W) + (other.Y * Z) - (other.Z * Y);
00252 tmp.Y = (other.W * Y) + (other.Y * W) + (other.Z * X) - (other.X * Z);
00253 tmp.Z = (other.W * Z) + (other.Z * W) + (other.X * Y) - (other.Y * X);
00254
00255 return tmp;
00256 }
00257
00258
00259
00260 inline quaternion quaternion::operator*(f32 s) const
00261 {
00262 return quaternion(s*X, s*Y, s*Z, s*W);
00263 }
00264
00265
00266 inline quaternion& quaternion::operator*=(f32 s)
00267 {
00268 X*=s;
00269 Y*=s;
00270 Z*=s;
00271 W*=s;
00272 return *this;
00273 }
00274
00275
00276 inline quaternion& quaternion::operator*=(const quaternion& other)
00277 {
00278 return (*this = other * (*this));
00279 }
00280
00281
00282 inline quaternion quaternion::operator+(const quaternion& b) const
00283 {
00284 return quaternion(X+b.X, Y+b.Y, Z+b.Z, W+b.W);
00285 }
00286
00287
00288
00289 inline matrix4 quaternion::getMatrix() const
00290 {
00291 core::matrix4 m;
00292 getMatrix_transposed(m);
00293 return m;
00294 }
00295
00296
00300 inline void quaternion::getMatrix( matrix4 &dest, const core::vector3df ¢er ) const
00301 {
00302 f32 * m = dest.pointer();
00303
00304 m[0] = 1.0f - 2.0f*Y*Y - 2.0f*Z*Z;
00305 m[1] = 2.0f*X*Y + 2.0f*Z*W;
00306 m[2] = 2.0f*X*Z - 2.0f*Y*W;
00307 m[3] = 0.0f;
00308
00309 m[4] = 2.0f*X*Y - 2.0f*Z*W;
00310 m[5] = 1.0f - 2.0f*X*X - 2.0f*Z*Z;
00311 m[6] = 2.0f*Z*Y + 2.0f*X*W;
00312 m[7] = 0.0f;
00313
00314 m[8] = 2.0f*X*Z + 2.0f*Y*W;
00315 m[9] = 2.0f*Z*Y - 2.0f*X*W;
00316 m[10] = 1.0f - 2.0f*X*X - 2.0f*Y*Y;
00317 m[11] = 0.0f;
00318
00319 m[12] = center.X;
00320 m[13] = center.Y;
00321 m[14] = center.Z;
00322 m[15] = 1.f;
00323
00324
00325 dest.setDefinitelyIdentityMatrix ( false );
00326 }
00327
00328
00329
00342 inline void quaternion::getMatrixCenter(matrix4 &dest,
00343 const core::vector3df ¢er,
00344 const core::vector3df &translation) const
00345 {
00346 f32 * m = dest.pointer();
00347
00348 m[0] = 1.0f - 2.0f*Y*Y - 2.0f*Z*Z;
00349 m[1] = 2.0f*X*Y + 2.0f*Z*W;
00350 m[2] = 2.0f*X*Z - 2.0f*Y*W;
00351 m[3] = 0.0f;
00352
00353 m[4] = 2.0f*X*Y - 2.0f*Z*W;
00354 m[5] = 1.0f - 2.0f*X*X - 2.0f*Z*Z;
00355 m[6] = 2.0f*Z*Y + 2.0f*X*W;
00356 m[7] = 0.0f;
00357
00358 m[8] = 2.0f*X*Z + 2.0f*Y*W;
00359 m[9] = 2.0f*Z*Y - 2.0f*X*W;
00360 m[10] = 1.0f - 2.0f*X*X - 2.0f*Y*Y;
00361 m[11] = 0.0f;
00362
00363 dest.setRotationCenter ( center, translation );
00364 }
00365
00366
00367 inline void quaternion::getMatrix_transposed( matrix4 &dest ) const
00368 {
00369 dest[0] = 1.0f - 2.0f*Y*Y - 2.0f*Z*Z;
00370 dest[4] = 2.0f*X*Y + 2.0f*Z*W;
00371 dest[8] = 2.0f*X*Z - 2.0f*Y*W;
00372 dest[12] = 0.0f;
00373
00374 dest[1] = 2.0f*X*Y - 2.0f*Z*W;
00375 dest[5] = 1.0f - 2.0f*X*X - 2.0f*Z*Z;
00376 dest[9] = 2.0f*Z*Y + 2.0f*X*W;
00377 dest[13] = 0.0f;
00378
00379 dest[2] = 2.0f*X*Z + 2.0f*Y*W;
00380 dest[6] = 2.0f*Z*Y - 2.0f*X*W;
00381 dest[10] = 1.0f - 2.0f*X*X - 2.0f*Y*Y;
00382 dest[14] = 0.0f;
00383
00384 dest[3] = 0.f;
00385 dest[7] = 0.f;
00386 dest[11] = 0.f;
00387 dest[15] = 1.f;
00388
00389 dest.setDefinitelyIdentityMatrix ( false );
00390 }
00391
00392
00393
00394
00395 inline quaternion& quaternion::makeInverse()
00396 {
00397 X = -X; Y = -Y; Z = -Z;
00398 return *this;
00399 }
00400
00401
00402 inline quaternion& quaternion::set(f32 x, f32 y, f32 z, f32 w)
00403 {
00404 X = x;
00405 Y = y;
00406 Z = z;
00407 W = w;
00408 return *this;
00409 }
00410
00411
00412
00413 inline quaternion& quaternion::set(f32 x, f32 y, f32 z)
00414 {
00415 f64 angle;
00416
00417 angle = x * 0.5;
00418 const f64 sr = sin(angle);
00419 const f64 cr = cos(angle);
00420
00421 angle = y * 0.5;
00422 const f64 sp = sin(angle);
00423 const f64 cp = cos(angle);
00424
00425 angle = z * 0.5;
00426 const f64 sy = sin(angle);
00427 const f64 cy = cos(angle);
00428
00429 const f64 cpcy = cp * cy;
00430 const f64 spcy = sp * cy;
00431 const f64 cpsy = cp * sy;
00432 const f64 spsy = sp * sy;
00433
00434 X = (f32)(sr * cpcy - cr * spsy);
00435 Y = (f32)(cr * spcy + sr * cpsy);
00436 Z = (f32)(cr * cpsy - sr * spcy);
00437 W = (f32)(cr * cpcy + sr * spsy);
00438
00439 return normalize();
00440 }
00441
00442
00443 inline quaternion& quaternion::set(const core::vector3df& vec)
00444 {
00445 return set(vec.X, vec.Y, vec.Z);
00446 }
00447
00448
00449 inline quaternion& quaternion::normalize()
00450 {
00451 const f32 n = X*X + Y*Y + Z*Z + W*W;
00452
00453 if (n == 1)
00454 return *this;
00455
00456
00457 return (*this *= reciprocal_squareroot ( n ));
00458 }
00459
00460
00461
00462 inline quaternion& quaternion::slerp(quaternion q1, quaternion q2, f32 time)
00463 {
00464 f32 angle = q1.dotProduct(q2);
00465
00466 if (angle < 0.0f)
00467 {
00468 q1 *= -1.0f;
00469 angle *= -1.0f;
00470 }
00471
00472 f32 scale;
00473 f32 invscale;
00474
00475 if ((angle + 1.0f) > 0.05f)
00476 {
00477 if ((1.0f - angle) >= 0.05f)
00478 {
00479 const f32 theta = acosf(angle);
00480 const f32 invsintheta = reciprocal(sinf(theta));
00481 scale = sinf(theta * (1.0f-time)) * invsintheta;
00482 invscale = sinf(theta * time) * invsintheta;
00483 }
00484 else
00485 {
00486 scale = 1.0f - time;
00487 invscale = time;
00488 }
00489 }
00490 else
00491 {
00492 q2.set(-q1.Y, q1.X, -q1.W, q1.Z);
00493 scale = sinf(PI * (0.5f - time));
00494 invscale = sinf(PI * time);
00495 }
00496
00497 return (*this = (q1*scale) + (q2*invscale));
00498 }
00499
00500
00501
00502 inline f32 quaternion::dotProduct(const quaternion& q2) const
00503 {
00504 return (X * q2.X) + (Y * q2.Y) + (Z * q2.Z) + (W * q2.W);
00505 }
00506
00507
00510 inline quaternion& quaternion::fromAngleAxis(f32 angle, const vector3df& axis)
00511 {
00512 const f32 fHalfAngle = 0.5f*angle;
00513 const f32 fSin = sinf(fHalfAngle);
00514 W = cosf(fHalfAngle);
00515 X = fSin*axis.X;
00516 Y = fSin*axis.Y;
00517 Z = fSin*axis.Z;
00518 return *this;
00519 }
00520
00521
00522 inline void quaternion::toAngleAxis(f32 &angle, core::vector3df &axis) const
00523 {
00524 const f32 scale = sqrtf(X*X + Y*Y + Z*Z);
00525
00526 if (core::iszero(scale) || W > 1.0f || W < -1.0f)
00527 {
00528 angle = 0.0f;
00529 axis.X = 0.0f;
00530 axis.Y = 1.0f;
00531 axis.Z = 0.0f;
00532 }
00533 else
00534 {
00535 const f32 invscale = reciprocal(scale);
00536 angle = 2.0f * acosf(W);
00537 axis.X = X * invscale;
00538 axis.Y = Y * invscale;
00539 axis.Z = Z * invscale;
00540 }
00541 }
00542
00543 inline void quaternion::toEuler(vector3df& euler) const
00544 {
00545 const f64 sqw = W*W;
00546 const f64 sqx = X*X;
00547 const f64 sqy = Y*Y;
00548 const f64 sqz = Z*Z;
00549
00550
00551 euler.Z = (f32) (atan2(2.0 * (X*Y +Z*W),(sqx - sqy - sqz + sqw)));
00552
00553
00554 euler.X = (f32) (atan2(2.0 * (Y*Z +X*W),(-sqx - sqy + sqz + sqw)));
00555
00556
00557 euler.Y = asinf( clamp(-2.0f * (X*Z - Y*W), -1.0f, 1.0f) );
00558 }
00559
00560
00561 inline vector3df quaternion::operator* (const vector3df& v) const
00562 {
00563
00564
00565 vector3df uv, uuv;
00566 vector3df qvec(X, Y, Z);
00567 uv = qvec.crossProduct(v);
00568 uuv = qvec.crossProduct(uv);
00569 uv *= (2.0f * W);
00570 uuv *= 2.0f;
00571
00572 return v + uv + uuv;
00573 }
00574
00575
00576 inline core::quaternion& quaternion::makeIdentity()
00577 {
00578 W = 1.f;
00579 X = 0.f;
00580 Y = 0.f;
00581 Z = 0.f;
00582 return *this;
00583 }
00584
00585 inline core::quaternion& quaternion::rotationFromTo(const vector3df& from, const vector3df& to)
00586 {
00587
00588
00589 vector3df v0 = from;
00590 vector3df v1 = to;
00591 v0.normalize();
00592 v1.normalize();
00593
00594 const f32 d = v0.dotProduct(v1);
00595 if (d >= 1.0f)
00596 {
00597 return makeIdentity();
00598 }
00599
00600 const f32 s = sqrtf( (1+d)*2 );
00601 const f32 invs = 1.f / s;
00602 const vector3df c = v0.crossProduct(v1)*invs;
00603 X = c.X;
00604 Y = c.Y;
00605 Z = c.Z;
00606 W = s * 0.5f;
00607
00608 return *this;
00609 }
00610
00611
00612 }
00613 }
00614
00615 #endif
00616